王博洋 ,關(guān)海杰 ,龔建偉 ,陳慧巖 ,趙卉菁

(1.北京大學(xué) 信息科學(xué)技術(shù)學(xué)院,北京 100871;2.北京理工大學(xué) 機(jī)械與車(chē)輛學(xué)院,北京 100081)

0 引言

無(wú)人駕駛車(chē)輛的運(yùn)動(dòng)規(guī)劃方法能夠在可通行區(qū)域內(nèi),結(jié)合車(chē)輛自身的運(yùn)動(dòng)特性約束,生成從起始位姿到終止位姿的無(wú)碰撞軌跡,是無(wú)人駕駛系統(tǒng)的重要組成部分[1-2]。目前基于現(xiàn)役裝備改造的無(wú)人駕駛履帶車(chē)輛和新近研發(fā)的無(wú)人駕駛履帶車(chē)輛,在轉(zhuǎn)向機(jī)構(gòu)的組成上存在顯著差異,如何對(duì)轉(zhuǎn)向機(jī)構(gòu)不同所導(dǎo)致的運(yùn)動(dòng)特性差異進(jìn)行建模,并將其融入到運(yùn)動(dòng)規(guī)劃系統(tǒng)中,是建立異構(gòu)履帶車(chē)輛統(tǒng)一運(yùn)動(dòng)規(guī)劃方法的關(guān)鍵。

在運(yùn)動(dòng)規(guī)劃方法的研究中,將連續(xù)的軌跡分割成離散的基本單元,繼而通過(guò)運(yùn)動(dòng)基元生成與選擇的方式構(gòu)建運(yùn)動(dòng)規(guī)劃方法,能夠顯著提升算法的效率[3]。運(yùn)動(dòng)基元生成的核心是解決兩點(diǎn)的邊界值問(wèn)題,即根據(jù)不同的目標(biāo)位姿生成一組備選基元[4]。Hybrid A*以車(chē)輛運(yùn)動(dòng)學(xué)模型為依托,通過(guò)對(duì)車(chē)輛的控制空間進(jìn)行采樣,生成一組備選基元集[5];也可以通過(guò)對(duì)狀態(tài)空間進(jìn)行采樣,完成備選基元集的設(shè)置,具體的采樣方式包括:以優(yōu)化后的參考線偏移點(diǎn)作為采樣點(diǎn)[6],對(duì)超越障礙物的局部位置進(jìn)行局部采樣[7],以晶格形式完成空間采樣[8],以及同時(shí)對(duì)時(shí)間和空間兩個(gè)維度進(jìn)行采樣[9]。雖然上述基元的生成方法能夠滿足城市環(huán)境與越野環(huán)境下的基本規(guī)劃需求,但生成的基元未能考慮異構(gòu)履帶車(chē)輛運(yùn)動(dòng)特性的差異。

在基元生成的基礎(chǔ)上,如何從備選基元集合中擴(kuò)展并選取適當(dāng)?shù)幕獦?gòu)成期望軌跡,是提升規(guī)劃軌跡質(zhì)量的另一項(xiàng)重要因素[10]。Klanˇcar 等著重討論了在基元擴(kuò)展過(guò)程中所要保證的曲率和速度的連續(xù)性[11]。但上述方法的平滑過(guò)渡為在線優(yōu)化,降低了規(guī)劃算法的效率。Dolgov 等在基元的選擇過(guò)程中既考慮了車(chē)輛的非完整性約束,又結(jié)合了環(huán)境約束的啟發(fā)項(xiàng)[12]。朱冰等通過(guò)基于安全距離模型的安全場(chǎng),對(duì)基元的擴(kuò)展與選擇提供了指引[13]。Hu 等在基元的選擇過(guò)程中綜合考慮了動(dòng)靜態(tài)障礙物約束與軌跡的平滑度約束[14]。Zhang 等則通過(guò)時(shí)空地圖構(gòu)建基元選擇的約束條件[15]。但上述基元的選擇代價(jià),主要是從環(huán)境約束的角度設(shè)定相應(yīng)的指標(biāo),未能關(guān)注異構(gòu)履帶車(chē)輛的行為差異性。尤其是如何在基元的選擇過(guò)程中權(quán)衡確定性較強(qiáng)的規(guī)定轉(zhuǎn)向半徑運(yùn)動(dòng)模式,與不確定性較強(qiáng)的非規(guī)定轉(zhuǎn)向半徑運(yùn)動(dòng)模式。

前期的研究已經(jīng)針對(duì)有級(jí)轉(zhuǎn)向履帶車(chē)輛軌跡與操控層的行為模型[16-17],以及無(wú)級(jí)轉(zhuǎn)向履帶車(chē)輛的規(guī)劃與控制方法開(kāi)展了相應(yīng)工作[18-19]。雖然研究人員通過(guò)建立不同類型車(chē)輛的運(yùn)動(dòng)微分約束,在基元生成過(guò)程中引入車(chē)輛的差異化特性[8],但對(duì)于異構(gòu)履帶車(chē)輛而言,很難從運(yùn)動(dòng)學(xué)模型角度區(qū)分車(chē)輛運(yùn)動(dòng)模式之間的差異。本文以運(yùn)動(dòng)基元的離線優(yōu)化生成與在線擴(kuò)展選擇為依托,既將異構(gòu)履帶車(chē)輛的行為特性差異融入到基元的優(yōu)化生成中,又在基元的選擇中考慮了規(guī)定轉(zhuǎn)向半徑模式與非規(guī)定轉(zhuǎn)向半徑模式的差異性,最終提出了能夠體現(xiàn)異構(gòu)履帶車(chē)輛特性差異的統(tǒng)一運(yùn)動(dòng)規(guī)劃方法,生成與車(chē)輛運(yùn)動(dòng)模式匹配性高的軌跡。

1 統(tǒng)一運(yùn)動(dòng)規(guī)劃方法整體流程

本文提出的統(tǒng)一運(yùn)動(dòng)規(guī)劃方法主要包含兩方面內(nèi)容:運(yùn)動(dòng)基元的優(yōu)化生成、運(yùn)動(dòng)基元的擴(kuò)展選擇。統(tǒng)一運(yùn)動(dòng)規(guī)劃方法的整體流程如圖1 所示。其中,運(yùn)動(dòng)基元的優(yōu)化生成方法,能夠在離線狀態(tài)下生成每一類異構(gòu)履帶車(chē)輛獨(dú)有的運(yùn)動(dòng)基元庫(kù);運(yùn)動(dòng)基元的在線擴(kuò)展,以可通行區(qū)域與上一時(shí)刻的擴(kuò)展基元簇為約束,擴(kuò)展出備選的基元集合;運(yùn)動(dòng)基元的在線選擇模塊,則從集合中選擇出最優(yōu)的基元,構(gòu)成運(yùn)動(dòng)基元序列,即具備時(shí)空信息的運(yùn)動(dòng)規(guī)劃結(jié)果。

圖1 統(tǒng)一運(yùn)動(dòng)規(guī)劃方法的整體流程Fig.1 Overall process of an unified motion planning method

2 運(yùn)動(dòng)基元的優(yōu)化生成

在異構(gòu)履帶車(chē)輛行為基元的優(yōu)化生成問(wèn)題中,優(yōu)化求解的變量為車(chē)體的縱橫向運(yùn)動(dòng)參量u,優(yōu)化的目標(biāo)函數(shù)g主要考慮了生成的運(yùn)動(dòng)基元在軌跡和速度層面的平滑度。優(yōu)化問(wèn)題的約束條件包括了履帶車(chē)輛的運(yùn)動(dòng)微分約束f、異構(gòu)履帶車(chē)輛的差異化行為約束B(niǎo)、優(yōu)化求解變量u與車(chē)輛狀態(tài)參量s的不等式約束U,以及運(yùn)動(dòng)基元的平滑過(guò)渡約束T,并且基元生成時(shí)車(chē)輛的運(yùn)動(dòng)具有單調(diào)性(前進(jìn)或后退)。優(yōu)化問(wèn)題的定義如(1)式所示:

式中:優(yōu)化求解的變量u(t)=[η(t),α(t)]T,η(t)為車(chē)輛縱向加速度的導(dǎo)數(shù),α(t)為橫向速度差變化率的導(dǎo)數(shù);s(t)為車(chē)輛的狀態(tài)參量,s(t)=[x(t),y(t),θ(t),vx(t),ax(t),Δv(t),ω(t)]T,x(t)、y(t)和θ(t)為全局坐標(biāo)系Oxy下的位置坐標(biāo)值和航向角,vx(t)為車(chē)體平臺(tái)坐標(biāo)系Opxpyp下沿x軸的縱向速度,ax(t)為相對(duì)應(yīng)的加速度,Δv(t)為兩側(cè)履帶的速度差,Δv(t)=vrx-vlx,vrx與vlx分別為右側(cè)和左側(cè)的履帶速度,ω(t)為兩側(cè)履帶速度差的變化率,ts為基元生成的起始時(shí)刻,te為基元生成的終止時(shí)刻。履帶車(chē)輛平臺(tái)關(guān)鍵參量的定義如圖2 所示。圖2 中:B為履帶車(chē)兩側(cè)履帶之間的距離,L為履帶車(chē)接地段的長(zhǎng)度。

圖2 履帶車(chē)輛平臺(tái)關(guān)鍵參量定義Fig.2 Definition of key parameters of tracked vehicle platform

2.1 履帶車(chē)輛的運(yùn)動(dòng)微分約束

本文研究的有級(jí)轉(zhuǎn)向履帶車(chē)輛與無(wú)級(jí)轉(zhuǎn)向履帶車(chē)輛在運(yùn)動(dòng)特性上存在較大差異,但履帶車(chē)輛的基本運(yùn)動(dòng)形式未發(fā)生本質(zhì)上的變化,因此滿足相同的運(yùn)動(dòng)微分約束關(guān)系。具體而言,雖然有級(jí)轉(zhuǎn)向履帶車(chē)輛僅有有限的規(guī)定轉(zhuǎn)向半徑,并且規(guī)定轉(zhuǎn)向半徑呈現(xiàn)離散而非連續(xù)的分布形式,但車(chē)輛的運(yùn)動(dòng)不會(huì)發(fā)生突變,因此有級(jí)轉(zhuǎn)向履帶車(chē)輛與無(wú)級(jí)轉(zhuǎn)向履帶車(chē)輛的運(yùn)動(dòng)微分約束如(2)式所示:

2.2 異構(gòu)履帶車(chē)輛的差異化行為約束

本文研究的異構(gòu)履帶車(chē)輛在所搭載的轉(zhuǎn)向機(jī)構(gòu)上存在顯著差異,離合器轉(zhuǎn)向機(jī)僅具備一個(gè)規(guī)定轉(zhuǎn)向半徑,二級(jí)行星轉(zhuǎn)向機(jī)具備兩個(gè)規(guī)定轉(zhuǎn)向半徑,雙側(cè)獨(dú)立電機(jī)驅(qū)動(dòng)則為無(wú)級(jí)轉(zhuǎn)向。

由于本文僅面向異構(gòu)履帶車(chē)輛的運(yùn)動(dòng)規(guī)劃問(wèn)題,只對(duì)軌跡層面的運(yùn)動(dòng)模式進(jìn)行差異化建模。將有級(jí)轉(zhuǎn)向履帶車(chē)的主要運(yùn)動(dòng)形式定義為規(guī)定轉(zhuǎn)向半徑運(yùn)動(dòng)與直線運(yùn)動(dòng)的模式組合。以Dubins 曲線為依托,基于起始點(diǎn)與終止點(diǎn)的位姿,生成以固定半徑圓弧和直線構(gòu)成的曲線,并從中提取若干采樣點(diǎn),構(gòu)成基元優(yōu)化生成的中間點(diǎn)約束。本文將倒車(chē)作為前進(jìn)的相反方向進(jìn)行考慮。

此外,還補(bǔ)充若干非規(guī)定轉(zhuǎn)向半徑模式到有級(jí)轉(zhuǎn)向履帶車(chē)輛的基元庫(kù)中,該模式下的基元生成方式與無(wú)級(jí)轉(zhuǎn)向履帶車(chē)輛的基元生成方式完全一致。無(wú)級(jí)轉(zhuǎn)向履帶車(chē)輛不對(duì)基元生成過(guò)程中的中間點(diǎn)做約束,僅根據(jù)行為類別對(duì)起末點(diǎn)的位姿進(jìn)行約束。

具體而言,異構(gòu)履帶車(chē)輛的運(yùn)動(dòng)基元庫(kù)中包含了多種典型駕駛行為類別,包括直駛、調(diào)頭、U 形彎、直角彎、障礙物規(guī)避、直駛方向校正等。每一種行為類別都對(duì)應(yīng)于一組設(shè)定的起末點(diǎn)位姿。以障礙物規(guī)避行為為例,起末點(diǎn)的位姿約束be如下:

式中:d為橫向移動(dòng)的距離。對(duì)于無(wú)級(jí)轉(zhuǎn)向履帶車(chē)輛而言,起末點(diǎn)的位姿約束be被直接作為無(wú)級(jí)轉(zhuǎn)向履帶車(chē)輛的行為約束B(niǎo)m=be。

有級(jí)轉(zhuǎn)向履帶車(chē)輛則是參照無(wú)級(jí)轉(zhuǎn)向履帶車(chē)輛生成的運(yùn)動(dòng)基元,選取近似的起始位姿{(lán)x(ts),y(ts),θ(ts)}與終止位姿{(lán)x(te),y(te),θ(te)}作為Dubins 曲線的起末狀態(tài)約束,以規(guī)定轉(zhuǎn)向半徑作為Dubins 曲線的轉(zhuǎn)向半徑約束,并在生成的Dubins 曲線上選取Ns個(gè)采樣點(diǎn)構(gòu)成有級(jí)轉(zhuǎn)向履帶車(chē)輛的行為約束B(niǎo)s={b1,…,bn,…,bNs}。其中,除去起始點(diǎn)b1與終止點(diǎn)bNs需要對(duì)位置和航向進(jìn)行約束外,其余各采樣點(diǎn)僅對(duì)位置進(jìn)行約束。

Dubins 曲線的規(guī)劃任務(wù)是以固定半徑圓弧與直線相組合的形式找到連接起末位姿的最短曲線,并且曲線的構(gòu)型需要包含在如下集合內(nèi):

式中:L 表示按規(guī)定轉(zhuǎn)向半徑左駛運(yùn)動(dòng)模式;R 表示按規(guī)定轉(zhuǎn)向半徑右駛運(yùn)動(dòng)模式;S 表示直駛運(yùn)動(dòng)模式。有級(jí)轉(zhuǎn)向履帶車(chē)輛行為約束中的Dubins 曲線生成示意如圖3 所示。圖3 中,r為規(guī)定轉(zhuǎn)向半徑,dl為左駛運(yùn)動(dòng)模式生成的路徑,ds為直駛運(yùn)動(dòng)模式生成的路徑,dr為右駛運(yùn)動(dòng)模式生成的路徑,α、β為規(guī)定轉(zhuǎn)向半徑轉(zhuǎn)過(guò)的角度。

圖3 基于Dubins 曲線生成的起末點(diǎn)間的最短路徑Fig.3 The shortest path between the start and end points generated based on the Dubins curve

2.3 車(chē)輛平臺(tái)特性的不等式約束

受制于車(chē)輛平臺(tái)執(zhí)行器的特性約束,在運(yùn)動(dòng)基元的生成過(guò)程中需要滿足如下不等式約束:

式中:vmin、vmax、amax、ηmax為車(chē)輛平臺(tái)縱向參量所對(duì)應(yīng)的極限值;Δvmax、ωmax、αmax為車(chē)輛平臺(tái)橫向參量所對(duì)應(yīng)的最大值。

2.4 基元之間的平滑過(guò)渡約束

由于最終生成的規(guī)劃軌跡是為各獨(dú)立基元組成的基元序列,為了保證基元之間速度以及曲率的平滑過(guò)渡,需要添加首末時(shí)刻的平滑過(guò)渡約束,約束的具體形式如下:

該平滑過(guò)渡約束為過(guò)渡基元生成時(shí)所使用的Hermite插值創(chuàng)造了二重零點(diǎn)條件。

2.5 優(yōu)化生成的目標(biāo)函數(shù)

運(yùn)動(dòng)基元優(yōu)化生成的目標(biāo)函數(shù)主要考慮了軌跡時(shí)序點(diǎn)的平滑度,具體的形式如(7)式所示:

式中:c1、c2、c3、c4、c5為各項(xiàng)評(píng)價(jià)指標(biāo)的權(quán)重系數(shù)。

2.6 異構(gòu)履帶車(chē)輛基元庫(kù)的優(yōu)化求解

通過(guò)(1)式～(7)式完成基元優(yōu)化生成問(wèn)題的定義后,在離線狀態(tài)下完成優(yōu)化問(wèn)題的求解。最終在統(tǒng)一算法框架下,依據(jù)異構(gòu)履帶車(chē)輛的差異化行為約束,生成突顯各異構(gòu)履帶車(chē)輛獨(dú)特運(yùn)動(dòng)特性的運(yùn)動(dòng)基元庫(kù),為后續(xù)運(yùn)動(dòng)基元的擴(kuò)展與選擇提供備選集合。

3 運(yùn)動(dòng)基元的擴(kuò)展選擇

運(yùn)動(dòng)基元的在線擴(kuò)展選擇是從離線生成的運(yùn)動(dòng)基元庫(kù)中優(yōu)化選擇出符合環(huán)境約束的基元序列,得到包含軌跡與速度信息的運(yùn)動(dòng)規(guī)劃結(jié)果。運(yùn)動(dòng)基元優(yōu)化選擇問(wèn)題的定義如(8)式所示,其中優(yōu)化問(wèn)題的目標(biāo)函數(shù)為基元序列中各基元代價(jià)的加和,優(yōu)化問(wèn)題的約束包含起末位姿的約束和在可通行區(qū)域內(nèi)的基元擴(kuò)展約束。

式中:pk為選擇得到的基元序列中第k個(gè)基元;M為基元序列中所包含基元的總數(shù)目;J為基元選擇的代價(jià)函數(shù);sb為基元初始狀態(tài),sbs為設(shè)定的基元初始狀態(tài),se為基元終止?fàn)顟B(tài),ses為設(shè)定的基元終止?fàn)顟B(tài);Ep為環(huán)境中的可通行區(qū)域;P為離線生成的行為基元庫(kù)。

3.1 基元的擴(kuò)展

基元的擴(kuò)展實(shí)質(zhì)是在可通行區(qū)域內(nèi),實(shí)現(xiàn)基元序列中相鄰基元的關(guān)聯(lián),其中的關(guān)鍵是保證基元序列中相鄰基元在曲率與速度上的平滑過(guò)渡。由于(6)式所示的基元平滑過(guò)渡約束,使得各獨(dú)立基元在起末點(diǎn)都處于勻速直線運(yùn)動(dòng)狀態(tài),保證了基元過(guò)渡處曲率的連續(xù)性。但由于離線生成的基元,在速度空間上呈現(xiàn)離散分布狀態(tài),相鄰基元之間有速度跳變的可能,因此需要在基元擴(kuò)展過(guò)程中,在線構(gòu)建平滑過(guò)渡基元,實(shí)現(xiàn)速度的連續(xù)平滑變化。

基元之間的過(guò)渡形式為變速直線運(yùn)動(dòng),并采用三次Hermite 插值完成過(guò)渡段中速度的生成,具體形式如(9)式～(11)式所示:

式中:vn為插值點(diǎn)的速度;l0=(t-t1)/(t0-t1),l1=(t-t0)/(t1-t0)。由于過(guò)渡基元所要關(guān)聯(lián)的起末點(diǎn)加速度都為0 即v·n=0,通過(guò)對(duì)v(t)求導(dǎo),能夠得到過(guò)渡基元加速度的函數(shù)a(t),該函數(shù)在ta=(t1-t0)/2 處取得最大值,在最大值處的取值如(12)式所示:

式中:v1-v0為基元要過(guò)渡的速度偏差。由(12)式可知,在已知最大加速度的條件下,能夠得到基元的時(shí)間尺度,也就能求解得到基元的長(zhǎng)度,完成基元擴(kuò)展中過(guò)渡基元的生成。

3.2 基元的選擇代價(jià)

基元的選擇代價(jià)包含3 方面內(nèi)容,分別是與期望軌跡與速度的偏差代價(jià)、軌跡的平滑代價(jià)以及碰撞風(fēng)險(xiǎn)代價(jià)。

3.2.1 與參考軌跡和期望速度的偏差代價(jià)

參考軌跡與期望速度作為運(yùn)動(dòng)規(guī)劃系統(tǒng)的輸入,能夠?yàn)橐?guī)劃提供最基本的參照與指引。在偏差代價(jià)的計(jì)算中,通過(guò)引入高斯函數(shù)作為各點(diǎn)偏差的權(quán)重系數(shù),使得近處的參考軌跡點(diǎn)發(fā)揮更主要的引導(dǎo)作用。此外,由于基元內(nèi)部的速度變化趨勢(shì)是基元本身的固有性質(zhì),在與期望速度匹配的過(guò)程中,僅對(duì)基元終點(diǎn)速度進(jìn)行考量?；c期望軌跡的偏差代價(jià)Jp如(13)式所示,與期望速度的偏差代價(jià)Jv如(16)式所示。

式中:Np為生成軌跡上等間隔采樣點(diǎn)的總數(shù)目;高斯函數(shù)g(·)為標(biāo)準(zhǔn)正態(tài)分布;jn為生成軌跡采樣點(diǎn)與參考線匹配點(diǎn)之間的偏差代價(jià)。

式中:μ為均值;σ為標(biāo)準(zhǔn)差;ωd與ωh分別為距離偏差與航向偏差的權(quán)重系數(shù);dn為與參考線的距離偏差;Δθn為與參考線的航向偏差。

式中:vr為參考線匹配點(diǎn)上的期望速度;ve為基元終點(diǎn)處的速度。

3.2.2 軌跡的平滑代價(jià)

軌跡的平滑度直接反映了基元上各點(diǎn)曲率的平均變化情況,該數(shù)值越小、曲率越平滑,軌跡平滑度的定義如(17)式所示:

式中:ωs為軌跡平滑度的權(quán)重系數(shù);κn為基元采樣點(diǎn)上的曲率值;Δsn為相鄰采樣點(diǎn)之間的間距。設(shè)和為依據(jù)基元類型與車(chē)輛平臺(tái)類型設(shè)定的權(quán)重系數(shù),無(wú)級(jí)轉(zhuǎn)向履帶車(chē)輛權(quán)重系數(shù)的定義如(18)式所示,有級(jí)轉(zhuǎn)向履帶車(chē)輛權(quán)重系數(shù)的定義如(19)式所示。

式中:ωM和ωR分別為無(wú)級(jí)轉(zhuǎn)向履帶車(chē)輛方向維持和反向基元的權(quán)重系數(shù);pk為基元序列中的第k個(gè)基元;和分別為直駛過(guò)規(guī)定轉(zhuǎn)向半徑方向維持和反向基元的權(quán)重系數(shù);和分別為非規(guī)定轉(zhuǎn)向半徑方向維持和反向基元的權(quán)重系數(shù)。

3.2.3 碰撞風(fēng)險(xiǎn)代價(jià)

為了避免與環(huán)境中的障礙物發(fā)生碰撞,需要定義碰撞風(fēng)險(xiǎn)代價(jià),并使得該數(shù)值盡可能低。以六覆蓋圓近似車(chē)身,能夠兼顧碰撞檢測(cè)的效率與對(duì)車(chē)身近似的精度。本文選用六覆蓋圓近似地表示車(chē)身,通過(guò)計(jì)算障礙物到圓心的距離作為碰撞風(fēng)險(xiǎn)代價(jià)的指標(biāo),具體計(jì)算公式如(20)式所示:

3.3 基元的在線優(yōu)化選擇

基元的在線選擇需要對(duì)定義的選擇代價(jià)進(jìn)行綜合評(píng)估,基元選擇的總代價(jià)如(21)式所示:

式中:ωp、ωv、ωs、ωc為相應(yīng)代價(jià)指標(biāo)的權(quán)重系數(shù)。

在計(jì)算得到備選基元的選擇代價(jià)后,在線選擇模塊基于計(jì)算得到的代價(jià)值從基元簇中選擇出代價(jià)最低的基元,逐步擴(kuò)展生成從起始點(diǎn)到終止點(diǎn)的基元序列,構(gòu)成最終的期望軌跡時(shí)序點(diǎn)。

4 實(shí)驗(yàn)結(jié)果與討論

本文選用3 種不同類型的異構(gòu)履帶車(chē)輛平臺(tái)完成對(duì)算法的驗(yàn)證,這3 種類型的履帶車(chē)輛分別是基于離合器轉(zhuǎn)向機(jī)的有級(jí)轉(zhuǎn)向履帶車(chē)輛(Ⅰ型平臺(tái))、基于二級(jí)行星轉(zhuǎn)向機(jī)的有級(jí)轉(zhuǎn)向履帶車(chē)輛(Ⅱ型平臺(tái))、基于雙側(cè)獨(dú)立電機(jī)的無(wú)級(jí)轉(zhuǎn)向履帶車(chē)輛(Ⅲ型平臺(tái))。

履帶車(chē)輛從轉(zhuǎn)向機(jī)理上能夠分為無(wú)級(jí)轉(zhuǎn)向履帶車(chē)輛與有級(jí)轉(zhuǎn)向履帶車(chē)輛兩大類。有級(jí)轉(zhuǎn)向履帶車(chē)輛又根據(jù)可選擇規(guī)定轉(zhuǎn)向半徑數(shù)目的不同,有了更細(xì)致的區(qū)分。本文選用的3 種不同類型履帶車(chē)輛平臺(tái)既能覆蓋有級(jí)和無(wú)級(jí)兩大類別,又能體現(xiàn)不同類型有級(jí)轉(zhuǎn)向履帶車(chē)輛的特點(diǎn),因此具有一定的代表性。

所選用的車(chē)輛平臺(tái)如圖4 所示,車(chē)輛平臺(tái)的結(jié)構(gòu)以及執(zhí)行器參數(shù)如表1 所示。

圖4 所選用的異構(gòu)履帶車(chē)輛平臺(tái)Fig.4 Seclected heterogeneous tracked vehicle platforms

表1 車(chē)輛平臺(tái)結(jié)構(gòu)及執(zhí)行器參數(shù)Tab.1 Vehicle platform structure and actuator parameters

本節(jié)首先展示各履帶車(chē)輛平臺(tái)基元離線生成的總體情況,并從高低速兩個(gè)狀態(tài)中分別選擇若干典型的運(yùn)動(dòng)基元進(jìn)行展示,突顯各異構(gòu)履帶車(chē)輛平臺(tái)之間以及高低速之間的基元特性差異;其次展示兩個(gè)典型越野場(chǎng)景下的運(yùn)動(dòng)規(guī)劃結(jié)果,從差異化的運(yùn)動(dòng)規(guī)劃結(jié)果、算法的時(shí)間消耗等層面對(duì)結(jié)果進(jìn)行討論。

4.1 異構(gòu)履帶車(chē)輛的運(yùn)動(dòng)基元庫(kù)

異構(gòu)履帶車(chē)輛的運(yùn)動(dòng)基元庫(kù)離線生成采用了統(tǒng)一的優(yōu)化算法框架,只是在行為約束與車(chē)輛模型兩個(gè)方面,對(duì)不同類別的履帶車(chē)輛平臺(tái)進(jìn)行了區(qū)隔。表2 反映了異構(gòu)履帶車(chē)輛運(yùn)動(dòng)基元庫(kù)的總體情況,圖5 展示了不同車(chē)輛平臺(tái)、不同速度區(qū)間下的典型運(yùn)動(dòng)基元簇。在基元的離線優(yōu)化生成中,(7)式中的各權(quán)重系數(shù)完全一樣,對(duì)速度變化和曲率變化引起的平滑度波動(dòng)同等考慮。

表2 運(yùn)動(dòng)基元庫(kù)總體情況Tab.2 Overall situation of motion primitive library

從表2 中可以觀察到:隨著車(chē)輛速度的提升,基元的數(shù)目逐漸下降,這主要是因?yàn)檐?chē)輛在低速區(qū)間具備較強(qiáng)的轉(zhuǎn)向校正能力,能夠組合出更多樣的運(yùn)動(dòng)基元,以應(yīng)對(duì)狹小空間內(nèi)的復(fù)雜運(yùn)動(dòng)規(guī)劃任務(wù);在高速區(qū)間內(nèi)轉(zhuǎn)向校正能力變?nèi)?能夠組合出的運(yùn)動(dòng)基元種類變少;基元的長(zhǎng)度基本上隨速度單調(diào)遞增,但Ⅰ型平臺(tái)和Ⅱ型平臺(tái)在中高速時(shí)長(zhǎng)度有所下降,這主要是因?yàn)樵撈?臺(tái)在中高速時(shí)僅存在非規(guī)定轉(zhuǎn)向半徑,非規(guī)定轉(zhuǎn)向半徑下轉(zhuǎn)向機(jī)構(gòu)中的部件處于滑磨狀態(tài),難以持續(xù)較長(zhǎng)的時(shí)間并且轉(zhuǎn)向不確定性較強(qiáng),因此轉(zhuǎn)向校正的時(shí)長(zhǎng)被嚴(yán)格限制,基元的平均長(zhǎng)度也顯著下降。

從圖5(a)、圖5(c)、圖5(d)中可以觀察到,有級(jí)轉(zhuǎn)向履帶車(chē)輛規(guī)定轉(zhuǎn)向半徑模式下的運(yùn)動(dòng)基元,由于受到了Dubins 曲線生成的行為約束引導(dǎo),在基元內(nèi)部的運(yùn)動(dòng)規(guī)律為轉(zhuǎn)向-大段直駛-轉(zhuǎn)向,完全符合有級(jí)轉(zhuǎn)向履帶車(chē)輛的運(yùn)動(dòng)特性,并且與圖5(e)、圖5(f)中無(wú)級(jí)轉(zhuǎn)向履帶車(chē)輛的運(yùn)動(dòng)基元存在顯著差異。

圖5 各異構(gòu)履帶車(chē)輛平臺(tái)典型運(yùn)動(dòng)基元Fig.5 Typical motion primitives of various heterogeneous tracked vehicle platforms

4.2 異構(gòu)履帶車(chē)輛的運(yùn)動(dòng)規(guī)劃結(jié)果

為驗(yàn)證運(yùn)動(dòng)基元的在線擴(kuò)展與選擇算法,本節(jié)選取兩種典型的非結(jié)構(gòu)化場(chǎng)景對(duì)算法進(jìn)行驗(yàn)證。在低速測(cè)試場(chǎng)景中,期望觸發(fā)U 形彎、直角彎、調(diào)頭等低速狀態(tài)下的典型駕駛行為;在中速測(cè)試場(chǎng)景中,期望觸發(fā)換道、直線升降速等中速狀態(tài)下的典型駕駛行為。圖6 和圖7 分別展示了兩個(gè)場(chǎng)景的基本設(shè)置情況,不同車(chē)輛平臺(tái)的規(guī)劃結(jié)果,以及基元擴(kuò)展與選擇的細(xì)節(jié)展示。圖8 和圖9 分別展示了規(guī)劃得到的速度v與軌跡曲率κ之間的對(duì)應(yīng)關(guān)系。表3 展示了規(guī)劃算法的部分關(guān)鍵性能指標(biāo)。圖6(a)和圖7(a)的場(chǎng)景設(shè)置圖中,標(biāo)注了參考線與期望速度,vr為參考速度,vs為速度切換點(diǎn)。

表3 規(guī)劃算法性能評(píng)估Tab.3 Performance evaluation of motion planning algorithm

圖6 面向非結(jié)構(gòu)化場(chǎng)景1 的軌跡規(guī)劃結(jié)果Fig.6 Trajectory planning results for unstructured scene 1

圖7 面向非結(jié)構(gòu)化場(chǎng)景2 的軌跡規(guī)劃結(jié)果Fig.7 Trajectory planning results for unstructured scene 2

圖8 面向非結(jié)構(gòu)化場(chǎng)景1 的速度與曲率對(duì)應(yīng)結(jié)果Fig.8 Corresponding results of velocity and curvature for unstructured scene 1

圖9 面向非結(jié)構(gòu)化場(chǎng)景2 的速度與曲率對(duì)應(yīng)結(jié)果Fig.9 Corresponding results of velocity and curvature for unstructured scene 2

在基元的在線選擇中,(21)式中權(quán)重系數(shù)ωp、ωv、ωc的數(shù)值都為1。對(duì)于無(wú)級(jí)轉(zhuǎn)向履帶車(chē)輛只對(duì)行駛的方向做區(qū)分,期望車(chē)輛盡量維持同樣的行駛方向,完成整個(gè)規(guī)劃任務(wù),ωM=1,ωR=10。對(duì)于有級(jí)轉(zhuǎn)向履帶車(chē)輛,不僅對(duì)行駛方向進(jìn)行區(qū)分,還對(duì)是否采用規(guī)定轉(zhuǎn)向半徑的基元模式、完成規(guī)劃任務(wù)進(jìn)行了限定,期望盡量通過(guò)確定性的規(guī)定轉(zhuǎn)向半徑運(yùn)動(dòng)完成規(guī)劃任務(wù),輔助以不確定性較強(qiáng)的非規(guī)定轉(zhuǎn)向半徑運(yùn)動(dòng),因此權(quán)重系數(shù)為10,=10=100。

從運(yùn)動(dòng)規(guī)劃得到的結(jié)果來(lái)看,有級(jí)轉(zhuǎn)向履帶車(chē)輛的規(guī)劃軌跡主要由直線行駛模式與規(guī)定轉(zhuǎn)向半徑模式組成。這主要是因?yàn)樵?21)式的選擇代價(jià)中,針對(duì)規(guī)定轉(zhuǎn)向半徑與非規(guī)定轉(zhuǎn)向半徑分別設(shè)定了不等的權(quán)重系數(shù),使得運(yùn)動(dòng)規(guī)劃系統(tǒng)偏向于選擇規(guī)定轉(zhuǎn)向半徑模式與直駛模式組合的方式,完成運(yùn)動(dòng)規(guī)劃任務(wù)。無(wú)級(jí)轉(zhuǎn)向履帶車(chē)輛則沒(méi)有進(jìn)行上述區(qū)分,并且基元自身也以連續(xù)轉(zhuǎn)向運(yùn)動(dòng)為主,因此無(wú)級(jí)轉(zhuǎn)向履帶車(chē)輛的軌跡更平滑,與有級(jí)轉(zhuǎn)向履帶車(chē)輛的規(guī)劃結(jié)果存在顯著差異。特殊地,如圖7(b)所示,由于Ⅰ型平臺(tái)在中高速情況下無(wú)法觸發(fā)規(guī)定轉(zhuǎn)向半徑,基元簇的構(gòu)成也以短距離的非規(guī)定轉(zhuǎn)向半徑為主,因此在該場(chǎng)景下的規(guī)劃軌跡由直線段和多次小幅的轉(zhuǎn)向修正構(gòu)成,與同為有級(jí)轉(zhuǎn)向履帶的Ⅱ型平臺(tái)存在顯著差異。此外,從圖6(c)、圖6(e)、圖6(g)的細(xì)節(jié)圖上也能直觀地體現(xiàn)出3 種不同的車(chē)輛平臺(tái)在完成相同規(guī)劃任務(wù)時(shí)的差異性(圖中的虛框表示上一個(gè)基元結(jié)束時(shí)車(chē)輛所處的位置)。

從算法的求解效率上來(lái)分析,由于本文所提出的運(yùn)動(dòng)規(guī)劃方法采用基元離線生成、在線擴(kuò)展選擇的方式,在基元擴(kuò)展過(guò)程中,僅在線完成基元簇的旋轉(zhuǎn)與過(guò)渡基元的生成,但由于運(yùn)動(dòng)基元庫(kù)中所包含的基元種類較豐富,在選擇過(guò)程的耗時(shí)平均比基元擴(kuò)展的耗時(shí)高出37 倍。在限制場(chǎng)景中基元的擴(kuò)展步數(shù)不超過(guò)8 步的前提下,能夠?qū)⒄w的時(shí)間消耗控制在120 ms 以內(nèi),整體而言滿足異構(gòu)履帶車(chē)輛實(shí)時(shí)運(yùn)動(dòng)規(guī)劃的需求。相反地,所選用的對(duì)比算法Hybrid A*,既不能體現(xiàn)車(chē)輛之間的特性差異,平均耗時(shí)也增加了3.2 倍。

5 結(jié)論

本文提出了一種異構(gòu)履帶車(chē)輛的統(tǒng)一運(yùn)動(dòng)規(guī)劃方法,在基元生成與擴(kuò)展選擇的框架下,生成了能夠體現(xiàn)異構(gòu)平臺(tái)差異性的規(guī)劃結(jié)果。本文的主要貢獻(xiàn)及所得主要結(jié)論如下。

1)利用Dubins 曲線構(gòu)建了有級(jí)轉(zhuǎn)向履帶車(chē)輛的差異化行為約束,并借助基于車(chē)輛模型的優(yōu)化生成方法,構(gòu)建了能夠體現(xiàn)異構(gòu)履帶車(chē)輛平臺(tái)特性差異的運(yùn)動(dòng)基元庫(kù)。

2)在基元的在線選擇過(guò)程中,通過(guò)設(shè)定不等權(quán)重系數(shù),提升了有級(jí)轉(zhuǎn)向履帶車(chē)輛規(guī)劃結(jié)果中,直線運(yùn)動(dòng)模式與規(guī)定轉(zhuǎn)向半徑運(yùn)動(dòng)模式的占比,在一般場(chǎng)景下有降低控制難度的可能。

3)所提出的運(yùn)動(dòng)基元離線生成與在線擴(kuò)展選擇框架結(jié)構(gòu),兼顧了規(guī)劃方法的通用性與異構(gòu)平臺(tái)的差異性,并且滿足實(shí)時(shí)運(yùn)動(dòng)規(guī)劃系統(tǒng)的效率需求。

在后續(xù)的研究工作中將開(kāi)展運(yùn)動(dòng)規(guī)劃與控制的集成研究,通過(guò)基元的類別信息構(gòu)建規(guī)劃與控制之間的關(guān)聯(lián),建立通用化的異構(gòu)履帶車(chē)輛運(yùn)動(dòng)規(guī)劃與控制系統(tǒng)。