一种基于双运动模型的主从式多AUV协同导航方法技术

本发明专利技术属于水下航行器导航研究领域，具体涉及一种基于双运动模型的主从式多AUV协同导航方法，包括以下步骤：领航AUV与跟随AUV进行水声测距，同时领航AUV将自身位置和速度信息广播发送给跟随AUV；建立跟随AUV与领航AUV间的相对运动状态空间模型；通过CKF估计跟随AUV与领航AUV的速度分量差值；建立双领航模式的多AUV协同导航状态空间模型；本发明专利技术提出将AUV相对运动状态空间模型与双领航模式的多AUV协同导航状态空间模型相结合，保障了多AUV协同导航系统的协同定位性能；本发明专利技术跟随AUV不需装备惯性导航设备和DVL，从而降低AUV系统配置的复杂性且节约了跟随AUV的内部空间、减轻重量。

A Master-Slave Multi-AUV Cooperative Navigation Method Based on Dual Motion Model

【技术实现步骤摘要】
一种基于双运动模型的主从式多AUV协同导航方法
本专利技术属于水下航行器导航研究领域，具体涉及一种基于双运动模型的主从式多AUV协同导航方法。
技术介绍
协同导航是目前中间层区域多自主式水下航行器最有效的导航方法之一，具有广阔的应用前景。通常情况下，多AUV协同导航定位具有两种形式：1)并行式，即系统中每个航行器的功能和结构相同，使用各自的导航系统进行定位，通过水声通信，获取其他航行器的位置信息；2)主从式，也称为领航式，即系统中少量领航AUV装备高精度导航设备，大量跟随AUV装备低精度导航设备，跟随AUV通过获得与领航AUV的位置关系提高自身导航精度，并通过水声通信确定自身在系统中的位置。并行式的结构简单，但每个AUV都装备高精度导航设备，成本将增加很多倍，而主从式兼顾了导航精度和成本，成为多AUV协同导航研究的主要方向。主从式协同导航系统中，领航AUV装备高精度惯性导航设备、多普勒速度计程仪(Dopplervelocitylog,DVL)、差分全球定位系统(DGPS)、水声通信设备等，导航系统以高精度惯导设备为主，初始位置通过DGPS获得，以DVL测量的绝对速度作为惯导外部输入，本文档来自技高网...

【技术保护点】
1.一种基于双运动模型的主从式多AUV协同导航方法，其特征在于，包括以下步骤：(1)领航AUV与跟随AUV进行水声测距，同时领航AUV将自身位置和速度信息广播发送给跟随AUV；(2)建立跟随AUV与领航AUV间的相对运动状态空间模型；(3)根据步骤(2)建立的相对运动状态空间模型，通过CKF估计跟随AUV与领航AUV的速度分量差值；(4)建立双领航模式的多AUV协同导航状态空间模型；(5)根据步骤(4)建立的协同导航状态空间模型，通过CKF估计跟随AUV位置信息。

【技术特征摘要】
1.一种基于双运动模型的主从式多AUV协同导航方法，其特征在于，包括以下步骤：(1)领航AUV与跟随AUV进行水声测距，同时领航AUV将自身位置和速度信息广播发送给跟随AUV；(2)建立跟随AUV与领航AUV间的相对运动状态空间模型；(3)根据步骤(2)建立的相对运动状态空间模型，通过CKF估计跟随AUV与领航AUV的速度分量差值；(4)建立双领航模式的多AUV协同导航状态空间模型；(5)根据步骤(4)建立的协同导航状态空间模型，通过CKF估计跟随AUV位置信息。2.根据权利要求1所述的一种基于双运动模型的主从式多AUV协同导航方法，其特征在于，所述相对运动状态空间模型包括状态方程与量测方程；状态方程：跟随AUV在tk时刻和tk+1时刻的位置向量分别为和领航AUV-1在tk时刻和tk+1时刻的位置向量分别为和领航AUV-2在tk时刻和tk+1时刻的位置向量分别为和分别为跟随AUV、领航AUV-1和领航AUV-2在tk时刻的东向位置；分别为跟随AUV、领航AUV-1和领航AUV-2在tk时刻的北向位置；分别为跟随AUV、领航AUV-1和领航AUV-2在tk+1时刻的东向位置；分别为跟随AUV、领航AUV-1和领航AUV-2在tk+1时刻的北向位置；跟随AUV在tk时刻的状态向量为其中，分别为跟随AUV在tk时刻的东向速度分量和北向速度分量；领航AUV-1在tk时刻的状态向量为其中，分别领航AUV-1在tk时刻的东向速度分量和北向速度分量；与的表达式如下：其中，分别为由DVL提供的领航AUV-1在tk时刻的右舷速度和前向速度；为由高精度惯性导航设备提供的领航AUV-1在tk时刻的航向角；跟随AUV的位置状态方程为：其中，δt为采样时间间隔；领航AUV-1的位置状态方程为：跟随AUV与领航AUV-1在tk时刻的相对运动状态向量为：其中，为跟随AUV相对领航AUV-1在tk时刻的东向距离；为跟随AUV相对领航AUV-1在tk时刻的北向距离；为跟随AUV相对领航AUV-1在tk时刻的东向速度差；为跟随AUV相对领航AUV-1在tk时刻的北向速度差；系统的离散状态方程为：Xk+1＝Fk+1|kXk其中，Xk＝(Δxk,Δyk,Δvx,k,Δvy,k)T为跟随AUV与领航AUV-1之间在tk时刻的相对运动状态向量；Xk+1为跟随AUV与领航AUV-1之间在tk+1时刻的相对运动状态向量；Fk+1|k为tk时刻到tk+1时刻的状态转移矩阵，量测方程：跟随AUV和领航AUV-1之间的坐标位置关系为：跟随AUV与领航AUV-1的相对运动离散时间状态空间模型如下：其中，k+1表示tk+1时刻；量测函数Δxk+1为跟随AUV相对领航AUV-1在tk+1时刻的东向距离；Δyk+1为跟随AUV相对领航AUV-1在tk+1时刻的北向距离；wk为过程噪声向量，vk+1为量测噪声向量，且wk、vk+1均为高斯白噪声，为领航AUV-1与跟随AUV之间在tk时刻的相对距离量测信息。3.根据权利要求1所述的一种基于双运动模型的主从式多AUV协同导航方法，其特征在于，所述根据步骤(2)建立的相对运动状态空间模型，通过CKF估计跟随AUV与领航AUV的速度分量差值，包括：时间更新：状态向量Xk＝(Δxk,Δyk,Δvx,k,Δvy,k)T在tk时刻系统后验概率状态为后验概率密度函数为通过Cholesky将状态误差协方差Pk|k分解为如下形式：其中，Sk|k＝chol{Pk|k}，chol{·}表示对矩阵进行Cholesky分解，为Sk|k的转置；计算Cubature点如下：其中，(i＝1,2.…,m，m＝2n)，n为状态方程维数，且定义以下变量：其中，m＝2n为容积点的个数；n＝4为状态方程维数；ξi为生成的容积点；[1]i为点集中的第i个容积点，容积点集为：wi为每一个容积点所占的权值；通过系统状态的转移矩阵函数传递Cubature点：Xi,k+1|k＝Fk+1|kXi,k|ktk+1时刻的状态预测值通过加权求和得到：计算tk+1时刻的状态误差协方差预测值：其中，Xi,k+1|k为通过系统状态的转移矩阵函数传递Cubature点，为...

【专利技术属性】
技术研发人员：徐博，李盛新，李金，张勇刚，王潇雨，郭瑜，张大龙，李志鹏，
申请(专利权)人：哈尔滨工程大学，
类型：发明
国别省市：黑龙江,23