A mapping and positioning method integrating lidar and depth camera point clouds

Abstract

The present invention discloses a mapping and positioning method that integrates laser radar and depth camera point clouds, comprising the following steps: S1, obtaining a laser point cloud and a depth camera point cloud to be processed; S2, performing external parameter calibration to determine the relationship between the laser radar and depth camera coordinate axes, and converting the depth camera point cloud to a laser radar coordinate system; S3, re-segmenting the laser radar and depth camera point clouds in the converted laser radar coordinate system according to the laser radar scan lines, and converting the depth camera point cloud according to the laser radar point cloud format, fusing them to form a frame of point cloud in the same format; S4, obtaining a point cloud map of the environment; S5, obtaining the absolute position of a quadruped robot in the environment, and integrating an IMU sensor and a foot odometer to obtain the position of the quadruped robot during movement. The present invention can obtain accurate positioning information and can adapt to the complex environment of a converter station; it uses few computing resources, and the algorithm can be installed on a general computing platform; and it has high repeatability.

Description

Technical Field

[0001] The present invention belongs to the field of robot positioning technology, and in particular relates to a mapping and positioning method that integrates laser radar and depth camera point cloud. Background Art

[0002] Thanks to in-depth research and breakthroughs in artificial intelligence, intelligent mobile robots have found widespread application in general fields. However, the complex and harsh environment of converter stations places even more stringent requirements on inspection robots, and their deployment presents many challenges. Autonomous mapping and positioning technologies are key to achieving intelligent inspection in converter stations.

[0003] Typically, robots are equipped with both a LiDAR and a camera to perform simultaneous positioning. However, LiDAR can only accurately locate areas at long distances, such as those within 80 meters and beyond 2 meters, while cameras can only accurately locate areas at short distances, such as those within 2 meters and beyond 0.1 meters. To achieve comprehensive and precise positioning at both long and short distances, the positioning information obtained by LiDAR and cameras must be integrated. Summary of the Invention

[0004] In order to overcome the shortcomings of the existing technology, the present invention provides a mapping and positioning method of fused lidar and depth camera point cloud, which can convert the point cloud information obtained by the depth camera into lidar lines, and fuse the positioning information obtained by the lidar and the depth camera to obtain accurate overall positioning information.

[0005] The technical solution adopted by the present invention to solve the technical problem is: a mapping and positioning method integrating laser radar and depth camera point cloud, comprising the following steps:

[0006] S1, obtaining the laser point cloud and depth camera point cloud to be processed, including point clouds belonging to the same spatial range and those not belonging to the same spatial range;

[0007] S2, calibrate the external parameters of the lidar and depth camera, determine the coordinate axis relationship between the lidar and depth camera, and convert the depth camera point cloud to the lidar coordinate system;

[0008] S3, based on the field of view of the depth camera and the lidar, re-segment the lidar and depth camera point clouds in the converted lidar coordinate system according to the lidar scan lines, and convert the depth camera point cloud according to the lidar point cloud format, fusing them into a frame of point cloud with the same format;

[0009] S4, based on the fusion of the point cloud in S3 and the IMU sensor data, the environment is mapped to obtain a point cloud map of the environment;

[0010] S5, based on the point cloud fused in S3, matches the point cloud map established in S4 to obtain the absolute position of the quadruped robot in the environment, and fuses the IMU sensor and foot odometer to obtain the position of the quadruped robot during movement.

[0011] Furthermore, the S1 step includes the following sub-steps:

[0012] S11, setting a device for synchronizing the time of collecting the laser radar point cloud and collecting the depth camera point cloud;

[0013] S12, filtering out inaccurate depth point clouds measured within the depth camera setting range;

[0014] S13, filtering out the LiDAR point clouds that are inaccurately measured by the LiDAR within the set range, as well as the point clouds that are projected onto the LiDAR device;

[0015] S14, filters out lidar and depth camera noise.

[0016] Furthermore, the step S2 includes the following sub-steps:

[0017] S21, calibrate the external parameters between the laser radar and the depth camera, including the rotation matrix R and translation vector t from the depth camera to the laser radar coordinate system; prepare a black and white checkerboard, collect several camera images and laser radar point clouds in which both the depth camera and the laser radar can observe the complete checkerboard, pair them in approximately synchronous time, import them into the Lidar Toolbox toolbox in Matlab, let the toolbox detect the checkerboard in the image, manually specify the point cloud falling on the checkerboard in the laser radar point cloud frame, and let the toolbox perform calibration to obtain the rotation matrix R and translation vector t from the depth camera to the laser radar coordinate system;

[0018] S22, construct a conversion function from the depth camera coordinate system to the lidar coordinate system. The conversion function is,

[0019]

[0020] Among them, X l , Y l , Z l Represents the three coordinate axes of the laser radar coordinate system, X l , Y l , Z l Represents the depth camera coordinate system;

[0021] S23, converting the depth camera point cloud to the lidar coordinate system according to the rotation matrix R and translation vector t of the depth camera to the lidar coordinate system calibrated in step S21 and the conversion function in step S22;

[0022] S24, determining, based on the field of view angles of the laser radar and the depth camera, point clouds obtained by the depth camera and the laser radar that belong to the same spatial range and point clouds that do not belong to the same spatial range.

[0023] Furthermore, the S3 step includes the following sub-steps:

[0024] S31, based on the field of view of the fusion of the laser radar and each depth camera, and in accordance with the concept of the laser radar scan line and the vertical angular resolution of the laser radar, the laser radar point cloud and each depth camera point cloud are divided into line bundles;

[0025] S32, converting the depth camera point cloud re-divided in step S31 into a lidar format point cloud, and then fusing the lidar point cloud to form a fused point cloud of the same format for output.

[0026] Furthermore, the step S31 includes the following sub-steps:

[0027] S311, based on the point clouds of the depth camera and the lidar point cloud determined in step S24 that belong to the same spatial range, the depth camera point cloud is used to fill the empty points between the lidar beams, and the point cloud belonging to the beam is recalculated;

[0028] S312, according to the point clouds determined in step S24 that the depth camera and lidar point clouds do not belong to the same spatial range, the original bundle definition is retained for the space containing only the lidar point cloud, and the bundle is expanded upward and downward at the lidar angular resolution for the space containing only the depth camera point cloud, and the point cloud around the bundle is identified as the bundle point cloud.

[0029] Furthermore, in the step S311, for the segmentation of the camera point cloud and the lidar point cloud beam within the same spatial range, the camera point cloud within the lidar beam at plus or minus half of the lidar angular resolution is divided into the beam point cloud according to the lidar resolution.

[0030] Furthermore, in the step S312, for the space containing only the depth camera point cloud, the camera point cloud of the expanded beam is divided into the beam point cloud according to the laser radar resolution by the positive and negative half laser radar angular resolution.

[0031] Furthermore, the step S32 includes the following sub-steps:

[0032] S321, after re-dividing the laser radar and camera point clouds into bundles according to step S31, determining and allocating bundle information for each laser point and depth camera point according to the divided bundles;

[0033] S322, adding depth camera point cloud reflection intensity information;

[0034] S323, uses the timestamp of the lidar point cloud release as the timestamp of the fused point cloud.

[0035] Furthermore, in step S322, for the depth camera point cloud within the same spatial range as the lidar point cloud, the reflection intensity is determined according to the reflection intensity of the nearest lidar point of the depth camera; for the depth camera point cloud within a different spatial range from the lidar point cloud, the camera point cloud below the lidar area is uniformly assigned reflection intensity based on the reflection intensity of the lidar illuminating the ground, and the camera point cloud above the lidar area is uniformly assigned reflection intensity information based on the reflection intensity of the lidar illuminating the white wall.

[0036] Furthermore, the S4 step includes the following sub-steps:

[0037] S41, pre-processing the fused point cloud, using inertial navigation to assist in removing nonlinear motion distortion of the fused point cloud, and performing compensation correction on the point cloud in each frame;

[0038] S42, performing feature extraction on the fused point cloud by extracting surface features and line features of the fused point cloud, where surface features include ground features and non-ground features, and filtering non-feature point data;

[0039] S43, the point cloud is matched by the NDT matching method to obtain the motion posture information between the two frames of the quadruped robot point cloud. The initial point cloud is the first frame of the fused point cloud data, until the point cloud key frame accumulates to five frames and then gradually eliminates the previous point cloud frames. When the posture change of the inertial navigation integration exceeds a certain threshold, the point cloud data is identified as the key frame point cloud data; wherein NDT assumes that the point cloud obeys the normal distribution, assuming that the point cloud obtained by the current scan is Use space conversion function to indicate the use of posture Come move The objective is to maximize the likelihood function:

[0040]

[0041] The function is nonlinearly solved to obtain the best matching pose, and the motion state is updated by iterative Kalman filtering. The information of inertial navigation pre-integration and inter-frame matching is integrated, and the zero bias error of the accelerometer and gyroscope in the inertial navigation is estimated in a timely manner.

[0042] S44, updating and saving the point cloud map after each matching until the establishment of the entire environment point cloud map is completed, and performing voxel filtering processing on the finally generated point cloud map.

[0043] Furthermore, the step S5 includes the following sub-steps:

[0044] S51, using the multi-threaded NDT matching method to match the point cloud fused in S2 with the global point cloud map to obtain the global pose of the quadruped robot in the environment;

[0045] S52, obtain the smooth quadruped robot posture based on the inertial navigation integral prediction and the foot odometer prediction, and estimate the true value of the predicted value and the observed value of the state vector according to the Kalman filter formula. The predicted value is the posture of the inertial navigation integral and the posture of the foot odometer, and the observed value is the global posture of the real-time fusion point cloud matching in step S51; after each frame of IMU and foot odometer data is input, the quadruped robot posture is predicted and updated, and the posture of the quadruped robot is corrected using the matching posture of the point cloud as the estimated true value, and finally the posture of the quadruped robot in the converter station is obtained.

[0046] The beneficial effects of the present invention are: 1) the respective characteristics of the lidar and the depth camera are fully utilized to obtain accurate positioning information and adapt to the complex environment of the converter station; 2) compared with the positioning technology of the laser point cloud fusion image, it uses fewer computing resources and the algorithm can be carried on a general computing platform; 3) the global posture is obtained by real-time matching of the lidar point cloud and the three-dimensional environment map, and then the high-frequency quadruped robot posture is obtained by fusing the IMU and the foot odometer, so that the repeated positioning accuracy of the quadruped robot in the converter station is within 6 cm, meeting the inspection requirements. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] Figure 1 This is a simplified structural diagram of the quadruped robot, depth camera, and lidar in Example 1 of the present invention.

[0048] Figure 2 This is a schematic diagram of publishing the fused point cloud using the frequency and timestamp of the lidar point cloud in the first embodiment of the present invention.

[0049] Figure 3 This is the multi-sensor fusion SLAM mapping framework in Example 1 of the present invention.

[0050] Figure 4 This is the cloud map of the converter site in the first embodiment of the present invention.

[0051] Figure 5 This is a block diagram of multi-sensor fusion positioning in Example 1 of the present invention. DETAILED DESCRIPTION

[0052] In order to enable those skilled in the art to better understand the solutions of the present invention, the following will provide a clear and complete description of the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts should fall within the scope of protection of the present invention.

[0053] A mapping and positioning method that integrates laser radar and depth camera point clouds includes the following steps:

[0054] S1, obtaining a laser point cloud and a depth camera point cloud to be processed, removing laser points and depth camera points within an inaccurate measurement range, and removing noise points in the laser point cloud and the depth camera point cloud, wherein the laser point cloud and the depth point cloud include point clouds that belong to the same spatial range and point clouds that do not belong to the same spatial range;

[0055] The above S1 step includes the following sub-steps:

[0056] S11, a laser radar for collecting the initial laser point cloud and a depth camera for collecting the initial depth point cloud are installed on the same device; and a device is provided to synchronize the time of collecting the laser radar point cloud and the time of collecting the depth camera point cloud;

[0057] S12. LiDAR and depth cameras complement each other in terms of depth measurement accuracy. LiDAR has an advantage in measuring the depth of distant objects, while depth cameras have an advantage in measuring the depth of nearby objects. Therefore, based on the characteristics of the depth camera used, inaccurate depth point clouds within the set range of the depth camera are filtered out.

[0058] S13, based on the characteristics of the adopted laser radar, filtering out the laser radar point cloud that is inaccurately measured by the laser radar within a set range, as well as the point cloud that is projected onto the laser radar device;

[0059] S14, filters out lidar and depth camera noise.

[0060] S2, calibrate the external parameters of the lidar and depth camera, determine the coordinate axis relationship between the lidar and depth camera, and convert the depth camera point cloud to the lidar coordinate system;

[0061] The above step S2 includes the following sub-steps:

[0062] S21, calibrate the external parameters between the laser radar and the depth camera, including the rotation matrix R and translation vector t from the depth camera to the laser radar coordinate system; prepare a black and white checkerboard, collect several camera images and laser radar point clouds in which both the depth camera and the laser radar can observe the complete checkerboard, pair them in approximately synchronous time, import them into the Lidar Toolbox toolbox in Matlab, let the toolbox detect the checkerboard in the image, manually specify the point cloud falling on the checkerboard in the laser radar point cloud frame, and let the toolbox perform calibration to obtain the rotation matrix R and translation vector t from the depth camera to the laser radar coordinate system;

[0063] S22, construct a conversion function from the depth camera coordinate system to the lidar coordinate system. The conversion function is,

[0064]

[0065] Among them, X l , Y l , Z l Represents the three coordinate axes of the laser radar coordinate system, X c , Y c , Z c Represents the depth camera coordinate system;

[0066] S23, converting the depth camera point cloud to the lidar coordinate system according to the rotation matrix R and translation vector t of the depth camera to the lidar coordinate system calibrated in step S21 and the conversion function in step S22;

[0067] S24, determining, based on the field of view angles of the laser radar and the depth camera, point clouds obtained by the depth camera and the laser radar that belong to the same spatial range and point clouds that do not belong to the same spatial range.

[0068] S3, based on the field of view angles of the depth camera and the lidar, the lidar and depth camera point clouds in the lidar coordinate system converted in step 2 are re-segmented according to the lidar scan lines, and the depth camera point cloud is converted according to the lidar point cloud format, and fused to form a frame of point cloud with the same format;

[0069] The above S3 step includes the following sub-steps:

[0070] S31, based on the field of view of the fusion of the laser radar and each depth camera, and in accordance with the concept of the laser radar scan line and the vertical angular resolution of the laser radar, the laser radar point cloud and each depth camera point cloud are divided into line bundles;

[0071] Step S31 includes the following sub-steps: S311, based on the point clouds of the depth camera and the lidar point cloud determined in step S24 that belong to the same spatial range, the depth camera point cloud is used to fill the empty points between the lidar beams, and the point cloud belonging to the beam is recalculated;

[0072] In the step S311, for the segmentation of the camera point cloud and the lidar point cloud beam within the same spatial range, the camera point cloud within the lidar beam at plus or minus half of the lidar angular resolution is divided into the beam point cloud according to the lidar resolution;

[0073] S312, based on the point clouds determined in step S24 that the depth camera and lidar point clouds do not belong to the same spatial range, for the space containing only the lidar point cloud, the original bundle definition is retained, and for the space containing only the depth camera point cloud, the bundle is expanded upward and downward at the lidar angular resolution, and the point cloud around the bundle is identified as the bundle point cloud;

[0074] In the step S312, for a space containing only depth camera point clouds, the camera point clouds of the expanded beam are divided into the beam point clouds according to the laser radar resolution by the positive and negative half laser radar angular resolution.

[0075] S32, converting the depth camera point cloud re-divided in step S31 into a lidar format point cloud, and then fusing the lidar point cloud to form a fused point cloud of the same format for output;

[0076] Step S31 includes the following sub-steps: S321, after re-dividing the laser radar and camera point clouds into bundles according to step S31, determining and allocating bundle information of each laser point and depth camera point according to the divided bundles;

[0077] S322, adding depth camera point cloud reflection intensity information;

[0078] In the step S322, for the depth camera point cloud within the same spatial range as the lidar point cloud, the reflection intensity is determined according to the reflection intensity of the nearest lidar point of the depth camera; for the depth camera point cloud within a different spatial range from the lidar point cloud, the camera point cloud below the lidar area is uniformly assigned a reflection intensity based on the reflection intensity of the lidar illuminating the ground, and the camera point cloud above the lidar area is uniformly assigned a reflection intensity based on the reflection intensity of the lidar illuminating the white wall;

[0079] S323, uses the timestamp of the lidar point cloud release as the timestamp of the fused point cloud.

[0080] S4, based on the fusion of the point cloud in S3 and the IMU sensor data, the environment is mapped to obtain a point cloud map of the environment;

[0081] The above-mentioned S4 step includes the following sub-steps:

[0082] S41, pre-process the fused point cloud, use inertial navigation to assist in removing the nonlinear motion distortion of the fused point cloud, and compensate and correct the point cloud in each frame; the fused point cloud is received after being reflected by the reflector. During the time from the emission to the receipt of the point cloud, the quadruped robot has moved a certain distance, and the coordinates of the received points cannot truly reflect the true relative position of the obstacle and the robot. Therefore, it is necessary to use inertial navigation to integrate the posture changes of the quadruped robot during this period of time, and then superimpose the posture of the point on the walking posture of the quadruped robot obtained by inertial navigation integration to finally obtain the fused point cloud with the motion distortion filtered out. Voxel filtering and deviation point filtering are performed on the point cloud after distortion removal to reduce the amount of data in each frame of the point cloud and ensure the accuracy of the point cloud data;

[0083] S42, performing feature extraction on the fused point cloud, by extracting surface features and line features of the fused point cloud, where surface features include ground features and non-ground features, filtering non-feature point data, reducing the amount of data for point cloud frame matching, and accelerating point cloud matching;

[0084] S43, the point cloud is matched by the NDT matching method to obtain the motion posture information between the two frames of the quadruped robot point cloud. The initial point cloud is the first frame of the fused point cloud data, until the point cloud key frame accumulates to five frames and then gradually eliminates the previous point cloud frames. When the posture change of the inertial navigation integration exceeds a certain threshold, the point cloud data is identified as the key frame point cloud data; wherein NDT assumes that the point cloud obeys the normal distribution. In order to find a posture that maximizes the probability that the current frame is located in the matching point cloud, it is assumed that the point cloud obtained by the current scan is Use space conversion function To express the use of pose transformation Come move The objective is to maximize the likelihood function:

[0085]

[0086] The function is nonlinearly solved to obtain the best matching pose, and the motion state is updated by iterative Kalman filtering. The information of the inertial navigation pre-integration is integrated with the information of the inter-frame matching, and the zero bias error of the accelerometer and gyroscope in the inertial navigation is timely estimated. The estimated zero bias error needs to be subtracted before the inertial navigation is integrated, thereby reducing the cumulative error in the inertial navigation integration process.

[0087] S44, updating and saving the point cloud map after each matching until the establishment of the entire environment point cloud map is completed, and performing voxel filtering on the finally generated point cloud map to reduce the data volume of the entire point cloud map;

[0088] S5, based on the point cloud fused in S3, matches the point cloud map established in S4 to obtain the absolute position of the quadruped robot in the environment, and fuses the IMU sensor and foot odometer to obtain the position of the quadruped robot during movement;

[0089] The above step S5 includes the following sub-steps: S51, using a multi-threaded NDT matching method to match the point cloud fused in S2 with the global point cloud map to obtain the global pose of the quadruped robot in the environment;

[0090] S52, obtain the smooth posture of the quadruped robot based on the inertial navigation integral prediction and the foot odometer prediction, and estimate the true value of the predicted value and the observed value of the state vector according to the Kalman filter formula. The predicted value is the posture of the inertial navigation integral and the posture of the foot odometer, and the observed value is the global posture of the real-time fusion point cloud matching in step S51; after each frame of IMU and foot odometer data is input, the posture of the quadruped robot is predicted and updated, and the matching posture of the point cloud is used as the estimated true value to correct the posture of the quadruped robot, and finally obtain the high-frequency and stable posture of the quadruped robot in the converter station.

[0091] Example 1

[0092] like Figure 1 As shown, the quadruped robot is equipped with a RealSense D435i depth camera (30° downward-facing), a RealSense D455 depth camera (10° upward-facing), and a RoboSense RS-LiDAR (16-line laser radar) at the front and rear. All four sensors are connected to the quadruped robot's computing platform. The RealSense D435i depth camera's characteristics allow for accurate depth point cloud measurements between 0.28m and 2m. Therefore, when reading the depth camera's point cloud, inaccurate points outside this range are filtered out. The RoboSense RS-LiDAR (16-line laser radar) achieves high ranging accuracy between 2.0m and 80m. Therefore, when reading the laser radar's point cloud, inaccurate points outside this range are filtered out. This demonstrates that depth cameras and laser radars complement each other in terms of ranging range. The arrangement of the depth camera's mounting angles further enhances the quadruped robot's field of view.

[0093] The coordinate axis relationship between the three depth cameras and the lidar on the quadruped robot is calibrated using the method in step S21, and the three depth camera point clouds are converted to the lidar coordinate system according to step S23. The lidar has a field of view of 30° and a vertical angular resolution of 2°, while the depth camera has a field of view of 58°. Based on the field of view of the lidar and depth camera, as well as their installation angles on the quadruped robot, it is possible to determine which point clouds belong to the same spatial range as the depth camera and lidar, and which do not. The RoboSense RS-LiDAR-16 laser radar beam is arranged with an angular resolution of 2°. For point clouds that belong to the same spatial range as the depth camera and lidar, the depth camera point cloud within ±1° of the lidar beam is classified as the beam point cloud, filling the empty points between the lidar beams. For spaces containing only depth camera point clouds, based on the lidar's 2° vertical angular resolution, the camera point cloud within ±1° of the expanded beam is classified as the new beam point cloud. After redividing the wiring harness, the harness was eventually expanded to 52 wires.

[0094] After dividing the line bundles, the depth camera point cloud needs to be converted into a LiDAR format point cloud. First, the line bundle information is assigned to the depth camera point cloud according to the re-divided line bundles. The reflection intensity information of the depth camera point cloud is added. For the depth camera point cloud within the same spatial range as the LiDAR point cloud, the reflection intensity is determined according to the reflection intensity of the depth camera's nearest LiDAR point. For the depth camera point cloud within a different spatial range from the LiDAR point cloud, the reflection intensity of the camera point cloud below the LiDAR area is uniformly assigned to the depth camera point cloud based on the reflection intensity of the LiDAR illuminating the ground. For the camera point cloud above the LiDAR area, the reflection intensity information is uniformly assigned to the depth camera point cloud based on the reflection intensity of the LiDAR illuminating the white wall.

[0095] Finally, the fused point cloud is published at the frequency and timestamp of the lidar point cloud, such as Figure 2 shown.

[0096] Finally, the fused point cloud is mapped and positioned:

[0097] By integrating LiDAR, a depth camera, and an IMU sensor, we build a mapping framework for an intelligent quadruped robot. The framework primarily includes sensor data reception and preprocessing, point cloud feature extraction, laser odometry, loop detection, and mapping modules. The sensor data reception and preprocessing module is divided into point cloud fusion and point cloud dedistortion between the depth camera and LiDAR, while the laser odometry module is divided into point cloud matching and IEKF data fusion.

[0098] Figure 3 The multi-sensor fusion framework is presented, which is mainly divided into the following five steps:

[0099] 1) Sensor data reception and preprocessing. The point cloud data from the LiDAR and depth camera are integrated and synchronized with the IMU inertial navigation data. Since the points in a point cloud frame are not collected at the same time, the point cloud will be distorted during the movement of the quadruped robot. The relative rotation angle and relative displacement of the quadruped robot are calculated based on the IMU inertial navigation data, and distortion is compensated for for each frame of the point cloud. At the same time, the IMU inertial navigation data is pre-integrated and passed to the IEKF filter as a prediction of the quadruped robot's motion state.

[0100] 2) Feature extraction. Feature extraction of point clouds is mainly for the registration of point cloud frames. By extracting surface features and line features, where surface features include ground features and non-ground features, a large amount of non-feature point data is filtered out, and the amount of data required for point cloud frame matching is reduced, achieving the goal of fast and accurate matching.

[0101] 3) Laser odometry: Frame matching of point clouds is performed using the Normal Distributions Transform (NDT) matching method to obtain the kinematic pose information between two frames of the quadruped robot’s point cloud. After obtaining the kinematic pose information, the IEKF motion state is updated. The odometry information pre-integrated from the IMU is used to predict the quadruped robot’s pose between the two frames, and the IMU’s zero bias error is estimated.

[0102] 4) Loop closure detection: Find candidate closed-loop matching frames in the historical keyframes, perform frame matching with the point cloud map, obtain the pose transformation, and thus optimize the overall point cloud map.

[0103] 5) Mapping. The generated local point cloud map is provided to the laser odometry for frame matching. The final point cloud map is filtered and saved in the format required for global positioning of the quadruped robot.

[0104] Finally, the converter station environment map established by the multi-sensor fusion SLAM mapping framework is as follows: Figure 4 As shown, you can see that the map’s representation of the environment is very accurate.

[0105] Multi-sensor fusion positioning framework and implementation

[0106] After establishing a global map of the converter station, in order to realize the intelligent inspection of the intelligent quadruped robot in the converter station, it is necessary to obtain the precise position and posture of the quadruped robot in the converter station in real time. This paper obtains the global position and posture by matching the fusion point cloud of the lidar and depth camera with the three-dimensional environment point cloud map of the converter station, and then obtains the high-frequency position and posture of the quadruped robot by fusing the IMU and foot odometer through UKF. The specific framework is as follows Figure 5 As shown:

[0107] The multi-sensor fusion positioning framework of the intelligent quadruped robot is mainly divided into two steps: point cloud matching pose and UKF data fusion.

[0108] 1) Point cloud matching pose. First, the lidar and camera point clouds are fused, and distortion compensation is performed on the fused point cloud according to the method in the mapping framework. Then, a multi-threaded NDT matching method is used to match the distortion-compensated fused point cloud with the global point cloud map to obtain the real-time pose of the quadruped robot in the environment.

[0109] 2) UKF data fusion. Generate 2n+1 sigma points near the state vector. Use the state transfer matrix to predict the sigma points of the observations, and calculate the mean and covariance matrix of the state vector based on the weights. Use the measurement matrix to obtain the predicted measurement value of the sigma point. Obtain the predicted value of the state vector based on the sigma point and the weight. According to the Kalman filter (KF) formula, the predicted value and observed value of the state vector are estimated as the true value. After each frame of IMU and foot odometer data is input, the quadruped robot's posture is predicted and updated, and then the matching posture of the point cloud is used as the estimated true value to correct the posture of the quadruped robot, and finally a high-frequency stable posture of the quadruped robot in the converter station is obtained.

[0110] The above specific embodiments are used to illustrate the present invention rather than to limit the present invention. Any modifications and changes made to the present invention within the spirit of the present invention and the protection scope of the claims shall fall within the protection scope of the present invention.

Claims

1. A mapping and positioning method that integrates laser radar and depth camera point clouds, characterized in that: The following steps are involved: S1, obtaining the laser point cloud and depth camera point cloud to be processed, including point clouds belonging to the same spatial range and those not belonging to the same spatial range; S2, calibrate the external parameters of the lidar and depth camera, determine the coordinate axis relationship between the lidar and depth camera, and convert the depth camera point cloud to the lidar coordinate system; S3, based on the field of view of the depth camera and the lidar, re-segment the lidar and depth camera point clouds in the converted lidar coordinate system according to the lidar scan lines, and convert the depth camera point cloud according to the lidar point cloud format, fusing them into a frame of point cloud with the same format; S4, based on the fusion of the point cloud in S3 and the IMU sensor data, the environment is mapped to obtain a point cloud map of the environment; S5, based on the point cloud fused in S3, matches the point cloud map established in S4 to obtain the absolute position of the quadruped robot in the environment, and fuses the IMU sensor and foot odometer to obtain the position of the quadruped robot during movement; The S2 step includes the following sub-steps: S21, calibrate the external parameters between the laser radar and the depth camera, including the rotation matrix R and translation vector t from the depth camera to the laser radar coordinate system; prepare a black and white checkerboard, collect several camera images and laser radar point clouds in which both the depth camera and the laser radar can observe the complete checkerboard, pair them in approximately synchronous time, import them into the Lidar Toolbox toolbox in Matlab, let the toolbox detect the checkerboard in the image, manually specify the point cloud falling on the checkerboard in the laser radar point cloud frame, and let the toolbox perform calibration to obtain the rotation matrix R and translation vector t from the depth camera to the laser radar coordinate system; S22, construct a conversion function from the depth camera coordinate system to the lidar coordinate system. The conversion function is, Among them, X l , Y l , Z l Represents the three coordinate axes of the laser radar coordinate system, X c , Y c , Z c Represents the depth camera coordinate system; S23, converting the depth camera point cloud to the lidar coordinate system according to the rotation matrix R and translation vector t of the depth camera to the lidar coordinate system calibrated in step S21 and the conversion function in step S22; S24, determining, based on the field of view angles of the laser radar and the depth camera, point clouds that the depth camera and the laser radar obtain and that do not belong to the same spatial range; The S3 step includes the following sub-steps: S31, based on the field of view of the fusion of the laser radar and each depth camera, and in accordance with the concept of the laser radar scan line and the vertical angular resolution of the laser radar, the laser radar point cloud and each depth camera point cloud are divided into line bundles; S32, converting the depth camera point cloud re-divided in step S31 into a lidar format point cloud, and then fusing the lidar point cloud to form a fused point cloud of the same format for output; The step S31 includes the following sub-steps: S311, based on the point clouds of the depth camera and the lidar point cloud determined in step S24 that belong to the same spatial range, the depth camera point cloud is used to fill the empty points between the lidar beams, and the point cloud belonging to the beam is recalculated; S312, according to the point clouds determined in step S24 that the depth camera and lidar point clouds do not belong to the same spatial range, the original bundle definition is retained for the space containing only the lidar point cloud, and the bundle is expanded upward and downward at the lidar angular resolution for the space containing only the depth camera point cloud, and the point cloud around the bundle is identified as the bundle point cloud.

2. The method for mapping and positioning by fusing laser radar and depth camera point clouds according to claim 1, characterized in that: The S1 step includes the following sub-steps: S11, setting a device for synchronizing the time of collecting the laser radar point cloud and collecting the depth camera point cloud; S12, filtering out inaccurate depth point clouds measured within the depth camera setting range; S13, filtering out the LiDAR point clouds that are inaccurately measured by the LiDAR within the set range, as well as the point clouds that are projected onto the LiDAR device; S14, filters out lidar and depth camera noise.

3. The method for mapping and positioning by fusing laser radar and depth camera point clouds according to claim 1, characterized in that: In the step S311, for the segmentation of the camera point cloud and the lidar point cloud beam within the same spatial range, the camera point cloud within the lidar beam at plus or minus half of the lidar angular resolution is divided into the beam point cloud according to the lidar resolution.

4. The method for mapping and positioning by fusing laser radar and depth camera point clouds according to claim 1, characterized in that: In the step S312, for a space containing only depth camera point clouds, the camera point clouds of the expanded beam are divided into the beam point clouds according to the laser radar resolution by the positive and negative half laser radar angular resolution.

5. The method for mapping and positioning by fusing laser radar and depth camera point clouds according to claim 1, characterized in that: The step S32 includes the following sub-steps: S321, after re-dividing the laser radar and camera point clouds into bundles according to step S31, determining and allocating bundle information for each laser point and depth camera point according to the divided bundles; S322, adding depth camera point cloud reflection intensity information; S323, uses the timestamp of the lidar point cloud release as the timestamp of the fused point cloud.

6. The method for mapping and positioning by fusing laser radar and depth camera point clouds according to claim 5, characterized in that: In the step S322, for the depth camera point cloud within the same spatial range as the lidar point cloud, the reflection intensity is determined according to the reflection intensity of the nearest lidar point of the depth camera; for the depth camera point cloud within a different spatial range from the lidar point cloud, the camera point cloud below the lidar area is uniformly assigned reflection intensity based on the reflection intensity of the lidar illuminating the ground, and the camera point cloud above the lidar area is uniformly assigned reflection intensity information based on the reflection intensity of the lidar illuminating the white wall.

7. The method for mapping and positioning by fusing laser radar and depth camera point clouds according to claim 1, characterized in that: The S4 step includes the following sub-steps: S41, pre-processing the fused point cloud, using inertial navigation to assist in removing nonlinear motion distortion of the fused point cloud, and performing compensation correction on the point cloud in each frame; S42, performing feature extraction on the fused point cloud by extracting surface features and line features of the fused point cloud, where surface features include ground features and non-ground features, and filtering non-feature point data; S43, the point cloud is matched by the NDT matching method to obtain the motion posture information between the two frames of the quadruped robot point cloud. The initial point cloud is the first frame of the fused point cloud data, until the point cloud key frame accumulates to five frames and then gradually eliminates the previous point cloud frames. When the posture change of the inertial navigation integration exceeds a certain threshold, the point cloud data is identified as the key frame point cloud data; wherein NDT assumes that the point cloud obeys the normal distribution, assuming that the point cloud obtained by the current scan is Use space conversion function To express the use of pose transformation Come move The objective is to maximize the likelihood function: The function is nonlinearly solved to obtain the best matching pose, and the motion state is updated by iterative Kalman filtering. The information of inertial navigation pre-integration and inter-frame matching is integrated, and the zero bias error of the accelerometer and gyroscope in the inertial navigation is estimated in a timely manner. S44, updating and saving the point cloud map after each matching until the establishment of the entire environment point cloud map is completed, and performing voxel filtering processing on the finally generated point cloud map.

8. The method for mapping and positioning by fusing laser radar and depth camera point clouds according to claim 7, characterized in that: The S5 step includes the following sub-steps: S51, using the multi-threaded NDT matching method to match the point cloud fused in S2 with the global point cloud map to obtain the global pose of the quadruped robot in the environment; S52, obtain the smooth quadruped robot posture based on the inertial navigation integral prediction and the foot odometer prediction, and estimate the true value of the predicted value and the observed value of the state vector according to the Kalman filter formula. The predicted value is the posture of the inertial navigation integral and the posture of the foot odometer, and the observed value is the global posture of the real-time fusion point cloud matching in step S51; after each frame of IMU and foot odometer data is input, the quadruped robot posture is predicted and updated, and the posture of the quadruped robot is corrected using the matching posture of the point cloud as the estimated true value, and finally the posture of the quadruped robot in the converter station is obtained.

Images