楊秀霞, 周硙硙, 張毅

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

?

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

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

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

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

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

0　引言

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

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

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

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

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

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

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

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

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

(1)

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

(2)

(3)

其中:

(4)

(5)

另外,五次PH路徑的6個路徑控制點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個控制點之間的關(guān)系式,可確定五次PH路徑的6個控制點。因此,由式(1)可實現(xiàn)UAV五次PH路徑規(guī)劃。

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

(11)

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

1.2五次PH螺線路徑生成

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

設(shè)定:

(12)

(13)

(14)

(15)

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

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

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

2.1侵犯安全圓問題

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

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

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

2.2解決方案

2.2.1二維平面圓心跟蹤算法

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

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

2.2.2安全系數(shù)法

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

2.2.3五次PH螺線路徑修正法

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

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

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

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

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

(16)

由式(16)可得:

(17)

又因為終止點P5位于障礙物的安全圓上,則:

(18)

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

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

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

(19)

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

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

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

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

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

4　仿真驗證

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

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

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

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

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

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

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

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

5　 結(jié)束語

通過分析現(xiàn)有避障算法忽略可能存在的避障重規(guī)劃路徑侵犯威脅障礙物安全圓、降低了UAV避障重規(guī)劃路徑的安全性問題,在對障礙物安全圓侵犯問題詳細分析基礎(chǔ)上,考慮了避障路徑重規(guī)劃的性能約束及相應(yīng)算法的復(fù)雜度和實時性要求,利用PH螺線路徑連接直線與圓的方法解決了避障重規(guī)劃路徑侵犯威脅障礙物安全圓的問題。仿真結(jié)果驗證了PH螺線路徑對避障路徑修正的有效性和可行性。本文提出的基于PH螺線的避障重規(guī)劃路徑修正方法實現(xiàn)簡單,可提高UAV在復(fù)雜環(huán)境下執(zhí)行任務(wù)的安全性。下一步將對三維空間避障重規(guī)劃路徑侵犯安全球的問題進行研究,并考慮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),高曉光.勢場理論的多無人機協(xié)同路徑規(guī)劃方法[J].火力與指揮控制,2012,37(3):10-12.

[4]張得舒,黃長強,丁達理,等.基于A*算法的無人機攻擊軌跡解算[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]呂盼稂,張珊珊,竺長安.基于Pythagorean速端螺線的平穩(wěn)加工路徑[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]蔡華輝,王國瑾.三次C-Bézier螺線構(gòu)造及其在道路設(shè)計中的應(yīng)用[J].浙江大學(xué)學(xué)報(工學(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ò)出版時間:2016-05-18 13:49

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

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

V249.1

A

1002-0853(2016)05-0086-05