Unmanned vehicle dynamics-assisted laser inertial positioning method

Abstract

The invention discloses a dynamics-assisted laser inertial positioning method for an unmanned vehicle, and belongs to the technical field of intelligent multi-source autonomous navigation. The method comprises the following steps: carrying out clock synchronization on a laser radar, an IMU and an unmanned vehicle dynamics model; carrying out state estimation on the laser-inertial subsystem by adopting iterative error state Kalman filtering; scene degradation is detected in real time by calculating characteristic values of a laser radar measurement matrix; when degradation occurs, dynamic model data are introduced, a dynamic-inertia subsystem is constructed, and the dynamic-inertia subsystem and a laser-inertia subsystem are alternately subjected to state fusion; and finally, constructing the incremental point cloud map based on the optimal estimation result. According to the method, the kinetic model of the unmanned vehicle is used as an internal auxiliary information source, the problem of positioning drift of the laser radar in feature degradation scenes such as tunnels and underground parking lots is effectively solved on the premise of not increasing extra hardware cost, and the robustness, environmental adaptability and overall reliability of a positioning system are remarkably improved.

Description

Technical Field

[0001] This invention belongs to the field of intelligent multi-source autonomous navigation technology, specifically relating to a laser inertial positioning method assisted by the dynamics of unmanned vehicles. Background Technology

[0002] Unmanned vehicles are widely used in emergency rescue operations, industrial automation, and venue exhibitions, and environmental perception and navigation based on SLAM technology is a fundamental skill for ground-based unmanned vehicles. LiDAR SLAM relies on radar to emit lasers to obtain the spatial position and shape information of three-dimensional objects, and has high measurement accuracy and stability. In particular, SLAM algorithms based on 3D LiDAR have achieved rapid development in recent years.

[0003] While pure LiDAR-based SLAM methods can achieve accurate real-time state estimation and mapping, their performance in handling rapid motion is insufficient. Therefore, much research has focused on multi-source sensor data fusion algorithms, particularly laser inertial SLAM algorithms incorporating IMU data, which have seen rapid development. However, in special scenarios such as long tunnels and underground parking lots, LiDAR can degrade due to sparse environmental textures or repetitive structures, leading to drift in localization and mapping. Common solutions include introducing other sensors such as cameras and GNSS, but these sensors still rely on external environments or equipment, thus having certain limitations. Summary of the Invention

[0004] To address the aforementioned technical problems, this invention proposes a laser inertial positioning method assisted by the dynamics of unmanned vehicles, which solves the problems of inaccurate positioning and mapping drift of lidar in degraded scenarios.

[0005] To achieve the above objectives, the present invention adopts the following technical solution: A laser inertial positioning method assisted by the dynamics of an unmanned vehicle includes the following steps: S1, Clock Synchronization: Receives data from LiDAR, IMU, and the autonomous vehicle's dynamics model, aligns the timestamps, and unifies them to the system's master clock; the timestamps of the LiDAR and dynamics model data are based on the timestamps of the adjacent IMU data on the left. S2. State estimation of the laser-inertial subsystem: Construct a subsystem with IMU and lidar as the core, and use iterative error state Kalman filtering for state estimation. S21. Define the state vector of the IMU. Input vector and noise vector ; ; in, , and These represent the attitude, position, and velocity of the IMU in the global coordinate system, respectively. and These are the biases of the accelerometer and gyroscope, respectively. g T This is the gravity vector in the global coordinate system; and These are measurements from the accelerometer and gyroscope; the measurement noise and bias instability are both white noise conforming to a Gaussian distribution, respectively. , and , ; S22, Forward Propagation: Assume the IMU starts from... Time's up The time interval is The forward propagation model is as follows: ; Using error states for propagation, the covariance propagation equation is: ; in, Let covariance matrix be the variance matrix. For noise The covariance matrix and Jacobian matrix and as follows: ; S23, Backward Propagation: Based on the pose estimated by the IMU forward propagation, each laser scanning point is converted to... At each moment, calculate the relative pose transformation of the laser scanning points: ; Define the relative pose transformation of the point after backpropagation as: Each laser scanning point Projected to the end of the scan : ; S24. LiDAR Iterative Update: The current LiDAR scan is matched with the historical map using a direct method to calculate pose transformation; the LiDAR points are projected onto the world coordinate system and registered on the map, and then searched using an ikd-tree. Corresponding points The five nearest neighbors are fitted to form a plane, and the normal vector of the plane is... Use a little The distance to this plane is used as the measurement residual of the lidar; ideally, the residual value is 0. ; in, , yes about Jacobian matrix with a value of 0 This is the measurement noise of the lidar; the posterior distribution of the lidar observations is obtained: ; in, yes about The partial derivative when the value is 0, It is the covariance of the error state: ; Combination The prior distribution and the posterior distribution measured by lidar are used to obtain the state. Maximum a posteriori estimate: ; in, Solve using an iterative Kalman filter, let , , , Substitute into the following formula to calculate: ; until If the value is less than a certain threshold, the iteration is considered to have converged, and the optimal state estimate can be obtained. and covariance Based on this optimal state, each lidar point is transformed to the global coordinate system and inserted into the global map using an ikd-tree. S3, Degradation Detection: This will detect degradation in the first iteration. The smallest eigenvalue As a criterion for judging the degradation of each frame of LiDAR scan, if , If a preset threshold is set, the current scenario is determined to be a LiDAR degradation scenario, and a dynamic model is triggered for assistance. S4. State estimation of the dual-system: In the degradation scenario, dynamic model data is introduced. The IMU and the dynamic model constitute a dynamic-inertial subsystem, which alternately performs state estimation with the laser-inertial subsystem. S41. Establish a dynamic model for a two-wheeled differential autonomous vehicle: A two-wheeled differential-driven unmanned vehicle, with its center of mass as the origin of the reference coordinate system, has the following kinematic model equations: ; in, This represents the pose of the autonomous vehicle in the world coordinate system. This represents the angular velocity matrix of the left and right drive wheels. The Jacobian matrix of the kinematic model is in the following form: ; Add constraint equations: ;in ; The Lagrange method is used to model the dynamics of the autonomous vehicle, and the equations are as follows: ; in: ; in, This is the total mass of the car excluding the wheels. It is the mass of a single wheel. It does not include the total moment of inertia of the wheels. It is the moment of inertia of a single wheel relative to the axis of its center of mass. It is the moment of inertia of a single wheel relative to the axis; the dynamic model matrix is ​​represented as: ; in, It is a positive definite inertia matrix. It is the centripetal Coriolis force matrix. It is the input transformation matrix. It is the left and right wheel drive force matrix. It is interference. These are unknown Lagrange multipliers; , and The specific format is as follows: ; Eliminating unknowns The final dynamic equations are obtained as follows: ; in, , , , The input to the dynamic model is torque. The output is the angular acceleration of the drive wheel. ; S42. The state of the dynamic-inertial subsystem is updated based on the error state Kalman filter; the angular acceleration of the unmanned vehicle wheels is obtained through the dynamic model, and the velocity in the vehicle coordinate system is obtained by integration. The values ​​are then converted to observations in the world coordinate system, and the velocity residuals are calculated as the observation error. ; An error-state Kalman filter framework is used to update the state of the dynamic-inertial subsystem; S5. Incremental Map Construction: After the laser-inertial subsystem completes state estimation, the laser radar scanning points are transformed into the world coordinate system, and an incremental point cloud map is constructed and updated using the ikd-tree data structure.

[0006] Compared with the prior art, the beneficial effects of the present invention are: This invention introduces the autonomous vehicle's own dynamic model as an intrinsic, low-cost data source into a multi-source fusion positioning framework. In special scenarios such as long tunnels and underground parking lots where LiDAR is prone to degradation due to feature sparsity or repetition, the system can automatically and accurately detect degradation by calculating the eigenvalues ​​of the LiDAR measurement matrix in real time. Upon confirmation of degradation, the system does not rely entirely on the unreliable LiDAR data but intelligently switches to a subsystem fusing the dynamic model and IMU data for state estimation. This effectively suppresses the positioning drift and mapping failure problems common in pure LiDAR or LiDAR-inertial systems in degraded scenarios. This saves costs and improves adaptability to closed, structured environments. Furthermore, by only introducing the dynamic model for calculation when necessary, continuous computational resource consumption is avoided, ensuring efficient system operation even in non-degraded scenarios. Without excessively increasing system complexity and cost, this significantly enhances the positioning robustness, accuracy, and overall navigation reliability of autonomous vehicles in diverse, especially challenging, environments. Attached Figure Description

[0007] To more clearly illustrate the technical solutions of the embodiments of the present invention, the accompanying drawings used in the description of the embodiments are briefly introduced below.

[0008] Figure 1 This is a framework diagram of a laser inertial positioning method assisted by the dynamics of unmanned vehicles.

[0009] Figure 2 This is a schematic diagram of the forward and backward propagation of the IMU.

[0010] Figure 3 This is a schematic diagram of the residual measurement using lidar.

[0011] Figure 4 This is a schematic diagram of a two-wheeled differential autonomous vehicle.

[0012] Figure 5 These are the localization and mapping effects of several mainstream laser-inertial algorithms in degraded environments.

[0013] Figure 6 This invention provides the effect of localizing and mapping degraded environments. Detailed Implementation

[0014] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be described in detail below with reference to the accompanying drawings and specific embodiments.

[0015] Example 1 See Figure 1 As shown in this embodiment, a laser inertial positioning method assisted by the dynamics of an unmanned vehicle includes the following steps: S1. Clock Synchronization: Simultaneously receive relevant data from LiDAR (10HZ), IMU (200HZ), and unmanned vehicle dynamics model (50HZ), and unify the three data onto the system's main clock. The timestamps of LiDAR and unmanned vehicle dynamics model data may fall between the two IMU data. Use the timestamp of the left IMU data as the timestamp of LiDAR data or dynamics model data.

[0016] S2. State estimation of the laser-inertial subsystem: The IMU and lidar constitute the laser-inertial subsystem. The iterative error state Kalman filter method is used to estimate the state of the system. The specific process includes: S2.1. To simplify notation and facilitate derivation, we use... and To represent manifolds The state error on the composite manifold used in this invention ,have: ; in, , , and This represents a bidirectional mapping from a rotation matrix to a rotation vector according to the Rodriguez formula.

[0017] The meanings of some of the symbols used are shown in Table 1: Table 1 ; An IMU typically consists of a gyroscope and an accelerometer, and can calculate the motion state of a vehicle by measuring its angular and linear acceleration; its state vector... ,enter and noise Defined as: ; in , and These represent the attitude, position, and velocity of the IMU in the global coordinate system, respectively. and These are the biases of the accelerometer and gyroscope, respectively. This is the gravity vector in the global coordinate system; and These are measurements from the accelerometer and gyroscope; the measurement noise and bias instability are both white noise conforming to a Gaussian distribution, respectively. , and , .

[0018] S2.2 Forward propagation, the specific process includes: Suppose the IMU starts from Time's up The time interval is The forward propagation model is as follows, where noise is set to zero: ; The forward propagation model can be used to predict state variables and their covariance. Since the laser inertial subsystem is based on the IESKF data fusion framework, error states are used for propagation. ; in, Let covariance matrix be the variance matrix. For noise The covariance matrix and Jacobian matrix and The following formula can be used for calculation: ; S2.3, Backpropagation, the specific process includes: like Figure 2 As shown, in arrive Point cloud accumulation within a time period completes one frame of LiDAR scanning; At any given time, the scan will participate as an observation in the filtering and fusion process to solve for the optimal state estimate. Ideally, the lidar scan is considered to be completed instantaneously; however, each scan point occurs at its respective sampling time point. The reference coordinate systems in which the measurements are taken are all different. Therefore, we use backpropagation to compensate for the sampling time of the scan points. and scan end time The relative motion.

[0019] The IMU frequency is greater than the lidar frequency. Before the time arrives, the IMU continues to propagate forward; when it arrives... At time 1, backpropagation is performed. Based on the pose estimated by the IMU forward propagation, each laser scanning point is converted to... At any given time, the scan point between two IMU frames is referenced to the left IMU frame. Using the IMU coordinate system as a reference, the relative pose transformation of the laser scan point is calculated using the following formula: ; Define the relative pose transformation of the point after backpropagation as: Each laser scanning point Projected to the end of the scan : ; S2.4, LiDAR iterative update, the specific process includes: Through backpropagation, all laser points in a single frame of scanning are compensated to the final moment. It can be assumed that the points in the scan are in Simultaneous sampling is performed at all times. We use a direct method, matching the current LiDAR scan with the historical map, to calculate the pose transformation between two scan frames. For example... Figure 3 First, through the formula Project the laser points onto the world coordinate system and register them on the map, then search using ikd-tree. Corresponding points The five nearest neighbors are fitted to form a plane, and the normal vector of the plane is... Use a little The distance to this plane is used as the measurement residual of the lidar; ideally, the residual value should be 0. ; in, , yes about Jacobian matrix with a value of 0 This is the measurement noise of the lidar. From this, the posterior distribution of lidar observations can be determined: ; From the forward propagation model, we can see that The prior distribution of, where yes about The partial derivative when the value is 0, It is the covariance of the error state: ; Combination From the prior distribution and the posterior distribution measured by the lidar, we obtain the state. Maximum a posteriori (MAP) estimate: ; in, The above optimization problem is solved using an iterative Kalman filter, let , , , Substitute into the following formula to calculate: ; The Kalman gain calculation formula was dimensionality-reduced to obtain an equivalent formula. The above calculation steps were repeated until… If the value is less than a certain threshold, the iteration is considered to have converged, and the optimal state estimate can be obtained. and covariance Based on this optimal state, each LiDAR point is transformed to the global coordinate system and inserted into the global map using an ikd-tree; S3. Degradation Detection: This involves performing relevant calculations on the LiDAR measurement matrix to determine whether the current scene is degraded. The specific process includes: For the aforementioned state estimation problem, the posterior distribution corresponds to the observation residuals of the lidar, which will be used in the first iteration. The smallest eigenvalue As a criterion for judging the degradation of each frame of LiDAR scan, if ( (Based on the experimental setup) it is assumed that the lidar has degraded. After the lidar degrades, dynamic model data will be introduced for state estimation, while simultaneously increasing the measurement noise of the lidar.

[0020] S4. Dual-Subsystem State Estimation: If degradation is detected, dynamic model data is introduced. The IMU and lidar constitute a laser-inertial subsystem, and the IMU and dynamic model data constitute a dynamic-inertial subsystem. The two subsystems work alternately to deal with lidar degradation scenarios and jointly estimate the system state. The specific process includes: S4.1 Dynamics modeling of a two-wheel differential autonomous vehicle, the specific process includes: like Figure 4 The two-wheeled differential-speed unmanned vehicle shown, with its center of mass as the origin of the reference coordinate system, has the following kinematic model equations: ; in, This represents the pose of the autonomous vehicle in the world coordinate system. This represents the angular velocity matrix of the left and right drive wheels. The Jacobian matrix of the kinematic model is in the following form: ; Since a two-wheeled differential autonomous vehicle is a non-holonomic system, specifically meaning it cannot move directly laterally, it can be represented by the following constraint equations: ; in ; The Lagrange method is used to model the dynamics of the autonomous vehicle, and the equations are as follows: ; in: ; The foregoing This is the total mass of the car excluding the wheels. It is the mass of a single wheel. It does not include the total moment of inertia of the wheels. It is the moment of inertia of a single wheel relative to the axis of its center of mass. This is the moment of inertia of a single wheel relative to the axis, represented by the dynamic model matrix as follows: ; In the dynamic model represented by the above equation, It is a positive definite inertia matrix. It is the centripetal Coriolis force matrix. It is the input transformation matrix. It is the left and right wheel drive force matrix. It is interference. These are unknown Lagrange multipliers, which need to be eliminated through further derivation; , and The specific format is as follows: ; Calculation yields: ; Differentiating both sides of the aforementioned kinematic equations with respect to time, we get: ; Eliminating unknowns The final dynamic equations are obtained by rearranging the equations: ; in, , , , The input to the dynamic model is torque. The output is the angular acceleration of the drive wheel. .

[0021] S4.2 and ESKF updates, the specific process includes: The dynamic inertial subsystem can be divided into two parts: forward propagation and update, and error state is processed. Covariance The propagation of [something]. The angular acceleration of the left and right wheels of the autonomous vehicle can be obtained through a dynamic model, and the velocity under the load system can be obtained after one integration. After two transformations, the observed values ​​in the world coordinate system are obtained, and the velocity residuals are calculated. Indicates the translation between the vehicle coordinate system and the IMU coordinate system: ; Assume the observation equation is The noise is The noise matrix is ,So: ; The observation equation is nonlinear and needs to be linearized. In the ESKF method, the error state is propagated and observed; therefore, the Jacobian matrix of the observation equation relative to the error state is calculated, and the chain rule is used to simplify the calculation. ; Then complete the ESKF update steps: ; S5. Incremental map construction: After each state estimation of the laser-inertial subsystem, the scan points are transformed into the world coordinate system based on the calculated odometry and an incremental point cloud map is constructed using ikd-tree. like Figure 5 The image shows the localization and mapping effects of several mainstream laser-inertial algorithms in a degraded LiDAR environment. It can be seen that these algorithms all fail. Figure 6 The image shows the positioning and mapping effect of the present invention in a LiDAR degradation environment. It can be seen that the method of the present invention can effectively cope with the LiDAR degradation situation and achieve high-precision positioning and mapping.

[0022] The preferred embodiments of the present invention disclosed above are merely illustrative of the invention. These preferred embodiments do not describe all details exhaustively, nor do they limit the invention to the specific implementations described. Clearly, many modifications and variations can be made based on the content of this specification.

Claims

1. A laser inertial positioning method assisted by the dynamics of an unmanned vehicle, characterized in that, Includes the following steps: A laser inertial positioning method assisted by the dynamics of an unmanned vehicle includes the following steps: S1, Clock Synchronization: Receives data from LiDAR, IMU, and the autonomous vehicle's dynamics model, aligns the timestamps, and unifies them to the system's master clock; the timestamps of the LiDAR and dynamics model data are based on the timestamps of the adjacent IMU data on the left. S2. State estimation of the laser-inertial subsystem: Construct a subsystem with IMU and lidar as the core, and use iterative error state Kalman filtering for state estimation. S21. Define the state vector of the IMU. Input vector and noise vector ; ； in, , and These represent the attitude, position, and velocity of the IMU in the global coordinate system, respectively. and These are the biases of the accelerometer and gyroscope, respectively. g T This is the gravity vector in the global coordinate system; and These are measurements from the accelerometer and gyroscope; the measurement noise and bias instability are both white noise conforming to a Gaussian distribution, respectively. , and , ; S22, Forward Propagation: Assume the IMU starts from... Time's up The time interval is The forward propagation model is as follows: ； Using error states for propagation, the covariance propagation equation is: ； in, Let covariance matrix be the variance matrix. For noise The covariance matrix and Jacobian matrix and as follows: ； S23, Backward Propagation: Based on the pose estimated by the IMU forward propagation, each laser scanning point is converted to... At each moment, calculate the relative pose transformation of the laser scanning points: ； Define the relative pose transformation of the point after backpropagation as: Each laser scanning point Projected to the end of the scan : ； S24. LiDAR Iterative Update: The current LiDAR scan is matched with the historical map using a direct method to calculate pose transformation; the LiDAR points are projected onto the world coordinate system and registered on the map, and then searched using an ikd-tree. Corresponding points The five nearest neighbors are fitted to form a plane, and the normal vector of the plane is... Use a little The distance to this plane is used as the measurement residual of the lidar; ideally, the residual value is 0. ； in, , yes about Jacobian matrix with a value of 0 This is the measurement noise of the lidar; the posterior distribution of the lidar observations is obtained: ； in, yes about The partial derivative when the value is 0, It is the covariance of the error state: ； Combination The prior distribution and the posterior distribution measured by lidar are used to obtain the state. Maximum a posteriori estimate: ； in, Solve using an iterative Kalman filter, let , , , Substitute into the following formula to calculate: ； until If the value is less than a certain threshold, the iteration is considered to have converged, and the optimal state estimate can be obtained. and covariance Based on this optimal state, each lidar point is transformed to the global coordinate system and inserted into the global map using an ikd-tree. S3, Degradation Detection: This will detect degradation in the first iteration. The smallest eigenvalue As a criterion for judging the degradation of each frame of LiDAR scan, if , If a preset threshold is set, the current scenario is determined to be a LiDAR degradation scenario, and a dynamic model is triggered for assistance. S4. State estimation of the dual-system: In the degradation scenario, dynamic model data is introduced. The IMU and the dynamic model constitute a dynamic-inertial subsystem, which alternately performs state estimation with the laser-inertial subsystem. S41. Establish a dynamic model for a two-wheeled differential autonomous vehicle: A two-wheeled differential-driven unmanned vehicle, with its center of mass as the origin of the reference coordinate system, has the following kinematic model equations: ； in, This represents the pose of the autonomous vehicle in the world coordinate system. This represents the angular velocity matrix of the left and right drive wheels. The Jacobian matrix of the kinematic model is in the following form: ； Add constraint equations: ;in ; The Lagrange method is used to model the dynamics of the autonomous vehicle, and the equations are as follows: ； in: ； in, This is the total mass of the car excluding the wheels. It is the mass of a single wheel. It does not include the total moment of inertia of the wheels. It is the moment of inertia of a single wheel relative to the axis of its center of mass. It is the moment of inertia of a single wheel relative to the axis; the dynamic model matrix is ​​represented as: ； in, It is a positive definite inertia matrix. It is the centripetal Coriolis force matrix. It is the input transformation matrix. It is the left and right wheel drive force matrix. It is interference. These are unknown Lagrange multipliers; , and The specific format is as follows: ； Eliminating unknowns The final dynamic equations are obtained as follows: ； in, , , , The input to the dynamic model is torque. The output is the angular acceleration of the drive wheel. ; S42. The state of the dynamic-inertial subsystem is updated based on the error state Kalman filter; the angular acceleration of the unmanned vehicle wheels is obtained through the dynamic model, and the velocity in the vehicle coordinate system is obtained by integration. The values ​​are then converted to observations in the world coordinate system, and the velocity residuals are calculated as the observation error. ； An error-state Kalman filter framework is used to update the state of the dynamic-inertial subsystem; S5. Incremental Map Construction: After the laser-inertial subsystem completes state estimation, the laser radar scanning points are transformed into the world coordinate system, and an incremental point cloud map is constructed and updated using the ikd-tree data structure.

Images