履帶式變形機(jī)器人樓梯攀爬運(yùn)動控制研究*

崔瑞超,司靜靜,遲明路

(1.河南工學(xué)院 電氣工程與自動化學(xué)院,河南 新鄉(xiāng) 453003;2.河南工學(xué)院 智能工程學(xué)院,河南 新鄉(xiāng) 453003)

0 引言

隨著機(jī)器人技術(shù)、人工智能技術(shù)的迅速發(fā)展,履帶式變形機(jī)器人在民用生活、軍用實(shí)戰(zhàn)、工業(yè)生產(chǎn)等方面已得到廣泛的應(yīng)用[1-2]。目前,履帶式變形機(jī)器人應(yīng)用于平坦環(huán)境的研究已經(jīng)相當(dāng)成熟,但對履帶變形機(jī)器人越障的研究還有很大的提升空間[3-4]。眾所周知,樓梯是環(huán)境中一種最典型、最特殊的障礙物,研究履帶式變形機(jī)器人攀爬樓梯的運(yùn)動控制具有重要的實(shí)際意義。

1 總體設(shè)計

本文提出一種履帶式變形機(jī)器人攀爬樓梯控制系統(tǒng),采用STM32F103作為主控芯片,主要包括主控部分、信息采集部分、執(zhí)行部分。信息采集部分負(fù)責(zé)感知環(huán)境信息和機(jī)器人的機(jī)身信息,主控部分負(fù)責(zé)分析感知到的信息并做出對應(yīng)的任務(wù)規(guī)劃,執(zhí)行部分負(fù)責(zé)完成規(guī)劃的運(yùn)動任務(wù)。系統(tǒng)整體具有感知環(huán)境信息、任務(wù)規(guī)劃以及決策執(zhí)行等方面的能力,系統(tǒng)框圖如圖1所示。

圖1 系統(tǒng)框圖

信息采集部分包括超聲波測距模塊、煙霧測量模塊、姿態(tài)測量模塊、溫濕度測量模塊;主控部分接收并處理采集到的數(shù)據(jù)信息,規(guī)劃路徑和向執(zhí)行模塊發(fā)送行為指令;執(zhí)行部分包括電機(jī)、編碼器、減速器、驅(qū)動電路,根據(jù)主控部分發(fā)出的行為指令對電機(jī)進(jìn)行控制,并將實(shí)時運(yùn)行信息反饋給控制器。機(jī)器人運(yùn)動控制平臺如圖2所示。

圖2 機(jī)器人運(yùn)動控制平臺

2 硬件設(shè)計

根據(jù)總體設(shè)計及功能要求,硬件部分主要包括電源模塊、通信模塊、傳感器模塊、主控部分、驅(qū)動控制模塊、電機(jī)。硬件設(shè)計框圖如圖3所示。

圖3 硬件設(shè)計框圖

2.1 STM32F103主控器

主控部分是整個控制系統(tǒng)的核心,負(fù)責(zé)處理傳感器模塊采集到的數(shù)據(jù)信息,實(shí)現(xiàn)對執(zhí)行部分的控制,使整個系統(tǒng)達(dá)到預(yù)期的功能要求。

主控器使用STM32F103芯片,該芯片采用Cortex-M3內(nèi)核架構(gòu)。STM32F103的SRAM和FLASH分別為64KB、512KB,處理器為32位,CPU主頻為72MHz,具有2個高級定時器、4個通用定時器、2個基本定時器、1個CAN、1個USB、1個SDIO接口、1個FSMC接口、1個12位DAC、3個12位ADC、2個IIC、3個SPI、5個串口、112個通用I/O口。STM32F103具有電壓能耗低、處理性能高、使用空間大、成本低、實(shí)時性能強(qiáng)、集中度高、開發(fā)利用相當(dāng)方便等優(yōu)點(diǎn)[5-6],可以滿足使用者的各種要求。

圖4 STM32F103主控器

2.2 測距傳感器

測距傳感器采用超聲波模塊,利用超聲波測距原理探測機(jī)器人到兩側(cè)障礙物的距離[7],傳感器原理如圖5所示。

圖5 測距傳感器工作原理圖

2.3 姿態(tài)傳感器

姿態(tài)傳感器選用可以獨(dú)立完成姿態(tài)算法處理的MPU-6050傳感器,該傳感器通過姿態(tài)運(yùn)算,可以把測量的模擬量轉(zhuǎn)化為數(shù)字量,算出俯仰角、偏航角、翻滾角[8-9]。其工作原理如圖6所示。

圖6 姿態(tài)傳感器工作原理圖

3 軟件設(shè)計

履帶式變形機(jī)器人在攀爬樓梯過程中的兩個控制變量分別為位置偏移比值Q和偏航角θ。位置偏移比值Q通過對測距傳感器采集的數(shù)據(jù)進(jìn)行計算得到,偏航角θ通過對姿態(tài)傳感器采集的數(shù)據(jù)計算得到。自主攀爬樓梯控制框圖如圖7所示,dL、dR分別表示機(jī)器人距離樓梯左側(cè)和右側(cè)的距離,Q表示離樓梯左右兩側(cè)距離的比值,V為機(jī)器人的勻速移動速度。

行駛方向控制器的主要作用是根據(jù)傳感器測量出的偏航角判斷機(jī)器人的運(yùn)行方向,其優(yōu)先級最高。在攀爬過程中,主控部分首先要判定機(jī)器人的偏航角度;當(dāng)機(jī)器人偏航角度較大時,履帶可能會出現(xiàn)打滑現(xiàn)象,導(dǎo)致機(jī)器人傾翻。居中位置控制器的主要作用是控制機(jī)器人沿樓梯中間位置爬行,其優(yōu)先級低于行駛方向控制器;機(jī)器人在攀爬過程中,若偏航角度超過主控部分設(shè)定的角度限值,行駛方向控制器發(fā)生作用;若沒有超過角度限值,居中位置控制器發(fā)生作用,并調(diào)節(jié)位置偏移比例值。

履帶式變形機(jī)器人以速度V勻速爬行時,主控部分根據(jù)傳感器采集的數(shù)據(jù)信息得出的偏航角度和位置偏移比值來控制輸出的脈沖寬度調(diào)制信號,調(diào)節(jié)左右電機(jī)的速度。

圖7 自主爬樓梯控制框圖

3.1 擺臂分析

履帶式變形機(jī)器人的履帶臂可以實(shí)現(xiàn)0—360°旋轉(zhuǎn),當(dāng)其遇到樓梯臺階等垂直障礙時,它的履帶臂會進(jìn)行90°翻轉(zhuǎn)完成攀爬任務(wù)。圖8為運(yùn)動模型示意圖。

圖8 運(yùn)動模型示意圖

在圖8中,(X、O、Y)是世界坐標(biāo)系,(XR、OR、YR)是位于機(jī)器人中心的坐標(biāo)系,XR與YR為正交關(guān)系,XR表示機(jī)器人的運(yùn)行方向,VL和VR分別代表左輪和右輪運(yùn)動速度,b表示機(jī)器人車身寬度。機(jī)器人在運(yùn)行中偏離X軸的角度用θ表示,定義θ>0表示機(jī)器人做逆時針方向運(yùn)動,θ<0表示機(jī)器人做順時針運(yùn)動。

對運(yùn)動模型進(jìn)行數(shù)學(xué)分析[10],可以得到機(jī)器人在圖形中的坐標(biāo)變化。機(jī)器人運(yùn)動的坐標(biāo)用(x,y)表示。設(shè)點(diǎn)(0,0)為機(jī)器人的起始坐標(biāo),經(jīng)過一段時間t,運(yùn)動軌跡x(t)、y(t)、θ(t)滿足:

(1)

(2)

(3)

機(jī)器人的履帶臂上安裝有驅(qū)動電機(jī),機(jī)器人遇到樓梯時通過擺臂運(yùn)動實(shí)現(xiàn)攀爬前進(jìn)。擺臂運(yùn)動模型和擺臂履帶系統(tǒng)如圖9所示。

設(shè)t=0時,機(jī)器人位于原點(diǎn)O處,t時間后,機(jī)器人以角速度ω轉(zhuǎn)動左右履帶前臂至旋轉(zhuǎn)角度θ處,可得出:

θ=ω·t

(4)

當(dāng)機(jī)器人兩個履帶前臂的驅(qū)動電機(jī)接收到相同大小、相同方向的速度信號時,機(jī)器人的旋轉(zhuǎn)角速度ω為:

(5)

式中,D1表示電機(jī)與旋轉(zhuǎn)臂上齒輪數(shù)之比,即減速比;n表示電機(jī)轉(zhuǎn)速,單位為r/min。由式(4)、(5)得:

(6)

通過對擺臂運(yùn)動進(jìn)行數(shù)學(xué)分析得出,當(dāng)機(jī)器人旋轉(zhuǎn)履帶臂的驅(qū)動電機(jī)接收到同向同速信號時,機(jī)器人會做出擺臂運(yùn)動來完成攀爬樓梯任務(wù)。

圖9 擺臂運(yùn)動模型和擺臂履帶系統(tǒng)

3.2 爬樓梯控制研究

履帶式變形機(jī)器人在攀爬樓梯時,運(yùn)動姿態(tài)直接影響攀爬效果,所以,機(jī)器人的姿態(tài)控制是本文主要的研究目標(biāo)。為了更好地識別環(huán)境信息,更準(zhǔn)確地判斷機(jī)器人姿態(tài),機(jī)器人本體安裝有傳感器模塊,主控器將傳感器采集到的信息進(jìn)行加權(quán)融合計算;計算后的數(shù)據(jù)更接近實(shí)際值,然后再將數(shù)據(jù)與主控器給定的目標(biāo)指令進(jìn)行比較;當(dāng)比較誤差較大時,機(jī)器人會調(diào)節(jié)電機(jī)轉(zhuǎn)速來減小誤差,調(diào)整機(jī)器人姿態(tài)。因此,機(jī)器人可以按照設(shè)定的軌跡完成攀爬樓梯任務(wù)。運(yùn)動控制流程如圖10所示。

圖10 運(yùn)動控制流程圖

3.3 控制算法設(shè)計

PI(Proportional Integral)調(diào)節(jié)器是由比例、積分組成的一種線性控制器,它根據(jù)給定值與實(shí)際輸出值構(gòu)成控制偏差,將偏差的比例積分和積分通過線性組合構(gòu)成控制量,對被控對象進(jìn)行控制。

(1)比例環(huán)節(jié)是對系統(tǒng)在某一時刻產(chǎn)生的偏差做出反應(yīng),即當(dāng)出現(xiàn)偏差時,向著偏差變小的環(huán)節(jié)進(jìn)行調(diào)節(jié)[11];

(2)積分環(huán)節(jié)是把偏差變化的累加輸出,當(dāng)出現(xiàn)偏差時積分輸出就會累加,輸出控制量來消除偏差[12]。

離散的PI表達(dá)式為:

(7)

式中,KP是比例系數(shù),e(k)為第k次采樣時刻輸入的偏差,u0為PI控制的初值,KI是積分系數(shù),T表示采樣的周期,TI表示為積分常數(shù)。

若采樣周期T很短,離散過程有可能接近模擬控制過程,若被控對象是變化的,變化的增量為Δu(k)。

增量式PI表達(dá)式為:

(8)

式中,KI為積分系數(shù),KP為比例系數(shù),u0為初始值。由式(7)、(8)得:

Δu(k)=KP[e(k)-e(k-1)]+KIe(k)

(9)

式中,e(k-1)、e(k)分別為第k-1次和第k次采樣時輸入的偏差。

機(jī)器人攀爬樓梯控制的目標(biāo)就是調(diào)整運(yùn)動姿態(tài),姿態(tài)傳感器檢測機(jī)器人位置信息,PI調(diào)節(jié)器控制機(jī)器人兩側(cè)電機(jī)的運(yùn)行速度,進(jìn)而達(dá)到調(diào)整機(jī)器人姿態(tài)的目的。機(jī)器人姿態(tài)控制框圖如圖11所示,當(dāng)偏航角度與目標(biāo)角度θ出現(xiàn)偏差時,機(jī)器人會通過PI調(diào)節(jié)器調(diào)節(jié)電機(jī)轉(zhuǎn)速,實(shí)現(xiàn)運(yùn)動姿態(tài)控制。

圖11 機(jī)器人姿態(tài)控制框圖

機(jī)器人開始樓梯攀爬動作時,通道1、2打開,機(jī)器人左右電機(jī)等速轉(zhuǎn)動,若機(jī)器人在運(yùn)行過程中出現(xiàn)方向偏移,PI調(diào)節(jié)器自動調(diào)節(jié)兩側(cè)電機(jī)速度,進(jìn)而機(jī)器人會自動校正軌跡。為實(shí)現(xiàn)對機(jī)器人運(yùn)動姿態(tài)的控制分析,本文對機(jī)器人左右電機(jī)速度進(jìn)行MATLAB仿真,結(jié)果如圖12所示。

圖12 速度仿真圖

由圖12可知,機(jī)器人在運(yùn)動過程中,左右輪速度基本可以保持同步。

4 試驗(yàn)及分析

為驗(yàn)證前述分析的正確性,制作了實(shí)物樣機(jī)并在實(shí)驗(yàn)室樓梯間進(jìn)行相關(guān)測試。測試過程中,通過傳感器采集數(shù)據(jù)信息得到機(jī)器人的工作狀態(tài),根據(jù)機(jī)器人的偏航角度和偏移位置實(shí)時調(diào)節(jié)電機(jī)速度,最終實(shí)現(xiàn)了機(jī)器人的攀爬動作。試驗(yàn)中選用的樓梯,每階寬30cm,高15cm,表1為爬樓梯試驗(yàn)結(jié)果。

表1 爬樓梯試驗(yàn)結(jié)果

由試驗(yàn)結(jié)果可知,當(dāng)機(jī)器人以不同的速度攀爬不同的臺階數(shù)時,都會存在一定的誤差;當(dāng)速度增大時,誤差會出現(xiàn)增大的趨勢,但平均測量誤差均保持在3cm內(nèi),運(yùn)行十分平穩(wěn)。

5 結(jié)論

(1)本文設(shè)計的履帶式變形機(jī)器人具備較高的攀爬樓梯的能力,通過搭建的硬件平臺驗(yàn)證了機(jī)器人具備較強(qiáng)的越障性能。

(2)本文設(shè)計的PI調(diào)節(jié)器能夠很好地保證兩側(cè)電機(jī)運(yùn)轉(zhuǎn)的同步性,MATLAB仿真結(jié)果表明,兩側(cè)電機(jī)的響應(yīng)曲線幾乎重合。

(3)在實(shí)驗(yàn)室樓梯間進(jìn)行了機(jī)器人攀爬樓梯測試,試驗(yàn)測試數(shù)據(jù)誤差較小,證明系統(tǒng)具有較高的穩(wěn)定性。