LiDAR-Inertial-Vehicle Fusion Positioning Method Based on B-Spline Function

Abstract

The present invention discloses a laser radar-inertial-vehicle fusion positioning method based on B-spline functions. The method comprises: using B-spline functions to model the vehicle's motion trajectory to obtain an IMU motion trajectory; processing the vehicle's wheel speedometer and steering angle sensor readings through a dynamic model to obtain vehicle dynamics measurements; performing discrete integration of the IMU data and vehicle dynamics measurements between adjacent control points of the B-spline function to initialize newly added spline control points; adding the vehicle's lateral acceleration constraint to the vehicle trajectory estimation; and optimizing the estimated vehicle trajectory using point cloud data, IMU data, and vehicle dynamics measurement data, and online estimating correction factors for vehicle dynamics model parameters. The present invention performs multi-sensor fusion positioning based on B-spline functions, improves the robustness and accuracy of unmanned vehicle positioning, and reduces the impact of changes in factors such as vehicle load and road surface on vehicle dynamics measurements.

Description

Technical Field

[0001] The present invention relates to the field of robot navigation and positioning, and in particular to a laser radar-inertial-vehicle fusion positioning method based on B-spline function. Background Art

[0002] Simultaneous Localization and Mapping (SLAM) is a key technology in mobile robots. This technology can estimate the current position of the vehicle using information from local measurement sensors without relying on external signals, while also building a map model of the surrounding environment. Currently, vision and lidar are the two main external sensors used in SLAM technology. Compared to vision sensors, lidar is widely used in practical applications because it can obtain high-precision distance information and is not affected by ambient light and texture. However, lidar relies heavily on the structural characteristics of the environment, resulting in degradation in environments such as tunnels and open flat areas.

[0003] To overcome the aforementioned shortcomings of lidar, some research has integrated other sensor types, such as IMUs and wheel speedometers, with lidar for positioning. Existing methods, when utilizing information from IMUs, wheel speedometers, and steering angle sensors, integrate relative changes between frames, rather than using the original acquired data. This integration process can easily amplify noise in the raw data, thus affecting fused positioning accuracy. Furthermore, during vehicle motion, lateral forces between the tires and the ground can cause the instantaneous velocity direction to deviate from the vehicle's heading. Kinematic models and wheel speedometers cannot accurately describe this common phenomenon during vehicle motion. Furthermore, vehicle model parameters can deviate from pre-calibrated values during driving due to factors such as load and ground conditions, introducing measurement errors.

[0004] Therefore, in order to reduce the impact of the above-mentioned phenomenon on fusion positioning, the present invention proposes a lidar-inertial-vehicle fusion positioning method based on B-spline function, and for the first time introduces the vehicle's lateral acceleration constraint into the positioning problem to describe the vehicle's motion characteristics and provide prior information for positioning, thereby improving the accuracy and robustness of the vehicle's autonomous positioning in complex environments. Summary of the Invention

[0005] The purpose of the present invention is to address the shortcomings of the existing technology and provide a lidar-inertial-vehicle fusion positioning method based on B-spline function. The method uses the vehicle dynamics model to process the wheel speed meter and angle sensor data, effectively taking into account the vehicle's side deviation phenomenon. At the same time, it can directly fuse the original data of the IMU and vehicle dynamics measurement to avoid noise amplification caused by integration, and online correct the drift of vehicle model parameters during driving. The vehicle's lateral acceleration constraint is introduced to ensure that the vehicle can achieve robust and high-precision autonomous positioning in complex and diverse operating environments.

[0006] The object of the present invention is achieved through the following technical solution: a laser radar-inertial-vehicle fusion positioning method based on B-spline function, comprising the following steps:

[0007] S1: Collects real-time scanning point clouds from the LiDAR, IMU data, wheel speedometer readings, and steering angle sensor readings; performs vehicle trajectory estimation: uses two B-spline functions to model the position p and attitude R of the onboard IMU, respectively, to obtain the position, attitude, and velocity data of the onboard IMU at any time, and then obtains the IMU motion trajectory;

[0008] S2: Use the vehicle dynamics model to process the wheel speed meter readings and the steering angle sensor readings in S1 to obtain vehicle dynamics measurements, optimize the correction factor vector online, and calibrate the vehicle dynamics model parameters;

[0009] S3: The time period between the last control point before the trajectory update and the newly added control point, by performing discrete integration processing on the IMU data in S1, the pose set corresponding to each IMU sampling moment in this time period is obtained and speed gather;

[0010] By performing discrete integration processing on the vehicle dynamics measurements in S2, the pose set corresponding to each moment of vehicle dynamics measurement in the time period is obtained. Taking all pose sets and velocity sets at discrete moments as measurement values, constraining the poses and velocities sampled by the B-spline function at the discrete moments, and constructing the first optimization problem to initialize the control points newly added to the B-spline;

[0011] S4: Add the vehicle lateral acceleration constraint to the vehicle trajectory estimation and calculate the lateral acceleration constraint residual;

[0012] S5: Use each frame of the lidar point cloud obtained in S1 to match the line features and surface features extracted from the environment map to calculate the lidar point cloud residual; calculate the IMU residual based on the IMU data in S1; calculate the vehicle dynamics measurement residual based on the vehicle dynamics measurement in S2;

[0013] S6: Construct the second optimization problem through the point cloud residual, IMU residual, vehicle dynamics measurement residual and lateral acceleration constraint residual. The control points of the newly added B-spline after initialization in S3 are used as the initial values of the optimization problem. At the same time, the spline function control points in S1 and the correction factor vector in S2 are optimized to obtain the real-time vehicle posture trajectory and vehicle model parameter correction values used for positioning.

[0014] Furthermore, two B-spline functions are used in S1 to model the position p and posture R of the vehicle-mounted IMU respectively, specifically:

[0015] R(t)=λ(t)d θ

[0016] p(t)=λ(t)d t

[0017] Among them, R(t) is the B-spline function representing the posture, p(t) is the B-spline function representing the position; λ(t) is the spline basis function, d θ is the spline parameter associated with the control point of the posture B-spline function, d t is the spline parameter related to the control point of the position B-spline function; the angular velocity and angular acceleration can be obtained by taking the first and second order derivatives of the B-spline function representing the above posture:

[0018]

[0019]

[0020] The linear velocity and linear acceleration can be obtained by taking the first and second order derivatives of the B-spline function representing the position:

[0021]

[0022]

[0023] The above describes the sampling values of the B-spline function of attitude, angular velocity, angular acceleration, position, linear velocity, and linear acceleration at time t.

[0024] Furthermore, the S2 is specifically as follows: using the vehicle's dynamic model, the vehicle's slip angle α and yaw angle change rate are obtained from the wheel speed meter reading and the steering angle sensor reading.

[0025] The linear velocity and angular velocity with the center of mass of the vehicle as the origin of the coordinate system are obtained by the slip angle and yaw angle change rate:

[0026]

[0027]

[0028] Add the vehicle dynamics model parameter correction factor vector [c v c α c ω ], then the vehicle dynamics measurement is:

[0029]

[0030]

[0031]

[0032] Among them, C r and C f are the cornering stiffness of the front and rear wheels of the vehicle, l r and l f are the distances from the rear wheel axle center and the front wheel axle center to the vehicle mass center, l = l r +l f ; m is the vehicle mass; is the vehicle wheel speedometer reading, δ is the vehicle steering angle sensor reading; The linear velocity portion of the vehicle dynamics measurement, The angular velocity part of the vehicle dynamics measurement; c v is the correction factor for the linear velocity, c α is the correction factor for the vehicle slip angle, c ω is a correction factor for the vehicle's angular velocity.

[0033] Furthermore, the S3 is specifically as follows: by performing discrete integration processing on the IMU data, a pose set corresponding to each IMU sampling moment in the time period is obtained. and speed set Perform discrete integration processing on the vehicle dynamics measurement to obtain the pose set corresponding to each vehicle dynamics measurement in the time period The recursively obtained discrete moment pose and velocity are used as measurement values, and the objective function of the first optimization problem is constructed as follows:

[0034]

[0035] Among them, φ new For the newly added control point, is the pose obtained by discrete integration of IMU, is the velocity obtained by discrete integration of IMU; Discretely integrate the vehicle dynamics measurements to obtain the pose; is the collection time of IMU data, The acquisition moment for vehicle dynamics measurements; It is the external parameter between the vehicle IMU coordinate system and the vehicle center of mass coordinate system. The superscript T represents transposition.

[0036] Furthermore, in S4, the vehicle lateral acceleration constraint is added to the vehicle trajectory estimation, and the vehicle lateral acceleration constraint residual is calculated as follows:

[0037]

[0038] Where U = [0 1 0] is the selection matrix, a V (t s ) is the linear acceleration converted from the IMU coordinate system to the vehicle center of mass coordinate system after the second-order derivative of the IMU motion trajectory, is the IMU gyroscope three-axis angular velocity reading, v V (t s ) is the linear velocity of the IMU motion trajectory converted from the IMU coordinate system to the vehicle center of mass coordinate system after the first-order derivative, [·] ∧ is the operation of converting a vector into its corresponding antisymmetric matrix, [·] z is the operation to obtain the axis angle transformation rate corresponding to the z axis; t s The set trajectory sampling time is consistent with the IMU acquisition time.

[0039] Furthermore, the vehicle dynamics measurement residual in S5 is specifically constructed by constructing the lidar point cloud residual, directly constructing the IMU residual using the original IMU three-axis acceleration and three-axis angular velocity measurements and the first-order and second-order derivatives of the IMU motion trajectory, and directly constructing the vehicle dynamics residual using the vehicle dynamics measurement and the first-order derivative of the estimated trajectory. The formula is as follows:

[0040]

[0041]

[0042]

[0043] Among them, e L 、e I 、e V They are lidar point cloud residual, IMU residual and vehicle dynamics residual; In order to convert the IMU trajectory in the world coordinate system to the LiDAR trajectory in the world coordinate system through external reference, is the sampling time of the kth sampling point of the laser radar in this frame, and π is the projection equation of the plane feature point to the plane and the edge feature point to the line; is the IMU trajectory in the world coordinate system, t I is the acquisition time of IMU data, ω B (tI ) is the angular velocity in the IMU coordinate system obtained by taking the first-order derivative of the IMU trajectory. is the three-axis angular velocity measurement of IMU, b a and b g are the bias of IMU accelerometer and gyroscope respectively; v v (t V ),ω V (t V ) are the linear velocity and angular velocity of the vehicle in the center of mass coordinate system obtained by the external parameters after the first-order derivative of the trajectory, t V is the acquisition time of vehicle dynamics measurement, c v is the correction factor for the linear velocity of vehicle dynamics, c α is the correction factor for the measured slip angle with respect to vehicle dynamics, c ω is a correction factor for the magnitude of the measured angular velocity with respect to vehicle dynamics.

[0044] Furthermore, the objective function of the second optimization problem in S6 is:

[0045]

[0046] Among them, the optimization variable is the correction factor vector c=[c v c α c ω ] and the set of activated control points of the trajectory Φ={φ i ,…,φ i+k-1}, k is the order of the B-spline function; e L 、e I 、e V 、e M They are the lidar point cloud residual, IMU residual, vehicle dynamics residual and vehicle lateral acceleration constraint residual; Σ L ,Σ I ,Σ V are the weight matrices of lidar point cloud residuals, IMU residuals, and vehicle dynamics residuals, respectively. is the Mahalanobis norm corresponding to the Σ weight matrix.

[0047] Compared with the prior art, the present invention has the following beneficial effects: the present invention uses a vehicle dynamics model to process the wheel speedometer and steering angle sensor data, taking into account the vehicle's side deviation phenomenon to obtain more accurate dynamic measurement values; uses a B-spline function to model the trajectory, and takes advantage of its properties of being sampled in continuous time and being smoothly differentiable to directly fuse the original data of the IMU and vehicle dynamics measurement, avoiding noise amplification caused by integration and affecting positioning accuracy; online correction of the drift of vehicle model parameters during driving reduces the impact of changes in factors such as vehicle load and driving road surface on vehicle dynamic measurement values; introduces a vehicle's lateral acceleration constraint to provide prior information for vehicle trajectory estimation, effectively improving the robustness and accuracy of vehicle autonomous positioning. BRIEF DESCRIPTION OF THE DRAWINGS

[0048] Figure 1 This is an overall flow chart of the lidar-inertial-vehicle fusion positioning method based on B-spline function of the present invention. DETAILED DESCRIPTION

[0049] An embodiment of the present invention provides a lidar-inertial-vehicle fusion positioning method based on B-spline function, which is used for robust and high-precision positioning of vehicles in complex scenarios.

[0050] To help those skilled in the art better understand the present invention, the following will provide a clear and complete description of the technical solution of the present invention in conjunction with the accompanying drawings of the embodiments of the present invention. Obviously, the embodiments described are only part of the embodiments of the present invention, not all of them. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort should fall within the scope of protection of the present invention.

[0051] The present invention provides a lidar-inertial-vehicle fusion positioning method based on B-spline function. In order to make the purpose, technical solution and advantages of the present invention clearer, the present invention is further described in detail below in conjunction with embodiments. Figure 1 The present invention is a flowchart of a laser radar-inertial-vehicle fusion positioning method based on B-spline function, which includes:

[0052] S1: Suppose there is a vehicle equipped with a lidar, an inertial measurement unit (IMU), a wheel speedometer, and a steering angle sensor. The lidar real-time scanning point cloud, IMU data, wheel speedometer readings, and steering angle sensor readings are collected. Vehicle trajectory estimation is performed: two B-spline functions are used to model the position p and posture R of the on-board IMU respectively, and the position, posture, and speed data of the on-board IMU at any time are obtained, thereby obtaining the IMU motion trajectory. The control points of the IMU motion trajectory and the B-spline function are updated online in real time.

[0053] The two B-spline functions are used to model the position p and posture R of the vehicle-mounted IMU respectively. Specifically, the control point sampling time interval of the B-spline function is set (such as 0.1s, and the specific time interval can be adjusted according to the actual situation).

[0054] R(t)=λ(t)d θ

[0055] p(t)=λ(t)d t

[0056] Among them, R(t) is the B-spline function representing the posture, p(t) is the B-spline function representing the position; λ(t) is the spline basis function, d θ is the spline parameter associated with the control point of the posture B-spline function, d t is the spline parameter related to the control point of the position B-spline function. The angular velocity and angular acceleration can be obtained by taking the first and second order derivatives of the B-spline function representing the above posture:

[0057]

[0058]

[0059] The linear velocity and linear acceleration can be obtained by taking the first and second order derivatives of the B-spline function representing the position:

[0060]

[0061]

[0062] The above describes the sampling values of the B-spline function of attitude, angular velocity, angular acceleration, position, linear velocity, and linear acceleration at time t.

[0063] S2: Use the vehicle dynamics model to process the wheel speed meter readings and the steering angle sensor readings in S1 to obtain the vehicle's slip angle α and yaw angle change rate from the wheel speed meter readings and the steering angle sensor readings.

[0064]

[0065]

[0066] Among them, C r and C f are the cornering stiffness of the front and rear wheels of the vehicle, l r and l f are the distances from the rear wheel axle center and the front wheel axle center to the vehicle mass center, l = l r +l f ; m is the vehicle mass; is the vehicle wheel speedometer reading, and δ is the vehicle steering angle sensor reading. The slip angle and yaw angle change rate are used to obtain the vehicle dynamics measurement, that is, the linear velocity and angular velocity of the coordinate system with the vehicle's center of mass as the origin:

[0067]

[0068]

[0069] In the vehicle dynamics measurement, the correction factor of the vehicle dynamics model parameters to be optimized is added [c v c α c ω ], then the vehicle dynamics measurement is:

[0070]

[0071]

[0072]

[0073] in, The linear velocity portion of the vehicle dynamics measurement, The angular velocity part of the vehicle dynamics measurement; c v is the correction factor for the linear velocity, c α is the correction factor for the vehicle slip angle, c ω is a correction factor for the vehicle's angular velocity.

[0074] S3: Initialize the newly added B-spline control points: In the time period between the last control point before the trajectory update and the newly added control point, the IMU data in S1 is discretely integrated to obtain the pose set corresponding to each IMU sampling moment in the time period. and speed gather;

[0075] By performing discrete integration processing on the vehicle dynamics measurements in S2, the pose set corresponding to each moment of vehicle dynamics measurement in the time period is obtained. Taking all the pose sets and velocity sets at the discrete moments as measurement values, constraining the poses and velocities sampled by the B-spline function at the discrete moments, and constructing a first optimization problem for initializing the control points newly added to the B-spline;

[0076] The objective function of the first optimization problem constructed is:

[0077]

[0078] Among them, φ new For the newly added control point, is the pose obtained by discrete integration of IMU, is the velocity obtained by discrete integration of IMU; Discretely integrate the vehicle dynamics measurements to obtain the pose; is the collection time of IMU data, The acquisition moment for vehicle dynamics measurements; It is the external parameter between the vehicle-mounted IMU coordinate system and the vehicle center of mass coordinate system.

[0079] Step (4): Add the vehicle lateral acceleration constraint to the vehicle trajectory estimation and calculate the vehicle lateral acceleration constraint residual as:

[0080]

[0081] Where U = [0 1 0] is the selection matrix, a V (t s ) is the linear acceleration converted from the IMU coordinate system to the vehicle center of mass coordinate system after the second-order derivative of the trajectory, is the IMU gyroscope three-axis angular velocity reading, v V (t s ) is the linear velocity of the trajectory converted from the IMU coordinate system to the vehicle center of mass coordinate system after the first-order derivative, [·] ∧ is the operation of converting a vector into its corresponding antisymmetric matrix, [·] z is the operation to obtain the axis angle transformation rate corresponding to the z axis; t s The trajectory sampling time is set to be consistent with the acquisition time of the IMU in the present invention.

[0082] Step (5): Extract the plane feature points and edge feature points of the lidar point cloud, construct the lidar point cloud residual, directly construct the IMU residual using the original IMU three-axis acceleration and three-axis angular velocity measurements and the first and second-order derivatives of the estimated trajectory, and directly construct the vehicle dynamics residual using the vehicle dynamics measurement and the first-order derivative of the estimated trajectory:

[0083]

[0084]

[0085]

[0086] Among them, e L 、e I 、e V They are lidar point cloud residuals, IMU residuals, and vehicle dynamics measurement residuals; In order to convert the IMU trajectory in the world coordinate system to the LiDAR trajectory in the world coordinate system through external reference, is the sampling time of the kth sampling point of the lidar in this frame, and π is the projection equation of the plane feature point to the plane and the edge feature point to the line. is the IMU trajectory in the world coordinate system, t I is the acquisition time of IMU data, ω B (t I ) is the angular velocity in the IMU coordinate system obtained by taking the first-order derivative of the IMU trajectory. is the three-axis angular velocity measurement of IMU, b a and b g They are the bias of the IMU accelerometer and gyroscope respectively. V (t V ),ω V (t V ) are the linear velocity and angular velocity of the vehicle in the center of mass coordinate system obtained by the external parameters after the first-order derivative of the trajectory, t V is the acquisition time of vehicle dynamics measurement, c v is the correction factor for the linear velocity of vehicle dynamics, c α is the correction factor for the measured slip angle with respect to vehicle dynamics, c ω is a correction factor for the magnitude of the measured angular velocity with respect to vehicle dynamics.

[0087] Step (6): Construct an optimization problem based on the point cloud residual, IMU residual, vehicle dynamics residual, and lateral acceleration constraint residual. Use the Levenberg-Marquardt algorithm to simultaneously optimize the trajectory spline function control points and the correction factor vector. The objective function of the optimization problem is:

[0088]

[0089] Among them, the optimization variable is the correction factor vector c=[c v c α c ω ] and the set of activated control points of the trajectory Φ={φ i ,…,φ i+k-1}, k is the order of the B-spline function; e L 、e I 、e V 、e M They are the lidar point cloud residual, IMU residual, vehicle dynamics residual, and vehicle lateral acceleration constraint residual. L ,Σ I ,Σ V are the weight matrices of lidar point cloud residuals, IMU residuals, and vehicle dynamics residuals, respectively. is the Mahalanobis norm corresponding to the Σ weight matrix.

[0090] In order to further illustrate that the present invention can effectively improve the positioning accuracy and robustness of the vehicle, the positioning trajectory experiments were carried out using the present invention in outdoor environments and tunnel degradation environments. The positioning results of the present invention and the comparison algorithm are shown in Table 1 and Table 2. In the outdoor environment, the root mean square error (RMSE) of the absolute trajectory error (ATE) of positioning is compared. The smaller the value, the higher the positioning accuracy; in the tunnel degradation environment, the loop error of positioning is compared. The smaller the error, the higher the positioning accuracy. As can be seen from Table 1, the effectiveness of each innovative module in the present invention, and compared with other methods, the present invention can obtain high-precision positioning results. As can be seen from Table 2, compared with other methods, the positioning of the present invention in a tunnel degradation environment is more robust.

[0091] Table 1

[0092]

[0093] Table 2

[0094] method The present invention LIO- SAM Loopback error (m) 1.798 7.28

[0095] The above embodiments are merely preferred embodiments of the present invention and are used to illustrate the technical solutions of the present invention, but are not intended to limit the scope of protection of the present invention. Although the present invention has been described in detail with reference to the above embodiments, the technical solutions described in the above embodiments may be modified or some of the technical features thereof may be replaced by equivalents without departing from the spirit and scope of the claims and their equivalents. Therefore, such modifications or replacements are within the scope of protection of this technical solution.

Claims

1. A laser radar-inertial-vehicle fusion positioning method based on B-spline function, characterized in that: The following steps are involved: S1: Collects real-time scanning point clouds from the LiDAR, IMU data, wheel speedometer readings, and steering angle sensor readings; performs vehicle trajectory estimation: uses two B-spline functions to model the position p and attitude R of the onboard IMU, respectively, to obtain the position, attitude, and velocity data of the onboard IMU at any time, and then obtains the IMU motion trajectory; S2: Use the vehicle dynamics model to process the wheel speed meter readings and the steering angle sensor readings in S1 to obtain vehicle dynamics measurements, optimize the correction factor vector online, and calibrate the vehicle dynamics model parameters; S3: The time period between the last control point before the trajectory update and the newly added control point, by performing discrete integration processing on the IMU data in S1, the pose set corresponding to each IMU sampling moment in this time period is obtained and speed gather; By performing discrete integration processing on the vehicle dynamics measurements in S2, the pose set corresponding to each moment of vehicle dynamics measurement in the time period is obtained. Taking all pose sets and velocity sets at discrete moments as measurement values, constraining the poses and velocities sampled by the B-spline function at the discrete moments, and constructing the first optimization problem to initialize the control points newly added to the B-spline; S4: Add the vehicle lateral acceleration constraint to the vehicle trajectory estimation and calculate the lateral acceleration constraint residual; S5: Use each frame of the lidar point cloud obtained in S1 to match the line features and surface features extracted from the environment map to calculate the lidar point cloud residual; calculate the IMU residual based on the IMU data in S1; calculate the vehicle dynamics measurement residual based on the vehicle dynamics measurement in S2; S6: Construct the second optimization problem through the point cloud residual, IMU residual, vehicle dynamics measurement residual and lateral acceleration constraint residual. The control points of the newly added B-spline after initialization in S3 are used as the initial values of the optimization problem. At the same time, the spline function control points in S1 and the correction factor vector in S2 are optimized to obtain the real-time vehicle posture trajectory and vehicle model parameter correction values used for positioning.

2. The B-spline function-based laser radar-inertial-vehicle fusion positioning method according to claim 1, characterized in that: In S1, two B-spline functions are used to model the position p and posture R of the vehicle-mounted IMU respectively, specifically: R(t)=λ(t)d θ p(t)=λ(t)d t Among them, R(t) is the B-spline function representing the posture, p(t) is the B-spline function representing the position; λ(t) is the spline basis function, d θ is the spline parameter associated with the control point of the posture B-spline function, d t is the spline parameter related to the control point of the position B-spline function; the angular velocity and angular acceleration can be obtained by taking the first and second order derivatives of the B-spline function representing the above posture: The linear velocity and linear acceleration can be obtained by taking the first and second order derivatives of the B-spline function representing the position: The above describes the sampling values of the B-spline function of attitude, angular velocity, angular acceleration, position, linear velocity, and linear acceleration at time t.

3. The B-spline function-based laser radar-inertial-vehicle fusion positioning method according to claim 1, characterized in that: The S2 is specifically as follows: using the vehicle's dynamic model, the vehicle's slip angle α and yaw angle change rate are obtained from the wheel speed meter reading and the steering angle sensor reading. The linear velocity and angular velocity with the center of mass of the vehicle as the origin of the coordinate system are obtained by the slip angle and yaw angle change rate: Add the vehicle dynamics model parameter correction factor vector [c v c α c ω ], then the vehicle dynamics measurement is: Among them, C r and C f are the cornering stiffness of the front and rear wheels of the vehicle, l r and l f are the distances from the rear wheel axle center and the front wheel axle center to the vehicle mass center, l = l r +l f ; m is the vehicle mass; is the vehicle wheel speedometer reading, δ is the vehicle steering angle sensor reading; The linear velocity portion of the vehicle dynamics measurement, The angular velocity part of the vehicle dynamics measurement; c v is the correction factor for the linear velocity, c α is the correction factor for the vehicle slip angle, c ω is a correction factor for the vehicle's angular velocity.

4. The B-spline function-based laser radar-inertial-vehicle fusion positioning method according to claim 1, characterized in that: The S3 is specifically: by performing discrete integration processing on the IMU data, the pose set corresponding to each IMU sampling moment in the time period is obtained. and speed set Perform discrete integration processing on the vehicle dynamics measurement to obtain the pose set corresponding to each vehicle dynamics measurement in the time period The recursively obtained discrete moment pose and velocity are used as measurement values, and the objective function of the first optimization problem is constructed as follows: Among them, φ new For the newly added control point, is the pose obtained by discrete integration of IMU, is the velocity obtained by discrete integration of IMU; Discretely integrate the vehicle dynamics measurements to obtain the pose; is the collection time of IMU data, The acquisition moment for vehicle dynamics measurements; It is the external parameter between the vehicle IMU coordinate system and the vehicle center of mass coordinate system. The superscript T represents transposition.

5. The B-spline function-based laser radar-inertial-vehicle fusion positioning method according to claim 1, characterized in that: In S4, the vehicle lateral acceleration constraint is added to the vehicle trajectory estimation, and the vehicle lateral acceleration constraint residual is calculated as follows: Where U = [0 1 0] is the selection matrix, a V (t s ) is the linear acceleration converted from the IMU coordinate system to the vehicle center of mass coordinate system after the second-order derivative of the IMU motion trajectory, is the IMU gyroscope three-axis angular velocity reading, v V (t s ) is the linear velocity of the IMU motion trajectory converted from the IMU coordinate system to the vehicle center of mass coordinate system after the first-order derivative, [·] ∧ is the operation of converting a vector into its corresponding antisymmetric matrix, [·] z is the operation to obtain the axis angle transformation rate corresponding to the z axis; t s The set trajectory sampling time is consistent with the IMU acquisition time.

6. The B-spline function-based laser radar-inertial-vehicle fusion positioning method according to claim 1, characterized in that: The vehicle dynamics measurement residual in S5 is specifically constructed by constructing the lidar point cloud residual, directly constructing the IMU residual using the original IMU three-axis acceleration and three-axis angular velocity measurements and the first and second-order derivatives of the IMU motion trajectory, and directly constructing the vehicle dynamics residual using the vehicle dynamics measurement and the first-order derivative of the estimated trajectory. The formula is as follows: Among them, e L 、e I 、e V They are lidar point cloud residual, IMU residual and vehicle dynamics residual; In order to convert the IMU trajectory in the world coordinate system to the LiDAR trajectory in the world coordinate system through external reference, is the sampling time of the kth sampling point of the laser radar in this frame, and π is the projection equation of the plane feature point to the plane and the edge feature point to the line; is the IMU trajectory in the world coordinate system, t I is the acquisition time of IMU data, ω B (t I ) is the angular velocity in the IMU coordinate system obtained by taking the first-order derivative of the IMU trajectory. is the three-axis angular velocity measurement of IMU, b a and b g are the bias of IMU accelerometer and gyroscope respectively; v V (t V ),ω V (t V ) are the linear velocity and angular velocity of the vehicle in the center of mass coordinate system obtained by the external parameters after the first-order derivative of the trajectory, t V is the acquisition time of vehicle dynamics measurement, c v is the correction factor for the linear velocity of vehicle dynamics, c α is the correction factor for the measured slip angle with respect to vehicle dynamics, c ω is a correction factor for the magnitude of the measured angular velocity with respect to vehicle dynamics.

7. The B-spline function-based laser radar-inertial-vehicle fusion positioning method according to claim 1, characterized in that: The objective function of the second optimization problem in S6 is: Among them, the optimization variable is the correction factor vector c=[c v c α c ω ] and the set of activated control points of the trajectory Φ={φ i ,…,φ i+k-1 }, k is the order of the B-spline function; e L 、e I 、e V 、e M They are the lidar point cloud residual, IMU residual, vehicle dynamics residual and vehicle lateral acceleration constraint residual; Σ L ,Σ I ,Σ V are the weight matrices of lidar point cloud residuals, IMU residuals, and vehicle dynamics residuals, respectively. is the Mahalanobis norm corresponding to the Σ weight matrix.

Images