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

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

0 引言

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

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

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

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

1.1 棚架果園的場(chǎng)景特征

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

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

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

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

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

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

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

1）自主環(huán)境感知，機(jī)器人自主感知果園周?chē)h(huán)境信息，從而確定作業(yè)樹(shù)行。

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

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

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

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

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

2 棚架果園快速上線方法

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

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

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

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

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

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

1）棚架果園中果樹(shù)行線提??；

2）獲取機(jī)器人相對(duì)果園樹(shù)行的精準(zhǔn)位姿；

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

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

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

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

式中ΔH1為極角角度補(bǔ)償值，rad；Pi為電子羅盤(pán)方位角，rad。

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

2.3 航向信息融合的精準(zhǔn)位姿獲取

2.3.1 最小二乘法提取果樹(shù)行線

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

式中ρi為點(diǎn)云極坐標(biāo)極徑，m；為點(diǎn)云極坐標(biāo)極角，rad；X為笛卡爾坐標(biāo)系橫軸，m；Y為笛卡爾坐標(biāo)系縱軸，m。

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

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

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

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

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

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

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

5）為避免因機(jī)器人航向偏差過(guò)大造成干擾樹(shù)行點(diǎn)云錯(cuò)誤剔除，導(dǎo)致果樹(shù)行線錯(cuò)誤識(shí)別的問(wèn)題，提出對(duì)激光雷達(dá)掃描點(diǎn)云極角補(bǔ)償，用補(bǔ)償后的點(diǎn)云擬合果樹(shù)行線。由式（2）得極角角度補(bǔ)償值ΔH1，補(bǔ)償后激光雷達(dá)點(diǎn)云坐標(biāo)為，其中

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

2.3.2 位姿檢測(cè)

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

式中Lj激光雷達(dá)航向角，rad。

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

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

車(chē)體相對(duì)樹(shù)行橫向偏差εF如式（14）所示：

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

2.6 最優(yōu)上線角

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

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

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

表2 初始橫向偏差1.4 m和航向偏差π/4下最優(yōu)上線角 計(jì)算結(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)上線角不隨航向偏差改變而改變。

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

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

2.7 快速上線仿真試驗(yàn)

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

依據(jù)生產(chǎn)機(jī)器人作業(yè)速度和自主導(dǎo)航精度要求，設(shè)定原地轉(zhuǎn)向履帶底盤(pá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é)方法，對(duì)左側(cè)狀態(tài)的機(jī)器人上線距離和時(shí)間進(jìn)行計(jì)算，結(jié)果如表3所示。

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

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

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

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

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

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

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

3.2 試驗(yàn)方法

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

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

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

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

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

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

由圖11可知，同樣的位置偏差時(shí)，航向信息融合的果樹(shù)行線提取算法能夠準(zhǔn)確地?cái)M合出果樹(shù)行線，但標(biāo)準(zhǔn)最小二乘法行線無(wú)法正確提取出果樹(shù)行線。

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

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

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

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

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

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

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

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

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

4 結(jié) 論

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

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

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

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