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

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

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