Tank wall climbing robot tracking and positioning method based on fixed-point laser radar

Abstract

The application discloses a kind of based on fixed-point laser radar's storage tank wall-climbing robot tracking positioning method, first laser radar is fixed on the surface of storage tank, laser radar obtains initial point cloud in real time during the movement of wall-climbing robot, target segmentation is carried out to initial point cloud, and robot point cloud is obtained;Then, robot point cloud is projected to the two-dimensional plane where laser radar visual angle is located, obtain two-dimensional point cloud, based on two-dimensional point cloud, robot bounding box is fitted by L-Shape algorithm, and the preliminary positioning of wall-climbing robot is realized;Finally, according to the best bounding box of known information, the position of robot bounding box is dynamically corrected by center point guide, and based on the center point coordinates of robot bounding box after position correction, wall-climbing robot is tracked and positioned using IMM-UKF algorithm.The method uses laser radar installed in the fixed position of storage tank to perceive the motion state of wall-climbing robot, effectively reduces the cumulative error caused by the motion of robot itself and the interference and limitation of sensor, and improves the positioning accuracy and stability.

Description

Technical Field

[0001] This invention belongs to the field of intelligent robot positioning technology, specifically a tracking and positioning method for a tank-climbing robot based on fixed-point lidar. Background Technology

[0002] Wall-climbing robots are specialized robots that utilize robotic mobility and wall-adhesion technology to work on the surfaces of large, nearly vertical structural components. Due to their high efficiency and safety, they can replace manual labor in the inspection and maintenance of large structural components and are widely used in various fields such as industrial equipment repair, building maintenance, and military applications. When performing inspection tasks, wall-climbing robots need to accurately obtain their own position information on the component surface in order to formulate effective path planning and control strategies.

[0003] Depending on the sensor installation location, wall-climbing robot localization can be broadly categorized into two methods: those based on onboard sensors and those based on external sensors. In onboard sensor localization methods, vision sensors and inertial measurement units (IMUs) are widely used. IMUs obtain the robot's current speed and position by integrating measured values. However, due to the drift property of IMUs, localization errors accumulate over time, limiting their application in long-distance movements. Vision sensors, on the other hand, move with the robot and perform localization by matching feature points between consecutive image frames. However, vision-based localization methods are highly dependent on environmental features, and the surfaces of the components the wall-climbing robot adheres to lack stable features, making accurate localization through feature point matching difficult.

[0004] Common localization methods based on external sensors include Ultra Wide Band (UWB) localization, which fuses an IMU (Instrument Measure) and multiple UWB signals to achieve localization of the wall-climbing robot by measuring the time-of-flight of wireless signals. However, rigid structures severely absorb wireless signals, thus limiting the application of this method. Microphone array-based localization methods use microphone arrays to pick up sound signals emitted by the robot for localization, but sound waves undergo multiple reflections on the surface of the structure, leading to multipath effects and affecting localization accuracy. Summary of the Invention

[0005] To address the shortcomings of existing technologies, the technical problem this invention aims to solve is to propose a tracking and positioning method for a tank-climbing robot based on fixed-point lidar.

[0006] The present invention solves the aforementioned technical problem by adopting the following technical solution:

[0007] A method for tracking and locating a tank-climbing robot based on fixed-point lidar, characterized by the following steps:

[0008] Step 1: Fix the lidar to the surface of the storage tank. During the movement of the wall-climbing robot, the lidar acquires the initial point cloud in real time. The initial point cloud is then segmented to obtain the robot point cloud.

[0009] Step 2: Project the robot point cloud onto the 2D plane where the LiDAR viewpoint is located to obtain a 2D point cloud; calculate the angle between the line connecting each point in the 2D point cloud to the LiDAR and the x-axis or y-axis of the LiDAR coordinate system, and denote the angle between the line connecting the point closest to the LiDAR and the LiDAR and the x-axis or y-axis of the LiDAR coordinate system as θ0; divide the points with an angle greater than or equal to θ0 into set A, and the points with an angle less than θ0 into set B; perform line fitting on the points in sets A and B respectively to obtain lines LA and LB; calculate the angle α between lines LA and LB. If the angle α is less than or equal to the decision threshold, use the L-Shape algorithm to fit all points in the 2D point cloud to obtain the robot bounding box; if the angle α is greater than the decision threshold, retain the line with more points and use it as the reference edge for the L-Shape algorithm fitting. Obtain another edge based on the constraint relationship of the perpendicularity of adjacent sides of the rectangle, and fit the other two edges of the bounding box based on these two edges to obtain the robot bounding box.

[0010] Step 3: Calculate the offsets Δx′ and Δy′ of the robot bounding box center point in the bounding box coordinate system according to equation (1):

[0011]

[0012] In the formula, (OrigBox) x OrigBox y () represents the original center coordinates of the robot's bounding box, BestBox l BestBox w These are the optimal length and width of the bounding box, respectively. (OrigBox) l OrigBox w These are the length and width of the robot's bounding box, respectively.

[0013] By using the coordinate transformation in equation (2), the coordinates (x, y) of the center point of the robot bounding box after position correction are obtained;

[0014]

[0015] In the formula, (x orig ,y orig () represents the original center point coordinates of the robot's bounding box in the lidar coordinate system. The angle between the lidar coordinate system and the bounding box coordinate system;

[0016] Based on the coordinates of the center point of the robot's bounding box after position correction, the IMM-UKF algorithm is used to track and locate the wall-climbing robot.

[0017] Compared with the prior art, the beneficial effects of the present invention are:

[0018] This invention employs a fixed-point lidar to sense the motion state of a wall-climbing robot. This deployment method is unaffected by changes in ambient lighting or vibrations caused by the robot's own movement, effectively reducing the cumulative errors caused by the robot's own motion and sensor interference and limitations, thus improving positioning accuracy and stability. An improved L-Shape algorithm bounding box fitting method is proposed to ensure the correctness of the fitting results; dynamic position correction guided by the center point solves the self-occlusion problem of the wall-climbing robot, improving tracking reliability. Attached Figure Description

[0019] Figure 1 This is the overall flowchart;

[0020] Figure 2 A schematic diagram of the autonomous positioning system for a wall-climbing robot;

[0021] Figure 3 Schematic diagrams of two-dimensional point clouds from different lidar perspectives;

[0022] Figure 4 This is a schematic diagram showing the positions of the robot's bounding box and the optimal bounding box.

[0023] Figure 5 A schematic diagram illustrating the position correction of the robot's bounding box;

[0024] Figure 6 This is a schematic diagram of the tracking path of a wall-climbing robot on a storage tank.

[0025] Figure 7 The position tracking error curve of the wall-climbing robot in the X-axis direction;

[0026] Figure 8 The position tracking error curve of the wall-climbing robot in the Y-axis direction. Detailed Implementation

[0027] Specific embodiments are given below with reference to the accompanying drawings. These specific embodiments are only used to describe the technical solution of the present invention in detail, and are not intended to limit the scope of protection of this application.

[0028] This invention proposes a tracking and positioning method for a tank-climbing robot based on fixed-point lidar (hereinafter referred to as the method, see [link]). Figures 1-8 This includes the following steps:

[0029] Step 1: As Figure 2As shown, the autonomous localization system for the wall-climbing robot includes a storage tank, a wall-climbing robot, and a lidar. The lidar is fixed to the surface of the storage tank by a magnetic adsorption device. During the movement of the wall-climbing robot, the lidar acquires the initial point cloud in real time. The Patchwork++ algorithm is used to segment the initial point cloud to obtain the robot point cloud.

[0030] Step 2: Use the improved L-Shape algorithm to perform target detection, obtain the robot bounding box, and achieve the initial localization of the wall-climbing robot;

[0031] First, the robot's point cloud is projected onto the two-dimensional plane where the lidar view is located to obtain a two-dimensional point cloud; Figure 3 Two-dimensional point clouds from different lidar perspectives;

[0032] Based on the geometric characteristics of the wall-climbing robot, an L-shaped intersecting straight line can be used to fit the robot's contour. First, calculate the angle between the line connecting each point in the 2D point cloud to the lidar and the x-axis or y-axis of the lidar coordinate system. Denote the angle between the line connecting the point closest to the lidar and the lidar and the x-axis or y-axis as θ0. Divide the points with angles greater than or equal to θ0 into set A, and the points with angles less than θ0 into set B. Then, use the RANSAC algorithm to perform straight line fitting on the points in sets A and B respectively, obtaining the straight line LA and... LB; Calculate the angle α between lines LA and LB. If the angle α is less than or equal to the decision threshold, it indicates that lines LA and LB are perpendicular, and all points are located on one side of the wall-climbing robot. In this case, the L-Shape algorithm is used to fit all points to obtain the robot's bounding box, achieving the initial localization of the wall-climbing robot. If the angle α is greater than the decision threshold, it indicates that lines LA and LB may not be perpendicular. Therefore, the positional relationship between lines LA and LB needs to be corrected, that is, the line LA or LB containing more points is retained as the reference edge y for the L-Shape algorithm fitting. ai =kx ai +b a The other side y is obtained based on the constraint that adjacent sides of the rectangle are perpendicular. bi =-kx bi +b b The other two edges of the bounding box are fitted based on these two edges to obtain the robot's bounding box, thus achieving the initial localization of the wall-climbing robot; the judgment threshold is set to 60°; as shown... Figure 4 As shown, the dashed rectangle is the robot's bounding box, and the solid rectangle is the optimal bounding box. The position of the optimal bounding box is known.

[0033] Step 3: The position of the robot's bounding box is dynamically corrected by guiding the center point, and the IMM-UKF algorithm is used to track and locate the wall-climbing robot;

[0034] Because the self-occlusion of the wall-climbing robot depends on its position relative to the LiDAR, this may cause the robot's bounding box to deviate from the optimal bounding box center point. Since the LiDAR's position tracker only tracks the robot's center point, when self-occlusion occurs, the robot's bounding box position needs to be corrected using known optimal bounding box information. Essentially, this involves compensating for the coordinates of the robot's bounding box center point. Specifically, the correction direction is determined by the robot's movement direction. A diagram illustrating robot bounding box position correction is shown below. Figure 5 As shown; the formulas for calculating the center point offsets Δx′ and Δy′ in the bounding box coordinate system are as follows:

[0035]

[0036] In the formula, (OrigBox) x OrigBox y () represents the original center coordinates of the robot's bounding box, BestBox l BestBox w These are the optimal length and width of the bounding box, respectively. (OrigBox) l OrigBox w These represent the length and width of the robot's bounding box, respectively; ± depends on the direction of the wall-climbing robot's movement, with "+" for forward movement and "-" for backward movement.

[0037] By using the coordinate transformation in equation (2), the coordinates (x, y) of the center point of the robot bounding box after position correction are obtained;

[0038]

[0039] In the formula, (x orig ,y orig () represents the original center point coordinates of the robot's bounding box in the lidar coordinate system. The angle between the lidar coordinate system and the bounding box coordinate system;

[0040] In the tracking and localization of the wall-climbing robot, considering the strong nonlinearity of the robot's motion model, an unscented Kalman filter (UKF) was used for state estimation to improve the accuracy of state estimation. Simultaneously, considering the motion characteristics of the wall-climbing robot, a constant velocity (CV) model and a constant turn rate and velocity (CTRV) model were selected for target tracking.

[0041] For the constant velocity model, the state variable of the wall-climbing robot is X. CV=(x,y,z,v) x ,v y ,v z ), where (x,y,z) are the coordinates of the center point of the robot's bounding box after position correction, i.e., the position of the wall-climbing robot, (v x ,v y ,v z () represents the three-axis velocity of the wall-climbing robot; for the constant-speed turning model, the state variables of the wall-climbing robot are: Where γ, ω represents the forward speed, heading angle, and angular velocity of the wall-climbing robot, respectively. Considering the nonlinearity of the wall-climbing robot's motion model, an unscented Kalman filter is used to estimate the robot's state. Therefore, for the constant-velocity model, the state transition relationship of the wall-climbing robot is:

[0042]

[0043] In the formula, These are the state variables of the wall-climbing robot at times t and t+1 in the constant velocity model, respectively.

[0044] For the constant-speed turning model, the state transition relationship of the wall-climbing robot is as follows:

[0045]

[0046] In the formula, γ t , ω t Let x represent the forward speed, heading angle, and angular velocity of the wall-climbing robot at time t, respectively. t y t Δt represents the x-axis and y-axis coordinates of the center point of the robot bounding box after position correction at time t, and Δt is the time interval between time t+1 and time t.

[0047] The basic idea of ​​the IMM algorithm is to match different motion modes of a wall-climbing robot with multiple different motion models. The transition probability between different models is a Markov matrix. Kalman filtering is used for state estimation and model probability updates of the wall-climbing robot. Let the constant-speed turning model be denoted as model i, and the constant-speed model as model j. Then, the formula for calculating the conditional model probability is:

[0048]

[0049] In the formula, μ (i|j),t+1 It is the conditional model probability at time t+1, p i,j μ is the transition probability from model i to j. i,t is the probability of model i at time t, and r is the number of models. In this embodiment, the number of models is 2.

[0050] Calculate the initial state and initial covariance of model j after UKF filtering according to equations (6) and (7), respectively;

[0051]

[0052]

[0053] In the formula, X represents the initial state and initial covariance of model j at time t+1 after UKF filtering. i,t P i,t These are the state and covariance of model i at time t after UKF filtering;

[0054] Mixed state estimation is performed using the UKF algorithm, which is... The UKF algorithm takes the position observation Z(t+1) at time t as input and outputs the mixture state X of model j at time t+1. j,t+1 and mixed covariance P j,t+1 ;

[0055] Finally, the state and covariance of the wall-climbing robot are estimated according to equations (8) and (9) to obtain the state estimate of the wall-climbing robot. The state estimate includes the position and velocity of the wall-climbing robot, thereby completing the tracking and positioning of the wall-climbing robot.

[0056]

[0057]

[0058] In the formula, The state estimate and covariance of the wall-climbing robot at time t+1 are μ. j,t It is the probability of model j at time t.

[0059] Figure 6 This is a schematic diagram of the tracking path of the wall-climbing robot on the storage tank. The total path length is 18.59 meters. The dashed line represents the actual path, and the solid line represents the tracking path. As can be seen from the figure, the method of the present invention achieves effective tracking and positioning of the wall-climbing robot within a certain range.

[0060] Figure 7 and Figure 8The evolution of the position tracking error of the wall-climbing robot in the X and Y axes over time is demonstrated. The position tracking error in the X-axis direction is controlled below 0.136m, with an average error of 0.033m; the position tracking error in the Y-axis direction is controlled below 0.129m, with an average error of 0.030m. These results indicate that the present invention achieves high positioning accuracy. This is due to the dynamic correction of the robot's bounding box position via center point guidance, the use of the IMM-UKF algorithm for tracking and positioning the robot's bounding box center point, and the use of a fixed-position external LiDAR for tracking and positioning the wall-climbing robot. This effectively reduces the cumulative error caused by interference and limitations from onboard sensors, thus improving positioning accuracy and stability.

[0061] In practical applications of wall-climbing robots, point cloud data collected by LiDAR within a certain distance is crucial for localization and tracking. However, beyond a certain distance, point cloud data may be incomplete or features may be unclear, significantly impacting tracking results. Through extensive experimentation, the operating domain of the wall-climbing robot was determined—the appropriate distance range between the robot and the LiDAR. This range ensures that the LiDAR can effectively acquire clear and accurate point cloud data, improving the tracking reliability of the robot within this operating domain.

[0062] Table 1. Tracking errors (RMSE) in the X and Y directions at different distances within the wall-climbing robot's operational range.

[0063]

[0064] Table 1 shows the tracking error of the wall-climbing robot at different distances within its working area. As can be seen from the table, the tracking effect decreases significantly when the wall-climbing robot exceeds the working area, which also indicates the criticality of the working area range and its significant impact on the tracking and positioning accuracy of the wall-climbing robot.

[0065] Any aspects not covered in this invention are applicable to existing technologies.

Claims

1. A method for tracking and locating a tank-climbing robot based on fixed-point lidar, characterized in that, The method includes the following steps: Step 1: Fix the lidar to the surface of the storage tank. During the movement of the wall-climbing robot, the lidar acquires the initial point cloud in real time. The initial point cloud is then segmented to obtain the robot point cloud. Step 2: Project the robot point cloud onto the 2D plane where the LiDAR viewpoint is located to obtain a 2D point cloud; calculate the angle between the line connecting each point in the 2D point cloud to the LiDAR and the x-axis or y-axis of the LiDAR coordinate system, and denote the angle between the line connecting the point closest to the LiDAR and the LiDAR and the x-axis or y-axis of the LiDAR coordinate system as θ0; divide the points with an angle greater than or equal to θ0 into set A, and the points with an angle less than θ0 into set B; perform line fitting on the points in sets A and B respectively to obtain lines LA and LB; calculate the angle α between lines LA and LB. If the angle α is less than or equal to the decision threshold, use the L-Shape algorithm to fit all points in the 2D point cloud to obtain the robot bounding box; if the angle α is greater than the decision threshold, retain the line with more points and use it as the reference edge for the L-Shape algorithm fitting. Obtain another edge based on the constraint relationship of the perpendicularity of adjacent sides of the rectangle, and fit the other two edges of the bounding box based on these two edges to obtain the robot bounding box. Step 3: Calculate the offsets Δx′ and Δy′ of the robot bounding box center point in the bounding box coordinate system according to equation (1): In the formula, (OrigBox) x OrigBox y () represents the original center coordinates of the robot's bounding box, BestBox l BestBox w These are the optimal length and width of the bounding box, respectively. (OrigBox) l OrigBox w These are the length and width of the robot's bounding box, respectively; By using the coordinate transformation in equation (2), the coordinates (x, y) of the center point of the robot bounding box after position correction are obtained; In the formula, (x orig ,y orig () represents the original center point coordinates of the robot's bounding box in the lidar coordinate system. The angle between the lidar coordinate system and the bounding box coordinate system; Based on the coordinates of the center point of the robot's bounding box after position correction, the IMM-UKF algorithm is used to track and locate the wall-climbing robot.

2. The tracking and positioning method for a tank-climbing robot based on fixed-point lidar according to claim 1, characterized in that, In the second step, the threshold is set at 60°.

Images