Problem 216647 · hard · Robotics · Level 02 Motion

A Millimetre Off, a Metre Lost

odometry · calibration · drift · exact arc · angle wrapping

A floor-scrubbing robot was configured with a wheel radius r_est and a wheel separation L_est that are slightly wrong: the real values are r_true and L_true (all in metres). Its encoders are fine, so the true motion and the robot's own estimate come from the same readings, worked out with different numbers. Your job is to measure the damage.

readings is the list of encoder reports (left_ticks, right_ticks), each the ticks counted since the previous report (ticks_per_rev per wheel revolution, negative when a wheel turns backwards). With a wheel radius r and separation L, a report gives wheel distances d_l, d_r (one revolution rolls 2\pi r metres), then d = (d_r + d_l)/2 and \Delta\theta = (d_r - d_l)/L. A pose (x, y, \theta) (x right, y up, \theta in radians counter-clockwise from +x) is updated by moving along the exact arc of length d that starts along the current heading and turns it by \Delta\theta (a straight segment when \Delta\theta = 0, a turn on the spot when d = 0), then adding \Delta\theta to the heading.

Both the true pose and the estimate start at (0, 0, 0). After each report, compare the two positions by their straight-line distance (the gap). Write drift(readings, ticks_per_rev, r_true, L_true, r_est, L_est, tol) that returns the tuple (first, worst, heading_error):

  • first: the index (counting from 0) of the first report after which the gap is greater than tol, or None if it never is;
  • worst: the largest gap after any report (0.0 with no reports);
  • heading_error: the estimated heading minus the true heading after the last report, wrapped into (-\pi, \pi].

Press Run with plot(...) on the two lists of positions to watch the paths come apart.

Examples

Input:  readings = [(400, 400), (-300, 300)] * 4, ticks_per_rev = 400,
        r_true = 0.05, L_true = 0.3, r_est = 0.05, L_est = 0.32, tol = 0.02
Output: (2, 0.09126441, -0.39269908)
Explanation: a square of 0.314 m sides with four quarter turns to the left. The robot
thinks it is wider than it is, so each turn looks like 0.3/0.32 of a quarter turn:
its third report already ends more than 2 cm off, and it believes it turned
pi/8 less than it did.

Input:  readings = [(400, 400), (-300, 300)] * 4, ticks_per_rev = 400,
        r_true = 0.05, L_true = 0.3, r_est = 0.052, L_est = 0.3, tol = 0.02
Output: (4, 0.05617468, 0.25132741)

Input:  readings = [(50, 50)] * 40, ticks_per_rev = 400,
        r_true = 0.05, L_true = 0.3, r_est = 0.05, L_est = 0.32, tol = 0.02
Output: (None, 0.0, 0.0)
Explanation: on a straight run the wheel separation plays no part.

Constraints

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

Goals

  • Run the same encoder readings through two robot models side by side
  • Measure how a wrong wheel radius or wheel separation makes the estimate drift from the truth
  • See which mistakes hurt on straight runs and which only on turns
Starting Python…