崔瑞超,司靜靜,遲明路

(1.河南工學(xué)院 電氣工程與自動(dòng)化學(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)成熟,但對(duì)履帶變形機(jī)器人越障的研究還有很大的提升空間[3-4]。眾所周知,樓梯是環(huán)境中一種最典型、最特殊的障礙物,研究履帶式變形機(jī)器人攀爬樓梯的運(yùn)動(dòng)控制具有重要的實(shí)際意義。

1 總體設(shè)計(jì)

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

圖1 系統(tǒng)框圖

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

圖2 機(jī)器人運(yùn)動(dòng)控制平臺(tái)

2 硬件設(shè)計(jì)

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

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

2.1 STM32F103主控器

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

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

圖4 STM32F103主控器

2.2 測(cè)距傳感器

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

圖5 測(cè)距傳感器工作原理圖

2.3 姿態(tài)傳感器

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

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

3 軟件設(shè)計(jì)

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

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

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

圖7 自主爬樓梯控制框圖

3.1 擺臂分析

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

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

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

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

(1)

(2)

(3)

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

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

θ=ω·t

(4)

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

(5)

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

(6)

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

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

3.2 爬樓梯控制研究

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

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

3.3 控制算法設(shè)計(jì)

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

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

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

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

(7)

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

若采樣周期T很短,離散過(guò)程有可能接近模擬控制過(guò)程,若被控對(duì)象是變化的,變化的增量為Δ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次采樣時(shí)輸入的偏差。

機(jī)器人攀爬樓梯控制的目標(biāo)就是調(diào)整運(yùn)動(dòng)姿態(tài),姿態(tài)傳感器檢測(cè)機(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)偏差時(shí),機(jī)器人會(huì)通過(guò)PI調(diào)節(jié)器調(diào)節(jié)電機(jī)轉(zhuǎn)速,實(shí)現(xiàn)運(yùn)動(dòng)姿態(tài)控制。

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

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

圖12 速度仿真圖

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

4 試驗(yàn)及分析

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

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

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

5 結(jié)論

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

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

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