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

高鵬，姜軍生，白陽，宋健

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

0 引言

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

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

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

1 機器人平臺

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

圖1 果園機器人硬件平臺

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

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

2 果園機器人建圖與定位算法

2.1 激光里程計

2.1.1 點云畸變補償

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

圖3 果樹點云畸變

式中：T0——起始時刻雷達位姿；

Ti——第i個激光點采集時雷達的位姿；

Pi——第i個激光點坐標；

2.1.2 線面特征提取

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

(2)

式中：c——曲率；

X——當前激光點坐標；

Xi——第i個點坐標。

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

(3)

pi——第k幀的點；

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

t——平移量。

(4)

(5)

式中：dε——當前幀激光點到上一幀直線的距離；

dΗ——當前幀激光點到平面的距離；

pa、pb——上下相鄰激光線上的激光點坐標；

pm、pl、pj——同一平面上按順時針排序的激光點坐標。

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

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

x=(R+H)cosαcosβ

(6)

y=(R+H)cosαsinβ

(7)

(8)

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

H——海拔高度；

R——地球半徑；

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

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

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

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

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

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

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

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

(9)

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

h(n)——當前節(jié)點到目標節(jié)點所需花費代價的估計值；

D——起始點到終點的距離；

d——當前點到終點的距離。

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

(a)當ε=1時計算出的路徑

3.2 動態(tài)窗口

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

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

(17)

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

圖5 速度離散采樣示意圖

4 果園機器人自主導(dǎo)航性能測試試驗

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

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

(a)A-LOAM

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

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

圖7 各算法離散點數(shù)量統(tǒng)計

4.2 果園機器人定位精度試驗

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

圖8 GNSS與本文算法定位坐標對比

采用歐式范數(shù)和軌跡插值計算來量化誤差，公式描述如式(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測量值；

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

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

(a)位置偏差分布

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

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

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

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

4.4 果園機器人導(dǎo)航偏差測試試驗

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

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

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

表1 機器人位置偏差

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

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

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

5 結(jié)論

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

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

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