基于互補(bǔ)重構(gòu)技術(shù)的多傳感器姿態(tài)融合方法
【專利摘要】基于互補(bǔ)重構(gòu)技術(shù)的多傳感器姿態(tài)融合方法，主要包括：四元素迭代更新及載體姿態(tài)計(jì)算，多傳感器載體姿態(tài)觀測及四元素觀測值計(jì)算；互補(bǔ)重構(gòu)參數(shù)估計(jì)；互補(bǔ)重構(gòu)四元素值計(jì)算等四個步驟。其主要優(yōu)點(diǎn)：利用四元素觀測值修正上一時刻四元素計(jì)算值，從而獲得互補(bǔ)重構(gòu)的四元素值，并利用此值進(jìn)行當(dāng)前時刻的四元素迭代更新，從機(jī)理上消除了算法的累積誤差；同時將四元素更新算法和重構(gòu)算法合二為一，提高了算法的執(zhí)行效率。
【專利說明】基于互補(bǔ)重構(gòu)技術(shù)的多傳感器姿態(tài)融合方法

【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及慣性導(dǎo)航領(lǐng)域，尤其是捷聯(lián)慣性導(dǎo)航系統(tǒng)在利用單一慣性傳感器（如 陀螺儀傳感器）數(shù)據(jù)進(jìn)行載體姿態(tài)測量時，雖然短時間精度好，但是長期運(yùn)行時的姿態(tài)測 量結(jié)果由于積分作用產(chǎn)生累積誤差，導(dǎo)致導(dǎo)航系統(tǒng)精度下降。本發(fā)明利用多傳感器信息進(jìn) 行數(shù)據(jù)融合，可以用較低成本提高捷聯(lián)慣性導(dǎo)航系統(tǒng)長時間精度。

【背景技術(shù)】
[0002] 捷聯(lián)慣性導(dǎo)航系統(tǒng)在計(jì)算時，需要把載體測得的加速度，角速度變化從載體坐標(biāo) 系轉(zhuǎn)換到導(dǎo)航坐標(biāo)系中，假設(shè)空間位置P在導(dǎo)航坐標(biāo)系η的坐標(biāo)表示為P (xn，yn，zn)，在載體 所在坐標(biāo)系b的坐標(biāo)表示為P (xb，yb，zb)，則兩者之間滿足如下關(guān)系：

【權(quán)利要求】
1. 基于互補(bǔ)重構(gòu)技術(shù)的多傳感器姿態(tài)融合方法，包括以下步驟： 1) 、利用陀螺儀傳感器的數(shù)據(jù)，根據(jù)旋轉(zhuǎn)矢量算法計(jì)算四元素迭代更新矩陣M' [q(h)]， 并根據(jù)下式： Q(tk) = M' [q(h)] · Q' （tj^) 更新四元素值Q(tk)，式中Q(tk)為當(dāng)前時刻四元素迭代值，M' [q(h)]為四元素更新矩 陣，Q'（tg)為上一時刻四元素重構(gòu)值；根據(jù)更新后的四元素計(jì)算值Q(tk)，計(jì)算得到載體姿 態(tài)信息，分別是航向角俯仰角Θ以及橫滾角Y ; 2) 、利用觀測傳感器數(shù)據(jù)，計(jì)算載體姿態(tài)信息的觀測值，分別是航向角#，俯仰角|以 及橫滾角^并進(jìn)一步計(jì)算姿態(tài)四元素的觀測值; 3) 、利用不同傳感器的數(shù)據(jù)，判斷和估計(jì)載體的運(yùn)動狀態(tài)，計(jì)算數(shù)據(jù)融合的互補(bǔ)重構(gòu)參 數(shù)a = g (Χι, χ2, χ3,...)，其中Χι, χ2, χ3,...為各個傳感器的數(shù)據(jù)； 4) 、根據(jù)步驟3)中的數(shù)據(jù)融合的互補(bǔ)重構(gòu)參數(shù)α，對步驟1)中得到的Q(tk) 和步驟2)中得到的進(jìn)行數(shù)據(jù)融合，重構(gòu)載體姿態(tài)四元素 Q'（tk)，其表達(dá)式為 ))，并對其進(jìn)行標(biāo)準(zhǔn)化處理后用于下一次迭代更新，并返回步驟1)。
2. 根據(jù)權(quán)利要求1所述的方法，其特征在于：在步驟1)所述計(jì)算載體的姿態(tài)信息時， 使用了旋轉(zhuǎn)矢量算法對載體姿態(tài)信息進(jìn)行了迭代更新，步驟1)中的載體的姿態(tài)信息的計(jì) 算包括以下步驟： (1. 1)獲取陀螺儀傳感器的數(shù)據(jù)，并對其進(jìn)行直流偏置修正處理。先預(yù)先采樣若干次計(jì) 算其均值作為三軸角速率的偏置值，再分別以三軸角速率的陀螺儀當(dāng)前采樣值減去該偏置 值作為修正后的三軸角速率的輸出值； (1. 2)根據(jù)權(quán)利要求書2所述步驟1)中獲得的陀螺儀傳感器的數(shù)據(jù)計(jì)算四元素更新矩 陣 M，[q(h)];
Φ00為前一次采樣時刻和當(dāng)前采樣時刻[tk，tk+1]時間段內(nèi)的等效旋轉(zhuǎn)矢量， Φχ〇ι)，Φ>)，Φζ〇ι)為的旋轉(zhuǎn)矢量三軸的分量； (1.3) 根據(jù)步驟4)中互補(bǔ)重構(gòu)后的四元素值Q'（tu)，按照下式更新姿態(tài)四元素的值： Q(tk) = M' [q(h)] · Q' （tj^) 其中，Q'為給定的四元素初始值； (1.4) 根據(jù)更新后的四元素迭代值Q(tk)，按照下式計(jì)算載體姿態(tài)信息，分別是航向角 爐，俯仰角Θ以及橫滾角 Y :
其中，qQ，qi，q2, q3為四元素 Q(tk)的分量。
3.根據(jù)權(quán)利要求1所述的方法，其特征在于：在所述步驟2)計(jì)算載體的姿態(tài)信息觀測 值時，使用觀測傳感器的信息進(jìn)行載體姿態(tài)信息觀測值的計(jì)算，并根據(jù)載體姿態(tài)信息觀測 值計(jì)算得到四元素的觀測值步驟2)中的載體的姿態(tài)信息觀測值的計(jì)算包括以下步 驟： (2. 1)獲取加速度傳感器的數(shù)據(jù)ax_OTg，ay_OTg，a z_OTg，進(jìn)行直流偏置修正。首先采樣若干 次計(jì)算其均值作為三軸加速度的偏置值，再分別以三軸加速度的當(dāng)前采樣值減去該偏置值 作為修正后的三軸加速度輸出值； (2.2) 根據(jù)力學(xué)原理計(jì)算靜態(tài)情況下的觀測傳感器（加速度傳感器）包含的載體姿 態(tài)信息，其中
：寸上述載體姿態(tài)信息進(jìn)行濾波，獲得加速度傳 感器包含的載體姿態(tài)信息氣^同時獲取磁阻傳感器數(shù)據(jù)，數(shù)據(jù)處理后得到載體航向角信息 ΦI (2.3) 根據(jù)權(quán)利要求書3所述步驟2)中載體姿態(tài)信息的觀測值忒么#，按計(jì)算載體姿 態(tài)矩陣元素的觀測值，具體計(jì)算公式如下：
(2. 4)根據(jù)權(quán)利要求書3所述步驟3)中載體姿態(tài)矩陣的觀測值計(jì)算載體姿態(tài)四元素的 觀測值0? >的各個分量，具體計(jì)算公式如下：
4. 根據(jù)權(quán)利要求1所述的方法，其特征在于：步驟3)所述根據(jù)多傳感器的數(shù)據(jù)，構(gòu)造 并計(jì)算得出表征載體運(yùn)動狀態(tài)信息的互補(bǔ)重構(gòu)參數(shù)α，其意義在于，α的值表示載體的運(yùn) 動狀態(tài)，α -1則表明載體的運(yùn)動比較劇烈可認(rèn)為載體處在高速運(yùn)動狀態(tài)，α -〇則表明 載體運(yùn)動不劇烈，可以認(rèn)為載體趨于靜止?fàn)顟B(tài)；步驟3)中的計(jì)算表征載體運(yùn)動狀態(tài)的互補(bǔ) 重構(gòu)參數(shù)α包含以下步驟： (3. 1)獲得陀螺儀傳感器的數(shù)據(jù)ωχ，coy，ωζ，再獲得加速度傳感器的數(shù)據(jù)^，\，并進(jìn) 行數(shù)據(jù)處理，由于az在重力方向有一個常值，以此az的值不能作為判斷載體運(yùn)行狀態(tài)的數(shù) 據(jù)； (3. 2)計(jì)算互補(bǔ)重構(gòu)參數(shù)a = g(Xl, χ2, χ3, . . . )，Χι, χ2, χ3,...為各傳感器數(shù)據(jù)。 優(yōu)選的，計(jì)算互補(bǔ)重構(gòu)參數(shù)數(shù)學(xué)表達(dá)式如下所示：ct = max/ (max+1);其中，max 是集合{Κω · I ωχ|，Κω · I ω」，Κω · I c〇z|，Ka· |ax|，Ka· |ay|}的最大值，其中
?s__，分別為陀螺儀和加速度傳感器靜態(tài)測得的最大讀 數(shù)。
5. 根據(jù)權(quán)利要求1所述的方法，其特征在于：步驟4)所述的對載體的姿態(tài)四元素計(jì)算 值Q(tk)和姿態(tài)四元素觀測值0(?)進(jìn)行融合時，考慮了載體運(yùn)動狀態(tài)信息，利用互補(bǔ)重構(gòu) 參數(shù)α融合計(jì)算值和觀測值得到新的載體姿態(tài)四元素，融合公式如下所示：
并對融合后的載體姿態(tài)四元素進(jìn)行標(biāo)準(zhǔn)歸一化處理。
【文檔編號】G01C21/20GK104101345SQ201410247629
【公開日】2014年10月15日 申請日期:2014年6月5日 優(yōu)先權(quán)日:2014年6月5日
【發(fā)明者】董利達(dá), 尹俊, 遲天陽, 管林波 申請人:杭州師范大學(xué)