r>[0039] 步骤三：陀螺仪112周期性运动后，陀螺仪112输出的角速度等于ω i = [ω1χ，ω1γ，ω1ζ]加上旋转调制机构 12 的角速度 ωχζι= [ω xzxi, coxzyi，ωχζζι]，记为 CO11 = [ω?χ?，tjIyi, ω?ζ?] 〇将此时的陀螺仪112输出的角速度值ω Ii-方面传输至1J误差补偿器2， 另一方面作为初始值传输到卡尔曼滤波模块32。此时加速度计111输出的值保持不变，仍 然为Ct 1= [a lx，aly，α1ζ]，将其传输给速度位置计算模块33和卡尔曼滤波模块32。
[0040] 步骤四：由误差补偿器2中的矩阵奇异值分解模块21计算旋转调制机构12的合 力Η，
合力H是一个m*n阶复矩阵，其中，b为惯性传感器11的坐标系 0xbybzb(即传感器载体坐标系，原点与惯性传感器11的质心重合，Z b垂直于惯性传感器11 向上，Xb与y b互相垂直并与Z b构成右手直角坐标系），r为旋转调制机构12坐标系（如图 2所示，X轴垂直于y、ζ轴并与y、ζ轴构成右手直角坐标系），f为旋转调制机构12绕b系 y轴和ζ轴进行前向和后向旋转时惯性传感器11的重力值，Tni是与旋转方向一致的三阶方 向余弦矩阵，f"是惯性传感器11在r坐标系中的重力，满足关系式：
式中，m =12, 3,4
[0042] 步骤五：由矩阵奇异值分解模块21对合力H进行矩阵奇异值分解，通过公式H = USVt，对H进行分解得到U和V两个酉矩阵。
[0043] 奇异值分解是线性代数中公知的一种重要的矩阵分解，合力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经公式q' = FM计算安装误差矩阵G，安装误 差矩阵Cf具体是b坐标系到r坐标系的变换矩阵。
[0045] 步骤七：HDR模块22先补偿陀螺仪112的随机漂移误差，再通过均值计算，对陀 螺仪112补偿前输出的角速度ω Η进行零值偏移误差ε。补偿，得角速度ω 2ι= ω Η-ε。； 然后通过启发式漂移消减算法对陀螺仪112输出的角速度ωΗ进行漂移误差补偿，补偿 后的陀螺仪112的角速度为CO htw= ω" +ε ^I1，其中，ed为随机漂移误差，COhtkiS 经过HDR算法补偿后的角速度值，ω"为角速度ω2ι经过低通滤波器的角速度值，I1 = Ii fAiicSignhi H丨为上一时刻的补偿因子，I ;为当前时刻的补偿因子，sign为取符号 函数，i。为常量，A ;为衰减函数，
为上一时刻的角速度值，Θ w为设定的阈 值，P是设定的衰减因子。
[0047] 步骤八：加速度计111输出的线加速度值α ι= [ α ix, a iy, α ιζ]传输给速度位置 计算模块33,速度位置计算模块33计算水下机器人的位置和速度。水下机器人在三个方向 的坐标位置为：
[0049] Sx。代表水下机器人在X方向的初始位置，S y。代表水下机器人在y方向的初始位 置，Szci代表水下机器人在z方向的初始位置。
[0050] 步骤九，误差补偿器2补偿后的角速度值传输到姿态矩阵计算模块31，姿态 矩阵计算模块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时刻的姿态四元数q = [q。，q!，q2, q3]:
[0056] 步骤十一：卡尔曼滤波模块32根据输入的量测值Zk = [aiX，aiy，aiZ， ω?Χ，Wiy，WiJ，采用卡尔曼滤波算法对姿态矩阵进行修正，计算出高精度 四元数。
[0057] 被估计矢量（四元数）初值：Χ。/。= [1 ο 0 0] τ，
[0058] 估计均方误差矩阵初值：P。/。= diag([0. 001 0· 001 0· 001 0· 001]) ;diag 代表对 角矩阵
[0059] 滤波具体过程如下：
[0060] 状态一步预测方程：Xk/k f Φ k,k A p
[0061] Xk为k_l时刻的系统估计状态量，即X k [q。，qi，q2, q3]。气k A X k凋X满 一步转移矩阵。Xk/k i是利用Xk i计算得到的对Xk的一步预测。
[0062] 状态估值方程：Xk= X k/k JKk (Zk_HkXk/k D，
[0063] X# k时刻的系统估计状态量，即Xk= [q。，q2, q3]。它是在一步预测Xk/k i的基 础上，根据量测值Zk= [a ix, aiy, aiz, ω?χ, coiy，ω?ζ]计算得到的。Hk为量测矩阵，表示量 测值4和估计值X k之间的数学关系；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为系统噪声的方差矩阵。Φ k,k J为一步转移矩阵Φ k,k满
[0068] 估计均方误差方程：Pk/k= (Ι-K kHk) Pk/k 1;
[0069] 通过上述五个公式，可以求得当前时刻的系统估计状态量Xk= [q。，q。q2, q3]，每 完成一次数据融合，计算一次姿态矩阵。
[0070] 步骤十二：姿态矩阵计算模块31通过卡尔曼滤波修正后的四元数向量Xk = [q。，qi，q2, q3]计算出水下机器人当前时刻的姿态矩阵<。
[0072] Ct表示地理坐标系η到水下机器人本体坐标系b的转换关系；
[0073] 步骤十三：姿态矩阵计算模块31将求得的姿态矩阵Ct传输到姿态计算模块34， 用于求取水下机器人的姿态信息，即航向角、横滚角和俯仰角。
[0074] 原始姿态信息分别表示为Φ、Θ和γ :
[0076] 其中r12、r22、r31、r 32、r33为姿态矩阵g中的矩阵元素。
[0077] 步骤十四：求取当前时刻真实姿态信息Θ it_ yitJ =当前时刻原始姿 态信息[^，Θ i，Y1] X安装误差矩阵C丨-旋转调制机构的双轴旋转角ωχζι
[0078] 步骤十五：根据上述求得的位置，姿态等信息传递给控制模块35,并根据实际要 求控制水下机器人的运动。
[0079] 以下提供本发明的一个应用例：水下机器人惯性导航系统的初始化参数为：当地 重力加速度g = 9. 8m/s2,采样时间t = 0. 02s。陀螺仪112的角速度为Φ = [A。死。]， 初值为零；加速度计111输出三个方向的加速度为ax，ay，az，初值为[0 0 g]。
[0080] 系统噪声方差Q和测量噪声方差R为：
[0081] Q = diag([0. 0001 0.0001 0.0001 0.0001])
[0082]