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

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

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

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

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

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

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

1.1 無人直升機(jī)飛行動(dòng)力學(xué)模型

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

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

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

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

圖1 無人直升機(jī)體軸系及受力示意圖

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

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

1.2 障礙物約束模型

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

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

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

式中

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

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

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

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

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

2 求解方法

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

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

1)時(shí)域變換.

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

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

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

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

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

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

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

式中，PN為N階Legendre多項(xiàng)式;i=0，1，…，N.

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

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

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

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

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

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

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

和路徑約束

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

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

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

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

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

3.1 三維空間避障機(jī)動(dòng)飛行仿真

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

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

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

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

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

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

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

圖3 空間位置隨時(shí)間的變化曲線

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

圖5 飛行速度隨時(shí)間的變化曲線

圖6 角速度隨時(shí)間的變化曲線

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

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

3.2 避障機(jī)動(dòng)分叉點(diǎn)及分析

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

圖8 多種避障機(jī)動(dòng)示意圖

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

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

4 結(jié)論

本文研究了復(fù)雜低空環(huán)境下無人直升機(jī)避障最優(yōu)軌跡規(guī)劃問題.建立了直升機(jī)避障機(jī)動(dòng)飛行的非線性最優(yōu)控制模型.與傳統(tǒng)的軌跡規(guī)劃方法相比，該模型考慮了復(fù)雜的直升機(jī)動(dòng)力學(xué)方程及其自身的飛行性能限制等因素，利用絕對(duì)值的性質(zhì)將三維障礙物的空間限制轉(zhuǎn)化為不等式約束.仿真結(jié)果表明，該方法能夠以較高的精度生成可行的飛行軌跡，可以為無人直升機(jī)自主飛行提供全局最優(yōu)軌跡.此外，針對(duì)最為常見的長(zhǎng)方體障礙物研究了其尺寸對(duì)最優(yōu)軌跡的影響.當(dāng)長(zhǎng)方體的縱橫尺寸比約小于1∶2時(shí)，垂直爬升為最優(yōu)避障機(jī)動(dòng);反之，水平轉(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)人工勢(shì)場(chǎng)法的移動(dòng)機(jī)器人路徑規(guī)劃［J］.哈爾濱工業(yè)大學(xué)學(xué)報(bào)，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］梁宵，王宏倫，李大偉，等.基于流水避石原理的無人機(jī)三維航路規(guī)劃方法［J］.航空學(xué)報(bào)，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