改進(jìn)A*算法的飛行器航跡快速規(guī)劃

馮文斌，楊易明，李濤濤

（200093上海市 上海理工大學(xué) 機械工程學(xué)院）

0 引言

路徑規(guī)劃作為移動機器人、無人機飛行等技術(shù)的重要研究內(nèi)容之一，目前已有許多算法應(yīng)用于不同實際場景。如A*算法有實時性強，搜索效率高等特點[1]；蟻群算法作為一種適用于并行計算的離散型算法，易與多種啟發(fā)式算法結(jié)合以改善性能[2]；遺傳算法全局搜索能力強，有良好的收斂性[3]。本文以智能飛行器為研究對象，數(shù)據(jù)來源于“華為杯”第16屆中國研究生數(shù)學(xué)建模競賽F題。該類飛行器因系統(tǒng)結(jié)構(gòu)限制，在飛行過程中會產(chǎn)生水平和垂直定位誤差，未經(jīng)過合適的校正點會導(dǎo)致任務(wù)失敗，并且該飛行器存在最小轉(zhuǎn)彎半徑，需進(jìn)行方向校正，因此，在復(fù)雜環(huán)境和多種約束條件下對飛行器的航跡進(jìn)行快速規(guī)劃有重要研究意義。本文采用改進(jìn)的A*算法，對啟發(fā)函數(shù)增設(shè)權(quán)值，改進(jìn)了算法的代價函數(shù)[4]，作為一種啟發(fā)式算法，計算速度快[5],并且理論上可以保證全局最優(yōu)解的收斂性[6]。

1 航跡約束

在三維空間中存在起點A、終點B以及若干水平校正點和垂直校正點(如圖1所示)，要求飛行器的航跡總長度盡可能小的同時，經(jīng)過的校正點數(shù)量盡可能少。飛行器初始水平誤差與垂直誤差均為0，每飛行1 m，兩類誤差均增加δ個單位。經(jīng)過水平/垂直校正點時，只能校正該類誤差為0，另一誤差不發(fā)生改變，同時需滿足以下條件才允許進(jìn)行誤差校正。

圖1 起點、終點及校正點坐標(biāo)Fig.1 Coordinates of starting point,ending point and correction point

允許飛行器進(jìn)行垂直誤差校正：Δv ≤Δα1，Δl ≤Δα2。

允許飛行器進(jìn)行水平誤差校正：Δv ≤Δβ1，Δl ≤Δβ2。

飛行器到達(dá)終點時：Δv ≤θ，Δl ≤θ。

飛行器的最大轉(zhuǎn)彎半徑為R。

其中：Δv——飛行器垂直誤差；Δl——飛行器水平誤差；α1=20，α2=10，β1=15，β2=20，θ=20，δ=0.001，R=200 m。

2 改進(jìn)A＊算法描述

A*搜索算法基本思想是設(shè)計合適的啟發(fā)式函數(shù)，以初始點作為父節(jié)點，從全局評價搜索可行域范圍內(nèi)節(jié)點的代價值，選取代價值最小的節(jié)點作為新的父節(jié)點，直至到達(dá)終點。節(jié)點代價評估函數(shù)通常可以表示為

式中：f（n）——起點經(jīng)過當(dāng)前節(jié)點n到終點的代價函數(shù)；g（n）——從起點到當(dāng)前節(jié)點n的實際代價，本文取為A點到當(dāng)前校正點經(jīng)過的路程；h（n）——從當(dāng)前節(jié)點n到終點的估計代價。

由于路徑規(guī)劃過程中，g（n）和h（n）對路徑評估的影響程度不同，設(shè)權(quán)值μ提高計算精度

式中：D（n）——飛行器當(dāng)前節(jié)點n到終點的歐氏距離：

因為飛行器航跡由若干段圓弧和直線組成，所以權(quán)值μ取值應(yīng)不小于1，選取合適的值能獲得更佳的航跡。

本文改進(jìn)的A*算法構(gòu)建了open表、close表與del表。open表用于記錄未被計算的節(jié)點，closed表用于記錄已經(jīng)被計算的節(jié)點，del表用于記錄待計算的節(jié)點。搜索過程中，首先從open表中找出在父節(jié)點可行域范圍內(nèi)的節(jié)點存入del表，從del表中選取代價評估函數(shù)值最小的節(jié)點作為新的父節(jié)點，并對3張表進(jìn)行修改，算法流程圖如圖2所示。

圖2 改進(jìn)A＊算法流程圖Fig.2 Improved A＊ algorithm flowchart

3 模型建立

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

采用A*算法對航跡進(jìn)行規(guī)劃。將起點A作為父節(jié)點，第一個校準(zhǔn)點為P0，飛行器初始水平誤差和垂直誤差均為0，需要對P0為水平校正點和垂直校正點2種情況下的航跡分別進(jìn)行計算。根據(jù)約束條件盡可能經(jīng)過少的校正點，本文規(guī)定前后兩個相鄰的校正點必定屬于不同類型。設(shè)變量γ，當(dāng)γ=0時，P0為水平校正點，當(dāng)γ=1時，P0為垂直校正點。

飛行器存在最小轉(zhuǎn)彎半徑R，通過父節(jié)點確定可行域節(jié)點范圍并加入del表時，需刪除飛行器無法航行到的節(jié)點,并通過計算父節(jié)點與子節(jié)點的方向向量進(jìn)一步求解圓弧的路徑。

飛行器航跡為直線AP0，PiPi+1，以及PnB由圓弧和直線組成的組合曲線，其目標(biāo)函數(shù)如下：

式中：A——起點；B——終點；P0——第1個校正點；N——所有校正點個數(shù)；Pi——第i+1個校正點；Mi——第i+1個飛行器轉(zhuǎn)彎航行結(jié)束點；λ——0代表下一個節(jié)點為水平誤差校正點，1代表下一個節(jié)點為垂直誤差校正點；Q——del表中節(jié)點。

3.2 轉(zhuǎn)彎航跡計算

飛行器轉(zhuǎn)彎航跡需要求解轉(zhuǎn)彎圓心Oi及轉(zhuǎn)彎航跡結(jié)束點Mi，如圖3所示。

圖3 飛行器轉(zhuǎn)彎航跡Fig.3 Aircraft's turning track

將該法向量與飛行器飛行方向向量叉乘，可獲得父節(jié)點Pi指向圓心Oi的方向向量

采用三維空間內(nèi)任一點繞定軸旋轉(zhuǎn)算法求解Mi坐標(biāo)，將通過點Oi的航跡平面法向量作為旋轉(zhuǎn)軸，點Pi繞該軸旋轉(zhuǎn)角度φ得到點Mi，經(jīng)過計算，總旋轉(zhuǎn)矩陣如式（8）：

其中：

點Mi坐標(biāo)由式（11）可得

3.3 飛行器航跡計算總體流程

由于飛行器航行約束條件較多，最終整體計算流程如圖4所示。

圖4 飛行器航跡計算流程圖Fig.4 Flight path flow chart of aircraft

4 飛行器航跡計算結(jié)果

經(jīng)計算測試，μ=1.0時，第1個校正點為垂直校正點時飛行器航跡較短；μ=1.3時，可獲得最優(yōu)航跡，航跡總長度最短且經(jīng)過校正點數(shù)目最少，此時，第1個校正點為水平校正點。參見表1。

最優(yōu)航跡如圖5、圖6所示。

表1 μ 取不同值時航跡對比Tab.1 Track comparison with different values ofμ

圖5 μ=1.3時飛行器航跡三維圖Fig.5 3D view of aircraft track atμ=1.3

圖6 μ=1.3時飛行器航跡平面圖Fig.6 Flight path plan of aircraft track atμ=1.3

5 結(jié)論

本文采用改進(jìn)的A*算法對多約束條件下的智能飛行器進(jìn)行了飛行航跡快速規(guī)劃。給出了目標(biāo)函數(shù)及約束條件表達(dá)式。討論了給定μ的值時，第1個校正點分別為水平校正點或垂直校正點的最優(yōu)解，并測試了當(dāng)μ=1.3時，飛行器的航跡最短且經(jīng)過的校正點比其余情況更少。考慮到飛行器存在最小轉(zhuǎn)彎半徑，對轉(zhuǎn)彎部分航跡進(jìn)行計算，并推導(dǎo)了繞定軸旋轉(zhuǎn)矩陣，進(jìn)一步求解轉(zhuǎn)彎結(jié)束點的坐標(biāo)。經(jīng)測試，改進(jìn)的A*算法能夠快速且精確地計算出飛行器的最佳航跡。