CN104819717B - 一种基于mems惯性传感器组的多旋翼飞行器姿态检测方法 - Google Patents

一种基于mems惯性传感器组的多旋翼飞行器姿态检测方法 Download PDF

Info

Publication number
CN104819717B
CN104819717B CN201510259903.6A CN201510259903A CN104819717B CN 104819717 B CN104819717 B CN 104819717B CN 201510259903 A CN201510259903 A CN 201510259903A CN 104819717 B CN104819717 B CN 104819717B
Authority
CN
China
Prior art keywords
matrix
attitude quaternion
attitude
estimated
acceleration sensor
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.)
Active
Application number
CN201510259903.6A
Other languages
English (en)
Other versions
CN104819717A (zh
Inventor
彭辉波
姜天宇
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Suzhou Lian Xinwei Electronics Co Ltd
Original Assignee
Suzhou Lian Xinwei Electronics Co Ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Suzhou Lian Xinwei Electronics Co Ltd filed Critical Suzhou Lian Xinwei Electronics Co Ltd
Priority to CN201510259903.6A priority Critical patent/CN104819717B/zh
Publication of CN104819717A publication Critical patent/CN104819717A/zh
Application granted granted Critical
Publication of CN104819717B publication Critical patent/CN104819717B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)
  • Gyroscopes (AREA)

Abstract

本发明公开了一种基于MEMS惯性传感器组的多旋翼飞行器姿态检测方法。本方法根据多旋翼飞行器工作过程中，惯性传感器组所引入的噪声干扰，动态调节陀螺仪与加速度计的增益系数，弥补了传统的姿态检测方法在加速度计受到干扰时精度下降的缺陷。步骤为：先验估计姿态四元数的获取，干扰噪声强度估计，改变卡尔曼滤波算法中的增益系数，计算后验估计姿态四元数。优点：根据噪声改变卡尔曼滤波中的增益系数能够很好的抑制惯性传感器组所引入的噪声干扰，提高多旋翼飞行器工作时的姿态检测精度。

Description

一种基于MEMS惯性传感器组的多旋翼飞行器姿态检测方法
技术领域
本发明涉及的是飞行器控制过程中，通过纯惯性传感器来检测多旋翼飞行器飞行姿态的算法。属于惯性导航技术领域。
背景技术
飞行器的姿态检测对于飞行器的控制是极其关键的，也是影响惯性导航系统精度的最主要因素。而由于MEMS工艺的惯性传感器精度较低，使得传统的姿态检测方法无法直接嫁接在其之上，需结合MEMS传感器的自身特性，以及飞行器的工作状态研究相应的姿态检测方法。
目前，对于基于MEMS惯性传感器组的姿态检测方法，主要是通过卡尔曼滤波算法对陀螺仪传感器和加速度传感器进行数据融合，从而检测出物体姿态信息。然后对于多旋翼飞行器，该方法并没有考虑到惯性传感器组在飞行器工作过程中所引入的干扰误差，从而导致该方法在飞行器加速减速过程中，姿态检测精度大幅下降。
发明内容
本发明所要解决的技术问题是针对上述缺陷，提出了一种对干扰误差进行处理从而提高飞行器姿态检测精度的基于MEMS惯性传感器组的多旋翼飞行器姿态检测方法。
为解决上述技术问题，本发明采用的技术方案是：
一种基于MEMS惯性传感器组的多旋翼飞行器姿态检测方法，其特征在于：通过陀螺仪传感器获取先验估计姿态四元数；通过加速度传感器的数据估计噪声强度并优化卡尔曼滤波器的增益系数；通过优化后的卡尔曼滤波器融合先验估计姿态四元数和加速度传感器数据计算后验姿态四元数。
在通过陀螺仪传感器获取先验估计姿态四元数的过程中，采用如下方法进行计算：
其中I为单位矩阵，ΔΘ为陀螺仪在[tk-1，tk]采样时间间隔内的角增量阵，Δθx、Δθy、Δθz为x、y、z陀螺仪在[tk-1，tk]采样时间间隔内的角增量，Q(tk-1)为上一时刻的后验估计姿态四元数，Q(tk)为当前时刻的先验估计姿态四元数。
在通过加速度传感器估计噪声强度并优化卡尔曼滤波器的增益系数的过程中，采用如下步骤：
(1)将加速度传感器的观测方程线性化，得到雅可比行列式Hk：
其中q0，q1，q2，q3为先验估计姿态四元数Q(tk)的元素；
(2)计算卡尔曼滤波器参数：状态量Xk，量测量Zk，系统方差阵Qk，量测量方差阵Rk，一步姿态预测矩阵Φk/k-1：
其中qk为先验估计姿态四元数，ak为加速度传感器数据，Qω为陀螺仪传感器的测量误差方差阵，Rg为加速度传感器的测量误差方差阵；
(3)计算卡尔曼滤波算法中的一步预测均方误差Pk/k-1：
其中Pk-1为k-1时刻的估计均方误差；
(4)通过UDUT分解卡尔曼滤波器中的一步均方误差Pk/k-1：
其中Uk/k-1为UDUT分解后的上三角矩阵，Dk/k-1为UDUT分解后的对角矩阵，为UDUT分解后的下三角矩阵，Fk，Gk，Wk均为后续计算需要用到的中间变量；
(5)计算有害加速度的方差矩阵
(51)计算加速度计的估计误差矩阵ΔZk：
ΔZk＝Zk-CkXk
其中Ck为k时刻的先验姿态四元数转换成的方向余弦矩阵：
(52)构造矩阵
其中ra,k-i为ΔZk矩阵中的第i行列向量，M为列向量的个数；
(53)求取外界有害加速度的方差矩阵
其中λi,k为矩阵Uk的特征值，ui,k为对应的特征向量，
(6)通过步骤(5)中的有害加速度方差矩阵优化卡尔曼滤波的增益阵Kk：
其中Gk，Wk为一步预测均方误差Pk/k-1进行UDUT分解时的中间变量。
直接计算方差矩阵算法计算量大，时间开销长，因此通过设定一阈值，判定是否在有明显有害加速度的情况下，再进行计算，可大大降低计算时间。设一阈值γ，当(λi,j-μi,j)<γ，
当(λi,j-μi,j)≧γ，
阈值的取值，需要根据系统的工作环境进行取值，最小为0，若所处的工作环境有害干扰较大，则取值要小一点，若所处的工作环境有害干扰较小，取值可以偏大一点。γ取0.1。
在通过优化后的卡尔曼滤波器融合先验估计姿态四元数和加速度传感器数据计算后验姿态四元数过程中，采用如下步骤：
通过优化后的增益矩阵Kk和加速度传感器的数据Zk计算后验估计姿态四元数
其中为估计误差四元数，即为后验姿态四元数；
根据后验估计姿态四元数估计均方误差Pk：
Pk＝Pk/k-1-KkHkPk/k-1
Pk作为k+1时刻的估计均方误差Pk+1/k继续姿态检测计算。
传统的姿态测量方案并没有考虑到当加速度传感器的测量值中包含有非惯性力产生的加速度数值的情况，当飞行器的运动状态进行变化时，引入的这些加速度会直接导致姿态测量精度的恶化，而本方案通过加速度传感器的测量值估计出有害加速度，并对卡尔曼滤波器的增益系数进行动态调整，避免了飞行器在进行加速、减速，急转等操作时，由于在加速度传感器中引入非惯性力所产生的加速度而导致测姿精度恶化的问题。
附图说明
图1为本发明检测方法原理框图；
图2为本发明计算流程；
图3为本发明的俯仰角精度测试结果；
图4为本发明的横滚角精度测试结果；
图5为本发明的航向角精度测试结果；
图6为本发明的抗干扰测试结果；
图7为本发明的抗干扰测试结果。
具体实施方式
下面结合附图，对本发明作详细说明：
本发明采用ST公司的MEMS传感器LSM330DLC作为惯性传感器单元，其中包括三自由度陀螺仪传感器和三自由度加速度传感器，并使用STM32F373RCT6作为主控MCU，组成小型嵌入式平台，实现本文的方法。将该平台与试验转台固联，通过与真实值的比较，测试本发明所述方法。检测的方法的步骤是：
首先通过陀螺仪传感器获取先验估计姿态四元数；其次通过加速度传感器估计噪声强度并优化卡尔曼滤波器的增益系数；最后通过优化后的卡尔曼滤波器融合先验估计姿态四元数和加速度传感器数据计算后验姿态四元数。具体方法如下：
(1)计算先验估计姿态四元数：
其中I为单位矩阵，ΔΘ为陀螺仪在[tk-1，tk]采样时间间隔内的角增量阵，Δθx、Δθy、Δθz为x、y、z陀螺仪在[tk-1，tk]采样时间间隔内的角增量，Q(tk-1)为上一时刻的后验估计姿态四元数，Q(tk)为当前时刻的先验估计姿态四元数。
(2)加速度传感器的观测方程线性化，得到如下的雅克比行列式Hk：
其中q0，q1，q2，q3为先验估计姿态四元数Q(tk)的元素。
(3)计算卡尔曼滤波器所需参数：
其中Φk/k-1为一步姿态预测矩阵，qk为先验估计姿态四元数，ak为加速度传感器数据，Qω为陀螺仪传感器的测量误差方差阵，Rg为加速度传感器的测量误差方差阵。
(4)计算卡尔曼滤波器中的一步预测均方误差Pk/k-1：
其中Pk-1为k-1时刻的估计均方误差。
(5)计算有害加速度的方差矩阵
ΔZk＝Zk-CkXk
其中Ck为k时刻的先验姿态四元数转换成的方向余弦矩阵，ΔZk为加速度计的估计误差矩阵。构造矩阵其中ra,k-i为ΔZk矩阵中的第i行列向量，M为列向量的个数。
外界有害加速度的方差矩阵可由以下公式求得：
Uk可表示为：
其中λi,k为矩阵Uk的特征值，ui,k为对应的特征向量。
令则外界有害加速度ab的方差矩阵可表示为：
最后，外界有害加速度ab的方差矩阵按如下步骤获取：
没有外界有害加速度时：即
存在外界有害加速度时：
γ为阈值，用以表征是否存在外界有害加速度，在本文中取0.1。
(6)通过UDUT分解卡尔曼滤波器中的一步均方误差Pk/k-1：
其中Uk/k-1为UDUT分解后的上三角矩阵，Dk/k-1为UDUT分解后的对角矩阵，为UDUT分解后的下三角矩阵，Fk，Gk，Wk均为后续计算需要用到的中间变量。
(7)通过优化后的增益矩阵和加速度传感器的数据计算后验估计姿态四元数：
其中为估计误差四元数，即为后验姿态四元数。
(8)计算估计均方误差Pk：
Pk＝Pk/k-1-KkHkPk/k-1 (12)
通过图3，图4，图5的精度测试比较，本文设计的姿态检测方法航向角动态精度在±2°以内，俯仰角与横滚角动态精度在±1.5°以内。
抗干扰测试以俯仰角为例，将转台绕Y轴迅速上下转动，用以模拟系统处于剧烈运动的环境中，此时俯仰角的测试结果如图6，图7所示。此时加速度计受到外界有害加速度的干扰，对比没有进行优化的姿态检测方法，性能有较大的提升。可见本文的方法对于飞行器在剧烈运动环境下的姿态检测精度有明显的提升作用。

Claims (2)

1.一种基于MEMS惯性传感器组的多旋翼飞行器姿态检测方法，其特征在于：通过陀螺仪传感器获取先验估计姿态四元数；通过加速度传感器的数据估计噪声强度并优化卡尔曼滤波器的增益系数；通过优化后的卡尔曼滤波器融合先验估计姿态四元数和加速度传感器数据计算后验估计姿态四元数；在通过陀螺仪传感器获取先验估计姿态四元数的过程中，采用如下方法进行计算：
<mrow> <mi>Q</mi> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mo>=</mo> <mo>&amp;lsqb;</mo> <mi>I</mi> <mrow> <mo>(</mo> <mn>1</mn> <mo>-</mo> <mfrac> <mrow> <msup> <mi>&amp;Delta;&amp;theta;</mi> <mn>2</mn> </msup> </mrow> <mn>8</mn> </mfrac> <mo>)</mo> </mrow> <mo>+</mo> <mrow> <mo>(</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <mo>-</mo> <mfrac> <mrow> <msup> <mi>&amp;Delta;&amp;theta;</mi> <mn>2</mn> </msup> </mrow> <mn>48</mn> </mfrac> <mo>)</mo> </mrow> <mi>&amp;Delta;</mi> <mi>&amp;Theta;</mi> <mo>&amp;rsqb;</mo> <mi>Q</mi> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> </mrow>
<mrow> <mi>&amp;Delta;</mi> <mi>&amp;Theta;</mi> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;Delta;&amp;theta;</mi> <mi>x</mi> </msub> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;Delta;&amp;theta;</mi> <mi>y</mi> </msub> </mrow> </mtd> <mtd> <mrow> <msub> <mi>&amp;Delta;&amp;theta;</mi> <mi>z</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>&amp;Delta;&amp;theta;</mi> <mi>x</mi> </msub> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <msub> <mi>&amp;Delta;&amp;theta;</mi> <mi>y</mi> </msub> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;Delta;&amp;theta;</mi> <mi>z</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>&amp;Delta;&amp;theta;</mi> <mi>x</mi> </msub> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;Delta;&amp;theta;</mi> <mi>y</mi> </msub> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <msub> <mi>&amp;Delta;&amp;theta;</mi> <mi>z</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>&amp;Delta;&amp;theta;</mi> <mi>x</mi> </msub> </mrow> </mtd> <mtd> <mrow> <msub> <mi>&amp;Delta;&amp;theta;</mi> <mi>y</mi> </msub> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;Delta;&amp;theta;</mi> <mi>z</mi> </msub> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> </mrow>
其中I为单位矩阵，ΔΘ为陀螺仪在[tk-1，tk]采样时间间隔内的角增量阵，Δθx、Δθy、Δθz为x、y、z陀螺仪在[tk-1，tk]采样时间间隔内的角增量，Q(tk-1)为上一时刻的后验估计姿态四元数，Q(tk)为当前时刻的先验估计姿态四元数；
在通过加速度传感器估计噪声强度并优化卡尔曼滤波器的增益系数的过程中，采用如下步骤：
(1)将加速度传感器的观测方程线性化，得到雅可比行列式Hk：
<mrow> <msub> <mi>H</mi> <mi>k</mi> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mo>-</mo> <mn>2</mn> <msub> <mi>q</mi> <mn>2</mn> </msub> </mrow> </mtd> <mtd> <mrow> <mn>2</mn> <msub> <mi>q</mi> <mn>3</mn> </msub> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <mn>2</mn> <msub> <mi>q</mi> <mn>0</mn> </msub> </mrow> </mtd> <mtd> <mrow> <mn>2</mn> <msub> <mi>q</mi> <mn>1</mn> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mn>2</mn> <msub> <mi>q</mi> <mn>1</mn> </msub> </mrow> </mtd> <mtd> <mrow> <mn>2</mn> <msub> <mi>q</mi> <mn>0</mn> </msub> </mrow> </mtd> <mtd> <mrow> <mn>2</mn> <msub> <mi>q</mi> <mn>3</mn> </msub> </mrow> </mtd> <mtd> <mrow> <mn>2</mn> <msub> <mi>q</mi> <mn>2</mn> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mn>2</mn> <msub> <mi>q</mi> <mn>0</mn> </msub> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <mn>2</mn> <msub> <mi>q</mi> <mn>1</mn> </msub> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <mn>2</mn> <msub> <mi>q</mi> <mn>2</mn> </msub> </mrow> </mtd> <mtd> <mrow> <mn>2</mn> <msub> <mi>q</mi> <mn>3</mn> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> </mrow>
其中q0，q1，q2，q3为先验估计姿态四元数Q(tk)的元素；
(2)计算卡尔曼滤波器参数：状态量Xk，量测量Zk，系统方差阵Qk，量测量方差阵Rk，一步姿态预测矩阵Φk/k-1：
<mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msub> <mi>X</mi> <mi>k</mi> </msub> <mo>=</mo> <msub> <mi>q</mi> <mi>k</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>Z</mi> <mi>k</mi> </msub> <mo>=</mo> <msub> <mi>a</mi> <mi>k</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>Q</mi> <mi>k</mi> </msub> <mo>=</mo> <msub> <mi>Q</mi> <mi>&amp;omega;</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>R</mi> <mi>k</mi> </msub> <mo>=</mo> <msub> <mi>R</mi> <mi>g</mi> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced>
<mrow> <msub> <mi>&amp;Phi;</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <mi>I</mi> <mrow> <mo>(</mo> <mn>1</mn> <mo>-</mo> <mfrac> <mrow> <msup> <mi>&amp;Delta;&amp;theta;</mi> <mn>2</mn> </msup> </mrow> <mn>8</mn> </mfrac> <mo>)</mo> </mrow> <mo>+</mo> <mrow> <mo>(</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <mo>-</mo> <mfrac> <mrow> <msup> <mi>&amp;Delta;&amp;theta;</mi> <mn>2</mn> </msup> </mrow> <mn>48</mn> </mfrac> <mo>)</mo> </mrow> <mi>&amp;Delta;</mi> <mi>&amp;Theta;</mi> </mrow>
其中qk为先验估计姿态四元数，ak为加速度传感器数据，Qω为陀螺仪传感器的测量误差方差阵，Rg为加速度传感器的测量误差方差阵；
(3)计算卡尔曼滤波算法中的一步预测均方误差Pk/k-1：
<mrow> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <msub> <mi>&amp;Phi;</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mi>&amp;Phi;</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>T</mi> </msubsup> <mo>+</mo> <msub> <mi>Q</mi> <mi>&amp;omega;</mi> </msub> </mrow>
其中Pk-1为k-1时刻的估计均方误差；
(4)通过UDUT分解卡尔曼滤波器中的一步均方误差Pk/k-1：
其中Uk/k-1为UDUT分解后的上三角矩阵，Dk/k-1为UDUT分解后的对角矩阵，为UDUT分解后的下三角矩阵，Fk，Gk，Wk均为后续计算需要用到的中间变量；
(5)计算有害加速度的方差矩阵
(51)计算加速度计的估计误差矩阵ΔZk：
ΔZk＝Zk-CkXk
其中Ck为k时刻的先验估计姿态四元数转换成的方向余弦矩阵：
<mrow> <msub> <mi>C</mi> <mi>k</mi> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msubsup> <mi>q</mi> <mn>0</mn> <mn>2</mn> </msubsup> <mo>+</mo> <msubsup> <mi>q</mi> <mn>1</mn> <mn>2</mn> </msubsup> <mo>-</mo> <msubsup> <mi>q</mi> <mn>2</mn> <mn>2</mn> </msubsup> <mo>-</mo> <msubsup> <mi>q</mi> <mn>3</mn> <mn>2</mn> </msubsup> </mrow> </mtd> <mtd> <mrow> <mn>2</mn> <mrow> <mo>(</mo> <msub> <mi>q</mi> <mn>1</mn> </msub> <msub> <mi>q</mi> <mn>2</mn> </msub> <mo>+</mo> <msub> <mi>q</mi> <mn>0</mn> </msub> <msub> <mi>q</mi> <mn>3</mn> </msub> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <mn>2</mn> <mrow> <mo>(</mo> <msub> <mi>q</mi> <mn>1</mn> </msub> <msub> <mi>q</mi> <mn>3</mn> </msub> <mo>+</mo> <msub> <mi>q</mi> <mn>0</mn> </msub> <msub> <mi>q</mi> <mn>2</mn> </msub> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mn>2</mn> <mrow> <mo>(</mo> <msub> <mi>q</mi> <mn>1</mn> </msub> <msub> <mi>q</mi> <mn>2</mn> </msub> <mo>-</mo> <msub> <mi>q</mi> <mn>0</mn> </msub> <msub> <mi>q</mi> <mn>3</mn> </msub> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <msubsup> <mi>q</mi> <mn>0</mn> <mn>2</mn> </msubsup> <mo>-</mo> <msubsup> <mi>q</mi> <mn>1</mn> <mn>2</mn> </msubsup> <mo>+</mo> <msubsup> <mi>q</mi> <mn>2</mn> <mn>2</mn> </msubsup> <mo>-</mo> <msubsup> <mi>q</mi> <mn>3</mn> <mn>2</mn> </msubsup> </mrow> </mtd> <mtd> <mrow> <mn>2</mn> <mrow> <mo>(</mo> <msub> <mi>q</mi> <mn>2</mn> </msub> <msub> <mi>q</mi> <mn>3</mn> </msub> <mo>+</mo> <msub> <mi>q</mi> <mn>0</mn> </msub> <msub> <mi>q</mi> <mn>1</mn> </msub> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mn>2</mn> <mrow> <mo>(</mo> <msub> <mi>q</mi> <mn>1</mn> </msub> <msub> <mi>q</mi> <mn>3</mn> </msub> <mo>+</mo> <msub> <mi>q</mi> <mn>0</mn> </msub> <msub> <mi>q</mi> <mn>2</mn> </msub> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <mn>2</mn> <mrow> <mo>(</mo> <msub> <mi>q</mi> <mn>2</mn> </msub> <msub> <mi>q</mi> <mn>3</mn> </msub> <mo>+</mo> <msub> <mi>q</mi> <mn>0</mn> </msub> <msub> <mi>q</mi> <mn>1</mn> </msub> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <msubsup> <mi>q</mi> <mn>0</mn> <mn>2</mn> </msubsup> <mo>-</mo> <msubsup> <mi>q</mi> <mn>1</mn> <mn>2</mn> </msubsup> <mo>-</mo> <msubsup> <mi>q</mi> <mn>2</mn> <mn>2</mn> </msubsup> <mo>+</mo> <msubsup> <mi>q</mi> <mn>3</mn> <mn>2</mn> </msubsup> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>;</mo> </mrow>
(52)构造矩阵
其中ra,k-i为ΔZk矩阵中的第i行列向量，M为列向量的个数；
(53)求取外界有害加速度的方差矩阵
<mrow> <msub> <mi>Q</mi> <mrow> <msub> <mi>a</mi> <mi>b</mi> </msub> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>M</mi> </munderover> <mrow> <mo>(</mo> <msub> <mi>&amp;lambda;</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>&amp;mu;</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>)</mo> </mrow> <msub> <mi>u</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <msubsup> <mi>u</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> </mrow> <mi>T</mi> </msubsup> </mrow>
其中λi,k为矩阵Uk的特征值，ui,k为对应的特征向量， (6)通过步骤(5)中的有害加速度方差矩阵优化卡尔曼滤波的增益阵Kk：
<mrow> <msub> <mi>K</mi> <mi>k</mi> </msub> <mo>=</mo> <msub> <mi>G</mi> <mi>k</mi> </msub> <msup> <mrow> <mo>(</mo> <msub> <mi>W</mi> <mi>k</mi> </msub> <mo>+</mo> <msub> <mi>Q</mi> <mrow> <msub> <mi>a</mi> <mi>b</mi> </msub> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>)</mo> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> </mrow>
其中Gk，Wk为一步预测均方误差Pk/k-1进行UDUT分解时的中间变量；
在通过优化后的卡尔曼滤波器融合先验估计姿态四元数和加速度传感器数据计算后验估计姿态四元数过程中，采用如下步骤：
(1)通过优化后的增益矩阵Kk和加速度传感器的数据Zk计算后验估计姿态四元数
<mrow> <mi>&amp;Delta;</mi> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mi>k</mi> </msub> <mo>=</mo> <msub> <mi>K</mi> <mi>k</mi> </msub> <msub> <mi>&amp;Delta;Z</mi> <mi>k</mi> </msub> </mrow>
<mrow> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mi>k</mi> </msub> <mo>=</mo> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <mi>&amp;Delta;</mi> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mi>k</mi> </msub> </mrow>
其中为估计误差四元数，即为后验估计姿态四元数；
(2)根据后验估计姿态四元数估计均方误差Pk：
Pk＝Pk/k-1-KkHkPk/k-1
Pk作为k+1时刻的估计均方误差Pk+1/k继续姿态检测计算。
2.如权利要求1所述的基于MEMS惯性传感器组的多旋翼飞行器姿态检测方法，其特征在于：设一阈值γ，当(λi,j-μi,j)<γ，
当(λi,j-μi,j)≧γ，
γ取0.1。
CN201510259903.6A 2015-05-20 2015-05-20 一种基于mems惯性传感器组的多旋翼飞行器姿态检测方法 Active CN104819717B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201510259903.6A CN104819717B (zh) 2015-05-20 2015-05-20 一种基于mems惯性传感器组的多旋翼飞行器姿态检测方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201510259903.6A CN104819717B (zh) 2015-05-20 2015-05-20 一种基于mems惯性传感器组的多旋翼飞行器姿态检测方法

Publications (2)

Publication Number Publication Date
CN104819717A CN104819717A (zh) 2015-08-05
CN104819717B true CN104819717B (zh) 2018-04-24

Family

ID=53730087

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201510259903.6A Active CN104819717B (zh) 2015-05-20 2015-05-20 一种基于mems惯性传感器组的多旋翼飞行器姿态检测方法

Country Status (1)

Country Link
CN (1) CN104819717B (zh)

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106289244B (zh) * 2016-08-10 2019-11-26 极翼机器人(上海)有限公司 一种抗加速度干扰的姿态确定方法
US10521703B2 (en) * 2017-06-21 2019-12-31 Caterpillar Inc. System and method for controlling machine pose using sensor fusion
US10401176B2 (en) * 2017-06-21 2019-09-03 Caterpillar Inc. System and method for determining machine state using sensor fusion
CN107656541A (zh) * 2017-09-12 2018-02-02 上海理工大学 无人机群飞行姿态实时监测系统
CN111649745B (zh) * 2020-05-18 2022-04-05 北京三快在线科技有限公司 电子设备的姿态估计方法、装置、电子设备和存储介质

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6453239B1 (en) * 1999-06-08 2002-09-17 Schlumberger Technology Corporation Method and apparatus for borehole surveying
CN102692225A (zh) * 2011-03-24 2012-09-26 北京理工大学 一种用于低成本小型无人机的姿态航向参考系统

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6453239B1 (en) * 1999-06-08 2002-09-17 Schlumberger Technology Corporation Method and apparatus for borehole surveying
CN102692225A (zh) * 2011-03-24 2012-09-26 北京理工大学 一种用于低成本小型无人机的姿态航向参考系统

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
An Extended Kalman filter for Quaternion-Based Orientation Estimation Using MARG Sensors;JL Marins et al;《Proceedings of the 2001 IEEE/RSJ International Conference on Intelligent Robots and Systems》;20011231;第2003-2011页 *
四旋翼飞行器控制系统研究;胡从坤等;《科技广场》;20141231;第50-54页 *
基于MARG的无人机抗干扰系统的研究;赵代弟等;《计算机测量与控制》;20150131;第23卷(第1期);第231-233 *

Also Published As

Publication number Publication date
CN104819717A (zh) 2015-08-05

Similar Documents

Publication Publication Date Title
CN104819717B (zh) 一种基于mems惯性传感器组的多旋翼飞行器姿态检测方法
CN108827299B (zh) 一种基于改进四元数二阶互补滤波的飞行器姿态解算方法
US9417091B2 (en) System and method for determining and correcting field sensors errors
CN110702113B (zh) 基于mems传感器的捷联惯导系统数据预处理和姿态解算的方法
JP2013543976A (ja) ジャイロセンサ較正のデバイス及び方法
CN110926460A (zh) 一种基于IMU的uwb定位异常值处理方法
CN110174121A (zh) 一种基于地磁场自适应修正的航姿系统姿态解算方法
KR101106048B1 (ko) 센서오차의 작동 중 자동교정 방법과 이를 이용한 관성항법장치
CN109682377A (zh) 一种基于动态步长梯度下降的姿态估计方法
CN107830871B (zh) 一种补偿陀螺仪角速度数据的方法、装置、陀螺仪和系统
RU2762143C2 (ru) Система определения курса и углового пространственного положения, выполненная с возможностью функционирования в полярной области
JP5419268B2 (ja) 三次元姿勢推定装置、三次元姿勢推定方法及び三次元姿勢推定プログラム
CN113566850B (zh) 惯性测量单元的安装角度标定方法、装置和计算机设备
JP5164645B2 (ja) カルマンフィルタ処理における繰り返し演算制御方法及び装置
CN114526731A (zh) 一种基于助力车的惯性组合导航方向定位方法
CN110873563B (zh) 一种云台姿态估计方法及装置
CN116147624B (zh) 一种基于低成本mems航姿参考系统的船舶运动姿态解算方法
Patel et al. Sensor fusion to improve state estimate accuracy using multiple inertial measurement units
CN108871323A (zh) 一种低成本惯性传感器在机动环境下的高精度导航方法
CN109506674B (zh) 一种加速度的校正方法及装置
CN106595669A (zh) 一种旋转体姿态解算方法
CN116182871B (zh) 一种基于二阶混合滤波的海缆探测机器人姿态估计方法
CN110375773B (zh) Mems惯导系统姿态初始化方法
CN112729348A (zh) 一种用于imu系统的姿态自适应校正方法
CN109674480B (zh) 一种基于改进互补滤波的人体运动姿态解算方法

Legal Events

Date Code Title Description
C06 Publication
PB01 Publication
EXSB Decision made by sipo to initiate substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant