本發(fā)明屬于無人機技術(shù)領(lǐng)域，具體涉及一種基于互補濾波算法和串級PID的四旋翼飛行器設(shè)計方法。

背景技術(shù)：

近年來，隨著電子科學(xué)技術(shù)、先進材料技術(shù)、無線通信技術(shù)、自動化技術(shù)、數(shù)字信號處理技術(shù)等的進步，小型四旋翼低空無人飛行器得到了迅速發(fā)展，在軍事和民用領(lǐng)域具有廣闊的應(yīng)用前景。和傳統(tǒng)的直升機相比，它有著自身的優(yōu)勢：當前后兩個旋翼逆時針旋轉(zhuǎn)，而左右兩側(cè)的旋翼順時針旋轉(zhuǎn)時，旋翼自身產(chǎn)生的扭矩將相互抵消。則尾部旋翼和旋翼傾斜問題可以被忽略。但小型四旋翼飛行器的運動涉及到三個坐標軸上的轉(zhuǎn)動，即俯仰角，橫滾角和偏航角，同時又涉及到三個坐標軸上的移動，共包含六個自由度，而控制系統(tǒng)由四個帶旋翼的電機組成，是一個欠驅(qū)動系統(tǒng)，因而飛行控制系統(tǒng)的設(shè)計有著較大的難度。

隨著微控制器的快速發(fā)展，意法半導(dǎo)體基于高性能、低成本、低功耗的要求設(shè)計了ARM Cortex-M3內(nèi)核，STM32系列處理器就是基于此內(nèi)核設(shè)計。其中增強型系列時鐘頻率可達72Mhz，具有同類產(chǎn)品中最高性能，為進行復(fù)雜的姿態(tài)控制算法提供了條件。

在無人機研究中，微小型無人機的技術(shù)要求更高更復(fù)雜，因此發(fā)展相對緩慢，上世紀90年代末才研制出第一個飛行樣機。由于其飛行速度相對較低、尺寸較小，因此小型無人機的空氣動力學(xué)原理會更加復(fù)雜，有很多技術(shù)問題都無法用現(xiàn)有的空氣動力學(xué)原理來解釋。微小型無人機在低速飛行時，機翼荷載和機體慣性都比較小，很容易受到不穩(wěn)定氣流的干擾，因此就要求微小型無人機的控制系統(tǒng)要更加靈活，同時擁有更高的精度。

四軸飛行器的研究包含了多種交叉學(xué)工科，涉及到微機電系統(tǒng)(MEMS)技術(shù)、片上(SOC)技術(shù)、高效能源技術(shù)、微動力系統(tǒng)技術(shù)等。

技術(shù)實現(xiàn)要素：

本發(fā)明針對現(xiàn)有技術(shù)的不足，提供了一種基于互補濾波算法和串級PID的四旋翼飛行器設(shè)計方法。

本發(fā)，包括基于互補濾波算法的姿態(tài)融合算法和基于串級PID的控制算法；

飛行器的運動控制系統(tǒng)的核心在于姿態(tài)的解算和控制。首先需要建立一個由互相垂直的三個坐標軸組成的慣性參考系，而后由傳感器來采集參考系各坐標軸方向上的角速度和加速度數(shù)據(jù)，進而計算出姿態(tài)信息。其中，加速度計測量的加速度數(shù)據(jù)可以通過與地面坐標系的轉(zhuǎn)換矩陣，計算出姿態(tài)。陀螺儀測量的角速度信息可以通過積分獲得角度信息。但兩個傳感器分別受限于自己的動態(tài)特性和靜態(tài)特性上的不足，分別會產(chǎn)生高頻噪聲和長期的靜態(tài)漂移。因此，對兩者的數(shù)據(jù)通過一個互補濾波器來進行融合計算，可以得到長期有效的姿態(tài)信息。

本發(fā)明的有益效果：自動控制系統(tǒng)中，PID是最為典型的控制算法之一，得到廣泛應(yīng)用。傳統(tǒng)的單環(huán)PID算法控制精度有限，可通過設(shè)計一個串級PID算法來增加系統(tǒng)穩(wěn)定性。串級PID算法以角速度環(huán)為內(nèi)環(huán)，角度環(huán)為內(nèi)環(huán)，內(nèi)環(huán)反饋直接由角速度傳感器獲取，避免了誤差的引入，而外環(huán)的輸出直接作為內(nèi)環(huán)的輸入。有效提高了控制精度和系統(tǒng)的穩(wěn)定性。

附圖說明

圖1為機體坐標系。

圖2為參考坐標系。

圖3為數(shù)據(jù)融合算法流程圖。

圖4為單環(huán)增量式PID控制。

圖5為串級PID控制器原理圖。

圖6為飛行器系統(tǒng)的軟件流程圖。

具體實施方式

以下結(jié)合附圖對本發(fā)明作進一步說明。

在對四旋翼飛行器進行控制之前，首先要進行機體姿態(tài)的解算。而機體的方向，角度，速度等信息都需要通過一個坐標系來準確的描述。以機體的質(zhì)心為原點，機頭方向為xb軸正方向確立載體坐標系B:[xb,yb,zb]，將地面的全局坐標系來作為系統(tǒng)的參考坐標系L:[xl,yl,zl]，機體飛行范圍相對較小，可忽略地球曲率，通過兩坐標系之間的轉(zhuǎn)換可得到機體的姿態(tài)信息。描述機體姿態(tài)的三個歐拉角，即俯仰角(Pitch)，滾轉(zhuǎn)角(Roll)，偏航角(Yaw)分別用θ，φ，ψ表示，機體坐標系如圖1所示。

機身的轉(zhuǎn)動可以分解為依次繞zl軸轉(zhuǎn)動ψ角，繞yl軸轉(zhuǎn)動θ角，繞xl軸轉(zhuǎn)動φ角的三次旋轉(zhuǎn)，由此，可以得到各向量從地面坐標系的機體坐標系的映射關(guān)系矩陣。

第一步繞zl軸旋轉(zhuǎn)，得到關(guān)于ψ角的映射矩陣：

第二步繞yl軸旋轉(zhuǎn)，得到關(guān)于θ角的映射矩陣：

第三步繞xl軸旋轉(zhuǎn)，得到關(guān)于φ角的映射矩陣：

由以上三個映射矩陣均為正交矩陣，相乘即可得出參考坐標系與機體坐標系的映射矩陣：

同時可以得出從機體坐標系到參考坐標系的反向映射矩陣：

四元常用于描述空間方位和運動，是一個矢量，由四個元素組成，分別為該矢量的方向和轉(zhuǎn)動角度的函數(shù)，因此可以用四元數(shù)來描述機體坐標系繞一個矢量轉(zhuǎn)過某一角度的單次旋轉(zhuǎn)。

單位四元數(shù)q的表示如上式，q0，q1，q2，q3為四個元素，并且

式中cos(βx),cos(βy),cos(βz)表示矢量q的方向余弦，即與地面參考坐標系三個坐標軸夾角的余弦值，相當于(μx/μ)(μy/μ)(μz/μ)。μ為角矢量，大小為μ，相當于α，μx，μy，μz是它的三個分量。機體坐標系以μ為旋轉(zhuǎn)軸，轉(zhuǎn)動角度μ后可與參考坐標系重合。

四元數(shù)和歐拉角之間的轉(zhuǎn)換公式分別是：

映射矩陣Rt的四元數(shù)表述為：

四旋翼飛行器的系統(tǒng)誤差主要有兩個方面，一是靜態(tài)誤差，該部分包含傳感器的固有誤差和放置引起的偏置誤差；二是動態(tài)誤差，該部分產(chǎn)生的主要原因是飛行過程中會產(chǎn)生一定的震動，由于傳感器的動態(tài)特性不足而引入誤差。

MEMS傳感器的固有誤差是指當傳感器處于靜態(tài)條件下時，輸出存在一定范圍內(nèi)的隨機變化。MEMS傳感器的數(shù)據(jù)手冊中一般會給出一個噪聲密度參數(shù)來描述固有誤差，一般可通過該參數(shù)來對固有誤差進行修正。

運動控制系統(tǒng)通常需要建立一個由三個相互垂直的坐標軸組成的慣性參考系，該慣性參考系給MEMS傳感器提供了方向參考。理想情況下，傳感器每個軸的方向會與參考系的坐標軸完全一致，圍繞一個坐標軸的旋轉(zhuǎn)只會引起傳感器中對應(yīng)軸向的輸出而不會引起其他變化。但是由于工藝等條件限制，傳感器坐標軸與參考系無法完全對準，圍繞一個坐標軸的旋轉(zhuǎn)將使另外的坐標軸也產(chǎn)生輸出。因此陀螺儀的對準誤差包含連個方面，即與參考系中另外兩軸的位置偏差。以圖2的參考坐標系為例，θXZ表示陀螺儀的X軸相對于坐標系Z軸的角度偏差，當繞Z軸旋轉(zhuǎn)時，該角度偏差引起的X軸上的誤差可以由下式定義：

ω_GX＝ω_ZR×sin(θ_XZ)

MEMS運動傳感器的對準誤差一般包括彼此相關(guān)的兩種類型，包括軸與封裝之間的對準誤差和軸與軸之間的對準誤差。軸與封裝間的對準誤差是由封裝的機械特性決定的，若系統(tǒng)不能在初始過程中對誤差進行校正，這一誤差將會引入到系統(tǒng)的總體誤差當中。另外由于焊接等機械加工工藝的限制，慣性測量單元在放置于系統(tǒng)上時，同樣會造成軸與系統(tǒng)之間的對準誤差。對系統(tǒng)封裝的機械特性進行一個容差的評估，會對這些對準誤差的修正提供參考。

而當MEMS慣性測量單元應(yīng)用于飛行器控制時，其在動態(tài)情況下的誤差將成為系統(tǒng)誤差的主要組成部分。陀螺儀具有可靠的動態(tài)響應(yīng)，在短時間內(nèi)可以提供準確的測量數(shù)據(jù)，但由于上述偏置誤差的存在，隨著時間的積累，其測量值隨積分產(chǎn)生的誤差會不斷累加，對系統(tǒng)產(chǎn)生極大影響，因此，對于系統(tǒng)姿態(tài)的測量還應(yīng)引入三軸加速度計來對三軸陀螺儀的誤差進行修正，加速度計所測得的數(shù)據(jù)具有長期穩(wěn)定的特性，無積累誤差，但由于其動態(tài)特性不佳，在飛行器的機身震動影響下，測得的數(shù)據(jù)在短期內(nèi)可能會存在一定的波動。因此獲取準確的姿態(tài)數(shù)據(jù)需要對兩者的測量值進行數(shù)據(jù)融合。

對于陀螺儀和加速度計的數(shù)據(jù)融合，常見的方法有硬件和軟件兩種，其中軟件進行數(shù)據(jù)融合可以通過卡爾曼濾波或者互補濾波來實現(xiàn)，考慮到互補濾波具有計算量小，處理速度快的特點，本設(shè)計中對數(shù)據(jù)融合采用互補濾波的算法來實現(xiàn)。

三軸加速度計是通過測量重力加速度在機體坐標系三個坐標軸的投影大小，然后通過與地面參考坐標系的轉(zhuǎn)換矩陣計算得到的。但加速度計測得的數(shù)據(jù)容易受到機身震動帶來的影響而產(chǎn)生誤差，但該誤差并不會隨時間積累，因此具有較好的靜態(tài)特性。因此對加速度計的數(shù)據(jù)處理常采用低通濾波來消去短時間內(nèi)的噪聲信號，得到長時間內(nèi)的可靠信號。而陀螺儀的動態(tài)特性較好，短期內(nèi)可通過對角速度的積分來測得角度，但在較長周期內(nèi)陀螺儀誤差會積累產(chǎn)生漂移，當機身姿態(tài)變化時，受加速度的影響，也會產(chǎn)生一定的漂移。因此對陀螺儀的數(shù)據(jù)采用高通濾波處理，濾除長時間內(nèi)積分產(chǎn)生的漂移。數(shù)據(jù)融合算法流程圖如圖3所示。

用互補濾波來融合角度的公式為

angle＝A×(angle+gyro×dt)+B×(acc)

式中A、B為濾波器的兩個參數(shù)，A具有低通濾波特性，B具有高通濾波特性，并且A+B＝1。angle代表角度，gyro代表陀螺儀輸出的角速度，與積分周期dt相乘得到角度，acc表示加速度測得的角度。由上式可計算互補濾波器的增益K為：

自動反饋控制系統(tǒng)中，PID控制器應(yīng)用最為廣泛。PID控制算法無需復(fù)雜的數(shù)學(xué)建模，通過對系統(tǒng)偏差的比例(P)，積分(I)，微分(D)來進行控制。早期的四旋翼飛行器常采用單環(huán)增量式PID控制，僅采用角度信息作為反饋，原理如圖4所示。

單環(huán)PID控制過程中，首先用期望角度減去當前角度得到偏差。比例項直接由偏差與比例系數(shù)相乘得到。由于角度的微分為角速度，可直接由陀螺儀的測量得到，與微分系數(shù)相乘即可得到微分項?？偪刂屏坑杀壤椇臀⒎猪椣嗉拥玫健８目刂扑惴ê唵斡行?，但只考慮了角度信息為反饋，控制精度不佳。

為了提高控制的精度，增加系統(tǒng)的穩(wěn)定性，在單環(huán)PID控制算法中加入角速度環(huán)，形成串級PID控制器，原理如圖5所示。

飛行器串級PID算法以角速度控制器作為內(nèi)環(huán)，以角度控制器為外環(huán)計算出期望角速度，作為內(nèi)環(huán)的輸入信號，陀螺儀采集到的角速度直接作為反饋信息，有效避免了外界干擾造成的影響。由兩個控制器來控制飛行器，有效增強了穩(wěn)定性，提高了角度變化靈敏度，使得飛行器具有更強的適應(yīng)能力。

整個飛行器系統(tǒng)的軟件流程圖如圖6所示。

飛行器的主控芯片采用STM32F103C8T6，無實時操作系統(tǒng)，因此程序主要依靠定時器來不斷執(zhí)行循環(huán)操作，查詢各個中斷條件，通過嵌套中斷來實現(xiàn)整體功能。

首先是對系統(tǒng)各個模塊進行初始化處理操作，檢測系統(tǒng)各個模塊是否正常。之后由于傳感器可能存在偏置產(chǎn)生的誤差，需要進行校正。結(jié)合之前的分析，偏置誤差產(chǎn)生的原因可能來自傳感器自身工藝限制帶來的軸間對準誤差，也可能因焊接放置時軸與機身坐標系沒有嚴格對準產(chǎn)生誤差，若不進行校正，則偏置誤差會引入之后的計算對系統(tǒng)控制造成影響。校正偏置需要約兩秒種的時間，此時系統(tǒng)不斷讀取各個軸向上的數(shù)據(jù)并通過滑動平均濾波得到偏差值，存儲各個軸向的偏差值并在之后的數(shù)據(jù)讀取中減去偏差值來校正偏置誤差。

之后進入100hz的主循環(huán)過程，在該循環(huán)中，系統(tǒng)完成的任務(wù)有傳感器數(shù)據(jù)讀取，濾波，融合，并進行PID控制來維持機身穩(wěn)定。這里對傳感器進行的濾波主要是對加速度計進行一次滑動平均濾波，以減少因傳感器的動態(tài)特性不足帶來的噪聲信號?；瑒悠骄鶠V波每讀取一次數(shù)據(jù)便可以更新一次數(shù)據(jù)，具有實時性，且計算量和空間占用量較小。而后經(jīng)過上述的互補濾波算法來完成層對濾波后的加速度計數(shù)據(jù)和陀螺儀數(shù)據(jù)的融合處理。計算當前角度與目標角度的偏差，通過雙環(huán)PID算法來控制機身達到目標姿態(tài)。

對于遙控器發(fā)來的控制信息捕獲通過一個50hz的循環(huán)來完成，在該循環(huán)中獲取遙控器控制信息后，講數(shù)據(jù)發(fā)送給處理器來解析目標姿態(tài)信息，并結(jié)合傳感器數(shù)據(jù)融合得到的當前角度，通過雙環(huán)PID算法計算控制量，達到目標姿態(tài)。

系統(tǒng)通過以上兩個循環(huán)操作，便可實現(xiàn)對飛行器的穩(wěn)定控制功能。