基于預(yù)測窗的輪式移動機(jī)器人最優(yōu)避障避碰算法

劉中常 王明杰 郭 戈

(1.大連海事大學(xué)船舶電氣工程學(xué)院,遼寧大連 116026;2.東北大學(xué)流程工業(yè)綜合自動化國家重點實驗室,遼寧沈陽 110004)

1 引言

具備自主感知、決策和執(zhí)行能力的移動機(jī)器人之間的相互避碰以及躲避(靜止或移動的)障礙物是機(jī)器人研究領(lǐng)域的一個重要問題,其基本目標(biāo)是控制移動機(jī)器人在有障礙物和其他移動機(jī)器人的環(huán)境中實現(xiàn)無碰撞運動并到達(dá)規(guī)定的目的點,這在倉儲物流、智能交通、區(qū)域搜索覆蓋等任務(wù)中有著廣泛的應(yīng)用前景.

常用的避障和避碰算法包括基于實時狀態(tài)的勢場函數(shù)法[1]和基于預(yù)測的動態(tài)窗法[2]、模型預(yù)測法[3]、障礙函數(shù)法[4]和速度障礙法[5]等及其混合方法[6].基于實時狀態(tài)的勢場函數(shù)法在結(jié)構(gòu)上比較簡單,因而得到了廣泛應(yīng)用,但是其容易陷于局部極值而使得機(jī)器人避碰或避障失敗或由于障礙物的存在而無法到達(dá)目標(biāo)點.基于預(yù)測的方法可以根據(jù)當(dāng)前時刻的狀態(tài)來估計未來會發(fā)生碰撞的情況,因而可以通過優(yōu)化的方法求得機(jī)器人當(dāng)前時刻的最優(yōu)避碰避障策略.大多數(shù)基于預(yù)測的算法[7–8]是針對無自主決策能力的障礙物來設(shè)計的,移動機(jī)器人根據(jù)預(yù)測的障礙物未來的運動軌跡來進(jìn)行避碰控制.例如,文獻(xiàn)[5]中通過構(gòu)造速度障礙(velocity obstacle,VO)的方法,把移動機(jī)器人與障礙物所有可能會發(fā)生碰撞的相對速度定義成一個速度障礙集合,通過在速度障礙集合之外選取最優(yōu)的避障速度來達(dá)到避障的目的.當(dāng)障礙物是具有主動避障能力的移動機(jī)器人時,簡單地把其看作一般被動障礙物來躲避就很可能會導(dǎo)致運動的振蕩[9–10],甚至可能發(fā)生碰撞.因此,文獻(xiàn)[10]中,在文獻(xiàn)[5]的基礎(chǔ)上提出了協(xié)作式的速度障礙(reciprocal velocity obstacle,RVO),將避障責(zé)任平均分配給每個移動機(jī)器人,實現(xiàn)了多移動機(jī)器人的協(xié)同避碰,但在機(jī)器人的數(shù)量較多時仍會導(dǎo)致運動的振蕩.為了彌補(bǔ)這個不足,文獻(xiàn)[11]繼續(xù)進(jìn)行改進(jìn),提出了最優(yōu)相互避碰算法(optimal reciprocal collision avoidance,ORCA),將無碰撞的安全速度區(qū)域用半平面來表示,然后通過優(yōu)化算法求解最優(yōu)的無碰行駛速度.由于這一算法能保證避障和相互避碰、具有最優(yōu)性且易于計算,因而得到了廣泛的應(yīng)用.例如,應(yīng)用于多機(jī)器人的區(qū)域覆蓋問題[12];最近,本文作者在文獻(xiàn)[13]中采用兩個預(yù)測周期來躲避高速移動的障礙物.但該算法只適用于具有完整約束動態(tài)的機(jī)器人,對于非完整約束機(jī)器人,已有文獻(xiàn)中的做法是將機(jī)器人模型近似轉(zhuǎn)換成完整約束機(jī)器人模型.例如,文獻(xiàn)[14]把差分驅(qū)動機(jī)器人近似成一種完整約束機(jī)器人,再應(yīng)用ORCA算法計算出最優(yōu)的避碰速度并轉(zhuǎn)換成左右輪的驅(qū)動速度;文獻(xiàn)[15]研究了后輪轉(zhuǎn)向固定而通過前輪進(jìn)行轉(zhuǎn)向的車式機(jī)器人,根據(jù)其跟蹤完整約束軌跡的最大誤差來增大有效避碰半徑,從而保證其能夠無碰行駛;文獻(xiàn)[16]將文獻(xiàn)[15]中的方法進(jìn)行了推廣,研究了更一般化的具有非完整約束的機(jī)器人的避碰問題.

除了上述問題,文獻(xiàn)[10–11]中基于速度障礙的避碰算法計算出的控制量是機(jī)器人的速度,并假設(shè)求得的新速度瞬時可達(dá),導(dǎo)致機(jī)器人的運動軌跡和速度是分段的,不夠平滑,且不適用于具有加速度約束的機(jī)器人.另外,這些算法假設(shè)障礙物在預(yù)測窗內(nèi)速度保持初始值不變,這并不利于躲避速度變化劇烈的障礙物.對此,文獻(xiàn)[17]提出了基于加速度—速度障礙(acceleration velocity obstacle,AVO)的相互避碰算法,采用比例控制的方法使機(jī)器人在預(yù)測窗內(nèi)加速到期望速度.文獻(xiàn)[18]中進(jìn)一步采用機(jī)器人位置的多階導(dǎo)數(shù)進(jìn)行比例反饋控制來對期望速度進(jìn)行跟蹤控制.文獻(xiàn)[19]采用LQR方法來設(shè)計速度跟蹤控制器.文獻(xiàn)[20]中提出了一種基于控制障礙(control obstacle,CO)的避碰算法,對機(jī)器人用于避碰的控制增量進(jìn)行規(guī)劃,從而將文獻(xiàn)[17]中的算法進(jìn)行了統(tǒng)一化處理,且能適用于不同類型的移動機(jī)器人之間的相互避碰.上述這些方法雖然可以在一定程度上平滑機(jī)器人的運動軌跡,但其本質(zhì)上都是對期望速度的跟蹤控制,不但會有跟蹤誤差,而且可能導(dǎo)致機(jī)器人的加速度變化劇烈,從而使得執(zhí)行機(jī)構(gòu)產(chǎn)生較大磨損.為此,文獻(xiàn)[21]將ORCA算法和模型預(yù)測控制相結(jié)合,對機(jī)器人的加速度進(jìn)行優(yōu)化控制,取得了良好的效果,但也只研究了線性的機(jī)器人運動模型.上述其他文獻(xiàn)(例如文獻(xiàn)[20])在處理非線性的機(jī)器人運動模型時,一般通過近似線性化轉(zhuǎn)為線性模型來研究,因而存在控制精度不高的問題.

為此,本文針對非線性、非完整約束的輪式移動機(jī)器人進(jìn)行避碰避障的研究,選取線加速度、角速度作為其控制量來解決機(jī)器人的速度突變的問題,并通過精確反饋線性化的方法將非線性的模型轉(zhuǎn)化為線性模型進(jìn)行研究.本文針對線性模型引入了加速度變化障礙(acceleration change obstacle,ACO)的新概念,并在此基礎(chǔ)上設(shè)計了可保證多機(jī)器人避障和相互避碰的算法.此算法中,把可能會導(dǎo)致碰撞的所有相對虛擬加速度變化定義成ACO;然后讓每個機(jī)器人在ACO之外構(gòu)建滿足自己最大加速度約束的避碰虛擬加速度變化集合;在此集合中通過優(yōu)化指標(biāo)函數(shù)得到最優(yōu)的虛擬加速度變化量,最后將其轉(zhuǎn)換成輪式移動機(jī)器人實際的控制輸入量.這種算法和基于速度障礙的相互避碰算法[10–16]相比,可以使機(jī)器人在避碰過程中的運動軌跡更為平滑.與基于速度跟蹤控制的算法[17–20]相比,機(jī)器人的線速度和方向角的變化幅值更小,且角速度和線加速度的變化也更為平順.另外,仿真比較顯示本文所提出的算法的平均計算時間比文獻(xiàn)[20]中的算法要短,因而在時效上也具有優(yōu)勢.

2 問題描述

2.1 符號

本文使用花體大寫字母A表示向量集合,斜體大寫字母A表示矩陣,黑體字母a表示矩陣向量,斜體小寫字母a表示標(biāo)量.定義如下的符號和運算:為矩陣向量x的歐幾里德范數(shù);x·y表示內(nèi)積運算;定義運算aX={ax|x∈X},X ⊕Y={x+y|x∈X,y∈Y},其中⊕表示閔可夫斯基和;X Y=;D(c,r)=r}為圓心在c半徑為r的圓盤.

2.2 機(jī)器人模型

本文研究的具有非完整約束的輪式移動機(jī)器人如圖1所示,其中:x,y為機(jī)器人在全局坐標(biāo)系XOY下的位置坐標(biāo),v為其行駛的線速度,θ為行駛的方向角.

圖1 輪式移動機(jī)器人示意圖Fig.1 Illustration of a wheeled mobile robot

在滿足非完整約束的假設(shè)下,輪式移動機(jī)器人的運動學(xué)模型表示如下[4,20]:

其中:ξ=[x y v θ]為當(dāng)前時刻輪式移動機(jī)器人狀態(tài)向量;a是機(jī)器人的線加速度;ω是角速度.定義u=[a ω]T為機(jī)器人的控制輸入.

假設(shè)機(jī)器人的運動場景中同時存在多個障礙物和移動機(jī)器人,且其運動學(xué)模型都用式(1)來描述,本文的目標(biāo)就是為每個機(jī)器人設(shè)計控制量u,使機(jī)器人在運動過程中避免與障礙物以及其他機(jī)器人發(fā)生碰撞.

本文采用的主要思路是首先將式(1)中的非線性模型通過反饋線性化轉(zhuǎn)化為易于處理的線性模型,然后基于預(yù)測窗的方法為每個機(jī)器人構(gòu)建可以保證其無碰行駛的虛擬控制量集合,最后在該集合中優(yōu)化求解最優(yōu)的無碰虛擬控制量并轉(zhuǎn)換為機(jī)器人的實際控制量u.

3 加速度變化障礙的構(gòu)建

本節(jié)首先通過反饋線性化把輪式移動機(jī)器人的非線性運動學(xué)模型轉(zhuǎn)化成線性模型;然后根據(jù)線性模型構(gòu)建加速度變化障礙,從而為后續(xù)的避障和相互避碰算法打下基礎(chǔ).

3.1 反饋線性化

考慮到輪式機(jī)器人的運動學(xué)模型(1)是受非完整約束的非線性系統(tǒng),依據(jù)此模型設(shè)計基于預(yù)測的避障、避碰算法將會使得計算非常復(fù)雜,甚至影響計算效率.因此,本文首先對系統(tǒng)(1)進(jìn)行反饋線性化處理.為此,先把系統(tǒng)(1)寫成如下標(biāo)準(zhǔn)的非線性系統(tǒng)的形式:

因為式(2)中是一個2輸入2輸出的系統(tǒng),且其相對階等于系統(tǒng)的階數(shù),所以可以對式(2)進(jìn)行反饋線性化.對于該系統(tǒng),本文構(gòu)造狀態(tài)反饋控制律如下:

其中:d是一個虛擬控制量,α(ξ),β(ξ)是待確定的函數(shù).根據(jù)反饋線性化的相關(guān)理論[22]可得

其中:r表示相對階;表示f(ξ)對g(ξ)的r階李導(dǎo)數(shù);表示混合李導(dǎo)數(shù).為了便于設(shè)計,

綜合上式可得

利用式(8),系統(tǒng)(2)就被轉(zhuǎn)換成如下的線性系統(tǒng):

其中:p=h(ξ)=[x y]T和v=[vxvy]T為該線性系統(tǒng)的狀態(tài)向量;a=[axay]T=d為系統(tǒng)(9)的虛擬控制量;η為系統(tǒng)的輸出;I是單位矩陣.

3.2 加速度變化障礙

為了計算出機(jī)器人最優(yōu)的無碰控制量,假設(shè)在未來一個較短的預(yù)測窗τ內(nèi)線性系統(tǒng)(9)的控制量a是恒定的,即機(jī)器人做勻變速運動.然后基于勻變速運動學(xué)規(guī)律引入加速度變化障礙(acceleration change obstacle,ACO)的概念,其構(gòu)造方法如下:

在二維歐氏空間中(見圖2),定義移動機(jī)器人A和機(jī)器人(或障礙物)B的半徑分別為rA,rB,令rAB=rA+rB.假設(shè)A,B在未來一段較短的預(yù)測窗τ內(nèi)的加速度是恒定的,即做勻變速運動,則其運動軌跡描述如下:?t∈(0,τ],

其中:pA=pA(0),pB=pB(0),vA=vA(0),vB=vB(0),aA=aA(0)和aB=aB(0)分別為A和B在當(dāng)前計算時刻相對于原點的位置、速度和加速度,?aA,?aB為待確定的加速度變化量(見圖2).則A,B的相對位置變化為

其中:vAB=vA?vB,aAB=aA?aB,?aAB=?aA??aB.若A,B在t時刻發(fā)生碰撞,則有AB(t)rAB,其示意圖見圖2.

Fig.2 兩個移動機(jī)器人的相對運動示意圖Fig.2 Illustration of the motions of two mobile robots

那么,可能導(dǎo)致A,B在預(yù)測窗τ內(nèi)發(fā)生碰撞的相對加速度變化集合為

圖3 加速度變化障礙示意圖Fig.3 Acceleration change obstacle

其中q±(p,r)為經(jīng)過原點的兩條直線,其與圓心在p點半徑為r的圓的兩邊分別相切,根據(jù)幾何關(guān)系有

圖4 速度障礙示意圖Fig.4 Velocity obstacle

4 基于加速度變化障礙的避障、避碰算法

本節(jié)將在上一節(jié)得到的線性化的運動模型和加速度變化障礙的基礎(chǔ)上,構(gòu)建機(jī)器人的避障和相互避碰算法.

4.1 協(xié)作式避碰加速度變化集合

首先考慮簡單的兩個輪式移動機(jī)器人A和B的情況,每個機(jī)器人都采用上一節(jié)中構(gòu)造加速度變化障礙的方法分別獨立計算相對于彼此的加速度變化障礙由定義式(14)可知設(shè)機(jī)器人A和B的線加速度的最大值分別為則其相對線加速度的最大值為可選取的加速度變化?aAB需滿足約束即?aAB∈D(?aAB,).則集合D()是滿足最大相對加速度約束的無碰相對加速度變化的集合,將其表示為那么移動機(jī)器人A和B在預(yù)測窗τ內(nèi)不發(fā)生碰撞的一個必要條件是

根據(jù)協(xié)作式避碰原則,機(jī)器人A,B主動獨立計算并執(zhí)行自己的加速度變化量?aA,?aB以使?aAB=?aA??aB滿足式(17).為了體現(xiàn)公平性,相對加速度變化量?aAB應(yīng)該平均分配給兩個機(jī)器人,即令

另外,每個機(jī)器人的加速度變化要滿足各自最大加速度約束條件即

基于上述要求,機(jī)器人A,B用于避碰的加速度變化量的取值集合可以分別設(shè)可以證明這兩個集合的閔可夫斯基差集是集合的子集:

因此,要為每個機(jī)器人提供無碰撞的加速度變化可行集合,需要首先計算其相對加速度變化量的可行集合考慮到可能是非凸的,為了提高算法的計算效率,用一個凸半平面C來代替,其滿足如下兩個條件:

同樣可以證明其閔可夫斯基差集是C的子集.

下面來求解半平面C.考慮到滿足式(18)約束的半平面C有無數(shù)多個,選取其中能最大化集合C ∩的那個,可通過下述方法求得.定義為集合的凸包(convex hull,CH),用符號?表示這一閉集的邊界.此邊界上最接近原點的點表示為w,其數(shù)學(xué)定義如下:

其中n為w點處的法線向量,示意圖見圖5.

圖5 半平面C(斜線標(biāo)注的區(qū)域)的示意圖Fig.5 The half-plane C (region marked with slashes)

圖6 和(斜線標(biāo)注的區(qū)域)的示意圖Fig.6 and(regions marked with slashes)

其示意圖見圖6.

類似可得移動機(jī)器人B用于躲避A且滿足最大加速度約束的避碰加速度變化集合為

4.2 多移動機(jī)器人的最優(yōu)相互避碰和避障方法

其是由多個半平面圍成的凸集,見圖7.

圖7 移動機(jī)器人A的(灰色區(qū)域)示意圖Fig.7 Illustration of (grey region)for robot A

其中性能指標(biāo)J的定義為

綜合式(8)(29)可得機(jī)器人A在避碰預(yù)測窗τ內(nèi)的最優(yōu)實際無碰控制量如下:

由于以上算法構(gòu)造過程中的機(jī)器人A具備一般性,其他任一移動機(jī)器人均可采用與之相同的方法獨立求解各自的最優(yōu)實際無碰控制量.因此本文的算法是分布式的.另外需要注意的是,如果計算出式(28)中的避碰加速度變化集合為空集,即當(dāng)前時刻預(yù)測未來的τ時間段內(nèi)不存在可以避碰的控制量,則令機(jī)器人以最大的減速度進(jìn)行減速,以盡量減輕可能發(fā)生的碰撞所造成的危害.在減速的過程中,機(jī)器人仍然在每個采樣時刻利用探測到的信息來重新計算集合一旦其變?yōu)榉强占蟿t立即求解和執(zhí)行避碰控制量.

5 仿真驗證

本節(jié)通過MATLAB仿真來展示本文所設(shè)計算法的有效性,并與文獻(xiàn)[20]中的基于控制障礙的CO算法進(jìn)行對比.仿真中選取采樣周期為0.1 s,避碰預(yù)測窗τ=2 s;所有移動機(jī)器人的半徑都設(shè)為r=0.35 m,最大線加速度都為1 m/s2,每個機(jī)器人的期望速度的大小為定常值1 m/s,方向由各自的當(dāng)前位置指向目標(biāo)位置.

仿真1 兩個移動機(jī)器人A,B初始位置分別位于[0 m,0 m]以及[10 m,10 m]處,初始線速度都為1 m/s,初始行駛角分別為初始線加速度和角速度都為0.仿真中兩個機(jī)器人需要互換位置,因而在相會處會避免發(fā)生碰撞.

采用本文的ACO算法和文獻(xiàn)[20]中的CO算法的移動機(jī)器人運動軌跡見圖8.可見,兩種避碰算法皆可實現(xiàn)輪式移動機(jī)器人的無碰撞運動,且機(jī)器人的運動軌跡相似.

圖8 分別采用兩種不同算法時機(jī)器人的運動軌跡Fig.8 Motion trajectories of the robots when using two different algorithms,respectively

但由圖9可見,采用本文避碰算法時,機(jī)器人A在避碰過程中線速度和角度的變化幅度都小于采用文獻(xiàn)[20]中的CO算法得到的值.圖10展示了機(jī)器人A采用這兩種算法時的控制量變化,可以看出機(jī)器人A的避碰控制量在CO算法下的波動比在本文ACO算法下更為劇烈,這會對機(jī)器人的驅(qū)動裝置提出更高的性能要求,也更容易使其發(fā)生磨損.因此,本文所設(shè)計的算法更為合理和實用.

圖9 分別采用CO和ACO兩種算法時機(jī)器人A的狀態(tài)對比圖Fig.9 Comparison of the states of robot A under algorithm CO and ACO,respectively

圖10 分別采用CO和ACO兩種算法時機(jī)器人A的控制量對比圖Fig.10 Comparison of robot A’s control inputs under algorithm CO and ACO,respectively

仿真2為了分析本文所提出的ACO算法的計算效率,取不同數(shù)量的移動機(jī)器人分別進(jìn)行避碰仿真,記錄在不同數(shù)量下平均每個機(jī)器人每次計算出各自的最優(yōu)控制量的平均時間,并且與文獻(xiàn)[20]中的CO避碰算法進(jìn)行對比,統(tǒng)計結(jié)果見圖11.可見,兩種算法的平均單次計算時間都與機(jī)器人的數(shù)量成近似線性關(guān)系,因而具有良好的可擴(kuò)展性.相比之下,本文算法比文獻(xiàn)[20]中的算法需要的平均計算時間要更短,因而在計算效率上優(yōu)于文獻(xiàn)[20]中的算法.

圖11 兩種算法的平均計算時間對比圖Fig.11 Comparison of average computation time needed by the two algorithms

仿 真3考慮3個移動機(jī)器人A,B,C和一個以線加速度0.1 m/s2做勻加速運動的移動障礙物O,初始二維坐標(biāo)位置分別為[0 m,0 m],[10 m,10 m],[0 m,10 m],[10 m,0 m],即位于二維平面的4個角上.其初始線速度都為1 m/s,初始行駛角分別為即位于對角位置的機(jī)器人或障礙物向?qū)Ψ降姆较蜻\動.移動機(jī)器人A,B,C的初始線加速度和角速度都為0.圖12展示了本文算法在輪式移動機(jī)器人相互避碰及同時躲避障礙問題上的有效性.

圖12 機(jī)器人A,B,C躲避移動障礙物O的運動軌跡Fig.12 The motion trajectories of three robots A,B and C avoiding the moving obstacle O

6 結(jié)論

針對輪式移動機(jī)器人的相互避碰和避障問題,本文首先對其運動學(xué)模型進(jìn)行了反饋線性化,然后在得到的線性模型的基礎(chǔ)上提出了一種適用于非完整約束系統(tǒng)的相互避碰算法,彌補(bǔ)了已有的基于速度障礙和速度跟蹤方法的不足,為移動機(jī)器人提供了平滑的運動軌跡,而且計算效率也具有優(yōu)勢.考慮到本文的避碰算法只用于同類型機(jī)器人之間的相互避碰,下一步的研究可將本算法推廣到不同類型的移動機(jī)器人之間的相互避碰中.另外,考慮到實際應(yīng)用中,機(jī)器人的角加速度也會存在約束,因此針對機(jī)器人在避碰和避障時的角加速度性能進(jìn)行研究也是有待解決的問題.