High-precision positioning mapping method and system based on FMCW speed and storage medium

Abstract

The invention discloses a high-precision positioning mapping method and system based on FMCW speed, and a storage medium. The method comprises the following steps: collecting point cloud data and inertial data; s2, estimating the FMCW speed of the mobile platform of the corresponding frame; s3, performing pre-integration fusion to obtain an FMCW-IMU pre-integration model of the (k + 1) th frame moving relative to the kth frame; s4, obtaining a residual term between the kth frame and the (k + 1) th frame; s5, minimizing the sum of squares of all items in a residual term between the kth frame and the (k + 1) th frame, and obtaining the maximum posteriori estimation of the corresponding frame; and S6, according to the maximum posteriori estimation of the kth frame, obtaining a pose parameter of the (k + 1) th frame to form a conversion matrix from the kth frame to the (k + 1) th frame, converting the newly obtained point cloud data of the (k + 1) th frame through the conversion matrix, and adding the converted point cloud data into the map to complete reconstruction. According to the method, the problem of accumulative errors caused by inertial navigation drift can be solved, and the positioning precision and robustness of mapping are improved.

Description

Technical Field

[0001] The present invention belongs to the field of simultaneous positioning and mapping technology, and specifically relates to a high-precision positioning and mapping method, system and storage medium based on FMCW speed. Background Art

[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, positioning and navigation technologies primarily rely on the Global Positioning System (GNSS). However, GNSS performance is insufficient in signal-restricted environments (such as indoors, in tunnels, and in jungles), making it difficult to meet the demands of complex scenarios. In contrast, SLAM technology uses sensors to autonomously perceive environmental information without relying on external signals, achieving high-precision pose estimation and map construction. This makes SLAM technology crucial for robotic navigation and path planning.

[0003] The core of SLAM technology lies in estimating the sensor's pose in real time while simultaneously building a map model of the environment. Pose estimation includes both position and orientation, and its accuracy directly determines the system's positioning capabilities. With the continuous advancement of sensor technology, SLAM is moving towards multi-sensor fusion. For example, current SLAM systems integrate multiple sensors (such as IMUs, lidars, gyroscopes, accelerometers, cameras, and wheel speedometers) to help improve system robustness and accuracy.

[0004] However, existing SLAM technologies generally rely on IMUs (inertial measurement units) for pose estimation. This requires time-integration of gyroscope and accelerometer outputs to obtain pose information. During this integration process, errors accumulate over time, ultimately leading to significant orientation deviations. Furthermore, while wheel speedometers can directly measure wheel speed, they are not suitable for non-wheeled platforms and require installation space, further limiting their application. Furthermore, FMCW (Frequency Modulated Continuous Wave) LiDAR (FMCW-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, leveraging the advantages of FMCW-LiDAR and developing effective algorithmic frameworks within SLAM systems to achieve higher positioning accuracy and robustness remains an unresolved challenge. Summary of the Invention

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

[0006] To achieve the above object, the technical solution adopted by the present invention is: The present invention proposes a high-precision positioning and mapping method based on FMCW velocity. The mobile platform includes an FMCW laser radar and an inertial sensor. The high-precision positioning and mapping method based on FMCW velocity includes the following steps: S1, respectively, collects point cloud data and inertial data in real time through FMCW lidar and inertial sensors. The inertial data includes acceleration and angular velocity. S2. According to k The frame point cloud data and the objective function are used to estimate the FMCW velocity of the mobile platform in the corresponding frame. The objective function is constructed based on the nonlinear least squares method and the loss function. S3, the k The FMCW velocity and inertial data of the frame mobile platform are pre-integrated and fused to obtain the first k +1 frame relative to k FMCW-IMU pre-integration model of frame motion; S4. According to k Frame point cloud data, k FMCW speed of the frame mobile platform and the k +1 frame relative to k FMCW-IMU pre-integration model of frame motion obtains the first k Frame and k +1 frame residual term, k Frame and k The residual term between +1 frames includes k The point-to-plane residuals of all points in the frame point cloud data, and the k Frame and k +1 frame of FMCW-IMU pre-integration model residuals, steering angle observation residuals, and speed observation residuals; S5. Minimize k Frame and k +1 The sum of the squares of the residual terms between frames to obtain the maximum a posteriori estimate of the corresponding frame; S6. According to k The maximum a posteriori estimate of the frame is obtained k +1 frame pose parameters to form the k Frame to k +1 frame's transformation matrix, and the newly acquired kThe point cloud data of the +1 frame is converted through the transformation matrix and added to the map to complete the reconstruction.

[0007] Preferably, according to k The frame point cloud data and the objective function estimate the FMCW velocity of the corresponding frame mobile platform, as follows: S21, the k Frame 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 absolute residual Element points smaller than the preset threshold are recorded as credible points, which means they are considered to meet the stationary condition; S24, according to the objective function, obtain the k 3D velocity components of the frame moving platform , calculate the k FMCW speed of frame moving platform , the formula is as follows: Where, Indicates the k The X-axis velocity component of the mobile platform in the FMCW lidar coordinate system at the frame time, Indicates the k The Y-axis velocity component of the mobile platform in the FMCW lidar coordinate system at the frame time, Indicates the k The Z-axis velocity component of the mobile platform in the FMCW lidar coordinate system at the frame time, k is a positive integer, OXYZ is the FMCW lidar coordinate system, and O is the origin of the FMCW lidar coordinate system.

[0008] Preferably, the absolute residual The formula is as follows: in, Where, represents the theoretical Doppler velocity, that is, the Doppler velocity under stationary conditions, represents the measured Doppler velocity in the point cloud data, dist Indicates the Euclidean distance between the element point and the origin of the FMCW lidar coordinate system, x Indicates the X-axis coordinate value of the element point in the FMCW lidar coordinate system. y Indicates the Y-axis coordinate value of the element point in the FMCW lidar coordinate system. zIndicates the Z-axis coordinate value of the element point in the FMCW lidar coordinate system. Represents the X-axis velocity component of the mobile platform in the FMCW lidar coordinate system, Represents the Y-axis velocity component of the mobile platform in the FMCW lidar coordinate system, Represents the Z-axis velocity component of the mobile platform in the FMCW lidar coordinate system; The objective function formula is as follows: Where, represents the loss function, r i Indicates the i The absolute residual of the credible points, i =1~ N , N represents the number of trustworthy points, δ Represents the scale parameter of the loss function, and the loss function is the Cauchy loss function.

[0009] 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 element points in the two-dimensional matrix are also screened out, as follows: S221. Perform multiple random sampling on each row of the two-dimensional matrix, and calculate the Euclidean distance between each randomly sampled element point and the origin of the FMCW laser radar coordinate system; S222: Eliminate the element points whose Euclidean distance is less than the preset distance, and calculate the absolute residual between the theoretical Doppler velocity of each retained element point and the measured Doppler velocity in the point cloud data. .

[0010] Preferably, minimize the k Frame and k +1 frames, and the sum of the squares of the residual terms between frames to obtain the maximum a posteriori estimate of the corresponding frame. k Maximum a posteriori estimation of frames The formula is as follows: in, represents the Huber kernel, represents the norm, Indicates the k Frame Point p The residual to the plane, , Indicates the k The collection of points in the frame point cloud data, Indicates the k Frame andk +1 frame FMCW-IMU pre-integration model residual, Indicates the k Frame and k +1 frame steering angle observation residual, Indicates the k Frame and k +1 frame of velocity observation residual.

[0011] Preferably, k +1 frame relative to k The FMCW-IMU pre-integration model of frame motion is expressed as follows: Where, Indicates the position of the mobile platform in the IMU coordinate system. k Frame to k +1 frame of IMU position pre-integration, Indicates the position of the mobile platform in the IMU coordinate system. k Frame to k +1 frame of IMU velocity pre-integration, Indicates the position of the mobile platform in the IMU coordinate system. k Frame to k +1 frame of IMU rotation pre-integration, Indicates the position of the mobile platform in the IMU coordinate system. k Frame to k +1 frame of FMCW velocity pre-integration, Indicates the inertial sensor at k The speed of the frame, which is measured by the inertial sensor at the k The acceleration before the frame is accumulated and integrated. Represents the gravitational acceleration in the world coordinate system, Indicates the k Frame to k +1 frame interval, Indicates the first k Frame acceleration, and Excluding gravity acceleration, Indicates the first k Frame angular velocity, Indicates the first k The bias of the frame acceleration, Indicates the first k The bias of the frame angular velocity, exp(·) represents the exponential mapping, Indicates the k The conversion matrix from the FMCW lidar coordinate system to the IMU coordinate system at the frame time, Indicates the k Frame the FMCW speed of the mobile platform.

[0012] Preferably, k The point-to-plane residuals of all points in the frame point cloud data, and the k Frame and k The FMCW-IMU pre-integration model residual, steering angle observation residual, and velocity observation residual of +1 frame are calculated as follows: 1) No. k Point-to-plane residuals of all points in the frame point cloud data: Respectively k The residual of the corresponding point to the plane is calculated from the point in the frame point cloud data. p Residual to plane The calculation is as follows: Where, Indicates a point p The centroid of the neighboring point set of express No. j The coordinates of the neighboring points, Represents a point projected into the world coordinate system p The coordinates of j =1~ M, M Indicates a point p The number of neighboring points, and the point p The priority queue with the Euclidean distance from small to large selects the first M neighboring points, Indicates the j neighboring points relative to The bias, T represents transpose, C represents the covariance matrix, represents the flatness index, express M The transpose of the fitted plane of neighboring points, express The distance to the fitting plane, represents the first standard deviation, represents the second standard deviation, represents the third standard deviation, represents the maximum eigenvalue, represents the intermediate eigenvalue, represents the minimum eigenvalue, and 、 、 From the covariance matrix C Perform eigenvalue decomposition to obtain; 2) No. k Frame and k +1 frame FMCW-IMU pre-integration model residual : Where, Indicates the k The conversion matrix from the IMU coordinate system to the world coordinate system at frame time, Indicates the k +1 frame displacement of the mobile platform in the world coordinate system, Indicates the k The displacement of the mobile platform in the world coordinate system at frame time, Indicates the k +1 frame is the IMU velocity of the mobile platform in the world coordinate system, Indicates the k The IMU velocity of the mobile platform in the world coordinate system at the frame time, Indicates the k The quaternion of the mobile platform's attitude in the world coordinate system at +1 frame, Indicates the k The quaternion of the mobile platform's posture in the world coordinate system at the frame time The inverse operation of express The inverse operation of represents the cross product, Indicates the k +1 frame time, the transformation matrix from the inertial coordinate system to the world coordinate system, Indicates the first k +1 frame acceleration bias, Indicates the first k +1 frame angular velocity bias; 3) No. k Frame and k +1 frame steering angle observation residual : Where, Indicates the k The quaternion of the mobile platform's posture in the world coordinate system at the frame time The converted yaw angle is Indicates the k The quaternion of the mobile platform's posture in the world coordinate system at the frame time The converted pitch angle is represents the yaw angle obtained by the FMCW lidar, Indicates the pitch angle obtained by the FMCW lidar; 4) No. k Frame and k +1 frame velocity observation residual : Where, Indicates the k Frame and k+ The residual of the FMCW velocity of the mobile platform in 1 frame, Indicates the k +1 frame and k The residual of the FMCW velocity of the frame moving platform, Indicates the k +1 frame FMCW speed of the mobile platform, Indicates the k The transformation matrix from the inertial coordinate system to the world coordinate system at the frame time.

[0013] Preferably, k Before calculating the point-to-plane residuals of all points in the frame point cloud data, the following operations are also performed: For the first k The frame point cloud data is downsampled to form cubes with a preset voxel size, and a preset number of points are taken in each cube, and the downsampled points are evenly distributed in the three-dimensional space; Traverse all downsampled points and calculate the residual from the corresponding point to the plane.

[0014] A high-precision positioning and mapping system based on FMCW speed, based on any of the above-mentioned high-precision positioning and mapping methods based on FMCW speed, comprises a data acquisition module, a data processing module, and a map generation module, wherein the data processing module comprises an FMCW speed estimation module, a pre-integration model generation module, a residual calculation module, and a maximum a posteriori estimation generation module, wherein: The data acquisition module is used to collect point cloud data and inertial data in real time through FMCW lidar and inertial sensors respectively. The inertial data includes acceleration and angular velocity; FMCW speed estimation module is used to estimate the speed of the k The frame point cloud data and the objective function are used to estimate the FMCW velocity of the mobile platform in the corresponding frame. The objective function is constructed based on the nonlinear least squares method and the loss function. Pre-integration model generation module is used to k The FMCW velocity and inertial data of the frame mobile platform are pre-integrated and fused to obtain the first k +1 frame relative to k FMCW-IMU pre-integration model of frame motion; The residual calculation module is used to calculate the k Frame point cloud data, k FMCW speed of the frame mobile platform and the k +1 frame relative to k FMCW-IMU pre-integration model of frame motion obtains the first k Frame and k +1 frame residual term, k Frame and k The residual term between +1 frames includes k The point-to-plane residuals of all points in the frame point cloud data, and the k Frame and k +1 frame of FMCW-IMU pre-integration model residuals, steering angle observation residuals, and speed observation residuals; The maximum a posteriori estimation generation module is used to minimize the k Frame and k +1 The sum of the squares of the residual terms between frames to obtain the maximum a posteriori estimate of the corresponding frame; Map generation module, used to k The maximum a posteriori estimate of the frame is obtained k +1 frame pose parameters to form the k Frame to k +1 frame's transformation matrix, and the newly acquired k The point cloud data of the +1 frame is converted through the transformation matrix and added to the map to complete the reconstruction.

[0015] A high-precision positioning and mapping storage medium based on FMCW speed is used to store a computer program. When the computer program is executed by a processor, it implements any of the above-mentioned high-precision positioning and mapping methods based on FMCW speed.

[0016] Compared with the prior art, the present invention has the following beneficial effects: This method obtains point cloud data and inertial data through FMCW lidar and inertial sensors, estimates the FMCW velocity of the mobile platform based on the point cloud data and the objective function, and obtains the corresponding FMCW-IMU pre-integration model and residual term; minimizes the sum of the squares of each term in the residual term to obtain the maximum a posteriori estimate of the corresponding frame, obtains the corresponding pose parameters based on the maximum a posteriori estimate, and adds them to the map after transformation through the transformation matrix to complete the reconstruction of the map. The point cloud data and inertial data are fused, which significantly reduces the displacement accumulation error caused by quadratic integration of traditional inertial sensors. Moreover, the FMCW lidar does not need to rely on the wheel speed meter and can directly infer its own motion through the Doppler velocity. It is suitable for various mobile platforms, including wheeled platforms, or non-wheeled platforms such as drones and robot dogs, and realizes long-term stable velocity estimation, effectively solves the inertial drift, and is simple to install, so that the SLAM system can achieve higher positioning accuracy and robustness in complex environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] Figure 1 This is a flow chart of the high-precision positioning and mapping method based on FMCW velocity of the present invention; Figure 2 This is a diagram showing the mapping effect of the high-precision positioning mapping method based on FMCW velocity in the present invention; Figure 3 This is a comparison diagram of the trajectory and true value of the high-precision positioning mapping method based on FMCW velocity of the present invention; Figure 4 This is a structural diagram of the high-precision positioning and mapping system based on FMCW velocity of the present invention. DETAILED DESCRIPTION

[0018] The following will be combined with the drawings in the embodiments of this application to clearly and completely describe the technical solutions in the embodiments of this application. Obviously, the embodiments described are only part of the embodiments of this application, not all of the embodiments. Based on the embodiments in this application, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of this application.

[0019] It should be noted that, unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by those skilled in the art in the art of this application. The terms used herein in the specification of this application are only for the purpose of describing specific embodiments and are not intended to limit this application.

[0020] Example 1: like Figure 1-Figure 3 As shown, a high-precision positioning and mapping method, system, and storage medium based on FMCW velocity are provided. The mobile platform includes an FMCW laser radar and an inertial sensor. The high-precision positioning and mapping method based on FMCW velocity includes the following steps: S1. Collect point cloud data and inertial data in real time through FMCW lidar and inertial sensors respectively. Inertial data includes acceleration and angular velocity.

[0021] Among them, the point cloud data and inertial data of the surrounding area are synchronously collected and preprocessed through FMCW laser radar and inertial sensor equipment. For example, the preprocessing is filtering and noise reduction in sequence. The point cloud data includes Doppler velocity, and the inertial data includes acceleration and angular velocity.

[0022] The IMU coordinate system of 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 coordinate axes in any direction. The world coordinate system is an absolute coordinate system used to describe the position and direction in a fixed environment.

[0023] S2. According to k The frame point cloud data and the objective function are used to estimate the FMCW velocity of the corresponding frame mobile platform. The objective function is constructed based on the nonlinear least squares method and the loss function.

[0024] In one embodiment, according to k The frame point cloud data and the objective function estimate the FMCW velocity of the corresponding frame mobile platform, as follows: S21, the k Frame 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 absolute residual Element points smaller than the preset threshold are recorded as credible points, which means they are considered to meet the stationary condition; S24, according to the objective function, obtain the k 3D velocity components of the frame moving platform , calculate the k FMCW speed of frame moving platform , the formula is as follows: Where, Indicates the k The X-axis velocity component of the mobile platform in the FMCW lidar coordinate system at the frame time, Indicates the k The Y-axis velocity component of the mobile platform in the FMCW lidar coordinate system at the frame time, Indicates the k The Z-axis velocity component of the mobile platform in the FMCW lidar coordinate system at the frame time, kis a positive integer, OXYZ is the FMCW lidar coordinate system, and O is the origin of the FMCW lidar coordinate system.

[0025] The point cloud data with Doppler velocity is mapped into a two-dimensional matrix and combined with 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 into a 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 the corresponding frame are traversed, and their unique position in the two-dimensional matrix is ​​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 efficient organization and processing of point cloud data, and each element in the two-dimensional matrix also includes an index into the point cloud data.

[0026] In one embodiment, the absolute residual The formula is as follows: in, Where, represents the theoretical Doppler velocity, that is, the Doppler velocity under stationary conditions, represents the measured Doppler velocity in the point cloud data, dist Indicates the Euclidean distance between the element point and the origin of the FMCW lidar coordinate system, x Indicates the X-axis coordinate value of the element point in the FMCW lidar coordinate system. y Indicates the Y-axis coordinate value of the element point in the FMCW lidar coordinate system. z Indicates the Z-axis coordinate value of the element point in the FMCW lidar coordinate system. Represents the X-axis velocity component of the mobile platform in the FMCW lidar coordinate system, Represents the Y-axis velocity component of the mobile platform in the FMCW lidar coordinate system, Represents the Z-axis velocity component of the mobile platform in the FMCW lidar coordinate system; The objective function formula is as follows: Where, represents the loss function, r i Indicates the i The absolute residual of the credible points, i =1~N , N represents the number of trustworthy points, δ Represents the scale parameter of the loss function, and the loss function is the Cauchy loss function.

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

[0028] 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 element points in the two-dimensional matrix are also screened out, as follows: S221. Perform multiple random sampling on each row of the two-dimensional matrix, and calculate the Euclidean distance between each randomly sampled element point and the origin of the FMCW laser radar coordinate system; S222: Eliminate the element points whose Euclidean distance is less than the preset distance, and calculate the absolute residual between the theoretical Doppler velocity of each retained element point and the measured Doppler velocity in the point cloud data. .

[0029] Before mapping to the two-dimensional data matrix, invalid and duplicate points can also be filtered out. Each row of the two-dimensional matrix is ​​randomly sampled multiple times, and the Euclidean distance between each element and the origin of the FMCW lidar coordinate system is calculated. Elements with Euclidean distances less than 2 meters are removed and considered 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 absolute residual is less than a preset threshold (set to 1), the point meets the stationary condition and is considered a reliable point, which is used as a subsequent FMCW velocity estimation sample. Subsequently, the sum of squares of the absolute residuals is optimized using a nonlinear least squares method and a loss function to obtain the velocity components of the mobile platform. The stationary background is filtered out using the Doppler velocity of the surrounding environment. Three-dimensional velocity components are obtained through a nonlinear optimization method, and the corresponding FMCW velocity of the mobile platform is calculated.

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

[0031] In one embodiment, the k +1 frame relative to k The FMCW-IMU pre-integration model of frame motion is expressed as follows: Where, Indicates the position of the mobile platform in the IMU coordinate system. k Frame to k +1 frame of IMU position pre-integration, Indicates the position of the mobile platform in the IMU coordinate system. k Frame to k +1 frame of IMU velocity pre-integration, Indicates the position of the mobile platform in the IMU coordinate system. k Frame to k +1 frame of IMU rotation pre-integration, Indicates the position of the mobile platform in the IMU coordinate system. k Frame to k +1 frame of FMCW velocity pre-integration, Indicates the inertial sensor at k The speed of the frame, which is measured by the inertial sensor at the k The acceleration before the frame is accumulated and integrated. Represents the gravitational acceleration in the world coordinate system, Indicates the k Frame to k +1 frame interval, Indicates the first k Frame acceleration, and Excluding gravity acceleration, Indicates the first k Frame angular velocity, Indicates the first k The bias of the frame acceleration, Indicates the first k The bias of the frame angular velocity, exp(·) represents the exponential mapping, Indicates the k The conversion matrix from the FMCW lidar coordinate system to the IMU coordinate system at the frame time, Indicates the k Frame the FMCW speed of the mobile platform.

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

[0033] In one embodiment, the k The point-to-plane residuals of all points in the frame point cloud data, and the k Frame and k The FMCW-IMU pre-integration model residual, steering angle observation residual, and velocity observation residual of +1 frame are calculated as follows: 1) No. k Point-to-plane residuals of all points in the frame point cloud data: Respectively k The residual of the corresponding point to the plane is calculated from the point in the frame point cloud data. p Residual to plane The calculation is as follows: Where, Indicates a point p The centroid of the neighboring point set of express No. j The coordinates of the neighboring points, Represents a point projected into the world coordinate system p The coordinates of j =1~ M, M Indicates a point p The number of neighboring points, and the point p The priority queue with the Euclidean distance from small to large selects the first M neighboring points, Indicates the jneighboring points relative to The bias, T represents transpose, C represents the covariance matrix, represents the flatness index, express M The transpose of the fitted plane of neighboring points, express The distance to the fitting plane, represents the first standard deviation, represents the second standard deviation, represents the third standard deviation, represents the maximum eigenvalue, represents the intermediate eigenvalue, represents the minimum eigenvalue, and 、 、 From the covariance matrix C Perform eigenvalue decomposition to obtain; 2) No. k Frame and k +1 frame FMCW-IMU pre-integration model residual : Where, Indicates the k The conversion matrix from the IMU coordinate system to the world coordinate system at frame time, Indicates the k +1 frame displacement of the mobile platform in the world coordinate system, Indicates the k The displacement of the mobile platform in the world coordinate system at frame time, Indicates the k +1 frame is the IMU velocity of the mobile platform in the world coordinate system, Indicates the k The IMU velocity of the mobile platform in the world coordinate system at the frame time, Indicates the k The quaternion of the mobile platform's attitude in the world coordinate system at +1 frame, Indicates the k The quaternion of the mobile platform's posture in the world coordinate system at the frame time The inverse operation of express The inverse operation of represents the cross product, Indicates the k +1 frame time, the transformation matrix from the inertial coordinate system to the world coordinate system, Indicates the first k +1 frame acceleration bias, Indicates the first k +1 frame angular velocity bias; 3) No. k Frame and k +1 frame steering angle observation residual : Where, Indicates the k The quaternion of the mobile platform's posture in the world coordinate system at the frame time The converted yaw angle is Indicates the k The quaternion of the mobile platform's posture in the world coordinate system at the frame time The converted pitch angle is represents the yaw angle obtained by the FMCW lidar, Indicates the pitch angle obtained by the FMCW lidar; 4) No. k Frame and k +1 frame velocity observation residual : Where, Indicates the k Frame and k+ The residual of the FMCW velocity of the mobile platform in 1 frame, Indicates the k +1 frame and k The residual of the FMCW velocity of the frame moving platform, Indicates the k +1 frame FMCW speed of the mobile platform, Indicates the k The transformation matrix from the inertial coordinate system to the world coordinate system at the frame time.

[0034] In one embodiment, the k Before calculating the point-to-plane residuals of all points in the frame point cloud data, the following operations are also performed: For the first k The frame point cloud data is downsampled to form cubes with a preset voxel size, and a preset number of points are taken in each cube, and the downsampled points are evenly distributed in the three-dimensional space; Traverse all downsampled points and calculate the residual from the corresponding point to the plane.

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

[0036] The downsampled points p Projected to the world coordinate system, we get Because the spatial data is divided into voxels and a priority queue is used (the point with the smallest distance is at the top of the queue), Filtering of adjacent points; such as before selection M neighboring points (set as M =20), calculate the centroid of the point set c and the offset of each neighboring point relative to the centroid , build the covariance matrix C . For the covariance matrix C Perform eigenvalue decomposition to obtain the flatness index , the value is between 0 and 1, 、 、 is the standard deviation, 、 、 is the eigenvalue of the decomposition. To evaluate whether the point set is close to a plane. If it is larger, it means that the point set is closer to a plane; if it is smaller, it means that the point set may not be flat. It is a measure of planarity. When the point cloud data presents an ideal plane shape, this value will be close to 1. Take the eigenvector with the largest eigenvalue n Serves as the normal vector to the fitted plane.

[0037] The estimation accuracy of the position and velocity of the mobile platform can be improved by using the point-to-plane residual, FMCW-IMU pre-integration model residual, steering angle observation residual, and velocity observation residual. The attitude quaternion consists of a real part and three imaginary parts, as shown in the first k The quaternion of the mobile platform's posture in the world coordinate system at the frame time Expressed as ,and and , calculated as follows: in, is the real part, indicating the position of the mobile platform in the world coordinate system.k Frame and k +1 frame rotation angle cosine half angle, [ ] is the imaginary part, The corresponding units of rotation axes in the three directions of X', Y', and Z' of the mobile platform in the world coordinate system are k Frame and k The product of the sine half-angle of the rotation angle for the first frame. Compared to the existing Euler angles, attitude quaternions avoid the gimbal lock problem and do not lose degrees of freedom during continuous rotation, resulting in high computational efficiency. The world coordinate system is represented as O'X'Y'Z'.

[0038] S5. Minimize k Frame and k The sum of the squares of the residual terms between +1 frames is used to obtain the maximum a posteriori estimate of the corresponding frame.

[0039] In one embodiment, minimizing k Frame and k +1 frames, and the sum of the squares of the residual terms between frames to obtain the maximum a posteriori estimate of the corresponding frame. k Maximum a posteriori estimation of frames The formula is as follows: in, represents the Huber kernel, represents the norm, Indicates the k Frame Point p The residual to the plane, , Indicates the k The collection of points in the frame point cloud data, Indicates the k Frame and k +1 frame FMCW-IMU pre-integration model residual, Indicates the k Frame and k +1 frame steering angle observation residual, Indicates the k Frame and k +1 frame of velocity observation residual.

[0040] S6. According to k The maximum a posteriori estimate of the frame is obtained k +1 frame pose parameters to form the k Frame to k +1 frame's transformation matrix, and the newly acquired k The point cloud data of the +1 frame is converted through the transformation matrix and added to the map to complete the reconstruction.

[0041] After solving the maximum a posteriori estimate, the first k +1 frame pose parameters (such as including the k The pose parameters of the +1 frame include the k +1 frame displacement of the mobile platform in the world coordinate system Hedi k +1 frame's posture quaternion of the mobile platform in the world coordinate system , which can be selected according to actual needs), according to k The pose parameters of the frame and the k The pose parameters of the +1 frame are calculated to k Frame to k +1 frame’s transformation matrix, the newly acquired k The point cloud data from frame +1 is transformed using a transformation matrix and then added to the map to complete the map reconstruction. This technique is well known to those skilled in the art and will not be further described here. Specifically, during the process of adding points from the point cloud data to the map, the point cloud data can also be downsampled using voxel filtering, retaining the most representative points within each voxel to reduce redundancy and maintain map compactness. The specific implementation process is similar to the downsampling process described above.

[0042] like Figure 2 As shown in the figure, it is the mapping effect diagram formed by the industrial park point cloud data collected in the experiment. Figure 3 The figure below compares the trajectory of the industrial park point cloud data collected during the experiment with the ground truth. This method is close to the true trajectory (GPS ground truth). Furthermore, the RMSE (Root Mean Square Error) of this method is 2.45m, which is smaller and more accurate than existing technologies.

[0043] It should be understood that although Figure 1 The steps in the flowchart are shown in sequence as indicated by the arrows, but these steps are not necessarily executed in the order indicated by the arrows. Unless otherwise specified in this document, there is no strict order restriction for the execution of these steps, and these steps can be executed in other orders. In addition, Figure 1 At least part of the steps may include multiple sub-steps or multiple stages. These sub-steps or stages are not necessarily executed 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 part of the sub-steps or stages of other steps.

[0044] Example 2: like Figure 4As shown, a high-precision positioning and mapping system based on FMCW speed is based on any high-precision positioning and mapping method based on FMCW speed in Example 1. The high-precision positioning and mapping system based on FMCW speed includes a data acquisition module, a data processing module, and a map generation module. The data processing module includes an FMCW speed estimation module, a pre-integration model generation module, a residual calculation module, and a maximum a posteriori estimation generation module, wherein: The data acquisition module is used to collect point cloud data and inertial data in real time through FMCW lidar and inertial sensors respectively. The inertial data includes acceleration and angular velocity; FMCW speed estimation module is used to estimate the speed of the k The frame point cloud data and the objective function are used to estimate the FMCW velocity of the mobile platform in the corresponding frame. The objective function is constructed based on the nonlinear least squares method and the loss function. Pre-integration model generation module is used to k The FMCW velocity and inertial data of the frame mobile platform are pre-integrated and fused to obtain the first k +1 frame relative to k FMCW-IMU pre-integration model of frame motion; The residual calculation module is used to calculate the k Frame point cloud data, k FMCW speed of the frame mobile platform and the k +1 frame relative to k FMCW-IMU pre-integration model of frame motion obtains the first k Frame and k +1 frame residual term, k Frame and k The residual term between +1 frames includes k The point-to-plane residuals of all points in the frame point cloud data, and the k Frame and k +1 frame of FMCW-IMU pre-integration model residuals, steering angle observation residuals, and speed observation residuals; The maximum a posteriori estimation generation module is used to minimize the k Frame and k +1 The sum of the squares of the residual terms between frames to obtain the maximum a posteriori estimate of the corresponding frame; Map generation module, used to k The maximum a posteriori estimate of the frame is obtained k +1 frame pose parameters to form the k Frame to k +1 frame's transformation matrix, and the newly acquired k The point cloud data of the +1 frame is converted through the transformation matrix and added to the map to complete the reconstruction.

[0045] The FMCW velocity-based high-precision positioning and mapping system 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 chip. The computer device can be a terminal device or other type of device. The FMCW velocity-based high-precision positioning and mapping system can implement all the steps described in the FMCW velocity-based positioning and mapping method embodiment and achieve the same technical effects. For the sake of brevity, the specific implementation details will not be repeated here.

[0046] It should be understood that the specific definition of a high-precision positioning and mapping system based on FMCW speed can be found in the definition of a high-precision positioning and mapping method based on FMCW speed in the embodiment, and will not be repeated here. Each module in the above-mentioned high-precision positioning and mapping system based on FMCW speed can be implemented in whole or in part by software, hardware, and a combination thereof. The above-mentioned modules can be embedded in or independent of the processor in the computer device in hardware form, or can be stored in the memory of the computer device in software form, so that the processor can call and execute the operations corresponding to the above modules.

[0047] Example 3: A high-precision positioning and mapping storage medium based on FMCW speed is used to store a computer program. When the computer program is executed by a processor, it implements any of the above-mentioned high-precision positioning and mapping methods based on FMCW speed.

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

[0049] 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 elements may be electrically connected to each other via one or more communication buses or signal lines. The storage medium stores a computer program executable on the processor, and the processor executes the computer program stored in the storage medium to implement the high-precision positioning and mapping method based on FMCW velocity in the embodiments of the present invention.

[0050] The storage medium may be, but is not limited to, a random access memory (RAM), a read-only memory (ROM), a programmable read-only memory (PROM), an erasable programmable read-only memory (EPROM), an electrically erasable programmable read-only memory (EEPROM), etc. The storage medium is used to store a computer program, and the processor executes the corresponding computer program after receiving an execution instruction.

[0051] The processor may be an integrated circuit chip with data processing capabilities. The aforementioned processor may be a general-purpose processor, including a central processing unit (CPU), a network processor (NP), etc. It can implement or execute the various methods, steps, and logic block diagrams disclosed in the embodiments of the present invention. The general-purpose processor may be a microprocessor or any conventional processor.

[0052] The technical features of the above-mentioned embodiments can be combined arbitrarily. In order to make the description concise, not all possible combinations of the technical features in the above-mentioned 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.

[0053] The above-described embodiments merely represent specific and detailed examples of the present application and should not be construed as limiting the scope of the present application. It should be noted that a person skilled in the art may make various modifications and improvements without departing from the spirit of the present application, and such modifications and improvements fall within the scope of protection of the present application. Therefore, the scope of protection of the present application shall be determined by the appended claims.

Claims

1. A high-precision positioning and mapping method based on FMCW velocity, applied to a mobile platform, characterized by: The mobile platform includes an FMCW laser radar and an inertial sensor. The high-precision positioning and mapping method based on FMCW velocity includes the following steps: S1. Collecting point cloud data and inertial data in real time using an FMCW lidar and an inertial sensor, respectively. The inertial data includes acceleration and angular velocity. S2. According to k Estimate an FMCW velocity of a mobile platform corresponding to the frame using the frame point cloud data and an objective function, wherein the objective function is constructed based on a nonlinear least squares method and a loss function; S3, the k The FMCW velocity and inertial data of the frame mobile platform are pre-integrated and fused to obtain the first k +1 frame relative to k FMCW-IMU pre-integration model of frame motion; S4. According to k Frame point cloud data, k FMCW speed of the frame mobile platform and the k +1 frame relative to k FMCW-IMU pre-integration model of frame motion obtains the first k Frame and k +1 frame, the residual term between k Frame and k The residual term between +1 frames includes k The point-to-plane residuals of all points in the frame point cloud data, and the k Frame and k +1 frame of FMCW-IMU pre-integration model residuals, steering angle observation residuals, and speed observation residuals; S5. Minimize k Frame and k +1 The sum of the squares of the residual terms between frames to obtain the maximum a posteriori estimate of the corresponding frame; S6. According to k The maximum a posteriori estimate of the frame is obtained k +1 frame pose parameters to form the k Frame to k +1 frame's transformation matrix, and the newly acquired k The point cloud data of the +1 frame is converted through the transformation matrix and added to the map to complete the reconstruction.

2. The high-precision positioning and mapping method based on FMCW velocity according to claim 1, characterized in that: According to the k The frame point cloud data and the objective function estimate the FMCW velocity of the corresponding frame mobile platform, as follows: S21, the k Frame 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 absolute residual Element points smaller than the preset threshold are recorded as credible points, which means they are considered to meet the stationary condition; S24, according to the objective function, obtain the k 3D velocity components of the frame moving platform , calculate the k FMCW speed of frame moving platform , the formula is as follows: Where, Indicates the k The X-axis velocity component of the mobile platform in the FMCW lidar coordinate system at the frame time, Indicates the k The Y-axis velocity component of the mobile platform in the FMCW lidar coordinate system at the frame time, Indicates the k The Z-axis velocity component of the mobile platform in the FMCW lidar coordinate system at the frame time, k is a positive integer, OXYZ is the FMCW lidar coordinate system, and O is the origin of the FMCW lidar coordinate system.

3. The high-precision positioning and mapping method based on FMCW velocity according to claim 2, characterized in that: The absolute residual The formula is as follows: in, Where, represents the theoretical Doppler velocity, that is, the Doppler velocity under stationary conditions, represents the measured Doppler velocity in the point cloud data, dist Indicates the Euclidean distance between the element point and the origin of the FMCW lidar coordinate system, x Indicates the X-axis coordinate value of the element point in the FMCW lidar coordinate system. y Indicates the Y-axis coordinate value of the element point in the FMCW lidar coordinate system. z Indicates the Z-axis coordinate value of the element point in the FMCW lidar coordinate system. Represents the X-axis velocity component of the mobile platform in the FMCW lidar coordinate system, Represents the Y-axis velocity component of the mobile platform in the FMCW lidar coordinate system, Represents the Z-axis velocity component of the mobile platform in the FMCW lidar coordinate system; The objective function formula is as follows: Where, represents the loss function, r i Indicates the i The absolute residual of the credible points, i =1~ N , N represents the number of trustworthy points, δ represents the scale parameter of the loss function, where the loss function is the Cauchy loss function.

4. The high-precision positioning and mapping method based on FMCW velocity according to 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 also screened out, as follows: S221. Perform multiple random sampling on each row of the two-dimensional matrix, and calculate the Euclidean distance between each randomly sampled element point and the origin of the FMCW laser radar coordinate system; S222: Eliminate the element points whose Euclidean distance is less than the preset distance, and calculate the absolute residual between the theoretical Doppler velocity of each retained element point and the measured Doppler velocity in the point cloud data. .

5. The high-precision positioning and mapping method based on FMCW velocity according to claim 1, wherein: The minimization k Frame and k +1 frames, and the sum of the squares of the residual terms between frames to obtain the maximum a posteriori estimate of the corresponding frame. k Maximum a posteriori estimation of frames The formula is as follows: in, represents the Huber kernel, represents the norm, Indicates the k Frame Point p The residual to the plane, , Indicates the k The collection of points in the frame point cloud data, Indicates the k Frame and k +1 frame FMCW-IMU pre-integration model residual, Indicates the k Frame and k +1 frame steering angle observation residual, Indicates the k Frame and k +1 frame of velocity observation residual.

6. The high-precision positioning and mapping method based on FMCW velocity according to claim 1, wherein: The said k +1 frame relative to k The FMCW-IMU pre-integration model of frame motion is expressed as follows: Where, Indicates the position of the mobile platform in the IMU coordinate system. k Frame to k +1 frame of IMU position pre-integration, Indicates the position of the mobile platform in the IMU coordinate system. k Frame to k +1 frame of IMU velocity pre-integration, Indicates the position of the mobile platform in the IMU coordinate system. k Frame to k +1 frame of IMU rotation pre-integration, Indicates the position of the mobile platform in the IMU coordinate system. k Frame to k +1 frame of FMCW velocity pre-integration, Indicates the inertial sensor at k The speed of the frame, which is measured by the inertial sensor at the k The acceleration before the frame is accumulated and integrated. Represents the gravitational acceleration in the world coordinate system, Indicates the k Frame to k +1 frame interval, Indicates the first k Frame acceleration, and Excluding gravity acceleration, Indicates the first k Frame angular velocity, Indicates the first k The bias of the frame acceleration, Indicates the first k The bias of the frame angular velocity, exp(·) represents the exponential mapping, Indicates the k The conversion matrix from the FMCW lidar coordinate system to the IMU coordinate system at the frame time, Indicates the k Frame the FMCW speed of the mobile platform.

7. The high-precision positioning and mapping method based on FMCW velocity according to claim 6, characterized in that: The said k The point-to-plane residuals of all points in the frame point cloud data, and the k Frame and k The FMCW-IMU pre-integration model residual, steering angle observation residual, and velocity observation residual of +1 frame are calculated as follows: 1) No. k Point-to-plane residuals of all points in the frame point cloud data: Respectively k The residual of the corresponding point to the plane is calculated from the point in the frame point cloud data. p Residual to plane The calculation is as follows: Where, Indicates a point p The centroid of the neighboring point set of express No. j The coordinates of the neighboring points, Represents a point projected into the world coordinate system p The coordinates of j =1~ M, M Indicates a point p The number of neighboring points, and the point p The priority queue with the Euclidean distance from small to large selects the first M neighboring points, Indicates the j neighboring points relative to The bias, T represents transpose, C represents the covariance matrix, represents the flatness index, express M The transpose of the fitted plane of neighboring points, express The distance to the fitting plane, represents the first standard deviation, represents the second standard deviation, represents the third standard deviation, represents the maximum eigenvalue, represents the intermediate eigenvalue, represents the minimum eigenvalue, and 、 、 From the covariance matrix C Perform eigenvalue decomposition to obtain; 2) No. k Frame and k +1 frame FMCW-IMU pre-integration model residual : Where, Indicates the k The conversion matrix from the IMU coordinate system to the world coordinate system at frame time, Indicates the k +1 frame displacement of the mobile platform in the world coordinate system, Indicates the k The displacement of the mobile platform in the world coordinate system at frame time, Indicates the k +1 frame is the IMU velocity of the mobile platform in the world coordinate system, Indicates the k The IMU velocity of the mobile platform in the world coordinate system at the frame time, Indicates the k The quaternion of the mobile platform's attitude in the world coordinate system at +1 frame, Indicates the k The quaternion of the mobile platform's posture in the world coordinate system at the frame time The inverse operation of express The inverse operation of represents the cross product, Indicates the k +1 frame time, the transformation matrix from the inertial coordinate system to the world coordinate system, Indicates the first k +1 frame acceleration bias, Indicates the first k +1 frame angular velocity bias; 3) No. k Frame and k +1 frame steering angle observation residual : Where, Indicates the k The quaternion of the mobile platform's posture in the world coordinate system at the frame time The converted yaw angle is Indicates the k The quaternion of the mobile platform's posture in the world coordinate system at the frame time The converted pitch angle is represents the yaw angle obtained by the FMCW lidar, Indicates the pitch angle obtained by the FMCW lidar; 4) No. k Frame and k +1 frame velocity observation residual : Where, Indicates the k Frame and k+ The residual of the FMCW velocity of the mobile platform in 1 frame, Indicates the k +1 frame and k The residual of the FMCW velocity of the frame moving platform, Indicates the k +1 frame FMCW speed of the mobile platform, Indicates the k The transformation matrix from the inertial coordinate system to the world coordinate system at the frame time.

8. The high-precision positioning and mapping method based on FMCW velocity according to claim 7, characterized in that: The said k Before calculating the point-to-plane residuals of all points in the frame point cloud data, the following operations are also performed: For the first k Downsampling the frame point cloud data, wherein the downsampling is to form cubes with a preset voxel size, and taking a preset number of points in each cube, and the downsampled points are evenly distributed in the three-dimensional space; Traverse all downsampled points and calculate the residual from the corresponding point to the plane.

9. A high-precision positioning and mapping system based on FMCW velocity, based on the high-precision positioning and mapping method based on FMCW velocity according to any one of claims 1 to 8, characterized in that: 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: The data acquisition module is used to respectively collect point cloud data and inertial data in real time through the FMCW laser radar and the inertial sensor, wherein the inertial data includes acceleration and angular velocity; The FMCW speed estimation module is used to k Estimate an FMCW velocity of a mobile platform corresponding to the frame using the frame point cloud data and an objective function, wherein the objective function is constructed based on a nonlinear least squares method and a loss function; The pre-integration model generation module is used to k The FMCW velocity and inertial data of the frame mobile platform are pre-integrated and fused to obtain the first k +1 frame relative to k FMCW-IMU pre-integration model of frame motion; The residual calculation module is used to calculate the residual error according to the k Frame point cloud data, k FMCW speed of the frame mobile platform and the k +1 frame relative to k FMCW-IMU pre-integration model of frame motion obtains the first k Frame and k +1 frame, the residual term between k Frame and k The residual term between +1 frames includes k The point-to-plane residuals of all points in the frame point cloud data, and the k Frame and k +1 frame of FMCW-IMU pre-integration model residuals, steering angle observation residuals, and velocity observation residuals; The maximum a posteriori estimation generation module is used to minimize the k Frame and k +1 The sum of the squares of the residual terms between frames to obtain the maximum a posteriori estimate of the corresponding frame; The map generation module is used to generate a map according to k The maximum a posteriori estimate of the frame is obtained k +1 frame pose parameters to form the k Frame to k +1 frame's transformation matrix, and the newly acquired k The point cloud data of the +1 frame is converted through the transformation matrix and added to the map to complete the reconstruction.

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

Images