一種自適應(yīng)殘差補(bǔ)償算法在移動(dòng)機(jī)器人姿態(tài)估計(jì)中的應(yīng)用研究*

胡佳佳,周翟和,沈 超,趙慶濤

(南京航空航天大學(xué)自動(dòng)化學(xué)院,南京 210016)

?

一種自適應(yīng)殘差補(bǔ)償算法在移動(dòng)機(jī)器人姿態(tài)估計(jì)中的應(yīng)用研究*

胡佳佳,周翟和*,沈 超,趙慶濤

(南京航空航天大學(xué)自動(dòng)化學(xué)院,南京 210016)

針對(duì)兩輪移動(dòng)機(jī)器人MEMS IMU姿態(tài)估計(jì)的數(shù)據(jù)融合問(wèn)題,提出一種以卡爾曼濾波為基礎(chǔ)的自適應(yīng)殘差補(bǔ)償算法。該算法結(jié)合慣性傳感器誤差模型與移動(dòng)機(jī)器人姿態(tài)模型構(gòu)建卡爾曼濾波器,利用卡爾曼濾波量測(cè)更新的加速度殘差自適應(yīng)補(bǔ)償非重力載體位移加速度對(duì)姿態(tài)估計(jì)的影響。實(shí)驗(yàn)結(jié)果表明,該算法有效的融合了MEMS IMU姿態(tài)測(cè)量數(shù)據(jù),抑制了傳感器隨機(jī)漂移誤差,同時(shí)自適應(yīng)補(bǔ)償了非重力載體位移加速度。

數(shù)據(jù)融合;姿態(tài)估計(jì);殘差補(bǔ)償;MEMS IMU;移動(dòng)機(jī)器人

當(dāng)今社會(huì),機(jī)器人是一種新型的生產(chǎn)工具,在提高企業(yè)生產(chǎn)效率、減輕工人勞動(dòng)強(qiáng)度以及在危險(xiǎn)情況下代替人工工作等方面具有強(qiáng)大的優(yōu)越性。本文所研究的兩輪移動(dòng)機(jī)器人是一種兩輪共軸、獨(dú)立驅(qū)動(dòng),車(chē)身重心倒置于車(chē)輪軸上方的移動(dòng)機(jī)器人系統(tǒng),是機(jī)器人研究領(lǐng)域的重要分支[1-2]。該移動(dòng)機(jī)器人通過(guò)慣性傳感器來(lái)檢測(cè)其傾斜角度與角速度,并將檢測(cè)到的信號(hào)傳遞給微控制器,由微控制器計(jì)算得到控制量驅(qū)動(dòng)左右電機(jī),保持機(jī)器人前進(jìn)后退轉(zhuǎn)向等運(yùn)動(dòng)方式[3-4]。考慮到本文研究的兩輪移動(dòng)機(jī)器人采用低成本MEMS IMU組成姿態(tài)測(cè)量系統(tǒng),因此其姿態(tài)融合具有一定的誤差[5]。文獻(xiàn)[6-7]利用卡爾曼濾波進(jìn)行姿態(tài)估計(jì),但是缺少對(duì)慣性傳感器誤差漂移特性進(jìn)行總結(jié)。文獻(xiàn)[8-9]在姿態(tài)估計(jì)時(shí),缺少考慮非重力載體位移加速度對(duì)姿態(tài)估計(jì)的影響。針對(duì)以上問(wèn)題的研究,本文提出一種以卡爾曼濾波為基礎(chǔ)的自適應(yīng)殘差補(bǔ)償算法,該算法首先利用零輸入狀態(tài)下低成本MEMS IMU的誤差漂移特性,建立傳感器誤差模型。然后通過(guò)加速度量測(cè)殘差自適應(yīng)補(bǔ)償非重力載體位移加速度對(duì)姿態(tài)估計(jì)的影響。將算法應(yīng)用于本文所研究的兩輪移動(dòng)機(jī)器人上,并進(jìn)行平衡控制實(shí)驗(yàn)驗(yàn)證,實(shí)驗(yàn)結(jié)果說(shuō)明該算法是可行的。

1 移動(dòng)機(jī)器人姿態(tài)建模

1.1 MEMS IMU定姿建模

兩輪移動(dòng)機(jī)器人平衡控制主要由俯仰軸姿態(tài)信息決定,本文利用陀螺儀與加速度計(jì)構(gòu)成一個(gè)低成本的姿態(tài)測(cè)量系統(tǒng)。陀螺儀能夠直接測(cè)量?jī)奢喴苿?dòng)機(jī)器人的俯仰軸角速度,并可以通過(guò)積分直接得到姿態(tài)角信息,即陀螺儀的信號(hào)與運(yùn)動(dòng)條件無(wú)關(guān)。但是陀螺儀存在著明顯的隨機(jī)漂移誤差,其測(cè)量模型為角速度、漂移、白噪聲的總和:

ωgyro=ω+εg+ng

(1)

式中:ωgyro為陀螺儀的實(shí)際測(cè)量值,ω為所測(cè)量的真實(shí)姿態(tài)角速度,εg為陀螺儀的漂移誤差,ng為均勻分布的高斯白噪聲。

加速度計(jì)信號(hào)可以提供重力加速度的固定垂直參考,但是只有在靜態(tài)或準(zhǔn)靜態(tài)下,加速度計(jì)信號(hào)才以重力加速度為主,即與運(yùn)動(dòng)條件有關(guān),因此加速度信號(hào)模型為重力、位移加速度、隨機(jī)漂移、白噪聲的總和:

fa=f+a+εa+na

(2)

式中:fa為加速度計(jì)的實(shí)際測(cè)量值,f為移動(dòng)機(jī)器人的重力加速度所引起的加速度量,a為動(dòng)態(tài)下的位移加速度,當(dāng)其處于水平靜止或穩(wěn)定狀態(tài)時(shí)a=0。εa為加速度計(jì)的漂移誤差,na為均勻分布的測(cè)量高斯白噪聲。由于兩輪移動(dòng)機(jī)器人在平衡時(shí),俯仰軸擺動(dòng)的角度θ較小,因此可近似認(rèn)為:

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

(3)

本文中的兩輪移動(dòng)機(jī)器人采用低成本的MEMS IMU,它們受溫度及噪聲影響很大,輸出信號(hào)會(huì)產(chǎn)生不可修正的隨機(jī)漂移,隨著時(shí)間累積,將導(dǎo)致其無(wú)法正常工作。因此,為了補(bǔ)償?shù)统杀綧EMS IMU本身特性中的誤差,現(xiàn)分別對(duì)陀螺儀加速度計(jì)在零輸入狀態(tài)下進(jìn)行數(shù)據(jù)采集,通過(guò)數(shù)據(jù)擬合分析測(cè)得的靜態(tài)數(shù)據(jù),總結(jié)MEMS IMU本身的誤差特性,建立傳感器誤差模型。

本文通過(guò)實(shí)驗(yàn)得到的數(shù)據(jù)經(jīng)最小二乘法進(jìn)行擬合,離散擬合模型[10]:

(4)

調(diào)整參數(shù)C1、C2、T使得擬合曲線殘差的平方二范數(shù)之和最小。

1.2 姿態(tài)估計(jì)濾波器設(shè)計(jì)

本文利用MEMS IMU構(gòu)建兩輪移動(dòng)機(jī)器人姿態(tài)測(cè)量系統(tǒng),通過(guò)結(jié)合慣性傳感器誤差模型與兩輪移動(dòng)機(jī)器人姿態(tài)模型構(gòu)建卡爾曼濾波器,對(duì)姿態(tài)測(cè)量數(shù)據(jù)進(jìn)行融合,考慮如下?tīng)顟B(tài)空間模型描述卡爾曼濾波器[11]:

(5)

式中:x是卡爾曼濾波器的狀態(tài)矢量;z是觀測(cè)矢量;Φ是狀態(tài)轉(zhuǎn)移矩陣;H是觀測(cè)矩陣;B和U分別是控制量系數(shù)矩陣與控制矢量;w和v分別是高斯過(guò)程白噪聲與測(cè)量白噪聲。考慮移動(dòng)機(jī)器人系統(tǒng)狀態(tài)向量:

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

(6)

得移動(dòng)機(jī)器人的狀態(tài)方程如下:

(7)

式中:Ts為系統(tǒng)采樣時(shí)間;Cg1、Cg2、Tg、Ca1、Ca2、Ta分別是陀螺儀與加速度計(jì)誤差模型擬合參數(shù)。過(guò)程噪聲矩陣可以有:

Q=E[wwT]=diag[q1q2q3]

(8)

由兩輪移動(dòng)機(jī)器人定姿模型得觀測(cè)矢量z(t),觀測(cè)矩陣H,觀測(cè)噪聲ν(t),可得:

(9)

式中:測(cè)量噪聲矩陣可以表示如下:

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

(10)

由于a與na不相關(guān),因此測(cè)量噪聲矩陣可以分隔成兩部分:

Mt=Racc+Ra

(11)

2 自適應(yīng)殘差補(bǔ)償算法

卡爾曼濾波算法是一種從信號(hào)有關(guān)的觀測(cè)量中通過(guò)算法估計(jì)出所需信號(hào)的濾波算法。在動(dòng)態(tài)條件下,加速度計(jì)不僅能夠測(cè)量到重力加速度,同時(shí)也能測(cè)量到載體位移加速度[12]。然而,加速度計(jì)讀數(shù)無(wú)法區(qū)分重力加速度與載體位移加速度。因此必須妥善解決動(dòng)態(tài)條件下的姿態(tài)估計(jì)問(wèn)題。由式(9)與卡爾曼濾波公式可得加速度計(jì)量測(cè)更新殘差:

(12)

同時(shí)可得:

(13)

(14)

可得非重力位移加速度協(xié)方差矩陣:

(15)

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

通過(guò)上式推導(dǎo)出的非重力載體位移加速度協(xié)方差矩陣,結(jié)合本文建立的兩輪移動(dòng)機(jī)器人卡爾曼濾波器模型,可得兩輪移動(dòng)機(jī)器人姿態(tài)估計(jì)步驟如下:

(1)時(shí)間更新:

(16)

(2)自適應(yīng)Racc更新算法如下:

狀態(tài)1:如果載體滿足位移加速度判斷公式,max(λj-μj)<γ(j=t,t-1,…,t-M2)則兩輪移動(dòng)機(jī)器人不存在非重力載體位移加速度:

Racc=0

(17)

狀態(tài)2:如果公式不滿足,則存在非重力載體位移加速度:

(18)

(3)量測(cè)更新:

(19)

式中:γ是一個(gè)標(biāo)量,與加速度計(jì)的噪聲有關(guān),避免Racc受正常的加速度計(jì)噪聲波動(dòng)。M2是為了保證從狀態(tài)2到狀態(tài)1轉(zhuǎn)變,需要連續(xù)滿足判斷準(zhǔn)則M2+1次,這是為了避免有非重力載體位移加速度時(shí),錯(cuò)誤的進(jìn)入狀態(tài)1。同時(shí),也保證無(wú)延遲的從狀態(tài)1到狀態(tài)2的轉(zhuǎn)變,使非重力載體位移加速度得到快速估計(jì)[13]。

3 移動(dòng)機(jī)器人姿態(tài)估計(jì)實(shí)驗(yàn)

3.1 實(shí)驗(yàn)條件

本實(shí)驗(yàn)中所用的慣性傳感器為低成本的MEMS陀螺儀ENC03與加速度計(jì)MMA7361,其中陀螺儀測(cè)量范圍±300 °/s,敏感系數(shù)0.67 mV/(°·s);加速度計(jì)測(cè)量范圍為±1.5gn,敏感系數(shù)800 mV/gn。為了自適應(yīng)補(bǔ)償載體位移加速度對(duì)兩輪移動(dòng)機(jī)器人在動(dòng)態(tài)情況下的姿態(tài)估計(jì)影響,M1=4,M2=3。其他參數(shù):過(guò)程噪聲q1=0.001,q2=0.005,q3=0.001;量測(cè)噪聲σ=0.5。通過(guò)1 min對(duì)MEMS慣性姿態(tài)測(cè)量系統(tǒng)進(jìn)行采集數(shù)據(jù)。

3.2 姿態(tài)估計(jì)實(shí)驗(yàn)

為了驗(yàn)證算法實(shí)際應(yīng)用的可行性,現(xiàn)分別進(jìn)行兩輪移動(dòng)機(jī)器人靜態(tài)實(shí)驗(yàn)、模擬動(dòng)態(tài)平臺(tái)實(shí)驗(yàn)、實(shí)際運(yùn)動(dòng)實(shí)驗(yàn)。如圖1～圖6所示,為兩輪移動(dòng)機(jī)器人姿態(tài)估計(jì)實(shí)驗(yàn)波形圖。

圖1 靜態(tài)下非重力加速度

圖1、圖2是兩輪移動(dòng)機(jī)器人在豎直放置情況下進(jìn)行靜態(tài)角度估計(jì)實(shí)驗(yàn)。此時(shí)兩輪移動(dòng)機(jī)器人處于零輸入平衡靜止?fàn)顟B(tài),可以認(rèn)為此時(shí)兩輪移動(dòng)機(jī)器人不受載體位移加速度的影響。由圖1所示的靜態(tài)下非重力加速度值,考慮標(biāo)量γ=0.015gn。通過(guò)對(duì)比圖2由加速度計(jì)估計(jì)的俯仰角度與自適應(yīng)殘差補(bǔ)償濾波融合的俯仰角度可得,后者處理后的姿態(tài)角,方差明顯減小,輸出平穩(wěn),且均值未偏離加速度計(jì)估計(jì)的原始角度值。說(shuō)明該算法能夠有效濾除噪聲干擾,消除隨機(jī)誤差。

圖4 模擬平移姿態(tài)估計(jì)實(shí)驗(yàn)

圖3 模擬平移情況下非重力加速度

圖3、圖4為動(dòng)態(tài)模擬平臺(tái)實(shí)驗(yàn)。通過(guò)利用模擬平移平臺(tái),不改變靜態(tài)實(shí)驗(yàn)估計(jì)的角度,施加平移位移加速度。理論上,角度不發(fā)生變化,實(shí)際上經(jīng)過(guò)卡爾曼濾波處理的角度信息方差較大,具有明顯的角度誤差。而圖3也反應(yīng)了非重力載體位移加速度已經(jīng)明顯超出γ的值。而經(jīng)過(guò)自適應(yīng)殘差補(bǔ)償算法后的角度信息誤差明顯減小。說(shuō)明自適應(yīng)殘差補(bǔ)償算法的有效性。

圖5 動(dòng)態(tài)下非重力加速度值

圖6 動(dòng)態(tài)下角度融合圖

圖5、圖6為兩輪移動(dòng)機(jī)器人動(dòng)態(tài)運(yùn)動(dòng)實(shí)驗(yàn),可以從圖中看出,前30 s,兩輪移動(dòng)機(jī)器人處于不穩(wěn)定狀態(tài),同時(shí)伴有明顯的位移加速度,此時(shí)自適應(yīng)殘差補(bǔ)償算法很好的補(bǔ)償了姿態(tài)誤差;后30 s,兩輪移動(dòng)機(jī)器人逐漸穩(wěn)定運(yùn)行,并沒(méi)有明顯的載體位移加速度,自適應(yīng)殘差補(bǔ)償算法與卡爾曼濾波估計(jì)效果一致。

由實(shí)驗(yàn)可得,自適應(yīng)殘差補(bǔ)償算法具有濾除靜態(tài)噪聲干擾,消除隨機(jī)誤差,并具有補(bǔ)償載體位移加速度的效果。

4 結(jié)論

本文通過(guò)研究?jī)奢喴苿?dòng)機(jī)器人姿態(tài)估計(jì)問(wèn)題,提出一種基于低成本MEMS慣性傳感器的姿態(tài)估計(jì)補(bǔ)償方法。該方法以卡爾曼濾波算法為基礎(chǔ),結(jié)合MEMS慣性傳感器隨機(jī)漂移誤差模型,建立姿態(tài)估計(jì)濾波器模型;同時(shí)該方法有效的抑制了噪聲干擾、融合了加速度計(jì)與陀螺儀數(shù)據(jù);并且能夠利用加速度量測(cè)殘差進(jìn)行自適應(yīng)補(bǔ)償非重力位移加速度因素對(duì)姿態(tài)估計(jì)的影響。本文為兩輪移動(dòng)機(jī)器人提供了一個(gè)低成本,精度高的姿態(tài)測(cè)量系統(tǒng)。

[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]賈瑞才. 基于四元數(shù)EKF的低成本MEMS姿態(tài)估計(jì)算法[J]. 傳感技學(xué)報(bào),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]李潮全,高學(xué)山,王樹(shù)三,等. 基于KF的自平衡機(jī)器人姿態(tài)角補(bǔ)償方法[J]. 北京理工大學(xué)學(xué)報(bào),2012,32(1):28-32.

[8]葉锃鋒,馮恩信. 基于四元數(shù)和卡爾曼濾波的兩輪車(chē)姿態(tài)穩(wěn)定方法[J]. 傳感技術(shù)學(xué)報(bào),2012,25(4):524-528.

[9]郜園園,阮曉鋼,宋洪軍,等. 兩輪自平衡機(jī)器人慣性傳感器濾波問(wèn)題的研究[J]. 傳感技術(shù)學(xué)報(bào),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]王曉宇,閆繼宏,秦勇,等. 基于擴(kuò)展卡爾曼濾波的兩輪機(jī)器人姿態(tài)估計(jì)[J]. 哈爾濱工業(yè)大學(xué)學(xué)報(bào),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-),男,現(xiàn)為南京航空航天大學(xué)自動(dòng)化學(xué)院碩士在讀研究生,主要研究方向?yàn)榉蔷€性濾波與機(jī)器人導(dǎo)航,hjj15088666336@163.com;

周翟和(1974-),男,博士,現(xiàn)為南京航空航天大學(xué)自動(dòng)化學(xué)院副教授,主要研究方向?yàn)闄C(jī)電控制及自動(dòng)化、數(shù)據(jù)融合與測(cè)控系統(tǒng),zzhcom@nuaa.edu.cn。

項(xiàng)目來(lái)源:中央高校基本科研業(yè)務(wù)費(fèi)專(zhuān)項(xiàng)資金項(xiàng)目(NS2014033);國(guó)家自然科學(xué)基金項(xiàng)目(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