一種三維空間UAV自主避障算法研究?

楊秀霞 周硙硙 張毅 華偉 孟啟源

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

一種三維空間UAV自主避障算法研究?

楊秀霞1周硙硙2張毅1華偉2孟啟源2

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

在復(fù)雜環(huán)境下，為提升UAV空間執(zhí)行任務(wù)的安全性和可靠性，提出一種三維空間UAV自主避障方法，建立三維空間碰撞錐和速度障礙錐模型，確定UAV空間自主避障的最小速度矢量偏轉(zhuǎn)角，并求解了UAV的避障點(diǎn)。同時(shí)，將確定的避障點(diǎn)和避障方向作為UAV三維五次Pythagorean Hodograph（PH）避障重規(guī)劃路徑的初始化條件進(jìn)行仿真驗(yàn)證，仿真結(jié)果驗(yàn)證了自主避障方法的有效性和可行性。

UAV；自主避障；空間速度障礙；路徑重規(guī)劃

Class NumberTP301

1 引言

近年來，許多學(xué)者對UAV在動(dòng)靜態(tài)環(huán)境中的自主避障算法進(jìn)行了研究，提高UAV在復(fù)雜環(huán)境中對威脅障礙物的“感知和避碰”能力［1］，如何提高UAV在三維空間復(fù)雜環(huán)境中執(zhí)行艱巨任務(wù)的生存能力成為UAV技術(shù)研究中亟待解決的關(guān)鍵問題之一［2］。為UAV配備高性能的自主避障系統(tǒng)可實(shí)現(xiàn)UAV在復(fù)雜環(huán)境下對動(dòng)靜態(tài)障礙物的避碰，即要求自主避障系統(tǒng)配備的自主避障算法能實(shí)現(xiàn)對動(dòng)靜態(tài)威脅障礙物的避碰；另外，提升UAV的自主避障系統(tǒng)性能，可提高UAV在復(fù)雜動(dòng)態(tài)環(huán)境中執(zhí)行任務(wù)的生存能力。

目前，針對三維空間自主避障的研究文獻(xiàn)相對較少。學(xué)者J.Ellerbroek［3～4］對三維空間中飛行器之間的相互避碰分離進(jìn)行了研究，通過空間障礙錐進(jìn)行建模，并采用降維方式實(shí)現(xiàn)避碰分離。F.Belk?houche［5］針對三維空間靜態(tài)威脅障礙物避碰，采用了平面降維的思想，利用避障圓錐法實(shí)現(xiàn)了避障，并采用擴(kuò)展比例導(dǎo)引律進(jìn)行避障導(dǎo)引，在導(dǎo)引律設(shè)計(jì)參數(shù)中考慮了飛行器的飛行速度、角速度等約束限制，可產(chǎn)生非直線導(dǎo)引航跡，但文中沒考慮對動(dòng)態(tài)威脅障礙物的避碰。文獻(xiàn)［6］同樣采用降維避障方法，利用避碰障礙錐，對三維避碰采用比例導(dǎo)引方法實(shí)現(xiàn)。文獻(xiàn)［7］采用三維勢場法避障，也存在跟二維勢場法相似地缺陷，會(huì)陷入局部最優(yōu)，并產(chǎn)生無效的運(yùn)動(dòng)。文獻(xiàn)［8］基于流水避石原理實(shí)現(xiàn)對三維空間靜態(tài)障礙物的避碰，并給出相應(yīng)的避障路徑規(guī)劃方法。

本文通過建立三維空間碰撞錐和三維空間速度障礙模型，在此基礎(chǔ)上確定UAV對威脅障礙物避碰的空間最小速度矢量偏轉(zhuǎn)角。同時(shí)，也對UAV的空間避障點(diǎn)進(jìn)行了求解，并將其作為三維五次PH避障重規(guī)劃路徑的初始條件，實(shí)現(xiàn)UAV對威脅障礙物的避障路徑重規(guī)劃，提升UAV在復(fù)雜環(huán)境中執(zhí)行任務(wù)的安全性和可靠性。

2 三維空間碰撞錐的建立

UAV在執(zhí)行任務(wù)的過程中，利用自身攜帶的傳感器裝置，可探測到周圍環(huán)境中存在的動(dòng)靜態(tài)障礙物。并可獲取動(dòng)靜態(tài)障礙物的位姿信息Pose3obs()

PO，vo；同時(shí)，UAV的飛行傳感器也可實(shí)時(shí)地獲取自身的位姿信息Pose3uav() PU，vu。動(dòng)靜態(tài)障礙物的位姿信息Pose3obs() PO，vo和UAV自身的位姿信息Pose3uav() PU，vu可表示為

式中：vo和vu分別為障礙物和UAV的速度矢量的大?。沪萶、φo和θu、φu分別為障礙物和UAV的速度矢量方向角。

另外，根據(jù)UAV攜帶的傳感器性能，設(shè)定其探測距離為d0；為簡化UAV對威脅障礙物的避碰，將UAV簡化為一點(diǎn)，障礙物相應(yīng)的“膨脹”為一個(gè)球體，障礙球的半徑記作R，且半徑R由UAV自身的尺寸和障礙物的威脅輻射范圍確定。過UAV的位置坐標(biāo)點(diǎn)PU作障礙球PO的切線，所有障礙球PO的切線形成三維空間錐面，而障礙球上所有的切點(diǎn)形成切線圓（Tangent Circle，TC），相應(yīng)的UAV與障礙球之間的空間碰撞錐（3-Dimensional Collision Cone，3-DCC）如圖1所示。

圖1三維空間碰撞錐示意圖

當(dāng)UAV感知到周圍環(huán)境中存在的動(dòng)靜態(tài)障礙物時(shí)，需要判定障礙物的威脅性。為簡化對UAV感知到的障礙物威脅性判定，依據(jù)示意圖1中的矢量幾何關(guān)系，確定矢量之間的夾角，從而給出UAV對障礙物威脅性判定的結(jié)論。

結(jié)論1：當(dāng)α≥αo時(shí)，感知到的障礙物不具有威脅性；當(dāng)α＜αo時(shí)，感知到的障礙物具有威脅性。其中，α為相對速度矢量vuo與3-DCC的軸線矢量之間的夾角；αo為空間障礙錐的半頂角。

而α與αo可由下面的關(guān)系式進(jìn)行求解，

為實(shí)現(xiàn)UAV對威脅障礙物的避碰，需要將位于3-DCC內(nèi)的相對速度矢量vuo偏轉(zhuǎn)出3-DCC；而UAV在實(shí)際避碰過程中控制UAV的速度矢量vu，那么需要對3-DCC進(jìn)行轉(zhuǎn)化。為簡化UAV空間避障模型，將3-DCC平移障礙物速度矢量vo，得到三維空間速度障礙（3-D Velocity Obstacle，3-DVO），示意圖如圖2所示。

3 UAV空間自主避障方向的確定

作平面P3-DVOK1K2，其中，平面P3-DVOK1K2由矢量vuo和確定；K1和K2為平面P3-DVOK1K2與TC的交點(diǎn)，如圖3所示。另外，平面P3-DVOK1K2與3-DVO錐面的交線為P3-DVOK1、P3-DVOK2，交線P3-DVOK1、P3-DVOK2也為3-DVO錐面的母線。當(dāng)UAV的速度矢量vu偏轉(zhuǎn)至母線P3-DVOK1、P3-DVOK2上時(shí)，該UAV速度矢量vu為臨界速度矢量，對應(yīng)的相對速度矢量vuo位于母線P3-DVOK1、P3-DVOK2上，因此UAV可實(shí)現(xiàn)對威脅障礙物的避碰。UAV的速度矢量vu與母線P3-DVOK1、P3-DVOK2相交的臨界狀態(tài)點(diǎn)A1、A2，且臨界狀態(tài)點(diǎn)A1、A2可由下列關(guān)系式進(jìn)行求解：

圖2三維空間速度障礙示意圖

圖3UAV避障臨界狀態(tài)點(diǎn)示意圖

根據(jù)式（8）～（11）可確定臨界狀態(tài)點(diǎn)A1(x1，y1，z1)的坐標(biāo)，臨界狀態(tài)點(diǎn)A2(x2，y2，z2)的坐標(biāo)可類似確定。那么，對應(yīng)臨界狀態(tài)點(diǎn)A1、A2的避障速度矢量也可確定。此時(shí)，臨界狀態(tài)點(diǎn)A1對應(yīng)的UAV速度矢量因避障而偏轉(zhuǎn)空間角度最??；臨界狀態(tài)點(diǎn)A2對應(yīng)的速度矢量因避障而偏轉(zhuǎn)空間角度最大。

4 UAV自主避障算法仿真驗(yàn)證

將在第三節(jié)確定的UAV空間自主避障速度矢量方向應(yīng)用到三維五次PH曲線［9］路徑規(guī)劃，并進(jìn)行仿真驗(yàn)證。為實(shí)現(xiàn)UAV對威脅障礙物的避碰PH路徑重規(guī)劃，需要確定避障點(diǎn)Pavo及避障方向，并以此作為三維五次PH避障路徑重規(guī)劃的初始化條件。文獻(xiàn)［10］研究了三維空間UAV可飛行PH路徑規(guī)劃方法，考慮了三維空間UAV路徑規(guī)劃的曲率、撓率和爬升角約束條件，并給出了規(guī)劃三維可飛行PH路徑參數(shù)選取方法。在此基礎(chǔ)上，對UAV空間自主避障算法進(jìn)行仿真驗(yàn)證。由于UAV自主避障方向在第3節(jié)已經(jīng)確定，下面對UAV的避障點(diǎn)Pavo進(jìn)行求解。

首先，UAV對威脅障礙物的避碰時(shí)間可確定為

則避碰點(diǎn)Pavo可表示為

在復(fù)雜環(huán)境下，UAV執(zhí)行從起飛點(diǎn)Ps(0，0，0)至目標(biāo)點(diǎn)Pf(600，400，125)的飛行任務(wù)。UAV在飛行過程中利用自身攜帶的傳感器裝置探測到周圍環(huán)境中存在的障礙物，UAV需要對威脅障礙物進(jìn)行避碰。表1給出了UAV對威脅障礙物O1避碰的初始化條件。

表1 威脅障礙物避碰的初始化條件

根據(jù)表1中UAV和威脅障礙物O1的初始化條件，可確定UAV對威脅障礙物O1的最小空間避碰速度矢量方向?yàn)镼avo(28.8，11.9)，避碰點(diǎn)為Pavo(216，180，41.7)。將其應(yīng)用于PH曲線路線規(guī)劃的仿真結(jié)果如圖4所示，仿真結(jié)果表明提出的三維空間UAV自主避障算法能夠?qū)崿F(xiàn)對威脅障礙物的避碰。

圖4UAV對威脅障礙物避碰仿真結(jié)果

5 結(jié)語

本文通過建立三維空間碰撞錐和速度障礙模型，確定了UAV對空間威脅障礙物避碰的最小速度矢量偏轉(zhuǎn)角，并求解了避障點(diǎn)的坐標(biāo)。同時(shí)，將得到的避障點(diǎn)和避障方向作為三維五次PH避障路徑重規(guī)劃的初始化條件，進(jìn)行了相應(yīng)的仿真驗(yàn)證。仿真結(jié)果表明：提出的UAV空間自主避障方法能夠?qū)崿F(xiàn)對威脅障礙物的避碰，對UAV在復(fù)雜環(huán)境下執(zhí)行艱巨任務(wù)的安全性和生存能力具有重要理論意義。

［1］齊曉慧，田慶民，甄紅濤.無人飛行器系統(tǒng)的感知與規(guī)避——研究與應(yīng)用［M］.北京：國防工業(yè)出版社，2014，154-177. QI Xiaohui，TIAN Qingmin，ZHEN Hongtao.Sense and Avoid in UAS：Research and Applications［M］.Beijing：National Defense Industry Press，2014，154-177.

［2］周歡，魏瑞軒，崔軍輝，等.面向不確定性環(huán)境的多無人機(jī)協(xié)同防撞［J］.電光與控制，2014，21（1）：91-96. ZHOU Huan，WEI Ruixuan，CUI Jiahui，et al.Multi-UAV Cooperative Collision Avoidance Against Uncertain Envi?ronment，Electronics Optics and Control，2014，21（1）：91-96.

［3］Ellerbroek J，Brantegem K C R，van Paassen M M，et al. Design of a coplanar airborne separation display［J］.IEEE TransactionsonHuman-MachineSystems，2013，43（3）：277-289.

［4］Ellerbroek J，Visser M，Van Dam S B J，et al.Design of an airborne three-dimensional separation assistance dis?play［J］.IEEE Transactions on Systems，Man，and Cyber?netics-Part A：Systems and Humans，2011，41（5）：863-875.

［5］F.Belkhouche，B.Bendjilali.Reactive Path Planning for 3-D Autonomous Vehicles［J］.IEEE Transactions on Con?trol Systems Technology，2012，20（1）：249-256.

［6］Su-Cheol Han，Hyochoong Banz，Chang-sun Yoo.Pro?portional Navigation-Based Collision Avoidance［J］.Inter?national Journal of Control，Automation，and Systems，2009，7（4）：553-565.

［7］洪曄，邊信黔.基于三維速度勢場的AUV局部避碰研究［J］.機(jī)器人，2007，29（1）：88-91. HONG Ye，BIAN Xinqian.An AUV Obstacle-Avoidan?ceM ethod Based on Three-di mensional［J］.Robot，2007，29（1）：88-91.

［8］梁宵，王紅倫，李大偉，等.基于流水避石原理的無人機(jī)三維航路規(guī)劃方法［J］.航空學(xué)報(bào)，2013，34（7）：1670-1681. LIANG Xiao，WANG Honglun，LI Dawei，et al.Three-di?mensional path planning for unmanned aerial vehicles based on principles of stream avoiding obstacles［J］.Acta Aeronauticaet AstronauticaSinica，2013，34（7）：1670-1681.

［9］Farouki R T，Sakkalis T.Pythagorean hodographs［J］. IBM Journal of Research and Development，1990，34（5）：736-752.

［10］Zhang Yi，Yang Xiuxia，Zhou Weiwei.Flyable path plan?ning for multiple UAVs in complicated threat environ?ment［C］//Multisensor Fusion and Information Integra?tion for Intelligent Systems（MFI），2014 International Conference on.IEEE，2014：1-5.

A Three-dimensional Autonomous Obstacle Avoidance Algorithm for UAV

YANG Xiuxia1ZHOU Weiwei2ZHANG Yi1HUA Wei2MENG Qiyuan2
（1.Department of Control Engineering，Naval Aeronautical and Astronautical University，Yantai264001）（2.Graduate Students'Brigade，Naval Aeronautical and Astronautical University，Yantai264001）

In order to improve the security and reliability of UAV's space mission in complex environment，a 3-D UAV auton?omous obstacle avoidance algorithm is proposed in the paper.3-D models of collision cone and velocity obstacle are established，the minimum velocity vector deflection angle of spatial autonomous obstacle avoidance is determined for UAV，and the obstacle avoid?ance point of UAV is solved.At the same time，the UAV's 3-D quintic Pythagorean Hodograph（PH）obstacle avoidance planning path is simulated and verified with the determined obstacle avoidance point and the direction of obstacle avoidance as the initial con?ditions，the simulation results verify the effectiveness and feasibility of the autonomous obstacle avoidance method.

UAV，autonomous obstacle avoidance，spatial velocity obstacle，path re-planning

TP301

10.3969/j.issn.1672-9722.2017.07.015

2017年1月14日，

2017年2月17日

楊秀霞，女，博士，副教授，研究方向：飛行器智能控制及制導(dǎo)。周硙硙，男，碩士，研究方向：飛行器導(dǎo)航、制導(dǎo)與控制。張毅，男，博士，副教授，研究方向：飛行器導(dǎo)航、制導(dǎo)與控制。華偉，男，碩士，研究方向：飛行器導(dǎo)航、制導(dǎo)與控制。孟啟源，男，碩士，研究方向：飛行器導(dǎo)航、制導(dǎo)與控制。