0 引言

    隨著越來越多的商業(yè)無人機(jī)廠商的興起，四旋翼如今已經(jīng)走進(jìn)了人們的日常生活，在社會(huì)生活的很多方面都發(fā)揮了重要作用，比如航拍、農(nóng)業(yè)、交通檢測(cè)、搶險(xiǎn)救災(zāi)、地形勘測(cè)等領(lǐng)域。四旋翼作為一個(gè)研究平臺(tái)，在科研領(lǐng)域也獲得了廣泛的應(yīng)用^[1-3]，一些研究者將一些先進(jìn)的控制技術(shù)應(yīng)用于四旋翼的飛行控制中，還有一些研究者著眼于使無人機(jī)更智能化，研究了無人機(jī)的視覺導(dǎo)航^[4]、智能飛行等。

    姿態(tài)解算對(duì)于四旋翼的控制有著非常重要的作用。要使飛機(jī)能在空中平穩(wěn)的飛行，精確的姿態(tài)解算不可或缺。常見的姿態(tài)解算算法有互補(bǔ)濾波、卡爾曼濾波以及擴(kuò)展卡爾曼濾波等。EKF雖然計(jì)算復(fù)雜，但精度高，也受到了越來越多人的青睞。本文首先推導(dǎo)了四旋翼的數(shù)學(xué)模型；之后提出了微機(jī)電傳感器的線性數(shù)學(xué)模型，并對(duì)傳感器進(jìn)行了仿真；介紹了擴(kuò)展卡爾曼濾波器的設(shè)計(jì)過程，并在MATLAB仿真模型中驗(yàn)證了濾波的效果；最后在實(shí)際飛機(jī)上進(jìn)行了驗(yàn)證。

1 四旋翼的數(shù)學(xué)模型

    四旋翼飛行器可以在機(jī)體坐標(biāo)系和慣性坐標(biāo)系下分別建立其動(dòng)力學(xué)模型^[5]。通常在機(jī)體坐標(biāo)系中建立它的動(dòng)力學(xué)方程,最主要的原因是傳感器所測(cè)量的值就是相對(duì)于機(jī)體坐標(biāo)系的，所以自然而然要在機(jī)體坐標(biāo)系中建立方程。

    在建立物理模型之前，需要建立兩個(gè)不同的笛卡爾參考坐標(biāo)系——慣性坐標(biāo)系和機(jī)體坐標(biāo)系。本文采用歐拉角的方式描述四旋翼的姿態(tài)。姿態(tài)角就是用來描述兩個(gè)坐標(biāo)系的位置關(guān)系。

1.1 姿態(tài)角的描述

    歐拉角是一種描述剛體相對(duì)于一個(gè)給定參考坐標(biāo)系位置的方式。按照慣例采用的旋轉(zhuǎn)順序是z-y-x，相對(duì)應(yīng)的角度分別為yaw、pitch、roll。3個(gè)基本旋轉(zhuǎn)矩陣的乘積就是兩個(gè)坐標(biāo)系之間的轉(zhuǎn)換矩陣，如式(1)所示^[6]。

1.2 動(dòng)力學(xué)方程

    對(duì)于四旋翼飛行器，在機(jī)體坐標(biāo)系下，使用牛頓-歐拉公式^[7]可得到剛體質(zhì)心的動(dòng)力學(xué)矩陣表達(dá)形式：

    四旋翼飛行器為欠驅(qū)動(dòng)系統(tǒng)，控制輸入量只有4個(gè)，即4個(gè)旋翼的轉(zhuǎn)速。本文選取的4個(gè)變量為3個(gè)姿態(tài)角（橫滾角、俯仰角、偏航角）和飛行高度。還要選擇合適的控制變量，使得動(dòng)力學(xué)方程相對(duì)于控制變量是線性的。                                             

    根據(jù)式(2)，可以得到四旋翼在機(jī)體坐標(biāo)下的數(shù)學(xué)模型^[7]：

2 傳感器模型

    四旋翼上的傳感器為導(dǎo)航和姿態(tài)控制系統(tǒng)提供信息。目前傳感器都采用的是微機(jī)電系統(tǒng)（MEMS）技術(shù)，通常把飛機(jī)上的傳感器合稱為慣性導(dǎo)航單元（IMU）。IMU主要有以下幾部分組成：加速度計(jì)、陀螺儀、磁力計(jì)。

    一個(gè)典型的三軸微機(jī)電傳感器的線性模型^[8]為：

其中，x_i為各軸的輸入值，y_i為輸出值，b_i為在各軸上的偏差，v_i為在軸上的高斯噪聲分布，S_i為標(biāo)量因子，M_ij為軸與軸之間的敏感度。

    式(4)是一個(gè)對(duì)MEMS傳感器建模的一般公式，即陀螺儀、加速度計(jì)、磁力計(jì)以及氣壓計(jì)都可以用這個(gè)公式來表示。下文在MATLAB仿真建模時(shí)，就是根據(jù)上面的數(shù)學(xué)模型設(shè)計(jì)的。

    完整的測(cè)量方程組如式(5)所示，y_a、y_g、y_m分別表示加速度計(jì)、陀螺儀和磁力計(jì)的輸出。

3 EKF姿態(tài)解算

    應(yīng)用標(biāo)準(zhǔn)卡爾曼濾波器的一個(gè)主要的限制是，它要求動(dòng)力學(xué)方程和測(cè)量方程相對(duì)于狀態(tài)變量來說都是線性的。但是在實(shí)際應(yīng)用中這樣的要求通常是不滿足的，所以應(yīng)用標(biāo)準(zhǔn)卡爾曼濾波受到了很大的限制。而擴(kuò)展卡爾曼濾波就是為了克服標(biāo)準(zhǔn)卡爾曼濾波器的這個(gè)缺點(diǎn)而提出的，它能適用于當(dāng)系統(tǒng)和測(cè)量模型是非線性的情況^[9-12]。

    EKF進(jìn)行線性化的操作是通過計(jì)算狀態(tài)矩陣和測(cè)量矩陣的雅克比矩陣來進(jìn)行的。針對(duì)連續(xù)EKF，定義雅克比矩陣的式子為：

    根據(jù)方程組(3)，可計(jì)算它們的雅克比矩陣。

    對(duì)測(cè)量方程進(jìn)行線性化，得到一個(gè)10×10的稀疏矩陣：

其中，J₁為加速度傳感器方程相對(duì)于線速度u、v、w的偏導(dǎo)數(shù)，J₂為加速度傳感器方程相對(duì)于角速度p、q、r的偏導(dǎo)數(shù)，J₃為磁力計(jì)相對(duì)于歐拉角的偏導(dǎo)數(shù)，J_a為標(biāo)量是對(duì)高度變量的偏導(dǎo)數(shù)。計(jì)算的表達(dá)式如方程組(9)所示。

    同理，對(duì)狀態(tài)方程進(jìn)行線性化，結(jié)果也為一個(gè)10×10的稀疏矩陣，具體計(jì)算的各項(xiàng)在這里就不一一列出。

    上面就是通過雅克比矩陣線性化后的狀態(tài)轉(zhuǎn)移矩陣和測(cè)量矩陣，可將新的F和H代入卡爾曼濾波的方程組中。

    EKF的數(shù)學(xué)描述就是在當(dāng)前時(shí)刻估計(jì)的狀態(tài)上，采用泰勒級(jí)數(shù)展開，并且去掉高階項(xiàng)，基本上只取一階的泰勒式子。線性化的方式就是針對(duì)轉(zhuǎn)移矩陣和測(cè)量矩陣，計(jì)算它們的雅克比矩陣。所以它的計(jì)算量比標(biāo)準(zhǔn)卡爾曼濾波大很多，因?yàn)樗獙?shí)時(shí)更新和計(jì)算狀態(tài)轉(zhuǎn)移矩陣和測(cè)量矩陣。

4 仿真實(shí)驗(yàn)

    根據(jù)上面所建立的四旋翼、傳感器和EKF的數(shù)學(xué)模型，在MATLAB上用Simulink搭建其模型。

    所搭建的整體仿真模型如圖1所示，由輸入信號(hào)、PID控制器、四旋翼仿真模型、傳感器仿真模塊（On-board IMU）、EKF濾波器模塊、狀態(tài)顯示等模塊組成。根據(jù)實(shí)驗(yàn)室所搭建的無人機(jī)，進(jìn)行相應(yīng)的估計(jì)，選擇近似的參數(shù)，一些主要參數(shù)：m=1.023 kg，I_xx=I_yy=9×10^-3，I_zz=1.8×10^-2，J_m=3.4×10^-5，L=18 cm。

    當(dāng)四旋翼處于懸停時(shí)，即給定的3個(gè)歐拉角為0，則經(jīng)過傳感器仿真模塊on-board IMU后，得到的傳感器的模擬圖像為圖2所示，因?yàn)閭鞲衅鞲鬏S的數(shù)據(jù)處理方式相同，所以這里只給出了加速度計(jì)X軸的仿真圖。

    經(jīng)過擴(kuò)展卡爾曼濾波后，可以從中解算出方程中的狀態(tài)變量，仿真結(jié)果如圖3～圖5所示。

    由圖像可知，信號(hào)經(jīng)過傳感器仿真模塊后，信號(hào)變“差”了，這正是本設(shè)計(jì)模擬實(shí)際傳感器希望達(dá)到的效果，這種信號(hào)更貼近實(shí)際四旋翼采集的傳感器信號(hào)，只不過這里的噪聲是人為添加的。當(dāng)這種有著噪聲的信號(hào)經(jīng)過EKF濾波后，信號(hào)相對(duì)來說變得干凈了，且能反映出信號(hào)變化的大體趨勢(shì)，并且穩(wěn)定在期望值附近，誤差也較小，表明擴(kuò)展卡爾曼濾波確實(shí)起到了效果，能濾除噪聲，解算出正確的姿態(tài)角。

    在實(shí)驗(yàn)室搭建的無人機(jī)^[13]上進(jìn)行了EKF濾波實(shí)驗(yàn)，測(cè)試無人機(jī)在懸停時(shí)的濾波效果。

    圖6～圖8記錄了無人機(jī)在懸停時(shí)解算出的線速度，可以看出各軸線速度值比較穩(wěn)定，在0值附近上下小幅度浮動(dòng)。

5 結(jié)論

    本文根據(jù)傳感器和EKF的數(shù)學(xué)模型，在MATLAB搭建了它們的仿真模型，對(duì)傳感器和擴(kuò)展卡爾曼濾波進(jìn)行了仿真驗(yàn)證。仿真實(shí)驗(yàn)是假設(shè)四旋翼在懸停狀態(tài)下，即3個(gè)歐拉角都為0。經(jīng)過傳感器模塊后，發(fā)現(xiàn)本來是很光滑的信號(hào)變成了有很多毛刺的信號(hào)。因?yàn)楦鶕?jù)傳感器的數(shù)學(xué)模型，在仿真模塊中加入了延遲、偏差、標(biāo)定因子、各個(gè)軸之間的影響因子等變量，所以信號(hào)就變得不光滑了。然后將信號(hào)送入到EKF模塊，并結(jié)合四旋翼的控制量，解算出在機(jī)體坐標(biāo)系下的狀態(tài)變量。從仿真結(jié)果可以看出，解算出來的狀態(tài)變量變得較為平滑，且大體上都在0附近，符合預(yù)期的值，說明EKF濾波起到了效果。最后也在實(shí)際的無人機(jī)上進(jìn)行了驗(yàn)證。

參考文獻(xiàn)

[1] Li Yibo，Song Shuxi.A survey of control algorithms for quadrotor unmanned helicopter[C].2012 IEEE fifth International Conference on Advanced Computational Intelligence(ICACI).Nanjin，Jiangsu，China：IEEE，2012：365-369.

[2] Zhang Xiaodong，Li Xiaoli，Wang Kang.A survey of modelling and identification of quadrotor robot[C].Abstractand Applied Analysis，2014.

[3] 蔡偉平，胡越黎，楊文榮，等.四旋翼雙環(huán)滑模姿態(tài)控制系統(tǒng)設(shè)計(jì)與仿真[J].電子技術(shù)應(yīng)用，2015，41(7)：150-153.

[4] MEIER L，TANSKANEN P，HENG L.PIXHAWK：A micro aerial vehicle design for autonomous flight using onboard computer vision[J].Autonomous Robots，2012，33(1)：21-39.

[5] MAHONY R，KUMAR V，CORKE P.Modeling，estimation，and control of quadrotor[J].IEEE Robotics & Automation，2012，19(3)：20-32.

[6] 秦永元.慣性導(dǎo)航(第2版)[M].北京：科學(xué)出版社，2014.

[7] BOUABDALLAH S.Design and control of quadrotors with application to autonomous flying[D].Lausanne:Lausanne Polytechnic University，2007.

[8] ASCORTI L.An application of the extended Kalman filter to the attitude control of a quadrotor[D].Milano：Politecnico Di Milano，2013.

[9] 鄒波，張華，姜軍.多傳感器融合的改進(jìn)擴(kuò)展卡爾曼濾波定姿[J].計(jì)算機(jī)應(yīng)用研究，2014，31(4)：1035-1038.

[10] 劉洪劍，王耀南，譚建豪，等.基于加速度變?cè)肼旹KF的無人機(jī)姿態(tài)融合算法[J].2016，30(3)：333-341.

[11] TARHAN M，ALTUG E.EKF based attitude estimation and stabilization of a quadrotor UAV using vanishing points in catadioptric images[J].Journal of Intelligent & Robotic Systems，2011，62(3)：587-607.

[12] MUNGUIA R F，GRAU A.A practical method for implementing an attitude and heading reference system[J].International journal of advanced robotic systems，2014，11(1)：1-12.

[13] 黃建，胡越黎，楊文榮，等.組合導(dǎo)航系統(tǒng)在四旋翼無人機(jī)上的實(shí)現(xiàn)[J].電子技術(shù)應(yīng)用，2015，41(5)：167-170.

作者信息：

金舒燦1，2，胡越黎1，2，3，張  賀1，2

（1.上海大學(xué) 機(jī)電工程與自動(dòng)化學(xué)院，上海200072；2.上海市電站自動(dòng)化技術(shù)重點(diǎn)實(shí)驗(yàn)室，上海200072；

3.上海大學(xué) 微電子研究與開發(fā)中心，上海200072）