0 引言

    隨著越來(lái)越多的商業(yè)無(wú)人機(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)用于四旋翼的飛行控制中，還有一些研究者著眼于使無(wú)人機(jī)更智能化,，研究了無(wú)人機(jī)的視覺(jué)導(dǎo)航^[4],、智能飛行等。

    姿態(tài)解算對(duì)于四旋翼的控制有著非常重要的作用,。要使飛機(jī)能在空中平穩(wěn)的飛行,，精確的姿態(tài)解算不可或缺。常見(jiàn)的姿態(tài)解算算法有互補(bǔ)濾波,、卡爾曼濾波以及擴(kuò)展卡爾曼濾波等,。EKF雖然計(jì)算復(fù)雜，但精度高,，也受到了越來(lái)越多人的青睞,。本文首先推導(dǎo)了四旋翼的數(shù)學(xué)模型,；之后提出了微機(jī)電傳感器的線性數(shù)學(xué)模型,，并對(duì)傳感器進(jìn)行了仿真；介紹了擴(kuò)展卡爾曼濾波器的設(shè)計(jì)過(guò)程,，并在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)角就是用來(lá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è)公式來(lái)表示。下文在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)變量來(lái)說(shuō)都是線性的,。但是在實(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)行線性化的操作是通過(guò)計(jì)算狀態(tài)矩陣和測(cè)量矩陣的雅克比矩陣來(lái)進(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)在這里就不一一列出,。

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

    EKF的數(shù)學(xué)描述就是在當(dāng)前時(shí)刻估計(jì)的狀態(tài)上,，采用泰勒級(jí)數(shù)展開(kāi),，并且去掉高階項(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)室所搭建的無(wú)人機(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)過(guò)傳感器仿真模塊on-board IMU后,，得到的傳感器的模擬圖像為圖2所示,，因?yàn)閭鞲衅鞲鬏S的數(shù)據(jù)處理方式相同，所以這里只給出了加速度計(jì)X軸的仿真圖,。

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

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

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

    圖6～圖8記錄了無(wú)人機(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)過(guò)傳感器模塊后,，發(fā)現(xiàn)本來(lái)是很光滑的信號(hào)變成了有很多毛刺的信號(hào)。因?yàn)楦鶕?jù)傳感器的數(shù)學(xué)模型,，在仿真模塊中加入了延遲,、偏差、標(biāo)定因子,、各個(gè)軸之間的影響因子等變量,，所以信號(hào)就變得不光滑了。然后將信號(hào)送入到EKF模塊,，并結(jié)合四旋翼的控制量,，解算出在機(jī)體坐標(biāo)系下的狀態(tài)變量。從仿真結(jié)果可以看出,，解算出來(lái)的狀態(tài)變量變得較為平滑,，且大體上都在0附近，符合預(yù)期的值,，說(shuō)明EKF濾波起到了效果,。最后也在實(shí)際的無(wú)人機(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的無(wú)人機(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)在四旋翼無(wú)人機(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é) 微電子研究與開(kāi)發(fā)中心，上海200072）