四旋翼無(wú)人機(jī)在線規(guī)避航跡規(guī)劃方法

趙文嬌，甘旭升

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

0 引言

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

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

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

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

1 四旋翼無(wú)人機(jī)空間運(yùn)動(dòng)模型

圖1 四旋翼無(wú)人機(jī)空間坐標(biāo)系

ω=pxB+qyB+γzB

(1)

式中：p,q和γ為3個(gè)坐標(biāo)方向的角速度分量。

每個(gè)轉(zhuǎn)子都有一個(gè)角速度ωi，并產(chǎn)生一個(gè)力Fi和力矩Mi：

(2)

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

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

(3)

式中：L為從旋翼旋轉(zhuǎn)軸到四旋翼中心的距離。

四旋翼無(wú)人機(jī)質(zhì)心，在空間坐標(biāo)系中每一時(shí)刻的位置向量用r表示。作用在四旋翼無(wú)人機(jī)系統(tǒng)上的力主要是-zW方向上的重力mg，和來(lái)自zB方向上每個(gè)轉(zhuǎn)子的合力u1。于是，將控制四旋翼無(wú)人機(jī)質(zhì)心加速度的牛頓運(yùn)動(dòng)方程表示為[19]：

(4)

式中：m是四旋翼無(wú)人機(jī)質(zhì)量；g是地球重力加速度。

根據(jù)歐拉方程，構(gòu)建四旋翼無(wú)人機(jī)角加速度模型：

(5)

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

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

(6)

2 四旋翼無(wú)人機(jī)飛行航跡建模

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

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

(7)

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

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

(8)

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

(9)

(10)

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

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

(11)

(12)

(13)

(14)

(15)

當(dāng)在線計(jì)算時(shí)，只需通過(guò)式(6)進(jìn)行：

(16)

圖2 采用分段多項(xiàng)式表示的飛行航跡

3 飛行航跡平滑估計(jì)

四旋翼無(wú)人機(jī)在有障礙物的空間順利通過(guò)的一個(gè)重要指標(biāo)要求是無(wú)人機(jī)與預(yù)定航跡的最小偏差。無(wú)人機(jī)與預(yù)定航跡的偏差評(píng)價(jià)可通過(guò)時(shí)間變量的一種多項(xiàng)式運(yùn)算獲得。

根據(jù)四旋翼無(wú)人機(jī)的運(yùn)動(dòng)方程式(4)可知，其控制信號(hào)(推力和推力力矩)依賴于位置r(t)∈r的4階導(dǎo)數(shù)r(4)(t)。為此，通過(guò)一組給定點(diǎn)的光滑曲線，采用一種變分方法來(lái)最小化r(4)(t)[1]：

(17)

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

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

(18)

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

(19)

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

(20)

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

(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]，作為當(dāng)前主要的航跡生成方法，常與歐氏符號(hào)距離場(chǎng)(euclidean signed distance field，ESDF)[21]相結(jié)合，可實(shí)現(xiàn)無(wú)人機(jī)航跡的自主規(guī)劃。但在歐氏符號(hào)距離場(chǎng)中，面對(duì)臨時(shí)出現(xiàn)、不可避免的障礙物，其周圍的梯度會(huì)產(chǎn)生較大差異。當(dāng)四旋翼無(wú)人機(jī)穿過(guò)通過(guò)歐氏符號(hào)距離場(chǎng)表示的區(qū)域時(shí)，歐氏符號(hào)距離場(chǎng)的梯度會(huì)在這些障礙物附近發(fā)生突變，使得航跡無(wú)法順利繞過(guò)這些障礙物。

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

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

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

fp1=λ1sfs+λ1gfg

(24)

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

參考文獻(xiàn)[21]，將平滑代價(jià)fs設(shè)計(jì)為一個(gè)彈性帶代價(jià)函數(shù)[21]：

(25)

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

(26)

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

第二部分，如圖4所示。

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

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

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

(27)

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

(28)

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

(29)

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

(30)

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

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

(31)

(32)

5 仿真分析

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

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

圖5 不同階多項(xiàng)式下的航跡變化

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

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

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

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

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

6 結(jié)論

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

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