A large scene repositioning method based on laser vision fusion data

Abstract

This invention relates to the field of LiDAR positioning technology and provides a large-scene relocalization method based on laser vision fusion data, comprising the following steps: Step 1: Create a laser point cloud map; use vehicle-mounted LiDAR to map the large-scene environment and generate a laser point cloud map; Step 2: Obtain initial pose estimation values; based on the laser point cloud map created in Step 1, use the currently acquired point cloud frame to match and optimize the feature points in the map, and finally calculate the current pose of the vehicle; Step 3: Obtain pose by matching visual data; firstly, extract feature points from the image information and calculate descriptors; match the existing feature vectors of the current image to calculate the relative pose transformation between the image and the map, and optimize the pose to obtain a more accurate pose estimation value; Step 4: Obtain the relocalization pose.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of laser radar positioning, and particularly relates to a large-scene repositioning method based on laser vision fusion data. BACKGROUND

[0002] With the rapid development of artificial intelligence and big data technology, automatic driving technology has developed rapidly under the promotion of computer and sensor related technologies. The concept of simultaneous localization and mapping (SLAM) was first proposed in the 1980s, originating from the field of robotics. SLAM mainly solves the problem of how to position a moving object carrying a sensor during movement while mapping the surrounding environment.

[0003] Although existing SLAM technology has achieved many results, for example, the visual and laser SLAM frameworks such as LeGO-LOAM, LIO-SAM, ORB-SLAM2, and VINS-Mono have excellent performance in static environments. Currently, in the fields of mobile robots and autonomous driving, laser SLAM technology and visual SLAM technology are widely used. However, in large scenes, using a single SLAM technology for positioning and mapping will have many problems. For example, laser SLAM needs to rely on map matching for repositioning, but due to the variability and unpredictability of large scenes, the matching effect of the map will be affected. Visual SLAM is also affected by factors such as light changes and occlusions, resulting in a decrease in positioning accuracy.

[0004] Therefore, a method and device are needed that can overcome the above problems and achieve high-precision positioning and mapping. SUMMARY

[0005] The purpose of the present application is to provide a large-scene repositioning method based on laser vision fusion data, which aims to collect scene data using laser sensors and camera sensors and use laser SLAM technology and visual SLAM technology for mapping and positioning.

[0006] The present application is implemented as follows: a large-scene repositioning method based on laser vision fusion data, comprising the following steps:

[0007] Step 1: building a repositioning point cloud map using a laser radar and a camera;

[0008] Step 2: using laser data for preliminary positioning to obtain an initial pose estimate;

[0009] Step 3: using visual data to obtain image features of the current scene and matching the image features with the previously established map to obtain a matched pose;

[0010] Step 4: Use the initial pose estimate and the matched pose to optimize the final relocalization pose.

[0011] Further technical solutions, the specific steps of step 1 include:

[0012] 1.1 Use laser radar to collect point cloud data in the required large scene environment.

[0013] 1.2 Preprocess the collected point cloud data, including denoising, filtering, clustering and other operations;

[0014] 1.3. Establish an initial point cloud map by point cloud registration algorithm for the processed point cloud data;

[0015] 1.4. With the movement of the vehicle or robot, continuously collect new point cloud data and add it to the existing point cloud map through the point cloud registration algorithm;

[0016] 1.5 Fuse and update the new point cloud data with the existing point cloud map to ensure the real-time and accuracy of the point cloud map;

[0017] 1.6 Set key frames in the point cloud map to facilitate subsequent relocalization operations;

[0018] 1.7 Save the updated point cloud map for subsequent relocalization operations.

[0019] Further technical solutions, the specific steps of step 2 include:

[0020] 2.1 First, denoise and filter the laser data to reduce data noise and errors;

[0021] 2.2 Extract feature points from the processed laser data, common feature point extraction methods include classifying point cloud data by calculating the curvature of nearby points, into corner points and face points;

[0022] 2.3 Match the feature points extracted from the current frame with the feature points in the map to get a preliminary pose estimate;

[0023] 2.4 Estimate the relative motion between the current frame and the previous frame using laser data and pose estimate to further optimize the pose estimate;

[0024] 2.5 Use optimization methods (such as nonlinear optimization) to optimize the pose estimate to get a more accurate pose estimate.

[0025] Further technical solutions, the specific steps of step 2.2 include:

[0026] 2.2.1 First, voxel grid processing is performed on the laser point cloud data;

[0027] 2.2.2 According to the curvature calculation formula, select the left and right ten points, and calculate the curvature information of the point cloud in each voxel.

[0028] 2.2.3 For each point, according to the size of the curvature in the voxel it is located in and the direction of the normal vector, determine whether the point is a curvature feature point.

[0029] Further technical solutions, the specific steps of the step 3 include:

[0030] 3.1 Use the feature point detection algorithm ORB to extract feature points with good repeatability and distinguishability from the image;

[0031] 3.2 For the extracted feature points, convert them into feature vectors that can be used for matching by using the feature description algorithm ORB descriptor;

[0032] 3.3 Match the feature vectors of the current image with the existing feature vectors in the map to find the feature points in the map that are most similar to the current image;

[0033] 3.4 Calculate the relative pose transformation between the current image and the map through the matched feature points;

[0034] 3.5 Optimize the matched pose with the previous pose to obtain a more accurate pose estimation value by using a graph optimization algorithm;

[0035] 3.6 Perform error judgment and optimization on the optimized pose estimation value to determine whether the matching result is reliable, and if it is reliable, update the current pose state; if it is not reliable, re-match or use other sensor information to assist positioning.

[0036] Further technical solutions, the specific steps of the step 3.3 include:

[0037] 3.3.1 Perform feature point extraction and matching on the image of the current frame to obtain the feature points in the current frame and their corresponding points in the map;

[0038] 3.3.2 Estimate the initial pose of the current frame relative to the map according to the matched feature points, and use the RANSAC algorithm for pose estimation;

[0039] 3.3.3 Use the ICP algorithm to further optimize the accuracy of the relative pose to obtain a more accurate relative pose transformation;

[0040] 3.3.4 Combine the obtained relative pose transformation with the absolute pose in the map to obtain the absolute pose of the current frame.

[0041] Further technical solutions, the specific steps of the step 4 include:

[0042] 4.1 Using the initial pose estimate and the matched pose as variables, jointly optimize them using the constraints;

[0043] 4.2 Restrict the variables using the constraints in the relocalization pose optimization problem, including the matching error of lidar data and image data, the smoothness of pose change, etc.;

[0044] 4.3 Bring the above constraints into the optimization problem and solve it using an optimization algorithm to obtain the optimal relocalization pose;

[0045] 4.4 Apply the new relocalization pose to the map and update the point cloud data and image data in the map to keep the map consistent with the environment;

[0046] 4.5 According to the error size of the relocalization pose and the optimization convergence, judge whether the relocalization is successful. If the relocalization fails, the pose estimation and matching need to be performed again;

[0047] Further technical solutions, the specific steps of step 4.3 include:

[0048] 4.3.1 Match the feature points in the point cloud map with the feature points in the current image to obtain the matched point pairs and their corresponding feature descriptors;

[0049] 4.3.2 Through these matched point pairs, the constraint conditions between the initial pose estimate and the matched pose can be constructed and brought into the optimization problem;

[0050] 4.3.3 Project the points in the point cloud map to the current camera coordinate system to obtain the corresponding two-dimensional pixel coordinates. Match these projected points with the feature points in the current image to obtain the matched point pairs and calculate their corresponding relative pose transformations. Bring the relative pose transformations of these matched point pairs into the optimization problem as the constraint conditions between the camera pose and the point cloud map;

[0051] 4.3.4 Bring the above constraints into the optimization function and use an optimization algorithm to solve the optimal relocalization pose.

[0052] Compared with the prior art, the beneficial effects of the present application are as follows:

[0053] The method provided by the present application is based on laser vision fusion data and uses the initial pose estimate and the matched pose for optimization to obtain the final relocalization pose. The new point cloud data is fused and updated with the existing point cloud map to ensure the real-time performance and accuracy of the point cloud map. Key frames are set in the point cloud map to facilitate subsequent relocalization operations. The updated point cloud map is saved for subsequent relocalization operations.

[0054] The application provides a large scene repositioning method based on laser vision fusion data, relative motion between a current frame and a previous frame is estimated by using laser data and a pose estimation value, and the pose estimation value is further optimized; the pose estimation value is optimized by using an optimization method (such as a nonlinear optimization), and a more accurate pose estimation value is obtained;

[0055] The application provides a large scene repositioning method based on laser vision fusion data, according to matched feature points, an initial pose of a current frame relative to a map is estimated, a RANSAC algorithm is used for pose estimation, and an ICP algorithm is used to further optimize the accuracy of the relative pose, and a more accurate relative pose transformation is obtained; the ICP iterates two sets of point clouds given and obtains an optimal rigid transformation matrix. BRIEF DESCRIPTION OF DRAWINGS

[0056] Fig. 1 A flowchart of the large scene repositioning method based on laser vision fusion data is provided for the application.

[0057] Fig. 2 A structural framework diagram of the large scene repositioning method based on laser vision fusion data is provided for the application. DETAILED DESCRIPTION

[0058] In order to make the purpose, technical scheme and advantages of the application more clear, the application is further described in detail below in combination with the drawings and examples. It should be understood that the specific examples described herein are only used to explain the application and do not limit the application.

[0059] The large scene repositioning method based on laser vision fusion data is described in detail below in combination with specific examples.

[0060] As shown in the method provided by the application, the method comprises: Figs. 1-2

[0061] Step 1, a laser radar and a camera are used to build a repositioning point cloud map.

[0062] The specific implementation steps are as follows:

[0063] 1.1 Start the vehicle-mounted laser radar, drive in the target scene at a stable speed, and collect point cloud data in the large scene environment required to be established;

[0064] 1.2 Perform denoising, filtering, point cloud clustering and other operations on the collected point cloud data, and this operation is referred to as point cloud preprocessing;

[0065] 1.3. The processed point cloud data is used to build an initial point cloud map by a point cloud registration algorithm; ​

[0066] 1.4. As the vehicle moves, new environmental point cloud data is continuously acquired and added to the existing point cloud map through a point cloud registration algorithm;

[0067] 1.5. The new point cloud data is fused and updated with the existing point cloud map to ensure the real-time and accuracy of the point cloud map;

[0068] 1.6. Key frames are set in the point cloud map to facilitate subsequent repositioning operations;

[0069] 1.7. The updated point cloud map is saved (.pcd format) for subsequent repositioning operations. Step 2: Use laser data to perform preliminary positioning and obtain initial pose estimation;

[0070] 2.1. First, the laser data needs to be denoised and filtered to reduce data noise and errors;

[0071] 2.2. Feature points are extracted from the processed laser data. Common feature point extraction methods include classifying point cloud data by calculating the curvature of points near the point cloud, into corner points and plane points;

[0072] 2.3. Match the feature points extracted from the current frame with the feature points in the map to obtain a preliminary pose estimation;

[0073] 2.4. Use laser data and pose estimation to estimate the relative motion between the current frame and the previous frame to further optimize the pose estimation;

[0074] 2.5. Use optimization methods (such as nonlinear optimization) to optimize the pose estimation to obtain more accurate pose estimation.

[0075] Further technical solutions, the specific steps of step 2.2 include:

[0076] 2.2.1. First, perform voxel grid processing on the laser point cloud data;

[0077] 2.2.2. According to the curvature calculation formula:

[0078]

[0079] Select ten points on the left and right, calculate the curvature information of the point cloud in each voxel. According to the size of the curvature, it can be divided into corner points and plane points. Store in the corresponding container to facilitate subsequent point-line registration and point-plane registration operations;

[0080] 2.2.3. For each point, according to the size of the curvature and the direction of the normal vector in the voxel it is located in, determine whether the point is a curvature feature point.

[0081] Step 3: Obtain image features of the current scene using visual data, and match the image features with the previously established map to obtain the matched pose;

[0082] 3.1 Use the feature point detection algorithm ORB to detect the corner points in the image using the FAST algorithm, and calculate the scale and direction of each corner point. Extract feature points with good repeatability and distinguishability from the image;

[0083] 3.2 For the extracted feature points, convert them into feature vectors that can be used for matching by using the feature description algorithm ORB descriptor;

[0084] 3.3 Match the feature vectors of the current image with the existing feature vectors in the map to find the most similar feature points in the map to the current image;

[0085] 3.4 Calculate the relative pose transformation between the current image and the map through the matched feature points;

[0086] 3.5 Optimize the matched pose with the previous pose to obtain a more accurate pose estimate by using a graph optimization algorithm;

[0087] 3.6 Perform error judgment and optimization on the optimized pose estimate to determine whether the matching result is reliable. If it is reliable, update the current pose state; if it is not reliable, re-match or use other sensor information to assist in positioning.

[0088] Further technical solutions, the specific steps of step 3.3 include:

[0089] 3.3.1 Extract and match feature points from the current frame image to obtain the feature points in the current frame and their corresponding points in the map;

[0090] 3.3.2 Estimate the initial pose of the current frame relative to the map according to the matched feature points, and use the RANSAC algorithm for pose estimation;

[0091] 3.3.3 Use the lCP algorithm to further optimize the accuracy of the relative pose to obtain a more accurate relative pose transformation. The lCP iterates on the given two sets of point clouds and obtains the optimal rigid transformation matrix. First, given two sets of matched point clouds P and P1, the centroids p and p' are calculated respectively, and the error term is constructed:

[0092] e i =p i -(Rp i ′+t)

[0093] where p i and p′ iThe point clouds in P and P1 respectively. According to the least square method, the following results are obtained:

[0094]

[0095] The left term is only related to the rotation matrix R, and the right term is related to both R and t. According to the obtained results, the relative rigid transformation T is solved in two steps.

[0096] 3.3.4 The obtained relative pose transformation is combined with the absolute pose in the map, and the absolute pose of the current frame is obtained.

[0097] Step 4: Use the initial pose estimate and the matched pose to optimize and obtain the final relocalization pose.

[0098] 4.1 Use the initial pose estimate and the matched pose as variables, and use the constraint conditions to jointly optimize them. The initial pose can be manually specified in the point cloud map using the "2D Pose Estimate" in the Rviz interface toolbar.

[0099] 4.2 Use the constraint conditions in the relocalization pose optimization problem to limit the variables. These constraint conditions include the matching error of lidar data and image data, the smoothness of pose change, etc.

[0100] 4.3 Bring the constraint conditions into the optimization problem and use the optimization algorithm to solve the optimal relocalization pose.

[0101] 4.4 Apply the new relocalization pose to the map and update the point cloud data and image data in the map to keep the map consistent with the environment.

[0102] 4.5 According to the error size of the relocalization pose and the optimization convergence condition, judge whether the relocalization pose error is less than the given threshold. If it is less than the threshold, it is considered that the relocalization is successful, and the relocalization is ended. If the relocalization pose error is greater than or equal to the threshold, or the optimization convergence condition is not good, the pose estimation and matching need to be performed again. If the pose estimation needs to be performed again, the initial pose estimate is obtained using the laser data, and the image features of the current scene are obtained using the visual data, and matched with the previously established map to obtain the current matched pose.

[0103] Further technical solutions, the specific steps of step 4.3 include:

[0104] 4.3.1 Match the feature points in the point cloud map with the feature points in the current image to obtain the matched point pairs and their corresponding feature descriptors.

[0105] 4.3.2 With these matched point pairs, the constraints between the initial pose estimate and the matched poses can be constructed and brought into the optimization problem;

[0106] 4.3.3 Project the points in the point cloud map to the current camera coordinate system to get the corresponding 2D pixel coordinates. Match these projected points with the feature points in the current image to get matched point pairs and compute their corresponding relative pose transformations. Bring these relative pose transformations of the matched point pairs into the optimization problem as constraints between the camera pose and the point cloud map;

[0107] 4.3.4 Bring the above constraints into the optimization function and solve the optimal relocalization pose using an optimization algorithm.

[0108] The above only describes the preferred embodiments of the present application and is not intended to limit the present application. Any modification, equivalent replacement and improvement made within the spirit and principle of the present application shall be included in the protection scope of the present application.

[0109] In addition, it should be understood that although the present specification is described in terms of embodiments, not every embodiment contains only one independent technical solution, and the description manner of the specification is only for the sake of clarity, and the skilled in the art should consider the specification as a whole, and the technical solutions in each embodiment can be properly combined to form other embodiments which can be understood by the skilled in the art.

Claims

1. A large-scene relocalization method based on laser vision fusion data, characterized in that, Includes the following steps: Step 1: Create a laser point cloud map; Using vehicle-mounted LiDAR to create a map of a large scene environment and generate a LiDAR point cloud map; Step 2: Obtain the initial pose estimate; Based on the laser point cloud map created in step 1, the currently acquired point cloud frame is matched with the feature points in the map and optimized to finally calculate the current pose of the vehicle. Step 3: Obtain pose by matching visual data; First, feature points are extracted from the image information, and descriptors are calculated. Then, based on the existing feature vectors of the current image, matching is performed to calculate the relative pose transformation between the image and the map, and the pose is optimized to obtain a more accurate pose estimate. Step 4: Obtain the repositioning pose; Step 4 specifically includes the following steps: Step 4.1: Using the initial pose estimate and the matched pose as variables, optimize them jointly using the constraints between the initial pose estimate and the matched pose; Step 4.2: Apply constraints to the variables using the constraints in the repositioning pose optimization problem; Step 4.3: Substitute the constraints from the above repositioning pose optimization problem into the optimization problem, and use the optimization algorithm to solve for the optimal repositioning pose; Step 4.4: Apply the new repositioning pose to the map, update the point cloud data and image data in the map, and make the map consistent with the environment; Step 4.5: Determine whether the repositioning was successful based on the magnitude of the repositioning pose error and the optimization convergence. If the repositioning fails, pose estimation and matching need to be performed again. In step 4.3, the adaptive MPC lateral controller is designed, which specifically includes the following steps: Step 4.3.1: Match the feature points in the point cloud map with the feature points in the current image to obtain matching point pairs and their corresponding feature descriptors; Step 4.3.2: Through these matching point pairs, constraints between the initial pose estimate and the matched pose can be constructed and substituted into the optimization problem; Step 4.3.3: Project the points in the point cloud map onto the current camera coordinate system to obtain the corresponding two-dimensional pixel coordinates; match these projected points with the feature points in the current image to obtain matching point pairs and calculate their corresponding relative pose transformations; The relative pose transformations of these matching point pairs are incorporated into the optimization problem as constraints between the camera pose and the point cloud map. Step 4.3.4: Substitute the above constraints into the optimization function and use the optimization algorithm to solve for the optimal relocation pose.

2. The large-scene relocalization method based on laser vision fusion data according to claim 1, characterized in that: Step 1 specifically includes the following steps: Step 1.1: Use LiDAR to collect point cloud data in the large-scale scene environment to be established; Step 1.2: Preprocess the collected point cloud data, including denoising, filtering, and clustering; Step 1.3: Use the processed point cloud data to establish an initial point cloud map using a point cloud registration algorithm; Step 1.4: As the vehicle or robot moves, continuously collect new point cloud data and add it to the existing point cloud map using a point cloud registration algorithm; Step 1.5: Integrate and update the new point cloud data with the existing point cloud map; Step 1.6: Set keyframes in the point cloud map; Step 1.7: Save the updated point cloud map.

3. The large-scene relocalization method based on laser vision fusion data according to claim 1, characterized in that: Step 2 specifically includes the following steps: Step 2.1: First, denoise and filter the laser data; Step 2.2: Extract feature points from the processed laser data; Step 2.3: Match the feature points extracted in the current frame with the feature points in the map to obtain a preliminary pose estimate; Step 2.4: Using laser data and pose estimation values, estimate the relative motion between the current frame and the previous frame to further optimize the pose estimation values; Step 2.5: Optimize the pose estimate using optimization methods.

4. The large-scene relocalization method based on laser vision fusion data according to claim 3, characterized in that: Step 2.2: Extract feature points from the processed laser data, specifically including the following steps: Step 2.2.1: First, perform voxel meshing on the laser point cloud data; Step 2.2.2: Based on the curvature calculation formula, select multiple points on the left and right sides, and calculate the curvature information of the point cloud in each voxel; Step 2.2.3: For each point, determine whether it is a curvature feature point based on the magnitude of the curvature in the voxel it belongs to and the direction of the normal vector.

5. The large-scene relocalization method based on laser vision fusion data according to claim 1, characterized in that: Step 3 specifically includes the following steps: Step 3.1: Use the ORB feature point detection algorithm to extract repeatable and distinguishable feature points from the image; Step 3.2: For the extracted feature points, use the ORB descriptor algorithm to transform them into feature vectors that can be used for matching; Step 3.3: Match the feature vector of the current image with the existing feature vectors in the map to find the feature point in the map that is most similar to the current image; Step 3.4: Calculate the relative pose transformation between the current image and the map using the matched feature points; Step 3.5: Optimize the matched pose with the previous pose by using a graph optimization algorithm to obtain a more accurate pose estimate. Step 3.6: Perform error judgment and optimization on the optimized pose estimation value, and determine whether the matching result is reliable. If it is reliable, update the current pose state. If unreliable, re-matching or using other sensor information is required to assist in positioning.

Images