1、文章編號：1001-9944(2013)11-0001-05基于卡爾曼濾波的四旋翼飛行器懸停控制研究吳友強1，紀浩1，鄒力涵2（1.武漢大學動力與機械學院，武漢430072；2.西安電子科技大學電子工程學院，西安710071）摘要：針對單一傳感器測量四旋翼飛行器姿態角不準確的缺點，通過建立懸停狀態下的四旋翼飛行器動力學模型，構建了四旋翼姿態測量硬件系統，利用理論分析和實驗的方法對四旋翼飛行器物理參數進行了測量，最后利用卡爾曼濾波算法將加速度計和陀螺儀測量的姿態角進行有效的融合，并設計了PID控制器，建立了四旋翼飛行器懸停狀態姿態角的模型。仿真結果顯示，設計的基于卡爾曼濾波的PID控制器滿足系統

2、要求。關鍵詞：四旋翼飛行器；懸停；卡爾曼濾波；PID中圖分類號：TP13文獻標志碼：AResearchonQuadrotorHoveringPIDControlBasedonKalmanFilteringWUYou-qiang1，JIHao1，ZOULi-han2（1.SchoolofPowerandMechanicalEngineering，WuhanUniversity，Wuhan430072，China：2.SchoolofElectronicEn-gineering，XidianUniversity，Xian710071，China）Abstract：Basedonasinglesen

3、sorforaircraftinhoverattitudemeasurementinaccuratefaultsofthequadrotor，throughquadrotordynamicsmodelinhovercondition，constructingofthequadrotorattitudemeasurementhardweresystem，hoverstate，onquadrotor，physicalparametersweremeasuredusingthemethodoftheoreticalanalysisandex-periments，theaccelerometerand

4、gyroscopemeasuredattitudeeffectivefusionusingKalmanfilteringalgorithm，aPIDcontrollerwasdesigned，builtfourrotorcraftinhoverattitudemodel，thesimulationresultsshowthedesignofthePIDcontrollerbasedonKalmanfiltercanmeetthesystemrequirements.Keywords：quadrotor；hovering；Kalmanfiltering；PID四旋翼飛行器因其結構簡單、相對載荷能

5、力強、垂直起降和機動性能好等優點，被當做了一種流行的無人駕駛飛行平臺，并引起了商業和科學研究的極大興趣1。本文以四旋翼飛行器懸?？刂茷槟繕?，利用歐拉-朗格朗日方程建立了理想懸停狀態下四旋翼飛行器的動力學模型，構建了四旋翼姿態測量硬件系統，利用卡爾曼濾波融合算法對加速度計和陀螺儀輸出數據進行了融合處理，設計了四旋翼飛行器懸停狀態下姿態角的PID控制器，并對卡爾曼濾波器的動態性能和穩定性進行了仿真。收稿日期：2013-06-04；修訂日期：2013-06-21作者簡介：吳友強（1988），男，在讀碩士研究生，研究方向為無人機控制系統設計。自動化與儀表1四旋翼飛行器系統結構四旋翼飛行器以TI公司的數

6、字信號處理器TMS320F2812作為主控制器，負責讀取和處理傳感器輸入的四旋翼飛行器姿態數據，結合控制目標，利用PID控制器，輸出特定占空比的PWM波對電機調速以達到懸?？刂疲?50MHz的時鐘頻率有效地保證了數據處理的實時性；采用低成本MEMS慣性測量單元（IMU）對四旋翼飛行器姿態角進行測量，IMU包括一個三軸的加速度計ADXL345和一2013(11)1個三軸陀螺儀L3G4200D，通過對IMU輸出的處理，得到四旋翼飛行器空間姿態角，由于單一的傳感器測量姿態角誤差較大，利用卡爾曼濾波對加速度計和陀螺儀的輸出值進行有效的數據融合處理，得到高精度的姿態角。四旋翼飛行器硬件系統框圖如圖1所示

7、。ADXL345加速度計的值為0g，Z軸輸出的值為1g，姿態角（，）可通過下式計算出，其中Ax、Ay和Az分別為加速度計各個軸相對于地球重力場g的測量值3。arctan（Ax/姨Ay+Az）×180/（1）L3G4200D陀螺儀（2）arctan（Ay/Az）×180/對于陀螺儀L3G4200D的輸出值為三個軸的旋轉角速率，通過對角速率的積分即可計算出特定時間內各個軸轉過的角度。即測量的角度表達式為IIC接口nRF9054*PWMTMS320F2812調速器UART電機（t）=乙（t）dt（3）式中：（t）代表各個軸輸出的角速率值，rad/s；（t）為一段時間內的角度值，r

8、ad。盡管陀螺儀L3G4200D各軸輸出角速率（t）的數學解析表達式，但通過實測獲取了大量數據，對這些數據處理后，得到各軸轉過的角度。主控單元DSP2812通過IIC接口從陀螺儀讀取的數據為離散化數據，通過設置DSP2812定時器中斷，每隔固定時間讀取陀螺儀輸出的角速率值，將這些值反復累加，就得到固定時間內的角度值，通過這種方式也可得到四旋翼飛行器的姿態角（，）。圖1四旋翼飛行器硬件系統框圖Fig.1Blockdiagramofquadrotorhardwaresystem1.1DSP2812系統采用美國TI公司的32位定點高性能數字信號處理器TMS320F2812作為核心運動控制器，該芯片成

9、本低、功耗小、處理速度快和外設資源豐富等優點2。四旋翼飛行器的控制原理是利用控制器輸出PWM的波形控制無刷直流電機的輸出功率，實現對電機轉速的控制，電機轉動再拖動螺旋槳產生向上的升力。由于在整個控制過程中已有一定的滯后性，所以系統的實時性要求高，DSP2812當選用內核供電電壓為1.9V時，其時鐘頻率高達150MHz，指令的平均執行時間約為6.67ns，可有效滿足四旋翼飛行器實時性要求，實現在線姿態估算和數據處理。另外，為實現四旋翼飛行器能夠良好的運行且豐富的底層外設資源可與其他外部設備通信或控制，對硬件資源的要求包括與上位機通訊的數字接口、控制電機的PWM波形輸出接口以及與所選用的傳感器能夠

10、有效的連接。1.3無線傳輸本文選用nRF905作為無線通信核心模塊，它自身集成了收發器、晶振、運放和數字接口，方便與外部主控制器通信。無線通信主要負責上位機和四旋翼飛行器之間的通信，上位機發送相關指令到四旋翼飛行器，飛行器接收指令。同時四旋翼飛行器也可以將實時的姿態角發送到上位機監控。在懸停狀態姿態角PID調試過程中，通過無線通訊，將PID參數發送到四旋翼飛行器主控板，并觀測其相關參數值，得到有效的PID參數。1.2慣性測量單元四旋翼飛行器上的慣性測量單元包括一個MEMS三軸的加速度計ADXL345和一個MEMS三軸陀螺儀L3G4200D，它們都是以IIC數字接口主控單元DSP2812進行數據

11、傳輸，MEMS傳感器具有成本低、體積小和功耗低等優點。慣性測量單元主要負責四旋翼飛行器姿態角數據的測量，加速度計和陀螺儀直接的測量輸出并不是姿態角，而是對測量值進行相關處理后才能得到姿態角。本文研究的四旋翼飛行中，假設四旋翼飛行器機體坐標系和加速度計坐標系以及陀螺儀坐標系重合。對于加速度計2懸停狀態動力學模型四旋翼飛行器控制系統中涉及到的角矢量、位置矢量和姿態角等多種變量，為了能清楚地描述四旋翼飛行器的動力學模型中變量間的關系，選用合適的坐標系和定義合適的坐標很有必要，合適的坐標能夠簡化系統狀態方程的描述，方便系統的分析和研究。圖2為四旋翼飛行器結構模型。文獻4介紹了四旋翼飛行器在運動時引進的

12、兩個坐標系，即慣性坐標系和機體坐標系；推導了兩個坐標系之間的轉換余弦矩陣；并在慣性坐標系和機體坐標系中，利用歐拉-拉格朗日方程建立了AutomationInstrumentation2013(11)ADXL345，機體姿態角從初始狀態（0，0，0）變化到（，）時，初始狀態加速度計的X軸和Y軸輸出f2f1不適合長時間單獨工作；加速度計靜態響應好，能準確提供靜態角度，但受動態加速度影響較大，不適合跟蹤動態角度運動。為實現加速度計和陀螺儀優勢互補，本文利用卡爾曼濾波來融合陀螺儀和加速度計的輸出信號，補償陀螺儀的漂移誤差和加速計的動態誤差，為系統提供最優姿態角輸入。利用卡爾曼濾波器定義了一種提供離散時

13、間狀態過程的預測狀態的方法，定義的線性狀態方程狀態空間模型為6f3Of4圖2四旋翼飛行器結構模型Fig.2Quadrotorairframe四旋翼飛行器懸停狀態的動力學模型。根據本文的懸停狀態下四旋翼飛行器質量分布均勻，忽略科里奧利力、空氣阻力和陀螺效應對四旋翼飛行器的影響。四旋翼飛行器懸停狀態下的動力學模型為xk=Axk-1+Buk-1+wk-1（5）咬Jx咬Jy（4）式中：xk為利用陀螺儀對當前狀態的估算值；A為前一狀態xk-1和后一狀態xk的狀態轉移矩陣；u為隨意控制輸入（若沒有則為0）；B為輸入相關矩陣；W為陀螺儀測量過程中的過程噪聲，當W無限逼近于咬咬QJz咬z0，意味著過程噪聲W為

14、高斯白噪聲N（0，Q），Q為過程噪聲的協方差矩陣，反之，W為有色噪聲。測量模型定義為fi=14im（coscos）-g式中：（，）為四旋翼飛行器的歐拉角滾轉角、俯仰角和偏航角；（，）分別為歐拉角（，）的外部轉矩；（JX，JY，JZ）為系統各軸的轉動慣量；m為機體質量；g為重力加速度；fi為各個旋翼產生的升力，i=1，2，3，4。（6）zk=Hxk+vk式中：H為測量相關狀態矩陣；zk為加速度計輸出的觀測值，當加速度計的觀測噪聲V逼近于0，意味著高斯白噪聲N（0，R），R為觀測噪聲協方差矩陣，反之，V為有色噪聲。基于卡爾曼濾波的PID控制結構圖如圖3所示。ydPIDu過程噪聲w3基于卡爾曼濾波的

15、PID控制單一的傳感器精度可滿足要求，但噪聲對其干G（s）yv測量噪聲vKalman濾波器ye擾無法有效消除，當出現噪聲干擾時，傳感器的數據輸出精度會受到影響和迅速發生改變。為得到更好的數據輸出，傳感器數據融合是一種簡單有效的方法，數據融合是利用數據估計的方法逼近真實值。本文利用卡爾曼濾波估算方法實現數據融合，這種方法可方便地利用C語言在嵌入式控制器中實現加速度計和陀螺儀姿態角數據融合。數據處理過程，包括數據獲取、傳感器輸入融合、姿態估算和PID閉環校正。從傳感器獲取的數據首先通過平均濾波器減小振動噪聲，然后再校正和處理?？柭鼮V波從加速度傳感器和陀螺儀獲取的角度數據融合處理，以確保姿態估算的

16、精度5。圖3基于卡爾曼濾波的PID控制結構圖Fig.3PIDcontrolstructurebasedonKalmanfilter本文將陀螺儀讀數過程中的過程噪聲為有色噪聲，陀螺儀測量噪聲為白色噪聲時，通常使用擴大狀態空間使過程噪聲為k狀態向量的一部分7。假設過程噪聲k滿足以下關系式8：（7）k=Lk-1+k-1式中，L為陀螺儀隨機漂移的AR模型，當k=0時，過程噪聲為高斯白噪聲。擴大狀態空間后的狀態空間方程為（8）Xkk-1+k-1+Wk-1（9）zk=BH0BXk+vkA0xkB其中：=；Xk=；=；Uk-1=B；uk-10B0Lwk0wk-1。Wk-1=k-13.1卡爾曼濾波陀螺儀雖動態

17、性能好，能提供瞬間動態角度變化，不受加速度變化的影響，但由于其本身固有的特性、溫度、積分過程的影響，存在累積漂移誤差，自動化與儀表BBBBBBB2013(11)通過上式分析可得到陀螺儀和加速度傳感器的狀態空間模型為測量，對于懸停狀態，主要是對比目標值姿態角和輸入值的大小，排除外界干擾的擾動，在所研究的k-1Xk=1-dt0dtU+󰀁󰀂WX+󰀁󰀂󰀁󰀂0L11k-1k-1（10）懸停狀態中有（，）=0。設計的PID控制器姿態角（，）控制，對于姿態角（，）的控制，參考動力學模型可知，分別由u2和u3決定，

18、所以其PID控制器的設計如下：t（11）zk=󰀁10󰀂Xk+vkT式中：Xk=󰀁；Wk-1為陀螺儀過程噪聲；dt為采b󰀂樣周期；zk為通用加速度計的間接測量角度值；vk為利用加速度計測量角速度值的測量噪聲?？柭鼮V波對數據的處理通常包括預測和更新兩個過程。預測過程主要為通過前一刻狀態估算現在狀態的過程，更新過程為通過前一刻的預測值和現在的觀測值確定狀態估測。對于四旋翼飛行器來說，姿態預測的問題主要是在任意離散時間過程處理中，利用陀螺儀提供的角速度值和加速度提供的相對加速度值，獲取系統實時的最優姿態角。不同的姿態估算方法有不同性能評

19、判標準，卡爾曼濾波則是以系統估計的最小協方差為標準。預測狀態方程：u2=Kp2（d-）+Kd2（）+Ki2（d-）d（17）dt0t乙u3=Kp3（d-）+Kd3d（d-）+Ki3（d-）d0乙（18）式中：Kpi，Kdi和Kii分別為PID控制器的比例系數、微分系數和積分系數；和分別為系統輸出角度值；d和d分別為系統的目標角度值，目標值一般通過自動規劃或者人為設置，它和時間沒有函數關系，可以視為常數，因此微分誤差可推導為觶de=d（y-y）=-dyd（19）xk|k-1Axk+Buk-1+k+1Pk|k-1APk|k-1AT+Q更新狀態：（12）（13）（14）（15）（16）對于姿態平衡控

20、制來說，角速率可直接取陀螺儀修正角速率，不用通過對角度微分獲得，有效避免了隨機噪聲的干擾以及微分引起的超調和誤差。同時，當四旋翼飛行器處于懸停狀態時，則近似的K=Pk|k-1H（HPk|k-1H+R）TTxk|kxk|k-1+K（Zk-Hxk|k-1）Pk|k=（I-KH）Pk|k-1d=0，d=0，所以姿態角PID控制又可表示為tu2=Kp2（-）+Kd2x+Ki2（-）d0t式中：xk|k-1為上一時刻狀態預測值；xk為這一時刻的最優值；Pk|k-1為上一狀態預測值xk|k-1的協方差；Pk|k為這一時刻狀態最優值xk|k的協方差；Q為系統過程噪聲協方差；R為系統測量噪聲協方差；K為卡爾曼

21、增益；I為單位矩陣。在預測步驟中卡爾曼濾波器預測下一個狀態和誤差協方差P，當每一步傳感器信息可用后，再次進行卡爾曼濾波，狀態X和誤差協方差P被計算出在糾正步驟中。完全的卡爾曼濾波的實現是通過循環方式持續跟蹤陀螺儀偏置和估算姿態，達到慣性傳感器工作在最優性能。每次循環中，按照預測和更新過程，比較濾波器系統的實時更新值和觀測到的預測值，這種所謂差異稱為“更新”。它是通過每個循環中計算出的卡爾曼增益K去衡量的，卡爾曼增益被作為更新狀態估計模型的輸入。另外，卡爾曼增益是決定過程誤差和測量誤差的最優平衡。值為乙（20）u3=Kp3（-）-Kd3y+Ki3（-）d乙（21）3.3實驗結果四旋翼飛行器懸停姿

22、態平衡實驗平臺中，濾波周期dt為0.001s，參考式（10）和式（11）系統參數取A=󰀁1-0.00101.003󰀂󰀁B=0.0010󰀂H=󰀁10󰀂卡爾曼濾波是一種循環遞推的算法，必須定義初始狀態P0和x0，文獻9和10中對P0和x0的取值進行了相關分析，對于P0只要不為0，對初始方差矩陣的取值對濾波效果影響很小，都能很快收斂，對于x0根據理想的初始狀態取值即可。x0=3.2姿態角PID控制本文重點研究四旋翼飛行器懸停狀態姿態角10.00P=󰀁󰀁0󰀂&

23、#983042;0010.0系統的過程噪聲Q和測量噪聲R為AutomationInstrumentation2013(11)signal/（P.U.）以滾轉角為例，將四旋翼飛行器的質量、角度參數、轉動慣量和轉矩帶入式（18）中可得到系統的傳遞函數為0.80.60.40.200signal/（P.U.）05×10-6Q0=05×10-6R0=5×10-91.21目標值實際值1.210.80.60.40.200目標值實際值G（s）=38.72s+51.2s（22）1t/s231t/s23通過將四旋翼飛行器機體傾斜5°，并保持此傾角不動，將四個電機保持相同的轉

24、速，在動態的環境下，測量加速度計和陀螺儀輸入卡爾曼濾波器之前的姿態角數據，以及經過卡爾曼濾波器輸出后的姿態角數據。如圖4（a）為加速度計輸入卡爾曼濾波器之前的姿態角數據，圖4（b）為陀螺儀輸入卡爾曼濾波器前的姿態角數據，圖4（c）為經過卡爾曼濾波融合后的姿態角數據，由圖3可知卡爾曼濾波能有效地濾除加速度計和陀螺儀姿態角的噪聲，為系統的PID控制器提供有效輸入，降低了噪聲對PID控制器的影響。signal/（°）15105000.511.522.5t/s33.544.5理想信號加速度計帶噪聲信號圖5加卡爾曼濾波前后的PID階躍響應曲線Fig.5StepresponsecurveofPI

25、DbeforeandafterKalmanfilter準確的問題，本文利用卡爾曼濾波算法將加速度計和陀螺儀輸出的姿態角進行融合處理，克服了單一傳感器對四旋翼飛行器姿態角測量不準確的缺點，結合懸停狀態下四旋翼飛行器的動力學模型，設計了姿態角的PID控制器和確定了系統傳遞函數，并在Matlab下進行了基于卡爾曼濾波的姿態角PID仿真，仿真結果顯示加卡爾曼濾波后的PID響應速度快、穩定性好，為四旋翼飛行器的研究奠定了基礎。參考文獻：1聶博文.微小型四旋翼飛行器的研究現狀與關鍵技術J.電光與控制，2007，14（6）：113-117.劉和平.數字信號處理器原理、結構及應用基礎M.北京：機械工業出版社，

26、2006：1-15.signal/（°）15105000.51（a）帶噪聲加速度計姿態角理想信號陀螺儀帶噪聲信號23MarcoCarminati，GiorgioFerrari，RiccardoGrassetti，etal.Real-timedatafusionandMEMSsensorsfaultdetectioninanaircraftemergencyattitudeunitbasekalmanfilteringJ.IEEESensorsJournal，2012，12（10）：2984-2992.1.52signal/（°）15105000.51（b）帶噪聲陀螺儀姿態角理想信號卡爾曼濾波后信號2.5t/s33.544.554KeunUkLee，YoungHunYun，WookChang，etal.