Control of Mobile Robots Regulation and trajectory tracking Prof. Luca Bascetta (luca.bascetta@polimi.it) Politecnico di Milano Dipartimento di Elettronica, Informazione e Bioingegneria
Organization and motivations 2 We complete the navigation part introducing the control problem and control techniques for mobile robots. The main topics on regulation and trajectory tracking are control review differentially flat systems exact linearization a trajectory tracking controller for a unicycle robot a regulation controller for a unicycle robot fundamental of odometric localization
Control review Lyapunov direct method 3 We start reviewing a fundamental tool to prove the stability of nonlinear control systems. Consider the following nonlinear time invariant system assuming ff 00 = 00, xx = 00 is an equilibrium state. We would like to introduce a tool to demonstrate the global asymptotic stability of the equilibrium of the nonlinear invariant system. The Lyapunov direct method is based on the following idea: if we can associate an energy-based description to an autonomous dynamic system, and for each system state, except for the equilibrium, the time derivative of the energy function is negative, then energy decreases along any system trajectory until it reaches its minimum at the equilibrium state this justifies an intuitive concept of stability.
Control review Lyapunov direct method 4 We introduce this energy-based function, as a scalar function VV xx of the system state, continuous together with its first derivative. VV xx is a Lyapunov function if The existence of such function ensures the global asymptotic stability of the equilibrium xx = 00. The equilibrium is globally asymptotically stable if a positive definite, radially unbounded function VV is found so that its time derivative along the system trajectories is negative definite.
Control review Lyapunov direct method 5 Let s introduce a quadratic form the time derivative is If ff xx renders the function VV xx negative definite, VV xx is a Lyapuov function and xx = 0 is globally asymptotically stable. If VV xx is not negative definite, nothing can be inferred on the stability of the system. One should resort to different choices of VV xx in order to find, if it is possible, a negative definite VV xx. The Lyapunov direct method gives only a sufficient condition.
Control review Lyapunov direct method (example) 6 Consider the equations of a pendulum with friction Let s introduce the following candidate Lyapunov function whose time derivative along system trajectories is VV xx is negative semidefinite but not negative definite, in fact VV xx = 0 for xx 2 = 0 irrespective of the value of xx 1.
Control review Lyapunov direct method (example) 7 Let s consider the physical system. VV xx is negative everywhere except on the line xx 2 = 0. According to the system dynamics, however assuming ππ < xx 1 < ππ, VV xx = 00 only for xx 1 = xx 2 = 0. Consequently, VV xx decreases toward 0 and, consequently, xx tt 0 as tt. This is consistent with the physical understanding that, due to friction, energy cannot remain constant while the system is in motion.
Control review Lyapunov direct method, La Salle s invariance principle 8 We can formalize the previous result as follows: if in a domain about the origin we can find a Lyapunov function whose derivative along the trajectories of the system is negative semidefinite, and if we can establish that no trajectory can stay identically at points where VV xx = 00 except at the origin, then the origin is globally asymptotically stable. This idea follows from La Salle s invariance principle. We still have one question to solve: La Salle s invariance principle does not hold for time varying systems, how to assess the stability of
Control review Lyapunov direct method, Barbalat lemma 9 Given a scalar function VV xx, tt so that VV xx, tt is lower bounded VV xx, tt 0 VV xx, tt is uniformly continuous then lim VV xx, tt = 0. tt 0 The last condition, that is difficult to verify, can be substituted by VV xx, tt is bounded Barbalat lemma can be used for time invariant systems as well, as an alternative to La Salle s invariance principle.
Differentially flat systems 10 We start introducing properties and tools that allow to transform a nonlinear dynamic system in a simpler form. The first property we introduce is called differential flatness. A nonlinear system is differentially flat if there exists a set of outputs yy, called flat outputs, such that the state xx and the control inputs uu can be expressed algebraically as a function of yy and its time derivatives up to a certain order
Differentially flat systems: the unicycle example 11 A unicycle kinematic model is a differentially flat system, when flat outputs zz 1 = xx and zz 2 = yy are selected. The state vector of the kinematic model can be expressed as a function of the flat outputs and their first derivatives
Differentially flat systems: the unicycle example 12 The input vector of the kinematic model can be expressed as a function of the flat outputs and their first and second order derivatives. Using the first two equations of the model we can derive a relation for the linear velocity (assuming vv > 0) From the third equation of the model and the flat relation on θθ we derive a relation for the angular velocity
Differentially flat systems: the unicycle example 13 Summarizing, the mappings obtained by the flat transformation are and We must observe that this mapping is singular when In this case the linear velocity is zero, and θθ and ωω are not well-defined.
Differentially flat systems: the bicycle example 14 A bicycle kinematic model is a differentially flat system, when flat outputs zz 1 = xx and zz 2 = yy are selected. The state vector of the kinematic model can be expressed as a function of the flat outputs and their first derivatives, as for the unicycle model
Differentially flat systems: the bicycle example 15 From the first two equations we get also and differentiating the first relation with respect to time From the last equation we now obtain
Differentially flat systems: the bicycle example 16 As for the input vector, the linear velocity can be obtained from the first two equations, as in the unicycle case (again assuming vv > 0) The second input can be instead derived from the last equation
Differentially flat systems 17 Once the states and inputs have been expressed in terms of the flat outputs, given an output trajectory yy(tt), the associated trajectory of the state xx and of the control inputs uu are uniquely determined.
Exact linearization of a nonlinear system: the unicycle kinematic model 18 The kinematic and dynamic models of mobile robots we have introduced so far are always represented by nonlinear dynamic systems. We now introduce a change of coordinates that transforms a nonlinear system into a linear one. We start considering the example of a unicycle model differentiating the first two equations with respect to time
Exact linearization of a nonlinear system: the unicycle kinematic model 19 Multiplying the two equations by cos θθ / sin θθ and summing them together Multiplying the two equations by sin θθ / cos θθ and subtracting them together
Exact linearization of a nonlinear system: the unicycle kinematic model 20 Summarizing, we found the two change of coordinates
Exact linearization of a nonlinear system: the unicycle kinematic model 21 Some remarks: the change of variables we have introduced is called feedback linearization as a result of the change of variables the closed-loop system (from aa xx to xx, or from aa yy to yy) is described by two independent double integrators the change of variables has a singularity when vv = 0 the change of variables is dynamic Can we introduce a different change of variables that is not affected by this singularity?
Exact linearization of a nonlinear system: the unicycle kinematic model wrt point P 22 Consider again the unicycle model, making reference to the motion of point PP and differentiating with respect to time We can now apply the same procedure used to derive the previous change of variable
Exact linearization of a nonlinear system: the unicycle kinematic model wrt point P 23 Multiplying the two equations by cos θθ / sin θθ and summing them together Multiplying the two equations by sin θθ / cos θθ and subtracting them together
Exact linearization of a nonlinear system: the unicycle kinematic model wrt point P 24 Summarizing, we found the two change of coordinates
Exact linearization of a nonlinear system: the unicycle kinematic model wrt point P 25 Some remarks: the change of coordinates has no more singularities the change of coordinates is static the closed-loop system is described by two independent integrators, it is thus more simple especially from a control point of view an outer control loop should regulate the position of point PP Can we use these changes of coordinates to linearize the bicycle kinematic model?
Exact linearization of a nonlinear system: the bicycle kinematic model 26 Consider a simplified version of the rear-wheel drive bicycle model We can apply a similar change of coordinates, considering that ωω = vv tan φφ. l Considering, for example, the linearization with respect to point PP, we have Let s concentrate on the second equation
Exact linearization of a nonlinear system: the bicycle kinematic model 27 Summarizing, the change of coordinates is and the closed-loop system is again characterized by two independent integrators But again the change of coordinates is singular when vv = 0.
Exact linearization of the unicycle an bicycle models: an interpretation 28 Before studying the linearization of a dynamic model, we would like to investigate more the exact linearization tool. In all the examples we started from a 3D configuration space, representing robot pose, and we end up with a reduced configuration space, representing robot position. From a physical point of view, the model in the new coordinates describes the motion of the robot as the motion of a particle. This particle can move in the 2D space and it is not subjected to any constraint. From a system theory point of view, the change of coordinates transforms a 3 rd order system into a 2 nd order system, it is thus a feedback that induces a loss of observability. For a robot model the heading is no more observable from the output.
Exact linearization of the unicycle an bicycle models: an interpretation 29 What are the main consequences? The linearizing feedback induces a hidden dynamics that can/cannot be asymptotically stable if it is not asymptotically stable the change of coordinates cannot be applied The heading is no more observable, an outer controller cannot control the heading of the robot Let s consider an example
Exact linearization of the unicycle an bicycle models: an interpretation 30 Consider the unicycle model linearized with respect to a point PP and the change of coordinates The closed-loop system is described by the following dynamic system
Exact linearization of the unicycle an bicycle models: an interpretation 31 We assume that PP moves at constant velocity along a straight line with vv PP, θθ PP constant and vv PP > 0. The equations of the closed-loop system become Focusing on the unobservable state, we define Δθθ = θθ θθ PP.
Exact linearization of the unicycle an bicycle models: an interpretation 32 The heading dynamics can be written as Unstable Asymptotically stable and the equilibria of this nonlinear system are θθ = θθ PP + 2kkkk, kk Z, asymptotically stable with a basin of attraction ( θθ PP + 2(kk
Exact linearization of a nonlinear system: the bicycle dynamic model 33 What about linearizing a dynamic model? We consider the example of the bicycle model As for the unicycle kinematic model we can linearize with respect to xx, yy or with respect to a point PP that does not belong to the vehicle. We consider the linearization with respect to a point PP.
Exact linearization of a nonlinear system: the bicycle dynamic model 34 Consider a point PP at a distance E from the vehicle CoG along the velocity vector and differentiating with respect to time
Exact linearization of a nonlinear system: the bicycle dynamic model 35 Introducing the sideslip dynamics in the previous equations we obtain the change of coordinates
Exact linearization of a nonlinear system: the bicycle dynamic model 36 Finally, from the definition of ωω we obtain Summarizing, if we apply the change of coordinates the bicycle dynamic model is transformed into two independent integrators
Exact linearization of a nonlinear system: the bicycle dynamic model 37 Some remarks: as for the kinematic model, the linearized system represent the motion of a particle sideslip and yaw dynamics are hidden by the change of coordinates the change of coordinates is static but is singular when VV = 0 the change of coordinates is now a function of the dynamic model parameters (MM, CC ff, CC rr, aa, bb), there can be robustness issues
Exact linearization of a nonlinear system: generic dynamic model 38 The generic expression of the dynamic model of a mobile robot we have introduced allows to devise a general but partial feedback linearization. Assuming a control availability assumption that is often satisfied, we can select where aa is the new control variable. The closed-loop system is reduced to mm independent integrators Kinematic model
Exact linearization of a nonlinear system: generic dynamic model 39 Some remarks: it is a general approach similar to the one used to linearize the model of a manipulator as for the previous linearization of dynamic models, the linearizing law entails model parameters, a robustness issue can be thus arise if the system is unconstrained and fully actuated the linearizing law reduces to and it is thus equivalent to an inverse dynamics control. The linearized system is equivalent to nn decoupled double integrators
The motion control problem 40 The motion control problem for a mobile robot can be formulated with respect to kinematic model dynamic model At least two reasons allow to go for the first option: Dynamic effects are handled by low level control systems or can be neglected (low velocities/accelerations) Dynamic effects are more important in autonomous vehicles than in mobile robotics dynamics can be cancelled out with a dynamic state feedback in the majority of the robots wheel torques cannot be accessed, as there are low level control loops integrated in the hardware architecture, and generalized velocities are usually the only accessible commands
The motion control problem 41 Two different control problems can be considered: trajectory or path tracking, the robot must asymptotically track a desired Cartesian path or trajectory posture regulation, the robot must asymptotically reach a given posture A planning step is not required, but the Cartesian trajectory of the robot cannot be predicted Forcing the robot to move along or close to a trajectory planned in advanced considerably reduces the risk of collisions
Deriving a trajectory tracking controller for the unicycle model 42 Let s assume that the desired trajectory xx dd tt, yy dd tt satisfies the unicycle kinematic model. Exploiting the flatness property we can compute the desired values for the angle θθ dd tt and for the linear and angular velocities vv dd tt and ωω dd tt and The role of a trajectory tracking controller is to ensure that the tracking error asymptotically vanishes. We thus need to define the tracking error
Deriving a trajectory tracking controller for the unicycle model 43 We could define the tracking error as but it is more convenient to project the positional part of the error onto a frame that has the same orientation of the robot local reference frame Let s compute the time derivative of these equations
Deriving a trajectory tracking controller for the unicycle model 44 Let s compute the time derivative of the first equation it follows Applying the same procedure to the second equation we obtain Finally
Deriving a trajectory tracking controller for the unicycle model 45 Summarizing Introducing now a change of variables on the inputs In general it is a time varying system We obtain linear term nonlinear terms
Deriving a trajectory tracking controller for the unicycle model 46 Starting from the system that describes the error dynamics we can derive two trajectory tracking controllers a linear controller based on the linearized error dynamics a nonlinear controller
A linear trajectory tracking controller for the unicycle model 47 A simple way to derive a linear controller, is by linearizing the error dynamics along the reference trajectory (ee = 00) we obtain a linear but time varying system. Introducing the control law we get a closed loop error dynamics described by the following linear time varying system
A linear trajectory tracking controller for the unicycle model 48 The characteristic polynomial of matrix AA tt is Assuming kk 1 = kk 3 we can rewrite the polynomial in this way The closed loop linearized error dynamics is characterized by a real pole kk 1 two real or complex poles
A linear trajectory tracking controller for the unicycle model 49 Let s assume that the second order polynomial is characterized by complex poles with natural frequency ωω nn and damping ξξ Summarizing, the closed loop linearized error dynamics is characterized by one real negative eigenvalue in 2ξξωω nn two complex eigenvalues with damping ξξ and natural frequency ωω nn
A linear trajectory tracking controller for the unicycle model 50 A few remarks: in general the closed loop linearized error dynamics is a time varying system, there is thus no guarantee that the system is asymptotically stable if vv dd and ωω dd are constant (circular and rectilinear trajectories), the linearized system is time invariant, and the choice of gains ensures it is asymptotically stabile. As a consequence, with the control law the origin of the original nonlinear error system is asymptotically stable (not globally!). For sufficiently small initial errors the unicycle converges to the desired trajectory.
A linear trajectory tracking controller for the unicycle model 51 in general the control law we have defined is linear but time varying the robot commands vv and ωω are given by the angular velocity ωω diverges when vv dd 0, this control law can be used only for persistent Cartesian trajectories for which with this control law motion inversions on the reference trajectory are not allowed
A nonlinear trajectory tracking controller for the unicycle model 52 Let s go back to the nonlinear error dynamics substituting the input transformation ωω = ωω dd uu 2 we obtain the nonlinear version of the previous control law
A nonlinear trajectory tracking controller for the unicycle model 53 where kk 1 vv dd, ωω dd > 0 and kk 3 vv dd, ωω dd > 0 are bounded functions with bounded derivatives kk 2 > 0 is constant If reference inputs vv dd and ωω dd are bounded with bounded derivatives do not both converge to zero the tracking error ee globally converges to zero (for any initial condition!).
A nonlinear trajectory tracking controller for the unicycle model 54 The corresponding velocity commands are given by Let s prove that the tracking error converges to zero globally.
A nonlinear trajectory tracking controller for the unicycle model 55 First of all the closed loop error dynamics are given by We consider the candidate Lyapunov function and its time derivative along the system trajectories that is negative semi-definite. As the system is time varying we have to use Barbalat lemma.
A nonlinear trajectory tracking controller for the unicycle model 56 Let s consider we have that VV xx, tt is lower bounded and ee is bounded VV xx, tt 0 VV xx, tt is uniformly continuous (or VV xx, tt is bounded) The system is time varying, we cannot use La Salle invariance principle and using Barbalat lemma we conclude that lim VV xx, tt = 0. This implies that ee 1 and ee 3 tt 0 tend to 0.
A nonlinear trajectory tracking controller for the unicycle model 57 Considering that ee 1 and ee 3 tend to zero, from system equations it follows that and thus ee 2 tends to zero as well, provided that at least one of the reference inputs is persistent.
A feedback linearization based trajectory tracking controller for the unicycle model 58 Remember that, making reference to the motion of point PP and introducing the feedback linearizing law we obtain The dynamics of point PP is now linear and can be controlled with a linear regulator
A feedback linearization based trajectory tracking controller for the unicycle model 59 A simple linear controller with kk 1, kk 2 > 0, guarantees exponential convergence to zero of the Cartesian tracking error with decoupled dynamics The orientation of the robot is not controlled and evolves according to but at least at steady state the equilibrium of this system is
Deriving a regulation controller for the unicycle model 60 The regulation problem is the problem of designing a feedback control law that drives the robot to a desired configuration qq dd (and stop there). None of the tracking controllers allow to solve this problem: the controller based on approximate linearization requires a persistent state trajectory the nonlinear controller requires a persistent state trajectory the feedback linearization based controller does not allow to control the final orientation at the destination point For these reasons, we now introduce two control laws for Cartesian and posture regulation.
A Cartesian regulation controller for the unicycle model 61 Consider the problem of controlling only the final position of the robot, without specifying the final orientation. Assume that the desired Cartesian position is the origin, the error is We can introduce the following control law Cartesian error projected on the sagittal axis where kk 1, kk 2 > 0. Let s analyze the stability of the closed loop system. Pointing error
A Cartesian regulation controller for the unicycle model 62 Consider the Lyapunov-like function that is only positive semi-definite at the origin (it is zero in all configurations such that xx = yy = 0, independently from the value of θθ). The time derivative along the system trajectories is that is negative semi-definite at the origin. We conclude that: VV xx, tt is lower bounded and ee PP is bounded VV xx, tt is uniformly continuous (or and Barbalat lemma implies that lim tt VV xx, tt is bounded) VV xx, tt = 0.
A Cartesian regulation controller for the unicycle model 63 From lim VV xx, tt = 0, it follows tt and thus the projection of the Cartesian error ee PP on the sagittal axis of the unicycle vanishes. This can happen only at the origin, otherwise the steering velocity would force the unicycle to rotate so as to align with ee PP. We can conclude that the Cartesian error tends to zero for any initial configuration.
A posture regulation controller for the unicycle model 64 We assume again that the desired configuration is the origin qq dd = 0 0 0 TT. We introduce the following variables to express the unicycle model in polar coordinates Let s start from the first equation
A posture regulation controller for the unicycle model 65 Summarizing Moving now to the second equation we get
A posture regulation controller for the unicycle model 66 Finally, for the last equation we get Summarizing, the unicycle model in polar coordinates is described by Note that the model is singular for ρρ = 0
A posture regulation controller for the unicycle model 67 We introduce the following feedback control law with kk 1, kk 2 > 0. Let s prove the asymptotic convergence of the closed loop system to the desired configuration qq dd.
A posture regulation controller for the unicycle model 68 Let s consider the following Lyapunov candidate The time derivative along the system trajectories is and is negative semi-definite.
A posture regulation controller for the unicycle model 69 We conclude that: VV xx, tt is lower bounded and the norm of the state is bounded VV xx, tt is uniformly continuous (or VV xx, tt is bounded) and Barbalat lemma implies that lim VV xx, tt = 0. tt As a consequence, ρρ and γγ tend to zero and and thus, to guarantee that qq dd is an equilibrium point, δδ must tend to zero.
Odometric localization 70 As we already know, to control a mobile robot we need to estimate in real-time the robot configuration at each time instant. This is called localization problem. Consider a unicycle robot moving under the action of velocity commands vv and ωω, constant within each sampling interval. In the sampling interval the robot moves along an arc of circle of radius RR = vv kk ωω kk a line segment if ωω kk = 0 Assume the robot configuration at time tt kk is known, qq tt kk vv kk and ωω kk in the interval [tt kk, tt kk+1 ). = qq kk, together with the inputs
Odometric localization 71 Using forward integration of the kinematic model with the Euler method, we can derive the configuration qq kk+1 at time tt kk+1 where TT ss = tt kk+1 tt kk. These relations are approximated as they assume θθ kk constant in the integration period This relation is exact
Odometric localization 72 A more accurate estimate can be achieved adopting the second-order Runge-Kutta integration method More complex and accurate methods can be devised. The average value of the orientation in the integration period We have now to relate our estimators to a set of available measurements, wheel encoder measurements.
Odometric localization 73 Consider, for example, the case of a differential drive robot. If Δφφ RR and Δφφ LL are the rotations of the wheels measured by the incremental encoders during a sampling interval The linear and angular velocities can be estimated as The method we have introduced is called odometric localization or passive localization or dead reckoning. This method is subject to an error that grows over time (drift) that quickly becomes significant over sufficiently long paths. This is due to several causes, including wheel slippage, inaccuracy in kinematic parameters, numerical errors in the integration,