Ultra Wide-Band Trajectory Following for anOmnidirectional Factory Automation Vehicle

Micho Radovnikovich, Paul Fleck and Kevin HallenbeckDataspeed, Inc.

Troy, Michigan 48098

Abstract—In this paper, an ultra wide-band (UWB) local-ization system is presented that is used in conjunction withon-board odometry sensors to precisely guide an autonomousfactory vehicle along a specified path. A precise UWB-basedsystem has many advantages over conventional automated pathfollowing technologies, but the most significant advantage is itsreconfigurability. Paths are specified in software, and changingthe path does not require any changes in factory infrastructure.Simulation and real-time experimental results demonstrate thatby combining localization estimates with odometry data, it isfeasible to achieve a very capable path following system in apractical factory environment.

I. INTRODUCTION

Current autonomous factory vehicles establish a navigationsystem using technologies like magnetic tape paths, and laserand optical localization systems. These techniques requireextensive infrastructure and are not easily reconfigured, whichlimits the usefulness of this technology. The work presentedin this paper shows that ultra wide-band (UWB) rangingtechnology is a viable alternative to conventional factorynavigation systems that can achieve the same accuracy andexceed capability.

In recent years, ultra-wideband (UWB) based localizationsystems have become feasible to implement in real-worldindoor localization applications because of the advancementand commercialization of the technology, combined with theirnon-line of sight capability. UWB localization research for themost part focuses on the signal processing of the UWB radiosignals and using different techniques for localizing the targetradios. Detailed and theoretical analyses of several differentUWB localization methods can be found in [1] and [2].

In most cases, UWB localization systems use a fixed arrayof base radios, whose geometry is known. The positions ofadditional radios are then tracked using a particular technique.In the literature, many approaches use the time difference ofarrival (TDOA) method of localization [3, 4]. In TDOA, atarget radio transmits a pulse that each of the base radiosreceive. Based on the different arrival times of the radio packet,the position of the target radio can be computed.

Many other approaches use the time of arrival (TOA)method [5, 6, 7]. In TOA, the time of flight of the radio packetis explicitly measured and converted into a correspondingrange using the known speed of light.

In this paper, the UWB localization system is developedusing the PulsOnr410 off-the-shelf ranging radio producedby TimeDomainr. The PulsOn radios compute the time of

flight of a UWB packet to produce an explicit measurementof the range between two radios.

The localization estimates derived from the raw rangemeasurements from the PulsOn radios are used to guide anautonomous factory vehicle along a specified path, wherethe vehicle augments the raw localization estimate by fusingit with on-board odometry sensors. Fusing odometry withUWB is successfully used in the literature, although manyof the techniques are applied to pedestrian position tracking[8, 9, 10]. This paper represents a continuation of the workpresented in [11], where significant improvements have beenmade in the effectiveness of the localization and sensor fusionalgorithms.

Real-time experiments were conducted with the internallydeveloped Dataspeed Inc. product called the Smart TrackingTransceiver (STT). The STT packages a PulsOn UWB radiowith the sensor chips that make up a six-axis IMU, aswell as on-board Bluetooth, USB and CAN connectivity forintegration in a multitude of different systems. Additionally,the STT includes a 32-bit CPU to run on-board sensor fusionalgorithms, hardware to log data to a memory card, and anintegrated, rechargable battery for extended mobile operation.The STT is packaged in a small form factor that is housedin an IP68 sealed enclosure for maximum durability in harshenvironments.

II. UWB LOCALIZATION

A. Range Filtering

UWB range measurements are very susceptible to multi-path reflections, resulting in spurious range data that disruptslocalization estimates drastically. A simple, yet effective fuzzylogic system is used to identify outliers and remove thembefore being used to produce a localization estimate.

Whenever a range to a target is made, the time stamp, baseradio ID and target radio ID are recorded along with it. Thenext time a range is made with the same base and target IDs,an implied velocity is computed:

vk =rk − rk−1

tk − tk−1

where rk is the current range sample, rk−1 is the previousrange sample, and tk and tk−1 are the time stamps.

If the implied velocity is very large, that means the currentrange measurement is most likely incorrect. By putting amaximum limit vmax on the velocity of a target, a simple fuzzy

membership function O(vk) can be constructed to classify agiven range measurement as an outlier. The fuzzy membershipis shown in Figure 1. The filtered range measurement r isthen a linear combination of the current and previous rangemeasurements:

rk = [1−O(vk)] rk + [O(vk)] rk−1

6

-����

vmax vmax + ∆v

1

O(vk)

Not Outlier

Outlier

vk (m/s)

Fig. 1. Outlier detection membership function.

B. Range Projection

The localization system presented in this paper is onlyconcerned with 2-D positioning. However, while it is assumedthat none of the radios in the system will change heightsduring operation, they are not necessarily at the same height.Therefore, the filtered line-of-sight range measurements r areprojected into a lateral range measurement by

l =√r2 −∆h2 (1)

where ∆h is the height difference between the two radiosinvolved in the range measurement.

C. Localization Algorithm

The proposed 2-D UWB localization algorithm is summa-rized in the flowchart in Figure 2. After obtaining a goodheight calibration, the algorithm checks to see if the lastlocalization estimate of the target exists. If there is no pastestimate, or if it is too old, then the position is estimatedexplicitly from three range measurements. If there is a highquality past position estimate, then two ranges are used toupdate the estimate using triangulation.

1) Explicit Trilateration: If the geometry of the base radiosis known and ranges from three different base radios aresampled, the 2-D position of the target can be solved in closedform by[

xtyt

]=

x2 − x1 y2 − y1x3 − x2 y3 − y2x1 − x3 y1 − y3

+

(x22 − x21) + (y22 − y21)− (l22t − l21t)(x23 − x22) + (y23 − y22)− (l23t − l22t)(x21 − x23) + (y21 − y23)− (l21t − l23t)

(2)

where (xt, yt) is the estimate of the target’s position,(x1,2,3, y1,2,3) are the coordinates of the three base radios,and lit are the lateral range measurements between base radioi and the target. The + operator represents the Moore-Penrosepseudo inverse, where [·]+ = ([·]T [·])−1[·]T .

(x1, y1)

(x2, y2)

d

θ

α

β

σ1 σ2

∆x

∆y

(xt, yt)d2t

d1t

Fig. 3. Geometry of the implicit triangulation.

2) Implicit Triangulation: Normally, using only two baseradios does not yield a unique 2-D localization solution.However, if a previous estimate (xt,k−1, yt,k−1) of the target’sposition is available, then the multi-valued solution can beresolved. The geometry of the implicit triangulation problemis shown in Figure 3, where d1t and d2t are the distances fromthe two base radios to the target based on the past estimate ofthe target’s location. These are distinguished from the lateraldistances l1t and l2t, which are measured quantities based onthe raw range measurements from the radios.

After sampling the lateral ranges l1t and l2t from the twoselected base radios to the target, the θ and α angles arecomputed by

θ = atan2(x2 − x1, y2 − y1) (3)

α = cos−1

(l21t + l22t − d2

2l1tl2t

)(4)

After computing θ and α, ∆x and ∆y are given by

∆x = l1t sin(θ ± α), ∆y = l1t cos(θ ± α)

Each of the two solutions for ∆x and ∆y are added to (x1, y1)to get the new localization estimate. This multi-valued solutionis resolved by taking the one that is close to the previousestimate.

If neither solution is within a distance tolerance ε of theprevious estimate, the algorithm is not confident in its estimate,and then aborts the procedure and instead performs an explicittrilateration. This procedure is summarized in Algorithm 1. Inthe case where the domain of the cos−1 function is violated in(4) or (5) because of poor distance measurements, the implicittriangulation is aborted and an explicit trilateration is initiatedinstead.

D. Selecting an Optimal Pair of Base Radios

An important piece of the localization algorithm is toselect which two base radios should be used when performingimplicit triangulation. If there are n base radios, then there areN =

∑n−1i=1 i unique pairs of radios that can be selected for

implicit triangulation.The task is to select the base radio pair that yields the

highest quality localization estimate. This selection is based

Fig. 2. Flowchart of the UWB localization algorithm.

Algorithm 1 — Localization using implicit triangulation.1: Input (xt,k−1, yt,k−1), (x1, y1) and (x2, y2)2: Sample lateral ranges l1t and l2t3: Compute θ and α from (3) and (4)4: ∆x+ ← l1t sin(θ + α), ∆y+ ← l1t cos(θ + α)5: ∆x− ← l1t sin(θ − α), ∆y− ← l1t cos(θ − α)

6: d+ ←√

(xt,k−1 − x1 −∆x+)2 + (yt,k−1 − y1 −∆y+)2

7: d− ←√

(xt,k−1 − x1 −∆x−)2 + (yt,k−1 − y1 −∆y−)2

8: if min(d+, d−) > ε then9: return . Abort and do explicit trilateration

10: end if11: if d+ < d− then12: xt,k ← x1 + ∆x+, yt,k ← y1 + ∆y+

13: else14: xt,k ← x1 + ∆x−, yt,k ← y1 + ∆y−

15: end if16: return (xt,k, yt,k)

on a cost function that incorporates many different factors,including:

• Shape of the triangle formed between the two base radiosand the target radio, as illustrated in Figure 3.

• Distance from last localization estimate to the base radios• Which pair of radios was used in the last triangulation

cycleOne of the biggest considerations is the shape of the trian-

gle. The localization estimate can vary within the intersectionarea of the range uncertainty annuli shown in Figure 3, whoseradii differences are equal to the standard deviations σ1 and σ2of the range measurements. The intersection area is minimizedif the angle β, given by

β = cos−1

(d21t + d22t − d2

2d1td2t

)(5)

is equal to π/2 radians. Therefore, pairs of base radios thatyield β close to π/2 tend to have lower cost and are selected

more often.Distance from the current localization estimate to the base

radios is also taken into consideration. In the cluttered environ-ment where the experiments were performed, it was found thatUWB signal quality started to deteriorate at ranges of greaterthan 90 meters. Therefore, if a pair of base radios containsone that is far away from the current localization estimate, itis assigned a higher cost value.

Finally, if a specific pair of base radios was not used inthe previous triangulation step, more cost is added to includehysteresis that prevents rapid switching between base radiopairs. The algorithm for finding the optimal pair of radios isoutlined in Algorithm 2.

Algorithm 2 — Find the best two radios for triangulation.1: Input: Last localization estimate (xt, yt)2: Corresponding base radios (x1, y1) and (x2, y2)3: for each unique pair of base radios do4: c← 0, cmin ←∞ . Initialize cost to zero5: Compute β from (5)6: c← c+ cangle|β − π/2| . Bad geometry cost7: c← c+ distCost (max(d1t, d2t)). Long range cost8: if radio pair not (x1, y1) and (x2, y2) then9: c← c+ chyst . Hysteresis cost

10: end if11: if c < cmin then cmin ← c12: end for13: return radio pair corresponding to cmin

The distCost function on Line 7 is illustrated in Fig-ure 4. The parameters cangle, chyst and cdist are specified costweighting constants.

III. SENSOR FUSION

Using UWB localization alone is not sufficient for a ro-bust local positioning system in a factory environment. Thedeficiencies of a strictly UWB-based solution are:

6

-����

90 m 110 m

cdist

0

distCost (d)

d

Fig. 4. Base radio selection cost function for being a long distance from thetarget position.

• Inability to robustly detect velocity and orientation of thetarget.

• Limited bandwidth – As more target radios are added tothe system, the update rate slows because the range mea-surements used for localization have to be multiplexedamong all target radios.

• Random communication dropouts caused by environmen-tal effects.

All of these deficiencies can be addressed by using the UWBlocalization estimate in conjunction with the other sensors onthe vehicle, including the wheel encoders and an IMU.

If the UWB localization drops out, the system can stillrely on the wheel speed and inertial sensors to maintain areasonably accurate position and orientation estimate. When-ever the UWB position measurements come back, they areused to correct any drift that has accumulated during the dropout period. The rest of this section describes the design of aKalman Filter that uses all the sensor information to producea robust estimate of the position, velocity and orientation.

A. State Dynamics

Consider an omnidirectional vehicle (ODV) moving in theglobal UWB reference frame, as shown in Figure 5. Thesystem dynamics are represented in state space form by

X = f(X,U)⇒

x = vx cosψ − vy sinψy = vx sinψ + vy cosψ

ψ = ψc

(6)

where the state vector X = [x y ψ]T consists of theposition and heading angle in the UWB frame, and the controlvector U = [vx vy ψc]

T consists of the independent valuesof the forward, lateral and rotational velocities, respectively.

vxvy

ψψc

x

y

Fig. 5. Omnidirectional ground vehicle moving in the UWB reference frame.

ω1 ω2

ω3 ω4

vx

ψvy

Fig. 6. Kinematics of a Mecanum-wheeled ODV.

B. ODV Kinematics

The experimental vehicle is an ODV that uses Mecanumwheels. A diagram illustrating the Mecanum-wheeled kine-matics is shown in Figure 6. A Mecanum ODV is driven byfour independent wheels, and the kinematics relate the wheelspeed signals to the system model’s control vector by

ω1

ω2

ω3

ω4

=

a −b −ca b ca b −ca −b c

vxvyψ

where ω1 through ω4 are the individual wheel speeds, and thea, b and c parameters are functions of the wheel radius andvehicle geometry.

C. Kalman Filter Observer

An Extended Kalman Filter (EKF) is used to processthe inherently noisy and incomplete vehicle sensor data andcombine it with the raw UWB localization estimate to producean estimate of the vehicle state X.

In addition to measuring the states of the system (6), it isalso desired to estimate the values of the control commands,as measured by the vehicle sensors. Therefore, the discreteEKF observer dynamics model is:

Xk+1 = f(Xk)⇒

xk+1 = xk + Ts[vx,k cos ψk − vy,k sin ψk]

yk+1 = yk + Ts[vx,k sin ψk + vy,k cos ψk]

ψk+1 = ψk + Tsˆψk

vx,k+1 = vx,kvy,k+1 = vy,kˆψk+1 =

ˆψk

(7)

where the · accent represents an estimated quantity, and theEKF state vector X = [ x y ψ vx, vy

ˆψ ]T .

The vehicle is equipped with wheel encoders to measurethe vehicle’s speed and a gyroscope to measure the yaw rate.It also receives the raw localization measurement from theUWB radios. These sensors make up the measurement vectoryk = h(Xk), where the measurement model function h(X) is

given by

yk = h(Xk

)⇒

UWBx = xkUWBy = yk

encvx = vxkencvy = vyk

encψ =ˆψk

gyro =ˆψk

(8)

The EKF model’s state and measurement Jacobian matricesare then defined by

Ak =∂f(X)

∂X, Ck =

∂h(X)

∂X=

1 0 0 0 0 00 1 0 0 0 00 0 0 1 0 00 0 0 0 1 00 0 0 0 0 10 0 0 0 0 1

The well-known EKF algorithm is then applied using thedynamics model (7), the measurement model (8), the Jacobianmatrices Ak and Ck, and the raw measurement vector yk.

On filter iteration steps where new UWB localization data isnot available, then it is not included in that step’s measurementvector yk, and the top two rows of the measurement modeland measurement Jacobian matrix are eliminated. This is doneso that the Kalman Filter does not try to track stale positionmeasurements, but instead relies on the speed and yaw ratesensors until new position data is available.

IV. RESULTS

Figure 7 shows a block diagram of how the real-timeexperiment was conducted. A central computer interfaces withthe four base radios and executes the localization system asdescribed by the flowchart in Figure 2. The current localizationestimate is transmitted over the UWB channel to the STTunit that is on the experimental ODV. The STT receiveswheel encoder data over USB from an on-board controlcomputer, and runs the EKF sensor fusion filter described inSection III-C. The STT then outputs the complete position,heading and velocity estimate to the control computer. Thecontrol computer is responsible for using this estimate to guidethe ODV along a specified path.

The ODV used in the experiments is shown in Figure 8a.It is equipped with an STT that implements the KalmanFilter sensor fusion algorithm to produce a robust position andorientation estimate, wheel encoders to measure the forward,lateral and rotation speed of the vehicle, and an on-boardcomputer that interfaces to the devices and runs the path-following algorithms.

The on-board computer is a custom designed product fromDataspeed Inc. called the Multi-Sensor Fusion Controller(MSFC). The MSFC contains hardware for CAN, USB, andmultiple Ethernet connections, and interfaces with a Kontronquad-core i7 Computer-on-Module (COM) single-board PC.the COM board runs Ubuntu and ROS, and is used to run thepath-following control software.

Fig. 7. Real-time experimental set-up. A central computer runs the local-ization algorithm by controlling the base STT units. The STT on the robotruns the sensor fusion filter and provides the on-board computer with a robuststate estimate. The on-board computer runs the path-following algorithm toautonomously follow a pre-recorded path.

(a) Experimental ODVwith a mounted STTdevice.

(b) One of the four base radioSTT devices mounted on thewall of the test facility.

Fig. 8. Pictures of the experimental equipment.

Paths are generated by first driving the vehicle manuallywhile recording the position data coming from the UWBlocalization system. The resulting points are post-processedto fit a sequence of Bezier curve segments. A path followingalgorithm is then used to guide the vehicle autonomously onthis fitted path.

Figure 9 shows a visualization of the robot following a pre-recorded path. While following the path, the vehicle was also

Fig. 9. Experimental ODV following a pre-recorded path while aligning itsheading with the tangent angle of the curve.

Fig. 10. Panoramic view of the experimental test facility.

Fig. 11. Detailed ROS simulation environment emulating the real test facility.Ranging characteristics of the TimeDomain radios are simulated, along withdisturbance anomalies that were detected at the real facility.

instructed to align itself with the tangent angle of the Beziercurve. The arrows represent the Kalman Filter position andorientation estimates as the vehicle moved along the path.

The experiments were conducted in a 40,000 square footmanufacturing facility, shown in Figure 10. This facility isrepresentative of a typical manufacturing operation, wherethere are metal racks between the alleys which contain avariety of materials.

To develop and debug the localization system, a detailedsimulation environment was developed in Robot OperatingSystem (ROS) to accurately emulate the real geometry of thebase radios, as well as the environmental anomalies frequentlyencountered in the real system. Screenshots of the simulationin the Rviz visualization tool are shown in Figure 11.

A world model was developed in the Gazebo tool thatsimulates the dynamics of the ODV, including the interactionof the rollers on the Mecanum wheels with the floor. Thecurrent position of the ODV is used to simulate the rangemeasurements from the actual TimeDomain UWB radios.Signal disturbance areas are placed manually to simulateenvironmental anomalies that have shown to cause minorwarping of the range measurements. If a simulated line-of-sight range passes through a disturbance area, its simulatedrange measurement is randomly affected.

V. FUTURE WORK

Future work will focus on formal testing to further quantifythe ranging capability in the presence of obstructions like bins,

fixed racks and equipment. Based on this analysis, methodsof predicting optimal base radio positions using simulatedsignal strength models and factory floor CAD models wouldbe investigated. The use of confidence maps will also beinvestigated, where a priori knowledge of the factory layoutand the expected UWB behavior will be used to estimatethe quality of the UWB range measurements in a given area.This knowledge would allow the vehicle to rely less on thelocalization estimate in uncertain situations and instead relymore on the odometry data.

VI. CONCLUSION

The localization and sensor fusion techniques presented inthis paper demonstrate the feasibility of using UWB-basedlocalization systems in a factory automation application. TheUWB localization alone would be insufficient, but by fusingthe position estimate with the on-board odometry sensors, afactory vehicle is able to follow trajectories very precisely.

REFERENCES

[1] H. Soganci et al., “Accurate positioning in ultra-wideband systems,”IEEE Trans. Wireless Commun., pp. 19–27, Apr. 2011.

[2] D. Dardari et al., “Ranging with ultrawide bandwidth signals in multi-path environments,” Proc. IEEE, vol. 97, no. 2, pp. 404–426, Feb. 2009.

[3] R. Ye et al., “High-precision indoor UWB localization: Technicalchallenges and method,” in 2010 IEEE Int. Conf. on Ultra-Wideband.IEEE, Sep. 2010, pp. 1–4.

[4] M. Segura et al., “Experimental demonstration of self-localized ultrawideband indoor mobile robot navigation system,” in 2010 Int. Conf.Indoor Positioning and Indoor Navigation, Sep. 2010, pp. 1–9.

[5] M. Dashti and M. Ghoraishi, “High-precision time-of-arrival estimationfor UWB localizers in indoor multipath channels,” in Novel Applicationsof the UWB Technologies, B. Lembrikov, Ed. InTech, 2011, ch. 19.

[6] D. Kocur et al., “Through-the-wall localization of a moving target bytwo independent ultra wideband (UWB) radar systems.” Sensors (Basel,Switzerland), vol. 13, no. 9, pp. 11 969–97, Jan. 2013.

[7] P. Tome et al., “UWB-based local positioning system: from a small-scaleexperimental platform to a large-scale deployable system,” in 2010 Int.Conf. Indoor Positioning and Indoor Navigation, no. September, 2010,pp. 1–10.

[8] L. Zwirello et al., “Sensor data fusion in UWB-supported inertialnavigation systems for indoor navigation,” in 2013 IEEE Int. Conf.Robotics and Automation. IEEE, May 2013, pp. 3154–3159.

[9] J. a. Corrales et al., “Hybrid tracking of human operators usingIMU/UWB data fusion by a Kalman filter,” in Proc. 3rd Int. Conf. onHuman Robot Interaction. New York, New York, USA: ACM Press,2008, p. 193.

[10] J. Youssef et al., “Loosely-coupled IR-UWB handset and ankle-mountedinertial unit for indoor navigation,” in 2011 IEEE Int. Conf. Ultra-Wideband. IEEE, Sep. 2011, pp. 160–164.

[11] K. C. Cheok et al., “UWB tracking of mobile robots,” in IEEE Int.Symp. Personal, Indoor and Mobile Radio Commun., Istanbul, Turkey,2010.