基于擴展卡爾曼濾波的球形機器人姿態解算

龍子洋 項鵬 隋國榮

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

關鍵詞：球形機器人;擴展卡爾曼濾波;互補濾波;姿態解算;四元數

中圖分類號：TP273 ? ? 文獻標識碼：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）

球形機器人作為一種新型移動機器人，其所有機械結構和控制系統都包含于球形外殼內，通過質心偏移、動量守恒等[1]原理實現球形機器人的全向移動，其具有運動能力強、抗傾倒和能耗低等特點。因此，球形機器人在安防巡檢、危險環境探測和野外偵察等[2]領域具有十分廣闊的應用前景。

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

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

2 ? 球形機器人系統設計（System design of spherical robot）

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

圖2為系統的控制結構。本文球形機器人系統采用STM32F103C8T6作為核心板;對于IMU模塊，采用MPU6050整合的六軸傳感器獲取陀螺儀和加速度計的數據，采用MAG3110三軸磁力計獲取磁場強度信息;使用HC05藍牙作為無線通信模塊，根據自定義通信協議將姿態數據發送至上位機。

3 ? 姿態解算與濾波算法（Attitude calculation and filtering algorithms）

3.1 ? 歐拉角與四元數姿態解算

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

用、、和、、分別表示慣性導航坐標系和載體坐標系，兩者空間關系對應如下：

（1）

式（1）中，為慣性導航坐標系到載體坐標系的變化矩陣。設四元數[6]，通過四元數表示為：

（2）

由式（2）中的四元數轉移矩陣可以得到四元數和歐拉角的轉換關系為：

（3）

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

（4）

式（4）中，為利用傳感器進行數據融合得到的載體坐標系下的三軸角速度。

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

（5）

再結合式（3）可以得到實時更新的歐拉角[7]。

3.2 ? 基于互補濾波的姿態估計

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

（6）

其中，為比例調節，能夠及時成比例地反映系統的偏差，比例增益的大小決定了濾波器的截止頻率;為積分調節，能夠使系統消除穩態誤差，適當地減小積分增益能夠提升濾波的性能?；赑I調節的互補姿態解算流程圖如圖4所示，其中Minimum error為根據實際情況設置的最小誤差值。

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

（7）

通過PI調節和矢量誤差對陀螺儀得到的角速度進行修正可得：

（8）

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

3.3 ? 基于擴展卡爾曼濾波的姿態估計

卡爾曼濾波[9]是一種線性最小方差估計，其通過方差對測量值和預測值進行加權而得到系統的估計值，廣泛使用于實時系統和嵌入式系統。其基本式如下：

（9）

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

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

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

4 ? 實驗與分析（Experiments and analysis）

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

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

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

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

5 ? 結論（Conclusion）

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

參考文獻（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] 戰強，李偉.球形移動機器人的研究進展與發展趨勢[J].機械工程學報，2019，55（09）：1-17.

[3] 李景輝，楊立才.基于多傳感器信息融合的人體姿態解算算法[J].山東大學學報（工學版），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] 余晨雨，章政，黃衛華，等.基于互補濾波和粒子濾波融合的球形機器人姿態解算[J].機器人，2021，43（03）：340-349.

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

[7] 秦永元.慣性導航[M].北京：科學出版社，2006：298-300.

[8] 張澤權.基于MEMS的運動捕捉裝置及姿態解算算法設計[D].杭州：浙江大學，2021.

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

作者簡介：

龍子洋（2000-），男，本科生.研究領域：多傳感器融合導航控制.

項 ? 鵬（2000-），男，本科生.研究領域：多傳感器融合導航控制.

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