Calibration method and system for front and back double laser radars in non-overlapping view field area

Abstract

The invention discloses a front and rear dual-laser radar calibration method and system for a non-overlapping view field area, and the method comprises the steps: enabling a front laser radar and a rear laser radar to be installed at the front and rear positions of an unmanned vehicle, and arranging an IMU sensor in the middle of a vehicle body; constructing a three-dimensional point cloud map map1 according to the front laser radar point cloud data and the IMU data, and constructing a three-dimensional point cloud map map2 according to the rear laser radar point cloud data and the IMU data; performing filtering and downsampling preprocessing on the map1 and the map2; performing coarse registration on the preprocessed map1 and map2 to obtain a matching initial matrix Tinit; performing fine registration on the preprocessed map1 and map2 according to Tinit, performing optimization to obtain an external parameter calibration matrix T *, and performing transformation on the front laser radar and the rear laser radar; the method has the advantages that the precision and the efficiency are improved, and laser radars installed front and back and areas with unobvious characteristics can also be calibrated.

Description

Technical Field

[0001] The present invention relates to the field of multi-sensor calibration and fusion, and in particular to a front and rear dual laser radar calibration method and system with non-overlapping field of view areas. Background Art

[0002] For mobile robots or unmanned vehicles, achieving intelligence and autonomy requires the installation of multiple sensors to fuse and perceive the surrounding environment. As one of the most critical sensors, lidar often requires the cooperation of multiple lidars to perceive the surrounding environment. The cooperation of multiple lidars requires the external parameter calibration of multiple lidars, and the use of external parameter matrices to unify multiple lidars.

[0003] Currently, there are two main types of multi-lidar calibration methods: one requires a calibration plate or marker, such as the unmanned multi-lidar calibration and fusion mapping method for complex environments disclosed in Chinese Patent Publication No. CN117872330A. It first uses ICP point cloud registration technology for calibration to obtain the transformation matrix T1, then solves the constraints of plane and boundary parameters to obtain the transformation matrix T2. The two are averaged and fused to achieve multi-lidar calibration. Finally, multi-lidar fusion mapping is achieved based on LEGO-LOAM. This method requires a calibration plate and is complex to operate. It must be based on the premise that each lidar scans the calibration plate. For front and rear lidars with non-overlapping field of view areas, it is impossible to complete the scanning of each lidar to the calibration plate, and it is impossible to establish the constraint relationship of the same calibration plate, and it is impossible to optimize and solve the external parameter calibration matrix.

[0004] Another laser radar calibration method does not need a calibration board or a marker, such as the laser radar online calibration method, device and system for unmanned mine truck disclosed in Chinese patent publication No. CN119738801A. The method first uses the main laser radar to construct a local map, and then uses other auxiliary laser radars to preprocess data and establish a matching relationship with the local map, that is, the corresponding relationship between the feature points obtained by preprocessing the auxiliary laser radar and the feature points obtained by preprocessing the local map, and optimizes the objective function to obtain the calibration external parameter result. This calibration method solves the problems of the above-mentioned method which needs a calibration board or a marker, but introduces new problems as follows: 1. Motion distortion exists in the main laser radar and the auxiliary laser radar in the motion state, which brings a large error to the local map and the current point cloud, so that the calibration accuracy is not enough. 2. For the front and rear mounted laser radars, the main laser radar and the auxiliary laser radar cannot establish a correlation relationship and cannot be matched and calibrated; 3. This method depends on feature points, and cannot match and calibrate in areas where features are not obvious; 4. Only the feature points of the laser point cloud of the auxiliary radar are matched with the local map of the main radar, and there are error feature points in the preprocessing of the auxiliary radar, which leads to mismatching and poor accuracy; 5. The point-to-point matching method of the corresponding relationship between the feature points obtained by preprocessing the auxiliary laser radar and the feature points obtained by preprocessing the local map is time-consuming and has large calculation amount. SUMMARY

[0005] The technical problem to be solved by the present application is that the existing laser radar calibration method without overlapping field of view area has the problems of insufficient calibration accuracy, low efficiency, and inability to calibrate for laser radars mounted in front and rear and areas where features are not obvious.

[0006] The present application solves the above technical problems by the following technical means: a front and rear dual laser radar calibration method without overlapping field of view area, comprising:

[0007] S1, the front laser radar and the rear laser radar are installed at two positions in front and rear of the unmanned vehicle, and an IMU sensor is arranged in the middle of the vehicle body;

[0008] S2, a three-dimensional point cloud map map1 is constructed according to the front laser radar point cloud data and the IMU data, and a three-dimensional point cloud map map2 is constructed according to the rear laser radar point cloud data and the IMU data;

[0009] S3, filtering and downsampling preprocessing are performed on map1 and map2;

[0010] S4, coarse registration is performed on the preprocessed map1 and map2 to obtain a matching initial matrix T init ;

[0011] S5, fine registration is performed on the preprocessed map1 and map2 according to T init , and an external parameter calibration matrix T* ;

[0012] S6. Calibrate the matrix T based on external parameters * Transform the front lidar and the rear lidar.

[0013] In the present invention, the front lidar and the rear lidar are installed at the front and rear positions of the unmanned vehicle, and an IMU sensor is set in the middle of the vehicle body. The point cloud data of the front and rear lidars and the IMU data are used to respectively construct a three-dimensional point cloud map map1 and a three-dimensional point cloud map map2 for matching. The external parameter calibration matrix obtained by matching is used to transform the lidar coordinates, thereby finally realizing the lidar calibration. In the whole process, the global point cloud directly matches the map, which solves the problem that the lidars installed at the front and rear and the areas with unclear features cannot be calibrated. In the registration process, coarse registration is first performed to improve efficiency, and then fine registration is performed to improve accuracy.

[0014] Furthermore, S1 includes:

[0015] The front lidar scans the 180° area in front, and the rear lidar scans the 180° area in the back; the vehicle is controlled to rotate one circle to collect point cloud data and IMU data from the two lidars.

[0016] Furthermore, the construction method of the three-dimensional point cloud map map1 and the three-dimensional point cloud map map2 is the same, and the specific process is:

[0017] S2.1. Predict the motion state of IMU sensor data based on forward propagation.

[0018] S2.2. Using the rotation matrix from the lidar coordinate system to the IMU coordinate system Translation matrix from the lidar coordinate system to the IMU coordinate system Convert the predicted state of the IMU into the state of the lidar point, use back propagation to get the state of the lidar point in each frame, and then project the lidar point in each frame to the end time of the frame scan to get the compensated lidar point;

[0019] S2.3. Convert the compensated lidar point to the global coordinate system to obtain a local map, and calculate a measurement model between the lidar point and its nearest neighbor in the local map. The measurement model is the relationship between the true state of the lidar point and the noise.

[0020] S2.4. Calculate the linearized residual between the lidar point and the nearest neighbor point in the local map based on the measurement model. The lidar linearized residual is a relationship between the error state of the IMU and the noise. The error state of the IMU is calculated based on the predicted state of the IMU.

[0021] S2.5. Construct a total error equation based on the IMU error state and the lidar linearization residual, and iteratively solve for the minimum error state based on the IMU predicted state at each moment.

[0022] S2.6. Correct the predicted state of the IMU based on the solved error state to obtain the optimal state of the IMU. Convert the optimal state of the IMU to the optimal state of the lidar based on the coordinate conversion relationship between the IMU and the lidar. Convert the optimal state of the lidar to the global coordinate system to construct a three-dimensional point cloud map.

[0023] Furthermore, S4 includes a method for calculating the eigenvectors and covariance matrices corresponding to points in map1 and map2. The two methods are the same, specifically including:

[0024] S4.1. For each point map_p of the 3D point cloud map i , find point map_p i Point map_p within the neighborhood range r j Fit the plane and get the covariance matrix Point map_p i Normal vector Point map_p j Normal vector

[0025] S4.2. Calculation point map_p i and the neighborhood point map_p j Three angle values:

[0026]

[0027] Among them, α is the normal vector With normal vector The angle of is the normal vector With vector map_p i map_p j The angle between the two, θ is the normal vector With vector map_p i map_p j The angle between

[0028] S4.3, α, The three angles θ are quantized into 11-dimensional histograms and concatenated into a 33-dimensional vector S(p);

[0029] S4.4, perform weighted accumulation of vector S(p) according to distance and calculate the eigenvector F(p i ):

[0030]

[0031] Among them, d(map_p i ,map_p j ) is the point map_p i With point map_p j The distance between them, k represents the point map_p i The total number of points within the neighborhood range r, i represents the i-th point of the three-dimensional point cloud map.

[0032] Furthermore, S4 also includes:

[0033] S4.5. Obtain point p1 in map1 according to S4.1 to S4.4 i The eigenvector F(p1 i ) and the covariance matrix Point q2 in map2 i The eigenvector F(q2 i ) and the covariance matrix

[0034] S4.6, according to the characteristic vector F(p1 i ) and the eigenvector F(q2 i ) Difference filter closest matching point pair M={(p1 i ,q2 i )|arg min‖F(p1 i )-F(q2 i )‖2};

[0035] S4.7. Constructing a matching error function based on the matching point pair M Calculate the matching initial matrix T init =[R init |t init ], where R init represents the initial rotation matrix, t init Represents the initial translation matrix.

[0036] Furthermore, S5 includes:

[0037] S5.1, assume that each point p1 in map1 i With each point q2 in map2 i It conforms to the Gaussian distribution, that is Among them, p1 i ,q2 i are the noise-contaminated measurements corresponding to map1 and map2, respectively; N represents Gaussian distribution;

[0038] S5.2. Calculate each point p1 in map1 iWith each point q2 in map2 i The fine registration residual of :

[0039] d i =p1 i -T * q2 i

[0040] Among them, T * represents the external parameter calibration matrix, d i is the precise registration residual, which conforms to the Gaussian distribution, that is,

[0041] S5.3. Given the matching initial matrix T init As the external parameter calibration matrix T * The initial value of T0 is calculated and used as the external parameter calibration matrix T for the next iteration * , iteratively calculate the external parameter calibration matrix T * The final value of is expressed as follows:

[0042]

[0043] T * =arg min(T0,T1,…,T n-1 ,T n )

[0044] Among them, M1 is the total number of sampling points in the 3D point cloud map, T n is the extrinsic calibration matrix T obtained in the nth iteration * The value of .

[0045] The present invention also provides a front and rear dual laser radar calibration system with non-overlapping field of view areas, comprising:

[0046] The data acquisition module is used for the front and rear lidars installed at the front and rear of the unmanned vehicle, and the IMU sensor is set in the middle of the vehicle body;

[0047] The mapping module is used to construct a 3D point cloud map map1 based on the front lidar point cloud data and the IMU data, and to construct a 3D point cloud map map2 based on the rear lidar point cloud data and the IMU data;

[0048] Preprocessing module, used for filtering and downsampling preprocessing of map1 and map2;

[0049] The coarse registration module is used to coarsely register the preprocessed map1 and map2 to obtain the matching initial matrix T init ;

[0050] Fine registration module, used toinit Perform precise registration on the preprocessed map1 and map2, and optimize the external parameter calibration matrix T * ;

[0051] Calibration module, used to calibrate the matrix T according to external parameters * Transform the front lidar and the rear lidar.

[0052] Furthermore, the data acquisition module is also used to:

[0053] The front lidar scans the 180° area in front, and the rear lidar scans the 180° area in the back; the vehicle is controlled to rotate one circle to collect point cloud data and IMU data from the two lidars.

[0054] Furthermore, the construction method of the three-dimensional point cloud map map1 and the three-dimensional point cloud map map2 is the same, and the specific process is:

[0055] S2.1. Predict the motion state of IMU sensor data based on forward propagation.

[0056] S2.2. Using the rotation matrix from the lidar coordinate system to the IMU coordinate system Translation matrix from the lidar coordinate system to the IMU coordinate system Convert the predicted state of the IMU into the state of the lidar point, use back propagation to get the state of the lidar point in each frame, and then project the lidar point in each frame to the end time of the frame scan to get the compensated lidar point;

[0057] S2.3. Convert the compensated lidar point to the global coordinate system to obtain a local map, and calculate a measurement model between the lidar point and its nearest neighbor in the local map. The measurement model is the relationship between the true state of the lidar point and the noise.

[0058] S2.4. Calculate the linearized residual between the lidar point and the nearest neighbor point in the local map based on the measurement model. The lidar linearized residual is a relationship between the error state of the IMU and the noise. The error state of the IMU is calculated based on the predicted state of the IMU.

[0059] S2.5. Construct a total error equation based on the IMU error state and the lidar linearization residual, and iteratively solve for the minimum error state based on the IMU predicted state at each moment.

[0060] S2.6. Correct the predicted state of the IMU based on the solved error state to obtain the optimal state of the IMU. Convert the optimal state of the IMU to the optimal state of the lidar based on the coordinate conversion relationship between the IMU and the lidar. Convert the optimal state of the lidar to the global coordinate system to construct a three-dimensional point cloud map.

[0061] Furthermore, the coarse registration module includes a method for calculating the eigenvectors and covariance matrices corresponding to points in map1 and map2. The two methods are the same, specifically including:

[0062] S4.1. For each point mao_p in the 3D point cloud map i , find point map_p i Point mao_p within the neighborhood range r j Fit the plane and get the covariance matrix Point map_p i Normal vector Point map_p j Normal vector

[0063] S4.2. Calculation point map_p i and the neighborhood point map_p j Three angle values:

[0064]

[0065]

[0066] Among them, α is the normal vector With normal vector The angle of is the normal vector With vector map_p i map_p j The angle between the two, θ is the normal vector With vector map_p i map_p j The angle between

[0067] S4.3, α, The three angles θ are quantized into 11-dimensional histograms and concatenated into a 33-dimensional vector S(p);

[0068] S4.4, perform weighted accumulation of vector S(p) according to distance and calculate the eigenvector F(p i ):

[0069]

[0070] Among them, d(map_p i ,map_p j ) is the point map_p i With point map_p j The distance between them, k represents the point map_p iThe total number of points within the neighborhood range r, i represents the i-th point of the three-dimensional point cloud map.

[0071] Furthermore, the coarse registration module is also used to:

[0072] S4.5. Obtain point p1 in map1 according to S4.1 to S4.4 i The eigenvector F(p1 i ) and the covariance matrix Point q2 in map2 i The eigenvector F(q2 i ) and the covariance matrix

[0073] S4.6, according to the characteristic vector F(p1 i ) and the eigenvector F(q2 i ) Difference filter closest matching point pair M={(p1 i ,q2 i )|arg min‖F(p1 i )-F(q2 i )‖2};

[0074] S4.7. Constructing a matching error function based on the matching point pair M Calculate the matching initial matrix T init =[R init |t init ], where R init represents the rotation matrix, t init Represents the translation matrix.

[0075] Furthermore, the calibration module is also used to:

[0076] S5.1, assume that each point p1 in map1 i With each point q2 in map2 i It conforms to the Gaussian distribution, that is Among them, p1 i ,q2 i are the noise-contaminated measurements corresponding to map1 and map2, respectively; N represents Gaussian distribution;

[0077] S5.2. Calculate each point p1 in map1 i With each point q2 in map2 i The fine registration residual of :

[0078] d i =p1 i -T * q2 i

[0079] Among them, T * represents the external parameter calibration matrix, d i is the precise registration residual, which conforms to the Gaussian distribution, that is,

[0080] S5.3. Given the matching initial matrix T init As the external parameter calibration matrix T * The initial value of T0 is calculated and used as the external parameter calibration matrix T for the next iteration * , iteratively calculate the external parameter calibration matrix T * The final value of is expressed as follows:

[0081]

[0082] T * =arg min(T0,T1,…,T n-1 ,T n )

[0083] Among them, M1 is the total number of sampling points in the 3D point cloud map, T n is the extrinsic calibration matrix T obtained in the nth iteration * The value of .

[0084] The advantages of the present invention are:

[0085] (1) The front laser radar and the rear laser radar of the present invention are installed at the front and rear positions of the unmanned vehicle, and an IMU sensor is set in the middle of the vehicle body. The point cloud data of the front and rear laser radars and the IMU data are used to respectively construct a three-dimensional point cloud map map1 and a three-dimensional point cloud map map2 for matching. The laser radar coordinate transformation is performed using the external parameter calibration matrix obtained by matching, thereby finally realizing the laser radar calibration. In the whole process, the global point cloud directly matches the map, solving the problem that the laser radars installed at the front and rear and the areas with unclear features cannot be calibrated. In addition, the registration process first performs coarse registration to improve efficiency, and then performs fine registration to improve accuracy.

[0086] (2) The present invention solves the problem of calibrating the front and rear dual laser radars with non-overlapping fields of view. The vehicle body is rotated one circle to collect the laser radar data and IMU data installed at the front and rear positions of the vehicle body, and three-dimensional point cloud maps of the environment of the front and rear laser radars are constructed respectively. The coarse alignment is used to provide the initial value to accelerate the matching iteration convergence speed of the fine alignment.

[0087] (3) The present invention combines the motion equation of the IMU with the measurement residual model of the lidar to construct a three-dimensional point cloud map, thereby avoiding the point cloud distortion in motion. At the same time, instead of using point cloud features, the global point cloud is used to directly match the map, thereby avoiding the poor mapping accuracy in environments with unclear features.

[0088] (4) The automated coarse calibration combined with fine registration method of the present invention does not require manual initial value provision. The distance matching error function is constructed by quantizing the encoded feature vector to obtain the initial calibration value, and the fine registration residual is constructed considering the local covariance matrix of the matching points to obtain the final extrinsic parameter calibration matrix. BRIEF DESCRIPTION OF THE DRAWINGS

[0089] Figure 1 The front and rear lidar installation positions and field of view diagram of a front and rear dual lidar calibration method with no overlapping field of view areas disclosed in an embodiment of the present invention, wherein: Figure 1 (a) is the left view, Figure 1 (b) is a top view;

[0090] Figure 2 This is a flow chart of a method for calibrating front and rear dual laser radars with non-overlapping fields of view disclosed in an embodiment of the present invention;

[0091] Figure 3 This is a visualization diagram of the three-dimensional point cloud data of the front and rear lidars in a calibration method for front and rear dual lidars with non-overlapping fields of view disclosed in an embodiment of the present invention, wherein: Figure 3 (a) is the visualization diagram of the front lidar 3D point cloud data. Figure 3 (b) is the visualization of the post-lidar 3D point cloud data;

[0092] Figure 4 The result of constructing a three-dimensional point cloud map in a front and rear dual laser radar calibration method with non-overlapping field of view areas disclosed in an embodiment of the present invention;

[0093] Figure 5 This is a verification diagram of unifying the front and rear lidar results using an extrinsic calibration matrix in a front and rear dual lidar calibration method with non-overlapping field of view areas disclosed in an embodiment of the present invention. DETAILED DESCRIPTION

[0094] To make the objectives, technical solutions, and advantages of the embodiments of the present invention more clear, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. All other embodiments obtained by ordinary technicians in this field based on the embodiments of the present invention without making any creative efforts shall fall within the scope of protection of the present invention.

[0095] Example 1

[0096] For the current autonomous navigation of unmanned vehicles, multiple LiDAR sensors are needed to sense objects around the vehicle body and realize fully autonomous movement. The first thing to be solved is to calibrate the external parameters of multiple LiDARs, so as to complete the unification of different LiDAR coordinate systems. The present invention mainly addresses the problem of front and rear mounted LiDARs with no overlapping front and rear field of view areas. Most of the current calibration methods are suitable for multiple LiDARs installed with overlapping areas. The two LiDAR models used in the present invention are hybrid solid-state MID-360, with a horizontal field of view of 360° and a vertical field of view of 59°. They output 200,000 points per second and have built-in six-axis IMU data, which can meet the needs of high-precision environmental map construction. Figure 1 As shown, the unmanned vehicle is in four-wheel differential motion mode, with two MID-360 laser radars installed at the front and rear, which can visualize 3D point cloud data in real time in RVIZ. The front laser radar can only scan the front 180° field of view, and the rear laser radar can only scan the rear 180° field of view. The middle vehicle body blocks the view so that there is no overlapping field of view. Figure 1 (a) is the left view, Figure 1 (b) is a top view. The IMU provides acceleration and gyroscope measurements and is located at the center of the vehicle. Alternatively, the MID-360's built-in IMU data can be used.

[0097] like Figure 2 As shown, embodiment 1 of the present invention provides a method for calibrating front and rear dual laser radars with non-overlapping field of view areas, and the specific steps are as follows:

[0098] S1. According to Figure 1 Determine the installation locations of two MID-360 lidar and IMU sensors, use ROS to subscribe to the front and back lidar topics and the IMU topic on a Ubuntu system, control the vehicle to rotate once, and collect lidar point cloud data and IMU scan data of the surrounding environment.

[0099] S2. Based on the LiDAR and IMU data, a 3D point cloud map is constructed. The front LiDAR point cloud data and IMU data are used to construct a 3D point cloud map map1, and the rear LiDAR point cloud data and IMU data are used to construct a 3D point cloud map map2. The visualization of the front and rear LiDAR 3D point cloud data is shown in the figure below. Figure 3 As shown, Figure 3 (a) is the visualization diagram of the front lidar 3D point cloud data. Figure 3 (b) is the visualization of the post-lidar 3D point cloud data. It includes the following sub-steps:

[0100] S2.1. Predict the motion state of the IMU sensor data based on forward propagation and calculate the predicted state for the i+1th time

[0101]

[0102] Among them, the true state x is defined as R I 、p I 、v I 、b w 、b a , g are rotation, position, speed, gyroscope bias, accelerometer bias, and gravity acceleration respectively. The position is calculated based on the speed and acceleration. I is the IMU coordinate system, L is the lidar coordinate system, and T is the transpose. is the i-th predicted state of IMU in the global coordinate system, and the IMU measurement value u is defined as w m 、a m are the angular velocity and acceleration measured by the IMU, respectively. f(x,u,w) is the kinematic model function of the IMU, and w is the gyroscope noise n ω , gyroscope bias noise n bω , acceleration noise n a , accelerometer bias noise n ba composition, is a generalized addition operator, Δt is the time interval between two adjacent IMU measurements, For f(x,u,w) in u=u i , kinematic model function of the IMU i-th measurement when w=0, is the IMU predicted state for the i+1th time, which is the IMU measurement value that is synchronized with the i+1th lidar point in time and space.

[0103] S2.2. Using the rotation matrix from the lidar coordinate system to the IMU coordinate system Translation matrix from the lidar coordinate system to the IMU coordinate system The LiDAR and IMU are unified in time and space. The state of each LiDAR point in each frame is obtained by back propagation based on the results of the IMU motion state prediction. Refer to the following formula. Then, each LiDAR point in each frame is projected to the end time of the frame scan to obtain the compensated LiDAR point. Projection means that the state of each LiDAR point in each frame is used as the state of the LiDAR point at the end time of the frame scan. The state formula of each LiDAR point in each frame is obtained by back propagation:

[0104]

[0105] Among them, x j is the true state of the jth lidar point, which is the state of the lidar point obtained by transforming the rotation matrix and translation matrix according to the predicted state of the IMU. j is the true state of j-1 laser radar points, u jis the IMU measurement value synchronized with the j-th lidar point in time and space, Δt is the time interval between two adjacent IMU measurements, is a generalized addition operator, f(x j ,u j ,0) is f(x,u,w) at x=x j ,u=u j , kinematic model function with w = 0. Typically, the MID-360 LiDAR data is discretely sampled, outputting 20,000 point cloud data points per frame at 10 Hz. After the rotating vehicle completes one rotation, it is impossible to guarantee that all point clouds in a frame will be in the same position, resulting in distortion and motion distortion. Motion distortion can cause errors in the collected 3D point cloud data, making high-precision mapping and calibration impossible. Therefore, backpropagation is used to unify a frame of point clouds to the moment the frame's scan ended, removing motion distortion and preventing the impact of motion on the collected LiDAR data.

[0106] S2.3. Convert the compensated lidar points to the global coordinate system, obtain the local map, and calculate the lidar points The nearest neighbor in the local map The measurement model:

[0107]

[0108] in, Contains the true state y of the lidar point at the kth moment k , so the above formula can be simplified to h j represents the functional relationship between the true state and noise of the j-th lidar point, is the normal vector of the local map plane, is the pose transformation matrix from the IMU coordinate system I to the global coordinate system G, is the external parameter transformation from the lidar coordinate system L to the IMU coordinate system I, is the jth point in the laser radar coordinate system L, is the noise of the jth point in the laser radar coordinate system L, is the outlier point in the local map The nearest neighbor of

[0109] S2.4. Calculate LiDAR points based on the measurement model The nearest neighbor in the local map The linearized residual of :

[0110]

[0111] Among them, z j is the residual and yk is the laser radar point true state at the kth moment, is the predicted state of the IMU at the kth moment, is the noise of the jth point in the laser radar coordinate system L, v j is the noise source, H j is the Jacobian matrix of the jth laser radar point, is the error state of the IMU at the kth moment, which can be obtained according to the motion state of the IMU at the kth moment represents a generalized subtraction operator. S2.4 and S2.3 mainly construct a residual error model of a three-dimensional point cloud and a local map, instead of using features to construct a method as most of the current methods do, but using a raw three-dimensional point cloud to directly construct, avoiding the failure of mapping or insufficient accuracy caused by the fact that the features in the environment are not obvious.

[0112] S2.5, according to the error state of the IMU and the linearized residual error of the laser radar, the total error equation is constructed, and the minimum error state is iteratively solved according to the predicted state of the IMU at each moment

[0113]

[0114] wherein, is the predicted state of the IMU at the kth moment, is the error state of the IMU at the kth moment, is the covariance of the IMU at the kth moment, z j is the residual error, H j is the Jacobian matrix of the jth laser radar point, R j is the Gaussian noise of the jth laser radar point.

[0115] S2.6, according to the solved error state of the kth moment correct the predicted state of the IMU at the kth moment obtain the optimal state of the IMU, because the laser radar and the IMU are time and space synchronized, according to the coordinate transformation relationship between the IMU and the laser radar, the optimal state of the IMU is converted into the optimal state of the laser radar, that is, the optimal state of the laser radar point at the kth moment is obtained

[0116] S2.7, continuously iteratively update, solve the optimal state of the laser radar point at each moment.

[0117] S2.8, according to the output optimal state, convert the laser radar into a global coordinate system, and construct a three-dimensional point cloud map map, such as Figure 4As shown in the figure, the front lidar point cloud data and IMU data construct a 3D point cloud map map1, and the rear lidar point cloud data and IMU data construct a 3D point cloud map map2.

[0118] S3. Perform filtering and downsampling preprocessing on map1 and map2. Filtering removes outliers and noise points and reduces interference from erroneous points. Downsampling is mainly performed using voxel filtering with a voxel grid size of 0.3 meters to reduce the amount of computation.

[0119] S4. Perform rough registration on the preprocessed map1 and map2 to obtain the matching initial value T init Providing matching initial values ​​can speed up the lidar precision registration process and avoid manual provision of initial values. Instead, the matching initial values ​​are automatically obtained based on the method of encoding feature vectors and building a residual precision registration model, avoiding manual intervention and greatly shortening the overall time consumption of the method. Specifically, it includes the following sub-steps:

[0120] S4.1. For each point map_p in the map i , find point map_p i Point map_p within the neighborhood range r (0.5m) j Fit the plane and get the covariance matrix Point map_p i Normal vector Point map_p j Normal vector The covariance matrix is ​​a matrix whose rows and columns are the serial numbers of the points in the map, and the elements in the covariance matrix are the covariances between the two points at corresponding positions.

[0121] S4.2. Calculation point map_p i and the neighborhood point map_p j The three angle values: α, θ

[0122]

[0123] Among them, α is the normal vector With normal vector The angle of is the normal vector With vector map_p i map_p j The angle between the two, θ is the normal vector map_p with vectors i map_p j Angle;

[0124] S4.3, the above α, The three angles θ are quantized into 11-dimensional histograms and concatenated into a 33-dimensional vector S(p);

[0125] S4.4, perform weighted accumulation of the eigenvector S(p) according to the distance and calculate the eigenvector F(p i ):

[0126]

[0127] Among them, d(map_p i ,map_p j ) is the point map_p i With point map_p j The distance between them, k represents the point map_p i The total number of points within the neighborhood range r, i represents the i-th point of the three-dimensional point cloud map.

[0128] S4.5. According to S4.1 to S4.4, point p1 in map1 can be obtained i Eigenvector F(p1 i ) and the covariance matrix Point q2 in map2 i Eigenvector F(q2 i ) and the covariance matrix

[0129] S4.6, according to the characteristic vector F(p1 i ) and the eigenvector F(q2 i ) Difference filter closest matching point pair M={(p1 i ,q2 i )|arg min‖F(p1 i )-F(q2 i )‖2};

[0130] S4.7, according to the point pair M = {p1 i ,q2 j}Build matching error function Calculate the rough registration to get the matching initial matrix T init =[R init |t init ]; R init represents the rotation matrix, t init Represents the translation matrix.

[0131] S5. According to T init Perform precise registration on the preprocessed map1 and map2, and optimize the external parameter calibration matrix T * ; Use the improved ICP algorithm and introduce it into the midpoint p1 of map1 i The covariance matrix of Point q2 in map2 i The covariance matrix of This improves point cloud registration and avoids the problem of reduced point-to-point matching accuracy caused by individual erroneous points in ICP. The introduction of local covariance in the point cloud can handle more complex environments, be more robust to non-overlapping fields of view, and obtain more stable and accurate results. This includes the following sub-steps:

[0132] S5.1, assume that each point p1 in map1 i With each point q2 in map2 i It conforms to the Gaussian distribution, that is Among them, p1 i ,q2 i are the noise-contaminated measurements corresponding to map1 and map2, respectively; N represents Gaussian distribution;

[0133] S5.2. Calculate each point p1 in map1 i With each point q2 in map2 i The fine registration residual of :

[0134] d i =p1 i -T * q2 i

[0135] Among them, T * represents the external parameter calibration matrix, d i is the precise registration residual, which conforms to the Gaussian distribution, that is,

[0136] S5.3. Given the matching initial matrix T init As the external parameter calibration matrix T * The initial value of T0 is calculated and used as the external parameter calibration matrix T for the next iteration * , iteratively calculate the external parameter calibration matrix T * The final value of:

[0137]

[0138] T * =arg min(T0,T1,…,T n-1 ,T n )

[0139] Among them, M1 is the total number of sampling points in the 3D point cloud map, T n is the extrinsic calibration matrix T obtained in the nth iteration * value.

[0140] S6. Calibrate the matrix T based on external parameters * Transform the front lidar and the rear lidar to verify the calibration results, such as Figure 5 As shown in the figure, the external parameter calibration matrix is ​​used to achieve high-precision coordinate unification of the front and rear installed MID-360 lidars, providing a strong guarantee for the subsequent unmanned vehicle to perceive surrounding obstacles.

[0141] Through the above technical solution, in unmanned vehicle systems, it is often necessary to install multiple laser radars around the vehicle body. The problem is that due to the obstruction of the vehicle body itself, there is a non-overlapping field of view area between the front and rear laser radars. This makes it impossible to establish a constraint relationship between the two laser radars and calibrate them. The present invention uses the above calibration method to solve the problem of multiple radar calibration in non-overlapping field of view areas using dual front and rear laser radars. At the same time, the present invention can perform laser radar calibration without the need for calibration plates or markers, and can adapt to laser radars with low line counts and small point cloud quantities. It is simple to operate and has high accuracy.

[0142] Example 2

[0143] Based on Example 1, Example 2 of the present invention further provides a front and rear dual laser radar calibration system with non-overlapping field of view areas, including:

[0144] The data acquisition module is used for the front and rear lidars installed at the front and rear of the unmanned vehicle, and the IMU sensor is set in the middle of the vehicle body;

[0145] The mapping module is used to construct a 3D point cloud map map1 based on the front lidar point cloud data and the IMU data, and to construct a 3D point cloud map map2 based on the rear lidar point cloud data and the IMU data;

[0146] Preprocessing module, used for filtering and downsampling preprocessing of map1 and map2;

[0147] The coarse registration module is used to coarsely register the preprocessed map1 and map2 to obtain the matching initial matrix T init ;

[0148] Fine registration module, used to init Perform precise registration on the preprocessed map1 and map2, and optimize the external parameter calibration matrix T * ;

[0149] Calibration module, used to calibrate the matrix T according to external parameters * Transform the front lidar and the rear lidar.

[0150] Specifically, the data acquisition module is also used to:

[0151] The front lidar scans the 180° area in front, and the rear lidar scans the 180° area in the back; the vehicle is controlled to rotate one circle to collect point cloud data and IMU data from the two lidars.

[0152] Specifically, the construction method of the three-dimensional point cloud map map1 and the three-dimensional point cloud map map2 is the same, and the specific process is:

[0153] S2.1. Predict the motion state of IMU sensor data based on forward propagation.

[0154] S2.2. Using the rotation matrix from the lidar coordinate system to the IMU coordinate system Translation matrix from the lidar coordinate system to the IMU coordinate system Convert the predicted state of the IMU into the state of the lidar point, use back propagation to get the state of the lidar point in each frame, and then project the lidar point in each frame to the end time of the frame scan to get the compensated lidar point;

[0155] S2.3. Convert the compensated lidar point to the global coordinate system to obtain a local map, and calculate a measurement model between the lidar point and its nearest neighbor in the local map. The measurement model is the relationship between the true state of the lidar point and the noise.

[0156] S2.4. Calculate the linearized residual between the lidar point and the nearest neighbor point in the local map based on the measurement model. The lidar linearized residual is a relationship between the error state of the IMU and the noise. The error state of the IMU is calculated based on the predicted state of the IMU.

[0157] S2.5. Construct a total error equation based on the IMU error state and the lidar linearization residual, and iteratively solve for the minimum error state based on the IMU predicted state at each moment.

[0158] S2.6. Correct the predicted state of the IMU based on the solved error state to obtain the optimal state of the IMU. Convert the optimal state of the IMU to the optimal state of the lidar based on the coordinate conversion relationship between the IMU and the lidar. Convert the optimal state of the lidar to the global coordinate system to construct a three-dimensional point cloud map.

[0159] More specifically, the coarse registration module includes a method for calculating the eigenvectors and covariance matrices corresponding to points in map1 and map2. The two methods are the same, specifically including:

[0160] S4.1. For each point map_p of the 3D point cloud map i , find point map_p i Point map_p within the neighborhood range r jFit the plane and get the covariance matrix Point map_p i Normal vector Point map_p j Normal vector

[0161] S4.2. Calculation point map_p i and the neighborhood point map_p j Three angle values:

[0162]

[0163] Among them, α is the normal vector With normal vector The angle of is the normal vector With vector map_p i map_p j The angle between the two, θ is the normal vector With vector map_p i map_p j Angle;

[0164] S4.3, α, The three angles θ are quantized into 11-dimensional histograms and concatenated into a 33-dimensional vector S(p);

[0165] S4.4, perform weighted accumulation of vector S(p) according to distance and calculate the eigenvector F(p i ):

[0166]

[0167] Among them, d(map_p i ,map_p j ) is the point map_p i With point map_p j The distance between them, k represents the point map_p i The total number of points within the neighborhood range r, i represents the i-th point of the three-dimensional point cloud map.

[0168] More specifically, the coarse registration module is also used to:

[0169] S4.5. Obtain point p1 in map1 according to S4.1 to S4.4 i The eigenvector F(p1 i ) and the covariance matrix Point q2 in map2 i The eigenvector F(q2 i ) and the covariance matrix

[0170] S4.6, according to the feature vector F(p1 i ) and the feature vector F(q2 i ) difference screening distance closest matching point pair M = {(p1 i , q2 i ) | arg min ‖F(p1 i ) - F(q2 i )‖2};

[0171] S4.7, according to the matching point pair M to construct the matching error function Calculate the matching initial matrix T init = [R init |t init ], wherein R init represents a rotation matrix, t init represents a translation matrix.

[0172] More specifically, the calibration module is further used for:

[0173] S5.1, assuming that each point p1 i in map1 and each point q2 i in map2 conform to Gaussian distribution, that is wherein p1 i , q2 i are the measurement values of map1 and map2 respectively with noise pollution; N represents Gaussian distribution.

[0174] S5.2, calculate the fine registration residual of each point p1 i in map1 and each point q2 i in map2:

[0175] d i = p1 i -T * ·q2 i

[0176] wherein T * represents the external parameter calibration matrix, d i is the fine registration residual, which conforms to Gaussian distribution, that is

[0177] S5.3, given the matching initial matrix T init as the initial value of the external parameter calibration matrix T * , calculate T0, take T0 as the external parameter calibration matrix T * of the next iteration, and constantly iterate to obtain the final value of the external parameter calibration matrix T * :

[0178]

[0179] T * =arg min(T0,T1,…,T n-1 ,T n )

[0180] Among them, M1 is the total number of sampling points in the 3D point cloud map, T n is the extrinsic calibration matrix T obtained in the nth iteration * value.

[0181] The above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit the same. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the various embodiments of the present invention.

Claims

1. A method for calibrating front and rear dual laser radars with non-overlapping field of view, characterized in that: include: S1, front lidar, and rear lidar are installed at the front and rear of the unmanned vehicle, and an IMU sensor is set in the middle of the vehicle body; S2, construct a 3D point cloud map map1 based on the front lidar point cloud data and IMU data, and construct a 3D point cloud map map2 based on the rear lidar point cloud data and IMU data; S3, filter and downsample map1 and map2; S4, perform rough registration on the pre-processed map1 and map2 to obtain the matching initial matrix T init ; S5. According to T init Perform precise registration on the preprocessed map1 and map2, and optimize the external parameter calibration matrix T * ; S6. Calibrate the matrix T based on external parameters * Transform the front lidar and the rear lidar.

2. The method for calibrating front and rear dual laser radars with non-overlapping fields of view according to claim 1, characterized in that: S1 includes: The front lidar scans the 180° area in front, and the rear lidar scans the 180° area in the back; the vehicle is controlled to rotate one circle to collect point cloud data and IMU data from the two lidars.

3. The method for calibrating front and rear dual laser radars with non-overlapping fields of view according to claim 1, characterized in that: The construction method of the three-dimensional point cloud map map1 and the three-dimensional point cloud map map2 is the same, and the specific process is: S2.

1. Predict the motion state of IMU sensor data based on forward propagation. S2.

2. Using the rotation matrix from the lidar coordinate system to the IMU coordinate system Translation matrix from the lidar coordinate system to the IMU coordinate system Convert the predicted state of the IMU into the state of the lidar point, use back propagation to get the state of the lidar point in each frame, and then project the lidar point in each frame to the end time of the frame scan to get the compensated lidar point; S2.

3. Convert the compensated lidar point to the global coordinate system to obtain a local map, and calculate a measurement model between the lidar point and its nearest neighbor in the local map. The measurement model is the relationship between the true state of the lidar point and the noise. S2.

4. Calculate the linearized residual between the lidar point and the nearest neighbor point in the local map based on the measurement model. The lidar linearized residual is a relationship between the error state of the IMU and the noise. The error state of the IMU is calculated based on the predicted state of the IMU. S2.

5. Construct a total error equation based on the IMU error state and the lidar linearization residual, and iteratively solve for the minimum error state based on the IMU predicted state at each moment. S2.

6. Correct the predicted state of the IMU based on the solved error state to obtain the optimal state of the IMU. Convert the optimal state of the IMU to the optimal state of the lidar based on the coordinate conversion relationship between the IMU and the lidar. Convert the optimal state of the lidar to the global coordinate system to construct a three-dimensional point cloud map.

4. The method for calibrating front and rear dual laser radars with non-overlapping fields of view according to claim 3, characterized in that: S4 includes methods for calculating the eigenvectors and covariance matrices corresponding to points in map1 and map2. The methods are the same for both, specifically including: S4.

1. For each point mao_p in the 3D point cloud map i , find point map_p i Point mao_p within the neighborhood range r j Fit the plane and get the covariance matrix Point map_p i Normal vector Point map_p j Normal vector S4.

2. Calculation point map_p i and the neighborhood point map_p j Three angle values: Among them, α is the normal vector With normal vector The angle of is the normal vector With vector map_p i map_p j The angle between the two, θ is the normal vector With vector map_p i map_p j The angle between S4.3, α, The three angles θ are quantized into 11-dimensional histograms and concatenated into a 33-dimensional vector S(p); S4.4, perform weighted accumulation of vector S(p) according to distance and calculate the eigenvector F(p i ): Among them, d(map_p i ,map_p j ) is the point map_p i With point map_p j The distance between them, k represents the point map_p i The total number of points within the neighborhood range r, i represents the i-th point of the three-dimensional point cloud map.

5. The method for calibrating front and rear dual laser radars with non-overlapping fields of view according to claim 4, characterized in that: The S4 also includes: S4.

5. Obtain point p1 in map1 according to S4.1 to S4.4 i The eigenvector F(p1 i ) and the covariance matrix Point q2 in map2 i The eigenvector F(q2 i ) and the covariance matrix S4.6, according to the characteristic vector F(p1 i ) and the eigenvector F(q2 i ) Difference filter closest matching point pair M={(p1 i ,q2 i )|arg min‖F(p1 i )-F(q2 i )‖2}; S4.

7. Constructing a matching error function based on the matching point pair M Calculate the matching initial matrix T init =[R init |t init ], where R init represents the rotation matrix, t init Represents the translation matrix.

6. The method for calibrating front and rear dual laser radars with non-overlapping fields of view according to claim 5, wherein S5 include: S5.1, assume that each point p1 in map1 i With each point q2 in map2 i It conforms to the Gaussian distribution, that is Among them, p1 i ,q2 i are the noise-contaminated measurements corresponding to map1 and map2, respectively; N represents Gaussian distribution; S5.

2. Calculate each point p1 in map1 i With each point q2 in map2 i The fine registration residual of : d i =p1 i -T * ·q2 i Among them, T * represents the external parameter calibration matrix, d i is the precise registration residual, which conforms to the Gaussian distribution, that is, S5.

3. Given the matching initial matrix T init As the external parameter calibration matrix T * The initial value of T0 is calculated and used as the external parameter calibration matrix T for the next iteration * , iteratively calculate the external parameter calibration matrix T * The final value of is expressed as follows: T * =arg min(T0,T1,…,T n-1 ,T n ) Among them, M1 is the total number of sampling points in the 3D point cloud map, T n is the extrinsic calibration matrix T obtained in the nth iteration * The value of .

7. A front and rear dual laser radar calibration system with non-overlapping field of view, characterized in that: include: The data acquisition module is used for the front and rear lidars installed at the front and rear of the unmanned vehicle, and the IMU sensor is set in the middle of the vehicle body; The mapping module is used to construct a 3D point cloud map map1 based on the front lidar point cloud data and the IMU data, and to construct a 3D point cloud map map2 based on the rear lidar point cloud data and the IMU data; Preprocessing module, used for filtering and downsampling preprocessing of map1 and map2; The coarse registration module is used to coarsely register the preprocessed map1 and map2 to obtain the matching initial matrix T init ; Fine registration module, used to init Perform precise registration on the preprocessed map1 and map2, and optimize the external parameter calibration matrix T * ; Calibration module, used to calibrate the matrix T according to external parameters * Transform the front lidar and the rear lidar.

8. The front and rear dual laser radar calibration system with non-overlapping field of view according to claim 7, characterized in that: The data acquisition module is also used to: The front lidar scans the 180° area in front, and the rear lidar scans the 180° area in the back; the vehicle is controlled to rotate one circle to collect point cloud data and IMU data from the two lidars.

9. The front and rear dual laser radar calibration system with non-overlapping field of view according to claim 7, characterized in that: The construction method of the three-dimensional point cloud map map1 and the three-dimensional point cloud map map2 is the same, and the specific process is: S2.

1. Predict the motion state of IMU sensor data based on forward propagation. S2.

2. Using the rotation matrix from the lidar coordinate system to the IMU coordinate system Translation matrix from the lidar coordinate system to the IMU coordinate system Convert the predicted state of the IMU into the state of the lidar point, use back propagation to get the state of the lidar point in each frame, and then project the lidar point in each frame to the end time of the frame scan to get the compensated lidar point; S2.

3. Convert the compensated lidar point to the global coordinate system to obtain a local map, and calculate a measurement model between the lidar point and its nearest neighbor in the local map. The measurement model is the relationship between the true state of the lidar point and the noise. S2.

4. Calculate the linearized residual between the lidar point and the nearest neighbor point in the local map based on the measurement model. The lidar linearized residual is a relationship between the error state of the IMU and the noise. The error state of the IMU is calculated based on the predicted state of the IMU. S2.

5. Construct a total error equation based on the IMU error state and the lidar linearization residual, and iteratively solve for the minimum error state based on the IMU predicted state at each moment. S2.

6. Correct the predicted state of the IMU based on the solved error state to obtain the optimal state of the IMU. Convert the optimal state of the IMU to the optimal state of the lidar based on the coordinate conversion relationship between the IMU and the lidar. Convert the optimal state of the lidar to the global coordinate system to construct a three-dimensional point cloud map.

10. The front and rear dual laser radar calibration system with non-overlapping field of view according to claim 9, characterized in that: The coarse registration module includes methods for calculating the eigenvectors and covariance matrices corresponding to points in map1 and map2. The two methods are the same, specifically including: S4.

1. For each point map_p of the 3D point cloud map i , find point map_p i Point map_p within the neighborhood range r j Fit the plane and get the covariance matrix Point map_p i Normal vector Point map_p j Normal vector S4.

2. Calculation point map_p i and the neighborhood point map_p j Three angle values: Among them, α is the normal vector With normal vector The angle of is the normal vector With vector map_p i map_p j The angle between the two, θ is the normal vector With vector map_p i map_p j The angle between S4.3, α, The three angles θ are quantized into 11-dimensional histograms and concatenated into a 33-dimensional vector S(p); S4.4, perform weighted accumulation of vector S(p) according to distance and calculate the eigenvector F(p i ): Among them, d(map_p i ,map_p j ) is the point map_p i With point map_p j The distance between them, k represents the point map_p i The total number of points within the neighborhood range r, i represents the i-th point of the three-dimensional point cloud map.

Images