Multi-sensor joint calibration method and system based on scale compensation

Abstract

The invention provides a multi-sensor joint calibration method and system based on scale compensation. The method comprises the following steps: collecting synchronous data of a laser radar, a camera and an RTK; obtaining a pose sequence of each sensor through a mileage calculation method, and converting RTK data into ENU local coordinates; performing time synchronization and track registration by adopting a nearest neighbor matching strategy based on a time threshold value; estimating an initial value of an anisotropic scale compensation matrix based on a standard deviation ratio of a displacement increment, estimating an initial value of a rotation matrix through covariance matrix SVD decomposition, and estimating an initial value of a translation vector based on a trajectory point mean value; and constructing a pose increment constraint and a geometric consistency constraint to carry out joint optimization solution, and outputting a sensor external parameter and a scale compensation matrix. According to the invention, the anisotropic scale compensation matrix is introduced to realize three-dimensional attitude alignment, a calibration plate is not needed, a special motion track is not needed, and the problem that the scale drift degrees of the odometer in all directions are different is effectively solved.

Description

Technical Field

[0001] This invention relates to the field of navigation technology, and specifically to a multi-sensor joint calibration method and system based on scale compensation. Background Technology

[0002] Multi-sensor fusion is a key technology for achieving high-precision positioning and environmental perception in autonomous driving and mobile robots. In practical applications, a single sensor often falls short of the positioning requirements in complex environments: LiDAR can provide high-precision 3D point cloud information, suitable for perception and mapping in structured environments, but LiDAR-based odometry suffers from cumulative drift during long-distance operation; cameras can acquire rich texture and semantic information at low cost and with a large amount of information, but monocular vision systems lack absolute scale information, while binocular and RGB-D cameras, although capable of scale recovery, have limited effective range; RTK (Real-Time Kinematics) can provide centimeter-level global positioning information, exhibiting drift-free characteristics in open environments and serving as an absolute position reference. Effectively fusing these multiple sensors can fully leverage their complementary advantages, achieving high-precision and robust positioning and perception capabilities across all scenarios.

[0003] The prerequisite for multi-sensor fusion is obtaining accurate extrinsic parameter relationships between the sensors, i.e., rigid body transformations between different sensor coordinate systems. The accuracy of extrinsic parameter calibration directly affects the overall performance of the fusion system. Traditional extrinsic parameter calibration methods usually require specific calibration objects (such as checkerboard calibration boards, reflective targets, etc.) in a controlled environment, which is cumbersome and difficult to adapt to changes in actual deployment scenarios. In recent years, calibration methods based on calibration objects have gradually gained attention. These methods estimate extrinsic parameters by analyzing the pose changes of sensors during motion, and have the advantages of high flexibility and strong adaptability. Among them, coordinate system alignment is an important technique. By aligning the pose trajectories of different sensors to a unified reference coordinate system, the relative pose relationships between the sensors can be estimated.

[0004] However, existing coordinate system alignment methods have the following shortcomings. First, most methods only consider the alignment of the two-dimensional yaw angle, based on the assumption that the sensor is mounted on a horizontal plane and only has rotational deviation around the vertical axis. However, in practical applications, due to factors such as installation errors, mechanical vibrations, and uneven ground, the sensor's mounting attitude often deviates in the pitch and roll directions. Alignment methods that only consider the yaw angle will introduce systematic errors. Second, existing methods usually assume that the trajectory output by the sensor's odometer has the same scale as the actual trajectory, or use only a single isotropic scale factor for compensation. But in reality, the scale drift generated by the odometer calculation method during long-distance operation often varies in different directions, which is closely related to factors such as environmental structure, motion patterns, and feature distribution. Using an isotropic scale factor is insufficient to accurately describe this direction-dependent scale drift characteristic.

[0005] In summary, the existing technology has the following problems: the coordinate system alignment process of multi-sensor fusion extrinsic parameter calibration has defects, which cannot adapt to the scale drift of the odometer in different directions, affecting the fusion accuracy and robustness. Summary of the Invention

[0006] The purpose of this invention is to address the shortcomings in the coordinate system alignment process of multi-sensor fusion extrinsic parameter calibration, adapt to the scale drift of odometers in different directions, and thus improve the fusion accuracy and robustness.

[0007] To this end, in one aspect, embodiments of the present invention provide a multi-sensor joint calibration method based on scale compensation, the method comprising the following steps:

[0008] Data is collected on a mobile robot platform, simultaneously collecting lidar point cloud data, camera image data, and RTK positioning data to ensure that the data from each sensor has a unified time reference.

[0009] Based on the collected data, the pose sequence in the lidar coordinate system is obtained through lidar odometry, and the pose sequence in the camera coordinate system is obtained through visual odometry. The WGS-84 geodetic coordinates output by the RTK receiver are converted into a global pose sequence in the ENU coordinate system with the first frame position as the origin.

[0010] A nearest neighbor matching strategy based on time threshold is adopted. For each odometry trajectory point, the point with the smallest time difference and which has not been matched is searched in the RTK sequence. When the time difference meets the threshold condition, a pairing relationship is established to obtain a set of registered trajectory point pairs.

[0011] A statistical analysis method based on displacement increments is adopted to calculate the displacement increments of adjacent points between the odometer trajectory and the RTK trajectory. The initial value of the scale compensation matrix is ​​estimated by the ratio of the standard deviations of the displacement increments in each direction. The cross covariance matrix is ​​constructed by the displacement increment sequence and the initial value of the rotation matrix is ​​estimated by singular value decomposition. The initial value estimate of the translation vector is calculated based on the mean of the trajectory points.

[0012] Based on the initial value estimation of the scale compensation matrix, rotation matrix, and translation vector, the lidar-RTK residual and camera-RTK residual are defined based on the pose increment residual constraint, and a multi-sensor geometric consistency constraint is constructed, thereby constructing the overall optimization objective function;

[0013] The overall optimization objective function is solved iteratively using a nonlinear optimization algorithm, which outputs the extrinsic parameters of the lidar to the global coordinate system, the extrinsic parameters of the camera to the global coordinate system, and the scale compensation matrix corresponding to each sensor.

[0014] On the other hand, embodiments of the present invention provide a multi-sensor joint calibration system based on scale compensation, comprising:

[0015] The data acquisition module is used to simultaneously acquire lidar point cloud data, camera image data, and RTK positioning data;

[0016] The pose estimation module is used to obtain the pose sequences of the lidar and camera through odometry and convert the RTK geodetic coordinates into a global pose sequence in the ENU coordinate system.

[0017] The time synchronization module is used to register the odometry trajectory with the RTK trajectory using a nearest neighbor matching strategy based on a time threshold.

[0018] The initial value estimation module is used to estimate the initial value of the scale compensation matrix based on the standard deviation ratio of the displacement increment, and to estimate the initial value of the rotation matrix through the singular value decomposition of the cross covariance matrix.

[0019] The optimization and solution module is used to construct an objective function that includes pose increment residuals and geometric consistency constraints. It is then solved iteratively using a nonlinear optimization algorithm to output the extrinsic parameters of each sensor and the scale compensation matrix.

[0020] The above technical solution has the following beneficial effects:

[0021] This invention is highly adaptable and requires no additional auxiliary conditions: it does not require calibration boards or other auxiliary calibration objects, nor does it require the robot to follow a special motion trajectory. It is flexible in deployment, can adapt to changes in real-world complex scenarios, and reduces operational complexity.

[0022] Covers complete attitude deviations and adapts to any installation scenario: Breaking through the limitations of traditional methods that only align two-dimensional heading angles, it achieves three-dimensional attitude alignment of heading angle, pitch angle, and roll angle. It can handle any installation attitude deviations caused by sensor installation errors, mechanical vibrations, uneven ground, etc., and avoid systematic errors.

[0023] This invention introduces an anisotropic three-dimensional positive definite diagonal scale compensation matrix, which compensates for the scale drift of the odometer in the x, y, and z directions by independent scale factors, effectively solving the problem of inconsistent drift in different directions and improving trajectory alignment accuracy.

[0024] This invention offers high calibration accuracy and robustness. It constructs geometric consistency constraints for three sensors—LiDAR, camera, and RTK—combined with pose increment constraints to form a joint optimization objective. The Levenberg-Marquardt algorithm is used for iterative solution, while the Huber robust kernel function is used to suppress the influence of outliers, ensuring the consistency and reliability of the calibration results.

[0025] This invention enables high-precision joint calibration of multiple sensors, and can ultimately output the extrinsic parameters of LiDAR and camera to the global coordinate system and the scale compensation matrix corresponding to each sensor. This provides a precise coordinate transformation basis for multi-sensor fusion, helping autonomous driving and mobile robots achieve full-scene, high-precision, and highly robust positioning and perception. Attached Figure Description

[0026] Figure 1 This is a flowchart of a multi-sensor joint calibration method based on scale compensation provided in an embodiment of the present invention;

[0027] Figure 2 This is a schematic diagram of a multi-sensor joint calibration system based on scale compensation provided in an embodiment of the present invention;

[0028] Figure 3 This is a flowchart of the first implementation of a multi-sensor joint calibration method based on scale compensation provided by an embodiment of the present invention;

[0029] Figure 4 This is a schematic diagram illustrating the relationship between various coordinate systems provided in the embodiments of the present invention;

[0030] Figure 5 This is a flowchart of a time synchronization and trajectory registration algorithm for a multi-sensor joint calibration method based on scale compensation provided in an embodiment of the present invention;

[0031] Figure 6 This is a schematic diagram of the pre-calibration results of a multi-sensor joint calibration method based on scale compensation provided in an embodiment of the present invention;

[0032] Figure 7This is a schematic diagram of the calibration results of a multi-sensor joint calibration method based on scale compensation provided in an embodiment of the present invention. Detailed Implementation

[0033] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0034] This invention proposes a multi-sensor coordinate system dynamic alignment and joint calibration method based on scale compensation, which mainly includes: First, extending the traditional two-dimensional heading angle alignment to complete three-dimensional attitude alignment, simultaneously estimating the rotational deviations of the three degrees of freedom of heading angle, pitch angle, and roll angle, applicable to sensor configurations with arbitrary installation attitudes; Second, introducing a scale compensation matrix S = diag(s x ,s y ,s z ) is used to describe the scale drift of the sensor's odometer trajectory in each direction, where s x ,s y ,s z The independent scale factors representing the three coordinate axes can more accurately compensate for direction-related scale errors. Third, a multi-sensor geometric consistency constraint is constructed, and the geometric constraint relationship between the three sensors (LiDAR, camera, and RTK) is used to improve the overall accuracy and robustness of the calibration.

[0035] In embodiments of the present invention, such as Figure 1 A multi-sensor joint calibration method based on scale compensation is provided, the method comprising the following steps:

[0036] S101: Data collection is performed on the mobile robot platform, simultaneously collecting lidar point cloud data, camera image data and RTK positioning data to ensure that the data from each sensor has a unified time reference.

[0037] S102: Based on the collected data, obtain the pose sequence in the lidar coordinate system through the lidar odometry, obtain the pose sequence in the camera coordinate system through the visual odometry, and convert the WGS-84 geodetic coordinates output by the RTK receiver into a global pose sequence in the ENU coordinate system with the first frame position as the origin.

[0038] The process of converting the WGS-84 geodetic coordinates output by the RTK receiver into a global pose sequence in the ENU coordinate system with the first frame position as the origin specifically includes:

[0039] Define the WGS-84 ellipsoid parameters and calculate the radii of curvature of the primordial and trochanteric spheres;

[0040] The geodetic coordinates are converted to geocentric rectangular coordinates. Using the RTK position of the first frame as the reference origin, the geocentric rectangular coordinates are converted to ENU local coordinates through a rotation matrix.

[0041] S103: A nearest neighbor matching strategy based on time threshold is adopted. For each odometry trajectory point, the point with the smallest time difference and which has not been matched is searched in the RTK sequence. When the time difference meets the threshold condition, a pairing relationship is established to obtain a set of registered trajectory point pairs.

[0042] The matching of the time threshold satisfies the following conditions:

[0043]

[0044] in, For the timestamp of the odometer track points, For RTK trajectory point timestamps, For the set of unmatched RTK point indices, δ t This is the time synchronization threshold.

[0045] S104: Using a statistical analysis method based on displacement increments, the displacement increments of adjacent points between the odometer trajectory and the RTK trajectory are calculated. The initial value of the scale compensation matrix is ​​estimated by the ratio of the standard deviations of the displacement increments in each direction. The cross covariance matrix is ​​constructed by the displacement increment sequence and the initial value of the rotation matrix is ​​estimated by singular value decomposition. The initial value estimate of the translation vector is calculated based on the mean of the trajectory points.

[0046] The scale compensation matrix is ​​an anisotropic three-dimensional positive definite diagonal matrix, specifically:

[0047] S = diag(s) x ,s y ,s z );

[0048] Where s x ,s y ,s z These represent independent scale factors in the x, y, and z directions of the odometer coordinate system, used to compensate for the inconsistent drift of the odometer in each direction.

[0049] The initial value of the scaling compensation matrix is ​​estimated using the following function:

[0050]

[0051] Where σ(·) is the standard deviation operator, Δx W ,Δy W ,Δz WLet Δx be the displacement increment in the global coordinate system. O ,Δy O ,Δz O This represents the displacement increment in the odometer coordinate system. From this, the initial scale compensation matrix can be constructed:

[0052] The initial value of the rotation matrix is ​​obtained in the following way:

[0053] Construct the cross-covariance matrix of displacement increments Where p O p is the odometer trajectory point. W Let H be the trajectory points in the global coordinate system, and K be the number of successfully matched point pairs. Perform singular value decomposition on H: H = UΣV T The initial value R of the rotation matrix is ​​obtained. (0) =VU T Σ is a diagonal matrix, where U and V are orthogonal matrices. The column vectors of U represent the principal directions of the odometer displacement increments, and the column vectors of V represent the principal directions of the global coordinate system displacement increments. If det(R... (0) If ) < 0, then the last column of V is negative to ensure that the determinant of the rotation matrix is ​​positive.

[0054] Based on the initial values ​​of the scale compensation matrix and rotation matrix, an initial estimate of the translation vector is calculated based on the mean of the trajectory points:

[0055]

[0056] S105: Based on the initial value estimates of the scale compensation matrix, rotation matrix, and translation vector, the lidar-RTK residual and camera-RTK residual are defined based on the pose increment residual constraint to construct multi-sensor geometric consistency constraints, thereby constructing the overall optimization objective function. The initial parameter estimation uses displacement increment, that is, only the change in position information is used for statistical analysis, which is a 3-dimensional vector to reduce computational complexity and improve the numerical stability of the initial value estimation. At this time, the nonlinear optimization uses pose increment, that is, the constraints are constructed simultaneously using position and attitude information, which is a 6-DOF transformation to obtain higher accuracy calibration results.

[0057] The geometric consistency constraint is:

[0058] T LW =T LC ·T CW ;

[0059] Among them, T LW T is the extrinsic parameter for laser radar to reach the global coordinate system. CW T is the extrinsic parameter from the camera to the global coordinate system. LC The extrinsic parameters of the lidar to the camera; the corresponding residual is defined as:

[0060]

[0061] The overall optimization objective function is:

[0062]

[0063] in, Let ε be the set of parameters to be optimized. L ε is the set of edges for the lidar pose increment; C Let be the set of camera pose increment edges; ρ(·) is the robust kernel function, and λ is the weight coefficient of the geometric consistency constraint.

[0064] S106: The overall optimization objective function is solved iteratively using a nonlinear optimization algorithm, and the external parameters of the laser radar to the global coordinate system, the external parameters of the camera to the global coordinate system, and the scale compensation matrix corresponding to each sensor are output.

[0065] The pose increment residual is defined using the logarithmic mapping of a Lie group.

[0066] The lidar-RTK residual is:

[0067]

[0068] in, For the pose increment of the lidar odometry, T represents the pose increment in the global coordinate system. LW S is the extrinsic parameter of the laser radar in the global coordinate system. L This is the scaling compensation matrix.

[0069] Example:

[0070] The core problem this invention aims to solve is: determining the transformation relationship between the odometer coordinate system and the RTK global coordinate system. Due to the following factors, there are rotations, translations, and scale offsets between the two coordinate systems, requiring calibration to determine the complete transformation parameters:

[0071] Rotational deviation: The sensor was not perfectly aligned with the carrier coordinate system during installation, and the robot's initial orientation had an angle with true north, resulting in a three-dimensional attitude deviation between the odometry coordinate system and the global coordinate system. Unlike traditional methods that only consider the two-dimensional heading angle, this invention simultaneously estimates the complete three-dimensional rotation (heading angle, pitch angle, and roll angle), enabling it to handle arbitrary sensor installation orientations.

[0072] Scale drift: Odometry experiences scale drift during long-distance travel due to accumulated errors in point cloud matching. This invention employs anisotropic scale compensation, estimating independent scale factors in three directions to address inconsistent drift levels across different directions.

[0073] Translational offset: The physical distance between the sensor installation position and the RTK antenna (lever effect), as well as the non-coincidence of the first frame positions of each sensor, cause an offset between the origins of the coordinate system.

[0074] Example:

[0075] The core problem this invention aims to solve is: determining the transformation relationship between the odometer coordinate system and the RTK global coordinate system. Due to the following factors, there are rotations, translations, and scale offsets between the two coordinate systems, requiring calibration to determine the complete transformation parameters:

[0076] 1) Rotational Deviation: The sensor was not perfectly aligned with the carrier coordinate system during installation, and the robot's initial orientation had an angle with true north, resulting in a three-dimensional attitude deviation between the odometry coordinate system and the global coordinate system. Unlike traditional methods that only consider the two-dimensional heading angle, this invention simultaneously estimates the complete three-dimensional rotation (heading angle, pitch angle, and roll angle), which can handle situations where the sensor is installed in any orientation.

[0077] 2) Scale drift: Odometry experiences scale drift during long-distance travel due to accumulated errors in point cloud matching. This invention employs anisotropic scale compensation, estimating independent scale factors in three directions to address inconsistent drift levels across different directions.

[0078] 3) Translational offset: The physical distance between the sensor installation position and the RTK antenna (lever effect), as well as the non-coincidence of the first frame positions of each sensor, result in an offset between the origins of the coordinate system.

[0079] like Figure 3 This invention provides a multi-sensor joint calibration method based on scale compensation. The invention relates to three coordinate systems, such as... Figure 4 The definitions and physical meanings of each coordinate system are as follows:

[0080] The RTK global coordinate system {W} uses the ENU (East-North-Up) coordinate system as the global reference coordinate system. This system uses the first frame's RTK position as its origin, with the E-axis pointing due east, the N-axis pointing due north, and the U-axis pointing towards the zenith, forming a right-handed coordinate system. The ENU coordinate system is a commonly used local tangent plane coordinate system in geodesy and navigation, facilitating the description of position and movement within a local area.

[0081] The lidar coordinate system {L} adopts the standard FLU (Front-Left-Up) coordinate system used in robotics. The X-axis points in the robot's forward direction, the Y-axis points to the robot's left, and the Z-axis points upward, forming a right-handed coordinate system. The pose output by the laser odometry is defined in this coordinate system.

[0082] Camera coordinate system {C}: It adopts the same definition as the lidar coordinate system, and the pose output by the visual odometry method is defined in this coordinate system.

[0083] Calibration Model: Based on the problem description, the calibration model includes rotations, translations, and scale offsets between the coordinate systems. First, the scale compensation matrix is ​​defined as a three-dimensional positive definite diagonal matrix:

[0084]

[0085] Where s x ,s y ,s z These represent independent scale factors in the x, y, and z directions, respectively, characterizing the scale drift of the odometry trajectory relative to the actual trajectory.

[0086] Secondly, the rotation relationship between the coordinate systems is parameterized using a three-dimensional rotation matrix. Since Euler angles have a clear physical meaning in the navigation field and are easy to understand and debug, the three-dimensional rotation matrix R∈SO(3) is parameterized using the ZYX Euler angle sequence:

[0087] R(ψ,θ,φ)=R z (ψ)·R y (θ)·R x (φ)

[0088] The yaw angle ψ represents the rotation angle around the Z-axis, describing the angular deviation between the robot's initial orientation and true north in the horizontal plane. The pitch angle θ represents the rotation angle around the Y-axis, describing the robot's forward and backward tilt relative to the horizontal plane. The roll angle φ represents the rotation angle around the X-axis, describing the robot's left and right tilt relative to the horizontal plane. The specific forms of each basic rotation matrix are as follows:

[0089] Combining scale and rotation matrices, the scale-compensated similarity transformation from the ENU coordinate system to the sensor coordinate system can be defined as follows:

[0090]

[0091] Where R∈SO(3) is a rotation matrix that satisfies the orthogonality constraint R T R = I and determinant constraint det(R) = 1; is the translation vector; S is the scale compensation matrix. After obtaining the parameters through calibration, the pose output by the odometer can be unified to the global coordinate system, realizing coordinate alignment and fusion of multi-sensor positioning data. For a three-dimensional point p = [x, y, z] T The transformation relationship is as follows:

[0092] p O =RSp W +t;

[0093] Where p W =[x W ,y W ,z W ] T p is a point in the ENU coordinate system. O =[x O ,y O ,z O ] T This refers to the corresponding point in the odometer coordinate system (LiDAR or vision). Expanded into component form:

[0094]

[0095] Where r ij Let [t] be the element in the i-th row and j-th column of the rotation matrix R. x ,t y ,t z ] T It is a translation vector.

[0096] Data preprocessing: Based on the established calibration model, this invention adopts a method for acquiring and preprocessing multi-source pose data to provide input data for subsequent optimization solutions.

[0097] RTK coordinate transformation: The raw positioning data output by the RTK receiver is latitude, longitude, and ellipsoidal height in the WGS-84 geodetic coordinate system. in λ is the geodetic latitude, λ is the geodetic longitude, and h is the ellipsoidal height. To align with the sensor odometer trajectory, it needs to be converted to the ENU local coordinate system with the first frame position as the origin.

[0098] The basic parameters of the WGS-84 ellipsoid are defined as follows:

[0099] a = 6378137.0m, e 2 =2f-f 2 ≈0.00669438

[0100] Where a is the semi-major axis of the reference ellipsoid, f is the flattening, and e is the semi-major axis of the reference ellipsoid. 2 Let be the square of the first eccentricity. The formula for calculating the radius of curvature perpendicular to the meridian plane at a point on the ellipsoid is:

[0101]

[0102] From this, we can obtain the transformation function from geodetic coordinates to geocentric rectangular coordinates:

[0103]

[0104] Select the first frame RTK position As the origin (reference point) of the ENU coordinate system. From the geodetic coordinates of any point... coordinates p in the ENU coordinate system W =[x W ,y W ,z W ] T The conversion formula is:

[0105]

[0106] Among them, R geo The rotation matrix from the geocentric rectangular coordinate system to the ENU coordinate system is expressed as follows:

[0107]

[0108] This rotation matrix converts the coordinate difference of the geocentric rectangular coordinate system into ENU coordinates at the reference point. Its physical meaning is to rotate and align the global coordinate system to the local horizontal plane at the reference point.

[0109] Pose data acquisition and processing: Assume a total of N pose data points are acquired. The lidar odometry uses a laser SLAM algorithm (such as LOAM, LIO-SAM, etc.) to obtain the pose sequence.

[0110]

[0111] Similarly, visual odometry obtains pose sequences through visual SLAM algorithms (such as ORB-SLAM, VINS, etc.):

[0112]

[0113] Where R∈SO(3) is the rotation matrix, These are translation vectors, each with its own first frame pose as the reference origin. Due to the cumulative drift characteristics of the odometry system, the pose sequence exhibits scale drift and attitude deviation, which need to be compensated for through subsequent calibration.

[0114] After the coordinate transformation described in Section 2.1, the raw RTK data yields the RTK pose sequence in the ENU coordinate system:

[0115]

[0116] Among them, the translation component The rotation component is directly obtained from the latitude, longitude, and altitude coordinates output by RTK. The pose can be deduced from the motion direction of adjacent positions or determined with the assistance of the inertial measurement unit. The global pose sequence serves as a reference for the calibration process, constraining the scale and attitude of the odometry pose.

[0117] Time synchronization and trajectory registration: Due to differences in sampling frequency and triggering time between the odometer and the RTK system, such as Figure 5 This invention uses a nearest neighbor matching strategy based on a time threshold to register trajectory points. Compared with pose interpolation methods, the nearest neighbor matching strategy used in this invention has the following advantages: (1) it avoids numerical errors introduced by interpolation operations; (2) it preserves the authenticity of the original measurement data; (3) it has low computational complexity and is suitable for large-scale trajectory data processing. The specific implementation is as follows:

[0118] Odometer trajectory sequence is RTK trajectory sequence is Where τ represents the timestamp, and p = (x, y, z) T This represents the three-dimensional position coordinates. For each odometry trajectory point i, the optimal matching point that satisfies the following conditions is searched in the RTK sequence:

[0119]

[0120] in This represents the set of RTK point indices that have not yet been matched. Only if the minimum time difference meets the threshold condition... When the pairing relationship is established, δ t The preset time synchronization threshold is typically 0.05 seconds. This threshold setting needs to consider both the sensor sampling frequency and the carrier's speed. To ensure unique registration, each RTK trajectory point is associated with at most one odometer trajectory point. Once RTK point j... * If a match is successfully made, the candidate is removed from the candidate set. After the above time synchronization process, a set of registered trajectory point pairs is obtained. Where K is the number of successfully matched point pairs. This registration result can be directly used for subsequent coordinate system transformation parameter estimation.

[0121] Joint optimization and solution: such as Figure 6 , Figure 7 As shown, based on the established calibration model and the acquired multi-source pose data, this invention constructs a joint optimization problem to solve for the calibration parameters. Through pose increment constraints and geometric consistency constraints, the joint estimation of the multi-sensor external participation scale factor is achieved.

[0122] Nonlinear optimization constraints and objective function: Pose increment describes the relative motion of the sensor between two time points. The pose increment from frame i to frame j is defined as ΔT. i,j =(Ti ) -1 ·T j The pose increment eliminates the influence of the selection of the origin of each sensor coordinate system, reflecting only the relative motion information of the sensors. Constraints constructed based on the pose increment exhibit better numerical stability for extrinsic parameter estimation. Ideally, the global pose increment, after extrinsic parameter transformation and scale compensation, should be consistent with the odometry pose increment. However, since scale drift mainly affects the translation component, the scale effect on the pose increment is defined as applying a scale transformation only to the translation component, while keeping the rotation component unchanged. Based on this, the LiDAR-RTK residual is defined as:

[0123]

[0124] in For the pose increment of the lidar odometry, T represents the pose increment in the global coordinate system. LW S is the extrinsic parameter from the lidar coordinate system to the global coordinate system. L Let be the scale compensation matrix of the lidar in section 1.3, log(·) ∨ The logarithmic mapping from Lie groups to Lie algebras maps pose errors to a six-dimensional vector space to facilitate computational optimization.

[0125] Similarly, the camera-RTK residual is defined as:

[0126]

[0127] in For the pose increment of visual odometry, T CW S is the extrinsic parameter from the camera coordinate system to the global coordinate system. C This is the camera's scale compensation matrix.

[0128] The extrinsic parameter relationships between the three sensors—LiDAR, camera, and RTK—should satisfy geometric consistency constraints. Let T... LW T is the extrinsic parameter for laser radar to reach the global coordinate system. CW T is the extrinsic parameter from the camera to the global coordinate system. LC For the extrinsic parameters of the laser radar to the camera, the following should be satisfied:

[0129] T LW =T LC ·T CW

[0130] The physical meaning of this constraint is: the result obtained from transforming from the lidar coordinate system to the global coordinate system, whether through a direct transformation or via the camera coordinate system, should be consistent. The geometric consistency constraint residual is defined as:

[0131]

[0132] Considering the above constraints, the overall optimization objective function is constructed as follows:

[0133]

[0134] in Let ε be the set of parameters to be optimized. L and ε C Let be the sets of pose increment edges for the LiDAR and the camera, respectively, and ρ(·) be the Huber robust kernel function used to suppress the influence of outliers. λ is the Mahalanobis distance, and λ1 is the weight coefficient of the geometric consistency constraint.

[0135] Initial parameter estimation: To ensure the convergence of nonlinear optimization, reasonable initial parameter values ​​are required. This invention employs a statistical analysis method based on displacement increments to estimate the initial values ​​of the scaling matrix, rotation matrix, and translation vector. This method achieves parameter estimation by analyzing the local motion characteristics of the trajectory and exhibits good numerical stability.

[0136] First, calculate the displacement increment of adjacent points on the odometer trajectory and the RTK trajectory:

[0137]

[0138] Where p O p is the odometer trajectory point. W These are trajectory points in the global coordinate system. The displacement increment reflects the local motion of the carrier between adjacent moments, naturally eliminating the influence of the coordinate origin offset.

[0139] Next, the initial values ​​for the scale in each direction are estimated. The scale factor is estimated by comparing the statistical dispersion of the displacement increments of the two trajectories in each coordinate direction.

[0140]

[0141] Where σ(·) represents the standard deviation operator, and its calculation formula is:

[0142]

[0143] The mean of the displacement increments is used, and the calculation method is similar for each direction. This method utilizes the distribution characteristics of displacement increments to reflect the amplitude of trajectory movement in each direction. Compared to the range statistic, the standard deviation has a stronger ability to suppress outliers. Therefore, a scale compensation matrix can be constructed:

[0144] Next, we need to estimate the initial value of the rotation matrix and construct the cross-covariance matrix using the displacement increment sequence:

[0145]

[0146] Perform singular value decomposition on H: H = UΣV T Σ is a diagonal matrix, where U and V are orthogonal matrices. The column vectors of U represent the principal directions of the odometer displacement increments, and the column vectors of V represent the principal directions of the global coordinate system displacement increments. The initial value of the rotation matrix is ​​obtained through decomposition as R. (0) =VU T ,like Then the last column of V is negative to ensure that the determinant of the rotation matrix is ​​positive.

[0147] Based on the initial values ​​of the scale compensation matrix and rotation matrix, an initial estimate of the translation vector is calculated based on the mean of the trajectory points:

[0148]

[0149] The above-mentioned initial value estimation method based on displacement increments makes full use of the local motion characteristics of the trajectory, avoids dependence on the global coordinate distribution, and provides a reliable initial solution for subsequent nonlinear optimization.

[0150] Optimized Solution: The Levenberg-Marquardt algorithm is used to iteratively solve the above nonlinear least squares problem. The specific steps are as follows:

[0151] Initialize the parameter Θ using the initial values ​​obtained above. (0) ;

[0152] In the current parameter Θ (k) Calculate the Jacobian matrix J and the residual vector r at the specified location;

[0153] Solving the damping normal equation yields the parameter increment (J). T Σ -1 J+μI)ΔΘ=-J T Σ -1 r, where μ > 0 is the damping factor used to balance gradient descent and the Gauss-Newton method;

[0154] Update parameters on the manifold in This indicates an exponential mapping update operation on the manifold, where the pose parameters on SE(3) are updated using Lie algebra parameterization;

[0155] Determine the convergence condition. If ||ΔΘ|| < ∈ or the iteration number k > k max If the iteration terminates, the result will be output.

[0156] In embodiments of the present invention, such as Figure 2 Furthermore, a multi-sensor joint calibration system based on scale compensation is provided, including:

[0157] Data acquisition module 21 is used to simultaneously acquire lidar point cloud data, camera image data and RTK positioning data;

[0158] The pose estimation module 22 is used to obtain the pose sequences of the lidar and camera through the odometry method and convert the RTK geodetic coordinates into a global pose sequence in the ENU coordinate system.

[0159] The time synchronization module 23 is used to register the odometry trajectory and the RTK trajectory using a nearest neighbor matching strategy based on a time threshold.

[0160] The initial value estimation module 24 is used to estimate the initial value of the scale compensation matrix based on the standard deviation ratio of the displacement increment, and to estimate the initial value of the rotation matrix through the singular value decomposition of the cross covariance matrix.

[0161] The optimization and solution module 25 is used to construct an objective function that includes pose increment residuals and geometric consistency constraints. It is solved iteratively through a nonlinear optimization algorithm and outputs the extrinsic parameters of each sensor and the scale compensation matrix.

[0162] A scale-compensated multi-sensor joint calibration system employs the aforementioned scale-compensated multi-sensor joint calibration method. Its principle and process are the same as those of the scale-compensated multi-sensor joint calibration method, and will not be repeated here.

[0163] This invention eliminates the need for auxiliary calibration materials such as calibration boards and requires no special motion trajectory requirements; it adapts to any sensor mounting posture through three-dimensional attitude alignment; it employs an anisotropic scale compensation matrix to effectively solve the problem of different degrees of scale drift in different directions of the odometer; it improves calibration accuracy and consistency through multi-sensor extrinsic parameter chain constraints; and it achieves high-precision joint calibration of three sensors: lidar, camera, and RTK.

[0164] It should be understood that the specific order or hierarchy of steps in the disclosed process is an example of an exemplary method. Based on design preferences, it should be understood that the specific order or hierarchy of steps in the process may be rearranged without departing from the scope of this disclosure. The appended method claims provide elements of various steps in an exemplary order and are not intended to limit the scope to the specific order or hierarchy described.

[0165] The specific embodiments described above further illustrate the purpose, technical solution, and beneficial effects of the present invention. It should be understood that the above description is only a specific embodiment of the present invention and is not intended to limit the scope of protection of the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A multi-sensor joint calibration method based on scale compensation, characterized in that, The method includes the following steps: Data is collected on a mobile robot platform, simultaneously collecting lidar point cloud data, camera image data, and RTK positioning data to ensure that the data from each sensor has a unified time reference. Based on the collected data, the pose sequence in the lidar coordinate system is obtained through lidar odometry, and the pose sequence in the camera coordinate system is obtained through visual odometry. The WGS-84 geodetic coordinates output by the RTK receiver are converted into a global pose sequence in the ENU coordinate system with the first frame position as the origin. A nearest neighbor matching strategy based on time threshold is adopted. For each odometry trajectory point, the point with the smallest time difference and which has not been matched is searched in the RTK sequence. When the time difference meets the threshold condition, a pairing relationship is established to obtain a set of registered trajectory point pairs. A statistical analysis method based on displacement increments is adopted to calculate the displacement increments of adjacent points between the odometer trajectory and the RTK trajectory. The initial value of the scale compensation matrix is ​​estimated by the ratio of the standard deviations of the displacement increments in each direction. The cross covariance matrix is ​​constructed by the displacement increment sequence and the initial value of the rotation matrix is ​​estimated by singular value decomposition. The initial value estimate of the translation vector is calculated based on the mean of the trajectory points. Based on the initial value estimation of the scale compensation matrix, rotation matrix, and translation vector, the lidar-RTK residual and camera-RTK residual are defined based on the pose increment residual constraint, and a multi-sensor geometric consistency constraint is constructed, thereby constructing the overall optimization objective function; The overall optimization objective function is solved iteratively using a nonlinear optimization algorithm, which outputs the extrinsic parameters of the lidar to the global coordinate system, the extrinsic parameters of the camera to the global coordinate system, and the scale compensation matrix corresponding to each sensor.

2. The multi-sensor joint calibration method based on scale compensation according to claim 1, characterized in that, The scale compensation matrix is ​​an anisotropic three-dimensional positive definite diagonal matrix, specifically: S=diag(s x ,s y ,s z ); Where s x ,s y ,s z These represent independent scale factors in the x, y, and z directions of the odometer coordinate system, used to compensate for the inconsistent drift of the odometer in each direction.

3. The multi-sensor joint calibration method based on scale compensation according to claim 1, characterized in that, The process of converting the WGS-84 geodetic coordinates output by the RTK receiver into a global pose sequence in the ENU coordinate system with the first frame position as the origin specifically includes: Define the WGS-84 ellipsoid parameters and calculate the radii of curvature of the primordial and trochanteric spheres; The geodetic coordinates are converted to geocentric rectangular coordinates. Using the RTK position of the first frame as the reference origin, the geocentric rectangular coordinates are converted to ENU local coordinates through a rotation matrix.

4. The multi-sensor joint calibration method based on scale compensation according to claim 1, characterized in that, The matching of the time threshold satisfies the following conditions: in, For the timestamp of the odometer track points, For RTK trajectory point timestamps, For the set of unmatched RTK point indices, δ t This is the time synchronization threshold.

5. The multi-sensor joint calibration method based on scale compensation according to claim 1, characterized in that, The initial value of the scaling compensation matrix is ​​estimated using the following function: Where σ(·) is the standard deviation operator, Δx W ,Δy W ,Δz W Let Δx be the displacement increment in the global coordinate system. O ,Δy O ,Δz O The displacement increment in the odometer coordinate system is used to construct the initial scale compensation matrix:

6. The multi-sensor joint calibration method based on scale compensation according to claim 1, characterized in that, The initial value of the rotation matrix is ​​obtained in the following way: Construct the cross-covariance matrix of displacement increments Where p O p is the odometer trajectory point. W Let H be the trajectory points in the global coordinate system, and K be the number of successfully matched point pairs; perform singular value decomposition on H: H = UΣV T The initial value R of the rotation matrix is ​​obtained. (0) =VU T Σ is an orthogonal matrix, where U and V are orthogonal matrices and Σ is a diagonal matrix; the column vectors of U represent the principal directions of the odometer displacement increments, and the column vectors of V represent the principal directions of the global coordinate system displacement increments. Based on the initial values ​​of the scale compensation matrix and rotation matrix, an initial estimate of the translation vector is calculated based on the mean of the trajectory points: Among them, S (0) This is the initial scale compensation matrix.

7. The multi-sensor joint calibration method based on scale compensation according to claim 1, characterized in that, The pose increment residual is defined using the logarithmic mapping of a Lie group. The lidar-RTK residual is: in, For the pose increment of the lidar odometry, T represents the pose increment in the global coordinate system. LW S is the extrinsic parameter of the laser radar in the global coordinate system. L This is the scaling compensation matrix.

8. The multi-sensor joint calibration method based on scale compensation according to claim 1, characterized in that, The geometric consistency constraint is: T LW =T LC ·T CW ; Among them, T LW T is the extrinsic parameter for laser radar to reach the global coordinate system. CW T is the extrinsic parameter from the camera to the global coordinate system. LC The extrinsic parameters of the lidar to the camera; the corresponding residual is defined as:

9. The multi-sensor joint calibration method based on scale compensation according to claim 1, characterized in that, The overall optimization objective function is: in, Let ε be the set of parameters to be optimized. L ε is the set of edges for the lidar pose increment; C Let be the set of camera pose increment edges; ρ(·) is the robust kernel function, and λ is the weight coefficient of the geometric consistency constraint.

10. A multi-sensor joint calibration system based on scale compensation, characterized in that, include: The data acquisition module is used to simultaneously acquire lidar point cloud data, camera image data, and RTK positioning data; The pose estimation module is used to obtain the pose sequences of the lidar and camera through odometry and convert the RTK geodetic coordinates into a global pose sequence in the ENU coordinate system. The time synchronization module is used to register the odometry trajectory with the RTK trajectory using a nearest neighbor matching strategy based on a time threshold. The initial value estimation module is used to estimate the initial value of the scale compensation matrix based on the standard deviation ratio of the displacement increment, and to estimate the initial value of the rotation matrix through the singular value decomposition of the cross covariance matrix. The optimization and solution module is used to construct an objective function that includes pose increment residuals and geometric consistency constraints. It is solved iteratively through a nonlinear optimization algorithm, and outputs the extrinsic parameters of each sensor and the scale compensation matrix.

Images