基于擴(kuò)展卡爾曼濾波的球形機(jī)器人姿態(tài)解算

龍子洋 項(xiàng)鵬 隋國榮

摘 ?要：針對球形機(jī)器人在姿態(tài)解算的過程中，慣性測量元件精度不高、穩(wěn)定性差和易受噪聲干擾從而導(dǎo)致無法精確控制其運(yùn)動姿態(tài)的問題，提出一種通過擴(kuò)展卡爾曼濾波融合IMU（Inertial Measurement Unit）慣性測量元件數(shù)據(jù)來進(jìn)行姿態(tài)解算的方法，利用多傳感器測量數(shù)據(jù)進(jìn)行融合，并使用擴(kuò)展卡爾曼濾波得到精確的姿態(tài)信息。通過相關(guān)實(shí)驗(yàn)充分驗(yàn)證了基于擴(kuò)展卡爾曼濾波的姿態(tài)解算方法的精度和魯棒性明顯提高，抗噪聲干擾能力更強(qiáng)。實(shí)驗(yàn)表明，該姿態(tài)解算方法相比于互補(bǔ)濾波的姿態(tài)解算，全姿態(tài)角均方根誤差和平均誤差分別下降了0.0601和0.1984，可見其對于球形機(jī)器人的運(yùn)動控制具有良好的適用性。

關(guān)鍵詞：球形機(jī)器人;擴(kuò)展卡爾曼濾波;互補(bǔ)濾波;姿態(tài)解算;四元數(shù)

中圖分類號：TP273 ? ? 文獻(xiàn)標(biāo)識碼：A

Attitude Calculation of Spherical Robot based on Extended Kalman Filter

LONG Ziyang， XIANG Peng， SUI Guorong

（College of Opto-electronic Information and Computer Engineering， University of Shanghai for Science and Technology， Shanghai 200093， China）

lzy_1194633164@163.com; charlottexp@163.com; suigr@usst.edu.cn

Abstract： In the process of attitude calculation of spherical robot， the inertial measurement element has problems of low accuracy， poor stability and being easy to be disturbed by noise， which makes it impossible to accurately control its motion attitude. In view of these problems， this paper proposes an attitude calculation method which integrates extended Kalman filter and IMU （Inertial Measurement Unit） data. Multi-sensor measurement data is used for fusion and extended Kalman filter is used to obtain accurate attitude information. Relevant experiments fully verifies that the accuracy and robustness of the attitude calculation method based on extended Kalman filter are significantly improved， and the anti-noise interference ability is stronger. Experiments show that compared with the attitude calculation of complementary filter， the root mean square error and average error of the full attitude angle have decreased by 0.0601 and 0.1984 respectively. It can be seen that this method has good applicability for the motion control of spherical robot.

Keywords： spherical robot; extended Kalman filter; complementary filter; attitude calculation; quaternion

1 ? 引言（Introduction）

球形機(jī)器人作為一種新型移動機(jī)器人，其所有機(jī)械結(jié)構(gòu)和控制系統(tǒng)都包含于球形外殼內(nèi)，通過質(zhì)心偏移、動量守恒等[1]原理實(shí)現(xiàn)球形機(jī)器人的全向移動，其具有運(yùn)動能力強(qiáng)、抗傾倒和能耗低等特點(diǎn)。因此，球形機(jī)器人在安防巡檢、危險環(huán)境探測和野外偵察等[2]領(lǐng)域具有十分廣闊的應(yīng)用前景。

球形機(jī)器人具有獨(dú)特的機(jī)械結(jié)構(gòu)和運(yùn)動方式，由于其運(yùn)動姿態(tài)具有明顯的非線性特征，實(shí)時獲取球形機(jī)器人的姿態(tài)信息對其運(yùn)動控制至關(guān)重要。近年來，IMU多傳感器融合廣泛應(yīng)用于導(dǎo)航系統(tǒng)和姿態(tài)解算。但是，球形機(jī)器人工作時因慣性元件受隨機(jī)噪聲和外力加速度等因素的干擾，導(dǎo)致獲取的姿態(tài)角誤差增大和控制穩(wěn)定性下降等問題。2013 年，李景輝等[3]通過PI（Proportion Integration）調(diào)節(jié)和互補(bǔ)濾波進(jìn)行控制，提高了角速度精度;2016 年，ALLOTTA等[4]提出無跡卡爾曼濾波，減小了系統(tǒng)的線性誤差，但是計算量相對較大;2021 年，余晨雨等[5]提出的基于互補(bǔ)濾波和粒子濾波融合的球形機(jī)器人姿態(tài)解算方法，提高了球形機(jī)器人姿態(tài)解算的實(shí)時性和準(zhǔn)確性。

針對IMU慣性測量元件精度不高、穩(wěn)定性較差和易受隨機(jī)噪聲干擾等問題，本文通過擴(kuò)展卡爾曼濾波融合IMU的測量數(shù)據(jù)來進(jìn)行姿態(tài)解算?？紤]四元數(shù)偏差和角速度的誤差，結(jié)合加速度計和磁力計的測量值，利用擴(kuò)展卡爾曼濾波更新四元數(shù)以得到實(shí)時的全姿態(tài)角，并設(shè)計實(shí)驗(yàn)對所提出的姿態(tài)解算系統(tǒng)進(jìn)行驗(yàn)證。

2 ? 球形機(jī)器人系統(tǒng)設(shè)計（System design of spherical robot）

本文所搭建的球形機(jī)器人系統(tǒng)如圖1所示，由圓形透明外殼球和內(nèi)部硬件驅(qū)動模塊組成，通過控制舵機(jī)改變?nèi)f向輪，驅(qū)動電機(jī)來實(shí)現(xiàn)運(yùn)動及轉(zhuǎn)向等功能。透明球殼由兩個直徑25 cm的圓形透明亞克力半球拼裝組合而成;內(nèi)部驅(qū)動硬件模塊包括含有兩個霍爾編碼器（額定功率為4.32 W）的直流電機(jī)，四節(jié)功率為5500 mWh的18650鋰電池，一個電源分配板向核心板和電機(jī)供電，24 塊鋁塊配重塊（每塊5 g）降低機(jī)器人重心。

圖2為系統(tǒng)的控制結(jié)構(gòu)。本文球形機(jī)器人系統(tǒng)采用STM32F103C8T6作為核心板;對于IMU模塊，采用MPU6050整合的六軸傳感器獲取陀螺儀和加速度計的數(shù)據(jù)，采用MAG3110三軸磁力計獲取磁場強(qiáng)度信息;使用HC05藍(lán)牙作為無線通信模塊，根據(jù)自定義通信協(xié)議將姿態(tài)數(shù)據(jù)發(fā)送至上位機(jī)。

3 ? 姿態(tài)解算與濾波算法（Attitude calculation and filtering algorithms）

3.1 ? 歐拉角與四元數(shù)姿態(tài)解算

球形機(jī)器人的姿態(tài)角獲取通常是將陀螺儀、加速度計和磁力計的測量值進(jìn)行數(shù)據(jù)融合的濾波姿態(tài)解算，在計算過程中涉及兩個坐標(biāo)系之間的變換。其中慣性導(dǎo)航坐標(biāo)系對應(yīng)的為北東地（North East Down，NED）坐標(biāo)系，而通常慣性傳感元件的測量值定義在載體坐標(biāo)系B中。通過慣性導(dǎo)航坐標(biāo)系與載體坐標(biāo)系不同X、Y、Z軸對應(yīng)的夾角來描述載體的歐拉角，、和分別為俯仰角（pitch）、橫滾角（roll）和偏航角（yaw），如圖3所示。

用、、和、、分別表示慣性導(dǎo)航坐標(biāo)系和載體坐標(biāo)系，兩者空間關(guān)系對應(yīng)如下：

（1）

式（1）中，為慣性導(dǎo)航坐標(biāo)系到載體坐標(biāo)系的變化矩陣。設(shè)四元數(shù)[6]，通過四元數(shù)表示為：

（2）

由式（2）中的四元數(shù)轉(zhuǎn)移矩陣可以得到四元數(shù)和歐拉角的轉(zhuǎn)換關(guān)系為：

（3）

四元數(shù)的微分方程，寫成矩陣形式為：

（4）

式（4）中，為利用傳感器進(jìn)行數(shù)據(jù)融合得到的載體坐標(biāo)系下的三軸角速度。

球形機(jī)器人在探測、執(zhí)行任務(wù)的過程中對姿態(tài)解算的精度與實(shí)時性具有較高的要求，通過多傳感器進(jìn)行數(shù)據(jù)融合得到三軸角速度信息，接著利用一階龍格庫塔法進(jìn)一步更新四元數(shù)可以得到：

（5）

再結(jié)合式（3）可以得到實(shí)時更新的歐拉角[7]。

3.2 ? 基于互補(bǔ)濾波的姿態(tài)估計

互補(bǔ)濾波算法通過不同傳感器噪聲的特性來實(shí)現(xiàn)姿態(tài)解算。陀螺儀具有良好的動態(tài)響應(yīng)特性，其在較短的時間內(nèi)具有很高的解算精度。然而隨著時間的增加，積分的誤差會逐漸增大，加速度計和磁力計解算的姿態(tài)角在短時間內(nèi)精度較差，但測量誤差不隨著時間累積。因此，將陀螺儀獲得的瞬時姿態(tài)角和加速度計、磁力計長時間積分計算得到的姿態(tài)角分別作為互補(bǔ)濾波器的兩個輸入?；パa(bǔ)濾波器利用兩者在頻域上的互補(bǔ)特性提高姿態(tài)角的解算精度。對大多數(shù)互補(bǔ)濾波使用PI調(diào)節(jié)[8]，即：

（6）

其中，為比例調(diào)節(jié)，能夠及時成比例地反映系統(tǒng)的偏差，比例增益的大小決定了濾波器的截止頻率;為積分調(diào)節(jié)，能夠使系統(tǒng)消除穩(wěn)態(tài)誤差，適當(dāng)?shù)販p小積分增益能夠提升濾波的性能。基于PI調(diào)節(jié)的互補(bǔ)姿態(tài)解算流程圖如圖4所示，其中Minimum error為根據(jù)實(shí)際情況設(shè)置的最小誤差值。

分別將導(dǎo)航坐標(biāo)系下的重力加速度和地磁場通過矩陣轉(zhuǎn)移到載體坐標(biāo)系下，并且進(jìn)行歸一化后得到加速度計測量值和磁力計測量值。接著，對轉(zhuǎn)化前后的矢量進(jìn)行叉乘得到矢量之間的誤差角，總誤差矢量為：

（7）

通過PI調(diào)節(jié)和矢量誤差對陀螺儀得到的角速度進(jìn)行修正可得：

（8）

式（8）中，為t時刻的角速度，為更新的角速度，利用修正后的角速度更新四元數(shù)進(jìn)而反解得到歐拉角。

3.3 ? 基于擴(kuò)展卡爾曼濾波的姿態(tài)估計

卡爾曼濾波[9]是一種線性最小方差估計，其通過方差對測量值和預(yù)測值進(jìn)行加權(quán)而得到系統(tǒng)的估計值，廣泛使用于實(shí)時系統(tǒng)和嵌入式系統(tǒng)。其基本式如下：

（9）

式（9）中，為第k時刻的一步狀態(tài)估計，表示上一時刻的狀態(tài)估計值;為狀態(tài)矩陣，為一步預(yù)測均方誤差陣，為系統(tǒng)噪聲矩陣;為測量矩陣，表示測量值和狀態(tài)量之間的關(guān)系，為測量噪聲矩陣;表示第k時刻的狀態(tài)估計，表示第k時刻的測量值;表示第k時刻的估計均方誤差矩陣，為單位矩陣。其中，前兩項(xiàng)式表示狀態(tài)更新，后三項(xiàng)式表示測量更新。通過給定初值和，利用第k時刻的測量值遞推計算得到第k時刻的狀態(tài)估計值。

上述經(jīng)典的卡爾曼濾波更適用于已知噪聲的先驗(yàn)統(tǒng)計的線性控制系統(tǒng)，但球形機(jī)器人屬于非線性系統(tǒng)，測量噪聲和系統(tǒng)噪聲較難獲取，不確定性高，實(shí)時性差，并且姿態(tài)解算抗干擾的能力較弱，因此采用基于擴(kuò)展卡爾曼濾波的姿態(tài)解算來對球形機(jī)器人進(jìn)行姿態(tài)估計。

在擴(kuò)展卡爾曼濾波算法中，通過將陀螺儀的偏差和四元數(shù)的誤差作為狀態(tài)量，分別將磁力計和加速度計的測量值作為對應(yīng)的測量更新，進(jìn)而利用角速度誤差糾正四元數(shù)，最終求得全姿態(tài)角?；跀U(kuò)展卡爾曼濾波姿態(tài)解算原理的設(shè)計如圖5所示。

4 ? 實(shí)驗(yàn)與分析（Experiments and analysis）

為了驗(yàn)證本文所提出的姿態(tài)解算算法的精度與有效性，本文對以STM32F103C8T6為核心板搭載的球形機(jī)器人進(jìn)行實(shí)驗(yàn)。系統(tǒng)利用Keil軟件進(jìn)行編程，通過MPU6050獲得的陀螺儀和加速度計數(shù)據(jù)，以及磁力計MAG3110得到的磁場強(qiáng)度分別進(jìn)行基于互補(bǔ)濾波算法和擴(kuò)展卡爾曼濾波算法的姿態(tài)解算，獲得不同解算得到的全姿態(tài)角數(shù)據(jù)。利用藍(lán)牙串口通信導(dǎo)出數(shù)據(jù)并通過Python軟件進(jìn)行作圖對比，從不同波形比較俯仰角、橫滾角和偏航角的解算結(jié)果。

實(shí)驗(yàn)具體步驟為首先保持球形機(jī)器人三軸固定，設(shè)置采樣頻率為10 Hz，選取連續(xù)采樣150 s的數(shù)據(jù)，得到不同算法下的俯仰角、橫滾角和偏航角，將對比結(jié)果繪制成波形，如圖6所示。

從圖6可以看出，相對于互補(bǔ)濾波，基于擴(kuò)展卡爾曼濾波的姿態(tài)解算能夠有效抑制高頻噪聲，減小姿態(tài)角數(shù)據(jù)突變率，并且使得姿態(tài)角曲線變得平滑，提高了控制精度和魯棒性。

在實(shí)驗(yàn)中，理想的俯仰角、橫滾角和偏航角輸出都應(yīng)該為0°，引入四種誤差性能指標(biāo)進(jìn)行綜合評價。從表1中互補(bǔ)濾波與擴(kuò)展卡爾曼濾波對比結(jié)果可得出，后者四個誤差比較指標(biāo)都有所下降。以橫滾角為例，擴(kuò)展卡爾曼濾波算法相比于互補(bǔ)濾波的姿態(tài)解算，其均方根誤差下降0.1439，平均絕對誤差下降0.0426。對于全姿態(tài)角，經(jīng)表1數(shù)據(jù)計算，其均方根誤差和平均絕對誤差分別下降了0.0601和0.1984。從實(shí)驗(yàn)中可以看出，擴(kuò)展卡爾曼濾波算法在球形機(jī)器人系統(tǒng)下的姿態(tài)解算比互補(bǔ)濾波算法更為精確，波動范圍更小，抗噪聲干擾能力更強(qiáng)，總體曲線更加趨近于理論結(jié)果。

5 ? 結(jié)論（Conclusion）

本文針對基于IMU多傳感器的慣性測量元件精度不高、穩(wěn)定性較差和易受隨機(jī)噪聲干擾等問題，提出一種基于擴(kuò)展卡爾曼濾波的球形機(jī)器人姿態(tài)解算方法，利用該算法實(shí)時更新四元數(shù)以反解歐拉角獲取全姿態(tài)信息。本文基于搭建的球形機(jī)器人系統(tǒng)，設(shè)計實(shí)驗(yàn)比較本文算法和互補(bǔ)算法的姿態(tài)解算結(jié)果，相比之下，擴(kuò)展卡爾曼濾波算法的姿態(tài)精度更高，抗噪聲干擾和收斂性效果更加顯著。因此，本文提出的姿態(tài)解算方法在球形機(jī)器人的運(yùn)動控制等領(lǐng)域具有良好的適用性，未來還需在球形機(jī)器人控制領(lǐng)域做進(jìn)一步研究，以期得到更優(yōu)的姿態(tài)解算效果。

參考文獻(xiàn)（References）

[1] LI M， GUO S， HIRATA H， et al. Design and performance evaluation of an amphibious spherical robot[J]. Robotics and Autonomous Systems， 2015（64）：21-34.

[2] 戰(zhàn)強(qiáng)，李偉.球形移動機(jī)器人的研究進(jìn)展與發(fā)展趨勢[J].機(jī)械工程學(xué)報，2019，55（09）：1-17.

[3] 李景輝，楊立才.基于多傳感器信息融合的人體姿態(tài)解算算法[J].山東大學(xué)學(xué)報（工學(xué)版），2013，43（05）：49-54.

[4] ALLOTTA B， CAITI A， CHISCI L， et al. An unscented Kalman filter based navigation algorithm for autonomous underwater vehicles[J]. Mechatronics， 2016（39）：185-195.

[5] 余晨雨，章政，黃衛(wèi)華，等.基于互補(bǔ)濾波和粒子濾波融合的球形機(jī)器人姿態(tài)解算[J].機(jī)器人，2021，43（03）：340-349.

[6] 賈瑞才.基于四元數(shù)EKF的低成本MEMS姿態(tài)估計算法[J].傳感技術(shù)學(xué)報，2014，27（01）：90-95.

[7] 秦永元.慣性導(dǎo)航[M].北京：科學(xué)出版社，2006：298-300.

[8] 張澤權(quán).基于MEMS的運(yùn)動捕捉裝置及姿態(tài)解算算法設(shè)計[D].杭州：浙江大學(xué)，2021.

[9] 張棟，焦嵩鳴，劉延泉.互補(bǔ)濾波和卡爾曼濾波的融合姿態(tài)解算方法[J].傳感器與微系統(tǒng)，2017，36（03）：62-65，69.

作者簡介：

龍子洋（2000-），男，本科生.研究領(lǐng)域：多傳感器融合導(dǎo)航控制.

項(xiàng) ? 鵬（2000-），男，本科生.研究領(lǐng)域：多傳感器融合導(dǎo)航控制.

隋國榮（1974-），男，博士，副教授.研究領(lǐng)域：光電檢測，機(jī)器視覺.本文通信作者.