Ultra-wideband lidar inertial navigation collaborative slam method and system

Abstract

The application discloses a kind of ultra-wideband laser radar inertial navigation cooperative SLAM method and system, it is applicable to the robot positioning and mapping task under the environment without GPS.This method fuses the high-frequency motion prior of IMU, the relative pose constraint of LiDAR and the absolute ranging information of UWB, constructs unified factor graph optimization model, realizes multi-sensor cooperative positioning;System initialization stage completes IMU bias calibration, UWB base station coordinate configuration and LiDAR initial attitude estimation;In the running process, the relative motion between key frames is obtained by using IMU pre-integration prediction pose and LiDAR point cloud registration, combined with UWB ranging to construct residual term and introduce adaptive weight mechanism to suppress ranging anomaly, improve fusion robustness;The system of the application supports geometric feature loop detection, and combined with UWB to construct cross-frame constraint for loop optimization, and the optimization result is used for real-time incremental update of global map;The method has the advantages of high positioning accuracy, strong anti-interference ability, wide application scene, etc.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of robot autonomous positioning and map construction, and in particular to an ultra-wideband laser radar inertial navigation cooperative SLAM method and system. BACKGROUND

[0002] With the wide application of mobile robots, unmanned vehicles and intelligent equipment in warehouse logistics, indoor navigation, underground mines and post-disaster rescue scenes, accurate positioning and real-time map construction (SLAM) in complex environments has become one of the key factors restricting system performance. Inertial measurement unit (IMU) can provide high-frequency attitude and speed information, and has the advantages of fast response and strong anti-interference in short-time motion estimation, but its inherent zero error and integral drift will accumulate rapidly with time, resulting in significant drift in position estimation. Laser radar (LiDAR) can achieve centimeter-level relative positioning and mapping in environments with rich features, thanks to its high-precision point cloud acquisition capability; however, when the surrounding geometric features are sparse, the line of sight is limited, or there are large-scale repeated structures, relying solely on laser radar for registration is prone to local matching errors and global drift.

[0003] In recent years, in order to make up for the performance shortcomings of a single sensor, academia and industry have tried to combine and fuse ultra-wideband (UWB), IMU and LiDAR. UWB can provide absolute scale constraints for indoor environments through ranging or time difference of arrival (TDoA), but under the conditions of multipath effect and non-line-of-sight (NLOS), the ranging error increases in pulses, and conventional weighting strategies cannot identify and suppress outliers in real time; although IMU-LiDAR loosely coupled or tightly coupled fusion framework can suppress drift to some extent, it often needs to perform high-dimensional nonlinear optimization on multi-source data, and the algorithm is very sensitive to computing resources, feature matching quality and multi-sensor time synchronization accuracy. Once the sensor observation confidence assessment is inaccurate, or the loop detection constraint is insufficient, it is easy to cause global map distortion and trajectory breakage.

[0004] Existing researches generally face the following pain points: first, IMU has high short-term accuracy but fast long-term drift, which needs to be compensated by external absolute coordinate sources; second, the relative positioning accuracy of LiDAR depends on the environmental geometric features, and once the features are scarce or unevenly distributed, the positioning stability decreases; third, UWB ranging information is easily disturbed by environmental factors in complex scenes, and there is a lack of reliable adaptive weight adjustment mechanism; fourth, the weight setting of each factor in the multi-sensor factor graph optimization framework is empirical, and it is difficult to balance real-time performance and robustness.

[0005] In summary, how to establish a unified and reliable fusion model between observations of different scales and different confidence levels is still a technical bottleneck for achieving stable and high-precision SLAM in indoor and outdoor GNSS-free scenarios. Summary of the Invention

[0006] This application provides an ultra-wideband lidar-inertial navigation cooperative SLAM method and system to solve problems such as large positioning drift, high ranging error and poor robustness of multi-sensor fusion in the prior art.

[0007] According to a first aspect, the present invention provides an ultra-wideband lidar-inertial navigation cooperative SLAM method, the method comprising:

[0008] S1. System initialization: Static calibration of the IMU is performed to obtain the initial bias of the gyroscope and accelerometer. The three-dimensional coordinate calibration of the UWB base station is completed using the geometric measurement method. The initial pose of the robot is determined by the initial LiDAR point cloud registration, realizing the calibration of the three-sensor coordinate system and the initialization of the system state.

[0009] S2, IMU data pre-integration and key frame selection: real-time acquisition of IMU angular velocity and acceleration data, use pre-integration algorithm to calculate pose increment between adjacent time moments, and trigger key frame selection when the cumulative rotation or displacement exceeds a preset threshold.

[0010] S3, LiDAR point cloud matching and relative pose estimation: For continuous keyframes, obtain the current point cloud and the point cloud of the previous keyframe, use the ICP algorithm to complete the registration, obtain the relative rotation matrix and translation vector, and construct the LiDAR factor constraint.

[0011] S4, UWB ranging acquisition and adaptive weight adjustment: synchronously acquire distance data between the robot and each UWB base station, establish a ranging residual function, calculate adaptive weights based on the residual size and dynamically adjust the ranging factor information matrix;

[0012] S5. Multi-source factor graph joint optimization: Under the factor graph framework, the IMU pre-integration factor, LiDAR relative pose factor and UWB ranging factor are integrated and solved iteratively using a nonlinear optimization algorithm to update position, velocity, attitude and IMU bias.

[0013] S6. Loop closure detection and UWB-assisted loop closure optimization: Periodically extract the geometric features of the point cloud of the current key frame, match them with historical key frames to complete loop closure detection, obtain loop closure pose constraints through fine registration, and construct a cross-frame residual addition factor map by combining the UWB ranging information at the same moment for loop closure optimization.

[0014] S7. Map building and incremental update: The local point cloud is transformed to the global coordinate system using the optimized keyframe pose, realizing real-time incremental map stitching; after the closed-loop optimization is completed, the positions of the historical keyframe point cloud are corrected and the global map is updated.

[0015] According to the two aspects, one embodiment provides an ultra-wideband lidar-inertial navigation cooperative SLAM system, the system comprising:

[0016] The system initialization module is used to perform static calibration of the IMU to obtain the initial bias of the gyroscope and accelerometer, complete the three-dimensional coordinate calibration of the UWB base station using geometric measurement method, and determine the initial pose of the robot through initial LiDAR point cloud registration, thereby realizing the calibration of the three-sensor coordinate system and the initialization of the system state.

[0017] The IMU data pre-integration and key frame selection module is used to acquire IMU angular velocity and acceleration data in real time, and uses a pre-integration algorithm to calculate the pose increment between adjacent time moments. When the cumulative rotation or displacement exceeds a preset threshold, key frame selection is triggered.

[0018] The LiDAR point cloud matching and relative pose estimation module is used to obtain the current point cloud and the point cloud of the previous key frame for consecutive key frames, complete the registration using the ICP algorithm, obtain the relative rotation matrix and translation vector, and construct the LiDAR factor constraint.

[0019] The UWB ranging acquisition and adaptive weight adjustment module is used to synchronously acquire distance data between the robot and each UWB base station, establish a ranging residual function, calculate adaptive weights based on the residual size, and dynamically adjust the ranging factor information matrix.

[0020] The multi-source factor graph joint optimization module is used to integrate IMU pre-integration factor, LiDAR relative pose factor and UWB ranging factor within the factor graph framework, and iteratively solve the problem using a nonlinear optimization algorithm to update position, velocity, attitude and IMU bias.

[0021] The loop closure detection and UWB-assisted loop closure optimization module is used to periodically extract the geometric features of the point cloud in the current key frame, match them with historical key frames to complete loop closure detection, obtain loop closure pose constraints through fine registration, and construct a cross-frame residual addition factor map in conjunction with the UWB ranging information at the same moment for loop closure optimization.

[0022] The map building and incremental update module is used to transform the local point cloud to the global coordinate system using the optimized keyframe poses, so as to realize real-time incremental map stitching; after the closed-loop optimization is completed, the positions of the historical keyframe point cloud are corrected and the global map is updated.

[0023] According to three aspects, one embodiment provides an electronic device.

[0024] The device includes: a processor and a memory;

[0025] The memory is used to store one or more program instructions;

[0026] The processor is configured to run one or more program instructions to perform the steps of an ultra-wideband lidar-inertial navigation cooperative SLAM method as described in any of the preceding claims.

[0027] According to a fourth aspect, one embodiment provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements the steps of an ultra-wideband lidar-inertial navigation cooperative SLAM method as described in any of the preceding claims.

[0028] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0029] This invention provides a method and system for simultaneous localization and mapping (SLAM) integrating UWB, LiDAR, and IMU. This method is suitable for robot navigation in GPS-free environments. Based on a unified graph optimization framework, it deeply integrates high-frequency motion priors provided by the IMU, relative pose constraints from LiDAR, and absolute ranging information from UWB to achieve multi-sensor collaborative localization and mapping. During the initialization phase, the system completes IMU bias calibration, UWB base station location configuration, and LiDAR initial attitude setting, providing an accurate starting state for subsequent operation. During operation, the IMU is used for short-term continuous pose prediction, and the LiDAR accurately estimates keyframe intervals through point cloud registration. For relative motion, UWB provides global scale constraints for the system, enhancing positioning stability. To improve robustness, a UWB adaptive weighting mechanism based on ranging residuals is proposed, which can dynamically adjust the optimization weights of UWB factors, effectively suppressing the impact of multipath effects and non-line-of-sight interference on ranging accuracy. Various sensor observation information is uniformly modeled in factor form and input into a nonlinear optimizer for joint solution, thereby simultaneously optimizing system states such as position, velocity, attitude, and IMU bias. Furthermore, the system supports a loop closure detection method based on geometric features, suitable for textureless or highly variable lighting environments, and combines UWB ranging information to construct cross-frame auxiliary constraints, enhancing loop closure consistency. The optimization results are used to update the global point cloud map in real time, achieving high-precision, stable continuous mapping and trajectory maintenance. This method has advantages such as high positioning accuracy, strong robustness, and wide environmental adaptability, making it suitable for complex scenarios such as warehousing and logistics, indoor navigation, underground space inspection, and emergency operations, and possessing high engineering application value. Attached Figure Description

[0030] Figure 1 This is a schematic diagram of the overall structure of the ultra-wideband lidar-inertial navigation cooperative SLAM method in an embodiment of the present invention;

[0031] Figure 2 This is a flowchart of the adaptive processing of UWB ranging fusion in an embodiment of the present invention;

[0032] Figure 3This is a flowchart illustrating the solution process for the multi-sensor factor graph joint optimization in this embodiment of the invention.

[0033] Figure 4 This is a flowchart of the closed-loop optimization process based on geometric loop closure detection combined with UWB in an embodiment of the present invention. Detailed Implementation

[0034] The present invention will now be described in further detail with reference to specific embodiments and accompanying drawings. Similar elements in different embodiments are referred to by associated similar element reference numerals. In the following embodiments, many details are described to facilitate a better understanding of this application. However, those skilled in the art will readily recognize that some features may be omitted in different situations, or may be replaced by other elements, materials, or methods. In some cases, certain operations related to this application are not shown or described in the specification. This is to avoid obscuring the core parts of this application with excessive description. For those skilled in the art, detailed description of these related operations is not necessary; they can fully understand the related operations based on the description in the specification and general technical knowledge in the art.

[0035] Furthermore, the features, operations, or characteristics described in the specification can be combined in any suitable manner to form various embodiments. At the same time, the steps or actions in the method description can be rearranged or adjusted in a manner obvious to those skilled in the art. Therefore, the various orders in the specification and drawings are only for the clear description of a particular embodiment and do not imply a necessary order, unless otherwise stated that a particular order must be followed.

[0036] like Figure 1 As shown, this invention provides a simultaneous localization and mapping method integrating UWB, LiDAR, and IMU, named UWB-LiDAR-Inertial SLAM (ULI-SLAM). Based on multi-sensor fusion, this invention improves the system's positioning accuracy and map building capabilities through the following three innovative strategies. First, as... Figure 2 As shown, the system employs an adaptive UWB ranging fusion scheme. This method constructs a ranging residual based on the theoretical distance between the actual ranging value and the estimated state position, and then determines whether this residual exceeds a preset threshold. For measurements exceeding the threshold, a residual scaling function is introduced to dynamically adjust the UWB weights in the factor graph, reducing the impact of non-line-of-sight or multipath errors, thereby achieving a more stable and reliable UWB constraint factor construction. Next, as... Figure 3As shown, when constructing the factor graph, the system state uniformly integrates IMU motion prediction, LiDAR relative pose measurement, and UWB absolute distance constraints. For all observation factors, a nonlinear optimization objective function is jointly constructed, and the state is iteratively solved through steps such as iterative linearization, solving for state increments, and convergence judgment, ultimately obtaining a globally consistent optimization estimation result. Figure 4 As shown, in the loop closure detection stage, this invention uses pure geometric features for loop closure candidate screening and combines ICP fine registration to obtain accurate relative pose. Simultaneously, to further improve the robustness of loop closure constraints, UWB ranging data from the loop closure frames are read to construct a cross-frame joint UWB constraint term, which is added to the factor graph as an additional loop closure factor. This strategy effectively compensates for the weak constraint problem of single geometric loop closure detection in scenarios with sparse features or severe loop closure drift, significantly enhancing global optimization capabilities.

[0037] Example:

[0038] The following combination Figure 1 A specific embodiment of the present invention will be described in detail below.

[0039] See Figure 1 An ultra-wideband lidar-inertial navigation cooperative SLAM method, the method includes:

[0040] S1. System initialization: Static calibration of the IMU is performed to obtain the initial bias of the gyroscope and accelerometer. The three-dimensional coordinate calibration of the UWB base station is completed using the geometric measurement method. The initial pose of the robot is determined by the initial LiDAR point cloud registration, thus realizing the calibration of the three-sensor coordinate system and the initialization of the system state.

[0041] Specifically, S1, system initialization, includes:

[0042] S101, IMU static calibration: Continuously acquire N frames of IMU observation data while the system is stationary, and estimate the gyroscope bias using the following formula:

[0043]

[0044] Estimate the accelerometer bias using the following formula:

[0045]

[0046] Where, ω t and a t ω and ωc are the observed values ​​of angular velocity and acceleration at time t, respectively, and g is the gravitational acceleration vector;

[0047] S102, UWB base station triangulation calibration: Given the robot's pose, record the distance data between the robot and each UWB base station, and obtain the three-dimensional coordinates of each base station using geometric trilateration.

[0048] b k =[x k ,y k ,z k ] T k = 1, 2, ..., M

[0049] Where M represents the number of base stations;

[0050] S103, LiDAR initial attitude determination, initial point cloud frame P t Registered with the environmental reference point cloud, the initial pose transformation matrix is ​​calculated:

[0051] T t =[R t |p t ]

[0052] Where R t Let p be a rotation matrix. t It is a translation vector used to define the spatial coordinate relationship at the initial moment of the system.

[0053] S2, IMU data pre-integration and key frame selection: IMU angular velocity and acceleration data are acquired in real time, and the pose increment between adjacent time moments is calculated using a pre-integration algorithm. When the cumulative rotation or displacement exceeds a preset threshold, key frame selection is triggered.

[0054] Specifically, S2 and IMU data pre-integration and keyframe selection include:

[0055] S201, based on the continuous-time motion model:

[0056]

[0057] Where R(θ) t ) is the attitude rotation matrix at time t, a t and ω t It represents the acceleration and angular velocity at time t. The calculation process includes position p, velocity v, attitude θ, and bias b. g b a ;

[0058] S202, calculate the rotation increment ΔR between adjacent keyframes using a discretized pre-integration method. t,t+1 velocity increment Δv t,t+1 and position increment Δp t,t+1 , to describe the state evolution of the robot from t to t+1;

[0059] When the attitude change accumulated by the IMU integral satisfies:

[0060]

[0061] If the cumulative displacement exceeds a preset threshold, the current frame is set as the new keyframe, where t ′ For the previous keyframe time, ∈ θ This is the angle threshold.

[0062] S3, LiDAR point cloud matching and relative pose estimation: For continuous keyframes, the current point cloud and the point cloud of the previous keyframe are obtained, and the ICP algorithm is used to complete the registration, obtain the relative rotation matrix and translation vector, and construct the LiDAR factor constraint.

[0063] Specifically, S3 and LiDAR point cloud matching and relative pose estimation include:

[0064] S301, Solve for pose transformation:

[0065] For the current frame point, This is the point corresponding to the previous keyframe;

[0066] S302, decompose the optimal transformation into:

[0067] in For relative rotation matrices, It is a relative translation vector;

[0068] S303, Constructing LiDAR residuals in the factor plot:

[0069]

[0070] Among them, R t p t For the rotation and position state variables of the current frame, the log operation represents the Lie algebra logarithmic mapping.

[0071] S4. UWB ranging acquisition and adaptive weight adjustment: synchronously acquire distance data between the robot and each UWB base station, establish a ranging residual function, calculate adaptive weights based on the residual size, and dynamically adjust the ranging factor information matrix.

[0072] Specifically, S4 and UWB ranging acquisition and adaptive weight adjustment include:

[0073] S401. Obtain the ranging information between the robot and the UWB base station, and establish a ranging model; if the robot's position at time t is p t The base station is located at b. k The measured distance is The ranging model is then expressed as:

[0074]

[0075] Where, p t =[x t ,y t ,z t ] T b represents the robot's position at time t. k =[b kx ,b kγ ,b kz ] T Let p be the location of the k-th UWB base station. t -b k ‖ represents the actual distance between the robot and the base station. This indicates Gaussian noise present during the ranging process;

[0076] S402. Based on the above model definition, the measurement residual is used to evaluate the error of the robot's current estimated position, and is expressed as:

[0077]

[0078] in, For distance measurement residuals;

[0079] S403. Adaptive weight evaluation: The weights of the ranging factors are dynamically adjusted based on the magnitude of the measurement residuals. The specific formula for the adaptive weight coefficients is as follows:

[0080]

[0081] Where, α t,k For adaptive weights; δ is the absolute value of the ranging residual, and δ is the residual threshold, which is set according to the actual scenario.

[0082] S404. Establish an information matrix for the ranging fusion factor to represent the uncertainty of UWB ranging. The information matrix after applying adaptive weights is defined as follows:

[0083]

[0084] in, This represents the fused information matrix. The variance of the ranging noise of the UWB sensor is determined based on the measurement accuracy of the actual UWB equipment.

[0085] S405. Define a cost function for UWB ranging fusion based on adaptive weights, which will be used in the subsequent factor map optimization steps. The specific formula is as follows:

[0086]

[0087] in, The cost function for a single UWB ranging factor represents the contribution of the ranging residual to the overall optimization.

[0088] S406. Calculate the UWB ranging residual function for position state p. t The Jacobian matrix is ​​expressed as:

[0089]

[0090] Expanding the above Jacobian matrix into its specific expression:

[0091]

[0092] in, For the ranging residual function with respect to the robot's position state p t The Jacobian matrix shows how positional changes affect the variation of the ranging residuals;

[0093] S407. When the robot observes multiple UWB base stations simultaneously, all base station ranging data are uniformly merged into a single overall UWB cost function:

[0094]

[0095] in, The overall cost function after fusing all UWB ranging information at time t is shown, K. t Let t be the set of all base stations that the robot can effectively measure distances from.

[0096] S5. Multi-source factor graph joint optimization: Under the factor graph framework, the IMU pre-integration factor, LiDAR relative pose factor and UWB ranging factor are integrated, and a nonlinear optimization algorithm is used to iteratively solve and update the position, velocity, attitude and IMU bias.

[0097] Specifically, the joint optimization of S5 and multi-source factor graphs includes:

[0098] S501. Construct an overall set of state variables to describe the robot's motion state at all keyframe moments. The overall state set is defined as follows:

[0099]

[0100] Where X represents the overall state vector to be optimized, which integrates the state information of all keyframes throughout the entire task process. t This represents the robot's motion state at discrete time t;

[0101] S502. Refine the definition of the robot state vector at a single moment, including position, attitude, velocity, and IMU bias term;

[0102] A single state is defined as follows:

[0103]

[0104] in, This represents the robot's position at time t, specifically as a three-dimensional vector. The robot's pose at time t is represented by a quaternion. The velocity of the robot at time t is represented as a three-dimensional vector. and These are the biases for the IMU sensor gyroscope and accelerometer, respectively;

[0105] S503. Define a unified target cost function J(X). During the process, this function comprehensively considers the differences between observations and predictions from all sensors. The overall cost function is expressed as:

[0106]

[0107] Where, r i (X) represents the residual value of the i-th observation factor; Ω i is the inverse of the covariance matrix of the corresponding factors;

[0108] S504. Define in detail the prediction factor residual function of the IMU sensor, which represents the error between the IMU observation and the current state estimate, in order to constrain the motion continuity between adjacent frames;

[0109] The IMU factor residual matrix is ​​defined as follows:

[0110]

[0111] in, Δv t,t+1 , Δp t,t+1 R(θ) represents the rotation, velocity, and position increments between adjacent keyframes calculated from pre-integrated IMU data. t () represents the rotation matrix consisting of attitude angles;

[0112] Define the LiDAR factor residual function to express the relative pose measurement error between adjacent frames of the lidar, and further enhance the local positioning accuracy.

[0113] The LiDAR factor residual matrix is ​​defined as follows:

[0114]

[0115] in, The rotation matrix between adjacent frames obtained after matching LiDAR point clouds. The translation matrix between adjacent frames obtained after matching LiDAR point clouds;

[0116] Define the residual function of the UWB ranging factor and use absolute scale information to provide position constraints to further eliminate drift error;

[0117] UWB ranging residual is defined as:

[0118]

[0119] b is the actual distance measured between the robot and the base station; k The known location of the k-th base station;

[0120] S505. Perform a linearization process to form a system of linear equations that can be solved iteratively;

[0121] The linearization process is represented as follows:

[0122] r i (X k +δX)≈r i (X k )+J i (X k )δX

[0123] Where, r i (X k +δX) represents X k The residual function at time +δX, J i (X k ) is the Jacobian matrix of the residual function, and δX is the state increment, representing the adjustment amount for each optimization;

[0124] Calculate the Hessian matrix H and gradient vector b during the optimization process. The Hessian matrix is:

[0125]

[0126] Where H is the Hessian matrix in factor graph optimization, and the detailed representation of the gradient vector is:

[0127]

[0128] Among them, J i (X k Ω is the Jacobian matrix, representing the rate of change of the residual function of the i-th factor with respect to the state variable, that is, reflecting the sensitivity of the residual to small changes in the state; i Let be the inverse of the error covariance matrix, and let represent the confidence level of the measurement data for the i-th factor.

[0129] Solve the above incremental equations using the Gauss-Newton method to obtain the state increment update, and then solve the linear equations:

[0130] HδX=b

[0131] S506. Update the current state estimate and proceed to the next optimization iteration to perform a state update:

[0132]

[0133] Determine the convergence condition to decide whether to terminate the optimization process; the criteria for determining convergence are:

[0134] ||δX||<∈

[0135] Where ∈ is the convergence threshold.

[0136] S6. Loop closure detection and UWB-assisted loop closure optimization: Periodically extract the geometric features of the point cloud in the current key frame, match them with historical key frames to complete loop closure detection, obtain loop closure pose constraints through fine registration, and construct a cross-frame residual addition factor map by combining the UWB ranging information at the same moment for loop closure optimization.

[0137] Specifically, S6, loop closure detection, and UWB-assisted closed-loop optimization include:

[0138] S601. Detect candidate loop closure keyframes and extract key geometric feature points from LiDAR keyframe point cloud data, including planar feature points and edge feature points, to quickly identify possible loop closure frames.

[0139] Prioritize calculating the curvature of local regions of the point cloud as features:

[0140]

[0141] Among them, c i Let p be the curvature of the i-th point in the point cloud. i Let N be the position of the i-th point. i For point p i The surrounding neighborhood point set, the ∥·∥ operator represents the Euclidean distance;

[0142] By comparing the feature descriptor similarity between the current frame and historical keyframes, potential loop closure keyframe pairs are quickly identified, and feature similarity is calculated.

[0143]

[0144] Among them, s t,j f is the similarity score between frame t and frame j. t,τ and f j,τ Let σ represent the τ-th feature descriptor in two frames. matchThe threshold for feature matching is represented by T, which represents the set of feature descriptors. If the calculated similarity exceeds the given threshold, it is considered a candidate keyframe for loop closure.

[0145] S602. Accurately estimate the pose of the loop closures. For the identified candidate loop closure frame pairs, further utilize the ICP algorithm to accurately estimate the poses of the point clouds in the two frames, obtaining the accurate relative pose relationship; the objective function is:

[0146]

[0147] Where R and t are the rotation matrix and translation matrix, respectively, and p m It is a point in the source point cloud, q n The corresponding matching points of the target point cloud, where P represents the set of successfully matched point pairs;

[0148] By iteratively minimizing the above function, the accurate pose transformation can be obtained:

[0149]

[0150] in, This is the precise pose transformation matrix between loop-loop frame pairs; and This is the rotation and translation estimation matrix corresponding to the loop frame;

[0151] S603. Introduce the geometric constraint information of UWB base stations on the loopback position to construct a UWB constraint factor based on multi-base station ranging; simultaneously, to fully utilize the information from multiple base stations, define a joint residual function that fuses the UWB constraints from multiple base stations:

[0152]

[0153] in, This represents the measured distance between the robot and the k-th UWB base station at time t. p represents the measured distance between the robot and the k-th UWB base station at time j of the loop closure matching frame. t and p j b represents the robot's position estimate in the global coordinate system at times t and j, respectively. k Let K be the location of the k-th UWB base station. t With K j This represents the set of UWB base stations that can simultaneously measure distances at times t and j.

[0154] By comparing the distance difference measured in two frames with the actual distance difference from the current estimated position to the same base station in these two frames, a stronger cross-frame positional geometric constraint relationship is constructed, further improving the reliability and accuracy of loopback pose optimization.

[0155] The Huber function is introduced into the joint UWB residual function to mitigate the impact of ranging outliers:

[0156]

[0157] in, γ is the robust kernel function of the UWB joint residual function, and γ is the threshold parameter of the robust kernel function, which is set according to the ranging error of the actual scene.

[0158] Calculate the Jacobian matrix of the joint UWB constraint, which will be used in the subsequent factor graph optimization linearization process:

[0159]

[0160] In the above equation, the part on the right-hand side is a first differentiation operation, that is, the derivative of the UWB joint residual function with respect to the change of the position estimation variables of the two frames.

[0161] S604. Construct closed-loop constraint factors and fused UWB factors. The relative pose estimated by the loop closure and the ranging constraints of UWB are combined as closed-loop constraint factors and incorporated into the factor graph optimization framework. The residual function of the loop closure factor is defined as:

[0162]

[0163] in, and R(θ) is the rotation and translation estimation matrix corresponding to the loopback frame. t ) T With R(θ) j Let be the rotation matrix currently estimated, and define the information matrix of the closure factor:

[0164]

[0165] in, and The noise variance representing the rotation and translation of the closed-loop estimation is then added to the information matrix Ω corresponding to the UWB factor. UWB The final total cost function is defined as the combination of all factors as follows:

[0166]

[0167] Among them, J prior (X) represents the prior cost function, which is the cost function composed of all existing observation factors of the system before the addition of the loop closure factor and the UWB factor;

[0168] S605. Linearize the global optimization problem and solve iteratively to obtain the updated Hessian matrix and gradient vector:

[0169]

[0170] Among them, H prior and b prior The prior Hessian matrix and prior gradient vector are used; then the linear equation is solved and the state increment is calculated to update the state; finally, through multiple iterations, convergence is achieved.

[0171] S7. Map building and incremental update: The local point cloud is transformed to the global coordinate system using the optimized keyframe pose, realizing real-time incremental map stitching; after the closed-loop optimization is completed, the positions of the historical keyframe point cloud are corrected and the global map is updated.

[0172] Specifically, S7, map building and incremental updates include:

[0173] S701, global stitching, local point cloud of keyframe t. The optimized pose transformation matrix is ​​used as follows:

[0174] T t =[R t |p t ]∈SE(3)

[0175] Map to the global coordinate system to obtain And add it to the global map;

[0176] Among them, R t It is a rotation matrix, p t It is a translation matrix;

[0177] S702, Incremental Update: When a new keyframe arrives, step S701 is only executed on the newly added point cloud to keep the map expanding in real time.

[0178] S703, Loop closure correction, when the corrected pose is obtained due to loop closure. Then recalculate:

[0179]

[0180] First, remove the old point cloud from the map, and then insert the corrected point cloud to maintain global consistency;

[0181] S704, trajectory output: Real-time output of keyframe trajectories updated according to optimization results, and continuous maintenance of the smoothness and ground accuracy of the closed-loop corrected trajectory. Figure 1 To the point of being responsive.

[0182] This embodiment constructs a multi-source collaborative optimization link that runs through the entire process by deeply integrating IMU high-frequency motion priors, LiDAR geometric registration information, and UWB absolute scale constraints within the factor graph framework: in the short term, IMU pre-integration ensures motion continuity; in the medium term, LiDAR keyframe registration suppresses local errors; and in the long term, adaptive weighted UWB ranging is used to "anchor" global coordinates in real time. The three complement each other, turning the localization and mapping errors from cumulative divergence to gradual convergence. This invention significantly reduces IMU drift and LiDAR matching instability in feature-sparse scenes, improving overall pose accuracy. It also eliminates reliance on GNSS or visual features, exhibiting inherent robustness to lighting and texture changes. Addressing the challenge of UWB errors exhibiting pulse-like spikes in multipath and NLOS environments, this invention proposes a residual-driven adaptive weighting mechanism: when the ranging residual exceeds a threshold, the information matrix weights are automatically reduced, "softly removing" outliers and fundamentally suppressing the destructive impact of jump noise on global optimization. Simultaneously, cross-frame UWB joint residuals are introduced in the loop closure phase, working with geometric loops to construct strong constraints, resulting in faster convergence and less drift for trajectory relocalization compared to traditional pure geometric loops. Practical verification shows that in long-distance operation in large indoor / underground spaces, global trajectory error can be reduced by more than 40%, fundamentally improving map distortion problems.

[0183] Corresponding to the ultra-wideband lidar-inertial navigation cooperative SLAM method disclosed in the above embodiments, this invention also discloses an ultra-wideband lidar-inertial navigation cooperative SLAM system, which specifically includes:

[0184] The system initialization module is used to perform static calibration of the IMU to obtain the initial bias of the gyroscope and accelerometer, complete the three-dimensional coordinate calibration of the UWB base station using geometric measurement method, and determine the initial pose of the robot through initial LiDAR point cloud registration, thereby realizing the calibration of the three-sensor coordinate system and the initialization of the system state.

[0185] The IMU data pre-integration and key frame selection module is used to acquire IMU angular velocity and acceleration data in real time, and uses a pre-integration algorithm to calculate the pose increment between adjacent time moments. When the cumulative rotation or displacement exceeds a preset threshold, key frame selection is triggered.

[0186] The LiDAR point cloud matching and relative pose estimation module is used to obtain the current point cloud and the point cloud of the previous key frame for consecutive key frames, complete the registration using the ICP algorithm, obtain the relative rotation matrix and translation vector, and construct the LiDAR factor constraint.

[0187] The UWB ranging acquisition and adaptive weight adjustment module is used to synchronously acquire distance data between the robot and each UWB base station, establish a ranging residual function, calculate adaptive weights based on the residual size, and dynamically adjust the ranging factor information matrix.

[0188] The multi-source factor graph joint optimization module is used to integrate IMU pre-integration factor, LiDAR relative pose factor and UWB ranging factor within the factor graph framework, and iteratively solve the problem using a nonlinear optimization algorithm to update position, velocity, attitude and IMU bias.

[0189] The loop closure detection and UWB-assisted loop closure optimization module is used to periodically extract the geometric features of the point cloud in the current key frame, match them with historical key frames to complete loop closure detection, obtain loop closure pose constraints through fine registration, and construct a cross-frame residual addition factor map in conjunction with the UWB ranging information at the same moment for loop closure optimization.

[0190] The map building and incremental update module is used to transform the local point cloud to the global coordinate system using the optimized keyframe poses, so as to realize real-time incremental map stitching; after the closed-loop optimization is completed, the positions of the historical keyframe point cloud are corrected and the global map is updated.

[0191] It should be noted that for a detailed description of the ultra-wideband lidar-inertial navigation cooperative SLAM system provided in the embodiments of the present invention, please refer to the relevant description of the ultra-wideband lidar-inertial navigation cooperative SLAM method provided in the embodiments of this application, which will not be repeated here.

[0192] The ultra-wideband lidar-inertial navigation cooperative SLAM system provided in this embodiment organically integrates three types of sensors—IMU, LiDAR, and UWB—in the spatiotemporal dimensions through a modular structure, possessing advantages such as rapid initialization, efficient data processing, and accurate state estimation. The system's modules have clear division of labor and operate collaboratively, enabling not only continuous estimation of short-term high-frequency motion and accurate registration of mid-range relative pose, but also effectively suppressing drift errors by introducing global scale constraints through UWB. Simultaneously, combined with an adaptive weighting mechanism and a loop closure-assisted optimization strategy, the system further enhances the robustness and closed-loop consistency of multi-source fusion, ensuring high-precision and highly stable real-time positioning and mapping even in GPS-free or complex environments, demonstrating good engineering practicality and scalability.

[0193] In addition, embodiments of the present invention also provide an electronic device, the device comprising: a processor and a memory; the memory being used to store one or more program instructions; the processor being used to execute one or more program instructions to perform the steps of an ultra-wideband lidar-inertial navigation cooperative SLAM method as described in any of the preceding embodiments.

[0194] It should be noted that for a detailed description of an electronic device provided in the embodiments of the present invention, please refer to the relevant description of an ultra-wideband lidar-inertial navigation cooperative SLAM method provided in the embodiments of this application, which will not be repeated here.

[0195] In addition, embodiments of the present invention also provide a computer-readable storage medium storing a computer program, which, when executed by a processor, implements the steps of the above-described ultra-wideband lidar-inertial navigation cooperative SLAM method.

[0196] It should be noted that for a detailed description of a computer-readable storage medium provided in the embodiments of the present invention, please refer to the relevant description of an ultra-wideband lidar-inertial navigation cooperative SLAM method provided in the embodiments of this application, which will not be repeated here.

[0197] Those skilled in the art will understand that all or part of the functions of the various methods in the above embodiments can be implemented by hardware or by computer programs. When all or part of the functions in the above embodiments are implemented by computer programs, the program can be stored in a computer-readable storage medium, which may include: read-only memory, random access memory, disk, optical disk, hard disk, etc., and the program is executed by a computer to achieve the above functions. For example, the program can be stored in the memory of a device, and when the program in the memory is executed by the processor, all or part of the above functions can be achieved. In addition, when all or part of the functions in the above embodiments are implemented by computer programs, the program can also be stored in a server, another computer, disk, optical disk, flash drive, or external hard drive, etc., and can be downloaded or copied to the memory of a local device, or the system of the local device can be updated. When the program in the memory is executed by the processor, all or part of the functions in the above embodiments can be achieved.

[0198] The above examples illustrate the present invention only to aid in understanding it and are not intended to limit the scope of the invention. Those skilled in the art can make various simple deductions, modifications, or substitutions based on the principles of this invention.

Claims

1. A method for ultra-wideband lidar-inertial navigation cooperative SLAM, characterized in that, The method includes: S1. System initialization: Static calibration of the IMU is performed to obtain the initial bias of the gyroscope and accelerometer. The three-dimensional coordinate calibration of the UWB base station is completed using the geometric measurement method. The initial pose of the robot is determined by the initial LiDAR point cloud registration, realizing the calibration of the three-sensor coordinate system and the initialization of the system state. S2, IMU data pre-integration and key frame selection: real-time acquisition of IMU angular velocity and acceleration data, use pre-integration algorithm to calculate pose increment between adjacent time moments, and trigger key frame selection when the cumulative rotation or displacement exceeds a preset threshold. S3, LiDAR point cloud matching and relative pose estimation: For continuous keyframes, obtain the current point cloud and the point cloud of the previous keyframe, use the ICP algorithm to complete the registration, obtain the relative rotation matrix and translation vector, and construct the LiDAR factor constraint. S4, UWB ranging acquisition and adaptive weight adjustment: synchronously acquire distance data between the robot and each UWB base station, establish a ranging residual function, calculate adaptive weights based on the residual size and dynamically adjust the ranging factor information matrix; The information matrix is ​​defined as: Where, α t,k For adaptive weights, This represents the fused information matrix. The variance of the ranging noise of the UWB sensor is determined based on the measurement accuracy of the actual UWB equipment. S5. Multi-source factor graph joint optimization: Under the factor graph framework, the IMU pre-integration factor, LiDAR relative pose factor and UWB ranging factor are integrated and solved iteratively using a nonlinear optimization algorithm to update position, velocity, attitude and IMU bias. S6. Loop closure detection and UWB-assisted loop closure optimization: Periodically extract the geometric features of the point cloud of the current key frame, match them with historical key frames to complete loop closure detection, obtain loop closure pose constraints through fine registration, and construct a cross-frame residual addition factor map by combining the UWB ranging information at the same moment for loop closure optimization. S7. Map building and incremental update: The local point cloud is transformed to the global coordinate system using the optimized keyframe pose, realizing real-time incremental map stitching; after the closed-loop optimization is completed, the positions of the historical keyframe point cloud are corrected and the global map is updated.

2. The ultra-wideband lidar-inertial navigation cooperative SLAM method according to claim 1, characterized in that, S1, system initialization, includes: S101, IMU static calibration: Continuously acquire N frames of IMU observation data while the system is stationary, and estimate the gyroscope bias using the following formula: Estimate the accelerometer bias using the following formula: Where, ω t and a t ω and ωc are the observed values ​​of angular velocity and acceleration at time t, respectively, and g is the gravitational acceleration vector; S102, UWB base station triangulation calibration: Given the robot's pose, record the distance data between the robot and each UWB base station, and obtain the three-dimensional coordinates of each base station using geometric trilateration. b k =[x k ,y k ,z k ] T ,k=1,2,…,M Where M represents the number of base stations; S103, LiDAR initial attitude determination, initial point cloud frame P t Registered with the environmental reference point cloud, the initial pose transformation matrix is ​​calculated: T t =[R t ∣p t ] Where R t Let p be a rotation matrix. t It is a translation vector used to define the spatial coordinate relationship at the initial moment of the system.

3. The ultra-wideband lidar-inertial navigation cooperative SLAM method according to claim 1, characterized in that, The S2, IMU data pre-integration and keyframe selection include: S201, based on the continuous-time motion model: Where R(θ) t ) is the attitude rotation matrix at time t, a t and ω t It represents the acceleration and angular velocity at time t. The calculation process includes position p, velocity v, attitude θ, and bias b. g b a ; S202, calculate the rotation increment ΔR between adjacent keyframes using a discretized pre-integration method. t,t+1 velocity increment Δv t,t+1 and position increment Δp t,t+1 , to describe the state evolution of the robot from t to t+1; When the attitude change accumulated by the IMU integral satisfies: If the cumulative displacement exceeds a preset threshold, the current frame is set as the new keyframe, where t ′ For the previous keyframe time, ∈ θ This is the angle threshold.

4. The ultra-wideband lidar-inertial navigation cooperative SLAM method according to claim 1, characterized in that, The S3, LiDAR point cloud matching, and relative pose estimation include: S301, Solve for pose transformation: For the current frame point, This is the point corresponding to the previous keyframe; S302, decompose the optimal transformation into: in For relative rotation matrices, It is a relative translation vector; S303, Constructing LiDAR residuals in the factor plot: Among them, R t p t For the rotation and position state variables of the current frame, the log operation represents the Lie algebra logarithmic mapping.

5. The ultra-wideband lidar-inertial navigation cooperative SLAM method according to claim 1, characterized in that, The S4, UWB ranging acquisition and adaptive weight adjustment include: S401. Obtain the ranging information between the robot and the UWB base station, and establish a ranging model; if the robot's position at time t is p t The base station is located at b. k The measured distance is The ranging model is then expressed as: Where, p t =[x t ,y t ,z t ] T b represents the robot's position at time t. k =[b kx ,b kγ ,b kz ] T Let p be the location of the k-th UWB base station. t -b k ‖ represents the actual distance between the robot and the base station. This indicates Gaussian noise present during the ranging process; S402. Based on the above model definition, the measurement residual is used to evaluate the error of the robot's current estimated position, and is expressed as: in, For distance measurement residuals; S403. Adaptive weight evaluation: The weights of the ranging factors are dynamically adjusted based on the magnitude of the measurement residuals. The specific formula for the adaptive weight coefficients is as follows: Where, α t,k For adaptive weights; δ is the absolute value of the ranging residual, and δ is the residual threshold, which is set according to the actual scenario. S404. Establish an information matrix for the ranging fusion factor to represent the uncertainty of UWB ranging. The information matrix after applying adaptive weights is defined as follows: in, This represents the fused information matrix. The variance of the ranging noise of the UWB sensor is determined based on the measurement accuracy of the actual UWB equipment. S405. Define a cost function for UWB ranging fusion based on adaptive weights, which will be used in the subsequent factor map optimization steps. The specific formula is as follows: in, The cost function for a single UWB ranging factor represents the contribution of the ranging residual to the overall optimization. S406. Calculate the UWB ranging residual function for position state p. t The Jacobian matrix is ​​expressed as: Expanding the above Jacobian matrix into its specific expression: in, For the ranging residual function with respect to the robot's position state p t The Jacobian matrix shows how positional changes affect the variation of the ranging residuals; S407. When the robot observes multiple UWB base stations simultaneously, all base station ranging data are uniformly merged into a single overall UWB cost function: in, The overall cost function after fusing all UWB ranging information at time t is shown, K. t Let t be the set of all base stations that the robot can effectively measure distances from.

6. The ultra-wideband lidar-inertial navigation cooperative SLAM method according to claim 1, characterized in that, The S5 and multi-source factor graph joint optimization includes: S501. Construct an overall set of state variables to describe the robot's motion state at all keyframe moments. The overall state set is defined as follows: Where X represents the overall state vector to be optimized, which integrates the state information of all keyframes throughout the entire task process. t This represents the robot's motion state at discrete time t; S502. Refine the definition of the robot state vector at a single moment, including position, attitude, velocity, and IMU bias term; A single state is defined as follows: in, This represents the robot's position at time t, specifically as a three-dimensional vector. The robot's pose at time t is represented by a quaternion. The velocity of the robot at time t is represented as a three-dimensional vector. and These are the biases for the IMU sensor gyroscope and accelerometer, respectively; S503. Define a unified target cost function J(X). During the process, this function comprehensively considers the differences between observations and predictions from all sensors. The overall cost function is expressed as: Where, r i (X) represents the residual value of the i-th observation factor; Ω i is the inverse of the covariance matrix of the corresponding factors; S504. Define in detail the prediction factor residual function of the IMU sensor, which represents the error between the IMU observation and the current state estimate, in order to constrain the motion continuity between adjacent frames; The IMU factor residual matrix is ​​defined as follows: in, Δv i,t+1 , Δp t,t+1 R(θ) represents the rotation, velocity, and position increments between adjacent keyframes calculated from pre-integrated IMU data. t () represents the rotation matrix consisting of attitude angles; Define the LiDAR factor residual function to express the relative pose measurement error between adjacent frames of the lidar, and further enhance the local positioning accuracy. The LiDAR factor residual matrix is ​​defined as follows: in, The rotation matrix between adjacent frames obtained after matching LiDAR point clouds. The translation matrix between adjacent frames obtained after matching LiDAR point clouds; Define the residual function of the UWB ranging factor and use absolute scale information to provide position constraints to further eliminate drift error; UWB ranging residual is defined as: b is the actual distance measured between the robot and the base station; k The known location of the k-th base station; S505. Perform a linearization process to form a system of linear equations that can be solved iteratively; The linearization process is represented as follows: r i (X k +δX)≈r i (X k )+J i (X k )δX Where, r i (X k +δX) represents X k The residual function at time +δX, J i (X k ) is the Jacobian matrix of the residual function, and δX is the state increment, representing the adjustment amount for each optimization; Calculate the Hessian matrix H and gradient vector b during the optimization process. The Hessian matrix is: Where H is the Hessian matrix in factor graph optimization, and the detailed representation of the gradient vector is: Among them, J i (X k Ω is the Jacobian matrix, representing the rate of change of the residual function of the i-th factor with respect to the state variable, that is, reflecting the sensitivity of the residual to small changes in the state; i Let be the inverse of the error covariance matrix, and let represent the confidence level of the measurement data of the i-th factor. Solve the above incremental equations using the Gauss-Newton method to obtain the state increment update, and then solve the linear equations: HδX=b S506. Update the current state estimate and proceed to the next optimization iteration to perform a state update: Determine the convergence condition to decide whether to terminate the optimization process; the criteria for determining convergence are: ||δX||<∈ Where ∈ is the convergence threshold.

7. The ultra-wideband lidar-inertial navigation cooperative SLAM method according to claim 1, characterized in that, The S6, loop closure detection, and UWB-assisted loop closure optimization include: S601. Detect candidate loop closure keyframes and extract key geometric feature points from LiDAR keyframe point cloud data, including planar feature points and edge feature points, to quickly identify possible loop closure frames. Prioritize calculating the curvature of local regions of the point cloud as features: Among them, c i Let p be the curvature of the i-th point in the point cloud. i Let N be the position of the i-th point. i For point p i The set of surrounding neighboring points, where the ||·|| operator represents the Euclidean distance; By comparing the feature descriptor similarity between the current frame and historical keyframes, potential loop closure keyframe pairs are quickly identified, and feature similarity is calculated. Among them, s t,j f is the similarity score between frame t and frame j. t,τ and f j,τ Let σ represent the τ-th feature descriptor in two frames. match The threshold for feature matching is represented by T, which represents the set of feature descriptors. If the calculated similarity exceeds the given threshold, it is considered a candidate keyframe for loop closure. S602. Accurately estimate the pose of the loop closures. For the identified candidate loop closure frame pairs, further utilize the ICP algorithm to accurately estimate the poses of the point clouds in the two frames, obtaining the accurate relative pose relationship; the objective function is: Where R and t are the rotation matrix and translation matrix, respectively, and p m It is a point in the source point cloud, q n The corresponding matching points of the target point cloud, where P represents the set of successfully matched point pairs; By iteratively minimizing the above function, the accurate pose transformation can be obtained: in, This is the precise pose transformation matrix between loop-loop frame pairs; and This is the rotation and translation estimation matrix corresponding to the loopback frame; S603. Introduce the geometric constraint information of UWB base stations on the loopback position to construct a UWB constraint factor based on multi-base station ranging; simultaneously, to fully utilize the information from multiple base stations, define a joint residual function that fuses the UWB constraints from multiple base stations: in, This represents the measured distance between the robot and the k-th UWB base station at time t. p represents the measured distance between the robot and the k-th UWB base station at time j of the loop closure matching frame. t and p j b represents the robot's position estimate in the global coordinate system at times t and j, respectively. k Let K be the location of the k-th UWB base station. t With K j This represents the set of UWB base stations that can simultaneously measure distances at times t and j. By comparing the distance difference measured in two frames with the actual distance difference from the current estimated position to the same base station in these two frames, a stronger cross-frame positional geometric constraint relationship is constructed, further improving the reliability and accuracy of loop closure pose optimization. The Huber function is introduced into the joint UWB residual function to mitigate the impact of ranging outliers: in, γ is the robust kernel function of the UWB joint residual function, and γ is the threshold parameter of the robust kernel function, which is set according to the ranging error of the actual scene. Calculate the Jacobian matrix of the joint UWB constraint, which will be used in the subsequent factor graph optimization linearization process: In the above equation, the part on the right-hand side is a first differentiation operation, that is, the derivative of the UWB joint residual function with respect to the change of the position estimation variables of the two frames. S604. Construct closed-loop constraint factors and fused UWB factors. The relative pose estimated by the loop closure and the ranging constraints of UWB are combined as closed-loop constraint factors and incorporated into the factor graph optimization framework. The residual function of the loop closure factor is defined as: in, and R(θ) is the rotation and translation estimation matrix corresponding to the loopback frame. t ) T With R(θ) j Let be the rotation matrix currently estimated, and define the information matrix of the closure factor: in, and The noise variance representing the rotation and translation of the closed-loop estimation is then added to the information matrix Ω corresponding to the UWB factor. UWB The final total cost function is defined as the combination of all factors as follows: Among them, J prior (X) represents the prior cost function, which is the cost function composed of all existing observation factors of the system before the addition of the loop closure factor and the UWB factor; S605. Linearize the global optimization problem and solve iteratively to obtain the updated Hessian matrix and gradient vector: Among them, H prior and b prior The prior Hessian matrix and prior gradient vector are used; then the linear equation is solved and the state increment is calculated to update the state; finally, through multiple iterations, convergence is achieved.

8. The ultra-wideband lidar-inertial navigation cooperative SLAM method according to claim 1, characterized in that, The S7, map building and incremental update include: S701, global stitching, local point cloud of keyframe t. The optimized pose transformation matrix is ​​used as follows: T t =[R t ∣p t ]∈SE (3) Map to the global coordinate system to obtain And add it to the global map; Among them, R t It is a rotation matrix, p t It is a translation matrix; S702, Incremental Update: When a new keyframe arrives, step S701 is only executed on the newly added point cloud to keep the map expanding in real time. S703, Loop closure correction, when the corrected pose is obtained due to loop closure. Then recalculate: First, remove the old point cloud from the map, and then insert the corrected point cloud to maintain global consistency; S704, Trajectory Output: Outputs keyframe trajectories updated in real time according to optimization results, and continuously maintains the smoothness of the trajectory after closed-loop correction and the consistency with the map.

9. An ultra-wideband lidar-inertial navigation cooperative SLAM system, characterized in that, The system includes: The system initialization module is used to perform static calibration of the IMU to obtain the initial bias of the gyroscope and accelerometer, complete the three-dimensional coordinate calibration of the UWB base station using geometric measurement method, and determine the initial pose of the robot through initial LiDAR point cloud registration, thereby realizing the calibration of the three-sensor coordinate system and the initialization of the system state. The IMU data pre-integration and key frame selection module is used to acquire IMU angular velocity and acceleration data in real time, and uses a pre-integration algorithm to calculate the pose increment between adjacent time moments. When the cumulative rotation or displacement exceeds a preset threshold, key frame selection is triggered. The LiDAR point cloud matching and relative pose estimation module is used to obtain the current point cloud and the point cloud of the previous key frame for consecutive key frames, complete the registration using the ICP algorithm, obtain the relative rotation matrix and translation vector, and construct the LiDAR factor constraint. The UWB ranging acquisition and adaptive weight adjustment module is used to synchronously acquire distance data between the robot and each UWB base station, establish a ranging residual function, calculate adaptive weights based on the residual size, and dynamically adjust the ranging factor information matrix. The information matrix is ​​defined as: Where, α t,k For adaptive weights, This represents the fused information matrix. The variance of the ranging noise of the UWB sensor is determined based on the measurement accuracy of the actual UWB equipment. The multi-source factor graph joint optimization module is used to integrate IMU pre-integration factor, LiDAR relative pose factor and UWB ranging factor within the factor graph framework, and iteratively solve the problem using a nonlinear optimization algorithm to update position, velocity, attitude and IMU bias. The loop closure detection and UWB-assisted loop closure optimization module is used to periodically extract the geometric features of the point cloud in the current key frame, match them with historical key frames to complete loop closure detection, obtain loop closure pose constraints through fine registration, and construct a cross-frame residual addition factor map in conjunction with the UWB ranging information at the same moment for loop closure optimization. The map building and incremental update module is used to transform the local point cloud to the global coordinate system using the optimized keyframe poses, so as to realize real-time incremental map stitching; after the closed-loop optimization is completed, the positions of the historical keyframe point cloud are corrected and the global map is updated.

10. An electronic device, characterized in that, The device includes: a processor and a memory; The memory is used to store one or more program instructions; The processor is configured to run one or more program instructions to perform the steps of an ultra-wideband lidar-inertial navigation cooperative SLAM method as described in any one of claims 1 to 8.

Images