一種基于卡爾曼濾波的四旋翼無人機姿態(tài)數(shù)據(jù)融合的方法
【技術領域】
[0001] 本發(fā)明涉及組合導航中多傳感器數(shù)據(jù)融合領域，尤其涉及一種基于卡爾曼濾波的 四旋翼無人機姿態(tài)數(shù)據(jù)融合的方法。
【背景技術】
[0002] 卡爾曼濾波是一個最優(yōu)化自回歸數(shù)據(jù)處理算法。近年來被廣泛應用于計算機圖像 處理方面。在四旋翼無人機飛行系統(tǒng)中，需要對陀螺儀、加速度傳感器和磁力計傳感器所測 得的數(shù)據(jù)進行綜合和校正。如果使用簡單的均值濾波，很難滿足精度和實時性上的要求。故 通過卡爾曼濾波將加速度傳感器、磁力傳感器和陀螺儀的數(shù)據(jù)進行融合，在計算四旋翼無 人機飛行器的實時姿態(tài)時很好地抑制了噪聲的干擾作用，以提高測量精度，為自主飛行控 制創(chuàng)造了條件。
[0003] 但是卡爾曼濾波在四旋翼無人機姿態(tài)運算過程中也存在一個明顯的缺點，其實也 是基于數(shù)字平臺的卡爾曼濾波算法都存在的問題，算法中方程組中有大量的矩陣乘法，有 的還有矩陣連乘。這會導致出現(xiàn)位寬很寬的中間結(jié)果，從而難以進行迭代運算。在數(shù)字實 現(xiàn)中面臨的最大困難即是矩陣求逆運算，當矩陣的階數(shù)增加時，其求逆運算的復雜度更是 以幾何級數(shù)增加?？柭鼮V波算法是通過迭代運算實現(xiàn)的，這就要求在每次迭代運算中，卡 爾曼濾波算法方程組都必須重新運算一次，這將會使CPU承受復雜而又極其繁重運算。根 據(jù)上述分析，可以看出應用卡爾曼濾波算法進行姿態(tài)數(shù)據(jù)融合，運算量非常大，對飛控板的 處理器芯片要求非常高，所以對于應用在四旋翼無人機姿態(tài)數(shù)據(jù)融合的卡爾曼濾波算法進 行簡化就顯得非常必要。

【發(fā)明內(nèi)容】

[0004] 本發(fā)明為解決現(xiàn)有的卡爾曼濾波在四旋翼無人機姿態(tài)運算過程中運算量太大而 無法實時獲取姿態(tài)數(shù)據(jù)的問題，而提出一種基于卡爾曼濾波的四旋翼無人機姿態(tài)數(shù)據(jù)融合 的方法。
[0005] 基于卡爾曼濾波的四旋翼無人機姿態(tài)數(shù)據(jù)融合方法的理論基礎：
[0006] 在四旋翼無人機姿態(tài)解算中，陀螺儀能提供瞬間的動態(tài)角度變化，保證載體具有 動態(tài)加速度時的穩(wěn)定性，但受其本身的固有特性、溫度及積分過程的影響，會隨著工作時間 的延長產(chǎn)生累積誤差；加速度傳感器和磁力傳感器能準確地提供靜態(tài)角度，但是運動時容 易受到噪聲干擾。所以在四旋翼無人機姿態(tài)解算過程中，應當綜合陀螺儀、加速度傳感器和 磁力傳感器的數(shù)據(jù)來解算四旋翼無人機的姿態(tài)。通過卡爾曼濾波算法可以很好地融合三個 傳感器的數(shù)據(jù)。
[0007] 本申請所述的基于卡爾曼濾波的四旋翼無人機姿態(tài)數(shù)據(jù)融合方法的基本思路是， 盡量減少矩陣乘法運算；避免矩陣的求逆運算；簡化迭代過程。
[0008] -種基于卡爾曼濾波的四旋翼無人機姿態(tài)數(shù)據(jù)融合的方法，按以下步驟進行：
[0009] -、根據(jù)四旋翼無人機姿態(tài)測量傳感器的特性，選擇以四旋翼無人機的實際姿態(tài) 角Θ和陀螺儀的測量誤差σ為狀態(tài)向量，以加速度傳感器和磁力傳感器測得的姿態(tài)角θ #為觀測值，得到四旋翼無人機姿態(tài)解算系統(tǒng)相應的狀態(tài)方程和觀測方程式，如公式（1)所 示，
[0010]
Cl)
[0011] 其中θ:為四旋翼無人機的實際姿態(tài)角Θ的微分，σ為陀螺儀的測量誤差〇的微 分，ω為陀螺儀輸出的角速度，COe為陀螺儀的測量白噪聲，ve為加速度傳感器和磁力傳感 器的測量白噪聲，《^與％相互獨立；
[0012] 二、將解算系統(tǒng)的連續(xù)信號離散化為計算機能處理的離散化信號，令解算系統(tǒng)的 采樣周期為Ts，得到離散化的狀態(tài)方程X (k)和測量方程V (k)，如公式（2)所示，
[0013]
(:2>
[0014] 其中ω (k-Ι)為k-Ι時刻陀螺儀輸出的角速度；以加速度傳感器和磁力傳感器在 k時刻測得的滾動角、俯仰角、偏航角為測量姿態(tài)角，(k)為此測量姿態(tài)角的白噪聲偏差， 此白噪聲偏差對應的協(xié)方差為測量噪聲協(xié)方差R ;
[0015] 三、在公式（2)的基礎上構(gòu)建卡爾曼濾波方程組（3)~（7)，其中當前狀態(tài)預測方 程，如公式（3)所示，
[0016] X (k I k_l) = AX (k_l I k_l)+B ω (k_l)， (3)
[0017] 其中A和B為解算系統(tǒng)參數(shù)，
是k_l時刻 最優(yōu)的姿態(tài)角，ω (k-Ι)為k-Ι時刻的控制量，即陀螺儀輸出的角速度，X(k|k-1)是根據(jù) X (k-1 |k-l)預測到的無人機當前姿態(tài)角；
[0018] 當前狀態(tài)誤差協(xié)方差預測方程如公式（4)所示，
[0019] P (k I k-1) = AP(k-l|k-l)AT+Q, (4)
[0020] 其中P (k_l I k_l)是X (k_l I k_l)對應的協(xié)方差，P (k I k_l)是X (k I k_l)對應的協(xié)方 差，以陀螺儀計算得到k時刻的姿態(tài)角偏差和k-Ι時刻的最優(yōu)姿態(tài)角之和為當前預測姿態(tài) 角，此當前預測姿態(tài)角的噪聲偏差為G (k)，此噪聲偏差對應的協(xié)方差為過程噪聲協(xié)方差Q ;
[0021] 將k時刻姿態(tài)角預測值與測量值以卡爾曼增益Kg(k)為比例進行融合，就可得到k 時刻的最優(yōu)姿態(tài)角X(k|k)，如公式（5)所示，
[0022] X(k|k) = X(k|k-1)+Kg(k) (V(k)-HX(k|k-l))， (5)
[0023] 其中H為測量系統(tǒng)參數(shù)，H = [10]，Kg(k)為卡爾曼增益，V(k)是加速度傳感器和 磁力傳感器在k時刻獲得姿態(tài)角的測量值，X (k I k-i)是k時刻姿態(tài)角的預測值；
[0024] 公式（5)中Kg(k)，即卡爾曼增益由下式求得，如公式（6)所示，
[0025]
(6)
[0026] 其中P (k |k-l)為X (k |k-l)對應的協(xié)方差，R的物理意義見步驟二中的說明；
[0027] k時刻的最優(yōu)姿態(tài)角X (k I k)的協(xié)方差P (k I k)如公式（7)所示，
[0028] P(k|k) = (I-KgGOH)P(kIk-1)， (7)
[0029] 其中，I為二階單位矩陣，P (k I k_l)是X (k I k_l)對應的協(xié)方差；
[0030] 四、對卡爾曼濾波方程組（3)~（7)進行數(shù)學處理，得到改進后的卡爾曼濾波方程 組（8)~（12)，具體過程如下：
[0031] 合并公式⑷和（7)可得公式（8):
[0032] P(k|k-1) = A((I-Kg(k-l)H)P(k-l|k-2))AT+Q， (8)
[0033] 根據(jù)公式（6)可得公式（9):
[0034]
(9)
[0035] 將公式（5)進行變形，可得公式（10):
[0036] X(k|k) = (I-Kg (k) H) X (k Ik-1)+Kg (k) V (k), (10)
[0037] 根據(jù)公式⑶可得公式（11):
[0038] X (k I k) = (I-Kg (k) H) (AX (k-11 k-1) +B ω (k)) +Kg (k) V (k)
[0039] = (I-Kg (k) H) AX (k-11 k-1) + (I-Kg (k) H) B ω (k) +Kg (k) V (k)， (11)
[0040] 將公式（11)中的（I-Kg(k)H) A和（I-Kg(k)H) Β，分別替換為為Ea (k)和Eb (k)，代入 公式（11)得公式（12):
[0041] X (k I k) = EA(k) X (k-11 k-1)+EB(k) ω (K)+Kg(k) Z (k)， (12)
[0042] 五、對公式（12)中的Ea(k)、Eb(k)和Kg(k)先求值，每次迭代過程中用到E a(k)、 EB(k)和1(1〇時，直接在飛控板處理器芯片內(nèi)調(diào)用，不斷重復計算公式（8)~（10)和（12)， 直至找到各個時刻的最優(yōu)姿態(tài)角。
[0043] 本發(fā)明包括以下有益效果：
[0044] 1、減少運算時間，從而滿足實時性要求：采用標準的基于卡爾曼濾波的四旋翼無 人機姿態(tài)數(shù)據(jù)融合算法，由于運算量大，導致處理時間太長，在工程實際中根本無法實現(xiàn)實 時獲取四旋翼姿態(tài)數(shù)據(jù)，而采用本申請改進算法，部分數(shù)據(jù)離線計算，大大減少飛控板處理 器的計算量，以常用的飛控板處理器STM32F103ZE為例，完成整個四旋翼無人機姿態(tài)數(shù)據(jù) 的獲取要小于1毫秒，滿足實時性要求；
[0045] 2、減小誤差，以滿足數(shù)據(jù)精度要求：采用標準的基于卡爾曼濾波的四旋翼無人機 姿態(tài)數(shù)據(jù)融合算法，在卡爾曼方程迭代運算中受處理器芯片位寬的限制必須不斷地截取數(shù) 據(jù)，從而造成數(shù)據(jù)精度的損失，導致最后姿態(tài)精度的誤差大于15 %，無法達到數(shù)據(jù)精度的要 求，而采用改進算法，EA(k)、EB(k)和&〇〇的離線計算不受飛控板處理器芯片位寬的限制， 最后姿態(tài)精度的誤差為2%~5%，滿足數(shù)據(jù)精度要求。
【附圖說明】
[0046] 圖1為基于卡爾曼濾波的四旋翼無人機姿態(tài)數(shù)據(jù)融合算法框圖，
[0047] 圖中加速度計即為加速度傳感器，磁力計即為磁力傳感器。
【具體實施方式】
[0048] 為使本發(fā)明的上述目的、特征和優(yōu)點能夠更加明顯易懂，下面結(jié)合圖1和具體實 施方式對本發(fā)明作進一步詳細的說明。
【具體實施方式】 [0049] 一、本實施方式所述一種基于卡爾曼濾波的四旋翼無人機姿態(tài)數(shù)據(jù) 融合的方法，按以下步驟進行：
[0050] -、根據(jù)四旋翼無人機姿態(tài)測量傳感器的特性，選擇以四旋翼無人機的實際姿態(tài) 角Θ和陀螺儀的測量誤差 〇為狀態(tài)向量，以加速度傳感器和磁力傳感器測得的姿態(tài)角Θ #為觀測值，得到四旋翼無人機姿態(tài)解算系統(tǒng)相應的狀態(tài)方程和觀測方程式，如公式（1)所 示，
[0051]
Cl)
[0052] 其中Q為四旋翼無人機的實際姿態(tài)角Θ的微分，σ為陀螺儀的測量誤差〇的微 分，ω為陀螺儀輸出的角速度，為陀螺儀的測量白噪聲，％為加速度傳感器和磁力傳感 器的測量白噪聲，《^與相互獨立；
[0053] 二、將解算系統(tǒng)的連續(xù)信號離散化為計算機能處理的離散化信號，令解算系統(tǒng)的 采樣周期為Ts，得到離散化的狀態(tài)方程X (k)和測量方程V (k)，如公式（2)所示，
[0054]
(2)
[0055] 其中ω (k-Ι)為k-Ι時刻陀螺儀輸出的角速度；以加速度傳感器和磁力傳感器在 k時刻測得的滾動角、俯仰角、偏航角為測量姿態(tài)角，(k)為此測量姿態(tài)角的白噪聲偏差， 此白噪聲偏差對應的協(xié)方差為測量噪聲協(xié)方差R ;
[0056] 三、在公式（2)的基礎上構(gòu)建卡爾曼濾波方程組（3)~（7)，其中當前狀態(tài)預測方 程，如公式（3)所示，
[0057] X (k I k_l) = AX (k_l I k_l)+B ω (k_l)， (3) LlN 丄UOldb 丄40 A I ^ 〇/〇 JM
[0058] 其中A和B為解算系統(tǒng)參數(shù)，.是k_l時刻
最優(yōu)的姿態(tài)角，ω (k-Ι)為k-Ι時刻的控制量，即陀螺儀輸出的角速度，X(k|k-1)是根據(jù) X (k-1 |k-l)預測到的無人機當前姿態(tài)角；
[0059] 當前狀態(tài)誤差協(xié)方差預測方程如公式（4)所示，
[0060] P (k I k-1) = AP (k-11 k-1) AT+Q, (4)
[0061] 其中P (k_l I k_l)是X (k_l I k_l)對應的協(xié)方差，P (k I k_l)是X (k I k_l)對應的協(xié)方 差，以陀螺儀計算得到k時刻的姿態(tài)角偏差和k-1時刻的最優(yōu)姿態(tài)角之和為當前預測姿態(tài) 角，此當前預測姿態(tài)角的噪聲偏差為G (k)，此噪聲偏差對應的協(xié)方差為過程噪聲協(xié)方差Q ;
[0062] 將k時刻姿態(tài)角預測值與測量值以卡爾曼增益Kg (k)為比例進行融合，