多約束條件下無人機(jī)航跡規(guī)劃

李昱奇，劉志乾，程凝怡，王瑩瑩，朱春麗

1.中國石油大學(xué)（北京）安全與海洋工程學(xué)院，北京 102249

2.中國石油大學(xué)（北京）機(jī)械與儲運(yùn)工程學(xué)院，北京 102249

3.中海油研究總院有限責(zé)任公司，北京 100027

無人機(jī)航跡規(guī)劃是全局優(yōu)化問題，在復(fù)雜的動態(tài)場環(huán)境下，基于各種約束條件，尋求最優(yōu)飛行路徑[1]。無人機(jī)只有依靠導(dǎo)航系統(tǒng)提供的信號，準(zhǔn)確地到達(dá)預(yù)定位置，才能保證任務(wù)順利完成[2-3]。目前遠(yuǎn)程長航時無人機(jī)配備的導(dǎo)航系統(tǒng)是以慣性導(dǎo)航系統(tǒng)為主，以衛(wèi)星導(dǎo)航、天文導(dǎo)航、多普勒導(dǎo)航等為輔助手段的組合導(dǎo)航系統(tǒng)[4]，但由于其精密程度高、結(jié)構(gòu)復(fù)雜、系統(tǒng)存在限制等原因，在飛行過程中會產(chǎn)生定位誤差，若單純依靠慣性導(dǎo)航，定位誤差會隨時間累積，當(dāng)誤差累積到一定程度將無法實(shí)現(xiàn)精確定位，影響任務(wù)執(zhí)行[5-7]。為此，無人機(jī)須前往相應(yīng)誤差校正點(diǎn)進(jìn)行誤差校正，由此引發(fā)的約束問題，稱為導(dǎo)航精度類約束；同理，若遇威脅因素，需要躲避而引發(fā)的約束稱為規(guī)避類約束。

大部分學(xué)者的研究點(diǎn)專注于如何提高導(dǎo)航精度[8-10]。如張楠[11]對高空長航時無人機(jī)組合導(dǎo)航系統(tǒng)的方案、模型和算法進(jìn)行了研究，并分析了方案的可行性，但并未與航跡規(guī)劃相結(jié)合。而針對航跡規(guī)劃問題，主要研究集中在規(guī)避類約束方面[12-17]。如2016 年，尹高揚(yáng)等人[18]采用改進(jìn)的快速擴(kuò)展隨機(jī)樹法解決三維空間下無人機(jī)航跡規(guī)劃問題，該算法在搜索最優(yōu)路徑的同時，可以有效躲避威脅和障礙。同年，Calik[19]針對雷達(dá)、山地等威脅障礙類約束，以無人機(jī)最小飛行時間為目標(biāo)函數(shù)，利用多智能體結(jié)構(gòu)蟻群算法（Ant Colony Optimization with a Multiagent Structure）求解無人機(jī)全局最優(yōu)路徑規(guī)劃問題，該算法可以快速有效規(guī)避障礙，并形成最優(yōu)路徑。但針對導(dǎo)航精度約束下航跡規(guī)劃問題研究較少，目前有2015 年劉世一等[20]基于慣性導(dǎo)航系統(tǒng)誤差模型和光學(xué)偵察載荷校正模型，分析了導(dǎo)航精度對長航時無人機(jī)航跡規(guī)劃的影響，但校正點(diǎn)設(shè)立較少，約束條件較為簡單。

綜上所述，以復(fù)雜類導(dǎo)航精度為約束的無人機(jī)航跡規(guī)劃問題研究尚淺。鑒于此，本文將基于Dijkstra 算法對導(dǎo)航精度約束下無人機(jī)航跡規(guī)劃問題進(jìn)行研究。

1 導(dǎo)航精度約束下無人機(jī)航跡規(guī)劃模型建立

無人機(jī)在飛行過程中會產(chǎn)生兩個方向的定位誤差——垂直誤差和水平誤差。假設(shè)無人機(jī)每飛行1 m，垂直誤差和水平誤差將各增加δ個單位，到達(dá)目的地時垂直誤差和水平誤差均應(yīng)小于θ個單位才能確保任務(wù)完成。

圖1 無人機(jī)航跡規(guī)劃區(qū)域示意圖

無人機(jī)飛行區(qū)域如圖1所示，A點(diǎn)為起點(diǎn)，B點(diǎn)為終點(diǎn)。在此飛行區(qū)域內(nèi)分布有N個誤差校正點(diǎn)（不包含A、B），藍(lán)點(diǎn)為垂直誤差校正點(diǎn)，只將垂直誤差校正為0，黃點(diǎn)為水平誤差校正點(diǎn)，只將水平誤差校正為0。當(dāng)無人機(jī)垂直誤差不大于α1個單位且水平誤差不大于α2個單位時，才能在垂直誤差校正點(diǎn)進(jìn)行垂直誤差校正。同理，當(dāng)無人機(jī)垂直誤差不大于β1個單位且水平誤差不大于β2個單位時，才能在水平誤差校正點(diǎn)進(jìn)行水平誤差校正。在滿足誤差要求前提下，通過規(guī)劃無人機(jī)航跡，使航跡長度最短且經(jīng)過的校正點(diǎn)數(shù)量最少。

1.1 決策變量

為保證無人機(jī)航跡的實(shí)時位置信息，將所有校正點(diǎn)是否執(zhí)行定位誤差校正功能Pb,i,j作為決策變量，其中b表示誤差校正點(diǎn)編號，為1,2,…,N-1,N；i表示實(shí)際航跡中行駛的第i個誤差校正點(diǎn)；j表示誤差校正點(diǎn)類型判斷標(biāo)志，j=1 為垂直校正點(diǎn)，j=0 為水平誤差校正點(diǎn)。例如b=125,i=3,j=1 時，P125,3,1表示編號125的點(diǎn)為垂直誤差校正點(diǎn)，且在所有航跡校正點(diǎn)中為第3個校正點(diǎn)。Pb,i,j=1 代表編號為b的校正點(diǎn)執(zhí)行誤差校正，Pb,i,j=0 代表不執(zhí)行誤差校正。

1.2 目標(biāo)函數(shù)

為保證無人機(jī)順利完成任務(wù)，飛行過程中應(yīng)綜合考慮兩個優(yōu)化目標(biāo)：

（1）無人機(jī)從起點(diǎn)A到終點(diǎn)B的航跡長度盡可能??；

（2）經(jīng)過校正區(qū)域進(jìn)行校正的次數(shù)盡可能小。

因此目標(biāo)函數(shù)可表示為：

式（1）中：n表示無人機(jī)從起點(diǎn)A出發(fā)經(jīng)過的所有校正點(diǎn)數(shù)量；L表示無人機(jī)航跡長度，表示第i-1 個校正點(diǎn)和第i個校正點(diǎn)之間的距離。

1.3 約束條件

（1）誤差增長約束

無人機(jī)在空間飛行時由系統(tǒng)限制所產(chǎn)生的垂直誤差和水平誤差只與飛行距離線性相關(guān)，無人機(jī)每飛行1 m，垂直誤差和水平誤差將各增加δ個單位。因?yàn)檎`差校正會影響無人機(jī)在之前飛行過程中所積累的誤差，所以取無人機(jī)在兩個相鄰校正點(diǎn)之間所產(chǎn)生的垂直誤差ei,v和水平誤差ei,h作為最小誤差計(jì)量單位，則定位誤差與兩個校正點(diǎn)之間距離di的關(guān)系如式（2）所示。

式（2）中：ei,v為第i-1 個誤差校正點(diǎn)與第i個誤差校正點(diǎn)之間的垂直誤差；ei,h為第i-1 個誤差校正點(diǎn)與第i個誤差校正點(diǎn)之間的水平誤差。

（2）終點(diǎn)誤差約束

為保證飛行任務(wù)的順利完成，到達(dá)終點(diǎn)時定位誤差應(yīng)小于θ個單位，因此，無人機(jī)所產(chǎn)生的垂直定位誤差和水平定位誤差應(yīng)滿足式（3）約束。

式（3）中：Ei,v表示無人機(jī)從起點(diǎn)出發(fā)經(jīng)過第i個校正點(diǎn)時所累積的總垂直誤差，EB,v表示終點(diǎn)B處的垂直誤差；Ei,h表示無人機(jī)從起點(diǎn)出發(fā)經(jīng)過第i個校正點(diǎn)時所累積的總水平誤差，EB,h表示終點(diǎn)B處的水平誤差。

（3）起點(diǎn)誤差約束

在起點(diǎn)A處，無人機(jī)的垂直和水平誤差均為0，則該約束可表示為：

（4）誤差校正點(diǎn)功能約束

當(dāng)無人機(jī)經(jīng)過垂直校正點(diǎn)并且執(zhí)行誤差校正時，其垂直誤差將變?yōu)?，水平誤差保持不變，該約束可表示為：

同理，當(dāng)無人機(jī)經(jīng)過水平校正點(diǎn)并且執(zhí)行誤差校正時，其水平誤差將變?yōu)?，垂直誤差保持不變，該約束可表示為：

（5）誤差校正點(diǎn)執(zhí)行條件約束

無人機(jī)的垂直誤差不大于α1個單位，水平誤差不大于α2個單位時才能進(jìn)行垂直誤差校正；無人機(jī)的垂直誤差不大于β1個單位，水平誤差不大于β2個單位時才能進(jìn)行水平誤差校正。若使路徑中第k個校正點(diǎn)能夠執(zhí)行垂直誤差校正功能，即Pb,k,1=1，需要滿足式（7）所示的約束條件。

若使路徑中第k個校正點(diǎn)能夠執(zhí)行水平誤差校正功能，即Pb,k,0=1，需要滿足式（8）所示的約束條件。

2 基于Dijkstra算法的航跡規(guī)劃法

2.1 傳統(tǒng)Dijkstra算法

Dijkstra 算法[21]作為路徑規(guī)劃經(jīng)典算法可從全局出發(fā)尋找最優(yōu)解，較高的搜索精度為其優(yōu)勢所在，其搜索過程如下：

（1）初始化，令d(v1)=0,d(vj)=w1j(j=2,3,…,n),S={v1}，R=VS={v2,v3,…,vp}。

（2）在R中尋找一個頂點(diǎn)vk，使得：

置S=S∪{vk},R=VS。若(vk,vp)∈E，即點(diǎn)vk與vp間距wkp滿足終點(diǎn)處的誤差約束，則算法終止，否則轉(zhuǎn)（3）。

（3）修正d(vj)，對R中每個vj，令d(vj)=min{d(vj),d(vk)+wkj},轉(zhuǎn)（2）。

但傳統(tǒng)算法將所有可能路徑都加入其中，計(jì)算量較大，效率低。為此通過校正策略優(yōu)選、校正方案優(yōu)選和O-D鄰接矩陣處理方式，簡化搜索路徑，降低計(jì)算量，提高搜索效率。

2.2 校正策略優(yōu)選

無人機(jī)在某一時刻到達(dá)特定編號的校正點(diǎn)Pk時，根據(jù)校正點(diǎn)功能只能進(jìn)行垂直誤差校正或水平誤差校正。因此在整個航跡規(guī)劃過程中會產(chǎn)生兩種誤差校正策略——完全交替校正和不完全交替校正。如圖2 所示，完全交替校正表示任意相鄰兩次誤差校正均不同，即垂直誤差校正與水平誤差校正交替進(jìn)行（以圖中實(shí)線表示，Pk與Pk+1為不同類型校正點(diǎn)）；不完全交替校正表示存在相鄰兩次誤差校正之間為同類型的誤差校正情況（以圖中虛線表示，Pk與為相同類型校正點(diǎn)）。

圖2 誤差校正策略示意圖

針對上述兩種誤差校正策略，分別計(jì)算并比較其校正效率，進(jìn)而確定模型求解過程中兩種策略的優(yōu)先選擇級，兩種校正策略的航跡長度具體計(jì)算步驟如下。

（1）完全交替校正策略

設(shè)定第k-1、k、k+1 點(diǎn)為相鄰3 個誤差校正點(diǎn)，分別記作Pk-1、Pk和Pk+1，且3 點(diǎn)的誤差校正類型依次為垂直誤差校正、水平誤差校正和垂直誤差校正。

①設(shè)定無人機(jī)在第k-1 個校正點(diǎn)Pk-1進(jìn)行垂直誤差校正之后的誤差為：

②無人機(jī)在第k個校正點(diǎn)Pk進(jìn)行誤差校正之前的垂直誤差和水平誤差分別為：

③無人機(jī)在第k個校正點(diǎn)Pk進(jìn)行誤差校正之后的垂直誤差和水平誤差分別為：

④無人機(jī)在第k+1 個校正點(diǎn)Pk+1進(jìn)行誤差校正之前的垂直誤差和水平誤差分別為：

⑤無人機(jī)在第k+1 個校正點(diǎn)Pk+1進(jìn)行誤差校正之后的垂直誤差和水平誤差分別為：

聯(lián)立式（9）～式（13）可求解無人機(jī)由校正點(diǎn)Pk-1到校正點(diǎn)Pk+1之間的最大航跡距離為max(dk+dk+1)。

（2）不完全交替校正策略

如圖2所示，設(shè)定第k-1、k、k+1 點(diǎn)為相鄰3個誤差校正點(diǎn)，分別記作Pk-1、Pk和P′k+1，且3點(diǎn)的誤差校正類型依次為垂直誤差校正、水平誤差校正和水平誤差校正。無人機(jī)在第k-1 個校正點(diǎn)Pk-1進(jìn)行垂直誤差校正之后的誤差與完全交替校正策略的誤差相同。

根據(jù)假設(shè)條件推理可知，無人機(jī)在第k+1 個校正點(diǎn)P′k+1進(jìn)行誤差校正之前的誤差變化和航跡距離約束方程可由式（9）～式（11）表示。

由于垂直誤差校正點(diǎn)與水平誤差校正點(diǎn)執(zhí)行校正功能的誤差約束范圍ε（包括誤差約束α1、α2、β1、β2）不同，即無人機(jī)航跡距離變量d′k+1的約束條件發(fā)生變化，故無人機(jī)在第k+1 個校正點(diǎn)P′k+1進(jìn)行誤差校正之前的垂直誤差和水平誤差分別為：

無人機(jī)在第k+1 個校正點(diǎn)P′k+1進(jìn)行誤差校正之后的垂直誤差和水平誤差分別為：

聯(lián)立可求解無人機(jī)由校正點(diǎn)Pk-1到校正點(diǎn)P′k+1之間的最大航跡距離為max(d′k+d′k+1)。

（3）最優(yōu)校正策略

對比兩種誤差校正策略可知，無人機(jī)飛行最大航跡距離越大，則越快達(dá)到終點(diǎn)，且經(jīng)過校正點(diǎn)較少，即若max(dk+dk+1)＜max(d′k+d′k+1)則采取完全交替校正，反之采取不完全交替校正。若max(dk+dk+1)=max(d′k+d′k+1)，可以看出，當(dāng)無人機(jī)飛行距離相同時，完全交替校正策略所用的校正次數(shù)最少，此時選用完全交替校正策略。

2.3 校正方案優(yōu)選

基于完全交替校正策略，存在兩種誤差校正順序：（1）無人機(jī)從起點(diǎn)A出發(fā)后先進(jìn)行垂直誤差校正，繼而與水平誤差校正交替進(jìn)行（VHV校正）；（2）無人機(jī)從起點(diǎn)A出發(fā)后先進(jìn)行水平校正，繼而與垂直誤差校正交替進(jìn)行（HVH校正）。

根據(jù)誤差約束范圍ε中α1、α2、β1、β2及δ這 5 個誤差校正參數(shù)的限制，可得到每步誤差校正時的距離約束，并將其與每一個誤差校正點(diǎn)處的誤差進(jìn)行比較，進(jìn)而將誤差約束轉(zhuǎn)化成對距離的約束。

當(dāng)無人機(jī)采用VHV 校正時，無人機(jī)從A點(diǎn)出發(fā)，起始為A-V 校正，過程為VHV 校正循環(huán)，最終到達(dá)B點(diǎn)，總體循環(huán)模式可借助向量表達(dá)，如式（16）所示：

VHV 校正循環(huán)中誤差變化滿足上述約束，聯(lián)立式（2）、式（4）～式（8），得到執(zhí)行VHV循環(huán)中各階段距離約束dA-V、dV-H、dH-V和累計(jì)距離約束dVHV，其中dVHV=dV-H+dH-V。同理，在無人機(jī)采用HVH校正時，聯(lián)立上式得到執(zhí)行HVH循環(huán)中各階段距離約束dA-H、d′H-V、d′V-H和累計(jì)距離約束dHVH，其中dHVH=d′H-V+d′V-H。

通過比較dVHV與dHVH大小，當(dāng)dVHV＞dHVH時，采用VHV校正策略，當(dāng)dVHV＜dHVH時，采用HVH校正策略，當(dāng)dVHV=dHVH時，根據(jù)dA-V與dA-H數(shù)值大小，確定校正策略。

2.4 O-D距離鄰接矩陣及其化簡

根據(jù)校正點(diǎn)(x,y,z)坐標(biāo)和兩種誤差校正點(diǎn)類型，建立所有校正點(diǎn)間初始的O-D距離鄰接矩陣[22]，命名為O0。在此基礎(chǔ)上，根據(jù)校正策略和校正方案對矩陣O0進(jìn)行化簡，實(shí)現(xiàn)流程如下：

（1）將所有校正點(diǎn)信息轉(zhuǎn)化為初始O-D鄰接距離矩陣O0，容易得到O0={dmn}，且m,n∈[0,N-1]，m,n∈[0,N+1]，則dmn表示無人機(jī)由第m個校正點(diǎn)飛到第n個的校正點(diǎn)所走過的直線距離。

（2）基于校正策略優(yōu)選，若選擇完全交替校正策略，則對矩陣O0中任一元素dmn，若jm=jn，則令dmn的值為無窮大，以dmn=inf 表示。若選擇不完全交替校正策略，則此步驟可忽略，執(zhí)行（3）。

（3）基于校正方案優(yōu)選，若選擇VHV 校正，則需要將矩陣O0中第一行數(shù)據(jù)中大于dA-V的值改為inf，對矩陣O0其余各行數(shù)據(jù)中任一元素dmn，若jm=1，且dmn ＞dV-H,則dmn=inf；若jm=0,且dmn ＞dH-V,則dmn=inf ；若選擇HVH校正，則需要將矩陣O0中第一行數(shù)據(jù)中大于dA-H的值改為inf，對矩陣O0其余各行數(shù)據(jù)中任一元素dmn，若jm=1，且dmn ＞d′V-H，則dmn=inf；若jm=0 ，且dmn ＞d′H-V，則dmn=inf。

通過上述步驟簡化搜索路徑，并將約束條件融入傳統(tǒng)算法，最終得到基于Dijkstra算法的航跡規(guī)劃法。

3 仿真驗(yàn)證

基于上述模型和算法，設(shè)置仿真參數(shù)，α1=25 ，α2=15，β1=20，β2=25，θ=30，δ=0.001，出發(fā)點(diǎn)、終點(diǎn)及校正點(diǎn)坐標(biāo)可根據(jù)Matlab 中隨機(jī)函數(shù)創(chuàng)建得到。同時，為得到可行解，校正點(diǎn)數(shù)量和三維空間大小應(yīng)根據(jù)仿真參數(shù)α1、α2、β1、β2、θ和δ的取值進(jìn)行合理分配。此處列出某次求解過程中對應(yīng)的參數(shù)值：起點(diǎn)坐標(biāo)A（0，50 000，5 000），終點(diǎn)坐標(biāo)B（10 000，59 652.34，5 022），單位m，校正點(diǎn)數(shù)量共611個，部分參數(shù)如表1所示。

表1 空中點(diǎn)跡坐標(biāo)位置關(guān)系圖

結(jié)合仿真參數(shù)，利用Matlab 編程實(shí)現(xiàn)上述算法，最終確定校正策略為完全交替校正，校正方案為VHV 校正，無人機(jī)航跡點(diǎn)為：

飛行軌跡三維視圖如圖3所示，圖中黃色點(diǎn)為水平誤差校正點(diǎn)，藍(lán)色點(diǎn)為垂直誤差校正點(diǎn)，無人機(jī)從起點(diǎn)出發(fā)經(jīng)過的誤差校正點(diǎn)編號及校正前后誤差如表2所示。

圖3 航跡規(guī)劃三維路線圖

表2 誤差校正點(diǎn)編號及校正前后誤差

無人機(jī)經(jīng)過校正點(diǎn)數(shù)目為10，所走路線長度為105 581.895 m。如表2所示，每一數(shù)據(jù)點(diǎn)處的誤差都滿足約束要求，這表明，基于Dijkstra算法的航跡規(guī)劃法在解決導(dǎo)航精度類約束下無人機(jī)航跡規(guī)劃問題方面具備正確性和有效性。

同時，為驗(yàn)證算法優(yōu)越性[23]，將上述結(jié)果與傳統(tǒng)Dijkstra算法和遺傳算法所得結(jié)果分別進(jìn)行對比。仿真實(shí)驗(yàn)涉及校正點(diǎn)數(shù)量611 個，數(shù)據(jù)量較大，遺傳算法容易陷入局部最優(yōu)[24]，為此，針對上述參數(shù)共進(jìn)行10 次仿真，取其中最優(yōu)結(jié)果。算法性能比較如表3 所示，可以看出各數(shù)據(jù)點(diǎn)誤差均滿足要求，三種算法皆具備正確性和有效性。傳統(tǒng)Dijkstra 算法的校正點(diǎn)數(shù)量、總路線長度和算法耗時均優(yōu)于遺傳算法結(jié)果，表明Dijkstra 算法更適用于解決導(dǎo)航精度約束問題，在計(jì)算精度與傳統(tǒng)算法僅相差691.343 m，即0.66%情況下，改進(jìn)的算法能夠利用更少的時間得到最優(yōu)解，在解決導(dǎo)航精度約束下無人機(jī)航跡規(guī)劃問題方面具備一定的先進(jìn)性。

表3 算法性能比較

4 結(jié)束語

以多約束下無人機(jī)航跡規(guī)劃問題為切入點(diǎn)，重點(diǎn)分析了導(dǎo)航精度約束下無人機(jī)航跡規(guī)劃問題。將誤差校正這一概念按照導(dǎo)航精度約束處理，建立導(dǎo)航精度約束下無人機(jī)航跡規(guī)劃模型，并設(shè)計(jì)基于Dijkstra 算法的航跡規(guī)劃法求解該模型。最后通過仿真實(shí)驗(yàn)驗(yàn)證該算法的正確性和有效性，并與傳統(tǒng)算法和遺傳算法進(jìn)行對比來驗(yàn)證算法先進(jìn)性，結(jié)果表明無人機(jī)在實(shí)際航行中可以完成導(dǎo)航精度約束問題，能夠快速實(shí)現(xiàn)最短航跡規(guī)劃。同時，校正點(diǎn)為隨機(jī)選取，路徑規(guī)劃可隨校正點(diǎn)位置信息改變而發(fā)生變化，算法適應(yīng)性較強(qiáng)。但研究中忽略了無人機(jī)轉(zhuǎn)彎半徑問題，存在一定誤差，鑒于此，可嘗試通過曲線方程修正模型，并根據(jù)轉(zhuǎn)彎半徑完善算法，從而使研究內(nèi)容更貼合實(shí)際。