一種用於IMU多傳感器數據融合的濾波方法與流程
2023-05-18 08:31:46
本發明屬於數字圖像處理領域，涉及濾波方法，特指一種用於IMU多傳感器數據融合的濾波方法。
背景技術：
隨著微機電系統技術的發展，為了滿足廣泛應用的需求，採用微機械陀螺儀、加速度計、磁力計等構造微小型低成本的航姿參考系統。同時微小型無人機具有成本低、隱蔽性好、生存能力強等特點，近年來在軍事、民用等相關領域中得到越來越多的應用。姿態測量是無人機實現姿態控制的前提，也是導航系統中不可分割的重要組成部分。無人機飛行器姿態主要包括姿態解算和飛行控制兩部分，其中，姿態解算是飛行器設計的核心和重點。
姿態解算需要依靠慣性測量器件，加速度計測量載體的姿態角主要利用地球重力場，當載體偏轉時，在各方向軸上受的重力不同，容易受其他力矩的影響，但具有較好的靜態特性，即低頻段的動態響應好；陀螺儀以較短時間間隔讀取測量載體坐標系下機體在各方向軸上的角速度，存在溫度漂移和零點漂移，隨著時間累積其通過積分計算的角速度信息會有較大的姿態誤差，但具有較好的高頻段動態特性。因此研究具有高可靠性、高精度的IMU多傳感器數據融合算法一直是國內外學者關注的焦點。
航姿參考系統利用慣性器件來測量載體姿態角，通常採用多種傳感器組合測量，輸出含有噪聲的姿態信息，經信息濾波篩選之後進行姿態解算，由於各傳感器的特點如動態響應速度、累計誤差等不同，需要採用相關算法對每個傳感器的數據信號進行融合，提高測量精度和系統的動態特性。
常用的數據融合算法有：卡爾曼濾波方法、擴展卡爾曼濾波方法、粒子濾波姿態估計法、梯度下降法、互補濾波方法等。卡爾曼濾波方法應用於慣性導航中取得了較大的成功，但該算法較複雜、運算量大，需要較高的硬體成本，電量耗損大。擴展卡爾曼濾波方法是應用最廣泛的一種非線性系統濾波方法，但存在線性化誤差、截斷誤差及發散問題。粒子濾波姿態估計法提高了濾波精度，解決了系統非線性和非白噪聲對姿態解算的影響，但計算量較大，不適用於低成本的航姿系統。梯度下降法在姿態解算中性能優於卡爾曼濾波，計算量少，對初始點要求低，但收斂速度較慢。互補濾波方法原理簡單、計算量小，通過平率分辨消除噪聲，能較好地結合加速度計的靜態精度和陀螺儀的高動態姿態數據，利用兩者在頻域角度的互補特性，提高姿態測量的精度和動態響應，以減少測量和估計的誤差，但其高通濾波器和低通濾波器的轉接頻率難以確定。
技術實現要素：
針對現有方法的不足，本發明提出一種用於IMU多傳感器數據融合的濾波方法。其創新性在於各傳感器信息的動態響應速度、累計誤差不同，解算出的姿態信息具有一定的互補性，通過對PI控制參數的引入和設定，設計含有負反饋信息的二階互補濾波算法對各傳感器信息融合，提高系統的測量精度和動態性能。
為實現上述技術目的，本發明的技術方案是，
一種用於IMU多傳感器數據融合的濾波方法，包括以下步驟：
(1)利用陀螺儀、加速度計組成的IMU慣性檢測系統獲取飛行器的姿態信息，對各傳感器的輸出信號進行表示，建立傳感器輸出信號的數學模型表達式；
陀螺儀的誤差主要由隨機漂移和噪聲構成，設ywt為陀螺儀的輸出信號，wt為真實角速度值，vwt為測量噪聲，b為漂移量，則數學模型可表示為：
ywt＝wt+vwt+b (1)
同理，加速度計的輸出信號為yat，載體運動的加速度為at，地球重力加速度為gt，其中為旋轉矩陣，vat表示高斯白噪聲，則加速度計的數學模型可表示為：
yat＝at-gt+vat (2)
(2)利用滑動均值濾波去除步驟(1)數學模型表達式中的測量噪聲和高斯白噪聲以及幹擾信息，提高系統的精度；其中滑動均值濾波器公式為：
其中yi為各傳感器輸出的原始數據，w為滑動均值濾波器的輸出結果，n為滑動均值濾波器階數。
由公式(3)可以看出，每次參與濾波的傳感器數據始終為當前傳感器最新採樣的n個數據，滑動均值濾波器只涉及線性運算，可以保證濾波的實時性。
(3)基於四元素向量法，將各傳感器去噪後的輸出數據轉化為四元素，建立載體坐標系與參考坐標系之間的空間角位置關係，利用變換矩陣得到姿態角的四元素表達式，分別進行加速度計和陀螺儀的旋轉加速度和旋轉角速度姿態角解算。
a.確定兩坐標系間的關係
飛行器的姿態由俯仰角θ、橫滾角φ、偏航角ψ描述，選取東北天坐標系(g系)為參考坐標系，即以坐標系載體的重心為坐標原點，Xg指向正北方向，Yg指向正東方向，Zg軸與Xg軸和Yg軸構成的平面垂直，指向天空；載體坐標系(b系)以載體作為參考，坐標原點為載體重心，Xb為載體橫軸，Yb為載體縱軸，Zb軸垂直於Xb軸和Yb軸構成的平面，XbYbZb構成右手坐標系。
參考坐標系被認為是固定不變的，載體坐標系相對於參考坐標系的方位變化，相對變化情況即為待求的載體姿態，常用姿態角表示，兩坐標系之間的關係可以用旋轉矩陣表示為：
b.將各傳感器經步驟(2)去噪處理後的輸出數據轉化為四元素表示形式。
由式(4)可知，當俯仰角θ為正負90°時，歐拉角計算中會出現奇異點，導致數值不穩定，因此可將傳感器採集到的姿態數據經過去噪處理之後，轉化為四元素表示方法，其表達式為：
其中q0、q1、q2、q3均為實數，i、j、k為相互正交的單位向量。
c.將四元素向量規範化處理之後，變換矩陣可表示為：
d.用四元素法中的參數推導得到姿態角表達式為：
基於四元素法進行姿態解算利用了加速度計和陀螺儀所測數據，加速度計主要是測量載體坐標系下旋轉加速度，陀螺儀主要測量載體坐標系下的旋轉角速度。以加速度計在無加速狀態(即靜止或勻速運動)時的輸出結果計算IMU慣性檢測系統的初始歐拉角，聯立(4)(5)(6)式求出初始四元素，對陀螺儀的角速度進行等時間間隔定時採樣得到IMU慣性檢測系統角增量，結合四元素微分方程，根據上一時刻姿態四元素求解當前時刻更新的姿態四元素，代入(7)式完成姿態解算。
四元素微分方程如下式所示：
其中Δθx、Δθy、Δθz分別為定時採樣時三個軸的角增量，I為單位矩陣，由th時刻定時採樣的角增量，結合四元素微分方程式可求得th+1時刻的四元素Q(th+1)。
(4)設計二階互補濾波器對姿態解算得到的加速度計姿態角進行低通濾波，對陀螺儀姿態角進行高通濾波，並實現各傳感器數據融合，最後輸出姿態角。
以俯仰角θ為例，θa為姿態解算得到的加速度計俯仰角，θw為姿態解算得到的陀螺儀俯仰角，在二階互補濾波器中的低通濾波部分，通過姿態解算得到的加速度計俯仰角θa與前一次濾波更新後姿態角中的俯仰角θ之間存在的誤差值θerr作為二階互補濾波器的二次輸入值即反饋角度作為新的輸入值形成負反饋，實現二階互補濾波的誤差修正，二階互補濾波器各部分的具體表達式為：
上式中，x1和x2為中間變量，Kp為比例係數變量，KI為積分係數變量，τ為濾波器時間常數，1/τ為濾波器截止頻率，dt為濾波器採樣時間，θt為最終輸出的俯仰角。
同理，利用二階互補濾波器對橫滾角φw和偏航角ψw姿態數據融合，得到最終輸出的橫滾角φt和偏航角ψt。
其中，Kp為比例係數變量，KI為積分係數變量的確定方法有很多，主要有臨界比例法、反映曲線法、衰減法等，一般先確定比例係數Kp，再確定積分係數KI。具體方法為首先設定積分係數KI為0，將二階互補濾波器變為純比例調節，然後將Kp的值由零開始逐步增大，直至二階互補濾波器出現振蕩，再反過來調節將Kp值逐步減小，直至二階互補濾波器振蕩消失，記錄當前的比例係數值Kp；比例係數值Kp確定之後，設定一個較大的積分時間常數Ti(一般設置為允許輸出最大值的60％至70％。)，然後逐漸減小，直至二階互補濾波器出現振蕩，再反過來增大Ti，直至二階互補濾波器振蕩消失，記錄當前的Ti值，積分係數Ki＝Kp/Ti。
本發明通過對PI控制參數的引入和設定，設計含有負反饋的二階互補濾波算法對各傳感器信息融合，提高系統的測量精度和動態性能。
附圖說明
圖1為本發明的原理圖
圖2為兩坐標系角度轉換關係示意圖
圖3為二階互補濾波器的原理圖
具體實施方式
為使本發明的目的、技術方案和優點更加清楚，下面將結合附圖對本發明實施方式作進一步地詳細描述。
參照圖1本發明提出了一種用於IMU多傳感器數據融合的濾波方法，首先對數據進行採集，由陀螺儀、加速度計等多個傳感器組成的IMU慣性檢測系統獲取飛行器的姿態數據，並建立傳感器輸出信號的數學模型表達式；其次對各傳感器的輸出數據進行處理，主要包括利用滑動均值濾波去除幹擾信息和噪聲，再基於四元素法解算各傳感器的旋轉角度；然後利用高通濾波器和低通濾波器相結合，在低通濾波部分加入PI控制參數，引入比例係數變量Kp和積分係數變量KI設計二階互補濾波器的誤差修正負反饋，實現各傳感器數據融合，最後輸出姿態角。下面對本發明的各個步驟作進一步的詳細說明。
(1)以三軸陀螺儀和三軸加速度計傳感器為例，首先建立傳感器的數學模型。
陀螺儀的誤差主要由隨機漂移和噪聲構成，設ywt為陀螺儀輸出信號，wt為真實角速度值，vwt為測量噪聲，b為漂移量，則數學模型可表示為：
ywt＝wt+vwt+b (1)
同理，加速度計的輸出信號為yat，載體運動的加速度為at，地球重力加速度為gt，其中為旋轉矩陣，vat表示高斯白噪聲，則加速度計的數學模型可表示為：
yat＝at-gt+vat (2)
(2)如圖1所示，首先對各傳感器採集的信號建立數學模型表示式，輸出值主要包括真實測量值、漂移量和幹擾噪聲。加速度傳感器可以將物體的加速度信息轉換為數字量的傳感器，其輸出信號主要代表了直線的加速度大小和重力方向夾角，以低頻信號為主，陀螺儀傳感器輸出信號具有高動態相應特點，輸出信號中含有噪聲，主要為高斯白噪聲，在姿態解算之前，可以利用低通濾波器或均值濾波器對傳感器輸出信息去噪。
由傳感器的誤差分析和數學建模表達式可知，無論是加速度計還是陀螺儀，輸出值中都存在著不同程度的測量誤差，為了剝離幹擾信號和噪聲的影響，提高系統的精度，對傳感器輸出信息進行數據處理。本實施例採用滑動均值濾波的方法，其濾波器公式為：
其中：yi為各傳感器輸出的原始數據，w為滑動均值濾波器的輸出結果，n為滑動濾波器階數。
由公式(3)可以看出，每次參與濾波的傳感器數據始終為當前傳感器最新採樣的n個數據，滑動濾波器只涉及線性運算，可以保證濾波的實時性。
(3)傳感器輸出信號去除噪聲之後，基於四元素法對其進行姿態角解算。
首先建立坐標系關係，如圖2所示。物體的運動都是相對的，計算載體的位置和姿態，需要選定參考坐標系，以參考坐標係為基準，研究載體坐標系與參考坐標系的相對運動。
飛行器的姿態由俯仰角θ、橫滾角φ、偏航角ψ描述，選取東北天坐標系(g系)為參考坐標系，即以坐標系載體的重心為坐標原點，Xg指向正北方向，Yg指向正東方向，Zg軸與Xg軸和Yg軸構成的平面垂直，指向天空；
載體坐標系(b系)以載體作為參考，載體與六軸傳感器固定相連，坐標原點為載體重心，Xb為載體橫軸，Yb為載體縱軸，Zb軸垂直於Xb軸和Yb軸構成的平面，XbYbZb構成右手坐標系。
參考坐標系被認為是固定不變的，載體坐標系相對於參考坐標系的方位變化，相對變化情況即為我們所求的載體姿態，常用姿態角表示，如圖2所示，兩坐標系之間的關係可以用旋轉矩陣表示為：
由式子(4)可知，當俯仰角θ為正負90°時，歐拉角計算中會出現奇異點，導致數值不穩定，因此可將傳感器採集到的姿態數據經過去噪處理之後，轉化為四元素表示方法，其表達式為：
其中q0、q1、q2、q3均為實數，i、j、k為相互正交的單位向量。
將四元素向量規範化處理之後，變換矩陣可表示為：
其中四元素向量規範化處理的過程：
進一步，可用四元素法中的參數推導得到姿態角表達式為：
基於四元素法進行姿態解算利用了加速度計和陀螺儀所測數據，加速度計主要是測量載體坐標系下旋轉加速度，陀螺儀主要測量載體坐標系下的旋轉角速度。以加速度計在無加速狀態(即靜止或勻速運動)時的輸出結果計算IMU慣性檢測系統的初始歐拉角，聯立(4)(5)(6)式求出初始四元素，對陀螺儀的角速度進行等時間間隔定時採樣得到IMU慣性檢測系統角增量，結合四元素微分方程，根據上一時刻姿態四元素求解當前時刻更新的姿態四元素，代入(7)式完成姿態解算。
(4)陀螺儀短時間內測量精度高，但存在漂移誤差，隨著時間累積，精度變差；加速度計短期精度差，但測量誤差不會隨著時間累積而增加。
由互補濾波器的設計原理可知，加速度計的測量輸出進行低通濾波，能有效的抑制加速度計的高頻噪聲，高通濾波器能很好地抑制陀螺儀輸出信號的低頻漂移。因此，利用兩者在頻率上的互補關係，設計互補濾波算法對姿態數據進行融合，提高姿態的測量精度和動態響應。
由於互補濾波算法中的低通濾波器阻帶衰減較慢，當噪聲較大時濾波效果不理想，因此在低通濾波部分加入PI參數控制，引入參數變量Kp作為比例係數和參數KI為積分係數，設計改進的互補濾波器即本實施例中提供的二階互補濾波器。其中PI控制器參數的工程整定方法有很多，主要有臨界比例法、反映曲線法、衰減法等，一般先確定比例係數Kp，再確定積分係數KI。具體方法為首先設定積分係數KI為0，將系統變為純比例調節，然後將Kp的值由零開始逐步增大，直至系統出現振蕩，再反過來調節將Kp值逐步減小，直至系統振蕩消失，記錄當前的比例係數值Kp；比例係數值確定之後，設定一個較大的積分時間常數Ti(一般設置為允許輸出最大值的60％至70％)，然後逐漸減小，直至系統出現振蕩，再反過來增大Ti，直至系統振蕩消失，記錄當前的Ti值，積分係數Ki＝Kp/Ti。
以俯仰角θ為例，θa為加速度計數據處理後輸出的俯仰角，θw為陀螺儀數據處理後輸出的俯仰角，如圖3所示。以x1和x2為中間變量，對低通濾波部分進行了改進，通過加速度計姿態解算後的俯仰角θa與前一次濾波更新後的俯仰角θ之間存在的誤差值θerr，然後將誤差值作為改進的互補濾波器的二次輸入值即反饋角度作為新的輸入值形成負反饋，實現二階互補濾波的誤差修正。
由圖3可知，改進後的互補濾波器各部分的具體表達式為：
上式中，τ為濾波器時間常數，1/τ為濾波器截止頻率，dt為濾波器採樣時間，θt為最終輸出的俯仰角。
同理，利用二階互補濾波器對橫滾角φw和偏航角ψw姿態數據融合，得到最終輸出的橫滾角φt和偏航角ψt。
以上包含了本發明優選實施例的說明，這是為了詳細說明本發明的技術特徵，並不是想要將發明內容限制在實施例所描述的具體形式中，依據本發明內容主旨進行的其他修改和變型也受本專利保護。本發明內容的主旨是由權利要求書所界定，而非由實施例的具體描述所界定。