CN-121994222-A - Laser inertial navigation odometer timestamp conversion error suppression method based on integer time sequence

CN121994222ACN 121994222 ACN121994222 ACN 121994222ACN-121994222-A

Abstract

The invention discloses a method for inhibiting timestamp conversion errors of a laser inertial navigation odometer based on integer time sequence, which inhibits the timestamp conversion errors of the laser inertial navigation odometer by integer time sequence lattice dotting processing on the premise of not adding an additional sensor and not introducing new characteristics, effectively reduces accumulated error drift of a system in a degradation environment, realizes real-time six-degree-of-freedom pose calculation with low cost and low power consumption, and improves positioning precision and robustness of the laser inertial navigation odometer in GNSS rejection and geometric degradation scenes.

Inventors

ZHUANG YAN
WANG CHENGMIN
YAN FEI

Assignees

大连理工大学

Dates

Publication Date: 20260508
Application Date: 20260122

Claims (9)

1. The utility model provides a laser inertial navigation odometer timestamp conversion error suppression method based on integer time sequence, inertial measurement unit detects the inertial measurement data of carrier, laser radar detects the laser point cloud data of carrier, laser inertial navigation odometer receives inertial measurement data and laser point cloud data and confirms the state of carrier, its characterized in that, the method includes: The method comprises the steps of S1, setting a 64-bit integer nanosecond timestamp in a laser inertial navigation odometer, indexing a data sequence corresponding to laser point cloud data and inertial measurement data, and determining a measurement group based on the data sequence corresponding to the laser point cloud data and the inertial measurement data; S2, acquiring a current system state vector of a laser inertial navigation odometer at the current moment, and determining a system state vector at the next moment based on the current system state vector, wherein the system is a carrier installed and arranged for the laser inertial navigation odometer; S3, for the inertial measurement data of each moment in the measurement set, determining the time interval between the inertial measurement data and the inertial measurement data of the last moment, and determining the system state vector of each moment based on the time interval and the system state vector of the last moment of the laser inertial navigation odometer; S4, calculating partial derivatives of the time intervals through system state vectors at each moment to obtain a time error jacobian matrix; S5, acquiring a process noise covariance matrix containing white noise and random walk noise of a gyroscope and an accelerometer, a noise driving matrix under discrete time and a state transition matrix under discrete time, and determining an error state covariance matrix at each moment by combining the time error jacobian matrix; s6, obtaining a transformation matrix from the inertial measurement unit coordinate system to the laser radar coordinate system, A transformation matrix from the moment global coordinate system to the inertial measurement unit coordinate system, The method comprises the steps of transforming a coordinate system of an inertial measurement unit to a global coordinate system, transforming a coordinate system of a laser radar to a coordinate system of the inertial measurement unit and measuring the position of an ith laser point of the laser radar under an instantaneous coordinate system, and determining the position of the ith laser point of the laser radar under a reference coordinate system; s7, obtaining a plane normal vector expressed under a global coordinate system, A gesture rotation matrix of the moment inertial measurement unit coordinate system in the global coordinate system, a gesture rotation matrix of the laser radar coordinate system in the inertial measurement unit coordinate system the position of the ith laser point measured by the laser radar under the reference coordinate system, the translation vector of the laser radar coordinate system under the inertial measurement unit coordinate system, Three-dimensional position vector of moment inertial measurement unit coordinate system in global coordinate system, arbitrary point on plane expressed in global coordinate system and said Transforming matrix from moment global coordinate system to inertial measurement unit coordinate system, and determining point-plane geometrical observation residual error; s8, stacking the extended observation residual errors to obtain stacked observation residual errors, and combining the error state covariance matrix to determine the total residual error of each iteration of a measurement group; and S9, superposing the optimal estimation value of the system state error on the nominal state of the system to obtain the optimal estimation value of the system state after error suppression of the laser inertial navigation odometer.
2. The method for suppressing a conversion error of a laser inertial navigation odometer timestamp based on an integer time sequence according to claim 1, wherein determining a measurement set based on a data queue of the laser point cloud data corresponding to inertial measurement data comprises: setting two data queues for respectively storing inertia measurement data and laser point cloud data, wherein the two data queues use 64-bit integer nanosecond time stamps to construct indexes, when the laser point cloud queues receive a new frame of laser point cloud data, firstly reading integer nanosecond start time recorded by the head of the laser point cloud data, calculating integer nanosecond end time of the frame of laser point cloud data by combining with scanning duration of the laser point cloud data, traversing the inertia measurement data queues, and screening out all time stamps to be in line with each other And packaging the laser point cloud data corresponding to the frame into a measurement group.
3. The method of claim 1, wherein determining the time interval between the inertial measurement data for each time in the measurement set and the inertial measurement data at the previous time, and determining the system state vector for each time based on the time interval and the system state vector at the previous time of the laser inertial measurement meter comprises: Wherein, the Is the first A system state vector of the moment laser inertial navigation odometer; Is the first A system state vector of the moment laser inertial navigation odometer; Representing generalized addition operations on a manifold for processing state updates on a rotating manifold; Is the first Time of day includes angular velocity And acceleration An inertial measurement unit of (a) measures an input vector; Is the first Measuring noise vector at moment; Is a time interval; Is in the state of the system The rate of change of time of day; Wherein, the A gaussian white noise vector that is a gyroscope; A gaussian white noise vector that is an accelerometer; a random walk noise vector that is zero offset to the gyroscope; a random walk noise vector that is zero offset to the accelerometer; a zero vector representing 3 rows and 1 columns; The rate of change for position is speed; is a gesture rotation matrix; is the angular velocity; Is acceleration; a gravity vector; 、 Zero bias for gyroscopes and accelerometers.
4. The method for suppressing a time stamp conversion error of a laser inertial navigation mileage meter based on an integer time sequence according to claim 3, wherein the calculating the time error jacobian matrix by partial derivative of the time interval by the system state vector of each moment comprises: Wherein, the Is a time error jacobian matrix; Is the first A system state vector of the moment laser inertial navigation odometer; Is a time interval; The rate of change for position is speed; is a gesture rotation matrix; is the angular velocity; Is acceleration; a gravity vector; 、 Zero bias for gyroscopes and accelerometers.
5. The method of claim 3, wherein the obtaining a process noise covariance matrix including gyroscope and accelerometer white noise and random walk noise, a noise driving matrix at discrete time, a state transition matrix at discrete time, and determining an error state covariance matrix at each moment in combination with the time error jacobian matrix comprises: Wherein, the Is the carrier at the first An error state covariance matrix of the moment; To at the first An error state covariance matrix of the moment; is a state transition matrix under discrete time; driving a matrix for noise at discrete time; a process noise covariance matrix including white noise and random walk noise of the gyroscope and the accelerometer; the time noise variance is calibrated according to the hardware clock precision of the inertial measurement unit.
6. The method for suppressing a time stamp conversion error of a laser inertial navigation mileage meter based on an integer time sequence according to claim 1, wherein the obtaining a transformation matrix from an inertial measurement unit coordinate system to a laser radar coordinate system, A transformation matrix from the moment global coordinate system to the inertial measurement unit coordinate system, The transformation matrix from the moment inertial measurement unit coordinate system to the global coordinate system, the transformation matrix from the laser radar coordinate system to the inertial measurement unit coordinate system, and the position of the ith laser point measured by the laser radar under the instantaneous coordinate system are determined, and the position of the ith laser point measured by the laser radar under the reference coordinate system is realized by the following expression: Wherein, the In a reference coordinate system for a laser radar The position of the ith laser spot measured down; the transformation matrix is from an inertial measurement unit coordinate system I to a laser radar coordinate system L; Is that Moment global coordinate system G to inertial measurement unit coordinate system Is a transformation matrix of (a); Is that Moment inertial measurement unit coordinate system A transformation matrix to the global coordinate system G; The transformation matrix is a transformation matrix from a laser radar coordinate system L to an inertial measurement unit coordinate system I; in an instantaneous coordinate system for a laser radar The position of the ith laser spot measured below.
7. The method for suppressing a conversion error of a laser inertial navigation mileage meter timestamp based on an integer time sequence according to claim 1, wherein the obtaining a plane normal vector expressed in a global coordinate system, Attitude rotation matrix of moment inertial measurement unit coordinate system in global coordinate system, attitude rotation matrix of laser radar coordinate system in inertial measurement unit coordinate system, and laser radar in reference coordinate system The position of the ith laser spot measured below, the translation vector of the laser radar coordinate system in the inertial measurement unit coordinate system, Three-dimensional position vector of moment inertial measurement unit coordinate system in global coordinate system, arbitrary point on plane expressed in global coordinate system and said The method comprises the steps of determining a point-plane geometrical observation residual, expanding the observation residual into an extended observation residual by using a transformation matrix from a moment global coordinate system to an inertial measurement unit coordinate system, and realizing the following expression: Wherein, the Is the observation residual; in a global coordinate system The planar normal vector indicated below; Is that Moment inertial measurement unit coordinate system A gesture rotation matrix in a global coordinate system G; the attitude rotation matrix of the laser radar coordinate system L in the inertial measurement unit coordinate system I; in a reference coordinate system for a laser radar The position of the ith laser spot measured down; the translation vector of the laser radar coordinate system L in the inertial measurement unit coordinate system I; Is that Moment inertial measurement unit coordinate system A three-dimensional position vector in the global coordinate system G; in a global coordinate system Any point on the plane indicated below; Wherein, the To expand the observation residual; The extended observation residual error is the extended observation residual error in the current nominal system state; for observing residual errors with respect to system state vectors Jacobian matrix of (a); A sensitivity jacobian vector for observing the residual with respect to time bias; Is a time interval; is a global coordinate system The planar normal vector indicated below; is a gesture rotation matrix; the attitude rotation matrix of the laser radar coordinate system L in the inertial measurement unit coordinate system I; the translation vector of the laser radar coordinate system L in the inertial measurement unit coordinate system I; The rate of change for position is speed; is a gesture rotation matrix; is the angular velocity; Zero offset rate of change; The attitude rotation matrix of the laser radar coordinate system L in the inertial measurement unit coordinate system I is adopted.
8. The method for suppressing the time stamp conversion error of the laser inertial navigation mileage meter based on the integer time sequence according to claim 1, wherein the method is characterized in that the extended observation residual is stacked to obtain a stacked observation residual, the total residual of each iteration of a measurement set is determined by combining the error state covariance matrix, the total residual is derived and is made to be zero to obtain the Kalman gain and the optimal estimated value of the system state error after a plurality of iterations, and the method is realized by the following expression: Wherein, the The total residual for the j-th iteration of the measurement set; representing the difference in manifold between the nominal state and the prior state of the current iteration, Generalized subtraction specific to manifold space; A jacobian representing a priori state of a jth iteration with respect to error states; A system state error for the jth iteration; extended observation residual at nominal state for jth iteration Is a stack of (a); Is the first An error state covariance matrix of the moment; is Kalman gain; stacking jacobian matrixes after multiple iterations; in order to observe the noise covariance matrix, The optimal estimation value of the system state error after multiple iterations is obtained; D is the stacking of time sensitivity matrixes after multiple iterations; the difference value between the nominal state and the priori state after multiple iterations on manifold; Representing the identity matrix.
9. The method for suppressing the time stamp conversion error of the laser inertial navigation meter based on the integer time sequence according to claim 8, wherein the step of superposing the optimal estimated value of the system state error onto the nominal state of the system to obtain the optimal estimated value of the system state after error suppression of the laser inertial navigation meter is achieved by the following expression: Wherein, the The optimal estimation value of the system state is obtained; is the nominal state of the system.

Description

Laser inertial navigation odometer timestamp conversion error suppression method based on integer time sequence Technical Field The invention relates to the technical field of error suppression of laser inertial navigation mileometers, in particular to a method for suppressing a time stamp conversion error of a laser inertial navigation mileometer based on integer time sequence. Background The laser inertial navigation odometer (LiDAR-InertialOdometry, LIO) calculates the six-degree-of-freedom pose (three-dimensional positions: x, y and z and orientations: roll, pitch and yaw) of the carrier in real time under the condition of no GNSS (GlobalNavigationSatelliteSystem) or preset marks through laser point cloud data and inertial measurement (IMU, inertialMeasurementUnit) data of the three-dimensional laser. The system time stamp is used for calibrating the data sampling time and determining the integral or registration time interval, and if the time stamp has conversion errors, pose recurrence deviation is directly introduced, so that the suppression of the time stamp errors is a key link for improving the positioning accuracy. The system carries out attitude calculation by high-frequency IMU recursion, registers environmental features acquired by the laser radar with historical frames, and periodically corrects drift, meanwhile, the laser inertial navigation odometer also can remove motion distortion of a laser radar acquisition point cloud by using motion information of the IMU, and the two complementary integration can continuously output tracks, so that the conventional precision is kept at a decimeter-meter level, and the system is sufficient for meeting most indoor and outdoor task requirements. Compared with the traditional scheme of relying on magnetic force labels, UWB (UltraWideBand) base stations, visual navigation lines or mechanical guide rails and the like, the laser inertial navigation odometer can work after equipment is electrified without arranging auxiliary facilities in advance on an operation site. However, in GNSS rejection environments such as tunnels, pipe galleries, mine holes, and the like, not only satellite signals are completely blocked, but also such environments are typical degradation scenes, the geometric information is sparse, the structure is repeated, the geometric information collected by the laser radar is insufficient, the laser radar is very easy to drift, positioning failure is often caused, and the navigation task is failed. In order to make up for the defect, the prior art either superimposes additional loads such as a camera and an RTK base station on a sensor end or mines new features from intensity and reflectivity on an algorithm end so as to improve the positioning success rate, but the method needs to synchronously process image data, the hardware cost and the calculation power consumption are obviously increased, or the RTK base station is required to be deployed in advance on site and the satellite signal stability is guaranteed depending on a double-antenna RTK base station positioning and matched lever arm compensation algorithm, so that the complexity and the maintenance cost of the system are greatly increased, or the new features are extracted through intensity gradient, the front-end descriptors are enriched under the geometric degradation scene, the additional intensity calculation and weight iteration increase the calculation force requirement, so that the algorithm module consumes excessive system resources, and the stable operation of other algorithm modules (such as scene understanding and autonomous navigation) cannot be ensured. Therefore, the method is replaced by increasing the sensor or adding new features to improve the positioning success rate, so that the cost, the power consumption and the complexity of the system are increased synchronously. Disclosure of Invention In view of the foregoing, it is necessary to provide a method, an apparatus, a computer device, and a storage medium for suppressing a time stamp conversion error of a laser inertial navigation odometer based on integer timing. The utility model provides a laser inertial navigation odometer timestamp conversion error suppression method based on integer time sequence, inertial measurement unit detects the inertial measurement data of carrier, laser radar detects the laser point cloud data of carrier, laser inertial navigation odometer receives inertial measurement data and laser point cloud data and confirms the state of carrier, the method includes: The method comprises the steps of S1, setting a 64-bit integer nanosecond timestamp in a laser inertial navigation odometer, indexing a data sequence corresponding to laser point cloud data and inertial measurement data, and determining a measurement group based on the data sequence corresponding to the laser point cloud data and the inertial measurement data; S2, acquiring a current system state vector of a laser inertial navigatio