本發(fā)明屬于數(shù)字圖像處理領(lǐng)域，涉及濾波方法，特指一種用于IMU多傳感器數(shù)據(jù)融合的濾波方法。

背景技術(shù)：

隨著微機電系統(tǒng)技術(shù)的發(fā)展，為了滿足廣泛應(yīng)用的需求，采用微機械陀螺儀、加速度計、磁力計等構(gòu)造微小型低成本的航姿參考系統(tǒng)。同時微小型無人機具有成本低、隱蔽性好、生存能力強等特點，近年來在軍事、民用等相關(guān)領(lǐng)域中得到越來越多的應(yīng)用。姿態(tài)測量是無人機實現(xiàn)姿態(tài)控制的前提，也是導(dǎo)航系統(tǒng)中不可分割的重要組成部分。無人機飛行器姿態(tài)主要包括姿態(tài)解算和飛行控制兩部分，其中，姿態(tài)解算是飛行器設(shè)計的核心和重點。

姿態(tài)解算需要依靠慣性測量器件，加速度計測量載體的姿態(tài)角主要利用地球重力場，當(dāng)載體偏轉(zhuǎn)時，在各方向軸上受的重力不同，容易受其他力矩的影響，但具有較好的靜態(tài)特性，即低頻段的動態(tài)響應(yīng)好；陀螺儀以較短時間間隔讀取測量載體坐標系下機體在各方向軸上的角速度，存在溫度漂移和零點漂移，隨著時間累積其通過積分計算的角速度信息會有較大的姿態(tài)誤差，但具有較好的高頻段動態(tài)特性。因此研究具有高可靠性、高精度的IMU多傳感器數(shù)據(jù)融合算法一直是國內(nèi)外學(xué)者關(guān)注的焦點。

航姿參考系統(tǒng)利用慣性器件來測量載體姿態(tài)角，通常采用多種傳感器組合測量，輸出含有噪聲的姿態(tài)信息，經(jīng)信息濾波篩選之后進行姿態(tài)解算，由于各傳感器的特點如動態(tài)響應(yīng)速度、累計誤差等不同，需要采用相關(guān)算法對每個傳感器的數(shù)據(jù)信號進行融合，提高測量精度和系統(tǒng)的動態(tài)特性。

常用的數(shù)據(jù)融合算法有：卡爾曼濾波方法、擴展卡爾曼濾波方法、粒子濾波姿態(tài)估計法、梯度下降法、互補濾波方法等?？柭鼮V波方法應(yīng)用于慣性導(dǎo)航中取得了較大的成功，但該算法較復(fù)雜、運算量大，需要較高的硬件成本，電量耗損大。擴展卡爾曼濾波方法是應(yīng)用最廣泛的一種非線性系統(tǒng)濾波方法，但存在線性化誤差、截斷誤差及發(fā)散問題。粒子濾波姿態(tài)估計法提高了濾波精度，解決了系統(tǒng)非線性和非白噪聲對姿態(tài)解算的影響，但計算量較大，不適用于低成本的航姿系統(tǒng)。梯度下降法在姿態(tài)解算中性能優(yōu)于卡爾曼濾波，計算量少，對初始點要求低，但收斂速度較慢?；パa濾波方法原理簡單、計算量小，通過平率分辨消除噪聲，能較好地結(jié)合加速度計的靜態(tài)精度和陀螺儀的高動態(tài)姿態(tài)數(shù)據(jù)，利用兩者在頻域角度的互補特性，提高姿態(tài)測量的精度和動態(tài)響應(yīng)，以減少測量和估計的誤差，但其高通濾波器和低通濾波器的轉(zhuǎn)接頻率難以確定。

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

針對現(xiàn)有方法的不足，本發(fā)明提出一種用于IMU多傳感器數(shù)據(jù)融合的濾波方法。其創(chuàng)新性在于各傳感器信息的動態(tài)響應(yīng)速度、累計誤差不同，解算出的姿態(tài)信息具有一定的互補性，通過對PI控制參數(shù)的引入和設(shè)定，設(shè)計含有負反饋信息的二階互補濾波算法對各傳感器信息融合，提高系統(tǒng)的測量精度和動態(tài)性能。

為實現(xiàn)上述技術(shù)目的，本發(fā)明的技術(shù)方案是，

一種用于IMU多傳感器數(shù)據(jù)融合的濾波方法，包括以下步驟：

(1)利用陀螺儀、加速度計組成的IMU慣性檢測系統(tǒng)獲取飛行器的姿態(tài)信息，對各傳感器的輸出信號進行表示，建立傳感器輸出信號的數(shù)學(xué)模型表達式；

陀螺儀的誤差主要由隨機漂移和噪聲構(gòu)成，設(shè)y_wt為陀螺儀的輸出信號，w_t為真實角速度值，v_wt為測量噪聲，b為漂移量，則數(shù)學(xué)模型可表示為：

y_wt＝w_t+v_wt+b (1)

同理，加速度計的輸出信號為y_at，載體運動的加速度為a_t，地球重力加速度為g_t，其中為旋轉(zhuǎn)矩陣，v_at表示高斯白噪聲，則加速度計的數(shù)學(xué)模型可表示為：

y_at＝a_t-g_t+v_at (2)

(2)利用滑動均值濾波去除步驟(1)數(shù)學(xué)模型表達式中的測量噪聲和高斯白噪聲以及干擾信息，提高系統(tǒng)的精度；其中滑動均值濾波器公式為：

其中y_i為各傳感器輸出的原始數(shù)據(jù)，w為滑動均值濾波器的輸出結(jié)果，n為滑動均值濾波器階數(shù)。

由公式(3)可以看出，每次參與濾波的傳感器數(shù)據(jù)始終為當(dāng)前傳感器最新采樣的n個數(shù)據(jù)，滑動均值濾波器只涉及線性運算，可以保證濾波的實時性。

(3)基于四元素向量法，將各傳感器去噪后的輸出數(shù)據(jù)轉(zhuǎn)化為四元素，建立載體坐標系與參考坐標系之間的空間角位置關(guān)系，利用變換矩陣得到姿態(tài)角的四元素表達式，分別進行加速度計和陀螺儀的旋轉(zhuǎn)加速度和旋轉(zhuǎn)角速度姿態(tài)角解算。

a.確定兩坐標系間的關(guān)系

飛行器的姿態(tài)由俯仰角θ、橫滾角φ、偏航角ψ描述，選取東北天坐標系(g系)為參考坐標系，即以坐標系載體的重心為坐標原點，Xg指向正北方向，Yg指向正東方向，Zg軸與Xg軸和Yg軸構(gòu)成的平面垂直，指向天空；載體坐標系(b系)以載體作為參考，坐標原點為載體重心，Xb為載體橫軸，Yb為載體縱軸，Zb軸垂直于Xb軸和Yb軸構(gòu)成的平面，XbYbZb構(gòu)成右手坐標系。

參考坐標系被認為是固定不變的，載體坐標系相對于參考坐標系的方位變化，相對變化情況即為待求的載體姿態(tài)，常用姿態(tài)角表示，兩坐標系之間的關(guān)系可以用旋轉(zhuǎn)矩陣表示為：

b.將各傳感器經(jīng)步驟(2)去噪處理后的輸出數(shù)據(jù)轉(zhuǎn)化為四元素表示形式。

由式(4)可知，當(dāng)俯仰角θ為正負90°時，歐拉角計算中會出現(xiàn)奇異點，導(dǎo)致數(shù)值不穩(wěn)定，因此可將傳感器采集到的姿態(tài)數(shù)據(jù)經(jīng)過去噪處理之后，轉(zhuǎn)化為四元素表示方法，其表達式為：

其中q₀、q₁、q₂、q₃均為實數(shù)，i、j、k為相互正交的單位向量。

c.將四元素向量規(guī)范化處理之后，變換矩陣可表示為：

d.用四元素法中的參數(shù)推導(dǎo)得到姿態(tài)角表達式為：

基于四元素法進行姿態(tài)解算利用了加速度計和陀螺儀所測數(shù)據(jù)，加速度計主要是測量載體坐標系下旋轉(zhuǎn)加速度，陀螺儀主要測量載體坐標系下的旋轉(zhuǎn)角速度。以加速度計在無加速狀態(tài)(即靜止或勻速運動)時的輸出結(jié)果計算IMU慣性檢測系統(tǒng)的初始歐拉角，聯(lián)立(4)(5)(6)式求出初始四元素，對陀螺儀的角速度進行等時間間隔定時采樣得到IMU慣性檢測系統(tǒng)角增量，結(jié)合四元素微分方程，根據(jù)上一時刻姿態(tài)四元素求解當(dāng)前時刻更新的姿態(tài)四元素，代入(7)式完成姿態(tài)解算。

四元素微分方程如下式所示：

其中Δθ_x、Δθ_y、Δθ_z分別為定時采樣時三個軸的角增量，I為單位矩陣，由t_h時刻定時采樣的角增量，結(jié)合四元素微分方程式可求得t_h+1時刻的四元素Q(t_h+1)。

(4)設(shè)計二階互補濾波器對姿態(tài)解算得到的加速度計姿態(tài)角進行低通濾波，對陀螺儀姿態(tài)角進行高通濾波，并實現(xiàn)各傳感器數(shù)據(jù)融合，最后輸出姿態(tài)角。

以俯仰角θ為例，θ_a為姿態(tài)解算得到的加速度計俯仰角，θ_w為姿態(tài)解算得到的陀螺儀俯仰角，在二階互補濾波器中的低通濾波部分，通過姿態(tài)解算得到的加速度計俯仰角θ_a與前一次濾波更新后姿態(tài)角中的俯仰角θ之間存在的誤差值θ_err作為二階互補濾波器的二次輸入值即反饋角度作為新的輸入值形成負反饋，實現(xiàn)二階互補濾波的誤差修正，二階互補濾波器各部分的具體表達式為：

上式中，x₁和x₂為中間變量，K_p為比例系數(shù)變量，K_I為積分系數(shù)變量，τ為濾波器時間常數(shù)，1/τ為濾波器截止頻率，d_t為濾波器采樣時間，θ_t為最終輸出的俯仰角。

同理，利用二階互補濾波器對橫滾角φ_w和偏航角ψ_w姿態(tài)數(shù)據(jù)融合，得到最終輸出的橫滾角φ_t和偏航角ψ_t。

其中，K_p為比例系數(shù)變量，K_I為積分系數(shù)變量的確定方法有很多，主要有臨界比例法、反映曲線法、衰減法等，一般先確定比例系數(shù)K_p，再確定積分系數(shù)K_I。具體方法為首先設(shè)定積分系數(shù)K_I為0，將二階互補濾波器變?yōu)榧儽壤{(diào)節(jié)，然后將K_p的值由零開始逐步增大，直至二階互補濾波器出現(xiàn)振蕩，再反過來調(diào)節(jié)將K_p值逐步減小，直至二階互補濾波器振蕩消失，記錄當(dāng)前的比例系數(shù)值K_p；比例系數(shù)值K_p確定之后，設(shè)定一個較大的積分時間常數(shù)T_i(一般設(shè)置為允許輸出最大值的60％至70％。)，然后逐漸減小，直至二階互補濾波器出現(xiàn)振蕩，再反過來增大T_i，直至二階互補濾波器振蕩消失，記錄當(dāng)前的T_i值，積分系數(shù)K_i＝K_p/T_i。

本發(fā)明通過對PI控制參數(shù)的引入和設(shè)定，設(shè)計含有負反饋的二階互補濾波算法對各傳感器信息融合，提高系統(tǒng)的測量精度和動態(tài)性能。

附圖說明

圖1為本發(fā)明的原理圖

圖2為兩坐標系角度轉(zhuǎn)換關(guān)系示意圖

圖3為二階互補濾波器的原理圖

具體實施方式

為使本發(fā)明的目的、技術(shù)方案和優(yōu)點更加清楚，下面將結(jié)合附圖對本發(fā)明實施方式作進一步地詳細描述。

參照圖1本發(fā)明提出了一種用于IMU多傳感器數(shù)據(jù)融合的濾波方法，首先對數(shù)據(jù)進行采集，由陀螺儀、加速度計等多個傳感器組成的IMU慣性檢測系統(tǒng)獲取飛行器的姿態(tài)數(shù)據(jù)，并建立傳感器輸出信號的數(shù)學(xué)模型表達式；其次對各傳感器的輸出數(shù)據(jù)進行處理，主要包括利用滑動均值濾波去除干擾信息和噪聲，再基于四元素法解算各傳感器的旋轉(zhuǎn)角度；然后利用高通濾波器和低通濾波器相結(jié)合，在低通濾波部分加入PI控制參數(shù)，引入比例系數(shù)變量K_p和積分系數(shù)變量K_I設(shè)計二階互補濾波器的誤差修正負反饋，實現(xiàn)各傳感器數(shù)據(jù)融合，最后輸出姿態(tài)角。下面對本發(fā)明的各個步驟作進一步的詳細說明。

(1)以三軸陀螺儀和三軸加速度計傳感器為例，首先建立傳感器的數(shù)學(xué)模型。

陀螺儀的誤差主要由隨機漂移和噪聲構(gòu)成，設(shè)y_wt為陀螺儀輸出信號，w_t為真實角速度值，v_wt為測量噪聲，b為漂移量，則數(shù)學(xué)模型可表示為：

y_wt＝w_t+v_wt+b (1)

同理，加速度計的輸出信號為y_at，載體運動的加速度為a_t，地球重力加速度為g_t，其中為旋轉(zhuǎn)矩陣，v_at表示高斯白噪聲，則加速度計的數(shù)學(xué)模型可表示為：

y_at＝a_t-g_t+v_at (2)

(2)如圖1所示，首先對各傳感器采集的信號建立數(shù)學(xué)模型表示式，輸出值主要包括真實測量值、漂移量和干擾噪聲。加速度傳感器可以將物體的加速度信息轉(zhuǎn)換為數(shù)字量的傳感器，其輸出信號主要代表了直線的加速度大小和重力方向夾角，以低頻信號為主，陀螺儀傳感器輸出信號具有高動態(tài)相應(yīng)特點，輸出信號中含有噪聲，主要為高斯白噪聲，在姿態(tài)解算之前，可以利用低通濾波器或均值濾波器對傳感器輸出信息去噪。

由傳感器的誤差分析和數(shù)學(xué)建模表達式可知，無論是加速度計還是陀螺儀，輸出值中都存在著不同程度的測量誤差，為了剝離干擾信號和噪聲的影響，提高系統(tǒng)的精度，對傳感器輸出信息進行數(shù)據(jù)處理。本實施例采用滑動均值濾波的方法，其濾波器公式為：

其中：y_i為各傳感器輸出的原始數(shù)據(jù)，w為滑動均值濾波器的輸出結(jié)果，n為滑動濾波器階數(shù)。

由公式(3)可以看出，每次參與濾波的傳感器數(shù)據(jù)始終為當(dāng)前傳感器最新采樣的n個數(shù)據(jù)，滑動濾波器只涉及線性運算，可以保證濾波的實時性。

(3)傳感器輸出信號去除噪聲之后，基于四元素法對其進行姿態(tài)角解算。

首先建立坐標系關(guān)系，如圖2所示。物體的運動都是相對的，計算載體的位置和姿態(tài)，需要選定參考坐標系，以參考坐標系為基準，研究載體坐標系與參考坐標系的相對運動。

飛行器的姿態(tài)由俯仰角θ、橫滾角φ、偏航角ψ描述，選取東北天坐標系(g系)為參考坐標系，即以坐標系載體的重心為坐標原點，X_g指向正北方向，Y_g指向正東方向，Z_g軸與X_g軸和Y_g軸構(gòu)成的平面垂直，指向天空；

載體坐標系(b系)以載體作為參考，載體與六軸傳感器固定相連，坐標原點為載體重心，X_b為載體橫軸，Y_b為載體縱軸，Z_b軸垂直于X_b軸和Y_b軸構(gòu)成的平面，X_bY_bZ_b構(gòu)成右手坐標系。

參考坐標系被認為是固定不變的，載體坐標系相對于參考坐標系的方位變化，相對變化情況即為我們所求的載體姿態(tài)，常用姿態(tài)角表示，如圖2所示，兩坐標系之間的關(guān)系可以用旋轉(zhuǎn)矩陣表示為：

由式子(4)可知，當(dāng)俯仰角θ為正負90°時，歐拉角計算中會出現(xiàn)奇異點，導(dǎo)致數(shù)值不穩(wěn)定，因此可將傳感器采集到的姿態(tài)數(shù)據(jù)經(jīng)過去噪處理之后，轉(zhuǎn)化為四元素表示方法，其表達式為：

其中q₀、q₁、q₂、q₃均為實數(shù)，i、j、k為相互正交的單位向量。

將四元素向量規(guī)范化處理之后，變換矩陣可表示為：

其中四元素向量規(guī)范化處理的過程：

進一步，可用四元素法中的參數(shù)推導(dǎo)得到姿態(tài)角表達式為：

基于四元素法進行姿態(tài)解算利用了加速度計和陀螺儀所測數(shù)據(jù)，加速度計主要是測量載體坐標系下旋轉(zhuǎn)加速度，陀螺儀主要測量載體坐標系下的旋轉(zhuǎn)角速度。以加速度計在無加速狀態(tài)(即靜止或勻速運動)時的輸出結(jié)果計算IMU慣性檢測系統(tǒng)的初始歐拉角，聯(lián)立(4)(5)(6)式求出初始四元素，對陀螺儀的角速度進行等時間間隔定時采樣得到IMU慣性檢測系統(tǒng)角增量，結(jié)合四元素微分方程，根據(jù)上一時刻姿態(tài)四元素求解當(dāng)前時刻更新的姿態(tài)四元素，代入(7)式完成姿態(tài)解算。

(4)陀螺儀短時間內(nèi)測量精度高，但存在漂移誤差，隨著時間累積，精度變差；加速度計短期精度差，但測量誤差不會隨著時間累積而增加。

由互補濾波器的設(shè)計原理可知，加速度計的測量輸出進行低通濾波，能有效的抑制加速度計的高頻噪聲，高通濾波器能很好地抑制陀螺儀輸出信號的低頻漂移。因此，利用兩者在頻率上的互補關(guān)系，設(shè)計互補濾波算法對姿態(tài)數(shù)據(jù)進行融合，提高姿態(tài)的測量精度和動態(tài)響應(yīng)。

由于互補濾波算法中的低通濾波器阻帶衰減較慢，當(dāng)噪聲較大時濾波效果不理想，因此在低通濾波部分加入PI參數(shù)控制，引入?yún)?shù)變量K_p作為比例系數(shù)和參數(shù)K_I為積分系數(shù)，設(shè)計改進的互補濾波器即本實施例中提供的二階互補濾波器。其中PI控制器參數(shù)的工程整定方法有很多，主要有臨界比例法、反映曲線法、衰減法等，一般先確定比例系數(shù)K_p，再確定積分系數(shù)K_I。具體方法為首先設(shè)定積分系數(shù)K_I為0，將系統(tǒng)變?yōu)榧儽壤{(diào)節(jié)，然后將K_p的值由零開始逐步增大，直至系統(tǒng)出現(xiàn)振蕩，再反過來調(diào)節(jié)將K_p值逐步減小，直至系統(tǒng)振蕩消失，記錄當(dāng)前的比例系數(shù)值K_p；比例系數(shù)值確定之后，設(shè)定一個較大的積分時間常數(shù)T_i(一般設(shè)置為允許輸出最大值的60％至70％)，然后逐漸減小，直至系統(tǒng)出現(xiàn)振蕩，再反過來增大T_i，直至系統(tǒng)振蕩消失，記錄當(dāng)前的T_i值，積分系數(shù)K_i＝K_p/T_i。

以俯仰角θ為例，θ_a為加速度計數(shù)據(jù)處理后輸出的俯仰角，θ_w為陀螺儀數(shù)據(jù)處理后輸出的俯仰角，如圖3所示。以x₁和x₂為中間變量，對低通濾波部分進行了改進，通過加速度計姿態(tài)解算后的俯仰角θ_a與前一次濾波更新后的俯仰角θ之間存在的誤差值θ_err，然后將誤差值作為改進的互補濾波器的二次輸入值即反饋角度作為新的輸入值形成負反饋，實現(xiàn)二階互補濾波的誤差修正。

由圖3可知，改進后的互補濾波器各部分的具體表達式為：

上式中，τ為濾波器時間常數(shù)，1/τ為濾波器截止頻率，d_t為濾波器采樣時間，θ_t為最終輸出的俯仰角。

同理，利用二階互補濾波器對橫滾角φ_w和偏航角ψ_w姿態(tài)數(shù)據(jù)融合，得到最終輸出的橫滾角φ_t和偏航角ψ_t。

以上包含了本發(fā)明優(yōu)選實施例的說明，這是為了詳細說明本發(fā)明的技術(shù)特征，并不是想要將發(fā)明內(nèi)容限制在實施例所描述的具體形式中，依據(jù)本發(fā)明內(nèi)容主旨進行的其他修改和變型也受本專利保護。本發(fā)明內(nèi)容的主旨是由權(quán)利要求書所界定，而非由實施例的具體描述所界定。