蛇形机器人互补滤波和四元数的姿态解算*

王超杰，苏 中，时佳斌，陈 庚

(北京信息科技大学 高动态导航技术北京市重点实验室，北京100101)

蛇形机器人互补滤波和四元数的姿态解算*

王超杰，苏 中，时佳斌，陈 庚

(北京信息科技大学 高动态导航技术北京市重点实验室，北京100101)

针对蛇形机器人姿态解算问题，陀螺存在漂移特性，加速度计的测量值包含重力加速度和运动加速度，磁强计易受周围环境地磁干扰，并且蛇形机器人采用嵌入式微处理器，需要减少计算量。设计了用互补滤波器来实现惯性传感器的数据融合，用四元数进行姿态解算的方法。经过实验验证表明:采用互补滤波和四元数进行姿态解算能有效融合各个惯性传感器的数据，计算量小，能够满足蛇形机器人对精度和实时性的要求。

蛇形机器人；姿态解算；互补滤波器；惯性传感器；四元数

0 引 言

蛇形机器人由于具有广阔的应用前景，使其成为了当前的研究热点。其中，姿态检测是控制机器人在未知环境空间运动的关键问题之一，通过融合各种传感器的数据来获取机器人的运动姿态，进而实现对机器人的运动控制与路径规划。姿态解算的精度和速度将直接影响对蛇形机器人的控制的稳定性和可靠性。随着MEMS技术的发展与对成本因素的考虑，低成本的捷联惯性测量单元越来越多的被应用到移动机器人中,其主要包括陀螺仪、加速度计和磁强计[1]。MEMS陀螺仪具有漂移特性，加速度计的测量值中同时包含了重力加速度和运动加速度，同时磁强计容易受到周围环境中外部磁场的干扰。怎样有效地融合这些惯性传感器的数据，滤除外部干扰，得到高精度的姿态数据，是一项非常具有挑战性的工作。同时，蛇形机器人采用STM32F103微处理器，不适用于计算量大的姿态解算算法。

本文设计了用互补滤波器实现惯性传感器的数据融合、用四元数法进行姿态解算的方法能够满足蛇形机器人对精度和实时性的要求。

1 蛇形机器人姿态描述

为了描述蛇形机器人的俯仰、偏航、横滚的姿态信息，需引入空间三维坐标系。现选择北东地作为导航坐标系(n系)，蛇形机器人的纵轴、横轴和竖轴构成的右手坐标系作为载体坐标系(b系)。

(1)

其中，φ为偏航角(yaw)，θ为俯仰角(pitch)，γ为横滚角(roll)，蛇形机器人运动过程中以这3个角作为姿态角。

2 姿态解算算法原理

姿态解算常用的算法有欧拉角法、方向余弦法和四元数法。欧拉角法在求姿态时存在奇点，不能用于全姿态的解算；方向余弦法可用于全姿态解算但计算量大，不能满足实时性的要求。本文采用四元数法[3]，其计算量小、无奇点且可以满足蛇形机器人运动过程中姿态的实时解算。

1)初始对准

姿态计算初始，将已知载体初始姿态角φ0,θ0,γ0代入下式，求出初始时刻的四元数，完成初始对准

(2)

2)四元数随时间的更新与求解

写成矩阵形式为

(3)

式中ωgx,ωgy,ωgz分别为陀螺仪在载体坐标系下测得的蛇形机器人x,y,z轴的角速度。设置采样周期T，每隔一段时间读取陀螺仪的数据，通过上式得到更新后的四元数，进而求得更新后的方向余弦矩阵。然而，MEMS陀螺仪存在温度漂移，积分运算也有一定的累积误差，所以，必须采用一定的补偿算法，融合加速度计和磁强计的数据，进行误差补偿以实现精确的姿态解算。

考虑到蛇形机器人姿态解算的实时性和精度要求，四元数更新采用一阶龙格—库塔法，利用传感器数据融合后得到的角速度ωx,ωy,ωz更新四元数

(4)

3)姿态角求解

每个周期更新完四元数，需要对四元数做归一化处理

(5)

(6)

对照公式(1)和公式(6)可得姿态角

(7)

3 互补滤波器的设计[4]

3.1 设计原理

设计互补滤波器的目的是综合加速度计、陀螺仪、磁强计各自的频率响应特性，从频域角度对3只传感器的数据进行融合，以减小姿态解算的误差。陀螺仪通过积分得到姿态角，能在较短的时间内提供准确的姿态数据，但长期精度较差；加速度计和磁强计的测量误差不随时间积累，但是短期内精度较差，动态响应较慢。利用其频率上的互补特性，通过互补滤波能很好地结合陀螺仪角速度的动态性能和加速度计、磁强计的静态精度。

设置互补滤波器的传递函数[5]为

G1(s)具有一阶低通滤波特性，G2(s)具有一阶高通特性,可以构造估计量

因此，通过互补滤波器能够消除高频噪声和低频误差的积累，能够很好地融合各传感器的数据。互补滤波器的原理框图如图1所示。

图1 互补滤波器原理框图Fig 1 Principle block diagram of complementary filter

互补滤波器在频域上具有互补性，设计的滤波器传递函数满足G1(s)+G2(s)=1，因此，角度的估计值是陀螺仪高通滤波和加速度计、磁强计低通滤波后的数据融合。

3.2 基于互补滤波和四元数的姿态求解[6,7]

图2 基于互补滤波器和四元数的姿态解算方框图Fig 2 Block diagram of attitude calculation based on complementary filter and quaternion

(8)

其中，(mx,my,mz)为磁强计测量的地磁场的矢量归一化后的值，将其转到导航坐标系中得到(hx,hy,hz)，由于导航坐标系为北东地，在y轴方向地磁场矢量应该为0，对(hx,hy,hz)进行一次校准得导航坐标系下的地磁场矢量(bx,by,bz)。

(9)

(10)

(11)

ω=ωg+δ.

(12)

其中，δ为误差e通过一个比例积分模块后生成的对陀螺漂移的补偿量，ωg为陀螺仪测量得到的角速度，ω为融合加速度计和磁强计数据校正后的角速度，将ω代入公式(4)就可得到随时间传递的准确的四元数，进而求得姿态矩阵和姿态角。

4 实验验证

蛇形机器人所用的MEMS惯性器件包括MPU6050和HMC5883，MPU6050集成3轴陀螺仪和3轴加速度计，HMC5883为3轴磁强计。为了验证本文设计的姿态解算算法的性能，用日本北阳公司生产的高精度的LPMS—B模块解算的角度作为参考角度，其姿态解算精度为动态情况下小于2°，静态情况下小于0.5°。

将蛇形机器人所用的MEMS惯性器件与LPMS—B模块一同固定在平整的电路板上，调整其基准一致，手动控制电路板模仿蛇形机器人的步态做各种动作，实验环境如图3所示。通过电脑的两路USB串口分别接收2个惯导模块姿态解算的数据，并Matlab绘图如图4～图6所示。

图3 实验环境Fig 3 Experimental environment

图4 横滚角Fig 4 Roll angle

图5 俯仰角Fig 5 Pitch angle

图6 偏航角Fig 6 Yaw angle

通过实验可以看出：通过手动控制电路板模拟蛇形机器人做运动时，俯仰角最大误差约为±2°，横滚角最大误差约为±1.5°，偏航角最大误差约为±3°，数据更新率为100 Hz，能够满足蛇形机器人对精度和实时性的要求。

5 结 论

本文针对蛇形机器人姿态解算的难点，设计了用互补滤波器来融合陀螺仪、加速度计和磁强计的测量数据，用四元数法进行姿态解算。

实验结果表明：采用互补滤波器进行数据融合、四元数法进行姿态求解的方法能有效融合陀螺仪、加速度计和磁强计的数据，输出的姿态角动态误差小，计算量小，能够满足蛇形机器人在运动过程中对精度和实时性的要求。

[1] 王玲玲,富 立,陈 三.基于低成本IMU的捷联航姿系统软件的设计与实现[J].中国惯性技术学报,2009,17(4):379-382.

[2] 赵 欣,王仕成,杨东方,等.一种改进的高动态捷联惯导解算算法[J].中国惯性技术学报，2011,19(2):163-169.

[3] 梁延德,程 敏,何福本，等.基于互补滤波器的四旋翼飞行器姿态解算[J].传感器与微系统,2011,30(11):56-61.

[4] 丁 君,赵忠华.AHRS航姿解算中的两种滤波方法的比较研究[J].计算机仿真,2013,30(9):60-65.

[5] 兰建军,谭力弓,朴 亨,等.惯性传感器和互补滤波器在姿态估计中的应用[J].制造业自动化,2013,17:65-67.

[6] 阎世梁,王银玲,张 华.基于改进互补滤波器的低成本微小飞行器姿态估计方法[J].计算机应用，2013,33(7):2078-2082.

[7] Euston M,Coote P,Mahony R,et al．A complementary filter for attitude estimation of a fixed-wing UAV[C]∥IEEE/RSJ 2008 International Conference on Intelligent Robots and Systems，Nice，France，2008:340-345．

[8] Carminati M,Ferrari G,Sampietro M,et al．Fault detection and isolation enhancement of an aircraft attitude and heading reference system based on MEMS inertial sensors[J].Procedia Chemistry,2009,1(1):509-512.

[9] Brás Sérgio,Rosa Paulo,Silvestre Carlos,et al. Global attitude and gyro bias estimation based on set-valued observers[J].Systems & Control Letters,2013,62(10):937-942.

Attitude calculation of snake-like robot using complementary filtering and quaternion*

WANG Chao-jie, SU Zhong, SHI Jia-bin, CHEN Geng

(Beijing Key Laboratory of High Dynamic Navigation Technology,University of Beijing Information Science & Technology,Beijing 100101,China)

Aiming at problem of attitude calculation of snake-like robot，gyro exists drift characteristics,measured value of accelerometer includes gravity and motion acceleration,magnetometer vulnerable to ambient geographic magnetic interference,and snake-like robot use embedded microcontrollers,which reduce the need of computation amount.Therefore,design inertial sensor data fusion is achieved with complementary filtering and attitude calculation is carried out with quaternion method.Experimental verification show that using complementary filtering and quaternion method can effectively fuse sensor datas,amount of calculation is small, and can meet requirements of precision and real-time for snake-like robot.

snake-like robot; attitude calculation; complementary filter; inertial sensor; quaternion

10.13873/J.1000—9787(2014)10—0137—04

2014—03—12

国家自然科学基金资助项目(61261160497)；北京市科技计划资助项目(Z121100001612007)；北京市教委科技成果转化和产业化项目(PXM2013—014224—000074)

TP 212

A

1000—9787(2014)10—0137—04

王超杰(1988-)，男，山东烟台人，硕士研究生，主要从事蛇形机器人多元信息融合的研究。