CN109974706A - 一种基于双运动模型的主从式多auv协同导航方法 - Google Patents
一种基于双运动模型的主从式多auv协同导航方法 Download PDFInfo
- Publication number
- CN109974706A CN109974706A CN201910177062.2A CN201910177062A CN109974706A CN 109974706 A CN109974706 A CN 109974706A CN 201910177062 A CN201910177062 A CN 201910177062A CN 109974706 A CN109974706 A CN 109974706A
- Authority
- CN
- China
- Prior art keywords
- auv
- moment
- navigator
- state
- follow
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
Abstract
本发明属于水下航行器导航研究领域，具体涉及一种基于双运动模型的主从式多AUV协同导航方法，包括以下步骤：领航AUV与跟随AUV进行水声测距，同时领航AUV将自身位置和速度信息广播发送给跟随AUV；建立跟随AUV与领航AUV间的相对运动状态空间模型；通过CKF估计跟随AUV与领航AUV的速度分量差值；建立双领航模式的多AUV协同导航状态空间模型；本发明提出将AUV相对运动状态空间模型与双领航模式的多AUV协同导航状态空间模型相结合，保障了多AUV协同导航***的协同定位性能；本发明跟随AUV不需装备惯性导航设备和DVL，从而降低AUV***配置的复杂性且节约了跟随AUV的内部空间、减轻重量。
Description
技术领域
本发明属于水下航行器导航研究领域，具体涉及一种基于双运动模型的主从式多AUV协同导航方法。
背景技术
协同导航是目前中间层区域多自主式水下航行器最有效的导航方法之一，具有广阔的应用前景。通常情况下，多AUV协同导航定位具有两种形式：1)并行式，即***中每个航行器的功能和结构相同，使用各自的导航***进行定位，通过水声通信，获取其他航行器的位置信息；2)主从式，也称为领航式，即***中少量领航AUV装备高精度导航设备，大量跟随AUV装备低精度导航设备，跟随AUV通过获得与领航AUV的位置关系提高自身导航精度，并通过水声通信确定自身在***中的位置。并行式的结构简单，但每个AUV都装备高精度导航设备，成本将增加很多倍，而主从式兼顾了导航精度和成本，成为多AUV协同导航研究的主要方向。主从式协同导航***中，领航AUV装备高精度惯性导航设备、多普勒速度计程仪(Doppler velocity log,DVL)、差分全球定位***(DGPS)、水声通信设备等，导航***以高精度惯导设备为主，初始位置通过DGPS获得，以DVL测量的绝对速度作为惯导外部输入，进一步提高了协同定位精度，跟随AUV装备低精度惯性导航设备、DVL、水声通信设备等。基于高低精度导航设备搭配成的传感网络，通过AUV间的相对测量与信息共享，进而实现整个编队的导航定位性能提升。然而在大量跟随AUV的情况下，即使跟随AUV装备低精度航位推算导航设备，其所需的惯性导航设备和DVL也仍是一笔不菲的成本，故在实际工程应用中，如果可以实现在跟随AUV没有装备惯性导航设备和DVL的情况下，还能够保障协同导航***定位精度在允许范围内，将具有重大的研究价值。
发明内容
本发明的目的在于提供一种在跟随AUV没有装备惯性导航设备和DVL的情况下，应用CKF进行跟随AUV位置状态估计的主从式多AUV协同导航方法，从而降低多AUV协同导航***成本。
一种基于双运动模型的主从式多AUV协同导航方法，包括以下步骤：
(1)领航AUV与跟随AUV进行水声测距，同时领航AUV将自身位置和速度信息广播发送给跟随AUV；
(2)建立跟随AUV与领航AUV间的相对运动状态空间模型；
(3)根据步骤(2)建立的相对运动状态空间模型，通过CKF估计跟随AUV与领航AUV的速度分量差值；
(4)建立双领航模式的多AUV协同导航状态空间模型；
(5)根据步骤(4)建立的协同导航状态空间模型，通过CKF估计跟随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时刻的相对距离量测信息。
所述根据步骤(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|k
tk+1时刻的状态预测值通过加权求和得到：
计算tk+1时刻的状态误差协方差预测值：
其中，Xi,k+1|k为通过***状态的转移矩阵函数传递Cubature点，为Xi,k+1|k的转置；为tk+1时刻的状态预测值，为的转置；***噪声协方差阵Qk＝E[wkwk T]，wk为高斯白噪声，wk T为wk转置；
量测更新：
通过Cholesky将状态误差协方差预测值分解为如下形式：
其中，Sk+1|k＝chol{Pk+1|k}，为Sk+1|k的转置；
计算Cubature点如下：
通过***的量测函数传递Cubature点：
Zi,k+1|k＝h(Xi,k+1|k,k+1)
tk+1时刻的观测预测值通过加权求和得到：
计算tk+1时刻的量测误差协方差预测值：
其中，Zi,k+1|k为***的量测函数传递Cubature点，为Zi,k+1|k的转置；为tk+1时刻的观测预测值，为的转置；量测噪声协方差阵vk+1为高斯白噪声，为vk+1转置；
互协方差矩阵：
估计卡尔曼滤波增益：
状态更新过程，获得tk+1时刻的状态估计：
利用协方差更新过程，获得tk+1时刻的状态估计误差协方差矩阵，即：
其中，为Kk+1的转置；
以此过程逐次递推至状态估计误差协方差达到稳定值，即得到跟随AUV与领航AUV-1的相对状态估计，由tk+1时刻的相对状态估计得到跟随AUV在tk+1时刻的东向速度估计值和北向速度估计值分别为和
其中，跟随AUV在tk+1时刻的估计航向角为 分别领航AUV-1在tk+1时刻的东向速度分量和北向速度分量；为跟随AUV相对领航AUV-1在tk+1时刻的东向距离估计值；为跟随AUV相对领航AUV-1在tk+1时刻的北向距离估计值；为跟随AUV相对领航AUV-1在tk+1时刻的东向速度差估计值；为跟随AUV相对领航AUV-1在tk+1时刻的北向速度差估计值。
所述建立双领航模式的多AUV协同导航状态空间模型，包括：
基于相对位置量测的协同导航***状态方程具体为：
领航AUV-1、领航AUV-2与跟随AUV之间的坐标位置关系为：
多AUV协同定位***的离散时间状态空间模型如下：
其中，k+1表示tk+1时刻；为跟随AUV在tk时刻的位置状态量；为跟随AUV在tk+1时刻的状态量；为通过步骤三计算所得到的在tk时刻跟随AUV的东向速度和北向速度估计值，为跟随AUV相对领航AUV-1在tk时刻的东向速度差估计值；为跟随AUV相对领航AUV-1在tk时刻的北向速度差估计值；为tk+1时刻的量测向量；为领航AUV在tk+1时刻的位置状态量；为过程噪声向量，为量测噪声向量，且均为Gauss白噪声；状态函数量测函数
所述根据步骤(4)建立的协同导航状态空间模型，通过CKF估计跟随AUV位置信息，包括：
时间更新：
状态向量在tk时刻***后验概率为后验概率密度函数为通过Cholesky将状态误差协方差分解为如下形式：
其中，chol{·}表示对矩阵进行Cholesky分解，为的转置；
计算Cubature点如下：
其中，n*为状态方程维数，且定义以下变量：
其中，m*＝2n*为容积点的个数；n*＝2为状态方程维数；为生成的容积点；为点集中的第i个容积点，容积点集为： 为每一个容积点所占的权值；
通过***状态的转移矩阵函数传递Cubature点：
tk+1时刻的状态预测值通过加权求和得到：
计算tk+1时刻的状态误差协方差预测值：
其中，为通过***状态的转移矩阵函数传递Cubature点，为的转置；为tk+1时刻的状态预测值，为的转置；***噪声协方差阵 为高斯白噪声，为转置；
量测更新：
通过Cholesky将状态误差协方差预测值分解为如下形式：
其中， 为的转置；
计算Cubature点如下：
通过***的量测函数传递Cubature点：
tk+1时刻的观测预测值通过加权求和得到：
计算tk+1时刻的量测误差协方差预测值：
其中，为***的量测函数传递Cubature点，为的转置；为tk+1时刻的观测预测值，为的转置；量测噪声协方差阵为高斯白噪声，为转置；
互协方差矩阵：
估计卡尔曼滤波增益：
状态更新过程，获得tk+1时刻的状态估计：
利用协方差更新过程，获得tk+1时刻的状态估计误差协方差矩阵，即：
其中，为的转置。
本发明的有益效果在于：
1.仅基于跟随AUV和领航AUV间的相对测量距离及领航AUV广播的自身位置和速度信息，计算得到跟随AUV的位置、速度和航向信息，节约了大量惯性导航设备和DVL，降低多AUV协同导航成本；
2.跟随AUV不需装备惯性导航设备和DVL，从而降低AUV***配置的复杂性且节约了跟随AUV的内部空间、减轻重量；
3.提出将AUV相对运动状态空间模型与双领航模式的多AUV协同导航状态空间模型相结合，保障了多AUV协同导航***的协同定位性能。
附图说明
图1为本发明的流程图；
图2为基于双领航AUV模式的水声通信示意图；
图3为跟随AUV、领航AUV-1与领航AUV-2的真实航行轨迹图；
图4为跟随AUV前向速度估计；
图5为跟随AUV航向角估计；
图6为基于双运动模型跟随AUV的估计轨迹；
图7为基于双运动模型跟随AUV的估计定位误差。
具体实施方式
下面结合附图对本发明做进一步描述。
协同导航是目前中间层区域多自主式水下航行器最有效的导航方法之一，具有广阔的应用前景。通常情况下，多AUV协同导航定位具有两种形式：1)并行式，即***中每个航行器的功能和结构相同，使用各自的导航***进行定位，通过水声通信，获取其他航行器的位置信息；2)主从式，也称为领航式，即***中少量领航AUV装备高精度导航设备，大量跟随AUV装备低精度导航设备，跟随AUV通过获得与领航AUV的位置关系提高自身导航精度，并通过水声通信确定自身在***中的位置。并行式的结构简单，但每个AUV都装备高精度导航设备，成本将增加很多倍，而主从式兼顾了导航精度和成本，成为多AUV协同导航研究的主要方向。主从式协同导航***中，领航AUV装备高精度惯性导航设备、多普勒速度计程仪(Doppler velocity log,DVL)、差分全球定位***(DGPS)、水声通信设备等，导航***以高精度惯导设备为主，初始位置通过DGPS获得，以DVL测量的绝对速度作为惯导外部输入，进一步提高了协同定位精度，跟随AUV装备低精度惯性导航设备、DVL、水声通信设备等。基于高低精度导航设备搭配成的传感网络，通过AUV间的相对测量与信息共享，进而实现整个编队的导航定位性能提升。然而在大量跟随AUV的情况下，即使跟随AUV装备低精度航位推算导航设备，其所需的惯性导航设备和DVL也仍是一笔不菲的成本，故在实际工程应用中，如果可以实现在跟随AUV没有装备惯性导航设备和DVL的情况下，还能够保障协同导航***定位精度在允许范围内，将具有重大的研究价值。
本发明的目的是这样实现的：
步骤一：领航AUV与跟随AUV进行水声测距，同时领航AUV将自身位置和速度信息广播发送给跟随AUV；
步骤二：建立跟随AUV与领航AUV间的相对运动状态空间模型；
步骤三：根据步骤二建立的相对运动状态空间模型，通过CKF估计跟随AUV与领航AUV的速度分量差值；
步骤四：建立双领航模式的多AUV协同导航状态空间模型；
步骤五：根据步骤四建立的协同导航状态空间模型，通过CKF估计跟随AUV位置信息。
在主从式多AUV协同导航***中，跟随AUV装备低精度惯性导航设备、DVL、水声通信设备等。然而在跟随AUV数量较多的情况下，就需要很多的惯性导航设备和DVL，***配置复杂，相应成本也随之提高。
针对上述问题，本专利以缩减多AUV协同导航***成本为目标，在传统多AUV协同导航方法的基础上，设计一种基于双运动模型的主从式多AUV协同导航方法，利用CKF进行状态估计，得到跟随AUV的实时航向信息和速度信息，再通过应用双领航模式的多AUV协同导航状态空间模型，进一步保障了多AUV***协同定位性能。
步骤一：领航AUV与跟随AUV进行水声测距，同时领航AUV将自身位置及速度信息广播发送给跟随AUV
在双领航模式下，领航AUV-1和领航AUV-2向跟随AUV发送水声信号，根据水声信号由领航AUV传输至跟随AUV的时间及水声通信的传输速度可分别得到跟随AUV与领航AUV-1和领航AUV-2之间的距离。同时，领航AUV将自身位置及速度信息广播发送给跟随AUV。
步骤二：建立跟随AUV与领航AUV间的相对运动状态空间模型
(1)状态方程
在实际水下多AUV协同导航***中，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时刻的状态转移矩阵，
(2)量测方程
跟随AUV在tk时刻通过水声通信获得的相对观测信息为领航AUV-1和领航AUV-2在tk时刻的位置信息和领航AUV-1和领航AUV-2与跟随AUV之间在tk时刻的相对距离量测信息以及领航AUV-1在tk时刻的东向速度分量和北向速度分量
由以上信息可得跟随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均为高斯白噪声。
步骤三：根据步骤二建立的相对运动状态空间模型，通过CKF估计跟随AUV与领航AUV-1的速度分量差值
CKF滤波算法依据Spherical-Radial准则，采用一组等权值的容积点，利用统计数值积分原理计算非线性变换后随机变量的均值和协方差，能够较为准确的获得状态更新及状态协方差矩阵更新。
(1)时间更新
假设状态向量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|k
tk+1时刻的状态预测值通过加权求和得到：
计算tk+1时刻的状态误差协方差预测值：
式中，Xi,k+1|k为通过***状态的转移矩阵函数传递Cubature点，为Xi,k+1|k的转置；为tk+1时刻的状态预测值，为的转置；***噪声协方差阵Qk＝E[wkwk T]，wk为高斯白噪声，wk T为wk转置。
(2)量测更新
通过Cholesky将状态误差协方差预测值分解为如下形式：
式中，Sk+1|k＝chol{Pk+1|k}，为Sk+1|k的转置。
计算Cubature点(i＝1,2.…,m，m＝2n)：
通过***的量测函数传递Cubature点：
Zi,k+1|k＝h(Xi,k+1|k,k+1)
tk+1时刻的观测预测值通过加权求和得到：
计算tk+1时刻的量测误差协方差预测值：
式中，Zi,k+1|k为***的量测函数传递Cubature点，为Zi,k+1|k的转置；为tk+1时刻的观测预测值，为的转置；量测噪声协方差阵vk+1为高斯白噪声，为vk+1转置。
互协方差矩阵：
估计卡尔曼滤波增益：
状态更新过程，获得tk+1时刻的状态估计：
利用协方差更新过程，获得tk+1时刻的状态估计误差协方差矩阵，即：
式中，为Kk+1的转置。
以此过程逐次递推至状态估计误差协方差达到稳定值，即得到跟随AUV与领航AUV-1的相对状态估计，由tk+1时刻的相对状态估计可得到跟随AUV在tk+1时刻的东向速度估计值和北向速度估计值分别为和跟随AUV在tk+1时刻的估计航向角为 分别领航AUV-1在tk+1时刻的东向速度分量和北向速度分量；为跟随AUV相对领航AUV-1在tk+1时刻的东向距离估计值；为跟随AUV相对领航AUV-1在tk+1时刻的北向距离估计值；为跟随AUV相对领航AUV-1在tk+1时刻的东向速度差估计值；为跟随AUV相对领航AUV-1在tk+1时刻的北向速度差估计值。
步骤四：建立双领航模式的多AUV协同导航状态空间模型；
由步骤二定义的跟随AUV在tk时刻和tk+1时刻的位置向量分别为和领航AUV-1在tk时刻和tk+1时刻的位置向量分别为和领航AUV-2在tk时刻和tk+1时刻的位置向量分别为和领航AUV-1和领航AUV-2与跟随AUV之间在tk时刻的相对测量距离分别为以及步骤三中通过滤波估计出的跟随AUV的东向速度和北向速度，可得基于相对位置量测的协同导航***状态方程具体为：
领航AUV-1、领航AUV-2与跟随AUV之间的坐标位置关系为：
基于式(8)和式(9)，建立多AUV协同定位***的离散时间状态空间模型(状态方程和量测方程)如下：
式中，k+1表示tk+1时刻；为跟随AUV在tk时刻的位置状态量；为跟随AUV在tk+1时刻的状态量；为通过步骤三计算所得到的在tk时刻跟随AUV的东向速度和北向速度估计值，为跟随AUV相对领航AUV-1在tk时刻的东向速度差估计值；为跟随AUV相对领航AUV-1在tk时刻的北向速度差估计值；为tk+1时刻的量测向量；为领航AUV在tk+1时刻的位置状态量；为过程噪声向量，为量测噪声向量，且均为Gauss白噪声；状态函数量测函数
步骤五：根据步骤三建立的协同导航状态空间模型，通过CKF估计跟随AUV位置信息(1)时间更新
假设状态向量在tk时刻***后验概率状态以及后验概率密度函数已知，通过Cholesky将状态误差协方差分解为如下形式：
式中，chol{·}表示对矩阵进行Cholesky分解，为的转置。
计算Cubature点(i＝1,2.…,m*，m*＝2n*)：
其中n*为状态方程维数，且定义以下变量：
式中，m*＝2n*为容积点的个数；n*＝2为状态方程维数；为生成的容积点；为点集中的第i个容积点，容积点集为： 为每一个容积点所占的权值。
通过***状态的转移矩阵函数传递Cubature点：
tk+1时刻的状态预测值通过加权求和得到：
计算tk+1时刻的状态误差协方差预测值：
式中，为通过***状态的转移矩阵函数传递Cubature点，为的转置；为tk+1时刻的状态预测值，为的转置；***噪声协方差阵 为高斯白噪声，为转置。
(2)量测更新
通过Cholesky将状态误差协方差预测值分解为如下形式：
式中， 为的转置。
计算Cubature点(i＝1,2.…,m，m＝2n)：
通过***的量测函数传递Cubature点：
tk+1时刻的观测预测值通过加权求和得到：
计算tk+1时刻的量测误差协方差预测值：
式中，为***的量测函数传递Cubature点，为的转置；为tk+1时刻的观测预测值，为的转置；量测噪声协方差阵 为高斯白噪声，为转置。
互协方差矩阵：
估计卡尔曼滤波增益：
状态更新过程，获得tk+1时刻的状态估计：
利用协方差更新过程，获得tk+1时刻的状态估计误差协方差矩阵，即：
式中，为的转置。
以此过程逐次递推至状态估计误差协方差达到稳定值，即得到***状态的估计，从而实现对跟随AUV的定位。
为进一步说明本发明的有益效果，对基于双运动模型的主从式多AUV协同导航方案进行了仿真验证：
仿真时间为3600s；采样周期为1s；
(1)跟随AUV的初始位置坐标为(0m，1000m)，前向速度为10kn，沿航向角为150°方向匀速直航；
(2)领航AUV-1的初始位置坐标为(0m，0m)，前向速度为8kn，初始航向角为30°，前1200s以角速率大小为0.6°/s做回旋运动，1200s后匀速直航；
(3)领航AUV-2的初始位置坐标为(0m，1000m)，前向速度为8kn，沿航向角为90°方向匀速直航；
仿真中还引入领航AUV-1、领航AUV-2与跟随AUV间水声测距的量测噪声，均为方差为10m2，均值为零的高斯白噪声，以及两个领航AUV的东向和北向位置误差，均为方差为5m2、均值为零的高斯白噪声。假设跟随AUV、领航AUV-1和领航AUV-2初始位置坐标已知，相对运动状态空间模型初始状态为Xk＝(0-0,1000-0,0-8·sin(30°),0-8·cos(30°))T，双领航模式的多AUV协同导航状态空间模型初始状态为进行仿真实验。
1.仅基于跟随AUV和领航AUV间的相对测量距离及领航AUV广播的自身位置和速度信息，计算得到跟随AUV的位置、速度和航向信息，节约了大量惯性导航设备和DVL，降低多AUV协同导航成本；
2.跟随AUV不需装备惯性导航设备和DVL，从而降低AUV***配置的复杂性且节约了跟随AUV的内部空间、减轻重量；
3.提出将AUV相对运动状态空间模型与双领航模式的多AUV协同导航状态空间模型相结合，保障了多AUV协同导航***的协同定位性能。
Claims (5)
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|k
tk+1时刻的状态预测值通过加权求和得到：
计算tk+1时刻的状态误差协方差预测值：
其中，Xi,k+1|k为通过***状态的转移矩阵函数传递Cubature点，为Xi,k+1|k的转置；为tk+1时刻的状态预测值，为的转置；***噪声协方差阵Qk＝E[wkwk T]，wk为高斯白噪声，wk T为wk转置；
量测更新：
通过Cholesky将状态误差协方差预测值分解为如下形式：
其中，Sk+1|k＝chol{Pk+1|k}，为Sk+1|k的转置；
计算Cubature点如下：
通过***的量测函数传递Cubature点：
Zi,k+1|k＝h(Xi,k+1|k,k+1)
tk+1时刻的观测预测值通过加权求和得到：
计算tk+1时刻的量测误差协方差预测值：
其中，Zi,k+1|k为***的量测函数传递Cubature点，为Zi,k+1|k的转置；为tk+1时刻的观测预测值，为的转置；量测噪声协方差阵vk+1为高斯白噪声，为vk+1转置；
互协方差矩阵：
估计卡尔曼滤波增益：
状态更新过程，获得tk+1时刻的状态估计：
利用协方差更新过程，获得tk+1时刻的状态估计误差协方差矩阵，即：
其中，为Kk+1的转置；
以此过程逐次递推至状态估计误差协方差达到稳定值，即得到跟随AUV与领航AUV-1的相对状态估计，由tk+1时刻的相对状态估计得到跟随AUV在tk+1时刻的东向速度估计值和北向速度估计值分别为和
其中，跟随AUV在tk+1时刻的估计航向角为 分别领航AUV-1在tk+1时刻的东向速度分量和北向速度分量；为跟随AUV相对领航AUV-1在tk+1时刻的东向距离估计值；为跟随AUV相对领航AUV-1在tk+1时刻的北向距离估计值；为跟随AUV相对领航AUV-1在tk+1时刻的东向速度差估计值；为跟随AUV相对领航AUV-1在tk+1时刻的北向速度差估计值。
4.根据权利要求1所述的一种基于双运动模型的主从式多AUV协同导航方法，其特征在于，所述建立双领航模式的多AUV协同导航状态空间模型，包括：
基于相对位置量测的协同导航***状态方程具体为：
领航AUV-1、领航AUV-2与跟随AUV之间的坐标位置关系为：
多AUV协同定位***的离散时间状态空间模型如下：
其中，k+1表示tk+1时刻；为跟随AUV在tk时刻的位置状态量；为跟随AUV在tk+1时刻的状态量；为通过步骤三计算所得到的在tk时刻跟随AUV的东向速度和北向速度估计值，为跟随AUV相对领航AUV-1在tk时刻的东向速度差估计值；为跟随AUV相对领航AUV-1在tk时刻的北向速度差估计值；为tk+1时刻的量测向量；为领航AUV在tk+1时刻的位置状态量；为过程噪声向量，为量测噪声向量，且均为Gauss白噪声；状态函数量测函数
5.根据权利要求1所述的一种基于双运动模型的主从式多AUV协同导航方法，其特征在于，所述根据步骤(4)建立的协同导航状态空间模型，通过CKF估计跟随AUV位置信息，包括：
时间更新：
状态向量在tk时刻***后验概率为后验概率密度函数为通过Cholesky将状态误差协方差分解为如下形式：
其中，chol{·}表示对矩阵进行Cholesky分解，为的转置；
计算Cubature点如下：
其中，n*为状态方程维数，且定义以下变量：
其中，m*＝2n*为容积点的个数；n*＝2为状态方程维数；为生成的容积点；为点集中的第i个容积点，容积点集为： 为每一个容积点所占的权值；
通过***状态的转移矩阵函数传递Cubature点：
tk+1时刻的状态预测值通过加权求和得到：
计算tk+1时刻的状态误差协方差预测值：
其中，为通过***状态的转移矩阵函数传递Cubature点，为的转置；为tk+1时刻的状态预测值，为的转置；***噪声协方差阵 为高斯白噪声，为转置；
量测更新：
通过Cholesky将状态误差协方差预测值分解为如下形式：
其中， 为的转置；
计算Cubature点如下：
通过***的量测函数传递Cubature点：
tk+1时刻的观测预测值通过加权求和得到：
计算tk+1时刻的量测误差协方差预测值：
其中，为***的量测函数传递Cubature点，为的转置；为tk+1时刻的观测预测值，为的转置；量测噪声协方差阵 为高斯白噪声，为转置；
互协方差矩阵：
估计卡尔曼滤波增益：
状态更新过程，获得tk+1时刻的状态估计：
利用协方差更新过程，获得tk+1时刻的状态估计误差协方差矩阵，即：
其中，为的转置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910177062.2A CN109974706B (zh) | 2019-03-08 | 2019-03-08 | 一种基于双运动模型的主从式多auv协同导航方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910177062.2A CN109974706B (zh) | 2019-03-08 | 2019-03-08 | 一种基于双运动模型的主从式多auv协同导航方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109974706A true CN109974706A (zh) | 2019-07-05 |
| CN109974706B CN109974706B (zh) | 2021-05-11 |
Family
ID=67078338
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910177062.2A Active CN109974706B (zh) | 2019-03-08 | 2019-03-08 | 一种基于双运动模型的主从式多auv协同导航方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109974706B (zh) |
Cited By (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111307136A (zh) * | 2020-02-12 | 2020-06-19 | 哈尔滨工程大学 | 一种双智能水下机器人水下航行地形匹配导航方法 |
| CN111595348A (zh) * | 2020-06-23 | 2020-08-28 | 南京信息工程大学 | 一种自主水下航行器组合导航***的主从式协同定位方法 |
| CN111928851A (zh) * | 2020-04-17 | 2020-11-13 | 中国科学院沈阳自动化研究所 | 基于tma技术的多自主水下机器人集群协同导航方法 |
| CN112697145A (zh) * | 2020-11-17 | 2021-04-23 | 武汉第二船舶设计研究所(中国船舶重工集团公司第七一九研究所) | 一种基于ckf的主从式多水下无人潜器协同定位方法 |
| CN112818819A (zh) * | 2021-01-28 | 2021-05-18 | 青岛澎湃海洋探索技术有限公司 | 一种基于动态模型和复杂网络理论的auv状态监测方法 |
| CN112985391A (zh) * | 2021-04-19 | 2021-06-18 | 中国人民解放军国防科技大学 | 一种基于惯性和双目视觉的多无人机协同导航方法和装置 |
| CN113324547A (zh) * | 2021-05-25 | 2021-08-31 | 江苏科技大学 | 基于迭代扩展rts平滑滤波算法的多auv协同定位方法 |
| CN114136323A (zh) * | 2021-11-29 | 2022-03-04 | 中国船舶重工集团公司第七0七研究所 | 基于运动补偿下tdoa量测的auv协同导航方法 |
| CN114370878A (zh) * | 2022-01-04 | 2022-04-19 | 江苏科技大学 | 一种基于stackf的多auv协同定位方法 |
| CN114840003A (zh) * | 2022-03-21 | 2022-08-02 | 哈尔滨工程大学 | 一种单领航多auv协同定位及轨迹跟踪控制方法 |
| CN114995405A (zh) * | 2022-05-19 | 2022-09-02 | 同济大学 | 一种基于开放动态环境多目标协同理论的agv协作搬运方法 |
| CN116680500A (zh) * | 2023-06-12 | 2023-09-01 | 哈尔滨工程大学 | 水下航行器在非高斯噪声干扰下的位置估计方法及*** |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103398712A (zh) * | 2013-08-02 | 2013-11-20 | 中国人民解放军63983部队 | 交替领航的协同导航方法 |
| CN103674029A (zh) * | 2013-12-19 | 2014-03-26 | 哈尔滨工程大学 | 一种基于水声通信的多艇协同导航编队构型的方法 |
| CN106441300A (zh) * | 2016-09-08 | 2017-02-22 | 哈尔滨工程大学 | 一种具有自适应的协同导航滤波方法 |
| CN107677272A (zh) * | 2017-09-08 | 2018-02-09 | 哈尔滨工程大学 | 一种基于非线性信息滤波的auv协同导航方法 |
| CN108489498A (zh) * | 2018-06-15 | 2018-09-04 | 哈尔滨工程大学 | 一种基于最大互相关熵无迹粒子滤波的auv协同导航方法 |
| CN108827305A (zh) * | 2018-05-25 | 2018-11-16 | 哈尔滨工程大学 | 一种基于鲁棒信息滤波的auv协同导航方法 |
-
2019
- 2019-03-08 CN CN201910177062.2A patent/CN109974706B/zh active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103398712A (zh) * | 2013-08-02 | 2013-11-20 | 中国人民解放军63983部队 | 交替领航的协同导航方法 |
| CN103674029A (zh) * | 2013-12-19 | 2014-03-26 | 哈尔滨工程大学 | 一种基于水声通信的多艇协同导航编队构型的方法 |
| CN106441300A (zh) * | 2016-09-08 | 2017-02-22 | 哈尔滨工程大学 | 一种具有自适应的协同导航滤波方法 |
| CN107677272A (zh) * | 2017-09-08 | 2018-02-09 | 哈尔滨工程大学 | 一种基于非线性信息滤波的auv协同导航方法 |
| CN108827305A (zh) * | 2018-05-25 | 2018-11-16 | 哈尔滨工程大学 | 一种基于鲁棒信息滤波的auv协同导航方法 |
| CN108489498A (zh) * | 2018-06-15 | 2018-09-04 | 哈尔滨工程大学 | 一种基于最大互相关熵无迹粒子滤波的auv协同导航方法 |
Non-Patent Citations (4)
| Title |
|---|
| 徐博等: "多AUV协同导航问题的研究现状与进展", 《自动化学报》 * |
| 房新鹏等: "双领航多自主水下航行器移动长基线定位最优队形研究", 《兵工学报》 * |
| 高伟等: "基于双主交替领航的多AUV协同导航方法", 《哈尔滨工程大学学报》 * |
| 高伟等: "基于双领航者的多AUV协同导航***可观测性分析", 《***工程与电子技术》 * |
Cited By (16)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111307136B (zh) * | 2020-02-12 | 2021-08-27 | 哈尔滨工程大学 | 一种双智能水下机器人水下航行地形匹配导航方法 |
| CN111307136A (zh) * | 2020-02-12 | 2020-06-19 | 哈尔滨工程大学 | 一种双智能水下机器人水下航行地形匹配导航方法 |
| CN111928851B (zh) * | 2020-04-17 | 2023-12-12 | 中国科学院沈阳自动化研究所 | 基于tma技术的多自主水下机器人集群协同导航方法 |
| CN111928851A (zh) * | 2020-04-17 | 2020-11-13 | 中国科学院沈阳自动化研究所 | 基于tma技术的多自主水下机器人集群协同导航方法 |
| CN111595348A (zh) * | 2020-06-23 | 2020-08-28 | 南京信息工程大学 | 一种自主水下航行器组合导航***的主从式协同定位方法 |
| CN112697145A (zh) * | 2020-11-17 | 2021-04-23 | 武汉第二船舶设计研究所(中国船舶重工集团公司第七一九研究所) | 一种基于ckf的主从式多水下无人潜器协同定位方法 |
| CN112818819A (zh) * | 2021-01-28 | 2021-05-18 | 青岛澎湃海洋探索技术有限公司 | 一种基于动态模型和复杂网络理论的auv状态监测方法 |
| CN112985391A (zh) * | 2021-04-19 | 2021-06-18 | 中国人民解放军国防科技大学 | 一种基于惯性和双目视觉的多无人机协同导航方法和装置 |
| CN113324547A (zh) * | 2021-05-25 | 2021-08-31 | 江苏科技大学 | 基于迭代扩展rts平滑滤波算法的多auv协同定位方法 |
| CN114136323A (zh) * | 2021-11-29 | 2022-03-04 | 中国船舶重工集团公司第七0七研究所 | 基于运动补偿下tdoa量测的auv协同导航方法 |
| CN114370878B (zh) * | 2022-01-04 | 2023-10-27 | 江苏科技大学 | 一种基于stackf的多auv协同定位方法 |
| CN114370878A (zh) * | 2022-01-04 | 2022-04-19 | 江苏科技大学 | 一种基于stackf的多auv协同定位方法 |
| CN114840003A (zh) * | 2022-03-21 | 2022-08-02 | 哈尔滨工程大学 | 一种单领航多auv协同定位及轨迹跟踪控制方法 |
| CN114995405A (zh) * | 2022-05-19 | 2022-09-02 | 同济大学 | 一种基于开放动态环境多目标协同理论的agv协作搬运方法 |
| CN116680500A (zh) * | 2023-06-12 | 2023-09-01 | 哈尔滨工程大学 | 水下航行器在非高斯噪声干扰下的位置估计方法及*** |
| CN116680500B (zh) * | 2023-06-12 | 2024-03-22 | 哈尔滨工程大学 | 水下航行器在非高斯噪声干扰下的位置估计方法及*** |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109974706B (zh) | 2021-05-11 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109974706A (zh) | 一种基于双运动模型的主从式多auv协同导航方法 | |
| CN109459040B (zh) | 基于rbf神经网络辅助容积卡尔曼滤波的多auv协同定位方法 | |
| Soares et al. | Joint ASV/AUV range-based formation control: Theory and experimental results | |
| Li et al. | Robust Student’s $ t $-Based Cooperative Navigation for Autonomous Underwater Vehicles | |
| CN105823480A (zh) | 基于单信标的水下移动目标定位算法 | |
| Xu et al. | A novel self-adapting filter based navigation algorithm for autonomous underwater vehicles | |
| Song et al. | Long-term inertial navigation aided by dynamics of flow field features | |
| CN102052923A (zh) | 一种小型水下机器人组合导航***及导航方法 | |
| Fan et al. | An advanced cooperative positioning algorithm based on improved factor graph and sum-product theory for multiple AUVs | |
| CN111595348A (zh) | 一种自主水下航行器组合导航***的主从式协同定位方法 | |
| Soares et al. | Triangular formation control using range measurements: An application to marine robotic vehicles | |
| Bo et al. | Cooperative localisation of AUVs based on Huber-based robust algorithm and adaptive noise estimation | |
| CN110186461A (zh) | 一种基于重力梯度信息测距的协同导航方法 | |
| CN109443356A (zh) | 一种含测量噪声的无人船位置与速度估计结构及设计方法 | |
| CN112945224A (zh) | 一种采用因子图和和积算法的多auv协同导航方法 | |
| Allotta et al. | Localization algorithm for a fleet of three AUVs by INS, DVL and range measurements | |
| CN112241589B (zh) | 一种基于***可观测度的多水下无人艇协同运动编队设计方法 | |
| Song et al. | FACON: A flow-aided cooperative navigation scheme | |
| Eickstedt et al. | Cooperative target tracking in a distributed autonomous sensor network | |
| Yuan et al. | Reaearch on underwater integrated navigation system based on SINS/DVL/magnetometer/depth-sensor | |
| CN110764126B (zh) | 一种gps信息缺失情况下的无人车导航方法及*** | |
| XU et al. | Low-cost multi-AUV cooperative localization method based on dual-model | |
| Garraffa et al. | Localization Based on Parallel Robots Kinematics As an Alternative to Trilateration | |
| Yan et al. | Cooperative navigation in unmanned surface vehicles with observability and trilateral positioning method | |
| Zhang et al. | Research and implementation on multi-beacon aided AUV integrated navigation algorithm |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |