Parameter adaptive joint calibration method and system based on double radar and differential chassis

Abstract

The application relates to the technical field of mobile robots and discloses a parameter self-adaptive joint calibration method and system based on a double radar and a differential chassis, which comprises the following steps: acquiring a pose output by a laser odometer in real time, synchronizing wheel speed calculation displacement based on wheel speed with a laser observation track based on the pose by registering wheel speed, constructing a rigid body chain between a laser radar coordinate system and a vehicle body center coordinate system through an SE(3) homogeneous transformation matrix, eliminating singularity of rotation representation by using Lie algebra logarithm mapping, ensuring that the radar external parameter is optimally matched with the kinematics of the differential chassis in the whole space scale, and evaluating the reliability of a calibration parameter set solved in a current window through a Jacobian matrix and selectively performing incremental updating of global parameters. The application can improve calibration energy efficiency and robustness, realize full parameter decoupling and high-precision consistency under complex motion conditions.

Description

Technical Field

[0001] This invention relates to the field of mobile robot technology, and in particular to a parameter adaptive joint calibration method and system based on dual radars and a differential chassis. Background Technology

[0002] Light Detection and Ranging (LiDAR) is a core sensor for environmental perception and localization / navigation. In the early applications of mobile robots, single-LiDAR solutions were common. However, as operational scenarios became more complex, the limitations of single-LiDAR in terms of field of view coverage, blind spot elimination, and localization robustness became increasingly apparent. To expand the perception range and eliminate blind spots, multi-LiDAR redundancy configurations have gradually become the mainstream technological trend. However, the effective operation of multi-LiDAR systems highly depends on high-precision extrinsic parameter calibration, i.e., determining the precise pose transformation relationships between each sensor and relative to the vehicle's coordinate system.

[0003] Traditional calibration methods primarily rely on offline environments, typically requiring specialized personnel to perform the calibration in a controlled setting using specific calibration targets such as checkerboard patterns, corner reflectors, or artificial markers. This approach is not only costly and inefficient, but the calibration results are also one-time parameters, unable to account for physical pose shifts caused by mechanical vibrations, collisions, or long-term use during robot operation. Therefore, online self-calibration technology without external references has become a hot research topic in the industry, aiming to automatically complete parameter calibration using trajectory information generated during normal robot operation.

[0004] In common mobile platforms such as differential chassis, traditional calibration often separates radar extrinsic parameter estimation from chassis kinematic parameters, treating wheel diameter and reduction ratio separately. However, in practical applications, it has been found that the accuracy of wheel speed odometers is limited by reduction ratio drift and tire wear, and there is a high degree of coupling between odometer error and radar extrinsic parameter error. Existing calibration methods mainly focus on three aspects: multi-sensor extrinsic parameter calibration, differential chassis kinematic modeling, and motion-based online self-calibration. Typical methods include offline extrinsic parameter calibration based on markers and online extrinsic parameter estimation methods based on hand-eye calibration principles.

[0005] Offline extrinsic parameter calibration based on markers is currently the most traditional calibration method in the field of mobile robotics. It primarily utilizes objects with specific geometric features or high-contrast textures as intermediaries to establish spatial relationships between sensors. This technique is typically performed in a controlled laboratory environment, using checkerboard patterns, corner reflectors, or 3D calibration frames. During calibration, multiple lidars simultaneously scan these markers, extracting their feature points in their respective coordinate systems. By minimizing the point-to-point or point-to-surface registration residuals, the relative rotation and translation between the lidars are solved. Online extrinsic parameter estimation methods based on the hand-eye calibration principle are mainly based on classical equations. The principle is as follows: During a certain period of motion, the pose increment of radar A is calculated through laser point cloud matching. ) and the pose increment of radar B ( If the radars satisfy rigid body constraints, then a transformation matrix exists. satisfy By collecting multiple sets of motion segments in different directions, singular value decomposition or nonlinear optimization can be used to solve for... The initial value of .

[0006] However, existing calibration methods also have some problems, including: 1. Data sampling is limited by time triggering, leading to low signal-to-noise ratio data pollution: Existing technologies typically employ time-interval sampling or full-data batch processing. During robot stationary, extremely low-speed, or frequent start-stop processes, time-triggered mechanisms collect a large amount of redundant data lacking motion excitation. This data contains a high proportion of sensor observation noise; forcibly incorporating it into the optimization equations can result in singular or ill-conditioned solutions in the calibration matrix, severely interfering with the convergence accuracy of the parameters.

[0007] 2. The kinematic model is overly simplified, exhibiting rotational singularities and parameter coupling: Existing solutions mostly perform calculations in 2D Euclidean space through linear fitting or decoupling steps. Current technologies often use Euler angles or simple 2D trigonometric functions to handle rotational transformations. When the robot is on uneven ground or experiences 3D disturbances, the lack of... Manifold constraints can easily lead to rotational singularities, resulting in distortion of the pose increment projection. Existing technologies often calibrate wheel speedometer intrinsic parameters and radar extrinsic parameters separately, neglecting the strong coupling relationship between wheel diameter wear, reduction ratio offset, and spatial installation pose. This causes residual errors from step-by-step calibration to accumulate continuously during long-term system operation.

[0008] 3. Lack of parameter uncertainty measurement leads to update divergence: Most existing online self-calibration methods rely solely on simple residual thresholds as update criteria. In feature-sparse environments such as long corridors and open spaces, although the residuals may be small, the parameters are unobservable in certain dimensions. Existing technologies cannot quantify this uncertainty, leading to erroneous calibration results directly overwriting historically high-precision parameters. The lack of a reliable confidence assessment mechanism renders the system incapable of self-awareness when faced with sudden noise or dynamic obstacle interference, easily causing the overall collapse of the online calibration system. Summary of the Invention

[0009] Therefore, the technical problem to be solved by the present invention is to overcome the shortcomings of the prior art and provide a parameter adaptive joint calibration method and system based on dual radar and differential chassis, which can improve calibration efficiency and robustness, and achieve full parameter decoupling and high-precision consistency under complex motion conditions.

[0010] To address the aforementioned technical problems, this invention provides a parameter adaptive joint calibration method based on dual radars and a differential chassis, comprising: The pose output by the laser odometer is acquired in real time, and the registration wheel speed at the corresponding moment is calculated by linear interpolation, so that the displacement calculated based on the wheel speed and the laser observation trajectory based on the pose achieve statistical consistency. pass The homogeneous transformation matrix constructs a rigid body chain between the lidar coordinate system and the vehicle center coordinate system. Lie algebraic logarithmic mapping is used to eliminate the singularity of the rotation representation, ensuring that the radar extrinsic parameters achieve optimal matching with the differential chassis kinematics across the entire spatial scale. The reliability of the calibration parameter set obtained within the current window is evaluated by the Jacobian matrix. When the calibration parameter set obtained within the current window meets the preset conditions, the incremental update of the global parameters is triggered; otherwise, the calibration parameter set obtained within the current window is discarded and the process returns to the step of obtaining the pose of the laser odometry output in real time.

[0011] Furthermore, the step of calculating the registration wheel speed at the corresponding moment of pose using linear interpolation, so that the displacement calculated based on the wheel speed and the laser observation trajectory based on the pose achieve statistical consistency, includes: Record No. The coordinates of the frame laser odometry in the global coordinate system are: , For the first The x-coordinate of the frame laser odometry in the global coordinate system. No. The y-coordinate of the frame laser odometry in the global coordinate system is calculated by accumulating discrete Euclidean distances to determine the current laser observation trajectory. When the laser observation trajectory satisfies At that time, extract the laser pose sequence and wheel speed sequence from the current buffer. For laser observation trajectory, Preset spatial threshold; For the captured laser pose sequence and wheel speed sequence, the registration wheel speed at the corresponding moment of pose is calculated by linear interpolation. exist Within the trajectory range, the wheel speed is estimated by adjusting the equivalent wheel radius to be calibrated based on discrete wheel speed samples, so that the estimated wheel speed displacement is synchronized with the laser observation trajectory, and the estimated wheel speed displacement is used as the displacement prediction of the differential chassis motion center.

[0012] Furthermore, the step of calculating the registration wheel speed at the corresponding moment of the pose using linear interpolation for the extracted laser pose sequence and wheel speed sequence includes: For any laser frame timestamp of the extracted laser pose sequence, denoted as Find adjacent timestamps in the extracted wheel speed sequence and denote them as follows: and ,satisfy The registration wheel speed at the corresponding moment is calculated using linear interpolation: , In the formula, for Registration wheel speed at any moment for Registration wheel speed at any moment for The registered wheel speed at any given moment.

[0013] Furthermore, the step of calculating the displacement based on discrete wheel speed samples by adjusting the equivalent wheel radius to be calibrated, so that the calculated displacement is synchronized with the laser observation trajectory, includes: The displacement calculated based on the wheel speed is as follows: , In the formula, To calculate displacement based on wheel speed, In order to be in The total number of samples within the trajectory interval. The equivalent wheel radius to be calibrated is... For the first The left wheel angular velocity at each wheel speed sampling point Let ω be the angular velocity of the right wheel at the j-th wheel speed sampling point. For a preset, tiny time slice; By adjusting This allows the displacement to be calculated based on the wheel speed. With laser observation trajectory To achieve the greatest possible statistical consistency.

[0014] Furthermore, the aforementioned through The homogeneous transformation matrix constructs a rigid body chain between the lidar coordinate system and the vehicle center coordinate system, including: Define the extrinsic transformation matrix from lidar A to lidar B as follows: , , for Given the homogeneous transformation matrix, the local pose increments observed by lidar A and lidar B satisfy the following rigid body constraint equations: , In the formula, Let A be the local pose increment observed by lidar A at time k. Let be the local pose increment observed by lidar B at time k; The motion of the differential chassis center is uniquely determined by the angular velocity and linear velocity provided by the wheel speed gauges. To integrate wheel speed information into the three-dimensional calibration framework, The instantaneous velocity vector in the Lie algebra space is projected onto the exponential map. The pose transformation matrix is ​​obtained as follows: , In the formula, Let be the pose transformation matrix of the differential chassis center at time k, and exp be the natural exponent. Let k be the linear velocity of the differential chassis center at time k. Let k be the angular velocity of the differential chassis center at time k. and Contains the equivalent wheel radius to be calibrated , For the hat operator, The sampling time interval; exist Within the trajectory interval, for each locally matched sample, the linear constraint residual is established as: , In the formula, For the first The linear constraint residuals of a locally matched sample The first time measured by laser odometry The rotation angle increment of each locally matched sample The equivalent wheel radius to be calibrated is... For the first The left wheel angular velocity at each wheel speed sampling point Let ω be the angular velocity of the right wheel at the j-th wheel speed sampling point. The wheelbase is the distance between the left and right wheels. The sampling time interval for the j-th wheel speed; The equivalent wheel radius to be calibrated is achieved by minimizing the sum of squares of the linear constraint residuals of all local matching samples, while simultaneously correcting the scaling factor of the wheel speed gauge.

[0015] Furthermore, the method of using Lie algebraic logarithmic mapping to eliminate the singularity of the rotation representation specifically involves multi-objective fusion of the linear constraint residuals and rigid body constraint equations, extracting the residual vector using Lie algebraic logarithmic mapping, and ultimately ensuring that the radar extrinsic parameters achieve optimal matching with the differential chassis kinematics across the entire spatial scale.

[0016] Furthermore, the method of evaluating the reliability of the calibration parameter set obtained within the current window using the Jacobian matrix includes: Define the set of calibration parameters obtained from solving within the current window as follows: , ,in, This represents the equivalent lateral displacement of the radar extrinsic pose in a planar motion model. This represents the equivalent longitudinal displacement of the radar extrinsic pose in a planar motion model. The heading angle of the radar extrinsic pose in the planar motion model. For the equivalent wheel radius after calibration, T is the transpose operation; for the linear constraint residual, a small perturbation is applied near the calibration parameter set obtained in the current window using the five-point central difference method to construct the system Jacobian matrix; Combining the information matrix from laser scanning matching and the system Jacobian matrix, the system's information matrix is ​​constructed as follows: , In the formula, For the system's information matrix, Let Jacobian matrix be the system's Jacobian matrix. Information matrix for laser scanning; By inverting the information matrix of the system, the covariance matrix of the state variables is obtained as follows: , In the formula, Let be the covariance matrix of the state variables. The square root of the diagonal element of the covariance matrix of the state variables is the standard deviation of each calibration parameter in the calibration parameter set obtained within the current window. If the standard deviation of each calibration parameter is small, it indicates high reliability.

[0017] Furthermore, the method for calculating the elements in the system Jacobian matrix is ​​as follows: , In the formula, Let be the element in the i-th row and j-th column of the system's Jacobian matrix. Let i be the i-th residual term in the residual vector. For the small perturbation applied to the j-th calibration parameter, N1 is the number of calibration parameters.

[0018] Furthermore, the step of triggering incremental updates of global parameters when the set of calibration parameters obtained in the current window meets preset conditions includes: If the standard deviation of each calibration parameter meets a preset threshold condition, an incremental update of the global parameters is triggered. The preset threshold condition is: , , In the formula, To determine the standard deviation of the equivalent wheel radius after calibration, The threshold value is the standard deviation of the equivalent wheel radius. For the trace of radar extrinsic uncertainty, Due to the uncertainty of radar extrinsic parameters, The trace threshold represents the uncertainty of radar extrinsic parameters.

[0019] This invention also provides a parameter adaptive joint calibration system based on dual radars and a differential chassis, comprising: The pose wheel speed synchronization module is used to acquire the pose output by the laser odometer in real time, and calculate the registration wheel speed at the corresponding moment of the pose by linear interpolation, so that the displacement calculated based on the wheel speed and the laser observation trajectory based on the pose achieve statistical consistency. Radar extrinsic parameter matching module, used to... The homogeneous transformation matrix constructs a rigid body chain between the lidar coordinate system and the vehicle center coordinate system. Lie algebraic logarithmic mapping is used to eliminate the singularity of the rotation representation, ensuring that the radar extrinsic parameters achieve optimal matching with the differential chassis kinematics across the entire spatial scale. The parameter calibration optimization module is used to evaluate the reliability of the calibration parameter set obtained in the current window through the Jacobian matrix. When the calibration parameter set obtained in the current window meets the preset conditions, it triggers the incremental update of global parameters; otherwise, it discards the calibration parameter set obtained in the current window and returns to the step of real-time acquisition of the pose of the laser odometry output.

[0020] Compared with the prior art, the above-described technical solution of the present invention has the following advantages: This invention synchronizes pose and wheel speed, and on this basis... The homogeneous transformation matrix constructs a rigid body chain between the lidar coordinate system and the vehicle center coordinate system, and the Lie algebra logarithmic mapping is used to eliminate the singularity of the rotation representation. Finally, the reliability of the calibration parameter set obtained in the current window is evaluated by the Jacobian matrix and self-sensory update is performed. This can improve the signal-to-noise ratio and solution energy efficiency ratio of online calibration, achieve full parameter decoupling and high-precision consistency under complex motion conditions, and ensure the robustness and maintenance-free operation of long-term operation. Attached Figure Description

[0021] To make the content of this invention easier to understand, the invention will be further described in detail below with reference to specific embodiments and accompanying drawings, wherein: Figure 1 This is a flowchart of a method in a preferred embodiment of the present invention.

[0022] Figure 2 This is a schematic diagram of the robot's kinematics and coordinate system in a preferred embodiment of the present invention.

[0023] Figure 3This is a schematic diagram of the differential chassis of the mobile robot in a preferred embodiment of the present invention.

[0024] Figure 4 A top view of the differential chassis of the mobile robot in a preferred embodiment of the present invention.

[0025] Figure 5 A physical image of a dual-laser radar differential chassis robot in a preferred embodiment of the present invention.

[0026] Figure 6 The preferred embodiment of the present invention shows the global motion trajectory diagram within the calibration period of the first set of environments in the simulation experiment.

[0027] Figure 7 The graph shows the change in the rotational speed of the left and right drive motors over time in the first environment of the simulation experiment in the preferred embodiment of the present invention.

[0028] Figure 8 In a preferred embodiment of the present invention, the pose increment map for each frame is calculated by the laser odometry of the first environment in the simulation experiment.

[0029] Figure 9 The linear mapping relationship diagram between the kinematic variables of the first group of environments in the preferred embodiment of the present invention.

[0030] Figure 10 The preferred embodiment of the present invention shows the global motion trajectory diagram within the calibration period of the second group of environments in the simulation experiment.

[0031] Figure 11 The graph shows the change in the rotational speed of the left and right drive motors over time in the second environment of the simulation experiment in the preferred embodiment of the present invention.

[0032] Figure 12 In a preferred embodiment of the present invention, the pose increment map for each frame is calculated by the laser odometry of the second environment in the simulation experiment.

[0033] Figure 13 The linear mapping relationship diagram between the kinematic variables of the second group of environments in the simulation experiment in the preferred embodiment of the present invention.

[0034] Figure 14 The preferred embodiment of the present invention shows the global motion trajectory diagram within the calibration period of the third environment in the simulation experiment.

[0035] Figure 15 The graph shows the change in the rotational speed of the left and right drive motors over time in the third environment of the simulation experiment in the preferred embodiment of the present invention.

[0036] Figure 16 The preferred embodiment of the present invention is the pose increment map calculated by the laser odometry of the third environment in the simulation experiment.

[0037] Figure 17 It is a linear mapping diagram between kinematic variables of the third group of environments in the simulation experiment according to the preferred embodiment of the present invention.

[0038] Figure 18 It is a global motion trajectory diagram within the calibration cycle of the fourth group of environments in the simulation experiment according to the preferred embodiment of the present invention.

[0039] Figure 19 It is a diagram showing the rotational speed of the left and right driving motors varying with time in the fourth group of environments in the simulation experiment according to the preferred embodiment of the present invention.

[0040] Figure 20 It is a diagram of the pose increment of each frame calculated by the laser odometer in the fourth group of environments in the simulation experiment according to the preferred embodiment of the present invention.

[0041] Figure 21 It is a linear mapping diagram between kinematic variables of the fifth group of environments in the simulation experiment according to the preferred embodiment of the present invention.

[0042] Figure 22 It is a schematic diagram of dual-lidar point cloud fusion in a feature-rich environment according to the preferred embodiment of the present invention.

[0043] Figure 23 It is a schematic diagram of dual-lidar point cloud fusion in a local feature environment according to the preferred embodiment of the present invention.

[0044] Figure 24 It is a schematic diagram of dual-lidar point cloud fusion in a sparse feature environment according to the preferred embodiment of the present invention. Detailed Description of Preferred Embodiments

[0045] The present invention will be further described below with reference to the accompanying drawings and specific embodiments, so that those skilled in the art can better understand the present invention and implement it. However, the illustrated embodiments are not intended to limit the present invention.

[0046] Referring to Figure 1 , the present invention discloses a parameter adaptive joint calibration method based on dual lidars and a differential chassis, comprising the following steps: S1: Acquiring the pose output by LiDAR Odometry in real time, and calculating the registered wheel speed corresponding to the pose moment by linear interpolation, so that the displacement estimated based on the wheel speed and the laser observation trajectory based on the pose achieve statistical consistency.

[0047] S1-1: Monitoring the pose increment output by LiDAR Odometry in real time. Record the frame's coordinates of the laser odometer in the global coordinate system are , is the The x-coordinate of the frame laser odometry in the global coordinate system. No. The y-coordinate of the frame laser odometry in the global coordinate system is calculated by accumulating discrete Euclidean distances to determine the current laser observation trajectory: (1), In the formula, denoted as the laser observation trajectory, and k represents the number of data frames acquired by the laser odometry.

[0048] S1-2: When the laser observation trajectory satisfies At that time, extract the laser pose sequence and wheel speed sequence from the current buffer. Set a preset spatial threshold; extract the laser pose sequence and wheel speed sequence in the current buffer, encapsulate them into an independent spatial calibration unit, and reset the cumulative counter to start the next round of monitoring. The value should be adjusted according to the actual situation; in this embodiment, it is set to... This mechanism ensures that subsequent optimization calculations are always based on effective displacements, eliminating the possibility of static drift segments.

[0049] S1-3: Since the sampling frequency of the wheel speed meter is much higher than that of the lidar, in order to achieve spatiotemporal consistency, the registered wheel speed at the corresponding moment of the pose is calculated by linear interpolation for the captured laser pose sequence and wheel speed sequence; thus realizing the resampling and alignment of the original wheel speed curve.

[0050] For any laser frame timestamp of the extracted laser pose sequence, denoted as Find adjacent timestamps in the extracted wheel speed sequence and denote them as follows: and ,satisfy The registration wheel speed at the corresponding moment is calculated using linear interpolation: (2), In the formula, for Registration wheel speed at any moment for Registration wheel speed at any moment for The registration wheel speed at any given time. Through formula (2), the asynchronously sampled wheel speed input can be converted into a control sequence that strictly corresponds to the laser scanning cycle, eliminating the phase difference between sensors caused by sampling delay.

[0051] S1-4: In Within the trajectory range, the wheel speed is estimated by adjusting the equivalent wheel radius to be calibrated based on discrete wheel speed samples, so that the estimated wheel speed displacement is synchronized with the laser observation trajectory, and the estimated wheel speed displacement is used as the displacement prediction of the differential chassis motion center.

[0052] Using the equivalent wheel radius to be calibrated ,exist Displacement calculated from wheel speed within the trajectory range By using tiny time slices The displacement is calculated by summing the average speeds within the range, thus constructing the displacement calculated from the wheel speed: (3), In the formula, To calculate displacement based on wheel speed, In order to be in The total number of samples within the trajectory interval. The equivalent wheel radius to be calibrated; the wheel speed gauge outputs the angular velocities of the left and right wheels, i.e. and , For the first The left wheel angular velocity at each wheel speed sampling point Let ω be the angular velocity of the right wheel at the j-th wheel speed sampling point. For a preset tiny time slice, The value should be adjusted according to the actual situation; By establishing the physical basis of the fitted slope, that is, by adjusting... This allows the displacement to be calculated based on the wheel speed. With laser observation trajectory To achieve the greatest possible statistical consistency. It is the actual motion distance / reference trajectory length calculated by laser odometry, which is obtained through continuous pose points of the laser odometry. It is obtained by summing the Euclidean distances. The chassis movement distance is obtained by integrating the angular velocities of the left and right wheels, the sampling time, and the equivalent wheel radius to be calibrated. and When the normalization error between the two is minimized, they are considered to have reached the maximum degree of statistical consistency. Specifically, this is achieved through continuous adjustment. Let the distance obtained by integrating the wheel speed (i.e. ) as close as possible to the distance observed by the laser odometry (i.e. ).when and If the difference is small enough, it means that the current... That's quite reasonable.

[0053] The core logic of step S1 is to transform the traditional time-equal interval sampling into spatial displacement-equal driving, so as to ensure that each calibration data window contains a certain amount of motion information, thereby solving the model inaccuracy problem caused by the mixing of dynamic and static data.

[0054] Step S1 addresses the core issues of energy efficiency and static interference resistance in online calibration data, employing a triggering logic distinct from traditional time-interval sampling. It utilizes a laser odometer to calculate the three-dimensional spatial displacement vector in real time, through... The logic of the differential equation replaces the fixed-frequency triggering and uses the length of the laser motion trajectory as a logic architecture for motion excitation sufficiency filtering. To address the time phase difference between the high-frequency wheel speed meter and the low-frequency laser scan, linear interpolation resampling based on the laser timestamp is employed to ensure motion increment alignment. Within a truncated spatial window, a synchronization process is implemented to align the wheel speed data with equivalent timestamps.

[0055] S2: Through The homogeneous transformation matrix constructs a rigid body chain between the lidar coordinate system and the vehicle center coordinate system. Lie algebraic logarithmic mapping is used to eliminate the singularity of the rotation representation, ensuring that the calibration residuals converge unimodally in the manifold space, so as to ultimately ensure that the radar extrinsic parameters achieve optimal matching with the differential chassis kinematics across the entire spatial scale.

[0056] S2-1: As Figure 2 As shown, since lidar A and lidar B are fixedly mounted diagonally on the same rigid frame, they can move in any... The trajectory of motion within the trajectory interval must satisfy the geometrical transfer law.

[0057] Define the extrinsic transformation matrix from lidar A to lidar B as follows: , , for Given the homogeneous transformation matrix, the local pose increments observed by lidar A and lidar B satisfy the following rigid body constraint equations: (4), In the formula, Let A be the local pose increment observed by lidar A at time k. Let be the local pose increment observed by lidar B at time k; Equation (4) shows that, without considering noise, the trajectory of lidar B after external parameter transformation should completely coincide with the trajectory of lidar A, which constitutes the core geometric basis for the system calibration of the relative offset of the two lidars.

[0058] S2-2: Differential chassis center (i.e.) The motion of ) is determined by the angular velocity (i.e., ) provided by the wheel speed gauge. ) and linear velocity (i.e. The only certainty is that, in order to integrate wheel speed information into the three-dimensional calibration framework, The instantaneous velocity vector in the Lie algebra space is projected onto the exponential map. The pose transformation matrix is ​​obtained as follows: (5), In the formula, Let be the pose transformation matrix of the differential chassis center at time k, and exp be the natural exponent. Let k be the linear velocity of the differential chassis center at time k. Let k be the angular velocity of the differential chassis center at time k. and Contains the equivalent wheel radius to be calibrated , For the hat operator, The sampling time interval is given by formula (5). Formula (5) transforms the one-dimensional velocity waveform into a spatial transformation with 6 degrees of freedom, so that the chassis motion can be directly aligned with the laser observation in terms of dimensions.

[0059] (6), (7); In the formula, For the first The left wheel angular velocity at each wheel speed sampling point For the first The angular velocity of the right wheel at each wheel speed sampling point The distance between the left and right wheels is the measured known value.

[0060] S2-3: Rotation angle increment measured by laser odometer Speed ​​difference between left and right wheels There exists a very strong linear proportional relationship between them. Within the trajectory interval, for each locally matched sample, the linear constraint residual is established as: (8), In the formula, For the first The linear constraint residuals of a locally matched sample The first time measured by laser odometry The rotation angle increment of each locally matched sample For the first The left wheel angular velocity at each wheel speed sampling point Let ω be the angular velocity of the right wheel at the j-th wheel speed sampling point. The wheelbase is the distance between the left and right wheels. Let j be the sampling time interval for the j-th wheel speed.

[0061] S2-4: The equivalent wheel radius to be calibrated is achieved by minimizing the sum of squares of the linear constraint residuals of all locally matched samples. The calibration of the wheel speed gauge is performed, and the proportional factor is corrected at the same time.

[0062] The proportional factor of the wheel speed gauge is: the actual proportional factor is , This is the scaling factor. Once the equivalent wheel radius is calibrated, it is combined with known... That is, it achieves the Corrections.

[0063] S2-5: The linear constraint residuals (i.e., formula (8)) are fused with the rigid body constraint equations (i.e., formula (4)) using a multi-objective method, utilizing the Lie algebraic logarithmic mapping (i.e. The residual vector is extracted to ensure that the radar extrinsic parameters achieve optimal matching with the differential chassis kinematics across the entire spatial scale.

[0064] In multi-objective fusion, a joint optimization objective function (joint residual function) is constructed based on the linear constraint residuals and rigid body constraint equations. The joint optimization objective function includes the dual-radar rigid body constraint residuals and the differential chassis kinematic constraint residuals. The dual-radar rigid body constraint residuals are used for constraint... Differential chassis kinematic constraint residuals are used for constraint And the proportional factor of the wheel speed gauge.

[0065] The residual vector extracted using Lie algebra logarithmic mapping represents the pose error generated by the dual radar rigid body constraints under the current extrinsic parameter estimation.

[0066] Radar extrinsic parameters are used The extrinsic parameters between the two radars are represented by the sum of squared residuals of the joint residuals of the rigid body constraints of the two radars and the kinematic constraints of the differential chassis, which are minimized through nonlinear least squares optimization. The result obtained is... The optimal external parameters that simultaneously satisfy "dual radar motion consistency" and "chassis wheel speed motion consistency" within the current data window. .

[0067] Based on optimal external parameters This allows us to obtain the equivalent lateral displacement (denoted as ) of the calibrated radar extrinsic pose in the planar motion model. The equivalent longitudinal displacement of the radar extrinsic pose in the planar motion model (denoted as...) ) and the heading angle of the radar extrinsic pose in the planar motion model (denoted as ) The set of calibration parameters obtained within the current window is denoted as... .

[0068] Step S2 is the core of solving the calibration accuracy problem. It is a method of unifying the modeling of physical wheel track, wheel diameter deviation, and sensor extrinsic parameters. The physical wheel diameter and reduction ratio deviation are uniformly modeled as parameters to be calibrated. Establish from motor angular velocity to Differential mapping of pose transformation is a method for simultaneously solving for the extrinsic parameters of the lidar and the intrinsic parameters of the chassis in nonlinear optimization. (In Lie group space) A joint objective function containing both "radar-radar" constraints and "radar-chassis" kinematic constraints is established. A 6-dimensional residual vector is extracted in the tangent space using Lie algebraic logarithmic mapping, replacing the traditional quaternion or Euler angle representation.

[0069] S3: After completion After nonlinear optimization within the trajectory interval, the reliability of the calibration parameter set obtained within the current window needs to be evaluated. Specifically, the reliability of the calibration parameter set obtained within the current window is evaluated using the Jacobian matrix. When the calibration parameter set obtained within the current window meets the preset conditions, an incremental update of the global parameters is triggered; otherwise, the calibration parameter set obtained within the current window is discarded, and the process returns to the step of real-time acquisition of the pose output of the laser odometry, i.e., returning to S1 to recalculate the calibration parameters.

[0070] Step S3 uses the perturbation method to analyze the sensitivity of the residual to the state variables, and achieves smooth and monotonically convergent calibration parameters through uncertainty quantification, avoiding erroneous parameter coverage under low excitation or high noise conditions.

[0071] S3-1: Define the set of calibration parameters obtained from the solution within the current window as follows: , ,in, This represents the equivalent lateral displacement of the radar extrinsic pose in a planar motion model. This represents the equivalent longitudinal displacement of the radar extrinsic pose in a planar motion model. The heading angle of the radar extrinsic pose in the planar motion model. For the equivalent wheel radius after calibration, T is the transpose operation; for the linear constraint residuals described in formula (8), a small perturbation (denoted as ) is applied near the calibration parameter set obtained within the current window using the five-point central difference method. The method for calculating the elements in the system Jacobian matrix is ​​as follows: (9), In the formula, Let be the element in the i-th row and j-th column of the system's Jacobian matrix. Let i be the i-th residual term in the residual vector. For the j-th small perturbation applied to the j-th calibration parameter, N1 is the number of calibration parameters, which is N1=4 in this embodiment. Formula (9) can capture the curvature of the residual surface at the current optimal solution. The numerical derivative method is more robust than simple linearization and can more accurately reflect the constraint strength of the observation on the parameters.

[0072] S3-2: Combining the information matrix from laser scanning matching, the system's information matrix is ​​constructed as follows: (10) In the formula, For the system's information matrix, Let Jacobian matrix be the system's Jacobian matrix. The information matrix for laser scanning matching has values ​​determined by the standard deviation of the registration residuals.

[0073] S3-3: By inverting the information matrix of the system, the covariance matrix of the state variables is obtained as follows: (11), In the formula, Let be the covariance matrix of the state variables. The square root of the diagonal elements of the covariance matrix of the state variables is the sum of the values ​​of each calibration parameter in the calibration parameter set obtained within the current window (i.e., ...). The standard deviation of these four calibration parameters; if The trajectory of motion includes a rich variety of rotations and translations, then The large eigenvalues ​​and small standard deviations of the corresponding calibration parameters indicate high reliability, meaning the confidence level of the calibration results is high.

[0074] If the standard deviation of each calibration parameter is less than the corresponding preset standard deviation threshold, it indicates that the reliability of the calibration parameter set obtained within the current window is high. The preset standard deviation threshold is adjusted according to actual conditions. In this embodiment, and The preset standard deviation threshold is set to 0.02m. The preset standard deviation threshold is set to 0.5°. The preset standard deviation threshold is set to 1%.

[0075] S3-4: To ensure the long-term stability of online calibration, a standard deviation-based update gate is set up. Incremental updates of global parameters are triggered only when the standard deviations of all current calibration parameters meet a preset threshold condition. The preset threshold condition is: (12), (13) In the formula, To determine the standard deviation of the equivalent wheel radius after calibration, The threshold value is the standard deviation of the equivalent wheel radius; For the trace of radar extrinsic uncertainty, Due to the uncertainty of radar extrinsic parameters, The trace threshold represents the uncertainty of radar extrinsic parameters. , The values ​​should be adjusted according to the actual situation. Radar extrinsic parameters are those used in step S2-5. The extrinsic parameters between the two radars are represented. From the covariance matrix of the state variables ( The sub-covariance matrix extracted from the radar extrinsic parameters is specifically corresponding to the radar extrinsic parameters.

[0076] In this embodiment, the global parameters specifically include the calibration parameters that the current system requires long-term maintenance and online correction. When an incremental update of the global parameters is triggered, the current estimated value is not used to completely overwrite the historical parameters. Instead, a weighted incremental approach is used to update the global parameters. When the robot is in stable motion and the environment is rich in features, the system actively updates the parameters. However, when entering a degraded environment, such as a long corridor causing a surge in longitudinal noise, the system automatically determines that the confidence level is insufficient, thus preventing the historically optimal parameters from being contaminated. This adaptive mechanism enables the robot to have the ability to self-detect faults.

[0077] Step S3 is crucial for addressing the robustness of online calibration and preventing parameter divergence; it represents an algorithm-level self-sensing admission mechanism. A small perturbation is applied to the calibration parameters using the five-point central difference method to detect the sensitivity of the residual surface to state variables in real time. By constructing and inverting the information matrix, the covariance matrix of all state variables, including extrinsic and intrinsic parameters, is obtained—a method for real-time evaluation of the confidence level of online calibration parameters using the information matrix. An update threshold based on standard deviation is set; online overwriting of memory parameters is only allowed when the information generated by motion excitation is sufficient to lower the threshold below a safety limit. This constructs a process for generating calibration uncertainty metrics based on a numerical perturbation method. By setting the standard deviation threshold, online closed-loop control logic achieves fault self-sensing and automatically blocks erroneous parameter overwriting.

[0078] This invention also discloses a parameter adaptive joint calibration system based on dual radars and a differential chassis, comprising: The pose wheel speed synchronization module is used to acquire the pose output by the laser odometer in real time, and calculate the registration wheel speed at the corresponding moment of the pose by linear interpolation, so that the displacement calculated based on the wheel speed and the laser observation trajectory based on the pose achieve statistical consistency. Radar extrinsic parameter matching module, used to... The homogeneous transformation matrix constructs a rigid body chain between the lidar coordinate system and the vehicle center coordinate system. Lie algebraic logarithmic mapping is used to eliminate the singularity of the rotation representation, ensuring that the radar extrinsic parameters achieve optimal matching with the differential chassis kinematics across the entire spatial scale. The parameter calibration optimization module is used to evaluate the reliability of the calibration parameter set obtained in the current window through the Jacobian matrix. When the calibration parameter set obtained in the current window meets the preset conditions, it triggers the incremental update of global parameters; otherwise, it discards the calibration parameter set obtained in the current window and returns to the step of real-time acquisition of the pose of the laser odometry output.

[0079] The present invention also discloses a computer-readable storage medium storing a computer program thereon, which, when executed by a processor, implements a parameter adaptive joint calibration method based on dual radars and a differential chassis.

[0080] The present invention also discloses a device including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement a parameter adaptive joint calibration method based on dual radar and differential chassis.

[0081] This invention addresses the shortcomings of existing online calibration technologies, such as low signal-to-noise ratio data interference, parameter coupling due to imprecise spatial modeling, and parameter divergence caused by the lack of confidence metrics. It provides a logically rigorous online adaptive joint calibration scheme with self-aware quality capabilities. This invention aims to solve the following three core technical problems: 1. Solving the problems of signal-to-noise ratio imbalance and data redundancy in online data acquisition. Existing technologies mainly employ time-interval sampling or full-data batch processing, which introduces a large amount of low signal-to-noise ratio noise data lacking motion excitation during robot stationary, extremely low-speed, or frequent start-stop processes, leading to ill-conditioned solutions in the calibration matrix. This invention aims to provide a dual-source odometry synchronous triggering mechanism based on spatial trajectory step size. By using a fixed displacement step size as the basic unit of calibration processing, it ensures that each set of windows participating in the calculation contains sufficient and definite physical displacement, filtering out invalid information at the source and significantly improving the data efficiency ratio and calculation convergence speed of online calibration.

[0082] 2. Solving the problems of rotational singularities and multi-parameter coupling under complex 3D motion. For multi-radar systems with diagonal layouts operating on uneven ground or in 3D disturbed environments, existing 2D calibration or linear fitting schemes are prone to rotational singularities and the mutual penetration of internal and external parameter errors. This invention aims to construct a system based on... A space-time consistency constraint model for manifolds is proposed. Rigid body transformation chains between radars and between radars and the vehicle body are established within the Lie group space, and pose residuals are processed using Lie algebraic logarithmic mappings to achieve external parameters. Internal parameters of equivalent wheel radius The synchronous optimal solution is calculated.

[0083] 3. Addressing the problem of blind parameter updates and divergence under environmental degradation conditions. To address the issue that existing online self-calibration algorithms suffer from blind overwriting of historical high-precision parameters and subsequent system crashes due to a lack of quantitative assessment of calibration result reliability when features are missing or observation noise surges, this invention aims to introduce a calibration quality assessment and adaptive update mechanism based on an information matrix. The standard deviation of parameter uncertainty is quantified using the numerical difference method. A threshold standard is established for online parameter updates. This ensures that calibration results are dynamically corrected only when motion excitation is sufficient and observation confidence is high, thereby guaranteeing the robustness and maintenance-free operation of the multi-radar system under long-term complex operating conditions.

[0084] Compared with the prior art, the advantages of the present invention are as follows: 1. Improve the signal-to-noise ratio and computational energy efficiency of online calibration. This invention changes the traditional sampling logic by implementing a spatial trajectory step-size triggering mechanism. It effectively eliminates low-excitation data generated under invalid working conditions such as the robot being stationary or spinning in place, ensuring that all data windows involved in the calculation contain sufficient displacement information. Compared to time-triggered methods, this solution... The point cloud and wheel speed sequence extracted within the spatial interval have a higher signal-to-noise ratio, avoiding ill-conditioned solutions caused by low-speed drift, and significantly accelerating the convergence speed of online calibration parameters.

[0085] 2. Achieve full parameter decoupling and high-precision consistency under complex motion conditions. This invention constructs a system based on... A space-time consistency constraint model for manifolds is used, and Lie algebra tools are employed to deeply align the chassis and radar motions. Attitude is expressed in Lie group space, avoiding rotational singularities under traditional Euler angle representations and ensuring computational stability for robot travel on uneven ground or slopes. Extrinsic parameters are realized by correlating the radar angular velocity increment with the wheel speed difference. With internal reference The simultaneous joint solution can accurately track the minute changes in the equivalent wheel radius caused by tire wear and load variations.

[0086] 3. Enhance self-sensing capabilities to ensure robustness and maintenance-free long-term operation. The quality assessment and adaptive update mechanism based on an information matrix introduced in this invention establishes an admission standard for online parameter updates. Through differential Jacobi calculation, the system can output the standard deviation of each calibration parameter in real time, thereby quantifying the current... The contribution of trajectory segments to parameter estimation. The confidence-based gate control system can automatically identify environmental degradation or abnormal sensor fluctuations. By triggering parameter updates only when observations are reliable, it prevents the contamination of historical high-precision parameters, achieving self-sensing online calibration and significantly reducing the costs of manual inspections and secondary calibrations after the deployment of large fleets.

[0087] To further demonstrate the beneficial effects of the present invention, this embodiment uses, for example... Figure 3 , Figure 4 A simulation experiment is conducted using the mobile robot shown. The mobile robot includes a rigid frame made of aluminum profiles, with LiDAR A and LiDAR B installed diagonally across the frame. A differential chassis has symmetrically arranged drive wheels in the center, with motor controllers providing real-time speed feedback. An industrial control computer computing unit is located in the center of the chassis, used to execute the joint calibration method described in this invention. This embodiment uses a differential chassis robot equipped with dual LiDARs. The actual vehicle platform is shown below. Figure 5 As shown, data was truncated according to the spatial trajectory step size in this invention, and a joint calibration algorithm was executed. Instance verification was performed in four environments, and the four core dimensions of the calibration process were demonstrated.

[0088] Figure 6 The global motion trajectory of the robot in the first set of environments during the calibration period is shown, denoted as Trajectory 1. The system calculates the trajectory length in real time. The blue curve from the starting point (green dot) to the ending point (red dot) in the figure constitutes a complete spatial calibration unit. The trajectory contains obvious circular arc segments, providing sufficient rotational excitation for the system, satisfying the observability criterion, and ensuring the decoupling of external participation from the equivalent wheel radius. Figure 7 The left and right drive motor speeds of the first set of environments were recorded. The speed varies over time. A linear interpolation method is used to synchronize the wheel speed signal, which exhibits step-like characteristics, with the lidar sampling frequency. Speed ​​differences at different times, such as... The nearby anisotropic rotations provide a rich source of Jacobian matrix perturbation for the information matrix, which helps to reduce the standard deviation of the calibration parameters. Figure 8 The image shows the pose increment calculated by laser odometry for each frame in the first set of environments. Obvious random fluctuations and spikes are visible in the figure, representing sensor observation noise. This invention utilizes... Joint manifold optimization can effectively suppress these outliers and ensure that the trajectory center trend remains consistent with the wheel speed gauge even in the presence of registration noise. Figure 9 This is the core verification of the calibration accuracy of this invention, demonstrating the linear mapping relationship between the kinematic variables of the first set of environments. The scatter distribution exhibits a very strong linear correlation. The slope of this linear fitting curve directly reflects the equivalent wheel radius. With wheelbase The physical proportional relationship. All scatter points are closely distributed around the fitted straight line, indicating the physical proportional relationship of the laser odometry observations. A high degree of spatial consistency was achieved with the rotational amount calculated by the wheel speed gauge. This proves that... The optimized parameters can accurately characterize the actual kinematic properties of the chassis, eliminating nonlinear errors caused by reduction ratio bias and wheel wear.

[0089] Figure 10 The global trajectory map records the robot's global motion trajectory in the second environment during the calibration period. Specifically, it is a complex hook-shaped path with a longitudinal span of approximately 25 meters, denoted as trajectory two. This provides the algorithm with at least five consecutive spatial trigger windows, ensuring the monotonic convergence of parameter estimation over long-range scales. Quantitative observation. Figure 11 The speed curve shows that within the range of 13 to 19 seconds, the system performed a significant differential steering maneuver. The left wheel speed decreased from -0.25 m / s to -0.4 m / s, while the right wheel remained around -0.15 m / s. This stable speed difference of up to 0.25 m / s created a very strong rotational excitation, causing... Figure 12 The corresponding rotation increment still exhibits a clear step response of approximately 0.003 rad / frame even against a noisy background. Although there are local observation glitches of about 1.5 mm in the translation increment, the system can still extract a highly stable center motion vector through robust optimization after logarithmic mapping. The most crucial... Figure 13 The linearity evaluation plot quantitatively demonstrates the high clustering of scatter points in the three velocity difference ranges of -0.2 m / s, 0 m / s, and 0.2 m / s. The slope of its fitted straight line is precisely and stably around 0.02. As the equivalent wheel radius to wheel track ratio, its residual standard deviation is calculated to be far below 0.0005 rad, thus mathematically passing the confidence test of the information matrix. This proves that the system possesses extremely high parameter tracking accuracy and deterministic online updates when dealing with dynamic conditions with significant step excitation. Through further analysis... Figures 10-13 The combined quantitative and qualitative analysis of the data further revealed the response characteristics of the algorithm under specific motion conditions.

[0090] Figure 14 The global trajectory plot shows the robot's global motion trajectory within the calibration period in the third environment, namely a near-circular trajectory with a diameter of approximately 25 meters, denoted as trajectory three. This highly consistent and continuous curvature change provides the algorithm with more than 10 stable displacement trigger intervals, greatly enhancing the statistical consistency of parameter estimation. Qualitative Observation Figure 15 The speed curve shows that the system performed low-speed differential steering (left and right wheel speed difference of about 0.2 m / s) from approximately 15 to 30 seconds, and then switched to large-radius rapid circular driving (right wheel maintained at a high position of 0.35 m / s) from 30 to 80 seconds. This switching between low-speed refined calibration and high-speed dynamic verification, in Figure 16The clear step change from 0.001 rad to 0.004 rad in the translation increment fully verifies the linear adaptability of the kinematic model across different velocity ranges. Although instantaneous impulse noise approaching -0.003 m appears in the translation increment around 85 seconds, reflecting the interference of abrupt changes in environmental characteristics on sensor observations, the system effectively isolates this type of noise from contaminating global parameters thanks to the sensitivity of the information matrix to anomalous observation variances. Figure 17 In the linear mapping diagram, the data points form an extremely dense cluster of physical features in the velocity difference range of 0.1 m / s and 0.2 m / s. The residual standard deviation of the fitted curve remains at a very small level, which not only quantitatively confirms the accuracy of the equivalent wheel radius parameter, but also proves that the system has excellent parameter drift suppression capability and physical determinism for online updates when facing long-period, large-scale motion.

[0091] Figure 18 The global trajectory diagram presents the robot's global motion trajectory within the calibration period for the fourth environment, namely a regular large circular arc with a radius of approximately 15 meters, denoted as trajectory four, providing continuous geometric constraints for up to 70 seconds. Figure 19 The speed curve reveals a distinct square-wave excitation characteristic: the right wheel speed switches frequently between 0.5 m / s and -0.1 m / s, while the left wheel fluctuates synchronously between 0.1 m / s and -0.1 m / s. This step-like speed difference of up to 0.6 m / s... Figure 20 The simulation induced highly regular displacement increment spikes, with the peak value of the translational increment approaching 0.008m, and the pulses of the rotational increment perfectly matching the velocity switching points, fully demonstrating that the linear interpolation algorithm can maintain microsecond-level synchronization accuracy even under drastic velocity fluctuations. Figure 21 In the linear mapping diagram, despite the presence of high-frequency periodic interference, the data points still exhibit a high degree of linear consistency within the speed difference range of -0.25 m / s to 0.2 m / s, with the scatter points closely surrounding the fitting center, demonstrating the robustness of the equivalent wheel radius parameter under dynamically changing operating conditions. Figures 18-21 In the data visualization analysis, the system exhibited extremely regular periodic pulse motion characteristics, which posed a rigorous test to the algorithm's transient response and spatiotemporal alignment accuracy.

[0092] To further verify the robustness of the joint calibration, the four sets of trajectory data were analyzed, and the results are shown in Table 1.

[0093] Table 1 Analysis of Four Sets of Trajectory Data

[0094] As shown in Table 1, the cumulative displacement of each sequence remained stable within the range of 4.28m to 5.67m, which highly matches the 5m spatial step triggering logic described in this invention. The linear fitting accuracy verifies... The rigor of the modeling is evident. The coefficients of determination for all four sequences are significantly higher than 0.95, reaching a maximum of 0.9813. This extremely high linearity quantitatively demonstrates that... Under manifold constraints, there is a very strong physical consistency between the angular velocity increment observed by the laser odometry and the speed difference between the left and right wheels of the chassis. Even in Track 3, which includes high-performance components... (about Under complex conditions of violent rotation, the model maintains extremely high descriptive accuracy and avoids rotational singularities that are prone to occur in traditional methods. The data reveals the dynamic tracking value of the equivalent wheel radius parameter. Quantitative comparison shows that the trajectory linear fitting slope exhibits extremely high consistency. This phenomenon strongly demonstrates that the robot's equivalent wheel radius remains consistent during long-term operation. Physical drift can occur due to tire wear, changes in air pressure, or changes in mechanical condition. This invention, through online adaptive calibration, accurately captures this long-period parameter drift, thereby ensuring positioning accuracy. This highlights the long-term stability and technical value of this invention in practical engineering deployments, eliminating the need for manual secondary calibration.

[0095] Figures 22-24 The image shows the spatial relationship between the dual lidars and the vehicle's central coordinate system under three different operating conditions, as well as the point cloud fusion effect of the dual lidars' online calibration. Figure 22 This is a schematic diagram of point cloud fusion using dual lidar systems in a feature-rich environment. Figure 23 This is a schematic diagram of point cloud fusion using dual lidar systems in a localized environment. Figure 24 This is a schematic diagram of point cloud fusion using dual lidar systems in a feature-sparse environment. Figures 22-24 The yellow point cloud represents data from LiDAR A, and the green point cloud represents data from LiDAR B. Using the online joint calibration method described in this invention, the pose relationship between LiDAR A, LiDAR B, and the vehicle center is accurately compensated. The yellow and green point clouds at the edges of the environment in the image are seamlessly stitched together in the overlapping area, demonstrating that the two radars achieve high-precision fusion of environmental feature observations under different motion poses, proving the accuracy of the extrinsic parameter and equivalent wheel radius estimations. The artificial structures in the environment maintain their rigidity, indicating that the extrinsic parameter estimation is stable and has not drifted.

[0096] Through measured data from a series of complex working conditions in simulation experiments, the significant technical advantages of this invention as an online adaptive calibration scheme were fully verified. First, the spatial displacement triggering mechanism solves the data pollution problem of traditional algorithms under low-speed or static conditions, ensuring that calibration calculations are always based on high-quality, high-excitation motion characteristics. Second, based on... The joint optimization architecture of the manifold enables dynamic tracking of the equivalent wheel radius and radar extrinsic parameters, mathematically avoiding rotational singularities and achieving deep decoupling of intrinsic and extrinsic parameters. Finally, the confidence gate mechanism introduced by the information matrix endows the system with strong self-sensing capabilities, ensuring the monotonic convergence and long-term stability of the calibration parameters during long-term operation. Experimental results show that, regardless of continuous circular motion or violent pulse excitation, this invention can achieve seamless high-precision fusion of dual-path radar point clouds, greatly improving the localization robustness of mobile robots in complex dynamic environments and possessing high practical application value.

[0097] Those skilled in the art will understand that embodiments of this application can be provided as methods, systems, or computer program products. Therefore, this application can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, this application can take the form of a computer program product embodied on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0098] This application is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of this application. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart... Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0099] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0100] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1The steps of the function specified in one or more boxes.

[0101] Obviously, the above embodiments are merely illustrative examples for clear explanation and are not intended to limit the implementation. Those skilled in the art will recognize that other variations or modifications can be made based on the above description. It is neither necessary nor possible to exhaustively list all possible implementations here. However, obvious variations or modifications derived therefrom are still within the scope of protection of this invention.

Claims

1. A parameter adaptive joint calibration method based on double radar and differential chassis, characterized in that, include: The pose output by the laser odometer is acquired in real time, and the registration wheel speed at the corresponding moment is calculated by linear interpolation, so that the displacement calculated based on the wheel speed and the laser observation trajectory based on the pose achieve statistical consistency. The step of calculating the registration wheel speed at the corresponding moment of pose using linear interpolation, so that the displacement calculated based on the wheel speed and the laser observation trajectory based on the pose achieve statistical consistency, includes: recording the first The coordinates of the frame laser odometry in the global coordinate system are , recording the first The x coordinate of the frame laser odometry in the global coordinate system, recording the first The y coordinate of the frame laser odometry in the global coordinate system, and the current laser observation trajectory is calculated by discrete Euclidean distance accumulation. When the laser observation trajectory satisfies At that time, extract the laser pose sequence and wheel speed sequence from the current buffer. For laser observation trajectory, Preset spatial threshold; For the captured laser pose sequence and wheel speed sequence, the registration wheel speed at the corresponding moment of pose is calculated by linear interpolation. exist Within the trajectory range, the wheel speed is estimated by adjusting the equivalent wheel radius to be calibrated based on discrete wheel speed samples, so that the estimated wheel speed displacement is synchronized with the laser observation trajectory, and the estimated wheel speed displacement is used as the displacement prediction of the differential chassis motion center. pass The homogeneous transformation matrix constructs a rigid body chain between the lidar coordinate system and the vehicle center coordinate system. Lie algebraic logarithmic mapping is used to eliminate the singularity of the rotation representation, ensuring that the radar extrinsic parameters achieve optimal matching with the differential chassis kinematics across the entire spatial scale. The passage The homogeneous transformation matrix constructs a rigid body chain between the lidar coordinate system and the vehicle center coordinate system, including: Define the extrinsic transformation matrix from lidar A to lidar B as follows: , , for Given the homogeneous transformation matrix, the local pose increments observed by lidar A and lidar B satisfy the following rigid body constraint equations: ， In the formula, Let A be the local pose increment observed by lidar A at time k. Let be the local pose increment observed by lidar B at time k; The motion of the differential chassis center is uniquely determined by the angular velocity and linear velocity provided by the wheel speed gauges. To integrate wheel speed information into the three-dimensional calibration framework, The instantaneous velocity vector in the Lie algebra space is projected onto the exponential map. The pose transformation matrix is ​​obtained as follows: ， In the formula, Let be the pose transformation matrix of the differential chassis center at time k, and exp be the natural exponent. Let k be the linear velocity of the differential chassis center at time k. Let k be the angular velocity of the differential chassis center at time k. and Contains the equivalent wheel radius to be calibrated , For the hat operator, The sampling time interval; exist Within the trajectory interval, for each locally matched sample, the linear constraint residual is established as: ， In the formula, For the first The linear constraint residuals of a locally matched sample The first time measured by laser odometry The rotation angle increment of each locally matched sample The equivalent wheel radius to be calibrated is... For the first The left wheel angular velocity at each wheel speed sampling point Let ω be the angular velocity of the right wheel at the j-th wheel speed sampling point. The wheelbase is the distance between the left and right wheels. The sampling time interval for the j-th wheel speed; The equivalent wheel radius to be calibrated is achieved by minimizing the sum of squares of the linear constraint residuals of all local matching samples, while correcting the scaling factor of the wheel speed gauge. The reliability of the calibration parameter set obtained within the current window is evaluated by the Jacobian matrix. When the calibration parameter set obtained within the current window meets the preset conditions, the incremental update of the global parameters is triggered; otherwise, the calibration parameter set obtained within the current window is discarded and the process returns to the step of obtaining the pose of the laser odometry output in real time.

2. The parameter adaptive joint calibration method based on dual radar and differential chassis according to claim 1, characterized in that: The step of calculating the registration wheel speed at the corresponding moment of the pose using linear interpolation for the extracted laser pose sequence and wheel speed sequence includes: For any laser frame timestamp of the extracted laser pose sequence, denoted as Find adjacent timestamps in the extracted wheel speed sequence and denote them as follows: and ,satisfy The registration wheel speed at the corresponding moment is calculated using linear interpolation: ， In the formula, for Registration wheel speed at any moment for Registration wheel speed at any moment for The registered wheel speed at any given moment.

3. The parameter adaptive joint calibration method based on dual radar and differential chassis according to claim 1, characterized in that: The step of calculating the displacement based on discrete wheel speed samples by adjusting the equivalent wheel radius to be calibrated, so that the calculated displacement is synchronized with the laser observation trajectory, includes: The displacement calculated based on the wheel speed is as follows: ， In the formula, To calculate displacement based on wheel speed, In order to be in The total number of samples within the trajectory interval. The equivalent wheel radius to be calibrated is... For the first The left wheel angular velocity at each wheel speed sampling point Let ω be the angular velocity of the right wheel at the j-th wheel speed sampling point. For a preset, tiny time slice; By adjusting This allows the displacement to be calculated based on the wheel speed. With laser observation trajectory To achieve the greatest possible statistical consistency.

4. The parameter adaptive joint calibration method based on dual radar and differential chassis according to claim 1, characterized in that: The method of eliminating singularity in rotation representation by using Lie algebraic logarithmic mapping specifically involves multi-objective fusion of the linear constraint residuals and rigid body constraint equations, extracting residual vectors using Lie algebraic logarithmic mapping, and ultimately ensuring that the radar extrinsic parameters achieve optimal matching with the differential chassis kinematics across the entire spatial scale.

5. The parameter adaptive joint calibration method based on dual radar and differential chassis according to claim 4, characterized in that: The method of evaluating the reliability of the calibration parameter set obtained within the current window using the Jacobian matrix includes: Define the set of calibration parameters obtained from solving within the current window as follows: , ,in, This represents the equivalent lateral displacement of the radar extrinsic pose in a planar motion model. This represents the equivalent longitudinal displacement of the radar extrinsic pose in a planar motion model. The heading angle of the radar extrinsic pose in the planar motion model. For the equivalent wheel radius after calibration, T is the transpose operation; for the linear constraint residual, a small perturbation is applied near the calibration parameter set obtained in the current window using the five-point central difference method to construct the system Jacobian matrix; Combining the information matrix from laser scanning matching and the system Jacobian matrix, the system's information matrix is ​​constructed as follows: ， In the formula, For the system's information matrix, Let Jacobian matrix be the system's Jacobian matrix. Information matrix for laser scanning; By inverting the information matrix of the system, the covariance matrix of the state variables is obtained as follows: ， In the formula, Let be the covariance matrix of the state variables. The square root of the diagonal element of the covariance matrix of the state variables is the standard deviation of each calibration parameter in the calibration parameter set obtained within the current window. If the standard deviation of each calibration parameter is small, it indicates high reliability.

6. The parameter adaptive joint calibration method based on dual radar and differential chassis according to claim 5, characterized in that: The method for calculating the elements in the Jacobian matrix of the system is as follows: ， In the formula, Let be the element in the i-th row and j-th column of the system's Jacobian matrix. Let i be the i-th residual term in the residual vector. For the small perturbation applied to the j-th calibration parameter, N1 is the number of calibration parameters.

7. The parameter adaptive joint calibration method based on dual radar and differential chassis according to claim 5, characterized in that: The step of triggering incremental updates of global parameters when the set of calibration parameters obtained in the current window meets preset conditions includes: If the standard deviation of each calibration parameter meets a preset threshold condition, an incremental update of the global parameters is triggered. The preset threshold condition is: ， ， In the formula, To determine the standard deviation of the equivalent wheel radius after calibration, The threshold value is the standard deviation of the equivalent wheel radius. For the trace of radar extrinsic uncertainty, Due to the uncertainty of radar extrinsic parameters, The trace threshold represents the uncertainty of radar extrinsic parameters.

8. A parameter adaptive joint calibration system based on dual radar and differential chassis, used to implement the parameter adaptive joint calibration method based on dual radar and differential chassis as described in any one of claims 1-7, characterized in that, include: The pose wheel speed synchronization module is used to acquire the pose output by the laser odometer in real time, and calculate the registration wheel speed at the corresponding moment of the pose by linear interpolation, so that the displacement calculated based on the wheel speed and the laser observation trajectory based on the pose achieve statistical consistency. Radar extrinsic parameter matching module, used to... The homogeneous transformation matrix constructs a rigid body chain between the lidar coordinate system and the vehicle center coordinate system. Lie algebraic logarithmic mapping is used to eliminate the singularity of the rotation representation, ensuring that the radar extrinsic parameters achieve optimal matching with the differential chassis kinematics across the entire spatial scale. The parameter calibration optimization module is used to evaluate the reliability of the calibration parameter set obtained in the current window through the Jacobian matrix. When the calibration parameter set obtained in the current window meets the preset conditions, it triggers the incremental update of global parameters; otherwise, it discards the calibration parameter set obtained in the current window and returns to the step of real-time acquisition of the pose of the laser odometry output.

Images