Lab 4: Trajectory Following


Lead TAs:

Challenge Statement:

Using feedback control, make a marker that is attached to a robot follow a trajectory of the form x_ycm.


  1. Place the robot in a position (for the center of the robot) and orientation on a plane specified by a TA (in cm and degrees respectively) and input A, α, β, phi_x and phi_yalso given by a TA for the equation above. Note that the initial position of the center of the robot may not be the start of the trajectory but will not be more than 5 cm away from the start.
  2. Press a button on the robot so that it follows the trajectory specified in the challenge statement where t is the time in seconds after the button press.
  3. After 30 seconds, the robot must come to a complete halt. If the robot is still moving after about 30 seconds, upto 20 points maybe docked depending on how long the robot still keeps moving.
  4. For grading purposes, the position of the marker will be approximately recorded at 10 second intervals giving a total of 3 samples. The error between the actual and measured value of position at these samples will determine a large portion of the grade for this lab.
  5. Three tries will be given in order to improve your score if needed. All tries are independent of each other. Progressively easier trajectories and intial configurations can be requested, each for a 10 point penalty and only once per try. A request cannot be made on the first try.

How to:

The following description applies only to a standard differential drive robot.

  1. Start by reusing the code from last lab and attaching a marker to the robot some distance away and to the front or back of the center.
  2. From here, two things can be done :
      i) The position of the center of the robot can be computed just like in the previous lab and from that, the position of the marker can be found using geometry. Important equations for this method are :
      l_x_y: Position of the marker wrt. the center of the robot in cartesian coordinates
      ii) Equations for the motion of the marker tip can be directly integrated. Important equations for this method are :

  3. Armed with the knowledge of where the marker is (from the numerical integration), where the marker should be (from the given trajectory) and the relationship between linear velocity of the pen and the linear and angular velocities of the center of the robot, a contorl law can be made that outputs wheel velocities that brings the robot closer to its desired location.
    Let p_cbe the current position of the marker and p_dthe desired position obtained from the trajectory. Our desired planar velocity is now proportional to p_c- p_d. So, vel_rel2 where κ is some positive real number. From the equations of motion for the marker tip given above, we can solve for v_w :
    Now wheel velocities (in degrees / second) can be obtained from v_w using the equations defined in Lab 3 :
  4. Now that the desired velocities for the left and right motors are known, we need a way to ensure that the motors spin at that speed. The only variable we can control is motor power which is only weakly related to motor rototational velociy. However, due to the proportional nature of our controller, if we set the motors to a slightly slower speed than desired, the desparity between current and desired position will grow larger and a higher velocity is demanded. So, set the motor velocities to the desired motor velocities (in degrees / second) * 0.1 and wait for a small time interval before proceding to the begining of the loop again so that the motors have some time to catch up to the velocity desired of them.

Other tips:

  1. It is essential to have a low PID update interval (2 - 10 ms).
  2. The integration loop must be small (5 - 20 ms) but no smaller than the PID udate interval so as to let the motor catch up to the desired velocity.
  3. Save this lab. It will be used in the next lab.
  4. Play around with the proportional constant in order to fine tune the controller. Increasing the constant causes the marker to catch up to the trajectory faster. In the worst case this will cause the marker to overshoot its desired position or request the robot wheels to spin faster than physically possible. Decreasing the constant causes the marker to catch up to the trajectory slower. Lagging behind too far may cause the controller to again, request more power than is physically possible from the motors.
  5. The controller described above is going to lag behind the desired trajectory by some amount. This can be reduced by either fine tuning the constants or looking ahead in the trajectory.


  1. Error score (out of 80 points) :
      i) Mean error > 5 cm - lscore points rounded to the nearest integer
      ii) Mean error 5 cm - 80 points
  2. Trajectory quality (subjective and determined by the TA) - out of 20 points

Grading Sheet: Lab 4

Keywords: Trajectory, Non - linear control

Last Updated : 10/16/07 by Kaushik Viswanathan
(c) 1999-2007: Howie Choset, Carnegie Mellon