利用磁强计及微机械加速度计和陀螺的姿态估计扩展卡尔曼滤波器_

1、第13卷第2期 中国惯性技术学报 2005年4月 文章编号：1005-6734(200502-0027-04利用磁强计及微机械加速度计和陀螺的姿态估计扩展卡尔曼滤波器黄 旭，王常虹，伊国兴，王玉峰（哈尔滨工业大学空间控制与惯性技术研究中心，哈尔滨 150001）摘要：提出了一种用于惯性测量组合(IMU姿态估计的扩展卡尔曼滤波器。此滤波器的状态转移阵利用基于四元数的等效旋转矢量来计算姿态。等效旋转矢量法可以避免欧拉角的奇异问题，并能有效抑制不可交换误差。测量四元数是利用高斯-牛顿迭代法解算以加速度计和磁强计输出数据为变量的方程组得出的。用从实际IMU 采集的数据对滤波器进行测试，并比较了带有滤波

2、器和没有滤波器的结果。关 键 词：卡尔曼滤波器；姿态；磁强计；加速度计；陀螺 中图分类号：U666.1 文献标识码：AExtended Kalman Filter for IMU Attitude Estimation Using Magnetometer, MEMS Accelerometer and GyroscopeHUANG Xu, WANG Chang-hong, YI Guo-xing, WANG Yu-feng(Space Control and Inertial Technology Research Center, Harbin Institute of Technology

3、, Harbin 150001, ChinaAbstract: This paper presents an extended Kalman filter for IMU attitude estimation. The transition matrix of this filter uses equivalent rotation vector based on quaternions to calculate the attitude, which can avoid the singularity problem of Euler angles and suppress the non

4、commutativity error. The measurement quaternion is derived from accelerometer and magnetometer data using Gauss-Newton iteration algorithm. The data collected from the real sensor are utilized to test the filter, and results are presented and compared for using and without using the filter. Key word

5、s: Kalman filter; attitude; magnetometer; accelerometer; gyroscope 0 引 言陀螺在许多应用场合用来估计姿态，例如：航天器、卫星、导弹等。但是机械陀螺、光纤陀螺和激光陀螺极高价格和大体积限制了陀螺在某些场合的应用。近年来，在汽车工业的需求推动下，微机械陀螺获得了很大发展。微机械陀螺与其它陀螺相比，具有体积小、成本低、易于批量生产等优点，从而扩展了陀螺的应用领域；但是它的精度较低，并且陀螺漂移会在短时间内引起较大的姿态误差。解决上述问题的常用方法是采用卡尔曼滤波器15。卡尔曼滤波器利用来自GPS 、磁强计、加速度计和倾斜计等传感器

6、的数据来补偿陀螺偏差引起的姿态误差。文献1描述了一个利用GPS 和磁强计来收稿日期：2005-01-08作者简介：黄旭（1973），男，哈尔滨工业大学博士生，从事组合导航研究。DOI:10.13695/ki.12-1222/o3.2005.02.00728 中国惯性技术学报 2005年4月补偿姿态误差的卡尔曼滤波器，但是未给出详细信息。文献2和文献3分别给出了一个用于跟踪问题的卡尔曼滤波器。本文提出一个用于估算IMU 姿态的扩展卡尔曼滤波器，该IMU 由九个传感器组成：三个加速度计、三个陀螺和三个磁强计。本文中的滤波器与Marins J. L.在文献4中给出的滤波器相似，例如，所选择的状态变量

7、，以及采用高斯-牛顿迭代法来计算测得的四元数。但是至少在以下两点是不同的：此滤波器采用了不同的状态转移阵，而且不需要地磁场模型来计算四元数测量值。1 四元数姿态就是联系动坐标系和参考坐标系角位置的参数。欧拉角和四元数是常用的表示姿态的两种方式。四元数由于可以避免欧拉角的奇异问题而应用得更加广泛。由Hamilton 引入的四元数可写为：0123q q q q =+n i j k (1其中，2221, , , =i j k ij k jk i ki j ik 。四元数乘法定义如下：01230123( (p p p p q q q q =+r i j j i j k =321001231032230

8、13210q q q q p p p p p p p p p p p p p p p p (2 四元数可用来表示矢量旋转，其矢量部分定义了旋转轴，标量部分定义了旋转角度。如果旋转轴由参考系的方向余弦来定义，并且四元数范数限制为单位范数，则可以方便地表示旋转。假设方向余弦角为、和，旋转角度为，则旋转四元数可表示为：TTcos cos sin cos sin cos sin cos sin =q n (3式中，n 为沿旋转轴的单位矢量，T coscos cos =n 。矢量u 绕n 轴旋转角度后的矢量为：1=v q u q (4式中，1q 是四元数 q 的逆，10123q q q q =qi j

9、k 。方程(4也可以写成矩阵乘法的形式：1=v q u q Tu (5其中，2222012312031302222212030123230122221302230101232(2( 2(2( 2( 2(q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q +=+T 。矩阵T 的方向余弦形式为： cos cos cos sin sin sin sin cos cos sin sin sin sin cos cos cos sin sin cos cos sin sin sin cos sin sin cos

10、 cos cos =+T 。 2 等效旋转矢量法本文采用的等效旋转矢量法由Miller 6提出。该算法为：分别计算时刻h h h t , 3/2, 3/1=时的增量角1、2和3；按照132310.45( 0.675( =+计算角度；按如下方式计算增量四元数( h q ：( h =r q C S ， 0cos(/2 =C ， 00(1/sin(/2 =S ， T 1/20( =r (6利用( ( ( T h T h +=Q Q q 计算更新后的四元数( T h +Q ，其中表示四元数乘法。第2期 黄旭等：利用磁强计及微机械加速度计和陀螺的姿态估计扩展卡尔曼滤波器设计 293 利用加速度计和磁强计

11、计算测量四元数选择参考坐标系为NED 坐标系，并忽略磁北极和真北之间的差别。当载体在参考坐标系中静止，并且体坐标系和参考坐标系重合时，加速度计和磁强计的测量输出为：00r g =A ， 0r n d m m =m (7其中，g 是重力加速度，n m 是地磁场在NED 坐标系中的北向分量，d m 是地磁场在NED 坐标系中的垂直分量。当载体处于任意姿态时，假定加速度计和磁强计的输出是：b xy z a a a =A ， b x y z m m m =m (8参考坐标系和体坐标系的关系可按方程(5表示：b r =A TA ，b r =m Tm ，可以得到如下方程：(22031q q q q g a

12、 x =， (21032q q q q g a y +=， (23222120q q q q g a z += (90 (2 ( (21032232221203021=+q q q q m q q q q m q q q q m z y x (10由此可见，利用高斯-牛顿迭代法解算上述方程，可得到相应的转动四元数。4 卡尔曼滤波器本文采用文献7中的伪线性卡尔曼滤波器。考虑如下系统：1k k k k +=+x Ax Bu w ， k k k =+y Cx v (11其中，x 是状态矢量，A 是状态转移矩阵，B 是控制矩阵，u 是系统输入矢量，w 是协方差阵为k Q 的过程噪声矢量；Y 是量测矢量

13、，C 是系统观测矩阵，v 是协方差阵为k R 的量测噪声矢量。卡尔曼滤波器是计算状态x 的最优估计x的递归算法。卡尔曼滤波器在利用几个不同噪声测量量来估计无法直接测量的变量方面非常有效。因此，尽管陀螺是通过对角速率信号积分来获得姿态，加速度计和磁强计却能提供不同噪声、无漂移的姿态。卡尔曼滤波器能够适当权衡两方面信息，充分利用从各个传感器得到的数据。下面给出了我们所采用的卡尔曼滤波器的算法7。 时间传播状态变量的时间传播根据：1/k k k k k k +=+xA x Bu (12 协方差阵的传播根据：T 1/k k k k k k k +=+P A P A Q (13测量更新计算卡尔曼增益矩阵

14、：T T 111/1/1k k k k k k +=+K P C CP C R (14更新状态估计：1/11/111/k k k k k k k k +=+xx K y Cx (15 更新协方差阵：T T 1/111/1111k k k k k k k k k +=+P I K C P I K C K R K (165 卡尔曼滤波器设计实际上，方程(7可看作差分方程，滤波器的状态矢量在方程(7的四元数基础上，增加了角速度：30 中国惯性技术学报 2005年4月 0123x y z q q q q =x (17其中，x 、y 和z 是陀螺输出的角速率信号，0q 、1q 、2q 和3q 是由第2节

15、所述方法得出的四元数的 各个分量。输入矢量是：0000( ( ( ( ( (x x y y z z T T T T T T T T T =+u (18因此，可得到状态转移矩阵A 和控制矩阵B ：0123103223013210000000000000000010000000100001h h h h h h h h h h h h h h h h q q q q q q q q q q q q q q q q =A ，0000000000100010001=B (19 式(19中，0h q 、1h q 、2h q 和3h q 是式 (9中( h q 的各个分量。 量测矢量和状态变量相同。但是，

16、量测四元数由方程（9）（10）得出。量测矩阵7=C I ，其中7I 是77单位矩阵。姿态估计的流程图如图1所示。6 滤波器测试结果采用我们正在开发的IMU 的实测数据对滤波器进行了测试。IMU 安装在单轴转台上，台面调水平，采样率分别为200 Hz和50 Hz，数据长度为10 000点。通过两种方式采集数据：转台静止和转台匀速转动。转台静止时采集的数据用来计算噪声阵Q 和R 。由于单轴转台保持水平，IMU 的横滚角和俯仰角一直为0，只能利用偏航角来评价滤波器；同时还要把转台位置的反馈信号记录下来，作为准确的偏航角，与通过计算IMU 传感器数据所得的偏航角进行比较。图2和图3是未采用滤波器，只利

17、用陀螺输出计算出的偏航角和转台位置反馈，偏航角明显开始发散。图4和图5是利用滤波器计算所得的偏航角和位置反馈。测试结束时，采样率为200 Hz ，采用滤波器时偏航角误差为0.36，采样率为50 Hz时偏航角误差为4.56。通过比较图2图5可以清楚看出，该滤波器能有效补偿陀螺漂移引起的姿态误差。 （下转第34页）图1 姿态估计流程图图4 采样率50 Hz时有滤波器偏航角和位置反馈变化曲线图5 采样率200 Hz时有滤波器偏航角和位置反馈变化曲线图3 采样率200 Hz时无滤波器偏航角和位置反馈变化曲线图2 采样率50 Hz时无滤波器偏航角和位置反馈变化曲线34 中国惯性技术学报 2005年4月

18、波器自身也有局限性，实际滤波结果与实际幅值并不完全吻合的。但在各 自允许的动态范围内对滤波结果进行数据处理后，两路均可以实现线性化输出(如图4，分别是大转速和小转速输入时检测系统的输出。在DSP 内部采用软件分档的处理方案。对输出结果根据最小二乘拟合，采用分段线性化进行线性化修正，可以实现全量程的线性化数字量输出。实验证明这样可以取得很好的效果，如图5。 实验用光纤陀螺的动态范围为100 ( /s，零偏稳定性达到10 ( /s，标度因数非线性度小于10-3的量级。4 结 论 结合分档方法的全数字处理方案有效地解决了开环方案动态范围局限性带来的难题，并克服了PZT 温度敏感性给开环系统引入的测量

19、误差。目前实验陀螺还存在零偏较大、零漂有待减小的问题，需进一步改善信号处理电路，如对低噪声放大电路、电路抗干扰等进行优化。对于数字信号的处理也在做进一步研究，解决好这些问题将进一步提高陀螺的性能。 参考文献：1 Bohm K, Petermann K. Signal processing schemes for the fiber-optic gyroscopeA. FiberOptic Gyros, Proc. SPIEC, 1986, Vol.719: 36-44. 2 常胜利. 实用中低精度光纤陀螺的信号处理设计J. 激光杂志，2002，（3）. 3 孙圣和. 光纤陀螺全数字信号处理技术

20、研究J. 仪器仪表学报，2000，（6）.4 杨远洪. 开环FOG 中PZT 调制器的在线研究J. 北京航空航天大学学报，1999，（6）.（上接第30页）7 结 论本文描述了一个用于惯测组合姿态估计的扩展卡尔曼滤波器。该滤波器利用加速度计和磁强计的数据有效补偿了陀螺漂移引起的姿态误差。需要指出的是该滤波器只适用于载体没有线加速度的情况。因此在IMU 应用中，需要先根据加速度计的输出，判断载体的状态。如果载体是静止或者没有线加速度，则使用此滤波器计算姿态，否则直接使用陀螺输出数据计算姿态，而不使用此滤波器。参考文献：1 Rios J A, White E. Fusion filter algo

21、rithm enhancements for a MEMS GPS/IMUA. ION Meeting ProceedingsC, San Diego, California, January 2002.2 Gebre-Egziabher D, Hayward R C, Powell J D. A low cost GPS/inertial attitude heading reference systemA. Proceedings of PLANS 98C, Rancho Mirage, C A, April 1998.3 Foxlin E. Inertial head-tracker sensor fusion by a complementary separate-bias Kalman filterA. Proceedings of VRAIS96, IEEEC, 1996: