一种基于SINS/LBL紧组合的高程约束水下自适应导航方法及装置

本发明涉及水下多传感器组合导航的技术领域，特别是涉及一种基于sins/lbl紧组合的高程约束水下自适应导航方法及装置。

背景技术：

随着水下导航器技术的不断发展，导航系统成为水下航行器研究的主要技术核心，实现水下精确定位成为目前水下航行器定位导航系统研究的一个重要分支。由于单一的导航自身的存在的不足，因而组合导航技术已成为导航技术发展的重要方向之一，研究sins与水声定位技术的组合导航具有重要意义。

目前的组合导航系统一般都将惯导设备作为主要参考导航系统。sins体积小、功耗低，有着良好的隐蔽性，不受工作介质的影响，且能够提供较为全面的导航信息，在短期内具有较高精度，数据更新频率高。缺点是当其单独长期工作时稳定性很差，位置和速度误差会随着时间积累，从而导致最终定位误差较大，甚至发散。尤其是高程通道，纯惯导系统只要受到扰动，其高度通道就会变得极其不稳定。特别是在水下工作时，由于水下环境复杂多变且不稳定，对导航系统的稳定性带来了很大的挑战。

技术实现要素：

本发明要解决的技术问题是：提供一种基于sins/lbl紧组合的高程约束水下自适应导航方法及装置，提高水下导航定位的准确性。

为了解决上述技术问题，本发明提供了一种基于sins/lbl紧组合的高程约束水下自适应导航方法，包括：

使用sins获取auv位置信息，根据所述auv的位置信息和海底各应答器的位置信息获取基于sins斜距差；其中，所述海底各应答器包括i个应答器和一个参考应答器，i的取值范围为1到3的正整数；

获取通过lbl声源到所述第i个应答器与lbl声源到参考应答器的斜距差，记为基于lbl斜距差；

将所述基于sins斜距差与所述基于lbl斜距差做差运算，获取所述基于sins与所述基于lbl的斜距差之差，记为外部观测值；

将所述外部观测值作为高程约束自适应的滤波计算的输入值，根据k时刻的量测值，通过高程约束自适应kalman滤波方程可得到k时刻的状态估计值；

用所述高程误差估计值对固有的sins输出值的误差进行修正处理，获取修正误差后的sins的导航信息。

进一步的，开始高程约束自适应kalman滤波前，还包括：

从线性离散系统的状态方程和量测方程中获取满足预设条件的系统激励噪音矩阵和量测噪音矩阵，再从满足第一预设条件的系统激励噪音矩阵和量测噪音矩阵中提取相对应的系统噪声方差阵和量测噪音方差阵，若所述系统噪声方差阵和量测噪音方差阵满足第二预设条件，则开始高程约束自适应kalman滤波。

进一步的，使用sins获取auv位置信息，根据所述auv的位置信息和海底各应答器的位置信息获取基于sins斜距差，具体为：

设sins解算的auv位置为(xs，ys，zs)，真实auv位置为(x，y，z)，海底四个应答器位置为(xj，yj，zj)，其中，j＝(0，1，2，3)，设第四个应答器(x0，y0，z0)为参考应答器，sins解算的auv位置与另外三个应答器的斜距分别与第四个斜距作差，得到基于sins的斜距差ρsinsi，其中，i＝(1，2，3)，即：

将上式相对auv真实位置进行taylor线性化得：

ρsinsi＝ri-r0+eixδx+eiyδy+eizδz

其中，ri表示声源与第i个应答器的斜距，i＝(0，1，2，3)；r0表示声源与参考应答器的斜距，有：

进一步的，获取通过lbl声源到所述第i个应答器和lbl声源到参考应答器的斜距差，记为基于lbl斜距差，具体为：

lbl声源到所述第i个应答器的斜距：

其中，i＝(1，2，3)；

lbl声源到参考应答器的斜距：

基于lbl斜距差：

其中，δri表示lbl斜距差的测量值误差，表示lbl斜距差中存在的噪音。

进一步的，将所述基于sins斜距差与所述基于lbl斜距差做差运算，获取所述基于sins与所述基于lbl的斜距差之差，记为外部观测值，具体为：

将所述外部观测值δρi记为外部观测值。

进一步的，通过高程约束自适应kalman滤波方程可得到k时刻的状态估计值，具体为：

若系统噪声方差阵为非负定阵，量测噪声方差阵为正定阵，则：

一步状态预测方程：

一步估计误差协方差阵方程：

增益矩阵：

协方差矩阵估计：

状态估计及高程约束过程：

其中：

为k时刻的状态向量估计；

表示的是k时刻高程误差的估计值；

k时刻sins解算的auv位置为(x(s，k)，y(s，k)，z(s，k))，其中，高程数据为z(s，k)；k时刻压力传感器输出的高程数据为z(p，k)，取二者差值，即：δh(p，k)＝z(s，k)-z(p，k)

δhp为压力传感器的精度，α按经验取值为α∈[0.8，1.5]；

若高程误差估计值超过阈值α·δhp，则要使用压力传感器对所述高程误差估计值进行约束；若未超过阈值，则继续下一时刻滤波估计。

进一步的，在使用本方法的同时还需要建立sins/lbl紧组合导航系统状态方程和量测方程，具体为：

sins/lbl紧组合状态方程可描述为：

其中，xsins、xlbl分别为sins、lbl的状态变量，fsins、flbl分别为sins、lbl的状态矩阵，wsins、wlbl分别为sins、lbl的状态噪声。

sins选择速度误差、姿态误差、位置误差、加速度计零偏、陀螺漂移作为状态量：

由sins误差方程可确定fsins，即：

lbl系统选择斜距差误差作为状态量，即：

xlbl＝[δr1δr2δr3]^t

其中，δr为斜距差误差，可用一阶markov过程表示。

lbl的量测矩阵为：

sins/lbl紧组合的量测方程可描述为：

其中，ρsins表示基于sins的斜距差；ρlbl表示基于lbl的斜距差；ρsins-ρlbl为基于sins的斜距差与基于lbl的斜距差的差值；η为量测噪声，h为量测矩阵。

进一步的，本发明还提供了一种基于sins/lbl紧组合的高程约束水下自适应导航装置，包括：第一斜距差模块、第二斜距差模块、斜距差计算模块、数据处理模块和修正模块，

所述第一斜距差模块用于使用sins获取auv位置信息，根据所述auv的位置信息和海底各应答器的位置信息获取基于sins斜距差；其中，所述海底各应答器包括i个应答器和一个参考应答器，i的取值范围为1到3的正整数；

所述第二斜距差模块用于获取通过lbl声源到所述第i个应答器与lbl声源到参考应答器的斜距差，记为基于lbl斜距差；

所述斜距差计算模块用于将所述基于sins斜距差与所述基于lbl斜距差做差运算，获取所述基于sins与所述基于lbl的斜距差之差，记为外部观测值；

所述数据处理模块用于将所述外部观测值作为高程约束自适应的滤波计算的输入值，根据k时刻的量测值，通过高程约束自适应kalman滤波方程可得到k时刻的状态估计值；

所述修正模块用于用所述高程误差估计值对固有的sins输出值的误差进行修正处理，获取修正误差后的sins的导航信息。

进一步的，一种终端设备，包括处理器、存储器以及存储在所述存储器中且被配置为由所述处理器执行的计算机程序，所述处理器执行所述计算机程序时实现如权利要求1至7任意一项所述的基于sins/lbl紧组合的高程约束水下自适应导航方法。

进一步的，一种计算机可读存储介质，所述计算机可读存储介质包括存储的计算机程序，其中，在所述计算机程序运行时控制所述计算机可读存储介质所在设备执行如权利要求1至7中任意一项所述的基于sins/lbl紧组合的高程约束水下自适应导航方法。

本发明实施例一种基于sins/lbl紧组合的高程约束水下自适应导航方法及装置，与现有技术相比，具有如下有益效果：

本发明提供的一种基于sins/lbl紧组合的高程约束水下自适应导航方法及装置，通过使用sins获取auv位置信息，根据所述auv的位置信息和海底各应答器的位置信息获取基于sins斜距差；获取通过lbl声源到所述第i个应答器与lbl声源到参考应答器的斜距差，记为基于lbl斜距差；将所述基于sins斜距差与所述基于lbl斜距差做差运算，获取所述基于sins与所述基于lbl的斜距差之差，记为外部观测值；将所述外部观测值作为高程约束自适应的滤波计算的输入值，根据k时刻的量测值，通过高程约束自适应kalman滤波方程可得到k时刻的状态估计值；用所述高程误差估计值对固有的sins输出值的误差进行修正处理，获取修正误差后的sins的导航信息。本发明相对于现有技术提高了水下导航定位的准确性。

附图说明

图1是本发明提供的基于sins/lbl紧组合的高程约束水下自适应导航方法的一种实施例的流程示意图；

图2是本发明提供的基于sins/lbl紧组合的高程约束水下自适应导航装置的一种实施例的结构示意图。

具体实施方式

下面将结合本发明中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

由于本发明提供的高程约束水下自适应导航方法是基于sins/lbl紧组合导航系统模型，所述sins/lbl紧组合导航系统模型建立的具体实现步骤如步骤s1和步骤s2所示；

s1、建立lbl斜距差模型。

由于海洋环境的不确定性，声信号在海水中的传播受到许多影响，因此对于斜距差的计算会造成一定误差。斜距差误差可以用一阶markov过程表示，即斜距差误差模型为：

其中，δri表示lbl斜距差的真实值；δri表示lbl斜距差的测量值误差；表示高斯白噪声；分别表示相关时间和驱动白噪声。

s2、建立sins/lbl紧组合导航系统状态方程和量测方程。

(1)状态方程的建立

sins/lbl紧组合状态方程可描述为：

其中，xsins、xlbl分别为sins、lbl的状态变量，fsins、flbl分别为sins、lbl的状态矩阵，wsins、wlbl分别为sins、lbl的状态噪声。

sins选择速度误差、姿态误差、位置误差、加速度计零偏、陀螺漂移作为状态量：

其中，15项状态量分别对应捷联惯导的东向速度误差、北向速度误差、天向速度误差、东向失准角、北向失准角、天向失准角、纬度误差、经度误差、高度误差、加速度计的三个轴向偏差、陀螺仪的三个轴向漂移。

由sins误差方程可确定fsins，即：

其中，矩阵f9×9中的元素为：

其中，是载体的姿态变换矩阵；rn是参考椭球子午面的曲率半径；re是垂直子午面内的曲率半径；re是参考椭球的长轴半径；e是椭球的第一偏心率。它们之间满足如下关系式：

rn＝re(1-2e+3esin²l)

re＝re(1+esin²l)

lbl系统选择斜距差误差作为状态量，即：

xlbl＝[δr1δr2δr3]^t

其中，δr为斜距差误差，可用一阶markov过程表示。

lbl的量测矩阵为：

(2)量测方程的建立

sins/lbl紧组合的量测方程可描述为：

其中，ρsins表示基于sins的斜距差；ρlbl表示基于lbl的斜距差；ρsins-ρlbl为基于sins的斜距差与基于lbl的斜距差的差值；η为量测噪声，h为量测矩阵。

对量测矩阵h进行推导，由于惯导输出的位置为经纬高形式，则需要将上式置换到大地坐标系后构造量测方程。δx，δy，δz到δl，δλ，δ置换公式为：

δx＝δhcoslcosλ-(re+h)sinlcosλδl-(re+h)coslsinλδλ

δy＝δhcoslsinλ-(re+h)sinlsinλδl-(re+h)coslcosλδλ

δz＝δhsinl+[re(1-e²)+h]coslδl

量测矩阵可表示为：

h＝[03×6h1(3×3)03×6-i3×3]

其中，i为单位向量，

矩阵中的各元素表达式如下：

ai1＝-(re+h)sinlcosλeix-(re+h)sinlsinλeiy+[re(1-e²)+h]cosleiz

ai2＝-(re+h)coslcosλeix-(re+h)coslcosλeiy

ai3＝coslcosλeix+sinlsinλeiy+sinleiz

其中，i＝(1，2，3)，e为椭球的扁心率，re为垂直子午面内的曲率半径。

基于上述步骤建立的sins/lbl紧组合导航系统模型，主要用于本发明所提供的一种基于sins/lbl紧组合的高程约束水下自适应导航方法及装置。

实施例1

参见图1，图1是本发明提供的基于sins/lbl紧组合的高程约束水下自适应导航方法的一种实施例的流程示意图，如图1所示，该方法包括步骤101到步骤105，具体如下：

步骤101：使用sins获取auv位置信息，根据所述auv的位置信息和海底各应答器的位置信息获取基于sins斜距差；其中，所述海底各应答器包括i个应答器和一个参考应答器，i的取值范围为1到3的正整数。

本实施例中，设sins解算的auv位置为(xs，ys，zs)，真实auv位置为(x，y，z)，海底四个应答器位置为(xj，yj，zj)，其中，j＝(0，1，2，3)，设第四个应答器(x0，y0，z0)为参考应答器，sins解算的auv位置与另外三个应答器的斜距分别与第四个斜距作差，得到基于sins的斜距差ρsinsi，其中，i＝(1，2，3)，即：

将上式相对auv真实位置进行taylor线性化得：

ρsinsi＝ri-r0+eixδx+eiyδy+eizδz

其中，ri表示声源与第i个应答器的斜距，i＝(0，1，2，3)；r0表示声源与参考应答器的斜距，有：

步骤102：获取通过lbl声源到所述第i个应答器与lbl声源到参考应答器的斜距差，记为基于lbl斜距差。

本实施例中，lbl声源到所述第i个应答器的斜距：

其中，i＝(1，2，3)；

lbl声源到参考应答器的斜距：

基于lbl斜距差：其中，δri表示lbl斜距差的测量值误差，表示lbl斜距差中存在的噪音。

步骤103：将所述基于sins斜距差与所述基于lbl斜距差做差运算，获取所述基于sins与所述基于lbl的斜距差之差，记为外部观测值。

本实施例中，

将所述外部观测值δρi记为外部观测值。

步骤104：将所述外部观测值作为高程约束自适应的滤波计算的输入值，根据k时刻的量测值，通过高程约束自适应kalman滤波方程可得到k时刻的状态估计值。

本实施例中，开始高程约束自适应kalman滤波前，还包括：

从线性离散系统的状态方程和量测方程中获取满足预设条件的系统激励噪音矩阵和量测噪音矩阵，线性离散系统的状态方程和量测方程可以描述为：

xk＝φk，k-1xk-1+гk-1wk-1

zk＝hkxk+vk

其中，xk表示tk时刻需要估计的状态向量；φk，k-1表示tk-1时刻到tk时刻的一步转移矩阵；гk-1表示系统噪声驱动矩阵；zk表示量测向量；hk为量测矩阵；wk-1表示系统激励噪声矩阵；vk表示量测噪声矩阵；工程上通常假定wk，vk互不相关的均值为零的高斯白噪声；

从满足第一预设条件的系统激励噪音矩阵和量测噪音矩阵中提取相对应的系统噪声方差阵和量测噪音方差阵，其中，第一预设条件为：

e[wk]＝0，e[wkwj^t]＝qkδkj

e[vk]＝0，e[vkvj^t]＝rkδkj

e[wkvj^t]＝0

其中，qk为系统噪声方差阵；vk为量测噪声方差阵；δkj是kroneckerδ函数；

若所述系统噪声方差阵和量测噪音方差阵满足第二预设条件，则开始高程约束自适应kalman滤波。其中，第二预设条件为系统噪声方差阵为非负定阵，量测噪声方差阵为正定阵。

所述外部观测值作为高程约束自适应的滤波计算的输入值，给定合适的初值和p0以及α，根据k时刻的量测值zk，通过高程约束自适应kalman滤波方程可得到k时刻的状态估计值，

一步状态预测方程：

一步估计误差协方差阵方程：

增益矩阵：

协方差矩阵估计：

状态估计及高程约束过程：

其中：

为k时刻的状态向量估计；

表示的是k时刻高程误差的估计值；

k时刻sins解算的auv位置为(x(s，k)，y(s，k)，z(s，k))，其中，高程数据为z(s，k)；k时刻压力传感器输出的高程数据为z(p，k)，取二者差值。即：δh(p，k)＝z(s，k)-z(p，k)；

δhp为压力传感器的精度，α按经验取值为α∈[0.8，1.5]；

若高程误差估计值超过阈值α·δhp，则要使用压力传感器对所述高程误差估计值进行约束；若未超过阈值，则继续下一时刻滤波估计。

步骤105：用所述高程误差估计值对固有的sins输出值的误差进行修正处理，获取修正误差后的sins的导航信息。

本实施例中，由于sins工作时稳定性较差，位置和速度误差会随着时间积累，从而导致最终的定位误差较大，甚至发散，因此用高程误差估计值对固有的sins输出值误差进行修正处理，得到修正后的sins导航信息。

参见图2，图2是本发明提供的基于sins/lbl紧组合的高程约束水下自适应导航装置的一种实施例的结构示意图，如图2所示，该装置包括：

第一斜距差模块201用于使用sins获取auv位置信息，根据所述auv的位置信息和海底各应答器的位置信息获取基于sins斜距差；其中，所述海底各应答器包括i个应答器和一个参考应答器，i的取值范围为1到3的正整数。

本实施例中，设sins解算的auv位置为(xs，ys，zs)，真实auv位置为(x，y，z)，海底四个应答器位置为(xj，yj，zj)，其中，j＝(0，1，2，3)，设第四个应答器(x0，y0，z0)为参考应答器，sins解算的auv位置与另外三个应答器的斜距分别与第四个斜距作差，得到基于sins的斜距差ρsinsi，其中，i＝(1，2，3)，即：

将上式相对auv真实位置进行taylor线性化得：

ρsinsi＝ri-r0+eixδx+eiyδy+eizδz

其中，ri表示声源与第i个应答器的斜距，i＝(0，1，2，3)；r0表示声源与参考应答器的斜距，有：

第二斜距差模块202用于获取通过lbl声源到所述第i个应答器与lbl声源到参考应答器的斜距差，记为基于lbl斜距差。

本实施例中，lbl声源到所述第i个应答器的斜距：

其中，i＝(1，2，3)；

lbl声源到参考应答器的斜距：

基于lbl斜距差：其中，δri表示lbl斜距差的测量值误差，表示lbl斜距差中存在的噪音。

斜距差计算模块203用于将所述基于sins斜距差与所述基于lbl斜距差做差运算，获取所述基于sins与所述基于lbl的斜距差之差，记为外部观测值。

本实施例中，

将所述外部观测值δρi记为外部观测值。

数据处理模块204用于将所述外部观测值作为高程约束自适应的滤波计算的输入值，根据k时刻的量测值，通过高程约束自适应kalman滤波方程可得到k时刻的状态估计值。

本实施例中，开始高程约束自适应kalman滤波前，还包括：

从线性离散系统的状态方程和量测方程中获取满足预设条件的系统激励噪音矩阵和量测噪音矩阵，线性离散系统的状态方程和量测方程可以描述为：

xk＝φk，k-1xk-1+гk-1wk-1

zk＝hkxk+vk

其中，xk表示tk时刻需要估计的状态向量；φk，k-1表示tk-1时刻到tk时刻的一步转移矩阵；гk-1表示系统噪声驱动矩阵；zk表示量测向量；hk为量测矩阵；wk-1表示系统激励噪声矩阵；vk表示量测噪声矩阵；工程上通常假定wk，vk互不相关的均值为零的高斯白噪声；

从满足第一预设条件的系统激励噪音矩阵和量测噪音矩阵中提取相对应的系统噪声方差阵和量测噪音方差阵，其中，第一预设条件为：

e[wk]＝0，e[wkwj^t]＝qkδkj

e[vk]＝0，e[vkvj^t]＝rkδkj

e[wkvj^t]＝0

其中，qk为系统噪声方差阵；vk为量测噪声方差阵；δkj是kroneckerδ函数；

若所述系统噪声方差阵和量测噪音方差阵满足第二预设条件，则开始高程约束自适应kalman滤波。其中，第二预设条件为系统噪声方差阵为非负定阵，量测噪声方差阵为正定阵。

所述外部观测值作为高程约束自适应的滤波计算的输入值，给定合适的初值和p0以及α，根据k时刻的量测值zk，通过高程约束自适应kalman滤波方程可得到k时刻的状态估计值，

一步状态预测方程：

一步估计误差协方差阵方程：

增益矩阵：

协方差矩阵估计：

状态估计及高程约束过程：

其中：

为k时刻的状态向量估计；

表示的是k时刻高程误差的估计值；

k时刻sins解算的auv位置为(x(s，k)，y(s，k)，z(s，k))，其中，高程数据为z(s，k)；k时刻压力传感器输出的高程数据为z(p，k)，取二者差值。即：δh(p，k)＝z(s，k)-z(p，k)；

δhp为压力传感器的精度，α按经验取值为α∈[0.8，1.5]；

若高程误差估计值超过阈值α·δhp，则要使用压力传感器对所述高程误差估计值进行约束；若未超过阈值，则继续下一时刻滤波估计。

修正模块205用于用所述高程误差估计值对固有的sins输出值的误差进行修正处理，获取修正误差后的sins的导航信息。

本实施例中，由于sins工作时稳定性较差，位置和速度误差会随着时间积累，从而导致最终的定位误差较大，甚至发散，因此用高程误差估计值对固有的sins输出值误差进行修正处理，得到修正后的sins导航信息。

综上，本发明提供的一种基于sins/lbl紧组合的高程约束水下自适应导航方法及装置，通过使用sins获取auv位置信息，根据所述auv的位置信息和海底各应答器的位置信息获取基于sins斜距差；获取通过lbl声源到所述第i个应答器与lbl声源到参考应答器的斜距差，记为基于lbl斜距差；将所述基于sins斜距差与所述基于lbl斜距差做差运算，获取所述基于sins与所述基于lbl的斜距差之差，记为外部观测值；将所述外部观测值作为高程约束自适应的滤波计算的输入值，根据k时刻的量测值，通过高程约束自适应kalman滤波方程可得到k时刻的状态估计值；用所述高程误差估计值对固有的sins输出值的误差进行修正处理，获取修正误差后的sins的导航信息，提高了水下导航定位的准确性。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和替换，这些改进和替换也应视为本发明的保护范围。