基于均勻B樣條曲線的移動(dòng)小車路徑規(guī)劃方法

柴 松 馬社祥 李 嘯

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

2(天津理工大學(xué)計(jì)算機(jī)科學(xué)與工程學(xué)院 天津 300384)

0 引 言

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

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

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

1 路徑搜索

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

1.1 路徑生成

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

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

(1)

(2)

為了簡(jiǎn)化符號(hào),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)計(jì)算出移動(dòng)小車行進(jìn)過(guò)程中的路徑p(t)。

圖1 N段分段多項(xiàng)式路徑

(5)

1.2 碰撞檢測(cè)

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

(6)

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

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

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

(7)

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

(8)

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

1.3 成本函數(shù)

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

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

(9)

式中：ρ是時(shí)間T的權(quán)重值。因?yàn)樵诔掷m(xù)時(shí)間τ上的控制輸入是定值,所以在持續(xù)時(shí)間τ路徑成本如式(10)所示。

(10)

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

(11)

本節(jié)設(shè)計(jì)了啟發(fā)式函數(shù)加快搜索算法的速度,通過(guò)應(yīng)用龐特里亞金最小原理(Pontryagins Minimum Principle PNP)來(lái)計(jì)算當(dāng)前狀態(tài)Xc到目標(biāo)狀態(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)化

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

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

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

2.1 均勻B樣條曲線

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

(15)

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

(16)

2.2 凸包性質(zhì)

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

圖3 三次均勻B樣條曲線

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

(17)

安全性是移動(dòng)小車行進(jìn)過(guò)程中非常重要的因素。在移動(dòng)小車行進(jìn)過(guò)程中,不僅需要滿足移動(dòng)路徑的無(wú)障礙物碰撞,而且還要保證移動(dòng)小車遠(yuǎn)離障礙物。

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

(18)

2.3 軟約束方法

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

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

(19)

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

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

(20)

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

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

(21)

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

(22)

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

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

(23)

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

(24)

3 仿真實(shí)驗(yàn)

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

3.1 實(shí)施細(xì)節(jié)

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

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

3.2 實(shí)施結(jié)果分析

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

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

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

(a) 路徑

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

表1 仿真實(shí)驗(yàn)對(duì)比分析

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

4 結(jié) 語(yǔ)

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

從仿真實(shí)驗(yàn)來(lái)看,基于均勻B樣條曲線的路徑能夠在室內(nèi)環(huán)境下進(jìn)行仿真驗(yàn)證。在未來(lái)的工作中,主要針對(duì)大規(guī)模或動(dòng)態(tài)環(huán)境等極端情況下,驗(yàn)證移動(dòng)小車路徑的安全性。