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

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

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

?

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

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

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

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

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

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

1 移動機器人姿態(tài)建模

1.1 MEMS IMU定姿建模

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

ωgyro=ω+εg+ng

(1)

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

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

fa=f+a+εa+na

(2)

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

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

(3)

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

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

(4)

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

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

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

(5)

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

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

(6)

得移動機器人的狀態(tài)方程如下:

(7)

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

Q=E[wwT]=diag[q1q2q3]

(8)

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

(9)

式中:測量噪聲矩陣可以表示如下:

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

(10)

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

Mt=Racc+Ra

(11)

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

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

(12)

同時可得:

(13)

(14)

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

(15)

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

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

(1)時間更新:

(16)

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

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

Racc=0

(17)

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

(18)

(3)量測更新:

(19)

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

3 移動機器人姿態(tài)估計實驗

3.1 實驗條件

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

3.2 姿態(tài)估計實驗

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

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

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

圖4 模擬平移姿態(tài)估計實驗

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

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

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

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

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

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

4 結(jié)論

本文通過研究兩輪移動機器人姿態(tài)估計問題,提出一種基于低成本MEMS慣性傳感器的姿態(tài)估計補償方法。該方法以卡爾曼濾波算法為基礎(chǔ),結(jié)合MEMS慣性傳感器隨機漂移誤差模型,建立姿態(tài)估計濾波器模型;同時該方法有效的抑制了噪聲干擾、融合了加速度計與陀螺儀數(shù)據(jù);并且能夠利用加速度量測殘差進行自適應(yīng)補償非重力位移加速度因素對姿態(tài)估計的影響。本文為兩輪移動機器人提供了一個低成本,精度高的姿態(tài)測量系統(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]. 傳感技學(xué)報,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é)山,王樹三,等. 基于KF的自平衡機器人姿態(tài)角補償方法[J]. 北京理工大學(xué)學(xué)報,2012,32(1):28-32.

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

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

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

項目來源:中央高?；究蒲袠I(yè)務(wù)費專項資金項目(NS2014033);國家自然科學(xué)基金項目(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