SLAM method, device and system based on mixed window joint optimization

Abstract

The invention relates to the technical field of SLAM, and particularly discloses an SLAM method, device and system based on mixed window joint optimization, and the method comprises the steps: obtaining laser radar point cloud data, carrying out the feature point extraction of the laser radar point cloud data, and obtaining point cloud feature point data; obtaining IMU data between two adjacent point cloud frame sampling moments, and constructing an IMU pre-integration residual error according to the IMU data; constructing a sliding window of a time sequence frame based on the key frame and the point cloud feature point data, and constructing a local map according to the sliding window; matching the point cloud feature point data with a local map to obtain a point cloud matching constraint residual error; and performing joint optimization according to the point cloud matching constraint residual error and the IMU pre-integration residual error to obtain the pose of the laser radar point cloud data in the world coordinate system. According to the SLAM method based on mixed window joint optimization provided by the invention, the robustness and precision of positioning and mapping of the SLAM system can be improved.

Description

Technical Field

[0001] The present invention relates to the field of SLAM technology, and in particular to a SLAM method based on mixed window joint optimization, a SLAM device based on mixed window joint optimization, and a SLAM system based on mixed window joint optimization. Background Art

[0002] With the rapid development of intelligent robots and autonomous driving technologies, Simultaneous Localization and Mapping (SLAM) has become a research field that has attracted much attention. SLAM plays a key role in providing accurate real-time positioning in intelligent robots and autonomous vehicles. Compared with vision-based SLAM, Light Laser Detection and Ranging (LiDAR) technology has gradually become a popular choice in the field of SLAM due to its superior positioning accuracy and insensitivity to lighting conditions. Compared with a single LiDAR odometer, the LiDAR / IMU (Inertial Measurement Unit) odometer exhibits smaller pose drift, especially in complex scenes where feature registration constraints are degraded.

[0003] In practical applications, it is common to maintain a sliding window consisting of fixed frames. By aligning the current frame with the local map formed by the sliding window and adjusting the poses of multiple frames in the sliding window using factor graph optimization techniques, ensuring the appropriate size of the local map and providing sufficient common view features becomes an important challenge. The key frame selection strategy needs to extract a single frame from a series of ordinary frames as a representative of the local frame. A reasonable key frame selection strategy can not only avoid the interference of irrelevant or erroneous data on the optimization process, but also effectively improve the accuracy of positioning and mapping. However, the existing selection strategy that relies on coarse angle and translation changes to determine the key frame is still insufficient in accuracy and stability.

[0004] Therefore, how to improve the robustness and accuracy of SLAM system positioning and mapping has become a technical problem that needs to be urgently solved by technical personnel in this field. Summary of the invention

[0005] The present invention provides a SLAM method based on mixed window joint optimization, a SLAM device based on mixed window joint optimization and a SLAM system based on mixed window joint optimization, which solve the problems of insufficient robustness and accuracy of SLAM system positioning and mapping in the related art.

[0006] As a first aspect of the present invention, a SLAM method based on mixed window joint optimization is provided, which includes:

[0007] Acquire laser radar point cloud data, and extract feature points from the laser radar point cloud data to obtain point cloud feature point data;

[0008] Acquire IMU data between sampling moments of two adjacent point cloud frames, and construct an IMU pre-integration residual based on the IMU data;

[0009] Constructing a sliding window of a time series frame based on a key frame and the point cloud feature point data, and constructing a local map according to the sliding window;

[0010] Matching the point cloud feature point data with the local map to obtain a point cloud matching constraint residual;

[0011] The point cloud matching constraint residual and the IMU pre-integration residual are jointly optimized to obtain the position and pose of the lidar point cloud data in the world coordinate system.

[0012] Furthermore, constructing a sliding window of a time series frame based on a key frame and the point cloud feature point data, and constructing a local map according to the sliding window, including:

[0013] Determine the time sequence frames adjacent to the historical key frame and the current frame;

[0014] Determine the historical key frame as a sliding window quantification, and determine the adjacent time sequence frame of the current frame as a sliding window variable;

[0015] Forming a sliding window according to the sliding window quantity and the sliding window variable;

[0016] Determine the relative pose of the lidar corresponding to each point cloud frame in the sliding window based on the IMU pre-integration residual and the carrier pose estimation;

[0017] The relative position of the lidar corresponding to each frame in the sliding window is projected to the IMU coordinate system to obtain a local map.

[0018] Furthermore, constructing a sliding window of a time series frame based on a key frame and the point cloud feature point data, and constructing a local map according to the sliding window, further comprising:

[0019] Update the sliding window according to the real-time point cloud feature point data.

[0020] Further, updating the sliding window according to the real-time point cloud feature point data includes:

[0021] Obtain the latest point cloud frame pose solution result based on real-time point cloud feature point data;

[0022] Adding the latest point cloud frame pose solution result to the sliding window variable of the sliding window;

[0023] Performing key frame detection on the point cloud frame with the furthest time in the sliding window variable;

[0024] If the furthest point cloud frame in the sliding window variable satisfies the key frame threshold, the furthest point cloud frame in the sliding window variable is added to the sliding window quantification of the sliding window, and the furthest key frame in history in the sliding window quantification is deleted;

[0025] If the point cloud frame with the furthest time in the sliding window variable does not meet the key frame threshold, the point cloud frame with the furthest time in the sliding window variable is deleted.

[0026] Further, performing key frame detection on the point cloud frame with the furthest time in the sliding window variable includes:

[0027] Calculate the feature similarity between the furthest point cloud frame in the sliding window variable and the closest historical key frame in the sliding window quantification to obtain a feature similarity calculation result;

[0028] comparing the feature similarity calculation result with an adaptive threshold determined based on motion information;

[0029] If the feature similarity calculation result is greater than the adaptive threshold, determining that the point cloud frame with the furthest time in the sliding window variable meets the key frame threshold;

[0030] If the feature similarity calculation result is not greater than the adaptive threshold, it is determined that the furthest point cloud frame in the sliding window variable does not meet the key frame threshold.

[0031] Further, the adaptive threshold determined based on the motion information at least includes an adaptive threshold determined based on the Euler angle, and the expression of the adaptive threshold is:

[0032]

[0033] Among them, t represents the adaptive threshold, t1 represents the fixed threshold used in stable conditions, t2 represents the angle transformation threshold, and ΔR represents the modulus of the Euler angle between the two latest point cloud frames.

[0034] Further, IMU data between two adjacent point cloud frame sampling moments is obtained, and an IMU pre-integration residual is constructed according to the IMU data, including:

[0035] Perform pre-integration calculation based on the IMU data between the sampling moments of two adjacent point cloud frames to obtain the pre-integration amount;

[0036] Pre-integration constraints are performed according to the pre-integration amount, and an IMU pre-integration residual is constructed according to the pre-integration constraint result.

[0037] Further, performing joint optimization based on the point cloud matching constraint residual and the IMU pre-integration residual includes:

[0038] Constructing optimized frame matching residuals and inter-frame pre-integration residuals according to the point cloud matching constraint residuals and the IMU pre-integration residuals;

[0039] Obtaining a target optimization function of a sliding variable in a sliding window according to the optimized frame registration residual and the inter-frame pre-integration residual;

[0040] Minimizing the target optimization function and updating the sliding variable in the sliding window according to the minimization result;

[0041] The target optimization function is jointly nonlinearly optimized according to the point cloud feature point data and the IMU data to obtain the maximum a posteriori estimate of the sliding variable in the sliding window.

[0042] As another aspect of the present invention, a SLAM device based on mixed window joint optimization is provided, which is used to implement the SLAM method based on mixed window joint optimization as described above, wherein the device comprises:

[0043] A point cloud feature extraction module is used to obtain laser radar point cloud data and extract feature points from the laser radar point cloud data to obtain point cloud feature point data;

[0044] A pre-integration residual construction module is used to obtain IMU data between two adjacent point cloud frame sampling moments and construct an IMU pre-integration residual based on the IMU data;

[0045] A local map construction module, used to construct a sliding window of a time series frame based on a key frame and the point cloud feature point data, and to construct a local map according to the sliding window;

[0046] A point cloud matching constraint residual obtaining module, used for matching the point cloud feature point data with the local map to obtain a point cloud matching constraint residual;

[0047] The joint optimization module is used to perform joint optimization based on the point cloud matching constraint residual and the IMU pre-integration residual to obtain the position and posture of the lidar point cloud data in the world coordinate system.

[0048] As another aspect of the present invention, a SLAM system based on mixed window joint optimization is provided, which includes: an inertial measurement device, a laser radar device and the SLAM device based on mixed window joint optimization as described above, wherein the inertial measurement device and the laser radar device are both communicatively connected to the SLAM device based on mixed window joint optimization;

[0049] The inertial measurement unit is used to provide IMU data;

[0050] The laser radar device is used to provide laser radar point cloud data;

[0051] The SLAM device based on mixed window joint optimization is used to obtain fused posture information after processing the laser radar point cloud data and IMU data.

[0052] The SLAM method based on hybrid window joint optimization provided by the present invention adopts a key frame and a time frame to form a sliding window, and constructs a local map based on the sliding window. The key frame is the optimized historical key frame data. Based on the combination of the historical key frame and the time frame to be optimized, it is possible to ensure that sufficient historical information is available during frame-to-map alignment, and it is also possible to ensure that the local map constructed based on the sliding window has an appropriate scale and sufficient common view features, thereby significantly improving the robustness and accuracy of the SLAM positioning and mapping process. BRIEF DESCRIPTION OF THE DRAWINGS

[0053] The accompanying drawings are used to provide further understanding of the present invention and constitute a part of the specification. Together with the following specific embodiments, they are used to explain the present invention, but do not constitute a limitation of the present invention.

[0054] Figure 1 The present invention provides a flow chart of a SLAM method based on mixed window joint optimization.

[0055] Figure 2 A flow chart of constructing IMU pre-integration residual provided by the present invention.

[0056] Figure 3 A flowchart of constructing a local map provided by the present invention.

[0057] Figure 4 A flow chart of updating a sliding window provided by the present invention.

[0058] Figure 5 A flowchart of the joint optimization provided by the present invention.

[0059] Figure 6 This is a structural block diagram of the SLAM device based on mixed window joint optimization provided by the present invention.

[0060] Figure 7 The present invention provides a block diagram of the SLAM system based on mixed window joint optimization. DETAILED DESCRIPTION

[0061] It should be noted that, in the absence of conflict, the embodiments of the present invention and the features in the embodiments may be combined with each other. The present invention will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.

[0062] In order to enable those skilled in the art to better understand the scheme of the present invention, the technical scheme in the embodiment of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiment of the present invention. Obviously, the described embodiment is only a part of the embodiment of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work should fall within the scope of protection of the present invention.

[0063] It should be noted that the terms "first", "second", etc. in the specification and claims of the present invention and the above-mentioned drawings are used to distinguish similar objects, and are not necessarily used to describe a specific order or sequence. It should be understood that the data used in this way can be interchanged where appropriate, so as to describe the embodiments of the present invention described herein. In addition, the terms "including" and "having" and any variations thereof are intended to cover non-exclusive inclusions, for example, a process, method, system, product or device that includes a series of steps or units is not necessarily limited to those steps or units clearly listed, but may include other steps or units that are not clearly listed or inherent to these processes, methods, products or devices.

[0064] In this embodiment, a SLAM method based on mixed window joint optimization is provided. Figure 1 is a flow chart of a SLAM method based on mixed window joint optimization according to an embodiment of the present invention, such as Figure 1 As shown, including:

[0065] S100, acquiring laser radar point cloud data, and extracting feature points from the laser radar point cloud data to obtain point cloud feature point data;

[0066] In the embodiment of the present invention, laser radar point cloud data is obtained, and feature points are extracted after dedistortion processing of the laser radar point cloud data to obtain point cloud feature point data. The point cloud feature point data is used for subsequent sliding window construction and local map matching.

[0067] S200, acquiring IMU data between two adjacent point cloud frame sampling moments, and constructing an IMU pre-integration residual according to the IMU data;

[0068] In an embodiment of the present invention, IMU data between the instants of adoption of the above-mentioned two adjacent point cloud frames is obtained, and an IMU pre-integration residual is constructed after IMU pre-integration calculation is performed based on the IMU data. It should be understood that the result after the IMU pre-integration calculation can assist in dedistortion processing of the above-mentioned lidar point cloud data, thereby making the point cloud feature point data extracted by feature point extraction more accurate and true.

[0069] S300, constructing a sliding window of a time series frame based on a key frame and the point cloud feature point data, and constructing a local map according to the sliding window;

[0070] In the embodiment of the present invention, a sliding window is constructed based on the key frame and the time series frame of the point cloud feature point data obtained above, and a local map is constructed based on the sliding window. It should be understood that in the sliding window, the time series frame is a variable, that is, it is updated dynamically in real time according to the real-time acquisition of the point cloud feature point data, so that the sliding window is also dynamically updated.

[0071] It should be noted here that the key frame is the point cloud feature point data that has been solved, that is, the pose data of the point cloud frame finally obtained in the world coordinate system, which is the optimized point cloud data, and the timing frame is the point cloud feature point data collected in real time, that is, the point cloud feature point data to be optimized. The sliding window is composed of key frames and timing frames, so as to ensure that the local map constructed based on the sliding window has an appropriate scale and sufficient common view features, thereby significantly improving the robustness and accuracy of the SLAM positioning and mapping process.

[0072] S400, matching the point cloud feature point data with the local map to obtain a point cloud matching constraint residual;

[0073] In an embodiment of the present invention, the point cloud feature point data is matched with the local map, and the point cloud matching constraint residual is obtained based on the matching result, so as to facilitate subsequent joint optimization based on the point cloud matching constraint residual and the IMU pre-integration residual.

[0074] S500, performing joint optimization based on the point cloud matching constraint residual and the IMU pre-integration residual to obtain the position and posture of the lidar point cloud data in the world coordinate system.

[0075] In an embodiment of the present invention, the above-mentioned point cloud matching constraint residual and IMU pre-integration residual are jointly optimized, so as to effectively improve the pose optimization accuracy.

[0076] Therefore, the SLAM method based on hybrid window joint optimization provided by the present invention adopts a key frame and a timing frame to form a sliding window, and constructs a local map based on the sliding window. The key frame is the optimized historical key frame data. Based on the combination of the historical key frame and the timing frame to be optimized, it is possible to ensure that sufficient historical information is available during frame-to-map alignment, and it is also possible to ensure that the local map constructed based on the sliding window has an appropriate scale and sufficient common view features, thereby significantly improving the robustness and accuracy of the SLAM positioning and mapping process.

[0077] It should be understood that the acquisition of lidar point cloud data can be carried out simultaneously with the acquisition of IMU data. After the IMU data is acquired, a pre-integration calculation is performed based on the IMU data, and the lidar point cloud data is dedistorted based on the result of the pre-integration calculation to remove the distorted data in the lidar point cloud data, thereby preparing for the subsequent accurate extraction of feature point data.

[0078] In the embodiment of the present invention, in order to improve the calculation efficiency, it is necessary to extract feature points from the laser radar point cloud data. Here, we are only interested in points on the plane or edge, and the selected feature points are selected by curvature and distance changes. The extraction method of feature points can refer to the LOAM algorithm. In the LOAM algorithm, by calculating the curvature of each point cloud in the input original point cloud set Q and comparing it with the curvature threshold, the plane points can be extracted. The following is a detailed description of the implementation of the process.

[0079] First, eleven consecutive points are selected on each laser radar line, represented as point set q = {q i ,q i+1 ,…q i+10}, in three-dimensional space, points on a smooth plane have smaller distance differences from other points on the same laser line, and therefore have lower curvature.

[0080] To calculate the midpoint q j The curvature of the object is simple to calculate and consumes less computing resources. The specific calculation is as follows:

[0081]

[0082] By calculating each point q j The curvature of the point cloud set Q can be used to obtain the curvature information of all points in the point cloud set Q. Then, by setting the edge point and plane point curvature thresholds, the single-frame radar point cloud is extracted as an edge feature point set F o and the plane feature point set F p , which provides an important information basis for the subsequent registration and local map optimization process.

[0083] In an embodiment of the present invention, the IMU data between the sampling moments of two adjacent point cloud frames is obtained, and the IMU pre-integration residual is constructed according to the IMU data, such as Figure 2 As shown, including:

[0084] S210, performing pre-integration calculation according to the IMU data between two adjacent point cloud frame sampling moments to obtain a pre-integration amount;

[0085] It should be understood that the pose optimization will continuously update the transformation of the carrier relative to the reference frame. In order to avoid repeated integration of IMU measurements during the iteration process, the IMU pre-integration is introduced to calculate the velocity, position and attitude pre-integration increment of the carrier relative to the integration starting point, eliminating the influence of the pose update of the integration starting point on the IMU data integration. This process can greatly reduce the amount of IMU state update calculations and is widely used in IMU-related nonlinear optimization systems.

[0086] S220, performing pre-integration constraints according to the pre-integration amount, and constructing an IMU pre-integration residual according to the pre-integration constraint result.

[0087] In the embodiment of the present invention, it is known that b i-1 The speed, position and attitude of the system relative to the n system are calculated using the IMU high-frequency measurement. i The speed of the system relative to the n system Location and posture

[0088]

[0089] The above formula [t i-1 ,t i The integral part of the IMU measurement data in the time period is defined as the pre-integrated velocity increment Pre-integrated position increment and pre-integrated attitude increment

[0090]

[0091] t k-1 ∈[t i-1 ,t i ]、t k ∈[t i-1 ,t i ] is the measurement time of two adjacent IMUs. k-1 The pre-integrated increment at time, using t k-1 and t k Update the mean ratio and mean angular velocity at time t k Moment pre-integration increment:

[0092]

[0093] The above formula is [t i-1 ,t i ] Multiple IMU measurements are recursively extrapolated within a period to obtain pre-integrated quantities by is the optimization variable, t i-1 Status x at the moment i-1 and t iStatus x at the moment i The time pre-integration constraints are as follows:

[0094]

[0095] The above formula [·] xyz Represents the three-dimensional vector composed of the imaginary part of the rotation quaternion. Based on the above formula, the IMU pre-integration residual is constructed:

[0096] r B (x i-1 ,x i )=||r B (x i-1 ,x i )|| 2 .

[0097] In an embodiment of the present invention, a sliding window of a time series frame based on a key frame and the point cloud feature point data is constructed, and a local map is constructed according to the sliding window, such as Figure 3 As shown, including:

[0098] S310, determining the time sequence frames adjacent to the historical key frame and the current frame;

[0099] In the embodiment of the present invention, the historical key frame is the position and posture of the optimized point cloud frame in the world coordinate system, and the adjacent time sequence frame at the current frame moment is the single-frame point cloud data of the laser radar collected in real time.

[0100] Specifically, historical keyframes and the adjacent time frames of the current frame

[0101] S320, determining the historical key frame as a sliding window quantification, and determining the adjacent time sequence frame of the current frame as a sliding window variable;

[0102] In the embodiment of the present invention, the position of the historical key frame relative to the n system is a fixed value, and the position of the time series frame relative to the n system is a variable, that is, the time series frame It is the sliding window frame to be optimized.

[0103] In order to integrate the LiDAR registration constraint and the IMU pre-integration constraint, the present invention uniformly adopts the transformation of b system to n system Construct the registration constraints and pre-integration constraints. The transformation of the l system to the n system can be obtained by It is expressed as:

[0104]

[0105] The current frame state and the timing frame state in the optimized sliding window together constitute the optimized state set X = [x i-N ,...,xi ].

[0106] It should be noted that, in the embodiment of the present invention, the b system represents the vehicle body coordinate system, the n system represents the IMU coordinate system, and the l system represents the radar coordinate system.

[0107] S330, forming a sliding window according to the sliding window quantity and the sliding window variable;

[0108] S340, determining the relative pose of the laser radar corresponding to each point cloud frame in the sliding window according to the IMU pre-integration residual and the carrier pose estimation;

[0109] S350, projecting the relative position of the laser radar corresponding to each frame in the sliding window to the IMU coordinate system to obtain a local map.

[0110] The local map is composed of key frame features and time frame features in the sliding window. Combining the pre-integration residual formula and the carrier pose estimation Get the relative position of each frame of LiDAR in the sliding window And project the corresponding frame features to the n system to complete the local map construction.

[0111] In an embodiment of the present invention, constructing a sliding window of a time series frame based on a key frame and the point cloud feature point data, and constructing a local map according to the sliding window, further includes:

[0112] Update the sliding window according to the real-time point cloud feature point data.

[0113] It should be understood that, since the time series frame is a variable, the window to be optimized composed of the time series frame is dynamically updated according to the real-time point cloud feature point data to update the sliding window.

[0114] Specifically, the sliding window is updated according to the real-time point cloud feature point data, such as Figure 4 As shown, including:

[0115] S361, obtaining the latest point cloud frame pose solution result according to the real-time point cloud feature point data;

[0116] S362, adding the latest point cloud frame pose solution result to the sliding window variable of the sliding window;

[0117] S363, performing key frame detection on the point cloud frame with the furthest time in the sliding window variable;

[0118] In an embodiment of the present invention, the latest point cloud pose solution result is obtained by solving the real-time point cloud feature point data, and the latest point cloud frame pose solution result is added to the sliding window variable, that is, added to the sliding window to be optimized, and the feature similarity between the oldest frame to be optimized and the latest key frame in the sliding window to be optimized is calculated, and the calculated feature similarity result is compared with the adaptive threshold determined based on the motion information, so as to select the key frame and update the size of the sliding window from then on.

[0119] Specifically, performing key frame detection on the point cloud frame with the furthest time in the sliding window variable includes:

[0120] 1) Calculating the feature similarity between the furthest point cloud frame in the sliding window variable and the closest historical key frame in the sliding window quantification to obtain a feature similarity calculation result;

[0121] 2) comparing the feature similarity calculation result with an adaptive threshold determined based on motion information;

[0122] 3) If the feature similarity calculation result is greater than the adaptive threshold, determining that the point cloud frame with the furthest time in the sliding window variable meets the key frame threshold;

[0123] 4) If the feature similarity calculation result is not greater than the adaptive threshold, it is determined that the furthest point cloud frame in the sliding window variable does not meet the key frame threshold.

[0124] In the embodiment of the present invention, the adaptive threshold determined based on the motion information at least includes an adaptive threshold determined based on the Euler angle, and the expression of the adaptive threshold is:

[0125]

[0126] Among them, t represents the adaptive threshold, t1 represents the fixed threshold used in stable conditions, t2 represents the angle transformation threshold, and ΔR represents the modulus of the Euler angle between the two latest point cloud frames.

[0127] S364: If the furthest point cloud frame in the sliding window variable satisfies the key frame threshold, the furthest point cloud frame in the sliding window variable is added to the sliding window quantification of the sliding window, and the furthest key frame in history in the sliding window quantification is deleted;

[0128] S365: If the point cloud frame with the furthest time in the sliding window variable does not meet the key frame threshold, the point cloud frame with the furthest time in the sliding window variable is deleted.

[0129] In an embodiment of the present invention, the size of the local map is composed of key frames and frames to be optimized (i.e., time-series frames). The distance threshold between key frames can ensure that the local map has an appropriate size, and adjacent time-series frames can provide more common visual features for frame-local map registration. When the pose solution of the i-th frame is completed, it is added to the optimization sliding window as a frame to be optimized. In order to avoid a continuous increase in the amount of state to be optimized, the farthest iN frame needs to be removed from the optimization sliding window, and the key frame detection is performed on the frame. If it meets the key frame threshold, it is set to the latest key frame and the farthest key frame in history is deleted. If it does not meet the key frame threshold, the frame is deleted from the sliding window.

[0130] Specifically, the edge and face features are first downsampled and then projected onto the two-dimensional feature matrix FM along the height axis of the point cloud. Each cell in FM contains a feature vector FV, which stores the height information of the points projected to the same cell. When a new frame is received, its FM is calculated and matched with the FM of the latest key frame to calculate their feature similarity. Subsequently, the feature similarity is compared with an adaptive threshold determined based on motion information for key frame selection. The adaptive threshold is intended to exclude key frames in unstable situations. In the following description of the present invention, the FM of the current frame is denoted as C and the FM of the latest key frame is denoted as L.

[0131] 1) Feature projection: The projection of the feature point cloud is performed along the height direction, and the point cloud is divided into units at equal intervals in the radial and tangential directions. In the radial and tangential directions, each unit of the segmentation corresponds to a row and a column of the feature matrix FM, respectively. It should be noted that the projection is based on the local coordinate system of the current posture of the LiDAR, thereby avoiding the introduction of cumulative positioning errors. In addition, since the feature similarity calculation only involves the latest key frame, the computational cost is low.

[0132] 2) Feature similarity calculation: Each FM consists of N c ×N r FVs, each of which stores the height of the point corresponding to its horizontal position, reflecting the geometric structure of the environment. Therefore, the similarity between FVs with the same index in C and L reflects the degree of change in geometric features between the current frame and the latest keyframe. In addition, the degree of feature change between two frames can be calculated by summarizing the feature similarities of all corresponding FVs in C and L.

[0133] It is worth noting that the sizes of FVs vary, and some are even zero-dimensional. To measure the distance between two FVs, the FV with lower dimension is supplemented by points with height 0 to ensure that they have the same dimension. In addition, each FV is sorted in descending order, ensuring that points with larger heights are matched together, and points with smaller heights are also matched accordingly. Each new frame goes through these procedures to obtain its FM and FV.

[0134] Based on the number of features contained in the FV, the distance calculation between the FVs with the same index in C and L can be divided into three cases. First, if both matching FVs are empty, it means that there is no feature at that position, they do not participate in the feature similarity calculation, and are subsequently filtered out. Second, if only one FV is empty, it indicates that the features at that horizontal position have changed significantly. Finally, if both FVs store points, the cosine similarity of the two is calculated as the feature similarity, reflecting the difference between the two. Finally, the feature similarity of each FV is calculated as follows:

[0135]

[0136] Where i and j represent the row and column indices of the feature matrix FM, respectively. ij and L ij is the feature vector FV of the current frame C and the latest keyframe L at position (i,j). s(i,j) is the similarity of the FVs of C and L at position (i,j). The more similar the two FVs are, the larger s(i,j) is. After traversing C and aligning with L, the feature similarity calculation of all FVs is completed. Next, the cumulative feature similarity of each FV is weighted using the global feature distance to quantify the difference between the two frames, as shown in the following formula. As the robot moves, new features will continue to appear frequently in the distance. Therefore, higher weights are assigned to points that are farther away to perceive feature changes more sensitively.

[0137]

[0138] Where d is the global feature distance and n is the number of non-empty FVs at the same index in C and L. The more similar C and L are, the smaller d is.

[0139] When the robot remains stationary, the features stored in each FV change very little, resulting in a small value of d. Although dynamic objects may affect the feature distribution of certain local FVs, the embodiment of the present invention takes into account all corresponding FVs in C and L, and is therefore robust to dynamic objects. When the robot enters a moving state, d increases, indicating that the features are gradually evolving. Once d exceeds the feature distance threshold, it indicates that there is a significant difference between the current frame and the latest keyframe. Traditional keyframe extraction methods typically include spatial and rotational constraints, and it is difficult to effectively adapt to setting thresholds for different vehicle speeds and operating conditions. In contrast, the present embodiment focuses on monitoring the degree of feature changes and is highly robust under different operating scenarios.

[0140] 3) Adaptive threshold selection: The quality of keyframes directly affects the accuracy of LiDAR odometer and mapping, so the stability of keyframes needs to be guaranteed. Traditional keyframes are selected based on feature distance calculation and threshold comparison between frames. However, under unstable working conditions, such as when a vehicle encounters a speed bump, the extracted features are significantly different from their preceding frames and should not be classified as keyframes, otherwise erroneous map information will be introduced, and the error is irreversible. Therefore, an embodiment of the present invention uses motion data integration to establish a motion adaptive feature threshold, thereby avoiding incorporating frames captured during unstable working conditions into the map.

[0141] The operating condition is evaluated by calculating the Euler angles between the two latest frames, because the rotation change more accurately reflects the intensity of motion. Therefore, as shown in the following formula, the adaptive adjustment of the feature distance threshold depends on the rotation information. For operating conditions with large rotation changes, the threshold is increased accordingly to avoid taking it as a keyframe at the moment of intense motion. It should be noted that this adaptation does not hinder the keyframe selection under normal conditions.

[0142]

[0143] Among them, t represents the adaptive threshold, t1 represents the fixed threshold used in stable conditions, t2 represents the angle transformation threshold, and ΔR represents the modulus of the Euler angle between the two latest point cloud frames.

[0144] It should be noted that when selecting key frames, by judging the relative rotation angle, relative Euclidean distance change and time interval between the current frame and the previous key frame, only the corresponding threshold needs to be set. If any one of the conditions is met, the frame point cloud can be set as a key frame. This method can also filter key frames by comparing the change amount with the set threshold.

[0145] In an embodiment of the present invention, a joint optimization is performed based on the point cloud matching constraint residual and the IMU pre-integration residual, such as Figure 5 As shown, including:

[0146] S510, constructing an optimized frame matching residual and an inter-frame pre-integration residual according to the point cloud matching constraint residual and the IMU pre-integration residual;

[0147] S520, obtaining a target optimization function of a sliding variable in a sliding window according to the optimized frame registration residual and the inter-frame pre-integration residual;

[0148] S530, performing minimization on the target optimization function, and updating the sliding variable in the sliding window according to the minimization result;

[0149] S540, performing joint nonlinear optimization on the target optimization function according to the point cloud feature point data and the IMU data to obtain a maximum a posteriori estimate of the sliding variable in the sliding window.

[0150] In the embodiment of the present invention, a weighted multimodal registration residual is constructed between the i-th frame and the local map. For ease of expression, the transformation matrix T is used here instead of the rotation quaternion q and the translation vector t:

[0151]

[0152] in, They represent the surface feature set and line feature set for building the registration constraints respectively. Combining the above formula with the pre-integration residual formula, we construct the optimized frame registration residual and inter-frame pre-integration residual, and obtain the target optimization function for the state set X:

[0153]

[0154] Minimize the above equation, obtain and update the state set X, perform joint nonlinear optimization, combine the relative lidar measurement and IMU pre-integration to obtain the maximum a posteriori estimate of the state.

[0155] In summary, the SLAM method based on hybrid window joint optimization provided by the present invention combines key frames and time series frames to form a sliding window, ensures the distance threshold between key frames, and keeps the local map at an appropriate size. In addition, adjacent time series frames provide rich common view features, which significantly improves the robustness and accuracy of laser SLAM in the positioning and mapping process. In addition, the embodiment of the present invention extracts stable key frames based on feature similarity and dynamically adjusted motion thresholds, thereby ensuring the appropriate size of the local map and further improving the accuracy of positioning and mapping.

[0156] As another embodiment of the present invention, a SLAM device 100 based on mixed window joint optimization is provided, which is used to implement the SLAM method based on mixed window joint optimization as described above, wherein: Figure 6 As shown, including:

[0157] The point cloud feature extraction module 110 is used to obtain laser radar point cloud data and extract feature points from the laser radar point cloud data to obtain point cloud feature point data;

[0158] A pre-integration residual construction module 120 is used to obtain IMU data between two adjacent point cloud frame sampling moments and construct an IMU pre-integration residual according to the IMU data;

[0159] A local map construction module 130, configured to construct a sliding window of a time series frame based on a key frame and the point cloud feature point data, and to construct a local map according to the sliding window;

[0160] A point cloud matching constraint residual obtaining module 140 is used to match the point cloud feature point data with the local map to obtain a point cloud matching constraint residual;

[0161] The joint optimization module 150 is used to perform joint optimization based on the point cloud matching constraint residual and the IMU pre-integration residual to obtain the position and posture of the lidar point cloud data in the world coordinate system.

[0162] In an embodiment of the present invention, the point cloud feature extraction module 110 obtains a single-frame point cloud of the laser radar and extracts features through curvature analysis. The pre-integration residual construction module 120 is used to obtain the IMU data between adjacent laser radar frames and the point cloud data at each sampling moment, perform IMU pre-integration and construct pre-integration residuals. The local map construction module 130 combines key frames and time series frames to maintain the local sliding window. By calculating the similarity between the oldest frame to be optimized and the latest key frame, and comparing it with the adaptive threshold, it is determined whether the current frame is used as a key frame. In addition, a local map with a fixed number of frames in the moving sliding window is generated to support the matching process of the laser radar frame. The point cloud matching constraint residual acquisition module 140 and the joint optimization module 150 jointly optimize the residual of the laser radar single-frame point cloud and the local map construction and the IMU pre-integration residual. Finally, the CERES solver is used to optimize the maximum a posteriori estimate to solve the pose, and the laser radar point cloud of each frame is transformed and spliced ​​according to the optimized pose results to generate a globally consistent point cloud map.

[0163] In summary, the SLAM device based on hybrid window joint optimization provided by the embodiment of the present invention adopts key frames and timing frames to form a sliding window, and constructs a local map based on the sliding window. The key frame is the optimized historical key frame data. Based on the combination of the historical key frame and the timing frame to be optimized, it is possible to ensure that sufficient historical information is available during frame-to-map alignment, and it is also possible to ensure that the local map constructed based on the sliding window has an appropriate scale and sufficient common view features, thereby significantly improving the robustness and accuracy of the SLAM positioning and mapping process.

[0164] The specific working principle of the SLAM device based on mixed window joint optimization provided by the embodiment of the present invention can refer to the description of the SLAM method based on mixed window joint optimization in the previous text, which will not be repeated here.

[0165] As another embodiment of the present invention, a SLAM system 10 based on mixed window joint optimization is provided, wherein: Figure 7As shown, it includes: an inertial measurement device 200, a laser radar device 300 and the SLAM device 100 based on mixed window joint optimization as described above, and the inertial measurement device 200 and the laser radar device 300 are both communicatively connected with the SLAM device 100 based on mixed window joint optimization;

[0166] The inertial measurement unit 200 is used to provide IMU data;

[0167] The laser radar device 300 is used to provide laser radar point cloud data;

[0168] The SLAM device 100 based on mixed window joint optimization is used to obtain fused pose information after processing the laser radar point cloud data and the IMU data.

[0169] In an embodiment of the present invention, the inertial measurement device may specifically include an IMU, and the laser radar device may specifically include a LIDAR.

[0170] Specifically, the IMU provides the vehicle with accelerometer and angular velocity information, while the LIDAR provides high-precision point cloud data. The SLAM device 100 based on hybrid window joint optimization is responsible for storing sensor data information and computer execution instructions, receiving data from the two sensors, and processing them to obtain high-precision fused pose information, thereby realizing some or all steps of the SLAM method based on hybrid window joint optimization in the above text.

[0171] In summary, the SLAM system based on hybrid window joint optimization provided by the embodiment of the present invention adopts the SLAM device based on hybrid window joint optimization mentioned above, and constructs a local map based on the sliding window by using key frames and timing frames to form a sliding window. The key frame is the optimized historical key frame data. Based on the combination of the historical key frame and the timing frame to be optimized, it is possible to ensure that sufficient historical information is available during frame-to-map alignment, and it is also possible to ensure that the local map constructed based on the sliding window has an appropriate scale and sufficient common view features, thereby significantly improving the robustness and accuracy of the SLAM positioning and mapping process.

[0172] The specific working principle of the SLAM system based on mixed window joint optimization in the embodiment of the present invention can be referred to the description of the SLAM method based on mixed window joint optimization in the above text, which will not be repeated here.

[0173] It is to be understood that the above embodiments are merely exemplary embodiments used to illustrate the principles of the present invention, but the present invention is not limited thereto. For those of ordinary skill in the art, various modifications and improvements can be made without departing from the spirit and essence of the present invention, and these modifications and improvements are also considered to be within the scope of protection of the present invention.

Claims

1. A SLAM method based on mixed window joint optimization, characterized in that: include: Acquire laser radar point cloud data, and extract feature points from the laser radar point cloud data to obtain point cloud feature point data; Acquire IMU data between sampling moments of two adjacent point cloud frames, and construct an IMU pre-integration residual based on the IMU data; Constructing a sliding window of a time series frame based on a key frame and the point cloud feature point data, and constructing a local map according to the sliding window; Matching the point cloud feature point data with the local map to obtain a point cloud matching constraint residual; The point cloud matching constraint residual and the IMU pre-integration residual are jointly optimized to obtain the position and pose of the lidar point cloud data in the world coordinate system.

2. The SLAM method based on mixed window joint optimization according to claim 1, characterized in that, Constructing a sliding window of a time series frame based on a key frame and the point cloud feature point data, and constructing a local map according to the sliding window, including: Determine the time sequence frames adjacent to the historical key frame and the current frame; Determine the historical key frame as a sliding window quantification, and determine the adjacent time sequence frame of the current frame as a sliding window variable; Forming a sliding window according to the sliding window quantity and the sliding window variable; Determine the relative position of the lidar corresponding to each point cloud frame in the sliding window based on the IMU pre-integration residual and the carrier pose estimation; The relative position of the lidar corresponding to each frame in the sliding window is projected to the IMU coordinate system to obtain a local map.

3. The SLAM method based on mixed window joint optimization according to claim 2, characterized in that, Constructing a sliding window of a time series frame based on a key frame and the point cloud feature point data, and constructing a local map according to the sliding window, further comprising: Update the sliding window according to the real-time point cloud feature point data.

4. The SLAM method based on mixed window joint optimization according to claim 3, characterized in that, Update the sliding window according to the real-time point cloud feature point data, including: Obtain the latest point cloud frame pose solution result based on real-time point cloud feature point data; Adding the latest point cloud frame pose solution result to the sliding window variable of the sliding window; Performing key frame detection on the point cloud frame with the furthest time in the sliding window variable; If the furthest point cloud frame in the sliding window variable satisfies the key frame threshold, the furthest point cloud frame in the sliding window variable is added to the sliding window quantification of the sliding window, and the furthest key frame in history in the sliding window quantification is deleted; If the point cloud frame with the furthest time in the sliding window variable does not meet the key frame threshold, the point cloud frame with the furthest time in the sliding window variable is deleted.

5. The SLAM method based on mixed window joint optimization according to claim 4, characterized in that, Performing key frame detection on the point cloud frame with the furthest time in the sliding window variable includes: Calculate the feature similarity between the furthest point cloud frame in the sliding window variable and the closest historical key frame in the sliding window quantification to obtain a feature similarity calculation result; Comparing the feature similarity calculation result with an adaptive threshold determined based on motion information; If the feature similarity calculation result is greater than the adaptive threshold, determining that the point cloud frame with the furthest time in the sliding window variable meets the key frame threshold; If the feature similarity calculation result is not greater than the adaptive threshold, it is determined that the furthest point cloud frame in the sliding window variable does not meet the key frame threshold.

6. The SLAM method based on mixed window joint optimization according to claim 5, characterized in that, The adaptive threshold determined based on the motion information at least includes an adaptive threshold determined based on the Euler angle, and the expression of the adaptive threshold is: Among them, t represents the adaptive threshold, t1 represents the fixed threshold used in stable conditions, t2 represents the angle transformation threshold, and ΔR represents the modulus of the Euler angle between the two latest point cloud frames.

7. The SLAM method based on mixed window joint optimization according to claim 1, characterized in that, Acquire the IMU data between the sampling moments of two adjacent point cloud frames, and construct the IMU pre-integration residual according to the IMU data, including: Perform pre-integration calculation based on the IMU data between the sampling moments of two adjacent point cloud frames to obtain the pre-integration amount; Pre-integration constraints are performed according to the pre-integration amount, and an IMU pre-integration residual is constructed according to the pre-integration constraint result.

8. The SLAM method based on mixed window joint optimization according to claim 1, characterized in that, Jointly optimizing the point cloud matching constraint residual and the IMU pre-integration residual includes: Constructing optimized frame matching residuals and inter-frame pre-integration residuals according to the point cloud matching constraint residuals and the IMU pre-integration residuals; Obtaining a target optimization function of a sliding variable in a sliding window according to the optimized frame registration residual and the inter-frame pre-integration residual; Minimizing the target optimization function and updating the sliding variable in the sliding window according to the minimization result; The target optimization function is jointly nonlinearly optimized according to the point cloud feature point data and the IMU data to obtain the maximum a posteriori estimate of the sliding variable in the sliding window.

9. A SLAM device based on mixed window joint optimization, used to implement the SLAM method based on mixed window joint optimization described in any one of claims 1 to 8, characterized in that: include: A point cloud feature extraction module is used to obtain laser radar point cloud data and extract feature points from the laser radar point cloud data to obtain point cloud feature point data; A pre-integration residual construction module is used to obtain IMU data between two adjacent point cloud frame sampling moments and construct an IMU pre-integration residual based on the IMU data; A local map construction module, used to construct a sliding window of a time series frame based on a key frame and the point cloud feature point data, and to construct a local map according to the sliding window; A point cloud matching constraint residual obtaining module, used for matching the point cloud feature point data with the local map to obtain a point cloud matching constraint residual; The joint optimization module is used to perform joint optimization based on the point cloud matching constraint residual and the IMU pre-integration residual to obtain the position and posture of the lidar point cloud data in the world coordinate system.

10. A SLAM system based on mixed window joint optimization, characterized in that: include: An inertial measurement device, a laser radar device, and the SLAM device based on mixed window joint optimization according to claim 9, wherein the inertial measurement device and the laser radar device are both communicatively connected to the SLAM device based on mixed window joint optimization; The inertial measurement unit is used to provide IMU data; The laser radar device is used to provide laser radar point cloud data; The SLAM device based on mixed window joint optimization is used to obtain fused posture information after processing the laser radar point cloud data and IMU data.

Images