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

摘 ?要：針對(duì)球形機(jī)器人在姿態(tài)解算的過(guò)程中，慣性測(cè)量元件精度不高、穩(wěn)定性差和易受噪聲干擾從而導(dǎo)致無(wú)法精確控制其運(yùn)動(dòng)姿態(tài)的問(wèn)題，提出一種通過(guò)擴(kuò)展卡爾曼濾波融合IMU（Inertial Measurement Unit）慣性測(cè)量元件數(shù)據(jù)來(lái)進(jìn)行姿態(tài)解算的方法，利用多傳感器測(cè)量數(shù)據(jù)進(jìn)行融合，并使用擴(kuò)展卡爾曼濾波得到精確的姿態(tài)信息。通過(guò)相關(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，可見(jiàn)其對(duì)于球形機(jī)器人的運(yùn)動(dòng)控制具有良好的適用性。

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

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

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

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

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

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

圖2為系統(tǒng)的控制結(jié)構(gòu)。本文球形機(jī)器人系統(tǒng)采用STM32F103C8T6作為核心板;對(duì)于IMU模塊，采用MPU6050整合的六軸傳感器獲取陀螺儀和加速度計(jì)的數(shù)據(jù)，采用MAG3110三軸磁力計(jì)獲取磁場(chǎng)強(qiáng)度信息;使用HC05藍(lán)牙作為無(wú)線通信模塊，根據(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ì)和磁力計(jì)的測(cè)量值進(jìn)行數(shù)據(jù)融合的濾波姿態(tài)解算，在計(jì)算過(guò)程中涉及兩個(gè)坐標(biāo)系之間的變換。其中慣性導(dǎo)航坐標(biāo)系對(duì)應(yīng)的為北東地（North East Down，NED）坐標(biāo)系，而通常慣性傳感元件的測(cè)量值定義在載體坐標(biāo)系B中。通過(guò)慣性導(dǎo)航坐標(biāo)系與載體坐標(biāo)系不同X、Y、Z軸對(duì)應(yīng)的夾角來(lái)描述載體的歐拉角，、和分別為俯仰角（pitch）、橫滾角（roll）和偏航角（yaw），如圖3所示。

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

（1）

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

（2）

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

（3）

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

（4）

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

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

（5）

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

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

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

（6）

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

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

（7）

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

（8）

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

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

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

（9）

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

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

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

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

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

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

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

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

5 ? 結(jié)論（Conclusion）

本文針對(duì)基于IMU多傳感器的慣性測(cè)量元件精度不高、穩(wěn)定性較差和易受隨機(jī)噪聲干擾等問(wèn)題，提出一種基于擴(kuò)展卡爾曼濾波的球形機(jī)器人姿態(tài)解算方法，利用該算法實(shí)時(shí)更新四元數(shù)以反解歐拉角獲取全姿態(tài)信息。本文基于搭建的球形機(jī)器人系統(tǒng)，設(shè)計(jì)實(shí)驗(yàn)比較本文算法和互補(bǔ)算法的姿態(tài)解算結(jié)果，相比之下，擴(kuò)展卡爾曼濾波算法的姿態(tài)精度更高，抗噪聲干擾和收斂性效果更加顯著。因此，本文提出的姿態(tài)解算方法在球形機(jī)器人的運(yùn)動(dòng)控制等領(lǐng)域具有良好的適用性，未來(lái)還需在球形機(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)，李偉.球形移動(dòng)機(jī)器人的研究進(jìn)展與發(fā)展趨勢(shì)[J].機(jī)械工程學(xué)報(bào)，2019，55（09）：1-17.

[3] 李景輝，楊立才.基于多傳感器信息融合的人體姿態(tài)解算算法[J].山東大學(xué)學(xué)報(bào)（工學(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ì)算法[J].傳感技術(shù)學(xué)報(bào)，2014，27（01）：90-95.

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

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

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

作者簡(jiǎn)介：

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

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

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