溫室環(huán)境監(jiān)測(cè)機(jī)器人路徑跟蹤算法與試驗(yàn)

韋鑒峰，呂恩利，王飛仁，林偉加，阮清松

(華南農(nóng)業(yè)大學(xué) 工程學(xué)院，廣州 510642)

0 引言

導(dǎo)航與定位是溫室機(jī)器人路徑跟蹤的關(guān)鍵技術(shù)之一[1]。溫室環(huán)境監(jiān)測(cè)機(jī)器人作為智能農(nóng)業(yè)發(fā)展重要組成部分，其路徑跟蹤控制是解決 “怎么走”的問題。溫室環(huán)境監(jiān)測(cè)機(jī)器人可以歸類為非完整約束輪式移動(dòng)機(jī)器人[2]，由于存在非完整約束,使運(yùn)動(dòng)控制難度較高[3]。對(duì)具有非完整約束的溫室環(huán)境監(jiān)測(cè)機(jī)器人的研究是解決其路跡跟蹤控制的重要途徑[4-5]。

針對(duì)非完整約束結(jié)構(gòu)輪式移動(dòng)機(jī)器人軌跡跟蹤問題，很多學(xué)者進(jìn)行了相關(guān)的研究。宋立博[6-8]等通過對(duì)輪式移動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)模型進(jìn)行分析，提出了不同工作要求下機(jī)器人的軌跡跟蹤控制算法，并設(shè)計(jì)了控制器，通過仿真證明了算法的有效性；但這些算法都只是基于計(jì)算機(jī)仿真驗(yàn)證，沒有通過試驗(yàn)驗(yàn)證算法的實(shí)用性和有效性。在難以準(zhǔn)確建立運(yùn)動(dòng)學(xué)模型的情況下，李逃昌[9-12]等應(yīng)用純跟蹤模型對(duì)路徑軌跡進(jìn)行跟蹤，并在相關(guān)的試驗(yàn)平臺(tái)上進(jìn)行算法試驗(yàn)，其跟蹤準(zhǔn)確性相對(duì)較高，獲得較好的研究成果。

為解決溫室環(huán)境監(jiān)測(cè)機(jī)器人路徑跟蹤控問題，本文通過對(duì)其運(yùn)動(dòng)學(xué)模型分析，采用基于航向角誤差算法的路徑跟蹤控制方法，以實(shí)現(xiàn)對(duì)機(jī)器人轉(zhuǎn)向和速度控制，并通過仿真和試驗(yàn)驗(yàn)證算法的有效性和實(shí)用性，為解決溫室環(huán)境監(jiān)測(cè)機(jī)器人路徑跟蹤控制問題提供了參考。

1 系統(tǒng)結(jié)構(gòu)與運(yùn)動(dòng)學(xué)模型

1.1 機(jī)器人系統(tǒng)結(jié)構(gòu)

溫室環(huán)境監(jiān)測(cè)機(jī)器人(簡稱機(jī)器人)由安全檢測(cè)系統(tǒng)、傳感器系統(tǒng)、電氣系統(tǒng)、控制器系統(tǒng)、電源供電系統(tǒng)、遠(yuǎn)程和上位機(jī)系統(tǒng)及避障系統(tǒng)等組成，如圖1所示。平臺(tái)長為550mm，寬為400mm，高為770mm，質(zhì)量為30kg。有4個(gè)輪子，分別為2個(gè)轉(zhuǎn)向輪和2個(gè)驅(qū)動(dòng)輪，屬汽車輪式移動(dòng)機(jī)器人；軸距為350mm，轉(zhuǎn)向輪通過操舵機(jī)構(gòu)實(shí)現(xiàn)轉(zhuǎn)向，驅(qū)動(dòng)輪通過連接差動(dòng)齒輪實(shí)現(xiàn)差速；機(jī)器人采用激光定位方式，由激光定位傳感器NAV350實(shí)現(xiàn)定位。

當(dāng)機(jī)器人運(yùn)行時(shí)，狀態(tài)指示燈亮綠燈，NAV350掃描周圍環(huán)境檢測(cè)反光標(biāo)貼，實(shí)時(shí)獲取機(jī)器人全局坐標(biāo)位姿，角度編碼器和速度傳感器分別實(shí)時(shí)獲取轉(zhuǎn)向輪轉(zhuǎn)角與機(jī)器人移動(dòng)速度；傳感器采集的數(shù)據(jù)通過I/O模塊輸入工控機(jī)器系統(tǒng)，系統(tǒng)將獲取的反饋數(shù)據(jù)，應(yīng)用相應(yīng)算法進(jìn)行處理，輸出下一時(shí)刻機(jī)器人的速度和轉(zhuǎn)角，實(shí)現(xiàn)機(jī)器人按照預(yù)設(shè)軌跡從起始點(diǎn)運(yùn)動(dòng)到目標(biāo)點(diǎn)。

1.2 機(jī)器人運(yùn)動(dòng)學(xué)模型分析

假設(shè)室內(nèi)路面平坦、運(yùn)動(dòng)速度可變，機(jī)器人實(shí)心輪胎無側(cè)偏、無縱側(cè)滑移，機(jī)器人的瞬時(shí)轉(zhuǎn)向中心是兩條車軸線的交點(diǎn)。根據(jù)機(jī)器人的結(jié)構(gòu)特點(diǎn)，建立如圖2所示的運(yùn)動(dòng)學(xué)模型。

1.急停開關(guān) 2.狀態(tài)指示燈 3.激光定位掃描儀NAV350 4.激光避障傳感器 5.I/O模塊 6.工控機(jī) 7.絕對(duì)值式角度編碼器 8.轉(zhuǎn)向輪9.24V鉛蓄電池 10.驅(qū)動(dòng)輪

圖2 機(jī)器人運(yùn)動(dòng)模型圖

圖2中，(X,O,Y)為笛卡爾全局坐標(biāo)；(XV,V,YV)為機(jī)器人局部坐標(biāo)；1、2為轉(zhuǎn)向輪；3、4為驅(qū)動(dòng)輪；5為等效轉(zhuǎn)向輪；6為等效驅(qū)動(dòng)輪；l為軸距(m)；θ為機(jī)器人航向角(rad)；ICR為機(jī)器人瞬時(shí)旋轉(zhuǎn)中心；v為機(jī)器人運(yùn)動(dòng)速度(m/s)；γ為轉(zhuǎn)向輪轉(zhuǎn)角(rad)，R2為機(jī)器人瞬時(shí)旋轉(zhuǎn)半徑(m)。

根據(jù)建立的機(jī)器人運(yùn)動(dòng)學(xué)模型，將其結(jié)構(gòu)簡化為輪5和輪6組成的二自由度輪式自行車模型，可以得到其運(yùn)動(dòng)學(xué)方程式為

(1)

式中ν—機(jī)器人行駛速度(m/s)；

ω—移動(dòng)機(jī)器人角度變化率(rad/s)；

θ—航向角(rad)；

γ—機(jī)器人等效轉(zhuǎn)向輪轉(zhuǎn)角(rad)；

l—機(jī)器人軸距(m)。

其中，V(x,y)為機(jī)器人運(yùn)動(dòng)學(xué)模型圖中V點(diǎn)坐標(biāo)，即驅(qū)動(dòng)輪中心點(diǎn)坐標(biāo)。式(1)表示移動(dòng)機(jī)器人的位姿p(x,y,θ)與移動(dòng)速度v和等效轉(zhuǎn)向輪轉(zhuǎn)角γ之間的關(guān)系。由圖2幾何關(guān)系可以得到

l/R2=tanγ

(2)

式中R—機(jī)器人瞬時(shí)轉(zhuǎn)彎半徑(m)；

由式(2)可以推導(dǎo)出機(jī)器人后輪軸中心處的瞬時(shí)轉(zhuǎn)彎半徑為R2=l/tanγ。

2 坐標(biāo)轉(zhuǎn)換與算法原理

2.1 坐標(biāo)轉(zhuǎn)換

上述系統(tǒng)結(jié)構(gòu)中，機(jī)器人的采用激光定位方式，所使用的激光定位傳感器NAV350具有建圖和全局定位功能，采用三點(diǎn)定位方式。機(jī)器人的路徑跟蹤問題可轉(zhuǎn)化為機(jī)器人跟蹤期望路徑上的連續(xù)點(diǎn)的問題，如圖3所示。

圖3 機(jī)器人路徑跟蹤示意圖

圖3中，(X,O,Y)為全局坐標(biāo)；(xc,V，yc)為機(jī)器人局部坐標(biāo)系；機(jī)器人當(dāng)前的坐標(biāo)點(diǎn)為V(XV,YV,θ)；A點(diǎn)為期望路徑上的一點(diǎn)，坐標(biāo)為A(XA,YA)，即為機(jī)器人跟蹤的目標(biāo)點(diǎn)，這兩點(diǎn)全局坐標(biāo)已知?，F(xiàn)為方便路徑算法的計(jì)算，要將已知的A點(diǎn)的坐標(biāo)A(XA,YA)轉(zhuǎn)化為機(jī)器人局部坐標(biāo)系(xc,V,yc)下的坐標(biāo)A1(xA,yA)。本研究采用的是幾何方法推算全局坐標(biāo)A(XA,YA)在局部坐標(biāo)系(xc,V,yc)下的坐標(biāo)，其幾何關(guān)系原理如圖4所示。

圖4 機(jī)器人目標(biāo)點(diǎn)坐標(biāo)轉(zhuǎn)換幾何原理圖

transformation of robot target point

圖4中，A點(diǎn)是圖3中的期望路徑上的點(diǎn)A的簡化形式；坐標(biāo)系(x′,V,y′)為全局坐標(biāo)系(X,O,Y)的平移坐標(biāo)系。由圖4可知：要求A點(diǎn)在局部坐標(biāo)系的坐標(biāo)即為求線段VB和VF的長度。由幾何關(guān)系有

(3)

(4)

公式(4)中，(xA,yA)即為目標(biāo)點(diǎn)A在機(jī)器人局部坐標(biāo)系下的坐標(biāo)，公式中所有的參數(shù)都是已知，由此可知，A的局部坐標(biāo)也可知。

2.2 航向角誤差算法

在機(jī)器人路徑跟蹤中，其跟蹤穩(wěn)定性、魯棒性、準(zhǔn)確性和精度等是評(píng)價(jià)機(jī)器人路徑跟蹤好壞的重要指標(biāo)。研究移動(dòng)機(jī)器人的速度和方向控制與優(yōu)化問題，是解決和優(yōu)化機(jī)器人軌跡跟蹤能力重要途徑，本文基于機(jī)器人運(yùn)動(dòng)學(xué)模型研究其方向和速度控制問題。為方便計(jì)算，記機(jī)器人運(yùn)動(dòng)速度為v，轉(zhuǎn)向角為γ，用β表示航向角誤差值，如圖5所示。

由圖5可知，機(jī)器人當(dāng)前位置與到目標(biāo)點(diǎn)的方向誤差值可以表示為

(5)

式中β—航向角誤差值(rad)。

其中，(xA,yA)為圖4中目標(biāo)點(diǎn)A在機(jī)器人局部坐標(biāo)系中的相對(duì)坐標(biāo)。在本研究中，機(jī)器人轉(zhuǎn)向角的大小為直接控制量。當(dāng)航向角誤差角β較小時(shí)，采用如下線性控制，即

γH=kH·β

(6)

式中γH—機(jī)器人轉(zhuǎn)角(rad)；

kH—常數(shù)系數(shù)。

圖5 機(jī)器人航向角誤差示意圖

式(6)為航向角誤差值與轉(zhuǎn)向角關(guān)系式，kH可根據(jù)需求取值。由式(6)可知：當(dāng)limβ=0時(shí)，limγH=0，即當(dāng)機(jī)器人航向角誤差值收斂于0時(shí)，機(jī)器人的轉(zhuǎn)向角γH也收斂于0；機(jī)器人轉(zhuǎn)向采用舵機(jī)驅(qū)動(dòng)，其轉(zhuǎn)向具有極限值γmax，當(dāng)γH>γmax時(shí)取γH=γmax。通過式(6)可算得機(jī)器人轉(zhuǎn)向角大小γH，實(shí)現(xiàn)了機(jī)器人的轉(zhuǎn)向控制。在機(jī)器人運(yùn)動(dòng)過程中，式(6)僅能控制其運(yùn)動(dòng)方向，無法控制其運(yùn)動(dòng)速度；然而機(jī)器人的運(yùn)動(dòng)速度也是影響其能否快速收斂于期望軌跡的關(guān)鍵因素，且機(jī)器人在不同位置其運(yùn)動(dòng)速度也不同。因此，采用合適的速度控制策略控制機(jī)器人的速度至關(guān)重要。在工程應(yīng)用中，機(jī)器人能否快速到目標(biāo)點(diǎn)，并在器人到達(dá)目標(biāo)點(diǎn)時(shí)能否鎮(zhèn)定，是評(píng)價(jià)機(jī)器人響應(yīng)性能的重要標(biāo)準(zhǔn)。本文采用線性控制方式對(duì)機(jī)器人的運(yùn)動(dòng)速度進(jìn)行控制，其控制策略如下，即

(7)

式中vH—機(jī)器人運(yùn)動(dòng)速度(m/s)；

kv—正的常系數(shù)。

其中，kv可根據(jù)實(shí)際情況取值。由式(7)可知：隨著機(jī)器人收斂于目標(biāo)點(diǎn)A，其相對(duì)坐標(biāo)(xA,yA)也越小，運(yùn)動(dòng)速度也越小。當(dāng)limxA=yA=0時(shí)，limvH=0，采用該速度控制策略可使機(jī)器人到達(dá)目標(biāo)點(diǎn)后鎮(zhèn)定，且不存在較大的運(yùn)動(dòng)加速度，不會(huì)對(duì)機(jī)器人結(jié)構(gòu)造成沖擊，表現(xiàn)出較好的機(jī)械性能。機(jī)器人的運(yùn)動(dòng)采用電機(jī)驅(qū)動(dòng)，其運(yùn)動(dòng)速度具有極限值，當(dāng)vH>vmax時(shí)，取vH=vmax。式(6)和式(7)為機(jī)器人在航向角較小時(shí)采用的一種控制機(jī)器人的轉(zhuǎn)向角和速度的方式和策略。在機(jī)器人航向角較大時(shí)，從式(6)可知：當(dāng)kH不變時(shí)，γH較大，會(huì)使機(jī)器人獲得一個(gè)較大轉(zhuǎn)向階躍信號(hào)，對(duì)機(jī)器人轉(zhuǎn)向輪轉(zhuǎn)向舵機(jī)產(chǎn)生較大機(jī)械沖擊，不利于機(jī)器人機(jī)構(gòu)的保護(hù)。因此，航向角較大時(shí)，可采用純跟蹤方法對(duì)機(jī)器人的轉(zhuǎn)向進(jìn)行控制。

2.3 純跟蹤算法原理

航向角誤差算法是當(dāng)航向角誤差值較小時(shí)采用的一種控制方法。當(dāng)航向角誤差較大時(shí)，可采用純跟蹤算法。純跟蹤(pure pursuit)算法是一種幾何計(jì)算法，原理是計(jì)算機(jī)器人到達(dá)期望點(diǎn)所需要走的圓弧長。該方法簡單、容易實(shí)現(xiàn)，是模擬汽車駕駛員的視覺、采用前視距離的方式，已經(jīng)廣泛應(yīng)用在路徑跟蹤領(lǐng)域上。該算法的原理如圖6所示。

圖6 純跟蹤算法模型

圖6中，(X,V,Y)構(gòu)成了機(jī)器人局部坐標(biāo)系，A(xA,yA)為機(jī)器人目標(biāo)點(diǎn)。根據(jù)幾何關(guān)系，得出該算法的表達(dá)式為

(8)

由式(8)整理得

R=L2/2y

(9)

(10)

式中L—圓弧弦長(m)；

R—圓弧的半徑(m)。

式(9)和式(10)中，L為連接機(jī)器人驅(qū)動(dòng)輪中點(diǎn)V和A圓弧段的弦長。

其中，O為該圓弧的圓心；L和y為可測(cè)量的已知量，且有，x=xA，y=yA。因此，R也已知，即機(jī)器人要從當(dāng)前點(diǎn)V運(yùn)動(dòng)到目標(biāo)點(diǎn)A，其運(yùn)動(dòng)半徑為R。

本文所研究的對(duì)象為后輪驅(qū)動(dòng)和前輪轉(zhuǎn)向的類汽車機(jī)器人，與傳統(tǒng)的差速轉(zhuǎn)向移動(dòng)機(jī)器人不同之處在于它是通過控制轉(zhuǎn)向輪的方向來改變機(jī)器人的方向，具有非完整特性，控制難度較高。因此，對(duì)這種汽車的機(jī)器人的轉(zhuǎn)向控制問題，很多研究人員都做了大量的研究工作，試圖運(yùn)用運(yùn)動(dòng)建模和數(shù)學(xué)幾何手段解決，但精確度受到了很大的限制。

機(jī)器人航向角誤差較大時(shí)，本節(jié)采用A.J.Kelly[13]提出的將四輪結(jié)構(gòu)簡化成二輪車自行車模型的方法。假設(shè)車輪與地面接觸無側(cè)向滑移，運(yùn)動(dòng)的時(shí)候沒有出現(xiàn)打滑失速現(xiàn)象。機(jī)器人簡化二輪車模型如圖7所示。

圖7 簡化二輪車運(yùn)動(dòng)學(xué)模型

圖7中，l為前輪和后輪的間距，即軸距(m)；R為機(jī)器人旋轉(zhuǎn)半徑(m)；γ為機(jī)器人等效轉(zhuǎn)角(rad)。由幾何關(guān)系可得

tanγ=l/R

(11)

結(jié)合圖6和式(11)可知：機(jī)器人要以圓弧的方式從點(diǎn)V到達(dá)目標(biāo)點(diǎn)A，則其圓弧半徑為R與圖(7)中的機(jī)器人旋轉(zhuǎn)半徑是一致的。由式(9)和式(11)可得

(12)

式中γc—等效轉(zhuǎn)向角(rad)。

由式(12)可知：只要選擇合適的目標(biāo)點(diǎn)，得到目標(biāo)A(XA,YA)的全局坐標(biāo)值和機(jī)器人在全局坐標(biāo)中的坐標(biāo)值V(XV,YV)，再通過式(4)求出(xA,yA)，即可求出機(jī)器人等效轉(zhuǎn)向的γc值，實(shí)現(xiàn)移動(dòng)機(jī)器人的轉(zhuǎn)向。與航向角誤差值法求解機(jī)器人轉(zhuǎn)向角大小方法一致，式(1)、(2)僅能求解機(jī)器人轉(zhuǎn)向角的大小γc，并不能控制其運(yùn)動(dòng)速度。為保證機(jī)器人的運(yùn)動(dòng)的平順性，在采用純跟蹤算法控制機(jī)器人運(yùn)動(dòng)階段，采用與式(7)相似的線性控制方式，其速度表達(dá)式為

(13)

式中vC—機(jī)器人運(yùn)動(dòng)速度(m/s)；

kC—正的常系數(shù)；

kx—正的常系數(shù)；

β—航向角誤差值(ad)。

由式(13)可知：始終有vC≤vmax，當(dāng)航向角誤差值β較大時(shí)，式子的分母也會(huì)比較大，vC會(huì)比較小。在本文中，當(dāng)β<π/6時(shí)采用航向角誤差控制算法；當(dāng)π/6≤β≤π/4時(shí)，即當(dāng)機(jī)器人獲得一個(gè)較大的航向角誤差值時(shí)，機(jī)器人當(dāng)前點(diǎn)與目標(biāo)點(diǎn)的橫向誤差較大。因此，機(jī)器人應(yīng)先慢速轉(zhuǎn)向，減小與目標(biāo)點(diǎn)的橫向誤差，再以較快的速度靠近目標(biāo)點(diǎn)；當(dāng)路徑為較曲折的路線時(shí)，采用該速度控制策略會(huì)使機(jī)器人更為精確地跟蹤期望軌跡。

2.4 前視距離的控制

前視距離是機(jī)器人路徑跟蹤中的一個(gè)重要因素，前視距離的選擇會(huì)影響機(jī)器人跟蹤路徑的準(zhǔn)確性。前視距離取值較大，其跟蹤圓弧半徑較大，跟蹤不會(huì)出現(xiàn)路線振蕩，但會(huì)產(chǎn)生機(jī)器人走“捷徑”的效果。如果路徑周邊所預(yù)留的空間較小，機(jī)器人會(huì)撞向路徑周圍的障礙物；如果前視距離取值較小，會(huì)出現(xiàn)機(jī)器人運(yùn)動(dòng)路徑產(chǎn)生振蕩，機(jī)器人運(yùn)行不穩(wěn)定。跟蹤效果如圖8所示。

圖8 不同前視距離跟蹤效果

由圖8可以看出：不同的前視距離得到的效果不一樣，因此選擇合適的前視距離才能提高機(jī)器人的跟蹤效果。通過軟件的仿真和進(jìn)行多次試驗(yàn)比較的方法獲取不同前視距離的跟蹤效果，然后篩選其中合適的固定值作為前視距離。這種方法在路徑比較簡單的情況下是可行的，但也有局限性。假設(shè)路徑比較復(fù)雜，路徑不具備導(dǎo)數(shù)的連續(xù)性，固定的前視距離往往不會(huì)取得較好的跟蹤效果。針對(duì)此問題，本文提出了一種適合機(jī)器人前視距離的選擇的方法，具體如圖9所示。

圖9 前視距離選擇策略

圖9中，C1(xc,yc)表示機(jī)器人的當(dāng)前點(diǎn)。以C1為圓心、R1為半徑，恰好與期望路徑相切，切點(diǎn)為C2(x2,y2)；再以C2為圓心、R2為半徑畫圓，與期望路徑交于點(diǎn)r1(xr1,yr1)、r2(xr2,yr2)，即為機(jī)器人的前視距離點(diǎn)；再根據(jù)機(jī)器人的前進(jìn)方向進(jìn)行選擇其中一點(diǎn)，完成機(jī)器人前視點(diǎn)的選擇。假設(shè)期望路徑L是由無數(shù)個(gè)點(diǎn)(xi,yi)組成，即

(14)

其中，n為正整數(shù)。

由以上的描述可得

(15)

(16)

式中R1—C1半徑(m)；

R2—C2半徑(m)。

同時(shí)，有R1=0.01×t(t=1,2…)。當(dāng)且僅當(dāng)C1與期望路徑只有1個(gè)交點(diǎn)時(shí)，停止取值。式(15)求得點(diǎn)C2，式(16)求得點(diǎn)r1、r2。假設(shè)某時(shí)刻機(jī)器人的前視點(diǎn)為r1，則r1的坐標(biāo)r1(xr1,yr1)取點(diǎn)策略為

(17)

式中θ—常數(shù)。

式(16)和式(17)中，R2=0.01×m(m=1,2，…)，且R2max=a。其中，a為正的常數(shù)，取a=0.5m。在計(jì)算機(jī)的計(jì)算過程中，當(dāng)R2<0.5時(shí)，R2的值不斷地疊加，當(dāng)且僅當(dāng)R2=R2max=a或者arctan(y2-yr1/x2-xr1)=θ時(shí)停止取值。本文中，θ=π/4時(shí)，r1(xr1,yr1)即為機(jī)器人當(dāng)前時(shí)刻的前視點(diǎn)。

3 仿真與試驗(yàn)

3.1 材料與設(shè)備

仿真在MatLab2015a環(huán)境下采用編程的方式進(jìn)行，硬件為筆記本電腦型號(hào)X260。試驗(yàn)材料為上述機(jī)器人、反光標(biāo)貼、上位機(jī)(型號(hào)為X260筆記本電腦)，軟件平臺(tái)為beckhoff/Twincat3。上位機(jī)與機(jī)器人之間采用WIFI通訊。機(jī)器人由直流伺服電機(jī)驅(qū)動(dòng)，直流伺服驅(qū)動(dòng)電機(jī)為24V電壓驅(qū)動(dòng)，型號(hào)為57B2C1230-SCO，額定功率120W，額定轉(zhuǎn)速3 000r/min。經(jīng)過測(cè)試，由電機(jī)驅(qū)動(dòng)的機(jī)器人速度從0m/s達(dá)到0.32m/s，用時(shí)0.9s。機(jī)器人轉(zhuǎn)向由轉(zhuǎn)向舵機(jī)驅(qū)動(dòng)，舵機(jī)型號(hào)為LF20MG，旋轉(zhuǎn)角度范圍為0°～90°，扭矩2Nm，其轉(zhuǎn)角從-0.45rad到0.4rad用時(shí)1.27s。速度和角度的響應(yīng)速度滿足試驗(yàn)和應(yīng)用要求。試驗(yàn)場(chǎng)地為11.4m×5.6m的長方形場(chǎng)地，其環(huán)境與溫室環(huán)境相似。圖10、圖11為反光標(biāo)貼與試驗(yàn)場(chǎng)地環(huán)境圖。

機(jī)器人采用激光定位方式，因此需采用圖中反光率比較強(qiáng)的反光材料。反光標(biāo)貼直徑為90mm，放置時(shí)盡可能與試驗(yàn)場(chǎng)地垂直。

圖11 試驗(yàn)場(chǎng)地環(huán)境圖

3.2 機(jī)器人路徑跟蹤仿真

仿真是基于上訴兩種算法的基礎(chǔ)上進(jìn)行，為了證明算法穩(wěn)定可靠和準(zhǔn)確性，采用圓弧和直線路徑進(jìn)行仿真。

平面上的直線可以用ax+by+c=0來表示一條直線，系數(shù)為(a,b,c)。選取機(jī)器人仿真直線為(a,b,c)=(1,-1,0)，機(jī)器人的起始點(diǎn)為(x0,y0,θ0)=(1,4,-π/2)，仿真結(jié)果如圖12所示。

圖12 機(jī)器人直線路徑仿真圖

由圖12可以看出：機(jī)器人在點(diǎn)(4,4)時(shí)開始收斂期望路徑，跟蹤穩(wěn)定，跟蹤誤差趨向于零。

選取起始點(diǎn)為(x0,y0,θ0)=(0.5,-0.5,-2π/3)，期望路徑為x2+y2=1，仿真結(jié)果如圖13所示。

由圖13可以看出：機(jī)器人大約在(0,1)位置第1次收斂于期望路徑，之后一直沿著期望路徑進(jìn)行運(yùn)動(dòng)，具有較好的跟蹤穩(wěn)定性。

3.3 機(jī)器人路徑跟蹤試驗(yàn)

試驗(yàn)同樣采用跟蹤直線和跟蹤圓弧軌跡的試驗(yàn)方案，驗(yàn)證機(jī)器人在工程應(yīng)用中的路徑跟蹤效果。跟蹤的直線為(a,b,c)=(1,1,-2.5)，機(jī)器人的起始點(diǎn)為(x0,y0,θ0)=(4.6,-1.4,1.9rad)，試驗(yàn)結(jié)果如圖14所示。

圖13 機(jī)器人圓弧路徑仿真圖

圖14 機(jī)器人直線路徑試驗(yàn)圖

由圖14可以看出：在試驗(yàn)過程中，機(jī)器人在點(diǎn)(1.5,-2)附近開始收斂，當(dāng)機(jī)器人到達(dá)點(diǎn)(1,-1)時(shí)，機(jī)器人完全收斂，具有較快的收斂性；中間大約有2cm的振蕩，最后趨向穩(wěn)定；當(dāng)跟蹤穩(wěn)定后，其橫向誤差約±8mm，符合溫室環(huán)境路徑限制的要求。

圓弧路徑跟蹤，選取起始點(diǎn)為(x0,y0,θ0)=(0.22,0.24,3.4rad)，期望路徑為x2+y2=2.25，試驗(yàn)結(jié)果如圖15所示。

由圖15可以看出：機(jī)器人在收斂于期望圓弧軌跡過程中，并不能直接較好地收斂。跟蹤過程中出現(xiàn)一次振蕩，這是由于機(jī)器人的轉(zhuǎn)向輪轉(zhuǎn)角范圍為-0.49～0.49rad，軸距為0.35m，根據(jù)公式(11)得機(jī)器人最小轉(zhuǎn)彎半徑為0.66m，該半徑大于機(jī)器人第1次可以直接收斂于期望圓弧路徑轉(zhuǎn)彎半徑，因此會(huì)出現(xiàn)1次振蕩。第1次收斂于期望軌跡約在點(diǎn)(-0.5,-1.5)，此后沒有出現(xiàn)機(jī)器人跟蹤路徑振蕩的情況發(fā)生；當(dāng)跟蹤穩(wěn)定后，其橫向誤差約±11mm，具有良好的精度和跟蹤效果。

圖15 機(jī)器人圓弧路徑試驗(yàn)圖

4 結(jié)論與討論

1)基于機(jī)器人運(yùn)動(dòng)學(xué)模型，提出基于航向角誤差算法的路徑跟蹤控制方法。計(jì)算機(jī)仿真和試驗(yàn)表明：所提出的算法能使機(jī)器人實(shí)現(xiàn)路徑跟蹤，且具有良好的跟蹤穩(wěn)定性。

2)分析了機(jī)器人在路徑跟蹤過程中不同前視距離點(diǎn)的選擇對(duì)機(jī)器人路徑跟蹤效果的影響，并提出了一種與航向角誤差大小相關(guān)的、前視距離可變的前視點(diǎn)的選擇方法，且在仿真和試驗(yàn)中驗(yàn)證了該方法的有效性。

3)試驗(yàn)表明：該算法在與溫室環(huán)境相近的試驗(yàn)場(chǎng)地中具有較好的跟蹤效果，能夠?qū)崿F(xiàn)工程應(yīng)用，為溫室環(huán)境監(jiān)測(cè)機(jī)器人的路徑跟蹤控制提供了參考。

在機(jī)器人運(yùn)動(dòng)過程中，當(dāng)跟蹤穩(wěn)定后、轉(zhuǎn)向輪會(huì)有左右抖動(dòng)的情況，這應(yīng)該與控制轉(zhuǎn)向角輸出的數(shù)據(jù)波動(dòng)關(guān)系，也同地面的平整度有關(guān)。因此，下一步優(yōu)化的方向可以采用濾波的方式減少輸入和輸出數(shù)據(jù)的波動(dòng)，解決轉(zhuǎn)向輪轉(zhuǎn)向抖動(dòng)問題。同時(shí)，可考慮采用俯仰腳校正算法提高其對(duì)地面不平整的定位精度，提高平臺(tái)的綜合性能，降低成本。