Robot Pose Estimation Through Ultra-Wide Band Devices

(1)

Robot Pose Estimation Through Ultra-Wide Band Devices

Nuno Miguel Baltazar Gomes Fialho nuno.fialho@tecnico.ulisboa.pt

Instituto Superior T´ ecnico, Lisboa, Portugal October 2015

Abstract

Improving Pose Estimation has became an important topic for robotic and autonomous systems.

Therefore there are the necessity to find suitable absolute positioning methods in order to overcome to limitations from relative methods such as one from odometry, sonars and lasers. Being a contribution to the MOnarCH Project, this study is related to mobile robot pose estimation (position and orientation) through absolute positioning involving only Ultra-WideBand (UWB) devices already deployed in pairs on the robots. Due to some limitations of the hardware devices, some experiments with different positioning method and architectures have been carried out in order to find the one that returns the best performance values regarding quality of estimates and computational complexity. Trilateration and Fingerprinting was the methods chosen to compose this study. These methods were tested along with Coupled and Uncoupled architectures. Latter are as simple as first estimate UWB devices position a then transform them into robots pose, whereas former estimates robot pose in only one step. Keywords: Positioning, Pose, UWB, Trilateration, Fingerprinting

1. Introduction

In Robotics, a robot is a device that perceives the environment in which it operates gathering data through its sensors, processing it, extracting useful information to perform actions using its actua- tors. Autonomous systems is one field of robotics in which robots operates without human supervi- sion. In order to accomplish that, robots must have a set of state variables describing as much as possible the surround environment to help it making optimal decisions to attain a specific goal. For every robot that needs to operate in space, such as 2d space for mobile robots, one of the most important state variable is its position. But robots are not equal regarding its shape. Therefore, in most of the cases, knowing only the current position is not enough. It is also important to know orientation of the robot. There are several techniques currently available suitable to estimate posture of the robot. These techniques are based on merging knowledge from different sources of sensor in order to produce the best estimate of posture. Odometry is commonly used as sensor source which is a relative positioning method. Another example is lasers to sense the surrounding environment in order to match it with a map priori created. The problem is due to the fact these sources can still be erroneous.

In order to overcome these problems, absolute positioning systems was introduced. It does not depend on an initial position of the robot and does not suf-

fer from cumulative errors like Odometry. One example is the well known Global Positioning Systems previously mentioned.

1.1. Problem Statement

The problem of estimating the Robot absolute posture needs to be divided into two smaller problems.

First estimate absolute position and then use latter information to compute orientation. A common way to estimate absolute position, and the one that is studied on this thesis, is using distancesR_i from the robot to a set of points (x_ci, y_ci) which its position relative to a map is a priori known. In order to compute the position in a<^N space,N + 1 distances will be needed. Since this study uses a mobile robot, only (xt, yt) is relevant and therefore 3 distances are enough to solve the problem. More distances can be added to the problem in order to improve accuracy.

Figure 1: 3 Anchors Position Solution

(2)

There are several electronic devices that can measure distance Di through several techniques, each one with their own advantages and limitations. But what they have in common is that all of them return a value of distance affected with different types of noise sourcesεu.

D_i=R_i+X

u

ε_u (1)

This noise constitutes a problem because it mod- ifies the radius of the circumference making the intersection of the 3 circumferences resulting in a re- gion instead of a point or no intersection at all.

Therefore, geometric intersection of 3 circumferences can not be applied to solve the problem.

1.2. State-of-the-Art

Because of the importance about indoor position and pose knowledge, there are several studies about this topic among scientific community. As it was previous described position estimation is directly or indirectly performed using relative distance between some known points in a map and the point which its position knowledge is unknown. Exist- ing systems can be described as a stack of ab- straction layers. Starting from the bottom where is the hardware implementation of some technology which allows ranging capabilities in somehow.

Bluetooth and Wifi Access Points are similar tech- nologies which can operate in same wireless com- munication band. Although these kind of devices can be used as hardware infrastructure for both TOA or RSS range technique, RSS is preferable since TOA requires a hardware device with accurate clocks, and these may have up to a 1usdeviation on packet transmission timing which leads to a 300 m error on final distance measurement.[7]. UltraWide- Band (UWB) devices employing a totally different method of transmission, is suitable to perform ranging techniques like TOA of other deriva- tions with acceptable accuracy. The main concern about UWB is NLoS environment. The presence of obstacle between Anchor and Tags might degrade range estimation due to multi-path errors [3].

Some of ranging techniques implemented in latter hardware can be Time-of-Flight (TOA), Angle-of- Arrival (AoA) and Received Signal Strength (RSS).

TOA counts how much time does a signal takes from Tag to the Anchor and back again to the Tag. AoA uses a special antenna arrays in order to compute what angle of the Electromagnetic wave arrives to the Anchor coming from the tag. Using this information, position estimate can result from triangula- tion methods [8]. The fact that an electromagnetic signal, when traveling in some medium, decrease its strength by the inverse of squared travel distance, can be used to generate a propagation model which

its reasonable to estimate distance. In the last ab- straction layer is where position or pose estimation is actually computed. There are several algorithms available to accomplish this goal. Used on [4] Trilat- eration consists in minimizing the squared residuals between current measurements and the distances calculated from Anchors position and target position. Fingerprinting is commonly used only when RSS is available. On a first stage it is necessary to create a fingerprinting map which is a database matching real points in the map with corresponding RSS values. The second stage can use K-nearest neighbor(kNN) [1] which is a simple search problem in order to find the point in the map which corresponding RSS values on database minimizes the errors between latter and current RSS measured. Another way to use RSS is regressing its propagation model with Gaussian Process [5]. It is a less parametric regression model which allows to approximate an extremely wide range of non-linear functions. Another advantages is the correct han- dling of uncertainty by Gaussian Process. [2] has proposed a novel method called EPTA. It consists in an iterative method of finding the best ranging measurement neighbors that leads to the smallest area originated by the intersection of the 3 circumferences previously mentioned. [6] has proposed a method for pose estimation which is the fusion of UWb ranging measurements with Inertial Measure- ment Unit (IMU) composed by a gyroscope and ac- celerometer. They use extend kalman filter to pre- form this fusion in order to mitigate outliers from multi-path errors from UWB. This method, which appeal to velocity with robot moves, is referred by [10] as a good method to mitigate multi-path errors.

1.3. Contributions

The Main contribution of this thesis is a complete ready to use Ros Module for Independent Absolute Pose Estimation. This Module will be integrated on MOnarCH Project in order to increase robust- ness of the positioning abilities derived from relative positioning.

This work can be divided into the following sections.

• Mobile Robot Pose Estimation architecture comparative study comprising trilateration through minimization process and Fingerprint- ing method.

• An approach of fingerprinting method using Approximate Gaussian Process Regression in an attempt to mitigate the presence of persis- tent non-LoS multipath errors.

• Methods fusion system in order to enhance accuracy of estimates by retrieving the advantages of both methods.

(3)

1.4. Testbed

The test was performed on Instituto Superior Tc- nico on 8th floor. It consists on the movement of a robot along a path. In order to obtain orientation, the robot is carrying 2 devices called Tags identified by its ids (1,2) that will return measurement of distance to each Anchor also identified by its ID (1,2,3). These Tags are not in the center of the robot. This displacement has to be taken into account. So they are placed in a known position (x_jr, y_jr) relatively to the robot referential.

And from now on, distance measurements will be referred asdij, meaning distance from Anchorito Tagj. Anchors were placed 2 in such a way it covers as much as possible area.

Figure 2: Map with Approximation Anchors Posi- tion

Regardingd_ij, there was the necessity to derive a statistical model comprising LoS and NLoS situations described on 2

dij =rij+los+nlos (2) where rij is real distance and los characterizes noise in Line-of-Sight (Los) situation. This noise can be seen as a random variable with an approxi- mated distribution of 3

los ∼ N(0, σ²) (3) nlosdepends on multi-path errors from NLoS environment and it characterization is somehow diffi- cult.

1.5. Structure

This thesis are divided into 3 sections.

• First section is dedicated to Position Esti- mation which describes a chosen Trilateration method from the public domain. It also pro- poses a fingerprinting method approach using an approximate Gaussian Process in an attempt to mitigate NLoS errors.

• Second section deepens the pose estimation subject with a comparative study on pose estimation architectures.

• Finally a section where an semi-autonomous method is described to find Anchors locations.

2. Position Estimation 2.1. Trilateration

It is now clear that the optimal situation is when there is no error. Therefore, the most suitable crite- rion for these cases is to minimize the error between real and measurement distancerij−dijfor any Tag jand all Anchorsi. One common way to minimize a function is using the well known Least Squares Error. This leads to the following cost function

Jj=

3

X

i=1

(nji−dji)² (4)

n_ji=p

(x−x_ci)²+ (y−y_ci)²) (5) However equation 5 is non-linear regarding (x, y), thus the common solution of solving Least Squares has no closed-form. Hence the problem become Non-Linear Least Squares. An iterative algorithm of minimization that uses the direction of gradient can be applied in order to find the value of (˜xj,y˜j) for which the cost function 4 is minimum leading to an approximate value of the real position of the Tag. Thus the used minimizer is ”Sequential Least Squares Programming” implementation from Scipy Optimize Module which supports constraint minimization.

2.2. Results

0 50 100 150 200

0 20 40 60 80 100 120 140 160 180

time[s]

Abs Error [cm]

Figure 3: Tag Position Absolute Error Estimate

X [cm]

Y [cm]

150 200 250 300 350 400 450 500 550

Groundtruth Estimation

Figure 4: Tag XY path Position Estimate As it can be observed on figures 3 and 4, simple estimation of tags position produce poor results. It

(4)

might be due to several sources of errors such as multi-path errors; bad Anchors positioning or computational process delay. As previously described, UWB devices is highly prone to multi-path errors which will degrade the final position estimation.

These results will affect Pose estimation. Further studies has to be made in order to prevent this propagation.

T ag M SE(abs) 3.7868×10³ Table 1: MSE of Tag’s Position 2.3. Fingerprinting

Fingerprinting is done using Gaussian Process. It uses database created at first stage to learn how the set of 3 distances are related with position i. e Function Regression . By doing this, ranging measurements affected by multi-path errors but belonging to the training set will lead to the real position of the robot. It is clear that a high quality training set is required to obtain an acceptable results. A problem arises from the Gaussian problem model.

It assumes independent variable to be noise-free i.e deterministic, although ranging measurements can be characterized by 2 which is a sum of random variable with a specific distribution making the posterior distribution of robot position intractable.

Some modifications was proposed on [9] in order to train a Gaussian Process with noisy-inputs but due its high complexity and computational power consumption its implementation is considered to be out of scope of this thesis. Therefore in order to test this method an approximation was made to distance model. los= 0 andnlos6= 0 when NLoS situation.

Gaussian Process being a Kernel-based probability distribution, predictions are strongly dependent on how well covariance functions are chosen. Some of the most common kernels has been tested in order to study the impact this choice has on the performance of estimate. Squared Exponential, Absolute Exponential, Infinite Exponential and Linear kernel have been tested. Although they all have produced poor results, absolute Exponential is what has produced better results depicted on figures 5 and 6.

T ag M SE(pos) 4.80×10³ Table 2: MSE(Absolute Exponential) 2.4. Kid Positioning

Although the main focus of this thesis is a robot pose estimation. MOnarCH also requires to estimate a position of a kid. This information is useful when playing ”hide and seek” for example. This

0 50 100 150 200

0 50 100 150 200 250

time[s]

Abs Error [cm]

Figure 5: Tag Position Absolute Error Estimate

X [cm]

Y [cm]

150 200 250 300 350 400 450 500 550

Groundtruth Estimation

Figure 6: Tag XY path Position Estimate

requirement can easily accomplish my attaching to the tag, which will be carried by the kid, a embed- ded Linux that establish a Wifi connection with the robot a send a set of ranging measurements in order to robot process them and find the kid.

3. Pose Estimation

Although the results of position estimation through UWB ranging is quite acceptable, pose estimation using only this technology is more challenging. If is feasible to estimate robot’s position with one tag attached to it, orientation could be computed using previous techniques to find 2 Tag’s placed on robot.

This section is dedicated to test different isolated approaches regarding pose estimation in order to find the one that shows highest performance values.

Then a fusion process is proposed in an attempt to take the advantages of both methods. Uncoupled approach are present as the simplest one. It uses tags position previously estimated to estimate final robot position and orientation through a set of transformations. Latter could be a geometric transformation that takes into account tags displacement relatively to robot referential. However this transformation could be too restrictive leading to an increase of estimation errors. Therefore a problem relaxation was made resulting in the following set of equations.

(˜x_t) =x˜1+ ˜x2

2 (6)

(˜yt) = y˜1+ ˜y2

2 (7)

(5)

θt=atan2(˜x1−x˜2,y˜2−y˜1) (8) With this, it is possible to estimate tags position through Trilateration or Fingerprinting and then compute final robots pose. It is worth mentioning that it is expected some errors regarding orientation due to nature of 8. Computing error propagation through it, reveals its high sensibility to prior tags position estimates.

Due to probabilistic nature of Gaussian Process, one advantages of this method is the entire probability distribution as estimation output. Thus it is possible to use variance as a uncertainty measure of quality of estimate. However Trilateration does not provide such measure. Therefore there was a necessity to estimate this uncertainty for Trilater- ation, but being an iterative minimization process, its analytical expression is not available. Unscented Transform (UT) suits our problem perfectly. It is a process to estimate the result of applying a non- linar function to a set of input random variables characterized by mean µ and covariance Σ. With this it is possible to overestimate uncertainty of trilateration which will be also useful to fusion process. Coupled approach arises in an attempt to mitigate this sensibility from position estimation. By doing geometric transformation, which relates tags position relatively to robot referential with robot posture relatively to map referential, before minimization process, for trilateration method, it is expected that error propagation would be also min- imized. Therefore resulting cost function 10 will take into account all ranging measurements to both tags comprising its relation with robots pose.

Dj =



 d1j

d2j

d3j



 (9)

Jc(x, y, θ, D1, D2) (10) Regarding Fingerprinting, transforming it to coupled approach is a matter of increase function regression dimensionality. On the first stage of this method, instead of creating a map for each tags position, only one map will be created matching all ranging measurements from both tags to robots pose (x_t, y_t, θ_t). Due to bounded domain of robots orientation [0,2π[, discontinuities may arises on observed variables. Since Gaussian Process smooth- ness property, it will miss some estimates decreasing its performance. One way to solve this situation is by transformθtin such a way that it keeps differen- tiable on all of its the domain described by figure 7.

This leads to a new model described by equation11

(xt, yt, cos(θt), sin(θt)) =f(D^∗₁, D^∗₂) (11)

0 500 1000 1500 2000

0 5 10

Theta [rad]

Time [s]

0 500 1000 1500 2000

−1 0 1

sin(Theta) [m]

Time [s]

0 500 1000 1500 2000

−1 0 1

Time [s]

cos(Theta) [m]

Figure 7: Discontinuity Problem ofθt

As described previously, both method as its own advantages and limitations. Since, at this point, there are uncertainty values available from both methods it is possible to fusion their results using equation 12 onx, y, θ. This is computing a weighted average based on inverse variance from the method, meaning as much confidence about its result one has regarding the other method, more importance it will has in the final result.

w_n= σ_LSQ² w_{F P} +σ_{F P}² w_LSQ

σ²_{F P}+σ_LSQ² (12) In order to test the performance of these approachs the following tests were made: Fusion Coupled (FC) - Merging estimates from Coupled methods; Fusion Uncoupled (FU) - Merging estimates from Uncou- pled Methods; Fusion Mixture 1 (FM1) - Merging estimates from Coupled Trilateration (CTri) with Uncoupled Fingerprinting; Fusion Mixture 2 (FM2) - Merging estimates from Uncoupled Trilateration and Coupled Fingerprinting. The result are depicted at figure 8

C Tri C Fg U Tri U Fg FC FU FM1 FM2 0

1000 2000 3000 4000 5000 6000

MSE Abs Error

(a) MSE Abs

C TriC Fg U TriU Fg FC FU FM1 FM2 0

0.5 1 1.5 2 2.5

MSE Orietation Error

(b) MSEθ

Figure 8: MSE Methods/Architecture Comparison As it was expected, problems from Position will propagate to Pose estimation. The main focus of this thesis is to minimize as much as possible this propagation. By observing figure 8, it is possible to conclude that Coupled methodologies works best. Regarding orientation Uncoupled methodologies does not mitigate error propagation of position

(6)

estimation. For this test, Fusion methodologies has returned better estimates than isolated ones. At it can be seen, FC which is merging Coupled methodologies has returned the minimum MSE. However this result could not be the best for every situation.

Fusion methodologies are dependent on several variables: Quality of Isolated methodologies and quality of uncertainty values. But, in turn, quality of Isolated methodologies are dependent of NLoS environment for Trilateration and quality of training set for Fingerprinting.

4. Mapping

Absolute positioning estimation has initial disad- vantage of needing priori knlowledge of Anchors position regarding a map. In most of the cases knowing this position is not feasible or not possible at all.

One solution is to inverse the Position estimation of the Tag using Anchors. Now Tag position must be known. Thus this method needs to rely on position knowledge from other methods such as amcl. Tri- lateration method can be seen as a generic method to, given a set of 3 points and distances from these points to a fourth point, compute the values of this latter point. Therefore, let these 3 points be Tag position taken at different instants of time 9. For each instant of time, distance to a specific Anchor is measured. At this point, one has every variables to compute position of that Anchor. These can be generalized to all Anchor.

Since there are already the necessity to build a map for position estimation through fingerprinting, and this map is a table matching Tag’s position and distances to Anchors, it is possible to extract enought data to execute this previusly described method.

Figure 9: Robot path

An aspect that might be concerning the reader is about what was told in ”Position Estimation through trilateration” section: ”In order to prop- erly estimate a point, this must lies inside the triangle formed the 3 known points”. In this case, Tag’s positions at different instants of time would be such that those points forms a triangle in which real An- chor position is inside. This concern is grounded as

Anchor would be placed into walls making impos- sible to robot to move arround it. This problem is mitigated by increasing known points. Instead of using Tag’s position at only 3 different instants of time, that number is increased, each one contribut- ing to the real position of the Anchor. Each time a Tag’s position is added, one can compute Anchor’s position and save. Doing this until Anchor’s position stops vary, let the algorithm converge to the real Anchor’s position 10.

Figure 10: Anchors Position Estimate 5. Conclusions

C Tri C Fg U Tri U Fg FC FU FM1 FM2 0

1000 2000 3000 4000 5000 6000 7000

MSE Abs Error

Path Thesis Path 1 Path 2

(a) MSE abs tests comparison

C Tri C Fg U Tri U Fg FC FU FM1 FM2

0 0.2 0.4 0.6 0.8 1 1.2 1.4

MSE Orietation Error

Path Thesis Path 1 Path 2

(b) MSEθtests comparison

Figure 11: MSE Test Comparison

The following tests are called stress tests due to the fact in order to create them, the robot has moved along a path which for some of (xt, yt, θt), they are far from those belonging to fingerprinting

(7)

training set. As it can be seen on figure 11, the errors on orientation estimation are increasing with tests as its difficulty increase. It is worth mentioning that Fingerprinting methods has more incon- sistencies than Trilateration. This suggests, again, the high dependency of Fingerprinting on its training set. Coupled Trilateration is a good starting point to mitigate some errors from ranging measurements.It has also the advantage of being lightweight in terms of complexity and therefore does not re- quire a lot of computational power. From the results one can conclude that it is possible to use fingerprinting to improve trilateration estimates. This is confirmed by Fusion Coupled that has been showing solid results along all tests. In almost every test it is the best estimate. Although the overall results has not medical precision, they are acceptable to simple situations were minor errors are not a problem. A better purpose to this estimates is to compose a positioning system such as AMCl which will introduce redundancy and mitigate limitations of relative positioning methods, improving the overall performance of positioning system.

Acknowledgements

This work could mean the end of an important jour- ney of my life. There was such a pleasure to be able to give my contribute to this project, for that i am thunkfull to my Professor and Research Advisor Ro- drigo Ventura. Not only that but for all the time he spent giving me advices and showing me the correct path which allowed me to conclude this work.

I am also thankfull to my collegue Joo Mendes who supported me with every detail i needed to be in- volved in this project. At least, i want to thanks my friend Alexandre Emdio who have helped me with long discussions about algorithms. This degree rep- resents to me a valuable tool which i will carry to rest of my life.

References

[1] T. Alhmiedat, G. Samara, and A. O. A. Salem.

An indoor fingerprinting localization approach for zigbee wireless sensor networks. arXiv preprint arXiv:1308.1809, 2013.

[2] P. Brida and J. Machaj. A novel enhanced positioning trilateration algorithm implemented for medical implant in-body localization. In- ternational Journal of Antennas and Propaga- tion, 2013, 2013.

[3] D. Dardari, A. Conti, U. Ferner, A. Giorgetti, and M. Z. Win. Ranging with ultrawide band- width signals in multipath environments. Pro- ceedings of the IEEE, 97(2):404–426, 2009.

[4] D. Espes, A. M. Pistea, C. Canaff, I. Iordache, P. L. Parc, and E. Radoi. New method for

localization and human being detection using uwb technology: Helpful solution for rescue robots. arXiv preprint arXiv:1312.4162, 2013.

[5] B. Ferris, D. Hhnel, and D. Fox. Gaussian pro- cesses for signal strength-based location estimation. In In Proc. of Robotics Science and Systems, 2006.

[6] J. D. Hol, F. Dijkstra, H. J. Luinge, and P. J.

Slycke. Tightly coupled uwb/imu pose estimation system and method. Technical report, Patent US 8.203.487 B2, 2012.

[7] A. Kotanen, M. Hnnikinen, H. Leppkoski, and T. D. Hmlinen, editors. Experiments on Local Positioning with Bluetooth, Korkeakoulunkatu 1 FIN-33720 Tampere Finland, 2003. Institute of Digital and Computer Systems.

[8] P. Ku lakowski, J. Vales-Alonso, E. Egea- L´opez, W. Ludwin, and J. Garc´ıa-Haro. Angle- of-arrival localization based on antenna arrays for wireless sensor networks. Computers &

Electrical Engineering, 36(6):1181–1186, 2010.

[9] A. Mchutchon and C. E. Rasmussen. Gaussian process training with input noise. In J. Shawe- Taylor, R. Zemel, P. Bartlett, F. Pereira, and K. Weinberger, editors,Advances in Neural In- formation Processing Systems 24, pages 1341–

1349. Curran Associates, Inc., 2011.

[10] C.-D. Wann and C.-S. Hsueh. Non-line of sight error mitigation in ultra-wideband ranging systems using biased kalman filtering. Journal of Signal Processing Systems, 64(3):389–400, 2011.