# Position Estimation and Error Correction of Mobile Robots Based on UWB and Multisensors

Authors: Jie Li; Jiameng Xue; Di Fu; Chao Gui; Xingsong Wang
Journal: Journal of Sensors
 (2022)

---

## Abstract

Since there are many interferences in the indoor environment, it is difficult to achieve the precise positioning of the mobile robot using a single sensor. This paper presents a position estimation and positioning error correction method of mobile robots based on multisensor data. The robot’s positioning sensor includes ultra-wideband (UWB) components, inertial measurement unit (IMU), and encoders. UWB multipath interference causes more ranging errors, which can be reduced by the correction equation after data fitting. The real-time coordinates of the UWB robot tag can be calculated based on multiple UWB anchor data and the least squares method. The coordinate dataxc,yc are acquired by UWB positioning subsystem, and the velocity data ẋc,ẏc are collected by IMU together with encoders. The multisensor data continuously update Kalman filter and estimate robot position. In the positioning process, the positioning data of different sensors can be mutually corrected and supplemented. The results of UWB ranging correction experiments indicate that data fitting can improve the UWB positioning accuracy. In the multisensor positioning experiments, compared with a single sensor, the positioning method based on data fusion of UWB, IMU, and encoders has higher accuracy and adaptability. When UWB signals are interfered or invalid, other sensors can still work normally and complete the robot positioning process. The multisensor positioning method not only improves the robot positioning accuracy but also has stronger environmental adaptability.


---

## Body

## 1. Introduction

Various positioning methods and sensors have been developed and applied to different fields, such as underground garages, smart factories, airports, and restaurants. In outdoor positioning, Global Positioning System (GPS) is an important positioning method, which can perform ranging and positioning of cars, ships, and mobile phones through satellites. However, due to the interference of obstacles (such as roofs, walls, and furniture) in the indoor environment, it is more difficult to achieve high-precision positioning. Positioning of indoor mobile robots has also become one of the research hotpots in recent years. Using different positioning sensors, the robot can obtain more accurate position information. Accurate positioning and navigation can improve the automation and intelligence of mobile robots.There are many indoor positioning methods with different sensors, such as Radio-Frequency Identification (RFID) positioning [1], ultrasonic positioning, Bluetooth positioning, wireless sensor networks (WSN) [2, 3], UWB positioning [4], Inertial Navigation System (INS), and Concurrent Mapping and Localization (CML/SLAM). The combination of ranging sensors is beneficial to improve the positioning and navigation accuracy of mobile robots, for example, a localization and pose estimation method for mobile robot navigation using passive RFID [5], a lightweight indoor robot positioning system which operated on cost-effective WiFi-based received signal strength (RSS) [6], a novel accurate hybrid global self-localization algorithm for mobile robots based on ultrasonic localization system [7], and an efficient indoor localization algorithm based on a light detection and ranging (LiDAR) device. The UWB positioning method has been gradually applied due to the advantages of long positioning distance and high stability [8–11].Compared with other positioning methods, UWB positioning devices can be installed and implemented in a short time. UWB positioning technology utilizes distributed sensor devices for distance estimation. The distances between multiple UWB anchors and the UWB robot tag are measured simultaneously to estimate the robot position. The positioning methods include the Angle of Arrival (AOA) algorithm [12], the received signal strength (RSS) algorithm [13], the Time of Arrival (TOA) algorithm [14], the Time Difference of Arrival (TDOA) algorithm [15], and other hybrid algorithms [16–18]. The stability and accuracy of measured data are critical to the robot positioning. However, in the indoor environment, obstacle interference may reduce UWB positioning accuracy and even cause UWB positioning failure. Usually, increasing the number of UWB anchors can expand its positioning range, but it will also bring more errors and cause instability. Therefore, the error correction of UWB positioning data is necessary to improve its positioning accuracy.It is difficult for a single sensor to achieve high accuracy due to sensor error or instability. The combined applications of multiple sensors are beneficial to improve positioning accuracy and stability. A method to accurately locate persons indoors by fusing INS with active RFID was presented in [19]. A method to integrate LiDAR and IMU was proposed to overcome the problem of the low accuracy and large accumulated errors of indoor mobile navigation and positioning [20]. In order to improve the accuracy of the data fusion filter, a tightly coupled UWB/INS-integrated scheme for indoor human navigation was investigated [21]. UWB has also been combined with other sensors for data fusion to achieve more precise positioning [22, 23].Due to the interference of obstacles in the indoor environment, data fusion and filtering of UWB and multisensors are developed to improve the positioning accuracy of mobile robots. This paper proposed a position estimation and error correction method based on multisensor data. The robot positioning system can simultaneously acquire and integrate the data from UWB, IMU, and encoders. The double-sided two way ranging (DS-TWR) and data fitting method is used to improve the UWB ranging accuracy and correct errors caused by multipath signals. Meanwhile, the robot position coordinates were estimated by UWB anchors and the robot UWB robot tag. In the process of multisensor robot positioning, the heading angle error of IMU can be corrected by UWB positioning data. The velocity data calculated by IMU together with encoders and the UWB positioning data continuously update Kalman filter to estimate the robot position. The multisensor positioning method is conducive to improve the robot positioning accuracy. Even if UWB signals are interfered or shielded, the robot positioning can still be executed by other sensor data, which improves the practicability and anti-interference ability of the robot positioning system.


## 2. Positioning System Design

### 2.1. Positioning System Architecture

In a complex and multi-interference environment, multisensor cooperation has the ability of improving positioning accuracy. As shown in Figure1, the robot positioning system consists of a UWB positioning subsystem, a remote control computer, and a mobile robot. The UWB positioning subsystem includes a UWB robot tag and some UWB anchors, which are fixedly installed in the surrounding environment. Based on the measured distance between the UWB robot tag and UWB anchors, the remote control computer executes positioning algorithms to calculate the position and coordinate of the robot.Figure 1
Robot positioning system.The remote control computer communicates with the robot through the wireless module. It is used to process positioning data, calculate robot’s coordinate, and control the mobile robot. The main functions include interactive communication, robot control, position estimation, data fusion and processing, and robot position display.A mecanum wheeled mobile robot is considered in this work. As shown in Figure2, the mobile robot consists of a central controller, four DC motors, four drivers, four encoders, and four mecanum wheels. The encoders are used to measure the rotational speed of each wheel and calculate the robot’s velocity in self coordinate system based on the kinematic equations. An IMU sensor, composed of a three-axis accelerometer, a three-axis gyroscope, and a three-axis magnetometer, is installed on the robot. This sensor is used to estimate the heading angle of the robot during driving.Figure 2
Mecanum wheeled mobile robot and positioning sensors.In terms of sensors that we used in the robot positioning system, the positioning data (i.e., the robot’s position and velocity) are obtained by three different sensors: UWB, encoders, and IMU. Each sensor inevitably has some errors during the positioning process, which may contain one or more of the following:(1)
During the running process, if the robot wheel slips, their rotational speeds measured by the encoders will have some errors. The heading angle and mileage of the robot integrated by velocities also have cumulative errors, which will increase over time(2)
The robot’s heading angle calculated by the IMU has less error in a short operating time. However, if the correction is not carried out for a long time, larger cumulative angle errors will occur(3)
UWB positioning is a direct positioning method; the positioning error will not accumulate over time. However, the accuracy and stability of UWB positioning are not as good as expected. Besides, UWB positioning may be invalid due to the interference of obstacles in the environmentAs discussed above, different positioning sensors have their own errors and shortcomings; we proposed a multisensor data fusion algorithm. By fusing the data of UWB, IMU, and encoders, the algorithm can effectively improve positioning accuracy, reduce cumulative errors, and eliminate obstacle interference. The positioning data obtained by the sensors are mutually corrected and supplemented. On the one hand, encoders and IMU have higher accuracy and freedom from obstacles in a short time but have larger cumulative errors. The cumulative angle errors can be reduced by using UWB positioning data, which will correct the heading angle calculated by the IMU. On the other hand, due to environmental interference, the positioning error of UWB may become larger. The UWB’s positioning error can be corrected through the IMU and encoder data as well, which will improve the robot positioning accuracy.


### 2.2. Software Development

As shown in Figure3, we developed a software interface of the robot system. Visualized software is developed on the remote control computer, and the main functions include coordinate display of UWB anchors, real-time coordinate and trajectory display of the robot, robot control, and data processing.Figure 3
Software interface of the robot positioning system.


## 2.1. Positioning System Architecture

In a complex and multi-interference environment, multisensor cooperation has the ability of improving positioning accuracy. As shown in Figure1, the robot positioning system consists of a UWB positioning subsystem, a remote control computer, and a mobile robot. The UWB positioning subsystem includes a UWB robot tag and some UWB anchors, which are fixedly installed in the surrounding environment. Based on the measured distance between the UWB robot tag and UWB anchors, the remote control computer executes positioning algorithms to calculate the position and coordinate of the robot.Figure 1
Robot positioning system.The remote control computer communicates with the robot through the wireless module. It is used to process positioning data, calculate robot’s coordinate, and control the mobile robot. The main functions include interactive communication, robot control, position estimation, data fusion and processing, and robot position display.A mecanum wheeled mobile robot is considered in this work. As shown in Figure2, the mobile robot consists of a central controller, four DC motors, four drivers, four encoders, and four mecanum wheels. The encoders are used to measure the rotational speed of each wheel and calculate the robot’s velocity in self coordinate system based on the kinematic equations. An IMU sensor, composed of a three-axis accelerometer, a three-axis gyroscope, and a three-axis magnetometer, is installed on the robot. This sensor is used to estimate the heading angle of the robot during driving.Figure 2
Mecanum wheeled mobile robot and positioning sensors.In terms of sensors that we used in the robot positioning system, the positioning data (i.e., the robot’s position and velocity) are obtained by three different sensors: UWB, encoders, and IMU. Each sensor inevitably has some errors during the positioning process, which may contain one or more of the following:(1)
During the running process, if the robot wheel slips, their rotational speeds measured by the encoders will have some errors. The heading angle and mileage of the robot integrated by velocities also have cumulative errors, which will increase over time(2)
The robot’s heading angle calculated by the IMU has less error in a short operating time. However, if the correction is not carried out for a long time, larger cumulative angle errors will occur(3)
UWB positioning is a direct positioning method; the positioning error will not accumulate over time. However, the accuracy and stability of UWB positioning are not as good as expected. Besides, UWB positioning may be invalid due to the interference of obstacles in the environmentAs discussed above, different positioning sensors have their own errors and shortcomings; we proposed a multisensor data fusion algorithm. By fusing the data of UWB, IMU, and encoders, the algorithm can effectively improve positioning accuracy, reduce cumulative errors, and eliminate obstacle interference. The positioning data obtained by the sensors are mutually corrected and supplemented. On the one hand, encoders and IMU have higher accuracy and freedom from obstacles in a short time but have larger cumulative errors. The cumulative angle errors can be reduced by using UWB positioning data, which will correct the heading angle calculated by the IMU. On the other hand, due to environmental interference, the positioning error of UWB may become larger. The UWB’s positioning error can be corrected through the IMU and encoder data as well, which will improve the robot positioning accuracy.


## 2.2. Software Development

As shown in Figure3, we developed a software interface of the robot system. Visualized software is developed on the remote control computer, and the main functions include coordinate display of UWB anchors, real-time coordinate and trajectory display of the robot, robot control, and data processing.Figure 3
Software interface of the robot positioning system.


## 3. UWB Positioning and Error Correction

### 3.1. UWB Ranging Method

The UWB positioning subsystem performs positioning and coordinate calculation by measuring the distances between the UWB tag and multiple UWB anchors. Time of Fly (TOF) method is used to measure the UWB signal flight distance; the formula is(1)Δd=Cu⋅Δt,where Cu is the UWB signal transmission speed, which approximately equals to the speed of light, and Δt is the time difference of signal flight.However, due to the inability to accurately synchronize the clock between the UWB robot tag and UWB anchors, the clock error will cause a large measurement error. For example, 1 ns clock deviation can cause a 30 cm ranging error. The DS-TWR method [24] was proposed to reduce the ranging error caused by clock unsynchronization and clock skews. As shown in Figure 4, the DS-TWR method can achieve more accurate UWB ranging by measuring the three round-trip times between the UWB robot tag and the UWB anchors. Figure 5 shows the working flowchart of the UWB robot tag and UWB anchors.Figure 4
UWB signal flight time diagram based on the DS-TWR method.Figure 5
Flowchart of the UWB robot tag and UWB anchors.The UWB signal flight timeTp between the UWB tag and the UWB anchor is
(2)Tp=Tround1×Tround2−Treply1×Treply2Tround1+Tround2+Treply1+Treply2.Because the signal flight timeTp is much shorter than the processing data time Treply1 and Treply2, the flight time of the signal after adding the clock errors approximately equals to
(3)T^p≈2⋅1+er1+ea1+er+1+eaTp,where er and ea are the clock skews of the UWB tag module and UWB anchor module, respectively.As a result, the ranging error by the DS-TWR method is(4)Te=T^p−Tp=er+ea+erea21+er1+eaT^p≈er+ea2T^p.Figure6 shows the DS-TWR ranging error curve between the UWB robot tag and the UWB anchors at different distances and different clock skews. The results indicate that the ranging errors caused by clock skews are much smaller than the UWB measurement accuracy (about 100 mm). When the measuring distance is 300 m and the clock skew is 20 PPM, the ranging error is about 6 mm. During the robot actual positioning process, the UWB module with 5 PPM clock was adopted to improve the measurement accuracy.Figure 6
UWB ranging errors at different clock skews.


### 3.2. Multipath Error Correction

As shown in Figure7(a), in a line-of-sight (LOS) environment [25], UWB signals can transmit in a straight line without any obstruction. However, multipath interference may exist due to the reflection of walls and objects. Signals often transmit through multiple paths and cause some ranging errors. As shown in Figure 7(b), paths A and B are direct paths, and paths C and D are multipath interference; their length relationship is D≈C>B≈A. Although UWB has a strong ability to resist multipath interference, it is difficult to guarantee its ranging accuracy due to its ultrawide bandwidth and interference of complex indoor reflection environment.Figure 7
Broadcast environment analysis of UWB signals: (a) LOS environment without obstructions; (b) UWB multipath interference.
(a)(b)The ranging error caused by multipath is related to the real environment. The method of data fitting is proposed to correct the error and improve the positioning accuracy. Assume that the relationship equation between the real distance and the UWB measurement distance is(5)dt=a0+a1dm+a2dm2+⋯+akdmk,where dt is the real distance value and dm is the UWB measured value. By measuring multiple sets of UWB data dmi,dti in the LOS environment, we can get
(6)dti=a0+a1dmi+a2dmi2+⋯+akdmik,i=1,2,⋯,n.SupposeAc=a0a1⋯akT and Dt=dt1dt2⋯dtnT; the matrix form of Equation (6) is
(7)Dt=DmAc,where Dm is
(8)Dm=1dm1⋯dm1k1dm2⋯dm2k1dm3⋮⋮1dm4⋯dmnk.Solving the matrix equation by the principle of least squares, we have(9)Ac=DmTDm−1DmTDt.By substituting the coefficient matrixAc=a0a1⋯akT into Equation (5), the relationship between the UWB measured distance and the real distance can be calculated. Data fitting is conducive to correcting the errors caused by UWB multipath and improving the ranging accuracy. A large number of experiments we have conducted indicate that when the data fitting equation is dt=a0+a1dm, the ranging accuracy is high enough to satisfy the operating requirements.


### 3.3. First Path Power

Due to the interference of UWB signals, the ranging accuracy of UWB anchors cannot be guaranteed. It means that if severely disturbed ranging data is taken into account, the positioning accuracy of the mobile robot will be greatly reduced. To solve this problem, the first path power (or direct path power) of UWB signals is collected to determine the availability of UWB ranging data, which can be calculated as(10)PF=10×log10F12+F22+F32N02−P0dBm,where F1, F2, F3, and N0 are internal register variables of the UWB module and P0 is a constant.The first path power can reflect the signal propagation state between the UWB robot tag and UWB anchors. When the UWB signal is blocked, the first path power will decrease significantly. Figure8 shows the first path power of the UWB signal under occlusion. In a noninterference environment, the first path power of the UWB signal is usually maintained between -76 dBm and -72 dBm. When the signal is blocked by obstacles, the first path power will drop dramatically to -82 dBm or lower.Figure 8
First path power of UWB signals under occlusion.By analyzing the first path power strength of UWB signals, the data availability of UWB anchors can be confirmed. For accurate positioning, the ranging data with power below a certain value will be discarded. It is considered that there are obstacles between the UWB robot tag and the UWB anchor; thus, this UWB anchor is invalid. If the number of available UWB anchors cannot satisfy the requirement of the robot positioning, UWB positioning will fail and the robot coordinate cannot be calculated.


### 3.4. UWB Anchor Layout Analysis

In the UWB positioning subsystem, multiple UWB anchor data are used simultaneously to calculate the robot position. The layout of UWB anchors has a significant impact on the robot positioning accuracy. For high-precision robot positioning, the layout and installation positions of the UWB anchors should meet the following principles:(1)
The operating area of the robot should be covered by at least 3 UWB anchor signals(2)
The signal coverage of UWB anchors should be fully utilized(3)
The horizontal dilution of precision (HDOP) of UWB positioning should be small enough and UWB anchors should avoid being clustered in a small areaIf the real distance between UWB anchors and the UWB robot tag isdir,i=1,2,3⋯ and the UWB ranging error is εε≥0, the measured distance can be expressed as
(11)dim∈dir−ε,dir+εi=1,2,3⋯.As shown in Figure9(a), assuming that three UWB anchors are located at points A1,A2, andA3, their measured distances are d1m, d2m, and d3m. The three circles, with points A1, A2, and A3 as the centers and d1m, d2m, and d3m as the radii, intersect to form Zone BCDEFG, which is the possible coordinate range of the robot. Its area SBCDEFG represents the robot positioning error.Figure 9
Geometrical relationship of UWB anchor layout: (a) measured distance error analysis; (b) layout analysis of UWB anchors.
(a)(b)Becauseε is small, Zone BCDEFG is approximately hexagon bcdefg in Figure 9(b), which is the circumscribed hexagon of circle T. The relative angles of three UWB anchors are α12, α23, and α13, respectively. According to its symmetry,
(12)STmbh≈STjekSTidj≈STlgmSTkfl≈SThci.The area of triangleTid is
(13)STid=ε22tanα232.Therefore, we have(14)STidj=ε2tanα232.Similarly,(15)STmbh=ε2tanα122,STkfl=ε2tanα132.The area of hexagonbcdefg is
(16)Sbcdefg=2ε2tanα132+tanα122+tanα232,α13+α12+α23=π.Whenα13=α12=α23, Sbcdefg has the smallest value:
(17)Sbcdefg=6ε213tanα132+tanα122+tanα232≥6ε2tanπ6.When three UWB anchors are used for robot positioning, the equilateral triangle layout is conducive to reducing positioning errors. Similarly, when the number of UWB anchors is four, the square layout is more favorable for the positioning.


## 3.1. UWB Ranging Method

The UWB positioning subsystem performs positioning and coordinate calculation by measuring the distances between the UWB tag and multiple UWB anchors. Time of Fly (TOF) method is used to measure the UWB signal flight distance; the formula is(1)Δd=Cu⋅Δt,where Cu is the UWB signal transmission speed, which approximately equals to the speed of light, and Δt is the time difference of signal flight.However, due to the inability to accurately synchronize the clock between the UWB robot tag and UWB anchors, the clock error will cause a large measurement error. For example, 1 ns clock deviation can cause a 30 cm ranging error. The DS-TWR method [24] was proposed to reduce the ranging error caused by clock unsynchronization and clock skews. As shown in Figure 4, the DS-TWR method can achieve more accurate UWB ranging by measuring the three round-trip times between the UWB robot tag and the UWB anchors. Figure 5 shows the working flowchart of the UWB robot tag and UWB anchors.Figure 4
UWB signal flight time diagram based on the DS-TWR method.Figure 5
Flowchart of the UWB robot tag and UWB anchors.The UWB signal flight timeTp between the UWB tag and the UWB anchor is
(2)Tp=Tround1×Tround2−Treply1×Treply2Tround1+Tround2+Treply1+Treply2.Because the signal flight timeTp is much shorter than the processing data time Treply1 and Treply2, the flight time of the signal after adding the clock errors approximately equals to
(3)T^p≈2⋅1+er1+ea1+er+1+eaTp,where er and ea are the clock skews of the UWB tag module and UWB anchor module, respectively.As a result, the ranging error by the DS-TWR method is(4)Te=T^p−Tp=er+ea+erea21+er1+eaT^p≈er+ea2T^p.Figure6 shows the DS-TWR ranging error curve between the UWB robot tag and the UWB anchors at different distances and different clock skews. The results indicate that the ranging errors caused by clock skews are much smaller than the UWB measurement accuracy (about 100 mm). When the measuring distance is 300 m and the clock skew is 20 PPM, the ranging error is about 6 mm. During the robot actual positioning process, the UWB module with 5 PPM clock was adopted to improve the measurement accuracy.Figure 6
UWB ranging errors at different clock skews.


## 3.2. Multipath Error Correction

As shown in Figure7(a), in a line-of-sight (LOS) environment [25], UWB signals can transmit in a straight line without any obstruction. However, multipath interference may exist due to the reflection of walls and objects. Signals often transmit through multiple paths and cause some ranging errors. As shown in Figure 7(b), paths A and B are direct paths, and paths C and D are multipath interference; their length relationship is D≈C>B≈A. Although UWB has a strong ability to resist multipath interference, it is difficult to guarantee its ranging accuracy due to its ultrawide bandwidth and interference of complex indoor reflection environment.Figure 7
Broadcast environment analysis of UWB signals: (a) LOS environment without obstructions; (b) UWB multipath interference.
(a)(b)The ranging error caused by multipath is related to the real environment. The method of data fitting is proposed to correct the error and improve the positioning accuracy. Assume that the relationship equation between the real distance and the UWB measurement distance is(5)dt=a0+a1dm+a2dm2+⋯+akdmk,where dt is the real distance value and dm is the UWB measured value. By measuring multiple sets of UWB data dmi,dti in the LOS environment, we can get
(6)dti=a0+a1dmi+a2dmi2+⋯+akdmik,i=1,2,⋯,n.SupposeAc=a0a1⋯akT and Dt=dt1dt2⋯dtnT; the matrix form of Equation (6) is
(7)Dt=DmAc,where Dm is
(8)Dm=1dm1⋯dm1k1dm2⋯dm2k1dm3⋮⋮1dm4⋯dmnk.Solving the matrix equation by the principle of least squares, we have(9)Ac=DmTDm−1DmTDt.By substituting the coefficient matrixAc=a0a1⋯akT into Equation (5), the relationship between the UWB measured distance and the real distance can be calculated. Data fitting is conducive to correcting the errors caused by UWB multipath and improving the ranging accuracy. A large number of experiments we have conducted indicate that when the data fitting equation is dt=a0+a1dm, the ranging accuracy is high enough to satisfy the operating requirements.


## 3.3. First Path Power

Due to the interference of UWB signals, the ranging accuracy of UWB anchors cannot be guaranteed. It means that if severely disturbed ranging data is taken into account, the positioning accuracy of the mobile robot will be greatly reduced. To solve this problem, the first path power (or direct path power) of UWB signals is collected to determine the availability of UWB ranging data, which can be calculated as(10)PF=10×log10F12+F22+F32N02−P0dBm,where F1, F2, F3, and N0 are internal register variables of the UWB module and P0 is a constant.The first path power can reflect the signal propagation state between the UWB robot tag and UWB anchors. When the UWB signal is blocked, the first path power will decrease significantly. Figure8 shows the first path power of the UWB signal under occlusion. In a noninterference environment, the first path power of the UWB signal is usually maintained between -76 dBm and -72 dBm. When the signal is blocked by obstacles, the first path power will drop dramatically to -82 dBm or lower.Figure 8
First path power of UWB signals under occlusion.By analyzing the first path power strength of UWB signals, the data availability of UWB anchors can be confirmed. For accurate positioning, the ranging data with power below a certain value will be discarded. It is considered that there are obstacles between the UWB robot tag and the UWB anchor; thus, this UWB anchor is invalid. If the number of available UWB anchors cannot satisfy the requirement of the robot positioning, UWB positioning will fail and the robot coordinate cannot be calculated.


## 3.4. UWB Anchor Layout Analysis

In the UWB positioning subsystem, multiple UWB anchor data are used simultaneously to calculate the robot position. The layout of UWB anchors has a significant impact on the robot positioning accuracy. For high-precision robot positioning, the layout and installation positions of the UWB anchors should meet the following principles:(1)
The operating area of the robot should be covered by at least 3 UWB anchor signals(2)
The signal coverage of UWB anchors should be fully utilized(3)
The horizontal dilution of precision (HDOP) of UWB positioning should be small enough and UWB anchors should avoid being clustered in a small areaIf the real distance between UWB anchors and the UWB robot tag isdir,i=1,2,3⋯ and the UWB ranging error is εε≥0, the measured distance can be expressed as
(11)dim∈dir−ε,dir+εi=1,2,3⋯.As shown in Figure9(a), assuming that three UWB anchors are located at points A1,A2, andA3, their measured distances are d1m, d2m, and d3m. The three circles, with points A1, A2, and A3 as the centers and d1m, d2m, and d3m as the radii, intersect to form Zone BCDEFG, which is the possible coordinate range of the robot. Its area SBCDEFG represents the robot positioning error.Figure 9
Geometrical relationship of UWB anchor layout: (a) measured distance error analysis; (b) layout analysis of UWB anchors.
(a)(b)Becauseε is small, Zone BCDEFG is approximately hexagon bcdefg in Figure 9(b), which is the circumscribed hexagon of circle T. The relative angles of three UWB anchors are α12, α23, and α13, respectively. According to its symmetry,
(12)STmbh≈STjekSTidj≈STlgmSTkfl≈SThci.The area of triangleTid is
(13)STid=ε22tanα232.Therefore, we have(14)STidj=ε2tanα232.Similarly,(15)STmbh=ε2tanα122,STkfl=ε2tanα132.The area of hexagonbcdefg is
(16)Sbcdefg=2ε2tanα132+tanα122+tanα232,α13+α12+α23=π.Whenα13=α12=α23, Sbcdefg has the smallest value:
(17)Sbcdefg=6ε213tanα132+tanα122+tanα232≥6ε2tanπ6.When three UWB anchors are used for robot positioning, the equilateral triangle layout is conducive to reducing positioning errors. Similarly, when the number of UWB anchors is four, the square layout is more favorable for the positioning.


## 4. Robot Position Estimation

### 4.1. UWB Coordinate Estimation

The UWB positioning subsystem includes UWB anchors and a UWB robot tag. UWB anchors are generally fixed at certain positions with known coordinates, and the UWB robot tag is installed on the robot. As shown in Figure10, there are some UWB anchors with known coordinatesxi,yi on a 2D plane: Anchor 1, Anchor 2, Anchor 3, ..., Anchor n, and the coordinate of the UWB robot tag is xc,yc. The distances from the UWB robot tag to the UWB anchors measured by the robot positioning system are dii=1,2,⋯,n. From the geometric relationship, we can get
(18)xc−x12+yc−y12=d12xc−x22+yc−y22=d22xc−x32+yc−y32=d32⋯xc−xn2+yc−yn2=dn2.Figure 10
UWB positioning and coordinate calculation.The following matrix equation can be obtained by Equation (18):
(19)AuXu=Du,where,
(20)Au=2x1−xn2y1−yn2x2−xn2y2−yn2x3−xn2y3−yn⋮2xn−1−xn2yn−1−yn,Du=x12−xn2+y12−yn2+dn2−d12x22−xn2+y22−yn2+dn2−d22x32−xn2+y32−yn2+dn2−d32⋮xn−12−xn2+yn−12−yn2+dn2−dn−12,Xu=xcyc.Because the UWB positioning subsystem has some positioning error, equation (19) can be corrected as
(21)AuXu+N=Du,where N is an error vector with n−1 dimensions.WhenN=Du−AuXu is the smallest, the measurement error will be the smallest. Supposefx=Du−AuXu, the least square estimate of X is
(22)Xu=AuTAu−1AuTDu.Therefore,xc,yc is the estimated coordinate value of the robot. When positioning the mobile robot in the plane, at least three UWB anchors are required. Generally, more UWB anchors can improve the robot positioning accuracy and stability. However, too many UWB anchor data may cause more calculations and other possible errors.


### 4.2. Robot Velocity Calculation

In the robot positioning process, the robot’s absolute coordinatexc,yc can be calculated by the UWB positioning subsystem. Meanwhile, the rotation speeds of the wheels are recorded by four encoders. According to the robot kinematics, we can get
(23)vxvyωc=R4⋅1111tanβ−tanβtanβ−tanβ1W+cotβ⋅L−1W+cotβ⋅L−1W+cotβ⋅L1W+cotβ⋅L⋅θ̇1θ̇2θ̇3θ̇4,where θ̇i is the rotation speed of robot’s wheels, β is the inclination angle of the inner rollers of the mecanum wheels, and β=π/4. W and L are the half of robot’s width and length, respectively. The running velocities of the robot are vx,vy,ωc, which represent the forward velocity, transverse velocity, and rotation velocity, respectively.Based on the Madgwick algorithm [26], the IMU can calculate the three-axis angle of the robot θc,αc,φc, which, respectively, represents the roll angle, heading angle, and tilt angle. Since these angles have cumulative errors over time, robot’s absolute coordinates xc,yc acquired by the UWB positioning subsystem are used to correct them. During the positioning process, the UWB positioning subsystem records the robot’s moving trajectory in real time at a frequency of 50 Hz. Within a certain period of time, the robot’s coordinates xi,yi,i=1,2,⋯,N are used to evaluate linearity of the moving trajectory. When the linearity is greater than 0.9, the slope of the moving trajectory is used to correct the IMU heading angle. Otherwise, the robot moving trajectory is regarded as nonlinear, and the IMU heading angle will be not corrected.Some errors may be caused by the sliding of robot’s mecanum wheels, and the heading angle integrated by robot’s rotation velocityωc may be inaccurate. As shown in Figure 11, the IMU heading angle αc and the robot’s running velocities vx,vy are combined to calculate robot velocities ẋc,ẏc in the absolute coordinate system xOy:
(24)ẋc=vx⋅cosαc−vy⋅sinαc,ẏc=vx⋅sinαc+vy⋅cosαc.Figure 11
Robot velocity calculation in the absolute coordinate system.


### 4.3. Multisensor Position Estimation

As we mentioned above, environmental interference can cause UWB positioning errors. Therefore, a multisensor position estimation method is proposed to improve the positioning accuracy of the robot. As shown in Figure12, the robot multisensor data are fused and combined for more accurate position coordinate estimation. Robot positioning data are acquired by multiple sensors, including UWB positioning subsystem, IMU, and encoders.Figure 12
Multisensor data fusion and robot positioning estimation.As the accuracy of the UWB positioning dataxc,yc is not high enough and ẋc,ẏc has cumulative errors over time, Kalman filter is used to fuse multisensor data. Both xc,yc and ẋc,ẏc are fed into the Kalman filter to estimate the robot coordinates and velocities xk,yk,ẋk,ẏk. The robot position estimation and prediction include two steps: first, combine the robot motion model and predict the robot state xk,yk,ẋk,ẏk at time k based on the optimal estimation xk−1,yk−1,ẋk−1,ẏk−1 at time k−1; second, use the measured data (UWB positioning subsystem, IMU, and encoders) to correct the predicted state and its parameters.During the positioning process, the robot receives coordinate dataxc,yc and velocity data ẋc,ẏcat frequencies of 50 Hz and 100 Hz, respectively. In each time period, assuming that the robot performs uniform motion, the state equation of the robot isXk=Φ⋅Xk‐1+Γwk−1,(24)whereXk=xkẋkykẏkTis the state vector of the robot and Φ is the state transition matrix of the uniform motion model.
(25)Φ=1dtui000100001dtui0001,where dtui is the sampling period of positioning data. Due to the different update frequencies of multisensor data, dtui includes two cases: dtu is the sampling period of UWB positioning dataxc,yc, and dti is the sampling period of velocity data ẋc,ẏc.Suppose the noise of the robot motion system isΓwk:
(26)Γ=dtui10000dtui1,wk=uxuy,where ux and uy are white Gaussian noise.The measurement equation of the robot positioning system is(27)Zk=Hui⋅Xk+vk,where Zk=xmymT is the measurement vector, including UWB positioning data xc,yc and velocity data ẋc,ẏc. vk is the measurement noise. Hui represents the transfer relationship matrix from the state vector Xk to the measurement vector Zk. The observation matrices Hui corresponding to xc,yc and ẋc,ẏc are
(28)Hu=10101010,Hi=01010101.Based on the state equation and measurement equation of the robot positioning system, Kalman filter prediction equation can be constructed as follows:(29)Xk∣k−1=Φ⋅Xk−1∣k−1+wk,where wk is noise, Xk−1∣k−1 is the optimal estimation vector at time k−1, and Xk∣k−1 is the prediction vector at time k predicted by positioning data at time k−1.The covariance of the robot system at timek is
(30)Pk∣k‐1=Φ⋅Pk‐1∣k‐1⋅ΦT+Q,where Pk‐1∣k‐1 is the covariance matrix of the robot system at time k−1 and Q is the covariance of system noise:
(31)Q=G⋅GT⋅Ke,G=et22et0000et22et.In the robot positioning process,Ke=0.25 and et=0.02.The optimal estimation vector of the robot system at timek is
(32)Xk∣k=Xk∣k‐1+KgkZk−HuiXk,where Zk is the measured value, including two types of data: xc,yc andẋc,ẏc. The Huicorresponding to different measurement data is Hu and Hi. Kalman filter gain Kgk is
(33)Kgk=Pk∣k−1HuiTHuiPk∣k−1HuiT+Rui−1,where Rui is the covariance matrix of the observed noise. The value of Rui represents the fluctuation of the observed data. Its value is larger, while the credibility of the data is lower, and its influence on the final estimation result is smaller; conversely, the smaller value of Rui has a greater effect on the estimation result. The matrix Rui corresponding to UWB positioning data xc,yc and velocity dataẋc,ẏc is
(34)Ru=0.09000.09,Ri=0.01000.01.Covariance matrixPk∣k of the robot system at time k is updated to
(35)Pk∣k=I−KgkHuiPk∣k−1.The optimal estimationxk,ẋk,yk,ẏk of the robot position at time k can be calculated from Equation (32). Based on the optimal estimate and the covariance matrix at time k, combined with the measured data at time k+1, the optimal estimate xk+1,ẋk+1,yk+1,ẏk+1 at time k+1 can be predicted successively.In the process of multisensor data fusion and robot positioning, no matter which set of positioning data (xc,yc or ẋc,ẏc) is acquired, it will be fed to the Kalman filter to estimate the robot’s position and velocities. At the same time, the covariance matrix of the Kalman filter is updated. If the UWB positioning signal is temporarily lost or interfered, the robot can still estimate its position using the data from IMU and encoders. Certainly, the robot can also complete the positioning operation only with UWB positioning data. Based on the method of data fusion and position estimation, multiple positioning data are mutually corrected and supplemented to improve robot positioning accuracy.


## 4.1. UWB Coordinate Estimation

The UWB positioning subsystem includes UWB anchors and a UWB robot tag. UWB anchors are generally fixed at certain positions with known coordinates, and the UWB robot tag is installed on the robot. As shown in Figure10, there are some UWB anchors with known coordinatesxi,yi on a 2D plane: Anchor 1, Anchor 2, Anchor 3, ..., Anchor n, and the coordinate of the UWB robot tag is xc,yc. The distances from the UWB robot tag to the UWB anchors measured by the robot positioning system are dii=1,2,⋯,n. From the geometric relationship, we can get
(18)xc−x12+yc−y12=d12xc−x22+yc−y22=d22xc−x32+yc−y32=d32⋯xc−xn2+yc−yn2=dn2.Figure 10
UWB positioning and coordinate calculation.The following matrix equation can be obtained by Equation (18):
(19)AuXu=Du,where,
(20)Au=2x1−xn2y1−yn2x2−xn2y2−yn2x3−xn2y3−yn⋮2xn−1−xn2yn−1−yn,Du=x12−xn2+y12−yn2+dn2−d12x22−xn2+y22−yn2+dn2−d22x32−xn2+y32−yn2+dn2−d32⋮xn−12−xn2+yn−12−yn2+dn2−dn−12,Xu=xcyc.Because the UWB positioning subsystem has some positioning error, equation (19) can be corrected as
(21)AuXu+N=Du,where N is an error vector with n−1 dimensions.WhenN=Du−AuXu is the smallest, the measurement error will be the smallest. Supposefx=Du−AuXu, the least square estimate of X is
(22)Xu=AuTAu−1AuTDu.Therefore,xc,yc is the estimated coordinate value of the robot. When positioning the mobile robot in the plane, at least three UWB anchors are required. Generally, more UWB anchors can improve the robot positioning accuracy and stability. However, too many UWB anchor data may cause more calculations and other possible errors.


## 4.2. Robot Velocity Calculation

In the robot positioning process, the robot’s absolute coordinatexc,yc can be calculated by the UWB positioning subsystem. Meanwhile, the rotation speeds of the wheels are recorded by four encoders. According to the robot kinematics, we can get
(23)vxvyωc=R4⋅1111tanβ−tanβtanβ−tanβ1W+cotβ⋅L−1W+cotβ⋅L−1W+cotβ⋅L1W+cotβ⋅L⋅θ̇1θ̇2θ̇3θ̇4,where θ̇i is the rotation speed of robot’s wheels, β is the inclination angle of the inner rollers of the mecanum wheels, and β=π/4. W and L are the half of robot’s width and length, respectively. The running velocities of the robot are vx,vy,ωc, which represent the forward velocity, transverse velocity, and rotation velocity, respectively.Based on the Madgwick algorithm [26], the IMU can calculate the three-axis angle of the robot θc,αc,φc, which, respectively, represents the roll angle, heading angle, and tilt angle. Since these angles have cumulative errors over time, robot’s absolute coordinates xc,yc acquired by the UWB positioning subsystem are used to correct them. During the positioning process, the UWB positioning subsystem records the robot’s moving trajectory in real time at a frequency of 50 Hz. Within a certain period of time, the robot’s coordinates xi,yi,i=1,2,⋯,N are used to evaluate linearity of the moving trajectory. When the linearity is greater than 0.9, the slope of the moving trajectory is used to correct the IMU heading angle. Otherwise, the robot moving trajectory is regarded as nonlinear, and the IMU heading angle will be not corrected.Some errors may be caused by the sliding of robot’s mecanum wheels, and the heading angle integrated by robot’s rotation velocityωc may be inaccurate. As shown in Figure 11, the IMU heading angle αc and the robot’s running velocities vx,vy are combined to calculate robot velocities ẋc,ẏc in the absolute coordinate system xOy:
(24)ẋc=vx⋅cosαc−vy⋅sinαc,ẏc=vx⋅sinαc+vy⋅cosαc.Figure 11
Robot velocity calculation in the absolute coordinate system.


## 4.3. Multisensor Position Estimation

As we mentioned above, environmental interference can cause UWB positioning errors. Therefore, a multisensor position estimation method is proposed to improve the positioning accuracy of the robot. As shown in Figure12, the robot multisensor data are fused and combined for more accurate position coordinate estimation. Robot positioning data are acquired by multiple sensors, including UWB positioning subsystem, IMU, and encoders.Figure 12
Multisensor data fusion and robot positioning estimation.As the accuracy of the UWB positioning dataxc,yc is not high enough and ẋc,ẏc has cumulative errors over time, Kalman filter is used to fuse multisensor data. Both xc,yc and ẋc,ẏc are fed into the Kalman filter to estimate the robot coordinates and velocities xk,yk,ẋk,ẏk. The robot position estimation and prediction include two steps: first, combine the robot motion model and predict the robot state xk,yk,ẋk,ẏk at time k based on the optimal estimation xk−1,yk−1,ẋk−1,ẏk−1 at time k−1; second, use the measured data (UWB positioning subsystem, IMU, and encoders) to correct the predicted state and its parameters.During the positioning process, the robot receives coordinate dataxc,yc and velocity data ẋc,ẏcat frequencies of 50 Hz and 100 Hz, respectively. In each time period, assuming that the robot performs uniform motion, the state equation of the robot isXk=Φ⋅Xk‐1+Γwk−1,(24)whereXk=xkẋkykẏkTis the state vector of the robot and Φ is the state transition matrix of the uniform motion model.
(25)Φ=1dtui000100001dtui0001,where dtui is the sampling period of positioning data. Due to the different update frequencies of multisensor data, dtui includes two cases: dtu is the sampling period of UWB positioning dataxc,yc, and dti is the sampling period of velocity data ẋc,ẏc.Suppose the noise of the robot motion system isΓwk:
(26)Γ=dtui10000dtui1,wk=uxuy,where ux and uy are white Gaussian noise.The measurement equation of the robot positioning system is(27)Zk=Hui⋅Xk+vk,where Zk=xmymT is the measurement vector, including UWB positioning data xc,yc and velocity data ẋc,ẏc. vk is the measurement noise. Hui represents the transfer relationship matrix from the state vector Xk to the measurement vector Zk. The observation matrices Hui corresponding to xc,yc and ẋc,ẏc are
(28)Hu=10101010,Hi=01010101.Based on the state equation and measurement equation of the robot positioning system, Kalman filter prediction equation can be constructed as follows:(29)Xk∣k−1=Φ⋅Xk−1∣k−1+wk,where wk is noise, Xk−1∣k−1 is the optimal estimation vector at time k−1, and Xk∣k−1 is the prediction vector at time k predicted by positioning data at time k−1.The covariance of the robot system at timek is
(30)Pk∣k‐1=Φ⋅Pk‐1∣k‐1⋅ΦT+Q,where Pk‐1∣k‐1 is the covariance matrix of the robot system at time k−1 and Q is the covariance of system noise:
(31)Q=G⋅GT⋅Ke,G=et22et0000et22et.In the robot positioning process,Ke=0.25 and et=0.02.The optimal estimation vector of the robot system at timek is
(32)Xk∣k=Xk∣k‐1+KgkZk−HuiXk,where Zk is the measured value, including two types of data: xc,yc andẋc,ẏc. The Huicorresponding to different measurement data is Hu and Hi. Kalman filter gain Kgk is
(33)Kgk=Pk∣k−1HuiTHuiPk∣k−1HuiT+Rui−1,where Rui is the covariance matrix of the observed noise. The value of Rui represents the fluctuation of the observed data. Its value is larger, while the credibility of the data is lower, and its influence on the final estimation result is smaller; conversely, the smaller value of Rui has a greater effect on the estimation result. The matrix Rui corresponding to UWB positioning data xc,yc and velocity dataẋc,ẏc is
(34)Ru=0.09000.09,Ri=0.01000.01.Covariance matrixPk∣k of the robot system at time k is updated to
(35)Pk∣k=I−KgkHuiPk∣k−1.The optimal estimationxk,ẋk,yk,ẏk of the robot position at time k can be calculated from Equation (32). Based on the optimal estimate and the covariance matrix at time k, combined with the measured data at time k+1, the optimal estimate xk+1,ẋk+1,yk+1,ẏk+1 at time k+1 can be predicted successively.In the process of multisensor data fusion and robot positioning, no matter which set of positioning data (xc,yc or ẋc,ẏc) is acquired, it will be fed to the Kalman filter to estimate the robot’s position and velocities. At the same time, the covariance matrix of the Kalman filter is updated. If the UWB positioning signal is temporarily lost or interfered, the robot can still estimate its position using the data from IMU and encoders. Certainly, the robot can also complete the positioning operation only with UWB positioning data. Based on the method of data fusion and position estimation, multiple positioning data are mutually corrected and supplemented to improve robot positioning accuracy.


## 5. Experiments and Results

### 5.1. UWB Ranging Correction Experiments

In the UWB ranging correction experiments, the UWB measured distance and the actual distance were collected and data fitting was performed to correct the ranging error. In the LOS environment, the distance between a UWB tag and a UWB anchor was measured by the DS-TWR method. In order to verify the reliability of the method, the experiment was implemented in two different environments: indoor correction experiment (Figure13(a)) and outdoor correction experiment (Figure 13(b)). It is worth to note that there was no obstruction between the UWB tag and the UWB anchor.Figure 13
UWB ranging correction experiments: (a) indoor correction experiment; (b) outdoor correction experiment.
(a)(b)Table1 shows the comparison of multiple UWB measurement distances and actual distances. After data fitting, the correction equations for UWB indoor and outdoor ranging are
(36)dt_in=0.9846×dm+0.3327,dt_out=0.9898×dm+0.1564,where dm is the original UWB measured value, dt_in and dt_out are correctional value in indoor and outdoor environments, respectively. The correction equation is used to reduce the ranging error and improve the measurement accuracy.Table 1
Measured distance results of UWB indoors and outdoors.
NumActual distance (m)Measured distance indoor (m)Measured distance outdoor (m)11.000.620.8322.001.681.8733.002.712.8744.003.713.8955.004.754.9066.005.835.9077.006.756.9088.007.837.9299.008.858.941010.009.779.951111.0010.810.971212.0011.8511.971313.0012.9113.001414.0013.8913.981515.0014.8815.001616.0015.9616.011717.0016.9417.011818.0017.9418.031919.0018.9219.022020.0019.9420.04Figure14 shows the curves of the original UWB ranging error and the correctional UWB ranging error. The correctional ranging error is significantly smaller than the original ranging error. In the indoor environment (Figure 14(a)), the root mean square error (RMSE) of the original UWB ranging data is 0.19 m, and the RMSE of correctional UWB data is 0.04 m. In the outdoor environment (Figure 14(b)), the RMSE of the original and correctional UWB ranging data is 0.08 m and 0.01 m, respectively.Figure 14
UWB ranging errors: (a) original ranging errors; (b) correctional ranging errors.
(a)(b)Compared with the outdoor environment, the indoor environment has lower ranging accuracy due to UWB multipath interference. The experimental results indicate that the correction method of data fitting can effectively reduce the UWB ranging error, especially in the case of severe UWB multipath interference. The maximum indoor ranging error is reduced to 0.1 m from original 0.4 m. Similarly, the maximum error of outdoor UWB ranging is corrected from 0.17 m to 0.04 m.


### 5.2. UWB Availability Experiment

In the indoor environment, the UWB robot tag may exceed the visible range of UWB anchors due to the occlusion of walls or obstacles, which will cause inaccurate UWB data at this case. As a result, during UWB positioning progress, the first path power of the UWB anchor signal will be captured to confirm the availability of UWB data. If the communication power of a UWB anchor is too low, the ranging data of this UWB anchor is unreliable; then, it will be discarded.Figure15 shows the first path power and ranging results of four UWB anchors as the robot’s position changed. When the robot was in Position I, Anchor 0 was blocked and interfered; its signal power was very low. At this stage, the ranging data of Anchor 0 was unstable, so it was discarded. When the robot moved to Position II, Anchor 0 was in the visible range of the robot, and its signal power gradually increased. All UWB anchors had stable signals, and their positioning data were available. When the robot left Position II and entered Position III, Anchor 3 left the visible area of the robot. The signal power of Anchor 3 gradually decreased, and its ranging data was discarded due to inaccuracy.Figure 15
UWB ranging power and availability analysis: (a) the first path power of UWB anchors; (b) measured distances of UWB anchors.
(a)(b)In the process of robot positioning, if the first path power of a UWB anchor is lower than -95 dBm, the robot positioning system will discard the ranging data of this anchor. When the total number of available UWB anchors is less than three, the UWB positioning status of the robot will be considered invalid at this time.


### 5.3. Multisensor Positioning Experiments

Multisensor positioning experiments were carried out to verify effects of the data fusion and robot positioning by UWB, IMU, and encoders. Figure16 shows the experimental equipment and environment, the UWB robot tag was installed on the mobile robot, and three UWB anchors (Anchor 1, Anchor 2, and Anchor 3) were distributed around. Meanwhile, the robot’s velocities were calculated based on the IMU sensor (MPU9250) and encoders. The laser tracker was used to record the running trajectory and position coordinates of the robot in real time. Within the coverage of UWB signals, multisensor data based on UWB, IMU, and encoders were simultaneously captured and used; outside the coverage of UWB signals, the robot could only use IMU and encoders for positioning.Figure 16
Multisensor positioning experiments.The given running path of the mobile robot is shown in Figure17. The robot started from point S, passed through point M, and finally reached point E. In the experimental environment, zone A was completely covered by UWB signals. UWB signals in ZoneBwere slightly disturbed by walls, and those in Zone C were invalid due to severe interference.Figure 17
Experiment environment and robot moving trajectory.Three positioning methods were tested and compared in the experiments:(1)
Positioning only by IMU and encoders (IMU positioning);(2)
Positioning only by the UWB subsystem (UWB positioning);(3)
Positioning by IMU, encoders, and UWB subsystem (IMU+UWB positioning).In the multisensor positioning experiments, the robot positioning system adopted these three different positioning methods to estimate the coordinates and angle of the mobile robot. The robot coordinate systemXOY was established, with Anchor 1 as the coordinate origin. Figure 18 shows the coordinate system and the running trajectories of the robot by three positioning methods. The coordinates of three UWB anchors were (0, 0), (2.0, 3.0), and (1.0, 6.0). In the first stage, the robot started from point S (1.0,1.0) and reached point M (1.0, 5.6) along the direction of the Y coordinate axis. In the second stage, the robot rotated 90° and then moved along the direction of the X coordinate axis to the point E (-8.0, 5.6).Figure 18
Coordinate systemXOY and robot running trajectories by three positioning methods.As shown in Figure18, the red lines represent the running trajectory of the robot using IMU positioning. In the whole process, the robot coordinates can be estimated and used, but as time accumulates, the positioning error gradually increases. The blue lines are the running trajectory of the robot using UWB positioning. In the Zone C, the robot positioning fails because the UWB signals are invalid. The green lines represent the running trajectory of the robot using IMU+UWB positioning. In Zone A and Zone B, the robot uses data fusion of UWB, IMU, and encoders for positioning, and the robot can still complete positioning using IMU and encoder data in Zone C.Figure19 shows the coordinate error of the robot using three positioning methods. Figure 19(a) shows the X coordinate errors when the robot moved from point S to point M (SM stage). In the initial stage, the error of IMU positioning is very small, but it keeps increasing over time. The error of UWB positioning is the largest. Since the UWB data has been used to correct the heading angle of IMU, the error of IMU+UWB positioning has some fluctuations while its high accuracy is guaranteed. The maximum X coordinate error of IMU+UWB positioning is less than 0.05 m.Figure 19
Robot coordinate errors by three positioning methods: (a)X coordinate errors; (b) Y coordinate errors.
(a)(b)Figure19(b) shows the Y coordinate error when the robot moved from point M to point e (ME stage). The error of IMU positioning keeps increasing, and the maximum error is close to 0.4 m. As UWB signals in Zone B are disturbed, the error of UWB positioning continues to increase. In Zone C, the method of UWB positioning fails and cannot estimate the robot coordinates. In the whole process, the error of IMU+UWB positioning is relatively small. Even if the UWB signals are invalid in Zone C, the robot can still complete the positioning with high accuracy. The Y coordinate error is kept within 0.16 m.Based on mathematical statistics, the RMSEs of IMU positioning, UWB positioning and IMU+UWB Positioning are 0.1972 m, 0.0902 m, and 0.0821 m, respectively. The experimental results indicate that the multisensor fusion positioning method based on UWB, IMU, and encoders has higher positioning accuracy and stability. The multisensor positioning method uses different sensor data to correct and supplement each other, thus having stronger anti-interference ability. Even if one sensor fails, the robot positioning can still operate normally based on other sensors.Figure20 shows changing curves of the robot’s heading angle in the experiments. In the SM stage (Figure 20(a)), the desired heading angle of the robot is 90°. In a short time, the heading angle of IMU positioning is more stable, but the angle error cannot be corrected automatically. The heading angle of UWB Positioning is calculated by coordinates with some large fluctuations. This is because UWB data is used to correct IMU errors, the heading angle of IMU+UWB positioning also has some slight fluctuations. In the ME stage (Figure 20(b)), the desired heading angle of the robot is 180°. The cumulative angle error of the IMU positioning gradually increases, which will cause greater positioning error. As UWB signals are interfered, UWB positioning has more angle errors, and it fails in Zone C, whereas the heading angle of IMU+UWB positioning is more stable. It can still operate normally in Zone C.Figure 20
Changing curves of the robot’s heading angle: (a) SM stage; (b) ME stage.
(a)(b)The experimental results indicate that the positioning accuracy of the multisensor positioning method has been greatly improved. The UWB data can correct the cumulative angle error of IMU, and the heading angle is more stable. When UWB signals are interfered or invalid, the robot can still use IMU and encoder data for positioning, which has stronger environmental adaptability.


## 5.1. UWB Ranging Correction Experiments

In the UWB ranging correction experiments, the UWB measured distance and the actual distance were collected and data fitting was performed to correct the ranging error. In the LOS environment, the distance between a UWB tag and a UWB anchor was measured by the DS-TWR method. In order to verify the reliability of the method, the experiment was implemented in two different environments: indoor correction experiment (Figure13(a)) and outdoor correction experiment (Figure 13(b)). It is worth to note that there was no obstruction between the UWB tag and the UWB anchor.Figure 13
UWB ranging correction experiments: (a) indoor correction experiment; (b) outdoor correction experiment.
(a)(b)Table1 shows the comparison of multiple UWB measurement distances and actual distances. After data fitting, the correction equations for UWB indoor and outdoor ranging are
(36)dt_in=0.9846×dm+0.3327,dt_out=0.9898×dm+0.1564,where dm is the original UWB measured value, dt_in and dt_out are correctional value in indoor and outdoor environments, respectively. The correction equation is used to reduce the ranging error and improve the measurement accuracy.Table 1
Measured distance results of UWB indoors and outdoors.
NumActual distance (m)Measured distance indoor (m)Measured distance outdoor (m)11.000.620.8322.001.681.8733.002.712.8744.003.713.8955.004.754.9066.005.835.9077.006.756.9088.007.837.9299.008.858.941010.009.779.951111.0010.810.971212.0011.8511.971313.0012.9113.001414.0013.8913.981515.0014.8815.001616.0015.9616.011717.0016.9417.011818.0017.9418.031919.0018.9219.022020.0019.9420.04Figure14 shows the curves of the original UWB ranging error and the correctional UWB ranging error. The correctional ranging error is significantly smaller than the original ranging error. In the indoor environment (Figure 14(a)), the root mean square error (RMSE) of the original UWB ranging data is 0.19 m, and the RMSE of correctional UWB data is 0.04 m. In the outdoor environment (Figure 14(b)), the RMSE of the original and correctional UWB ranging data is 0.08 m and 0.01 m, respectively.Figure 14
UWB ranging errors: (a) original ranging errors; (b) correctional ranging errors.
(a)(b)Compared with the outdoor environment, the indoor environment has lower ranging accuracy due to UWB multipath interference. The experimental results indicate that the correction method of data fitting can effectively reduce the UWB ranging error, especially in the case of severe UWB multipath interference. The maximum indoor ranging error is reduced to 0.1 m from original 0.4 m. Similarly, the maximum error of outdoor UWB ranging is corrected from 0.17 m to 0.04 m.


## 5.2. UWB Availability Experiment

In the indoor environment, the UWB robot tag may exceed the visible range of UWB anchors due to the occlusion of walls or obstacles, which will cause inaccurate UWB data at this case. As a result, during UWB positioning progress, the first path power of the UWB anchor signal will be captured to confirm the availability of UWB data. If the communication power of a UWB anchor is too low, the ranging data of this UWB anchor is unreliable; then, it will be discarded.Figure15 shows the first path power and ranging results of four UWB anchors as the robot’s position changed. When the robot was in Position I, Anchor 0 was blocked and interfered; its signal power was very low. At this stage, the ranging data of Anchor 0 was unstable, so it was discarded. When the robot moved to Position II, Anchor 0 was in the visible range of the robot, and its signal power gradually increased. All UWB anchors had stable signals, and their positioning data were available. When the robot left Position II and entered Position III, Anchor 3 left the visible area of the robot. The signal power of Anchor 3 gradually decreased, and its ranging data was discarded due to inaccuracy.Figure 15
UWB ranging power and availability analysis: (a) the first path power of UWB anchors; (b) measured distances of UWB anchors.
(a)(b)In the process of robot positioning, if the first path power of a UWB anchor is lower than -95 dBm, the robot positioning system will discard the ranging data of this anchor. When the total number of available UWB anchors is less than three, the UWB positioning status of the robot will be considered invalid at this time.


## 5.3. Multisensor Positioning Experiments

Multisensor positioning experiments were carried out to verify effects of the data fusion and robot positioning by UWB, IMU, and encoders. Figure16 shows the experimental equipment and environment, the UWB robot tag was installed on the mobile robot, and three UWB anchors (Anchor 1, Anchor 2, and Anchor 3) were distributed around. Meanwhile, the robot’s velocities were calculated based on the IMU sensor (MPU9250) and encoders. The laser tracker was used to record the running trajectory and position coordinates of the robot in real time. Within the coverage of UWB signals, multisensor data based on UWB, IMU, and encoders were simultaneously captured and used; outside the coverage of UWB signals, the robot could only use IMU and encoders for positioning.Figure 16
Multisensor positioning experiments.The given running path of the mobile robot is shown in Figure17. The robot started from point S, passed through point M, and finally reached point E. In the experimental environment, zone A was completely covered by UWB signals. UWB signals in ZoneBwere slightly disturbed by walls, and those in Zone C were invalid due to severe interference.Figure 17
Experiment environment and robot moving trajectory.Three positioning methods were tested and compared in the experiments:(1)
Positioning only by IMU and encoders (IMU positioning);(2)
Positioning only by the UWB subsystem (UWB positioning);(3)
Positioning by IMU, encoders, and UWB subsystem (IMU+UWB positioning).In the multisensor positioning experiments, the robot positioning system adopted these three different positioning methods to estimate the coordinates and angle of the mobile robot. The robot coordinate systemXOY was established, with Anchor 1 as the coordinate origin. Figure 18 shows the coordinate system and the running trajectories of the robot by three positioning methods. The coordinates of three UWB anchors were (0, 0), (2.0, 3.0), and (1.0, 6.0). In the first stage, the robot started from point S (1.0,1.0) and reached point M (1.0, 5.6) along the direction of the Y coordinate axis. In the second stage, the robot rotated 90° and then moved along the direction of the X coordinate axis to the point E (-8.0, 5.6).Figure 18
Coordinate systemXOY and robot running trajectories by three positioning methods.As shown in Figure18, the red lines represent the running trajectory of the robot using IMU positioning. In the whole process, the robot coordinates can be estimated and used, but as time accumulates, the positioning error gradually increases. The blue lines are the running trajectory of the robot using UWB positioning. In the Zone C, the robot positioning fails because the UWB signals are invalid. The green lines represent the running trajectory of the robot using IMU+UWB positioning. In Zone A and Zone B, the robot uses data fusion of UWB, IMU, and encoders for positioning, and the robot can still complete positioning using IMU and encoder data in Zone C.Figure19 shows the coordinate error of the robot using three positioning methods. Figure 19(a) shows the X coordinate errors when the robot moved from point S to point M (SM stage). In the initial stage, the error of IMU positioning is very small, but it keeps increasing over time. The error of UWB positioning is the largest. Since the UWB data has been used to correct the heading angle of IMU, the error of IMU+UWB positioning has some fluctuations while its high accuracy is guaranteed. The maximum X coordinate error of IMU+UWB positioning is less than 0.05 m.Figure 19
Robot coordinate errors by three positioning methods: (a)X coordinate errors; (b) Y coordinate errors.
(a)(b)Figure19(b) shows the Y coordinate error when the robot moved from point M to point e (ME stage). The error of IMU positioning keeps increasing, and the maximum error is close to 0.4 m. As UWB signals in Zone B are disturbed, the error of UWB positioning continues to increase. In Zone C, the method of UWB positioning fails and cannot estimate the robot coordinates. In the whole process, the error of IMU+UWB positioning is relatively small. Even if the UWB signals are invalid in Zone C, the robot can still complete the positioning with high accuracy. The Y coordinate error is kept within 0.16 m.Based on mathematical statistics, the RMSEs of IMU positioning, UWB positioning and IMU+UWB Positioning are 0.1972 m, 0.0902 m, and 0.0821 m, respectively. The experimental results indicate that the multisensor fusion positioning method based on UWB, IMU, and encoders has higher positioning accuracy and stability. The multisensor positioning method uses different sensor data to correct and supplement each other, thus having stronger anti-interference ability. Even if one sensor fails, the robot positioning can still operate normally based on other sensors.Figure20 shows changing curves of the robot’s heading angle in the experiments. In the SM stage (Figure 20(a)), the desired heading angle of the robot is 90°. In a short time, the heading angle of IMU positioning is more stable, but the angle error cannot be corrected automatically. The heading angle of UWB Positioning is calculated by coordinates with some large fluctuations. This is because UWB data is used to correct IMU errors, the heading angle of IMU+UWB positioning also has some slight fluctuations. In the ME stage (Figure 20(b)), the desired heading angle of the robot is 180°. The cumulative angle error of the IMU positioning gradually increases, which will cause greater positioning error. As UWB signals are interfered, UWB positioning has more angle errors, and it fails in Zone C, whereas the heading angle of IMU+UWB positioning is more stable. It can still operate normally in Zone C.Figure 20
Changing curves of the robot’s heading angle: (a) SM stage; (b) ME stage.
(a)(b)The experimental results indicate that the positioning accuracy of the multisensor positioning method has been greatly improved. The UWB data can correct the cumulative angle error of IMU, and the heading angle is more stable. When UWB signals are interfered or invalid, the robot can still use IMU and encoder data for positioning, which has stronger environmental adaptability.


## 6. Discussion of the Experiments

Compared with the single-sensor positioning method, the positioning system based on UWB and multisensors can improve the positioning accuracy and adaptability of the robot. Through data fitting, the robot positioning system can effectively reduce the UWB ranging error caused by multipath interference. The indoor and outdoor maximum ranging errors are increased to 0.1 m and 0.04 m, respectively. According to the first path power of the UWB anchor signals, the robot positioning system can judge the availability of UWB positioning data and prevent the error positioning data from being used. Meanwhile, the positioning system can select stable UWB anchors to collect positioning data.In general, the developed robot positioning system based on data fusion and error correction has the following advantages:(1)
The mutual correction of multisensor data improves the stability and accuracy of positioning data. During the movement of the robot, IMU heading angle can be corrected by UWB data, which reduces the cumulative angle error(2)
Based on multisensor data fusion, the robot positioning system can achieve higher-precision positioning. When multisensor data are available simultaneously, the positioning error of the robot is less than 0.05 m(3)
The robot positioning system has better anti-interference ability and environmental adaptability. Even if UWB positioning data is disturbed, the robot can still use IMU and encoder data for positioning, and the maximum positioning error is within 0.16 m


## 7. Conclusion

This paper presents a position estimation and positioning error correction method of mobile robot based on UWB, IMU, and encoders. The robot positioning system includes a mecanum wheeled mobile robot, a remote control computer, and UWB positioning subsystem. UWB ranging error is corrected by the DS-TWR method and data fitting. The UWB positioning system is composed of UWB anchors, and a UWB robot tag can calculate robot position coordinates. The multisensor positioning method can improve the robot positioning accuracy. The robot coordinatesxc,yc are captured by the UWB subsystem, and the robot velocities ẋc,ẏc are calculated by the IMU and encoders. Multisensor data are continuously fed to the Kalman filter to estimate the robot position. In the positioning process, UWB data are also used to reduce the cumulative angle error generated by the IMU. In the UWB ranging correction experiments, the correction equation after data fitting can effectively improve the UWB ranging accuracy. The first path power of the UWB anchor signal is captured to confirm the availability of UWB data. In the multisensor positioning experiments, the multisensor positioning method (IMU+ WB positioning) has higher accuracy and stability than a single-sensor positioning method (IMU positioning and UWB positioning). The experimental results indicate that the multisensor positioning method based on UWB, IMU, and encoders can effectively improve the positioning accuracy of the mobile robot, and this method has stronger anti-interference ability.In the future, we will optimize the robot positioning system to improve the positioning accuracy. For instance, more different types of sensors can be combined in the process of robot positioning and error correction. Besides, more multisensor positioning tests will be carried out to expand its application scope.



---

*Source: 7071466-2022-03-11.xml*