惯性传感器和互补滤波器在姿态估计中的应用

兰建军，谭力弓，朴 亨，王 鹏

LAN Jian-jun, TAN Li-gong, PIAO Heng, WANG Peng

（东北电力大学 自动化工程学院，吉林 132012）

0 引言

机器人的姿态控制研究对于揭示飞行器等系统的控制原理和方法具有重要的理论和实际应用意义[1]。姿态检测是控制机器人在未知环境空间运动的关键问题，通过融合各种传感器的数据来获取机器人的运动姿态，进而实现对机器人的运动路径规划。机器人的导航和控制是利用各种传感器（如激光、视觉等传感器）数据和数字信号处理技术的方法实现，而测量数据基本都是通过惯性传感器来获取的，因此惯性测量方法作为一种首选的方案得到广泛的应用[2,3]。目前惯性传感器主要包含三轴加速度计、三轴角速率计和三轴磁场计等，通过融合这些传感器的数据实现机器人的姿态预估。

加速度计和陀螺仪是惯性测量中最常用的传感器，鉴于陀螺仪和加速度的频率响应特性，单独使用独立的传感器进行姿态检测，都具有较大的误差[4]。因此，在实际应用研究中，融合陀螺仪和加速度计传感器数据进行姿态预估的方案得到许多研究者的青睐[5,6]。

常用的数据融合算法有卡尔曼、互补滤波算法，本次研究在现有互补滤波算法的基础上，对互补滤波算法进行改进，提出一种自适应互补滤波算法，算法可实现互补滤波器参数自动调节。解决了噪声干扰与姿态最优估计问题，并在双轮直立行走机器人装置上进行了实验验证，给出了实验测试数据。

1 姿态估计原理

1.1 姿态估计

惯性坐标系中把X、Y、Z方向定义成北、东、天方向，简称北东天坐标系，绕惯性坐标系X、Y、Z三轴旋转的角度分别定义为横滚角（φ）、俯仰角（θ）和航向角（ψ）。旋转物体的方向通常用物体坐标系描述，运动系统的旋转角度通常以欧拉角的形式描述。两个坐标系之间的转换关系如图1所示。利用惯性坐标系三维坐标空间Cn和相对于三维坐标空间的3×3转换矩阵来描述物体的姿态变化。矩阵的列对应于基准空间的三轴坐标 X，Y ，Z ，矩阵的行对应于转移后的坐标空间xˆ，，，二者转换关系可用通过式（1）来描述。

图1 惯性坐标系和物体坐标系下的欧拉角构建示意图

1.2 姿态解算

传统的姿态测控方案通常采用高精度航姿参考系统（AHRS-Attitude and Heading Reference System），其虽然具有较高的测量精度，但其价格成本较高。随着微机电系统（MEMS）技术的发展，出现了许多性价比较高的中低精度的加速度计和陀螺仪。如果用加速度计测出重力加速度后，就可利用式（1）通过投影向量来解算欧拉角，但是该方案需要准确地进行转移矩阵的实时更新[7]，计算和迭代量较大。采用加速度计进行惯性测量时，如果忽略其他加速度影响，可直接利用式（2）通过重力加速度信息计算姿态[8]。gx，gy，gz分别为x轴、y轴、z轴方向上的重力加速度分量；θ为仰俯角，φ为横滚角。

单独使用加速度计虽然也可以实现姿态估计，但是加速度传感器受动态加速度变化影响较大，一旦受外力干扰或加速度发生变化时，姿态测量误差较大，因此采用单一加速度传感器进行物体姿态检测的方案采用较少。陀螺仪以其动态性能好、不受加速度变化影响的优点，可以有效解决加速度变化的影响，但是陀螺仪存在累积漂移误差，并且温漂现象较为明显，静态性能较差。因此，综合采用加速度计和陀螺仪的测量方法是目前低成本惯性测量中使用最为广泛的方法[9]。

2 互补滤波器设计

2.1 设计原理

互补滤波算法的目的就是综合加速度计和陀螺仪各自的频率响应优势，从频域角度对两个传感器数据进行融合，以减小测量和估计的误差。为了获取传感器的频率响应范围，对研究采用的陀螺仪和加速度计分别进行了频率响应测试。通过图2的曲线数据表明，加速度计在低频范围时表现出较好的动态响应，当频率超过2.35rad/s时出现了偏离现象。陀螺仪在低频范围时，测量数据误差较大，当频率超过3.48 rad/s时对角度跟踪的质量逐渐变好。

图2 传感器动态响应曲线

尽管两个传感器无法在所有频率范围中都有良好的动态响应，但是两个传感器的动态响应频率覆盖了整个频率范围。选用从频率域来处理噪声的互补滤波算法具有很大的优势，互补滤波器的基本原理框图如图3所示。

图3 互补滤波器原理框图

其中，u1，u2分别为传感器测量中引入的高频和低频噪声，加速度计中的高频噪声u1用低通滤波器 F1(s)消除，陀螺仪中的低频噪声u2用高通滤波器 F2(s)消除。为了充分发挥二者在各自频率范围的动态响应优势，设计了如图4所示的互补滤波观测器，两个滤波器的传递函数计算如下。

图4 互补滤波器

两个滤波器在频域上具有互补性，选用的滤波器传递函数满足 F1(s)1+F2(s)=1，因此角度估计值是陀螺仪积分后高通滤波和加速度计低通滤波后的数据融合，互补滤波器设计的关键在于高、低通滤波器转接频率的确定。

2.2 自适应互补滤波器

文献[10]提出一种通过选定互补滤波器的权重系数来确定互补滤波器的转接频率的方法，计算公式如式（5）所示，系数的确定是依据时间常数和采样时间来选定的，该方法计算简单、计算量小。

其中，agy为陀螺仪权重系数，aac为加速度计权重系数，满足agy+aac=1。

由于权重系数的固定，当被测对象加速度发生变化或者有外力干扰的情况时，估计误差较大。因此，自适应互补滤波器设计的关键问题在于根据测量对象的运行状态，自行选择合适的权重系数进行互补滤波，以获得最佳的姿态估计。根据图2所示的传感器动态响应曲线，低频时加速度计测量值更为可靠，因此加速度权重系数可以增加；高频时，陀螺仪测量效果好，可以加大陀螺仪权重系数。依据实验曲线中获取的关键转接频率点，可以在两个转点频率范围内，根据物体实际转动频率来自动调整权重系数，权重系数自适应算法如式（6）所示：

n表示采样更新次序，λ1，2λ都为大于0的常数，取值和连续两次采样的角度差和采样时间有关，m值为在0～0.92范围内变化的变量，根据连续两次采样的角度差计算得出。

3 实验与结果分析

为了验证方案的可行性，设计了双轮直立行走机器人控制装置对自适应互补滤波器进行效果验证。惯性传感器分别选用MMA7361加速度计（测量范围：±6g）和ENC-03（测量范围：±300(deg/s)）陀螺仪，采用频率200Hz。以俯仰角（θ）测量为例，分别进行了静态和动态测试。

静态测试实验时，将双轮直立行走机器人保持固定的17°倾斜角度静止于地面，加速度计、陀螺仪和自适应互补滤波器输出的仰俯角测量曲线如图5(a)所示。动态测试时，控制机器人按预定的17°倾角运动，测试曲线如图5(b)所示。实验数据表明，通过动态调整加速度计和陀螺仪的权重系数，能够获得良好的互补滤波效果，对角度跟踪和预估准确。

4 结论

本文通过加速度计和陀螺仪传感器设计了运动物体姿态检测系统，通过自适应互补滤波器对加速度计和陀螺仪数据进行融合，能够根据运动物体状态自适应调整互补滤波器的交接频率，有效消除加速度变化和外力干扰引起的姿态预估误差。在双轮直立行走机器人上分别进行的静态和动态实验，实验数据表明，利用自适应互补滤波算法可以准确可靠的获取机器人的姿态信息，系统的姿态预估精度在±2°内，可实现较高精度的运动物体姿态预估。

图5 自适应互补滤波器测试曲线

[1]EI-Hawwary M I,Elshafei A L, Emara H M.Adaptive fuzzy control of the inverted pendulum problem[J].Transactions on Control Systems Technology,2009,14(6)：1135-1144.

[2]解旭辉,刘危,张明亮,等.微惯性测量组合关键技术与应用.光线精密工程[J].2002,10(2)：154-158.

[3]丁明理,王祁.无陀螺惯性测量组合研究现状概述.中国惯性技术学报[J].2005,13(4)：83-88.

[4]秦勇,臧希喆,王晓宇,等.基于MEMS惯性传感器的机器人姿态检测系统的研究[J].传感技术学报,2007,20(2)：298-301.

[5]赵建伟,阮晓钢.柔性两轮直立式自平衡仿人机器人的建模及控制[J].机器人,2009,31(2)：179-182.

[6]元震宇.基于姿态控制的平行轮智能电动车[J].机电一体化,2003,9(6)：38-40.

[7]刘亚,康波.基于MEMS陀螺仪的惯性跟踪装置的设计及算法实现[J].电子测量,2010,(9)：15-17.

[8]史智宁,韩波.徐玉,等.基于重力场自适应互补滤波的无人直升机水平姿态估计方法[J].传感技术学报,2009,22(7)：993-996.

[9]Zhu Rong, Sun Dong, Zhou Zhaoying et al, A Linear Fusion Algorithm for Attitude Determination Using Low Cost MEMS-Based Sensors[J].Journal of the International Measurement Confederation,2007,40(3)：322-328.

[10]张吉昌,程凯,郑荣儿.单轴双轮自平衡车姿态检测方案设计.中国海洋大学学报[J].2009,30(增刊)：467-470.