基于障礙物代價(jià)勢場的移動(dòng)機(jī)器人避障算法

遲勝凱,謝永芳,陳曉方,彭帆

(中南大學(xué) 自動(dòng)化學(xué)院,長沙 410083)

隨著機(jī)器人技術(shù)的快速發(fā)展,機(jī)器人在工業(yè)、服務(wù)業(yè)、安防等領(lǐng)域的應(yīng)用越來越廣泛,機(jī)器人也越來越多的出現(xiàn)在生活中。移動(dòng)機(jī)器人最重要的功能是路徑規(guī)劃和避障,導(dǎo)航的目的是找到從起點(diǎn)到終點(diǎn)具有避障能力的最優(yōu)或次優(yōu)路徑[1]。現(xiàn)實(shí)世界的環(huán)境是動(dòng)態(tài)變化的,需要不斷更新地圖中的障礙物信息,使機(jī)器人在現(xiàn)實(shí)環(huán)境中實(shí)現(xiàn)靈活的自主移動(dòng)。因此,在保證路徑規(guī)劃最優(yōu)或次優(yōu)的前提下,如何讓機(jī)器人對(duì)場景中的靜態(tài)和動(dòng)態(tài)障礙物快速的做出響應(yīng),同時(shí)保證路徑的平滑度和安全性,對(duì)于機(jī)器人的避障來說非常重要。

機(jī)器人所處的環(huán)境通常是三維的,三維環(huán)境地圖[2-3]是描述真實(shí)三維世界最直接的方法,在三維地圖中可以獲得障礙物更多的形狀、輪廓、姿態(tài)、語義等信息,對(duì)于輔助機(jī)器人的導(dǎo)航和避障來說意義重大。而基于三維點(diǎn)云地圖的三維路徑規(guī)劃[4]具有很高的時(shí)間復(fù)雜度和空間復(fù)雜度,不利于實(shí)時(shí)避障的實(shí)現(xiàn)。因此在二維地圖場景下利用三維障礙物信息,實(shí)現(xiàn)基于二維地圖的導(dǎo)航和路徑規(guī)劃,對(duì)于提高機(jī)器人避障的靈活性是很有必要的。

柵格法環(huán)境建模是移動(dòng)機(jī)器人導(dǎo)航的經(jīng)典方法,該方法將機(jī)器人所處的實(shí)際環(huán)境分割為大小相同的柵格,每個(gè)柵格的不同值對(duì)應(yīng)地圖的不同障礙物占用狀態(tài)[5]。因此在柵格地圖下利用三維場景信息及三維傳感器數(shù)據(jù),可以大大提升移動(dòng)機(jī)器人導(dǎo)航與避障的靈活性與安全性。在柵格地圖場景下,常用的路徑規(guī)劃算法有A*算法、人工勢場法、蟻群算法、快速擴(kuò)展隨機(jī)樹(rapidly-exploring random tree star,RRT*)算法、遺傳算法、D*算法等[6]。A*算法是求解最優(yōu)路徑非常有效的算法,但是當(dāng)路徑規(guī)劃的場景較大時(shí),并且存在障礙物遮擋時(shí),A*算法的計(jì)算量就會(huì)非常大,內(nèi)存占用嚴(yán)重,并且運(yùn)算時(shí)間長,路徑規(guī)劃的效率較低,趙曉等[7]針對(duì)此問題提出了基于跳點(diǎn)搜索的改進(jìn)算法,通過篩選跳點(diǎn)進(jìn)行擴(kuò)展,直到生成最終路徑,擴(kuò)展過程中使用跳點(diǎn)代替A*算法中大量可能被添加到有效點(diǎn)集合和無效點(diǎn)集合的不必要節(jié)點(diǎn),從而減少計(jì)算量。王洪斌等[5]針對(duì)多目標(biāo)復(fù)雜場景,提出了A*算法與動(dòng)態(tài)窗口法相結(jié)合的混合算法,通過設(shè)置多個(gè)臨時(shí)目標(biāo)點(diǎn),利用目標(biāo)成本函數(shù)對(duì)所有目標(biāo)進(jìn)行優(yōu)先級(jí)判定,進(jìn)而利用改進(jìn)的A*算法規(guī)劃出一條經(jīng)過多個(gè)目標(biāo)點(diǎn)的最優(yōu)路徑,一定程度上改善了算法的效率問題,但是對(duì)于導(dǎo)航路徑的安全性及路徑平滑性的問題,依然沒有很好的解決。Liu等[8]采用了改進(jìn)蟻群算法進(jìn)行全局路徑規(guī)劃,在搜索全局路徑的過程中,將信息素?cái)U(kuò)散與勢場力相結(jié)合,使信息素沿障礙物勢場力的方向擴(kuò)散,提高了蟻群算法的收斂速度,但蟻群算法為找到全局最優(yōu)路徑,搜索空間必須足夠大,會(huì)消耗大量的內(nèi)存空間,依然存在最優(yōu)路徑搜索效率低的問題。林依凡等[9]提出了一種無碰撞檢測改進(jìn)RRT*運(yùn)動(dòng)規(guī)劃算法,針對(duì)RRT*算法碰撞檢測過程中耗時(shí)過長的問題,提出使用在代價(jià)函數(shù)中增加碰撞風(fēng)險(xiǎn)評(píng)估函數(shù)的方法取代所有的碰撞檢測計(jì)算,提高了算法的尋路效率,但是并沒有改善導(dǎo)航的路徑平滑性不好、算法不穩(wěn)定的特點(diǎn),且沒有考慮與障礙物的安全距離。薛學(xué)華等[10]提出了一種改進(jìn)人工勢場法,針對(duì)局部最優(yōu)值問題提出了一種自適應(yīng)參數(shù)調(diào)整回溯法,解決了傳統(tǒng)人工勢場的路徑不可達(dá)與局部最優(yōu)值問題,通過人工勢場法與可視圖法切換的策略克服了人工勢場目標(biāo)不可達(dá)問題,但是該算法同樣沒有考慮導(dǎo)航路徑的安全性及平滑性,且得到的路徑長度通常不是最優(yōu)的。

Zafar和Mohanta[6]對(duì)目前現(xiàn)有的移動(dòng)機(jī)器人的路徑規(guī)劃和避障算法及改進(jìn)方案進(jìn)行了研究和對(duì)比,現(xiàn)有研究在解決同一場景下機(jī)器人避障問題時(shí)只考慮了單一的路徑評(píng)價(jià)指標(biāo),如只考慮路徑最短、平滑度最優(yōu)、耗能最少或算法的效率最高,在單一指標(biāo)上能夠達(dá)到較優(yōu)的效果,因此只能適用一些特定的場景。以往研究的另一個(gè)局限是,在進(jìn)行靜態(tài)或動(dòng)態(tài)環(huán)境下局部路徑規(guī)劃過程中,沒有考慮機(jī)器人的大小,也沒有考慮移動(dòng)中的障礙物對(duì)于移動(dòng)機(jī)器人避障的影響。而在一些工業(yè)應(yīng)用場景下,不僅要求路徑盡可能地短,出于安全考慮需要機(jī)器人和某些障礙物保持安全距離,并且要求路徑具有較高的平滑度。

基于以上原因,本文提出了一種基于障礙物代價(jià)勢場的動(dòng)態(tài)路徑生成及路徑調(diào)整算法。根據(jù)激光雷達(dá)、深度相機(jī)等傳感器數(shù)據(jù)獲取動(dòng)態(tài)障礙物運(yùn)動(dòng)狀態(tài)信息。然后由柵格地圖構(gòu)建障礙物的代價(jià)勢場,根據(jù)機(jī)器人的形狀、大小及其他避障參數(shù)選擇某一合適高度的等勢曲線,通過求解由機(jī)器人當(dāng)前位置、目標(biāo)點(diǎn)、等勢線及其切線得到的最小生成子樹,獲得初始候選路徑。對(duì)初始候選路徑采樣獲得初始候選路徑錨點(diǎn)。并在代價(jià)勢場下對(duì)初始候選路徑錨點(diǎn)的障礙物代價(jià)和路徑長度代價(jià)進(jìn)行調(diào)整,得到最終的路徑。算法在保證路徑和障礙物的安全距離以及較好的路徑平滑度的同時(shí),能夠使規(guī)劃得到的路徑盡可能地短。對(duì)于動(dòng)態(tài)移動(dòng)的障礙物,通過引入障礙物速度對(duì)代價(jià)勢場的影響,使得在移動(dòng)障礙物場景下也能獲得較好的結(jié)果。

1 機(jī)器人避障問題建模

1.1 機(jī)器人模型

在平面環(huán)境中,差動(dòng)轉(zhuǎn)向移動(dòng)機(jī)器人的機(jī)械結(jié)構(gòu)簡單、運(yùn)動(dòng)靈活、穩(wěn)定性高,是目前較為流行的地面移動(dòng)機(jī)器人。本文假設(shè)機(jī)器人裝有穩(wěn)定的底層控制系統(tǒng),可以通過陀螺儀、激光雷達(dá)、里程計(jì)等傳感器數(shù)據(jù)完成對(duì)機(jī)器人速度、姿態(tài)的跟蹤。設(shè)E-N為機(jī)器人所處的世界坐標(biāo)系,X-Y為機(jī)器人本體的坐標(biāo)系,如圖1所示,機(jī)器人的當(dāng)前狀態(tài)可以用一個(gè)向量[e,n,θw]表示,其中[e,n]表示機(jī)器人在世界坐標(biāo)系中的坐標(biāo),θw表示機(jī)器人的航向角。設(shè)機(jī)器人左右兩側(cè)驅(qū)動(dòng)輪距離為W,驅(qū)動(dòng)輪直徑為L,右輪和左輪角速度分別為和,機(jī)器人的運(yùn)動(dòng)狀態(tài)可以表示為

圖1 差動(dòng)轉(zhuǎn)向機(jī)器人的俯視示意圖Fig.1 Top view of differential steering robot

1.2 動(dòng)態(tài)地圖模型

雖然機(jī)器人只會(huì)在一個(gè)二維的平面內(nèi)運(yùn)動(dòng),但機(jī)器人所處環(huán)境是三維的,而直接在三維地圖下進(jìn)行路徑規(guī)劃和避障,會(huì)使計(jì)算的復(fù)雜度大大增加,不利于實(shí)時(shí)的實(shí)現(xiàn)和大尺度環(huán)境下的應(yīng)用。為了在導(dǎo)航過程中充分利用三維數(shù)據(jù),實(shí)現(xiàn)本文的避障算法,通?；诙嗑€激光雷達(dá)數(shù)據(jù)實(shí)現(xiàn)動(dòng)態(tài)地圖的更新,通過建立由三維點(diǎn)云數(shù)據(jù)到二維占用柵格地圖的動(dòng)態(tài)映射來更新動(dòng)態(tài)障礙物地圖信息。由本地機(jī)器人在動(dòng)態(tài)二維地圖的基礎(chǔ)上完成避障任務(wù)。

機(jī)器人在進(jìn)行導(dǎo)航和避障之前會(huì)建立一個(gè)全局場景的靜態(tài)柵格地圖,假設(shè)這個(gè)初始場景的柵格地圖不會(huì)發(fā)生變化。機(jī)器人在運(yùn)動(dòng)過程中會(huì)對(duì)激光雷達(dá)產(chǎn)生的點(diǎn)云數(shù)據(jù)進(jìn)行實(shí)時(shí)處理,根據(jù)具體機(jī)器人的大小和尺寸進(jìn)行濾波去除無效的點(diǎn)云信息,利用可能會(huì)與機(jī)器人發(fā)生碰撞的障礙物有效點(diǎn)云信息更新對(duì)應(yīng)的障礙物占用柵格地圖[11](occupancy grid map),得到針對(duì)不同動(dòng)態(tài)障礙物的動(dòng)態(tài)概率柵格地圖,即不同柵格中存在障礙物的概率。根據(jù)占用柵格地圖中的障礙物表示方法,對(duì)于地圖中的任意柵格,定義柵格狀態(tài)算子s,p(s=0)表示該柵格被障礙物占用的概率,使用p(s=1)表示該柵格不被障礙物占用的概率,二者的和為1,使用二者的比值Odd(s)=p(s=1)/p(s=0)描述當(dāng)前柵格的障礙物占用狀態(tài),初始狀態(tài)下默認(rèn)p(s=1)=p(s=0)。

在地圖更新過程中假設(shè)不同柵格間的狀態(tài)變化互不影響,對(duì)于地圖中的任一柵格單元,假設(shè)在t時(shí)刻之前的障礙物占用狀態(tài)為Oddt-1(s),在t時(shí)刻有新的測量值zt,其在t時(shí)刻的障礙物占用狀態(tài)按式(3)進(jìn)行計(jì)算,Odd(s|zt)表示在zt發(fā)生的條件下s的發(fā)生概率。

由貝葉斯公式可知

由式(4)代入式(3)后得

同時(shí)對(duì)式(5)兩邊取對(duì)數(shù)可得

因此,只需根據(jù)當(dāng)前傳感器測量值對(duì)ln(p(zt|s=1)/p(zt|s=0))進(jìn)行更新,即可實(shí)現(xiàn)障礙物地圖數(shù)據(jù)的更新,記lt為t時(shí)刻?hào)鸥駟卧膶?duì)數(shù)占用狀態(tài)式,lt-1為t-1時(shí)刻?hào)鸥駟卧膶?duì)數(shù)占用狀態(tài)式,lmeas為傳感器測量值的對(duì)數(shù)描述形式,則可以簡化為lt=lt-1+lmeas。 測量值的模型有2種狀態(tài),分別為free和occupied,帶迭代更新過程中都是定值。這樣,即可基于占用柵格地圖對(duì)動(dòng)態(tài)障礙物進(jìn)行描述,lt的值越大,則表示對(duì)應(yīng)柵格存在障礙物的概率越大。在此基礎(chǔ)設(shè)ρ為更新后得柵格障礙物概率描述式,Cmax表示地圖中允許的柵格代價(jià)最大值,為一個(gè)常數(shù)。則當(dāng)柵格的概率描述值lt＜0時(shí),ρ=0。當(dāng)0≤lt＜Cmax時(shí),ρ=lt/Cmax,當(dāng)lt＞Cmax時(shí),ρ=1。

假設(shè)本地機(jī)器人的全局靜態(tài)柵格地圖是由m1行m2列的柵格組成,則全局靜態(tài)柵格地圖可以用一個(gè)二維矩陣M(x,y)表示,柵格障礙物概率為

則由激光雷達(dá)數(shù)據(jù)得到的局部動(dòng)態(tài)障礙物地圖可以表示為M′(x,y)(其中x與y滿足條件0＜x＜m1,0＜y＜m2)。在執(zhí)行動(dòng)態(tài)避障任務(wù)時(shí)局部動(dòng)態(tài)障礙物地圖會(huì)和全局靜態(tài)地圖進(jìn)行疊加。

1.3 靜態(tài)障礙物的表示

本文基于三維傳感器數(shù)據(jù)得到了機(jī)器人對(duì)應(yīng)的二維概率柵格地圖,提出了基于障礙物代價(jià)勢場的機(jī)器人障礙物表示算法。靜態(tài)障礙物柵格的占用情況由柵格代價(jià)值C和柵格存在障礙物概率ρ這2個(gè)參數(shù)描述。先需要建立全局靜態(tài)柵格地圖的代價(jià)勢場。設(shè)ρop=ρ為靜態(tài)障礙物代價(jià)勢場中的柵格障礙物概率,其中ρ為由占用柵格地圖更新得到的障礙物概率,下標(biāo)p為距離當(dāng)前柵格最近且障礙物概率不為0的柵格、o為當(dāng)前柵格。對(duì)于障礙物概率ρ≠0的柵格,其障礙物代價(jià)Up=Cmaxρop。對(duì)于障礙物概率ρ=0的柵格,障礙物代價(jià)為

式中:若p(xp,yp)為距離當(dāng)前柵格最近且障礙物概率不為0的柵格坐標(biāo),ρop為柵格p的障礙物概率;dop為p到當(dāng)前柵格的歐氏距離;dmax為障礙物柵格所能影響的最遠(yuǎn)距離,為一個(gè)常數(shù)。這樣可以得到靜態(tài)柵格地圖的代價(jià)勢場,如圖2所示,柵格的邊長為單位1,黑色柵格代表障礙物概率ρ≠0的柵格,帶有灰度的柵格為由式(12)計(jì)算得到的代價(jià)勢場。

圖2 障礙物附近的代價(jià)勢場柵格示意圖Fig.2 Schematic diagram of cost potential field grid near obstacle

1.4 動(dòng)態(tài)障礙物的表示

在實(shí)際生產(chǎn)過程中機(jī)器人面對(duì)的不僅有靜態(tài)的障礙物,還需要對(duì)場景中的動(dòng)態(tài)障礙物做出靈活的響應(yīng)。由三維場景下的激光數(shù)據(jù)可以獲得動(dòng)態(tài)障礙物相對(duì)于機(jī)器人的速度。根據(jù)機(jī)器人在世界坐標(biāo)系下的姿態(tài)[e,n,θw],可以得到當(dāng)前時(shí)刻動(dòng)態(tài)障礙物坐標(biāo)系相對(duì)于世界坐標(biāo)系下的速度。定義Oobs[x,y]和v分別為動(dòng)態(tài)障礙物當(dāng)前時(shí)刻的位置和速度,其中[x,y]為動(dòng)態(tài)障礙物對(duì)象位置,向量v為動(dòng)態(tài)障礙物的移動(dòng)速度,動(dòng)態(tài)障礙物的移動(dòng)速度為

式中:A(xa,ya)為上一時(shí)刻動(dòng)態(tài)障礙物的坐標(biāo);t為2次坐標(biāo)更新的時(shí)間差。設(shè)B(xb,yb)為地圖中障礙物概率ρ=0的任一柵格坐標(biāo),p(xp,yp)為動(dòng)態(tài)障礙物Oobs中距離點(diǎn)B最近且ρ≠0的柵格坐標(biāo)。令向量d=(xb-xp,yb-yp),障礙物速度對(duì)代價(jià)勢場的影響為

式中:λ為障礙物速度對(duì)代價(jià)勢場的影響系數(shù),值越大表示障礙物速度對(duì)障礙物代價(jià)勢場的影響越大;ρop為柵格p的障礙物概率;φ為向量v和向量d夾角的余弦值,即

由式(8)和式(10)可得,動(dòng)態(tài)障礙物場景下的障礙物代價(jià)為

2 路徑生成與避障

2.1 切線的計(jì)算

為了計(jì)算避障過程中的初始候選路徑,根據(jù)第1節(jié)本文算法計(jì)算柵格地圖中的障礙物代價(jià)勢場。如圖3所示,在柵格地圖中,設(shè)點(diǎn)R為機(jī)器人的當(dāng)前位置,黑色柵格表示障礙物概率不為0的區(qū)域,白色柵格表示障礙物概率為0的區(qū)域,由式(12)更新障礙物代價(jià)勢場,然后根據(jù)具體機(jī)器人的形狀、大小、避障規(guī)則等選擇合適高度的等勢曲線。圖3中紅色曲線為障礙物的一條等勢曲線,由于在柵格地圖中長度的計(jì)算是不連續(xù)的,柵格地圖中的等勢曲線可以用圖3中的綠色柵格表示。為方便計(jì)算,設(shè)點(diǎn)A為等勢曲線的起點(diǎn),按順時(shí)針方向,等勢曲線可以由一系列離散的柵格a1,a2,…,ane表示,其中ne為等勢曲線包含柵格總個(gè)數(shù)。其長度lne按如下方法計(jì)算:

圖3 基于代價(jià)勢場計(jì)算切線及等勢曲線長度Fig.3 Calculate tangent and equipotential curve length based on cost potential field

式中:di(i=1,2,…,ne-1)為柵格點(diǎn)ai到ai+1的曼哈頓距離,dne為柵格點(diǎn)ane到a1的曼哈頓距離。

為計(jì)算等勢曲線的切線,需要計(jì)算等勢曲線中柵格點(diǎn)與機(jī)器人當(dāng)前時(shí)刻位置P0(x0,y0)間的斜率。設(shè)按順時(shí)針方向柵格點(diǎn)ai(i=1,2,…,ne)坐標(biāo)表示為(xi,yi),則經(jīng)過柵格點(diǎn)ai的斜率si為

沿順時(shí)針方向依次計(jì)算柵格斜率,在順時(shí)針方向上斜率出現(xiàn)極值的柵格點(diǎn)即為等勢曲線上的切點(diǎn),如圖3中的點(diǎn)A、B、C、D。對(duì)于出現(xiàn)斜率的絕對(duì)值無窮大的柵格點(diǎn),需要進(jìn)行單獨(dú)判斷,即判斷等勢曲線中其所在鄰域內(nèi)的柵格點(diǎn)是否出現(xiàn)在過該點(diǎn)與機(jī)器人當(dāng)前位置R間連線的同一側(cè),如果是,則可判斷為切點(diǎn)。

為了保證切點(diǎn)的有效性,需要保證在按順時(shí)針方向?qū)ふ仪悬c(diǎn)的過程中,相鄰切點(diǎn)間的等勢曲線長度大于常數(shù)dm,且相鄰的有效切點(diǎn)不能出現(xiàn)相同的斜率,dm表示等勢曲線上相鄰切點(diǎn)間允許的最短等勢曲線長度,一般設(shè)置為一個(gè)較小的數(shù),其值越大則等勢曲線上所允許的切點(diǎn)越稀疏。如圖3中如果切點(diǎn)B與切點(diǎn)C間的長度小于dm,則可以認(rèn)為切點(diǎn)B或切點(diǎn)C是無效的。對(duì)于求出的任意有效切點(diǎn),如圖3中的切點(diǎn)A,其到點(diǎn)R的連線長度表示為切點(diǎn)A到機(jī)器人當(dāng)前位置R間的歐氏距離。

2.2 候選路徑生成

根據(jù)得到的障礙物代價(jià)勢場和切線的計(jì)算方法,可以得到不同障礙物的等勢曲線及相應(yīng)的切線。為了更好地描述在避障過程中候選路徑的生成方式,如圖4所示,圖中沒有畫出具體的網(wǎng)格,其中黑色實(shí)線為障礙物的邊界,包含實(shí)時(shí)檢測到的動(dòng)態(tài)障礙物,灰色曲線為其中一條障礙物代價(jià)值為hc的等勢曲線。假設(shè)機(jī)器人當(dāng)前位于點(diǎn)R,機(jī)器人當(dāng)前的目標(biāo)點(diǎn)為點(diǎn)G,為了得到最初的備選路徑,分別計(jì)算經(jīng)過點(diǎn)R和點(diǎn)G的障礙物等勢曲線的切線,如圖4的紅色直線和綠色直線所示,為分別經(jīng)過點(diǎn)R和點(diǎn)G的若干個(gè)切點(diǎn)。由于圖中存在3個(gè)不連通的障礙物,等勢曲線分為獨(dú)立的3段,根據(jù)圖4中切點(diǎn)所在的等勢曲線可以將所有的切點(diǎn)分為3組,分別為set1={1,6,10,12}、set2={2,3,8,11}、set3={4,5,7,9},其中點(diǎn)R和點(diǎn)G自成一個(gè)分組。

圖4 障礙物等勢曲線的切點(diǎn)Fig.4 Tangent point of obstacle equipotential curve

在實(shí)際求解過程中,每個(gè)分組內(nèi)的切點(diǎn)數(shù)目是不確定的,假設(shè)包含起始點(diǎn)和目標(biāo)點(diǎn)在內(nèi)共有nt個(gè)節(jié)點(diǎn),每個(gè)節(jié)點(diǎn)可以和剩下的nt-1個(gè)節(jié)點(diǎn)產(chǎn)生連接,而每2個(gè)節(jié)點(diǎn)間的連接是無向的,無需計(jì)算2次,因此可以得到連接的邊數(shù)為nt(nt-1)/2,實(shí)際上在同一個(gè)分組內(nèi)的點(diǎn)是不需要進(jìn)行連接的;所以最終得到的有效連接數(shù)會(huì)少于nt(nt-1)/2。如圖5所示,圖中繪出的點(diǎn)10和點(diǎn)12的部分有效連接。

對(duì)2點(diǎn)之間的代價(jià)做如下定義:對(duì)于同一個(gè)分組內(nèi)的任意2點(diǎn),連接代價(jià)為其所在等勢曲線上2點(diǎn)之間較短的一條曲線長度,即柵格代價(jià)較小的一側(cè)。對(duì)于不在同一個(gè)分組內(nèi)的兩點(diǎn),如果其間存在直線連接,并且沒有穿越等勢曲線,即沒有穿過障礙物代價(jià)值為大于hc的柵格,其連接代價(jià)為2點(diǎn)之間的歐式距離,如圖5中點(diǎn)R到點(diǎn)1之間的連接;如果其間的直線連接穿越了等勢曲線,其連接代價(jià)為相應(yīng)的等勢曲線部分與2點(diǎn)間直線部分的長度之和,如圖5中點(diǎn)12到點(diǎn)9之間的連接,其連接代價(jià)包含了兩部分,一部分為點(diǎn)12和圖中點(diǎn)p之間曲線連接中較短的一段等勢曲線的長度,另一部分為點(diǎn)p到點(diǎn)9之間直線連接的長度。

圖5 基于等勢曲線切點(diǎn)生成的部分連接代價(jià)Fig.5 Partial connection cost generated based on equipotential curve tangent points

應(yīng)用切線計(jì)算方法和代價(jià)定義,可以得到包括機(jī)器人當(dāng)前位置和目標(biāo)點(diǎn)在內(nèi)的所有切點(diǎn)之間的有效連接及相應(yīng)的連接代價(jià),所有的節(jié)點(diǎn)及其間的連接代價(jià)構(gòu)成了一個(gè)包含起始點(diǎn)和目標(biāo)點(diǎn)的無向圖。為了找到一條可以從起始點(diǎn)到目標(biāo)點(diǎn)的有效且連接代價(jià)最小的候選路徑,需要對(duì)無向圖進(jìn)行遍歷,可以采用深度優(yōu)先或廣度優(yōu)先算法,或基于最小生成樹的求解算法,求解包含起始點(diǎn)和目標(biāo)點(diǎn)的最小子樹,即為代價(jià)最小的有效路徑。如圖6所示,圖中展示了3條連接代價(jià)較小的路徑。

圖6 由最小生成樹得到的3條代價(jià)較小的初始路徑Fig.6 3initial paths with lower cost obtained from the minimum spanning tree

通過路徑生成規(guī)則,最終可以得到1條無碰撞的有效候選路徑,記為P*,如圖7所示,其中紅色曲線為在U型障礙物中的初始候選路徑,R為機(jī)器人的位置,G為當(dāng)前目標(biāo)點(diǎn)。此時(shí)得到的路徑已經(jīng)相對(duì)較為平滑,但對(duì)于機(jī)器人來說并不是一條代價(jià)最優(yōu)的路徑,需要對(duì)路徑進(jìn)行進(jìn)一步的調(diào)整。

圖7 U型障礙物中候選路徑生成示意圖Fig.7 Schematic diagram of candidate path generation in U-shaped obstacle

2.3 機(jī)器人不同初始姿態(tài)下的候選路徑

圖8 初試姿態(tài)及最小轉(zhuǎn)向半徑約束下的初始路徑Fig.8 Initial path under constraints of initial attitude and the minimum turning radius

2.4 候選路徑調(diào)整

在柵格地圖中,初始候選路徑是由緊密相鄰的柵格表示的,不利于后續(xù)路徑的調(diào)整,因此,需要對(duì)初始候選柵格路徑按照一定的規(guī)則進(jìn)行采樣,得到一系列離散的點(diǎn),稱之為錨點(diǎn)。機(jī)器人的路徑可以用一系列的錨點(diǎn)ps,p1,p2,…,pe表示,其中,ps表示路徑起始點(diǎn),pe表示路徑終點(diǎn)。在采樣過程中使用曼哈頓距離來表示柵格地圖中2個(gè)柵格之間的距離。在調(diào)整過程中,相鄰錨點(diǎn)間的距離過小會(huì)使錨點(diǎn)所受合力因相互抵消而變小,需要更多的調(diào)整輪數(shù),同時(shí)也會(huì)增大計(jì)算量,距離過大則會(huì)使錨點(diǎn)過于稀疏,路徑的平滑度變差,因此,為保證整條路徑中錨點(diǎn)稀疏程度的一致性,設(shè)Dmax為路徑調(diào)整過程中的采樣間距,約定在對(duì)候選路徑進(jìn)行采樣時(shí),滿足采樣后相鄰2個(gè)錨點(diǎn)之間的距離Di在小于常量Dmax的前提下取最大值。此時(shí)采樣后的候選路徑P*,可以由錨點(diǎn)集合P*={ps,p1,p2,…,pe}表示,如圖9所示。

圖9 柵格地圖中初始路徑采樣表示Fig.9 Sampling representation of initial path in raster map

下面定義一種路徑調(diào)整算法,將候選路徑P調(diào)整為近似最短的安全路徑。設(shè)錨點(diǎn)ps,p1,p2,…,pe所在柵格的障礙物代價(jià)值為cs,c1,c2,…,ce。則候選路徑的障礙物代價(jià)Cobs為

式中:ci為錨點(diǎn)所在柵格的代價(jià)值,cs為機(jī)器人當(dāng)前位置的柵格代價(jià)值;ce為目標(biāo)點(diǎn)的代價(jià)值;na為采樣錨點(diǎn)的個(gè)數(shù)。

設(shè)錨點(diǎn)ps,p1,p2,…,pe中相鄰錨點(diǎn)間的距離代價(jià)值為h1,h2,…,he。則候選路徑的長度代價(jià)Hobs為

式中:hi(i=1,2,3,…,e)為相鄰錨點(diǎn)之間的曼哈頓距離。此時(shí)機(jī)器人由當(dāng)前位置到目標(biāo)點(diǎn)加權(quán)代價(jià)Ccost為

式中:μ為障礙物代價(jià)和距離代價(jià)對(duì)應(yīng)的權(quán)值系數(shù)。

為了進(jìn)一步對(duì)路徑進(jìn)行調(diào)整,定義相鄰錨點(diǎn)之間存在吸引力。通過錨點(diǎn)之間的引力可以進(jìn)一步調(diào)整候選路徑的長度。約定每個(gè)錨點(diǎn)只會(huì)受到相鄰錨點(diǎn)的影響而不會(huì)受到其他錨點(diǎn)的影響。設(shè)p1的坐標(biāo)為(i1,j1),p2的坐標(biāo)為(i2,j2),p3的坐標(biāo)為(i3,j3),f1表示由點(diǎn)p2到p1的引力向量,f2表示由點(diǎn)p2到p3的引力向量,如圖10所示,可以計(jì)算錨點(diǎn)p2受到的合力F2=f1+f2。此時(shí)錨點(diǎn)p2由于受到合力F2的作用沿F2的方向移動(dòng)。若合力F2表示為(Fx,Fy),則錨點(diǎn)p2調(diào)整之后的坐標(biāo)為p2(i2+σFx,j2+σFy),其中σ為調(diào)整增益,滿足0＜σ＜1。

圖10 候選路徑中錨點(diǎn)p2所受合力F2Fig.10 Resultant force F2on anchor point p2in candidate path

為保證調(diào)整過程中整條路徑中錨點(diǎn)稀疏程度的一致性,約定在路徑調(diào)整過程中相鄰2個(gè)錨點(diǎn)的距離Di滿足約束條件:dmin＜Di＜2dmin,其中dmin為相鄰2個(gè)錨點(diǎn)的最小距離,一般取dmin=0.75Dmax,并且初始錨點(diǎn)ps和目標(biāo)點(diǎn)錨點(diǎn)pe為固定點(diǎn),在調(diào)整過程中不會(huì)發(fā)生變化。當(dāng)出現(xiàn)2個(gè)不相鄰錨點(diǎn)pi-1和pi+1之間的距離Di＜dmin時(shí),可以通過刪除錨點(diǎn)pi來滿足約束條件。當(dāng)2個(gè)相鄰錨點(diǎn)pi-1和pi之間的距離Di＞2dmin時(shí),可以通過插入錨點(diǎn)pnew來滿足約束條件,pnew的坐標(biāo)為

本節(jié)給出了在滿足距離代價(jià)最優(yōu)的條件下候選路徑的調(diào)整方案,下面考慮在滿足障礙物代價(jià)最優(yōu)的條件下候選路徑的調(diào)整方案。

在建立障礙物的二維柵格地圖時(shí),建立了從障礙物邊界向外梯度下降的代價(jià)勢場,用以表示機(jī)器人經(jīng)過此處的障礙物代價(jià)。候選路徑P*是由障礙物等勢曲線得到的,因此可以基于障礙物代價(jià)勢場對(duì)路徑P*進(jìn)行調(diào)整,使路徑的總代價(jià)Ccost最小。

對(duì)于柵格地圖中候選路徑的每一個(gè)錨點(diǎn),按照當(dāng)前位置梯度下降的方向進(jìn)行調(diào)整。需要計(jì)算每一個(gè)錨點(diǎn)處的梯度大小和方向,對(duì)于候選路徑中的任意錨點(diǎn)p(i,j),以p點(diǎn)為中心的3×3柵格進(jìn)行卷積計(jì)算。由梯度的計(jì)算式:

定義E(x,y)為代價(jià)勢場中以錨點(diǎn)p為中心的3×3矩陣,則錨點(diǎn)p在x方向上的梯度為

可以得到錨點(diǎn)p的梯度大小為

由式(23)計(jì)算錨點(diǎn)p的梯度方向:

為了避免候選路徑在調(diào)整的過程中因梯度方向的改變導(dǎo)致調(diào)整后的路徑不平滑,定義單位向量g=(Gx/G,Gy/G)為梯度的方向向量。錨點(diǎn)p在x方向上的梯度調(diào)整量為

在y方向上的梯度調(diào)整量為

式中:ω為梯度調(diào)整量的增益系數(shù)。則錨點(diǎn)p在梯度調(diào)整后的坐標(biāo)為p(i-gx,j-gy)。梯度下降的方向?yàn)楫?dāng)前待調(diào)整錨點(diǎn)在障礙物代價(jià)勢場作用下的調(diào)整方向。候選路徑的總代價(jià)Ccost為

為了保證路徑調(diào)整的可靠性,路徑在每一步調(diào)整的過程中需要滿足如下約束條件:

1)若hc為初始候選路徑中所選等勢曲線對(duì)應(yīng)的障礙物代價(jià)值,在路徑調(diào)整過程中候選路徑中的任何一個(gè)錨點(diǎn)都不會(huì)從當(dāng)前所在柵格移動(dòng)到障礙物代價(jià)值高于hc的柵格。如果針對(duì)當(dāng)前錨點(diǎn)的調(diào)整會(huì)使得調(diào)整之后的錨點(diǎn)所在柵格的障礙物代價(jià)大于hc,則跳過對(duì)當(dāng)前錨點(diǎn)的位置調(diào)整繼續(xù)調(diào)整下一個(gè)錨點(diǎn)。

2)如果考慮機(jī)器人的初始姿態(tài)及最小轉(zhuǎn)彎半徑R0,則應(yīng)當(dāng)保證調(diào)整之后的路徑錨點(diǎn)不會(huì)出現(xiàn)在最小轉(zhuǎn)向圓內(nèi)部。如果針對(duì)當(dāng)前錨點(diǎn)的調(diào)整會(huì)使得調(diào)整之后的錨點(diǎn)與最近的轉(zhuǎn)向圓的圓心距離小于R0,則跳過對(duì)當(dāng)前錨點(diǎn)的位置調(diào)整繼續(xù)調(diào)整下一個(gè)錨點(diǎn)。

3)為了保證路徑在調(diào)整過程中的平滑度,設(shè)Rmin(Rmin＞R0)為路徑調(diào)整過程中的最小曲率半徑,對(duì)于任意相鄰的3個(gè)錨點(diǎn),其確定的圓半徑r不小于最小曲率半徑Rmin。在路徑調(diào)整過程中,對(duì)錨點(diǎn)沿梯度方向調(diào)整和沿相鄰錨點(diǎn)合力方向上的調(diào)整均會(huì)影響該錨點(diǎn)處的曲率圓半徑,而在調(diào)整過程中不存在2種調(diào)整方式都使錨點(diǎn)處的曲率圓半徑變小的情況,在對(duì)錨點(diǎn)沿梯度方向調(diào)整過程或沿相鄰錨點(diǎn)合力方向上的調(diào)整過程中,針對(duì)已滿足最小轉(zhuǎn)彎半徑條件的錨點(diǎn),如果調(diào)整后的結(jié)果會(huì)破壞最小轉(zhuǎn)彎半徑的條件,則跳過對(duì)當(dāng)前錨點(diǎn)的位置調(diào)整繼續(xù)調(diào)整下一個(gè)錨點(diǎn)。如果初始路徑中存在不滿足最小轉(zhuǎn)彎半徑條件的錨點(diǎn),針對(duì)這樣的錨點(diǎn),如果經(jīng)過路徑長度或勢場代價(jià)的調(diào)整會(huì)使調(diào)整后的結(jié)果對(duì)應(yīng)的曲率圓半徑減小,則跳過對(duì)當(dāng)前錨點(diǎn)的相應(yīng)調(diào)整繼續(xù)調(diào)整下一個(gè)錨點(diǎn)。因此,通過本文算法可以保證路徑的平滑度,使路徑滿足最小轉(zhuǎn)彎半徑的約束條件。路徑中錨點(diǎn)的轉(zhuǎn)彎半徑按式(27)進(jìn)行計(jì)算。

圖11為錨點(diǎn)p1、p2、p3所確定的曲率圓,p1的坐標(biāo)為(i1,j1),p2的坐標(biāo)為(i2,j2),p3的坐標(biāo)為(i3,j3),圓心坐標(biāo)O(x,y)為p1、p2與p2、p3所決定的2條中垂線交點(diǎn)決定,即解下面二元一次方程組:

圖11 錨點(diǎn)p2對(duì)應(yīng)的曲率圓半徑Fig.11 Radius of circle of curvature corresponding to anchor point p2

則p1、p2、p3所確定的曲率圓半徑為

本文提出如下路徑規(guī)劃算法來調(diào)整候選路徑P*,具體過程如下:

步驟1以Dmax為采樣間距對(duì)初始候選路徑進(jìn)行采樣,使得采樣后相鄰2個(gè)錨點(diǎn)之間的曼哈頓距離Di在小于常量Dmax的前提下取最大值,得到候選路徑錨點(diǎn)集合P={ps,p1,p2,…,pe}。在調(diào)整過程中任意相鄰的2個(gè)錨點(diǎn)的曼哈頓距離Di滿足約束條件dmin＜Di＜2dmin,其中dmin為任意2個(gè)錨點(diǎn)的最小距離。

步驟2執(zhí)行以下循環(huán):

1)計(jì)算錨點(diǎn)pk(k=1,2,3,…,e-1)所受到的合力F(pk)。

2)嘗試沿F(pk)方向移動(dòng)錨點(diǎn)pk,判斷從起始點(diǎn)到當(dāng)前錨點(diǎn)的Cobs是否減小,如果是,則更新當(dāng)前錨點(diǎn)。

3)判斷錨點(diǎn)是否滿足距離約束條件,如果Dk＜dmin則刪除錨點(diǎn),如果Di＜2dmin則插入新錨點(diǎn)。

4)循環(huán)執(zhí)行第2步直至完成集合中所有錨點(diǎn)的調(diào)整。

步驟3執(zhí)行以下步驟:

1)計(jì)算錨點(diǎn)pk(k=1,2,3,…,e-1)處的梯度大小和方向,并沿著負(fù)梯度的方向根據(jù)梯度的絕對(duì)值大小移動(dòng)錨點(diǎn)pk。

2)判斷錨點(diǎn)是否滿足距離約束條件,如果Dk＜dmin,則刪除錨點(diǎn),如果Di＜2dmin則插入新錨點(diǎn)。

3)重復(fù)第1步和第2步,直至完成集合中所有錨點(diǎn)的調(diào)整。

4)比較第3步調(diào)整之后的路徑總代價(jià)Ccost是否比調(diào)整之前小,如果是則第3步調(diào)整生效,否則不生效。

步驟4重復(fù)執(zhí)行步驟2和步驟3并迭代更新錨點(diǎn),直至Ccost的變化量ΔCcost＜Ccon或循環(huán)達(dá)到一定次數(shù),則退出循環(huán)。其中Ccon＞0為一個(gè)給定的閾值。

根據(jù)本文提出的局部避障算法,可以得到一條由采樣錨點(diǎn)構(gòu)成,相對(duì)較短且安全的路徑。機(jī)器人在實(shí)際路徑跟隨的過程中可以采用分段2次或3次樣條插值法對(duì)調(diào)整后包含有限錨點(diǎn)的路徑進(jìn)行跟隨。

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

為了驗(yàn)證本文算法的特性,在MATLAB中進(jìn)行了仿真。設(shè)定柵格地圖的分辨率為1200×1000m。在本節(jié)中,分別驗(yàn)證了本文避障算法在靜態(tài)障礙物環(huán)境中及包含移動(dòng)障礙物的動(dòng)態(tài)場景下的性能。在場景中,靜態(tài)障礙物可以是墻、機(jī)器和設(shè)備。動(dòng)態(tài)障礙物可以是行走的人,移動(dòng)的設(shè)備或其他機(jī)器人。如圖12所示,機(jī)器人可以通過靜態(tài)柵格地圖及基于傳感器的障礙物信息生成包含障礙物速度信息的代價(jià)勢場,并基于本文算法實(shí)現(xiàn)動(dòng)態(tài)避障到達(dá)目的地。本文重點(diǎn)針對(duì)動(dòng)態(tài)障礙物場景下單個(gè)機(jī)器人的避障進(jìn)行仿真測試實(shí)驗(yàn)。

圖12 靜態(tài)柵格地圖的代價(jià)勢場Fig.12 Cost potential field of static grid map

3.1 靜態(tài)場景下的避障分析

在計(jì)算機(jī)仿真中,通過計(jì)算柵格地圖中障礙物柵格的代價(jià),可以得到如圖12所示的代價(jià)勢場,高度越高的區(qū)域表示機(jī)器人由此處經(jīng)過的代價(jià)越大。在更新障礙物代價(jià)勢場時(shí)可以得到代價(jià)勢場的等勢曲線,圖13為5組對(duì)應(yīng)不同障礙物代價(jià)值的等勢曲線。由圖12與圖13可知,等勢曲線對(duì)應(yīng)的代價(jià)值hc取值越大,則等勢曲線越靠近障礙物。因此,為保證移動(dòng)機(jī)器人導(dǎo)航過程的安全性,避免與障礙物發(fā)生碰撞,機(jī)器人導(dǎo)航過程中障礙物代價(jià)值hc的選擇應(yīng)使得等勢曲線與障礙物的邊界間的最短距離dc略大于0.5倍機(jī)器人的寬度或機(jī)器人外接圓半徑,由圖13可知hc的取值過小會(huì)使得路徑過于遠(yuǎn)離障礙物進(jìn)而無法通過一些狹窄的通道,因此實(shí)際應(yīng)用中應(yīng)根據(jù)機(jī)器人的具體外形參數(shù)選擇合適的hc。其中令式(8)中dop的值取機(jī)器人的外接圓半徑,此時(shí)計(jì)算出的Up的值即為hc對(duì)應(yīng)的障礙物代價(jià)臨界值。

圖13 障礙物代價(jià)勢場的等勢曲線Fig.13 Contour plot in cost potential field of obstacles

假設(shè)機(jī)器人當(dāng)前位于點(diǎn)(550,900)m,向目標(biāo)點(diǎn)柵格(950,100)m行進(jìn),如圖14所示,其中黑色區(qū)域代表障礙物。取障礙物代價(jià)勢場的最大值Cmax=600,初始候選路徑中所選等勢曲線對(duì)應(yīng)的障礙物代價(jià)值hc=200,可以在圖12所示的代價(jià)勢場中得到對(duì)應(yīng)代價(jià)值hc=200的若干段平滑的等勢線。

然后分別尋找經(jīng)過起始點(diǎn)和目標(biāo)點(diǎn)的等高線的nt個(gè)切點(diǎn),不同分組內(nèi)的點(diǎn),其間的連接代價(jià)包含柵格地圖中的障礙物代價(jià)Cobs和距離代價(jià)Hobs。使用經(jīng)典的Prim算法可以得到包含nt-1條邊的最小生成樹,最優(yōu)候選路徑即為最小生成樹中包含起始點(diǎn)和目標(biāo)點(diǎn)的最小子樹,如圖14中的藍(lán)色曲線所示,即為最優(yōu)候選路徑由采樣之后的錨點(diǎn)組成的路徑。此時(shí)的路徑雖然可以用于機(jī)器人的導(dǎo)航,但路徑的代價(jià)和平滑度都不是最優(yōu)的,需要進(jìn)行調(diào)整。根據(jù)2.4節(jié)中的路徑調(diào)整算法,對(duì)候選路徑進(jìn)行調(diào)整,這里障礙物代價(jià)和距離代價(jià)對(duì)應(yīng)的權(quán)值系數(shù)μ取0.65,最終得到圖14中紅色曲線所示的調(diào)整后的最優(yōu)路徑。

圖14 靜態(tài)障礙物場景中本文算法避障效果Fig.14 Obstacle avoidance effect of the proposed method instatic obstacle scene

3.2 動(dòng)態(tài)場景下的避障分析

為了測試本文算法在動(dòng)態(tài)場景下的性能,設(shè)定了如圖15所示的動(dòng)態(tài)障礙物場景。假設(shè)機(jī)器人從R點(diǎn)出發(fā),目標(biāo)點(diǎn)為G,其中A為移動(dòng)的行人障礙物,B和C為2個(gè)移動(dòng)的小車障礙物,在機(jī)器人避障的過程中其位置會(huì)發(fā)生變化。黑色的區(qū)域?yàn)殪o態(tài)障礙物。場景中的障礙物小車B和C及行人障礙物A運(yùn)動(dòng)軌跡為隨機(jī)的曲線。在避障過程中動(dòng)態(tài)障礙物的位置和速度會(huì)發(fā)生變化,因此,在生成動(dòng)態(tài)障礙物的代價(jià)勢場時(shí)需要考慮障礙物的速度大小和方向。按照2.4節(jié)提到的算法生成動(dòng)態(tài)障礙物的速度代價(jià)勢場,和靜態(tài)地圖的代價(jià)勢場進(jìn)行疊加,得到包含動(dòng)態(tài)障礙物信息的代價(jià)勢場。圖16為按照第2節(jié)提出的路徑生成和調(diào)整方案在實(shí)現(xiàn)動(dòng)態(tài)避障過程中機(jī)器人與動(dòng)態(tài)障礙物交互的仿真圖。當(dāng)機(jī)器人移動(dòng)到圖16(a)中位置時(shí),動(dòng)態(tài)障礙物代價(jià)勢場及實(shí)時(shí)路徑如圖16(b)所示,當(dāng)機(jī)器人移動(dòng)到圖16(c)中位置時(shí),動(dòng)態(tài)障礙物代價(jià)勢場及實(shí)時(shí)路徑如圖16(d)所示。通過對(duì)比發(fā)現(xiàn),機(jī)器人可以很好地根據(jù)障礙物的運(yùn)動(dòng)狀態(tài)對(duì)當(dāng)前局部路徑進(jìn)行調(diào)整,獲得較好的避障效果。在動(dòng)態(tài)障礙物場景下機(jī)器人避障路徑及與動(dòng)態(tài)障礙物的交互結(jié)果如圖17所示,R為機(jī)器人的起始位置,G為機(jī)器人的當(dāng)前目標(biāo)點(diǎn)。圖17展示了機(jī)器人及動(dòng)態(tài)障礙物在運(yùn)動(dòng)過程中的運(yùn)動(dòng)軌跡,其中藍(lán)色的路線為障礙物小車B的移動(dòng)軌跡,黃色的路線為障礙物小車C的移動(dòng)軌跡,橙色路徑為行人障礙物A的移動(dòng)軌跡。紅色路徑為機(jī)器人在避障過程中的運(yùn)動(dòng)軌跡,可以看出,在動(dòng)態(tài)移動(dòng)障礙物場景下機(jī)器人可以保持較為平滑的運(yùn)動(dòng)軌跡。圖18展示了機(jī)器人在移動(dòng)的過程中距離動(dòng)態(tài)障礙物A、B、C及當(dāng)前目標(biāo)點(diǎn)G的實(shí)時(shí)距離,其中橫坐標(biāo)是移動(dòng)機(jī)器人從起始點(diǎn)出發(fā)運(yùn)行的時(shí)間,縱坐標(biāo)是和動(dòng)態(tài)障礙物之間的距離。由于在避障過程中引入了障礙物速度對(duì)代價(jià)勢場的影響,在障礙物前進(jìn)方向上的區(qū)域會(huì)產(chǎn)生對(duì)應(yīng)的障礙物速度代價(jià)。可以看出,由于機(jī)器人在運(yùn)動(dòng)過程中通過障礙物速度可以考慮動(dòng)態(tài)障礙物未來一段時(shí)間的位置對(duì)避障過程的影響,在移動(dòng)障礙物場景下具有較高的靈活性。

圖15 初始動(dòng)態(tài)場景中障礙物運(yùn)動(dòng)狀態(tài)Fig.15 Obstacle motion state in initial dynamic scene

圖16 動(dòng)態(tài)場景中機(jī)器人實(shí)時(shí)避障路徑Fig.16 Real-time obstacle avoidance path of robots in dynamic scenarios

圖17 機(jī)器人與動(dòng)態(tài)障礙物交互過程中的運(yùn)動(dòng)軌跡Fig.17 Movement trajectory during interaction between robot and dynamic obstacles

圖18 機(jī)器人距離障礙物A、B、C及目標(biāo)點(diǎn)的實(shí)時(shí)距離Fig.18 Real-time distance of robot from obstacles A,B,C and target point

3.3 不同算法的結(jié)果比較

本文提出了一種基于等勢線和最小生成樹的路徑生成及代價(jià)勢場下的路徑調(diào)整算法。本節(jié)中,為了比較不同路徑規(guī)劃算法的特性和性能,構(gòu)建了如圖19所示的障礙物場景,對(duì)比了模擬人工勢場(APF)算法、RRT*算法、A*算法和本文算法。為了方便不同算法搜索時(shí)間及搜索空間的對(duì)比,定義了地圖遍歷率的評(píng)價(jià)指標(biāo),其值定義為在完成1次路徑規(guī)劃過程中,柵格地圖中被訪問過的柵格數(shù)量與地圖總柵格數(shù)量的比值,反映了算法的空間搜索效率。同時(shí)基于式(17)的路徑評(píng)價(jià)指標(biāo),本文對(duì)比了4種算法在時(shí)間消耗及路徑代價(jià)上的量化指標(biāo),如表1和表2所示,其中障礙物權(quán)值系數(shù)μ取0.40。

經(jīng)典的APF算法[13]定義了2個(gè)主要的勢場:引力勢場和斥力勢場。引力勢場會(huì)吸引機(jī)器人向著目標(biāo)點(diǎn)移動(dòng),障礙物產(chǎn)生的斥力勢場會(huì)排斥機(jī)器人。最終機(jī)器人由受到的虛擬引力和虛擬斥力的合力牽引著向目標(biāo)點(diǎn)移動(dòng),但是他也存在著一些缺點(diǎn),如局部最優(yōu)值問題和目標(biāo)不可達(dá)問題。如圖19中的綠色曲線所示,由于存在局部最優(yōu)值,APF算法無法完成路徑規(guī)劃。當(dāng)不存在局部最優(yōu)值時(shí),如圖20所示障礙物場景,APF算法可以在人工勢場中根據(jù)梯度下降法或鄰域搜索法快速完成路徑搜索,具有較高的時(shí)間效率及較低的地圖遍歷率,然而由表1和表2中的數(shù)據(jù)可知,由于沒有考慮路徑和障礙物間的距離及路徑平滑度等因素,其路徑具有較高的長度代價(jià)和加權(quán)代價(jià),并不是最優(yōu)的選擇。RRT*算法[14]是以起始點(diǎn)為根節(jié)點(diǎn),通過隨機(jī)采樣的算法增加葉子節(jié)點(diǎn),生成一棵隨機(jī)擴(kuò)展樹,當(dāng)存在葉子節(jié)點(diǎn)進(jìn)入目標(biāo)區(qū)域,就得到了從起始點(diǎn)到目標(biāo)點(diǎn)的路徑。他的優(yōu)點(diǎn)是實(shí)現(xiàn)較為簡單,速度快,無論自由度多少,多復(fù)雜的約束都能適用。但缺點(diǎn)也非常明顯,如圖19和圖20中的橙色曲線所示,由于其路徑的隨機(jī)生長方式,其生成的路徑通常包含棱角,不夠光滑,并且靠近障礙物,具有較高的時(shí)間代價(jià)和障礙物代價(jià)。根據(jù)RRT*算法中擴(kuò)展樹的隨機(jī)生長方式,算法在執(zhí)行過程中需要對(duì)整個(gè)地圖空間內(nèi)進(jìn)行隨機(jī)采樣,會(huì)產(chǎn)生很多無效的柵格采樣點(diǎn),因此其地圖遍歷率較高。其路徑規(guī)劃參數(shù)對(duì)比如表1和表2所示,其路徑長度代價(jià)和障礙物代價(jià)都相對(duì)較高,并且不是最優(yōu)的。

圖19 場景1中不同算法的路徑規(guī)劃結(jié)果對(duì)比Fig.19 Comparison of path planning results of different methods in Scenario1

表1 場景1中不同算法路徑代價(jià)指標(biāo)的10次平均值對(duì)比Table1 Ten mean comparison of path cost indexes of different methods in Scenario1

表2 場景2中不同算法路徑代價(jià)指標(biāo)的10次平均值對(duì)比Table2 Ten mean comparison of path cost indexes of different methods in Scenario2

圖20 場景2中不同算法的路徑規(guī)劃結(jié)果對(duì)比Fig.20 Comparison of path planning results of different methods in Scenario1

A*算法[15]的顯著特點(diǎn)是基于柵格地圖完成路徑的搜索,由起點(diǎn)開始,每次優(yōu)先選擇距離起點(diǎn)代價(jià)和距離終點(diǎn)的預(yù)計(jì)代價(jià)之和最小的節(jié)點(diǎn)作為下一個(gè)要遍歷的節(jié)點(diǎn)依次遍歷,直到到達(dá)終點(diǎn)。A*算法沒有考慮和障礙物的安全距離,并且生成的路徑不夠平滑,如圖19和圖20中的藍(lán)色曲線所示。在實(shí)際應(yīng)用過程中需要進(jìn)行進(jìn)一步的優(yōu)化。在目標(biāo)點(diǎn)被障礙物遮擋且地圖分辨率較高的柵格地圖場景中,A*算法會(huì)有大量的無效節(jié)點(diǎn)柵格被加入有效點(diǎn)集合,因此搜索過程中花較多的時(shí)間查找有效點(diǎn)集合,仿真中耗時(shí)高于另外3種算法,并且具有較高的地圖遍歷率,同時(shí)路徑的障礙物代價(jià)值也相對(duì)較高,在實(shí)際過程中很容易與障礙物發(fā)生碰撞。當(dāng)目標(biāo)點(diǎn)不被障礙物遮擋或存在較少的障礙物遮擋時(shí),如圖20所示仿真場景,算法的地圖遍歷率和耗時(shí)相比圖19仿真場景有所降低。APF的時(shí)間復(fù)雜度相對(duì)較低,并且具有較小的障礙物代價(jià)值,但由于其存在局部最優(yōu)問題,在存在局部最優(yōu)值的場景中無法完成避障任務(wù)。RRT*算法采用概率隨機(jī)樹的方式生成避障路徑,也存在障礙物代價(jià)高的問題,并且由于樹的生長方式是隨機(jī)的,在相同的場景下每次的時(shí)間消耗和障礙物代價(jià)都可能發(fā)生變化。由表3的數(shù)據(jù)可知,RRT*算法在相同場景下進(jìn)行多次的路徑規(guī)劃仿真時(shí),算法的長度代價(jià)方差與障礙物代價(jià)方差都比較大,得到的路徑具有隨機(jī)性,這對(duì)于移動(dòng)機(jī)器人的動(dòng)態(tài)避障是不利的,相比之下APF、A*及本文算法在多次重復(fù)實(shí)驗(yàn)時(shí)差較小,具有較高的穩(wěn)定性。本文算法同時(shí)考慮了路徑長度,障礙物代價(jià),路徑平滑度等多種因素,通過求解代價(jià)勢場等勢線切點(diǎn)和求解最小生成子樹的方式獲得初始路徑,假設(shè)錨點(diǎn)間存在引力,并且會(huì)受到代價(jià)勢場的影響,通過調(diào)整路徑錨點(diǎn)獲得有效路徑,由表1和表2的數(shù)據(jù)可知,本文算法在時(shí)間復(fù)雜度和障礙物代價(jià)值上都具有較好的指標(biāo),同時(shí)也具有良好的地圖遍歷率指標(biāo)。

表3 場景2中不同算法路徑代價(jià)方差對(duì)比Table3 Variance comparison of path costs of different methods in Scenario2

3.4 復(fù)雜場景下的自主避障

本節(jié)測試了在相對(duì)較為復(fù)雜的場景下本文算法的避障效果,并對(duì)比了APF算法、A*算法和本文算法的不同表現(xiàn),柵格地圖的大小同樣是1200×1000m,如圖21所示,黑色柵格區(qū)域?yàn)檎系K物,紅色點(diǎn)(300,900)為起始點(diǎn),綠色點(diǎn)(900,60)為目標(biāo)點(diǎn)。選擇高度為hc=120的等勢線可以在代價(jià)勢場中得到圖22紅色曲線所示的初始候選路徑。為了方便比較,地圖中沒有設(shè)置局部最優(yōu)值,保證了APF算法也能夠完成避障。

圖21 復(fù)雜場景下的避障結(jié)果對(duì)比Fig.21 Comparison of obstacle avoidance results in complex scenarios

圖22 基于等勢曲線生成的初始候選路徑Fig.22 Initial candidate path generated based on equipotential curve

表4 不同算法評(píng)價(jià)指標(biāo)的10次平均值對(duì)比Table4 Ten average comparison of cost indexes of different methods

實(shí)驗(yàn)結(jié)果表明,本文算法通過對(duì)候選路徑在路徑長度和障礙物代價(jià)上的調(diào)整,得到的路徑具有較好的平滑性和安全性,同時(shí)加權(quán)路徑代價(jià)也相對(duì)較小。

4 結(jié) 論

針對(duì)動(dòng)態(tài)障礙物場景下的移動(dòng)機(jī)器人安全避障問題,本文提出了基于障礙物代價(jià)勢場的避障路徑生成及調(diào)整算法,并進(jìn)行了仿真驗(yàn)證。

1)本文所提算法可以得到較為平滑的路徑,能夠在和障礙物保持安全距離的前提下做到路徑最短。在靜態(tài)場景下,如果障礙物本身的位置不會(huì)發(fā)生變化,通過所提算法動(dòng)態(tài)的生成可行路徑,并在機(jī)器人不斷運(yùn)行的過程中對(duì)路徑進(jìn)行調(diào)整,可以獲得可靠的導(dǎo)航和避障結(jié)果。

2)對(duì)于移動(dòng)的障礙物,所提算法加入了其移動(dòng)速度對(duì)障礙物代價(jià)勢場的影響,在障礙物前進(jìn)方向的區(qū)域范圍內(nèi)產(chǎn)生較強(qiáng)的代價(jià)勢場,為機(jī)器人規(guī)避移動(dòng)中的障礙物預(yù)留了充分的裕量,使得機(jī)器人可以對(duì)障礙物未來的運(yùn)動(dòng)態(tài)勢進(jìn)行一定程度的預(yù)測,對(duì)于移動(dòng)中的障礙物也能夠達(dá)到較好的響應(yīng)效果。

3)所提算法綜合考慮了路徑的長度和障礙物代價(jià),相比其他算法,在路徑平滑指標(biāo)、長度代價(jià)等單一指標(biāo)上不是最優(yōu)的,但是通過所提算法對(duì)路徑的調(diào)整使路徑具有較好的安全性及較小的加權(quán)代價(jià),有利于實(shí)際應(yīng)用中實(shí)現(xiàn)良好的路徑跟蹤和避障。