Robot Goalkeeper

Uppsala UniversityProject in Embedded Control Systems

Zeid Al-HusseinyFiliph AppelgrenJeton Mustini

January 20, 2014

Abstract

The goal of this project has been to create a robot goalkeeper for the

embedded control systems course at Uppsala University. The problem

the robot goalkeeper solves is to detect a ball, calculate the ball’s po-

sition, estimate its trajectory and predict a future point to intercept

the ball. The goalkeeper was constructed with the LEGO Mindstorms

development kit and programmed in the IDE leJOS using Java. The

distance to the target were measured using triangulation with two

cameras from Mindsensors which were placed on an extended front

part that could rotate freely in 150 degrees. Using a PID controller

the target were always in the cameras vision, allowing the robot to

move in one direction while the cameras followed the ball and its mo-

tion in another. Another PID controller for the odometry was used

together with two alpha-beta filters that improved the distance mea-

surements by reducing noise in the measurements. The algorithm

were successfully implemented and deviated only in the cases when

the cameras struggled.

1

Contents

1 Introduction 41.1 Problemformulation . . . . . . . . . . . . . . . . . . . . . . . . 4

2 Hardware Tools 52.1 The LEGO-NXT brick . . . . . . . . . . . . . . . . . . . . . . 52.2 Motors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52.3 NXTCam . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5

2.3.1 Camera models . . . . . . . . . . . . . . . . . . . . . . 6

3 Software Tools 73.1 leJOS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73.2 IDE . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73.3 NXTCamView . . . . . . . . . . . . . . . . . . . . . . . . . . 73.4 Matlab . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

4 Filters & Regulators 84.1 Alpha-Beta filter . . . . . . . . . . . . . . . . . . . . . . . . . 84.2 PID-controller . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

4.2.1 PID tuning . . . . . . . . . . . . . . . . . . . . . . . . 9

5 Goalkeeper construction 10

6 Target detection 11

7 Position determination 137.1 Odometry . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 137.2 Triangulation . . . . . . . . . . . . . . . . . . . . . . . . . . . 14

8 Trajectory estimation & Interception 178.1 Least square algorithm . . . . . . . . . . . . . . . . . . . . . . 17

8.1.1 Arc maneuver & PID-controller . . . . . . . . . . . . . 178.2 Predator - Prey algorithm . . . . . . . . . . . . . . . . . . . . 18

8.2.1 PID-controllers . . . . . . . . . . . . . . . . . . . . . . 218.2.2 Arc maneuver . . . . . . . . . . . . . . . . . . . . . . . 23

9 Results 259.1 Object detection . . . . . . . . . . . . . . . . . . . . . . . . . 259.2 Position & Velocity measurements . . . . . . . . . . . . . . . . 259.3 Alpha - Beta filter . . . . . . . . . . . . . . . . . . . . . . . . 269.4 Least-square algorithm . . . . . . . . . . . . . . . . . . . . . . 26

2

9.5 Predator - Prey algorithm . . . . . . . . . . . . . . . . . . . . 27

10 Discussion & Conclusions 2810.1 Least-square . . . . . . . . . . . . . . . . . . . . . . . . . . . 2810.2 Predator - Prey . . . . . . . . . . . . . . . . . . . . . . . . . . 28

11 Issues and experiences 30

3

List of Figures

1 NXTCam . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52 Robot Goalkeeper . . . . . . . . . . . . . . . . . . . . . . . . . 103 NXTCamView . . . . . . . . . . . . . . . . . . . . . . . . . . 114 Relationship between pixel and angle . . . . . . . . . . . . . . 125 Odometry . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 146 Triangulation . . . . . . . . . . . . . . . . . . . . . . . . . . . 157 Goalkeeper and ball position . . . . . . . . . . . . . . . . . . . 168 Interception . . . . . . . . . . . . . . . . . . . . . . . . . . . . 189 Prediction of interception point . . . . . . . . . . . . . . . . . 2010 Success of interception . . . . . . . . . . . . . . . . . . . . . . 2011 Prediction of changed interception point . . . . . . . . . . . . 2012 Success of interception with direction change . . . . . . . . . . 2113 Ball and predicted point relative robot and camera . . . . . . 2314 Wrong maneuver . . . . . . . . . . . . . . . . . . . . . . . . . 2415 Correct maneuver . . . . . . . . . . . . . . . . . . . . . . . . . 2416 Goalkeeper y-position . . . . . . . . . . . . . . . . . . . . . . . 2517 Goalkeeper y-position . . . . . . . . . . . . . . . . . . . . . . . 2618 Simulated least-square attempt . . . . . . . . . . . . . . . . . 27

4

1 Introduction

In the last decennium robotics has grown from a simple vision to a realitywhere they exist in industries and among people [1][2]. Encouraging workersand doing part of their work to make the process more efficient and moresolid. The level of detail that robots can extinguish and the speed theyoperate in, as well as the ability to operate day and night differs greatlyfrom what humans are capable of. The idea is therefore seen with a greatbusiness perspective. The introduction of robotics has grown widely overthe years and many are now trying to incorporate the concept to fit in withsociety. One of those steps is to try to introduce the concept of automationinto sports. Not as a player, but possibly a training partner. This report isa small step in that direction.

1.1 Problemformulation

The purpose of the project was to create a robot goalkeeper that interceptsa randomly thrown ball using the LEGO mindstorms development kit. Inorder to do this the goalkeeper needs to gather information and solve thefollowing tasks which were established before the project:

1. Target (ball) detection.

2. Always keep track of target, keeping visual contact.

3. Position and velocity determination of target.

4. Position determination of the goalkeeper.

5. Trajectory estimation and position prediction of the target.

6. Interception of target at predicted position.

5

2 Hardware Tools

2.1 The LEGO-NXT brick

The LEGO mindstorms NXT is a programmable robotics kit developed byLEGO [3]. Its main component is a brick-shaped computer called the NXTIntelligent Brick which has a capacity of taking four sensors as input andcontrolling three motors. The brick is build with 100 x 60 pixel monochromeLCD display. It has a 32-bit ARM7TDMI-core Atmel AT91SAM7S256 mi-crocontroller with 256KB FLASH memory and 64KB of RAM. Including a8-bit Atmel AVR ATmega48 microcontroller and bluetooth support. Poweris supplied by a Li-Ion rechargeable battery. Uploading and downloadingsoftware is done by either bluetooth or connecting an A to B USB 2.0 cable.

2.2 Motors

Although the LEGONXT kit is shipped with various sensors, the only sensorsused from the original kit where three servo motors. The motors have built-inreduction gear assemblies with internal optical rotary encoders (tachometers)that sense their rotations within two degrees of accuracy.

2.3 NXTCam

Figure 1: NXTCam (v4)

The NXTCam is a camera manufactured by Mindsensors[4] mainly for theuse together with the LEGO NXT brick. But can be used together withany device that supports the correct voltage output and has an I2C port.The camera can track up to 8 colored objects at 30 frames per second. Thecolormap is uploaded to the camera before the tracking starts. The process ofuploading colormaps has to be done on a computer, using a USB connection,with a software such as NXTCamView (see section 3.3). The camera thenshows the pixels of the colormap it tracks as rectangles.

6

2.3.1 Camera models

Two camera models of NXTCam were used, v3 and v4. The models had twomain differences. First, the v3 model could track objects in a big interval,while the v4 firmware had a threshold that would categorize an object asnoise if it was too small, which excludes targets that were too far away.This threshold stopped the tracking of the ball used when the target wasfurther away than 60cm. The other difference is that the v3, dependingon the brightness in the room, could detect a ball as two, three or possiblymore separate objects. This resulted in multiple rectangles very close to eachother. The v4 model on the other hand, has a built in “blob merge” filterwhich always showed one complete object, presenting a camera more suitablefor accurate tracking. To achieve maximum performance both models wereused in the final algorithm, were the v4 model had higher priority than thev3 when it could detect the ball.

7

3 Software Tools

3.1 leJOS

The programming language used has been Java together with the leJOSfirmware for the LEGO NXT brick[5]. LeJOS is a tiny Java Virtual Machinewhich supports a complete set of methods and functions to be used with theLEGO NXT brick and other accessories such as the NXTCam [6]. The mainreason for this choice was that the leJOS API supported the use of the camerawhile most languages had trouble with this, making the implementation alot simpler as well as avoiding the need of having to write most of the codefrom scratch. As time was very short, this last fact of saving time becamecrucial.

3.2 IDE

Because leJOS NXJ is a Java project and is build on the rich functionalityinherent from the Java platform, the integrated development environment(IDE) used for the project was Eclipse which has a plugin support for leJOSNXJ.

3.3 NXTCamView

The software used with the NXTCam is the NXTCamView which controlsand configures the NXTCam. NXTCamView is an open source software[7]developed for uploading colormaps to the NXTCam for tracking purposes.The colormaps are configured manually and uploaded to the NXTCam, seesection 6.

3.4 Matlab

The numerical computing environment and programming language Matlab[8] was extensively used for simulating, testing algorithms and plotting data.Matlab were used since there weren’t any acceptable way to simulate thebehaviour of the system besides running it and displaying values to the screen,which was a tedious task. A simulator was created in Matlab to test eachtheory before implementing it in practice.

8

4 Filters & Regulators

The filters and regulators used in the project have been alpha-beta filtersand PID-controllers.

4.1 Alpha-Beta filter

As measurements often contains a lot of noise and errors it is of great im-portance to be able to parry such events. A simple and effective way toenhance a defence mechanism for these events would be to implement a fil-ter. For position and velocity measurements, the most simple filter would bean alpha-beta filter which with right parameters and initial conditions couldbe just as optimal as a kalman filter [9]. The benefits of an alpha-beta filteris that it does not require a detailed system model.

An alpha-beta filter uses two states, were one of the states are obtainedby integrating the other over time. Velocity is assumed to be constant dueto small time intervals between measurements. The predicted position at thenext sampling instant is defined in (1).

�xn = �xn−1 +∆t · �vn−1 (1)

Because velocity is assumed constant, its value at the next sampling instantis the same as the previous.

�vn = �vn−1 (2)

By calculating the difference between real measurements and estimated po-sitions one can expect to get some deviation. This deviation is defined as theresidual and is designated here as �rn for time-step n.

�rn = xn − �xn (3)

The estimate of both the position and the velocity are then corrected accord-ing to the residual and the pre-set alpha and beta parameters, (4) respectively(5).

�xn = �xn + α · �rn (4)

�vn = �vn +β

∆t· �rn (5)

Alpha and Beta are predefined constants which are adjusted according tothe amount of process and measurement noise. Most often, as well as in ourcase, alpha and beta are determined empirically.

9

4.2 PID-controller

A proportional-integral-derivative controller is a generic control loop, feed-back mechanism. A PID controller calculates an error value e(t), as thedifference between a measured process value and a desired reference value.The controller aims to minimize the error by adjusting the process controlinputs [10][11]. The algorithm contains three parts: P, I and D. Where Pdepends on the present error, I on the accumulation of previous errors and Dis based on the current rate of change. The controller output u(t) is definedas follows:

u(t) = Kpe(t) +Ki

t�

0

e(t) dt+Kd

dte(t) (6)

where:

• Kp is the proportional gain

• Ki is the integral gain

• Kd is the derivative gain

These parameter are later tuned for our specific task, see 4.2.1. PID-controllerwere used in the project for regulating the goalkeeper wheels speed, and therotating part of the cameras that followed the targets motion, see section8.2.1.

4.2.1 PID tuning

The PID parameters were manually tuned. Starting with only the propor-tional part, Kp was increased until the output started to oscillate. Whensuch occurred Kp was assigned half of that value reaching a stable output.If an integral part was desired, Ki was decreased until the system got dis-turbed, and Ki was assigned the double of that value. Finally if a derivativepart weredesired,Kd was increased until the system got disturbed, and wasassigned half of that value [10].

10

5 Goalkeeper construction

The robot goalkeeper was built as a two wheeled lego car with two 2.7 cmradius wheels and a lego stick behind for support. The wheels are separated16.3 cm apart and controlled using two servo motors which are rotatableforwards and backwards according to power or angular velocity (section 2.2).Three cameras are mounted on the third motor which is attached above thebrick. It’s rotational capacity is 150 degrees, 75 degrees in both directions.The middle (v4) camera is a more accurate but limited to short ranges, usedonly to center the target when able to detect (section 2.3). The outer (v3)cameras are separated 17.6 cm from each other. They are less accurate andare used for detection as well as distance estimation via triangulation [12].The brick is attached to the construction horizontally and turned upside-down. The reason for this is so the third motor which is controlling therotation of the cameras won’t block the brick’s screen and buttons. A pictureof the final construction is illustrated in figure 2.

Figure 2: Constructed robot goalkeeper

11

6 Target detection

The NXTCamView software (described in section 3.3) is used to configurethe colormaps used for object detection. Once the NXTCams are connectedto the software they are ready to capture an image of the target. The targetbeing used in the project is a red ball. After an image of the ball has beencaptured, a colormap is selected corresponding to all shades and colors of theball. Colors are shown as ranges of red, green and blue. After the desiredcolor ranges are selected they are uploaded to the NXTCam. By using thetracking window one can see what object blobs are returned from the targetobject. This process is illustrated in figure 3 on two violet lined sticks.

Figure 3: Interface of NXTCamView viewing two violet colored sticks.

LeJOS support functions to retrieve direct information from the cameras.What can be retrieved are the rectangular blobs x and y positions relativethe cameras upper-left corner, and the rectangle’s width and height. Sincethe cameras tracking resolution are already known we can calculate in pixelshow much the tracked object is to the left or right of the camera center.

If the ball is to the right of the center the difference is positive, otherwisenegative. To convert this difference to angles, the cameras field of vision thatcovered 43 degrees were used, together with the horizontal window resolutionof 176 pixels.

12

The relationship between pixels and angles the ball is from the center ofthe camera can be seen in figure 4. The relationship was found linear byinterpolating the measurements.

Figure 4: Relationship between pixels and angles on the camera.

13

7 Position determination

Three positions are especially important to determine, relative a global ref-erence system that is established at the start of a run. The goalkeeper’sposition using odometry, the target ball’s position using triangulation andthe predicted interception point described later, section 8.

7.1 Odometry

Odometry is the use of data from moving motors to estimate the change inposition over time. This can be used to acquisition information about thegoalkeeper, for instance the angle it has turned, the distance it has traveledand its velocity [13]. By using the built-in functions in leJOS for the angularvelocities we get the robot’s angle as follows.

angle = θ =ωl − ωr

Rr∆T (7)

Here ωl and ωr are left respective right angular velocities. R is the distancebetween the wheels, and r is the radius of the wheels. ∆T is the time be-tween two consecutive measurements.

By taking the average of the angular velocities and multiplying with theradius and time we retrieve the distance the robot has traveled.

distance = d =ωl + ωr

2r∆T (8)

Leaving out the time we get the speed of the robot.

speed = s =ωl + ωr

2r (9)

In order to get the velocity and position of the robot one simply goes frompolar- to cartesian coordinates as shown in figure 5 and in (10)-(11).

14

Figure 5: Global reference system initiated at start of a run where the posi-tion of the goalkeeper is monitored using the odometry.

xrobot = d · sin(θ) , yrobot = d · cos(θ) (10)

vx,robot = s · sin(θ) , vy,robot = s · cos(θ) (11)

7.2 Triangulation

The coordinates and distance to the target (ball) relative the goalkeeper’scameras can be calculated with triangulation which is the process of deter-mining the location of a point by measuring the angles to it from knownmultiple points [12]. Since the position of the goalkeeper and its cameras areknown from the previous section, that information can be used to determinethe target’s position. Figure 6 shows the scenario where the left and rightcamera are used and the ball is between the cameras.

15

Figure 6: Scenario of when target lies between the two outer cameras, leftcamera sees a positive angle to target while right camera receives a negativeangle to target.

One can note that ϕ1 is always greater than ϕ2 due to the definition of thecoordinate systems. ϕ1 is the angle measured by the left camera and ϕ2 is theangle measured by the right camera. D is the distance between the cameras(see figure 6). Depending on which side the target is relative the cameras, adifferent scenario occurs, with the respective equations shown below. Theseequations have been derived using simple trigonometry together with thelaws of sine and cosine, see (12)-(16).

• if ϕ1 < 0 and ϕ2 < 0 then

◦ y =cos(ϕ1) cos(ϕ2)

sin(ϕ1 − ϕ2)D and x = tan(ϕ1)y −

D

2(12)

• if ϕ1 > 0 and ϕ2 < 0 then

◦ y =cos(ϕ1) cos(ϕ2)

sin(|ϕ1|+ |ϕ2|)D and (13)

16

� x = tan(ϕ1) · y −D

2when |ϕ1| < |ϕ2| (14)

� x = tan(ϕ2) · y +D

2when |ϕ1| > |ϕ2| (15)

• if ϕ1 > 0 and ϕ2 > 0 then

◦ y =cos(ϕ1) cos(ϕ2)

sin(ϕ1 − ϕ2)D and x = tan(ϕ2)y +

D

2(16)

This returns the ball’s position relative the reference system of the middlecamera. But what’s requested is the position relative the global referencesystem. To be able to acquire that, one first needs the position relative therobot goalkeepers body, which is not the same as the cameras, since theyare rotating. The transformation from the rotating frame reference to thecamera, to inertial frame reference to the robot body, is shown below:

xrelative = x · cos(cam.angle) + y · sin(cam.angle), (17)

yrelative = x · sin(cam.angle) + y · cos(cam.angle) (18)

Where cam.angle is the angle amount the beam that support the camerashas turned. Finally the robot goalkeeper’s position is added to the relativeposition of the target to achieve the target’s position in the global referencesystem described in (19), see figure 7.

xball = xrobot + xrelative and yball = yrobot + yrelative (19)

Figure 7: Position of ball and robot relative the global reference frame.

17

8 Trajectory estimation & Interception

One important part of the work has been to find a good interception al-gorithm for the robot goalkeeper to intercept the ball, together with a tra-jectory estimation. Two algorithms has been implemented and studied, theleast square algorithm and a theory that is widely used in games called thepredator - prey algorithm [14].

8.1 Least square algorithm

Assuming that the trajectory of the ball is a straight line, theoretically, itbecomes possible to measure the position of the ball at different time pointsand estimate a trajectory all the way to the goal line. This can be done byminimizing the least squares to determine the point of where the ball willcross the goal line. Setting the speed of the goalkeeper accordingly to theball, the goalkeeper can either intercept at the final moment of when the ballcrosses the goal line or simply head for the interception point at maximumspeed and wait for the ball to arrive.

When the camera see the ball for the first time, it starts a timer that ticksup at every iteration the ball lies in the cameras field of vision. The programsaves the position of the ball at every iteration. When the timer hits a spe-cific value for how many measurements to use, the algorithm fits a straightline to all measurement that was collected by minimizing the least-squares.At this point the robot still has not moved, until the last measurement hasbeen collected. The fact that the ball will follow a straight line, and thatthe robot begins at the origin of every experiment, the trajectory can becalculated and where the interception point will be.

Instead of intercepting the ball at the goal line, it is possible to minimizethe distance which the robot will travel. This can be calculated by using theperpendicular distance to the estimated trajectory. Once again the speed ofthe robot can be set accordingly to the ball, if the robot should interceptthe moment it reaches the interception point or simply by heading there firstand wait for the ball to arrive.

8.1.1 Arc maneuver & PID-controller

To gain an arc maneuver to the perpendicular distance a PID controller wereimplemented. The PID controller receives a weighted error that is based ontwo parts, the distance to the estimated trajectory and the bearing between

18

goalkeeper and target. The PID thus calculates the future heading of therobot as

eweighted = (1− k) · edistance + k · eangle (20)

From equation (20) the weighting factor k can be chosen to focus on mini-mizing the angle or minimizing distance to the trajectory. If focus should lieon minimizing the distance to the estimated trajectory, k should be chosenlow. But this value cannot be chosen too low since the goalkeeper then willforget about the target and drive “in darkness” towards the estimated tra-jectory. This is fine if the estimated trajectory is close to the true one butis devastating if not, since the goalkeeper then will drive to a “imaginary”interception point without caring about the target.

8.2 Predator - Prey algorithm

The basic idea of this intercept algorithm is to be able to predict a futureposition of the ball and move toward that position and reach it at the sametime as the ball. This concept is shown in figure 8.

Figure 8: Interception.

At first, it might appear that the predicted interception point is simply thepoint along the trajectory of the ball that is closest to the location of therobot. This is called the shortest-distance problem, where the shortest dis-tance from a point to the line is along a line segment that is perpendicularto the line. This might not necessarily be the interception point because theshortest-distance problem does not consider the relative velocities betweenthe robot and the ball.

To find the point where the robot and the ball will meet at the same time,one must consider their relative velocities. So, instead of just knowing theball’s current position, the robot also must know the ball’s current velocity,that is, its speed and heading. This information will be used to predict where

19

the ball will be at some time in the future. Then, that predicted positionwill become the target toward which the robot will head to make the inter-ception. The robot must then continuously monitor the ball’s position andvelocity, along with it’s own, and update the predicted interception point ac-cordingly. This facilitates the robot changing course to adapt to any evasivemaneuvers the ball might make due to friction or inclination in the floor.How far ahead the prediction the ball’s position should occur depends on therelative positions and velocities of both the robot and the ball. First thingthe robot does is to calculate its velocity relative a global reference systemusing the wheels radius and their rotational speeds described earlier. Fromthe odometry, triangulation and the α − β filter the relative velocities andrelative positions between robot and the ball are calculated. This is calledthe relative velocity and relative position which is simply the vector differ-ence between the ball’s velocity/position and the robot’s velocity/position.With this information the ball’s velocity relative the global reference systemis found.

Vball = Vrelative + Vrobot (21)

Here the relative velocity is denoted Vrelative. The Second step involves cal-culating the ball’s position.

Sball = Srelative + Srobot (22)

The relative position, between the robot and ball is denoted Srelative. Nowthere’s enough information to facilitate calculating the time to close. Thetime to close is the average time it will take to travel a distance equal tothe relative distance to close, while traveling at a speed equal to the relativespeed, from (21) and (22) we get.

tc =

��Sr

����Vr

�� (23)

The time to close, tc , is equal to the magnitude of the relative position,Srelative, divided by the magnitude of the relative velocity vector Vrelative.Now knowing the time to close, one can predict where the ball will be tc timeunits in the future. The current position of the ball is Sball and it is travelingat Vball speed. Because speed multiplied by the time yields average distancetraveled, one can calculate how far the ball will travel over a time intervaltc traveling at Vball and add it to the current position to yield the predictedposition, as follows.

Sp = Sball + Vball · tc (24)

20

Here, Sp is the predicted position of the ball tc time units in the future. It’sthis predicted position, Sp , that now becomes the target, or aim, for therobot. This equation (22) should be called every time through the processso that the robot constantly updates the predicted interception point andits own trajectory. The following figures illustrates the behaviour of thealgorithm.

Figure 9: Initial trajectories of the robot and the ball. An interception pointis as well established.

Figure 10: Robot successfully intercepts ball.

Figure 11: Corrective action, shows how robust the algorithm is when theball makes some evasive maneuvers. One can see that the initial predictedintercept point, as illustrated by the trail of dots ahead of the ball. However,after the ball makes an evasive move to the right, the predicted interceptpoint is immediately updated and the robot takes corrective action so as tohead toward the new interception point.

21

Figure 12: Successfully interception even when target change direction.

The interception algorithm is quite robust due to the fact that it allows therobot to continuously update its trajectory to effect an interception.

8.2.1 PID-controllers

Two PID-controllers were implemented, one regulating the wheels to thepredicted interception point and one for controlling the cameras that followedthe ball. The PID following the ball had two modes. One for the middlecamera as it had much better target identification but could only detecttargets at short distance, 60 cm and less. And a second mode for the othertwo cameras on the side when the target was at larger distance.

8.2.1.1 Camera PID The angle that the middle camera measured weresent as the error to the PID whose job was to minimize this by sending acontrol signal to the motor which rotated the cameras.

error = anglecamera,middle

When the target was far away that the middle camera could not identify thetarget, the second mode would be initiated and the sum of other two angleswere sent to the PID as error instead.

error = anglecamera,left + anglecamera,right

If the ball would be exactly between both cameras, meaning in front of themiddle one, the sum of the side cameras would be zero, as one is positiveand the other negative. The PID constantly tries to minimize this error bysending the control signal to the motor rotating the cameras, always keepingthe ball in the middle.

22

8.2.1.2 Odometry PID The PID for the wheels had the job of mini-mizing the angle between the goalkeepers vertical axis and the point of inter-ception. When the estimated interception point were calculated, the anglebetween the current direction and the estimated point would be calculatedas well and sent to the PID as error, see (25)-(27).

• If predicted interception point y-coordinate < 0 (behind goal line) and

◦ predicted interception point x-coordinate < 0

error = − arctan

�ypredictedxpredicted

�− 90− anglerobot (25)

◦ predicted interception point x-coordinate > 0

error = − arctan

�ypredictedxpredicted

�+ 90− anglerobot (26)

• If predicted interception point y-coordinate > 0

error = arctan

�xpredicted

ypredicted

�− anglerobot (27)

The PID would thus minimize this error to zero by sending a control signalto the wheels. If the angle between the current direction and the estimatedinterception point was large, meaning the error was big, then so was thecontrol signal, resulting in a sharper turn. When the angle was small theerror would be too, and the goalkeeper would simply go straight for theinterception point. A constant speed were set on both motors from thebeginning which the goalkeeper would always have if it went straight. Thecontrol signal from the PID would thus be added to one motor and subtractedfrom the other resulting in a turn, see figure 13.

23

Figure 13: The dashed line in black is the camera viewpoint and the dashedline in light blue the viewpoint of the robot. The error sent to the PID ofthe wheels is the angle in light blue whom sends the respective control signalto the motors which should keep minimizing this to zero.The error to thecamera PID is the angle in black which keeps the ball centered relative tothe middle camera.

8.2.2 Arc maneuver

To gain a clean interception one of the goals has been to move the goalkeeperin an arc maneuver when intercepting its target. Normally, since the robotis stationed vertically in the reference plane and has two wheels, moving likea car, it would most often not be possible to meet the ball from the front ofits course. Since the robot would instead move forward first then turn slowlytowards the interception point, see figure 14.

24

Figure 14: Wrong maneuver.

The arc maneuver implementation is preceded by instantly making the robotturn its direction 45 degrees if it is estimated that the interception point willbe too far to either side. After the turn has occurred, the goalkeeper moves tothe estimated interception point resulting in an arc. If the ball is heading ina straight course against the robot no turn is needed and the arc maneuver isneglected. This implementation allows the robot turn in an arc interceptingthe ball from the front of its course, see figure 15.

Figure 15: Correct arc maneuver.

25

9 Results

9.1 Object detection

All theory in the software relied on the target detection and its measurementsof position. If the detection were lost, last position and velocity would bepresumed and the alpha-beta filter would estimate the next position. Thesemeasurements came strictly from the cameras and would only work whenboth cameras could detect the ball. If only one camera could detect theball, the robot would follow the ball using the angle between robot and ballinstead of the predicted intercept point.

9.2 Position & Velocity measurements

When the ball and robot goalkeeper was in motion at the same time, the outercameras used for position determination had major difficulties detecting theball. Therefore no good measurements could be established in that aspect.Instead measurements were done on a non-moving ball, to test the theory inpractice. Figure 16 shows the robot’s y-position calculated with odometryand 17 the relative y-position between robot and ball. As expected, in figure16 the distance from start shuld increase linearly. And in figure 17 thedistance between robot and ball decrease linearly.

Figure 16: Goalkeeper’s y-position relative global reference system.

26

Figure 17: Relative y-position between moving goalkeeper and non-movingball.

9.3 Alpha - Beta filter

The two alpha-beta filters that were implemented, one for the x-axis and onefor the y-axis, improved the measurements significantly. Since the measure-ments were trustworthy but could at some instance contain a large error,the filter would parry these events. The first measurements would alwayscontain a larger bias. Thus presetting the first two positions and velocitiesof the target accordingly presented better results, even if they were off at thestart.

9.4 Least-square algorithm

Simulation in Matlab proved that the optimal value of k in equation (20)was 0.6 and that this value changed to 0.9 if edistance ¡ 20 cm, meaning thatfocus should lie more on minimizing the angle to the target. In figure 18 asimulation from Matlab is shown where the car has started to move.

27

Figure 18: The red circle and red dashed lines corresponds to the robot andits vision respectively. The green circles are position measurements done bythe camera (we only get readings when the ball is in the vision of the camera),the dashed green line is the estimated trajectory and the solid blue line is thetrue path of the ball. The robot started at the origin and has now traversedabout 25 cm in a horizontal manner and 75 cm in a vertical manner.

It is visible from the simulation that the deviation between the estimatedtrajectory (dashed green line) and the true path of the ball (solid blue line)is clearly unacceptable, proving this algorithm as non-functioning.

9.5 Predator - Prey algorithm

The predator-prey algorithm together with the arc maneuver intercepts theball successfully as desired. The interception works on all random cases andthe maneuver only acts when the ball is too far away to the sides. The caseswhere the interception fails is when the ball has a very high velocity and thecameras are unable to keep track of the ball at the same time.

28

10 Discussion & Conclusions

10.1 Least-square

Since time is very critical when the ball is in movement, it was not possible togather enough measurements for the least squares minimization to converge.The weighting factor that weights the bearing to the target and the distanceto the estimated trajectory must be chosen very closely to one, to obtainconvergence, which transforms the algorithm into an ordinary tracking algo-rithm. By choosing a value close to one you lose the advantages and purposeof this algorithm.

Simulations in Matlab showed that a slight increase in measurement errorsor a fewer number of gathered samples had a devastating impact on the out-come. It also showed that the variance of the outcome is very large meaningthat the estimated trajectories could differ very much within the same set-upof the experiment.

Two main issues with this algorithm was that it could not handle changes tothe heading of the ball, since the assumption was that it followed a straightline. And that the trajectory estimation was so sensitive to errors that theestimation could not be trusted. This could be improved by increasing theamount of data points used in the least-squares minimization, but at thesame time this increases the stall time at the beginning when the robotstands still and gathers measurements. For long distance targets when moremeasurements could be gathered, the measurements contained more errorsproving the algorithm useless for all practical use. At the time when thismethod was developed the camera was stationary mounted to the robot, asolution to the problem that the camera lost vision of the ball when the robotstarted to move. Implementing this with a rotating camera and continuouslyminimizing the least squares might have resulted in different outcome

10.2 Predator - Prey

The goal of creating a robot goalkeeper that intercepts a rolling ball in itscourse using an arc maneuver had been successfully accomplished. A moresmooth maneuver would have been desired but the results accomplished werevery satisfiable. One should emphasize that, if the v4 camera worked as goodat long range as it did on short range, much better measurements would havebeen expected and thus a better working goalkeeper. It took a long time totune the cameras and to get them to work properly, and even then the v4

29

model worked better at short range. All computations also included someminor noise which came from varying factors such as system noise and frictionfrom the floors. Adding these computations to one another resulted in largernoise as the program were running. In some cases this was the decidingfactor which caused the goalkeeper to miss its target in the final moments ofinterception. Which meant that if the program ran for a long time one couldexpect many misses. Another limiting factor was the computational time onthe brick. Putting more cameras to increase the reliability of measurementsand tracking resulted in longer computation time. With one camera thecomputation time did not exceed 30ms for an iteration, while with threecameras one could expect iterations around 70ms. This limitation camefrom the cameras frame rate of 30 frames per second.

30

11 Issues and experiences

Programming The programming language for the project has been Java.The main reason is due to that the leJOS API uses Java as its primary lan-guage, and most of the group members have had previous experience withJava from past classes. LeJOS is supported by two IDE/Editors, they arethe opensource IDE Eclipse and Netbeans which belongs to Oracle. Eclipsewas chosen because it had better documentation and is more widely used bydevelopers.

The ideal programming language for the project would have been Matlab,but there weren’t any support for the cameras in Matlab, so that had to beneglected.

Recommendations when choosing programming environment in the futureis to select one that support good test and debugging tools. In the beginningvariable values were printed to the small screen of the brick to check theresults. Later a logging system was set up to print the result to a file wheremore data could be stored. But there was no way of manipulating the data,that was resolved by loading the files to Matlab for further analysis.

Cameras A lot of our time has gone to troubleshooting the cameras. Theyare unreliable (forgets colormaps and does not start recording at some time)and has poor performance (a slight change in lighting condition has tremen-dous impact on the recorded object size). After some time we got hold of anewer version of the cameras, v4, which had a more robust tracking systembut had a big drawback. It’s threshold that separates noise between actualobjects is very high leading to a maximum distance measurement of 50 cmwhich is unacceptable in our case.

Distance from camera Many different approaches were tried to gain adistance directly from the rectangle area obtained using the cameras. Butas these areas proved to fluctuate all the time, no reliable distance could begrasped. The distance was calculated manually correspondent to the rect-angle area and interpolated, without any good results. Another approachused the fact that the inverted area was proportional to the distance. Thisalso resulted in nothing reliable. Neither did the fact to use the size of theball together with the focal length of the camera. The one reliable calcula-tion which could be retrieved from the cameras was the angles used in thetriangulation.

31

Distance sensor Mindsensors [4] provided a distance sensor which couldmeasure the distance to a target using IR sensors. Both the long range andmedium range were tried but as the field of vision was too narrow none ofthem had any practical use for our purposes.

Motors In the LEGO NXT kit three motors were included which couldbe used together with the LEGO NXT brick. In each motor there was abuilt in tachometers that made it possible to retrieve the amount of anglesrotated and set the motors to turn a specific set of degrees using simple com-mands in leJOS. According to mindstorms [ ] there existed a deviation onthe tachometer of ±2 degrees which was acceptable for the odometry. Butas many motors had been extensively used before most of them were wornout. Resulting in two motors rotating at different speeds in if they were setthe same. This also affected the deviation which had grown widely on mostmotors. Finding two motors for the wheels that worked together in harmonywas one of the biggest issues, since the deviation accumulated with each it-eration a motor with small deviation was of great importance.

The same fact applied for the motor rotating the cameras. As a small de-viation could have great impact on the performance of following the ball,finding a motor that was not worn out became a large issue. Many motorshad also the problem of being too loose or too fixed that lead the rotationto fluctuate from the desired behaviour.

Datalogging A method for data logging has been one of the most helpfuladvantages in this project. The data are stored in a file and then uploadedfor use in matlab and the matlab simulator. This allows to process the dataand gain an overview of how the goalkeeper treated each part. Sometimesthis would become too ambitious and it would have been preferred to have asimpler system that showed the data live instead of storing it on a file. TheLCD screen of the brick was enough on some cases but a more subtle wayusing bluetooth and displaying it on the computer would have been preferred.

Matlab simulation It has been a real advantage to be able to simulatenew algorithms and procedures in Matlab before each actual implementa-tion begun. We should have realized this at the beginning of the project butthese simulations were developed when our first algorithm, the LS-algorithm,crashed and we didn’t know why. Our simulations showed that the erroneousmeasurement data caused the robot to follow a false estimated trajectory pre-venting the robot to catch the ball. If these simulations had been done at

32

the beginning, we would probably not have implemented the LS-algorithmand saved us a lot of time.

Since we were more comfortable with Matlab syntax than with Eclipse, wecould write preliminary object classes and scripts in Matlab and then trans-late these into Java code. We created three classes (a robot class, a cameraclass and a target class) and two scripts (the LS-algorithm and the intercep-tion algorithm). To get real-like simulations we added noise to both distancemeasurements and angle measurements done by the camera. By making afew experiments on the real camera we could estimate the standard deviationof these two errors. We also added restrictions on the cameras vision, bothhow far and how wide it could see and also a function that would allow thecamera to rotate on a separate axis.

The ability to simulate algorithms created an opportunity to make estimatesof different variables that we used in the implementation design, for examplePID-parameters, weighting factors etc. This was a huge relief since these arehard to estimate after the program has been implemented and tested.

An Alpha-beta filter was also developed in Matlab, but only for the intercep-tion algorithm since we already had discarded the LS-algorithm. Tuning ofthe two parameters in this filter (alpha and beta) is really hard and very timeconsuming in Eclipse, since one needs to upload the program with new set ofparameters and then visually determine if this set performed better or worsethan before. Using Matlab this could be done much easier. By importingreal measurement data from experiments we could construct a nonlinear costfunction and then evaluate the filter with all kinds of different parametersand then find an optimal value of alpha and beta. The cost function was thedifference in position between the true position of the ball and the estimatedposition done by the Alpha-beta filter. This approach for a few experimentsallowed us to find well suited parameters for our cause.

A sophisticated graphical user interfaced was developed so that one couldfollow the performance of the algorithms in “real” time and to get a visualtroubleshooting tool. Figure 18 is a snapshot of how this GUI looked in asimulation.

33

References

[1] http://asimo.honda.com

[2] http://new.abb.com/products/robotics

[3] mindstorms.lego.com

[4] http://www.mindsensors.com

[5] http://www.lejos.org

[6] http://www.lejos.org/nxt/nxj/api/ (for built in functions)

[7] http://nxtcamview.sourceforge.net

[8] http://www.mathworks.se/ (for simulations)

[9] http://collaboration.cmc.ec.gc.ca/science/rpn/biblio/ddj/Website/articles/CUJ/1993/9307/penoyer/penoyer.htm

[10] T Glad, L Ljung: Reglerteknik - Grundlaggande teori, fjarde upplagan,studentlitteratur 2006

[11] T Glad, L Ljung: Reglerteori - Flervariabla och olinjara metoder, andraupplagan, studentlitteratur 2003

[12] http://www.math.tamu.edu/ dallen/physics/gps/gps.htm

[13] http://www.robotnav.com/position-estimation/

[14] David M Bourg, Glenn Seemann - AI for Game Developers Publisher:O’Reilly Media Released: July 2004 Pages: 392

34