一種基于PH螺線的避障重規(guī)劃路徑修正方法

楊秀霞, 周硙硙, 張毅

(1.海軍航空工程學(xué)院 控制工程系, 山東 煙臺(tái) 264001;2.海軍航空工程學(xué)院 研究生管理大隊(duì), 山東 煙臺(tái) 264001)

?

一種基于PH螺線的避障重規(guī)劃路徑修正方法

楊秀霞1, 周硙硙2, 張毅1

(1.海軍航空工程學(xué)院 控制工程系, 山東 煙臺(tái) 264001;2.海軍航空工程學(xué)院 研究生管理大隊(duì), 山東 煙臺(tái) 264001)

針對(duì)現(xiàn)有避障算法忽略重規(guī)劃路徑侵犯威脅障礙物安全圓的問(wèn)題,提出了一種基于Pythagorean Hodograph(PH)螺線修正避障重規(guī)劃路徑的方法。該方法能夠考慮無(wú)人飛行器路徑規(guī)劃的相關(guān)性能約束,且較其他路徑修正算法復(fù)雜度較低、實(shí)時(shí)性高,能夠滿足UAV在線規(guī)避動(dòng)靜態(tài)障礙物的要求。仿真結(jié)果驗(yàn)證了該方法的有效性和可行性。

避障; PH螺線; 路徑規(guī)劃; 無(wú)人飛行器

0　引言

在未知?jiǎng)討B(tài)復(fù)雜環(huán)境下,為確保UAV安全飛行到達(dá)目的地,需要對(duì)飛行過(guò)程中探測(cè)到的動(dòng)靜態(tài)障礙物進(jìn)行避碰,因此,規(guī)劃出滿足約束條件安全可飛行的路徑是UAV飛行控制技術(shù)的重要方面。

近年來(lái),在機(jī)器人和無(wú)人飛行器領(lǐng)域,眾多學(xué)者對(duì)復(fù)雜環(huán)境下的避障算法進(jìn)行了研究,這些算法包括:微分幾何法[1]、導(dǎo)引法[2]、勢(shì)場(chǎng)法[3]、隨機(jī)搜索算法[4]和曲線規(guī)劃法[5]等。文獻(xiàn)[1]基于微分幾何實(shí)現(xiàn)了對(duì)動(dòng)靜態(tài)障礙物的威脅判斷和避碰;文獻(xiàn)[2]通過(guò)導(dǎo)引的方式實(shí)現(xiàn)了對(duì)探測(cè)到的威脅障礙物避碰;文獻(xiàn)[3]通過(guò)勢(shì)場(chǎng)中障礙物的斥力和目標(biāo)點(diǎn)的引力完成了對(duì)當(dāng)前航跡的避障重規(guī)劃;文獻(xiàn)[4]在搜索算法的基礎(chǔ)上確定安全航跡點(diǎn),并對(duì)航跡進(jìn)行平滑完成避障路徑重規(guī)劃;文獻(xiàn)[5]基于參數(shù)化曲線實(shí)現(xiàn)UAV的避障路徑重規(guī)劃。但在這些方法中,忽略了避障重規(guī)劃路徑可能對(duì)威脅障礙物安全圓的侵犯問(wèn)題,且該問(wèn)題會(huì)影響UAV的安全飛行。文獻(xiàn)[6]給出了一種解決UAV避障重規(guī)劃路徑侵犯威脅障礙安全圓問(wèn)題的圓心跟蹤算法,算法需要通過(guò)迭代的方式確定安全圓上新的航跡點(diǎn),因此,該算法的實(shí)時(shí)性較差且較為復(fù)雜。

在工程領(lǐng)域內(nèi),螺線具有廣泛的實(shí)際應(yīng)用,如機(jī)器人的路徑模擬、道路設(shè)計(jì)等。Pythagorean Hodograph(PH)螺線具有優(yōu)良的性質(zhì),該曲線的曲率符號(hào)保持不變,且曲率大小隨螺線參數(shù)單調(diào)變化。另外,同PH曲線一樣,PH螺線弧長(zhǎng)可以被精確計(jì)算且其等距線具有有理形式。文獻(xiàn)[7]將PH螺線應(yīng)用于平穩(wěn)加工的路徑規(guī)劃中,實(shí)現(xiàn)了高速切削加工平穩(wěn)的曲線過(guò)渡,解決了由于加速度不連續(xù)變化導(dǎo)致的數(shù)控機(jī)床激烈抖動(dòng)問(wèn)題。Bass[8]將道路設(shè)計(jì)中的曲線過(guò)渡分為5種情況:直線與直線之間的過(guò)渡;直線與圓之間的過(guò)渡;圓與圓之間的C型螺線過(guò)渡;圓與圓之間的S型螺線過(guò)渡;圓與其內(nèi)部圓弧之間的過(guò)渡。由于回旋曲線的曲率和弧長(zhǎng)呈線性關(guān)系,文獻(xiàn)[9]為滿足道路工程設(shè)計(jì)的需求,基于三次C-Bézier螺線實(shí)現(xiàn)了不同道路設(shè)計(jì)情況下的曲線過(guò)渡,解決了回旋曲線不適合計(jì)算機(jī)設(shè)計(jì)系統(tǒng)的問(wèn)題。

由于五次PH螺線內(nèi)部沒(méi)有尖點(diǎn)和拐點(diǎn),可選擇其為過(guò)渡曲線。另外,當(dāng)沿曲線運(yùn)動(dòng)的速度保持恒定時(shí),法向加速度和向心力與曲率大小變化正相關(guān)。因此,以五次PH螺線為過(guò)渡曲線可有效地避免向心力突變的問(wèn)題。本文針對(duì)避障算法中未考慮避障重規(guī)劃路徑侵犯威脅障礙物安全圓的問(wèn)題,提出了一種基于PH螺線的避障路徑修正方法。

1　PH螺線路徑的構(gòu)造

1.1五次PH曲線路徑規(guī)劃

本文采用五次PH螺線路徑對(duì)存在侵犯威脅障礙物安全圓的避障路徑進(jìn)行修正,五次PH螺線路徑的構(gòu)造是基于五次PH路徑。

在初始點(diǎn)Ps(xs,ys,φs)和終止點(diǎn)Pf(xf,yf,φf(shuō))之間生成的滿足約束條件的可飛行路徑可表示為:

二維五次PH路徑r(t)以控制點(diǎn)的形式可表示為:

(1)

且x(t)和y(t)滿足:

(2)

(3)

其中:

(4)

(5)

另外,五次PH路徑的6個(gè)路徑控制點(diǎn)Pk(k=0,1,2,3,4,5)之間存在下列關(guān)系:

(6)

P2=P1+0.2(u0u1-v0v1)T+0.2(u0v1+u1v0)N

(7)

(4u1v1+u0v2+u2v0)N/15

(8)

P4=P3+0.2(u1u2-v1v2)T+0.2(u1v2+u2v1)N

(9)

(10)

那么,根據(jù)規(guī)劃五次PH路徑的初始化條件和6個(gè)控制點(diǎn)之間的關(guān)系式,可確定五次PH路徑的6個(gè)控制點(diǎn)。因此,由式(1)可實(shí)現(xiàn)UAV五次PH路徑規(guī)劃。

另外,五次PH路徑的曲率表達(dá)式為:

(11)

為使規(guī)劃UAV的路徑可飛行,規(guī)劃的PH路徑需滿足曲率約束條件,文獻(xiàn)[5]給出了生成滿足曲率約束條件路徑的控制參數(shù)選取策略。本文在此基礎(chǔ)上構(gòu)造滿足約束條件的五次PH螺線路徑,對(duì)存在侵犯威脅障礙物安全圓的重規(guī)劃路徑進(jìn)行修正。

1.2五次PH螺線路徑生成

五次PH曲線路徑的規(guī)劃是基于相應(yīng)的初始化條件,對(duì)式(4)和式(5)中的二次多項(xiàng)式u(t)和v(t)系數(shù)進(jìn)行求解。在五次PH曲線路徑的基礎(chǔ)上,需要對(duì)二次多項(xiàng)式u(t)和v(t)中的系數(shù)進(jìn)行定義,使得構(gòu)造的五次PH路徑的曲率滿足螺線路徑曲率要求,從而實(shí)現(xiàn)五次PH螺線路徑的規(guī)劃。

設(shè)定:

(12)

(13)

(14)

(15)

因此,對(duì)起始點(diǎn)P0(Ps)、終止點(diǎn)P5(Pf)以及終止點(diǎn)P5處的切向量和曲率半徑R進(jìn)行初始化,可以確定在設(shè)定條件下的五次PH路徑,且關(guān)于該五次PH路徑的曲率有以下結(jié)論:在設(shè)定條件下,五次PH路徑r(t)的曲率κ(t)具有下列性質(zhì):

稱具有上述性質(zhì)的五次PH路徑為螺線路徑。

2　重規(guī)劃路徑侵犯安全圓問(wèn)題

2.1侵犯安全圓問(wèn)題

在復(fù)雜環(huán)境下執(zhí)行飛行任務(wù)的UAV,當(dāng)自身攜帶的傳感器裝置探測(cè)到來(lái)自未知?jiǎng)討B(tài)環(huán)境中的動(dòng)/靜態(tài)障礙物時(shí),可利用相應(yīng)的避障算法重規(guī)劃當(dāng)前的航跡,實(shí)現(xiàn)對(duì)威脅障礙物的避碰。通常,避障路徑重規(guī)劃需要在UAV的當(dāng)前時(shí)刻位置坐標(biāo)Pt、避障點(diǎn)Qavo和目標(biāo)點(diǎn)P5之間分別生成可飛行的安全路徑。但在規(guī)劃Qavo和P5之間的路徑時(shí),由于存在目標(biāo)點(diǎn)位于威脅障礙物盲區(qū)內(nèi)的情況,則可能存在著重規(guī)劃路徑侵犯威脅障礙物安全圓的問(wèn)題。避障點(diǎn)和目標(biāo)點(diǎn)之間的避障重規(guī)劃路徑侵犯威脅障礙物安全圓的情況如圖1所示。

圖1　UAV重規(guī)劃路徑侵犯威脅障礙安全圓示意圖Fig.1　The sketch that re-planning path of UAV violating threatening obstacle safety circle

關(guān)于避障重規(guī)劃路徑侵犯威脅障礙物安全圓問(wèn)題研究的文獻(xiàn)較少,很多威脅障礙物避障算法忽略了該問(wèn)題,從而降低了避障重規(guī)劃路徑的安全性。文獻(xiàn)[2]針對(duì)三維情況下的避障重規(guī)劃路徑侵犯主體障礙物安全球問(wèn)題,提出了一種球心跟蹤算法。該算法通過(guò)確定一個(gè)球面上的虛擬目標(biāo)點(diǎn),UAV沿球面飛行到虛擬目標(biāo)點(diǎn),直到重新規(guī)劃的航跡不在侵犯主體障礙物的安全球。但該算法需要不斷地迭代,使得重規(guī)劃的避障路徑與主體障礙物的安全球沒(méi)有交點(diǎn),從而導(dǎo)致算法較為復(fù)雜,使得算法的實(shí)時(shí)性和可實(shí)現(xiàn)性降低。

2.2解決方案

2.2.1二維平面圓心跟蹤算法

采用類似三維空間球心跟蹤算法[2]的二維平面圓心跟蹤算法解決威脅障礙物安全圓侵犯問(wèn)題,其示意圖如圖2所示。

圖2　圓心跟蹤算法示意圖Fig.2　The sketch of center-tracking algorithm

2.2.2安全系數(shù)法

在避障算法中,通常將UAV看作質(zhì)點(diǎn),并根據(jù)UAV與障礙物之間的相對(duì)大小將障礙物“膨化”為圓。為解決避障重規(guī)劃路徑侵犯威脅障礙物安全圓問(wèn)題,可在安全圓半徑上乘上一個(gè)安全系數(shù),通過(guò)控制安全系數(shù)的大小解決安全圓侵犯問(wèn)題。但安全系數(shù)會(huì)增加避障重規(guī)劃路徑的長(zhǎng)度,使得避障重規(guī)劃路徑更加偏離最優(yōu)初始可飛行路徑,從而滿足不了重規(guī)劃路徑偏離原路徑小的要求。

2.2.3五次PH螺線路徑修正法

五次PH螺線路徑內(nèi)部沒(méi)有尖點(diǎn)和拐點(diǎn),且其曲率具有良好的性質(zhì)。采用五次PH螺線路徑對(duì)侵犯威脅障礙物安全圓的重規(guī)劃路徑進(jìn)行修正,計(jì)算簡(jiǎn)單、耗時(shí)少,能夠滿足UAV在線避障路徑重規(guī)劃的實(shí)時(shí)性要求,且能考慮UAV在避障點(diǎn)和目標(biāo)點(diǎn)處的方向角約束以及自身的最大曲率半徑等約束。因此,基于五次PH螺線的避障路徑修正方法比二維平面圓心跟蹤算法和安全系數(shù)法更具優(yōu)勢(shì)。為此,本文采用五次PH螺線路徑解決UAV在避障點(diǎn)和目標(biāo)點(diǎn)之間的重規(guī)劃路徑侵犯威脅障礙物安全圓的問(wèn)題。

3　基于PH螺線的避障路徑修正

本文將數(shù)學(xué)上利用五次PH螺線連接直線與圓的過(guò)渡方法應(yīng)用到UAV避障路徑的修正,有效地解決了UAV在避障點(diǎn)和目標(biāo)點(diǎn)之間重規(guī)劃路徑侵犯主體障礙物安全圓問(wèn)題,且修正算法避免了其他方法計(jì)算復(fù)雜、耗時(shí)多等問(wèn)題。與此同時(shí),也考慮了UAV避障路徑重規(guī)劃的性能約束。

3.1規(guī)劃PH螺線路徑

由式(6)～式(10),可確定起始點(diǎn)P0和終止點(diǎn)P5之間的關(guān)系為:

(16)

由式(16)可得:

(17)

又因?yàn)榻K止點(diǎn)P5位于障礙物的安全圓上,則:

(18)

式中:h為障礙物安全圓圓心到坐標(biāo)軸T的距離。

根據(jù)式(17)和式(18),并令γ=cosφf(shuō)可得:

f(γ)=18Rγ2+(11R-60h)γ+91R-60h

(19)

對(duì)安全圓半徑R和h進(jìn)行初始化,使得多項(xiàng)式方程f(γ)=0存在解。將該解代入式(6)～式(10)和式(1),即可確定PH螺線路徑。

3.2PH螺線路徑的調(diào)整

由3.1節(jié)可規(guī)劃出初始連接威脅障礙物安全圓的PH螺線路徑。為使UAV的整個(gè)避障重規(guī)劃路徑曲率連續(xù),需要確保在避障點(diǎn)處的曲率連續(xù)。因此,通過(guò)PH螺線路徑起始點(diǎn)處與避障點(diǎn)處速度矢量方向角的差值,基于相應(yīng)的變換矩陣,可確保在避障點(diǎn)處的曲率連續(xù)。另外,PH螺線路徑的起始點(diǎn)可通過(guò)坐標(biāo)平移的方法平移到UAV的實(shí)際避障點(diǎn)處。

假設(shè)PH路線路徑起始點(diǎn)處與避障點(diǎn)處速度矢量方向角的差值為δ,則相應(yīng)的變換矩陣為:

再通過(guò)安全圓的圓心平移重合,可實(shí)現(xiàn)PH螺線路徑的起始點(diǎn)與避障點(diǎn)重合,且速度矢量的方向角也重合,從而實(shí)現(xiàn)對(duì)避障重規(guī)劃路徑的修正,解決避障重規(guī)劃路徑侵犯威脅障礙物安全圓問(wèn)題。此時(shí),PH螺線路徑的終止點(diǎn)為設(shè)定的虛擬目標(biāo)點(diǎn),在虛擬目標(biāo)點(diǎn)與目標(biāo)點(diǎn)處的路徑重規(guī)劃可通過(guò)文獻(xiàn)[5]的方法實(shí)現(xiàn)。

4　仿真驗(yàn)證

在避障路徑修正過(guò)程中,避障點(diǎn)與虛擬目標(biāo)點(diǎn)之間存在順時(shí)針旋轉(zhuǎn)和逆時(shí)針旋轉(zhuǎn)兩種位置關(guān)系。順時(shí)針旋轉(zhuǎn)位置關(guān)系如圖3所示,圖4為PH螺線路徑的曲率變化。

圖3　順時(shí)針PH螺線路徑修正仿真Fig.3　Simulation diagram of path correction based on clockwise PH spiral

此時(shí),PH螺線路徑的最大曲率半徑為κmax=0.043 m-1,而威脅障礙物的安全圓半徑為R=20 m。因此,PH螺線路徑滿足相應(yīng)的曲率約束條件,且不會(huì)侵犯威脅障礙物的安全圓。

圖4　順時(shí)針PH螺線路徑曲率變化Fig.4　Curvature variation of clockwise PH spiral path

逆時(shí)針旋轉(zhuǎn)位置關(guān)系如圖5所示,圖6則為PH螺線路徑的曲率變化。

圖5　逆時(shí)針PH螺線路徑修正仿真Fig.5　Simulation diagram of path correction 　based on anticlockwise PH spiral

圖6　逆時(shí)針PH螺線路徑曲率變化Fig.6　Curvature variation of anticlockwise PH spiral path

此時(shí),PH螺線路徑的最大曲率半徑為κmax=0.038 m-1,而威脅障礙物的安全圓半徑為R=25 m。因此,PH螺線路徑滿足相應(yīng)的曲率約束條件,且不會(huì)侵犯威脅障礙物的安全圓。

5　 結(jié)束語(yǔ)

通過(guò)分析現(xiàn)有避障算法忽略可能存在的避障重規(guī)劃路徑侵犯威脅障礙物安全圓、降低了UAV避障重規(guī)劃路徑的安全性問(wèn)題,在對(duì)障礙物安全圓侵犯問(wèn)題詳細(xì)分析基礎(chǔ)上,考慮了避障路徑重規(guī)劃的性能約束及相應(yīng)算法的復(fù)雜度和實(shí)時(shí)性要求,利用PH螺線路徑連接直線與圓的方法解決了避障重規(guī)劃路徑侵犯威脅障礙物安全圓的問(wèn)題。仿真結(jié)果驗(yàn)證了PH螺線路徑對(duì)避障路徑修正的有效性和可行性。本文提出的基于PH螺線的避障重規(guī)劃路徑修正方法實(shí)現(xiàn)簡(jiǎn)單,可提高UAV在復(fù)雜環(huán)境下執(zhí)行任務(wù)的安全性。下一步將對(duì)三維空間避障重規(guī)劃路徑侵犯安全球的問(wèn)題進(jìn)行研究,并考慮UAV空間避障路徑的曲率和撓率約束條件。

[1]White B A,Shin H S,Tsourdos A.UAV obstacle avoidance using differential geometry concepts[C]//Proceeding of the 18th International Federation of Automatic Control World Congress.2011:6325-6330.

[2]Han S C,Bang H,Yoo C S.Proportional navigation-based collision avoidance for UAVs[J].International Journal of Control,Automation and Systems,2009,7(4):553-565.

[3]包勇,符小衛(wèi),高曉光.勢(shì)場(chǎng)理論的多無(wú)人機(jī)協(xié)同路徑規(guī)劃方法[J].火力與指揮控制,2012,37(3):10-12.

[4]張得舒,黃長(zhǎng)強(qiáng),丁達(dá)理,等.基于A*算法的無(wú)人機(jī)攻擊軌跡解算[J].電光與控制,2011,18(3):18-20.

[5]張毅,楊秀霞,周硙硙.基于速度障礙法的多UAV可飛行航跡優(yōu)化生成[J].系統(tǒng)工程與電子技術(shù),2015,37(2):323-330.

[6]Mujumdar A,Padhi R.Reactive collision avoidance using nonlinear geometric and differential geometric guidance[J].Journal of Guidance,Control,and Dynamics,2011,34(1):303-311.

[7]呂盼稂,張珊珊,竺長(zhǎng)安.基于Pythagorean速端螺線的平穩(wěn)加工路徑[J].中國(guó)機(jī)械工程,2006(23):2451-2453.

[8]Baass K G.The use of clothoid templates in highway design[J].Transportation Forum,1984(1):47-52.

[9]蔡華輝,王國(guó)瑾.三次C-Bézier螺線構(gòu)造及其在道路設(shè)計(jì)中的應(yīng)用[J].浙江大學(xué)學(xué)報(bào)(工學(xué)版),2010,44(1):68-74.

(編輯：崔立峰)

A re-planning path correction method for collision avoidance based on PH spiral

YANG Xiu-xia1, ZHOU Wei-wei2, ZHANG Yi1

(1.Department of Control Engineering, Naval Aeronautical and Astronautical University,Yantai 264001, China;2.The Bridge of Graduate Students, Naval Aeronautical and Astronautical University,Yantai 264001, China)

For the problem of existing obstacle avoidance algorithms, which ignores that the re-planning path violates threatening obstacle safety circle. A re-planning path correction method for collision avoidance based on Pythagorean Hodograph(PH) spiral was proposed. The related constraints of UAV path planning were considered in this method, and comparing to other correction algorithms, it had low complexity and high real-time performance, which can meet the requirements that UAV online avoided the dynamic and static obstacles. The simulation results show the practicality and efficiency of the re-planning path correction method for collision avoidance based on PH spiral.

obstacle avoidance; PH spiral; path planning; UAV

2016-01-08;

2016-04-19; 網(wǎng)絡(luò)出版時(shí)間:2016-05-18 13:49

航空科學(xué)基金資助(20135584010)

楊秀霞(1975-)，女，山東萊州人，副教授，博士，研究方向?yàn)轱w行器智能控制及制導(dǎo)。

V249.1

A

1002-0853(2016)05-0086-05