基于模糊卡尔曼滤波的姿态估计算法

Ｉｎｓｔｒｕｍｅｎｔ　Ｔｅｃｈｎｉｑｕｅ　ａｎｄ　Ｓｅｎｓｏｒ

仪表技术与传感器

２０１９　Ｎｏ􀆰４　

基于模糊卡尔曼滤波的姿态估计算法

２．中国科学院大学，北京　１０００４９；　３．上海师范大学信息与机电工程学院，上海　２００２３４）

（１．中国科学院上海微系统与信息技术研究所宽带无线技术实验室，上海　２０００５０；

李鲁明１，２，赵鲁阳１，唐晓红１，３，何　为１，李凤荣１

　　摘要：针对载体运动情况下由于运动加速度引起的载体姿态估计误差，提出了一种基于模糊控制与卡尔曼滤波相结合的姿态估计算法。以加速度计采集值作为模糊控制器输入，自适应地估计卡尔曼滤波量测噪声方差。通过静止、转动及运动实验采集加速度计和陀螺仪数据，分别使用互补滤波法、梯度下降法和模糊卡尔曼滤波法进行姿态估计，结果表明模糊卡尔曼滤波在运动情况下准确性大幅提高，在静止、转动和运动下均具有更好的实时性。

关键词：姿态估计；模糊控制；卡尔曼滤波；陀螺仪；加速度计；微机电系统

中图分类号：ＴＰ２１２　　　文献标识码：Ａ　　　文章编号：１００２－１８４１（２０１９）０４－０１００－０６

ＡｔｔｉｔｕｄｅＥｓｔｉｍａｔｉｏｎＡｌｇｏｒｉｔｈｍＢａｓｅｄｏｎＦｕｚｚｙＫａｌｍａｎＦｉｌｔｅｒ

（１．ＬａｂｏｒａｔｏｒｙｏｆＢｒｏａｄｂａｎｄＷｉｒｅｌｅｓｓＴｅｃｈｎｏｌｏｇｙ，ＳｈａｎｇｈａｉＩｎｓｔｉｔｕｔｅｏｆＭｉｃｒｏｓｙｓｔｅｍａｎｄ

ＩｎｆｏｒｍａｔｉｏｎＴｅｃｈｎｏｌｏｇｙ，ＣｈｉｎｅｓｅＡｃａｄｅｍｙｏｆＳｃｉｅｎｃｅｓ，Ｓｈａｎｇｈａｉ２０００５０，Ｃｈｉｎａ；

２．ＵｎｉｖｅｒｓｉｔｙｏｆＣｈｉｎｅｓｅＡｃａｄｅｍｙｏｆＳｃｉｅｎｃｅｓ，Ｂｅｉｊｉｎｇ１０００４９，Ｃｈｉｎａ；

３．ＣｏｌｌｅｇｅｏｆＩｎｆｏｒｍａｔｉｏｎ，ＭｅｃｈａｎｉｃａｌａｎｄＥｌｅｃｔｒｉｃａｌＥｎｇｉｎｅｅｒｉｎｇ，ＳｈａｎｇｈａｉＮｏｒｍａｌＵｎｉｖｅｒｓｉｔｙ，Ｓｈａｎｇｈａｉ２００２３４，Ｃｈｉｎａ）

Ａｂｓｔｒａｃｔ：Ｔｏａｄｄｒｅｓｓｔｈｅｐｒｏｂｌｅｍｏｆａｔｔｉｔｕｄｅｅｓｔｉｍａｔｉｏｎｅｒｒｏｒｃａｕｓｅｄｂｙｍｏｔｉｏｎａｃｃｅｌｅｒａｔｉｏｎ，ａｎａｔｔｉｔｕｄｅｅｓｔｉｍａｔｉｏｎａｌｇｏｒｉｔｈｍｂａｓｅｄｏｎｆｕｚｚｙＫａｌｍａｎｆｉｌｔｅｒｗａｓｐｒｏｐｏｓｅｄ．Ｕｓｉｎｇｔｈｅａｃｃｅｌｅｒａｔｉｏｎａｓｔｈｅｉｎｐｕｔｏｆｆｕｚｚｙｃｏｎｔｒｏｌｌｅｒ，ｔｈｅｍｅａｓｕｒｅｍｅｎｔｎｏｉｓｅｃｏｖａｒｉ⁃ｔｈｅｃａｒｒｉｅｒｗａｓｓｔａｔｉｃ，ｒｏｔａｔｉｎｇａｎｄｍｏｖｉｎｇ．Ｔｈｅｃｏｍｐｌｅｍｅｎｔａｒｙｆｉｌｔｅｒａｌｇｏｒｉｔｈｍ，ｔｈｅｇｒａｄｉｅｎｔｄｅｓｃｅｎｔａｌｇｏｒｉｔｈｍａｎｄｔｈｅｆｕｚｚｙＫａｌ⁃ｇｒｅａｔｌｙｉｍｐｒｏｖｅｄｉｎｔｈｅｃａｓｅｏｆｍｏｔｉｏｎ，ａｎｄｉｔｈａｓｂｅｔｔｅｒｒｅａｌ⁃ｔｉｍｅｐｅｒｆｏｒｍａｎｃｅｉｎｔｈｅｃａｓｅｏｆｒｅｓｔ，ｒｏｔａｔｉｏｎａｎｄｍｏｔｉｏｎ．Ｋｅｙｗｏｒｄｓ：ａｔｔｉｔｕｄｅｅｓｔｉｍａｔｉｏｎ；ｆｕｚｚｙｃｏｎｔｒｏｌ；Ｋａｌｍａｎｆｉｌｔｅｒ；ｇｙｒｏｓｃｏｐｅ；ａｃｃｅｌｅｒｏｍｅｔｅｒ；ＭＥＭＳ

ａｎｃｅｃａｎｂｅｅｓｔｉｍａｔｅｄｂｙｔｈｅｏｕｔｐｕｔｏｆｆｕｚｚｙｃｏｎｔｒｏｌｌｅｒａｄａｐｔｉｖｅｌｙ．Ｔｈｅｄａｔａｏｆａｃｃｅｌｅｒｏｍｅｔｅｒａｎｄｇｙｒｏｓｃｏｐｅｗｅｒｅｃｏｌｌｅｃｔｅｄｗｈｅｎｍａｎｆｉｌｔｅｒａｌｇｏｒｉｔｈｍｗｅｒｅｕｓｅｄｔｏｅｓｔｉｍａｔｅｔｈｅａｔｔｉｔｕｄｅｏｆｃａｒｒｉｅｒ．ＴｈｅｒｅｓｕｌｔｓｓｈｏｗｔｈａｔｔｈｅａｃｃｕｒａｃｙｏｆｔｈｅｆｕｚｚｙＫａｌｍａｎｆｉｌｔｅｒｉｓ

ＬＩＬｕ⁃ｍｉｎｇ１，２，ＺＨＡＯＬｕ⁃ｙａｎｇ１，ＴＡＮＧＸｉａｏ⁃ｈｏｎｇ１，３，ＨＥＷｅｉ１，ＬＩＦｅｎｇ⁃ｒｏｎｇ１

０　引言

惯性导航与卫星导航系统相结合的组合导航系统被广泛应用于导弹制导、无人驾驶、飞行器定位等多个领域，ＭＥＭＳ惯性器件因为其低成本、体积小、质量轻等优势被广泛应用于民用领域。惯性导航系统中的一个重要环节是载体姿态估计，通常使用姿态角表示载体在三维空间中的姿态，以此进行不同坐标系间的映射，用于惯性导航中的位置及速度更新。

由于ＭＥＭＳ传感器的精度差、噪声高，直接使用陀螺仪采集得到的三轴角速度进行姿态估计会导致误差快速累积，只能进行短时间姿态估计，长时间使用会导致姿态角发散，增大位置及速度误差［１］。采用

基金项目：中科院科技服务网络计划项目（ＫＦＪ－ＳＴＳ－ＺＤＴＰ－０１７）；上海市青年科技英才扬帆计划项目（１５ＹＦ１４１４５００）；中国科学院青年创新促进会项目收稿日期：２０１８－０１－２６

加速度计和陀螺仪进行信息融合，可有效降低姿态估计误差，Ａ．ＥｌＨａｄｒｉ［２］、ＪｉｎＷｕ［３］等提出采用互补滤波的信息融合以及四元数更新，可以降低陀螺仪累积误差，但动态响应较差，收敛时间较慢。ＭａｚｅｙａｒＣｈｅｇｕｉ⁃ｎｉ［４］、孙金秋［５］等提出采用梯度下降法进行姿态估计，以四元数构造多元向量函数作为误差函数，采用梯度下降法修正四元数误差。ＬｕｉｚＣ．ＱｕｅｒｉｎｏＦｉｌｈｏ［６］、吴涛［７］等提出采用卡尔曼滤波进行姿态估计，采用四元数作为状态量，加速度计作为观测量进行状态更新，该算法实时性较好，收敛较快。以上方法中的控制量以及噪声方差均为定值，适用于载体静止及转动下的姿态估计，当载体运动时加速度计采集得到的加速度除了重力加速度还有运动加速度，运动加速度的存在会导致使用加速度计估计得到的姿态角具有较大的误差，但所设定的控制量无法快速跟随到状态变化，法进行姿态估计，使用ＰＩ控制器进行加速度和角速度

　第４期

导致姿态角估计误差增大。

李鲁明等：基于模糊卡尔曼滤波的姿态估计算法

１０１　　

本文针对载体运动情况下姿态角估计不准确的问题，提出采用模糊卡尔曼滤波算法进行姿态估计，将模糊控制器融入卡尔曼滤波，使用加速度计的数据作为模糊控制器输入，不断调节卡尔曼滤波量测噪声方差。本文通过静止、转动及运动多组实验对互补滤波、梯度下降法及本文算法进行误差及实时性对比，验证本文提出算法的有效性。１　姿态角原理

文中采用的地理坐标系（ｎ系）为北东地，载体坐标系（ｂ系）为前右下，即ｘ轴指向载体前方，ｙ轴指向载体右方，ｚ轴指向载体下方。姿态角由３个角度组成，分别是横滚角φ、俯仰角θ和航向角ψ，使用姿态角表示地理坐标系到载体坐标系的旋转关系，姿态角均为一个轴指向观察者时，绕该轴逆时针旋转角度为正。由于姿态角的旋转不可变性，不同的旋转顺序会

ｃｏｓθｃｏｓψéê

＝ê－ｃｏｓϕｓｉｎψ＋ｓｉｎϕｓｉｎθｃｏｓψＣｂｎ

ê

ëｓｉｎϕｓｉｎψ＋ｃｏｓϕｓｉｎθｃｏｓψ

得到不同的坐标系变换关系［８］，本文采用ＺＹＸ旋转顺序，坐标系变换过程为ｘｎｙｎｚｎ绕ｚ轴旋转φ得到坐标系ｘ１ｙ１ｚ１，再绕ｙ轴旋转θ得到坐标系ｘ２ｙ２ｚ２，再绕ｘ轴旋转φ得到坐标系ｘｂｙｂｚｂ。

由于使用欧拉角表示载体姿态时可能会出现万

向节死锁现象，因此通常采用四元数来表示载体姿态，在避免万向节死锁的同时也避免了大量的三角函数运算，能够减小计算量、提高实时性，四元数通常表示为Ｑ（ｑ０，ｑ１，ｑ２，ｑ３）＝ｑ０＋ｑ１ｌ＋ｑ２ｊ＋ｑ３ｋ。

→

→

→

地理坐标系与载体坐标系之间的变换关系可以

采用变换矩阵Ｃｂｎ进行表示，变换矩阵可以分别用欧拉角和四元数进行表示。

ｘｘééêｂùúêｎùú

ｂ

êｙｂú＝Ｃｎêｙｎúêúêúêúêëｚûëｚúû

ｂ

ｎ

（１）

＋２－２－２２（ｑ１ｑ２＋ｑ０ｑ３）２（ｑ１ｑ３－ｑ０ｑ２）ùｑ２é０ｑ１ｑ２ｑ３

êú

２２２２ｂ

Ｃｎ＝ê２（ｑ１ｑ２－ｑ０ｑ０）ｑ０－ｑ１＋ｑ２－ｑ３２（ｑ２ｑ３＋ｑ０ｑ１）ú

êú

２２２２úêë２（ｑ１ｑ３＋ｑ０ｑ２）２（ｑ２ｑ３－ｑ０ｑ１）ｑ０－ｑ１－ｑ２＋ｑ３û

　　载体处于静止状态时，ｎ系的加速度为ａｎ＝时保证了系统的实时性。［０　０　－ｇ］Ｔ，由变换矩阵可以得到静止时载体加速度与姿态角的关系。

ａéｓｉｎθ·ｇéùêｘùúêúｂ

ａｂ＝êａｙú＝Ｃｎａｎ＝ê－ｓｉｎϕｃｏｓθ·ｇú

êúê－úêëｃｏｓϕｃｏｓθ·ｇûëａúû

ｚ

－ｓｉｎϕｃｏｓψ＋ｃｏｓϕｓｉｎθｓｉｎψ

ｃｏｓϕｃｏｓψ＋ｓｉｎϕｓｉｎθｓｉｎψ

ｃｏｓθｓｉｎψ

－ｓｉｎθù

ú

ｓｉｎϕｃｏｓθú

ú

ｃｏｓϕｃｏｓθû

（２）

（３）

互补滤波法姿态估计的具体步骤如下：

第一步：将ｎ系中的静止加速度归一化并使用旋

转矩阵映射到ｂ系。

（４）（５）（６）

θ＝ａｒｃｔａｎ

ａｘ

２ｙ

ａ＋ａａｙａｚ

２ｚ

－２（ｑ１ｑ３－ｑ０ｑ２）ùｖé０ùééêúêｘùúêúｂ

êｖｙú＝Ｃｎê０ú＝ê－２（ｑ２ｑ３＋ｑ０ｑ１）ú（７）

êúêúê－úê２２２２úêë１ûë－ｑ０＋ｑ１＋ｑ２－ｑ３ûëｖｚúû

第二步：将加速度计采集得到的加速度归一化。

［ａ′ｘ　ａ′ｙ　ａ′ｚ］＝

Ｔ

ϕ＝ａｒｃｔａｎ

２　姿态估计算法２．１　互补滤波法

［ａｘ　ａｙ　ａｚ］Ｔ

ａ＋ａ＋ａ

２ｘ

２ｙ

２ｚ

（８）

第三步：使用向量叉乘计算２个向量误差。ｅａ′ｖéêｘùúéêｘùúéêｘùúêｅｙú＝êａ′ｙú⊗êｖｙú（９）êúêúêúêëｅｚúûêëａ′ｚúûêëｖｚúû

第四步：使用ＰＩ控制器修正陀螺仪采样值。ｗ′ｗｅééêｘùúéêｘùúêｘùú

êｗｙ′ú＝êｗｙú＋ｋｐ·êｅｙú＋ｋｉ·ｅＩｎｔ（１１）êúêúêúêúêúêｗ′ｗｅëｚûëûëｚúû

第五步：使用陀螺仪数据及四元数微分方程更新

ｅＩｎｔ＝ｅＩｎｔ＋［ｅｘ　ｅｙ　ｅｚ］Ｔ·ｔｓ

（１０）

载体姿态估计可以通过陀螺仪的数值结合四元数微分方程进行积分更新，陀螺仪进行姿态更新具有较好的动态响应，但陀螺仪的数值与真实值之间具有一定的偏差，不断地进行积分运算将导致姿态误差不断累积，误差呈现发散趋势［９］。使用加速度计估计载体姿态不存在累积误差，但动态响应较差。因此可以采用互补滤波器对两者的估计值进行融合，优化系统的估计精度及动态响应能力，有效抑制累积误差的同

　１０２　四元数。

ＩｎｓｔｒｕｍｅｎｔＴｅｃｈｎｉｑｕｅａｎｄＳｅｎｓｏｒ

Ａｐｒ􀆰２０１９　

第五步：使用陀螺仪及四元数微分方程进行四元éê０－ｗｘ－ｗｙ－ｗｚùΩ（ｗｂ

）＝êê

ｗ０ｗｗú

ｘｚ－ｙúêú（１２）

êｗｙ－ｗｚ０ｗｘúêëｗｚｗｙ－ｗｘ０úúûéêｑ̇ê０ùúéêｑ̇êｑ０ùúê１úúêｑúêｑ̇２ú＝１×Ω（ｗｂ′）·ê１ú（１３）

êú２êｑ２úëｑ̇３úêû

êëｑ３úúûéêｑ０ùúéêｑ０ùúéêｑ̇０ùêêｑ１ú̇úêú＝êêｑ１úêúêｑ１úú·ｔ（１４）

êｑê２úêｑ２ú＋êêｑ̇úｓ２ëｑ３úúûêêëｑ３úúûêëｑ̇ú３úû

第六步：将四元数转换为姿态角。

ϕ＝ａｒｃｔａｎ２（ｑ２ｑ２ｑ３＋ｑ０ｑ１）

θ＝ａｒｃｓｉｎ［－２（０－２２２

ｑｑｑ１－２．２　梯度下降法

３）ｑ－２＋ｑｑ３

１０ｑ２］

（１５）（１６）

梯度下降法是沿着误差函数梯度方向对四元数进行修正，以消除陀螺仪积分中累积误差导致的姿态估计不准确。梯度下降法姿态估计中误差函数为静止重力加速度映射到载体坐标系结果与加速度计采样值误差，误差函数为多元向量函数，其变量为四元数的４个参数。梯度下降法姿态估计的具体步骤如下：

第一步：将ｎ系中的静止加速度归一化并使用旋

转矩阵映射到ｂ系［ｖｘ̇ｙ第二步：将加速度计采集得到的加速度归一化　ｖ

　ｖｚ］Ｔ。［ａｘ′　ａｙ′　ａｚ′］Ｔ第三步：将两者求差得到多元向量函数。

ｆ（ｑ，ａ），及其雅克比矩阵Ｊ（ｑ）。

éｖ　ｆ（ｑ，ａ）＝êｘ－ａｘùéê－２（ｑ１ｑ３－ｑêｖúê０ｑ２）－ａｘùúｘê－ａｙú＝êê－２（ｑ２ｑ３＋ｑ０ｑ１）－ａｙúëｖúｚúê２２２２ú（１７）é－ａｚûë－Ｊ（ｑ）＝êêêê－２２ｑｑ２ｑｑ０＋ｑ１＋ｑ２－ｑ３－ａｚú－２û３２ｑ０－２ｑ１ùú

１－２ｑ０－２ｑ３－２ｑ２ú（１８）

ë第四步：计算梯度下降法修正步长的方向－２ｑ０２ｑ１２ｑｑú２－２３úû

。

ｓｔｅｐｓｔｅｐ＝Ｊ（＝

ｑ）Ｔ·ｆ（ｑ，ｑ）（１９）

‖ｓｔｅｐ‖ｓｔｅｐ

（２０）

数更新，与梯度下降法进行融合更新éêｑêｑ０ùúéêｑ０ùú

éêｑ

̇。

１ú̇０ùúêú＝êêｑ１úê１úêú＋êｑ

ú·ｔｓ－β·ｓｔｅｐ·（２１）

êｑ２úêｑ２úê̇êëｑ３úúûêêëｑêｑ２úúｔｓ

３úúûêëｑ̇ú

第六步：将四元数转换为姿态角３û

。２．３　模糊卡尔曼滤波法

针对互补滤波法、梯度下降法、卡尔曼滤波法等姿态估计方法中当载体运动时加速度计采集得到的加速度中包含重力加速度和运动加速度，会造成加速

度估计误差增大，导致使用加速度误差修正四元数时出现较大偏差，得到的姿态角估计值存在较大误差。本文提出一种模糊控制改进卡尔曼滤波姿态估计算法，使用模糊控制理论依据合加速度以及合加速度变化率对卡尔曼滤波量测噪声方差进行估计，以此自适

应地改变量测噪声方差，修正由于载体自身运动加速度导致的量测噪声方差变化。

卡尔曼滤波状态量为四元数ｘ＝［ｑ状态方程为陀螺仪更新四元数所采用的四元数微分０　ｑ１　ｑ２　ｑ３］Ｔ，

方程［１０］。

éｋｋ－１̇

êｑｑｋ－１ê０ùｋúéｑé０ùúêｑ１úêêｑ０ùｋ－úê̇ｋ－１êｋú＝ê１

１úêú＋êｑ１

ú̇ú·ｔｓ（２２）

êｑê２úêｑｋ－１ｋ－１úëｑｋúê２úêｑ３úûêëｑｋ３－１úúê２

úûêëｑ３̇ｋ－１úû

ｘｋ＝［Ｉ４×４＋

１

量测值为加速度计采样值计算得到的四元数估２

Ω（ｗｂ）·ｔｓ］·ｘｋ－１（２３）

计值，采用加速度计采集得到的加速度值对载体横滚角和俯仰角进行估计。

θ＝ａｒｃｔａｎ

ａｘ

ａ２ｙ

（２４）ϕ＝ａｒｃｔａｎ

ａ＋ａ２ｚａｙ

ｚ

（２５）

将估计得到的横滚角及俯仰角转换为四元数估计值作为模糊卡尔曼滤波量测值，记ｃϕ＝ｃｏｓ（ϕ／２），ｓϕ＝ｓｉｎ（ｓｉｎ（ϕ／ψ／２），２），ｃθ可以得到量测方程如式＝ｃｏｓ（θ／２），ｓθ＝ｓｉｎ（θ／２），ｃψ＝ｃｏｓ（ψ／２），ｓψ＝éêｑｃϕｃθｃψ（２６）所示。

Ｚ＝êêｑ０ùúéê＋ｓϕｓθｓψùú１úêｑú２ú＝êêｓϕｃθｃψ－ｃψｓθｓψúêｃϕｓθｃψ＋ｓϕｃθｓψú

（２６）

êêëｑ３úúú

ûêëｃϕｃθｓψ－ｓϕｓθｃψúû

　第４期

Ｚｋ＝ｘｋ

李鲁明等：基于模糊卡尔曼滤波的姿态估计算法

（２７）

１０３　　

载体运动时量测方程计算得到的四元数估计值偏差较大，导致量测噪声方差增大，由于运动加速度与量测噪声方差间的关系无法进行准确描述，提出使用模糊逻辑来对量测噪声方差进行估计，以此自适应地修改卡尔曼滤波的量测噪声方差，使得姿态估计更加准确，同时保证实时性。

模糊控制器的输入为加速度计采集到的合加速度与重力加速度差值ＡＣＣ以及合加速度的变化率ＤＡＣＣ，值以及控制值均划分为控制量为卡尔曼滤波量测噪声方差５档，分别是负大（ＲＮＬ）、。将输入负小（ＮＳ）、确定输入变量和控制量的隶属度函数零（Ｚ）、正小（ＰＳ）、正大（ＰＬ）。［１１］根据专家经验度函数变化的平缓性，提出采用高斯函数和，考虑到隶属ｓｉｇｍｏｉｄ函数相结合的隶属度函数，输入变量ＡＣＣ、ＤＡＣＣ以及

控制量Ｒ的隶属度函数分别如图１、图２、图３所示。图１　输入加速度隶属度函数

图２　输入加速度变化率隶属度函数

根据专家经验指定规则库，使得量测噪声能够跟随载体运动加速度的变化而变化，同时尽量避免过度修正而再次引入误差，在考虑运动加速度值大小的同时考虑加速度的变化趋势，对控制量进行适当的调节，根据专家经验指定的规则库如表１所示。

图３　输出控制量隶属度函数表１　模糊控制器规则库

量测噪声协方差

合加速度

ＮＬ

ＮＬＮＬＮＳ

ＮＬＮＬＺ

ＰＳＰＬ合加速度ＮＳＮＬＮＳＮＳ变化率

ＰＳＺ

ＮＬＮＬＰＳ

Ｚ

ＰＳＺ

ＮＳＮＳＰＬ　　模糊卡尔曼滤波姿态估计算法具体步骤如下ＰＬＺ

ＰＳＺ

ＰＳＺ

ＰＬ

ＰＬＰＬ

ＰＬＰＬ

第一步：计算ｋ时刻状态一步预测。

：

利用ｋ－１时刻状态最优估计结果ｘ􀭴ｘ｜ｋ－１＝Ａ·ｘ􀭴ｋ－１＋Ｂ·ｕｋ

，由状态方程对（２８）ｋ

时刻ｘ作一步状态预测，若系统没有控制量则取ｕｋ第二步：计算ｋ时刻协方差一步预测利用ｋ－１时刻协方差结果＝。

＝０。

Ｐｋ｜ｋ－１Ａ·Ｐｋ－１·，计算ＡＴ＋Ｑ

ｋ时刻协方差一

（２９）

步预测。

第三步：将合加速度及合加速度变化率输入模糊控制器，得到估计的量测噪声协方差矩阵Ｒ。

第四步：计算ｋ时刻卡尔曼滤波增益。

ＫｇＰｋ｜ｋ－１ｋ＝

Ｈ·Ｐ·ＨＴ

ｋ｜ｋ－１·ＨＴ＋Ｒ（３０）第五步：计算ｘ􀭴ｋ＝ｋ时刻状态最优估计值。第六步：计算ｘ􀭴ｋ｜ｋ－１ｋ时刻协方差＋Ｋｇｋ·（ｚｋ－。

Ｈ·ｘ􀭴ｋ｜ｋ＝１）（３１）Ｐｋ将状态量四元数转化为姿态角＝（Ｉ－Ｋｇｋ·Ｈ）·Ｐｋ｜ｋ－１

第七步：。（３２）

模糊卡尔曼滤波载体姿态估计算法流程图如图４

所示。

３　实验及结果分析３．１　载体静止实验

将载体处于静止状态进行加速度计和陀螺仪数据采集，分别使用互补滤波法、梯度下降法和模糊卡

　１０４　ＩｎｓｔｒｕｍｅｎｔＴｅｃｈｎｉｑｕｅａｎｄＳｅｎｓｏｒ

性和准确性的要求。３．２　载体转动实验

Ａｐｒ􀆰２０１９　

波法收敛较快，且与真实值误差较小，同时满足实时

将载体绕ｘ轴进行转动，保持俯仰角不变，横滚角转动采集加速度计和陀螺仪数据，分别采用３种算法图４　模糊卡尔曼滤波姿态估计算法流程图

尔曼滤波法进行载体姿态估计，３种算法得到的横滚角及俯仰角估计结果分别如图５、图６所示。

图５　载体静止横滚角估计

图６　载体静止俯仰角估计

差如表３种算法估计值与真实值的平均误差和均方根误

２所示。

表２　载体静止姿态估计算法对比

（°）

算法平均误差均方根误差

互补滤波法３．０３９１３．３４４横滚角

梯度下降法０．０４９０．１３１模糊卡尔曼０．００２０．００６互补滤波法－０．００２１．２８５俯仰角

梯度下降法－０．０５０．１３１模糊卡尔曼

－０．０００３

０．００７

　收敛较慢　由上述结果可以看出互补滤波法和梯度下降法，收敛后与真实值误差较小，模糊卡尔曼滤

进行载体姿态估计，得到横滚角及俯仰角分别如图７、图８所示。

图７　载体转动横滚角估计

图８　载体转动俯仰角估计

差如表３种算法估计值与真实值的平均误差和均方根误

３所示。

表３　载体转动姿态估计算法对比

（°）

算法平均误差均方根误差互补滤波法

０．０００５０．０２４横滚角梯度下降法０．００１０．１３９模糊卡尔曼

０．０００４０．０２３互补滤波法

０．０００３０．０２５俯仰角

梯度下降法０．０００４０．１４７模糊卡尔曼

０．０００３

０．０１１

　模糊卡尔曼滤波法效果较好　由上述结果可以看出在转动情况下互补滤波和，在快速转动下仍能跟随姿态角变化，梯度下降法虽然能够跟随姿态角变化但整体波动较大。３．３　载体运动实验

将载体进行加速、爬升、转弯、下降及减速等多种运动，采集加速度计和陀螺仪数据，分别使用３种算法进行载体姿态估计，载体运动轨迹如图９所示，３种算法得到的横滚角及俯仰角估计结果分别如图１０、图１１

　第４期所示。

李鲁明等：基于模糊卡尔曼滤波的姿态估计算法

１０５　　

现较大偏差，模糊卡尔曼滤波算法能够根据载体的各图９　载体运动轨迹

图１０　载体运动横滚角估计

图１１　载体运动俯仰角估计

　差如表　３种算法估计值与真实值的平均误差和均方根误４所示。

表４　３种算法估计值与真实值的平均误差和均方根误差

（°）

平均误差

均方根误差互补滤波法

０．６４３２．２９４横滚角

梯度下降法０．１１４０．３９２模糊卡尔曼０．０３８０．０５９互补滤波法

０．３６３２．１５６俯仰角

梯度下降法０．３４２１．６２２模糊卡尔曼

０．０８４

０．１１５

　速度的存在导致姿态估计偏差较静止和转动下增大　由上述结果可以看出当载体运动时由于运动加，

互补滤波法和梯度下降法在运动情况下出现较大偏差，主要由合加速度的变化导致加速度计姿态估计的偏差引起，当载体进行加速、转弯、减速等运动时均出

种运动状态通过模糊控制器自适应地调整量测噪声方差，较好地跟随了载体姿态变化，均方根误差较互补滤波法下降了９４．９８％，相比于梯度下降法降低了９２．９１％。４　结束语

针对载体运动情况下姿态估计算法存在的问题，本文提出基于模糊卡尔曼滤波的载体姿态估计算法，

并给出了详细的算法流程以及模糊控制器各项参数，通过载体静止、转动及运动３组实验对比了３种算法的准确性及实时性，实验表明模糊卡尔曼滤波载体估计算法在静止和转动情况下稍有优势，在运动情况下能够大幅降低载体姿态角误差，同时模糊卡尔曼滤波在各个情况下均具有更好的实时性，为惯性导航速度及位置解算提供精度保证。

参考文献：

［１］　吴春俊仪表技术与传感器，周炳红．基于，２０１３（８）：７６ＭＥＭＳ的无线姿态测量系统－７８．

［Ｊ］．

［２］　ｂａｓｅｄＨＡＤＲＩＡＥ，ＢＥＮＺＩＡＮＥＬ，ＳＥＢＡＡ，ｅｔａｌ．ＳｅｎｓｏｒｓｔｉｍａｔｉｏｎｄａｔａＩｎｔｅｒｎａｔｉｏｎａｌａｎｄｆｕｓｉｏｎｓｔａｂｉｌｉｚａｔｉｏｎ［Ｃ］．ｐｒｏｃｅｅｄｉｎｇｓｕｓｉｎｇｃｏｍｐｌｅｍｅｎｔａｒｙｆｉｌｔｅｒｓ（ＩＣＲＡ），Ｆ１６Ｃｏｎｆｅｒｅｎｃｅ－２１Ｍａｙ２０１６．

ｏｎＲｏｂｏｔｉｃｓｏｆａｎｄｔｈｅｆｏｒａｔｔｉｔｕｄｅｍｏｄｅｌ

２０１６ｅｓ⁃ＡｕｔｏｍａｔｉｏｎＩＥＥＥ［３］　ＷＵＪ，ＺＨＯＵＺ，ＣＨＥＮＪ，ｅｔａｌ．Ｆａｓｔｃｏｍｐｌｅｍｅｎｔａｒｙｆｉｌｔｅｒｆｏｒ

ＳｅｎｓｏｒｓａｔｔｉｔｕｄｅＪｏｕｒｎａｌ，２０１６，１６（１８）：６９９７ｅｓｔｉｍａｔｉｏｎｕｓｉｎｇｌｏｗ⁃ｃｏｓｔＭＡＲＧ－７００７．ｓｅｎｓｏｒｓ［Ｊ］．ＩＥＥＥ

［４］　ｏｎＣＨＥＧＵＩＮＩＩＥＥＥＧｒａｄｉｅｎｔＭ，ＲＵＩＺ４ｔｈＣｏｌｏｍｂｉａｎＤｅｓｃｅｎｔａｌｇｏｒｉｔｈｍ［Ｆ．Ｒｅａｌ⁃ｔｉｍｅＷｏｒｋｓｈｏｐＣ］．ＰｒｏｃｅｅｄｉｎｇｓａｔｔｉｔｕｄｅｅｓｔｉｍａｔｉｏｎｏｎＣｉｒｃｕｉｔｓａｎｄｏｆｔｈｅｂａｓｅｄ

Ｓｙｓｔｅｍｓ２０１２［５］　（ＣＷＣＡＳ），２０１２．

孙金秋的姿态解算算法，游有鹏，傅忠云［Ｊ］．传感技术学报．基于共轭梯度法和互补滤波相结合

，２０１４（４）：５２４－５２８．［６］　ＦＩＬＨＯｅｓｔｉｍａｔｉｏｎＬＣｏｆＱ，ＦＩＬＨＯａｓｉｍｕｌａｔｅｄＪＦＲ，ＳＩＬＶＡｆｌｉｇｈｔａｎｄＮＧＰＳＢＦＤ，ｅｔｐｏｓｉｔｉｏｎｉｎｇａｌ．Ａｔｔｉｔｕｄｅ

Ｋａｌｍａｎｗｉｔｈ

［７］　ｆｅｒｅｎｃｅ吴涛，白茹ｏｎｆｉｌｔｅｒ［Ｃ］．ｐｒｏｃｅｅｄｉｎｇｓＵｎｍａｎｎｅｄ，朱礼尧，等Ａｉｒｃｒａｆｔｏｆ．基于卡尔曼滤波的航姿参考系统Ｓｙｓｔｅｍｓ（ＩＣＵＡＳ），２０１５．ｔｈｅ２０１５ＩｎｔｅｒｎａｔｉｏｎａｌＣｏｎ⁃设计［Ｊ］．传感技术学报，２０１６（４）：５３１－５３５．［８］　角测量研究刘震，王雪梅［Ｊ］．，倪文波中国测试．基于，２０１７（２）：６ＭＥＭＳ传感器的高精度姿态－１２．

［９］　ＤＵＯＮＧｂｙＤＱ，ＳＵＮＪ，ＮＧＵＹＥＮＴ［Ｃ］．ｐｒｏｃｅｅｄｉｎｇｓｕｓｉｎｇＭＥＭＳＩＭＵｏｆｔｈｅｗｉｔｈ２０１６ＦｕｚｚｙＩＥＥＥＴｕｎｅｄＰ，ｅｔａｌ．ＡｔｔｉｔｕｄｅＩｎｔｅｒｎａｔｉｏｎａｌＣｏｍｐｌｅｍｅｎｔａｒｙｅｓｔｉｍａｔｉｏｎ

ＣｏｎｆｅｒｅｎｃｅＦｉｌｔｅｒＥｌｅｃｔｒｏｎｉｃＩｎｆｏｒｍａｔｉｏｎａｎｄＣｏｍｍｕｎｉｃａｔｉｏｎｏｎ

ＩＣＴ），２０１６．

Ｔｅｃｈｎｏｌｏｇｙ（ＩＣＥ⁃（下转第１１０页）

　１１０　ＩｎｓｔｒｕｍｅｎｔＴｅｃｈｎｉｑｕｅａｎｄＳｅｎｓｏｒ

Ａｐｒ􀆰２０１９　

表１　处理前后不同粘接试件非线性系数的均方差

固化剂含量／％１２２５５０７５固化剂含量／％１２２５５０７５

试件１０．１８４３０．１４１７０．１２７００．１６３８试件１０．１８５６０．１５２８０．１３０９０．１６６７

处理前的相对非线性系数试件２０．１９７３０．１４１９０．１３７９０．１７８４

试件３０．１８３２０．１６１１０．１２７４０．１６４３

均值０．１８８７０．１５１９０．１３２１０．１６９８

均方差０．００８０．００９０．００５０．００７

次谐波幅值计算得到的非线性系数能准确表征试件的粘接强度，提高了非线性超声检测方法对缺陷的表征能力。

参考文献：

［１］　税国双，汪越胜，曲建民，等．材料力学性能退化的超声无

损检测与评价［Ｊ］．力学进展，２００５，３５（１）：５２－６８．展［Ｊ］．机械工程学报，２０１１，０８：２－１１．

［２］　周正干，刘斯明．非线性无损检测技术的研究、应用和发［３］　ＭＡＴＺＶ，ＳＭＩＤＲ，ＳＴＡＲＭＡＮＳ，ｅｔａｌ．Ｓｉｇｎａｌ⁃ｔｏ⁃ｎｏｉｓｅｒａｔｉｏ

ｅｎｈａｎｃｅｍｅｎｔｂａｓｅｄｏｎｗａｖｅｌｅｔｆｉｌｔｅｒｉｎｇｉｎｕｌｔｒａｓｏｎｉｃｔｅｓｔｉｎｇ［Ｊ］．Ｕｌｔｒａｓｏｎｉｃｓ，２００９，４９（８）：７５２－７５９．

处理后的相对非线性系数试件２０．１８４４０．１４２１０．１２７４０．１６４２

试件３０．１８４７０．１４１９０．１２７４０．１６４３

均值０．１８４５０．１４１９０．１２７３０．１６４１

均方差０．０００２０．０００２０．０００２０．０００２

［４］　安志武，王小民，毛捷，等．粘接件拉伸强度的超声检测实

验研究［Ｊ］．声学学报，２０１１（４）：３８４－３８８．

［５］　陈斌，杨平，施克仁．Ｈｉｌｂｅｒｔ⁃Ｈｕａｎｇ变换在非线性超声无

（８）：１３６９－１３７２．

　　试件非线性系数随固化剂含量变化规律如图９所示。随着固化剂含量从１２％增加至７５％，β先减小后增大。它反映了有限幅度超声波在材料中传播时应力－应变非线性效应的程度，与文献［１２］的研究结论“试件的粘接强度随固化剂含量增加呈现先增大后减小”的规律一致。图９中不同粘接试件的非线性系数偏差较小，且变化规律一致说明该信号处理方法有效，提高了非线性超声检测结果的准确性和鲁棒性。

损检测中的应用［Ｊ］．清华大学学报（自然科学版），２００６

［６］　苏文胜，王奉涛，朱泓，等．双树复小波域隐Ｍａｒｋｏｖ树模

２０１１（６）：４７－５２．

型降噪及在机械故障诊断中的应用［Ｊ］．振动与冲击，

［７］　ＣＥＬＩＫＴ，ＯＺＫＡＲＡＭＡＮＬＨ，ＤＥＭＩＲＥＬＨ．Ｆａｃｉａｌｆｅａｔｕｒｅ

ｅｘｔｒａｃｔｉｏｎｕｓｉｎｇｃｏｍｐｌｅｘｄａｕｌ⁃ｔｒｅｅｗａｖｅｌｅｔｔｒａｎｓｆｏｒｍ［Ｊ］．ＣｏｍｐｕｔｅｒＶｅｒｓｉｏｎａｎｄＩｍａｇｅＵｎｄｅｒｓｔａｎｄｉｎｇ，２００８，１１１（２）：

［８］　胥永刚，孟志鹏，赵国亮．基于双树复小波变换的轴承复

合故障诊断研究［Ｊ］．仪器仪表学报，２０１４（２）：４４７－４５２．噪研究［Ｊ］．仪器仪表学报，２０１３（５）：１１６０－１１６６．［９］　王芳，季忠，彭承琳．基于双树复小波变换的心电信号去［１０］　ＸＵＹＳ，ＷＥＡＶＥＲＪＢ，ＨＥＡＬＹＤＭｅｔａｌ．Ｗａｖｅｌｅｔｔｒａｎｓ⁃

２２９－２４６．

ｆｏｒｍｄｏｍａｉｎｆｉｌｔｅｒｓ：ａｓｐａｔｉａｌｌｙｓｅｌｅｃｔｉｖｅｎｏｉｓｅｆｉｌｔｒａｔｉｏｎ

图９　非线性系数随固化剂含量变化规律

４　结论

本文提出了基于双树复小波系数层间相关性结合软阈值滤波算法的非线性二次谐波提取方法。利用双树复小波变换近似的平移不变性和抗混叠等优点，将非线性超声信号分解为基频、二倍频等不同中心频率的信号分量，根据小波系数层间相关性及软阈值算法非线性超声信号中提取出中基波和二次谐波信号。研究结果表明，该算法滤波效果良好，提取的二（上接第１０５页）

［１１］　ＤＯＮＯＨＯＤＬ．Ｄｅ⁃ｎｏｉｓｉｎｇｂｙｓｏｆｔ⁃ｔｈｒｅｓｈｏｌｄｉｎｇ［Ｊ］．ＩＥＥＥ［１２］　刘一波，刘伟，孙越邈，等．超硬磨具用环氧树脂胶粘剂

（４）：６－９．

ＴｒａｎｓｏｎＩｎｆｏｒｍａｔｉｏｎＴｈｅｏｒｙ，１９９５，３：６１３－６２７．

１９９４，３（６）：７４７－７５８．

ｔｅｃｈｎｉｑｕｅ［Ｃ］．ＩＥＥＥＴｒａｎｓａｃｔｉｏｎｏｎＩｍａｇｅＰｒｏｃｅｓｓｉｎｇ，

粘接强度影响因素的研究［Ｊ］．超硬材料工程，２００９，２１

作者简介：江念（１９８８—），工程师，博士，主要研究领域为信号

与信息处理。Ｅ⁃ｍａｉｌ：ｊｉａｎｇ＿ｎｉａｎ＠ｙｅａｈ．ｎｅｔ

王召巴（１９６７—），教授，博士生导师，主要研究领域为超声无损检测。Ｅ⁃ｍａｉｌ：ｗａｎｇｚｂ＠ｎｕｃ．ｅｄｕ．ｃｎ

作者简介：李鲁明（１９９３—），硕士研究生，主要研究惯性导航系

统，组合导航信息融合算法。Ｅ⁃ｍａｉｌ：ｌｕｍｉｎｇ．ｌｉ＠ｍａｉｌ．ｓｉｍ．ａｃ．ｃｎ

赵鲁阳（１９７０—），副研究员，博士，主要研究无线传感网络应用，围界防入侵，数字信号处理。Ｅ⁃ｍａｉｌ：ｚｈａｏｌｕｙａｎｇ＠ｍａｉｌ．ｓｉｍ．ａｃ．ｃｎ

［１０］　陈伟．基于四元数和卡尔曼滤波的姿态角估计算法研究

与应用［Ｄ］．秦皇岛：燕山大学，２０１５．防御技术，２００８（３）：５１－５５．

［１１］　葛新成，胡永霞．模糊控制的现状与发展概述［Ｊ］．现代