李成雷， 賀繼林， 鄧 宇,2， 敖小樂

(1.中南大學(xué) 高性能復(fù)雜制造國家重點(diǎn)實(shí)驗(yàn)室,湖南 長沙 410083；2.山河智能裝備股份有限公司,湖南 長沙 410100)

0 引 言

如何規(guī)劃出一條無碰撞、滿足連續(xù)性約束且能保證輸出穩(wěn)定性的航跡，實(shí)現(xiàn)無人機(jī)的自主巡航[1]，已成為無人機(jī)智能化的研究熱點(diǎn)。早期針對(duì)地面移動(dòng)機(jī)器人開發(fā)的人工勢(shì)場、諧函數(shù)等矢量場方法近年來被不少研究人員應(yīng)用于無人機(jī)規(guī)劃[2,3]，但難以避免陷入局部極小點(diǎn)以及曲線振蕩等問題。A*，D*等圖搜索算法在三維空間中計(jì)算復(fù)雜度會(huì)劇增。快速搜索隨機(jī)樹[4](rapidly-exploring random tree,RRT)算法通過隨機(jī)抽取一些采樣點(diǎn)來進(jìn)行搜索，可以大大降低算法的時(shí)間和空間復(fù)雜度，在高維度空間中有著更明顯的優(yōu)勢(shì)。為進(jìn)一步提高RRT算法的效率，國內(nèi)外學(xué)者在此基礎(chǔ)上有不同程度的改進(jìn)，目標(biāo)偏向RRT，RRT-connect等算法都具有不錯(cuò)的效果[5～7]。

而RRT算法得到的路徑是由直線段依次連接而成的路徑，在轉(zhuǎn)折節(jié)點(diǎn)處不光滑，不利于后續(xù)的跟蹤控制。因而需要對(duì)折線路徑進(jìn)行擬合平滑處理。常用的Dubin曲線不能保證加速度的連續(xù)性，螺旋曲線(clothoid curves)由于沒有解析解不適用于實(shí)時(shí)處理[8]，貝塞爾曲線構(gòu)造簡單但受控制點(diǎn)的限制，靈活性差。B樣條曲線與控制點(diǎn)的數(shù)量獨(dú)立，可對(duì)各分段的曲線作局部修正，并保證各分段區(qū)間之間的連續(xù)性。

本文對(duì)于四旋翼無人機(jī)在分布大量障礙物的低空復(fù)雜環(huán)境下的航跡規(guī)劃問題，提出了一種對(duì)RRT-connect算法的改進(jìn)方法，并采用B樣條函數(shù)進(jìn)行后處理。最后，通過實(shí)驗(yàn)驗(yàn)證了該算法的有效性和穩(wěn)定性。

1 路徑規(guī)劃

1.1 問題描述

定義C?Rd為規(guī)劃問題的d維位形空間(configuration space)，Cobs?C表示障礙物空間，Cfree=CCobs為自由空間。對(duì)于給定的初始位形qinit∈Cfree及目標(biāo)位形qgoal∈Cfree，規(guī)劃問題可以描述為：找到一條連續(xù)無碰撞路徑σ:[0,1]→Cfree滿足σ(0)=qinit和σ(1)=qgoal。

四旋翼無人機(jī)是欠驅(qū)動(dòng)系統(tǒng)，如果在其12維狀態(tài)空間中進(jìn)行路徑規(guī)劃會(huì)造成問題求解的代價(jià)非常高。利用四旋翼飛行器[9]的微分平坦特性降低規(guī)劃空間的維數(shù)，可以大大降低規(guī)劃問題的復(fù)雜度。微分平坦系統(tǒng)是指系統(tǒng)的狀態(tài)和輸入都可以表示成平坦輸出及其有限階導(dǎo)數(shù)的函數(shù)[10]，本文選取平坦輸出為

ξ=[xyzψ]

式中x,y,z為飛行器的質(zhì)心在慣性坐標(biāo)系的坐標(biāo)；ψ為偏航角。通過飛行器的平坦輸出可以唯一確定全部狀態(tài)和輸入[11]。為進(jìn)一步簡化問題，本文選取[x,y,z]作為規(guī)劃空間，平坦輸出ψ則保持為arctan(y/x)。

1.2 RRT-connect算法步驟

1)分別以初始位形qinit和目標(biāo)位形qgoal為根節(jié)點(diǎn)，初始化兩棵搜索樹Tinit和Tgoal。

2)在自由空間Cfree中抽取采樣點(diǎn)qrand，并在樹Tinit中搜索與qrand歐氏距離最小的節(jié)點(diǎn)qnear；在qnear與qrand的連線上，從qnear處以步長r截取節(jié)點(diǎn)qnew,1。若qnear～qnew,1之間均在空間Cfree中，則將節(jié)點(diǎn)qnew,1添加到樹Tinit中，否則重復(fù)本步驟。

3)在樹Tgoal中搜索與步驟(2)中qnew,1歐氏距離最小節(jié)點(diǎn)qnear，在qnear與qnew,1的連線上，從qnear處以步長r截取節(jié)點(diǎn)qnew,2，若qnear到qnew,2之間均在空間Cfree中，則將節(jié)點(diǎn)qnew,2添加到樹Tgoal中。重復(fù)本步驟，當(dāng)兩棵樹相遇時(shí)終止程序并輸出路徑，遇到障礙物時(shí)進(jìn)入下一步。

4)交換兩棵樹Tinit,Tgoal，返回執(zhí)行步驟(2)。

2 RRT-connect算法的改進(jìn)

2.1 規(guī)劃策略

從RRT-connect算法步驟可知：由于采樣點(diǎn)qrand的隨機(jī)分布，導(dǎo)致輸出路徑節(jié)點(diǎn)過于發(fā)散，路徑中包含有大量鋸齒狀的折線，路徑長度偏離最優(yōu)值較遠(yuǎn)。為提高路徑質(zhì)量，有必要對(duì)算法加以改進(jìn):

1)轉(zhuǎn)角約束

在RRT-connect算法步驟(2)和步驟(3)中，通過查找與搜索樹中歐氏距離最小的節(jié)點(diǎn)qnear就可以進(jìn)行后續(xù)的擴(kuò)展。為了避免生成路徑中存在“打結(jié)”的現(xiàn)象，本文提出在擴(kuò)展前施加一個(gè)轉(zhuǎn)角約束，即

(1)

2)路徑修剪

設(shè)RRT-connect算法輸出的路徑節(jié)點(diǎn)序列為P=[P1,P2,…,Pn]。如果對(duì)于節(jié)點(diǎn)對(duì)(Pi,Pi+2)是無碰撞的，則節(jié)點(diǎn)Pi+1就是冗余路徑節(jié)點(diǎn)，將其從節(jié)點(diǎn)序列P中刪除。在遍歷了整個(gè)路徑節(jié)點(diǎn)序列后，就可以去除冗余的節(jié)點(diǎn)使輸出路徑的質(zhì)量得到大大改善。如圖1所示。

圖1 轉(zhuǎn)角約束和路徑修剪示意

2.2 平滑策略

改進(jìn)后RRT-connect算法生成的路徑仍是折線，四旋翼飛行器在跟蹤所生成的路徑時(shí)需要在轉(zhuǎn)折點(diǎn)附近減速并完成轉(zhuǎn)向來減小軌跡跟蹤誤差[13]，這導(dǎo)致飛行效率大幅下降。因此，需要對(duì)路徑作平滑處理，以獲得幾何連續(xù)的軌跡。

2.2.1 B樣條曲線

在規(guī)劃空間[x,y,z]中，飛行器軌跡的各分量可以用一個(gè)含有n+1個(gè)控制點(diǎn)的k階B樣條曲線來表示，即

(2)

式中Bi,k(u)為基函數(shù)，Pi為第i個(gè)控制點(diǎn)向量?；瘮?shù)表達(dá)式由de Boor-Cox遞推公式[14]給出

(3)

(4)

定義節(jié)點(diǎn)向量為U=[u0u1…um]，其中各元素保持單調(diào)不減(ui≤ui+1)，且m=k+n+1。本文取u∈[0,1]作為參數(shù)區(qū)間。

由上述遞推公式可知，基函數(shù)與節(jié)點(diǎn)向量各元素的分布以及曲線的階數(shù)有關(guān)，而與控制點(diǎn)獨(dú)立，這使得曲線具有良好的局部可修改性。

2.2.2 控制點(diǎn)調(diào)整策略

與文獻(xiàn)[11,15]中通過施加硬約束來保證分段多項(xiàng)式連續(xù)的平滑策略不同，本文將利用全部路徑節(jié)點(diǎn)作為控制點(diǎn)生成單條連續(xù)的參數(shù)化軌跡，可以預(yù)先求得解析解。為了使曲線經(jīng)過指定的起點(diǎn)和終點(diǎn)，令節(jié)點(diǎn)向量U中兩端元素滿足k+1次重合度，并使得內(nèi)部節(jié)點(diǎn)元素均勻分布，即

(5)

雖然B樣條曲線在控制點(diǎn)的作用下對(duì)路徑有較好的跟隨性，但仍然存在軌跡偏離路徑節(jié)點(diǎn)較遠(yuǎn)的情況，特別是在控制邊較長及相鄰控制邊轉(zhuǎn)角較大時(shí)尤其明顯[16]，這大大增加了平滑后軌跡發(fā)生碰撞的概率。為此，本文在每條控制邊等分點(diǎn)處插入若干中間控制點(diǎn)，降低軌跡偏離原路徑的程度。在每次生成平滑曲線后都進(jìn)行碰撞檢測，直到插入的控制點(diǎn)使曲線無碰撞為止。

如圖2所示，通過在每條控制邊中點(diǎn)處各插入一個(gè)控制點(diǎn)，使平滑后的軌跡更加貼合原始路徑，避免了生成的軌跡與障礙物間的干涉。

圖2 插入中間控制點(diǎn)前、后軌跡

3 算法驗(yàn)證與分析

3.1 算法驗(yàn)證

實(shí)驗(yàn)中，將規(guī)劃空間限定為100 m×100 m×30 m的三維區(qū)域中，障礙物區(qū)域由若干個(gè)半徑為8 m的隨機(jī)分布的球形區(qū)域組成。設(shè)定規(guī)劃任務(wù)的起點(diǎn)坐標(biāo)為(5,5,10)m，目標(biāo)點(diǎn)坐標(biāo)為(95,95,15)m，規(guī)劃步長r為3，B樣條曲線階數(shù)k為3。

圖3所示為某次規(guī)劃的結(jié)果。圖中兩個(gè)圓形標(biāo)記分別表示起點(diǎn)和目標(biāo)點(diǎn)，虛線路徑表示修剪前的路徑，點(diǎn)劃線為修剪后的路徑，實(shí)線為平滑后的最終軌跡。從圖3可以看出，規(guī)劃算法能找到一條連續(xù)路徑避開所有障礙區(qū)域，連接起點(diǎn)和目標(biāo)點(diǎn)。

圖3 規(guī)劃結(jié)果

圖4(a)為插入中間控制節(jié)點(diǎn)后生成的平滑軌跡，與圖3(b)中實(shí)線路徑相對(duì)應(yīng)，“+”標(biāo)記的為新插入控制節(jié)點(diǎn)的位置。從圖中可以看出生成的B樣條軌跡貼近原始路徑，能避免與障礙物發(fā)生碰撞。圖4(b)為圖4(a)中軌跡對(duì)應(yīng)的曲率隨參數(shù)u變化的曲線，可以看出最終軌跡的曲率是連續(xù)有界變化的，且在起始、目標(biāo)位置曲率為0，這說明三階B樣條曲線能保證加速度的連續(xù)性，且是有界的。

圖4 B樣條曲線軌跡與對(duì)應(yīng)參數(shù)曲率變化

3.2 性能分析

由于本文算法包含隨機(jī)性，為了分析其綜合性能及穩(wěn)定性，以圖3(a)所示環(huán)境模型為基礎(chǔ)在MATLAB 2017上進(jìn)行1 000次重復(fù)規(guī)劃實(shí)驗(yàn)，并與標(biāo)準(zhǔn)RRT算法,RRT-connect算法及目標(biāo)偏向RRT(RRT-biasing)算法規(guī)劃結(jié)果進(jìn)行對(duì)比。4種規(guī)劃算法的運(yùn)行時(shí)間、路徑長度分布如圖5所示，詳細(xì)統(tǒng)計(jì)數(shù)據(jù)見表1。

圖5 各算法性能對(duì)比

算法時(shí)間/s平均值標(biāo)準(zhǔn)差路徑長度/m平均值標(biāo)準(zhǔn)差本文算法0.02960.0115140.466.94RRT-connect0.02400.0116173.6813.36標(biāo)準(zhǔn)RRT0.50110.6367204.7227.67Biasing-RRT0.02950.0095167.6111.81

從圖5(a)中可以看出，本文算法的運(yùn)行時(shí)間相較于標(biāo)準(zhǔn)RRT算法，分布更集中且均在0.1 s以內(nèi)，可以滿足實(shí)時(shí)性要求。結(jié)合表1的數(shù)據(jù)，本文算法平均運(yùn)行時(shí)間與RRT-connect算法相比慢0.005 6 s，與RRT-biasing算法相比大致持平，這是由于本文算法在RRT-connect算法基礎(chǔ)上增加了路徑修剪及平滑處理過程；從運(yùn)行時(shí)間的標(biāo)準(zhǔn)差來看，本文算法與RRT-connect算法幾乎一致，這說明本文算法的附加處理過程對(duì)算法的穩(wěn)定性影響十分微小。

如表1所示，本文算法路徑長度的平均值為140.46 m(十分接近直線距離127.37 m)，與RRT-connect算法、標(biāo)準(zhǔn)RRT算法及RRT-biasing算法相比分別縮短了19 %,31 %和16 %；在圖5(b)中可以看出，本文算法輸出路徑長度主要分布在150 m以內(nèi)，少數(shù)離群異常點(diǎn)也都能維持在160 m左右。這說明本文的路徑修剪策略是有效的，與其他三種算法的結(jié)果相比有較大優(yōu)勢(shì)。同時(shí)表1的標(biāo)準(zhǔn)差數(shù)據(jù)表明本文算法輸出路徑長度的穩(wěn)定性也要優(yōu)于其他三種算法。

4 結(jié) 論

在進(jìn)行1 000次重復(fù)規(guī)劃實(shí)驗(yàn)后，結(jié)果表明：本文算法運(yùn)行時(shí)間較短具有實(shí)時(shí)性，路徑長度接近理論最優(yōu)值，規(guī)劃質(zhì)量得到提高。