溫室環境監測機器人路徑跟蹤算法與試驗

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

(華南農業大學 工程學院，廣州 510642)

0 引言

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

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

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

1 系統結構與運動學模型

1.1 機器人系統結構

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

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

1.2 機器人運動學模型分析

假設室內路面平坦、運動速度可變，機器人實心輪胎無側偏、無縱側滑移，機器人的瞬時轉向中心是兩條車軸線的交點。根據機器人的結構特點，建立如圖2所示的運動學模型。

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

圖2 機器人運動模型圖

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

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

(1)

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

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

θ—航向角(rad)；

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

l—機器人軸距(m)。

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

l/R2=tanγ

(2)

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

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

2 坐標轉換與算法原理

2.1 坐標轉換

上述系統結構中，機器人的采用激光定位方式，所使用的激光定位傳感器NAV350具有建圖和全局定位功能，采用三點定位方式。機器人的路徑跟蹤問題可轉化為機器人跟蹤期望路徑上的連續點的問題，如圖3所示。

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

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

圖4 機器人目標點坐標轉換幾何原理圖

transformation of robot target point

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

(3)

(4)

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

2.2 航向角誤差算法

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

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

(5)

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

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

γH=kH·β

(6)

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

kH—常數系數。

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

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

(7)

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

kv—正的常系數。

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

2.3 純跟蹤算法原理

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

圖6 純跟蹤算法模型

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

(8)

由式(8)整理得

R=L2/2y

(9)

(10)

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

R—圓弧的半徑(m)。

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

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

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

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

圖7 簡化二輪車運動學模型

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

tanγ=l/R

(11)

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

(12)

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

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

(13)

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

kC—正的常系數；

kx—正的常系數；

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

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

2.4 前視距離的控制

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

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

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

圖9 前視距離選擇策略

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

(14)

其中，n為正整數。

由以上的描述可得

(15)

(16)

式中R1—C1半徑(m)；

R2—C2半徑(m)。

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

(17)

式中θ—常數。

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

3 仿真與試驗

3.1 材料與設備

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

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

圖11 試驗場地環境圖

3.2 機器人路徑跟蹤仿真

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

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

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

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

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

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

3.3 機器人路徑跟蹤試驗

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

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

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

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

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

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

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

4 結論與討論

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

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

3)試驗表明：該算法在與溫室環境相近的試驗場地中具有較好的跟蹤效果，能夠實現工程應用，為溫室環境監測機器人的路徑跟蹤控制提供了參考。

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