Trajectory tracking & Path-following control

Transcription

1 Cooperative Control of Multiple Robotic Vehicles: Theory and Practice Trajectory tracking & Path-following control EECI Graduate School on Control Supélec, Feb , 2011

2 A word about T Tracking and P Following Path following Reference path given in a time-free parameterization Constant forward velocity Smoother convergence to the path Reference path Trajectory Tracking Time and space reference trajectory The vehicle may turn back in its attempt to be at a given reference point at a prescribed time Reference trajectory Space Possible vehicle trajectory Space x Time Path-following is motivated by applications in which spatial errors are more critical than temporal errors

3 Reference-tracking versus path-following Additional design of freedom The reference-tracking problem is subjected to the limitations imposed by the unstable zero-dynamics. The path-following problem is not subjected to these limitations The freedom to design a timing law is a major advantage of pathfollowing over reference tracking. A. Pedro Aguiar, João P. Hespanha, and Petar Kokotović, Path-Following for Non-Minimum Phase Systems Removes Performance Limitations. IEEE Transactions on Automatic Control, Vol. 50, No. 2, pp , Feb

4 Position tracking of an underactuated Hovercraft Goal: force the hovercraft to track a circular trajectory (black) due to side-slip the velocity of the hovercraft is not tangent to the trajectory

5 Problem statement Consider an underactuated vehicle modeled as a rigid body subject to external forces and torques Kinematics Dynamics Trajectory-tracking problem Given a trajectory p d :[0,1)! R 3, we want the tracking error p(t)-p d (t) to converge to a neighborhood of the origin that can be made arbitrarily small The solution should be robust with respect to parametric modeling uncertainty later extended to path following

6 Position tracking of an underactuated Hovercraft Goal: force the hovercraft to track a circular trajectory (black) due to side-slip the velocity of the hovercraft is not tangent to the trajectory

7 Controller design Step 1. Coordinate Transformation tracking error in body frame Step 2. Convergence of e error only in position! <0 linear velocity viewed as a virtual control input

8 Controller design Step 3. Backstepping for z 1 virtual control input control input It will not always be possible to drive z 1 to zero! Instead, we will drive z 1 to a small constant δ dominated by the first term <0 <0 1st control signal has been assigned angular velocity viewed as a virtual control input

9 Controller design Step 4. Backstepping for z 2 dominated by the first term <0 <0 <0 2nd control signal has been assigned Using Young s inequality, 9 γ > 0 can be made arbitrarily small All signals remain bounded and converges to ball of radius proportional to δ

10 Position tracking of an underactuated Hovercraft Goal: force the hovercraft to track a circular trajectory (black) due to side-slip the velocity of the hovercraft is not tangent to the trajectory

11 Model parameter uncertainty What happens if there is parametric modeling uncertainty? Coefficient of viscous friction assumed used by the controller is 10% of the real value Closed-loop system still stable but considerable performance degradation

12 Supervisory control supervisor σ controller 1 bank of candidate controllers controller n switching signal σ u control signal exogenous disturbance/ noise w process y measured output Key ideas: 1. Build a bank of alternative controllers (one for each possible value/range of the unknown parameter) 2. Supervisor places in the feedback loop the controller that seems more promising based on the available measurements

13 Estimator-based supervision s setup w exogenous disturbance/noise control signal u process y measured output Process is assumed to be in a family parametric uncertainty For each admissible process model M p, there is one candidate controller C p that provides adequate performance. How to determine which admissible model matches the real process?

14 measured output control signal Estimator-based supervisor y u multiestimator + + y decision logic σ switching signal Process is assumed to be in family process is M p, p2 P controller C p provides adequate performance Multi-estimator y p estimate of the process output y that would be correct if the process was M p e p output estimation error that would be small if the process was M p Decision logic: e p small process is likely to be M p Certainty equivalence inspired should use C p

15 Multi-estimator for the vehicle model Process model uncertainty in the dynamic equations through: M, J, f v, f ω Family of estimator equations (p2 P) scalar positive functions estimation errors The correct estimation error e p* satisfies convergence to zero small integral norm

16 measured output control signal Estimator-based supervisor y u Process is assumed to be in family process is M p, p2 P but not the converse controller C p provides adequate performance Multi-estimator y p estimate of the process output y that would be correct if the process was M p e p output estimation error that would be small if the process was M p Decision logic: multiestimator + + y decision logic A stability argument cannot be based on this because typically process is M p ) e p small σ switching signal e p small process is likely to be M p use C p Certainty equivalence inspired

17 measured output control signal Estimator-based supervisor y u Process is assumed to be in family process is M p, p2 P controller C p provides adequate performance Multi-estimator y p estimate of the process output y that would be correct if the process was M p e p output estimation error that would be small if the process was M p Decision logic: e p small multiestimator process is likely to be M p + + y use C p decision logic overall system is detectable through e p Certainty equivalence inspired, but formally justified by detectability σ switching signal detectable means small e p ) small state overall state is small

18 Detectability property e p small process is likely to be M p use C p overall system is detectable through e p detectable means small e p ) small state overall state is small Using the original Lyapunov function and positive constant constant in L 1 in L 2 When e p is small, all signals remain bounded in L 1 & e converges to ball of radius proportional to δ

19 Scale-independent hysteresis switching How to pick a small estimation error? start monitoring signals integral norm of estimation error p 2 P y n hysteresis constant measure of the size of e p over a window of length 1/λ µ forgetting factor wait until current monitoring signal becomes significantly larger than some other one

20 Simulation results Estimator-based supervisor controller e x [m] e y [m] q-q d [degree] time [s] time [s] time [s] 8 6 d v time [s] Position tracking in the presence of parametric uncertainty and measurement noise

21 Path-following Consider an underactuated vehicle modeled as a rigid body subject to external forces and torques Kinematics Dynamics Path-following problem Given a geometric path {p d (γ)2 R 3 : γ2[0,1)} and speed assignment v r (γ) 2 R, we want the position to converge and remain inside a tube centered around the desired path than can be made arbitrarily thin, and satisfy (asymptotically) the desired speed assignment, i.e., γ! v r as t!1

22 Tracking and path-following of an underwater vehicle (3-D space) The Sirene AUV developed for Deep Sea Intervention on Future Benthic Laboratories Goal: force the underactuated AUV to track a desired helix trajectory

23 Goal Path-following Given a geometric path {p d (γ)2 R 3 : γ2[0,1)} and speed assignment v r (γ) 2 R, we want the position to converge and remain inside a tube centered around the desired path than can be made arbitrarily thin, and satisfy (asymptotically) the desired speed assignment, i.e., γ! v r as t!1 Define speed error Choosing Same conclusions as before

24 Simulation results Trajectory tracking Path-following roll [degree] pitch [degree] yaw [degree] time [s] time [s] time [s] roll [degree] pitch [degree] yaw [degree] time [s] time [s] time [s]

25 UAV Path Following Concept Path-following for an Unmanned Aerial vehicle (UAV) Objective: follow predefined spatial 3D paths paths are time-independent: decoupling between space and time = separation of 3D path and speed speed can be used as an additional DOF for time coordination

26 UAV Path Following System Architecture Inner/Outer Loop Solution Trajectory Generation Onboard PC104 Boundary conditions polynomial path Path following (Outer loop) Pitch rate Yaw rate commands L 1 adaptive controller Onboard A/P (Inner loop) User Laptop

27 Problem Geometry F: Serret-Frenet frame W: wind frame I: inertial frame desired trajectory UAV speed flight path angle heading angle path length position of UAV in inertial frame Inertial Frame {I} z I Desired path to follow q I y I y F (N) p c UAV x I Q q F V s z F (B) z 1 x F (T) y 1 s 1 P Serret Frenet Frame {F} 3D Kinematics Equations Input:

28 UAV Path Following Key idea: use virtual target to determine desired location on the path Minimize the distance of the UAV from the virtual target on the path Reduce the angle between the vehicle velocity vector and local tangent to the path Virtual target s motion extra degree of freedom Path q Q s 1 y 1 {I} : Inertial Frame Virtual Target P {F} : Serret-Frenet Frame

29 UAV Path Following (cont.) Control the evolution of the virtual target : added degree of freedom Path Q {I} : Inertial Frame P

30 Kinematics Coordinat e systems I Kinematics equations in I F W - difference between and in F - Euler angles from F to W Error Equations in F where

31 Kinematic Control Law Desired shaping functions Path following control laws