Factor graph-based LIDAR / UWB / INS tight coupling indoor positioning method

Abstract

According to the LIDAR / UWB / INS tight coupling indoor positioning method based on the factor graph, firstly, through a ground point extraction technology based on a lightweight point cloud segmentation method and in combination with ground height prior information provided by an ultra wide band, dynamic correction of a ground point cloud is realized, and the robustness of a ground model is significantly improved. Meanwhile, non-line-of-sight errors in UWB ranging data are effectively eliminated through a multi-information fusion decision model based on information entropy. In the positioning optimization, the high-precision local information of the laser point cloud, the global constraint information of the UWB and the front and back correlation constraint information of the inertial measurement unit are fused to construct an elastic factor graph model, so that accurate and reliable position estimation is realized, and the positioning precision and robustness of the system in a complex environment are effectively improved. The method supports continuous positioning in a satellite rejection environment, has the characteristics of high robustness, low calculation cost and centimeter-level precision, and is particularly suitable for navigation and positioning of a quadruped robot in a challenging scene.

Description

Technical Field

[0001] The invention belongs to the technical field of indoor positioning, in particular to a LIDAR / UWB / INS tightly coupled indoor positioning method based on a factor graph. Background Art

[0002] In the past three decades, mobile robots have attracted much attention due to their wide applications in complex environments, industrial manufacturing, rescue operations and other fields. According to the different modes of movement, mobile robots can be divided into wheeled, tracked and legged robots. Among them, wheeled and tracked robot technologies are relatively mature, but their performance in unstructured terrain is limited. In contrast, legged robots are inspired by the movement patterns of humans and animals and have the ability to move freely in complex terrains. In particular, quadruped robots have become a research hotspot due to their stability and flexibility.

[0003] At present, the fusion technology of the global navigation satellite system (GNSS) and the inertial navigation system (INS) can meet some positioning needs, but its continuity and reliability cannot be guaranteed in an environment where satellite signals are blocked. For this reason, the simultaneous localization and mapping (SLAM) method based on LiDAR has become an important alternative. LiDAR is widely used in robot positioning due to its characteristics of being unaffected by light and high-precision measurement. However, due to the low vertical resolution of LiDAR, the elevation error accumulates over time and becomes a major bottleneck. In order to enhance the elevation constraint of the system, the study uses a ground segmentation algorithm to extract ground point cloud information, thereby optimizing the positioning accuracy.

[0004] On the other hand, it is difficult to meet actual needs by relying solely on the relative positioning of LiDAR, especially in scenarios that require absolute positioning, such as fire rescue. To solve this problem, some studies have begun to integrate indoor positioning technologies, such as Bluetooth, Wi-Fi, and ultra-wideband (UWB). Among them, UWB technology has become the first choice for high-precision indoor positioning due to its centimeter-level positioning accuracy, strong anti-multipath effect, and low power consumption. However, UWB positioning still faces the challenge of non-line-of-sight (NLOS) environment, which will lead to a significant increase in positioning error. Existing studies have attempted to identify and correct NLOS errors through signal path loss models or environmental context information, but there are problems such as difficulty in data acquisition or strong model dependence. In addition, the existing fusion methods have insufficient processing capabilities for abnormalities of each sensor and cannot provide effective compensation when single sensor data fails. Therefore, how to combine lightweight LiDAR ground segmentation algorithms and high-precision UWB positioning technology, while effectively dealing with NLOS problems in complex environments, is a technical problem that needs to be solved in the field of autonomous positioning of mobile robots.

[0005] Publication (Announcement) No. CN 114674311 B discloses an indoor positioning and mapping method and system, which combines UWB, laser, and inertia to eliminate cumulative errors for positioning. However, this method may be affected by environmental factors such as lidar performance and UWB data quality, resulting in fluctuations in positioning accuracy and stability in some cases.

[0006] Publication (Announcement) No. CN 115598634 A discloses a mobile robot positioning method based on the information fusion of ultra-wideband radar and lidar. This method obtains the original distance data from the ultra-wideband radar and the original point cloud data from the lidar respectively, performs the judgment and fusion of the degradation degree of information, and finally realizes pose estimation. However, this method uses a non-line-of-sight error elimination neural network model to eliminate the non-line-of-sight error of the ultra-wideband, which requires a large amount of training data and a complex operation process, and is not suitable for lightweight positioning systems. Summary of the Invention

[0007] In view of the above problems, the present invention proposes a tightly coupled indoor positioning method of LIDAR / UWB / INS based on factor graph, which solves the problems of large ranging errors, unstable ground point extraction, obvious influence of non-line-of-sight environment, and insufficient multi-sensor data fusion that easily occur in existing positioning systems in complex environments. The specific steps are as follows:

[0008] To achieve the above object, the technical solution adopted by the present invention is:

[0009] The tightly coupled indoor positioning method of LIDAR / UWB / INS based on factor graph includes the following steps:

[0010] Step 1, perform point cloud segmentation on the laser point cloud data based on a lightweight point cloud segmentation method, and use the ground height prior information provided by UWB to dynamically correct the extraction of ground points in the laser point cloud, enhancing the robustness of the ground model;

[0011] Step 2, design a non-line-of-sight (NLOS) identification based on the decision of information entropy, and eliminate the NLOS error in the ultra-wideband (UWB) ranging data through multi-information fusion decision-making;

[0012] Step 3, fuse the high-precision local information of the laser point cloud, the global constraint information of UWB, and the front-back correlation constraint information of the inertial measurement unit (IMU), and jointly construct a reliable position estimate of the elastic factor graph model with multiple constraints.

[0013] As a further improvement of the present invention, the specific content of Step 1 is as follows:

[0014] (1.1) Filter and preprocess the collected laser point cloud data containing the target environment, and calculate the height h between the current lidar and the ground:

[0015] h=s*cosθ

[0016] Among them, s is the distance from the laser point to the laser radar, θ is the laser angle, and according to the calculated h value and the initial height h 0 The difference of |hh 0 |Judge ground points and non-ground points;

[0017] (1.2) Mark the location of the laser radar as the center of the circle and process the point cloud data P filtered According to certain rules, the sector is divided into several small sectors. The sectors are represented by a unified polar coordinate grid. The sector S is divided into multiple small areas with regular intervals in radial and azimuthal directions, namely rings and sectors. Then, in each point cloud set G = {g i |i=1,2,...,k} randomly select three points for plane fitting, and the fitting equation is:

[0018]

[0019] Where n is the normal vector of the plane, and d is the intercept of the plane equation;

[0020] (1.3) Based on the preliminary ground plane model and the robot's position P, calculate the robot's height h relative to the ground initial , the ground height is dynamically corrected using UWB ranging data, and the parameters of the ground plane model are readjusted to construct the distance from the minimization point to the fitting plane as the cost function of the plane fitting. The cost function is: The plane with the minimum cost function after multiple iterations is taken as the final fitted ground plane.

[0021] As a further improvement of the present invention, the step 2 is specifically as follows:

[0022] (2.1) For the i-th UWB ranging value, calculate the difference between the total received signal strength rx and the first path signal strength fp at the current time k: Δr i =rx i -fp i , calculate the UWB tag measurement value m i and laser / IMU odometry estimates o i Difference size Λd i :Δd i =m i -o i ;

[0023] (2.2) According to the above error value, the probability estimation is based on the normal distribution assumption:

[0024]

[0025] where Δ i is the difference of the i-th base station under each sub-decision criterion, μ is the mean under the current decision criterion, σ is the standard deviation, and (2.3) For each sub-decision criterion, calculate its information entropy: H(Δ i ) = -P(Δ i ) log(P(Δ i ))). Then, the exponential weighted moving average method is introduced to update the current information entropy:

[0026] H k (Δ i ) = α · H k (Δ i ) + (1 - α) · H k-1 (Δ i )

[0027] where α is the smoothing coefficient;

[0028] (2.4) At each time point, sum the updated signal strength error entropy and ranging signal residual entropy to calculate the joint information entropy: H k (Δr i , Δd i ) = H k (Δr i ) + H k (Δd i ). Finally, perform NLOS judgment based on the joint information entropy.

[0029] As a further improvement of the present invention, step 3 is specifically as follows:

[0030] (3.1) Based on the point cloud edge feature point set and the surface feature point set and the previous frame edge feature point set and the surface feature point set perform feature matching on the feature points to obtain the laser point cloud odometer residual Based on the IMU pre-integration to obtain the pre-integration residual

[0031] (3.2) Based on the laser ground point cloud segmentation result, construct the ground constraint residual Based on the UWB observation signal, construct the ranging residual r Range (Z k , X). In addition, if the number of UWB ranging in LOS reaches the threshold, then obtain the global position based on the least squares and construct the position constraint residual r Position (Z, X);

[0032] (3.3) Construct the corresponding factor nodes, construct the objective function of the least squares problem, and obtain the state value to be estimated by solving the nonlinear least squares to minimize the error function of each factor:

[0033]

[0034] Among them, the five residuals represent the odometry constraint of the laser point cloud information, the ground constraint, the IMU pre-integration constraint within the inter-frame time period, the ranging observation constraint provided by UWB, and the position constraint factor.

[0035] The advantages of the present invention compared with the prior art are as follows:

[0036] Aiming at the problems of large ranging errors, unstable ground point extraction, obvious influence of non-line-of-sight environments, and insufficient multi-sensor data fusion that easily occur in existing positioning systems in complex environments, the present invention proposes a tightly coupled indoor positioning system based on a factor graph for LIDAR / UWB / INS. Through the ground point extraction technology based on a lightweight point cloud segmentation method and combined with the prior ground height information provided by UWB, the dynamic correction of the ground point cloud is realized, and the robustness of the ground model is significantly improved. At the same time, through the multi-information fusion decision-making model based on information entropy, the NLOS errors in the UWB ranging data are effectively eliminated. In positioning optimization, the high-precision local information of the laser point cloud, the global constraint information of UWB, and the front and back correlation constraint information of IMU are fused to construct an elastic factor graph model, realizing accurate and reliable position estimation, and effectively improving the positioning accuracy and robustness of the system in complex environments. Compared with the prior art, the present invention has the advantages of high computational efficiency, flexible deployment, and strong environmental adaptability. Description of the Drawings

[0037] Figure 1 is the flow chart of the method provided by the present invention;

[0038] Figure 2 is the schematic diagram of excluding ground point clouds based on triangles;

[0039] Figure 3 is the schematic diagram of the fusion positioning factor graph model. Detailed Embodiments

[0040] The following will clearly and completely describe the technical solutions in the embodiments of the present invention with reference to the accompanying drawings in the embodiments of the present invention. The following embodiments are used to illustrate the present invention but are not used to limit the scope of the present invention.

[0041] As Figure 1As shown in the figure, the present invention provides a tightly coupled indoor positioning system based on factor graph for LIDAR / UWB / INS, which solves problems such as large ranging errors, unstable ground point extraction, obvious influence of non-line-of-sight environment, and insufficient multi-sensor data fusion that are prone to occur in existing positioning systems in complex environments.

[0042] As a specific embodiment of the present invention, the technical solution adopted by the present invention is as follows:

[0043] Step S1: Perform point cloud segmentation on the lidar point cloud data based on a lightweight point cloud segmentation method, and use the prior ground height information provided by UWB to dynamically correct the extraction of ground points in the lidar point cloud, enhancing the robustness of the ground model. Specifically, it includes:

[0044] S1.1: Filter and preprocess the collected lidar point cloud data containing the target environment. Since lidar point cloud data usually comes with noise and clutter points. Use regional constraint conditions to remove clutter points in non-active areas according to the actual region of interest (such as within 20 meters of the robot's activity range), thereby filtering out irrelevant point cloud data. Then, apply the voxel filtering method to downsample the point cloud to reduce the data volume and improve the subsequent processing speed. Calculate the height h of the current lidar from the ground:

[0045] h = s * cosθ

[0046] where s is the length from the lidar point to the lidar, and θ is the vertical angle of the lidar beam. According to the calculated difference |h - h 0 | between the lidar height h value and the initial height h 0 |, judge the ground points and non-ground points, as Figure 2 shown;

[0047] S1.2: Mark the position of the lidar as the center of the circle, and use a unified polar coordinate grid to represent the sector, and divide the processed point cloud data P filtered into several small fan-shaped areas according to certain rules. To ensure that the number of point clouds in each area is the same, divide the sector S into multiple small areas, namely rings and sectors, at regular intervals in the radial and azimuth angles. Multiple small areas, namely rings and sectors. Then randomly select three points in each point cloud set G = {g i |i = 1, 2,..., k} for plane fitting, and the fitting equation is:

[0048]

[0049] where n is the normal vector of the plane, and d is the intercept of the plane equation;

[0050] S1.3: According to the preliminary ground plane model and the position P of the robot, calculate the height h of the robot relative to the ground initial, the ground height is dynamically corrected using UWB ranging data, and the parameters of the ground plane model are readjusted. A cost function for plane fitting is constructed by minimizing the distance from points to the fitted plane, and the cost function is: The plane when the cost function is minimized through multiple iterations is the finally fitted ground plane.

[0051] Step S2: Design NLOS identification based on information entropy decision-making to eliminate NLOS errors in UWB ranging data through multi-information fusion decision-making. Specifically, it includes:

[0052] S2.1: For the i-th UWB ranging value, calculate the difference between the total received signal strength rx and the first-path signal strength fp at the current moment k: Δr i = rx i - fp i . Calculate the difference Λd i between the UWB tag measurement value m i and the laser / IMU odometer estimate value o i : Δd i = m i - o i ;

[0053] S2.2: Based on the above error values, perform probability estimation under the assumption of normal distribution:

[0054]

[0055] where Δ i is the difference of the i-th base station under each sub-decision criterion, μ is the mean under the current decision criterion, and σ is the standard deviation;

[0056] S2.3: For each sub-decision criterion, calculate its information entropy: H(Δ i ) = -P(Δ i )log(P(Δ i )). Then, introduce the exponential weighted moving average method to update the current information entropy:

[0057] H k (Δ i ) = α·H k (Δ i )+(1-α)·H k-1 (Δ i )

[0058] where α is the smoothing coefficient;

[0059] S2.4: At each time point, sum the updated signal strength error entropy and ranging signal residual entropy to calculate the joint information entropy: H k (Δr i ,Δdi ) = H k (Δr i ) + H k (Δd i ). Finally, NLOS judgment is performed according to the joint information entropy.

[0060] Step S3: Integrate the high-precision local information of the laser point cloud, the global constraint information of UWB, and the front and rear correlation constraint information of IMU, and jointly construct a reliable position estimate of the elastic factor graph model, as Figure 3 shown. Specifically, it includes:

[0061] S3.1: Based on the set of edge feature points and the set of surface feature points and the set of edge feature points of the previous frame and the set of surface feature points feature points for feature matching to obtain the laser point cloud odometry residual Based on IMU pre-integration to obtain the pre-integration residual

[0062] S3.2: Based on the laser ground point cloud segmentation result, construct the ground constraint residual Based on the UWB observation signal, construct the ranging residual r Range (Z k , X). In addition, if the number of UWB ranging in LOS reaches the threshold, the global position is obtained based on the least squares, and the position constraint residual r Position (Z, X) is constructed. In order to prevent the existence of uneven ground or even small slopes in large indoor places such as underground parking lots and factories, in this case, if ground constraints are blindly added, it may instead add incorrect information and be counterproductive. Therefore, it is necessary to continuously fit the angle θ between the ground plane normal vectors between key frames. If it exceeds the given threshold range, according to the actual situation, the slope is defined as θ > 8°. By continuously calculating the angle between the ground normal vectors between key frames, if it is greater than the threshold, it is considered that the robot is not moving on a horizontal ground, that is, no ground constraint will be added in the pose optimization at the back end. On the contrary, if the robot is moving on a horizontal ground, the corresponding ground constraint is added to improve the accuracy and robustness of the pose estimation;

[0063] S3.3: Construct the corresponding factor nodes, construct the objective function of the least squares problem, and find the state value to be estimated by solving the nonlinear least squares to minimize the error function of each factor:

[0064]

[0065] Among them, the five residuals respectively represent the odometry constraint of Lidar point cloud information, the ground constraint, the IMU pre-integration constraint within the inter-frame time period, the ranging observation constraint provided by UWB, and the position constraint factor.

[0066] The above are only the preferred embodiments of the present invention, and do not constitute any other form of limitation to the present invention. Any modification or equivalent change made according to the technical essence of the present invention still falls within the scope of protection required by the present invention.

Claims

1. A LIDAR / UWB / INS tightly coupled indoor positioning method based on factor graph, characterized by: The following steps are involved: Step 1: perform point cloud segmentation based on the lightweight point cloud segmentation method for the laser point cloud data, use the ground height prior information provided by UWB to dynamically correct the ground point extraction in the laser point cloud, and enhance the robustness of the ground model; Step 2: Design a non-line-of-sight NLOS recognition method based on information entropy decision-making, and eliminate the NLOS error in ultra-wideband UWB ranging data through multi-information fusion decision-making; Step 3: Fuse the local information of the laser point cloud, the global constraint information of the UWB, and the contextual constraint information of the inertial measurement unit (IMU), and combine multiple constraints to construct an elastic factor graph model for reliable position estimation.

2. The LIDAR / UWB / INS tightly coupled indoor positioning method based on factor graph according to claim 1, characterized in that: The step 1 is specifically as follows: (1.1) Filter and preprocess the collected laser point cloud data containing the target environment, and calculate the height h between the current laser radar and the ground: h=s*cosθ Where s is the length from the laser point to the laser radar, θ is the laser angle, and the ground point and non-ground point are judged based on the calculated h value and the difference between the initial height h0 |h-h0|; (1.2) Mark the location of the laser radar as the center of the circle and process the point cloud data P filtered According to certain rules, the sector is divided into several small sectors. The sectors are represented by a unified polar coordinate grid. The sector S is divided into multiple small areas with regular intervals in radial and azimuthal directions, namely rings and sectors. Then, in each point cloud set G = {g i |i=1,2,...,k} randomly select three points for plane fitting, and the fitting equation is: Where n is the normal vector of the plane, and d is the intercept of the plane equation; (1.3) Based on the preliminary ground plane model and the robot's position P, calculate the robot's height h relative to the ground initial , the ground height is dynamically corrected using UWB ranging data, and the parameters of the ground plane model are readjusted to construct the distance from the minimization point to the fitting plane as the cost function of the plane fitting. The cost function is: The plane with the minimum cost function after multiple iterations is taken as the final fitted ground plane.

3. The LIDAR / UWB / INS tightly coupled indoor positioning method based on factor graph according to claim 1, characterized in that: The step 2 is specifically as follows: (2.1) For the i-th UWB ranging value, calculate the difference between the total received signal strength rx and the first path signal strength fp at the current time k: Δr i =rx i -fp i , calculate the UWB tag measurement value m i and laser / IMU odometry estimates o i Difference size Λd i :Δd i =m i -o i ; (2.2) According to the above error value, the probability estimation is based on the normal distribution assumption: Where Δ i is the difference of the ith base station under each sub-decision criterion, μ is the mean under the current decision criterion, σ is the standard deviation, (2.3) For each sub-decision criterion, calculate its information entropy: H (Δ i )=-P(Δ i )log(P(Δ i )), then, the exponentially weighted moving average method is introduced to update the current information entropy: H k (D i )=α·H k (D i )+(1-α)·H k-1 (D i ) Where α is the smoothing coefficient; (2.4) At each time point, the updated signal strength error entropy and ranging signal residual entropy are added to calculate the joint information entropy: H k (Δr i ,Δd i )=H k (Δr i )+H k (Δd i ), finally, NLOS judgment is performed based on the joint information entropy.

4. The LIDAR / UWB / INS tightly coupled indoor positioning method based on factor graph according to claim 1, characterized in that: The step 3 is as follows: (3.1) Based on the point cloud edge feature point set And surface feature point set And the edge feature point set of the previous frame And surface feature point set Feature matching of feature points to obtain laser point cloud odometry residual Pre-integration residual obtained based on IMU pre-integration (3.2) Constructing ground constraint residual based on laser ground point cloud segmentation results Constructing ranging residual r based on UWB observation signal Range (Z k ,X),In addition, if the number of UWB ranging in LOS reaches the threshold, the global position is obtained based on the least squares method, and the position constraint residual r is constructed Position (Z,X); (3.3) Construct the corresponding factor nodes, construct the least squares problem objective function, and solve the error function of each factor by solving the nonlinear least squares minimization to obtain the estimated state value: Among them, the five residuals represent the odometer constraint of the laser point cloud information, the ground constraint, the IMU pre-integration constraint in the inter-frame time period, the ranging observation constraint provided by UWB, and the position constraint factor.

Images