Method, device, apparatus and storage medium for concurrent localization and mapping

Abstract

The application relates to a method for concurrent positioning and mapping under water. An IMU inertial unit and a sonar unit are arranged on a robot. When the robot dives into water, a buoy connected with the robot floats on the water surface. The robot walks and links the buoy. The position and observation speed of the buoy are obtained by a laser radar on the shore. When the SLAM algorithm performs motion estimation, the observation speed of the buoy is decomposed into x-axis speed and y-axis speed and updated into two-dimensional running speed of the robot when the angular velocity is lower than a preset angular velocity threshold. The polar coordinates of obstacles in the robot coordinate system in the map are associated with the polar coordinates of current frame sonar data by Mahalanobis distance, the polar coordinates of new obstacles are converted into the world coordinate system, and the new obstacles are added into the map. The application has the advantages of not needing to depend on underwater visibility and not needing to place equipment in advance, and has good generalization and stability.

Description

Technical Field

[0001] This invention relates to a method, apparatus, device, and storage medium for concurrent positioning and mapping, particularly a method, apparatus, device, and storage medium suitable for underwater and / or structured environments. Technical Background

[0002] SLAM (Simultaneous Localization and Mapping) is an algorithm for robot localization, mapping, and path planning. SLAM comprises three processes: perception, localization, and mapping. Perception: The robot acquires information about its surrounding environment through sensors. Localization: Using current and historical information from sensors, the robot infers its own position and orientation. Mapping: Based on its own pose and sensor information, the robot depicts the appearance of its environment. Summary of the Invention

[0003] In recent years, the demand for backfill operations at large port high-pile berths has been increasing. Due to the increasingly severe backfilling problem, the height of silt accumulation on the surface has been rising year by year, causing damage and fractures to the pile foundations. A survey of over 100 high-pile berths along the Zhejiang coast revealed that approximately 90% of the berths have severe siltation problems beneath and behind the berths, posing safety hazards. To meet the increasing berthing depth requirements of large vessels, the berth front is regularly dredged; on the other hand, the need for flaw detection of underwater pile foundation damage is becoming increasingly frequent and urgent. Traditionally, these operations were conducted manually, but this method is inefficient and highly dangerous, and has been gradually replaced by automated underwater robotic operations in recent years.

[0004] One of the main problems with underwater robots is obtaining their underwater pose during operation. Currently, the more mature underwater positioning methods mainly fall into the following categories: The first is acoustic baseline system positioning, which includes long, short, and ultra-short baselines. This typically involves an underwater transponder array installed on the underwater vehicle and a transmitting array installed on the vehicle itself. The three-dimensional position of the measured vehicle is calculated using acoustic signal communication. This method requires the pre-installation of transponder arrays, and the number of arrays increases rapidly with the scope of application, making it difficult to generalize. The second category relies on Doppler velocity measurement equipment (DVL) for dead reckoning to calculate positioning information. This method is relatively simple, but DVL is costly. The third category relies on inertial measurement units (IMUs) to calculate positioning information by measuring the object's three-axis attitude angles and acceleration. This method is advantageous because it relies solely on the IMU without external information assistance. However, the disadvantage is that IMU measurements have a cumulative error effect, leading to significant drift and large errors over time. The fourth type uses visual positioning via optical sensors such as cameras. However, the seawater in ports is often very turbid, and the visibility of optical sensors is very limited. In such an environment, visual positioning is difficult to maintain. Therefore, there is an urgent need for a method that can provide underwater autonomous positioning for operating robots behind and below the dock.

[0005] A first aspect of the present invention aims to provide a method, apparatus, device, and storage medium for concurrent underwater positioning and mapping that does not rely on underwater visibility or require prior equipment placement, and has good generalizability and stability.

[0006] A method for concurrent underwater localization and mapping, which generates input data from the robot's motion in a physical environment; the robot is equipped with an IMU (Inertial Measurement Unit) and a sonar unit. The IMU acquires the robot's angular velocity and acceleration in the physical environment; the sonar unit acquires the relative position of obstacles to the sonar, expressed in polar coordinates of the obstacles in the robot's coordinate system; the sonar unit outputs one frame of sonar data per scan cycle; the outputs of the IMU and sonar units are the inputs to a SLAM (Simultaneous Localization and Mapping) algorithm, which outputs a map in the world coordinate system, containing the first... i ( i The coordinates of (1, 2, 3...) obstacles in the world coordinate system The robot's coordinates in the world coordinate system and the robot's yaw angle The robot's two-dimensional running speed u、v ] and yaw angle and angular velocity r, u Indicates the speed of movement along the x-axis. v Indicates the speed of movement along the y-axis;

[0007] Its features include: when the robot dives underwater, a buoy connected to the robot floats on the water surface; when the robot moves, it moves in conjunction with the buoy, and the buoy's position and observation speed are obtained by a lidar on the shore.

[0008] When the SLAM algorithm performs motion estimation, if the angular velocity is lower than a preset angular velocity threshold, the velocity observed by the buoy is decomposed into x-axis velocity and y-axis velocity and updated to the robot's two-dimensional running velocity; if the angular velocity is higher than the preset angular velocity threshold, the acceleration obtained by the IMU inertial unit is converted into velocity, and the x-axis velocity and y-axis velocity are decomposed and updated to the robot's two-dimensional running velocity.

[0009] The output data of the sonar unit is the coordinates of the obstacles in the robot coordinate system. The coordinates of each obstacle in a frame of sonar data are polar coordinates relative to the position of the sonar at the start of the scan. The polar coordinates of the obstacles in the robot coordinate system in the map are correlated with the polar coordinates of the current frame of sonar data using Mahalanobis distance. The polar coordinates of the new obstacles are transformed to the world coordinate system and added to the map.

[0010] In some embodiments, the following operations are performed before each frame of sonar data is output: the relative position of each obstacle to the sonar is transformed to polar coordinates in the robot coordinate system, the coordinates of the obstacles are clustered into multiple independent regions, the geometric center point of each independent region is taken as a point feature, and the polar coordinates of the point feature are used as the obstacle coordinates for output.

[0011] In some embodiments, when converting obstacle coordinates in the robot coordinate system to obstacle coordinates in the world coordinate system, the following operations are performed: obtain the polar coordinates of each obstacle in the robot coordinate system, correlate the obstacle coordinates of the current frame sonar data with the polar coordinates of the obstacles in the map using Mahalanobis distance, and if the correlation result is greater than a preset value, it is considered a new obstacle, the new obstacle is converted to the world coordinate system and added to the map; otherwise, the robot pose and the position of the pile foundation in the map are corrected and updated using linearly constrained extended Kalman filtering.

[0012] In some embodiments, a water pressure sensor is installed on the robot to obtain elevation information, which is then used as the Z-axis value and added to the map; the robot's coordinates in the world coordinate system are represented as [ ].

[0013] In some embodiments, the SLAM algorithm uses state vectors To express the robot's state and the obstacle's position; the system's state at time t-1 is known. With covariance matrix In the case of t, the system motion state prediction model at time t is expressed as:

[0014]

[0015]

[0016]

[0017] in, , The angular velocity of the yaw angle measured by the IMU. To convert the acceleration measured by the IMU to the x-axis acceleration value in the world coordinate system, This represents the acceleration value along the y-axis; It is Gaussian white noise present during robot movement; It is the covariance matrix at time t; It is the motion prediction model for the system state vector Jacobian matrix; It is Gaussian white noise present when the robot moves. yes The covariance matrix;

[0018] The device for concurrent underwater positioning and mapping includes the following modules:

[0019] Robots equipped with buoys;

[0020] An IMU (Inertial Measurement Unit) that acquires the angular velocity and acceleration of a robot in a physical environment;

[0021] A sonar unit acquires the relative position of an obstacle and the sonar, which is expressed in the polar coordinates of the obstacle in the robot coordinate system; each scan cycle of the sonar unit outputs one frame of sonar data.

[0022] The computational module that runs the SLAM algorithm; the outputs of the IMU inertial unit and sonar unit are the inputs to the SLAM algorithm. The SLAM algorithm outputs a map in the world coordinate system, which contains the... i ( i The coordinates of (1, 2, 3...) obstacles in the world coordinate system The robot's coordinates in the world coordinate system and the robot's yaw angle The robot's two-dimensional running speed [u, v] and yaw angle angular velocity r, where u represents the running speed along the x-axis and v represents the running speed along the y-axis;

[0023] When the robot dives underwater, a buoy connected to the robot floats on the surface. When the robot moves, it moves in conjunction with the buoy, and the buoy's position and observation speed are obtained by the lidar on the shore.

[0024] When the SLAM algorithm performs motion estimation, if the angular velocity is lower than a preset angular velocity threshold, the velocity observed by the buoy is decomposed into x-axis velocity and y-axis velocity, and then updated into the robot's two-dimensional running velocity; if the angular velocity is higher than the preset angular velocity threshold, the acceleration obtained by the IMU inertial unit is converted into velocity, and decomposed into x-axis velocity and y-axis velocity, and then updated into the robot's two-dimensional running velocity.

[0025] The output data of the sonar unit is the coordinates of the obstacles in the robot coordinate system. The coordinates of each obstacle in a frame of sonar data are polar coordinates relative to the position of the sonar at the start of the scan. The polar coordinates of the obstacles in the robot coordinate system in the map are correlated with the polar coordinates of the current frame of sonar data using Mahalanobis distance. The polar coordinates of the new obstacles are transformed to the world coordinate system and added to the map.

[0026] An apparatus for underwater concurrent positioning and mapping, the apparatus comprising at least one processor and at least one memory for storing at least one computer program; the processor executes the computer program in the memory to implement the above-described method for underwater concurrent positioning and mapping.

[0027] A computer storage medium storing a computer program, the computer program being executed by a computer to implement the above-described method for underwater concurrent positioning and mapping.

[0028] Furthermore, taking a wharf environment as an example, wharf pile foundations, as a key component of the wharf structure, are used to support the superstructure and transfer loads to the depths of the foundation. Considering load transfer efficiency, bending resistance, and shear resistance, the pile foundations are often arranged in a linear, structured manner, meaning that in a certain direction, a row of pile foundations is arranged in a straight line, such as... Figure 2 As shown. Such good prior structured constraints have often been wasted in previous techniques.

[0029] A second aspect of the present invention aims to provide a method, system, apparatus, and storage medium for simultaneous localization and mapping of robots in structured environments that improves the estimation accuracy and reduces uncertainty in SLAM.

[0030] A method for concurrent localization and mapping in structured environments involves a robot moving in a physical environment and acquiring point clouds of the surrounding environment. 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. This method is based on the pre-defined structured features of the environment.

[0031] The point cloud acquisition unit outputs one frame of point cloud data within one 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. After the point cloud acquisition unit completes one scanning cycle, it performs Mahalanobis distance correlation between the polar coordinates of the obstacles in the robot coordinate system and the polar coordinates of the current frame of point cloud data. If the obstacle in the current world coordinate system is a new obstacle, the coordinates of the new obstacle and the structured features are linearly constrained according to the X-axis coordinate to determine the correspondence between the new obstacle and the structured features, and the new obstacle is updated to the region of the corresponding structured feature.

[0032] In some embodiments, the linear constraint is specifically implemented as follows: for the updated world coordinates of the new obstacle feature points... Its variance vector in the covariance matrix P is To simplify calculations, it is assumed that the y-axis of the world coordinate system is aligned with the direction of the pile foundation arrangement, and a distance threshold is set. To determine the location of new obstacle feature points:

[0033]

[0034] in and These are the x-coordinate estimates of the first and second column features adjacent to each other in the structured environment, respectively, and their corresponding variances are respectively... and , x ni It is the i-th new obstacle feature point observed. x Axis coordinates y ni It is the i-th new obstacle feature point observed. y Axis coordinates; for the first or second column... A new obstacle feature point x Linear constraints are applied to update the axis coordinates and variance:

[0035]

[0036] Will x The variance of the axis coordinates and the direction of those coordinates Write state vector Covariance Matrix The corresponding position in the middle.

[0037] In some embodiments, the point cloud acquisition unit is a lidar, a binocular camera, or a sonar.

[0038] An apparatus for concurrent localization and mapping in a structured environment, the apparatus comprising at least one processor and at least one memory for storing at least one computer program; the processor executing the computer program in the memory to implement the above-described method for concurrent localization and mapping in a structured environment.

[0039] A computer storage medium storing a computer program, the computer program being executed by a computer to implement the above-described method for concurrent localization and map creation in a structured environment.

[0040] A third aspect of the present invention aims to provide a method, apparatus, device, and storage medium for concurrent localization and mapping of a submerged robot below a high-pile pier.

[0041] A method for concurrent localization and mapping of underwater robots operating below a dock. This method generates input data from the robot's motion in a physical environment. The robot is equipped with an IMU (Inertial Measurement Unit) and a sonar unit. The IMU acquires the robot's angular velocity and acceleration in the physical environment. The sonar unit acquires the relative position of obstacles to the sonar, expressed in polar coordinates of the obstacles in the robot's coordinate system. The sonar unit outputs one frame of sonar data per scan cycle. The outputs of the IMU and sonar units serve as input to a SLAM (Simultaneous Localization and Mapping) algorithm. The SLAM algorithm outputs a map in the world coordinate system, containing the first... i ( i The coordinates of (1, 2, 3...) obstacles in the world coordinate system The robot's coordinates in the world coordinate system and the robot's yaw angle The robot's two-dimensional running speed u、v ] and yaw angle angular velocity r , u express x The operating speed of the shaft, v express y The operating speed of the shaft;

[0042] Its features include: when the robot dives underwater, a buoy connected to the robot floats on the water surface; when the robot moves, it moves in conjunction with the buoy, and the buoy's position and observation speed are obtained by a lidar on the shore.

[0043] When the SLAM algorithm performs motion estimation, if the angular velocity is lower than a preset angular velocity threshold, the velocity observed by the buoy is decomposed into x-axis velocity and y-axis velocity, and then updated to the robot's two-dimensional running velocity; if the angular velocity is higher than the preset angular velocity threshold, the acceleration obtained by the IMU inertial unit is converted into velocity, and decomposed into x-axis velocity and y-axis velocity, and then updated to the robot's two-dimensional running velocity.

[0044] The output data of the sonar unit is the coordinates of obstacles in the robot coordinate system. The coordinates of each obstacle in a frame of sonar data are polar coordinates relative to the position of the sonar at the start of the scan. The polar coordinates of the obstacles in the robot coordinate system in the map are correlated with the polar coordinates of the current frame of sonar data using Mahalanobis distance. The polar coordinates of the new obstacles are transformed to the world coordinate system. The coordinates of the new obstacles and the structured features are linearly constrained according to the X-axis coordinate to determine the correspondence between the new obstacles and the structured features. The new obstacles are then updated to the region of the corresponding structured features.

[0045] The advantages of this invention are: 1. It provides robot speed information by observing surface buoys using onshore lidar, which is much cheaper than DVL. 2. It uses the concept of multi-sensor fusion, where multiple sensors complement each other to improve the stability of the SLAM method. 3. It incorporates the linear arrangement constraint of the dock pile foundation into the traditional extended Kalman filter SLAM, proposing a linear constraint extended Kalman filter method to improve the estimation accuracy of SLAM and reduce uncertainty. 4. This method is independent of water quality and turbidity, and does not require pre-installation of equipment, exhibiting good generalization ability. Attached Figure Description

[0046] Figure 1 This is a flowchart for a method of concurrent underwater localization and mapping.

[0047] Figure 2 This is a schematic diagram of the layout of the dock's pile foundations.

[0048] Figure 3 It is a flowchart for concurrent localization and map drawing methods in a structured environment.

[0049] Figure 4 This is a block diagram of a method for concurrent localization and mapping of underwater robots used in operations below a dock.

[0050] Figure 5 This is a flowchart of a method for concurrent localization and mapping of underwater robots used in operations below a dock. Detailed Implementation

[0051] like Figure 1As shown, this method is used for concurrent underwater localization and mapping. The method generates input data from the robot's motion in a physical environment. The robot is equipped with an IMU (Inertial Measurement Unit) and a sonar unit. The IMU acquires the robot's angular velocity and acceleration in the physical environment. The sonar unit acquires the relative position of obstacles to the sonar, expressed in polar coordinates of the obstacles in the robot's coordinate system. The sonar unit outputs one frame of sonar data per scan cycle. The outputs of the IMU and sonar units are the inputs to a SLAM (Simultaneous Localization and Mapping) algorithm. The SLAM algorithm outputs a map in the world coordinate system, containing the first... i ( i The coordinates of (1, 2, 3...) obstacles in the world coordinate system The robot's coordinates in the world coordinate system and the robot's yaw angle The robot's two-dimensional running speed u、v ] and yaw angle and angular velocity r, u Indicates the speed of movement along the x-axis. v Indicates the speed of movement along the y-axis;

[0052] When the robot dives underwater, a buoy connected to the robot floats on the surface. When the robot moves, it moves in conjunction with the buoy, and the buoy's position and observation speed are obtained by the lidar on the shore.

[0053] When the SLAM algorithm performs motion estimation, if the angular velocity is lower than a preset angular velocity threshold, the velocity observed by the buoy is decomposed into x-axis velocity and y-axis velocity and updated to the robot's two-dimensional running velocity; if the angular velocity is higher than the preset angular velocity threshold, the acceleration obtained by the IMU inertial unit is converted into velocity, and the x-axis velocity and y-axis velocity are decomposed and updated to the robot's two-dimensional running velocity.

[0054] The output data of the sonar unit is the coordinates of the obstacles in the robot coordinate system. The coordinates of each obstacle in a frame of sonar data are polar coordinates relative to the position of the sonar at the start of the scan. The polar coordinates of the obstacles in the robot coordinate system in the map are correlated with the polar coordinates of the current frame of sonar data using Mahalanobis distance. The polar coordinates of the new obstacles are transformed to the world coordinate system and added to the map.

[0055] In some embodiments, the following operations are performed before each frame of sonar data is output: the relative position of each obstacle to the sonar is transformed to polar coordinates in the robot coordinate system, the coordinates of the obstacles are clustered into multiple independent regions, the geometric center point of each independent region is taken as a point feature, and the polar coordinates of the point feature are used as the obstacle coordinates for output.

[0056] In some embodiments, when converting obstacle coordinates in the robot coordinate system to obstacle coordinates in the world coordinate system, the following operations are performed: obtain the polar coordinates of each obstacle in the robot coordinate system, correlate the obstacle coordinates of the current frame sonar data with the polar coordinates of the obstacles in the map using Mahalanobis distance, and if the correlation result is greater than a preset value, it is considered a new obstacle, the new obstacle is converted to the world coordinate system and added to the map; otherwise, the robot pose and the position of the pile foundation in the map are corrected and updated using linearly constrained extended Kalman filtering.

[0057] In some embodiments, a water pressure sensor is installed on the robot to obtain elevation information, which is then used as the Z-axis value and added to the map; the robot's coordinates in the world coordinate system are represented as [ ].

[0058] In some embodiments, the SLAM algorithm uses state vectors To express the robot's state and the obstacle's position; the system's state at time t-1 is known. With covariance matrix In the case of t, the system motion state prediction model at time t is expressed as:

[0059]

[0060]

[0061]

[0062] in, , The angular velocity of the yaw angle measured by the IMU. To convert the acceleration measured by the IMU to the x-axis acceleration value in the world coordinate system, This represents the acceleration value along the y-axis; It is Gaussian white noise present during robot movement; It is the covariance matrix at time t; It is the motion prediction model for the system state vector Jacobian matrix; It is Gaussian white noise present when the robot moves. yes The covariance matrix;

[0063] In some embodiments, a device for concurrent localization and mapping of robots working underwater includes the following modules:

[0064] Robots equipped with buoys;

[0065] An IMU (Inertial Measurement Unit) that acquires the angular velocity and acceleration of a robot in a physical environment;

[0066] A sonar unit acquires the relative position of an obstacle and the sonar, which is expressed in the polar coordinates of the obstacle in the robot coordinate system; each scan cycle of the sonar unit outputs one frame of sonar data.

[0067] The computational module that runs the SLAM algorithm; the outputs of the IMU inertial unit and sonar unit are the inputs to the SLAM algorithm. The SLAM algorithm outputs a map in the world coordinate system, which contains the... i ( i The coordinates of (1, 2, 3...) obstacles in the world coordinate system The robot's coordinates in the world coordinate system and the robot's yaw angle The robot's two-dimensional running speed [u, v] and yaw angle angular velocity r, where u represents the running speed along the x-axis and v represents the running speed along the y-axis;

[0068] When the robot dives underwater, a buoy connected to the robot floats on the surface. When the robot moves, it moves in conjunction with the buoy, and the buoy's position and observation speed are obtained by the lidar on the shore.

[0069] When the SLAM algorithm performs motion estimation, if the angular velocity is lower than a preset angular velocity threshold, the velocity observed by the buoy is decomposed into x-axis velocity and y-axis velocity, and then updated into the robot's two-dimensional running velocity; if the angular velocity is higher than the preset angular velocity threshold, the acceleration obtained by the IMU inertial unit is converted into velocity, and decomposed into x-axis velocity and y-axis velocity, and then updated into the robot's two-dimensional running velocity.

[0070] The output data of the sonar unit is the coordinates of the obstacles in the robot coordinate system. The coordinates of each obstacle in a frame of sonar data are polar coordinates relative to the position of the sonar at the start of the scan. The polar coordinates of the obstacles in the robot coordinate system in the map are correlated with the polar coordinates of the current frame of sonar data using Mahalanobis distance. The polar coordinates of the new obstacles are transformed to the world coordinate system and added to the map.

[0071] In some embodiments, an apparatus for underwater concurrent positioning and mapping includes at least one processor and at least one memory for storing at least one computer program; the processor executes the computer program in the memory to implement the above-described method for underwater concurrent positioning and mapping.

[0072] In some embodiments, a computer storage medium stores a computer program that is executed by a computer to implement the above-described method for underwater concurrent positioning and mapping.

[0073] In some embodiments, such as Figure 2 ,3 As shown, this method is used for concurrent localization and mapping in a structured environment. The method involves a robot moving in the physical environment and acquiring point clouds of the surrounding environment. The robot's motion data and point cloud data are used as input data for the SLAM algorithm. The SLAM algorithm performs motion estimation and concurrent localization and mapping. The method is based on the structured features of the environment.

[0074] The point cloud acquisition unit outputs one frame of point cloud data within one 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. After the point cloud acquisition unit completes one scanning cycle, it performs Mahalanobis distance correlation between the polar coordinates of the obstacles in the robot coordinate system and the polar coordinates of the current frame of point cloud data. If the obstacle in the current world coordinate system is a new obstacle, the coordinates of the new obstacle and the structured features are linearly constrained according to the X-axis coordinate to determine the correspondence between the new obstacle and the structured features, and the new obstacle is updated to the region of the corresponding structured feature.

[0075] In some embodiments, the linear constraint is specifically implemented as follows: for the updated world coordinates of the new obstacle feature points... Its variance vector in the covariance matrix P is To simplify calculations, it is assumed that the y-axis of the world coordinate system is aligned with the direction of the pile foundation arrangement, and a distance threshold is set. To determine the location of new obstacle feature points:

[0076]

[0077] in and These are the x-coordinate estimates of the first and second column features adjacent to each other in the structured environment, respectively, and their corresponding variances are respectively... and , x ni It is the i-th new obstacle feature point observed. x Axis coordinates y ni It is the i-th new obstacle feature point observed. y Axis coordinates; for the first or second column... A new obstacle feature point x Linear constraints are applied to update the axis coordinates and variance:

[0078]

[0079] Will x The variance of the axis coordinates and the direction of those coordinates Write state vector Covariance Matrix The corresponding position in the middle.

[0080] In some embodiments, the point cloud acquisition unit is a lidar, a binocular camera, or a sonar.

[0081] In some embodiments, an apparatus for concurrent localization 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 in the memory to implement the above-described method for concurrent localization and mapping in a structured environment.

[0082] In some embodiments, a computer storage medium stores a computer program that is executed by a computer to implement the above-described method for concurrent localization and mapping in a structured environment.

[0083] like Figure 4 , 5 As shown, a method for concurrent localization and mapping of an underwater robot operating below a dock is described. This method generates input data from the robot's motion in a physical environment. The robot is equipped with an IMU (Inertial Measurement Unit) and a sonar unit. The IMU acquires the robot's angular velocity and acceleration in the physical environment. The sonar unit acquires the relative position of obstacles to the sonar, expressed in polar coordinates of the obstacles in the robot's coordinate system. The sonar unit outputs one frame of sonar data per scan cycle. The outputs of the IMU and sonar units serve as input to a SLAM (Simultaneous Localization and Mapping) algorithm. The SLAM algorithm outputs a map in the world coordinate system, containing the first... i ( i The coordinates of (1, 2, 3...) obstacles in the world coordinate system The robot's coordinates in the world coordinate system and the robot's yaw angle The robot's two-dimensional running speed u、v ] and yaw angle angular velocity r , u express x The operating speed of the shaft, v express y The operating speed of the shaft;

[0084] When the robot dives underwater, a buoy connected to the robot floats on the surface. When the robot moves, it moves in conjunction with the buoy, and the buoy's position and observation speed are obtained by the lidar on the shore.

[0085] When the SLAM algorithm performs motion estimation, if the angular velocity is lower than a preset angular velocity threshold, the velocity observed by the buoy is decomposed into x-axis velocity and y-axis velocity, and then updated to the robot's two-dimensional running velocity; if the angular velocity is higher than the preset angular velocity threshold, the acceleration obtained by the IMU inertial unit is converted into velocity, and decomposed into x-axis velocity and y-axis velocity, and then updated to the robot's two-dimensional running velocity.

[0086] The output data of the sonar unit is the coordinates of obstacles in the robot coordinate system. The coordinates of each obstacle in a frame of sonar data are polar coordinates relative to the position of the sonar at the start of the scan. The polar coordinates of the obstacles in the robot coordinate system in the map are correlated with the polar coordinates of the current frame of sonar data using Mahalanobis distance. The polar coordinates of the new obstacles are transformed to the world coordinate system. The coordinates of the new obstacles and the structured features are linearly constrained according to the X-axis coordinate to determine the correspondence between the new obstacles and the structured features. The new obstacles are then updated to the region of the corresponding structured features.

[0087] The various embodiments of the present invention can be implemented independently or combined with each other. Alternatively, the technical solutions formed by combining the features in the embodiments are all within the scope of the present invention.

[0088] Example 1

[0089] S1: Data acquired by the IMU (Inertial Measurement Unit) and LiDAR is used as input to the SLAM algorithm for coarse robot pose estimation. A buoy is used to connect to the underwater robot, and LiDAR is used on the approach bridge to detect the buoy's position. The LiDAR obtains a point cloud image, which is then downsampled to reduce noise and complexity. Density-based noise spatial clustering (DBSCAN) is then applied for clustering and segmentation to extract the buoy's position. Alternatively, other existing algorithms can be used to segment the point cloud and extract the buoy's position.

[0090] Due to the influence of water flow speed, there is a certain deviation between the buoy position and the underwater robot position. However, when the robot moves in a fixed direction, the deviation value is approximately constant for a short period of time. Therefore, the difference in buoy position between two consecutive frames can be extracted as a rough estimate of the robot's running speed.

[0091] The method for determining the pose prediction of each frame by the lidar is as follows: First, it is determined whether the angular velocity in the IMU inertial unit is greater than the set threshold. If it is not greater than the threshold and the buoy is observable, it is assumed that the robot's motion direction remains unchanged, and the motion velocity estimated by the lidar and the IMU measurement value are used together to predict the robot's pose at the current moment. If the angular velocity measurement value is greater than the set threshold, it is assumed that the robot's motion direction has changed significantly, and the robot's pose prediction is performed using only the IMU.

[0092] S2: Sensing sensors use mechanical scanning sonar to acquire data. Because mechanical scanning sonar has a very slow scanning speed, the complete sonar scan image obtained by the AUV during movement will be distorted, leading to incorrect results when extracting environmental features from such sonar images. Therefore, while using the sonar beam to capture images, the robot's coarse pose estimated in S1 is used to compensate for the image distortion caused by motion, reducing the impact of distortion. Each scanning cycle outputs one frame of sonar data; the coordinates of each obstacle in one frame of sonar data are polar coordinates relative to the sonar's position at the start of the scan, with the sonar's position at the start of the scan serving as the origin of the current coordinate system.

[0093] Preprocessing sonar data to extract obstacle point features can be performed using the DBSCAN algorithm. Since underwater environments are often open and obstacles are mostly pile foundations and distant walls, clustering algorithms can be used to identify obstacles within a fixed range in the robot's coordinate system. The center point of these obstacles is taken as the feature, and the obstacle coordinates in the current frame of sonar data are correlated with the polar coordinates of obstacles on the map using Mahalanobis distance.

[0094] S3: If the association result is less than the set threshold, it is considered that a pile foundation point in the map has been observed, and the robot pose and the pile foundation position in the map are corrected and updated using linear constraint extended Kalman filter; if the association result is greater than the threshold, it is considered that a new pile foundation feature has appeared, and it is stored in the map for global mapping.

[0095] Example 2

[0096] The difference between this embodiment and embodiment 1 is that S4 is added: for the corrected and updated robot pose fusion water pressure sensor elevation information, the elevation information is used as the robot's z-axis coordinate in the world coordinate system to achieve three-dimensional positioning of the robot.

[0097] The above specific implementation examples are used to explain and illustrate the present invention, and are only preferred embodiments of the present invention, not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc., made to the present invention within the spirit and scope of the claims shall fall within the protection scope of the present invention.

Claims

1. A method for concurrent underwater localization and mapping, where input data is generated by the robot's motion in a physical environment; the robot is equipped with an IMU (Inertial Measurement Unit) and a sonar unit. The IMU acquires the robot's angular velocity and acceleration in the physical environment; the sonar unit acquires the relative position of obstacles to the sonar, expressed in polar coordinates of the obstacles in the robot's coordinate system; the sonar unit outputs one frame of sonar data per scan cycle; the outputs of the IMU and sonar units are the inputs to the SLAM (Simplified Chinese Algorithm for Multi-Object Arrays), which outputs a map in the world coordinate system, containing the coordinates of the i-th (i=1, 2, 3...) obstacle in the world coordinate system. The robot's coordinates in the world coordinate system and the robot's yaw angle The robot's two-dimensional running speed [u, v] and yaw angle angular velocity r, where u represents the running speed along the x-axis and v represents the running speed along the y-axis; Its features are: When the robot dives underwater, a buoy connected to the robot floats on the surface. When the robot moves, it moves in conjunction with the buoy, and the buoy's position and observation speed are obtained by the lidar on the shore. When the SLAM algorithm performs motion estimation, if the angular velocity is lower than a preset angular velocity threshold, the velocity observed by the buoy is decomposed into x-axis velocity and y-axis velocity and updated to the robot's two-dimensional running velocity; if the angular velocity is higher than the preset angular velocity threshold, the acceleration obtained by the IMU inertial unit is converted into velocity, and the x-axis velocity and y-axis velocity are decomposed and updated to the robot's two-dimensional running velocity. The output data of the sonar unit is the coordinates of the obstacles in the robot coordinate system. The coordinates of each obstacle in a frame of sonar data are polar coordinates relative to the position of the sonar at the start of the scan. The polar coordinates of the obstacles in the robot coordinate system in the map are correlated with the polar coordinates of the current frame of sonar data using Mahalanobis distance. The polar coordinates of the new obstacles are transformed to the world coordinate system and added to the map. The feature is that before outputting each frame of sonar data, the following operations are performed: the relative position of each obstacle and the sonar is transformed to polar coordinates in the robot coordinate system, the coordinates of the obstacles are clustered into multiple independent regions, the geometric center point of each independent region is taken as a point feature, and the polar coordinates of the point feature are used as the obstacle coordinates for output. The feature is that when converting obstacle coordinates in the robot coordinate system to obstacle coordinates in the world coordinate system, the following operations are performed: obtain the polar coordinates of each obstacle in the robot coordinate system, correlate the obstacle coordinates of the current frame sonar data with the polar coordinates of the obstacles in the map using Mahalanobis distance, if the correlation result is greater than a preset value, it is considered a new obstacle, the new obstacle is converted to the world coordinate system and added to the map; otherwise, the robot pose and map are corrected and updated using linearly constrained extended Kalman filtering.

Images