方曉波,錢 宏,劉朕明,孟德壯

中國(guó)船舶及海洋工程設(shè)計(jì)研究院,上海 200011)(*通信作者電子郵箱xiaobofang@foxmail.com)

基于路徑跟蹤控制方法的拖掛式機(jī)器人系統(tǒng)路徑規(guī)劃算法

方曉波*,錢 宏,劉朕明,孟德壯

中國(guó)船舶及海洋工程設(shè)計(jì)研究院,上海 200011)(*通信作者電子郵箱xiaobofang@foxmail.com)

為解決拖掛式移動(dòng)機(jī)器人系統(tǒng)路徑規(guī)劃算法精準(zhǔn)性低、穩(wěn)定性差和無法考慮系統(tǒng)間安全性等的問題,提出一種基于路徑跟蹤方法的路徑規(guī)劃算法。該算法融合快速拓展隨機(jī)樹(RRT)基本算法和路徑跟蹤控制方程,通過自動(dòng)擬合樣條曲線,跟蹤并生成節(jié)點(diǎn)間軌跡,以此提高路徑精準(zhǔn)性;加入系統(tǒng)夾角約束條件和節(jié)點(diǎn)擊中機(jī)制提高算法穩(wěn)定性和結(jié)果安全性；此外,加入貪心優(yōu)化算法,針對(duì)結(jié)果路徑進(jìn)行優(yōu)化處理。通過仿真實(shí)驗(yàn)結(jié)果表明,相較基本RRT算法,改進(jìn)算法搜索得到的路徑更貼近實(shí)際運(yùn)動(dòng)軌跡,在安全性和成功率上優(yōu)于原算法,能夠滿足快速設(shè)計(jì)或?qū)崟r(shí)系統(tǒng)的需求。

拖掛式系統(tǒng);路徑規(guī)劃;路徑跟蹤; 快速拓展隨機(jī)樹;機(jī)器人

0 引言

拖掛式移動(dòng)機(jī)器人(tractor-trailer mobile robots)是由機(jī)器人(牽引車)以鉸接等形式掛接若干拖車組合而成。這類機(jī)器人系統(tǒng)一般通過正、倒車和轉(zhuǎn)向驅(qū)動(dòng),帶動(dòng)拖車完成指定運(yùn)輸任務(wù),通常被用于港口碼頭、自動(dòng)化工廠、車站、酒店、核環(huán)境等場(chǎng)所,能代替人力執(zhí)行一些危險(xiǎn)或精度要求高的物資運(yùn)輸工作。研究拖掛式機(jī)器人涉及的領(lǐng)域十分廣泛,包括運(yùn)動(dòng)學(xué)建模、運(yùn)動(dòng)規(guī)劃、路徑跟蹤、可控性分析等。與自由運(yùn)動(dòng)的單體機(jī)器人不同,拖掛式移動(dòng)機(jī)器人系統(tǒng)是一個(gè)具有高復(fù)雜性和高綜合性的非完整、欠驅(qū)動(dòng)、非線性系統(tǒng),因此,一直是機(jī)器人領(lǐng)域研究的熱點(diǎn)和難點(diǎn)。

路徑跟蹤是通過設(shè)計(jì)合理的控制方法,實(shí)現(xiàn)機(jī)器人按照指定路徑完成行進(jìn)動(dòng)作。拖掛式機(jī)器人系統(tǒng)根據(jù)連接方式可分為離軸式和連軸式,根據(jù)運(yùn)動(dòng)特性又可分為正車和倒車,由于系統(tǒng)結(jié)構(gòu)的復(fù)雜性,不同工況下系統(tǒng)的路徑跟蹤控制方法[1]都各不相同,尤其針對(duì)離軸式倒車系統(tǒng),通常容易發(fā)生牽引車和拖車折疊、擠壓的現(xiàn)象。因此,控制方法的好壞將直接影響拖掛式移動(dòng)機(jī)器人在跟蹤路徑上的安全性和穩(wěn)定性。

路徑規(guī)劃是指機(jī)器人從所處的環(huán)境中自動(dòng)搜索出一條從初始狀態(tài)到目標(biāo)狀態(tài)的避碰、最優(yōu)或次優(yōu)路徑。通常選用的算法包括人工勢(shì)場(chǎng)(Artificial Potential Field)法、遺傳算法(Genetic Algorithm， GA)、蟻群優(yōu)化(Ant Colony Optimization, ACO)算法[2]、模擬退火算法(Simulated Annealing， SA)、快速擴(kuò)展隨機(jī)樹(Rapid-exploring Random Tree,RRT)算法等。鞏敦衛(wèi)等[3]基于改進(jìn)模擬退火算法,針對(duì)全局靜態(tài)移動(dòng)機(jī)器人,通過引入脫障算子和一致尋優(yōu)算子,提出提出了一種新的狀態(tài)產(chǎn)生方法,提高了尋優(yōu)效率和全局尋優(yōu)能力。劉傳領(lǐng)[4]基于勢(shì)場(chǎng)法和遺傳算法,針對(duì)單體移動(dòng)機(jī)器人的路徑規(guī)劃算法,提出了一些改進(jìn)的及新的解決方案,以提高算法的計(jì)算效率,擴(kuò)展其使用范圍。杜明博等[5]針對(duì)車輛路徑規(guī)劃問題,基于快速擴(kuò)展隨機(jī)樹算法,采用目標(biāo)偏向采樣策略以及連續(xù)曲率后處理技術(shù)實(shí)現(xiàn)了車輛連續(xù)軌跡的生成,但降低了算法的穩(wěn)定性和計(jì)算速度。

拖掛式移動(dòng)機(jī)器人屬于多主體系統(tǒng),在約束和控制方法上相對(duì)復(fù)雜,一般的單體無約束機(jī)器人的路徑規(guī)劃算法無法滿足實(shí)際需求。本文在基本RRT算法[6]中嵌入了該系統(tǒng)的路徑跟蹤控制技術(shù),并加入了樣條曲線擬合、節(jié)點(diǎn)擊中機(jī)制、貪心(Greedy)優(yōu)化[7]等算法,得到一種符合系統(tǒng)運(yùn)動(dòng)學(xué)特性的新型規(guī)劃算法。在此基礎(chǔ)上,通過VC++開發(fā)完成了可交互的拖掛式移動(dòng)機(jī)器人路徑規(guī)劃軟件,驗(yàn)證了算法的準(zhǔn)確性和可靠性。

1 路徑跟蹤控制方法

1.1 運(yùn)動(dòng)學(xué)模型

根據(jù)帶一節(jié)拖車的拖掛式移動(dòng)機(jī)器人系統(tǒng),建立運(yùn)動(dòng)學(xué)模型,如圖1所示。

圖1 拖掛式移動(dòng)機(jī)器人系統(tǒng)示意圖

(1)

根據(jù)拖車和牽引車的連接幾何關(guān)系可得:

(2)

1.2 路徑跟蹤控制方程

本文主要研究帶一節(jié)拖車的單體兩輪機(jī)器人系統(tǒng)模型。

圖2 路徑跟蹤示意圖

1.2.1 離軸式和連軸式正車路徑跟蹤

根據(jù)系統(tǒng)連接方式的不同,拖掛式機(jī)器人系統(tǒng)可分為離軸式(c1>0)和連軸式(c1=0),兩種連接方式在正車驅(qū)動(dòng)中的運(yùn)動(dòng)方程相同,故可用相同的控制方法進(jìn)行路徑跟蹤。

根據(jù)文獻(xiàn)[8,10]可知:設(shè)期望路徑為ρd(xd,yd)、xd和yd三階可導(dǎo)且三階導(dǎo)數(shù)有界,期望路徑的曲率滿足kd≤1/r1min[9],若牽引車的控制率取為:

(3)

其中:u1、ω1為牽引車的線速度和角速度;系數(shù)k2,k3>0。期望線速度u1d、期望角速度ωd、誤差ex、ey、eθ滿足式(4)：

(4)

則牽引車的運(yùn)動(dòng)將漸近收斂到期望路徑。

1.2.2 離軸式倒車系統(tǒng)路徑跟蹤控制方程

在實(shí)際使用情況中拖掛式機(jī)器人系統(tǒng)除了要向前正車完成路徑跟蹤外,還希望以向后倒車的方式沿著期望路徑將拖車控制到指定位置。若只改變u1d,沿用正車控制方程,將會(huì)出現(xiàn)拖車與牽引車擠壓、碰撞,甚至倒掛的現(xiàn)象。因此需要重新設(shè)計(jì)控制方法,將拖車和牽引車的夾角控制在合理的安全范圍內(nèi)。

(5)

根據(jù)運(yùn)動(dòng)學(xué)方程可反推得到離軸式倒車系統(tǒng)的控制方程為:

(6)

1.2.3 連軸式倒車系統(tǒng)路徑跟蹤控制方程

對(duì)于連軸式系統(tǒng),由于c1=0,運(yùn)動(dòng)學(xué)方程顯然已經(jīng)不適用,根據(jù)模型可推得該系統(tǒng)下運(yùn)動(dòng)學(xué)方程為:

(7)

根據(jù)u2=u1cos (θ1-θ2),取控制率的u1為:

(8)

則

(9)

(10)

2 基于路徑跟蹤控制的改進(jìn)RRT算法

2.1 基本RRT算法

RRT算法[11]是一種隨機(jī)釆樣的典型樹結(jié)構(gòu)算法,采用特定的增量方式進(jìn)行構(gòu)造,其基本思想是由控制理論決定隨機(jī)樹的增長(zhǎng)方式,通過在狀態(tài)空間隨機(jī)采樣狀態(tài)點(diǎn)將搜索導(dǎo)向空白區(qū)域,逐步縮短隨機(jī)狀態(tài)點(diǎn)與樹的期望節(jié)點(diǎn)即規(guī)劃目標(biāo)點(diǎn)間的距離,從而找到一條連接起始點(diǎn)與目標(biāo)點(diǎn)的規(guī)劃路徑。這種規(guī)劃方式拋棄狀態(tài)空間對(duì)障礙物精確定義的要求,選用碰撞檢測(cè)函數(shù)(Extend)來判定系統(tǒng)每個(gè)位形與障礙物的關(guān)系。這種方式可以簡(jiǎn)化對(duì)空間的建模,搜索速度快,不會(huì)出現(xiàn)柵格法、人工勢(shì)場(chǎng)法等算法中易出現(xiàn)的維數(shù)災(zāi)難問題。因此用于解決復(fù)雜環(huán)境下復(fù)雜系統(tǒng)的路徑規(guī)劃問題[12-13]。

圖3 基本RRT算法

2.2 基于路徑跟蹤控制方法的改進(jìn)RRT算法

2.2.1 嵌入路徑跟蹤控制方法

基本RRT算法在Extend中采用直線連接的方式拓展新節(jié)點(diǎn),并根據(jù)該直線生成軌跡線進(jìn)行碰撞檢測(cè),此方法適用于無運(yùn)動(dòng)特性約束的單體機(jī)器人,可以提高路徑搜索速度。但在拖掛式移動(dòng)機(jī)器人系統(tǒng)中,由于系統(tǒng)結(jié)構(gòu)復(fù)雜,各個(gè)主體(牽引車和拖車)運(yùn)動(dòng)特性不同,采用直線擬合軌跡并進(jìn)行碰撞檢測(cè)是不切實(shí)際的。

圖4 TTMT-RRT節(jié)點(diǎn)拓展示意圖

為了合理的將系統(tǒng)運(yùn)動(dòng)特性融合入RRT算法中,并生成合理的路徑軌跡,本文引入樣條擬合控制方法,如圖4所示,在節(jié)點(diǎn)拓展時(shí),根據(jù)Tnear點(diǎn)拖車或牽引車的方位角信息與Tnew點(diǎn)的坐標(biāo)值,進(jìn)行兩點(diǎn)間的切向擬合,形成至Tnew的期望路徑,如圖5(a)所示。根據(jù)系統(tǒng)當(dāng)前工況(正、倒車、離軸式、連軸式),選擇對(duì)應(yīng)的控制方程(3)、(6)、(10),針對(duì)期望路徑進(jìn)行路徑跟蹤,圖5(b)曲線分別為牽引車后輪、拖車后輪的中點(diǎn)的跟蹤軌跡線以及拖車的雙輪軌跡線。

圖5 連軸式倒車系統(tǒng)的路徑跟蹤結(jié)果

2.2.2 節(jié)點(diǎn)擊中機(jī)制

牽引車和拖車之間由于幾何形狀的干涉約束,往往存在設(shè)計(jì)最大夾角,即牽引車與拖車縱向軸的最大夾角(約50°～90°)。為了在路徑規(guī)劃中考慮夾角安全性問題,避免倒掛、擠壓等問題的出現(xiàn),算法添加了夾角約束檢測(cè)機(jī)制,針對(duì)跟蹤結(jié)果進(jìn)行最大夾角約束檢測(cè),若滿足安全閾值要求,則進(jìn)一步進(jìn)行碰撞檢測(cè)等;反之進(jìn)行重新搜索。

由于在Extend()時(shí)加入了夾角約束條件,提高了節(jié)點(diǎn)拓展的難度,極易陷入某個(gè)局部最優(yōu)的無限循環(huán)中,即節(jié)點(diǎn)樹中的某一節(jié)點(diǎn)Ti成為隨機(jī)選擇的高概率點(diǎn),但由于離環(huán)境障礙物過近或者方位角過大,無法繼續(xù)拓展,則會(huì)造成算法搜索緩慢甚至搜索失敗。為了避免這種情況出現(xiàn),就需要在擴(kuò)展樹中刪除這種無效點(diǎn)Ti。本文采用節(jié)點(diǎn)擊中機(jī)制,記錄每次拓展所選擇的Tnear點(diǎn)以及其擊中次數(shù),選擇合理的效率閾值(亦為節(jié)點(diǎn)最大子節(jié)點(diǎn)數(shù)),在擊中次數(shù)超出效率閾值的情況時(shí),刪除Tnear點(diǎn)以及其子樹(子樹由于控制約束,一般無法繼承至Tnear的父項(xiàng))。同時(shí)在每次一次拓展失敗時(shí),嘗試將Tnear更改為其父節(jié)點(diǎn)進(jìn)行重新拓展。經(jīng)過測(cè)試,該拓展方法很好地解決局部最優(yōu)問題,提高了算法穩(wěn)定性。

2.2.3 基于貪心思想的結(jié)果優(yōu)化算法

由于加入了路徑跟蹤控制方程,一般采用上述算法搜索得到的計(jì)算結(jié)果已經(jīng)可以直接用于實(shí)際操作,屬于可行解,但不是最優(yōu)解,需要對(duì)結(jié)果路徑進(jìn)行進(jìn)一步的優(yōu)化。參考貪心思想,對(duì)節(jié)點(diǎn)樹主干節(jié)點(diǎn)進(jìn)行優(yōu)化,優(yōu)化方法可分為正向優(yōu)化與反向優(yōu)化兩種,此系統(tǒng)采用正向優(yōu)化方法。

Optimize(T:RRT Tree Points)

1)

Pruning(T);

2)

NewTOpt;

3)

TOpt.add (T0,S0);

4)

for (inti=0;i

5)

for (j=T.st-1;j>I)

6)

if (Extend (T,Tj,Ti,env)==TRUE)

7)

TOpt.add (Tj);i=j;break;

8)

elsej--;

9)

end for

10)

end for

上述算法中,步驟1)為對(duì)節(jié)點(diǎn)樹T進(jìn)行剪枝處理,留下主干節(jié)點(diǎn)(T0,T1,…,Tn)。步驟2)～10)為從Ti點(diǎn)和該點(diǎn)系統(tǒng)狀態(tài),依次嘗試連接拓展Tn,Tn-1,…,Ti+1,拓展方法Extend()參照TTMRPathPlanning算法,直至拓展成功,若Tj為Tn則結(jié)束優(yōu)化。優(yōu)化結(jié)果如圖6所示。

圖6 貪心思想優(yōu)化算法示意圖

由貪心思想得到的Ti,由于改變了Ti點(diǎn)原有的系統(tǒng)狀態(tài),無法保證Tn,Tn-1,…,Ti+1中存在成功拓展點(diǎn),若沒有則需要進(jìn)行退回處理,當(dāng)Ti退回至T0時(shí)必有成功拓展點(diǎn)T1,因此,此優(yōu)化算法是封閉有解的。

2.2.4 改進(jìn)算法流程

圖7為基于路徑跟蹤的改進(jìn)RRT算法流程。

圖7 基于路徑跟蹤的改進(jìn)RRT算法流程

TTMRPathPlanning(env: environment,T: RRT Tree Points,Si:System state)

1)

VarT0,S0;

2)

T.add (T0,S0);

3)

TTarget=ChooseTarget ()

4)

Tnear=NearestNeighbour (TTarget);

5)

Tnew=Calulate (TTarget,Tnear,dl);

6)

if(Extend (T,Tnew,Tnear,env)==TRUE)

7)

T.add (Tnew,Snew)

8)

if(Tnew==Tgoal)

9)

Optimize(T) and return;

Extend (T,Tnew,Tnear,env)代碼如下：

1)

if (HitTimes (Tnear)>n)

2)

T.delet (Tnear);

3)

return FALSE;

4)

else

5)

if (PathTracking (Tnew,Tnear)==TRUE)

6)

if (CollideTest (env)==FALSE)

7)

return TRUE;

8)

else return FALSE;

9)

else return FALSE;

其中:第1)行中T0、S0為系統(tǒng)初始狀態(tài),包括牽引車和拖車的初始位置和初始角度(x10,y10,z10),(x20,y20,z20),與常規(guī)RRT算法不同,TTMR-RRT每個(gè)節(jié)點(diǎn)記錄了系統(tǒng)6個(gè)自由度的狀態(tài)信息。第2)～8)行為基本RRT算法步驟,第9)行為對(duì)結(jié)果的貪心優(yōu)化。Extend函數(shù)為改進(jìn)的節(jié)點(diǎn)拓展方法,其中對(duì)所選擇的Tnear進(jìn)行擊中次數(shù)檢測(cè),超出設(shè)定的閾值則刪減樹中的Tnear點(diǎn)以及其子樹；否則,根據(jù)樣條曲線控制方法擬合期望路徑,計(jì)算該曲線的一至三階導(dǎo)數(shù),并根據(jù)當(dāng)前拖掛式系統(tǒng)的工況選擇合適的路徑跟蹤控制方程,進(jìn)行路徑跟蹤(第5)行),跟蹤的結(jié)果滿足系統(tǒng)安全夾角要求dθmax=|θ1-θ2}<θrequire,則進(jìn)行碰撞檢測(cè),碰撞檢測(cè)算法一般采用二維面片干涉檢查算法,檢測(cè)通過則在樹上添加新節(jié)點(diǎn)。當(dāng)Tnew為Tgoal,終止搜索,對(duì)搜索結(jié)果進(jìn)行優(yōu)化處理(見圖6),并根據(jù)優(yōu)化結(jié)果進(jìn)行重新擬合和跟蹤。

改進(jìn)RRT算法可以使搜索結(jié)果滿足拖掛式移動(dòng)機(jī)器人系統(tǒng)的運(yùn)動(dòng)學(xué)特性以及安全性要求,同時(shí),能保證系統(tǒng)到達(dá)目標(biāo)點(diǎn)的方位角滿足需求設(shè)定。當(dāng)系統(tǒng)狀態(tài)為倒車時(shí),可精準(zhǔn)控制拖車到達(dá)目標(biāo)點(diǎn)的朝向;當(dāng)系統(tǒng)狀態(tài)為正車時(shí),可控制牽引車到達(dá)目標(biāo)點(diǎn)的朝向。

3 實(shí)驗(yàn)與分析

3.1 仿真初始信息與典型計(jì)算結(jié)果

根據(jù)上述路徑跟蹤控制方程以及改進(jìn)RRT算法,搭建環(huán)境模型和系統(tǒng)模型結(jié)構(gòu),匯編成軟件,并進(jìn)行實(shí)驗(yàn)驗(yàn)證。以連軸式倒車系統(tǒng)為例,實(shí)驗(yàn)輸入數(shù)據(jù)包括起始點(diǎn),起始點(diǎn)朝向A0=180°,目標(biāo)點(diǎn)Tgoal(18,0),目標(biāo)點(diǎn)朝向Agoal=160°,牽引車初始速度u0=-1,初始角速度ω0=0.6,系統(tǒng)安全夾角閾值為60°,搜索步長(zhǎng)取為1.5 m,目標(biāo)點(diǎn)隨機(jī)概率取30%,路徑跟蹤密度取1 000。典型路徑搜索結(jié)果如圖8所示。

根據(jù)當(dāng)前系統(tǒng)輸入?yún)?shù)和環(huán)境模型,進(jìn)行100次重復(fù)計(jì)算,統(tǒng)計(jì)結(jié)果路徑的路徑長(zhǎng)度、搜索時(shí)間以及系統(tǒng)間最大夾角(見圖9)。

圖8 TTMR-RRT搜索結(jié)果

圖9 100次仿真計(jì)算結(jié)果信息

3.2 仿真結(jié)果分析

圖8(a)為TTMT-RRT搜索得到的節(jié)點(diǎn)樹狀圖,圖8(b)為剪枝以及優(yōu)化之后的結(jié)果。路徑行進(jìn)時(shí)間為20.736s,牽引車與拖車最大夾角為47.01°,滿足安全閾值60°的要求。由圖8所示,可行路徑主要分為上下兩種。

1)結(jié)果切實(shí)性。

由圖8可知,改進(jìn)算法搜索得到的路徑軌跡為連續(xù)曲線,且區(qū)分拖車和牽引車各自的運(yùn)動(dòng)形態(tài),存在一定的甩尾現(xiàn)象,相較原算法的線段式樹狀圖(圖3(a))更貼近實(shí)際情況,因此,碰撞檢測(cè)結(jié)果也更為準(zhǔn)確。

2)結(jié)果穩(wěn)定性和優(yōu)化能力。

為了驗(yàn)證改進(jìn)算法的結(jié)果穩(wěn)定性以及時(shí)效性,根據(jù)設(shè)定的環(huán)境和輸入信息,取20次計(jì)算結(jié)果中拖車雙輪中點(diǎn)的軌跡線,匯總成圖10。由圖10可知,由于設(shè)定的輸入信息中標(biāo)定目標(biāo)點(diǎn)朝向?yàn)?60°(偏上),出現(xiàn)上路結(jié)果的概率明顯大于下路;其次,雖然算法具有一定隨機(jī)性,但經(jīng)過后處理優(yōu)化后,路徑結(jié)果基本趨向穩(wěn)定。由此可知,相對(duì)于基本RRT算法,改進(jìn)算法考慮了終點(diǎn)定位約束,在結(jié)果穩(wěn)定性上明顯優(yōu)于原算法;同時(shí),解決了基本RRT算法存在的繞遠(yuǎn)問題,在時(shí)效性上更勝一籌。

3)成功率和安全性。

根據(jù)100次仿真結(jié)果,將各結(jié)果中系統(tǒng)夾角、路徑長(zhǎng)度和搜索時(shí)間匯總成表1，成功率為100%。

表1 100次仿真計(jì)算統(tǒng)計(jì)結(jié)果

由表1與成功率可知,相對(duì)于未添加節(jié)點(diǎn)擊中機(jī)制情況下的算法搜索成功率(56.5%),提高了43.5個(gè)百分點(diǎn),可見算法解決了陷入局部最優(yōu)問題，提高了搜索穩(wěn)定性,在封閉性和收斂性上是相對(duì)成功的;同時(shí),系統(tǒng)間最大夾角為59.99°,未超過設(shè)定的安全夾角閾值60°,因此,算法在結(jié)果安全性上也能夠予以保證。

圖10 拖車雙輪中點(diǎn)軌跡線20次計(jì)算結(jié)果匯總圖

4)結(jié)果時(shí)效性。

根據(jù)統(tǒng)計(jì)結(jié)果繪制正態(tài)分布曲線(圖11～12),其中,路徑搜索耗時(shí)的標(biāo)準(zhǔn)方差為1.62s,路徑長(zhǎng)度的標(biāo)準(zhǔn)方差為2.13m。

圖11 搜索時(shí)間統(tǒng)計(jì)結(jié)果

圖12 路徑長(zhǎng)度統(tǒng)計(jì)結(jié)果

由圖12可知,路徑長(zhǎng)度分布較為平均,搜索結(jié)果處于在20～25m中較多,平均路徑長(zhǎng)度為22.873m,最短路徑為19.097m,兩者相差16.51%,進(jìn)一步驗(yàn)證了改了算法在結(jié)果時(shí)效性上的提高。根據(jù)路徑長(zhǎng)度正態(tài)分布曲線可知,平均只需搜索10次以內(nèi),即可得到最優(yōu)路徑,結(jié)合搜索時(shí)間亦可確定此輸入狀態(tài)的系統(tǒng),在搜索20s以內(nèi)可以得到可行的最優(yōu)路徑。

5)搜索快速性。

在搜索速度方面,基本RRT算法在環(huán)境變量一致的前提下,搜索平均耗時(shí)在0.78s,從統(tǒng)計(jì)結(jié)果來看,由于改進(jìn)算法加入了路徑跟蹤計(jì)算以及夾角約束條件,犧牲了部分快速性。搜索以及結(jié)果優(yōu)化的平均耗時(shí)為2.15s,由圖11可知,結(jié)果在2s以下的占66%,在1.5s以下的占47%,較原算法耗時(shí)增加2倍左右,該時(shí)間滿足快速設(shè)計(jì)的需求,但還不足夠用于實(shí)時(shí)系統(tǒng),有一定改進(jìn)空間。由于搜索耗時(shí)和環(huán)境復(fù)雜度、路徑長(zhǎng)度、搜索步長(zhǎng)、每步長(zhǎng)跟蹤間隔、電腦性能都有密切關(guān)系,本結(jié)果中為了提高結(jié)果的精確度,設(shè)定每步長(zhǎng)搜索間隔為1/1 000,若用于實(shí)時(shí)搜索,在滿足精度的前提下,可將間隔減低至1/300,經(jīng)過驗(yàn)證,搜索速度將以相同比例提升。

根據(jù)上訴仿真算例可知,TTMT-RRT算法在結(jié)果穩(wěn)定性、切實(shí)性、成功率、時(shí)效性以及安全性上都優(yōu)于原算法;反之,不可避免地在快速性上有所犧牲,在后續(xù)研究中需要進(jìn)一步優(yōu)化改進(jìn)。

4 結(jié)語(yǔ)

本文針對(duì)帶一節(jié)拖車的拖掛式移動(dòng)機(jī)器人系統(tǒng),提出了一種基于路徑跟蹤技術(shù)的自動(dòng)路徑規(guī)劃算法。該算法融合RRT快速隨機(jī)數(shù)拓展算法,通過引入樣條曲線擬合方法和路徑跟蹤控制技術(shù),提高了原算法的精準(zhǔn)性;加入節(jié)點(diǎn)擊中機(jī)制和貪心優(yōu)化算法優(yōu)化了算法搜索成功率以及時(shí)效性。通過仿真實(shí)驗(yàn)分析,進(jìn)一步驗(yàn)證了本文算法的正確性和可靠性。

由于加入了諸多約束和計(jì)算方程,算法在搜索耗時(shí)上高于基本RRT算法,犧牲了原有算法搜索速度快的優(yōu)點(diǎn),雖然速度可以通過降低搜索精度的方法予以提高,但治標(biāo)不治本,如何在保證精度的前提下盡可能提高速度將是后續(xù)研究的重點(diǎn)。此外,本文研究的基本對(duì)象為帶一節(jié)拖車的拖掛式移動(dòng)機(jī)器人系統(tǒng),理論上可以通過改動(dòng)路徑跟蹤控制方程拓展為多節(jié)拖車,在后續(xù)研究中,也將針對(duì)多節(jié)拖車系統(tǒng)進(jìn)行進(jìn)一步研究和驗(yàn)證。

)

[1]MYOUNGKUKP,WOOLINC,MUNSANGK.Controlofamobilerobotwithpassivemultipletrailers[C]//Proceedingsofthe2004IEEEInternationalConferenceonRoboticsandAutomation.Piscataway,NJ:IEEE, 2004:4369-4374.

[2] 王沛棟. 改進(jìn)蟻群算法及在路徑規(guī)劃問題的應(yīng)用研究[D]. 青島:中國(guó)海洋大學(xué), 2012.(WANGPD.Researchonimprovedantcolonyalgorithmsandapplicationsforpathplanningproblems[D].Qingdao:OceanUniversityofChina, 2012.)

[3] 鞏敦衛(wèi), 曾現(xiàn)峰, 張勇. 基于改進(jìn)模擬退火算法的機(jī)器人全局路徑規(guī)劃[J]. 系統(tǒng)仿真學(xué)報(bào), 2013, 25(3):480-483.(GONGDW,ZENGXF,ZHANGY.Globalpathplanningmethodofrobotbasedonmodifiedsimulatedannealingalgorithm[J].JournalofSystemSimulation, 2013, 25(3):480-483.)

[4] 劉傳領(lǐng). 基于勢(shì)場(chǎng)法和遺傳算法的機(jī)器人路徑規(guī)劃技術(shù)研究[D]. 南京:南京理工大學(xué), 2012.(LIUCL.Researchesontechnologiesforrobotpathplanningbasedonartificialpotentialfieldandgeneticalgorithm[D].Nanjing:NanjingUniversityofScience&Technology, 2012.)

[5] 杜明博, 梅濤, 陳佳佳, 等. 復(fù)雜環(huán)境下基于RRT的智能車輛運(yùn)動(dòng)規(guī)劃算法[J]. 機(jī)器人, 2015(4):443-450.(DUMB,MEID,CHENJJ,etal.RRT-basedmotionplanningalgorithmforintelligentvehicleincomplexenvironments[J].Robot, 2015(4):443-450.)

[6]LAVALLESM.Rapidly-exploringrandomtrees:anewtoolforpathplanning[EB/OL]. [2016- 03- 01].http://msl.cs.illinois.edu/～lavalle/papers/Lav98c.pdf.

[7] 馮林, 賈菁輝. 基于對(duì)比優(yōu)化的RRT路徑規(guī)劃改進(jìn)算法[J]. 計(jì)算機(jī)工程與應(yīng)用, 2011, 47(3): 210-213.(FENGL,JIAJH.ImprovedalgorithmofRRTpathplanningbasedoncomparisonoptimization[J].ComputerEngineeringandApplications, 2011, 47(3):210-213.)

[8] 苑晶, 黃亞樓, 孫鳳池. 兩種連接形式的拖掛式移動(dòng)機(jī)器人路徑跟蹤控制[J]. 控制理論與應(yīng)用, 2008, 25(3):398-406.(YUANJ,HUANGYL,SUNFC.Pathfollowingcontrolfortractor-trailermobilerobotswithtwokindsofconnectionforms[J].ControlTheory&Applications, 2008, 25(3):398-406.)

[9] 徐國(guó)華. 帶拖車的輪式移動(dòng)機(jī)器人系統(tǒng)的研究[D]. 天津: 南開大學(xué), 1998.(XUGH.Researchesontractor-trailermobilerobotssystem[D].Tianjin:NankaiUniversity, 1998.)

[10] 劉作軍, 孔鑫, 楊鵬, 等. 帶拖車移動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)分析與仿真[C]// 中國(guó)控制與決策學(xué)術(shù)年會(huì)文集. 沈陽(yáng): 東北大學(xué), 2007: 1309-1316.(LIUZJ,KONGX,YANGP,etal.Kinematicsanalyzeandsimulationfortractor-trailermobilerobot[C]//Proceedingsofthe2006ChineseControlandDecisionConference.Shengyang:NortheasternUniversity, 2007: 1309-1316.)

[11]LAVALLESM,KUFFNERJJ.Randomizedkinodynamicplanning[J].InternationalJournalofRoboticsResearch, 1999, 20(5):473-479.

[12] 徐娜, 陳雄, 孔慶生, 等.非完整約束下的機(jī)器人運(yùn)動(dòng)規(guī)劃算法[J]. 機(jī)器人, 2011, 33(6):666-672.(XUN,CHENX,KONGQS,etal.Motionplanningforrobotwithnonholonomicconstraints[J].Robot, 2011, 33(6):666-672.)

[13]CHENGP,LAVALLESM.ReducingRRTmetricsensitivityformotionplanningwithdifferentialconstraints[EB/OL]. [2016- 03- 01].http://msl.cs.uiuc.edu/～pcheng1/papers/thesis.ps.

ThisworkispartiallysupportedbytheFundforBasicResearchfromNationalScientificalandIndustrialTechnologyCommittee(A0720132001).

FANG Xiaobo, born in 1992, M. S. candidate. His research interests include computer aided design of ships, computer simulation.

QIAN Hong, born in 1966, M. S., research fellow. His research interests include computer aided design of ships, computer simulation.

LIU Zhenming, born in 1988, M. S. candidate, engineer. His research interests include computer aided design of ships.

MENG Dezhuang, born in 1989, M. S., engineer. His research interests include computer aided design of ships.

Path planning algorithm of tractor-trailer mobile robots system based on path-following control method

FANG Xiaobo*, QIAN Hong, LIU Zhenming, MENG Dezhuang

(Marine Design & Research Institute of China, Shanghai 200011, China)

Concerning the low accuracy, poor stability and security of the path planning algorithm of tractor-trailer mobile robots system, a path planning algorithm based on path-following method was proposed. On the basis of Rapid-exploring Random Tree (RRT) method and the equations of path-following, the path accuracy was improved by automatically fitting spline curve and tracking and generating the path between nodes; an angle constraint condition between systems and node hitting mechanism were added to the algorithm to improve the stability of algorithm and the security of results. In addition, an optimization algorithm based on greedy strategy was added to optimize results. The simulations results indicate that compared with the basic RRT algorithm, the path calculated by the improved algorithm is more close to the actual trajectory, and the success rate and security are better than the original algorithm, which can meet the requirement of quick design and real-time systems.

trailer system; path-planning; path-following; Rapid-exploring Random Tree (RRT); robot

2016- 08- 09;

2016- 12- 27。 基金項(xiàng)目:國(guó)防基礎(chǔ)科研基金資助項(xiàng)目(A0720132001)。

方曉波(1992—),男,上海人,碩士研究生,主要研究方向:計(jì)算機(jī)船舶輔助設(shè)計(jì)、計(jì)算機(jī)仿真; 錢宏(1966—),男,上海人,研究員,碩士,主要研究方向:計(jì)算機(jī)船舶輔助設(shè)計(jì)、計(jì)算機(jī)仿真; 劉朕明(1988—),男,上海人,工程師,碩士研究生,主要研究方向:計(jì)算機(jī)船舶輔助設(shè)計(jì); 孟德壯(1989—),男,河北邢臺(tái)人,工程師,碩士,主要研究方向:計(jì)算機(jī)船舶輔助設(shè)計(jì)。

1001- 9081(2017)04- 1116- 06

10.11772/j.issn.1001- 9081.2017.04.1116

TP24

A