融合安全A*算法與動態窗口法的機器人路徑規劃

詹京吳，黃宜慶

（1.安徽工程大學 電氣工程學院，安徽 蕪湖 241000；2.安徽省電氣傳動與控制重點實驗室，安徽 蕪湖 241000）

0 概述

路徑規劃作為移動機器人運動中的重要組成部分，是機器人完成自主導航的核心技術。根據環境和目標的不同，路徑規劃可分為全局路徑規劃和局部路徑規劃，應用于全局路徑規劃的算法有Dijkstra 算法［1］、蟻群算法［2］、粒子群算法［3］、A*［4-5］算法等，應用于局部路徑規劃的算法有動態窗口法（Dynamic Window Approach，DWA）［6］、人工勢場法［7］等。

A*算法由HART 等［8］于1968 年提出，該算法通過啟發信息指引搜索方向，搜索時間短，搜索效率高，被廣泛應用于移動機器人的路徑規劃。但A*算法規劃出的路徑拐點多、不平滑，不利于機器人的運行。針對以上缺點，許多研究者對標準A*算法進行了改進。文獻［9］提出將標準A*算法的搜索鄰域從8 個提高到無限個，雖然解決了路徑規劃方向限定問題，但同時降低了搜索速度。文獻［10］提出采用跳點搜索方法對A*算法進行改進，優化了算法的搜索策略，并且采用貝塞爾曲線對路徑進行平滑優化，大幅提高了路徑的規劃效率，但算法是在全局環境已知的條件下進行擴展和搜索的，無法實現對未知障礙物的動態避障。文獻［11］提出將改進的人工勢場法與A*算法相結合，實現了對隨機障礙物的避讓，但人工勢場法所規劃出的路徑不夠平滑，不利于機器人的運行。動態窗口法結構簡單，規劃出來的路徑較為平滑，具有良好的避障能力，但算法容易陷入局部最優，無法獲得全局最優路徑［12］。文獻［13］提出一種改進的動態窗口法，根據激光雷達觀測到的障礙物信息選取較優方位角，從而得到通過密集障礙物區的最佳路徑，大幅提高了算法的運行效率。文獻［14］提出的斯坦利路徑跟蹤方法使用A*算法進行路徑規劃，得到了有效的路徑信息。文獻［15］通過提取A*算法規劃路徑的關鍵節點定義新的評價函數，解決了標準動態窗口法易陷入“C”形障礙物且規劃路徑不平滑的問題。

上述文獻雖然對A*算法進行了有效改進，但都忽視了機器人與障礙物之間的距離對機器人運行的影響，無法保證搜索路徑的安全性。文獻［16］提出一種有限損傷A*算法，該算法定義了一個損壞量，在損壞上限的范圍內尋找到一條次優路徑長度的安全路徑。文獻［17］提出一種動態生成啟發式的方法，避免A*算法陷入局部極小值，大幅提高了算法的運算效率。文獻［18］設計一種安全A*算法，通過加入安全距離矩陣的方法來獲得一條安全的規劃路徑，但該算法規劃出的路徑轉折點多，計算時間長。

本文在標準A*算法的啟發函數中引入安全距離因子，以此提高路徑的安全性。同時，采用平面結構法對算法規劃出的路徑進行優化，刪除冗余節點，減少轉折，并將優化后的A*算法與動態窗口法結合，實現移動機器人對未知障礙物的動態避障，獲得實時避障的最優平滑路徑。

1 安全A＊算法

1.1 對A＊算法的改進思路

A*算法是一種可根據定義的估價函數在靜態環境下進行全局路徑規劃的啟發式搜索算法，被廣泛用于移動機器人的路徑規劃研究。該算法在盡可能保證最優路徑的同時，能夠大幅減少搜索時間，提高路徑的搜索效率［19］。然而標準A*算法在規劃從起點到終點的路徑時，所得到的全局路徑無法保障安全性、冗余節點多，且運動軌跡轉折較多［20］。為提高路徑的安全性，本文定義安全距離因子并將其引入到標準A*算法的啟發式函數中，以此進行改進。

定義安全距離因子如下：

其中：lmax為移動機器人與障礙物之間接觸的最大距離；lmin為機器人與障礙物之間接觸的最小距離；li為機器人所在位置與障礙物之間的距離。

將安全距離因子引入標準A*算法的估價函數中，可推得：

其中：F(i)為第i個節點的代價值；G(i-1)為起點到第(i-1)個節點的實際代價值；li-1,i為第(i-1)到第i個節點與障礙物之間的距離；li,goal為第i個節點到目標點與障礙物之間的距離。

1.2 基于平面結構法的路徑優化

標準A*算法的路徑規劃是由連續的柵格中心連成的，這樣很容易導致這個最優路徑存在冗余節點。根據相鄰節點與障礙物之間的位置關系可以判斷相鄰節點間是否存在障礙物，由此減少路徑拐點數，實現優化路徑的目的［21］。

判斷路徑是否有障礙物的基本原理是線段相交定理。首先需要判斷點的位置關系，如圖1 所示，其中，A、B、C、D 分別為平面上的任意4 個點，代表障礙物附著節點，坐標分別為(0,0)、(xb,yb)、(xc,yc)、(xd,yd)。

圖1 兩點位置關系判斷Fig.1 Relationship judgement of two points positions

假設4 個點都在xoy平面上，則這4 個點的z軸值恒為0。μ、α、β分別代表圖1 所示的向量。μ與α的向量積、μ與β的向量積分別為：

其中：m、n為向量積的系數。規定垂直紙面向外的方向為正方向，根據右手定則可知，當m、n為負數時，α在μ的順時針方向，即C 在μ的順時針的方向；反之，C 在μ的逆時針方向。同理，可以判斷D 和β與μ的位置關系。由此可知：當m·n＜0 時，C 和D 位于μ的異側；當m·n＞0 時，C 和D 位于μ的同側。

線段相交的判定如下：在同一平面內的任意4 個點，由這4 點連接成2 條不同的線段。由上文介紹的點與線段的位置關系可以判斷除該線段的兩端點之外其他兩點的關系，由此可確定兩線段的關系。根據線段相交原理，將路徑中某一節點的相鄰節點連接成線段n，判斷該線段上是否存在障礙物。若無障礙物則剔除；反之則保留。判斷線段n與障礙物的對角線是否相交，若相交則有障礙物，該路徑不變；若不相交則沒有障礙物，將兩節點中后一個節點剔除，如圖2 所示，其中：直線路徑為原始算法生成的路徑；虛線路徑為剔除冗余節點后的路徑。

圖2 剔除冗余節點示例Fig.2 Example of removing redundant nodes

基于安全A*算法的機器人路徑規劃流程如圖3所示。

圖3 基于安全A＊算法的機器人路徑規劃流程Fig.3 Robot path planning procedure based on safety A＊algorithm

2 本文融合方法

2.1 動態窗口法

動態窗口法由于具備較強的局部避障能力、靈活性強等優點，被廣泛用于移動機器人的局部路徑規劃中。動態窗口法的基本思想是依據機器人的運動學模型和運動特征，在規劃過程中對機器人的速度窗口(vt,ωt)進行約束，然后根據速度窗口，結合機器人的運動學模型進行軌跡的推演，最后利用評價函數確定最優的運動軌跡，直至到達目的地［22］。機器人的搜索空間受到機器人的運動學約束Vk、動力學約束Vb和障礙物約束Vo的限制，相關約束描述如下［23］：其中：vmin和vmax表示機器人速度的上限和下限；ωmin和ωmax表示機器人角速度的上限和下限；vc是小車的當前線速度；ωc是當前角速度；分別對應線加速度的上限和下限；分別對應角加速度的上限和下限；dmin(v,ω)表示機器人在下一時刻到障礙物的距離。

移動機器人運動學公式推導如下：

其 中：［xt+dt，yt+dt，θt+dt］T表示時刻t+dt機器人在世界坐標系中的位置；［xt，yt，θt］T表示時刻t機器人在世界坐標系中的位置；［dx，dy，dθ］T表示時刻dt機器人的位置；底盤坐標系中的理想變化量［dx，dy，dθ］T=［vxdt，vydt，ωdt］T；vx和vy為機器人在底盤坐標系中沿x軸和y軸的線速度；ω表示移動機器人的角速度。

在計算多組采樣速度的軌跡后，應用評估函數進行評分并選擇最佳組［24］。評估函數描述如下：

其中：heading（v，ω）用于測量朝向目標的進度，當機器人直接移動到目標時，該進度最大；dist（v，ω）表示到靜態障礙物的最近距離；vel（v，ω）表示前進速度；ε、τ、γ是權重；σ是3個評價函數的歸一化參數。

2.2 融合安全A＊算法與DWA 的路徑規劃方法

通過設計路徑融合子函數fusion(v,ω)，擴展原始動態窗口法的評價函數，計算公式如下：

其中：(x1,y1)為動態窗口法基于采樣軌跡而推演出的局部路徑末端坐標；(x2,y2)為安全A*算法獲得的全局路徑節點坐標。

為了與安全A*算法相結合，同時考慮導航方法的實時性，根據上文推導，更新動態窗口法的評價函數如下：

其中：κ為權重。

通過采用更新后的評價函數，可以在全局地圖中獲取考慮實時避障的最優平滑路徑?；谌诤戏椒ǖ臋C器人動態避障流程如圖4 所示。

圖4 基于融合方法的機器人動態避障流程Fig.4 Robot dynamic obstacle avoidance procedure based on fusion method

3 仿真驗證與實物驗證

3.1 仿真驗證及分析

3.1.1 安全A*算法驗證

在Intel?CoreTMi5-10300H CPU @ 2.50 GHz 的Matlab 仿真平臺上驗證本文融合方法的有效性和可行性。首先依據上文環境模型的搭建基礎，設置20 m×20 m和30 m×30 m 的柵格地圖。使用文獻［25］提出的改進A*算法、本文提出的安全A*算法、標準A*算法以及Dijkstra 算法進行多組仿真對比實驗。仿真結果如圖5和圖6 所示，性能指標如表1 和表2 所示?？梢钥闯觯涸诤唵苇h境下，本文算法較文獻［25］算法減少5.2%的路徑長度和5 個轉折點，縮短近43%的搜索時間；在復雜環境下，由于環境的復雜程度不相同，本文算法的路徑長度和搜索時間較文獻［25］算法有較大提升；4 種算法得到的轉折點相差不大，由于需要獲得安全的路徑，因此本文算法較標準A*算法會犧牲一定的距離代價，但每個路徑節點到最近障礙物的平均距離有所增加，增強了路徑的安全性。

表1 簡單環境下的路徑規劃性能對比Table 1 Comparison of path planning performance under simple environment

表2 復雜環境下的路徑規劃性能對比Table 2 Comparison of path planning performance under complex environment

圖5 簡單環境下的路徑仿真結果對比Fig.5 Comparison of path simulation results under simple environment

圖6 復雜環境下的路徑仿真結果對比Fig.6 Comparison of path simulation results under complex environment

3.1.2 融合方法性能驗證及靈敏度分析

依據上文簡單環境的配置，設置融合方法規劃的參數。根據文獻［21］中的權值設置，通過多組仿真數據對比，選擇各個參數值為：ε=0.15，τ=0.25，γ=0.25，β=0.35。通過在環境中設置突然出現的障礙物，可以有效驗證融合方法的避障性能。對融合方法的驗證結果如圖7 所示。從圖7（a）中可以看出，當地圖中沒有隨機障礙物時，安全A*算法和安全A*融合動態窗口法都能規劃出一條從起始點到終點的路徑。當加入一個隨機障礙物時，如圖7（b）所示，顯然安全A*算法無法規避隨機加入的障礙物，而安全A*融合動態窗口法能有效識別隨機障礙物并進行動態避障。隨著障礙物的增加，如圖7（c）、圖7（d）所示，融合方法都能有效避開環境中加入的隨機障礙物，同時準確追蹤基于安全A*算法生成的全局路徑，從而驗證了本文融合方法的有效性和可靠性。

圖7 隨機障礙物避障路徑仿真結果Fig.7 Simulation results of random obstacle avoidance paths

路徑長度和時間隨加入障礙物個數的變化如圖8 所示，可以看出，隨著障礙物的增加，融合方法規劃出的路徑長度和規劃時間呈線性增加。機器人控制參數反饋如圖9 所示。

圖8 路徑長度和時間隨障礙物個數的變化Fig.8 Variation of path length and time with the number of obstacles

圖9 機器人角度、速度、角速度輸出結果Fig.9 Output results of robot angle，velocity and angular velocity

3.2 實物驗證及分析

借助自主搭建的機器人平臺對本文提出的融合方法進行驗證，如圖10 所示。

圖10 機器人驗證平臺和地圖環境Fig.10 Robot verification platform and map environment

機器人參數設置如下：RIKIBOT-四驅-主控-工控機-stm32 驅動板-思嵐A1 雷達組裝。

在實驗過程中，通過在環境中設置障礙物，驗證本文融合方法的有效性。首先機器人在靜止環境中進行導航，當機器人回到起點時，隨機改變障礙物的位置，驗證導航算法在動態環境中的導航性能。機器人導航過程如圖11 所示，可以看出，機器人在執行一個來回的任務時，雖然環境中的障礙物位置發生改變，但是機器人仍然可以安全抵達位置。輸出的機器人參數如圖12 所示，結果表明，本文融合方法具備實時避障的能力，且能安全地抵達目的地。

圖11 機器人導航過程Fig.11 Process of robot navigation

圖12 機器人狀態輸出Fig.12 Robot state output

4 結束語

針對標準A*算法規劃路徑冗余點多、安全性低以及無法在復雜環境下隨機避障的缺點，本文提出一種融合安全A*算法和動態窗口法的方法，充分利用兩種算法的優勢。與標準A*算法、文獻［25］改進A*算法和Dijkstra 算法的仿真對比結果表明，本文融合方法實現了路徑長度、運算效率、平滑性以及安全性的優化。在機器人平臺上的實驗結果也表明，本文融合方法能高效地完成實際環境中的路徑規劃任務，實現對隨機障礙物的有效避障。由于本文只研究了對于靜態未知障礙物的實時避障，未考慮到動態障礙物，因此下一步將會對動態窗口法進行改進，并在現有環境中加入動態障礙物，使本文融合方法在各種復雜環境中均能得到最優路徑。