Formulation and Calibration of Fast,
Accurate Vehicle Motion Models
Thesis proposal
Neal Seegmiller
November 30, 2012
1
Outline
• Problem Definition
– How should we produce motion models for MPP?
• Progress to Date
• Research Plan
• Contributions to Robotics
Terms:
•motion model
•WMR (wheeled mobile robot)
•model predictive control (MPC)
•model predictive planning (MPP)
2
Prior work on Model Predictive
Planning and Control
3
Prior work on MPC & MPP cont.
*where does the model come from?
4
WMR motion models must be accurate
• MPP requires accurate models to plan safe, feasible paths
– Sometimes relying on feedback isn’t good enough!
planned
actual
5
Fukushima nuclear power plant
(models must be 3D)
6
WMR motion models must be fast
Receding horizon motion planning
[Howard 2009]
Lattice motion planning
[Pivtoraiko 2009]
7
Challenges to model generation
1. Tradeoff between fidelity and speed in model
formulation
2. Difficulty of Calibration
8
Challenge 1. Fidelity/speed tradeoff
CarSim
Adams/Car
ROAMS [Jain 2003]
High fidelity/slow
Low fidelity/fast
2D Dubins car
http://planning.cs.uiuc.edu/node788.html
9
General approaches to formulating models
Related work on planning with moderate to high-fidelity models:
•[Ishigami 2011] complete dynamic simulation of planetary rovers
•[Iagnemma 2001] A* planning for planetary rovers
•[Yu 2010] Skid-steered vehicles
•[Howard 2009] multiple platforms
A general approach by
[Tarokh & McDermott 2005].
Extends the 2D approach by
[Muir & Neuman 1986]
10
Challenge 2. Difficulty of calibration
1. Analyze subsystems in isolation
Single-wheel testbed [Ishigami 2007]
2. Execute preprogrammed maneuvers
UMBmark [Borenstein 1996]
3. Self-calibrate during normal operation
Fast and easy odometery calibration
[Kelly 2004]. See also [Antonelli 2005],
[Martinelli 2007]
11
What is desired?
Formulation requirements:
•Account for 3D articulation (in a modular way)
•Account for wheel slip
•Account for powertrain dynamics
•Predict the onset of extreme conditions
•Be capable of simulation 100x real time (an
order of magnitude faster than SOA)
Calibration requirements:
•Run online during normal operation
•Require only intermittent observations of pose
•Be computationally tractable
•Learn a model of non-systematic error
(uncertainty)
•Adapt quickly without overfitting.
My hypothesis:
Can be met by enhanced 3D
velocity kinematic models.
Can be met by IEE
calibration method
12
Outline
• Problem Definition
• Progress to Date
– Formulation
– Calibration
– How I plan to address limitations
• Research Plan
• Contributions to Robotics
13
A Vector algebra-based approach to
velocity kinematics
Applies to position and
its derivatives (linear
velocity, etc.)
The Transport Theorem:
Notation
wrt frame f
(o)bject
(m)oving
r: position
of frame m
v: velocity
ω: angular velocity
(f)ixed
See [Kelly 2012] FSR paper
Based on [Luh 1980] Recursive Newton-Euler Algorithm for manipulators
14
The wheel equation
(offset steering example)
c
s
Velocity of wheel/terrain contact point
v
x
Linear vehicle velocity
Dimensions
y
w
Angular vehicle velocity
Frames:
(w)orld
(v)ehicle
Steering rate
(s)teer
(c)ontact
15
Inverse and Forward Kinematics
Inverse Kinematics
•Use wheel equations directly!
•Steer angle should align forward axis of wheel frame
with velocity vector
(not aligned)
c
Forward Kinematics
•Rearrange wheel equations into a linear system
•Solve for vehicle velocity using pseudoinverse
Use skew-symmetric matrices
to represent cross products
Vehicle frame velocity
(4 wheel offset steering example)
16
Video: 4 wheel offset steering example
17
Formulating 3D motion prediction as
the solution of a DAE
Semi-explicit DAE consists of ODE + constraints:
(constraints are provided by the wheel equations!)
The ODE. x: state, u: inputs
Holonomic constraints, e.g. terrain following
Non-holonomic constraints, e.g. no wheel slip
Unconstrained integration +
non linear optimization
Solve directly for constrained
motion using DAE
vs.
18
3D example, the Zoë rover
2 passive DOF for each axle. 4 independently driven wheels
19
Zoë ramp experiment
Similar accuracy to
dynamics simulation but
computationally cheaper!
Physical Experiment
Dynamics Simulation
(2nd order)
Kinematics Simulation
(1st order)
2.5°
20
Formulation Limitations
•
•
•
•
Not modular
How best to solve DAE?
Only no-slip constraints supported
Speed comparison not possible yet
21
Integrated Equation Error (IEE)
approach to Model Identification
System Differential Equation
System Integral
vs.
state inputs parameters
IEE pro’s
•No numerical differentiation
•Compounded errors (a good thing!)
•Optimize for chosen horizon directly
IEE con’s
•More computation (but tractable)
•Must linearize an integral to compute Jacobian
•Tricky to account for measurement uncertainty
22
Linearized systematic & stochastic
error dynamics
observations
parameters
[Stengel 1994]
systematic
t
stochastic
t0
23
IEE applied to vehicle model
identification
Suspension deflections are ignored
Motion is restricted to a tangent plane
y
x
The differential equation
Body frame velocity consists of
(n)ominal and (s)lip components
See [Seegmiller 2011] ISRR paper
Slip is parameterized over
nominal velocity, centripetal
acceleration, gravity (in C)
24
Crusher at Camp Roberts
Crusher
6 wheel skid-steer, active suspension
Path at Camp Roberts
Roll: -28 to 29°
Pitch: -22 to 17°
Top speed: 6 m/s, 4 rad/s (commanded)
Image captured by one of
Crusher’s cameras
Crusher traversed steep grassy
slopes and a dirt road
25
Crusher at Camp Roberts
Predicted path with slip calibration
Prediction uncertainty (1σ, .683)
Actual path (GPS)
Predicted path using basic kinematic model
VR
VL
x
y
assumes no slip!
W (track width)
26
Crusher at Camp Roberts,
Systematic Calibration
27
Crusher at Camp Roberts,
Stochastic Calibration
28
Calibrating powertrain dynamics using IEE
angular
acceleration
(commanded)
angular velocity
time constant
time delay
See [Seegmiller 2012] ICRA paper
LandTamer
29
Calibration Method Limitations
• Currently the method only supports:
– Simplified models
– Slip parameterization about body frame inputs
• Insufficient experimental validation
30
Related work on modeling wheel slip
1. Rigid wheel on rigid terrain
Use Coulomb friction model
2. Deformable wheel on rigid terrain
Use an empirical model [Salaani 2007]
3. Rigid Wheel on deformable terrain
Use a terramechanics-based model
[Ishigami 2007]
31
How to compute wheel reaction forces?
6 x 3n
6x1
[Iagnemma 2001]
Use the force balance equation
underdetermined for > 2 wheels!
[Hung 1999] suggests choosing an objective function and
formulating as a Linear or Quadratic Programming problem
32
Related work on stability margins
Predict rollover
[Diaz-Calderon 2005] based on
[Papadopoulos & Rey 1996]
Predict loss of traction
[Brach 2009]
33
Related work on Rigid Body Dynamics
[Featherstone 1987]
Multilegged vehicles
[McMillan & Orin 1998]
• The most computationally efficient rigid body dynamics algorithms
were developed by roboticists
• Inverse vs. forward dynamics
• Maximal vs. generalized coordinates
• Various ways to handle contact between the wheels & ground
34
Outline
• Problem Definition
• Progress to Date
• Research Plan
– Theoretical objectives
– Experimental objectives
– Schedule
• Contributions to Robotics
35
Research Plan, Theoretical objectives
•
•
•
•
•
Formulate motion prediction as the solution of a DAE in a modular way
Formulate and enforce slip constraints
Incorporate a powertrain dynamics model
Incorporate an extreme conditions predictor
Calibrate enhanced 3D kinematic models using the IEE approach
Deliverables to make my approach accessible:
• Software library
• Documentation of step-by-step approach
36
Research Plan, Experimental objectives
• Quantitative comparison my proposed models with alternatives
(accuracy and speed)
• Quantitative comparison of slip models
• Experimental verification of adaptability
37
Research Plan, Resources
Software resources for physics-based simulation:
Open Dynamics Engine
CarSim
Available platforms:
Zoë rover
MiniCrusher
38
Proposed Schedule
WBS Task Name
Year 1
Year 2
Q1 Q2 Q3 Q4 Q1 Q2 Q3 Q4
1.0
Theory and Software Infrastructure
1.1
C/C++ library for constructing & simulating first-order WMR DAE models
1.2
Implement/evaluate multiple slip models
1.3
Integrate IEE calibration
1.4
Implement prediction of extreme conditions
2.0
Experimental Evaluation
2.1
Obtain experimental data (articulated rover, high speed WMR)
2.2
Calibrate models to experimental data using IEE, evaluate.
2.3
Compare accuracy & speed of slip model alternatives
2.4
Compare accuracy & speed with alternative models
3.0
Documentation and Publication
3.1
Write Dissertation
39
Contributions to Robotics
What is novel?
• An automated, modular approach to simulating WMR velocity kinematics
(compare to [Tarokh & McDermott 2005])
• Enhanced models: wheel slip, powertrain dynamics, extreme conditions
• Convenient self-calibration method
• Analysis of the trade offs between kinematic vs. dynamic models
Why it’s useful:
• Broadly applicable, works for any WMR design
• Readily accessible (software library, documentation)
• A foundation for future research in planning, mobile manipulation, etc.
40
References
Antonelli, G., et al.: A calibration method for odometry of mobile robots based on the least-squares technique. IEEE Trans. Robot. 21(5), 994-1004 (2005)
Borenstein, J., Feng, L.: Measurement and correction of systematic odometry errors in mobile robots. IEEE Trans. Robot. Autom. 12(6), 869-880 (1996)
Brach, R.M., Brach, R.M.: Tire models for vehicle dynamic simulation and accident reconstruction. SAE Technical Paper 2009-01-0102 (2009)
Diaz-Calderon, A., Kelly, A.: On-line stability margin and attitude estimation for dynamic articulating mobile robots. IJRR 24(10) (2005)
Featherstone, R.: Robot dynamics algorithms. Kluwer, Boston/Dordrecht/Lancaster (1987)
Howard, T.: Adaptive model-predictive motion planning for navigation in complex environments. Tech. Report, CMU-RI-TR-09-32 (2009)
Iagnemma, K.: Rough-terrain mobile robot planning and control with application to planetary exploration. MIT Ph.D. Thesis (2001)
Ishigami, G., et al.: Terramechanics-based model for steering maneuver of planetary exploration rovers on loose soil. JFR 24(3), 233-250 (2007)
Ishigami, G., et al.: Path planning and evaluation for planetary rovers based on dynamic mobility index. ICRA (2011)
Jain, A., et al.: ROAMS: Planetary surface rover simulation environment. i-SAIRAS (2003)
Kelly, A.: Fast and easy systematic and stochastic odometry calibration. ICRA (2004)
Kelly, A., Seegmiller, N.: A vector algebra formulation of mobile robot velocity kinematics. FSR (2012)
Luh, J., Walker, M., Paul, R.: On-line computational scheme for mechanical manipulators. J. Dyn. Sys., Meas., Control 102(2), 69-76 (1980)
Martinelli, A., et al.: Simultaneous localization and odometry self calibration for mobile robot. Autonomous Robots 22(1), 75-85 (2007)
McMillan, S., Orin, D.: Forward dynamics of multilegged vehicles using the composite rigid body method. ICRA (1998)
Muir, P., Neuman, C.: Kinematic modeling of wheeled mobile robots. Tech. Report, CMU-RI-TR-86-12 (1986)
Pivtoraiko, M., et al.: Differentially constrained mobile robot motion planning in state lattices. JFR 26(1), 308-333 (2009)
Salaani, M.K.: Analytical tire forces and moments model with validated data. SAE World Congress 2007-01-0816 (2007)
Seegmiller, N., et al.: A unified perturbative dynamics approach to vehicle model identification. ISRR (2011)
Seegmiller, N., et al.: Online calibration of vehicle powertrain and pose estimation parameters using integrated dynamics. ICRA (2012)
Stengel, R.: Optimal Control and Estimation, Dover, New York (1994)
Tarokh, M., McDermott, G.: Kinematics modeling and analyses of articulated rovers. IEEE Trans. Robot. 21(4), 539-553 (2005)
Yu, W., et al.: Analysis and experimental verification for dynamic modeling of a skid-steered wheeled vehicle. IEEE Trans. Robot. 26(2), 340-353 (2010)
41