Fusion positioning mapping method using single anchor point external confidence source

Abstract

The invention provides a fusion positioning mapping method using a single anchor point external confidence source. The method comprises the following steps: step 1, arranging UWB anchor points; 2, initializing the mobile robot; 3, processing a front-end point cloud map; step 4, processing UWB ranging information; step 5, key frame judgment; and step 6, optimizing a back-end factor graph. The invention provides a multi-sensor synchronous positioning and mapping system integrating a 3D laser radar, an inertial measurement unit and an ultra-wideband sensor, and aims to improve the positioning precision and trajectory estimation performance of a mobile robot in a complex environment. According to the method, a robust and efficient positioning scheme is provided by combining data of three sensors through a factor graph optimization method.

Description

Technical Field

[0001] The present invention relates to the field of robot positioning technology, and in particular to a fusion positioning and mapping method using a single anchor point external signal source. Background Art

[0002] LiDAR and IMU sensors are widely used in robotics and autonomous driving technologies. LiDAR has the ability to directly obtain depth information of environmental point clouds and is not affected by lighting conditions, making it an important player in environmental perception and three-dimensional mapping. At the same time, IMU, with its ability to provide high-frequency continuous attitude information, combined with LiDAR sensors, greatly improves the accuracy of positioning and mapping.

[0003] The LiDAR inertial odometer formed by this combination performs well in local environments, enabling high-precision map construction and accurate motion trajectory tracking in the short term. However, the LiDAR inertial odometer system is susceptible to sensor noise, drift, etc. during long-distance or long-term movement, resulting in accumulated positioning errors. In addition, in harsh environments (such as low light, high dust, etc.), the performance of pure vision or LiDAR systems will drop significantly.

[0004] Loop detection is widely used to correct long-term positioning errors. It detects when the robot returns to an area that it has previously explored and adjusts its current position based on previous data, thereby reducing drift. However, loop detection requires the robot to pass through the same or similar scenes multiple times during movement, which places certain restrictions on path design and is difficult to meet in many scenarios. In addition, during the data matching process, loop detection and posture optimization consume a lot of computing resources.

[0005] Satellite positioning and navigation systems, such as the Global Positioning System (GPS) and Beidou positioning and navigation systems, can perform multi-sensor information fusion with lidar inertial odometers to correct trajectory drift, which is effective in open outdoor environments. However, in complex environments such as urban canyons, forests, or indoors, satellite navigation signals are often blocked, affecting positioning accuracy. Therefore, the above-mentioned system that integrates satellite positioning and navigation with lidar inertial odometers will inevitably have significantly reduced reliability and accuracy.

[0006] In recent years, the SLAM system that integrates other sensors, such as UWB, has become a research hotspot in multi-sensor fusion technology. Compared with GPS, UWB sensors can provide more reliable global positioning information in areas with poor satellite navigation signals, and are therefore regarded as a potential technology for providing global positioning in environments without satellite navigation signals.

[0007] The existing technical solutions are as follows:

[0008] In order to enhance the positioning accuracy of the LiDAR odometer and reduce trajectory drift errors, the existing technical solutions mainly include:

[0009] (1) GPS fusion solution: Correcting the drift of the LIO system by fusing GPS signals works well in open areas, but the unavailability of GPS signals in indoor or other signal-blocked scenarios limits the application of this solution. In addition, the GPS device itself will increase the complexity and cost of the system.

[0010] (2) UWB fusion solution: UWB can provide relatively stable location information in an environment without satellite navigation, but relying solely on UWB positioning cannot perceive the surrounding environment and build a map. It lacks obstacle information and cannot achieve safe navigation. Therefore, with the development of UWB technology, exploration of integrating UWB with lidar or visual odometer systems has begun. At present, most fusion solutions adopt a loosely coupled approach, that is, UWB is independently positioned through multiple anchor points, and then later aligned with lidar or IMU data, which improves the system accuracy to a certain extent. However, the loosely coupled approach fails to fully utilize the correlation between sensors, and its real-time and robustness are poor. The use of multi-anchor UWB deployment makes the system deployment cost high.

[0011] The method most similar to the present invention is the paper “Range-focused fusion of camera-IMU-UWB for accurate and drift-reduced localization” published by Thien Hoang Nguyen et al. This method achieves a tightly coupled fusion scheme of single-anchor UWB and visual inertial odometer (VIO) by adding UWB factors, thereby reducing system drift. However, the disadvantage of this solution is that it uses SLAM based on visual sensors, which is greatly affected by ambient lighting and visual features, and its performance in complex environments is limited; in addition, this method processes all UWB observation data, and the increased data processing and calculation greatly increases the system's computational burden and memory resource usage.

[0012] Defects and shortcomings of the existing technology:

[0013] (1) Lack of global sensor information and limitations in scenarios without GPS signals: In complex environments (such as indoors, urban canyons, forests, and underwater), GNSS signals are often blocked or interfered with, causing the drift correction scheme based on GPS to fail. This makes the existing laser inertial odometry (LIO) system unable to provide continuous, stable, and precise positioning in these scenarios, and is prone to long-term odometer drift problems. Although the LIO system performs well in short-term positioning accuracy, it cannot solve long-term positioning drift. In these cases, UWB (ultra-wideband) can provide stable global ranging information, but existing methods fail to effectively utilize this advantage.

[0014] (2) High deployment cost and complexity of multiple UWB anchor solutions: Existing UWB-based multi-sensor fusion SLAM systems usually require the deployment of multiple UWB anchors to provide global location information. Although such a multi-anchor solution can improve positioning accuracy, it also significantly increases the deployment complexity and cost of the system, especially in large-scale or complex scenarios. It is difficult and uneconomical to deploy multiple anchors.

[0015] (3) The existing SLAM system that integrates UWB is heavy, and the filter-based method used is not robust enough: the existing method that relies on filters (such as the extended Kalman filter EKF) for state estimation updates the current state in a recursive manner, and cannot trace back historical data and adjust past estimates. If there is an error in the initial estimate, the system state may gradually deviate from the true trajectory over time, resulting in a decrease in accuracy. In addition, when processing multi-sensor information, the filter method is difficult to achieve global optimal estimation and has limited robustness.

[0016] (4) The existing single-anchor UWB fusion visual inertial odometry method has the problem that the visual perception environment is easily affected by lighting, texture, etc., resulting in insufficient adaptability and reliability of the method. In addition, the existing method processes all UWB observation data. The increased data processing and calculation greatly increases the system's computational burden and memory resource usage, which is not conducive to deployment on robot platforms with limited computing resources.

[0017] Reliable positioning and mapping are one of the core functions of intelligent mobile robots and intelligent vehicles. In scenarios where satellite navigation fails, the robot mainly relies on its own sensors for positioning and mapping. Commonly used sensors include cameras, lidars, etc. Among them, laser inertial odometry (LiDAR Inertial Odometry, LIO) and simultaneous localization and mapping (SLAM) based on lidar sensors and inertial measurement units (IMU) sensors are better solutions. Compared with solutions based on visual cameras, lidar sensors can directly obtain depth information of feature points in the environment and are not affected by lighting conditions. Therefore, in terms of robustness, accuracy and computing resource consumption, this type of solution has significant advantages over visual odometry (VO).

[0018] However, due to the existence of sensor noise and calculation errors, the LiDAR-based positioning and mapping system will inevitably produce cumulative drift over time. Loop detection is one of the common means to correct the cumulative drift, but it has requirements for the robot's motion trajectory, requiring a trajectory that revisits the same scene, and loop closure and pose optimization require a lot of computing resources. In addition, the essence of loop detection is to distribute the accumulated error along the trajectory, and real-time correction cannot be achieved. Therefore, it is still very necessary to restore the trajectory of historical key frames in a short time.

[0019] In complex environments such as insufficient lighting and similar structures, the introduction of ultra-wideband (UWB) technology can provide stable and reliable distance information for the LiDAR inertial odometry (LIO) system, improving the performance of positioning and mapping. At present, most existing methods are implemented in a loosely coupled manner, that is, UWB independently estimates the position in space through multi-anchor positioning, and then aligns and fuses it with the pose of the LIO or VIO system. Although this loosely coupled method based on multiple anchors can improve positioning accuracy, it will increase the deployment cost and complexity of the system, making it difficult to deploy in large-scale scenarios and lightweight computing robot systems.

[0020] The present invention aims to solve the technical problem of reliable positioning and mapping of intelligent mobile robots and intelligent vehicles in an environment without satellite navigation signals.

[0021] (1) Many robot navigation scenarios (such as indoors, tunnels, forests, etc.) do not have satellite navigation signals and cannot achieve accurate positioning through satellite navigation.

[0022] (2) The existing laser inertial odometry (LIO) and simultaneous localization and mapping (SLAM) solutions based on lidar sensors and inertial measurement units can achieve positioning and mapping in scenarios without satellite navigation, but there is a problem of cumulative trajectory drift. That is, the existing lidar SLAM solutions are prone to error drift that accumulates over time. Especially in scenarios where no loop detection occurs for a long time, the errors will gradually accumulate, thereby affecting the accuracy of mapping.

[0023] (3) Adding ultra-wideband UWB-assisted positioning and performing multi-sensor fusion with LIO or laser-based SLAM can help enhance positioning reliability and accuracy, but existing methods usually rely on multiple UWB anchor points to provide accurate location information, resulting in high deployment costs and complexity. Existing methods process and fuse all observed UWB data, which increases the computational burden of the system and is not conducive to deployment on robot platforms with limited computing resources. Summary of the invention

[0024] In order to solve the current technical problems, the main purpose of the present invention is to provide a fusion positioning and mapping method using a single anchor point external signal source. This method integrates a multi-sensor simultaneous localization and mapping (SLAM) system of 3D laser radar (LiDAR), inertial measurement unit (IMU) and ultra-wideband (UWB) sensor, effectively improving the positioning accuracy and trajectory estimation performance of mobile robots in complex environments; the present invention provides a robust and efficient positioning solution by combining the data of three sensors through a factor graph optimization method.

[0025] In order to realize the above technical features, the object of the present invention is achieved as follows: a fusion positioning and mapping method using a single anchor point external source comprises the following steps:

[0026] Step 1: Deploy UWB anchor points to ensure that the mobile robot can obtain UWB information in real time;

[0027] Step 2: Initialize the mobile robot; obtain an estimate of the current pose through the IMU on the mobile robot, and fuse it with the initial radar point cloud to obtain the initial pose;

[0028] Step 3: Front-end point cloud map processing: After the radar point cloud is calibrated and feature extracted, it is determined whether it is a key frame. If it is, the historical key frame data is updated and point cloud matching is performed;

[0029] Step 4: UWB ranging information processing: performing outlier removal, sliding average filtering and data alignment processing on the received UWB ranging information;

[0030] Step 5: Key frame judgment: When the robot displacement exceeds s or the rotation angle exceeds r, or the time interval exceeds T, the current frame is selected as the key frame to optimize the calculation efficiency and the construction of the global map;

[0031] Step 6: Back-end factor graph optimization: Use the factor graph optimization algorithm to integrate and optimize the IMU, LiDAR key frame and UWB key frame moment data, and output the point cloud map and pose results. This process combines IMU, LiDAR and UWB data, and outputs the final pose and point cloud map through the optimization algorithm.

[0032] Step 1 specifically includes: Deploy a static UWB anchor point in the environment to be positioned. The anchor point should be placed within the robot's line of sight and connected to a power source. Ensure that the UWB anchor point and the UWB tag carried by the mobile robot are in the same operating frequency and mode.

[0033] Step 2 specifically includes: turning on the mobile robot device, controlling the mobile robot to move when the UWB anchor point is visible, the mobile robot obtains IMU data and performs IMU pre-integration processing, calculates the current posture through the IMU odometer, and sets the initial position and posture estimation.

[0034] Step 3 specifically includes: during the movement of the robot, the LiDAR point cloud data is acquired in real time through the laser radar carried by the robot and point cloud correction processing is performed; after filtering denoising and distortion correction operations, feature points are extracted from the corrected point cloud, point cloud registration is performed based on the extracted feature points, and whether the current frame is a key frame is determined based on the feature points: if it is a key frame, the historical key frame data is updated; if not, the current point cloud map continues to be updated.

[0035] Step 4 specifically includes: on the mobile robot platform, the IMU and the UWB tag are installed on the same rigid object, so the translation offset between the UWB antenna and the IMU can be ignored. Therefore, the distance measurement between the UWB tag and the UWB anchor point directly reflects the distance change between the static anchor point and the robot. The onboard computer obtains the UWB ranging information through a wired connection, and performs anomaly removal processing, performs sliding window average filtering on the UWB data after the anomaly is removed, and aligns the filtered UWB anchor point data with other data in time.

[0036] In step 5, the value of s is 0.5m, the value of r is 0.1 radian, and the value of T is 1s.

[0037] Step 6 specifically includes: using a factor graph optimization algorithm to integrate and optimize the IMU, LiDAR and UWB data; the IMU factor, LiDAR factor and UWB factor constitute the basic unit of the factor graph; the IMU factor uses data from the inertial measurement unit to estimate the robot's motion state and provide basic dynamic information for continuous posture estimation; the LiDAR factor uses data from the 3D LiDAR for precise spatial positioning, helping the system to build an environmental map and optimize the trajectory with high precision; the UWB factor uses the distance measurement information provided by the UWB sensor to constrain the robot's position estimation relative to the static anchor point; combining the IMU, LiDAR and UWB data, outputting the final pose and point cloud map.

[0038] The IMU factors are specifically:

[0039] By pre-integrating the IMU data between two LiDAR frames i and j, the following increments are obtained:

[0040] Velocity increment Δv ij :

[0041]

[0042] Displacement increment Δp ij :

[0043]

[0044] Rotation increment ΔR ij :

[0045]

[0046] The main optimization parameter is the bias of the IMU;

[0047] In the formula, The transpose of the rotation matrix at frame i represents the transformation from the world coordinate system to the frame i coordinate system;

[0048] v j : velocity vector at frame j;

[0049] v i : velocity vector at frame i;

[0050] g: gravitational acceleration vector, which is a fixed vector in the world coordinate system;

[0051] Δt ij : The time difference between frame i and frame j, indicating the integration time of IMU data;

[0052] p j : displacement vector at frame j;

[0053] p i : displacement vector at frame i;

[0054] Δp ij : represents the displacement increment between frame i and frame j, reflecting the position change of the device during this period;

[0055] The force of gravity on an object at time Δt ij Position changes caused by

[0056] R j : is the rotation matrix at frame j.

[0057] The LiDAR Factor:

[0058] LiDAR scans extract planar and edge features by evaluating the roughness of points within a local area; points with low roughness are classified as planar features: Points with high roughness are classified as edge features: The complete feature set in the LiDAR frame at time i is represented as:

[0059]

[0060] To improve computational efficiency, a keyframe strategy is adopted: only frames with significant posture changes are selected as keyframes and added to the factor graph for optimization. The sliding window method selects the nearest n keyframes for motion estimation and maps them to the global coordinate system W, denoted as M i ={F i-n , …, F i}, the corresponding transformation matrix is ​​{T i-n , …, T i},M i Contains plane features: and edge features Two maps, in order to calculate the LiDAR frame F i+1 First, transform Transformed to the global coordinate system W, we get Use the transformation predicted by the IMU and then identify M i The corresponding plane or edge features in , the distances from point to line and point to plane are defined as follows:

[0061] Point to line distance:

[0062]

[0063] Distance from point to plane:

[0064]

[0065] Among them, k, u, v, w are the indices of feature points. For plane features For example, yes The corresponding points in ; and yes Corresponding to In The lidar sensor pose is estimated by minimizing these distances using the Gauss-Newton method and the estimated pose is Added as constraints to the factor graph of the lidar sensor odometry.

[0066] The UWB Factor:

[0067] The UWB sensor provides the distance information to the static anchor point. By estimating the position of the static anchor point, the global coordinates p of the static anchor point can be obtained. a , the UWB factor is mainly used to constrain the position estimation of the robot, and the formula is as follows:

[0068]

[0069] In the formula, Indicates the ranging information of the UWB sensor, p bk Represents the position information of the robot at time point k.

[0070] The present invention has the following beneficial effects:

[0071] 1. Improve the real-time and robustness of the system: This invention proposes an innovative multi-sensor deep fusion solution by adopting a tightly coupled fusion solution of LiDAR and Inertial Measurement Unit (IMU) sensors, combined with the precise global distance information provided by ultra-wideband (UWB) sensors. This fusion solution achieves rapid response and precise measurement of the environment by synchronizing and processing data from LiDAR, IMU and UWB in real time. Compared with existing loosely coupled solutions, tightly coupled fusion can more effectively utilize sensor data and reduce error propagation, thereby providing more accurate positioning information in dynamic and complex environments.

[0072] 2. Adapt to GNSS signal limited environment: The present invention is particularly suitable for environments where GNSS signals are limited or completely unable to receive GNSS signals, such as indoors, in urban canyons or tunnels. In these environments, the use of UWB sensors can provide accurate distance measurement, and after data fusion with LiDAR and IMU, high-precision positioning and navigation can be achieved even in the absence of GPS signals, ensuring the reliability and accuracy of the system in these challenging environments.

[0073] 3. Reduce the computational burden of the system: The present invention adopts a more efficient UWB information fusion method and only introduces the UWB factor in the key frame of the laser radar, which greatly reduces the occupation of computing resources; compared with the existing loop detection solution, the present invention simplifies the matching and verification process of historical data and avoids the consumption of computing resources by large-scale data. Therefore, the present invention improves the working efficiency and response speed of the system, thereby reducing the computational burden while maintaining high-precision positioning. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0075] Figure 1 It is a specific method flow chart of the present invention.

[0076] Figure 2 It is the factor graph optimization process of the present invention.

[0077] Figure 3 It is a comparison chart of the park data set trajectory of the present invention.

[0078] Figure 4 It is a comparison chart of the campus data set trajectory of the present invention. DETAILED DESCRIPTION

[0079] The embodiments of the present invention are further described below in conjunction with the accompanying drawings.

[0080] Embodiment 1:

[0081] The key workflow of the present invention is as follows: first, the system receives data from LiDAR, IMU and UWB sensors, and makes a preliminary prediction of the pose through the pre-integration method of the IMU; secondly, the pose is optimized using the plane and edge feature points in the LiDAR frame to correct the preliminary prediction results; finally, the UWB factor is inserted into the LiDAR key frame, and the global pose is optimized by combining the data of the IMU and LiDAR.

[0082] The specific method flow of the present invention is as follows Figure 1 shown. Figure 1 The data flow of each sensor and the optimization process of the factor graph are shown. The data of the lidar sensor, IMU and UWB sensor are fused and finally optimized in real time by the factor graph optimizer. Among them, the factor graph optimizer of the present invention, a preferred solution is to select the iSAM2 optimizer to utilize its incremental characteristics to efficiently process the newly added observation data.

[0083] Figure 2 The process of factor graph optimization is further explained. At the lidar key frame, the UWB factor is inserted, and then the multi-sensor fusion pose optimization is performed to update the corrected robot pose.

[0084] The main body of the mobile robot is equipped with an inertial measurement unit, a lidar and an ultra-wideband device, which respectively collect information such as the angle and acceleration of the mobile robot, the lidar point cloud information and the distance to the UWB anchor point, and transmit the collected information to the onboard computer through a wired connection for subsequent processing.

[0085] Step 1: Deploy UWB anchor points;

[0086] Deploy a static UWB anchor point in the environment to be positioned. The anchor point should be placed within the robot's line of sight and connected to a power source. Ensure that the UWB anchor point and the UWB tag carried by the mobile robot are at the same operating frequency and mode.

[0087] Step 2: Initialize the mobile robot;

[0088] Turn on the mobile robot device and control the mobile robot to move in the case of visible UWB anchor point. The mobile robot obtains IMU data and performs IMU pre-integration processing. The current position and posture are calculated through the IMU odometer, and the initial position and posture estimation are set.

[0089] Step 3: Front-end point cloud map processing;

[0090] During the robot's movement, the LiDAR point cloud data is acquired in real time through the laser radar carried by the robot and the point cloud is corrected. After filtering, denoising, distortion correction and other operations, feature points are extracted from the corrected point cloud, point cloud registration is performed based on the extracted feature points, and whether the current frame is a key frame is determined based on the feature points: if it is a key frame, the historical key frame data is updated. If not, the current point cloud map continues to be updated.

[0091] Step 4: UWB ranging information processing;

[0092] On the mobile robot platform, the IMU and UWB tag are installed on the same rigid object, so the translation offset between the UWB antenna and the IMU can be ignored. Therefore, the distance measurement between the UWB tag and the UWB anchor point directly reflects the distance change between the static anchor point and the robot. The onboard computer obtains the UWB ranging information through a wired connection, removes abnormal points, performs sliding window average filtering on the UWB data after the abnormal points are removed, and aligns the filtered UWB anchor point data with other data in time.

[0093] Step 5: Backend factor graph optimization;

[0094] The IMU, LiDAR, and UWB data are integrated and optimized using a factor graph optimization algorithm. The IMU factor, LiDAR factor, and UWB factor form the basic unit of the factor graph. The IMU factor uses data from the inertial measurement unit to estimate the robot's motion state and provide basic dynamic information for continuous posture estimation. The LiDAR factor uses data from 3D LiDAR for precise spatial positioning, helping the system build an environmental map and optimize the trajectory with high accuracy. The UWB factor mainly uses the distance measurement information provided by the UWB sensor to constrain the robot's position estimation relative to the static anchor point. Combining IMU, LiDAR, and UWB data, the final pose and point cloud map are output.

[0095] Implementation 2:

[0096] The present invention proposes a multi-sensor simultaneous localization and mapping (SLAM) system that integrates 3D laser radar (LiDAR), inertial measurement unit (IMU) and ultra-wideband (UWB) sensors, aiming to improve the positioning accuracy and trajectory estimation performance of mobile robots in complex environments. The present invention provides a robust and efficient positioning solution by combining the data of three sensors through a factor graph optimization method. (Incremental Sparse Smoothing and Mapping Algorithm) updates and optimizes the factor graph in real time. Compared with the traditional Bayesian network method, the incremental optimization based on the factor graph has the advantages of high computational efficiency and strong robustness, especially when processing long sequences and large-scale data, it can dynamically update historical trajectories and optimize new postures.

[0097] The system composition and main modules include IMU factor, LiDAR factor and UWB factor.

[0098] 1) IMU factor (IMU Preintegration Factor) is as follows:

[0099] By pre-integrating the IMU data between two LiDAR frames i and j, the following increments are obtained:

[0100] Velocity increment Δv ij :

[0101]

[0102] Displacement increment Δp ij :

[0103]

[0104] Rotation increment ΔR ij :

[0105]

[0106] The main optimization parameter is the bias of the IMU;

[0107] In the formula, The transpose of the rotation matrix at frame i represents the transformation from the world coordinate system to the frame i coordinate system;

[0108] v j : velocity vector at frame j;

[0109] v i : velocity vector at frame i;

[0110] g: gravitational acceleration vector, which is a fixed vector in the world coordinate system;

[0111] Δt ij : The time difference between frame i and frame j, indicating the integration time of IMU data;

[0112] p j : displacement vector at frame j;

[0113] p i : displacement vector at frame i;

[0114] Δp ij : represents the displacement increment between frame i and frame j, reflecting the position change of the device during this period;

[0115] The force of gravity on an object at time Δt ij Position changes caused by

[0116] R j : is the rotation matrix at frame j.

[0117] 2) LiDAR Factor:

[0118] LiDAR scans extract planar and edge features by evaluating the roughness of points within a local area; points with low roughness are classified as planar features: Points with high roughness are classified as edge features: The complete feature set in the LiDAR frame at time i is represented as:

[0119]

[0120] To improve computational efficiency, a keyframe strategy is adopted: only frames with significant posture changes are selected as keyframes and added to the factor graph for optimization. The sliding window method selects the nearest n keyframes for motion estimation and maps them to the global coordinate system W, denoted as M i ={F i-n , …, F i}, the corresponding transformation matrix is ​​{T i-n ,…,T i},M i Contains plane features: and edge features Two maps, in order to calculate the LiDAR frame F i+1 First, transform Transformed to the global coordinate system W, we get Use the transformation predicted by the IMU and then identify M i The corresponding plane or edge features in , the distances from point to line and point to plane are defined as follows:

[0121] Point to line distance:

[0122]

[0123] Distance from point to plane:

[0124]

[0125] Among them, k, u, v, w are the indices of feature points. For plane features For example, yes The corresponding points in ; and yes Corresponding to In The lidar sensor pose is estimated by minimizing these distances using the Gauss-Newton method and the estimated pose is Added as constraints to the factor graph of the lidar sensor odometry.

[0126] 3) The UWB Factor:

[0127] The UWB sensor provides the distance information to the static anchor point. By estimating the position of the static anchor point, the global coordinates p of the static anchor point can be obtained. a , the UWB factor is mainly used to constrain the position estimation of the robot, and the formula is as follows:

[0128]

[0129] In the formula, Indicates the ranging information of the UWB sensor, p bk Represents the position information of the robot at time point k.

[0130] The key points of the present invention are:

[0131] 1) Tightly coupled LiDAR-Inertial Odometry (LIO) fusion method based on a single UWB anchor: This paper proposes a fusion solution that tightly couples LiDAR and IMU sensors with a single UWB anchor. This method does not require multiple UWB anchors, significantly simplifies system deployment and cost, and improves positioning accuracy through tight coupling optimization. Fusion with a single UWB anchor instead of multiple anchors can effectively reduce the complexity of system deployment and improve system application scenarios.

[0132] 2) Factor graph optimization mechanism of introducing UWB factors into key frames: UWB factors are only introduced into key frames in the factor graph, which reduces the consumption of system computing resources. This mechanism significantly improves the work efficiency and real-time performance of the system while ensuring global optimization.

[0133] 3) Avoiding dependence on loop detection and improving real-time and robustness: The present invention uses UWB sensors to provide global positioning information, reduces dependence on loop detection, improves real-time and robustness in complex scenarios, and especially avoids the computational overhead of loop detection in large-scale data matching.

[0134] Embodiment 3:

[0135] The present invention has verified through experiments on relevant data sets that the system is more accurate in anchor point position estimation than the original scheme, and can provide a more accurate reference for subsequent trajectory correction. The experiment used the publicly available LIO-SAM data set and generated a simulated single anchor point data sequence based on the real value. A virtual static single anchor point is placed in the global map, and the distance measurement value of each time frame is the distance from the virtual anchor point to the real value plus Gaussian white noise N (0, 0.05). The park and campus (small) data set in the LIO-SAM data set provides a complete GPS trajectory, which is used as a real value reference in the experiment by the present invention. A virtual UWB anchor point is placed at the starting position of the robot, and the generated trajectory is as follows Figure 3 , Figure 4 And as shown in Table 1.

[0136] In order to evaluate the effectiveness of the algorithm of the present invention, the present invention compares it with LIO-SAM without loop closure and loop closure factor (denoted as LIO-SAM-Odo) and LIO-SAM with loop closure enabled (denoted as LIO-SAM-Loop). In the park dataset, the method of the present invention generates smoother trajectories, especially when turning, which are closer to the true value.

[0137] Table 1 Comparison of absolute translation trajectory errors

[0138] Dataset algorithm RMSE / m Mean / m Median / m park LIO-SAM-Odo 0.6492 0.5971 0.5131 LIO-SAM-Loop 0.6276 0.5751 0.4630 Ours 0.5652 0.5401 0.5236 campus LIO-SAM-Odo 0.7658 0.6120 0.6130 LIO-SAM-Loop 0.7597 0.6325 0.4965 Ours 0.7093 0.6100 0.4669

[0139] Advantages of the invention:

[0140] Compared with the method proposed in "Ange-focused fusion of camera-imu-uwbfor accurate and drift-reduced localization" by Thien Hoang Nguyen et al., the advantages of the present invention are as follows:

[0141] Sensor and computing efficiency advantages: The method of the present invention is based on LiDAR (laser radar) sensors instead of cameras. LiDAR can directly obtain depth information in three-dimensional space and is not affected by conditions such as lighting and environmental texture. Therefore, it has higher robustness in complex environments than camera solutions. At the same time, LiDAR can directly output accurate point cloud data, which has a smaller computing burden and a more efficient system compared to the image processing and feature extraction required by cameras.

[0142] Reduce computational burden and improve efficiency: Unlike the method of Nguyen et al., which uses high-frequency UWB distance data, the method of the present invention only introduces UWB factors at key frames. This strategy effectively reduces the amount of data processed by the system, avoids the computational burden of processing UWB data for each frame, and significantly improves the operating efficiency of the system, especially in large-scale scenarios, ensuring the real-time performance of the system.

[0143] This solution uses a single UWB-LiDAR-IMU tight coupling, and with the help of UWB distance information, adds a constraint to the system, improving the robustness and accuracy of the odometer. It can be applied to many scenarios with degraded perception, such as areas with strong reflectivity, heavy smoke, and single features. It can also be applied to robots or unmanned driving based on laser sensor perception.

Claims

1. A fusion positioning and mapping method using a single anchor point external source, characterized in that: The following steps are involved: Step 1: Deploy UWB anchor points; Ensure that mobile robots can obtain UWB information in real time; Step 2: Initialize the mobile robot; obtain an estimate of the current pose through the IMU on the mobile robot, and fuse it with the initial radar point cloud to obtain the initial pose; Step 3: Front-end point cloud map processing; After the radar point cloud is calibrated and feature extracted, it is determined whether it is a key frame. If it is, the historical key frame data is updated and point cloud pairing is performed; Step 4: UWB ranging information processing: performing outlier removal, sliding average filtering and data alignment processing on the received UWB ranging information; Step 5: key frame determination; When the robot displacement exceeds s or the rotation angle exceeds r, or the time interval exceeds T, the current frame is selected as the key frame to optimize the computational efficiency and the construction of the global map; Step 6: Back-end factor graph optimization: Use the factor graph optimization algorithm to integrate and optimize the IMU, LiDAR key frame and UWB key frame moment data, and output the point cloud map and pose results. This process combines IMU, LiDAR and UWB data, and outputs the final pose and point cloud map through the optimization algorithm.

2. According to claim 1, a fusion positioning and mapping method using a single anchor point external source is characterized in that: Step 1 specifically includes: Deploy a static UWB anchor point in the environment to be positioned. The anchor point should be placed within the robot's line of sight and connected to a power source. Ensure that the UWB anchor point and the UWB tag carried by the mobile robot are in the same operating frequency and mode.

3. According to claim 1, a fusion positioning and mapping method using a single anchor point external source is characterized in that: Step 2 specifically includes: turning on the mobile robot device, controlling the mobile robot to move when the UWB anchor point is visible, the mobile robot obtains IMU data and performs IMU pre-integration processing, calculates the current posture through the IMU odometer, and sets the initial position and posture estimation.

4. According to claim 1, a fusion positioning and mapping method using a single anchor point external source is characterized in that: Step 3 specifically includes: during the movement of the robot, the LiDAR point cloud data is acquired in real time through the laser radar carried by the robot and point cloud correction processing is performed; after filtering denoising and distortion correction operations, feature points are extracted from the corrected point cloud, point cloud registration is performed based on the extracted feature points, and whether the current frame is a key frame is determined based on the feature points: if it is a key frame, the historical key frame data is updated; if not, the current point cloud map continues to be updated.

5. According to claim 1, a fusion positioning and mapping method using a single anchor point external source is characterized in that: Step 4 specifically includes: on the mobile robot platform, the IMU and the UWB tag are installed on the same rigid object, so the translation offset between the UWB antenna and the IMU can be ignored. Therefore, the distance measurement between the UWB tag and the UWB anchor point directly reflects the distance change between the static anchor point and the robot. The onboard computer obtains the UWB ranging information through a wired connection, and performs anomaly removal processing, performs sliding window average filtering on the UWB data after the anomaly is removed, and aligns the filtered UWB anchor point data with other data in time.

6. According to claim 1, a fusion positioning and mapping method using a single anchor point external source is characterized in that: In step 5, the value of s is 0.5m, the value of r is 0.1 radian, and the value of T is 1s.

7. According to claim 1, a fusion positioning and mapping method using a single anchor point external source is characterized in that: Step 6 specifically includes: using a factor graph optimization algorithm to integrate and optimize the IMU, LiDAR and UWB data; the IMU factor, LiDAR factor and UWB factor constitute the basic unit of the factor graph; the IMU factor uses data from the inertial measurement unit to estimate the robot's motion state and provide basic dynamic information for continuous posture estimation; the LiDAR factor uses data from the 3D LiDAR for precise spatial positioning, helping the system to build an environmental map and optimize the trajectory with high precision; the UWB factor uses the distance measurement information provided by the UWB sensor to constrain the robot's position estimation relative to the static anchor point; combining the IMU, LiDAR and UWB data, outputting the final pose and point cloud map.

8. According to claim 7, a fusion positioning and mapping method using a single anchor point external source is characterized in that: The IMU factors are specifically: By pre-integrating the IMU data between two LiDAR frames i and j, the following increments are obtained: Velocity increment Δv oj : Displacement increment Δp ij : Rotation increment ΔR ij : The main optimization parameter is the bias of the IMU; In the formula, The transpose of the rotation matrix at frame i represents the transformation from the world coordinate system to the frame i coordinate system; v j : velocity vector at frame j; v i : velocity vector at frame i; g: gravitational acceleration vector, which is a fixed vector in the world coordinate system; Δt ij : The time difference between frame i and frame j, indicating the integration time of IMU data; p j : displacement vector at frame j; p i : displacement vector at frame i; Δp ij : represents the displacement increment between frame i and frame j, reflecting the position change of the device during this period; The force of gravity on an object at time Δt ij Position changes caused by R j : is the rotation matrix at frame j.

9. According to claim 7, a fusion positioning and mapping method using a single anchor point external source is characterized in that: The LiDAR Factor: LiDAR scans extract planar and edge features by evaluating the roughness of points within a local area; points with low roughness are classified as planar features: Points with high roughness are classified as edge features: The complete feature set in the LiDAR frame at time i is represented as: To improve computational efficiency, a keyframe strategy is adopted: only frames with significant posture changes are selected as keyframes and added to the factor graph for optimization. The sliding window method selects the nearest n keyframes for motion estimation and maps them to the global coordinate system W, denoted as M i ={F i-n , …, F i }, the corresponding transformation matrix is ​​{T i-n ,…,T i },M i Contains plane features: and edge features Two maps, in order to calculate the LiDAR frame F i+1 First, transform Transformed to the global coordinate system W, we get Use the transformation predicted by the IMU and then identify M i The corresponding plane or edge features in , the distances from point to line and point to plane are defined as follows: Point to line distance: Distance from point to plane: Among them, k, u, v, w are the indices of feature points. For plane features For example, yes The corresponding points in ; and yes Corresponding to In The lidar sensor pose is estimated by minimizing these distances using the Gauss-Newton method and the estimated pose is Added as constraints to the factor graph of the lidar sensor odometry.

10. The method for fusion positioning and mapping using a single anchor point external source according to claim 7, characterized in that: The UWB Factor: The UWB sensor provides the distance information to the static anchor point. By estimating the position of the static anchor point, the global coordinates p of the static anchor point can be obtained. a , the UWB factor is mainly used to constrain the position estimation of the robot, and the formula is as follows: In the formula, Indicates the ranging information of the UWB sensor, p bk Represents the position information of the robot at time point k.

Images