CN103245793A - 一种组合导航洋流速度测量方法 - Google Patents

Abstract

本发明涉及的是一种信息测量方法，具体涉及一种船舶水面航行时水面组合导航系统利用卡尔曼滤波测量洋流方法。本发明包括：采集船舶捷联惯性导航系统中陀螺和加速度计的采样值，递推测量水面运载器速度值与位置值；查找船舶航行海域内洋流模型东向参数和洋流模型的北向参数；设置水面组合导航方法状态14维变量；获取水面组合导航方法观测量与观测矩阵；进行卡尔曼滤波，测量出东向洋流速度和北向洋流速度。本发明的卡尔曼滤波方法可以更快速而准确的测量出洋流速度，测量结果无滞后，满足快速性，测量的误差为10-3m，远小于洋流速度，精度更高。

Description

一种组合导航洋流速度测量方法

技术领域

本发明涉及的是一种信息测量方法，具体涉及一种船舶水面航行时水面组合导航系统利用卡尔曼滤波测量洋流方法。

背景技术

捷联惯导系统SINS具有独立自主工作的优点，但其导航误差随时间积累；GPS可以提供精确的位置信息，但其自主性能差且易受干扰；多普勒计程仪DVL可提供船舶的速度信息，但其作用距离有限，受限于DVL自身性能，船舶体积，功耗等方面的限制，大多数情况下，DVL只能提供对水速度。上述单一导航系统由于自身存在的不足,已无法满足船舶导航的要求。因此，通常将上述三种系统组合起来使用,便可取长补短，提高导航精度。本方法水面组合导航模式，通过SINS/DVL/GPS的组合，构造状态方程和观测方程，利用卡尔曼滤波可以快速而准确的测量出洋流速度大小，解决了DVL观测距离的局限性，具有实际意义。

船舶水面航行时，针对DVL量程不够，将DVL设置为对水速度，为了解决DVL量程不够带来的误差，我们需要对洋流速度进行测量。2007年发表于“鱼雷技术”，名称为航速惯导组合导航系统在AUV上的应用；2004年发表于“中国航海”上的文章“AUV中SINS/DVL组合导航技术研究”，上述成果与已有的专利和文章均以对水速度作为船舶组合滤波器的观测量，去估测SINS的误差，会由于洋流速度的存在使得滤波发散，测量失败（滤波器需要地速作为观测量，而对水速度等于地速减去洋流速度）。本文设计了辅助的组合导航方法，利用洋流模型测量出洋流速度，结合DVL对水速度获得地速值，输入滤波器，完成滤波测量任务。

发明内容

本发明的目的在于提供一种更快速、准确测量出洋流速度的组合导航洋流速度测量方法。

本发明的目的是这样实现的：

本发明包括如下步骤：

（1）采集船舶捷联惯性导航系统中陀螺和加速度计的采样值，递推测量水面运载器速度值与位置值，测量速度值为v＝[v_E v_N v_U]^T，测量位置值为纬度L、经度λ；

（2）在电子海图或洋流数据库中，查找船舶航行海域内洋流模型东向参数β_x和洋流模型的北向参数β_y；

（3）设置水面组合导航方法状态14维变量：

$X={\left[\begin{array}{cccccccccccccc}\mathrm{\δL}& \mathrm{\δ\λ}& {\mathrm{\δv}}_E& \mathrm{\δ}{v}_N& {\mathrm{\φ}}_E& {\mathrm{\φ}}_N& {\mathrm{\φ}}_U& {\▿}_E& {\▿}_N& {\mathrm{\ϵ}}_E& {\mathrm{\ϵ}}_N& {\mathrm{\ϵ}}_U& V_{\mathrm{CE}}& V_{\mathrm{CN}}\end{array}\right]}^T$ 其中，δL、δλ分别为捷联惯性导航系统经度、纬度误差，δv_E、δv_N分别为捷联惯性导航系统东、北向速度误差，φ_E、φ_N、φ_U为东向、北向、天向平台失准角，分别为东、北向加速度计零偏，ε_E、ε_N、ε_U为东、北、天向陀螺漂移，V_CE、V_CN分别为洋流的东、北向速度；

（4）获取水面组合导航方法观测量与观测矩阵：

$Z=\left[\begin{array}{c}L-L_{\mathrm{GPS}}\\ \mathrm{\λ}-{\mathrm{\λ}}_{\mathrm{GPS}}\\ v_E-V_{\mathrm{DE}}\\ v_N-V_{\mathrm{DN}}\end{array}\right]=\mathrm{HX}+M,$ $H=\left[\begin{array}{cccccccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 1\end{array}\right],$

其中，L_GPS是GPS提供的纬度值，λ_GPS是GPS提供的经度值，V_DE是多普勒计程仪的东向速度，V_DN是多普勒计程仪的北向速度，M为4×1维的量测噪声；

（5）进行卡尔曼滤波，测量出东向洋流速度V_CE和北向洋流速度V_CN。

本发明的有益效果在于：

本发明的卡尔曼滤波方法可以更快速而准确的测量出洋流速度，测量结果无滞后，满足快速性，测量的误差为10-3m，远小于洋流速度，精度更高。

附图说明

图1.基于DVL对水工作模式下SINS/GPS/DVL洋流测量方法的流程图；

图2.采用本方法测量的洋流与设置的洋流速度作差仿真图。

具体实施方式

下面结合附图对本发明做进一步描述：

DVL对水工作模式下的SINS/GPS/DVL组合导航测量洋流速度方法，利用船舶装备的捷联惯性导航系统SINS，多普勒计程仪DVL，结合洋流速度模型，工作于组合导航状态，实时测量洋流速度大小。

试验条件SINS的误差设置为：陀螺常值漂移ε_x＝ε_y＝ε_z＝0.01°h，随机漂移为0.001°h，刻度系数误差为10^-4；加速度计的随机常值偏置为

随机漂移为10^-5g。船舶工作于匀速直航状态，航速10kn，航向135°。洋流速度V_Cx＝1kn,V_Cy＝1kn，洋流模型的参数β_x＝5.4358×10^-5 β_x＝5.3291×10^-5；

船舶装备的SINS和DVL、GPS通过数据传输电缆相联通，确保它们的信息传输通畅；

步骤1、采集船舶SINS中陀螺和加速度计的采样值，递推测量水面运载器速度值与位置值。

所述的船舶SINS测量速度值为v＝[v_E v_N v_U]^T，

所述的船舶SINS测量位置值为纬度L、经度λ；

步骤2、由电子海图或洋流数据库中，查找船舶航行海域内，洋流模型东向参数β_x和洋流模型的北向参数β_y；

步骤3、设置水面组合导航方案状态变量为14维：

$X={\left[\begin{array}{cccccccccccccc}\mathrm{\δL}& \mathrm{\δ\λ}& {\mathrm{\δv}}_E& \mathrm{\δ}{v}_N& {\mathrm{\φ}}_E& {\mathrm{\φ}}_N& {\mathrm{\φ}}_U& {\▿}_E& {\▿}_N& {\mathrm{\ϵ}}_E& {\mathrm{\ϵ}}_N& {\mathrm{\ϵ}}_U& V_{\mathrm{CE}}& V_{\mathrm{CN}}\end{array}\right]}^T$ 其中，δL、δλ分别为SINS经度、纬度误差，δv_E、δv_N分别为SINS系统东、北向速度误差，φ_E、φ_N、φ_U为东、北、天向平台失准角，

分别为东、北向加速度计零偏，ε_E、ε_N、ε_U为东、北、天向陀螺漂移，V_CE、V_CN分别为洋流的东、北向速度。

步骤4、水面组合导航方法观测量与观测矩阵分别为

$Z=\left[\begin{array}{c}{L}_{\mathrm{SINS}}-L_{\mathrm{GPS}}\\ {\mathrm{\λ}}_{\mathrm{SINS}}-{\mathrm{\λ}}_{\mathrm{GPS}}\\ v_E-V_{\mathrm{DE}}\\ v_N-V_{\mathrm{DN}}\end{array}\right]=\mathrm{HX}+M,$ $H=\left[\begin{array}{cccccccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 1\end{array}\right]$

其中，L_GPS是GPS提供的纬度值，λ_GPS是GPS提供的经度值，V_DE是DVL的东向速度，V_DN是DVL的北向速度，M为4×1维的量测噪声。

步骤5、通过卡尔曼滤波，可以测量出东向洋流速度V_CE和北向洋流速度V_CN。

本专利还包括如下的技术特征：步骤3和4中，卡尔曼滤波器用到的系统状态方程和量测方程如下：

$\left\{\begin{array}{c}\stackrel{\·}{X}=\mathrm{FX}+\mathrm{GW}\\ Z=\mathrm{HX}+M\end{array}\right.$

其中，X表示14维状态序列，F是14×14维的一步转移矩阵，G为14×14维单位阵，H为4×14维量测阵，W为系统噪声，为14×1维的白噪声序列，M为量测噪声，为4×1维白噪声序列。

式中，一步转移矩阵

$F=\left[\begin{array}{cccccc}{F}_1& F_2& 0_{2\×3}& 0_{2\×2}& 0_{2\×3}& 0_{2\×2}\\ F_3& F_4& F_5& C_{b2\×2}^n& 0_{2\×3}& 0_{2\×2}\\ F_6& F_7& F_8& 0_{3\×2}& C_b^n& 0_{3\×2}\\ 0_{5\×2}& 0_{5\×2}& 0_{5\×3}& 0_{5\×2}& 0_{5\×3}& 0_{5\×2}\\ 0_{2\×2}& 0_{2\×2}& 0_{2\×3}& 0_{2\×2}& 0_{2\×3}& F_9\end{array}\right]$

其中

$F_1=\left[\begin{array}{cc}0& 0\\ \frac{v_E\tan L\sec L}{R}& 0\end{array}\right],$ $F_2=\left[\begin{array}{cc}0& \frac{1}{R}\\ \frac{\sec L}{R}& 0\end{array}\right],$ $F_3=\left[\begin{array}{cc}2\mathrm{\Ω}\cos {\mathrm{Lv}}_N+\frac{v_E{v}_N}{R}{\sec }^{2}L& 0\\ -(2\mathrm{\Ω}\cos {\mathrm{Lv}}_E+\frac{{v_E}^2}{R}{\sec }^{2}L)& 0\end{array}\right]$

$F_4=\left[\begin{array}{cc}\frac{v_N}{R}\tan L& 2\mathrm{\Ω}\sin L+\frac{v_E}{R}\tan L\\ -(2\mathrm{\Ω}\sin L+\frac{2v_E}{R}\mathrm{tnaL})& 0\end{array}\right],$ $F_5=\left[\begin{array}{ccc}0& -f_U& f_N\\ f_U& 0& -f_E\end{array}\right]$

$F_6=\left[\begin{array}{cc}0& 0\\ -\mathrm{\Ω}\sin L& 0\\ \mathrm{\Ω}\cos L+\frac{v_E}{R}{\sec }^{2}L& 0\end{array}\right],$ $F_7=\left[\begin{array}{cc}0& -\frac{1}{R}\\ \frac{1}{R}& 0\\ \frac{\tan L}{R}& 0\end{array}\right]$

$F_8=\left[\begin{array}{ccc}0& \mathrm{\Ω}\sin L+\frac{v_E\tan L}{R}& \mathrm{\Ω}\cos L+\frac{v_E}{R}\\ -(\mathrm{\Ω}\sin L+\frac{v_E\tan L}{R})& 0& \frac{v_N}{R}\\ \mathrm{\Ω}\cos L+\frac{v_E}{R}& \frac{-v_N}{R}& 0\end{array}\right],$ $F_9=\left[\begin{array}{cc}-{\mathrm{\β}}_E& 0\\ 0& -{\mathrm{\β}}_N\end{array}\right]$

选取观测量 $Z=\left[\begin{array}{c}{L}_{\mathrm{SINS}}-L_{\mathrm{GPS}}\\ {\mathrm{\λ}}_{\mathrm{SINS}}-{\mathrm{\λ}}_{\mathrm{GPS}}\\ v_E-V_{\mathrm{DE}}\\ v_N-V_{\mathrm{DN}}\end{array}\right]=\mathrm{HX}+M$

则量测阵为

$H=\left[\begin{array}{cccccccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 1\end{array}\right]$

其中，

是东向和北向的加速度计零偏，ε_E、ε_N、ε_U分别是东向、北向和天向的陀螺漂移。地球自转角速率Ω＝7.2921158×10^-5弧度，地球半径R=6378393米。

DVL对水工作模式下的SINS/GPS/DVL组合导航测量洋流速度方法，利用船舶装备的捷联惯性导航系统SINS，多普勒计程仪DVL，结合洋流速度模型，工作于组合导航状态，实时测量洋流速度大小。

对所述方案进行仿真验证，仿真条件设置为：试验条件SINS的误差设置为：陀螺常值漂移ε_x＝ε_y＝ε_z＝0.01°h，随机漂移为0.001°h，刻度系数误差为10^-4；加速度计的随机常值偏置为

随机漂移为10^-5g。船舶工作于匀速直航状态，航速5.144m，航向135°。洋流速度V_Cx＝0.5144m,V_Cy＝0.5144m，洋流模型的参数β_x＝5.4358×10^-5β_x＝5.3291×10^-5；

本方法测量的洋流与设置的洋流速度作差仿真结果如图2所示。可以看到，通过和设置洋流速度比较，本方法设置的卡尔曼滤波方法可以快速而准确的测量出洋流速度，测量结果无滞后，满足快速性，测量的误差为10^-3m，远小于设置洋流速度V_Cx＝0.5144m,V_Cy＝0.5144m，精度较高，因此达到了本方法设计的目的。

Claims (1)

1.一种组合导航洋流速度测量方法，其特征是，包括如下步骤：

（1）采集船舶捷联惯性导航系统中陀螺和加速度计的采样值，递推测量水面运载器速度值与位置值，测量速度值为v=[v_E v_N v_U]^T，测量位置值为纬度L、经度λ；

（2）在电子海图或洋流数据库中，查找船舶航行海域内洋流模型东向参数β_x和洋流模型的北向参数β_y；

（3）设置水面组合导航方案状态14维变量：

$X={\left[\begin{array}{cccccccccccccc}\mathrm{\δL}& \mathrm{\δ\λ}& {\mathrm{\δv}}_E& \mathrm{\δ}{v}_N& {\mathrm{\φ}}_E& {\mathrm{\φ}}_N& {\mathrm{\φ}}_U& {\▿}_E& {\▿}_N& {\mathrm{\ϵ}}_E& {\mathrm{\ϵ}}_N& {\mathrm{\ϵ}}_U& V_{\mathrm{CE}}& V_{\mathrm{CN}}\end{array}\right]}^T$ 其中，δL、δλ分别为捷联惯性导航系统经度、纬度误差，δv_E、δv_N分别为捷联惯性导航系统东、北向速度误差，φ_E、φ_N、φ_U为东向、北向、天向平台失准角，

分别为东、北向加速度计零偏，ε_E、ε_N、ε_U为东、北、天向陀螺漂移，V_CE、V_CN分别为洋流的东、北向速度；

（4）获取水面组合导航方案观测量与观测矩阵：

$Z=\left[\begin{array}{c}L-L_{\mathrm{GPS}}\\ \mathrm{\λ}-{\mathrm{\λ}}_{\mathrm{GPS}}\\ v_E-V_{\mathrm{DE}}\\ v_N-V_{\mathrm{DN}}\end{array}\right]=\mathrm{HX}+M,$ $H=\left[\begin{array}{cccccccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 1\end{array}\right],$

其中，L_GPS是GPS提供的纬度值，λ_GPS是GPS提供的经度值，V_DE是多普勒计程仪的东向速度，V_DN是多普勒计程仪的北向速度，M为4×1维的量测噪声；

（5）进行卡尔曼滤波，测量出东向洋流速度V_CE和北向洋流速度V_CN。

Images