履帶式移動機器人自主跟隨算法研究

蔣建東，張 鈞，李聰聰，章恩光

(浙江工業大學 特種裝備制造與先進加工技術教育部重點實驗室，浙江 杭州 310014)

履帶式移動機器人自主跟隨算法研究

蔣建東，張 鈞，李聰聰，章恩光

(浙江工業大學 特種裝備制造與先進加工技術教育部重點實驗室，浙江 杭州 310014)

基于履帶式移動機器人自主跟隨控制平臺，研究并設計了一種履帶式農業機械自主跟隨控制系統.結合神經模糊控制(ANFIS)思想，提出一種自適應前視距離自主跟隨算法，并進行了履帶式移動機器人的運動學求解并建立其純追蹤模型，在純追蹤算法中對前視距離參數進行動態調節.通過將引導車行駛路徑劃分成多段直線路徑組合成的折線路徑，使跟隨車自主跟隨的本質轉化為對折線路徑的快速跟蹤，實現跟隨車對引導車的自主跟隨.仿真及試驗結果表明：折線路徑平均橫向偏差由0.092 m減少為0.022 m，調節時間縮短約50%，該算法對折線路徑的跟蹤精度較高，響應時間短,滿足自主跟隨需求.

履帶式；自主跟隨；前視距離；模糊神經；路徑跟蹤

當前由于農業機械自主導航控制系統成本高昂[1]，其研究多局限于實驗室環境，對于應付道路行駛與農田耕作的惡劣環境及突發情況的能力還有待提高，無法大規模應用.由此尋找一種既能提高農業機械自動化、智能化，同時又能兼顧其成本，可靠性與穩定性的新型農機智能化控制系統已成為各國研究的重點.為了兼顧上述特點，歐美日開展了農業機械自主跟隨控制系統的研究[2]，該系統由引導車與跟隨車組成，引導車由駕駛員駕駛，跟隨車在其后自主跟隨.由于該系統中包含了駕駛人員，有效提高了該系統的智能化程度，使得其應對道路行駛，惡劣耕作環境和突發情況的能力有了較大提高.

當前農用車輛自主跟隨控制系統研究多集中于輪式車輛直線路徑跟隨前進，既有采用固定前視距離純追蹤算法進行農業機械導航控制[3]，但存在超調，也有采用模糊控制進行導航控制[4]，橫向偏差與航向偏差為輸入，前輪轉角為期望輸出，同時模糊規則由駕駛人員主觀提出，跟蹤誤差大，無法快速修正.上述跟蹤算法對于復雜多變路徑的跟蹤精度均不能滿足農業精確作業要求，即使有些系統可以沿曲線路徑跟隨行駛，但由于算法過于復雜，成本過高，不適合大規模推廣[5-6].針對提高復雜路徑的跟隨響應和跟蹤精度，筆者通過研究兩車協同作業的路徑跟隨方法，提出了一種基于ANFIS的自適應前視距離自主跟隨算法.通過這種路徑跟隨方式，有效提高了模糊規則的客觀性和準確性，較好地改善了履帶式移動機器人的自主跟隨效果.

1 履帶式移動機器人平臺及其硬件架構

履帶式移動機器人平臺自主跟隨控制系統硬件框圖如圖1所示，主要包括PC端上位機系統，MCU主控單元，GPS/DR(Global positioning system/dead reckoning)導航控制單元，視頻處理單元、無線收發單元，速度測量單元，電機驅動單元，其中STM32F103ZET6作為MCU主控芯片.履帶式移動機器人自主跟隨控制系統實物圖如圖2所示.

圖1 履帶式移動機器人硬件系統框圖Fig.1 Block diagram of hardware system

圖2 履帶式移動機器人自主跟隨系統Fig.2 Autonomous following control system

2 自主跟隨軟件流程及其算法

2.1 自主跟隨軟件流程

履帶式移動機器人自主跟隨控制系統采用兩種模式，分別是遙控模式，自主跟隨模式.遙控模式下操控人員通過PC端上位機軟件直接發送指令控制履帶式移動機器人行駛.自主跟隨模式流程圖如圖3所示，設置路徑更新周期T與傳感器采樣周期t，令T=nt,每隔一個采樣周期讀取引導車、跟隨車坐標和速度信息，每隔一個路徑更新周期更新一次跟蹤路徑，不斷循環，實現跟隨車自主跟隨引導車.

圖3 自主跟隨模式軟件流程圖Fig.3 Software flow chart of autonomous following mode

2.2 兩車協同作業的路徑跟隨方法

路徑規劃是移動機器人自主跟隨控制系統的重要組成部分[7].自主跟隨控制系統通過引導車與跟隨車信息互聯的方式獲取周圍環境信息進行局部路徑規劃[8-9].通過確定起始點為A，終止點為B來確定一條導航直線AB進行直線路徑跟蹤，取履帶式移動機器人行駛時的一段轉彎路徑進行分析，如圖4所示，黑色虛線矩形代表引導車，黑色實線矩形代表跟隨車，A，B，C，D，E所在點代表引導車重心位置.自主跟隨模式下，令間距等于規定行使速度與路徑更新周期的乘積，使得引導車行駛完一個路徑更新周期時跟隨車恰好能到達引導車之前一段路徑的起始點附近，有利于跟隨車快速準確地跟隨下一段路徑.引導車以一定速度行駛，每隔一個路徑更新周期生成一條直線跟蹤路徑，跟隨車實時跟蹤引導車生成的最新跟蹤路徑，實現自主跟隨.以這種方式進行的自主跟隨系統，將轉彎路徑中的曲線路徑分為幾段直線路徑進行路徑跟蹤，即AB，BC，CD，DE段，使得引導車行駛路徑成為多段直線路徑組合成的折線路徑，跟隨車自主跟隨的本質便轉換為對折線路徑的快速跟蹤.該方法不但減少了計算量，還便易工程實現.

圖4 轉彎路徑規劃Fig.4 Turning path planning

2.3 履帶式移動機器人運動學求解與自主跟隨算法

純追蹤算法作為一種路徑跟蹤算法已有許多年的歷史[10]，該算法是一種幾何算法，用來計算車輛到達目標位置的軌跡弧長，其核心在于確定一個合適的前視距離.該算法模擬車內駕駛員的視覺，具有簡單、直觀且兼具仿生學的特點，被廣泛應用在路徑跟蹤領域上.

如圖5所示建立導航坐標系(X—O—Y)和車體坐標系(vX—p—vY)，設履帶式移動機器人轉彎曲率為γ，γ為有符號數，機器人逆時針旋轉時為正，順時針旋轉時為負，圖5以順時針旋轉為例，故γ<0；R為移動機器人此刻的瞬時轉彎半徑，為有符號數；d為移動機器人相對于跟蹤路徑l的橫向偏差，為有符號數，同時規定機器人在沿期望路徑前進方向左側時d<0,反之，d>0，圖5所示d<0；φ為移動機器人相對于跟蹤路徑l的航向偏差，航向偏差等于當前航向角度與跟蹤直線相對于X軸正向角度之差，規定車體坐標系vY軸偏向期望路徑左側時為正，右側時為負；Ld為前視距離；ψ為移動機器人沿著轉向弧線到達目標點T(vXt,vYt)時的航向變化角度；(vXt,vYt)為T點在車體坐標系下的坐標；α為跟蹤路徑l與大地坐標系X軸正向的角度差；θ為機器人前進方向與大地坐標系X軸正向的角度差.

圖5 運動學、純追蹤模型幾何示意圖Fig.5 Geometry sketch map of kinematics and pure pursuit models

由履帶式移動機器人運動學模型可得

(1)

式中：L為履帶式移動機器人雙側履帶中心間距；iL和iR分別為履帶式移動機器人左右兩側履帶的滑轉率，可表示為

(2)

vl,vr分別為履帶式移動機器人左右兩側履帶的實際速度，在不考慮履帶滑轉率的情況下，履帶的理論速度等于實際速度，即

vL=vl,vR=vr

(3)

由于實際行駛過程中，履帶滑轉率由多方因素決定，很難測定，假設履帶式移動機器人在行駛過程中雙側履帶沒有產生滑動，結合式(1～3)可得履帶式移動機器人運動學方程為

(4)

式中P為履帶式移動機器人當前的位姿矩陣.對式(4)積分后得

(5)其中:(xp0,yp0,θ0)為履帶式移動機器人在t=0時的位置和方向;(xp,yp,θ)為機器人當前位置和方向.

履帶式移動機器人在P點的線速度和角速度可分別表示為

(6)

假設履帶式移動機器人的轉彎半徑為R，由式vP=RωP可得機器人當前轉彎半徑為

(7)

由圖5可得履帶式移動機器人純追蹤方程為

(8)

已知R=1/γ，結合式(8)得

(9)

由圖5可得

(10)

(11)

由式(11)可得

(12)

2.4 基于ANFIS的前視距離仿真

2.4.1 前視距離控制策略

由于前視距離是純追蹤算法中的一個關鍵參數[11]，如何確定一個合適的前視距離是本節研究的重點.前視距離的大小與農業機械的橫向偏差、航向偏差和車速有直接關系.路徑跟蹤的主要目的便是使跟蹤車輛的橫向偏差與航向偏差都趨于零.當橫向偏差與航向偏差過大時，農機需要較小的前視距離快速減小偏差，提高系統響應速度；當橫向偏差與航向偏差較小時，需要增大前視距離防止系統響應超調，提高系統穩定性.當車速過大時，需要適當增大前視距離，防止系統響應超調，當車速較小時，適當減小前視距離有利于提高路徑跟蹤精度.由于農機行駛速度較低且基本無變化，可認為是恒速，故筆者不考慮速度對前視距離的影響.

目前，一般通過仿真與實驗確定一個合理的前視距離，但該方法由于前視距離固定，無法根據農機當前位置誤差實時調整前視距離，并非是最優解.除此之外，也有采用模糊算法，通過歸納專家的操作經驗來制定模糊規則表，從而在跟蹤過程中對前視距離進行實時調節.由于農業機械駕駛人員的主觀性導致利用直覺操作經驗或直觀感知來制定模糊規則的準確性大大降低，極大地提高了后期調試的工作量.因此將神經網絡引入模糊系統，采用MATLAB中的ANFIS[12]，根據大量數據，通過自適應建模方法建立起模糊推理系統.由于用神經網絡建立FIS是對數據進行處理的結果，更具客觀性，同時采用輸出量是數值函數的Sugeno型模糊模型，便于軟件編程實現.

2.4.2 ANFIS訓練樣本采集

首先確定橫向偏差的模糊論域為[-0.3,0.3](單位為m)，航向偏差的模糊論域為[-π/2,π/2](單位為弧度)，其中橫向偏差以0.07 m為一檔，航向偏差以15度為一檔進行等比例劃分仿真.通過對不同分檔下不同大小的固定前視距離Matlab/Simulink仿真，確定行駛速度為0.3 m/s時各分檔下的最佳前視距離.將這一系列仿真所得數據采集得到ANFIS訓練樣本，由于樣本數據較多，只列出部分數據，如表1所示.

2.4.3 生成初始FIS

首先建立T-S型模糊模型，選擇“And method”為“prod”, “Defuzzification”為“wtaver”,將上表所得訓練樣本裝入ANFIS，選擇網格分割法，網格分割法是將已裝入ANFIS的數據用“網格”加以分割，然后按照設定的參數，依據模糊C-均值聚類方法建立起模糊系統.橫向偏差與航向偏差均用7個模糊子集來覆蓋輸入變量，模糊語言值分別為橫向偏差：{左大，左中，左小，零，右小，右中，右大}={LB,LM,LS,Z,RS,RM,RB}，航向偏差：{負大，負中，負小，零，正小，正中，正大}={FB,FM,FS,Z,ZS,ZM,ZB}.選擇高斯型函數作為兩個輸入變量的隸屬度函數，在輸出量隸屬函數類型中選則“linear”，表示選定輸出量為輸入量的線性函數.圖6所示為ANFIS模型結構圖，其中x1為橫向偏差d,x2為航向偏差φ，y為前視距離Ld.

表1 自適應神經模糊系統訓練樣本

圖6 ANFIS模型結構圖Fig.6 ANFIS model structure diagram

2.4.4 生成隸屬函數圖及模糊規則

選用“hybrid”方法對樣本數據進行訓練，設置“Error Tolerance”為0，訓練次數為500次，訓練結束后得到誤差為0.004 3，訓練后得到橫向偏差隸屬度函數圖、航向偏差隸屬度函數圖如圖7所示.

由高斯型隸屬函數公式與各隸屬函數參數可得

覆蓋輸入量的各模糊子集隸屬度函數表達式.整理輸出函數參數可得出49個輸出函數表達式.由此便可得出ANFIS根據訓練樣本建立的關于前視距離的2階Sugeno型FIS的模糊規則,由于控制規則較多，所以只列出了部分規則：

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 兩種偏差隸屬度函數圖Fig.7 Two kinds of membership function diagram

3 自主跟隨算法對比試驗

設定履帶式移動機器人初始位置在(0,0)(單位為米)處，，初始航向角為45°，行駛速度為0.3 m/s，固定前視距離取車速為2～3 s走過的距離時與人的駕駛行為接近[13]，故取0.6 m，圖8黑色軌跡(農田作業邊界和作業路徑為典型折線段)為引導車行駛時傳遞給跟隨車的期望跟蹤路徑，即

圖8 折線路徑試驗場景Fig.8 Experiments of poly line path

兩種路徑跟蹤算法對比如圖9所示，其中實線代表動態前視距離算法，虛線代表固定前視距離算法.可知在y=x段時，兩種算法效果相當，當x=

0.5進入折線路徑時，動態Ld算法能快速跟蹤期望路徑，并將誤差控制在較小范圍內，其橫向偏差最大為0.065 m，平均為0.024 m，需要時間2.17 s，固定Ld算法橫向偏差快速增大且長期維持較大誤差直至進入第三條直線路徑，其最大橫向偏差為0.153 m，平均橫向偏差為0.103 m.進入y=x-1直線段后，動態Ld算法依舊能快速跟蹤期望路徑，橫向偏差最大為0.065 m，平均為0.022 m，需要時間2.13 s，而固定Ld算法由于在y=0.5段積累的誤差導致其無法及時跟蹤期望路徑，其橫向偏差仍舊維持在較大水平，最大橫向偏差為0.152 m，平均橫向偏差為0.092 m.綜上所述可知動態Ld算法在跟蹤折線路徑時的調節時間，最大橫向偏差，平均橫向偏差均明顯小于固定Ld算法，滿足自主跟隨模式下跟蹤路徑的連續變化.

圖9 兩種算法對比圖Fig.9 Comparison of two algorithms

4 結 論

以履帶式移動機器人為平臺，針對履帶式移動機器人兩車協同跟隨作業，進行了自主跟隨控制平臺及控制系統設計，以及兩車協同作業的路徑跟隨模型及算法研究，提出了一種動態最優前視距離的農業履帶移動機器人自主跟隨算法，其核心在于通過跟隨過程中不同橫向偏差與航向偏差信息采集，通過自適應神經模糊算法確定出跟隨模型的最佳前視距離，從而提高路徑跟隨精度，最后進行了典型折線路徑跟隨仿真及試驗.結果表明：采用動態最優前視距離自主跟隨算法的調節時間縮短約50%，折線路徑平均橫向偏差由0.092 m減少為0.022 m.因此該算法具有良好的實時調節性能，滿足農機裝備在農田作業復雜路徑下的自主跟隨需求.

[1] 張立彬,計時鳴,胥芳,等.農業機器人的主要應用領域和關鍵技術[J].浙江工業大學學報,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濾波和純追蹤模型的農業機械導航控制[J].農業機械學報,2009,40(增):6-12.

[4] 陳軍,蔣浩然,劉沛,等.果園移動機器人曲線路徑導航控制[J].農業機械學報,2012,43(4):179-187.

[5] 陳軍,朱忠祥,鳥巢諒,等.拖拉機沿曲線路徑的跟蹤控制[J].農業工程學報，2006,22(11)：108-111.

[6] 朱忠祥,松正河,謝斌,等.拖拉機隊列自動控制系統[J].農業機械學報,2008,40(8):149-154.

[7] 王燕,張立彬,楊慶華.黃瓜采摘機器人運動規劃與控制系統研究[D].杭州：浙江工業大學,2010.

[8] 王宇,董輝,徐建明,等.一種新的智能小車控制策略[J].浙江工業大學學報,2012,40(2):217-221.

[9] 陳曉瞇,孟志青,徐杰.基于混合禁忌搜索算法的動態車輛路徑研究[J].浙江工業大學學報,2009,37(5):580-585.

[10] 陳軍,鳥巢諒,朱忠祥,等.拖拉機在牧草地上自動引導行走的控制[J].農業機械學報,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及其在控制系統中的應用[J].華東船舶工業學院學報(自然科學版),2001,15(5):27-31.

[13] POMERLEAU D A. Neural network perception for mobile robot guidance[M]. Massachusetts: Kluwer Academic Publishers Norwell,1993.

(責任編輯：劉 巖)

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

國家自然科學基金資助項目(51375456)；浙江省重大專項項目(2013C01048)

蔣建東(1974—)，男，四川廣安人，教授，博士，主要從事機電系統設計與分析方面的研究,E-mail:jiangjd@zjut.edu.cn.

S232；TP242

A

1006-4303(2017)04-0355-06