SLAM method of desilting robot in structured sewage pool

Abstract

The invention discloses an SLAM method of a desilting robot in a structured sewage pool, and belongs to the field of robot-assisted sewage treatment and environmental engineering. According to the method, based on a Hough transform feature extraction method, the linear features of the pool boundary are estimated and extracted by performing polar coordinate data conversion after preprocessing and target extraction of sonar data, and the method is applied to the visual pool environment in a feedback world coordinate system in the construction of a feature map. In the front-end registration process, descriptor vectors are established based on extracted linear features, and feature data matching is carried out by using an Euclidean distance matching method and setting threshold constraint filtering. In the back-end optimization process, secondary map updating is carried out based on environmental feature constraints to reduce mapping errors. According to the method, the purpose of completing sewage pool desilting work under the condition that a factory does not stop production or work, the robot is positioned in real time, meanwhile, the working efficiency is improved, the cost is reduced, the factory benefit loss is reduced, and compared with a traditional method, the method has higher safety and reliability.

Description

Technical Field

[0001] The present invention belongs to the field of robot-assisted sewage treatment and environmental engineering, and specifically relates to a simultaneous localization and mapping (SLAM) method for a dredging robot in a structured sewage pool, which realizes the environmental perception and positioning of the dredging robot in a non-visible sewage pool in real time. Background Art

[0002] Currently, manual desilting remains the primary method for desilting sewage ponds in my country. However, traditional manual desilting operations are susceptible to weather and other environmental factors, and are subject to issues such as large equipment, long construction times, and secondary pollution during transportation. Furthermore, traditional manual desilting is inefficient, and the treatment of sewage sludge produces a variety of toxic, hazardous, or flammable and explosive gases, such as carbon monoxide, carbon dioxide, hydrogen sulfide, sulfur dioxide, and methane. These gases, at certain concentrations, can pose significant risks to the human body and seriously threaten the safety of frontline workers. To address these challenges, robotic desilting technology has rapidly developed. Currently, desilting equipment designed for industrial wastewater ponds, such as chemical wastewater ponds, is already effective in desilting. However, most equipment requires real-time remote control during operation, and even requires personnel to monitor operations after the water is shut down and drained to prevent accidents. Industrial wastewater ponds are typically 3 to 5 meters deep. Manual or automated desilting often requires closing the sewage inlet and outlet valves and draining the upper layer to a low level. These preparatory steps undoubtedly reduce plant efficiency.

[0003] If the dredging robot is to be able to complete the dredging work without stopping production or work, it needs to have certain environmental perception and real-time positioning capabilities. Based on this, the present invention uses sonar sensor equipment and an inertial measurement unit (IMU) to enable the dredging robot to perceive the surrounding environment and achieve simultaneous positioning and mapping when working in an invisible sewage environment. Taking into account the problem of high observation noise of the sonar sensor caused by acoustic characteristics, the present invention performs data-level preprocessing and obstacle target extraction on the information collected by the sonar. Taking into account the highly structured characteristics of the sewage pool environment, the present invention uses the Hough transform method to extract straight line features as the pool boundary observation information. Taking into account the problem of motion distortion of sonar data caused by the slow scanning characteristics of the sonar sensor, the present invention adopts a motion distortion correction method based on IMU data, and a secondary correction method based on straight line features to eliminate error accumulation. Taking into account the uncertainty factors and excessive noise interference in the sewage environment, the present invention adopts a method based on environmental feature constraints to optimize the SLAM backend. Summary of the Invention

[0004] In response to the problems of low efficiency, high cost and need for production and work stoppage in traditional industrial sewage pool desilting methods, the present invention provides a SLAM method for underwater desilting robots in a structured environment to achieve sewage pool cleaning without stopping production and work. The present invention is based on the Hough transform feature extraction method. It estimates and extracts the straight line features of the pool boundary by pre-processing and target extraction of sonar data and then performing polar coordinate data conversion. It is applied to the construction of feature maps to feedback the visual pool environment in the world coordinate system. In the process of front-end alignment, a descriptor vector is established based on the extracted straight line features, and the Euclidean distance matching method is used and threshold constraint filtering is set to perform feature data matching. In the process of back-end optimization, a secondary map update is performed based on environmental feature constraints to reduce mapping errors.

[0005] In order to achieve the above object, the technical solution of the present invention is:

[0006] A SLAM method for a dredging robot in a structured sewage pool comprises the following steps:

[0007] Step 1: The dredging robot is equipped with a mechanical scanning sonar (MSIS) and an inertial measurement unit (IMU) as information collection sensors, and an Intel microprocessor as an information processing terminal. SLAM work is performed in the Robot Operating System (ROS) under the Linex system, and information exchange between the sensor and processor is achieved by publishing and subscribing to topics. While operating, the dredging robot collects MSIS data and IMU data with the same timestamp, subscribes to the topic published by the MSIS, obtains the scan data, and preprocesses the scan data to eliminate acoustic noise and extract the correct obstacle targets. Specifically:

[0008] Step 1.1: Extract single-frame (Ping) sonar information from the continuous scanning information. Single-frame sonar information is a string of binary data. The number of bits is determined by the scanning range, and the size of each bit is the echo intensity (Bin) corresponding to the unit step. Read the obstacle position information with a certain intensity to establish a rectangular coordinate system and a polar coordinate system with the MSIS as the coordinate origin. Extract the Bin intensity information from the single-frame data using the calculation formula:

[0009] (1)

[0010] Where: i is the number of pings, h is the position of the bin in the current ping, and N is the total number of bins; is the rectangular coordinate parameter corresponding to each Bin in the actual area, x h For this Bin's horizontal coordinate, y h The vertical coordinate of this Bin; is the polar coordinate parameter corresponding to each Bin in the actual area, Indicates the polar coordinate angle of the current ping.

[0011] By using formula (1), the coordinate parameters of each bin are calculated in the coordinate system centered on the sonar, which converts the original echo intensity and occlusion rate format of the sonar data into a data format similar to that of lidar.

[0012] Step 1.2: Due to the working characteristics of the MSIS itself, there is high-intensity noise interference near the device. To eliminate the impact of this high-intensity abnormal noise, the first few bits of data near the origin are assigned a value of 0. The number of abnormal data bits depends on the scanning range. The solution formula for the abnormal data bits is:

[0013] (2)

[0014] Where: Indicates the minimum detection distance, which is 0.75m: n is the total number of Bins in a single Ping data: L max The detection distance currently set; This is the number of abnormal data bits required to be solved.

[0015] Step 1.3: Set the threshold Bin_T to remove a large amount of small background noise. The method is to take the median value of the intensity of each Bin in each Ping data, first calculate the maximum and minimum values ​​of the Bin, and then calculate the threshold Bin_T using formula (3). Bins smaller than this threshold Bin_T are deleted. The formula is:

[0016] (3)

[0017] Wherein, Bin_T represents the threshold value; Bin_Min represents the minimum value of Bin; Bin_Max represents the maximum value of Bin.

[0018] Step 1.4: Use the target extraction algorithm based on the threshold combined with the nearest neighbor method to extract the correct obstacle target and eliminate the high-intensity noise caused by the acoustic multipath effect.

[0019] Step 1.5: Predict the state vector X and state covariance matrix P based on the IMU data collected at the same timestamp. The state vector X reflects the current position and posture information of the dredging robot, and the state covariance matrix P is used to describe the uncertainty of the state estimation. X is expressed as:

[0020] (4)

[0021] Since the dredging robot can be regarded as a movement on a two-dimensional plane when working, the state in the z direction is not considered. x is the horizontal coordinate of the dredging robot in the world coordinate system; y is the vertical coordinate of the dredging robot in the world coordinate system; θ is the posture angle; v x is the velocity in the x-axis direction; v y is the speed in the y-axis direction; m represents the map feature; n is the number of map features.

[0022] Step 2: Use IMU data to correct the motion distortion of the MSIS data processed in Step 1. Use the IMU-measured pose angle and displacement changes of the dredging robot to correct the rectangular coordinate and polar coordinate representations of the next frame of MSIS data. Specifically, the pose angle changes and displacement changes measured by the IMU are used to compensate for the rectangular coordinate and polar coordinate positions of the current scan data to eliminate the motion distortion caused by the slow scanning characteristics of the mechanical scanning sonar.

[0023] Step 3: Perform Hough transform-based line feature extraction on the accumulated frames of MSIS data processed in step 2. At least three frames of MSIS scan data must be obtained to successfully extract line features. Based on the extracted line features, secondary correction is performed on the next frame of MSIS data to eliminate accumulated errors.

[0024] Step 4: Create a description vector for the line features extracted in step 3, save it, and complete simultaneous positioning and mapping to obtain the state estimation results and map update results, specifically:

[0025] Step 4.1: Create a descriptor vector D for the line features processed in step 3 i And save it as the historical frame descriptor vector, repeat step 3 to extract the straight line features and establish the descriptor vector D j As the current frame descriptor vector and the historical frame descriptor vector D i Perform Euclidean distance feature matching and set a threshold matrix to eliminate incorrectly extracted features; specifically:

[0026] Step 4.1.1: Since the main environmental feature of the structured sewage pool is the straight line feature of the pool boundary, its feature position remains unchanged in the world coordinate system. Simplify the straight line feature obtained in step 3 into a straight line in the world coordinate system. Each straight line feature contains its corresponding two-dimensional spatial coordinate information in the world coordinate system. Establish the historical frame descriptor vector D i for:

[0027] (5)

[0028] Where: is the direction angle of the line; Indicates the length of a straight line; Indicates the vertical distance between the straight line and the origin; ( , ) is the coordinate of the center point of the historical frame line feature in the world coordinate system.

[0029] Step 4.1.2: Repeat step 4.1.1 to create the current frame descriptor vector D j The historical frame descriptor vector D obtained in step 4.1.1 i Perform Euclidean distance matching. The matching method is to calculate the shortest straight-line distance between two vectors:

[0030] (6)

[0031] Where: d is the Euclidean distance result, ( , ) is the center point coordinate of the current frame straight line feature in the world coordinate system.

[0032] Step 4.1.3: Set the threshold matrix T, including retaining the matching pairs D with Euclidean distance d less than 0.2 i and D j And update the display of the current frame D j , and set the angle difference threshold to 5° and the length ratio threshold to 0.8. If all of the above parameters are less than the set thresholds, the match is considered established.

[0033] (7)

[0034] Where: Descriptor vector D for the current frame j and the historical frame descriptor vector D i The angle difference; Descriptor vector D for the current frame j and the historical frame descriptor vector D i d represents the Euclidean distance calculated in step 4.1.2; T represents the threshold matrix.

[0035] Step 4.2: Use the extended Kalman filter (EKF) method to predict and update the state vector X and the state covariance equation P;

[0036] Step 4.3: Based on the historical frame descriptor vector D established in step 4.1 i , the new state vector X obtained in step 4.2 is updated i and the state covariance equation P i Perform a secondary update to obtain the state estimation results and map updates constrained by environmental characteristics;

[0037] Step 5: Save the state estimation results and map update results obtained in step 4, publish the map topic, and implement visual synchronous positioning and mapping in Rviz.

[0038] The effects and benefits of the present invention are:

[0039] The present invention uses an MSIS and IMU equipped on a dredging robot to obtain structured sewage pool environmental information and robot posture information, and combines the data of the two sensors to realize the dredging robot's environmental perception, synchronous positioning and mapping in the structured industrial sewage pool, so as to achieve the purpose of completing the sewage pool dredging work without stopping the factory. It also enables the staff to clearly obtain the positioning information of the robot in real time when it is working, while improving work efficiency, reducing costs and reducing factory benefit losses. Compared with traditional methods, it has higher safety and reliability. BRIEF DESCRIPTION OF THE DRAWINGS

[0040] Figure 1 Flowchart of the SLAM method for structured sewage pool provided by the present invention;

[0041] Figure 2 is a flow chart of a feature-based cumulative error correction method;

[0042] Figure 3 Schematic diagram of the boundary characteristics of the structured sewage pool;

[0043] Figure 4 The result of building the feature map. DETAILED DESCRIPTION

[0044] The specific implementation of the present invention is further described below in conjunction with the accompanying drawings and technical solutions.

[0045] Figure 1 Flowchart of the structured sewage pool SLAM method provided in an embodiment of the present invention. Figure 1 The structured sewage pool SLAM method proposed in this paper is based on the EKF-SLAM method. By extracting pool boundary features and applying them to a secondary updated environmental map to constrain them, it adapts to the uncertainty noise in sewage and reduces mapping errors. During the front-end registration process, the descriptor vector D is established based on the linear features of the pool boundary extracted by the Hough transform. The Euclidean distance matching method is used for feature matching. A threshold matrix is ​​also introduced to constrain the feature similarity between the current frame and the previous frames to avoid erroneous matching errors.

[0046] See also Figure 1 In this embodiment, the SLAM method under the structured sewage pool includes the following steps:

[0047] Step 1: The dredging robot is equipped with a mechanical scanning sonar (MSIS) and an inertial measurement unit (IMU) as information collection sensors, and an Intel microprocessor as an information processing terminal. SLAM work is performed in the Robot Operating System (ROS) under the Linex system, and information exchange between the sensor and processor is achieved by publishing and subscribing to topics. While operating, the dredging robot collects MSIS data and IMU data with the same timestamp, subscribes to the topic published by the MSIS, obtains the scan data, and preprocesses the scan data to eliminate acoustic noise and extract the correct obstacle targets. Specifically:

[0048] Step 1.1: Extract single-frame (Ping) sonar information from the continuous scanning information. Single-frame sonar information is a string of binary data. The number of bits is determined by the scanning range, and the size of each bit is the echo intensity (Bin) corresponding to the unit step. Read the obstacle position information with a certain intensity to establish a rectangular coordinate system and a polar coordinate system with the MSIS as the coordinate origin. Extract the Bin intensity information from the single-frame data using the calculation formula:

[0049] (1)

[0050] Where: i is the number of pings, h is the position of the bin in the current ping, and N is the total number of bins; is the rectangular coordinate parameter corresponding to each Bin in the actual area, x h For this Bin's horizontal coordinate, y h The vertical coordinate of this Bin; is the polar coordinate parameter corresponding to each Bin in the actual area, Indicates the polar coordinate angle of the current ping.

[0051] By using formula (1), the coordinate parameters of each bin are calculated in the coordinate system centered on the sonar, which converts the original echo intensity and occlusion rate format of the sonar data into a data format similar to that of lidar.

[0052] Step 1.2: Due to the working characteristics of the MSIS itself, there is high-intensity noise interference near the device. To eliminate the impact of this high-intensity abnormal noise, the first few bits of data near the origin are assigned a value of 0. The number of abnormal data bits depends on the scanning range. The solution formula for the abnormal data bits is:

[0053] (2)

[0054] Where: Indicates the minimum detection distance, which is 0.75m: n is the total number of Bins in a single Ping data: L max The detection distance currently set; This is the abnormal range of the required solution.

[0055] Step 1.3: Set the threshold Bin_T to remove a large amount of small background noise. The method is to take the median value of the intensity of each Bin in each Ping data, first calculate the maximum and minimum values ​​of the Bin, and then calculate the threshold Bin_T using formula (3). Bins smaller than this threshold Bin_T are deleted. The formula is:

[0056] (3)

[0057] Wherein, Bin_T represents the threshold value; Bin_Min represents the minimum value of Bin; Bin_Max represents the maximum value of Bin.

[0058] Step 1.4: Use a target extraction algorithm that combines a threshold with the nearest neighbor method to extract the correct obstacle target and eliminate the high-intensity noise caused by acoustic multipath.

[0059] Step 1.5: Predict the state vector X and state covariance matrix P based on the IMU data collected at the same timestamp. The state vector X reflects the current position and posture information of the dredging robot, and the state covariance matrix P is used to describe the uncertainty of the state estimation. X is expressed as:

[0060] (4)

[0061] Since the dredging robot can be regarded as a movement on a two-dimensional plane when working, the state in the z direction is not considered. x is the horizontal coordinate of the dredging robot in the world coordinate system; y is the vertical coordinate of the dredging robot in the world coordinate system; θ is the posture angle; v x is the velocity in the x-axis direction; v y is the speed in the y-axis direction; m represents the map feature; n is the number of map features.

[0062] Step 2: Use IMU data to correct the motion distortion of the MSIS data processed in Step 1. Use the IMU-measured pose angle and displacement changes of the dredging robot to correct the rectangular coordinate and polar coordinate representations of the next frame of MSIS data. Specifically, the pose angle changes and displacement changes measured by the IMU are used to compensate for the rectangular coordinate and polar coordinate positions of the current scan data to eliminate the motion distortion caused by the slow scanning characteristics of the mechanical scanning sonar.

[0063] Step 3: Perform Hough transform based line feature extraction on the accumulated frames of MSIS data processed in step 2. At least 3 frames of MSIS scan data are required to successfully extract the line features. Based on the extracted line features, the next frame of MSIS data is corrected to eliminate the accumulated error. The method flow is as follows: Figure 2 shown.

[0064] Step 4: Create a description vector for the line features extracted in step 3 and save it to complete the simultaneous positioning and mapping, and obtain the state estimation result and map update result, such as Figure 3 Specifically shown are:

[0065] Step 4.1: Create a descriptor vector D for the line features processed in step 3 i And save it as the historical frame descriptor vector, repeat step 3 to extract the straight line features and establish the descriptor vector D j As the current frame descriptor vector and the historical frame descriptor vector D i Perform Euclidean distance feature matching and set a threshold matrix to eliminate incorrectly extracted features; specifically:

[0066] Step 4.1.1: Since the main environmental feature of the structured sewage pool is the straight line feature of the pool boundary, its feature position remains unchanged in the world coordinate system. Simplify the straight line feature obtained in step 3 into a straight line in the world coordinate system. Each straight line feature contains its corresponding two-dimensional spatial coordinate information in the world coordinate system. Establish the historical frame descriptor vector D i for:

[0067] (5) Where: is the direction angle of the line; Indicates the length of a straight line; Indicates the vertical distance between the straight line and the origin; ( , ) is the coordinate of the center point of the historical frame line feature in the world coordinate system.

[0068] Step 4.1.2: Repeat step 4.1.1 to create the current frame descriptor vector D j The historical frame descriptor vector D obtained in step 4.1.1 i Perform Euclidean distance matching. The matching method is to calculate the shortest straight-line distance between two vectors:

[0069] (6)

[0070] Where: d is the Euclidean distance result, ( , ) is the center point coordinate of the current frame straight line feature in the world coordinate system.

[0071] Step 4.1.3: Set the threshold matrix T, including retaining the matching pairs D with Euclidean distance d less than 0.2 i and D j And update the display of the current frame D j, and set the angle difference threshold to 5° and the length ratio threshold to 0.8. If all of the above parameters are less than the set thresholds, the match is considered established.

[0072] (7)

[0073] Where: Descriptor vector D for the current frame j and the historical frame descriptor vector D i The angle difference; Descriptor vector D for the current frame j and the historical frame descriptor vector D i d represents the Euclidean distance calculated in step 4.1.2; T represents the threshold matrix.

[0074] Step 4.2: Use the extended Kalman filter (EKF) method to predict and update the state vector X and the state covariance equation P;

[0075] Step 4.3: Based on the historical frame descriptor vector D established in step 4.1 i , the new state vector X obtained in step 4.2 is updated i and the state covariance equation P i Perform a secondary update to obtain the state estimation results and map updates constrained by environmental characteristics;

[0076] Step 5: Save the state estimation results and map update results obtained in step 4, such as Figure 4 The figure shows the mapping effect. The map topic is published, and visual synchronous positioning and mapping are realized in Rviz.

[0077] The descriptions presented in the above exemplary embodiments are only intended to illustrate the technical solutions of the present invention and are not intended to be exhaustive or to limit the present invention to the precise forms described. Obviously, it is possible for a person of ordinary skill in the art to make many changes and variations based on the above teachings. The exemplary embodiments are selected and described in order to explain the specific principles of the present invention and its practical applications, so that other persons skilled in the art can easily understand, implement and utilize the various exemplary embodiments of the present invention and its various selected forms and modified forms. The scope of protection of the present invention is intended to be defined by the appended claims and their equivalents.

Claims

1. A SLAM method for a dredging robot in a structured sewage pool, characterized in that: The following steps are involved: Step 1: The dredging robot is equipped with a mechanical scanning sonar (MSIS) and an inertial measurement unit (IMU) as information collection sensors, and an Intel microprocessor as an information processing terminal. When the dredging robot is working, it collects MSIS data and IMU data with the same timestamp, obtains scanning data, and pre-processes the scanning data to eliminate acoustic noise and extract the correct obstacle targets. Step 2: Apply IMU data to correct the motion distortion of the MSIS data processed in step 1 to eliminate the motion distortion caused by the slow scanning characteristics of the mechanical scanning sonar; Step 3: Perform Hough transform-based line feature extraction on the accumulated frames of MSIS data processed in step 2, obtain at least 3 frames of MSIS scan data to extract line features, and synchronously perform secondary correction on the next frame of MSIS data based on the extracted line features to eliminate accumulated errors; Step 4: Create a description vector for the line features extracted in step 3, save it, and complete simultaneous positioning and mapping to obtain the state estimation result and map update result; Step 5: Save the state estimation results and map update results obtained in step 4, and realize visual simultaneous positioning and mapping.

2. The SLAM method of a dredging robot in a structured sewage pool according to claim 1, characterized in that: The step 1 is specific as follows: Step 1.1: Extract single-frame Ping sonar information from the continuous scanning information. Each bit of the single-frame sonar information is the echo intensity bin corresponding to the unit step. Establish a rectangular coordinate system and a polar coordinate system with the MSIS as the coordinate origin; extract the bin intensity information in the single-frame data; Step 1.2: Assign the value 0 to the data near the origin. The number of abnormal data bits depends on the size of the scanning range. Step 1.3: Set the threshold Bin_T. By taking the median value of the intensity of each Bin in each Ping data, first calculate the maximum and minimum values ​​of the Bin, and then calculate the threshold Bin_T using formula (3). Bins smaller than the threshold Bin_T are deleted. The formula is: （3）， Among them, Bin_T represents the threshold; Bin_Min represents the minimum value of Bin; Bin_Max represents the maximum value of Bin; Step 1.4: Use a target extraction algorithm that combines a threshold with the nearest neighbor method to extract the correct obstacle target and eliminate the high-intensity noise caused by the acoustic multipath effect; Step 1.5: Predict the state vector X and state covariance matrix P based on the IMU data collected at the same timestamp. The state vector X reflects the current position and pose information of the dredging robot, and the state covariance matrix P is used to describe the uncertainty of the state estimation.

3. The SLAM method of a dredging robot in a structured sewage pool according to claim 2, characterized in that: In step 1.1, the calculation formula for the Bins intensity information in a single frame of data is: （1）， Where: i is the number of pings, h is the position of the bin in the current ping, and N is the total number of bins; is the rectangular coordinate parameter corresponding to each Bin in the actual area, x h For this Bin's horizontal coordinate, y h The vertical coordinate of this Bin; is the polar coordinate parameter corresponding to each Bin in the actual area, Indicates the polar coordinate angle of the current Ping; The coordinate parameters of each Bin are calculated in the coordinate system centered on the sonar using formula (1).

4. The SLAM method for a dredging robot in a structured sewage pool according to claim 2, characterized in that: In step 1.2, the formula for calculating the number of abnormal data digits is: （2）， Where: Indicates the minimum detection distance, which is 0.75m: n is the total number of Bins in a single Ping data: L max The detection distance currently set; This is the number of abnormal data bits required to be solved.

5. The SLAM method for a dredging robot in a structured sewage pool according to claim 2, characterized in that: In step 1.5, the state vector X is expressed as: （4） Since the dredging robot can be regarded as a movement on a two-dimensional plane when working, the state in the z direction is not considered. x is the horizontal coordinate of the dredging robot in the world coordinate system; y is the vertical coordinate of the dredging robot in the world coordinate system; θ is the posture angle; v x is the velocity in the x-axis direction; v y is the speed in the y-axis direction; m represents the map feature; n is the number of map features.

6. The SLAM method for a dredging robot in a structured sewage pool according to claim 1, characterized in that: In step 2, the rectangular coordinate system and polar coordinate system representation of the next frame of MSIS data are corrected using the posture angle change and displacement change of the dredging robot measured by the IMU. Specifically, the posture angle change and displacement change of the dredging robot measured by the IMU are used to compensate for the rectangular coordinate system position and polar coordinate system position of the current scanning data.

7. The SLAM method for a dredging robot in a structured sewage pool according to claim 1, characterized in that: The step 4 is specifically as follows: Step 4.1: Create a descriptor vector D for the line features processed in step 3 i And save it as the historical frame descriptor vector, repeat step 3 to extract the straight line features and establish the descriptor vector D j As the current frame descriptor vector and the historical frame descriptor vector D i Perform Euclidean distance feature matching and set a threshold matrix to eliminate incorrectly extracted features; Step 4.2: Use the extended Kalman filter (EKF) method to predict and update the state vector X and the state covariance equation P; Step 4.3: Based on the historical frame descriptor vector D established in step 4.1 i , the new state vector X obtained in step 4.2 is updated i and the state covariance equation P i Perform a secondary update to obtain the state estimation results and map updates constrained by environmental characteristics.

8. The SLAM method for a dredging robot in a structured sewage pool according to claim 7, characterized in that: The step 4.1 is specifically as follows: Step 4.1.1: Simplify the line features obtained in step 3 into lines in the world coordinate system. Each line feature contains its corresponding two-dimensional spatial coordinate information in the world coordinate system; establish the historical frame descriptor vector D i for: （5）， Where: is the direction angle of the line; Indicates the length of a straight line; Indicates the vertical distance between the straight line and the origin; ( , ) is the coordinate of the center point of the historical frame line feature in the world coordinate system; Step 4.1.2: Repeat step 4.1.1 to create the current frame descriptor vector D j The historical frame descriptor vector D obtained in step 4.1.1 i Perform Euclidean distance matching; Step 4.1.3: Set the threshold matrix T, including retaining the matching pairs D with Euclidean distance d less than 0.2 i and D j , and update the display of the current frame D j , and set the angle difference threshold to 5° and the length ratio threshold to 0.

8. If all of the above parameters are less than the set thresholds, the match is considered established; （7）， Where: Descriptor vector D for the current frame j and the historical frame descriptor vector D i The angle difference; Descriptor vector D for the current frame j and the historical frame descriptor vector D i Length ratio; d represents the Euclidean distance calculated in step 4.1.2; T represents the threshold matrix.

9. The SLAM method for a dredging robot in a structured sewage pool according to claim 8, characterized in that: In step 4.1.2, the matching method is to calculate the shortest straight-line distance between the two vectors: （6）， Where: d is the Euclidean distance result, ( , ) is the center point coordinate of the current frame straight line feature in the world coordinate system.

Images