Mapping method based on multi-sensor fusion quadruped robot

Abstract

The invention discloses a quadruped robot mapping method based on multi-sensor fusion, and belongs to the technical field of robot environment perception and SLAM. The method comprises the steps of sensor calibration, laser radar inertial fusion SLAM mapping, global point cloud registration, visual inertial fusion SLAM mapping and sliding window optimization marginalization processing. By introducing ground segmentation of a concentric region model, ground likelihood estimation, degradation robust decoupling point cloud registration, hybrid optical flow tracking and an improved Scherr marginalization strategy, the mapping precision and robustness in complex terrains, dynamic illumination and unstructured environments are significantly improved. Experiments show that the method is superior to an existing mainstream algorithm on a public data set and an actual quadruped robot platform, and is suitable for high-precision map construction of the quadruped robot in complex scenes such as uneven ground and illumination variation.

Description

Technical Field

[0001] This invention belongs to the field of robotics and environmental perception technology, specifically relating to synchronous localization and mapping technology for quadruped robots, and particularly to a method for environmental mapping of quadruped robots based on multi-sensor fusion. Background Technology

[0002] In the field of multi-sensor fusion SLAM algorithms for quadruped robots, significant research achievements have been made both domestically and internationally. Various algorithms, such as visual SLAM, laser SLAM, and visual-radar fusion SLAM, have continuously evolved, providing technical support for localization and mapping in complex environments. Among them, the V-LOAM algorithm improves system robustness and accuracy through staged pose optimization (Zhang J, Singh S. Visual-lidarodometry and mapping: Low-drift, robust, and fast[C]. 2015 IEEE internationalconference on robotics and automation (ICRA). IEEE, 2015: 2174-2181.). The LIMO scheme combines high-precision depth information from lidar with monocular visual texture information (Graeter J, Wilczynski A, Lauer M. Limo: Lidar-monocular visual odometry[C]. 2018 IEEE / RSJ international conference on intelligent robots and systems (IROS). IEEE, 2018: 7872-7879.), making it suitable for complex environments. VIL-SLAM integrates three parts: stereo visual inertial odometry, LiDAR mapping, and visual loop closure detection (Shao W, Vijayarangan S, Li C, et al. Stereovisual inertial lidar simultaneous localization and mapping[C]. 2019 IEEE / RSJ international conference on intelligent robots and systems (IROS). IEEE,2019: 370-377.). LIC_Fusion, based on the MSCKF framework, achieves multi-sensor fusion and is suitable for high real-time scenarios (Zuo X, Geneva P, Lee W, et al. Lic-fusion: Lidar-inertial-camera odometry[C]. 2019 IEEE / RSJ International Conference on Intelligent Robots and Systems(IROS). IEEE, 2019: 5848-5854.).VL-SLAM incorporates all measurements into backend optimization in a tightly coupled manner (Chou CC, Chou CF. Efficient and accurate tightly-coupled visual-lidar slam[J]. IEEE Transactions on Intelligent Transportation Systems, 2021, 23(9):14509-14523.). LVI-SAM, based on LIO-SAM, couples with visual inertial odometry (Shan T, Englot B, Ratti C, et al. Lvi-sam: Tightly-coupled lidar-visual-inertial odometry viasmoothing and mapping[C]. 2021 IEEE international conference on robotics and automation (ICRA). IEEE, 2021: 5692-5698.), thus improving performance.

[0003] However, existing algorithms have significant shortcomings: First, they have poor adaptability. Most multi-sensor fusion SLAM algorithms are designed for wheeled robots and are unable to cope with the problems of body swaying and varied postures when quadruped robots move. This leads to difficulties in processing ground and aerial features by filtering algorithms, frequent failures in optimization algorithms, and excessive load on deep learning algorithms. Second, they have a high risk of sensor failure. When a single sensor fails, the positioning error increases dramatically and the algorithm cannot effectively utilize information from other sensors. Third, they have poor adaptability to complex environments. In dynamic environments, dynamic objects can interfere with feature extraction and matching. In unstructured terrain, the ground segmentation accuracy of LiDAR decreases and the feature tracking of visual cameras is unstable. Summary of the Invention

[0004] To address the aforementioned problems, this invention proposes a mapping method based on a multi-sensor fusion quadruped robot. This method aims to solve point cloud interference caused by robot swaying, ground segmentation of unstructured terrain, and feature tracking under dynamic lighting conditions. Ultimately, it achieves "robustness and accuracy superior to mainstream methods with lower-cost sensors (16-line radar, D435i camera)." The method includes the following steps:

[0005] Step 1, Sensor Calibration: Perform intrinsic parameter calibration and joint extrinsic parameter calibration on the camera and IMU respectively, and perform joint extrinsic parameter calibration on the LiDAR and IMU to ensure synchronous data transmission from multiple sensors, providing an accurate foundation for subsequent data fusion.

[0006] Step 2: LiDAR-Inertial Fusion SLAM Mapping:

[0007] S1. Divide the point cloud region based on the concentric region polar coordinate table: Define the 3D LiDAR point cloud at the current moment as P, and denote each point in the point cloud as . ,Depend on Composition, including ground point-related base point G and non-ground point base points. Two categories. The estimated ground points are defined as follows: The specific expressions are shown in formulas (1) and (2) below:

[0008] (1)

[0009] (2)

[0010] Where TP represents an actual ground point correctly identified as a ground point, FP represents a non-ground point incorrectly identified as a ground point, TN represents a non-ground point correctly identified as a non-ground point, and FN represents an actual ground point incorrectly identified as a non-ground point. The goal is to estimate as few FP and FN as possible from the point cloud P.

[0011] To estimate as few FP and FN values ​​as possible from the point cloud P, the point cloud is first divided into multiple rings and fan-shaped containers with regular intervals in radial and azimuth directions using a polar coordinate table S. Based on this, a polar coordinate grid representation method (C) based on the concentric region model CZM is proposed, dividing the point cloud P into four ring-shaped units of different sizes: Z1 central region, Z2 quarter region, Z3 half region, and Z4 outer region. Each unit is composed of arc-shaped modules. Let Nr and N θ The numbers of rings and sectors are respectively used to divide S into radial dimensions. The arc-shaped area, in which This represents the maximum boundary. As shown in formulas (3), (4), and (5):

[0012] (3)

[0013] (4)

[0014] (5)

[0015] Formula (3) For the number of regions, in formulas (4) and (5), For the m-th region based on the concentric polar coordinate table, and They are respectively The minimum and maximum radial boundaries. Further divided into Each unit consists of arc-shaped modules of different sizes.

[0016] Each arc module can be used The definition is shown in the following formula (6):

[0017] (6)

[0018] Formula 6 , , Specifically, it is represented by the following formula (7):

[0019] (7)

[0020] In the CZM polar grid, the cell sizes of Z1 and Z4 are relatively large, which can solve the problems of point cloud sparsity and representativeness. Therefore, the polar grid representation method (C) proposed in this invention improves representativeness and allows for robust estimation of normal vectors, preventing under-segmentation of the ground.

[0021] S2. After completing the region division, some ground points are allocated and estimated using the Regional Ground Plane Fitting Method (R-GPF). These are then merged and principal component analysis (PCA) is used to complete the ground point estimation. Take any arc-shaped region... This represents the covariance matrix of the point cloud within a unit space of this region, containing three eigenvalues. and its corresponding eigenvector The calculation is shown in formula (8):

[0022] (8)

[0023] in, Assuming Then the eigenvector This represents the optimal estimate of the normal vector to the ground plane. Let... Then the plane coefficient d can be expressed by formula (9):

[0024] (9)

[0025] in, This represents the average point per unit space. (Used) This represents the nth arc-shaped unit, whose quantity is equal to... .if If the value is large enough, then the point with the lowest height is selected as the initial seed point. The average Z-value of the total number of selected seed points is used to obtain the initial estimated set of ground points. The details are as follows:

[0026] (10)

[0027] in, This indicates the z-value of the returned point. The height margin is represented by an iterative method, where the ground point set in the Lth iteration is denoted as . We use the points within it to obtain the corresponding normal vector, denoted as... Planarity factor and The specific calculation formula is as follows:

[0028] (11)

[0029] (12)

[0030] in, and This represents the distance between planes. Compared to the original R-GPF algorithm, this invention prevents the algorithm from converging to a local minimum by adaptively selecting the initial seed, resulting in better fitting performance and fewer mis-estimation points.

[0031] S3. Ground Likelihood Estimation: Ground likelihood estimation (GLE) is used to reduce the false detection rate of ground point clouds, achieving [the desired result]. Whether it belongs to the actual ground is a stable determination. The GLE method can improve the accuracy of the overall ground points and eliminate those initial planes that are not composed of ground points and are not within the expected range.

[0032] Representing GLE as ,in Represents all parameters of the ground segmentation method for the concentric region model, where X represents the parameter following the density function. The random variable is a continuously distributed probability variable, and each region is independent of the others. This can be expressed as the following formula:

[0033] (13)

[0034] in, and Each represents Parameters and random variables. Subscript The parameter indicates that it comes from Each The probability of becoming a ground point is defined by verticality, elevation, and flatness, denoted as follows: , and The details are as follows:

[0035] (14)

[0036] in, , and These represent the average value of Z, the distance between the origin and the centroid, and the surface variables, respectively. The verticality index function proposed using geometric features is as follows:

[0037] (15)

[0038] in, and This indicates that the vertical margin is set to 45°, which means The angle between the sensor and the XY plane. When large objects approach the sensor frame, occlusion occurs, leading to localized observation problems. To address this issue, a conditional logic function is proposed. :

[0039] (16)

[0040] in, Represents the adaptive midpoint function, the function follows It grows exponentially. Finally, a flatness function is set to recover some of the FN that was removed due to elevation filtering. In practice, if... Indicates a very steep uphill slope and ,but Sometimes through They were filtered out. To solve this problem, variables were used. To check what is considered FN Smoothness. Smoothness function. As shown below:

[0041] (17)

[0042] in, The numerical value representing the gain. This represents the threshold of the surface variable. The GLE value increases when on a steep uphill slope, even though... The value is higher than These can still be considered valid estimates of the ground. The final estimated ground points are represented as follows:

[0043] (18)

[0044] If the condition in parentheses is met, it is a ground point; otherwise, it is not.

[0045] In summary, the ground segmentation method based on the concentric region model (CZM) proposed in this invention is a regional segmentation method: First, the point cloud is divided into multiple subsets using CZM; then, the center point, normal vector, and flatness (flatness is the variance of the points in the subset along the normal vector) of each subset are output using regional ground plane fitting (R-GPF); finally, the estimated ground is verified to be the actual ground using ground likelihood estimation (GLE).

[0046] Step 3: Global Point Cloud Registration Algorithm

[0047] S1. Represent the source point cloud and target point cloud captured by the 3D radar sensor as P and Q, respectively, where each point in the point cloud... and From the Cartesian coordinate system Composition, making A contains an inherent set of outliers O, such that The correspondence between each pair is shown in Figure 23 below:

[0048] (twenty three)

[0049] Where R∈SO(3) and t represent relative rotation and translation, respectively. Indicates unknown measurement noise, if If , then it represents Gaussian noise; if If the error is irregular, then the objective function is defined as shown in Figure 24:

[0050] (twenty four)

[0051] in, Let r(.) represent the surrogate function to suppress the error generated by O, and r(.) represent the squared residual function. The ultimate goal of the registration method is to estimate the translation and rotation components. At the same time, we should try to suppress the impact of outliers as much as possible.

[0052] S2, Maximum Clique Interior Point Algorithm: Based on FPFH features, voxel-level sampling is performed and a graph structure is constructed. The parallel branch and bound method is used to collaboratively search for the maximum clique. Combined with the innovative pruning technique of local density and angle consistency, invalid branches are effectively eliminated, and finally the set of interior points corresponding to the maximum clique in the graph is found.

[0053] S3. Hierarchical nonconvex function estimation of pose rotation components: To obtain the objective function mentioned in section 24 above. It requires the use of the two pairs from the previous formula. Addition and subtraction to cancel each other out The effect is assumed by the following formula:

[0054] (25)

[0055] K is the total number of these translation-invariant measurements (TIMs). Computational cost is minimized by subtracting multiple consecutive pairs of TIMs constructed in a chain-like manner. Translation-invariant measurements (TIMs) are key quantities used in decoupling problems to estimate scale, translation, and rotation; they are quantities that are invariant to a subset of transformations (scaling, rotation, and translation). Given and The relative positions of the two points are shown below:

[0056] (26)

[0057] The translation t cancels out in the subtraction, through calculation. and To obtain translation-invariant measurements, TIM satisfies the following mathematical model:

[0058] (27)

[0059] Among them, if the first The and the first If each measurement item is an interior point, then It is zero and To measure the noise term, the generative model of TIM depends on only two unknowns. and The upper limit of the quantity is as shown in Equation 28:

[0060] (28)

[0061] and Relationship expression The estimates are as follows:

[0062] (29)

[0063] (30)

[0064] in, This represents the weighting coefficient for each pair. This represents the cutoff parameter to suppress the influence of potential outliers. Then, COTE is calculated using component shift estimation. The specific calculation formula is as follows:

[0065] (31)

[0066] in, The boundary indicating noise. , Represents the 3D vector's first... Each element.

[0067] S4. Specify the relative rotation matrix R as... , representing rotation about the Z, Y, and X axes respectively, and , representing yaw, pitch, and roll angles respectively. Because the relative changes in pitch and roll are much smaller than yaw in an urban environment, this allows for... , Let represent a 3×3 identity matrix. Through this assumption, we can... Thus, directly estimate The coordinates, and this final assumption reduces the rotational degrees of freedom from 3 to 1, making the method robust to environmental degradation problems. For simplicity, let... .

[0068] In order to estimate A graded nonconvex method with truncated least squares (GNC) is introduced, where the solution obtained in each iteration is used as the initial guess for subsequent iterations. Then the proxy function If it becomes a convex function, Then it becomes a truncated least squares function, where =0.15. Therefore, the equations are first rewritten using Black-Rangarajan duality, as shown in equations (32) and (33):

[0069] (32)

[0070] (33)

[0071] in, Since the objective function is a penalty function, the objective equation cannot be solved directly and needs to be solved by using alternating optimization, as shown in equations (34) and (35):

[0072] (34)

[0073] (35)

[0074] Where the superscript t indicates the t-th iteration, The solution can be obtained by truncating the closed form, as shown in the following equation:

[0075] (36)

[0076] in, express Each iteration It will be updated to , initial value , Factors that increase the size of non-convexity.

[0077] if The iteration process ends when the differential value becomes sufficiently small. It can effectively suppress the influence of outliers because most ground surfaces can be considered sufficiently flat, and most structures tend to be orthogonal to the ground. Therefore, local geometric features, such as surface normals and density, are similar along the normal direction of the ground. It can be decomposed into two terms: one term is parallel to the ground, i.e., the xy plane, and the other term satisfies... .

[0078] S5. Component-wise translation estimation: Extracting translation components: Estimating the relative translation in the form of components, setting a boundary interval set. It is a binary set, with a lower bound of . The upper boundary is And assume all elements are arranged in ascending order. Then, let the g-th consensus set be:

[0079] (37)

[0080] in, Then through the non-empty set To estimate using a weighted average The specific formula is as follows:

[0081] (38)

[0082] in, The objective function is to minimize the truncation. The prerequisite for using COTE is that the estimated rotation accuracy is sufficient.

[0083] The degenerate robust decoupled point cloud registration process proposed in this invention is as follows: First, outliers are removed by using maximum clique in-point selection (MCIS); then, Quasi-SO(3) rotation estimation is performed based on hierarchical nonconvexity, and relative rotation is solved by alternating optimization and further denoising is performed; finally, based on the consensus set, the component translation estimation (COTE) method is used to calculate the translation amounts in the x, y, and z directions respectively.

[0084] Step 4: Visual-inertial fusion SLAM mapping:

[0085] S1. CNN Extraction: A CNN is used to extract illumination-invariant feature maps and score maps from the image. The former is used to construct a feature pyramid for sparse optical flow, and the latter is used for keypoint extraction. The network construction is required first.

[0086] To make the network more lightweight, four convolutional layers are used: first, a W×H×16 shared feature map is extracted from the W×H×3 input image (W is the pixel width and H is the pixel height), and then a 1×1 convolutional kernel is used to convert it into a light-invariant feature map (containing less high-level semantic information and more low-level image information) and a keypoint score map. The network consists of a shared encoder and a feature and score map decoder. The shared encoder converts the W×H×3 input into W×H×16. The first two layers use 3×3 convolutional kernels, and the last layer uses a 1×1 convolutional kernel to expand the channels to 16. After each convolution, ReLU is used for activation while maintaining the original resolution. The feature and score map decoder uses a 1×1 convolutional kernel to reduce the channels of the shared feature map to 4 (the first 3 channels are light-invariant feature maps, and the last 1 channel is a keypoint score map). After convolution, the score map is activated by sigmoid (value limit [0,1]), and finally W×H×1 score map and W×H×3 light-invariant feature map are obtained.

[0087] S2. The illumination-invariant feature map optical flow method is adopted: keypoints are extracted from the fractional image as initial positions. The brightness constancy assumption is replaced with the illumination-invariant feature map constancy (same as the cross-image convolution feature vector of keypoints). It includes two steps: keypoint extraction and pyramid optical flow. Keypoint extraction borrows from OpenCV's GoodFeaturesToTrack approach, using 3×3 neighborhood NMS to preserve local maxima and remove points below a threshold. Maximum interval sampling ensures uniform distribution, meeting the requirements of the optical flow method and improving tracking robustness.

[0088] Pyramid optical flow method: The pyramid optical flow method consists of three steps. First, calculate the illumination-invariant feature map. The spatial and time derivatives are... , , Then, the derivatives of all key points are combined into a coefficient matrix A and a constant vector b, as shown in the following equation:

[0089] (41)

[0090] in It refers to the number of key points. Finally, the equation is solved. Obtain optical flow velocity This method still uses the standard LK optical flow method, but modifies the brightness constancy assumption to the convolution feature constancy assumption. Therefore, this method is called the hybrid optical flow method.

[0091] The hybrid optical flow method first extracts image features through a shared encoder, and after decoding, obtains a fractional image S and an illumination-invariant feature map F. Keypoints are extracted using S and identified through non-maximum suppression, and then a pyramid optical flow is constructed based on F. Starting from the top of the pyramid, keypoints are tracked in another image using the feature map, and the results from the upper layer are used as initial values ​​for the lower layer, finally outputting a sparse optical flow.

[0092] S3. Construction of Deep Network and Loss Function: For image pairs, a shallow network (consistent with the network in step S2) extracts the score map S and feature map F, and a deep network (only for training assistance) extracts the dense descriptor map D. The specific process is as follows: first, obtain S and F through the shallow network, then obtain D through the deep network, and finally calculate the key point loss, illumination-invariant feature loss and descriptor loss based on the results.

[0093] Keypoint loss, illumination-invariant feature loss, and descriptor loss are used to train three different outputs. Keypoint losses include reprojection loss, line peak loss, and reliability loss. The NRE and mNRE functions are used for descriptor loss and illumination-invariant feature loss, respectively.

[0094] Keypoint loss function

[0095] The keypoint loss function consists of three parts: reprojection loss, line peak loss, and reliability loss, which are used to ensure keypoint repeatability, improve positioning accuracy, and promote matching efficiency, respectively.

[0096] Reprojection loss function

[0097] Key points are extracted from two images simultaneously under different conditions. The reprojection error is defined as the distance between the projected point and the extracted point. The images... Points in Projected onto image Above, the projection point is The reprojection error for a single instance is:

[0098] (42)

[0099] in, It is an image Extraction points in and Let L2 be the norm of the vector. The reprojection error loss is defined in symmetric form:

[0100] (43)

[0101] Where N represents the image Can be seen in the image The number of points found This refers to the reprojection error of the i-th feature point among N feature points, where M represents the image. Can be seen in the image The number of points found This represents the reprojection error of the i-th feature among M features.

[0102] Linear peak loss function

[0103] In a fractional graph, key points should exhibit a sharp peak shape. Consider the area near key point p in the fractional graph. Image patch of size, position of each pixel The distance between the key point and the key point is:

[0104] (44)

[0105] Peak loss is used to reduce the score at more distant locations within an image patch, and is defined as follows:

[0106] (45)

[0107] in, It is the location in the image patch near the keypoint p. The corresponding score. This definition includes pixels within all image patches, such that the score map forms a locally linear shape during training. Specifically, it includes four line patterns: horizontal, vertical, left diagonal, and right diagonal, with added penalty weights for the line shape. In one of them, near the keypoint p... In the image patch, four line widths Defined as:

[0108] (46)

[0109] in, It refers to the pixel position within an image block. and These are the coordinates of the key point p. It is a Gaussian distribution, and by using these weights, the line peak loss function is defined as Equations (47) and (48), where the max function represents selecting the maximum value from the four line patterns:

[0110] (47)

[0111] (48)

[0112] Reliability loss function

[0113] Accurate and repeatable keypoints are not enough; keypoint matching must also be guaranteed. The reliability loss function perfectly addresses this shortcoming. For calculating images... Key points The matching property, and its corresponding descriptor and images Dense descriptor mapping in The vector distance between them is calculated as shown in equation (49):

[0114] (49)

[0115] in, This is called a similarity map, representing key points. and images The similarity between the positions of each pixel in the graph is calculated. Then, a normalization function is used to set the score of positions with high similarity to 1 and the score of positions with low similarity to 0. Therefore, the matching probability map can be defined as Equation (50):

[0116] (50)

[0117] Where exp represents the exponential function, and t=0.02 represents the parameter controlling the shape of the exponential function. For key points... Its projection position A higher matching probability means higher reliability, therefore key points The reliability loss function can be defined as equation (51):

[0118] (51)

[0119] in, It means In position The bilinear sampling function at that point. Next, consider the image. The key points in the data are identified, and the scores of less reliable key points are penalized to obtain the reliability loss function:

[0120] (52)

[0121] (53)

[0122] In the above formula, Representing an image The number of all key points in the middle, Indicate key points The score, This represents the score of the projected position. Based on the three loss functions A, B, and C above, the final keypoint loss function is:

[0123] (54)

[0124] Where k1=1, k2=0.5, and k3=1 are weighting coefficients. These are images and The number of key points in the text.

[0125] Descriptor loss function

[0126] The NRE function can be used to learn descriptors. Due to its good performance, this invention uses it as a descriptor loss function, and explains the definition of the NRE function through the cross-entropy function. A detailed analysis follows. The new matching probability graph is defined as follows:

[0127] (55)

[0128] The normalization function softmax converts similarity into probability, and satisfies the condition that the sum of all elements is 1. For a good descriptor It should be related to the projection position. The descriptors at each location are as similar as possible and as far away from all other descriptors as possible. Therefore, the matching probability obtained by using the same sampling function in equation (51) can be defined as:

[0129] (56)

[0130] Maximize the projected position under the constraint that the sum of all elements equals 1. The matching probability at a given position implies minimizing the matching probabilities at other positions; therefore, the descriptor loss function is defined as:

[0131] (57)

[0132] in, and Representing images respectively and The number of key points, through The function transforms the maximization problem into a minimization problem.

[0133] Illumination-invariant feature loss function

[0134] Illumination-invariant feature maps are similar to dense descriptor maps because both attempt to extract features from an image.

[0135] Illumination-invariant feature maps are generated through only four convolutions, with 3 or 1 channels, far fewer than dense descriptor maps (typically obtained through multiple convolutions with 64 or 128 channels), making it difficult to learn globally discriminative features. Therefore, this invention uses the mNRE loss function to learn locally discriminative illumination-invariant feature maps. First, a mask function is defined near keypoints. .

[0136] (58)

[0137] Where d=80 is the range of the mask, and then the mask is added to the NRE loss function of equation (55) to obtain the local matching probability map, as shown in equation (59):

[0138] (59)

[0139] in, far away The region has a zero value. Similar to the NRE loss function, the local matching probability of the projected location is calculated as follows:

[0140] (60)

[0141] Finally, based on the image and The loss function is calculated based on the local matching probability of all keypoints in the dataset.

[0142] (61)

[0143] Step 5: Construct a new Shure complement matrix for the sliding window optimization module and edge-divide it:

[0144] S1, the core principle is to retain only state variables for a short period while discarding historical data that is no longer relevant. In this way, both computational and storage requirements can be kept within a controllable and reasonable range, ensuring stable system operation. System optimization variables within the sliding window. Defined as equation (62):

[0145] (62)

[0146] (63)

[0147] in, The sliding window number calculated by the IMU sensor represents the first... Frame state vector, Indicates the first The depth of each feature point and represents the extrinsic parameters between the camera and the IMU, n represents the number of keyframes to be optimized within the sliding window, and m represents the number of feature points to be optimized within the sliding window.

[0148] This invention uses a sliding window with a fixed length and step size (window length 11, step size 1) to store recent state values. When a new state value is received, if the number of state values ​​in the window has reached the upper limit, the oldest state is marginalized and incorporated into the new state value, thereby updating the data in the window.

[0149] S2. Edge optimization based on Shure complement: The sliding window method optimizes a fixed number of frames, improving efficiency while ensuring accuracy; when the sliding window is running, new image frames are continuously added and old image frames are deleted synchronously.

[0150] The sliding window operation aims to limit the number of keyframes to ensure the real-time performance of VIO. To avoid losing information from removed frames, an edge-mapping technique is used to transform them from state estimation into prior constraints, which are then incorporated into subsequent optimizations.

[0151] The goal of edge detection is to preserve valid information from the deleted frames, including image information, IMU information, etc. This information is then transformed into prior information, processed, and incorporated into the nonlinear optimization process. Let's assume the state to be edged is... The ones to be retained are The incremental equation becomes:

[0152] (64)

[0153] Marginalization employs the Schur complement method, a key tool in statistics, matrix transformations, and other fields. The Schur complement plays an indispensable role in transforming any partitioned matrix M into a triangular matrix, as shown in the following formula:

[0154] (65)

[0155] in, Let M be the Schur complement of A under the premise that A is an invertible matrix. The formula allows for the rapid acquisition of matrix M and its diagonal transformation form, and the quick solution for the inverse of matrix M. Applying the Schur complement to the incremental equation (64) yields:

[0156] (66)

[0157] Solve the equation:

[0158] (67)

[0159] The original incremental equation is derived as follows:

[0160] (68)

[0161] Therefore, the equivalent prior error after marginalization is:

[0162] (69)

[0163] When optimizing the sliding window mechanism on the backend, an incremental equation is constructed. The overall incremental equation is:

[0164] (70)

[0165] The above formula can be simplified as follows:

[0166] (71)

[0167] The Shure complement is fused with IMU and visual constraints, constructed based on the corresponding Jacobian matrix, which remains constant. Prior information is generated by marginalizing relevant variables, and its matrix structure is shown in the figure. All state variables are uniformly represented by the following formula:

[0168] (72)

[0169] In edge detection operations, incorporating all state variables into the same Shure complement matrix for optimization leads to resource waste. Therefore, this invention employs a two-step optimization strategy: taking the oldest frame as an example, its associated IMU data I_0 and observed landmark points f0~fk are converted into prior information and added to the overall objective function. The total state variables are:

[0170] (73)

[0171] in, Let S represent the SpeedBias variable and P represent the Pose variable. The variables that need to be marginalized are:

[0172] (74)

[0173] In the step-by-step optimization process, the velocity information and feature point depth information are first marginalized to form a new H matrix. Then, the pose information is marginalized to improve the marginalized Schur complement matrix.

[0174] This invention compares several mainstream multi-sensor fusion SLAM algorithms on the KITTI and M2DGR datasets, and also verifies the mapping performance on a quadruped robot platform in outdoor scenes. Experiments show that the proposed algorithm has better localization accuracy and mapping quality than LVI-SAM, providing a reliable solution for quadruped robot map construction. Furthermore, based on LVI-SAM, a SLAM algorithm with tight coupling of multi-line LiDAR, vision, and IMU is proposed, with improvements including: P-LIO ground segmentation accuracy, recall, and F1 score increased by 5.07%, 2.37%, and 0.041 respectively; Quatro global point cloud registration success rate of 99.63% (better than NDT algorithm, enhancing robustness in degraded environments); hybrid optical flow method improves feature tracking rate by 43.1% compared to LK optical flow under dynamic lighting; new Shure complement edge-compensation operation optimizes computational efficiency and achieves higher localization accuracy than VINS-MONO on the Euroc dataset; and experiments on public datasets and the Aliengo quadruped robot demonstrate that the algorithm's adaptability and accuracy are superior to the original LVI-SAM. Attached Figure Description

[0175] Figure 1 Specific steps for camera calibration.

[0176] Figure 2 This is a schematic diagram of a polar coordinate grid description diagram based on CZM.

[0177] Figure 3 The figures show a comparison of the ground likelihood estimation performance. In the figures, (a) shows the estimation performance of the R-GPF algorithm; and (b) shows the ground likelihood estimation performance.

[0178] Figure 4 This describes the process of a segmentation method based on a concentric region model.

[0179] Figure 5 A comparison of ground estimation results for frame 420 of sequence 00 in the Semantic-KITTI dataset. (a) is RANSAC; (b) is Linefit; (c) is GPF; (d) is Patchwork++; (e) is R-GPF; (f) is the present invention.

[0180] Figure 6 A comparison of ground estimation results for the 1000th frame of the 00 sequence in the Semantic-KITTI dataset. (a) is RANSAC; (b) is Linefit; (c) is GPF; (d) is Patchwork++; (e) is R-GPF; and (f) is the present invention.

[0181] Figure 7 TIM paradigm generated for the complete graph in the Bunny dataset.

[0182] Figure 8 This describes a global point cloud registration process based on degenerate Lubang decoupling.

[0183] Figure 9 This is a schematic diagram of Quatro under degenerate conditions.

[0184] Figure 10 The image shows the point cloud registration results for frames 1398 to 3554 of the KITTI dataset 00 sequence. Among them, (a) is the result of the NDT algorithm; (b) is the result of the Quatro algorithm.

[0185] Figure 11 The results of the 00 sequence evo evaluation are shown. Among them, (a) is the trajectory comparison of the 00 sequence; (b) is the absolute error of the 00 sequence PQLIO.

[0186] Figure 12 The results are the EVO evaluation results for the 01 sequence. Among them, (a) is the comparison of the 01 sequence trajectories; (b) is the absolute error of the PQLIO of the 01 sequence.

[0187] Figure 13 The results of the EVO evaluation for the O2 sequence are shown. Among them, (a) shows the trajectory comparison of the O2 sequence; (b) shows the absolute error of the PQLIO of the O2 sequence.

[0188] Figure 14 This is an example diagram of the hybrid optical flow method.

[0189] Figure 15 This is a flowchart of the grid training process.

[0190] Figure 16 These are typical image pairs. Among them, (a) represents dynamic lighting; (b) represents an active light source; and (c) represents image blur.

[0191] Figure 17 A comparison of the effects of optical flow methods in dynamic lighting scenarios. Among them, (a) is the hybrid optical flow method; (b) is the original optical flow method.

[0192] Figure 18 The comparison shows the optical flow method in blurred image scenes. Among them, (a) is the hybrid optical flow method; (b) is the original optical flow method.

[0193] Figure 19 The images show a comparison of optical flow methods under active light source scenarios. (a) shows the hybrid optical flow method; (b) shows the original optical flow method.

[0194] Figure 20 For key point tracking, the rejection rate is compared.

[0195] Figure 21 To improve the post-edgeization strategy process.

[0196] Figure 22This is a schematic diagram of a keyframe sliding window.

[0197] Figure 23 Construct the Schul complement matrix for marginalization.

[0198] Figure 24 This is the improved Schul complement matrix.

[0199] Figure 25 The images show a comparison of the trajectories of four algorithms and the actual values ​​for the KITTI dataset 05 sequence. Specifically, (a) is the trajectory of the FAST-LIO205 sequence; (b) is the trajectory of the LIO-SAM05 sequence; (c) is the trajectory of the LVI-SAM05 sequence before improvement; and (d) is the trajectory of the 05 sequence using the algorithm of this invention.

[0200] Figure 26 The following diagram compares the door02 trajectory of four algorithms with the true trajectory. Among them, (a) is the door02 trajectory diagram of the FAST-LIO2 algorithm; (b) is the door02 trajectory diagram of the LIO-SAM algorithm; (c) is the door02 trajectory diagram of the LVI-SAM algorithm; and (d) is the door02 trajectory diagram of the algorithm of this invention.

[0201] Figure 27 This is a picture of the outdoor garden on the first floor.

[0202] Figure 28 This is a scene from an outdoor parking lot.

[0203] Figure 29 Comparison of point cloud images for outdoor garden scene algorithms. Among them, (a) is the LVI-SAM outdoor garden point cloud; (b) is the outdoor garden point cloud of the algorithm of this invention.

[0204] Figure 30 A comparison of point cloud images for outdoor parking lot algorithms is provided. (a) shows the LVI-SAM outdoor parking lot point cloud; (b) shows the outdoor parking lot point cloud using the algorithm of this invention.

[0205] Figure 31 The images show a comparison of algorithm trajectory maps for outdoor scenes. (a) shows a comparison of trajectories in an outdoor garden; (b) shows a comparison of trajectories in an outdoor parking lot. Detailed Implementation

[0206] The present invention will be further described in conjunction with the accompanying drawings and embodiments.

[0207] The mapping method for a quadruped robot based on multi-sensor fusion described in this invention includes the following steps:

[0208] Step 1: Sensor calibration ensures accurate fusion of sensor data. The specific steps are as follows:

[0209] S1. Camera Calibration: Camera intrinsic parameters were calibrated using the Kalibr tool. First, the calibration board parameters were modified (see Table 1), and a 297×210 mm alumina diffuse reflection calibration board was selected. After adjusting the camera's topic frequency, data packets were recorded, and the size and distance of the calibration board in the field of view were changed by moving the calibration board. Finally, the data was processed using Kalibr to output intrinsic and distortion parameters.

[0210] Calibration parameter table 1:

[0211]

[0212] S2, IMU Calibration: Use the imu_utils toolkit to perform random error calibration on the IMU and obtain the noise parameters and deviations of the accelerometer and gyroscope.

[0213] S3. Camera and IMU Joint Calibration: Using the Kalibr tool, record and match camera / IMU data at different timestamps, and calculate the extrinsic parameter matrix; check the reprojection error through visualization tools to ensure accurate calibration.

[0214] S4. Joint calibration of LiDAR and IMU: The lidar_align tool is used to match the LiDAR point cloud and IMU data by timestamp to achieve joint calibration and obtain the extrinsic parameter matrix.

[0215] Step 2: LiDAR-Inertial Fusion SLAM Mapping

[0216] To verify the effectiveness of the proposed CZM-based ground segmentation method, the following experiments were conducted:

[0217] The traditional Region-based Ground Fitting (R-GPF) algorithm and the novel ground segmentation method based on a concentric region model proposed in this invention were validated on the Semantic-KITTI dataset. The results are as follows: Figure 3 As shown, (a) represents the estimation result of the R-GPF algorithm, and (b) represents the effect after applying Ground Likelihood Estimation (GLE). The green, blue, and red dots in the figure represent TP, FN, and FP, respectively. GLE successfully filters out the erroneous estimates, significantly reducing FN.

[0218] The ground likelihood estimation function is executed in three steps: initialization, seed point extraction, and iterative extraction of ground points.

[0219] To quantitatively evaluate performance, four metrics are used: recall, accuracy, precision, and F1 score. , , and Let be the number of points in TP, TN, FP, and FN, respectively, defined as follows:

[0220] (19)

[0221] (20)

[0222] (twenty one)

[0223] (twenty two)

[0224] The parameter settings are as follows:

[0225] • Concentric region model: number of rings Set to 2,4,4,4, number of sectors Set to 16, 32, 54, 32. Minimum radial boundary value. Set to 2.7 m, maximum radial boundary value Set to 80 m, vertical margin Set to 45°.

[0226] • Regional ground plane fitting algorithm: height margin =0.5, distance from the plane =0.15, height threshold =-1.1.

[0227] • Ground likelihood estimation algorithm: Let , Surface variables Points less than 0.01 are considered as planes.

[0228] On the Semantic-KITTI dataset, the method of this invention is compared with algorithms such as RANSAC, LineFit, GPF, R-GPF and Patchwork++. Figure 5 and Figure 6 The segmentation results of frames 420 and 1000 of the 00 sequence are shown respectively. It can be seen that RANSAC, LineFit, LEGO-LOAM and other methods often use fixed-size meshes or plane fitting, which are prone to undersegmentation / oversegmentation in sparse point clouds and uneven ground (lawn, sidewalk) scenes of 16-line LiDAR. According to the performance comparison table 2 drawn based on the effect diagram, the method of this invention achieved an accuracy of 92.47 and a recall of 93.43, and an F1 score of 0.93. Therefore, the F1 score is improved by more than 0.041 compared with the traditional method, and the overall performance is the best. This proves that the method effectively overcomes the problem of undersegmentation and its robustness in uneven ground environments.

[0229] Table 2 Comparison of algorithm performance on the Semantic-KITTI dataset

[0230]

[0231] Step 3: Global Point Cloud Registration Algorithm To verify the effectiveness of the proposed global point cloud registration method based on degenerate robust decoupling, the following experiments were conducted:

[0232] The global point cloud registration process based on degradation-robust decoupling is as follows: Figure 8 As shown. Figure 9 The process of Quatro algorithm in degenerate conditions is shown: (a) outlier matching results, (b) output after enabling maximum clique in-point selection (MCIS), (c)-(e) iterative process of Quasi-SO(3) estimation by GNC, and (f) schematic diagram before and after COTE application.

[0233] The algorithm was tested using the KITTI dataset under closed-loop conditions. Error metrics included translation and rotation metrics for the mean relative pose error, defined as follows (the subscript GT represents the true value):

[0234] (39)

[0235] (40)

[0236] Parameter settings: =0.15 and =1.4. Maximum permissible error =0.3m, estimated radius of normal for FPFH voxel sampling =0.5m.

[0237] Experiments were conducted using the 00, 02, and 05 sequences from the KITTI dataset. Figure 10 The point cloud registration results of the NDT and Quatro algorithms are shown in frames 1266 to 1300 of the 00 sequence. The Quatro method, due to its outlier pruning procedure, exhibits more stable performance. Table 3 shows the registration success rates of each algorithm within the 9-12m range under closed-loop conditions. NDT has a low registration success rate (only 31%-51%) in distant, sparse, or degraded point cloud scenarios (long corridors, single geometric environments), while Quatro achieves a success rate exceeding 99% in all three sequences.

[0238] Table 3. Registration success rate (%) of each algorithm on the KITTI dataset

[0239]

[0240] The improved ground segmentation and point cloud registration algorithm is fused together and termed the PQLIO algorithm. The performance of PQLIO and LIO-SAM algorithms is compared on the 00, 01, and 02 sequences of the KITTI dataset.

[0241] In the 00 sequence (urban environment), the absolute trajectory error of PQLIO is significantly lower than that of LIO-SAM ( Figure 11 ).

[0242] • In 0-1 sequences (highways), due to the open scene and sparse features, both methods have relatively large errors, but PQLIO still outperforms LIO-SAM. Figure 12 ).

[0243] In the 02 sequence (urban environment, including closed loop), PQLIO shows high repetition between the trajectory and the true trajectory at the closed loop, and its accuracy is better than LIO-SAM. Figure 13 ).

[0244] Detailed comparisons of absolute positional errors are shown in Table 4. PQLIO outperforms LIO-SAM in most error metrics for the 00, 01, and 02 sequences, showing significant improvements, especially with a 77.3% reduction in the average value for the 00 sequence and an 82.2% reduction in the average value for the 02 sequence.

[0245] Table 4. Error comparison between PQLIO and LIO-SAM on the KITTI dataset (unit: m)

[0246]

[0247] In summary, in the KITTI dataset sequences 00, 01, and 02, the PQLIO algorithm outperforms the LIO-SAM algorithm in terms of absolute position error. This demonstrates that the improvement significantly enhances loop closure detection, and the more accurate ground point cloud segmentation method provides stable and reliable ground feature points for subsequent feature matching.

[0248] Step 4: Visual-Inertial Fusion SLAM Mapping To verify the effectiveness of the proposed hybrid optical flow tracking method, the following experiments were conducted:

[0249] For training the lightweight CNN network model, the MegaDepth dataset was used, and the training parameters and environment configuration are shown in Table 5.

[0250] Table 5 Training Parameters and Training Environment

[0251]

[0252] Tests were conducted in three typical scenarios: dynamic light sources, active light sources, and image blur. For example... Figure 14-16 As shown, the feature tracking performance of the hybrid optical flow method (left) and the original method (right) is compared in a 0-10 frame image sequence (green represents the optical flow trajectory). In dynamic lighting and active light source scenes, the hybrid optical flow method has smoother optical flow and a lower false tracking rate; when the image is blurred, the performance of the two methods is similar.

[0253] Table 6 shows the comparison of feature point tracking accuracy of different methods in tests with a maximum of 300 keypoints on a 480×640 image. The proposed method achieved the highest tracking rate in all scenarios, with the most significant improvement in active lighting environments. Compared to the LK optical flow method, it improved by 43.1% and 135.1% in dynamic lighting and active lighting scenarios, respectively; compared to D2-Net, it improved by 6.4% and 8.75%; compared to ALIKE, it improved by 15.4% and 58.1%; and compared to SuperPoint, it improved by 69.3% and 81.2%, fully validating the effectiveness of the proposed method.

[0254] Table 6 Comparison of Feature Point Tracking Accuracy of Different Methods

[0255]

[0256] The proposed method, which continuously tracks key points, was tested under dynamic illumination within an active light source sequence. Figure 17 The traditional LK optical flow method fails due to light source interference, resulting in a significantly higher rejection rate. Figure 20 ).

[0257] Step 5: Sliding Window Optimization and Edge Mitigation

[0258] To validate the improved edge-diversification strategy ( Figure 18 The edge-diffusion speeds of the original and improved algorithms were compared on multiple sequences in the Euroc dataset. Each algorithm was run independently five times, and the average edge-diffusion time of the most recent five runs was calculated, followed by an overall average. The results are shown in Table 7, demonstrating that the improved algorithm significantly improves edge-diffusion speed, saving up to 7ms.

[0259] Table 7. Comparison of edge-setting speeds (ms) between the original algorithm and the improved algorithm

[0260]

[0261] Comparison and validation of algorithms on public datasets

[0262] To comprehensively evaluate the improved algorithm of this invention, the 05 sequence from the KITTI dataset and the door02 sequence from the M2DGR dataset were selected for comparative analysis.

[0263] (1) KITTI dataset 05 sequence

[0264] In the KITTI 05 sequence (urban road, 2760 frames, 2200m), the algorithm of this invention was compared with FAST-LIO2, LIO-SAM, LVI-SAM. The trajectory comparison results are as follows: Figure 22 As shown, the trajectory of the improved LVI-SAM algorithm of this invention is closer to the true value. The error comparison is shown in Table 8; the algorithm of this invention outperforms the comparative algorithms in all error categories.

[0265] Table 8. Error values ​​of four algorithms for the KITTI dataset 05 sequence (unit: m)

[0266]

[0267] (2) M2DGR dataset door02 sequence

[0268] In the M2DGR door02 sequence (alternating indoor and outdoor environments with dynamic lighting), the trajectory comparisons of the four algorithms are as follows: Figure 23 As shown in Table 9, the algorithm of this invention exhibits optimal trajectory consistency and stability, further verifying its superior robustness and positioning accuracy. Error comparisons are shown in Table 9.

[0269] Table 9. Error values ​​of four algorithms for the door02 sequence in the M2DGR dataset (unit: m)

[0270]

[0271] Outdoor mapping verification

[0272] Field tests were conducted in an outdoor garden (uneven cobblestone ground) and an open-air parking lot (alternating sunlight and shade) at a university research institute in Jiangxi Province to compare the mapping and localization performance of the algorithm of this invention with that of LVI-SAM. The environment was as follows: Figure 24 , 25 As shown.

[0273] Point cloud diagrams, for example Figure 26 , 27 As shown, the algorithm of this invention has a more uniform point cloud distribution and more accurate recognition. Trajectory comparison example Figure 28 As shown, a significant deviation occurs at the end corner of the LVI-SAM in the parking lot scene, while the algorithm of this invention is closer to the actual motion.

[0274] The positioning error compared with the actual trajectory is shown in Table 10. The algorithm of the present invention has a shorter trajectory length and significantly lower error.

[0275] Table 10. Average positioning error between the algorithm of this invention and the LVI-SAM algorithm (unit: m)

[0276]

[0277] in conclusion

[0278] This invention addresses the mapping needs of quadruped robots, employing the PQLIO algorithm and a hybrid optical flow method, and has been systematically validated on a real-world platform. Trajectory and mapping results were compared with mainstream multi-sensor fusion SLAM algorithms on the KITTI and M2DGR datasets. Furthermore, mapping tests were conducted in real-world scenarios such as a campus garden and a parking lot using a constructed quadruped robot experimental platform. Experiments demonstrate that this algorithm outperforms the original LVI-SAM algorithm in both localization accuracy and mapping quality, providing an effective solution for quadruped robot map construction.

Claims

1. A mapping method based on multi-sensor fusion quadruped robot, characterized in that, The method comprises the following steps: Step one, sensor calibration: calibrate the camera, IMU, respectively, and joint parameter calibration, calibrate the laser radar and IMU, and ensure the synchronization of multi-sensor data transmission; Step two, laser radar inertia fusion SLAM mapping: based on the concentric region polar coordinate table, the point cloud region is divided, the regional ground plane fitting method is used to distribute and estimate part of the ground points, after merging, the principal component analysis algorithm is used to complete the ground point estimation, and the ground point cloud error detection rate is reduced by using the ground likelihood estimation method; Step three, global point cloud registration algorithm: the maximum in-group point algorithm is used for outlier rejection, the rotation component of the pose is estimated based on the hierarchical non-convex function, and the translation component is extracted by the component-by-component translation estimation; Step four, visual inertia fusion SLAM mapping: the CNN is used to extract the illumination invariant feature map and the score map from the image, the illumination invariant feature map optical flow method is used for key point tracking, and a deep network and a loss function are constructed; Step five, sliding window optimization module constructs a new Schur complement matrix for marginalization: the sliding window is used to manage the state quantity, and the Schur complement is used for marginalization processing.

2. The method of claim 1, wherein, The specific way of dividing the point cloud region based on the concentric region polar coordinate table in step two is: The point cloud is divided into four ring units of center area, quarter area, half area and outer area, and each unit is composed of arc modules; The point cloud is divided into ring and sector containers with regular intervals in radial and azimuth directions by the polar coordinate table.

3. The method of claim 1, wherein, The regional ground plane fitting method in step two includes: Adaptive initial seed selection is used to prevent the algorithm from converging to a local minimum value; The principal component analysis algorithm is used to calculate the normal vector and plane coefficient of the ground plane; The ground likelihood estimation method uses the verticality, altitude and flatness indicators to distinguish the ground points, and optimizes the estimation results through conditional logic functions and flatness functions.

4. The method of claim 1, wherein, The maximum in-group point algorithm in step three is based on the FPFH feature for voxel-level sampling and graph structure construction, and the maximum group is searched by using the parallel branch and bound method, and the invalid branches are removed by combining the pruning techniques of local density and angle consistency.

5. The method of claim 1, wherein, The hierarchical non-convex function estimation of the rotation component of the pose in step three adopts the hierarchical non-convex method of the truncated least squares method, and the relative rotation matrix is solved by alternating optimization.

6. The method of claim 1, wherein, The component-by-component translation estimation in step three estimates the translation component by weighted average of the consensus set, and the estimation accuracy of the rotation component meets the preset condition.

7. The method of claim 1, wherein, When the CNN is used to extract the illumination invariant feature map and the score map in step four: The shared encoder converts the input image into a shared feature map; The feature and score map decoder converts the shared feature map into the illumination invariant feature map and the key point score map; Key point extraction ensures uniform distribution of key points through non-maximum suppression and maximum interval sampling.

8. The method of claim 1, wherein, The illumination invariant feature map optical flow method changes the brightness constancy assumption to the convolution feature constancy assumption, and calculates the optical flow velocity by the pyramid optical flow method.

9. The method of claim 1, wherein, The deep network and loss function in step four include key point loss, descriptor loss and illumination invariant feature loss; wherein the key point loss is composed of re-projection loss, line peak loss and reliability loss.

10. The method of claim 1, wherein, The sliding window optimization module in the fifth step uses a sliding window with fixed length and step to store state variables, and performs edgeization on the oldest state when the window is full; In the edgeization process, the state variables are converted into prior constraints by the Schulz complement method, and the Schulz complement matrix structure is improved to optimize the calculation efficiency; The construction of the Schulz complement matrix includes step-by-step optimization: first, the velocity information and the feature point depth information are edgeized to form a new H matrix, and then the attitude information is edgeized.

Images