Lidar odometer method using piecewise linear continuous time trajectory

Abstract

Disclosed in the present invention is a LiDAR odometer method using a piecewise linear continuous time trajectory. The present invention comprises the following steps: first, using a time window to divide an initial LiDAR point cloud time sequence so as to obtain LiDAR point cloud sub-sequences under a plurality of time windows, and constructing an initial local point cloud map; and then, on the basis of the initial local point cloud map, optimizing LiDAR poses of the LiDAR point cloud sub-sequences under the time windows to obtain an overall continuous time trajectory and a final local point cloud map. The present invention achieves high-precision motion estimation for LiDARs, has high robustness and real-time performance, is applicable to the fields such as robot navigation, simultaneous localization and mapping, automated driving, and provides a relatively simple and efficient solution that can effectively handle the characteristics of LiDARs as streaming sensors.

Description

A lidar odometry method using piecewise linear continuous-time trajectories Technical Field

[0001] The present invention relates to technologies related to simultaneous localization and mapping in the field of robotics, and more specifically to an odometry method that relies solely on input from a laser radar and uses piecewise linear continuous-time trajectories. Background Art

[0002] LiDAR odometry is a crucial component in many robotics applications. Due to LiDAR's excellent distance sensing capabilities, it plays a key role in robot navigation and path planning in GPS-restricted environments.

[0003] After years of exploration and development, methods that rely solely on a single lidar sensor lack accuracy and robustness. Multi-sensor fusion methods are widely considered to be the best solution for practical applications. In particular, lidar-inertial odometry (LIO), which is tightly coupled with information from an inertial measurement unit (IMU), has been widely used in practice. Compared with existing methods that rely solely on lidar information, the LIO method offers two advantages: first, the high-frequency IMU state facilitates more accurate motion compensation; second, the IMU enhances the robustness of the system by directly imposing motion constraints. However, stable LIO relies heavily on accurate calibration of IMU parameters, and IMU data is very sensitive to temperature changes and mechanical shock. In addition, even the most advanced LIO systems currently have difficulty handling situations where the dynamic state exceeds the measurement range of the IMU.

[0004] LiDAR is a streaming sensor that continuously moves, capturing scene structure. Previous research on LiDAR odometry has represented motion trajectories as a series of discrete time poses, breaking the inherent connection between the acquired geometric information and sensor motion. Consequently, current odometry methods that use only LiDAR input are far less accurate than multi-sensor fusion odometry methods.

[0005] Summary of the Invention

[0006] To address the challenges presented in the background art, this paper provides a novel continuous-time trajectory parameterization method for motion estimation using lidar odometry. This method is applicable to various types of lidars, including multi-line scanning and non-repeating scanning lidars, as well as single or multiple lidar system configurations.

[0007] The technical solution adopted by the present invention includes the following steps:

[0008] 1. A LiDAR Odometry Method Using Piecewise Linear Continuous-Time Trajectories

[0009] S1: After dividing the initial lidar point cloud time series into time windows, obtain lidar point cloud subsequences under multiple time windows and construct an initial local point cloud map;

[0010] S2: After linear parameterization of the lidar point cloud subsequence under each time window, the initial continuous time trajectory is obtained;

[0011] S3: Based on the current continuous-time trajectory and the current local point cloud map, the point-to-plane error corresponding to each point in the current time window is calculated and formed into a point-to-plane error constraint, and the trajectory smoothness constraint is established;

[0012] S4: Based on the point-to-surface error constraints and trajectory smoothness constraints, the Gauss-Newton method is used to minimize the objective function, and the continuous-time trajectory within the current time window is estimated and updated;

[0013] S5: Repeat S3 and S4, and perform iterative optimization of the pose in the current time window according to the updated continuous-time trajectory until the objective function is less than the preset threshold, and obtain the optimal continuous-time trajectory of the current time window and the corresponding lidar point cloud in the world coordinate system; and add the lidar point cloud in the world coordinate system of the current time window to the current local point cloud map;

[0014] S6: Repeat S2-S5, and optimize the lidar pose of the lidar point cloud subsequence in each time window in turn to obtain the total continuous time trajectory.

[0015] In S3, for each point in the current time window, the step of calculating the point-to-plane error corresponding to each point is specifically as follows:

[0016] First, use the current continuous time trajectory to get the current point p i The position of the lidar in the world coordinate system at the corresponding moment T i , find several points closest to the current point in the current local point cloud map and record the closest point among the several points as q i , use the principal component analysis method to calculate the normal vector n corresponding to several points i Finally, the point-to-plane error corresponding to each point is calculated using the following formula:

[0017] Among them, e i Indicates the current point p i The corresponding point-to-plane error, T represents the transpose.

[0018] In S3, the formula for the trajectory smoothness constraint is as follows:

[0019] in, represents the trajectory smoothness constraint within the time window k, represents the generalized velocity in time window k, represents the pseudo velocity measurement of time window k; T k-1 and T k are the starting and ending poses of the current window k respectively; and They are the starting and ending poses after optimization within the previous time window k-1.

[0020] The objective function is formulated as follows:

[0021] Where E represents the total trajectory error, e i Represents the current point p in the current time window i The corresponding point-to-plane error, represents the trajectory smoothness constraint within the current time window k, and T represents the transpose.

[0022] 2. A computer device

[0023] The computer device includes a memory and a processor, wherein the memory stores a computer program, and the processor implements the steps of the method when executing the computer program.

[0024] 3. A computer-readable storage medium

[0025] A computer program is stored on the computer-readable storage medium, and when the computer program is executed by a processor, the steps of the method described above are implemented.

[0026] The beneficial effects of the present invention are:

[0027] This paper proposes a motion representation method for piecewise linear continuous-time trajectories. The motion within a time interval can be divided into multiple time intervals based on the motion situation. The motion within each time interval can be represented using simple linear interpolation. This segmented trajectory representation solves the problem of linear interpolation being unable to represent complex motion, thereby providing a concise and efficient mathematical representation for subsequent odometry estimation.

[0028] When completing the point cloud registration part of the lidar odometry, the present invention additionally considers the timestamp information of each point, thereby utilizing the continuous-time trajectory representation method proposed above, eliminating the additional point cloud dedistortion step and reducing dependence on other sensors.

[0029] In the odometer optimization part, the present invention introduces motion constraints based on the smoothness of the continuous trajectory, which can achieve stable registration when the system dynamics state exceeds the general IMU measurement range.

[0030] The continuous time representation adopted by the present invention provides a unified odometry framework that can be applied to non-scanning lidar sensors and can also simultaneously fuse multiple asynchronous lidar sensors. BRIEF DESCRIPTION OF THE DRAWINGS

[0031] FIG1 is an overall flow chart of a lidar odometer method using piecewise linear continuous-time trajectories according to an embodiment of the present invention.

[0032] FIG2 is a framework of a continuous-time trajectory lidar odometer method according to an embodiment of the present invention. DETAILED DESCRIPTION

[0033] The following will be combined with the accompanying drawings of the present invention to clearly and completely describe the technical solution of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without doing creative work are within the scope of protection of the present invention.

[0034] In order to make the objectives, technical solutions and advantages of the present invention more clearly understood, embodiments of the present invention will be described in detail below with reference to the accompanying drawings.

[0035] As shown in Figures 1 and 2, the present invention includes the following steps:

[0036] S1: After dividing the initial lidar point cloud time series into time windows, obtain lidar point cloud subsequences under multiple time windows and construct an initial local point cloud map;

[0037] Specifically: the initial lidar point cloud time series is represented as T(t), where t is the time variable and the time range of interest is [t0,t K ) within the trajectory. Each time point corresponds to a laser radar posture, denoted by Tt. All postures are expressed in the world coordinate system, where the world coordinate system is the starting position of the laser radar. The posture is specifically the translation and rotation of the three-dimensional rigid body transformation. In order to handle fast motion, the present invention divides the time window into K small segments with equal time intervals This divides the initial LiDAR point cloud time series into K point cloud subsets, each containing LiDAR point cloud data collected during that time interval. Initially, the initial local point cloud map is constructed using the point cloud data acquired in the first three seconds. The map structure uses a voxel hash table, where each point is stored in the corresponding voxel based on the hash value of its 3D coordinates.

[0038] S2: According to its initial pose T k-1 and the ending pose T k After linear parameterization of the lidar point cloud subsequence under each time window, the initial continuous time trajectory is obtained; in each segment [t k-1 ,k k ), the motion is determined by the initial pose T k-1 and the ending pose T k Linear parameterized trajectory, each point can linearly interpolate its position in the world coordinate system based on the timestamp information.

[0039] S3: Based on the current continuous-time trajectory and the current local point cloud map, the point-to-plane error corresponding to each point in the current time window k is calculated and formed into a point-to-plane error constraint, and the trajectory smoothness constraint is established;

[0040] In S3, for each point in the current time window k, the calculation steps of the point-to-plane error corresponding to each point are as follows:

[0041] First, assume that the kth time window [t k-1 , t k ) has a point p i , the timestamp corresponding to the collection time is t i . Use the current continuous time trajectory to get the current point p i The position of the lidar in the world coordinate system at the corresponding moment T i , position T i According to the starting time posture T corresponding to the current time window k-1 and the end time pose T k Obtained by linear interpolation, the calculation formula is as follows:

[0042] Among them, Log and Exp are the logarithmic mapping and exponential mapping of the three-dimensional space rigid transformation respectively, α i represents the time scale coefficient, and τ represents the tangent space vector corresponding to the pose transformation.

[0043] Then according to the position T i , find several points closest to the current point in the current local point cloud map, and record the closest point among the several points as q i , use the principal component analysis method to calculate the normal vector n corresponding to several points i Finally, the point-to-plane error corresponding to each point is calculated using the following formula:

[0044] Among them, e i Indicates the current point p i The corresponding point-to-plane error, T represents the transpose.

[0045] This error is used to measure the geometric relationship between the LiDAR measurement data and the map, and is therefore used for motion estimation. The point-surface error constraint takes into account the continuity of the continuous-time trajectory, allowing the present invention to more accurately estimate the motion of the LiDAR.

[0046] In S3, the formula for trajectory smoothness constraint is as follows:

[0047] in, represents the trajectory smoothness constraint within the time window k, represents the generalized velocity in time window k, represents the pseudo velocity measurement of time window k, which is composed of the pose estimate of the previous segment and is used to measure the trajectory change within the adjacent segment; T k-1 and T k are the starting and ending poses of the current window k, respectively, which will be updated during the optimization process; and They are the starting and ending poses after the optimization in the previous time window k-1, and remain unchanged in the optimization of the current window k.

[0048] S4: Based on the point-to-surface error constraints and trajectory smoothness constraints, the Gauss-Newton method is used to minimize the objective function, and the continuous-time trajectory within the current time window k is estimated and updated;

[0049] The objective function formula is as follows:

[0050] Where E represents the total trajectory error, e i Represents the current point p in the current time window i The corresponding point-to-plane error, represents the trajectory smoothness constraint within the current time window k, and T represents the transpose.

[0051] The objective function is minimized by iterative Gauss-Newton method to obtain the final pose estimation result. The present invention lists the explicit Jacobian matrix required in the optimization process to improve the optimization efficiency.

[0052] Among them, the plane error e i The corresponding Jacobian ratio matrix is:

[0053] Among them, J r and J l are the right Jacob ratio matrix and the left Jacob ratio matrix in three-dimensional space, R i Represents a rotation matrix.

[0054] Smooth Constraint The Jacobian ratio matrix is:

[0055] S5: Repeat S3 and S4, and perform iterative optimization of the pose in the current time window according to the updated continuous-time trajectory until the objective function is less than the preset threshold, and obtain the optimal continuous-time trajectory of the current time window and the corresponding lidar point cloud in the world coordinate system; and add the lidar point cloud in the world coordinate system of the current time window to the current local point cloud map;

[0056] S6: Repeat S2-S5, and optimize the lidar pose of the lidar point cloud subsequences under each time window in turn to obtain the total continuous time trajectory, that is, time t K Inner posture.

[0057] This method is applicable to fields such as robot navigation, simultaneous localization and mapping, and autonomous driving. It provides high-precision lidar motion estimation for robots, is applicable to various types of lidar and motion scenarios, and exhibits high robustness and real-time performance. It is widely applicable in fields such as robot navigation, simultaneous localization and mapping, and autonomous driving.

[0058] Finally, it should be noted that the above embodiments and explanations are intended only to illustrate the technical solutions of the present invention and are not intended to limit the present invention. It should be understood by those skilled in the art that modifications or equivalent substitutions to the technical solutions of the present invention may be made without departing from the spirit and scope of the technical solutions disclosed herein, and all such modifications or equivalent substitutions shall be encompassed within the scope of protection of the claims of the present invention.

Claims

1. A laser radar odometer method using piecewise linear continuous-time trajectories, characterized in that: The following steps are involved: S1: After dividing the initial lidar point cloud time series by time windows, obtain lidar point cloud subsequences under multiple time windows and construct an initial local point cloud map; S2: After linear parameterization of the lidar point cloud subsequence under each time window, the initial continuous time trajectory is obtained; S3: According to the current continuous time trajectory and the current local point cloud map, the point-to-plane error corresponding to each point in the current time window is calculated and the point-to-plane error constraint is formed, and the trajectory smoothness constraint is established; S4: Based on the point-surface error constraints and trajectory smoothness constraints, the Gauss-Newton method is used to minimize the objective function, and the continuous-time trajectory in the current time window is estimated and updated; S5: Repeat S3 and S4, perform iterative optimization of the pose in the current time window according to the updated continuous time trajectory, until the objective function is less than the preset threshold, obtain the optimal continuous time trajectory of the current time window and the corresponding lidar point cloud in the world coordinate system; and add the lidar point cloud in the world coordinate system of the current time window to the current local point cloud map; S6: Repeat S2-S5, optimize the lidar pose for the lidar point cloud subsequences in each time window in turn, and obtain the total continuous time trajectory.

2. A laser radar odometer method using piecewise linear continuous-time trajectory according to claim 1, characterized in that: In S3, for each point in the current time window, the step of calculating the point-to-plane error corresponding to each point is specifically as follows: First, use the current continuous time trajectory to obtain the current point p i The position of the laser radar in the world coordinate system at the corresponding time T i , find several points closest to the current point in the current local point cloud map and record the closest point among the several points as q i , use the principal component analysis method to calculate the normal vector n corresponding to several points i Finally, the point-to-plane error corresponding to each point is calculated using the following formula: Among them, e i Indicates the current point p i The corresponding point-to-plane error, T represents the transpose.

3. A laser radar odometer method using piecewise linear continuous-time trajectory according to claim 1, characterized in that: In S3, the formula for the trajectory smoothness constraint is as follows: in, represents the trajectory smoothness constraint within the time window k, represents the generalized velocity in time window k, represents the pseudo velocity measurement in time window k; T k-1 and T k point are the starting and ending poses of the current window k; and They are respectively the starting and ending poses after optimization within the previous time window k-1.

4. A laser radar odometer method using piecewise linear continuous-time trajectory according to claim 1, characterized in that: The objective function formula is as follows: Where E represents the total trajectory error, e i Represents the current point p in the current time window i The corresponding point-to-plane error, represents the trajectory smoothness constraint within the current time window k, and T represents the transpose.

5. A computer device comprising a memory and a processor, wherein the memory stores a computer program, wherein: When the processor executes the computer program, the steps of the method according to any one of claims 1 to 4 are implemented.

6. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the steps of the method according to any one of claims 1 to 4 are implemented.

Images