農(nóng)業(yè)機器人路徑規(guī)劃算法研究

成家駿，謝 銳，荊瑞俊

（1.山西農(nóng)業(yè)大學(xué) 軟件學(xué)院，晉中030800；2.中北大學(xué) 電氣與控制工程學(xué)院，太原030051）

規(guī)?；r(nóng)業(yè)已成為現(xiàn)代農(nóng)業(yè)的主流形式，在此背景下，作物檢測對人力物力消耗極大，成為現(xiàn)代農(nóng)業(yè)發(fā)展過程中，亟待解決的問題?，F(xiàn)階段有多種算法可用于農(nóng)業(yè)機器人的路徑規(guī)劃，比如A* 算法[1-2]、RRT 算法[3-4]、Dijkstra 算法[5]等。這些算法能找到較為可靠路徑，但存在各種缺點。Dijkstra 算法進行搜索時，搜索以輻射狀擴散，雖能找到最優(yōu)路徑，但遍歷點過多，嚴(yán)重增加了算法時間復(fù)雜度；A* 算法在Dijkstra 算法基礎(chǔ)上加入啟發(fā)式函數(shù)， 有較強的目的性，但也存在不能保證最優(yōu)路徑的問題；RRT算法進行隨機點連線搜索，難以在狹小空間找到最優(yōu)路徑。為使算法能夠更好的適應(yīng)復(fù)雜環(huán)境，隨之出現(xiàn)許多改進算法，如文獻[6]提出一種改進多步長的蟻群算法，用來提高算法搜索效率；文獻[7]運用人工勢場算法調(diào)整信息素量的方法提高螞蟻避障能力。蟻群算法ACA（ant colony algorithm）是一種模擬螞蟻覓食的啟發(fā)式隨機搜索算法[8-11]，具有自組織性、正反饋等優(yōu)點，并在此基礎(chǔ)上有較好的魯棒性，是很多文獻解決最短路徑問題研究的算法之一。

1 農(nóng)業(yè)機器人模型

1.1 激光雷達(dá)傳感器

激光探測及測距LiDAR（LightLaser Deteetion and Ranging）系統(tǒng)中，激光器是雷達(dá)的輻射源。其工作原理是向目標(biāo)發(fā)射探測信號，將接收到的從目標(biāo)反射回來的目標(biāo)回波與發(fā)射信號進行比較，可以獲取目標(biāo)物體距離、方位、高度、速度等參數(shù)的電子設(shè)備，被廣泛用于機器人二維感知的檢測傳感器。

根據(jù)設(shè)備特征，八叉樹搜索策略如圖1 所示，建立仿真模型。

圖1 八叉樹搜索策略Fig.1 Octree search strategy

圖中，灰色圓形代表機器人，在實際仿真工作中每步長移動的起點和終點均為柵格地圖中柵格的中心點，也為機器人實際體積的中心點；白色圓環(huán)代表激光雷達(dá)掃描范圍；黑色方塊為膨脹后在柵格地圖中機器人感知范圍。采用八叉樹搜索策略，機器人可以根據(jù)算法進行八方移動，解決了路線易發(fā)生偏移、轉(zhuǎn)彎較多、路線長度增加等不足，使得柵格之間移動更加自由，路徑搜索效率更高。

1.2 坐標(biāo)變換與位姿模型

在三維空間中，由于移動機器通常在平面中運動，可以簡化模型，從而建立二維固定的世界坐標(biāo)系xOy；把小車看成三維空間的剛體，建立左手正交坐標(biāo)系x′O′y′。小車在空間運動，從而產(chǎn)生坐標(biāo)系的空間變換，運動學(xué)模型如圖2 所示。

當(dāng)機器人從點O（x，y）運動到點O′（x′，y′）時，有

圖2 運動學(xué)模型示意圖Fig.2 Schematic diagram of kinematics model

由式（1）（2），可推出從以坐標(biāo)原點O 為原點的xOy 到x′O′y′的坐標(biāo)變換，其矩陣形式為

其中

為本次旋轉(zhuǎn)的旋轉(zhuǎn)矩陣，再加上平移量，得

由上述推導(dǎo)可以得出，在固定不動的世界坐標(biāo)系中，可以通過三維坐標(biāo)系中任意2 個坐標(biāo)系的旋轉(zhuǎn)，平移得出位置變換，從而獲得機器人在運動中的運動方向位置信息。但由于移動機器人在運動的過程中， 激光雷達(dá)必然會出現(xiàn)不可避免的誤差，通常由回環(huán)檢測、 噪聲濾波來矯正傳輸回來的數(shù)據(jù)，達(dá)到更加精準(zhǔn)的目的。

由于坐標(biāo)變換與位姿模型中計算多為矩陣與向量計算， 故將經(jīng)典算法中坐標(biāo)表示繪制路徑用quiver 二維矢量函數(shù)進行處理繪制出每一步的方向，并采用1.2 步長突出每一步的方向變化。

1.3 農(nóng)田鳥瞰圖模型建立

在路徑規(guī)劃的環(huán)境模型建立中，大多應(yīng)用柵格法建立環(huán)境建模，因此在MatLab 環(huán)境下建立30 m×30 m 柵格仿真地圖（如圖3 所示），作為農(nóng)田鳥瞰圖模型。

圖中，3×3 的灰格為農(nóng)田，白格為通路；起點為左上角（0.5，19.5），終點為右下角（19.5，0.5），從起點開始到終點結(jié)束。

圖3 仿真環(huán)境柵格地圖Fig.3 Raster map of simulated environment

2 退火蟻群算法

經(jīng)典蟻群算法， 模擬螞蟻覓食的過程模型，由于正反饋機制的存在，在算法運行到一定程度時容易進入局部最優(yōu)解的現(xiàn)象。在整個蟻群算法迭代完成后進行模擬退火的運行優(yōu)化可能路徑，在完成一輪蟻群算法迭代后，取精英螞蟻進行節(jié)點段的平移和連接操作，并進行組合以產(chǎn)生新解，按照退火規(guī)則接受新解，從而完成模擬退火與蟻群算法的結(jié)合，完成基于模擬退火算法改進的蟻群算法。具體步驟如圖4 所示。

圖4 退火蟻群算流程Fig.4 Calculation flow chart of anneal ant colony algorithm

在此以退火蟻群算法為基礎(chǔ)，加入機器人每次移動改變的位姿及坐標(biāo)變換的角度變化，以及坐標(biāo)信息的輸出規(guī)則，并在算法模擬中用箭頭來表示。假設(shè)，螞蟻數(shù)量為n，螞蟻從固定節(jié)點出發(fā)，為了保證算法比較迭代次數(shù)相同，如果經(jīng)典蟻群算法迭代次數(shù)為m，則退火蟻群算法中，蟻群算法迭代次數(shù)為m/2，預(yù)設(shè)溫度為蟻群算法運行2 次（蟻群算法→退火算法→蟻群算法→結(jié)束）。

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

該測試所用的操作系統(tǒng)為Windows 10，內(nèi)存容量為8 GB，電源功率為120 W。為檢驗?zāi)M退火算法對蟻群算法的優(yōu)化效果， 通過MatLab 仿真軟件將其與經(jīng)典蟻群算進行對比試驗，對比路徑結(jié)果如圖5 所示。由圖可見，機器人路徑沿左上→右下對角線基本對稱， 但在圖5a 中縱坐標(biāo)30—25 處經(jīng)典算法產(chǎn)生略微偏差，陷入局部最優(yōu)解的情況。

圖5 不同蟻群算法的路徑Fig.5 Paths of different ant colony algorithms

退火蟻群算法與經(jīng)典算法變化折線圖如圖6所示。由圖可見，在50 次迭代前，兩算法最短路徑變化抖動都較為明顯；在50 次迭代后，退火蟻群算法在略微抖動后趨于收斂，與抖動較為明顯的經(jīng)典算法形成較強的對比。

圖6 退火蟻群算法與經(jīng)典算法的變化趨勢Fig.6 Change trend of annealed ant colony algorithm and classical algorithm

4 結(jié)語

通過深入研究退火蟻群算法在農(nóng)業(yè)機器人路徑規(guī)劃問題，分析了通過激光雷達(dá)在算法中的膨脹處理，并在算法中加入了坐標(biāo)及位姿變換相關(guān)內(nèi)容，可以輸出每次坐標(biāo)旋轉(zhuǎn)角度以及位置姿勢的變化情況， 最后在MatLab 仿真環(huán)境中進行了退火蟻群算法，驗證了基于退火蟻群算法在農(nóng)業(yè)機器人路徑規(guī)劃方面， 可以極大程度地改善算法的收斂情況，并有效緩解經(jīng)典算法陷入局部最優(yōu)解的問題。