動態路徑規劃形式化集成建模方法

曹 凱，奉 柳，劉秉政，沈 鵬，楊 旭

(山東理工大學 交通與車輛工程學院，山東 淄博 255049)

靜態環境中機器人的路徑規劃問題主要解決以下3個問題：確定機器人出發的初始點和到達的目標點；規劃機器人從初始點到達目標點同時躲避靜止障礙物的軌跡；控制機器人追蹤軌跡到達目標點。然而，動態環境中的路徑規劃問題會更加復雜，它需要面對移動的障礙物不斷地評估和調整規劃方案，以適應新的環境動態變化，這種動態環境中的路徑規劃方法對于機器人的開發才更有實用價值。

已有很多種動態環境路徑規劃方法被廣泛應用，常用的包括人工勢場法、粒子群法、概率路線圖方法以及一些改進的啟發式方法等。其中，人工勢場法通過創建勢能地形，并利用勢能梯度下降進行導航和局部規劃。該方法結構簡單、執行時間短，便于底層實時控制，廣泛應用于動態環境中多機器人問題。例如，多無人機避碰問題[1-2]、足球機器人路徑規劃問題[3-4]和移動機器人[5-8]等等。但是，其在路徑尋優過程中還存在著可能陷入布局最優的局限性。為此，本文提出將形式化隨機可達集建模方法與人工勢場法相結合，在機器人與多個移動障礙物之間建立Ad-Hoc網絡，提供機器人與移動障礙物之間的相對動態關系更加準確的表達方式和安全評估方法，從而提高動態環境中機器人避障路徑規劃的成功率。

隨機可達性分析通過在有限時間內評估系統狀態是否保持在狀態空間的期望子集內，或者躲避狀態空間的不期望子集來判斷系統狀態的安全性，為動態系統的安全驗證提供了一種有效方法。然而，隨機可達集的計算復雜度隨系統狀態維度增加呈指數級增長，因此利用其在動態環境中評估障礙物移動碰撞概率是不可行的。為此借鑒文獻[9]提出采用離線計算在線查詢的方法。然而，這種離線計算在線查詢的方法對移動障礙物反應的靈敏度有限，因而易受到障礙物移動沖突的影響。

為此，本文利用隨機可達集遍歷系統動態的表現優勢以及人工勢場法在動態環境中局部規避較為靈活的特點，采用預先離線成對計算機器人與每個移動障礙物的隨機可達集，并為每個障礙物構筑斥力場，然后在路徑規劃階段在線查詢。這里隨機可達集的任務是提供機器人與移動障礙物之間碰撞概率的精確描述，而人工勢場法依據斥力梯度提供機器人與障礙物碰撞的可能性。

實際上，在具有多個障礙物的動態環境中，由于是孤立地為每一個移動障礙物計算可達集，所以多個可達集的交集顯然不能提供嚴格的安全保證。盡管存在這種限制，但是與其他Ad-Hoc法相比，由于是相對于機器人-障礙物動態計算隨機可達集，因此隨機可達集為碰撞概率與斥力場的關聯提供了一種形式化方法。也就是說，可以使用Ad-Hoc方法生成可比較的斥力勢場，然后將這種斥力勢場與障礙物和機器人的相對運動聯系起來。

1 基本假設

1.1 障礙物動態

為了提高計算隨機可達集的時效性，本文采用降低維度的方法。為此，在所觀察的動態環境中，假設移動障礙物具有如下特性：移動障礙物的速度具有隨機性；移動障礙物遵循的軌跡為直線、弧線或由直線和弧線疊加的任意曲線。

(1)

而對于具有角速度w∈W和半徑r∈R+的弧線運動，動態被離散化為

(2)

1.2 機器人-障礙物相對動態

(3)

(4)

式中：us是速度；uw是獨輪車的角速度。

用時間步長Δ離散化機器人動態式(3)的結果為

(5)

離散化機器人動態式(4)的結果為

(6)

(7)

式中：fr(·)由式(5)和式(6)給出；fo(·)由式(1)和式(2)給出，那么碰撞可以定義為

(8)

1.3 隨機可達集

(9)

(10)

Δfo(w,n))p(w)

Δfo(w,n))p(w)}

(11)

確定躲避碰撞的系統最優控制輸入u。

(12)

式中Bi為對應于機器人避免與障礙物i碰撞的事件集合。因此，需要分別檢查機器人與每個障礙物的避碰概率，并且取避碰概率的最小值為總體避碰概率的上界。雖然這個避碰概率上限不一定能完全保證安全，但它至少可以告知機器人哪條路徑更可能躲避碰撞。由于本文的目的是在眾多可選路徑中找到一條具有更高成功率的到達目的地路徑，而不是從理論上保證無沖突路徑，因此上限式(12)是適合的。

2 算法集成

為了將隨機可達集與人工勢場法集成使用，需要利用隨機可達集生成障礙物梯度和引向目標的梯度。為此必須先改進隨機可達集以適應人工勢場法，即將隨機可達集合并到梯度計算中，然后更新機器人的控制律。

使用隨機可達集來預示勢場變化的一大難點是式(11)最優值可能存在非平滑點。一般來說，這種平滑性是難以保證的。這一點可以通過圖1給出的驗證結果來說明。該圖驗證的是在直線運動模式下，機器人-障礙物相對動態的隨機可達集平滑性。驗證從坐標原點開始，每圖右面的彩色柱狀圖表明碰撞概率由小到大的變化程度。機器人動態分別采用了1.2節中考慮的兩類機器人模型，即完整質點模型和非完整獨輪車模型。圖1(a)和(c)表現的是高斯卷積平滑處理前，兩類機器人模型的隨機可達集不連續狀態表現；圖1(b)和(d)表現的是使用了參數為N(μ=0,σ2=0.152)的高斯卷積平滑處理后，兩類機器人模型的隨機可達集連續狀態對比。由此可見，高斯卷積平滑處理在表現隨機可達集連續狀態的作用。

(a)不連續狀態 (d)平滑處理后的連續狀態

(c)不連續狀態 (d)平滑處理后的連續狀態圖1 隨機可達集平滑處理對比Fig.1 Comparison of random reachable sets smoothing

對于機器人附近的每個障礙物O，如果O在相對距離dmin范圍內，則需要查詢O對機器人的勢場影響。要計算這個梯度，首先從距離機器人當前相對位置找到隨機可達集中的最小鄰近值pi,j；然后用以pi,j為中心的二階中心有限差分計算這個梯度，再將每個障礙物的梯度合成在一起，生成所有障礙物最終共同的梯度。而函數goal-gradient是一個不斷指向目標的梯度向量，將函數goal-gradient與每個障礙物的梯度合成獲得最終的人工勢場梯度，記為APFvector，如圖2所示。

圖2 障礙物梯度和合成勢場梯度Fig.2 Obstacle gradient and synthetic potential field gradient

在算法1中，子函數U-O發揮了關鍵作用，即通過不斷更新障礙物位置形成動態環境。為此，在算法2中將給出子函數U-O的具體計算過程，其中子函數U-O采用與計算隨機可達集相同的動態方程，而且在每個采樣時刻，算法2基于可能的速度分布p(w)來評估障礙物的速度w，并用該速度更新障礙物動態。

在獲得APFvector之后，分別對兩種系統模型利用函數calcControl(APFvector)計算控制輸入u，即對于完整質點模型，有u=APFvector；對于非完整獨輪車模型，從APFvector中提取航向和速度以便構建u=(us,uw)。為此，首先將uw設置為APFvector方向上的最大轉彎率；然后將us設置為APFvector方向上的最大速度，這樣獨輪車的最大速度就與隨機可達集計算中使用的最大速度相同；最后，使用u來更新機器人的控制律。

3 算法驗證

為了演證本文提出的A-S算法對機器人避碰的路徑引導中的效果，將A-S算法與具有參數N(0,σ=(0.15 )2)和N(0,σ=(0.45 )2)的高斯人工勢場法[12]，依次進行噪聲影響、完整質點模型和非完整獨輪車模型的驗證。

3.1 噪聲影響驗證

在現實動態環境中，機器人的環境感知經常面臨許多不確定性。因此，實驗中采用以障礙物的真實位置為中心的有界均勻分布，對每個時間步長感知到的障礙物位置進行隨機抽樣來模擬不確定性。其中均勻分布以障礙物寬度(1 m)的10%、25%和50%為界。例如，如果噪聲被限定在50%范圍內，并且障礙物的真實位置在原點，則該障礙物的感知位置可以在(0.5 m，0.5 m)、(-0.5 m，0.5 m)、(-0.5 m，-0.5m)以及(0.5 m，-0.5 m)的范圍內的任何地方。

圖3表示了在障礙物位置噪聲存在的條件下，障礙物數量對成功率的影響。實際上，10%的噪聲水平就意味著所感測到的障礙物位置可以根據均勻隨機分布偏離真實位置，最大可達10%的障礙物寬度。

(a)A-S算法(b)σ = 0.15的高斯方法(c)σ = 0.45的高斯方法圖3 噪聲水平與障礙物數量對成功率影響的對比Fig.3 Comparison of the effects of different noise levels and number of obstacles

從圖3(a)、(b)和(c)對比可知，對于所有噪聲水平，A-S算法比高斯方法具有更高的成功率。驗證可知，除了在擁有最大噪聲量的高度擁擠情況下，A-S算法的成功率基本不受傳感器不確定性的影響。

3.2 完整質點模型驗證

如圖4所示，在這個驗證環境中，最大速度為0.36 m/s的完整質點模型機器人從起點車道位置S(-10 m，2.5 m)到目標位置G(15 m，7.5 m)，需跨越5條有移動障礙物的車道。動態障礙物在車道上行駛，并且利用障礙物動態式(1)對每個障礙物的速度進行隨機采樣。車道內障礙物運行的方向是從左向右和從右向左交替進行的，目的是增加對機器人向目標位置移動干擾的復雜度。為了保持障礙物的密度，障礙物在離開區域邊界時被傳送到其反向車道。

圖4 多車道變道驗證模型Fig.4 Verification model for lane changing in multi-lane

圖5 算法成功率比較Fig.5 Comparison of success rates of three algorithm

圖5展示A-S算法與其他方法成功率的比較，由于將形式化的隨機可達集與Ad-Hoc 人工勢場法集成，A-S算法提供了機器人-障礙物之間的相對動態更加準確的表達，從而提高了機器人對障礙物沖突反應的靈敏度，因此較其他方法有更高的成功率。

3.3 非完整獨輪車模型驗證

在這個實驗中，采用非完整獨輪車模型式(4)建模機器人動態，且從初始點S向目標點G行駛，而障礙物遵循隨機方向的直線軌跡運行。實際上，在非完整約束條件下，如果不對路徑進行修改，就無法用文獻[9]中提出的離線計算隨機可達集在線查詢方法來求解這個問題。

圖6(a)比較了A-S方法和高斯方法的成功率。由此圖可以看到，與性能表現最好的高斯方法比較，A-S方法的成功率提高了約50%。這是由于隨機可達集的引入，找到距離機器人當前相對位置的最小鄰近值pi,j，再以pi,j為中心計算更為有效的躲避引導梯度，從而使得A-S方法能夠做出更優指向目標的路徑規劃決策。

圖6(b)展示了單次運行每種方法路徑變化示例。期初在遇到移動障礙物較少時，三種方法的路徑差異不大。由圖6(b)的黑色圓部分可見，A-S方法與高斯方法在此處由于障礙物的沖突而產生背離運行，這種背離是由于用隨機可達集所構造勢場梯度的形狀發揮的作用。另外，由于獨輪車速度方向從正向負急劇變化，從而產生方向急劇反轉，所以這些路徑比完整機器人的路徑更不穩定，因此機器人采取的是更激進的躲避方式來避開障礙物。由圖6(b)最后路段三種方法比較可見，由于高斯方法在即將到達目標位之前與障礙物發生碰撞而停止不前，最后只有A-S方法躲過所有障礙物到達目標位。

圖6(c)顯示了所有方法與障礙物之間的斥力間隙的比較，由此表明無論環境如何變化，A-S方法的斥力勢場的作用更加穩定，因而能產生更靈活多變的路徑。

(a) A-S方法和高斯方法的成功率 (b)每種方法路徑變化示例(c)不同方法與障礙物之間斥力間隙的比較圖6 路徑選擇與成功率比較Fig.6 Comparison of path selection and success rate

4 結論

本文提出的形式化方法與Ad-Hoc方法集成具有如下優勢：

1)形式化的隨機可達集提供了碰撞概率的精確表示，其用于產生準確反映碰撞概率的勢能梯度。

2)與高斯人工勢場法比較，機器人面對移動障礙物的沖突的反應更加靈敏，不易被快速移動的障礙物所伏擊。

3)該方法通過組合多個隨機可達集來生成具有多個移動障礙物的近似避碰概率，從而容易表達和處理多移動障礙物的情況。

雖然研究結果表明提出的方法優于可比方法，但質點模型機器人是實際機器人的簡化，盡管更符合實際的機器人模型可以集成到隨機可達集的計算中，但需要更大的額外計算成本。另外，在實際應用中，離線計算的隨機可達集數據庫需要不斷更新，才能適應范圍動態變化的環境。