CN106643709A - 一种海上运载体的组合导航方法及装置 - Google Patents

Abstract

本发明公开了一种海上运载体的组合导航方法，所述海上运载体上装备有惯性导航主系统、水声定位子系统、偏振光导航子系统；首先利用惯性导航主系统进行捷联解算，利用水声定位子系统进行测距计算，利用偏振光导航子系统进行方位计算；然后建立以惯性导航主系统的误差为状态量、水声定位子系统的测距信息和偏振光导航子系统的航向信息为观测量的扩展卡尔曼滤波模型，对惯性导航主系统的误差进行闭环修正。本发明还公开了一种海上运载体的组合导航装置。相比现有技术，本发明一方面有效提高了导航精度，另一方面由于摆脱了对卫星定位技术的依赖，因此具有更好的隐蔽性和抗干扰性。

Description

一种海上运载体的组合导航方法及装置

技术领域

本发明涉及一种导航方法，尤其涉及一种海上运载体的组合导航方法及装置，属于导航定位技术领域。

背景技术

惯性导航是海上运载体最主要的导航方式之一，具有不依赖外界信息、隐蔽性好、抗辐射性强、全天候等优点，是完全自主的导航。由于陀螺仪和加速度计的误差漂移，惯导系统必须借助于其他导航方式的辅助才能长时间使用。通常可以采用多普勒测速仪(DVL，Doppler Velocity Log)辅助惯导的工作方式，该方式具有自主性高、设备简单的特点，通过DVL提供的速度信息，可以校正惯导的速度误差、横滚角误差和俯仰角误差，然而，随时间累积的定位误差和航向角误差是无法校正的。

为了提高惯导系统的精度，研究者提出了利用GPS导航系统、重力辅助导航系统、天文导航系统等与惯导系统相结合的多种组合导航方法。

文献[潘学松.基于SINS/DVL/GPS的AUV组合导航系统关键技术研究[D].青岛：中国海洋大学，2011.]给出了SINS/DVL/GPS组合导航方式，该方式利用多普勒测速仪提供的速度信息，校正惯导的速度和姿态误差，但无法对航向进行持续有效的校正；利用GPS进行位置修正，但其可靠性和抗干扰性较差；利用基于遗传算法的神经网络滤波来进行数据融合，提高了滤波精度和可靠性，但增加了算法的复杂性且难以满足实时性的要求。

文献[彭富清.海洋重力辅助导航方法及应用[D].郑州：解放军信息工程大学，2009.]给出了海洋重力辅助惯性导航的方法，该方法利用重力仪的测量值与重力图进行匹配来校正惯性导航位置误差，保证了系统的自主性，但其难以对航向进行有效修正，并且该导航方式对重力异常图的制作精度要求较高且仅适用于重力特征明显区域，无法成为一种独立应用的导航手段。

文献[孙剑明.基于星图识别的舰船天文导航关键技术研究[D].哈尔滨：哈尔滨工程大学，2013.]给出了一种恒星敏感器辅助惯性的传统天文导航方法，该方法利用星图识别技术进行定姿定向，校正惯性误差，但难以提供有效的位置修正，且数据更新率较低。

发明内容

本发明所要解决的技术问题在于克服现有技术不足，提供一种海上运载体的组合导航方法及装置，能够在摆脱对卫星定位技术的依赖的同时，有效提高导航精度。

本发明具体采用以下技术方案解决上述技术问题：

一种海上运载体的组合导航方法，所述海上运载体上装备有惯性导航主系统、水声定位子系统、偏振光导航子系统；首先利用惯性导航主系统进行捷联解算，利用水声定位子系统进行测距计算，利用偏振光导航子系统进行方位计算；然后建立以惯性导航主系统的误差为状态量、水声定位子系统的测距信息和偏振光导航子系统的航向信息为观测量的扩展卡尔曼滤波模型，对惯性导航主系统的误差进行闭环修正。

优选地，所述水声定位子系统为长基线水声定位子系统或者伪长基线水声定位子系统。

优选地，作为扩展卡尔曼滤波模型的状态量的惯性导航子系统的误差为15阶误差状态，如下式所示：

其中，φ_E、φ_N、φ_U分别为东向、北向和天向的数学平台失准角，δv_E、δv_N、δv_U分别为东向、北向和天向的速度误差，δλ、δL、δh分别为经度、纬度和高度误差，ε_x、ε_y、ε_z分别三轴陀螺的误差，▽_x、▽_y、▽_z分别三轴加速度计的误差。

优选地，所述扩展卡尔曼滤波模型的观测方程如下：

其中，(X,Y,Z)为利用惯性导航主系统定位得到的海上运载体的坐标；(X_ri,Y_ri,Z_ri)为三个位置已知的固定基站的坐标；为海上运载体中心与声纳接收机之间的杆臂效应；表示载体系与导航系之间的转换矩阵；为利用惯性导航主系统所估计的位置信息计算出的水声定位子系统中的接收机与所述三个位置已知的固定基站之间的距离；R_1m、R_2m、R_3m为利用水声定位子系统测量得到的水声定位子系统中的接收机与所述三个固定基站之间的距离；为利用惯性导航主系统解算得到的海上运载体的航向角；γ_m为利用偏振光导航子系统计算得到的海上运载体的航向角。

根据相同的发明思路还可以得到以下技术方案：

一种海上运载体的组合导航装置，所述组合导航装置包括惯性导航主系统、水声定位子系统、偏振光导航子系统，以及数据融合单元，所述数据融合单元用于根据惯性导航主系统的捷联解算结果、水声定位子系统的测距计算结果、偏振光导航子系统的方位计算结果，建立以惯性导航主系统的误差为状态量、水声定位子系统的测距信息和偏振光导航子系统的航向信息为观测量的扩展卡尔曼滤波模型，对惯性导航主系统的误差进行闭环修正。

优选地，所述水声定位子系统为长基线水声定位子系统或者伪长基线水声定位子系统。

优选地，作为扩展卡尔曼滤波模型的状态量的惯性导航子系统的误差为15阶误差状态，如下式所示：

其中，φ_E、φ_N、φ_U分别为东向、北向和天向的数学平台失准角，δv_E、δv_N、δv_U分别为东向、北向和天向的速度误差，δλ、δL、δh分别为经度、纬度和高度误差，ε_x、ε_y、ε_z分别三轴陀螺的误差，▽_x、▽_y、▽_z分别三轴加速度计的误差。

优选地，所述扩展卡尔曼滤波模型的观测方程如下：

其中，(X,Y,Z)为利用惯性导航主系统定位得到的海上运载体的坐标；(X_ri,Y_ri,Z_ri)为三个位置已知的固定基站的坐标；为海上运载体中心与声纳接收机之间的杆臂效应；表示载体系与导航系之间的转换矩阵；为利用惯性导航主系统所估计的位置信息计算出的水声定位子系统中的接收机与所述三个位置已知的固定基站之间的距离；R_1m、R_2m、R_3m为利用水声定位子系统测量得到的水声定位子系统中的接收机与所述三个固定基站之间的距离；为利用惯性导航主系统解算得到的海上运载体的航向角；γ_m为利用偏振光导航子系统计算得到的海上运载体的航向角。

相比现有技术，本发明具有以下有益效果：

本发明针对海上运载体提出了惯性/APS/偏振光组合的自主导航方式，构建了以惯导误差为状态量，声学距离、偏振航向为观测量的扩展卡尔曼滤波模型，来对惯导误差进行闭环修正，一方面有效提高了导航精度，另一方面由于摆脱了对卫星定位技术的依赖，因此具有更好的隐蔽性和抗干扰性。

附图说明

图1为具体实施方式中水声定位子系统的测量原理示意图；

图2为天空偏振光分布模型；

图3为具体实施方式中偏振光导航子系统的航向角解算过程示意图；

图4为验证试验中的载体航路图；

图5为验证试验中的组合导航位置误差；

图6为验证试验中的组合导航航向角误差。

具体实施方式

针对现有组合导航技术的不足，本发明提出了一种惯性/APS/偏振光组合的导航方法，利用惯性导航主系统进行捷联解算，利用水声定位子系统进行测距计算，利用偏振光导航子系统进行方位计算；然后建立以惯性导航主系统的误差为状态量、水声定位子系统的测距信息和偏振光导航子系统的航向信息为观测量的扩展卡尔曼滤波模型，对惯性导航主系统的误差进行闭环修正。本发明一方面可有效提高导航精度，另一方面由于摆脱了对卫星定位技术的依赖，因此具有更好的隐蔽性和抗干扰性。

为了便于公众理解，下面以一个具体实施例并结合附图来对本发明的技术方案进行详细说明：

首先利用海上运载体所装备的惯性导航主系统、水声定位子系统、偏振光导航子系统(为简便起见，下文分别简称为惯导、声纳、偏振)分别进行导航计算。

其中的水声定位子系统可包括现有的各种声学基线，如长基线(LBL，Long BaseLine)、超短基线(USBL，Ultra Short Base Line)等，本发明优选采用长基线水声定位子系统或者伪长基线水声定位子系统。长基线系统包含两部分，一部分是安装在载体上的接收机，另一部分是位置已知的固定在海底的基站。基站之间的距离构成基线，长度在上百米到几千米之间。长基线系统是通过测量接收机和基站之间的距离，采用测量的交会对目标定位。伪长基线定位系统即PLBL(Pseudo Long Base Line)，它的声纳基站具备伪随机码信号(Pseudo Random Code)发射能力，测距精度和抗干扰能力进一步增强。本实施例中采用PLBL系统，图1即显示了三个基站的PLBL测量原理示意图，三个基站的位置已知，并能通过伪随机码发送给声纳接收机。

设三个基站坐标分别为(X_r1,Y_r1,Z_r1)、(X_r2,Y_r2,Z_r2)、(X_r3,Y_r3,Z_r3)，载体中心坐标为(X,Y,Z)，载体中心与声纳接收机之间的杆臂效应为设测量距离为R₁、R₂和R₃，则有：

式(1)中表示载体系与导航系之间的转换矩阵。

上述距离测量值利用解析矢量可表示为：

式(2)中

则可以通过惯导估计位置和已知基站位置计算距离从而与声学测量的距离R_i作差构成观测量。

偏振光导航起源于对蜜蜂、沙蚁等昆虫捷径返巢能力的探索，是一种以太阳方向矢量为基准的定向方式，具有抗干扰及误差不随时间累积等优点。大气散射辐射具有偏振特性，蜜蜂、沙蚁等昆虫便是利用自身复眼敏感偏振信息来获得方位。在晴朗无云的条件下，大气对于太阳光的散射主要是瑞利散射，散射出射的光线主要是线偏振光，以偏振度和偏振方位角来描述。天空偏振光分布模型如图2所示，其中，O表示地面上观测点，S表示太阳，箭头指向表示偏振方向。天空偏振模式具有两条对称线：一条是与太阳角距为90°的最大偏振线；另一条是过太阳和天顶的太阳子午线。本具体实施例中通过偏振光导航传感器敏感大气偏振特性，获取航向参数的具体过程如图3所示，具体如下：

首先根据偏振导航模型获取载体长轴与太阳子午线的夹角ψ；然后参考惯性导航信息消除偏振方位的180°方向模糊性问题；接着计算出太阳方向矢量，即高度角hs和方位角As，在天球坐标系中，拟通过以下两式求解：

其中，δ为太阳赤纬，为地面上观测点O的纬度，t为太阳时角。δ和t可以根据天文历换算而得。

则航向角由下式确定：

γ＝ψ+A_S (5)

建立以惯性导航主系统的误差为状态量、水声定位子系统的测距信息和偏振光导航子系统的航向信息为观测量的扩展卡尔曼滤波模型，对惯性导航主系统的误差进行闭环修正。

该组合导航系统的状态方程如下：

状态变量选为15阶惯性误差状态，如式(7)所示。

其中，φ_E、φ_N、φ_U分别为东向、北向和天向的数学平台失准角，δv_E、δv_N、δv_U分别为东向、北向和天向的速度误差，δλ、δL、δh分别为经度、纬度和高度误差，ε_x、ε_y、ε_z分别三轴陀螺的误差，▽_x、▽_y、▽_z分别三轴加速度计的误差。

F(t)为系统的状态矩阵，可由惯导的动力学方程确定，W(t)为系统的噪声矩阵，包括三轴陀螺仪和三轴加速度计的白噪声。

设该组合导航系统的观测方程如下：

Y(t)＝H(t)X(t)+V(t) (8)

其中，Y(t)为系统的观测向量，H(t)为观测矩阵，V(t)为观测白噪声向量。

由惯导估计位置和已知基站位置计算的距离和可表示为：

其中，R_i为距离的真值，δR_i为陀螺仪和加速度计的漂移引起的距离误差。

PLBL测量的距离R_1m、R_2m和R_3m可表示为：

R_im＝R_i+v_i (10)

其中，R_i仍为距离的真值，v_i为量测白噪声，为了简化分析，本实施例中不考虑水声传播的时延误差和多路径效应。

惯导计算的航向角可表示为：

其中，γ为航向角的真值，δγ为惯导计算的航向角误差。

偏振光测量的航向角γ_m可表示为：

γ_m＝γ+v_γ (12)

其中，γ仍为航向角的真值，v_γ为偏振测角的量测白噪声。

由式(9)—(12)可得系统的观测方程如下：

根据以上构建的扩展卡尔曼滤波模型，即可对惯性导航主系统的误差进行闭环修正。

为了验证本发明的效果，构建了半实物仿真系统来进行仿真验证，各仿真计算机之间通过串口进行通信并传输信息。仿真过程说明如下：航迹发生器产生载体导航参数真值，惯导仿真计算机根据航迹真值生成IMU真值，与实际IMU的噪声合成IMU输出值，然后进行捷联解算得到导航参数计算值；PLBL仿真计算机根据航迹真值以及布设的基站位置生成声学距离测量值；偏振光仿真计算机根据航迹真值以及天文历生成航向角测量值；三者通过组合导航计算机作为数据融合单元，进行扩展卡尔曼滤波计算，得到状态估计值并进行误差校正，更新导航参数。

假定载体的初始导航时间为2016年5月1日上午(北京时间)8时30分，初始位置为北纬30.7°、东经124.2°，初始姿态水平。载体的航迹设置为“割草机”式航路，如图4所示，航行速度为4节，仿真时间共4400s。传感器相关参数如下：陀螺漂移为1(°)/h，加速度计零偏为10^-4g；声纳测距精度为1m；偏振光传感器测角精度为0.2°。

图5、图6分别为试验中所得到的组合导航位置误差、航向角误差。由图5可知，通过声纳提供的测距信息与惯性的组合，有效的校正了惯导的定位误差，位置输出不再发散，整个仿真时间段位置精度均在4m以内；由图6可知，通过偏振光提供的航向角观测量，能准确估计出惯导的方位失准角，航向角精度始终在0.15以内°。

Claims (8)

1.一种海上运载体的组合导航方法，所述海上运载体上装备有惯性导航主系统、水声定位子系统、偏振光导航子系统；首先利用惯性导航主系统进行捷联解算，利用水声定位子系统进行测距计算，利用偏振光导航子系统进行方位计算；然后建立以惯性导航主系统的误差为状态量、水声定位子系统的测距信息和偏振光导航子系统的航向信息为观测量的扩展卡尔曼滤波模型，对惯性导航主系统的误差进行闭环修正。

2.如权利要求1所述组合导航方法，其特征在于，所述水声定位子系统为长基线水声定位子系统或者伪长基线水声定位子系统。

3.如权利要求1所述组合导航方法，其特征在于，作为扩展卡尔曼滤波模型的状态量的惯性导航主系统的误差为15阶误差状态，如下式所示：

其中，φ_E、φ_N、φ_U分别为东向、北向和天向的数学平台失准角，δv_E、δv_N、δv_U分别为东向、北向和天向的速度误差，δλ、δL、δh分别为经度、纬度和高度误差，ε_x、ε_y、ε_z分别为三轴陀螺的误差，▽_x、▽_y、▽_z分别为三轴加速度计的误差。

4.如权利要求1所述组合导航方法，其特征在于，所述扩展卡尔曼滤波模型的观测方程如下：

$Y=\left[\begin{array}{c}{\hat{R}}_1-R_{1m}\\ {\hat{R}}_2-R_{2m}\\ {\hat{R}}_3-R_{3m}\\ \widehat{\mathrm{\γ}}-{\mathrm{\γ}}_m\end{array}\right]=\left[\begin{array}{cccccccc}0& 0& 0& 0_{1\×3}& \frac{X_1}{R_1}& \frac{Y_1}{R_1}& \frac{Z_1}{R_1}& 0_{1\×6}\\ 0& 0& 0& 0_{1\×3}& \frac{X_2}{R_2}& \frac{Y_2}{R_2}& \frac{Z_2}{R_2}& 0_{1\×6}\\ 0& 0& 0& 0_{1\×3}& \frac{X_3}{R_3}& \frac{Y_3}{R_3}& \frac{Z_3}{R_3}& 0_{1\×6}\\ 0& 0& 1& 0_{1\×3}& 0& 0& 0& 0_{1\×6}\end{array}\right]$

其中，(X,Y,Z)为利用惯性导航主系统定位得到的海上运载体的坐标；(X_ri,Y_ri,Z_ri)为三个位置已知的固定基站的坐标；为海上运载体中心与声纳接收机之间的杆臂效应；表示载体系与导航系之间的转换矩阵；为利用惯性导航主系统所估计的位置信息计算出的水声定位子系统中的接收机与所述三个位置已知的固定基站之间的距离；R_1m、R_2m、R_3m为利用水声定位子系统测量得到的水声定位子系统中的接收机与所述三个固定基站之间的距离；为利用惯性导航主系统解算得到的海上运载体的航向角；γ_m为利用偏振光导航子系统计算得到的海上运载体的航向角。

5.一种海上运载体的组合导航装置，其特征在于，所述组合导航装置包括惯性导航主系统、水声定位子系统、偏振光导航子系统，以及数据融合单元，所述数据融合单元用于根据惯性导航主系统的捷联解算结果、水声定位子系统的测距计算结果、偏振光导航子系统的方位计算结果，建立以惯性导航主系统的误差为状态量、水声定位子系统的测距信息和偏振光导航子系统的航向信息为观测量的扩展卡尔曼滤波模型，对惯性导航主系统的误差进行闭环修正。

6.如权利要求5所述组合导航装置，其特征在于，所述水声定位子系统为长基线水声

定位子系统或者伪长基线水声定位子系统。

7.如权利要求5所述组合导航装置，其特征在于，作为扩展卡尔曼滤波模型的状态量的惯性导航主系统的误差为15阶误差状态，如下式所示：

其中，φ_E、φ_N、φ_U分别为东向、北向和天向的数学平台失准角，δv_E、δv_N、δv_U分别为东向、北向和天向的速度误差，δλ、δL、δh分别为经度、纬度和高度误差，ε_x、ε_y、ε_z分别为三轴陀螺的误差，▽_x、▽_y、▽_z分别为三轴加速度计的误差。

8.如权利要求5所述组合导航装置，其特征在于，所述扩展卡尔曼滤波模型的观测方程如下：

$Y=\left[\begin{array}{c}{\hat{R}}_1-R_{1m}\\ {\hat{R}}_2-R_{2m}\\ {\hat{R}}_3-R_{3m}\\ \widehat{\mathrm{\γ}}-{\mathrm{\γ}}_m\end{array}\right]=\left[\begin{array}{cccccccc}0& 0& 0& 0_{1\×3}& \frac{X_1}{R_1}& \frac{Y_1}{R_1}& \frac{Z_1}{R_1}& 0_{1\×6}\\ 0& 0& 0& 0_{1\×3}& \frac{X_2}{R_2}& \frac{Y_2}{R_2}& \frac{Z_2}{R_2}& 0_{1\×6}\\ 0& 0& 0& 0_{1\×3}& \frac{X_3}{R_3}& \frac{Y_3}{R_3}& \frac{Z_3}{R_3}& 0_{1\×6}\\ 0& 0& 1& 0_{1\×3}& 0& 0& 0& 0_{1\×6}\end{array}\right]$

其中，(X,Y,Z)为利用惯性导航主系统定位得到的海上运载体的坐标；(X_ri,Y_ri,Z_ri)为三个位置已知的固定基站的坐标；为海上运载体中心与声纳接收机之间的杆臂效应；表示载体系与导航系之间的转换矩阵；为利用惯性导航主系统所估计的位置信息计算出的水声定位子系统中的接收机与所述三个位置已知的固定基站之间的距离；R_1m、R_2m、R_3m为利用水声定位子系统测量得到的水声定位子系统中的接收机与所述三个固定基站之间的距离；为利用惯性导航主系统解算得到的海上运载体的航向角；γ_m为利用偏振光导航子系统计算得到的海上运载体的航向角。