成家駿，謝 銳，荊瑞俊

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

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

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

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

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

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

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

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

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

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

當(dāng)機(jī)器人從點(diǎn)O（x，y）運(yùn)動(dòng)到點(diǎn)O′（x′，y′）時(shí)，有

圖2 運(yùn)動(dòng)學(xué)模型示意圖Fig.2 Schematic diagram of kinematics model

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

其中

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

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

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

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)田，白格為通路；起點(diǎn)為左上角（0.5，19.5），終點(diǎn)為右下角（19.5，0.5），從起點(diǎn)開始到終點(diǎn)結(jié)束。

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

2 退火蟻群算法

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

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

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

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

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

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

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

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

4 結(jié)語(yǔ)

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