趙文嬌，甘旭升

(1 山西工程科技職業(yè)大學交通工程學院，山西 晉中 030619；2 空軍工程大學空管領航學院，西安 710051)

0 引言

隨著神舟十二號載人航天順利著陸、神舟十三號載人航天成功發(fā)射，各型四旋翼無人機頻頻出境，在載人航天搜救回收任務中發(fā)揮了重要作用，已成為當前執(zhí)行搜救回收任務的主要承載方式。四旋翼無人機相對傳統(tǒng)無人機而言，有著垂直升降、機動性強和操作方便等多方面的優(yōu)點，這使得四旋翼無人機在航天發(fā)射與回收、軍事偵察、警方搜索、火災救援、農情監(jiān)測以及影視拍攝等諸多領域發(fā)揮重要作用[1-4]。

航跡規(guī)劃在實現(xiàn)四旋翼無人機機動高效和安全飛行中發(fā)揮著重要作用，它是根據當前地形環(huán)境為四旋翼無人機規(guī)劃出滿足若干任務要求的一條由起點到終點的最優(yōu)或次優(yōu)航行路徑，是當前四旋翼無人機領域的重要研究方向[5-6]。相關的航跡規(guī)劃研究也吸引了大量學者，例如文獻[7]針對四旋翼無人機低空飛行實時規(guī)避需求，構建目標和威脅的虛擬力場，提出了一種基于人工勢場的二維航跡規(guī)劃方法，使得無人機沿著邊緣規(guī)避障礙。文獻[8]針對無人機航跡規(guī)劃通常假設環(huán)境靜態(tài)且需要地形先驗知識的現(xiàn)狀，提出了一種實時局部航跡重規(guī)劃方法(real-time trajectory, RTT)，以更有效地應對未知、動態(tài)的障礙物。文獻[9]基于經典Teach-and-Repeat框架，提出了一種完整、魯棒的四旋翼無人機航跡自主規(guī)劃方法(teach repeat replan，TRR)，具有全局規(guī)劃和局部感知的綜合性能，進一步改善了航跡規(guī)劃在全局誤差、定位漂移和動態(tài)規(guī)避等方面問題。文獻[10]為進一步準確獲取無人機未來時刻位置，采取限制角速度的方式對航跡規(guī)劃模型進行改進，從而提出了一種基于卷積神經網絡的無人作戰(zhàn)飛機飛行軌跡預測算法，以提高無人機機動決策的預見性。文獻[11]針對無人機航跡規(guī)劃高運算率、高執(zhí)行性需求，改進ARA*三維航跡規(guī)劃模型，提出了一種無人飛行器在線航路籌劃方法，進一步適應動態(tài)規(guī)劃與時間最優(yōu)性的綜合要求。

城市高樓林立、山區(qū)地形起伏、深林植被茂密，很難達到固定翼飛機的起降標準，而四旋翼無人機以其垂直起降、空中懸停等優(yōu)勢具有更強的適應能力。四旋翼無人機為了適應不同的、復雜未知的環(huán)境，避免飛行過程中與障礙物發(fā)生碰撞，必須具備在線規(guī)避和航跡重規(guī)劃能力[5，12]。目前，多數(shù)無人機仍采用地面遠程操控或跟蹤規(guī)劃的方式來進行航跡規(guī)劃，所使用的航跡規(guī)劃方法多數(shù)是在飛行環(huán)境完全已知，且飛行過程中環(huán)境條件不變的情況下，飛行器按照事先規(guī)劃好的航跡飛行[7，13-14]。此外，四旋翼無人機是一個多輸入、欠驅動、強耦合的非線性系統(tǒng)，飛行時沿空間直角坐標系3個方向位移運動并繞坐標系3軸旋轉運動，但僅有4個可控電機轉速輸入變量控制，一般的航跡算法在精度上可能有所不足[15-17]。

針對上述問題，提出一種面向未知復雜環(huán)境的四旋翼無人機在線規(guī)避航跡規(guī)劃方法，采用分段多項式對四旋翼無人機飛行航跡進行建模，在保證系統(tǒng)安全性的前提下優(yōu)化規(guī)避航跡，在C++環(huán)境中搭建仿真模型并驗證方法可行性。

1 四旋翼無人機空間運動模型

圖1 四旋翼無人機空間坐標系

ω=pxB+qyB+γzB

(1)

式中：p,q和γ為3個坐標方向的角速度分量。

每個轉子都有一個角速度ωi，并產生一個力Fi和力矩Mi：

(2)

式中：kF和kM分別為力系數(shù)和力矩系數(shù)。

現(xiàn)實實踐中，與剛體動力學和空氣動力學相比，電機動力學反應時間相對較快，因此假設它們可以瞬間實現(xiàn)，將系統(tǒng)的控制輸入以u表示，其中u1是u2,u3,u4的合力，并將轉子轉速表示為[18]：

(3)

式中：L為從旋翼旋轉軸到四旋翼中心的距離。

四旋翼無人機質心，在空間坐標系中每一時刻的位置向量用r表示。作用在四旋翼無人機系統(tǒng)上的力主要是-zW方向上的重力mg，和來自zB方向上每個轉子的合力u1。于是，將控制四旋翼無人機質心加速度的牛頓運動方程表示為[19]：

(4)

式中：m是四旋翼無人機質量；g是地球重力加速度。

根據歐拉方程，構建四旋翼無人機角加速度模型：

(5)

式中：Γ是沿著xB-yB-zB軸質心方向的慣性矩陣。

于是，四旋翼無人機的空間狀態(tài)可由其質心位置、速度、方向以及角速度參數(shù)聯(lián)合表示：

(6)

2 四旋翼無人機飛行航跡建模

采用分段多項式，將四旋翼無人機每一段飛行航跡pi(t)表示為N階多項式[19]：

pi(t)=αTβ(t),t∈[0,Tc]

(7)

式中：α∈R(N+1)×3為系數(shù)；Tc為四旋翼無人機飛行持續(xù)時間；β(t)=(1,t,t2,…,tN)T為基函數(shù)。

對于奇階多項式航跡，由于N為奇數(shù)，系數(shù)矩陣與邊界條件之間的映射是雙目標的，pi(t)的(N-1)/2階導數(shù)為：

(8)

令d(t)=B(t)α，則有：

(9)

(10)

式中：當N為奇數(shù)時，A(Tc)是方陣，否則A(Tc)為超定陣；A(Tc)=(BT(0),BT(Tc))T為2[(N+1)/2]行(N+1)列的映射矩陣。

實際上A-1(Tc)的所有項都是Tc的冪函數(shù)，應用高斯消去法可得到它的解析形式，當在線計算A-1(Tc)時，為了不再需要耗時的操作，需預先計算相關的系數(shù)矩陣E，F(xiàn)，G，U和W∈RS×S，其中S=(N+1)/2[20]：

(11)

(12)

(13)

(14)

(15)

當在線計算時，只需通過式(6)進行：

(16)

圖2 采用分段多項式表示的飛行航跡

3 飛行航跡平滑估計

四旋翼無人機在有障礙物的空間順利通過的一個重要指標要求是無人機與預定航跡的最小偏差。無人機與預定航跡的偏差評價可通過時間變量的一種多項式運算獲得。

根據四旋翼無人機的運動方程式(4)可知，其控制信號(推力和推力力矩)依賴于位置r(t)∈r的4階導數(shù)r(4)(t)。為此，通過一組給定點的光滑曲線，采用一種變分方法來最小化r(4)(t)[1]：

(17)

式中：p*(t)為兩個后續(xù)航跡點之間所期望的航跡。

將式(17)用歐拉-拉格朗日微分方程表示為：

(18)

可將所期望的規(guī)避航跡指定為時間的7次冪多項式，例如：

(19)

多項式pi(t),i=1,2,…,n(n為要通過的控制點數(shù)),必須滿足一定的約束條件才能確定所有的參數(shù)αij(i=1,2,…,n;j=0,1,…,7)。其中，可由式(20)確保多項式能夠在各航跡點上移動：

(20)

式中：i=1,2,…,n。此外，在初始位置和最終位置，四旋翼無人機的速度、加速度和懸停值均為零，引入式(21)和式(22)以平滑相鄰多項式：

(21)

(22)

于是，可將含有未知數(shù)αij的式(20)～式(22)寫成如式(7)的矩陣形式：

A=αTb

(23)

式中：A為8n×8n的矩陣；α為所需參數(shù)；b為8n×3的矩陣。

4 在線規(guī)避航跡優(yōu)化

典型梯度軌跡最優(yōu)(gradient-based trajectory optimization, GTO)[20]，作為當前主要的航跡生成方法，常與歐氏符號距離場(euclidean signed distance field，ESDF)[21]相結合，可實現(xiàn)無人機航跡的自主規(guī)劃。但在歐氏符號距離場中，面對臨時出現(xiàn)、不可避免的障礙物，其周圍的梯度會產生較大差異。當四旋翼無人機穿過通過歐氏符號距離場表示的區(qū)域時，歐氏符號距離場的梯度會在這些障礙物附近發(fā)生突變，使得航跡無法順利繞過這些障礙物。

通常情況下，歐氏符號距離場難以及時應對這些臨時出現(xiàn)、不可避免的障礙物，尤其是在復雜的現(xiàn)實環(huán)境中。為了進一步解決這一問題，引入額外的參量信息，加入距離代價、碰撞代價以及速度和加速度懲罰項，構建四旋翼無人機在線規(guī)避航跡優(yōu)化模型。將整個飛行航跡細分為b個控制段，以更有效地評估不同階段的規(guī)避成本。對于臨時出現(xiàn)障礙物的階段，將該段航跡重新參數(shù)化為{Q0,Q1,…,QN}個控制點。

四旋翼無人機在線規(guī)避航跡優(yōu)化模型由兩個不同的部分組成。第一部分，如圖3所示，針對四旋翼無人機預定飛行航跡(綠色曲線)，根據A*或RRT*等傳統(tǒng)算法獲得規(guī)避當前障礙物的幾何引導路徑(黃色折線)，通過幾何引導將初始航跡吸引到自由空間以構成規(guī)避航跡。將該部分的優(yōu)化目標函數(shù)構建為：

圖3 在線規(guī)避航跡生成

fp1=λ1sfs+λ1gfg

(24)

式中：fs為飛行軌跡的平滑代價；fg為懲罰路徑與b′條航跡之間的距離代價；λ1s和λ1g分別為代價系數(shù)。

參考文獻[21]，將平滑代價fs設計為一個彈性帶代價函數(shù)[21]：

(25)

由于b′段航跡的形狀是由各控制點精細控制后形成的，利用這一性質來簡化距離代價fg的設計。對于每個控制點Qi，在其引導路徑上分配一個關聯(lián)點Gi，該關聯(lián)點Gi沿引導路徑均勻采樣。于是，將距離代價fg定義為這些關聯(lián)點之間的歐式距離的平方和：

(26)

在該部分，歐氏符號距離場沿航跡的梯度變化平穩(wěn)，如圖3中的紅色箭頭所示，目標函數(shù)的梯度將與航跡向自由空間演化方向相一致。

第二部分，如圖4所示。

圖4 在線規(guī)避航跡優(yōu)化

根據第3節(jié)的航跡平滑估計方法，對飛行航跡的平滑度、動態(tài)可行性進一步優(yōu)化，并在此基礎上構建在線規(guī)避航跡優(yōu)化目標函數(shù)：

fp2=λ2sfs+λ2cfc+λ2d(fv+fa)

(27)

式中：fc為歐氏符號距離場中評估的碰撞代價，當飛行航跡接近障礙物時，碰撞成本將迅速增長；fv和fa用于懲罰不可行的速度和加速度；λ2s，λ2c和λ1d分別為代價系數(shù)。其中，將碰撞代價fc表示為各控制點上障礙物的排斥力的累計函數(shù)：

(28)

式中：d(Qi)為控制點Qi與障礙物之間的距離函數(shù)；Fc(·)是一個可微分的潛在代價函數(shù)，與表示障礙物清除的閾值dthr相關。

(29)

為了使四旋翼無人機飛行穩(wěn)定，將對超過最大允許速度或加速度的飛行航跡進行懲罰。

(30)

式中：?為不同坐標軸方向的飛行速度(v)或加速度(a)，μ∈(x,y,z)。

基于F(?μ)，構建速度懲罰函數(shù)fv和加速度懲罰函數(shù)fa：

(31)

(32)

5 仿真分析

提出的航跡規(guī)劃方法是在C++11中使用通用的非線性優(yōu)化求解器NLopt4來實現(xiàn)的，并分別在仿真環(huán)境和真實環(huán)境中檢驗方法的有效性。設置在線規(guī)避航跡優(yōu)化模型代價系數(shù)λ1s=9,λ1g=2.3,λ2s=10,λ2c=0.8,λ2d=0.01。

四旋翼無人機在未知環(huán)境下飛行時，由于感知范圍有限，需要頻繁地重新規(guī)劃飛行航跡，即當預定飛行航跡與新出現(xiàn)的障礙物將發(fā)生碰撞時，需要在線進行航跡重規(guī)劃，以確保一旦檢測到任何碰撞，就有新的安全飛行航跡可用[23]。為此，構建一個未知環(huán)境以進行四旋翼無人機在線規(guī)避航跡規(guī)劃仿真實驗。仿真實驗中，將整個空間環(huán)境設為未知灰色地帶，四旋翼無人機具備半徑為10 m的感知范圍。在相同約束條件下，采用分段多項式，將每一段航跡表示為一個N階多項式，通過文中方法在線規(guī)劃獲取最優(yōu)規(guī)避航跡。不同階多項式下的航跡變化如圖5所示，選擇較大的N雖然可以確保高階連續(xù)性，與此同時也會增加計算時間和運算復雜度。

圖5 不同階多項式下的航跡變化

選取多項式階數(shù)N=5，四旋翼無人機在線規(guī)避航跡規(guī)劃結果如圖6所示，其中，綠色曲線表示瞬時航跡方向，紅色曲線表示多項式N=5時的最優(yōu)在線規(guī)避航跡。面對非結構化未知環(huán)境、有限的感知范圍以及較高的飛行速度，文中方法能夠持續(xù)、快速地生成規(guī)避航跡。

圖6 四旋翼無人機在線規(guī)避航跡生成

為了客觀地說明文中方法的有效性，將其與兩種先進的方法real-time trajectory(RTT)[8]和teach repeat replan(TRR)[9]進行了仿真對比。對比實驗分別在40 m×40 m×5 m地圖上設置低密度、中密度和高密度3種復雜環(huán)境，四旋翼無人機最大速度和加速度限制分別設為3 m/s和2 m/s2。公平起見，運用相同的參考航跡[22]初始化所有方法，并將優(yōu)化時間限制在15 ms以內，相關數(shù)據統(tǒng)計如表1所示，仿真效果如圖7所示。對比結果可知，文中方法始終能夠保持較高的成功率在未知環(huán)境中獲得可行的規(guī)避航跡。相比較而言，其他兩種方法的成功率隨著環(huán)境復雜性的增加而降低，整體計算時間更長。

表1 不同復雜環(huán)境下3種算法仿真數(shù)據對比

圖7 不同復雜環(huán)境下3種算法航跡規(guī)劃效果

6 結論

提出了一種四旋翼無人機在線規(guī)避航跡規(guī)劃方法。在分析四旋翼無人機空間運動的基礎上，以其質心位置、速度、方向以及角速度參數(shù)聯(lián)合構建運動模型。面向無人機在線避障需求，采用分段多項式對四旋翼無人機飛行航跡進行建模。針對四旋翼無人機預定飛行航跡，通過幾何引導將初始航跡吸引到自由空間以構成規(guī)避航跡。結合航跡平滑估計，對規(guī)避航跡的平滑度、動態(tài)可行性進行優(yōu)化，從而實現(xiàn)四旋翼無人機的在線規(guī)避和航跡重規(guī)劃能力。

在復雜環(huán)境模型中，從計算時間、運算復雜度以及成功率等方面對比仿真表明，選擇高階多項式雖然可以確保高階連續(xù)性，但也會增加計算時間和運算復雜度，文中方法相較于RTT算法和TRR算法運算耗時平均低54%、成功率平均高出15.8%，更適用于四旋翼的在線規(guī)避航跡規(guī)劃。