LiDAR and GPS joint calibration method and system based on planar trajectory constraint
Patent Information
- Application Number
- CN202611099736.8
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2026-07-23
- Publication Date
- 2026-10-09
AI Technical Summary
然而，车辆多数时间在平面内运动，高度、横滚角和俯仰角观测不足
[0029]（1）本技术方案将车辆平面运动场景下的LiDAR与GPS联合标定问题由三维SE(3)问题转化为问题，仅优化x、y和yaw，降低z、roll和pitch弱可观造成的标定误差；
Smart Images
Figure CN122883151A_ABST
Abstract
Description
Technical Field
[0001] This invention belongs to the technical fields of mobile robots, autonomous vehicles, multi-sensor fusion positioning and extrinsic parameter calibration, and specifically relates to a LiDAR and GPS joint calibration method and system based on planar trajectory constraints. Background Technology
[0002] In mobile robots, unmanned vehicles, industrial park logistics vehicles, and autonomous driving systems, LiDAR is typically used to construct point cloud maps and output positioning trajectories in the MAP coordinate system; GPS or GPS / INS systems are typically used to output global positioning trajectories in the ENU coordinate system. To achieve consistency between global positioning results and point cloud map positioning results, it is necessary to determine the transformation relationship between the ENU coordinate system and the MAP coordinate system.
[0003] Meanwhile, since GPS antennas, inertial measurement units, and LiDAR are typically mounted in different locations on a vehicle, there is a fixed installation offset between the sensor origins. During vehicle turning or rotation, different sensor origins will form trajectories with different radii around the same instantaneous center of rotation. If this installation offset is ignored, the GPS trajectory and LiDAR trajectory are difficult to directly coincide, thus affecting the accuracy of multi-sensor fusion positioning.
[0004] Traditional methods typically employ manual measurement, single-trajectory rigid body registration, or full 3D SE(3) joint optimization for calibration. However, vehicles spend most of their time moving in a plane, resulting in insufficient observation of height, roll angle, and pitch angle. Directly performing 3D or 12-DOF joint optimization can easily lead to problems such as weak parameter observability, result drift, or coupling between coordinate system transformation and installation extrinsic parameters.
[0005] Therefore, there is an urgent need for a joint LiDAR and GPS calibration method for vehicle planar motion scenarios. This method should simultaneously estimate the planar transformation from the ENU coordinate system to the MAP coordinate system and the installation extrinsic parameters from the LiDAR coordinate system to the GPS coordinate system, using only x, y, and yaw coordinates, thereby improving calibration stability and engineering applicability. Summary of the Invention
[0006] The purpose of this invention is to overcome the shortcomings of existing technologies and propose a LiDAR and GPS joint calibration method and system based on planar trajectory constraints. This method utilizes the LiDAR trajectory in the MAP coordinate system and the GPS trajectory in the ENU coordinate system, through... Planar pose constraints are determined, and the coordinate transformation from ENU coordinate system to MAP coordinate system and the installation extrinsic parameters from LiDAR coordinate system to GPS coordinate system are solved simultaneously.
[0007] According to some embodiments, the present invention adopts the following technical solution:
[0008] A LiDAR and GPS joint calibration method based on planar trajectory constraints includes the following steps:
[0009] (1) Obtain GPS trajectory data in the ENU coordinate system and LiDAR trajectory data in the point cloud map MAP coordinate system;
[0010] (2) Extract the GPS timestamp, planar position coordinates and GPS heading angle from the GPS trajectory data, and extract the LiDAR timestamp, planar position coordinates and LiDAR heading angle from the LiDAR trajectory data;
[0011] (3) Correct the GPS heading angle by coordinate definition to obtain the corrected GPS heading angle;
[0012] (4) Based on the GPS timestamp and LiDAR timestamp, interpolate the LiDAR trajectory to the GPS timestamp to obtain the time-synchronized LiDAR trajectory;
[0013] (5) Based on the GPS planar position and the corrected GPS heading angle, construct the GPS coordinate system in the ENU coordinate system. Pose matrix; construct the LiDAR in the MAP coordinate system based on the time-synchronized LiDAR planar position and LiDAR heading angle. Pose matrix;
[0014] (6) Set the planar pose transformation from ENU coordinate system to MAP coordinate system and the installation extrinsic parameters from LiDAR coordinate system to GPS coordinate system as the parameters to be calibrated;
[0015] (7) Based on the parameters to be calibrated, GPS in the ENU coordinate system Using the pose matrix and the aforementioned mounting extrinsic parameters, the pose of LiDAR in the MAP coordinate system is predicted.
[0016] (8) Construct a residual function based on the predicted LiDAR pose and the observed LiDAR pose;
[0017] (9) Perform nonlinear least squares optimization on the residual function to obtain the coordinate transformation parameters from ENU coordinate system to MAP coordinate system and the installation extrinsic parameters from LiDAR coordinate system to GPS coordinate system.
[0018] Preferably, the parameters to be calibrated include , , , , and ,in, , and This represents the planar translation parameters and heading angle parameters from the ENU coordinate system to the MAP coordinate system. , and This represents the installation translation parameters and installation heading angle parameters from the LiDAR coordinate system to the GPS coordinate system.
[0019] Preferably, the coordinate definition correction of the GPS heading angle satisfies ,in, The original heading angle is obtained by solving the quaternion from the GPS trajectory data. This is the corrected GPS heading angle. This indicates that the angle is normalized to... The range; the LiDAR heading angle is calculated from the quaternions in the LiDAR trajectory data, and no further processing is performed. Correction to the form of coordinate definition.
[0020] Preferably, the time synchronization includes: determining a common time interval between the GPS trajectory and the LiDAR trajectory, retaining only the GPS trajectory within the common time interval, and linearly interpolating the x, y, and yaw of the LiDAR trajectory according to the GPS timestamp.
[0021] Preferably, the The pose matrix is derived from the planar pose. It is constructed to have the following form:
[0022] .
[0023] Preferably, the predicted pose of LiDAR in the MAP coordinate system satisfies ,in, This is a transformation from the ENU coordinate system to the MAP coordinate system. This represents the GPS pose in the ENU coordinate system. The installation extrinsic parameters for converting the LiDAR coordinate system to the GPS coordinate system.
[0024] Preferably, the residual function includes planar position residual and heading angle residual; wherein, the error pose satisfies The planar position residual is The first two terms in the third column of the matrix have the heading angle residuals as follows: Each frame's residual vector consists of the product of the position residual weight and the x-direction position residual, the product of the position residual weight and the y-direction position residual, and the product of the heading angle residual weight and the heading angle residual.
[0025] Preferably, in nonlinear least squares optimization, boundary constraints are set for the parameters to be calibrated to limit the range of translation and heading angles from the ENU coordinate system to the MAP coordinate system, and the range of installation translation and heading angles from the LiDAR coordinate system to the GPS coordinate system; during the optimization process, the middle stable segment of the trajectory is selected to participate in the residual construction to reduce the influence of the trajectory start segment, end segment, interpolation boundary and sensor start and stop on the calibration results.
[0026] Preferably, after optimization, based on the results obtained from the optimization... and The LiDAR trajectory is re-predicted, and the root mean square error of the plane position and the root mean square error of the heading angle between the predicted and observed LiDAR trajectories are calculated to evaluate the calibration results.
[0027] This invention also provides a LiDAR and GPS joint calibration system based on planar trajectory constraints, including a data acquisition module, a pose extraction module, a heading angle correction module, a time synchronization module, a pose construction module, a residual construction module, an optimization solution module, and a verification output module. The data acquisition module acquires GPS trajectory data and LiDAR trajectory data. The pose extraction module extracts timestamps, planar positions, and heading angles. The heading angle correction module corrects the GPS heading angle. The time synchronization module interpolates the LiDAR trajectory to the GPS timestamp. The pose construction module constructs... The pose matrix; the residual construction module is used to construct the planar position residual and the heading angle residual; the optimization solution module is used to solve the calibration parameters; the verification output module is used to output the calibration results and error statistics results.
[0028] Compared with the prior art, the beneficial effects of the present invention are as follows:
[0029] (1) This technical solution transforms the LiDAR and GPS joint calibration problem in the vehicle planar motion scenario from a three-dimensional SE(3) problem into a three-dimensional SE(3) problem. The problem is to optimize only x, y, and yaw to reduce calibration errors caused by the weak observability of z, roll, and pitch;
[0030] (2) This technical solution simultaneously estimates the coordinate transformation from the ENU coordinate system to the MAP coordinate system and the installation extrinsic parameters from the LiDAR coordinate system to the GPS coordinate system, which can compensate for the difference in the rotation trajectory radius caused by the different installation positions of the GPS antenna and the LiDAR.
[0031] (3) This technical solution performs GPS heading angle analysis. The coordinate definition was corrected to make the GPS trajectory heading angle consistent with the vehicle's planar motion coordinate definition;
[0032] (4) This technical solution achieves time synchronization by screening common time intervals and interpolating LiDAR trajectories, thereby reducing errors caused by inconsistent sampling frequencies of different sensors;
[0033] (5) This technical solution improves the consistency between GPS-predicted LiDAR trajectory and LiDAR observation trajectory under straight, turning and rotation conditions by jointly optimizing the position residual and heading angle residual. Attached Figure Description
[0034] Figure 1 This is a basic flowchart of the LiDAR and GPS joint calibration method provided in the embodiments of the present invention;
[0035] Figure 2 This is a schematic diagram of the coordinate transformation relationship between LiDAR, GPS, ENU and MAP provided in the embodiments of the present invention;
[0036] Figure 3 This is a schematic diagram of the residual construction between the predicted LiDAR trajectory and the observed LiDAR trajectory provided in an embodiment of the present invention;
[0037] Figure 4 This is a schematic diagram illustrating the coordinate relationship between LiDAR and GPS installation extrinsic parameters based on motion capture equipment, provided in an embodiment of the present invention. Detailed Implementation
[0038] The present invention will be further described below with reference to the accompanying drawings and embodiments. It should be understood that the following detailed description is merely illustrative and intended to provide a clearer explanation of the present invention, and does not constitute a limitation on the scope of protection of the present invention.
[0039] like Figure 1 As shown, a LiDAR and GPS joint calibration method based on planar trajectory constraints includes the following steps:
[0040] Step 1: GPS and LiDAR Track Data Acquisition
[0041] GPS / INS odometry tracks and LiDAR odometry tracks are simultaneously acquired on a mobile platform. The GPS / INS track represents the pose of the GPS coordinate system in the ENU coordinate system, and the LiDAR track represents the pose of the LiDAR coordinate system in the MAP coordinate system. Each track contains at least a ROS timestamp, position coordinates, and attitude quaternions.
[0042] Step 2: Planar pose extraction and GPS heading angle correction
[0043] Extract from GPS track , and original heading angle and according to Obtain the corrected GPS heading angle. This correction is used to eliminate the difference between the GPS / ENU heading angle definition and the vehicle's horizontal heading angle definition.
[0044] Extracting from LiDAR tracks , and LiDAR heading angle is not adjusted. Form correction. The GPS heading angle sequence and the LiDAR heading angle sequence are unwrapped separately to avoid angle jumps near π and -π.
[0045] Step 3: Track Time Synchronization
[0046] Calculate the common time interval between the GPS track and the LiDAR track, and retain the GPS track within the common time interval; then perform linear interpolation on the x, y, and yaw of the LiDAR track to make the LiDAR track and the GPS track correspond one-to-one at the GPS timestamp.
[0047] Step 4: Construction of pose matrix
[0048] For any planar pose ,structure matrix Its first act The second line The third line This yields the pose matrix of each GPS frame in the ENU coordinate system. And the LiDAR observation pose matrix in the MAP coordinate system .
[0049] Step 5: Setting parameters to be optimized
[0050] The parameters to be optimized include two sets of three-degree-of-freedom plane transformations. The first set of three degrees of freedom is... The first group corresponds to the x-direction translation, y-direction translation, and yaw rotation from the ENU coordinate system to the MAP coordinate system; the second group of three degrees of freedom is... This corresponds to the x-direction installation offset, y-direction installation offset, and yaw installation angle error between the LiDAR coordinate system and the GPS coordinate system.
[0051] Step 6: Predicting Links and Constructing Residuals
[0052] like Figure 2 As shown, using Predict the pose of LiDAR in the MAP coordinate system. Then, based on... Construct error pose and from Extracting planar position error from a matrix , and heading angle error .
[0053] Step 7: Nonlinear Least Squares Optimization
[0054] The residual vector for each frame is set to The calibration parameters are solved using nonlinear least squares optimization with boundary constraints. The boundary constraints can be set based on the site area, vehicle dimensions, and installation measurement priors to prevent the optimizer from absorbing coordinate system translations and installation extrinsic parameters.
[0055] Step 8: Verification of calibration results
[0056] After optimization, use the results obtained from the optimization. and Re-predict the LiDAR trajectory and compare it with the LiDAR observation trajectory, calculating the root mean square error of the plane position and the root mean square error of the heading angle. Figure 3 As shown, if the predicted LiDAR trajectory and the observed LiDAR trajectory are basically coincident under straight, turning and rotation conditions, the calibration results are considered to meet the engineering requirements.
[0057] Step 9: Verification of motion capture equipment
[0058] In a site with suitable motion capture capabilities, a motion capture rigid body coordinate system B can be established on the vehicle, and the fixed transformation from the vehicle rigid body coordinate system to the LiDAR coordinate system can be measured. and the fixed transformation from the vehicle rigid body coordinate system to the GPS coordinate system .like Figure 4 As shown, the motion capture device outputs the pose of the vehicle rigid body in the motion capture coordinate system M. Thus we obtain and Further calculations and the optimized The results were compared to verify the accuracy of the installed external parameters.
[0059] Through the above method, the present invention provides a LiDAR and GPS joint calibration method and system based on planar trajectory constraints, which can complete the calibration using trajectory data during normal vehicle driving, turning or rotating, and provide an accurate coordinate transformation basis for multi-sensor fusion positioning of mobile robots and autonomous vehicles.
[0060] Industrial applicability:
[0061] This invention can be applied to unmanned vehicles, mobile robots, mining vehicles, industrial park logistics vehicles, outdoor inspection robots, and other mobile platforms equipped with LiDAR and GPS / INS. This method does not rely on a calibration board and can complete coordinate system unification and installation extrinsic parameter calibration using the vehicle's motion trajectory, making it suitable for rapid deployment and periodic recalibration in engineering projects.
[0062] Those skilled in the art should understand that the above embodiments are only used to illustrate the technical solutions of the present invention, and are not intended to limit the present invention. Several improvements and modifications can be made without departing from the spirit and scope of the present invention, and these improvements and modifications should also be considered within the protection scope of the present invention.
Claims
1. A LiDAR and GPS joint calibration method based on planar trajectory constraints, characterized in that, Includes the following steps: (1) Obtain GPS trajectory data in the ENU coordinate system and LiDAR trajectory data in the point cloud map MAP coordinate system; (2) Extract the GPS timestamp, planar position coordinates and GPS heading angle from the GPS trajectory data, and extract the LiDAR timestamp, planar position coordinates and LiDAR heading angle from the LiDAR trajectory data; (3) Correct the GPS heading angle by coordinate definition to obtain the corrected GPS heading angle; (4) Based on the GPS timestamp and LiDAR timestamp, interpolate the LiDAR trajectory to the GPS timestamp to obtain the time-synchronized LiDAR trajectory; (5) Based on the GPS planar position and the corrected GPS heading angle, construct the GPS coordinate system in the ENU coordinate system. Pose matrix; construct the LiDAR in the MAP coordinate system based on the time-synchronized LiDAR planar position and LiDAR heading angle. Pose matrix; (6) Set the planar pose transformation from ENU coordinate system to MAP coordinate system and the installation extrinsic parameters from LiDAR coordinate system to GPS coordinate system as the parameters to be calibrated; (7) Based on the parameters to be calibrated, GPS in the ENU coordinate system Using the pose matrix and the aforementioned mounting extrinsic parameters, the pose of LiDAR in the MAP coordinate system is predicted. (8) Construct a residual function based on the predicted LiDAR pose and the observed LiDAR pose; (9) Perform nonlinear least squares optimization on the residual function to obtain the coordinate transformation parameters from ENU coordinate system to MAP coordinate system and the installation extrinsic parameters from LiDAR coordinate system to GPS coordinate system.
2. The LiDAR and GPS joint calibration method based on planar trajectory constraints as described in claim 1, characterized in that, The parameters to be calibrated include , , , , and ,in, , and This represents the planar translation parameters and heading angle parameters from the ENU coordinate system to the MAP coordinate system. , and This represents the installation translation parameters and installation heading angle parameters from the LiDAR coordinate system to the GPS coordinate system.
3. The LiDAR and GPS joint calibration method based on planar trajectory constraints as described in claim 1, characterized in that, The coordinate definition correction of the GPS heading angle satisfies ,in, The original heading angle is obtained by solving the quaternion from the GPS trajectory data. This is the corrected GPS heading angle. This indicates that the angle is normalized to... The range; the LiDAR heading angle is calculated from the quaternions in the LiDAR trajectory data, and no further processing is performed. Correction to the form of coordinate definition.
4. The LiDAR and GPS joint calibration method based on planar trajectory constraints as described in claim 1, characterized in that, The time synchronization includes: determining the common time interval of the GPS track and the LiDAR track, retaining only the GPS track within the common time interval, and linearly interpolating the x, y, and yaw of the LiDAR track according to the GPS timestamp.
5. The LiDAR and GPS joint calibration method based on planar trajectory constraints as described in claim 1, characterized in that, The The pose matrix is derived from the planar pose. It is constructed to have the following form: 。 6. The LiDAR and GPS joint calibration method based on planar trajectory constraints as described in claim 1, characterized in that, Predict the pose of LiDAR in the MAP coordinate system to satisfy ,in, This is a transformation from the ENU coordinate system to the MAP coordinate system. This represents the GPS pose in the ENU coordinate system. The installation extrinsic parameters for converting the LiDAR coordinate system to the GPS coordinate system.
7. The LiDAR and GPS joint calibration method based on planar trajectory constraints as described in claim 1, characterized in that, The residual function includes planar position residual and heading angle residual; wherein, the error pose satisfies The planar position residual is The first two terms in the third column of the matrix have the heading angle residuals as follows: Each frame's residual vector consists of the product of the position residual weight and the x-direction position residual, the product of the position residual weight and the y-direction position residual, and the product of the heading angle residual weight and the heading angle residual.
8. The LiDAR and GPS joint calibration method based on planar trajectory constraints as described in claim 1, characterized in that, In nonlinear least squares optimization, boundary constraints are set for the parameters to be calibrated to limit the range of translation and heading angles from the ENU coordinate system to the MAP coordinate system, as well as the range of installation translation and heading angles from the LiDAR coordinate system to the GPS coordinate system. During the optimization process, the middle stable segment of the trajectory is selected to participate in the residual construction to reduce the influence of the trajectory start segment, end segment, interpolation boundary, and sensor start and stop moments on the calibration results.
9. The LiDAR and GPS joint calibration method based on planar trajectory constraints as described in claim 1, characterized in that, After optimization, based on the results obtained... and The LiDAR trajectory is re-predicted, and the root mean square error of the plane position and the root mean square error of the heading angle between the predicted and observed LiDAR trajectories are calculated to evaluate the calibration results.
10. A LiDAR and GPS joint calibration system based on planar trajectory constraints, characterized in that, The system includes a data acquisition module, a pose extraction module, a heading angle correction module, a time synchronization module, a pose construction module, a residual construction module, an optimization solution module, and a verification output module. The data acquisition module acquires GPS and LiDAR trajectory data. The pose extraction module extracts timestamps, planar positions, and heading angles. The heading angle correction module corrects the GPS heading angle. The time synchronization module interpolates the LiDAR trajectory to the GPS timestamp. The pose construction module constructs... The pose matrix; the residual construction module is used to construct the planar position residual and the heading angle residual; the optimization solution module is used to solve the calibration parameters; the verification output module is used to output the calibration results and error statistics results.