CN108121890A - 一种基于线性卡尔曼滤波的航姿信息融合方法 - Google Patents
一种基于线性卡尔曼滤波的航姿信息融合方法 Download PDFInfo
- Publication number
- CN108121890A CN108121890A CN201611063441.1A CN201611063441A CN108121890A CN 108121890 A CN108121890 A CN 108121890A CN 201611063441 A CN201611063441 A CN 201611063441A CN 108121890 A CN108121890 A CN 108121890A
- Authority
- CN
- China
- Prior art keywords
- mrow
- mtd
- mtr
- msub
- msubsup
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Classifications
-
- G—PHYSICS
- G16—INFORMATION AND COMMUNICATION TECHNOLOGY [ICT] SPECIALLY ADAPTED FOR SPECIFIC APPLICATION FIELDS
- G16Z—INFORMATION AND COMMUNICATION TECHNOLOGY [ICT] SPECIALLY ADAPTED FOR SPECIFIC APPLICATION FIELDS, NOT OTHERWISE PROVIDED FOR
- G16Z99/00—Subject matter not provided for in other main groups of this subclass
Landscapes
- Navigation (AREA)
Abstract
本发明提供了一种基于线性卡尔曼滤波的航姿信息融合方法，其特征在于，步骤如下：建立低精度惯导误差方程、建立失准角与水平加速度间关系以及线性卡尔曼滤波器模型，接着使用陀螺测量值进行姿态更新、把加速度测量值转换到导航系、计算加速度误差以及用估算的失准角修正姿态四元数，实现实时估计‑修正，最后转换成欧拉角显示。该方法解决了EKF算法计算量大和状态向量偏导数难以解析求取的问题，同时又可以实时调节比例系数和估计陀螺仪零偏；对陀螺仪建立的一阶马尔科夫模型，避免滤波器过渡收敛的问题；在性能上优于互补滤波和梯度下降算法，与EKF算法性能相当，但在计算量和变成复杂度上又小于EKF算法，是一种比较适合工程实现的方法。
Description
技术领域
本发明涉及机器人SLAM领域，尤其涉及一种基于线性卡尔曼滤波的航姿信息融合方法。
背景技术
对SLAM(simultaneous localization and mapping，即时定位和建图)是实现全自主移动机器人的关键。其中localization(定位)的核心是要知道机器人的航向、姿态(以后简称为航姿)、速度、位置等运动学参数。当前航姿一般是采用IMU(InertialMeasurement Unit，惯性测量单元)，配合上一套航姿算法来解算机器人的航姿信息。
航姿算法的基本思想是：对用IMU的加速度计信息获取的近似水平姿态角信息和陀螺仪积分得到的姿态信息进行融合。需要融合的原因是陀螺仪因为零偏的存在导致积分得到的航姿信息会逐渐发散，但在短期内的积分精度比较高；加速度计得到的姿态角则不存在发散的问题，但是加速度计得到的姿态则会受外界干扰加速度的影响(算法中对于加速度计输出全部认为是重力加速分量，但机器人运动过程中会有运动加速度)。加速度计和陀螺仪获取的航姿误差特性不同，具有互补特征，如果能够有合适的算法对这两个信息进行融合，则会得到一个既不怎么发散，也不怎么受加速度干扰的航向、姿态。
当前的航向、姿态信息融合方法主要有互补滤波、基于梯度下降算法的方法、基于扩展卡尔曼滤波(EKF)的方法。
互补滤波的原理是基于陀螺仪和加速度计的高频和低频特性分开的特点，通过调节陀螺积分和加速度计得到的两个角度的比例系数来平衡短期和长期精度。此法优点是计算量小，容易实现。缺点是无法自动调节比例系数，适应性并不好，且存在对陀螺仪、加速度计的适应性问题，无法充分发挥器件的精度。
梯度下降算法根据建立的姿态四元数误差(陀螺积分的角度与加速度计计算的角度)代价函数，使之取最小，据此估计角度误差量，然后反馈。梯度下降算法的优点是计算量小，缺点是对陀螺仪、加速度计的适应性问题，无法充分发挥器件的精度，因为其不对器件误差特性建模。
EKF(扩展卡尔曼滤波)融合算法的基本原理是基于加速度误差来估计航姿误差，建立器件(陀螺仪、加速度计)的误差模型、建立关心的物理量的状态空间模型，使用EKF推导状态向量的雅可比(Jacbian)矩阵，Jacbian矩阵是一个偏导数矩阵，不宜求。这个算法的计算量大，且Jackbian矩阵很难求取解析式。优点是可以实时更新陀螺积分得到航姿与加速度计得到的姿态的比例系数，且可以估计陀螺仪的零偏，然后反馈。
发明内容
本发明的目的在于针对上述技术中存在的缺陷，提出了一种基于线性卡尔曼滤波的航姿信息融合方法。该方法解决了EKF算法计算量大和状态向量偏导数难以解析求取的问题，同时又具有可以实时调节比例系数和可以估计陀螺仪零偏的特点；且对陀螺仪建立了一阶马尔科夫模型，避免滤波器过渡收敛的问题；在性能上优于互补滤波和梯度下降算法、与EKF算法性能相当，但在计算量和变成复杂度上又小于EKF算法，是一种比较适合工程实现的方法。
为了实现上述目的，本发明提供的技术方案如下：一种基于线性卡尔曼滤波的航姿信息融合方法，其特征在于，包括如下步骤：
步骤1：建立低精度惯导***误差方程，公式如下：
其中，wε和w▽分别为陀螺角速率白噪声和加计比力白噪声，和分别为陀螺和加速度计一阶马尔科夫过程随机误差，如下：
和
τGi和τAi是相应的相关时间常数，wrGi和wrAi是一阶马尔科夫过程激励白噪声；
步骤2：建立失准角与水平加速度之间的关系
根据惯导比力方程，在低加速度下近似有
由关系式可得将代入式(2)，并且忽略加速度计测量误差，则式(2)变为
再记和式(3)进一步化为
上式(4)左边反对称阵是不可逆的，因而根据和不能完整地求出失准角φ；但是，注意到在低加速度时近似有代入上式(4)得
上式中符号“*”表示不需关注的元素，不妨取为0，记水平失准角φh＝[φE φN 0]T和水平计算加速度(误差)由公式(5)可得失准角与水平加速度之间的关系如下：
步骤3：建立线性卡尔曼滤波器模型
选择水平失准角φE、φN和陀螺漂移作为状态，以水平加速度和作为量测，根据步骤1中的公式(1a)、(1b)及(1c)和步骤2中的公式(5)，可得线性卡尔曼滤波器模型如下：
其中
V为加速度测量噪声；
步骤4：使用陀螺测量值进行姿态更新，公式如下：
步骤5：把加速度测量值转换到导航系(n)，如下：
步骤6：计算加速度误差，
步骤7：以加速度误差为观测量Z，代入步骤3中的线性卡尔曼滤波模型公式式，估计出失准角φ；以估计出的失准角φ，修正姿态四元数公式如下：
上述步骤4-7以一定的时间间隔进行循环，即可实现实时估计-修正，即可得到实时的姿态四元数，然后转成欧拉角显示即可。
本发明具有如下有益效果：基于线性卡尔曼滤波的方法解决了EKF算法计算量大和状态向量偏导数难以解析求取的问题，同时又具有可以实时调节比例系数和可以估计陀螺仪零偏的特点；且对陀螺仪建立了一阶马尔科夫模型，避免滤波器过渡收敛的问题；在性能上优于互补滤波和梯度下降算法、与EKF算法性能相当，但在计算量和变成复杂度上又小于EKF算法，是一种比较适合工程实现的方法。
附图说明
图1为本发明所述的基于线性卡尔曼滤波的航姿信息融合方法流程图。
具体实施方式
下面结合具体实施例对本发明做进一步的说明。
在扫地机器人上采用了一个低端的IMU芯片MPU6050，数据输出采用IIC接口，主控CPU采用Linux操作***，以10ms时钟定时采集IMU数据，以10ms为周期定时运行航姿算法。
一种基于线性卡尔曼滤波的航姿信息融合方法，该方法包括以下步骤：如附图1所示，
步骤1：建立低精度惯导***误差方程，公式如下：
其中，wε和w▽分别为陀螺角速率白噪声和加计比力白噪声，和分别为陀螺和加速度计一阶马尔科夫过程随机误差，如下：
和
τGi和τAi是相应的相关时间常数，wrGi和wrAi是一阶马尔科夫过程激励白噪声；
步骤2：建立失准角与水平加速度之间的关系
根据惯导比力方程，在低加速度下近似有
由关系式可得将代入式(2)，并且忽略加速度计测量误差，则式(2)变为
再记和式(3)进一步化为
上式(4)左边反对称阵是不可逆的，因而根据和不能完整地求出失准角φ；但是，注意到在低加速度时近似有代入上式(4)得
上式中符号“*”表示不需关注的元素，不妨取为0，记水平失准角φh＝[φE φN 0]T和水平计算加速度(误差)由公式(5)可得失准角与水平加速度之间的关系如下：
步骤3：建立线性卡尔曼滤波器模型
选择水平失准角φE、φN和陀螺漂移作为状态，以水平加速度和作为量测，根据步骤1中的公式(1a)、(1b)及(1c)和步骤2中的公式(5)，可得线性卡尔曼滤波器模型如下：
其中
V为加速度测量噪声；
步骤4：使用陀螺测量值进行姿态更新，公式如下：
步骤5：把加速度测量值转换到导航系(n)，如下：
步骤6：计算加速度误差，
步骤7：以加速度误差为观测量Z，代入步骤3中的线性卡尔曼滤波模型公式式，估计出失准角φ；以估计出的失准角φ，修正姿态四元数公式如下：
上述步骤4-7以10ms循环，即可实现实时估计-修正，即可得到实时的姿态四元数，然后转成欧拉角显示即可。
以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内的所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。
Claims (1)
1.一种基于线性卡尔曼滤波的航姿信息融合方法，其特征在于，包括如下步骤：
步骤1：建立低精度惯导误差方程，公式如下：
<mrow>
<mover>
<mi>&phi;</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>=</mo>
<mo>-</mo>
<msubsup>
<mi>C</mi>
<mi>b</mi>
<mi>n</mi>
</msubsup>
<mrow>
<mo>(</mo>
<msubsup>
<mi>&epsiv;</mi>
<mi>r</mi>
<mi>b</mi>
</msubsup>
<mo>+</mo>
<msub>
<mi>w</mi>
<mi>&epsiv;</mi>
</msub>
<mo>)</mo>
</mrow>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>1</mn>
<mi>a</mi>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<mi>&delta;</mi>
<msup>
<mover>
<mi>v</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>n</mi>
</msup>
<mo>=</mo>
<msubsup>
<mi>f</mi>
<mrow>
<mi>s</mi>
<mi>f</mi>
</mrow>
<mi>n</mi>
</msubsup>
<mo>&times;</mo>
<mi>&phi;</mi>
<mo>+</mo>
<msubsup>
<mi>C</mi>
<mi>b</mi>
<mi>n</mi>
</msubsup>
<mrow>
<mo>(</mo>
<msubsup>
<mo>&dtri;</mo>
<mi>r</mi>
<mi>b</mi>
</msubsup>
<mo>+</mo>
<msub>
<mi>w</mi>
<mo>&dtri;</mo>
</msub>
<mo>)</mo>
</mrow>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>1</mn>
<mi>b</mi>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<mi>&delta;</mi>
<msup>
<mover>
<mi>p</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>n</mi>
</msup>
<mo>=</mo>
<msup>
<mi>&delta;v</mi>
<mi>n</mi>
</msup>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>1</mn>
<mi>c</mi>
<mo>)</mo>
</mrow>
</mrow>
其中，wε和分别为陀螺角速率白噪声和加计比力白噪声，和分别为陀螺和加速度计一阶马尔科夫过程随机误差，如下：
和
τGi和τAi是相应的相关时间常数，wrGi和wrAi是一阶马尔科夫过程激励白噪声；
步骤2：建立失准角与水平加速度之间的关系
根据惯导比力方程，在低加速度下近似有
<mrow>
<mn>0</mn>
<mo>&ap;</mo>
<msubsup>
<mi>C</mi>
<mi>b</mi>
<mi>n</mi>
</msubsup>
<msubsup>
<mi>f</mi>
<mrow>
<mi>s</mi>
<mi>f</mi>
</mrow>
<mi>b</mi>
</msubsup>
<mo>+</mo>
<mi>g</mi>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>2</mn>
<mo>)</mo>
</mrow>
</mrow>
由关系式可得将代入式(2)，并且忽略加速度计测量误差，则式(2)变为
<mrow>
<mn>0</mn>
<mo>=</mo>
<mrow>
<mo>(</mo>
<mi>I</mi>
<mo>+</mo>
<mi>&phi;</mi>
<mo>&times;</mo>
<mo>)</mo>
</mrow>
<msubsup>
<mover>
<mi>C</mi>
<mo>~</mo>
</mover>
<mi>b</mi>
<mi>n</mi>
</msubsup>
<msubsup>
<mover>
<mi>f</mi>
<mo>~</mo>
</mover>
<mrow>
<mi>s</mi>
<mi>f</mi>
</mrow>
<mi>b</mi>
</msubsup>
<mo>+</mo>
<msup>
<mi>g</mi>
<mi>n</mi>
</msup>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>3</mn>
<mo>)</mo>
</mrow>
</mrow>
再记和式(3)进一步化为
<mrow>
<msubsup>
<mover>
<mi>f</mi>
<mo>~</mo>
</mover>
<mrow>
<mi>s</mi>
<mi>f</mi>
</mrow>
<mi>n</mi>
</msubsup>
<mo>&times;</mo>
<mi>&phi;</mi>
<mo>=</mo>
<mi>&delta;</mi>
<msubsup>
<mover>
<mi>f</mi>
<mo>~</mo>
</mover>
<mrow>
<mi>s</mi>
<mi>f</mi>
</mrow>
<mi>n</mi>
</msubsup>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>4</mn>
<mo>)</mo>
</mrow>
</mrow>
上式(4)左边反对称阵是不可逆的，因而根据和不能完整地求出失准角φ；但是，注意到在低加速度时近似有代入上式(4)得
<mrow>
<mo>-</mo>
<msup>
<mi>g</mi>
<mi>n</mi>
</msup>
<mo>&times;</mo>
<mi>&phi;</mi>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mi>g</mi>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>&times;</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>&phi;</mi>
<mi>E</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>&phi;</mi>
<mi>N</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>&phi;</mi>
<mi>U</mi>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mi>g</mi>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>&times;</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>&phi;</mi>
<mi>E</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>&phi;</mi>
<mi>N</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mo>*</mo>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<mi>&delta;</mi>
<msubsup>
<mover>
<mi>f</mi>
<mo>~</mo>
</mover>
<mrow>
<mi>s</mi>
<mi>f</mi>
<mi>x</mi>
</mrow>
<mi>n</mi>
</msubsup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>&delta;</mi>
<msubsup>
<mover>
<mi>f</mi>
<mo>~</mo>
</mover>
<mrow>
<mi>s</mi>
<mi>f</mi>
<mi>y</mi>
</mrow>
<mi>n</mi>
</msubsup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mo>*</mo>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mi>&delta;</mi>
<msubsup>
<mover>
<mi>f</mi>
<mo>~</mo>
</mover>
<mrow>
<mi>s</mi>
<mi>f</mi>
</mrow>
<mi>n</mi>
</msubsup>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>5</mn>
<mo>)</mo>
</mrow>
</mrow>
上式中符号“*”表示不需关注的元素，不妨取为0，记水平失准角φh＝[φE φN 0]T和水平计算加速度(误差)由公式(5)可得失准角与水平加速度之间的关系如下：
步骤3：建立线性卡尔曼滤波器模型
选择水平失准角φE、φN和陀螺漂移作为状态，以水平加速度和作为量测，根据步骤1中的公式(1a)、(1b)及(1c)和步骤2中的公式(5)，可得线性卡尔曼滤波器模型如下：
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<mover>
<mi>X</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>=</mo>
<mi>F</mi>
<mi>X</mi>
<mo>+</mo>
<mi>G</mi>
<mi>W</mi>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>Z</mi>
<mo>=</mo>
<mi>H</mi>
<mi>X</mi>
<mo>+</mo>
<mi>V</mi>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
其中
<mrow>
<mi>X</mi>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>&phi;</mi>
<mi>E</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>&phi;</mi>
<mi>N</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msubsup>
<mi>&epsiv;</mi>
<mi>r</mi>
<mi>b</mi>
</msubsup>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>,</mo>
<mi>F</mi>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>C</mi>
<mn>1</mn>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>C</mi>
<mn>2</mn>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>&beta;</mi>
<mi>G</mi>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>,</mo>
<mi>G</mi>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>C</mi>
<mn>1</mn>
</msub>
</mrow>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>1</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>C</mi>
<mn>2</mn>
</msub>
</mrow>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>1</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mi>I</mi>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>,</mo>
<mi>W</mi>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>w</mi>
<mi>&epsiv;</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>w</mi>
<mrow>
<mi>r</mi>
<mi>G</mi>
</mrow>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
<mrow>
<mi>Z</mi>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<mi>&delta;</mi>
<msubsup>
<mover>
<mi>f</mi>
<mo>~</mo>
</mover>
<mrow>
<mi>s</mi>
<mi>f</mi>
<mi>x</mi>
</mrow>
<mi>n</mi>
</msubsup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>&delta;</mi>
<msubsup>
<mover>
<mi>f</mi>
<mo>~</mo>
</mover>
<mrow>
<mi>s</mi>
<mi>f</mi>
<mi>y</mi>
</mrow>
<mi>n</mi>
</msubsup>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>,</mo>
<msub>
<mi>H</mi>
<mi>G</mi>
</msub>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mtable>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mi>g</mi>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mi>g</mi>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
</mtable>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>2</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
为加速度测量噪声；
步骤4：使用陀螺测量值进行姿态更新，公式如下：
步骤5：把加速度测量值转换到导航系(n)，如下：
步骤6：计算加速度误差，
步骤7：以加速度误差为观测量Z，代入步骤3中的线性卡尔曼滤波模型公式式，估计出失准角φ；以估计出的失准角φ，修正姿态四元数公式如下：
<mrow>
<msubsup>
<mover>
<mi>Q</mi>
<mo>~</mo>
</mover>
<mrow>
<mi>b</mi>
<mrow>
<mo>(</mo>
<mi>m</mi>
<mo>)</mo>
</mrow>
</mrow>
<mi>n</mi>
</msubsup>
<mo>=</mo>
<msubsup>
<mi>Q</mi>
<msup>
<mi>n</mi>
<mo>&prime;</mo>
</msup>
<mi>n</mi>
</msubsup>
<msubsup>
<mover>
<mi>Q</mi>
<mo>~</mo>
</mover>
<mrow>
<mi>b</mi>
<mrow>
<mo>(</mo>
<mi>m</mi>
<mo>)</mo>
</mrow>
</mrow>
<mi>n</mi>
</msubsup>
<mo>,</mo>
<msubsup>
<mi>Q</mi>
<msup>
<mi>n</mi>
<mo>&prime;</mo>
</msup>
<mi>n</mi>
</msubsup>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<mi>c</mi>
<mi>o</mi>
<mi>s</mi>
<mrow>
<mo>(</mo>
<mfrac>
<mi>&phi;</mi>
<mn>2</mn>
</mfrac>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mfrac>
<mrow>
<mi>&Delta;</mi>
<mover>
<mi>&phi;</mi>
<mo>&RightArrow;</mo>
</mover>
</mrow>
<mrow>
<mi>&Delta;</mi>
<mi>&phi;</mi>
</mrow>
</mfrac>
<mi>sin</mi>
<mrow>
<mo>(</mo>
<mfrac>
<mi>&phi;</mi>
<mn>2</mn>
</mfrac>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
上述步骤4-7以一定的时间间隔进行循环，即可实现实时估计-修正，即可得到实时的姿态四元数，然后转成欧拉角显示即可。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201611063441.1A CN108121890A (zh) | 2016-11-28 | 2016-11-28 | 一种基于线性卡尔曼滤波的航姿信息融合方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201611063441.1A CN108121890A (zh) | 2016-11-28 | 2016-11-28 | 一种基于线性卡尔曼滤波的航姿信息融合方法 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN108121890A true CN108121890A (zh) | 2018-06-05 |
Family
ID=62224676
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201611063441.1A Pending CN108121890A (zh) | 2016-11-28 | 2016-11-28 | 一种基于线性卡尔曼滤波的航姿信息融合方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN108121890A (zh) |
Cited By (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109631895A (zh) * | 2019-01-04 | 2019-04-16 | 京东方科技集团股份有限公司 | 一种物体的位姿估计方法和装置 |
| CN110857860A (zh) * | 2018-08-23 | 2020-03-03 | 凌宇科技（北京）有限公司 | 一种定位转换方法及其*** |
| CN111426318A (zh) * | 2020-04-22 | 2020-07-17 | 中北大学 | 基于四元数-扩展卡尔曼滤波的低成本ahrs航向角补偿方法 |
| CN112378401A (zh) * | 2020-08-28 | 2021-02-19 | 中国船舶重工集团公司第七0七研究所 | 一种惯导***运动加速度估计方法 |
| CN113282005A (zh) * | 2021-05-26 | 2021-08-20 | 惠州中国科学院遥感与数字地球研究所空间信息技术研究院 | 无刷云台调控方法、装置、计算机设备和存储介质 |
-
2016
- 2016-11-28 CN CN201611063441.1A patent/CN108121890A/zh active Pending
Cited By (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110857860A (zh) * | 2018-08-23 | 2020-03-03 | 凌宇科技（北京）有限公司 | 一种定位转换方法及其*** |
| CN110857860B (zh) * | 2018-08-23 | 2022-03-04 | 凌宇科技（北京）有限公司 | 一种定位转换方法及其*** |
| CN109631895A (zh) * | 2019-01-04 | 2019-04-16 | 京东方科技集团股份有限公司 | 一种物体的位姿估计方法和装置 |
| CN111426318A (zh) * | 2020-04-22 | 2020-07-17 | 中北大学 | 基于四元数-扩展卡尔曼滤波的低成本ahrs航向角补偿方法 |
| CN111426318B (zh) * | 2020-04-22 | 2024-01-26 | 中北大学 | 基于四元数-扩展卡尔曼滤波的低成本ahrs航向角补偿方法 |
| CN112378401A (zh) * | 2020-08-28 | 2021-02-19 | 中国船舶重工集团公司第七0七研究所 | 一种惯导***运动加速度估计方法 |
| CN113282005A (zh) * | 2021-05-26 | 2021-08-20 | 惠州中国科学院遥感与数字地球研究所空间信息技术研究院 | 无刷云台调控方法、装置、计算机设备和存储介质 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN103776446B (zh) | 一种基于双mems-imu的行人自主导航解算算法 | |
| CN108121890A (zh) | 一种基于线性卡尔曼滤波的航姿信息融合方法 | |
| CN103630137B (zh) | 一种用于导航***的姿态及航向角的校正方法 | |
| CN106979780B (zh) | 一种无人车实时姿态测量方法 | |
| CN103822633B (zh) | 一种基于二阶量测更新的低成本姿态估计方法 | |
| CN110398245B (zh) | 基于脚戴式惯性测量单元的室内行人导航姿态估计方法 | |
| CN110398257A (zh) | Gps辅助的sins***快速动基座初始对准方法 | |
| CN104075713B (zh) | 一种惯性/天文组合导航方法 | |
| CN104698486B (zh) | 一种分布式pos用数据处理计算机***实时导航方法 | |
| CN102706366B (zh) | 一种基于地球自转角速率约束的sins初始对准方法 | |
| CN104374388B (zh) | 一种基于偏振光传感器的航姿测定方法 | |
| CN106123900B (zh) | 基于改进型互补滤波的室内行人导航磁航向解算方法 | |
| CN104698485B (zh) | 基于bd、gps及mems的组合导航***及导航方法 | |
| CN107478223A (zh) | 一种基于四元数和卡尔曼滤波的人体姿态解算方法 | |
| CN109163735B (zh) | 一种晃动基座正向-正向回溯初始对准方法 | |
| CN108318038A (zh) | 一种四元数高斯粒子滤波移动机器人姿态解算方法 | |
| CN102538821B (zh) | 一种快速、参数分段式捷联惯性导航***自对准方法 | |
| CN102937450B (zh) | 一种基于陀螺测量信息的相对姿态确定方法 | |
| CN104697526A (zh) | 用于农业机械的捷联惯导***以及控制方法 | |
| CN106643715A (zh) | 一种基于bp神经网络改善的室内惯性导航方法 | |
| CN107479076B (zh) | 一种动基座下联合滤波初始对准方法 | |
| CN105806363A (zh) | 基于srqkf的sins/dvl水下大失准角对准方法 | |
| CN103727941A (zh) | 基于载体系速度匹配的容积卡尔曼非线性组合导航方法 | |
| CN112857398B (zh) | 一种系泊状态下舰船的快速初始对准方法和装置 | |
| CN103674064B (zh) | 捷联惯性导航***的初始标定方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| WD01 | Invention patent application deemed withdrawn after publication |
Application publication date: 20180605 |
|
| WD01 | Invention patent application deemed withdrawn after publication |