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

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

0 引言

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

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

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

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

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

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

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

1.2 機器人運動學(xué)模型分析

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

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

圖2 機器人運動模型圖

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

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

(1)

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

ω—移動機器人角度變化率(rad/s)；

θ—航向角(rad)；

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

l—機器人軸距(m)。

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

l/R2=tanγ

(2)

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

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

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

2.1 坐標轉(zhuǎn)換

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

圖3 機器人路徑跟蹤示意圖

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

圖4 機器人目標點坐標轉(zhuǎn)換幾何原理圖

transformation of robot target point

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

(3)

(4)

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

2.2 航向角誤差算法

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

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

(5)

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

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

γH=kH·β

(6)

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

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

圖5 機器人航向角誤差示意圖

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

(7)

式中vH—機器人運動速度(m/s)；

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

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

2.3 純跟蹤算法原理

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

圖6 純跟蹤算法模型

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

(8)

由式(8)整理得

R=L2/2y

(9)

(10)

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

R—圓弧的半徑(m)。

式(9)和式(10)中，L為連接機器人驅(qū)動輪中點V和A圓弧段的弦長。

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

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

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

圖7 簡化二輪車運動學(xué)模型

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

tanγ=l/R

(11)

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

(12)

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

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

(13)

式中vC—機器人運動速度(m/s)；

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

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

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

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

2.4 前視距離的控制

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

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

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

圖9 前視距離選擇策略

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

(14)

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

由以上的描述可得

(15)

(16)

式中R1—C1半徑(m)；

R2—C2半徑(m)。

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

(17)

式中θ—常數(shù)。

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

3 仿真與試驗

3.1 材料與設(shè)備

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

機器人采用激光定位方式，因此需采用圖中反光率比較強的反光材料。反光標貼直徑為90mm，放置時盡可能與試驗場地垂直。

圖11 試驗場地環(huán)境圖

3.2 機器人路徑跟蹤仿真

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

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

圖12 機器人直線路徑仿真圖

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

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

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

3.3 機器人路徑跟蹤試驗

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

圖13 機器人圓弧路徑仿真圖

圖14 機器人直線路徑試驗圖

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

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

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

圖15 機器人圓弧路徑試驗圖

4 結(jié)論與討論

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

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

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

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