基于改進A*與DWA算法融合的溫室機器人路徑規劃

勞彩蓮 李 鵬 馮 宇

(1.中國農業大學現代精細農業系統集成研究教育部重點實驗室， 北京 100083；2.中國農業大學農業農村部農業信息獲取技術重點實驗室， 北京 100083)

0 引言

隨著我國農業的發展，溫室技術變得高度智能化和高效化。在植株高度密集、高溫和高濕度甚至有毒的溫室環境下，不利于長時間進行人工作業，再加上勞動力不足和成本增加的影響，溫室機器人應用越來越多[1]。利用移動機器人完成溫室自主化作業符合精細化農業的需求，路徑規劃是移動機器人在溫室作業自主導航的重要前提[2-4]。根據機器人的作業范圍，可以將路徑規劃分為基于靜態全局環境的全局路徑規劃和基于局部障礙的局部路徑規劃[5]。常見的全局路徑規劃算法包括以Dijkstra[6-7]、A*[8-9]為代表的搜索規劃算法，以RRT[10]為代表的采樣規劃算法和以遺傳算法[11]為代表的啟發式規劃算法。依據實時傳感器數據的局部路徑規劃算法有人工勢場法[12]和動態窗口法[13]。

Dijkstra算法采用遍歷搜索方式，規劃節點數較多，節點網絡非常龐大，算法效率低下[14]。在Dijkstra算法的基礎上，A*算法引入目標點到當前點的估計代價，根據估計代價決定路徑搜索方向，提高了算法效率[15]。A*算法在已知環境下能快速實現移動機器人無碰、最短全局路徑規劃，主要通過節點狀態檢測和簡單的估值功能規劃出一條最佳的安全道路。但是，該算法忽略了機器人的運動約束，不能保證曲率連續，導致運動參量在拐點處發生跳變。文獻[16]動態計算出機器人的旋轉方向及角度，簡化了路徑,但增加了運行時間，實時性不高。文獻[17]使用跳點搜索減小了算法搜索面積，加快了速度，但是規劃出來的路徑拐點過多。文獻[18]采用直線-圓弧策略對路徑進行平滑處理,消除了鋸齒效應。文獻[19]使用微分方法減少了拐點數，但數學計算過多，比較耗時。文獻[20]優化了A*算法的啟發函數，改進了關鍵點的選取策略，減少了冗余的路徑點。文獻[21]提出對鄰域進行擴展，將8鄰域擴展到24鄰域，使機器人可以小角度行進，從而軌跡更加平滑。但這些改進方法沒有考慮未知的障礙物，不具備動態避障能力。

動態窗口法[13，22-23](Dynamic window approach， DWA)通過結合傳感器的數據進行在線實時局部路徑規劃，具有良好的避障能力，適用于動態環境中的機器人路徑規劃，但并不能滿足全局路徑的最優。將局部路徑規劃的算法結合到全局路徑規劃之中，在實際作業過程中，既考慮到環境中的障礙物，又能保證路徑規劃的完備性、實時性，滿足機器人的運動約束。

本文提出基于改進A*算法與動態窗口法結合的溫室機器人路徑規劃算法，利用選取策略對路徑進行優化。通過結合動態窗口算法與超聲波信息進行局部避障。在一維弦機器人操作系統上對改進A*算法與Dijkstra算法、A*算法、RRT算法進行仿真對比，驗證改進A*算法的有效性?？紤]機器人的實際尺寸，在真實環境下對柵格地圖進行膨脹化處理，并結合超聲波進行局部避障，完成真實環境下的機器人自主導航任務，并對算法的可行性進行驗證。

1 基于改進A*算法的路徑規劃

1.1 傳統A*算法

A*算法采用啟發式搜索與廣度優先算法結合，是靜態環境中用于求解最優路徑最有效的直接搜索算法。A*算法通過一個代價函數F(n)，選擇搜索方向，從起點開始向周圍擴展，通過啟發函數H(n)計算得到周圍每個節點的代價值，選擇最小代價值作為下一個擴展點，重復這個過程，直到到達終點，生成從起點到終點的路徑。在搜索過程中，由于路徑上的每一個節點都是具有最小代價節點，得到的路徑代價是最小的。A*算法的代價函數為

F(n)=G(n)+H(n)

(1)

式中G(n)——從起點到當前節點的實際路徑代價

F(n)——當前節點所在路徑從起點到終點預估的路徑代價總和

A*算法在執行路徑規劃時主要用2個表實現節點的擴展和最優點的選擇，即通過Openlist表和Closelist表來記錄節點。其中Openlist表作用是保存搜索過程中遇到的擴展節點，同時將這些節點按代價進行排序，取出F(n)值最小的節點，作為當前節點；然后將當前節點的所有鄰節點按鄰節點規則加入到Openlist表中。

1.2 A*算法優化策略

傳統A*算法根據載入的地圖，按照8鄰域搜索得到一系列的點，規劃出的路徑轉折次數太多，包含了一部分冗余點。如果轉折點過多，移動機器人每次只能移動很短的距離，會造成機器人卡頓現象。在實際的機器人移動過程中，單次指令下，機器人的運動距離越長，機器人移動流暢度越好，產生的誤差也會越少。這些問題造成在機器人的運動中平滑性欠缺，因此本文在傳統A*算法基礎上進行關鍵點的路徑優化策略。該策略能夠在柵格地圖上求解出更優的路徑，移動機器人路徑執行具有了更高效率。

傳統A*算法規劃的路徑如圖1a所示，其中起始點為綠色方格，終點為紅色方格，灰色方格為規劃路徑，從圖中可以看出，路徑轉折點過多。

本文先從A*算法規劃出的軌跡獲得一系列的點，然后選取當前節點運動方向相同的點，最終優化成一條線段。通過相鄰節點的斜率對機器人的運動方向進行判斷。從路徑規劃的第2個節點開始，如果當前節點與前一個節點的運動方向一致，則認為前一個節點為冗余的，刪除前一個節點，依次遍歷所有路徑的節點，刪除所有的冗余點。機器人只需執行包含起點、拐點的一系列路徑點，減少向機器人下達運動命令的次數，保證了機器人執行路徑平滑性。

經過關鍵點選取策略處理后的路徑如圖1b所示，其中點A到點B有18格子，機器人的運動方向都是朝下，將A與B之間的點優化成2個點，即點A和點B，刪除AB之間的冗余點，更新路徑，使得機器人運動較為流暢。使用相同的優化策略，形成BC、CD、DE段路徑，最終將路徑優化成沒有冗余點的最優路徑。經過改進A*算法的路徑平滑后，圖1a中的多余轉折點被消除，得到如圖1b所示的路徑。

2 動態窗口算法

在結構化溫室環境中，作物按照一定的規則進行放置，其相對位置已知，但存在一些易變因素，很難獲得完整的環境信息。為避免損害植物、保證機器人安全性，本文在全局路徑規劃的基礎上，采用動態窗口法進行局部路徑規劃。動態窗口法根據機器人超聲傳感器檢測到局部環境問題，進行實時的路徑規劃，具有良好避障能力。載入全局地圖后，通過改進A*算法進行全局路徑規劃，下達給機器人運動命令開始運動。由于機器人本身的運動學約束和環境因素影響，造成機器人在運動過程中產生誤差。動態窗口法主要是在速度空間內采樣線速度和角速度，并根據機器人的運動學計算預測其下一時間間隔的軌跡。對其獲得的待評價軌跡進行評分，從而獲得更加安全、平滑的最優局部路徑。

2.1 機器人運動模型

動態窗口法將移動機器人的位置控制轉換為速度控制[24]。在利用速度模式對機器人運動軌跡進行預測時，首先需要對機器人的運動模型進行分析。移動機器人采用的是兩輪差速模型，v(t)和w(t)分別代表機器人在世界坐標系下的平移速度與角速度，反映了機器人的運動軌跡。在機器人的編碼器采樣周期Δt內，位移較小，機器人作勻速直線運動，則機器人運動模型為

(2)

式中x(t)、y(t)、θ(t)——t時刻機器人在世界坐標下的位姿

2.2 速度采樣

動態窗口法將避障問題描述為速度空間中帶約束的優化問題，其中約束主要包括差速機器人的非完整約束、環境障礙物的約束以及機器人結構的動力學約束。DWA算法的速度矢量空間示意圖如圖2所示，橫坐標為機器人角速度w，縱坐標為機器人線速度v，其中vmax、vmin為機器人最大、最小線速度，wmax、wmin為機器人最大、最小角速度；整個區域為Vs，所有白色區域Va為機器人安全區域，Vd為考慮電機扭矩在控制周期內限制的機器人可達速度范圍，Vr為上述3個集合的交集最終確定的動態窗口。

根據機器人的速度限制，定義Vs為機器人線速度與角速度的集合，即動態窗口算法搜索求解的最大范圍，滿足

Vs={(v,w)|vmin≤v≤vmax,wmin≤w≤wmax}

(3)

整個機器人的運動軌跡，可以細分為若干個直線或圓弧運動，為保證機器人安全區域，在最大減速度條件下，當前速度應能在撞擊障礙物之前減速為0，則定義機器人碰撞可行區域的線速度與角速度集合Va滿足

(4)

dist(v,w)——軌跡上與障礙物最近的距離

由于機器人電機轉矩的限制，采樣周期Δt內存在機器人最大、最小可到達的速度v和角速度w范圍，需要進一步縮小動態窗口。在給定當前線速度vc和角速度wc條件下，下一時刻動態窗口Vd滿足

(5)

最終的速度范圍為上述3個速度的合集，定義動態窗口Vr滿足

Vr=Vs∩Va∩Vd

(6)

在速度矢量空間Vr中，根據線速度、角速度采樣點數，將連續的速度矢量空間Vr離散化，得到離散的采樣點(v,w)。對于每一個采樣點，根據機器人運動學模型預測下一時刻機器人的多個運動軌跡生成，如圖3所示。

2.3 評價函數

通過設計評價函數，在速度空間內有選取最優軌跡。在局部規劃中，保證最優軌跡靠近A*算法的全局路徑，完成避障任務，朝向目標快速運動。定義評價函數為

G(v,w)=k(αHeading(v,w)+βGoal(v,w)+
γPath(v,w)+σOcc(v,w))

(7)

式中k——平滑函數

α、β、γ、σ——各子函數的加權系數

Path(v,w)——路徑評價子函數，計算軌跡末端點到全局路徑的距離，距離越短說明軌跡越靠近全局路徑

Heading(v,w)——方位角不斷地朝向終點位置函數

Goal(v,w)——評價機器人局部路徑末端點到終點的距離函數，其目的是不斷縮短與終點的距離

Occ(v,w)——評價機器人軌跡到障礙物距離函數

當G(v,w)值最小時，獲得最佳路徑。本文中通過改進A*算法路徑規劃獲得全局路徑，如圖4中黑色實線所示，虛線為局部規劃路徑，之間的距離計算公式為

(8)

式中 (x1,y1)——機器人根據運動學模型估計的局部路徑末端點坐標

(x2,y2)——全局路徑規劃的節點坐標

在移動過程中，Head(v,w)函數用于使機器人的朝向不斷趨向終點方向，θ越小，說明與終點的方位角越小，其示意圖如圖5所示，計算公式為

Head(v,w)=180°-θ

(9)

Goal(v,w)函數用于評價機器人局部路徑末端與終點的距離，通過函數不斷縮短與終點的距離，其示意圖如圖6所示。

Occ(v,w)函數用于評價機器人軌跡到障礙物的距離，體現了機器人的避障能力，如果機器人的軌跡到障礙物的距離大于機器人半徑，則沒有發生碰撞的危險；反之，就說明碰撞風險大，舍棄這條軌跡。其示意圖如圖7所示，R為機器人底盤半徑，H為機器人軌跡到障礙物的距離。

3 融合算法

移動機器人超聲波檢測溫室局部環境信息，結合動態窗口法進行在線實時局部路徑規劃，檢測窗口滾動前進，具有避障能力，獲取局部的最優路徑。通過全局路徑規劃與局部路徑規劃結合，將改進A*算法進行全局路徑規劃與動態窗口算法融合，以保證動態規劃路徑的全局最優性。

在已知地圖上進行全局路徑規劃，根據當前速度、角速度、線速度計算并發布軌跡，通過評價標準下發各個軌跡，拋棄不合理的軌跡和遇到障礙的軌跡，運用4個評價函數對某一路徑進行評價，將各個評價函數累加，將成本最低的軌跡設定為最佳軌跡。如果當前最佳軌跡是可通過的，依照速度模式根據最佳軌跡的速度移動；如果遇到障礙，根據相關規則進行避障，融合算法流程如圖8所示。

4 仿真實驗與分析

4.1 改進A*算法仿真

Moro機器人是中國一維弦公司研發的移動機器人，其配套EwayOS機器人操作系統包含了SDK接口、運動算法等模塊，能夠對Moro機器人路徑規劃算法進行離線仿真，驗證其算法的有效性。本文構建5個尺寸一致的柵格地圖場景1～5，其水平方向上32個方格，豎直方向上29個方格，分辨率為10 cm×10 cm，其中黑色方塊代表環境中的障礙物信息，白色區域為可行區域。本文將改進A*算法與傳統A*、Dijkstra、RRT算法進行對比仿真實驗，設定相同的起點坐標為(0,0)，終點坐標為(31,28)，其仿真實驗結果如圖9～13所示。

5個柵格地圖場景障礙物的覆蓋率逐漸增大，復雜程度也逐漸增大，其中包含了規則和不規則障礙物，具有一定代表性。針對每個柵格地圖場景，本文使用4種路徑規劃算法運行10次，初始設置參數一致，得到10組路徑規劃算法的路徑長度、計算時間以及拐點數，取其平均值，結果如表1所示。

從表1來看，在5個不同柵格地圖場景中，4種路徑規劃算法都能最終規劃出路徑，但其路徑長度、計算時間、拐點數存在一定差異。RRT算法的計算時間最短，但RRT算法是由隨機樹節點組成，導致生成的路線轉折點過多，并非理想的平滑曲線，這對于機器人運動來說不利。當機器人運動是從一個節點走到另一個節點，首先需要在原地完成轉向后，轉向下一個節點再繼續行走，拐點過多或路徑過長，會造成機器人時間和能量的消耗。而改進A*算法、傳統A*算法、Dijkstra算法獲得的全局路徑較優。Dijkstra算法是一種遍歷算法，相對于改進A*算法更加耗時，尤其面對不規則障礙時，改進A*算法規劃的路徑是最優的。改進A*算法相對于傳統A*算法在計算時間、路徑長度和拐點數都有較大的改善。從實驗數據可以看出，改進A*算法有較好的實時性，路徑更短，路徑平滑性更好，便于機器人執行路徑軌跡達到目標點。

表1 不同路徑規劃算法的路徑長度、計算時間與拐點數對比Tab.1 Comparison of length, calculation time and number of inflection points of different path planning algorithms

4.2 環境模型處理

在算法仿真實驗中機器人理想化成一點，忽略了機器人本體尺寸，在真實環境中還要考慮機器人尺寸,避免機器人邊緣和障礙物發生碰撞，執行路徑失敗。用柵格圖構建機器人的工作環境，移動機器人在柵格間運動時，將固定障礙和機器人運動過程中超聲波檢測到的障礙設置為不可行區域。在此基礎上對地圖的障礙物進行膨脹化處理，保證機器人與障礙物間的安全區域，提高路徑的可行性。如圖14所示，膨脹后的路徑，設有安全區域，更利于移動機器人安全執行規劃后的路徑。

在已知溫室環境地圖基礎上，移動機器人通過超聲波獲取未知的動態障礙信息。本文使用的Moro機器人底盤前方的4個超聲波雷達，檢測范圍為0～250 cm。移動機器人在地圖中的位置已知，超聲波雷達將檢測到與動態障礙物的距離轉換到世界坐標系下，增加局部障礙物，將其加入全局地圖，機器人再根據融合算法進行局部路徑規劃。在實際環境中，如圖15a所示，Moro機器人遇到動態障礙物，超聲檢測到障礙物如圖15b所示，白色表示障礙，檢測到障礙物與機器人坐標原點的距離為80 cm,實際測量值為82 cm,測量結果誤差較小，可以滿足實際的精度要求。如圖16a所示,機器人對局部障礙進行膨脹處理，生成局部地圖，重新規劃路徑，避開障礙物；圖16b中障礙物厚度增加，綠色的線即為重新規劃出的路徑。

通過構建如圖17a所示環境，驗證機器人運動的定位精度，起點為綠色，終點為紅色。EwayOS機器人操作系統實時記錄融合算法的路徑，即圖17a中的灰色曲線與實際運動軌跡和圖17a中的黃色曲線。Moro機器人按照規劃路徑成功抵達終點，路線中存在不重合的地方，分析實時的跟蹤數據，如圖17b所示，機器人實際跟蹤誤差始終小于0.22 m,基本滿足實際需求。

5 真實環境下的實時路徑規劃

本文的融合算法適用于小范圍結構化環境，其環境具有一定的特殊性，選擇的3個與實驗相似的結構化環境1～3，對算法可行性進行驗證。利用Moro機器人分別在樓層走廊、模擬溫室環境和真實的溫室環境進行實驗。3個實驗環境地面光滑程度相似，其柵格地圖中黑色為機器人可以到達的無障礙空間，白色為障礙物信息，融合算法的實驗步驟描述為：①加載環境柵格地圖，設定機器人作業起點與目標點。②改進A*算法全局路徑規劃。通過機器人定位方法，獲取機器人實時位置信息，然后通過改進A*算法規劃機器人當前位置至目標點的全局路徑。③障礙物信息處理。機器人運動過程中通過超聲波雷達實時檢測環境障礙信息，并將傳感器信息轉換為機器人地圖上的障礙信息，得到膨脹后的地圖。④動態窗口法局部路徑規劃。根據機器人的當前位置信息，結合實時超聲波信息，在全局路徑的基礎上進行動態窗口法局部路徑規劃，獲得機器人最優運動軌跡，機器人按照最優軌跡對應的線速度與角速度執行運動。⑤根據機器人當前位置到目標點的距離判斷是否到達。如果未到達終點，則返回步驟③繼續運動；如果到達了目標點，機器人導航任務結束。

機器人的實驗參數設置如下：最大線速度為0.6 m/s,最大角速度為0.3 rad/s,線速度采樣個數為10，角速度采樣個數為20，采樣周期為0.2 s，子函數的加權系數α=0.1、β=10.0、γ=0.1、σ=0.2。

環境1為中國農業大學信息與電氣工程學院樓層走廊,地面為光滑地板。圖18a所示走廊實際長為10.5 m，寬為6.5 m,圖18b所示柵格地圖分辨率為10 cm×10 cm，起點為(1.0 m,1.0 m)，終點設為(9.1 m,4.2 m)，實際機器人路線如圖18b中紅線所示，終點為(9.3 m，4.0 m)，誤差為0.22 m。

環境2為模擬溫室盆栽環境，如圖19a所示，設置2排10個盆栽花盆，地面為光滑地板。場景實際長為6 m，寬為4 m, 圖19b所示柵格地圖分辨率為10 cm×10 cm，起點為(0.5 m,0.5 m)，終點設為(5.5 m,3.5 m)，實際機器人路線如圖19b中紅線所示，終點為(5.5 m,3.4 m)，誤差為0.1 m。

環境3為實際溫室環境，如圖20a所示，為復合薄膜型結構化溫室，地面為光滑的地板，有4排植物生長金屬框架。場景的實際長為8 m，寬為6 m，圖20b所示柵格地圖分辨率為10 cm×10 cm，起點為(0.5 m,0.5 m)，終點設為(7.0 m,2.5 m)，實際機器人路線如圖20b中紅線所示，終點為(7.1 m,2.3 m)，誤差為0.28 m。

從實驗結果來看，本文的融合算法能夠實現避障，繞過障礙物前進到目標點，且規劃的路徑能夠保持全局最優，機器人基本按照規劃路徑抵達目標。隨著場景面積變大和復雜程度增高，機器人到達的實際目標點與設定的目標點之間的誤差逐漸增大，主要是由于機器人轉彎時與地面打滑以及超聲波出現噪聲點造成的，其誤差不大于0.28 m，滿足實際需求。

6 結論

(1)根據環境信息已知的結構化溫室環境進行機器人路徑規劃，從路徑的平滑性、實時性和局部避障出發，提出一種基于改進A*算法與動態窗口法相結合的融合算法，在全局最優路徑的基礎上，進行實時避障，保證了路徑的平滑性和實時性。

(2)仿真結果表明，相較于傳統A*算法、Dijkstra算法和RRT算法，改進A*算法在兼顧實時性基礎上采取的關鍵點策略，使路徑更為平滑，符合實際機器人運動需求。

(3)通過對環境模型處理，保證機器人運動的安全性。首先，對柵格地圖的障礙進行膨脹化處理，結合超聲波傳感器對動態障礙進行檢測，并將膨脹化處理添加到全局地圖中。在仿真環境中對融合算法的有效性進行了驗證，結果表明，其跟蹤誤差保持在0.22 m以內。

(4)在3個相似的結構化環境下進行了Moro機器人實驗，結果表明，測量定位誤差不大于0.28 m，能夠滿足實際需求。驗證了融合算法在溫室機器人路徑規劃中的可行性。