Robust positioning method for indoor mobile robot based on fusion of uwb and lidar

Abstract

The application discloses a robust positioning method for an indoor mobile robot based on UWB and laser radar fusion, and relies on three UWB detectors fixed on the mobile robot to detect UWB anchor points with known position information in the environment. The UWB ranging principle adopts a signal flight time method to calculate the distance between two modules; a distance interval is set, and the real mobile robot position point is selected from two points which may be the real mobile robot position information; the application can improve the positioning matching accuracy, realize the fusion of UWB and laser radar, and optimize the accurate robust positioning technology of the indoor mobile robot position. The fusion system realizes the function of real-time accurate positioning of the mobile robot position information, improves the accurate positioning of the mobile robot in an unknown environment, and can accurately obtain the distance information and angle information of the mobile robot in a world coordinate system.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of optimizing indoor mobile robot positioning, and particularly relates to an indoor mobile robot positioning method based on UWB and laser radar fusion. BACKGROUND

[0002] UWB is a signal-based positioning method, which can provide reliable absolute positioning in a line-of-sight environment, but is also subject to the complexity of the indoor environment, and in a non-line-of-sight environment and multipath effect, the positioning accuracy will decrease. LiDAR SLAM positioning technology can provide reliable spatial characteristics, and has high positioning accuracy, but using LiDAR SLAM positioning technology alone has problems such as long search and positioning time for the starting point of the robot, and inability to quickly reposition after laser point cloud matching fails. In addition, in long straight tunnels and other environments, the laser radar cannot determine the position because the measurement values at different places are the same, and in this case, UWB needs to be combined to realize simultaneous localization and mapping function.

[0003] The traditional method mainly uses the UWB positioning system to eliminate the cumulative error of laser SLAM in a large scene. These methods bring the original ranging information of UWB into the cost function to optimize the robot position, which can improve the positioning accuracy of the robot moving in a long distance to a certain extent. However, due to the ranging accuracy of UWB itself being 10-20 cm, the positioning error of the mobile robot optimized by using the ranging information is often greater than the error, which leads to the inability to meet the accuracy requirements of some indoor mobile robots. At the same time, most of the UWB and laser radar fusion algorithms are optimized at all times, and it takes a certain amount of time to find the optimal pose, resulting in a large trajectory error. Without the sliding window method, only part of the new data is optimized. SUMMARY

[0004] The technical problem to be solved by the present application is to overcome the defects in the current technology, to solve the problem of autonomous positioning of mobile robots in unknown environments, and to propose a UWB and laser radar fusion optimized mobile robot positioning method. The method obtains the accurate position and angle information of the mobile robot by optimizing the laser radar position information through the position information of two known UWB anchor points and the UWB detector, which to a certain extent alleviates the accumulation of positioning errors in long-term SLAM.

[0005] In order to achieve the above purpose, the present application adopts the following technical scheme:

[0006] 1) Relying on three UWB detectors fixed on the mobile robot to detect the UWB anchor points with known position information in the environment. When a certain anchor point is detected, the three UWB detectors measure the distance information respectively, which is recorded as range1, range2, and range3.

[0007] 2) Its UWB ranging principle adopts signal flight time method, sets module A as UWB detector, module B as UWB anchor point, uses the time difference of data packet between two modules to calculate the distance of the target to be measured. The time of signal one-way propagation in the air is:

[0008] T TOF = (T TOT -T TAT ) / 2

[0009] T TOT is the time difference of module A from sending signal to receiving signal, T TAT is the time difference of module B from receiving signal to sending signal.

[0010] 3) The distance d T (t) between two modules:

[0011] d T (t) = c * T TOF

[0012] c is the speed of light, T TOF is the speed of signal propagation in the air.

[0013] 4) Since the distance between two modules obtained in step 3, i.e. UWB distance information d T (t), is affected by multipath effect and other reasons, in complex indoor environment, obstacles (pedestrians, vehicles, walls, etc.) will cause reflection, refraction and penetration of UWB pulse signal in the propagation process, causing a certain degree of signal attenuation, time delay and large propagation distance, d T (t) will often be larger than the actual distance information. To correct the error, a distance interval (range min , range max ) is set. range min = 0.8 * dT(t), range max = 1.2 * dT(t), the distance range of the distance interval contains the UWB true ranging value. Then through the distance information d L (t) between the known UWB detector and laser radar, the angle θ between the UWB detector and the UWB anchor point to the laser radar, and the range (range min , range max ) obtained before, the cosine theorem

[0014] range min 2 = dL(t) 2 + distance min 2 - 2 * distance min*dL(t)*cosθ

[0015] range max 2 =dL(t) 2 +distance max 2 -2*distance max *dL(t)*cosθ

[0016] The distance range from the laser radar to the UWB anchor point can be calculated. min， distance max ).

[0017] 5) Solve for the true distance between the UWB anchor point and the laser sensor using the sliding window method in target detection.

[0018] Following step 5, the following steps are also included:

[0019] 51) Use a UWB detector to find the initial range at time (tk) and time t respectively. i Based on range i Establish a distance range min range max Meanwhile, the UWB anchor point is set at the same height as the lidar, and the spatial relationship between the lidar and the UWB detector is known.

[0020] 52) Combining the two, the true distance from the UWB anchor point to the lidar at time (tk) and time t can be derived. i The distance between two points in the radar point cloud that meet the conditions at two different times is the true distance. i ;

[0021] 53) Place another UWB anchor point 2 and repeat step 3 to calculate the actual distance from this anchor point to the lidar. At this point, the actual distances and angles from the two UWB anchor points to the lidar are obtained, namely distance1, θ1 and distance2, θ2. Using these four known data points, the positions of the two points corresponding to the lidar can be found.

[0022] 54) At this point, the precise distances 1 and 2, and the two UWB anchor points P are known. anchor i The location information is expressed by the following formula for the LiDAR P to be optimized. Lidar Two possible location information points:

[0023] distance1-[PLidar1 (x1, y1, z1) - P archor1 (x, y, z)] = 0

[0024] distance2-[P Lidar2 (x2, y2, z2) - P archor2 (x, y, z)] = 0

[0025]

[0026]

[0027] 55) Construct the cost function according to the above formula:

[0028]

[0029] 56) distance i is the distance between the two optimized measurements of the laser radar and the UWB anchor point, P anchor i is the position information of the two UWB anchors, x is the precise position information of the mobile robot to be solved, T is the spatial transformation matrix of the mobile robot position to the laser radar, and i is the number of UWB anchors;

[0030] 57) After obtaining the optimization equation, use the Ceres library to solve it, set the optimization variables and residual terms, and solve the optimized data as the mobile robot positioning result through the Ceres solver, according to Figure 4 the optimized positioning result is two.

[0031] 6) Select the real mobile robot position point from the two possible real mobile robot position points, which can be solved by the following steps:

[0032] A) In the xy plane of the world coordinate system, the mobile robot position coordinates are known, and in the xy plane of the robot coordinate system, the coordinates of the two UWB anchors are known: the coordinates of UWB anchor 1 P anchor1 (x1, y1), and the coordinates of UWB anchor 2 P archor2 (x2, y2);

[0033] B) Let the first position of the mobile robot be (x a , y a ), and the second position be (x b , y b );

[0034] C) The known vectors in the two coordinate systems are cross-multiplied using the vector cross product method to obtain two values, respectively

[0035] k1 = (x1 - x a)*(y2-y a )-(y1-y a )*(x2-x a )

[0036] k2=(x1-x b )*(y2-y b )-(y1-y b )*(x2-x b )

[0037] D) two circles are composed of the figure is with the line of centers as the axis of symmetry, the intersection of two circles is the line of centers two corresponding points on both sides, so that k1, k2 are positive and negative, the z axis with the same direction as the world coordinate system is the real mobile robot position coordinates;

[0038] The application can improve the positioning matching accuracy, realize the fusion of UWB and laser radar, and optimize the accurate robust positioning technology of indoor mobile robot position. The fusion system has no cumulative error compared with laser radar, realizes the function of real-time accurate positioning of mobile robot position information, improves the accurate positioning of mobile robot in unknown environment, and can accurately obtain the distance information and angle information of mobile robot in the world coordinate system. Through simulation experiment, the positioning accuracy of robot position information can be 3cm within the positioning error of UWB measurement error of 10cm. BRIEF DESCRIPTION OF DRAWINGS

[0039] Figure 1 It is the TOF ranging principle of the application.

[0040] Figure 2 It is the laser radar optimized UWB positioning framework of the application.

[0041] Figure 3 It is the actual robot movement graph of the application.

[0042] Figure 4 It is the laser radar position information determined by two UWB anchor points of the application.

[0043] Figure 5 It is the algorithm block diagram of the fusion positioning system in the application.

[0044] Figure 6 It is the specific steps of optimizing the positioning result of mobile robot in the application. DETAILED DESCRIPTION

[0045] The application will be described in detail below in combination with the drawings and examples.

[0046] An optimized mobile robot positioning method based on UWB and laser radar fusion. The method obtains the accurate position and angle information of the mobile robot by optimizing the position information of the laser radar based on the position information of two known UWB anchors and the UWB detector. The accumulation of positioning errors in long-term SLAM is alleviated to a certain extent.

[0047] In order to achieve the above-mentioned purpose, the technical scheme adopted by the present application is as follows:

[0048] 1) Three UWB detectors fixed on the mobile robot detect the UWB anchors with known position information in the environment. When a certain anchor is detected, the three UWB detectors measure the distance information as range1, range2 and range3 respectively.

[0049] 2) The UWB ranging principle adopts the time of flight (TOF) method (as shown in Figure 1 ), which calculates the distance of the target to be measured by using the time difference between the round trip of data packets between two modules. The one-way propagation time of the signal in the air is:

[0050] T TOF (T TOT -T TAT ) / 2

[0051] 3) Wherein module A is the UWB detector, module B is the UWB anchor, T_TOT is the time difference from sending a signal to receiving a signal of module A, and T_TAT is the time difference from receiving a signal to sending a signal of module B. The distance between the two modules is:

[0052] d T (t)=c×T TOF

[0053] c is the speed of light, which is the speed of signal propagation in the air.

[0054] 4) The distance between the two modules obtained in step 3, i.e. the UWB distance information d T (t), is affected by multipath effect and other reasons. In a complex indoor environment, obstacles (pedestrians, vehicles, pillars, etc.) will cause reflection, refraction and penetration of UWB pulse signals during propagation, resulting in a certain degree of signal attenuation, time delay and large propagation distance. Therefore, d T (t) is often larger than the actual distance information. In order to correct the error, a distance interval (range min , range max ) is set. range min =0.8*dT(t), range max= 1.2 * dT(t), the distance range of the distance interval contains the real distance value of UWB. Then through the distance information d L (t) between the UWB detector and the laser radar, the angle θ between the UWB detector and the UWB anchor point to the laser radar, and the range min (range max ) obtained in the previous step, the cosine theorem is used

[0055] range min 2 = dL(t) 2 + distance min 2 - 2 * distance min * dL(t) * cos θ

[0056] range max 2 = dL(t) 2 + distance max 2 - 2 * distance max * dL(t) * cos θ

[0057] The distance interval (distance min , distance max ) of the laser radar to the UWB anchor point can be obtained.

[0058] 5) The real distance of the UWB anchor point relative to the laser sensor is solved through the sliding window method in target detection.

[0059] After step 5, the following steps are included:

[0060] 6) The initial range i at time (t-k) and time t is found respectively using the UWB detector, and based on the range i , a distance interval (range min , range max ) is set up, and the spatial relationship between the laser radar and the UWB detector is known, and it is set that the UWB anchor point and the laser radar are at the same height.

[0061] 7) The real distance distance i of the UWB anchor point to the laser radar at time (t-k) and time t can be derived from the interval, so as to find the radar point cloud that meets the conditions at the two times, and the distance corresponding to the point cloud of the overlapping part of the two times is the real distance i ;

[0062] The specific steps are as follows Figure 2 The relative positions of the laser radar and the UWB detector in the actual robot are as shown in Figure 3 ;

[0063] 8) Place another UWB anchor point 2 and repeat step 3 to calculate the real distance from the laser radar to the anchor point. At this time, the real distances and angles of the two UWB anchor points to the laser radar can be obtained, which are distance1, θ1 and distance2, θ2 respectively. Through the four known data, the positions of the two points conforming to the laser radar can be found, as shown in Figure 4 ;

[0064] 9) At this time, the accurate distance1, distance2 and the position information of the two UWB anchor points P anchor i are known, and the two possible position information points of the laser radar P Lidar to be optimized can be expressed by the following formulas:

[0065] distance1-[P Lidar1 (x1, y1, z1)-P archor1 (x, y, z)] = 0

[0066] distance2-[P Lidar2 (x2, y2, z2)-P archor2 (x, y, z)] = 0

[0067]

[0068]

[0069] 10) The cost function is constructed according to the above formulas:

[0070]

[0071] 11) distance i is the distance from the laser radar to the UWB anchor point measured after optimization, P anchor i is the position information of the two known UWB anchor points, x is the accurate position information of the robot to be solved, T is the spatial transformation matrix of the mobile robot position to the laser radar, and i is the number of UWB anchor points;

[0072] 12) After obtaining the optimization equation, the Ceres library is used for solving. As long as the optimization variables and residual terms are set, the optimized data can be solved by the Ceres solver as the positioning result of the mobile robot. According to Figure 4 , the optimized positioning result is two.

[0073] The algorithm block diagram of the whole fusion positioning system is as shown in the figure Figure 5 ;

[0074] 13) Select the real mobile robot position point from two possible real mobile robot position information points, which can be solved by the following steps:

[0075] A) The mobile robot position coordinates are known in the xy plane of the world coordinate system, and the coordinates of the two UWB anchor points are known in the xy plane of the robot coordinate system: the coordinates of UWB anchor point 1 P anchor1 are (x1, y1), and the coordinates of UWB anchor point 2 P archor2 are (x2, y2);

[0076] B) Let the first position of the mobile robot be (x a , y a ), and the second position be (x b , y b );

[0077] C) The known vectors in the two coordinate systems are crossed by using the vector cross product method to obtain two values

[0078] k1=(x1-x a )*(y2-y a )-(y1-y a )*(x2-x a )

[0079] k2=(x1-x b )*(y2-y b )-(y1-y b )*(x2-x b )

[0080] D) The figure composed of two circles is a figure symmetrical about the bisector, and the intersection of the two circles is the corresponding two points on both sides of the bisector, so k1 and k2 are positive and negative, and the z-axis with a positive number is the same as the direction of the world coordinate system, which is the real mobile robot position coordinates;

[0081] The application can improve the positioning matching accuracy, realize the fusion of UWB and laser radar, and optimize the accurate and robust positioning technology of the indoor mobile robot position.

[0082] The specific steps of optimizing the mobile robot positioning result are as shown in the figure Figure 6 .

Claims

1. A robust localization method for indoor mobile robots based on UWB and lidar fusion, characterized in that, The method includes the following steps: 1) Relying on three UWB detectors fixed on the mobile robot, the robot detects UWB anchor points in the environment whose location information is known; when an anchor point is detected, the three UWB detectors measure the distance information and record it as range1, range2, and range3 respectively. 2) Its UWB ranging principle adopts the signal time-of-flight method. Module A is set as the UWB detector, and module B is the UWB anchor point. The distance to the target is calculated using the round-trip time difference of the data packet between the two modules; the one-way propagation time of the signal in the air is: T TOF =(T TOT -T TAT ) / 2 T TOT T is the time difference between when module A sends a signal and when it receives a signal. TAT It is the time difference between module B receiving the signal and transmitting the signal; 3) The distance d between the two modules T (t): d T (t)=c×T TOF c is the speed of light, T TOF The speed at which a signal travels through the air; 4) Due to the distance between the two modules obtained in step 3, i.e., the UWB distance information d T (t) Subject to multipath effects, in complex indoor environments, d T (t) is larger than the actual distance information; to correct the error, a distance interval is established. min range max ); range min = 0.8*dT(t), range max = 1.2*dT(t), the distance range of this interval includes the actual UWB ranging value; then, using the known distance information d between the UWB detector and the lidar... L (t), the angle θ between the UWB detector and the UWB anchor point to the lidar, and the previously obtained (range) min range max Using the Law of Cosines range min 2 =dL(t) 2 +distance min 2 -2*distance min *dL(t)*cosθ range max 2 =dL(t) 2 +distance max 2 -2*distance max *dL(t)*cosθ Find the distance range from the laser radar to the UWB anchor point. min distance max ); 5) Using the sliding window method in target detection, calculate the true distance between the UWB anchor point and the laser sensor; then place another UWB anchor point 2 and repeat step 3 to calculate the true distance from the anchor point to the laser radar, obtaining the true distances and angles from the two UWB anchor points to the laser radar, namely distance1, θ1 and distance2, θ2; using these four known data, find the two points that match the position of the laser radar. 6) Select the actual mobile robot location from the two possible locations using the following steps: A) The position coordinates of the mobile robot can be determined in the xy plane of the world coordinate system, and the coordinates of the two UWB anchor points can be determined in the xy plane of the robot coordinate system: Given UWB anchor point 1P anchor1 The coordinates are (x1, y1), and the UWB anchor point is 2P. archor2 The coordinates are (x2, y2); B) Let the first position of the mobile robot be (x) a ,y a ), the second position is (x b ,y b ); C) Given vectors in two coordinate systems, the cross product of the vectors yields two values: k1=(x1-x a )*(y2-y a )-(y1-y a )*(x2-x a ) k2=(x1-x b )*(y2-y b )-(y1-y b )*(x2-x b ) D) The figure formed by the two circles is a figure with the line connecting their centers as the axis of symmetry. The intersection of the two circles is the two points on the two sides of the line connecting their centers. The k1 and k2 must be one positive and one negative. The z-axis with positive numbers is in the same direction as the world coordinate system and is the true position coordinate of the mobile robot.

2. The robust localization method for indoor mobile robots based on UWB and lidar fusion as described in claim 1, characterized in that, Step 5 includes the following steps: 51) Use a UWB detector to find the initial range at time (tk) and time t respectively. i Based on range i Establish a distance range min range max Meanwhile, the UWB anchor point is set at the same height as the lidar, and the spatial relationship between the lidar and the UWB detector is known. 52) Combining the two, we can derive the true distance from the UWB anchor point to the lidar at time (tk) and time t. i The interval is defined to find radar point clouds that meet the conditions at two different times. The distance corresponding to the point cloud in the overlapping part of these two times is the true distance. i ; 53) At this point, the precise distances 1 and 2, and the two UWB anchor points P are known. anchori The location information is expressed by the following formula for the LiDAR P to be optimized. Lidar Two possible location information points: distance1-[P Lidar1 (x1,y1,z1)-P archor1 (x,y,z)]=0 distance2-[P Lidar2 (x2,y2,z2)-P archor2 (x,y,z)]=0 54) The cost function is constructed as follows: 55) distance i For the two optimized measurements of the distance from the lidar to the UWB anchor point, P anchori Given the position information of two known UWB anchor points, x is the precise position information of the robot to be determined, T is the spatial transformation matrix from the position of the mobile robot to the LiDAR, and i is the nth UWB anchor point; 56) After obtaining the optimization equation, use the Ceres library to solve it, set the optimization variables and residual terms, and use the Ceres solver to obtain the optimized data as the mobile robot localization result.

Images