ME 495 Special Topics: Motion Planning and Control Under Uncertainty
Course Information, Spring Quarter, 2006

    Instructor: Prof. Kevin Lynch, kmlynch@northwestern.edu
    Class Hours: MWF 3:30-4:20, Tech M166
    Teaching Assistant: James Solberg (j-solberg@northwestern.edu)

    Course Description
    Classical techniques from stochastic optimal control theory including Kalman filtering and linear quadratic Gaussian problems; recent computational techniques in Bayesian inference and Markov decision processes; applications to control of robot systems under sensor and actuator uncertainty.

    Course flier

    Texts
    Probabilistic Robotics, S. Thrun, W. Burgard, and D. Fox, MIT Press, 2005. Link to the book's homepage.
    Optimal Control and Estimation, R. F. Stengel, Dover, 1994.
    Other material will be taken from Planning Algorithms, S. LaValle, Cambridge University Press (to appear), available online at http://planning.cs.uiuc.edu/.

    Approximate Syllabus
    Throughout the course we will be reading papers applying the ideas under study to robot exploration, localization, and motion planning and control problems. Chapter numbers below refer to chapters in the Thrun book unless otherwise specified.

    Final Projects
    You may indicate your preferences among the final project suggestions outlined here.

    Relevant Research Papers
    See James Solberg's depository.

    Assignments

    1. Due Monday Apr 3: Exercises 2.2, 2.3, and 2.4.

    2. Due Monday Apr 10: Exercises 4.1, 4.2, 4.4, and 4.5. You do not have to compare your results to those of the extended Kalman filter, which we have not covered yet.

    3. Due Wednesday Apr 19: The problem formulation (intro), related work, and bibliography sections of your final project report. This document will be the beginning of your final report, and should be written in a standard conference or journal paper format, two columns and 10 point font.

      The problem formulation section should give a clear description of your problem, possibly introducing variable definitions. There should be a figure or two to illustrate the problem. This section should be approximately 1/2 to one page. The related work section should cite approximately 10 papers that are most relevant to your project, and should give a one paragraph description of the main ideas, novelty, and results of each. It should also describe how the problem studied is the same or different than your problem. (In other words, I expect to see more than just a superficial restatement of the abstracts.) The bibliography should be in standard conference or journal format. (It is acceptable to have more than 10 references, but no more than 10 should have the kind of review described above. The others would be considered secondary references.)

      Finally, you should give a pointer to a website where you have the papers archived in pdf format. Not all relevant papers are available on the web! If not, go to the library, make a copy of the paper, and scan it.

      To find relevant papers, you can check out papers cited in relevant papers you already have. You can do a search using the INSPEC (or other) databases on the Science and Engineering Library website. Google Scholar is a great resource. You can search by topic, or you can look for papers that cite papers you know to be relevant. When deciding which papers to include in your bibliography, the number one criterion is relevance to your problem. A secondary criterion is the number of times the paper is cited (from Google Scholar, for instance), and journal papers are preferred over conference papers when they are otherwise equal.

      One last piece of the assignment: Choose one paper that you think is particularly relevant or interesting for your project. You will present that paper in class one Friday for about 25 minutes, and the rest of the 25 minutes you will lead a discussion on the paper. Everyone in class should have read the paper in advance, and your job will be to get the discussion going. You will also come up with one or two homework problems based on the reading. They could be an implementation of an algorithm in the paper, or analysis of a simplified problem using the techniques presented in the paper. These problems will be due in the homework set due on the Monday one week and three days later.

    4. Due Wednesday Apr 26:
      • Read Chapters 11.1-5 and 12.2 of Steve LaValle's book Planning Algorithms. You can download the chapters for free here.
      • Read this paper to appear at the 2006 Robotics Science and Systems conference and be prepared to discuss it Friday April 28 in class.
      • Simulate the EKF algorithm for the mobile robot described in class on Monday Apr 17. The mobile robot starts out at (x,y,theta) = (-10, 10, 0), where x and y are measured in meters and theta in degrees. It senses a landmark at (0, 0) by range and bearing measurements. The robot knows the landmark location from its map, but does not know precisely its own location. The initial mean of the robot's state belief pdf is (-12, 9, 10) with a diagonal covariance matrix of (3, 3, 20). The robot moves for 4 seconds, with constant forward velocity u1 = 5 m/s. During the first second, u2 = 0 deg/s; during the next second, u2 = -90 deg/s; and during the last two seconds, u2 = 90 deg/s. The range sensor has Gaussian noise with stdev = 1 m and the bearing sensor has Gaussian noise with stdev = 10 deg. The motion model gives you a new configuration error of max({0.1, abs[u1t*cos(theta_{t-1})]})*dt*eps1 in the x direction, max({0.1, abs[u1t*sin(theta_{t-1})]})*dt*eps2 in the y direction, and max({1, abs[u2t]})*dt*eps3 in the theta direction, where dt is the time of the step, eps1 and eps2 are random variables from normal distributions of 0 mean and variance 0.1, and eps3 is a random variable also from a normal distribution of 0 mean and variance 0.1.

        For all plots below, make sure the x and y axes are equal! Your plots will look wrong if one axis is stretched relative to the other!

        • Plot the landmark and the nominal motion of the robot (no motion uncertainty) as a curve from its initial position (-10,10). You do not have to indicate the orientation, as the heading is always tangent to the curve.
        • Simulate the belief propagation forward for the first one-second time step using the motion model. Before sensing, plot the (x,y) position belief as a 1 sigma uncertainty ellipse centered at the mean. This ellipse is the projection of the full 3d uncertainty ellipse using the top left 2x2 submatrix of the covariance matrix. You can plot the orientation uncertainty of the robot by plotting arrows or line segments stemming from the mean robot location and pointing in the +/- 1 sigma orientation directions (use the bottom right element of the covariance matrix).
          Now the robot senses a distance of 11.2 m from the landmark and a bearing (relative to the robot's bearing) of -63.43 degrees. Incorporate this data and plot the new belief. Your final plot should show both the belief before the sensor data and after.
        • Now simulate the belief over the entire four-second motion assuming the robot does not sense. Simulate the belief propagation in 0.1 time second increments, but plot the belief at time increments 0, 1, 2, 3, and 4 s.
        • Now assume that the robot senses the landmark at times 0, 1, 2, 3, and 4 s. (The motion model is still updated every 0.1 s.) Assume the measurements it gets are the noiseless measurements it would have gotten moving exactly on the nominal path from the first part of this problem. (Of course, the robot does not know its motion and sensing are perfect.) Plot the belief at times 0, 1, 2, 3, and 4 s after the sensing (it is assumed the sensing happens after the motion).
        • Making sure the two previous plots are on the same axes, overlay them on top of each other so we can compare the belief propagation with and without sensing over the entire four-second motion.
        • Explain in a few sentences how the motion and noise models might be unrealistic and how they could be improved (perhaps violating the linear Gaussian assumptions). If the noise models violated the Gaussian assumptions, but you still wanted to use a Kalman filter, what approximations might you make?
        • Turn in your code and plots.

    5. Due Wednesday May 3: You will use a Kalman filter to estimate the landing point of a ball imaged by a camera. The ball flies in a vertical plane, and the camera takes an image every 1/30 s and returns (x,y) coordinates. All positions are measured in cm and all velocities in cm/s.
      • The state of the ball is written (x,y,xdot,ydot). Gravity is measured to be 981 cm/s^2 in the negative y direction. Due to perturbations, the motion model includes a random variable with a covariance matrix given by diag(1,1,900,900). Write the motion model (specified by A, B, R) that advances the estimate 1/30 s assuming no wind resistance (i.e., the nominal flight of the ball is a parabola).
      • The sensor measures the (x,y) position of the ball with a covariance matrix given by diag(25,25). Write the sensor model (specified by C, Q).
      • Your filter initially has no information on the ball's location. It then receives data spaced at 1/30 s intervals, for 1 second, beginning at time t = 0. There are 31 data points for the x measurements and the y measurements. After the first measurement at t=0, you have no information on the ball's velocity, but you can begin to estimate the velocity after the second measurement. At t=1/3 s, 2/3 s, and 1 s (i.e., after 11, 21, and 31 measurements, respectively), plot the uncertainty ellipse in the ball's (x,y) location on the same plot. Make sure your x and y axes have equal scaling. Also give the full covariance matrix at these times. (If you want to do a sanity check, you could try doing a least squares fit of a parabola to all the data and see what nominal trajectory this would give.)
      • You want to estimate where the ball will land (when y = 0). Give your best estimate of the landing location after 1/3 s, 2/3 s, and 1 s. (You can get this by taking the mean of your estimate after each of these times and integrating it forward until y = 0.) Explain in words how you would estimate the uncertainty in these best estimates in terms of the x landing location after each of these times.

    6. Due Wednesday May 10:
      • Problem from Mike and James (forthcoming).
      • You are swimming in a river, with your position given by (x1, x2). You swim at 1 km/hr at an angle u. (Your control is u; your swimming velocity is fixed.) The river flows in the positive x1 direction at 2 km/hr, so your total velocity in the river is (dx1/dt, dx2/dt) = (2 + cos u, sin u). You are trying to swim to the position (x, y) = (0, 0) in minimum time. (a) Form the Hamiltonian and use the necessary conditions for an extremal control to determine the form of the optimal controls. (b) You start at the position (-3, 1). Can you achieve the goal? If so, what is the optimal control, and how long does the optimal motion take? (c) You start at the position (-4, 5). Can you achieve the goal? If so, what is the optimal control, and how long does the optimal motion take?

    7. Due Wednesday May 17:
      • Problem from Aaron and Laura.
      • Problem 5.3.1 (p. 490) from the Stengel book: Use Matlab's lqr function to find the optimal feedback controller for the system dynamics given and the cost function implicit in the integrand. (Ignore the question asked and the limits of the integration.) Explain, in words, why the controller you find using lqr is optimal for an LQG problem such as this (there is one key idea). Also, independent of this particular example, explain in a couple of sentences (again, there is one key idea here) why the optimal lqr controller for a linear system with no uncertainty (regulated to an operating point, for example) is guaranteed to make the system stable, assuming the system is stabilizable.

    8. Due Wednesday May 24: Problem from Ben and Tom, plus a draft of your final paper in its current state.

    9. Due Wednesday May 31: Problem from Tiffany and Peng.

    10. Due Wednesday June 7: Your final paper is due.

    Friday Classes