技术领域
[0001] 本
发明属于导航技术领域，涉及水下滑翔器的导航
定位，具体涉及一种水下滑翔器导航系统及其低精度与高精度工作状态之间的切换方法。
背景技术
[0002] 水下滑翔器在实际导航定位工作中需要兼顾精度与能耗的基本条件，高精度工作状态需要更多的
传感器同时工作，同时
算法复杂度也会大幅提升，但这样会导致较高的
能量消耗，因此系统需要在精度和能耗间做出选择。目前，基于微
机电系统的惯性测量单元(MEMS-IMU)因其重量轻，体积小，功耗低，且短时间内精度较高的优点成为了水下滑翔器的首选导航元件。但MEMS-IMU自身存在一些固有误差，加上噪声影响，工作时会产生非静态的随机误差和严重的非线性，并且误差会不断累计。因此，如何最大化减少MEMS-IMU的各项误差成为了精确导航定位中的首要问题。
[0003] 水下滑翔器处于低精度工作状态时，采用传统的四元数算法和航位推算即可完成基本的
姿态解算和
位置估计，从而满足低精度工作要求。当水下滑翔器处于高精度工作状态时，上述方法并不能满足工作要求，同时GPS在水下容易遇到失
锁、多径效应问题，无法实现其高精度的导航定位功能，因此高精度工作状态需要采用一定的滤波算法来提升系统的精度。
[0004] 目前水下滑翔器的主要问题在于精度与能耗之间的权衡关系，高精度容易导致系统的高能耗，但低精度又无法满足特定的导航定位要求，已有的技术方案通过间断性浮出水面利用GPS进行精确校正，但尚未能做到不损失精度的情况下在低精度和高精度工作状态之间直接灵活切换。
发明内容
[0005] 发明目的：为了克服
现有技术中存在的不足，提供一种用于水下滑翔器不同精度要求下的自主导航，具有灵活性高，
稳定性好，适应性强一系列优点，能够在兼顾功耗与精度的要求下准确有效地得到水下滑翔器的
位姿信息的水下滑翔器导航系统。
[0006] 技术方案：为实现上述目的，本发明提供一种水下滑翔器导航系统，包括
微机电系统惯性测量单元，全球卫星定位系统接收模
块，
电子罗盘，三轴磁
力计，压力计，数字
信号处理模块。
[0007] 所述微机电系统惯性测量单元集成了三轴
加速度计和三轴
陀螺仪。
[0008] 所述全球卫星定位系统接收模块用于系统在状态切换过程中对水下滑翔器的位置和姿态进行校正，并将校正融合后的数据传递给下一个工作状态。
[0009] 所述
电子罗盘和三轴磁力计用来辅助微机电系统惯性测量单元进行航向信息的校正。
[0010] 所述压力计用来测量水下滑翔器的垂直方向的位置信息。
[0011] 所述
数字信号处理模块完成系统在不同工作状态下的导航解算，当系统状态切换时，数字
信号处理模块负责姿态、位置信息的传递以及全球卫星定位系统接收模块对姿态、位置的校正融合，最终实现姿态、位置数据的输出。
[0012] 进一步地，导航系统具有低精度和高精度两种不同精度的工作状态并且两种工作状态可以互相切换，所述低精度工作状态下系统启用微机电系统惯性测量单元与三轴磁力计，系统功耗较低；所述高精度工作状态下系统启用微机电系统惯性测量单元、电子罗盘和压力计，系统功耗较高。
[0013] 进一步地，所述微机电系统惯性测量单元集成了三轴加速度计和三轴陀螺仪主要用来测量系统的姿态、速度以及航向。
[0014] 所述水下滑翔器导航系统其解算部件为数字信号处理模块，数字信号处理模块完成所有的
数据处理、传输工作从而实现导航解算和状态切换。
[0015] 一种水下滑翔器导航系统的低精度和高精度工作状态的切换方法，包含以下步骤：
[0016] 1)当水下滑翔器导航系统处于低精度工作状态时，系统启用微机电系统惯性测量单元和三轴磁力计，采用四元数算法与航位推算估计系统的姿态与位置。当需要进行状态切换时，首先判断水下滑翔器能否浮出水面，若能浮出水面，利用全球卫星定位系统接收模块对系统的姿态和位置进行校正，并将融合后的数据传递给下一工作状态；若水下滑翔器不满足浮出水面条件，则系统直接将当前姿态、位置信息传递给下一状态。
[0017] 2)当水下滑翔器导航系统处于高精度工作状态时，系统启用微机电系统惯性测量单元、电子罗盘和压力计，对各传感器进行误差补偿和去噪校正，并采用基于互补滤波的四元数算法和基于AEKF的位置推算估计系统的姿态与位置。当需要进行状态切换时，首先判断水下滑翔器能否浮出水面，若能浮出水面，利用全球卫星定位系统接收模块对系统的姿态和位置进行校正，并将融合后的数据传递给下一工作状态；若不满足浮出水面条件，则系统直接将当前姿态、位置信息传递给下一状态。
[0018] 进一步地，系统通过压力计辅助判断水下滑翔器是否浮于水面。
[0019] 有益效果：本发明与现有技术相比，因为水下滑翔器的导航定位精度要求并非一成不变，而是随着实际要求的变化而变化，考虑到能耗与精度的利弊关系，为水下滑翔器设定低精度与高精度两种工作状态，根据指令需要切换状态时，当前状态的速度、位置、姿态信息传输给下一状态并作为初始状态，继续进行工作，满足水下滑翔器在不同工作环境、不同精度要求下进行状态切换，做到能耗与导航精度之间的灵活选择，最大化解决能耗与精度之间的矛盾问题，最终实现高实时性，长航时，高精度，高稳定性的综合性能。
附图说明
[0020] 图1为本发明的系统结构及低精度与高精度工作状态转换示意图。
具体实施方式
[0021] 如图1所示，本发明提供本发明提供一种水下滑翔器导航系统，包括微机电系统惯性测量单元(MEMS-IMU)，全球卫星定位系统接收模块(GPS)，电子罗盘，三轴磁力计，压力计，数字信号处理模块(DSP)。以下分别将微机电系统惯性测量单元、全球卫星定位系统接收模块和数字信号处理模块分别表述为MEMS-IMU、GPS和DSP。
[0022] 所述MEMS-IMU集成了三轴加速度计和三轴陀螺仪用来测量系统的姿态、速度以及航向。
[0023] 所述GPS用于系统在状态切换过程中对水下滑翔器的位置和姿态进行校正，并将校正融合后的数据传递给下一个工作状态。需要说明的是由于GPS在水下容易遇到失锁、多径等问题，无法实现精确导航、定位的功能，在本系统中，GPS仅当水下滑翔器系统浮于水面时工作，即在状态切换过程中，GPS对水下滑翔器上一状态的姿态、位置进行校正，并将融合后的数据传递给下一个工作状态。
[0024] 所述电子罗盘和三轴磁力计用来辅助MEMS-IMU进行航向信息的校正，三轴磁力计用于低精度情况下辅助MEMS-IMU进行航向信息的校正，电子罗盘为三维电子罗盘，具有倾
角补偿的特性，因此更适用于高精度的工作状态。
[0025] 所述压力计用来测量水下滑翔器的垂直方向的位置信息，可用来辅助判断水下滑翔器是否浮于水面。
[0026] 所述DSP模块负责整个导航系统的数据处理与传输，DSP完成系统在不同工作状态下的导航解算，当系统状态切换时，DSP负责姿态、位置信息的传递以及利用GPS对姿态、位置的校正融合，最终实现姿态、位置数据的输出。
[0027] 如图1所示，导航系统具有低精度和高精度两种不同精度的工作状态并且两种工作状态可以互相切换，所述低精度工作状态下系统启用MEMS-IMU与三轴磁力计，系统功耗较低；所述高精度工作状态下系统启用MEMS-IMU、电子罗盘和压力计，系统功耗较高；一种用于水下滑翔器导航系统低精度与高精度转换的方法，包含以下步骤：
[0028] (1)当水下滑翔器导航系统处于低精度工作状态时，系统启用MEMS-IMU和三轴磁力计模块，采用四元数算法和航位推算估计系统的姿态和位置，四元数算法可以参见秦永元所编《惯性导航》一书，基本的航位推算可以参见吉林大学谢彦新的硕士学位论文《基于神经网络的AUV航位推算算法研究》。
[0029] (2)当水下滑翔器导航系统处于高精度工作状态时，系统首先对各个传感器进行误差补偿和去噪校正，在保证传感器精度的
基础上进行数据的采集与处理。
[0030] 在姿态解算方面，采用了基于互补滤波的四元数算法，步骤如下：
[0032] 首先把加速度计的数据归一化，对三维向量的长度进行等倍数的变换，得到单位向量。向量的方向并没有改变，归一化的过程也是为了与单位四元数进行对应。实现方式为：
[0033]
[0034] 其中，ax,ay,az表示归一化之前的三维加速度向量，a'x,a'y,a'z表示归一化之后的三维加速度向量。
[0035] ②提取四元数的方向余弦矩阵中的重力分量
[0036] 此步骤需要将当前姿态的重力在三个轴上的分量分离出来，根据四元数方向余弦矩阵的实际意义，矩阵的第三行的三个元素就是对应的重力分量，由方向余弦矩阵
变形可得：
[0037]
[0038] 为了方便表示，式中T指代方向余弦矩阵中的元素，因此可得旋转过的重力为：
[0039]
[0040] 其中，gb表示载体
坐标系下的重力向量，gn表示参考坐标系下的且已经归一化过的重力向量，由最终结果可以看出，要提取的重力分量就是原来四元数的方向余弦矩阵的第三行的行向量转置。
[0041] ③向量叉乘得到姿态误差
[0042] 在解算过程中，会得到两个重力向量，一个是加速度计数据得到的重力向量，一个是由陀螺仪积分解算姿态后推导出的重力向量，即第②步所述工作。测得的重力向量与计算得到的重力向量往往会存在偏差，根据向量叉乘的性质可以将两个重力向量进行叉乘得到偏差。
[0043] ④误差积分
[0044] 对当前姿态分离出的重力分量与当前加速度计测得量的差值进行积分消除误差。
[0046] 陀螺积分不断更新，也不停的进行误差修正，同时代表的姿态也在不断更新。误差输入PI
控制器后与陀螺仪输出的角速度信息
叠加，可以得到一个修正的输入量，再将新的输入量代入龙格-库塔方程来更新q0,q1,q2,q3。
[0047] 在位置估计方面，采用了自适应
扩展卡尔曼滤波，步骤如下：
[0048] (a)时间更新：
[0049]
[0050] 其中， 表示k-1时刻的位置状态量，Pk-1|k-1为k-1时刻对应的误差协方差矩阵，Q为过程噪声协方差矩阵，F为状态转换矩阵，uk为控制输入。
[0052] 初始化：
[0053] 首先使用 更新 其中上标l表示迭代次数。
[0054] 范围量测方程在 处的雅克比矩阵：
[0055]
[0056] 第l+1次迭代时的卡尔曼增益：
[0057]
[0058] 第l+1次迭代k时刻的状态量及误差协方差：
[0059]
[0060]
[0061] 再使用 更新
[0062] 范围量测方程在 处的雅克比矩阵：
[0063]
[0064] K时刻的噪声协方差：
[0065]
[0066] 更新过的中间时刻的误差协方差：
[0067]
[0068] 最终的滤波结果为：
[0069]
[0070] 其中上标N表示迭代了N次，一般取合适的较大数值。
