CN104567931B - 一种室内惯性导航定位的航向漂移误差消除方法 - Google Patents

Abstract

一种室内惯性导航定位的航向漂移误差消除方法，基于惯性测量单元实现，该惯性测量单元包括有三轴加速度计、三轴磁力计和三轴陀螺仪，预先通过三轴加速度计和三轴磁力计来计算姿态四元数初值；在室内惯性导航定位过程中，先通过三轴陀螺仪读取角速度ω(ω_x,ω_y,ω_z)来判断惯性测量单元是否处于运动状态，若是，则结合角速度ω(ω_x,ω_y,ω_z)并通过四元数微分方程计算实时的姿态四元数Q(q₀,q₁,q₂,q₃)，若否，则保持当前姿态四元数不变；最后，通过实时的姿态四元数求解惯性测量单元的航向角和姿态角。本发明提出的航向漂移误差的消除方法，可以有效的解决由于静止时陀螺仪不工作导致的航向漂移误差，从而提高室内行人惯性导航定位系统的精度。

Description

一种室内惯性导航定位的航向漂移误差消除方法

技术领域

本发明涉及室内导航领域，特别是一种室内惯性导航定位的航向漂移误差消除方法。

背景技术

随着社会的发展，人们对室内位置服务的需求越来越迫切，如可以应用于手机购物、移动电子商务、个性化广告/优惠信息等。用户会希望能够直接获取商店或者所需产品的位置，而不用耗费大量时间苦苦寻找。由于GPS等文星导航定位方式无法在室内环境中正常工作，所以如何解决室内定位问题成为近年来人们研究的重点。

目前，已有的室内定位系统的解决方案主要有:WiFi定位、超声定位、射频定位和惯性导航定位等。惯性导航定位系统具有低成本，自主定位，可以在任何环境中工作的特点，越来越受到人们的青睐，成为各大研究机构研究的热点。

室内行人惯性导航定位系统的原理如下：首先，由三轴陀螺仪测量的三轴角速度，通过四元数微分方程计算实时姿态四元数，由姿态四元数可以得到航向角和姿态角。然后，通过四元数姿态矩阵，将载体坐标系的三轴加速度变换到导航坐标系(东-北-天)。最后，对导航坐标系的加速度进行积分运算可以得到每一步的步长，每一步的步长和航向进行矢量叠加就可以得到行人的运动轨迹，从而实现导航定位的功能。由此可见，影响室内行人惯性导航系统定位精度的主要原因是积分漂移误差和航向漂移误差，航向漂移是指由于静止时陀螺仪不工作，导致由姿态四元数解算出来的航向角以一定的斜率变化，严重影响室内行人惯性导航定位系统的精度，消除积分漂移误差和航向漂移误差成为提高定位精度的关键。

发明内容

本发明的主要目的在于提出一种室内惯性导航定位的航向漂移误差消除方法，可以有效的解决由于静止时陀螺仪不工作导致的航向漂移误差，从而提高室内行人惯性导航定位系统的精度。

本发明采用如下技术方案：

一种室内惯性导航定位的航向漂移误差消除方法，基于惯性测量单元实现，该惯性测量单元包括有三轴加速度计、三轴磁力计和三轴陀螺仪，其特征在于：预先通过三轴加速度计和三轴磁力计来计算姿态四元数初值；在室内惯性导航定位过程中，先通过三轴陀螺仪读取角速度(ω_x,ω_y,ω_z)来判断惯性测量单元是否处于运动状态，若是，则结合角速度ω(ω_x,ω_y,ω_z)并通过四元数微分方程计算实时的姿态四元数Q(q₀,q₁,q₂,q₃)，若否，则保持当前姿态四元数不变；最后，通过实时的姿态四元数求解惯性测量单元的航向角和姿态角。

优选的，所述的姿态四元数初值通过如下步骤计算得到:首先，通过三轴加速度计确定初始姿态角，根据该初始姿态角计算坐标变换矩阵；其次，通过三轴磁力计的磁场强度结合坐标变换矩阵得到与当地水平面平行的坐标系的磁场强度，根据该坐标系的X轴和Y轴的磁场强度计算初始航向角；最后，通过欧拉角和四元数之间的转换公式，得到初始姿态四元数。

优选的，通过三轴加速度计得到的加速度(a_x,a_y,a_z)确定初始姿态角：其中,θ为俯仰角，γ为翻滚角，g为重力加速度。

优选的，通过初始姿态角计算坐标变换矩阵具体是指:

记三轴磁力计的初始位置坐标系为b，与当地水平面平行的坐标系为n，中间坐标系为1，坐标变换矩阵为其中：

优选的，记三轴磁力计磁场强度为通过坐标变换矩阵得到与当地水平面平行坐标系n的磁场强度：由坐标系n的X轴和Y轴的磁场强度可以计算此时的初始航向角：

优选的，通过欧拉角和四元数之间的转换公式，得到初始姿态四元数Q(q₀,q₁,q₂,q₃)：

优选的，当所述的保持当前姿态四元数不变，具体是指：当判定惯性测量单元处于静止状态，则设定三轴陀螺仪输出的角速度ω_x＝ω_y＝ω_z＝0，使得四元数的增量为零，从而实现保持当前姿态四元数不变。

由上述对本发明的描述可知，与现有技术相比，本发明具有如下有益效果：

本发明提出的航向漂移误差的消除方法，可以有效的解决由于静止时陀螺仪不工作导致的航向漂移误差，从而提高室内行人惯性导航定位系统的精度。

附图说明

图1为本发明的惯性测量单元的结构框图；

图2为本发明初始姿态四元数的技术流程图；

图3为本发明方法的原理图；

图4为现有技术航向漂移示意图；

图5为采用本发明方法的航向漂移示意图。

具体实施方式

以下通过具体实施方式对本发明作进一步的描述。

参照图1至图3，一种室内惯性导航定位的航向漂移误差消除方法，基于惯性测量单元实现，该惯性测量单元包括有三轴加速度计、三轴磁力计、三轴陀螺仪和MCU等。先通过三轴加速度计和三轴磁力计来计算姿态四元数初值。姿态四元数初值通过如下步骤计算得到:

首先，通过三轴加速度计得到的加速度(a_x,a_y,a_z)确定初始姿态角：其中,θ为俯仰角，γ为翻滚角，g为重力加速度g＝9.8m/s²。记三轴磁力计x轴、y轴、z轴组成的初始位置坐标系为b，与当地水平面平行的坐标系为n，中间坐标系为1，坐标变换矩阵为其中：

其次，由于初始时刻的磁力计的位置不一定处于水平位置，利用加速度计得到初始姿态用于磁力计测量数据的坐标变换，将任意初始位置的磁力计测量数据变换到与当地水平面平行的坐标系，这样就可以利用变换后的X轴和Y轴的测量数据计算绝对航向角。记三轴磁力计磁场强度为通过坐标变换矩阵得到与当地水平面平行坐标系n的磁场强度：由坐标系n的X轴和Y轴的磁场强度可以计算此时的初始航向角ψ：

最后，通过欧拉角和四元数之间的转换公式，得到初始姿态四元数Q(q₀,q₁,q₂,q₃)：

由陀螺仪测量数据计算得到的航向角是一个相对值，而用于导航定位计算的航向角是一个绝对值，所以采用加速度计和磁力计测量数据计算得到初始的航向角，作为后续航向角的基准，保证航向角的准确性。

在室内惯性导航定位过程中，通过惯性测量单元实时检测行人的运动状态。具体的误差消除方法如下：先通过三轴陀螺仪读取角速度(ω_x,ω_y,ω_z)来判断惯性测量单元是否处于运动状态，若是，则结合角速度ω(ω_x,ω_y,ω_z)并通过四元数微分方程计算实时的姿态四元数Q(q₀,q₁,q₂,q₃)。若否，则判定惯性测量单元处于静止状态，保持当前姿态四元数不变，此时陀螺仪不工作，航向角和姿态角保持前一时刻的值不变。设定三轴陀螺仪输出的角速度ω_x＝ω_y＝ω_z＝0，使得四元数的增量为零，从而实现保持当前姿态四元数不变，进一步展开四元数微分方程可得：

“□q”表示四元数Q的增量。“Δt”指的是采样时间间隔。

最后，通过实时的姿态四元数求解惯性测量单元的航向角和姿态角。

本发明的惯性测量单元采用MPU-9150九轴传感器芯片，集成MEMS三轴加速度计、MEMS三轴磁力计和MEMS三轴陀螺仪于一体。控制单元MCU采用的是意法半导体公司的STM32F103单片机。控制单元MCU通过I²C接口读取MEMS加速度计、MEMS磁力计、MEMS陀螺仪的输出数据。

惯性测量单元静止放置采用本方法前后的对比参照图4、图5所示，可知采用本发明提出的方法前，航向角出现不断漂移的问题。采用本发明提出的方法后，静止时，航向角大小保持不变，有效的解决了航向漂移问题。

上述仅为本发明的具体实施方式，但本发明的设计构思并不局限于此，凡利用此构思对本发明进行非实质性的改动，均应属于侵犯本发明保护范围的行为。

Claims (6)

1.一种室内惯性导航定位的航向漂移误差消除方法，基于惯性测量单元实现，该惯性测量单元包括有三轴加速度计、三轴磁力计和三轴陀螺仪，其特征在于：预先通过三轴加速度计和三轴磁力计来计算姿态四元数初值；在室内惯性导航定位过程中，先通过三轴陀螺仪读取角速度ω(ω_x,ω_y,ω_z)来判断惯性测量单元是否处于运动状态，若是，则结合角速度ω(ω_x,ω_y,ω_z)并通过四元数微分方程计算实时的姿态四元数Q(q₀,q₁,q₂,q₃)，若否，则保持当前姿态四元数不变；最后，通过实时的姿态四元数求解惯性测量单元的航向角和姿态角；

所述的姿态四元数初值通过如下步骤计算得到:首先，通过三轴加速度计确定初始姿态角，根据该初始姿态角计算坐标变换矩阵；其次，通过三轴磁力计的磁场强度结合坐标变换矩阵得到与当地水平面平行的坐标系的磁场强度，根据该坐标系的X轴和Y轴的磁场强度计算初始航向角；最后，通过欧拉角和四元数之间的转换公式，得到初始姿态四元数。

2.如权利要求1所述的一种室内惯性导航定位的航向漂移误差消除方法，其特征在于:通过三轴加速度计得到的加速度(a_x,a_y,a_z)确定初始姿态角：其中,θ为俯仰角，γ为翻滚角，g为重力加速度。

3.如权利要求2所述的一种室内惯性导航定位的航向漂移误差消除方法，其特征在于：通过初始姿态角计算坐标变换矩阵具体是指:

记三轴磁力计的初始位置坐标系为b，与当地水平面平行的坐标系为n，中间坐标系为1，坐标变换矩阵为其中：

$C_n^1=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\γ}& -\sin \mathrm{\γ}\\ 0& \sin \mathrm{\γ}& \cos \mathrm{\γ}\end{array}\right],C_1^b=\left[\begin{array}{ccc}cos\mathrm{\θ}& 0& sin\mathrm{\θ}\\ 0& 1& 0\\ -\sin \mathrm{\θ}& 0& \cos \mathrm{\θ}\end{array}\right].$

4.如权利要求3所述的一种室内惯性导航定位的航向漂移误差消除方法，其特征在于：记三轴磁力计磁场强度为通过坐标变换矩阵得到与当地水平面平行坐标系n的磁场强度：由坐标系n的X轴和Y轴的磁场强度可以计算此时的初始航向角：

5.如权利要求4所述的一种室内惯性导航定位的航向漂移误差消除方法，其特征在于：通过欧拉角和四元数之间的转换公式，得到初始姿态四元数Q(q₀,q₁,q₂,q₃)：

$\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]=\left[\begin{array}{c}cos\frac{\mathrm{\ψ}}{2}cos\frac{\mathrm{\θ}}{2}cos\frac{\mathrm{\γ}}{2}-sin\frac{\mathrm{\ψ}}{2}sin\frac{\mathrm{\θ}}{2}sin\frac{\mathrm{\γ}}{2}\\ sin\frac{\mathrm{\ψ}}{2}cos\frac{\mathrm{\θ}}{2}sin\frac{\mathrm{\γ}}{2}-cos\frac{\mathrm{\ψ}}{2}sin\frac{\mathrm{\θ}}{2}cos\frac{\mathrm{\γ}}{2}\\ -\sin \frac{\mathrm{\ψ}}{2}\sin \frac{\mathrm{\θ}}{2}\cos \frac{\mathrm{\γ}}{2}-\cos \frac{\mathrm{\ψ}}{2}\cos \frac{\mathrm{\θ}}{2}\sin \frac{\mathrm{\γ}}{2}\\ -\cos \frac{\mathrm{\ψ}}{2}\sin \frac{\mathrm{\θ}}{2}\sin \frac{\mathrm{\γ}}{2}-\sin \frac{\mathrm{\ψ}}{2}\cos \frac{\mathrm{\θ}}{2}\cos \frac{\mathrm{\γ}}{2}\end{array}\right].$

6.如权利要求1所述的一种室内惯性导航定位的航向漂移误差消除方法，其特征在于：当所述的保持当前姿态四元数不变，具体是指：当判定惯性测量单元处于静止状态，则设定三轴陀螺仪输出的角速度ω_x＝ω_y＝ω_z＝0，使得四元数的增量为零，从而实现保持当前姿态四元数不变。