Problem 687942 · medium · Robotics · Level 06 Estimation and Autonomy

The Campus Shuttle Steers by Its Error

PID control · heading error · waypoint following · simulation · angle wrapping · saturation

A small campus shuttle drives at a constant forward speed v (m/s) and steers by choosing its turning rate \omega (rad/s, positive counter-clockwise). It visits the points of waypoints in order, and a PID controller on the heading error chooses \omega.

The pose is (x, y, theta) (metres; theta in radians counter-clockwise from +x). gains = (kp, ki, kd). The controller keeps a running sum I (starting at 0) and the previous error (none at first). The simulation runs in steps of dt seconds. At each position, starting with pose, in this order:

  1. Arrive. While the current waypoint is within 0.25 m of the shuttle (distance <= 0.25), move on to the next waypoint (several may be passed at once).
  2. Stop if every waypoint has been reached, or if steps steps have already been taken.
  3. Error. e = the direction from the shuttle to the current waypoint (atan2) minus theta, wrapped to (-\pi, \pi] (add or subtract 2\pi; exactly -\pi becomes \pi).
  4. Control. I = I + e * dt; D = (e - previous error) / dt, or 0 if there is no previous error yet; remember e as the previous error. \omega = k_p e + k_i I + k_d D, then limited to [-1, 1] rad/s. Nothing is reset when the waypoint changes.
  5. Move (one Euler step, position with the old heading): x += v * cos(theta) * dt, y += v * sin(theta) * dt, then theta = theta + omega * dt, wrapped to (-\pi, \pi].

Write follow_waypoints(pose, waypoints, gains, v, dt, steps) that returns (x, y, theta, reached, taken): the final pose, the number of waypoints reached and the number of steps taken. Collect the positions and press Run with plot(xs, ys) to see the route.

Examples

Input:  pose = (0, 0, 0), waypoints = [(1.02, 0)], gains = (1, 0, 0), v = 0.5, dt = 0.1, steps = 100
Output: (0.8000000000000002, 0.0, 0.0, 1, 16)
Explanation: 0.05 m per step straight ahead; at x = 0.75 the waypoint is still 0.27 m
away, at x = 0.8 it is 0.22 m away and counts as reached.

Input:  pose = (0, 0, 0), waypoints = [(2, 0), (2, 2)], gains = (2, 0, 0.1), v = 0.5, dt = 0.1, steps = 200
Output: (2.064671372621046, 1.7649311952276134, 1.8276718581382891, 2, 77)

Input:  pose = (0, 0, math.pi), waypoints = [(3, 0)], gains = (1.5, 0, 0), v = 0.4, dt = 0.1, steps = 200
Output: (2.798224854254143, -0.06115595880402403, 0.2942360286507172, 1, 105)
Explanation: the waypoint is straight behind: the first error is 0 - pi = -pi, which wraps
to +pi, so the shuttle turns counter-clockwise (through -y) to come round.

Constraints

  • answers are compared with a tolerance of 1e-6

Goals

  • Steer a unicycle robot toward waypoints with a PID controller on the wrapped heading error
  • Simulate the loop with a fixed order of updates, a turn-rate limit and a waypoint acceptance radius
Starting Python…