基于D*算法的農用履帶機器人路徑規(guī)劃研究

陳 靖，辜麗川，李倩倩，何嶼彤，吳亞文，焦 俊

(安徽農業(yè)大學信息與計算機學院，安徽 合肥 230036)

隨著計算機技術和地理信息科學的發(fā)展，GIS(地理信息系統(tǒng))因其強大的網絡分析功能越來越多的應用到人們的日常生活中。路徑規(guī)劃無論是在交通運輸，還是在城市規(guī)劃等方面，都起到了至關重要的作用。路徑規(guī)劃部分作為機器人領域的一個重點，經過數十年的不斷發(fā)展，已經取得了較為明顯的進步[1]。各國學者提出了很多針對性的方法，常見的有：柵格法、人工勢場法、神經網絡、遺傳算法、蟻群算法等。文獻[2-3]將柵格法應用于路徑規(guī)劃過程中，便于計算機存儲，信息更新快，但柵格法本身搜索具有盲目性，依賴于對精度的要求，當環(huán)境復雜時，算法搜索效率較低；文獻[4-5]通過對人工勢場法的拓展，使之可以進行動態(tài)環(huán)境下的路徑規(guī)劃，但人工勢場法本身的一些問題沒有解決，比如存在局部最優(yōu)等。文獻[6]和文獻[7]將神經網絡應用于動態(tài)路徑規(guī)劃，有較好的學習能力，但在大規(guī)模不確定環(huán)境下網絡結構過于龐大。文獻[8-9]分別用遺傳算法和蟻群算法進行動態(tài)環(huán)境下的路徑規(guī)劃，取得了不錯的效果，但都在不同程度上存在實時性較差的問題。

D*算法是一種基于信息部分已知動態(tài)環(huán)境下的算法，具有計算量小、實時性強、復雜程度低易于與其他算法結合等優(yōu)點。本文采用D*算法對農用履帶機器人進行路徑規(guī)劃方法研究，重點對路徑規(guī)劃過程中生成路徑的平滑設計、碰撞檢測和算法的實時性進行研究，并將其應用于農用履帶機器人，旨在實現農用履帶機器人在農田環(huán)境下的自主導航功能。

1 D*算法

A*算法是一種經典的啟發(fā)式搜索算法，它結合了BFS(Best First Search)算法和Dijkstra算法的優(yōu)點，通過定義估價函數來評估代價而確定最優(yōu)路徑[10]。A*算法將實際代價看成兩部分之和，即已經付出的代價和將要付出的代價，該代價函數可表示為式(1)

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

(1)

式中：f(n)是節(jié)點的估計代價函數，g(n)是從起點開始到目前位置節(jié)點n的消耗的代價，通過以起始節(jié)點出發(fā)到達當前節(jié)點n的歐氏距離來計算；h(n)是從當前節(jié)點n出發(fā)到終點的估計需要消耗的代價，同以當前節(jié)點到目標節(jié)點的歐氏距離表示[11]。

D*算法是在A*算法的基礎上發(fā)展而來的動態(tài)路徑搜索算法，適用于動態(tài)環(huán)境下的路徑規(guī)劃[12]。D*算法開辟了三個狀態(tài)列表OPEN、CLOSED和NEW，分別存儲不同的路徑代價：OPEN列表的集合為A，用于存儲未經訪問節(jié)點的路徑代價值；CLOSE列表的集合為B，用于存儲已訪問的路徑代價；NEW列表的集合為C，用于存儲待更新節(jié)點的路徑代價[13]。D*算法中每個節(jié)點X都有一個標識函數t(X)

(2)

為了尋求最短路徑p(最優(yōu)解)的單調序列Y，設k(X)為Y的最小路徑估計價值，即

k(X)=min[h(X),X∈B]

(3)

首先，對OPEN、CLOSED和NEW賦初始值。其中，G，φ,W分別為OPEN、CLOSED和NEW的初始化集合。

從集合B中遍歷路徑估計函數h(Y，X)，按式(3)分析最小路徑估計值k(X)的變化，結合式(2)，動態(tài)更新路徑估計函數h(Y，X)，以下述三種情況更新OPEN和CLOSED的所有出邊：

新的路徑估計值過低，k(X)

新的路徑估價值無變化，k(X)=h(Y)，且OPEN和CLOSED有節(jié)點遷移，在OPEN表中插入當前路徑估價值h(Y)+c(X，Y)。

新的路徑估價值過高，k(X)>h(Y)，且OPEN和CLOSED有節(jié)點遷移，在OPEN表中更新當前路徑估價值h(Y)。

如此循環(huán)執(zhí)行，直至滿足終止條件：

或者是相應的路徑序列不存在。從而獲得最短路徑p。

2 仿真試驗

2.1 仿真試驗環(huán)境

根據以上分析，在MATLAB平臺上進行仿真實驗，如圖1所示，本文采用柵格地圖的方式對機器人的工作環(huán)境進行建模，在模擬地圖中，分別使用傳統(tǒng)A*算法、D*算法在相同柵格地圖下對比[14]。假設機器人的工作環(huán)境為有限的二維空間區(qū)域，采用柵格法將空間區(qū)域劃分為固定大小的柵格，將其映射到空間坐標系中。柵格地圖中單元柵格為邊長1m，地圖尺寸10m×10m，模擬規(guī)劃時，設置起點坐標(2，10)，終點坐標(5，1.4)，模擬區(qū)域范圍[0 10 0 10]。柵格地圖中白色區(qū)域是機器人的可規(guī)劃軌跡空間為可通過路徑(如農田中的道路)，黑色區(qū)域為不可操作區(qū)域(如農作物種植區(qū))，黑色圓為隨機障礙物。根據機器人運動學模型，設置農用機器人穩(wěn)定行駛時速度為2.0m/s。

2.2 仿真及結果分析

傳統(tǒng)A*算法采用8領域搜索節(jié)點，這樣不僅讓節(jié)點擴展過于繁瑣，算法的計算量大，而且會導致鋸齒效應，造成路徑折線長、拐點多[15]。圖1為傳統(tǒng)A*算法和D*算法在相同柵格地圖下的仿真實實驗。表1列出了相同的實驗條件采用兩種不同算法產生的數據，由仿真結果分析可知兩種算法均能計算出起點到終點的最短路徑，但是D*算法縮短了優(yōu)化路徑的長度，減少了機器人搜索時間，提高了算法的搜索效率。D*算法在面對尖角拐彎處時，能夠對尖角附近的路徑重新規(guī)劃，其他區(qū)域路徑不變，具有較好的實時性，能夠避免機器人在拐點處工作時發(fā)生碰撞或側翻情況，而傳統(tǒng)的A*算法是通過全局路徑重規(guī)劃對檢測到的障礙物進行避障，需要對整個環(huán)境地圖的柵格進行處理來生產一個全局最優(yōu)路徑，導致算法重復計算節(jié)點的比較多，算法效率偏低。由此可以看出，D*算法更能有效的解決移動機器人的路徑規(guī)劃問題。

圖1 傳統(tǒng)A*算法和D*算法在相同柵格環(huán)境下中仿真軌跡示意圖

算法搜索長度搜索時間/s A?算法3517.5D?算法2814比較73.5

3 農用機器人的路徑與導航試驗

在對系統(tǒng)需求進行分析的基礎上，主要從各節(jié)點結構和功能兩個方面設計各節(jié)點模塊方案：

(1)控制系統(tǒng)主節(jié)點A被設計為路徑規(guī)劃和數據存儲服務端的節(jié)點，主要功能是：① 接收機器人控制器B傳入的機器人位姿數據，并在地圖上對機器人行駛軌跡進行標點；② 將指定目標點發(fā)送至機器人控制器B后，機器人實現對指定目標點的精準跟蹤。

(2)利用處理器S3C2440為主控單元，添加傳感器和模塊電路，其中有：網絡通信模塊、數據采集及傳輸系統(tǒng)、運動控制模塊等構成機器人控制器，作為系統(tǒng)節(jié)點B；主要功能是：① 采集、解析慣導設備的位姿數據后，發(fā)送至系統(tǒng)主節(jié)點A；② 將解析后的位姿數據與主節(jié)點發(fā)送的指定目標點進行坐標轉換、比較后，輸入至控制器，利用控制器完成對指定目標點的精準跟蹤。

圖2農業(yè)機器人路徑規(guī)劃研究實現流程圖

3.1 CAN通信協(xié)議

CAN總線具有通信速率快、可靠性高、成本低、抗干擾能力強的優(yōu)點，可以將經緯度信息數據以很高的通信速率可靠的發(fā)送給上層計算機，有效的解決農業(yè)機器人在工作環(huán)境惡劣下的通信實時性較差、電磁干擾、傳輸距離短等問題，所以本試驗采用CAN總線作為通信方式[16]。針對控制系統(tǒng)通信網絡需求，結合機器人控制系統(tǒng)中通信的特點以及用戶自我需求，自行設計CAN總線應用層通信協(xié)議，對總線傳輸的數據進行了分類，重新定義了協(xié)議控制域和數據域[17]。

首先需要知曉慣導輸出的數據格式，經查找相關資料后了解到SPAN—CPT內部差分解算后，可輸出多種格式的機器人位姿數據[18]。農業(yè)機器人慣導實時輸出的機器人位姿數據如圖3所示，當機器人正常工作時，節(jié)點B將慣導采集到的機器人位置數據進行解析后，經過總線傳輸給節(jié)點A，由于CAN總線數據傳輸采用短幀結構，重新定義的數據幀結構包括幀起始、協(xié)議控制域、控制域、協(xié)議數據域、CRC域、應答域、幀結尾七個部分，每幀數據域最多為8個字節(jié)，在數據打包過程中，對于相關數據可以打包到1幀中，以保證數據的發(fā)送速率和總線寬帶的利用率[19]。在協(xié)議中，字節(jié)1-7為數據域的實際數據。以經度：117.249 910 607、緯度：31.866 776 105數據為例，對數據域的分段編碼和數據段進行設置后如表2～表3所示。

圖3 慣導輸出機器人坐標數據

表2 經度數據117.249 910 607傳輸示例

表3 緯度數據31.866 776 105傳輸示例

3.2 位置信息標注

通過SPAN-CPT的慣導定位系統(tǒng)來獲取農用機器人的位置信息，將慣導系統(tǒng)架載在農用機器人上，在使用CAN 總線接收到慣導系統(tǒng)產生的機器人位置信息后[20]，可以通過ArcGIS Engine二次開發(fā)提供的類和接口實現機器人在地圖上的標注[21]，再利用多線程管理實現機器人實時位置信息的標注，即實現路徑跟蹤的功能[22]。完成位置標注的功能程序流程如圖4所示。

圖4 經緯度信息位置標注

打開ArcMap軟件，導入得到的經緯度信息數據信息[23]，選擇對應的經緯度坐標信息，即可在ArcMap窗口中生成點數據，關鍵代碼如下，

private void button4-Click(object sender， EventArgs e)

{

da.Fill(ds)；//讀取數據庫中數據

IPoint point；

for (int i=0； i< 7； i++)

{

point=new PointClass()；

point.SpatialReference=this.axMapControl1.SpatialReference；//設置點的坐標和參考系

point.X=(Double)ds.Tables[0].Rows[i][1]；//獲取經度坐標

point.Y=(Double)ds.Tables[0].Rows[i][2]；//獲取緯度坐標

point.PutCoords(point.X，point.Y)；//設置一個接口，將坐標信息賦值給X，Y

IMappMap=this.axMapControl1.Map；//設置一個控件

pGraphicsContainer=pMap as IGraphicsContainer；//在地圖進行線的標注

IActiveViewpActiveView=pMap as IActiveView；//將pMap轉為IActiveView接口類型

}

}

3.3 農田環(huán)境下的實時路徑規(guī)劃試驗

為驗證仿真結果，將其用于農用履帶機器人并在模擬農田環(huán)境進行路徑規(guī)劃試驗[24]。本試驗的移動平臺是THNYJQR-1型農業(yè)履帶機器人，在對履帶機器人進行軌跡跟蹤試驗前需要完成慣導移動站和基站、嵌入式控制系統(tǒng)硬件、電機驅動、CDMA數傳模塊、外圍傳感器等設備的架設、校準以及相應參數配置，如圖5所示。

圖5 試驗平臺搭建

圖6 農田環(huán)境路徑規(guī)劃

基于GIS方法構建的實際地圖如圖6所示。其中，規(guī)劃路徑的起點為農田入口點A(0，0)，目標點為機器人工作?？奎cE(2，0)。

試驗過程中，機器人系統(tǒng)實時記錄傳統(tǒng)A*算法(圖6中左側曲線)D*算法規(guī)劃的路徑(圖6中右側曲線)。表4～表5為在農田環(huán)境下傳統(tǒng)A*算法和D*算法實時行駛狀態(tài)表，對比觀察可知，基于A*算法時機器人按照規(guī)劃路徑由起點A經由拐點(B2、C2、D2)到達終點E，行駛速度為2m/s，總花費時間為161.452s；基于D*算法時機器人按照規(guī)劃路徑由起點A經由拐點(B1、C1、D1)到達終點E，行駛速度為2m/s，總花費時間為145.936s；由此可知D*算法花費時間更少，效率更高。

表4 傳統(tǒng)A*算法

表5 D*算法

4 結論

本文針對農用機器在了農田環(huán)境下的路徑規(guī)劃問題，采用了一種更適合履帶機器人在農田環(huán)境下工作的D*算法[25]，解決了傳統(tǒng)A*算法只是求解最短路徑，而未考慮到拐點處路徑是否圓滑，是否適合機器人的正常工作的問題。本文采用了D*算法規(guī)劃路徑的方式，縮小了算法的搜索空間，降低了搜索路徑長度和搜索時間，提高了算法在路徑規(guī)劃中的搜索效率，并且在生成平滑路徑的同時兼顧了機器人本體的碰撞問題，更加符合實際需求，保證了算法的實時性。