全向運(yùn)動(dòng)球形機(jī)器人的設(shè)計(jì)與研究

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

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

關(guān)鍵詞：球形;機(jī)器人;全向運(yùn)動(dòng)

一、介紹

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

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

二、項(xiàng)目的研究思路和方法

（一）運(yùn)動(dòng)模塊設(shè)計(jì)

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

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

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

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

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

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

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

三、模型建立

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

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

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

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

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

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

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

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

四、研究成果展示

參考文獻(xiàn)：

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

[2]楊偉.一種全向運(yùn)動(dòng)的球形機(jī)器人研究[D].天津大學(xué)，2018.

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

作者簡介：

王淼（1998-），男，漢族，安徽蕪湖人，江蘇大學(xué)17級(jí)本科生，專業(yè)自動(dòng)化，研究方向?yàn)樽詣?dòng)控制

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

儲(chǔ)金城（1999-），男，漢族，江蘇常州人，江蘇大學(xué)17級(jí)本科生，專業(yè)自動(dòng)化，研究方向?yàn)樽詣?dòng)控制

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