多約束條件下的無人機(jī)軌跡快速規(guī)劃

吳成振，孫大運(yùn)

（1.200093 上海市 上海理工大學(xué) 機(jī)械工程學(xué)院；2.200093 上海市 上海理工大學(xué) 醫(yī)療器械與食品學(xué)院）

0 引言

航空科學(xué)和現(xiàn)代信息技術(shù)的發(fā)展為無人機(jī)在現(xiàn)代戰(zhàn)爭(zhēng)中扮演重要角色提供了重要基礎(chǔ)，而航路規(guī)劃是實(shí)現(xiàn)無人機(jī)自主作戰(zhàn)高效能的關(guān)鍵步驟，備受國(guó)內(nèi)外學(xué)者的重視[1]。目前，無人機(jī)在民用領(lǐng)域的應(yīng)用日漸廣泛，比如無人機(jī)能夠在搶險(xiǎn)救災(zāi)、環(huán)境監(jiān)測(cè)、精準(zhǔn)農(nóng)業(yè)生產(chǎn)、航空攝影等領(lǐng)域發(fā)揮巨大作用[2]。由于實(shí)際應(yīng)用中復(fù)雜的飛行環(huán)境和約束條件，所以建立合理的規(guī)劃模型和高效規(guī)劃算法是無人機(jī)航跡快速規(guī)劃的一個(gè)重要內(nèi)容，其核心內(nèi)容為在滿足各項(xiàng)約束前提下，快速規(guī)劃出無人機(jī)在目標(biāo)任務(wù)下的最優(yōu)航跡[3]。飛行航路規(guī)劃的常見的約束條件有規(guī)劃中的導(dǎo)航精度約束、威脅約束、飛行器物理性能約束等，其中的部分約束條件還是相互耦合的，因而多約束問題是航路規(guī)劃的一個(gè)至關(guān)重要的問題[4]。由于無人機(jī)物理性能的限制，其定位系統(tǒng)會(huì)因飛行距離產(chǎn)生定位誤差，如果定位誤差累計(jì)超過系統(tǒng)允許的最大范圍就會(huì)導(dǎo)致任務(wù)失敗，所以，校正無人機(jī)的飛行過程產(chǎn)生的定位誤差是無人機(jī)航跡規(guī)劃中的一項(xiàng)重要任務(wù)。此外，飛行器在轉(zhuǎn)彎過程中受到自身結(jié)構(gòu)限制，其轉(zhuǎn)彎過程必須存在合理的轉(zhuǎn)彎半徑，所以，在三維航跡規(guī)劃過程中，空間圓弧軌跡的處理也是一大重點(diǎn)。

貪心算法是求解過程相對(duì)簡(jiǎn)單的算法，其思想是從待求解問題的初始狀態(tài)出發(fā)，由當(dāng)前每一步的局部最優(yōu)決策得到該算法下的全局最優(yōu)解[5]。雖然貪心算法不一定得到所有問題的整體最優(yōu)解，但它能得到大部分問題的整體最優(yōu)解或者近似最優(yōu)解[6]。因此，本文通過構(gòu)造無人機(jī)在定位誤差約束下的數(shù)學(xué)模型，結(jié)合航跡點(diǎn)和轉(zhuǎn)彎半徑推導(dǎo)出空間圓弧軌跡方程，利用貪心算法對(duì)無人機(jī)的航跡進(jìn)行快速規(guī)劃，結(jié)合軟件仿真得到飛行過程中的最優(yōu)航跡方案。

1 多約束條件

無人機(jī)在飛行過程中，其由系統(tǒng)結(jié)構(gòu)限制原因產(chǎn)生的定位誤差會(huì)隨飛行軌跡越來越大，因此在其飛行區(qū)域必須設(shè)置校正點(diǎn)，而校正點(diǎn)需在一定誤差范圍內(nèi)才能成功校正誤差。校正點(diǎn)分為垂直和水平兩種，垂直校正點(diǎn)和水平校正點(diǎn)可校正的誤差允許范圍也不相同，且達(dá)到終點(diǎn)時(shí)也需要滿足誤差范圍，具體的約束條件如下：

（1）無人機(jī)從起始點(diǎn)到達(dá)終點(diǎn)的航跡長(zhǎng)度盡可能短，經(jīng)過校正點(diǎn)處的要校正次數(shù)盡可能少；

（2）無人機(jī)在空間飛行過程中的定位誤差有垂直誤差和水平誤差，誤差增長(zhǎng)量均為δ個(gè)單位每米，假設(shè)當(dāng)兩種誤差均小于θ個(gè)單位時(shí)，無人機(jī)仍按照預(yù)定路徑正常飛行；

（3）無人機(jī)在出發(fā)點(diǎn)處不存在定位誤差，其飛行過程途經(jīng)垂直誤差校正點(diǎn)可將垂直誤差清零，水平誤差不變，途經(jīng)水平誤差校正點(diǎn)可將水平誤差清零，垂直誤差不變；

（4）在垂直誤差校正點(diǎn)能夠進(jìn)行校正的條件為無人機(jī)的垂直誤差不超過α1個(gè)單位且水平誤差不超過α2個(gè)單位。在水平誤差校正點(diǎn)能夠進(jìn)行校正的條件為無人機(jī)的垂直誤差不超過β1個(gè)單位且水平誤差不超過β2個(gè)單位；

（5）無人機(jī)由于自身結(jié)構(gòu)限制，其最小轉(zhuǎn)彎半徑為rmin。

2 航跡規(guī)劃模型

因?yàn)樵撃Ｐ鸵鬅o人機(jī)的航跡盡可能短，且通過校正點(diǎn)進(jìn)行校正次數(shù)盡可能少，因此該問題屬于多約束條件下的最短路徑求解模型[7]。

2.1 模型建立

假設(shè)無人機(jī)經(jīng)過的兩個(gè)連續(xù)校正點(diǎn)Pi和Pj之間的軌跡長(zhǎng)度Dij，集合E 的元素為無人機(jī)經(jīng)過的所有校正點(diǎn)P（ii=0，1，…，n），決策變量為xij。當(dāng)xij=1 時(shí)，表示航跡連續(xù)經(jīng)過校正點(diǎn)Pi和Pj，所有決策變量構(gòu)成的集合為C，將進(jìn)行校正的點(diǎn)Vi中的垂直校正點(diǎn)構(gòu)成集合T，水平校正點(diǎn)構(gòu)成集合H,并設(shè)到達(dá)校正點(diǎn)Pi時(shí)的累計(jì)垂直誤差為Terr（Pi），累計(jì)水平誤差為Herr（Pi），ci=1 表示該校正點(diǎn)進(jìn)行校正，ci=0 表示經(jīng)過校正點(diǎn)但不需要進(jìn)行校正。

2.1.1 建立目標(biāo)函數(shù)

根據(jù)無人機(jī)航跡軌跡盡可能短和經(jīng)過校正區(qū)域進(jìn)行校正次數(shù)盡可能少的要求，建立目標(biāo)函數(shù)如下：

2.1.2 確定約束條件

約束條件1：飛行器到達(dá)終點(diǎn)的定位誤差滿足Terr（Pn）＜θ，Herr（Pn）＜θ；

約束條件2：飛行器在水平校正點(diǎn)能進(jìn)行校正需要滿足Terr（Hi）＜β1，Herr（Hi）＜β2。

約束條件3：飛行器在垂直校正點(diǎn)能進(jìn)行校正需要滿足Terr（Ti）＜α1，Herr（Ti）＜α2。

綜上所述，可建立飛行器在本問題約束條件下飛行器航跡快速規(guī)劃的數(shù)學(xué)模型為

2.1.3 空間圓弧軌跡求解

由于無人機(jī)存在不小于rmin的轉(zhuǎn)彎半徑，如果無人機(jī)連續(xù)經(jīng)過空間的3 個(gè)校正點(diǎn)不共線，則該段軌跡必定存在空間圓弧，則計(jì)算該3 點(diǎn)形成軌跡產(chǎn)生的飛行誤差以及繪制飛行器軌跡時(shí)，就必須求得該段軌跡的空間圓弧的方程?？臻g曲線方程復(fù)雜，為避免空間幾何求解的復(fù)雜性，本文利用三點(diǎn)共面的性質(zhì)，先將空間中每一段圓弧軌跡的求解轉(zhuǎn)換為平面幾何問題。如圖1 所示，假設(shè)無人機(jī)連續(xù)經(jīng)過的3 個(gè)已知點(diǎn)為Pk，Pk+1（作為圓弧軌跡的切入點(diǎn)坐標(biāo)），Pk+2，無人機(jī)選擇的轉(zhuǎn)彎半徑為rk，以Pk為坐標(biāo)原點(diǎn)，PkPk+2為X軸建立相應(yīng)的局部平面坐標(biāo)系，很顯然，的長(zhǎng)度仍為其空間上的幾何直線長(zhǎng)度。

圖1 航跡平面圖Fig.1 Track plan of UAV

其中，D 可由任意一點(diǎn)已知坐標(biāo)所決定。

根據(jù)空間距離公式可得

因?yàn)槭剑?）是根據(jù)實(shí)際幾何圖形建立的，很顯然此方程必有解，由一元二次方程求根公式可得式（5）的解為

運(yùn)用同樣的方法容易解得圓心Ok+1的空間坐標(biāo)。因此，可求得無人機(jī)飛行軌跡存在轉(zhuǎn)彎半徑時(shí)的航跡長(zhǎng)度以及每段圓弧軌跡的圓心坐標(biāo)和圓弧軌跡的切出點(diǎn)坐標(biāo)。

2.2 基于貪心算法的求解

貪心算法的思想是，在求解問題的過程中，對(duì)每一步都做出當(dāng)前最好的判斷。本文航跡求解思想如圖2 所示。

*式（5）中：

圖2 基于貪心算法的航跡求解思想Fig.2 Model of track solving based on greedy algorithm

第1 步：在起始點(diǎn) P0的可選范圍內(nèi)隨機(jī)選擇一個(gè)點(diǎn)P1。

第2 步：以P1為圓心，結(jié)合上述的空間圓弧軌跡模型，以滿足模型約束條件的最大距離作為搜索半徑，選擇一個(gè)距離終點(diǎn) B 最近的點(diǎn)P2作為下一步要走的點(diǎn)。

第3 步：判斷是否到達(dá)終點(diǎn)，如果沒有到達(dá)終點(diǎn)，計(jì)算P1和P2之間的轉(zhuǎn)彎半徑，使P1和P2兩點(diǎn)間的曲線長(zhǎng)度最小，計(jì)算該段曲線中的圓弧軌跡的切出點(diǎn)T，令P0=T，P1=P2，轉(zhuǎn)第2 步；否則，轉(zhuǎn)第4 步。

第4 步：停止。

3 仿真實(shí)例及結(jié)果分析

因?yàn)楸締栴}的航線軌跡是空間曲線圖而不是折線圖，為了便于軌跡的繪制，需要得到每段空間圓弧的參數(shù)方程。每段圓弧的圓心坐標(biāo)Ok+1（Ox，Oy，Oz），圓弧半徑r，以及圓弧所在平面的法向量nk+1=（A，B，C）可由上文得知，則圓弧的參數(shù)方程可以表示為

本文以“華為杯”第十六屆中國(guó)研究生數(shù)學(xué)建模競(jìng)賽F 題中的附件1 校正點(diǎn)數(shù)據(jù)作為仿真數(shù)據(jù)，無人機(jī)最小轉(zhuǎn)彎半徑為300 m，附件1 數(shù)據(jù)的約束參數(shù)為：α1=25，α2=15，β1=20，β2=25，θ=30，δ=0.001。

基于上述模型與算法，在MATLAB 中編寫程序[8]得到的無人機(jī)飛行的校正點(diǎn)的數(shù)據(jù)仿真結(jié)果如表1 所示，且求得每段圓弧軌跡的轉(zhuǎn)彎半徑均為300 m，最優(yōu)航跡為A-200-509-28-183-194-450-594-397-B。

表1 航跡規(guī)劃結(jié)果Tab.1 Results of UAV trajectory fast planning

圖3 為求得的航跡圖。因?yàn)楦鲌A弧段半徑相對(duì)于兩航跡點(diǎn)間的距離非常小，在整體航跡圖上基本看不出圓弧軌跡，本文采用局部放大示意圖。

圖3 航跡圖及局部放大圖Fig.3 Map of UAV trajectory fast planning

礙于圖中視角以及距離比過大原因，本文求出的所有圓弧軌跡的圓心坐標(biāo)以及求得的切出點(diǎn)坐標(biāo)如表2 所示。

表2 圓弧軌跡圓心和切出點(diǎn)坐標(biāo)Tab.2 Center and cut-out point coordinates of arc trajectory

4 結(jié)語(yǔ)

本文建立了無人機(jī)關(guān)于定位誤差的多約束條件下的飛行模型，通過推導(dǎo)空間曲線航跡的數(shù)學(xué)方程，解決了空間圓弧的繪制問題，利用貪心算法快速規(guī)劃出最優(yōu)航跡。此外，基于此模型和算法，還可應(yīng)用于倉(cāng)儲(chǔ)物流路徑規(guī)劃領(lǐng)域以及車輛無人駕駛目標(biāo)路徑規(guī)劃中。