葡萄園履帶式移動平臺自動導航系統的設計與試驗

李偉，楊術明，李茂強，王乾

(1.寧夏大學機械工程學院，寧夏 銀川 750021；2.北方民族大學機電工程學院，寧夏 銀川 750021)

寧夏是釀酒葡萄的最佳產區之一，賀蘭山東麓釀酒葡萄種植面積約3.53萬hm2，且種植面積逐年擴大，年產量20萬t，綜合產值達166億元[1].寧夏釀酒葡萄種植面積約占全國種植面積的四分之一[2]，釀酒葡萄的園間管理包含起藤、打藥、除草、剪枝、灌溉、埋藤等生產作業環節，但隨著釀酒葡萄種植面積的逐漸擴大，與其生產配套的自動化生產設備卻相對滯后[3]，一定程度制約了釀酒葡萄產業的發展.為了推動寧夏釀酒葡萄作業裝備的發展，研究寧夏釀酒葡萄園環境下的自動導航系統，對于實現釀酒葡萄場景下智能化作業具有重要意義.

農業環境下的自動導航是智能農業方面的重要研究內容，現有的自動導航技術主要通過衛星導航、激光雷達導航、超聲波導航和視覺導航等實現[4].Diego等基于RGB-D攝像頭和深度學習，研究了葡萄園自動導航運動規劃器[5].Guy等[6]研究了用于葡萄園噴霧機器人的定位和導航數據融合算法.Backman等[7]研究了一種農業機械導航系統，通過二維激光雷達提取導航跟蹤線，采用GPS和慣性測量單元識別機具的前進方位.謝永良等[8]應用滾動優化原理，以AGV偏差作為輸入獲得前輪期望導向角，提出輪式AGV沿葡萄園防碰撞避障算法.宋廣虎等[9]基于深度學習網絡模型分割葡萄園前景，優化葡萄園行間路徑檢測，提取導航基準線.楊舜禾等[10]基于GPS和激光雷達研究出一套葡萄園拖拉機自動導航系統，能夠較為快速的修正導航橫向偏差.

現有的導航技術對葡萄園生產作業適應性不強，由于葡萄生產伴隨起藤和埋藤等復雜作業，對路面平整性破壞較大，容易造成機具作業的橫向偏差，同時葡萄園各生產環節的精細化操作又需要快速修正橫向偏差，避免碰撞籬架.針對此類問題，研究葡萄園履帶式移動平臺自動導航系統，能夠快速修正環境造成的橫向偏差，沿著規劃路徑實現自動導航，解決目前葡萄生產作業的導航需求，促進葡萄產業智能化發展.

1 材料與方法

1.1 導航系統總體方案設計

寧夏的葡萄多為籬架種植模式，籬架高約1.8 m，每行寬約3 m，葡萄藤依附于籬架生長，移動平臺在兩行籬架間進行作業.采用全局導航方式，確定葡萄園內的導航起點A及導航終點B，且規定葡萄園內主要路線的選擇，將導航起點A、主要路線和導航終點B的路徑作為自動導航的基準線，導航基準線示意如圖1所示.

圖1 導航基準線示意圖

根據葡萄園導航作業需求，將自動導航系統從宏觀上分為環境感知層、決策控制層和運動執行層，環境感知層進行對外界環境信息的感知，決策控制層對獲取到的環境信息進行路徑規劃的決策，控制運動執行層實現導航運動，移動平臺導航系統總體方案如圖2所示.

圖2 導航系統總體方案

1.2 導航系統硬件選型

根據導航系統總體方案對硬件進行選型，履帶式移動平臺整車分為機械部分和電氣部分，機械部分由底盤部分和載體部分組成，電氣部分由電源部分和電路部分組成，履帶式移動平臺總體組成如圖3所示.

圖3 履帶式移動平臺總體組成

移動平臺采用兩種控制模式，分別是自動導航模式和手動控制模式，2種控制模式可互相切換[11].葡萄園籬架種植模式呈行狀態，且葡萄園道路坑泥低洼、高低起伏，結合作業的工作強度及環境條件，移動平臺性能參數如表1所示.

表1 移動平臺性能參數

1.3 環境感知層研究

1.3.1 數據獲取 環境感知是導航系統研究的前提，分析導航傳感器特性，選取激光雷達作為導航系統的環境感知傳感器.激光雷達傳感器為自動導航系統提供環境信息，如今激光雷達可以在葡萄園內大范圍的靜態環境下做到同步定位與地圖構建，圍繞導航系統的建圖與定位展開研究，激光雷達掃描環境示意如圖4所示.

圖4 激光雷達掃描環境示意圖

1.3.2 同步定位與地圖構建 導航系統訂閱多傳感器信息并發布地圖和位置信息，配置成對應的工作模式則調用Cartographer核心算法[12]，葡萄園建圖與定位算法框圖如圖5所示.三維激光雷達掃描的一系列的子圖構成了葡萄園地圖，通過回環檢測來減少長時間累積的誤差，全局優化構建葡萄園地圖[13]，其中葡萄園導航地圖中白色區域為工作區域，黑色區域為葡萄籬架區域，而灰色區域是未探測區域.

圖5 建圖與定位算法框圖

二維激光雷達通過感知道路信息，從而確定自身位置.激光雷達擬合籬架如圖6所示，建立移動平臺的動態坐標系OXY，其中點(XL,YL)和點(XR,YR)表示擬合直線與籬架的交點，lL和lR表示葡萄園擬合的左右兩側種植行邊界線.激光雷達與左右籬架擬合越好，則表明定位越好，為了獲取準確位置信息，需要估計移動平臺的位置和姿態，通過不斷地運動和測量，逐漸降低自身位置的不確定性[14]，獲取移動平臺所處葡萄園環境中的準確位姿信息，保證環境感知層穩定且可靠.

圖6 激光雷達擬合籬架示意圖

1.4 決策控制層研究

決策控制層是導航系統研究的核心，在此基礎上，決策控制層將獲取到的環境信息進行路徑規劃.導航路徑規劃主要有兩個層次的模塊組成，分為全局路徑規劃和局部路徑規劃，全局路徑規劃將導航路徑直接規劃至終點，局部路徑規劃跟隨全局路徑并對遇到的障礙實時進行規避，這兩個模塊組成了葡萄園作業的路徑規劃部分.

1.4.1 全局路徑規劃 全局路徑規劃采用圖搜索的方法，引入了圖的概念可以很好的計算葡萄園地圖中導航點之間的路徑規劃問題[15]，生成一條從起點A到終點B的葡萄園全局路徑.全局路徑規劃的本質上是一種貪心算法，通過二維激光雷達感知葡萄園環境信息，在已知環境中使用迪杰斯特拉算法進行全局路徑規劃[16]，確定葡萄園2個基本要素，分別是頂點a和邊s，用V來保存導航起點A到各個路口區域轉向點的最短距離，用數學語言描述G為：

G=(V，E)

(1)

假定葡萄園籬架和藤為直線且平行等長，若對于導航起點A存在能直接到達的邊s，則把其納入數組S中，同時把其余所有移動平臺不能直接到達的路徑長度假設為無窮遠.通過算法的每一次迭代，直到路徑中包含了地圖中所有頂點，且抵達了導航終點B，這樣所有到達的邊就組成了導航的全局路徑.

1.4.2 局部路徑規劃 在全局路徑規劃把移動平臺拉向導航目標終點時，局部路徑規劃則實時根據路況施力進行避障.局部路徑規劃引入TEB算法，施加外力改變局部路徑以實時規避障礙[17]，局部路徑規劃示意如圖7所示.

圖7 局部路徑規劃示意圖

局部路徑約束的2個目標分別為跟隨已知全局路徑和根據實時路況進行避障[18]，這種約束使得移動平臺以若干弧段組成的平滑的軌跡運動.定義移動平臺的橫縱位置和姿態，則空間內序列為：

Q={xi}i=-0,1,…,nn∈N

(2)

移動平臺運動時兩個位姿間的時間間隔定義為ΔTi，來表示移動平臺由一個狀態運動到下一個狀態所需時間，則時間序列為：

τ={ΔTi}i=0,1,2,…,n-1

(3)

移動平臺在平面運動具有三個自由度，將位姿及時間序列合并后，通過加權多目標優化獲取最優的路徑點，結合導航全局路徑，實時規劃出一條無碰撞的葡萄園局部最優路徑，實現導航作業的路徑規劃.

1.5 運動執行層研究

運動執行層是導航系統研究的關鍵，控制移動平臺跟蹤規劃的路徑，實現自動導航運動.根據葡萄種植模式，分析移動平臺的運動狀態，建立移動平臺運動學模型，研究導航系統轉向控制算法.

1.5.1 建立運動學模型 為了更好地適應葡萄園作業環境，移動平臺選取履帶式運動方式，分析移動平臺的運動狀態，從幾何學角度考慮移動平臺的運動規律，使其在導航過程中滿足幾何約束.移動平臺采取左右履帶輪差速運動方式，在此基礎上，基于移動平臺的運動學特性建立了相關運動學模型[19]，運動學模型及轉向示意圖如圖8所示.

圖8 運動學模型及轉向示意圖

坐標系OXY表示慣性坐標系，點C(x，y)表示移動平臺的中心點坐標，θ表示移動平臺的行駛方位角，移動平臺左右履帶的速度分別表示為VL和VR，左右履帶轉動角速度分別用ωL和ωR表示，移動平臺運動狀態可用Xt表示：

Xt=[xt,yt,θt]T

(4)

通過運動學分析，假定移動平臺驅動履帶與地面接觸運動為純滾動無滑動，也沒有側移情況下，移動平臺的運動學模型可以表示為：

(5)

對移動平臺的行駛過程中的轉向模型進行簡化，分析建立移動平臺轉向示意圖，θτ表示平臺行駛方位角，Φ表示平臺中心點轉向夾角，通過控制左右履帶輪的速度實現導航運動的轉向.

1.5.2 運動狀態分析 移動平臺采用差速驅動方式，由于左右履帶輪速度VL和VR存在速度差關系，通過控制左右履帶輪的驅動速度，可以實現移動平臺不同的運動狀態，所以移動平臺具有不同的運動狀態.當左右履帶輪分別以VL>VR、VL=VR和VL=-VR的速度運動時，移動平臺可以實現曲線運動、直線運動、旋轉運動，為導航系統提供了轉向運動基礎，移動平臺運動狀態如圖9所示.

圖9 移動平臺運動狀態圖

1.5.3 轉向控制設計 移動平臺轉向的被控對象是所分析的左右履帶輪，為了更一般的描述平臺運動，直觀的控制量是運動學建模中的左右履帶輪速度.轉向控制采用比例積分微分控制(PID)的方式[20]，控制系統主要由控制器、執行機構、被控對象等組成，運動執行機構控制框圖如圖10所示.導航系統控制決策給定速度信息，由左右電機控制實現移動平臺的轉向運動，編碼器反饋實現導航系統的閉環控制，以此來控制跟蹤導航路徑線.

圖10 運動執行機構控制框圖

控制移動平臺由當前點導航到指定目標點，其核心是令移動平臺在控制器作用下，持續地沿規劃路徑朝向導航目標點運動，這樣可實現葡萄園導航至目標點的運動控制過程，移動平臺導航運動示意如圖11所示.

圖11 導航運動示意圖

2 結果與分析

圍繞導航系統進行關鍵技術研究，為驗證導航系統的可行性，開展葡萄園環境下履帶式移動平臺自動導航系統試驗，選取寧夏賀蘭山東麓釀酒葡萄種植基地作為試驗場地，試驗地點環境如圖12所示.

圖12 試驗地點環境圖

葡萄園行間距約為3 m，葡萄藤高約為1.8 m，葡萄園路口寬約5 m.移動平臺初始航向與葡萄種植方向偏差不應大于45°，試驗預先規定移動平臺位于導航線的右側時橫向偏差為正，左側時為負.開展履帶式移動平臺自動導航系統性能試驗，葡萄園導航試驗如圖13所示.

圖13 葡萄園導航試驗

根據葡萄園機具生產作業的速度，試驗選取以0.5 m/s速度和1 m/s速度下的導航效果.記錄導航橫向偏差數據，根據葡萄園導航試驗橫向偏差數據，導航試驗橫向偏差如圖14所示.

A：以0.5 m/s速度下導航橫向偏差圖；B：以1 m/s速度下導航橫向偏差圖；C：以0.5 m/s速度下導航橫向偏差圖；D：以1 m/s速度下導航橫向偏差圖.

導航橫向偏差控制在10 cm范圍內具有一定的可行性，由試驗橫向偏差圖可知，移動平臺分別以0.5 m/s和1 m/s的速度試導航時，導航初始位置與與中心路徑存在一定偏差，但在行駛約10 m后，消除橫向偏差駛入導航中心路徑，并且當移動平臺遇到坑洼地勢偏離預定路徑時，能夠消除橫向偏差駛入導航中心路徑.

分析導航試驗橫向偏差數據，移動平臺分別以0.5 m/s和1 m/s速度導航時，平均橫向偏差分別為3.4 cm和3.6 cm，最大橫向偏差分別為9.1 cm和9.8 cm，均方差分別為4.2 cm和4.2 cm，試驗數據統計如表2所示.

表2 導航系統試驗橫線偏差統計

3 討論與結論

為了更好地實現葡萄園環境下作業的自動導航，解決復雜地形造成導航偏離路徑后能快速修正偏差，以免碰撞籬架等問題.以履帶式移動平臺導航控制為研究對象，研究了一套葡萄園自動導航系統，通過葡萄園導航試驗，認為該自動導航系統能夠較快的修正導航橫向偏差，實現葡萄園內自動導航系統路徑規劃及轉向控制，具有一定的可行性.

分析試驗結果可知，移動平臺分別以0.5 m/s和1 m/s速度導航時，導航平均橫向偏差分別為3.4 cm和3.6 cm，最大橫向偏差分別為9.1 cm和9.8 cm，均方差分別為4.2 cm和4.2 cm.導航橫向偏差控制在合理范圍內，當移動平臺遇到坑洼地形出現較大的位置偏差時，導航控制系統能較為快速、準確地修正橫向偏差，使移動平臺駛入中心路徑.

圍繞自動導航系統進行研究，目前雖取得一定程度上成果，但分析試驗數據可知，導航的最大橫向偏差主要出現在速度較高的情況下.為了確保導航系統在葡萄園環境下具有更好的準確定及穩定性，保證導航路徑與中心線擬合更優，本文后續將對平臺的導航控制算法進行優化研究，提高移動平臺路徑擬合性及運行穩定性，以更好地適應葡萄園自動導航作業需求.