WO2025208653A1 - 用于并发定位和地图绘制的方法、设备、装置及存储介质 - Google Patents

用于并发定位和地图绘制的方法、设备、装置及存储介质

Info

Publication number
WO2025208653A1
WO2025208653A1 PCT/CN2024/086416 CN2024086416W WO2025208653A1 WO 2025208653 A1 WO2025208653 A1 WO 2025208653A1 CN 2024086416 W CN2024086416 W CN 2024086416W WO 2025208653 A1 WO2025208653 A1 WO 2025208653A1
Authority
WO
WIPO (PCT)
Prior art keywords
robot
obstacle
coordinate system
coordinates
sonar
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.)
Pending
Application number
PCT/CN2024/086416
Other languages
English (en)
French (fr)
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.)
Zhejiang University
Original Assignee
Zhejiang 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 Zhejiang University filed Critical Zhejiang University
Publication of WO2025208653A1 publication Critical patent/WO2025208653A1/zh
Anticipated expiration legal-status Critical
Pending legal-status Critical Current

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
    • G01C21/203—Instruments for performing navigational calculations specially adapted for water-borne vessels
    • 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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • 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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G01C21/1652—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with ranging devices, e.g. LIDAR or RADAR
    • 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/38—Electronic maps specially adapted for navigation; Updating thereof
    • G01C21/3804—Creation or updating of map data
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S15/00—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems
    • G01S15/88—Sonar systems specially adapted for specific applications
    • G01S15/93—Sonar systems specially adapted for specific applications for anti-collision purposes
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders

Definitions

  • FIG3 is a flow chart of a method for concurrent localization and mapping in a structured environment.
  • FIG4 is a block diagram of a concurrent localization and mapping method for an underwater robot operating beneath a dock.
  • FIG5 is a flow chart of a concurrent localization and mapping method for an underwater robot operating under a dock.
  • a method for concurrent underwater positioning and mapping generates input data from the movement of a robot in a physical environment.
  • the robot is equipped with an IMU inertial unit and a sonar unit.
  • the IMU inertial unit obtains the angular velocity and acceleration of the robot in the physical environment.
  • the sonar unit obtains the relative position of the obstacle and the sonar, and the relative position is expressed in the polar coordinates of the obstacle in the robot coordinate system.
  • the sonar unit outputs a frame of sonar data in each scanning cycle.
  • the output of the IMU inertial unit and the sonar unit is the input of the SLAM algorithm, and the SLAM algorithm outputs a map in the world coordinate system.
  • the output data of the sonar unit is the coordinates of the obstacle in the robot coordinate system.
  • the coordinates of each obstacle in a frame of sonar data are the polar coordinates relative to the sonar position at the start of the scan.
  • the polar coordinates of the obstacles in the map in the robot coordinate system are correlated with the polar coordinates of the current frame of sonar data using the Mahalanobis distance.
  • the polar coordinates of the new obstacles are converted to the world coordinate system and added to the map.
  • the following operations are performed before each frame of sonar data is output: the relative position of each obstacle and the sonar is converted into polar coordinates in the robot coordinate system, the coordinates of the obstacles are clustered into multiple independent areas, the geometric center point of each independent area is removed as a point feature, and the polar coordinates of the point feature are output as the obstacle coordinates.
  • the output data of the sonar unit is the coordinates of the obstacle in the robot coordinate system.
  • the coordinates of each obstacle in a frame of sonar data are the polar coordinates relative to the sonar position at the start of the scan.
  • the polar coordinates of the obstacles in the map in the robot coordinate system are correlated with the polar coordinates of the current frame of sonar data using the Mahalanobis distance.
  • the polar coordinates of the new obstacles are converted to the world coordinate system and added to the map.
  • a method for concurrent localization and mapping in a structured environment wherein a robot moves in a physical environment and acquires a point cloud of the surrounding environment, and the robot's motion data and point cloud data are used as input data for a SLAM algorithm; the SLAM algorithm performs motion estimation and concurrent localization and mapping; the method is pre-set with respect to the structured features of the environment;
  • the point cloud acquisition unit outputs a frame of point cloud data within a scanning cycle.
  • the coordinates of each obstacle in the same frame of point cloud data are polar coordinates relative to the position of the point cloud acquisition unit at the start of the scan.
  • the point cloud acquisition unit After the point cloud acquisition unit completes a scanning cycle, it associates the polar coordinates of the obstacle in the map in the robot coordinate system with the polar coordinates of the current frame of point cloud data using the Mahalanobis distance. If the obstacle in the current world coordinate system is a new obstacle, the coordinates of the new obstacle are linearly constrained with the structured feature based on the X-axis coordinate to determine the corresponding relationship between the new obstacle and the structured feature, and the new obstacle is updated to the area corresponding to the structured feature.
  • a device for concurrent positioning and mapping in a structured environment includes at least one processor and at least one memory for storing at least one computer program; the processor executes the computer program on the memory to implement the above-mentioned method for concurrent positioning and mapping in a structured environment.
  • a computer storage medium stores a computer program, which is executed by a computer to implement the above method for concurrent positioning and mapping in a structured environment.
  • a method for concurrent positioning and mapping of an underwater robot for operations under a dock is described.
  • the method generates input data from the movement of the robot in a physical environment.
  • the robot is equipped with an IMU inertial unit and a sonar unit.
  • the IMU inertial unit obtains the angular velocity and acceleration of the robot in the physical environment.
  • the sonar unit obtains the relative position of the obstacle and the sonar, and the relative position is expressed in the polar coordinates of the obstacle in the robot coordinate system.
  • Each scanning cycle of the sonar unit outputs a frame of sonar data.
  • the output of the IMU inertial unit and the sonar unit is the input of the SLAM algorithm, and the SLAM algorithm outputs a map in the world coordinate system.
  • a buoy connected to the robot floats on the water surface.
  • the robot moves, it moves with the buoy, and the shore-based lidar obtains the buoy's position and observed speed.
  • the buoy observation velocity is decomposed into the x-axis velocity and the y-axis velocity, and then updated to the two-dimensional running speed of the robot;
  • the acceleration obtained by the IMU inertial unit is converted into velocity, and the x-axis velocity and the y-axis velocity are decomposed, and then updated to the two-dimensional running speed of the robot;
  • the output data of the sonar unit is the obstacle coordinates in the robot coordinate system.
  • the coordinates of each obstacle in a frame of sonar data are the polar coordinates relative to the sonar position at the start of the scan.
  • the polar coordinates of the obstacle in the map in the robot coordinate system are associated with the polar coordinates of the current frame of sonar data using the Mahalanobis distance.
  • the polar coordinates of the new obstacle are converted to the world coordinate system.
  • the coordinates of the new obstacle and the structured feature are linearly constrained according to the X-axis coordinate to determine the corresponding relationship between the new obstacle and the structured feature.
  • the new obstacle is then updated to the area corresponding to the structured feature.
  • S1 Use data from the IMU and LiDAR as input to the SLAM algorithm to estimate the robot's coarse pose.
  • a buoy is attached to the underwater robot, and the buoy's position is detected using LiDAR on the approach bridge.
  • the LiDAR preprocesses the point cloud by downsampling it to reduce noise and complexity.
  • the density-based spatial clustering of noise applications (DBSCAN) algorithm is then used for clustering and segmentation to extract the buoy's flat position.
  • Other existing algorithms can also be used to segment the point cloud and extract the buoy's flat position.
  • the method for predicting the posture of each frame of the LiDAR is as follows: First, determine whether the angular velocity in the IMU inertial unit is greater than the set threshold. If it does not exceed the threshold and the buoy is observable, it is considered that the robot's movement direction remains unchanged. The movement speed estimated by the LiDAR and the IMU measurement value are used together to predict the robot's current posture; if the angular velocity is greater than the set threshold, the robot's current posture is predicted. If the measured value is greater than the set threshold, it is considered that the robot's motion direction has changed significantly, and only the IMU is used to predict the robot's posture.
  • the perception sensor uses a mechanical scanning sonar to collect data. Due to the slow scanning speed of mechanical scanning sonars, the complete sonar scan image obtained by the AUV during motion will be distorted, resulting in erroneous results when extracting environmental features from such sonar images. Therefore, while the sonar beam is used to capture images, the robot's coarse pose estimated in S1 is used to compensate for image distortion caused by motion and reduce its impact.
  • Each sonar scan cycle outputs a frame of sonar data; the coordinates of each obstacle in a frame of sonar data are polar coordinates relative to the sonar's position at the start of the scan, which serves as the origin of the current coordinate system.
  • Preprocess the sonar data to extract obstacle point features Preprocess the sonar data to extract obstacle point features.
  • the DBSCAN algorithm can be used to extract obstacle point features. Since obstacles in open underwater environments are mostly piles and distant walls, the clustering algorithm considers points within a fixed range as obstacle features in the robot coordinate system. The center point is taken as the point feature. The obstacle coordinates of the current frame of sonar data are correlated with the obstacle polar coordinates on the map using the Mahalanobis distance.

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Electromagnetism (AREA)
  • Acoustics & Sound (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

用于在水下并发定位和地图绘制的方法，机器人上设有IMU惯性单元和声呐单元，机器人潜入水下时，有与机器人相连的浮标浮在水面，机器人行走时联动浮标，由岸上激光雷达获取浮标位置和浮标观测速度；在SLAM算法在进行运动估计时，在角速度低于预设角速度阈值时，用浮标观测速度分解为x轴速度和y轴速度、更新为机器人的二维运行速度；将地图里障碍物在机器人坐标系下的极坐标与当前帧声呐数据的极坐标进行马氏距离关联，将新障碍物的极坐标转换到世界坐标系下，加入地图。本发明具有无需依赖水下的能见度，也无需提前安置设备，具有好的泛化性和稳定性的优点。

Description

用于并发定位和地图绘制的方法、设备、装置及存储介质 技术领域
本发明涉及一种用于并发定位和地图绘制的方法、装置、设备及存储介质，特别是一种适合水下环境和、或结构化环境的用于并发定位和地图绘制的方法、装置、设备及存储介质。
技术背景
SLAM算法是实现机器人定位、建图、路径规划的一种算法。SLAM是同步定位与地图构建(Simultaneous Localization And Mapping)的缩写。SLAM包含了感知、定位、建图这三个过程。感知：机器人能够通过传感器获取周围的环境信息。定位：通过传感器获取的当前和历史信息，推测出自身的位置和姿态。建图：根据自身的位姿以及传感器获取的信息，描绘出自身所处环境的样貌。
发明内容
近年来，大型港口高桩码头后方作业需求越发增大。由于后方回淤问题愈发严重，泥面淤积高度逐年升高，引起桩基损伤断裂，通过对浙江沿海百余个高桩码头泊位后方回淤情况调查发现，约90％的码头存在码头下方及后方严重淤积问题，存在安全隐患。为了满足大型船舶靠泊水深不断增大的需求，码头前沿定期浚深；另一方面，对于水下桩基受损情况等的探伤检测也越发频繁迫切。这些作业的传统方法是的人工作业，但其效率低且危险性高，近年来已经逐步被水下自动化机器人作业所取代。
水下作业机器人存在的主要问题之一是获取作业时的水下位姿，目前较为成熟的水下定位方式主要有以下几类：第一类是声学基线系统定位，主要有长、短、超短基线，一般通过水下事安装有应答基阵，航行器载体上安装有发射基阵，利用声信号通信传结算被测载体的三维位置，其需提前安装应答基阵，且数目随着使用范围迅速增长，且难以泛化。第二类是依托多普勒速度仪(DVL)等里程测量设备进行航位推算，进而计算定位信息，该类方法的优势是较为简单，但DVL成本较高。第三类是依托惯性测量单元(IMU)，通过测量物体三轴姿态角以及加速度来计算定位信息，该类方法的优势是仅凭借IMU而无需外源信息辅助，劣势是IMU测量会有误差累计效应，随着时间的增长，漂移严重，误差较大。第四类是通过相机等光学传感器进行视觉定位，但港口处的海水往往非常浑浊，光学传 感器可见度非常有限，这种环境下，视觉定位难以维持。因此急需一种可以在码头后方及下方为作业机器人提供水下自主定位的方法。
本发明的第一方面，其目的是提供一种无需依赖水下的能见度，也无需提前安置设备，具有好的泛化性和稳定性的在水下并发定位和地图绘制的方法、装置、设备及存储介质。
用于在水下并发定位和地图绘制的方法，该方法由机器人在物理环境下的运动产生输入数据；机器人上设有IMU惯性单元和声呐单元，IMU惯性单元获取机器人在物理环境下的角速度和加速度；声呐单元获取障碍物与声呐的相对位置，该相对位置以机器人坐标系下的障碍物极坐标表达；声呐单元的每一个扫描周期输出一帧声呐数据；IMU惯性单元和声呐单元的输出是SLAM算法的输入，SLAM算法输出世界坐标系下的地图，地图包含第i(i＝1、2、3……)个障碍物在世界坐标系下的坐标[xi，yi]、机器人在世界坐标系下的坐标[x y]和机器人的偏航角度θ、机器人的二维运行速度[u、v]和偏航角角速度r,u表示x轴的运行速度，v表示y轴的运行速度；
其特征在于：机器人潜入水下时，有与机器人相连的浮标浮在水面，机器人行走时联动浮标，由岸上激光雷达获取浮标位置和浮标观测速度；
在SLAM算法在进行运动估计时，在角速度低于预设角速度阈值时，用浮标观测速度分解为x轴速度和y轴速度、更新为机器人的二维运行速度；在角速度低于预设角速度阈值时，使用IMU惯性单元获取的加速度换算成速度、并分解x轴速度和y轴速度、再将其更新为机器人的二维运行速度；
声呐单元的输出数据为机器人坐标系下的障碍物坐标，一帧声呐数据的各障碍物坐标均是相对于扫描起始时声呐所处位置的极坐标；将地图里障碍物在机器人坐标系下的极坐标与当前帧声呐数据的极坐标进行马氏距离关联，将新障碍物的极坐标转换到世界坐标系下，加入地图。
在一些实施例中，每一帧声呐数据输出前执行以下操作：将每一个障碍物与声呐的相对位置转换到机器人坐标系下的极坐标，将障碍物的坐标聚类成多个相互独立的区域，对每个独立区域去几何中心点作为点特征，以点特征的极坐标作为障碍物坐标输出。
在一些实施例中，将机器人坐标系下的障碍物坐标转换为世界坐标系下地图 的障碍物坐标时执行以下操作：获取地图中各障碍物在机器人坐标系下的障碍物极坐标，将当前帧声呐数据的障碍物坐标跟地图的障碍物极坐标进行马氏距离关联，如果出现关联结果大于预设值的，就认为新增障碍物，将新增障碍物转换到世界坐标系，添加到地图；否则，用线性约束扩展卡尔曼滤波对机器人位姿及地图中桩基位置进行修正更新。
在一些实施例中，机器人上设置水压传感器，从水压传感器中获取高程信息，将高程信息作为Z轴值，加入地图；机器人在世界坐标系下的坐标表示为[x，y，z]。
在一些实施例中，SLAM算法中以状态向量X＝[x，y，θ，u，v，r，x1，y1，...，xi，yi]T来表达机器人状态和障碍物位置；已知t-1时刻系统的状态Xt-1与协方差矩阵Pt-1的情况下，t时刻系统运动状态预测模型表达为：


其中，μt＝[ax，ay，r],r为IMU测得的偏航角的角速度，ax为IMU测得的加速度转换到世界坐标系下x轴加速度值，ay为y轴的加速度值；wk是机器人运动时存在的高斯白噪声；是t时刻的协方差矩阵；Gt是运动预测模型对系统状 态向量X的雅各比矩阵；wt是机器人运动时存在的高斯白噪声，Rt是噪声wt的协方差矩阵；
在水下并发定位和地图绘制的设备，包括以下模块：
带有浮标的机器人；
获取机器人在物理环境下的角速度和加速度的IMU惯性单元；
获取障碍物与声呐的相对位置的声呐单元，该相对位置以机器人坐标系下的障碍物极坐标表达；声呐单元的每一个扫描周期输出一帧声呐数据；
运行SLAM算法的计算模块；IMU惯性单元和声呐单元的输出是SLAM算法的输入，SLAM算法输出世界坐标系下的地图，地图包含第i(i＝1、2、3……)个障碍物在世界坐标系下的坐标[xi，yi]、机器人在世界坐标系下的坐标[x y]和机器人的偏航角度θ、机器人的二维运行速度[u、v]和偏航角角速度r,u表示x轴的运行速度，v表示y轴的运行速度；
机器人潜入水下时，有与机器人相连的浮标浮在水面，机器人行走时联动浮标，由岸上激光雷达获取浮标位置和浮标观测速度；
在SLAM算法在进行运动估计时，在角速度低于预设角速度阈值时，用浮标观测速度分解为x轴速度和y轴速度、再更新为机器人的二维运行速度；在角速度低于预设角速度阈值时，使用IMU惯性单元获取的加速度换算成速度、并分解x轴速度和y轴速度、再将其更新为机器人的二维运行速度；
声呐单元的输出数据为机器人坐标系下的障碍物坐标，一帧声呐数据的各障碍物坐标均是相对于扫描起始时声呐所处位置的极坐标；将地图里障碍物在机器人坐标系下的极坐标与当前帧声呐数据的极坐标进行马氏距离关联，将新障碍物的极坐标转换到世界坐标系下，加入地图。
用于水下并发定位和地图绘制的方法装置，该装置包括至少一个处理器，至少一个存储器，用于存储至少一个计算机程序；所述处理器执行存储器上的计算机程序以实现上述用于水下并发定位和地图绘制的方法。
一种计算机存储介质，述计算机存储介质上存储有计算机程序，所述计算机程序被计算机运行以实现上述用于水下并发定位和地图绘制的方法。
另外，以码头环境为例，码头桩基作为码头结构中的关键部分，用于支撑上 部结构并将荷载传递到地基深处，而考虑到载荷传递效率，抗弯，抗剪切能力，桩基的排列往往呈线性结构化排列的形式，即在某一方向上，一列桩基的排布呈一条直线，如图二所示。这种良好的先验结构化约束在以往的技术中也往往被浪费掉。
本发明的第二方面，目的在提供一种在结构化环境下，提升SLAM的估计精度、降低不确定性的用于结构化环境下机器人的同步定位与建图方法、系统、装置和存储介质。
用于结构化环境下并发定位和地图绘制的方法，该方法由机器人在物理环境运动并获取周围环境的点云，以机器人的运动数据和点云数据作为SLAM算法的输入数据；SLAM算法进行运动估计并发定位和地图绘制；该方法预设有环境的结构化特征；
点云获取单元在一个扫描周期内输出一帧点云数据，同一帧点云数据的各障碍物坐标均是相对于扫描起始时点云获取单元所处位置的极坐标；点云获取单元扫描完一个周期后，将地图里障碍物在机器人坐标系下的极坐标与当前帧点云数据的极坐标进行马氏距离关联；如果当前世界坐标系下的障碍物是新障碍物，则将新障碍物的坐标与结构化特征根据X轴坐标进行线性约束，确定新障碍物与结构化特征的对应关系，将新障碍物更新到对应结构化特征的区域内。
在一些实施例中，线性约束的具体执行为：针对更新后得到的新障碍物特征点世界坐标Zn＝(x1，y1，...，xi，yi)n，其在协方差矩阵P中对应的方差向量为(δ1，，...，δi)n，为简化运算假设世界坐标系y轴与桩基排列方向一致，以设定距离阈值dTh来进行判断新障碍物特征点的位置：
其中mx1和mx2分别为结构化环境相邻的第一列特征的横坐标估计值和第二列特征的横坐标估计值，其对应的方差分别为δm1和δm2，xni是所观测到的第i个新障碍物特征点的x轴坐标，yni是所观测到的第i个新障碍物特征点的y轴坐标；对于被判断为第一列或第二列的第i个新障碍物特征点的x轴坐标和方差进 行线性约束更新：
将x轴坐标和该坐标方向的方差δm1写入状态向量X和协方差矩阵P中对应位置。
在一些实施例中，点云获取单元为激光雷达、双目相机、或者声呐。
用于结构化环境下并发定位和地图绘制的装置，该装置包括至少一个处理器，至少一个存储器，用于存储至少一个计算机程序；所述处理器执行存储器上的计算机程序以实现上述用于结构化环境下并发定位和地图绘制的方法。
一种计算机存储介质，述计算机存储介质上存储有计算机程序，所述计算机程序被计算机运行以实现上述用于结构化环境下并发定位和地图绘制的方法。
本发明的第三方面，目的在提供一种能在高桩码头下方对潜入水下的机器人并发定位和地图绘制的方法、装置、设备及存储介质。
用于码头下方作业的水下机器人并发定位和地图绘制的方法，该方法由机器人在物理环境下的运动产生输入数据；机器人上设有IMU惯性单元和声呐单元，IMU惯性单元获取机器人在物理环境下的角速度和加速度；声呐单元获取障碍物与声呐的相对位置，该相对位置以机器人坐标系下的障碍物极坐标表达；声呐单元的每一个扫描周期输出一帧声呐数据；IMU惯性单元和声呐单元的输出是SLAM算法的输入，SLAM算法输出世界坐标系下的地图，地图包含第i(i＝1、2、3……)个障碍物在世界坐标系下的坐标[xi，yi]、机器人在世界坐标系下的坐标[x y]和机器人的偏航角度θ、机器人的二维运行速度[u、v]和偏航角角速度r,u表示x轴的运行速度，v表示y轴的运行速度；
其特征在于：机器人潜入水下时，有与机器人相连的浮标浮在水面，机器人行走时联动浮标，由岸上激光雷达获取浮标位置和浮标观测速度；
在SLAM算法在进行运动估计时，在角速度低于预设角速度阈值时，用浮标观测速度分解为x轴速度和y轴速度、再更新为机器人的二维运行速度；在角速 度低于预设角速度阈值时，使用IMU惯性单元获取的加速度换算成速度、并分解x轴速度和y轴速度、再更新为机器人的二维运行速度；
声呐单元的输出数据为机器人坐标系下的障碍物坐标，一帧声呐数据的各障碍物坐标均是相对于扫描起始时声呐所处位置的极坐标；将地图里障碍物在机器人坐标系下的极坐标与当前帧声呐数据的极坐标进行马氏距离关联，将新障碍物的极坐标转换到世界坐标系下，将新障碍物的坐标与结构化特征根据X轴坐标进行线性约束，确定新障碍物与结构化特征的对应关系，将新障碍物更新到对应结构化特征的区域内。
本发明的优点在于：1、提供了岸上激光雷达观测水面浮标的方式提供机器人的速度信息，比DVL成本低许多。2、使用了多元传感器融合的思想，多种传感器相辅相成，提高SLAM方法稳定性。3、在传统的扩展卡尔曼滤波slam中加入了码头桩基的线性排列约束，提出了线性约束扩展卡尔曼滤波方法，提高SLAM的估计精度，并降低不确定性.4、此方法无需依赖水质及浑浊程度，且无需提前安置设备，具有良好的泛化性。
附图说明
图1是用于在水下并发定位和地图绘制方法的流程图。
图2是码头桩基排布示意图。
图3是用于结构化环境下并发定位和地图绘制方法的流程图。
图4是用于码头下方作业的水下机器人并发定位和地图绘制方法的框图。
图5是用于码头下方作业的水下机器人并发定位和地图绘制方法的流程图。
具体实施方式
如图1所示，用于在水下并发定位和地图绘制的方法，该方法由机器人在物理环境下的运动产生输入数据；机器人上设有IMU惯性单元和声呐单元，IMU惯性单元获取机器人在物理环境下的角速度和加速度；声呐单元获取障碍物与声呐的相对位置，该相对位置以机器人坐标系下的障碍物极坐标表达；声呐单元的每一个扫描周期输出一帧声呐数据；IMU惯性单元和声呐单元的输出是SLAM算法的输入，SLAM算法输出世界坐标系下的地图，地图包含第i(i＝1、2、3……)个障碍物在世界坐标系下的坐标[xi，yi]、机器人在世界坐标系下的坐标[x y]和机器人的偏航角度θ、机器人的二维运行速度[u、v]和偏航角角速度r,u表示x 轴的运行速度，v表示y轴的运行速度；
机器人潜入水下时，有与机器人相连的浮标浮在水面，机器人行走时联动浮标，由岸上激光雷达获取浮标位置和浮标观测速度；
在SLAM算法在进行运动估计时，在角速度低于预设角速度阈值时，用浮标观测速度分解为x轴速度和y轴速度、更新为机器人的二维运行速度；在角速度低于预设角速度阈值时，使用IMU惯性单元获取的加速度换算成速度、并分解x轴速度和y轴速度、再将其更新为机器人的二维运行速度；
声呐单元的输出数据为机器人坐标系下的障碍物坐标，一帧声呐数据的各障碍物坐标均是相对于扫描起始时声呐所处位置的极坐标；将地图里障碍物在机器人坐标系下的极坐标与当前帧声呐数据的极坐标进行马氏距离关联，将新障碍物的极坐标转换到世界坐标系下，加入地图。
在一些实施例中，每一帧声呐数据输出前执行以下操作：将每一个障碍物与声呐的相对位置转换到机器人坐标系下的极坐标，将障碍物的坐标聚类成多个相互独立的区域，对每个独立区域去几何中心点作为点特征，以点特征的极坐标作为障碍物坐标输出。
在一些实施例中，将机器人坐标系下的障碍物坐标转换为世界坐标系下地图的障碍物坐标时执行以下操作：获取地图中各障碍物在机器人坐标系下的障碍物极坐标，将当前帧声呐数据的障碍物坐标跟地图的障碍物极坐标进行马氏距离关联，如果出现关联结果大于预设值的，就认为新增障碍物，将新增障碍物转换到世界坐标系，添加到地图；否则，用线性约束扩展卡尔曼滤波对机器人位姿及地图中桩基位置进行修正更新。
在一些实施例中，机器人上设置水压传感器，从水压传感器中获取高程信息，将高程信息作为Z轴值，加入地图；机器人在世界坐标系下的坐标表示为[x，y，z]。
在一些实施例中，SLAM算法中以状态向量X＝[x，y，θ，u，v，r，x1，y1，...，xi，yi]T来表达机器人状态和障碍物位置；已知t-1时刻系统的状态Xt-1与协方差矩阵Pt-1的情况下，t时刻系统运动状态预测模型表达为：


其中，μt＝[ax，ay，r],r为IMU测得的偏航角的角速度，ax为IMU测得的加速度转换到世界坐标系下x轴加速度值，ay为y轴的加速度值；wk是机器人运动时存在的高斯白噪声；是t时刻的协方差矩阵；Gt是运动预测模型对系统状态向量X的雅各比矩阵；wt是机器人运动时存在的高斯白噪声，Rt是噪声wt的协方差矩阵；
在一些实施例中，用于水下工作的机器人并发定位和地图绘制的设备，包括以下模块：
带有浮标的机器人；
获取机器人在物理环境下的角速度和加速度的IMU惯性单元；
获取障碍物与声呐的相对位置的声呐单元，该相对位置以机器人坐标系下的障碍物极坐标表达；声呐单元的每一个扫描周期输出一帧声呐数据；
运行SLAM算法的计算模块；IMU惯性单元和声呐单元的输出是SLAM算法的输入，SLAM算法输出世界坐标系下的地图，地图包含第i(i＝1、2、3……)个障碍物在世界坐标系下的坐标[xi，yi]、机器人在世界坐标系下的坐标[x y]和机器人的偏航角度θ、机器人的二维运行速度[u、v]和偏航角角速度r,u表示x轴的运行速度，v表示y轴的运行速度；
机器人潜入水下时，有与机器人相连的浮标浮在水面，机器人行走时联动浮标，由岸上激光雷达获取浮标位置和浮标观测速度；
在SLAM算法在进行运动估计时，在角速度低于预设角速度阈值时，用浮标观测速度分解为x轴速度和y轴速度、再更新为机器人的二维运行速度；在角速度低于预设角速度阈值时，使用IMU惯性单元获取的加速度换算成速度、并分解x轴速度和y轴速度、再将其更新为机器人的二维运行速度；
声呐单元的输出数据为机器人坐标系下的障碍物坐标，一帧声呐数据的各障碍物坐标均是相对于扫描起始时声呐所处位置的极坐标；将地图里障碍物在机器人坐标系下的极坐标与当前帧声呐数据的极坐标进行马氏距离关联，将新障碍物的极坐标转换到世界坐标系下，加入地图。
在一些实施例中，用于水下并发定位和地图绘制的方法装置，该装置包括至少一个处理器，至少一个存储器，用于存储至少一个计算机程序；所述处理器执行存储器上的计算机程序以实现上述用于水下并发定位和地图绘制的方法。
在一些实施例中，一种计算机存储介质，述计算机存储介质上存储有计算机程序，所述计算机程序被计算机运行以实现上述用于水下并发定位和地图绘制的方法。
在一些实施例中，如图2、3所示，用于结构化环境下并发定位和地图绘制的方法，该方法由机器人在物理环境运动并获取周围环境的点云，以机器人的运动数据和点云数据作为SLAM算法的输入数据；SLAM算法进行运动估计并发定位和地图绘制；该方法预设有环境的结构化特征；
点云获取单元在一个扫描周期内输出一帧点云数据，同一帧点云数据的各障碍物坐标均是相对于扫描起始时点云获取单元所处位置的极坐标；点云获取单元扫描完一个周期后，将地图里障碍物在机器人坐标系下的极坐标与当前帧点云数据的极坐标进行马氏距离关联；如果当前世界坐标系下的障碍物是新障碍物，则将新障碍物的坐标与结构化特征根据X轴坐标进行线性约束，确定新障碍物与结构化特征的对应关系，将新障碍物更新到对应结构化特征的区域内。
在一些实施例中，线性约束的具体执行为：针对更新后得到的新障碍物特征点世界坐标Zn＝(x1，y1，...，xi，yi)n，其在协方差矩阵P中对应的方差向量为(δ1，，...，δi)n，为简化运算假设世界坐标系y轴与桩基排列方向一致，以设定距 离阈值dTh来进行判断新障碍物特征点的位置：
其中mx1和mx2分别为结构化环境相邻的第一列特征的横坐标估计值和第二列特征的横坐标估计值，其对应的方差分别为δm1和δm2，xni是所观测到的第i个新障碍物特征点的x轴坐标，yni是所观测到的第i个新障碍物特征点的y轴坐标；对于被判断为第一列或第二列的第i个新障碍物特征点的x轴坐标和方差进行线性约束更新：
将x轴坐标和该坐标方向的方差δm1写入状态向量X和协方差矩阵P中对应位置。
在一些实施例中，点云获取单元为激光雷达、双目相机、或者声呐。
在一些实施例中，用于结构化环境下并发定位和地图绘制的装置，该装置包括至少一个处理器，至少一个存储器，用于存储至少一个计算机程序；所述处理器执行存储器上的计算机程序以实现上述用于结构化环境下并发定位和地图绘制的方法。
在一些实施例中，一种计算机存储介质，述计算机存储介质上存储有计算机程序，所述计算机程序被计算机运行以实现上述用于结构化环境下并发定位和地图绘制的方法。
如图4、5所示，用于码头下方作业的水下机器人并发定位和地图绘制的方法，该方法由机器人在物理环境下的运动产生输入数据；机器人上设有IMU惯性单元和声呐单元，IMU惯性单元获取机器人在物理环境下的角速度和加速度；声呐单元获取障碍物与声呐的相对位置，该相对位置以机器人坐标系下的障碍物极坐标表达；声呐单元的每一个扫描周期输出一帧声呐数据；IMU惯性单元和声呐单元的输出是SLAM算法的输入，SLAM算法输出世界坐标系下的地图，地图包含 第i(i＝1、2、3……)个障碍物在世界坐标系下的坐标[xi，yi]、机器人在世界坐标系下的坐标[x y]和机器人的偏航角度θ、机器人的二维运行速度[u、v]和偏航角角速度r,u表示x轴的运行速度，v表示y轴的运行速度；
机器人潜入水下时，有与机器人相连的浮标浮在水面，机器人行走时联动浮标，由岸上激光雷达获取浮标位置和浮标观测速度；
在SLAM算法在进行运动估计时，在角速度低于预设角速度阈值时，用浮标观测速度分解为x轴速度和y轴速度、再更新为机器人的二维运行速度；在角速度低于预设角速度阈值时，使用IMU惯性单元获取的加速度换算成速度、并分解x轴速度和y轴速度、再更新为机器人的二维运行速度；
声呐单元的输出数据为机器人坐标系下的障碍物坐标，一帧声呐数据的各障碍物坐标均是相对于扫描起始时声呐所处位置的极坐标；将地图里障碍物在机器人坐标系下的极坐标与当前帧声呐数据的极坐标进行马氏距离关联，将新障碍物的极坐标转换到世界坐标系下，将新障碍物的坐标与结构化特征根据X轴坐标进行线性约束，确定新障碍物与结构化特征的对应关系，将新障碍物更新到对应结构化特征的区域内。
本发明的各实施例可以独立实施，也可以相互组合，也可以是实施例中的特征相互组合形成技术方案都属于本发明记载的范围。
实施例1
S1：使用IMU惯性单元及激光雷达获取的数据作为SLAM算法的输入，进行机器人粗位姿估计。使用浮标连接水下机器人并在引桥上使用激光雷达探测浮标位置。激光雷达通过得到对点云图进行下采样预处理降低噪点和复杂度随后采用基于密度的噪声应用空间聚类(DBSCAN)算法进行聚类分割，提取浮标平求解位置；也可以使用其他现有算法分割点云提取浮标平求解位置。
由于水流速度影响，浮标位置与水下机器人位置存在一定偏差，但在机器人沿固定方向前进时，短时间内该偏差值近似恒定，因此可提取连续两帧之间的浮标位置之差，作为机器人运行速度粗估计。
判断激光雷达每一帧位姿预测方法如下：首先判断IMU惯性单元中角速度是否大于设定阈值，若未超过阈值，且浮标可观测，认为机器人运动方向不变，使用激光雷达预估的运动速度与IMU测量值一同预测机器人当前时刻位姿；若角速 度测量值大于设定阈值，认为机器人运动方向发生显著改变，仅用IMU进行机器人位姿预测。
S2：感知类传感器机械扫描声纳进行数据采集。由于机械扫描式声纳的扫描速度很慢,使得AUV在运动中得到的完整声纳扫描图像会出现失真,从这种声纳图像中提取环境特征就会产生错误的结果。因此,在利用声纳束捕捉图像的同时,使用S1中估计的机器人粗位姿对运动产生的图像失真进行补偿,降低失真的影响。声的每一个扫描周期输出一帧声呐数据；一帧声呐数据的各障碍物坐标均是相对于扫描起始时声呐所处位置的极坐标，扫描起始时声呐所处位置作为当前坐标系的原点。
对声呐数据进行预处理提取障碍物点特征，可采用DBSCAN算法提取障碍物点特征，由于水下环境空旷障碍物大多为桩基及远处的墙壁。聚类算法中点数在固定范围内的可视为机器人坐标系下的障碍物特征，取其中心点为点特征将当前帧声呐数据的障碍物坐标跟地图的障碍物极坐标进行马氏距离关联；
S3：若关联结果小于设定阈值则认为观测到了地图中存在的桩基点，使用线性约束扩展卡尔曼滤波对机器人位姿及地图中桩基位置进行修正更新；若关联结果大于阈值，则认为出现了新的桩基特征，将其存入地图中，进行全局建图。
实施例2
本实施例与实施例1的区别在于：增加S4：对于修正更新的机器人位姿融合水压传感器的高程信息，以高程信息作为机器人在世界坐标系下的z轴坐标，实现对机器人的三维定位。
上述具体实施案例用来解释说明本发明，仅为本发明的优选实施例，而不是对本发明进行限制，在本发明的精神和权利要求的保护范围内，对本发明作出的任何修改、等同替换、改进等，都落入本发明的保护范围。

Claims (10)

  1. 用于在水下并发定位和地图绘制的方法，该方法由机器人在物理环境下的运动产生输入数据；机器人上设有IMU惯性单元和声呐单元，IMU惯性单元获取机器人在物理环境下的角速度和加速度；声呐单元获取障碍物与声呐的相对位置，该相对位置以机器人坐标系下的障碍物极坐标表达；声呐单元的每一个扫描周期输出一帧声呐数据；IMU惯性单元和声呐单元的输出是SLAM算法的输入，SLAM算法输出世界坐标系下的地图，地图包含第i(i＝1、2、3……)个障碍物在世界坐标系下的坐标[xi，yi]、机器人在世界坐标系下的坐标[x y]和机器人的偏航角度θ、机器人的二维运行速度[u、v]和偏航角角速度r,u表示x轴的运行速度，v表示y轴的运行速度；
    其特征在于：机器人潜入水下时，有与机器人相连的浮标浮在水面，机器人行走时联动浮标，由岸上激光雷达获取浮标位置和浮标观测速度；
    在SLAM算法在进行运动估计时，在角速度低于预设角速度阈值时，用浮标观测速度分解为x轴速度和y轴速度、更新为机器人的二维运行速度；在角速度低于预设角速度阈值时，使用IMU惯性单元获取的加速度换算成速度、并分解x轴速度和y轴速度、再将其更新为机器人的二维运行速度；
    声呐单元的输出数据为机器人坐标系下的障碍物坐标，一帧声呐数据的各障碍物坐标均是相对于扫描起始时声呐所处位置的极坐标；将地图里障碍物在机器人坐标系下的极坐标与当前帧声呐数据的极坐标进行马氏距离关联，将新障碍物的极坐标转换到世界坐标系下，加入地图。
  2. 如权利要求1所述的用于在水下并发定位和地图绘制的方法，其特征在于：每一帧声呐数据输出前执行以下操作：将每一个障碍物与声呐的相对位置转换到机器人坐标系下的极坐标，将障碍物的坐标聚类成多个相互独立的区域，对每个独立区域去几何中心点作为点特征，以点特征的极坐标作为障碍物坐标输出。
  3. 如权利要求1所述的用于在水下并发定位和地图绘制的方法，其特征在于：将机器人坐标系下的障碍物坐标转换为世界坐标系下地图的障碍物坐标时执行以下操作：获取地图中各障碍物在机器人坐标系下的障碍物极坐标，将当前帧声呐数据的障碍物坐标跟地图的障碍物极坐标进行马氏距离关联，如果出现关联结果大于预设值的，就认为新增障碍物，将新增障碍物转换到世界坐标系，添加 到地图；否则，用线性约束扩展卡尔曼滤波对机器人位姿及地图中桩基位置进行修正更新。
  4. 如权利要求1所述的用于在水下并发定位和地图绘制的方法，其特征在于：机器人上设置水压传感器，从水压传感器中获取高程信息，将高程信息作为Z轴值，加入地图；机器人在世界坐标系下的坐标表示为[x，y，z]。
  5. 如权利要求1所述的用于在水下并发定位和地图绘制的方法，其特征在于：SLAM算法中以状态向量X＝[x，y，θ，u，v，r，x1，y1，...，xi，yi]T来表达机器人状态和障碍物位置；已知t-1时刻系统的状态Xt-1与协方差矩阵Pt-1的情况下，t时刻系统运动状态预测模型表达为：


    其中，μt＝[ax，ay，r],r为IMU测得的偏航角的角速度，ax为IMU测得的加速度转换到世界坐标系下x轴加速度值，ay为y轴的加速度值；wk是机器人运动时存在的高斯白噪声；是t时刻的协方差矩阵；Gt是运动预测模型对系统状态向量X的雅各比矩阵；wt是机器人运动时存在的高斯白噪声，Rt是噪声wt的协方差矩阵；
  6. 在水下并发定位和地图绘制的设备，包括以下模块：
    带有浮标的机器人；
    获取机器人在物理环境下的角速度和加速度的IMU惯性单元；
    获取障碍物与声呐的相对位置的声呐单元，该相对位置以机器人坐标系下的障碍物极坐标表达；声呐单元的每一个扫描周期输出一帧声呐数据；
    运行SLAM算法的计算模块；IMU惯性单元和声呐单元的输出是SLAM算法的输入，SLAM算法输出世界坐标系下的地图，地图包含第i(i＝1、2、3……)个障碍物在世界坐标系下的坐标[xi，yi]、机器人在世界坐标系下的坐标[x y]和机器人的偏航角度θ、机器人的二维运行速度[u、v]和偏航角角速度r,u表示x轴的运行速度，v表示y轴的运行速度；
    机器人潜入水下时，有与机器人相连的浮标浮在水面，机器人行走时联动浮标，由岸上激光雷达获取浮标位置和浮标观测速度；
    在SLAM算法在进行运动估计时，在角速度低于预设角速度阈值时，用浮标观测速度分解为x轴速度和y轴速度、再更新为机器人的二维运行速度；在角速度低于预设角速度阈值时，使用IMU惯性单元获取的加速度换算成速度、并分解x轴速度和y轴速度、再将其更新为机器人的二维运行速度；
    声呐单元的输出数据为机器人坐标系下的障碍物坐标，一帧声呐数据的各障碍物坐标均是相对于扫描起始时声呐所处位置的极坐标；将地图里障碍物在机器人坐标系下的极坐标与当前帧声呐数据的极坐标进行马氏距离关联，将新障碍物的极坐标转换到世界坐标系下，加入地图。
  7. 用于水下并发定位和地图绘制的方法装置，该装置包括至少一个处理器，至少一个存储器，用于存储至少一个计算机程序；所述处理器执行存储器上的计算机程序以实现上述用于水下并发定位和地图绘制的方法。
  8. 一种计算机存储介质，述计算机存储介质上存储有计算机程序，所述计算机程序被计算机运行以实现上述用于水下并发定位和地图绘制的方法。
  9. 用于结构化环境下并发定位和地图绘制的方法，该方法由机器人在物理环境运动并获取周围环境的点云，以机器人的运动数据和点云数据作为SLAM算法的输入数据；SLAM算法进行运动估计并发定位和地图绘制；该方法预设有环境的结构化特征；
    点云获取单元在一个扫描周期内输出一帧点云数据，同一帧点云数据的各障 碍物坐标均是相对于扫描起始时点云获取单元所处位置的极坐标；点云获取单元扫描完一个周期后，将地图里障碍物在机器人坐标系下的极坐标与当前帧点云数据的极坐标进行马氏距离关联；如果当前世界坐标系下的障碍物是新障碍物，则将新障碍物的坐标与结构化特征根据X轴坐标进行线性约束，确定新障碍物与结构化特征的对应关系，将新障碍物更新到对应结构化特征的区域内。
  10. 用于结构化环境下并发定位和地图绘制的方法，其特征在于，线性约束的具体执行为：针对更新后得到的新障碍物特征点世界坐标Zn＝(x1，y1，...，xi，yi)n，其在协方差矩阵P中对应的方差向量为(δ1，，...，δi)n，为简化运算假设世界坐标系y轴与桩基排列方向一致，以设定距离阈值dTh来进行判断新障碍物特征点的位置：
    其中mx1和mx2分别为结构化环境相邻的第一列特征的横坐标估计值和第二列特征的横坐标估计值，其对应的方差分别为δm1和δm2，xni是所观测到的第i个新障碍物特征点的x轴坐标，yni是所观测到的第i个新障碍物特征点的y轴坐标；对于被判断为第一列或第二列的第i个新障碍物特征点的x轴坐标和方差进行线性约束更新：
    将x轴坐标和该坐标方向的方差δm1写入状态向量X和协方差矩阵P中对应位置。
PCT/CN2024/086416 2024-04-01 2024-04-07 用于并发定位和地图绘制的方法、设备、装置及存储介质 Pending WO2025208653A1 (zh)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN202410389018.9 2024-04-01
CN202410389018.9A CN118424284B (zh) 2024-04-01 2024-04-01 用于并发定位和地图绘制的方法、设备、装置及存储介质

Publications (1)

Publication Number Publication Date
WO2025208653A1 true WO2025208653A1 (zh) 2025-10-09

Family

ID=92322449

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/CN2024/086416 Pending WO2025208653A1 (zh) 2024-04-01 2024-04-07 用于并发定位和地图绘制的方法、设备、装置及存储介质

Country Status (2)

Country Link
CN (2) CN122384813A (zh)
WO (1) WO2025208653A1 (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN121325188A (zh) * 2025-10-28 2026-01-13 上海船舶运输科学研究所有限公司 基于多源激光雷达感知融合的船舶智能引水方法及系统
CN121346766A (zh) * 2025-10-20 2026-01-16 中国煤炭科工集团太原研究院有限公司 一种煤矿掘进巷道中掘锚一体机定位方法与系统
CN122086079A (zh) * 2026-04-24 2026-05-26 国网浙江省电力有限公司电力科学研究院 基于四足机器人的电缆隧道巡视方法、系统、设备及介质

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20100049436A1 (en) * 2008-08-20 2010-02-25 Lockheed Martin Corporation Undersea position and velocity measuring system and process
CN206192363U (zh) * 2016-08-19 2017-05-24 长信智控网络科技有限公司 应用于内河河道的导航系统
CN107231181A (zh) * 2017-05-03 2017-10-03 上海交通大学 一种跨介质通信的海空协同监测系统及其使用方法
CN109856638A (zh) * 2019-02-28 2019-06-07 中国计量大学 一种特定水下目标自动搜索定位的方法
WO2020192000A1 (zh) * 2019-03-27 2020-10-01 浙江大学 一种基于自主导航的畜禽信息感知机器人与地图构建方法
CN113985419A (zh) * 2021-10-22 2022-01-28 中国科学院合肥物质科学研究院 水面机器人协同障碍物检测及避障方法及系统
CN115933714A (zh) * 2022-12-19 2023-04-07 杭州聆巡科技有限公司 一种水下机器人辅助作业系统及方法

Family Cites Families (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20060287824A1 (en) * 2005-01-29 2006-12-21 American Gnc Corporation Interruption free navigator
CN107833270B (zh) * 2017-09-28 2020-07-03 浙江大学 基于深度相机的实时物体三维重建方法
CN111258313B (zh) * 2020-01-20 2022-06-07 深圳市普渡科技有限公司 多传感器融合slam系统及机器人
CN112562052B (zh) * 2020-12-03 2021-07-27 广东工业大学 一种面向近岸水域的实时定位与建图方法
CN112946660A (zh) * 2021-01-28 2021-06-11 西北工业大学 一种基于多波束前视声呐的同时定位与建图方法
CN113075665B (zh) * 2021-03-24 2023-06-20 鹏城实验室 水下定位方法、水下载体航行器及计算机可读存储介质
CN113340295B (zh) * 2021-06-16 2021-12-21 广东工业大学 一种多个测距传感器的无人艇近岸实时定位与建图方法
CN116380079B (zh) * 2023-04-12 2025-03-25 东南大学 一种融合前视声呐与orb-slam3的水下slam方法
CN116659510B (zh) * 2023-06-02 2024-07-26 海南大学 水下机器人定位与避障方法、装置及存储介质

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20100049436A1 (en) * 2008-08-20 2010-02-25 Lockheed Martin Corporation Undersea position and velocity measuring system and process
CN206192363U (zh) * 2016-08-19 2017-05-24 长信智控网络科技有限公司 应用于内河河道的导航系统
CN107231181A (zh) * 2017-05-03 2017-10-03 上海交通大学 一种跨介质通信的海空协同监测系统及其使用方法
CN109856638A (zh) * 2019-02-28 2019-06-07 中国计量大学 一种特定水下目标自动搜索定位的方法
WO2020192000A1 (zh) * 2019-03-27 2020-10-01 浙江大学 一种基于自主导航的畜禽信息感知机器人与地图构建方法
CN113985419A (zh) * 2021-10-22 2022-01-28 中国科学院合肥物质科学研究院 水面机器人协同障碍物检测及避障方法及系统
CN115933714A (zh) * 2022-12-19 2023-04-07 杭州聆巡科技有限公司 一种水下机器人辅助作业系统及方法

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN121346766A (zh) * 2025-10-20 2026-01-16 中国煤炭科工集团太原研究院有限公司 一种煤矿掘进巷道中掘锚一体机定位方法与系统
CN121325188A (zh) * 2025-10-28 2026-01-13 上海船舶运输科学研究所有限公司 基于多源激光雷达感知融合的船舶智能引水方法及系统
CN122086079A (zh) * 2026-04-24 2026-05-26 国网浙江省电力有限公司电力科学研究院 基于四足机器人的电缆隧道巡视方法、系统、设备及介质

Also Published As

Publication number Publication date
CN122384813A (zh) 2026-07-14
CN118424284A (zh) 2024-08-02
CN118424284B (zh) 2026-05-15

Similar Documents

Publication Publication Date Title
KR102566724B1 (ko) 항만 모니터링 장치 및 항만 모니터링 방법
Rahman et al. Sonar visual inertial SLAM of underwater structures
KR102530691B1 (ko) 접안 모니터링 장치 및 방법
Ribas et al. SLAM using an imaging sonar for partially structured underwater environments
CN102042835B (zh) 自主式水下机器人组合导航系统
Ribas et al. Underwater SLAM in a marina environment
CN118424284B (zh) 用于并发定位和地图绘制的方法、设备、装置及存储介质
CN207908979U (zh) 一种无人船的目标识别追踪系统
CN116659510A (zh) 水下机器人定位与避障方法、装置及存储介质
Lin et al. Environment perception and object tracking for autonomous vehicles in a harbor scenario
CN114061565B (zh) 一种无人船舶slam及其应用方法
Noguchi et al. Tracking omnidirectional surfaces using a low-cost autonomous underwater vehicle
Yin et al. Study on underwater simultaneous localization and mapping based on different sensors
KR102278674B1 (ko) 해수면을 고려한 항만 및 선박 모니터링 방법 및 장치
Akram et al. Long-range vision-based UAV-assisted localization for unmanned Surface Vehicles
Yu et al. A dynamic docking system of AUV based on bearings-only acoustic and visual coupled localization
CN112611376B (zh) 一种RGI-Lidar/SINS紧耦合AUV水下导航定位方法与系统
Aulinas et al. Vision-based underwater SLAM for the SPARUS AUV
Kondo et al. Detailed object observation by autonomous underwater vehicle with localization involving uncertainty of magnetic bearings
Noguchi et al. Wide area seafloor imaging by a low-cost AUV
Han et al. Three-dimensional reconstruction of a semi-submersible offshore platform with an unmanned surface vehicle
Kazmi et al. Dam wall detection and tracking using a mechanically scanned imaging sonar
Sangekar et al. Development of a landing algorithm for autonomous underwater vehicles using laser profiling
JP7328378B2 (ja) 水域物体検出システム、船舶および周辺物体検出システム
Cristi et al. Motion estimation and modeling of the environment for underwater vehicles

Legal Events

Date Code Title Description
ENP Entry into the national phase

Ref document number: 2025565887

Country of ref document: JP

Kind code of ref document: A

WWE Wipo information: entry into national phase

Ref document number: 2025565887

Country of ref document: JP

121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 24933589

Country of ref document: EP

Kind code of ref document: A1