modeling mobile robots -...

Modeling Mobile Robots … for High Speed and Off Road

… autonomous and supervisory control

Alonzo Kelly Professor

Robotics Institute Carnegie Mellon University

1 Modeling Mobile Robots 9/16/2014

Outline

• Autonomy • WMR Models • Applications • Conclusion

9/16/2014 Modeling Mobile Robots 2

Outline

• Autonomy – Computation – Architecture

• WMR Models • Applications • Conclusion

9/16/2014 Modeling Mobile Robots 3

Computations

9/16/2014 Modeling Mobile Robots 4

Symbolic Logical Search

Sequential Deliberative

Abstract

Policy

Strategic

Control

Physical

Tactical

Spat-Temp Arithmetic Repetitive Parallel Reactive Concrete

objectives

goals status

set points states

cmds feedback

• Upper levels: – Symbols – Graphs – Propositions – Concepts

• Lower levels: – Signals – Fields – Vectors

Outline

• Autonomy – Computation – Architecture

• WMR Models • Applications • Conclusion

9/16/2014 Modeling Mobile Robots 5

Autonomy in 5 Layers

• Nested control loops. – Commands, state, and

models at all levels.

• Processing Levels – Supervise = … – Deliberate = decide – Perceive = see – React = …

Global W Model

Local W Model

Deliberative Planning &

Control

Perceptive Planning &

Control

Platform State

Reactive Planning &

Control

Vehicle Actuators

Proprioception Sensors

Perception Sensors

Prior Data

Reactive Autonomy

Perceptive Autonomy

Deliberative Autonomy

Hardware Platform

Situation & World Model

Task Level Supervision

Supervised Autonomy

State Estimation

Local Processing

Global Processing

Human Awareness

9/16/2014 Modeling Mobile Robots 6

Outline

• Autonomy • WMR Models

– Motivation – Nature – Formulation – Calibration

• Applications • Conclusion

9/16/2014 Modeling Mobile Robots 7

Need Fast, Accurate Models • Need correct predictions

for: – Estimation – Control – Planning – Human interfaces

• Need 1000X faster than real time (with 1% CPU). – 10 X a second – simulate 10 seconds

motion – for 10 trajectories.

8

Trying to avoid the obstacle On left side at high speed

will cause a collision

9/16/2014 Modeling Mobile Robots

Outline

• Autonomy • WMR Models

– Motivation – Nature – Formulation – Calibration

• Applications • Conclusion

9/16/2014 Modeling Mobile Robots 9

WMR Models : Nature

• Differential: • Underactuated: • OverConstrained:

9/16/2014 Modeling Mobile Robots 10

Manipulator

WMR

x c 123L3 c12L2 c1L1+ +( )=y s123L3 s 12L2 s1 L1+ +( )=ψ ψ1 ψ2 ψ3+ +=

tdd x t( )

y t( )ζ t( )

ζcos t( ) ζsin t( )– 0ζsin t( ) ζcos t( ) 00 0 1

Vx t( )

Vy t( )

ζ· t( )

=

�̇�𝒙 = 𝒇𝒇(𝒙𝒙,𝒖𝒖, 𝒕𝒕)

𝒙𝒙 𝝐𝝐 ℜ𝒏𝒏 𝒖𝒖 𝝐𝝐 ℜ𝒎𝒎

𝒗𝒗𝒘𝒘 ∙ 𝒚𝒚� = 𝟎𝟎

𝒛𝒛𝒘𝒘 = 𝒛𝒛𝒕𝒕𝒕𝒕𝒕𝒕𝒕𝒕𝒕𝒕𝒕𝒕𝒏𝒏(x,y)

Implications

• IK does not exist in closed form. – Best case is Fresnel

integrals. – Requires a numerical

approach • Solution does not

exist at all for arbitrary trajectories. – Only some motions

are feasible. 9/16/2014 Modeling Mobile Robots 11

Manipulator

WMR

ψ2k1

2 k22+( ) L2

2 L12+( )–

2L2 L1----------------------------------------------------acos=

Outline • Autonomy • WMR Models

– Motivation – Nature – Formulation

Kinematics DAEs Constraints

– Calibration • Applications • Conclusion

9/16/2014 Modeling Mobile Robots 12

Enabling Kinematics - Transport Theorem

• Basic mechanism to convert measurements from moving (robot) frame to fixed (world) frame.

13

f

m

o

object 𝑟𝑟𝑚𝑚𝑓𝑓

𝑟𝑟𝑜𝑜𝑚𝑚 𝜔𝜔 �⃑�𝑣𝑜𝑜𝑚𝑚

𝑟𝑟𝑚𝑚𝑓𝑓

r = position v =velocity ω= ang vel

of frame m

wrt frame f

Notation

9/16/2014 Modeling Mobile Robots

Wheel Equation

• Vector formulation that relates wheel rotation rates to body linear and angular velocities.

14

dimensions

angular velocity steering

linear velocity

Kelly & Seegmiller, Recursive Kinematic Propagation, to appear IJRR. 9/16/2014 Modeling Mobile Robots

Example: 4 Wheel Steer

15 9/16/2014 Modeling Mobile Robots

Outline • Autonomy • WMR Models

– Motivation – Nature – Formulation

Kinematics DAEs Constraints

– Calibration • Applications • Conclusion

9/16/2014 Modeling Mobile Robots 16

Why DAEs

• Solves for unknown constraint forces/velocities automatically.

• Provides constrained derivatives needed for fast, accurate ODE solvers.

9/16/2014 Modeling Mobile Robots 17

𝑉𝑉 ∙ 𝑑𝑑𝑑𝑑

𝑉𝑉 ∙ 𝑑𝑑𝑑𝑑 ≠ 𝑉𝑉𝑐𝑐𝑜𝑜𝑐𝑐𝑐𝑐 ∙ 𝑑𝑑𝑑𝑑

𝑉𝑉𝑐𝑐𝑜𝑜𝑐𝑐𝑐𝑐 ∙ 𝑑𝑑𝑑𝑑

Notation

9/16/2014 Modeling Mobile Robots 18

�̇�𝒙 = 𝒇𝒇(𝒙𝒙,𝒖𝒖, 𝒕𝒕)

“state” (x,y,θ)

“state derivative” (velocity etc.)

“inputs” (speed, steer)

time (omitted)

𝒙𝒙 = � 𝒇𝒇 𝒙𝒙,𝒖𝒖, 𝒕𝒕 𝒅𝒅𝒕𝒕 𝒕𝒕

𝟎𝟎

• We formulate velocity kinematics for wheeled vehicles as a constrained, first order, differential equation:

• Compare that with Lagrange dynamics:

DAEs

19

𝑐𝑐 (𝑥𝑥, �̇�𝑥,𝑢𝑢) = 0

2nd order ODE

Constraints

𝑐𝑐 (𝑥𝑥,𝑢𝑢) = 0

1st order ODE

Constraints

�̈�𝑥 = 𝑓𝑓 (�̇�𝑥, 𝑥𝑥,𝑢𝑢)

�̇�𝑥 = 𝑓𝑓 (𝑥𝑥,𝑢𝑢)

9/16/2014 Modeling Mobile Robots

DAE Formulation • Constrained ODE:

• Solve for Lagrange Multipliers (or do nullspace projection) at each iteration:

• Then integrate w.r.t. time.

system dynamics

terrain following

wheel no-slip

Kelly & Seegmiller, WMR modelling with DAEs, submitted IJRR.

22

Example of DAE Models

9/16/2014 Modeling Mobile Robots

Outline • Autonomy • WMR Models

– Motivation – Nature – Formulation

Kinematics DAEs Constraints

– Calibration • Applications • Conclusion

9/16/2014 Modeling Mobile Robots 23

Wheel Slip Constraint • Write the wheel equation in contact point

coordinates.

𝒗𝒗𝒄𝒄𝒘𝒘 = 𝑯𝑯𝑽𝑽𝑽𝑽 + 𝑯𝑯�̇�𝜽�̇�𝜽

• Set lateral component to zero.

𝒙𝒙� ∙ 𝒗𝒗𝒄𝒄𝒘𝒘 = 𝟎𝟎

• This is a constraint on V.

24 9/16/2014 Modeling Mobile Robots

Terrain Following Constraint • Disallow wheel motion

along terrain normal.

• Compute the gradient of this by dot product with system Jacobian.

25 9/16/2014 Modeling Mobile Robots

Outline

• Autonomy • WMR Models

– Motivation – Nature – Formulation – Calibration

• Applications • Conclusion

9/16/2014 Modeling Mobile Robots 26

System Identification - Slip

• Model prediction error as an unknown variation (perturbation).

• Form prediction residuals and solve for parameters iteratively in real time.

27

state observation

Measurement update

9/16/2014 Modeling Mobile Robots

Real Time Slip Model Identification

28 9/16/2014 Modeling Mobile Robots

Results at Extremes

9/16/2014 Modeling Mobile Robots 29

Kelly & Seegmiller, Integrated Prediction Error Minimization, IJRR.

Results at Extremes

9/16/2014 Modeling Mobile Robots 30

20° Roll Angle 20° Pitch Angle

System Identification - Mass

• Calibrate parameters (c.g., stiffness) in motion and structural dynamics.

• Use results for adaptive stability control.

9/16/2014 Modeling Mobile Robots 31

Cg Calibration and Adaptive Stability Control

9/16/2014 32 Diaz-Calderon & Kelly, Online Stability Margin …, IJRR

Outline

• Autonomy • WMR Models • Applications

– State Estimation – Trajectory Generation – Motion Planning in Traffic – Motion Planning in General – Remote Control

• Conclusion

9/16/2014 Modeling Mobile Robots 33

State Est: Inertial Navigation

• Performance of Tactical Grade Inertial Nav: – Governed by velocity aiding – Wheel slip corrupts those

measurements.

34

Key R - Position

V – Velocity

Ψ – Orientation (Euler)

f – Non-gravitational

a – acceleration

g – Gravity

ω – Angular rate

δ R – Position error

δ V – Velocity error

δ Ψ – Orientation error

δ f – Accelerometer bias

δ ω – Gyro bias

z – Kalman measurement

Inertial Navigation

R,V,Ψ

ComplementaryKalman Filter

f, ω

z=δV

δR, δV, δΨ δf, δω

g

9/16/2014 Modeling Mobile Robots

INS Results: Performance

• Unaided (free) inertial is not viable at all.

• Odometry + slip is far better than odometry alone.

• IMU + odometry + slip model somewhat better than IMU + odometry. – Azimuth error is the

dominant component and gyro is already excellent.

9/16/2014 Modeling Mobile Robots 36

Calibrating Odometry in 3D

• Calibrate – Kinematics – Slip

• Results after travelling 200 meters + 4 three-point turns – 0.25 m (0.1%) – 2.3° yaw

9/16/2014

Zoe Rover Traversing Ramps Repeatedly

Modeling Mobile Robots 37

Seegmiller and Kelly, Enhanced Kinematic Models, RSS 2014.

Results

9/16/2014 Modeling Mobile Robots 38

Outline • Autonomy • WMR Models • Applications

– State Estimation – Inverting Dynamics – Trajectory Generation – Motion Planning in Traffic – Motion Planning in General – Remote Control

• Conclusion

9/16/2014 Modeling Mobile Robots 39

Inverting Dynamics • The equivalent of inverse

kinematics in a manipulator is…

• Invert a differential equation. Yikes!!

• In general, there is no solution. – For arbitrary trajectory x.

• In practice, you need one anyway.

40 9/16/2014 Modeling Mobile Robots

41

JPL Field Experimentation

Impaired Mobility and Wheel Slip Models (May/June 2007)

Initial State

Goal Target State

Result of Trajectory Generated w/o Model

Trench Developed by Dragging Wheel vx

ωz

9/16/2014 Modeling Mobile Robots

Outline • Autonomy • WMR Models • Applications

– State Estimation – Inverting Dynamics – Trajectory Generation – Motion Planning in Traffic – Motion Planning in General – Remote Control

• Conclusion

9/16/2014 Modeling Mobile Robots 42

Trajectory Gen

• Original motivation was to get a rover/fork truck to a particular terminal pose.

43

( )∫=ft

f dtt0

,,uxfx( )t,,uxfx =

x(t): state u(t): inputs fx specified )(tuSolve for Pallet

Forktruck 9/16/2014 Modeling Mobile Robots

E.g. Polynomial Spirals

• Parameterization:

9/16/2014 Modeling Mobile Robots 44

𝜿𝜿 𝒔𝒔 = 𝒕𝒕 + 𝒃𝒃𝒔𝒔 + 𝒄𝒄𝒔𝒔𝟐𝟐 + 𝒅𝒅𝒔𝒔𝟑𝟑 + 𝒕𝒕𝒔𝒔𝟒𝟒

p3 u(p)

p1 p2 )()( ttp xu →→

Optimal Control

Nonlinear Programming

)(tuu = ),( tpuu =

( )t,,uxfx = ( )t,pfx =

Kelly & Nagy, Parametric Optimal Control, IJRR.

45

Architecture: 3 Loops

Integration Suspension

u(p) xs(t) xp(t)

x(t) u(t)

Input Generation

terrain

Endpoint Prediction

x(p) ∫ft

dt0

System Model

( )t,pfx =

Parameter Update

( ) ( )pΔxp

pΔxp ff

1−

∂

∂−=∆

Front View

Overhead View

Initial

Final

9/16/2014 Modeling Mobile Robots

46

Convergence & Solution

Howard & Kelly, Rough Terrain Trajectory Generation, IJRR.

9/16/2014 Modeling Mobile Robots

Trajectory Control

9/16/2014 Modeling Mobile Robots 47

Outline

• Autonomy • WMR Models • Applications

– State Estimation – Inverting Dynamics – Trajectory Generation – Motion Planning in Traffic – Motion Planning in General – Remote Control

• Conclusion 9/16/2014 Modeling Mobile Robots 48

State Space Sampling • Road Navigation example:

– Controls satisfy terminal pose constraints. – Search available option for safe and feasible

trajectory.

49

Howard, Green, & Kelly State Space Sampling, FSR 2007

In Lane Traffic Planner

Page 50

Outline

• Autonomy • WMR Models • Applications

– State Estimation – Inverting Dynamics – Trajectory Generation – Motion Planning in Traffic – Motion Planning in General – Remote Control

• Conclusion 9/16/2014 Modeling Mobile Robots 53

54

Symmetric, Feasible Controls

• Forms the basis of a symmetric reachability graph.

9/16/2014 Modeling Mobile Robots

For Real

55 Pivtoraiko, Kelly & Knepper, Planning in State Lattices, JFR 2009

Graduated Fidelity

56 Pivtoraiko & Kelly, Graduated Fidelity, IROS 2008

Outline

• Autonomy • WMR Models • Applications

– State Estimation – Inverting Dynamics – Trajectory Generation – Path Following – Motion Planning – Remote Control

• Conclusion 9/16/2014 Modeling Mobile Robots 57

9/16/2014 58 Modeling Mobile Robots

Hardware and Synthetic Imagery

Xilinx Spartan 3 FPGA Based

Processing Unit

Stereo Camera

FLIR Camera Ladar

Electronics

9/16/2014 59 Modeling Mobile Robots

Video Reprojection real colorized range camera

virtual camera

computer graphics data base

model building process

rendering process

Computer Vision

Computer Graphics

Rendered Information Comes from Real Video So its highly realistic

Latency Compensation via Motion Prediction

Distort video gathered at posn 1

Last Image to Arrive at OCS Present

Position

Commands arrive

Posn 1

Posn 3 Posn 2

To produce video that would be sensed at posn 3

9/16/2014 Modeling Mobile Robots 60

9/16/2014 61 Modeling Mobile Robots

3D Video

Test Results First Result : 30% Reduction in Test Course Completion Time

9/16/2014 Modeling Mobile Robots 62

Outline

• Autonomy • WMR Models • Applications

– State Estimation – Inverting Dynamics – Trajectory Generation – Path Following – Motion Planning – Remote Control

• Conclusion 9/16/2014 Modeling Mobile Robots 64

Conclusion

• (Self) Modeling is the most basic ingredient in predictive control.

• Formulated correctly, it is a DAE. • WMRs much harder than manipulators.

– But doable!

• Once done, leads to capacity to act much more intelligently in real applications.

9/16/2014 Modeling Mobile Robots 65

66

Collaborators

• Tom Howard • Ross Knepper • Mihail Pitvoraiko • Forrest Rogers-

Markovitz • Michael George • Michel Laverne • Neal Seegmiller

• Issa Nesnas • Antonio Diaz-Calderon • Paul Schenker

At CMU At JPL

http://www.frc.ri.cmu.edu/ ~alonzo/resume/detailedresearchinterests.html 9/16/2014 Modeling Mobile Robots