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

Abstract

A method for simultaneous localization and mapping underwater. A robot is equipped with an IMU inertial unit and a sonar unit. When the robot submerges underwater, a buoy connected to the robot floats on the water surface, and the buoy moves in coordination with the movement of the robot. The position and observed velocity of the buoy are obtained by a shore-based lidar. During motion estimation in a SLAM algorithm, when an angular velocity is below a preset angular velocity threshold, the observed velocity of the buoy is decomposed into x-axis velocity and y-axis velocity, and updated as the two-dimensional operating velocity of the robot. The polar coordinates of an obstacle in a map under the coordinate system of the robot are associated with the polar coordinates of sonar data of a current frame by using the Mahalanobis distance. The polar coordinates of a new obstacle are converted to the world coordinate system and added to the map. The present invention has the advantages of not relying on underwater visibility, not requiring pre-installed devices, and possessing good generalization and stability.

Description

Method, device, apparatus and storage medium for concurrent positioning and mapping Technical Field

[0001] The present invention relates to a method, device, equipment and storage medium for concurrent positioning and mapping, and in particular to a method, device, equipment and storage medium for concurrent positioning and mapping suitable for underwater environments and / or structured environments. Technical Background

[0002] The SLAM algorithm is an algorithm that enables robot positioning, mapping, and path planning. SLAM stands for Simultaneous Localization and Mapping. SLAM encompasses three processes: perception, positioning, and mapping. Perception: The robot uses sensors to acquire information about its surroundings. Positioning: The robot infers its position and posture based on current and historical information obtained by sensors. Mapping: The robot creates a representation of its surroundings based on its position and information obtained by sensors.

[0003] Summary of the Invention

[0004] In recent years, demand for operations behind high-pile berths at large ports has been increasing. Due to the increasing severity of siltation, the height of the mud surface has increased year by year, causing damage and fracture to the pile foundations. A survey of siltation behind over 100 high-pile berths along the Zhejiang coast revealed that approximately 90% of the berths experience severe siltation below and behind the berths, posing a safety hazard. To meet the increasing berthing depths for large ships, the front of the berths is regularly dredged. At the same time, flaw detection inspections for damage to underwater pile foundations are becoming increasingly frequent and urgent. These operations traditionally rely on manual labor, which is inefficient and highly dangerous. In recent years, this has been gradually replaced by automated underwater robotics.

[0005] One of the main challenges facing underwater robots is determining their underwater position during operation. Currently, the following are the most mature underwater positioning methods: The first is acoustic baseline positioning, which primarily includes long, short, and ultra-short baselines. This method typically involves installing a transponder array underwater and a transmitter array on the vehicle. The three-dimensional position of the measured vehicle is determined using acoustic signals. This requires the installation of transponder arrays in advance, and the number of these arrays increases rapidly with the scope of use, making generalization difficult. The second method relies on dead reckoning using odometry devices such as Doppler velocimeters (DVLs) to calculate positioning information. This method is relatively simple, but DVLs are relatively expensive. The third method relies on inertial measurement units (IMUs), which calculate positioning information by measuring the object's three-axis attitude angles and accelerations. This method relies solely on the IMU, requiring no external information. However, the disadvantage is that IMU measurements can accumulate errors, resulting in significant drift over time and large errors. The fourth method uses visual positioning, such as cameras, for optical sensors. However, the water at ports is often very turbid, and optical sensors have very limited visibility, making visual positioning difficult to maintain. Therefore, a method that can provide autonomous underwater positioning for robots operating behind and below docks is urgently needed.

[0006] The 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 and does not require pre-placement of equipment, and has good generalization and stability.

[0007] A method for concurrent underwater positioning and mapping, wherein input data is generated by the movement of a robot in a physical environment; the robot is provided 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, the map containing the coordinates [x i ,y i ], the robot's coordinates in the world coordinate system [xy] and the robot's yaw angle θ, the robot's two-dimensional running speed [u, v] and the yaw angular velocity r, u represents the running speed of the x-axis, and v represents the running speed of the y-axis;

[0008] The robot is characterized by a buoy connected to the robot floating on the water surface when it dives underwater. The robot moves in conjunction with the buoy, and the shore-based laser radar obtains the buoy's position and observed speed.

[0009] When the SLAM algorithm is performing motion estimation, if the angular velocity is lower than the preset angular velocity threshold, the buoy observation velocity is decomposed into the x-axis velocity and the y-axis velocity, and updated to the two-dimensional running speed of the robot; if the angular velocity is lower than the preset angular velocity threshold, 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;

[0010] 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.

[0011] In some embodiments, 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.

[0012] In some embodiments, when the obstacle coordinates in the robot coordinate system are converted to the obstacle coordinates of the map in the world coordinate system, the following operations are performed: the obstacle polar coordinates of each obstacle in the map in the robot coordinate system are obtained, and the obstacle coordinates of the current frame sonar data are correlated with the obstacle polar coordinates of the map using Mahalanobis distance. If the correlation result is greater than a preset value, it is considered that a new obstacle is added, and the new obstacle is converted to the world coordinate system and added to the map; otherwise, the robot posture and the pile position in the map are corrected and updated using a linear constrained extended Kalman filter.

[0013] In some embodiments, a water pressure sensor is provided on the robot, and elevation information is obtained from the water pressure sensor. The elevation information is used as the Z-axis value and added to the map; the coordinates of the robot in the world coordinate system are expressed as [x, y, z].

[0014] In some embodiments, the SLAM algorithm uses the state vector X = [x, y, θ, u, v, r, x1, y1, ..., x i ,y i ] T To express the robot state and obstacle position; the state X of the system at time t-1 is known t-1 With the covariance matrix P t-1 In the case of , the system motion state prediction model at time t is expressed as:

[0015] Among them, μt =[a x , a y ,r], r is the angular velocity of the yaw angle measured by IMU, a x The acceleration measured by the IMU is converted to the x-axis acceleration value in the world coordinate system, a y is the acceleration value of the y-axis; w k is the Gaussian white noise present when the robot moves; is the covariance matrix at time t; G t is the Jacobian matrix of the motion prediction model for the system state vector X; w t is the Gaussian white noise when the robot moves, R t is the noise w t The covariance matrix of

[0016] A device for concurrent underwater positioning and mapping, comprising the following modules:

[0017] a robot with a buoy;

[0018] IMU inertial unit to obtain the angular velocity and acceleration of the robot in the physical environment;

[0019] A sonar unit that obtains the relative position of the obstacle and the sonar, where the relative position is expressed in the polar coordinates of the obstacle in the robot coordinate system. The sonar unit outputs one frame of sonar data in each scanning cycle.

[0020] The computing module runs the SLAM algorithm; 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 map contains the coordinates of the i-th (i=1, 2, 3...) obstacle in the world coordinate system [x i ,y i ], the robot's coordinates in the world coordinate system [xy] and the robot's yaw angle θ, the robot's two-dimensional running speed [u, v] and the yaw angular velocity r, u represents the running speed of the x-axis, and v represents the running speed of the y-axis;

[0021] When the robot dives underwater, a buoy connected to the robot floats on the water surface. When the robot moves, it moves with the buoy, and the shore-based lidar obtains the buoy's position and observed speed.

[0022] When the SLAM algorithm is performing motion estimation, if the angular velocity is lower than the preset angular velocity threshold, 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; if the angular velocity is lower than the preset angular velocity threshold, 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;

[0023] 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.

[0024] A method and device for underwater concurrent positioning and mapping, the device 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 underwater concurrent positioning and mapping.

[0025] A computer storage medium having a computer program stored thereon, wherein the computer program is executed by a computer to implement the above-mentioned method for underwater concurrent positioning and mapping.

[0026] Furthermore, taking the dock environment as an example, the dock pile foundation is a key component of the dock structure, supporting the superstructure and transferring loads deep into the foundation. Considering load transfer efficiency, bending resistance, and shear resistance, the pile foundations are often arranged in a linear structured form. That is, in a certain direction, a row of pile foundations is arranged in a straight line, as shown in Figure 2. This good prior structural constraint is often wasted in previous technologies.

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

[0028] 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 presupposes structured features of the environment;

[0029] 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. 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.

[0030] In some embodiments, the linear constraint is specifically implemented as follows: for the new obstacle feature point world coordinate Z obtained after the update n =(x1, y1, ..., x i ,y i ) n , the corresponding variance vector in the covariance matrix P is (δ1,,...,δ i ) n To simplify the calculation, we assume that the y-axis of the world coordinate system is consistent with the arrangement direction of the pile foundation, and set the distance threshold d Th To determine the location of the new obstacle feature point:

[0031] where m x1 and m x2 They are the estimated values ​​of the horizontal coordinates of the first column of features and the estimated values ​​of the horizontal coordinates of the second column of features adjacent to the structured environment, and their corresponding variances are δ m1 and δ m2 , x ni is the x-axis coordinate of the observed i-th new obstacle feature point, y ni is the y-axis coordinate of the observed i-th new obstacle feature point; the x-axis coordinate and variance of the i-th new obstacle feature point judged to be in the first or second column are linearly constrained and updated:

[0032] The x-axis coordinate and the variance δ in the coordinate direction m1 Write the corresponding positions in the state vector X and the covariance matrix P.

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

[0034] A device for concurrent positioning and mapping in a structured environment, the device comprising 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.

[0035] A computer storage medium stores a computer program, which is executed by a computer to implement the method for concurrent positioning and mapping in a structured environment.

[0036] A third aspect of the present invention is to provide a method, apparatus, device and storage medium capable of concurrently positioning and mapping a submerged robot beneath a high-piled pier.

[0037] A method for concurrent positioning and mapping of an underwater robot for operations beneath a dock, wherein input data is generated by the movement of the robot in a physical environment; the robot is provided with an IMU inertial unit and a sonar unit, wherein 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 an 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 a world coordinate system, wherein the map contains the coordinates [x] of the i-th (i=1, 2, 3, ...) obstacle in the world coordinate system. i ,y i ], the robot's coordinates in the world coordinate system [xy] and the robot's yaw angle θ, the robot's two-dimensional running speed [u, v] and the yaw angular velocity r, u represents the running speed of the x-axis, and v represents the running speed of the y-axis;

[0038] The robot is characterized by a buoy connected to the robot floating on the water surface when it dives underwater. The robot moves in conjunction with the buoy, and the shore-based laser radar obtains the buoy's position and observed speed.

[0039] When the SLAM algorithm is performing motion estimation, if the angular velocity is lower than the preset angular velocity threshold, 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; if the angular velocity is lower than the preset angular velocity threshold, 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;

[0040] 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.

[0041] The advantages of this invention are: 1. It provides a method for onshore laser radar to observe water surface buoys to provide robot speed information, which is much lower in cost than direct-visit laser radar (DVL). 2. It uses the concept of multi-sensor fusion, with multiple sensors complementing each other to improve the stability of the SLAM method. 3. It incorporates the linear arrangement constraints of the dock pile foundation into the traditional extended Kalman filter (SLAM), and proposes a linear constrained extended Kalman filter method to improve SLAM estimation accuracy and reduce uncertainty. 4. This method does not rely on water quality or turbidity, and does not require pre-installed equipment, and has good generalization properties. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] FIG1 is a flow chart of a method for concurrent underwater positioning and mapping.

[0043] Figure 2 is a schematic diagram of the wharf pile foundation arrangement.

[0044] FIG3 is a flow chart of a method for concurrent localization and mapping in a structured environment.

[0045] FIG4 is a block diagram of a concurrent localization and mapping method for an underwater robot operating beneath a dock.

[0046] FIG5 is a flow chart of a concurrent localization and mapping method for an underwater robot operating under a dock. DETAILED DESCRIPTION

[0047] As shown in FIG1 , a method for concurrent underwater positioning and mapping generates input data from the movement of a robot in a physical environment. The robot is provided 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, which outputs a map in the world coordinate system. The map contains the coordinates [x ] of the i-th (i=1, 2, 3, ...) obstacle in the world coordinate system. i ,y i ], the robot's coordinates in the world coordinate system [xy] and the robot's yaw angle θ, the robot's two-dimensional running speed [u, v] and the yaw angular velocity r, u represents the running speed of the x-axis, and v represents the running speed of the y-axis;

[0048] When the robot dives underwater, a buoy connected to the robot floats on the water surface. When the robot moves, it moves with the buoy, and the shore-based lidar obtains the buoy's position and observed speed.

[0049] When the SLAM algorithm is performing motion estimation, if the angular velocity is lower than the preset angular velocity threshold, the buoy observation velocity is decomposed into the x-axis velocity and the y-axis velocity, and updated to the two-dimensional running speed of the robot; if the angular velocity is lower than the preset angular velocity threshold, 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;

[0050] 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.

[0051] In some embodiments, 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.

[0052] In some embodiments, when the obstacle coordinates in the robot coordinate system are converted to the obstacle coordinates of the map in the world coordinate system, the following operations are performed: the obstacle polar coordinates of each obstacle in the map in the robot coordinate system are obtained, and the obstacle coordinates of the current frame sonar data are correlated with the obstacle polar coordinates of the map using Mahalanobis distance. If the correlation result is greater than a preset value, it is considered that a new obstacle is added, and the new obstacle is converted to the world coordinate system and added to the map; otherwise, the robot posture and the pile position in the map are corrected and updated using a linear constrained extended Kalman filter.

[0053] In some embodiments, a water pressure sensor is provided on the robot, and elevation information is obtained from the water pressure sensor. The elevation information is used as the Z-axis value and added to the map; the coordinates of the robot in the world coordinate system are expressed as [x, y, z].

[0054] In some embodiments, the SLAM algorithm uses the state vector X = [x, y, θ, u, v, r, x1, y1, ..., x i ,y i ] T To express the robot state and obstacle position; the state X of the system at time t-1 is known t-1 With the covariance matrix P t-1 In the case of , the system motion state prediction model at time t is expressed as:

[0055] Among them, μt =[a x , a y ,r], r is the angular velocity of the yaw angle measured by IMU, a x The acceleration measured by the IMU is converted to the x-axis acceleration value in the world coordinate system, a y is the acceleration value of the y-axis; w k is the Gaussian white noise present when the robot moves; is the covariance matrix at time t; G t is the Jacobian matrix of the motion prediction model for the system state vector X; w t is the Gaussian white noise when the robot moves, R t is the noise w t The covariance matrix of

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

[0057] a robot with a buoy;

[0058] IMU inertial unit to obtain the angular velocity and acceleration of the robot in the physical environment;

[0059] A sonar unit that obtains the relative position of the obstacle and the sonar, where the relative position is expressed in the polar coordinates of the obstacle in the robot coordinate system. The sonar unit outputs one frame of sonar data in each scanning cycle.

[0060] The computing module runs the SLAM algorithm; 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 map contains the coordinates of the i-th (i=1, 2, 3...) obstacle in the world coordinate system [x i ,y i ], the robot's coordinates in the world coordinate system [xy] and the robot's yaw angle θ, the robot's two-dimensional running speed [u, v] and the yaw angular velocity r, u represents the running speed of the x-axis, and v represents the running speed of the y-axis;

[0061] When the robot dives underwater, a buoy connected to the robot floats on the water surface. When the robot moves, it moves with the buoy, and the shore-based lidar obtains the buoy's position and observed speed.

[0062] When the SLAM algorithm is performing motion estimation, if the angular velocity is lower than the preset angular velocity threshold, 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; if the angular velocity is lower than the preset angular velocity threshold, 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;

[0063] 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.

[0064] In some embodiments, a method and apparatus for underwater concurrent positioning and mapping comprises 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 underwater concurrent positioning and mapping.

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

[0066] In some embodiments, as shown in Figures 2 and 3, a method for concurrent localization and mapping in a structured environment is provided, 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;

[0067] 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. 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.

[0068] In some embodiments, the linear constraint is specifically implemented as follows: for the new obstacle feature point world coordinate Z obtained after the update n =(x1, y1, ..., x i ,y i ) n , the corresponding variance vector in the covariance matrix P is (δ1,,...,δ i ) n To simplify the calculation, we assume that the y-axis of the world coordinate system is consistent with the arrangement direction of the pile foundation, and set the distance threshold d Th To determine the location of the new obstacle feature point:

[0069] where m x1 and m x2 They are the estimated values ​​of the horizontal coordinates of the first column of features and the estimated values ​​of the horizontal coordinates of the second column of features adjacent to the structured environment, and their corresponding variances are δ m1 and δ m2 , x ni is the x-axis coordinate of the observed i-th new obstacle feature point, y ni is the y-axis coordinate of the observed i-th new obstacle feature point; the x-axis coordinate and variance of the i-th new obstacle feature point judged to be in the first or second column are linearly constrained and updated:

[0070] The x-axis coordinate and the variance δ in the coordinate direction m1 Write the corresponding positions in the state vector X and the covariance matrix P.

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

[0072] In some embodiments, 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.

[0073] In some embodiments, 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.

[0074] As shown in Figures 4 and 5, a method for concurrent positioning and mapping of an underwater robot for operations under a dock is provided. The method generates input data from the movement of the robot in a physical environment. The robot is provided 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. The SLAM algorithm outputs a map in the world coordinate system. The map contains the coordinates [x i ,y i ], the robot's coordinates in the world coordinate system [xy] and the robot's yaw angle θ, the robot's two-dimensional running speed [u, v] and the yaw angular velocity r, u represents the running speed of the x-axis, and v represents the running speed of the y-axis;

[0075] When the robot dives underwater, a buoy connected to the robot floats on the water surface. When the robot moves, it moves with the buoy, and the shore-based lidar obtains the buoy's position and observed speed.

[0076] When the SLAM algorithm is performing motion estimation, if the angular velocity is lower than the preset angular velocity threshold, 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; if the angular velocity is lower than the preset angular velocity threshold, 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;

[0077] 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.

[0078] The various embodiments of the present invention may be implemented independently or in combination with each other, or the features in the embodiments may be combined with each other to form a technical solution, which falls within the scope of the present invention.

[0079] Example 1

[0080] 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.

[0081] Due to the influence of water flow velocity, 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 in a short period of time. Therefore, the difference in the buoy position between two consecutive frames can be extracted as a rough estimate of the robot's running speed.

[0082] 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, and 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 measurement value is greater than the set threshold, it is considered that the robot's movement direction has changed significantly, and only the IMU is used to predict the robot's posture.

[0083] S2: 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.

[0084] 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.

[0085] S3: If the correlation result is less than the set threshold, it is considered that the pile foundation point in the map has been observed, and the linear constrained extended Kalman filter is used to correct and update the robot posture and the pile foundation position in the map; if the correlation 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.

[0086] Example 2

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

[0088] The above specific implementation cases are used to illustrate the present invention and are only preferred embodiments of the present invention, rather than limiting the present invention. Any modifications, equivalent substitutions, improvements, etc. made to the present invention within the spirit of the present invention and the scope of protection of the claims shall fall within the scope of protection of the present invention.

Claims

1. A method for concurrent underwater positioning and mapping, wherein input data is generated by 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; 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, the map containing the coordinates of the i-th (i=1, 2, 3, ...) obstacle in the world coordinate system [x i ,y i ], the robot's coordinates in the world coordinate system [xy] and the robot's yaw angle θ, the robot's two-dimensional running speed [u, v] and the yaw angular velocity r, u represents the running speed of the x-axis, and v represents the running speed of the y-axis; Its characteristics are: When the robot dives underwater, a buoy connected to the robot floats on the water surface. When the robot moves, it moves with the buoy, and the shore-based lidar obtains the buoy's position and observed speed. When the SLAM algorithm is performing motion estimation, if the angular velocity is lower than the preset angular velocity threshold, the buoy observation velocity is decomposed into the x-axis velocity and the y-axis velocity, and updated to the two-dimensional running speed of the robot; if the angular velocity is lower than the preset angular velocity threshold, 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 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.

2. The method for concurrent underwater positioning and mapping according to claim 1, wherein: Before outputting each frame of sonar data, the following operations are performed: the relative position of each obstacle and the sonar is converted 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 used as a point feature, and the polar coordinates of the point feature are output as the obstacle coordinates.

3. The method for concurrent underwater positioning and mapping according to claim 1, wherein: 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 map in the robot coordinate system, perform Mahalanobis distance correlation between the obstacle coordinates in the current frame sonar data and the polar coordinates of the obstacle in the map. If the correlation result is greater than the preset value, it is considered a new obstacle, converted to the world coordinate system, and added to the map; Otherwise, the linear constrained extended Kalman filter is used to correct and update the robot's posture and the position of the pile foundation in the map.

4. The method for concurrent underwater positioning and mapping according to claim 1, wherein: A water pressure sensor is installed on the robot, and elevation information is obtained from the water pressure sensor. The elevation information is used as the Z-axis value and added to the map; the coordinates of the robot in the world coordinate system are expressed as [x, y, z].

5. The method for concurrent underwater positioning and mapping according to claim 1, wherein: In the SLAM algorithm, the state vector X = [x, y, θ, u, v, r, x1, y1, ..., x i ,y i ] T To express the robot state and obstacle position; the state X of the system at time t-1 is known t-1 With the covariance matrix P t-1 In the case of , the system motion state prediction model at time t is expressed as: Among them, μ t =[a x , a y ,r], r is the angular velocity of the yaw angle measured by IMU, a x The acceleration measured by the IMU is converted to the x-axis acceleration value in the world coordinate system, a y is the acceleration value of the y-axis; w k is the Gaussian white noise present when the robot moves; is the covariance matrix at time t; G t is the Jacobian matrix of the motion prediction model for the system state vector X; w t is the Gaussian white noise when the robot moves, R t is the noise w t The covariance matrix of 6. An apparatus for concurrent underwater positioning and mapping, comprising the following modules: a robot with a buoy; IMU inertial unit to obtain the angular velocity and acceleration of the robot in the physical environment; A sonar unit that obtains the relative position of the obstacle and the sonar, where the relative position is expressed in the polar coordinates of the obstacle in the robot coordinate system. The sonar unit outputs one frame of sonar data in each scanning cycle. The computing module runs the SLAM algorithm; 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 map contains the coordinates of the i-th (i=1, 2, 3...) obstacle in the world coordinate system [x i ,y i ], the robot's coordinates in the world coordinate system [xy] and the robot's yaw angle θ, the robot's two-dimensional running speed [u, v] and the yaw angular velocity r, u represents the running speed of the x-axis, and v represents the running speed of the y-axis; When the robot dives underwater, a buoy connected to the robot floats on the water surface. When the robot moves, it moves with the buoy, and the shore-based lidar obtains the buoy's position and observed speed. When the SLAM algorithm is performing motion estimation, if the angular velocity is lower than the preset angular velocity threshold, 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; if the angular velocity is lower than the preset angular velocity threshold, 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 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.

7. A method and device for underwater concurrent positioning and mapping, the device comprising 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 underwater concurrent positioning and mapping.

8. A computer storage medium having a computer program stored thereon, wherein the computer program is executed by a computer to implement the above-mentioned method for underwater concurrent positioning and mapping.

9. A method for concurrent localization and mapping in a structured environment. This method involves a robot moving in a physical environment and acquiring a point cloud of the surrounding environment. The robot's motion data and the point cloud data serve as input to a SLAM algorithm. The SLAM algorithm performs motion estimation and concurrent localization and mapping. This method presupposes the structured characteristics of the environment. The point cloud acquisition unit outputs a frame of point cloud data in one scanning cycle. The coordinates of obstacles 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 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 point cloud data in the current frame using the Mahalanobis distance. If the obstacle in the current world coordinate system is new, 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.

10. A method for concurrent localization and mapping in a structured environment, characterized in that The specific implementation of the linear constraint is: for the new obstacle feature point world coordinate Z after the update n =(x1, y1, ..., x i ,y i ) n , the corresponding variance vector in the covariance matrix P is (δ1,,...,δ i ) n To simplify the calculation, we assume that the y-axis of the world coordinate system is consistent with the arrangement direction of the pile foundation, and set the distance threshold d Th To determine the location of the new obstacle feature point: where m x1 and m x2 They are the estimated values ​​of the horizontal coordinates of the first column of features and the estimated values ​​of the horizontal coordinates of the second column of features adjacent to the structured environment, and their corresponding variances are δ m1 and δ m2 , x ni is the x-axis coordinate of the observed i-th new obstacle feature point, y ni is the y-axis coordinate of the observed i-th new obstacle feature point; the x-axis coordinate and variance of the i-th new obstacle feature point judged to be in the first or second column are linearly constrained and updated: The x-axis coordinate and the variance δ in the coordinate direction m1 Write the corresponding positions in the state vector X and the covariance matrix P.

Images