楊 萌，王 玥

（北京理工大學(xué)機(jī)電學(xué)院，北京100081）

0 引言

近年來，隨著無人機(jī)技術(shù)的不斷發(fā)展，無人機(jī)被逐漸應(yīng)用于航空噴灑、航空物探、航空攝影、電力巡線、石油作業(yè)等通航作業(yè)領(lǐng)域。為保障飛行安全，通常將無人機(jī)限制在特定空域內(nèi)，與有人機(jī)隔離運(yùn)行。在有限的空域資源下，隔離運(yùn)行方式將難以滿足無人機(jī)、有人機(jī)日益增長的應(yīng)用需求，無人機(jī)進(jìn)入低空空域融合運(yùn)行是我國通用航空事業(yè)發(fā)展的必然趨勢［1］。

在低空空域內(nèi)運(yùn)行的中小型無人機(jī)具有機(jī)動能力強(qiáng)、受天氣等因素影響大、活動空間范圍和類型廣泛等特點(diǎn)，融合運(yùn)行的安全問題嚴(yán)峻。其中，如何避免無人機(jī)間或無人機(jī)與有人機(jī)間的空中碰撞沖突，是實(shí)現(xiàn)融合運(yùn)行的一個核心技術(shù)。美國NASA的Ames研究中心將飛行沖突解脫分為3個階段：1）戰(zhàn)略階段，在20min前實(shí)現(xiàn)沖突解脫；2）戰(zhàn)術(shù)階段，在2min前實(shí)現(xiàn)沖突解脫；3）空中防撞系統(tǒng)（Traffic Alert and Collision Avoidance Sys?tem，TCAS）階段，即在上一階段解脫失敗的情況下，飛行器直接進(jìn)行垂直機(jī)動［2］。對于低空運(yùn)行的安全保障而言，地面通信、導(dǎo)航和監(jiān)視系統(tǒng)只能有效覆蓋終端區(qū)等管制空域，大部分低空飛行活動并不能獲得管制服務(wù)，且中小型無人機(jī)由于成本及體積限制，通常并不搭載TCAS系統(tǒng)。因此，在低空空域內(nèi)運(yùn)行的中小型無人機(jī)必須具備戰(zhàn)術(shù)解脫能力。

1 感知與規(guī)避系統(tǒng)

各國進(jìn)行的無人機(jī)空域安全研究主要可分為陸基感知與規(guī)避、空中感知與規(guī)避2種。

陸基感知與規(guī)避系統(tǒng)利用陸基系統(tǒng)使飛行器在規(guī)定的空域內(nèi)保持一定距離。陸基系統(tǒng)包括雷達(dá)、顯示設(shè)備、通信設(shè)備和軟件。陸基感知與規(guī)避的方案將逐漸削減現(xiàn)有自動化系統(tǒng)的限制，幫助無人機(jī)實(shí)現(xiàn)在國家空域系統(tǒng)內(nèi)的自由飛行，可以很好地適用于大中型無人機(jī)系統(tǒng)［3］。

空中感知與規(guī)避系統(tǒng)注重發(fā)展能夠同時進(jìn)行自動分離與避免碰撞的機(jī)載設(shè)備，保障一定范圍內(nèi)的飛行安全。該性能的目的在于使有人機(jī)飛行員和無人機(jī)操作員能夠在任何空域中通過安全高效的方式避免沖突，避免與其他飛行器發(fā)生碰撞。無人機(jī)必須在目標(biāo)到達(dá)危險前進(jìn)行避讓機(jī)動，最終恢復(fù)到原飛行軌跡并與入侵機(jī)重新保持安全間隔［4］，整個沖突的避讓流程如圖1所示。

本文擬針對感知與規(guī)避實(shí)際背景，對避讓航跡規(guī)劃模塊進(jìn)行算法設(shè)計與模型優(yōu)化，并通過仿真驗(yàn)證其可靠性與優(yōu)化效率。

圖1 無人機(jī)感知與規(guī)避流程Fig.1 Process of sense and avoidance of UAV

2 避讓航跡規(guī)劃算法設(shè)計

目前，無人機(jī)在航跡規(guī)劃中使用較多的智能優(yōu)化算法包括A?算法、遺傳算法、蟻群算法、粒子群算法和人工勢場法等。A?算法適合進(jìn)行全局最優(yōu)航跡規(guī)劃，理論上可以保證全局最優(yōu)解，但搜索空間的增大、內(nèi)部所存儲的數(shù)據(jù)的增加、計算的增大、搜索時間的變長，導(dǎo)致其實(shí)時性無法獲得保證［5］。遺傳算法和粒子群算法具有較強(qiáng)的魯棒性，能自適應(yīng)地調(diào)整搜索方向，不受搜索空間條件的約束，但其局部搜索能力低，在進(jìn)化后期收斂速度慢且容易陷入局部最優(yōu)［6?7］。蟻群算法在對路徑進(jìn)行搜索時是隨機(jī)的，由于信息素?fù)]發(fā)的原因，其可能陷入局部最優(yōu)情況［8］。人工勢場法具有結(jié)構(gòu)簡單、計算量小、實(shí)時性好等優(yōu)點(diǎn)，但如果目標(biāo)距離障礙物太近或斥力與引力相等，則容易陷入局部極小值和局部震蕩［9］。

考慮到感知與規(guī)避系統(tǒng)對高實(shí)時性的要求，放棄A?算法、遺傳算法、蟻群算法等搜索算法，采用實(shí)時性更強(qiáng)的人工勢場法進(jìn)行規(guī)避航跡的計算。

文獻(xiàn)［10］將人工勢場法和蟻群算法進(jìn)行了結(jié)合，通過人工勢場法構(gòu)建了環(huán)境與目標(biāo)之間的啟發(fā)因子，并將其作為初始信息素帶入蟻群算法，幫助蟻群算法快速收斂航跡。文獻(xiàn)［11］通過搜索未知環(huán)境中的目標(biāo)點(diǎn)，設(shè)計了一種無死鎖人工勢場法，有效解決了局部極小值問題。文獻(xiàn)［12］設(shè)計了一種新的勢能函數(shù)，避免了計算過程中繁瑣的逆運(yùn)動學(xué)計算，大大降低了算法的復(fù)雜程度。文獻(xiàn)［13］在傳統(tǒng)人工勢場法中加入了調(diào)節(jié)因子，同時采用遍歷搜索法解決了局部極小值問題。文獻(xiàn)［14］引入了斥力偏轉(zhuǎn)模型，引導(dǎo)機(jī)器人在路徑規(guī)劃時避開局部極小點(diǎn)，并同時通過引入斥力增益系數(shù)函數(shù)，優(yōu)化了航向改變過大的問題。文獻(xiàn)［15］提出了一種虛擬目標(biāo)法，解決了局部極小值的問題。文獻(xiàn)［16］提出了二次引力勢場法，通過傳統(tǒng)人工勢場法計算出了一條預(yù)規(guī)劃航跡，而后以此路徑所提供的航跡點(diǎn)作為基礎(chǔ)引力點(diǎn)，再次利用人工勢場法規(guī)劃航跡。

針對前文所述的人工勢場法容易陷入局部極小值的問題，國內(nèi)外諸多學(xué)者已進(jìn)行了多項研究，并提出了多種改進(jìn)方法，本文不再贅述。以下將結(jié)合感知與規(guī)避問題的實(shí)際背景，給出人工勢場法局部震蕩問題的解決方案，并對威脅區(qū)模型進(jìn)行優(yōu)化。

2.1 人工勢場法的基本原理

人工勢場法的基本思想是將飛行器在規(guī)劃空間中的運(yùn)動看作一種在虛擬力場中的受力運(yùn)動，障礙物或威脅區(qū)域?qū)ζ洚a(chǎn)生排斥力，目標(biāo)點(diǎn)對其產(chǎn)生吸引力，飛行器在合力的作用下朝著目標(biāo)點(diǎn)運(yùn)動［17］。人工勢場法航跡規(guī)劃的二維示意圖如圖2所示。

圖2 人工勢場法航跡規(guī)劃的二維示意圖Fig.2 Two-dimensional diagram of artificial potential field method

引力勢場函數(shù)與斥力勢場函數(shù)可分別表示為

其中，X=（x，y，z）為飛行器的位置向量，α和β分別為引力與斥力增益系數(shù)，X、Xg、Xo分別表示當(dāng)前點(diǎn)、 目標(biāo)點(diǎn)、 障礙物的空間位置，ρ(X，Xg)、ρ(X，Xo)分別為當(dāng)前點(diǎn)到目標(biāo)點(diǎn)和障礙物的空間距離，ρ0為障礙物的斥力作用范圍。

對于二元及以上函數(shù)而言，某一點(diǎn)方向?qū)?shù)的方向可以有無數(shù)個。所有方向?qū)?shù)中存在一個最大值，其方向?yàn)樘荻鹊姆较?，方向?qū)?shù)最大值的大小為梯度的模。

對引力勢場函數(shù)和斥力勢場函數(shù)求負(fù)梯度，可得相應(yīng)引力函數(shù)與斥力函數(shù)

飛行器所受合力為

2.2 局部震蕩及其改進(jìn)

在利用人工勢場法進(jìn)行航跡規(guī)劃時，其規(guī)劃航跡有可能出現(xiàn)不必要的震蕩現(xiàn)象。主要原因是在某一航點(diǎn)處引力大于斥力，而在下一航點(diǎn)處斥力大于引力，兩種情況多次循環(huán)導(dǎo)致航跡來回震蕩。如圖3所示，由式（3）和式（4）可知，距離目標(biāo)點(diǎn)d越遠(yuǎn)的點(diǎn)所受引力越大，距離威脅點(diǎn)z越遠(yuǎn)的點(diǎn)所受斥力越小。在規(guī)劃平面內(nèi)，必然有1條斥力與引力大小相等（方向不一定相反）的平衡線L。當(dāng)前點(diǎn)mi所受到的引力大于斥力，則其必然向上前進(jìn)1個步長l，得到航點(diǎn)mi+1。此時，點(diǎn)mi+1可能越過平衡線L，使其受到的引力小于斥力。向遠(yuǎn)離威脅點(diǎn)方向規(guī)劃，得到航點(diǎn)mi+2。點(diǎn)mi+2將再次越過平衡線，以此類推，形成局部震蕩。

圖3 局部震蕩示意圖Fig.3 Diagram of local oscillation

針對感知與規(guī)避實(shí)際背景，可在航跡規(guī)劃時加入無人機(jī)最大轉(zhuǎn)彎角約束。由于無人機(jī)自身動力學(xué)性能的影響，無人機(jī)飛行方向的改變受到最大轉(zhuǎn)彎角（或最小轉(zhuǎn)彎半徑）的約束。設(shè)無人機(jī)最大轉(zhuǎn)彎角為θmax，最小轉(zhuǎn)彎半徑為Rmin，無人機(jī)飛行的轉(zhuǎn)彎角為α，轉(zhuǎn)彎半徑為r，要求規(guī)劃出的航跡中的每一點(diǎn)的曲率半徑都必須大于Rmin，則約束表達(dá)式為

其中，li為步長，最小轉(zhuǎn)彎半徑可由下式得出

其中，vmin為無人機(jī)的最小飛行速度，Nymax為無人機(jī)最大法向牽引力。

對于加入最大轉(zhuǎn)彎角約束的人工勢場法，當(dāng)規(guī)劃到圖3所示的斥力與引力的平衡線附近的點(diǎn)mi+1時，受最大轉(zhuǎn)彎角θmax的約束，其下一點(diǎn)不會做出類似圖中點(diǎn)mi+2的大機(jī)動，可有效減緩或消除局部震蕩影響，其改進(jìn)程度受最大轉(zhuǎn)彎角θmax大小的影響。

未進(jìn)行局部震蕩改進(jìn)的仿真結(jié)果如圖4所示。其中，起始點(diǎn)為A（0，0，0），目標(biāo)點(diǎn)為B（100，100，100），威脅點(diǎn)為C（50，50，50），規(guī)劃步長l=1，目標(biāo)點(diǎn)引力增益系數(shù)α=0.1，威脅點(diǎn)斥力增益系數(shù)β=500，威脅區(qū)勢場范圍P=35。加入最大轉(zhuǎn)彎角約束的仿真結(jié)果如圖5所示。其中，起始點(diǎn)為A′（0，0，0），目標(biāo)點(diǎn)為B′（100，100，100），障礙點(diǎn)為C′（50，50，50），規(guī)劃步長l=1，目標(biāo)點(diǎn)引力增益系數(shù)α=0.1，威脅點(diǎn)斥力增益系數(shù)β=500，威脅區(qū)勢場范圍P=35，最大轉(zhuǎn)彎角θmax=45°。

圖4 原算法的仿真結(jié)果Fig.4 Simulation results of original algorithm

圖5 改進(jìn)后的算法仿真結(jié)果Fig.5 Simulation results of improved algorithm

2.3 威脅區(qū)模型優(yōu)化

避讓航跡規(guī)劃過程中的威脅區(qū)域來源于航跡預(yù)測模塊得出的預(yù)測航跡。結(jié)合一般飛行器機(jī)動性能，得到威脅區(qū)的結(jié)構(gòu)如圖6所示，類似于椎體。

圖6 威脅區(qū)的結(jié)構(gòu)示意圖Fig.6 Diagram of threat area structure

在感知與規(guī)避實(shí)際背景下，在利用人工勢場法進(jìn)行航跡規(guī)劃的過程中，其威脅區(qū)勢場模型若采用傳統(tǒng)的以威脅區(qū)域中心為圓心構(gòu)建球體勢場的方法，容易出現(xiàn)因規(guī)劃空間過小而導(dǎo)致的規(guī)劃航跡與實(shí)際最優(yōu)解偏差過大的問題，進(jìn)而造成空域和能量資源的浪費(fèi)。

為此，本文采用將威脅區(qū)域結(jié)構(gòu)模型進(jìn)行分解的方法構(gòu)建威脅區(qū)勢場模型。將威脅區(qū)域Z非均勻離散為多個威脅點(diǎn)｛z0，z1，…，zmax｝，則越靠近航跡預(yù)測點(diǎn)的區(qū)域威脅點(diǎn)分布越密集。傳感器最終探測點(diǎn)在XY平面的投影為z0（xz0，yz0），Kalman濾波預(yù)測Δt時刻后航點(diǎn)在XY平面的投影為zk（xzk，yzk）。飛行器從最終探測點(diǎn)開始，以最大偏航角θ′max向Y軸正向飛行Δt時刻后在XY平面的投影為zm（xzm，yzm），則有

最大偏航點(diǎn)zm和最終探測點(diǎn)z0的連線與X軸的夾角為θm0，航跡預(yù)測點(diǎn)zk和最大偏航點(diǎn)zm的連線與X軸的夾角為θkm，則有

二維威脅點(diǎn)集z1的分布為

其中，p為密集因子，p越大則航跡預(yù)測點(diǎn)zk附近的威脅點(diǎn)越密集。

改進(jìn)后的威脅點(diǎn)分布如圖7所示。威脅區(qū)結(jié)構(gòu)類似于椎體結(jié)構(gòu)，圖中的各條線構(gòu)成了整體威脅區(qū)結(jié)構(gòu)的模型輪廓，椎體底面各線匯集于航跡預(yù)測模塊給出的預(yù)測點(diǎn)。作為入侵飛行器的高概率出現(xiàn)點(diǎn)，其周圍威脅點(diǎn)分布密度較高。在遠(yuǎn)離預(yù)測點(diǎn)的區(qū)域，威脅點(diǎn)分布密度越來越低，在錐頂處達(dá)到最低。

每個威脅點(diǎn)zi單獨(dú)形成斥力勢場Urepi與斥力Frepi，飛行器所受斥力合力為所有斥力的疊加，則有

圖7 威脅點(diǎn)分布仿真圖Fig.7 Simulation of threat points distribution

2.4 無人機(jī)性能約束模型

在利用人工勢場法進(jìn)行避讓航跡規(guī)劃的過程中，所規(guī)劃航跡必須滿足無人機(jī)自身性能的約束。除上述最大轉(zhuǎn)彎角約束之外，還有最大航程約束、飛行高度約束和最大爬升/俯沖角約束，具體模型如下。

（1）最大航程約束

由于攜帶的動力能源有限，無人機(jī)面臨著最大飛行距離的約束，即最大航程約束，這要求避讓航跡長度lavoid與其余段航跡長度lbefore的和必須小于無人機(jī)能夠飛行的最大航程值lmax。 設(shè)每2個航點(diǎn)之間的航跡段長度為li，航跡總長度的示意圖如圖8所示。

圖8 航跡總長度示意圖Fig.8 Schematic diagram of total track length

則無人機(jī)的航跡總長度ltotal應(yīng)滿足

（2）飛行高度約束

我國通用航空管理局對各類無人機(jī)的飛行活動制定了飛行高度限制，設(shè)無人機(jī)飛行高度的約束區(qū)間為［Hmin，Hmax］，則避讓航跡中各航點(diǎn)的高度h需滿足

（3）最大爬升/俯沖角約束

最大爬升/俯沖角是指無人機(jī)在三維空間中改變其飛行高度時可機(jī)動達(dá)到的最大角度，角度過大可能會引起失速。設(shè)最大爬升/俯沖角為γmax，無人機(jī)實(shí)際爬升/俯沖角為φ，當(dāng)前航跡節(jié)點(diǎn)的坐標(biāo) 為 (xi，yi，zi)，下 一 航 跡 節(jié) 點(diǎn) 的 坐 標(biāo) 為(xi+1，yi+1，zi+1)，則約束滿足下式

3 仿真驗(yàn)證

根據(jù)上述威脅區(qū)模型的優(yōu)化方法，對感知與規(guī)避過程中的避讓航跡規(guī)劃進(jìn)行仿真驗(yàn)證，并將其與改進(jìn)之前的算法進(jìn)行對比。其中，起始點(diǎn)、目標(biāo)點(diǎn)、威脅點(diǎn)、引力增益系數(shù)、斥力增益系數(shù)固定不變，僅對威脅區(qū)勢場范圍進(jìn)行修改，并比較兩種算法的仿真結(jié)果。

假設(shè)我方無人機(jī)于t時刻發(fā)現(xiàn)入侵飛行器，以此時我方無人機(jī)的位置作為原點(diǎn)建立直角坐標(biāo)系。 威脅區(qū)域中心點(diǎn)的坐標(biāo)為（xobs，yobs，zobs），威脅區(qū)勢場范圍為P1，以此構(gòu)造威脅區(qū)模型，進(jìn)行基于傳統(tǒng)人工勢場的避讓航跡規(guī)劃。入侵飛行器航跡預(yù)測點(diǎn)的坐標(biāo)為（xforecast，yforecast，zforecast），威脅點(diǎn)zi的自坐標(biāo)為（xobsi，yobsi，zobsi），威脅點(diǎn)的勢場范圍為P1，以此構(gòu)造改進(jìn)威脅區(qū)模型，并進(jìn)行改進(jìn)后的避讓航跡規(guī)劃。

為清晰觀察傳統(tǒng)算法的規(guī)劃結(jié)果是否成功繞開了威脅區(qū)域，需在傳統(tǒng)算法避讓航跡規(guī)劃的仿真結(jié)果中加入威脅點(diǎn)，以顯示威脅區(qū)域，且不將其作為障礙點(diǎn)代入計算。傳統(tǒng)避讓航跡規(guī)劃仿真參數(shù)的設(shè)置為：規(guī)劃步長l=1，目標(biāo)點(diǎn)引力增益系數(shù)α=0.1，威脅點(diǎn)斥力增益系數(shù)β=500，威脅區(qū)域中心（79，50，70）。當(dāng)威脅區(qū)勢場范圍P1=15時，得出的仿真結(jié)果如圖9、圖10所示。航跡從威脅區(qū)穿過，未成功規(guī)避威脅區(qū)域。

保持其他條件不變，擴(kuò)大威脅區(qū)勢場范圍，使規(guī)劃航點(diǎn)提前受斥力作用。當(dāng)威脅區(qū)勢場范圍P1=30時，得出的仿真結(jié)果如圖11、圖12所示。航跡從威脅區(qū)穿過，未成功規(guī)避威脅區(qū)域。

圖9 傳統(tǒng)算法的仿真結(jié)果（P1＝15）Fig.9 Simulation results of original algorithm （P1＝15）

圖10 傳統(tǒng)算法的仿真結(jié)果（圖9曲線右轉(zhuǎn)一定角度）Fig.10 Simulation results of original algorithm（curve in Fig.9 turns right at a certain angle）

圖11 傳統(tǒng)算法的仿真結(jié)果（P1＝30）Fig.11 Simulation results of original algorithm （P1＝30）

圖12 傳統(tǒng)算法的仿真結(jié)果（圖11曲線右轉(zhuǎn)一定角度）Fig.12 Simulation results of original algorithm（curve in Fig.11 turns right at a certain angle）

保持其他條件不變，再次擴(kuò)大威脅區(qū)勢場范圍，使規(guī)劃航點(diǎn)提前受斥力作用。當(dāng)威脅區(qū)勢場范圍P1=40時，得出的仿真結(jié)果如圖13、圖14所示。航跡未從威脅區(qū)穿過，成功規(guī)避威脅區(qū)域，此時總航點(diǎn)數(shù)為263。

改進(jìn)后的避讓航跡規(guī)劃仿真參數(shù)的設(shè)置為：規(guī)劃步長l=1，威脅區(qū)勢場范圍P2=15，目標(biāo)點(diǎn)引力增益系數(shù)α=0.1，威脅點(diǎn)斥力增益系數(shù)β=500。 得出的仿真結(jié)果如圖15、圖16所示，成功規(guī)避威脅區(qū)域，總航點(diǎn)數(shù)為219個。

圖13 傳統(tǒng)算法的仿真結(jié)果（P1＝40）Fig.13 Simulation results of original algorithm （P1＝40）

圖14 傳統(tǒng)算法的仿真結(jié)果（圖13曲線右轉(zhuǎn)一定角度）Fig.14 Simulation results of original algorithm（curve in Fig.13 turns right at a certain angle）

圖15 改進(jìn)算法的仿真結(jié)果Fig.15 Simulation results of improved algorithm

圖16 改進(jìn)算法的仿真結(jié)果（圖15曲線右轉(zhuǎn)一定角度）Fig.16 Simulation results of improved algorithm（curve in Fig.15 turns right at a certain angle）

4 結(jié)論

在相同規(guī)劃步長的條件下，避讓航跡總航點(diǎn)數(shù)可直接反映航跡總長度，影響無人機(jī)動力能源的消耗。由上文可知，在感知與規(guī)避實(shí)際背景下，本文提出的改進(jìn)人工勢場法成功避讓航跡的總航點(diǎn)數(shù)較傳統(tǒng)人工勢場法減少了16.7%。在節(jié)約了動力能源的同時，無人機(jī)的避讓飛行耗時也有所減少，提高了任務(wù)執(zhí)行效率。

雖然基于人工勢場法的航跡規(guī)劃實(shí)時性高，但本文采用的威脅區(qū)模型優(yōu)化的改進(jìn)人工勢場方法擴(kuò)大了威脅勢場的數(shù)量，增加了計算量，其實(shí)時性也會成為未來實(shí)物實(shí)驗(yàn)中的隱患，日后需在算法效率方面繼續(xù)改進(jìn)。