Intro Robot Planning and Estimation Homework #8 Solution

Notice:

Please upload 2 les for this homework. One to include the written part of the homework (problem 1, and the extra credit problem 3), and one python les for the second exercise. In your assignment include the names of the other students you have collaborated with to do the homework problems. The Collaboration Policy for this course is detailed in the syllabus.

Homework exercises:

    1. Assume that you have a robot equipped with a sensor capable of measuring the distance and bearing to point landmarks. The sensor furthermore provides you with the identity of the observed landmarks. A sensor measurement z = (zr; z )> is composed of the measured distance zr and the measured bearing z to the landmark l. Both the range and the bearing measurements are subject to zero-mean Gaussian noise with variances r2, and 2, respectively. The range and the bearing measurements are independent of each other. A sensor model models the probability of a measurement z of landmark l observed by the robot from pose x.

        (a) Design a sensor model p(zjx; l) for this type of sensor. Furthermore, explain your sensor model.

        (b) Assume that it is known that your sensor can only provide measurements with a maximum range value of zmax. Modify the sensor model in part (a) appropriately.

    2. A particle lter consists of three steps, which you will implement in the following. The steps are listed in the following:

        (a) Sample new particle poses using the motion model.

        (b) Compute weights for the new particles using the sensor model.

        (c) Compute the new belief by sampling particles proportional to their weight with replacement.

Consider a robot moving close to a wall (of length [  50; 50]) and with 3 doors at locations [0:5; 1],

[2; 2:5], and [22:5; 23]. Do the following:

    (a) Implement a probabilistic motion model function of the robot p(xtjxt 1; ut) given as a truncated Gaussian random variable of mean xt 1 + ut and variance u2 = 0:5, for ut 6= 0. When ut = 0, then the motion is deterministic, xt+1 = xt. The truncated Gaussian should give values between [ 50; 50].

    (b) Implement the sensor likelihood model function:

P (z = 1j0:5    x    1; or 2    x    2:5 or 22:5    x    23) = 0:7

P (z = 0jx at door locations) = 0:3

P (z = 0jx not at door locations) = 0:85

P (z = 1jx not at door locations) = 0:15



1
        (c) Consider the control data ut = 1 for 1 t 14 time steps, ut = 0 for 2 steps, 15 t 16, and ut = 1, for 17 t 38 steps. The sensor sequence is fztg = f0; 0; 0; 1; 0; 1; 0; 0; 0; 0; 0; 0; 0; 0; 1; 1; 0; 0; 1; 0; 0; 0; 0; 0; 0; 0; 0; 1; 0; 0; 0; 0; 1; 0; 0; 0; 0; 0g at each of the 38 steps. Implement a particle lter by using the most basic resampling method discussed in class. This lter should be a function of the number of particles. re-draw a new particle until you get something within [ 50; 50]. Note: Particle deprivation e ects could be apparent after a small number of iterations t with a small number of particles. Find out at what iteration and what number of M you may observe such problems. Visualize the particle locations at di erent times t by means of plots of vertical lines at the locations x[tm] and with height 1.

    3. (Extra credit problem) Many early robots navigated using arti cial landmarks in the environment that were easy to recognize. A good place to mount the markers is a ceiling, and a classical example is a visual marker. Suppose that you have the marker of Figure 1 attached to the ceiling.










Figure 1: Marker for Exercise 3

Let the world coordinates be xm ym and its orientation relative to the global coordinate system    m.
We will denote the robot’s pose by xr; yr; and  r.

Now assume that we are given a routine that can detect the marker in the image plane of a perspective camera at the robot. Let xi, yi denote the coordinates of the marker on the image plane, and i its angular orientation. The camera has focal length of f. From projective geometry, we know that each displacement d in the x y space gets projected to a proportional distance d fh in the image plane, for some constant h. (You have to make some choices of your coordinate systems; make these choices explicit.) Do the following:

    (a) Describe mathematically where to expect the marker (in global coordinates xm; ym; m) when its image coordinates are xi; yi; i and the robot is at xr; yr; r.

    (b) Provide a mathematical equation for computing the image coordinates xi; yi; i from the robot pose xr; yr; r and the marker coordinates xm; ym; m.

    (c) Now give a mathematical equation for determining the robot coordinates xr; yr; r assuming we know the true marker coordinates xm; ym; m and the image coordinates xi; yi; i.

    (d) So far, we have assumed there is a single marker. Now suppose that there are multiple (indistin-guishable) markers of the type shown above. How many such markers must a robot be able to see to uniquely indentify its pose? Draw such con guration, and argue why it is su cient.
















2