基于均勻B樣條曲線的移動小車路徑規劃方法

柴 松 馬社祥 李 嘯

1(天津理工大學電氣電子工程學院 天津 300384)

2(天津理工大學計算機科學與工程學院 天津 300384)

0 引 言

為了滿足人們對生活便利和生活質量的追逐,無人車、服務型機器人、消防救援機器人等應用逐漸進入日常的生活。移動機器人路徑規劃是機器人研究領域的重要組成部分,是指在未知環境中,根據給定的指標(經過路徑最短、花費時間最少、距離障礙物最遠),尋找一條從初始狀態到目標狀態最優或者近似最優的無碰撞路徑。移動機器人路徑規劃問題是一種典型的NP-hard[1](非典型性多項式困難)問題,針對移動機器人路徑規劃[2],傳統的方法主要有模擬退火算法[3]、人工勢場算法[4]、模糊邏輯算法[5]、柵格算法等。柵格算法適用于結構簡單的環境中,當空間結構變復雜時,存在計算效率低、存儲空間大的問題;模擬退火算法存在計算量大、效率低等問題。隨著環境復雜程度的增加,出現了免疫算法[6]、遺傳算法[7]、人工魚群算法[8]、A*算法[9]、神經網絡算法[10-11]等,這類算法在移動機器人路徑規劃取得了顯著的成果。

近年來基于B樣條曲線的方法也逐漸應用于移動小車的路徑規劃中。張敬寒等[12]提出一種基于擴大搜索鄰域A*算法的平滑路徑規劃算法,該算法擴大A*算法當前節點搜索鄰域范圍,采用三次均勻B樣條曲線規劃路徑的平滑處理,有效消除了原規劃路徑上的路徑尖峰拐點。劉紫燕等[13]提出一種改進RRT算法的室內移動機器人路徑規劃算法,該算法采用目標偏向策略和氣味擴散法來改善擴展節點的選取,選取基于三次B樣條曲線的路徑平滑方法,極大地提升了搜索效率和路徑質量。目前路徑規劃技術仍有兩個關鍵技術有待解決。首先,在時間和計算資源有限的情況下,移動小車行進過程中,安全性能是首要考慮因素,不僅和環境地圖中的障礙物無碰撞,而且需要保持相對安全距離。其次,移動機器人在未知環境中移動,應該在很短的時間內不斷地重新規劃路徑,避免在緊急情況下碰到障礙物。

本文針對以上的問題,提出一種基于均勻B樣條曲線的移動小車路徑規劃方法。首先,和上述算法不同的是,前端本文選取控制空間中離散化加速度控制變量作為擴展節點,確保移動小車的速度的連續性,消除初始路徑的尖峰拐點使得初始路徑平滑。然后,采用軟優化的方式,結合環境障礙物的距離信息、路徑的平滑度和移動小車的速度、加速度信息優化均勻B樣條曲線的控制點,使得均勻B樣條曲線的控制點遠離障礙物。應用均勻B樣條曲線[14]的凸包性質(均勻B樣條曲線恒位于其凸包之內),保證移動小車路徑更加安全。最后,優化控制點和時間分配關系,確保速度和加速度在可控范圍內。

1 路徑搜索

本節的前端路徑搜索方法是選取控制空間中離散化加速度控制變量作為擴展節點,將移動小車的控制成本和啟發式搜索成本相結合,在室內環境地圖中計算出耗時最少、控制成本和啟發式搜索成本最小的無碰撞初始路徑。

1.1 路徑生成

移動小車行進過程中,系統位置狀態是由二維路徑坐標多項式組成,如式(1)和式(2)所示。

p(t)=[px(t),py(t)]T

(1)

(2)

為了簡化符號,v(t)=p′(t)表示系統的速度,u(t)=p″(t)∈U=[-umax,umax]2?R2表示系統控制輸入,X(t)=[p(t)T,v(t)T]T∈R4表示系統的狀態向量。狀態空間模型如式(3)所示。

X′(t)=AX(t)+Bu(t)

(3)

狀態方程的通解如式(4)所示。

(4)

給出初始位置信息p(0)=[px(0),py(0)]T,初始速度信息v(0)=[vx(0),vy(0)]T和控制輸入u(t),根據式(4)計算出移動小車行進過程中的路徑p(t)。

圖1 N段分段多項式路徑

(5)

1.2 碰撞檢測

碰撞檢測是路徑搜索中必不可少的一部分,它作用是判定移動小車行進路徑是否和環境地圖中的障礙物發生碰撞,篩選出可以通行的初始路徑。本節給定的一組初始狀態、控制輸入和持續時間,在時間t∈[0,τ]中,滿足式(6)。

(6)

式中：Pfree表示無障礙物可通行的自由空間;v(t)是時間t的多項式函數,只需要計算在時間周期[0,τ]內v(t)的極值和閾值相比較。當多項式函數的階數小于等于5時,很容易求解在閉合區間上的極值。

為了確保路徑的安全可行性,本節選取沿著路徑遍歷的一組位置進行驗證如式(7)所示。

P={p(ti)|ti∈[0,τ],i=0,1,…,I}

(7)

對于所有的i∈{0,1,…,I},使得p(ti)∈Pfree,即視為該路徑沒有碰撞到障礙物,離散化路徑點的數量如式(8)所示。

(8)

式中：R表示占用網格的分辨率。式(8)的條件確保兩個連續的采樣路徑點之間的最大距離不超過網格的分辨率,保證移動小車行進過程中無障礙物碰撞。

1.3 成本函數

移動小車行進過程中,通常有多條路徑可以到達目標位置,引入成本函數的目的是在多條可到達目標位置的路徑中選擇一條無碰撞沖突、移動成本和啟發式成本最小的初始路徑。

路徑的移動成本定義為控制輸入2-范數的平方加上時間成本,如式(9)所示。

(9)

式中：ρ是時間T的權重值。因為在持續時間τ上的控制輸入是定值,所以在持續時間τ路徑成本如式(10)所示。

(10)

則從開始狀態到終點狀態最優路徑的實際成本如式(11)所示。

(11)

本節設計了啟發式函數加快搜索算法的速度,通過應用龐特里亞金最小原理(Pontryagins Minimum Principle PNP)來計算當前狀態Xc到目標狀態Xg的啟發式成本HC(t),如式(12)所示。

pi(t)=ai3t3+ai2t2+ai1t+ai0

vi(t)=3ai3t2+2ai2t+ai1

ui(t)=6ai3t+2ai2

ai0=pic,ai1=vic

(12)

2 B樣條路徑優化

移動小車行進過程中,前端路徑搜索產生的路徑可能是次優的。由于忽略了移動小車自身的半徑和環境地圖中自由空間的距離信息,搜索出的路徑通常接近障礙物,如圖2虛線所示。即使搜索產生的路徑在自由空間中,當移動小車接近障礙物時,也很容易發生碰撞。為了降低這種風險,需要一條遠離障礙物的路徑。傳統方法是通過將障礙物膨脹到比移動小車大得多的半徑來解決的。但是,這種過度膨脹策略并不適合在障礙物雜亂的室內環境中進行路徑規劃。

圖2 前端搜索路徑和后端優化路徑

本節提出基于三次均勻B樣條的軟優化算法優化路徑。根據移動小車行進過程中,安全性能是首要因素進行如下改進：由于改變均勻B樣條曲線的局部控制點,則相應控制點的分段曲線也會改變,根據均勻B樣條曲線恒位于其凸包之內的性質,通過引入軟約束總成本函數,分別對控制點間的平滑度、控制點和障礙物的安全距離信息、移動路徑上速度和加速度的限制,優化移動路徑上的控制點的位置。優化后的移動小車路徑和障礙物保持安全距離,并且優化之后的路徑更加平滑。下面進行詳細說明。

2.1 均勻B樣條曲線

均勻B樣條曲線是由控制點{Q0,Q1,…,QN}、次數pb和節點向量[t0,t1,…,tM]組成的分段多項式。其中：{Q0,Q1,…,QN}?R2,{t0,t1,…,tM}?R。控制節點數、次數和節點向量數滿足式(13)。

M=N+pb+1

(13)

[Bi-pb,pb+1(s(t))Bi-pb+1,pb+1(s(t)) …Bi,pb+1(s(t))]=[1s(t)s2(t) …spb(t)]Mpb+1

(14)

式中：Mpb+1是由次數確定的常數矩陣[15]。本文設置均勻B樣條曲線的次數pb=3,常數矩陣M4如式(15)所示。

(15)

則路徑P(s(t))如式(16)所示。

(16)

2.2 凸包性質

三次均勻B樣條曲線是被每段控制點組成的多邊形包圍,如圖3所示。均勻B樣條曲線的導數仍然是均勻B樣條曲線。

圖3 三次均勻B樣條曲線

對于運動可行性,只需約束所有速度控制點{V0,V1,…,VN-1}和加速度控制點{A0,A1,…,AN-1},使得Vi∈[-vmax,vmax]2和Ai∈[-amax,amax]2,其中Vi和Ai如式(17)所示。

(17)

安全性是移動小車行進過程中非常重要的因素。在移動小車行進過程中,不僅需要滿足移動路徑的無障礙物碰撞,而且還要保證移動小車遠離障礙物。

根據均勻B樣條曲線的凸包性質,則凸包內的路徑也是無障礙碰撞的。如圖4所示,保證dp>0,其中dp是凸包中的任意一個路徑點到柵格障礙物的距離,通過三角不等式,使得dp≥dc-rp,其中：dc是控制點到柵格障礙物的距離;rp是控制點到任意一個路徑點的距離。因為P在凸包內,得到控制點到路徑點的距離rp≤r12+r23+r34,其中r12、r23、r34是相鄰控制點的距離,因此dp>dc-(r12+r23+r34)。為了保證凸包的安全性,只需滿足式(18)。

(18)

2.3 軟約束方法

三次均勻B樣條曲線在本質上是連續的,不需要顯式地強制連續性,軟約束方法[16-17]利用距離信息將路徑推離到距離障礙物較安區的地方,提高了計算效率和收斂速度。由N+1個控制點{Q0,Q1,…,QN}定義的三次均勻B樣條路徑,本節只需要優化N-5個控制點{Q3,Q4,…,QN-3},前三個和后三個控制點分別對應當前狀態和目標狀態不需要更改,軟約束總成本函數ftotal[18]如式(19)所示。

ftotal=λ1fs+λ2fc+λ3(fv+fa)

(19)

式中：fs是平滑度成本;fc是碰撞成本;fv、fa分別是速度和加速度的限制成本;λ1、λ2和λ3分別是平滑性、安全性、運動可行性的權重值。

本節通過控制點的幾何信息并且不依賴時間信息的函數fs來定義平滑度成本如式(20)所示。

(20)

式中：控制點Q1、Q2、QN-2、QN-1是不需要進行優化,但是需要計算總體的平滑度。從物理層面看,式(20)中Qi+1-Qi和Qi-1-Qi分別是連接節點Qi+1、Qi和節點Qi-1、Qi的兩個彈簧的結合力。如果所有項都等于零,則所有的控制點都將均勻分布在一條直線上,這是理想狀態的平滑。

本節通過作用于控制點的障礙物的排斥力來定義碰撞成本fc,如式(21)所示。

(21)

式中：d(Qi)表示控制點Qi和障礙物之間的距離;Fc是可微分的函數。通過設置指定障礙物距離的閾值dthr來計算懲罰值,如式(22)所示。

(22)

因為碰撞成本函數將控制點推離障礙物,所以式(18)中dc>0顯而易見,此外,ri,i+1僅依賴均勻B樣條曲線的參數,根據大量實驗證明,只需要設置ri,i+1<0.2,路徑在大多數情況下符合安全性要求,但是在極端情況下可能是無效的,例如,室內環境非常混亂。即使這樣,也能通過重新設置均勻B樣條曲線參數,選擇較小的ri,i+1,仍然滿足式(18)。

本節通過懲罰路徑上的速度和加速度超過閾值vmax、amax來定義懲罰項,速度和加速度的懲罰項如式(23)所示。

(23)

由均勻B樣條曲線的凸包性質,得到不可行速度和加速度控制點的限制成本如式(24)所示。

(24)

3 仿真實驗

本節針對提出的基于均勻B樣條曲線的移動小車路徑規劃方法的實驗結果分析和說明。實驗平臺配備TITAN Xp GPU,Intel Xeon(R) E5-26900 CPU,2.9 GHz×32,64 GB內存,機器人操作系統(Robot Operating System,ROS),Ubuntu16.04操作系統。本文提出的路徑規劃方法在C++11環境下通過非線性優化解算器NLopt[19]實現。在仿真實驗中,路徑搜索時間間隔τ=0.5,移動小車半徑r=0.1 m,優化設置λ1=10,λ2=1,λ3=0.1,本節提供室內三維環境地圖數據來驗證基于均勻B樣條曲線的移動小車路徑規劃方法的性能。

3.1 實施細節

當一定移動小車在未知室內環境中運動時,由于傳感范圍有限,它不得不頻繁地重新規劃自己的路徑。為了提高效率,我們采用了循環時間規劃方案,其中路徑只在已知空間內生成。一旦路徑在該范圍之外結束,前端路徑搜索就終止,隨后是后端路徑優化。在未知的空間進行規劃往往是徒勞的,因此可以提升路徑搜索的速度。

重新規劃路徑機制在兩種情況下觸發。首先,如果當前路徑與新出現的障礙物發生碰撞,就會觸發重新規劃路徑機制。這確保了一旦檢測到任何碰撞,就有新的安全路徑可用。其次,在固定的時間間隔調用重新規劃路徑機制,使得最新的環境信息能夠融入到路徑搜索中。

3.2 實施結果分析

為了驗證基于均勻B樣條曲線的移動小車路徑規劃方法的安全性能,進行仿真實驗。本節將初始搜索路徑和三次均勻B樣條曲線優化后的路徑進行安全性能對比。選取了室內三維環境地圖進行驗證,如圖5所示。圖6為初始搜索路徑和初始搜索路徑障礙物距離曲線,可以看出,初始搜索路徑存在路徑尖峰拐點,并且移動小車質心和障礙物的最短距離最小值約為0.138 m,考慮到移動小車自身半徑為0.1 m,在移動小車行進過程中不符合安全性能的需求。

圖5 室內三維環境地圖

安全性對于路徑規劃是非常重要的因素。圖7為三次均勻B樣條曲線優化后的路徑和路徑障礙物距離曲線,可以看出,優化后的路徑消除了路徑尖峰拐點,并且移動小車質心和障礙物的最短距離最小值約為0.303 m,優化了移動小車轉向時靠近障礙物的路徑,優化后的路徑滿足移動小車行進過程中安全性能的需求。

(a) 路徑

最后,選取簡單室內環境仿真地圖,將擴大搜索鄰域A*算法和本文算法相比較,設定移動小車起點坐標為(5,-5),兩段終點目標分別是(-4,7)和(-5,-7)。圖8為兩種路徑規劃算法效果。選擇路徑長度、搜索時間和采樣節點數作為評價指標,記錄30次實驗的平均值如表1所示。本文算法搜索時間更短、搜索速度更快,在確保路徑安全性的前提下,路徑變長。

表1 仿真實驗對比分析

(a) 擴大搜索鄰域A*算法 (b) 本文算法圖8 兩種算法路徑規劃效果比較

4 結 語

路徑規劃是移動機器人研究領域的一個重要研究方向。移動小車行進過程中,安全性是首要考慮因素,本文針對安全性提出一種基于均勻B樣條曲線的移動小車路徑規劃方法。前端采用運動路徑搜索方法,尋找一條初始路徑,后端結合距離信息和均勻B樣條曲線優化方法進一步提高路徑的安全性和平滑性。利用均勻B樣條曲線的凸包性質,與擴大搜索鄰域A*算法相比,顯著提高了移動路徑的安全性和實時性。

從仿真實驗來看,基于均勻B樣條曲線的路徑能夠在室內環境下進行仿真驗證。在未來的工作中,主要針對大規模或動態環境等極端情況下,驗證移動小車路徑的安全性。