王宏倫 ，吳健發(fā) ，3，姚 鵬

（1.北京航空航天大學(xué)自動(dòng)化科學(xué)與電氣工程學(xué)院，北京 100191；2.北京航空航天大學(xué)飛行器控制一體化技術(shù)重點(diǎn)實(shí)驗(yàn)室，北京 100191；3.北京航空航天大學(xué)高等理工學(xué)院，北京 100191；4.中國(guó)海洋大學(xué)工程學(xué)院，青島 266100）

1 引言

航路規(guī)劃技術(shù)是反映無(wú)人機(jī)自主控制能力等級(jí)的關(guān)鍵技術(shù)之一。多年來(lái)，國(guó)內(nèi)外學(xué)者對(duì)航路規(guī)劃問(wèn)題展開了廣泛研究，取得了大量研究成果并已應(yīng)用于實(shí)際無(wú)人機(jī)平臺(tái)[1-3]，這些航路規(guī)劃方法主要以二維水平面規(guī)劃為主。但隨著無(wú)人機(jī)所執(zhí)行的任務(wù)逐漸復(fù)雜化，例如低空突防、編隊(duì)飛行、協(xié)同攻擊等，其使用空域從中、高空不斷向低空、甚至超低空拓展，規(guī)劃環(huán)境也從簡(jiǎn)單向復(fù)雜轉(zhuǎn)變。其復(fù)雜性具體體現(xiàn)在：環(huán)境非結(jié)構(gòu)化（包含多種類型密集障礙物、存在非凸區(qū)域）；環(huán)境動(dòng)態(tài)性（存在突發(fā)威脅、移動(dòng)威脅、運(yùn)動(dòng)目標(biāo)等）；環(huán)境不確定性（規(guī)劃空間信息部分未知或完全未知）；任務(wù)背景復(fù)雜化（從單純的避障飛行變?yōu)樵谝?guī)避障礙的同時(shí)完成例如目標(biāo)跟蹤、區(qū)域搜索、編隊(duì)飛行等任務(wù)）等。這對(duì)無(wú)人機(jī)的航路規(guī)劃能力提出了諸多新的挑戰(zhàn)。無(wú)人機(jī)僅依靠水平機(jī)動(dòng)已難以很好地滿足此類復(fù)雜環(huán)境和任務(wù)的特殊要求，甚至存在著一些嚴(yán)重的隱患。因此，必須將航路規(guī)劃方法由二維空間擴(kuò)展為三維空間。對(duì)此，大量研究人員進(jìn)行了不懈的探索。目前所提出三維航路規(guī)劃方法可以被大致劃分為：基于模型預(yù)測(cè)控制的規(guī)劃方法[4]、基于優(yōu)化算法的規(guī)劃方法[5]、基于隨機(jī)規(guī)劃的方法[6]、基于幾何計(jì)算規(guī)劃方法[7]，以及基于人工勢(shì)場(chǎng)的規(guī)劃方法[8]等。前四類方法在由二維空間擴(kuò)展為三維空間后，往往存在如下問(wèn)題：當(dāng)?shù)匦苇h(huán)境較為復(fù)雜時(shí)，算法計(jì)算量將呈爆炸式增長(zhǎng)；通過(guò)將規(guī)劃空間離散進(jìn)行航路規(guī)劃時(shí)，生成航路的平滑性難以保障，在應(yīng)用中還必須設(shè)計(jì)額外的航路平滑策略[9]，增大了航路的復(fù)雜度。而基于人工勢(shì)場(chǎng)的方法是基于物理中的勢(shì)場(chǎng)相關(guān)概念所提出的，目標(biāo)點(diǎn)對(duì)無(wú)人機(jī)產(chǎn)生“引力”，障礙物對(duì)無(wú)人機(jī)產(chǎn)生“斥力”，最后通過(guò)求合力來(lái)控制無(wú)人機(jī)的運(yùn)動(dòng)。其物理意義明確，計(jì)算量較小，能夠滿足實(shí)時(shí)航路規(guī)劃的要求，規(guī)劃航路相對(duì)平滑，因此目前看來(lái)，基于人工勢(shì)場(chǎng)的方法是比較適合于三維航路規(guī)劃的。但是這類方法存在以下兩個(gè)不容忽視的問(wèn)題：

（1）在人工勢(shì)場(chǎng)中，沒(méi)有障礙形狀（包絡(luò)）的概念，完全依靠調(diào)整力場(chǎng)生成航路，因此當(dāng)力場(chǎng)參數(shù)調(diào)整不恰當(dāng)時(shí)，無(wú)人機(jī)有可能進(jìn)入障礙內(nèi)部，導(dǎo)致避障失?。?/p>

（2）力場(chǎng)容易陷入局部最優(yōu)。針對(duì)人工勢(shì)場(chǎng)法存在的問(wèn)題，立足于勢(shì)場(chǎng)的基本思想，在文獻(xiàn)[10]中首次提出基于流水避石思想的三維航路規(guī)劃方法。該方法借鑒了自然界水流流動(dòng)的宏觀特征：當(dāng)無(wú)障礙物時(shí)，水流沿直線流動(dòng)；當(dāng)遇到障礙物時(shí)，水流會(huì)平滑地繞過(guò)該障礙并最終流向終點(diǎn)。引入三維障礙外包絡(luò)的概念，將航路規(guī)劃與流體計(jì)算有機(jī)結(jié)合，通過(guò)流體力學(xué)方法對(duì)三維地形進(jìn)行流場(chǎng)模擬，并綜合考慮無(wú)人機(jī)性能約束、飛行安全性、航路代價(jià)等指標(biāo)進(jìn)行航路優(yōu)選，最終得到滿足任務(wù)要求的三維光滑可飛航路。由于水流必定能夠找到入口和出口之間的路徑，因此能夠避免勢(shì)場(chǎng)法容易陷入局部勢(shì)場(chǎng)極小點(diǎn)的問(wèn)題。

遺憾的是，傳統(tǒng)流水避石方法仍存在一定的局限性：其中的解析法僅能處理球體障礙，對(duì)于其他立體障礙（例如柱形、錐形等）難以獲得其解析解；而其中的數(shù)值法由于需要采用CFD進(jìn)行模擬，計(jì)算量過(guò)大，僅能用于離線航路規(guī)劃。另外，傳統(tǒng)流水避石方法由于自身的復(fù)雜性，難以與其他任務(wù)背景相結(jié)合，僅能做單純的避障機(jī)動(dòng)，極大的限制了其應(yīng)用。

從航路規(guī)劃的角度講，對(duì)于更為復(fù)雜的障礙物，可以放寬對(duì)流體物理性質(zhì)的限制，重點(diǎn)關(guān)注流體的避障特性，從而降低方程求解的難度。因此，針對(duì)傳統(tǒng)流水避石方法存在的問(wèn)題，在文獻(xiàn)[11]中首次提出了擾動(dòng)流體動(dòng)態(tài)系統(tǒng)方法（Interfered Fluid Dynamical System，IFDS），該算法以解析法為基礎(chǔ)，但避免了求解帶有復(fù)雜邊界條件的流體方程，便于處理復(fù)雜的地形和不同形狀的障礙物。規(guī)劃航路不僅具有仿流水避石的自然特性，而且環(huán)境建模簡(jiǎn)單，計(jì)算量小，大大拓展了流水避石方法的適用范圍。

2 基本原理

2.1 靜態(tài)IFDS

在利用IFDS進(jìn)行航路規(guī)劃前，首先需要對(duì)障礙進(jìn)行預(yù)處理?，F(xiàn)實(shí)中的大部分障礙可以用標(biāo)準(zhǔn)凸面體及其組合體進(jìn)行描述。對(duì)于地形障礙（例如建筑和山峰），可以簡(jiǎn)化為柱體、椎體、立方體、半球體及其組合；對(duì)于移動(dòng)威脅（例如入侵機(jī)），可以化簡(jiǎn)為球體。標(biāo)準(zhǔn)凸面體障礙方程如下：

其中ξ=（x，y，z）為無(wú)人機(jī)的位置；ξ0=（x0，y0，z0）為凸面體中心點(diǎn)位置；a，b，c為凸面體三維軸長(zhǎng)；d，e，f為凸面體形狀參數(shù)，a，b，c，d，e，f六個(gè)參數(shù)共同決定凸面體的形狀。

擾動(dòng)流體動(dòng)態(tài)系統(tǒng)算法（IFDS）提取了自然界流水避石現(xiàn)象與避障航路規(guī)劃問(wèn)題的相似之處：河流中的石頭可看做無(wú)人機(jī)需躲避的障礙物；筆直的流水可看做初始流場(chǎng)，初始流場(chǎng)流線即為無(wú)障礙環(huán)境下的初始航路；繞過(guò)石頭的流水可等效為擾動(dòng)流場(chǎng)，擾動(dòng)流場(chǎng)流線即為障礙環(huán)境下的規(guī)劃航路。靜態(tài)環(huán)境下，我們?cè)谀繕?biāo)點(diǎn)處（左下角）引入?yún)R流作為初始流場(chǎng)，匯流是一種以恒定流速均勻地從四周流入目標(biāo)點(diǎn)的流體，因此當(dāng)不存在障礙物或威脅時(shí)，無(wú)人機(jī)以任意一點(diǎn)作為起始點(diǎn)都能沿直線到達(dá)目標(biāo)，如圖1（a）所示。當(dāng)規(guī)劃空間中存在障礙物時(shí)，障礙物的擾動(dòng)使得初始流場(chǎng)發(fā)生改變，但受擾動(dòng)流場(chǎng)仍具有全局收斂性與穩(wěn)定性，即流場(chǎng)流線仍能夠繞開障礙物并到達(dá)目標(biāo)點(diǎn)，如圖1（b）所示。由上述分析可知，IFDS算法的關(guān)鍵在于求解擾動(dòng)流場(chǎng)的流速。

三維靜態(tài)環(huán)境下的IFDS算法步驟如下：首先建立初始流場(chǎng)即匯流，然后將障礙物對(duì)初始流場(chǎng)的擾動(dòng)影響用擾動(dòng)矩陣量化表示，接著通過(guò)修正初始流場(chǎng)流速獲得擾動(dòng)流場(chǎng)流速，最后對(duì)其迭代積分即可得到擾動(dòng)流場(chǎng)流線，即無(wú)人機(jī)規(guī)劃航路。

圖1 流體擾動(dòng)示意圖

假設(shè)無(wú)人機(jī)目的地為ξd=（xd，yd，zd），任務(wù)區(qū)域內(nèi)共有K個(gè)障礙，所有障礙都按式（1）完成了建模。

當(dāng)不存在障礙時(shí)，流線應(yīng)呈一條從無(wú)人機(jī)當(dāng)前位置指向目的點(diǎn)的直線段，無(wú)人機(jī)將以V的速度幅值沿該直線段飛行，此時(shí)無(wú)人機(jī)的速度被稱為原始流速（又稱匯）u，可沿地面三軸坐標(biāo)系描述為式（2）的形式：

接下來(lái)，障礙對(duì)原始流速的影響可用總的擾動(dòng)矩陣M來(lái)描述：

其中，ωk為第k個(gè)障礙的權(quán)重系數(shù)；Mk為第k個(gè)障礙的擾動(dòng)矩陣。ωk和Mk可分別表示為

其中，Γk和Γi由式（1）計(jì)算得出；I為吸引矩陣，為三階單位陣，其作用類似于人工勢(shì)場(chǎng)中的吸引場(chǎng)；為第k個(gè)障礙的單位法向量；pk為第k個(gè)障礙的排斥反應(yīng)系數(shù)，通常擾動(dòng)矩陣與障礙的排斥反應(yīng)系數(shù)密切相關(guān)，排斥反應(yīng)系數(shù)越大，流場(chǎng)流線躲避障礙的時(shí)機(jī)越早，因此可以通過(guò)調(diào)節(jié)排斥反應(yīng)系數(shù)獲得不同形狀的流線；為排斥矩陣，作用類似于人工勢(shì)場(chǎng)中的斥力場(chǎng)。

擾動(dòng)矩陣M是整個(gè)IFDS的核心，相應(yīng)的概念源于如下流體力學(xué)中超球體障礙對(duì)繞流影響的相關(guān)理論[12]。

在得到擾動(dòng)矩陣后，修正初始流場(chǎng)流速，得到擾動(dòng)流場(chǎng)流速：

最后，對(duì)積分，獲得下一規(guī)劃航點(diǎn)：

其中，ΔT為計(jì)算步長(zhǎng)。如此迭代求解得到一系列離散點(diǎn)，將其連線即形成擾動(dòng)流線，即規(guī)劃的避障航路。

經(jīng)分析可知，IFDS生成的流線符合流水避石現(xiàn)象的一般特性：

定理1：擾動(dòng)流線滿足地形邊界約束條件，因此無(wú)人機(jī)能安全躲避各類障礙物。

定理2：擾動(dòng)流線最終仍收斂到目標(biāo)點(diǎn)。

定理3：流線有較好的地形回避/跟隨特性。

三個(gè)定理的證明過(guò)程見(jiàn)文獻(xiàn)[11-13]。

2.2 動(dòng)態(tài)IFDS

復(fù)雜動(dòng)態(tài)環(huán)境下，往往存在各類動(dòng)態(tài)威脅。本文引入了無(wú)人機(jī)與移動(dòng)威脅的相對(duì)速度，將動(dòng)態(tài)問(wèn)題轉(zhuǎn)化為靜態(tài)問(wèn)題來(lái)考慮，所以動(dòng)態(tài)威脅環(huán)境下的流場(chǎng)性質(zhì)類似于靜態(tài)障礙下的流場(chǎng)性質(zhì)，仍符合流水避石現(xiàn)象的一般特性：流線在保證避碰的同時(shí)，還能夠由起點(diǎn)流向終點(diǎn)，且具有一定的地形跟蹤特性。

重點(diǎn)考慮了質(zhì)心平移和繞質(zhì)心旋轉(zhuǎn)這兩種運(yùn)動(dòng)方式。如果凸面體威脅滿足條件a=b（如球體、圓柱、圓錐、圓臺(tái)等），繞質(zhì)心的旋轉(zhuǎn)不會(huì)改變威脅空間定義，此時(shí)只需考慮質(zhì)心平移。如果凸面體滿足條件a≠b（如平行六面體、橢球體、橢圓柱等），轉(zhuǎn)動(dòng)的影響不能忽略，兩種運(yùn)動(dòng)方式均需考慮。

首先以單障礙為例，基于t時(shí)刻的障礙物信息（假設(shè)可在線獲得），可預(yù)測(cè)t+1時(shí)刻障礙物的質(zhì)心平移速度vL和vR旋轉(zhuǎn)角速度。定義v為威脅的參考運(yùn)動(dòng)速度：

其中λ為正值，通常λ越大，流線躲避動(dòng)態(tài)障礙物的時(shí)機(jī)越早。當(dāng)無(wú)人機(jī)離障礙物較遠(yuǎn)時(shí)，按式（1）定義的Γ（ξ）較大，障礙物影響會(huì)隨著指數(shù)衰減而迅速消除[14]，此時(shí)參考速度v趨于0，可避免無(wú)人機(jī)不必要的機(jī)動(dòng)，提高避障效率。

構(gòu)建相對(duì)初始流場(chǎng)，其中流速定義為u-v，而相對(duì)擾動(dòng)流場(chǎng)的流速為。在相對(duì)初始流場(chǎng)下，移動(dòng)威脅可看作靜止的，因此可計(jì)算相對(duì)擾動(dòng)流場(chǎng)的流速[13]：

則實(shí)際擾動(dòng)流場(chǎng)流速為：

對(duì)該流速迭代積分，即可獲得擾動(dòng)流線。

當(dāng)存在多個(gè)障礙時(shí)，式（8）將修正為：

其中下標(biāo)k表示第k個(gè)障礙物。

然后對(duì)多個(gè)障礙的速度進(jìn)行加權(quán)求和，得到多障礙的合成速度v：

然后再代入式（10）即可。

動(dòng)態(tài)障礙環(huán)境下相應(yīng)定理證明與靜態(tài)環(huán)境類似，本文不再贅述。

3 方法改進(jìn)

經(jīng)典IFDS算法能較好地模擬自然界的流水特性，規(guī)劃出安全平滑的可飛航路，但該經(jīng)典算法也存在著易陷入陷阱區(qū)域與局部最小點(diǎn)（駐點(diǎn)）問(wèn)題，這是由其本身的缺陷引起的。因此，我們主要對(duì)這兩種局部極小問(wèn)題進(jìn)行分析，并給出了一些針對(duì)性的改進(jìn)策略。與此同時(shí)，如何保證無(wú)人機(jī)在規(guī)劃過(guò)程中能夠生成高質(zhì)量的可飛航路，本文亦給出了一定的解決方案。

3.1 陷阱區(qū)問(wèn)題分析及改進(jìn)方案

當(dāng)規(guī)劃空間中多個(gè)障礙物部分重疊時(shí)，會(huì)產(chǎn)生一個(gè)凹陷區(qū)域即陷阱區(qū)域（Trap area, TA），如圖2陰影部分所示，此時(shí)若擾動(dòng)流速指向TA，流線會(huì)落入該陷阱區(qū)域而無(wú)法逃離。最終，流線到達(dá)兩個(gè)障礙物的交界線處，此時(shí)Γi（ξ）=1與Γi+1（ξ）=1同時(shí)成立，導(dǎo)致式（4）中的分子分母均為0，因此規(guī)劃失敗。

圖2 陷阱區(qū)示意圖

改進(jìn)方案1：引入虛擬障礙

在文獻(xiàn)[11]中，提出引入虛擬障礙來(lái)使無(wú)人機(jī)提前規(guī)避陷阱區(qū)。設(shè)置虛擬的平行六面體障礙的總體思想是分層次地對(duì)凹形陷阱進(jìn)行填充，在盡量不浪費(fèi)可飛空間的前提下防止流線進(jìn)入陷阱區(qū)域。

改進(jìn)方案2：引入虛擬目標(biāo)點(diǎn)

在文獻(xiàn)[15]中，提出引入虛擬目標(biāo)點(diǎn)（Virtual target，VT）來(lái)解決陷阱區(qū)域問(wèn)題，當(dāng)滿足同時(shí)滿足以下條件時(shí)：（1） 多個(gè)障礙物Oi，...Oj重疊；（2）擾動(dòng)流速指向TA；（3）成立，即可構(gòu)建虛擬目標(biāo)點(diǎn)。條件（3）中，d（ξ，Oj）表示無(wú)人機(jī)與障礙物表面的距離，dTA為陷阱區(qū)域判定閾值，可決定流線躲避陷阱區(qū)域的時(shí)機(jī)，該值通常與重疊障礙物的數(shù)量、尺寸等相關(guān)。

構(gòu)建虛擬目標(biāo)點(diǎn)的過(guò)程如下。首先，定義虛擬目標(biāo)曲面Av，虛擬目標(biāo)點(diǎn)Pv可從該曲面上選取。

然后，將曲面Av分別按垂直方向、水平方向等分為N1、N2份，則曲面Av被離散化為N1、N2個(gè)點(diǎn)，將它們作為備選的虛擬目標(biāo)點(diǎn)。

最后構(gòu)造啟發(fā)式評(píng)價(jià)函數(shù)篩選出最優(yōu)目標(biāo)點(diǎn)進(jìn)行即可。

3.2 駐點(diǎn)問(wèn)題分析及改進(jìn)方案

當(dāng)無(wú)人機(jī)起點(diǎn)與目標(biāo)點(diǎn)的連線正好經(jīng)過(guò)障礙中心時(shí)，如圖3所示，由于吸引速度與排斥速度恰好共線且方向相反，則擾動(dòng)流速|(zhì)u|逐漸減小，流線到達(dá)障礙物表面時(shí)為0，因此流線會(huì)停滯于該點(diǎn)，稱為駐點(diǎn)（Stagnation point，SP）。

改進(jìn)方案：引入附加虛擬速度

在文獻(xiàn)[11]中，本課題組提出引入附加虛擬速度（Additional virtual velocity，AVV）來(lái)解決該類問(wèn)題。以無(wú)人機(jī)的最小飛行速度vmin作為判定閾值，當(dāng)同時(shí)滿足以下條件時(shí)：（1）；（2）吸引速度與排斥速度恰好共線且方向相反，即可引入附加虛擬速度virtual。virtual需同時(shí)滿足以下條件：。然后，以合速度引導(dǎo)無(wú)人機(jī)飛行，避免停滯于駐點(diǎn)。采用該種改進(jìn)策略，無(wú)人機(jī)的逃離航路將較為平滑，而且不改變流線的避障特性。

圖3 駐點(diǎn)問(wèn)題示意圖

3.3 局部極小問(wèn)題的統(tǒng)一解決方案：切向矩陣的引入

上述改進(jìn)方案的基本思路都是引入一個(gè)“附加控制場(chǎng)”，使無(wú)人機(jī)在接近陷阱區(qū)和駐點(diǎn)時(shí)能夠及時(shí)“跳出”，雖然能夠在一定程度上改善局部極小問(wèn)題，但必須針對(duì)兩種情況分別設(shè)計(jì)相應(yīng)的策略，其過(guò)程較為繁瑣。實(shí)際上，無(wú)論是陷阱區(qū)還是駐點(diǎn)問(wèn)題，都是局部極小問(wèn)題的不同形式而已，既然如此，理論上應(yīng)該有統(tǒng)一的解決方案來(lái)一次性解決局部極小問(wèn)題。

進(jìn)一步分析可知，之所以在經(jīng)典IFDS中會(huì)出現(xiàn)局部極小問(wèn)題，根本原因在于對(duì)擾動(dòng)矩陣M的定義不夠全面、客觀。因此，圍繞對(duì)擾動(dòng)矩陣M，仍然是借鑒之前的“附加控制場(chǎng)”思路，在文獻(xiàn)[16]中，提出在擾動(dòng)矩陣M中，除了吸引矩陣和切向矩陣外，仿照排斥矩陣的形式，再引入一個(gè)切向矩陣，其表達(dá)式為：

其中，τk∈（-1，1），其取值與避撞所處狀態(tài)有關(guān)，避撞開始為正，結(jié)束為負(fù)；σk與排斥反應(yīng)系數(shù)ρk類似，其大小決定“附加控制”的強(qiáng)弱，稱為切向反應(yīng)系數(shù)[17]。

在文獻(xiàn)[15，18，19]中，進(jìn)一步改進(jìn)切向矩陣。對(duì)第k個(gè)障礙物來(lái)說(shuō)，在與徑向法向量nk垂直的切平面S上，定義兩個(gè)互相垂直的切向量：

其中tk,1為水平切向量，切平面S與各向量的定義如圖4所示。以tk,1為x'軸，tk,2為y'軸，nk為z'軸建立坐標(biāo)系，o'-x'y'z'則切平面S上的任意單位切向量在o'-x'y'z'中表示為：

其中，θk∈[-π,π]表示任意切向量與向量tk,1（即x'軸）的夾角，稱為切向方向系數(shù)。

圖4 各向量示意圖

切向量t'k在o-xyz中表示為tk，經(jīng)坐標(biāo)變換可得該向量：

其中Rk表示坐標(biāo)系o'- x'y'z'到o- xyz的坐標(biāo)旋轉(zhuǎn)矩陣。

因此，修改過(guò)的IFDS的擾動(dòng)矩陣M最終可表示為：

其中，擾動(dòng)矩陣由吸引矩陣、排斥矩陣和切向矩陣組成，共有三個(gè)可調(diào)參數(shù)，分別為排斥反應(yīng)系數(shù)ρk、切向反應(yīng)系數(shù)σk以及切向方向系數(shù)θk。

改進(jìn)后的IFDS仍滿足2.1節(jié)的定理1-3，詳細(xì)證明過(guò)程見(jiàn)文獻(xiàn)[15]。通過(guò)在擾動(dòng)矩陣中引入切向矩陣，拓寬了流體的物理特性與擾動(dòng)流速的空間表達(dá)，有效地解決了局部極小問(wèn)題。

3.4 反應(yīng)系數(shù)優(yōu)化問(wèn)題

由于無(wú)人機(jī)飛行環(huán)境是動(dòng)態(tài)變化的，因此必須實(shí)時(shí)更新環(huán)境信息，進(jìn)行在線規(guī)劃。為解決在線規(guī)劃問(wèn)題，采用了滾動(dòng)優(yōu)化策略實(shí)現(xiàn)對(duì)局部航路的在線優(yōu)化：設(shè)定有限步優(yōu)化時(shí)域N，依據(jù)實(shí)時(shí)檢測(cè)到的動(dòng)態(tài)威脅或目標(biāo)等信息，預(yù)測(cè)未來(lái)它們的運(yùn)動(dòng)狀態(tài)，然后優(yōu)化反應(yīng)系數(shù)，尋找滿足無(wú)人機(jī)動(dòng)力學(xué)約束與環(huán)境約束的局部可飛最優(yōu)航路，然后只以局部航路飛行一步，再重復(fù)上述動(dòng)作，引導(dǎo)無(wú)人機(jī)完成飛行任務(wù)。利用此方法，可以充分考慮未來(lái)運(yùn)動(dòng)態(tài)勢(shì)，提前確定大致的可靠飛行航路。

優(yōu)化過(guò)程主要分為如下幾步：

（1）設(shè)置航路評(píng)價(jià)指標(biāo)。一般來(lái)說(shuō)，包括航路長(zhǎng)度代價(jià)、航路安全代價(jià)、航路機(jī)動(dòng)代價(jià)等。航路長(zhǎng)度代價(jià)不僅要考慮未來(lái)N步航路的長(zhǎng)度，還要考慮該N步航路之后的航路總長(zhǎng)度；航路安全代價(jià)一般為無(wú)人機(jī)與障礙表面相對(duì)距離的倒數(shù)；航路機(jī)動(dòng)代價(jià)一般為無(wú)人機(jī)航向角和爬升角變化量的絕對(duì)值。

（2）對(duì)各類子指標(biāo)進(jìn)行歸一化處理，最終確定局部?jī)?yōu)化指標(biāo)J（t:t+N）。然后，即可優(yōu)化各障礙物的反應(yīng)系數(shù)，得到t時(shí)刻的最優(yōu)反應(yīng)系數(shù)組合：

（3）設(shè)計(jì)滾動(dòng)優(yōu)化策略中的解算器。為了求解式（20），除了將參數(shù)離散化后進(jìn)行遍歷搜索以外，還可以設(shè)計(jì)更高效的求解器來(lái)滿足實(shí)時(shí)解算的要求，例如文獻(xiàn)[11，20，21]分別采用遺傳算法、改進(jìn)灰狼優(yōu)化算法以及改進(jìn)鯨魚優(yōu)化算法解算優(yōu)化反應(yīng)系數(shù)。

3.5 航路可飛性問(wèn)題解決方案

傳統(tǒng)的航路規(guī)劃方法（包括經(jīng)典IFDS，如文獻(xiàn)[11，13，16]）大多對(duì)于無(wú)人機(jī)動(dòng)力學(xué)約束考慮較少，導(dǎo)致規(guī)劃的航路可能不滿足可飛性要求，無(wú)法實(shí)現(xiàn)精確航路跟蹤。為解決這一問(wèn)題，提出以下三種思路：

（1） 在設(shè)計(jì)航路評(píng)價(jià)指標(biāo)時(shí)加入航路可飛性評(píng)價(jià)指標(biāo)，當(dāng)不滿足該指標(biāo)時(shí)，其值置為無(wú)窮或一個(gè)較大懲罰值，然后進(jìn)行迭代搜索尋優(yōu)。

該方法思路比較直接，但比較容易陷入局部最優(yōu)解。該方法具體見(jiàn)文獻(xiàn)[18，19]。

（2）將軌跡延拓策略（Trajectory propagation，TP）融入IFDS。為了判斷航路是否可飛，需確保飛行器沿航路飛行的各主要狀態(tài)（如動(dòng)壓、迎角、滾轉(zhuǎn)角等）連續(xù)變化、滿足給定的約束且在不同階段航路的連接點(diǎn)處能夠匹配。采取軌跡延拓來(lái)進(jìn)行軌跡分析，該方法考慮無(wú)人機(jī)的動(dòng)力學(xué)模型，通過(guò)迭代求解，不但可得到能夠同時(shí)滿足給定軌跡約束和飛行器動(dòng)力學(xué)約束的各狀態(tài)變量剖面，同時(shí)還能給出制導(dǎo)指令。軌跡延拓實(shí)際上是在給定軌跡約束的前提下，求解飛行器動(dòng)力學(xué)微分方程的過(guò)程。此外，采用軌跡延拓算法解算出的相關(guān)變量，能對(duì)排斥系數(shù)或切向系數(shù)的調(diào)整提供參考。假設(shè)無(wú)人機(jī)躲避球形威脅，經(jīng)分析可知，排斥系數(shù)越大，擾動(dòng)速度中的垂直分量越大，而切向系數(shù)越大，擾動(dòng)速度中的水平分量越大。因此，假設(shè)飛行爬升角或迎角超出約束范圍，說(shuō)明擾動(dòng)速度中的垂直分量過(guò)大，排斥系數(shù)需減小或切向系數(shù)需增大；假設(shè)轉(zhuǎn)彎角速率太大，說(shuō)明擾動(dòng)速度中的水平分量過(guò)大，排斥系數(shù)需增大或切向系數(shù)需減小。

雖然該方法解算出的狀態(tài)量較為連續(xù)，但當(dāng)障礙環(huán)境較為復(fù)雜時(shí)，非線性微分方程可能存在解算失敗或解算超時(shí)的風(fēng)險(xiǎn)。該方法具體見(jiàn)文獻(xiàn)[15]。

（3）將無(wú)人機(jī)運(yùn)動(dòng)學(xué)/動(dòng)力學(xué)模型與約束直接融入IFDS。首先將獲得的擾動(dòng)流速視為無(wú)人機(jī)期望速度，然后利用其解算出期望的航向角和爬升角，再對(duì)其進(jìn)行限幅，可得受約束的航向角和爬升角，然后再反算出期望過(guò)載，判斷過(guò)載是否在可用范圍內(nèi)，若不在范圍內(nèi)，則將過(guò)載與航向角和爬升角進(jìn)行反復(fù)修正迭代，直至滿足要求。然后輸出實(shí)際可達(dá)航向角和爬升角至無(wú)人機(jī)運(yùn)動(dòng)學(xué)方程，從而計(jì)算出位移（即可飛航路）。

該方法相當(dāng)于對(duì)軌跡延拓的簡(jiǎn)化，其計(jì)算復(fù)雜度低于軌跡延拓，實(shí)時(shí)性更好，但從理論上來(lái)說(shuō)，定理1-3在指令受限的情況下有可能不再滿足，因此必須與滾動(dòng)時(shí)域優(yōu)化結(jié)合。該方法具體見(jiàn)文獻(xiàn)[21]。

3.6 與其他避障方法的融合

除了與3.4節(jié)的滾動(dòng)時(shí)域優(yōu)化/模型預(yù)測(cè)控制方法相結(jié)合以外，IFDS還可與其他類型的避障方法相融合，體現(xiàn)了較好的兼容性。例如在文獻(xiàn)[22]中，IFDS與快速擴(kuò)展隨機(jī)樹（RRT）算法（基于隨機(jī)規(guī)劃的航路規(guī)劃方法）相結(jié)合，以IRRT算法為基本粗規(guī)劃算法，并采用IFDS算法進(jìn)行精細(xì)的新節(jié)點(diǎn)擴(kuò)展，該方法兼具IFDS生成航路平滑與RRT算法具有概率意義完備性，全局搜索能力強(qiáng)的優(yōu)點(diǎn)；文獻(xiàn)[23]提出將動(dòng)態(tài)避障區(qū)[24]與IFDS相結(jié)合，建立了多層空中防撞決策機(jī)制。

4 方法應(yīng)用

目前，除了傳統(tǒng)的避障航路規(guī)劃以外，IFDS還廣泛應(yīng)用于復(fù)雜地形下單機(jī)/多機(jī)協(xié)同目標(biāo)對(duì)峙跟蹤[18-19]，城市環(huán)境下太陽(yáng)能無(wú)人機(jī)能量最優(yōu)飛行[21]，城市區(qū)域搜索中的避障轉(zhuǎn)場(chǎng)[25]，電力巡線[26]等。本文以目標(biāo)跟蹤和太陽(yáng)能無(wú)人機(jī)應(yīng)用為例，闡述IFDS的應(yīng)用前景。

4.1 單機(jī)/多機(jī)協(xié)同目標(biāo)對(duì)峙跟蹤

對(duì)峙跟蹤是指無(wú)人機(jī)在追蹤運(yùn)動(dòng)目標(biāo)的過(guò)程中，無(wú)人機(jī)與目標(biāo)的水平距離保持恒定的對(duì)峙距離，從而保證無(wú)人機(jī)傳感器對(duì)目標(biāo)的覆蓋，減小定位誤差，降低無(wú)人機(jī)暴露風(fēng)險(xiǎn)。目前，自由空間或簡(jiǎn)單環(huán)境下的目標(biāo)對(duì)峙跟蹤問(wèn)題已得到了較好的解決，但當(dāng)無(wú)人機(jī)飛行環(huán)境較為復(fù)雜如障礙物密集分布時(shí)，傳統(tǒng)方法的計(jì)算效率較低甚至得不到可行解，難以規(guī)劃安全軌跡。因此需尋找一種復(fù)雜條件下面向目標(biāo)對(duì)峙跟蹤問(wèn)題的有效求解框架，在提高無(wú)人機(jī)跟蹤目標(biāo)效率的同時(shí)保證航路安全性。

在文獻(xiàn)[18]中，針對(duì)三維復(fù)雜障礙環(huán)境下的對(duì)峙跟蹤問(wèn)題，提出將Lyapunov導(dǎo)航向量場(chǎng)方法（LGVF）[27]與IFDS相結(jié)合，在對(duì)峙跟蹤的同時(shí)規(guī)避障礙。LGVF在三維自由空間的向量場(chǎng)如圖5所示，生成的向量場(chǎng)逐漸收斂于所設(shè)定的Lyapunov極限環(huán)上，軌跡流暢平滑，具備流體的宏觀特性，易與IFDS結(jié)合。因此一種有效的求解思路是將LGVF代替匯流，作為IFDS的初始流場(chǎng)，也就是把按下式所定義的速度代替匯流速度即可：

其中，Vd為定義的Lyapunov距離函數(shù)；V0為無(wú)人機(jī)速率；r為無(wú)人機(jī)與目標(biāo)的相對(duì)距離；R和H分別為設(shè)定的對(duì)峙跟蹤半徑和高度；α為速率調(diào)節(jié)因子；λ為縱向收斂速度調(diào)節(jié)因子。

圖5 LGVF在三維自由空間的向量場(chǎng)

兩種算法因此有效結(jié)合起來(lái)。由于IIFDS方法不改變?cè)到y(tǒng)的穩(wěn)定性，因此擾動(dòng)流線即規(guī)劃航路可保證無(wú)人機(jī)在避障的同時(shí)仍能夠跟蹤目標(biāo)。文獻(xiàn)[19]在此基礎(chǔ)上，進(jìn)一步引入Lyapunov相位函數(shù)確定多架無(wú)人機(jī)期望速率，引導(dǎo)無(wú)人機(jī)按相位平均分布在極限環(huán)上，實(shí)現(xiàn)了復(fù)雜地形下多無(wú)人機(jī)協(xié)同對(duì)峙跟蹤，效果如圖6所示。

4.2 城市地形下太陽(yáng)能無(wú)人機(jī)能量最優(yōu)飛行

太陽(yáng)能無(wú)人機(jī)的能量生產(chǎn)與其自身姿態(tài)和日照的相對(duì)幾何關(guān)系息息相關(guān)。太陽(yáng)能無(wú)人機(jī)在城市環(huán)境下飛行時(shí)，除了要考慮規(guī)避建筑物以外，還要盡可能確保自身能量生產(chǎn)最大化，而城市中的建筑物除了會(huì)威脅無(wú)人機(jī)的飛行安全外，還會(huì)對(duì)陽(yáng)光起到遮擋效果，惡化太陽(yáng)能無(wú)人機(jī)的能量生產(chǎn)狀況。因此文獻(xiàn)[21]提出以IFDS為基礎(chǔ)航路規(guī)劃算法，反解算出太陽(yáng)能無(wú)人機(jī)的姿態(tài)角（即3.5節(jié)的第三種思路），然后依據(jù)姿態(tài)角與太陽(yáng)角的相對(duì)關(guān)系計(jì)算出太陽(yáng)能無(wú)人機(jī)的凈能量生產(chǎn)值，并在考慮陽(yáng)光遮擋的情況下綜合規(guī)劃出兼顧能量最優(yōu)與時(shí)間最優(yōu)的航路，效果見(jiàn)圖7、8。圖8中，綠線為所提出方法在飛行過(guò)程中的凈能量生產(chǎn)值。

圖6 復(fù)雜地形下多無(wú)人機(jī)對(duì)峙跟蹤運(yùn)動(dòng)目標(biāo)效果圖

圖7 太陽(yáng)能無(wú)人機(jī)飛行軌跡

圖8 太陽(yáng)能無(wú)人機(jī)凈能量生產(chǎn)值

5 結(jié)論

本文闡述了基于擾動(dòng)流體動(dòng)態(tài)系統(tǒng)的復(fù)雜地形條件下無(wú)人機(jī)三維航路規(guī)劃方法。在基于流水避石思想航路規(guī)劃方法的前期研究工作基礎(chǔ)上，適當(dāng)放寬對(duì)流體物理性質(zhì)的限制，重點(diǎn)關(guān)注流體的避障特性，從而得到了流體擾動(dòng)的一般解析形式，并將其推廣到無(wú)人機(jī)航路規(guī)劃中。針對(duì)經(jīng)典IFDS存在的幾個(gè)關(guān)鍵性問(wèn)題，例如局部最小問(wèn)題、參數(shù)優(yōu)化問(wèn)題、航路可飛性問(wèn)題等，設(shè)計(jì)了多種策略加以改進(jìn)與探索。除三維避障航路規(guī)劃以外，IFDS還廣泛應(yīng)用于目標(biāo)跟蹤、能量最優(yōu)飛行、區(qū)域搜索、電力巡線等多項(xiàng)無(wú)人機(jī)特定任務(wù)中，體現(xiàn)出較高的應(yīng)用價(jià)值。目前，流水避石思想及其延伸擾動(dòng)流體動(dòng)態(tài)系統(tǒng)方法的相關(guān)成果不斷涌現(xiàn)，形成了從理論到應(yīng)用的完整體系，逐漸成為人工勢(shì)場(chǎng)法的重要分支，展現(xiàn)出旺盛的生命力。

在未來(lái)，可繼續(xù)圍繞IFDS就以下方面進(jìn)行探索：

（1） 目前，以深度強(qiáng)化學(xué)習(xí)為代表的人工智能技術(shù)正蓬勃發(fā)展，可以考慮將此類人工智能技術(shù)與IFDS相融合，從而提高避障的效率和航路質(zhì)量。

（2） 在將無(wú)人機(jī)運(yùn)動(dòng)學(xué)/動(dòng)力學(xué)模型與約束直接融入IFDS后，能否能進(jìn)一步給出在指令受限的情況下定理1-3的證明過(guò)程，這對(duì)于后續(xù)實(shí)現(xiàn)航路規(guī)劃與控制一體化具有重要意義。

（3） 可以考慮在結(jié)合相應(yīng)環(huán)境與任務(wù)背景后，將IFDS推廣至其他智能載具，例如自主水下潛航器等。

參 考 文 獻(xiàn)

［1］Tisdale J，Kim Z W，Hedrick J K.Autonomous UAV Path Planning and Estimation：An Online Path Planning Framework for Cooperative Search and Localization［J］.IEEE Robotics & Automation Magazine，2009，16（2）：35-42.

［2］Chao H Y，Cao Y C，Chen Y Q.Autopilots for small unmanned aerial vehicles： a survey［J］.International Journal of Control，Automation and Systems，2010，8（1）：36-44.

［3］Goerzen C，Kong Z，Mettler B.A survey of motion planning algorithms from the perspective of autonomous UAV guidance ［J］.Journal of Intelligent and Robotic Systems，2010，57（1-4）：65-100.

［4］Wu J，Wang H，Li N，et al.Distributed trajectory optimization for multiple solar-powered UAVs target tracking in urban environment by Adaptive Grasshopper Optimization Algorithm［J］.Aerospace Science and Technology，2017，70：497-510.

［5］陳謀，肖健，姜長(zhǎng)生.基于改進(jìn)蟻群算法的無(wú)人機(jī)三維航路規(guī)劃［J］.吉林大學(xué)學(xué)報(bào)（工學(xué)版），2008，38（4）：991-995.

［6］尹高揚(yáng)，周紹磊，吳青坡.無(wú)人機(jī)快速三維航跡規(guī)劃算法［J］.西北工業(yè)大學(xué)學(xué)報(bào)，2016，34（4）：564-570.

［7］Seo J，Kim Y，Kim S，et al.Collision Avoidance Strategies for Unmanned Aerial Vehicles in Formation Flight ［J］.IEEE Transactions on Aerospace and Electronic Systems，2017，53（6）：2718-2734.

［8］Chen Y，Luo G，Mei Y，et al.UAV path planning using artificial potential field method updated by optimal control theory ［J］.International Journal of Systems Science，2016，47（6）：1407-1420.

［9］Zhang X，Duan H.An improved constrained differential evolution algorithm for unmanned aerial vehicle global route planning ［J］.Applied Soft Computing，2015，26：270-284.

［10］梁宵，王宏倫，李大偉，等.基于流水避石原理的無(wú)人機(jī)三維航路規(guī)劃方法［J］.航空學(xué)報(bào)，2013，34（7）：1670-1681.

［11］Wang H，Lyu W，Yao P，et al.Three-dimensional path planning for unmanned aerial vehicle based on interfered fluid dynamical system ［J］.Chinese Journal of Aeronautics，2015，28（1）：229-239.

［12］L.M.Milne-Thomson.Theoretical hydrodynamics［M］.4th ed.London：Macmillan，1960.

［13］Yao P，Wang H，Liu C.3-D dynamic path planning for UAV based on interfered fluid flow［C］.Proceedings of 2014 IEEE Chinese Guidance，Navigation and Control Conference，Aug.2014，Yantai，China，997-1002.

［14］Khansari-Zadeh S M，Billard A.Realtime avoidance of fast moving objects：A dynamical system-based approach［C］.2012 IEEE/RSJ International Conference on Intelligent Robotics Systems （IROS）.IEEE，2012：121-126.

［15］Yao P，Wang H，Su Z.UAV feasible path planning based on disturbed fluid and trajectory propagation［J］.Chinese Journal of Aeronautics，2015，28（4）：1163-1177.

［16］王宏倫，雷玉鵬，姚鵬，等.基于流體擾動(dòng)計(jì)算的無(wú)人機(jī)三維無(wú)碰撞航路規(guī)劃［J］.控制理論與應(yīng)用，2016，33（3）：295-303.

［17］王宏倫，姚鵬，梁宵，等.基于流水避石原理的無(wú)人機(jī)三維航路規(guī)劃［J］.電光與控制，2015，22（10）：1- 6

［18］Yao P，Wang H，Su Z.Real-time path planning of unmanned aerial vehicle for target tracking and obstacle avoidance in complex dynamic environment［J］.Aerospace Science and Technology，2015，47：269-279.

［19］Yao P，Wang H，Su Z.Cooperative path planning with applications to target tracking and obstacle avoidance for multi-UAVs［J］.Aerospace Science and Technology，2016，54：10-22.

［20］姚鵬，王宏倫.基于改進(jìn)流體擾動(dòng)算法與灰狼優(yōu)化的無(wú)人機(jī)三維航路規(guī)劃［J］.控制與決策，2016，31（4）：701-708.

［21］Wu J，Wang H，Li N，et al.Path planning for solar-powered UAV in urban environment［J］.Neurocomputing，2018，275：2055-2065.

［22］Yao P，Wang H，Su Z.Hybrid UAV path planning based on interfered fluid dynamical system and improved RRT［C］.Industrial Electronics Society，IECON 2015-41st Annual Conference of the IEEE.IEEE，2015：829-834.

［23］Liu C，Wang H，Yao P.UAV collision avoidance decision and 3D path re-planning method based on improved interfered fluid dynamical system and rolling optimization［C］.Guidance，Navigation and Control Conference （CGNCC），2016 IEEE Chinese.IEEE，2016：2238-2243.

［24］劉暢，王宏倫，姚鵬，等.面向空中威脅的無(wú)人機(jī)動(dòng)態(tài)碰撞區(qū)建模與分析［J］.北京航空航天大學(xué)學(xué)報(bào)，2015，41（7）：1231-1238.

［25］Yao P，Wang H，Ji H.Gaussian mixture model and receding horizon control for multiple UAV search in complex environment［J］.Nonlinear Dynamics，2017，88（2）：903-919.

［26］Shuai C，Wang H，Zhang W，et al.Binocular vision perception and obstacle avoidance of visual simulation system for power lines inspection with UAV［C］.Control Conference（CCC），2017 36th Chinese.IEEE，2017：10480-10485.

［27］Frew E W，Lawrence D A，Morris S.Coordinated Standoff Tracking of Moving Targets Using Lyapunov Guidance Vector Fields ［J］.Journal of Guidance Control and Dynamics，2008，31（2）：290- 306.