Modeling Mobile Robots -...
Transcript of Modeling Mobile Robots -...
![Page 1: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/1.jpg)
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
![Page 2: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/2.jpg)
Outline
• Autonomy • WMR Models • Applications • Conclusion
9/16/2014 Modeling Mobile Robots 2
![Page 3: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/3.jpg)
Outline
• Autonomy – Computation – Architecture
• WMR Models • Applications • Conclusion
9/16/2014 Modeling Mobile Robots 3
![Page 4: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/4.jpg)
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
![Page 5: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/5.jpg)
Outline
• Autonomy – Computation – Architecture
• WMR Models • Applications • Conclusion
9/16/2014 Modeling Mobile Robots 5
![Page 6: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/6.jpg)
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
![Page 7: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/7.jpg)
Outline
• Autonomy • WMR Models
– Motivation – Nature – Formulation – Calibration
• Applications • Conclusion
9/16/2014 Modeling Mobile Robots 7
![Page 8: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/8.jpg)
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
![Page 9: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/9.jpg)
Outline
• Autonomy • WMR Models
– Motivation – Nature – Formulation – Calibration
• Applications • Conclusion
9/16/2014 Modeling Mobile Robots 9
![Page 10: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/10.jpg)
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)
![Page 11: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/11.jpg)
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=
![Page 12: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/12.jpg)
Outline • Autonomy • WMR Models
– Motivation – Nature – Formulation
Kinematics DAEs Constraints
– Calibration • Applications • Conclusion
9/16/2014 Modeling Mobile Robots 12
![Page 13: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/13.jpg)
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
![Page 14: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/14.jpg)
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
![Page 15: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/15.jpg)
Example: 4 Wheel Steer
15 9/16/2014 Modeling Mobile Robots
![Page 16: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/16.jpg)
Outline • Autonomy • WMR Models
– Motivation – Nature – Formulation
Kinematics DAEs Constraints
– Calibration • Applications • Conclusion
9/16/2014 Modeling Mobile Robots 16
![Page 17: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/17.jpg)
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
𝑉𝑉 ∙ 𝑑𝑑𝑑𝑑
𝑉𝑉 ∙ 𝑑𝑑𝑑𝑑 ≠ 𝑉𝑉𝑐𝑐𝑜𝑜𝑐𝑐𝑐𝑐 ∙ 𝑑𝑑𝑑𝑑
𝑉𝑉𝑐𝑐𝑜𝑜𝑐𝑐𝑐𝑐 ∙ 𝑑𝑑𝑑𝑑
![Page 18: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/18.jpg)
Notation
9/16/2014 Modeling Mobile Robots 18
�̇�𝒙 = 𝒇𝒇(𝒙𝒙,𝒖𝒖, 𝒕𝒕)
“state” (x,y,θ)
“state derivative” (velocity etc.)
“inputs” (speed, steer)
time (omitted)
𝒙𝒙 = � 𝒇𝒇 𝒙𝒙,𝒖𝒖, 𝒕𝒕 𝒅𝒅𝒕𝒕 𝒕𝒕
𝟎𝟎
![Page 19: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/19.jpg)
• 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
![Page 20: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/20.jpg)
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.
![Page 21: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/21.jpg)
22
Example of DAE Models
9/16/2014 Modeling Mobile Robots
![Page 22: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/22.jpg)
Outline • Autonomy • WMR Models
– Motivation – Nature – Formulation
Kinematics DAEs Constraints
– Calibration • Applications • Conclusion
9/16/2014 Modeling Mobile Robots 23
![Page 23: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/23.jpg)
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
![Page 24: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/24.jpg)
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
![Page 25: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/25.jpg)
Outline
• Autonomy • WMR Models
– Motivation – Nature – Formulation – Calibration
• Applications • Conclusion
9/16/2014 Modeling Mobile Robots 26
![Page 26: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/26.jpg)
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
![Page 27: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/27.jpg)
Real Time Slip Model Identification
28 9/16/2014 Modeling Mobile Robots
![Page 28: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/28.jpg)
Results at Extremes
9/16/2014 Modeling Mobile Robots 29
Kelly & Seegmiller, Integrated Prediction Error Minimization, IJRR.
![Page 29: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/29.jpg)
Results at Extremes
9/16/2014 Modeling Mobile Robots 30
20° Roll Angle 20° Pitch Angle
![Page 30: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/30.jpg)
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
![Page 31: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/31.jpg)
Cg Calibration and Adaptive Stability Control
9/16/2014 32 Diaz-Calderon & Kelly, Online Stability Margin …, IJRR
![Page 32: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/32.jpg)
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
![Page 33: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/33.jpg)
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
![Page 34: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/34.jpg)
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
![Page 35: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/35.jpg)
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.
![Page 36: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/36.jpg)
Results
9/16/2014 Modeling Mobile Robots 38
![Page 37: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/37.jpg)
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
![Page 38: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/38.jpg)
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
![Page 39: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/39.jpg)
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
![Page 40: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/40.jpg)
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
![Page 41: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/41.jpg)
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
![Page 42: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/42.jpg)
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.
![Page 43: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/43.jpg)
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
![Page 44: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/44.jpg)
46
Convergence & Solution
Howard & Kelly, Rough Terrain Trajectory Generation, IJRR.
9/16/2014 Modeling Mobile Robots
![Page 45: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/45.jpg)
Trajectory Control
9/16/2014 Modeling Mobile Robots 47
![Page 46: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/46.jpg)
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
![Page 47: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/47.jpg)
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
![Page 48: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/48.jpg)
In Lane Traffic Planner
Page 50
![Page 49: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/49.jpg)
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
![Page 50: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/50.jpg)
54
Symmetric, Feasible Controls
• Forms the basis of a symmetric reachability graph.
9/16/2014 Modeling Mobile Robots
![Page 51: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/51.jpg)
For Real
55 Pivtoraiko, Kelly & Knepper, Planning in State Lattices, JFR 2009
![Page 52: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/52.jpg)
Graduated Fidelity
56 Pivtoraiko & Kelly, Graduated Fidelity, IROS 2008
![Page 53: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/53.jpg)
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
![Page 54: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/54.jpg)
9/16/2014 58 Modeling Mobile Robots
Hardware and Synthetic Imagery
Xilinx Spartan 3 FPGA Based
Processing Unit
Stereo Camera
FLIR Camera Ladar
Electronics
![Page 55: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/55.jpg)
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
![Page 56: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/56.jpg)
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
![Page 57: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/57.jpg)
9/16/2014 61 Modeling Mobile Robots
3D Video
![Page 58: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/58.jpg)
Test Results First Result : 30% Reduction in Test Course Completion Time
9/16/2014 Modeling Mobile Robots 62
![Page 59: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/59.jpg)
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
![Page 60: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/60.jpg)
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
![Page 61: Modeling Mobile Robots - ppniv14.irccyn.ec-nantes.frppniv14.irccyn.ec-nantes.fr/material/session4/Kelly/presentation.pdf · 1 L 1 ys = 123 L 3 +s 12 L 2 + s 1 L 1 ψψ= 1 + ψ 2 +](https://reader036.fdocuments.us/reader036/viewer/2022070821/5f1fcdeb44607025af2e696f/html5/thumbnails/61.jpg)
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