全向運動球形機器人的設(shè)計與研究

王淼　馮紀(jì)超　儲金城

摘 要：本文探討了一種基于四元數(shù)的擴展卡爾曼濾波器（EKF），該濾波器用于使用三軸陀螺儀、加速度計和磁強計估計非完整球形機器人的姿態(tài)。低成本慣性測量單元（IMU）和磁強計安裝在球形機器人上，并將測量數(shù)據(jù)與EKF進(jìn)行融合，以確定機器人的姿態(tài)。球形機器人的姿態(tài)時間參數(shù)曲線，因此是驗證其全局態(tài)度的理想曲線。在圓形和三葉形軌跡上進(jìn)行了室內(nèi)實驗。建立了利用估計姿態(tài)對機器人運動學(xué)進(jìn)行積分，并與參考軌跡進(jìn)行比較。

關(guān)鍵詞：球形;機器人;全向運動

一、介紹

姿態(tài)和方位估計是導(dǎo)航和控制飛行器、仿人機器人、室內(nèi)定位和跟蹤應(yīng)用的關(guān)鍵步驟。利用慣性、GPS、里程表、激光、雷達(dá)和聲納傳感器來估計移動機器人的姿態(tài)。由于測量不準(zhǔn)確，低成本傳感器很少提供良好的性能在不同的環(huán)境中。 隨著微機電系統(tǒng)技術(shù)的出現(xiàn)，使用緊湊、低成本的慣性測量單元的情況呈指數(shù)增長。 慣性傳感器的輸出結(jié)合磁強計和基于視覺的系統(tǒng)進(jìn)行融合，確定移動機器人的航向角。一般來說，態(tài)度可以分為三種形式，即歐拉角、方向余弦矩陣和四元數(shù)。由于九種元素，方向余弦矩陣需要很大的存儲空間，歐拉角受到一個稱為“萬向節(jié)鎖”的奇點的影響，即一在特定的方向上失去一個自由度。四元數(shù)只需要四個元素來存儲，而不會受到歐拉角所面臨的奇點，因此它是首選。

姿態(tài)估計算法的驗證需要昂貴的三自由度含氣模擬器。球形機器人相對于其他輪式機構(gòu)的最大優(yōu)勢之一是全向移動。得到的各種數(shù)學(xué)模型在Euler-Poincare方程中得到了不同執(zhí)行器配置的球形機器人擺動驅(qū)動在和滑動質(zhì)量驅(qū)動。公式提供了使用三個內(nèi)部轉(zhuǎn)子的球形動力學(xué)的幾何可控性證明。

二、項目的研究思路和方法

（一）運動模塊設(shè)計

本項目中使用TB6612FNG微型雙路直流電機驅(qū)動模塊。電機自帶編碼器，實現(xiàn)力矩、速度以及點位控制，完成球形機器人前進(jìn)、后退以及速度控制。TB6612FNG內(nèi)部集成了2個H橋電機驅(qū)動電路、6個控制信號輸入端（其中2個為PWM控制信號輸入端、4個為方向信號輸入端），可同時控制球形機器人2個電機的正轉(zhuǎn)、反轉(zhuǎn)和制動。球形機器人轉(zhuǎn)向動作由舵機控制，舵機具有位置（角度）伺服驅(qū)動功能，滿足球形機器人的轉(zhuǎn)向要求。通過設(shè)置單片機的PWM占空比值來控制舵機偏轉(zhuǎn)角，從而實現(xiàn)球型機器人的轉(zhuǎn)向控制。

（二）姿態(tài)檢測模塊設(shè)計

姿態(tài)檢測模塊選用MPU-6050六軸傳感器，該模塊集成了3軸微機電系統(tǒng)（micro-electro-mechanical system，MEMS）陀螺儀和3軸MEMS加速度計，以及一個可擴展的數(shù)字運動處理器（digital motion processor，DMP）。當(dāng)產(chǎn)生外部中斷時，單片機進(jìn)入中斷，并讀取MPU-6050六軸傳感器中的各姿態(tài)參數(shù)值，此參數(shù)值通過卡爾曼濾波、增量式PID控制算法修正后發(fā)送給球體的控制單元，實時調(diào)整球體姿態(tài)，控制球體在運動過程中的自身平衡。

（三）控制系統(tǒng)軟件設(shè)計

因球形機器人控制系統(tǒng)具有非線性、不穩(wěn)定的特點，相應(yīng)的軟件設(shè)計就成為了關(guān)鍵。根據(jù)系統(tǒng)要求，軟件主要任務(wù)是完成系統(tǒng)的初始化設(shè)置、球體姿態(tài)信息采集、卡爾曼濾波、角速度檢測、電機雙閉環(huán)PID控制以及藍(lán)牙通信等。

（1）雙閉環(huán)PID

控制當(dāng)球型機器人運動時，球體姿態(tài)隨之改變，為了保證球體自身平衡，需設(shè)計相應(yīng)的控制方法，這里采用工程實踐中應(yīng)用最為廣泛的增量式PID控制算法，分別設(shè)計球體的姿態(tài)PID控制和球體速度PID控制。

三、模型建立

圖1所示的球形機器人示意圖由一個在水平面上滾動的質(zhì)量為m、半徑為r的球殼組成。假設(shè)機器人的重心與幾何中心。點O1的位置坐標(biāo)關(guān)于O用（x，y）表示。車身骨架的定位（Xb，Yb，Zb）相對于慣性系（Xi，Yi，Zi）的機器人由旋轉(zhuǎn)矩陣R∈SO給出。該機器人有三個獨立控制的轉(zhuǎn)子，轉(zhuǎn)子上有慣性輪軸線。因此，三個獨立的力矩作用在生成車身坐標(biāo)系。表示轉(zhuǎn)子角度乘以Ψi，i=1，2，3。球形機器人的配置空間，忽略轉(zhuǎn)子角度，Q=R2×SO由曲線q（t）=（x（t），y（t），R（t））∈q。球形機器人受到無滑移約束。

式中，ω∈R3表示物體角速度，Ω∈R3表示物體角速度是機器人的空間角速度。的運動學(xué)球形機器人由下式給出

其中旋轉(zhuǎn)矩陣R的行用R1，R2，R3表示。

IMU由加速度計、陀螺儀和磁力計組成，安裝在球形機器人的中心位置，提供原始數(shù)據(jù)測量。陀螺儀測量物體的角速度ω=（ωx，ωy，ωz）。加速度計和磁強計分別測量線加速度（ax，ay，az）和地球慣性系中的磁場矢量（mx，my，mz）。傳感器假定讀數(shù)具有恒定的偏差b=（ba，bg，bm）和被獨立的高斯白噪聲污染η=（ηa，ηΨ，ηm）加速度計的平均值和方差為零σ2 = （σ2a， σ2g， σ2m），陀螺儀和磁強計。因此，9自由度傳感器模型有

從加速度計和磁強計的數(shù)據(jù)組成部分，可以得到如下歐拉角

測量矢量包括每個時代的加速度計和磁強計讀數(shù)。 慣性系中的參考加速度a0和磁場矢量m0被選擇為a0=（0，0，1）a分別為dM0=（MX，0，MZ）。 物體框架內(nèi)的加速度和磁場矢量可以在任意時刻k下計算為

關(guān)于xk的雅可比是狀態(tài)方程中的觀測矩陣Hk。 通過泰勒展開，F(xiàn)和H的雅可比矩陣是f和h的偏導(dǎo)數(shù)

擴展卡爾曼濾波是一種兩步迭代隨機算法，它給出了每個時代的狀態(tài)和誤差協(xié)方差。它首先根據(jù)系統(tǒng)模型預(yù)測狀態(tài)先驗，然后是根據(jù)實際觀測結(jié)果進(jìn)行估計。 假設(shè)狀態(tài)的初始狀態(tài)和誤差協(xié)方差矩陣分別為 和 ，而Kk是卡爾曼增益。 Qk和Rk是過程噪聲協(xié)方差矩陣和測量噪聲協(xié)方差矩陣。

四、研究成果展示

參考文獻(xiàn)：

[1]劉家念.球形機器人未來發(fā)展方向研究[J].科技創(chuàng)新與應(yīng)用，2015（18）：61.

[2]楊偉.一種全向運動的球形機器人研究[D].天津大學(xué)，2018.

[3]吳晗，徐開蕓，朱昊，李家釗.基于單片機的球型機器人控制系統(tǒng)設(shè)計[J].機械設(shè)計與制造工程，2018，47（11）：53-57.

作者簡介：

王淼（1998-），男，漢族，安徽蕪湖人，江蘇大學(xué)17級本科生，專業(yè)自動化，研究方向為自動控制

馮紀(jì)超（1999-），男，漢族，江蘇徐州人，江蘇大學(xué)17級本科生，專業(yè)自動化，研究方向為自動控制

儲金城（1999-），男，漢族，江蘇常州人，江蘇大學(xué)17級本科生，專業(yè)自動化，研究方向為自動控制

基金項目：本文是江蘇大學(xué)2019年度大學(xué)生科研課題立項資助項目，項目編號：18B007