[发明专利]基于惯性系统的室内行人航向融合约束算法

权利要求书

1.一种基于惯性系统的室内行人航向融合约束算法，其特征在于，包括：

形成室内环境信息地图，所述室内环境信息地图使用XML语言对道路信息进行存储，建立室内无向地图；

根据行人运动在零速区间的误差方程，获得卡尔曼滤波器的状态变量，计算公式如下：

其中，姿态角误差为位置误差为δγ，速度误差为δν；

根据行人的行走航向与行人所处道路航向之差的模值设置阈值Ω₁；

获得所述卡尔曼滤波器的观测量，计算公式如下：

其中，测量矩阵为

观测噪声矩阵为R＝diag{(0.03)² (0.01)² (0.01)² (0.01)²}

根据所述观测量与所述阈值Ω₁判断行人是否偏离所述室内环境信息地图的道路中心线航向。

2.根据权利要求1所述的基于惯性系统的室内行人航向融合约束算法，其特征在于，所述根据行人运动在零速区间的误差方程，获得卡尔曼滤波器的状态变量的步骤还包括：

获得线性化系统状态转移矩阵，计算公式如下：

其中，观测量为行人处于步态周期之中的站立期的实际速度值，计算公式为：观测矩阵为H＝[0_3×3 0_3×3 I_3×3]；

通过更新姿态变换矩阵获得姿态误差补偿，计算公式如下：

其中，

3.根据权利要求1所述的基于惯性系统的室内行人航向融合约束算法，其特征在于，所述根据行人运动在零速区间的误差方程，获得卡尔曼滤波器的状态变量的步骤还包括：

当行人处于步态周期之中的站立期时进行时间更新和量测信息更新；

当行人处于步态周期之中的非站立期时进行时间更新；

根据系统状态方程和量测信息方程获得惯性导航系统的数学模型，计算公式如下：

其中，X_k是(k)时刻的系统n维状态变量，Φ_k,k-1是系统从(k-1)时刻到(k)时刻的n×n维状态转移矩阵，W_k-1是(k-1)时刻系统的过程噪声，Z_k是(k)时刻系统的观测量，H_k是(k)时刻系统的观测矩阵，V_k是(k)时刻系统的观测噪声。

4.根据权利要求1所述的基于惯性系统的室内行人航向融合约束算法，其特征在于，还包括：

当所述观测量小于所述阈值Ω₁时，所述室内环境信息地图匹配航向约束算法；

当所述观测量大于所述阈值Ω₁时，根据单个复步周期之内行人的行走航向角φ(k)的变换量，判断行人是否处于直线行走状态；

根据行人的行走航向设置阈值Ω₂，通过比较相邻两个零速区间的航向角变化率与阈值Ω₂的大小判断行人是否处于直线行走状态，判定方程如下：

当时，行人处于直线行走状态；

当时，行人处于曲线行走状态。

5.根据权利要求4所述的基于惯性系统的室内行人航向融合约束算法，其特征在于，还包括：

当行人处于直线行走状态时，所述卡尔曼滤波器的观测量表示为：

6.根据权利要求4所述的基于惯性系统的室内行人航向融合约束算法，其特征在于，还包括：

当行人处于曲线行走状态时，所述卡尔曼滤波器的观测量表示为：