微小型無人直升機避障最優(yōu)軌跡規(guī)劃

孟少華 向錦武 羅漳平 任毅如

(北京航空航天大學(xué) 航空科學(xué)與工程學(xué)院，北京 100191)

無人機在國土防御和經(jīng)濟(jì)建設(shè)領(lǐng)域得到了越來越廣泛的應(yīng)用，無人直升機更因其獨特的垂直起降和空中懸停性能而倍受青睞.在復(fù)雜、受限的空間中，無人直升機可以利用其懸停、橫飛、倒飛等飛行方式穿梭于障礙物之間，完成戰(zhàn)場偵查、地形測量、搶險救災(zāi)等任務(wù).與地面車輛不同的是，任何碰撞對于無人直升機都是致命的，直接導(dǎo)致預(yù)定任務(wù)失敗，自主避險能力是其順利完成任務(wù)的關(guān)鍵所在.

軌跡規(guī)劃技術(shù)作為無人直升機自主飛行研究的重要內(nèi)容，受到了國內(nèi)外學(xué)者的廣泛關(guān)注.現(xiàn)有的路圖法(roadmap)、人工勢場法(artificial potential field)、單元分解法(cell decomposition)等方法僅能夠得出安全的飛行路徑，并不能考慮飛行器自身的動力學(xué)特性，屬于路徑規(guī)劃方法，研究內(nèi)容主要側(cè)重于算法的有效性和魯棒性，無法保證路徑是真實可行的.區(qū)別于路徑規(guī)劃，軌跡規(guī)劃不僅要尋找一條從起點到終點的最優(yōu)無碰撞路徑，同時還要給出相應(yīng)的狀態(tài)量和控制輸入量，被廣泛應(yīng)用于機器人、無人車輛和無人飛行器等各種操縱平臺上［1－4］.

目前，針對軌跡規(guī)劃的研究大都采用最優(yōu)控制法，應(yīng)用領(lǐng)域也主要局限在動力學(xué)方程較為簡單的地面無人車輛.文獻(xiàn)［5］建立了地面車輛的三自由度動力學(xué)方程，采用最優(yōu)控制法研究了二維平面內(nèi)的軌跡規(guī)劃.而目前對于無人直升機軌跡規(guī)劃的研究較少，文獻(xiàn)［6］將無人直升機的三維運動簡化為質(zhì)點運動模型，以加速度為偽控制輸入量，得出避障機動的最優(yōu)軌跡.由于在此過程中并沒有考慮直升機復(fù)雜的動力學(xué)特性，也就無法保證軌跡是真實可飛的.因此有必要開展考慮無人直升機動力學(xué)特性的軌跡規(guī)劃方法的研究.

本文以某無人直升機為例研究了避障最優(yōu)軌跡規(guī)劃問題.以最短避障時間為目標(biāo)，考慮了無人直升機六自由度動力學(xué)方程以及飛行性能為約束，利用絕對值性質(zhì)將三維空間障礙物的限制轉(zhuǎn)化為不等式約束，建立了無人直升機避障飛行的最優(yōu)控制模型，利用高斯偽譜法對其進(jìn)行求解.為無人直升機在低空、復(fù)雜、受限空間內(nèi)的避障飛行提供了全局軌跡規(guī)劃方法.

1 避障最優(yōu)軌跡規(guī)劃建模

1.1 無人直升機飛行動力學(xué)模型

本文采用無人直升機六自由度剛體運動方程，向量［x，y，z，φ，θ，ψ］T表示直升機在慣性坐標(biāo)系下的位置和歐拉角，其運動學(xué)方程為

式中，［u，v，w］T為體軸坐標(biāo)系下的直升機線速度矢量;［p，q，r］T為角速度矢量.

無人直升機體軸坐標(biāo)系下的飛行動力學(xué)微分方程如下:

式中，m為全機質(zhì)量;Ixx，Iyy，Izz為機體3個軸方向的慣性矩;下標(biāo) mr，fus，tr，vf，ht分別表示主旋翼、機身、尾槳、垂直和水平安定面;X(·)，Y(·)，Z(·)和 L(·)，M(·)，N(·)表示直升機各部件產(chǎn)生的力和力矩在體軸系下的投影(圖1)，其中主旋翼和尾槳產(chǎn)生的力和力矩是控制量 ucon= ［δcol，δlon，δlat，δped］T的函數(shù)，具體表達(dá)式參見文獻(xiàn)［7 －8］.

圖1 無人直升機體軸系及受力示意圖

為了考慮操縱系統(tǒng)對控制速率的限制，同時也為了避免控制輸入量出現(xiàn)跳躍或“bang-bang”型控制，使用操縱量的一階導(dǎo)數(shù)代替操縱量作為控制輸入，而將操縱量視為狀態(tài)變量，實現(xiàn)對狀態(tài)方程的擴維.

最后可寫作非線性微分方程的標(biāo)準(zhǔn)形式:

1.2 障礙物約束模型

當(dāng)前障礙物建模大都采用p-norm方法［9］，對于球形或圓柱形障礙物可以取p=2.然而對于長方體形狀的障礙物，p只有取較大值時才能得到較為滿意的近似結(jié)果，同時會導(dǎo)致計算量過大，對于復(fù)雜動力學(xué)方程問題極有可能不收斂.因此針對長方體形狀的障礙物，本文構(gòu)建了一種基于絕對值的約束方式.

假設(shè)障礙物在全局坐標(biāo)系下占據(jù)的空間位置為［X1，X2］，［Y1，Y2］，［Z1，Z2］，則障礙物的尺寸為 ΔX=X2-X1，ΔY=Y2-Y1，ΔZ=Z2-Z1.

假設(shè)(x，y，z)為空間任意一點，構(gòu)建標(biāo)量:

式中

如果C＜0，當(dāng)且僅當(dāng)XX ＜0，YY＜0，ZZ ＜0時成立，即 x∈［X1，X2］，y∈［Y1，Y2］，z∈［Z1，Z2］，表明點(x，y，z)位于障礙物內(nèi)部，否則位于外部.

1.3 非線性最優(yōu)控制模型

無人直升機避障機動飛行的最優(yōu)控制問題可以表述為:在滿足直升機自身性能和避障安全約束的條件下，尋找控制輸入量，使直升機飛行時間最短.為了使控制輸入較為平滑，選擇控制輸入的一階導(dǎo)數(shù)加權(quán)作為目標(biāo)函數(shù)的一部分，具體形式如下:

式中 ωi(i=1，2，3，4)為權(quán)重系數(shù).同時狀態(tài)變量和控制變量滿足動力學(xué)微分方程(9)以及障礙物引起的路徑約束:

式中M為障礙物的個數(shù).

2 求解方法

高斯偽譜法(GPM，Gauss Pseudospectral Method)是近些年來發(fā)展起來的一種直接法［10］，該方法利用全局插值多項式在一系列的配點上近似狀態(tài)量和控制量，相比于其他的直接配點法，具有能夠以較少的節(jié)點獲得較高精度的優(yōu)點.此外GPM離散化得到的NLP(Non-Linear Programming)問題的 KKT(Karush-Kuhn-Tucher)條件與離散的哈密爾頓邊值問題一階最優(yōu)性條件是等價的［11］，能夠保證解的最優(yōu)性.

本文采用高斯偽譜法將連續(xù)無窮維的最優(yōu)控制問題轉(zhuǎn)化為離散有限維的NLP問題，然后利用SQP(Sequential Quadratic Programming)法對其進(jìn)行求解，具體過程如下:

1)時域變換.

由于高斯偽譜法的配點分布在區(qū)間［-1，1］上，需要將初始優(yōu)化問題的時間區(qū)間由t∈［t0，tf］轉(zhuǎn)換到τ∈［-1，1］，故對時間變量t做如下映射變換:

2)微分方程約束轉(zhuǎn)化為代數(shù)約束.

高斯偽譜法需要在一系列的離散點上對狀態(tài)變量和控制變量進(jìn)行全局插值多項式逼近，選擇包括初始點t0，N個LG(Legendre-Gauss)點及終止點tf在內(nèi)的N+2個配點對連續(xù)的狀態(tài)變量和控制變量進(jìn)行離散化插值逼近:

式中Lagrange插值基函數(shù):

式中 τi(i=1，2，…，N)為 LG 點.

對式(13)進(jìn)行求導(dǎo)，可得

式中微分矩陣D∈RN×(N+1)的表達(dá)式為

式中，PN為N階Legendre多項式;i=0，1，…，N.

將式(13)、式(14)及式(17)代入到式(9)，可將動力學(xué)微分方程約束轉(zhuǎn)換為代數(shù)約束，即

式中 k=1，2，…，N.

3)終端狀態(tài)約束的離散化.

雖然高斯偽譜法中的配點包括了終端點τf≡1，但式(13)中并未定義終端狀態(tài)Xf.由于終端狀態(tài)滿足動力學(xué)方程約束，為此需要用Gauss積分來近似，可得

4)離散條件下的性能指標(biāo).

將性能指標(biāo)函數(shù)中的積分項用Gauss積分來近似，可得離散條件下的性能指標(biāo)函數(shù):

采用上述方法，利用高斯偽譜法將連續(xù)最優(yōu)控制問題離散化，轉(zhuǎn)化為NLP問題:以狀態(tài)變量(X0，X1，…，XN)、控制變量(U1，U2，…，UN)、初始時刻t0和終端時刻tf(如果t0和tf未知)作為決策變量，使性能指標(biāo)最小，并滿足配點處狀態(tài)約束，終端約束以及邊界條件

和路徑約束

式中 k=1，2，…，N.

最后得到的NLP問題可以采用序列二次規(guī)劃算法進(jìn)行求解［12］.該算法收斂性好、計算效率高，可以有效處理約束條件.

3 數(shù)值結(jié)果

無人直升機的結(jié)構(gòu)參數(shù)見文獻(xiàn)［7］，飛行性能約束可以轉(zhuǎn)化為對狀態(tài)變量和控制變量的參數(shù)約束，如表1所示.

表1 模型直升機狀態(tài)量和控制量邊界

3.1 三維空間避障機動飛行仿真

為了驗證該方法的有效性，在虛擬的包含多個障礙物的三維空間進(jìn)行仿真，障礙物位置參數(shù)如表2所示.

表2 障礙物位置參數(shù) m

初始時刻直升機在點A(0，0，10)穩(wěn)定懸停，目標(biāo)點B的坐標(biāo)為(95，0，10)，終端狀態(tài)量亦為懸停，目標(biāo)函數(shù)選擇為飛行時間.考慮到時間最優(yōu)所產(chǎn)生的飛行軌跡必然會緊貼著障礙物，而直升機是有尺寸的，所以對障礙物模型進(jìn)行放大，以保證所得軌跡切實可行，此處選擇最小安全距離為飛行器尺寸的2倍.

仿真試驗采用無障礙物飛行軌跡為初始猜測值，設(shè)置了80個插值點，求得的最短飛行時間為12.7481 s，計算耗時 3 s左右.

圖2給出了最優(yōu)軌跡的三維視圖.從中可以看出，生成的機動飛行軌跡較為平滑，而且靈巧地避開障礙物威脅.

圖2 無人直升機最優(yōu)避障軌跡三維視圖

圖3～圖6給出了狀態(tài)量隨時間的變化曲線.從中可以看出，計算結(jié)果均滿足表1中所設(shè)定的性能約束條件.直升機從懸停狀態(tài)開始加速飛行，在左轉(zhuǎn)躲避障礙物1的同時拉升高度躲避障礙物2，為了保持飛行時間最短，前飛速度大約從4.2～8.2 s一直保持最大.飛躍障礙物2后，直升機開始減速并右轉(zhuǎn)躲避障礙物3，最后安全抵達(dá)目標(biāo)點.在整個避障飛行過程中，滾轉(zhuǎn)角先增大后減小，俯仰角和偏航角先減小后增大，且關(guān)于飛行時間的一半近似對稱，主要是由于仿真場景近似對稱.

圖3 空間位置隨時間的變化曲線

圖4 姿態(tài)角隨時間的變化曲線

圖5 飛行速度隨時間的變化曲線

圖6 角速度隨時間的變化曲線

圖7給出了操縱輸入隨時間的變化曲線，可以看出均滿足操縱邊界限制，同時也無出現(xiàn)操縱量急劇變化的情況.

圖7 最優(yōu)軌跡的控制量變化曲線

3.2 避障機動分叉點及分析

考慮到SQP算法為一種局部尋優(yōu)求解策略，障礙物形狀和尺寸對最優(yōu)軌跡有一定的影響.如圖8所示，對于最為常見的長方體障礙物，避障機動動作可以選擇爬升或轉(zhuǎn)彎，單憑經(jīng)驗無法判斷哪種是最優(yōu)的.鑒于此，本文研究了其縱向尺寸H和橫向尺寸L對避障軌跡的影響.如圖9所示，爬升機動和轉(zhuǎn)彎機動飛行時間分別隨著縱向和橫向尺寸增加而增加，縱橫尺寸相同時，轉(zhuǎn)彎機動所需時間較小.通過插值處理即可得到在相同飛行時間下爬升機動和轉(zhuǎn)彎機動所對應(yīng)的障礙物縱向和橫向尺寸，如圖10所示.二者關(guān)系近似為一條斜率為0.5的直線.當(dāng)障礙物的縱橫尺寸落在左上區(qū)域時，轉(zhuǎn)彎機動為最優(yōu)機動動作;反之，爬升機動為最優(yōu)機動動作.

圖8 多種避障機動示意圖

圖9 飛行時間隨障礙物尺寸的變化曲線

圖10 避障機動動作與障礙物尺寸的關(guān)系

4 結(jié)論

本文研究了復(fù)雜低空環(huán)境下無人直升機避障最優(yōu)軌跡規(guī)劃問題.建立了直升機避障機動飛行的非線性最優(yōu)控制模型.與傳統(tǒng)的軌跡規(guī)劃方法相比，該模型考慮了復(fù)雜的直升機動力學(xué)方程及其自身的飛行性能限制等因素，利用絕對值的性質(zhì)將三維障礙物的空間限制轉(zhuǎn)化為不等式約束.仿真結(jié)果表明，該方法能夠以較高的精度生成可行的飛行軌跡，可以為無人直升機自主飛行提供全局最優(yōu)軌跡.此外，針對最為常見的長方體障礙物研究了其尺寸對最優(yōu)軌跡的影響.當(dāng)長方體的縱橫尺寸比約小于1∶2時，垂直爬升為最優(yōu)避障機動;反之，水平轉(zhuǎn)彎為最優(yōu).

References)

［1］ Pettersson P O，Doherty P.Probabilistic roadmap based path planning for an autonomous unmanned helicopter［J］.Journal of Intelligent and Fuzzy Systems，2006，17(4):395 －405

［2］于振中，閆繼宏，趙杰，等.改進(jìn)人工勢場法的移動機器人路徑規(guī)劃［J］.哈爾濱工業(yè)大學(xué)學(xué)報，2011，43(1):50 －55 Yu Zhenzhong，Yan Jihong，Zhao Jie，et.al.Mobile robot path planning based on improved artificial potential field method［J］.Journal of Harbin Institute of Technology，2011，43(1):50 － 55(in Chinese)

［3］ Tsenkov P，Howlett JK，Whalley M，et al.A system for3D autonomous rotorcraft navigation in urban environments［R］.AIAA 2008-7412，2008

［4］梁宵，王宏倫，李大偉，等.基于流水避石原理的無人機三維航路規(guī)劃方法［J］.航空學(xué)報，2013，34(7):1670 －1681 Liang Xiao，Wang Honglun，Li Dawei，et al.Three-dimensional path planning for unmanned aerial vehicles based on principles of stream avoiding obstacles［J］.Acta Aeronautica et Astronautica Sinica，2013，34(7):1670 －1681(in Chinese)

［5］ Hurni M A，Sekhavat P，Ross IM.Autonomous path planning using real-time information updates［R］.AIAA 2008-6305，2008

［6］ Moon J，Prasad JV R.Minimum-time approach to obstacle avoidance constrained by envelope protection for autonomous UAVs［J］.Mechatronics，2011，21(5):861 －875

［7］ Koo T J，Sastry S.Output tacking control design of a helicopter model based on approximate linearization［C］//Proceedings of the 37th IEEE Conference on Decision ＆ Control.Tampa:IEEE，1998:3635－3640

［8］ Gavrilets V，Mettler B，F(xiàn)eron E.Nonlinear model for a small-size acrobatic helicopter［R］.AIAA 2001-4333，2001

［9］ Gatzke B T.Trajectory optimization for helicopter unmanned aerial vehicles［D］.Monterey:Naval Postgraduate School，2010

［10］陳功，傅瑜，郭繼峰.飛行器軌跡優(yōu)化方法綜述［J］.飛行力學(xué)，2011，29(4):1 －5 Chen Gong，F(xiàn)u Yu，Guo Jifeng.Survey of aircraft trajectory optimization methods［J］.Flight Dynamics，2011，29(4):1 － 5(in Chinese)

［11］ Benson D.A Gauss pseudospectral transcription for optimal control［D］.Bostom:Massachusetts Institute of Technology，2004

［12］ Gill P E，Murray W，Saunders M A.SNOPT:an SQP algorithm for large-scale constrained optimization ［J］.Society for Industrial and Applied Mathematics，2005，47(1):99 － 131