CN110031882B - 一种基于sins/dvl组合导航系统的外量测信息补偿方法 - Google Patents

Abstract

本发明涉及组合导航系统领域，具体涉及一种基于SINS/DVL组合导航系统的外量测信息补偿方法。根据全球定位GPS系统得到载体初始时刻位置信息，导入导航计算机中，进行光纤陀螺捷联惯导SINS系统预热，根据光纤陀螺仪和石英加速度计得到载体姿态角，进行初始对准并建立初始捷联矩阵，根据惯性组件IMU得到载体的角运动和线运动信息，进行导航解算，得到捷联矩阵

建立卡尔曼滤波模型，修正得到经卡尔曼滤波模型修正后捷联矩阵

根据DVL测速的SINS/DVL量测方程，将步骤四所述的经卡尔曼滤波模型修正后捷联矩阵

补偿到DVL测速。本发明能够在载体航行过程中通过估算载体的姿态，并将其补偿给DVL测速误差,来抑制DVL测速误差，提高DVL测速精度，且方法简单，易操作。

Description

一种基于SINS/DVL组合导航系统的外量测信息补偿方法

技术领域

本发明涉及组合导航系统领域，具体涉及一种基于SINS/DVL组合导航系统的外量测信息补偿方法。

背景技术

船舶航速的测定，特别是对地航速的测定越来越引起人们的重视。

由于现实中对船舶航速测定的要求越来越高，例如大型船舶在进港和靠码头时，要求的测速精度较高，甚至达到0.01节。对于水下航行的潜艇，测速也是导航重要手段之一。特别是水下导弹发射时，潜艇本身的速度将影响导弹的出水姿态。因此船舶航速的测定显得尤为重要。

目前多普勒计程仪是一种常用船用计程仪，它是利用声波或超声波在水中的多普勒效应制成的计程仪，由于它可以直接测量船舶对地的速度，并且稳定可靠，所以近年来受到广泛应用。在船舶导航系统中，多普勒计程仪测速，即DVL测速通常用作辅助捷联罗经系统，DVL测速及其测量速度的差分值，即加速度提供给捷联罗经系统，这样可以去除船舶速度和加速度对于捷联罗经系统导航的影响。但从多普勒计程仪的测速原理中可分析得出，由于水下超声波变化、公式近似、波束宽度、载体颠簸摇摆和洋流误差等的影响，多普勒计程仪所测速度存在误差。

申请号为CN103940416A的专利，一种基于捷联惯导系统的多普勒计程仪测速误差抑制方法，将多普勒计程仪输出速度与捷联惯导系统输出速度进行综合处理。这种方法需要经过高通数字滤波，对输出速度进行优化后，存在一定的时间延迟；申请号为CN106908086A的专利，一种多普勒计程仪测速误差的修正方法，用惯性导航系统INS提供的姿态信息，修正由载体俯仰角变化引起的DVL的测速误差，从而提高INS/DVL组合导航系统精度。但因其使用前后两个不同时刻的载体信息，存在时间延迟，引入了不同时刻坐标系的误差，不能有效的对付噪声的影响。

发明内容

本发明的目的在于提供一种基于SINS/DVL组合导航系统的外量测信息补偿方法，以抑制DVL测速误差，提高DVL测速精度。

本发明实施例提供一种基于SINS/DVL组合导航系统的外量测信息补偿方法，包括：

步骤一：根据全球定位GPS系统得到载体初始时刻的位置信息，将所述载体初始时刻的位置信息进行装订并导入导航计算机中，同时进行光纤陀螺捷联惯导SINS系统预热工作；

步骤二：根据光纤陀螺仪和石英加速度计采集到的输出数据得到载体姿态角，通过所述的载体姿态角以及步骤一所述的载体初始时刻的位置在导航计算机进行光纤陀螺捷联惯导SINS系统初始对准并建立初始捷联矩阵；

步骤三：根据惯性组件IMU得到载体的角运动和线运动信息，将所述的载体的角运动和线运动信息进行导航解算，更新步骤二所述的初始捷联矩阵得到捷联矩阵

步骤四：根据建立的卡尔曼滤波模型，修正步骤三所述的捷联矩阵

得到经卡尔曼滤波模型修正后捷联矩阵

步骤五：根据DVL测速的SINS/DVL量测方程，将步骤四所述的经卡尔曼滤波模型修正后捷联矩阵

补偿到DVL测速，得到最终DVL测速值；

所述步骤一，包括：

根据全球定位GPS系统得到载体初始时刻的位置信息，将所述载体初始时刻的位置信息进行装订并导入导航计算机中，同时进行光纤陀螺捷联惯导SINS系统预热工作；

a)其中，所述的载体初始时刻的位置信息包括：载体初始时刻所在位置的纬度和载体初始时刻所在位置的经度；

所述步骤三，包括：

根据惯性组件IMU得到载体的角运动和线运动信息，将所述的载体的角运动和线运动信息进行导航解算，更新步骤二所述的初始捷联矩阵得到捷联矩阵

a)其中，所述的捷联矩阵

的参数设置为：b表示载体坐标系，c表示光纤陀螺捷联惯导SINS系统解算得到的地理坐标系；

所述步骤四，包括：

根据建立的卡尔曼滤波模型，修正步骤三所述的捷联矩阵

得到经卡尔曼滤波模型修正后捷联矩阵

a)其中，所述的卡尔曼滤波模型的系统状态方程为：

其参数状态量为：X(t)＝[δL δλ δv_E δv_N φ_x φ_y φ_z ΔA_x ΔA_y ε_E ε_N ε_U θ_CE θ_CN]，

式中参数设置为：δL，δλ分别表示纬度L的变化量和经度λ的变化量，δv_E，δv_N分别表示光纤陀螺捷联惯导SINS系统东向速度v_E的变化量和北向速度v_N的变化量，φ_x，φ_y，φ_z分别表示光纤陀螺捷联惯导SINS系统东向失准角、北向失准角和方位失准角，ε_E，ε_N，ε_U为东向陀螺漂移、北向陀螺漂移和天向陀螺漂移，ΔA_x，ΔA_y分别为东向加速度计零偏和北向加速度计零偏；

b)其中，所述的卡尔曼滤波模型的系统噪声为：

W(t)＝[0 0 0 0 0 0 0 ω_dE ω_dN ω_gE ω_gN ω_gU ω_E ω_N]^T

所述的卡尔曼滤波模型的状态矩阵为：

式中参数设置为：

式中参数设置为：ω_dE、ω_dN分别是加速度计东向噪声和加速度计北向噪声，ω_gE、ω_gN、ω_gU分别是陀螺东向噪声、陀螺北向噪声和陀螺天向噪声，ω_E、ω_N是洋流中东向白噪声和洋流中北向白噪声，L是纬度，λ是经度，ω_ie是地球自转角速率，R是地球半径，h是高度，海平面以上为正，海平面以下为负，v_E是光纤陀螺捷联惯导SINS系统东向速度，v_N是光纤陀螺捷联惯导SINS系统北向速度，f是加速度计测得比例，β_E、β_N分别是东向相关时间常数和北向相关时间常数，

为所述的经卡尔曼滤波模型修正后捷联矩阵，b表示载体坐标系，n表示导航坐标系；

c)其中，所述的经卡尔曼滤波模型修正后捷联矩阵

表示为：

所述步骤五，包括：

根据DVL测速的SINS/DVL量测方程，将步骤四所述的经卡尔曼滤波模型修正后捷联矩阵

补偿到DVL测速，得到最终DVL测速值；

a)其中，所述的DVL测速的SINS/DVL量测方程为：

Z＝HX+V，

式中参数设置为：θ_CE，θ_CN分别为洋流的东向速度和洋流的北向速度，v_DVL是DVL的测速，V为2×1速度量测噪声向量，

为所述的经卡尔曼滤波模型修正后捷联矩阵

的第三行第二列元素，

为修正后的纵摇角，H为观测矩阵

本发明的有益效果通过Matlab仿真试验得以验证：

1.Matlab仿真条件，仿真环境参数设定如下：

地球半径：R＝6378393m；

重力加速度：g＝9.78049m/s²；

地球自转角速度：ω_ie＝7.2921158e-5rad/s；

当地(哈尔滨地区)经纬度：经度λ＝126.6705°，纬度L＝45.7796°；

采样时间Hn＝1s；

三个轴常值陀螺漂移是：0.01°/h；

三个轴加速度计的常值零偏是：1×10^-4g；

陀螺的随机漂移是：

三个轴陀螺刻度因子误差是：δK_gx＝δK_gy＝δK_gz＝20ppm；

三个轴加速度计刻度因子误差是：δK_ax＝δK_ay＝δK_az＝20ppm；

陀螺和加速计安装误差角均是：10^-5rad；

初始失准角设置为随机值；

2.载体摇摆运动状态设定如下为：

设定载体作三轴摇摆运动，其运动模型是：

其中摇摆的幅值分别设置为：yaw_m＝2°、pitch_m＝3°、roll_m＝2°；

为摇摆角频率，摇摆周期分别设置为：T_h＝10s，T_p＝8s，T_r＝6s；摇摆初始相位φ_h、φ_p、φ_r设置为随机值，姿态角初始值：yaw_k、roll_k、pitch_k也设置为随机值；船体航行的东向速度为2m/s，北向速度为2m/s，仿真时间设置为1小时；

3.Matlab仿真试验结果：

如图4、图5和图6所示，补偿纵摇后，北向失准角、方位失准角误差明显减小，东向失准角误差抑制不明显，故选取仿真时间的前2分钟的北向失准角误差进行比较，如图7。在前半段补偿船体纵摇时产生的测速误差后，不但可以改进东向、北向、方位失准角误差，还可以消除由船体纵摇所造成的振荡性，验证了此发明的可行性。

附图说明

图1为一种基于SINS/DVL组合导航系统的外量测信息补偿方法的流程图；

图2为船体纵摇示意图；

图3为姿态补偿抑制测速误差原理图；

图4为本发明Matlab仿真试验东向失准角误差图(仿真时间1h)；

图5为本发明Matlab仿真试验北向失准角误差图(仿真时间1h)；

图6为本发明Matlab仿真试验方向失准角误差图(仿真时间1h)；

图7为本发明Matlab仿真试验北向失准角误差图(仿真时间2min)；

具体实施方式

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图对本发明做进一步描述：

图1为一种基于SINS/DVL组合导航系统的外量测信息补偿方法的流程图；

图2为船体纵摇示意图；

图3为姿态补偿抑制测速误差原理图；

图4为本发明Matlab仿真试验东向失准角误差图(仿真时间1h)；

图5为本发明Matlab仿真试验北向失准角误差图(仿真时间1h)；

图6为本发明Matlab仿真试验方向失准角误差图(仿真时间1h)；

图7为本发明Matlab仿真试验北向失准角误差图(仿真时间2min)；

图3中，α为安装角，γ为纵摇角。

本发明的技术方案是这样实现的：

步骤1、利用全球定位GPS系统提供载体初始时刻的位置信息，并装订至导航计算机中；所述的位置信息包括:载体初始时刻所在位置的纬度和经度；

步骤2、光纤陀螺捷联惯导系统进行预热后，采集光纤陀螺仪和石英加速度计输出的数据，然后确定载体姿态角，完成系统预热准备和初始对准；

步骤3、利用惯性组件测量载体的角运动和线运动信息，进行导航解算，更新捷联矩阵

其中，b表示载体坐标系，c表示SINS解算的地理坐标系；

步骤4、建立卡尔曼滤波模型，对捷联矩阵

修正；

系统状态方程为：

其中，状态量：

X(t)＝[δL δλ δv_E δv_N φ_x φ_y φ_z ΔA_x ΔA_y ε_E ε_N ε_U θ_CE θ_CN]，δL，δλ分别表示纬度L，经度λ的变化，δv_E，δv_N分别表示捷联惯导系统东向速度v_E，北向速度v_N的变化，φ_x，φ_y，φ_z分别表示捷联惯导系统东向、北向、方位失准角，ε_E，ε_N，ε_U为东、北、天向陀螺漂移，ΔA_x，ΔA_y分别为东、北向加速度计零偏，θ_CE，θ_CN分别为洋流的东、北向速度。

系统噪声W(t)＝[0 0 0 0 0 0 0 ω_dE ω_dN ω_gE ω_gN ω_gU ω_E ω_N]^T

状态矩阵

其中

其中ω_dE、ω_dN分别是加速度计东、北向噪声，ω_gE、ω_gN、ω_gU分别是陀螺东北天向噪声，ω_E、ω_N是洋流中东、北向白噪声，L是纬度，λ是经度，ω_ie是地球自转角速率，R是地球半径，h是高度，海面为正，海面以下为负，v_E是捷联惯导系统东向速度,v_N是捷联惯导系统北向速度，f是加速度计测得比力，β_E、β_N是东向和北向的相关时间常数，

为经过卡尔曼滤波校正后捷联矩阵，b表示载体坐标系，n表示导航坐标系，

步骤5、利用步骤4校正后的

补偿到DVL测速进行补偿。其所述的SINS/DVL量测方程为：

Z＝HX+V

其中v_DVL是DVL的测量速度，V为2×1速度量测噪声向量，

为步骤4校正后

的第三行第二列元素。观测矩阵：

Claims (4)

1.一种基于SINS/DVL组合导航系统的外量测信息补偿方法，其特征在于，包括：

步骤一：根据全球定位GPS系统得到载体初始时刻的位置信息，将所述载体初始时刻的位置信息进行装订并导入导航计算机中，同时进行光纤陀螺捷联惯导SINS系统预热工作；

步骤二：根据光纤陀螺仪和石英加速度计采集到的输出数据得到载体姿态角，通过所述的载体姿态角以及步骤一所述的载体初始时刻的位置在导航计算机进行光纤陀螺捷联惯导SINS系统初始对准并建立初始捷联矩阵；

步骤三：根据惯性组件IMU得到载体的角运动和线运动信息，将所述的载体的角运动和线运动信息进行导航解算，更新步骤二所述的初始捷联矩阵得到捷联矩阵

步骤四：根据建立的卡尔曼滤波模型，修正步骤三所述的捷联矩阵

得到经卡尔曼滤波模型修正后捷联矩阵

系统状态方程为：

其中，X(t)＝[δL δλ δv_E δv_N φ_x φ_y φ_z ΔA_x ΔA_y ε_E ε_N ε_U θ_CEθ_CN]；

δL，δλ分别表示纬度L，经度λ的变化；δv_E，δv_N分别表示捷联惯导系统东向速度v_E，北向速度v_N的变化；φ_x，φ_y，φ_z分别表示捷联惯导系统东向、北向、方位失准角；ε_E，ε_N，ε_U为东、北、天向陀螺漂移；ΔA_x，ΔA_y分别为东、北向加速度计零偏；θ_CE，θ_CN分别为洋流的东、北向速度；

系统噪声为：W(t)＝[0 0 0 0 0 0 0 ω_dE ω_dN ω_gE ω_gN ω_gU ω_E ω_N]^T；

状态矩阵为：

其中：

其中，ω_dE、ω_dN分别是加速度计东、北向噪声；ω_gE、ω_gN、ω_gU分别是陀螺东北天向噪声，ω_E、ω_N是洋流中东、北向白噪声；ω_ie是地球自转角速率；R是地球半径；h是高度，海面为正，海面以下为负；f是加速度计测得比力；β_E、β_N是东向和北向的相关时间常数；

为经过卡尔曼滤波校正后捷联矩阵，b表示载体坐标系，n表示导航坐标系，

步骤五：根据DVL测速的SINS/DVL量测方程，将步骤四所述的经卡尔曼滤波模型修正后捷联矩阵

补偿到DVL测速，得到最终DVL测速值。

2.根据权利要求1所述的一种基于SINS/DVL组合导航系统的外量测信息补偿方法，其特征在于，所述步骤一，包括：

根据全球定位GPS系统得到载体初始时刻的位置信息，将所述载体初始时刻的位置信息进行装订并导入导航计算机中，同时进行光纤陀螺捷联惯导SINS系统预热工作；

a)其中，所述的载体初始时刻的位置信息包括：载体初始时刻所在位置的纬度和载体初始时刻所在位置的经度。

3.根据权利要求1所述的一种基于SINS/DVL组合导航系统的外量测信息补偿方法，其特征在于：所述步骤三，包括：

根据惯性组件IMU得到载体的角运动和线运动信息，将所述的载体的角运动和线运动信息进行导航解算，更新步骤二所述的初始捷联矩阵得到捷联矩阵

a)其中，所述的捷联矩阵

的参数设置为：b表示载体坐标系，c表示光纤陀螺捷联惯导SINS系统解算得到的地理坐标系。

4.根据权利要求1所述的一种基于SINS/DVL组合导航系统的外量测信息补偿方法，其特征在于：所述步骤五，包括：

根据DVL测速的SINS/DVL量测方程，将步骤四所述的经卡尔曼滤波模型修正后捷联矩阵

补偿到DVL测速，得到最终DVL测速值；

a)其中，所述的DVL测速的SINS/DVL量测方程为：

Z＝HX+V，

式中参数设置为：θ_CE，θ_CN分别为洋流的东向速度和洋流的北向速度，v_DVL是DVL的测速，V为2×1速度量测噪声向量，

为所述的经卡尔曼滤波模型修正后捷联矩阵

的第三行第二列元素，

γ为修正后的纵摇角，H为观测矩阵

Images