何毅杰，艾劍良

(復(fù)旦大學(xué) 航空航天系，上海 200433)

目前對多旋翼(Multi rotor)無人機(Unmanned Aerial Vehicle， UAV)的控制的研究是一個熱點．多旋翼無人機具有機動性好、便攜、廉價等特點，可以承擔(dān)多樣化的飛行任務(wù).在執(zhí)行某些特殊的飛行任務(wù)時，無人機的質(zhì)量、重心、慣性矩會發(fā)生變化，這會降低無人機控制系統(tǒng)的穩(wěn)定性，甚至導(dǎo)致控制系統(tǒng)發(fā)散．對動力學(xué)參數(shù)的辨識可以幫助調(diào)整控制系統(tǒng)的參數(shù)，從而適應(yīng)這種變化．在辨識的過程中，狀態(tài)信號所含的噪聲對辨識結(jié)果的精確度有著重要的影響．為了控制小型無人機的成本，所使用的廉價傳感器往往不夠精確，噪聲比較大.為了保證參數(shù)辨識的精確度，需要合理地降低噪聲的影響．本文所用的互補濾波器(Complementary filter)將高頻噪聲和低頻噪聲結(jié)合，最大程度地消除誤差、保留有效數(shù)據(jù)．另外，本文注意到，參數(shù)辨識模型中的狀態(tài)向量含有輸出信號的微分信號，而用一步差分近似求得的微分信號在精度上有先天的不足，故本文采用了跟蹤微分器(Tracking Differentiator， TD)來更精確地取得輸出信號的微分信號．仿真結(jié)果表明，采用這些方法可以比較好地提高參數(shù)辨識的精確度.

1 參數(shù)辨識的基本概念

參數(shù)辨識就是假設(shè)系統(tǒng)輸入u(t)和輸出y(t)已知[1]，求系統(tǒng)的傳遞函數(shù)G(s)，即通過采集系統(tǒng)的輸入輸出數(shù)據(jù)，研究確定系統(tǒng)數(shù)學(xué)模型的理論和方法．一個連續(xù)時間的動態(tài)系統(tǒng)一般可用高階微分方程來描述(離散系統(tǒng)用差分方程來描述)，通過引進一些中間變量(狀態(tài)變量)，可以化為一些1階微分方程(差分方程)，這個表示系統(tǒng)輸入輸出關(guān)系的方程組稱為狀態(tài)空間模型.

系統(tǒng)辨識的4個要素是數(shù)據(jù)、模型類、準則、優(yōu)化方法.具體來說，就是基于一種準則，采用某種優(yōu)化方法，在一類模型中找到與數(shù)據(jù)最吻合的模型.無人機動力學(xué)模型以物理定律為依據(jù)，因此是先驗的.需要辨識的參數(shù)包括: 無人機的轉(zhuǎn)動慣量、質(zhì)量、重心的位置，旋翼的升力系數(shù)、阻力系數(shù).考慮到飛行過程中參數(shù)變化的情況，故采用在線辨識的形式.

2 四旋翼無人機的動力學(xué)模型

為了辨識無人機的動力學(xué)參數(shù)，首先要建立起四旋翼無人機的動力學(xué)模型.

先建立機體坐標系，確定原點位置及各個坐標軸的方向: 以重心G為原點，機頭方向為x軸，垂直機身向下為z軸，側(cè)向為y軸，如圖1所示．然后可以據(jù)此定義歐拉角(即無人機各姿態(tài)角)的符號: 俯仰角θ以抬頭為正，偏航角ψ以向右為正，滾轉(zhuǎn)角φ以向右為正.

圖1 四旋翼無人機機體坐標系的示意圖Fig.1 Schematic diagram of body coordinate system of quadrotor UAV

基于機體坐標系的建立方法，推導(dǎo)出6自由度動力學(xué)方程．將無人機認為剛體，忽略彈性形變的影響.

根據(jù)質(zhì)心運動定理[2]:

(1)

慣性系下，質(zhì)心的運動方程為

(2)

質(zhì)心的動量矩定理[2]:

(3)

(4)

(5)

兩個坐標系下角速度的坐標變換公式為

(6)

令式(6)兩邊對時間t求導(dǎo)，得:

(7)

機體坐標系下的合外力矩表達式:

(8)

其中：kT是單個旋翼的拉力系數(shù)；kM是單個旋翼的力矩系數(shù).

以上各式就是無人機一般運動的全部動力學(xué)方程．本模型忽略空氣阻力的影響．α，β如圖1所示，實際上是飛機的幾何中心t在以重心G為原點的機體坐標系中的坐標，可以表示重心的位置．z軸坐標與動力學(xué)模型無關(guān)，不需辨識.

3 辨識算法

第2節(jié)建立的6自由度模型是一個線性參數(shù)系統(tǒng)，意味著它可以寫成如下形式:

y(t)=φ(t)θ+ξ(t).

(9)

其中:y(t)是系統(tǒng)輸出；ξ(t)是干擾噪聲；φ(t)是由系統(tǒng)的輸入輸出數(shù)據(jù)構(gòu)成的狀態(tài)向量；θ是待辨識的參數(shù)向量．這樣的形式稱為最小二乘格式，因為這種形式的系統(tǒng)易于被最小二乘法辨識[1].最小二乘法作為一種凸優(yōu)化方法[3]，最大的優(yōu)點就是求得的極小點必定是全局極小點，不會陷入局部最優(yōu)．因此，線性參數(shù)系統(tǒng)優(yōu)先選擇使用最小二乘法來辨識[4].

最小二乘法的基本形式如下: 令

(10)

優(yōu)化目標是極小化F(θ).假設(shè)fi(θ)=φiθ-yi,i=1,2，…,m，則

(11)

在實際應(yīng)用中，飛行器的模型參數(shù)不會保持不變．為了應(yīng)對這種變化，參數(shù)辨識和控制需要同時進行，因此要對飛行器的參數(shù)做在線辨識，故本文采用最小二乘法的遞推形式:

(12)

根據(jù)式(5)和(8),可以將動力學(xué)模型改寫成最小二乘格式:

(13)

根據(jù)式(13)，即可應(yīng)用遞推最小二乘法來辨識參數(shù).

4 跟蹤微分器

適當(dāng)選取函數(shù)f，令

(14)

跟蹤微分器的輸出信號本質(zhì)上是對輸入信號的微分的一種近似．相對于差分算法，跟蹤微分器能夠以產(chǎn)生少量的偏差為代價，抑制噪聲的放大效應(yīng)．此外，與差分算法相比，跟蹤微分器對突變信號的跟蹤比較平滑，誤差比較?。櫸⒎制鞯男Ч饕cR的取值有關(guān)．R太大，噪聲的放大效應(yīng)反而增強；R太小，則跟蹤信號的偏差過大．根據(jù)輸入信號中所含噪聲的方差大小，為R取到恰當(dāng)?shù)闹?，則x1可以準確地跟蹤輸入信號，x2可以準確地跟蹤輸入信號的微分信號，并且相對于差分算法，微分器對噪聲的放大效應(yīng)得到了抑制.

5 互補濾波算法

測量多旋翼無人機飛行狀態(tài)的元件主要包括陀螺儀和加速度計．陀螺儀輸出姿態(tài)角的角速度信號，而加速度計輸出飛機的加速度信號，然后通過積分得到姿態(tài)角、位移和速度信號．由于測量儀器本身的誤差、外部環(huán)境的擾動誤差以及模型誤差，這些數(shù)據(jù)不能準確反映無人機的真實運動狀態(tài)，由此造成的測量結(jié)果和真實值的差異稱為噪聲．其中，姿態(tài)角及其1階、2階微分信號的噪聲對參數(shù)辨識的精確度影響最大.

從實驗的結(jié)果來看，加速度計產(chǎn)生的是高頻噪聲，而陀螺儀產(chǎn)生的是低頻噪聲．由于加速度計的測量原理，它的測量值與姿態(tài)角有換算關(guān)系，因此，可以用加速度計的信號來修正陀螺儀的信號，這就是互補濾波器[5].

互補濾波器由一個低通濾波器和一個高通濾波器組成，且它們的傳遞函數(shù)和為1.這樣的濾波器可以確保有用的信息不會損失，所以又稱作無損濾波器[6].

互補濾波器的構(gòu)造如下[7-8]:

假設(shè)有兩個信號y1=x+μ1，y2=x+μ2，其中：x為信號內(nèi)容；μ1，μ2為噪聲．構(gòu)造兩個濾波器G1(s)，G2(s)，且令G1(s)+G2(s)=1，則

(15)

其中：Y1(s)，Y2(s)，X(s),μ1(s),μ2(s)分別是y1,y2,x,μ1,μ2經(jīng)拉氏變換后的表達式.

(16)

則

(17)

分析濾波器的幅頻特性，由于G1(s)和G2(s)分別是高通和低通濾波器，而截止頻率都是kp(單位為rad/s)，因此適合對一組分別帶低頻噪聲和高頻噪聲的信號進行濾波．因為G1(s)作用于陀螺儀信號(低頻噪聲)，而G2(s)作用于加速度計信號(高頻噪聲)，所以這樣的結(jié)構(gòu)是合理的.

在參數(shù)辨識中，需要用到姿態(tài)角和它的1階、2階微分信號．由于微分器對噪聲在頻域上的低頻部分(Ω<1 rad/s)有抑制效應(yīng)，高頻部分(Ω>1 rad/s)有放大效應(yīng)，而積分器則相反．因此，對于姿態(tài)角信號的濾波，應(yīng)當(dāng)偏向高頻部分，即讓kp更大，而對于1階、2階微分的濾波，應(yīng)當(dāng)偏向低頻部分，即讓kp更?。侠碚{(diào)整kp的值，可以在高、低頻噪聲之間取得平衡，有效地降低噪聲的方差，取得最佳的濾波效果.

6 計算實例

本算例運用Simulink建立如上文所描述的四旋翼無人機動力學(xué)模型[9]，配置微分器和濾波器，并為待辨識的各個參數(shù)賦值，所賦的值綜合考慮了3D建模軟件的計算結(jié)果和實際測量的無人機參數(shù)．Simulink仿真模塊的示意圖如圖2所示.

圖2 Simulink仿真模塊的示意圖Fig.2 Schematic diagram of Simulink simulation module

設(shè)計輸入使得飛機做出機動動作，并給輸出數(shù)據(jù)加上噪聲，然后用遞推最小二乘法進行估算．計算仿真中的有色噪聲可以將白噪聲通過一個成形濾波器得到．為盡量真實地還原噪聲，將白噪聲的采樣頻率設(shè)置為10 000 Hz，成形濾波器的z變換式如下:

可以計算出有色噪聲的均值為0.00，均方差為0.02.

配置跟蹤微分器的參數(shù)．令R取到合適的值，那么微分器的輸出信號趨近于輸入信號的微分信號，并且抑制了噪聲的放大效應(yīng)．而a應(yīng)當(dāng)接近1但不為1，使得微分器是非線性的．為降低微分器的超調(diào)，根據(jù)經(jīng)驗公式，令b=3.1(a-0.5)+1.17.因此取參數(shù)a=1.75，b=5.045，R=50 000.

配置互補濾波器的參數(shù)．對于姿態(tài)角信號，以高頻噪聲為主，取較大的kp，令kp=5；對于1階、2階微分信號，高頻噪聲經(jīng)由微分器放大，不應(yīng)再用來修正低頻噪聲，故令kp=0.

參數(shù)辨識不必對飛行軌跡有過高的要求，而是要盡量激發(fā)出飛行器的各種模態(tài)，使得所有待辨識參數(shù)的相關(guān)信息都包含在輸出數(shù)據(jù)當(dāng)中．本文采取“多模態(tài)、大幅度”的原則來設(shè)計飛行動作，并采用PID(Proportional integral derivative)狀態(tài)反饋控制器進行控制．為加大震蕩幅度，只保留PID控制器的比例環(huán)節(jié)，并且使得比例系數(shù)k較大，取k=100.采樣頻率為20 Hz，總仿真時間為20 s.

為了最大限度地降低噪聲的影響，為參數(shù)辨識設(shè)計了3種典型模態(tài)，分別對應(yīng)3次仿真飛行，得到3組仿真數(shù)據(jù)．具體如下:

模態(tài)1 前10 s飛機上升到5 m高度并懸停，后10 s飛機以3 m/s的速度向右飛行.

模態(tài)2 前10 s飛機上升到5 m高度并懸停，后10 s飛機以3 m/s的速度向前飛行.

模態(tài)3 前10 s飛機上升到5 m高度并懸停，后10 s飛機繞z軸旋轉(zhuǎn)4周.

各組飛行仿真的數(shù)據(jù)如圖3～圖8所示.

圖3 模態(tài)1無人機的位置變化Fig.3 Location change of UAV in mode 1

圖4 模態(tài)1無人機的姿態(tài)角變化Fig.4 Attitude angle change of UAV in mode 1

圖5 模態(tài)2無人機的位置變化Fig.5 Location change of UAV in mode 2

圖6 模態(tài)2無人機的姿態(tài)角變化Fig.6 Attitude angle change of UAV in mode 2

圖7 模態(tài)3無人機的位置變化Fig.7 Location change of UAV in mode 3

圖8 模態(tài)3無人機的姿態(tài)角變化Fig.8 Attitude angle change of UAV in mode 3

在典型模態(tài)下，各個參數(shù)與輸入輸出數(shù)據(jù)的相關(guān)性有大小之分，此時各個參數(shù)的辨識準確度受噪聲的影響也各不相同．相關(guān)性越大，辨識準確度受噪聲的影響就越小．為了盡量準確地辨識出所有參數(shù)，我們采取分步辨識的方法:

設(shè)已辨識的參數(shù)向量為θd，待辨識的參數(shù)向量為θr，那么可以將參數(shù)模型式(9)改寫成:

y=φdθd+φrθr.

(18)

(19)

對式(19)應(yīng)用遞推最小二乘法，即可辨識出余下的參數(shù).

具體辨識步驟如下:

第1步 在模態(tài)1的數(shù)據(jù)中辨識Ix，Ixy，α，β，kT，在模態(tài)2的數(shù)據(jù)中辨識Iy，在模態(tài)3的數(shù)據(jù)中辨識Iyz，Ixz；

第2步 在模態(tài)2的數(shù)據(jù)中辨識kM；

第3步 在模態(tài)3的數(shù)據(jù)中辨識Iz.

辨識結(jié)果如表1所示.

表1 參數(shù)辨識值與所賦實際值的對照

7 結(jié) 語

從上述仿真結(jié)果可以看到，所較?。渲蠭xy，Iyz的誤差略微偏大．參數(shù)辨識結(jié)果的誤差應(yīng)當(dāng)與該參數(shù)在線性參數(shù)模型中對姿態(tài)角及其微分信號所帶的噪聲相對敏感有關(guān)．仿真結(jié)果表明，對于多旋翼無人機的參數(shù)辨識而言，如何處理姿態(tài)角和其1階、2階微分信號及所含的噪聲，對參數(shù)辨識的精確度具有顯著的影響，故而參數(shù)辨識對傳感器的精度要求很高．與此同時，運用包括跟蹤微分器、互補濾波器在內(nèi)的方法處理姿態(tài)角和其1階、2階微分信號及所含的噪聲，可以有效地提高參數(shù)辨識的精確度．另外，研究注意到，在噪聲不可避免的情況下，對輸入和輸出信號的選取也對參數(shù)辨識的精確度有很大的影響．一般來說，設(shè)計辨識飛行實驗的時候要盡量避免運動姿態(tài)的耦合，并采集運動幅度較大時段的飛行數(shù)據(jù)，這對參數(shù)辨識非常有利，可大大降低噪聲的影響．在未來的研究中，可以考慮將有色噪聲的成形濾波器參數(shù)也作為辨識的對象，將噪聲與信號分離開來，從而進一步提高參數(shù)辨識的精確度．