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

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

0 引言

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

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

1 球形輪組設(shè)計(jì)

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

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

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

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

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

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

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

圖2 全向輪分布簡(jiǎn)圖

圖3 運(yùn)動(dòng)簡(jiǎn)圖

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

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

1.2 電氣設(shè)計(jì)

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

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

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

圖4 實(shí)物圖

圖5 物理模型簡(jiǎn)圖

2 動(dòng)力學(xué)分析

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

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

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

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

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

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

3 控制系統(tǒng)

3.1 控制算法

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

理想情況如下：

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

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

（3）各輪之間是絕對(duì)的互成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)動(dòng)學(xué)計(jì)算，從而控制直流電機(jī)讓球形輪組處于動(dòng)態(tài)平衡，系統(tǒng)不斷地進(jìn)行5 ms 中斷從而進(jìn)行小車姿態(tài)識(shí)別。其中姿態(tài)采集需要用到濾波算法才可得到穩(wěn)定的姿態(tài)數(shù)據(jù)。濾波部分分別通過(guò)低通和均值濾波處理加速度計(jì)和陀螺儀的原始數(shù)據(jù)，然后通過(guò)互補(bǔ)濾波實(shí)現(xiàn)兩個(gè)數(shù)據(jù)的融合，從而獲得俯仰角和側(cè)傾角數(shù)據(jù)。

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

圖6 控制算法流程圖

圖7 部分核心代碼

3.2 控制系統(tǒng)

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

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

圖8 硬件框圖

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

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

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

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

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

圖11 仿真實(shí)驗(yàn)

5 結(jié)束語(yǔ)

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

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