Laser inertial odometer simulation data generation method, system, device and medium

Abstract

The specification discloses a laser inertial odometer simulation data generation method, system, device and medium, which reads three-dimensional environment model configuration file data, configures a scanning mode and an internal parameter of a LiDAR, and configures relative external parameters between the LiDAR and an IMU; receives an input initial trajectory to perform smoothing and time parameterization processing, and generates a continuous time dimension trajectory true value; solves IMU theoretical readings through an inverse kinematics equation, calls an IMU error module to inject selected error items into the theoretical readings, and generates error-bearing IMU simulation data; obtains non-distortion point cloud coordinates through intersection of laser rays and scene planes of a three-dimensional environment model, scans time stamps point by point, reprojects the non-distortion point cloud coordinates to a coordinate system of a frame at a starting moment of the LiDAR through reverse pose interpolation, selectively injects point cloud coordinate errors according to pre-configured instructions, and generates error-bearing simulation point cloud data; and outputs the simulation IMU simulation data and the simulation point cloud data, and saves corresponding physical true values.

Description

Technical Field

[0001] This invention relates to the field of autonomous driving and robot positioning technology, and more specifically, to a method, system, device, and medium for generating laser inertial odometry simulation data. Background Technology

[0002] Laser inertial odometry (LIO) is one of the core technologies for autonomous navigation in autonomous vehicles, mobile robots, and drones. By fusing LiDAR point cloud data and IMU data, it can provide high-precision pose estimation in environments without GNSS signals. The development, testing, and evaluation of LIO algorithms require a large amount of high-quality labeled data. However, the acquisition cost of real-world data is high, labeling is difficult, and it is hard to control the influence of various error sources, which severely restricts the iteration speed of LIO algorithms.

[0003] Existing methods for generating LIO simulation data have several shortcomings. First, the error model is singular and uncontrollable, leading to a significant discrepancy between the simulated and real data. Second, motion distortion simulation is inaccurate; traditional methods typically use inter-frame pose interpolation to simulate motion distortion, ignoring the temporal characteristics of LiDAR point-by-point scanning, resulting in geometric distortion in the generated point cloud data. Furthermore, the error model of existing simulation tools is tightly coupled with the core generation logic, making it impossible to independently enable or disable specific error sources, thus lacking modular design. Additionally, some simulation tools generate IMU data through forward dynamics simulation, which introduces numerical integration errors, causing inconsistencies between IMU readings and the true trajectory value, resulting in insufficient accuracy of the true value and making it unsuitable as a benchmark for algorithm evaluation.

[0004] Therefore, there is an urgent need to study a method for generating simulation data of laser inertial odometry (LIO) to generate high-fidelity, modular simulation data that supports precise control of multiple error sources, so as to meet the data requirements for the development, testing and evaluation of LIO algorithms. Summary of the Invention

[0005] This specification provides a method, system, device, and medium for generating laser inertial odometer simulation data, in order to overcome at least one technical problem existing in related technologies.

[0006] According to a first aspect of the embodiments of this specification, a method for generating simulation data for a laser inertial odometry is provided, comprising: Read the configuration file data of the 3D environment model, configure the scanning mode and intrinsic parameters of the LiDAR laser detection and ranging system according to the configuration file data, and configure the relative extrinsic parameters between the LiDAR and the inertial measurement unit (IMU); The initial trajectory is received and smoothed and time-parameterized using a cubic B-spline curve to generate the true trajectory value in the continuous time dimension. Based on the true value of the trajectory, the theoretical reading of the IMU is calculated by solving the inverse kinematic equation. According to the pre-configured error configuration instructions, the pluggable IMU error module is called to inject the selected error term into the theoretical reading, thereby generating IMU simulation data with error. Based on the true value of the trajectory and the LiDAR intrinsic parameters, the distortion-free point cloud coordinates are obtained by finding the intersection point between the laser ray and the scene plane of the 3D environment model. The timestamp is scanned point by point, and the distortion-free point cloud coordinates are reprojected onto the coordinate system of the LiDAR start time frame through reverse pose interpolation to simulate motion distortion. Point cloud coordinate errors are selectively injected according to pre-configured instructions to generate simulated point cloud data with errors. Output the simulation data from the simulated IMU and the simulation point cloud data, and simultaneously save the corresponding physical truth values.

[0007] Preferably, the step of smoothing and time-parameterizing the initial trajectory received as input using a cubic B-spline curve to generate the true value of the trajectory in the continuous time dimension includes: Receive the initial trajectory input, and randomly generate a preset number of control vertices corresponding to keyframes after trajectory generation. Assign an orientation angle to each keyframe Assign a timestamp to each keyframe Physical constraint verification is performed on the trajectory to maintain the distance between adjacent control vertices within a preset range. Within the range, and the change in orientation angle does not exceed a preset threshold. The interval between adjacent timestamps is [ Within the range; The initial trajectory is fitted and smoothed using a cubic B-spline curve. The expression is in, The coordinates are in the IMU coordinate system. To control the number of vertices, Let be a cubic B-spline basis function, defined as in, For node vectors, the node vector is defined as U=[ , ,…, ], To control the number of vertices; For those with Cubic uniform B-spline with control vertices, the node vectors satisfy: in, For node vectors, To control the number of vertices; Adjust the position of the control vertex to generate a smooth motion trajectory, parameterize the trajectory over time, and then... Mapping to time This ensures that the velocity and acceleration of the trajectory meet the physical constraints, and that the time resolution of the trajectory is configured to be consistent with the IMU sampling frequency; Output the trajectory ground truth at time resolution, including the position in the world coordinate system. Rotation matrix ,speed acceleration and angular velocity .

[0008] Preferably, the step of solving the IMU theoretical readings using inverse kinematic equations includes: Inversion calculations are performed based on the rigid body kinematics equations. Using the rotation matrix of the IMU coordinate system relative to the world coordinate system, and the acceleration, angular velocity, and gravitational acceleration vector in the world coordinate system, error-free specific force and angular velocity readings are obtained. Through formula Calculate the error-free specific force in the IMU body coordinate system using the formula Calculate the error-free angular velocity in the IMU body coordinate system, where Let be the transpose of the rotation matrix obtained from the true value of the trajectory. It is the gravity vector; The central difference method is used to calculate velocity, acceleration, and angular velocity in the world coordinate system, as shown in the following formulas: in, For time step, For the logarithmic mapping of the rotation matrix; The theoretical IMU readings are obtained, including timestamps and velocity. acceleration angular velocity .

[0009] Preferably, the step of injecting selected error terms into the theoretical readings by calling a pluggable IMU error module according to a pre-configured error configuration instruction to generate IMU simulation data with errors includes: According to the pre-configured error configuration instructions, the pluggable IMU error module is invoked. The IMU error model adopts a pluggable modular design, including a Gaussian white noise module, a zero-offset module, a proportional coefficient error module, and a temperature drift module. Each module uses a loosely coupled architecture, can be independently enabled or disabled, and receives the enable status flag and parameter configuration table through a unified configuration interface. The Gaussian white noise module adds independent zero-mean Gaussian white noise to the accelerometer and gyroscope respectively, and the standard deviation of the noise is set through the configuration file; The zero-offset module is modeled using a first-order Markov process. It simulates random walk zero-offset by configuring the relevant time constant and the noise standard deviation. The relevant time constant and the noise standard deviation are set through the configuration file. The scaling factor error module uses a diagonal matrix to represent the deviation between the IMU's scale factor and the true value. The scaling factor error is set through a configuration file. The temperature drift module simulates the nonlinear effect of ambient temperature changes on the IMU zero offset by configuring a reference temperature and a temperature coefficient, where the temperature coefficient is set through a configuration file. Selected error terms are injected into the theoretical readings. These selected error terms include individually enabling any one error module or combining multiple error modules for superposition calculation. IMU simulation data with errors is generated according to the selected error combination.

[0010] Preferably, the step of obtaining distortion-free point cloud coordinates by finding the intersection points of laser rays and the scene plane of the 3D environment model based on the trajectory ground truth and LiDAR intrinsic parameters includes: For each scan point of LiDAR, a ray equation is constructed from the origin in the LiDAR coordinate system. The intersection points of the ray equation with the planar geometric elements in the 3D environment model are calculated. The intersection points of the ray and the scene are accelerated by the KD-Tree spatial indexing algorithm. The polygonal patches in the 3D environment model are preprocessed to construct the KD-Tree. For each laser ray, the potential intersecting surface subset selected by the KD-Tree is traversed to calculate the intersection points. The orthogonal point closest to the origin is selected as the precise coordinates of the scanning point. If the distance between the intersection points exceeds the maximum ranging range of LiDAR, the point is determined to be invalid, and the coordinates of the distortion-free point cloud are obtained.

[0011] Preferably, the step of reprojecting the distortion-free point cloud coordinates onto the coordinate system of the LiDAR start time frame through reverse pose interpolation to simulate motion distortion includes: Calculate the precise scan time for each scan point based on the start time, end time, and total number of points in the LiDAR frame scan; Based on the true value of the trajectory, the pose of the IMU in the world coordinate system at the precise scanning time is obtained by cubic spline interpolation. The error-free and distortion-free point cloud is transformed from the world coordinate system to the coordinate system at the start of the LiDAR frame. The coordinates of the scan points with motion distortion are obtained by point-by-point reverse pose interpolation and reprojection.

[0012] Preferably, the step of selectively injecting point cloud coordinate errors according to pre-configured instructions to generate simulated point cloud data with errors includes: The point cloud coordinates are injected with ranging error and angle error. The ranging error is injected by superimposing distance-related Gaussian noise on the true distance, and the angle error is injected by superimposing Gaussian noise on the horizontal scanning angle and the vertical scanning angle, respectively.

[0013] According to a second aspect of the embodiments of this specification, a laser inertial odometry simulation data generation system is provided, comprising a parameter configuration module, a trajectory simulation module, an IMU simulation module, a point cloud simulation module, and a data output module, wherein... The parameter configuration module is configured to read the configuration file data of the three-dimensional environment model, configure the scanning mode and intrinsic parameters of the LiDAR laser detection and ranging system according to the configuration file data, and configure the relative extrinsic parameters between the LiDAR and the inertial measurement unit (IMU). The trajectory simulation module is configured to receive the input initial trajectory, and use a cubic B-spline curve to smooth and parameterize the initial trajectory over time to generate the true value of the trajectory in the continuous time dimension. The IMU simulation module is configured to calculate the theoretical IMU readings based on the true value of the trajectory by solving the inverse kinematic equations, and to inject selected error terms into the theoretical readings by calling the pluggable IMU error module according to the pre-configured error configuration instructions, thereby generating IMU simulation data with errors. The point cloud simulation module is configured to obtain distortion-free point cloud coordinates by finding the intersection points of laser rays and the scene plane of the 3D environment model based on the trajectory true value and LiDAR intrinsic parameters, scan timestamps point by point, reproject the distortion-free point cloud coordinates onto the coordinate system of the LiDAR start time frame through reverse pose interpolation, simulate motion distortion, and selectively inject point cloud coordinate errors according to pre-configured instructions to generate simulated point cloud data with errors. The data output module is configured to output the simulation IMU simulation data and the simulation point cloud data, and simultaneously save the corresponding physical truth values.

[0014] According to a third aspect of the embodiments of this specification, a computing device is provided, including a storage device and a processor, the storage device being used to store a computer program, and the processor running the computer program to cause the computing device to perform the steps of the laser inertial odometry simulation data generation method.

[0015] According to a fourth aspect of the embodiments of this specification, a storage medium is provided that stores a computer program used in the computing device, which, when executed by a processor, implements the steps of the laser inertial odometry simulation data generation method.

[0016] The beneficial effects of the embodiments in this specification are as follows: This specification provides an embodiment of a method, system, device, and medium for generating simulation data of a laser inertial odometry (IMU). The method reads configuration file data from a 3D environment model, configures the LiDAR's scanning mode, intrinsic parameters, and relative extrinsic parameters with the IMU, receives the initial trajectory, performs smoothing and time parameterization processing to generate the true value of the trajectory in the continuous time dimension, solves the IMU theoretical readings using inverse kinematics equations, calls the IMU error module to inject selected error terms into the theoretical readings, and generates IMU simulation data with errors. Distortion-free point cloud coordinates are obtained by finding the intersection points of the laser ray and the scene plane of the 3D environment model. Timestamps are scanned point by point, and the distortion-free point cloud coordinates are reprojected onto the coordinate system of the LiDAR's initial time frame using inverse pose interpolation. Point cloud coordinate errors are selectively injected according to pre-configured instructions to generate simulated point cloud data with errors. The method outputs the simulated IMU data and the simulated point cloud data, and saves the corresponding physical true values. The embodiments in this specification achieve multiple combinations and controllability of error models through relatively independent error modules, making the simulation data closer to real data. By introducing time parameters into keyframes and simulating motion distortion based on time characteristics, the quality of simulation data is improved. This achieves high-fidelity, modular, and precise control of multiple error sources in simulation data, which is progressive.

[0017] The innovative aspects of the embodiments in this specification include: 1. In this specification, error-free IMU data is generated by rigorous kinematic equation inversion, ensuring that the IMU readings are completely consistent with the true trajectory values. The LiDAR motion distortion is accurately simulated by point-by-point reverse pose interpolation and reprojection, and the true geometric values ​​of the point cloud are absolutely accurate. This is one of the innovative points of the embodiments in this specification.

[0018] 2. In this specification, various errors are designed as pluggable modules, which can be enabled or disabled independently or in combination and their parameters can be configured. This facilitates isolated analysis of the impact of specific error sources on the performance of the LIO algorithm, can easily expand to more error types, all error parameters can be precisely configured, the error injection process is repeatable, and it can generate multiple sets of data with the same truth value but different error characteristics. This is one of the innovative points of the embodiments in this specification.

[0019] 3. In this specification, Gaussian white noise, null offset, scaling factor error, and temperature drift of the IMU are simulated, as well as motion distortion, ranging error, and angle error of the LiDAR and other real sensor characteristics. The statistical characteristics of the simulated data are highly consistent with those of the real data. Compared with existing simulation tools, the KL divergence of the data generated by this invention is reduced by more than 65% compared with the real data, which is one of the innovations of the embodiments in this specification. Attached Figure Description

[0020] To more clearly illustrate the technical solutions in the embodiments or related technologies of this specification, the drawings used in the description of the embodiments or related technologies will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0021] Figure 1 A schematic flowchart illustrating a laser inertial odometry simulation data generation method provided in one embodiment of this specification; Figure 2 A schematic diagram of a laser inertial odometry simulation data generation system provided in one embodiment of this specification; Figure 3 This is a schematic diagram of the structure of a computing device provided in one embodiment of this specification; Figure 4 This is a schematic diagram of the structure of a storage medium provided in one embodiment of this specification. Detailed Implementation

[0022] The technical solutions in the embodiments of this specification will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0023] It should be noted that the terms "comprising" and "having," and any variations thereof, in the embodiments and drawings of this specification are intended to cover non-exclusive inclusion. For example, a process, method, system, product, or device that includes a series of steps or units is not limited to the steps or units listed, but may optionally include steps or units not listed, or may optionally include other steps or units inherent to these processes, methods, products, or devices.

[0024] This specification discloses a method, system, device, and medium for generating simulation data of a laser inertial odometry, which will be described in detail below.

[0025] Figure 1 This is a schematic flowchart illustrating a laser inertial odometry simulation data generation method provided in one embodiment of this specification. Figure 1 As shown, a method for generating simulation data for a laser inertial odometry system includes: S110. Read the configuration file data of the three-dimensional environment model, configure the scanning mode and intrinsic parameters of the LiDAR laser detection and ranging system according to the configuration file data, and configure the relative extrinsic parameters between the LiDAR and the inertial measurement unit (IMU).

[0026] Sensor parameters include the LiDAR scanning mode, intrinsic parameters, and relative extrinsic parameters between the LiDAR and IMU. The LiDAR scanning mode includes mechanical rotation scanning, solid-state MEMS scanning, or Flash scanning. The LiDAR intrinsic parameters include horizontal field of view, vertical field of view, horizontal resolution, vertical resolution, scanning frequency, and maximum ranging range. The relative extrinsic parameters between the LiDAR and IMU include translation vectors and rotation matrices, used to define the spatial transformation relationship between the LiDAR coordinate system and the IMU coordinate system.

[0027] It supports loading 3D simulation environment models in various formats, including PLY, OBJ, and STL. The environment model consists of multiple planar polygons, each containing vertex coordinates, normal vectors, and material properties. To improve the efficiency of ray intersection calculation, the system uses a KD-Tree to spatially partition the environment model, reducing the complexity of intersection calculation from... Reduce to .

[0028] S120: Receive the input initial trajectory, and use a cubic B-spline curve to smooth and parameterize the initial trajectory over time to generate the true value of the trajectory in the continuous time dimension.

[0029] The initial trajectory can be entered in three ways: manually input keyframe poses, import trajectory data collected from the real world, or automatically generate a random trajectory.

[0030] In a specific embodiment, the step of receiving the input initial trajectory and smoothing and time-parameterizing the initial trajectory using a cubic B-spline curve to generate the true value of the trajectory in the continuous time dimension includes: Receive the initial trajectory input, and randomly generate a preset number of control vertices corresponding to keyframes after trajectory generation. Assign an orientation angle to each keyframe Assign a timestamp to each keyframe Physical constraint verification is performed on the trajectory to maintain the distance between adjacent control vertices within a preset range. Within the range, and the change in orientation angle does not exceed a preset threshold. The interval between adjacent timestamps is [ Within the range.

[0031] The initial trajectory is fitted and smoothed using a cubic B-spline curve, wherein the cubic B-spline curve... The expression is in, The coordinates are in the IMU coordinate system. To control the number of vertices, Let be a cubic B-spline basis function, defined as in, For node vectors, the node vector is defined as U=[ , ,…, ], To control the number of vertices.

[0032] For those with Cubic uniform B-spline with control vertices, where the node vectors satisfy: in, For node vectors, To control the number of vertices; Adjust the position of the control vertex to generate a smooth motion trajectory, parameterize the trajectory over time, and then... Mapping to time This ensures that the velocity and acceleration of the trajectory meet physical constraints, and that the time resolution of the trajectory is configured to be consistent with the IMU sampling frequency.

[0033] Output the trajectory ground truth at time resolution, including the position in the world coordinate system. Rotation matrix ,speed acceleration and angular velocity .

[0034] S130. Based on the true value of the trajectory, the theoretical reading of the IMU is calculated by solving the inverse kinematic equation. According to the pre-configured error configuration instruction, the pluggable IMU error module is called to inject the selected error term into the theoretical reading, thereby generating IMU simulation data with error.

[0035] In a specific embodiment, the step of solving the IMU theoretical readings through inverse kinematic equations includes: Inversion calculations are performed based on the rigid body kinematics equations. Using the rotation matrix of the IMU coordinate system relative to the world coordinate system, and the acceleration, angular velocity, and gravitational acceleration vector in the world coordinate system, error-free specific force and angular velocity readings are obtained. Through formula Calculate the error-free specific force in the IMU body coordinate system using the formula Calculate the error-free angular velocity in the IMU body coordinate system, where Let be the transpose of the rotation matrix obtained from the true value of the trajectory. This is the gravity vector.

[0036] The central difference method is used to calculate velocity, acceleration, and angular velocity in the world coordinate system, as shown in the following formulas: in, For time step, For the logarithmic mapping of the rotation matrix; The theoretical IMU readings are obtained, including timestamps and velocity. acceleration angular velocity .

[0037] In a specific embodiment, the step of injecting selected error terms into the theoretical readings by calling a pluggable IMU error module according to a pre-configured error configuration instruction to generate IMU simulation data with errors includes: According to the pre-configured error configuration instructions, the pluggable IMU error module is invoked. The IMU error model adopts a pluggable modular design, including a Gaussian white noise module, a zero-offset module, a proportional coefficient error module, and a temperature drift module. Each module uses a loosely coupled architecture, can be independently enabled or disabled, and receives the enable status flag and parameter configuration table through a unified configuration interface. The Gaussian white noise module adds independent zero-mean Gaussian white noise to the accelerometer and gyroscope respectively. The standard deviation of noise , Configure via configuration file.

[0038] The zero-offset module is modeled using a first-order Markov process. By configuring the relevant time constant and the standard deviation of the zero-offset noise, it simulates the random walk zero-offset. in, and For the relevant time constant, and The zero-position offset noise standard deviation is set via a configuration file, along with the relevant time constant and noise standard deviation.

[0039] The scaling factor error module uses a diagonal matrix to represent the deviation between the IMU's calibration factor and the true value. in, and This is the scaling factor error matrix, and the scaling factor error is set through the configuration file.

[0040] The temperature drift module simulates the nonlinear effect of ambient temperature changes on the IMU zero-point offset by configuring a reference temperature and temperature coefficient. in For reference temperature, and This refers to the temperature coefficient, which is set via a configuration file.

[0041] Selected error terms are injected into the theoretical readings. These selected error terms include individually enabling any one error module or combining multiple error modules for superposition calculation. IMU simulation data with errors is generated according to the selected error combination.

[0042] S140. Based on the true value of the trajectory and the LiDAR intrinsic parameters, the distortion-free point cloud coordinates are obtained by finding the intersection point between the laser ray and the scene plane of the three-dimensional environment model. The timestamp is scanned point by point, and the distortion-free point cloud coordinates are reprojected onto the coordinate system of the LiDAR starting frame through reverse pose interpolation to simulate motion distortion. Point cloud coordinate errors are selectively injected according to pre-configured instructions to generate simulated point cloud data with errors.

[0043] In a specific embodiment, the step of obtaining distortion-free point cloud coordinates by finding the intersection point between the laser ray and the scene plane of the 3D environment model based on the trajectory true value and LiDAR intrinsic parameters includes: For each scan point of LiDAR, a ray equation is constructed from the origin in the LiDAR coordinate system. The intersection points of the ray equation with the planar geometric elements in the 3D environment model are calculated. The KD-Tree spatial indexing acceleration algorithm is used to calculate the intersection points of the ray and the scene. The polygonal patches in the 3D environment model are preprocessed to construct the KD-Tree.

[0044] For each laser ray, the potential intersecting surface subset selected by the KD-Tree is traversed to calculate the intersection points. The orthogonal point closest to the origin is selected as the precise coordinates of the scanning point. If the distance between the intersection points exceeds the maximum ranging range of LiDAR, the point is determined to be invalid, and the coordinates of the distortion-free point cloud are obtained.

[0045] For mechanically rotating LiDAR, the horizontal and vertical angles at each scan point can be calculated using the following formulas: (The formulas provided are not directly related to the calculation of the ray direction.) in, The rotational angular velocity of LiDAR, The time of the scan point, The initial horizontal angle, The minimum vertical angle, For vertical resolution, This is the laser channel number.

[0046] The unit direction vector of each scan point in the LiDAR coordinate system is: .

[0047] To find the intersection of a ray and a plane, a KD-Tree is used to accelerate the process. For each ray, the KD-Tree is first used to find a subset of planes that may intersect, and then the intersection points of the ray and the planes are calculated one by one.

[0048] The formula for finding the intersection of a ray and a plane is as follows: Let the equation of the plane be ,in Let be the unit normal vector of the plane. Let be the distance from the plane to the origin. Then, apply the ray equation... Substituting into the plane equation, we get: like and ( (where LiDAR's maximum ranging range is given), the ray intersects the plane, and the distance between the intersection points is... .

[0049] Point cloud generation: For each scanned point, the minimum intersection distance is taken as the ranging value for that point, and then the coordinates of that point in the LiDAR coordinate system are calculated. If no valid intersection point is found, the point is invalid and marked as invalid. .

[0050] For LiDAR's first There are 1 scan point, and its unit direction vector in the LiDAR coordinate system is 1. Then the ray equation is: in, Using the origin of the LiDAR coordinate system, find the intersections of the ray with all planes in the scene model, and take the minimum distance between the intersection points. Then the coordinates of that point are: like If the distance exceeds the maximum ranging range of LiDAR, the point is invalid.

[0051] In a specific embodiment, the step of reprojecting the distortion-free point cloud coordinates onto the coordinate system of the LiDAR start time frame through reverse pose interpolation to simulate motion distortion includes: Calculate the precise scan time for each scan point based on the start time, end time, and total number of points in the LiDAR frame scan; Based on the true value of the trajectory, the pose of the IMU in the world coordinate system at the precise scanning time is obtained by cubic spline interpolation. The error-free and distortion-free point cloud is transformed from the world coordinate system to the coordinate system at the start of the LiDAR frame. The coordinates of the scan points with motion distortion are obtained by point-by-point reverse pose interpolation and reprojection.

[0052] First, record the start time of the LiDAR frame. and end time For the first The precise scanning time for each scan point is: Where N is the total number of points in a point cloud frame.

[0053] Furthermore, based on the true value of the trajectory, interpolation yields... Position of the IMU in the world coordinate system .

[0054] Then, transform the distortion-free point cloud to the world coordinate system: Finally, the points in the world coordinate system are reprojected into the coordinate system at the start of the LiDAR frame: This method accurately considers the temporal characteristics of LiDAR point-by-point scanning and can generate motion distortion point clouds consistent with reality.

[0055] In a specific embodiment, the step of selectively injecting point cloud coordinate errors according to pre-configured instructions to generate simulated point cloud data with errors includes: The point cloud coordinates are injected with ranging error and angle error. The ranging error is injected by superimposing distance-related Gaussian noise on the true distance, and the angle error is injected by superimposing Gaussian noise on the horizontal scanning angle and the vertical scanning angle, respectively.

[0056] Add distance-dependent Gaussian noise: in, This represents the standard deviation of the near-range ranging error. This is the distance correlation coefficient.

[0057] Add Gaussian noise at both horizontal and vertical angles: in, and These are the horizontal angle and the vertical angle, respectively.

[0058] S150. Output the simulation data of the simulation IMU and the simulation point cloud data, and save the corresponding physical truth values ​​simultaneously.

[0059] The physical truth file includes a continuous time-dimensional IMU pose truth sequence, containing position, rotation matrix, velocity, acceleration, and angular velocity; error-free point cloud coordinates corresponding one-to-one with LiDAR scan points; and error-free acquisition timestamps.

[0060] The simulation data output format supports ROS bag format or a custom binary format. Among them, the ROS bag format is the most commonly used format and can be directly used by the LIO algorithm in the ROS system.

[0061] The output simulation data includes: IMU data: timestamp, acceleration, angular velocity; LiDAR point cloud data: timestamp, point cloud coordinates, reflectivity; trajectory ground truth: timestamp, position, rotation, velocity, acceleration, angular velocity; IMU ground truth: timestamp, error-free acceleration, error-free angular velocity; point cloud ground truth: timestamp, error-free and distortion-free point cloud coordinates. All data timestamps are synchronized to the same clock source to ensure data time consistency.

[0062] Figure 2 This is a schematic diagram of a laser inertial odometry simulation data generation system provided in one embodiment of this specification. Figure 2 As shown, a laser inertial odometry simulation data generation system 200 includes a parameter configuration module 210, a trajectory simulation module 220, an IMU simulation module 230, a point cloud simulation module 240, and a data output module 250, wherein... The parameter configuration module 210 is configured to read the configuration file data of the three-dimensional environment model, configure the scanning mode and intrinsic parameters of the laser detection and ranging system LiDAR according to the configuration file data, and configure the relative extrinsic parameters between LiDAR and inertial measurement unit (IMU).

[0063] The trajectory simulation module 220 is configured to receive the input initial trajectory, and use a cubic B-spline curve to smooth and parameterize the initial trajectory over time to generate the true value of the trajectory in the continuous time dimension.

[0064] The IMU simulation module 230 is configured to calculate the theoretical IMU readings based on the true value of the trajectory by solving the inverse kinematic equations, and to inject selected error terms into the theoretical readings by calling a pluggable IMU error module according to a pre-configured error configuration instruction, thereby generating IMU simulation data with errors.

[0065] The point cloud simulation module 240 is configured to obtain distortion-free point cloud coordinates by finding the intersection points of laser rays and the scene plane of the 3D environment model based on the trajectory true value and LiDAR intrinsic parameters, scan timestamps point by point, reproject the distortion-free point cloud coordinates onto the coordinate system of the LiDAR start time frame through reverse pose interpolation, simulate motion distortion, and selectively inject point cloud coordinate errors according to pre-configured instructions to generate simulated point cloud data with errors.

[0066] The data output module 250 is configured to output the simulation IMU simulation data and the simulation point cloud data, and simultaneously save the corresponding physical truth values.

[0067] Figure 3 This is a schematic diagram of the structure of a computing device provided in one embodiment of this specification. Figure 3 As shown, a computing device 300 includes a storage device 310 and a processor 320. The storage device 310 stores a computer program, and the processor 320 runs the computer program to enable the computing device 300 to perform the steps of the laser inertial odometry simulation data generation method.

[0068] Figure 4 This is a schematic diagram of the structure of a storage medium provided in one embodiment of this specification. For example... Figure 4 As shown, a storage medium 400 stores a computer program 410 used in the computing device, which, when executed by a processor, implements the steps of the laser inertial odometry simulation data generation method.

[0069] In summary, the embodiments of this specification provide a method, system, device, and medium for generating laser inertial odometry simulation data. This method employs point-by-point reverse pose interpolation and reprojection, accurately considering the precise scanning time of each scanning point, and can generate motion distortion point cloud simulation data consistent with the real situation, meeting the data requirements for the development and evaluation of LIO algorithms for various platforms such as autonomous driving, robotics, and drones.

[0070] Those skilled in the art will understand that the accompanying drawings are merely schematic diagrams of one embodiment, and the modules or processes shown in the drawings are not necessarily essential for implementing the present invention.

[0071] Those skilled in the art will understand that the modules in the apparatus of the embodiments can be distributed in the apparatus of the embodiments as described in the embodiments, or they can be located in one or more devices different from this embodiment with corresponding changes. The modules of the above embodiments can be combined into one module, or they can be further divided into multiple sub-modules.

[0072] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A method for generating simulation data for a laser inertial odometry system, characterized in that, include: Read the configuration file data of the 3D environment model, configure the scanning mode and intrinsic parameters of the LiDAR laser detection and ranging system according to the configuration file data, and configure the relative extrinsic parameters between the LiDAR and the inertial measurement unit (IMU); The initial trajectory is received and smoothed and time-parameterized using a cubic B-spline curve to generate the true trajectory value in the continuous time dimension. Based on the true value of the trajectory, the theoretical reading of the IMU is calculated by solving the inverse kinematic equation. According to the pre-configured error configuration instructions, the pluggable IMU error module is called to inject the selected error term into the theoretical reading, thereby generating IMU simulation data with error. Based on the true value of the trajectory and the LiDAR intrinsic parameters, the distortion-free point cloud coordinates are obtained by finding the intersection point between the laser ray and the scene plane of the 3D environment model. The timestamp is scanned point by point, and the distortion-free point cloud coordinates are reprojected onto the coordinate system of the LiDAR start time frame through reverse pose interpolation to simulate motion distortion. Point cloud coordinate errors are selectively injected according to pre-configured instructions to generate simulated point cloud data with errors. Output the simulation data from the simulated IMU and the simulation point cloud data, and simultaneously save the corresponding physical truth values.

2. The method according to claim 1, characterized in that, The steps of receiving the initial trajectory input and smoothing and time-parameterizing the initial trajectory using a cubic B-spline curve to generate the true value of the trajectory in the continuous time dimension include: Receive the initial trajectory input, and randomly generate a preset number of control vertices corresponding to keyframes after trajectory generation. Assign an orientation angle to each keyframe Assign a timestamp to each keyframe Physical constraint verification is performed on the trajectory to maintain the distance between adjacent control vertices within a preset range. Within the range, and the change in orientation angle does not exceed a preset threshold. The interval between adjacent timestamps is [ Within the range; The initial trajectory is fitted and smoothed using a cubic B-spline curve, wherein the cubic B-spline curve... The expression is in, The coordinates are in the IMU coordinate system. To control the number of vertices, Let be a cubic B-spline basis function, defined as in, For node vectors, the node vector is defined as U=[ , ,…, ], To control the number of vertices; For those with Cubic uniform B-spline with control vertices, the node vectors satisfy: in, For node vectors, To control the number of vertices; Adjust the position of the control vertex to generate a smooth motion trajectory, parameterize the trajectory over time, and then... Mapping to time This ensures that the velocity and acceleration of the trajectory meet the physical constraints, and that the time resolution of the trajectory is configured to be consistent with the IMU sampling frequency; Output the trajectory ground truth at time resolution, including the position in the world coordinate system. Rotation matrix ,speed acceleration and angular velocity .

3. The method according to claim 2, characterized in that, The steps for solving the theoretical IMU readings using inverse kinematic equations include: Inversion calculations are performed based on the rigid body kinematics equations. Using the rotation matrix of the IMU coordinate system relative to the world coordinate system, and the acceleration, angular velocity, and gravitational acceleration vector in the world coordinate system, error-free specific force and angular velocity readings are obtained. Through formula Calculate the error-free specific force in the IMU body coordinate system using the formula Calculate the error-free angular velocity in the IMU body coordinate system, where Let be the transpose of the rotation matrix obtained from the true value of the trajectory. It is the gravity vector; The central difference method is used to calculate velocity, acceleration, and angular velocity in the world coordinate system, as shown in the following formulas: in, For time step, For the logarithmic mapping of the rotation matrix; The theoretical IMU readings are obtained, including timestamps and velocity. acceleration angular velocity .

4. The method according to claim 3, characterized in that, The step of injecting selected error terms into theoretical readings by calling a pluggable IMU error module according to a pre-configured error configuration instruction to generate IMU simulation data with errors includes: According to the pre-configured error configuration instructions, the pluggable IMU error module is invoked. The IMU error model adopts a pluggable modular design, including a Gaussian white noise module, a zero-offset module, a proportional coefficient error module, and a temperature drift module. Each module uses a loosely coupled architecture, can be independently enabled or disabled, and receives the enable status flag and parameter configuration table through a unified configuration interface. The Gaussian white noise module adds independent zero-mean Gaussian white noise to the accelerometer and gyroscope respectively, and the standard deviation of the noise is set through the configuration file; The zero-offset module is modeled using a first-order Markov process. It simulates random walk zero-offset by configuring the relevant time constant and the noise standard deviation. The relevant time constant and the noise standard deviation are set through the configuration file. The scaling factor error module uses a diagonal matrix to represent the deviation between the IMU's scale factor and the true value. The scaling factor error is set through a configuration file. The temperature drift module simulates the nonlinear effect of ambient temperature changes on the IMU zero offset by configuring a reference temperature and a temperature coefficient, where the temperature coefficient is set through a configuration file. Selected error terms are injected into the theoretical readings. These selected error terms include individually enabling any one error module or combining multiple error modules for superposition calculation. IMU simulation data with errors is generated according to the selected error combination.

5. The method according to claim 2, characterized in that, The step of obtaining distortion-free point cloud coordinates by finding the intersection points of laser rays and the scene plane of the 3D environment model based on the true trajectory value and LiDAR intrinsic parameters includes: For each scan point of LiDAR, a ray equation is constructed from the origin in the LiDAR coordinate system. The intersection points of the ray equation with the planar geometric elements in the 3D environment model are calculated. The intersection points of the ray and the scene are accelerated by the KD-Tree spatial indexing algorithm. The polygonal patches in the 3D environment model are preprocessed to construct the KD-Tree. For each laser ray, the potential intersecting surface subset selected by the KD-Tree is traversed to calculate the intersection points. The orthogonal point closest to the origin is selected as the precise coordinates of the scanning point. If the distance between the intersection points exceeds the maximum ranging range of LiDAR, the point is determined to be invalid, and the coordinates of the distortion-free point cloud are obtained.

6. The method according to claim 5, characterized in that, The step of reprojecting the distortion-free point cloud coordinates onto the coordinate system of the LiDAR start time frame through reverse pose interpolation to simulate motion distortion includes: Calculate the precise scan time for each scan point based on the start time, end time, and total number of points in the LiDAR frame scan; Based on the true value of the trajectory, the pose of the IMU in the world coordinate system at the precise scanning time is obtained by cubic spline interpolation. The error-free and distortion-free point cloud is transformed from the world coordinate system to the coordinate system at the start of the LiDAR frame. The coordinates of the scan points with motion distortion are obtained by point-by-point reverse pose interpolation and reprojection.

7. The method according to claim 6, characterized in that, The step of selectively injecting point cloud coordinate errors according to pre-configured instructions to generate simulated point cloud data with errors includes: The point cloud coordinates are injected with ranging error and angle error. The ranging error is injected by superimposing distance-related Gaussian noise on the true distance, and the angle error is injected by superimposing Gaussian noise on the horizontal scanning angle and the vertical scanning angle, respectively.

8. A laser inertial odometry simulation data generation system, characterized in that, It includes a parameter configuration module, a trajectory simulation module, an IMU simulation module, a point cloud simulation module, and a data output module, among which... The parameter configuration module is configured to read the configuration file data of the three-dimensional environment model, configure the scanning mode and intrinsic parameters of the LiDAR laser detection and ranging system according to the configuration file data, and configure the relative extrinsic parameters between the LiDAR and the inertial measurement unit (IMU). The trajectory simulation module is configured to receive the input initial trajectory, and use a cubic B-spline curve to smooth and parameterize the initial trajectory over time to generate the true value of the trajectory in the continuous time dimension. The IMU simulation module is configured to calculate the theoretical IMU readings based on the true value of the trajectory by solving the inverse kinematic equations, and to inject selected error terms into the theoretical readings by calling the pluggable IMU error module according to the pre-configured error configuration instructions, thereby generating IMU simulation data with errors. The point cloud simulation module is configured to obtain distortion-free point cloud coordinates by finding the intersection points of laser rays and the scene plane of the 3D environment model based on the trajectory true value and LiDAR intrinsic parameters, scan timestamps point by point, reproject the distortion-free point cloud coordinates onto the coordinate system of the LiDAR start time frame through reverse pose interpolation, simulate motion distortion, and selectively inject point cloud coordinate errors according to pre-configured instructions to generate simulated point cloud data with errors. The data output module is configured to output the simulation IMU simulation data and the simulation point cloud data, and simultaneously save the corresponding physical truth values.

9. A computing device, characterized in that, The device includes a storage device and a processor, the storage device being used to store a computer program, and the processor running the computer program to cause the computing device to perform the steps of the method according to any one of claims 1-7.

10. A storage medium, characterized in that, It stores a computer program used in the computing device of claim 9, which, when executed by a processor, implements the steps of the method of any one of claims 1-7.

Images