Slam method, apparatus and system based on hybrid window joint optimization

Abstract

The present invention relates to the technical field of SLAM, and specifically to a SLAM method, apparatus and system based on hybrid window joint optimization, comprising: acquiring LiDAR point cloud data, and performing feature point extraction on the LiDAR point cloud data to obtain point cloud feature point data; acquiring IMU data between moments at which two adjacent point cloud frames are acquired, and constructing an IMU preintegration residual according to the IMU data; constructing a sliding window based on a key frame and a time-ordered frame of 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; and performing joint optimization according to the point cloud matching constraint residual and the IMU preintegration residual to obtain a pose of the LiDAR point cloud data in the World Coordinate System. The SLAM method based on hybrid window joint optimization provided by the present invention improves robustness and accuracy of SLAM system localization and mapping.

Description

SLAM method, apparatus and system based on hybrid window joint optimization Technical Field

[0001] This invention relates to the field of SLAM technology, and in particular to a SLAM method, a SLAM device, and a SLAM system based on hybrid window joint optimization. Background Technology

[0002] With the rapid development of intelligent robots and autonomous driving technologies, Simultaneous Localization and Mapping (SLAM) has become a highly regarded research area. SLAM plays a crucial role in providing accurate real-time localization in intelligent robots and autonomous vehicles. Compared to vision-based SLAM, LiDAR (Light Laser Detection and Ranging) technology has gradually become a popular choice in the SLAM field due to its superior localization accuracy and insensitivity to lighting conditions. Compared to a single LiDAR odometry, LiDAR / IMU (Inertial Measurement Unit) odometry exhibits less pose drift, especially in complex scenarios with degraded feature registration constraints.

[0003] In practical applications, maintaining a sliding window consisting of fixed frames is a common practice. Registering the current frame with the local map formed by the sliding window and adjusting the poses of multiple frames within the window using factor map optimization techniques to ensure the appropriate size of the local map and provide sufficient common-view features presents a significant challenge. Keyframe selection strategies require extracting a single frame from a series of ordinary frames as a representative of the local map. A reasonable keyframe selection strategy can not only avoid interference from irrelevant or erroneous data in the optimization process but also effectively improve the accuracy of localization and mapping. However, existing keyframe selection strategies that rely on coarse angle and translation changes still suffer from insufficient accuracy and stability.

[0004] Therefore, how to improve the robustness and accuracy of SLAM system positioning and mapping has become a technical problem that urgently needs to be solved by those skilled in the art. Summary of the Invention

[0005] This invention provides a SLAM method, a SLAM device, and a SLAM system based on hybrid window joint optimization, which solves the problems of insufficient robustness and accuracy in localization and mapping of SLAM systems in related technologies.

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

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

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

[0009] Construct a sliding window based on keyframes and the point cloud feature point data in a time sequence, and construct a local map based on the sliding window;

[0010] The point cloud feature point data is matched with the local map to obtain the point cloud matching constraint residual;

[0011] The pose of the lidar point cloud data in the world coordinate system is obtained by jointly optimizing the point cloud matching constraint residual and the IMU pre-integration residual.

[0012] Further, a sliding window is constructed based on keyframes and the temporal frames of the point cloud feature point data, and a local map is constructed according to the sliding window, including:

[0013] Determine the historical keyframes and the adjacent time frames of the current frame;

[0014] The historical keyframes are determined as sliding window quantifications, and the time-series frames adjacent to the current frame are determined as sliding window variables;

[0015] A sliding window is formed based on the sliding window quantification and the sliding window variable;

[0016] The relative pose of the lidar corresponding to each point cloud frame within the sliding window is determined based on the IMU pre-integration residual and the carrier pose estimation.

[0017] The relative poses of the LiDAR corresponding to each frame within the sliding window are projected onto the IMU coordinate system to obtain a local map.

[0018] Furthermore, constructing a sliding window based on keyframes and the temporal frames of the point cloud feature point data, and constructing a local map based on the sliding window, also includes:

[0019] The sliding window is updated based on real-time point cloud feature point data.

[0020] Furthermore, the sliding window is updated based on real-time point cloud feature point data, including:

[0021] The latest point cloud frame pose calculation results are obtained based on real-time point cloud feature point data;

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

[0023] Keyframe detection is performed on the farthest point cloud frame in the sliding window variables;

[0024] If the furthest point cloud frame in the sliding window variables meets the keyframe threshold, then the furthest point cloud frame in the sliding window variables is added to the sliding window quantification of the sliding window, and the furthest historical keyframe in the sliding window quantification is deleted.

[0025] If the furthest point cloud frame in the sliding window variables does not meet the keyframe threshold, then the furthest point cloud frame in the sliding window variables will be deleted.

[0026] Furthermore, keyframe detection is performed on the furthest point cloud frame in the sliding window variable, including:

[0027] The feature similarity calculation results are obtained by comparing the furthest point cloud frame in the sliding window variables with the most recent historical key frame in the sliding window quantification.

[0028] The feature similarity calculation result is compared with an adaptive threshold determined based on motion information;

[0029] If the feature similarity calculation result is greater than the adaptive threshold, then the point cloud frame with the furthest time among the sliding window variables is determined to meet the keyframe threshold.

[0030] If the calculated result of the feature similarity is not greater than the adaptive threshold, then it is determined that the point cloud frame with the furthest time in the sliding window variable does not meet the keyframe threshold.

[0031] Furthermore, the adaptive threshold determined based on motion information includes at least an adaptive threshold determined based on Euler angles, and the expression for the adaptive threshold is:

[0032] ,

[0033] in, Indicates an adaptive threshold. This represents a fixed threshold used in steady-state conditions. Indicates the threshold for angle transformation. This represents the modulus of the Euler angle between the two latest point cloud frames.

[0034] Further, IMU data is acquired between sampling times of two adjacent point cloud frames, and IMU pre-integration residuals are constructed based on the IMU data, including:

[0035] Pre-integration is performed based on the IMU data between two adjacent point cloud frame sampling times to obtain the pre-integrated quantity;

[0036] Pre-integration constraints are performed based on the pre-integral quantity, and the IMU pre-integration residuals are constructed based on the pre-integration constraint results.

[0037] Furthermore, joint optimization is performed based on the point cloud matching constraint residual and the IMU pre-integration residual, including:

[0038] Based on the point cloud matching constraint residual and the IMU pre-integration residual, construct the optimized frame matching residual and the inter-frame pre-integration residual;

[0039] The objective optimization function of the sliding variable in the sliding window is obtained based on the optimized frame registration residual and the inter-frame pre-integration residual.

[0040] The objective optimization function is minimized, and the sliding variables in the sliding window are updated based on the minimization result.

[0041] The objective optimization function is jointly nonlinearly optimized based on 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 apparatus based on hybrid window joint optimization is provided for implementing the SLAM method based on hybrid window joint optimization described above, wherein the apparatus includes:

[0043] The point cloud feature extraction module is used to acquire lidar point cloud data and extract feature points from the lidar point cloud data to obtain point cloud feature point data.

[0044] The pre-integration residual construction module is used to acquire IMU data between the sampling times of two adjacent point cloud frames and construct IMU pre-integration residuals based on the IMU data.

[0045] The local map construction module is used to construct a sliding window based on keyframes and the time-series frames of the point cloud feature point data, and to construct a local map based on the sliding window;

[0046] The point cloud matching constraint residual acquisition module is used to match the point cloud feature point data with the local map to obtain the 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 pose of the lidar point cloud data in the world coordinate system.

[0048] As another aspect of the present invention, a SLAM system based on hybrid window joint optimization is provided, comprising: an inertial measurement unit, a lidar unit, and the aforementioned SLAM system based on hybrid window joint optimization, wherein the inertial measurement unit and the lidar unit are both communicatively connected to the SLAM system based on hybrid window joint optimization.

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

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

[0051] The SLAM device based on hybrid window joint optimization is used to obtain fused pose information by processing the lidar point cloud data and IMU data.

[0052] The SLAM method based on hybrid window joint optimization provided by this invention uses a sliding window composed of key frames and temporal frames to construct a local map. The key frames are optimized historical key frame data. By combining the historical key frames with the temporal frames to be optimized, sufficient historical information can be ensured during frame-to-map registration. It can also 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 localization and mapping process. Attached Figure Description

[0053] The accompanying drawings are provided to further illustrate the invention and form part of the specification. They are used together with the following detailed description to explain the invention, but do not constitute a limitation thereof.

[0054] Figure 1 is a flowchart of the SLAM method based on hybrid window joint optimization provided by the present invention.

[0055] Figure 2 is a flowchart of constructing the IMU pre-integration residual provided by the present invention.

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

[0057] Figure 4 is a flowchart of the updated sliding window provided by the present invention.

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

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

[0060] Figure 7 is a structural block diagram of the SLAM system based on hybrid window joint optimization provided by the present invention. Detailed Implementation

[0061] It should be noted that, unless otherwise specified, the embodiments and features described in the present invention can be combined with each other. The present invention will now be described in detail with reference to the accompanying drawings and embodiments.

[0062] To enable those skilled in the art to better understand the present invention, the technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings of the embodiments. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort 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, claims, and accompanying drawings of this invention are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence. It should be understood that such data can be interchanged where appropriate for the embodiments of the invention described herein. Furthermore, the terms "comprising" and "having," and any variations thereof, are intended to cover a non-exclusive inclusion; for example, a process, method, system, product, or apparatus that comprises a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units not explicitly listed or inherent to such processes, methods, products, or apparatus.

[0064] This embodiment provides a SLAM method based on hybrid window joint optimization. Figure 1 is a flowchart of the SLAM method based on hybrid window joint optimization provided according to an embodiment of the present invention. As shown in Figure 1, it includes:

[0065] S100. Acquire lidar point cloud data and extract feature points from the lidar point cloud data to obtain point cloud feature point data.

[0066] In this embodiment of the invention, lidar point cloud data is acquired, and after distortion correction processing, feature points are extracted to obtain point cloud feature point data. This point cloud feature point data is used for subsequent sliding window construction and local map matching, etc.

[0067] S200: Obtain IMU data between the sampling times of two adjacent point cloud frames, and construct the IMU pre-integration residual based on the IMU data;

[0068] In this embodiment of the invention, IMU data between the times of the above two adjacent point cloud frames is obtained, and 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 of the IMU pre-integration calculation can assist the above lidar point cloud data in distortion removal processing, thereby making the point cloud feature point data extracted from the feature points more accurate and realistic.

[0069] S300. Construct a sliding window based on key frames and the point cloud feature point data in a time sequence, and construct a local map based on the sliding window;

[0070] In this embodiment of the invention, a sliding window is constructed based on the keyframes and the time-series frames 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 this sliding window, the time-series frames are variables, that is, they are dynamically updated in real time according to the real-time acquisition of the point cloud feature point data, so the sliding window is also dynamically updated.

[0071] It should be noted that keyframes are the point cloud feature point data that has been solved, which is the pose data of the final point cloud frame in the world coordinate system. They are optimized point cloud data. On the other hand, time-series frames are point cloud feature point data collected in real time, which are the point cloud feature point data to be optimized. The sliding window is composed of keyframes and time-series frames, which can ensure that the local map built based on the sliding window has an appropriate scale and sufficient common-view features, thereby significantly improving the robustness and accuracy of the SLAM localization and mapping process.

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

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

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

[0075] In this embodiment of the invention, the pose optimization accuracy can be effectively improved by jointly optimizing the point cloud matching constraint residual and the IMU pre-integration residual.

[0076] Therefore, the SLAM method based on hybrid window joint optimization provided by the present invention uses a sliding window composed of key frames and temporal frames, and constructs a local map based on the sliding window. The key frames are the optimized historical key frame data. The combination of the historical key frames and the temporal frames to be optimized can ensure that there is sufficient historical information when registering frames to maps, and can also 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 localization 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, pre-integration calculation is performed based on the IMU data, and distortion removal processing is performed on the LiDAR point cloud data based on the result of the pre-integration calculation to remove distorted data in the LiDAR point cloud data, thereby preparing for the subsequent accurate extraction of feature point data.

[0078] In this embodiment of the invention, to improve computational efficiency, feature point extraction is required from the LiDAR point cloud data. Here, only points on a plane or edge are of interest, and the selected feature points are chosen based on curvature and distance variations. The feature point extraction method can refer to the LOAM algorithm. In this LOAM algorithm, the input original point cloud set is calculated... The curvature of each point cloud is calculated, and by comparing it with a curvature threshold, planar points can be extracted. The following is a detailed description of the process.

[0079] First, select eleven consecutive points on each lidar line, representing them as a point set. In three-dimensional space, the distance difference between a point on a smooth plane and other points on the same laser line is small, thus having a low curvature.

[0080] To calculate the midpoint The curvature is calculated using a simple method with relatively low computational resource consumption. The specific calculation is as follows:

[0081] ,

[0082] By calculating each point The curvature can be used to obtain a point cloud set. The curvature information of all points is then extracted. Then, by setting curvature thresholds for edge points and planar points, a single-frame radar point cloud is extracted into a set of edge feature points. and the set of planar feature points This provides an important information foundation for the subsequent registration and local map optimization processes.

[0083] In this embodiment of the invention, IMU data between sampling times of two adjacent point cloud frames is acquired, and IMU pre-integration residuals are constructed based on the IMU data, as shown in Figure 2, including:

[0084] S210. Perform pre-integration calculation based on the IMU data between the sampling times of two adjacent point cloud frames to obtain the pre-integration quantity;

[0085] It should be understood that pose optimization continuously updates the transformation of the carrier relative to the reference frame. To avoid repeatedly integrating IMU measurements during iteration, IMU pre-integration is introduced to calculate the velocity, position, and attitude pre-integration increments of the carrier relative to the integration starting point, eliminating the influence of the pose update at the integration starting point on IMU data integration. This process can significantly reduce the computational load of IMU state updates and is widely used in IMU-related nonlinear optimization systems.

[0086] S220. Perform pre-integration constraints based on the pre-integration quantity, and construct the IMU pre-integration residual based on the pre-integration constraint result.

[0087] In embodiments of the present invention, it is known that Relative The velocity, position, and attitude of the system are calculated using high-frequency measurements from an IMU. Relative The speed of the system ,Location and posture :

[0088] ,

[0089] ,

[0090] ,

[0091] The above formula The integral portion of the IMU measurement data within the time period is defined as the pre-integral velocity increment. Pre-integral position increment and pre-integral attitude increment :

[0092] ,

[0093] ,

[0094] ,

[0095] These are the measurement times of two adjacent IMUs. (Known) Time-based pre-integral increment, utilizing and Updates to the mean ratio and mean angular velocity at time points Pre-integral increment at time:

[0096] ,

[0097] ,

[0098] ,

[0099] The above formula is correct. Multiple IMU measurements within a time period are recursively calculated to obtain the pre-integral quantity. .by To optimize variables, Moment State and Moment State The pre-integral constraints are as follows:

[0100] ,

[0101] The above formula Representing a three-dimensional vector composed of the imaginary parts of rotated quaternions, the IMU pre-integral residual is constructed based on the above formula:

[0102] .

[0103] In this embodiment of the invention, a sliding window based on keyframes and the point cloud feature point data is constructed, and a local map is constructed according to the sliding window, as shown in Figure 3, including:

[0104] S310. Determine the historical keyframes and the time-series frames adjacent to the current frame.

[0105] In this embodiment of the invention, the historical keyframe is the pose of the optimized point cloud frame in the world coordinate system. The adjacent time frames at the current frame are the real-time acquired single-frame point cloud data from the lidar.

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

[0107] S320. The historical keyframes are determined as sliding window variables, and the time-series frames adjacent to the current frame are determined as sliding window variables.

[0108] In this embodiment of the invention, historical keyframes are relative The position of the system For a fixed value, the time frame is relative. The pose of the system is a variable, i.e., sequence frame. The sliding window frame needs optimization.

[0109] To integrate LiDAR registration constraints and IMU pre-integration constraints, this invention adopts a unified approach. Relative System conversion Construct registration constraints and pre-integration constraints. Relative The conversion of systems can be achieved by , Represented as:

[0110] .

[0111] The current frame state and the temporal frame states within the optimized sliding window together constitute the optimized state set. .

[0112] It should be noted that, in the embodiments of the present invention The system represents the vehicle body coordinate system. The system represents the IMU coordinate system. The system represents the radar coordinate system.

[0113] S330. A sliding window is formed based on the sliding window quantity and the sliding window variable;

[0114] S340. Determine the relative pose of the lidar corresponding to each point cloud frame within the sliding window based on the IMU pre-integration residual and the carrier pose estimation.

[0115] S350. Project the relative pose of the LiDAR corresponding to each frame in the sliding window onto the IMU coordinate system to obtain a local map.

[0116] The local map is composed of keyframe features within the sliding window and temporal frame features. This is combined with the pre-integration residual formula and carrier pose estimation. Obtain the relative pose of LiDAR in each frame within the sliding window. And project the corresponding frame features onto The system has completed the construction of a local map.

[0117] In this embodiment of the invention, constructing a sliding window based on keyframes and the point cloud feature point data in a time sequence, and constructing a local map based on the sliding window, further includes:

[0118] The sliding window is updated based on real-time point cloud feature point data.

[0119] It should be understood that, since the time-series frames are variables, the window to be optimized, composed of time-series frames, is dynamically updated based on real-time point cloud feature point data in order to update the sliding window.

[0120] Specifically, the sliding window is updated based on real-time point cloud feature point data, as shown in Figure 4, including:

[0121] S361. Obtain the latest point cloud frame pose calculation result based on real-time point cloud feature point data;

[0122] S362. Add the latest point cloud frame pose calculation result to the sliding window variable of the sliding window;

[0123] S363. Perform keyframe detection on the farthest point cloud frame in the sliding window variables;

[0124] In this embodiment of the invention, the latest point cloud pose calculation result is obtained by solving the real-time point cloud feature point data. The latest point cloud frame pose calculation result is added to the sliding window variable, that is, added to the sliding window to be optimized. 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 motion information to select the key frame. From this point on, the size of the sliding window is updated.

[0125] Specifically, keyframe detection is performed on the furthest point cloud frame in the sliding window variable, including:

[0126] 1) Calculate the feature similarity between the furthest point cloud frame in the sliding window variables and the most recent historical keyframe in the sliding window quantification, and obtain the feature similarity calculation result;

[0127] 2) Compare the feature similarity calculation results with the adaptive threshold determined based on motion information;

[0128] 3) If the feature similarity calculation result is greater than the adaptive threshold, then the point cloud frame with the furthest time among the sliding window variables is determined to meet the key frame threshold;

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

[0130] In this embodiment of the invention, the adaptive threshold determined based on motion information includes at least an adaptive threshold determined based on Euler angles, and the expression for the adaptive threshold is:

[0131] ,

[0132] in, Indicates an adaptive threshold. This represents a fixed threshold used in steady-state conditions. Indicates the threshold for angle transformation. This represents the modulus of the Euler angle between the two latest point cloud frames.

[0133] S364. If the furthest point cloud frame in the sliding window variable meets the key frame threshold, then add the furthest point cloud frame in the sliding window variable to the sliding window quantity and delete the furthest historical key frame in the sliding window quantity.

[0134] S365. If the furthest point cloud frame in the sliding window variable does not meet the keyframe threshold, then delete the furthest point cloud frame in the sliding window variable.

[0135] In this embodiment of the invention, the local map size is composed of keyframes and frames to be optimized (instantaneous frames). A distance threshold between keyframes ensures the local map has an appropriate size, and adjacent temporal frames provide more common features for frame-local map registration. When the first... After the frame pose is calculated, it is added to the optimization window as the frame to be optimized. To prevent the number of states to be optimized from continuously increasing, the furthest frame needs to be selected. The frame is removed from the optimization sliding window, and keyframe detection is performed on it. If it meets the keyframe threshold, it is set as the latest keyframe and the oldest historical keyframe is deleted; otherwise, the frame is deleted from the sliding window.

[0136] Specifically, the edge and surface features are first downsampled, and then projected onto a two-dimensional feature matrix along the height axis of the point cloud. superior. Each unit in the array contains a feature vector. It stores the height information of points projected onto the same cell. When a new frame is received, its height is calculated. and with the latest keyframe Matching is performed to calculate their feature similarity. The feature similarity is then compared to an adaptive threshold determined based on motion information for keyframe selection. This adaptive threshold is designed to exclude keyframes in unstable situations. In the later description of the invention, the current frame's... Represented as The latest keyframe Represented as .

[0137] 1) Feature Projection: The projection of the feature point cloud is performed along the height direction. The point cloud is divided into units at equal intervals in the radial and tangential directions. Each unit in the radial and tangential directions corresponds to a feature matrix. The projection is a row and a column. It should be noted that the projection is based on the local coordinate system of the LiDAR's current pose, thus avoiding the introduction of accumulated positioning errors. Furthermore, since feature similarity calculation only involves the latest keyframe, the computational cost is low.

[0138] 2) Feature similarity calculation: Each Depend on indivual Composition, each The height of the point corresponding to its horizontal position reflects the geometry of the environment. Therefore, and Those with the same index The similarity between them reflects the degree of geometric feature change between the current frame and the latest keyframe. Furthermore, it can be summarized... and All corresponding The feature similarity is used to calculate the degree of feature change between two frames.

[0139] It is worth noting that, The size varies, and some are even zero-dimensional. To measure two... The distance between them, in lower dimensions Supplement with points of height 0 to ensure they have the same dimensions. Furthermore, each... Sort in descending order, ensuring that points with larger heights are matched together, while points with smaller heights are matched accordingly. Each new frame goes through these procedures to obtain its... and .

[0140] based on The number of features contained in and Same index in The distance calculation between two matched pairs can be divided into three cases. First, if two matched pairs... If all are empty, it means there are no features at that location, and they are not included in the feature similarity calculation, so they are filtered out. Secondly, if there is only one... An empty value indicates a significant change in the characteristics of that horizontal position. Finally, if two... If both points are stored, their cosine similarity is calculated as the feature similarity, reflecting the differences between them. Ultimately, each... The feature similarity is calculated as follows:

[0141] ,

[0142] in, and Representing the characteristic matrix respectively Row and column indexes. and In position The current frame and the latest keyframe eigenvectors . yes and In position place Similarity. Two The more similar, The larger it becomes. After traversing... and After alignment, all Feature similarity calculation has been completed. Next, global feature distance will be used for each... The cumulative feature similarity is weighted to quantify the difference between two frames, as shown in the formula below. As the robot moves, new features will frequently appear at a distance. Therefore, points that are farther away are assigned higher weights to be more sensitive to feature changes.

[0143] ,

[0144] in, It is the global feature distance. yes and Non-empty at the same index quantity. and The more similar, The smaller it is.

[0145] When the robot remains stationary, each The features stored in the middle change very little, resulting in The value is relatively small. Although dynamic objects may affect certain local areas... The characteristic distribution of the present invention is considered in this embodiment. and All correspondences Therefore, it is robust to dynamic objects. When the robot enters a state of motion, An increase indicates that the feature has gradually evolved. Once... Exceeding the feature distance threshold indicates a significant difference between the current frame and the latest keyframe. Traditional keyframe extraction methods typically include spatial and rotational constraints, making it difficult to effectively adapt threshold settings to different vehicle speeds and operating conditions. In contrast, the embodiments described herein focus on monitoring the degree of feature changes, exhibiting strong robustness under various operating scenarios.

[0146] 3) Adaptive Threshold Selection: The quality of keyframes directly affects the accuracy of LiDAR odometry and mapping, therefore, the stability of keyframes needs to be ensured. Traditionally, keyframes are selected based on feature distance calculations and threshold comparisons between frames. However, under unstable operating conditions, such as when a vehicle encounters a speed bump, the extracted features are significantly different from those of its preceding frames and should not be classified as keyframes; otherwise, incorrect map information will be introduced, and the error will be irreversible. Therefore, this embodiment of the invention uses motion data integration to establish a motion-adaptive feature threshold, thereby avoiding the inclusion of frames captured during periods of operational instability in the map.

[0147] Operational conditions are evaluated by calculating the Euler angles between the two most recent frames, as rotational changes more accurately reflect motion intensity. Therefore, as shown in the formula below, the adaptive adjustment of the feature distance threshold depends on rotational information. For operational conditions with large rotational changes, the threshold is increased accordingly to avoid using moments of intense motion as keyframes. It should be noted that this adaptation does not hinder keyframe selection under normal conditions.

[0148] ,

[0149] in, Indicates an adaptive threshold. This represents a fixed threshold used in steady-state conditions. Indicates the threshold for angle transformation. This represents the modulus of the Euler angle between the two latest point cloud frames.

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

[0151] In this embodiment of the invention, joint optimization is performed based on the point cloud matching constraint residual and the IMU pre-integration residual, as shown in Figure 5, including:

[0152] S510. Construct optimized frame matching residuals and inter-frame pre-integration residuals based on the point cloud matching constraint residuals and the IMU pre-integration residuals;

[0153] S520. Obtain the target optimization function of the sliding variable in the sliding window based on the optimized frame registration residual and the inter-frame pre-integration residual.

[0154] S530. Minimize the objective optimization function and update the sliding variables in the sliding window based on the minimization result;

[0155] S540. Perform joint nonlinear optimization on the objective optimization function based on 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.

[0156] In this embodiment of the invention, the first... Weighted multimodal registration residuals between frames and local maps. For ease of representation, a transformation matrix is ​​used here. Replace rotation quaternions Translation vector :

[0157] ,

[0158] ,

[0159] ,

[0160] in, , Let represent the surface feature set and line feature set used to construct the registration constraints, respectively. Combining the above equation and the pre-integration residual formula, we construct the optimized frame registration residual and the inter-frame pre-integration residual to obtain information about the state set. Objective optimization function:

[0161] .

[0162] Minimize the above expression to obtain and update the state set. Joint nonlinear optimization is performed, combining relative lidar measurements and IMU pre-integration to obtain the maximum a posteriori estimate of the state.

[0163] In summary, the SLAM method based on hybrid window joint optimization provided by this invention, by jointly constructing a sliding window from keyframes and temporal frames, ensures the distance threshold between keyframes, thus maintaining an appropriate size for the local map. Furthermore, adjacent temporal frames provide rich common-view features, significantly improving the robustness and accuracy of laser SLAM in the localization and mapping process. Additionally, the embodiments of this invention extract stable keyframes based on feature similarity and dynamically adjusted motion thresholds, thereby ensuring an appropriate size for the local map and further improving the accuracy of localization and mapping.

[0164] As another embodiment of the present invention, a SLAM device 100 based on hybrid window joint optimization is provided to implement the SLAM method based on hybrid window joint optimization described above, wherein, as shown in FIG6, it includes:

[0165] The point cloud feature extraction module 110 is used to acquire lidar point cloud data and extract feature points from the lidar point cloud data to obtain point cloud feature point data.

[0166] The pre-integration residual construction module 120 is used to acquire IMU data between the sampling times of two adjacent point cloud frames and construct IMU pre-integration residuals based on the IMU data.

[0167] The local map construction module 130 is used to construct a sliding window based on key frames and time-series frames of the point cloud feature point data, and to construct a local map based on the sliding window;

[0168] The point cloud matching constraint residual acquisition module 140 is used to match the point cloud feature point data with the local map to obtain the point cloud matching constraint residual.

[0169] 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 pose of the lidar point cloud data in the world coordinate system.

[0170] In this embodiment of the invention, the point cloud feature extraction module 110 acquires single-frame point clouds from the LiDAR and extracts features through curvature analysis. The pre-integration residual construction module 120 acquires IMU data between adjacent LiDAR frames and point cloud data at each sampling time, performs IMU pre-integration, and constructs pre-integration residuals. The local map construction module 130 maintains a local sliding window by combining keyframes and temporal frames. By calculating the similarity between the oldest frame to be optimized and the latest keyframe and comparing it with an adaptive threshold, it determines whether to use the current frame as a keyframe. In addition, a local map with a fixed number of frames in the moving sliding window is generated to support the matching process of LiDAR frames. The point cloud matching constraint residual acquisition module 140 and the joint optimization module 150 jointly optimize the residuals of the single-frame LiDAR point cloud, the local map construction, and the IMU pre-integration residuals. Finally, the CERES solver is used to optimize the maximum a posteriori estimate to solve the pose. By transforming and stitching the LiDAR point clouds of each frame according to the optimized pose results, a globally consistent point cloud map is generated.

[0171] In summary, the SLAM device based on hybrid window joint optimization provided in this embodiment of the invention uses a sliding window composed of key frames and temporal frames to construct a local map. The key frames are optimized historical key frame data. By combining the historical key frames with the temporal frames to be optimized, sufficient historical information can be ensured during frame-to-map registration. Furthermore, the local map constructed based on the sliding window can be ensured to have an appropriate scale and sufficient common-view features, thereby significantly improving the robustness and accuracy of the SLAM localization and mapping process.

[0172] The specific working principle of the SLAM device based on hybrid window joint optimization provided in the embodiments of the present invention can be referred to the description of the SLAM method based on hybrid window joint optimization above, and will not be repeated here.

[0173] As another embodiment of the present invention, a SLAM system 10 based on hybrid window joint optimization is provided, wherein, as shown in FIG7, it includes: an inertial measurement device 200, a lidar device 300 and the aforementioned SLAM device 100 based on hybrid window joint optimization, wherein the inertial measurement device 200 and the lidar device 300 are both communicatively connected to the SLAM device 100 based on hybrid window joint optimization.

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

[0175] The lidar device 300 is used to provide lidar point cloud data;

[0176] The SLAM device 100 based on hybrid window joint optimization is used to obtain fused pose information after processing the lidar point cloud data and IMU data.

[0177] In this embodiment of the invention, the inertial measurement device may specifically include an IMU, and the lidar device may specifically include a LiDAR.

[0178] Specifically, the IMU provides accelerometer and angular velocity information to the vehicle; 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 executing computer instructions. It receives data from both sensors, processes it, and obtains high-precision fused pose information, thereby realizing some or all of the steps of the SLAM method based on hybrid window joint optimization mentioned above.

[0179] In summary, the SLAM system based on hybrid window joint optimization provided in this embodiment of the invention adopts the SLAM device based on hybrid window joint optimization described above. It uses a sliding window composed of key frames and temporal frames to construct a local map based on the sliding window. The key frames are the optimized historical key frame data. The combination of the historical key frames and the temporal frames to be optimized can ensure that there is sufficient historical information when registering frames to maps. It can also 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 localization and mapping process.

[0180] The specific working principle of the SLAM system based on hybrid window joint optimization in this embodiment of the invention can be referred to the description of the SLAM method based on hybrid window joint optimization above, and will not be repeated here.

[0181] It is understood that the above embodiments are merely exemplary implementations used to illustrate the principles of the present invention, and the present invention is not limited thereto. For those skilled 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 hybrid window joint optimization, characterized in that, include: Acquire lidar point cloud data and extract feature points from the lidar point cloud data to obtain point cloud feature point data; Acquire IMU data between sampling times of two adjacent point cloud frames, and construct IMU pre-integration residuals based on the IMU data; Construct a sliding window based on keyframes and the point cloud feature point data in a time sequence, and construct a local map based on the sliding window; The point cloud feature point data is matched with the local map to obtain the point cloud matching constraint residual; The pose of the lidar point cloud data in the world coordinate system is obtained by jointly optimizing the point cloud matching constraint residual and the IMU pre-integration residual.

2. The SLAM method based on hybrid window joint optimization according to claim 1, characterized in that, Constructing a sliding window based on keyframes and the point cloud feature point data in a time sequence, and constructing a local map based on the sliding window, includes: Determine the historical keyframes and the adjacent time frames of the current frame; The historical keyframes are determined as sliding window quantifications, and the time-series frames adjacent to the current frame are determined as sliding window variables; A sliding window is formed based on the sliding window quantification and the sliding window variable; The relative pose of the lidar corresponding to each point cloud frame within the sliding window is determined based on the IMU pre-integration residual and the carrier pose estimation. The relative poses of the LiDAR corresponding to each frame within the sliding window are projected onto the IMU coordinate system to obtain a local map.

3. The SLAM method based on hybrid window joint optimization according to claim 2, characterized in that, Constructing a sliding window based on keyframes and the point cloud feature point data in a time sequence, and constructing a local map based on the sliding window, further includes: The sliding window is updated based on real-time point cloud feature point data.

4. The SLAM method based on hybrid window joint optimization according to claim 3, characterized in that, The sliding window is updated based on real-time point cloud feature point data, including: The latest point cloud frame pose calculation results are obtained based on real-time point cloud feature point data; Add the latest point cloud frame pose calculation result to the sliding window variable of the sliding window; Keyframe detection is performed on the farthest point cloud frame in the sliding window variables; If the furthest point cloud frame in the sliding window variables meets the keyframe threshold, then the furthest point cloud frame in the sliding window variables is added to the sliding window quantification of the sliding window, and the furthest historical keyframe in the sliding window quantification is deleted. If the furthest point cloud frame in the sliding window variables does not meet the keyframe threshold, then the furthest point cloud frame in the sliding window variables will be deleted.

5. The SLAM method based on hybrid window joint optimization according to claim 4, characterized in that, Keyframe detection is performed on the furthest point cloud frame in the sliding window variables, including: The feature similarity calculation results are obtained by comparing the furthest point cloud frame in the sliding window variables with the most recent historical key frame in the sliding window quantification. The feature similarity calculation result is compared with an adaptive threshold determined based on motion information; If the feature similarity calculation result is greater than the adaptive threshold, then the point cloud frame with the furthest time among the sliding window variables is determined to meet the keyframe threshold. If the calculated result of the feature similarity is not greater than the adaptive threshold, then it is determined that the point cloud frame with the furthest time in the sliding window variable does not meet the keyframe threshold.

6. The SLAM method based on hybrid window joint optimization according to claim 5, characterized in that, The adaptive threshold determined based on motion information includes at least an adaptive threshold determined based on Euler angles, and the expression for the adaptive threshold is: ， in, Indicates an adaptive threshold. This represents a fixed threshold used in steady-state conditions. Indicates the threshold for angle transformation. This represents the modulus of the Euler angle between the two latest point cloud frames.

7. The SLAM method based on hybrid window joint optimization according to claim 1, characterized in that, Acquire IMU data between sampling times of two adjacent point cloud frames, and construct IMU pre-integration residuals based on the IMU data, including: Pre-integration is performed based on the IMU data between two adjacent point cloud frame sampling times to obtain the pre-integrated quantity; Pre-integration constraints are performed based on the pre-integral quantity, and the IMU pre-integration residuals are constructed based on the pre-integration constraint results.

8. The SLAM method based on hybrid window joint optimization according to claim 1, characterized in that, Joint optimization is performed based on the point cloud matching constraint residual and the IMU pre-integration residual, including: Based on the point cloud matching constraint residual and the IMU pre-integration residual, construct the optimized frame matching residual and the inter-frame pre-integration residual; The objective optimization function of the sliding variable in the sliding window is obtained based on the optimized frame registration residual and the inter-frame pre-integration residual. The objective optimization function is minimized, and the sliding variables in the sliding window are updated based on the minimization result. The objective optimization function is jointly nonlinearly optimized based on 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 apparatus based on hybrid window joint optimization, used to implement the SLAM method based on hybrid window joint optimization as described in any one of claims 1 to 8, characterized in that, include: The point cloud feature extraction module is used to acquire lidar point cloud data and extract feature points from the lidar point cloud data to obtain point cloud feature point data. The pre-integration residual construction module is used to acquire IMU data between the sampling times of two adjacent point cloud frames and construct IMU pre-integration residuals based on the IMU data. The local map construction module is used to construct a sliding window based on keyframes and the time-series frames of the point cloud feature point data, and to construct a local map based on the sliding window; The point cloud matching constraint residual acquisition module is used to match the point cloud feature point data with the local map to obtain the 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 pose of the lidar point cloud data in the world coordinate system.

10. A SLAM system based on hybrid window joint optimization, characterized in that, include: The inertial measurement unit, the lidar device, and the SLAM device based on hybrid window joint optimization as described in claim 9, wherein the inertial measurement unit and the lidar device are both communicatively connected to the SLAM device based on hybrid window joint optimization; The inertial measurement unit is used to provide IMU data; The lidar device is used to provide lidar point cloud data; The SLAM device based on hybrid window joint optimization is used to obtain fused pose information by processing the lidar point cloud data and IMU data.

Images