CN104406592A - 一种用于水下滑翔器的导航系统及姿态角校正和回溯解耦方法 - Google Patents

Abstract

本发明公布了一种用于水下滑翔器的导航系统及姿态角校正和回溯解耦方法，属于水下滑翔器导航定位技术领域。该组合导航系统包括数字信号处理(DSP)模块、微机电系统(MEMS)惯性测量单元(IMU)等。由于俯仰或横滚运动使得姿态角(航向角、俯仰角、横滚角)之间的交叉耦合更加明显，姿态角之间的交叉耦合导致姿态角输出不准甚至错误，进而使随后的速度、位置等其它导航信息解算发生错误，基于回溯思想提出姿态角回溯解耦方法来消除姿态角之间的交叉耦合。本系统设计可以满足水下滑翔器导航系统低功耗、小体积、长航时的需求，姿态角回溯解耦方法有效地解决了姿态角之间的交叉耦合，大大提高了姿态角精度，达到了水下滑翔器长航时、低功耗、高精度导航定位的目的。

Description

一种用于水下滑翔器的导航系统及姿态角校正和回溯解耦方法

技术领域

本发明涉及一种用于水下滑翔器的导航系统及姿态角回溯解耦方法，属于水下滑翔器导航定位技术领域。

背景技术

水下滑翔器是一种使用内在制动器通过控制自身浮力和姿态角来随水流滑翔的自主水下载体。水下滑翔器具有结构简单、功耗较低、能在水下长时间作业等特点，所以被用作海洋勘测、海洋数据搜集等作业。近年，水下滑翔器已经成为临海和开放海洋观测的重要部分。在水下执行的任务中，精确的位姿信息是至关重要的，因此水下导航是一个难点也是重点问题。

水下滑翔器的体积小，成本低，所以不能安装太多高精度水下导航传感器。对于陆地车载来说，含有差分校正的全球定位系统(DGPS)能提供高精度的位姿信息，而且成本较低。但全球定位系统(GPS)信号在水下不能使用。惯性导航系统(INS)可以在短时间内提供精确的位姿信息，且能在没有GPS的情况下实现自主导航，基于微机电系统(MEMS)的INS的优势在使得MEMS IMU在低成本惯性技术领域起到了重要作用。然而，因为陀螺仪和加速度计自身的固有偏差使得INS的误差随时间不断积累，积累的误差将会导致姿态角和位置的巨大偏差，所以需要其他传感来补偿INS的误差。

传统的方式采用电子罗盘，这个在抑制INS的位姿漂移能起到一定作用，但仅仅是电子罗盘，所达到的效果还是局限的。用航位推算方式(DR)能在保证功耗、成本等不增加的情况下尽可能达到所需位姿精度要求。所以采用INS/DR组合方式完成水下滑翔器导航。

对于水下滑翔器，虽然在一定深度下，水流比较平稳均匀，滑翔器随水流滑翔，但俯仰和横滚运动是不可避免的，。对于惯性测量单元，由于安装轴和相应的参考轴之间的误差会造成姿态角(航向角、俯仰角和横滚角)之间的交叉耦合，非零的俯仰角和横滚角使得姿态角的交叉耦合更加明显，从而造成姿态角及其它导航信息解算不准甚至错误。俯仰和横滚运动在实际中是普遍存在的，滑翔器在一定深度的水中是以较平稳的速度随水滑翔，但是其特殊的构造，必须依靠水的浮力和调节自身的俯仰角来形成锯齿波状运动，通过这种运动使得滑翔器向前滑翔。惯性测量单元作为搭载在水下滑翔器的主要导航元件，其导航信息解算的精度对滑翔器的导航与定位起着至关重要的作用。因此，低成本、低功耗、长航时的水下滑翔器导航系统及高精度的位姿估计方法是目前国内外研究的重点与难点。

发明内容

发明目的：为了克服现有技术中存在的不足，本发明提供一种新的用于水下滑翔器的导航系统及姿态角回溯解耦方法。

为实现上述目的，本发明采用的技术方案为：

一种用于水下滑翔器的导航系统姿态角校正方法，包括以下步骤：步骤A，根据陀螺仪输出的角速度、加速度计输出的加速度、磁力计输出的磁场强度进行导航信息解算,经导航解算后得到姿态角、速度、位置信息的导航数据；

步骤B，根据导航解算出的导航数据，判断因俯仰或横滚运动，使得姿态角交叉耦合明显增大而造成的姿态角解算错误的节点，用姿态角交叉耦合造成解算错误的前一步解算数据重新计算，得出新的姿态角、速度、位置信息；

步骤C，步骤B中回溯解耦后的姿态角、速度及位置信息经前置滤波器去噪后，一方面利用误差方程得到状态量，一方面和步骤A中经导航解算后得到姿态角、速度、位置信息分别作差得到观测量；将该状态量和观测量进入基于卡尔曼的自适应滤波算法，进行姿态角、速度、位置误差的最优估计，所得误差估计值校正惯导所得的导航信息，最终得到校正后准确的姿态角、速度和位置。

所述步骤B中的回溯解耦方法包括以下步骤：

步骤B1，根据导航解算得出的导航数据，判断因俯仰或横滚运动，使得姿态角交叉耦合明显增大而造成的姿态角解算错误的节点；

步骤B2，用因俯仰或横滚运动，使得姿态角交叉耦合明显增大而造成解算错误的前一步解算数据，计算载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影；

步骤B3，用载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影通过解算得到新的姿态角、速度、位置信息。

所述步骤B1中判断因俯仰或横滚运动，使得姿态角交叉耦合明显增大而造成的姿态角解算错误的节点的方法，包括以下步骤：

步骤B11，因俯仰或横滚运动，引起的姿态角交叉耦合而造成三个失准角φ_x,φ_y,φ_z错误，其中φ_x,φ_y,φ_z分别为载体坐标系的三轴偏离导航坐标系对应的三轴的偏离角，也即失准角；

步骤B12，将三个失准角φ_x,φ_y,φ_z代入姿态角校正方程其中是错误解算节点前一步正确的导航坐标系到载体坐标系的姿态角矩阵， $C^T=\left[\begin{array}{ccc}1& {\mathrm{\φ}}_z& -{\mathrm{\φ}}_y\\ -{\mathrm{\φ}}_z& 1& {\mathrm{\φ}}_x\\ {\mathrm{\φ}}_y& -{\mathrm{\φ}}_x& 1\end{array}\right]$ 为姿态校正矩阵；

步骤B13，根据姿态角校正方程可得出四元数方程：

$\left|q_0\right|=0.5*\sqrt{1+C_{\mathrm{nco}}^b\left(\mathrm{0,0}\right)+C_{\mathrm{nco}}^b\left(\mathrm{1,1}\right)+C_{\mathrm{nco}}^b\left(\mathrm{2,2}\right)}$

$\left|q_1\right|=0.5*\sqrt{1+C_{\mathrm{nco}}^b\left(\mathrm{0,0}\right)-C_{\mathrm{nco}}^b\left(\mathrm{1,1}\right)-C_{\mathrm{nco}}^b\left(\mathrm{2,2}\right)}---\left(11\right)$

$\left|q_2\right|=0.5*\sqrt{1-C_{\mathrm{nco}}^b\left(\mathrm{0,0}\right)+C_{\mathrm{nco}}^b\left(\mathrm{1,1}\right)-C_{\mathrm{nco}}^b\left(\mathrm{2,2}\right)}$

$\left|q_3\right|=0.5*\sqrt{1-C_{\mathrm{nco}}^b\left(\mathrm{0,0}\right)-C_{\mathrm{nco}}^b\left(\mathrm{1,1}\right)+C_{\mathrm{nco}}^b\left(\mathrm{2,2}\right)};$

步骤B14，判断式(11)中的根号下的计算结果是否是负数，若是负数，则四元数q₀、q₁、q₂、q₃错误，后续的导航解算会依次错误；从而得出上述式(11)中根号下计算结果出现负数是因姿态角交叉耦合造成导航解算错误的节点。

所述步骤B2中载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影为：

${\mathrm{\ω}}_{\mathrm{nb}}^b={\mathrm{\ω}}_{\mathrm{ib}}^b-C_n^b({\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)$

其中，为载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影；为陀螺仪输出的角速率；为地球坐标系相对于惯性坐标系的自转角速率在导航坐标系中的投影；为导航坐标系相对于地球坐标系的角速率在导航坐标系上的投影；。

所述步骤B3中用载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影通过解算得出新的姿态角、速度、位置信息的方法，包括以下步骤：

步骤B31，令 $\mathrm{\Δ\Θ}={\∫}_{t_k}^{t_{k+1}}{M}^{*}\left({\mathrm{\ω}}_{\mathrm{nb}}^b\right)\mathrm{dt}---\left(13\right);$

将代入 $\mathrm{\Δ\Θ}={\∫}_{t_k}^{t_{k+1}}{M}^{*}\left({\mathrm{\ω}}_{\mathrm{nb}}^b\right)\mathrm{dt}$

其中， $M^{*}\left({\mathrm{\ω}}_{\mathrm{nb}}^b\right)=\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bx}}& -{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{by}}& -{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bz}}\\ {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bx}}& 0& {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bz}}& -{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{by}}\\ {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{by}}& -{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bz}}& 0& {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bx}}\\ {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bz}}& {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{by}}& -{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bx}}& 0\end{array}\right],$ 为在三个轴x、y、z轴上的分量；

步骤B32，四元数是由四个元素构成，将其定义为：Q(q₀,q₁,q₂,q₃)＝q₀+q₁i+q₂j+q₃k，q₀、q₁、q₂、q₃是实数，i、j、k是相互正交的单位向量。采用毕卡逼近方法求解四元数微分方程：

$Q\left(t_{k+1}\right)=e^{\frac{1}{2}\mathrm{\Δ\Θ}}Q\left(t_k\right)---\left(14\right)$

其中，Q(t_k+1)、Q(t_k)分别代表t_k+1、t_k时刻的四元数向量。将式(14)进行泰勒级数展开得：

$Q\left(t_{k+1}\right)=e^{\frac{1}{2}\mathrm{\Δ\Θ}}Q\left(t_k\right)=[I+\frac{\frac{1}{2}\mathrm{\Δ\Θ}}{1!}+\frac{{\left(\frac{1}{2}\mathrm{\Δ\Θ}\right)}^2}{2!}+...]Q\left(t_k\right)---\left(15\right)$

将式(15)写成三角形式：

$Q\left(t_{k+1}\right)=[I\cos \frac{\mathrm{\Δ\θ}}{2}+\mathrm{\Δ\Θ}\frac{\sin \frac{\mathrm{\Δ\θ}}{2}}{\mathrm{\Δ\θ}}]Q\left(t_k\right)---\left(16\right)$

其中，Δθ_x、Δθ_y、Δθ_z分别为x、y、z陀螺在[t_k t_k+1]采样时间间隔内的角增量；

归一化后的四元数：

${\hat{q}}_i=\frac{q_i}{\sqrt{q_0^2+q_1^2+q_2^2+q_3^2}},i=\mathrm{0,1,2,3}---\left(17\right)$

步骤B33，用新的四元数更新姿态矩阵：

${\hat{C}}_n^b=\left[\begin{array}{ccc}{\hat{q}}_0^2+{\hat{q}}_1^2-{\hat{q}}_2^2-{\hat{q}}_3^2& 2({\hat{q}}_1{\hat{q}}_2+{\hat{q}}_0{\hat{q}}_3)& 2({\hat{q}}_1{\hat{q}}_3-{\hat{q}}_0{\hat{q}}_2)\\ 2({\hat{q}}_1{\hat{q}}_2-{\hat{q}}_0{\hat{q}}_3)& {\hat{q}}_0^2-{\hat{q}}_1^2+{\hat{q}}_2^2-{\hat{q}}_3^2& 2({\hat{q}}_2{\hat{q}}_3+{\hat{q}}_0{\hat{q}}_1)\\ 2({\hat{q}}_1{\hat{q}}_3+{\hat{q}}_0{\hat{q}}_2)& 2({\hat{q}}_2{\hat{q}}_3-{\hat{q}}_0{\hat{q}}_1)& {\hat{q}}_0^2-{\hat{q}}_1^2-{\hat{q}}_2^2+{\hat{q}}_3^2\end{array}\right]---\left(18\right)$

更新三个姿态角：

航向角

俯仰角 $\widehat{\mathrm{\θ}}=\arcsin \left[{\hat{C}}_n^b\left(\mathrm{1,2}\right)\right]---\left(19\right)$

横滚角 $\widehat{\mathrm{\γ}}=-\arctan \left[\frac{{\hat{C}}_n^b\left(\mathrm{0,2}\right)}{{\hat{C}}_n^b\left(\mathrm{2,2}\right)}\right]$

用准确的姿态变换矩阵代入公式中，算得比力fⁿ，从而计算出新的速度及位置。

一种基于用于水下滑翔器的导航系统，包括DSP处理单元、MEMS IMU导航元件；所述DSP处理单元包括存储模块、回溯解耦模块、前置滤波去噪模块、误差模块、观测量模块以及基于卡尔曼的自适应滤波算法模块；所述MEMS IMU导航元件用于采集原始的角速度、加速度及磁场数据，所有原始数据进入DSP处理单元进行解算，得到姿态角、速度及位置信息，并将得到的姿态角、速度及位置信息传送给DSP处理单元；

所述DSP处理单元用于接收MEMS IMU导航元件传送过来的姿态角、速度及位置信息，并将该姿态角、速度及位置信息传送给回溯解耦模块和观测量模块；

所述回溯解耦模块用于根据回溯解耦方法，将正常解算过程中解算的导航信息存储起来，当解算过程错误判断后，姿态角发生奇异突变，调用上一次正确解算的导航参数进行重新解算，得出新的姿态角、速度及位置信息并对其进行更新，同时将更新后的姿态角、速度及位置信息发送给前置滤波去噪模块；若姿态角未发生奇异突变，则将姿态角、速度及位置信息发送给前置滤波去噪模块；

所述滤波去噪模块用于接收回溯解耦模块传送过来姿态角、速度及位置信息；同时将姿态角、速度和位置信息进行滤波去噪，并将滤波去噪后的姿态角、速度和位置信息分别传送给误差模块和观测量模块；

所述误差模块用于根据滤波去噪模块传送的姿态角、速度和位置信息利用误差方程求导航信息误差得到状态量，并将该状态量传送给基于卡尔曼的自适应滤波算法模块；

所述观测量模块用于根据DSP处理单元推送的姿态角、速度及位置信息与滤波去噪模块推送的姿态角、速度及位置信息作差作为滤波器的观测量，并将该观测量传送给基于卡尔曼的自适应滤波算法模块；

所述基于卡尔曼的自适应滤波算法模块用于根据状态量和观测量进行姿态角、速度、位置误差的最优估计，再用所得误差的估计值校正惯导所得的姿态角、速度及位置，最终得到校正后准确的姿态角、速度和位置信息。

一种用于水下滑翔器的导航系统及姿态角回溯解耦方法，包括以下步骤：步骤一，根据陀螺仪输出的角速度、加速度计输出的加速度、磁力计输出的磁场强度进行导航信息解算,经导航解算后得到姿态角、速度、位置信息的导航数据；

步骤二，根据导航解算得出的导航数据，判断因俯仰或横滚运动，使得姿态角交叉耦合明显增大而造成的姿态角解算错误的节点；

步骤三，用因俯仰或横滚运动，使得姿态角交叉耦合明显增大而造成解算错误的前一步解算数据，计算载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影；

步骤四，用载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影通过解算得到新的姿态角、速度、位置信息。

步骤二中判断因为俯仰或横滚运动使得姿态角交叉耦合明显增大而导致姿态角解算错误的节点的方法，包括以下步骤：

步骤二a，因俯仰或横滚运动，使得姿态角交叉耦合明显增大而造成三个失准角φ_x,φ_y,φ_z错误，其中φ_x,φ_y,φ_z分别为载体坐标系的三轴偏离导航坐标系对应的三轴的偏离角,也即是失准角；

步骤二b，将三个失准角φ_x,φ_y,φ_z代入姿态角校正方程其中是错误解算节点前一步正确的导航坐标系到载体坐标系的姿态角矩阵，

$C^T=\left[\begin{array}{ccc}1& {\mathrm{\φ}}_z& -{\mathrm{\φ}}_y\\ -{\mathrm{\φ}}_z& 1& {\mathrm{\φ}}_x\\ {\mathrm{\φ}}_y& -{\mathrm{\φ}}_x& 1\end{array}\right]$ 为姿态校正矩阵；

步骤二c，根据姿态角校正方程可得出四元数方程：

$\left|q_0\right|=0.5*\sqrt{1+C_{\mathrm{nco}}^b\left(\mathrm{0,0}\right)+C_{\mathrm{nco}}^b\left(\mathrm{1,1}\right)+C_{\mathrm{nco}}^b\left(\mathrm{2,2}\right)}$

$\left|q_1\right|=0.5*\sqrt{1+C_{\mathrm{nco}}^b\left(\mathrm{0,0}\right)-C_{\mathrm{nco}}^b\left(\mathrm{1,1}\right)-C_{\mathrm{nco}}^b\left(\mathrm{2,2}\right)}---\left(11\right)$

$\left|q_2\right|=0.5*\sqrt{1-C_{\mathrm{nco}}^b\left(\mathrm{0,0}\right)+C_{\mathrm{nco}}^b\left(\mathrm{1,1}\right)-C_{\mathrm{nco}}^b\left(\mathrm{2,2}\right)}$

$\left|q_3\right|=0.5*\sqrt{1-C_{\mathrm{nco}}^b\left(\mathrm{0,0}\right)-C_{\mathrm{nco}}^b\left(\mathrm{1,1}\right)+C_{\mathrm{nco}}^b\left(\mathrm{2,2}\right)};$

步骤二d，判断式(11)中的根号下的计算结果是否是负数，若是负数，则四元数q₀、q₁、q₂、q₃错误，后续的导航解算会依次错误，则上述式(11)中根号下计算结果出现负数是因姿态角交叉耦合造成导航解算错误的节点。

所述步骤三中载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影为：

${\mathrm{\ω}}_{\mathrm{nb}}^b={\mathrm{\ω}}_{\mathrm{ib}}^b-C_n^b({\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)$

其中，为载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影；为陀螺仪输出的角速率；为地球坐标系相对于惯性坐标系的自转角速率在导航坐标系中的投影；为导航坐标系相对于地球坐标系的角速率在导航坐标系上的投影；

所述步骤四中通过载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影导航得出新的姿态、速度、位置信息的方法，包括以下步骤：

步骤四a，将代入 $\mathrm{\Δ\Θ}={\∫}_{t_k}^{t_{k+1}}{M}^{*}\left({\mathrm{\ω}}_{\mathrm{nb}}^b\right)\mathrm{dt}---\left(13\right);$

其中， $M^{*}\left({\mathrm{\ω}}_{\mathrm{nb}}^b\right)=\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bx}}& -{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{by}}& -{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bz}}\\ {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bx}}& 0& {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bz}}& -{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{by}}\\ {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{by}}& -{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bz}}& 0& {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bx}}\\ {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bz}}& {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{by}}& -{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bx}}& 0\end{array}\right],$ 为在三个轴x、y、z轴上的分量；

步骤四b，四元数是有四个元素构成的，将其定义为：Q(q₀,q₁,q₂,q₃)＝q₀+q₁i+q₂j+q₃k，q₀、q₁、q₂、q₃是实数，i、j、k是相互正交的单位向量，采用毕卡逼近方法求解四元数微分方程：

$Q\left(t_{k+1}\right)=e^{\frac{1}{2}\mathrm{\Δ\Θ}}Q\left(t_k\right)---\left(14\right)$

其中，Q(t_k+1)、Q(t_k)分别代表t_k+1、t_k时刻的四元数向量。将式(14)进行泰勒级数展开得：

$Q\left(t_{k+1}\right)=e^{\frac{1}{2}\mathrm{\Δ\Θ}}Q\left(t_k\right)=[I+\frac{\frac{1}{2}\mathrm{\Δ\Θ}}{1!}+\frac{{\left(\frac{1}{2}\mathrm{\Δ\Θ}\right)}^2}{2!}+...]Q\left(t_k\right)---\left(15\right)$

将式(15)写成三角形式：

$Q\left(t_{k+1}\right)=[I\cos \frac{\mathrm{\Δ\θ}}{2}+\mathrm{\Δ\Θ}\frac{\sin \frac{\mathrm{\Δ\θ}}{2}}{\mathrm{\Δ\θ}}]Q\left(t_k\right)---\left(16\right)$

其中，Δθ_x、Δθ_y、Δθ_z分别为x、y、z陀螺在[t_k t_k+1]采样时间间隔内的角增量；

则Q(q₀,q₁,q₂,q₃)＝q₀+q₁i+q₂j+q₃k，故归一化后的四元数：

${\hat{q}}_i=\frac{q_i}{\sqrt{q_0^2+q_1^2+q_2^2+q_3^2}},i=\mathrm{0,1,2,3}---\left(17\right)$

步骤四c，用新的四元数更新姿态矩阵：

${\hat{C}}_n^b=\left[\begin{array}{ccc}{\hat{q}}_0^2+{\hat{q}}_1^2-{\hat{q}}_2^2-{\hat{q}}_3^2& 2({\hat{q}}_1{\hat{q}}_2+{\hat{q}}_0{\hat{q}}_3)& 2({\hat{q}}_1{\hat{q}}_3-{\hat{q}}_0{\hat{q}}_2)\\ 2({\hat{q}}_1{\hat{q}}_2-{\hat{q}}_0{\hat{q}}_3)& {\hat{q}}_0^2-{\hat{q}}_1^2+{\hat{q}}_2^2-{\hat{q}}_3^2& 2({\hat{q}}_2{\hat{q}}_3+{\hat{q}}_0{\hat{q}}_1)\\ 2({\hat{q}}_1{\hat{q}}_3+{\hat{q}}_0{\hat{q}}_2)& 2({\hat{q}}_2{\hat{q}}_3-{\hat{q}}_0{\hat{q}}_1)& {\hat{q}}_0^2-{\hat{q}}_1^2-{\hat{q}}_2^2+{\hat{q}}_3^2\end{array}\right]---\left(18\right)$

更新三个姿态角：

航向角

俯仰角 $\widehat{\mathrm{\θ}}=\arcsin \left[{\hat{C}}_n^b\left(\mathrm{1,2}\right)\right]---\left(19\right)$

横滚角 $\widehat{\mathrm{\γ}}=-\arctan \left[\frac{{\hat{C}}_n^b\left(\mathrm{0,2}\right)}{{\hat{C}}_n^b\left(\mathrm{2,2}\right)}\right];$

用准确的姿态变换矩阵代入公式中，算得比力fⁿ，从而计算出新的速度及位置。

本发明提供的一种用于水下滑翔器的导航系统及姿态角校正和回溯解耦方法，相比现有技术，具有以下有益效果：

(1)设计了基于MEMS IMU的水下导航系统，结构简单、体积较小、功耗较低，且能高精度、长航时测姿定位。

(2)提出了姿态角回溯解耦方法，智能判断姿态角因为交叉耦合造成的错误输出，实时回溯重新解算，再次更新导航信息得到准确的姿态角速度及位置信息。

综上所述，本发明可以满足水下滑翔器导航系统低功耗、小体积、长航时的需求，姿态角回溯解耦方法有效地解决了姿态角之间的交叉耦合，大大提高了姿态角精度，达到了水下滑翔器长航时、低功耗、高精度导航定位的目的。

附图说明

图1为用于水下滑翔器的水下导航系统及姿态角回溯解耦方法流程图；

图2为水下滑翔器模型；

图3为水下滑翔器导航系统整体框图；

具体实施方式

下面结合附图对本发明作更进一步的说明。

一种用于水下滑翔器的导航系统姿态角校正方法，如图1所示，包括以下步骤：步骤A，根据陀螺仪输出的角速度、加速度计输出的加速度、磁力计输出的磁场强度进行导航信息解算,经导航解算后得到姿态角、速度、位置信息的导航数据；

步骤B，根据导航解算出的导航数据，判断因俯仰或横滚运动，引起的姿态角交叉耦合而造成的姿态角解算错误的节点，用姿态角交叉耦合造成解算错误的前一步解算数据重新计算，得出新的姿态角、速度、位置信息；

步骤C，步骤B中回溯解耦后的姿态角、速度及位置信息经前置滤波器去噪后，一方面利用误差方程得到状态量，一方面和步骤A中经导航解算后得到姿态角、速度、位置信息分别作差得到观测量；将该状态量和观测量进入基于卡尔曼的自适应滤波算法，进行姿态角、速度、位置误差的最优估计，所得误差估计值校正惯导所得的导航信息，最终得到校正后准确的姿态角、速度和位置。

所述步骤B中的回溯解耦方法包括以下步骤：

步骤B1，根据导航解算得出的导航数据，判断因俯仰或横滚运动，使得姿态角交叉耦合明显增大而造成的姿态角解算错误的节点；

步骤B2，用因俯仰或横滚运动，使得姿态角交叉耦合明显增大而造成解算错误的前一步解算数据，计算载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影；

步骤B3，用载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影通过解算得到新的姿态角、速度、位置信息。

所述步骤B1中判断因俯仰或横滚运动，使得姿态角交叉耦合明显增大而造成的姿态角解算错误的节点的方法，包括以下步骤：

步骤B11，因俯仰或横滚运动，使得姿态角交叉耦合明显增大而造成三个失准角φ_x,φ_y,φ_z错误，其中φ_x,φ_y,φ_z分别为载体坐标系的三轴偏离导航坐标系对应的三轴的偏离角，也即失准角；

步骤B12，将三个失准角φ_x,φ_y,φ_z代入姿态角校正方程其中是错误解算节点前一步正确的导航坐标系到载体坐标系的姿态角矩阵， $C^T=\left[\begin{array}{ccc}1& {\mathrm{\φ}}_z& -{\mathrm{\φ}}_y\\ -{\mathrm{\φ}}_z& 1& {\mathrm{\φ}}_x\\ {\mathrm{\φ}}_y& -{\mathrm{\φ}}_x& 1\end{array}\right]$ 为姿态校正矩阵；

步骤B13，根据姿态角校正方程可得出四元数方程：

$\left|q_0\right|=0.5*\sqrt{1+C_{\mathrm{nco}}^b\left(\mathrm{0,0}\right)+C_{\mathrm{nco}}^b\left(\mathrm{1,1}\right)+C_{\mathrm{nco}}^b\left(\mathrm{2,2}\right)}$

$\left|q_1\right|=0.5*\sqrt{1+C_{\mathrm{nco}}^b\left(\mathrm{0,0}\right)-C_{\mathrm{nco}}^b\left(\mathrm{1,1}\right)-C_{\mathrm{nco}}^b\left(\mathrm{2,2}\right)}---\left(11\right)$

$\left|q_2\right|=0.5*\sqrt{1-C_{\mathrm{nco}}^b\left(\mathrm{0,0}\right)+C_{\mathrm{nco}}^b\left(\mathrm{1,1}\right)-C_{\mathrm{nco}}^b\left(\mathrm{2,2}\right)}$

$\left|q_3\right|=0.5*\sqrt{1-C_{\mathrm{nco}}^b\left(\mathrm{0,0}\right)-C_{\mathrm{nco}}^b\left(\mathrm{1,1}\right)+C_{\mathrm{nco}}^b\left(\mathrm{2,2}\right)};$

步骤B14，判断式(11)中的根号下的计算结果是否是负数，若是负数，则四元数q₀、q₁、q₂、q₃错误，后续的导航解算会依次错误；从而得出上述式(11)中根号下计算结果出现负数是因姿态角交叉耦合造成导航解算错误的节点。

所述步骤B2中载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影为：

${\mathrm{\ω}}_{\mathrm{nb}}^b={\mathrm{\ω}}_{\mathrm{ib}}^b-C_n^b({\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)$

其中，为载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影；为陀螺仪输出的角速率；为地球坐标系相对于惯性坐标系的自转角速率在导航坐标系中的投影；为导航坐标系相对于地球坐标系的角速率在导航坐标系上的投影；

所述步骤B3中用载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影通过解算得出新的姿态角、速度、位置信息的方法，包括以下步骤：

步骤B31，令 $\mathrm{\Δ\Θ}={\∫}_{t_k}^{t_{k+1}}{M}^{*}\left({\mathrm{\ω}}_{\mathrm{nb}}^b\right)\mathrm{dt}---\left(13\right);$

将代入 $\mathrm{\Δ\Θ}={\∫}_{t_k}^{t_{k+1}}{M}^{*}\left({\mathrm{\ω}}_{\mathrm{nb}}^b\right)\mathrm{dt}$

其中， $M^{*}\left({\mathrm{\ω}}_{\mathrm{nb}}^b\right)=\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bx}}& -{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{by}}& -{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bz}}\\ {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bx}}& 0& {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bz}}& -{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{by}}\\ {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{by}}& -{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bz}}& 0& {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bx}}\\ {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bz}}& {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{by}}& -{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bx}}& 0\end{array}\right],$ 为在三个轴x、y、z轴上的分量；

步骤B32，四元数是有四个元素构成的，将其定义为：Q(q₀,q₁,q₂,q₃)＝q₀+q₁i+q₂j+q₃k，q₀、q₁、q₂、q₃是实数，i、j、k是相互正交的单位向量，采用毕卡逼近方法求解四元数微分方程：

$Q\left(t_{k+1}\right)=e^{\frac{1}{2}\mathrm{\Δ\Θ}}Q\left(t_k\right)---\left(14\right)$

其中，Q(t_k+1)、Q(t_k)分别代表t_k+1、t_k时刻的四元数向量。将式(14)进行泰勒级数展开得：

$Q\left(t_{k+1}\right)=e^{\frac{1}{2}\mathrm{\Δ\Θ}}Q\left(t_k\right)=[I+\frac{\frac{1}{2}\mathrm{\Δ\Θ}}{1!}+\frac{{\left(\frac{1}{2}\mathrm{\Δ\Θ}\right)}^2}{2!}+...]Q\left(t_k\right)---\left(15\right)$

将式(15)写成三角形式：

$Q\left(t_{k+1}\right)=[I\cos \frac{\mathrm{\Δ\θ}}{2}+\mathrm{\Δ\Θ}\frac{\sin \frac{\mathrm{\Δ\θ}}{2}}{\mathrm{\Δ\θ}}]Q\left(t_k\right)---\left(16\right)$

其中，Δθ_x、Δθ_y、Δθ_z分别为陀螺的x、y、z轴在[t_k t_k+1]采样时间间隔内的角增量；

归一化后的四元数：

${\hat{q}}_i=\frac{q_i}{\sqrt{q_0^2+q_1^2+q_2^2+q_3^2}},i=\mathrm{0,1,2,3}---\left(17\right)$

步骤B33，用新的四元数更新姿态矩阵：

${\hat{C}}_n^b=\left[\begin{array}{ccc}{\hat{q}}_0^2+{\hat{q}}_1^2-{\hat{q}}_2^2-{\hat{q}}_3^2& 2({\hat{q}}_1{\hat{q}}_2+{\hat{q}}_0{\hat{q}}_3)& 2({\hat{q}}_1{\hat{q}}_3-{\hat{q}}_0{\hat{q}}_2)\\ 2({\hat{q}}_1{\hat{q}}_2-{\hat{q}}_0{\hat{q}}_3)& {\hat{q}}_0^2-{\hat{q}}_1^2+{\hat{q}}_2^2-{\hat{q}}_3^2& 2({\hat{q}}_2{\hat{q}}_3+{\hat{q}}_0{\hat{q}}_1)\\ 2({\hat{q}}_1{\hat{q}}_3+{\hat{q}}_0{\hat{q}}_2)& 2({\hat{q}}_2{\hat{q}}_3-{\hat{q}}_0{\hat{q}}_1)& {\hat{q}}_0^2-{\hat{q}}_1^2-{\hat{q}}_2^2+{\hat{q}}_3^2\end{array}\right]---\left(18\right)$

更新三个姿态角：

航向角

俯仰角 $\widehat{\mathrm{\θ}}=\arcsin \left[{\hat{C}}_n^b\left(\mathrm{1,2}\right)\right]---\left(19\right)$

横滚角 $\widehat{\mathrm{\γ}}=-\arctan \left[\frac{{\hat{C}}_n^b\left(\mathrm{0,2}\right)}{{\hat{C}}_n^b\left(\mathrm{2,2}\right)}\right]$

用准确的姿态变换矩阵代入公式中，算得比力fⁿ，从而计算出新的速度及位置。

一种基于用于水下滑翔器的导航系统，如图3所示，包括DSP处理单元、MEMS IMU导航元件；所述DSP处理单元包括存储模块、回溯解耦模块、前置滤波去噪模块、误差模块、观测量模块以及基于卡尔曼的自适应滤波算法模块；所述MEMS IMU导航元件用于采集原始的角速度、加速度及磁场数据，所有原始数据进入DSP处理单元进行解算，得到姿态角、速度及位置信息，并将得到的姿态角、速度及位置信息传送给DSP处理单元；

所述DSP处理单元用于接收MEMS IMU导航元件传送过来的姿态角、速度及位置信息，并将该姿态角、速度及位置信息传送给回溯解耦模块和观测量模块；

所述回溯解耦模块用于根据回溯解耦方法，将正常解算过程中解算的导航信息存储起来，当解算过程错误判断后，姿态角发生奇异突变，调用上一次正确解算的导航参数进行重新解算，得出新的姿态角、速度及位置信息并对其进行更新，同时将更新后的姿态角、速度及位置信息发送给前置滤波去噪模块；若姿态角未发生奇异突变，则将姿态角、速度及位置信息发送给前置滤波去噪模块；

所述滤波去噪模块用于接收回溯解耦模块传送过来姿态角、速度及位置信息；同时将姿态角、速度和位置信息进行滤波去噪，并将滤波去噪后的姿态角、速度和位置信息分别传送给误差模块和观测量模块；

所述误差模块用于根据滤波去噪模块传送的姿态角、速度和位置信息利用误差方程求导航信息误差得到状态量，并将该状态量传送给基于卡尔曼的自适应滤波算法模块；

所述观测量模块用于根据DSP处理单元推送的姿态角、速度及位置信息与滤波去噪模块推送的姿态角、速度及位置信息作差作为滤波器的观测量，并将该观测量传送给基于卡尔曼的自适应滤波算法模块；

所述基于卡尔曼的自适应滤波算法模块用于根据状态量和观测量进行姿态角、速度、位置误差的最优估计，再用所得误差的估计值校正惯导所得的姿态角、速度及位置，最终得到校正后准确的姿态角、速度和位置信息。

一种用于水下滑翔器的导航系统及姿态角回溯解耦方法，如图1、2所示，包括以下步骤：步骤一，根据陀螺仪输出的角速度、加速度计输出的加速度、磁力计输出的磁场强度进行导航信息解算,经导航解算后得到姿态角、速度、位置信息的导航数据；

步骤二，根据导航解算得出的导航数据，判断因俯仰或横滚运动，使得姿态角交叉耦合明显增大而造成的姿态角解算错误的节点；

步骤三，用因俯仰或横滚运动，使得姿态角交叉耦合明显增大而造成解算错误的前一步解算数据，计算载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影；

步骤四，用载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影通过解算得到新的姿态角、速度、位置信息。

步骤二中判断因为俯仰或横滚运动使得姿态角交叉耦合明显增大导致姿态角解算错误的节点的方法，包括以下步骤：

步骤二a，因俯仰或横滚运动，使得姿态角交叉耦合明显增大而造成三个失准角φ_x,φ_y,φ_z错误，其中φ_x,φ_y,φ_z分别为载体坐标系的三轴偏离导航坐标系对应的三轴的偏离角，也即失准角；

步骤二b，将三个失准角φ_x,φ_y,φ_z代入姿态角校正方程其中是错误解算节点前一步正确的导航坐标系到载体坐标系的姿态角矩阵， $C^T=\left[\begin{array}{ccc}1& {\mathrm{\φ}}_z& -{\mathrm{\φ}}_y\\ -{\mathrm{\φ}}_z& 1& {\mathrm{\φ}}_x\\ {\mathrm{\φ}}_y& -{\mathrm{\φ}}_x& 1\end{array}\right]$ 为姿态校正矩阵；

步骤二c，根据姿态角校正方程可得出四元数方程：

$\left|q_0\right|=0.5*\sqrt{1+C_{\mathrm{nco}}^b\left(\mathrm{0,0}\right)+C_{\mathrm{nco}}^b\left(\mathrm{1,1}\right)+C_{\mathrm{nco}}^b\left(\mathrm{2,2}\right)}$

$\left|q_1\right|=0.5*\sqrt{1+C_{\mathrm{nco}}^b\left(\mathrm{0,0}\right)-C_{\mathrm{nco}}^b\left(\mathrm{1,1}\right)-C_{\mathrm{nco}}^b\left(\mathrm{2,2}\right)}---\left(11\right)$

$\left|q_2\right|=0.5*\sqrt{1-C_{\mathrm{nco}}^b\left(\mathrm{0,0}\right)+C_{\mathrm{nco}}^b\left(\mathrm{1,1}\right)-C_{\mathrm{nco}}^b\left(\mathrm{2,2}\right)}$

$\left|q_3\right|=0.5*\sqrt{1-C_{\mathrm{nco}}^b\left(\mathrm{0,0}\right)-C_{\mathrm{nco}}^b\left(\mathrm{1,1}\right)+C_{\mathrm{nco}}^b\left(\mathrm{2,2}\right)};$

步骤二d，判断式(11)中的根号下的计算结果是否是负数，若是负数，则四元数q₀、q₁、q₂、q₃错误，后续的导航解算会依次错误，则上述式(11)中根号下计算结果出现负数是因姿态角交叉耦合造成导航解算错误的节点。

所述步骤三中载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影为：

${\mathrm{\ω}}_{\mathrm{nb}}^b={\mathrm{\ω}}_{\mathrm{ib}}^b-C_n^b({\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)$

其中，为载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影；为陀螺仪输出的角速率；为地球坐标系相对于惯性坐标系的自转角速率在导航坐标系中的投影；为导航坐标系相对于地球坐标系的角速率在导航坐标系上的投影；所述步骤四中通过载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影导航得出新的姿态、速度、位置信息的方法，包括以下步骤：

步骤四a，将代入 $\mathrm{\Δ\Θ}={\∫}_{t_k}^{t_{k+1}}{M}^{*}\left({\mathrm{\ω}}_{\mathrm{nb}}^b\right)\mathrm{dt}---\left(13\right);$

其中， $M^{*}\left({\mathrm{\ω}}_{\mathrm{nb}}^b\right)=\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bx}}& -{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{by}}& -{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bz}}\\ {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bx}}& 0& {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bz}}& -{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{by}}\\ {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{by}}& -{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bz}}& 0& {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bx}}\\ {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bz}}& {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{by}}& -{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bx}}& 0\end{array}\right],$ 为在三个轴x、y、z轴上的分量；

步骤四b，四元数是由四个元素构成的，将其定义为：Q(q₀,q₁,q₂,q₃)＝q₀+q₁i+q₂j+q₃k，q₀、q₁、q₂、q₃是实数，i、j、k是相互正交的单位向量，采用毕卡逼近方法求解四元数微分方程：

$Q\left(t_{k+1}\right)=e^{\frac{1}{2}\mathrm{\Δ\Θ}}Q\left(t_k\right)---\left(14\right)$

其中，Q(t_k+1)、Q(t_k)分别代表t_k+1、t_k时刻的四元数向量。将式(14)进行泰勒级数展开得：

$Q\left(t_{k+1}\right)=e^{\frac{1}{2}\mathrm{\Δ\Θ}}Q\left(t_k\right)=[I+\frac{\frac{1}{2}\mathrm{\Δ\Θ}}{1!}+\frac{{\left(\frac{1}{2}\mathrm{\Δ\Θ}\right)}^2}{2!}+...]Q\left(t_k\right)---\left(15\right)$

将式(15)写成三角形式：

$Q\left(t_{k+1}\right)=[I\cos \frac{\mathrm{\Δ\θ}}{2}+\mathrm{\Δ\Θ}\frac{\sin \frac{\mathrm{\Δ\θ}}{2}}{\mathrm{\Δ\θ}}]Q\left(t_k\right)---\left(16\right)$

其中，Δθ_x、Δθ_y、Δθ_z分别为陀螺的x、y、z轴在[t_k t_k+1]采样时间间隔内的角增量；

归一化后的四元数：

${\hat{q}}_i=\frac{q_i}{\sqrt{q_0^2+q_1^2+q_2^2+q_3^2}},i=\mathrm{0,1,2,3}---\left(17\right)$

步骤四c，用新的四元数更新姿态矩阵：

${\hat{C}}_n^b=\left[\begin{array}{ccc}{\hat{q}}_0^2+{\hat{q}}_1^2-{\hat{q}}_2^2-{\hat{q}}_3^2& 2({\hat{q}}_1{\hat{q}}_2+{\hat{q}}_0{\hat{q}}_3)& 2({\hat{q}}_1{\hat{q}}_3-{\hat{q}}_0{\hat{q}}_2)\\ 2({\hat{q}}_1{\hat{q}}_2-{\hat{q}}_0{\hat{q}}_3)& {\hat{q}}_0^2-{\hat{q}}_1^2+{\hat{q}}_2^2-{\hat{q}}_3^2& 2({\hat{q}}_2{\hat{q}}_3+{\hat{q}}_0{\hat{q}}_1)\\ 2({\hat{q}}_1{\hat{q}}_3+{\hat{q}}_0{\hat{q}}_2)& 2({\hat{q}}_2{\hat{q}}_3-{\hat{q}}_0{\hat{q}}_1)& {\hat{q}}_0^2-{\hat{q}}_1^2-{\hat{q}}_2^2+{\hat{q}}_3^2\end{array}\right]---\left(18\right)$

更新三个姿态角：

航向角

俯仰角 $\widehat{\mathrm{\θ}}=\arcsin \left[{\hat{C}}_n^b\left(\mathrm{1,2}\right)\right]---\left(19\right)$

横滚角 $\widehat{\mathrm{\γ}}=-\arctan \left[\frac{{\hat{C}}_n^b\left(\mathrm{0,2}\right)}{{\hat{C}}_n^b\left(\mathrm{2,2}\right)}\right];$

用准确的姿态变换矩阵代入公式中，算得比力fⁿ，从而计算出新的速度及位置。

为了便于理解本发明，现将本发明的原理进行如下说明：

一种用于水下滑翔器的导航系统：

如图3所述，(1)选用低功耗元器件，包括数字信号处理(DSP)单元、微机电系统(MEMS)惯性测量单元(IMU)模块等，使系统设计简单、集成度高、体积小、功耗低。选用新型的基于MEMS的IMU模块，其中含有三轴陀螺仪、三轴加速度计及三轴磁力计。该IMU模块结合了iMEMS与混合信号处理技术，提供较准的数字惯性检测。SPI接口和简单的输出寄存器结构实现了方便的数据访问与配置控制。IMU内部的惯性传感器在各个轴上执行精密对准，并对失调和灵敏度进行较准。MCU等核心CPU能动态补偿对内部传感器的主要影响。系统还具有以下特点：条件监控、数字滤波及采样速率、辅助数字输入/输出、自动检测、系统内自动偏置较准及电源管理等。

所述的DSP单元选用TI公司生产的C5500系列芯片，MEMS IMU模块选用AD公司生产的ADIS16400系列产品。DSP芯片和MEMS IMU模块都是低功耗产品，在满足导航精度的前提下将功耗降到最低，体积减小到最小。

由IMU输出的原始数据(陀螺仪的角速度、加速度计的加速度、磁力计的磁场强度)进入导航解算模块进行位姿解算。本发明中优化了导航解算模块，使该模块准确高效。设计思路是：采用一个计时器定时触发测量，所有测量过程靠中断推进，用查询方式不断检查测量是否完成，完成即进入解算。这样，解算和测量同时进行，不会在等待测量上浪费大量时间，通过计时器触发测量，最大程度地保证积分间隔的准确性。

(2)姿态角的交叉耦合使得姿态的测量误差大大增加，甚至造成错误，建立水下滑翔器模型，分析姿态角交叉耦合原因。姿态角回溯解耦方法包括姿态角错误判断、重新解算、导航信息再更新等步骤。判断阶段准确判断出姿态角解算错误的节点，在正常导航解算过程中存储重要参数信息，然后利用存储的重要参数回溯解耦得到正确的解算数据，最后重新更新得到准确的姿态角速度位置信息。

水下滑翔器姿态角耦合建模如下：

(a)水下滑翔器在一定深度下随水滑翔，速度稳定且均匀。

(b)如图2所示，水下滑翔器的线速度和角速度分别定义为v＝[v_x v_y v_z]^T和w＝[ω_x ω_y ω_z]^T。姿态角定义为η＝[ψ θ γ]^T，姿态角包括航向角、俯仰角和横滚角。一般地，假定滑翔器动力学是不受滑翔器在惯性坐标系下三个方向的绝对位置的影响。

(c)动力学方程为：

$\left[\begin{array}{ccc}M& D^T& 0_3\\ D& J& 0_3\\ 0_3& 0_3& I_3\end{array}\right]\left[\begin{array}{c}\stackrel{\·}{v}\\ \stackrel{\·}{w}\\ \stackrel{\·}{\mathrm{\η}}\end{array}\right]=\left[\begin{array}{c}(\mathrm{Mv}+D^{T}w)\×w+F\\ (\mathrm{Dv}+\mathrm{Jw})\×w+(\mathrm{Mv}+D^{T}w)\×v+M\\ R\left(\mathrm{\η}\right)w\end{array}\right]---\left(1\right)$

其中， $R\left(\mathrm{\η}\right)=\left[\begin{array}{ccc}1& \sin \mathrm{\γ}\tan \mathrm{\θ}& \cos \mathrm{\γ}\tan \mathrm{\θ}\\ 0& \cos \mathrm{\γ}& -\sin \mathrm{\γ}\\ 0& \frac{\sin \mathrm{\γ}}{\cos \mathrm{\θ}}& \frac{\cos \mathrm{\γ}}{\cos \mathrm{\θ}}\end{array}\right]$

0₃代表零矩阵，I₃代表单位阵，定义

$M=\left[\begin{array}{ccc}{m}_x& 0& 0\\ 0& m_y& 0\\ 0& 0& m_z\end{array}\right]$ 和 $J=\left[\begin{array}{ccc}{J}_x& 0& 0\\ 0& J_y& 0\\ 0& 0& J_z\end{array}\right]---\left(2\right)$

其中质量项m_x，增加的质量项m_y＝m_z；惯性项J_y，增加的惯性项J_x＝J_z。

(d)交叉耦合矩阵为：

$D=\left[\begin{array}{ccc}0& {\mathrm{mz}}_{\mathrm{cg}}& {\mathrm{mx}}_{\mathrm{cg}}\\ {-\mathrm{mz}}_{\mathrm{cg}}& 0& {-\mathrm{my}}_{\mathrm{cg}}-Z_{\stackrel{.}{q}}\\ {-\mathrm{mx}}_{\mathrm{cg}}& {\mathrm{my}}_{\mathrm{cg}}+Z_{\stackrel{.}{q}}& 0\end{array}\right]---\left(3\right)$

重力中心在[x_cg y_cg z_cg]^T，船体质量为m。假定滑翔器的重心和浮力中心没有横向偏移，即x_cg＝0。若滑翔器对于xz轴不对称，则交叉增加的质量项非零。

(e)滑翔器的速度、冲角α和侧滑角β满足下式：

α＝tan^-1(v_z/v_y)_,β＝sin^-1(v_x/v)，(4)

其中 $v=\left|v\right|=\sqrt{v_x^2+v_y^2+v_z^2}$

在交叉耦合模型中，滑翔器滑翔速度随水流并且比较平稳，，可假定v＝v₀，v₀是滑翔器在某一深度水中的初始滑翔速度。

ω_y(t)和γ(t)假定为随时间变化的参数，将方程(1)简化，将线速度v_x、v_z代入α、β，可得方程为：

其中χ_θ:＝T₁[α ω_x θ]^T、T₁、T₂分别为系数。

在线性模型中忽略式(5)中的β是一种有必要的对称方法，运动方程可变为：

$E\stackrel{.}{\mathrm{\χ}}=f(\mathrm{\χ},\mathrm{\δ},t)---\left(6\right)$

其中

(f)对于非零的横滚角，即γ(t)≠0和ω_y(t)≠0，在点χ₀＝0和δ₀＝0附近线性化方程(6)得：

$E\stackrel{.}{\mathrm{\χ}}=A_{\mathrm{\γ}}\left(t\right)\mathrm{\χ}+\mathrm{B\δ}---\left(7\right)$

其中

$A_{\mathrm{\θ}}\left(t\right)=\left[\begin{array}{ccc}{a}_{22}& a_{23}& 0\\ a_{32}& a_{33}& a_{34}\\ 0& \cos \mathrm{\γ}\left(t\right)& 0\end{array}\right],$ B是系数矩阵。

系统矩阵A_γ(t)中包含非零的非对角耦合项：

$A_{12}\left(t\right)={-A}_{21}\left(t\right)=\left[\begin{array}{ccc}{\mathrm{\ω}}_y\left(t\right)v_0{m}_x& -{\mathrm{\ω}}_y\left(t\right)({\mathrm{my}}_{\mathrm{cg}}+Z_{\stackrel{.}{q}})& 0\\ {-\mathrm{\ω}}_y\left(t\right)v_0({\mathrm{my}}_{\mathrm{cg}}+Z_{\stackrel{.}{q}})& {\mathrm{\ω}}_y\left(t\right)(J_z-J_y)& 0\\ 0& -\sin \mathrm{\γ}\left(t\right)& 0\end{array}\right]---\left(8\right)$

(g)对于航向角俯仰角θ，式(7)的第三列和第六列显示了运动耦合项，对于俯仰角θ，则有

其中为T₃系数。

对于非零的横滚角γ(t)，航向角速率ω_z和俯仰角速率ω_x不再是航向角和俯仰角θ的导数。从方程(7)和(8)能清楚地看出，非零的横滚角使得姿态角之间的交叉耦合更加明显。

(h)同理，非零的俯仰角也使得姿态角之间的交叉耦合更加明显。在实际水下滑翔器中，俯仰角和横滚角的非零情况是经常发生的，所以交叉耦合是普遍存在的。

一种用于水下滑翔器的水下导航姿态角回溯解耦方法，包括下列步骤：

(1)判断因为交叉耦合造成的姿态角解算错误的节点。在水下滑翔器姿态角耦合建模过程中，可以看出，由于姿态角之间的交叉耦合会造成三个失准角φ_x,φ_y,φ_z不正确，姿态校正矩阵 $C^T=\left[\begin{array}{ccc}1& {\mathrm{\φ}}_z& -{\mathrm{\φ}}_y\\ -{\mathrm{\φ}}_z& 1& {\mathrm{\φ}}_x\\ {\mathrm{\φ}}_y& -{\mathrm{\φ}}_x& 1\end{array}\right]$ 会发生错误，导致姿态角校正方程

$C_{\mathrm{nco}}^b=C^T{C}_n^b---\left(10\right)$

错误，其中是错误解算前一步正确的导航坐标系到载体坐标系的姿态角矩阵。

可得四元数方程为：

$\left|q_0\right|=0.5*\sqrt{1+C_{\mathrm{nco}}^b\left(\mathrm{0,0}\right)+C_{\mathrm{nco}}^b\left(\mathrm{1,1}\right)+C_{\mathrm{nco}}^b\left(\mathrm{2,2}\right)}$

$\left|q_1\right|=0.5*\sqrt{1+C_{\mathrm{nco}}^b\left(\mathrm{0,0}\right)-C_{\mathrm{nco}}^b\left(\mathrm{1,1}\right)-C_{\mathrm{nco}}^b\left(\mathrm{2,2}\right)}---\left(11\right)$

$\left|q_2\right|=0.5*\sqrt{1-C_{\mathrm{nco}}^b\left(\mathrm{0,0}\right)+C_{\mathrm{nco}}^b\left(\mathrm{1,1}\right)-C_{\mathrm{nco}}^b\left(\mathrm{2,2}\right)}$

$\left|q_3\right|=0.5*\sqrt{1-C_{\mathrm{nco}}^b\left(\mathrm{0,0}\right)-C_{\mathrm{nco}}^b\left(\mathrm{1,1}\right)+C_{\mathrm{nco}}^b\left(\mathrm{2,2}\right)};$

因上述的发生错误，会导致根号下的变成负数，造成四元数q₀错误。同理q₁、q₂、q₃也会发生类似的错误。本次滤波过程中姿态角解算公式：

航向角

俯仰角 $\mathrm{\θ}=\arcsin \left[C_{\mathrm{nco}}^b\left(\mathrm{1,2}\right)\right]---\left(12\right)$

横滚角 $\mathrm{\γ}=-\arctan \left[\frac{C_{\mathrm{nco}}^b\left(\mathrm{0,2}\right)}{C_{\mathrm{nco}}^b\left(\mathrm{2,2}\right)}\right]$

因错误，导致式(12)的三个姿态角本次解算结果是错误的，同时比力方程的计算以及速度位置的计算都会发生错误。错误的四元数q₀、q₁、q₂、q₃又会引起下次整个更新过程的错误。所以一旦式(11)中根号下的计算发生突变或者变负，说明本次解算过程已经出现问题，要停止解算。

(2)回溯解耦将正常解算过程中的解算数据存储起来，当解算过程发生错误时，调用上一次正确的解算参数进行重新解算。具体过程如下：

用因姿态角交叉耦合造成解算错误的前一步解算数据计算为载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影；为陀螺仪输出的角速率；为地球坐标系相对于惯性坐标系的自转角速率在导航坐标系中的投影；为导航坐标系相对于地球坐标系的角速率在导航坐标系上的投影；

将上述计算的代入 $\mathrm{\Δ\Θ}={\∫}_{t_k}^{t_{k+1}}{M}^{*}\left({\mathrm{\ω}}_{\mathrm{nb}}^b\right)\mathrm{dt}---\left(13\right)$

其中 $M^{*}\left({\mathrm{\ω}}_{\mathrm{nb}}^b\right)=\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bx}}& -{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{by}}& -{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bz}}\\ {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bx}}& 0& {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bz}}& -{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{by}}\\ {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{by}}& -{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bz}}& 0& {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bx}}\\ {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bz}}& {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{by}}& -{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bx}}& 0\end{array}\right]$

为在三个轴x、y、z轴上的分量。

四元数是由四个元素构成的，将其定义为：Q(q₀,q₁,q₂,q₃)＝q₀+q₁i+q₂j+q₃k，q₀、q₁、q₂、q₃是实数，i、j、k是相互正交的单位向量，采用毕卡逼近方法求解四元数微分方程： $Q\left(t_{k+1}\right)=e^{\frac{1}{2}\mathrm{\Δ\Θ}}Q\left(t_k\right)---\left(14\right)$

其中，Q(t_k+1)、Q(t_k)分别代表t_k+1、t_k时刻的四元数向量。将式(14)进行泰勒级数展开得：

$Q\left(t_{k+1}\right)=e^{\frac{1}{2}\mathrm{\Δ\Θ}}Q\left(t_k\right)=[I+\frac{\frac{1}{2}\mathrm{\Δ\Θ}}{1!}+\frac{{\left(\frac{1}{2}\mathrm{\Δ\Θ}\right)}^2}{2!}+...]Q\left(t_k\right)---\left(15\right)$

将式(15)写成三角形式：

$Q\left(t_{k+1}\right)=[I\cos \frac{\mathrm{\Δ\θ}}{2}+\mathrm{\Δ\Θ}\frac{\sin \frac{\mathrm{\Δ\θ}}{2}}{\mathrm{\Δ\θ}}]Q\left(t_k\right)---\left(16\right)$

其中，Δθ_x、Δθ_y、Δθ_z分别为陀螺的x、y、z轴在[t_k t_k+1]采样时间间隔内的角增量。

归一化后的四元数：

${\hat{q}}_i=\frac{q_i}{\sqrt{q_0^2+q_1^2+q_2^2+q_3^2}},i=\mathrm{0,1,2,3}---\left(17\right)$

(3)重新更新姿态、速度及位置等信息。

用新的四元数更新姿态矩阵：

${\hat{C}}_n^b=\left[\begin{array}{ccc}{\hat{q}}_0^2+{\hat{q}}_1^2-{\hat{q}}_2^2-{\hat{q}}_3^2& 2({\hat{q}}_1{\hat{q}}_2+{\hat{q}}_0{\hat{q}}_3)& 2({\hat{q}}_1{\hat{q}}_3-{\hat{q}}_0{\hat{q}}_2)\\ 2({\hat{q}}_1{\hat{q}}_2-{\hat{q}}_0{\hat{q}}_3)& {\hat{q}}_0^2-{\hat{q}}_1^2+{\hat{q}}_2^2-{\hat{q}}_3^2& 2({\hat{q}}_2{\hat{q}}_3+{\hat{q}}_0{\hat{q}}_1)\\ 2({\hat{q}}_1{\hat{q}}_3+{\hat{q}}_0{\hat{q}}_2)& 2({\hat{q}}_2{\hat{q}}_3-{\hat{q}}_0{\hat{q}}_1)& {\hat{q}}_0^2-{\hat{q}}_1^2-{\hat{q}}_2^2+{\hat{q}}_3^2\end{array}\right]---\left(18\right)$

更新三个姿态角：

航向角

俯仰角 $\widehat{\mathrm{\θ}}=\arcsin \left[{\hat{C}}_n^b\left(\mathrm{1,2}\right)\right]---\left(19\right)$

横滚角 $\widehat{\mathrm{\γ}}=-\arctan \left[\frac{{\hat{C}}_n^b\left(\mathrm{0,2}\right)}{{\hat{C}}_n^b\left(\mathrm{2,2}\right)}\right]$

用准确的姿态变换矩阵代入公式中，算得比力fⁿ，从而计算出速度及位置。

由上述可知：导航解算之后输出姿态、速度、位置信息，判断是否因姿态角之间的交叉耦合造成姿态角发生奇异，若没有发生奇异，则正常输出进入前置滤波去噪模块；若发生奇异，则进入姿态角回溯解耦阶段。用因耦合造成错误的上一步正确的数据进行重新解算。求载体坐标系相对于导航坐标系的转动角速率在载体坐标系上的投影用毕卡逼近法求解算是微分方程，经泰勒公式及三角形式转换后，进行四元数归一化，得到准确的四元数值。用准确的四元数更新三个姿态角，用准确的姿态变换矩阵代入公式中，算得比力，得到速度及位置。得到的位姿信息进入前置滤波去噪，经滤波去噪后，得到惯性导航器件算得的姿态、速度和位置，这些导航信息作为三方面的输入：(a)进入误差模块利用误差方程求状态量，作为基于卡尔曼的自适应滤波算法的输入；(b)当作滤波算法前的校正量；(c)和航位推算(DR)的估计值做差作为滤波器的观测量。状态量X和观测量Z同时进入基于卡尔曼的自适应滤波算法，进行姿态角、速度、位置的最优估计，再用所得误差状态的估计值去校正惯导所得的位姿数据，最终得到校正后的准确的姿态角、速度和位置信息。

以上所述仅是本发明的优选实施方式，应当指出：对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (10)

1.一种用于水下滑翔器的导航系统姿态角校正方法，其特征在于，包括以下步骤：

步骤A，根据陀螺仪输出的角速度、加速度计输出的加速度、磁力计输出的磁场强度进行导航信息解算,经导航解算后得到姿态角、速度、位置信息的导航数据；

步骤B，根据导航解算出的导航数据，判断因俯仰或横滚运动，使得姿态角交叉耦合明显增大而造成的姿态角解算错误的节点，用姿态角交叉耦合造成解算错误的前一步解算数据重新计算，得出新的姿态角、速度、位置信息；

步骤C，步骤B中回溯解耦后的姿态角、速度及位置信息经前置滤波器去噪后，一方面利用误差方程得到状态量，一方面和步骤A中经导航解算后得到姿态角、速度、位置信息分别作差得到观测量；将该状态量和观测量进入基于卡尔曼的自适应滤波算法，进行姿态角、速度、位置误差的最优估计，所得误差估计值校正惯导所得的导航信息，最终得到校正后准确的姿态角、速度和位置。

2.根据权利要求1所述的一种用于水下滑翔器的导航系统姿态角校正方法，其特征在于：所述步骤B中的回溯解耦方法包括以下步骤：

步骤B1，根据导航解算得出的导航数据，判断因俯仰或横滚运动，使得姿态角交叉耦合明显增大而造成的姿态角解算错误的节点；

步骤B2，用因俯仰或横滚运动，使得姿态角交叉耦合明显增大而造成解算错误的前一步解算数据，计算载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影；

步骤B3，用载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影通过解算得到新的姿态角、速度、位置信息。

3.根据权利要求2所述的一种用于水下滑翔器的导航系统姿态角校正方法，其特征在于：步骤B1中判断因俯仰或横滚运动，使得姿态角交叉耦合明显增大而造成的姿态角解算错误的节点的方法，包括以下步骤：

步骤B11，因俯仰或横滚运动，使得姿态角交叉耦合而造成三个失准角φ_x,φ_y,φ_z错误，其中φ_x,φ_y,φ_z分别为载体坐标系的三轴偏离导航坐标系对应的三轴的偏离角，也即失准角；

步骤B12，将三个失准角φ_x,φ_y,φ_z代入姿态角校正方程其中是错误解算节点前一步正确的导航坐标系到载体坐标系的姿态角矩阵，

$C^T=\left[\begin{array}{ccc}1& {\mathrm{\φ}}_z& -{\mathrm{\φ}}_y\\ -{\mathrm{\φ}}_z& 1& {\mathrm{\φ}}_x\\ {\mathrm{\φ}}_y& {-\mathrm{\φ}}_x& 1\end{array}\right]$ 为姿态校正矩阵；

步骤B13，根据姿态角校正方程可得出四元数方程：

$\left|q_0\right|=0.5*\sqrt{1+C_{\mathrm{nco}}^b\left(\mathrm{0,0}\right)+C_{\mathrm{nco}}^b\left(\mathrm{1,1}\right)+C_{\mathrm{nco}}^b\left(\mathrm{2,2}\right)}$

$\left|q_1\right|=0.5*\sqrt{1+C_{\mathrm{nco}}^b\left(\mathrm{0,0}\right)-C_{\mathrm{nco}}^b\left(\mathrm{1,1}\right)-C_{\mathrm{nco}}^b\left(\mathrm{2,2}\right)}---\left(11\right)$

$\left|q_2\right|=0.5*\sqrt{1-C_{\mathrm{nco}}^b\left(\mathrm{0,0}\right)+C_{\mathrm{nco}}^b\left(\mathrm{1,1}\right)-C_{\mathrm{nco}}^b\left(\mathrm{2,2}\right)}$

$\left|q_3\right|=0.5*\sqrt{1-C_{\mathrm{nco}}^b\left(\mathrm{0,0}\right)-C_{\mathrm{nco}}^b\left(\mathrm{1,1}\right)+C_{\mathrm{nco}}^b\left(\mathrm{2,2}\right)};$

步骤B14，判断式(11)中的根号下的计算结果是否是负数，若是负数，则四元数q₀、q₁、q₂、q₃错误，后续的导航解算会依次错误；从而得出上述式(11)中根号下计算结果出现负数是因姿态角交叉耦合造成导航解算错误的节点。

4.根据权利要求3所述的一种用于水下滑翔器的导航系统姿态角校正方法，其特征在于：所述步骤B2中载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影为：

${\mathrm{\ω}}_{\mathrm{nb}}^b={\mathrm{\ω}}_{\mathrm{ib}}^b-C_n^b({\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)$

其中，为载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影；为陀螺仪输出的角速率；为地球坐标系相对于惯性坐标系的自转角速率在导航坐标系中的投影；为导航坐标系相对于地球坐标系的角速率在导航坐标系上的投影。

5.根据权利要求4所述的一种用于水下滑翔器的导航系统姿态角校正方法，其特征在于：所述步骤B3中用载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影通过解算得出新的姿态角、速度、位置信息的方法，包括以下步骤：

步骤B31，令 $\mathrm{\Δ\Θ}={\∫}_{t_k}^{t_{k+1}}{M}^{*}\left({\mathrm{\ω}}_{\mathrm{nb}}^b\right)\mathrm{dt}---\left(13\right);$

将代入 $\mathrm{\Δ\Θ}={\∫}_{t_k}^{t_{k+1}}{M}^{*}\left({\mathrm{\ω}}_{\mathrm{nb}}^b\right)\mathrm{dt}$

其中， $M^{*}\left({\mathrm{\ω}}_{\mathrm{nb}}^b\right)=\left[\begin{array}{cccc}0& {-\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bx}}& {-\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{by}}& {-\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bz}}\\ {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bx}}& 0& {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bz}}& -{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{by}}\\ {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{by}}& -{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bz}}& 0& {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bx}}\\ {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bz}}& {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{by}}& {-\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bx}}& 0\end{array}\right],$ 为在三个轴x、y、z轴上的分量；

步骤B32，四元数是由四个元素构成，将其定义为：Q(q₀,q₁,q₂,q₃)＝q₀+q₁i+q₂j+q₃k，q₀、q₁、q₂、q₃是实数，i、j、k是相互正交的单位向量，采用毕卡逼近方法求解四元数微分方程：

$Q\left(t_{k+1}\right)=e^{\frac{1}{2}\mathrm{\Δ\Θ}}Q\left(t_k\right)---\left(14\right)$

其中，Q(t_k+1)、Q(t_k)分别代表t_k+1、t_k时刻的四元数向量，将式(14)进行泰勒级数展开得：

$Q\left(t_{k+1}\right)=e^{\frac{1}{2}\mathrm{\Δ\Θ}}Q\left(t_k\right)=[I+\frac{\frac{1}{2}\mathrm{\Δ\Θ}}{1!}+\frac{{\left(\frac{1}{2}\mathrm{\Δ\Θ}\right)}^2}{2!}+...]Q\left(t_k\right)---\left(15\right)$

将式(15)写成三角形式：

$Q\left(t_{k+1}\right)=[I\cos \frac{\mathrm{\Δ\θ}}{2}+\mathrm{\Δ\Θ}\frac{\sin \frac{\mathrm{\Δ\θ}}{2}}{\mathrm{\Δ\θ}}]Q\left(t_k\right)---\left(16\right)$

其中，Δθ_x、Δθ_y、Δθ_z分别为陀螺的x、y、z轴在[t_k t_k+1]采样时间间隔内的角增量；

故归一化后的四元数：

${\hat{q}}_i=\frac{q_i}{\sqrt{q_0^2+q_1^2+q_2^2+q_3^2}},i=\mathrm{0,1,2,3}---\left(17\right)$

步骤B33，用新的四元数更新姿态矩阵：

${\hat{C}}_n^b=\left[\begin{array}{ccc}{\hat{q}}_0^2+{\hat{q}}_1^2-{\hat{q}}_2^2-{\hat{q}}_3^2& 2({\hat{q}}_1{\hat{q}}_2+{\hat{q}}_0{\hat{q}}_3)& 2({\hat{q}}_1{\hat{q}}_3-{\hat{q}}_0{\hat{q}}_2)\\ 2({\hat{q}}_1{\hat{q}}_2-{\hat{q}}_0{\hat{q}}_3)& {\hat{q}}_0^2-{\hat{q}}_1^2+{\hat{q}}_2^2-{\hat{q}}_3^2& 2({\hat{q}}_2{\hat{q}}_3-{\hat{q}}_0{\hat{q}}_1)\\ 2({\hat{q}}_1{\hat{q}}_3+{\hat{q}}_0{\hat{q}}_2)& 2({\hat{q}}_2{\hat{q}}_3-{\hat{q}}_0{\hat{q}}_1)& {\hat{q}}_0^2-{\hat{q}}_1^2-{\hat{q}}_2^2+{\hat{q}}_3^2\end{array}\right]---\left(18\right)$

更新三个姿态角：

航向角

俯仰角 $\widehat{\mathrm{\θ}}=\arcsin \left[{\hat{C}}_n^b\left(\mathrm{1,2}\right)\right]---\left(19\right)$

横滚角 $\widehat{\mathrm{\γ}}=-\arctan \left[\frac{{\hat{C}}_n^b\left(\mathrm{0,2}\right)}{{\hat{C}}_n^b\left(\mathrm{2,2}\right)}\right]$

用准确的姿态变换矩阵代入公式中，算得比力fⁿ，从而计算出新的速度及位置。

6.一种基于权利要求1所述的用于水下滑翔器的导航系统，其特征在于：包括DSP处理单元、MEMS IMU导航元件；所述DSP处理单元包括存储模块、回溯解耦模块、前置滤波去噪模块、误差模块、观测量模块以及基于卡尔曼的自适应滤波算法模块；所述MEMS IMU导航元件用于采集原始的角速度、加速度及磁场数据，所有原始数据进入DSP处理单元进行解算，得到姿态角、速度及位置信息，并将得到的姿态角、速度及位置信息传送给DSP处理单元；

所述DSP处理单元用于接收MEMS IMU导航元件传送过来的姿态角、速度及位置信息，并将该姿态角、速度及位置信息传送给回溯解耦模块和观测量模块；

所述回溯解耦模块用于根据回溯解耦方法，将正常解算过程中解算的导航信息存储起来，当解算过程错误判断后，姿态角发生奇异突变，调用上一次正确解算的导航参数进行重新解算，得出新的姿态角、速度及位置信息并对其进行更新，同时将更新后的姿态角、速度及位置信息发送给前置滤波去噪模块；若姿态角未发生奇异突变，则将姿态角、速度及位置信息发送给前置滤波去噪模块；

所述滤波去噪模块用于接收回溯解耦模块传送过来姿态角、速度及位置信息；同时将姿态角、速度和位置信息进行滤波去噪，并将滤波去噪后的姿态角、速度和位置信息分别传送给误差模块和观测量模块；

所述误差模块用于根据滤波去噪模块传送的姿态角、速度和位置信息利用误差方程求导航信息误差得到状态量，并将该状态量传送给基于卡尔曼的自适应滤波算法模块；

所述观测量模块用于根据DSP处理单元推送的姿态角、速度及位置信息与滤波去噪模块推送的姿态角、速度及位置信息作差作为滤波器的观测量，并将该观测量传送给基于卡尔曼的自适应滤波算法模块；

所述基于卡尔曼的自适应滤波算法模块用于根据状态量和观测量进行姿态角、速度、位置误差的最优估计，再用所得误差的估计值校正惯导所得的姿态角、速度及位置，最终得到校正后准确的姿态角、速度和位置信息。

7.一种基于权利要求1所述的用于水下滑翔器的导航系统及姿态角回溯解耦方法，其特征在于，包括以下步骤：

步骤一，根据陀螺仪输出的角速度、加速度计输出的加速度、磁力计输出的磁场强度进行导航信息解算,经导航解算后得到姿态角、速度、位置信息的导航数据；

步骤二，根据导航解算得出的导航数据，判断因俯仰或横滚运动，引起的姿态角交叉耦合而造成的姿态角解算错误的节点；

步骤三，用因俯仰或横滚运动，使得姿态角交叉耦合明显增大而造成解算错误的前一步解算数据，计算载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影；

步骤四，用载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影通过解算得到新的姿态角、速度、位置信息。

8.根据权利要求1所述7的用于水下滑翔器的导航系统及姿态角回溯解耦方法，其特征在于：步骤二中判断因为俯仰或横滚运动而使得姿态角交叉耦合明显增大导致姿态角解算错误的节点的方法，包括以下步骤：

步骤二a，因俯仰或横滚运动，引起的姿态角交叉耦合而造成三个失准角φ_x,φ_y,φ_z错误，其中φ_x,φ_y,φ_z分别为载体坐标系的三轴偏离导航坐标系对应的三轴的偏离角，也即失准角；

步骤二b，将三个失准角φ_x,φ_y,φ_z代入姿态角校正方程其中是错误解算节点前一步正确的导航坐标系到载体坐标系的姿态角矩阵，

$C^T=\left[\begin{array}{ccc}1& {\mathrm{\φ}}_z& {-\mathrm{\φ}}_y\\ {-\mathrm{\φ}}_z& 1& {\mathrm{\φ}}_x\\ {\mathrm{\φ}}_y& {-\mathrm{\φ}}_x& 1\end{array}\right]$ 为姿态校正矩阵；

步骤二c，根据姿态角校正方程可得出四元数方程：

$\left|q_0\right|=0.5*\sqrt{1+C_{\mathrm{nco}}^b\left(\mathrm{0,0}\right)+C_{\mathrm{nco}}^b\left(\mathrm{1,1}\right)+C_{\mathrm{nco}}^b\left(\mathrm{2,2}\right)}$

$\left|q_1\right|=0.5*\sqrt{1+C_{\mathrm{nco}}^b\left(\mathrm{0,0}\right)-C_{\mathrm{nco}}^b\left(\mathrm{1,1}\right)-C_{\mathrm{nco}}^b\left(\mathrm{2,2}\right)}---\left(11\right)$

$\left|q_2\right|=0.5*\sqrt{1-C_{\mathrm{nco}}^b\left(\mathrm{0,0}\right)+C_{\mathrm{nco}}^b\left(\mathrm{1,1}\right)-C_{\mathrm{nco}}^b\left(\mathrm{2,2}\right)}$

$\left|q_3\right|=0.5*\sqrt{1-C_{\mathrm{nco}}^b\left(\mathrm{0,0}\right)-C_{\mathrm{nco}}^b\left(\mathrm{1,1}\right)+C_{\mathrm{nco}}^b\left(\mathrm{2,2}\right)};$

步骤二d，判断式(11)中的根号下的计算结果是否是负数，若是负数，则四元数q₀、q₁、q₂、q₃错误，后续的导航解算会依次错误，则上述式(11)中根号下计算结果出现负数是因姿态角交叉耦合造成导航解算错误的节点。

9.根据权利要求1所述7的一种用于水下滑翔器的导航系统及姿态角回溯解耦方法，其特征在于：所述步骤三中载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影为：

${\mathrm{\ω}}_{\mathrm{nb}}^b={\mathrm{\ω}}_{\mathrm{ib}}^b-C_n^b({\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)$

其中，为载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影；为陀螺仪输出的角速率；为地球坐标系相对于惯性坐标系的自转角速率在导航坐标系中的投影；为导航坐标系相对于地球坐标系的角速率在导航坐标系上的投影；是错误解算节点前一步正确的导航坐标系到载体坐标系的姿态矩阵。

10.根据权利要求1所述7的一种用于水下滑翔器的导航系统及姿态角回溯解耦方法，其特征在于：所述步骤四中通过载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影导航得出新的姿态、速度、位置信息的方法，包括以下步骤：

步骤四a，将代入 $\mathrm{\Δ\Θ}={\∫}_{t_k}^{t_{k+1}}{M}^{*}\left({\mathrm{\ω}}_{\mathrm{nb}}^b\right)\mathrm{dt}---\left(13\right);$

其中， $M^{*}\left({\mathrm{\ω}}_{\mathrm{nb}}^b\right)=\left[\begin{array}{cccc}0& {-\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bx}}& {-\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{by}}& {-\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bz}}\\ {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bx}}& 0& {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bz}}& -{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{by}}\\ {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{by}}& -{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bz}}& 0& {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bx}}\\ {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bz}}& {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{by}}& {-\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bx}}& 0\end{array}\right],$ 为在三个轴x、y、z轴上的分量；

步骤四b，四元数是由四个元素构成，将其定义为：Q(q₀,q₁,q₂,q₃)＝q₀+q₁i+q₂j+q₃k，，q₀、q₁、q₂、q₃是实数，i、j、k是相互正交的单位向量，采用毕卡逼近方法求解四元数微分方程：

$Q\left(t_{k+1}\right)=e^{\frac{1}{2}\mathrm{\Δ\Θ}}Q\left(t_k\right)---\left(14\right)$

其中，Q(t_k+1)、Q(t_k)分别代表t_k+1、t_k时刻的四元数向量，将式(14)进行泰勒级数展开得：

$Q\left(t_{k+1}\right)=e^{\frac{1}{2}\mathrm{\Δ\Θ}}Q\left(t_k\right)=[I+\frac{\frac{1}{2}\mathrm{\Δ\Θ}}{1!}+\frac{{\left(\frac{1}{2}\mathrm{\Δ\Θ}\right)}^2}{2!}+...]Q\left(t_k\right)---\left(15\right)$

将式(15)写成三角形式：

$Q\left(t_{k+1}\right)=[I\cos \frac{\mathrm{\Δ\θ}}{2}+\mathrm{\Δ\Θ}\frac{\sin \frac{\mathrm{\Δ\θ}}{2}}{\mathrm{\Δ\θ}}]Q\left(t_k\right)---\left(16\right)$

其中，Δθ_x、Δθ_y、Δθ_z分别为陀螺的x、y、z轴在[t_k t_k+1]采样时间间隔内的角增量；

故归一化后的四元数：

${\hat{q}}_i=\frac{q_i}{\sqrt{q_0^2+q_1^2+q_2^2+q_3^2}},i=\mathrm{0,1,2,3}---\left(17\right)$

步骤四c，用新的四元数更新姿态矩阵：

${\hat{C}}_n^b=\left[\begin{array}{ccc}{\hat{q}}_0^2+{\hat{q}}_1^2-{\hat{q}}_2^2-{\hat{q}}_3^2& 2({\hat{q}}_1{\hat{q}}_2+{\hat{q}}_0{\hat{q}}_3)& 2({\hat{q}}_1{\hat{q}}_3-{\hat{q}}_0{\hat{q}}_2)\\ 2({\hat{q}}_1{\hat{q}}_2-{\hat{q}}_0{\hat{q}}_3)& {\hat{q}}_0^2-{\hat{q}}_1^2+{\hat{q}}_2^2-{\hat{q}}_3^2& 2({\hat{q}}_2{\hat{q}}_3-{\hat{q}}_0{\hat{q}}_1)\\ 2({\hat{q}}_1{\hat{q}}_3+{\hat{q}}_0{\hat{q}}_2)& 2({\hat{q}}_2{\hat{q}}_3-{\hat{q}}_0{\hat{q}}_1)& {\hat{q}}_0^2-{\hat{q}}_1^2-{\hat{q}}_2^2+{\hat{q}}_3^2\end{array}\right]---\left(18\right)$

更新三个姿态角：

航向角

俯仰角 $\widehat{\mathrm{\θ}}=\arcsin \left[{\hat{C}}_n^b\left(\mathrm{1,2}\right)\right]---\left(19\right)$

横滚角 $\widehat{\mathrm{\γ}}=-\arctan \left[\frac{{\hat{C}}_n^b\left(\mathrm{0,2}\right)}{{\hat{C}}_n^b\left(\mathrm{2,2}\right)}\right];$

用准确的姿态变换矩阵代入公式中，算得比力fn，从而计算出新的速度及位置。