r>[0039] 步驟三：陀螺儀112周期性運動后，陀螺儀112輸出的角速度等于ω i = [ω1χ，ω1γ，ω1ζ]加上旋轉調(diào)制機構 12 的角速度 ωχζι= [ω xzxi, coxzyi，ωχζζι]，記為 CO11 = [ω?χ?，tjIyi, ω?ζ?] 〇將此時的陀螺儀112輸出的角速度值ω Ii-方面?zhèn)鬏斨?J誤差補償器2， 另一方面作為初始值傳輸?shù)娇柭鼮V波模塊32。此時加速度計111輸出的值保持不變，仍 然為Ct 1= [a lx，aly，α1ζ]，將其傳輸給速度位置計算模塊33和卡爾曼濾波模塊32。
[0040] 步驟四：由誤差補償器2中的矩陣奇異值分解模塊21計算旋轉調(diào)制機構12的合 力Η，
合力H是一個m*n階復矩陣，其中，b為慣性傳感器11的坐標系 0xbybzb(即傳感器載體坐標系，原點與慣性傳感器11的質(zhì)心重合，Z b垂直于慣性傳感器11 向上，Xb與y b互相垂直并與Z b構成右手直角坐標系），r為旋轉調(diào)制機構12坐標系（如圖 2所示，X軸垂直于y、ζ軸并與y、ζ軸構成右手直角坐標系），f為旋轉調(diào)制機構12繞b系 y軸和ζ軸進行前向和后向旋轉時慣性傳感器11的重力值，Tni是與旋轉方向一致的三階方 向余弦矩陣，f"是慣性傳感器11在r坐標系中的重力，滿足關系式：
式中，m =12, 3,4
[0042] 步驟五：由矩陣奇異值分解模塊21對合力H進行矩陣奇異值分解，通過公式H = USVt，對H進行分解得到U和V兩個酉矩陣。
[0043] 奇異值分解是線性代數(shù)中公知的一種重要的矩陣分解，合力H是一個m*n階復矩 陣，U為m階酉陣，V為η階酉陣，U和V中分別是H的奇異向量，￥ 1是V的轉置，U 1是U的 轉置。S = diag(〇 i，σ 2,……，〇r)，〇i>〇(i = 1，…，r)，〇1為!1的奇異值，r的值為H 的秩。
[0044] 步驟六：由矩陣奇異值分解模塊21經(jīng)公式q' = FM計算安裝誤差矩陣G，安裝誤 差矩陣Cf具體是b坐標系到r坐標系的變換矩陣。
[0045] 步驟七：HDR模塊22先補償陀螺儀112的隨機漂移誤差，再通過均值計算，對陀 螺儀112補償前輸出的角速度ω Η進行零值偏移誤差ε。補償，得角速度ω 2ι= ω Η-ε。； 然后通過啟發(fā)式漂移消減算法對陀螺儀112輸出的角速度ωΗ進行漂移誤差補償，補償 后的陀螺儀112的角速度為CO htw= ω" +ε ^I1，其中，ed為隨機漂移誤差，COhtkiS 經(jīng)過HDR算法補償后的角速度值，ω"為角速度ω2ι經(jīng)過低通濾波器的角速度值，I1 = Ii fAiicSignhi H丨為上一時刻的補償因子，I ;為當前時刻的補償因子，sign為取符號 函數(shù)，i。為常量，A ;為衰減函數(shù)，
為上一時刻的角速度值，Θ w為設定的閾 值，P是設定的衰減因子。
[0047] 步驟八：加速度計111輸出的線加速度值α ι= [ α ix, a iy, α ιζ]傳輸給速度位置 計算模塊33,速度位置計算模塊33計算水下機器人的位置和速度。水下機器人在三個方向 的坐標位置為：
[0049] Sx。代表水下機器人在X方向的初始位置，S y。代表水下機器人在y方向的初始位 置，Szci代表水下機器人在z方向的初始位置。
[0050] 步驟九，誤差補償器2補償后的角速度值傳輸?shù)阶藨B(tài)矩陣計算模塊31，姿態(tài) 矩陣計算模塊31計算陀螺儀112的旋轉矢量。旋轉矢量Φ = [Φχ ΦΖ]，ΦΧΚ表陀螺 儀112在χ軸方向的角增量，Φγ代表陀螺儀112在y軸方向的角增量，Φ ζ代表陀螺儀112 在ζ軸方向的角增量；當前時刻角速度cohd"= [ω hd"x，cohd"y，ωω_]，上一時刻的角速度 ^ hdri I I- ^ hdrix Ij ^ hdriy Ij ^ hdriz 1-1 ?
[0052] 步驟十：通過陀螺儀112的旋轉矢量旋轉矢量Φ = [ Φχ <i)y Φζ]計算機器人在 t時刻的姿態(tài)四元數(shù)q = [q。，q!，q2, q3]:
[0056] 步驟十一：卡爾曼濾波模塊32根據(jù)輸入的量測值Zk = [aiX，aiy，aiZ， ω?Χ，Wiy，WiJ，采用卡爾曼濾波算法對姿態(tài)矩陣進行修正，計算出高精度 四元數(shù)。
[0057] 被估計矢量（四元數(shù)）初值：Χ。/。= [1 ο 0 0] τ，
[0058] 估計均方誤差矩陣初值：P。/。= diag([0. 001 0· 001 0· 001 0· 001]) ;diag 代表對 角矩陣
[0059] 濾波具體過程如下：
[0060] 狀態(tài)一步預測方程：Xk/k f Φ k,k A p
[0061] Xk為k_l時刻的系統(tǒng)估計狀態(tài)量，即X k [q。，qi，q2, q3]。氣k A X k凋X滿 一步轉移矩陣。Xk/k i是利用Xk i計算得到的對Xk的一步預測。
[0062] 狀態(tài)估值方程：Xk= X k/k JKk (Zk_HkXk/k D，
[0063] X# k時刻的系統(tǒng)估計狀態(tài)量，即Xk= [q。，q2, q3]。它是在一步預測Xk/k i的基 礎上，根據(jù)量測值Zk= [a ix, aiy, aiz, ω?χ, coiy，ω?ζ]計算得到的。Hk為量測矩陣，表示量 測值4和估計值X k之間的數(shù)學關系；Vk為量測值高斯白噪聲序列，其均值E[v(k)] =0,方 差E [V (k) V (k)T] = Rk，Rk即測量噪聲方差；為了求得X k的值，必須知道增益矩陣K k即測量 噪聲方差；
[0064] 濾波增益方程：
[0065] ///是量測矩陣Hk的轉置；R k即測量噪聲方差；
[0066] -步預測均方誤差方程
[0067] Qk i為系統(tǒng)噪聲的方差矩陣。Φ k,k J為一步轉移矩陣Φ k,k滿
[0068] 估計均方誤差方程：Pk/k= (Ι-K kHk) Pk/k 1;
[0069] 通過上述五個公式，可以求得當前時刻的系統(tǒng)估計狀態(tài)量Xk= [q。，q。q2, q3]，每 完成一次數(shù)據(jù)融合，計算一次姿態(tài)矩陣。
[0070] 步驟十二：姿態(tài)矩陣計算模塊31通過卡爾曼濾波修正后的四元數(shù)向量Xk = [q。，qi，q2, q3]計算出水下機器人當前時刻的姿態(tài)矩陣<。
[0072] Ct表示地理坐標系η到水下機器人本體坐標系b的轉換關系；
[0073] 步驟十三：姿態(tài)矩陣計算模塊31將求得的姿態(tài)矩陣Ct傳輸?shù)阶藨B(tài)計算模塊34， 用于求取水下機器人的姿態(tài)信息，即航向角、橫滾角和俯仰角。
[0074] 原始姿態(tài)信息分別表示為Φ、Θ和γ :
[0076] 其中r12、r22、r31、r 32、r33為姿態(tài)矩陣g中的矩陣元素。
[0077] 步驟十四：求取當前時刻真實姿態(tài)信息Θ it_ yitJ =當前時刻原始姿 態(tài)信息[^，Θ i，Y1] X安裝誤差矩陣C丨-旋轉調(diào)制機構的雙軸旋轉角ωχζι
[0078] 步驟十五：根據(jù)上述求得的位置，姿態(tài)等信息傳遞給控制模塊35,并根據(jù)實際要 求控制水下機器人的運動。
[0079] 以下提供本發(fā)明的一個應用例：水下機器人慣性導航系統(tǒng)的初始化參數(shù)為：當?shù)?重力加速度g = 9. 8m/s2,采樣時間t = 0. 02s。陀螺儀112的角速度為Φ = [A。死。]， 初值為零；加速度計111輸出三個方向的加速度為ax，ay，az，初值為[0 0 g]。
[0080] 系統(tǒng)噪聲方差Q和測量噪聲方差R為：
[0081] Q = diag([0. 0001 0.0001 0.0001 0.0001])
[0082]