1

Formulation and Calibration of Fast, Accurate Vehicle Motion Models

Thesis proposalNeal Seegmiller

November 30, 2012

2

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)

3

Prior work on Model Predictive Planning and Control

4

Prior work on MPC & MPP cont.

*where does the model come from?

5

WMR motion models must be accurate

• MPP requires accurate models to plan safe, feasible paths– Sometimes relying on feedback isn’t good enough!

plannedactual

6

Fukushima nuclear power plant (models must be 3D)

7

WMR motion models must be fast

Receding horizon motion planning [Howard 2009] Lattice motion planning

[Pivtoraiko 2009]

8

Challenges to model generation

1. Tradeoff between fidelity and speed in model formulation

2. Difficulty of Calibration

9

Challenge 1. Fidelity/speed tradeoffAdams/Car ROAMS [Jain 2003]CarSim

2D Dubins carhttp://planning.cs.uiuc.edu/node788.html

High fidelity/slow Low fidelity/fast

10

General approaches to formulating models

A general approach by [Tarokh & McDermott 2005].

Extends the 2D approach by [Muir & Neuman 1986]

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

11

Challenge 2. Difficulty of calibration

1. Analyze subsystems in isolationSingle-wheel testbed [Ishigami 2007]

2. Execute preprogrammed maneuversUMBmark [Borenstein 1996]

3. Self-calibrate during normal operationFast and easy odometery calibration[Kelly 2004]. See also [Antonelli 2005], [Martinelli 2007]

12

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.

Can be met by enhanced 3D velocity kinematic models.

My hypothesis:

Can be met by IEE calibration method

13

Outline

• Problem Definition• Progress to Date– Formulation– Calibration– How I plan to address limitations

• Research Plan• Contributions to Robotics

14

A Vector algebra-based approach to velocity kinematics

See [Kelly 2012] FSR paperBased on [Luh 1980] Recursive Newton-Euler Algorithm for manipulators

of frame m

wrt frame f

(f)ixed(m)oving

(o)bject

r: positionv: velocityω: angular velocity

Notation

The Transport Theorem:Applies to position and its derivatives (linear velocity, etc.)

15

The wheel equation (offset steering example)

Velocity of wheel/terrain contact point

Linear vehicle velocity

Steering rateAngular vehicle velocity

Dimensions

Frames:(w)orld(v)ehicle

sc

x

yv

w

(s)teer(c)ontact

16

Inverse and Forward KinematicsInverse Kinematics•Use wheel equations directly!•Steer angle should align forward axis of wheel frame with velocity vector

c

(not aligned)

Vehicle frame velocity

Use skew-symmetric matrices to represent cross products

(4 wheel offset steering example)

Forward Kinematics•Rearrange wheel equations into a linear system•Solve for vehicle velocity using pseudoinverse

17

Video: 4 wheel offset steering example

18

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.

19

3D example, the Zoë rover

2 passive DOF for each axle. 4 independently driven wheels

20

Zoë ramp experiment

2.5°

Similar accuracy to dynamics simulation but computationally cheaper!

Dynamics Simulation(2nd order)

Kinematics Simulation(1st order)

Physical Experiment

21

Formulation Limitations

• Not modular• How best to solve DAE?• Only no-slip constraints supported• Speed comparison not possible yet

22

Integrated Equation Error (IEE) approach to Model Identification

System IntegralSystem Differential Equation

state inputs parameters

vs.

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

23

Linearized systematic & stochastic error dynamics

observations parameters

systematic

stochastic

t0

t

[Stengel 1994]

24

xy

IEE applied to vehicle model identification

See [Seegmiller 2011] ISRR paper

The differential equation Body frame velocity consists of (n)ominal and (s)lip components

Slip is parameterized over nominal velocity, centripetal acceleration, gravity (in C)

Suspension deflections are ignoredMotion is restricted to a tangent plane

25

Crusher at Camp Roberts

Image captured by one of Crusher’s camerasCrusher traversed steep grassy slopes and a dirt road

Crusher6 wheel skid-steer, active suspension

Path at Camp RobertsRoll: -28 to 29°Pitch: -22 to 17°Top speed: 6 m/s, 4 rad/s (commanded)

26

Crusher at Camp Roberts

Predicted path with slip calibrationPrediction uncertainty (1σ, .683)Actual path (GPS)Predicted path using basic kinematic model

W (track width)

VRVL

x

y

assumes no slip!

27

Crusher at Camp Roberts, Systematic Calibration

28

Crusher at Camp Roberts, Stochastic Calibration

29

Calibrating powertrain dynamics using IEE

angular acceleration

(commanded)angular velocity

time delaytime constant

LandTamer

See [Seegmiller 2012] ICRA paper

30

Calibration Method Limitations

• Currently the method only supports:– Simplified models– Slip parameterization about body frame inputs

• Insufficient experimental validation

31

Related work on modeling wheel slip

3. Rigid Wheel on deformable terrainUse a terramechanics-based model [Ishigami 2007]

2. Deformable wheel on rigid terrainUse an empirical model [Salaani 2007]

1. Rigid wheel on rigid terrainUse Coulomb friction model

32

How to compute wheel reaction forces?

[Iagnemma 2001]

Use the force balance equationunderdetermined for > 2 wheels![Hung 1999] suggests choosing an objective function andformulating as a Linear or Quadratic Programming problem

6 x 3n

6 x 1

33

Related work on stability margins

Predict rollover[Diaz-Calderon 2005] based on[Papadopoulos & Rey 1996]

Predict loss of traction[Brach 2009]

34

Related work on Rigid Body Dynamics

Multilegged vehicles [McMillan & Orin 1998][Featherstone 1987]

• 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

35

Outline

• Problem Definition• Progress to Date• Research Plan– Theoretical objectives– Experimental objectives– Schedule

• Contributions to Robotics

36

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

37

Research Plan, Experimental objectives

• Quantitative comparison my proposed models with alternatives (accuracy and speed)

• Quantitative comparison of slip models• Experimental verification of adaptability

38

Research Plan, ResourcesSoftware resources for physics-based simulation:

Open Dynamics Engine CarSim

Zoë rover

Available platforms:

MiniCrusher

39

Proposed Schedule

1.0 Theory and Software Infrastructure

1.1 C/C++ library for constructing & simulating first-order WMR DAE models1.2 Implement/evaluate multiple slip models1.3 Integrate IEE calibration1.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 alternatives2.4 Compare accuracy & speed with alternative models

3.0 Documentation and Publication

3.1 Write Dissertation

Q2 Q3 Q4WBS Task Name

Year 1 Year 2Q1 Q2 Q3 Q4 Q1

40

Contributions to RoboticsWhat 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.

41

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)