Real-time ego speed estimation and odometry trajectory prediction method and system based on fmcw lidar

Abstract

The application discloses a real-time self speed estimation and odometer trajectory prediction method and system based on FMCW laser radar, comprising: constructing an observation equation between the radial velocity of a static environment point and the self speed of a sensor, obtaining the self speed of the sensor and a Doppler bias through robust estimation; calculating a Doppler residual based on the self speed of the sensor and the Doppler bias, and separating static points and dynamic points according to the Doppler residual; performing motion de-distortion processing on a scanning point cloud based on the self speed of the sensor and the angular velocity prior; performing scanning-to-map registration based on high-confidence static point cloud and a local map, and outputting real-time self speed of the sensor, odometer pose and the local map. The application improves the initial motion estimation accuracy, reduces the interference of dynamic points on registration, reduces the registration error caused by scanning distortion, and meets the real-time operation requirements of robots, autonomous vehicles, low-altitude unmanned aerial vehicles and inspection equipment.

Description

Technical Field

[0001] This invention relates to the field of autonomous driving technology, and in particular to a method and system for real-time self-velocity estimation and odometry trajectory prediction based on FMCW lidar. Background Technology

[0002] Autonomous mobile bodies such as mobile robots, low-altitude drones, autonomous vehicles, port / warehousing logistics robots, and tunnel inspection robots need to obtain their own speed and odometry pose in real time in scenarios without GPS, with weak textures, strong dynamics, or high-speed movement. LiDAR odometry has the advantages of being insensitive to illumination, having high efficiency in building 3D environment maps, and requiring no external infrastructure. Its pose can be estimated through geometric registration between neighboring point clouds or between the current point cloud and a local map. Therefore, it is widely used in high-speed movement scenarios with weak signals and strong dynamics.

[0003] Traditional pulsed or Time-of-Flight (ToF) lidar primarily outputs three-dimensional coordinates, echo intensity, and timestamps, failing to provide velocity information for each three-dimensional point. In contrast, coherent Frequency Modulated Continuous Wave (FMCW) lidar can derive the target's radial velocity from the beat frequency and Doppler frequency shift while simultaneously measuring distance. Therefore, for the radial velocity of static environmental points caused by the sensor's own motion, FMCW lidar naturally provides a set of observation equations directly related to its own velocity.

[0004] For low-altitude UAVs, flight speed, attitude changes, and scanning time collectively cause point cloud motion distortion. Simultaneously, dynamic factors such as ground vehicles, pedestrians, swaying trees, and localized motion caused by rotor airflow disrupt conventional point cloud registration. If the Doppler radial velocity of each point on an FMCW lidar can be used to estimate its own velocity within a single frame or short time window, and dynamic points can be eliminated and distortion corrected through scanning, followed by registration using a high-confidence static point cloud, the robustness of real-time odometry in dynamic and low-altitude flight scenarios could be significantly improved.

[0005] Existing FMCW lidar technologies mainly include the following categories: lidar odometry based on geometric registration, odometry based on inertial / wheel speed / GNSS fusion, point cloud preprocessing technology based on dynamic point detection, target detection and tracking technology based on FMCW or Doppler information, and point cloud registration technology based on Doppler constraints. However, these technologies still have drawbacks such as sensitivity to dynamic scenes, lack of direct observation of the velocity of a single frame, difficulty in eliminating scanning motion distortion by geometry alone, reliance on external sensors or priors, and failure to fully utilize the velocity information of each point in FMCW. This leads to problems such as low motion estimation accuracy, poor anti-interference performance, and large distortion registration errors in scenarios with many dynamic objects, degraded geometric features, fast-moving flight / vehicle platforms, and unavailable GNSS.

[0006] Therefore, existing technologies still need improvement. Summary of the Invention

[0007] The technical problem to be solved by the present invention is to provide a real-time self-velocity estimation and odometry trajectory prediction method and system based on FMCW lidar, in order to solve the problems of low motion estimation accuracy, poor anti-interference performance and large distortion registration error in scenarios with many dynamic objects, degraded geometric features, fast movement of flight / vehicle platforms and unavailable GNSS.

[0008] The technical solution adopted by this invention to solve the technical problem is as follows: In a first aspect, the present invention provides a method for real-time self-velocity estimation and odometry trajectory prediction based on FMCW lidar, including: An observation equation is constructed between the radial velocity of a static environmental point and the sensor's own velocity. The sensor's own velocity and Doppler bias are obtained through robust estimation. The Doppler residual is calculated based on the sensor's own velocity and the Doppler bias, and the static point and dynamic point are separated according to the Doppler residual. Based on the prior velocity and angular velocity of the sensor itself, motion distortion correction processing is performed on the scanned point cloud; Based on high-confidence static point cloud and local map, scanning and map registration are performed, and the sensor's own velocity, odometry pose and local map are output in real time.

[0009] In one implementation, the step of constructing the observation equation between the radial velocity of the static environment point and the sensor's own velocity, and obtaining the sensor's own velocity and Doppler bias through robust estimation, includes: Collect point cloud data and preprocess the point cloud data; wherein, each point in the point cloud data includes three-dimensional coordinates, radial velocity, sampling time or relative scan time, validity flag, intensity, SNR, line number and echo sequence number; Based on the preprocessed point cloud data, an observation equation is constructed between the radial velocity of the static environment point and the velocity of the sensor itself within a single frame or a preset time window. The sensor's own velocity and Doppler bias in the lidar coordinate system are obtained through robust estimation.

[0010] In one implementation, obtaining the sensor's own velocity and Doppler bias in the lidar coordinate system through robust estimation includes: The sensor's own velocity and Doppler bias are solved by the following robust estimation: ; in, These represent the sensor's own velocity and Doppler bias, respectively. These represent the optimized sensor velocity and Doppler bias, respectively. This represents the weight of the i-th point; (.) indicates robust loss; Represents a static environment point; This represents the standard deviation of the radial velocity noise at the i-th point; This indicates the velocity of the previous frame, IMU integration, flight control estimation, or zero velocity prior. This represents the slowly varying prior of the Doppler bias.

[0011] In one implementation, the step of calculating the Doppler residual based on the sensor's own velocity and the Doppler bias, and separating the static point and the dynamic point based on the Doppler residual, includes: The radial velocity residual of each point relative to the static model is calculated based on the sensor's own velocity and the Doppler bias to obtain the Doppler residual; Based on the Doppler residual, the static probability of each point is determined by a hard threshold determination method or a soft probability calculation method, and the static point and the dynamic point are separated based on the static probability of each point. The hard threshold in the hard threshold determination method is either a fixed threshold or a threshold adaptively determined based on velocity noise, distance, SNR, incident angle, scan line, and historical residual distribution. The soft probability in the soft probability calculation method is the probability calculated based on the geometric consistency probability and temporal consistency probability of each point.

[0012] In one implementation, the motion distortion correction processing of the scanned point cloud based on the estimated sensor's own velocity and angular velocity prior includes: The platform's motion speed is calculated based on the sensor's own speed and the prior angular velocity. The time difference is calculated based on the scanning reference time and the sampling time of the corresponding point. A distortion correction model is constructed based on the platform's motion speed, the time difference, and the original coordinates of the point. The distortion correction model is used to perform distortion correction on the scanned point cloud. Local geometric consistency is recalculated on the distorted point cloud, and the static point weights are updated to obtain the distorted static points.

[0013] In one implementation, the step of scanning to map registration based on a high-confidence static point cloud and a local map, and outputting real-time sensor velocity, odometry pose, and local map, includes: Construct the local map; wherein, each map cell in the local map stores information such as the number of points, centroid, normal vector, covariance, timestamp, static confidence level, and number of observations; For a static point after distortion removal, the nearest plane, point element, or NDT element is found in the local map to obtain the corresponding centroid and normal vector. Based on the centroid and the normal vector, calculate the residual from the distortion-free static point to the nearest plane; Based on the residuals, static probabilities, and geometric robust loss, the odometry pose of the distortion-free static points is calculated using a robust optimization estimation method; wherein, the robust optimization estimation method includes any one of the following: Gauss-Newton algorithm, Levenberg-Marquardt method, iterative nearest point, NDT optimization, and factor graph incremental optimization; The output includes optimized sensor velocity, odometer pose, and local map.

[0014] In one implementation, the step of scanning to map registration based on a high-confidence static point cloud and a local map, outputting real-time sensor velocity, odometry pose, and local map, then includes: The geometric velocity is obtained by dividing the pose translation between the current frame and the previous frame by the time interval. The geometric velocity is then compared with the sensor's own velocity obtained by robust estimation. If the difference between the two is large, the sensor's own velocity is optimized by adding a degradation flag, reducing the map update weight, or triggering a re-estimation.

[0015] Secondly, the present invention provides a real-time self-velocity estimation and odometry trajectory prediction system based on FMCW lidar, comprising: The point cloud acquisition and preprocessing module is used to acquire point cloud data and preprocess the point cloud data. The robust estimation module is used to construct the observation equation between the radial velocity of a static environmental point and the sensor's own velocity, and obtains the sensor's own velocity and Doppler bias through robust estimation. The static / dynamic point separation module is used to calculate the Doppler residual based on the sensor's own velocity and the Doppler bias, and to separate static points and dynamic points according to the Doppler residual; The distortion correction module is used to perform motion distortion correction processing on the scanned point cloud based on the sensor's own velocity and angular velocity prior. The static point cloud to map registration module is used to perform scan-to-map registration based on high-confidence static point clouds and local maps, and outputs real-time sensor speed, odometry pose and local map.

[0016] Thirdly, the present invention provides a terminal, comprising: a processor and a memory, wherein the memory stores a real-time self-velocity estimation and odometer trajectory prediction program based on FMCW lidar, and the real-time self-velocity estimation and odometer trajectory prediction program based on FMCW lidar, when executed by the processor, is used to implement the operation of the real-time self-velocity estimation and odometer trajectory prediction method based on FMCW lidar as described in the first aspect.

[0017] Fourthly, the present invention also provides a computer-readable storage medium storing a real-time self-velocity estimation and odometer trajectory prediction program based on an FMCW lidar, wherein the real-time self-velocity estimation and odometer trajectory prediction program based on an FMCW lidar, when executed by a processor, is used to implement the operation of the real-time self-velocity estimation and odometer trajectory prediction method based on an FMCW lidar as described in the first aspect.

[0018] The present invention, by employing the above technical solution, has the following effects: This invention uses FMCW radial velocity for self-velocity estimation, dynamic point culling, and scan distortion correction. It can improve the accuracy of initial motion estimation, reduce the interference of dynamic points on registration, and reduce registration errors caused by scan distortion in scenarios with many dynamic objects, degraded geometric features, fast-moving flight / vehicle platforms, and unavailable GNSS. It also meets the real-time operation requirements of robots, autonomous vehicles, low-altitude drones, and inspection equipment. Attached Figure Description

[0019] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art 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 the structures shown in these drawings without creative effort.

[0020] Figure 1 This is a flowchart of the real-time self-velocity estimation and odometry trajectory prediction method based on FMCW lidar in this invention.

[0021] Figure 2 This is a schematic diagram of the architecture of the real-time self-velocity estimation and odometer trajectory prediction system based on FMCW lidar in this invention.

[0022] Figure 3 This is the algorithm flowchart of the overall scheme in this invention.

[0023] Figure 4 This is a schematic diagram of the geometric model for estimating the radial velocity of the FMCW in this invention.

[0024] Figure 5 This is a schematic diagram of static / dynamic point separation based on Doppler residuals in this invention.

[0025] Figure 6 This is a functional schematic diagram of the terminal in one implementation of the present invention.

[0026] The objectives, features, and advantages of this invention will be further explained in conjunction with the embodiments and with reference to the accompanying drawings. Detailed Implementation

[0027] To make the objectives, technical solutions, and advantages of this invention clearer and more explicit, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention.

[0028] Exemplary methods Existing FMCW lidar technologies mainly include the following categories: 1) LiDAR odometry based on geometric registration: for example, ICP, point-to-area ICP, NDT, LOAM and their improved methods. These methods perform adjacent frames or scans to the map registration using geometric features such as edges, planes, voxels or surfels.

[0029] 2) Odometry based on inertial / wheel speed / GNSS fusion: By fusing IMU, wheel speed gauge, barometer, vision or GNSS with LiDAR, the degradation problem of single sensor can be alleviated.

[0030] 3) Point cloud preprocessing based on dynamic point detection: Dynamic objects are removed by semantic segmentation, clustering, temporal consistency, and occupancy grid or motion consistency to avoid dynamic points from participating in mapping and registration.

[0031] 4) Target detection and tracking based on FMCW or Doppler information: The radial velocity at each point is used to distinguish moving objects and estimate the target state. However, the target is usually an external moving target, rather than using Doppler constraints as a unified front end for its own velocity estimation and odometry.

[0032] 5) Point cloud registration based on Doppler constraints: Some methods use radial velocity as one of the registration constraints, but usually do not form a real-time closed-loop system of "single frame self-velocity estimation → static / dynamic filtering → scan distortion removal → static point cloud to map registration".

[0033] However, these technologies still have the following drawbacks: 1) Sensitive to dynamic scenes: Conventional ICP, NDT or LOAM will mistake dynamic points for static structures when there are a large number of non-static points in the scene containing a large number of vehicles, pedestrians, mobile robotic arms or low-altitude flight, causing registration offset and map pollution.

[0034] 2) Lack of direct observation of its own velocity in a single frame: The point cloud geometric registration of traditional ToF LiDAR requires adjacent frames or maps, and it is not easy to stably give its own velocity when the geometry is degraded, the point cloud is sparse, the maneuver is fast or the initial pose is inaccurate.

[0035] 3) Scan motion distortion is difficult to eliminate using geometry alone: ​​For mechanical scanning or line-by-line scanning LiDAR, the sampling time of each point within a single frame is different. Without reliable motion estimation, the point cloud before registration is already distorted, and subsequent optimization is prone to getting trapped in local optima.

[0036] 4) Reliance on external sensors or priors: IMU, GNSS, wheel speedometer, barometer, etc. can provide motion priors, but increase cost, calibration complexity and system failure modes; GNSS is unstable in indoor, urban canyon, tunnel and low-altitude near-ground flight.

[0037] 5) Failure to fully utilize the velocity information per point of FMCW: Existing FMCW LiDAR applications often use Doppler as a target velocity or target segmentation cue, rather than using it throughout robot velocity estimation, static point recognition, point cloud distortion correction, and odometry registration.

[0038] In summary, existing technologies suffer from low motion estimation accuracy, poor anti-interference performance, and large distortion registration errors in scenarios with many dynamic objects, degraded geometric features, fast-moving flight / vehicle platforms, and unavailable GNSS.

[0039] To address the above technical problems, this invention provides a real-time self-velocity estimation and odometry trajectory prediction method based on FMCW lidar. The method includes: constructing an observation equation between the radial velocity of a static environmental point and the sensor's own velocity; obtaining the sensor's own velocity and Doppler bias through robust estimation; calculating the Doppler residual based on the sensor's own velocity and the Doppler bias; separating static points from dynamic points based on the Doppler residual; performing motion distortion correction on the scanned point cloud based on the sensor's own velocity and angular velocity priors; performing scan-to-map registration based on the high-confidence static point cloud and a local map; and outputting real-time sensor self-velocity, odometry pose, and a local map. This invention improves the accuracy of initial motion estimation, reduces the interference of dynamic points on registration, reduces registration errors caused by scan distortion, and meets the real-time operational requirements of robots, autonomous vehicles, low-altitude drones, and inspection equipment.

[0040] like Figure 1 As shown, this embodiment of the invention provides a method for real-time self-velocity estimation and odometry trajectory prediction based on FMCW lidar, including the following steps: Step S100: Construct the observation equation between the radial velocity of the static environment point and the sensor's own velocity, and obtain the sensor's own velocity and Doppler bias through robust estimation.

[0041] In this embodiment, to implement the method, a real-time self-velocity estimation and odometry trajectory prediction system based on FMCW lidar is provided.

[0042] like Figure 2 As shown, the system includes: an FMCW lidar 101, an inertial measurement unit 102 (optional unit, including IMU, wheel speedometer, barometer, angular velocity and motion prior), a real-time processor 103, a memory 104, a point cloud acquisition and preprocessing module 105, a robust Doppler self-velocity estimation module 106 (hereinafter referred to as robust estimation module), a static / dynamic point separation module 107, a Doppler-assisted scanning distortion correction module 108 (hereinafter referred to as distortion correction module), a static point cloud to map registration module 109, a local map module 110, and a status output module 111.

[0043] The FMCW lidar 101 can directly output the radial velocity at each point; specifically, it is used to collect point cloud data, where each point contains at least three-dimensional coordinates. radial velocity Sampling time Alternatively, it may include relative scan time and validity indicators; optionally, each point may also include information such as intensity, SNR, line number, and echo sequence number.

[0044] The inertial measurement unit 102 is used to provide angular velocity, acceleration or attitude priors, especially for rotation compensation in scan distortion correction; in the absence of an IMU, the angular velocity can be estimated from the registration result of the previous frame.

[0045] Real-time processor 103 and memory 104: used to run the algorithm process and save the point cloud, velocity estimation, odometry status, local map and quality indicators of the most recent frames.

[0046] The robust estimation module 106 is used for its own velocity estimation; the static / dynamic point separation module 107, the distortion removal module 108, and the static point cloud to map registration module 109 together form the odometer front end.

[0047] The local map module 110 is used to update the local map in a rolling manner; specifically, it maintains a high-confidence static point structure in the form of voxels, planes, Surfels, KD-trees, hash rasters, or NDT grids, and performs real-time scanning to map registration.

[0048] In this embodiment, the system utilizes the characteristic of FMCW lidar to obtain the radial Doppler velocity of each point while outputting a 3D point cloud. Within a single frame or short time window, it constructs an observation equation between the radial velocity of static environmental points and the sensor's own velocity. Through robust estimation, it obtains the sensor's own linear velocity and Doppler bias. Subsequently, based on the Doppler residual, it separates static and dynamic points, uses the estimated self-velocity and angular velocity priors to perform motion distortion correction on the scanned point cloud, and uses only the high-confidence static point cloud and the local map for scan-to-map registration, thereby outputting the real-time self-velocity, odometry pose, and local map.

[0049] In order to accurately estimate the sensor's own linear velocity and Doppler bias, this embodiment needs to define the coordinate system and variables, as follows: Define the world coordinate system W, the lidar body coordinate system L, and the current scanning reference time. The coordinates of the i-th point in the lidar coordinate system are: The distance is The line-of-sight unit vector is The FMCW lidar measured the radial velocity of this point relative to the sensor as follows: For a static point, its radial velocity relative to the motion sensor is mainly determined by the linear velocity of the lidar itself. This results in the following approximate model being satisfied: ; in, The linear velocity of the laser radar in its coordinate system is itself. For Doppler zero bias or systematic speed bias, To measure noise. For dynamic object points, there will also be additional object motion terms. Therefore, its Doppler residuals will deviate significantly from the static model.

[0050] This embodiment utilizes an overdetermined system of equations formed by a large number of static environment points to estimate in real time within each frame or short time window. and Points that deviate from the model are identified as dynamic or anomalous candidate points.

[0051] like Figure 3 As shown, based on the coordinate system, variable definitions, and FMCW radial velocity model described above, the real-time process in this embodiment includes the following steps:

[0052] Step A (Robust Doppler self-velocity estimation) corresponds to self-velocity estimation; Steps B, C, and D together constitute the odometer front end.

[0053] As an example, the construction of the observation equation between the radial velocity of a static environmental point and the sensor's own velocity, and the acquisition of the sensor's own velocity and Doppler bias through robust estimation, includes: acquiring point cloud data and preprocessing the point cloud data; wherein each point in the point cloud data contains three-dimensional coordinates, radial velocity, sampling time or relative scanning time, validity flag, intensity, SNR, line number, and echo sequence number; based on the preprocessed point cloud data, constructing the observation equation between the radial velocity of the static environmental point and the sensor's own velocity within a single frame or a preset time window, and acquiring the sensor's own velocity and Doppler bias in the lidar coordinate system through robust estimation.

[0054] In this embodiment, step A does not rely on external maps, nor does it require that the point clouds of two adjacent frames have been registered. Instead, it directly solves for the linear velocity of the lidar itself using the radial velocity equation of a large number of points within a single frame or short window.

[0055] Specifically, the point cloud data acquired by the FMCW lidar is first preprocessed in the following ways: 1) Remove invalid points, points that are too close or too far away, points with too low SNR, and points whose radial velocity absolute value exceeds the physical limit.

[0056] 2) Based on the sensor calibration parameters, convert the point coordinates, radial velocity symbol, and timestamp to a unified lidar coordinate system L.

[0057] 3) Assign initial weights to each point based on intensity, distance, incident angle, echo sequence number, velocity measurement accuracy, etc. .

[0058] 4) Optionally, first eliminate sky points, isolated outer points, rotor / fuselage obstruction points, or reflection points of fixed installation structures.

[0059] Furthermore, for the preprocessed point cloud data, a robust estimation model is used to estimate the sensor's own velocity and Doppler bias in the lidar coordinate system. For ease of solution, the static point model is written in linear residual form in this embodiment: .

[0060] Since point clouds may contain dynamic points, ordinary least squares cannot be used directly. Therefore, this embodiment solves for the sensor's own velocity using the following robust estimation method. and Doppler bias : ; in, These represent the sensor's own velocity and Doppler bias, respectively. These represent the optimized sensor velocity and Doppler bias, respectively. This represents the weight of the i-th point; (.) represents robust loss, which can be Huber robust loss, Tukey robust loss, Cauchy robust loss, or Geman-McClure robust loss; Represents a static environment point; The radial velocity noise standard deviation at the i-th point can be estimated from the sensor's nominal velocity accuracy, SNR, or historical residuals. This indicates the velocity of the previous frame, IMU integration, flight control estimation, or zero velocity prior. This represents the slowly varying prior of the Doppler bias.

[0061] In this embodiment, the geometric model for estimating the radial velocity of the FMCW is as follows: Figure 4 As shown, the sensor's own velocity can be robustly solved within a single frame. and Doppler bias Specifically, the following two methods can be used to solve this problem: One implementation method is to use RANSAC (Random Sample Consensus Algorithm) to solve the problem. Specifically, this involves randomly selecting several points and finding a candidate... Then, the Doppler residual of how many points is less than a threshold is counted, and the points corresponding to the largest consistent set are selected as static candidate points; subsequently, the sensor's own velocity is obtained based on the static candidate points. and Doppler bias .

[0062] Another implementation method is to use the IRLS algorithm (an IRLS algorithm is a method for handling linear regression problems with sparse constraints) for iterative solution. Sensor speed and Doppler bias The initial values ​​can be given by ordinary least squares, RANSAC minimum set sampling, or the velocity of the previous frame; during each iteration, the weights can be updated according to the residuals (points with smaller residuals have larger weights, and points with larger residuals have smaller weights), and the damped linear normal equations can be solved. If the observability matrix... If the rank is lower than the preset threshold, the system outputs a degradation flag and increases the prior weight or introduces IMU / flight control speed constraints.

[0063] In one implementation, since radial velocity only constrains the projection of velocity in the line-of-sight direction, the observability of lateral and vertical velocities decreases when the field of view (FOV) is narrow, all points are concentrated in a small forward-facing angle region, or the scene geometry is simple. Therefore, this embodiment improves system stability by performing observability, bias, and sign self-calibration in the following ways: 1) Monitor the condition number and minimum eigenvalue of the observability matrix H, and use the previous frame's velocity, IMU, flight control, barometer, or ground constraints for weakly observable directions. The condition number is an indicator that measures the degree to which small perturbations in the input data affect the output results during the solution of the linear equations.

[0064] 2) Use it as a state variable for low-pass filtering or random walk estimation to avoid interpreting zero-bias velocity as true platform motion.

[0065] 3) During the sensor access phase, the s_d symbol is determined by static calibration or short-distance round-trip motion; during operation, the symbol can also be verified online by the consistency between the geometric odometry speed and the Doppler speed.

[0066] 4) For low-altitude scenarios of UAVs, the stability of three-dimensional velocity estimation can be improved by combining altitude change constraints, gravity direction, machine system mounting angle and flight control attitude.

[0067] In this embodiment, step A outputs the self-velocity in the lidar coordinate system. Doppler bias velocity covariance The outputs include the number of valid points, the proportion of interior points, and the root mean square of robust residuals. These outputs can be used as independent velocity estimation results, or in subsequent steps B to D, to form a real-time odometry.

[0068] Compared to existing odometry systems that rely solely on point cloud geometric registration, this embodiment utilizes the radial velocity and line-of-sight direction of each point on the FMCW lidar to robustly solve for the sensor's own velocity within a single frame or short window. and Doppler bias It can obtain its own speed even without a map or before the registration has converged, which improves initialization and real-time performance.

[0069] like Figure 1 As shown, this embodiment of the invention provides a method for real-time self-velocity estimation and odometry trajectory prediction based on FMCW lidar, including the following steps: Step S200: Calculate the Doppler residual based on the sensor's own velocity and the Doppler bias, and separate the static point and the dynamic point according to the Doppler residual.

[0070] In this embodiment, step B is mainly used to separate points in the FMCW point cloud that conform to the static environment model from dynamic object points or anomaly points. Its core is to use the self-velocity and Doppler bias obtained in step A to calculate the radial velocity residual of each point relative to the static model: The principle of separation is: if point i belongs to the static environment, then... It should be close to zero; if point i belongs to a vehicle, pedestrian, rotor disturbance area, oscillating object, or measurement anomaly, then It usually deviates from zero.

[0071] Specifically, in one implementation of this embodiment, the step of calculating the Doppler residual based on the sensor's own velocity and the Doppler bias, and separating static points and dynamic points based on the Doppler residual, includes: calculating the radial velocity residual of each point relative to the static model based on the sensor's own velocity and the Doppler bias to obtain the Doppler residual; determining the static probability of each point using a hard threshold determination method or a soft probability calculation method based on the Doppler residual, and separating the static point and the dynamic point based on the static probability of each point; wherein, the hard threshold in the hard threshold determination method is a fixed threshold or a threshold adaptively determined based on velocity noise, distance, SNR, incident angle, scan line, and historical residual distribution; the soft probability in the soft probability calculation method is a probability calculated based on the geometric consistency probability and temporal consistency probability of each point.

[0072] In this embodiment, static / dynamic point separation can be achieved by performing static probability and threshold discrimination on each point in the following two ways: In one implementation, a hard threshold is used for determination: ; in, The threshold can be fixed or adaptively determined based on velocity noise, distance, SNR, incident angle, scan lines, and historical residual distribution. For example, Alternatively, take several times the standard deviation of the high-confidence static pattern in the residual distribution of the current frame.

[0073] In another implementation, soft probability determination is used: ; in, This represents the probability of geometric consistency; for example, a point has a higher probability if its distance to the local map is smaller. This represents the probability of temporal consistency; for example, a higher probability is given if multiple consecutive frames exhibit a static structure. The system will use soft probabilities. Points exceeding the threshold are sent for odometer registration.

[0074] In this embodiment, the static / dynamic point separation result based on Doppler residuals is as follows: Figure 5 As shown, step B directly reduces the interference of dynamic points on the scanned map registration, making the local map consist of a high-confidence static structure; at the same time, the dynamic point set can still be retained for safety obstacle avoidance, traffic target tracking, or environmental understanding. Compared with dynamic point removal based on semantic networks, this embodiment can complete the dynamic consistency judgment in new scenes without relying on a large amount of labeled data, using sensor physical velocity observations, thus reducing the pollution of odometry registration and map updates by dynamic objects.

[0075] like Figure 1 As shown, this embodiment of the invention provides a method for real-time self-velocity estimation and odometry trajectory prediction based on FMCW lidar, including the following steps: Step S300: Based on the prior velocity and angular velocity of the sensor itself, motion distortion correction processing is performed on the scanned point cloud.

[0076] In this embodiment, during point cloud data acquisition, the mechanical scanning or line-by-line scanning LiDAR does not simultaneously acquire all points within a single frame scan. When the platform moves during scanning, directly treating all points as acquired at the same moment would result in motion distortion. Therefore, step C utilizes the estimated velocity from step A, combined with prior angular velocity data from the sensors, to perform motion compensation for each point according to the sampling time, thereby achieving Doppler-assisted scanning distortion correction.

[0077] Specifically, in one implementation of this embodiment, the motion distortion correction processing of the scanned point cloud based on the estimated sensor's own velocity and angular velocity prior includes: calculating the platform's motion velocity based on the sensor's own velocity and the angular velocity prior, calculating the time difference based on the scanning reference time and the sampling time of the corresponding point, constructing a distortion correction model based on the platform's motion velocity, the time difference, and the original coordinates of the point; performing distortion correction processing on the scanned point cloud based on the distortion correction model, recalculating the local geometric consistency on the distorted point cloud, and updating the static point weights to obtain the distorted static points.

[0078] In this embodiment, the distortion correction model used is as follows: Let the scan reference time be The sampling time of point i is The relative time is Let the platform's movement speed during the scanning period be... ,in, To estimate its own speed, Let be the angular velocity (which can be derived from the IMU gyroscope, the registration angular velocity of the previous frame, or a short-window geometric estimate). Then, point i can be compensated to the reference time as follows: .in, This represents the original position coordinates of the point; This indicates a static point to remove distortion.

[0079] Under a small-angle approximation, it can also be written as: .

[0080] For scenarios involving high-speed movement of drones and vehicles. It can be obtained from the scan line number, point number, or hardware timestamp; if the sensor has provided a motion compensation switch, this embodiment preferably turns off the sensor’s internal compensation based on external speed, retains the original Doppler self-motion signal, and completes distortion removal uniformly in the process of this embodiment.

[0081] In one implementation, this embodiment can couple the distortion correction and static point selection steps. Specifically, the velocity is first estimated on the undistorted point cloud using step A, and then distortion correction is performed using this velocity. Subsequently, the local geometric consistency is recalculated on the distorted point cloud, and the static point weights are updated. Two to three alternating iterations can be performed if necessary: ​​Doppler velocity estimation → static selection → distortion correction → geometric consistency check.

[0082] This coupling process is particularly suitable for scenarios where low-altitude drones pass through roads, under bridges, at the edges of forests, or in building complexes, because dynamic objects and scanning distortion often coexist.

[0083] Step C in this embodiment eliminates geometric distortion caused by scanning time before registration, reducing point-to-plane residuals and registration nonlinearity, making it easier for subsequent step D to converge to the correct pose. Compared to methods that rely solely on IMU distortion correction, this embodiment utilizes FMCW radial velocity to directly estimate linear velocity, reducing dependence on external linear velocity sensors and improving subsequent registration accuracy.

[0084] like Figure 1 As shown, this embodiment of the invention provides a method for real-time self-velocity estimation and odometry trajectory prediction based on FMCW lidar, including the following steps: Step S400: Based on the high-confidence static point cloud and the local map, scan to map registration is performed, and the real-time sensor speed, odometry pose and local map are output.

[0085] In this embodiment, step D is used to register the static point cloud obtained in step C with the local map to obtain the pose at the current scanning reference time. Since dynamic points have been downweighted or eliminated in step B, registration is mainly constrained by static structures.

[0086] Specifically, in one implementation of this embodiment, the step of scanning to map registration based on a high-confidence static point cloud and a local map, and outputting real-time sensor velocity, odometry pose, and local map, includes: constructing the local map; wherein each map cell in the local map stores information such as the number of points, centroid, normal vector, covariance, timestamp, static confidence, and number of observations; for a distortion-corrected static point, finding the nearest plane, point element, or NDT cell in the local map to obtain the corresponding centroid and normal vector; calculating the residual from the distortion-corrected static point to the nearest plane based on the centroid and normal vector; calculating the odometry pose of the distortion-corrected static point using a robust optimization estimation method based on the residual, static probability, and geometric robust loss; wherein the robust optimization estimation method includes any one of the following: Gauss-Newton algorithm, Levenberg-Marquardt method, iterative nearest point, NDT optimization, and factor graph incremental optimization; and outputting the optimized sensor velocity, odometry pose, and local map.

[0087] In this embodiment, during the local map construction process, the local map can employ voxel hashing, KD-tree, Surfel map, planar patch map, NDT raster, or multi-resolution submap. Each map cell can store the number of points and centroid. Normal vector Covariance, timestamp, static confidence level, and number of observations. When updating the map, only high-confidence static points or long-term stable points are added to the map; dynamic points, low-confidence points, and high residual points are not added to the map or are only added to the short-term obstacle layer.

[0088] For each distortion-reduced static point Find the nearest plane, Surfel, or NDT element in the local map to obtain the corresponding centroid. and normal vector The point-to-plane residual is: ;in, , Let be the rotation matrix and translation vector of the transformation between the two coordinate systems; Normal vector The transpose of .

[0089] The current pose can be robustly optimized and estimated as follows: ; in, Represents the set of static points. This indicates the error intensity at each point. It can be obtained by integrating the pose of the previous frame and the velocity of the device itself in step A. The static probability from step B, The loss is geometrically robust. Optimization can be achieved using any of the following methods: Gauss-Newton algorithm, Levenberg-Marquardt method, iterative nearest point, NDT optimization, or factor graph incremental optimization.

[0090] This embodiment uses only high-confidence static point clouds and local maps for scan-to-map registration, and utilizes Doppler velocity to form pose prediction, thereby enhancing the odometry robustness in dynamic and weak geometry scenes.

[0091] In this embodiment, based on the pose output in step D, the registered pose increment can be used to back-verify the velocity estimation in step A, so as to achieve a closed loop between the pose output and its own velocity estimation.

[0092] Specifically, in one implementation of this embodiment, the step of scanning to map registration based on high-confidence static point cloud and local map, outputting real-time sensor velocity, odometry pose, and local map, then includes: dividing the pose translation between the current frame and the previous frame by the time interval to obtain the geometric velocity, and comparing the geometric velocity with the sensor velocity obtained by robust estimation. If the difference between the two is large, the sensor velocity is optimized by adding a degradation flag, reducing the map update weight, or triggering re-estimation.

[0093] As an example, during the velocity estimation process in step A of the reverse verification, the geometric velocity is obtained by dividing the translation amount between the current frame and the previous frame by the time interval, and then compared with the velocity estimated by Doppler. If the difference between the two is large, the system can add a degradation flag, reduce the map update weight, or trigger a re-estimation.

[0094] In the enhanced implementation, steps A and D can also be jointly optimized, and the optimized state variables include: , and The objective function includes point-to-plane residuals, Doppler velocity residuals, and velocity-pose consistency residuals. ;in, This represents the distance from a point to a plane. This represents the velocity residual.

[0095] This enhanced implementation can further improve consistency in highly dynamic or weakly geometric scenes.

[0096] This embodiment outputs quality indicators such as velocity covariance, static point ratio, registration residual, and degradation flag in each frame, and adaptively adjusts the threshold and map update according to the indicators, which can improve engineering reliability and facilitate the safe use of flight control / vehicle controllers. Furthermore, by jointly optimizing pose, velocity, and Doppler bias in a unified objective function or factor graph, the consistency between velocity and pose estimation can be further improved.

[0097] In the practical application scenarios of this embodiment, the above technical solution can be deployed on in-vehicle computing platforms, drone edge computing platforms, robot controllers, FPGA / SoC, or GPU acceleration platforms. To meet real-time requirements, the following strategies can be adopted: 1) Point cloud downsampling: While maintaining field of view coverage, voxel downsampling, distance layer sampling or feature point sampling are used to control the number of registration points within a preset range.

[0098] 2) Module pipeline: Step A runs immediately after a new frame arrives; Steps B and C are processed in parallel on the CPU; Step D uses a multi-threaded KD-tree or GPU neighborhood search.

[0099] 3) Local map scrolling window: Only maintains the static map within the most recent few meters or seconds, controlling memory and computational load.

[0100] 4) Quality index output: Output the speed covariance, static point ratio, registration residual, internal point ratio, and degradation flag for each frame, which are used for upper-level control or security redundancy.

[0101] 5) Failure protection: When the proportion of static points is too low, the speed observability is insufficient, or the registration residual is too large, the system can degenerate into IMU / flight control / wheel speed prediction and suspend map updates.

[0102] As an example, based on the above strategy, the specific implementation method is as follows: Example 1: Real-time odometry for low-altitude unmanned aerial vehicles (UAVs): The FMCW LiDAR 101 is fixed at the forward or downward viewing position of the low-altitude UAV, and the flight control provides angular velocity or attitude priors. The UAV flies at speeds from 2 m / s to 20 m / s in environments such as roads, parks, warehouses, or under bridges. The system uses step A to estimate the velocity of the airborne LiDAR in each frame; step B to remove dynamic points such as vehicles, pedestrians, swaying leaves, and water reflections; step C to compensate for translation and rotation during scanning; and step D to register with static structures such as the ground, walls, lampposts, and building edges to output the relative trajectory of the UAV.

[0103] Example 2: Autonomous driving or mobile robots: The FMCW LiDAR is mounted on a vehicle or mobile robot. When the vehicle is running in a road or warehouse scene, dynamic objects such as surrounding vehicles, pedestrians, and forklifts will affect traditional LiDAR SLAM. In this embodiment, step A estimates the vehicle speed using the radial velocity of static points such as roads, buildings, and shelves; step B removes moving objects with abnormal Doppler residuals from the registration point set; step D uses only static structures for point-to-surface or NDT registration to achieve stable odometry in dynamic environments.

[0104] Example 3: Systems without IMU or with weak IMU: On cost-sensitive mobile platforms, a high-performance IMU can be omitted. The system's linear velocity is estimated independently by the FMCW LiDAR, while the angular velocity is estimated by geometric registration between adjacent frames or by a low-cost IMU. This embodiment reduces hardware costs and is particularly suitable for low-speed warehouse robots, inspection robots, and indoor positioning devices.

[0105] It is understood that those skilled in the art can improve or replace some of the technologies in the above-described solutions of this embodiment in the relevant application scenarios through the following methods: 1) Sensor form variations: FMCW LiDAR can be mechanical scanning, MEMS scanning, OPA solid-state scanning, Flash type, linear array scanning or multi-laser combination; it can also be fused with millimeter-wave radar, ToF LiDAR, vision camera or event camera.

[0106] 2) Velocity estimation variations: The robust loss function can be replaced by Huber, Tukey, Cauchy, RANSAC, MLESAC, maximum likelihood, Bayesian filtering, EKF / UKF, particle filtering, or neural network with physical constraint estimation.

[0107] 3) Static filtering variations: Static points can be obtained by using hard thresholding, soft probability, clustering, graph cut, conditional random field, Bayesian occupancy grid, learned segmentation network, or multi-frame consistency.

[0108] 4) Distortion and deformation correction: Rotation compensation can be obtained from IMU, flight control attitude, visual inertial odometry, geometric registration angular velocity or multi-frame fitting; linear velocity can be obtained from step A output, joint optimization output or short window smoothing output.

[0109] 5) Registration deformation: Step D can use point-to-point ICP, point-to-line / point-to-surface ICP, GICP, NDT, Surfel registration, direct voxel registration, factor map optimization or neural implicit map registration.

[0110] 6) Map deformation: Local maps can be dense point clouds, voxel maps, NDT maps, planar maps, semantic maps, dynamic / static layered maps, or short-term-long-term two-layer maps.

[0111] 7) Platform Variation: The embodiments of the present invention can also be used in drones, unmanned vehicles, autonomous vehicles, robot dogs, AGV / AMR, port machinery, mining vehicles, inspection equipment, surveying backpacks and handheld scanning devices.

[0112] 8) Quality control variation: Thresholds can be dynamically adjusted based on velocity covariance, residual distribution, static point ratio, map matching score, H matrix condition number, etc., or the credibility can be output to the upper-level controller.

[0113] 9) Software and hardware implementation variations: can be implemented on CPU, GPU, FPGA, DSP, NPU, automotive SoC or cloud-edge collaborative platform; can be used as ROS / ROS2 node, embedded firmware, automotive middleware module or flight control plug-in.

[0114] For those skilled in the art, any improvements or modifications made based on the above description should fall within the scope of the embodiments of the present invention.

[0115] This embodiment achieves the following technical effects through the above technical solution: This invention uses FMCW radial velocity for self-velocity estimation, dynamic point culling, and scan distortion correction. It can improve the accuracy of initial motion estimation, reduce the interference of dynamic points on registration, and reduce registration errors caused by scan distortion in scenarios with many dynamic objects, degraded geometric features, fast-moving flight / vehicle platforms, and unavailable GNSS. It also meets the real-time operation requirements of robots, autonomous vehicles, low-altitude drones, and inspection equipment.

[0116] Exemplary device Based on the above embodiments, the present invention also provides a real-time self-velocity estimation and odometry trajectory prediction system based on FMCW lidar, comprising: The point cloud acquisition and preprocessing module is used to acquire point cloud data and preprocess the point cloud data. The robust estimation module is used to construct the observation equation between the radial velocity of a static environmental point and the sensor's own velocity, and obtains the sensor's own velocity and Doppler bias through robust estimation. The static / dynamic point separation module is used to calculate the Doppler residual based on the sensor's own velocity and the Doppler bias, and to separate static points and dynamic points according to the Doppler residual; The distortion correction module is used to perform motion distortion correction processing on the scanned point cloud based on the sensor's own velocity and angular velocity prior. The static point cloud to map registration module is used to perform scan-to-map registration based on high-confidence static point clouds and local maps, and outputs real-time sensor speed, odometry pose and local map.

[0117] Based on the above embodiments, the present invention also provides a terminal, the principle block diagram of which can be as follows: Figure 6 As shown.

[0118] The terminal includes: a processor, a memory, an interface, a display screen, and a communication module connected via a system bus; wherein, the processor of the terminal provides computing and control capabilities; the memory of the terminal includes a computer-readable storage medium and internal memory; the computer-readable storage medium stores an operating system and computer programs; the internal memory provides an environment for the operation of the operating system and computer programs in the computer-readable storage medium; the interface is used to connect to external devices; the display screen is used to display relevant information; and the communication module is used to communicate with a cloud server or other devices.

[0119] When executed by the processor, this computer program is used to implement a real-time self-velocity estimation and odometry trajectory prediction method based on FMCW lidar.

[0120] It will be understood by those skilled in the art that Figure 6 The schematic diagram shown is merely a partial structural diagram related to the present invention and does not constitute a limitation on the terminal to which the present invention is applied. A specific terminal may include more or fewer components than those shown in the figure, or combine certain components, or have different component arrangements.

[0121] In one embodiment, a terminal is provided, comprising: a processor and a memory, the memory storing a real-time self-velocity estimation and odometry trajectory prediction program based on FMCW lidar, the real-time self-velocity estimation and odometry trajectory prediction program based on FMCW lidar being executed by the processor to implement the operation of the above-described real-time self-velocity estimation and odometry trajectory prediction method based on FMCW lidar.

[0122] In one embodiment, a computer-readable storage medium is provided, wherein the computer-readable storage medium stores a real-time self-velocity estimation and odometry trajectory prediction program based on an FMCW lidar, which, when executed by a processor, is used to implement the operation of the above-described real-time self-velocity estimation and odometry trajectory prediction method based on an FMCW lidar.

[0123] Those skilled in the art will understand that all or part of the processes in the methods of the above embodiments can be implemented by a computer program instructing related hardware. The computer program can be stored in a non-volatile storage medium, and when executed, it can include the processes of the embodiments of the methods described above. Any references to memory, storage, database, or other media used in the embodiments provided by this invention can include both non-volatile and volatile memory.

[0124] In summary, this invention provides a method and system for real-time self-velocity estimation and odometry trajectory prediction based on FMCW lidar, including: constructing an observation equation between the radial velocity of static environmental points and the sensor's own velocity; obtaining the sensor's own velocity and Doppler bias through robust estimation; calculating the Doppler residual based on the sensor's own velocity and the Doppler bias; separating static points and dynamic points based on the Doppler residual; performing motion distortion correction processing on the scanned point cloud based on the sensor's own velocity and angular velocity priors; performing scan-to-map registration based on the high-confidence static point cloud and the local map; and outputting real-time sensor self-velocity, odometry pose, and the local map. This invention improves the accuracy of initial motion estimation, reduces the interference of dynamic points on registration, reduces registration errors caused by scan distortion, and meets the real-time operational requirements of robots, autonomous vehicles, low-altitude drones, and inspection equipment.

[0125] It should be understood that the application of the present invention is not limited to the examples above. Those skilled in the art can make improvements or modifications based on the above description, and all such improvements and modifications should fall within the protection scope of the appended claims.

Claims

1. A method for real-time self-velocity estimation and odometry trajectory prediction based on FMCW lidar, characterized in that, include: An observation equation is constructed between the radial velocity of a static environmental point and the sensor's own velocity. The sensor's own velocity and Doppler bias are obtained through robust estimation. The Doppler residual is calculated based on the sensor's own velocity and the Doppler bias, and the static point and dynamic point are separated according to the Doppler residual. Based on the prior velocity and angular velocity of the sensor itself, motion distortion correction processing is performed on the scanned point cloud; Based on high-confidence static point cloud and local map, scanning and map registration are performed, and the sensor's own velocity, odometry pose and local map are output in real time.

2. The method for real-time self-velocity estimation and odometry trajectory prediction based on FMCW lidar according to claim 1, characterized in that, The construction of the observation equation between the radial velocity of the static environment point and the sensor's own velocity, and the robust estimation to obtain the sensor's own velocity and Doppler bias, includes: Collect point cloud data and preprocess the point cloud data; wherein, each point in the point cloud data includes three-dimensional coordinates, radial velocity, sampling time or relative scan time, validity flag, intensity, SNR, line number and echo sequence number; Based on the preprocessed point cloud data, an observation equation is constructed between the radial velocity of the static environment point and the velocity of the sensor itself within a single frame or a preset time window. The sensor's own velocity and Doppler bias in the lidar coordinate system are obtained through robust estimation.

3. The method for real-time self-velocity estimation and odometry trajectory prediction based on FMCW lidar according to claim 2, characterized in that, The process of obtaining the sensor's own velocity and Doppler bias in the lidar coordinate system through robust estimation includes: The sensor's own velocity and Doppler bias are solved by the following robust estimation: ； in, These represent the sensor's own velocity and Doppler bias, respectively. These represent the optimized sensor velocity and Doppler bias, respectively. This represents the weight of the i-th point; (.) indicates robust loss; Represents a static environment point; This represents the standard deviation of the radial velocity noise at the i-th point; This indicates the velocity of the previous frame, IMU integration, flight control estimation, or zero velocity prior. This represents the slowly varying prior of the Doppler bias.

4. The method for real-time self-velocity estimation and odometry trajectory prediction based on FMCW lidar according to claim 1, characterized in that, The step of calculating the Doppler residual based on the sensor's own velocity and the Doppler bias, and separating the static point and the dynamic point based on the Doppler residual, includes: The radial velocity residual of each point relative to the static model is calculated based on the sensor's own velocity and the Doppler bias to obtain the Doppler residual; Based on the Doppler residual, the static probability of each point is determined by a hard threshold determination method or a soft probability calculation method, and the static point and the dynamic point are separated based on the static probability of each point. The hard threshold in the hard threshold determination method is either a fixed threshold or a threshold adaptively determined based on velocity noise, distance, SNR, incident angle, scan line, and historical residual distribution. The soft probability in the soft probability calculation method is the probability calculated based on the geometric consistency probability and temporal consistency probability of each point.

5. The method for real-time self-velocity estimation and odometry trajectory prediction based on FMCW lidar according to claim 1, characterized in that, The motion distortion correction processing of the scanned point cloud based on the estimated sensor's own velocity and angular velocity priors includes: The platform's motion speed is calculated based on the sensor's own speed and the prior angular velocity. The time difference is calculated based on the scanning reference time and the sampling time of the corresponding point. A distortion correction model is constructed based on the platform's motion speed, the time difference, and the original coordinates of the point. The distortion correction model is used to perform distortion correction on the scanned point cloud. Local geometric consistency is recalculated on the distorted point cloud, and the static point weights are updated to obtain the distorted static points.

6. The method for real-time self-velocity estimation and odometry trajectory prediction based on FMCW lidar according to claim 1, characterized in that, The process of scanning and registering the sensor with a high-confidence static point cloud and a local map, and outputting real-time sensor velocity, odometry pose, and a local map, includes: Construct the local map; wherein, each map cell in the local map stores information such as the number of points, centroid, normal vector, covariance, timestamp, static confidence level, and number of observations; For a static point after distortion removal, the nearest plane, point element, or NDT element is found in the local map to obtain the corresponding centroid and normal vector. Based on the centroid and the normal vector, calculate the residual from the distortion-free static point to the nearest plane; Based on the residuals, static probabilities, and geometric robust loss, the odometry pose of the distortion-free static points is calculated using a robust optimization estimation method; wherein, the robust optimization estimation method includes any one of the following: Gauss-Newton algorithm, Levenberg-Marquardt method, iterative nearest point, NDT optimization, and factor graph incremental optimization; The output includes optimized sensor velocity, odometer pose, and local map.

7. The method for real-time self-velocity estimation and odometry trajectory prediction based on FMCW lidar according to claim 6, characterized in that, The process involves scanning and registering the data with a high-confidence static point cloud and a local map, outputting real-time sensor velocity, odometry pose, and a local map. This is followed by: The geometric velocity is obtained by dividing the pose translation between the current frame and the previous frame by the time interval. The geometric velocity is then compared with the sensor's own velocity obtained by robust estimation. If the difference between the two is large, the sensor's own velocity is optimized by adding a degradation flag, reducing the map update weight, or triggering a re-estimation.

8. A real-time self-velocity estimation and odometry trajectory prediction system based on FMCW lidar, characterized in that, include: The point cloud acquisition and preprocessing module is used to acquire point cloud data and preprocess the point cloud data. The robust estimation module is used to construct the observation equation between the radial velocity of a static environmental point and the sensor's own velocity, and obtains the sensor's own velocity and Doppler bias through robust estimation. The static / dynamic point separation module is used to calculate the Doppler residual based on the sensor's own velocity and the Doppler bias, and to separate static points and dynamic points according to the Doppler residual; The distortion correction module is used to perform motion distortion correction processing on the scanned point cloud based on the sensor's own velocity and angular velocity prior. The static point cloud to map registration module is used to perform scan-to-map registration based on high-confidence static point clouds and local maps, and outputs real-time sensor speed, odometry pose and local map.

9. A terminal, characterized in that, include: The processor and memory, wherein the memory stores a real-time self-velocity estimation and odometry trajectory prediction program based on FMCW lidar, and the real-time self-velocity estimation and odometry trajectory prediction program based on FMCW lidar, when executed by the processor, is used to implement the operation of the real-time self-velocity estimation and odometry trajectory prediction method based on FMCW lidar as described in any one of claims 1-7.

10. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a real-time self-velocity estimation and odometer trajectory prediction program based on FMCW lidar. When executed by a processor, the real-time self-velocity estimation and odometer trajectory prediction program based on FMCW lidar is used to implement the operation of the real-time self-velocity estimation and odometer trajectory prediction method based on FMCW lidar as described in any one of claims 1-7.

Images