基于非線性制導的四旋翼軌跡跟蹤控制

陳運劍,劉 暢,馬武舉,帥 超

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

0 引言

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

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

本文將非線性制導算法運用于四旋翼軌跡跟蹤控制，通過構建導引回路與控制回路來實現軌跡跟蹤控制器的設計，并通過Simulink構建了該軌跡跟蹤控制器模型，實現了對二維以及三維復雜軌跡的跟蹤仿真，分析驗證了該控制器能夠很好的跟蹤復雜軌跡，論證了算法的可行性、正確性。

1 導引回路設計

導引回路主要任務是引導四旋翼按照期望軌跡進行飛行，具體通過非線性制導算法以及對非線性制導算法的補充部分生成四旋翼當前的加速度指令，并傳遞給控制回路進行姿態控制。

1.1 非線性制導算法

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

L=2Rsinη

(1)

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

(2)

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

e⊥=eV×eL×eV

(3)

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

(4)

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

圖1 非線性制導算法示意圖

1.2 對非線性制導算法補充

通過非線性制導算法可以生成垂向加速度指令，理想狀態下四旋翼的加速度總是垂直于速度方向，于是在飛行過程中可以保持速度的大小不變。但是由于四旋翼各通道的響應速度不同，在執行加速度指令過程中，會在速度方向上產生附加加速度，從而導致四旋翼不能保持設定的速度飛行。因此需要產生速度方向加速度指令對非線性制導算法得到垂向加速度指令進行補充修正。

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

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

(5)

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

(6)

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

2 控制回路設計

圖2展示了四旋翼軌跡跟蹤控制器結構組成，控制回路將導引回路的加速度指令信息進行逆向求解得到姿態控制指令，然后姿態控制器執行姿態指令，控制四旋翼按照期望加速度飛行。導引回路、控制回路構成了軌跡跟蹤控制器的內外環，形成反饋閉環。

圖2 軌跡跟蹤控制器結構框圖

2.1 逆向求解姿態指令

由導引回路得到加速度指令不能直接作用于飛行控制器，需要將其逆向求解轉化為姿態控制指令，這就需要建立加速度與姿態角之間的關系，其關系可以由下述表達式進行描述。

(7)

(8)

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

TS=Lim(T，Tmin，Tmax)

(9)

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

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

(10)

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

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

2.2 姿態控制器設計

四旋翼姿態控制器的結構框圖如圖3所示，采用串級PID作為姿態控制算法。通過串級PID算法生成俯仰通道、滾轉通道、偏航通道的控制量U2、U3、U4，高度通道控制量U1即為旋翼槳葉拉力指令TS。姿態控制量與姿態角的關系可以用下式描述。

(11)

上式為四旋翼繞質心運動的動力學方程，推導過程見文獻[10]，式中，Jx、Jy、Jz為四旋翼繞機體軸的轉動慣量。通過控制U2、U3、U4可以控制四旋翼的姿態。

圖3 姿態控制器框圖

四旋翼的動力來源為四個中心對稱電機的旋轉，其通過改變四個電機的轉速來產生偏轉力矩，從而改變旋翼當前的姿態，對于“X”字型四旋翼，四個通道的控制量與電機的轉速關系可以用下式進行描述。

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

(12)

式中，ω1、ω2、ω3、ω4為四個電機轉速、d為四旋翼機體臂長，Kp為旋翼槳葉升力系數，Kd為槳葉力矩系數。

四旋翼三個姿態通道的控制信號由串級PID控制算法計算得到，以俯仰通道為例，串級PID控制框圖如圖4所示。

圖4 俯仰通道串級PID控制框圖

串級PID控制相對于單級PID，其具有更好動態特性，抗干擾性、適應性，為了獲得更好的控制性能，選擇串級PID算法設計姿態控制回路。本文所設計的姿態控制器，外回路控制姿態角，采用P控制得到期望角速率，內回路控制姿態角速率，采用PD控制輸出期望的姿態控制量，控制方程如下：

U1=TS

(13)

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

3 仿真驗證

3.1 仿真模型搭建

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

表1 四旋翼參數

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

表2 控制參數

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

3.2 二維軌跡跟蹤仿真

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

(14)

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

圖5 二維軌跡跟蹤仿真結果

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

圖7 跟蹤過程中不同時刻的加速度

圖8 跟蹤過程中不同時刻的姿態角

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

通過上述二維軌跡仿真結果可以發現，所設計的軌跡跟蹤控制器能夠對正弦復雜軌跡進行精確跟蹤。

3.3 三維軌跡跟蹤仿真

為了驗證所設計的四旋翼軌跡跟蹤控制器執行更復雜曲線任務的能力，對三維曲線軌跡進行跟蹤控制仿真。四旋翼的初始條件與3.1節所述條件相同，其所需跟蹤的軌跡為三維空間里的正弦曲線，可以通過下述曲線方程進行描述。

(15)

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

圖9 三維軌跡跟蹤仿真結果

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

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

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

通過三維軌跡仿真可以發現除了受自身控制力限制的飛行段外，所設計的軌跡跟蹤器能夠對復雜的三維軌跡進行精確跟蹤。

4 結束語

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