2. 认知无线电与信息处理教育部重点实验室, 广西 桂林 541004
2. Key Laboratory of Cognitive Radio and Information Processing of Ministry of Education, Guilin Guangxi 541004, China
近年来,低成本飞行器在物流业、环境监测、地质勘测及军事侦察等领域的应用越来越广泛,作为控制低成本飞行器的关键技术的姿态解算算法成为当前热点研究之一[1]。陀螺仪、加速度计和磁场计等低成本惯性测量单元输出的测量数据是飞行器载体姿态解算的原始数据,但是受到器件自身测量误差、温度和各种噪声等诸多因素的影响,姿态解算往往误差较大[2-3]。通过信息融合算法将多种传感器的数据进行融合往往能取长补短,解算出更精确的姿态角[4]。
在多传感器姿态解算的信息融合方案上,国内外研究者们提出了多种算法和改进策略。贾瑞才[5]、吴涛等[6]提出利用扩展卡尔曼滤波 (Extended Kalman Filter, EKF) 实现对陀螺仪随机漂移误差的补偿,提高了姿态解算精度,但忽略了加速度计的高频干扰问题,及EKF算法固有的线性化、截断误差和发散问题。为克服EKF固有的缺陷,Pan等[7]实现基于无迹卡尔曼滤波 (Unscented Kalman Filter, UKF) 的姿态估算算法,取得了比EKF更高的稳定性、解算精度及收敛速度; 而Tang等[8]表示应用在航天飞行器姿态解算上的容积卡尔曼滤波 (Cubature Kalman Filter, CKF) 算法估算性能要比UKF好, 但在低成本飞行器中,基于卡尔曼滤波的姿态算法存在计算量大、硬件要求高、难实现等缺点。Euston等[9]、Wu等[10]在低成本无人机上利用互补滤波器 (Complementary Filter, CF) 实现载体姿态估算,充分体现了CF原理简单、计算量小、容易实现、收敛稳定等优点,但是未考虑因加速度计不能区分运动加速度与重力加速度而产生的CF算法解算误差。Wu等[11]、阎世梁等[12]在应用CF解算姿态时通过计算三轴加速度计的输出的合加速度的大小来判断载体的运动状态,然后根据载体运动状态调节CF的转换频率,提高了基于CF的姿态解算精度,但是存在PI参数离散和高动态下互补滤波作用大大降低等问题。孙金秋等[13]设计了PI参数能自适应的CF姿态解算算法,通过合加速度大小来连续调节PI参数,实现连续使姿态估算误差小于1.47°,但存在对姿态三角无区分地使用同一个PI参数调节的问题,依然有高动态下互补滤波作用大大降低的问题。
本文提出自适应限幅滤波与互补滤波组合滤波的策略,其中自适应限幅滤波能较准确区分三轴重力加速度分量和三轴运动加速度分量,同时还保留了互补滤波器的计算量小、收敛稳定及易于在低成本飞行器中实现的优点,使基于重力加速度输入的互补滤波算法也能在载体带有运动加速度时解算出更精确的姿态角。
1 四元数法姿态解算设由运载载体的机体轴确定的坐标系为b,惯性导航系统所采用的导航坐标系为n,其中n系为北、西、天的地理坐标系,
运载体的姿态矩阵为由b到n系的坐标转换矩阵Cnb。由于n系和b系均为直角坐标系,两个坐标系间的空间角位置关系可以理解为刚体的定点转动,它们的转动夹角为载体航向的姿态角[14]。记载体的横滚角
载体坐标系与导航坐标系的转动关系可以用四元数标记为:
$Q={{q}_{0}}+{{q}_{1}}\text{i}+{{q}_{2}}\text{j}+{{q}_{3}}\text{k}$ | (1) |
四元数确定出b系到n系的坐标变换矩阵为:
$\mathit{\boldsymbol{C}}_{n}^{b}=\left[ \begin{matrix} 2q_{0}^{2}+2q_{1}^{2}-1 & 2{{q}_{1}}{{q}_{2}}+2{{q}_{0}}{{q}_{3}} & 2{{q}_{1}}{{q}_{3}}-2{{q}_{0}}{{q}_{2}} \\ 2{{q}_{1}}{{q}_{2}}-2{{q}_{0}}{{q}_{3}} & 2q_{0}^{2}+2q_{2}^{2}-1 & 2{{q}_{1}}{{q}_{2}}+2{{q}_{0}}{{q}_{3}} \\ 2{{q}_{1}}{{q}_{3}}+2{{q}_{0}}{{q}_{2}} & 2{{q}_{2}}{{q}_{3}}-2{{q}_{0}}{{q}_{1}} & 2q_{0}^{2}+2q_{3}^{2}-1 \\ \end{matrix} \right]$ | (2) |
n系与b系基本旋转对应的坐标变换矩阵记为:
$\begin{align} & \mathit{\boldsymbol{C}}_{n}^{b}=\left[ \begin{matrix} {{T}_{11}} & {{T}_{12}} & {{T}_{13}} \\ {{T}_{21}} & {{T}_{22}} & {{T}_{23}} \\ {{T}_{31}} & {{T}_{32}} & {{T}_{33}} \\ \end{matrix} \right]= \\ & \left[ \begin{matrix} \cos \theta \cos \psi & \cos \theta \sin \psi & -\sin \theta \\ \sin \phi \sin \theta \cos \psi -\cos \phi \sin \psi & \sin \phi \sin \theta \sin \psi +\cos \phi \cos \psi & \sin \phi \cos \theta \\ \cos \phi \sin \theta \cos \psi +\sin \phi \sin \psi & \cos \phi \sin \theta \sin \psi -\sin \phi \cos \psi & \cos \phi \cos \theta \\ \end{matrix} \right] \\ \end{align}$ | (3) |
对比式 (2)、(3) 可得:
$\left\{ \begin{array}{*{35}{l}} \phi ={{\tan }^{-1}}\left( {{T}_{23}}/{{T}_{33}} \right)\text{ , } & \phi \in \left[ -{{180}^{{}^\circ }},{{180}^{{}^\circ }} \right] \\ \theta ={{\sin }^{-1}}\left( -{{T}_{13}} \right)\text{ ,} & \theta \in \left[ -{{90}^{{}^\circ }},{{90}^{{}^\circ }} \right] \\ \psi ={{\tan }^{-1}}\left( {{T}_{12}}/{{T}_{11}} \right)\text{ ,} & \psi \in \left[ -{{180}^{{}^\circ }},{{180}^{{}^\circ }} \right] \\ \end{array} \right.$ | (4) |
可见载体的姿态更新实质上是如何计算四元数,记b系中xyz三轴旋转的角速率向量[ωx ωy ωz]T为ω,四元数向量[q0 q1 q2 q3]T为q,则更新四元数的微分方程为:
$\left[ {\begin{array}{*{20}{c}} {{{\dot q}_0}}\\ {{{\dot q}_1}}\\ {{{\dot q}_2}}\\ {{{\dot q}_3}} \end{array}} \right] = \frac{1}{2}\left[ {\begin{array}{*{20}{c}} 0&{ - {\omega _x}}&{ - {\omega _y}}&{ - {\omega _z}}\\ {{\omega _x}}&0&{{\omega _z}}&{ - {\omega _y}}\\ {{\omega _y}}&{ - {\omega _z}}&0&{{\omega _x}}\\ {{\omega _z}}&{{\omega _y}}&{ - {\omega _x}}&0 \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {{q_0}}\\ {{q_1}}\\ {{q_2}}\\ {{q_3}} \end{array}} \right]{\rm{ = }}\frac{{\rm{1}}}{{\rm{2}}}\;\mathit{\boldsymbol{\omega }} \times {\bf{q}}$ | (5) |
其中,式 (5) 中的ω由式 (6) 求取:
$\mathit{\boldsymbol{\omega }}{\rm{ = }}\mathit{\boldsymbol{\omega }}_{ib}^b - \mathit{\boldsymbol{C}}_n^b\left( {\mathit{\boldsymbol{\omega }}_{ie}^n{\rm{ + }}\mathit{\boldsymbol{\omega }}_{en}^n} \right)$ | (6) |
其中:ωibb为陀螺仪输出的角速率,Cnb为最新姿态矩阵,ωien为地球自转速率,ωenn为位置速率。由于地球自转角速率为15.041 08°/h (已知常量),也即0.004 178°/s,短期低速影响较小,一般在计算时可以忽略,近似为ω=ωibb,即陀螺仪的输出[14]。
2 基于互补滤波器的四元数法姿态解算短时间内,陀螺仪提供高精度的姿态结算数据,而加速度计和磁场计精度较差;长时间内,陀螺仪因漂移的影响,产生积分误差,姿态精度随时间增长而下降,但是加速度计和磁场计的测量误差不会随时间增加。因此,根据两者在频域上的特性的互补,可以采用互补滤波器融合这三种传感器的数据,提高姿态解算的精度和系统的动态特性[13]。
记姿态角为
$\mathit{\boldsymbol{\tilde \eta }}{\rm{ = }}\mathit{\boldsymbol{\eta }} + \Delta {\mathit{\boldsymbol{\eta }}_{\rm{H}}}{H_{\rm{L}}}\left( s \right) + \Delta {\mathit{\boldsymbol{\eta }}_{\rm{L}}}{H_{\rm{H}}}\left( s \right) \approx \mathit{\boldsymbol{\eta }}$ | (7) |
取C(s)=kP+kI/s,kP为低通滤波器与高通滤波器的转接频率,引入积分环节kI/s是为了补偿陀螺仪的漂移误差[15]。在四元数法姿态解算算法流程中应用互补滤波器时,算法流程为图 1。
最新的姿态矩阵Cnb中第三列即ξg=[T13 T23 T33]T代表了重力在b系归一化后的三个坐标轴分量,将加速度计输出的重力加速度g归一化后与ξg叉乘作为重力误差向量eg。姿态矩阵中ξm=[T11 T120]T代表地球磁场在b系归一化后的北天两个方向的分量,将磁场计输出的地球磁场m归一化后与ξm叉乘作为磁场误差向量em。向量误差e表示将估计值得到的向量旋转成与加速度、磁场计输出得到的向量平行所需要的旋转方向和旋转量,所以误差向量表示了四元数估计的误差。向量误差e经过PI运算后,与陀螺仪的输出ω作互补滤波融合得到包含有重力信息和磁场信息的角速度向量
由基于互补滤波器的姿态解算过程可见,跟卡尔曼滤波算法相比,互补滤波法有计算量少、容易实现的优点。
但是,图中的要求加速度计输出的是重力加速度g在b系的三轴分量时,才能使更新四元数得到高精度解算。实际上,在飞行器动态飞行过程中,往往受到的是重力加速度和运动加速度的合力,而加速计无法区分这二者。这个原因当飞行器作非匀速飞行时,即运动加速度非零时,基于互补滤波器的姿态解算误差随运动加速度的误差增大而增大。
3 自适应限幅滤波与互补滤波组合给限幅滤波器设置一个阀值,通过限制输入的幅值可消除无用或者有害幅值。于是可以设计一个自适应限幅滤波器,使其限幅阀值始终跟随到略大于重力加速度的大小的位置,在飞行器非匀速飞行时最大化地滤除运动加速度。设计的自适应限幅滤波器结构如图 2。
记归一化的三轴加速度增量为ΔaN,N表示当前时刻输出,则:
$\Delta {{\mathit{\boldsymbol{\bar a}}}_N}{\rm{ = }}\frac{\mathit{\boldsymbol{a}}}{{\left\| \mathit{\boldsymbol{a}} \right\|}} - {\xi _g}$ | (8) |
记ρ为设计的限幅滤波器的自适应限幅阀值,ρ的生成规则为:
$\mathit{\boldsymbol{\rho }} = \mathit{\boldsymbol{\omega }} \cdot \Delta t + \frac{1}{{N - 1}}\sum\limits_{n = 1}^{N - 1} {\Delta {{\mathit{\boldsymbol{\bar a}}}_n}} $ | (9) |
若η0表示姿态角任一时刻的角度值,由于低成本飞行器的输出频率高,旋转角速度较小,所以ω·Δt也很小,那么有:
$\begin{array}{l} \left| {\frac{\mathit{\boldsymbol{g}}}{{\left\| \mathit{\boldsymbol{g}} \right\|}}\left[ {{\rm{sin(}}\mathit{\boldsymbol{\omega }} \cdot \Delta t + {\eta _0}){\rm{ - sin}}{\eta _0}} \right]} \right| \le \\ \left| {\frac{\mathit{\boldsymbol{g}}}{{\left\| \mathit{\boldsymbol{g}} \right\|}}\left[ {{\rm{sin(}}\mathit{\boldsymbol{\omega }} \cdot \Delta t + 0){\rm{ - sin}}\;0} \right]} \right| < \left| {\mathit{\boldsymbol{\omega }} \cdot \Delta t} \right| \end{array}$ | (10) |
其中:ω·Δt代表重力加速度增量的最大可能值,而
获得限幅滤波器的自适应限幅阀值ρ后,对ΔaN进行限幅滤波,即:
$\left\{ {\begin{array}{*{20}{l}} {\Delta \mathit{\boldsymbol{\bar g}} = \Delta {{\mathit{\boldsymbol{\bar a}}}_N}{\rm{,}}}&{\left| {\Delta {{\mathit{\boldsymbol{\bar a}}}_N}} \right| < \left| \mathit{\boldsymbol{\rho }} \right|}\\ {\Delta \mathit{\boldsymbol{\bar g}} = \mathit{\boldsymbol{\rho }}{\rm{,}}}&{\left| {\Delta {{\mathit{\boldsymbol{\bar a}}}_N}} \right| \ge \left| \mathit{\boldsymbol{\rho }} \right|} \end{array}} \right.$ | (11) |
其中Δg为自适应限幅滤波的输出。
最后,通过ξg×Δ获得重力误差向量eg作为图 1互补滤波的误差向量eg完成进一步的数据融合和姿态解算。
4 实验测试及分析 4.1 实验平台算法设计验证及分析平台为Matlab R2011b。用于实验测试的软件及装置如图 3所示。
航姿参考系统和数据采集选用微型航姿参考系统MAHRS 3DM-E10A,该系统通过RS-232串行接口方便连接,提供100 Hz的实时三轴惯性原始数据和参考姿态角。3DM-E10A提供的横滚角、俯仰角静态误差±0.1°,动态误差±1.0°;航向角静态误差±0.5°,动态误差±2.0°。
如图 3(a)所示,双轴电动转台使用TT-3DM-2E-10 Version 1.2, 具备串行输出实时角度位置数据、串行输入控制指令及旋转角位置动态跟踪测量与控制等优点,主轴与俯仰轴转角范围连续无限、角度位置综合测量精度为±0.08°、控制到位分辨率为±0.01°及速率范围为0.1°/s~300°/s,满足实验测试算法需求。
图 3(b)为TT-3DM-2E-10配套的转台控制Labview软件。图 3(c)为作者开发的用于通过串口记录理论姿态角及3DM-E10A系统输出的9个原始数据的Labview软件,记录频率与3DM-E10A输出频率同样。
4.2 静态及低动态测试静态及低动态测试数据采集方法为:通过图 3(b)所示控制转台的Labview开发的软件通过串口控制如图 3(a)所示双轴转台转动,如图 3(c)所示的Labview开发的软件通过串口记录理论姿态角及惯性元件输出9个原始数据。
利用自适应限幅滤波与互补滤波器组合滤波算法解算的静态及低动态下载体的姿态角解算及与航姿参考系统的误差如图 4。
由图 4可见,在静态状态下,基于组合滤波算法解算的姿态角误差在±0.2°内;在低动态状态下,误差在±0.7°内。基于典型互补滤波算法的解算结果,在静态下的误差与基于组合滤波算法解算的姿态角误差基本一致,但是低动态下,特别是角速率比较大时,解算最大误差可达±2°。
静态及低动态测试表明,在载体静态下,该算法保留了典型互补滤波算法的优点;在低动态下,在载体角速率较大时,自适应限幅滤波也发挥作用,有效减弱了运动加速度带来的影响,提高了姿态解算的精度。
4.3 快速滑动测试为了验证算法消除运动加速度对互补滤波法姿态解算影响的效果,将3DM-E10A置于一个接近水平面的平面上滑动,并通过串口采集原始数据。其中该实验用的平面为固定钢化玻璃且几乎没有弹性,经反复测量横滚角约-0.3°,俯仰角约-0.5°,由于测试不同算法时使用的是同一个滑动过程采集的数据,所以不会对对比实验产生影响,可视为理论上的0°。
图 5为滑动时,三轴加速度计输出的重力加速度和运动加速度的合加速度。图 6为基于组合滤波、典型互补滤波及经典卡尔曼滤波这三个姿态解算算法的比较。
结合图 5和图 6可见:理论上均为0°的横滚角和俯仰角由于较大的运动加速度的对互补滤波算法解算的影响产生了约6°的误差,对经典卡尔曼滤波的解算产生了7°的影响,而基于组合滤波基本不受影响。基于互补滤波解算的航向角在停止滑动时也未能达到收敛的稳定值,还需要约200 ms后才能稳定,经典卡尔曼滤波收敛速度较快;基于组合滤波收敛最快。
快速滑动测试表明,在运动加速度突变或较大时,基于自适应限幅滤波与互补滤波器组合的姿态解算算法极大地减少了运动加速度对姿态解算影响,性能优于基于典型的互补滤波算法及经典卡尔曼滤波的姿态解算。
5 结语本文分析了基于互补滤波器的姿态解算原理和实现方法,指出了运动加速度对姿态解算的影响,并提出了根据陀螺仪输出和运动状态的判断来设计一个具有自适应限幅门阀的限幅滤波器的方法,将三轴加速度计的输出通过这个限幅滤波器后再用互补滤波算法解算出载体的姿态角。测试表明该算法在匀速或者较高动态状态下,都能达到较高的解算精度,提高了姿态解算的精度。
[1] | 傅忠云, 刘文波, 孙金秋, 等. 自适应混合滤波算法在微型飞行器姿态估计中的应用[J]. 传感技术学报, 2014, 27(5): 698-703. ( FU Z Y, LIU W B, SUN J Q, et al. Application of adaptive hybrid filter algorithm in the estimation of the micro air vehicle attitude[J]. Chinese Journal of Sensors and Actuators, 2014, 27(5): 698-703. ) |
[2] | 金光明, 张国良, 陈林鹏, 等. MEMS陀螺仪静态漂移模型与滤波方法研究[J]. 传感器与微系统, 2007, 26(11): 48-50. ( JIN G M, ZHANG G L, CHEN L P, et al. Research on filter method and model of MEMS gyro static drift[J]. Transducer and Microsystem Technologies, 2007, 26(11): 48-50. doi: 10.3969/j.issn.1000-9787.2007.11.016 ) |
[3] | DI L, FROMM T, CHEN Y Q. A data fusion system for attitude estimation of low-cost miniature UAVs[J]. Journal of Intelligent & Robotic Systems, 2012, 65(1): 621-635. |
[4] | 彭孝东, 张铁民, 李继宇, 等. 基于传感器校正与融合的农用小型无人机姿态估计算法[J]. 自动化学报, 2015, 41(4): 854-860. ( PENG X D, ZHANG T M, LI J Y, et al. Attitude estimation algorithm of agricultural small-UAV based on sensors fusion and calibration[J]. Acta Automatica Sinica, 2015, 41(4): 854-860. ) |
[5] | 贾瑞才. 基于四元数EKF的低成本MEMS姿态估计算法[J]. 传感技术学报, 2014, 27(1): 90-95. ( JIA R C. Attitude estimation algorithm for low cost MEMS based on quaternion EKF[J]. Chinese Journal of Sensors and Actuators, 2014, 27(1): 90-95. ) |
[6] | 吴涛, 白茹, 朱礼尧, 等. 基于卡尔曼滤波的航姿参考系统设计[J]. 传感技术学报, 2016, 29(4): 531-535. ( WU T, BAI R, ZHU L Y, et al. Design of AHRS based on Kalman filter[J]. Chinese Journal of Sensors and Actuators, 2016, 29(4): 531-535. ) |
[7] | PAN Y, SONG P, LI K, et al. Attitude estimation of miniature unmanned helicopter using unscented Kalman filter[C]//Proceedings of the 2011 International Conference on Transportation, Mechanical, and Electrical Engineering. Piscataway, NJ:IEEE, 2011:1548-1551. |
[8] | TANG X, WEI J, CHEN K. Square-root adaptive cubature Kalman filter with application to spacecraft attitude estimation[C]//Proceedings of the 201215th International Conference on International Conference on Information Fusion. Piscataway, NJ:IEEE, 2012:1406-1412. |
[9] | EUSTON M, COOTE P, MAHONY R, et al. A complementary filter for attitude estimation of a fixed-wing UAV[C]//Proceedings of the 2008 IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway, NJ:IEEE, 2008:340-345. |
[10] | WU J, ZHOU Z, CHEN J, et al. Fast complementary filter for attitude estimation using low-cost MARG sensors[J]. IEEE Sensors Journal, 2016, 16(18): 6997-7007. doi: 10.1109/JSEN.2016.2589660 |
[11] | WU Z Y, ZENG Y J, SHAO H X, et al. Betterment of attitude estimation based on complementary filter applied in drumstick[C]//Proceedings of the 2012 World Congress on Information and Communication Technologies. Piscataway, NJ:IEEE, 2012:1162-1165. |
[12] | 阎世梁, 王银玲, 张华. 基于改进互补滤波器的低成本微小飞行器姿态估计方法[J]. 计算机应用, 2013, 33(7): 2078-2082. ( YAN S L, WANG Y L, ZHANG H. Improved complementary filter for attitude estimation of micro air vehicles using low-cost inertial measurement units[J]. Journal of Computer Applications, 2013, 33(7): 2078-2082. ) |
[13] | 孙金秋, 游有鹏, 傅忠云. 基于自适应显式互补滤波的姿态解算方法[J]. 测控技术, 2015, 34(4): 24-27. ( SUN J Q, YOU Y P, FU Z Y. Attitude estimation based on adaptive explicit complementary filter[J]. Measurement & Control Technology, 2015, 34(4): 24-27. ) |
[14] | 秦永元. 惯性导航[M]. 2版.北京: 科学出版社, 2014 : 244 -260. ( QIN Y Y. Inertial Navigation[M]. 2nd ed. Beijing: Science Press, 2014 : 244 -260. ) |
[15] | 梁延德, 程敏, 何福本, 等. 基于互补滤波器的四旋翼飞行器姿态解算[J]. 传感器与微系统, 2011, 30(11): 56-58. ( LIANG Y D, CHENG M, HE F B, et al. Attitude estimation of a quad-rotor aircraft based on complementary filter[J]. Transducer and Microsystem Technologies, 2011, 30(11): 56-58. doi: 10.3969/j.issn.1000-9787.2011.11.017 ) |