Posture algorithm for mobile robot based on ultrawide band

Inertial measurement unit or other navigation devices are used to obtain the posture information of most mobile robots based on ultrawide band. However, problems, such as zero drift, temperature drift, and error increasing with time, exist in inertial measurement unit and other navigation devices. To overcome these problems, a posture algorithm for mobile robot based on ultrawide band is proposed in this article. Real-time posture information can be obtained by the posture algorithm without using inertial measurement unit and other navigation devices. Simulated results show that the mean absolute error and root mean square error of posture angle in the global coordinate system are 0.951° and 1.169°, respectively. Both the simulated and experimental trajectories show good agreement with the ideal trajectory. The results indicate that the posture data can be obtained accurately by the posture algorithm and the accuracy can meet the requirements for most mobile robots. This article is expected to be of great guiding significance for the development of mobile robots.


Introduction
Positioning refers to the process of confirming the position of a robot in its operating environment. One of the most basic and important functions of a mobile robot is to perceive environmental information by using sensors to obtain reliable positioning. 1 Ultrawide band (UWB) technology is widely used in indoor navigation, personnel tracking, underground positioning, unmanned transportation, and other related fields because of high positioning accuracy, low power consumption, good penetration, and strong antiinterference ability. 2,3 At present, the most common UWB positioning algorithms include the time of arrival (TOA) and the time difference of arrival (TDOA). 4 The TOA positioning algorithm calculates the distance between the tag and the base station by recording the transmission time and reception time of the signal between the tag and the base station. Subsequently, the positioning information can be solved based on the knowledge of circle geometry. The TOA positioning algorithm is very simple, and its hardware requirements are relatively low. 5,6 The TDOA positioning algorithm obtains the positioning information by recording the TDOA between the tag and different base stations.
Although the positioning accuracy of TDOA is better than that of TOA, the hardware requirements of the former are much higher than that of the latter, and the clock synchronization between the tag and the base station is needed. 7,8 In fact, only the coordinate position of the tag can be obtained by executing the TOA positioning algorithm or the TDOA positioning algorithm. The prerequisite for the mobile robot to complete the task successfully is to obtain relatively reliable navigation data, such as the coordinate position, posture angle, and speed. However, most mobile robots that obtain posture information by using inertial measurement unit (IMU) need to be corrected continuously to reach the predetermined position. [9][10][11][12] The reason is that IMU has inevitable defects, such as zero drift, low sensitivity, and temperature drift, due to the influence of manufacturing technology, and these defects lead to various errors in posture information. [13][14][15] To overcome the above problems, Liu et al. proposed an approach to combine IMU, long-range Wi-Fi and short-range UWB for cooperative positioning in the infrastructure-less environment. The experimental results show that the combination of Wi-Fi and UWB measurements achieve a better positioning accuracy. 16 Nguyen and Xie studied the four degree-offreedom relative transformation estimation problem based on the fusion of odometry and UWB ranging data. The simulated and experimental results show that the proposed methods outperform state-of-the-art methods, especially in geometrically poor or large measurement noise conditions. 17 Cao et al. proposed an approach for relative localization without any infrastructure based on the fusion of multiple UWB ranging measurements from different nodes installed on the robots. The location accuracy is improved by utilizing the odometry constraints through a sliding window-based optimization. 18 Zhao et al. combined the UWB and inertial navigation sensor (INS) to form a coupled UWB/INS localization framework, in which a minimum variance unbiased finite impulse response (MVU FIR) method is applied to obtain accurate position and velocity estimations from noisy measurements. The simulated and experimental results show that the MVU FIR filter exhibits better immunity to the errors about a priori knowledge of noise variances. 19,20 In addition, lots of researcher studied the sensor fusion technology based on Kalman filtering in the robotics and vision. 21 Although the accuracy evaluation is improved by the combination of UWB and IMU, the algorithms are complicated.
In this article, a posture algorithm is proposed for the mobile robot based on the UWB. Taking the TOA positioning algorithm as an example, two UWB tags and three UWB base stations are used in the posture algorithm. The coordinate data of the two tags are calculated by the TOA positioning algorithm. Subsequently, the posture information of the mobile robot can be solved by using the coordinate data of the two tags. In addition to the TOA algorithm, this posture algorithm can also be combined with most UWB-based positioning algorithms. This work is expected to have a guiding significance for the application of UWB and the development of mobile robots.

TOA algorithm
The TOA algorithm, in which a tag and at least three base stations BS i (i ¼ 1, 2, 3) are required, achieves the positioning based on circumferential geometry knowledge. The distance r i between the tag and the base station can be obtained by the bilateral two-way ranging. 22 Assuming that the tag and the base station are on the same plane, the three circles, which select the base station as the circle center and the distance r i between the tag and the base station as the radius, will intersect at one point ideally. The point is regarded as the position of the tag, and its coordinate is ðx 0 ; y 0 Þ, as shown by the red dot in Figure 1. The coordinates of the three base stations BS i in the plane coordinate system are ðx 1 ; y 1 Þ, ðx 2 ; y 2 Þ, and ðx 3 ; y 3 Þ, respectively.
By using the distance formula between two points in the plane coordinate system, the equations can be obtained as follows The coordinate data of the target point ðx 0 ; y 0 Þ can be obtained by equation (1), given as follows In fact, the data receiving time is longer than the transmission time because of the accuracy of the hardware, multipath effects, non-line of sight errors, and other factors. 23,24 Thus the three circles will intersect in a region rather than at a point. In general, the algorithms, such as weighted least square algorithm, Fang algorithm, and Chan algorithm, can be used to estimate the results to obtain the optimal solution. 25,26 Posture algorithm based on UWB Although the TOA algorithm can obtain the coordinate data of the target in the plane coordinate system, the coordinate data are not enough in the field of robotics. Therefore, knowing the posture data of the robot, which can be obtained by the posture algorithm based on UWB, is necessary for the application of the robot.
The coordinate axes in the plane global coordinate system are defined as X I and Y I . The position of the robot in the global coordinate system is ðx I ; y I Þ, and the angle between the positive direction of the robot and the X I direction of the coordinate axis is q I . Thus, the posture and the motion velocity of the robot in the global coordinate system can be described as Taking the plane centroid of the robot as the local coordinate origin, the coordinate axes in the local coordinate system are X R and Y R . Hence, the motion velocity of the robot in the local coordinate system can be described as _ The relationship between _ X I and _ X R is where R q ð Þ is the orthogonal rotation matrix, given as follows The positioning information of the robot can be obtained by using two UWB tags in the posture algorithm. The two tags are installed on both sides of the robot head. The left one is Tag 1 , and the right one is Tag 2 , as shown in Figure 2. Suppose that the coordinate data of the two tags obtained by the TOA positioning algorithm are ðx 1 ; y 1 Þ and ðx 2 ; y 2 Þ, the relevant posture information of the robot can be calculated by using the coordinate data of the two tags.
Posture of the robot in the global coordinate system. There are four postures for the robot in the global coordinate system, as shown in Figure. 3. The positioning information of the robot in the global coordinate system can be obtained by using the coordinate data ðx 1 ; y 1 Þ and ðx 2 ; y 2 Þ of the two tags. Take Figure 3(a) as an example, we can obtain the positioning information of the robot in the X I direction as and in the Y I direction as y I ¼ y 1 þy 2 2 . Therefore, the positioning information of the robot can be rewritten as A straight line formed by connecting two tags intersects the X I axis of the global coordinate system in point A. The angle q A is formed by the straight line and the X I axis. Take Figure 3(a) as an example again, we can obtain the tangent of q A Here q A will be specified in degrees not radians, so The calculated q A 2 ½0 ; 90 Þ, and it can be treated as follows: 1) If x 1 x 2 and y 1 > y 2 , q I ¼ 90 À q A , then the posture diagram is shown in Figure 3(a). 2) If x 1 > x 2 and y 1 y 2 , q I ¼ 90 þq A , then the posture diagram is shown in Figure 3(b). 3) If x 1 x 2 and y 1 y 2 , q I ¼ 90 þq A , then the posture diagram is shown in Figure 3(c). 4) If x 1 > x 2 and y 1 > y 2 , q I ¼ 90 À q A , then the posture diagram is shown in Figure 3(d).
Speed of the robot in the coordinate system. In general, the speed of the robot is measured by the motor encoder. However, the use of the motor encoder results in errors caused by motor slippage. To overcome this problem, we use the positioning information of the two UWB tags of the robot to calculate the speeds of the robot in the global and local coordinate systems. From equation (5), the positioning information in the X I direction for the robot with two UWB tags is x 1 ðt 1 Þþx 2 ðt 1 Þ 2 at time t 1 and x 1 ðt 2 Þþx 2 ðt 2 Þ 2 at time t 2 . Thus, the speed of the robot in the X I direction is Similarly, the speed of the robot in the Y I direction can be obtained The angle velocity _ q I is the rate of the change of the angle over time, so it can be given as follows The speeds of the robot in the global coordinate systems can be rewritten as Here x 1 ðt 1 Þ, x 1 ðt 2 Þ, x 2 ðt 1 Þ, x 2 ðt 2 Þ, y 1 ðt 1 Þ, y 1 ðt 2 Þ, y 2 ðt 1 Þ, y 2 ðt 2 Þ, q I ðt 1 Þ, and q I ðt 2 Þ represent the global posture data of the robot at t 1 and t 2 , and t 2 À t 1 represents a very short period. Consequently, the speed in the local coordinate system of the robot can be obtained by combination equation (3) and equation (11), given as Relative posture of the robot and the target. The relative posture of the robot and the target can be represented by d q T ð Þ T , where d and q T are the distance and the relative angle between the robot and the target, respectively. The positioning information for the robot is . According to the Pythagorean theorem, the d can be obtained as d¼ ffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi Here x 3 and y 3 are the coordinates of the target point. q T needs to be handled differently according to various situations. If the target is in the positive direction of the robot shown in Figure 4, the angle q T þq I is So Therefore, the relative posture of the robot and the target can be given as If the target is in the opposite direction of the robot as shown in Figure 5, then the relative posture can be obtained similarly, and given as

Results and discussion
The analysis shows that the accuracy of the algorithm is closely related to the UWB positioning accuracy that is affected by the measure errors and the position accuracy of the base stations. 27,28 At present, the positioning accuracy has been improved by the researchers, and the simulation error of 2 cm is chosen as the lower limit in this article. 29 We suppose the coordinates of the three base stations are (0,0), (0, 800), and (800, 0); the robot centroid coordinates (400, 400); target point position (800, 800); actual posture angle 45 ; and the installation distance of the two tags 60 cm. The posture angle of the robot q I in the plane coordinate system is simulated with the positioning errors of 8, 6, 4, and 2 cm ( Figure 6). The accuracy of the algorithm is closely related to the positioning accuracy of UWB when the tag installation distance is fixed. The angle errors increase with the increase of the positioning errors.  This phenomenon can be explained by equation (7), in which the angle solved by the algorithm is calculated by the positioning coordinates. The smaller the positioning error is, the more accurate the posture information obtained by the algorithm will be. Here the mean absolute error (MAE) and root mean square error (RMSE) are used as metrics for performance evaluation. 30 As shown in Table 1, the MAE and RMSE of the posture angle in the plane coordinate system are 1.222 and 1.596 when the tag installation distance is 60 cm and the positioning error is within 2 cm. Such values can meet the accuracy requirements of most mobile robots. We assume that the coordinates of the three base stations are (0,0), (0, 800), and (800, 0); the robot centroid coordinates (400, 400); target point position (800, 800); and the positioning error within 2 cm. The posture angle of the robot q I in the global plane coordinate system is simulated with the orientations of the robot changing from 0 to 180 when the installation distances are 50, 60, 70, and 80 cm between two tags, as shown in Figure 7. To show the differences of the four graphs in Figure 7 more obviously, the part of the curves is enlarged in the inset. The simulated curves become smoother with the increase of the installation distance when the positioning error is within a certain range. The accuracy of the simulated posture angle is almost unchanged when the orientations of the robot change from 0 to 180 . Equation (7) also shows that the greater the tag installation distance is, the smaller the influence of the positioning error on the posture angle will be. Table 2 shows that when the positioning error is within 2 cm and the tag installation distance is 80 cm, the MAE and the RMSE of the posture angle in the plane coordinate system reach 0.951 and 1.169 , respectively. Such finding indicates that the robot based on the posture algorithm can be applied in many occasions, such as catering and logistics.
To verify the validity of the posture algorithms, the control platform in which the TM32F103-ZET6 is selected as the control core is built to test the robot (Figure 8). The coordinates of the three base stations are (0,0), (0, 800), and  (800, 0). The robot is placed at the point (400,400), and the target point is set at (800, 800). The posture angle in the global coordinate system is adjusted to 45 and the installation distance is 24 cm between two tags. The posture data solved by the algorithm are transmitted through Bluetooth. Figure 9 shows the distribution of the experimental posture angles. The results show that the posture angle obtained by the posture algorithm fluctuates around 45 , and the MAE and RMSE are 2.3469 and 2.7093 , respectively. The simulated and experimental trajectories of the robot which moves from the point (400,400) to (800, 800) are shown in Figure 10. Both the simulated and experimental trajectories   show good agreement with the ideal trajectory, while from the point (700, 700) to (780, 780) the experimental trajectory cannot fit the ideal trajectory very well. The maximum offset is about 8.26 cm (see the inset). The experimental results are slightly worse than the simulated results because it is difficult to obtain a positioning error of 2 cm for UWB. The results show that the posture data can be obtained accurately by the posture algorithm and the accuracy can meet the requirements of most mobile robots.

Conclusions
A posture algorithm based on UWB is proposed in this article. The posture angle, speed, and other real-time posture data of the mobile robot in the established plane coordinate system can be calculated, and the problems by using IMU can be solved by the posture algorithm. The simulated results show that the posture accuracy increases with the decrease of the positioning errors when the installation distance of the two UWB tags is constant, and the error accuracy increases with the increase of the installation distance of the two UWB tags when the positioning error is within a certain range. Both the simulated and experimental trajectories show good agreement with the ideal trajectory.
The results indicate that the posture data can be obtained by the posture algorithm and the accuracy can meet the requirements for most mobile robots. In fact, the accuracy of UWB is influenced by the non-line-of sight blocking, 31 which is not considered in this article. It will be studied in the following work.