0 引言

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

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

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

    四旋翼飛行器可以在機體坐標系和慣性坐標系下分別建立其動力學模型^[5]。通常在機體坐標系中建立它的動力學方程,最主要的原因是傳感器所測量的值就是相對于機體坐標系的，所以自然而然要在機體坐標系中建立方程。

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

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

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

1.2 動力學方程

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

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

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

2 傳感器模型

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

    一個典型的三軸微機電傳感器的線性模型^[8]為：

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

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

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

3 EKF姿態(tài)解算

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

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

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

    對測量方程進行線性化，得到一個10×10的稀疏矩陣：

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

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

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

    EKF的數(shù)學描述就是在當前時刻估計的狀態(tài)上，采用泰勒級數(shù)展開，并且去掉高階項，基本上只取一階的泰勒式子。線性化的方式就是針對轉(zhuǎn)移矩陣和測量矩陣，計算它們的雅克比矩陣。所以它的計算量比標準卡爾曼濾波大很多，因為它要實時更新和計算狀態(tài)轉(zhuǎn)移矩陣和測量矩陣。

4 仿真實驗

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

    所搭建的整體仿真模型如圖1所示，由輸入信號、PID控制器、四旋翼仿真模型、傳感器仿真模塊（On-board IMU）、EKF濾波器模塊、狀態(tài)顯示等模塊組成。根據(jù)實驗室所搭建的無人機，進行相應(yīng)的估計，選擇近似的參數(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。

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

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

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

    在實驗室搭建的無人機^[13]上進行了EKF濾波實驗，測試無人機在懸停時的濾波效果。

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

5 結(jié)論

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

參考文獻

[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].電子技術(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] 秦永元.慣性導航(第2版)[M].北京：科學出版社，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].計算機應(yīng)用研究，2014，31(4)：1035-1038.

[10] 劉洪劍，王耀南，譚建豪，等.基于加速度變噪聲EKF的無人機姿態(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] 黃建，胡越黎，楊文榮，等.組合導航系統(tǒng)在四旋翼無人機上的實現(xiàn)[J].電子技術(shù)應(yīng)用，2015，41(5)：167-170.

作者信息：

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

（1.上海大學 機電工程與自動化學院，上海200072；2.上海市電站自動化技術(shù)重點實驗室，上海200072；

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