CN114265047B - 一种大潜深auv的定位基阵联合标校方法 - Google Patents
一种大潜深auv的定位基阵联合标校方法 Download PDFInfo
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 22
- 239000011159 matrix material Substances 0.000 claims abstract description 51
- 238000001914 filtration Methods 0.000 claims abstract description 5
- 238000005259 measurement Methods 0.000 claims description 32
- 238000004364 calculation method Methods 0.000 claims description 9
- 238000005070 sampling Methods 0.000 claims description 9
- 230000003321 amplification Effects 0.000 claims description 3
- 238000003199 nucleic acid amplification method Methods 0.000 claims description 3
- 238000012512 characterization method Methods 0.000 claims 1
- 238000005516 engineering process Methods 0.000 description 4
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 2
- 241000282414 Homo sapiens Species 0.000 description 1
- 230000001419 dependent effect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000009189 diving Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
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(Autonomous Underwater Vehicle,自主水下机器人)的定位基阵联合标校方法,属于AUV水下声学导航领域。
背景技术
随着人类对海洋资源的不断重视和进行不断地探索和开发,水下机器人领域得到了极大的发展,在水下机器人关键技术中,水下环境中机器人作业所需的定位技术是水下机器人发展的关键技术之一。在水下环境中,目前水下定位方式主要基于水声定位***和惯性导航***,水声定位***作为一种重要的水下定位技术,目前主要以长基线、短基线和超短基线三种水声定位***为代表,目前在水下机器人领域已经得到了广泛的应用。
在水下声学定位***中,长基线水声定位***在定位精度方面有更大的优势,在实际使用中,首先要布放多个水下基元构成基阵,基阵中基元之间的距离即基线长度通常在几百米到几千米,获取各个水下基阵的基元坐标后,基阵中各个基元测量水下机器人到各个基元之间的距离从而计算水下机器人位置。长基线水下声学定位***具有明显的优势,但是在实际使用中,对长基线进行基阵标校时,尤其在深海环境中,由母船标校基阵中各个基元的位置相对较为困难,而且实际情况下标校精度也受到明显的影响。
发明内容
本发明目的是为了解决大潜深AUV(Autonomous Underwater Vehicle,自主水下机器人)利用声学基元进校水下定位时基阵校准困难、校阵准确度低的问题,本发明提供了一种大潜深AUV的定位基阵联合标校方法。
本发明所述一种大潜深AUV的定位基阵联合标校方法,它包括:
S1、水面母船依次布放水下声学定位基阵中的各个基元和AUV(AutonomousUnderwater Vehicle,自主水下机器人),待AUV下潜至指定深度后,水面母船利用超短基线定位***对AUV位置进行修正后撤离,AUV以该位置信息为原点建立北东地坐标系;
S2、AUV按预置梳状扫测路径开始作业,同时探测水下声学定位基阵中基元的距离和方位信息;
S5、当AUV探测水下基元时:
如果未探测到基元,仅进行AUV速度和艏向的量测更新;
如果探测到状态量中所表示的第i个基元,则进行AUV速度、艏向和涉及第i个基元的量测更新;
如果探测到状态量中不存在的基元,计算该基元在此时AUV载体系XOY平面上的极坐标参数[ρ,θ],将该基元位置信息扩增至状态向量中,并扩增协方差矩阵,同时进行AUV速度和艏向的量测更新;
S6、AUV探测到所有基元后,结束较阵作业。
xv0、yv0分别为初始时刻AUV的X坐标和Y坐标,vx0、vy0分别为初始时刻AUV载体系下的X轴线速度和Y轴线速度,ψv0为初始时刻AUV艏向角;
优选的,S4所述利用强跟踪无迹卡尔曼滤波算法进行时间更新的具体方法包括:
时间更新所用***方程如下:
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点采样:
χi,k|k-1=f(χi,k-1)
S5所述的仅进行AUV速度和艏向的量测更新,具体包括:
观测量为z=[vx_sensor vy_sensor ψv_sensor]T;
其中,v1为观测噪声,其协方差为R1,vx_sensor、vy_sensor和ψv_sensor分别表示传感器测得的AUV载体系下的X轴线速度、Y轴线速度和AUV艏向角。
优选的,S5所述的进行AUV速度、艏向和涉及第i个特征点的量测更新,具体包括:
量测方程zk表示为:
优选的,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-1的协方差:
计算{δi}k-1和{χi}k-1的协方差:
计算{χi}k|k-1和{δi}k-1的协方差:
进一步计算获得协方差Pk:
计算Sigma点集{χi}k通过非线性量测方程的传播:
δi,k|=h(χi,k)
进行量测方程的更新:
Kk=Pxz(I+Pzz)-1
优选的,S5所述将该基元位置信息扩增至状态向量中,并扩增协方差矩阵,具体包括:
状态向量的矩阵扩增方程为:
本发明的优点:本发明提出的一种大潜深自主水下机器人的定位基阵联合标校方法,能够实现大潜深的水下基阵标校,并提高标校的精度。
附图说明
图1是本发明所述一种大潜深自主水下机器人的定位基阵联合标校方法的流程框图;
图2是采用AUV进行较阵作业的示意图,其中:a表示基元,b表示AUV,c表示AUV预置梳状扫测路径。
具体实施方式
下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本发明一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有作出创造性劳动的前提下所获得的所有其他实施例,都属于本发明保护的范围。
需要说明的是,在不冲突的情况下,本发明中的实施例及实施例中的特征可以相互组合。
具体实施方式一:下面结合图1说明本实施方式,本实施方式所述一种大潜深自主水下机器人(AUV)的定位基阵联合标校方法,它包括:
S1、水面母船依次布放水下声学定位基阵中的各个基元和AUV,待AUV下潜至指定深度后,水面母船利用超短基线定位***对AUV位置进行修正后撤离,AUV以该位置信息为原点建立北东地坐标系;
S2、AUV按预置梳状扫测路径开始作业,同时探测水下声学定位基阵中基元的距离和方位信息;
S5、当AUV探测水下基元时:
如果未探测到基元,仅进行AUV速度和艏向的量测更新;
如果探测到状态量中所表示的第i个基元,则进行AUV速度、艏向和涉及第i个基元的量测更新;
如果探测到状态量中不存在的基元,计算该基元在此时AUV载体系XOY平面上的极坐标参数[ρ,θ],将该基元位置信息扩增至状态向量中,并扩增协方差矩阵,同时进行AUV速度和艏向的量测更新;
S6、AUV探测到所有基元后,结束较阵作业。
xv0、yv0分别为初始时刻AUV的X坐标和Y坐标,vx0、vy0分别为初始时刻AUV载体系下的X轴线速度和Y轴线速度,ψv0为初始时刻AUV艏向角;
具体实施方式三:本实施方式对具体实施方式二作进一步说明,S4所述利用强跟踪无迹卡尔曼滤波算法进行时间更新的具体方法包括:
时间更新所用***方程如下:
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点采样:
χi,k|k-1=f(χi,k-1)
具体实施方式四:本实施方式对具体实施方式三作进一步说明,S5所述的仅进行AUV速度和艏向的量测更新,具体包括:
观测量为z=[vx_sensor vy_sensor ψv_sensor]T;
其中,v1为观测噪声,其协方差为R1,vx_sensor、vy_sensor和ψv_sensor分别表示传感器测得的AUV载体系下的X轴线速度、Y轴线速度和AUV艏向角。
具体实施方式五:本实施方式对具体实施方式四作进一步说明,S5所述的进行AUV速度、艏向和涉及第i个特征点的量测更新,具体包括:
量测方程zk表示为:
具体实施方式六:本实施方式对具体实施方式四或五作进一步说明,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-1的协方差:
计算{δi}k-1和{χi}k-1的协方差:
计算{χi}k|k-1和{δi}k-1的协方差:
进一步计算获得协方差Pk:
计算Sigma点集{χi}k通过非线性量测方程的传播:
δi,k|=h(χi,k)
进行量测方程的更新:
Kk=Pxz(I+Pzz)-1
具体实施方式七:本实施方式对具体实施方式六作进一步说明,S5所述将该基元位置信息扩增至状态向量中,并扩增协方差矩阵,具体包括:
状态向量的矩阵扩增方程为:
虽然在本文中参照了特定的实施方式来描述本发明,但是应该理解的是,这些实施例仅仅是本发明的原理和应用的示例。因此应该理解的是,可以对示例性的实施例进行许多修改,并且可以设计出其他的布置,只要不偏离所附权利要求所限定的本发明的精神和范围。应该理解的是,可以通过不同于原始权利要求所描述的方式来结合不同的从属权利要求和本文中所述的特征。还可以理解的是,结合单独实施例所描述的特征可以使用在其他所述实施例中。
Claims (2)
1.一种大潜深AUV的定位基阵联合标校方法,其特征在于,它包括:
S1、水面母船依次布放AUV和水下声学定位基阵中的各个基元,待AUV下潜至指定深度后,水面母船利用超短基线定位***对AUV位置进行修正后撤离,AUV以该位置信息为原点建立北东地坐标系;
S2、AUV按预置梳状扫测路径开始作业,同时探测水下声学定位基阵中基元的距离和方位信息;
S5、当AUV探测水下基元时:
如果未探测到基元,仅进行AUV速度和艏向的量测更新;
如果探测到状态量中所表示的第i个基元,则进行AUV速度、艏向和涉及第i个基元的量测更新;
如果探测到状态量中不存在的基元,计算该基元在此时AUV载体坐标系XOY平面上的极坐标参数[ρ,θ],将该基元位置信息扩增至AUV的状态向量中,并扩增AUV的协方差矩阵,同时进行AUV速度和艏向的量测更新;
S6、AUV探测到所有基元后,结束较阵作业;
xv0、yv0分别为初始时刻AUV的X坐标和Y坐标,vx0、vy0分别为初始时刻AUV载体坐标系下的X轴线速度和Y轴线速度,ψv0为初始时刻AUV艏向角;
S4所述利用强跟踪无迹卡尔曼滤波算法进行时间更新的具体方法包括:
时间更新所用***方程如下:
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点采样:
χi,k|k-1=f(χi,k-1)
式中,为求一阶统计特性时的权系数,i=1,...,L;为求二阶统计特性时的权系数,i=1,...,L;χi,k-1为Sigma点集{χi}k-1中的点;χi,k|k-1为Sigma点集{χi}k|k-1中的点;Pxx为中间变量;
S5所述的仅进行AUV速度和艏向的量测更新,具体包括:
观测量为z=[vx_sensor vy_sensor ψv_sensor]T;
其中,v1为观测噪声,其协方差为R1,vx_sensor、vy_sensor和ψv_sensor分别表示传感器测得的AUV载体坐标系下的X轴线速度、Y轴线速度和AUV艏向角;
S5所述的进行AUV速度、艏向和涉及第i个特征点的量测更新,具体包括:
量测方程zk表示为:
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-1的协方差:
计算{δi}k-1和{χi}k-1的协方差:
计算{χi}k|k-1和{δi}k-1的协方差:
进一步计算获得协方差Pk:
计算Sigma点集{χi}k通过非线性量测方程的传播:
δi,k=h(χi,k)
进行量测方程的更新:
Kk=Pxz(I+Pzz)-1
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)
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)
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 | 哈尔滨工程大学 | 一种海上声学相位中心校准方法 |
-
2021
- 2021-12-31 CN CN202111675884.7A patent/CN114265047B/zh active Active
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安装误差角标定方法 | |
CN103697910B (zh) | 自主水下航行器多普勒计程仪安装误差的校正方法 | |
CN106679662B (zh) | 一种基于tma技术的水下机器人单信标组合导航方法 | |
CN101949703B (zh) | 一种捷联惯性/卫星组合导航滤波方法 | |
Karimi et al. | A comparison of DVL/INS fusion by UKF and EKF to localize an autonomous underwater vehicle | |
CN105737832A (zh) | 基于全局最优数据融合的分布式slam方法 | |
CN111596333B (zh) | 一种水下定位导航方法及*** | |
CN107966145B (zh) | 一种基于稀疏长基线紧组合的auv水下导航方法 | |
CN112284384A (zh) | 考虑量测异常的集群式多深海潜航器的协同定位方法 | |
CN111750865B (zh) | 一种用于双功能深海无人潜器导航***的自适应滤波导航方法 | |
CN111982105B (zh) | 一种基于sins/lbl紧组合的水下导航定位方法及*** | |
CN115307643A (zh) | 一种双应答器辅助的sins/usbl组合导航方法 | |
CN110703205B (zh) | 基于自适应无迹卡尔曼滤波的超短基线定位方法 | |
CN112747748A (zh) | 一种基于逆向解算的领航auv导航数据后处理方法 | |
CN115979253A (zh) | 一种基于抗差滤波的水下机器人多传感器紧组合导航方法 | |
CN116772903B (zh) | 基于迭代ekf的sins/usbl安装角估计方法 | |
Braginsky et al. | Correction of DVL error caused by seafloor gradient | |
KR100879701B1 (ko) | 무향 칼만 필터를 이용한 동시 위치 추정 및 지도 작성시스템 및 방법 | |
CN112666519B (zh) | 一种基于广义二阶时延差的水下目标高精度定位方法 | |
CN116222578B (zh) | 基于自适应滤波和最优平滑的水下组合导航方法及*** | |
CN112445243B (zh) | 一种适用于自主水下机器人的海底目标搜寻方法 | |
CN111964684B (zh) | 一种基于sins/lbl紧组合的水下导航混合定位方法及*** | |
CN112945245A (zh) | 基于条件数理论在多auv协同导航***中可观测性分析方法 |
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 |