基于改進A*算法和動態窗口法的機器人路徑規劃*

郭園園,袁 杰,趙克剛

(新疆大學電氣工程學院，新疆 烏魯木齊 830047)

1 引言

移動機器人的路徑規劃主要是搜索出一條無碰撞并且最優的路徑[1]。最為常見的路徑規劃研究主要是基于移動機器人的靜態工作環境，常見算法有群智能算法中的遺傳算法[2]、粒子群算法[3]、蟻群算法[4]和灰狼算法[5]等，以及經典算法中的人工勢場法[6]、RRT(Rapidly exploring Random Tree)算法[7]、A*算法[8]和Dijkstra算法[9]等。然而，機器人工作的地圖環境并不是固定不變的，既有可以事先感知到的靜態障礙物，也有無法預知的臨時障礙物和移動障礙物。移動機器人根據自身安裝的傳感器檢測到一定距離內出現的動態障礙物時，如果繼續依照先前在靜態環境下規劃出的路徑移動勢必會發生碰撞，導致移動機器人無法完成從起始點至終止點的路徑規劃。因此，當臨時或移動障礙物出現時，為了使機器人能夠繼續完成路徑規劃，對于移動機器人進行動態路徑規劃研究十分有必要。

國內外有眾多研究人員對機器人的動態路徑規劃研究做出了貢獻。文獻[10]提出了一種基于改進A*算法與動態窗口法的混合算法，有效地克服了傳統路徑轉折角多的缺點，但沒有考慮對臨時障礙物的處理。文獻[11]將入侵雜草算法融合到人工勢場算法中，在全局內能夠指示性地產生子目標點，引導移動機器人擺脫局部最優，但當目標點附近存在障礙物時，規劃路徑可能過長甚至規劃失敗。文獻[12]提出了一種改進的RRT算法，通過分支修剪、重新連接和重新生成過程來維護所有節點的有效性，但由于缺少全局最優路徑的指引導致遍歷空間過大和搜索時間過長。文獻[13]將深度強化學習應用于未知環境的動態路徑規劃，設計了獎懲功能和訓練方法，可以對移動障礙物實現避障，但局部規劃時獲取的不是最短路徑，使整體效率下降。

本文針對路徑規劃領域存在的上述問題，提出了一種改進的A*算法與動態窗口法相結合的混合算法。首先，采用改進的A*算法在靜態環境中預先規劃出一條全局最優路徑，以降低轉彎代價和減少遍歷節點數;然后,根據規劃出的全局最優路徑信息和環境中出現的移動障礙物和臨時障礙物信息，利用改進的動態窗口法完成局部路徑規劃，以實現規劃的實時性。

2 A*算法

2.1 環境模型的建立

為了解決在復雜環境下的路徑規劃問題，本文采用柵格法對機器人的工作環境進行建模。其中，已知的固定不變的靜態障礙物用黑色柵格表示；機器人可以任意行走的柵格用白色柵格表示。柵格環境模型如圖1所示。

Figure 1 Model of raster environment 圖1 柵格環境模型

2.2 傳統A*算法

A*算法的規劃原理主要是：首先，以柵格環境中的起始位置為開始端，搜索起始位置附近8個方向的子柵格位置，每次從這些子柵格位置中選擇一個函數評價值最小的子柵格作為下一個搜索的開始端，選中的子柵格被稱為當前節點；然后，再次搜索與當前開始端相鄰近的子柵格，并從中選取函數評價值最小的子柵格作為新的當前節點，直到當前節點就是終止位置所在的柵格。A*算法的評價函數記作f(n)，如式(1)所示：

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

(1)

其中,n為當前節點，g(n)為起始點到當前點的代價函數,h(n)為當前點到終止點的啟發函數。

3 改進A*算法

傳統A*算法規劃出的路徑有與障礙物棱角存在交點、路徑轉折點多和路徑不平滑等問題，對此本文提出了針對以上問題的改進策略。

3.1 障礙規避策略

傳統A*算法在選擇當前節點的下一節點時，會首先要求下一節點為非障礙節點，然后在非障礙節點集合中選擇評價函數值最小的節點作為當前節點的下一節點。這種選擇策略的弊端在于把下一個障礙節點兩側的節點當作可選節點處理了，導致規劃出的路徑(如圖2a中的路徑L2)與障礙物(如圖2a中的B1和B2)的棱角存在交點甚至會在2個障礙物(如圖2a中的B1和B3)的對接棱角之間規劃路徑(如圖2a中的路徑L1)，這樣降低了移動機器人行走的安全性。

Figure 2 Obstacle avoidance strategy圖2 障礙規避策略

針對上述問題，本文提出了障礙規避策略，其原理如圖2b所示。中心柵格為當前柵格，將周圍的8個節點分為2類：障礙節點影響集合(圖2b中和當前柵格挨著的4個障礙柵格所在節點)和障礙節點不影響集合(圖2b中的4個拐角處柵格所在節點)。在前者集合中，每個柵格元素兩側的柵格節點可能造成路徑與障礙柵格存在交點，將其作為當前柵格的障礙子柵格處理；在后者集合中，每個柵格元素斜對角處的柵格節點對規劃的路徑不會產生影響，不做處理。采用上述障礙規避策略后，在圖2a中規劃出的安全路徑如路徑L3所示。

以上過程可以用式(2)描述說明：

(2)

其中,L-pipi+1表示向量pipi+1的大小，即點pi到pi+1的路徑長度；‖pipi+1‖1是向量pipi+1的L1范數；‖pipi+1‖2是向量pipi+1的L2范數；P是本節所定義的障礙節點影響集合；Q是本節所定義的障礙節點不影響集合。

為了驗證障礙規避策略的有效性，本文在20×20的柵格地圖中與傳統A*算法進行了仿真對比實驗，規劃結果如圖3所示，性能對比如表1所示。

Figure 3 Path before and after obstacle 圖3 障礙規避前后的路徑

表1 障礙規避前后性能對比

從圖3可以看出，采用障礙規避策略的A*算法規劃出的路徑離障礙物有足夠的距離，機器人移動起來更加安全。從表1可以看出，采用了障礙規避策略后，總遍歷節點數減少了31%，運行時間縮短了15%，考慮安全距離后路徑長度增加8%，但從路徑的安全性上考慮，改進效果明顯。

3.2 遞歸二分法優化策略

從圖3中規劃出的路徑可以看出，路徑經過多次轉折，導致路徑不是最短的且平滑度較差，本文對A*算法規劃出的路徑采用遞歸二分法優化策略刪除冗余節點，減少轉折度數和路徑長度。具體原理如下所示：

(1)判斷2點之間的連線有無障礙物。如圖4a所示，判斷A點和E點之間是否存在障礙物，已知A、E2點的信息，可以確定A點和E點的中點C的信息，以此中點分別向兩邊遞歸查找，分別得到AC線段的中點B和CE線段的中點D,判斷B點和D點所在的柵格的代價值(障礙柵格代價值為1，自由柵格代價值為0)，通過計算可以得出，這2點的代價值均為1，表明A點和E點的連線上存在障礙物。

(2)剔除直線冗余點。如圖4b所示，去除實線的冗余節點，首先將A點作為初始判斷節點，從第3個節點C點開始判斷，看到A點和C點之間的連線不存在障礙物，說明第2個節點B點是冗余節點，將其剔除，C點成為新一輪的第2個節點；對新一輪的第3個節點D點進行判斷，發現在A點與D點中間并不存在障礙，說明新一輪的第2個節點C點為冗余節點，將其去除，則D點成為下一輪的第2個節點；以此類推，直到F點成為第2個節點，這時G點和A點的連線之間存在障礙物，將F點作為轉折點保存下來，G點是目標點，循環到此結束，最終判斷出來的路線節點為：A、F和G。

以上過程可以用式(3)和式(4)描述說明：

(3)

(4)

其中,L={Li|i=1,2,…,N},表示未采用優化策略之前路徑點的集合,N表示路徑上的節點數；Lr為單元柵格的代價值，障礙柵格代價值為1，自由柵格代價值為0；{xLi+j|j=1,2,…,xLi+2-xLi-1}為路徑點Li和Li+2之間的單位距離的點的集合；O為障礙柵格的集合；F為自由柵格的集合；LU{Li+1}為集合{Li+1}相對于集合L的補集;l表示采用優化策略之后路徑點的集合。

Figure 4 Optimization strategy of recursive dichotomy圖4 遞歸二分法優化策略

為了驗證遞歸二分法優化策略在去除冗余節點方面的有效性，本文在20×20的柵格地圖中與只采用了障礙規避策略的A*算法進行了仿真對比，規劃結果如圖5所示，性能對比如表2所示。

從圖5可以看出，采用遞歸二分法優化策略的A*算法規劃出的路徑長度和轉折次數明顯減少。

Figure 5 Path before and after removing redundant points圖5 去除冗余節點前后路徑

表2 去除冗余節點前后性能對比

從表2可以看出，去除冗余節點后，總轉折次數減少46%，路徑長度縮短6%，總轉折角度減小57%，改進效果明顯。

3.3 動態內切圓平滑策略

在靜態障礙環境下，路徑的安全性和平滑性是移動機器人路徑規劃需要考慮的重要指標。由于將路徑的折線型優化成弧線型更有利于機器人的移動，本文提出動態內切圓平滑策略，將折線角優化成弧線角。

基于動態內切圓平滑策略規劃出的某條路徑如圖6所示，經過節點A(xA，yA)、B(xB，yB)、C(xC，yC)和D(xD，yD)，路徑的起始位置為A，終止位置為D。具體實現步驟如下所示：

步驟1計算路段AB和路段BC的長度，再選取較短路段AB的首端點A為第一次切點，過點A的垂線與∠B的平分線交于點c1(xc1，yc1)，即過A的內切圓的圓心。

過點A和點B的一般式直線方程如式(5)所示：

Figure 6 Smoothing strategy of dynamic inscribed circle圖6 動態內切圓平滑策略

(yB-yA)x+(xA-xB)y+C1=0

(5)

直線Ac1和AB垂直，其一般式直線方程如式(6)所示：

(xA-xB)x+(yA-yB)y+C2=0

(6)

過點B和點C的一般式直線方程如式(7)所示：

(yC-yB)x+(xB-xC)y+C3=0

(7)

由點n1在直線BC上，可得式(8)：

(yC-yB)xn1+(xB-xC)yn1+C3=0

(8)

式(5)～式(7)中的C1、C2和C3是常數，分別為(yAxB-yBxA)，(xB-xA)xA+(yB-yA)yA和(yBxC-yCxB)。

(9)

(10)

(11)

由式(9)～式(11)聯立可求得點n1的坐標，進而得直線n1c1的一般式方程,如式(12)所示：

(xn1-xB)x+(yn1-yB)y+C4=0

(12)

其中，C4是常數，其值為(xB-xn1)xn1+(yB-yn1)yn1，聯立式(6)和式(12)可求得交點c1的坐標。

圓心c2到直線AB的距離如式(13)所示：

(13)

由余弦定理可求得∠B的角度：

(14)

根據三角形Bc2m2的邊角關系可得式(15)：

(15)

圓c2的方程設為式(16)的形式：

(16)

從選取的內切圓c2應滿足的第2個條件可知，內切圓上的頂點坐標為(xo1，yo1)，滿足如式(17)的關系式：

(17)

(18)

當式(18)取等號時，內切圓c2恰好與障礙物的頂點相交；當式(18)取大于號時，障礙物的頂點在內切圓的內部。

步驟4判斷用劣弧代替的折線的轉折點是否是規劃路徑的最后一個轉折點，如果不是，返回步驟1，繼續執行后續路段的平滑處理；如果是，結束執行。

4 改進動態窗口法

在動態環境下進行路徑規劃時，需要考慮突然加入的臨時障礙物和活動的移動障礙物產生的影響，這就要求機器人能夠局部實時避障，對此本文引入了動態窗口法來解決這個問題。

4.1 運動學模型

動態窗口法的思路是在由線速度和角速度組成的集合{(ν，ω)}中采樣多組速度，并分別模擬這些速度在下一個周期內的軌跡，然后通過評價函數對各組軌跡進行評估選取最優軌跡。因此，可得如式(19)～式(21)所示的運動學模型：

xt+1=xt+ν·Δt·cos(θ(t))

(19)

yt+1=yt+ν·Δt·sin(θ(t))

(20)

θ(t+1)=θ(t)+ω·Δt

(21)

其中,xt+1是t+1時刻的運動坐標，ν是時間間隔Δt內的線速度，ω是時間間隔Δt內的角速度，θ(t)表示機器人的運動方向與水平方向的夾角。

4.2 速度采樣空間

在移動機器人的速度空間中，存在多個速度組(ν，ω)。但是，因為機器人受到自身硬件和外在環境約束其速度被限制在一定范圍內，約束條件如下所示：

(1)移動機器人的速度約束如式(22)所示：

Vm={(ν,ω)|ν∈[νmin,νmax],

ω∈[ωmin,ωmax]}

(22)

(2)在預測時間間隔內受電機加減速性能約束,如式(23)所示：

(23)

(3)為保證機器人實現安全動態避障，需要與障礙物發生碰撞前以最大減速度，將速度降至0，其剎車速度約束如式(24)所示：

(24)

其中，dist(ν，ω)表示機器人在速度(ν，ω)下與障礙物的最近距離。

4.3 改進評價函數

滿足約束條件的速度采樣空間內，仍有一些對應的預測軌跡是可行的，本文通過評價函數對這些預測軌跡進行評估。對于傳統的評價函數，存在以下問題：目標點附近存在障礙物導致路徑變長甚至規劃失敗；遇到凹型槽類障礙物易陷入局部最優；與A*算法相結合導致局部路徑與全局最優路徑距離較遠。針對上述問題，本文在評價函數中加入了軌跡末端到目標點的距離和軌跡末端到全局最優路徑的距離，改進后的評價函數如式(25)所示：

G(ν,ω)=σ(α·DP(ν,ω)+β·DT(ν,ω)+

δ·DO(ν,ω)+γ·V(ν,ω))

(25)

其中,DP(ν，ω)為結合全局最優路徑的方位角評價函數，表示預測軌跡末端到全局最優規劃路徑的最短距離；DT(ν，ω)表示預測軌跡末端到目標點的歐幾里得距離；DO(ν，ω)表示預測軌跡末端到障礙物的距離；V(ν，ω)為預測速度大小的評價函數。加權系數包括α,β,δ,γ,σ∈(0,1)，且β+δ=1。

為了驗證動態窗口法中加入軌跡末端到目標點距離的有效性，在20×20的柵格地圖中進行了仿真實驗，規劃結果如圖7所示。

Figure 7 Obstacles near the target point圖7 目標點附近存在障礙物

從圖7可以看出，當目標點附近存在障礙物時，傳統的動態規劃算法檢測到當前點與障礙物的距離過近，DO(ν，ω)所占的比重變大，軌跡末端遠離障礙物，導致在目標點附近循環轉圈致使路徑規劃失敗；改進的動態窗口法規劃的軌跡快到達目標點時，DT(ν，ω)所占的比重變大，使障礙物對軌跡末端的影響變小，可以直接到達目標點。

5 仿真與分析

為了分析評價本文的改進A*算法和混合算法在復雜環境下路徑規劃的質量，在Matlab2018a環境下對本文提出的算法進行仿真驗證。

5.1 靜態環境下改進A*算法的仿真對比

為檢驗所提改進A*算法在地圖環境中進行路徑規劃的有效性，將動態內切圓平滑策略分別應用到文獻[14]的蟻群算法、Dijkstra算法和傳統的A*算法中并進行仿真，得到的結果如圖8所示，改進A*算法與其他算法的性能對比如表3所示。

Figure 8 Simulation comparison of four algorithms in static environment圖8 靜環境下4種算法仿真對比

從圖8中可以看出，改進A*算法規劃出的路徑更加平滑和安全，結合表3的數據，與傳統A*算法相比，在轉折角度和遍歷節點數方面分別減少了27.5 %和28.2%；對節點的特殊處理使路徑長度僅減少了4.4%；與其他算法相比本文改進A*算法效果提高明顯。綜合來看，對A*算法的改進增加了路徑的平滑性，縮小了搜索空間，縮短了運行時間，增加了安全性。

Table 3 Comparison of performance of four algorithms in static environment表3 靜態環境下4種算法性能對比

5.2 動態環境下混合算法的仿真對比

將改進A*算法與動態窗口法相結合，先用改進A*算法規劃出一條全局最優路徑，再使用動態窗口法進行局部避障，以解決動態環境下的路徑規劃問題。

5.2.1 凹型槽問題的仿真

動態窗口法在面對凹型槽類障礙物時，容易陷入局部最優，在凹型槽內180°翻轉循環會導致路徑規劃失敗。將動態窗口法與A*算法相結合后，借助于A*算法的全局最優路徑(虛線)指引，動態窗口法在局部規劃時能夠跳出局部最優到達目標點，仿真對比結果如圖9所示。

Figure 9 Simulation result of concave groove problem圖9 凹型槽問題仿真結果

5.2.2 臨時障礙物環境下的仿真

為了檢驗混合算法在動態環境下的規劃能力，在規劃好的全局最優路徑上加入臨時障礙物，并與傳統的混合算法、文獻[15]中的混合算法進行仿真對比，結果如圖10所示，性能指標對比如表4所示。

Figure 10 Comparison of path planning results of hybrid algorithms in temporary obstacle environment圖10 臨時障礙物環境下混合算法路徑規劃結果對比

表4 臨時障礙物環境下3種算法性能對比

從圖10中的規劃結果來看，傳統混合算法偏離全局最優路徑(虛線)較遠，在保障移動安全的情況下，本文混合算法比文獻[15]的混合算法更加貼合全局最優路徑。從表4中得出，與傳統混合算法和文獻[15]混合算法相比，本文混合算法路徑長度分別縮短了13.2%和4.1%，運行時間分別縮短了65.8%和36.0%，說明本文所提算法在路徑長度和運行時間上均優于其他2種算法。

5.2.3 移動和臨時障礙物環境下的仿真

為了進一步檢驗混合算法的有效性，在規劃好的全局最優路徑上加入臨時障礙物的同時，在柵格地圖中再加入移動障礙物，并增加地圖環境的復雜度。與傳統的混合算法、文獻[15]中的混合算法進行仿真對比，結果如圖11所示，性能對比如表5所示。

Figure 11 Comparison of path planning results of hybrid algorithms in moving and temporary obstacles environment圖11 移動和臨時障礙物環境下混合算法路徑規劃結果對比

表5 移動和臨時障礙物環境下3種算法性能對比

從圖11中的規劃結果來看，3種混合算法均能躲避移動障礙物，本文混合算法比其他2種混合算法更加貼合全局最優路徑(虛線)。從表5中可知，與傳統混合算法和文獻[15]中混合算法相比，本文混合算法的路徑長度分別縮短了13.9%和5.1%，運行時間分別縮短了44.9%和19.8%，說明本文所提算法在路徑長度和運行時間上均優于其他2種算法。

6 結束語

為了提升移動機器人在復雜環境下路徑規劃的效率，本文提出了一種改進A*算法和動態窗口法相混合的算法。在靜態環境下，在改進A*算法的基礎上結合障礙規避策略提升路徑安全性，結合去除冗余節點策略減小轉折角度，再結合動態內切圓平滑策略增加路徑平滑度；在動態環境下，改進動態窗口法實時規劃路徑，彌補了A*算法在實時規劃方面的不足，在躲避臨時和移動障礙物時能規劃出一條最優路徑。通過實驗與其他算法進行性能對比，驗證了本文所提算法的有效性。下一步計劃在特定的場合下進一步采用定量的理論分析來說明路徑規劃的收斂情況，或把該算法運用在多機器人的協同作業或物流機器人的配送上以進一步驗證該算法在現實環境中規劃的路徑質量。