A real-time fusion and mapping method of SLAM and UWB

Abstract

The present invention discloses a real-time fusion and mapping method of SLAM and UWB, including: step 1, in the process of movement of an intelligent body carrying a laser radar, according to a distributed algorithm, obtaining the positioning information of the intelligent body and each ultra-wideband node at the current moment; step 2, based on an extended Kalman filter algorithm, correcting the positioning information at the current moment; step 3, based on a Gauss-Newton method, obtaining the optimal posture of the intelligent body at the current moment, using the optimal posture to correct the posture estimation of the intelligent body at the current moment and the position of each ultra-wideband node, and updating the laser radar map and the UWB map at the same time; the posture estimation of the intelligent body at the current moment output by step 3 and the position of each ultra-wideband node are used as inputs of step 2. The present invention fuses the LiDAR sensor with the UWB sensor, fuses UWB and LiDAR to improve the accuracy and robustness of positioning and mapping based on UWB, and provides detailed map information of ultra-wideband node coordinates and surrounding environment.

Description

Technical Field

[0001] The present invention relates to the technical field of robot positioning and navigation, and particularly to a method for real-time fusion and mapping of SLAM and UWB. Background Art

[0002] SLAM (Simultaneous Localization and Mapping) has been widely applied in the field of mobile robots. Since a radar (LiDAR) can accurately measure the distances to nearby objects, many methods use the radar. However, the performance of SLAM is very vulnerable to cumulative errors.

[0003] In LiDAR-based SLAM, a method of fusing UWB sensors can be used to eliminate cumulative errors and enhance robustness. However, the distance resolution of UWB is lower than that of LiDAR (the laser ranging error is about 1 cm, which is about one-tenth of the UWB ranging error). The fusion effect of directly constructing a LiDAR map based on the UWB positioning results is not ideal. Summary of the Invention

[0004] In view of this, the present invention provides a method for real-time fusion and mapping of SLAM and UWB, which combines a LiDAR sensor and a UWB (Ultra Wide Band) sensor, fuses UWB and LiDAR to improve the accuracy and robustness of UWB-based positioning and mapping, and provides anchor point coordinates and detailed map information of the surrounding environment.

[0005] The present invention discloses a method for real-time fusion and mapping of SLAM and UWB, including:

[0006] Step 1: During the movement of an intelligent agent carrying a lidar, according to a distributed algorithm, obtain the positioning information of the intelligent agent and each ultra-wideband node at the current moment;

[0007] Step 2: Based on the extended Kalman filter algorithm, correct the positioning information at the current moment;

[0008] Step 3: Based on the Gauss-Newton method, obtain the optimal pose of the intelligent agent at the current moment, use the optimal pose to correct the pose estimation of the intelligent agent at the current moment and the positions of each ultra-wideband node, and simultaneously update the lidar map and the UWB map; use the pose estimation of the intelligent agent at the current moment and the positions of each ultra-wideband node output by Step 3 as the input of Step 2;

[0009] Step 4: When the intelligent agent moves to the next moment, set the next moment equal to the current moment, and repeat Steps 1 to 3 until the intelligent agent completes a preset task.

[0010] Further, step 1 includes:

[0011] After the agent enters the unknown area, it throws ultra-wideband nodes with known positions and ultra-wideband nodes with unknown positions, establishes communication with each ultra-wideband node, and measures the distances between them; the agent moves continuously in the area and collects lidar feedback data at each position.

[0012] Further, during the movement of the agent in step 1, when the preset condition is not met, it is necessary to continue to throw ultra-wideband nodes around it; the preset condition is that the number of ultra-wideband nodes within the measurable range around the agent is greater than 3.

[0013] Further, step 2 includes:

[0014] Construct a dynamic observation model based on the positioning information of the agent and each ultra-wideband node output by step 1;

[0015] Use the extended Kalman filter algorithm to update the dynamic observation model to obtain an updated state estimate and an updated covariance estimate, that is, complete the correction of the positioning information.

[0016] Further, the dynamic observation model includes state noise; the positioning information includes pose and velocity.

[0017] Further, use the scan matching process based on the Gauss-Newton method to match the beam endpoints observed at the current moment with the lidar map at the current moment to find the best transformation for better matching.

[0018] Further, the scan matching process is carried out in a multi-resolution grid, mapping from a low-resolution grid to a high-resolution grid, so that it is more likely to find a global solution instead of being trapped in a local solution.

[0019] Further, when establishing the lidar map, through transformation and rotation, set an ultra-wideband node fixed at the origin, and set another ultra-wideband node always located on the positive x-axis; the speeds of the two set ultra-wideband nodes are zero; construct and update the lidar map under the local frame established by the two set ultra-wideband nodes.

[0020] Due to the adoption of the above technical solutions, the present invention has the following advantages: integrating the LiDAR sensor with the UWB (UltraWide Band) sensor, fusing UWB and LiDAR to improve the accuracy and robustness of UWB-based positioning and mapping, and providing anchor point coordinates and detailed map information of the surrounding environment. BRIEF DESCRIPTION OF THE DRAWINGS

[0021] To more clearly illustrate the technical solutions in the embodiments of the present invention, the following briefly introduces the accompanying drawings required for the description of the embodiments. Obviously, the accompanying drawings in the following description are only some embodiments described in the embodiments of the present invention. For those of ordinary skill in the art, other accompanying drawings can also be obtained based on these drawings.

[0022] Figure 1 It is a schematic flow chart of a real-time fusion and mapping method of SLAM and UWB according to an embodiment of the present invention;

[0023] Figure 2 It is a schematic diagram of the UWB node positioning coordinate system according to an embodiment of the present invention;

[0024] Figure 3 It is a node topology network structure diagram according to an embodiment of the present invention;

[0025] Figure 4 It is a schematic diagram of the simulation environment according to an embodiment of the present invention;

[0026] Figure 5 It is a schematic diagram of the mapping process according to an embodiment of the present invention;

[0027] Figure 6 It is a schematic diagram of the mapping effect according to an embodiment of the present invention;

[0028] Figure 7 It is the mapping map output diagram according to an embodiment of the present invention. Detailed implementation manners

[0029] The present invention will be further described in conjunction with the accompanying drawings and embodiments. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all of the embodiments. All other embodiments obtained by those of ordinary skill in the art shall fall within the scope of protection of the embodiments of the present invention.

[0030] Embodiment 1:

[0031] See Figure 1 , the present invention provides an embodiment of a real-time fusion and mapping method of SLAM and UWB, including:

[0032] S1. During the movement of the intelligent agent carrying the lidar, according to the distributed algorithm, obtain the positioning information of the intelligent agent and each ultra-wideband node at the current moment;

[0033] S2. Based on the extended Kalman filter algorithm, correct the positioning information at the current moment;

[0034] S3. Obtain the optimal pose of the agent at the current moment based on the Gauss-Newton method, correct the pose estimation of the agent at the current moment and the positions of each ultra-wideband node using the optimal pose, and simultaneously update the lidar map and the UWB map; use the pose estimation of the agent at the current moment and the positions of each ultra-wideband node output by S3 as the input of S2.

[0035] S4. The agent moves to the next moment, set the next moment equal to the current moment, and repeat S1 to S3 until the agent completes the preset task.

[0036] In this embodiment, S1 includes:

[0037] After the agent enters the unknown area, throw ultra-wideband nodes with known positions and ultra-wideband nodes with unknown positions, establish communication with each ultra-wideband node, and measure the distances between each other; the agent moves continuously in the area and collects lidar feedback data at each position.

[0038] In this embodiment, during the movement of the agent in S1, when the preset condition is not met, it is necessary to continue to throw ultra-wideband nodes around it; the preset condition is that the number of ultra-wideband nodes within the measurable range around the agent is greater than 3.

[0039] In this embodiment, S2 includes:

[0040] Construct a dynamic observation model according to the positioning information of the agent and each ultra-wideband node output by S1;

[0041] Use the extended Kalman filter algorithm to update the dynamic observation model to obtain the updated state estimation and the updated covariance estimation, that is, complete the correction of the positioning information.

[0042] In this embodiment, the dynamic observation model includes state noise; the positioning information includes pose and velocity.

[0043] In this embodiment, use the scan matching process based on the Gauss-Newton method to match the beam endpoints observed at the current moment with the lidar map at the current moment to find the best transformation for better matching.

[0044] In this embodiment, the scan matching process is carried out in a multi-resolution grid, mapping from a low-resolution grid to a high-resolution grid, so as to be more likely to find the global solution rather than being trapped in the local solution.

[0045] In this embodiment, when establishing the lidar map, through transformation and rotation, set an ultra-wideband node fixed at the origin, and set another ultra-wideband node always located on the positive x-axis; the velocities of the two set ultra-wideband nodes are zero; construct and update the lidar map under the local frame established by the two set ultra-wideband nodes.

[0046] NLOS (Non Line of Sight) propagation has a great impact on high-resolution positioning systems because it introduces non-negligible biases in distance measurements, thus reducing the positioning accuracy. If the power difference (i.e., the received power minus the power of the first path) is greater than 10 dB, the channel is very likely to be non-line of sight (NLOS), and then the ranging under NLOS can be ignored in the EKF update.

[0047] Due to the accuracy difference between UWB ranging and LiDAR ranging, it is not possible to directly construct a LiDAR map based on the UWB positioning, which prompts to match the scan endpoints by optimizing the pose and orientation estimation of the robot. HectorSLAM uses a scan matching process based on the Gauss-Newton method to match the beam endpoints observed at time t with the latest LiDAR map to find the best transformation for better matching. The scan matching process is carried out in a multi-resolution grid, mapping from a low-resolution grid to a high-resolution grid mapping, making it more likely to find the global solution rather than being trapped in a local solution. The UWB ranging based on EKF (Extended Kalman Filter) can obtain the pose and velocity estimation of the robot and the anchors, and is used for the initialization of the scan matching process.

[0048] Example Two:

[0049] This example takes 9 ultra-wideband nodes as an example, which includes 4 ultra-wideband nodes with known positions and 5 ultra-wideband nodes with unknown positions, to elaborate the method of this example.

[0050] In an area where humans cannot enter, the GPS signal is weak, and there is no other reference signal, it is necessary to dispatch agents to conduct area detection and establish a local map. The agents all carry ultra-wideband modules and have no prior information on positioning. After the agents enter the unknown area, they throw the initial UWB positioning nodes and establish communication with each node to measure the distances between them. The agents move continuously in the area and collect radar feedback data at various positions. In the area far from the initial position, the UWB positioning nodes can be thrown again for cooperative positioning. Based on the extended Kalman filter method, the UWB fusion information is filtered to obtain the information such as the position, velocity, and orientation of the current agent. Taking this position as the prior information, the Gauss-Newton method is used to match the radar data to obtain the accurate information of the current position and establish a map.

[0051] In this example, the provided real-time fusion and mapping method based on SLAM and UWB is applied. Through the distributed positioning algorithm, the positioning of unknown UWB nodes in the three-dimensional space is achieved. At the same time, the motion information of the agent will deviate under the influence of noise, while the positioning result of UWB can correct the position information in time, reducing the probability of the matching algorithm converging to the local optimal solution. The following uses a specific example to implement and verify its accuracy. The experimental environment is as Figure 4 shown.

[0052] Step 1: Distributed positioning.

[0053] Overall framework of the distributed positioning system: Multiple UWB modules use DR distributed ranging and communicate with the serial port of the computing device carried by the agent; the computing device establishes wireless communication with the host computer. Assume that the topological structure of the network nodes is as Figure 3 shown. Node p 1 , p 2 , p 3 , p 4 are anchor points with known positions, and the rest are ordinary nodes in the network with unknown coordinates. For any point p 1 , p 2 , p 3 , p 4 in the convex tetrahedron composed of p 0 , there exist barycentric coordinates w 01 , w 02 , w 03 , w 04 such that

[0054] w 01 + w 02 + w 03 + w 04 = 1

[0055] p 0 = w 01 * p 1 + w 02 * p 2 + w 03 * p 3 + w 04 * p 4

[0056] Then:

[0057]

[0058]

[0059] All nodes to be positioned can be expressed as the sum of the products of the barycentric coordinates and their corresponding neighbors:

[0060] p5 = w 51 p 1 + w 56 p 6 + w 58 p 8 + w 59 p 9

[0061] p 6 = w 62 p 2 + w 65 p 5 + w 67 p 7 + w 69 p 9

[0062] p 7 = w 73 p 3 + w 76 p 6 + w 78 p 8 + w 79 p 9

[0063] p 8 = w 84 p 4 + w 85 p 5 + w 87 p 7 + w 89 p 9

[0064] p 9 = w 95 p 5 + w 96 p 6 + w 97 p 7 + w 98 p 8

[0065] Convert the above formula into matrix representation:

[0066]

[0067] p n = Cp n + Bp a

[0068] where p a represents the anchor point, i.e., p a = [p 1 , p 2 , p3 , p 4 T ; p n represents the general node to be located, p n = [p 5 , p 6 , p 7 , p 8 , p 9 T , where B and C are matrices composed of barycentric coordinates. In this example, B and C are as follows

[0069]

[0070]

[0071] It can be equivalently written as:

[0072] (I - C)p n = Bp a

[0073] Let M = I - C, and multiply both sides of the equation by M T , let A = M T M and b = M T Bp a , the positioning problem can be expressed as a least - squares problem as follows:

[0074] Ap n = b

[0075] Decompose the matrix A:

[0076] A = D - L - U

[0077] where the matrix D represents the matrix composed of the diagonal of A, and - L and - U represent the strict lower - triangular matrix and the strict upper - triangular matrix of A respectively. At this time, take K = D and N = L + U, and express the problem in an iterative form:

[0078]

[0079] Then, perform Jacobi iterative estimation in the following way:

[0080]

[0081] Next, modify the Jacobi iterative estimation method to improve its convergence speed and obtain an iterative form more suitable for harsh environments.

[0082] Introduce a relaxation parameter α ∈ (0, 1) in the iteration, such that The expression of is ​​The weighted sum of the above formula. This form is called JOR (Jacobi Over-Relaxation) iteration and is given by the following formula:

[0083]

[0084] Next, the above recursive algorithm is developed into a distributed algorithm as follows:

[0085]

[0086] where a ii represents the element of A, b i represents the element of, and represents the i-th row of the matrix p n , that is, the coordinate position of the i-th node.

[0087] Step 2: Extended Kalman filter positioning.

[0088] Let r (UWB) represent the ranging based on UWB.

[0089] x t = [p 0,x,t , p 0,y,t , v 0,x,t , v 0,y,t T

[0090]

[0091] where x t contains the pose and velocity of the robot at time t, represents the pose and velocity of the anchor point under the UWB map.

[0092] In the dynamic observation model

[0093] x t = F t x t-1 + G t w t

[0094] where the transformation matrix:

[0095]

[0096]

[0097] The state noise w t is zero-mean and has a covariance:

[0098]

[0099] δ is the sampling interval, updated by the standard Extended Kalman Filter (EKF):

[0100]

[0101]

[0102]

[0103]

[0104]

[0105] P t|t =(I 4(Nt+1) -K t H t )P t|t-1

[0106] is the updated state estimate, and P t|t is the updated covariance estimate. H t is defined as

[0107] Due to the problem of anchor point positioning without global positioning information being processed, only four nodes are initially known. Therefore, based on UWB ranging, only the relative geometric position relationships of all nodes that can be arbitrarily transformed and rotated can be obtained, such as Figure 2 . When building the LiDAR map, this geometric uncertainty needs to be eliminated. To this end, through transformation and rotation, one anchor point is always at the origin, and another anchor point is always located on the positive x-axis. In addition, the velocities of these two selected anchor points are forced to be zero. Then, the LiDAR map can be constructed and updated under the local frame established by these two anchor points. The map building process is as shown in Figure 5 .

[0108] Step 3: Map building based on the Gauss-Newton method for matching.

[0109] Assume that the representation of the robot in the two-dimensional plane of the world coordinate system is ξ=(p x , p y , ψ) T , and the function M is the map information, and its value is the occupancy value of the grid map. Use the least squares method to solve for the optimal robot pose: ξ * is the optimal pose this time, that is, the result of the scan matching target.

[0110] The pose ξ * of the robot at time t and the pose ξ at the previous time have the relationship ξ+Δξ = ξ *, it is required that the total occupancy deviation of all laser points in the grid map is as small as possible, that is, it is required that Δξ satisfies the following requirements:

[0111]

[0112] Expand at Δξ:

[0113]

[0114] Take the derivative with respect to Δξ:

[0115]

[0116] Let:

[0117]

[0118]

[0119] Then the differentiated expression can be changed to:

[0120] 2(K - H·Δξ) = 0

[0121] That is:

[0122] Δξ = H -1 ·K

[0123] According to the definition:

[0124]

[0125]

[0126] Each grid of the grid map is represented by 0 - 1 for the probability of obstacle occupancy. Use bilinear interpolation to calculate the occupancy value M(P m ), P i,j = (s i,x , s i,y ) T

[0127]

[0128] Take the partial derivative of the above formula:

[0129]

[0130]

[0131] Replace P m in the above formula with S i (ξ), then the result M(S i(ξ) ) is obtained. The mapping effect is as Figure 6 shown, and the mapping result is as Figure 7 shown.

[0132] After simulation verification, compared with the commonly used anchor-tag positioning scheme in the market, the real-time fusion and mapping method based on SLAM and UWB proposed in this example can currently achieve a positioning accuracy of less than 2 cm for the distributed cooperative positioning of multi-agent systems based on UWB, as Figure 7 shown, far exceeding the positioning accuracy of existing UWB devices in the market (usually 15 cm).

[0133] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit them. Although the present invention has been described in detail with reference to the above embodiments, those of ordinary skill in the art should understand that: modifications or equivalent replacements can still be made to the specific implementation manners of the present invention, and any modification or equivalent replacement that does not depart from the spirit and scope of the present invention shall be covered by the protection scope of the claims of the present invention.

Claims

1. A real-time fusion and mapping method of SLAM and UWB, characterized in that: include: Step 1: During the movement of the intelligent agent carrying the laser radar, the positioning information of the intelligent agent and each ultra-wideband node at the current moment is obtained according to the distributed algorithm; Step 2: Based on the extended Kalman filter algorithm, the positioning information at the current moment is corrected; Step 3: Obtain the optimal posture of the agent at the current moment based on the Gauss-Newton method, use the optimal posture to correct the pose estimate of the agent at the current moment and the position of each ultra-wideband node, and update the lidar map and the UWB map at the same time; use the pose estimate of the agent at the current moment and the position of each ultra-wideband node output in step 3 as the input of step 2; Step 4: When the agent moves to the next moment, the next moment is set equal to the current moment, and steps 1 to 3 are repeated until the agent completes the preset task; In an area that humans cannot enter and where the GPS signal is weak and there are no other reference signals, intelligent agents are dispatched to conduct regional exploration and establish a map of the local area; the intelligent agents all carry ultra-wideband modules and have no prior information on positioning; after the intelligent agents enter the unknown area, they cast ultra-wideband nodes with known positions and ultra-wideband nodes with unknown positions, establish communication with each node, and measure the distance between each other; the intelligent agents continue to move in the area, collecting radar feedback data at various locations; in an area far away from the initial position, UWB positioning nodes are cast again for collaborative positioning; based on the extended Kalman filter method, the UWB fusion information is filtered to obtain relevant information of the current intelligent agent, including position, speed and direction; using this position as prior information, the Gauss-Newton method is used to match the radar data to obtain accurate information on the current position and establish a map; The step 2 comprises: Constructing a dynamic observation model according to the positioning information of the agent and each ultra-wideband node output in step 1; The dynamic observation model is updated by using an extended Kalman filter algorithm to obtain an updated state estimate and an updated covariance estimate, thereby completing the correction of the positioning information; The dynamic observation model includes state noise; the positioning information includes position and velocity; Using a scan matching process based on the Gauss-Newton method, the beam endpoints observed at the current moment are matched with the lidar map at the current moment to find the best transformation for a better match; The scan matching process is performed on a multi-resolution grid, mapping from a low-resolution grid to a high-resolution grid, thereby making it more likely to find a global solution rather than being trapped in a local solution; When establishing the laser radar map, through transformation and rotation, one ultra-wideband node is set to be fixed at the origin, and another ultra-wideband node is set to always be located on the positive x-axis; the speeds of the two set ultra-wideband nodes are zero; and the laser radar map is constructed and updated under a local framework established with the two set ultra-wideband nodes.

2. The method according to claim 1, characterized in that During the movement of the intelligent agent in step 1, when the preset condition is not met, it is necessary to continue to throw ultra-wideband nodes around it; the preset condition is that the number of ultra-wideband nodes within the measurable range around the intelligent agent is greater than 3.

Images