基于姿態(tài)角的輪腿機(jī)器人驅(qū)動(dòng)控制系統(tǒng)研究

(東南大學(xué) 儀器科學(xué)與工程學(xué)院，江蘇 南京 210096)

移動(dòng)機(jī)器人作為機(jī)器人領(lǐng)域的重要分支，可在地震、核輻射及火災(zāi)等復(fù)雜惡劣環(huán)境中代替人類進(jìn)行探索和作業(yè)，具有廣闊的應(yīng)用前景[1]。其中輪腿型機(jī)器人同時(shí)具有攀爬能力強(qiáng)和操控簡(jiǎn)單的特征，完全可以勝任城市環(huán)境下尤其是攀爬樓梯等工作[2]。且已有研究表明，五星形輪腿結(jié)構(gòu)同時(shí)兼顧了機(jī)器人的越障能力和速度等性能[3]。

圖1中的機(jī)器人為本項(xiàng)目組團(tuán)隊(duì)研制的五星形輪腿結(jié)構(gòu)機(jī)器人，其結(jié)構(gòu)緊湊，尺寸適中，裝配有一個(gè)四自由度機(jī)械手，可實(shí)現(xiàn)在城市復(fù)雜危險(xiǎn)環(huán)境中探索作業(yè)，尤其適合攀爬樓梯等復(fù)雜任務(wù)。該輪腿機(jī)器人的各項(xiàng)結(jié)構(gòu)參數(shù)、質(zhì)量參數(shù)和機(jī)電性能如表1所示。

但是在該類輪腿機(jī)器人通過樓梯時(shí)，往往會(huì)因輪腿的相位角差異、摩擦打滑、輪腿磨損等因素導(dǎo)致輪腿機(jī)器人在攀爬過程中產(chǎn)生大幅偏移[4]。例如圖1中，由相位角差異導(dǎo)致左前輪腿和右前輪腿的主受力腿分別踏在上下兩層不同的樓梯上，因?yàn)闄C(jī)器人機(jī)體較大，較小的偏航角也會(huì)導(dǎo)致機(jī)器人在樓梯上產(chǎn)生較大的偏移，即增大了機(jī)器人不穩(wěn)定性，容易造成機(jī)器人傾翻。同時(shí)由于遠(yuǎn)程無線傳輸?shù)臅r(shí)延和操控者反應(yīng)滯后等因素，極易造成機(jī)器人偏航角度增大，進(jìn)而碰撞到左右墻壁。圖2為遠(yuǎn)程遙操作機(jī)器人時(shí)機(jī)器人姿態(tài)傳感器傳回的偏航角和俯仰角的變化圖形，圖中淺灰色曲線為機(jī)器人在爬樓前標(biāo)定初始數(shù)值后計(jì)算所得的偏航角，可以用于實(shí)時(shí)反饋機(jī)器人的偏航程度。所以在操控者遙操作機(jī)器人攀爬樓梯時(shí)，可利用偏航角的姿態(tài)實(shí)時(shí)調(diào)整機(jī)器人。

圖1 輪腿機(jī)器人攀爬樓梯照片

項(xiàng)目參數(shù)機(jī)器人尺寸外形尺寸(含輪腿)(長(zhǎng)/寬/高)/mm980/670/340輪距(前后/左右)/mm614/630整備質(zhì)量/kg40輪子外圍直徑/mm380電機(jī)性能控制模式速度模式額定轉(zhuǎn)速/r·min-155額定扭矩/N·m22.3爬樓轉(zhuǎn)速/r·min-112

圖2 攀爬樓梯過程中姿態(tài)曲線圖

為實(shí)現(xiàn)基于姿態(tài)角對(duì)輪腿機(jī)器人進(jìn)行控制，快速、低成本地開發(fā)出機(jī)器人閉環(huán)控制系統(tǒng)。擬采用基于建模與仿真的虛擬樣機(jī)技術(shù)對(duì)機(jī)械和控制系統(tǒng)進(jìn)行聯(lián)合分析，即通過仿真軟件ADAMS 和Simulink建立輪腿機(jī)器人虛擬樣機(jī)的聯(lián)合仿真，并對(duì)整體特性進(jìn)行反復(fù)的仿真測(cè)試，減少實(shí)際機(jī)器人調(diào)試的時(shí)間[5]。為驗(yàn)證機(jī)器人攀爬樓梯時(shí)的穩(wěn)定性，采用重心投影法，分析了機(jī)器人攀爬過程中的穩(wěn)定性能。

1 基于姿態(tài)的輪腿機(jī)器人驅(qū)動(dòng)控制

1.1 機(jī)器人驅(qū)動(dòng)控制目標(biāo)

輪腿機(jī)器人在攀爬樓梯前進(jìn)過程中，4個(gè)輪腿的相位角度等狀況不可能絕對(duì)同步，所以在其攀爬樓梯過程中，偏航角始終處于一定范圍內(nèi)變動(dòng)。本驅(qū)動(dòng)控制系統(tǒng)的設(shè)計(jì)目標(biāo)是防止前側(cè)左右2個(gè)輪腿出現(xiàn)圖1中分別跨越上下2層臺(tái)階而導(dǎo)致機(jī)器人出現(xiàn)較大偏航。圖3所示為測(cè)得的機(jī)器人前側(cè)左右2個(gè)輪腿分別跨越2層臺(tái)階和同處于1層臺(tái)階下的各3次的偏航角。

圖3 攀爬樓梯過程中姿態(tài)曲線圖

由圖3中兩種情況下各3次的偏航角的情況可知，前側(cè)左右2個(gè)輪腿同處于1層臺(tái)階時(shí)的偏航角約在4°包絡(luò)線內(nèi)。左右2個(gè)輪腿跨越2層臺(tái)階時(shí)的偏航角會(huì)超過5°，所以選用5°為此次仿真中是否進(jìn)行偏航控制的判斷依據(jù)。

1.2 機(jī)器人動(dòng)力學(xué)建模

通過SOLIDWORKS建立五星形輪腿機(jī)器人和樓梯的三維模型。本仿真設(shè)置一旦出現(xiàn)5°偏航角時(shí)就對(duì)機(jī)器人的姿態(tài)開始調(diào)整，所以仿真模型中直接設(shè)置機(jī)器人初始位置有5°的偏航角。為簡(jiǎn)化在ADAMS/Simulink聯(lián)合仿真軟件中操作并降低計(jì)算量，對(duì)實(shí)際模型進(jìn)行適當(dāng)簡(jiǎn)化，其中機(jī)器人搭載的機(jī)械臂和內(nèi)部的電機(jī)、驅(qū)動(dòng)控制器等均簡(jiǎn)化成等重量、同重心的立方體重塊置于機(jī)器人機(jī)體內(nèi)，輪腿機(jī)器人采用4個(gè)獨(dú)立的減速電機(jī)單獨(dú)驅(qū)動(dòng)。樓梯模型根據(jù)GB 50352-2005民用建筑設(shè)計(jì)通則[6]，設(shè)置寬度為28 mm，高度設(shè)為18 mm。將SOLIDWORKS中的三維模型導(dǎo)入ADAMS/VIEW中模型如圖4所示。

圖4 輪腿機(jī)器人ADAMS/VIEW模型

將三維模型導(dǎo)入后，為系統(tǒng)添加相關(guān)的載荷和約束，其中除了4個(gè)輪腿的旋轉(zhuǎn)副外，還添加了機(jī)器人可轉(zhuǎn)動(dòng)腰部關(guān)節(jié)的旋轉(zhuǎn)副。再分別設(shè)置左側(cè)兩路電機(jī)和右側(cè)兩路電機(jī)的轉(zhuǎn)速為2個(gè)輸入變量；設(shè)置樓梯重心為基準(zhǔn)，測(cè)量機(jī)器人重心的偏航角YAW變量為輸出變量。

1.3 聯(lián)合仿真模型

利用ADAMS/Control 模塊，可以將機(jī)械系統(tǒng)仿真分析工具與控制設(shè)計(jì)仿真軟件Matlab有機(jī)地連接起來，實(shí)現(xiàn)以下功能：

① 把復(fù)雜的控制加到ADAMS模型上，可以一次仿真整個(gè)系統(tǒng)，遇到問題，可從機(jī)械系統(tǒng)和控制系統(tǒng)協(xié)調(diào)的角度來解決[7]。

② 直接從ADAMS模型中產(chǎn)生機(jī)械系統(tǒng)仿真模型，而不用推導(dǎo)，列寫較復(fù)雜的方程描述機(jī)械系統(tǒng)，大大簡(jiǎn)化了建模過程[8]。

③ 分析在ADAMS環(huán)境或者控制應(yīng)用程序環(huán)境獲得的聯(lián)合仿真結(jié)果[9]。

圖5 ADAMS/Matlab聯(lián)合仿真數(shù)據(jù)交換

在Matlab的Simulink中建立以偏航角YAW為誤差，對(duì)左右兩側(cè)電機(jī)轉(zhuǎn)速采用PID驅(qū)動(dòng)控制系統(tǒng)，其中PID模塊用于減小誤差，抑制超調(diào)[10]，限幅模塊限制電機(jī)轉(zhuǎn)速范圍。因?yàn)楸鞠到y(tǒng)是在出現(xiàn)大偏航后才進(jìn)行速度控制的，所以系統(tǒng)始終保持初始輸入轉(zhuǎn)速72°/s，仿真框圖如圖6所示。

圖6 Simulink仿真框圖

1.4 聯(lián)合仿真結(jié)果分析

設(shè)置本次控制目標(biāo)誤差小于1°，穩(wěn)定時(shí)間小于2 s。經(jīng)過反復(fù)對(duì)PID模塊3個(gè)系數(shù)的調(diào)試，最終設(shè)置比例系數(shù)P=5，積分系數(shù)I=0.2，微分系數(shù)D=0，經(jīng)PID驅(qū)動(dòng)控制系統(tǒng)調(diào)整的偏航角YAW變化曲線如圖7所示。

該仿真表明，基于姿態(tài)角的機(jī)器人速度PID控制模型可以實(shí)現(xiàn)輪腿機(jī)器人在出現(xiàn)較大偏航角后，快速調(diào)整機(jī)器人姿態(tài)。

1.5 機(jī)器人驅(qū)動(dòng)控制系統(tǒng)

將基于機(jī)器人的姿態(tài)的驅(qū)動(dòng)控制系統(tǒng)添加于原來的機(jī)器人控制系統(tǒng)內(nèi)，原機(jī)器人下層控制系統(tǒng)框圖如圖8所示。

圖7 聯(lián)合仿真調(diào)整后機(jī)器人偏航角變化曲線

圖8 輪腿機(jī)器人下位機(jī)控制系統(tǒng)框圖

其中姿態(tài)傳感器采用的是整合了6軸運(yùn)動(dòng)處理組件的MPU6050，原始的ADC數(shù)據(jù)從傳感器讀取后，經(jīng)過姿態(tài)解算，可獲得目標(biāo)載體的俯仰角、滾轉(zhuǎn)角和航向角等數(shù)據(jù)，測(cè)量精度為0.1°。由于傳感器在不同的地點(diǎn)采集到的航向角均不同，所以需對(duì)航向角先進(jìn)行一段時(shí)間的采集并做歸一化求均值后，作為是否偏航的基準(zhǔn)值，該任務(wù)可由傳感器控制模塊完成，一旦檢測(cè)到超過5°，再向主控制器發(fā)送偏航角度。主控制對(duì)機(jī)器人機(jī)體運(yùn)動(dòng)控制的流程圖如圖9所示。

圖9 下位機(jī)主控制器軟件流程圖

2 輪腿機(jī)器人攀爬樓梯穩(wěn)定性驗(yàn)證

在輪腿機(jī)器人攀爬樓梯時(shí)，除了需要通過對(duì)姿態(tài)角的監(jiān)測(cè)進(jìn)而控制電機(jī)，防止偏航角進(jìn)一步變大，還需要對(duì)機(jī)器人的穩(wěn)定性進(jìn)行驗(yàn)證，防止輪腿機(jī)器人在攀爬樓梯時(shí)傾翻。

機(jī)器人的穩(wěn)定性能分析一般分為靜態(tài)和動(dòng)態(tài)兩種，其中靜態(tài)穩(wěn)定性分析有：重心投影法、靜態(tài)穩(wěn)定邊界法、能量穩(wěn)定邊界法等[11]。本機(jī)器人在攀爬樓梯過程中比正常行走的速度慢，所以僅采取靜態(tài)分析。童俊華[12]等人通過以判定機(jī)器人重力線是否越過傾覆線為標(biāo)準(zhǔn)，通過基于圖形的計(jì)算來判斷。輪腿機(jī)器人攀爬樓梯示意圖如圖10所示，由該圖給出輪腿機(jī)器人不發(fā)生傾翻的條件公式為

(h+Rsinφ)sinθ<(L-Rcosφ)cosθ

(1)

式中，h為機(jī)器人重心到輪腿中心的距離；R為輪腿中心至輪腿與地面接觸質(zhì)點(diǎn)的距離；L為重心到兩后輪腿中心線法向距離；θ為機(jī)器人俯仰角；φ為輪腿與地面接觸點(diǎn)與機(jī)器人平面的夾角。

圖10 輪腿機(jī)器人攀爬樓梯傾翻示意圖

為保證輪腿機(jī)器人在攀爬樓梯過程中遇到極限情況也不傾翻，可將式(1)變換為式(2)。

(2)

由圖2中機(jī)器人PITCH俯仰角等變化情況可知

(3)

將輪腿機(jī)器人的參數(shù)L=365 mm，h=25 mm，Rmax=200 mm帶入式(2)中，可知該不等式在機(jī)器人攀爬樓梯過程中成立，即本輪腿機(jī)器人在極限特殊情況下也能滿足較高的穩(wěn)定性能，滿足攀爬樓梯等復(fù)雜城市環(huán)境的需求。

3 結(jié)束語

通過ADAMS和Simulink對(duì)五星形輪腿機(jī)器人的聯(lián)合仿真，基于實(shí)時(shí)獲取的偏航角角度，利用PID控制器對(duì)機(jī)器人左右兩側(cè)的電機(jī)分別調(diào)控。直接將該驅(qū)動(dòng)控制系統(tǒng)移植進(jìn)原機(jī)器人控制系統(tǒng)中，該控制系統(tǒng)可以減少機(jī)器人在攀爬樓梯過程中出現(xiàn)大偏航的情況，有效防止機(jī)器人碰撞上左右兩側(cè)的墻壁。利用虛擬樣機(jī)設(shè)計(jì)方法，可極大降低研發(fā)成本，加快開發(fā)進(jìn)度[13]。本文還借助重心投影法，利用圖形化計(jì)算確定了輪腿機(jī)器人在本驅(qū)動(dòng)控制系統(tǒng)下，即使在極限特殊情況下，同樣能保持穩(wěn)定不傾翻的性能。