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

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

Info

Publication number
CN114265047B
CN114265047B CN202111675884.7A CN202111675884A CN114265047B CN 114265047 B CN114265047 B CN 114265047B CN 202111675884 A CN202111675884 A CN 202111675884A CN 114265047 B CN114265047 B CN 114265047B
Authority
CN
China
Prior art keywords
auv
heading
underwater
equation
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.)
Active
Application number
CN202111675884.7A
Other languages
English (en)
Other versions
CN114265047A (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

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 GDA0003843765380000011
及其协方差矩阵P0，所述初始时刻即0时刻；
S4、在k-1时刻利用强跟踪无迹卡尔曼滤波算法进行时间更新，计算k时刻的系统的先验状态量
Figure GDA0003843765380000021
和协方差矩阵Pk|k-1；
S5、当AUV探测水下基元时：
如果未探测到基元，仅进行AUV速度和艏向的量测更新；
如果探测到状态量中所表示的第i个基元，则进行AUV速度、艏向和涉及第i个基元的量测更新；
如果探测到状态量中不存在的基元，计算该基元在此时AUV载体系XOY平面上的极坐标参数[ρ,θ]，将该基元位置信息扩增至状态向量中，并扩增协方差矩阵，同时进行AUV速度和艏向的量测更新；
S6、AUV探测到所有基元后，结束较阵作业。
优选的，S3所述初始时刻状态量为：
Figure GDA0003843765380000022
xv0、yv0分别为初始时刻AUV的X坐标和Y坐标，vx0、vy0分别为初始时刻AUV载体系下的X轴线速度和Y轴线速度，ψv0为初始时刻AUV艏向角；
协方差矩阵P0为：
Figure GDA0003843765380000023
其中，
Figure GDA0003843765380000024
和
Figure GDA0003843765380000025
分别表示xv0、yv0、vx0、vy0和ψv0的初始方差。
优选的，S4所述利用强跟踪无迹卡尔曼滤波算法进行时间更新的具体方法包括：
时间更新所用系统方程如下：
Figure GDA0003843765380000031
Figure GDA0003843765380000032
其中，
Figure GDA0003843765380000033
表示k-1时刻的系统的状态量，
Figure GDA0003843765380000034
表示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 GDA0003843765380000035
和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 GDA0003843765380000036
和一步误差协方差阵预测估计Pk|k-1：
χi,k|k-1＝f(χi,k-1)
Figure GDA0003843765380000037
Figure GDA0003843765380000038
Figure GDA0003843765380000039
式中，
Figure GDA0003843765380000041
为求一阶统计特性时的权系数；
Figure GDA0003843765380000042
为求二阶统计特性时的权系数；χi,k-1为Sigma点集{χi}k-1中的点；χi,k|k-1为Sigma点集χi,k|k-1中的点；Pxx为中间变量。
S5所述的仅进行AUV速度和艏向的量测更新，具体包括：
量测方程
Figure GDA0003843765380000043
表示为：
Figure GDA0003843765380000044
观测量为z＝[vx_sensor vy_sensor ψv_sensor]T；
其中，v1为观测噪声，其协方差为R1，vx_sensor、vy_sensor和ψv_sensor分别表示传感器测得的AUV载体系下的X轴线速度、Y轴线速度和AUV艏向角。
优选的，S5所述的进行AUV速度、艏向和涉及第i个特征点的量测更新，具体包括：
量测方程zk表示为：
Figure GDA0003843765380000045
观测量为
Figure GDA0003843765380000046
其中，v2为观测噪声，其协方差矩阵为R2，d为声呐所探测到的基元到AUV的距离在XOY平面上的投影，
Figure GDA0003843765380000047
为声呐所探测基元相对于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 GDA0003843765380000051
Figure GDA0003843765380000052
Figure GDA0003843765380000053
计算{δi}k-1的协方差：
Figure GDA0003843765380000054
计算{δi}k-1和{χi}k-1的协方差：
Figure GDA0003843765380000055
计算{χi}k|k-1和{δi}k-1的协方差：
Figure GDA0003843765380000056
进一步计算获得协方差Pk：
Figure GDA0003843765380000057
式中
Figure GDA0003843765380000058
In为n维的单位矩阵；
根据Pk，
Figure GDA0003843765380000059
采取采样策略计算Sigma点集{χi}k；
计算Sigma点集{χi}k通过非线性量测方程的传播：
δi,k|＝h(χi,k)
Figure GDA0003843765380000061
Figure GDA0003843765380000062
Figure GDA0003843765380000063
进行量测方程的更新：
Kk＝Pxz(I+Pzz)-1
Figure GDA0003843765380000064
优选的，S5所述将该基元位置信息扩增至状态向量中，并扩增协方差矩阵，具体包括：
状态向量的矩阵扩增方程为：
Figure GDA0003843765380000065
其中，d为声呐探测到的基元到AUV的距离在XOY平面上的投影，
Figure GDA0003843765380000066
为声呐所探测到的基元相对于AUV艏向的方位角；
计算雅可比矩阵：
Figure GDA0003843765380000067
协方差的矩阵扩增方程为：
Figure GDA0003843765380000068
本发明的优点：本发明提出的一种大潜深自主水下机器人的定位基阵联合标校方法，能够实现大潜深的水下基阵标校，并提高标校的精度。
附图说明
图1是本发明所述一种大潜深自主水下机器人的定位基阵联合标校方法的流程框图；
图2是采用AUV进行较阵作业的示意图，其中：a表示基元，b表示AUV，c表示AUV预置梳状扫测路径。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。
需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。
具体实施方式一：下面结合图1说明本实施方式，本实施方式所述一种大潜深自主水下机器人(AUV)的定位基阵联合标校方法，它包括：
S1、水面母船依次布放水下声学定位基阵中的各个基元和AUV，待AUV下潜至指定深度后，水面母船利用超短基线定位系统对AUV位置进行修正后撤离，AUV以该位置信息为原点建立北东地坐标系；
S2、AUV按预置梳状扫测路径开始作业，同时探测水下声学定位基阵中基元的距离和方位信息；
S3、AUV在S1所述深度处按照S2所述的路径进行运动，定义初始时刻状态量
Figure GDA0003843765380000073
及其协方差矩阵P0，所述初始时刻即0时刻；
S4、在k-1时刻利用强跟踪无迹卡尔曼滤波算法进行时间更新，计算k时刻的系统的先验状态量
Figure GDA0003843765380000071
和协方差矩阵Pk|k-1；
S5、当AUV探测水下基元时：
如果未探测到基元，仅进行AUV速度和艏向的量测更新；
如果探测到状态量中所表示的第i个基元，则进行AUV速度、艏向和涉及第i个基元的量测更新；
如果探测到状态量中不存在的基元，计算该基元在此时AUV载体系XOY平面上的极坐标参数[ρ,θ]，将该基元位置信息扩增至状态向量中，并扩增协方差矩阵，同时进行AUV速度和艏向的量测更新；
S6、AUV探测到所有基元后，结束较阵作业。
具体实施方式二：本实施方式对具体实施方式一作进一步说明，S3所述初始时刻状态量为：
Figure GDA0003843765380000072
xv0、yv0分别为初始时刻AUV的X坐标和Y坐标，vx0、vy0分别为初始时刻AUV载体系下的X轴线速度和Y轴线速度，ψv0为初始时刻AUV艏向角；
协方差矩阵P0为：
Figure GDA0003843765380000081
其中，
Figure GDA0003843765380000086
和
Figure GDA0003843765380000087
分别表示xv0、yv0、vx0、vy0和ψv0的初始方差。
具体实施方式三：本实施方式对具体实施方式二作进一步说明，S4所述利用强跟踪无迹卡尔曼滤波算法进行时间更新的具体方法包括：
时间更新所用系统方程如下：
Figure GDA0003843765380000082
Figure GDA0003843765380000083
其中，
Figure GDA0003843765380000084
表示k-1时刻的系统的状态量，
Figure GDA0003843765380000085
表示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 GDA0003843765380000091
和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 GDA0003843765380000092
和一步误差协方差阵预测估计Pk|k-1：
χi,k|k-1＝f(χi,k-1)
Figure GDA0003843765380000093
Figure GDA0003843765380000094
Figure GDA0003843765380000095
式中，
Figure GDA0003843765380000096
为求一阶统计特性时的权系数；
Figure GDA0003843765380000097
为求二阶统计特性时的权系数；χi,k-1为Sigma点集{χi}k-1中的点；χi,k|k-1为Sigma点集χi,k|k-1中的点；Pxx为中间变量。
具体实施方式四：本实施方式对具体实施方式三作进一步说明，S5所述的仅进行AUV速度和艏向的量测更新，具体包括：
量测方程
Figure GDA0003843765380000098
表示为：
Figure GDA0003843765380000099
观测量为z＝[vx_sensor vy_sensor ψv_sensor]T；
其中，v1为观测噪声，其协方差为R1，vx_sensor、vy_sensor和ψv_sensor分别表示传感器测得的AUV载体系下的X轴线速度、Y轴线速度和AUV艏向角。
具体实施方式五：本实施方式对具体实施方式四作进一步说明，S5所述的进行AUV速度、艏向和涉及第i个特征点的量测更新，具体包括：
量测方程zk表示为：
Figure GDA0003843765380000101
观测量为
Figure GDA0003843765380000102
其中，v2为观测噪声，其协方差矩阵为R2，d为声呐所探测到的基元到AUV的距离在XOY平面上的投影，
Figure GDA0003843765380000103
为声呐所探测基元相对于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 GDA0003843765380000104
Figure GDA0003843765380000105
Figure GDA0003843765380000106
计算{δi}k-1的协方差：
Figure GDA0003843765380000111
计算{δi}k-1和{χi}k-1的协方差：
Figure GDA0003843765380000112
计算{χi}k|k-1和{δi}k-1的协方差：
Figure GDA0003843765380000113
进一步计算获得协方差Pk：
Figure GDA0003843765380000114
式中
Figure GDA0003843765380000115
In为n维的单位矩阵；
根据Pk，
Figure GDA0003843765380000116
采取采样策略计算Sigma点集{χi}k；
计算Sigma点集{χi}k通过非线性量测方程的传播：
δi,k|＝h(χi,k)
Figure GDA0003843765380000117
Figure GDA0003843765380000118
Figure GDA0003843765380000119
进行量测方程的更新：
Kk＝Pxz(I+Pzz)-1
Figure GDA00038437653800001110
具体实施方式七：本实施方式对具体实施方式六作进一步说明，S5所述将该基元位置信息扩增至状态向量中，并扩增协方差矩阵，具体包括：
状态向量的矩阵扩增方程为：
Figure GDA0003843765380000121
其中，d为声呐探测到的基元到AUV的距离在XOY平面上的投影，
Figure GDA0003843765380000122
为声呐所探测到的基元相对于AUV艏向的方位角；
计算雅可比矩阵：
Figure GDA0003843765380000123
协方差的矩阵扩增方程为：
Figure GDA0003843765380000124
虽然在本文中参照了特定的实施方式来描述本发明，但是应该理解的是，这些实施例仅仅是本发明的原理和应用的示例。因此应该理解的是，可以对示例性的实施例进行许多修改，并且可以设计出其他的布置，只要不偏离所附权利要求所限定的本发明的精神和范围。应该理解的是，可以通过不同于原始权利要求所描述的方式来结合不同的从属权利要求和本文中所述的特征。还可以理解的是，结合单独实施例所描述的特征可以使用在其他所述实施例中。

Claims (2)

1.一种大潜深AUV的定位基阵联合标校方法，其特征在于，它包括：
S1、水面母船依次布放AUV和水下声学定位基阵中的各个基元，待AUV下潜至指定深度后，水面母船利用超短基线定位系统对AUV位置进行修正后撤离，AUV以该位置信息为原点建立北东地坐标系；
S2、AUV按预置梳状扫测路径开始作业，同时探测水下声学定位基阵中基元的距离和方位信息；
S3、AUV在S1所述深度处按照S2所述的路径进行运动，定义AUV的初始时刻状态量
Figure FDA0003840897420000011
及其协方差矩阵P0，所述初始时刻即0时刻；
S4、在k-1时刻利用强跟踪无迹卡尔曼滤波算法进行时间更新，计算k时刻的AUV的先验状态量
Figure FDA0003840897420000012
和协方差矩阵Pk|k-1；
S5、当AUV探测水下基元时：
如果未探测到基元，仅进行AUV速度和艏向的量测更新；
如果探测到状态量中所表示的第i个基元，则进行AUV速度、艏向和涉及第i个基元的量测更新；
如果探测到状态量中不存在的基元，计算该基元在此时AUV载体坐标系XOY平面上的极坐标参数[ρ,θ]，将该基元位置信息扩增至AUV的状态向量中，并扩增AUV的协方差矩阵，同时进行AUV速度和艏向的量测更新；
S6、AUV探测到所有基元后，结束较阵作业；
S3所述初始时刻状态量为：
Figure FDA0003840897420000013
xv0、yv0分别为初始时刻AUV的X坐标和Y坐标，vx0、vy0分别为初始时刻AUV载体坐标系下的X轴线速度和Y轴线速度，ψv0为初始时刻AUV艏向角；
协方差矩阵P0为：
Figure FDA0003840897420000014
其中，
Figure FDA0003840897420000015
和
Figure FDA0003840897420000016
分别表示xv0、yv0、vx0、vy0和ψv0的初始方差；
S4所述利用强跟踪无迹卡尔曼滤波算法进行时间更新的具体方法包括：
时间更新所用系统方程如下：
Figure FDA0003840897420000021
Figure FDA0003840897420000022
其中，
Figure FDA0003840897420000023
表示k-1时刻的系统的状态量，
Figure FDA0003840897420000024
表示k时刻的先验状态估计，f(·)表示系统方程；
xv、yv分别为AUV的X坐标和Y坐标，vx、vy分别表示当前时刻AUV载体坐标系下的X轴线速度和Y轴线速度，ψv为当前时刻AUV艏向角，xi、yi分别表示Xk内存储的第i个特征点的X坐标和Y坐标，i＝1,2,…,n，所述特征点即基元，下标k和k-1表示状态对应的时刻，w表示系统方程的过程噪声，w的协方差矩阵为Q；
进行Sigma点采样：
根据
Figure FDA0003840897420000025
和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 FDA0003840897420000026
和一步误差协方差阵预测估计Pk|k-1：
χi,k|k-1＝f(χi,k-1)
Figure FDA0003840897420000027
Figure FDA0003840897420000031
Figure FDA0003840897420000032
式中，
Figure FDA0003840897420000033
为求一阶统计特性时的权系数，i＝1,...,L；
Figure FDA0003840897420000034
为求二阶统计特性时的权系数，i＝1,...,L；χi,k-1为Sigma点集{χi}k-1中的点；χi,k|k-1为Sigma点集{χi}k|k-1中的点；Pxx为中间变量；
S5所述的仅进行AUV速度和艏向的量测更新，具体包括：
量测方程
Figure FDA0003840897420000035
表示为：
Figure FDA0003840897420000036
观测量为z＝[vx_sensor vy_sensor ψv_sensor]T；
其中，v1为观测噪声，其协方差为R1，vx_sensor、vy_sensor和ψv_sensor分别表示传感器测得的AUV载体坐标系下的X轴线速度、Y轴线速度和AUV艏向角；
S5所述的进行AUV速度、艏向和涉及第i个特征点的量测更新，具体包括：
量测方程zk表示为：
Figure FDA0003840897420000037
观测量为
Figure FDA0003840897420000038
其中，v2为观测噪声，其协方差矩阵为R2，d为声呐所探测到的基元到AUV的距离在AUV载体坐标系XOY平面上的投影，
Figure FDA0003840897420000039
为声呐所探测基元相对于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 FDA0003840897420000041
Figure FDA0003840897420000042
Figure FDA0003840897420000043
计算{δi}k-1的协方差：
Figure FDA0003840897420000044
计算{δi}k-1和{χi}k-1的协方差：
Figure FDA0003840897420000045
计算{χi}k|k-1和{δi}k-1的协方差：
Figure FDA0003840897420000046
进一步计算获得协方差Pk：
Figure FDA0003840897420000047
式中
Figure FDA0003840897420000051
In为n维的单位矩阵；
根据Pk，
Figure FDA0003840897420000052
采取采样策略计算Sigma点集{χi}k；
计算Sigma点集{χi}k通过非线性量测方程的传播：
δi,k＝h(χi,k)
Figure FDA0003840897420000053
Figure FDA0003840897420000054
Figure FDA0003840897420000055
进行量测方程的更新：
Kk＝Pxz(I+Pzz)-1
Figure FDA0003840897420000056
2.根据权利要求1所述的一种大潜深AUV的定位基阵联合标校方法，其特征在于，S5所述将该基元位置信息扩增至状态向量中，并扩增协方差矩阵，具体包括：
状态向量的矩阵扩增方程为：
Figure FDA0003840897420000057
其中，d为声呐探测到的基元到AUV的距离在AUV载体坐标系XOY平面上的投影，
Figure FDA0003840897420000058
为声呐所探测到的基元相对于AUV艏向的方位角；
计算雅可比矩阵：
Figure FDA0003840897420000059
协方差的矩阵扩增方程为：
Figure FDA00038408974200000510
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 CN114265047A (zh) 2022-04-01
CN114265047B true 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)

Families Citing this family (2)

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

Family Cites Families (6)

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

Also Published As

Publication number Publication date
CN114265047A (zh) 2022-04-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
CN107990891B (zh) 基于长基线和信标在线标定的水下机器人组合导航方法
CN112284384B (zh) 考虑量测异常的集群式多深海潜航器的协同定位方法
CN105737832A (zh) 基于全局最优数据融合的分布式slam方法
CN111596333B (zh) 一种水下定位导航方法及系统
CN103697910A (zh) 自主水下航行器多普勒计程仪安装误差的校正方法
CN107966145B (zh) 一种基于稀疏长基线紧组合的auv水下导航方法
CN106017467A (zh) 一种基于多水下应答器的惯性/水声组合导航方法
CN111750865B (zh) 一种用于双功能深海无人潜器导航系统的自适应滤波导航方法
CN111982105B (zh) 一种基于sins/lbl紧组合的水下导航定位方法及系统
CN115307643A (zh) 一种双应答器辅助的sins/usbl组合导航方法
CN110703205B (zh) 基于自适应无迹卡尔曼滤波的超短基线定位方法
CN112747748A (zh) 一种基于逆向解算的领航auv导航数据后处理方法
CN115979253A (zh) 一种基于抗差滤波的水下机器人多传感器紧组合导航方法
Braginsky et al. Correction of DVL error caused by seafloor gradient
KR100879701B1 (ko) 무향 칼만 필터를 이용한 동시 위치 추정 및 지도 작성시스템 및 방법
CN112666519B (zh) 一种基于广义二阶时延差的水下目标高精度定位方法
Zhang et al. A robust calibration method for the underwater transponder position based on Gauss-Newton iteration algorithm
CN111964684B (zh) 一种基于sins/lbl紧组合的水下导航混合定位方法及系统
CN116772903B (zh) 基于迭代ekf的sins/usbl安装角估计方法
CN111721284A (zh) 一种无源模式下的sins/usbl组合导航定位方法
CN112445243B (zh) 一种适用于自主水下机器人的海底目标搜寻方法

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