Multi-sensor fusion based mobile robot navigation and positioning system and method

Abstract

This invention belongs to the field of robot navigation and positioning, and discloses a mobile robot navigation and positioning system based on multi-sensor fusion. The system includes: at least four UWB base stations installed in a given manner within the working area of ​​the mobile robot; a combined tag device consisting of two UWB positioning tags installed at set intervals on a connecting component and a gyroscope for measuring the rotational angular velocity of the connecting component; and a main control device for receiving tag data, gyroscope data, and mobile robot odometer data, performing real-time fusion processing on the received data, and outputting current tag device positioning data and heading angle data. This invention solves the problems of large positioning errors and lack of heading angle information in UWB positioning systems by fusing data from the odometer, gyroscope, and multiple UWB tags. It can provide relatively accurate navigation and positioning information for various indoor and outdoor mobile robots such as autonomous forklifts, stackers, tractors, and AGVs. A corresponding method is also disclosed.

Description

Technical Field

[0001] This invention belongs to the field of robot navigation and positioning, and particularly relates to a mobile robot navigation and positioning system and method based on multi-sensor fusion. Background Technology

[0002] Currently, the mainstream navigation and positioning methods in the mobile robot field are QR code navigation and laser SLAM navigation. QR code navigation is essentially a type of visual navigation. It uses a camera to capture images of QR codes placed on the ground, and then uses image recognition algorithms to identify the QR codes and calculate the vehicle's pose. It has advantages such as wide applicability, high positioning accuracy, and low algorithm complexity. However, it has disadvantages such as complicated installation and deployment, the navigation route cannot be arbitrarily changed, and high maintenance costs for the QR codes. Laser SLAM positioning has advantages such as high accuracy, high heading angle accuracy, and convenient deployment. However, it is expensive. In addition, laser radar has strict requirements for the use scenario, requiring a high degree of ground flatness and a sufficient number of fixed objects with reflective characteristics in the surrounding area. It cannot be adapted to large open areas, sites with many glass walls, or scenarios where production equipment moves frequently. As more and more application scenarios incorporate intelligent transformation, the navigation and positioning solutions mentioned above can no longer meet the navigation and positioning needs of mobile robots, necessitating new solutions. Among these, UWB (Ultra-Wideband) positioning technology is a promising solution that has seen considerable research activity in recent years. UWB positioning systems based on the TDOA (Time Difference of Arrival) principle have been widely used for personnel and material positioning in various locations. However, due to factors such as poor positioning accuracy caused by NLOS (No-Light-Of-Sight) interference and the lack of heading angle information, UWB positioning systems are currently still difficult to apply to the field of mobile robot navigation and positioning.

[0003] Currently, integrating other sensors with UWB positioning is a feasible solution. However, most existing research is theoretical, and the few published patents largely fail to consider the practical needs of robot navigation and positioning, making application deployment difficult. For example, patent application CN112833876A (application number CN202011625879.0) discloses a multi-robot collaborative positioning method integrating odometry and UWB. This method uses a complex graph optimization algorithm and requires multiple robots to cooperate, making practical deployment conditions demanding. Patent application CN114915913A (application number 202210536034.7) discloses a UWB-IMU combined indoor positioning method based on sliding window factor graphs. This method uses IMU pre-integrated data to perform graph optimization on UWB positioning data, which can reduce NLOS interference to some extent. However, due to the lack of a global heading angle tracking correction system, gyroscope error integration leads to a large cumulative error in the reference attitude, rendering the IMU coordinate system data unusable.

[0004] Currently, there are two main technical solutions for UWB-based positioning. One solution is based on TDOA (Time Difference of Arrival), which requires the tag to send a signal once. The system then obtains the time difference between the arrival of the signal at each base station and combines this with the coordinates of each base station to calculate the tag's position. This solution has advantages such as fast position calculation speed and large system tag capacity. However, the positioning accuracy is heavily dependent on the time synchronization accuracy between the base stations. Currently, the time synchronization accuracy of mainstream base stations is around 1ns. Multiplying this time by the speed of light gives the tag's positioning error, which is about 30cm. This positioning accuracy can basically meet the positioning needs of personnel, materials, and equipment, but it cannot meet the navigation and positioning needs of mobile robots. Another approach is based on the TWR (Two-Way-Ranging) principle. This method obtains the time-of-flight (TOF) of electromagnetic waves between the tag and the base station through multiple communications. Multiplying this value by the speed of light yields the distance between the base station and the tag. Under line-of-sight propagation, the distance error between the tag and the base station measured using this method is generally around 10cm. Obtaining the distance from the tag to at least three base stations allows for 2D planar positioning with an accuracy of approximately 10cm using the trilateration principle. However, in non-line-of-sight (NLOS) propagation scenarios (such as refraction, reflection, and object penetration), the ranging values ​​will deviate significantly, leading to substantial positioning errors. Therefore, a key challenge for UWB positioning algorithms is filtering NLOS interference. Currently, most publicly available algorithms filter the positioning results to reduce positioning errors caused by NLOS replay, with few algorithms directly filtering the raw TWR ranging data. This invention proposes a gradient outlier detection filtering algorithm based on a sliding window, which can effectively detect and remove data with ranging errors exceeding a certain threshold. Summary of the Invention

[0005] To address the aforementioned technical problems, this invention proposes a mobile robot navigation and positioning system and method based on multi-sensor fusion. By fusing data from odometry, gyroscopes, and multiple UWB tags, it solves the problems of large positioning errors and lack of heading angle information in UWB positioning systems. This provides relatively accurate navigation and positioning information for various indoor and outdoor mobile robots, such as autonomous forklifts, stackers, tractors, and AGVs. Its ultimate goal is to provide mobile robots with more accurate positioning and heading angle information. Positioning information refers to the robot's spatial location on a map, usually represented by spatial coordinates. Heading angle information refers to the robot's orientation, typically expressed as the angle between the robot's positive X-axis and the map's X-axis. The heading angle is essential information for the mobile robot's path navigation and motion control.

[0006] To achieve the above objectives, the present invention provides a mobile robot navigation and positioning system based on multi-sensor fusion, comprising:

[0007] The UWB base station system includes at least four UWB base stations installed within the working area of ​​the mobile robot;

[0008] A combined tagging device includes at least two UWB tags spaced apart on a connecting component and a gyroscope for measuring the rotational angular velocity of the connecting component, the gyroscope being mounted at the center of the connecting component.

[0009] The main control device includes a tag interface, a gyroscope interface, and an odometer interface. It is used to receive data from the combined tag device, gyroscope data, and mobile robot odometer data, and to perform real-time fusion processing on the received data to output the current positioning data and heading angle data of the combined tag device.

[0010] The mobile robot's odometer is connected to the main control device via an odometer interface.

[0011] Preferably, the UWB base station is divided into different positioning areas, which include indoor, outdoor, or cross-indoor and outdoor areas.

[0012] Preferably, the installation distance between the UWB tags is set according to the size of the robot body; the gyroscope is an inertial navigation device that includes a gyroscope and an accelerometer.

[0013] A second aspect of the present invention provides a method for navigation and localization of a mobile robot based on multi-sensor fusion, comprising:

[0014] The specific method is as follows:

[0015] S1: Construct a sliding window filter to detect and filter the distance data from the tag containing NLOS to each base station, and obtain the filtered distance data from the tag to each base station. This includes the following specific steps:

[0016] S11: Obtain distance data from tags to each base station in real time using the tags, and store the distance data into corresponding arrays according to the base station ID. Assuming there are m base stations and the sliding window size is set to n, then m distance arrays of length n can be obtained: D1, D2...D m , where D = D(1), D(2)...D(n)

[0017] S12: Let one of the arrays be D. i Then, the absolute value array d of the distance gradient is first calculated using the following formula. i :

[0018] d i (n)=|D i(n)-D i (n-1)|

[0019] Calculate d i The standard deviation is used to determine whether the value is greater than or less than a set threshold. If it is less than the set variance threshold, it indicates a distance from array D. i There are no significant numerical jumps, and it is minimally affected by NLOS noise; the corresponding distance data can remain at their original values. If it exceeds the set placement threshold, it indicates that the distance array D... i There are significant numerical jumps, which are greatly affected by NLOS. Data with large gradient changes are more likely to contain NLOS noise, which needs to be identified and removed, as shown in S13.

[0020] S13: Iterate through the gradient array d whose standard deviation is greater than the set standard deviation threshold. i If the j-th value d i (j) If the distance is less than the set gradient threshold, the original distance value is retained, i.e., FD. i (j)=D i (j), if d i (j) If the distance value is greater than the set gradient threshold, the distance value at that location is discarded and replaced with the previous value, i.e., FD. i (j)=D i (j-1);

[0021] S14: For each distance array D1, D2...D m Repeating steps s12 and s13 will yield distance data FD1, FD2...FD from each base station, with NLOS noise largely filtered out. m .

[0022] Preferably, the present invention proposes an improved least squares trilateration algorithm: S2: Divide the filtered distance data into multiple groups of 3, calculate the positioning data of multiple groups using the least squares method, calculate the confidence of each group of positioning data, and obtain the final tag positioning data by weighting according to the confidence level. The specific implementation steps are as follows:

[0023] S21: Divide the m base stations in the current positioning area into k groups of 3 according to the permutation and combination algorithm, and calculate the coordinates of each group using the least squares algorithm. The general formula for least squares is as follows:

[0024] X = (A T A) -1 A T b

[0025] Where X is the matrix to be solved, and in a 2D planar trilateration system, its specific form is X = x, y

[0026] Where A is the parameter matrix, and in a 2D planar trilateration system, A takes the following form:

[0027]

[0028] In the above formula, x1,y1, x2,y2, and x3,y3 are the x-axis and y-axis coordinates of the three base stations, respectively.

[0029] Where b is a parameter matrix, and in a 2D planar trilateration system, b takes the following form:

[0030]

[0031] In the above formula, x1, y1, x2, y2, and x3, y3 are the x-axis and y-axis coordinates of the three base stations, respectively, and s1, s2, and s3 are the horizontal distances from the tag to the three base stations, which can be calculated based on the tag installation height and the distance from the tag to the base station. The formula used is as follows:

[0032]

[0033] In the above formula, d represents the straight-line distance from the tag to the base station with the specified ID, which can be obtained from the filtered distance data output by S1: FD1, FD2...FD m The distance from the tag to the base station is obtained as follows: for example, if ID = 1 and the latest data is taken as the measurement result, then the distance from the tag to the base station is: d = FD1(n); z is the z-axis coordinate of the current base station, and h is the installation height of the tag.

[0034] S22: By substituting the specific values ​​of each parameter into the corresponding formula according to the calculation method provided in S21, k positioning data sets can be obtained: p1, p2...p k A common algorithm is to directly average the data set as the final positioning result. While simple, this method cannot effectively remove positioning data containing NLOS noise, resulting in low positioning accuracy. This invention proposes a least squares calculation result evaluation algorithm to calculate the confidence level of each positioning result, remove data with low confidence levels, and then calculate a weighted average of the remaining data according to their confidence levels to output the final positioning result. This can further reduce the interference of NLOS noise and improve positioning accuracy. The specific implementation method is as follows:

[0035] 1. First, calculate the sum of squares of the differences between the distance from the location point to each base station and the distance measured by the tag, using the following formula:

[0036] In the above formula, x p ,y p For the location data of one of the base station combinations, x i ,y iLet be the x-axis and y-axis coordinates of the i-th base station, where i ∈ 1, 2, 3, and s i Let be the horizontal distance from the tag to the i-th base station. Theoretically, the more accurate the distance measurement, the more accurate the positioning result calculated by the least squares solution. The smaller the sum value, the larger the measurement error. The larger the positioning error, the larger the sum value. For ease of later calculation and intuitive display, the sum value is transformed by a formula to obtain the confidence level c of the positioning result. The formula is as follows:

[0037]

[0038] In the above formula, c is the confidence level of the positioning result, and its value ranges from (0,1). The higher the confidence level, the smaller the possible positioning error.

[0039] 2. Based on the method given in S1, perform a localization analysis on the array p1, p2...p k By calculating the confidence scores of each location data point, the corresponding confidence score arrays c1, c2...c can be obtained. k By iterating through the array and removing data with confidence scores below a set threshold while keeping data with confidence scores above the set threshold, a new confidence array c1, c2...c can be obtained. n and the corresponding location result array p1, p2...p n When n = 0, it means that the confidence level of all positioning results is less than the set threshold, and the positioning solution fails. When 1 ≤ n ≤ k, the final positioning result p is obtained by weighting the array of positioning results according to their confidence levels. mean The formulas for calculating x and y are as follows:

[0040]

[0041] S23: Performing operations S1, S21, and S22 on labels A and B respectively will yield the location data for both labels: p tag1 x, y, p tag2 x, y;

[0042] Preferably, S3: Calculate the heading angle of the combined tag device using the positioning data of the two tags, and obtain the fused heading angle data by coupling the tag heading angle data and the gyroscope angular velocity using a Kalman filter algorithm; specifically, this includes the following steps:

[0043] S31: Calculate the vector angle between label B and label A based on the coordinates of label A and label B on the map. Then, based on the installation angle of the label connection component relative to the robot's X-axis, the robot's heading angle can be calculated. Since the label positioning data will fluctuate within a certain error range, and the label vector magnitude is limited, the calculated heading angle data will contain a large amount of high-frequency noise and cannot be directly used for robot navigation. However, from the perspective of the global map, the long-term cumulative error of this heading angle is close to 0. Therefore, it can be used as the observation input for the Kalman filter to calibrate the cumulative error generated by the gyroscope integration. The specific calculation formula is as follows:

[0044]

[0045] In the above formula, yaw mes For the vehicle's observed heading angle output, (x tag1 ,y tag1 ), (x tag2 ,y tag2 ) are the location data p for labels A and B, respectively. tag1 x, y, p tag2 x, y, and fix_angle are the mounting angles of the label connection component relative to the X-axis of the robot body;

[0046] S32: Heading angle calculated using two-tag positioning. mes For measurement input, the gyroscope angular velocity ω is used as the prediction input. A Kalman filter equation system is constructed for heading angle fusion. In this system, the state transition matrix, control matrix, and measurement matrix are all 1×1 identity matrices. Substituting them into the standard Kalman filter equation system yields the simplified Kalman filter equation system:

[0047] X(k|k-1)=X(k-1|k-1)+U(k)

[0048] The above equation is the state prediction equation for the Kalman system, where X is the fused heading angle yaw. fusion X(k|k-1) is the optimal estimate of the current system, i.e., the optimal estimate of the current heading angle. X(k-1|k-1) is the optimal estimate of the previous system. U(k) is the control variable of the system. In this system, U(k) = ω × Δt, where ω is the angular velocity value returned by the gyroscope measurement, and Δt is the Kalman filter update time interval.

[0049] P(k|k-1)=P(k-1|k-1)+Q

[0050] The above equation is the Kalman system covariance prediction equation, where P(k|k-1) is the optimal estimate of the prior covariance of the system in this case, P(k-1|k-1) is the optimal estimate of the prior covariance of the system in the previous case, and Q is the process noise covariance matrix of the system. In this system, Q is the gyroscope angular velocity measurement noise.

[0051]

[0052] The above equation is the Kalman gain calculation equation, where K(k) represents the Kalman gain of the system at time k, P(k-1|k-1) is the previous optimal estimate of the system's prior covariance, and R is the system's measurement noise covariance matrix. In this system, R is the tag heading angle yaw. mes Measurement noise;

[0053] X(k|k)=X(k-1|k-1)+K(k)Z(k)-X(k|k-1)

[0054] The above equation is the optimal state estimation equation for a Kalman system, where X(k|k) is the current optimal state estimate of the system, i.e., the current fused heading angle yaw. fusion X(k|k-1) is the optimal state estimate of the system at time k-1, K(k) is the Kalman gain of the system, X(k-1|k-1) is the optimal state estimate of the system at time k-2, and Z(k) is the measured value of the system. In this system, Z(k) = yaw mes ;

[0055] P(k|k)=1-K(k)P(k|k-1)

[0056] The above equation is the posterior covariance equation for the Kalman system, where P(k|k) is the posterior covariance of the system in this case, K(k) is the Kalman gain of the system in this case, and P(k|k-1) is the optimal estimation result of the posterior covariance of the system at time k-1.

[0057] S33: Repeating S31 and S32 to iteratively update the Kalman filter yields the real-time fused heading angle yaw. fusion .

[0058] Preferably, S4: The average value of the two tag positioning data is taken as the positioning data of the combined tag device, and then the tag positioning data, odometer data and heading angle data are coupled using a first-order low-pass filtering algorithm to obtain fused positioning data p. fusion Specifically, it includes the following steps:

[0059] S41: Take the average of the positioning data from the two tags as the positioning data p for the combined tag device. tag ,Right now:

[0060]

[0061] S42: Preprocess the odometer feedback data to ensure that the odometer input data format is the instantaneous speed relative to the vehicle body at the previous moment. Then, it is transformed to the center of the tag connection component to obtain the instantaneous velocity of the combined tag device relative to the vehicle body at the previous moment. Considering only two-dimensional planar motion, based on the fused heading angle yaw fusion The Euler angles (yaw) of the vehicle relative to the map coordinate system can be obtained. fusion For ease of calculation, the expression (,0,0) can be converted into an attitude quaternion q, and then the quaternion can be used to... Perform a coordinate transformation to determine the velocity of the combined label device in the map coordinate system. The calculation formula is as follows:

[0062]

[0063] In the above formula, q represents the four attitude elements. -1 It is the inverse of a quaternion;

[0064] S43: Employ a first-order low-pass filtering algorithm to couple tag positioning data p tag and the tag movement speed calculated by the odometer The calculation formula is as follows:

[0065]

[0066] In the above formula, p fusion (t) represents the fused positioning data at the current moment, p fusion (t-1) represents the fused positioning data from the previous time step, p tag (t) Tag location data, p tag (t-1) represents the label location data from the previous time step. Δt is the speed of the combined label device in the map coordinate system at the current moment, Δt is the time interval between time t and time t-1, and w is the weighting coefficient of the odometer data.

[0067] According to the above technical solution, the real-time positioning data p of the positioning device can be obtained by repeatedly executing S1, S2, S3 and S4. fusion [x, y] and heading angle data yaw fusion The two can be combined to form fused pose data. fusion [x,y,yaw] This data can provide positioning and navigation support for various indoor and outdoor mobile robots.

[0068] Compared with the prior art, the present invention has at least the following beneficial effects:

[0069] The method provided by the present invention can effectively remove the TWR distance data and positioning data containing NLOS errors in the UWB positioning system, and effectively couples the tag positioning data, gyroscope data, and odometer data by using algorithms such as Kalman filtering and first-order low-pass filtering. While improving the positioning and heading angle accuracy, it ensures the continuity of data output and improves the reliability and robustness of the navigation positioning system; the positioning data p fusion [x, y] and the heading angle data yaw fusion can be combined into the fused pose data pose fusion [x, y, yaw], and this data can provide positioning and navigation support for various indoor and outdoor mobile robots.

[0070] The above description is only an overview of the technical solution of the present invention. In order to enable relevant technical personnel to more clearly understand the technical means of the present invention and implement it in accordance with the content of the specification, the present invention will be further described below with reference to the accompanying drawings. Description of the Drawings

[0071] Figure 1 is the structural diagram of the mobile robot navigation and positioning system based on multi-sensor fusion;

[0072] Figure 2 is the flow chart of the mobile robot navigation and positioning method based on multi-sensor fusion;

[0073] Figure 3 is the visualization diagram of the positioning result of the mobile robot navigation and positioning system based on multi-sensor fusion;

[0074] In the figure: 1. UWB base station system; 11. UWB base station; 2. Combined tag device; 21. Tag A; 22. Tag B; 23. Gyroscope; 3. Main control device. Detailed Embodiments

[0075] Refer Figure 1 As shown, this embodiment provides a mobile robot navigation and positioning system based on multi-sensor fusion, including:

[0076] The UWB base station system 1 for jointly completing TWR ranging with tags, which includes at least 4 UWB base stations 11 installed in the working area of the mobile robot in a given manner; the number and installation position of the UWB base stations 11 can be flexibly set according to the actual application scenario, but the following basic principles should be followed:

[0077] 1) The maximum effective transmission distance of the UWB base station 11 is generally about 50m - 100m, and it should be ensured that tags can receive the signals of at least 4 UWB base stations 11 at any position in the working area;

[0078] 2) The layout shape of the UWB base station 11 should be as close as possible to a rectangle with an aspect ratio of 1:1, and the maximum aspect ratio should not exceed 3:1;

[0079] 3) The UWB base station 11 should be arranged as far away from the metal reflecting surface as possible. If it is restricted by the actual site conditions and cannot be achieved, absorbing materials can be pasted on the reflecting surface;

[0080] 4) The distance between the installation point of the UWB base station 11 and the corner should be greater than 1.0 m, and the distance from the wall surface should be greater than 0.5 m;

[0081] 5) The UWB base stations 11 should be installed on the same plane as much as possible, and ensure that the distance between the plane of the UWB base station 11 and the working plane of the tag is greater than 1.0 m;

[0082] The combined tag device 2 composed of the tag A21, the tag B22 and the gyroscope 23, where the tag A21 and the tag B22 are installed at the left and right ends of the connection component at an interval of about 0.8 m, and the gyroscope 23 is installed at the center position of the connection component; the connection component can be selected as a rigid bar-shaped material with dimensions of about 820x30x3 mm. The actual size can be determined according to factors such as the size of the robot body and the tag distance interval, as long as the connection is stable and reliable;

[0083] The main control device 3 is used to receive the data of the combined tag device 2, the data of the gyroscope 23, and the mobile robot odometer data, and perform real-time fusion processing on the received data, and output the current positioning data and heading angle data of the combined tag device 2; on the one hand, the main control device 3 should include a sufficient number of suitable hardware connection ports, which can be used to connect sensors or data input interfaces such as UWB tags, gyroscopes 23, and vehicle-mounted odometers and complete data interaction; on the other hand, the main control device 3 should have sufficient data storage, data caching, and data operation resources to meet the operation requirements of the navigation and positioning method proposed in this invention; the following are the parameters of the embedded main control device used in this embodiment:

[0084] 1. Hardware interface: UARTx4, CANx2, IOx8

[0085] 2. CPU: ARMCortex-A7 core, running frequency 900 MHz, 128 KB L2 cache

[0086] 3. RAM: 16-bit LP-DDR2, DDR3 / DDR3L 1 GB

[0087] 4. ROM: 16-bit parallel NORFLASH / PSRAM 16 GB

[0088] Parameter Figure 2As shown, this embodiment provides a mobile robot navigation and localization method based on multi-sensor fusion, including the following steps:

[0089] S1: Construct a sliding window filter to detect and filter the distance data from the tag containing NLOS to each UWB base station 11, and obtain the filtered distance data from the tag to each UWB base station 11. This includes the following specific steps:

[0090] S11: Obtain distance data from tags to each UWB base station 11 in real time using the tags, and store the distance data into corresponding arrays according to the UWB base station 11 ID. Assuming there are m UWB base stations 11 and the sliding window size is set to n, then m distance arrays of length n can be obtained: D1, D2...D m Where D = D(1), D(2)...D(n),

[0091] The tag data feedback frequency used in this embodiment is 20Hz, and the corresponding sliding window size n is set to 20.

[0092] S12: Let one of the arrays be D. i Then, the absolute value array d of the distance gradient is first calculated using the following formula. i :

[0093] d i (n)=|D i (n)-D i (n-1)|

[0094] Calculate d i The standard deviation is used to determine whether the value is greater than or less than a set threshold. If it is less than the set variance threshold, it indicates a distance from array D. i There are no significant numerical jumps, and it is minimally affected by NLOS noise; the corresponding distance data can remain at their original values. If it exceeds the set placement threshold, it indicates that the distance array D... i There are significant numerical jumps, which are greatly affected by NLOS. Data with large gradient changes are more likely to contain NLOS noise, which needs to be identified and removed, as shown in S13.

[0095] S13: Set the standard deviation threshold sd_threshold = 0.2m and the gradient threshold gradient_threshold = 0.3m. Smaller threshold values ​​make it easier to remove data containing NLOS errors, but removing NLOS data may also remove some valid data, resulting in insufficient usable data. Therefore, this parameter needs to be set appropriately based on the actual test conditions; iterate through the gradient array d whose standard deviation is greater than sd_threshold. i If the j-th value d i(j) If the distance is less than the set gradient_threshold, the original distance value is retained, i.e., FD. i (j)=D i (j), if d i (j) If the distance value is greater than the set gradient_threshold, then the distance value at that location is discarded and replaced with the previous value, i.e., FD i (j)=D i (j-1);

[0096] S14: For each distance array D1, D2...D m Repeating steps s12 and s13 will yield distance data FD1, FD2...FD from each UWB base station 11, with NLOS noise largely filtered out. m .

[0097] Furthermore, this invention proposes an improved least squares trilateration algorithm: S2: Divide the filtered distance data into multiple groups of three, calculate the positioning data of multiple groups using the least squares method, calculate the confidence score of each group of positioning data, and obtain the final tag positioning data by weighting according to the confidence score. The specific implementation steps are as follows:

[0098] S21: Divide the m UWB base stations 11 in the current positioning area into k groups of 3 according to the permutation and combination algorithm, and calculate the coordinates of each group using the least squares algorithm. The general formula for least squares is as follows:

[0099] X = (A T A) -1 A T b

[0100] Where X is the matrix to be solved, and in a 2D planar trilateration system, its specific form is X = x, y

[0101] Where A is the parameter matrix, and in a 2D planar trilateration system, A takes the following form:

[0102]

[0103] In the above formula, x1,y1, x2,y2, and x3,y3 are the x-axis and y-axis coordinates of the three UWB base stations 11, respectively.

[0104] Where b is a parameter matrix, and in a 2D planar trilateration system, b takes the following form:

[0105]

[0106] In the above formula, x1, y1, x2, y2, and x3, y3 are the x-axis and y-axis coordinates of the three UWB base stations 11, respectively, and s1, s2, and s3 are the horizontal distances from the tag to the three UWB base stations 11, respectively. These distances can be calculated based on the tag installation height and the distance from the tag to the UWB base station 11, using the following formula:

[0107]

[0108] In the above formula, d represents the straight-line distance from the tag to the specified IDUWB base station 11, which can be obtained from the filtered distance data output by S1: FD1, FD2...FD m The distance from the tag to the UWB base station 11 is obtained as follows: for example, if ID = 1 and the latest data is taken as the measurement result, then the distance from the tag to the UWB base station 11 is: d = FD1(n); z is the z-axis coordinate of the current UWB base station 11, and h is the installation height of the tag.

[0109] S22: By substituting the specific values ​​of each parameter into the corresponding formula according to the calculation method provided in S21, k positioning data sets can be obtained: p1, p2...p k A common algorithm is to directly average the data set as the final positioning result. While simple, this method cannot effectively remove positioning data containing NLOS noise, resulting in low positioning accuracy. This invention proposes a least squares calculation result evaluation algorithm to calculate the confidence level of each positioning result, remove data with low confidence levels, and then calculate a weighted average of the remaining data according to their confidence levels to output the final positioning result. This can further reduce the interference of NLOS noise and improve positioning accuracy. The specific implementation method is as follows:

[0110] 1. First, calculate the sum of squares of the differences between the distance from the location point to each UWB base station and the distance measured by the tag. The formula used is as follows:

[0111] In the above formula, x p ,y p For the positioning data of one of the UWB base station combinations 11, x i ,y i Let be the x-axis and y-axis coordinates of the i-th UWB base station 11, where i∈1,2,3, and s i Let be the horizontal distance from the tag to the i-th UWB base station 11. Theoretically, the more accurate the distance measurement, the more accurate the positioning result calculated by the least squares solution. The smaller the sum value, the larger the measurement error. The larger the positioning error, the larger the sum value. For ease of later calculation and intuitive display, the sum value is transformed by a formula to obtain the confidence level c of the positioning result. The formula is as follows:

[0112]

[0113] In the above formula, c is the confidence level of the positioning result, and its value ranges from (0,1). The higher the confidence level, the smaller the possible positioning error.

[0114] 3. Based on the method given in S1, perform a localization analysis on the array p1, p2...p k By calculating the confidence scores of each location data point, the corresponding confidence score arrays c1, c2...c can be obtained. k By iterating through the array and removing data with confidence scores below a set threshold while keeping data with confidence scores above the set threshold, a new confidence array c1, c2...c can be obtained. n and the corresponding location result array p1, p2...p n When n = 0, it means that the confidence level of all positioning results is less than the set threshold, and the positioning solution fails. When 1 ≤ n ≤ k, the final positioning result p is obtained by weighting the array of positioning results according to their confidence levels. mean The formulas for calculating x and y are as follows:

[0115]

[0116] In this embodiment, the location confidence threshold is set to 0.4. The smaller the confidence threshold is set, the more stringent the requirements for the location results are, and the higher the probability of removing location data containing NLOS errors. However, this may also lead to an increased probability of tag location failure. Of course, this invention integrates odometer data, and tag location failure within a short period of time will not affect the system's fused location output. However, tag location data is still needed to correct the accumulated odometer error. Therefore, this parameter cannot be set too small to ensure that enough effective tag location data can be obtained.

[0117] S23: Performing operations S1, S21, and S22 on labels A and B respectively will yield the location data for both labels: p tag1 x, y, p tag2 x, y;

[0118] Further, S3: Calculate the heading angle of the combined tag device 2 using the positioning data from the two tags, and obtain the fused heading angle data by coupling the tag heading angle data and the angular velocity of the gyroscope 23 using a Kalman filter algorithm; specifically, this includes the following steps:

[0119] S31: Calculate the vector angle between tag B and tag A based on the coordinates of tag A and tag B on the map. Then, based on the installation angle of the tag connection component relative to the robot's X-axis, the robot's heading angle can be calculated. Since the tag positioning data will fluctuate within a certain error range, and the tag vector magnitude is limited, the calculated heading angle data will contain a large amount of high-frequency noise and cannot be directly used for robot navigation. However, from the perspective of the global map, the long-term cumulative error of this heading angle is close to 0. Therefore, it can be used as the observation input for the Kalman filter to calibrate the cumulative error generated by the gyroscope's 23-integral step. The specific calculation formula is as follows:

[0120]

[0121] In the above formula, yaw mes For the vehicle's observed heading angle output, (x tag1 ,y tag1 ), (x tag2 ,y tag2 ) are the location data p for labels A and B, respectively. tag1 x, y, p tag2 x, y, and fix_angle are the mounting angles of the label connection component relative to the X-axis of the robot body;

[0122] S32: Heading angle calculated using two-tag positioning. mes As the measurement input, the gyroscope's angular velocity ω is used as the prediction input. A Kalman filter equation system is constructed for heading angle fusion. In this system, the state transition matrix, control matrix, and measurement matrix are all 1×1 identity matrices. Substituting them into the standard Kalman filter equation system yields the simplified Kalman filter equation system:

[0123] X(k|k-1)=X(k-1|k-1)+U(k)

[0124] The above equation is the state prediction equation for the Kalman system, where X is the fused heading angle yaw. fusion X(k|k-1) is the optimal estimate of the current system, i.e., the optimal estimate of the current heading angle. X(k-1|k-1) is the optimal estimate of the previous system. U(k) is the control variable of the system. In this system, U(k) = ω × Δt, where ω is the angular velocity value returned by the gyroscope 23, and Δt is the Kalman filter update time interval.

[0125] P(k|k-1)=P(k-1|k-1)+Q

[0126] The above equation is the Kalman system covariance prediction equation, where P(k|k-1) is the optimal estimate of the prior covariance of the system in this case, P(k-1|k-1) is the optimal estimate of the prior covariance of the system in the previous case, and Q is the process noise covariance matrix of the system. In this system, Q is the angular velocity measurement noise of the gyroscope 23, and its magnitude is set to 0.0001.

[0127]

[0128] The above equation is the Kalman gain calculation equation, where K(k) represents the Kalman gain of the system at time k, P(k-1|k-1) is the previous optimal estimate of the system's prior covariance, and R is the system's measurement noise covariance matrix. In this system, R is the tag heading angle yaw. mes The measurement noise is set to a value of 10.0;

[0129] X(k|k)=X(k-1|k-1)+K(k)Z(k)-X(k|k-1)

[0130] The above equation is the optimal state estimation equation for a Kalman system, where X(k|k) is the current optimal state estimate of the system, i.e., the current fused heading angle yaw. fusion X(k|k-1) is the optimal state estimate of the system at time k-1, K(k) is the Kalman gain of the system, X(k-1|k-1) is the optimal state estimate of the system at time k-2, and Z(k) is the measured value of the system. In this system, Z(k) = yaw mes ;

[0131] P(k|k)=1-K(k)P(k|k-1)

[0132] The above equation is the posterior covariance equation for the Kalman system, where P(k|k) is the posterior covariance of the system in this case, K(k) is the Kalman gain of the system in this case, and P(k|k-1) is the optimal estimation result of the posterior covariance of the system at time k-1.

[0133] S33: Repeating S31 and S32 to iteratively update the Kalman filter yields the real-time fused heading angle yaw. fusion .

[0134] Further, S4: Take the average of the two tag positioning data as the positioning data of the combined tag device 2, and then use a first-order low-pass filtering algorithm to couple the tag positioning data, odometer data, and heading angle data to obtain the fused positioning data p. fusion Specifically, it includes the following steps:

[0135] S41: Take the average of the positioning data of the two tags as the positioning data p of the combined tag device 2. tag ,Right now:

[0136]

[0137] S42: Preprocess the odometer feedback data to ensure that the odometer input data format is the instantaneous speed relative to the vehicle body at the previous moment. Then, it is transferred to the center of the tag connection component to obtain the instantaneous velocity of the combined tag device 2 relative to the vehicle body at the previous moment. Considering only two-dimensional planar motion, based on the fused heading angle yaw fusion The Euler angles (yaw) of the vehicle relative to the map coordinate system can be obtained. fusion For ease of calculation, the expression (,0,0) can be converted into an attitude quaternion q, and then the quaternion can be used to... Perform a coordinate transformation to determine the velocity of the combined label device 2 in the map coordinate system. The calculation formula is as follows:

[0138]

[0139] In the above formula, q represents the four attitude elements. -1 It is the inverse of a quaternion;

[0140] S43: Employ a first-order low-pass filtering algorithm to couple tag positioning data p tag and the tag movement speed calculated by the odometer The calculation formula is as follows:

[0141]

[0142] In the above formula, p fusion (t) represents the fused positioning data at the current moment, p fusion (t-1) represents the fused positioning data from the previous time step, p tag (t) Tag location data, p tag (t-1) represents the label location data from the previous time step. Δt is the speed of the combined tag device 2 in the map coordinate system at the current moment, Δt is the time interval between time t and time t-1, w is the odometer data weighting coefficient, and its value range is [0,1]. The larger w is set, the greater the influence of odometer data on fused positioning data and the smaller the influence of tag positioning data on fused positioning data. The specific value should be set according to the accuracy of odometer and the application scenario. The typical value is 0.1.

[0143] According to the above technical solution, the real-time positioning data p of the positioning device can be obtained by repeatedly executing S1, S2, S3 and S4. fusion [x, y] and heading angle data yaw fusion .

[0144] Experiments have shown that the method provided by this invention can effectively remove TWR distance data and positioning data containing NLOS errors in UWB positioning systems. Furthermore, it employs algorithms such as Kalman filtering and first-order low-pass filtering to effectively couple tag positioning data, gyroscope data, and odometer data. This improves positioning and heading angle accuracy while ensuring data output continuity, thereby enhancing the reliability and robustness of the navigation and positioning system. The positioning data p output by this system... fusion [x,y] and heading angle data yaw fusion It can be combined into fused pose data. fusion [x, y, yaw], this data can provide positioning and navigation support for various indoor and outdoor mobile robots. See the example running results. Figure 3 .

[0145] The above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention. It should be noted that for those skilled in the art, several improvements and modifications can be made without departing from the technical principles of the present invention, and these improvements and modifications should also be considered within the scope of protection of the present invention.

Claims

1. A navigation and localization method for mobile robots based on multi-sensor fusion, characterized in that, include: The UWB base station system includes at least four UWB base stations installed within the working area of ​​the mobile robot; A combined tagging device includes at least two UWB tags spaced apart on a connecting component and a gyroscope for measuring the rotational angular velocity of the connecting component, the gyroscope being mounted at the center of the connecting component. The main control device includes a tag interface, a gyroscope interface, and an odometer interface. It is used to receive data from the tag device, gyroscope data, and mobile robot odometer data, and to perform real-time fusion processing on the received data to output the current tag device positioning data and heading angle data. The mobile robot's odometer is connected to the main control device via an odometer interface. The method includes the following steps: S1: Construct a sliding window filter to detect and filter the distance data from the tag containing NLOS to each base station, and obtain the filtered distance data from the tag to each base station. S2: Divide the distance data in S1 into multiple groups of 3, solve the multiple groups of positioning data using the least squares trilateration algorithm, and obtain the tag positioning data by weighting according to the confidence level. Use this algorithm to obtain the positioning data of tag A and tag B respectively. S3: Calculate the heading angle of the tag device using the positioning data of tag A and tag B, and obtain the fused heading angle data by coupling the tag heading angle data and the gyroscope angular velocity using the Kalman filter algorithm; S4: Take the average of the positioning data of tag A and tag B as the positioning data of the combined tag device, and then use a first-order low-pass filtering algorithm to couple the tag positioning data, odometer data and heading angle data to obtain the fused positioning data. ; S1 includes the following steps: S11: Obtain the distance data from the tag to each base station in real time through the tag, and store the distance data into the corresponding array according to the base station ID. Assuming there are m base stations and the sliding window size is set to n, then m distance arrays of length n can be obtained: ,in ; S12: Let one of the arrays be... First, calculate the absolute value array of the distance gradient using the following formula. : ； calculate The standard deviation is used to determine whether the value is greater than or less than a set standard deviation threshold. If it is less than the set standard deviation threshold, it indicates that the distance from the array is significant. There are no significant numerical jumps, and the impact of NLOS noise is minimal; therefore, the distance data can remain at its original value. If it exceeds the set standard deviation threshold, it indicates that the distance array... There are significant numerical jumps, which are greatly affected by NLOS. Data with large gradient changes are more likely to contain NLOS noise, which needs to be identified and removed. The method is step S13. S13: Iterate through the gradient array whose standard deviation is greater than the set standard deviation threshold. If the j-th value If the distance is less than the set gradient threshold, the original distance value is retained. ,like If the distance value is greater than the set gradient threshold, the original distance value is discarded and replaced with the previous value. ; S14: For each distance array Repeating steps s12 and s13 will yield distance data from tags to each base station that has been largely filtered out of NLOS noise. .

2. The mobile robot navigation and positioning method based on multi-sensor fusion according to claim 1, characterized in that, S2 includes the following steps: S21, according to the permutation and combination algorithm, divide the m base stations in the current positioning area into k groups of 3, and use the least squares algorithm to calculate the coordinates of each group respectively; S22, following the calculation method provided in S21, substitute the specific values ​​of each parameter into the corresponding formula to calculate and obtain k sets of positioning data: The least squares calculation result evaluation algorithm is used to calculate the confidence of each positioning result, and the data with low confidence are removed. The remaining data are then weighted and averaged according to their confidence levels to output the final positioning result. S23, performing operations S1, S21, and S22 on label A and label B respectively will yield the location data for label A and label B: , .

3. The mobile robot navigation and positioning method based on multi-sensor fusion according to claim 2, characterized in that: S3 includes the following steps: S31: Calculate the vector angle between label B and label A based on the coordinates of label A and label B on the map. Then, based on the installation angle of the label connecting component relative to the robot's X-axis, the robot's heading angle can be calculated. The specific calculation formula is as follows: ； In the above formula, For vehicle body observation heading angle output, , Location data for label A and label B respectively , fix_angle is the mounting angle of the label connection component relative to the X-axis of the robot body; S32: Heading angle calculated using two-tag positioning For measurement input, gyroscope angular velocity To predict the input, a Kalman filter equation system is constructed for heading angle fusion. In this system, the state transition matrix, control matrix, and measurement matrix are all of size . Substituting the identity matrix into the standard Kalman filter equations yields the simplified Kalman filter equations: ； The above equation is the state prediction equation for the Kalman filter system, where X is the fused heading angle. , The state prediction value at time k. This is the optimal state estimate at time k-1. For the control variables of the system, in this system , The gyroscope measures the returned angular velocity value. The Kalman filter update time interval; ； The above equation is the covariance prediction equation for a Kalman filter system. Let k be the prediction covariance. Let Q be the estimated covariance at time k-1, and let Q be the process noise covariance matrix of the system. In this system, Q is the gyroscope angular velocity measurement noise. ； The above equation is the Kalman gain calculation equation, where Let R represent the Kalman gain at time k, and R be the measurement noise covariance matrix of the system. In this system, R is the tag heading angle. Measurement noise; ； The above equation is the optimal state estimation equation for a Kalman filter system, where The optimal state estimate at time k is the current fused heading angle. , Let k be the observation value at time k in this system. ; ； The above equation is the posterior estimation covariance equation for a Kalman filter system, where Let $\mathbf{k}$ be the estimated covariance at time $k$. S33: Repeat steps S31 and S32 to iteratively update the Kalman filter, and the real-time fused heading angle can be obtained. .

4. The mobile robot navigation and positioning method based on multi-sensor fusion according to claim 3, characterized in that: The heading angle Acquired via a single tag with an array antenna.

5. A mobile robot navigation and positioning method based on multi-sensor fusion according to claim 4, characterized in that: S4 includes the following steps: S41: Take the average of the positioning data from the two tags as the positioning data for the combined tag device. ,Right now: ； S42: Preprocess the odometer feedback data to ensure that the odometer input data format is the instantaneous speed relative to the vehicle body at the previous moment. The speed of the combined tag device relative to the vehicle body at the previous moment is obtained by transferring the speed to the center of the tag connection component. Considering only two-dimensional planar motion, based on the fused heading angle The Euler angles of the vehicle body relative to the map coordinate system can be obtained. To facilitate calculation, it can be converted into an attitude quaternion q, and then the attitude quaternion can be used to... Perform a coordinate transformation to determine the velocity of the combined label device in the map coordinate system. The calculation formula is as follows: ； In the above formula, q represents the four attitude elements. It is the inverse of a quaternion; S43: Use a first-order low-pass filtering algorithm to couple tag positioning data. and the tag movement speed calculated by the odometer The calculation formula is as follows: ； In the above formula To integrate location data at the current moment, To integrate the location data from the previous moment, Tag location data, This is the tag location data from the previous moment. The velocity of the combined label device in the map coordinate system at the current moment. Let t be the time interval between time t and time t-1, and w be the odometer data weighting coefficient.

6. A mobile robot navigation and positioning method based on multi-sensor fusion according to claim 5, characterized in that: The odometer data sources include single steering wheel, dual steering wheel, dual-wheel differential, McCann wheel, or Ackermann structure.

7. The mobile robot navigation and localization method based on multi-sensor fusion according to claim 1, characterized in that: The UWB base station is divided into different positioning areas, which include indoor, outdoor, or both indoor and outdoor areas.

8. A mobile robot navigation and positioning method based on multi-sensor fusion according to claim 1, characterized in that: The installation distance between the UWB tags is set according to the size of the robot body; the gyroscope is an inertial navigation device that includes a gyroscope and an accelerometer.

Images