一种用于水下无人航行器在极区导航的初始粗对准方法与流程

本发明属于水下无人航行器导航的粗对准算法领域，尤其涉及适用于水下无人航行器在极区航行过程中导航的初始粗对准方法。

背景技术：

水下无人航行器是海洋探索与海洋开发的重要智能装备。由于在完成任务的过程中不需要人类参与其中，在海洋探索，海洋开发和海底铺管作业等发挥着重要的作用，准确的导航是水下无人航行器完成任务的重要前提。由于水下无人航行器主要采用捷联惯导系统，初始条件对于捷联惯导系统的准确性有重要的影响。因而初始对准算法十分重要。

针对非极区的初始对准，学者们提出了许多方法初始粗对准算法，在非极区可以实现较好的粗对准。如马荟在文章《舰船捷联惯导系统粗对准方法研究》(发表于2013，哈尔滨工程大学，工学硕士论文)中，以及何东旭在文章《auv水下导航关键技术研究》(发表于2013,哈尔滨工程大学，工学博士论文)中提到的解析式粗对准算法、水平二阶调平+方位估算粗对准算法、基于姿态矩阵的粗对准算法以及基于姿态四元数的粗对准算法等。但是由于极区自然条件和导航条件十分恶劣，特别是地理经线在极点附近快速收敛，会为传统的粗对准算法在极区应用是带来困难。由于用于水下无人航行器非极区导航的粗对准算法是基于传统指北方位的捷联惯导系统的粗对准算法。在极区，自然环境和地理环境十分特殊，在极点附近经线快速收敛，这使得传统的指北方位的捷联惯导系统在极区应用过程中会存在计算溢出和误差放大等问题。因而传统的用于水下无人航行器非极区导航的粗对准算法在极区无法应用。

技术实现要素：

本发明的目的是提供一种能够有效的用于极区导航并提高粗对准精度的用于水下无人航行器在极区导航的初始粗对准方法。

步骤一：获取水下无人航行器在极区所处位置的经纬度，确定初始对准时刻，地心地固坐标系e到格网坐标系g之间的方向余弦矩阵

步骤二：选取时间间隔δt，确定惯性坐标系i与地心地固坐标系e之间的方向余弦矩阵

步骤三：由水下无人航行器中，捷联惯导系统中陀螺仪的输出，确定载体坐标系b到初始时刻与惯性坐标系相固连的载体坐标系b0之间的方向余弦矩阵

步骤四：确定初始对准时刻与惯性坐标系固连的载体坐标系b0到惯性坐标系i的方向余弦矩阵

步骤五：由步骤一至步骤四结果，确定载体坐标系b到格网坐标系g的方向余弦矩阵即为所要确定的初始对准姿态矩阵；

步骤六：根据步骤五得到的初始对准姿态矩阵解算提取水下无人航行器经过初始粗对准后得到的初始姿态角φx，φy和φz以及初始姿态误差角δφx，δφy和δφz。

本发明方法还满足：

地心地固坐标系e到格网坐标系g之间的方向余弦矩阵满足：

其中，为地心地固坐标系到地理坐标系之间的方向余弦矩阵，为地理坐标系到格网坐标系之间的方向余弦矩阵，λ和l分别为水下无人航行器所在位置的经度和纬度值，格网坐标系天向分量与地理坐标系天向分量之间的夹角σ满足：

初始对准姿态矩阵满足：

本发明有益效果：本发明解决传统粗对准方法在极区不可用的问题，考虑了水下无人航行器导航的特点以及极区自然环境和导航环境的特点，引入格网坐标系g作为导航坐标系，建立水下无人航行器极区导航的初始粗对准方法。设计了基于格网坐标系的水下无人航行器极区航行粗对准方法，可以有效提高水下无人航行器在极区导航过程中的初始粗对准精度。更加适用于水下无人航行器在极区的航行。

附图说明

图1是本发明提出的用于水下无人航行器在极区导航的初始粗对准方法流程图；

图2是本发明提出的水下无人航行器在极区导航的初始粗对准方法与传统粗对准算法应用在水下无人航行器上的横摇角误差曲线的对比；

图3是本发明提出的水下无人航行器在极区导航的初始粗对准方法与传统粗对准算法应用在水下无人航行器上的纵摇角误差曲线的对比；

图4是本发明提出的水下无人航行器在极区导航的初始粗对准方法与传统粗对准算法应用在水下无人航行器上的航向角误差曲线的对比。

具体实施方式

下面结具体实施方式对本发明作进一步的详细说明：

本发明的主要目的是结合水下无人航行器导航的特点，以及极区自然环境和导航环境的特点，设计适用于水下无人航行器在极区导航的初始粗对准方法。

为了实现以上目的，本发明的技术方案主要包括以下步骤：

(1)根据水下无人航行器在极区所处位置的经纬度，确定初始对准时刻，地心地固坐标系e到格网坐标系g之间的方向余弦矩阵

其中，为地心地固坐标系到地理坐标系之间的方向余弦矩阵，为地理坐标系到格网坐标系之间的方向余弦矩阵。λ和l分别为水下无人航行器所在位置的经度和纬度值。σ是水下无人航行器所在位置处，格网坐标系天向分量与地理坐标系天向分量之间的夹角可以通过下式确定。

(2)根据时间间隔δt确定惯性坐标系与地心地固坐标系之间的方向余弦矩阵

δt＝t-t0,(4)

其中，t0为水下无人航行器极区导航对准的初始时刻，ωie为地球自转角速度。

(3)根据水下无人航行器中，捷联惯导系统中陀螺仪的输出，确定载体坐标系b到初始时刻与惯性坐标系相固连的载体坐标系b0之间的方向余弦矩阵

其中，为的反对称矩阵，为的反对称矩阵，为载体坐标系下b0坐标系相对于b坐标系的旋转角速度，为载体坐标系下惯性坐标系相对于载体坐标系的旋转角速度。

(4)根据重力加速度在惯性空间中的圆锥慢漂原理，确定初始对准时刻与惯性坐标系固连的载体坐标系b0到惯性坐标系i的方向余弦矩阵在地理坐标系和惯性坐标系中，重力加速度分别可以表示为：

g^g＝[00-g]^t,(7)

为了抑制干扰误差，引入积分运算。

由于静止状态的水下无人航行器导航中的比力由重力加速度，干扰加速度和加速度计偏移组成，那么比力和比力的积分运算可以分别表示为：

忽略干扰加速度和加速度计偏移积分后较小的量，上式可以简化为；

分别选取t0时刻和tk时刻，带入上式，并于综合可得初始对准时刻与惯性坐标系固连的载体坐标系b0到惯性坐标系i的方向余弦矩阵

(5)综合以上步骤得到的结果，确定载体坐标系b到格网坐标系g的方向余弦矩阵即为所要确定的初始对准姿态矩阵。

(6)根据上一步得到的初始对准姿态矩阵解算提取水下无人航行器经过初始粗对准后得到的初始姿态角φx，φy和φz以及初始姿态误差角δφx，δφy和δφz。

具体实施方案：

将利用matlab仿真软件验证本发明的有效性。在matlab仿真软件中建立水下无人航行器在极区导航时的初始粗对准模型。设定初始条件，进行试验仿真。将本发明设计的适用于水下无人航行器在极区航行时的初始粗对准方法，即格网粗对准方法，与传统的粗对准算法分别用于水下无人航行器极区导航过程中的初始对准，通过比较两种方法的优劣。

matlab仿真条件设置如下：

水下无人航行器的位置为东经126°，北纬80°；仿真时间设置为120秒；滤波周期设置为0.1秒；横摇、纵摇和艏摇角的幅值分别为4°,5°和3°，周期分别为3s5s和7s，初始值分别为0°,0°和0°；陀螺常值漂移和随机漂移分别为0.03°/h和(0.001°/h)²；加速度计常值漂移和随机漂移分别为1×10^-4g0和(1×10^-6g0)²。

仿真结果：

基于上述仿真条件的设置，以及通过matlab仿真软件对所发明方法的编写，经过仿真可以得到如图2到图4的仿真实验结果。

根据图2和图4的仿真实验结果对比，本发明所提出的适用于水下无人航行器在极区导航过程的初始粗对准方法经过100次试验后三个坐标轴方向上的失准角分别与基于传统的粗对准算法的三个轴上的失准角相比较。可以看出，本发明所提出的适用于水下无人航行器极区导航的初始粗对准算法能够有效的用于水下无人航行器在极区的初始粗对准过程，相比于传统的粗对准算法具有更好的准确性。