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 Representing Position & Orientation & State 5-Aug (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 1
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 2
Control Theory The use of feedback to regulate a signal Desired signal x d Controller Signal x Control input u Plant Error e = x-x d (By convention, x d = 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 3
PID control Proportional-Integral-Derivative controller A workhorse of 1D control systems Model-free Proportional Case: Gain u(t) = K p x(t) Negative sign assumes control acts in the same direction as x x t PID control: Integral term Integral gain u t = K p x t K i I t t I(t) = x t dt 0 (accumulation of errors) x t Residual steady-state errors driven asymptotically to 0 4
PID control: Integral term: Instability I adds a pole If not tuned correctly this adds instability Ex: For a 2 nd order system (momentum), P control Divergence x t PID control: Derivative term Derivative gain u(t) = K p x(t) K d x (t) x 5
PID control: Together P+I+D: u(t) = K p x(t) K i I(t) K d x (t) t I(t) = x t dt 0 Stability and Convergence System is stable if errors stay bounded System is convergent if errors -> 0 6
Example: Trajectory following Say a trajectory x des (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) = K p (x des (t)- x(t)) - K i I(t) + K d (x des (t)-x (t)) t I(t) = x des t x t dt 0 The designer of x des needs to be knowledgeable about the controller s behavior! x des (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 7
Multivariate Systems x = f(x, u) x X Rn u U Rm Because m n, and variables are coupled, This is not as easy as setting n PID controllers Linear Quadratic Regulator x = Ax + Bu Objective: minimize quadratic cost x T Q x + u T R u dt Error term Effort penalization Over an infinite horizon 8
Closed form LQR solution Closed form solution u = -K x, with K = R -1 BP Where P is a symmetric matrix that solves the Riccati equation A T P + PA PBR -1 B T P + Q = 0 Derivation: calculus of variations Packages available for finding solution Toy Nonlinear Systems Cart-pole Acrobot Mountain car 9
Deterministic Linear Quadratic Regulation Deterministic Linear Quadratic Regulation 10
Optimal Regulation Optimal Regulation 11
Optimal Regulation Optimal State Feedback 12
Optimal State Feedback Optimal State Feedback 13
LQR In MATLAB From Linear to Nonlinear We know how to solve (assuming g t, U t, X t convex): How about nonlinear dynamics: (1) 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 Sequential Quadratic Programming (SQP) = either of the above methods, but instead of using linearization, linearize equality constraints, convex-quadratic approximate objective function 14
Model Predictive Control Given: For k=0, 1, 2,, T Solve Execute u k 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 15
Example Shooting Example Collocation 16
Practical Benefits and Issues with Shooting + : 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 2 nd order Taylor expansion at the optimum Practical Benefits and Issues with Collocation + : 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 17
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 x 1,,x n, u 1,,u m Build transition function x j = f(x i,u k )dt for all i,k State machine with costs dt J(x i ) for staying in state I Find u(x i ) that minimizes sum of total costs. Value iteration: repeated dynamic programming over V(x i ) = sum of total future costs Value function for 1-joint acrobot 18
Receding Horizon Control (aka model predictive control)... horizon 1 horizon h Estimation 19
Along multiple dimensions State Space We collect our set of uncertain variables into a vector x = [x 1, x 2,, x N ] 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 20
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 21
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 22
General Problem Duals and Dual Terminology 23
Estimation Process in Pictures Kalman Filter Process 24
KF Process in Equations KF Considerations 25
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 ) 26
Case Study I: Gryphon Demining Robot Bang-Bang Control! 27
Gryphon: Mine Scanning Robot Landmines: Smart for one, dumb for all 28
Sensor Mobility Is Critical Back to Gyrphon 29
Part of a Robotic Solution Stereo vision camera Optional groundpenetrating radar Network camera Counterweight All terrain vehicle Cleared area Metal detector Minefield Gryphon Schematic Joint3 Ground frame F G Manipulator Camera z Joint 2 Wrist joints Sensor Manipulator frame F M Terrain x Compliant base Counter-weight Joint 1 (yaw) 30
Velocity Velocity Robust Control: Command Shaping for Vibration Reduction Integrated Planner Controller Command Shapping + Σ Error Regulator Plant Tunning Sensor Command Shaping Original velocity profile * Time Command-shaped velocity profile Input shaper Time Time 31
Command Shaping in Position Space Command Shaping: Zero Vibration and Derivative i 1,2 For Gryphon: Axis 1 Axis 2 & 3 At ρ 0 =1.5 [m] At ρ 1 =3.0 [m] ω 2.32 1.81 ζ 0 0 ω 3.3 3.0 ζ 0 0 32
Control Robustness ( Autonomy ) Gryphon: Comparison to other tracked robots Mechanical Robustness Terrain Map Model: Conditional Planar Filter Map: Terrain Mesh Model Compute Normals Apply filter(s) 33
Deviation from ideal path [mm] Effect of Overall Calibration Matrix 30 25 20 With Overall Calibration Matrix correction Without Overall Calibration Matrix correction 15 10 5 0-5 -10 0 0.5 1 1.5 Circular distance [m] Scanning speed: 100 mm/s Scanning gap: 100 mm Path Generation (II) Orientation: Advanced Terrain Following Control points (a) (b) (c) (d) (e) 34
Contour Following Detector Envelope Potential collisions Terrain Terrain Modeling: Find a good model to characterize 35
Deviation from ideal path [mm] Experiments: Scanning Over Obstacle Scanning on ~ Level Terrain - Measurements 20 15 Unfiltered Gaussian filtered 10 Conditional Planar filtered 5 0-5 Laser range finder -10 0 0.5 1 1.5 Circular distance [m] Scan pass Manipulator Scanning speed: 100 mm/s Scanning gap: 100 mm 36
Deviation from ideal path [mm] Scanning on Rough Terrain - Measurements 30 20 10 Unfiltered Gaussian filtered Conditional Planar filtered 0-10 -20-30 Scanning speed: 100 mm/s Scanning gap: 100 mm 20 cm Laser range finder ~70º slope Obstacle location -40 Scan pass 0 0.5 1 1.5 Circular distance [m] Rough terrain obstacle Manipulator 50 cm Command Shaping Tests: Step-Response Reduced Joint Encoder Vibration Joint 1 (ATV Yaw) Encoder: Reduced Tip Acceleration Joint 3 (Arm Extend) Encoder: 37
High-Level Control Software Extensive Field Tests 38
Gryphon: Field Tests in Croatia & Cambodia Terrain & Estimation Detector t 0 t i Terrain Target IF we know terrain Triangulation IF we know depth SNR gives terrain characteristic Estimate both simultaneously ( solution up to scale) 39
SECaT Time! Brought To You By the Number 5 40