柴 松 馬社祥 李 嘯

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

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

0 引 言

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

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

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

1 路徑搜索

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

1.1 路徑生成

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

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

(1)

(2)

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

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

(3)

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

(4)

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

圖1 N段分段多項式路徑

(5)

1.2 碰撞檢測

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

(6)

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

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

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

(7)

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

(8)

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

1.3 成本函數(shù)

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

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

(9)

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

(10)

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

(11)

本節(jié)設計了啟發(fā)式函數(shù)加快搜索算法的速度,通過應用龐特里亞金最小原理(Pontryagins Minimum Principle PNP)來計算當前狀態(tài)Xc到目標狀態(tài)Xg的啟發(fā)式成本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樣條路徑優(yōu)化

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

圖2 前端搜索路徑和后端優(yōu)化路徑

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

2.1 均勻B樣條曲線

均勻B樣條曲線是由控制點{Q0,Q1,…,QN}、次數(shù)pb和節(jié)點向量[t0,t1,…,tM]組成的分段多項式。其中：{Q0,Q1,…,QN}?R2,{t0,t1,…,tM}?R?？刂乒?jié)點數(shù)、次數(shù)和節(jié)點向量數(shù)滿足式(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是由次數(shù)確定的常數(shù)矩陣[15]。本文設置均勻B樣條曲線的次數(shù)pb=3,常數(shù)矩陣M4如式(15)所示。

(15)

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

(16)

2.2 凸包性質(zhì)

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

圖3 三次均勻B樣條曲線

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

(17)

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

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

(18)

2.3 軟約束方法

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

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

(19)

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

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

(20)

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

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

(21)

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

(22)

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

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

(23)

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

(24)

3 仿真實驗

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

3.1 實施細節(jié)

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

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

3.2 實施結果分析

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

圖5 室內(nèi)三維環(huán)境地圖

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

(a) 路徑

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

表1 仿真實驗對比分析

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

4 結 語

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

從仿真實驗來看,基于均勻B樣條曲線的路徑能夠在室內(nèi)環(huán)境下進行仿真驗證。在未來的工作中,主要針對大規(guī)?；騽討B(tài)環(huán)境等極端情況下,驗證移動小車路徑的安全性。