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

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

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

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

1 D*算法

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

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

(1)

式中：f(n)是節(jié)點(diǎn)的估計(jì)代價(jià)函數(shù)，g(n)是從起點(diǎn)開(kāi)始到目前位置節(jié)點(diǎn)n的消耗的代價(jià)，通過(guò)以起始節(jié)點(diǎn)出發(fā)到達(dá)當(dāng)前節(jié)點(diǎn)n的歐氏距離來(lái)計(jì)算；h(n)是從當(dāng)前節(jié)點(diǎn)n出發(fā)到終點(diǎn)的估計(jì)需要消耗的代價(jià)，同以當(dāng)前節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的歐氏距離表示[11]。

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

(2)

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

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

(3)

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

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

新的路徑估計(jì)值過(guò)低，k(X)

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

新的路徑估價(jià)值過(guò)高，k(X)>h(Y)，且OPEN和CLOSED有節(jié)點(diǎn)遷移，在OPEN表中更新當(dāng)前路徑估價(jià)值h(Y)。

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

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

2 仿真試驗(yàn)

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

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

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

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

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

算法搜索長(zhǎng)度搜索時(shí)間/s A?算法3517.5D?算法2814比較73.5

3 農(nóng)用機(jī)器人的路徑與導(dǎo)航試驗(yàn)

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

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

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

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

3.1 CAN通信協(xié)議

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

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

圖3 慣導(dǎo)輸出機(jī)器人坐標(biāo)數(shù)據(jù)

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

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

3.2 位置信息標(biāo)注

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

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

打開(kāi)ArcMap軟件，導(dǎo)入得到的經(jīng)緯度信息數(shù)據(jù)信息[23]，選擇對(duì)應(yīng)的經(jīng)緯度坐標(biāo)信息，即可在ArcMap窗口中生成點(diǎn)數(shù)據(jù)，關(guān)鍵代碼如下，

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

{

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

IPoint point；

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

{

point=new PointClass()；

point.SpatialReference=this.axMapControl1.SpatialReference；//設(shè)置點(diǎn)的坐標(biāo)和參考系

point.X=(Double)ds.Tables[0].Rows[i][1]；//獲取經(jīng)度坐標(biāo)

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

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

IMappMap=this.axMapControl1.Map；//設(shè)置一個(gè)控件

pGraphicsContainer=pMap as IGraphicsContainer；//在地圖進(jìn)行線的標(biāo)注

IActiveViewpActiveView=pMap as IActiveView；//將pMap轉(zhuǎn)為IActiveView接口類型

}

}

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

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

圖5 試驗(yàn)平臺(tái)搭建

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

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

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

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

表5 D*算法

4 結(jié)論

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