管軍,易文俊,常思江,梁振東,呂一品

(1.南京理工大學(xué)瞬態(tài)物理國家重點(diǎn)實(shí)驗(yàn)室,江蘇南京210094;2.南京理工大學(xué)能源與動(dòng)力工程學(xué)院,江蘇南京210094)

某型無人機(jī)三維空間航跡跟蹤控制方法研究

管軍1,易文俊1,常思江2,梁振東1,呂一品1

(1.南京理工大學(xué)瞬態(tài)物理國家重點(diǎn)實(shí)驗(yàn)室,江蘇南京210094;2.南京理工大學(xué)能源與動(dòng)力工程學(xué)院,江蘇南京210094)

研究某型無人機(jī)三維空間航跡跟蹤問題,通過在理想航跡上選取一系列航跡點(diǎn),將航跡跟蹤問題轉(zhuǎn)換為航跡點(diǎn)跟蹤問題。建立了無人機(jī)的動(dòng)力學(xué)模型,將無人機(jī)的速度坐標(biāo)系對準(zhǔn)航跡點(diǎn)所在的理想坐標(biāo)系,使位置跟蹤問題轉(zhuǎn)化為姿態(tài)跟蹤問題。推導(dǎo)了無人機(jī)的制導(dǎo)律,并利用滑模變結(jié)構(gòu)控制理論和反步控制理論分別設(shè)計(jì)了姿態(tài)控制器和速度控制器,姿態(tài)控制器使飛行器的速度矢量方向?qū)?zhǔn)航跡點(diǎn)所在的方向,然后利用速度控制器控制速度的大小使飛行器到達(dá)預(yù)定航跡點(diǎn)。對整個(gè)制導(dǎo)、控制系統(tǒng)進(jìn)行了全系統(tǒng)仿真,仿真結(jié)果表明:所設(shè)計(jì)的控制器具有較高的跟蹤精度,且具有較強(qiáng)的抗干擾能力。

控制科學(xué)與技術(shù);反步控制;滑模變結(jié)構(gòu)控制;無人機(jī);航跡點(diǎn)跟蹤;Lyapunov函數(shù)

DOI:10.3969/j.issn.1000-1093.2016.01.010

0 引言

無人機(jī)在軍用和民用領(lǐng)域均有廣闊的應(yīng)用前景,如何更好地提高無人機(jī)的自主控制能力已經(jīng)成為當(dāng)前國內(nèi)外研究的熱點(diǎn)問題。

目前國內(nèi)外很多學(xué)者已經(jīng)對無人機(jī)的控制問題進(jìn)行了廣泛的研究。Enns等[1]在小型無人機(jī)控制律設(shè)計(jì)中應(yīng)用了非線性動(dòng)態(tài)逆控制,Sieberlin等[2]在整個(gè)飛行包線內(nèi)將動(dòng)態(tài)逆理論與增益調(diào)節(jié)控制方法相結(jié)合設(shè)計(jì)了控制律,并取得良好的效果,劉芳等[3]采用魯棒動(dòng)態(tài)逆方法對自旋導(dǎo)彈控制系統(tǒng)進(jìn)行設(shè)計(jì),陶冶等[4]設(shè)計(jì)了帶死區(qū)變增益PID自適應(yīng)控制律,劉重等[5]將反步法和非線性動(dòng)態(tài)逆結(jié)合起來實(shí)現(xiàn)了某型無人機(jī)的航跡跟蹤控制。除此之外,還有模型預(yù)測控制,H∞魯棒控制,神經(jīng)網(wǎng)絡(luò)自適應(yīng)控制,自適應(yīng)模糊控制等[6-11]。

本文針對固定翼的小型無人機(jī)設(shè)計(jì)了相關(guān)的控制器來達(dá)到對飛行軌跡的準(zhǔn)確跟蹤。對于常規(guī)固定翼的小型無人機(jī),其執(zhí)行機(jī)構(gòu)一般只有副翼舵、俯仰舵、偏航舵和推力裝置,而無人機(jī)在三維空間的運(yùn)動(dòng)通常具有6個(gè)自由度,即獨(dú)立控制變量的個(gè)數(shù)小于系統(tǒng)自由度的個(gè)數(shù),所以固定翼無人機(jī)控制系統(tǒng)是典型的欠驅(qū)動(dòng)控制系統(tǒng)。相比于完全驅(qū)動(dòng)系統(tǒng),欠驅(qū)動(dòng)系統(tǒng)結(jié)構(gòu)簡單,便于進(jìn)行整體的動(dòng)力學(xué)分析和試驗(yàn)。但是由于系統(tǒng)的高度非線性、參數(shù)攝動(dòng)、多目標(biāo)控制要求及控制量受限等原因,欠驅(qū)動(dòng)系統(tǒng)又足夠的復(fù)雜。本文針對欠驅(qū)動(dòng)系統(tǒng)提出了相關(guān)的控制方法。對于航跡跟蹤問題,在給定飛行軌跡中選取一系列的航跡點(diǎn),若能實(shí)現(xiàn)對飛行軌跡上的航跡點(diǎn)進(jìn)行準(zhǔn)確跟蹤,則可認(rèn)為能夠?qū)︼w行軌跡進(jìn)行跟蹤。本文的控制思想可簡單描述為:將無人機(jī)的速度坐標(biāo)系對準(zhǔn)航跡點(diǎn)所在的理想坐標(biāo)系,通俗地講,即控制無人機(jī)的速度方向指向航跡點(diǎn),然后以一個(gè)理想速度去接近航跡點(diǎn),根據(jù)該思想,無人機(jī)在有限的時(shí)間內(nèi)一定能夠到達(dá)航跡點(diǎn)所在的位置,從而實(shí)現(xiàn)對無人機(jī)航跡的跟蹤。

1 系統(tǒng)建模

1.1 定義和相關(guān)說明

例:假設(shè)V1=[v1v2v3]T,則有

1.2 質(zhì)心運(yùn)動(dòng)數(shù)學(xué)建模

根據(jù)文獻(xiàn)[7],飛行器空中質(zhì)心運(yùn)動(dòng)模型可表示為

式中:Vb是飛行器質(zhì)心的速度矢量;是機(jī)體速度;Vb相對于風(fēng)速的相對速度;從機(jī)體系到速度坐標(biāo)系的轉(zhuǎn)換矩陣可表示為

式中:攻角α和側(cè)滑角β可分別表示為

(6)式中:

相對速度在速度坐標(biāo)系下的投影可表示為

假設(shè)風(fēng)速是常量或者變化很小,根據(jù)牛頓第二定律,機(jī)體系的相對加速度可表示為

式中:ρ是空氣密度;S是翼展面積;C(·)是氣動(dòng)力系數(shù);CL=CL0+CLαα.

對(7)式求導(dǎo),可得

(9)式代入(11)式,可得

(13)式為速度變化的動(dòng)力學(xué)模型,在后文設(shè)計(jì)速度控制器的時(shí),只要通過改變推力的大小即可改變總速度的大小。

1.3 繞質(zhì)心轉(zhuǎn)動(dòng)數(shù)學(xué)建模

本文利用機(jī)體系和地理系之間的轉(zhuǎn)動(dòng)四元數(shù)對無人機(jī)的姿態(tài)信息進(jìn)行描述。單位四元數(shù)的數(shù)學(xué)模型可表示為

對姿態(tài)信息的描述同樣還可以利用旋轉(zhuǎn)矩陣進(jìn)行描述,旋轉(zhuǎn)矩陣(姿態(tài)矩陣)Rnb四元數(shù)之間的關(guān)系為

根據(jù)文獻(xiàn)[3]基于四元數(shù)的轉(zhuǎn)動(dòng)動(dòng)力學(xué)模型可表示為

式中:

無人機(jī)角度動(dòng)力學(xué)方程可以寫為

式中:

(20)式中u=[δaδeδr]T為飛行器的3個(gè)舵偏角,作為飛控系統(tǒng)的控制量。并有

本文設(shè)計(jì)姿態(tài)控制器的目的是調(diào)整無人機(jī)的姿態(tài)信息使得其速度坐標(biāo)系對準(zhǔn)理想坐標(biāo)系,理想坐標(biāo)系是理想航跡點(diǎn)所在的坐標(biāo)系,其定義及詳細(xì)說明可見參考文獻(xiàn)[9],定義速度坐標(biāo)系和理想坐標(biāo)系之間的轉(zhuǎn)動(dòng)四元數(shù)為qdw:

qdw有兩個(gè)平衡點(diǎn):這兩個(gè)點(diǎn)雖然數(shù)學(xué)表達(dá)式不同,但具有完全相同的物理意義,即速度坐標(biāo)系和理想坐標(biāo)系的各個(gè)軸所指的方向完全相同,不需要任何轉(zhuǎn)動(dòng)就能使兩個(gè)坐標(biāo)系完全重合。根據(jù)文獻(xiàn)[4],定義一個(gè)誤差四元數(shù):

假設(shè) ?t,有sign(ηdw(t))=sign(ηdw(t)).該假設(shè)可以保證在推導(dǎo)控制器的過程中考慮任意一個(gè)平衡點(diǎn)其結(jié)果都是一樣的。

引理 在假設(shè)1成立的情況下,下述不等式成立:

該證明過程詳見參考文獻(xiàn)[9-10]。

2 制導(dǎo)律設(shè)計(jì)

制導(dǎo)系統(tǒng)的目的是給后續(xù)的控制系統(tǒng)提供理想的參考信息。從本文整體的控制思路來講,文中將位置誤差投影到期望的姿態(tài)信息上,將速度誤差投影到期望的角速度矢量上,所以推導(dǎo)出期望的姿態(tài)信息和期望的角速度信息是本節(jié)的任務(wù)。本文所述控制方法的基本思想是將位置和速度誤差投影到Xd軸上,然后利用發(fā)動(dòng)機(jī)推力使其到達(dá)預(yù)定航跡點(diǎn)。

圖1 地理坐標(biāo)系、機(jī)體坐標(biāo)系和理想坐標(biāo)系的位置關(guān)系Fig.1 The relationship among geographic frame,body frame and desired frame

從(30)式可以看出:將地理坐標(biāo)系中的位置誤差投影到了理想期望的Xd軸上。定義建立地理系和理想坐標(biāo)系之間的旋轉(zhuǎn)四元數(shù),根據(jù)四元數(shù)理論,其旋轉(zhuǎn)角和旋轉(zhuǎn)軸可分別表示為

則期望的旋轉(zhuǎn)四元數(shù)方程可以表示為

期望的旋轉(zhuǎn)矩陣可表示為

有了(33)式或(34)式,即可得到期望的姿態(tài)信息。

為了找到理想角速度,對(30)式求導(dǎo),可推出:

在理想坐標(biāo)系中跟蹤誤差的導(dǎo)數(shù)亦可表示為

(36)式表明速度誤差在Yd和Zd軸的分量為0.由于且

從(37)式可以看出,僅僅在Yd軸和Zd軸具有分量,其物理意義表明當(dāng)求解理想角速度時(shí),可以忽略Xd分量的值,且并不影響最終的結(jié)果。

式中:?表示偽逆,且僅需要期望的俯仰和偏航角速度就能將速度指向期望的方向。通過對(39)式求導(dǎo)可以得到理想坐標(biāo)系中的理想角加速度。如果無人機(jī)能夠跟蹤上述所推導(dǎo)的理想姿態(tài)、角速度和角加速度,那么跟蹤誤差就會(huì)投影到Xd軸上,且在推力矢量的作用下趨近于0,并穿梭在兩個(gè)航跡點(diǎn)之間,從而實(shí)現(xiàn)對軌跡的動(dòng)態(tài)跟蹤控制。

3 速度控制器設(shè)計(jì)

在設(shè)計(jì)速度控制器的過程中,本文設(shè)計(jì)了反步控制器來對速度進(jìn)行控制。記理想速度VTd＞0,速度誤差為令VT-VTd,結(jié)合(13)式推導(dǎo)出

根據(jù)Lyapunov穩(wěn)定性理論,(χ1,z)→(0,0),繼而χ2→0,有 VT→VTd,從而實(shí)現(xiàn)了對速度的控制。

4 姿態(tài)控制器設(shè)計(jì)

本文設(shè)計(jì)姿態(tài)控制器的目的是,調(diào)整無人機(jī)的姿態(tài)使其速度矢量指向航跡點(diǎn)所在的位置。利用滑模變結(jié)構(gòu)控制理論對姿態(tài)進(jìn)行控制。為了不失一般性,設(shè) eq=eq+,Te=Te(eq+).定義滑模面方程[12-13]為

式中:sb是滑模變量;是參考坐標(biāo)系相對于地理坐標(biāo)系的轉(zhuǎn)動(dòng)角速度,該變量的引入可詳見參考文獻(xiàn)[14];γ是正增益系數(shù)。在(50)式左乘轉(zhuǎn)動(dòng)慣量矩陣J,并對其求導(dǎo),將(24)式代入,又有可推導(dǎo)出

5 數(shù)值仿真

本文對無人機(jī)制導(dǎo)與控制系統(tǒng)進(jìn)行了全系統(tǒng)仿真。仿真的初始條件;理想速度信息VTd=40 m/s;路徑點(diǎn)矩陣選為

圖2 推力變化趨勢Fig.2 The change trend of thrust

圖3 3個(gè)舵偏角變化趨勢Fig.3 The change trend of tree deflection angles

圖4 位置跟蹤誤差Fig.4 The position tracking error

圖5 速度跟蹤誤差Fig.5 The velocity tracking error

圖6 角度跟蹤誤差Fig.6 The angle tracking error

當(dāng)機(jī)體位置和路徑點(diǎn)之間的距離小于5 m的時(shí)候切換到下一個(gè)參考點(diǎn)作為控制目標(biāo)。圖2和圖3表示的是控制律隨時(shí)間的變化關(guān)系,分別是推力T和舵偏角μ的變化曲線。圖4和圖5分別是位置ed和速度跟蹤誤差VT-VTd.分析圖4的數(shù)據(jù)可以看出,在航跡點(diǎn)附近誤差基本為0,即系統(tǒng)可以很好地跟蹤目標(biāo)航跡點(diǎn)。圖6和圖7分別是角度誤差eq和角速度誤差。從圖6可以看出,角度誤差很快收斂到0,大約在50 s左右的時(shí)候開始跟蹤第一個(gè)航跡點(diǎn),并大約在65 s左右時(shí)收斂到0,220 s左右的時(shí)候調(diào)整跟蹤第二個(gè)航跡點(diǎn),這時(shí)誤差出現(xiàn)波動(dòng),但很快收斂到0.同理,角速度誤差也表現(xiàn)出同樣的特性。綜上所述,本文所設(shè)計(jì)的控制器,跟蹤精度較高,具有較強(qiáng)的魯棒性,對實(shí)際工程應(yīng)用和理論研究具有一定的參考價(jià)值。

圖7 角速度跟蹤誤差Fig.7 The angular velocity tracking error

6 結(jié)論

本文設(shè)計(jì)了無人機(jī)航跡跟蹤控制器,基于路徑點(diǎn)的軌跡跟蹤,系統(tǒng)實(shí)現(xiàn)較簡單,在工程實(shí)踐中會(huì)表現(xiàn)出較大的優(yōu)越性。針對于欠驅(qū)動(dòng)系統(tǒng),將不能被直接控制的狀態(tài)量轉(zhuǎn)化為其他可被直接控制的狀態(tài)量,進(jìn)而實(shí)現(xiàn)全狀態(tài)控制。推導(dǎo)了制導(dǎo)律,基于Lyapunov穩(wěn)定性理論設(shè)計(jì)了相關(guān)的控制器。仿真結(jié)果表明該系統(tǒng)具有較高的跟蹤精度,具有較強(qiáng)的穩(wěn)定性。

References)

[1]Enns D,Bugajski D,Hendrick R,et al.Dynamic invention-an evolving methodology for flight control design[J].International of Control,1994,59(1):71-91.

[2]Sieberling S,Chu Q P,Mulder J A.Robust flight control using incremental nonlinear dynamic inversion and angular acceleration prediction[J].Jornal of Guidance,Control and Dynamics,2010,33(6):1732-1742.

[3]劉芳,陳萬春.應(yīng)用Lyapunov方法分析自旋導(dǎo)彈動(dòng)態(tài)逆控制器魯棒性[J].北京航空航天大學(xué)學(xué)報(bào),2013,39(1):132-137.LIU Fang,CHEN Wan-chun.Spinning missile dynamic inversion controller robustness analysis using Lyapunov methods[J].Journal of Beijing University of Aeronautics and Astronautics,2013,39(1):132-137.(in Chinese)

[4]陶冶,房建成,盛蔚.一種小型無人機(jī)帶死區(qū)增益PID自適應(yīng)控制器設(shè)計(jì)與實(shí)現(xiàn)[J].自動(dòng)化學(xué)報(bào),2008,34(6):716-721.TAO Ye,FANG Jian-cheng,SHENG Wei.Design and realization of piecewise PID controller with dead zone for micro UAV[J].Acta AutomaticSinica,2008,34(6):716-721.(in Chinese)

[5]劉重,高曉光,符小衛(wèi),等.基于反步法和非線性動(dòng)態(tài)逆的無人機(jī)三維航路跟蹤制導(dǎo)控制[J].兵工學(xué)報(bào),2014,35(12): 2030-2040.LIU Zhong,GAO Xiao-guang,FU Xiao-wei,et al.Three-dimensional path tracking guidance and control for unmanned aerial vehical based on back-stepping and nonlinear dynamic inversion[J].Acta Armamentarii,2014,35(12):2030-2040.(in Chinese)

[6]Khalil H K.Nonlinear systems[M].3rd ed.Upper Saddle River,NJ,US:Preentice Hall,2002.

[7]Wibowo S S.Aircraft flight dynamics control and simulation-using MATLAB and SIMULINK:cases and algorithm,apporach[M].Kuala Lumpur,Malaysia:University of Kuala Lumpur-Malaysian Institute of Aviation Technology,2007.

[8]Egeland O,Gravdahl J T.Modeling and simulation for automatic control[M].Norwegian:Marine Cybernetics,2002.

[9]Kristiansen R.Dynamic synchronization of spacecraft-modeling and coordinated control of leader-follower spacecraft formations [D].Norwegian:Norwegian University of Science and Technology,2008.

[10]Oland E,Kristiansen R.Adapative flight control with constrained actuation[C]//2014 American Control Conference.Portland,Oregon,US:the American Automatic Control Council,2014.

[11]Oland E,Schlanbusch R.Underactuated way point tracking of a fixed-wing UAV[C]//Proceedings of the 2nd IFAC Workshop on Research,Education and Development of Unmanned Aerial Systems.Compiegne,France:IFAC,2013.

[12]Shima T.Sliding-mode control for integrated missile autopilot guidance[J].Journal of Guidance,Control,and Dynamics,2006,29(2):250-260.

[13]Roberts A,Tayebi A.Adaptive position tracking of VTOL UAVs [J].IEEE Transactions on Robotics,2011,27(1):129-142.

[14]Slotine J E,Li W.Adaptive manipulator control:a case study [J].IEEE Transaction on Automatic Control,1998,33(11): 995-1003.

Study of Flight Path Tracking and Control of an UAV in 3D Space

GUAN Jun1,YI Wen-jun1,CHANG Si-jiang2,LIANG Zhen-dong1,LYU Yi-pin1
(1.National Key Laboratory of Transient Physics,Nanjing University of Science and Technology,Nanjing 210094,Jiangsu,China; 2.School of Power and Engineering,Nanjing University of Science and Technology,Nanjing 210094,Jiangsu,China)

A flight path tracking problem is studied for underactuated UAV in 3D space.The question of flight path tracking is transformed into way-point tracking by choosing a serious point on flight path.A mathematic model is established.The speed frame is alined to the desired frame so that position tracking is converted into attitude tracking.The guidance law is also deduced,and the theories of sliding variable structure and back-stepping are used to design the attitude controller and velocity controller.The direction of velocity vector is aligned to the position of a way-point by using the attitude controller.The speed of UAV is controlled by the velocity controller to make the UAV closing to a way-point.The guidance and control systems are simulated.The result shows that the designed controller is perfect in tracking and have a strong robustness.

control science and technology;back-stepping;sliding variable structure;UAV;flight path tracking;Lyapunov function

V249.1

A

1000-1093(2016)01-0064-07

2015-04-16

國家自然科學(xué)基金項(xiàng)目(11472136、11402117);江蘇省普通高校研究生科研創(chuàng)新計(jì)劃項(xiàng)目(KYLX15-0419)

管軍(1987—),男,博士研究生。E-mail:guanjun8710@163.com;易文俊(1970—),男,教授,博士生導(dǎo)師。E-mail:wjy@mail.njust.edu.cn