A high-precision positioning and mapping method, system, and storage medium based on FMCW speed

Abstract

This invention discloses a high-precision positioning and mapping method, system, and storage medium based on FMCW velocity, comprising: S2, acquiring point cloud data and inertial data; S3, estimating the FMCW velocity of the corresponding frame's moving platform; and S4, performing pre-integration fusion to obtain the first... k +1 frame relative to the first k FMCW-IMU pre-integration model for frame motion; S4, obtain the first frame. k Frame and the k +1 frames of residual term; S5, minimize the residual term between frames. k Frame and the k S6. Sum of the squares of each term in the residual term between +1 frames to obtain the maximum a posteriori estimate of the corresponding frame; k The maximum a posteriori estimate of the frame is obtained. k +1 frame of pose parameters to form the first k Frame to the k +1 frame of transformation matrix, and the newly acquired first frame k The point cloud data of frame +1 is transformed using a transformation matrix and then added to the map to complete the reconstruction. This method can solve the problem of accumulated errors caused by inertial navigation drift, and improve the positioning accuracy and robustness of the map.

Description

Technical Field

[0001] This invention belongs to the field of simultaneous localization and mapping technology, specifically relating to a high-precision localization and mapping method, system, and storage medium based on FMCW speed. Background Technology

[0002] Simultaneous Localization and Mapping (SLAM) is a core challenge in robotics and measurement, and a crucial foundation for intelligent perception in unmanned systems. Currently, localization and navigation technologies primarily rely on the Global Positioning Satellite System (GNSS). However, GNSS performance is insufficient in signal-constrained environments (such as indoors, tunnels, and jungles), making it difficult to meet the demands of complex scenarios. In contrast, SLAM technology can autonomously perceive environmental information through sensors without relying on external signals, achieving high-precision pose estimation and map building. This makes SLAM technology key to robot navigation and path planning.

[0003] The core of SLAM technology lies in real-time estimation of sensor pose while simultaneously constructing a map model of the environment. Pose estimation includes both position and orientation, and its accuracy directly determines the system's localization capability. With the continuous advancement of sensor technology, SLAM is developing towards multi-sensor fusion. For example, current SLAM systems improve system robustness and accuracy by fusing multiple sensors (such as IMU, LiDAR, gyroscope, accelerometer, camera, and wheel velocity sensor).

[0004] However, existing SLAM technologies generally rely on IMUs (Inertial Measurement Units) for pose estimation, requiring time integration of the outputs from gyroscopes and accelerometers to obtain pose information. During integration, errors accumulate over time, eventually leading to significant orientation shifts. Furthermore, while wheel speed meters can directly measure wheel speed, they are unsuitable for non-wheeled platforms and require specific installation space, further limiting their application. Meanwhile, FMCW (Frequency Modulated Continuous Wave) LiDAR, as a cutting-edge LiDAR technology, has become a research hotspot due to its ability to provide richer environmental information. However, SLAM framework algorithms based on FMCW-LiDAR are currently lacking. Therefore, how to fully utilize the advantages of FMCW-LiDAR and develop an effective algorithm framework in SLAM systems to achieve higher positioning accuracy and robustness remains a pressing challenge. Summary of the Invention

[0005] The purpose of this invention is to address the above-mentioned problems by proposing a high-precision positioning and mapping method, system, and storage medium based on FMCW velocity, which can solve the problem of cumulative error caused by inertial navigation drift and improve the positioning accuracy and robustness of mapping.

[0006] To achieve the above objectives, the technical solution adopted by the present invention is as follows:

[0007] This invention proposes a high-precision positioning and mapping method based on FMCW velocity. The mobile platform includes an FMCW lidar and an inertial sensor. The high-precision positioning and mapping method based on FMCW velocity includes the following steps:

[0008] S1. Point cloud data and inertial data are collected in real time by FMCW lidar and inertial sensor respectively. The inertial data includes acceleration and angular velocity.

[0009] S2, according to the first k The frame point cloud data and objective function estimate the FMCW speed of the corresponding frame mobile platform. The objective function is constructed based on nonlinear least squares method and loss function.

[0010] S3, the first k The FMCW velocity and inertial data of the frame-based mobile platform are pre-integrated and fused to obtain the first... k +1 frame relative to the first k FMCW-IMU pre-integration model for frame motion;

[0011] S4, according to the first k Frame point cloud data, the first k FMCW speed and the first frame mobile platform k +1 frame relative to the first k FMCW-IMU pre-integration model for frame motion obtains the first frame. k Frame and the k The residual term between +1 frames, the first k Frame and the k The residual terms between +1 frames include the first... k The point-to-plane residuals of all points in the frame point cloud data, and the first k Frame and the k +1 frame of FMCW-IMU pre-integration model residuals, steering angle observation residuals, and velocity observation residuals;

[0012] S5, Minimize the first k Frame and the k The sum of the squares of each term in the residual term between +1 frames is used to obtain the maximum a posteriori estimate of the corresponding frame.

[0013] S6, according to the first k The maximum a posteriori estimate of the frame is obtained. k+1 frame of pose parameters to form the first k Frame to the k +1 frame of transformation matrix, and the newly acquired first frame k The point cloud data of frame +1 is transformed by a transformation matrix and then added to the map to complete the reconstruction.

[0014] Preferably, according to the first k The frame point cloud data and objective function estimate correspond to the FMCW speed of the mobile platform in the frame, as detailed below:

[0015] S21, the first k Frame point cloud data mapped to a two-dimensional matrix;

[0016] S22. Calculate the absolute residual between the theoretical Doppler velocity of each element point in the two-dimensional matrix and the measured Doppler velocity in the point cloud data. and the absolute residual Element points that are less than a preset threshold are recorded as reliable points, which means they are considered to meet the static condition.

[0017] S24. Obtain the corresponding objective function. k Three-dimensional velocity components of the frame motion platform Calculate the first k FMCW speed of frame-moving platforms The formula is as follows:

[0018]

[0019] In the formula, Indicates the first k The X-axis velocity component of the time-of-frame moving platform in the FMCW lidar coordinate system. Indicates the first k The Y-axis velocity component of the mobile platform in the FMCW lidar coordinate system at frame time. Indicates the first k The Z-axis velocity component of the time-of-frame moving platform in the FMCW lidar coordinate system. k , where OXYZ is a positive integer, and O is the origin of the FMCW lidar coordinate system.

[0020] Preferably, absolute residual The formula is as follows:

[0021]

[0022] in,

[0023]

[0024]

[0025] In the formula, This represents the theoretical Doppler velocity, that is, the Doppler velocity under stationary conditions. This represents the measured Doppler velocity in the point cloud data. dist This represents the Euclidean distance between the element point and the origin of the FMCW lidar coordinate system. x This represents the X-axis coordinate of the element point in the FMCW lidar coordinate system. y This represents the Y-axis coordinate of the element point in the FMCW lidar coordinate system. z This represents the Z-axis coordinate of the element point in the FMCW lidar coordinate system. This represents the X-axis velocity component of the mobile platform in the FMCW lidar coordinate system. This represents the Y-axis velocity component of the mobile platform in the FMCW lidar coordinate system. This represents the Z-axis velocity component of the mobile platform in the FMCW lidar coordinate system;

[0026] The objective function formula is as follows:

[0027]

[0028] In the formula, Represents the loss function. r i Indicates the first i The absolute residuals at each credible point i =1~ N , N Indicates the number of credible points. δ This represents the scaling parameter of the loss function, which is the Cauchy loss function.

[0029] Preferably, the absolute residual between the theoretical Doppler velocity of each element point in the two-dimensional matrix and the measured Doppler velocity in the point cloud data is calculated. Before that, the elements in the two-dimensional matrix are also filtered out, as follows:

[0030] S221. Perform multiple random samplings on each row of the two-dimensional matrix, and calculate the Euclidean distance between each randomly sampled element and the origin of the FMCW lidar coordinate system.

[0031] S222. Remove element points whose Euclidean distance is less than a preset distance, and calculate the absolute residual between the theoretical Doppler velocity and the measured Doppler velocity in the point cloud data for each retained element point. .

[0032] Preferably, minimize the first k Frame and the k The sum of the squares of the residual terms between frames +1 is used to obtain the maximum a posteriori estimate of the corresponding frame.k Maximum a posteriori estimation of frames The formula is as follows:

[0033]

[0034] in, Indicates Huber core, Represents the norm, Indicates the first k Frame point p Residual to the plane , Indicates the first k The set of points in a frame point cloud data. Indicates the first k Frame and the k +1 frame of FMCW-IMU pre-integral model residuals Indicates the first k Frame and the k +1 frame of steering angle observation residual, Indicates the first k Frame and the k +1 frame velocity observation residual.

[0035] Preferably, the first k +1 frame relative to the first k The FMCW-IMU pre-integration model for frame motion is represented as follows:

[0036]

[0037]

[0038]

[0039]

[0040] In the formula, Indicates the mobile platform in the IMU coordinate system. k Frame to the k +1 frame IMU position pre-integration Indicates the mobile platform in the IMU coordinate system. k Frame to the k +1 frame IMU velocity pre-integration Indicates the mobile platform in the IMU coordinate system. k Frame to the k +1 frame of IMU rotation pre-integration Indicates the mobile platform in the IMU coordinate system. k Frame to the k +1 frame FMCW velocity pre-integration Indicates the inertial sensor in the first...k The frame rate, i.e., the rate determined by the inertial sensor in the first frame... k The acceleration is accumulated and integrated before the frame. Represents gravitational acceleration in the world coordinate system. Indicates the first k Frame to the k +1 frame interval time, The first data collected by the inertial sensor k Frame acceleration, and Without gravitational acceleration, The first data collected by the inertial sensor k Frame angular velocity, The first data collected by the inertial sensor k Frame acceleration bias, The first data collected by the inertial sensor k The frame angular velocity offset, exp(·) represents the exponential mapping. Indicates the first k The transformation matrix from the time-of-frame FMCW lidar coordinate system to the IMU coordinate system. Indicates the first k FMCW speed of the frame-moving platform.

[0041] Preferably, the first k The point-to-plane residuals of all points in the frame point cloud data, and the first k Frame and the k The residuals of the FMCW-IMU pre-integration model, the steering angle observation residuals, and the velocity observation residuals for +1 frame are calculated as follows:

[0042] 1) No. k Point-to-plane residuals of all points in the frame point cloud data:

[0043] For the first k Calculate the residual from the point to the plane corresponding to each point in the frame point cloud data, then the point... p residuals to the plane The calculation is as follows:

[0044]

[0045]

[0046]

[0047]

[0048]

[0049]

[0050] In the formula, Point p The centroid of the nearest neighbor set, express The j The coordinates of the nearest points Represents a point projected onto the world coordinate system. p coordinates j =1~ M, M Point p The number of neighboring points, and using the point p Priority queues are selected based on Euclidean distance from smallest to largest. M Neighboring points, Indicates the first j The nearest neighbor points relative to The bias, T Indicates transpose. C Represents the covariance matrix. Indicates flatness index, express M Transpose of the fitting plane of the nearest neighbor points express Distance to the fitted plane, Indicates the first standard deviation. Indicates the second standard deviation. Indicates the third standard deviation. Represents the largest eigenvalue. Indicates intermediate feature values. This represents the smallest eigenvalue, and , , From the covariance matrix C Obtained by eigenvalue decomposition;

[0051] 2) No. k Frame and the k FMCW-IMU pre-integral model residuals at +1 frame :

[0052]

[0053] In the formula, Indicates the first k Transformation matrix from frame-time IMU coordinate system to world coordinate system Indicates the first k +1 frame's displacement in world coordinates Indicates the first k The displacement of the mobile platform in the world coordinate system in a frame. Indicates the first k +1 frame IMU speed of the mobile platform in world coordinates Indicates the firstk The IMU speed of the mobile platform in the world coordinate system at frame rate. Indicates the first k +1 frame: the pose quaternion of the mobile platform in world coordinates. Indicates the first k pose quaternions of a frame-time mobile platform in the world coordinate system The inverse operation, express The inverse operation, Indicates cross product. Indicates the first k The transformation matrix from the inertial coordinate system to the world coordinate system at frame +1. The first data collected by the inertial sensor k +1 frame acceleration bias, The first data collected by the inertial sensor k +1 frame angular velocity offset;

[0054] 3) No. k Frame and the k +1 frame of steering angle observation residual :

[0055]

[0056] In the formula, Indicates the first k pose quaternions of a frame-time mobile platform in the world coordinate system The converted yaw angle Indicates the first k pose quaternions of a frame-time mobile platform in the world coordinate system The resulting pitch angle This indicates the yaw angle obtained by the FMCW lidar. This indicates the elevation angle obtained by the FMCW lidar;

[0057] 4) No. k Frame and the k +1 frame velocity observation residual :

[0058]

[0059] In the formula, Indicates the first k Frame and the k+ The residual of FMCW speed on a 1-frame mobile platform Indicates the first k +1 frame and the k The residual of FMCW speed on the frame-moving platform Indicates the first k+1 frame FMCW speed on mobile platforms Indicates the first k The transformation matrix from the frame-time inertial coordinate system to the world coordinate system.

[0060] Preferably, the first k Before calculating the point-to-plane residuals of all points in the frame point cloud data, the following operations are performed:

[0061] For the k The frame point cloud data is downsampled to form a cube with a preset voxel size, and a preset number of points are taken in each cube, and the downsampled points are evenly distributed in three-dimensional space.

[0062] Calculate the residual from the point to the plane after traversing all downsampled points.

[0063] A high-precision positioning and mapping system based on FMCW velocity, based on any of the aforementioned high-precision positioning and mapping methods based on FMCW velocity, includes a data acquisition module, a data processing module, and a map generation module. The data processing module includes an FMCW velocity estimation module, a pre-integration model generation module, a residual calculation module, and a maximum a posteriori estimation generation module, wherein:

[0064] The data acquisition module is used to collect point cloud data and inertial data in real time through FMCW lidar and inertial sensor respectively. The inertial data includes acceleration and angular velocity.

[0065] The FMCW velocity estimation module is used to estimate the velocity based on the first... k The frame point cloud data and objective function estimate the FMCW speed of the corresponding frame mobile platform. The objective function is constructed based on nonlinear least squares method and loss function.

[0066] The pre-integral model generation module is used to generate the first... k The FMCW velocity and inertial data of the frame-based mobile platform are pre-integrated and fused to obtain the first... k +1 frame relative to the first k FMCW-IMU pre-integration model for frame motion;

[0067] The residual calculation module is used to calculate the residual based on the first... k Frame point cloud data, the first k FMCW speed and the first frame mobile platform k +1 frame relative to the first k FMCW-IMU pre-integration model for frame motion obtains the first frame. k Frame and the k The residual term between +1 frames, the first k Frame and the k The residual terms between +1 frames include the first... kThe point-to-plane residuals of all points in the frame point cloud data, and the first k Frame and the k +1 frame of FMCW-IMU pre-integration model residuals, steering angle observation residuals, and velocity observation residuals;

[0068] Maximum a posteriori estimation generation module, used to minimize the 1st... k Frame and the k The sum of the squares of each term in the residual term between +1 frames is used to obtain the maximum a posteriori estimate of the corresponding frame.

[0069] The map generation module is used to generate maps based on the first... k The maximum a posteriori estimate of the frame is obtained. k +1 frame of pose parameters to form the first k Frame to the k +1 frame of transformation matrix, and the newly acquired first frame k The point cloud data of frame +1 is transformed by a transformation matrix and then added to the map to complete the reconstruction.

[0070] A high-precision positioning and mapping storage medium based on FMCW speed is provided for storing computer programs. When the computer programs are executed by a processor, they implement any of the aforementioned high-precision positioning and mapping methods based on FMCW speed.

[0071] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0072] This method acquires point cloud data and inertial data using FMCW lidar and inertial sensors. Based on the point cloud data and an objective function, it estimates the FMCW velocity of the mobile platform and obtains the corresponding FMCW-IMU pre-integration model and residual terms. By minimizing the sum of squares of each term in the residual terms, it obtains the maximum a posteriori (MAP) estimate for the corresponding frame. Based on the MAP estimate, it obtains the corresponding pose parameters, which are then transformed using a transformation matrix and added to the map to complete map reconstruction. This method fuses point cloud data and inertial data, significantly reducing the cumulative displacement error caused by double integration in traditional inertial sensors. Furthermore, the FMCW lidar does not rely on wheel velocity meters and can directly infer its own motion from Doppler velocity. It is applicable to various mobile platforms, including wheeled platforms, or non-wheeled platforms such as drones and robotic dogs. It achieves long-term stable velocity estimation, effectively solves inertial navigation drift, and is easy to install, enabling the SLAM system to achieve higher positioning accuracy and robustness in complex environments. Attached Figure Description

[0073] Figure 1 This is a flowchart of the high-precision positioning and mapping method based on FMCW velocity of the present invention;

[0074] Figure 2 This is a mapping effect diagram of the high-precision positioning and mapping method based on FMCW velocity of the present invention;

[0075] Figure 3 This is a comparison diagram of the trajectory and true value of the high-precision positioning and mapping method based on FMCW velocity of the present invention;

[0076] Figure 4 This is a schematic diagram of the high-precision positioning and mapping system based on FMCW speed according to the present invention. Detailed Implementation

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

[0078] It should be noted that, unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art. The terminology used herein is for the purpose of describing particular embodiments only and is not intended to limit the scope of the application.

[0079] Example 1:

[0080] like Figures 1-3 As shown, a high-precision positioning and mapping method, system, and storage medium based on FMCW velocity are disclosed. The mobile platform includes an FMCW lidar and an inertial sensor. The high-precision positioning and mapping method based on FMCW velocity includes the following steps:

[0081] S1. Point cloud data and inertial data are collected in real time by FMCW lidar and inertial sensor respectively. The inertial data includes acceleration and angular velocity.

[0082] The system simultaneously collects point cloud data and inertial data of the surrounding area using FMCW lidar and inertial sensor equipment, and performs preprocessing such as filtering and noise reduction. The point cloud data includes Doppler velocity, and the inertial data includes acceleration and angular velocity.

[0083] The IMU coordinate system in this application is a local coordinate system established on the inertial sensor body, and the FMCW lidar coordinate system is a local coordinate system established on the FMCW lidar body. The local coordinate system can be established with any direction as the coordinate axis, and the world coordinate system is an absolute coordinate system used to describe the position and orientation in a fixed environment.

[0084] S2, according to the first k The frame point cloud data and objective function estimate the FMCW speed of the corresponding frame mobile platform. The objective function is constructed based on nonlinear least squares method and loss function.

[0085] In one embodiment, according to the first k The frame point cloud data and objective function estimate correspond to the FMCW speed of the mobile platform in the frame, as detailed below:

[0086] S21, the first k Frame point cloud data mapped to a two-dimensional matrix;

[0087] S22. Calculate the absolute residual between the theoretical Doppler velocity of each element point in the two-dimensional matrix and the measured Doppler velocity in the point cloud data. and the absolute residual Element points that are less than a preset threshold are recorded as reliable points, which means they are considered to meet the static condition.

[0088] S24. Obtain the corresponding objective function. k Three-dimensional velocity components of the frame motion platform Calculate the first k FMCW speed of frame-moving platforms The formula is as follows:

[0089]

[0090] In the formula, Indicates the first k The X-axis velocity component of the time-of-frame moving platform in the FMCW lidar coordinate system. Indicates the first k The Y-axis velocity component of the mobile platform in the FMCW lidar coordinate system at frame time. Indicates the first k The Z-axis velocity component of the time-of-frame moving platform in the FMCW lidar coordinate system. k , where OXYZ is a positive integer, and O is the origin of the FMCW lidar coordinate system.

[0091] This method maps point cloud data with Doppler velocity to a two-dimensional matrix and combines random sampling and nonlinear optimization to quickly and accurately estimate the self-velocity of the mobile platform (the FMCW velocity of the mobile platform). Each point in the point cloud data is mapped to the two-dimensional matrix based on its ring and column attributes (referring to the ring position and column position in the FMCW lidar scan, respectively). All points within a corresponding frame are traversed, and their unique positions in the two-dimensional matrix are determined based on their ring and column attributes. Specifically, the two-dimensional matrix has 60 rows, corresponding to the maximum number of rings (i.e., the number of scan layers) in the FMCW lidar scan, and 10,000 columns, representing the maximum number of points that can be collected on each ring. This two-dimensional matrix structure facilitates the effective organization and processing of point cloud data, and each element in the two-dimensional matrix can also include an index from the point cloud data.

[0092] In one embodiment, absolute residual The formula is as follows:

[0093]

[0094] in,

[0095]

[0096]

[0097] In the formula, This represents the theoretical Doppler velocity, that is, the Doppler velocity under stationary conditions. This represents the measured Doppler velocity in the point cloud data. dist This represents the Euclidean distance between the element point and the origin of the FMCW lidar coordinate system. x This represents the X-axis coordinate of the element point in the FMCW lidar coordinate system. y This represents the Y-axis coordinate of the element point in the FMCW lidar coordinate system. z This represents the Z-axis coordinate of the element point in the FMCW lidar coordinate system. This represents the X-axis velocity component of the mobile platform in the FMCW lidar coordinate system. This represents the Y-axis velocity component of the mobile platform in the FMCW lidar coordinate system. This represents the Z-axis velocity component of the mobile platform in the FMCW lidar coordinate system;

[0098] The objective function formula is as follows:

[0099]

[0100] In the formula, Represents the loss function. r i Indicates the first i The absolute residuals at each credible point i =1~ N , N Indicates the number of credible points. δ This represents the scaling parameter of the loss function, which is the Cauchy loss function.

[0101] The absolute residual is used to minimize the FMCW velocity estimate of the mobile platform, making the measured data close to the model prediction. This embodiment uses a nonlinear least squares method based on Ceres Solver for optimization, aiming to estimate the three-dimensional velocity components of the mobile platform. The objective function for optimization employs the Cauchy loss function. It is easy to understand that the loss function can also be one of the Huber loss function, SoftLone loss function, or Tukey loss function, and can be adjusted according to actual needs. δ The scaling parameter of the loss function is used to control the robustness to outliers.

[0102] In one embodiment, the absolute residual between the theoretical Doppler velocity of each element point in the two-dimensional matrix and the measured Doppler velocity in the point cloud data is calculated. Before that, the elements in the two-dimensional matrix are also filtered out, as follows:

[0103] S221. Perform multiple random samplings on each row of the two-dimensional matrix, and calculate the Euclidean distance between each randomly sampled element and the origin of the FMCW lidar coordinate system.

[0104] S222. Remove element points whose Euclidean distance is less than a preset distance, and calculate the absolute residual between the theoretical Doppler velocity and the measured Doppler velocity in the point cloud data for each retained element point. .

[0105] Before mapping to the two-dimensional data matrix, invalid and duplicate points can be filtered out. Multiple random samples are taken from each row of the two-dimensional matrix, and the Euclidean distance between that point and the origin of the FMCW lidar coordinate system is calculated. Points with an Euclidean distance less than 2 meters are discarded as interference points. Assuming the mobile platform is stationary, the absolute residual between the theoretical Doppler velocity and the measured Doppler velocity in the point cloud data is compared. If the residual is less than a preset threshold (set to 1), the point meets the stationary condition and is considered a reliable point, used as a sample for subsequent FMCW velocity estimation. Subsequently, the sum of squares of the absolute residuals is optimized using nonlinear least squares and a loss function to obtain the velocity components of the mobile platform. A stationary background is selected by filtering the Doppler velocity of the surrounding environment, and the three-dimensional velocity components are obtained through nonlinear optimization methods to calculate the corresponding FMCW velocity of the mobile platform.

[0106] S3, the first k The FMCW velocity and inertial data of the frame-based mobile platform are pre-integrated and fused to obtain the first... k +1 frame relative to the first k FMCW-IMU pre-integration model for frame motion.

[0107] In one embodiment, the first k +1 frame relative to the firstk The FMCW-IMU pre-integration model for frame motion is represented as follows:

[0108]

[0109]

[0110]

[0111]

[0112] In the formula, Indicates the mobile platform in the IMU coordinate system. k Frame to the k +1 frame IMU position pre-integration Indicates the mobile platform in the IMU coordinate system. k Frame to the k +1 frame IMU velocity pre-integration Indicates the mobile platform in the IMU coordinate system. k Frame to the k +1 frame of IMU rotation pre-integration Indicates the mobile platform in the IMU coordinate system. k Frame to the k +1 frame FMCW velocity pre-integration Indicates the inertial sensor in the first... k The frame rate, i.e., the rate determined by the inertial sensor in the first frame... k The acceleration is accumulated and integrated before the frame. Represents gravitational acceleration in the world coordinate system. Indicates the first k Frame to the k +1 frame interval time, The first data collected by the inertial sensor k Frame acceleration, and Without gravitational acceleration, The first data collected by the inertial sensor k Frame angular velocity, The first data collected by the inertial sensor k Frame acceleration bias, The first data collected by the inertial sensor k The frame angular velocity offset, exp(·) represents the exponential mapping. Indicates the first k The transformation matrix from the time-of-frame FMCW lidar coordinate system to the IMU coordinate system. Indicates the first k FMCW speed of the frame-moving platform.

[0113] S4, according to the firstk Frame point cloud data, the first k FMCW speed and the first frame mobile platform k +1 frame relative to the first k FMCW-IMU pre-integration model for frame motion obtains the first frame. k Frame and the k The residual term between +1 frames, the first k Frame and the k The residual terms between +1 frames include the first... k The point-to-plane residuals of all points in the frame point cloud data, and the first k Frame and the k +1 frame of FMCW-IMU pre-integration model residuals, steering angle observation residuals, and velocity observation residuals.

[0114] In one embodiment, the first k The point-to-plane residuals of all points in the frame point cloud data, and the first k Frame and the k The residuals of the FMCW-IMU pre-integration model, the steering angle observation residuals, and the velocity observation residuals for +1 frame are calculated as follows:

[0115] 1) No. k Point-to-plane residuals of all points in the frame point cloud data:

[0116] For the first k Calculate the residual from the point to the plane corresponding to each point in the frame point cloud data, then the point... p residuals to the plane The calculation is as follows:

[0117]

[0118]

[0119]

[0120]

[0121]

[0122]

[0123] In the formula, Point p The centroid of the nearest neighbor set, express The j The coordinates of the nearest points Represents a point projected onto the world coordinate system. p coordinates j =1~ M, M Pointp The number of neighboring points, and using the point p Priority queues are selected based on Euclidean distance from smallest to largest. M Neighboring points, Indicates the first j The nearest neighbor points relative to The bias, T Indicates transpose. C Represents the covariance matrix. Indicates flatness index, express M Transpose of the fitting plane of the nearest neighbor points express Distance to the fitted plane, Indicates the first standard deviation. Indicates the second standard deviation. Indicates the third standard deviation. Represents the largest eigenvalue. Indicates intermediate feature values. This represents the smallest eigenvalue, and , , From the covariance matrix C Obtained by eigenvalue decomposition;

[0124] 2) No. k Frame and the k FMCW-IMU pre-integral model residuals at +1 frame :

[0125]

[0126] In the formula, Indicates the first k Transformation matrix from frame-time IMU coordinate system to world coordinate system Indicates the first k +1 frame's displacement in world coordinates Indicates the first k The displacement of the mobile platform in the world coordinate system in a frame. Indicates the first k +1 frame IMU speed of the mobile platform in world coordinates Indicates the first k The IMU speed of the mobile platform in the world coordinate system at frame rate. Indicates the first k +1 frame: the pose quaternion of the mobile platform in world coordinates. Indicates the first k pose quaternions of a frame-time mobile platform in the world coordinate system The inverse operation, express The inverse operation, Indicates cross product. Indicates the first k The transformation matrix from the inertial coordinate system to the world coordinate system at frame +1. The first data collected by the inertial sensor k +1 frame acceleration bias, The first data collected by the inertial sensor k +1 frame angular velocity offset;

[0127] 3) No. k Frame and the k +1 frame of steering angle observation residual :

[0128]

[0129] In the formula, Indicates the first k pose quaternions of a frame-time mobile platform in the world coordinate system The converted yaw angle Indicates the first k pose quaternions of a frame-time mobile platform in the world coordinate system The resulting pitch angle This indicates the yaw angle obtained by the FMCW lidar. This indicates the elevation angle obtained by the FMCW lidar;

[0130] 4) No. k Frame and the k +1 frame velocity observation residual :

[0131]

[0132] In the formula, Indicates the first k Frame and the k+ The residual of FMCW speed on a 1-frame mobile platform Indicates the first k +1 frame and the k The residual of FMCW speed on the frame-moving platform Indicates the first k +1 frame FMCW speed on mobile platforms Indicates the first k The transformation matrix from the frame-time inertial coordinate system to the world coordinate system.

[0133] In one embodiment, the first k Before calculating the point-to-plane residuals of all points in the frame point cloud data, the following operations are performed:

[0134] For thek The frame point cloud data is downsampled to form a cube with a preset voxel size, and a preset number of points are taken in each cube, and the downsampled points are evenly distributed in three-dimensional space.

[0135] Calculate the residual from the point to the plane after traversing all downsampled points.

[0136] The point-to-plane residual is obtained by projecting each point in the point cloud data onto the fitting plane of the target point cloud data, calculating the distance from that point to the fitting plane, and obtaining the point-to-plane residual. Specifically, the input point cloud data is first downsampled by forming cubes with a voxel size of 0.3, and a preset number of points are selected from each cube to ensure that the downsampled points are evenly distributed in three-dimensional space, thus reducing the computational burden.

[0137] The downsampled points p Projecting onto the world coordinate system yields Because spatial data is divided into voxels and a priority queue is used (keeping the point with the smallest distance at the top of the queue), this enables the processing of data. Filtering neighboring points; such as selecting the previous point. M neighboring points (set to) M =20), calculate the centroid of this point set. c and the offset of each neighboring point relative to the centroid Establish the covariance matrix C For the covariance matrix C Eigenvalue decomposition is performed to obtain the flatness index. The value is between 0 and 1. , , Standard deviation , , These are the eigenvalues ​​of the decomposition. They are determined by the flatness index. To evaluate whether the point set is close to a plane. If A larger value indicates that the point set is closer to the plane; a smaller value indicates that the point set may not be flat. This is a measure of planarity; when point cloud data exhibits an ideal planar shape, this value will approach 1. The eigenvector with the largest eigenvalue is selected. n As the normal vector of the fitted plane.

[0138] By using point-to-plane residuals, FMCW-IMU pre-integral model residuals, steering angle observation residuals, and velocity observation residuals, the estimation accuracy of the mobile platform's position and velocity can be improved. The attitude quaternion consists of one real part and three imaginary parts, as shown in the... k pose quaternions of a frame-time mobile platform in the world coordinate system Represented as ,and and The calculation is as follows:

[0139]

[0140]

[0141] in, The real part indicates that the mobile platform is in the world coordinate system. k Frame and the k +1 frame of rotation angle cosine half angle, [ [ is the imaginary part] The corresponding coordinates represent the unit rotation axes of the mobile platform in the X', Y', and Z' directions in the world coordinate system, respectively, and the... k Frame and the k The product of the sine half-angle of the rotation angle of +1 frame. Compared to existing Euler angles, attitude quaternions avoid gimbal lock issues and do not suffer from degree-of-freedom loss during continuous rotations, resulting in high computational efficiency. The world coordinate system is represented as O'X'Y'Z'.

[0142] S5, Minimize the first k Frame and the k The sum of the squares of each term in the residual term between +1 frames is used to obtain the maximum a posteriori estimate of the corresponding frame.

[0143] In one embodiment, minimize the first k Frame and the k The sum of the squares of the residual terms between frames +1 is used to obtain the maximum a posteriori estimate of the corresponding frame. k Maximum a posteriori estimation of frames The formula is as follows:

[0144]

[0145] in, Indicates Huber core, Represents the norm, Indicates the first k Frame point p Residual to the plane , Indicates the first k The set of points in a frame point cloud data. Indicates the first k Frame and the k +1 frame of FMCW-IMU pre-integral model residuals Indicates the first k Frame and the k +1 frame of steering angle observation residual, Indicates the firstk Frame and the k +1 frame velocity observation residual.

[0146] S6, according to the first k The maximum a posteriori estimate of the frame is obtained. k +1 frame of pose parameters to form the first k Frame to the k +1 frame of transformation matrix, and the newly acquired first frame k The point cloud data of frame +1 is transformed by a transformation matrix and then added to the map to complete the reconstruction.

[0147] Once the maximum a posteriori (MAP) estimate is obtained, the i-th MAP can be directly extracted from the MAP estimate. k +1 frame pose parameters (such as including the first frame) k The pose parameters of frame +1 include the first frame. k +1 frame displacement of the moving platform in the world coordinate system and the k At frame +1, the pose quaternion of the mobile platform in the world coordinate system (Specific options can be selected based on actual needs), which can be based on the first... k The pose parameters of the frame and the first k The pose parameters of frame +1 are calculated to obtain the first frame. k Frame to the k The transformation matrix of frame +1 will be used to transform the newly acquired frame. k The point cloud data of frame +1 is transformed by a transformation matrix and then added to the map to complete the map reconstruction. This is a technique well-known to those skilled in the art and will not be described in detail here. Specifically, in the process of adding points from the point cloud data to the map, voxel filtering can also be used to downsample the point cloud data, retaining the most representative points within each voxel to reduce redundancy and maintain the compactness of the map. The specific process is the same as the downsampling process described above.

[0148] like Figure 2 The image shown is a mapping result created from the point cloud data collected in the industrial park during the experiment. Figure 3 The image shows a comparison between the trajectory formed by the point cloud data collected in the industrial park during the experiment and the true value. It can be seen that the proposed method is close to the true trajectory (GPS true value). Moreover, the RMSE (Root Mean Square Error) value of the proposed method is 2.45m, which is smaller and more accurate than existing technologies.

[0149] It should be understood that, although Figure 1 The steps in the flowchart are shown sequentially as indicated by the arrows, but these steps are not necessarily executed in the order indicated by the arrows. Unless otherwise specified herein, there is no strict order in which these steps are executed, and they can be performed in other orders. Figure 1 At least some of the steps in the process may include multiple sub-steps or multiple stages. These sub-steps or stages are not necessarily completed at the same time, but can be executed at different times. The execution order of these sub-steps or stages is not necessarily sequential, but can be executed in turn or alternately with other steps or at least some of the sub-steps or stages of other steps.

[0150] Example 2:

[0151] like Figure 4 As shown, a high-precision positioning and mapping system based on FMCW velocity is described. Based on any of the high-precision positioning and mapping methods based on FMCW velocity in Embodiment 1, the high-precision positioning and mapping system based on FMCW velocity includes a data acquisition module, a data processing module, and a map generation module. The data processing module includes an FMCW velocity estimation module, a pre-integration model generation module, a residual calculation module, and a maximum a posteriori estimation generation module, wherein:

[0152] The data acquisition module is used to collect point cloud data and inertial data in real time through FMCW lidar and inertial sensor respectively. The inertial data includes acceleration and angular velocity.

[0153] The FMCW velocity estimation module is used to estimate the velocity based on the first... k The frame point cloud data and objective function estimate the FMCW speed of the corresponding frame mobile platform. The objective function is constructed based on nonlinear least squares method and loss function.

[0154] The pre-integral model generation module is used to generate the first... k The FMCW velocity and inertial data of the frame-based mobile platform are pre-integrated and fused to obtain the first... k +1 frame relative to the first k FMCW-IMU pre-integration model for frame motion;

[0155] The residual calculation module is used to calculate the residual based on the first... k Frame point cloud data, the first k FMCW speed and the first frame mobile platform k +1 frame relative to the first k FMCW-IMU pre-integration model for frame motion obtains the first frame. k Frame and the k The residual term between +1 frames, the first k Frame and the k The residual terms between +1 frames include the first... k The point-to-plane residuals of all points in the frame point cloud data, and the first k Frame and the k +1 frame of FMCW-IMU pre-integration model residuals, steering angle observation residuals, and velocity observation residuals;

[0156] Maximum a posteriori estimation generation module, used to minimize the 1st... k Frame and the k The sum of the squares of each term in the residual term between +1 frames is used to obtain the maximum a posteriori estimate of the corresponding frame.

[0157] The map generation module is used to generate maps based on the first... k The maximum a posteriori estimate of the frame is obtained. k +1 frame of pose parameters to form the first k Frame to the k +1 frame of transformation matrix, and the newly acquired first frame k The point cloud data of frame +1 is transformed by a transformation matrix and then added to the map to complete the reconstruction.

[0158] This high-precision positioning and mapping system based on FMCW speed can be installed on a mobile platform or an external computer. It can be a standalone computer device or a component within a computer device, such as an integrated circuit or a chip. This computer device can be a terminal device or other types of devices. This high-precision positioning and mapping system based on FMCW speed can implement all the steps described in the embodiments of the positioning and mapping method based on FMCW speed and achieve the same technical effects. For the sake of brevity, the specific implementation details will not be repeated here.

[0159] It should be understood that specific limitations regarding a high-precision positioning and mapping system based on FMCW speed can be found in the limitations of a high-precision positioning and mapping method based on FMCW speed in the embodiments, and will not be repeated here. Each module in the aforementioned high-precision positioning and mapping system based on FMCW speed can be implemented entirely or partially through software, hardware, or a combination thereof. These modules can be embedded in or independent of the processor in a computer device in hardware form, or stored in the memory of a computer device in software form, so that the processor can call and execute the operations corresponding to each module.

[0160] Example 3:

[0161] A high-precision positioning and mapping storage medium based on FMCW speed is provided for storing computer programs. When the computer programs are executed by a processor, they implement any of the aforementioned high-precision positioning and mapping methods based on FMCW speed.

[0162] When the processor executes the computer program, it can implement all the steps described in the embodiment of the FMCW-based localization and mapping method and achieve the same technical effect. For the sake of brevity, the specific implementation details will not be repeated here.

[0163] It should be understood that the storage medium and the processor are electrically connected directly or indirectly to enable data transmission or interaction. For example, these components can be electrically connected to each other via one or more communication buses or signal lines. The storage medium stores a computer program that can run on the processor. The processor implements the high-precision positioning and mapping method based on FMCW speed in this embodiment of the invention by running the computer program stored in the storage medium.

[0164] The storage medium can be, but is not limited to, Random Access Memory (RAM), Read Only Memory (ROM), Programmable Read-Only Memory (PROM), Erasable Programmable Read-Only Memory (EPROM), and Electrically Erasable Programmable Read-Only Memory (EEPROM). The storage medium is used to store computer programs, and the processor executes the corresponding computer program after receiving execution instructions.

[0165] The processor may be an integrated circuit chip with data processing capabilities. The aforementioned processor can be a general-purpose processor, including a Central Processing Unit (CPU), a Network Processor (NP), etc. It can implement or execute the methods, steps, and logic block diagrams disclosed in the embodiments of this invention. The general-purpose processor can be a microprocessor or any conventional processor.

[0166] The technical features of the above embodiments can be combined in any way. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0167] The embodiments described above are merely specific and detailed examples of the embodiments described in this application, and should not be construed as limiting the scope of the application. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of this application, and these modifications and improvements all fall within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the appended claims.

Claims

1. A high-precision positioning mapping method based on FMCW speed, applied to a mobile platform, characterized in that: The mobile platform comprises a FMCW laser radar and an inertial sensor, and the high-precision positioning mapping method based on FMCW velocity comprises the following steps: S1, real-time corresponding point cloud data and inertial data are collected by the FMCW laser radar and the inertial sensor respectively, and the inertial data comprises acceleration and angular velocity; S2, the FMCW velocity of the corresponding frame of the mobile platform is estimated according to the kth frame of point cloud data and a target function, and the target function is constructed based on a nonlinear least square method and a loss function; S3, the FMCW-IMU pre-integration model of the movement of the k+1th frame relative to the kth frame is obtained by pre-integrating the FMCW velocity of the kth frame of the mobile platform and the inertial data; The FMCW-IMU pre-integration model of the movement of the k+1th frame relative to the kth frame is represented as follows: ； ； ； ； wherein, denotes the IMU position pre-integration of the mobile platform from the k-th frame to the k+1-th frame in the IMU coordinate system, denotes the IMU velocity pre-integration of the mobile platform from the k-th frame to the k+1-th frame in the IMU coordinate system, denotes the IMU rotation pre-integration of the mobile platform from the k-th frame to the k+1-th frame in the IMU coordinate system, denotes the FMCW velocity pre-integration of the mobile platform from the k-th frame to the k+1-th frame in the IMU coordinate system, denotes the velocity of the inertial sensor at the k-th frame, i.e., obtained by the cumulative integration of the acceleration of the inertial sensor before the k-th frame, denotes the gravity acceleration in the world coordinate system, denotes the interval time from the k-th frame to the k+1-th frame, denotes the k-th frame acceleration collected by the inertial sensor, and not containing the gravity acceleration, denotes the k-th frame angular velocity collected by the inertial sensor, denotes the bias of the k-th frame acceleration collected by the inertial sensor, denotes the bias of the k-th frame angular velocity collected by the inertial sensor, exp(·) denotes the exponential mapping, denotes the conversion matrix from the FMCW lidar coordinate system to the IMU coordinate system at the k-th frame, denotes the FMCW velocity of the mobile platform at the k-th frame; S4, the residual term between the kth frame and the k+1th frame is obtained according to the kth frame of point cloud data, the FMCW velocity of the kth frame of the mobile platform and the FMCW-IMU pre-integration model of the movement of the k+1th frame relative to the kth frame, and the residual term between the kth frame and the k+1th frame comprises the point-to-plane residual of all points of the kth frame of point cloud data, and the FMCW-IMU pre-integration model residual, the steering angle observation residual and the velocity observation residual of the kth frame and the k+1th frame; S5, the sum of squares of each term in the residual term between the kth frame and the k+1th frame is minimized to obtain the maximum a posteriori estimation of the corresponding frame; S6, the pose parameters of the k+1th frame are obtained according to the maximum a posteriori estimation of the kth frame to form the conversion matrix from the kth frame to the k+1th frame, and the newly obtained point cloud data of the k+1th frame is converted by the conversion matrix and added to the map to complete the reconstruction. 2.The FMCW speed-based high-precision positioning mapping method of claim 1, wherein: The FMCW velocity of the corresponding frame of the mobile platform is estimated according to the kth frame of point cloud data and a target function, and the specific steps are as follows: S21, the kth frame of point cloud data is mapped to a two-dimensional matrix; S22, calculate the absolute residual between the theoretical Doppler velocity of each element point in the two-dimensional matrix and the measured Doppler velocity in the point cloud data , and the element points with the absolute residual less than the preset threshold are recorded as trusted points, that is, considered to meet the static condition; S24, obtaining the three-dimensional velocity component of the kth frame of the mobile platform according to the target function , calculating the FMCW velocity of the kth frame of the mobile platform , the formula is as follows: ； In the formula, represents the X-axis velocity component of the mobile platform in the FMCW laser radar coordinate system at the kth frame, represents the Y-axis velocity component of the mobile platform in the FMCW laser radar coordinate system at the kth frame, represents the Z-axis velocity component of the mobile platform in the FMCW laser radar coordinate system at the kth frame, k is a positive integer, OXYZ is the FMCW laser radar coordinate system, and O is the origin of the FMCW laser radar coordinate system. 3.The FMCW speed-based high-precision positioning mapping method of claim 2, wherein: The absolute residual error The formula is as follows: ； Wherein, ； ； In the formula, represents the theoretical Doppler velocity, i.e., the Doppler velocity under the static condition, represents the measured Doppler velocity in the point cloud data, dist represents the Euclidean distance of the element point from the origin of the FMCW laser radar coordinate system, x represents the X-axis coordinate value of the element point under the FMCW laser radar coordinate system, y represents the Y-axis coordinate value of the element point under the FMCW laser radar coordinate system, and z represents the Z-axis coordinate value of the element point under the FMCW laser radar coordinate system, represents the X-axis velocity component of the mobile platform under the FMCW laser radar coordinate system, represents the Y-axis velocity component of the mobile platform under the FMCW laser radar coordinate system, represents the Z-axis velocity component of the mobile platform under the FMCW laser radar coordinate system; The target function formula is as follows: ； In the formula, represents a loss function, r i represents the absolute residual of the i th trusted point, i =1~N, N represents the number of trusted points, and δ represents a scale parameter of the loss function, which is a Cauchy loss function. 4.The FMCW speed-based high-precision positioning mapping method of claim 2, wherein: The absolute residual between the theoretical Doppler velocity of each element point in the two-dimensional matrix and the measured Doppler velocity in the point cloud data is calculated Before that, the element points in the two-dimensional matrix are screened out, specifically as follows: S221, each row of element points of the two-dimensional matrix is randomly sampled multiple times, and the Euclidean distance of each randomly sampled element point from the origin of the FMCW laser radar coordinate system is calculated; S222, screen out the element points with the Euclidean distance less than the preset distance, calculate the absolute residual error between the theoretical Doppler velocity of each reserved element point and the measured Doppler velocity in the point cloud data . 5.The FMCW speed-based high-precision positioning mapping method of claim 1, wherein: The minimum of the sum of squares of each term in the residual term between the kth frame and the k+1th frame is obtained, and the maximum a posteriori estimation of the corresponding frame is obtained, and the maximum a posteriori estimation of the kth frame is obtained The formula is as follows: ； wherein, denotes Huber kernel, denotes norm, denotes the residual of the k-th frame point p to the plane, , denotes the set of points in the k-th frame point cloud data, denotes the FMCW-IMU pre-integration model residual of the k-th frame and the k+1-th frame, denotes the steering angle observation residual of the k-th frame and the k+1-th frame, denotes the velocity observation residual of the k-th frame and the k+1-th frame. 6.The FMCW speed-based high-precision positioning mapping method of claim 1, wherein: The point-to-plane residual of all points of the kth frame of point cloud data, and the FMCW-IMU pre-integration model residual, the steering angle observation residual and the velocity observation residual of the kth frame and the k+1th frame are calculated as follows: 1) The point-to-plane residual of all points of the kth frame of point cloud data: The point-to-plane residual of a point p in the kth frame of point cloud data is calculated, and the point-to-plane residual of the point p is calculated as follows: The point-to-plane residual of a point p in the kth frame of point cloud data is calculated, and the point-to-plane residual of the point p is calculated as follows: ； ； ； ； ； ； In the formula, a centroid of a neighboring point set of point p, a jth neighboring point of a coordinate of the jth neighboring point of a coordinate of a projection of point p to a world coordinate system, j=1~M, M represents a number of neighboring points of point p, and the first M neighboring points are selected in a priority queue in ascending order of Euclidean distance from point p, a bias of the jth neighboring point relative to T represents a transpose, and C represents a covariance matrix, a flatness index, a transpose of a fitting plane of the M neighboring points, a distance from to the fitting plane, a first standard deviation, a second standard deviation, a third standard deviation, a maximum eigenvalue, an intermediate eigenvalue, a minimum eigenvalue, and , , obtained by eigenvalue decomposition of the covariance matrix C; 2) FMCW-IMU pre-integration model residuals for the kth frame and the k+1th frame : ； wherein, denotes the transformation matrix from the IMU coordinate system to the world coordinate system at the kth frame, denotes the displacement of the mobile platform in the world coordinate system at the k+1th frame, denotes the displacement of the mobile platform in the world coordinate system at the kth frame, denotes the IMU velocity of the mobile platform in the world coordinate system at the k+1th frame, denotes the IMU velocity of the mobile platform in the world coordinate system at the kth frame, denotes the attitude quaternion of the mobile platform in the world coordinate system at the k+1th frame, denotes the attitude quaternion of the mobile platform in the world coordinate system at the kth frame denotes the inverse operation of denotes the inverse operation of denotes the cross product, denotes the cross product, denotes the transformation matrix from the inertial coordinate system to the world coordinate system at the k+1th frame, denotes the bias of the k+1th frame acceleration collected by the inertial sensor, denotes the bias of the k+1th frame angular velocity collected by the inertial sensor; 3) steering angle observation residual of the kth frame and the k+1th frame : ； In the formula, represents the attitude quaternion of the mobile platform in the world coordinate system at the kth frame the yaw angle converted by the yaw angle, represents the attitude quaternion of the mobile platform in the world coordinate system at the kth frame the pitch angle converted by the pitch angle, represents the yaw angle obtained by the FMCW laser radar, represents the pitch angle obtained by the FMCW laser radar; 4) velocity observation residuals of the kth frame and the k+1th frame : ； wherein denotes the residual of the FMCW velocity of the kth frame and the k+1th frame of the moving platform, denotes the residual of the FMCW velocity of the k+1th frame and the kth frame of the moving platform, denotes the FMCW velocity of the k+1th frame of the moving platform, denotes the transformation matrix from the kth frame of the inertial coordinate system to the world coordinate system.

7. The high-precision positioning mapping method based on FMCW speed according to claim 6, characterized in that: Before calculating the point-to-plane residual of all points of the kth frame of point cloud data, the following operations are performed: Downsample the kth frame of point cloud data, the downsampling is to form a cube with a preset size of voxel, and a preset number of points are taken in each cube, and the downsampled points are uniformly distributed in three-dimensional space; All downsampled points are traversed to calculate the corresponding point-to-plane residual.

8. A high-precision positioning mapping system based on FMCW velocity, based on the high-precision positioning mapping method based on FMCW velocity in any one of claims 1-7, characterized in that: The high-precision positioning mapping system based on FMCW velocity comprises a data acquisition module, a data processing module and a map generation module, the data processing module comprises an FMCW velocity estimation module, a pre-integration model generation module, a residual calculation module and a maximum a posteriori estimation generation module, and the data processing module comprises an FMCW velocity estimation module, a pre-integration model generation module, a residual calculation module and a maximum a posteriori estimation generation module. The data acquisition module is configured to acquire point cloud data and inertial data in real time through the FMCW laser radar and the inertial sensor respectively, and the inertial data includes acceleration and angular velocity. The FMCW velocity estimation module is configured to estimate the FMCW velocity of the mobile platform according to the kth frame of point cloud data and a target function, and the target function is constructed based on a nonlinear least square method and a loss function. The pre-integration model generation module is configured to fuse the FMCW velocity of the kth frame of mobile platform and the inertial data to obtain a FMCW-IMU pre-integration model of the k+1th frame relative to the kth frame of motion. The residual calculation module is configured to obtain a residual term between the kth frame and the k+1th frame according to the kth frame of point cloud data, the FMCW velocity of the kth frame of mobile platform and the FMCW-IMU pre-integration model of the k+1th frame relative to the kth frame of motion, and the residual term between the kth frame and the k+1th frame includes point-to-plane residuals of all points of the kth frame of point cloud data, and FMCW-IMU pre-integration model residuals, steering angle observation residuals and velocity observation residuals of the kth frame and the k+1th frame. The maximum a posteriori estimation generation module is configured to minimize the sum of squares of each term in the residual term between the kth frame and the k+1th frame to obtain the maximum a posteriori estimation of the corresponding frame. The map generation module is configured to obtain the pose parameters of the k+1th frame according to the maximum a posteriori estimation of the kth frame to form a conversion matrix from the kth frame to the k+1th frame, and add the newly acquired point cloud data of the k+1th frame to the map after conversion through the conversion matrix to complete reconstruction.

9. A high-precision positioning mapping storage medium based on FMCW speed, characterized by: The high-precision positioning and mapping storage medium based on FMCW velocity is configured to store a computer program, and the computer program is executed by a processor to implement the high-precision positioning and mapping method based on FMCW velocity according to any one of claims 1-7.

Images