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

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

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

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

1 D*算法

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

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

(1)

式中：f(n)是節(jié)點的估計代價函數(shù)，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，用于存儲未經(jīng)訪問節(jié)點的路徑代價值；CLOSE列表的集合為B，用于存儲已訪問的路徑代價；NEW列表的集合為C，用于存儲待更新節(jié)點的路徑代價[13]。D*算法中每個節(jié)點X都有一個標識函數(shù)t(X)

(2)

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

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

(3)

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

從集合B中遍歷路徑估計函數(shù)h(Y，X)，按式(3)分析最小路徑估計值k(X)的變化，結(jié)合式(2)，動態(tài)更新路徑估計函數(shù)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)境

根據(jù)以上分析，在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ī)劃軌跡空間為可通過路徑(如農(nóng)田中的道路)，黑色區(qū)域為不可操作區(qū)域(如農(nóng)作物種植區(qū))，黑色圓為隨機障礙物。根據(jù)機器人運動學模型，設置農(nóng)用機器人穩(wěn)定行駛時速度為2.0m/s。

2.2 仿真及結(jié)果分析

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

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

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

3 農(nóng)用機器人的路徑與導航試驗

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

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

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

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

3.1 CAN通信協(xié)議

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

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

圖3 慣導輸出機器人坐標數(shù)據(jù)

表2 經(jīng)度數(shù)據(jù)117.249 910 607傳輸示例

表3 緯度數(shù)據(jù)31.866 776 105傳輸示例

3.2 位置信息標注

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

圖4 經(jīng)緯度信息位置標注

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

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

{

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

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]；//獲取經(jīng)度坐標

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轉(zhuǎn)為IActiveView接口類型

}

}

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

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

圖5 試驗平臺搭建

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

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

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

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

表5 D*算法

4 結(jié)論

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