Calibration method and system between 2D LiDAR and 3D TOF camera

Abstract

This invention discloses a calibration method between a 2D LiDAR and a 3D TOF camera, comprising the following steps: S1, controlling a laser forklift to move and changing its current pose; S2, calculating the poses of the 2D LiDAR and the 3D TOF camera, and storing them respectively in a pose set pose_list. 2d pose list 3d In S3, after the pose data in pose_list2d and pose_list3d reaches the quantity threshold, the pose data is processed through pose_list... 2d and pose_list 3d Each pair of pose_list 2d (x 2d ,y 2d ,θ 2d ) and pose_list 3d (x 3d ,y 3d ,θ 3d A nonlinear least squares problem is constructed to calculate the pose transformation tf(tx,ty,angle) of the 3D TOF camera relative to the 2D LiDAR. This achieves high-precision calibration between the 2D LiDAR on a 2D laser forklift and the 3D TOF camera on a pallet, without relying on additional equipment and solely on the natural environment. Furthermore, the calibration does not require the laser forklift to perform specific actions, thus minimizing the requirements on the mobile robot. The improved positioning accuracy, achieved through 2D repeated scanning of a high-precision map and 3D point-to-surface matching, leads to improved calibration accuracy.

Description

Technical Field

[0001] This invention belongs to the field of robot positioning technology, and more specifically, this invention relates to a calibration method and system between a 2D laser sensor and a 3D TOF camera. Background Technology

[0002] With economic development and technological advancements, mobile robots are increasingly involved in manufacturing processes to improve production efficiency and reduce labor costs. Automated forklifts primarily handle material handling and transportation, and positioning and navigation technology is fundamental for these tasks. Currently, most indoor AGVs use 2D LiDAR for positioning and navigation. However, 2D LiDAR sensors can only calculate the forklift's pose on a global map. When automatically picking up pallets, the pallets need to be placed precisely in fixed positions, significantly hindering the forklift's flexibility. To address this, a pallet recognition system needs to be installed above the forklift's picks. This system typically uses a 3D Time-of-Flight (TOF) camera as a sensor. The system processes the point cloud data from the 3D TOF camera using algorithms to calculate the pallet's pose within the camera's image, assisting the forklift in picking up inaccurately placed pallets. Since the 3D TOF camera and 2D LiDAR are not in the same coordinate system, they cannot be used directly; the relative pose relationship between the two sensors needs to be calibrated beforehand.

[0003] Patent Publication No.: CN 114578324 A, Patent Title: Joint Calibration Method and System for Single-Line and Multi-Line LiDAR Based on Straight-Line Features, calibrating 2D point cloud sensors and 3D point cloud sensors. The multi-line LiDAR point cloud is cropped and projected onto the XOY plane to obtain two straight lines. The 2D LiDAR is then cropped, also yielding two straight lines. The two straight lines scanned by different LiDARs represent the scanning results of different LiDARs on the same two planes. The relative pose relationship between the two sensors is then obtained through point cloud matching. This method has the following problems:

[0004] 1) This solution has requirements for the scanning angle and scanning distance of 3D radar, and is not suitable for 3D TOF point cloud data;

[0005] 2) During calibration, this scheme directly uses the projected straight line point cloud for registration calculation without multiple measurements and optimizations under different conditions, which easily leads to getting trapped in local optima and resulting in low calibration accuracy. Summary of the Invention

[0006] This invention provides a calibration method between a 2D lidar and a 3D TOF camera, aiming to improve the calibration accuracy between the two.

[0007] This invention is implemented as follows: a calibration method between a 2D lidar and a 3D TOF camera, the method comprising the following steps:

[0008] S1. Control the laser forklift to move and change its current position.

[0009] S2. Calculate the poses of the 2D LiDAR and the 3D TOF camera, and store them in the pose set pose_list respectively. 2d pose list 3d middle;

[0010] S3, Detect pose set pose_list 2d pose list 3d If the number of poses in the data is greater than the number threshold, if the detection result is no, then proceed to step S1; if the detection result is yes, then proceed to step S4.

[0011] S4. Using the pose list 2d and pose_list 3d Each pair of poses (x) 2d ,y 2d ,θ 2d ) and (x 3d ,y 3d ,θ 3d A nonlinear least squares problem is constructed to calculate the pose transformation tf(tx,ty,angle) of the 3d TOF camera relative to the 2d LiDAR.

[0012] Furthermore, before obtaining the pose of the 2D LiDAR, the following steps are also included:

[0013] Construct a 2D laser point cloud map (map_points), and calculate the pose of the 2D LiDAR based on the 2D laser point cloud map (map_points).

[0014] 3. The calibration method between a 2D LiDAR and a 3D TOF camera as described in claim 1, characterized in that the method for constructing the 2D LiDAR point cloud map (map_points) specifically includes the following steps:

[0015] (1) Stop the laser forklift in the middle of the panel surrounding area, start the 2D laser radar and start scanning. The point cloud data scanned in each frame is curr_points;

[0016] (2) Arrange the scanned point cloud curr_points into the container list_points according to the radar scan angle;

[0017] (3) Determine whether the current number of scans meets the number threshold. If not, return to step (2). If it does, execute step (4).

[0018] (4) Calculate the scan angle θ for each of the container list_points i The standard deviation σ of the distance measurement of the effective points in the point set i If the current scanning angle θ i The number of valid points in the point set is greater than the quantity threshold, and σ i If the maximum distance deviation is less than the maximum allowable distance measurement deviation, then proceed to step (5).

[0019] (5) Calculate the coordinates of the valid points in the 2D laser point cloud map map_points. and the calculated points Store it in the high-precision map map_points.

[0020] Furthermore, the 2D LiDAR localization method based on 2D LiDAR point cloud map (map_points) specifically includes the following steps:

[0021] Scan the current environment to obtain the current frame point cloud data point_cloud, take the pose of the previous moment as the current initial pose pose(x,y,θ), and calculate the precise pose of the laser forklift in the current state.

[0022] Furthermore, the calculation process for the precise pose is as follows:

[0023] (1) Scan the current environment to obtain the current frame point cloud data point_cloud, sequentially traverse the current point cloud point_cloud, and perform the following steps for each point pi(x,y) in the current point cloud point_cloud;

[0024] (2) Using the pose of the previous moment as the current initial pose pose(x,y,θ), project point pi(x,y) onto the high-precision map map_points and obtain the projected point p′. i Find the distance from p′ in the high-precision map map_points i The two closest points p′1 and p′2, if the projection point p′ i If the distances to points p′1 and p′2 are both less than the maximum allowable deviation distance, then proceed to step (3);

[0025] (3) Calculate point p′ i The distance g to the line p′1p′2 is used to construct the objective function:

[0026]

[0027] (4) After completing the traversal of all points in the point cloud, the pose increment Δpose is calculated by using nonlinear least squares. The current pose is then added to the pose increment Δpose to obtain the optimized pose. The optimized pose is used as the initial pose for the next iteration. The current iteration count is checked to see if it has reached the threshold. If the check result is yes, the iteration is stopped and the output pose is the current accurate pose. Otherwise, the process returns to step (2) and the next iteration begins.

[0028] Furthermore, the specific method for obtaining the pose of the 3D TOF camera is as follows:

[0029] (1) The point cloud data curr_pcd is obtained by scanning the environment in real time with a 3d TOF camera. Voxel filtering is performed on the point cloud data curr_pcd to obtain the filtered point cloud data curr_pcd_ds.

[0030] (2) Change the pose of the previous frame t-1 (x t-1 ,y t-1 ,θ t-1 Using the current initial pose as an example, iterate through the current point cloud count curr_pcd_ds, and set the point p in the current point cloud as the initial pose. i (x i ,y i ,z i Project it into the point cloud map map_pcd;

[0031] (3) Find the points in the point cloud map map_pcd that are the same as the projected points. The five most recent points, if these five points are... If the distances are all less than the maximum allowable deviation distance, proceed to step (4);

[0032] (4) Fit a plane using these five points. If the distances d from the five points to the plane all satisfy the threshold, then proceed to step (5).

[0033] (5) After traversing all points in the point cloud curr_pcd_ds, the objective function can be constructed based on the point-to-plane distance equation:

[0034]

[0035] The pose increment Δpose and the current pose pose are calculated using nonlinear least squares. tThe optimized pose is obtained by adding the pose increment Δpose. This optimized pose is used as the initial pose for the next iteration. The system checks if the current iteration count has reached the threshold. If yes, the system obtains the current precise pose of the TOF sensor, projects the points in curr_pcd_ds onto the map based on the current precise pose, and performs voxel filtering on the map. Otherwise, it returns to step (2) and begins the next iteration. This invention is implemented as follows: a calibration system between a 2D LiDAR and a 3D TOF camera, the system comprising:

[0036] A panel is irregularly arranged around the forklift, a 3D TOF camera is located at the front of the forklift, and a 2D LiDAR is integrated on the forklift. The 2D LiDAR and the 3D TOF camera are connected to a calibration unit. The calibration unit obtains the pose transformation tf(tx,ty,angle) of the 3D TOF camera relative to the 2D LiDAR based on the above calibration method between the 2D LiDAR and the 3D TOF camera.

[0037] Furthermore, the 3D TOF camera can measure two non-parallel panels in real time. The height of the panels must cover the field of view of the 3D TOF camera and must not be higher than the scanning line of the 2D LiDAR.

[0038] Furthermore, the panels are arranged in a room with flat walls on all sides and a flat floor.

[0039] This invention achieves high-precision calibration between a 2D laser radar on a 2D laser forklift and a 3D TOF camera for pallet recognition, without relying on additional equipment and relying solely on the natural environment to complete the calibration process. Furthermore, the calibration does not require the laser forklift to perform specific actions, thus reducing the requirements for the mobile robot. It also utilizes 2D repeated scanning high-precision map positioning and 3D point-to-surface matching positioning, and the improved positioning accuracy leads to improved calibration accuracy. Attached Figure Description

[0040] Figure 1 This invention provides a calibration system between a 2D lidar and a 3D TOF camera.

[0041] Figure 2 This invention provides a calibration method between a 2D lidar and a 3D TOF camera. Detailed Implementation

[0042] The specific embodiments of the present invention will be further described in detail below with reference to the accompanying drawings, so as to help those skilled in the art to have a more complete, accurate and in-depth understanding of the inventive concept and technical solution of the present invention.

[0043] Figure 1This is a schematic diagram of the calibration system between a 2D lidar and a 3D TOF camera provided in an embodiment of the present invention. For ease of explanation, only the parts related to the embodiment of the present invention are shown.

[0044] The system includes:

[0045] A 3D TOF camera is installed at the front of the forklift. The forklift is equipped with a 2D LiDAR. The 3D TOF camera is used to obtain the position of the pallet to be picked up, and the 2D LiDAR is used for the navigation and positioning of the forklift. The 2D LiDAR and the 3D TOF camera are connected to the calibration unit.

[0046] Panels are irregularly arranged around the forklift. The panels are arranged in a room with flat walls on all sides and a flat floor.

[0047] A 3D TOF camera can measure two non-parallel panels in real time. The height of the panels must cover the field of view of the 3D TOF camera and must not be higher than the scanning line of the 2D LiDAR.

[0048] Before using a 3D TOF camera to measure the pose of a pallet, the forklift needs to calculate the relative pose transformation tf(tx,ty,angle) between the 2D LiDAR and the 3D TOF camera. Figure 2 The flowchart below shows the calibration method between a 2D lidar and a 3D TOF camera provided in this embodiment of the invention. The method specifically includes the following steps:

[0049] S1. The laser forklift moves within the area surrounded by the panel. Whenever the movement distance exceeds 20 cm and the rotation angle exceeds 10 degrees, the laser forklift stops moving and step S2 is executed.

[0050] S2. Calculate the current poses of the 2D LiDAR and the 3D TOF camera, and store them in the pose set pose_list respectively. 2d pose list 3d middle;

[0051] S3, pose list 2d pose list 3d Once the pose data exceeds 50 records, the laser forklift will stop moving and cease data collection.

[0052] S4, regarding the pose list 2d and each pair of poses (x) in pose_list3d 2d ,y 2d ,θ 2d ) and (x 3d ,y 3d ,θ 3d ), (x 2d,y 2d ,θ 2d )∈pose_list 2d , (x 3d ,y 3d ,θ 3d )∈pose_list 3d Based on the pose transformation tf(tx,ty,angle), the following system of equations can be established:

[0053]

[0054] Traverse the pose list 2d and pose_list 3d A nonlinear least squares problem can be constructed and solved to calculate the pose transformation tf(tx,ty,angle) of the 3D TOF camera relative to the 2D LiDAR.

[0055] In this embodiment of the invention, before locating the 2D LiDAR, localization involves obtaining the pose of the 2D LiDAR in a 2D LiDAR point cloud map (map_points). Before localization, the 2D LiDAR point cloud map (map_points) needs to be constructed. The method for constructing the 2D LiDAR point cloud map (map_points) specifically includes the following steps:

[0056] (1) Stop the laser forklift in the middle of the panel surrounding area, start the 2D laser radar and start scanning. The point cloud data scanned in each frame is curr_points;

[0057] (2) Arrange the scanned point cloud curr_points into the container list_points according to the radar scan angle: The number of points N scanned by the radar each time is fixed, and the scan angle of each point is also fixed. So the container list_points can be preset to N dimensions, and the scan angle of each dimension is also fixed. Add each point in the point cloud data curr_points into each dimension of the container list_points in turn.

[0058] (3) Determine whether the current number of scans meets 50. If not, return to step (2) to continue scanning. If it does, execute step (4).

[0059] (4) Calculate the scan angle θ for each of the container list_points i The standard deviation of the distance measurement of valid points in the point set is calculated using the following formula:

[0060]

[0061] Where, σ i For the scanning angle θi The standard deviation of the distance measurement of the corresponding point set, where n is the scanning angle θ. i The number of valid points in the corresponding point set, d j For the scanning angle θ i The distance measurement value of the j-th valid point in the corresponding point set. For the scanning angle θ i The average distance measurement of all valid points in the corresponding point set, if the current scanning angle θ i The number of valid points in the point set is greater than 40, and the standard deviation of the distance measurement is less than the maximum allowable distance measurement deviation max_error_d, i.e., σ i If the value is less than max_error_d, then calculate the coordinates of the above valid points in the 2D laser point cloud map map_points.

[0062]

[0063]

[0064] θ i For scanning angles and calculating points Store it in the high-precision map map_points.

[0065] In this embodiment of the invention, the 2D LiDAR localization method based on 2D laser point cloud map (map_points) specifically includes the following steps:

[0066] The current environment is scanned to obtain the current frame point cloud data (point_cloud) and the pose from the previous moment as the current initial pose (pose(x, y, θ)). The precise pose of the laser forklift in the current state is then calculated. The specific calculation process for the precise pose is as follows:

[0067] (1) Scan the current environment to obtain the current frame point cloud data point_cloud;

[0068] (2) Iterate through the current point cloud in sequence and extract the next point p from the point cloud. i (x,y);

[0069] (3) The pose of the previous moment is used as the current initial pose pose(x, y, θ), and point p i After projecting (x,y) onto the high-precision map map_points, obtain the projection point p′. i Find the distance from p′ in the high-precision map map_points i The two closest points p′1 and p′2, if the projection point p′ iIf the distance between the projection point p′1 and p′2 exceeds the maximum allowable distance deviation (1 meter), return to step (2) and iterate to the next point. If the projection point p′ i If the distances to points p′1 and p′2 are both less than the maximum allowable distance deviation, then proceed to step (4);

[0070] (4) Calculate point p′ i The distance to the line p′1p′2 is calculated using the following formula:

[0071]

[0072] Where g is point p′ i The distance to the line p′1p′2, Let p′1 and p′2 be the vector formed by the endpoints of the line. Let p' be the point i The vector formed by the endpoint p′1 of the line can be used to construct the objective function:

[0073]

[0074] (5) After completing the traversal of all points in the point cloud, use nonlinear least squares to solve the problem and calculate the pose increment Δpose. Add the pose increment Δpose to the current pose to obtain the optimized pose. Use the optimized pose as the initial pose for the next iteration. Check if the current iteration count is 30. If the result is yes, stop the iteration and output the current accurate pose. Otherwise, return to step (2) to continue the iteration calculation.

[0075] In this embodiment of the invention, the 3d TOF camera performs mapping and localization simultaneously, and the localization method is as follows:

[0076] (1) The point cloud data curr_pcd is obtained by scanning the environment in real time with a 3d TOF camera. Voxel filtering is performed on the point cloud data curr_pcd to obtain the filtered point cloud data curr_pcd_ds.

[0077] (2) Determine whether the point cloud map map_pcd is empty. If the result is yes, set the current pose pose(0,0,0), store all the points in the point cloud data curr_pcd_ds into the point cloud map map_pcd, and return to step (1) to continue scanning.

[0078] (3) If the cloud map map_pcd is not empty, then iterate through the current point cloud number curr_pcd_ds and calculate the pose of the previous frame. t-1 (x t-1 ,y t-1 ,θ t-1Let p be the current initial pose, and let p be the point in the current point cloud. i (x i y i ,z i The projection equations are as follows: (The projection is then applied to the point cloud map map_pcd.)

[0079]

[0080] (4) Find the points in the point cloud map map_pcd that are the same as the projected points. The five most recent points, if these five points are... If the distances are all less than the allowable distance deviation (1m), proceed to step (5); otherwise, return to step (3) and continue to traverse the next point in the current point cloud data curr_pcd_ds.

[0081] (5) Fit a plane using these five points. If the distance d between the five points on the plane meets the threshold, proceed to step (6). Otherwise, return to step (3) and continue to traverse the next point in the current point cloud data curr_pcd_ds.

[0082] (6) After traversing all points in the point cloud data curr_pcd_ds, the objective function can be constructed based on the point-to-plane distance equation:

[0083]

[0084] The pose increment Δpose is calculated using nonlinear least squares, and the current pose pose is then determined. t Adding the pose increment Δpose yields the optimized pose. This optimized pose is then used as the initial pose for the next iteration. The iteration count is checked to see if 30 iterations have been completed. If yes, the iteration stops, and the current precise pose of the TOF sensor is obtained. t (x t ,y t ,θ t Based on precise pose t (x t ,y t ,θ t Add the point cloud data curr_pcd_ds to the point cloud map map_pcd; otherwise, return to step (2) to continue iterative calculation. In this embodiment of the invention, the nonlinear least squares solution process is as follows:

[0085] For nonlinear least squares problems that need to be solved:

[0086]

[0087] Performing a Taylor expansion on F(x), and substituting a first-order linear term into the above equation:

[0088]

[0089] Where J(x) is the Jacobian matrix of F(x); the derivative of the above equation with respect to Δx is set to 0:

[0090] J(x) T J(x)Δx=-J(x) T F(x)

[0091] Let A = J(x) T J(x),B=-J(x) T If F(x), the above equation can be simplified to AΔx=B. Solving this system of linear equations will yield the pose increment Δx.

[0092] The present invention has been described by way of example. Obviously, the specific implementation of the present invention is not limited to the above-described manner. Any non-substantial improvements made using the inventive concept and technical solution of the present invention, or the direct application of the inventive concept and technical solution of the present invention to other occasions without modification, are all within the protection scope of the present invention.

Claims

1. A method for calibrating between a 2d lidar and a 3d TOF camera, characterized in that, The method includes the following steps: S1. Control the laser forklift to move and change its current position. S2, calculate the pose of 2d lidar and 3d TOF camera, respectively stored in pose set and pose set ; S3, Detected pose set pose set If the number of poses in the data is greater than the number threshold, if the detection result is no, then proceed to step S1; if the detection result is yes, then proceed to step S4. S4, through the pose set and Each pair of poses in and Construct a nonlinear least squares problem to calculate the pose transformation of the 3d TOF camera relative to the 2d lidar ; Based on 2D laser point cloud map The 2D lidar positioning method specifically includes the following steps: Scan the current environment to obtain the current frame's point cloud data. The pose of the previous moment is used as the initial pose of the current moment. The precise pose of the laser forklift in its current state is calculated. The specific calculation process for the precise pose is as follows: (1) Scan the current environment to obtain the point cloud data of the current frame. traverse the current point cloud sequentially Regarding the current point cloud Each point in Perform the following steps; (2) The pose at the previous moment is taken as the current initial pose. , will point Projected onto high-precision map Then obtain the projection point In high-precision maps Searching for The two most recent points , If the projection point and , If the distance between the two points is less than the maximum allowable deviation distance, then proceed to step (3). (3) Calculation points to the straight line distance Based on distance Construct the objective function: ； (4) Complete the point cloud After traversing all points, the pose increment is calculated using nonlinear least squares. Make the current pose Add pose increment The optimized pose can be obtained. The optimized pose is used as the initial pose for the next iteration. The current iteration count is checked to see if the threshold is reached. If the check result is yes, the iteration is stopped and the output pose is the current accurate pose. Otherwise, return to step (2) and start the next iteration. The specific method for obtaining the pose of a 3D TOF camera is as follows: (1) A 3D TOF camera scans the environment in real time to obtain point cloud data The point cloud data is voxel filtered to obtain filtered point cloud data ;​ (2) Set the pose of the previous frame Using the current initial pose, iterate through the point cloud data at the current moment. The points in the current point cloud Projected onto point cloud map middle; (3) In the point cloud map Find the points in the projection The five most recent points, if these five points are... If the distances are all less than the maximum allowable deviation distance, proceed to step (4). (4) Fit a plane using these five points. If the distances from the five points to the plane are... If all thresholds are met, proceed to step (5). (5) Complete point cloud After traversing all points in the middle, according to the point-to-plane distance equation, the objective function can be constructed: ； The pose increment is calculated by using nonlinear least squares. Current pose Add pose increment The optimized pose is obtained and used as the initial pose for the next iteration. The number of iterations is checked to see if the threshold has been reached. If the check is yes, the current precise pose of the TOF sensor is obtained. The points in the map are projected onto the map based on the current precise pose, and the map is subjected to voxel filtering. Otherwise, return to step (2) and start the next iteration.

2. The method of claim 1, wherein the 2D laser radar and 3D TOF camera calibration method is characterized by, Before obtaining the pose of the 2D LiDAR, the following steps are also included: Constructing 2d laser point cloud maps , based on 2d laser point cloud maps Computing pose of 2d lidar.

3. The method of claim 1, wherein the 2D laser radar and 3D TOF camera calibration method is characterized by, 2D laser point cloud map The construction method specifically includes the following steps: (1) Position the laser forklift in the center of the panel enclosure area, activate the 2D laser radar, and begin scanning. The point cloud data scanned in each frame is as follows: ; (2) The scanned point cloud Arrange them into containers according to radar scan angle. middle; (3) Determine whether the current number of scans meets the number threshold. If not, return to step (2). If it does, execute step (4). (4) Calculate the container Each scanning angle Standard deviation of distance measurement of effective points in the point set If the current scan angle The number of valid points in the point set is greater than the quantity threshold, and ,in To determine the maximum allowable ranging deviation, proceed to step (5). (5) Calculate the effective points in the 2D laser point cloud map coordinates in and the calculated points Save high-precision map middle.

4. A system for calibration between a 2d lidar and a 3d TOF camera, characterized in that, The system includes: A 3D Time-of-Flight (TOF) camera is mounted at the front of the forklift, which is surrounded by an irregularly arranged panel. A 2D LiDAR is integrated on the forklift. The 2D LiDAR and the 3D TOF camera are communicatively connected to a calibration unit. The calibration unit obtains the pose transformation of the 3D TOF camera relative to the 2D LiDAR based on the calibration method between the 2D LiDAR and the 3D TOF camera as described in any one of claims 1 to 3. .

5. The calibration system between a 2D lidar and a 3D TOF camera as described in claim 4, characterized in that, A 3D TOF camera can measure two non-parallel panels in real time. The height of the panels must cover the field of view of the 3D TOF camera and must not be higher than the scanning line of the 2D LiDAR.

6. The calibration system between a 2D lidar and a 3D TOF camera as described in claim 4, characterized in that, The panel is placed in a room with flat walls on all four sides and a flat floor.

Images