陀螺仪辅助定位方法

陀螺仪辅助定位方法

陀螺仪辅助定位方法

CN 103644911 A 摘要

本发明公开了一种陀螺仪辅助定位方法，包括以下步骤：首先、姿态角的测量:对加速度计的测量值和磁

传感器的测量值分别经过高斯牛顿迭代得到姿态误差速率,

陀螺仪直接通过四元数微分方程得到姿态四元数速率,两个

加起来积分得到姿态四元数；其次、初始点的

获取：利用加速度计和陀螺仪芯片两者的数据，进行轨迹推算，以将轨迹周期提高到惯性测量单元的数据周期；第三、速度和移动距离的计算；第四、计算位置点；

第五：纠正位置。本发明针对性的解决了陀螺仪的漂移、载体的线性加速度和周围局部磁场的干扰问题，根据道路数据，对定位结果进行投影修正，并利用修正结果

计算下一个位置点，这样就有效地减小累计误差。

权利要求(1)

1.一种陀螺仪辅助定位方法，其特征在于包括以下步骤:

首先、姿态角的测量:对加速度计的测量值和磁传感器的测量值分别经过高斯牛顿迭代得到姿态误差速率，陀螺仪直接通

过四元数微分方程得到姿态四元数速率，两

个加起来积分得到姿态四元数；其次、初始点的获取:利用加速度计和陀螺仪芯片两者的数据，进行轨迹推算，以将轨迹周期提高到惯性测量单元的数据周期；

第三、速度和移动距离的计算:根据修正过的欧拉角，计算三轴的加速度的分量，从而计算出实际的加速度；在已知初速度，加速度和时间的情况下，通过对初速度

积分和加速度二次积分，计算出移动距离；第四、计算位置点:根据移动距离和方向计算当前的位置点的经纬度；

第五:纠正位置:根据计算的位置点，在道路上进行投影，进行平滑处理，得到纠正后的位置点。

说明

陀螺仪辅助定位方法

技术领域

[0001] 本发明涉及辅助定位方法，尤其是涉及一种脱落及辅助定位及其方法。

背景技术

[0002]

现有的陀螺仪辅助导航方法是采用陀螺仪定位方式，陀螺仪定位方式是基于角速度检测的原理而设计，通过一系列的运

算，得到的定位位置是相对于上次定位的一个

相对值。现有的陀螺仪辅助导航方法虽然已经使得空中指向定位成为可能，但由于得到的定位位置只是相对于上次定位的一个相对值，指向性存在一定的偏差，当长

时间偏差得不到补偿时，偏差就会在一定程度上进行累加，以至于体验越来越差，在某些特定场合，它还不能完全满足需要，另外传统的陀螺仪辅助导航是基于最后

的位置点进行导航，在导航过程中会因为时间的增加累计误差不断的增大。

发明内容

[0003] 本发明所要解决的技术问题是针对上述现有技术的不足，提供一种精度高、误差小的陀螺仪辅助定位方法。[0004] 为解决上述技术问题，本发明采取的技术方案为:陀螺仪辅助定位方法，其特征在于包括以下步骤:

首先、姿态角的测量:对加速度计的测量值和磁传感器的测量值分别经过高斯牛顿迭代得到姿态误差速率，陀螺仪直接通过四元数微分方程得到姿态四元数速率，两个加起来积分得到姿态四元数；

其次、初始点的获取:利用加速度计和陀螺仪芯片两者的数据，进行轨迹推算，以将轨迹周期提高到惯性测量单元的数据周期；

第三、速度和移动距离的计算:根据修正过的欧拉角，计算

三轴的加速度的分量，从而计算出实际的加速度；在已知初速度，加速度和时间的情况下，通过对初速度积分和加速度二次积分，计算出移动距离；

第四、计算位置点:根据移动距离和方向计算当前的位置点的经纬度；

第五:纠正位置:根据计算的位置点，在道路上进行投影，进行平滑处理，得到纠正后的位置点。

[0005]

陀螺仪的漂移、载体的线性加速度和周围局部磁场的干扰是制约MARG传感器姿态测量精度的主要问题。本发明基于已有的惯性测量单元，设计了一个基于四元数

的Kalman滤波器，通过建立MARG传感器模型，引入传感器偏差补偿和自适应的测量噪声协方差矩阵构造方法来提

高姿态测量精度，减小载体线性加速度和

周围局部磁场的干扰，实现三自由度的姿态测量。传统的陀螺仪辅助导航是基于最后的位置点进行导航，在导航过程中会因为时间的增加累计误差不断的增大，本发

明是根据道路数据，对定位结果进行投影修正，并利用修正结果计算下一个位置点，这样就有效地减小累计误差。总之，现有技术没有解决陀螺仪的漂移、载体的线

性加速度和周围局部磁场的干扰和长时间的累积误差问题，本发明针对性的解决了陀螺仪的漂移、载体的线性加速度和

周围局部磁场的干扰问题，并把陀螺仪的空间

定位技术和实际的导航应用相结合，解决了长时间的累积误差问题，而且根据道路数据，对定位结果进行投影修正，并利用修正结果计算下一个位置点，这样就有效

地减小累计误差。

附图说明

[0006] 图1为姿态角测量的数学式图。

[0007] 图2为惯性加速度矢量图。

[0008] 图3为纠正后的位置点图。

具体实施方式

[0009] 实施例1

本陀螺仪辅助定位方法，包括以下步骤:

首先、姿态角的测量:对加速度计的测量值和磁传感器的测量值分别经过高斯牛顿迭代得到姿态误差速率，陀螺仪直接通过四元数微分方程得到姿态四元数速率，两

个加起来积分得到姿态四元数。参见图1，本陀螺仪辅助定位方法基于已有的惯性测量单元，设计了一个基于四元数的Kalman滤波器，通过建立MARG传感

器模型，引入传感器偏差补偿和自适应的测量噪声协方差矩阵构造方法来提高姿态测量精度，减小载体线性加速度和周围局部磁场的干扰，实现三自由度的姿态测

量。

[0010]

其次、初始点的获取:利用加速度计和陀螺仪芯片两者的数据，进行轨迹推算，以将轨迹周期提高到惯性测量单元的数据周期。众所周知GPS有一定的误差，如果

GPS的误差在15米范围的话，远超过了车道的宽度，要想定位在车道上，确实是个难度。可以利用固定GPS基站的

方式来校准精度，但还有一个问题，芯片提

供的定位周期最快只有一秒。就是说，这一秒范围内的轨迹是空白的，当速度达到一定程度的时候，一秒的距离已经很远，甚至已经完成了一个快速小角度偏转动

作。那么这样的话精度将会大大降低。为了填补这一秒中的轨迹空白，于是我们利用加速度计和陀螺仪芯片，利用两者数据推算轨迹，将轨迹周期提高到惯性测量单

元的数据周期，即20毫秒。

[0011]

第三、速度和移动距离的计算:根据修正过的欧拉角，计算三轴的加速度的分量，从而计算出实际的加速度；在已知初速度，加速度和时间的情况下，通过对初速度

积分和加速度二次积分，计算出移动距离；惯性加速度矢量图如图2所示，得到了惯性加速度矢量的三个分量，根据计算得出的欧拉角即可计算出我们实际方向的加