基于非線性制導(dǎo)的四旋翼軌跡跟蹤控制

陳運(yùn)劍,劉 暢,馬武舉,帥 超

(中國船舶重工集團(tuán)公司 第七一〇研究所，湖北 宜昌 443003)

0 引言

四旋翼是一種能夠垂直起降、空中懸停的無人飛行器，由于其機(jī)動(dòng)靈活、操作簡便、體積輕便等特點(diǎn)，四旋翼飛行器在科研、民用以及軍用領(lǐng)域上得到了廣泛的運(yùn)用[1]。在四旋翼無人機(jī)的相關(guān)研究中，最為引人關(guān)注的是軌跡跟蹤控制問題，也就是四旋翼能夠按照一定的速度沿著預(yù)置的軌跡飛行，執(zhí)行相關(guān)的任務(wù)[2]。四旋翼軌跡跟蹤在民用方面可以應(yīng)用于快遞投送、商業(yè)表演、電力巡檢等用途，在軍用方面可以應(yīng)用于執(zhí)行無人偵察、戰(zhàn)場巡邏等任務(wù)。

軌跡跟蹤一般作為無人機(jī)控制系統(tǒng)的外回路，也就是導(dǎo)引回路，目前常用的算法主要有PID算法、反步法以及非線性制導(dǎo)算法等。文獻(xiàn)[3]研究了PID算法在四旋翼無人機(jī)定點(diǎn)跟蹤控制中的運(yùn)用，通過PID位置控制并逆向求解得到所需的期望姿態(tài)來構(gòu)建控制系統(tǒng)的導(dǎo)引回路。文獻(xiàn)[4]研究了基于反步法的四旋翼無人機(jī)軌跡跟蹤，通過反步法構(gòu)建李雅普諾夫函數(shù)來設(shè)計(jì)無人機(jī)控制率。文獻(xiàn)[5]研究了自適應(yīng)反步法軌跡跟蹤，提高了反步法軌跡跟蹤器的控制精度。文獻(xiàn)[6-7]詳細(xì)介紹了非線性制導(dǎo)算法，并證明了其適用性。文獻(xiàn)[8]研究了非線性制導(dǎo)算法中引導(dǎo)長度的限制問題，并通過大量仿真計(jì)算驗(yàn)證了其正確性。文獻(xiàn)[9]研究了非線性制導(dǎo)算法在固定翼無人機(jī)中的運(yùn)用，并通過自適應(yīng)調(diào)整引導(dǎo)長度來提高軌跡跟蹤的精度。

本文將非線性制導(dǎo)算法運(yùn)用于四旋翼軌跡跟蹤控制，通過構(gòu)建導(dǎo)引回路與控制回路來實(shí)現(xiàn)軌跡跟蹤控制器的設(shè)計(jì)，并通過Simulink構(gòu)建了該軌跡跟蹤控制器模型，實(shí)現(xiàn)了對(duì)二維以及三維復(fù)雜軌跡的跟蹤仿真，分析驗(yàn)證了該控制器能夠很好的跟蹤復(fù)雜軌跡，論證了算法的可行性、正確性。

1 導(dǎo)引回路設(shè)計(jì)

導(dǎo)引回路主要任務(wù)是引導(dǎo)四旋翼按照期望軌跡進(jìn)行飛行，具體通過非線性制導(dǎo)算法以及對(duì)非線性制導(dǎo)算法的補(bǔ)充部分生成四旋翼當(dāng)前的加速度指令，并傳遞給控制回路進(jìn)行姿態(tài)控制。

1.1 非線性制導(dǎo)算法

非線性制導(dǎo)算法的實(shí)現(xiàn)過程如圖1所示。L為導(dǎo)引長度，通過L可以在期望軌跡上選取導(dǎo)引點(diǎn)，實(shí)現(xiàn)過程可以理解為以四旋翼當(dāng)前位置為原點(diǎn)畫半徑為L的圓球，此圓球與期望軌跡的交點(diǎn)即為導(dǎo)引點(diǎn)，隨著時(shí)間變化，導(dǎo)引點(diǎn)沿著期望軌跡向前運(yùn)動(dòng)。在控制周期里，四旋翼通過做圓周運(yùn)動(dòng)沿著圓弧的預(yù)期軌跡飛向?qū)бc(diǎn)，隨著時(shí)間的推移，四旋翼的真實(shí)軌跡逐漸向期望軌跡逼近。圖中，V為四旋翼當(dāng)前速度，R為做圓周運(yùn)動(dòng)的半徑，η為四旋翼與導(dǎo)引點(diǎn)連線方向與速度方向的夾角，a⊥為向心加速度，于是可以得到如下關(guān)系：

L=2Rsinη

(1)

根據(jù)牛頓第二定律與式(1)的幾何關(guān)系可以得到，四旋翼做圓周運(yùn)動(dòng)所需要的垂向加速度，其大小可以表達(dá)如下：

(2)

所需的垂向加速度方向通過方向向量矢量叉乘可以得到，計(jì)算過程如下：

e⊥=eV×eL×eV

(3)

式中，e⊥為垂向加速度方向單位矢量，eV為速度方向單位矢量，eL為四旋翼與導(dǎo)引點(diǎn)連線方向單位矢量。結(jié)合式(2)和式(3)可以得到垂向加速度指令的表達(dá)式，如下：

(4)

由式(4)可以看出，確定了導(dǎo)引長度，知道了四旋翼當(dāng)前的位置、速度，就可以通過非線性制導(dǎo)算法得到當(dāng)前的加速度指令。為了簡化控制過程，在飛行過程中可以控制速度大小的不變，這就需要對(duì)式(4)得到的加速度指令進(jìn)行修正。

圖1 非線性制導(dǎo)算法示意圖

1.2 對(duì)非線性制導(dǎo)算法補(bǔ)充

通過非線性制導(dǎo)算法可以生成垂向加速度指令，理想狀態(tài)下四旋翼的加速度總是垂直于速度方向，于是在飛行過程中可以保持速度的大小不變。但是由于四旋翼各通道的響應(yīng)速度不同，在執(zhí)行加速度指令過程中，會(huì)在速度方向上產(chǎn)生附加加速度，從而導(dǎo)致四旋翼不能保持設(shè)定的速度飛行。因此需要產(chǎn)生速度方向加速度指令對(duì)非線性制導(dǎo)算法得到垂向加速度指令進(jìn)行補(bǔ)充修正。

本算法采用比例控制來產(chǎn)生速度方向加速度指令，其計(jì)算過程如下所示：

a//=KV(VS·eV-V)

(5)

式中，VS為期望速度標(biāo)量，KV為速度修正系數(shù)。結(jié)合式(4)和式(5)可以得到修正后的導(dǎo)引回路加速度指令，表達(dá)式如式(6)所示：

(6)

通過非線性制導(dǎo)算法以及其補(bǔ)充修正設(shè)計(jì)的四旋翼軌跡跟蹤導(dǎo)引回路不僅可以引導(dǎo)四旋翼沿著期望軌跡飛行，也可以控制速度大小為固定期望值。通過式(6)可以發(fā)現(xiàn)，導(dǎo)引回路的輸入為期望軌跡、期望速度，輸出為加速度指令，得到的加速度指令作用于姿態(tài)控制回路引導(dǎo)旋翼按照期望軌跡飛行。

2 控制回路設(shè)計(jì)

圖2展示了四旋翼軌跡跟蹤控制器結(jié)構(gòu)組成，控制回路將導(dǎo)引回路的加速度指令信息進(jìn)行逆向求解得到姿態(tài)控制指令，然后姿態(tài)控制器執(zhí)行姿態(tài)指令，控制四旋翼按照期望加速度飛行。導(dǎo)引回路、控制回路構(gòu)成了軌跡跟蹤控制器的內(nèi)外環(huán)，形成反饋閉環(huán)。

圖2 軌跡跟蹤控制器結(jié)構(gòu)框圖

2.1 逆向求解姿態(tài)指令

由導(dǎo)引回路得到加速度指令不能直接作用于飛行控制器，需要將其逆向求解轉(zhuǎn)化為姿態(tài)控制指令，這就需要建立加速度與姿態(tài)角之間的關(guān)系，其關(guān)系可以由下述表達(dá)式進(jìn)行描述。

(7)

(8)

通過式(8)可以將加速度指令轉(zhuǎn)換為姿態(tài)指令，在求解過程需要知道四旋翼的偏航角，在對(duì)偏航角不作特殊要求情況下為方便計(jì)算可以將其期望值設(shè)置為固定值ψS。由于四旋翼載重能力以及安全裕度的限制，需要對(duì)式(8)的槳葉拉力指令T做進(jìn)一步的限幅處理，處理過程如下：

TS=Lim(T，Tmin，Tmax)

(9)

θS=Lim(θ1，θmin，θmax)

φS=Lim(φ1，φmin，φmax)

(10)

式中，θmin，θmax，φmin，φmax分別為俯仰角、滾轉(zhuǎn)角限幅范圍，所設(shè)計(jì)的控制器中，偏航角為期望值，因此不做限幅處理。

通過上述加速度指令的逆向求解以及限幅處理，可以得到姿態(tài)控制器能夠接受的姿態(tài)控制指令(TS，θS，φS，ψS)。

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

四旋翼姿態(tài)控制器的結(jié)構(gòu)框圖如圖3所示，采用串級(jí)PID作為姿態(tài)控制算法。通過串級(jí)PID算法生成俯仰通道、滾轉(zhuǎn)通道、偏航通道的控制量U2、U3、U4，高度通道控制量U1即為旋翼槳葉拉力指令TS。姿態(tài)控制量與姿態(tài)角的關(guān)系可以用下式描述。

(11)

上式為四旋翼繞質(zhì)心運(yùn)動(dòng)的動(dòng)力學(xué)方程，推導(dǎo)過程見文獻(xiàn)[10]，式中，Jx、Jy、Jz為四旋翼繞機(jī)體軸的轉(zhuǎn)動(dòng)慣量。通過控制U2、U3、U4可以控制四旋翼的姿態(tài)。

圖3 姿態(tài)控制器框圖

四旋翼的動(dòng)力來源為四個(gè)中心對(duì)稱電機(jī)的旋轉(zhuǎn)，其通過改變四個(gè)電機(jī)的轉(zhuǎn)速來產(chǎn)生偏轉(zhuǎn)力矩，從而改變旋翼當(dāng)前的姿態(tài)，對(duì)于“X”字型四旋翼，四個(gè)通道的控制量與電機(jī)的轉(zhuǎn)速關(guān)系可以用下式進(jìn)行描述。

U4=Kd(ω12+ω22-ω32-ω42)

(12)

式中，ω1、ω2、ω3、ω4為四個(gè)電機(jī)轉(zhuǎn)速、d為四旋翼機(jī)體臂長，Kp為旋翼槳葉升力系數(shù)，Kd為槳葉力矩系數(shù)。

四旋翼三個(gè)姿態(tài)通道的控制信號(hào)由串級(jí)PID控制算法計(jì)算得到，以俯仰通道為例，串級(jí)PID控制框圖如圖4所示。

圖4 俯仰通道串級(jí)PID控制框圖

串級(jí)PID控制相對(duì)于單級(jí)PID，其具有更好動(dòng)態(tài)特性，抗干擾性、適應(yīng)性，為了獲得更好的控制性能，選擇串級(jí)PID算法設(shè)計(jì)姿態(tài)控制回路。本文所設(shè)計(jì)的姿態(tài)控制器，外回路控制姿態(tài)角，采用P控制得到期望角速率，內(nèi)回路控制姿態(tài)角速率，采用PD控制輸出期望的姿態(tài)控制量，控制方程如下：

U1=TS

(13)

控制回路將導(dǎo)引回路的加速度指令轉(zhuǎn)換為姿態(tài)指令并進(jìn)行限幅處理，最后通過串級(jí)PID算法計(jì)算輸出四個(gè)通道的控制量，來控四旋翼按照期望加速度飛行，最終效果是四旋翼能夠按照期望軌跡、期望速度飛行。

3 仿真驗(yàn)證

3.1 仿真模型搭建

采用Matlab/Simulink搭建圖2框圖所示的軌跡跟蹤控制器來仿真四旋翼軌跡跟蹤控制器軌跡跟蹤效果。仿真所采用的四旋翼參數(shù)如表1所示。

表1 四旋翼參數(shù)

軌跡跟蹤控制器控制參數(shù)選取如表2所示。導(dǎo)引回路的控制參數(shù)L、KV取為經(jīng)驗(yàn)值，分別為20、0.2;推力與姿態(tài)角限制取所采用的四旋翼模型的真實(shí)值；控制回路中的串級(jí)PID控制器調(diào)節(jié)參數(shù)利用工程試湊法來確定。對(duì)飛行過程中的偏航角不做特殊要求，為簡化仿真過程，本文取期望偏航角為0°。

表2 控制參數(shù)

仿真的輸入為期望軌跡、期望速度等，輸出為實(shí)際的軌跡以及跟蹤過程中的速度、加速度、姿態(tài)角等，通過對(duì)比分析真實(shí)值與期望值來驗(yàn)證所提軌跡跟蹤方法的可行性。本仿真分為二維軌跡與三維軌跡跟蹤仿真。

3.2 二維軌跡跟蹤仿真

假設(shè)四旋翼初始姿態(tài)角均為0，初始位置為原點(diǎn)(0，0，0)，初始速度為(15，0，0)，其所需跟蹤的二維軌跡為水平面內(nèi)的正弦曲線，期望速度為15 m/s，期望偏航角為0°，跟蹤軌跡曲線描述如下：

(14)

四旋翼軌跡跟蹤控制器執(zhí)行式(14)所描述的平面曲線軌跡任務(wù)時(shí)的仿真結(jié)果與相關(guān)參數(shù)如圖5～8所示。由圖5的軌跡仿真結(jié)果可以看出該跟蹤控制器具有很好的跟蹤效果，四旋翼的起始速度方向與期望軌跡所需速度方向不一致，在導(dǎo)引回路作用下，四旋翼的速度方向逐漸與期望軌跡所需速度方向保持一致，航跡也與期望軌跡幾乎重合。由圖6的距離偏差隨時(shí)間變化圖可以看出，除了由起始速度方向不一致導(dǎo)致的起始Y向距離偏差外，在執(zhí)行任務(wù)航跡過程中，Y向距離偏差均維持在±0.6 m以內(nèi)。由圖6的跟蹤速度隨時(shí)間變化關(guān)系可以看出，四旋翼的速度起伏較小，基本保持在15～17 m/s內(nèi)，符合速度期望，速度誤差控制在2 m/s之內(nèi)。

圖5 二維軌跡跟蹤仿真結(jié)果

圖6 跟蹤過程中不同時(shí)刻的速度與距離偏差

圖7 跟蹤過程中不同時(shí)刻的加速度

圖8 跟蹤過程中不同時(shí)刻的姿態(tài)角

圖7為四旋翼執(zhí)行航跡任務(wù)過程中的加速度曲線，可以看出實(shí)際加速度與加速度指令基本重合，部分位置實(shí)際加速度不能達(dá)到加速度指令峰值，從而導(dǎo)致了跟蹤過程中距離誤差與速度誤差。從圖9跟蹤過程中姿態(tài)角變化曲線可以看出，滾轉(zhuǎn)角與俯仰角均沒有超過姿態(tài)角限制(-30°，30°)，而偏航角保持在-2°～2°以內(nèi)，基本符合偏航角的期望0°，所設(shè)計(jì)軌跡跟蹤控制器在滿足姿態(tài)限制要求的前提下，保證了了軌跡跟蹤精度。跟蹤過程中加速度、姿態(tài)起伏變化較劇烈的點(diǎn)發(fā)生在任務(wù)軌跡拐彎處，這符合運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)規(guī)律。

通過上述二維軌跡仿真結(jié)果可以發(fā)現(xiàn)，所設(shè)計(jì)的軌跡跟蹤控制器能夠?qū)φ覐?fù)雜軌跡進(jìn)行精確跟蹤。

3.3 三維軌跡跟蹤仿真

為了驗(yàn)證所設(shè)計(jì)的四旋翼軌跡跟蹤控制器執(zhí)行更復(fù)雜曲線任務(wù)的能力，對(duì)三維曲線軌跡進(jìn)行跟蹤控制仿真。四旋翼的初始條件與3.1節(jié)所述條件相同，其所需跟蹤的軌跡為三維空間里的正弦曲線，可以通過下述曲線方程進(jìn)行描述。

(15)

軌跡仿真結(jié)果以及相關(guān)參數(shù)如圖9～11所示。由圖9的軌跡仿真結(jié)果可以看出所設(shè)的軌跡跟蹤控制對(duì)三維軌跡也有很好跟蹤效果，除了在拐彎處，真實(shí)軌跡與期望軌跡基本重合。由圖10跟蹤過程中速度變化曲線可以看去，跟蹤過程速度維持在15～17 m/s，速度誤差小于2 m/s。由圖10的距離偏差變化曲線可以看出，除了起始0 s、50 s、100 s左右范圍時(shí)間段存在較大的距離偏差，在整個(gè)跟蹤過程中，距離偏差不超過±4 m。在0時(shí)刻范圍處存在較大偏差是期望軌跡所需速度方向與實(shí)際速度方向偏差較大導(dǎo)致的，相當(dāng)于起始誤差。

圖9 三維軌跡跟蹤仿真結(jié)果

圖10 三維軌跡跟蹤過程中的速度與距離偏差

圖11 三維軌跡跟蹤過程中不同時(shí)刻的加速度

結(jié)合分析圖11的加速度曲線，可以看出在50 s、100 s左右范圍時(shí)間段內(nèi)，三個(gè)方向的實(shí)際加速度與加速度指令存在較大的偏差，這就導(dǎo)致了在50 s、100 s時(shí)間段范圍出現(xiàn)較大距離偏差?？梢园l(fā)現(xiàn)在上述兩個(gè)時(shí)間段，四旋翼剛好處于軌跡的波峰處，此時(shí)x、y方向的加速度指令需要有較大的升力，而z方向的加速度指令需要有較小的升力，這種矛盾是四旋翼自身控制方式(升力的分量產(chǎn)生側(cè)向控制力)導(dǎo)致的，控制器執(zhí)行加速指令的過程如式(9)、(10)，對(duì)得到推力指令和姿態(tài)指令進(jìn)行限幅處理，因此此處的加速度指令與實(shí)際加速度存在較大偏差。

通過三維軌跡仿真可以發(fā)現(xiàn)除了受自身控制力限制的飛行段外，所設(shè)計(jì)的軌跡跟蹤器能夠?qū)?fù)雜的三維軌跡進(jìn)行精確跟蹤。

4 結(jié)束語

本文采用補(bǔ)充改進(jìn)的非線性制導(dǎo)算法設(shè)計(jì)了四旋翼軌跡跟蹤控制器的導(dǎo)引回路，采用串級(jí)PID算法設(shè)計(jì)了控制回路。仿真分析結(jié)果表明，該軌跡跟蹤控制器有很好的跟蹤效果，既能跟蹤二維復(fù)雜軌跡，也能跟蹤三維復(fù)雜軌跡；在跟蹤過程中，能維持速度與偏航角在期望值附近；在滿足姿態(tài)角與推力限制條件下也能有很好跟蹤效果。導(dǎo)引長度是本文所提四旋翼軌跡跟蹤控制器的關(guān)鍵控制參數(shù)，其大小對(duì)軌跡跟蹤效果的影響以及其取值范圍是后續(xù)研究的方向。