CN114265047A - 一种大潜深auv的定位基阵联合标校方法 - Google Patents

一种大潜深auv的定位基阵联合标校方法 Download PDF

Info

Publication number
CN114265047A
CN114265047A CN202111675884.7A CN202111675884A CN114265047A CN 114265047 A CN114265047 A CN 114265047A CN 202111675884 A CN202111675884 A CN 202111675884A CN 114265047 A CN114265047 A CN 114265047A
Authority
CN
China
Prior art keywords
auv
positioning
heading
underwater
point set
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
Application number
CN202111675884.7A
Other languages
English (en)
Other versions
CN114265047B (zh
Inventor
张强
牛伯城
曹旭东
黄传智
俞泽天
张雯
马腾
李晔
姜言清
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Harbin Engineering University
Original Assignee
Harbin Engineering University
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Harbin Engineering University filed Critical Harbin Engineering University
Priority to CN202111675884.7A priority Critical patent/CN114265047B/zh
Publication of CN114265047A publication Critical patent/CN114265047A/zh
Application granted granted Critical
Publication of CN114265047B publication Critical patent/CN114265047B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
    • Y02A90/00—Technologies having an indirect contribution to adaptation to climate change
    • Y02A90/30—Assessment of water resources

Landscapes

  • Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)

Abstract

一种大潜深AUV的定位基阵联合标校方法，属于AUV水下声学导航定位领域，本发明为解决大潜深自主水下机器人利用声学基元进校水下定位时基阵校准困难、校阵准确度低的问题。它包括：大潜深AUV下潜至指定深度后母船利用超短基线定位系统AUV位置进行修正；AUV按预置梳状扫测路径开始作业，同时探测水下声学定位基阵中基元的距离和方位信息；AUV利用强跟踪无迹卡尔曼滤波算法深度融合自身导航系统信息和水声定位基元的距离与方位信息，扫测作业完成水声基阵中各基元位置自主标校，并利用基元位置标校信息修正自身导航系统误差。本发明适用于AUV的长时序水声定位。

Description

一种大潜深AUV的定位基阵联合标校方法
技术领域
本发明涉及一种大潜深AUV(Autonomous Underwater Vehicle，自主水下机器人)的定位基阵联合标校方法，属于AUV水下声学导航领域。
背景技术
随着人类对海洋资源的不断重视和进行不断地探索和开发，水下机器人领域得到了极大的发展，在水下机器人关键技术中，水下环境中机器人作业所需的定位技术是水下机器人发展的关键技术之一。在水下环境中，目前水下定位方式主要基于水声定位系统和惯性导航系统，水声定位系统作为一种重要的水下定位技术，目前主要以长基线、短基线和超短基线三种水声定位系统为代表，目前在水下机器人领域已经得到了广泛的应用。
在水下声学定位系统中，长基线水声定位系统在定位精度方面有更大的优势，在实际使用中，首先要布放多个水下基元构成基阵，基阵中基元之间的距离即基线长度通常在几百米到几千米，获取各个水下基阵的基元坐标后，基阵中各个基元测量水下机器人到各个基元之间的距离从而计算水下机器人位置。长基线水下声学定位系统具有明显的优势，但是在实际使用中，对长基线进行基阵标校时，尤其在深海环境中，由母船标校基阵中各个基元的位置相对较为困难，而且实际情况下标校精度也受到明显的影响。
发明内容
本发明目的是为了解决大潜深AUV(Autonomous Underwater Vehicle，自主水下机器人)利用声学基元进校水下定位时基阵校准困难、校阵准确度低的问题，本发明提供了一种大潜深AUV的定位基阵联合标校方法。
本发明所述一种大潜深AUV的定位基阵联合标校方法，它包括：
S1、水面母船依次布放水下声学定位基阵中的各个基元和AUV(AutonomousUnderwater Vehicle，自主水下机器人)，待AUV下潜至指定深度后，水面母船利用超短基线定位系统对AUV位置进行修正后撤离，AUV以该位置信息为原点建立北东地坐标系；
S2、AUV按预置梳状扫测路径开始作业，同时探测水下声学定位基阵中基元的距离和方位信息；
S3、AUV在S1所述深度处按照S2所述的路径进行运动，定义初始时刻状态量
Figure BDA0003451280090000011
及其协方差矩阵P0，所述初始时刻即0时刻；
S4、在k-1时刻利用强跟踪无迹卡尔曼滤波算法进行时间更新，计算k时刻的系统的先验状态量
Figure BDA0003451280090000021
和协方差矩阵Pk|k-1；
S5、当AUV探测水下基元时：
如果未探测到基元，仅进行AUV速度和艏向的量测更新；
如果探测到状态量中所表示的第i个基元，则进行AUV速度、艏向和涉及第i个基元的量测更新；
如果探测到状态量中不存在的基元，计算该基元在此时AUV载体系XOY平面上的极坐标参数[ρ,θ]，将该基元位置信息扩增至状态向量中，并扩增协方差矩阵，同时进行AUV速度和艏向的量测更新；
S6、AUV探测到所有基元后，结束较阵作业。
优选的，S3所述初始时刻状态量为：
Figure BDA0003451280090000022
xv0、yv0分别为初始时刻AUV的X坐标和Y坐标，vx0、vy0分别为初始时刻AUV载体系下的X轴线速度和Y轴线速度，ψv0为初始时刻AUV艏向角；
协方差矩阵P0为：
Figure BDA0003451280090000023
其中，
Figure BDA0003451280090000024
和
Figure BDA0003451280090000025
分别表示xv0、yv0、vx0、vy0和ψv0的初始方差。
优选的，S4所述利用强跟踪无迹卡尔曼滤波算法进行时间更新的具体方法包括：
时间更新所用系统方程如下：
Figure BDA0003451280090000031
Figure BDA0003451280090000032
其中，
Figure BDA0003451280090000033
表示k-1时刻的系统的状态量，
Figure BDA0003451280090000034
表示k时刻的先验状态估计，f(·)表示系统方程；
xv、yv分别为AUV的X坐标和Y坐标，vx、vy分别表示当前时刻AUV载体系下的X轴线速度和Y轴线速度，ψv为当前时刻AUV艏向角，xi、yi，i＝1,2,…,n分别表示Xk内存储的第i个特征点的X坐标和Y坐标，所述特征点即基元，下标k和k-1表示状态对应的时刻，w表示系统方程的过程噪声，w的协方差矩阵为Q；
进行Sigma点采样：
根据
Figure BDA0003451280090000035
和Pk|k-1采取采样策略得到k时刻状态Xk估计的sigma点集{χi}k-1，(i＝1…L)；
将Sigma点集{χi}k-1代入系统方程f(·)传播得到时间更新后的点集{χi}k|k-1，由χi,k|k-1计算得状态向量Xk的一步预测估计
Figure BDA0003451280090000036
和一步误差协方差阵预测估计Pk|k-1：
χi,k|k-1＝f(χi,k-1)
Figure BDA0003451280090000037
Figure BDA0003451280090000038
Figure BDA0003451280090000039
式中，
Figure BDA0003451280090000041
为求一阶统计特性时的权系数；
Figure BDA0003451280090000042
为求二阶统计特性时的权系数；χi,k-1为Sigma点集{χi}k-1中的点；χi,k|k-1为Sigma点集χi,k|k-1中的点；Pxx为中间变量。4、根据权利要求3所述的一种大潜深AUV的定位基阵联合标校方法，其特征在于，S5所述的仅进行AUV速度和艏向的量测更新，具体包括：
量测方程
Figure BDA0003451280090000043
表示为：
Figure BDA0003451280090000044
观测量为z＝[vx_sensor vy_sensor ψv_sensor]T；
其中，v1为观测噪声，其协方差为R1，vx_sensor、vy_sensor和ψv_sensor分别表示传感器测得的AUV载体系下的X轴线速度、Y轴线速度和AUV艏向角。
优选的，S5所述的进行AUV速度、艏向和涉及第i个特征点的量测更新，具体包括：
量测方程zk+1表示为：
Figure BDA0003451280090000045
观测量为
Figure BDA0003451280090000046
其中，v2为观测噪声，其协方差矩阵为R2，d为声呐所探测到的基元到AUV的距离在XOY平面上的投影，
Figure BDA0003451280090000047
为声呐所探测基元相对于AUV艏向的方位角，arctan2为反三角函数。
优选的，S5所述的仅进行AUV速度和艏向的量测更新或进行AUV速度、艏向和涉及第i个特征点的量测更新，量测方程的更新方法包括：
计算Sigma点集{χi}k-1、{χi}k|k-1通过非线性量测方程的传播，获得Sigma点集{δi}k|k-1、{δi}k-1：
δi,k|k-1＝h(χi,k|k-1)；
δi,k|k-1表示Sigma点集{δi}k|k-1中的点；
δi,k-1＝h(χi,k-1)；
δi,k-1表示什么Sigma点集{δi}k-1中的点；
分别计算δi,k|k-1、δi,k-1的相应均值
Figure BDA0003451280090000051
Figure BDA0003451280090000052
Figure BDA0003451280090000053
计算{δi}k-1的协方差：
Figure BDA0003451280090000054
计算{δi}k-1和{χi}k-1的协方差：
Figure BDA0003451280090000055
计算{δi}k-1和{δi}k|k-1的协方差：
Figure BDA0003451280090000056
进一步计算获得协方差Pk：
Figure BDA0003451280090000057
式中
Figure BDA0003451280090000058
In为n维的单位矩阵；
根据Pk，
Figure BDA0003451280090000059
采取采样策略计算Sigma点集{χi}k；
计算Sigma点集{χi}k通过非线性量测方程的传播：
δi,k|＝h(χi,k)
Figure BDA0003451280090000061
Figure BDA0003451280090000062
Figure BDA0003451280090000063
进行量测方程的更新：
Kk＝Pxz(I+Pzz)-1
Figure BDA0003451280090000064
优选的，S5所述将该基元位置信息扩增至状态向量中，并扩增协方差矩阵，具体包括：
状态向量的矩阵扩增方程为：
Figure BDA0003451280090000065
其中，d为声呐探测到的基元到AUV的距离在XOY平面上的投影，
Figure BDA0003451280090000066
为声呐所探测到的基元相对于AUV艏向的方位角；
计算雅可比矩阵：
Figure BDA0003451280090000067
协方差的矩阵扩增方程为：
Figure BDA0003451280090000068
本发明的优点：本发明提出的一种大潜深自主水下机器人的定位基阵联合标校方法，能够实现大潜深的水下基阵标校，并提高标校的精度。
附图说明
图1是本发明所述一种大潜深自主水下机器人的定位基阵联合标校方法的流程框图；
图2是采用AUV进行较阵作业的示意图，其中：a表示基元，b表示AUV，c表示AUV预置梳状扫测路径。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。
需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。
具体实施方式一：下面结合图1说明本实施方式，本实施方式所述一种大潜深自主水下机器人(AUV)的定位基阵联合标校方法，它包括：
S1、水面母船依次布放水下声学定位基阵中的各个基元和AUV，待AUV下潜至指定深度后，水面母船利用超短基线定位系统对AUV位置进行修正后撤离，AUV以该位置信息为原点建立北东地坐标系；
S2、AUV按预置梳状扫测路径开始作业，同时探测水下声学定位基阵中基元的距离和方位信息；
S3、AUV在S1所述深度处按照S2所述的路径进行运动，定义初始时刻状态量
Figure BDA0003451280090000071
及其协方差矩阵P0，所述初始时刻即0时刻；
S4、在k-1时刻利用强跟踪无迹卡尔曼滤波算法进行时间更新，计算k时刻的系统的先验状态量
Figure BDA0003451280090000072
和协方差矩阵Pk|k-1；
S5、当AUV探测水下基元时：
如果未探测到基元，仅进行AUV速度和艏向的量测更新；
如果探测到状态量中所表示的第i个基元，则进行AUV速度、艏向和涉及第i个基元的量测更新；
如果探测到状态量中不存在的基元，计算该基元在此时AUV载体系XOY平面上的极坐标参数[ρ,θ]，将该基元位置信息扩增至状态向量中，并扩增协方差矩阵，同时进行AUV速度和艏向的量测更新；
S6、AUV探测到所有基元后，结束较阵作业。
具体实施方式二：本实施方式对具体实施方式一作进一步说明，S3所述初始时刻状态量为：
Figure BDA0003451280090000073
xv0、yv0分别为初始时刻AUV的X坐标和Y坐标，vx0、vy0分别为初始时刻AUV载体系下的X轴线速度和Y轴线速度，ψv0为初始时刻AUV艏向角；
协方差矩阵P0为：
Figure BDA0003451280090000081
其中，
Figure BDA0003451280090000082
和
Figure BDA0003451280090000083
分别表示xv0、yv0、vx0、vy0和ψv0的初始方差。
具体实施方式三：本实施方式对具体实施方式二作进一步说明，S4所述利用强跟踪无迹卡尔曼滤波算法进行时间更新的具体方法包括：
时间更新所用系统方程如下：
Figure BDA0003451280090000084
Figure BDA0003451280090000085
其中，
Figure BDA0003451280090000086
表示k-1时刻的系统的状态量，
Figure BDA0003451280090000087
表示k时刻的先验状态估计，f(·)表示系统方程；
xv、yv分别为AUV的X坐标和Y坐标，vx、vy分别表示当前时刻AUV载体系下的X轴线速度和Y轴线速度，ψv为当前时刻AUV艏向角，xi、yi，i＝1,2,…,n分别表示Xk内存储的第i个特征点的X坐标和Y坐标，所述特征点即基元，下标k和k-1表示状态对应的时刻，w表示系统方程的过程噪声，w的协方差矩阵为Q；
进行Sigma点采样：
根据
Figure BDA0003451280090000091
和Pk|k-1采取采样策略得到k时刻状态Xk估计的sigma点集{χi}k-1，(i＝1…L)；
将Sigma点集{χi}k-1代入系统方程f(·)传播得到时间更新后的点集{χi}k|k-1，由χi,k|k-1计算得状态向量Xk的一步预测估计
Figure BDA0003451280090000092
和一步误差协方差阵预测估计Pk|k-1：
χi,k|k-1＝f(χi,k-1)
Figure BDA0003451280090000093
Figure BDA0003451280090000094
Figure BDA0003451280090000095
式中，
Figure BDA0003451280090000096
为求一阶统计特性时的权系数；
Figure BDA0003451280090000097
为求二阶统计特性时的权系数；χi,k-1为Sigma点集{χi}k-1中的点；χi,k|k-1为Sigma点集χi,k|k-1中的点；Pxx为中间变量。
具体实施方式四：本实施方式对具体实施方式三作进一步说明，S5所述的仅进行AUV速度和艏向的量测更新，具体包括：
量测方程
Figure BDA0003451280090000098
表示为：
Figure BDA0003451280090000099
观测量为z＝[vx_sensor vy_sensor ψv_sensor]T；
其中，v1为观测噪声，其协方差为R1，vx_sensor、vy_sensor和ψv_sensor分别表示传感器测得的AUV载体系下的X轴线速度、Y轴线速度和AUV艏向角。
具体实施方式五：本实施方式对具体实施方式四作进一步说明，S5所述的进行AUV速度、艏向和涉及第i个特征点的量测更新，具体包括：
量测方程zk+1表示为：
Figure BDA0003451280090000101
观测量为
Figure BDA0003451280090000102
其中，v2为观测噪声，其协方差矩阵为R2，d为声呐所探测到的基元到AUV的距离在XOY平面上的投影，
Figure BDA0003451280090000103
为声呐所探测基元相对于AUV艏向的方位角，arctan2为反三角函数。
具体实施方式六：本实施方式对具体实施方式四或五作进一步说明，S5所述的仅进行AUV速度和艏向的量测更新或进行AUV速度、艏向和涉及第i个特征点的量测更新，量测方程的更新方法包括：
计算Sigma点集{χi}k-1、{χi}k|k-1通过非线性量测方程的传播，获得Sigma点集{δi}k|k-1、{δi}k-1：
δi,k|k-1＝h(χi,k|k-1)；
δi,k|k-1表示Sigma点集{δi}k|k-1中的点；
δi,k-1＝h(χi,k-1)；
δi,k-1表示什么Sigma点集{δi}k-1中的点；
分别计算δi,k|k-1、δi,k-1的相应均值
Figure BDA0003451280090000104
Figure BDA0003451280090000105
Figure BDA0003451280090000106
计算{δi}k-1的协方差：
Figure BDA0003451280090000107
计算{δi}k-1和{χi}k-1的协方差：
Figure BDA0003451280090000111
计算{δi}k-1和{δi}k|k-1的协方差：
Figure BDA0003451280090000112
进一步计算获得协方差Pk：
Figure BDA0003451280090000113
式中
Figure BDA0003451280090000114
In为n维的单位矩阵；
根据Pk，
Figure BDA0003451280090000115
采取采样策略计算Sigma点集{χi}k；
计算Sigma点集{χi}k通过非线性量测方程的传播：
δi,k|＝h(χi,k)
Figure BDA0003451280090000116
Figure BDA0003451280090000117
Figure BDA0003451280090000118
进行量测方程的更新：
Kk＝Pxz(I+Pzz)-1
Figure BDA0003451280090000119
具体实施方式七：本实施方式对具体实施方式六作进一步说明，S5所述将该基元位置信息扩增至状态向量中，并扩增协方差矩阵，具体包括：
状态向量的矩阵扩增方程为：
Figure BDA0003451280090000121
其中，d为声呐探测到的基元到AUV的距离在XOY平面上的投影，
Figure BDA0003451280090000122
为声呐所探测到的基元相对于AUV艏向的方位角；
计算雅可比矩阵：
Figure BDA0003451280090000123
协方差的矩阵扩增方程为：
Figure BDA0003451280090000124
虽然在本文中参照了特定的实施方式来描述本发明，但是应该理解的是，这些实施例仅仅是本发明的原理和应用的示例。因此应该理解的是，可以对示例性的实施例进行许多修改，并且可以设计出其他的布置，只要不偏离所附权利要求所限定的本发明的精神和范围。应该理解的是，可以通过不同于原始权利要求所描述的方式来结合不同的从属权利要求和本文中所述的特征。还可以理解的是，结合单独实施例所描述的特征可以使用在其他所述实施例中。

Claims (7)

1.一种大潜深AUV的定位基阵联合标校方法，其特征在于，它包括：
S1、水面母船依次布放水下声学定位基阵中的各个基元和AUV，待AUV下潜至指定深度后，水面母船利用超短基线定位系统对AUV位置进行修正后撤离，AUV以该位置信息为原点建立北东地坐标系；
S2、AUV按预置梳状扫测路径开始作业，同时探测水下声学定位基阵中基元的距离和方位信息；
S3、AUV在S1所述深度处按照S2所述的路径进行运动，定义初始时刻状态量
Figure FDA0003451280080000011
及其协方差矩阵P0，所述初始时刻即0时刻；
S4、在k-1时刻利用强跟踪无迹卡尔曼滤波算法进行时间更新，计算k时刻的系统的先验状态量
Figure FDA0003451280080000012
和协方差矩阵Pk|k-1；
S5、当AUV探测水下基元时：
如果未探测到基元，仅进行AUV速度和艏向的量测更新；
如果探测到状态量中所表示的第i个基元，则进行AUV速度、艏向和涉及第i个基元的量测更新；
如果探测到状态量中不存在的基元，计算该基元在此时AUV载体系XOY平面上的极坐标参数[ρ,θ]，将该基元位置信息扩增至状态向量中，并扩增协方差矩阵，同时进行AUV速度和艏向的量测更新；
S6、AUV探测到所有基元后，结束较阵作业。
2.根据权利要求1所述的一种大潜深AUV的定位基阵联合标校方法，其特征在于，S3所述初始时刻状态量为：
Figure FDA0003451280080000013
xv0、yv0分别为初始时刻AUV的X坐标和Y坐标，vx0、vy0分别为初始时刻AUV载体系下的X轴线速度和Y轴线速度，ψv0为初始时刻AUV艏向角；
协方差矩阵P0为：
Figure FDA0003451280080000014
其中，
Figure FDA0003451280080000015
和
Figure FDA0003451280080000016
分别表示xv0、yv0、vx0、vy0和ψv0的初始方差。
3.根据权利要求2所述的一种大潜深AUV的定位基阵联合标校方法，其特征在于，S4所述利用强跟踪无迹卡尔曼滤波算法进行时间更新的具体方法包括：
时间更新所用系统方程如下：
Figure FDA0003451280080000021
Figure FDA0003451280080000022
其中，
Figure FDA0003451280080000023
表示k-1时刻的系统的状态量，
Figure FDA0003451280080000024
表示k时刻的先验状态估计，f(·)表示系统方程；
xv、yv分别为AUV的X坐标和Y坐标，vx、vy分别表示当前时刻AUV载体系下的X轴线速度和Y轴线速度，ψv为当前时刻AUV艏向角，xi、yi，i＝1,2,…,n分别表示Xk内存储的第i个特征点的X坐标和Y坐标，所述特征点即基元，下标k和k-1表示状态对应的时刻，w表示系统方程的过程噪声，w的协方差矩阵为Q；
进行Sigma点采样：
根据
Figure FDA0003451280080000025
和Pk|k-1采取采样策略得到k时刻状态Xk估计的sigma点集{χi}k-1，(i＝1…L)；
将Sigma点集{χi}k-1代入系统方程f(·)传播得到时间更新后的点集{χi}k|k-1，由χi,k|k-1计算得状态向量Xk的一步预测估计
Figure FDA0003451280080000026
和一步误差协方差阵预测估计Pk|k-1：
χi,k|k-1＝f(χi,k-1)
Figure FDA0003451280080000027
Figure FDA0003451280080000031
Figure FDA0003451280080000032
式中，
Figure FDA0003451280080000033
为求一阶统计特性时的权系数；
Figure FDA0003451280080000034
为求二阶统计特性时的权系数；χi,k-1为Sigma点集{χi}k-1中的点；χi,k|k-1为Sigma点集χi,k|k-1中的点；Pxx为中间变量。
4.根据权利要求3所述的一种大潜深AUV的定位基阵联合标校方法，其特征在于，S5所述的仅进行AUV速度和艏向的量测更新，具体包括：
量测方程
Figure FDA0003451280080000035
表示为：
Figure FDA0003451280080000036
观测量为z＝[vx_sensor vy_sensor ψv_sensor]T；
其中，v1为观测噪声，其协方差为R1，vx_sensor、vy_sensor和ψv_sensor分别表示传感器测得的AUV载体系下的X轴线速度、Y轴线速度和AUV艏向角。
5.根据权利要求4所述的一种大潜深AUV的定位基阵联合标校方法，其特征在于，S5所述的进行AUV速度、艏向和涉及第i个特征点的量测更新，具体包括：
量测方程zk+1表示为：
Figure FDA0003451280080000037
观测量为
Figure FDA0003451280080000038
其中，v2为观测噪声，其协方差矩阵为R2，d为声呐所探测到的基元到AUV的距离在XOY平面上的投影，
Figure FDA0003451280080000041
为声呐所探测基元相对于AUV艏向的方位角，arctan2为反三角函数。
6.根据权利要求4或5所述的一种大潜深AUV的定位基阵联合标校方法，其特征在于，S5所述的仅进行AUV速度和艏向的量测更新或进行AUV速度、艏向和涉及第i个特征点的量测更新，量测方程的更新方法包括：
计算Sigma点集{χi}k-1、{χi}k|k-1通过非线性量测方程的传播，获得Sigma点集{δi}k|k-1、{δi}k-1：
δi,k|k-1＝h(χi,k|k-1)；
δi,k|k-1表示Sigma点集{δi}k|k-1中的点；
δi,k-1＝h(χi,k-1)；
δi,k-1表示什么Sigma点集{δi}k-1中的点；
分别计算δi,k|k-1、δi,k-1的相应均值
Figure FDA0003451280080000042
Figure FDA0003451280080000043
Figure FDA0003451280080000044
计算{δi}k-1的协方差：
Figure FDA0003451280080000045
计算{δi}k-1和{χi}k-1的协方差：
Figure FDA0003451280080000046
计算{δi}k-1和{δi}k|k-1的协方差：
Figure FDA0003451280080000047
进一步计算获得协方差Pk：
Figure FDA0003451280080000051
式中
Figure FDA0003451280080000052
In为n维的单位矩阵；
根据Pk，
Figure FDA0003451280080000053
采取采样策略计算Sigma点集{χi}k；
计算Sigma点集{χi}k通过非线性量测方程的传播：
δi,k|＝h(χi,k)
Figure FDA0003451280080000054
Figure FDA0003451280080000055
Figure FDA0003451280080000056
进行量测方程的更新：
Kk＝Pxz(I+Pzz)-1
Figure FDA0003451280080000057
7.根据权利要求6所述的一种大潜深AUV的定位基阵联合标校方法，其特征在于，S5所述将该基元位置信息扩增至状态向量中，并扩增协方差矩阵，具体包括：
状态向量的矩阵扩增方程为：
Figure FDA0003451280080000058
其中，d为声呐探测到的基元到AUV的距离在XOY平面上的投影，
Figure FDA0003451280080000059
为声呐所探测到的基元相对于AUV艏向的方位角；
计算雅可比矩阵：
Figure FDA00034512800800000510
协方差的矩阵扩增方程为：
Figure FDA00034512800800000511
CN202111675884.7A 2021-12-31 2021-12-31 一种大潜深auv的定位基阵联合标校方法 Active CN114265047B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111675884.7A CN114265047B (zh) 2021-12-31 2021-12-31 一种大潜深auv的定位基阵联合标校方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111675884.7A CN114265047B (zh) 2021-12-31 2021-12-31 一种大潜深auv的定位基阵联合标校方法

Publications (2)

Publication Number Publication Date
CN114265047A true CN114265047A (zh) 2022-04-01
CN114265047B CN114265047B (zh) 2022-11-01

Family

ID=80832392

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111675884.7A Active CN114265047B (zh) 2021-12-31 2021-12-31 一种大潜深auv的定位基阵联合标校方法

Country Status (1)

Country Link
CN (1) CN114265047B (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116164747A (zh) * 2022-12-15 2023-05-26 广东智能无人系统研究院(南沙) 一种水下机器人的定位与导航方法及系统
CN116840784A (zh) * 2023-05-16 2023-10-03 中国人民解放军91550部队 一种基于多平台的水下定点目标快速标校系统及方法

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR20130024077A (ko) * 2011-08-30 2013-03-08 주식회사 한화 자율 무인 잠수정의 해저면 위치 보정 방법 및 자율 무인 잠수정의 위치 측정 장치
CN106546956A (zh) * 2016-10-25 2017-03-29 中国海洋石油总公司 一种超短基线接收基阵基元位置的精确标定方法
CN107132520A (zh) * 2017-05-03 2017-09-05 江苏中海达海洋信息技术有限公司 一种基于水声超短基线定位系统的声线修正方法及系统
CN108802737A (zh) * 2018-05-25 2018-11-13 哈尔滨工程大学 一种用于浅水域水下定位的超短基线系统及校准方法
CN109613520A (zh) * 2018-12-14 2019-04-12 东南大学 一种基于滤波的超短基线安装误差在线标定方法
CN113156369A (zh) * 2021-04-26 2021-07-23 哈尔滨工程大学 一种海上声学相位中心校准方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR20130024077A (ko) * 2011-08-30 2013-03-08 주식회사 한화 자율 무인 잠수정의 해저면 위치 보정 방법 및 자율 무인 잠수정의 위치 측정 장치
CN106546956A (zh) * 2016-10-25 2017-03-29 中国海洋石油总公司 一种超短基线接收基阵基元位置的精确标定方法
CN107132520A (zh) * 2017-05-03 2017-09-05 江苏中海达海洋信息技术有限公司 一种基于水声超短基线定位系统的声线修正方法及系统
CN108802737A (zh) * 2018-05-25 2018-11-13 哈尔滨工程大学 一种用于浅水域水下定位的超短基线系统及校准方法
CN109613520A (zh) * 2018-12-14 2019-04-12 东南大学 一种基于滤波的超短基线安装误差在线标定方法
CN113156369A (zh) * 2021-04-26 2021-07-23 哈尔滨工程大学 一种海上声学相位中心校准方法

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
ZHANG QIANG: "Range-Only Navigation Algorithm for Positioning of Deep-Diving AUV", 《2017 IEEE INTERNATIONAL CONFERENCE ON CYBERNETICS AND INTELLIGENT SYSTEMS (CIS) AND IEEE CONFERENCE ON ROBOTICS, AUTOMATION AND MECHATRONICS (RAM)》 *
刘百峰 等: "一种适用长基线系统绝对测阵的新航路", 《舰船电子工程》 *
张强 等: "大潜深AUV下潜定位误差纯距离修正算法研究", 《第35届中国控制会议论文集》 *
王先华: "长基线水下导航定位系统测阵校阵及系统集成", 《中国优秀博硕士学位论文全文数据库(硕士)工程科技Ⅱ辑》 *
翟笃文: "长基线导航系统阵元位置综合校准关键技术研究", 《中国优秀博硕士学位论文全文数据库(硕士)基础科学辑》 *

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116164747A (zh) * 2022-12-15 2023-05-26 广东智能无人系统研究院(南沙) 一种水下机器人的定位与导航方法及系统
CN116164747B (zh) * 2022-12-15 2023-09-05 广东智能无人系统研究院(南沙) 一种水下机器人的定位与导航方法及系统
CN116840784A (zh) * 2023-05-16 2023-10-03 中国人民解放军91550部队 一种基于多平台的水下定点目标快速标校系统及方法
CN116840784B (zh) * 2023-05-16 2024-04-30 中国人民解放军91550部队 一种基于多平台的水下定点目标快速标校系统及方法

Also Published As

Publication number Publication date
CN114265047B (zh) 2022-11-01

Similar Documents

Publication Publication Date Title
CN114265047B (zh) 一种大潜深auv的定位基阵联合标校方法
CN109737956B (zh) 一种基于双应答器的sins/usbl相位差紧组合导航定位方法
CN110132308B (zh) 一种基于姿态确定的usbl安装误差角标定方法
Karimi et al. A comparison of DVL/INS fusion by UKF and EKF to localize an autonomous underwater vehicle
CN113819906A (zh) 一种基于统计相似度量的组合导航鲁棒滤波方法
CN105737832A (zh) 基于全局最优数据融合的分布式slam方法
CN107966145B (zh) 一种基于稀疏长基线紧组合的auv水下导航方法
CN103697910A (zh) 自主水下航行器多普勒计程仪安装误差的校正方法
CN111750865B (zh) 一种用于双功能深海无人潜器导航系统的自适应滤波导航方法
CN111366156A (zh) 基于神经网络辅助的变电站巡检机器人导航方法及系统
CN115307643A (zh) 一种双应答器辅助的sins/usbl组合导航方法
CN110703205B (zh) 基于自适应无迹卡尔曼滤波的超短基线定位方法
CN112859133A (zh) 一种基于雷达与北斗数据的船舶深度融合定位方法
CN115979253A (zh) 一种基于抗差滤波的水下机器人多传感器紧组合导航方法
Braginsky et al. Correction of DVL error caused by seafloor gradient
CN112666519B (zh) 一种基于广义二阶时延差的水下目标高精度定位方法
KR100879701B1 (ko) 무향 칼만 필터를 이용한 동시 위치 추정 및 지도 작성시스템 및 방법
CN112083425B (zh) 一种引入径向速度的sins/lbl紧组合导航方法
CN111721284B (zh) 一种无源模式下的sins/usbl组合导航定位方法
CN111964684B (zh) 一种基于sins/lbl紧组合的水下导航混合定位方法及系统
Belge et al. Sensor fusion based on integrated navigation data of sea surface vehicle with machine learning method
CN116772903B (zh) 基于迭代ekf的sins/usbl安装角估计方法
CN114370879B (zh) 基于水下环境特征的auv鲁棒vbhiakf-slam导航方法
CN112445243B (zh) 一种适用于自主水下机器人的海底目标搜寻方法
Pan et al. AUV Tightly Coupled Terrain Aided Navigation Strategy Based on Isogonal MBES Modeling Method

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