棚架果園自主導航機器人快速上線方法與試驗

劉繼展，何 蒙，解彬彬，彭 赟，單海勇

（1. 江蘇大學農業工程學院，鎮江 212013；2. 現代農業裝備與技術教育部重點實驗室，鎮江 212013）

0 引言

果園棚架栽培模式具有受光面積大、抗逆性強、單位效益高及田間管理方便等優點，在國內外廣泛應用于鮮食葡萄、獼猴桃等生產[1]。自主導航是果園機器人的核心技術之一[2]，機器人進入果樹行間、途中恢復作業時均需要自主快速行駛至作業行，即自主上線。所以自主上線對于機器人的自主導航能力和性能至關重要。

國內外專家學者對大田中農業機械的自主上線開展了研究，賈全[3]針對RTK-GNSS定位導航提出將最大曲率約束、起止點航向約束和最大轉向角約束綜合考慮的農機運動學模型，使用量子遺傳算法規劃距離最短的可行駛上線軌跡，初始位置到目標點距離6.5 m，上線距離為23.7 m，最大跟蹤誤差為0.10 m。Samson[4]對于機器人運動穩定性問題，提出橫向絕對控制的上線控制方法，穩定減小目標點相對于目標軌跡的橫向距離。陳軍等[5]和Bayar等[6]對曲線路徑跟蹤問題，提出拖拉機遵循正弦曲線路徑和圓形路徑的跟蹤方法，但是并沒考慮輪角決策量范圍的影響。王輝等[7]對拖拉機導航中輪角決策量范圍固定，提出一種預瞄追蹤模型的農機導航路徑跟蹤控制方法，田間試驗結果顯示，初始航向偏差0°和初始位置偏差1.5 m條件下，上線時間為9.4 s、上線距離9.33 m、超調量為8.5 cm。以上研究是基于衛星定位的導航上線研究，是基于導航上線點不動的上線方法。棚架果園環境中濃密樹冠形成一張天然的衛星信號干擾層，棚架中無法使用衛星定位的方式導航。所以基于上線點不動的上線方法不適用于棚架果園。文獻[8-12]提出通過傳感器感知周圍環境并獲得機器人相對樹行的位置和姿態信息，對這種環境的定位方式提出路徑跟蹤方法，其中包括PID控制方法[8-9]、模糊控制方法[10]、純追蹤模型[11-12]等。試驗表明，這些方法在機器人位姿偏差較小時具有較好的路徑跟蹤效果，但應用在田頭上線存在上線時間長、上線距離長的問題[13-14]。所以，棚架果園的機器人自主上線需要解決機器人相對于莖桿纖細、稀疏種植的果樹位姿識別和大位姿偏差下的快速上線方法。

針對上述研究的不足和棚架果園行頭自主上線的實際要求，本文提出一種相對樹行定位導航的快速上線方法。利用激光雷達和電子羅盤航向信息融合得到精準位姿信息觸發相應上線軌跡程序，并以求解的最優上線角度實現快速上線，在搭建的模擬棚架果園環境下進行試驗，試驗驗證該算法的有效性。

1 棚架環境自主上線的技術難點

1.1 棚架果園的場景特征

棚架栽培模式是棚架底部用支撐柱支撐，頂部以橫桿或鉛絲牽引，形成網狀架面，枝蔓布于架上的果樹種植方式。葡萄、梨、獼猴桃等均適合棚架模式種植，本文以棚架葡萄園為對象開展研究。棚架葡萄園的場景特征如下：

1）枝蔓布置于網狀架面之上，形成類似“天花板”的狀態。

2）棚架內為行距、株距3～4 m的纖細葡萄樹干和立柱，樹干與立柱分別單獨成行或間隔排列成行；

3）行間為疏松土壤路面，存在低矮雜草（高度低于40 cm）、溝坎（豎直高度變化小于15 cm）等。

4）葡萄樹莖部直徑在2.13～4.76 cm之間，棚架高度為1.8～2.1 m。

1.2 棚架環境下的自主導航特殊性

依據棚架果園場景特征，自主導航系統要求如下：

1）自主環境感知，機器人自主感知果園周圍環境信息，從而確定作業樹行。

2）自主位姿判定，依據傳感器信息確定機器人在果園中的位置和姿態信息。

棚架果園枝蔓布置與架上濃密樹冠形成的衛星信號遮擋層，使得衛星定位的導航方式不穩定，基于衛星定位的絕對定位方式不適用。機器人要自主感知環境信息并確定位姿信息，棚架中莖桿纖細、稀疏種植的果園環境為機器人自主感知帶來困難。

1.3 棚架環境下的自主上線的技術要求

基于衛星定位方式的上線研究中，機器人初始位姿和上線點的位置均為固定值。在速度恒定、運動控制不變的情況下，影響上線性能因素只有上線軌跡。所以衛星定位的上線研究主要是完成初始點與上線點之間上線軌跡規劃。

棚架果園中無法使用衛星定位，所以棚架環境下的自主上線是獲得機器人位姿后規劃上線軌跡，具體技術包括：首先提取果樹行線；在得到果樹行線后機器人通過控制器處理傳感器數據判斷機器人精準位姿；最后規劃機器人上線軌跡完成上線。棚架果園自主上線過程如圖1所示。

2 棚架果園快速上線方法

2.1 棚架果園自主導航系統

履帶式行走機構對路況具有很強的適應能力，果園環境土壤疏松、路面顛簸不平。本研究以常州桑普履帶式果園管理機（3GPY-300）為機器人行走機構硬件，整機結構如圖2所示。

激光雷達水平安裝于機器人車體前端，距離地面高度540 mm，電子羅盤水平安裝于車體后部，為避免受到電機等磁性物體影響，布置遠離直流電機。在其上安裝自主研發的棚架果園自主導航控制平臺，該平臺由導航控制器、運動控制器、位姿感知模塊和人機交互模塊構成，如圖3所示。

其中，位姿感知模塊由激光雷達和電子羅盤組成。激光雷達采用德國SICK公司生產的LMS111，測距量程20 m，角度掃描范圍270°，角度分辨率0.25°，每組掃描點云數據為541個測距值，最大掃描頻率50 Hz，電子羅盤選擇慧聯公司生產的SEC335傾角補償型三維電子羅盤，航向精度為1°，分辨率為0.3°，俯仰和橫滾精度為0.15°，分辨率為0.01°，最大采樣頻率50 Hz。根據處理器運算性能差異，以英偉達Jetson TX2開發板作為導航控制器；STM32F103RCT6為運動控制器。導航控制器負責對獲取的棚架環境信息數據進行處理，計算出導航路徑，并將電機速度信息通過USB串口通信發送至運動控制器。運動控制器負責接收解析上位機發送的信息并輸出PWM調制脈沖分別驅動左右驅動器，驅動器控制直流電機轉動的速度和轉角。

2.2 棚架果園快速上線技術構成

本文提出電子羅盤與激光雷達航向信息融合的位姿檢測和履帶機器人快速上線控制方法，技術流程如下：

1）棚架果園中果樹行線提取；

2）獲取機器人相對果園樹行的精準位姿；

3）快速上線軌跡規劃。

快速上線控制方法流程圖如圖4所示。

如圖4所示，棚架果園果樹種植朝向與農藝密切相關，棚架種植朝向Di不隨時間的變化而變化，通過人機交互界面設定棚架方向后便可長久使用，但局部果樹樹行朝向與種植朝向Di會有部分偏差。激光雷達在局部果樹行線識別上具有出色性能，所以融合兩者航向信息實現兩傳感器性能互補。

初始電子羅盤的航向角Pj（rad）與棚架種植朝向Di（rad）一致。方位角Pi與航向角Pj差值即此時電子羅盤獲得的航向偏差，電子羅盤航向偏差作為極角補償對激光雷達點云補償，具體如式（1）～（2）所示

式中ΔH1為極角角度補償值，rad；Pi為電子羅盤方位角，rad。

補償后的樹行點云數據對電子羅盤航向進行修正，修正后的電子羅盤航向角與激光雷達航向角融合得到精確位姿，如圖5所示。融合后橫向偏差和航向偏差為上線判定的2個指標，設計2個指標的不同閾值進行車體位姿狀態歸類，機器人根據檢測位姿判定所屬狀態并觸發相應上線軌跡程序。若未完成，將再次進行位姿檢測，重新檢測機器人的姿態，直至完成上線。

2.3 航向信息融合的精準位姿獲取

2.3.1 最小二乘法提取果樹行線

棚架果園為半結構化設施農業環境，地面沒有高莖稈雜草且激光雷達安裝位置高于低矮雜草，雜草對獲取樹干點云信息無干擾。因此，本文假設獲取的樹干點云信息無干擾點，可直接用于果樹行線擬合。行線擬合方法如下：

式中ρi為點云極坐標極徑，m；為點云極坐標極角，rad；X為笛卡爾坐標系橫軸，m；Y為笛卡爾坐標系縱軸，m。

2）X軸正方向與果園導航航向一致，然后以象限區分左右側樹行，通過設置Y軸距離閾值剔除干擾樹行并得到左右側樹行點云，縱軸閾值分割獲得左右樹干點云坐標Sli(Xli，Yli)，Sri（Xri，Yri），閾值選擇如式（5）和式（6）所示：

式中ΔY為縱軸閾值，m。

3）果樹行點云具有較好的線性關系，設左側果樹行直線方程為Yl=alXl+bl，右側果樹行直線方程為Yr=arXr+br，根據最小二乘法的基本原理，要使誤差平方取最小值，對參數al和bl分別求偏導：

根據式（7）可得參數al和bl的最優估計值為

同理可求得右側樹行直線方程參數ar、br的值。

4）樹行中心線即是樹行行間導航線。

機器人與導航路徑之間位姿關系通過橫向偏差εt和航向偏差θt表示，其計算公式如下：

5）為避免因機器人航向偏差過大造成干擾樹行點云錯誤剔除，導致果樹行線錯誤識別的問題，提出對激光雷達掃描點云極角補償，用補償后的點云擬合果樹行線。由式（2）得極角角度補償值ΔH1，補償后激光雷達點云坐標為，其中

按照步驟1）～4）得到極角補償后的位姿，極角補償過程如圖6所示。

2.3.2 位姿檢測

融合激光雷達和電子羅盤航向偏差，使自主上線系統能夠實時穩定采集航向偏差。由2.3.1節計算得到機器人補償后相對于導航線的航向偏差θt，如式（11）：

式中Lj激光雷達航向角，rad。

自主導航需要根據硬件的精度和導航精度設定一個可容納誤差Δθ。若航向偏差θt大于可容納誤差Δθ，則需要對電子羅盤航向角進行修正，以獲得精確電子羅盤航向角jPΛ，如式（12）所示：

組合信息融合航向偏差θF如式（13）所示：

車體相對樹行橫向偏差εF如式（14）所示：

獲得準確橫向偏差和航向偏差后，控制器根據2個指標的閾值進行車體位姿狀態歸類，機器人根據檢測位姿判定所屬狀態并觸發相應上線軌跡程序。

2.4 雙指標快速上線軌跡規劃

為了實現自主導航機器人在樹行頭快速到達導航線附近，以機器人位姿的橫向偏差和航向偏差作為航位偏差檢測指標，先消除橫向偏差再消除航向偏差，解決以往橫向偏差和航向偏差同步消除帶來的上線距離長的問題。按照生產作業所需上線距離盡可能較短，將機器位姿調整至滿足導航線可容納橫向偏差（-ε0，+ε0）、可容納航向偏差（-θ0，+θ0）范圍內。

機器人航向偏差閾值為（-π/4，π/4）：當θt>π/4或θt<-π/4時，機器人相對于樹行已經嚴重傾斜，在實際機器人自主導航時并不會出現如此大的航向偏差。所以。果樹樹行種植行距為3 m，在任何情況下橫向偏差均在-1.5和1.5之間，超出此范圍說明擬合錯誤樹行，導航處于失效的狀態。所以橫向偏差閾值為（-1.5，1.5）。

如表1所示，是基于橫向偏差和航向偏差2個指標的位姿狀態分類，在導航可行域范圍內，將機器人位姿分為9種狀態，機器人在航線左側橫向偏差為正。機器人左偏航向偏差為正，A2B2為完成上線位姿狀態（機器人與導航線的橫向偏差|εF|<|ε0|且航向偏差|θF|<|θ0|時，稱機器人完成上線行間導航狀態），同理A2B2為機器人位于導航線右側且航向為左偏即右側左偏，其他狀態以此類推。處于其他狀態時即啟動對應的調控程序向理想的位姿狀態調整[15-16]。

表1 基于橫向偏差和航向偏差的機器人位姿狀態分類 Table 1 Robot pose state classification based on lateral deviation and heading deviation

機器相對導航線左右側狀態調整方法一致、路徑對稱，針對左側5種狀態的機器人展開詳細敘述。A3B1、A3B2、A3B3狀態下，機器人車體中心位于導航線左側，橫向偏差εF>0，車體處于較安全的狀態，其中A3B1狀態下車頭偏向導航線外側，控制車體原地調整航向至A3B3狀態，沿著最優上線角行駛至A2B3狀態，控制車體原地調整航向至A2B2狀態。A3B2、A3B3狀態下履帶車橫向偏離導航線，先將車身調整至與導航線成最優上線角的A2B3狀態，再按照A2B3狀態進行調節。控制履帶車快速調整到上線的行間導航各種狀態，如圖7所示。

2.5 上線系統性能指標

根據最優控制理論，性能指標的內容和形式取決于最終的目標[17-19]。由于機器人相對導航線左右側狀態調整方法一致、路徑對稱，因此本文針對左側狀態的機器人展開詳細敘述。選擇上線時間t0和上線距離s0為衡量上線性能的指標，其中上線距離s0為

由式（15）可知，快速上線方法的上線距離與橫向偏差、可容納橫向偏差和上線角度θs有關，但在機器人位姿確定后橫向偏差和可容納橫向偏差均為定值，上線距離的長短只與上線角度有關。

上線時間與機器人運動部件的運動模型相關，本文研究采用履帶底盤，是一種典型的差速轉向車輛，通過調節左右側主動輪的轉速來控制航向和速度。本研究將履帶車體視為剛體，其任意時刻的瞬時運動均可認為是繞某一瞬時轉動中心的轉動，由瞬心法[20-23]機器人運動學模型如式（16）和（17）所示：

式中vl為左側履帶速度，m/s；vr為右側履帶速度，m/s；B為履帶軌距，m；v為車體速度，m/s；ω為車體角速度，rad/s。

不同航向偏差的上線時間如式（18）所示：

將式（16）～（17）代入式（18）得：

由式（19）可知，機器人駛進果園后初始橫向偏差、初始航向偏差、履帶軌距和可容納橫向偏差等參數均為定值，機器人在A2B3狀態下時，上線時間為固定值，A3B1、A3B2、A3B3狀態下，快速上線方法的上線時間僅與上線角度有關，所以耗時最短時的上線角度尤為重要，即最優上線角。

2.6 最優上線角

由上文分析可知，上線角度直接決定上線時間的長短，依據機器人作業速度和自主導航精度要求，本文在機器人履帶軌距、車體速度、角速度、可容納位姿偏差均恒定為0.8 m、0.3 m/s、0.75 rad/s、0.1 m的條件下研究機器人初始位姿與最優上線角度關系。根據式（19），以上線角為自變量，上線時間為因變量，使用MATLAB工具箱生成機器人在εF=1.4 m、θF=π/4位姿下上線時間與上線角的變化趨勢，如圖8所示。

由圖8可知，按照0.78 rad的上線角上線時，上線時間最短為7.81 s。

為了得到其他位姿的最優上線角，分別求解不同初始橫向偏差和航向偏差下的最優上線角。根據文獻[6]，選取橫向偏差1.4、1.1、0.8、0.5、0.2 m，航向偏差-π/4、-π/6、0、π/6、π/4，開展試驗，求解εF=1.4 m時不同航向偏差下最優上線角和θF=π/4時不同橫向偏差下最優上線角，結果如表2所示。

表2 初始橫向偏差1.4 m和航向偏差π/4下最優上線角 計算結果 Table 2 Calculated results of optimal online angle under initial lateral deviation 1.4 m and course deviation π/4

由表2可知，在初始航向偏差不變下，最優上線角隨橫向偏差增大而增大。在初始橫向偏差不變下，最優上線角不隨航向偏差改變而改變。

綜上所述，只有初始橫向偏差對最優上線角有影響，所以最優上線時間僅與初始橫向偏差有關。為了得到各位姿下最優上線角度值，在MATLAB中對不同橫向偏差下最優上線角數據擬合，得到最優上線角與橫向偏差函數關系，如圖9所示。

擬合結果的R2=0.999 1，RMSE=0.006 3 rad，方程對測量值擬合程度較高。

2.7 快速上線仿真試驗

為了驗證快速上線方法具體性能，在Matlab仿真環境下進行仿真試驗[24-25]。

依據生產機器人作業速度和自主導航精度要求，設定原地轉向履帶底盤左右輪速度分別為vl=0.3 m/s、vr=0.3 m/s，履帶軌距B=0.8 m，可容納橫向偏差ε0=0.1 m，可容納航向偏差θ0=3°。根據2.5節方法，對左側狀態的機器人上線距離和時間進行計算，結果如表3所示。

表3 行駛速度為0.3 m/s時上線性能仿真結果 Table 3 Online performance simulation results when the driving speed is 0.3 m/s

由表3可知，在航向偏差固定情況下，隨著橫向偏差增大，上線距離增加；在橫向偏差不變的情況下，航向偏差變化，上線距離不變，上線距離最大值為1.3 m。

在航向偏差固定情況下，隨著橫向偏差增大，上線時間增加；在橫向偏差固定情況下，隨著航向偏差變大，上線時間變大；上線時間最大值為7.81 s。文獻[26]使用快速糾偏方法，在初始航向偏差5°、橫向偏差0.01 m、速度0.3 m/s時穩定跟蹤路徑時間在0.8～1.2 s，，總體上，本文棚架葡萄園下快速上線方法能夠快速消除偏差完成上線。

3 棚架果園試驗與結果分析

3.1 試驗材料與設備

為了驗證快速上線方法和檢測上線性能，按照本文方法研制了棚架果園自主導航機器人試驗平臺，如圖10a所示。

由于新冠疫情的影響，試驗在江蘇大學農業工程學院內空曠的草地進行，依據棚架葡萄園場景特征，搭建帳篷支架模擬棚架葡萄園環境，4組3 m×6 m規格帳篷支架按照行距3 m、株距3 m的標準布置，形成長度15 m寬度9 m的試驗場地，如圖10b所示。以機器人中心投影到地面的點為定位點，通過在機器人中心安裝滴水掛瓶記錄下機器人定位點，測量儀器選擇量程200 m、精度1 mm皮尺[25]。

3.2 試驗方法

將機器人遙控至樹行行頭，通過人機交互界面輸入棚架行朝向Di并啟動機器人上線作業，系統初始化控制器獲取傳感器數據：首先提取棚架中果樹行線，然后融合航向信息獲取機器人相對果園樹行的精準位姿，最后規劃機器人快速上線軌跡并完成上線。依據本文方法的技術構成，試驗分為果樹行線提取和快速上線性能驗證2個部分：

果樹行線提取試驗：為了判斷航向信息融合后的精準位姿對果樹行線提取準確性的影響，采用標準最小二乘法行線提取算法設置對照組。依據機器人在棚架下位姿偏差可行域將試驗分成5組，橫向偏差1.4 m，分別在航向偏差-π/4，-π/18，0，π/18，π/4下進行果樹樹行提取精度試驗。每組進行20次，通過運行ROS中行線擬合節點并在RVIZ中顯示提取結果，判斷人工分析算法提取的果樹航線是否與真實果樹行線重合，若提取的果樹行線與實際作業行樹行重合，即果樹行線提取成功，否則果樹行線提取失敗。

快速上線性能試驗：為檢驗機器人在位姿偏差下快速上線性能，橫向偏差設為最大值1.4 m，航向偏差為-π/4，-π/18，0，π/18，π/4，共5組，每組做3次試驗，統計每組試驗條件下快速上線的距離和時間。

3.3 試驗結果與分析

3.3.1 果樹行線提取結果與分析

試驗結果如圖11，從左至右分別是機器人在橫向偏差1.4 m、航向偏差π/4的大位姿偏差下，航向信息融合的果樹行線提取圖像和標準最小二乘法所得行線提取圖像。

由圖11可知，同樣的位置偏差時，航向信息融合的果樹行線提取算法能夠準確地擬合出果樹行線，但標準最小二乘法行線無法正確提取出果樹行線。

為了得到2種行線提取算法行線識別平均精度，分別對試驗數據進行統計，如表4所示，融合航向的果樹行線檢測算法識別平均精度達到94%，比標準最小二乘法行線提取算法平均精度提升40個百分點。機器人相對果樹行線確定位姿，果樹行線平均識別精度的提高使得機器人獲得更精準的位姿。精準位姿是實現快速上線的前提，所以航向信息融合的果樹行線提取為棚架果園下機器人快速上線奠定基礎。

表4 不同行線提取算法性能對比（εF=1.4 m） Table 4 Performance comparison of different row extraction algorithms（εF=1.4 m）

3.3.2 快速上線性能試驗結果與分析

上線性能包括上線時間和上線距離，上線性能試驗結果如表5所示。

從表5中可以看出，在初始橫向偏差1.4 m下，平均上線時間在6.11～8.9 s之間、平均上線距離在1.357～1.403 m之間；在初始航向偏差π/4下，上線時間的試驗結果比理論結果多1.09 s、上線距離的試驗結果比理論結果大0.103 m。由于路面顛簸不平、運動執行滯后等原因，試驗效果稍差于理論結果。

表5 不同初始航向偏差上線性能試驗結果（εF=1.4 m） Table 5 Online performance test results with different initial heading deviations (εF=1.4 m)

從總體上來看，模擬棚架葡萄園上線平均時間隨著初始航向偏差增大而增加，上線距離隨初始航向偏差變化不大，上線時間和上線距離總體變化趨勢與理論結果一致。

初始橫向偏差1.4 m、航向偏差π/4的快速上線過程如圖12所示。

結合圖7，機器人在初始橫向偏差為1.4 m、航向偏差為π/4時，機器人處于A3B1狀態，機器人快速上線狀態變化過程為A3B1→A3B3→A2B3→A2B2。圖12中0～2.3 s機器人原地旋轉將車體航向角調整至-π/4即A3B3狀態，此時機器人橫向偏差不變航向偏差轉至-π/4左右。2.3～6.86 s橫向偏差線性減小至可容納橫向偏差ε0以內，機器人航向偏差震蕩調整、橫向偏差逐漸減小直至機器人處于狀態A2B3。6.86～8.91 s階段橫向偏差在ε0附近震蕩、航向偏差逐漸減小至可容納航向偏差θ0附近，此時機器人處于A2B2狀態，即機器人完成上線。試驗結果表明機器人按照快速上線方法完成上線，但試驗效果稍差于仿真結果，上線時間誤差不超過1.42 s，上線距離誤差不超過0.16 m，相比于傳統路徑跟蹤的上線方法，提高了棚架果園自主導航系統的上線性能，上線時間減少，上線距離縮短，誤差在許用范圍內[27-29]。

4 結 論

針對目前果園田頭對行上線存在上線時間長、上線距離長問題，提出了一種基于電子羅盤與激光雷達航向信息融合的位姿檢測方法，通過在模擬棚架葡萄園環境下的試驗，證明該方法可以在大初始橫向偏差和航向偏差條件下，快速穩定獲得機器人位姿并按照規劃路徑上線，相比于傳統路徑跟蹤的上線方法，提高了棚架果園自主導航系統的上線性能，其中包括減少上線時間和縮短上線距離。

1）本文從理論上分析了機器人依照最優上線角的快速上線方法在位姿可行域范圍內的上線時間和上線距離。在初始橫向偏差為0～1.4 m、初始航向偏差-π/4～π/4范圍內，在航向偏差一定的情況下，上線時間隨著橫向偏差的增大而增加；在橫向偏差一定的情況下，隨著航向偏差增大，上線時間也隨之增加；上線時間最大值為7.81 s，上線距離最大值為1.3 m。總體上，快速上線方法能夠在棚架葡萄園環境下快速消除位姿偏差完成上線。

2）初始橫向偏差1.4 m、航向偏差π/4的快速上線試驗，結果表明機器人能夠按照快速上線方法完成上線。

3）為了驗證快速上線方法的性能，進行了模擬棚架葡萄園環境下的快速上線試驗，在初始橫向偏差1.4 m的條件下，初始航向偏差-π/4，-π/18，0，π/18，π/4條件下，上線時間分別為6.11、7.15、7.46、7.74、8.9 s，上線距離分別為1.357、1.367、1.387、1.383、1.403 m。本文提出的最優上線角快速上線方法具有較好的上線性能，可為棚架果園無人化作業技術研究提供參考。