一種果園履帶機器人路徑規劃方法與流程

本發明涉及機器人算法領域，具體是一種果園履帶機器人路徑規劃方法。

背景技術：

傳統的農業機械在農作過程中存在誤操作、能源利用率低及作業人員重負荷等問題。針對以上問題進行路徑規劃已經成為發展智能化農業機械的重要研究內容。路徑規劃是指根據某種最優原則(如：工作代價最小，行走路線最短，行走時間最短)，在工作空間中尋找一條從起始位置到目標位置的、避開障礙物的最優路徑。而針對農用機器人的路徑規劃是指合理而高效的搜尋一條在農田環境內除障礙物及農作物以外的最優無碰撞路徑。

路徑規劃的核心是算法的設計，目前路徑規劃算法已經得到了廣泛的發展。基于遺傳算法的路徑規劃是一種局部路徑規劃，適用于變化的環境，實時性較好，但規劃速度較慢及占用存儲空間較多；蟻群算法有很好的穩定性及適應性，能夠找到全局最優解，與遺傳算法相比，搜索時間顯著減少，但在處理較大區域的路徑規劃時，會出現停滯現象或陷入局部最優解；而最基礎的Dijkstra算法適用于小范圍的路徑搜索計算，較易實現，但遍歷計算節點多，效率低。

技術實現要素：

本發明的目的是提供一種果園履帶機器人路徑規劃方法，以解決現有技術機器人路徑算法存在的問題。

為了達到上述目的，本發明所采用的技術方案為：

一種果園履帶機器人路徑規劃方法，其特征在于：利用C#結合ArcGIS Engine二次開發編程實現農用機器人基于A*和B樣條算法的路徑規劃軟件平臺，首先需要繪制用于農用機器人路徑規劃的地圖；再基于ArcGIS Engine組件和C#語言的Windows桌面應用軟件的開發；在此基礎上編寫A*算法以實現機器人最短路徑規劃及B樣條曲線算法平滑路徑，進而實現農用機器人最優路徑規劃，最后利用Socket網絡編程及3G模塊傳輸，將規劃好的路徑傳輸到農用機器人上，使機器人按該最優路徑運行，具體過程如下：

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

地圖數據的獲取及處理為基礎工作，是為了在基于ArcGIS Engine開發中提供圖層數據；首先使用地圖下載軟件獲取地圖數據，并使用ArcMap軟件對下載的地圖數據進行圖像配準；再繪制道路shp圖，利用ArcToolbox工具箱中的數據管理工具--要素--要素轉點工具將道路圖轉化為道路關鍵點shp圖，兩圖通過Id屬性相關聯；由此即完成對地圖數據的處理，建立了用于路徑規劃的地圖；

(2)、基于ArcGIS Engine的開發：

2.1)加載地圖文檔：在Visual Studio2010軟件中新建基于C#語言的Windows桌面應用程序，并加入ArcGIS控件和ArcGIS Engine庫引用，由此便可開發基于ArcGIS Engine的程序，在窗體中添加LicenseControl和AxMapControl控件，再放置一個名為打開文件的button控件，編寫程序實現將地圖數據加載到AxMapControl控件中；

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

2.3)創建地圖矩陣：在對路徑進行A*算法規劃之前需要創建所需路徑，即地圖矩陣；首先獲取加載的地圖的圖層信息ILayer接口，由于IFeatureLayer是ILayer的子類，IFeature是IFeatureLayer中的一個要素，獲得IFeatureLayer后，可以從IFeatureLayer中得到IFeatureClass，再利用游標IFeatureCursor就可以把IFeature一個個遍歷出來，即可把道路圖層中的每一條道路和道路關鍵點圖層中的每一點取出以備創建地圖矩陣；

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

(3)、A*及B樣條算法的實現：

在完成基于ArcGIS Engine的開發后，即獲得了實現A*及B樣條算法的數據來源，首先依據地圖矩陣進行A*算法尋路，得到最短路徑后再進行B樣條曲線算法優化路徑，使路徑更加光滑平穩以便農用機器人順暢運行，過程如下：

3.1)A*算法尋路：根據實現A*算法的流程需要，首先創建開啟和關閉列表分別用于存放未被查詢和已被查詢過的節點，對于開啟列表的每一節點在地圖矩陣中查找與其相通且距離最短的相鄰節點，加入到關閉列表中；將相鄰節點的父節點設為當前節點，并記錄該相鄰節點的G和F值；判斷當終點節點加入到開啟列表時，結束算法，路徑已被找到；

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

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

本發明實現了基于A*與B樣條算法的農用機器人路徑規劃。首先分析研究路徑規劃算法，使用Matlab仿真論證了A*及B樣條算法在路徑規劃上的可行性；并使用Visual C#.NET與ArcGIS Engine結合編程設計實現了基于A*與B樣條算法的農用機器人路徑規劃的軟件平臺，以供農用機器人路徑規劃使用，實現農用機器人根據最優路徑運行，從而提高農業作業的高效化和智能化。

附圖說明

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

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

圖3為A*算法執行流程圖。

圖4為路徑規劃軟件平臺示意圖。

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

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

具體實施方式

本發明原理包括：

一、A*算法原理：

A*算法是一種經典的啟發式搜索算法，是基于Dijkstra算法基礎上的最佳優先搜索算法。A*算法的最主要特征是在搜索選擇下一節點時引入了已知的路徑節點信息，計算所有候選節點到目標節點的代價，本發明選取最短距離作為代價，以此為據進而優先選擇具有最短距離的節點作為下一個路徑節點，直到搜索到目標節點。

建立A*算法的關鍵是確立如下形式的啟發函數：

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

其中，f(n)為經過候選節點n的最低代價解的估計代價值，g(n)為從起始節點到當前節點n的實際花費的代價，用起點到當前節點的歐氏距離來表示；而h(n)為從當前節點n到目標節點的估計代價，在實驗中將h(n)定義為當前節點到目標節點的歐氏距離。

假設在地圖中，選定起始節點S(S_x,S_y)，目標節點T(T_x,T_y)，當前節點C(C_x,C_y)，那么啟發式A*算法的估價函數即可表示為：

A*算法流程及仿真：

A*算法原理簡單的說就是從起始點，每走一步都選擇代價最小的節點走，直至終止點。具體實現步驟如下：

(1)創建Open和Close表，并將起始節點放入Open表中，Close表初始化為空；

(2)重復如下步驟：

a.在Open表中搜索f值最小的節點，并將該節點設為當前節點；

b.把當前節點從Open表中刪除，加入Close表中；

c.對當前節點相鄰的每一個節點執行：

①若該相鄰節點不可通行或已在Close表中，則什么也不執行，繼續檢驗下一節點；

②若該相鄰節點不在Open表中，則加入到Open表中，并將該相鄰節點的父節點設為當前節點，并保存該節點的g值和f值；

③若該相鄰節點已在Open表中，則判斷由當前節點到該相鄰節點的g值是否小于原存的g值，則將該相鄰節點的父節點設為當前節點,并重新設置該相鄰節點的g值和f值.

d.循環結束條件：

①當目標節點被加入到Open表中，表示路徑已被找到,此時應終止循環；

②當Open表為空，表明已無可以添加的新節點,而已檢驗的節點中沒有目標節點則意味著路徑無法被找到,此時也結束循環。

(3)從目標節點開始沿父節點遍歷,直到起點，遍歷所得的節點就是最后得到的路徑；

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

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

B樣條曲線的特性之一為曲線會落在曲線階數的控制點所形成的凸多邊形內。由于三次B樣條曲線在連接處二階連續，將其用于路徑規劃時，速度和加速度都是連續的。因此，采用三次均勻B樣條來平滑A*算法已規劃出的路徑。

n次B樣條曲線的數學表達式為：

式中，P_i為給定n+1個控制點P_i(i＝0,1,2,...,n)的坐標，F_i,n(t)為n次B樣條基函數，其表達式為：

式中：

當n＝3時，則有三次B樣條曲線的基函數為：

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

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

在經過對A*算法及B樣條曲線算法的理論證明后，得知利用這兩種算法生成最優路徑是可行的。故本發明將利用C#結合ArcGIS Engine二次開發編程實現農用機器人基于A*和B樣條算法的路徑規劃軟件平臺。首先需要繪制用于農用機器人路徑規劃的地圖；再基于ArcGIS Engine組件和C#語言的Windows桌面應用軟件的開發；在此基礎上編寫A*算法以實現機器人最短路徑規劃及B樣條曲線算法平滑路徑，進而實現農用機器人最優路徑規劃，最后利用Socket網絡編程及3G模塊傳輸，將規劃好的路徑傳輸到農用機器人上，使機器人按該最優路徑運行，具體過程如下：

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

地圖數據的獲取及處理為基礎工作，是為了在基于ArcGIS Engine開發中提供圖層數據。首先使用地圖下載軟件獲取地圖數據，并使用ArcMap軟件對下載的地圖數據進行圖像配準；再繪制道路shp圖，利用ArcToolbox工具箱中的數據管理工具--要素--要素轉點工具將道路圖轉化為道路關鍵點shp圖，兩圖通過Id屬性相關聯。由此即完成對地圖數據的處理，建立了用于路徑規劃的地圖。

(2)、基于ArcGIS Engine的開發：

2.1)加載地圖文檔。在Visual Studio2010軟件中新建基于C#語言的Windows桌面應用程序，并加入ArcGIS控件和ArcGIS Engine庫引用，由此便可開發基于ArcGIS Engine的程序。在窗體中添加LicenseControl和AxMapControl控件，再放置一個名為“打開文件”的button控件，編寫程序實現將地圖數據加載到AxMapControl控件中。關鍵代碼如下：

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

2.3)創建地圖矩陣。在對路徑進行A*算法規劃之前需要創建所需路徑，即地圖矩陣。首先獲取加載的地圖的圖層信息ILayer接口，由于IFeatureLayer是ILayer的子類，IFeature是IFeatureLayer中的一個要素，獲得IFeatureLayer后，可以從IFeatureLayer中得到IFeatureClass，再利用游標IFeatureCursor就可以把IFeature一個個遍歷出來，即可把道路圖層中的每一條道路和道路關鍵點圖層中的每一點取出以備創建地圖矩陣。關鍵代碼如下：

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

(3)、A*及B樣條算法的實現：

在完成基于ArcGIS Engine的開發后，即獲得了實現A*及B樣條算法的數據來源。首先依據地圖矩陣進行A*算法尋路，得到最短路徑后再進行B樣條曲線算法優化路徑，使路徑更加光滑平穩以便農用機器人順暢運行。

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

圖3為代碼執行流程圖，圖4為路徑規劃軟件平臺，圖5為A*算法尋路結果。圖4中標注了起點及終點位置，并尋找到了從起點到終點的最短路徑，但由圖可看出在標注點10、13、3處路徑尖銳，不夠平滑，將會引起農業機器人運行不便，故本發明將使用B樣條曲線算法平滑路徑。

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

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