球形輪組結(jié)構(gòu)及其控制系統(tǒng)設(shè)計*

翟建麗，莫浩明，曾德勝，周 杰，王可涵，陸厚霖，胡恰鋒，王作桓

（廣東工業(yè)大學(xué)華立學(xué)院，廣州 511300）

0 引言

輪式移動作為運(yùn)輸工具的主要形式，其制造簡便和價值成本低，而球形輪組具有靈活性、占地面積小、易于搬動，可以運(yùn)用在代步工具、機(jī)器人和危險地帶運(yùn)輸?shù)取鴥?nèi)外為此做了大量研究，球形輪組結(jié)構(gòu)最早應(yīng)用在機(jī)器人中。2006年美國卡內(nèi)基梅隆大學(xué)將球形輪組結(jié)構(gòu)運(yùn)用在BBR機(jī)器人[1]；2008年日本東北學(xué)（TGU）采用3 個全向輪結(jié)構(gòu)驅(qū)動技術(shù)和2010年蘇黎世理工聯(lián)邦理工學(xué)院開發(fā)了類似于TGU 的原型，具有較高的魯棒性[2]；2019年加州大學(xué)圣地亞哥大學(xué)對BBR機(jī)器人的機(jī)械設(shè)計、動態(tài)分析等[3]；2012年同濟(jì)大學(xué)的單球機(jī)器人建模和控制[4]；2014年以阮曉鋼為首對獨(dú)輪機(jī)器人進(jìn)行自抗擾研究[5]。

本文根據(jù)球形輪組的工作原理和自身存在的非線性、強(qiáng)耦合和欠驅(qū)動等問題進(jìn)行研究。利用三維建模軟件建立物理模型進(jìn)行動力學(xué)分析，設(shè)計控制算法與ROS 系統(tǒng)建立的上位機(jī)聯(lián)合，利用注模技術(shù)建立樣機(jī)對球形輪組進(jìn)行研究。

1 球形輪組設(shè)計

1.1 機(jī)械結(jié)構(gòu)設(shè)計

球形輪組由3 個全向輪、球輪和車身組成，車身上搭載電源、監(jiān)測系統(tǒng)、控制系統(tǒng)和執(zhí)行系統(tǒng)等部分構(gòu)成，如圖1所示。全向輪通過聯(lián)軸器直接與電機(jī)連接，控制電機(jī)的力矩間接控制全向輪在球輪上的轉(zhuǎn)速和方向，機(jī)身由兩層構(gòu)成，在頂層用于固定電源和主控板，主控板包括控制模塊和監(jiān)測模塊；底層用于固定與全向輪直接連接的電機(jī)。

圖1 物理模型設(shè)計圖

當(dāng)全向輪組受到外界干擾時，3個全向輪通過驅(qū)動球輪朝傾斜方向運(yùn)動來維持自身平衡，俯視圖下與電機(jī)相連的3個全向輪兩兩相互成120°，因此通過控制電機(jī)的力矩調(diào)整全向輪的轉(zhuǎn)速和方向，俯視圖下全向輪分布簡圖如圖2所示。

圖中，vr1,vr2,vr3分別為3個全向輪的速度，v為某一時刻的速度，根據(jù)公式推導(dǎo)出3個全向輪的速度和某一時刻的速度關(guān)系式如下所示：

式中：vr1～vr3為全向輪1～3的速度；v為某一時刻的速度。

球形輪組通過利用MPU6050陀螺儀傳感器監(jiān)視機(jī)身的姿態(tài)變化，并進(jìn)行處理后傳入給控制模塊，及時做出反應(yīng)去調(diào)整電機(jī)的轉(zhuǎn)速和方向。球形輪組在平面上的運(yùn)動速度與全向輪速度的關(guān)系，在xy平面內(nèi)的運(yùn)動簡圖如圖3所示。

圖2 全向輪分布簡圖

圖3 運(yùn)動簡圖

根據(jù)簡圖和公式推導(dǎo)出如下的速度關(guān)系式：

式中：ω為輪組的角速度；L為全向輪的中心與底盤的中心距離；vA、vB、vC為3個全向輪的轉(zhuǎn)速；vx、vy為球形輪組在x、y方向的運(yùn)動速度。

1.2 電氣設(shè)計

球形輪組為了保證自身姿態(tài)平衡的同時，保證在平面內(nèi)任意方向移動，其電氣部分需要包括電源、監(jiān)測、控制和執(zhí)行部分——電機(jī)。電源是為各設(shè)備提供穩(wěn)定的電壓，另外需要考慮到整體的重量和體積，因此需要選擇重量輕和體積小的電源。因此采用12 V的鋰電池作為總電源，分別通過電機(jī)驅(qū)動和電壓轉(zhuǎn)換器提供相應(yīng)電壓給電機(jī)、控制系統(tǒng)和MPU6050陀螺儀傳感器等。

監(jiān)測系統(tǒng)采用陀螺儀傳感器可以實(shí)時監(jiān)測球形輪組的姿態(tài)變化和速度變化；控制系統(tǒng)中采用STM32 芯片，可以根據(jù)監(jiān)測系統(tǒng)采集的數(shù)據(jù)進(jìn)行計算處理獲得直流電機(jī)的控制變量；執(zhí)行系統(tǒng)根據(jù)控制部分的控制變量去控制電機(jī)的扭矩。

根據(jù)球形輪組的機(jī)械結(jié)構(gòu)與電氣部分的設(shè)計，利用注模技術(shù)建立試樣機(jī)的機(jī)身，根據(jù)圖1 的設(shè)計圖設(shè)計的試樣機(jī)如圖4所示。

圖4 實(shí)物圖

圖5 物理模型簡圖

2 動力學(xué)分析

球形輪組是一個多剛體機(jī)械結(jié)構(gòu)，非線性、強(qiáng)耦合和欠驅(qū)動，為適應(yīng)自身的自平衡，需要設(shè)計一套控制算法。在設(shè)計控制算法前需要先進(jìn)行動力學(xué)分析，因為與外部環(huán)境接觸受到干擾，需要根據(jù)控制部分的控制變量來控制電機(jī)的力矩，所以需要考慮球形輪組的電機(jī)驅(qū)動扭矩與驅(qū)動輪的力和力矩。球形輪組屬于多剛體結(jié)構(gòu)，因此采用拉格朗日方程對球形輪組的動力學(xué)分析，可以不考慮剛體內(nèi)部的關(guān)系，同時把球形輪組分成全向輪、球輪和機(jī)身三部分。如圖5 的物理模型簡圖所示，由于球形輪組是一個質(zhì)心高于支點(diǎn)的特征，在自身重力作用下無法保證球形輪組的自身平衡。根據(jù)全向輪與球輪和電機(jī)的電機(jī)關(guān)系，有利于非線性控制算法的設(shè)計，其拉格朗日方程[6]為：

式中：T為系統(tǒng)總動能，W為系統(tǒng)總勢能。則：

通過拉格朗日方程建模和分析，得到電機(jī)在球輪上的力矩為：

3個電機(jī)的實(shí)際力矩為：

式（3）～（6）中：T為總動能；W為總勢能；下標(biāo)b、w、a分別為球輪、全向輪、機(jī)身；Tb為電機(jī)在球輪上的力矩；rw為全向輪半徑；T1、T2、T3分別為3個電機(jī)的實(shí)際力矩。

依據(jù)剛體力學(xué)分析，部分零件不受到力的影響或者可以在球輪組內(nèi)實(shí)現(xiàn)力系平衡，可以不考慮到整體運(yùn)動中去，所以機(jī)械結(jié)構(gòu)簡化后得到如圖5所示的物理結(jié)構(gòu)簡圖。

3 控制系統(tǒng)

3.1 控制算法

球形輪組的控制主要包括自平衡控制與位置控制，采用兩個相同的LQR狀態(tài)反饋控制器分別對單球自平衡移動機(jī)器人等效的兩個一級倒立擺進(jìn)行自平衡控制與位置跟蹤控制[7-8]。

理想情況如下：

（1）全向輪不與球面打滑，同時球面有足夠摩擦力；

（2）電機(jī)軸線中心正是底盤重心；

（3）各輪之間是絕對的互成120°安裝；

（4）電機(jī)軸線中心正是底盤重心。

軟件控制流程如圖6 所示，小車上電后，系統(tǒng)模塊進(jìn)入初始化，系統(tǒng)進(jìn)入當(dāng)前狀態(tài)平衡點(diǎn)采集，采集完后進(jìn)入系統(tǒng)5 ms 中斷服務(wù)函數(shù)，進(jìn)行姿態(tài)數(shù)據(jù)采集和逆運(yùn)動學(xué)計算，從而控制直流電機(jī)讓球形輪組處于動態(tài)平衡，系統(tǒng)不斷地進(jìn)行5 ms 中斷從而進(jìn)行小車姿態(tài)識別。其中姿態(tài)采集需要用到濾波算法才可得到穩(wěn)定的姿態(tài)數(shù)據(jù)。濾波部分分別通過低通和均值濾波處理加速度計和陀螺儀的原始數(shù)據(jù)，然后通過互補(bǔ)濾波實(shí)現(xiàn)兩個數(shù)據(jù)的融合，從而獲得俯仰角和側(cè)傾角數(shù)據(jù)。

部分控制算法如圖7所示。

圖6 控制算法流程圖

圖7 部分核心代碼

3.2 控制系統(tǒng)

球形輪組的控制系統(tǒng)用來接收MPU6050陀螺儀傳感器獲取的姿態(tài)信息以及上位機(jī)發(fā)送的控制信息，最后根據(jù)所編寫的控制算法求出在某一時刻的狀態(tài)所對應(yīng)的電機(jī)控制量。球形輪組的研究采用ARM公司的STM32F103ZET6芯片，其最高工作頻率為72 MHz，F(xiàn)LASH 為512 k 和32 位的CPU，具有豐富的I/O接口、庫函數(shù)和開源代碼，因此開發(fā)容易，并滿足球形輪組底層平衡控制的要求。

根據(jù)圖8 的硬件框圖所示，MPU6050 陀螺儀傳感器采集輪組姿態(tài)數(shù)據(jù)并經(jīng)過卡爾曼濾波處理后輸入到控制系統(tǒng)ROS，然后與姿態(tài)平衡點(diǎn)的條件進(jìn)行比較，MCU 根據(jù)姿態(tài)偏離程度向電機(jī)驅(qū)動發(fā)出糾正信號，驅(qū)動步進(jìn)電機(jī)進(jìn)行動態(tài)平衡調(diào)節(jié)。

圖8 硬件框圖

如圖9 所示，在MCU 上搭載好ROS 系統(tǒng)，在ROS 中輸入ifconfig命令查詢小車的IP地址，確保上位機(jī)控制端與小車是處于同一局域網(wǎng)下。接著修改控制端的bashrc 配置文件，把小車IP地址填入。因為小車是作為ROS的主節(jié)點(diǎn)，所以需要修改export ROS_MASTER_URI的地址為小車IP地址。完成上述步驟后，通過SSH 連接ROS 小車。可以用命令rosrun rviz rviz查看小車的rviz運(yùn)動方向。用上位機(jī)控制小車運(yùn)動和實(shí)時PID調(diào)參，如圖10所示。

圖9 rviz運(yùn)動狀態(tài)查看

圖10 控制系統(tǒng)界面

4 仿真結(jié)構(gòu)

結(jié)合上述對球形輪組軟、硬件的設(shè)計和分析，需要驗證其自平衡和控制系統(tǒng)的可實(shí)驗性。首先將建立的物理模型導(dǎo)入仿真軟件MATLAB 中，并結(jié)合串級PID控制的對應(yīng)參數(shù)設(shè)置進(jìn)行仿真性試驗，得出如圖11 所示的仿真對比實(shí)驗結(jié)果。由圖可知，球形輪組的姿態(tài)角與平衡值發(fā)生偏離時，控制模塊能夠及時接收到傳感器傳送過來的姿態(tài)數(shù)據(jù)與串級PID 控制算法結(jié)合計算出電機(jī)的控制變量，迅速改變電機(jī)的扭矩來調(diào)整相應(yīng)全向輪轉(zhuǎn)動速度和方向，保證自身的平衡，但是不能保證與理想條件的平衡點(diǎn)相重合，因為研究過程中屬于一個理想條件下，沒有考慮到能量方面的損失。

圖11 仿真實(shí)驗

5 結(jié)束語

針對輪式移動運(yùn)輸工具的不靈活性、占地面積大、不易于搬動的問題，本文提出了球形輪組結(jié)構(gòu)和控制系統(tǒng)的設(shè)計，實(shí)現(xiàn)球形輪組無轉(zhuǎn)彎半徑并且靈活地運(yùn)動。采用拉格朗日方程進(jìn)行動力學(xué)分析，設(shè)計出結(jié)構(gòu)最佳的球形輪組，推出電機(jī)扭矩與3個全向輪在球輪上的運(yùn)動關(guān)系，據(jù)此設(shè)計出擁有傾角環(huán)、速度環(huán)、轉(zhuǎn)向環(huán)的串級PID算法，同時設(shè)定好PID參數(shù)的初始值。本文提出運(yùn)用ROS 系統(tǒng)建立上位機(jī)實(shí)現(xiàn)人機(jī)交互，讓用戶時時刻刻能監(jiān)控輪組的姿態(tài)并做出調(diào)整，同時進(jìn)一步完善PID參數(shù)設(shè)置。仿真試驗驗證了串級PID對球形輪組自平衡控制的可行性。

最后針對球形輪組自平衡的實(shí)驗與實(shí)地測試，證明了拉格朗日力學(xué)分析的方法能更直觀地分析球形輪組物理結(jié)構(gòu)與剛體力系劃分；ROS上位機(jī)在調(diào)試PID的方面具有快速反應(yīng)、實(shí)時監(jiān)控、簡化調(diào)試的優(yōu)點(diǎn)。兩種方法互補(bǔ)，把抽象的物理力學(xué)平衡用程序?qū)崿F(xiàn)，為此類復(fù)雜繁瑣的調(diào)試做出了系統(tǒng)的實(shí)施方案。