CN109901205B - 一种水下机器人多传感器融合与运动轨迹预测方法 - Google Patents

一种水下机器人多传感器融合与运动轨迹预测方法 Download PDF

Info

Publication number
CN109901205B
CN109901205B CN201910191520.8A CN201910191520A CN109901205B CN 109901205 B CN109901205 B CN 109901205B CN 201910191520 A CN201910191520 A CN 201910191520A CN 109901205 B CN109901205 B CN 109901205B
Authority
CN
China
Prior art keywords
motion
underwater robot
sensor
east
detection
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
CN201910191520.8A
Other languages
English (en)
Other versions
CN109901205A (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.)
Nanjing University of Information Science and Technology
Original Assignee
Nanjing University of Information Science and Technology
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 Nanjing University of Information Science and Technology filed Critical Nanjing University of Information Science and Technology
Priority to CN201910191520.8A priority Critical patent/CN109901205B/zh
Publication of CN109901205A publication Critical patent/CN109901205A/zh
Application granted granted Critical
Publication of CN109901205B publication Critical patent/CN109901205B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Abstract

本发明是一种水下机器人多传感器融合与运动轨迹预测方法，包括以下步骤：首先，对多个声呐观测数据进行融合；其次，建立预测模型；最后，根据目标水下机器人过去和现在的运行轨迹，进行运行轨迹预测。该方法通过多传感器数据融合，采用多传感器信息融合获取水下环境信息的方式，充分利用数据冗余性和互补性,保证水下环境感知的准确性、快速性、稳定性，本专利根据声呐观测到的水下机器人的数据利用关联算法进行航迹识别，对于不同声呐观测到水下机器人的航迹进行相似性判别,数据融合，最后进行轨迹预测。

Description

一种水下机器人多传感器融合与运动轨迹预测方法
技术领域
本发明属于机器人控制技术领域，具体的说是一种水下机器人多传感器融合与运动轨迹预测方法。
背景技术
随着陆地资源日益枯竭，人们对海洋的探索越来越强烈。海洋资源开发潜力巨大，但是，海洋开发需要借助各种设备及工具。人们希望借助这些工具，突破人类探索的局限，深入海底，探测海底资源、环境。首先开发水下机器人水下探测逐渐走向成熟，人们开始尝试进行不断技术革新，传统意义上水下机器人根据任务要求搭载不同的传感器进行水下环境的感知，完成相应的任务，这些只保证水下机器人作为个体完成任务要求，未进行全局监控，数据处理，将采集信息进行有效处理，使得过多获取信息，导致大量数据冗余，信息获取与处理未达到匹配，也无法根据当前监测到水下机器人数据，对水下机器人运动轨迹相对预测。
发明内容
本发明要解决的技术问题是提供一种水下机器人多传感器融合与运动轨迹预测方法，该方法通过对多个声呐探测器采集到数据进行有效处理，根据采样数据的变化，进行运动轨迹、位置、方向的预估，从而对水下机器人进行有效控制。
为解决上述技术问题，本发明采用的技术方案为：
一种水下机器人多传感器融合与运动轨迹预测方法，其特征是：监控测量船上安装有多个探测声呐，水下机器人上安装有响应信标，所述的水下机器人上安装有水深传感器，所述的监控测量船上安装有方向传感器和GPS模块，所述的探测声呐用于按照周期向周围发射探测脉冲信号，所述的响应信标用于接收探测脉冲信号，并进行响应，探测声呐测出响应信标相对于探测声呐的距离和方向角度，所述的水深传感器可以用来测量水下机器人所处的位置深度，所述的方向传感器和GPS模块用于将探测声呐测量到的水下机器人的距离和方向角度转化为大地坐标下的位置信息；具体检测方法如下：
步骤1，对多个传感器的观测数据进行融合；
步骤2，建立观测模型；
步骤3，根据目标过去和现在的位置轨迹，对后续的短时刻内的运行轨迹进行预测。
所述的步骤1的具体计算过程如下：
设随机向量x∈Rn,基于L个探测声呐传感器，已知L个无偏估计为
Figure BDA0001994452330000021
即
Figure BDA0001994452330000022
设已知估计误差
Figure BDA0001994452330000023
的方差阵
Figure BDA0001994452330000024
和互协方差
Figure BDA0001994452330000025
阵。寻求x的按矩阵加权无偏估计，融合估计，按矩阵加权线性最小方差最优融合公式如下：
Figure BDA0001994452330000026
其中Ai为加权矩阵，最优加权阵[A1,…AL]＝(eTP-1e)-1eTP-1,最优融合估计误差方差阵为P0＝(eTP-1e)-1。
所述的步骤2中观测模型的建立方法具体为：
设采样周期T0,首先考虑一维坐标与时间的观测模型：
y(t)＝x(t)+δ(t)
其中y(t)表示tT0处声呐定位的观测值，x(t)表示被测目标在采样时刻tT0处真实位置，δ(t)表示声呐定位观测，可以假设零均值、方差为
Figure BDA0001994452330000027
的白噪声；方差
Figure BDA0001994452330000028
的估值可通过大量声呐观测数据用统计方法获得。
所述的步骤3中运行轨迹的预测方法具体为：
设采样周期为T0,假设地面为平面，以地面为基准面，由于被测目标的深度是渐变的，若不考虑深度的变化预测为一个估计包含二维位置坐标(x,y)与时间t信息的三维空间轨迹；
若不考虑深度变化，被测目标的运动可以分解为向东运动和向北运动，被测目标向东运动状态变量xe(t)、
Figure BDA0001994452330000029
分别表示为在时刻tT0向东位置、速度、加速度；同理，设被测目标向北运动的状态向量xn(t)，
Figure BDA00019944523300000210
被测目标在时刻tT0处与坐标原点的距离ρ(t)和速度v(t)分别为：
Figure BDA0001994452330000031
Figure BDA0001994452330000032
由匀加速运动规律得被测目标向东运动方程：
Figure BDA0001994452330000033
Figure BDA0001994452330000034
Figure BDA0001994452330000035
Figure BDA0001994452330000036
Figure BDA0001994452330000037
向北运动方程为：
Figure BDA0001994452330000038
Figure BDA0001994452330000039
Figure BDA00019944523300000310
yxn(t)＝xn(t)+δxn(t)
Figure BDA00019944523300000311
其中，ue(t)为向东加速度，un(t)为向北加速度，we(t)、wn(t)、δxe(t)、δve(t)、δxn(t)、δvn(t)为两两相互独立的白噪声；水下环境复杂，会出现不同程度的干扰，因此会产生一些误差，we(t)是向东方向的加速度误差，wn(t)向北方向的加速度误差，δxe(t)向东方向运动的位移误差，δve(t)向东方向运动的速度误差，δxn(t)向北方向运动位移误差，δvn(t)向北运动的速度误差；
Figure BDA0001994452330000041
Figure BDA0001994452330000042
Figure BDA0001994452330000043
向东和向北状态空间模型为：
Xe(t+1)＝Φxe(t)+Bue(t)+Γwn(t)
Ye(t)＝HXe(t)+ve(t)
和
Xn(t+1)＝Φxn(t)+Bun(t)+Γwn(t)
Yn(t)＝HXn(t)+vn(t)
当向东和向北两个方向的噪声不相关时，由于向东和向北具有相同的状态空间模型，可对向东和向北两个方向上的运动进行单独处理，并对向东和向北两个方向的运动进行分别预测。
所述的向东和向北两个方向上状态空间模型结合深度变化规律，对水下机器人的运动轨迹进行完整预测。
该种水下机器人多传感器融合与运动轨迹预测方法能够产生的有益效果为：本专利是通过多传感器数据融合，采用多传感器信息融合获取水下环境信息的方式，充分利用数据冗余性和互补性,保证水下环境感知的准确性、快速性、稳定性。该方法根据声呐观测到的水下机器人的数据利用关联算法进行航迹识别，对于不同声呐观测到水下机器人的航迹进行相似性判别,数据融合，最后进行轨迹预测。
附图说明
图1为本发明一种水下机器人多传感器融合与运动轨迹预测方法中水下机器人航迹测量原理图。
图2为本发明一种水下机器人多传感器融合与运动轨迹预测方法中多个观测数据融合逻辑框图。
图3为本发明一种水下机器人多传感器融合与运动轨迹预测方法的融合后的经度时间历程图。
图4为本发明一种水下机器人多传感器融合与运动轨迹预测方法的融合后的经度、纬度时间历程图。
具体实施方式
以下结合说明书附图和具体优选的实施例对本发明作进一步描述。
如图1所示，一种水下机器人多传感器融合与运动轨迹预测系统，包括在监控测量船上安装有多个探测声呐，水下机器人上安装有响应信标，所述的水下机器人上安装有水深传感器，所述的监控测量船上安装有方向传感器和GPS模块，所述的探测声呐用于按照周期向周围发射探测脉冲信号，所述的响应信标用于接收探测脉冲信号，并进行响应，探测声呐测出响应信标相对于探测声呐的距离和方向角度，所述的水深传感器可以用来测量水下机器人所处的位置深度，所述的方向传感器和GPS模块用于将探测声呐测量到的水下机器人的距离和方向角度转化为大地坐标下的位置信息。
一种水下机器人多传感器融合与运动轨迹预测方法如下：
步骤1，对多个传感器的观测数据进行融合；设随机向量x∈Rn,基于L个探测声呐传感器，已知L个无偏估计为
Figure BDA0001994452330000051
即
Figure BDA0001994452330000052
设已知估计误差
Figure BDA0001994452330000053
的方差阵
Figure BDA0001994452330000054
和互协方差
Figure BDA0001994452330000055
阵。寻求x的按矩阵加权无偏估计，融合估计，按矩阵加权线性最小方差最优融合公式如下：
Figure BDA0001994452330000056
其中Ai为加权矩阵，最优加权阵[A1,…AL]＝(eTP-1e)-1eTP-1,最优融合估计误差方差阵为P0＝(eTP-1e)-1。
步骤2，建立观测模型；设采样周期T0,首先考虑一维坐标与时间的观测模型：
y(t)＝x(t)+δ(t)
其中y(t)表示tT0处声呐定位的观测值，x(t)表示被测目标在采样时刻tT0处真实位置，δ(t)表示声呐定位观测，可以假设零均值、方差为
Figure BDA0001994452330000061
的白噪声；方差
Figure BDA0001994452330000062
的估值可通过大量声呐观测数据用统计方法获得。
步骤3，根据目标过去和现在的位置轨迹，对后续的短时刻内的运行轨迹进行预测，设采样周期为T0,假设地面为平面，以地面为基准面，由于被测目标的深度是渐变的，若不考虑深度的变化预测为一个估计包含二维位置坐标(x,y)与时间t信息的三维空间轨迹；
若不考虑深度变化，被测目标的运动可以分解为向东运动和向北运动，被测目标向东运动状态变量xe(t)、
Figure BDA0001994452330000063
分别表示为在时刻tT0向东位置、速度、加速度；同理，设被测目标向北运动的状态向量xn(t)，
Figure BDA0001994452330000064
被测目标在时刻tT0处与坐标原点的距离ρ(t)和速度v(t)分别为：
Figure BDA0001994452330000065
Figure BDA0001994452330000066
由匀加速运动规律得被测目标向东运动方程：
Figure BDA0001994452330000067
Figure BDA0001994452330000068
Figure BDA0001994452330000069
Figure BDA00019944523300000610
Figure BDA00019944523300000611
向北运动方程为：
Figure BDA0001994452330000071
Figure BDA0001994452330000072
Figure BDA0001994452330000073
yxn(t)＝xn(t)+δxn(t)
Figure BDA0001994452330000074
其中，ue(t)为向东加速度，un(t)为向北加速度，we(t)、wn(t)、δxe(t)、δve(t)、δxn(t)、δvn(t)为两两相互独立的白噪声；水下环境复杂，会出现不同程度的干扰，因此会产生一些误差，we(t)是向东方向的加速度误差，wn(t)向北方向的加速度误差，δxe(t)向东方向运动的位移误差，δve(t)向东方向运动的速度误差，δxn(t)向北方向运动位移误差，δvn(t)向北运动的速度误差。
Figure BDA0001994452330000075
Figure BDA0001994452330000076
Figure BDA0001994452330000077
向东和向北状态空间模型为：
Xe(t+1)＝Φxe(t)+Bue(t)+Γwn(t)
Ye(t)＝HXe(t)+ve(t)
和
Xn(t+1)＝Φxn(t)+Bun(t)+Γwn(t)
Yn(t)＝HXn(t)+vn(t)
进一步的，当向东和向北两个方向的噪声不相关时，由于向东和向北具有相同的状态空间模型，可对向东和向北两个方向上的运动进行单独处理，并对向东和向北两个方向的运动进行分别预测。
进一步的，所述的向东和向北两个方向上状态空间模型结合深度变化规律，对水下机器人的运动轨迹进行完整预测。
该种多传感器下的水下机器认航行轨迹的融合与预测是最优状态估计问题，根据不同声呐对于同一目标的观测状态，融合出各个声呐信息的最优状态，这样可以消除产生的随机误差，通过融合后的最优状态，进一步对水下机器人的航行轨迹进行预测。
以上仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，应视为本发明的保护范围。

Claims (5)

1.一种水下机器人多传感器融合与运动轨迹预测方法，其特征是：监控测量船上安装有多个探测声呐，水下机器人上安装有响应信标，所述的水下机器人上安装有水深传感器，所述的监控测量船上安装有方向传感器和GPS模块，所述的探测声呐用于按照周期向周围发射探测脉冲信号，所述的响应信标用于接收探测脉冲信号，并进行响应，探测声呐测出响应信标相对于探测声呐的距离和方向角度，所述的水深传感器可以用来测量水下机器人所处的位置深度，所述的方向传感器和GPS模块用于将探测声呐测量到的水下机器人的距离和方向角度转化为大地坐标下的位置信息；具体检测方法如下：
步骤1，对多个传感器的观测数据进行融合；
步骤2，建立观测模型；
步骤3，根据目标过去和现在的位置轨迹，对后续的短时刻内的运行轨迹进行预测；
所述的步骤1的具体计算过程如下：
设随机向量x∈Rn，基于L个探测声呐传感器观测，已知L个无偏估计为
Figure FDA0003617786010000011
即
Figure FDA0003617786010000012
设已知估计误差
Figure FDA0003617786010000013
的方差阵
Figure FDA0003617786010000014
和互协方差
Figure FDA0003617786010000015
阵，寻求x的按矩阵加权无偏估计，融合估计，按矩阵加权线性最小方差最优融合公式如下：
Figure FDA0003617786010000016
其中Ai为加权矩阵，最优加权阵[A1，…AL]＝(eTP-1e)-1eTP-1，最优融合估计误差方差阵为P0＝(eTP-1e)-1。
2.根据权利要求1所述的一种水下机器人多传感器融合与运动轨迹预测方法，其特征在于：所述的步骤2中观测模型的建立方法具体为：
设采样周期T0，首先考虑一维坐标与时间的观测模型：
y(t)＝x(t)+δ(t)
其中y(t)表示tT0处声呐定位的观测值，x(t)表示被测目标在采样时刻tT0处真实位置，δ(t)表示声呐定位观测，可以假设零均值、方差为
Figure FDA0003617786010000021
的白噪声；方差
Figure FDA0003617786010000022
的估值可通过大量声呐观测数据用统计方法获得。
3.根据权利要求2所述的一种水下机器人多传感器融合与运动轨迹预测方法，其特征在于：所述的步骤3中运行轨迹的预测方法具体为：
设采样周期为T0，假设地面为平面，以地面为基准面，由于被测目标的深度是渐变的，若不考虑深度的变化预测为一个估计包含二维位置坐标(x，y)与时间t信息的三维空间轨迹；
若不考虑深度变化，被测目标的运动可以分解为向东运动和向北运动，被测目标向东运动状态变量xe(t)、
Figure FDA0003617786010000023
分别表示为在时刻tT0向东位置、速度、加速度；同理，设被测目标向北运动的状态向量xn(t)，
Figure FDA0003617786010000024
被测目标在时刻tT0处与坐标原点的距离ρ(t)和速度v(t)分别为：
Figure FDA0003617786010000025
Figure FDA0003617786010000026
由匀加速运动规律得被测目标向东运动方程：
Figure FDA0003617786010000027
Figure FDA0003617786010000028
Figure FDA0003617786010000029
Figure FDA00036177860100000210
Figure FDA00036177860100000211
向北运动方程为：
Figure FDA0003617786010000031
Figure FDA0003617786010000032
Figure FDA0003617786010000033
yxn(t)＝xn(t)+δxn(t)
Figure FDA0003617786010000034
其中，ue(t)为向东加速度，un(t)为向北加速度，we(t)、Wn(t)、δxe(t)、δve(t)、δxn(t)、δvn(t)为两两相互独立的白噪声；水下环境复杂，会出现不同程度的干扰，因此会产生一些误差，we(t)是向东方向的加速度误差，Wn(t)向北方向的加速度误差，δxe(t)向东方向运动的位移误差，δve(t)向东方向运动的速度误差，δxn(t)向北方向运动位移误差，δvn(t)向北运动的速度误差；
Figure FDA0003617786010000035
Figure FDA0003617786010000036
Figure FDA0003617786010000037
向东和向北状态空间模型为：
Xe(t+1)＝Φxe(t)+Bue(t)+Γwn(t)
Ye(t)＝HXe(t)+ve(t)
和
Figure FDA0003617786010000041
4.根据权利要求3所述的一种水下机器人多传感器融合与运动轨迹预测方法，其特征在于：当向东和向北两个方向的噪声不相关时，由于向东和向北具有相同的状态空间模型，可对向东和向北两个方向上的运动进行单独处理，并对向东和向北两个方向的运动进行分别预测。
5.根据权利要求4所述的一种水下机器人多传感器融合与运动轨迹预测方法，其特征在于：所述的向东和向北两个方向上状态空间模型结合深度变化规律，对水下机器人的运动轨迹进行完整预测。
CN201910191520.8A 2019-03-14 2019-03-14 一种水下机器人多传感器融合与运动轨迹预测方法 Active CN109901205B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910191520.8A CN109901205B (zh) 2019-03-14 2019-03-14 一种水下机器人多传感器融合与运动轨迹预测方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910191520.8A CN109901205B (zh) 2019-03-14 2019-03-14 一种水下机器人多传感器融合与运动轨迹预测方法

Publications (2)

Publication Number Publication Date
CN109901205A CN109901205A (zh) 2019-06-18
CN109901205B true CN109901205B (zh) 2022-07-01

Family

ID=66953203

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910191520.8A Active CN109901205B (zh) 2019-03-14 2019-03-14 一种水下机器人多传感器融合与运动轨迹预测方法

Country Status (1)

Country Link
CN (1) CN109901205B (zh)

Families Citing this family (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110412583B (zh) * 2019-07-22 2022-12-27 武汉理工大学 基于多rov协同的水下不明运动目标侦测系统
CN113535672B (zh) * 2020-04-16 2024-03-12 中国科学院沈阳自动化研究所 一种基于自主水下机器人平台的浊度数据处理方法
CN111563914A (zh) * 2020-04-22 2020-08-21 中国电子科技集团公司电子科学研究院 一种水下定位跟踪方法、装置及可读存储介质
CN111722231B (zh) * 2020-06-18 2021-03-16 中国科学院声学研究所 一种主动声纳目标运动信息融合方法
CN112764421B (zh) * 2020-12-28 2022-12-23 武汉第二船舶设计研究所(中国船舶重工集团公司第七一九研究所) 无人深潜器自主导航轨迹预测积分控制方法及装置
CN112904897B (zh) * 2021-01-26 2022-02-15 华中科技大学 一种用于水下精确投放海洋传感器的轨迹优化方法及设备
CN113671508B (zh) * 2021-08-19 2023-12-08 江南大学 水下淤泥测量方法、装置及系统
CN116449291B (zh) * 2023-06-12 2023-08-25 中国人民解放军国防科技大学 基于定位误差分析的被动全向声呐浮标补投方法和装置

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8228760B2 (en) * 2008-02-12 2012-07-24 The United States Of America, As Represented By The Secretary Of The Navy Airborne laser-acoustic mine detection system
CN101750611B (zh) * 2009-12-02 2012-05-09 哈尔滨工程大学 一种水下机器人目标探测装置及探测方法
CN105607064B (zh) * 2015-09-08 2018-04-24 南京国业科技有限公司 一种水下声呐系统
CN107010188B (zh) * 2017-05-11 2023-05-23 深圳潜水侠创新动力科技有限公司 水下机器人和水下信息采集系统
CN109239717A (zh) * 2018-08-15 2019-01-18 杭州电子科技大学 一种基于水下无人航行器的合成孔径声呐实时成像方法

Also Published As

Publication number Publication date
CN109901205A (zh) 2019-06-18

Similar Documents

Publication Publication Date Title
CN109901205B (zh) 一种水下机器人多传感器融合与运动轨迹预测方法
CN108444478B (zh) 一种用于水下航行器的移动目标视觉位姿估计方法
CN109856638B (zh) 一种特定水下目标自动搜索定位的方法
US20090287414A1 (en) System and process for the precise positioning of subsea units
Jacobi et al. Multi sensor underwater pipeline tracking with AUVs
Chen et al. Towards autonomous localization and mapping of AUVs: a survey
CN101900558A (zh) 集成声纳微导航的自主式水下机器人组合导航方法
JP2008128968A (ja) 水中測位システムおよび水中測位方法
Mirza et al. Collaborative localization for fleets of underwater drifters
CN109375646A (zh) 基于fmsrupf算法的auv对接回收自主导航方法
US20230243658A1 (en) Systems, Methods and Devices for Map-Based Object's Localization Deep Learning and Object's Motion Trajectories on Geospatial Maps Using Neural Network
Salavasidis et al. Terrain‐aided navigation for long‐range AUVs in dynamic under‐mapped environments
CN113108774B (zh) 水下机器人及水下机器人导航定位方法
Ma et al. A review of terrain aided navigation for underwater vehicles
CN111220146B (zh) 一种基于高斯过程回归学习的水下地形匹配定位方法
CN112611376A (zh) 一种RGI-Lidar/SINS紧耦合AUV水下导航定位方法与系统
Lager et al. Underwater terrain navigation using standard sea charts and magnetic field maps
Bingham et al. Integrating precision relative positioning into JASON/MEDEA ROV operations
RU2378663C1 (ru) Способ определения горизонтальных координат неподвижного подводного источника гидроакустических навигационных сигналов
Moreno-Salinas et al. Optimal sensor trajectories for mobile underwater target positioning with noisy range measurements
Naughton et al. Self-localization of a deforming swarm of underwater vehicles using impulsive sound sources of opportunity
Chen et al. Range-only SLAM for underwater navigation system with uncertain beacons
CN111080788A (zh) 一种海底地形绘制方法及装置
Song et al. Autonomous vehicle localization in a vector field: Underwater vehicle implementation
KR102263037B1 (ko) 수중 운동체와 수중 음향 탐지 장비를 이용한 수중 환경 지도 작성 시스템 및 그 방법

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