基于改進(jìn)A*算法的果園移動(dòng)機(jī)器人建圖定位與路徑規(guī)劃方法與試驗(yàn)*

高鵬，姜軍生，白陽(yáng)，宋健

(1.山東科技大學(xué)機(jī)械電子工程學(xué)院，山東青島，266590；2.濰坊學(xué)院機(jī)電與車(chē)輛工程學(xué)院，山東濰坊，261061)

0 引言

水果產(chǎn)業(yè)在整個(gè)農(nóng)業(yè)經(jīng)濟(jì)中占有很大比例，隨著城鄉(xiāng)居民生活水平的提升，人們對(duì)水果的消費(fèi)需求日益增加，但是果園自動(dòng)化程度較低與勞動(dòng)力資源缺乏制約了水果產(chǎn)業(yè)的進(jìn)一步發(fā)展，所以將智能機(jī)器人引入果園代替人工作業(yè)顯得尤為重要[1-4]。果農(nóng)們需要定期在果園內(nèi)進(jìn)行除草、修枝、噴藥、施肥等工作，將機(jī)器人技術(shù)應(yīng)用在果園環(huán)境中，可以把果農(nóng)們從這些高溫、有毒等危險(xiǎn)的任務(wù)中解放出來(lái)，而果園機(jī)器人實(shí)現(xiàn)智能化的前提是可以在沒(méi)有人類(lèi)干預(yù)的情況下精確感知和自主規(guī)劃決策到達(dá)目標(biāo)地點(diǎn)，隨后執(zhí)行下一步特定的果園工作。

自主導(dǎo)航技術(shù)在城市、道路等場(chǎng)景中已廣泛應(yīng)用，但在農(nóng)業(yè)領(lǐng)域發(fā)展緩慢，其原因在于果園環(huán)境相對(duì)復(fù)雜，大量不規(guī)則的樹(shù)冠和雜亂的枝葉增加了機(jī)器人感知和定位的難度。隨著近年來(lái)增強(qiáng)現(xiàn)實(shí)、無(wú)人駕駛技術(shù)的興起，SLAM技術(shù)也越來(lái)越受到關(guān)注。相比于相機(jī)，激光雷達(dá)將自身作為光源感知周?chē)h(huán)境能夠?qū)崟r(shí)獲取三維點(diǎn)云數(shù)據(jù)，感知范圍大，在黑暗環(huán)境中工作更有優(yōu)勢(shì)，克服了相機(jī)光學(xué)錯(cuò)覺(jué)和測(cè)距不足的缺陷。主流的三維激光SLAM算法主要包括LOAM、A-LOAM、LeGO-LOAM等[5-6]，這些算法未針對(duì)果園環(huán)境進(jìn)行特定優(yōu)化，所以在果園復(fù)雜環(huán)境中容易出現(xiàn)地圖漂移、特征點(diǎn)提取困難、噪聲點(diǎn)過(guò)多等問(wèn)題。除了建立地圖和準(zhǔn)確定位外，機(jī)器人控制器還要快速規(guī)劃出沒(méi)有障礙物阻擋的可行路線(xiàn)，通過(guò)路徑生成及優(yōu)化算法，最終得到實(shí)際機(jī)器人可以運(yùn)行的、符合動(dòng)力學(xué)約束的、花費(fèi)代價(jià)最小的光滑路線(xiàn)。常用的全局規(guī)劃有Dijkstra、A*、JPS、PRM、RRT等算法[7-14]。除此之外，機(jī)器人還需具備主動(dòng)避障能力，這需要實(shí)時(shí)局部規(guī)劃算法來(lái)實(shí)現(xiàn)，如Trajectory Rollous、DWA、TEB等，上述全局規(guī)劃算法在果園復(fù)雜環(huán)境中容易找不到最優(yōu)路徑，本文對(duì)A*算法進(jìn)行了改進(jìn)，解決了原始算法得不到最短路徑的問(wèn)題。

本文提出一種果園自主導(dǎo)航系統(tǒng)，采用基于圖優(yōu)化的緊耦合LiDAR-IMU里程計(jì)算法實(shí)現(xiàn)機(jī)器人建圖與定位功能，對(duì)數(shù)據(jù)進(jìn)行降維處理，融合廣度優(yōu)先搜索和改進(jìn)啟發(fā)式函數(shù)的A*算法完成路徑規(guī)劃并進(jìn)行跟蹤控制，應(yīng)用到實(shí)際履帶式移動(dòng)機(jī)器人上進(jìn)行自主導(dǎo)航效果測(cè)試。

1 機(jī)器人平臺(tái)

果園移動(dòng)機(jī)器人由車(chē)載硬件系統(tǒng)和程序控制系統(tǒng)兩部分組成，如圖1所示，機(jī)器人整體尺寸(長(zhǎng)×寬×高)為1 020 mm×750 mm×405 mm，為了使移動(dòng)機(jī)器人在果園環(huán)境內(nèi)有更好的地形適應(yīng)能力及軌跡跟蹤能力，移動(dòng)機(jī)器人底盤(pán)部分采用履帶傳動(dòng)機(jī)構(gòu)，可原地轉(zhuǎn)彎，采用克里斯蒂和瑪?shù)贍栠_(dá)四輪組合平衡懸掛系統(tǒng)，左右各裝有獨(dú)立油壓阻尼避震器，具有很強(qiáng)的吸震承載能力，可負(fù)載100 kg重物爬越30°的斜坡。配備2 kW 大容量鋰電池和高精度伺服電機(jī)，可在6.5 km/h的運(yùn)行速度下連續(xù)作業(yè)30 min。機(jī)器人頂部裝有16線(xiàn)機(jī)械式激光雷達(dá)，掃描頻率為10 Hz，最大量程150 m，測(cè)量精度小于2 cm，水平角度分辨率0.18°。慣性測(cè)量單元(IMU)為基于MEMS慣性傳感技術(shù)的三加速度計(jì)，可輸出加速度、角速度、歐拉角位姿信息，在振動(dòng)和磁場(chǎng)扭曲狀態(tài)下仍然具有高精確度跟蹤性能。機(jī)器人還搭載了高精度定位測(cè)向系統(tǒng)，使機(jī)器人可在遮擋嚴(yán)重及高頻動(dòng)態(tài)環(huán)境下定位與測(cè)向得到快速準(zhǔn)確響應(yīng)，并且融合絕對(duì)位姿信息補(bǔ)償IMU模塊帶來(lái)的累計(jì)誤差，輔助記錄機(jī)器人位置信息，用于對(duì)比驗(yàn)證建圖與定位效果。

圖1 果園機(jī)器人硬件平臺(tái)

機(jī)器人控制器放置在內(nèi)倉(cāng)中，控制器為NUC10i7FNK微型計(jì)算機(jī)，由Intel core i7-10710U CPU和16GB DDR4 RAM組成，安裝了Ubuntu16.04LTS操作系統(tǒng)與ROS kinetic開(kāi)源機(jī)器人開(kāi)發(fā)框架，使用C++編程語(yǔ)言，構(gòu)建開(kāi)機(jī)自啟動(dòng)程序，運(yùn)行建圖定位和軌跡規(guī)劃算法及相關(guān)功能包，通過(guò)CAN通信協(xié)議與電機(jī)驅(qū)動(dòng)器通訊下達(dá)速度指令完成軌跡跟蹤，機(jī)器人程序控制系統(tǒng)整體流程圖如圖2所示。

圖2 程序控制系統(tǒng)整體流程圖

2 果園機(jī)器人建圖與定位算法

2.1 激光里程計(jì)

2.1.1 點(diǎn)云畸變補(bǔ)償

由于IMU誤差隨時(shí)間的積累，需要與其他傳感器融合，從而不斷相互矯正輸出位姿，由于機(jī)器人不斷運(yùn)動(dòng)和激光雷達(dá)本身機(jī)械結(jié)構(gòu)的影響，還需要進(jìn)行幀間點(diǎn)云畸變補(bǔ)償，如圖3所示。首先要計(jì)算出該幀每個(gè)激光點(diǎn)相對(duì)于坐標(biāo)原點(diǎn)的坐標(biāo)變化，然后將其作為補(bǔ)償量對(duì)激光點(diǎn)坐標(biāo)進(jìn)行補(bǔ)償，計(jì)算公式如式(1)所示。

圖3 果樹(shù)點(diǎn)云畸變

式中：T0——起始時(shí)刻雷達(dá)位姿；

Ti——第i個(gè)激光點(diǎn)采集時(shí)雷達(dá)的位姿；

Pi——第i個(gè)激光點(diǎn)坐標(biāo)；

2.1.2 線(xiàn)面特征提取

對(duì)點(diǎn)云進(jìn)行線(xiàn)面特征提取匹配策略，計(jì)算激光點(diǎn)相對(duì)于水平面的傾角，按照線(xiàn)數(shù)對(duì)激光束分割，根據(jù)雷達(dá)到激光線(xiàn)上前后5個(gè)點(diǎn)與當(dāng)前點(diǎn)的距離來(lái)計(jì)算曲率，按照曲率大小篩選角點(diǎn)與平面上的點(diǎn)，考慮到不同距離下激光點(diǎn)的分布疏密不同，進(jìn)行歸一化處理。

(2)

式中：c——曲率；

X——當(dāng)前激光點(diǎn)坐標(biāo)；

Xi——第i個(gè)點(diǎn)坐標(biāo)。

根據(jù)線(xiàn)和面的不重合度反推出雷達(dá)位姿變化[15-16]，由于當(dāng)前真實(shí)位姿未知，假設(shè)機(jī)器人上一時(shí)刻到當(dāng)前時(shí)刻為勻速運(yùn)動(dòng)，根據(jù)兩幀位置計(jì)算出機(jī)器人速度、角速度，可得到當(dāng)前機(jī)器人的猜測(cè)位姿，利用猜測(cè)位姿將當(dāng)前幀點(diǎn)云轉(zhuǎn)換到上一幀點(diǎn)云坐標(biāo)系下，點(diǎn)云轉(zhuǎn)換方程如式(3)所示。

(3)

pi——第k幀的點(diǎn)；

U——旋轉(zhuǎn)矩陣；

t——平移量。

(4)

(5)

式中：dε——當(dāng)前幀激光點(diǎn)到上一幀直線(xiàn)的距離；

dΗ——當(dāng)前幀激光點(diǎn)到平面的距離；

pa、pb——上下相鄰激光線(xiàn)上的激光點(diǎn)坐標(biāo)；

pm、pl、pj——同一平面上按順時(shí)針排序的激光點(diǎn)坐標(biāo)。

2.2 GNSS數(shù)據(jù)處理

高精度GNSS信息可提供自身絕對(duì)坐標(biāo)來(lái)輔助移動(dòng)機(jī)器人完成準(zhǔn)確建圖和定位初始化操作，在點(diǎn)云地圖中加入GNSS的6DOF姿態(tài)作為索引，使機(jī)器人在先驗(yàn)地圖的初始位置能夠計(jì)算先驗(yàn)位姿與當(dāng)前位置的距離完成當(dāng)前時(shí)刻定位，由于定位信息坐標(biāo)系不同，需要將GNSS與機(jī)器人坐標(biāo)系進(jìn)行統(tǒng)一，將GNSS系統(tǒng)提供的經(jīng)緯度坐標(biāo)轉(zhuǎn)化為機(jī)器人載體本身的笛卡爾坐標(biāo)系下的坐標(biāo)，轉(zhuǎn)換坐標(biāo)如式(6)～式(8)所示。

x=(R+H)cosαcosβ

(6)

y=(R+H)cosαsinβ

(7)

(8)

式中：α，β——經(jīng)緯度信息；

H——海拔高度；

R——地球半徑；

a，b——赤道半徑和極地半徑。

轉(zhuǎn)換得到的坐標(biāo)與機(jī)器人初始姿態(tài)仿射對(duì)齊到先驗(yàn)地圖中完成重定位。

2.3 基于圖優(yōu)化的融合定位方案

基于濾波的融合方案包含了馬爾可夫假設(shè)，設(shè)定過(guò)去和未來(lái)數(shù)據(jù)都是獨(dú)立的，當(dāng)前時(shí)刻狀態(tài)只與上一時(shí)刻狀態(tài)有關(guān)，不受其他時(shí)刻影響，導(dǎo)致先前時(shí)刻的誤差會(huì)累積到當(dāng)前時(shí)刻，而基于優(yōu)化的融合方案，采用scan to map的方式，將之前所有時(shí)刻的狀態(tài)以關(guān)鍵幀的形式保存下來(lái)[17]，不斷利用觀測(cè)數(shù)據(jù)和回環(huán)迭代修正累積誤差，可以消除整個(gè)軌跡的誤差，構(gòu)建因子圖，將各個(gè)傳感器的測(cè)量值作為因子加入到系統(tǒng)中。為保證實(shí)時(shí)性，引入滑動(dòng)窗口和邊緣化方法，不斷往滑動(dòng)窗口內(nèi)添加新的幀，通過(guò)舒爾補(bǔ)模型把先前的關(guān)鍵幀約束保留下來(lái)，并進(jìn)行Hessian矩陣稀疏化處理，以進(jìn)一步提高系統(tǒng)實(shí)時(shí)性。

3 果園機(jī)器人路徑規(guī)劃算法

3.1 加入啟發(fā)式函數(shù)權(quán)重的A*算法

A*算法是在Dijkstra算法的基礎(chǔ)上增加了貪心算法，不再像Dijkstra那樣毫無(wú)目的地搜索，使得路徑搜索更具有導(dǎo)向性，更加高效地找到目標(biāo)節(jié)點(diǎn)，兩者搜索方法都是基于廣度優(yōu)先搜索，BFS逐層輻射式擴(kuò)展節(jié)點(diǎn)，構(gòu)建搜索樹(shù)，將擴(kuò)展與未擴(kuò)展節(jié)點(diǎn)分別放入open list與close list隊(duì)列容器中，當(dāng)目標(biāo)節(jié)點(diǎn)被發(fā)現(xiàn)時(shí)，停止擴(kuò)展新的節(jié)點(diǎn)，將最終節(jié)點(diǎn)作為子節(jié)點(diǎn)不斷迭代找出所有的父節(jié)點(diǎn)，從而回溯出最短路徑[18-20]。但單純的A*算法有時(shí)會(huì)盲目?jī)A向目標(biāo)節(jié)點(diǎn)導(dǎo)致路徑距離變長(zhǎng)，為解決此問(wèn)題，改進(jìn)后的A*節(jié)點(diǎn)擴(kuò)展規(guī)則f(n)公式表達(dá)如式(9)所示。

f(n)=g(n)+εh(n)

(9)

式中：g(n)——從起點(diǎn)到節(jié)點(diǎn)n的累積代價(jià)之和；

h(n)——當(dāng)前節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)所需花費(fèi)代價(jià)的估計(jì)值；

D——起始點(diǎn)到終點(diǎn)的距離；

d——當(dāng)前點(diǎn)到終點(diǎn)的距離。

權(quán)重系數(shù)ε與機(jī)器人的工作環(huán)境和目標(biāo)點(diǎn)位置有關(guān)，根據(jù)目標(biāo)點(diǎn)不同距離不斷調(diào)整ε值，因此ε并不是固定值。當(dāng)ε=0時(shí)，A*算法完全退化為Dijkstra算法，生成的路徑一定為最優(yōu)路徑。在犧牲一定效率的情況下，權(quán)衡搜索速度與路徑距離找到最優(yōu)ε值，減小啟發(fā)式函數(shù)在評(píng)價(jià)函數(shù)內(nèi)的比重進(jìn)而減小算法貪婪程度，避免算法過(guò)于貪婪，結(jié)合了Dijkstra的路徑最優(yōu)性和貪心算法的導(dǎo)向性，解決了生成路徑不是最優(yōu)的問(wèn)題，如圖4所示。

(a)當(dāng)ε=1時(shí)計(jì)算出的路徑

3.2 動(dòng)態(tài)窗口

本研究采用動(dòng)態(tài)窗口算法完成機(jī)器人局部路徑規(guī)劃，在全局地圖上使用A*算法進(jìn)行全局規(guī)劃后，機(jī)器人接收到速度指令開(kāi)始運(yùn)動(dòng)，將局部規(guī)劃問(wèn)題用數(shù)學(xué)描述為帶約束的優(yōu)化問(wèn)題，其中約束主要包括差速模型的非完整約束，機(jī)器人自身的動(dòng)力學(xué)約束以及障礙物約束等，簡(jiǎn)化履帶式移動(dòng)機(jī)器人為差分輪式底盤(pán)數(shù)學(xué)模型。

為保證機(jī)器人在安全區(qū)域內(nèi)行駛，需要滿(mǎn)足在撞到障礙物前以最大加速度將當(dāng)前速度vn與當(dāng)前角速度ωn減為0，則下一時(shí)刻動(dòng)態(tài)窗口VD滿(mǎn)足式(17)。

(17)

根據(jù)線(xiàn)速度和角速度采樣點(diǎn)數(shù)對(duì)連續(xù)的速度矢量空間進(jìn)行離散化處理，得到離散采樣點(diǎn)，通過(guò)采樣點(diǎn)預(yù)測(cè)生成機(jī)器人下一時(shí)刻的軌跡，如圖5所示，引入評(píng)價(jià)函數(shù)對(duì)預(yù)測(cè)軌跡進(jìn)行評(píng)分，選取最靠近全局路徑的軌跡。

圖5 速度離散采樣示意圖

4 果園機(jī)器人自主導(dǎo)航性能測(cè)試試驗(yàn)

4.1 果園環(huán)境建圖試驗(yàn)

建立準(zhǔn)確且特征明顯的三維點(diǎn)云地圖是機(jī)器人完成定位與導(dǎo)航等后續(xù)工作的關(guān)鍵，為驗(yàn)證本研究建圖方法的可靠性，在真實(shí)果園環(huán)境中采集激光雷達(dá)原始點(diǎn)云數(shù)據(jù)、IMU數(shù)據(jù)。試驗(yàn)區(qū)長(zhǎng)30 m，寬25 m，以0.5 m/s 的速度在果樹(shù)行間行走，在同一數(shù)據(jù)源下運(yùn)行目前主流的A-LOAM、LeGO-LOAM、LIO-mapping建圖算法與本研究方法進(jìn)行對(duì)比，建圖效果如圖6所示。

(a)A-LOAM

結(jié)果表明，A-LOAM算法得到的地圖離群點(diǎn)數(shù)量多于其他算法，LeGO-LOAM與LIO-mapping雖然占用資源少，運(yùn)行速度快，但點(diǎn)云密度稀疏，果樹(shù)樹(shù)干特征不明顯，機(jī)器人在不平整路況下運(yùn)行地圖出現(xiàn)漂移情況導(dǎo)致建圖失敗。本研究建圖算法雖然占用計(jì)算資源較大，但建圖精度高，果樹(shù)樹(shù)干特征明顯，適合轉(zhuǎn)化為二維柵格地圖，相比于A-LOAM、LeGO-LOAM和LIO-mapping算法準(zhǔn)確性與魯棒性均有所提高。

在建圖過(guò)程中統(tǒng)計(jì)各算法點(diǎn)云地圖中離群點(diǎn)數(shù)量，如圖7所示，本文算法在建圖開(kāi)始的10 s內(nèi)離群點(diǎn)數(shù)量劇增，是由于初始階段點(diǎn)云數(shù)據(jù)過(guò)少，沒(méi)有足夠的線(xiàn)面特征進(jìn)行匹配，導(dǎo)致點(diǎn)云地圖雜亂點(diǎn)較多。

圖7 各算法離散點(diǎn)數(shù)量統(tǒng)計(jì)

4.2 果園機(jī)器人定位精度試驗(yàn)

為了驗(yàn)證機(jī)器人定位算法精度，選取了沒(méi)有枝葉遮擋的衛(wèi)星信號(hào)較好區(qū)域進(jìn)行定位對(duì)比試驗(yàn)，在上位機(jī)上發(fā)送指令使機(jī)器人按照S型路線(xiàn)行走，以 0.5 m/s 的速度進(jìn)行直線(xiàn)行走，以0.2 m/s的速度進(jìn)行270°轉(zhuǎn)彎，記錄GNSS測(cè)量值與機(jī)器人系統(tǒng)基于先驗(yàn)地圖的重定位數(shù)據(jù)，將GNSS得到的測(cè)量值作為真值，使GNSS和雷達(dá)的定位數(shù)據(jù)統(tǒng)一到相同坐標(biāo)系下，如圖8所示。

圖8 GNSS與本文算法定位坐標(biāo)對(duì)比

采用歐式范數(shù)和軌跡插值計(jì)算來(lái)量化誤差，公式描述如式(10)～式(12)所示。

Ex(t)=x(t)G-x(t)S

(10)

Ey(t)=y(t)G-y(t)S

(11)

(12)

式中：x(t)G、y(t)G——GNSS測(cè)量值；

x(t)S、y(t)S——機(jī)器人定位算法得到的坐標(biāo)值。

由于機(jī)器人定位算法精度與建圖精度緊密相關(guān)，在機(jī)器人轉(zhuǎn)向時(shí)點(diǎn)云地圖會(huì)出現(xiàn)漂移現(xiàn)象，位置與航向定位誤差隨之增加。機(jī)器人開(kāi)始運(yùn)行后，對(duì)定位誤差進(jìn)行采樣，航向偏離向左為正，向右為負(fù)，偏差分布如圖9所示。

(a)位置偏差分布

結(jié)果表明該定位算法定位精度較好，歐氏距離定位誤差取絕對(duì)值后最大值為0.191 2 m，最小值為0.014 3 m，航向角度誤差取絕對(duì)值后最大值為7.991 1°，最小值為0.033 6°，機(jī)器人轉(zhuǎn)彎時(shí)與直行時(shí)相比位置偏差明顯增加，但位置偏差總體幅度小于0.17 m，航向誤差小于13°，能夠滿(mǎn)足機(jī)器人定位需求。

4.3 果園機(jī)器人路徑規(guī)劃仿真試驗(yàn)

為驗(yàn)證本文算法較原始算法得到的路徑更短，在Matlab仿真環(huán)境下進(jìn)行路徑規(guī)劃仿真，構(gòu)建面積大小為900 m2的二維柵格仿真地圖，模擬果園環(huán)境，如圖10所示，其中黑色表示障礙物，白色為可行區(qū)域，圖內(nèi)三角形所在位置為機(jī)器人起始位置，圓形位置為機(jī)器人終點(diǎn)位置。仿真結(jié)果表示改進(jìn)后算法較原始算法路徑長(zhǎng)度減少2.164 m，路徑更加平緩，曲折線(xiàn)段減少4處，更適合機(jī)器人實(shí)際運(yùn)行，在真實(shí)果園環(huán)境中測(cè)試，機(jī)器人采用改進(jìn)后的路徑規(guī)劃算法到達(dá)目標(biāo)點(diǎn)所用的時(shí)間更短，運(yùn)行路徑較為改進(jìn)前減少5.77 m，運(yùn)行平穩(wěn)，未出現(xiàn)轉(zhuǎn)彎幅度過(guò)大的現(xiàn)象。

(a)A*算法仿真結(jié)果

4.4 果園機(jī)器人導(dǎo)航偏差測(cè)試試驗(yàn)

將得到的三維點(diǎn)云地圖轉(zhuǎn)化為可用于導(dǎo)航的二維柵格地圖，上位機(jī)界面如圖11所示。機(jī)器人可在白色區(qū)域內(nèi)自由移動(dòng)，黑色柵格為障礙物層，灰色柵格表示膨脹層，運(yùn)行move_base導(dǎo)航框架，在上位機(jī)上規(guī)定好目標(biāo)點(diǎn)與目標(biāo)姿態(tài)后，機(jī)器人規(guī)劃出全局路徑向目標(biāo)點(diǎn)移動(dòng)，如果中途檢測(cè)到障礙物，機(jī)器人會(huì)規(guī)劃出局部路徑進(jìn)行避障。

圖11 自主導(dǎo)航過(guò)程中上位機(jī)界面

向機(jī)器人系統(tǒng)發(fā)送不同速度指令，跟蹤規(guī)劃路徑分別以0.2 m/s、0.5 m/s和0.8 m/s的速度在果園環(huán)境中完成自主導(dǎo)航，在運(yùn)行過(guò)程中設(shè)置20個(gè)中間目標(biāo)導(dǎo)航點(diǎn)用于測(cè)量機(jī)器人系統(tǒng)導(dǎo)航偏差，相鄰導(dǎo)航點(diǎn)間隔一米，以機(jī)器人的初始位置作為坐標(biāo)原點(diǎn)，機(jī)器人坐標(biāo)系符合右手坐標(biāo)系，將GNSS測(cè)量值作為真值，測(cè)量機(jī)器人系統(tǒng)實(shí)際導(dǎo)航路徑與目標(biāo)路徑的橫向偏差、縱向偏差，規(guī)定機(jī)器人到達(dá)導(dǎo)航點(diǎn)左側(cè)為正橫向偏差，右側(cè)為負(fù)橫向偏差，到達(dá)導(dǎo)航點(diǎn)前方為正縱向偏差，后方為負(fù)縱向偏差。在各運(yùn)行速度下重復(fù)試驗(yàn)3次，以各項(xiàng)試驗(yàn)數(shù)據(jù)的平均值作為測(cè)量點(diǎn)處的偏差，如表1所示。

表1 機(jī)器人位置偏差

除了導(dǎo)航的橫向偏差與縱向偏差外，機(jī)器人實(shí)際位姿與目標(biāo)位姿采用航向角誤差衡量，機(jī)器人到達(dá)目標(biāo)點(diǎn)后實(shí)際航向在目標(biāo)航向左側(cè)為正偏差，在目標(biāo)航向右側(cè)為負(fù)偏差，航向誤差如表2所示。

表2 機(jī)器人姿態(tài)偏差

隨著果園機(jī)器人的運(yùn)行速度增加，位置橫向偏差、縱向偏差以及航向誤差均有所增加，原因在于運(yùn)行速度過(guò)快會(huì)影響建圖效果以及定位精度，但三種速度下橫向偏差與縱向偏差平均值不超過(guò)10 cm，標(biāo)準(zhǔn)差不超過(guò)7 cm，航向誤差平均值不超過(guò)10°，標(biāo)準(zhǔn)差不超過(guò)6°。試驗(yàn)結(jié)果證明該系統(tǒng)精度滿(mǎn)足果園機(jī)器人在真實(shí)果園內(nèi)自主導(dǎo)航作業(yè)的要求。

5 結(jié)論

1)針對(duì)果園自動(dòng)化程度低的問(wèn)題，本文提出一種果園機(jī)器人自主導(dǎo)航系統(tǒng)。針對(duì)果樹(shù)點(diǎn)云畸變特性，計(jì)算出補(bǔ)償量對(duì)點(diǎn)云進(jìn)行補(bǔ)償以消除點(diǎn)云畸變，采用提取線(xiàn)面特征的點(diǎn)云匹配方式計(jì)算各時(shí)刻相對(duì)位姿變化，避免了在果園復(fù)雜環(huán)境內(nèi)地圖漂移的現(xiàn)象，并提高了激光里程計(jì)的準(zhǔn)確性，使用基于圖優(yōu)化的融合方案實(shí)現(xiàn)建圖與定位功能。

2)對(duì)原始A*算法進(jìn)行改進(jìn)，針對(duì)果園環(huán)境降低了原始算法的貪婪程度，實(shí)際測(cè)試中路徑長(zhǎng)度減少5.77 m，路徑更加平緩，更適合機(jī)器人實(shí)際運(yùn)動(dòng)，加入了動(dòng)態(tài)窗口算法，使機(jī)器人具有了局部避障的能力。

3)將本文的建圖定位與路徑規(guī)劃算法應(yīng)用到果園機(jī)器人上，在真實(shí)果園內(nèi)進(jìn)行自主導(dǎo)航試驗(yàn)，結(jié)果表明，以0.5 m/s的速度運(yùn)行時(shí)導(dǎo)航效果最好，橫向偏差與縱向偏差平均值不超過(guò)10 cm，航向誤差平均值不超過(guò)10°。滿(mǎn)足果園機(jī)器人作業(yè)要求。