A method for acquiring robot laser odometry based on dynamic target tracking

Abstract

This invention relates to a method for acquiring robot laser odometry based on dynamic target tracking. The method includes preprocessing all laser points in each frame of the lidar point cloud acquired by the lidar system; using a point cloud segmentation network to detect potential dynamic targets in the preprocessed point cloud; performing state estimation on the detected potential dynamic targets using a Kalman filter to obtain the optimal estimate of the centroid state of the potential dynamic target at time t, and calculating the geometric and intensity information of the potential dynamic target; classifying the potential dynamic targets into static and dynamic targets based on their calculated velocity; and updating the obtained pose estimation based on a sliding window and keyframes to obtain the latest laser odometry. This invention introduces the inherent attribute intensity information of the object for joint association based on the geometric features of the target point cloud, reducing target mismatches, ensuring the accuracy of multi-target tracking, and facilitating subsequent pose and pose transformation calculations.

Description

Technical Field

[0001] This invention relates to the field of laser SLAM technology, and in particular to a pose estimation method for a front-end laser odometer in dynamic environments. Background Technology

[0002] Simultaneous Localization and Mapping (SLAM) is the foundation of AI-powered robot navigation services, and highly sensitive environmental perception directly impacts robot navigation performance. Addressing the limitations of single-sensor environmental information acquisition, the fusion of LiDAR SLAM and Inertial Measurement Unit (IMU) has been widely studied due to its insensitivity to changes in lighting and low texture in the environment. However, most current research assumes static or slow-moving scenes. Achieving stable environmental perception and high-precision mapping remains a challenge in the complexities of real-world applications.

[0003] Currently, SLAM technology in dynamic scenarios has the following problems:

[0004] (1) In traditional SLAM methods, the laser odometry module ignores the influence of dynamic targets in the environment on the robot's own pose estimation, or the tracking method for dynamic targets is not accurate enough, which causes most existing SLAM methods to work well in static scenes, but fail in dynamic scenes with moving obstacles.

[0005] (2) Traditional feature matching methods in laser odometry only utilize the geometric distance of point clouds and ignore the geometric distribution information of point clouds. Summary of the Invention

[0006] In view of the above-mentioned problems in the existing technology, the technical problem to be solved by the present invention is: how to solve the influence of dynamic targets on robot localization and how to calculate more accurate laser odometry estimation results.

[0007] To solve the above-mentioned technical problems, the present invention adopts the following technical solution: a method for acquiring robot laser odometry based on dynamic target tracking, comprising the following steps:

[0008] S1: Preprocess all laser points in each frame of the lidar point cloud acquired by the lidar, and use an FCNN-based point cloud segmentation network to detect potential dynamic targets in the preprocessed point cloud.

[0009] S2: Use the Kalman filter method to estimate the state of the detected potential dynamic target, and obtain the optimal estimate of the centroid state of the potential dynamic target at time t. in, Represents the planar coordinates of the potential dynamic target at time t. This represents the velocity of the potential dynamic target at time t.

[0010] S3: Calculate the geometric and intensity information of potential dynamic targets.

[0011] S4: Integrate the geometric and intensity information of potential dynamic targets, and use the correlation method to detect all potential dynamic targets. Based on the velocity of the potential dynamic targets calculated in S2, if the velocity of the potential dynamic target at time t is 0, then the potential dynamic target is determined to be a static target; otherwise, it is a dynamic target.

[0012] S5: Extract feature points from static targets using curvature.

[0013] S6: Assign different weights to feature points based on curvature, then construct a pose constraint function based on the weighting function and solve it to obtain the pose estimate of the lidar.

[0014] S7: Based on the sliding window, keyframes, and pose estimation obtained in S6, the laser points corresponding to the static target are updated with distortion compensation to obtain the latest laser odometry, thus obtaining the pose transformation relationship ΔT between the current frame and the previous frame. k,k+1 And the latest coordinates of the laser point corresponding to the updated static target.

[0015] As an improvement, the preprocessing step in S1 for all laser points in the point cloud acquired by the lidar is as follows:

[0016] Let P be the laser point cloud acquired in the k-th scan. k This indicates that the transformation relationship between the lidar coordinate system and the IMU coordinate system is utilized. This allows the laser point cloud to be mapped onto the IMU coordinate system:

[0017]

[0018] In the formula p I With p L These represent the laser point in the IMU coordinate system and the laser point in the radar coordinate system, respectively.

[0019] θ e θ represents the angle between the end scan line and the start line. n The angle of the current scan line relative to the starting line is represented as: The time at the current point is expressed as:

[0020]

[0021] Among them, t s represents the start scanning time of the lidar, and f represents the lidar rotation frequency.

[0022] After serializing the unordered point cloud, the time sequence is used to find the point cloud with respect to time t. k The two most recent frames of IMU data are used for pre-integration calculation, and then the calculation results are projected onto the world coordinate system W to obtain t. k IMU posture under W at all times Similarly, we can obtain t k+1 Moment IMU Attitude Based on this, t can be obtained. k+1 time relative to t k Position at any given moment:

[0023]

[0024] Finally passed Perform transformation calculations to obtain the result at t n Motion compensation transformation matrix of each laser point at any given time

[0025]

[0026] To achieve motion distortion compensation, the compensated laser points are obtained.

[0027]

[0028] As an improvement, the process of detecting potential dynamic targets using the FCNN-based point cloud segmentation network in S1 is as follows:

[0029] S1.1: Dimensionality reduction is performed on the preprocessed point cloud. Then, the 3D space of the preprocessed point cloud is divided into regions according to the bird's-eye view projection and its 2D index is established. Then, the feature information of the point pillars in each grid in the projection map is extracted. This feature information is used to generate the input of the FCNN network, i.e., the two-dimensional feature map.

[0030] The FCNN network consists of a feature encoder, a feature decoder, and an attribute predictor.

[0031] S1.2: The feature encoder extracts semantic features from the two-dimensional feature map through the convolution of the convolutional layer, and continuously downsamples the spatial resolution of the two-dimensional feature map, finally outputting a feature vector. The convolutional layer includes a trained VGG network and a ResNet network.

[0032] S1.3: The feature vector is input into the feature decoder, and an upsampling layer is used to obtain a prediction map of the same size as the two-dimensional feature map.

[0033] S1.4: The attribute predictor calculates four attribute values ​​for each grid in the prediction graph, namely, the center offset value, the center offset direction value, the attribute probability value, and the object height value.

[0034] S1.5: Construct a bias pointer for each grid based on the center offset value and center offset direction of each grid, and the cell pointed to by each of the multiple pointers is used as a candidate classification object.

[0035] For candidate classification objects, if the attribute probability value of the classification object is not less than the probability threshold, it is considered a reliable clustering object;

[0036] The reliable clustered objects selected are the potential dynamic targets.

[0037] As an improvement, the calculation process for the geometric and intensity information of the potential dynamic target in S3 is as follows:

[0038] S3.1: Calculate the geometric information of the potential dynamic target, i.e., calculate the centroid position of the potential dynamic target. Use the coordinates of the laser point corresponding to the potential dynamic target to calculate the centroid position of the potential dynamic target. As shown in formula (6):

[0039]

[0040] In the formula, n represents the number of laser points corresponding to the potential dynamic target, and x i ,y i ,z i This represents the coordinates of the i-th laser point corresponding to the dynamic target. The coordinates of the centroid of the dynamic target.

[0041] S3.2: Convert the raw laser point data output by the lidar into decimal representation and perform normalization to obtain the intensity information of each laser point.

[0042] S3.3: Calibrate the intensity information of the laser point using a mapping function:

[0043]

[0044] Where d represents the distance information of the original laser point, and η k η represents the intensity information of the k-th original laser point. k ′ represents the intensity information of the k-th laser point after calibration.

[0045] As an improvement, the process of detecting all potential dynamic targets using the association method in S4 is as follows:

[0046] S4.1: Calculate the surface reflectance coefficient ρ′ of the laser point corresponding to the potential dynamic target:

[0047]

[0048] Where M is a constant and α is the laser incident angle.

[0049] S4.2: Determine whether the potential dynamic targets in different frames are the same target. If they satisfy formula (9), then the potential dynamic targets in different frames are considered to be the same target; otherwise, they are not.

[0050]

[0051] In the formula d thr and ρ thr These represent the set distance threshold and intensity threshold, respectively. This represents the centroid positions of the potential dynamic target a at time t and time t-1. The value of a represents the average intensity at time t and time t-1. The potential dynamic targets in the image at time t are associated with the potential dynamic targets in the image at time t-1 by formula (9), and the potential dynamic targets occluded by obstacles are associated, thereby detecting all potential dynamic targets.

[0052] As an improvement, the process of extracting feature points from static targets using curvature in S5 is as follows:

[0053] S5.1: A lidar scan has M parallel readings on a vertical plane, with N points on each parallel line. Let P represent all the laser points corresponding to all static targets in the k-th lidar scan. k ′, representing each laser point as Where m∈[1,M], n∈[1,N], x k ,y k ,z k Let be the three-dimensional coordinates of the k-th laser point.

[0054] Will The continuous point set of the wire bundle in the horizontal direction is used This indicates that for The plane formed by adjacent points, curvature The calculation formula is:

[0055]

[0056] In the formula N s express The number of laser points in the middle, express The j-th laser point in the m-th row.

[0057] Then, based on the value of c, All laser points are classified, with those having a c value greater than a set threshold being classified as corner points or edge points, and those having a c value less than or equal to the set threshold being classified as planar feature points.

[0058] S5.2: The point is set by the threshold σ. Marked as candidate edge points and candidate planar feature points. The data acquired from each scan line... The region is divided into four sub-regions. Each sub-region can extract at most two edge feature points and four planar feature points. The set of all edge feature points in the four sub-regions is denoted as ε. k Let s be the set of all planar feature points in the four sub-regions. k .

[0059] As an improvement, the process of obtaining the lidar pose estimate in S6 is as follows:

[0060] S6.1: Represent all laser points corresponding to all static targets in the k-th lidar scan as P. k ′, P k A laser point in ' is represented as p' k ={x k ,y k ,z k ,1} T Let the pose of the lidar in the k-th frame be represented by T. k The 6-DOF pose transformation between two consecutive frames k-1 and k is:

[0061]

[0062] in, yes The Lie algebra representation. The function log(·) is the mapping from the homogeneous transformation matrix to the Lie algebra, from the k-th frame interval to the short time interval δ. t The pose transformation within is represented as:

[0063]

[0064]

[0065] In the formula This represents the position of all laser points corresponding to the static target in the current k-th frame, reprojected to the position at the beginning of the current frame.

[0066] Assume ε k Mid-edge feature point p ε The local smoothness is c ε s k Mid-plane feature point p s The local smoothness is c s Their weights are defined as shown in formula (14):

[0067]

[0068] Among them, W(p) ε ), W(p s ) represent p respectively ε and p s The weight, Represents ε k The j-th edge feature point in the i-th row The curvature.

[0069] S6.2: Calculate p for each edge feature point ε The covariance matrix of neighboring points in the global edge feature set, where the global edge feature set is the set of all ε-values ​​contained in a single frame of an image. k If the covariance matrix of a set contains the largest eigenvalue, then p ε In global edge features, the nearest points are distributed along a straight line, and the eigenvector corresponding to this largest eigenvalue is... Consider the direction of a straight line, the geometric center of the nearest point Consider the position as a straight line, at which point the edge feature point p... ε The distance to the global edge feature is:

[0070]

[0071] In the formula p n It is a unit vector;

[0072]

[0073] S6.3: Calculate each planar feature point p S The covariance matrix of the model with its neighbors in the global plane feature set. The global plane feature set is the set of all the s in a frame of an image. k If a set of neighboring points has a covariance matrix containing a minimum eigenvalue, it means that these neighboring points are distributed on a plane, and the eigenvector corresponding to this minimum eigenvalue is... Consider the direction of the surface normal and the geometric center of the nearest point. This can be viewed as the position of a plane, where the feature point p of the plane is... S The distance to the global plane feature set is expressed as:

[0074]

[0075] S6.4: The optimization function is defined as follows:

[0076]

[0077] S6.5: Solve the nonlinear equation of formula (18) using the Gauss-Newton method to obtain the optimal attitude estimate, and then use the model with left perturbation to estimate the Jacobian matrix J. p :

[0078]

[0079] Among them, [Tp] × That is, [T] k p k ] × δ and ξ represent short time intervals, respectively. t 6-DOF pose transformation between two consecutive frames T k p k Let J represent the Jacobian matrix J of the marginal residuals, obtained by combining a 4x4 matrix and a 4x1 vector. ε Calculated using the following formula:

[0080]

[0081] Jacobian matrix J of planar residuals s :

[0082]

[0083] The iteration increment ΔT is represented by the Jacobian matrix. k Using ΔT k Update the laser odometry estimation results and iteratively optimize the attitude until ΔT is reached. k Once convergence is achieved, the pose estimate T of the current lidar can be calculated. k .

[0084] As an improvement, the process of obtaining the latest laser odometry output in S7 is as follows:

[0085] S7.1: Keyframes are selected by setting a pose transformation threshold. That is, when the pose transformation of the robot between time k and time k+1 exceeds the pose transformation threshold, the current frame is selected as the keyframe F. k+1 And add a sliding window, this F k+1 With p k The poses are matched; then, the i closest keyframes are extracted temporally, and these keyframes are represented as a subset of keyframes {F}. k-i ,...,F k}, through the transformation matrix {T' between W and W k-i ,...,T' k Projecting the sub-keyframe set onto W yields the projection map M. k .

[0086] Mk Projection map of edge feature point cloud and planar feature point cloud projection map Composition. Each keyframe and its corresponding projection map M k They are interconnected, and the relationships between them are defined as follows:

[0087]

[0088] in, These represent the edge feature points of the k-th and ki-th keyframes in the sub-keyframe set, respectively. These represent the planar feature points of the k-th and ki-th keyframes in the sub-keyframe set, respectively.

[0089] S7.2: Transfer the latest k+1 keyframes to keyframe F. k+1 Transform to W and match the projection image M k Correlate and obtain the incremental pose transformation ΔT of the (k+1)th frame. k,k+1 :

[0090]

[0091] in, T represents k The inverse matrix, T k+1 This represents the pose estimation of the lidar in frame k+1.

[0092] The distortion was recalculated using a 6-DOF pose transformation, and the following results were obtained:

[0093]

[0094]

[0095] In the formula It is ΔT k,k+1 The Lie algebra representation is given by equation (19). The optimal positioning estimate after the lidar update, ΔT, can be calculated using this equation. k,k+1 This indicates the pose transformation relationship between two adjacent keyframes. This represents the latest coordinates of the laser point corresponding to the updated static target.

[0096] Compared with the prior art, the present invention has at least the following advantages:

[0097] This invention integrates dynamic target detection and tracking methods into the traditional laser odometry framework. It considers the impact of dynamic targets on robot localization and environmental mapping (since the lidar is fixed on the robot, the robot's odometry estimate can be determined by the lidar's odometry estimate). In the dynamic target tracking part, this invention introduces the inherent attribute intensity information of the object to jointly correlate with the geometric features of the target point cloud, reducing false matching of targets, ensuring the accuracy of multi-target tracking, and making it more conducive to the calculation of pose and pose transformation in subsequent laser odometry.

[0098] In the pose calculation part, compared with the traditional feature matching method, this invention only performs feature point extraction and feature matching on the segmented static point cloud. On the other hand, it simultaneously utilizes the geometric distance information and local geometric distribution of the point cloud for feature matching, and further updates the distortion compensation of the feature-matched point cloud based on the sliding window and keyframes, thereby improving the accuracy of the pose transformation matrix and robot pose obtained in the point cloud registration process. Attached Figure Description

[0099] Figure 1 This is a flowchart illustrating a method for acquiring robot laser odometry based on dynamic target tracking.

[0100] Figure 2 This is a framework diagram of a segmentation network model based on FCNN.

[0101] Figure 3 This is a framework diagram of a multi-target tracking method.

[0102] Figure 4 This is a schematic diagram illustrating the state estimation effect based on KF.

[0103] Figure 5 This is a single-frame target detection result image.

[0104] Figure 6 This is a diagram showing the effect of multi-target tracking. Detailed Implementation

[0105] The present invention will now be described in further detail.

[0106] A method for acquiring robot laser odometry based on dynamic target tracking includes the following steps:

[0107] S1: Preprocess all laser points in each frame of the lidar point cloud acquired by the lidar, and use an FCNN-based point cloud segmentation network to detect potential dynamic targets in the preprocessed point cloud.

[0108] S2: Use the Kalman filter method to estimate the state of the detected potential dynamic target, and obtain the optimal estimate of the centroid state of the potential dynamic target at time t. in, Represents the planar coordinates of the potential dynamic target at time t. This represents the velocity of the potential dynamic target at time t;

[0109] S3: Calculate the geometric and intensity information of potential dynamic targets;

[0110] S4: Integrate the geometric and intensity information of potential dynamic targets, and use an association method to detect all potential dynamic targets. Based on the velocity of the potential dynamic targets calculated in S2, if the velocity of the potential dynamic target at time t is 0, then the potential dynamic target is determined to be a static target; otherwise, it is a dynamic target.

[0111] S5: Extract feature points from static targets using curvature;

[0112] S6: Assign different weights to feature points based on curvature, then construct a pose constraint function based on the weighting function and solve it to obtain the pose estimate of the static target;

[0113] S7: Based on the sliding window, keyframes, and pose estimation obtained in S6, the laser points corresponding to the static target are updated with distortion compensation to obtain the latest laser odometry, which yields the pose transformation relationship ΔT between adjacent keyframes. k,k+1 And the latest coordinates of the laser points corresponding to the updated static targets. The pose transformation of the LiDAR in two adjacent frames can be obtained from the pose transformation of the static target point clouds in two adjacent frames. By using the pose transformation relationship of the current frame relative to the previous frame and the pose of the LiDAR in the previous frame, the pose estimate of the LiDAR in the current frame can be calculated. The initial pose of the LiDAR is obtained by IMU pre-integration. Then, the pose transformation of the LiDAR in two adjacent frames can be obtained from the pose transformation of the static target point clouds in two adjacent frames. By using the pose transformation relationship of the current frame relative to the previous frame and the pose of the LiDAR in the previous frame, the pose estimate of the LiDAR in the current frame can be calculated.

[0114] Specifically, the preprocessing steps in S1 for all laser points in the point cloud acquired by the lidar are as follows:

[0115] When a linear mechanical lidar rotates to acquire 3D environmental information, it operates at a frequency of 10 Hz, meaning a single scan takes only 0.1 seconds. This means that during robot movement, due to rotational or translational motion, all points within a single lidar point cloud frame may not be acquired from the same location. Furthermore, as the platform moves forward, the coordinate systems of the point cloud acquired in the same frame may differ. To compensate for this motion distortion, data compensation is necessary to eliminate the distortion in the lidar point cloud; that is, the point cloud of a particular frame is reprojected onto the same coordinate system at that moment.

[0116] Let P be the laser point cloud acquired in the k-th scan. kThis means that point cloud distortion compensation is to adjust the P... k All laser points in the middle are reprojected onto t k In the radar coordinate system at the end of the time step, the transformation relationship between the lidar coordinate system and the IMU coordinate system is utilized. This allows the laser point cloud to be mapped onto the IMU coordinate system:

[0117]

[0118] In the formula p I With p L These represent the laser point in the IMU coordinate system and the laser point in the radar coordinate system, respectively.

[0119] Since the point cloud acquired by lidar only contains geometric and intensity information, and not the acquisition sequence, it needs to be serialized before interpolation. LiDAR rotates at a constant angular velocity, and based on the time sequence of scanning one revolution of the point cloud, the point cloud can be numbered according to the angular difference, θ. e θ represents the angle between the end scan line and the start line. n The angle of the current scan line relative to the starting line is represented as: The time at the current point is expressed as:

[0120]

[0121] Among them, t s represents the start scanning time of the lidar, and f represents the lidar rotation frequency.

[0122] After serializing the unordered point cloud, the time sequence is used to find the point cloud with respect to time t. k The two most recent frames of IMU data are used for pre-integration calculation, and then the calculation results are projected onto the world coordinate system W to obtain t. k IMU posture under W at all times Similarly, we can obtain t k+1 Moment IMU Attitude Based on this, t can be obtained. k+1 time relative to t k Position at any given moment:

[0123]

[0124] Finally passed Perform transformation calculations to obtain the result at t n Motion compensation transformation matrix of each laser point at any given time

[0125]

[0126] To achieve motion distortion compensation, the compensated laser points are obtained.

[0127]

[0128] Specifically, the process of detecting potential dynamic targets using the FCNN-based point cloud segmentation network in S1 is as follows:

[0129] S1.1: Dimensionality reduction is performed on the preprocessed point cloud. Then, the 3D space of the preprocessed point cloud is divided into regions according to the bird's-eye view projection and its 2D index is established. Then, the feature information of the point pillars in each grid in the projection map is extracted. This feature information is used to generate the input of the FCNN network, i.e., the two-dimensional feature map.

[0130] Specifically, eight feature information points are extracted from the points within each grid cell of the projection map. These eight features include the number of laser points in the cell, average intensity, average height, intensity of the highest point, maximum height, angular deviation and distance between the cell center and the origin, and a binary representation of whether the cell is occupied. These features are used as input to generate the FCNN network, i.e., a two-dimensional feature map. The bird's-eye view projection point cloud data is segmented using a two-dimensional grid, with each laser point falling into a grid cell, thus transforming it into an ordered two-dimensional matrix. A grid cell may contain several or zero laser points.

[0131] The FCNN network includes a feature encoder, a feature decoder, and an attribute predictor;

[0132] S1.2: The feature encoder extracts semantic features from the two-dimensional feature map through the convolution of the convolutional layer, and continuously downsamples the spatial resolution of the two-dimensional feature map, and finally outputs a feature vector. The convolutional layer includes a trained VGG network and a ResNet network.

[0133] S1.3: The feature vector is input into the feature decoder, and an upsampling is performed using a deconvolution layer to obtain a prediction map of the same size as the two-dimensional feature map;

[0134] S1.4: The attribute predictor calculates four attribute values ​​for each grid in the prediction map, namely, the center offset value, the center offset direction value, the attribute probability value, and the object height value;

[0135] S1.5: Construct a bias pointer for each grid based on the center offset value and center offset direction of each grid, and the cell pointed to by each of the multiple pointers is used as a candidate classification object.

[0136] For candidate classification objects, if the attribute probability value of the classification object is not less than the probability threshold, it is considered a reliable clustering object;

[0137] The reliable clustered objects selected are the potential dynamic targets.

[0138] Specifically, the calculation process for the geometric and intensity information of the potential dynamic target in S3 is as follows:

[0139] S3.1: Calculate the geometric information of the potential dynamic target, i.e., calculate the centroid position of the potential dynamic target. Use the coordinates of the laser point corresponding to the potential dynamic target to calculate the centroid position of the potential dynamic target. As shown in formula (6):

[0140]

[0141] In the formula, n represents the number of laser points corresponding to the potential dynamic target, and x i ,y i ,z i This represents the coordinates of the i-th laser point corresponding to the dynamic target. The coordinates representing the centroid of the dynamic target;

[0142] S3.2: Convert the raw laser point data output by the lidar into decimal representation and perform normalization to obtain the intensity information of each laser point.

[0143] S3.3: The intensity channel of the lidar is noisy due to target surface characteristics such as roughness, surface reflectivity, and acquisition geometry. Therefore, to reduce interference from these factors, the intensity information of the laser point is calibrated using a mapping function:

[0144]

[0145] Where d represents the distance information of the original laser point, and η k η represents the intensity information of the k-th original laser point. k ′ represents the intensity information of the k-th laser point after calibration.

[0146] Specifically, the process of detecting all potential dynamic targets using the association method in S4 is as follows:

[0147] S4.1: Calculate the surface reflectance coefficient ρ′ of the laser point corresponding to the potential dynamic target:

[0148]

[0149] Where M is a constant, which is related to the lidar's emission power, optical transmission system, atmospheric transmission coefficient, etc., and α is the laser incident angle;

[0150] S4.2: Determine whether the potential dynamic targets in different frames are the same target. If they satisfy formula (9), then the potential dynamic targets in different frames are considered to be the same target; otherwise, they are not.

[0151]

[0152] In the formula d thr and ρ thr These represent the set distance threshold and intensity threshold, respectively. This represents the centroid positions of the potential dynamic target a at time t and time t-1. The average intensity of a at time t and time t-1 is given by ρ′. The reflectivity ρ of the surface of the potential dynamic target can be obtained by averaging the reflectivity ρ′ of each laser point corresponding to the potential dynamic target.

[0153] By using formula (9), the potential dynamic targets in the image at time t are associated with the potential dynamic targets in the image at time t-1, and the potential dynamic targets that are occluded by obstacles are associated, thereby detecting all potential dynamic targets.

[0154] After the association method is completed, the target in the current frame can be associated with the target in the previous frame, such as... Figure 3 As shown, the multi-target tracking method combines the correlation results and the Kalman filter to predict the target's state to obtain the tracking results. That is, the tracking results include the target's motion trajectory and state such as position and velocity. Then, the target is divided into static targets and dynamic targets based on whether the target's velocity is 0.

[0155] Specifically, the process of extracting feature points from a static target using curvature in step S5 is as follows:

[0156] S5.1: A lidar scan has M parallel readings on a vertical plane, with N points on each parallel line. Let P represent all the laser points corresponding to all static targets in the k-th lidar scan. k ′, representing each laser point as Where m∈[1,M], n∈[1,N], x k ,y k ,z k Let K be the three-dimensional coordinates of the k-th laser point;

[0157] Will The continuous point set of the wire bundle in the horizontal direction is used This indicates that for The plane formed by adjacent points, curvature The calculation formula is:

[0158]

[0159] In the formula N s express The number of laser points in the middle, express The j-th laser point in the m-th row.

[0160] Then, based on the value of c, All laser points are classified, with those having a c value greater than a set threshold being classified as corner points or edge points, and those having a c value less than or equal to the set threshold being classified as planar feature points.

[0161] S5.2: The point is set by the threshold σ. These are marked as candidate edge points and candidate planar feature points. To obtain uniformly distributed feature points, the data acquired from each scan line are... The region is divided into four sub-regions. Each sub-region can extract at most two edge feature points and four planar feature points. The set of all edge feature points in the four sub-regions is denoted as ε. k Let s be the set of all planar feature points in the four sub-regions. k .

[0162] Specifically, the process of obtaining the static target pose estimate in S6 is as follows:

[0163] S6.1: Represent all laser points corresponding to all static targets in the k-th lidar scan as P. k ′, P k A laser point in ' is represented as p' k ={x k ,y k ,z k ,1} T Let the pose of the lidar in the k-th frame be represented by T. k The 6-DOF pose transformation between two consecutive frames k-1 and k is:

[0164]

[0165] in, yes The Lie algebra representation. The function log(·) is the mapping from the homogeneous transformation matrix to the Lie algebra, from the k-th frame interval to the short time interval δ. t The pose transformation within is represented as:

[0166]

[0167]

[0168] In the formula This represents the position of all laser points corresponding to the static target in the current k-th frame, reprojected to the position at the beginning of the current frame.

[0169] Assume ε k Mid-edge feature point p ε The local smoothness is c ε s kMid-plane feature point p s The local smoothness is c s Their weights are defined as shown in formula (14):

[0170]

[0171] Among them, W(p) ε ), W(p s ) represent p respectively ε and p s The weight, Represents ε k The j-th edge feature point in the i-th row The curvature.

[0172] S6.2: Calculate p for each edge feature point ε The covariance matrix of neighboring points in the global edge feature set, where the global edge feature set is the set of all ε-values ​​contained in a single frame of an image. k If the covariance matrix of a set contains the largest eigenvalue, then p ε In global edge features, the nearest points are distributed along a straight line, and the eigenvector corresponding to this largest eigenvalue is... Consider the direction of a straight line, the geometric center of the nearest point Consider the position as a straight line, at which point the edge feature point p... ε The distance to the global edge feature is:

[0173]

[0174] In the formula, the symbol · represents the dot product, p n It is a unit vector;

[0175]

[0176] S6.3: Calculate each planar feature point p S The covariance matrix of the model with its neighbors in the global plane feature set. The global plane feature set is the set of all the s in a frame of an image. k If a set of neighboring points has a covariance matrix containing a minimum eigenvalue, it means that these neighboring points are distributed on a plane, and the eigenvector corresponding to this minimum eigenvalue is... Consider the direction of the surface normal and the geometric center of the nearest point. This can be viewed as the position of a plane, where the feature point p of the plane is... S The distance to the global plane feature set is expressed as:

[0177]

[0178] S6.4: The optimization function is defined as follows:

[0179]

[0180] S6.5: Solve the nonlinear equation of formula (12) using the Gauss-Newton method to obtain the optimal attitude estimate, and then use the model with left perturbation to estimate the Jacobian matrix J. p :

[0181]

[0182] Among them, [Tp] × That is, [T] k p k ] × This represents converting the 4D point expression {x,y,z,1} into the 3D point expression {x,y,z} and calculating its skew-symmetric matrix. δ and ξ represent short-time values, respectively. t 6-DOF pose transformation between two consecutive frames T k p k Let J represent the Jacobian matrix J of the marginal residuals, obtained by combining a 4x4 matrix and a 4x1 vector. ε Calculated using the following formula:

[0183]

[0184] Jacobian matrix J of planar residuals s :

[0185]

[0186] The iteration increment ΔT is represented by the Jacobian matrix. k Using ΔT k Update the laser odometry estimation results and iteratively optimize the attitude until ΔT is reached. k Once convergence is achieved, the pose estimate T of the current lidar can be calculated. k .

[0187] Specifically, the process of obtaining the latest laser odometry output in S7 is as follows:

[0188] S7.1: Keyframes are selected by setting a pose transformation threshold. The translation and rotation thresholds are set to 1m and 10°, respectively. That is, when the pose transformation of the robot between time k and time k+1 exceeds the pose transformation threshold, the current frame will be selected as the keyframe F. k+1 And add a sliding window, this F k+1 With p kThe poses are matched; to store a fixed number of projection maps of the nearest frame laser point cloud, a stack is first maintained as a sliding window. Then, the i nearest keyframes are extracted in time and represented as a subset of keyframes {F}. k-i ,...,F k}, through the transformation matrix {T' between W and W k-i ,...,T' k Projecting the sub-keyframe set onto W yields the projection map M. k .

[0189] M k Projection map of edge feature point cloud and planar feature point cloud projection map Composition. Each keyframe and its corresponding projection map M k They are interconnected, and the relationships between them are defined as follows:

[0190]

[0191] in, These represent the edge feature points of the k-th and ki-th keyframes in the sub-keyframe set, respectively. These represent the planar feature points of the k-th and ki-th keyframes in the sub-keyframe set, respectively.

[0192] S7.2: Transfer the latest k+1 keyframes to keyframe F. k+1 Transform to W and match the projection image M k Correlate and obtain the incremental pose transformation ΔT of the (k+1)th frame. k,k+1 :

[0193]

[0194] in, T represents k The inverse matrix, T k+1 This represents the pose estimation of the lidar in frame k+1.

[0195] The distortion was recalculated using a 6-DOF pose transformation, and the following results were obtained:

[0196]

[0197]

[0198] In the formula It is ΔT k,k+1 The Lie algebra representation is given by equation (19). The optimal positioning estimate after the lidar update, ΔT, can be calculated using this equation. k,k+1 This indicates the pose transformation relationship between two adjacent keyframes. This represents the latest coordinates of the laser point corresponding to the updated static target;

[0199] All laser points corresponding to the static target in the (k+1)th lidar scan are represented by P. k ′ +1 P k ′ +1 A laser point is represented as p k+1 , For the 6-DOF pose transformation between two consecutive frames k and k+1, based on ΔT of the k+1 frame... k,k+1 Calculations show that This represents the latest coordinates of the laser point corresponding to the updated static target.

[0200] Output The relationship between pose transformation ΔT between two adjacent keyframes k,k+1 .

[0201] Experimental verification:

[0202] The classic LOAM method based on LiDAR SLAM framework was used as the baseline model framework, and the proposed method was added to the front-end odometry part of the LOAM framework. The following sets of comparative experiments were conducted: the first set is a comparative experiment between the proposed method and other target tracking methods based on deep learning, and the second set is a comparative experiment between front-end odometry methods in different LiDAR SLAM frameworks.

[0203] 1. Experimental Dataset

[0204] To train the FCNN-based point cloud segmentation network, this invention uses a subset of the KITTI dataset's object set. This object set includes 7481 training point cloud frames, 7518 test point cloud frames, and 80256 labeled objects, and is also used in the object tracking experiments. The comparative experiments for front-end odometry use the RawData dataset from the KITTI dataset. This dataset contains 22 sets of data, with the first 11 sequences (0-10) containing ground truth trajectories, and the remaining 11 sequences (11-21) without ground truth trajectories. Detailed information for each sequence is shown in Table 1.

[0205] Table 1. Information on each sequence in the KITTI dataset.

[0206]

[0207] 2. Evaluation Criteria

[0208] Key metrics for evaluating the performance of multi-object tracking methods include: the percentage of most tracked trajectories (MT), the percentage of most lost trajectories (ML), tracking accuracy (MOTA), and tracking recall (MOTP). MT refers to the percentage of ground truth tracks that match a trajectory more than 80% of the time. ML refers to the percentage of ground truth tracks that match a trajectory less than 20% of the time. MOTA provides an intuitive measure of the tracker's performance in object detection (TN, FP) and trajectory preservation (IDSW).

[0209] The relevant metrics for evaluating the performance of laser odometry methods include the absolute pose error (ATE) and relative pose error (RPE) of the output trajectory. The absolute pose error (ATE) is obtained by calculating the error between the trajectory output by the SLAM method and the actual trajectory, and it can evaluate the accuracy of the method and the consistency of the trajectory. Let the method output pose at time i be p. i At this point, the ground truth value of the pose is q. i And if the transformation matrix from the output pose to the true pose is T, then the absolute pose difference E at time i is... i for:

[0210]

[0211] E is usually used. i The root mean square error (RMSE) is used as an ATE metric to evaluate global positioning consistency. The RMSE of the global trajectory can be calculated based on the absolute pose error at each time step.

[0212]

[0213] In the formula, m represents the pose number in the trajectory, and trans(E) i ) represents the translation of the absolute error.

[0214] Relative pose error (RPE) describes the error of odometry by calculating the pose difference between two frames at equal time intervals. It helps us understand the pose changes between adjacent frames and reflects the accuracy and robustness of the local pose estimation method within a global trajectory. Similarly, root mean square error (RMSE) is typically used as the RPE metric. First, the relative pose error at each time point is calculated. Assuming the equal time interval is Δ, the pose difference F at time i is... i for:

[0215]

[0216] The relative pose difference at each time step is taken as RMSE:

[0217]

[0218] Where n represents the number of poses in the trajectory, trans(F i ) represents the translation portion of the relative error.

[0219] 3. Experimental Setup

[0220] The accuracy of the proposed method is verified by comparing it with two cutting-edge deep learning-based multi-object tracking methods, TrackR-CNN and MOTSFusion, on the object3D subset of the KITTI dataset. Simultaneously, the laser odometry method is experimentally validated using the 00-08 sequence of the dataset, and compared with two existing LiDAR SLAM methods, LOAM and LeGO-LOAM. LOAM is a classic pure LiDAR SLAM framework that obtains pose estimation between adjacent frames through edge and planar feature matching. LeGO-LOAM adds point cloud segmentation and denoising to LOAM and incorporates IMU data, representing a cutting-edge method with loose coupling between LiDAR and IMU. Neither of these methods considers the impact of dynamic targets and important feature points on localization and mapping.

[0221] 4. Analysis of Experimental Results

[0222] Figure 5 The experimental results of target detection show that even in complex scenes with many targets, the method used in this invention can still detect all moving targets and accurately output the target category and 3D bounding box. Figure 6 The tracking results are based on INNDA multi-target tracking in real-world scenarios. All potential motions detected by the multi-target system are individually labeled, and each target has its own unique ID number. The state of each potential moving target, including speed and trajectory, is estimated in real time through the target tracking method. The running speed is used to determine whether the target is stationary or moving. Then, in the subsequent scene segmentation, moving targets with non-zero speeds are removed from the original point cloud based on the ID number of the tracked target.

[0223] Table 2 shows the comparative experimental results of the tracking methods. According to the data in Table 2, the multi-target tracking method proposed in this invention is comparable to deep learning-based multi-target tracking methods in various performance evaluation metrics, and its average time consumption is also much lower. In summary, the multi-target tracking method of this invention achieves a balance between accuracy and computational efficiency, and can perform real-time dynamic and static point cloud segmentation of the original point cloud in dynamic scenes.

[0224] Table 2 Multi-target tracking results

[0225]

[0226] Table 3 Comparison of pose errors under the KITTI dataset

[0227]

[0228] Table 3 shows the absolute pose error (ATE) and relative pose error (RPE) results of each laser odometry method on the 00-08 sequence in the KITTI multi-sensor dataset, demonstrating the accuracy and stability of the system positioning. As can be seen from the table, the pose estimation output from the front-end odometry of the SLAM method of this invention is more accurate in all sequences, indicating that the accuracy of the laser odometry of this invention is improved compared to LOAM and LeGO-LOAM. In the urban scene 07 sequence, where the distance to the odometry is relatively short, there is no significant difference in pose error among the three methods. However, in situations with distances as long as 3000 meters and complex scenes, such as the 00 and 08 sequences, the absolute pose error of this invention is significantly lower than the other two methods, proving that using information from important feature points for pose estimation and performing secondary distortion correction can improve the positioning accuracy of the system.

[0229] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit it. Although the present invention has been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications or equivalent substitutions can be made to the technical solutions of the present invention without departing from the spirit and scope of the technical solutions of the present invention, and all such modifications or substitutions should be covered within the scope of the claims of the present invention.

Claims

1. A method for acquiring a robot laser odometry based on dynamic target tracking, characterized in that, The method comprises the following steps: S1: preprocessing all laser points in each frame of laser radar point cloud collected by the laser radar, and detecting potential dynamic targets by a point cloud segmentation network based on FCNN after preprocessing the point cloud; S2: using Kalman filtering method to estimate the state of the detected potential dynamic target, obtaining the optimal estimation value X of the mass center state of the potential dynamic target at time t t =(x t ,y t ,v t ), wherein (x t ,y t ) represents the plane coordinates of the potential dynamic target at time t, and v t represents the speed of the potential dynamic target at time t; S3: calculating the geometric information and intensity information of the potential dynamic targets; S4: fusing the geometric information and intensity information of the potential dynamic targets, detecting all potential dynamic targets by a correlation method, and determining whether the potential dynamic target is a static target or a dynamic target according to the speed of the potential dynamic target calculated in S2, if the speed of the potential dynamic target at time t is 0, the potential dynamic target is determined as a static target, otherwise, the potential dynamic target is determined as a dynamic target; S5: extracting feature points of the static target by curvature; S6: assigning different weights to the feature points by curvature, then constructing a pose constraint function based on a weighted function and solving to obtain the pose estimation of the laser radar; S7: Based on the sliding window and the key frame and the pose estimation obtained in S6, the laser points corresponding to the static target are updated for distortion compensation, and the latest laser odometry is obtained, i.e., the pose transformation relationship ΔT between adjacent key frames is obtained k,k+1 and the latest coordinates of the updated laser points corresponding to the static target.

2. The method for acquiring a robot laser odometry based on dynamic target tracking as claimed in claim 1, characterized in that: The preprocessing step of all laser points in the point cloud collected by the laser radar in S1 is as follows: Let the laser point cloud collected by the kth scan be P k , the transformation relationship between the laser radar coordinate system and the IMU coordinate system is represented as , the laser point cloud can be mapped to the IMU coordinate system: where p I and p L respectively represent the laser point in the IMU coordinate system and the laser point in the radar coordinate system; θ e denotes the angle of the end scan line relative to the start line, θ n is the angle of the current scan line relative to the start line, the instant at which the current point lies is denoted as: where t s represents the start time of the laser radar, and f represents the rotation frequency of the laser radar. After the unordered point cloud is serialized, the two frames of IMU data closest to t k are found based on time sequence and pre-integration calculation is performed, and then the calculation result is projected to the world coordinate system W, so as to obtain the IMU pose of t k in W Similarly, the IMU pose of t k+1 can be obtained Based on this, the pose of t k+1 relative to t k can be obtained Finally, by transform calculation, obtain the motion compensation transformation matrix of each laser point at t n ​ Compensation of motion distortion is achieved, resulting in compensated laser spots 3. The method of acquiring a robot laser odometry based on dynamic target tracking as claimed in claim 2, wherein: The process of detecting potential dynamic targets by the point cloud segmentation network based on FCNN in S1 is as follows: S1.1: performing dimension reduction processing on the preprocessed point cloud, then dividing the 3D space of the preprocessed point cloud into regions according to the bird's eye projection and establishing a 2D index thereof, then extracting feature information of each grid point column in the projection graph, and the feature information is used to generate the input of the FCNN network, i.e. a two-dimensional feature map; The FCNN network comprises a feature encoder, a feature decoder and an attribute predictor; S1.2: the feature encoder extracts semantic features of the two-dimensional feature map by convolution of a convolution layer, and continuously down-samples the spatial resolution of the two-dimensional feature map, finally outputting a feature vector, wherein the convolution layer comprises a trained VGG network and a ResNet network; S1.3: the feature vector is input into the feature decoder, an inverse convolution layer is adopted for up-sampling to obtain a prediction map with the same size as the two-dimensional feature map; S1.4: the attribute predictor calculates four attribute values of each grid in the prediction map, the four attribute values are center offset value, center offset direction value, attribute probability value and object height value; S1.5: constructing a bias pointer of each grid according to the center offset value and the center offset direction of each grid, and each multiple pointer points to a cell as a candidate classification object; For the candidate classification object, if the attribute probability value of the classification object is not less than a probability threshold, it is considered as a reliable clustering object; The screened reliable clustering object is a potential dynamic target.

4. The method for acquiring a robot laser odometry based on dynamic target tracking as claimed in claim 2, wherein: The calculation process of the geometric information and intensity information of the potential dynamic target in S3 is as follows: S3.1: Calculate the geometric information of the potential dynamic target, that is, calculate the centroid position of the potential dynamic target, calculate the centroid position of the potential dynamic target by using the laser point coordinates corresponding to the potential dynamic target As shown in formula (6): where n represents the number of laser points corresponding to the potential dynamic target, x i ,y i ,z i represents the coordinates of the i-th laser point corresponding to the dynamic target, represents the coordinates of the centroid of the dynamic target; S3.2: converting the original laser point data output by the laser radar into a decimal representation, and performing normalization processing to obtain the intensity information of each laser point; S3.3: correcting the intensity information of the laser point by a mapping function: wherein d is the distance information of the original laser point, η k represents the intensity information of the kth original laser point, η k ′ represents the intensity information of the kth laser point after calibration.

5. The method of acquiring a robot laser odometry based on dynamic target tracking as claimed in claim 4, wherein: The process of detecting all potential dynamic targets by a correlation method in S4 is as follows: S4.1: calculating the surface reflection coefficient ρ' of the laser point corresponding to the potential dynamic target: wherein M is a constant, and a is the laser incidence angle; S4.2: Determine whether the potential dynamic target in different frames is the same target. If formula (9) is satisfied, it is considered that the potential dynamic target in different frames is the same target, otherwise it is not: where d thr and p thr respectively represent the distance threshold and intensity threshold set, represents the centroid position of the potential dynamic target a at time t and time t-1, represents the intensity average of a at time t and time t-1, Through formula (9), the potential dynamic target in the t time picture is associated with the potential dynamic target in the t-1 time picture, and the potential dynamic target blocked by the obstacle is associated, so as to detect all potential dynamic targets.

6. The method for acquiring a robot laser odometry based on dynamic target tracking as claimed in claim 2, wherein: The process of extracting feature points of the static target in S5 through curvature is as follows: S5.1: Laser radar scans a vertical plane with M parallel readings, each parallel line has N points, all the laser points corresponding to all static targets in the kth laser radar scan are denoted as P k , each laser point is denoted as wherein m∈[1,M], n∈[1,N], x k ,y k ,z k are three-dimensional coordinates of the kth laser point; The set of consecutive points of the online beam in the horizontal direction is denoted as The set of consecutive points of the online beam in the horizontal direction is denoted as The set of consecutive points of the online beam in the horizontal direction is denoted as The plane formed by adjacent points, The curvature of the plane formed by adjacent points The calculation formula is: wherein N s represents the number of laser points in the middle row, represents the jth laser point in the mth row in the middle Then all the laser points in the image are classified according to the value of c, the laser points with c value greater than a set threshold are regarded as corner points or edge points, and the laser points with c value less than or equal to the set threshold are regarded as planar feature points. Then all the laser points in the image are classified according to the value of c, the laser points with c value greater than a set threshold are regarded as corner points or edge points, and the laser points with c value less than or equal to the set threshold are regarded as planar S5.2: Points are selected by setting a threshold σ The edge points and plane feature points collected by each scan line are marked as candidate edge points and candidate plane feature points The average is divided into four sub-regions, and at most two edge feature points and four plane feature points are extracted in each sub-region. The set of all edge feature points in the four sub-regions is denoted as ε k , and the set of all plane feature points in the four sub-regions is denoted as s k .

7. The method of acquiring a robot laser odometry based on dynamic target tracking as claimed in claim 6, wherein: The process of obtaining the laser radar pose estimation in S6 is as follows: S6.1: Represent all laser points corresponding to all static targets of the kth laser scan as P k k k k k k T , where T k is the pose of the laser in the kth frame, the 6-DOF pose transformation between two consecutive frames k-1 and k is:​​​​​​ where is the Lie algebra representation, the function is the mapping from the homogeneous transformation matrix to the Lie algebra, the k-th inter-frame to short-time δ t pose transformation within the interval [tk, tk+1] is represented as: In the formula represents the position of all laser points corresponding to the static target of the current k frame re-projected to the start time of the current frame; Assume ε k The local smoothness of edge feature point p ε is c ε , s k The local smoothness of middle plane feature point p s is c s Their weights are defined as shown in equation (14): Among them, W(p) ε ), W(p s ) represent p respectively ε and p s The weight, Represents ε k The j-th edge feature point in the i-th row The curvature; S6.2: Calculate the distance between p and each edge feature point in the global edge feature set ε The covariance matrix of the neighboring points in the global edge feature set, i.e. all ε k in a frame of image, if the covariance matrix contains the largest eigenvalue, it means that p ε The neighboring points in the global edge feature are distributed on a straight line, and the eigenvector corresponding to the largest eigenvalue is taken as the direction of the straight line, and the geometric center of the neighboring points is taken as the position of the straight line, at this time the distance between p and the global edge feature ε is: where p n is a unit vector; S6.3: Calculate each planar feature point p S , the covariance matrix of the neighboring points in the global planar feature set, i.e. all s k , the set of global planar features contained in a frame, if the covariance matrix contains a smallest eigenvalue, it means that these neighboring points are distributed on a plane, the eigenvector corresponding to the smallest eigenvalue is taken as the direction of the surface normal, and the geometric center of the neighboring points is taken as the position of the plane, at this time the distance of the planar feature point p S to the global planar feature set is represented as: S6.4: The optimization function is defined as: S6.5: Solve the nonlinear equation of formula (18) by Gauss-Newton method to get the optimal attitude estimation, apply the model with left perturbation to estimate the Jacobian matrix J p : where [Tp] × i.e., [T k p k ] × , δ, ξ respectively represent the 6-DOF pose transformation between two consecutive frames and the short-time δ t T k p k represents a 4*4 matrix and a 4*1 vector to obtain a 4*1 vector, the Jacobian matrix J ε is calculated by the following formula:​ jacobian matrix j of the planar residuals s : The iterative increment ΔT is expressed by a Jacobian matrix k , the laser odometry estimation result is updated using ΔT k , the posture is repeatedly iteratively optimized until ΔT k converges, and the current laser radar pose estimation T k is calculated.

8. The method of acquiring a robot laser odometry based on dynamic target tracking as claimed in claim 7, wherein: The process of obtaining the latest laser odometry in S7 is as follows: S7.1: Select key frame by setting a threshold of pose transformation, i.e. when the pose transformation between the kth and k+1th frame exceeds the threshold, the current frame is selected as the key frame F k+1 and add a sliding window, the F k+1 is matched with the pose of p k ; then extract the i closest key frames in time sequence and represent these key frames as a sub-key frame set {F k-i ,K,F k}, project the sub-key frame set to W to obtain the projection map M k by the transformation matrix {T' k-i ,K,T' k} between W and the sub-key frame set. M k from the edge feature point cloud and the plane feature point cloud Each key frame is associated with a corresponding projection map M k with each other, and the association relationship is defined as follows: wherein, respectively represent edge feature points of the kth key frame and the k-i key frame in the sub key frame set, respectively represent plane feature points of the kth key frame and the k-i key frame in the sub key frame set; S7.2: the newly obtained k+1 key frames F k+1 transformed to W and associated with the projection map M k obtaining the incremental pose transformation ATk+1 k,k+1 : wherein, represents T k the inverse matrix of T k+1 represents the pose estimate of the k+1 frame lidar; Using 6-DOF pose transformation to recalculate distortion, we get: wherein is the Lie algebra representation of ΔT k,k+1 The updated optimal positioning estimation of the lidar, ΔT k,k+1 represents the pose transformation relationship between adjacent two key frames, represents the latest coordinates of the updated laser points corresponding to the static target.

Images