Upload
others
View
4
Download
0
Embed Size (px)
Citation preview
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