uq robotics teachingrobotics.itee.uq.edu.au/~metr4202/2015/lectures/l13.lqr.pdf · 2015-10-28 · 3...
TRANSCRIPT
![Page 1: UQ Robotics Teachingrobotics.itee.uq.edu.au/~metr4202/2015/lectures/L13.LQR.pdf · 2015-10-28 · 3 12-Aug Robot Kinematics Review (& Ekka Day) 4 19-Aug Robot Dynamics 5 26-Aug Robot](https://reader030.vdocuments.us/reader030/viewer/2022040820/5e684aeede77ef20b1724bed/html5/thumbnails/1.jpg)
1
© 2015 School of Information Technology and Electrical Engineering at the University of Queensland
TexPoint fonts used in EMF.
Read the TexPoint manual before you delete this box.: AAAAA
Schedule
Week Date Lecture (W: 12:05-1:50, 50-N201)
1 29-Jul Introduction
2 5-Aug Representing Position & Orientation & State
(Frames, Transformation Matrices & Affine Transformations)
3 12-Aug Robot Kinematics Review (& Ekka Day)
4 19-Aug Robot Dynamics
5 26-Aug Robot Sensing: Perception
6 2-Sep Robot Sensing: Multiple View Geometry
7 9-Sep Robot Sensing: Feature Detection (as Linear Observers)
8 16-Sep Probabilistic Robotics: Localization
9 23-Sep Quiz
30-Sep Study break
10 7-Oct Motion Planning
11 14-Oct State-Space Modelling
12 21-Oct Shaping the Dynamic Response
13 28-Oct LQR + Course Review
![Page 2: UQ Robotics Teachingrobotics.itee.uq.edu.au/~metr4202/2015/lectures/L13.LQR.pdf · 2015-10-28 · 3 12-Aug Robot Kinematics Review (& Ekka Day) 4 19-Aug Robot Dynamics 5 26-Aug Robot](https://reader030.vdocuments.us/reader030/viewer/2022040820/5e684aeede77ef20b1724bed/html5/thumbnails/2.jpg)
2
Encore! Guest Lecture from MIT
• SLAM & Autopilot:
The robotics of navigation and
self-driving “robots”
• MIT Prof. John Leonard
– co-inventor of SLAM
– Leading expert on
all things autonomous!
• Friday's “Tutorial”
– Axon 104 at 10am
LQR
![Page 3: UQ Robotics Teachingrobotics.itee.uq.edu.au/~metr4202/2015/lectures/L13.LQR.pdf · 2015-10-28 · 3 12-Aug Robot Kinematics Review (& Ekka Day) 4 19-Aug Robot Dynamics 5 26-Aug Robot](https://reader030.vdocuments.us/reader030/viewer/2022040820/5e684aeede77ef20b1724bed/html5/thumbnails/3.jpg)
3
Control Theory
• The use of feedback to regulate a signal
Controller
Plant
Desired
signal xd
Signal x Control input u
Error e = x-xd
(By convention, xd = 0) x’ = f(x,u)
Model-free vs model-based
• Two general philosophies:
– Model-free: do not require a dynamics model to be provided
– Model-based: do use a dynamics model during computation
• Model-free methods:
– Simpler (eg. PID)
– Tend to require much more manual tuning to perform well
• Model-based methods:
– Can achieve good performance (optimal w.r.t. some cost function)
– Are more complicated to implement
– Require reasonably good models (system-specific knowledge)
– Calibration: build a model using measurements before behaving
– Adaptive control: “learn” parameters of the model online from sensors
![Page 4: UQ Robotics Teachingrobotics.itee.uq.edu.au/~metr4202/2015/lectures/L13.LQR.pdf · 2015-10-28 · 3 12-Aug Robot Kinematics Review (& Ekka Day) 4 19-Aug Robot Dynamics 5 26-Aug Robot](https://reader030.vdocuments.us/reader030/viewer/2022040820/5e684aeede77ef20b1724bed/html5/thumbnails/4.jpg)
4
PID control
• Proportional-Integral-Derivative controller
– A workhorse of 1D control systems
– Model-free
• Proportional Case:
– 𝑢(𝑡) = −𝐾𝑝 𝑥(𝑡)
– Negative sign assumes control acts in the same direction as x
x t
Gain
PID control: Integral term
• 𝑢 𝑡 = −𝐾𝑝 𝑥 𝑡 − 𝐾𝑖 𝐼 𝑡
• 𝐼(𝑡) = 𝑥 𝑡 𝑑𝑡𝑡
0 (accumulation of errors)
x t
Residual steady-state errors
driven asymptotically to 0
Integral gain
![Page 5: UQ Robotics Teachingrobotics.itee.uq.edu.au/~metr4202/2015/lectures/L13.LQR.pdf · 2015-10-28 · 3 12-Aug Robot Kinematics Review (& Ekka Day) 4 19-Aug Robot Dynamics 5 26-Aug Robot](https://reader030.vdocuments.us/reader030/viewer/2022040820/5e684aeede77ef20b1724bed/html5/thumbnails/5.jpg)
5
PID control: Integral term: Instability
• I adds a pole
• If not tuned correctly this adds instability
• Ex: For a 2nd order system (momentum), P control
x t
Divergence
PID control: Derivative term
• 𝑢(𝑡) = −𝐾𝑝 𝑥(𝑡) – 𝐾𝑑 𝑥’(𝑡)
x
Derivative gain
![Page 6: UQ Robotics Teachingrobotics.itee.uq.edu.au/~metr4202/2015/lectures/L13.LQR.pdf · 2015-10-28 · 3 12-Aug Robot Kinematics Review (& Ekka Day) 4 19-Aug Robot Dynamics 5 26-Aug Robot](https://reader030.vdocuments.us/reader030/viewer/2022040820/5e684aeede77ef20b1724bed/html5/thumbnails/6.jpg)
6
PID control: Together
• P+I+D:
– 𝑢(𝑡) = −𝐾𝑝 𝑥(𝑡) − 𝐾𝑖 𝐼(𝑡) − 𝐾𝑑 𝑥’(𝑡)
– 𝐼(𝑡) = 𝑥 𝑡 𝑑𝑡𝑡
0
Stability and Convergence
• System is stable if errors stay bounded
• System is convergent if errors -> 0
![Page 7: UQ Robotics Teachingrobotics.itee.uq.edu.au/~metr4202/2015/lectures/L13.LQR.pdf · 2015-10-28 · 3 12-Aug Robot Kinematics Review (& Ekka Day) 4 19-Aug Robot Dynamics 5 26-Aug Robot](https://reader030.vdocuments.us/reader030/viewer/2022040820/5e684aeede77ef20b1724bed/html5/thumbnails/7.jpg)
7
Example: Trajectory following
• Say a trajectory xdes(t) has been designed
– E.g., a rocket’s ascent, a steering path for a car, a plane’s landing
• Apply PID control
– u(t) = Kp (xdes(t)- x(t)) - Ki I(t) + Kd (x’des(t)-x’(t))
– I(t) = 𝑥𝑑𝑒𝑠 𝑡 − 𝑥 𝑡 𝑑𝑡𝑡
0
• The designer of xdes needs to be knowledgeable about the
controller’s behavior!
xdes(t) x(t)
x(t)
Controller Tuning Workflow
• Hypothesize a control policy
• Analysis:
– Assume a model
– Assume disturbances to be handled
– Test performance either through mathematical analysis, or
through simulation
• Go back and redesign control policy
– Mathematical techniques give you more insight to improve
redesign, but require more work
![Page 8: UQ Robotics Teachingrobotics.itee.uq.edu.au/~metr4202/2015/lectures/L13.LQR.pdf · 2015-10-28 · 3 12-Aug Robot Kinematics Review (& Ekka Day) 4 19-Aug Robot Dynamics 5 26-Aug Robot](https://reader030.vdocuments.us/reader030/viewer/2022040820/5e684aeede77ef20b1724bed/html5/thumbnails/8.jpg)
8
Multivariate Systems
• 𝑥’ = 𝑓(𝑥, 𝑢)
• 𝑥 𝑋 𝑅𝑛
• 𝑢 𝑈 𝑅𝑚
• Because m n, and variables are coupled,
• This is not as easy as setting n PID controllers
Linear Quadratic Regulator
• 𝑥’ = 𝐴𝑥 + 𝐵𝑢
• Objective: minimize quadratic cost
𝑥𝑇𝑄 𝑥 + 𝑢𝑇𝑅 𝑢 𝑑𝑡
• Over an infinite horizon
Error term “Effort” penalization
![Page 9: UQ Robotics Teachingrobotics.itee.uq.edu.au/~metr4202/2015/lectures/L13.LQR.pdf · 2015-10-28 · 3 12-Aug Robot Kinematics Review (& Ekka Day) 4 19-Aug Robot Dynamics 5 26-Aug Robot](https://reader030.vdocuments.us/reader030/viewer/2022040820/5e684aeede77ef20b1724bed/html5/thumbnails/9.jpg)
9
Closed form LQR solution
• Closed form solution
u = -K x, with K = R-1BP
• Where P is a symmetric matrix that solves the Riccati
equation
– ATP + PA – PBR-1BTP + Q = 0
– Derivation: calculus of variations
• Packages available for finding solution
Toy Nonlinear Systems
Cart-pole Acrobot
Mountain car
![Page 10: UQ Robotics Teachingrobotics.itee.uq.edu.au/~metr4202/2015/lectures/L13.LQR.pdf · 2015-10-28 · 3 12-Aug Robot Kinematics Review (& Ekka Day) 4 19-Aug Robot Dynamics 5 26-Aug Robot](https://reader030.vdocuments.us/reader030/viewer/2022040820/5e684aeede77ef20b1724bed/html5/thumbnails/10.jpg)
10
Deterministic Linear Quadratic Regulation
Deterministic Linear Quadratic Regulation
![Page 11: UQ Robotics Teachingrobotics.itee.uq.edu.au/~metr4202/2015/lectures/L13.LQR.pdf · 2015-10-28 · 3 12-Aug Robot Kinematics Review (& Ekka Day) 4 19-Aug Robot Dynamics 5 26-Aug Robot](https://reader030.vdocuments.us/reader030/viewer/2022040820/5e684aeede77ef20b1724bed/html5/thumbnails/11.jpg)
11
Optimal Regulation
Optimal Regulation
![Page 12: UQ Robotics Teachingrobotics.itee.uq.edu.au/~metr4202/2015/lectures/L13.LQR.pdf · 2015-10-28 · 3 12-Aug Robot Kinematics Review (& Ekka Day) 4 19-Aug Robot Dynamics 5 26-Aug Robot](https://reader030.vdocuments.us/reader030/viewer/2022040820/5e684aeede77ef20b1724bed/html5/thumbnails/12.jpg)
12
Optimal Regulation
Optimal State Feedback
![Page 13: UQ Robotics Teachingrobotics.itee.uq.edu.au/~metr4202/2015/lectures/L13.LQR.pdf · 2015-10-28 · 3 12-Aug Robot Kinematics Review (& Ekka Day) 4 19-Aug Robot Dynamics 5 26-Aug Robot](https://reader030.vdocuments.us/reader030/viewer/2022040820/5e684aeede77ef20b1724bed/html5/thumbnails/13.jpg)
13
Optimal State Feedback
Optimal State Feedback
![Page 14: UQ Robotics Teachingrobotics.itee.uq.edu.au/~metr4202/2015/lectures/L13.LQR.pdf · 2015-10-28 · 3 12-Aug Robot Kinematics Review (& Ekka Day) 4 19-Aug Robot Dynamics 5 26-Aug Robot](https://reader030.vdocuments.us/reader030/viewer/2022040820/5e684aeede77ef20b1724bed/html5/thumbnails/14.jpg)
14
LQR In MATLAB
From Linear to Nonlinear • We know how to solve (assuming gt, Ut, Xt convex):
• How about nonlinear dynamics:
Shooting Methods (feasible)
Iterate for i=1, 2, 3, …
Execute (from solving (1))
Linearize around resulting trajectory
Solve (1) for current linearization
Collocation Methods (infeasible)
Iterate for i=1, 2, 3, …
--- (no execution)---
Linearize around current solution of (1)
Solve (1) for current linearization
(1)
Sequential Quadratic Programming (SQP) = either of the above methods, but instead of using
linearization, linearize equality constraints, convex-quadratic approximate objective function
![Page 15: UQ Robotics Teachingrobotics.itee.uq.edu.au/~metr4202/2015/lectures/L13.LQR.pdf · 2015-10-28 · 3 12-Aug Robot Kinematics Review (& Ekka Day) 4 19-Aug Robot Dynamics 5 26-Aug Robot](https://reader030.vdocuments.us/reader030/viewer/2022040820/5e684aeede77ef20b1724bed/html5/thumbnails/15.jpg)
15
Model Predictive Control
• Given:
• For k=0, 1, 2, …, T
– Solve
– Execute uk
– Observe resulting state,
Iterative LQR versus Sequential Convex
Programming • Both can solve
• Can run iterative LQR both as a shooting method or as a collocation method, it’s just a
different way of executing “Solve (1) for current linearization.” In case of shooting, the
sequence of linear feedback controllers found can be used for (closed-loop) execution.
• Iterative LQR might need some outer iterations, adjusting “t” of the log barrier
Shooting Methods
Iterate for i=1, 2, 3, …
Execute feedback controller (from solving (1))
Linearize around resulting trajectory
Solve (1) for current linearization
Collocation Methods
Iterate for i=1, 2, 3, …
--- (no execution)---
Linearize around current solution of (1)
Solve (1) for current linearization
Sequential Quadratic Programming (SQP) = either of the above methods, but instead of using
linearization, linearize equality constraints, convex-quadratic approximate objective function
![Page 16: UQ Robotics Teachingrobotics.itee.uq.edu.au/~metr4202/2015/lectures/L13.LQR.pdf · 2015-10-28 · 3 12-Aug Robot Kinematics Review (& Ekka Day) 4 19-Aug Robot Dynamics 5 26-Aug Robot](https://reader030.vdocuments.us/reader030/viewer/2022040820/5e684aeede77ef20b1724bed/html5/thumbnails/16.jpg)
16
Example Shooting
Example Collocation
![Page 17: UQ Robotics Teachingrobotics.itee.uq.edu.au/~metr4202/2015/lectures/L13.LQR.pdf · 2015-10-28 · 3 12-Aug Robot Kinematics Review (& Ekka Day) 4 19-Aug Robot Dynamics 5 26-Aug Robot](https://reader030.vdocuments.us/reader030/viewer/2022040820/5e684aeede77ef20b1724bed/html5/thumbnails/17.jpg)
17
+ :
At all times the sequence of controls is meaningful, and the
objective function optimized directly corresponds to the
current control sequence
-- :
For unstable systems, need to run feedback controller during
forward simulation – Why? Open loop sequence of control inputs computed for the linearized
system will not be perfect for the nonlinear system. If the nonlinear system is
unstable, open loop execution would give poor performance.
– Fixes:
• Run Model Predictive Control for forward simulation
• Compute a linear feedback controller from the 2nd order Taylor expansion
at the optimum
Practical Benefits and Issues with Shooting
+ :
Can initialize with infeasible trajectory. Hence if you have a
rough idea of a sequence of states that would form a
reasonable solution, you can initialize with this sequence of
states without needing to know a control sequence that
would lead through them, and without needing to make them
consistent with the dynamics
-- :
Sequence of control inputs and states might never converge
onto a feasible sequence
Practical Benefits and Issues with Collocation
![Page 18: UQ Robotics Teachingrobotics.itee.uq.edu.au/~metr4202/2015/lectures/L13.LQR.pdf · 2015-10-28 · 3 12-Aug Robot Kinematics Review (& Ekka Day) 4 19-Aug Robot Dynamics 5 26-Aug Robot](https://reader030.vdocuments.us/reader030/viewer/2022040820/5e684aeede77ef20b1724bed/html5/thumbnails/18.jpg)
18
Direct policy synthesis: Optimal control
• Input: cost function J(x), estimated dynamics f(x,u), finite
state/control spaces X, U
• Two basic classes:
– Trajectory optimization: Hypothesize control sequence u(t),
simulate to get x(t), perform optimization to improve u(t), repeat.
– Output: optimal trajectory u(t) (in practice, only a locally optimal
solution is found)
– Dynamic programming: Discretize state and control spaces,
form a discrete search problem, and solve it.
– Output: Optimal policy u(x) across all of X
Discrete Search example
• Split X, U into cells x1,…,xn, u1,…,um
• Build transition function xj = f(xi,uk)dt for all i,k
• State machine with costs dt J(xi) for staying in state I
• Find u(xi) that minimizes sum
of total costs.
• Value iteration: repeated
dynamic programming over
V(xi) = sum of total future
costs
Value function for 1-joint acrobot
![Page 19: UQ Robotics Teachingrobotics.itee.uq.edu.au/~metr4202/2015/lectures/L13.LQR.pdf · 2015-10-28 · 3 12-Aug Robot Kinematics Review (& Ekka Day) 4 19-Aug Robot Dynamics 5 26-Aug Robot](https://reader030.vdocuments.us/reader030/viewer/2022040820/5e684aeede77ef20b1724bed/html5/thumbnails/19.jpg)
19
Receding Horizon Control (aka model predictive
control)
...
horizon 1 horizon h
Estimation
![Page 20: UQ Robotics Teachingrobotics.itee.uq.edu.au/~metr4202/2015/lectures/L13.LQR.pdf · 2015-10-28 · 3 12-Aug Robot Kinematics Review (& Ekka Day) 4 19-Aug Robot Dynamics 5 26-Aug Robot](https://reader030.vdocuments.us/reader030/viewer/2022040820/5e684aeede77ef20b1724bed/html5/thumbnails/20.jpg)
20
Along multiple dimensions
State Space
• We collect our set of uncertain variables into a vector …
x = [x1, x2,…, xN]T
• The set of values that x might take on is termed the state
space
• There is a single true value for x,
but it is unknown
![Page 21: UQ Robotics Teachingrobotics.itee.uq.edu.au/~metr4202/2015/lectures/L13.LQR.pdf · 2015-10-28 · 3 12-Aug Robot Kinematics Review (& Ekka Day) 4 19-Aug Robot Dynamics 5 26-Aug Robot](https://reader030.vdocuments.us/reader030/viewer/2022040820/5e684aeede77ef20b1724bed/html5/thumbnails/21.jpg)
21
State Space Dynamics
Measured versus True
• Measurement errors are inevitable
• So, add Noise to State...
– State Dynamics becomes:
• Can represent this as a “Normal” Distribution
![Page 22: UQ Robotics Teachingrobotics.itee.uq.edu.au/~metr4202/2015/lectures/L13.LQR.pdf · 2015-10-28 · 3 12-Aug Robot Kinematics Review (& Ekka Day) 4 19-Aug Robot Dynamics 5 26-Aug Robot](https://reader030.vdocuments.us/reader030/viewer/2022040820/5e684aeede77ef20b1724bed/html5/thumbnails/22.jpg)
22
Recovering The Truth
• Numerous methods
• Termed “Estimation” because we are trying to estimate
the truth from the signal
• A strategy discovered by Gauss
• Least Squares in Matrix Representation
Recovering the Truth: Terminology
![Page 23: UQ Robotics Teachingrobotics.itee.uq.edu.au/~metr4202/2015/lectures/L13.LQR.pdf · 2015-10-28 · 3 12-Aug Robot Kinematics Review (& Ekka Day) 4 19-Aug Robot Dynamics 5 26-Aug Robot](https://reader030.vdocuments.us/reader030/viewer/2022040820/5e684aeede77ef20b1724bed/html5/thumbnails/23.jpg)
23
General Problem…
Duals and Dual Terminology
![Page 24: UQ Robotics Teachingrobotics.itee.uq.edu.au/~metr4202/2015/lectures/L13.LQR.pdf · 2015-10-28 · 3 12-Aug Robot Kinematics Review (& Ekka Day) 4 19-Aug Robot Dynamics 5 26-Aug Robot](https://reader030.vdocuments.us/reader030/viewer/2022040820/5e684aeede77ef20b1724bed/html5/thumbnails/24.jpg)
24
Estimation Process in Pictures
Kalman Filter Process
![Page 25: UQ Robotics Teachingrobotics.itee.uq.edu.au/~metr4202/2015/lectures/L13.LQR.pdf · 2015-10-28 · 3 12-Aug Robot Kinematics Review (& Ekka Day) 4 19-Aug Robot Dynamics 5 26-Aug Robot](https://reader030.vdocuments.us/reader030/viewer/2022040820/5e684aeede77ef20b1724bed/html5/thumbnails/25.jpg)
25
KF Process in Equations
KF Considerations
![Page 26: UQ Robotics Teachingrobotics.itee.uq.edu.au/~metr4202/2015/lectures/L13.LQR.pdf · 2015-10-28 · 3 12-Aug Robot Kinematics Review (& Ekka Day) 4 19-Aug Robot Dynamics 5 26-Aug Robot](https://reader030.vdocuments.us/reader030/viewer/2022040820/5e684aeede77ef20b1724bed/html5/thumbnails/26.jpg)
26
Ex: Kinematic KF: Tracking
• Consider a System with Constant Acceleration
In Summary
• KF:
– The true state (x) is separate from the measured (z)
– Lets you combine prior controls knowledge with
measurements to filter signals and find the truth
– It regulates the covariance (P)
• As P is the scatter between z and x
• So, if P 0, then z x (measurements truth)
• EKF:
– Takes a Taylor series approximation to get a local “F” (and
“G” and “H”)
![Page 27: UQ Robotics Teachingrobotics.itee.uq.edu.au/~metr4202/2015/lectures/L13.LQR.pdf · 2015-10-28 · 3 12-Aug Robot Kinematics Review (& Ekka Day) 4 19-Aug Robot Dynamics 5 26-Aug Robot](https://reader030.vdocuments.us/reader030/viewer/2022040820/5e684aeede77ef20b1724bed/html5/thumbnails/27.jpg)
27
Case Study I:
Gryphon Demining Robot
“Bang-Bang Control!”
![Page 28: UQ Robotics Teachingrobotics.itee.uq.edu.au/~metr4202/2015/lectures/L13.LQR.pdf · 2015-10-28 · 3 12-Aug Robot Kinematics Review (& Ekka Day) 4 19-Aug Robot Dynamics 5 26-Aug Robot](https://reader030.vdocuments.us/reader030/viewer/2022040820/5e684aeede77ef20b1724bed/html5/thumbnails/28.jpg)
28
Gryphon: Mine Scanning Robot
Landmines: Smart for one, dumb for all…
![Page 29: UQ Robotics Teachingrobotics.itee.uq.edu.au/~metr4202/2015/lectures/L13.LQR.pdf · 2015-10-28 · 3 12-Aug Robot Kinematics Review (& Ekka Day) 4 19-Aug Robot Dynamics 5 26-Aug Robot](https://reader030.vdocuments.us/reader030/viewer/2022040820/5e684aeede77ef20b1724bed/html5/thumbnails/29.jpg)
29
Sensor Mobility Is Critical
Back to Gyrphon …
![Page 30: UQ Robotics Teachingrobotics.itee.uq.edu.au/~metr4202/2015/lectures/L13.LQR.pdf · 2015-10-28 · 3 12-Aug Robot Kinematics Review (& Ekka Day) 4 19-Aug Robot Dynamics 5 26-Aug Robot](https://reader030.vdocuments.us/reader030/viewer/2022040820/5e684aeede77ef20b1724bed/html5/thumbnails/30.jpg)
30
Cleared area
Minefield
All terrain vehicle
Counter- weight
Stereo vision camera
Optional ground- penetrating radar
Metal detector
Network camera
Part of a Robotic Solution…
Gryphon Schematic
Wrist joints
z
x
Manipulator
Sensor
Terrain
Camera
Ground frame FG
Joint 1 (yaw)
Joint 2
Joint3
Compliant base
Manipulator frame FM
Counter-weight
![Page 31: UQ Robotics Teachingrobotics.itee.uq.edu.au/~metr4202/2015/lectures/L13.LQR.pdf · 2015-10-28 · 3 12-Aug Robot Kinematics Review (& Ekka Day) 4 19-Aug Robot Dynamics 5 26-Aug Robot](https://reader030.vdocuments.us/reader030/viewer/2022040820/5e684aeede77ef20b1724bed/html5/thumbnails/31.jpg)
31
Robust Control:
Command Shaping for Vibration Reduction
Command
ShappingRegulator
Integrated
Planner
Controller
Error PlantΣ+
–
SensorTunning
Command Shaping
Original velocity profile
Input shaper
Command-shaped velocity profile
Ve
locity
Ve
locity
Time
Time
Time*
![Page 32: UQ Robotics Teachingrobotics.itee.uq.edu.au/~metr4202/2015/lectures/L13.LQR.pdf · 2015-10-28 · 3 12-Aug Robot Kinematics Review (& Ekka Day) 4 19-Aug Robot Dynamics 5 26-Aug Robot](https://reader030.vdocuments.us/reader030/viewer/2022040820/5e684aeede77ef20b1724bed/html5/thumbnails/32.jpg)
32
Command Shaping in Position Space
Command Shaping:
Zero Vibration and Derivative
2,1i
For Gryphon: At ρ0=1.5 [m] At ρ1=3.0 [m]
ω 2.32 1.81 Axis 1
ζ 0 0
ω 3.3 3.0 Axis 2 & 3
ζ 0 0
![Page 33: UQ Robotics Teachingrobotics.itee.uq.edu.au/~metr4202/2015/lectures/L13.LQR.pdf · 2015-10-28 · 3 12-Aug Robot Kinematics Review (& Ekka Day) 4 19-Aug Robot Dynamics 5 26-Aug Robot](https://reader030.vdocuments.us/reader030/viewer/2022040820/5e684aeede77ef20b1724bed/html5/thumbnails/33.jpg)
33
Gryphon: Comparison to other tracked robots
Mechanical Robustness
Co
ntr
ol
Ro
bu
stn
ess
(“A
uto
no
my
”)
Terrain MapModel:
Conditional Planar Filter
Compute
Normals
Map:
Terrain
Mesh
Model
Apply
filter(s)
![Page 34: UQ Robotics Teachingrobotics.itee.uq.edu.au/~metr4202/2015/lectures/L13.LQR.pdf · 2015-10-28 · 3 12-Aug Robot Kinematics Review (& Ekka Day) 4 19-Aug Robot Dynamics 5 26-Aug Robot](https://reader030.vdocuments.us/reader030/viewer/2022040820/5e684aeede77ef20b1724bed/html5/thumbnails/34.jpg)
34
Effect of Overall Calibration Matrix
-10
-5
0
5
10
15
20
25
30
0 0.5 1 1.5
Circular distance [m]
Devi
atio
n f
rom
ide
al p
ath [
mm
]
With Overall Calibration Matrixcorrection
Without Overall Calibration Matrixcorrection
Scanning speed: 100 mm/s
Scanning gap: 100 mm
Path Generation (II)
• Orientation: Advanced Terrain Following
(a) (b) (c)
(d) (e)
Control
points
![Page 35: UQ Robotics Teachingrobotics.itee.uq.edu.au/~metr4202/2015/lectures/L13.LQR.pdf · 2015-10-28 · 3 12-Aug Robot Kinematics Review (& Ekka Day) 4 19-Aug Robot Dynamics 5 26-Aug Robot](https://reader030.vdocuments.us/reader030/viewer/2022040820/5e684aeede77ef20b1724bed/html5/thumbnails/35.jpg)
35
Contour Following
Detector Envelope
Terrain
Potential collisions
Terrain Modeling: Find a good model to characterize
![Page 36: UQ Robotics Teachingrobotics.itee.uq.edu.au/~metr4202/2015/lectures/L13.LQR.pdf · 2015-10-28 · 3 12-Aug Robot Kinematics Review (& Ekka Day) 4 19-Aug Robot Dynamics 5 26-Aug Robot](https://reader030.vdocuments.us/reader030/viewer/2022040820/5e684aeede77ef20b1724bed/html5/thumbnails/36.jpg)
36
Experiments: Scanning Over Obstacle
Scanning on ~ Level Terrain - Measurements
-10
-5
0
5
10
15
20
0 0.5 1 1.5
Circular distance [m]
Devi
atio
n f
rom
ide
al p
ath [
mm
]
Unfiltered
Gaussian filtered
Conditional Planar filtered
Scanning speed: 100 mm/s
Scanning gap: 100 mm
Manipulator
Laser range
finder
Scan pass
![Page 37: UQ Robotics Teachingrobotics.itee.uq.edu.au/~metr4202/2015/lectures/L13.LQR.pdf · 2015-10-28 · 3 12-Aug Robot Kinematics Review (& Ekka Day) 4 19-Aug Robot Dynamics 5 26-Aug Robot](https://reader030.vdocuments.us/reader030/viewer/2022040820/5e684aeede77ef20b1724bed/html5/thumbnails/37.jpg)
37
Scanning on Rough Terrain - Measurements
-40
-30
-20
-10
0
10
20
30
0 0.5 1 1.5
Circular distance [m]
Devi
atio
n f
rom
ide
al p
ath [
mm
]
Unfiltered
Gaussian filtered
Conditional Planar filtered
Obstacle location
50 cm
20 cm
Scan pass
Rough terrain
obstacle
~70º slope
Manipulator
Laser range
finder
Scanning speed: 100 mm/s
Scanning gap: 100 mm
Command Shaping Tests: Step-Response
• Reduced Joint
Encoder Vibration • Reduced Tip
Acceleration Joint 1 (ATV Yaw) Encoder:
Joint 3 (Arm Extend) Encoder:
![Page 38: UQ Robotics Teachingrobotics.itee.uq.edu.au/~metr4202/2015/lectures/L13.LQR.pdf · 2015-10-28 · 3 12-Aug Robot Kinematics Review (& Ekka Day) 4 19-Aug Robot Dynamics 5 26-Aug Robot](https://reader030.vdocuments.us/reader030/viewer/2022040820/5e684aeede77ef20b1724bed/html5/thumbnails/38.jpg)
38
High-Level Control Software
Extensive Field Tests
![Page 39: UQ Robotics Teachingrobotics.itee.uq.edu.au/~metr4202/2015/lectures/L13.LQR.pdf · 2015-10-28 · 3 12-Aug Robot Kinematics Review (& Ekka Day) 4 19-Aug Robot Dynamics 5 26-Aug Robot](https://reader030.vdocuments.us/reader030/viewer/2022040820/5e684aeede77ef20b1724bed/html5/thumbnails/39.jpg)
39
Gryphon: Field Tests in Croatia & Cambodia
Terrain & Estimation
• IF we know terrain Triangulation
• IF we know depth SNR gives terrain “characteristic”
• Estimate both simultaneously ( solution up to scale)
Detector
Terrain
t0 ti
Target
![Page 40: UQ Robotics Teachingrobotics.itee.uq.edu.au/~metr4202/2015/lectures/L13.LQR.pdf · 2015-10-28 · 3 12-Aug Robot Kinematics Review (& Ekka Day) 4 19-Aug Robot Dynamics 5 26-Aug Robot](https://reader030.vdocuments.us/reader030/viewer/2022040820/5e684aeede77ef20b1724bed/html5/thumbnails/40.jpg)
40
SECaT Time! … Brought To You By the Number 5