A laser SLAM pose prediction method for substation inspection robots

Abstract

The present invention discloses a method for predicting the pose of a substation inspection robot using laser synchronized localization and mapping (SLAM), comprising the following steps: (1) calculating the translational linear velocity of the inspection robot; (2) calculating the rotational angular velocity of the inspection robot; (3) correcting the rotational angular velocity; (4) predicting the pose of the inspection robot at the current moment; and (5) determining whether the sensor has new data. If so, the method returns to step (1); otherwise, the method ends. When predicting the prior pose, the method uses the four data at the end of the data queue for prediction, and uses the angular velocity calculated from the point cloud pose to correct the calculation result of the inertial measurement unit (IMU) data. The method effectively improves the effect of the laser SLAM pose prediction of the substation inspection robot and has good application prospects.

Description

Technical Field

[0001] The present invention relates to the technical field of synchronous positioning and mapping of substation inspection robots, and in particular to a laser SLAM posture prediction method for substation inspection robots. Background Art

[0002] With the rise of artificial intelligence (AI) technology, a variety of unmanned vehicles and mobile robots have emerged one after another. SLAM plays a crucial role in the autonomous driving sector. Based on the primary sensor type, researchers categorize SLAM technology into two main categories: visual SLAM and laser SLAM. Visual SLAM uses cameras as its primary sensor, analyzing and processing images captured by the camera to construct and represent the surrounding environment. Laser SLAM uses lidar (LiDAR) as its primary sensor, constructing an environmental map by scan-matching point cloud data collected by the LiDAR. Compared to vision-based SLAM methods, laser SLAM is insensitive to lighting variations and can accurately capture environmental information in indoor and outdoor scenes with drastic lighting changes. Laser SLAM algorithms, characterized by their high measurement accuracy and wide detection range, have attracted extensive research and exploration by researchers worldwide. However, when the IMU data source fluctuates significantly or is unreliable, the traditional method of calculating linear and angular velocities by taking the average of the data queue at the beginning and end of the data stream is also unreliable, resulting in increased errors in laser SLAM pose predictions. Summary of the Invention

[0003] To address the aforementioned technical issues, the present invention provides a method for predicting the pose of a substation inspection robot using laser SLAM. This method first uses the four data points at the end of a data queue to calculate the robot's linear velocity and angular velocity. Secondly, the pose transformation data is used to calculate the robot's angular change increment, and the angular change increment calculated using the IMU data is corrected. Finally, the corrected pose increment is used to predict the robot's pose. This method can effectively improve the performance of laser SLAM pose prediction for substation inspection robots, especially when the IMU data source fluctuates significantly or is unreliable.

[0004] The technical solutions of the present invention are as follows:

[0005] A laser SLAM pose prediction method for a substation inspection robot includes the following steps:

[0006] (1) Calculate the translational linear velocity of the inspection robot;

[0007] (2) Calculate the rotational angular velocity of the inspection robot;

[0008] (3) Correction of rotation angular velocity;

[0009] (4) Predict the position and posture of the inspection robot at the current moment;

[0010] (5) Determine whether the sensor has new data. If so, return to step (1). If not, end.

[0011] The steps for calculating the translational linear velocity of the inspection robot in step (1) are as follows:

[0012] Step (1-1): The lidar sensor collects data multiple times to form a pose data queue;

[0013] Step (1-2): Read the last four data from the pose data queue;

[0014] Step (1-3): Calculate the translational linear velocity according to formula (1).

[0015] lv=[0.8*(p t-1 -p t-2 )+0.2*(p t-3 -p t-4 )] / T (1)

[0016] In formula (1), lv represents the translational linear velocity of the substation inspection robot, p represents the coordinates of the point cloud pose, t represents the latest point cloud pose timestamp, and T represents an operation cycle.

[0017] The steps for correcting the rotational angular velocity in step (3) are as follows:

[0018] Step (3-1): Calculate the quaternion of the rotation increment between the two frames of point cloud according to formula (2);

[0019] ΔPose=quatDivide(R2,R1) (2)

[0020] In formula (2), R1 and R2 represent the quaternions of the rotation of the two-frame point cloud pose, quatDivide represents the quaternion division function, and ΔPose represents the quaternion of the rotation increment between the two-frame point cloud pose.

[0021] Step (3-2): Convert the quaternion of the rotation increment between the two frames of point cloud pose into the rotation Euler angle according to formula (3).

[0022]

[0023] In formula (3), φ pose ,θ pose 、 They represent the yaw angle increment, pitch angle increment, and roll angle increment of the substation inspection robot calculated from the posture, respectively. quatToEul represents the function of converting quaternion into Euler angle.

[0024] Step (3-3): Convert the quaternion of the rotation increment between the two IMU poses into the rotation Euler angle according to formula (4).

[0025]

[0026] In formula (4), φ imu ,θ imu 、 They respectively represent the yaw angle increment, pitch angle increment, and roll angle increment of the substation inspection robot calculated by the IMU data.

[0027] Step (3-4): According to formula (5), the Euler angle increment calculated by the point cloud pose is used to correct the Euler angle increment calculated by the IMU data.

[0028]

[0029] In formula (5), euler new represents the corrected angle change increment of the substation inspection robot, α and β represent weights, and α+β=1.

[0030] Step (3-5): Convert the corrected Euler angle increment into a quaternion according to formula (6);

[0031] R=eulToQuat(euler new ) (6)

[0032] In formula (6), R represents the corrected rotation increment quaternion, and eulToQuat represents the calculation function for converting Euler angles into quaternions.

[0033] The steps for predicting the current posture in step (4) are as follows:

[0034] Step (4-1): Calculate the x- and y-direction components of the inspection robot's linear velocity according to formula (7);

[0035]

[0036] In formula (7), lv x Represents the component of the inspection robot's linear velocity in the x direction, lv y Represents the component of the inspection robot's linear velocity in the y direction, The angle between the inspection robot's linear velocity lv and the x-axis of the coordinate system.

[0037] Step (4-2): According to formula (8), add the posture increment to the posture of the previous moment to predict the posture of the inspection robot at the current moment.

[0038]

[0039] In formula (8), x and y represent the coordinates of the inspection robot in the map, θ represents the Euler angle of the inspection robot, the subscript cur represents the predicted pose at the current moment, and the subscript last represents the pose at the previous moment.

[0040] Beneficial effects of the present invention:

[0041] When predicting the prior pose, the method of the present invention uses the four data at the end of the data queue for prediction, and uses the angular velocity calculated from the point cloud pose to correct the calculation result of the inertial measurement unit (IMU) data. It effectively improves the laser SLAM pose prediction effect of the substation inspection robot and has good application prospects. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] Figure 1 is a system flow chart of the present invention;

[0043] Figure 2 A schematic diagram for comparing mapping effects. DETAILED DESCRIPTION

[0044] The present invention will be further described below with reference to the accompanying drawings.

[0045] like Figure 1 As shown in FIG, a novel substation inspection robot path planning method includes the following steps:

[0046] (1) Calculate the translational linear velocity of the inspection robot;

[0047] (2) Calculate the rotational angular velocity of the inspection robot;

[0048] (3) Correction of rotation angular velocity;

[0049] (4) Predict the position and posture of the inspection robot at the current moment;

[0050] (5) Determine whether the sensor has new data. If so, return to step (1). If not, end.

[0051] The steps for calculating the translational linear velocity of the inspection robot in step (1) are as follows:

[0052] Step (1-1): The lidar sensor collects data multiple times to form a pose data queue;

[0053] Step (1-2): Read the last four data from the pose data queue;

[0054] Step (1-3): Calculate the translational linear velocity according to formula (1).

[0055] lv=[0.8*(p t-1 -p t-2 )+0.2*(p t-3 -p t-4 )] / T (1)

[0056] In formula (1), lv represents the translational linear velocity of the substation inspection robot, p represents the coordinates of the point cloud pose, t represents the latest point cloud pose timestamp, and T represents an operation cycle.

[0057] The steps for correcting the rotational angular velocity in step (3) are as follows:

[0058] Step (3-1): Calculate the quaternion of the rotation increment between the two frames of point cloud according to formula (2);

[0059] ΔPose=quatDivide(R2,R1) (2)

[0060] In formula (2), R1 and R2 represent the quaternions of the rotation of the two-frame point cloud pose, quatDivide represents the quaternion division function, and ΔPose represents the quaternion of the rotation increment between the two-frame point cloud pose.

[0061] Step (3-2): Convert the quaternion of the rotation increment between the two frames of point cloud pose into the rotation Euler angle according to formula (3).

[0062]

[0063] In formula (3), φ pose ,θ pose 、 They represent the yaw angle increment, pitch angle increment, and roll angle increment of the substation inspection robot calculated from the posture, respectively. quatToEul represents the function of converting quaternion into Euler angle.

[0064] Step (3-3): Convert the quaternion of the rotation increment between the two IMU poses into the rotation Euler angle according to formula (4).

[0065]

[0066] In formula (4), φ imu ,θ imu 、 They respectively represent the yaw angle increment, pitch angle increment, and roll angle increment of the substation inspection robot calculated by the IMU data.

[0067] Step (3-4): According to formula (5), the Euler angle increment calculated by the point cloud pose is used to correct the Euler angle increment calculated by the IMU data.

[0068]

[0069] In formula (5), euler new represents the corrected angle change increment of the substation inspection robot, α and β represent weights, and α+β=1.

[0070] Step (3-5): Convert the corrected Euler angle increment into a quaternion according to formula (6);

[0071] R=eulToQuat(euler new ) (6)

[0072] In formula (6), R represents the corrected rotation increment quaternion, and eulToQuat represents the calculation function for converting Euler angles into quaternions.

[0073] The steps for predicting the current posture in step (4) are as follows:

[0074] Step (4-1): Calculate the x- and y-direction components of the inspection robot's linear velocity according to formula (7);

[0075]

[0076] In formula (7), lv x Represents the component of the inspection robot's linear velocity in the x direction, lv y Represents the component of the inspection robot's linear velocity in the y direction, The angle between the inspection robot's linear velocity lv and the x-axis of the coordinate system.

[0077] Step (4-2): According to formula (8), add the posture increment to the posture of the previous moment to predict the posture of the inspection robot at the current moment.

[0078]

[0079] In formula (8), x and y represent the coordinates of the inspection robot in the map, θ represents the Euler angle of the inspection robot, the subscript cur represents the predicted pose at the current moment, and the subscript last represents the pose at the previous moment.

[0080] The improved laser SLAM pose prediction method for substation inspection robot is used for mapping simulation experiment, and the mapping comparison effect is shown in the figure. Figure 2 As shown. The mapping effect using the traditional laser SLAM method is as follows Figure 2 As shown in (a), Figure 2As can be seen in (a), there are overlapping parts in the map where the arrows point, which indicates that the mapping effect of the traditional laser SLAM algorithm is poor. The mapping effect after using the improved laser SLAM pose prediction method for substation inspection robots is shown in the figure. Figure 2 As shown in (b), Figure 2 As can be seen in (a), after the improvement, the algorithm effectively eliminates the overlapping parts of the map pointed by the arrows, which shows that the improved laser SLAM pose prediction method for substation inspection robots can effectively improve the mapping effect.

[0081] To further demonstrate the advantages of the present invention, a comparative evaluation of the absolute error and mean square error of the mapping is performed, and the error evaluation results are shown in Table 1. As can be seen from Table 1, the use of the improved laser SLAM pose prediction method for substation inspection robots can effectively reduce the absolute error and mean square error of rotation and translation, thereby achieving the purpose of improving the mapping effect.

[0082] Table 1. Laser SLAM mapping error evaluation results before and after improvement

[0083]

[0084]

Claims

1. A laser SLAM pose prediction method for a substation inspection robot, characterized by: The following steps are involved: (1) Calculate the translational linear velocity of the inspection robot; (2) Calculate the rotational angular velocity of the inspection robot; (3) Correction of rotational angular velocity; (4) Predict the position and posture of the inspection robot at the current moment; (5) Determine whether the sensor has new data. If so, return to step (1). If not, end. The steps for calculating the translational linear velocity of the inspection robot in step (1) are as follows: Step (1-1): The lidar sensor collects data multiple times to form a pose data queue; Step (1-2): Read the last four data from the pose data queue; Step (1-3): Calculate the translational linear velocity according to formula (1); lv=[0.8*(p t-1 -p t-2 )+0.2*(p t-3 -p t-4 )] / T (1) In formula (1), lv represents the translational linear velocity of the substation inspection robot, p represents the coordinates of the point cloud pose, t represents the latest point cloud pose timestamp, and T represents an operation cycle; The steps for correcting the rotational angular velocity in step (3) are as follows: Step (3-1): Calculate the quaternion of the rotation increment between the two frames of point cloud according to formula (2); △Pose=quatDivide(R2,R1) (2) In formula (2), R1 and R2 represent the quaternions of the rotation of the two-frame point cloud pose, quatDivide represents the quaternion division function, and △Pose represents the quaternion of the rotation increment between the two-frame point cloud pose; Step (3-2): Convert the quaternion of the rotation increment between the two frames of point cloud pose into the rotation Euler angle according to formula (3); In formula (3), φ pose ,θ pose 、 They represent the yaw angle increment, pitch angle increment, and roll angle increment of the substation inspection robot calculated from the posture, respectively. quatToEul represents the function of converting quaternion into Euler angle. Step (3-3): Convert the quaternion of the rotation increment between the two IMU poses into the rotation Euler angle according to formula (4); In formula (4), φ imu ,θ imu 、 They represent the yaw angle increment, pitch angle increment, and roll angle increment of the substation inspection robot calculated by IMU data respectively; Step (3-4): According to formula (5), the Euler angle increment calculated by the point cloud pose is used to correct the Euler angle increment calculated by the IMU data; In formula (5), euler new represents the corrected angle change increment of the substation inspection robot, α and β represent weights, and α+β=1; Step (3-5): Convert the corrected Euler angle increment into a quaternion according to formula (6); R=eulToQuat(euler new ) (6) In formula (6), R represents the corrected rotation increment quaternion, and eulToQuat represents the calculation function for converting Euler angles into quaternions.

2. The laser SLAM pose prediction method for a substation inspection robot according to claim 1, characterized in that: The steps for predicting the current posture in step (4) are as follows: Step (4-1): Calculate the x- and y-direction components of the inspection robot's linear velocity according to formula (7); In formula (7), lv x Represents the component of the inspection robot's linear velocity in the x direction, lv y Represents the component of the inspection robot's linear velocity in the y direction, Represents the angle between the inspection robot's linear velocity lv and the x-axis of the coordinate system; Step (4-2): According to formula (8), add the posture increment to the posture of the previous moment to predict the posture of the inspection robot at the current moment; In formula (8), x and y represent the coordinates of the inspection robot in the map, θ represents the Euler angle of the inspection robot, the subscript cur represents the predicted pose at the current moment, and the subscript last represents the pose at the previous moment.

Images