Simultaneous maximum-likelihood calibration of robot and sensor parameters Andrea Censi, Luca Marchionni, Giuseppe Oriolo
Differential-drive kinematics driftless affine system: q x ( t ) v ( t ) cos q θ ( t ) d = q y ( t ) v ( t ) sin q θ ( t ) linear in the input, dt q θ ( t ) ω ( t ) almost linear in parameters ( ) ( ) � v ( t ) � � ω L ( t ) � = J ω ( t ) ω R ( t ) transformation depends on the odometry » – » – J 11 J 12 + r L / 2 + r R / 2 J = = J 21 J 22 − r L /b + r R /b q k robot pose (world frame) R L T linear and angular velocities v ( t ) , ω ( t ) s J wheels velocities (available) ω R ( t ) , ω L ( t ) v ( t ) , ω ( t ) wheels radii r R , r L 3 parameters R b wheelbase b
...and a range-finder on top. pose of range finder in robot frame (3 parameters) robot motion sensor motion (estimable through scan-matching) composition (group operation in SE(2))
Algorithm overview • Input: – wheel velocities – range-finder readings • Overview: 1. Drive the robot along any trajectory. 2. Pre-process range readings using scan - matching to obtain estimate of displacements. 3. Based on estimate of rotation, find two elements of J using linear least squares . 4. Solve a constrained quadratic minimization problem to find the other 4 parameters. 5. Detect outliers ; repeat.
Finding the first two parameters • Assume wheel velocities are constant. – Engineering decision for easy implementation; we provide formulas for the general case. • Two elements of J can be found using linear least squares. can ignore for orientation ℓ ⊕ s k = o k ⊕ ℓ ( J 21 ω L + J 22 ω R ) T k = s k θ • Collect all measurements: . . . . . . � J 21 . . . � ω k L T k ω k R T k s k ˆ ˆ = ˆ + errors θ J 22 . . . . . . . . .
Finding the other 4 parameters • Define the unknown vector: � T � b cos ℓ θ sin ℓ θ x = ℓ x ℓ y • Then, we show the ML is equivalent to the following constrained quadratic minimization problem: min x T M x subject to x 2 4 + x 2 5 = 1 x 1 > 0 This is solvable in closed form. • Nonlinearity makes it hard to estimate uncertainty.
Dealing with outliers
Some related work • This precise problem has never been tackled in literature. • The availability of measurements of small motions allows for much simpler math wrt literature. Comparison with Antonelli et al [2003] • Uses 4 independent numbers for J. – problem becomes completely linear • Uses full trajectories. Other “classic” approaches • Assumption: measures are expensive; assume : : one wants to measure only endpoints. • Borenstein [1996] has only 2 DOF. • Kelly [2004] provides the solution for generic parameters and trajectories under ? ? linearization.
EKF for calibration (SLAC) • The EKF can be used to calibrate robot and sensor parameters. • Idea: define extended state with robot pose and parameters. • However: – no easy outlier detection – issues with convergence/consistency – hard to characterize statistical properties • Use filtering only when needed.
Summary • Method properties: – Model-based, closed-form ML estimate without approximations/linearization. – Very practical: • Trajectories can be freely chosen. • No need for external sensors. • No need for nominal parameters. • Tips learned: – Use physical parameters and simple methods (ML)! – Considering small parts of a trajectory leads to easy math. – Minimization problems in SE(2) often have a closed- form solution. • Software and logs available at my website.
TODO • Currently working on: – Characterizing the uncertainty of the estimate. – Generation of optimal calibration trajectories. – How does the bias on measurements influence the estimates?
Comparison with Roy & Thrun • Uses model-free approach:
Roy & Thrun
More formally • Divide the trajectory in small intervals delimited by two range readings. • Assume constant wheel velocities over interval. – we provide formulas for the general case, but this approximation leads to a simple • Obtain list of measurements tuple: • Given the relation Obtain estimate by minimizing the following:
Hokuyo
Recommend
More recommend
