Simultaneous maximum-likelihood calibration of robot and sensor - - PowerPoint PPT Presentation

Simultaneous maximum-likelihood calibration of robot and sensor parameters

Andrea Censi, Luca Marchionni, Giuseppe Oriolo

Differential-drive kinematics

s ωR(t), ωL(t) v(t), ω(t)

R L

v(t), ω(t) J J = » J11 J12 J21 J22 – = » +rL/2 +rR/2 −rL/b +rR/b – qk T

rR, rL b

R

b

d dt   qx(t) qy(t) qθ(t)   =   v(t) cos qθ(t) v(t) sin qθ(t) ω(t)   ( ) ( ) v(t) ω(t)

• = J

ωL(t) ωR(t)

• transformation depends on the odometry

robot pose (world frame) linear and angular velocities wheels velocities (available) wheels radii wheelbase 3 parameters driftless affine system: linear in the input, almost linear in parameters

...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.

• Collect all measurements:

(J21ωL + J22ωR) T k = sk

θ

    . . . . . . ˆ ωk

L T k

ˆ ωk

R T k

. . . . . .     J21 J22

• =

    . . . ˆ sk

θ

. . .     + errors ℓ ⊕ sk = ok ⊕ ℓ can ignore for orientation

Finding the other 4 parameters

• Define the unknown vector:
• Then, we show the ML is equivalent to the following

constrained quadratic minimization problem: This is solvable in closed form.

• Nonlinearity makes it hard to estimate uncertainty.

x =

• b

ℓx ℓy cos ℓθ sin ℓθ T min xT Mx subject to x2

4 + x2 5 = 1

x1 > 0

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
• ne 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