mobile tracking - uppsala university · mobile tracking robots can be very versatile and useful in...

MOBILE TRACKING

SALIOU CIZANIKLAS GUNNARSSONMANNE HENRIKSSON

JIMMY LY

Abstract. The goal of this project was to design and construct a mobile tracking robotusing the LEGO Mindstorms NXT set. Using information from a camera onboard thefollower robot, the object was to follow a leader robot which moves in a random manner.A self-balancing robot and remote control via Bluetooth was also added to the project.Three methods have been tested to solve the problem; Alpha-Beta filter, linear Kalmanfilter and particle filter. To be able to use the linear Kalman filter, a linear systemrepresentation is needed. A system identification has been done using a black box modelapproach. The PID controller was implemented to control the final systems. An LQGcontroller was also tested but the performance was not satisfactory. All methods weresuccessfully implemented with varying performance. The follower robot using the linearKalman filter was the most responsive and gave the most satisfying result. The particlefilter was limited by the CPU power of the Brick.

Date: January 21, 2013.

1

2 SALIOU CIZA NIKLAS GUNNARSSON MANNE HENRIKSSON JIMMY LY

1. Introduction 31.1. Problem formulation 31.2. State of the art analysis 41.3. Solution 41.4. Individual contribution 5

2. Equipment 52.1. LEGO Mindstorms NXT 52.2. Programming environment 7

3. System representation 73.1. System identification 73.2. Linear filters 113.3. Non-linear filters 13

4. Controllers 144.1. PID controller 154.2. LQG controller 16

5. Leader robots 165.1. Random walk robot 165.2. Self-balancing robot 16

6. Implementation 186.1. Robot design 186.2. Black box model 186.3. Kalman filter 226.4. Alpha-Beta filter 226.5. Particle filter 236.6. Leader robot 23

7. Results 267.1. Black box model / Kalman filter / PID controller 267.2. Alpha-Beta filter 267.3. Particle filter 267.4. LQG 27

8. Discussion and conclusion 278.1. Conclusion 278.2. Limitations 298.3. Further developments 308.4. Programming issues and recommendations 30

References 33

Contents

MOBILE TRACKING 3

1. Introduction

Mobile tracking robots can be very versatile and useful in a wide variety of applications.Intelligent vehicles are currently under development for many different fields. Imagine beingstuck in a traffic jam, where you can simply enable a driver assistance system which willautomatically follow the car in front while maintaining a safe distance from its surroundings.This will reduce the human interaction as well as the pollution emissions. This can also beused in military operations where a robot pack mule can be loaded with heavy equipmentand execute a leader-follower behavior while carrying all the extra weight. Vehicle trackinghas also been implemented in airborne vehicles which continually keep track of objects inits environment in order to keep track of them or to avoid colliding with them.

1.1. Problem formulation.The goal of this project is to design a vision-based mobile tracker using the LEGO Mind-storms NXT set. Two non-interacting robots are to be built: the leader and the follower.This concept is often referred to as mobile tracking, vehicle tracking or leader-follower for-mation.The leader robot moves randomly in a bounded environment. Using a camera, the followerrobot has to track the movement of the leader robot and follow accordingly at a fixeddistance by adjusting bearing and distance, see figure 1. These kinds of robots have awide range of applications within unmanned transportation, autonomous navigation andsurveillance.

Figure 1. Leader robot moving randomly with the follower robot,equipped with a camera (triangle), tracking its movement by adjusting dis-tance and bearing.


1.2. State of the art analysis.The state of the art for mobile tracking stretches across several fields and industries and hasbeen implemented in many different applications. Camera-based tracking, mobile robottracking, leader-follower behavior, trajectory tracking and vehicle tracking from movingplatform are the main keywords that cover this topic.

Among the papers published within the field, both linear and non-linear approaches havebeen successfully implemented with good results [1][2][3][4][5]. To estimate the motionstate of a tracked object, you need a good model of the system. This is usually done byderiving the motion equations of the system or by doing a system identification.

There are two main techniques used for tracking: Kalman filter -based method and particlefilter -based method, also known as the sequential Monte Carlo method. The Kalman filter,in its various forms, is the most common tracking technique used. It consists of a predictionstep and a correction step to estimate the states. However, the basic Kalman filter is onlyapplicable to linear systems. If our system should turn out to be non-linear, we can insteadturn to the extended or the unscented Kalman filter to deal with the non-linearity. Theparticle filter deals with non-linear systems but is generally much more complex than theKalman filter -based one. Therefore, Kalman provides promising results at much lowercomputational cost.

1.3. Solution.The basic task for the follower robot is to accurately follow the leader robot which movesrandomly. Two solutions for the tracking follower robots are sought:

(1) Head towards the leader robot until the minimum allowed distance is reached

(2) Dynamic control of bearing and distance by estimating position and speed of theleader

Since a fully functional object tracking camera is provided in the building set, the focus willbe on translating the camera images to useful information in order to track the movementof the leader robot. To achieve this, we must have a system representation to accuratelydescribe the behavior of the system. Our approach will be to try different methods forthe follower robots in order to evaluate each method and its performance. The line-up ofrobots will be the following:

Follower robots

• Alpha-Beta filter

• Kalman filter

• Particle filter

Different regulators will be tested and implemented to control the final systems. The mainchallenges will be to continually track a moving object from a non-stationary, monocular

MOBILE TRACKING 5

camera and to accurately describe the system.

Leader robots

• Three-wheeled robot

– Two modes: random walk and remote control

– Collision avoidance

• Two-wheeled robot

– Self-balancing

– Remote control

We will design two leader robots for the followers to track. A three-wheeled robot wasnaturally chosen for its simplicity and straight-forward approach. A self-balancing robotwas added to extend the project and for the inverted pendulum control challenge. Themain challenges will be to come up with a random walk behavior, remote control the robotsvia Bluetooth and to stabilize the self-balancing robot.

1.4. Individual contribution.

Saliou Ciza: First edition of random walk robot, proofread the report.

Niklas Gunnarsson: (Project leader) System identification, Kalman filter, randomwalk robot.

Manne Henriksson: Alpha-Beta filter, Particle filter.

Jimmy Ly: Self-balancing robot, remote control, PID, general debugging.

2. Equipment

2.1. LEGO Mindstorms NXT. [6]LEGO Mindstorms NXT is a programmable robotics kit released by LEGO. The kit isprovided with an intelligent microcomputer, several sensors and motors and a complete setof building bricks to build a programmable robot.

2.1.1. NXT Brick.The Brick is the main component in the kit and features:

• 32-bit ARM7 microprocessor.

• 256 KB Flash memory.

• 64 kb RAM memory


• 1 USB 2.0 port.

• 4 input ports for sensors.

• 3 output ports for motors.

• Support for Bluetooth wireless communication.

2.1.2. Servo motors.The servo motor has a built-in rotation sensor that measures speed and distance. It alsofeatures a built-in tachometer that keeps track of the current angle and can rotate withprecise steps (within one degree of accuracy) or with chosen speed.

2.1.3. NXTCam v3.0.The camera used on the follower was the NXTCam v3.0. The software that was used forrecognizing objects was NXTCamView. NXTCamView is trained to recognize objects byphotographing them and defining the object in the picture based on its color. NXTCam isable to detect up to 8 objects using different predefined colors. When an object is detectedby the camera, the object is represented as a rectangle object with the four attributes:height, width, x- and y-value of the top left corner. The resolution of the camera is174× 144 pixels and it operates at 30 frames per second.

2.1.4. Gyro Sensor.The gyroscopic sensor detects rotation along an axis. The one used in the project was theHiTechnic Gyro Sensor. It is capable of measuring angular velocity in degrees per secondas well as indicating the direction of rotation along a single axis, see figure 2. The gyrosensor can measure ±360◦ and operates at 300Hz.

Figure 2. HiTechnic Gyro Sensor and its axis of rotation.

MOBILE TRACKING 7

2.1.5. UltraSonic Sensor.The Ultrasonic Sensor measures distances and sees where objects are by transmitting soundwaves and receiving reflected waves back from the obstacle. In order to make good approx-imations the sensor and the surface of the obstacle have to be perpendicular. The sensoris able to measure a maximum distance of 255cm with an uncertainty of 3cm.

2.1.6. Touch Sensor.The touch sensor simply reacts to being pushed or released and returns a boolean value.Four of these were used for the remote controller, one for each direction.

2.2. Programming environment.There are multiple alternatives for the choice of programming language and environment forgenerating code for the NXT. The standard software for LEGO Mindstorms programmingis block-based and not as dynamic as written code. The language chosen for this projectwas Java which can be written in the environment Eclipse. Since we wanted to write inJava and required Mac support, leJOS NXJ was the natural choice. leJOS NXJ is an opensource firmware replacement for the LEGO Mindstorms NXT set and also includes a Javavirtual machine and a library of Java classes.

3. System representation

3.1. System identification.Derivation of the dynamics of a system can either be done by physical connection or throughblack-box models. An analytic physical description uses knowledge about the dynamicswhile a black box model only searches for a relationship between input data and outputdata from the system.

3.1.1. Analytical model.The most natural way to model the position of the target for the follower is to use polarcoordinates. The distance from the camera to the target is denoted d and the bearing,here defined as the angle between the direction of the follower and the target, is denotedφ. In order to get a dynamic model of the system, the distance and the bearing need tobe given as a function of earlier values and the angular velocities of the two wheels. Letdn and φn be the current values of the distance and bearing and let dn+1 and φn+1 be thevalues at the next instance. Let ωr and ωl be the angular speed of the right and left wheelrespectively, r the radius of the wheel, R the distance between the wheels and ∆T be theelapsed time between the two instances, see figure 3. The traveled distance, denoted by xbetween the two instances will be the arc of a circle, but as ∆T → 0, x will be a straightline and thus given by

x =ωl + ωr

2r∆T. (3.1)

The distances x, dn and dn+1 form the sides in a triangle.


Figure 3. The geometry of the position of the target in polar coordinatesfrom the follower.

The law of cosine gives the updated distance:

dn+1 =√d2n + x2 − 2xdncos(φn). (3.2)

The law of sine gives the relationship

sin(φn+1) =dndn+1

sin(φn). (3.3)

However, also the global turning of the follower contributes to the change in bearing. Thischange in angle is given by

θ =ωr − ωlR

r∆T. (3.4)

MOBILE TRACKING 9

Combining (3.3) and (3.4) gives

φn+1 = sin−1(dndn+1

sin(φn)) +∆T (ωr − ωl)r

R. (3.5)

According to (3.2) and (3.5) the dynamics of the system is non-linear and involve squareroots and trigonometric identities. Since the purpose of the robot is to follow the target,some assumptions can be made that significantly simplifies the geometry. The first assump-tion is that when the instantaneous goal is mainly to decrease the distance, the bearingis close to zero. This would set the term cos(φ1) to one in equation 3.2. Completing thesquares gives the linear update:

dn+1 = dn − x = dn −ωr + ωl

2r∆T. (3.6)

The second assumption is that when the instantaneous goal is mainly to decease the angle,the ratio of the distances is close to zero. Then the sine and inverse sine in equation 3.5cancel each other and the update rule becomes:

φn+1 = φn +∆T (ωr − ωl)r

R. (3.7)

3.1.2. Black box model.The majority of all practical systems are more or less non-linear. However, it does notprevent us from describing part of the system with a linear model. One usual manner isto add no physical knowledge about the system to the model and only search for a linearrelationship between the input data and the output data. Those kinds of models are calledblack box models, see figure 4. System identification for a black box model uses statisticalmethods to design mathematical models of dynamical systems.

Figure 4. A black box model only describes the relationship between theoutput and the input and uses no physical description of the system.


A general model to describe a discrete time system looks like

A(q−1)y(t) =B(q−1)

F (q−1)u(t) +

C(q−1)

D(q−1)e(t) (3.8)

where y and u is the output and the input data, e is white noise with no mean and varianceλ2. A, B, C, D, F are polynomials with the time shift operator q−1 as variable. The aimis to estimate the parameters in the polynomial to get a model that is as close to the truesystem as possible. By using collected data from the system the predicted error will looklike

e(t|θ) = y(t)− y(t|θ), (3.9)

where θ is the parameters. The best estimation of θ means that the predicted error e willhave as low variance as possible. By collecting a large amount of data N , the variance canbe approximated with

VN (θ) =1

N

N∑t=1

e(t|θ)2. (3.10)

The best estimation of θ is then given by

θ = arg minθVN (θ). (3.11)

Depending on the character of the polynomial in (3.8) the solution of (3.11) will be easieror harder. By choosing C(q−1) = D(q−1) = F (q−1) = 1 the model will have an ARXstructure and the solution can be found analytically by linear regression[7]. The predictionof the model can be written

y(t|θ) = [1−A(q, θ)]y(t) +B(q, θ)u(t)

= [a1...anab1...bnb][−y(t− 1)...− y(t− na)u(t− nk)...u(t− nk − nb+ 1)]T

= θTϕ(t)

(3.12)

where θ = [a1...anab1...bnb]T , ϕ(t) = [−y(t−1)...−y(t−na)u(t−nk)...u(t−nk−nb+1)]T , na

is the order of the A polynomial, nb the order of the B polynomial and nk is the time delay.By using linear regression, the estimation that minimizes the loss function (equation) isfinally given as

θN = R−1N fN (3.13)

where RN = 1N

∑Nt=1 ϕ(t)ϕT (t) and fN = 1

N

∑Nt=1 ϕ(t)y(t) are covariance functions of the

collected data y and u.

MOBILE TRACKING 11

For other models than ARX it is not possible to find an analytic solution of (3.8) and re-quires a numerical solution instead, like Newton-Raphson. Models of interest can be

ARMAX : A(q−1)y(t) = B(q−1)u(t) + C(q−1)e(t) (3.14)

Output− Error : y(t) =B(q−1)

F (q−1)u(t) + e(t) (3.15)

Box− Jenkins : y(t) =B(q−1)

F (q−1)u(t) +

C(q−1)

D(q−1)e(t). (3.16)

The approach to use when estimating a black box model is first of all to choose a representa-tive dataset as input data. The model should be able to describe the behavior of the systemin a large spectrum. Therefore it is necessary to use an input signal with high energy overa large frequency band. Typical signals are PRBS (Pseudo Random Binary Sequence)[8]or white noise. A better variance estimation requires a large number of datasets. Selectan input dataset and measure the output for this large number of samples. To removetrends and false behavior for low frequencies, the datasets might need to be pre-filtered.Pre-filtered data can be done by removing means and if necessary use for example a lowpass filter. A good choice is to split the datasets in two parts. Use one part for estima-tion of the model and the second part for validation. Choose the structure of the modeland estimate the parameters. Validate the model with the validation data and if it is notsatisfying, try a new model. Figure 5 shows a flowchart of the process when estimating amodel.

3.2. Linear filters.

3.2.1. Kalman filter.Knowledge about a system requires knowledge about the states. In cases when it is impos-sible to measure the states, an observer is required. Under linearity and Gaussian conditionfor a dynamical system, a linear Kalman filter is the optimal observer that minimizes theestimate error for the states[9]. For any linear system

x(t+ 1) = Ax(t) +Bu(t) +Nv1 (3.17)

y(t) = Cx(t) +Du(t) + v2 (3.18)

where

[v1v2

]is gaussian white noise with intensity

[R1 R12

RT12 R2

]. The Kalman prediction

is then given by

x(t+ 1|t) = Ax(t|t− 1) +Bu(t) +K(y(t)− Cx(t|t− 1)−Du(t) (3.19)

where

K = (APCT +NR12)(CPCT +R2)

−1 (3.20)

and P is the solution to the Riccati equation

P = APAT +NR1NT − (APCT +NR12)(CPC

T +R2)−1(APCT +NR12)T . (3.21)


Figure 5. A flowchart of the different steps when estimating a black box model.

P is also the covariance of the estimate error, x(t)− x(t|t− 1). The estimation of (3.19) isonly based on previous measurement of y(s), s ≤ t − 1. To include the last measurementof the output, y(t), the estimate becomes

x(t|t) = x(t|t− 1) + K(y(t)− Cx(t|t− 1)−Du(t). (3.22)

The variance of the error, x(t|t) = x(t)− x(t|t) can be described as

Ex(t|t)xT (t|t) = (I − KC)P (I − KC)T + KR2(K)T (3.23)

and by minimizing the right hand side of (3.23) with respect to K gives

K = PCT (CPCT +R2)−1. (3.24)

Finally, the Kalman algorithm can be written as

x(t+ 1|t) = Ax(t|t− 1) +Bu(t) +K(y(t)− Cx(t|t− 1)−Du(t) (3.25)

x(t|t) = x(t|t− 1) + K(y(t)− Cx(t|t− 1)−Du(t). (3.26)

MOBILE TRACKING 13

3.2.2. Alpha-Beta filter.The Alpha-beta filter resembles the Kalman filter in its structure, but is less complex andits computational burden is therefore smaller. The Alpha-beta filter assumes a simple statespace with two states; Position x and velocity v where the second one is the derivative ofthe first one. The states are updated as

xk ← xk−1 + vk−1 ∗∆T (3.27)

and

vk ← vk−1 (3.28)

where ∆T is the sample time. Because of noise and dynamic effects, the measurement willdeviate from the prediction. This error is called the residual rkand is defined as

rk = xk − xk (3.29)

where xk is the measured position. Using the residual, the state variables can be correctedas:

xk ← xk + α ∗ rk (3.30)

and

vk ← vk + β/∆T ∗ rk (3.31)

where 0 < α < 1 and 0 < β < 2 in order to suppress noise. Note that α = 0 corresponds toignoring the measurement and α = 1 corresponds to ignoring the prediction. The constantsα and β can either be predefined or changed dynamically. With some specific values on αand β, the filter becomes a steady-state Kalman filter.

3.3. Non-linear filters.

3.3.1. Particle filter.The particle filter [11], also known as sequential Monte Carlo method, is a method forestimating the state space in a statistical way. The basic idea of the particle filter is touse multiple ”particles”- all vectors belonging to the state space that is to be modeled.Instead of predicting and updating the value of the state space vector as in the previoussections, the particle filter predicts and updates the distribution, or the probability densityfunction, of the state space vector. The greatest advantage with the particle filter is itsability to handle nonlinear systems, as the standard Kalman filter can not.Let xn be the state space vector at time instance n and let yn be the observation of thestate at the same time. At each time instance, the probability density function of thestate vector given all observation until this time instance is sought and is given by Bayesrule:

p(xn|yn, yn−1, ..., y0) =p(yn|xn, y0, ..., yn−2, yn−1)p(xn|y0, ..., yn−2, yn−1)

p(yn|y0, ..., yn−2, yn−1). (3.32)

There are three factors in equation (3.32) to be identified. The denominator is indepen-dent of x and thus does not have any impact on the probability distribution. For the


factor p(yn|xn, y0, ..., yn−2, yn−1), we make the assumption that the observations y0, y1, ..are conditionally independent given x0, x1, .... This gives:

p(yn|xn, y0, ..., yn−2, yn−1) = p(yn|xn) (3.33)

which will be given by the properties of the camera. The last factor to identify isp(xn|y0, ..., yn−2, yn−1), that is, the state distribution given all observations but the lastone. This distribution can be calculated using the probability p(xn−1|y0, ..., yn−2, yn−1),which is known from the last iteration, and letting it propagate through the model of thesystem.

Since the number of particles used is a finite number, the distributions are not probabilitydensity functions (except for p(yn|xn) ) but rather the discrete counter pad, probabilitymass functions. To model these actual functions, each particle is assigned a weight such

that the particle and its weigh form a pair x(i)n ω

(i)n where i denotes the i:th particle. The

weights are updated in each iteration as discussed above, and normalized such that theirsum equals to one using:

ω(i)n =

ω(i)n∑N

i=1 ω(i)n

(3.34)

where N is the number of particles. One big problem using particle filters is that as theparticles propagate through the system, most of their weight will approach zero while onlya few of them have any significant weight. This can be avoided by resampling, either ineach iteration or when the weights attain some undesirable property. One way to performthe resampling is to create the cumulative probability function from the weights and thendraw new particles from this function. Since these particles are independent identicallydistributed, the weight for each of the is set to 1/N . Another way to resample, used in theGaussian particle filter, is to calculate the weighted sample mean and variance from theparticles by

µn =N∑i=1

w(i)n x

(i)n (3.35)

where µn is the sample mean, and

Σn =N∑i=1

w(i)n (x(i)n − µn)T (x(i)n − µn) (3.36)

where Σn is the sample variance.Assuming the particles are normally distributed, the new particles are then drawn andassigned the weight 1/N .

4. Controllers

The filtering process gives estimations of the distance and the bearing to the target. Theseestimations can be used in order to change the position of the follower according to the

MOBILE TRACKING 15

desired distance and bearing . This can be done by using the estimations as input toa controller, which will output the changes in distance and bearing the follower shouldperform. Two controllers used in this project were the PID-controller and the LQG-controller.

4.1. PID controller.The proportional-integral-derivative controller (PID) is a widely used simple feedback con-troller [10]. The controller uses the error e(t), defined as the difference between a desiredset point value and an estimation of the current value as input and outputs the processcontrol. The controller uses a weighted sum of the current error, the accumulation of pasterrors and the prediction of future errors. The output of the process is given by:

u(t) = Kpe(t) +Ki

∫ t

0e(τ)dτ +Kd

d

dte(t) (4.1)

where:u(t) is the desired controlKp is the constant for the proportional partKi is the constant for the accumulation of past errors andKd is the constant for the prediction of future errors.

4.1.1. PID Tuning.Choosing the appropriate PID variables for the control loop is not a simple task. Thereare several different ways to determine a set of constants, with varying complexity andaccuracy. The most common way is a trial and error approach where the step response tothe system is analyzed. In this case, the parameters are manually set and the overshoot,rise time, settling time and steady state error of the step response are studied. We want tokeep these as small as possible without making the system unstable. For manual tuning,no knowledge about the system is necessary, which can be both an advantage as well as adisadvantage. The rule of thumb is to start off with setting Ki and Kd to zero. Increase Kp

until the output starts to oscillate. Set Kp to half that value and increase Ki until steadystate offset is eliminated. A too large value of Ki will however lead to instability. Lastly,introduce a small Kd for faster settling time. This manual tuning method is a version ofthe Ziegler-Nichols method [10]. The effect of each parameter can be seen in table 1. Thisis a simple way to get a good enough set of constants but is inaccurate and time consuming.The Ziegler-Nichols method is a proven method and there are different variations of it withthis one being one of the more aggressive ones. Another tuning approach is software tuningwhich gathers data, develops process models and computes the optimal tuning parameterfor the system. However, for this you need a mathematical model of the system. Thismethod offers a consistent tuning but is far more complex.


Table 1. The PID parameters and their effect on the step response.

Parameter Overshoot Rise time Settling time Steady stateKp Increase Decrease Small change DecreaseKi Increase Decrease Increase EliminateKd Decrease Small Decrease Decrease None

4.2. LQG controller.For a linear system

x(t+ h) = Ax(t) +Bu(t) +Nv1(t)

z(t) = Mx(t)

y(t) = Cx(t) + v2(t)

(4.2)

with gaussian noises, the optimal linear regulator that minimizes the criteria

V = ||z||2Q1+ ||u||2Q2

=∑

z(t)TQ1z(t) + u(t)TQ2u(t) (4.3)

for any positive definite matrix Q1 and a positive semi definite matrix Q2 is called LinearQuadratic Gaussian control (LQG)[9]. The LQG controller is given by

u(t) = −Lx(t) + l0r(t)

x(t+ 1) = Ax(t) +Bu(t) +K(y(t)− Cx(t))(4.4)

where K is the Kalman gain (See chapter 3.2.1) and L in the control law is given by

L = (BTSB +Q2)−1BTSA (4.5)

. S is found as the unambiguous, semi definite, symmetric solution to the Riccati equa-tion

S = ATSA+MTQ1M −ATSB(BTSB +Q2)−1BTSA. (4.6)

5. Leader robots

5.1. Random walk robot.One mode for the leader robot is to move randomly in the environment. The robot shouldbe able to move with different velocities and bearings. The robot should also be able toavoid obstacles such as walls.

5.2. Self-balancing robot.Two-wheeled balancing robots is a relatively well researched field where the stability ofdifferent models and controllers have been tested for this well known problem. Thereeven exists many stable and well-working models using the LEGO Mindstorms NXT setsuch as the NXTWay-GS, Marvin, HTWay, AnyWay and GELWay to mention a few,[12, 13, 14, 15, 16]. The physical structure of a two-wheeled inverted pendulum makesit a naturally unstable system. The challenge is to keep the center of gravity centered

MOBILE TRACKING 17

over the wheels at all times. If we introduce the slightest tilt, the system will becomeunstable and fall over if the disturbance is not corrected for, see figure 6. A lot of work andoptimization has already been done on self-balancing robots using the NXT Mindstormsplatform. There are four terms that are of importance for balancing: tilt angle ψ, anglevelocity ψ, motor angle θ, and motor velocity θ. All of these should be 0 in order for therobot to stay upright. In order to keep the system stable, we need to constantly measurethese states and correct them by adjusting the power to the motors.

The NXTWay-GS was developed in 2008 by Yorihisa Yamamoto. He designed an impres-sive, stable controller for this two-wheeled inverted pendulum using the LEGO MindstormsNXT set and many of the existing self-balancing robots are based on his work. Yamamotobased his work on a mathematical model, using Lagrange’s equations to derive the motionequations. He then assumes that the balancing point won’t deviate too much from theequilibrium and linearizes this non-linear system around its equilibrium in order to designa LQG controller. A servo controller is then implemented to control the system, [12]. Mar-vin - the balancing robot further improved the work done by Yamamoto, and continued toimprove the stability by using a PID controller and manually tuning the control parametersas well as implementing a PC remote control, [13]. The GELWay continued to build uponthe work from Marvin - the balancing robot and focused on improving performance by op-timizing the controller parameters and checking stability margins, [16]. For the derivationof the motion equations and the control system, we refer to these works where it is welldocumented.

Figure 6. Inputs and outputs of the balance controller


6. Implementation

The software programming of the robots are written in Eclipse using the programminglanguage Java. While the follower robot can track the leader the tracking is controlled bythe control system. If the follower loses the leader the speed of the following robot is set to95% of what it was before and the difference between the speed of the wheels is set to 1.1times of the old value. The robot will slow down and start rotating in the same directionas where it saw the target earlier. To prevent the increase of rotation speed while thefollowing robot can’t find the target the rotation speed stops increasing when it reaches aspeed of 400.

6.1. Robot design.The follower robot was built as simple as possible while fulfilling our requirements onmaneuverability, quickness, ability to hold a camera at the right height and ability to holdan NXT. For maneuverability, the robot was built with two large wheels connected tomotors and one smaller wheel with the ability to rotate 360 degrees around the axis whichconnects it to the robot. One alternative to the small wheel was using a ball attachedloosely to the robot in order to give support to its weight. In order to hold the cameraat the required height, approximately 10 cm from the ground, the motors were built in anupright position. To be able to have space for the NXT, the robot had to be built with adistance between the front wheels and the back wheel of about 10 cm. Except from theserequirements on the construction, the design was kept simple in order to make the robotas quick as possible. The follower robots can be seen in figure 7.

6.2. Black box model.When estimating a model for the system, two independent models were made. One modelthat handles the distance and one for the bearing. This assumes that these behaviors areindependent of each other and can be approximated as two independent systems.[

ydyb

]=

[Gd 00 Gb

] [udub

]+

[vbvd

]. (6.1)

The first system uses the angular velocity of the wheel as an input and the distance to theobject as the output. From the camera it is only possible to collect the width of the trackedobject in pixels but by taking the inverse of the width we can relate this to distance (towork with integers the inverse was even multiplied by 1000). For the other system, thedifference of the angular velocities was used as input and to handle the bearing, the pixelerror (the number of pixels the tracked object is located from the centre of the camerapicture) was used as output.

6.2.1. Distance model.To estimate a model for the distance, a PBRS signal[8] was used as an input. The inputsignal and the measurement output are shown in figure 8.

MOBILE TRACKING 19

Figure 7. The two different follower robots. The left one uses a black boxsystem identification, a kalman filter and a PID controller. The one to theright uses both an alpha-beta filter and a particle filter.

By using the Identification Toolbox in Matlab different model structures were analyzed.One model that satisfied the requirements is the following ARMAX(3,1,1,4) model:

[1− 1.519q−1 + 0.5566q−2 − 0.03672q−3]yd(t)

= −0.01617q−4ud(t) + [1− 0.8906q−1]ed(t).(6.2)

The model was created by using the first 500 samples of the collected data. Comparingthe last 500 samples with a 1-step-prediction from the model resulted in a fit of 75.44 %.This model can be described by the following state space description

xd(t+ h) = Adx(t) +Bdu(t) +Ndvd1(t)

zd(t) = Mdx(t)

yd(t) = Cdx(t) + vd2(t)

(6.3)


Figure 8. Collected data to estimate a model for the distance to thetracked object. A PBRS signal was used as input (first graph). The lowergraph shows the output signal.

Ad =

1.5188 −0.5566 0.1469 0.0367

1 0 0 00 1 0 00 0 1 0

, Bd =

0.125

000

Cd = Md =

(0 0 0 −0.1294

).

(6.4)

6.2.2. Bearing system.To ensure that the tracking object always is within the camera view during the collection ofdata when estimating a model for the bearing system, a protractor was placed in the frontof the camera. An investigation of how far the robot could turn until it lost the trackedobject was made and the camera viewing angle was measured to 43o. A square signal withthe period time as the time it takes for the robot to oscillate between seeing the trackedobject in the far left to the far right was used as an input. The input square signal and themeasured output signal are shown in figure 9. By using Identification toolbox in Matlabagain a good model order was chosen. The model turns out to be a ARX(3,5,2)-model

MOBILE TRACKING 21

Figure 9. Collected data for estimating a model for the bearing. A squaresignal was used as input (first graph). The lower graph shows the outputsignal.

with parameters:

[1− 0.6652q−1 − 0.2772q−2 − 0.03197q−3]yb(t) =

[−0.05752q−2 − 0, 01599q−3 − 0.01989q−4 − 0.05453q−5 − 0.02062q−6]ub(t) + eb(t).(6.5)

The bearing model was, in the same way as the distance model, created by using the first500 samples of the collected data. The last 500 samples were used for validation with a1-step-prediction from the model which gave a fit of 93.49 %. The model represented instate space description as (6.3) gives

Ab =

0.6652 0.5544 0.1279 0 0 0.5

0.5 0 0 0 0 00 0.5 0 0 0 00 0 1 0 0 00 0 0 1 0 00 0 0 0 0 0

, Bb =

000001

Cb = Mb =

(−0.1150 −0.0640 −0.1591 −0.4362 −0.1649 0

).

(6.6)


6.3. Kalman filter.

6.3.1. Distance model.To estimate the states, a Kalman filter was used as an observer. By trying differentvariances for the process distribution and measurement noise, we chose R1 = Ev21(t) = 10,R2 = Ev22(t) = 1 with no correlation between the noises, R12 = Ev1v

T2 = 0 results in a

satisfying model. The estimator gains were calculated to

Kd =

−0.7103−0.7100−0.7074−0.6997

, Kd =

−0.7100−0.7074−0.7000−0.6803

. (6.7)

By using the Kalman algorithm (3.25) the estimated output zd(t) was given by

zd(t) = Mdxd(t|t). (6.8)

6.3.2. Bearing model.If the camera loses the tracked object, it should remember in which corner the object waslast seen and continue to rotate in that direction. With that in mind, the robot will keeprotating around until it finds an object. By using a Kalman filter with a low variance ofthe measurement noise, the camera will be able to track the object while it’s rotating anddoesn’t interpret it as noise. Because of that the process distribution with R1 = 50 andmeasurement noise with R2 = 1 and no correlation between them, R12 = 0 was chosen.That results in the following estimator gains

Kd =

−1.7961−1.0325−0.3484−0.2826−0.2465

0

, Kd =

−2.0650−0.6969−0.2826−0.2465−0.0841

0

. (6.9)

In the same way as for the distance model, the estimated output zb(t) was given by

zb(t) = Mbxb(t|t). (6.10)

6.4. Alpha-Beta filter.The Alpha-Beta filter was implemented on the distance and the bearing independently.The constants α and β were set by experimenting to find a tradeoff between suppressingnoise and adaption to quick changes. For the distance the values of the constants wereα = 0.25, β = 0.0102 and for the bearing they were set to α = 0.3, β = 0.006. Theestimated distance and bearing were used in two independent PID-regulators.

MOBILE TRACKING 23

6.5. Particle filter.Each particle was defined as a 4 by 1 vector with the elements representing: distance, speed,angle and angular speed, all for the target relative to the reference system of the follower.In order to implement the particle filter algorithm on the NXT, which has highly limitedcalculation power, the filter was made as simple as possible. The distribution of the camerameasurement in equation 3.33 was calculated beforehand assuming a Gaussian distributionwith the variance from the camera experiment and put in a table. These numerical valueswere then selected in the update step using the difference between the predicted value andthe measured to find the right index for the table. For the resampling, the procedure offinding the right value from the cumulated probability function was first used, but wastoo time consuming and instead the technique of the Gaussian particle filter in equations3.35 and 3.36 was used. For situations when all weights turned to zero, the particles wereresampled from a normal distribution using a measured value from the camera as meanand some predefined variance. The number of particles was then set as the maximumpossible in order to still have an iteration time below 150 milliseconds, which was around60 particles.The weighted sample means of the particles for distance and angle were used as input totwo PID controllers independently.

6.6. Leader robot.

6.6.1. Random walk robot.The leader robot is built with a low center of gravity as can be seen in figure 10. The redball is used as the tracking object for the camera. Putting the target on the top makes itpossible for the following robot to track the target from all angles. The robot is equippedwith two servo motors, one NXT Brick and one Ultrasonic sensor in the front. The velocity(degrees/second) of the wheels is set to a fixed speed of 300 in the beginning. The speedwill be set rectangular randomly in a interval of -200 to +400 of the old velocity. If thespeed of one of the wheels becomes three times greater than the speed of the other wheelor the speed of any wheel becomes greater than 1000, the speed of both wheels are setto default (300). Every fifth of a second the robot measures the distance to the closestobject in front of it with the Ultrasonic sensor. If the object is too close, the robot will gobackwards for 2 seconds with a speed of 200 and then turn for 1.1 seconds with a speed of200 to avoid the object.

6.6.2. Self-balancing robot.Considering all the work that has already been done in the field, we chose not to startfrom scratch and instead continue on existing models. Therefore our robot is, with somemodifications, mostly based on the GELWay. What we needed to do was to modify thecode for our design and to add our own Bluetooth remote control. Our self-balancing robotcan be seen in figure 11.


Figure 10. A picture of the random walk leader. The red ball is used asthe tracking object.

The biggest challenge was to make the robot capable of remaining upright while performingother tasks, i.e. waiting for Bluetooth commands and executing them while maintainingbalance. This was done by threading since we need several tasks running simultaneouslyto accomplish this. However, the NXT Brick is very limited in the aspect computationalefficiency. Three threads were needed, in descending priority order:

(1) Balancing

(2) Bluetooth listener

(3) Driving behavior

6.6.3. Remote control.The ability to control the movement of the leader robots from a distance was desirable.For the three-wheeled robot we also wanted to be able to switch between a random walkbehavior and remote control. This was done by designing a remote control out of a secondNXT Brick and four touch sensors, one for each direction of movement, see figure 12. Byestablishing a Bluetooth connection between the remote control and the robot, we can send

MOBILE TRACKING 25

Figure 11. The self-balancing robot. The ball can be fit between the wheels.

and receive commands between the units. If a sensor is pushed on the remote control, thecorresponding integer is sent. In this case we assigned the following integers:

• Forward - 2

• Left - 4

• Stay - 5

• Right - 6

• Backward - 8

The robot on the other hand runs an active Bluetooth listener thread which constantlylistens for new integers. If a new integer has been sent, the integer is read and the robotexecutes the maneuver corresponding to that integer.


Figure 12. Remote control

7. Results

7.1. Black box model / Kalman filter / PID controller.Following figures show the results from a run by using the robot with the Kalman filterdeveloped from the black box models and controlled by a PID-controller. The red lineshows the Kalman estimation of the outputs while the blue dotted is the measurementfrom the camera. Figure 13 shows the distance to the object and figure 14 shows thebearing measured in pixel error.

7.2. Alpha-Beta filter.Implementing the Alpha-Beta filter with two independent state spaces for distance andbearing and using the results as input to a PID-controller yields the results in figure 15and figure 16. The filter behaves in a desired manner: For the noisy measurements of thedistance (blue) it gives a smoothed estimation following outline of the graph (red), andfor the more stable measurements of the bearing, it gives a slightly smoothed estimation.

7.3. Particle filter.Using the gaussian particle filter with 60 particles and using the estimated values fordistance and bearing as input to a PID-controller yields the results in figure 17 and 18.The filter behaves in a desired manner: For the noisy measurements of the distance (blue)it gives a smoothed estimation following outline of the graph (red), and for the more stable

MOBILE TRACKING 27

Figure 13. The measured and the estimated distance by using a Kalmanfilter. The estimates follow the trends but eliminates the noise due to itsinertia.

measurements of the bearing, it gives a slightly smoothed estimation. These results canhowever not be compared directly to those using Kalman filter and Alpha-Beta filter sincethe target was moving slower when using the particle filter in the follower.

7.4. LQG.A follower using an LQG controller was implemented using a black box model. The resultwas not as desired.

8. Discussion and conclusion

8.1. Conclusion.The goal of creating a tracking robot was successfully accomplished. The follower usingthe particle filter technique did not have the same ability to follow a quickly moving targetas the followers using linear Kalman filter and Alpha-Beta filter. The main reason forthis was that the NXT Brick has very limited computational power which resulted in aniteration time around 200 milliseconds for the particle filter while the other two methodswere primarily limited by the frame rate of the camera which was 30 frames per second.Both the follower using the linear Kalman filter and the one using the Alpha-Beta filterwere able to follow the target while moving quickly. However, the follower using the linearKalman filter moved faster and smoother. The follower using the Alpha-Beta filter made


Figure 14. The measured and the estimate bearing by using a Kalmanfilter. The estimates follow the measured data which is desired behavior.

Figure 15. The measured and estimated distance using an Alpha-Beta filter.

short time pulses of acceleration which resulted in ”trembling” movement. This problemmainly occurred in the backward-forward direction and not in the turning.

For the LQG, the linear estimation of the system was successful, but to apply a controllerto it was not. This might have been caused by the noise not being gaussian which is oneof the criterions for when the LQG is the optimal controller.

The self-balancing robot could stay upright for quite some time however drifting in somedirection due to gyro offset drift over time. Since the balance controller was developed for

MOBILE TRACKING 29

Figure 16. The measured and estimated bearing using an Alpha-Beta filter.

Figure 17. The measured and estimated distance using a particle filter.

a slightly different design, the robot was not completely stable. The remote control wassuccessfully implemented to both leader robots. Due to slow Bluetooth connection whichupdates every 40ms and threading in the Brick, we experienced a short delay from actionto movement.

8.2. Limitations.The CPU power of the NXT brick caused limitation on the number of particles and theiteration time for the follower using the particle filter. The camera was not perfect and oneof its shortages was that the tracking was performed only by recognizing colors. Becauseof this, the tracked object had to be round in order to get the correct measurement inde-pendent of which side of the target being observed. The camera was also very sensitiveto changes in lighting conditions which affected its perception of color and therefore theability to track the target. Also, the low quality of the camera images resulted in noisymeasurements. The target robot had to be designed with this in mind with the red balllying on top of it. A limitation for the motors was that they were controlled by the speed


Figure 18. The measured and estimated bearing using a particle filter.

as input and not the acceleration. If the motors would have been able to use acceleration asinput, the movements might have been smoother. We also noticed a motor backlash whichin worst cases could be as much as 10◦. It was hard to get the motors moving synchronizedas the Brick tended to send commands to one motor at a time which resulted in a slightdelay for one of the motors in cases when both motors should receive the same commandsimultaneously. Another limitation was when using the integrated Bluetooth connectionwhich turned out to be very slow. It would have simplified some parts of the project if wecould have communicated with the Bricks fast and wireless, e.g. when tuning the controlparameters.

8.3. Further developments.When the black box model was estimated, two independent systems were assumed, one forthe distance and one for the bearing. For further development, this independency shouldnot be assumed. Instead, subspace identification can be used to estimate a dependentMIMO-system. Since the self-balancing robot was added as a side project and we continuedfrom existing work, the motion equations and balance controller was not entirely adaptedfor our design which differed a bit from the GELWay. For further work, the balancecontroller could be stabilized by doing this from scratch. Also the movement of the robotcould be altered so that the robot moves smoother.

8.4. Programming issues and recommendations.Some of the leJOS classes that were used in this project, for instance the PIDcontrollerclass, uses integers as input and output. This caused some problems concerning accuracy.For instance when the bearing was measured in degrees, an angle of 1.5 degrees had to berounded to either 1 or 2, which gave different output from the controller. This problemwas solved by scaling up the units in which the bearing and distance were measured. Thecamera behaved in an unexpected manner in situations when it suddenly detected an objectwhich it did not detect in the frame before. For approximately the first 200 milliseconds,

MOBILE TRACKING 31

the rectangle representing the detected object received from the camera was much smallerthan the actual size, fooling the follower to believe the target being further away than itactually was. This problem was solved automatically by the Kalman filter by consideringthese measurements as noise and in the other methods by temporarily decreasing theaggressiveness of the controller when such situations occurred. Using the stop functionfor the motors caused some problems with the two wheels not moving equally when theiteration time was small, this problem was solved by avoiding this function. When datawas logged in more than one data file during a run it resulted in a problem. The loggeddata got corrupted and the robot acted unfamiliar. When restarting the robot by removingthe battery it behaved normally for a while but the problem reappeared again after a fewruns.


1 Leader robot moving randomly with the follower robot, equipped with a camera(triangle), tracking its movement by adjusting distance and bearing. 3

2 HiTechnic Gyro Sensor and its axis of rotation. 6

3 The geometry of the position of the target in polar coordinates from the follower. 8

4 A black box model only describes the relationship between the output and theinput and uses no physical description of the system. 9

5 A flowchart of the different steps when estimating a black box model. 12

6 Inputs and outputs of the balance controller 17

7 The two different follower robots. The left one uses a black box systemidentification, a kalman filter and a PID controller. The one to the right uses bothan alpha-beta filter and a particle filter. 19

8 Collected data to estimate a model for the distance to the tracked object. A PBRSsignal was used as input (first graph). The lower graph shows the output signal. 20

9 Collected data for estimating a model for the bearing. A square signal was usedas input (first graph). The lower graph shows the output signal. 21

10 A picture of the random walk leader. The red ball is used as the tracking object. 24

11 The self-balancing robot. The ball can be fit between the wheels. 25

12 Remote control 26

13 The measured and the estimated distance by using a Kalman filter. The estimatesfollow the trends but eliminates the noise due to its inertia. 27

14 The measured and the estimate bearing by using a Kalman filter. The estimatesfollow the measured data which is desired behavior. 28

15 The measured and estimated distance using an Alpha-Beta filter. 28

16 The measured and estimated bearing using an Alpha-Beta filter. 29

17 The measured and estimated distance using a particle filter. 29

18 The measured and estimated bearing using a particle filter. 30

List of Figures

MOBILE TRACKING 33

References

[1] Xu S. and Chang A. Robust object tracking using Kalman filters with dynamic covariance, CornellUniversity.

[2] Schulz D. et. al. Tracking multiple moving targets with a mobile robot using particle filters and statisticaldata association

[3] Teoh S. and Brunl T. A reliability point and Kalman filter-based vehicle tracking technique, InternationalConference on Intelligent Systems.

[4] Mariottini G. et. al. Vision-based localization of leader-follower formations[5] Vela P. et. al. Vision-based range regulation of a leader-follower formation[6] http://mindstorms.lego.com/en-us/products/default.aspx[7] L. Ljung and T. Glad, (2004) Modellbygge och simulering, Studentlitteratur, Lund.[8] T. Soderstrom and P. Stoica, (1989) System Identification, Prentice-Hall International, Hemel Hemp-

stead, UK.[9] L. Ljung and T. Glad, (2003) Reglerteori - Flervariabla och olinjara metoder, Studentlitteratur, Lund.

[10] L. Ljung and T. Glad, (1981) Reglerteknik - Grundlaggande teori, Studentlitteratur, Lund.[11] Olov Rosen and Alexander Medvedev, (2011) Efficient Parallel Implementation of State Estimation

Algorithms on Multicore Platforms, Institution of Information Technology, Department of Systems andControl, Uppsala University.

[12] Yamamoto Y., (2008) NXTWay-GS (Self-Balancing Two-Wheeled Robot) Controller Design, CybernetSystems Co.

[13] Rieper J. et. al., (2009) Marvin - The Balancing Robot, University of Aarhus.[14] HiTechnic HTWay - A Segway type robot, HiTechnic, www.hitechnic.com[15] Valk. L AnyWay, RobotSquare, www.robotsquare.com[16] Witzand J., (2009) Coordinated LEGO Segways, The University of New South Wales, Australian

Defence Force Academy.

mobile tracking - uppsala university · mobile tracking robots can be very versatile and useful in...

Documents