SINS/DVL水下初始对准与定位导航

李贤义,齐建宇,赵鹏飞

(北京航天自动控制研究所，北京 100854)



【信息科学与控制工程】

SINS/DVL水下初始对准与定位导航

李贤义,齐建宇,赵鹏飞

(北京航天自动控制研究所，北京 100854)

在仅有初始位置信息的条件下，给出一种SINS/DVL组合导航系统初始对准与定位导航的方法；以惯性坐标系作为SINS解算的参考基准并借助DVL测量的信息，用四元数卡尔曼滤波进行初始对准；计算载体在惯性凝固载体坐标系下的位移增量再转换至导航坐标系，完成在对准过程中及对准完成以后的定位导航；处理试验数据表明：该方法能够实现SINS/DVL组合导航系统水下航向大失准角初始对准与定位导航，可在600 s内完成初始对准，在航行4 000 s后定位精度约为航程的0.4%。

SINS/DVL；初始对准；四元数；定位导航

目前，常见捷联惯导/声多普勒速度计程仪(SINS/DVL)组合导航系统是在水面系泊或航行状态下根据GPS等信号源提供的速度位置信息，进行初始对准，由GPS等信号源装订组合导航阶段的初始位置信息，然后利用SINS和DVL测量的信息进行组合导航。在特定情况下，SINS/DVL组合导航系统也需要具备在水下完成初始对准的能力。水下环境复杂，信息源少，GPS信号在水中衰减很快，在水深范围内无法直接接收卫星信号，无法获得连续的位置信息。因此在仅有初始位置信息的条件下，利用SINS/DVL组合导航系统自身的测量信息，完成水下航向大失准角初始对准，满足建立精确的姿态、速度和位置信息的要求，并兼顾考虑对准时间、计算量、数据存储容量的要求，是一项有意义的研究工作。

针对惯导系统初始对准和对准完成时刻位置信息精确求解， Thompson IC[1]、严恭敏[2]提出了逆向组合导航算法，正向卡尔曼滤波、正向导航解算和逆向卡尔曼滤波、逆向导航解算同时进行，对历史数据进行逆向计算，重建航迹得到对准结束时刻精确位置。李万里[2]提出了与之类似的SINS/DVL组合导航回溯算法，并提出了有效的惯性测量数据压缩方案，降低了数据存储量，提升了运算速度。严恭敏[4]还提出在车载条件下，将载体的位移增量先转换到惯性坐标系，再根据每次更新求得的姿态矩阵将位移增量转换至导航坐标系，具备在对准完成时刻修正位置信息的能力。

本文在文献[4]的基础上，给出一种SINS/DVL组合导航系统水下航向大失准角初始对准与定位导航的方法，以惯性坐标系作为SINS解算参考基准并借助DVL测量的信息，初始对准和定位导航同时进行，将四元数卡尔曼滤波用于姿态信息更新，按时间分段计算载体在惯性凝固载体坐标系下的位移增量，具备了在对准完成时刻获得较为精确位置信息的能力，并可以适用于更长时间的定位导航。

1 姿态初始对准

SINS/DVL组合导航系统在水下容易受到浪涌、振动等影响，现有技术无法保证载体在水下处于完全静止状态，因此载体在水下保持悬停也无法根据陀螺和加速度计的输出直接计算姿态矩阵。本文以惯性坐标系作为SINS解算参考基准并借助DVL测量的信息完成初始对准，涉及的坐标系包括：当地地理坐标系(空间指向东北天)、惯导载体坐标系(b系)、惯性凝固载体坐标系(ib0系)、惯性凝固导航坐标系(in0系)，选取当地地理坐标系为导航坐标系(n系)，具体定义见文献[2]。

(1)

(2)

其中，c表示cos，s表示sin，Δ=λt-λ0+wiet。

(3)

(4)

(5)

(6)

(7)

(8)

2 四元数卡尔曼滤波

2.1 量测方程

将四元数矢量部分放在标量部分的前面，以下用b、r表示b(t)、r(t)，将b、r表示成四元数的形式：

(9.1)

(9.2)

用Q表示in0到ib0的旋转四元数：

(10)

(11)

将3×1的矩阵x映射到4×4的矩阵γ(x)、χ(x)，映射关系如下：

(12.1)

(12.2)

根据以上定义，将矩阵的元素直接代入并整理化简得到

(13.1)

(13.2)

由方程(11)、(13)，有：

(14)

(15)

令：

(16)

(17)

(18)

将方程(15)～方程(18)代入方程(14)，得到

(19)

定义如下4×3的矩阵Ξ(Q)：

(20)

将矩阵元素代入并整理化简，得到：

(21)

将方程(21)代入方程(19)，得到：

(22)

设tk时刻的四元数Q为Qk，量测阵H记为Hk，则tk时刻离散化的观测方程为：

(23)

假设δb为零均值的高斯白噪声，协方差阵为Rk，即：

(24.1)

(24.2)

2.2 状态方程

Q表示in0到ib0的旋转四元数，惯性凝固载体坐标系ib0和惯性凝固导航坐标系in0相对于惯性空间均保持不变，所以Q在整个对准过程中为常值。设tk-1时刻的四元数Q为Qk-1，有

(25)

2.3 量测噪声方程

将量测方程改写如下：

(26)

(27)

δb的协方差阵Rk可以选取以下两种形式进行建模分析：

(28.1)

(28.2)

(29)

(30)

其中Ei、e1、e2、e3定义如下：

方程(29)中：

(31)

(32)

将方程(31)(32)代入方程(29)，得到：

(33)

2.4 四元数归一化处理

(34)

(35)

3 定位导航

载体在导航坐标系下的速度计算公式如下：

(36)

本文通过求解载体在ib0系下的位移增量实现对准过程中及对准完成以后的定位导航。在SINS/DVL组合导航系统对准时间T内，载体自身运动在ib0系下的位移增量是：

(37)

利用分部积分公式，载体在导航坐标系n下的位移增量如下：

(38)

(39.1)

(39.2)

在对准完成以后，载体按照以下方式进行位置更新：

(40)

4 试验验证

本文采用船载试验的实测数据进行算法的性能分析和验证。主要试验设备的指标如下：激光陀螺零偏稳定性0.01°/h(1σ)，加速度计零偏稳定性1×10-4g0(1σ)，更新频率100 Hz；DVL的测速精度0.5%V，更新频率1 Hz；对比用的GPS接收机，位置更新频率1 Hz。

把组合导航系统本身内置算法称为算法1，试验过程按算法1要求进行，在对准阶段试验船漂浮在长江中央，对准结束后试验船缓慢加速至约8 knot航行。本文所用算法称为算法2，算法2起始时刻选为算法1的对准完成时刻。为评价算法2提供的姿态、速度、位置的准确性，以算法1提供的姿态、速度、GPS提供的位置作为参考基准。

图1，图2分别是两种算法提供的姿态角曲线，速度曲线。由图1，图2可知，算法1在图1中0时刻提供的航向角为308.18°，算法2在初始姿态角大小安全未知的情况下，假设初始航向角、俯仰角、横滚角均为0，在对准时间T=600 s后提供的姿态角、速度便能够很好的跟踪算法1提供的姿态角、速度，该初始对准算法采用线性滤波，对初始航向角大小没有要求，不需要复杂的线性化和非线性建模与滤波计算，在系统获得初值位置信息后即可启动，不需要专门的系泊对准时间，增强了系统的快速响应能力。

图1 姿态角曲线

图2 速度曲线

图3是以GPS提供的位置信息为基准，算法2的位置误差、里程及定位精度曲线，在600 s之前随着姿态估计精度的提高，位置误差和定位精度逐步趋于收敛；在600 s以后，位置误差随里程的增加呈增加的趋势，定位精度基本维持在0.4%航程以内，说明本文所给出的定位导航方法，在水下无GPS等信号源提供连续位置信息情况下，具备在对准完成时刻获得较为精确位置信息的能力，相比文献[4]，可以适用于更长时间的定位导航，与航迹重建补偿算法、回溯导航算法和逆向导航算法相比，不需要存储大量的数据和回溯计算，适合于工程应用。

图3 位置误差、里程及定位精度

5 结论

本文给出了一种SINS/DVL组合导航系统水下航向大失准角初始对准与定位导航的方法。在仅给定初始位置信息的情况下，即可实现SINS/DVL组合导航系统连续提供姿态、速度、位置信息。处理试验数据表明：本文所给方法有效可行。

[1] THOMPSON I C，KENNETH J，MORGAN S.Rapid Self-alignment of a Strapdown Inertial System through Real-time Reprocessing[P].United States Patent:US7 739045B2，2010-6-15.

[2] 严恭敏,严卫生，徐德民.逆向导航算法及其在捷联罗经动基座初始对准中的应用[C] //第27届中国控制会议论文集.北京：北京航空航天大学出版社，2008:724-729.

[3] LI Wanli,WU Wenqi,WANG Jinling.A Novel Backtracking Navigation Scheme for Automoumous Underwater Vehicles[J].Measurerment,2013(9)：22.

[4] 严恭敏，翁俊，白亮，等.基于惯性参考系的动基座初始对准与定位导航[J].系统工程与电子技术，2011，33(3):618-621.

[5] CHOUKROUN D,BAR-ITZHACK I Y,OSHMAN Y.A novel quaternion Kalman filter[R].TAE Report 930,Faculty of Aerospace Engineering,Technion-Israel Institude of Technology,Haifa,Israel,2004.

[6] BAR-ITZHACK I Y,OSHMAN J.Attitude Determination from Vector Observations:Quaternion Estimation[J].IEEE Trans On AES,1985,21(1):128-135.

[7] 张力军，张士峰，杨华波.基于 MEKF 的航天器姿态确定算法[J].国防科技大学学报，2013，35(6)：46-52.

[8] 尹剑.捷联姿态解算中方向余弦与四元数法分析比较[J].四川兵工学报，2015，6(9):106-110.

[9] 项凤涛.捷联系统四元素姿态解算的精细积分法[J].四川兵工学报，2010,31(5):103-106.

[10]CHOUKROUN D,BAR-ITZHACK I Y,OSHMAN Y.Novel quaternion Kalman filter[J].IEEE Transactions on Aerospace and Electronic Systems,2006,42(1):174-190.

(责任编辑 杨继森)

Initial Underwater Alignment and PositionDetermination for SINS/DVL

LI Xian-yi, QI Jian-yu, ZHAO Peng-fei

(Beijing Aerospace Automatic Control Institute， Beijing 100854， China)

The paper presented a method of initial underwater alignment and position determination for SINS/DVL integrated navigation system under the condition of the only initial position information. By taking inertial frame as computing reference and with the adding of DVL, a Kalman filter for estimating the quaternion was established to realize the initial alignment; Moreover, a real-time position determination ability was accessed by adding the distance of the carrier travelled in the navigation frame, which is transformed from that in the inertial frame, avoiding recording mass of data and “playback” calculation. Finally, some tests were carried out to prove the correctness and effectiveness of the presented method of initial underwater alignment with large course misalignment angle and position determination for SINS/DVL integrated navigation system, and the results show that it can achieve the initial alignment in 600 seconds and the position accuracy is about 0.4% of the distance travelled in 4 000 seconds.

SINS/DVL；initial alignment；quaternion；position determination

2016-11-22；

2016-12-25 作者简介：李贤义 (1990—)，男，硕士，主要从事惯性导航研究。

10.11809/scbgxb2017.04.029

李贤义,齐建宇,赵鹏飞.SINS/DVL水下初始对准与定位导航[J].兵器装备工程学报，2017(4):132-136.

format:LI Xian-yi, QI Jian-yu, ZHAO Peng-fei.Initial Underwater Alignment and Position Determination for SINS/DVL[J].Journal of Ordnance Equipment Engineering,2017(4):132-136.

V249.31

A

2096-2304(2017)04-0132-05