基於互補重構技術的多傳感器姿態融合方法
2023-12-09 16:58:11
基於互補重構技術的多傳感器姿態融合方法
【專利摘要】基於互補重構技術的多傳感器姿態融合方法，主要包括：四元素迭代更新及載體姿態計算，多傳感器載體姿態觀測及四元素觀測值計算；互補重構參數估計；互補重構四元素值計算等四個步驟。其主要優點：利用四元素觀測值修正上一時刻四元素計算值，從而獲得互補重構的四元素值，並利用此值進行當前時刻的四元素迭代更新，從機理上消除了算法的累積誤差；同時將四元素更新算法和重構算法合二為一，提高了算法的執行效率。
【專利說明】基於互補重構技術的多傳感器姿態融合方法
【技術領域】
[0001] 本發明涉及慣性導航領域，尤其是捷聯慣性導航系統在利用單一慣性傳感器（如 陀螺儀傳感器）數據進行載體姿態測量時，雖然短時間精度好，但是長期運行時的姿態測 量結果由於積分作用產生累積誤差，導致導航系統精度下降。本發明利用多傳感器信息進 行數據融合，可以用較低成本提高捷聯慣性導航系統長時間精度。
【背景技術】
[0002] 捷聯慣性導航系統在計算時，需要把載體測得的加速度，角速度變化從載體坐標 系轉換到導航坐標系中，假設空間位置P在導航坐標系η的坐標表示為P (xn，yn，zn)，在載體 所在坐標系b的坐標表示為P (xb，yb，zb)，則兩者之間滿足如下關係：
【權利要求】
1. 基於互補重構技術的多傳感器姿態融合方法，包括以下步驟： 1) 、利用陀螺儀傳感器的數據，根據旋轉矢量算法計算四元素迭代更新矩陣M' [q(h)]， 並根據下式： Q(tk) = M' [q(h)] · Q' （tj^) 更新四元素值Q(tk)，式中Q(tk)為當前時刻四元素迭代值，M' [q(h)]為四元素更新矩 陣，Q'（tg)為上一時刻四元素重構值；根據更新後的四元素計算值Q(tk)，計算得到載體姿 態信息，分別是航向角俯仰角Θ以及橫滾角Y ; 2) 、利用觀測傳感器數據，計算載體姿態信息的觀測值，分別是航向角#，俯仰角|以 及橫滾角^並進一步計算姿態四元素的觀測值; 3) 、利用不同傳感器的數據，判斷和估計載體的運動狀態，計算數據融合的互補重構參 數a = g (Χι, χ2, χ3,...)，其中Χι, χ2, χ3,...為各個傳感器的數據； 4) 、根據步驟3)中的數據融合的互補重構參數α，對步驟1)中得到的Q(tk) 和步驟2)中得到的進行數據融合，重構載體姿態四元素 Q'（tk)，其表達式為 ))，並對其進行標準化處理後用於下一次迭代更新，並返回步驟1)。
2. 根據權利要求1所述的方法，其特徵在於：在步驟1)所述計算載體的姿態信息時， 使用了旋轉矢量算法對載體姿態信息進行了迭代更新，步驟1)中的載體的姿態信息的計 算包括以下步驟： (1. 1)獲取陀螺儀傳感器的數據，並對其進行直流偏置修正處理。先預先採樣若干次計 算其均值作為三軸角速率的偏置值，再分別以三軸角速率的陀螺儀當前採樣值減去該偏置 值作為修正後的三軸角速率的輸出值； (1. 2)根據權利要求書2所述步驟1)中獲得的陀螺儀傳感器的數據計算四元素更新矩 陣 M，[q(h)];
Φ00為前一次採樣時刻和當前採樣時刻[tk，tk+1]時間段內的等效旋轉矢量， Φχ〇ι)，Φ>)，Φζ〇ι)為的旋轉矢量三軸的分量； (1.3) 根據步驟4)中互補重構後的四元素值Q'（tu)，按照下式更新姿態四元素的值： Q(tk) = M' [q(h)] · Q' （tj^) 其中，Q'為給定的四元素初始值； (1.4) 根據更新後的四元素迭代值Q(tk)，按照下式計算載體姿態信息，分別是航向角 爐，俯仰角Θ以及橫滾角 Y :
其中，qQ，qi，q2, q3為四元素 Q(tk)的分量。
3.根據權利要求1所述的方法，其特徵在於：在所述步驟2)計算載體的姿態信息觀測 值時，使用觀測傳感器的信息進行載體姿態信息觀測值的計算，並根據載體姿態信息觀測 值計算得到四元素的觀測值步驟2)中的載體的姿態信息觀測值的計算包括以下步 驟： (2. 1)獲取加速度傳感器的數據ax_OTg，ay_OTg，a z_OTg，進行直流偏置修正。首先採樣若干 次計算其均值作為三軸加速度的偏置值，再分別以三軸加速度的當前採樣值減去該偏置值 作為修正後的三軸加速度輸出值； (2.2) 根據力學原理計算靜態情況下的觀測傳感器（加速度傳感器）包含的載體姿 態信息，其中
：寸上述載體姿態信息進行濾波，獲得加速度傳 感器包含的載體姿態信息氣^同時獲取磁阻傳感器數據，數據處理後得到載體航向角信息 ΦI (2.3) 根據權利要求書3所述步驟2)中載體姿態信息的觀測值忒麼#，按計算載體姿 態矩陣元素的觀測值，具體計算公式如下：
(2. 4)根據權利要求書3所述步驟3)中載體姿態矩陣的觀測值計算載體姿態四元素的 觀測值0? >的各個分量，具體計算公式如下：
4. 根據權利要求1所述的方法，其特徵在於：步驟3)所述根據多傳感器的數據，構造 並計算得出表徵載體運動狀態信息的互補重構參數α，其意義在於，α的值表示載體的運 動狀態，α -1則表明載體的運動比較劇烈可認為載體處在高速運動狀態，α -〇則表明 載體運動不劇烈，可以認為載體趨於靜止狀態；步驟3)中的計算表徵載體運動狀態的互補 重構參數α包含以下步驟： (3. 1)獲得陀螺儀傳感器的數據ωχ，coy，ωζ，再獲得加速度傳感器的數據^，\，並進 行數據處理，由於az在重力方向有一個常值，以此az的值不能作為判斷載體運行狀態的數 據； (3. 2)計算互補重構參數a = g(Xl, χ2, χ3, . . . )，Χι, χ2, χ3,...為各傳感器數據。 優選的，計算互補重構參數數學表達式如下所示：ct = max/ (max+1);其中，max 是集合{Κω · I ωχ|，Κω · I ω」，Κω · I c〇z|，Ka· |ax|，Ka· |ay|}的最大值，其中
?s__，分別為陀螺儀和加速度傳感器靜態測得的最大讀 數。
5. 根據權利要求1所述的方法，其特徵在於：步驟4)所述的對載體的姿態四元素計算 值Q(tk)和姿態四元素觀測值0(?)進行融合時，考慮了載體運動狀態信息，利用互補重構 參數α融合計算值和觀測值得到新的載體姿態四元素，融合公式如下所示：
並對融合後的載體姿態四元素進行標準歸一化處理。
【文檔編號】G01C21/20GK104101345SQ201410247629
【公開日】2014年10月15日 申請日期:2014年6月5日 優先權日:2014年6月5日
【發明者】董利達, 尹俊, 遲天陽, 管林波 申請人:杭州師範大學