TexPoint fonts used in EMF. Read the TexPoint manual before you delete this box.: AAAAA

Transcription

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 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 Oct State-Space Modelling Oct Shaping the Dynamic Response Oct LQR + Course Review 1

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 2

3 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

4 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

5 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

6 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

7 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

8 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

9 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

10 Deterministic Linear Quadratic Regulation Deterministic Linear Quadratic Regulation 10

11 Optimal Regulation Optimal Regulation 11

12 Optimal Regulation Optimal State Feedback 12

13 Optimal State Feedback Optimal State Feedback 13

14 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

15 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

16 Example Shooting Example Collocation 16

17 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

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 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

19 Receding Horizon Control (aka model predictive control)... horizon 1 horizon h Estimation 19

20 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

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 21

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 22

23 General Problem Duals and Dual Terminology 23

24 Estimation Process in Pictures Kalman Filter Process 24

25 KF Process in Equations KF Considerations 25

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 ) 26

27 Case Study I: Gryphon Demining Robot Bang-Bang Control! 27

28 Gryphon: Mine Scanning Robot Landmines: Smart for one, dumb for all 28

29 Sensor Mobility Is Critical Back to Gyrphon 29

30 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

31 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

32 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] ω ζ 0 0 ω ζ

33 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

34 Deviation from ideal path [mm] Effect of Overall Calibration Matrix With Overall Calibration Matrix correction Without Overall Calibration Matrix correction 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

35 Contour Following Detector Envelope Potential collisions Terrain Terrain Modeling: Find a good model to characterize 35

36 Deviation from ideal path [mm] Experiments: Scanning Over Obstacle Scanning on ~ Level Terrain - Measurements Unfiltered Gaussian filtered 10 Conditional Planar filtered Laser range finder Circular distance [m] Scan pass Manipulator Scanning speed: 100 mm/s Scanning gap: 100 mm 36

37 Deviation from ideal path [mm] Scanning on Rough Terrain - Measurements Unfiltered Gaussian filtered Conditional Planar filtered Scanning speed: 100 mm/s Scanning gap: 100 mm 20 cm Laser range finder ~70º slope Obstacle location -40 Scan pass 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

38 High-Level Control Software Extensive Field Tests 38

39 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

40 SECaT Time! Brought To You By the Number 5 40