無陀螺慣性測量系統(tǒng)約束角速度估計方法
【專利摘要】本發(fā)明提供了一種無陀螺慣性測量系統(tǒng)約束角速度估計方法，該方法具有估計步驟清晰明了、估計誤差不隨時間變化的特點，可用于無陀螺慣性測量系統(tǒng)冗余加速度計配置情況下角速度的估計，能夠有效提高角速度的估計精度。
【專利說明】
無陀螺慣性測量系統(tǒng)約束角速度估計方法
技術領域
[0001] 本發(fā)明屬于慣性測量應用領域，涉及慣性測量系統(tǒng)高精度導航計算方法，特指一 種基于擴展Kalman濾波技術的無陀螺慣性測量系統(tǒng)約束角速度估計方法。
【背景技術】
[0002] 傳統(tǒng)的慣性測量系統(tǒng)包括三個陀螺儀以測量載體角速度，三個加速度計測量三個 方向比力，直接積分角速度可用，從而得到載體姿態(tài)，所以所得數(shù)值解(估計值）中的姿態(tài)誤 差隨著時間發(fā)生線性增長，而位置涉及到比力和姿態(tài)的二次積分，使得位置誤差隨著時間 的增長發(fā)生立方級數(shù)的增長。在無陀螺慣性導航系統(tǒng)中，載體的角加速度正比于加速度計 輸出，需經(jīng)過一次積分才能得到載體的角速度估計值，由于加速度計測量誤差的影響，這使 得角速度估計值的誤差隨時間的增加而增大，而傳統(tǒng)的有陀螺儀慣性測量系統(tǒng)，可以直接 用于測量角速度，測量精度只與陀螺儀本身的精度相關，避免了誤差增大對角速度結果的 影響。正是因為無陀螺慣性測量系統(tǒng)需要積分才能得到角速度，其姿態(tài)誤差與時間的平方 成正比，而位置誤差與時間的四次方成正比，所以無陀螺慣性測量系統(tǒng)誤差累積速度比傳 統(tǒng)慣性系統(tǒng)更快。但正是由于無陀螺慣性測量系統(tǒng)中沒有陀螺儀部件，所以具有可靠性高、 成本低的優(yōu)勢。
[0003] 對于無陀螺慣性測量系統(tǒng)而言，研究高精度的角速度估計方法非常重要，可以有 效降低無陀螺慣性測量系統(tǒng)導航計算誤差積累速度，提高導航精度。這可以從兩個方面考 慮：一方面是利用優(yōu)良的誤差補償方法提高導航計算精度;另一方面是直接從導航解算方 法上入手。由于無陀螺慣性測量系統(tǒng)一般都采用冗余加速度計設計，加速度計個數(shù)一般都 大于六個，例如采用九加速度計構型、十二加速度計構型，每個加速度計都能夠獲得一個獨 立測量量，而待解導航參數(shù)是加速度和角加速度，只有六個量，因此無陀螺慣性測量系統(tǒng)中 存在大量的冗余信息，利用無陀螺慣性測量系統(tǒng)加速度計輸出信息可以直接得到角速度乘 積項、平方項的估計結果，但并不能夠據(jù)此直接得到準確度較高的角速度的估計值。

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

[0004] 本發(fā)明提供了一種無陀螺慣性測量系統(tǒng)約束角速度估計方法，該方法解決了現(xiàn)有 技術中對無陀螺慣性測量系統(tǒng)的冗余信息利用度低、所得角速度估計值與真實值之間的誤 差會隨的技術問題。
[0005] 本發(fā)明提供的一種無陀螺慣性測量系統(tǒng)約束角速度估計方法，步驟(1):對于所含 加速度計大于12個且能獲得角速度二次項估計值的無陀螺慣性測量系統(tǒng)，利用所有加速度 計的輸出得到線加速度、角加速度、角速度平方項、角速度交叉項次項的線性估計和估計方 差；
[0006] 步驟(1)包括以下步驟：
[0007] 假設無陀螺慣性測量系統(tǒng)包含N，其中N 2 12個加速度計，加速度計矢量Y的輸出表 示為：
[0009]其中?為載體待測的三維角加速度矢量，妒為待測的載體三維視加速度矢量，ωχ， ωγ，ωζ*角速度ω的三個分量，Υ為Ν個加速度計組成的測量矢量；
，di為第i個加速度計敏感軸方向，<表示矢量di的轉(zhuǎn) 置，η為第i個加速度計中心位置矢量4 ^ ?3^，1 = 1，2，-，小為加速度計個數(shù)，了 稱為構型矩陣：
[0011]
:，則?ω為NX6維矩陣
，其中，Χ3為角速度交叉 項矢量，Χ4為角速度平方項矢量，Χ3和Χ4稱為角速度二次項，貝lj:
[0013]其中X3為角速度交叉項、X4為角速度平方項，根據(jù)式(6)，角加速度矢量、視加速度 矢量、角速度交叉項和角速度平方項的線性估計為：
[0015] 此處所得為關于角加速度矢量?、視加速度矢量妒、角速度交叉項X3和角速度平 方項Χ4的中間估計量；
[0016] 無陀螺慣性測量系統(tǒng)中的各加速度計誤差已完成標定和補償，加速度計測量誤差 為零均值高斯白噪聲，測量方差為σ2,無陀螺系統(tǒng)中的各加速度計測量方差都相同，式(7) 的估計方差為：
[0017] Qest=([J D?]T[J DU])-1D[Y] = ([J D?]t[J Dm])-V (8)
[0018] 其中Qest為12X12維矩陣；
[0019] 步驟(2):根據(jù)角加速度的估計值，建立角速度微分方程，根據(jù)角加速度的估計方 差，構建角速度狀態(tài)方程過程噪聲，并將狀態(tài)方程離散化；
[0020] 步驟(2)包括以下步驟：
[0021] 實際待估變量是角速度矢量ω，令Kalman濾波中的狀態(tài)變量為角速度矢量ω = [ωχ c0y ωζ]τ，在離散情況下，角速度差分方程可表示為：
[0023]其中，Δ t為時間步長，cox,k、coy,k、co z,k*別為k時刻的Χ、Υ、Ζ三個方向的角速度 值，ω χ,η、ω y,η、ω ζ,η分別為k-Ι時刻的X、Υ、Ζ三個方向的角速度值，△ t為濾波時間步 長，'^,1{-1，《^,1^1，'?2,1^1為1^-1時刻角加速度解算的隨機噪聲，定義過程量 ：
[0025]實際計算中u可以根據(jù)式(7)估計的角加速度石乘以時間步長得到為：
[0027] 將式（11)代入式(9)，同時寫成矢量形式，角速度ω的離散化狀態(tài)動力學模型可表 示為：
[0028] ωk= ωk-i+Uk-i+Wk-i Δ t (2)
[0029] 根據(jù)式(8)得到的角加速度估計方差是加速度計隨機誤差的線性組合，為矩陣Qest 的第一行至第三行以及第一列至第三列組成的矩陣，式（12)動力學模型的過程噪聲方差 為：
[0030] Q" = ( At)2Qest(l:3,l:3) (13)
[0031] 假設角速度狀態(tài)變量初值為：
[0032] ?0,ο=[4? 4α]Γ (Μ)
[0033] 其中?Λ。，今.。，黽。是1^ = 0的初始時刻X、Υ、Ζ三個方向的角速度初始值，角速度初始 值的方差假設為Ρο/ο，為3 X 3矩陣，具體計算時根據(jù)情況給定；
[0034]角速度狀態(tài)變量的預測值為：
[0035] =iVw-i+?i--i (!5)
[0036] 其中，表示k-Ι時刻的估計值，％/A_AKalman濾波中k時刻的預測值，Uk-i表 示第k-Ι時刻的計算值，根據(jù)Kalman濾波算法，得到預測值％的方差為：
[0037] Pk/k-i = Pk-i/k-i+QMk-i (16)
[0038] 其中Pk-i/k-i為k_ 1時刻估計值的方差矩陣，Pk/k-i為k時刻角速度預測值, A 4的方差 矩陣，Quk-i為k-Ι時刻過程的方差矩陣，由（13)式給出；
[0039] 步驟(3):根據(jù)角速度二次項的估計值和估計方差，建立角速度測量方程以及測量 噪聲的方差矩陣，并將方程離散化；
[0040] 步驟(3)包括以下步驟：
[0041] 將式(7)給出的角速度二次項估計值作為濾波過程中的測量模型，同時考慮加速 度計測量所存在的隨機誤差，得到角速度二次項的測量模型表示為：
[0043 ]其中Vk是角速度二次項估計值的隨機誤差；
[0044] 隨機誤差方差表示為：
[0045] Rk = Qest(7:12,7:12) (18)
[0046] 同樣根據(jù)式(8)可知，式（12)所示動力學模型的過程噪聲與式（17)所示測量模型 噪聲的協(xié)方差矩陣為矩陣式(8)所示Q est的第1行到3行和第7列到12列組成的矩陣：
[0047] Mk = Qest(l:3,7:12)At (19)
[0048] Mk為動力學模型過程噪聲與測量模型噪聲協(xié)方差陣為3X6維矩陣，Δ t為時間步 長，與式(10)中時間步長相同；
[0049] 測量模型的Jacobian矩陣Hk為：
[0051 ]其中為式(15)中的角速度預報值，S為求偏導數(shù)符號；
[0052]則Kalman濾波增益矩陣的更新方程為：
[0054]濾波方差的修正方程為：
[0056]其中，Pk/k為k時刻角速度估計值的方差矩陣，角速度變量的修正方程為：
[0058] 其中，表示k時刻的無陀螺慣性系統(tǒng)角速度估計值：
是根據(jù)(7)式計 算得到的角速度二次項解向量；
[0059] 根據(jù)結果，式（15)、式（16)、式(21)、式(22)和式(23)即為無陀螺慣性系統(tǒng)角速度 約束估計中Ka 1 man濾波遞推公式，根據(jù)Ka 1 man濾波遞推公式和濾波初值和初始方差 Ρθ/0即可得出％jPPk/k。
[0060] 本發(fā)明的技術效果：
[0061] 1、本發(fā)明提供無陀螺慣性測量系統(tǒng)約束角速度估計方法充分利用冗余加速度計 測量所得輸出，首先獲得角加速度、角速度二次項的線性估計，然后基于Kalman濾波法得到 準確度較高的角速度估計值，本發(fā)明提供方法得到的角速度估計值的誤差不隨測量時間的 增加而增大。
[0062] 2、本發(fā)明提供的無陀螺慣性測量系統(tǒng)約束角速度估計方法簡單明了，所得結果準 確可靠，誤差不隨時間的增加而變化。
[0063] 3、本發(fā)明提供的無陀螺慣性測量系統(tǒng)約束角速度估計方法中角速度估計誤差不 隨測量時間增加而變大，使得所得估計值能等價于陀螺儀的角速度測量結果，減少所得估 計值與真實值之間的差值。
[0064] 4、本發(fā)明提供的無陀螺慣性測量系統(tǒng)約束角速度估計方法可根據(jù)實際使用中的 初始值進行實時遞推計算。因此該方法可以有效提高無陀螺慣性測量系統(tǒng)角速度估計精 度。
[0065] 具體請參考根據(jù)本發(fā)明的無陀螺慣性測量系統(tǒng)約束角速度估計方法提出的各種 實施例的如下描述，將使得本發(fā)明的上述和其他方面顯而易見。
【附圖說明】
[0066] 圖1是本發(fā)明優(yōu)選實施例無陀螺慣性測量系統(tǒng)約束角速度估計方法的流程示意 圖；
[0067] 圖2是典型12加速度計無陀螺慣性測量系統(tǒng)的幾何構型示意圖，箭頭方向表示加 速度計的敏感軸指向，加速度計中心在立方體棱的中點處，共四組12個加速度計，立方體邊 長為0.2m;
[0068] 圖3是本發(fā)明優(yōu)選實施例方法用于仿真計算中時所得X方向角速度估計值減去角 速度真值的差值隨濾波時間變化過程的示意圖；
[0069] 圖4是本發(fā)明優(yōu)選實施例方法用于仿真計算中時所得Y方向角速度估計值減去角 速度真值的差值隨濾波時間的變化過程示意圖；
[0070] 圖5是本發(fā)明優(yōu)選實施例方法用于仿真計算中時所得Z方向角速度估計值減去角 速度真值時的差值隨濾波時間的變化過程示意圖；
[0071] 圖6是本發(fā)明優(yōu)選實施例方法用于仿真計算中時所得X、Y、Z三個方向上角速度估 計平均平方誤差根隨時間變化的情況示意圖。
【具體實施方式】
[0072] 構成本申請的一部分的附圖用來提供對本發(fā)明的進一步理解，本發(fā)明的示意性實 施例及其說明用于解釋本發(fā)明，并不構成對本發(fā)明的不當限定。
[0073] 本文中得到的利用冗余信息提高角加速度和線加速度的計算精度，是無陀螺慣性 測量系統(tǒng)導航計算方法研究中的主要方向。眾多研究人員對這一問題提出了多種角速度計 算方法，核心思想是利用無陀螺慣性系統(tǒng)中的冗余估計值估計載體角速度，但其算法設計 和估計精度上仍然有待完善和提尚。
[0074]為完成無陀螺慣性測量系統(tǒng)角速度的高精度估計，首先根據(jù)無陀螺慣性測量系統(tǒng) 構型特點和冗余加速度計輸出，給出線加速度、角加速度、以及角速度二次項的線性估計； 其次根據(jù)角速度微分方程建立動力學模型，根據(jù)角速度二次項估計建立測量模型；最后利 用Kalman濾波技術得到角速度的高精度估計。
[0075] 本發(fā)明的技術方案是:首先根據(jù)無陀螺慣性系統(tǒng)構型特點和冗余加速度計輸出， 給出線加速度、角加速度以及角速度二次項的線性估計;其次根據(jù)角速度微分方程建立動 力學模型，根據(jù)角速度二次項估計值建立測量模型;最后利用擴展Kalman濾波技術得到角 速度的高精度估計。本發(fā)明適用于具有12個以上加速度計且能獲得角速度二次項估計值的 無陀螺慣性系統(tǒng)。
[0076] 參見圖1，本發(fā)明提供的一種無陀螺慣性系統(tǒng)約束角速度估計方法，包括以下步 驟：
[0077] 本發(fā)明提供的一種無陀螺慣性測量系統(tǒng)約束角速度估計方法，步驟(1):對于所含 加速度計大于12個且能獲得角速度二次項估計值的無陀螺慣性測量系統(tǒng)，利用所有加速度 計的輸出得到線加速度、角加速度、角速度平方項、角速度交叉項次項的線性估計和估計方 差；
[0078] 步驟(1)包括以下步驟：
[0079]假設無陀螺慣性測量系統(tǒng)包含N，其中N 2 12個加速度計，加速度計矢量Y的輸出表 示為：
[0081]其中?為載體待測的三維角加速度矢量，爐為待測的載體三維視加速度矢量， ωχ，ωγ，（〇2為角速度ω的三個分量，Υ為Ν個加速度計組成的測量矢量；
di為第i個加速度計敏感軸方向，<表示矢量di的轉(zhuǎn) 置，ri為第i個加速度計中心位置矢量，ri = [rxi ryi rzi]T，i = 1，2，··，，N為加速度計個數(shù)，J 稱為構型矩陣：
[0083]
<則0?為~乂6維矩陣
其中，X3為角速度交叉 項矢量，X4為角速度平方項矢量，X3和X4稱為角速度二次項，貝1J:
[0085]其中X3為角速度交叉項、X4為角速度平方項，根據(jù)式(6)，角加速度矢量、視加速度 矢量、角速度交叉項和角速度平方項的線性估計為：
[0087] 此處所得為關于角加速度矢量?、視加速度矢量#、角速度交叉項X3和角速度平 方項X4的中間估計量；
[0088] 無陀螺慣性測量系統(tǒng)中的各加速度計誤差已完成標定和補償，加速度計測量誤差 為零均值高斯白噪聲，測量方差為σ2,無陀螺系統(tǒng)中的各加速度計測量方差都相同，式(7) 的估計方差為：
[0089] Qest=([J D?]T[J DU])-1D[Y] = ([J D?]t[J ?ω])^2 (8)
[0090] 其中Qest為12X12維矩陣；
[0091]步驟(2):根據(jù)角加速度的估計值，建立角速度微分方程，根據(jù)角加速度的估計方 差，構建角速度狀態(tài)方程過程噪聲，并將狀態(tài)方程離散化；
[0092] 步驟(2)包括以下步驟：
[0093]實際待估變量是角速度矢量ω，令Kalman濾波中的狀態(tài)變量為角速度矢量ω = [ωχ c0y ωζ]τ，在離散情況下，角速度差分方程可表示為：
[0095] 其中，Δ t為時間步長，cox,k、coy,k、co z,k分別為k時刻的Χ、Υ、Ζ三個方向的角速度 值，ω χ,η、ω y,η、ω ζ,η分別為k-Ι時刻的X、Υ、Ζ三個方向的角速度值，△ t為濾波時間步 長，'^,1{-1，《^,1^1，'?2,1^1為1^-1時刻角加速度解算的隨機噪聲，定義過程量 ：
[0096] u = [?.Δ/' ?. Δ/' ? Δ/'j (10)
[0097] 實際計算中u可以根據(jù)式(7)估計的角加速度?乘以時間步長得到為： |- ^ -.J'
[0098] u- ojM (h^At ? Α? 〇 ^)
[0099] 將式（11)代入式(9)，同時寫成矢量形式，角速度ω的離散化狀態(tài)動力學模型可表 示為：
[0100] ωk= ωk-i+Uk-i+Wk-i Δ t (3)
[0101] 根據(jù)式(8)得到的角加速度估計方差是加速度計隨機誤差的線性組合，為矩陣Qest 的第一行至第三行以及第一列至第三列組成的矩陣，式（12)動力學模型的過程噪聲方差 為：
[0102] Q" = ( At)2Qest(l:3,l:3) (13)
[0103] 假設角速度狀態(tài)變量初值為：
[0105] 其中倉"，今."，黽Q是1^ = 0的初始時刻X、Y、Z三個方向的角速度初始值，角速度初始 值的方差假設為Po/o，為3 X 3矩陣，具體計算時根據(jù)情況給定；
[0106] 角速度狀態(tài)變量的預測值為：
[0107] i〇klk_, =i〇k_Vk^+uk_i (15)
[0108] 其中，表示k-1時刻的估計值，Ai^Kalman濾波中k時刻的預測值，Uk-1表 示第k-Ι時刻的計算值，根據(jù)Kalman濾波算法，得到預測值％,η的方差為：
[0109] Pk/k-i = Pk-i/k-i+QMk-i (16)
[0110] 其中Pk-Vk-Ak-l時刻估計值的方差矩陣，Pk/k-Ak時刻角速度預測值的方差 矩陣，Quk-i為k-Ι時刻過程的方差矩陣，由（13)式給出；
[0111] 步驟(3):根據(jù)角速度二次項的估計值和估計方差，建立角速度測量方程以及測量 噪聲的方差矩陣，并將方程離散化；
[0112] 步驟(3)包括以下步驟：
[0113] 將式(7)給出的角速度二次項估計值作為濾波過程中的測量模型，同時考慮加速 度計測量所存在的隨機誤差，得到角速度二次項的測量模型表示為：
[0115] 其中外是角速度二次項估計值的隨機誤差；
[0116] 隨機誤差方差表示為：
[0117] Rk = Qest(7:12,7:12) (18)
[0118] 同樣根據(jù)式(8)可知，式（12)所示動力學模型的過程噪聲與式（17)所示測量模型 噪聲的協(xié)方差矩陣為矩陣式(8)所示Q est的第1行到3行和第7列到12列組成的矩陣：
[0119] Mk = Qest(l:3,7:12)At (19)
[0120] Mk為動力學模型過程噪聲與測量模型噪聲協(xié)方差陣為3X6維矩陣，Δ t為時間步 長，與式(10)中時間步長相同；
[0121] 測量模型的Jacobian矩陣Hk為：
[0123] 其中為式(15)中的角速度預報值，(3為求偏導數(shù)符號；
[0124] 貝ijKalman濾波增益矩陣的更新方程為：
[0126]濾波方差的修正方程為：
[0128]其中，Pk/k為k時刻角速度估計值化,&的方差矩陣，角速度變量的修正方程為：

[0130] 其中，4,1表示k時刻的無陀螺慣性系統(tǒng)角速度估計值： 是根據(jù)(7)式計 算得到的角速度二次項解向量；
[0131] 根據(jù)結果，式（15)、式（16)、式(21)、式(22)和式(23)即為無陀螺慣性系統(tǒng)角速度 約束估計中Kalman濾波遞推公式，根據(jù)Kalman濾波遞推公式和濾波初值(UP初始方差Po/o 即可得出和Pk/k。
[0132] 通過上述方法可以實現(xiàn)對角速度的估計，所得角速度估計值的誤差不隨測量時間 的增加而增大。
[0133] 具體步驟如下：
[0134] 步驟(1):計算無陀螺慣性系統(tǒng)角速度二次項估計
[0135] 假設無陀螺慣性測量系統(tǒng)包含N(N 2 12)個加速度計，無陀螺慣性測量系統(tǒng)基本測 量方程為
[0137]其中&為載體待測的三維角加速度矢量，妒為待測的載體三維視加速度矢量，Ω 為角速度矢量形成的反對稱矩陣，
，ωχ，《y，ωζ為角速度ω的三個分 量，Υ為Ν個加速度計組成的測量矢量，cU為第i個加速度計敏感軸方向，rff表示矢量cU的轉(zhuǎn) 置，ri為第i個加速度計中心位置矢量，N為加速度計個數(shù)，J稱為構型矩陣：
[0139] 在加速度計輸出方程式（1)中，= 項可改寫為：
[0145]
，則?ω為NX 6維矩陣
，.其中，X3為角速度交 叉項矢量，X4為角速度平方項矢量，X3和X4稱為角速度二次項，貝lj
[0147]無陀螺慣性系統(tǒng)要求能獲得角速度二次項估計，則角加速度矢量威、線視加速度 矢量#、角速度交叉項和角速度平方項的線性估計為
[0149] 假設無陀螺慣性測量系統(tǒng)中的加速度計誤差已完成標定和補償，加速度計測量誤 差為零均值高斯白噪聲，測量方差為σ2,無陀螺系統(tǒng)中加速度計測量方差都相同?？梢钥?出，此時角加速度?和線視加速度#的解算不需要角速度信息，同時還可以得到角速度交 叉項和平方項二次項的線性估計。式(7)的估計方差為
[0150] Qest=([J D?]T[J DU])-1D[Y] = ([J D?]t[J ?ω])^2 (8)
[0151] 其中Qest為12X12維矩陣，角加速度、線視加速度以及角速度二項項的估計方差只 與加速度計測量精度和無陀螺慣性測量系統(tǒng)構型參數(shù)有關，而與時間無關，從而避免了工 作時間對估計結果的影響。
[0152] 步驟(2):根據(jù)角加速度估計值，建立角速度動力學方程，根據(jù)角加速度的估計方 差，給出動力學方程噪聲，并將動力學方程離散化；
[0153] 令狀態(tài)變量為角速度矢量ω=[ωχ c0y ωζ]τ，在離散情況下，角速度差分方程可 表示為，
[0155] 其中Δ t為時間步長，cox,k、coy,k、《^分別為k時刻的Χ、Υ、Ζ三個方向的角速度值， ωχ^、ωγ,!^、ωζ,Η分別為k-Ι時刻的Χ、Υ、Ζ三個方向的角速度值，Δ t為濾波時間步長， ￥\1{-1，《^,1^1，'?2,1^1為1^-1時刻角加速度解算的隨機噪聲，定義過程量
[0156] u - 0)t At (b (10)
[0157] 實際計算中u可以根據(jù)式(7)估計的角加速度石乘以時間步長得到，
[0158] u - ?λΑ? ?νΑ? (11)
[0159] 將式（11)代入式(9)，同時寫成矢量形式，角速度ω的離散化狀態(tài)動力學模型可表 示為
[0160] ωk= ω k-i+Uk-i+Wk-i Δ t (4)
[0161] 根據(jù)式(8)得到的角加速度估計方差是加速度計隨機誤差的線性組合，為矩陣Qest 的第一行至第三行和第一列至第三列組成的矩陣，式（12)動力學模型的過程噪聲方差為
[0162] Q" = ( At)2Qest(l:3,l:3) (13)
[0163] 假設角速度狀態(tài)變量初值為
[0164] ?〇/?=[??0 0r0 ωζ0] (14)
[0165] 其中命c，黽。是初始時刻X、Υ、Ζ三個方向的角速度值初值，角速度初始值方差假 設為Ρ〇/〇，為3 X 3矩陣，初值<&_和初始方差Ρο/ο需要在實際應用時給定；
[0166] 角速度狀態(tài)變量的預測值為
[0167] (hklk^ =&k^,k_i + uk^ Π 5)
[0168] 其中，％.__表示k-1時刻的估計值，為Kalman濾波中k時刻的預測值，Uk-i表 示第k-Ι時刻的計算值，根據(jù)Kalman濾波算法，得到預測值化^的方差為：
[0169] Pk/k-i = Pk-i/k-i+Quk-i (16)
[0170] 其中Pk-Vk-Ak-l時刻估計值的方差矩陣，Pk/k-Ak時刻角速度預測值的方差 矩陣，Qcjk-i為k_l時刻過程方差矩陣，由（13)式給出；
[0171] 步驟(3):根據(jù)角速度二次項的估計值和估計方差，建立角速度測量方程以及測量 噪聲的方差矩陣，并將方程離散化；由于步驟（1)中并未得到角速度估計值，只得到了角加 速度和角速度二次項估計值，所以還需要進一步得到角速度估值。
[0172] 將式(7)給出的角速度二次項估計值作為濾波過程中的測量模型，同時考慮加速 度計測量所存在的隨機誤差，所得角速度二次項的測量模型可以表示為：
[0174] 其中Vk是角速度二次項估計值的隨機誤差。根據(jù)式(8)可知，該隨機誤差方差是加 速度計隨機誤差的線性組合，為矩陣Q est的第7行到第12行和第7列到第12列組成的方陣，為
[0175] Rk = Qest(7:12,7:12) (18)
[0176] 同樣根據(jù)式(8)可知，式（12)所示動力學模型的過程噪聲與式（17)所示測量模型 噪聲的協(xié)方差矩陣為矩陣式(8)所示Q est的第1行到3行和第7列到12列組成的矩陣
[0177] Mk = Qest(l:3,7:12)At (19)
[0178] Mk為動力學模型過程噪聲與測量模型噪聲協(xié)方差陣，At為時間步長，為3X6維。
[0179] 測量模型的Jacobian矩陣Hk為：
[0181] 其中％_為式(15)中的角速度預報值，Θ為求偏導數(shù)符號。
[0182] 貝ijKalman濾波增益矩陣的更新方程為：
[0184]濾波方差的修正方程為
[0186]其中Pk/k為k時刻角速度估計值的方差矩陣，角速度變量的修正方程為
[0188] 其中A,A表示k時刻的估計值 是根據(jù)(7)式計算得到的角速度二次項解
向量。
[0189] 根據(jù)結果，式（15)、式（16)、式(21)、式(22)、式(23)即是無陀螺慣性系統(tǒng)角速度約 束估計中Kalman濾波遞推公式，本發(fā)明同時給出了上述5個公式中所有變量的意義及計算 公式，這是無陀螺慣性系統(tǒng)角速度約束估計的關鍵所在。在實際應用中給定濾波初值0_和 初始方差Po/o,即可遞推得到相應的估計值。％?Sk時刻無陀螺慣性系統(tǒng)角速度估計值， Pk/k為k時刻角速度估計方差矩陣。
[0190] 為了驗證本發(fā)明提供的方法，具體算例如下：
[0191]根據(jù)載體建立體坐標系，為笛卡爾直角坐標系。載體三個方向的加速度分別為[1 0.5 2]m/s2,載體三軸都存在角運動，三方向角加速度為0.1X[-1 0.4t-l 2]rad/s2,角速 度初值為[3 -2 4]rad/s，其中t為時間，計算步長為0.01秒。為簡便起見，假設重力加速度 始終被一外力平衡。
[0192] 為驗證本發(fā)明的結果，采用一種十二加速度計無陀螺慣性測量系統(tǒng)作為計算例 子，圖2給出了該系統(tǒng)的構型幾何，但本發(fā)明并不僅限于該構型，可以適用于滿足前提條件 的任意構型。MEMS加速度計測量隨機誤差為零均值高斯白噪聲，根據(jù)目前MEMS加速度計商 業(yè)貨架產(chǎn)品的主流精度水平，選擇輸出噪聲均方差~=lmg，則加速度計測量方差〇 2 = 0.0001m2/s4〇
[0193] 步驟(1):根據(jù)圖2中所示的12加速度計無陀螺慣性系統(tǒng)，構型矩陣為

[0198]則加速度計輸出矢量可表示為
[0200]根據(jù)式(7)角加速度矢量?、線視加速度矢量妒、角速度交叉項和角速度平方項的 線性估計為
[0202]式(7)的估計方差為
[0204] Qest為12X12維矩陣，與時間無關。
[0205] 步驟(2):根據(jù)角加速度估計值，建立角速度動力學方程，根據(jù)角加速度的估計方 差，給出動力學方程噪聲，并將動力學方程離散化，時間步長為0.01秒。
[0206] 令狀態(tài)變量為角速度矢量ω=[ωχ c0y ωζ]τ，在離散情況下，角速度差分方程可 表示為，
[0208] 其中cox,k、coy,k、ωζ>1^別為k時刻的X、Y、Z三個方向的角速度值，co x,k-!、coy,k- ?★:分別為k-i時刻的Χ、γ、ζ三個方向的角速度值，mwy.k^wz.k-Ak-i時刻角加速度 解算的隨機噪聲，定義過程量
[0210]實際計算中u可以根據(jù)式⑴估計的角加速度I乘以時間步長得到，
[0212] 將式（11)代入式(9)，同時寫成矢量形式，角速度ω的離散化狀態(tài)動力學模型可表 示為
[0213] ω k = ω k-i+Uk-i+0.01 wk-i (5)
[0214] 根據(jù)式(8)得到的角加速度估計方差是加速度計隨機誤差的線性組合，為矩陣Qest 的第一行至第三行和第一列至第三列組成的矩陣，式（12)動力學模型的過程噪聲方差為
[0216]角速度狀態(tài)變量濾波初值取為
[0218] 角速度狀態(tài)變量的預測值為
[0219] ioklk_{ =cbt^t+ut^ (15j
[0220] 預測值U勺方差為
[0221] Pk/k-i = Pk-ι/k-l+Qcjk-1 (16)
[0222] 步驟(3):根據(jù)角速度二次項的估計值和估計方差，建立角速度測量方程以及測量 噪聲的方差矩陣，并將方程離散化；
[0223] 將式(7)給出的角速度二次項估計值作為濾波過程中的測量模型，同時考慮加速 度計測量所存在的隨機誤差，所得角速度二次項的測量模型可以表示為
[0225]其中vk是角速度二次項估計值的隨機誤差。根據(jù)式(8)可知，該隨機誤差方差是加 速度計隨機誤差的線性組合，為矩陣Qest的第7行到第12行和第7列到第12列組成的方陣，為
[0226]
[0227] 式（12)所示動力學模型的過程噪聲與式（17)所示測量模型噪聲的協(xié)方差矩陣為 矩陣式(8)所示Qest的第1行到3行和第7列到12列組成的矩陣
[0228]
[0229] Mk為動力學模型過程噪聲與測量模型噪聲協(xié)方差陣，為3 X 6維矩陣。
[0230] 測量模型的Jacobian矩陣Hk見式（20)，Kalman濾波增益矩陣的更新方程見（21)， 濾波方差的修正方程見(22 )，角速度變量的修正方程見(23)式。
[0231 ]濾波初始方差取為
[0233] 根據(jù)式(15)、式（16)、式(21)、式(22)、式(23)給出的遞推公式，式(11)、式（13)、式 (18)、式（19)、式(20)給出的相關變量計算公式，以及濾波初值0_和初始方差p Q/Q，即可完 成無陀螺慣性系統(tǒng)角速度的遞推估計。
[0234] 所得結果列于圖3~6中，由圖3可見X方向角速度估計的誤差不會隨時間的增加而 變大，最大值不超過〇.〇1弧度/秒，有圖4可見，Y方向角速度估計的誤差不會隨時間的增加 而變大，同樣的圖5所示為Z方向角速度估計誤差不會隨時間的增加而變大。圖6中所示為X、 Y、Z三個方向角速度估計值的均方差，可以看出均方差隨時間增加緩慢變小。這說明本發(fā)明 提供的角速度估計方法所得角速度誤差估計不隨時間增加而變大，可以有效提供無陀螺慣 性測量系統(tǒng)角速度估計精度。
[0235] 本發(fā)明所提出無陀螺慣性測量系統(tǒng)約束角速度估計方法具有以下優(yōu)點：（1)估計 方法步驟清晰，簡單明了，合理可行；（2)角速度估計誤差不隨測量時間增加而變大，等價于 陀螺儀的角速度測量功能；（3)該方法可實時遞推計算。因此該方法可以有效提高無陀螺慣 性測量系統(tǒng)角速度估計精度。
[0236] 本領域技術人員將清楚本發(fā)明的范圍不限制于以上討論的示例，有可能對其進行 若干改變和修改，而不脫離所附權利要求書限定的本發(fā)明的范圍。盡管己經(jīng)在附圖和說明 書中詳細圖示和描述了本發(fā)明，但這樣的說明和描述僅是說明或示意性的，而非限制性的。 本發(fā)明并不限于所公開的實施例。
[0237] 通過對附圖，說明書和權利要求書的研究，在實施本發(fā)明時本領域技術人員可以 理解和實現(xiàn)所公開的實施例的變形。在權利要求書中，術語"包括"不排除其他步驟或元素， 而不定冠詞"一個"或"一種"不排除多個。在彼此不同的從屬權利要求中引用的某些措施的 事實不意味著這些措施的組合不能被有利地使用。權利要求書中的任何參考標記不構成對 本發(fā)明的范圍的限制。
【主權項】
1. 一種無陀螺慣性測量系統(tǒng)約束角速度估計方法，其特征在于，包括以下步驟： 步驟（1):對于所含加速度計大于12個且能獲得角速度二次項估計值的無陀螺慣性測 量系統(tǒng)，利用所有所述加速度計的輸出得到線加速度、角加速度、角速度平方項、角速度交 叉項次項的線性估計和估計方差； 所述步驟(1)包括以下步驟： 所述無陀螺慣性測量系統(tǒng)包含N個加速度計，其中N 2 12，所述加速度計矢量Y的輸出表 示為：(5) 其中?為載體待測的三維角加速度矢量，#為待測的載體三維視加速度矢量，ωχ，ωγ， ω ζ為角速度ω的三個分量，Υ為Ν個加速度計組成的測量矢量；，di為第i個加速度計敏感軸方向，< 表示矢量di的 轉(zhuǎn)置，ri為第i個加速度計中心位置矢量，ri = [rxi ryi rzi]T，i = l，2，···Ν，Ν為加速度計個 數(shù)，J為構塑矩陣： ?2ι i_\ j， "/ -i/yx0 -，則為NX6維矩陣，，其中，X3為角速度交叉項矢 量，X4為角速度平方項矢量，X3和X4為角速度二次項，則：(6) 其中X3為角速度交叉項、X4為角速度平方項，根據(jù)式(6)，所述角加速度矢量、視加速度 矢量、角速度交叉項和角速度平方項的線性估計為： (7) 此處所得為關于角加速度矢量士、.視加速度矢量#、角速度交叉項X3和角速度平方項X4 的中間估計量； 所述無陀螺慣性測量系統(tǒng)中的各加速度計誤差已完成標定和補償，加速度計測量誤差 為零均值高斯白噪聲，測量方差為σ2,所述無陀螺系統(tǒng)中的各所述加速度計測量方差都相 同，式(7)的估計方差為： Qest=([J D?]T[J DU])-1D[Y] = ([J D?]t[J ?ω])-ν (8) 其中Qest為12X12維矩陣； 步驟(2):根據(jù)所述角加速度的估計值，建立角速度微分方程，根據(jù)所述角加速度的估 計方差，構建角速度狀態(tài)方程過程噪聲，并將所述狀態(tài)方程離散化； 所述步驟(2)包括以下步驟： 實際待估變量是角速度矢量ω，令Kalman濾波中的狀態(tài)變量為角速度矢量ω = [ωχ ?y ωζ]τ，在離散情況下，角速度差分方程表示為：(9) 其中，At為時間步長，cox,k、coy,k、co z,k分別為k時刻的Χ、Υ、Ζ三個方向的角速度值， ωχ,^、coz,k_^別為k-Ι時刻的Χ、Υ、Ζ三個方向的角速度值，At為濾波時間步長，通 過解算公式(9)，得到所述角速度矢量ω，1,1 {-1，^,1{-1，1,1{- 1為卜1時刻角加速度解算的隨機 噪聲，定義過程量：(10) 實際計算中u可以根據(jù)式(7)估計的角加速度?乘以時間步長得到： L」 （11) 將式（11)代入式（9)，同時寫成矢量形式，角速度ω的離散化狀態(tài)動力學模型可表示 為： ω k= ω k-i+Uk-i+Wk-i A t (1) 根據(jù)式(8)得到的所述角加速度估計方差是加速度計隨機誤差的線性組合，為矩陣Qest 的第一行至第三行以及第一列至第三列組成的矩陣，式（12)動力學模型的過程噪聲方差 為： Θω=( At)2Qest(l：3,l：3) (13) 假設角速度狀態(tài)變量初值為：(14) 其中4。，今.。，是k = 0的初始時刻X、Y、Z三個方向的角速度初始值，所述角速度初始值 (15) 的方差假設為Po/o，為3 X 3矩陣，具體計算時根據(jù)情況給定； 角速度狀態(tài)變量的預測值為： 其中，表示k-Ι時刻的估計值，為Kalman濾波中k時刻的預測值，Uk-i表示第 k-Ι時刻的計算值，根據(jù)Kalman濾波算法，得到預測值的方差為： Pk/k-l - Pk-1/k-1+QcJk-1 ( 16 ) 其中Pk-Vk-^k-l時刻估計值的方差矩陣，pk/k-Ak時刻角速度預測值的方差矩 陣，Qui^Sk-l時刻過程的方差矩陣，由（13)式給出； 步驟(3):根據(jù)角速度二次項的估計值和估計方差，建立角速度測量方程以及測量噪聲 的方差矩陣，并將方程離散化； 所述步驟(3)包括以下步驟： 將式(7)給出的角速度二次項估計值作為濾波過程中的測量模型，同時考慮所述加速 度計測量所存在的隨機誤差，得到角諫度二次煩的測量樽型表示為：Π 7) 其中Vk是角速度二次項估計值的隨機誤差； 所述隨機誤差方差表示為： Rk = Qest(7:12,7:12) (18) 同樣根據(jù)式(8)可知，式（12)所示動力學模型的過程噪聲與式（17)所示測量模型噪聲 的協(xié)方差矩陣為矩陣式(8)所示Qd的第1行到3行和第7列到12列組成的矩陣： Mk = Qest(l:3,7:12) At (19) Mk為動力學模型過程噪聲與測量模型噪聲協(xié)方差陣為3X6維矩陣，At為時間步長，與 式(10)中時間步長相同； 所述測量模型的Jacobian矩陣Hk為：(2Π) 其中為式(15)中的角速度預報值，δ為求偏導數(shù)符號； 貝ijKalman濾波增益矩陣的更新方程為：(2D 濾波方差的修正方程為：(22) 其中，Pk/k為k時刻角速度估計值的方差矩陣，角速度變量的修正方程為： (23) 其中，,表示k時刻的無陀螺慣性系統(tǒng)角速度估計值，只=&是根據(jù)(7)式計算得 到的角速度二次項解向量； 根據(jù)所述結果，式（15)、式（16)、式(21)、式(22)和式(23)即為無陀螺慣性系統(tǒng)角速度 約束估計中Kalman濾波遞推公式，根據(jù)所述Kalman濾波遞推公式和濾波初值4/()和初始方 差Pq/q即可得出所述％ /4_和所述Pk/k。
【文檔編號】G01P3/00GK105866459SQ201610180095
【公開日】2016年8月17日
【申請日】2016年3月25日
【發(fā)明人】楊華波, 李安梁, 張士峰
【申請人】中國人民解放軍國防科學技術大學