一种自适应残差补偿算法在移动机器人姿态估计中的应用研究*

胡佳佳,周翟和,沈 超,赵庆涛

(南京航空航天大学自动化学院,南京 210016)



一种自适应残差补偿算法在移动机器人姿态估计中的应用研究*

胡佳佳,周翟和*,沈 超,赵庆涛

(南京航空航天大学自动化学院,南京 210016)

针对两轮移动机器人MEMS IMU姿态估计的数据融合问题,提出一种以卡尔曼滤波为基础的自适应残差补偿算法。该算法结合惯性传感器误差模型与移动机器人姿态模型构建卡尔曼滤波器,利用卡尔曼滤波量测更新的加速度残差自适应补偿非重力载体位移加速度对姿态估计的影响。实验结果表明,该算法有效的融合了MEMS IMU姿态测量数据,抑制了传感器随机漂移误差,同时自适应补偿了非重力载体位移加速度。

数据融合;姿态估计;残差补偿;MEMS IMU;移动机器人

当今社会,机器人是一种新型的生产工具,在提高企业生产效率、减轻工人劳动强度以及在危险情况下代替人工工作等方面具有强大的优越性。本文所研究的两轮移动机器人是一种两轮共轴、独立驱动,车身重心倒置于车轮轴上方的移动机器人系统,是机器人研究领域的重要分支[1-2]。该移动机器人通过惯性传感器来检测其倾斜角度与角速度,并将检测到的信号传递给微控制器,由微控制器计算得到控制量驱动左右电机,保持机器人前进后退转向等运动方式[3-4]。考虑到本文研究的两轮移动机器人采用低成本MEMS IMU组成姿态测量系统,因此其姿态融合具有一定的误差[5]。文献[6-7]利用卡尔曼滤波进行姿态估计,但是缺少对惯性传感器误差漂移特性进行总结。文献[8-9]在姿态估计时,缺少考虑非重力载体位移加速度对姿态估计的影响。针对以上问题的研究,本文提出一种以卡尔曼滤波为基础的自适应残差补偿算法,该算法首先利用零输入状态下低成本MEMS IMU的误差漂移特性,建立传感器误差模型。然后通过加速度量测残差自适应补偿非重力载体位移加速度对姿态估计的影响。将算法应用于本文所研究的两轮移动机器人上,并进行平衡控制实验验证,实验结果说明该算法是可行的。

1 移动机器人姿态建模

1.1 MEMS IMU定姿建模

两轮移动机器人平衡控制主要由俯仰轴姿态信息决定,本文利用陀螺仪与加速度计构成一个低成本的姿态测量系统。陀螺仪能够直接测量两轮移动机器人的俯仰轴角速度,并可以通过积分直接得到姿态角信息,即陀螺仪的信号与运动条件无关。但是陀螺仪存在着明显的随机漂移误差,其测量模型为角速度、漂移、白噪声的总和:

ωgyro=ω+εg+ng

(1)

式中:ωgyro为陀螺仪的实际测量值,ω为所测量的真实姿态角速度,εg为陀螺仪的漂移误差,ng为均匀分布的高斯白噪声。

加速度计信号可以提供重力加速度的固定垂直参考,但是只有在静态或准静态下,加速度计信号才以重力加速度为主,即与运动条件有关,因此加速度信号模型为重力、位移加速度、随机漂移、白噪声的总和:

fa=f+a+εa+na

(2)

式中:fa为加速度计的实际测量值,f为移动机器人的重力加速度所引起的加速度量,a为动态下的位移加速度,当其处于水平静止或稳定状态时a=0。εa为加速度计的漂移误差,na为均匀分布的测量高斯白噪声。由于两轮移动机器人在平衡时,俯仰轴摆动的角度θ较小,因此可近似认为:

fa=-gsinθ+a+εa+na≈-gθ+a+εa+na

(3)

本文中的两轮移动机器人采用低成本的MEMS IMU,它们受温度及噪声影响很大,输出信号会产生不可修正的随机漂移,随着时间累积,将导致其无法正常工作。因此,为了补偿低成本MEMS IMU本身特性中的误差,现分别对陀螺仪加速度计在零输入状态下进行数据采集,通过数据拟合分析测得的静态数据,总结MEMS IMU本身的误差特性,建立传感器误差模型。

本文通过实验得到的数据经最小二乘法进行拟合,离散拟合模型[10]:

(4)

调整参数C1、C2、T使得拟合曲线残差的平方二范数之和最小。

1.2 姿态估计滤波器设计

本文利用MEMS IMU构建两轮移动机器人姿态测量系统,通过结合惯性传感器误差模型与两轮移动机器人姿态模型构建卡尔曼滤波器,对姿态测量数据进行融合,考虑如下状态空间模型描述卡尔曼滤波器[11]:

(5)

式中:x是卡尔曼滤波器的状态矢量;z是观测矢量;Φ是状态转移矩阵;H是观测矩阵;B和U分别是控制量系数矩阵与控制矢量;w和v分别是高斯过程白噪声与测量白噪声。考虑移动机器人系统状态向量:

x(t)=[θεgεa]T

(6)

得移动机器人的状态方程如下:

(7)

式中:Ts为系统采样时间;Cg1、Cg2、Tg、Ca1、Ca2、Ta分别是陀螺仪与加速度计误差模型拟合参数。过程噪声矩阵可以有:

Q=E[wwT]=diag[q1q2q3]

(8)

由两轮移动机器人定姿模型得观测矢量z(t),观测矩阵H,观测噪声ν(t),可得:

(9)

式中:测量噪声矩阵可以表示如下:

Mt=E[ν(t)ν(t)T]

(10)

由于a与na不相关,因此测量噪声矩阵可以分隔成两部分:

Mt=Racc+Ra

(11)

2 自适应残差补偿算法

卡尔曼滤波算法是一种从信号有关的观测量中通过算法估计出所需信号的滤波算法。在动态条件下,加速度计不仅能够测量到重力加速度,同时也能测量到载体位移加速度[12]。然而,加速度计读数无法区分重力加速度与载体位移加速度。因此必须妥善解决动态条件下的姿态估计问题。由式(9)与卡尔曼滤波公式可得加速度计量测更新残差:

(12)

同时可得:

(13)

(14)

可得非重力位移加速度协方差矩阵:

(15)

式中ut是正交特征向量,λt、μt是特征值。

通过上式推导出的非重力载体位移加速度协方差矩阵,结合本文建立的两轮移动机器人卡尔曼滤波器模型,可得两轮移动机器人姿态估计步骤如下:

(1)时间更新:

(16)

(2)自适应Racc更新算法如下:

状态1:如果载体满足位移加速度判断公式,max(λj-μj)<γ(j=t,t-1,…,t-M2)则两轮移动机器人不存在非重力载体位移加速度:

Racc=0

(17)

状态2:如果公式不满足,则存在非重力载体位移加速度:

(18)

(3)量测更新:

(19)

式中:γ是一个标量,与加速度计的噪声有关,避免Racc受正常的加速度计噪声波动。M2是为了保证从状态2到状态1转变,需要连续满足判断准则M2+1次,这是为了避免有非重力载体位移加速度时,错误的进入状态1。同时,也保证无延迟的从状态1到状态2的转变,使非重力载体位移加速度得到快速估计[13]。

3 移动机器人姿态估计实验

3.1 实验条件

本实验中所用的惯性传感器为低成本的MEMS陀螺仪ENC03与加速度计MMA7361,其中陀螺仪测量范围±300 °/s,敏感系数0.67 mV/(°·s);加速度计测量范围为±1.5gn,敏感系数800 mV/gn。为了自适应补偿载体位移加速度对两轮移动机器人在动态情况下的姿态估计影响,M1=4,M2=3。其他参数:过程噪声q1=0.001,q2=0.005,q3=0.001;量测噪声σ=0.5。通过1 min对MEMS惯性姿态测量系统进行采集数据。

3.2 姿态估计实验

为了验证算法实际应用的可行性,现分别进行两轮移动机器人静态实验、模拟动态平台实验、实际运动实验。如图1～图6所示,为两轮移动机器人姿态估计实验波形图。

图1 静态下非重力加速度

图1、图2是两轮移动机器人在竖直放置情况下进行静态角度估计实验。此时两轮移动机器人处于零输入平衡静止状态,可以认为此时两轮移动机器人不受载体位移加速度的影响。由图1所示的静态下非重力加速度值,考虑标量γ=0.015gn。通过对比图2由加速度计估计的俯仰角度与自适应残差补偿滤波融合的俯仰角度可得,后者处理后的姿态角,方差明显减小,输出平稳,且均值未偏离加速度计估计的原始角度值。说明该算法能够有效滤除噪声干扰,消除随机误差。

图4 模拟平移姿态估计实验

图3 模拟平移情况下非重力加速度

图3、图4为动态模拟平台实验。通过利用模拟平移平台,不改变静态实验估计的角度,施加平移位移加速度。理论上,角度不发生变化,实际上经过卡尔曼滤波处理的角度信息方差较大,具有明显的角度误差。而图3也反应了非重力载体位移加速度已经明显超出γ的值。而经过自适应残差补偿算法后的角度信息误差明显减小。说明自适应残差补偿算法的有效性。

图5 动态下非重力加速度值

图6 动态下角度融合图

图5、图6为两轮移动机器人动态运动实验,可以从图中看出,前30 s,两轮移动机器人处于不稳定状态,同时伴有明显的位移加速度,此时自适应残差补偿算法很好的补偿了姿态误差;后30 s,两轮移动机器人逐渐稳定运行,并没有明显的载体位移加速度,自适应残差补偿算法与卡尔曼滤波估计效果一致。

由实验可得,自适应残差补偿算法具有滤除静态噪声干扰,消除随机误差,并具有补偿载体位移加速度的效果。

4 结论

本文通过研究两轮移动机器人姿态估计问题,提出一种基于低成本MEMS惯性传感器的姿态估计补偿方法。该方法以卡尔曼滤波算法为基础,结合MEMS惯性传感器随机漂移误差模型,建立姿态估计滤波器模型;同时该方法有效的抑制了噪声干扰、融合了加速度计与陀螺仪数据;并且能够利用加速度量测残差进行自适应补偿非重力位移加速度因素对姿态估计的影响。本文为两轮移动机器人提供了一个低成本,精度高的姿态测量系统。

[1]Pathak K,Franch J,Agrawal S K. Velocity and Position Control of a Wheeled Inverted Pendulum by Partial Feedback Linearization[J]. IEEE Transactions on Robotics,2005,21(3):505-513.

[2]Ruan X,Song K. An Adaptive Extended Kalman Filter for Attitude Estimation of Self-Balancing Two-Wheeled Robot[C]//Electric Information and Control Engineering(ICEICE),2011 International Conference on. IEEE,2011:4760-4763.

[3]Suh Y S. Attitude Estimation Using Low Cost Accelerometer and Gyroscope[C]//7th Korea-Russia International Symposium on Science and Technology,Proceedings KORUS 2003.(IEEE Cat. No.03EX737). 2003,2:423-427.

[4]Yu N,Li Y,Ruan X,et al. Research on Attitude Estimation of Small Self-Balancing Two-Wheeled Robot[C]//Control Conference(CCC),2013 32nd Chinese. IEEE,2013:5872-5876.

[5]贾瑞才. 基于四元数EKF的低成本MEMS姿态估计算法[J]. 传感技学报,2014,27(1):90-95.

[6]Ruan X,Gao Y,Song H,et al. Robot Attitude Estimation Based on Kalman Filter and Application to Balance Control[J]. Advanced Science Letters,2012,6(1):542-546.

[7]李潮全,高学山,王树三,等. 基于KF的自平衡机器人姿态角补偿方法[J]. 北京理工大学学报,2012,32(1):28-32.

[8]叶锃锋,冯恩信. 基于四元数和卡尔曼滤波的两轮车姿态稳定方法[J]. 传感技术学报,2012,25(4):524-528.

[9]郜园园,阮晓钢,宋洪军,等. 两轮自平衡机器人惯性传感器滤波问题的研究[J]. 传感技术学报,2010,23(5):696-700.

[10]Barshan B,Durrant-Whyte H F. Inertial Navigation Systems for Mobile Robots[J]. Robotics and Automation,IEEE Transaction on,1995,11(3):328-342.

[11]王晓宇,闫继宏,秦勇,等. 基于扩展卡尔曼滤波的两轮机器人姿态估计[J]. 哈尔滨工业大学学报,2007,39(12):1920-1924.

[12]Lee J K,Park E J,Robinovitch S N. Estimation of Attitude and External Acceleration Using Inertial Sensor Measurement during Various Dynamic Conditions[J]. Instrumentation and Measurement,IEEE Transactions on,2012,61(8):2262-2273.

[13]Suh Y S. Orientation Estimation Using a Quaternion-Based Indirect Kalman Filter with Adaptive Estimation of External Acceleration[J]. Instrumentation and Measurement,IEEE Transactions on,2010,59(12):3296-3305.

The Mobile Robot Attitude Estimated Research Based on Adaptive Residual Compensation Algorithms*

HUJiajia,ZHOUZhaihe*,SHENChao,ZHAOQingtao

(College of Automation Engineering,Nanjing University of Aeronautics and Astronautics,Nanjing 210016,China)

Aiming at the data fusion from MEMS IMU of a two-wheeled mobile robot,an adaptive residual compensation algorithms based on the Kalman filter was proposed. It combines the inertial sensor error model and mobile robot posture model to build the equation of Kalman filter. With the acceleration residuals of Kalman filter measurement update,the impact of external acceleration towards the attitude estimation is adaptively compensated. Experimental results show that the algorithm coalesces the MEMS IMU attitude measuring data efficiently,with this method,the sensors random drift error is suppressed and the external acceleration is adaptively compensated.

data fusion;attitude estimation;residual compensation;MEMS IMU;mobile robot

胡佳佳(1989-),男,现为南京航空航天大学自动化学院硕士在读研究生,主要研究方向为非线性滤波与机器人导航,hjj15088666336@163.com;

周翟和(1974-),男,博士,现为南京航空航天大学自动化学院副教授,主要研究方向为机电控制及自动化、数据融合与测控系统,zzhcom@nuaa.edu.cn。

项目来源:中央高校基本科研业务费专项资金项目(NS2014033);国家自然科学基金项目(61174102)

2014-10-16 修改日期:2014-12-16

C:2575

10.3969/j.issn.1004-1699.2015.03.012

TP212.9

A

1004-1699(2015)03-0363-04