蔣建東,張 鈞,李聰聰,章恩光
(浙江工業(yè)大學(xué) 特種裝備制造與先進(jìn)加工技術(shù)教育部重點實驗室,浙江 杭州 310014)
履帶式移動機(jī)器人自主跟隨算法研究
蔣建東,張 鈞,李聰聰,章恩光
(浙江工業(yè)大學(xué) 特種裝備制造與先進(jìn)加工技術(shù)教育部重點實驗室,浙江 杭州 310014)
基于履帶式移動機(jī)器人自主跟隨控制平臺,研究并設(shè)計了一種履帶式農(nóng)業(yè)機(jī)械自主跟隨控制系統(tǒng).結(jié)合神經(jīng)模糊控制(ANFIS)思想,提出一種自適應(yīng)前視距離自主跟隨算法,并進(jìn)行了履帶式移動機(jī)器人的運動學(xué)求解并建立其純追蹤模型,在純追蹤算法中對前視距離參數(shù)進(jìn)行動態(tài)調(diào)節(jié).通過將引導(dǎo)車行駛路徑劃分成多段直線路徑組合成的折線路徑,使跟隨車自主跟隨的本質(zhì)轉(zhuǎn)化為對折線路徑的快速跟蹤,實現(xiàn)跟隨車對引導(dǎo)車的自主跟隨.仿真及試驗結(jié)果表明:折線路徑平均橫向偏差由0.092 m減少為0.022 m,調(diào)節(jié)時間縮短約50%,該算法對折線路徑的跟蹤精度較高,響應(yīng)時間短,滿足自主跟隨需求.
履帶式;自主跟隨;前視距離;模糊神經(jīng);路徑跟蹤
當(dāng)前由于農(nóng)業(yè)機(jī)械自主導(dǎo)航控制系統(tǒng)成本高昂[1],其研究多局限于實驗室環(huán)境,對于應(yīng)付道路行駛與農(nóng)田耕作的惡劣環(huán)境及突發(fā)情況的能力還有待提高,無法大規(guī)模應(yīng)用.由此尋找一種既能提高農(nóng)業(yè)機(jī)械自動化、智能化,同時又能兼顧其成本,可靠性與穩(wěn)定性的新型農(nóng)機(jī)智能化控制系統(tǒng)已成為各國研究的重點.為了兼顧上述特點,歐美日開展了農(nóng)業(yè)機(jī)械自主跟隨控制系統(tǒng)的研究[2],該系統(tǒng)由引導(dǎo)車與跟隨車組成,引導(dǎo)車由駕駛員駕駛,跟隨車在其后自主跟隨.由于該系統(tǒng)中包含了駕駛?cè)藛T,有效提高了該系統(tǒng)的智能化程度,使得其應(yīng)對道路行駛,惡劣耕作環(huán)境和突發(fā)情況的能力有了較大提高.
當(dāng)前農(nóng)用車輛自主跟隨控制系統(tǒng)研究多集中于輪式車輛直線路徑跟隨前進(jìn),既有采用固定前視距離純追蹤算法進(jìn)行農(nóng)業(yè)機(jī)械導(dǎo)航控制[3],但存在超調(diào),也有采用模糊控制進(jìn)行導(dǎo)航控制[4],橫向偏差與航向偏差為輸入,前輪轉(zhuǎn)角為期望輸出,同時模糊規(guī)則由駕駛?cè)藛T主觀提出,跟蹤誤差大,無法快速修正.上述跟蹤算法對于復(fù)雜多變路徑的跟蹤精度均不能滿足農(nóng)業(yè)精確作業(yè)要求,即使有些系統(tǒng)可以沿曲線路徑跟隨行駛,但由于算法過于復(fù)雜,成本過高,不適合大規(guī)模推廣[5-6].針對提高復(fù)雜路徑的跟隨響應(yīng)和跟蹤精度,筆者通過研究兩車協(xié)同作業(yè)的路徑跟隨方法,提出了一種基于ANFIS的自適應(yīng)前視距離自主跟隨算法.通過這種路徑跟隨方式,有效提高了模糊規(guī)則的客觀性和準(zhǔn)確性,較好地改善了履帶式移動機(jī)器人的自主跟隨效果.
履帶式移動機(jī)器人平臺自主跟隨控制系統(tǒng)硬件框圖如圖1所示,主要包括PC端上位機(jī)系統(tǒng),MCU主控單元,GPS/DR(Global positioning system/dead reckoning)導(dǎo)航控制單元,視頻處理單元、無線收發(fā)單元,速度測量單元,電機(jī)驅(qū)動單元,其中STM32F103ZET6作為MCU主控芯片.履帶式移動機(jī)器人自主跟隨控制系統(tǒng)實物圖如圖2所示.
圖1 履帶式移動機(jī)器人硬件系統(tǒng)框圖Fig.1 Block diagram of hardware system
圖2 履帶式移動機(jī)器人自主跟隨系統(tǒng)Fig.2 Autonomous following control system
2.1 自主跟隨軟件流程
履帶式移動機(jī)器人自主跟隨控制系統(tǒng)采用兩種模式,分別是遙控模式,自主跟隨模式.遙控模式下操控人員通過PC端上位機(jī)軟件直接發(fā)送指令控制履帶式移動機(jī)器人行駛.自主跟隨模式流程圖如圖3所示,設(shè)置路徑更新周期T與傳感器采樣周期t,令T=nt,每隔一個采樣周期讀取引導(dǎo)車、跟隨車坐標(biāo)和速度信息,每隔一個路徑更新周期更新一次跟蹤路徑,不斷循環(huán),實現(xiàn)跟隨車自主跟隨引導(dǎo)車.
圖3 自主跟隨模式軟件流程圖Fig.3 Software flow chart of autonomous following mode
2.2 兩車協(xié)同作業(yè)的路徑跟隨方法
路徑規(guī)劃是移動機(jī)器人自主跟隨控制系統(tǒng)的重要組成部分[7].自主跟隨控制系統(tǒng)通過引導(dǎo)車與跟隨車信息互聯(lián)的方式獲取周圍環(huán)境信息進(jìn)行局部路徑規(guī)劃[8-9].通過確定起始點為A,終止點為B來確定一條導(dǎo)航直線AB進(jìn)行直線路徑跟蹤,取履帶式移動機(jī)器人行駛時的一段轉(zhuǎn)彎路徑進(jìn)行分析,如圖4所示,黑色虛線矩形代表引導(dǎo)車,黑色實線矩形代表跟隨車,A,B,C,D,E所在點代表引導(dǎo)車重心位置.自主跟隨模式下,令間距等于規(guī)定行使速度與路徑更新周期的乘積,使得引導(dǎo)車行駛完一個路徑更新周期時跟隨車恰好能到達(dá)引導(dǎo)車之前一段路徑的起始點附近,有利于跟隨車快速準(zhǔn)確地跟隨下一段路徑.引導(dǎo)車以一定速度行駛,每隔一個路徑更新周期生成一條直線跟蹤路徑,跟隨車實時跟蹤引導(dǎo)車生成的最新跟蹤路徑,實現(xiàn)自主跟隨.以這種方式進(jìn)行的自主跟隨系統(tǒng),將轉(zhuǎn)彎路徑中的曲線路徑分為幾段直線路徑進(jìn)行路徑跟蹤,即AB,BC,CD,DE段,使得引導(dǎo)車行駛路徑成為多段直線路徑組合成的折線路徑,跟隨車自主跟隨的本質(zhì)便轉(zhuǎn)換為對折線路徑的快速跟蹤.該方法不但減少了計算量,還便易工程實現(xiàn).
圖4 轉(zhuǎn)彎路徑規(guī)劃Fig.4 Turning path planning
2.3 履帶式移動機(jī)器人運動學(xué)求解與自主跟隨算法
純追蹤算法作為一種路徑跟蹤算法已有許多年的歷史[10],該算法是一種幾何算法,用來計算車輛到達(dá)目標(biāo)位置的軌跡弧長,其核心在于確定一個合適的前視距離.該算法模擬車內(nèi)駕駛員的視覺,具有簡單、直觀且兼具仿生學(xué)的特點,被廣泛應(yīng)用在路徑跟蹤領(lǐng)域上.
如圖5所示建立導(dǎo)航坐標(biāo)系(X—O—Y)和車體坐標(biāo)系(vX—p—vY),設(shè)履帶式移動機(jī)器人轉(zhuǎn)彎曲率為γ,γ為有符號數(shù),機(jī)器人逆時針旋轉(zhuǎn)時為正,順時針旋轉(zhuǎn)時為負(fù),圖5以順時針旋轉(zhuǎn)為例,故γ<0;R為移動機(jī)器人此刻的瞬時轉(zhuǎn)彎半徑,為有符號數(shù);d為移動機(jī)器人相對于跟蹤路徑l的橫向偏差,為有符號數(shù),同時規(guī)定機(jī)器人在沿期望路徑前進(jìn)方向左側(cè)時d<0,反之,d>0,圖5所示d<0;φ為移動機(jī)器人相對于跟蹤路徑l的航向偏差,航向偏差等于當(dāng)前航向角度與跟蹤直線相對于X軸正向角度之差,規(guī)定車體坐標(biāo)系vY軸偏向期望路徑左側(cè)時為正,右側(cè)時為負(fù);Ld為前視距離;ψ為移動機(jī)器人沿著轉(zhuǎn)向弧線到達(dá)目標(biāo)點T(vXt,vYt)時的航向變化角度;(vXt,vYt)為T點在車體坐標(biāo)系下的坐標(biāo);α為跟蹤路徑l與大地坐標(biāo)系X軸正向的角度差;θ為機(jī)器人前進(jìn)方向與大地坐標(biāo)系X軸正向的角度差.
圖5 運動學(xué)、純追蹤模型幾何示意圖Fig.5 Geometry sketch map of kinematics and pure pursuit models
由履帶式移動機(jī)器人運動學(xué)模型可得
(1)
式中:L為履帶式移動機(jī)器人雙側(cè)履帶中心間距;iL和iR分別為履帶式移動機(jī)器人左右兩側(cè)履帶的滑轉(zhuǎn)率,可表示為
(2)
vl,vr分別為履帶式移動機(jī)器人左右兩側(cè)履帶的實際速度,在不考慮履帶滑轉(zhuǎn)率的情況下,履帶的理論速度等于實際速度,即
vL=vl,vR=vr
(3)
由于實際行駛過程中,履帶滑轉(zhuǎn)率由多方因素決定,很難測定,假設(shè)履帶式移動機(jī)器人在行駛過程中雙側(cè)履帶沒有產(chǎn)生滑動,結(jié)合式(1~3)可得履帶式移動機(jī)器人運動學(xué)方程為
(4)
式中P為履帶式移動機(jī)器人當(dāng)前的位姿矩陣.對式(4)積分后得
(5)其中:(xp0,yp0,θ0)為履帶式移動機(jī)器人在t=0時的位置和方向;(xp,yp,θ)為機(jī)器人當(dāng)前位置和方向.
履帶式移動機(jī)器人在P點的線速度和角速度可分別表示為
(6)
假設(shè)履帶式移動機(jī)器人的轉(zhuǎn)彎半徑為R,由式vP=RωP可得機(jī)器人當(dāng)前轉(zhuǎn)彎半徑為
(7)
由圖5可得履帶式移動機(jī)器人純追蹤方程為
(8)
已知R=1/γ,結(jié)合式(8)得
(9)
由圖5可得
(10)
(11)
由式(11)可得
(12)
2.4 基于ANFIS的前視距離仿真
2.4.1 前視距離控制策略
由于前視距離是純追蹤算法中的一個關(guān)鍵參數(shù)[11],如何確定一個合適的前視距離是本節(jié)研究的重點.前視距離的大小與農(nóng)業(yè)機(jī)械的橫向偏差、航向偏差和車速有直接關(guān)系.路徑跟蹤的主要目的便是使跟蹤車輛的橫向偏差與航向偏差都趨于零.當(dāng)橫向偏差與航向偏差過大時,農(nóng)機(jī)需要較小的前視距離快速減小偏差,提高系統(tǒng)響應(yīng)速度;當(dāng)橫向偏差與航向偏差較小時,需要增大前視距離防止系統(tǒng)響應(yīng)超調(diào),提高系統(tǒng)穩(wěn)定性.當(dāng)車速過大時,需要適當(dāng)增大前視距離,防止系統(tǒng)響應(yīng)超調(diào),當(dāng)車速較小時,適當(dāng)減小前視距離有利于提高路徑跟蹤精度.由于農(nóng)機(jī)行駛速度較低且基本無變化,可認(rèn)為是恒速,故筆者不考慮速度對前視距離的影響.
目前,一般通過仿真與實驗確定一個合理的前視距離,但該方法由于前視距離固定,無法根據(jù)農(nóng)機(jī)當(dāng)前位置誤差實時調(diào)整前視距離,并非是最優(yōu)解.除此之外,也有采用模糊算法,通過歸納專家的操作經(jīng)驗來制定模糊規(guī)則表,從而在跟蹤過程中對前視距離進(jìn)行實時調(diào)節(jié).由于農(nóng)業(yè)機(jī)械駕駛?cè)藛T的主觀性導(dǎo)致利用直覺操作經(jīng)驗或直觀感知來制定模糊規(guī)則的準(zhǔn)確性大大降低,極大地提高了后期調(diào)試的工作量.因此將神經(jīng)網(wǎng)絡(luò)引入模糊系統(tǒng),采用MATLAB中的ANFIS[12],根據(jù)大量數(shù)據(jù),通過自適應(yīng)建模方法建立起模糊推理系統(tǒng).由于用神經(jīng)網(wǎng)絡(luò)建立FIS是對數(shù)據(jù)進(jìn)行處理的結(jié)果,更具客觀性,同時采用輸出量是數(shù)值函數(shù)的Sugeno型模糊模型,便于軟件編程實現(xiàn).
2.4.2 ANFIS訓(xùn)練樣本采集
首先確定橫向偏差的模糊論域為[-0.3,0.3](單位為m),航向偏差的模糊論域為[-π/2,π/2](單位為弧度),其中橫向偏差以0.07 m為一檔,航向偏差以15度為一檔進(jìn)行等比例劃分仿真.通過對不同分檔下不同大小的固定前視距離Matlab/Simulink仿真,確定行駛速度為0.3 m/s時各分檔下的最佳前視距離.將這一系列仿真所得數(shù)據(jù)采集得到ANFIS訓(xùn)練樣本,由于樣本數(shù)據(jù)較多,只列出部分?jǐn)?shù)據(jù),如表1所示.
2.4.3 生成初始FIS
首先建立T-S型模糊模型,選擇“And method”為“prod”, “Defuzzification”為“wtaver”,將上表所得訓(xùn)練樣本裝入ANFIS,選擇網(wǎng)格分割法,網(wǎng)格分割法是將已裝入ANFIS的數(shù)據(jù)用“網(wǎng)格”加以分割,然后按照設(shè)定的參數(shù),依據(jù)模糊C-均值聚類方法建立起模糊系統(tǒng).橫向偏差與航向偏差均用7個模糊子集來覆蓋輸入變量,模糊語言值分別為橫向偏差:{左大,左中,左小,零,右小,右中,右大}={LB,LM,LS,Z,RS,RM,RB},航向偏差:{負(fù)大,負(fù)中,負(fù)小,零,正小,正中,正大}={FB,FM,FS,Z,ZS,ZM,ZB}.選擇高斯型函數(shù)作為兩個輸入變量的隸屬度函數(shù),在輸出量隸屬函數(shù)類型中選則“l(fā)inear”,表示選定輸出量為輸入量的線性函數(shù).圖6所示為ANFIS模型結(jié)構(gòu)圖,其中x1為橫向偏差d,x2為航向偏差φ,y為前視距離Ld.
表1 自適應(yīng)神經(jīng)模糊系統(tǒng)訓(xùn)練樣本
圖6 ANFIS模型結(jié)構(gòu)圖Fig.6 ANFIS model structure diagram
2.4.4 生成隸屬函數(shù)圖及模糊規(guī)則
選用“hybrid”方法對樣本數(shù)據(jù)進(jìn)行訓(xùn)練,設(shè)置“Error Tolerance”為0,訓(xùn)練次數(shù)為500次,訓(xùn)練結(jié)束后得到誤差為0.004 3,訓(xùn)練后得到橫向偏差隸屬度函數(shù)圖、航向偏差隸屬度函數(shù)圖如圖7所示.
由高斯型隸屬函數(shù)公式與各隸屬函數(shù)參數(shù)可得
覆蓋輸入量的各模糊子集隸屬度函數(shù)表達(dá)式.整理輸出函數(shù)參數(shù)可得出49個輸出函數(shù)表達(dá)式.由此便可得出ANFIS根據(jù)訓(xùn)練樣本建立的關(guān)于前視距離的2階Sugeno型FIS的模糊規(guī)則,由于控制規(guī)則較多,所以只列出了部分規(guī)則:
R1:ifdis LB andφis FB thenLd=-0.567 6d-0.152 4φ-0.055 34
R2:ifdis LB andφis FM thenLd=-0.446 7d-0.154 7φ+0.078 13
R3:ifdis LB andφis FS thenLd=-0.472 7d-0.148 8φ+0.152 1
?
R47:ifdis RB andφis ZS thenLd=1.005d+0.188 1φ-0.095 37
R48:ifdis RB andφis ZM thenLd=0.993 4d+0.196 7φ-0.198 7
R49:ifdis RB andφis ZB thenLd=1.003d+0.173 5φ-0.274 1
圖7 兩種偏差隸屬度函數(shù)圖Fig.7 Two kinds of membership function diagram
設(shè)定履帶式移動機(jī)器人初始位置在(0,0)(單位為米)處,,初始航向角為45°,行駛速度為0.3 m/s,固定前視距離取車速為2~3 s走過的距離時與人的駕駛行為接近[13],故取0.6 m,圖8黑色軌跡(農(nóng)田作業(yè)邊界和作業(yè)路徑為典型折線段)為引導(dǎo)車行駛時傳遞給跟隨車的期望跟蹤路徑,即
圖8 折線路徑試驗場景Fig.8 Experiments of poly line path
兩種路徑跟蹤算法對比如圖9所示,其中實線代表動態(tài)前視距離算法,虛線代表固定前視距離算法.可知在y=x段時,兩種算法效果相當(dāng),當(dāng)x=
0.5進(jìn)入折線路徑時,動態(tài)Ld算法能快速跟蹤期望路徑,并將誤差控制在較小范圍內(nèi),其橫向偏差最大為0.065 m,平均為0.024 m,需要時間2.17 s,固定Ld算法橫向偏差快速增大且長期維持較大誤差直至進(jìn)入第三條直線路徑,其最大橫向偏差為0.153 m,平均橫向偏差為0.103 m.進(jìn)入y=x-1直線段后,動態(tài)Ld算法依舊能快速跟蹤期望路徑,橫向偏差最大為0.065 m,平均為0.022 m,需要時間2.13 s,而固定Ld算法由于在y=0.5段積累的誤差導(dǎo)致其無法及時跟蹤期望路徑,其橫向偏差仍舊維持在較大水平,最大橫向偏差為0.152 m,平均橫向偏差為0.092 m.綜上所述可知動態(tài)Ld算法在跟蹤折線路徑時的調(diào)節(jié)時間,最大橫向偏差,平均橫向偏差均明顯小于固定Ld算法,滿足自主跟隨模式下跟蹤路徑的連續(xù)變化.
圖9 兩種算法對比圖Fig.9 Comparison of two algorithms
以履帶式移動機(jī)器人為平臺,針對履帶式移動機(jī)器人兩車協(xié)同跟隨作業(yè),進(jìn)行了自主跟隨控制平臺及控制系統(tǒng)設(shè)計,以及兩車協(xié)同作業(yè)的路徑跟隨模型及算法研究,提出了一種動態(tài)最優(yōu)前視距離的農(nóng)業(yè)履帶移動機(jī)器人自主跟隨算法,其核心在于通過跟隨過程中不同橫向偏差與航向偏差信息采集,通過自適應(yīng)神經(jīng)模糊算法確定出跟隨模型的最佳前視距離,從而提高路徑跟隨精度,最后進(jìn)行了典型折線路徑跟隨仿真及試驗.結(jié)果表明:采用動態(tài)最優(yōu)前視距離自主跟隨算法的調(diào)節(jié)時間縮短約50%,折線路徑平均橫向偏差由0.092 m減少為0.022 m.因此該算法具有良好的實時調(diào)節(jié)性能,滿足農(nóng)機(jī)裝備在農(nóng)田作業(yè)復(fù)雜路徑下的自主跟隨需求.
[1] 張立彬,計時鳴,胥芳,等.農(nóng)業(yè)機(jī)器人的主要應(yīng)用領(lǐng)域和關(guān)鍵技術(shù)[J].浙江工業(yè)大學(xué)學(xué)報,2002,30(1):36-41.
[2] TORII T. Research in autonomous agriculture vehicles in Japan[J]. Computers and electronics in agriculture,2000,25(1/2):133-133.
[3] 張智剛,羅淆文,趙祚喜,等.基于Kalman濾波和純追蹤模型的農(nóng)業(yè)機(jī)械導(dǎo)航控制[J].農(nóng)業(yè)機(jī)械學(xué)報,2009,40(增):6-12.
[4] 陳軍,蔣浩然,劉沛,等.果園移動機(jī)器人曲線路徑導(dǎo)航控制[J].農(nóng)業(yè)機(jī)械學(xué)報,2012,43(4):179-187.
[5] 陳軍,朱忠祥,鳥巢諒,等.拖拉機(jī)沿曲線路徑的跟蹤控制[J].農(nóng)業(yè)工程學(xué)報,2006,22(11):108-111.
[6] 朱忠祥,松正河,謝斌,等.拖拉機(jī)隊列自動控制系統(tǒng)[J].農(nóng)業(yè)機(jī)械學(xué)報,2008,40(8):149-154.
[7] 王燕,張立彬,楊慶華.黃瓜采摘機(jī)器人運動規(guī)劃與控制系統(tǒng)研究[D].杭州:浙江工業(yè)大學(xué),2010.
[8] 王宇,董輝,徐建明,等.一種新的智能小車控制策略[J].浙江工業(yè)大學(xué)學(xué)報,2012,40(2):217-221.
[9] 陳曉瞇,孟志青,徐杰.基于混合禁忌搜索算法的動態(tài)車輛路徑研究[J].浙江工業(yè)大學(xué)學(xué)報,2009,37(5):580-585.
[10] 陳軍,鳥巢諒,朱忠祥,等.拖拉機(jī)在牧草地上自動引導(dǎo)行走的控制[J].農(nóng)業(yè)機(jī)械學(xué)報,2005,36(7):104-107.
[11] LI C H, ZHU X J, CAO G Y, et al. Identification of the Hammerstein model of a PEMFC stack based on least squares support vector machines[J]. Journal of power sources,2008,175(1):303-316.
[12] 劉懷國,孫建華,張冰,等.ANFIS及其在控制系統(tǒng)中的應(yīng)用[J].華東船舶工業(yè)學(xué)院學(xué)報(自然科學(xué)版),2001,15(5):27-31.
[13] POMERLEAU D A. Neural network perception for mobile robot guidance[M]. Massachusetts: Kluwer Academic Publishers Norwell,1993.
(責(zé)任編輯:劉 巖)
Study on autonomous following algorithm of crawler-type mobile robot
JIANG Jiandong, ZHANG Jun, LI Congcong, ZHANG Enguang
(Key Laboratory of Special Purpose Equipment and Advanced Manufacturing Technology, Ministry of Education, Zhejiang University of Technology, Hangzhou 310014, China)
An autonomous tracking control system for agricultural machinery is studied and designed based on the autonomous tracking control platform of crawler-type mobile robot. An algorithm for adaptive look-ahead distance is proposed combined with neural fuzzy control (ANFIS). The kinematic model and the pure pursuit model for crawler-type mobile robot ate established and the parameters of look-ahead distance are adjusted dynamically. By dividing the driving path of the guided vehicle into a poly line path combined with multiple linear paths, the essence of the following vehicle is transformed into the fast track of the poly line path, and the guided vehicle is autonomously tracked by the following vehicle. The experimental results indicated that the average lateral deviation of the line path is reduced from 0.092 m to 0.022 m, and the adjust time is reduced by about 50%. This algorithm can meet the demands of autonomous tracking.
crawler-type; autonomous following; look-ahead distance; fuzzy neural; path following
2016-11-02
國家自然科學(xué)基金資助項目(51375456);浙江省重大專項項目(2013C01048)
蔣建東(1974—),男,四川廣安人,教授,博士,主要從事機(jī)電系統(tǒng)設(shè)計與分析方面的研究,E-mail:jiangjd@zjut.edu.cn.
S232;TP242
A
1006-4303(2017)04-0355-06