A method and system for implementing SLAM based on solid-state radar

Abstract

The present invention discloses a method and system for implementing SLAM based on a solid-state radar. The method includes: sampling a target environment using the solid-state radar to obtain point cloud data, and acquiring gyroscope data and acceleration based on an IMU; performing segmentation and screening preprocessing on the point cloud data obtained by each frame scan of the solid-state radar; then performing feature extraction to obtain plane features and edge features, and further constructing a local feature map using the plane features and edge features, where the local feature map is used to update the global map, and adding the uncoincident features to the global map for updating; based on the global map and the local feature map, calculating the residuals by using similar point matching feature points to determine the pose of the current frame; identifying whether the current frame is a key frame, and if it is a key frame, performing secondary optimization on the pose of the key frame using the IMU data. The technical solution of the present invention effectively improves the mapping accuracy by introducing key frames and fusing IMU data for secondary optimization.

Description

Technical Field

[0001] The present invention belongs to the technical field of SLAM, and particularly relates to a method and system for implementing SLAM based on a solid-state radar. Background Art

[0002] SLAM (Simultaneous Localization and Mapping) is a technology used for unmanned systems or robot navigation, which can simultaneously establish an environmental map and estimate the pose of a robot in real time. It combines technologies such as perception, control, and computer vision, and realizes autonomous navigation and positioning of a robot in an unknown environment through the processing of sensor data and the calculation of algorithms.

[0003] Most of the existing research is based on mechanical radars, and relatively less research has been conducted on solid-state radars. Solid-state radars do not require mechanical components for scanning, so they have high reliability and long life. In addition, there are some relatively prominent features, such as high stability and fast response, which enable it to achieve high-precision and high-resolution detection even in harsh environments.

[0004] However, conventional radars scan through parallel baselines, but this scanning method is not suitable for solid-state radars. The special structure of solid-state radars requires an irregular scanning method to obtain point clouds. The following are the existing research methods for the special scanning method of solid-state radars:

[0005] Apply the Radar Odometry and Mapping (LOAM) to small Field of View (FOV) solid-state lidars (Livox Mid-40 and Mid-100) with irregular scanning methods, and design a method for selecting points at the front end for the irregular scanning method; optimize the pose iteratively at the back end to achieve similar tracking accuracy. However, this method will drift during continuous iteration.

[0006] Different from traversing along the incident angle and deflection angle to select candidate points in, LiLi-OM proposes a new feature extraction method, processes its irregular scanning pattern during preprocessing, and proposes a tightly coupled scheme, marginalizes through a sliding window and directly fuses the measurements of the inertial sensor IMU and LiDAR.

[0007] However, the above methods have limitations in back-end optimization and lack accuracy when facing challenging scenarios. For example, when the amount of original data processed by the LOAM algorithm applied to solid-state radars reaches a certain level, the accumulated error will cause the state update equation to be abnormal. This will result in information loss and drift problems. Compared with the LOAM algorithm, LiLi-OM can obtain robust mapping by using local factor graph optimization at the back end. However, in order to achieve good real-time performance, the features of this method are too sparse. Therefore, this method also has the problem of low mapping accuracy. Summary of the Invention

[0008] The object of the present invention is to improve the SLAM mapping accuracy based on a solid-state radar. To this end, a SLAM implementation method and system based on a solid-state radar are provided. The method provided by the technical solution of the present invention proposes a key frame concept, that is, the radar pose of the key frame is optimized by means of an IMU to achieve more accurate positioning.

[0009] To this end, the present invention provides the following technical solutions:

[0010] A SLAM implementation method based on a solid-state radar includes the following steps:

[0011] S1: Sampling the target environment by using a solid-state radar to obtain point cloud data and acquiring gyroscope data and acceleration based on an inertial measurement unit (IMU);

[0012] S2: Performing segmentation and screening preprocessing on the point cloud data obtained by each frame scan of the solid-state radar;

[0013] S3: Extracting plane features and edge features from each frame of point cloud data after segmentation and screening preprocessing, and then constructing a local feature map by using the plane features and edge features. The local feature map is used to update the global map, and the non-coincident features are updated and added to the global map;

[0014] S4: Based on the global map and the local feature map, calculating the residuals by using similar point matching feature points to determine the pose of the current frame;

[0015] S5: Identifying whether the current frame is a key frame. If it is a key frame, the pose of the key frame is optimized twice by using the data of the inertial measurement unit (IMU) to obtain a more accurate pose.

[0016] Further optionally, in step S5, a threshold method is used to determine whether the current frame is a key frame. Among them, the point cloud change rate of the current frame compared with the previous key frame is greater than or equal to the determination threshold, and the current frame is a key frame; otherwise, the current frame is a regular frame;

[0017] Among them, the determination threshold is an empirical value or an adaptive threshold determined based on the following formula 1 or formula 2:

[0018] Formula 1:

[0019]

[0020] Where I j is the adaptive threshold determined by formula 1, K p is the amount of changing point cloud in the reference frame matching the previous key frame, E c E rrespectively represent all the point cloud amounts in the current frame and the reference frame, T c , T r respectively represent the point cloud amounts of the overlapping parts between the current frame and the reference frame and the previous key frame;

[0021] Formula 2:

[0022]

[0023] where, I sa is the adaptive threshold determined based on Formula 2, and the custom coefficient λ, satisfies: Δd c,p is the distance between the current frame and the previous key frame.

[0024] Key frames were originally applied to visual odometry to achieve real-time and accurate tracking. And often under limited on-board computing power, the selection of key frames is particularly important. The technical solution of the present invention introduces key frames. In order to accurately select key frames, the above-mentioned adaptive threshold setting method is set. Among them, when the 3D radar moves, the current frame and the previous key frame will be affected by the distance and become no longer similar; and a relatively long distance will reduce the overlapping part, which may lead to tracking loss. In order to more effectively select key frames and make the threshold more in line with the actual situation, the technical solution of the present invention further improves the adaptive threshold, defines the coefficient λ to update the threshold and the coefficient to refine the threshold, further ensuring the reliability of key frame selection and improving the system positioning accuracy and stability.

[0025] Further optionally, in step S4, the pose of the current frame is determined according to the following optimization equation, specifically:

[0026]

[0027] In the formula, m and n are respectively the number of edge features and the number of plane features that the current frame matches with the global map, is the distance metric corresponding to the distance from the edge feature point to the fitting line of the maximum eigenvalue, is the distance metric corresponding to the distance from the plane feature point to the fitting plane, is the point-to-line residual corresponding to the edge feature point, is the point-to-plane residual corresponding to the plane feature point, represents the pose of the current frame. is the pose of the scan point in the world coordinate system in the current state, that is, the result of converting the scan point in the radar coordinate system to the world coordinate system, and the scan point is an edge feature point or a plane feature point.

[0028] Further optionally, the residuals and are expressed as:

[0029]

[0030] Among them, is the pose of the scan point in the world coordinate system in the current state, that is, the result of converting the scan point in the radar coordinate system to the world coordinate system. This residual formula In the formula, the scan point is a point in the set of edge feature points ; Search for the 5 nearest feature points on the projection line of the point in the set of edge feature points and the 5 feature points are on the same straight line. represents the first point on this straight line, represents the last point on this straight line, R(q) is the rotation matrix, and t represents the current moment;

[0031]

[0032] This residual formula In the formula, the scan point is a point in the set of plane feature points ; Search for the 5 nearest feature points on the projection plane of the point in the set of plane feature points and the 5 feature points are in the same plane. represents three points selected on the said plane.

[0033] Further optionally, the method further includes: The process of secondarily optimizing the pose of the key frame by using the data of the inertial measurement unit IMU in step S5 is as follows:

[0034] Among them, the secondarily optimized function is expressed as:

[0035]

[0036] In the formula, represents the prior edge residual term of the marginalization measurement of the sliding window, and respectively represent the error terms of the radar and the IMU, represents the pose of the key frame; η and κ respectively represent the start point and the end point of the set sliding window, represents the state at a certain moment within the sliding window, k represents the key frame, that is, the set sliding window is used to limit the number of key frames, calculate the key frames within the sliding window, and further optimize the key frame pose.

[0037] The technical solution of the present invention incorporates the error terms of the IMU as factors into the factor graph to constrain the relative motion between key frames; the obtained error terms of the radar and the IMU are used to optimize the pose of the radar key frames by minimizing the cost function to obtain the maximum a posteriori estimate.

[0038] Further optionally, the prior marginal residual term, and the error terms of the radar and the IMU are expressed as follows:

[0039]

[0040]

[0041]

[0042]

[0043] where Γ p and H p are the latest marginalization parameters obtained by performing the Schur complement operation on , T represents the matrix transpose symbol, m and n are respectively the number of edge features and the number of plane features that the current frame matches with the global map, is the distance metric corresponding to the distance from the edge feature point to the fitting line of the maximum eigenvalue, i corresponds to the edge feature point, is the distance metric corresponding to the distance from the plane feature point to the fitting plane, j corresponds to the plane feature point; P k+1 , P k represent the poses of the keyword k + 1 and the key frame k respectively, R k represents the rotation matrix from the key frame k to the key frame k + 1, represents the matrix transpose of the rotation matrix R k , V k represents the velocity corresponding to the key frame k, ΔV k,k+1 , Δt k,k+1 represent the velocity difference and the time difference between the key frame k and the key frame k + 1 respectively, q is a quaternion, q k+1 , Δq k,k+1 represent the quaternions corresponding to the key frame k, the key frame k + 1 and the difference between the quaternions respectively; are the acceleration biases corresponding to the key frame k and the key frame k + 1 respectively, are the gravity acceleration biases corresponding to the key frame k and the key frame k + 1 respectively, g w is the gravity acceleration.

[0044] represents the array composed of the gyroscope and accelerometer readings between the key frame k and k + 1; represents the vector part of the quaternion q; Represents the corresponding noise covariance during the pre-integration process Is a constraint operation in the IMU error term operation, which is an existing technology; Is a custom matrix.

[0045] Further optionally, the segmentation and screening preprocessing includes removing dynamic points, adding labels, and ground segmentation:

[0046] Among them, the process of deleting dynamic points is: classifying the point cloud data to obtain seed points, and then using the region growing algorithm on the seed point set to determine and delete dynamic points, 4 < |z| < 6, p ∈ p SD , z represents the value of point p on the z-axis, unit: (m), p SD Represents the seed point, used for region growth;

[0047] The process of adding labels is: classifying the point cloud data to obtain background points and non-background point sets, that is, |z| ≥ 4, p ∈ p BG , 0.4 < |z| < 4, p ∈ p NBG In the formula, p BG Represents the background point, p NBG Represents the non-background point; then judge the former occlusion relationship of the background point according to the following formula:

[0048] Among them, retrieve the corresponding distance d and angle φ of the background point in the labeled background point array. If the same φ, the point with a smaller distance d value is the occluded point, and the point with a larger distance d value is the occluding point;

[0049]

[0050] d represents the distance of point p f from the origin, p f(x) Represents point p f value on the x-axis in the global coordinate system, p f(y) Represents point p f value on the y-axis, φ represents the angle above the horizontal direction, θ f(x),f(y) is the angle between point p f and the origin.

[0051] In addition, the present invention also provides a system based on the above method, including:

[0052] A data acquisition unit for sampling the target environment using a solid-state radar to obtain point cloud data and acquiring gyroscope data and acceleration based on an inertial measurement unit IMU;

[0053] A preprocessing unit for performing segmentation and screening preprocessing on the point cloud data obtained by each frame scan of the solid-state radar, and the segmentation and screening preprocessing at least includes removing the point cloud data of dynamic objects;

[0054] A feature extraction unit, which is used to extract features from each frame of point cloud data after segmentation, screening and preprocessing to obtain plane features and edge features, and then construct a local feature map by using the plane features and edge features. The local feature map is used to update the global map and add the uncoincident features to the global map.

[0055] A pose optimization unit, which is used to calculate the residual by using similar point matching feature points based on the global map and the local feature map to determine the pose of the current frame.

[0056] A key frame pose optimization unit, which is used to identify whether the current frame is a key frame. If it is a key frame, the data of the inertial measurement unit (IMU) is used to perform secondary optimization on the pose of the key frame.

[0057] The present invention also provides an electronic terminal, which at least includes:

[0058] One or more processors;

[0059] And a storage medium storing one or more computer programs;

[0060] Wherein, the processor calls the computer program to implement:

[0061] The steps of a method for implementing SLAM based on a solid-state radar.

[0062] The present invention also provides a computer-readable storage medium storing a computer program, and the computer program is called by a processor to execute:

[0063] The steps of a method for implementing SLAM based on a solid-state radar.

[0064] Beneficial effects

[0065] Compared with the existing methods, the advantages of the present invention are as follows:

[0066] 1. The technical solution of the present invention adopts an iterative optimization method to ensure the effective association between poses; and proposes the application of key frames, and by means of IMU data, fuses IMU measurements to optimize the radar pose of key frames, realizing more accurate positioning, which can effectively solve the problems of drift and information loss.

[0067] 2. In practical applications, the technical solution of the present invention enables the system to maintain good accuracy and mapping effects under conditions such as large swings and high speeds, which enables it to work stably in some challenging scenarios. During the continuous update of the pose, the previous state determines the accuracy of the next state. Especially with the help of high-frequency IMU, more accurate poses can be obtained. Description of the drawings

[0068] Figure 1 It is a schematic diagram of the system architecture corresponding to the SLAM implementation method provided by the present invention;

[0069] Figure 2 It is a schematic diagram of a tightly coupled lidar inertial based on iterative optimization provided by the present invention. Specific implementation manners

[0070] A SLAM implementation method based on a solid-state radar provided by the present invention effectively improves the mapping accuracy. One of its cores is to introduce key frames and fuse IMU data to perform secondary optimization on the radar pose corresponding to the key frames; the second core is to introduce the residuals corresponding to edge features and plane features as feature factors, and use the method of iterative optimization in the backend to ensure the effective association between poses; the third core is to preprocess the original point cloud data, that is, to include removing dynamic objects and providing technical means to distinguish occluded points and occluding points for the occlusion problem, laying a foundation for effectively solving the occlusion problem. The purpose of the method of the present invention is to obtain accurate estimation of the six-degree-of-freedom motion of the radar and obtain a globally consistent map. The present invention will be further described below in conjunction with embodiments.

[0071] Embodiment 1:

[0072] A SLAM implementation method based on a solid-state radar provided by this embodiment includes the following steps:

[0073] S1: Use a solid-state radar to sample the target environment to obtain point cloud data and obtain gyroscope data and acceleration based on an inertial measurement unit (IMU).

[0074] In this embodiment, the solid-state radar livox Avia is selected, which has a larger vertical resolution and a more uniform FoV coverage range. Compared with traditional 64-line mechanical radars, its price is much cheaper than that of mechanical radars with the same performance. As Figure 1 shown, the 3D lidar samples the environment and outputs point clouds at a frequency of 10 Hz, and the six-axis IMU provides gyroscope data and acceleration at a higher frequency.

[0075] S2: Perform segmentation and screening preprocessing on the point cloud data obtained by each frame scan of the solid-state radar. The purpose of the segmentation and screening preprocessing in this embodiment is to remove the point cloud data of dynamic objects and perform label classification on the point cloud, retain more effective point cloud data, and lay a foundation for subsequent mapping. In other feasible embodiments, it is not restricted to the above segmentation and screening preprocessing technology, that is, on the basis of meeting the basic requirements of ground segmentation, it is not restricted to performing the above preprocessing means such as label classification.

[0076] During the lidar scanning process, dynamic objects may appear in the map at multiple times, resulting in unstable mapping. In this embodiment, the point cloud of one frame of scanning is downsampled and corrected, and then the corrected point cloud is segmented for the ground. Before segmentation, it is necessary to evaluate and select the ground points and then classify and label them. Among them, the role of the label is also to better remove other bad points (outlier points).

[0077] First, for dynamic objects, it is necessary to remove the point cloud corresponding to the dynamic objects.

[0078] Set the 3D point coordinate as p = [x, y, z], and classify the points on the ground according to the following conditions:

[0079]

[0080] In the formula, z represents the value of the point p on the z-axis, p BG represents the background point, p NBG represents the non-background point, p SD represents the seed point for region growing. As can be seen from the above, each point set is delimited according to the z-axis coordinate value. Among them, the seed point is used for region growing, the dynamic points are screened out and deleted. In "Egocentric ratio of pseudo occupancy-based dynamic object removal for static 3D point cloud map building", each frame of point cloud and the sub-map are divided into grids, the descriptors are calculated and the potential dynamic regions are screened. This invention adopts this method, uses the region growing algorithm, selects the first seed point, screens the points similar to the nature of the seed within the threshold range and merges them into the region where the seed point is located, and then spreads around; then compares the distance of the seed point in the vertical direction with the threshold, and regards the points exceeding the threshold as potential dynamic points. Since this step is the prior art, no specific statement is made for it.

[0081] In some complex environments, tall or short objects may appear, and there is a problem of mutual occlusion of points on the ground, which may cause the loss of some feature points. To solve this problem, the present invention defines the points on the ground as background points, non-background points, and seed points according to the above formula. Among them, considering that in certain specific situations, there is a problem of mutual occlusion of the background at a certain height, that is, whether the background point is occluded by the foreground or the foreground point is occluded by the background, and which should be processed first, there is a front-back relationship. Therefore, the technical solution of the present invention uses the following formula (2) to identify the occluding point and the occluded point. On the one hand, the occluding point is processed more preferentially than the occluded point, that is, for feature extraction. It should be understood that these points will be stored in an array. When performing feature matching, an ordered array and an unordered array, the former must have a faster processing speed, and thus the speed of retrieving the point cloud can be improved; on the other hand, by using labels to represent the occlusion relationship, it is possible to prevent the occluded point from losing features due to occlusion problems. After being marked, it can be included in the preselection of feature points at the next moment.

[0082]

[0083] That is, retrieve in the array of labeled background points. If the same φ, the point with a smaller distance d value is the occluding point, and the point with a larger distance d value is the occluded point. Among them, the array of labeled background points is the data of the previous frame. d represents the distance of point p f from the origin, and p f(x) represents the value of this point on the x coordinate system, and p f(y) represents the value of this point on the y coordinate system, and φ represents the angle in the horizontal direction.

[0084] It should be noted that the former preprocessing is the preferred solution of this embodiment. In other feasible embodiments, it is not limited to the above preprocessing method. On the basis of not departing from the core idea of the following steps of the present invention, using other preprocessing technical means also falls within the protection scope of the present invention.

[0085] S3: Perform feature extraction on each frame of point cloud data after segmentation and screening preprocessing to obtain plane features and edge features, and then use the plane features and edge features to construct a local feature map. The local feature map is used to update the global map, and the uncoincident features are updated and added to the global map.

[0086] In this embodiment, the method of "LOAM: Lidar Odometry and Mapping in Real-Time" is adopted to extract features from the preprocessed point cloud data, that is, calculate the curvature values of candidate points and sort them, and then determine planar feature points and edge feature points according to the set threshold. Since there are many existing technical means for feature extraction, the present invention does not restrict the specific implementation means for extracting edge feature points and planar feature points. In addition, updating the global map with the local feature map is also a conventional idea and means in this field, and no specific statement will be made about this.

[0087] S4: Based on the global map and the local feature map, use similar point matching to match feature points for residual calculation to determine the pose of the current frame. Due to the scanning method of the characteristics of solid-state radar, the extracted features cannot always be matched within two adjacent frames. Therefore, an iterative optimization method is proposed to estimate the pose. Different from the conventional method of estimating the pose by matching, this paper uses the residuals from the features for constraint optimization.

[0088] Define a single scan as W, and extract the corresponding edge feature points on the radar frame ζ. If there is an edge feature point on the radar frame, first search for 5 adjacent points that meet the corresponding relationship in the local feature map M w (project the radar frame points onto the local map and search for the five adjacent points), then calculate the coordinate mean and covariance matrix of them, and perform eigenvalue decomposition; calculate the largest eigenvalue and fit it into a straight line (this process is an existing technology and will not be described in detail). Select two points on the straight line The metric relationship corresponding to the distance from the point to the line is as follows

[0089]

[0090] In the formula, represents the radar pose in the global frame. R(q) represents the rotation matrix about q. represents two points on the fitted straight line. is the edge feature point. t represents the current moment.

[0091] For the planar feature point p s , find 5 nearby planar feature points in the local feature map. Its specific implementation can refer to "Towards high-performance solid-state-lidar-inertial odometry and mapping," solve the overdetermined equation by plane fitting, and then normalize the fitted normal vector. The metric from the point to the plane can be expressed as:

[0092]

[0093] Among them, Indicates the normalization processing of the fitting vector. Indicates the variable in the QR solution of the overdetermined line equation. p w Is the plane feature point p s The point converted to the global map, representing the radar pose in the global frame.

[0094] The set pose is defined as As:

[0095]

[0096] Among them, Represents the position and orientation of the current frame, Represents the velocity, Represents the quaternion (existing, related to IMU data), b = [b a , b g Represents the biases of the gyroscope and accelerometer from the IMU.

[0097] Such as Figure 2 As shown, for iterative optimization, the poses of each frame are optimized using the residuals from the edge feature points and plane feature points. Define a point on the global map Represents the set of edge feature points of the current frame. Search in the set The 5 nearest feature points on the projection line, and then calculate the mean and covariance matrix of these feature points. The corresponding point-to-line residual is calculated as

[0098]

[0099] Among them, select the 5 points closest to the point And ensure they are on a straight line. Represents the first point on this line, Represents the last point on this line. Represents the pose of the current point in the global coordinates, R(q) is the rotation matrix; Is the pose of the scan point in the world coordinate system in the current state, that is, the result of converting the scan point in the radar coordinate system to the world coordinate system. In this formula, the scan point Is a point in the edge feature point set In.

[0100] Similar to the edge points, Represents the set of plane points of the current frame. Search in the set The 5 nearest feature points on the projection plane, and then calculate the mean and covariance matrix of these feature points. The corresponding point-to-plane residual is calculated as:

[0101]

[0102] Among them, select the 5 points closest to the point and ensure they are on a plane. Represent three points selected for this plane. Represents the pose of the scan point in the world coordinate system in the current state, that is, the result of converting the scan point in the radar coordinate system to the world coordinate system. In this formula, the scan point is a point in the set of plane feature points among them.

[0103] To achieve lightweight, during the scan matching process, the frame-to-model method is used to estimate and optimize the current pose using the method of the confidence domain:

[0104]

[0105] In the formula, m and n are the number of edge features and the number of plane features for the current frame matching the global map respectively, is the distance corresponding metric from the edge feature point to the fitting line of the maximum eigenvalue, is the distance corresponding metric from the plane feature point to the fitting plane, is the residual from the point corresponding to the edge feature point to the line, is the residual from the point corresponding to the plane feature point to the plane, represents the pose of the current frame, where the initial bias b of the IMU can be given in advance.

[0106] It should be noted that the radar pose of the current frame can be determined by solving according to the above formula.

[0107] S5: Identify whether the current frame is a key frame. If it is a key frame, use the data of the inertial measurement unit IMU to perform secondary optimization on the pose of the key frame. Among them, in step S5, the threshold method is used to judge whether the current frame is a key frame. Among them, the point cloud change rate of the current frame compared with the previous key frame is greater than or equal to the determination threshold, and the current frame is a key frame; otherwise, the current frame is a regular frame. Among them, the determination threshold is an empirical value or an adaptive threshold determined based on the following formula (9) or formula (10). And often under limited airborne computing power, the selection of key frames is particularly important. Therefore, the technical solution of the present invention proposes an adaptive key frame method to select appropriate key frames. In this embodiment, the adaptive threshold determined by formula (10) is preferably used. In other feasible embodiments, it is not limited thereto.

[0108] Since there is no state of the previous key frame during initialization, it is assumed that the current frame is similar to the reference frame, and then the initial threshold is calculated using the points that track the changes. The initial threshold is defined as formula (9):

[0109]

[0110] Among them, K p is the amount of point cloud change in the reference frame matching the previous key frame. E c , E r respectively represent the total amount of point cloud in the current frame and the reference frame. T c , T r respectively represent the amount of point cloud in the overlapping parts of the current frame and the reference frame with the previous key frame.

[0111] When the 3D radar moves, the current frame and the previous key frame will be affected by the distance and become dissimilar. In addition, a longer distance will reduce the overlapping part, which may lead to tracking loss. To more effectively select key frames and make the threshold more in line with the actual situation, a coefficient λ is defined to update the threshold and a coefficient is used to refine the threshold. The adaptive threshold is defined as shown in formula (10):

[0112]

[0113] Among them, Δd c,p is the distance from the current frame to the previous key frame.

[0114] It should be noted that after determining the threshold, it is judged whether the current frame is a key frame by whether the point cloud change rate between the current frame and the previous key frame is greater than or equal to the threshold. If it is greater than or equal to the threshold, the current frame is regarded as a key frame; otherwise, it is regarded as a regular frame.

[0115] Selecting valid key frames can improve the accuracy and stability of the system. However, it is important to maintain sparsity during the backend fusion process. Therefore, the present invention preferably proposes a sliding window to limit the number of key frames. On the other hand, a fixed-size window means discarding some old key frames. In this paper, the Schur elimination process is used to fill in the key frames, and the key frames will no longer be used after being marginalized. Accordingly, a new prior is calculated and added to the existing prior factors to facilitate using this estimate in the next window.

[0116] The technical solution of the present invention optimizes the pose of the key frame according to the following formula. Among them, the quadratic optimization function is expressed as:

[0117]

[0118] In the formula, represents the prior marginal residual term of the sliding window marginalized measurement, and respectively represent the error terms of the radar and the IMU. Indicates the pose of the key frame. η and κ respectively represent the starting point and the ending point of the set sliding window. Indicates the state at a certain moment k within the sliding window. The set sliding window is used to limit the number of key frames.

[0119] Among them, the prior edge residual expression is as follows:

[0120]

[0121] In the formula, Γ p and H p are the latest marginalized parameters obtained through Schur complement operation. T represents a matrix. Since the calculation of the parameters Γ p and H p is prior art, no detailed description will be given.

[0122] The radar error term comes from the geometric constraint of the feature information. When aligning the edges and plane features observed in the global map, its error term is expressed as:

[0123]

[0124] Among them, and represent the measurement formulas for the distance from the edge feature points of the current key frame to the line and from the plane feature points to the plane.

[0125] On the other hand, the present invention incorporates the error term of the IMU as a factor into the factor graph to constrain the relative motion between key frames. The IMU error term is defined as:

[0126]

[0127]

[0128] Among them, Γ p and H p are the latest marginalized parameters obtained through Schur complement operation. T represents the matrix transpose symbol. m and n are respectively the number of edge features and the number of plane features matched by the current frame with the global map. is the distance corresponding measurement from the edge feature point to the fitting line of the maximum eigenvalue. is the distance corresponding measurement from the plane feature point to the fitting plane; P k+1 , P k respectively represent the poses of the keyword k + 1 and the key frame k. R k represents the rotation matrix from the key frame k to the key frame k + 1. V k represents the velocity corresponding to the key frame k. ΔV k,k+1 , Δt k,k+1respectively represent the velocity difference and time difference between key frame k and key frame k + 1, q is a quaternion, q k+1 , Δq k,k+1 respectively represent the quaternions corresponding to key frame k, key frame k + 1, and the difference between the quaternions; are respectively the acceleration biases corresponding to key frame k and key frame k + 1, are respectively the gravitational acceleration biases corresponding to key frame k and key frame k + 1, g w is the gravitational acceleration;

[0129] represents an array composed of gyroscope and accelerometer readings between key frame k and k + 1; represents the vector part of the quaternion q; represents the corresponding noise covariance during the pre-integration process, represents the constraint operation in the IMU error term operation.

[0130] It should be noted that the non-linear least squares problem can be solved by Ceres Slove. In addition, the jointly optimized state value will be used as the initial value of the next state of the IMU, which can avoid the drift of the IMU state. The state of the IMU is inserted into the lidar odometry by linear interpolation, which can solve the motion blur problem and reduce the error of the lidar odometry.

[0131] Embodiment 2:

[0132] The embodiment of the present invention also provides a system based on the above method, including: a data acquisition unit, a preprocessing unit, a feature extraction unit, a pose optimization unit, and a key frame pose optimization unit.

[0133] Among them, the data acquisition unit is used to sample the target environment using a solid-state lidar to obtain point cloud data and acquire gyroscope data and acceleration based on an inertial measurement unit IMU; the preprocessing unit is used to perform segmentation and screening preprocessing on the point cloud data obtained by each frame scan of the solid-state lidar, and the segmentation and screening preprocessing at least includes removing the point cloud data of dynamic objects; the feature extraction unit is used to extract features from each frame of point cloud data after the segmentation and screening preprocessing to obtain plane features and edge features, and then construct a local feature map using the plane features and edge features, and the local feature map is used to update the global map and add the uncoincident features to the global map; the pose optimization unit is used to calculate the residuals by matching feature points of similar points based on the global map and the local feature map to determine the pose of the current frame; the key frame pose optimization unit is used to identify whether the current frame is a key frame, and if it is a key frame, use the data of the inertial measurement unit IMU to perform secondary optimization on the pose of the key frame.

[0134] It should be understood that the implementation process of each module can refer to the content description of the foregoing method. The division of the above functional modules is only a division of logical functions. In actual implementation, there may be other division methods. For example, multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed.

[0135] Embodiment 3:

[0136] The embodiment of the present invention further provides an electronic terminal, which at least includes: one or more processors; and a storage medium storing one or more computer programs; wherein, the processor calls the computer program to implement: the steps of a method for implementing SLAM based on solid-state radar.

[0137] Among them, the specific execution steps are S1 - S5.

[0138] Among them, the implementation process of each step can refer to the detailed steps of Embodiment 1. The memory may include high-speed RAM memory, and may also include non-volatile memory, such as at least one disk memory.

[0139] If the memory and the processor are implemented independently, the memory, the processor and the communication interface can be connected to each other through a bus and complete communication with each other. The bus can be an Industry Standard Architecture bus, an External Device Interconnect bus or an Extended Industry Standard Architecture bus, etc. The bus can be divided into an address bus, a data bus, a control bus, etc.

[0140] Optionally, in specific implementation, if the memory and the processor are integrated on a chip, the memory and the processor can complete communication with each other through an internal interface.

[0141] It should be understood that in the embodiment of the present invention, the so-called processor may be a Central Processing Unit (CPU), and this processor may also be other general-purpose processors, Digital Signal Processors (DSPs), Application Specific Integrated Circuits (ASICs), Field-Programmable Gate Arrays (FPGAs) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. The general-purpose processor may be a microprocessor or this processor may also be any conventional processor, etc. The memory may include a read-only memory and a random access memory, and provide instructions and data to the processor. A part of the memory may also include a non-volatile random access memory. For example, the memory may also store information about the device type.

[0142] Example 4:

[0143] The embodiment of the present invention further provides a computer-readable storage medium storing a computer program, which is called by a processor to execute the steps of a method for implementing SLAM based on a solid-state radar.

[0144] Among them, the specific execution steps are S1-S5.

[0145] For the specific implementation process of each step, please refer to the description of the foregoing method.

[0146] The readable storage medium is a computer-readable storage medium, which may be an internal storage unit of the controller described in any of the foregoing embodiments, such as the hard disk or memory of the controller. The readable storage medium may also be an external storage device of the controller, such as a plug-in hard disk, a smart media card (SMC), a secure digital (SD) card, a flash card, etc. equipped on the controller. Further, the readable storage medium may also include both the internal storage unit of the controller and the external storage device. The readable storage medium is used to store the computer program and other programs and data required by the controller. The readable storage medium may also be used to temporarily store the data that has been output or will be output.

[0147] Based on such an understanding, the technical solution of the present invention, in essence, or the part that contributes to the prior art, or all or part of the technical solution, may be embodied in the form of a software product. The computer software product is stored in a storage medium and includes several instructions for causing a computer device (which may be a personal computer, a server, or a network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of the present invention. And the foregoing readable storage medium includes: various media such as a USB flash drive, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a magnetic disk, or an optical disc that can store program codes.

[0148] Simulation verification:

[0149] In summary, the method provided by the technical solution of the present invention enables a ground vehicle to perform high-precision mapping in a challenging environment. By considering the extraction efficiency of valid points in the front-end preprocessing process, the point cloud is corrected, dynamic objects are filtered out, and the point cloud is labeled and classified to further reduce the number of useless points. To reduce the pose error, a feature k-d tree is constructed through global map retrieval, and a feature factor is generated to optimize the pose of the radar. Then, the IMU measurements are fused using a sliding window based on key frames to perform a secondary optimization of the pose. The proposed method is comprehensively evaluated by collecting datasets on two platforms. Compared with LiHo and LiLi-OM, the Li-HFP of the technical solution of the present invention can achieve higher-precision pose estimation and mapping. Future work will improve the real-time performance while ensuring high precision.

[0150] It should be emphasized that the examples described in the present invention are illustrative rather than restrictive. Therefore, the present invention is not limited to the examples described in the specific embodiments. Any other embodiments obtained by those skilled in the art based on the technical solution of the present invention, whether modified or replaced, as long as they do not depart from the purpose and scope of the present invention, also belong to the protection scope of the present invention.

Claims

1. A method for implementing SLAM based on solid-state radar, characterized in that: It includes the following steps: S1: Sampling the target environment using a solid-state radar to obtain point cloud data, and acquiring gyroscope data and acceleration based on an Inertial Measurement Unit (IMU); S2: Performing segmentation, screening, and preprocessing on the point cloud data obtained from each frame scan of the solid-state radar; S3: Extracting features from each frame of the point cloud data after segmentation, screening, and preprocessing to obtain plane features and edge features, and then constructing a local feature map using the plane features and edge features. The local feature map is used to update the global map, and the uncoincident features are updated and added to the global map; S4: Based on the global map and the local feature map, calculating the residuals by using similar point matching feature points to determine the pose of the current frame; S5: Identifying whether the current frame is a key frame. If it is a key frame, using the data of the Inertial Measurement Unit (IMU) to perform secondary optimization on the pose of the key frame to obtain a more accurate pose; Among them, the process of performing secondary optimization on the pose of the key frame by using the data of the Inertial Measurement Unit (IMU) in step S5 is as follows: Among them, the secondary optimization function is expressed as: In the formula, represents the prior marginal residual term of the sliding window marginalization measurement, and represent the error terms of the radar and IMU respectively, represents the pose of the key frame; η and κ represent the starting point and the ending point of the set sliding window respectively, represents the state at a certain moment within the sliding window, k represents the key frame, and the set sliding window is used to limit the number of key frames.

2. The method according to claim 1, characterized in that: In step S5, the threshold method is used to determine whether the current frame is a key frame. Among them, if the point cloud change rate of the current frame compared with the previous key frame is greater than or equal to the determination threshold, the current frame is a key frame; otherwise, the current frame is a regular frame; Among them, the determination threshold is an empirical value or an adaptive threshold determined based on the following formula 1 or formula 2: Formula 1: Among them, I j is the adaptive threshold determined by Formula 1, K p is the amount of point cloud that changes in the reference frame matching the previous key frame, E c , E r respectively represent the total amount of point cloud in the current frame and the reference frame, T c , T r respectively represent the amount of point cloud in the overlapping parts of the current frame and the reference frame with the previous key frame; Formula 2: Among them, I sa is the adaptive threshold determined based on Formula 2, and the custom coefficient λ, satisfies: Δd c,p is the distance between the current frame and the previous key frame.

3. The method according to claim 1, wherein: In step S4, the pose of the current frame is determined according to the following optimization equation, specifically: Where m and n are the number of edge features and planar features that match between the current frame and the global map, respectively, is the corresponding metric of the distance from the edge feature point to the fitting line of the maximum eigenvalue, is the corresponding metric of the distance from the planar feature point to the fitting plane, is the point-to-line residual corresponding to the edge feature point, is the point-to-plane residual corresponding to the planar feature point, represents the pose of the current frame, is the pose of the scan point in the world coordinate system in the current state, that is, the result of converting the scan point in the radar coordinate system to the world coordinate system. The scan point is an edge feature point or a planar feature point.

4. The method according to claim 3, characterized in that: Residual and is expressed as: Among them, is the pose of the scanning point in the world coordinate system in the current state, that is, the result of converting the scanning point in the radar coordinate system to the world coordinate system. This residual formula in, the scanning point is a point in the set of edge feature points ; Search for the 5 nearest feature points on the projection line of the point in the set of edge feature points and the 5 feature points are on the same straight line. represents the first point on this straight line, represents the last point on this straight line, R(q) is the rotation matrix, and t represents the current moment;​ This residual formula in which the scanning point is a point in the set of planar feature points Search for the point in the set of planar feature points The 5 nearest feature points on the projection plane and in the same plane represent three points selected on the said plane.

5. The method according to claim 1, characterized in that: The prior edge residual term and the error terms of the radar and the IMU are expressed as follows: where, Γ p and H p are the latest marginalized parameters obtained through Schur complement operation, T represents the matrix transpose symbol, m and n are the number of edge features and planar features that match between the current frame and the global map respectively, is the distance metric corresponding to the distance from the edge feature point to the fitting line of the maximum eigenvalue, is the distance metric corresponding to the distance from the planar feature point to the fitting plane; P k+1 and P k represent the poses of keyframe k + 1 and keyframe k respectively, R k represents the rotation matrix from keyframe k to keyframe k + 1, represents the matrix transpose of the rotation matrix R k , V k represents the velocity corresponding to keyframe k, ΔV k,k+1 , and Δt k,k+1 represent the velocity difference and time difference between keyframe k and keyframe k + 1 respectively, q is a quaternion, q k+1 , and Δq k,k+1 represent the quaternions corresponding to keyframe k and keyframe k + 1 and the difference between the quaternions respectively; are the acceleration biases corresponding to keyframe k and keyframe k + 1 respectively, are the gravitational acceleration biases corresponding to keyframe k and keyframe k + 1 respectively, g w is the gravitational acceleration; represents the array composed of gyroscope and accelerometer readings between keyframe k and k + 1; represents the vector part of the quaternion q; represents the corresponding noise covariance during the pre-integration process, is the constraint operation of the IMU residual, is a custom matrix.

6. The method according to claim 1, characterized in that: The segmentation, screening, and preprocessing include removing dynamic points, adding labels, and ground segmentation: Among them, the process of deleting dynamic points is as follows: classify the point cloud data to obtain seed points, then use the region growing algorithm on the seed point set to determine and delete dynamic points, where 4 < |z| < 6, p ∈ p SD , where z represents the value of point p on the z-axis, unit: m, and p SD represents the seed point for region growth; The process of adding tags is as follows: Classify the point cloud data to obtain background points and non-background point sets, that is, |z|≥4, p∈p BG , 0.4<|z|<4, p∈p NBG In the formula, p BG represents background points, and p NBG represents non-background points; then judge the former occlusion relationship of background points according to the following formula: Among them, in the array of background points with labels, retrieve the distance d and the angle φ corresponding to the background points. If the φ is the same, the point with a smaller distance d value is the occluded point, and the point with a larger distance d value is the occluding point; d represents the point p f the distance from the origin, p f(x) represents the point p f the value on the x-axis in the global coordinate system, p f(y) represents the point p f the value on the y-axis, φ represents the angle above the horizontal direction, θ f(x),f(y) is the point p f the angle with the origin.

7. A system based on the method according to any one of claims 1-6, characterized in that: It includes: A data acquisition unit for sampling the target environment using a solid-state radar to obtain point cloud data and acquiring gyroscope data and acceleration based on an Inertial Measurement Unit (IMU); A preprocessing unit for performing segmentation, screening, and preprocessing on the point cloud data obtained from each frame scan of the solid-state radar. The segmentation, screening, and preprocessing at least include removing the point cloud data of dynamic objects; A feature extraction unit for extracting features from each frame of the point cloud data after segmentation, screening, and preprocessing to obtain plane features and edge features, and then constructing a local feature map using the plane features and edge features. The local feature map is used to update the global map, and the uncoincident features are updated and added to the global map; A pose optimization unit for calculating the residuals by using similar point matching feature points based on the global map and the local feature map to determine the pose of the current frame; A key frame pose optimization unit for identifying whether the current frame is a key frame. If it is a key frame, using the data of the Inertial Measurement Unit (IMU) to perform secondary optimization on the pose of the key frame.

8. An electronic terminal, characterized in that: It at least includes: One or more processors; And a storage medium storing one or more computer programs; Among them, the processor calls the computer program to implement: Steps of the method for implementing SLAM based on solid-state radar according to any one of claims 1-6.

9. A computer-readable storage medium, characterized in that: A computer program is stored, and the computer program is called by a processor to execute: Steps of the method for implementing SLAM based on solid-state radar according to any one of claims 1-6.

Images