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

说明书

本发明提供一种基于惯性系统的室内行人航向融合约束算法，根据行人的行走航向与行人所处道路航向之差的模值设定阈值，使用阈值来判断行人是否偏离室内地图的道路中心线航向。当行人的行走航向与行人所处道路航向之差的模值小于阈值，继续使用地图道路航向约束。当行人的行走航向与行人所处道路航向之差的模值大于阈值，判断行人是否处于直线行走状态。当行人处于直线行走状态时，采用行人行走方向航向约束。当行人处于非直线行走状态时，采用航向自约束算法，从而实现在不同室内场景地形和行人不同行走状态之下的航向角约束的目的。

技术领域

本发明涉及导航、制导与控制技术领域，尤其涉及一种基于惯性系统的室内行人航向融合约束算法。

背景技术

随着微机电技术的发展，体积小、质量轻、功耗低的MEMS微惯性测量单元(MicroInertial Measurement Unit，MIMU)的导航定位技术逐渐成为一个热门的研究方向。这种基于自包含传感器微惯性导航系统无需依靠外部设备(如全球卫星定位系统，测距设备等)，即可实时地提供行人位置、速度、姿态等导航定位信息，从而独立自主的实现导航定位和轨迹跟踪。

在传统的室内行人导航定位算法中，大多只采取了单一的航向角约束算法。主要有基于地图匹配的航向角约束算法，利用磁力计输出值抑制零速区间航向误差算法、行人行走方向航向约束算法，航向自观测、自约束算法等，这些算法都能够在一定的程度上抑制航向漂移，从而修正航向误差实现导航定位功能，但都存在明显的局限性。室内地图匹配算法仅适用于行人行走方向与道路一致的情况下，当行人行走偏离道路路向，如存在避让行人、非直线行走等特殊情况时，基于地图匹配的航向角约束算法不再实用。利用磁力计输出可以有效抑制零速区间航向误差，但室内金属材料较多，对磁力计的干扰较大，因此会对航向误差造成一定影响。当行人行走方向与设定方向相匹配时，使用该条道路主方向约束行人行走航向，仅限于抑制行人直线行走时的航向误差，具体应用场景比较少。航向自观测、自约束算法利用零速区间除第一个采样外的其余点的航向角等于第一个采样点对应的航向角，或航向角等于零速区间航向角均值，从而提高航向角精度，这种方法虽不受使用场景的限制，但精度不高。

发明内容

为解决现有技术存在的局限和缺陷，本发明提供一种基于惯性系统的室内行人航向融合约束算法，包括：

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

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

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

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

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

其中，测量矩阵为

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

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

可选的，所述根据行人运动在零速区间的误差方程，获得卡尔曼滤波器的状态变量的步骤还包括：

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