Problem 237799 · medium · Robotics · Level 02 Motion

Dead Reckoning in the Orchard

odometry · wheel encoder · exact arc · dead reckoning · angle wrapping

An orchard robot has no GPS under the trees. It keeps track of where it is by odometry: adding up how far its wheels have rolled. Every few milliseconds its two wheel encoders report how many ticks they counted since the previous report; readings is the list of these reports (left_ticks, right_ticks), in order. A wheel turning backwards gives a negative count.

The robot is a differential drive: wheels of radius r metres, L metres apart, ticks_per_rev ticks per wheel revolution. A pose is (x, y, theta): position in metres (x to the right, y up) and heading in radians, counter-clockwise from +x. For each reading, in order:

  1. Turn the counts into the wheels' distances d_l and d_r (one revolution rolls a wheel 2\pi r metres), then d = (d_r + d_l)/2 and \Delta\theta = (d_r - d_l)/L.
  2. Move the position along the path the robot really took if both wheels turned at steady speeds during the reading: a straight segment of length d along the current heading when \Delta\theta = 0 (equal counts), otherwise an arc of a circle of length d that starts along the current heading and turns it by \Delta\theta. Compute the end of that arc exactly; do not approximate it.
  3. Add \Delta\theta to the heading.

Write odometry(start, readings, ticks_per_rev, r, L) that returns the final pose estimate (x, y, theta), with theta wrapped into (-\pi, \pi].

Examples

Input:  start = (0, 0, 0), readings = [(500, 500), (250, 750), (-200, 200)],
        ticks_per_rev = 500, r = 0.04, L = 0.3
Output: (0.47427086, 0.09926082, 1.50796447)
Explanation: one revolution straight ahead (0.2513 m), then an arc of 0.2513 m
turning by 0.8378 rad to the left, then a turn on the spot by 0.6702 rad.

Input:  start = (1.0, 1.0, 1.5707963267948966), readings = [(1000, 600)],
        ticks_per_rev = 400, r = 0.05, L = 0.4
Output: (1.23431458, 1.56568542, 0.78539816)
Explanation: d = 0.6283 m and dtheta = -0.7854 rad: an eighth of a clockwise circle
of radius 0.8 m.

Constraints

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

Goals

  • Integrate a stream of encoder readings into a pose estimate
  • Use the exact arc update for each reading, with the straight case handled on its own
  • Keep the heading as a running total and wrap it once at the end
Starting Python…