Mobile Robot Localization by using Particle Filter

1. Mobile Robot Localization by using Particle Filter GuanghuiLuo, Chong Wang and Chong Fu Instructor: Dr. Dan Wu

2. Outline • Introduction • Experiment platform and setup • Hardware • Boundary Area Setup • Programming Environment • Implementation Details • Particle Filter • Odometry error measurement • Demo

3. Introduction • Mobile robot localization is the problem of determining the position of a robot relative to a given map of the environment. It is a fundamental problem for mobile robots system. If a robot doesn’t know where it is, it is difficult to decide what to do. • In order to localize itself, a robot has access to relative and absolute measurement giving the robot feedback about its driving actions and the situation of the environment around the robot. Given this information, the robot has to determine its location as accurately as possible. What makes this difficult is the existence of uncertainty in both the driving and the sensing of the robot. In our project, we implement the Particle Filter algorithm to acquire the position of roomba by repeating to calculate the data which response from the bumper sensor of roomba.

4. Experiment platform and setup • Hardware • Roomba • BAM • Bluetooth • Laptop

5. Roomba is a low-cost vacuum robot available for research and education.

6. A roomba has its built-in sensory and actuation abilities. Sensors/Inputs : Actuation/Outputs: buttons(3-4) drive wheels(L&R) bump sensors(2) LEDs(5-7,color) IR wall sensor(1) IR receiver(0-255) cliff sensors(7) virtual wall remote control

7. Open Interface Commands: IRobot Create comes with a serial protocol called the Open Interface (OI) which consists of a set of commands that we can use to control the robot and read its sensors. The types of commands available include: • Demo commands • Driving commands • Song commands • Sensor commands • Cargo Bay Connector commands • Scripting commands

8. The Element Direct BAM (short for Bluetooth Adapter Module) enables wireless control of the roomba from a Windows, Mac, or Linux PC. The BAM provides a virtual serial port connection between a Bluetooth host and Roomba.

9. Using a Bluetooth USB radio is often the simplest way to add Bluetooth support to a computer. • The Most flexible method of communication • Allowing a range up to 100 meters • Connection quality over distance drops slowly

10. The Hardware Environment consists of 4 parts as follows:

11. Boundary Area Setup In our project, we construct a 3.10m (Width) * 1.80m (Height) bounded area with boards and walls. We give two different environments. One is without any obstacle, and the other one is that a 0.24m*0.19m size obstacle lying in some fixed position of boundary area.

12. two different environments: Initial position of roomba.

13. Programming Environment • Eclipse SDK Version 3.4.1: Eclipse is an open source community, whose projects are focused on building an open development platform. It can be used to develop applications in java, by means of plug-ins, in other languages as well –C/C++, Python, PERL, PHP and more. • Java 1.5 + RXTXSerial ( rxtxSerial.dll) : If you want to access the serial or parallel port with java, you need to install a extend library. Install either javax.comm from SUN or better install the rxtxSerial from rxtx.org. • RXTXComm (RXTXcomm.jar + roombactrl-1.0.jar): RoombaComm is a Java library that allows users to communicate with Roomba through a language Java via serial or parallel port. • Processing (core.zip): Processing is an open source programming language and environment. It can be used to program images, animation, and interactions. Core.zip is java library supporting processing language and can be imported by Eclipse Development Environment.

14. How to know if the localization is correct? we design a graphic interface program to simulate the motion of the roomba and its corresponding environment. Finally, the concentrative particles will represent the location of the roomba. If the actual location matches the position where the concentrative particles stay, we can consider our project reaching objective.

15. Setup The initial position of the roomba is on the center of bounded area. When the roomba starts to be connected, it initially rotates 60 degree counterclockwise, and then goes forward. Once the roomba hits the boundary (the wall or obstacle), it automatically rotates 120 degree and then goes forward …… In our program, we set the sizes of the bounded area, roomba and obstacle proportional to those of the real corresponding things. The number of particles is given by 10000. The coordinates and angles of all particles are given randomly. Let the weight of each particle be the same (average weight).

16. Problems with Roomba: • Totally blind, has only the bumper sensor, over relying on motion model • When bumps the wall (obstacle), Roomba gives a feedback lasting a short time • We can send command, but can not check the status of the command and the same case when you get information from roomba • When rotate a certain angle, Roomba needs time • Sometimes Roomba may rotate twice when hitting the wall (obstacle)

17. Outline • Introduction • Experiment platform and setup • Hardware • Boundary Area Setup • Programming Environment • Implementation Details • Particle Filter • Odometry error measurement • Demo

18. Particle Filter The main technique that we use in our project is Particle Filter algorithm. We implemented a particle filter solution to track the position of the roomba.

19. Particle Filter • The key idea of the particle filter is to represent the posterior density by a set of random particles with associated weights. Posterior density Sample space

20. Fundamental concepts of particle filter • Particle Filter The particle filter algorithm recursively operates in two phases: • Prediction ( eg. Using particles to predict where the roomba is) • Update (eg. Update the weight of particles and resample )

21. How the processing work in eclipse • The processing is an application base on applet and it is called PApplet in the program. • Therefore, any java program using processing must be a sub-class extends PApplet. • public class Roomba extends PApplet

22. How the processing work in eclipse • There are two basic methods in the processing program. • public void setup() { //Initialize the program //Draw static image • } • public void draw() { • //Draw animation • }

23. What is the program doing at every timestep • 1. Draw the map firstly; • 2. Randomly select 10000 particles, uniformly distributed and start the roomba; • 3. Update all particle positions using the motion model; • 4. Monitor bumper sensors each 0.03s. If the roomba does not hit the wall, the program repeat steps 3 - 4 , otherwise do next; • 5. Calculate the new weight of particles; • 6. Resample and generate the new set of particles; • 7. Repeat steps 3-7 for each timestep.

24. What is the program doing • Step 2.At the beginning, randomly select 10000 particles, uniformly distributed • PARTICLE_X[i1] = Math.random() * SCREEN_WIDTH ; • PARTICLE_Y[i1] = Math.random() * SCREEN_HEIGHT; • PARTICLE_A[i1] = Math.random() * PI * 2 - PI; • WEIGHT[i1] = (double) (1) / PARTICLE_NUMBER;

25. What is the program doing • Step 3. Update all particle positions using the motion model • We use the robot odometry as the particle filter motion model. • Motion Model: • NOISE_DISTANCE = RMBA_ADJUSTIVE_DISTANCE* (1 + Distance_NOISE * Math.random()); • PARTICLE_X[i] += (NOISE_DISTANCE) * (Math.cos(PARTICLE_A[i])); • PARTICLE_Y[i] += (NOISE_DISTANCE) * (Math.sin(PARTICLE_A[i]));

26. What is the program doing • If roomba hits the wall/obstacle, calculate the new weight of particles, • For the particles are adjacent to the wall / obstacle: • WEIGHT[i] = Math.random() * 0.2 + 0.7; • WEIGHT[i] /= SUM; • The other particles: • WEIGHT[i] = Math.random() * 0.2 + 0.3; • WEIGHT[i] /= SUM;

27. What is the program doing We use “Algorithm Low Variance Sampler” to resample the particles. • 1: Algorithm Low variance sampler(Xt;Wt): • 2: Xt = NULL; • 3: r = rand(0;1/M) • 4: c = w[1] • 5: i= 1 • 6: for m = 1 to M do • 7: u = r + (m - 1) * 1/M • 8: while u > c • 9: i= i + 1 • 10: c = c + w[i] • 11: endwhile • 12: add x[i] to Xt • 13: endfor • 14: return Xt

28. What is the program doing • Implement “Algorithm Low Variance Sampler” If the weight of one particle x[i] is much greater than 1/M, c ( c=c+w[i] ) will also much greater than u. The coordinate and angle of X[i] + numRandom() * 0.05 will be selected as sample to replace the low weight particle {numRandom(#: -1 to 1)} Therefore the more low weight particles will congregate to the position of high weight particles.

29. What is the program doing • We do the resample again and again, the particles will congregate in one point finally. • We have found that the particle filter gave an adequate solution to the localization problem in an our environment.

30. Outline • Introduction • Experiment platform and setup • Hardware • Boundary Area Setup • Programming Environment • Implementation Details • Particle Filter • Odometry error measurement • Demo

31. Odometry motion model Odometry measurement model is based on calculating the roomba’s motion over time, which tend to be more accurate than velocity model. We use the algorithm for sampling from p(xt |ut ; xt-1) based on odometry information.

32. Modeling odometry error In order to increase the accuracy of predicting the probability distribution of the pose of the moving robot after a motion we need to pay more attention on the model of the effect of noise. The error could be divided into rotational error and translational error. These errors can be modeled statistically by random variables drawn from three Gaussians with zero mean and σrot,σtrans,σdrift standard deviations. σrot—occursduring pure rotation of the roomba σtrans,σdrift—occurs during a forward translation of the roomba

33. Rotation ^ When the roomba performs a relative rotation by δθ the noise from the odometry error is modeled as a Gaussian with mean(Mδrot) experimentally established and sigma proportional to δθ. ^ ^ ^ ^ ^ θk+1 = θk + δθ + N(Mδrot , σrotδθ) Arbitrary motion [∆x, ∆y]т of roomba . At time t= k- 1 the pose is [x, y, θ], after the motion at time= k the pose is [x̒, y̒, θk̒]. The robot first rotates to orientation θk̒ and then translates by ρk . ^ ^ ^

34. Translation • The two different sources of error in modeling the forward translation: • the first related to the actual distance traveled — σtrans • the second related to changes in orientation during the forward translation — σdrift

35. The emphasis is to experimentally estimate the rate of odometry error buildup in a small drive research roomba, and model its behavior probabilistically.

36. Error in rotation and translation is relative to different angles and different speed and different surfaces In our project we set roomba a fixed speed in rotation and translation command. Rotation error Send roomba a command roombacomm.spinRight(90) We gather twenty samples find the noise σrot=0.045⁰/degree

37. Modeling the translation of the roomba is more difficult because the noise model is more complex. The simplest approximation of the error is to model the translation as a partial rotation followed by a translation followed by a second rotation. Send roomba a command roombacomm.goForward(1000) The effect of σtrans,σdrift for the forward translation: σtrans = 2.4cm/m σdrift = 2.3⁰/m

38. Bumping noises Not only roomba needs a certain time to send the bumping signal to the computer, but also there exist a small interval it receive the rotation command. This short time period cause the most effect in roomba angle. The noises of roomba_angle predicting consist of three factors: rotation, drift, and bumping errors. When roomba bump to the wall or object we add the bumping noise to the angle of particle filter: NOISE_ANGLE = RMBA_ANGLE * (1 + ANGLE_NOISE * Math.random() + WALL_NOISE*numRandom());

39. Demo1

40. Demo2

41. Evaluation • We try to determine the location of the roomba in the map of program which is correspondence to the real environment. For both of two different environments (with and without obstacle), the robot can work successfully even though some noise added. We find that it is much faster to localize the roomba with higher probability of success when an obstacle exists. There are two reasons cause to this situation. • Firstly, there is obviously higher opportunity for roomba to hit the walls/obstacle that would increase the times of particle resample. • Secondly, this obstacle is unsymmetrical in the map that is easier to confirm localization.