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

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

（1. 江蘇大學(xué)農(nóng)業(yè)工程學(xué)院，鎮(zhèn)江 212013；2. 現(xiàn)代農(nóng)業(yè)裝備與技術(shù)教育部重點實驗室，鎮(zhèn)江 212013）

0 引言

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

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

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

1 棚架環(huán)境自主上線的技術(shù)難點

1.1 棚架果園的場景特征

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

1）枝蔓布置于網(wǎng)狀架面之上，形成類似“天花板”的狀態(tài)。

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

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

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

1.2 棚架環(huán)境下的自主導(dǎo)航特殊性

依據(jù)棚架果園場景特征，自主導(dǎo)航系統(tǒng)要求如下：

1）自主環(huán)境感知，機器人自主感知果園周圍環(huán)境信息，從而確定作業(yè)樹行。

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

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

1.3 棚架環(huán)境下的自主上線的技術(shù)要求

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

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

2 棚架果園快速上線方法

2.1 棚架果園自主導(dǎo)航系統(tǒng)

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

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

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

2.2 棚架果園快速上線技術(shù)構(gòu)成

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

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

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

3）快速上線軌跡規(guī)劃。

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

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

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

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

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

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

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

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

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

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

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

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

根據(jù)式（7）可得參數(shù)al和bl的最優(yōu)估計值為

同理可求得右側(cè)樹行直線方程參數(shù)ar、br的值。

4）樹行中心線即是樹行行間導(dǎo)航線。

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

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

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

2.3.2 位姿檢測

融合激光雷達和電子羅盤航向偏差，使自主上線系統(tǒng)能夠?qū)崟r穩(wěn)定采集航向偏差。由2.3.1節(jié)計算得到機器人補償后相對于導(dǎo)航線的航向偏差θt，如式（11）：

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

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

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

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

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

2.4 雙指標快速上線軌跡規(guī)劃

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

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

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

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

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

2.5 上線系統(tǒng)性能指標

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

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

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

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

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

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

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

2.6 最優(yōu)上線角

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

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

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

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

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

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

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

2.7 快速上線仿真試驗

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

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

表3 行駛速度為0.3 m/s時上線性能仿真結(jié)果 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時穩(wěn)定跟蹤路徑時間在0.8～1.2 s，，總體上，本文棚架葡萄園下快速上線方法能夠快速消除偏差完成上線。

3 棚架果園試驗與結(jié)果分析

3.1 試驗材料與設(shè)備

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

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

3.2 試驗方法

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

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

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

3.3 試驗結(jié)果與分析

3.3.1 果樹行線提取結(jié)果與分析

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

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

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

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

3.3.2 快速上線性能試驗結(jié)果與分析

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

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

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

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

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

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

4 結(jié) 論

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

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

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

3）為了驗證快速上線方法的性能，進行了模擬棚架葡萄園環(huán)境下的快速上線試驗，在初始橫向偏差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。本文提出的最優(yōu)上線角快速上線方法具有較好的上線性能，可為棚架果園無人化作業(yè)技術(shù)研究提供參考。