Joint construction method of laser point cloud map and Apriltag two-dimensional code map

Abstract

The invention belongs to the technical field of mobile robot positioning and navigation, and relates to a laser point cloud map and Apriltag two-dimensional code map combined construction method. Three sensors including a 3d laser radar, a camera and an inertial measurement unit (IMU) are adopted, a transformation matrix from each sensor to a base coordinate system base is calibrated in advance, a transformation matrix from a laser radar coordinate system lidarlink to the base coordinate system base is Tbase-lid, a transformation matrix from a camera coordinate system camera to the base coordinate system base is Tbase-lid, a transformation matrix from an IMU coordinate system imlink to the base coordinate system base is Tbase-lid, and a transformation matrix from the IMU coordinate system imlink to the base coordinate system base is Tbase-imu. Wherein the laser radar and the camera are synchronously triggered through an external source signal, and laser point cloud data and image data are synchronously collected. According to the method, the architecture is uniformly optimized, and the laser point cloud map and the AprilTag two-dimensional code map are synchronously optimized, so that the problem of how to uniformly construct the laser point cloud map and the AprilTag two-dimensional code map is solved, meanwhile, the complexity of map construction operation is reduced, and the map construction precision is improved.

Description

Technical Field

[0001] The present invention belongs to the technical field of mobile robot positioning and navigation, and relates to a method for jointly constructing a laser point cloud map and an Apriltag two-dimensional code map. Background Art

[0002] With the acceleration of industrial upgrading and intelligentization, the application of mobile robots continues to expand in fields such as industrial manufacturing, smart agriculture, intelligent logistics, and security inspection. In complex operating environments, accurate and reliable positioning and navigation technologies are the core technical support for robots to efficiently execute tasks. Due to the diverse requirements of different application scenarios for positioning accuracy, environmental adaptability, and real-time performance, the shortcomings of single positioning technologies are becoming increasingly prominent, driving the continuous innovation of multi-source integrated positioning solutions.

[0003] 3D LiDAR positioning technology, leveraging its reliable environmental perception capabilities, builds high-precision three-dimensional maps from real-time scene point cloud data, providing continuous pose estimation for mobile robots and demonstrating excellent navigation performance in structured environments. However, this technology has significant environmental limitations: in open areas, positioning ambiguity can easily occur due to a lack of effective matching features. Furthermore, interference factors such as aerosol particles and water vapor can significantly affect the quality of the laser echo signal. Furthermore, its positioning accuracy is limited by the complexity of the point cloud registration algorithm and the computational resources required, making it difficult to meet the millimeter-level positioning requirements of precision operations. A positioning method based on AprilTag QR codes uses machine vision algorithms to calculate the spatial coordinates of pre-placed QR codes, providing millimeter-level six-degree-of-freedom pose information for robots. This method is suitable for demanding tasks such as precision assembly and precise grasping, requiring precise positioning. However, this technology relies on densely packed non-coplanar QR codes within its field of view, making it difficult to adapt to open spaces or complex terrain and subject to significant reliance on QR code visibility.

[0004] To address the inherent limitations of single technologies, 3D LiDAR positioning and AprilTag QR code positioning are integrated. In non-precision operating areas, LiDAR enables continuous position tracking without manual markers. In critical operating areas, AprilTag QR codes provide a high-precision position reference, forming a complementary positioning architecture with scene adaptability. However, current algorithms for jointly constructing point cloud maps and QR code maps suffer from low accuracy and complex operations.

[0005] Chinese patent CN117109561A discloses a method and system for creating and positioning a long-range QR code map that integrates laser positioning. This technical solution requires first establishing a laser map of the mapping area, and then constructing a QR code map through laser positioning trajectory constraints, dividing the mapping process into two steps, which becomes cumbersome. Moreover, only when a new QR code is observed for the first time, the spatial pose of the QR code is calculated using the laser positioning pose, and the pose is added to the factor graph. The factor graph is then used to uniformly optimize the laser positioning pose at all times and the pose of each QR code. It only adds the laser positioning factor and the QR code pose factor, and does not add the laser loop constraint factor and the QR code loop constraint factor. The factor graph is not constrained enough, which will affect the accuracy of QR code mapping.

[0006] Chinese patent CN116228870B discloses a mapping method and system based on QR code SLAM precision control. In this technical solution, the QR code's posture needs to be measured in advance using a total station. When constructing the posture graph, the QR code's posture is not used as an item to be optimized. Only the QR code positioning result is used as a constraint item, and then the trajectory of the laser SLAM is constrained through graph optimization. Summary of the Invention

[0007] In order to solve the above problems, the present invention provides a method for jointly constructing a laser point cloud map and an AprilTag QR code map.

[0008] The technical solution adopted by the present invention to achieve the above purpose is: a method for jointly constructing a laser point cloud map and an Apriltag QR code map, using three sensors including a 3D laser radar, a camera, and an inertial measurement unit (IMU), and calibrating the transformation matrix of each sensor to the base coordinate system base_link in advance. The transformation matrix from the laser radar coordinate system lidar_link to base_link is T base→lid , the transformation matrix from camera coordinate system camera_link to base_link is T base→cam , the transformation matrix from the IMU coordinate system imu_link to base_link is T base→imu , where the laser radar and camera are synchronously triggered by external signals to synchronously collect laser point cloud data and image data;

[0009] The entire map joint construction system includes a laser odometry module, a QR code detection module, and a graph optimization module: the laser odometry module uses the open source FAST_LIO algorithm. FAST_LIO tightly couples 3D laser point cloud data with IMU data through ESKF, and outputs point cloud data in the base_link coordinate system after motion distortion correction, as well as the position of base_link in the FAST_LIO local map coordinate system local_map; the QR code detection module uses the open source AprilTag_ros algorithm. AprilTag_ros receives image data and detects two The graph optimization module receives the point cloud and pose output by FAST_LIO, as well as the QR code name and pose output by AprilTag_ros, and constructs a factor graph through loop detection. The loop detection uses the ICP algorithm in the PCL library to calculate the loop constraint from the current frame to the historical frame, and then optimizes the factor graph through the ISAM2 algorithm of the gtsam library to calculate the pose of the laser point cloud and the QR code in the global map coordinate system global_map, as well as the transformation matrix T from local_map to global_map. glb→loc , when the system is initialized, T glb→loc Set to unit conversion.

[0010] The construction method specifically includes the following steps:

[0011] S1. Sensor driver releases data: After the robot is powered on, each sensor driver starts to collect data and releases laser point cloud data, image data, and IMU data. Steps S2 and S3 are started simultaneously.

[0012] S2, front-end odometer positioning: FAST_LIO receives laser point cloud data and IMU data, calculates and outputs the point cloud pcd_base after motion distortion correction in the base_link coordinate system, and the pose T of base_link in local_map loc→base , go to step S4.

[0013] S3, QR code detection: AprilTag_ros receives image data and outputs a QR code ID after processing. tag And the pose T of the QR code in the camera coordinate system cam→tag , go to step S5.

[0014] S4, the image optimization module processes the laser frame data: the image optimization module synchronously receives pcd_body and T loc→base , calculate the pose T of base_link under global_map glb→base =T glb→loc*T loc→base , pcd_base and T glb→base Combined into the current laser frame Frame i , specifically including:

[0015] S41, build laser frame node and prior factor: the current laser frame Frame i Constructed as the first node of the factor graph

[0016] Node0, set the node ID to 0, and the node's posture to T glb→base , and construct the factor graph prior factor

[0017] PriorFactor, PriorFactor's ID is 0, and PriorFactor's constraint transformation is T glb→base ;

[0018] S42. Construct laser frame nodes and associated factors: Node on the known factor graph i-1 The ID is id i-1 , the pose is T i-1 , then the current node Node i The ID is id i =id i-1 +1, the current node's position is T glb→base , and at the same time construct the factor graph associated factor BetweenFactor, the ID associated with BetweenFactor is id i-1 and id i , the constraint transformation of BetweenFactor is

[0019] S43: Add the currently constructed laser frame node and factors into the factor graph, and proceed to step S6.

[0020] If the current laser frame i If it is the first time to receive the laser frame, the process goes to step S41 ; otherwise, the process goes to step S42 .

[0021] S5. The graph optimization module processes the QR code data: If the currently received message contains a QR code, the ID of the known QR code tag and the pose T of the QR code in the camera coordinate system cam→tag , according to T base→cam And the laser positioning posture T calculated in step S4 glb→base , calculate the pose T of the QR code under global_map glb→tag =T glb→base *T base→cam *T cam→tag , specifically including:

[0022] S51. Construct QR code nodes and factors: The ID of a node on the known factor graph is id i-1 , the pose is T i-1 , then the ID of the current QR code node is id i =id i-1 +1, the current QR code node's position is T glb→tag , and at the same time construct the factor graph associated factor BetweenFactor, the ID associated with BetweenFactor is id i-1 and id i , the constraint transformation of BetweenFactor is

[0023] S52. Constructing QR code factor: querying the identification name ID in the cache tag Node j , the ID of the QR code node is id j , the ID of a node on the known factor graph is id i-1 , the pose is T i-1 , construct the factor graph associated factor BetweenFactor, the ID associated with BetweenFactor is id i-1 and id j , the constraint transformation of BetweenFactor is

[0024] S53: Add the currently constructed QR code node and factors into the factor graph, and proceed to step S7.

[0025] If the ID is observed for the first time tag , then go to step S51, otherwise go to step S52.

[0026] S6. Constructing a laser loopback factor, specifically including:

[0027] S61, find the distance from the current laser frame in the historical laser frame i The nearest laser frame j , if Frame j With Frame i If the time interval difference is greater than 30 seconds, the distance difference is less than 15 meters, and ij>25, then it is considered that Frame j It's Frame i The loopback laser frame;

[0028] S62, in Frame j Take 25 adjacent laser frames before and after and add Frame jThere are 51 laser frames in total. The point clouds of these 51 frames are transformed into the global pose T corresponding to each frame. glb→base Convert to the global map coordinate system and construct the loop point cloud map pcd_loop_map;

[0029] S63, known current frame Frame i The pose T i and point cloud pcd_base i , loop point cloud map pcd_loop_map and loop frame pose T j , use the ICP algorithm of the PCL library to match point_cloud_body i With pcd_loop_map, get the loop constraint transformation T j→i ;

[0030] S64. Construct the factor graph correlation factor BetweenFactor. The IDs of the two nodes associated with BetweenFactor are id i and id j , the constraint transformation of BetweenFactor is T j→i , add the current factor into the factor graph and go to step S7.

[0031] S7. Add all the nodes and factors constructed above into ISAM2 for processing and optimization, obtain the optimized pose of each node, and find the latest laser frame Frame last Optimized global pose T glb→base , the known pose T of Framelast in local_map loc→base , the latest

[0032] S8. After the entire mapping site is traversed and all QR codes in the site are added to the factor graph, the final optimization is started to obtain the globally optimized pose of each laser frame and the pose of each QR code. The point cloud of each laser frame is converted to its global pose to obtain a global point cloud. All the global point clouds of the laser frames are collected together to obtain a globally optimized point cloud map. Collecting each optimized QR code pose gives the globally optimal QR code map.

[0033] The accuracy of QR code detection is closely related to the distance and angle between the camera and the QR code. The distance between the camera and the QR code refers to the distance between the origin of the camera coordinate system and the origin of the QR code coordinate system, and the angle between the camera and the QR code refers to the angle between the camera imaging plane and the QR code plane. When the camera is close to the QR code and the angle is small, the QR code detection accuracy can reach within 1 cm. When the camera is far away from the QR code or the angle is large, the error in QR code detection can reach over 10 cm. In addition, due to factors such as motion speed and ground bumps, the quality of camera imaging can also vary, which can also lead to significant differences in QR code detection accuracy. The current factor graph does not consider the impact of different distances, angles, and image quality on QR code detection accuracy when adding QR code correlation factors, which may lead to increased errors in QR code mapping. Therefore, during the mapping process, the detected QR codes can be screened to filter out those with large distances and angles. In addition, the QR code detection module can also output the covariance of the QR code detection to filter out QR codes with excessively large covariance. When adding QR code association factors to the factor graph, the QR code detection covariance data can also be integrated.

[0034] The present invention solves the problem of how to uniformly construct laser point cloud maps and AprilTag QR code maps through a unified optimization architecture and synchronously optimizes the laser point cloud map and the AprilTag QR code map, while reducing the complexity of map construction operations and improving the accuracy of map construction. BRIEF DESCRIPTION OF THE DRAWINGS

[0035] Figure 1 Flow chart of the method of the present invention. DETAILED DESCRIPTION

[0036] The following is a further description of this patent with reference to the accompanying drawings and specific embodiments:

[0037] Example

[0038] like Figure 1 The method for jointly constructing a laser point cloud map and an Apriltag QR code map shown in the figure uses three sensors including a 3D laser radar, a camera, and an inertial measurement unit (IMU). The transformation matrix from each sensor to the base coordinate system base_link is calibrated in advance. The transformation matrix from the laser radar coordinate system lidar_link to base_link is T base→lid , the transformation matrix from camera coordinate system camera_link to base_link is T base→cam , the transformation matrix from the IMU coordinate system imu_link to base_link is T base→imu , where the laser radar and camera are synchronously triggered by external signals to synchronously collect laser point cloud data and image data;

[0039] The entire map joint construction system includes a laser odometry module, a QR code detection module, and a graph optimization module: the laser odometry module uses the open source FAST_LIO algorithm. FAST_LIO tightly couples 3D laser point cloud data with IMU data through ESKF, and outputs point cloud data in the base_link coordinate system after motion distortion correction, as well as the position of base_link in the FAST_LIO local map coordinate system local_map; the QR code detection module uses the open source AprilTag_ros algorithm. AprilTag_ros receives image data and detects two The graph optimization module receives the point cloud and pose output by FAST_LIO, as well as the QR code name and pose output by AprilTag_ros, and constructs a factor graph through loop detection. The loop detection uses the ICP algorithm in the PCL library to calculate the loop constraint from the current frame to the historical frame, and then optimizes the factor graph through the ISAM2 algorithm of the gtsam library to calculate the pose of the laser point cloud and the QR code in the global map coordinate system global_map, as well as the transformation matrix T from local_map to global_map. glb→loc , when the system is initialized, T glb→loc Set to unit conversion.

[0040] The construction method specifically includes the following steps:

[0041] S1. Sensor driver releases data: After the robot is powered on, each sensor driver starts to collect data and releases laser point cloud data, image data, and IMU data. Steps S2 and S3 are started simultaneously.

[0042] S2, front-end odometer positioning: FAST_LIO receives laser point cloud data and IMU data, calculates and outputs the point cloud pcd_base after motion distortion correction in the base_link coordinate system, and the pose T of base_link in local_map loc→base , go to step S4.

[0043] S3, QR code detection: AprilTag_ros receives image data and outputs a QR code ID after processing. tag And the pose T of the QR code in the camera coordinate system cam→tag , go to step S5.

[0044] S4, the image optimization module processes the laser frame data: the image optimization module synchronously receives pcd_body and T loc→base , calculate the pose T of base_link under global_map glb→base =Tglb→loc *T loc→base , pcd_base and T glb→base Combined into the current laser frame Frame i , specifically including:

[0045] S41, build laser frame node and prior factor: the current laser frame Frame i Construct the first node Node0 of the factor graph, set the ID of the node to 0, and the pose of the node to T glb→base , and at the same time construct the factor graph prior factor PriorFactor, the ID of PriorFactor is 0, and the constraint transformation of PriorFactor is T glb→base ;

[0046] S42. Construct laser frame nodes and associated factors: Node on the known factor graph i-1 The ID is id i-1 , the pose is T i-1 , then the current node Node i The ID is id i =id i-1 +1, the current node's position is T glb→base , and at the same time construct the factor graph associated factor BetweenFactor, the ID associated with BetweenFactor is id i-1 and id i , the constraint transformation of BetweenFactor is

[0047] S43: Add the currently constructed laser frame node and factors into the factor graph, and proceed to step S6.

[0048] If the current laser frame i If it is the first time to receive the laser frame, the process goes to step S41 ; otherwise, the process goes to step S42 .

[0049] S5. The graph optimization module processes the QR code data: If the currently received message contains a QR code, the ID of the known QR code tag and the pose T of the QR code in the camera coordinate system cam→tag , according to T base→cam And the laser positioning posture T calculated in step S4 glb→base , calculate the pose T of the QR code under global_map glb→tag =T glb→base *T base→cam *T cam→tag , specifically including:

[0050] S51. Construct QR code nodes and factors: The ID of a node on the known factor graph is id i-1 , the pose is T i-1 , then the ID of the current QR code node is id i =id i-1 +1, the current QR code node's position is T glb→tag , and at the same time construct the factor graph associated factor BetweenFactor, the ID associated with BetweenFactor is id i-1 and id i , the constraint transformation of BetweenFactor is

[0051] S52. Constructing QR code factor: querying the identification name ID in the cache tag Node j , the ID of the QR code node is id j , the ID of a node on the known factor graph is id i-1 , the pose is T i-1 , construct the factor graph associated factor BetweenFactor, the ID associated with BetweenFactor is id i-1 and id j , the constraint transformation of BetweenFactor is

[0052] S53: Add the currently constructed QR code node and factors into the factor graph, and proceed to step S7.

[0053] If the ID is observed for the first time tag , then go to step S51, otherwise go to step S52.

[0054] S6. Constructing a laser loopback factor, specifically including:

[0055] S61, find the distance from the current laser frame in the historical laser frame i The nearest laser frame j , if Frame j With Frame i If the time interval difference is greater than 30 seconds, the distance difference is less than 15 meters, and ij>25, then it is considered that Frame j It's Frame i The loopback laser frame;

[0056] S62, in Frame j Take 25 adjacent laser frames before and after and add Frame j There are 51 laser frames in total. The point clouds of these 51 frames are transformed into the global pose T corresponding to each frame.glb→base Convert to the global map coordinate system and construct the loop point cloud map pcd_loop_map;

[0057] S63, known current frame Frame i The pose T i and point cloud pcd_base i , loop point cloud map pcd_loop_map and loop frame pose T j , use the ICP algorithm of the PCL library to match point_cloud_body i With pcd_loop_map, get the loop constraint transformation T j→i ;

[0058] S64. Construct the factor graph correlation factor BetweenFactor. The IDs of the two nodes associated with BetweenFactor are id i and id j , the constraint transformation of BetweenFactor is T j→i , add the current factor into the factor graph and go to step S7.

[0059] S7. Add all the nodes and factors constructed above into ISAM2 for processing and optimization, obtain the optimized pose of each node, and find the latest laser frame Frame last Optimized global pose T glb→base , the known pose T of Framelast in local_map loc→base , the latest

[0060] S8. After the entire mapping site is traversed and all QR codes in the site are added to the factor graph, the final optimization is started to obtain the globally optimized pose of each laser frame and the pose of each QR code. The point cloud of each laser frame is converted to its global pose to obtain a global point cloud. All the global point clouds of the laser frames are collected together to obtain a globally optimized point cloud map. Collecting each optimized QR code pose gives the globally optimal QR code map.

[0061] Example 2

[0062] All steps in this embodiment are the same as those in Example 1, except that: during the mapping process, the detected QR codes are screened to filter out QR codes with relatively large distances and angles; in addition, the QR code detection module outputs the covariance of the QR code detection to filter out QR codes with excessively large covariance, and the QR code detection covariance data is integrated into the factor graph when adding the QR code association factor.

Claims

1. A method for jointly constructing a laser point cloud map and an Apriltag QR code map, characterized by: Use three sensors including 3D laser radar, camera and inertial measurement unit IMU, and calibrate the transformation matrix of each sensor to the base coordinate system base_link in advance. The transformation matrix from the laser radar coordinate system lidar_link to base_link is T base→lid , the transformation matrix from camera coordinate system camera_link to base_link is T base→cam , the transformation matrix from the IMU coordinate system imu_link to base_link is T base→imu , where the laser radar and camera are synchronously triggered by external signals to synchronously collect laser point cloud data and image data; The entire map joint construction system includes a laser odometry module, a QR code detection module, and a graph optimization module: the laser odometry module uses the open source FAST_LIO algorithm. FAST_LIO tightly couples 3D laser point cloud data with IMU data through ESKF, and outputs point cloud data in the base_link coordinate system after motion distortion correction, as well as the position of base_link in the FAST_LIO local map coordinate system local_map; the QR code detection module uses the open source AprilTag_ros algorithm. AprilTag_ros receives image data and detects two The graph optimization module receives the point cloud and pose output by FAST_LIO, as well as the QR code name and pose output by AprilTag_ros, and constructs a factor graph through loop detection. The loop detection uses the ICP algorithm in the PCL library to calculate the loop constraint from the current frame to the historical frame, and then optimizes the factor graph through the ISAM2 algorithm of the gtsam library to calculate the pose of the laser point cloud and the QR code in the global map coordinate system global_map, as well as the transformation matrix T from local_map to global_map. glb→loc , when the system is initialized, T glb→loc Set to unit conversion.

2. The method for jointly constructing a laser point cloud map and an Apriltag QR code map according to claim 1, characterized in that: The construction method specifically includes the following steps: S1. Sensor driver releases data: After the robot is powered on, each sensor driver starts to collect data and releases laser point cloud data, image data, and IMU data. Steps S2 and S3 are started simultaneously. S2, front-end odometer positioning: FAST_LIO receives laser point cloud data and IMU data, calculates and outputs the point cloud pcd_base after motion distortion correction in the base_link coordinate system, and the pose T of base_link in local_map loc→base , proceed to step S4; S3, QR code detection: AprilTag_ros receives image data and outputs a QR code ID after processing. tag And the pose T of the QR code in the camera coordinate system cam→tag , proceed to step S5; S4, the image optimization module processes the laser frame data: the image optimization module synchronously receives pcd_body and T loc→base , calculate the pose T of base_link under global_map glb→base =T glb→loc *T loc→base , pcd_base and T glb→base Combined into the current laser frame Frame i ; S5. The graph optimization module processes the QR code data: If the currently received message contains a QR code, the ID of the known QR code tag and the pose T of the QR code in the camera coordinate system cam→tag , according to T base→cam And the laser positioning posture T calculated in step S4 glb→base , calculate the pose T of the QR code under global_map glb→tag =T glb→base *T base→cam *T cam→tag ; S6, constructing laser loopback factor; S7. Add all the nodes and factors constructed above into ISAM2 for processing and optimization, and obtain the optimized pose of each node, and find the global pose T after the optimization of the latest laser frame Framelast. glb → base , the known pose T of Framelast in local_map loc→base , the latest S8. After the entire mapping site is traversed and all QR codes in the site are added to the factor graph, the final optimization is started to obtain the globally optimized pose of each laser frame and the pose of each QR code. The point cloud of each laser frame is converted to its global pose to obtain a global point cloud. All the global point clouds of the laser frames are collected together to obtain a globally optimized point cloud map. Collecting each optimized QR code pose gives the globally optimal QR code map.

3. The method for jointly constructing a laser point cloud map and an Apriltag QR code map according to claim 2, characterized in that: The step S4 specifically includes: S41, build laser frame node and prior factor: the current laser frame Frame i Construct the first node Node0 of the factor graph, set the ID of the node to 0, and the pose of the node to T glb→base , and at the same time construct the factor graph prior factor PriorFactor, the ID of PriorFactor is 0, and the constraint transformation of PriorFactor is T glb→base ; S42. Construct laser frame nodes and associated factors: Node on the known factor graph i-1 The ID is id i-1 , the pose is T i-1 , then the current node Node i The ID is id i =id i-1 +1, the current node's position is T glb→base , and at the same time construct the factor graph associated factor BetweenFactor, the ID associated with BetweenFactor is id i-1 and id i , the constraint transformation of BetweenFactor is S43: Add the currently constructed laser frame node and factors into the factor graph, and proceed to step S6.

4. The method for jointly constructing a laser point cloud map and an Apriltag QR code map according to claim 3, characterized in that: If the current laser frame i If it is the first time to receive the laser frame, the process goes to step S41 ; otherwise, the process goes to step S42 .

5. The method for jointly constructing a laser point cloud map and an Apriltag QR code map according to claim 2, characterized in that: The step S5 specifically includes: S51. Construct QR code nodes and factors: The ID of a node on the known factor graph is id i-1 , the pose is T i-1 , then the ID of the current QR code node is id i =id i-1 +1, the current QR code node's position is T glb→tag , and at the same time construct the factor graph associated factor BetweenFactor, the ID associated with BetweenFactor is id i-1 and id i , the constraint transformation of BetweenFactor is S52. Constructing QR code factor: querying the identification name ID in the cache tag Node j , the ID of the QR code node is id j , the ID of a node on the known factor graph is id i-1 , the pose is T i-1 , construct the factor graph associated factor BetweenFactor, the ID associated with BetweenFactor is id i-1 and id j , the constraint transformation of BetweenFactor is S53: Add the currently constructed QR code node and factors into the factor graph, and proceed to step S7.

6. The method for jointly constructing a laser point cloud map and an Apriltag QR code map according to claim 5, characterized in that: If the ID is observed for the first time tag , then go to step S51, otherwise go to step S52.

7. The method for jointly constructing a laser point cloud map and an Apriltag QR code map according to claim 2, characterized in that: The step S6 specifically includes: S61, find the distance from the current laser frame in the historical laser frame i The nearest laser frame j , if Frame j With Frame i If the time interval difference is greater than 30 seconds, the distance difference is less than 15 meters, and ij>25, then it is considered that Frame j It's Frame i The loopback laser frame; S62, in Frame j Take 25 adjacent laser frames before and after and add Frame j There are 51 laser frames in total. The point clouds of these 51 frames are transformed into the global pose T corresponding to each frame. glb→base Convert to the global map coordinate system and construct the loop point cloud map pcd_loop_map; S63, known current frame Frame i The pose T i and point cloud pcd_base i , loop point cloud map pcd_loop_map and loop frame pose T j , use the ICP algorithm of the PCL library to match point_cloud_body i With pcd_loop_map, get the loop constraint transformation T j→i ; S64. Construct the factor graph correlation factor BetweenFactor. The IDs of the two nodes associated with BetweenFactor are id i and id j , the constraint transformation of BetweenFactor is T j→i , add the current factor into the factor graph and go to step S7.

Images