本發(fā)明涉及機(jī)器人算法領(lǐng)域，具體是一種果園履帶機(jī)器人路徑規(guī)劃方法。

背景技術(shù)：

傳統(tǒng)的農(nóng)業(yè)機(jī)械在農(nóng)作過(guò)程中存在誤操作、能源利用率低及作業(yè)人員重負(fù)荷等問(wèn)題。針對(duì)以上問(wèn)題進(jìn)行路徑規(guī)劃已經(jīng)成為發(fā)展智能化農(nóng)業(yè)機(jī)械的重要研究?jī)?nèi)容。路徑規(guī)劃是指根據(jù)某種最優(yōu)原則(如：工作代價(jià)最小，行走路線最短，行走時(shí)間最短)，在工作空間中尋找一條從起始位置到目標(biāo)位置的、避開(kāi)障礙物的最優(yōu)路徑。而針對(duì)農(nóng)用機(jī)器人的路徑規(guī)劃是指合理而高效的搜尋一條在農(nóng)田環(huán)境內(nèi)除障礙物及農(nóng)作物以外的最優(yōu)無(wú)碰撞路徑。

路徑規(guī)劃的核心是算法的設(shè)計(jì)，目前路徑規(guī)劃算法已經(jīng)得到了廣泛的發(fā)展。基于遺傳算法的路徑規(guī)劃是一種局部路徑規(guī)劃，適用于變化的環(huán)境，實(shí)時(shí)性較好，但規(guī)劃速度較慢及占用存儲(chǔ)空間較多；蟻群算法有很好的穩(wěn)定性及適應(yīng)性，能夠找到全局最優(yōu)解，與遺傳算法相比，搜索時(shí)間顯著減少，但在處理較大區(qū)域的路徑規(guī)劃時(shí)，會(huì)出現(xiàn)停滯現(xiàn)象或陷入局部最優(yōu)解；而最基礎(chǔ)的Dijkstra算法適用于小范圍的路徑搜索計(jì)算，較易實(shí)現(xiàn)，但遍歷計(jì)算節(jié)點(diǎn)多，效率低。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明的目的是提供一種果園履帶機(jī)器人路徑規(guī)劃方法，以解決現(xiàn)有技術(shù)機(jī)器人路徑算法存在的問(wèn)題。

為了達(dá)到上述目的，本發(fā)明所采用的技術(shù)方案為：

一種果園履帶機(jī)器人路徑規(guī)劃方法，其特征在于：利用C#結(jié)合ArcGIS Engine二次開(kāi)發(fā)編程實(shí)現(xiàn)農(nóng)用機(jī)器人基于A*和B樣條算法的路徑規(guī)劃軟件平臺(tái)，首先需要繪制用于農(nóng)用機(jī)器人路徑規(guī)劃的地圖；再基于ArcGIS Engine組件和C#語(yǔ)言的Windows桌面應(yīng)用軟件的開(kāi)發(fā)；在此基礎(chǔ)上編寫A*算法以實(shí)現(xiàn)機(jī)器人最短路徑規(guī)劃及B樣條曲線算法平滑路徑，進(jìn)而實(shí)現(xiàn)農(nóng)用機(jī)器人最優(yōu)路徑規(guī)劃，最后利用Socket網(wǎng)絡(luò)編程及3G模塊傳輸，將規(guī)劃好的路徑傳輸?shù)睫r(nóng)用機(jī)器人上，使機(jī)器人按該最優(yōu)路徑運(yùn)行，具體過(guò)程如下：

(1)、地圖數(shù)據(jù)的處理：

地圖數(shù)據(jù)的獲取及處理為基礎(chǔ)工作，是為了在基于ArcGIS Engine開(kāi)發(fā)中提供圖層數(shù)據(jù)；首先使用地圖下載軟件獲取地圖數(shù)據(jù)，并使用ArcMap軟件對(duì)下載的地圖數(shù)據(jù)進(jìn)行圖像配準(zhǔn)；再繪制道路shp圖，利用ArcToolbox工具箱中的數(shù)據(jù)管理工具--要素--要素轉(zhuǎn)點(diǎn)工具將道路圖轉(zhuǎn)化為道路關(guān)鍵點(diǎn)shp圖，兩圖通過(guò)Id屬性相關(guān)聯(lián)；由此即完成對(duì)地圖數(shù)據(jù)的處理，建立了用于路徑規(guī)劃的地圖；

(2)、基于ArcGIS Engine的開(kāi)發(fā)：

2.1)加載地圖文檔：在Visual Studio2010軟件中新建基于C#語(yǔ)言的Windows桌面應(yīng)用程序，并加入ArcGIS控件和ArcGIS Engine庫(kù)引用，由此便可開(kāi)發(fā)基于ArcGIS Engine的程序，在窗體中添加LicenseControl和AxMapControl控件，再放置一個(gè)名為打開(kāi)文件的button控件，編寫程序?qū)崿F(xiàn)將地圖數(shù)據(jù)加載到AxMapControl控件中；

2.2)添加選取路徑起點(diǎn)及終點(diǎn)功能：在窗體中添加選取起點(diǎn)和選取終點(diǎn)的button控件，并添加axMapControl1_OnMouseDown事件，編程使當(dāng)按下選取起點(diǎn)或終點(diǎn)按鈕時(shí)，利用ITextElement類的cre ateTextElement方法繪制起點(diǎn)或終點(diǎn)，并將該起點(diǎn)或終點(diǎn)通過(guò)axMapControl1.ActiveView.GraphicsCont ainer.AddElement和axMapControl1.Refresh方法繪制到地圖控件中；

2.3)創(chuàng)建地圖矩陣：在對(duì)路徑進(jìn)行A*算法規(guī)劃之前需要?jiǎng)?chuàng)建所需路徑，即地圖矩陣；首先獲取加載的地圖的圖層信息ILayer接口，由于IFeatureLayer是ILayer的子類，IFeature是IFeatureLayer中的一個(gè)要素，獲得IFeatureLayer后，可以從IFeatureLayer中得到IFeatureClass，再利用游標(biāo)IFeatureCursor就可以把IFeature一個(gè)個(gè)遍歷出來(lái)，即可把道路圖層中的每一條道路和道路關(guān)鍵點(diǎn)圖層中的每一點(diǎn)取出以備創(chuàng)建地圖矩陣；

在獲得點(diǎn)集和道路線集之后，利用ArcGIS Engine提供的IRelationalOperator接口的Contain方法，判定點(diǎn)是否在道路線上，若在，則該點(diǎn)與此道路線上的所有點(diǎn)都是連通的，矩陣中的值為1，若不在，則在矩陣中設(shè)定為0。由此，即可創(chuàng)建出地圖矩陣；

(3)、A*及B樣條算法的實(shí)現(xiàn)：

在完成基于ArcGIS Engine的開(kāi)發(fā)后，即獲得了實(shí)現(xiàn)A*及B樣條算法的數(shù)據(jù)來(lái)源，首先依據(jù)地圖矩陣進(jìn)行A*算法尋路，得到最短路徑后再進(jìn)行B樣條曲線算法優(yōu)化路徑，使路徑更加光滑平穩(wěn)以便農(nóng)用機(jī)器人順暢運(yùn)行，過(guò)程如下：

3.1)A*算法尋路：根據(jù)實(shí)現(xiàn)A*算法的流程需要，首先創(chuàng)建開(kāi)啟和關(guān)閉列表分別用于存放未被查詢和已被查詢過(guò)的節(jié)點(diǎn)，對(duì)于開(kāi)啟列表的每一節(jié)點(diǎn)在地圖矩陣中查找與其相通且距離最短的相鄰節(jié)點(diǎn)，加入到關(guān)閉列表中；將相鄰節(jié)點(diǎn)的父節(jié)點(diǎn)設(shè)為當(dāng)前節(jié)點(diǎn)，并記錄該相鄰節(jié)點(diǎn)的G和F值；判斷當(dāng)終點(diǎn)節(jié)點(diǎn)加入到開(kāi)啟列表時(shí)，結(jié)束算法，路徑已被找到；

3.2)B樣條算法光滑路徑：得到最短路徑后，采用三次B樣條曲線生成算法，產(chǎn)生出連續(xù)、平滑的路徑曲線，以優(yōu)化最短路徑；首先得到最短路徑中的路徑點(diǎn)作為型值點(diǎn)，進(jìn)而求出控制點(diǎn)，每相鄰的兩型值點(diǎn)之間用一條B樣條曲線連接，而每段B樣條曲線由4個(gè)控制點(diǎn)組成，即可通過(guò)B樣條算法方程求出優(yōu)化曲線的路徑點(diǎn)。

本發(fā)明中采用的A*算法適合完全已知的環(huán)境信息，算法精巧、高效，在給定的代價(jià)函數(shù)和環(huán)境表示下，只要路徑存在，便定能找到一條最短路徑。

本發(fā)明實(shí)現(xiàn)了基于A*與B樣條算法的農(nóng)用機(jī)器人路徑規(guī)劃。首先分析研究路徑規(guī)劃算法，使用Matlab仿真論證了A*及B樣條算法在路徑規(guī)劃上的可行性；并使用Visual C#.NET與ArcGIS Engine結(jié)合編程設(shè)計(jì)實(shí)現(xiàn)了基于A*與B樣條算法的農(nóng)用機(jī)器人路徑規(guī)劃的軟件平臺(tái)，以供農(nóng)用機(jī)器人路徑規(guī)劃使用，實(shí)現(xiàn)農(nóng)用機(jī)器人根據(jù)最優(yōu)路徑運(yùn)行，從而提高農(nóng)業(yè)作業(yè)的高效化和智能化。

附圖說(shuō)明

圖1為基于A*算法的模擬果園路徑規(guī)劃圖。

圖2為基于三次B樣條曲線平滑A*算法已規(guī)劃出的路徑圖。

圖3為A*算法執(zhí)行流程圖。

圖4為路徑規(guī)劃軟件平臺(tái)示意圖。

圖5為A*算法尋路示意圖。

圖6為三次B樣條曲線光滑路徑示意圖。

具體實(shí)施方式

本發(fā)明原理包括：

一、A*算法原理：

A*算法是一種經(jīng)典的啟發(fā)式搜索算法，是基于Dijkstra算法基礎(chǔ)上的最佳優(yōu)先搜索算法。A*算法的最主要特征是在搜索選擇下一節(jié)點(diǎn)時(shí)引入了已知的路徑節(jié)點(diǎn)信息，計(jì)算所有候選節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的代價(jià)，本發(fā)明選取最短距離作為代價(jià)，以此為據(jù)進(jìn)而優(yōu)先選擇具有最短距離的節(jié)點(diǎn)作為下一個(gè)路徑節(jié)點(diǎn)，直到搜索到目標(biāo)節(jié)點(diǎn)。

建立A*算法的關(guān)鍵是確立如下形式的啟發(fā)函數(shù)：

f(n)＝g(n)+h(n) (1)

其中，f(n)為經(jīng)過(guò)候選節(jié)點(diǎn)n的最低代價(jià)解的估計(jì)代價(jià)值，g(n)為從起始節(jié)點(diǎn)到當(dāng)前節(jié)點(diǎn)n的實(shí)際花費(fèi)的代價(jià)，用起點(diǎn)到當(dāng)前節(jié)點(diǎn)的歐氏距離來(lái)表示；而h(n)為從當(dāng)前節(jié)點(diǎn)n到目標(biāo)節(jié)點(diǎn)的估計(jì)代價(jià)，在實(shí)驗(yàn)中將h(n)定義為當(dāng)前節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的歐氏距離。

假設(shè)在地圖中，選定起始節(jié)點(diǎn)S(S_x,S_y)，目標(biāo)節(jié)點(diǎn)T(T_x,T_y)，當(dāng)前節(jié)點(diǎn)C(C_x,C_y)，那么啟發(fā)式A*算法的估價(jià)函數(shù)即可表示為：

A*算法流程及仿真：

A*算法原理簡(jiǎn)單的說(shuō)就是從起始點(diǎn)，每走一步都選擇代價(jià)最小的節(jié)點(diǎn)走，直至終止點(diǎn)。具體實(shí)現(xiàn)步驟如下：

(1)創(chuàng)建Open和Close表，并將起始節(jié)點(diǎn)放入Open表中，Close表初始化為空；

(2)重復(fù)如下步驟：

a.在Open表中搜索f值最小的節(jié)點(diǎn)，并將該節(jié)點(diǎn)設(shè)為當(dāng)前節(jié)點(diǎn)；

b.把當(dāng)前節(jié)點(diǎn)從Open表中刪除，加入Close表中；

c.對(duì)當(dāng)前節(jié)點(diǎn)相鄰的每一個(gè)節(jié)點(diǎn)執(zhí)行：

①若該相鄰節(jié)點(diǎn)不可通行或已在Close表中，則什么也不執(zhí)行，繼續(xù)檢驗(yàn)下一節(jié)點(diǎn)；

②若該相鄰節(jié)點(diǎn)不在Open表中，則加入到Open表中，并將該相鄰節(jié)點(diǎn)的父節(jié)點(diǎn)設(shè)為當(dāng)前節(jié)點(diǎn)，并保存該節(jié)點(diǎn)的g值和f值；

③若該相鄰節(jié)點(diǎn)已在Open表中，則判斷由當(dāng)前節(jié)點(diǎn)到該相鄰節(jié)點(diǎn)的g值是否小于原存的g值，則將該相鄰節(jié)點(diǎn)的父節(jié)點(diǎn)設(shè)為當(dāng)前節(jié)點(diǎn),并重新設(shè)置該相鄰節(jié)點(diǎn)的g值和f值.

d.循環(huán)結(jié)束條件：

①當(dāng)目標(biāo)節(jié)點(diǎn)被加入到Open表中，表示路徑已被找到,此時(shí)應(yīng)終止循環(huán)；

②當(dāng)Open表為空，表明已無(wú)可以添加的新節(jié)點(diǎn),而已檢驗(yàn)的節(jié)點(diǎn)中沒(méi)有目標(biāo)節(jié)點(diǎn)則意味著路徑無(wú)法被找到,此時(shí)也結(jié)束循環(huán)。

(3)從目標(biāo)節(jié)點(diǎn)開(kāi)始沿父節(jié)點(diǎn)遍歷,直到起點(diǎn)，遍歷所得的節(jié)點(diǎn)就是最后得到的路徑；

為了證明該算法的可用性，本發(fā)明利用Matlab軟件進(jìn)行了仿真實(shí)驗(yàn)，如圖1所示，首先建立地圖為32m×32m的柵格地圖，建立直角坐標(biāo)系，以兩點(diǎn)間的歐氏距離為道路權(quán)重，假設(shè)每條道路上的速度是一致的。本發(fā)明地圖環(huán)境中，黑色空心點(diǎn)表示為可通行點(diǎn)，黑色實(shí)心點(diǎn)表示為障礙物點(diǎn)，設(shè)定起點(diǎn)為(3,3)，終點(diǎn)為(29,22)，運(yùn)用A*算法得出如圖1所示的路徑,最短路徑長(zhǎng)度為151.8857m。由圖可看出在路徑是由一些線段組成的，在某些路徑拐角處如(4,10)、(10,8)、(19,14)、(19,20)、(28,23)等處比較尖銳，不夠平滑，易導(dǎo)致農(nóng)用機(jī)器人在拐彎時(shí)因?yàn)檫\(yùn)動(dòng)方向突變傷害減速齒輪，由于減速打滑而帶來(lái)位置誤差。因此本發(fā)明采用三次B樣條曲線生成算法，產(chǎn)生出連續(xù)、平滑的路徑曲線，以優(yōu)化最短路徑。

二、B樣條算法平滑路徑：

B樣條曲線的特性之一為曲線會(huì)落在曲線階數(shù)的控制點(diǎn)所形成的凸多邊形內(nèi)。由于三次B樣條曲線在連接處二階連續(xù)，將其用于路徑規(guī)劃時(shí)，速度和加速度都是連續(xù)的。因此，采用三次均勻B樣條來(lái)平滑A*算法已規(guī)劃出的路徑。

n次B樣條曲線的數(shù)學(xué)表達(dá)式為：

式中，P_i為給定n+1個(gè)控制點(diǎn)P_i(i＝0,1,2,...,n)的坐標(biāo)，F(xiàn)_i,n(t)為n次B樣條基函數(shù)，其表達(dá)式為：

式中：

當(dāng)n＝3時(shí)，則有三次B樣條曲線的基函數(shù)為：

因此，三次B樣條曲線方程為：

用三次B樣條曲線來(lái)對(duì)基于A*算法的尖峰路徑進(jìn)行光滑處理，圖2為利用Matlab仿真生成的B樣條曲線。仿真實(shí)驗(yàn)初始設(shè)置條件為：n＝3,t∈[0,1],地圖為32m×32m。圖中黑實(shí)線為A*算法生成的最短路徑，虛線為B樣條曲線算法處理后的最優(yōu)路徑，由圖可看出經(jīng)過(guò)B樣條曲線的處理，(4,10)、(10,8)、(19,14)、(19,20)、(28,23)等處的路徑已不再尖銳，路徑相比基于A*算法生成的路徑更加光滑平穩(wěn)。由此可知，結(jié)合A*算法和B樣條曲線算法應(yīng)用于農(nóng)用機(jī)器人路徑規(guī)劃上是可行的。

在經(jīng)過(guò)對(duì)A*算法及B樣條曲線算法的理論證明后，得知利用這兩種算法生成最優(yōu)路徑是可行的。故本發(fā)明將利用C#結(jié)合ArcGIS Engine二次開(kāi)發(fā)編程實(shí)現(xiàn)農(nóng)用機(jī)器人基于A*和B樣條算法的路徑規(guī)劃軟件平臺(tái)。首先需要繪制用于農(nóng)用機(jī)器人路徑規(guī)劃的地圖；再基于ArcGIS Engine組件和C#語(yǔ)言的Windows桌面應(yīng)用軟件的開(kāi)發(fā)；在此基礎(chǔ)上編寫A*算法以實(shí)現(xiàn)機(jī)器人最短路徑規(guī)劃及B樣條曲線算法平滑路徑，進(jìn)而實(shí)現(xiàn)農(nóng)用機(jī)器人最優(yōu)路徑規(guī)劃，最后利用Socket網(wǎng)絡(luò)編程及3G模塊傳輸，將規(guī)劃好的路徑傳輸?shù)睫r(nóng)用機(jī)器人上，使機(jī)器人按該最優(yōu)路徑運(yùn)行，具體過(guò)程如下：

(1)、地圖數(shù)據(jù)的處理：

地圖數(shù)據(jù)的獲取及處理為基礎(chǔ)工作，是為了在基于ArcGIS Engine開(kāi)發(fā)中提供圖層數(shù)據(jù)。首先使用地圖下載軟件獲取地圖數(shù)據(jù)，并使用ArcMap軟件對(duì)下載的地圖數(shù)據(jù)進(jìn)行圖像配準(zhǔn)；再繪制道路shp圖，利用ArcToolbox工具箱中的數(shù)據(jù)管理工具--要素--要素轉(zhuǎn)點(diǎn)工具將道路圖轉(zhuǎn)化為道路關(guān)鍵點(diǎn)shp圖，兩圖通過(guò)Id屬性相關(guān)聯(lián)。由此即完成對(duì)地圖數(shù)據(jù)的處理，建立了用于路徑規(guī)劃的地圖。

(2)、基于ArcGIS Engine的開(kāi)發(fā)：

2.1)加載地圖文檔。在Visual Studio2010軟件中新建基于C#語(yǔ)言的Windows桌面應(yīng)用程序，并加入ArcGIS控件和ArcGIS Engine庫(kù)引用，由此便可開(kāi)發(fā)基于ArcGIS Engine的程序。在窗體中添加LicenseControl和AxMapControl控件，再放置一個(gè)名為“打開(kāi)文件”的button控件，編寫程序?qū)崿F(xiàn)將地圖數(shù)據(jù)加載到AxMapControl控件中。關(guān)鍵代碼如下：

2.2)添加選取路徑起點(diǎn)及終點(diǎn)功能。在窗體中添加“選取起點(diǎn)”和“選取終點(diǎn)”的button控件，并添加axMapControl1_OnMouseDown事件，編程使當(dāng)按下選取起點(diǎn)或終點(diǎn)按鈕時(shí)，利用ITextElement類的cre ateTextElement方法繪制起點(diǎn)或終點(diǎn)，并將該起點(diǎn)或終點(diǎn)通過(guò)axMapControl1.ActiveView.GraphicsContainer.AddElement和axMapControl1.Refresh方法繪制到地圖控件中。關(guān)鍵代碼如下：

2.3)創(chuàng)建地圖矩陣。在對(duì)路徑進(jìn)行A*算法規(guī)劃之前需要?jiǎng)?chuàng)建所需路徑，即地圖矩陣。首先獲取加載的地圖的圖層信息ILayer接口，由于IFeatureLayer是ILayer的子類，IFeature是IFeatureLayer中的一個(gè)要素，獲得IFeatureLayer后，可以從IFeatureLayer中得到IFeatureClass，再利用游標(biāo)IFeatureCursor就可以把IFeature一個(gè)個(gè)遍歷出來(lái)，即可把道路圖層中的每一條道路和道路關(guān)鍵點(diǎn)圖層中的每一點(diǎn)取出以備創(chuàng)建地圖矩陣。關(guān)鍵代碼如下：

在獲得點(diǎn)集和道路線集之后，利用ArcGIS Engine提供的IRelationalOperator接口的Contain方法，判定點(diǎn)是否在道路線上，若在，則該點(diǎn)與此道路線上的所有點(diǎn)都是連通的，矩陣中的值為1，若不在，則在矩陣中設(shè)定為0。由此，即可創(chuàng)建出地圖矩陣。關(guān)鍵代碼如下：

(3)、A*及B樣條算法的實(shí)現(xiàn)：

在完成基于ArcGIS Engine的開(kāi)發(fā)后，即獲得了實(shí)現(xiàn)A*及B樣條算法的數(shù)據(jù)來(lái)源。首先依據(jù)地圖矩陣進(jìn)行A*算法尋路，得到最短路徑后再進(jìn)行B樣條曲線算法優(yōu)化路徑，使路徑更加光滑平穩(wěn)以便農(nóng)用機(jī)器人順暢運(yùn)行。

3.1)A*算法尋路。根據(jù)上文中實(shí)現(xiàn)A*算法的流程需要，首先創(chuàng)建開(kāi)啟和關(guān)閉列表分別用于存放未被查詢和已被查詢過(guò)的節(jié)點(diǎn)，對(duì)于開(kāi)啟列表的每一節(jié)點(diǎn)在地圖矩陣中查找與其相通且距離最短的相鄰節(jié)點(diǎn)，加入到關(guān)閉列表中；將相鄰節(jié)點(diǎn)的父節(jié)點(diǎn)設(shè)為當(dāng)前節(jié)點(diǎn)，并記錄該相鄰節(jié)點(diǎn)的G和F值；判斷當(dāng)終點(diǎn)節(jié)點(diǎn)加入到開(kāi)啟列表時(shí)，結(jié)束算法，路徑已被找到。關(guān)鍵代碼如下：

圖3為代碼執(zhí)行流程圖，圖4為路徑規(guī)劃軟件平臺(tái)，圖5為A*算法尋路結(jié)果。圖4中標(biāo)注了起點(diǎn)及終點(diǎn)位置，并尋找到了從起點(diǎn)到終點(diǎn)的最短路徑，但由圖可看出在標(biāo)注點(diǎn)10、13、3處路徑尖銳，不夠平滑，將會(huì)引起農(nóng)業(yè)機(jī)器人運(yùn)行不便，故本發(fā)明將使用B樣條曲線算法平滑路徑。

3.2)B樣條算法光滑路徑。得到最短路徑后，采用三次B樣條曲線生成算法，產(chǎn)生出連續(xù)、平滑的路徑曲線，以優(yōu)化最短路徑。首先得到最短路徑中的路徑點(diǎn)作為型值點(diǎn)，進(jìn)而求出控制點(diǎn)，每相鄰的兩型值點(diǎn)之間用一條B樣條曲線連接，而每段B樣條曲線由4個(gè)控制點(diǎn)組成；由上文(6)式方程即可求出優(yōu)化曲線的路徑點(diǎn)，關(guān)鍵代碼如下：

圖6為三次B樣條曲線實(shí)現(xiàn)光滑路徑的效果圖，黑實(shí)線為A*算法求出的最短路徑，黑虛線為光滑路徑后的最優(yōu)路徑。由圖可看出上文提到的一些尖峰道路段(如10到13段、13到3段)已得到優(yōu)化，路徑更加平穩(wěn)光滑，農(nóng)用機(jī)器人將運(yùn)行的更加順暢。