Factor graph-based IMU / LiDAR external parameter online estimation method
Through the IMU/LiDAR external parameter online estimation method based on factor graph, the impact of IMU and LiDAR external parameter error on unmanned parking position estimation is solved, and high-precision external parameter estimation and system robustness are achieved, which is suitable for high dynamic applications. Scenarios reduce dependence on manual intervention.
Patent Information
- Application Number
- CN202411456456.9
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-10-18
- Publication Date
- 2024-11-22
- Estimated Expiration
- Not applicable · inactive patent
AI Technical Summary
In the prior art, the external parameter errors between IMU and LiDAR have a great impact on the estimation accuracy of unmanned parking spaces, and the existing external parameter estimation methods are insufficient in high dynamic conditions, making it difficult to adapt to the installation environment of automobile sensors.
Using the IMU/LiDAR external parameters online estimation method based on factor graph, the nonlinear least squares problem and singular value decomposition are constructed, combined with the data of IMU and LiDAR for fusion, and the old nodes are eliminated using the sliding window strategy to realize the online estimation of external parameters. and optimize.
Effectively isolate the impact of external parameter error on position estimation, enhance system robustness and reliability, reduce manual intervention, and is suitable for high dynamic application scenarios, improve automation and convenience of use.
Smart Images

Figure CN119001684A_ABST
Abstract
Description
Technical Field
[0001] The present invention belongs to the field of sensor extrinsic parameter calibration, and in particular relates to an IMU / LiDAR extrinsic parameter online estimation method based on a factor graph. Background Art
[0002] With the development of autonomous driving technology, unmanned vehicles are increasingly used in various industrial scenarios such as inspection, logistics, and security. For tasks in such known scenarios, the commonly used navigation solution is to pre-build a point cloud map of the scene, and then use a map matching algorithm to obtain the position of the unmanned vehicle. Therefore, how to build a high-precision point cloud map in a large range of scenarios is one of the current research focuses of researchers.
[0003] In the task of simultaneous positioning and mapping, each laser point of the LiDAR is generated in a different reference posture during the motion process, which is the root cause of the LiDAR motion distortion. In autonomous driving, the motion error of the laser frame caused by motion cannot be ignored, so the motion distortion of the LiDAR is usually eliminated with the help of IMU. The construction and positioning of high-precision maps are highly dependent on the fusion of LiDAR and IMU. The installation error of IMU and LiDAR is also called the external parameter error, which will have a great impact on the accuracy of pose estimation. Therefore, accurate external parameter estimation of LiDAR and IMU is crucial for determining the coordinate relationship between them and performing sensor fusion. The external parameter estimation methods in the prior art include Lidar-align and discrete time-based methods. Lidar-align is used to find the external calibration between 3D LiDAR and 6-DOF pose sensor, but this method points out that accurate results require highly non-planar motion, which makes this technology less suitable for calibrating sensors installed on cars. Discrete time-based methods use interpolation between key moment poses, but this sacrifices accuracy, especially in highly dynamic situations. Summary of the invention
[0004] The purpose of the present invention is to propose an IMU / LiDAR extrinsic parameter online estimation method based on factor graph, which can effectively isolate the influence of extrinsic parameter errors on the pose estimation accuracy.
[0005] The present invention is achieved through the following technical solutions: An IMU / LiDAR extrinsic parameter online estimation method based on factor graph comprises the following steps: Step S1: Based on the trajectories measured by IMU at multiple times And the trajectory solved by LiDAR odometer , construct a nonlinear least squares problem , the nonlinear least squares problem is solved to obtain and The initial estimate of Represents the arm between IMU and LiDAR, Represents the rotation matrix from the machine system to the navigation coordinate system at the initial moment, express k The position solved by IMU at that moment, express k The pose calculated by LiDAR odometer at this moment The corresponding translation vector, express The initial estimate of Step S2: Based on the factor graph framework, the IMU information and the LiDAR odometer are fused, and the loss function used for the fusion is , the fusion process uses the data obtained in step S1 and The initial estimate of represents the IMU measurement residual, represents the LiDAR measurement residual, , represents the covariance matrix of the IMU measurement, Represents the measurement value of the LiDAR odometer, represents the covariance matrix of LiDAR measurements, express k The position of the unmanned vehicle in the navigation coordinate system at this moment.
[0006] Furthermore, in step S1, the nonlinear least squares problem is first solved by singular value decomposition to obtain , and then according to the trajectory measured by IMU at multiple times and the trajectory solved by LiDAR odometer, As the initial value, the nonlinear least squares problem is solved by nonlinear optimization, and we get and The initial estimate of .
[0007] Furthermore, the machine system coincides with the LiDAR coordinate system. The LiDAR coordinate system takes the center of the LiDAR as the origin, the X-axis points forward along the horizontal axis of the LiDAR, the Y-axis is perpendicular to the X-axis and points to the left, and the Z-axis is perpendicular to the XY plane, forming a right-handed coordinate system. The navigation coordinate system is a northeast sky system with the initial position of the IMU as the origin.
[0008] Furthermore, in step S1, the relationship between the IMU measurement and the LiDAR measurement is expressed as ,in, express The corresponding rotation matrix.
[0009] Furthermore, in step S2, the IMU measurement residual is expressed as ,in, express k The three-dimensional coordinates of the vehicle equipped with IMU and LiDAR at all times in the navigation coordinate system. express k The rotation matrix from the machine coordinate system to the navigation coordinate system at this moment.
[0010] Furthermore, in step S2, the LiDAR measurement residual is expressed as ,in, , Indicates that only The imaginary part of represents the state quantity to be estimated, , express k The rotation matrix of the vehicle posture transformation from time -1 to time 0, express k The position of the vehicle in the navigation coordinate system at all times, express k The rotation matrix of the LiDAR coordinate system pose transformation from time -1 to time 0, express k Time has come k -1 is the translation part of the change in the body coordinate system attitude measured by the radar at time, express k Time has come k -1 is the rotation part of the change in the body coordinate system attitude measured by the radar at time, express k -1 The rotation quaternion of the body coordinate system relative to the navigation coordinate system at the moment of attitude transformation, express k The rotation quaternion of the body coordinate system relative to the navigation coordinate system at this moment, express k The translation part of the radar coordinate system attitude transformation from time to time 0, express k The translation part of the radar coordinate system attitude transformation from time -1 to time 0.
[0011] Furthermore, in step S2, a sliding window strategy is used to remove old nodes, and a fixed number of nodes and constraints are saved for fusion.
[0012] The present invention has the following beneficial effects: The present invention firstly constructs a nonlinear least squares problem according to the trajectories measured by the IMU and the trajectories solved by the LiDAR odometer in multiple moments, and solves the nonlinear least squares problem to obtain the initial values of external parameters, wherein the external parameters include the lever arm between the IMU and the LiDAR and the rotation matrix from the machine system to the navigation coordinate system at the initial moment, and then fuses the IMU information and the LiDAR odometer based on the factor graph framework, so as to realize online estimation of the external parameters, effectively deal with the sensor installation error and the drift generated during long-term use, enhance the robustness and reliability of the system, perform outstandingly in applications such as high-precision positioning and mapping, and is suitable for application scenarios such as unmanned driving and robot navigation that require high dynamic effects, and the realization of online estimation reduces the dependence on manual intervention, does not require frequent offline calibration, and greatly improves the automation level and ease of use of the system. BRIEF DESCRIPTION OF THE DRAWINGS
[0013] The present invention is further described in detail below with reference to the accompanying drawings.
[0014] Figure 1 It is a schematic block diagram of the principle of the present invention.
[0015] Figure 2 Schematic diagram of each coordinate system of the present invention.
[0016] Figure 3 Schematic diagram of the factor graph framework of the present invention. DETAILED DESCRIPTION
[0017] In this embodiment, the LiDAR (laser radar) and IMU (inertial measurement unit) are fixed on the vehicle. First, make sure to choose the right location and fixture to ensure that the two sensors will not be affected by vibration or other factors during the vehicle's driving. Next, power on the two sensors to ensure that they work properly. Then, connect the data ports of the two sensors to a computer equipped with the Ubuntu operating system to facilitate data transmission and processing.
[0018] After everything is connected, start the vehicle and initiate the calibration process. To ensure the accuracy of the calibration, the vehicle needs to be operated in different driving modes, including driving in a straight line and making left and right turns. During this process, the calibration program will start to collect data from the lidar and IMU in real time and perform online calibration. In this way, the position information and motion data of the sensors can be accurately matched, thereby improving the positioning and navigation accuracy of the vehicle. The entire process needs to be carefully monitored to ensure the integrity of the data collection and the accuracy of the calibration results.
[0019] The coordinate system used in the present invention is defined as follows: Figure 2 As shown, the LiDAR coordinate system ( lThe X-axis is based on the LiDAR center as the origin. l X Along the LiDAR horizontal axis pointing forward, the Y axis l Y The Z axis is perpendicular to the X-axis and points to the left, and the Z axis is perpendicular to the XY plane, forming a right-handed coordinate system. The navigation coordinate system ( n The coordinate system is the northeast sky system with the initial position of the IMU as the origin, and the body coordinate system ( b The IMU coordinate system ( i The origin of the coordinate system is located at the center of the IMU, and the directions of the coordinate axes are the same as those of the LiDAR coordinate system.
[0020] like Figure 1 As shown in FIG. 1 , the IMU / LiDAR external parameter online estimation method based on factor graph includes the following steps: Step S1: Based on the trajectories measured by IMU at multiple times And the trajectory solved by LiDAR odometer , construct a nonlinear least squares problem , the nonlinear least squares problem is solved to obtain and The initial estimate of Represents the arm between IMU and LiDAR, Represents the rotation matrix from the machine system to the navigation coordinate system at the initial time (time 0), express k The IMU solves the navigation coordinate system at this moment ( n System) under the position, express k The pose calculated by LiDAR odometer at this moment The corresponding translation vector, express The initial estimate of The process of obtaining the trajectory solved by the LiDAR odometer includes: extracting features and matching features on the original point cloud obtained by the LiDAR to obtain trajectory points. This process is a prior art.
[0021] set up k At this moment, the pose solved by LiDAR is , the three-dimensional position information solved by IMU is , then the relationship between IMU measurement and LiDAR measurement is constructed as ,in, express The corresponding rotation matrix.
[0022] When solving the nonlinear least squares problem, we first solve the nonlinear least squares problem by singular value decomposition. , and then according to the trajectory measured by IMU at multiple times and the trajectory solved by LiDAR odometer, As the initial value, the nonlinear least squares problem is solved by nonlinear optimization, and we get and Since the unmanned vehicle does not have large pitch and roll motions during movement, the error caused by the Z-axis component of the lever arm is difficult to be excited. Therefore, this paper only estimates the X-axis component and Y-axis component of the lever arm.
[0023] Step S2: Based on the factor graph framework, the IMU information and the LiDAR odometer are fused, and the loss function used for the fusion is , the fusion process uses the data obtained in step S1 and The initial estimate of represents the IMU measurement residual, represents the LiDAR measurement residual, , Indicates the positioning result of IMU in the navigation coordinate system, represents the covariance matrix of the IMU measurement, Represents the measurement value of the LiDAR odometer, represents the covariance matrix of LiDAR measurements, express k The position of the vehicle equipped with IMU and LiDAR in the navigation coordinate system at all times.
[0024] like Figure 3 The factor graph framework shown in Figure 1 uses LiDAR odometer observations and IMU observations as constraints to optimize the two state quantities of vehicle position and sensor extrinsic parameters. In order to reduce the amount of calculation, only the pose of the key frame is optimized during the calculation, and the distance of the key frame is 0.5m. The state quantity to be estimated is expressed as , .
[0025] The three-dimensional position information obtained by IMU The sensor is located in the northeast sky, so it is necessary to use k The attitude of the moment With lever arm Will Compensate to LiDAR center: , then the IMU measurement residual is expressed as ,in, express kThe three-dimensional coordinates of the unmanned vehicle in the navigation coordinate system at that moment. express k The rotation matrix from the machine coordinate system to the navigation coordinate system at this moment.
[0026] To reduce the influence of odometer drift on pose estimation, LiDAR constructs pose constraints between two frames. k-1 Moment and k The poses solved by LiDAR at the moment are and , then the LiDAR measurement is , thus constructing the LiDAR measurement residual as ,in, Indicates that only The imaginary part of express k The rotation matrix part of the vehicle posture transformation from time -1 to time 0, express k The position of the vehicle in the navigation coordinate system at all times, express k The rotation matrix of the LiDAR coordinate system pose transformation from time -1 to time 0, express k Time has come k -1 is the translation part of the change in the body coordinate system attitude measured by the radar at time, express k Time has come k -1 is the rotation part of the change in the body coordinate system attitude measured by the radar at time, express k -1 The rotation quaternion of the body coordinate system relative to the navigation coordinate system at the moment of attitude transformation, express k The rotation quaternion of the body coordinate system relative to the navigation coordinate system at this moment, express k The translation part of the radar coordinate system attitude transformation from time to time 0, express k The translation part of the radar coordinate system attitude transformation from time -1 to time 0.
[0027] In the backend graph optimization, the number of nodes will gradually increase over time, accompanied by a gradual increase in the amount of calculation. In order to limit the amount of calculation, the present invention adopts a sliding window strategy to remove old nodes and save a fixed number of nodes and constraints for optimization.
[0028] The above description is only a preferred embodiment of the present invention, and therefore cannot be used to limit the scope of implementation of the present invention. That is, equivalent changes and modifications made according to the scope of the patent application of the present invention and the contents of the specification should still fall within the scope covered by the patent of the present invention.
Claims
1. A factor graph-based IMU / LiDAR extrinsic parameter online estimation method, characterized by: The steps include: Step S1: Based on the trajectories measured by IMU at multiple times And the trajectory solved by LiDAR odometer , construct a nonlinear least squares problem , the nonlinear least squares problem is solved to obtain and The initial estimate of Represents the arm between IMU and LiDAR, Represents the rotation matrix from the machine system to the navigation coordinate system at the initial moment, express k The position solved by IMU at that moment, express k The pose calculated by LiDAR odometer at this moment The corresponding translation vector, express The initial estimate of Step S2: Based on the factor graph framework, the IMU information and the LiDAR odometer are fused, and the loss function used for the fusion is , the fusion process uses the data obtained in step S1 and The initial estimate of represents the IMU measurement residual, represents the LiDAR measurement residual, , represents the covariance matrix of the IMU measurement, Represents the measurement value of the LiDAR odometer, represents the covariance matrix of LiDAR measurements, express k The position of the unmanned vehicle in the navigation coordinate system at this moment.
2. The method for online estimation of IMU / LiDAR extrinsic parameters based on factor graph according to claim 1, characterized in that: In step S1, the nonlinear least squares problem is first solved by singular value decomposition to obtain Then, according to the trajectory measured by IMU at multiple times and the trajectory solved by LiDAR odometer, As the initial value, the nonlinear least squares problem is solved by nonlinear optimization, and we get and The initial estimate of .
3. The method for online estimation of IMU / LiDAR extrinsic parameters based on factor graph according to claim 1, characterized in that: The aircraft coordinate system coincides with the LiDAR coordinate system. The LiDAR coordinate system takes the center of the LiDAR as the origin, the X-axis points forward along the horizontal axis of the LiDAR, the Y-axis is perpendicular to the X-axis and points to the left, and the Z-axis is perpendicular to the XY plane, forming a right-handed coordinate system. The navigation coordinate system is a northeast sky system with the initial position of the IMU as the origin.
4. The method for online estimation of IMU / LiDAR extrinsic parameters based on factor graph according to claim 1, 2 or 3, characterized in that: In step S1, the relationship between the IMU measurement and the LiDAR measurement is expressed as: ,in, express The corresponding rotation matrix.
5. The method for online estimation of IMU / LiDAR extrinsic parameters based on factor graph according to claim 1, 2 or 3, characterized in that: In step S2, the IMU measurement residual is expressed as ,in, express k The three-dimensional coordinates of the vehicle equipped with IMU and LiDAR at all times in the navigation coordinate system. express k The rotation matrix from the machine coordinate system to the navigation coordinate system at this moment.
6. The method for online estimation of IMU / LiDAR extrinsic parameters based on factor graph according to claim 1, 2 or 3, characterized in that: In step S2, the LiDAR measurement residual is expressed as ,in, , Indicates that only The imaginary part of represents the state quantity to be estimated, , express k The rotation matrix of the vehicle posture transformation from time -1 to time 0, express k The position of the vehicle in the navigation coordinate system at all times, express k The rotation matrix of the LiDAR coordinate system pose transformation from time -1 to time 0, express k Time has come k -1 is the translation part of the change in the body coordinate system attitude measured by the radar at time, express k Time has come k -1 is the rotation part of the change in the body coordinate system attitude measured by the radar at time, express k -1 The rotation quaternion of the body coordinate system relative to the navigation coordinate system at the moment of attitude transformation, express k The rotation quaternion of the body coordinate system relative to the navigation coordinate system at this moment, express k The translation part of the radar coordinate system attitude transformation from time to time 0, express k The translation part of the radar coordinate system attitude transformation from time -1 to time 0.
7. The method for online estimation of IMU / LiDAR extrinsic parameters based on factor graph according to claim 1, 2 or 3, characterized in that: In step S2, a sliding window strategy is used to remove old nodes, and a fixed number of nodes and constraints are saved for fusion.
Citation Information
Patent Citations
Multi-sensor fusion positioning and mapping method based on graph optimization
CN116772828A
LiDAR-GPS / IMU self-calibration method of movable carrier
CN116794640A
External parameter calibration method and apparatus for combined navigation device and laser radar
WO2023131123A1
