郭園園,袁 杰,趙克剛

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

1 引言

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

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

本文針對路徑規(guī)劃領(lǐng)域存在的上述問題，提出了一種改進的A*算法與動態(tài)窗口法相結(jié)合的混合算法。首先，采用改進的A*算法在靜態(tài)環(huán)境中預(yù)先規(guī)劃出一條全局最優(yōu)路徑，以降低轉(zhuǎn)彎代價和減少遍歷節(jié)點數(shù);然后,根據(jù)規(guī)劃出的全局最優(yōu)路徑信息和環(huán)境中出現(xiàn)的移動障礙物和臨時障礙物信息，利用改進的動態(tài)窗口法完成局部路徑規(guī)劃，以實現(xiàn)規(guī)劃的實時性。

2 A*算法

2.1 環(huán)境模型的建立

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

Figure 1 Model of raster environment 圖1 柵格環(huán)境模型

2.2 傳統(tǒng)A*算法

A*算法的規(guī)劃原理主要是：首先，以柵格環(huán)境中的起始位置為開始端，搜索起始位置附近8個方向的子?xùn)鸥裎恢茫看螐倪@些子?xùn)鸥裎恢弥羞x擇一個函數(shù)評價值最小的子?xùn)鸥褡鳛橄乱粋€搜索的開始端，選中的子?xùn)鸥癖环Q為當(dāng)前節(jié)點；然后，再次搜索與當(dāng)前開始端相鄰近的子?xùn)鸥?，并從中選取函數(shù)評價值最小的子?xùn)鸥褡鳛樾碌漠?dāng)前節(jié)點，直到當(dāng)前節(jié)點就是終止位置所在的柵格。A*算法的評價函數(shù)記作f(n)，如式(1)所示：

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

(1)

其中,n為當(dāng)前節(jié)點，g(n)為起始點到當(dāng)前點的代價函數(shù),h(n)為當(dāng)前點到終止點的啟發(fā)函數(shù)。

3 改進A*算法

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

3.1 障礙規(guī)避策略

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

Figure 2 Obstacle avoidance strategy圖2 障礙規(guī)避策略

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

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

(2)

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

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

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

表1 障礙規(guī)避前后性能對比

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

3.2 遞歸二分法優(yōu)化策略

從圖3中規(guī)劃出的路徑可以看出，路徑經(jīng)過多次轉(zhuǎn)折，導(dǎo)致路徑不是最短的且平滑度較差，本文對A*算法規(guī)劃出的路徑采用遞歸二分法優(yōu)化策略刪除冗余節(jié)點，減少轉(zhuǎn)折度數(shù)和路徑長度。具體原理如下所示：

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

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

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

(3)

(4)

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

Figure 4 Optimization strategy of recursive dichotomy圖4 遞歸二分法優(yōu)化策略

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

從圖5可以看出，采用遞歸二分法優(yōu)化策略的A*算法規(guī)劃出的路徑長度和轉(zhuǎn)折次數(shù)明顯減少。

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

表2 去除冗余節(jié)點前后性能對比

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

3.3 動態(tài)內(nèi)切圓平滑策略

在靜態(tài)障礙環(huán)境下，路徑的安全性和平滑性是移動機器人路徑規(guī)劃需要考慮的重要指標(biāo)。由于將路徑的折線型優(yōu)化成弧線型更有利于機器人的移動，本文提出動態(tài)內(nèi)切圓平滑策略，將折線角優(yōu)化成弧線角。

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

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

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

Figure 6 Smoothing strategy of dynamic inscribed circle圖6 動態(tài)內(nèi)切圓平滑策略

(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是常數(shù)，分別為(yAxB-yBxA)，(xB-xA)xA+(yB-yA)yA和(yBxC-yCxB)。

(9)

(10)

(11)

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

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

(12)

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

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

(13)

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

(14)

根據(jù)三角形Bc2m2的邊角關(guān)系可得式(15)：

(15)

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

(16)

從選取的內(nèi)切圓c2應(yīng)滿足的第2個條件可知，內(nèi)切圓上的頂點坐標(biāo)為(xo1，yo1)，滿足如式(17)的關(guān)系式：

(17)

(18)

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

步驟4判斷用劣弧代替的折線的轉(zhuǎn)折點是否是規(guī)劃路徑的最后一個轉(zhuǎn)折點，如果不是，返回步驟1，繼續(xù)執(zhí)行后續(xù)路段的平滑處理；如果是，結(jié)束執(zhí)行。

4 改進動態(tài)窗口法

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

4.1 運動學(xué)模型

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

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

(19)

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

(20)

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

(21)

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

4.2 速度采樣空間

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

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

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

ω∈[ωmin,ωmax]}

(22)

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

(23)

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

(24)

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

4.3 改進評價函數(shù)

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

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

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

(25)

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

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

Figure 7 Obstacles near the target point圖7 目標(biāo)點附近存在障礙物

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

5 仿真與分析

為了分析評價本文的改進A*算法和混合算法在復(fù)雜環(huán)境下路徑規(guī)劃的質(zhì)量，在Matlab2018a環(huán)境下對本文提出的算法進行仿真驗證。

5.1 靜態(tài)環(huán)境下改進A*算法的仿真對比

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

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

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

Table 3 Comparison of performance of four algorithms in static environment表3 靜態(tài)環(huán)境下4種算法性能對比

5.2 動態(tài)環(huán)境下混合算法的仿真對比

將改進A*算法與動態(tài)窗口法相結(jié)合，先用改進A*算法規(guī)劃出一條全局最優(yōu)路徑，再使用動態(tài)窗口法進行局部避障，以解決動態(tài)環(huán)境下的路徑規(guī)劃問題。

5.2.1 凹型槽問題的仿真

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

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

5.2.2 臨時障礙物環(huán)境下的仿真

為了檢驗混合算法在動態(tài)環(huán)境下的規(guī)劃能力，在規(guī)劃好的全局最優(yōu)路徑上加入臨時障礙物，并與傳統(tǒng)的混合算法、文獻[15]中的混合算法進行仿真對比，結(jié)果如圖10所示，性能指標(biāo)對比如表4所示。

Figure 10 Comparison of path planning results of hybrid algorithms in temporary obstacle environment圖10 臨時障礙物環(huán)境下混合算法路徑規(guī)劃結(jié)果對比

表4 臨時障礙物環(huán)境下3種算法性能對比

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

5.2.3 移動和臨時障礙物環(huán)境下的仿真

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

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

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

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

6 結(jié)束語

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