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

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

【技术实现步骤摘要】
一种用于水下滑翔器的导航系统及姿态角校正和回溯解耦方法
本专利技术涉及一种用于水下滑翔器的导航系统及姿态角回溯解耦方法，属于水下滑翔器导航定位

技术介绍
水下滑翔器是一种使用内在制动器通过控制自身浮力和姿态角来随水流滑翔的自主水下载体。水下滑翔器具有结构简单、功耗较低、能在水下长时间作业等特点，所以被用作海洋勘测、海洋数据搜集等作业。近年，水下滑翔器已经成为临海和开放海洋观测的重要部分。在水下执行的任务中，精确的位姿信息是至关重要的，因此水下导航是一个难点也是重点问题。水下滑翔器的体积小，成本低，所以不能安装太多高精度水下导航传感器。对于陆地车载来说，含有差分校正的全球定位系统(DGPS)能提供高精度的位姿信息，而且成本较低。但全球定位系统(GPS)信号在水下不能使用。惯性导航系统(INS)可以在短时间内提供精确的位姿信息，且能在没有GPS的情况下实现自主导航，基于微机电系统(MEMS)的INS的优势在使得MEMSIMU在低成本惯性
起到了重要作用。然而，因为陀螺仪和加速度计自身的固有偏差使得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代入姿态角校正方程其中是错误解算节点前一步正确的导航坐标系到载体坐标系的姿态角矩阵，为姿态校正矩阵；步骤B13，根据姿态角校正方程可得出四元数方程：步骤B14，判断式(11)中的根号下的计算结果是否是负数，若是负数，则四元数q0、q1、q2、q3错误，后续的导航解算会依次错误；从而得出上述式(11)中根号下计算结果出现负数是因姿态角交叉耦合造成导航解算错误的节点。所述步骤B2中载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影为：其中，为载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影；为陀螺仪输出的角速率；为地球坐标系相对于惯性坐标系的自转角速率在导航坐标系中的投影；为导航坐标系相对于地球坐标系的角速率在导航坐标系上的投影；。所述步骤B3中用载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影通过解算得出新的姿态角、速度、位置信息的方法，包括以下步骤：步骤B31，令将代入其中，为在三个轴x、y、z轴上的分量；步骤B32，四元数是由四个元素构成，将其定义为：Q(q0,q1,q2,q3)＝q0+q1i+q2j+q3k，q0、q1、q2、q3是实数，i、j、k是相互正交的单位向量。采用毕卡逼近方法求解四元数微分方程：其中，Q(tk+1)、Q(tk)分别代表tk+1、tk时刻的四元数向量。将式(14)进行泰勒级数展开得：将式(15)写成三角形式：其中，Δθx、Δθy、Δθz分别为x、y、z陀螺在[tktk+1]采样时间间隔内的角增量；归一化后的四元数：步骤B33，用新的四元数更新姿态矩阵：更新三个姿态角：航向角俯仰角横滚角用准确的姿态变换矩阵代入公式中，算得比力fn，从而计算出新的速度及位置。一种基于用于水下滑翔器的导航系统，包括DSP处理单元、MEMSIMU导航元件；所述DSP处理单元包括存储模块、回溯解耦模块、前置滤波去噪模块、误差模块、观测量模块以及基于卡尔曼的自适应滤波算法模块；所述MEMSIMU导航元件用于采集原始的角速度、加速度及磁场数据，所有原始数据进入DSP处理单元进行解算，得到姿态角、速度及位置信息，并将得到的姿态角、速度及位置信息传送给DSP处理单元；所述DSP处理单元用于接收MEMSIMU导航元件传送过来的姿态角、速度及位置信息，并将该姿态角、速度及位置信息传送给回溯解耦模块和观测量模块；所述回溯解耦模块用于根据回溯解耦方法，将正常解算过程中解算的导航信息存储起来，当解算过程错误判断后，姿态角发生奇异突变，调用上一次正确解算的导航参数进行重新解算，得出新的姿态角、速度及位置信息并对其进行更新，同时将更新后的姿态角、速度及位置信息发送给前置滤波去噪模块；若姿态角未发生奇异突变，则将姿态角、速度及位置信息发送给前置滤波去噪模块；所述滤波去噪模块用于接收回溯解耦模块传送过来姿态角、速度及位置信息；同时将姿态角、速度和位置信息进行滤本文档来自技高网...

【技术保护点】
一种用于水下滑翔器的导航系统姿态角校正方法，其特征在于，包括以下步骤：步骤A，根据陀螺仪输出的角速度、加速度计输出的加速度、磁力计输出的磁场强度进行导航信息解算,经导航解算后得到姿态角、速度、位置信息的导航数据；步骤B，根据导航解算出的导航数据，判断因俯仰或横滚运动，使得姿态角交叉耦合明显增大而造成的姿态角解算错误的节点，用姿态角交叉耦合造成解算错误的前一步解算数据重新计算，得出新的姿态角、速度、位置信息；步骤C，步骤B中回溯解耦后的姿态角、速度及位置信息经前置滤波器去噪后，一方面利用误差方程得到状态量，一方面和步骤A中经导航解算后得到姿态角、速度、位置信息分别作差得到观测量；将该状态量和观测量进入基于卡尔曼的自适应滤波算法，进行姿态角、速度、位置误差的最优估计，所得误差估计值校正惯导所得的导航信息，最终得到校正后准确的姿态角、速度和位置。

【技术特征摘要】
1.一种用于水下滑翔器的导航系统姿态角校正方法，其特征在于，包括以下步骤：步骤A，根据陀螺仪输出的角速度、加速度计输出的加速度、磁力计输出的磁场强度进行导航信息解算,经导航解算后得到姿态角、速度、位置信息的导航数据；步骤B，根据导航解算出的导航数据，判断因俯仰或横滚运动，使得姿态角交叉耦合明显增大而造成的姿态角解算错误的节点，用姿态角交叉耦合造成解算错误的前一步解算数据重新计算，得出新的姿态角、速度、位置信息；所述步骤B中的回溯解耦方法包括以下步骤：步骤B1，根据导航解算得出的导航数据，判断因俯仰或横滚运动，使得姿态角交叉耦合明显增大而造成的姿态角解算错误的节点；步骤B1中判断因俯仰或横滚运动，使得姿态角交叉耦合明显增大而造成的姿态角解算错误的节点的方法，包括以下步骤：步骤B11，因俯仰或横滚运动，使得姿态角交叉耦合而造成三个失准角φx,φy,φz错误，其中φx,φy,φz分别为载体坐标系的三轴偏离导航坐标系对应的三轴的偏离角，也即失准角；步骤B12，将三个失准角φx,φy,φz代入姿态角校正方程其中是错误解算节点前一步正确的导航坐标系到载体坐标系的姿态角矩阵，为姿态校正矩阵；步骤B13，根据姿态角校正方程可得出四元数方程：步骤B14，判断式(11)中的根号下的计算结果是否是负数，若是负数，则四元数q0、q1、q2、q3错误，后续的导航解算会依次错误；从而得出上述式(11)中根号下计算结果出现负数是因姿态角交叉耦合造成导航解算错误的节点；步骤B2，用因俯仰或横滚运动，使得姿态角交叉耦合明显增大而造成解算错误的前一步解算数据，计算载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影；所述步骤B2中载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影为：其中，为载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影；为陀螺仪输出的角速率；为地球坐标系相对于惯性坐标系的自转角速率在导航坐标系中的投影；为导航坐标系相对于地球坐标系的角速率在导航坐标系上的投影；步骤B3，用载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影通过解算得到新的姿态角、速度、位置信息；所述步骤B3中用载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影通过解算得出新的姿态角、速度、位置信息的方法，包括以下步骤：步骤B31，令将代入其中，为在三个轴x、y、z轴上的分量；步骤B32，四元数是由四个元素构成，将其定义为：Q(q0,q1,q2,q3)＝q0+q1i+q2j+q3k，q0、q1、q2、q3是实数，i、j、k是相互正交的单位向量，采用毕卡逼近方法求解四元数微分方程：其中，Q(tk+1)、Q(tk)分别代表tk+1、tk时刻的四元数向量，将式(14)进行泰勒级数展开得：将式(15)写成三角形式：其中，Δθx、Δθy、Δθz分别为陀螺的x、y、z轴在[tktk+1]采样时间间隔内的角增量；故归一化后的四元数：步骤B33，用新的四元数更新姿态矩阵：更新三个姿态角：用准确的姿态变换矩阵代入公式中，算得比力fn，从而计算出新的速度及位置；步骤C，步骤B中回溯解耦后的姿态角、速度及位置信息经前置滤波器去噪后，一方面利用误差方程得到状态量，一方面和步骤A中经导航解算后得到姿态角、速度、位置信息分别作差得到观测量；将该状态量和观测量进入基于卡尔曼的自适应滤波算法，进行姿态角、速度、位置误差的最优估计，所得误差估计值校正惯导所得的导航信息，最终得到校正后准确的姿态角、速度和位置。2.一种基于权利要求1所述的用于水下滑翔器的导航系统姿态角校正方法的导航系统，其特征在于：包括DSP处理单元、MEMSIMU导航元件；所述DSP处理单元包括存储模块、回溯解耦模块、滤波去噪模块、误差模块、观测量模块以及基于卡尔曼的自适应滤波算法模块；所述MEMSIMU导航元件用于采集原始的角速度、加速度及磁场数据，所有原始数据进入DSP处理单元进行解算，得到姿态角、速度及位置信息，并将得到的姿态角、速度及位置信息传送给DSP处理单元；所述DSP处理单元用于接收MEMSIMU导航元件传送过来的姿态角、速度及位置信息，并将该姿态角、速度及位置信息传送给回溯解...

【专利技术属性】
技术研发人员：陈熙源，黄浩乾，周智恺，吕才平，
申请(专利权)人：东南大学，
类型：发明
国别省市：江苏;32