詹京吳，黃宜慶

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

0 概述

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

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

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

本文在標(biāo)準(zhǔn)A*算法的啟發(fā)函數(shù)中引入安全距離因子，以此提高路徑的安全性。同時，采用平面結(jié)構(gòu)法對算法規(guī)劃出的路徑進行優(yōu)化，刪除冗余節(jié)點，減少轉(zhuǎn)折，并將優(yōu)化后的A*算法與動態(tài)窗口法結(jié)合，實現(xiàn)移動機器人對未知障礙物的動態(tài)避障，獲得實時避障的最優(yōu)平滑路徑。

1 安全A＊算法

1.1 對A＊算法的改進思路

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

定義安全距離因子如下：

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

將安全距離因子引入標(biāo)準(zhǔn)A*算法的估價函數(shù)中，可推得：

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

1.2 基于平面結(jié)構(gòu)法的路徑優(yōu)化

標(biāo)準(zhǔn)A*算法的路徑規(guī)劃是由連續(xù)的柵格中心連成的，這樣很容易導(dǎo)致這個最優(yōu)路徑存在冗余節(jié)點。根據(jù)相鄰節(jié)點與障礙物之間的位置關(guān)系可以判斷相鄰節(jié)點間是否存在障礙物，由此減少路徑拐點數(shù)，實現(xiàn)優(yōu)化路徑的目的［21］。

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

圖1 兩點位置關(guān)系判斷Fig.1 Relationship judgement of two points positions

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

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

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

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

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

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

2 本文融合方法

2.1 動態(tài)窗口法

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

移動機器人運動學(xué)公式推導(dǎo)如下：

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

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

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

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

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

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

為了與安全A*算法相結(jié)合，同時考慮導(dǎo)航方法的實時性，根據(jù)上文推導(dǎo)，更新動態(tài)窗口法的評價函數(shù)如下：

其中：κ為權(quán)重。

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

圖4 基于融合方法的機器人動態(tài)避障流程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 仿真平臺上驗證本文融合方法的有效性和可行性。首先依據(jù)上文環(huán)境模型的搭建基礎(chǔ)，設(shè)置20 m×20 m和30 m×30 m 的柵格地圖。使用文獻［25］提出的改進A*算法、本文提出的安全A*算法、標(biāo)準(zhǔn)A*算法以及Dijkstra 算法進行多組仿真對比實驗。仿真結(jié)果如圖5和圖6 所示，性能指標(biāo)如表1 和表2 所示。可以看出：在簡單環(huán)境下，本文算法較文獻［25］算法減少5.2%的路徑長度和5 個轉(zhuǎn)折點，縮短近43%的搜索時間；在復(fù)雜環(huán)境下，由于環(huán)境的復(fù)雜程度不相同，本文算法的路徑長度和搜索時間較文獻［25］算法有較大提升；4 種算法得到的轉(zhuǎn)折點相差不大，由于需要獲得安全的路徑，因此本文算法較標(biāo)準(zhǔn)A*算法會犧牲一定的距離代價，但每個路徑節(jié)點到最近障礙物的平均距離有所增加，增強了路徑的安全性。

表1 簡單環(huán)境下的路徑規(guī)劃性能對比Table 1 Comparison of path planning performance under simple environment

表2 復(fù)雜環(huán)境下的路徑規(guī)劃性能對比Table 2 Comparison of path planning performance under complex environment

圖5 簡單環(huán)境下的路徑仿真結(jié)果對比Fig.5 Comparison of path simulation results under simple environment

圖6 復(fù)雜環(huán)境下的路徑仿真結(jié)果對比Fig.6 Comparison of path simulation results under complex environment

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

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

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

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

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

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

3.2 實物驗證及分析

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

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

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

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

圖11 機器人導(dǎo)航過程Fig.11 Process of robot navigation

圖12 機器人狀態(tài)輸出Fig.12 Robot state output

4 結(jié)束語

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