CN103900570B - 一种基于室内主导方向的航向误差测量方法 - Google Patents

Abstract

本发明提供例了一种基于室内主导方向的航向误差测量方法。利用规则建筑物具有相对走廊平行、相邻走廊以直角正交的特点，以已知的室内主导方向提供的航向作为参考值，解决了航向误差的不可观测性问题，采用精确的室内主导方向提供的航向和步幅航向的差值作为系统状态变量，应用卡尔曼滤波器对其进行估计，得到航向误差，克服了航向误差漂移大的缺点，提高了导航精度；本发明方法简单，稳定性和可靠性高，给出了一种航向误差测量方法，有效的提高了导航精度。

Description

一种基于室内主导方向的航向误差测量方法

技术领域

本发明涉及的是一种室内导航技术，尤其涉及一种基于室内主导方向的航向误差测量方法。

背景技术

由于航向误差的不可观测性，用于室内导航的微惯性测量单元存在严重的航向误差漂移，误差漂移导致系统无法正确进行正确导航，最终失去导航的意义。如何抑制航向误差漂移，提高导航精度一直是室内导航的难题，当一个人等单一移动体在规则建筑物内运动时，室内主导方向能够为移动体运动时的航向提供参考值，以室内主导方向提供的航向和步幅航向的差值作为状态变量，利用卡尔曼滤波器对状态变量进行估计，得到航向误差值，能够抑制航向误差漂移，提高导航精度。

发明内容

本发明的目的在于提供一种能解决现有的微惯性测量单元由于航向误差的不可观测导致的误差漂移问题的基于室内主导方向的航向误差测量方法。

本发明的目的是这样实现的：

步骤一：采集固定于移动体脚面的微惯性测量单元输出的量测信息，包括陀螺输出的角速度w_ib、加速度计输出的比力f和电子罗盘输出的航向

步骤二：根据步骤一中得到的角速度w_ib得到移动体航向

步骤三：根据步骤一中得到的比力f得到移动体位置p；

步骤四：根据步骤三中得到的移动体位置p得到移动体步幅航向θ，其量测方程为：

$\mathrm{\θ}=\arctan \left(\frac{p_{E,m}-p_{E,m-1}}{p_{N,m}-p_{N,m-1}}\right)$

其中，arctan为反正切函数，(p_E，m p_N,m)为第m步人体在东向和北向的位置坐标，(p_E,m-1 p_N,m-1)为第m-1步移动体在东向和北向的位置坐标；

步骤五：根据步骤一中得到的航向和步骤二中得到的移动体航向得到航向偏差第m步的航向偏差量测方程为：

式中，为移动体在第m步的航向误差，为移动体在第k步的电子罗盘输出，为由步骤四得到的移动体在第k步的航向，ε为零均值高斯白噪声；

步骤六：根据步骤四得到的步幅航向θ和步骤五得到航向偏差得到修正的步幅航向θ₁，其量测方程为：

步骤七：根据移动体轨迹确定室内主导方向根据步骤六得到修正的步幅航向θ₁和室内主导方向得到系统状态变量Δθ，其测量方程为：

利用卡尔曼滤波器对状态变量进行估计，得到航向误差Δθ。

本发明提供的基于室内主导方向的航向误差测量方法，利用规则建筑物具有相对走廊平行、相邻走廊以直角正交的特点，以已知的室内主导方向提供的航向作为参考值，解决了航向误差的不可观测性问题，采用精确的室内主导方向提供的航向和步幅航向的差值作为系统状态变量，应用卡尔曼滤波器对其进行估计，得到航向误差，克服了航向误差漂移大的缺点，提高了导航精度；本发明方法简单，稳定性和可靠性高，给出了一种航向误差测量方法，有效的提高了导航精度。

附图说明

图1是基于室内主导方向的航向误差测量方法的流程图；

图2是室内主导方向示意图；

图3步幅航向示意图。

具体实施方式

下面结合附图举例对本发明做更详细的描述。

本发明的基于室内主导方向的航向误差测量方法包括以下步骤：

步骤一：将微惯性测量单元固定于人体脚面，采集测量单元输出的量测信息，包括陀螺输出的角速度w_ib、加速度计输出的比力f和电子罗盘输出的航向

步骤二：根据步骤一中得到的角速度w_ib得到人体航向

步骤三：根据步骤一中得到的比力f得到人体位置p；

步骤四：根据步骤三中得到的人体位置p得到人体步幅航向θ，其量测方程为：

$\mathrm{\θ}=\arctan \left(\frac{p_{E,m}-p_{E,m-1}}{p_{N,m}-p_{N,m-1}}\right)$

其中，arctan为反正切函数，(p_E,m p_N,m)为第m步人体在东向和北向的位置坐标，(p_E,m-1 p_N,m-1)为第m-1步人体在东向和北向的位置坐标；

步骤五：根据步骤一中得到的航向和步骤二中得到的人体航向得到航向偏差第m步的航向偏差量测方程为：

式中，为人体在第m步的航向误差，为人体在第k步的电子罗盘输出，为由步骤四得到的人体在第k步的航向，ε为零均值高斯白噪声；

步骤六：根据步骤四得到的步幅航向θ和步骤五得到航向偏差得到修正的步幅航向θ₁，其量测方程为：

步骤七：根据人体轨迹确定室内主导方向根据步骤六得到修正的步幅航向θ₁和室内主导方向得到系统状态变量Δθ，其测量方程为：

利用卡尔曼滤波器对状态变量进行估计，得到航向误差Δθ。

Claims (1)

1.一种基于室内主导方向的航向误差测量方法，其特征在于包括以下步骤：

步骤一：采集固定于移动体脚面的微惯性测量单元输出的量测信息，包括陀螺输出的角速度w_ib、加速度计输出的比力f和电子罗盘输出的航向

步骤二：根据步骤一中得到的角速度w_ib得到移动体航向

步骤三：根据步骤一中得到的比力f得到移动体位置p；

步骤四：根据步骤三中得到的移动体位置p得到移动体步幅航向θ，其量测方程为：

其中，arctan为反正切函数，(p_E,m p_N,m)为第m步单兵在东向和北向的位置坐标，(p_E,m-1 p_N,m-1)为第m-1步移动体在东向和北向的位置坐标；

步骤五：根据步骤一中得到的航向和步骤二中得到的移动体航向得到航向偏差移动体在第m步的航向偏差量测方程为：

式中，为移动体在第m步的航向偏差，为移动体在第k步的电子罗盘输出，为由步骤二得到的移动体在第k步的航向，ε为零均值高斯白噪声；

步骤六：根据步骤四得到的步幅航向θ和步骤五得到航向偏差得到修正的步幅航向θ₁，其量测方程为：

步骤七：根据移动体轨迹确定室内主导方向根据室内主导方向和步骤六得到修正的步幅航向θ₁得到系统状态变量Δθ，其测量方程为：

利用卡尔曼滤波器对系统状态变量进行估计，得到精确的系统状态变量。