Lyapunov Stability

The second method of Lyapunov will be introduced briefly. The method gives sufficient conditions for (asymptotic) stability of equilibrium points of a nonlinear dynamical system given by $\dot{\mathit{x}}=\mathit{f}(\mathit{x})$, $\mathit{x}\in \mathbb{R}$. First, it is assumed that the equilibrium lies at x = 0. The approach is based on positive definite scalar functions $V(\mathit{x}):{\mathbb{R}}^n\to \mathbb{R}$ that fulfill the following: V (x) = 0 if x = 0, and V (x) > 0 if x≠0. The stability of the equilibrium point is analyzed by checking the derivative of the function V. It is very important that the derivative is obtained along the solutions of the differential equation of the system:

$\begin{array}{l}\hfill \dot{V}=\frac{\partial V}{\partial \mathit{x}}\dot{\mathit{x}}=\frac{\partial V}{\partial \mathit{x}}\mathit{f}(\mathit{x})\end{array}$

If $\dot{V}\le 0$ ($\dot{V}$ is negative semidefinite), the equilibrium is (locally) stable. If $\dot{V}<0$ except at x = 0 ($\dot{V}$ is negative definite), the equilibrium is (locally) asymptotically stable. If ${lim}_{|\mathit{x}|\to \infty }V(\mathit{x})=\infty$, the results are global. The approach is therefore based on searching for the functions with the above properties that are referred to as Lyapunov functions. Very often a quadratic Lyapunov function candidate is chosen, and if we are able to show its derivative is negative or at least zero, stability of the system is concluded.

A classical interpretation of Lyapunov functions is based on the system energy point of view. If system energy is used as a Lyapunov function, and the system is dissipative, the energy in the system cannot increase (the derivative of the function is nonpositive). Consequently, all the signals remain bounded, and the system stability can be confirmed. But it needs to be stressed that a Lyapunov function might not be related to system energy. Above all, “system stability” is an incorrect term in the nonlinear framework. Rather, stability of equilibrium points, or more generally of invariant sets, is to be analyzed. It is easy to find the systems where stable and unstable equilibria coexist.

Control Design in the Lyapunov Stability Framework

We shall show in the following how the Lyapunov stability theorem can be used for control design purposes. Our nonlinear system (3.38) has three states with equilibrium points at e = 0. We would like to design a control that would make this point stable and if possible asymptotically stable, which means that all the trajectories would eventually converge to the reference trajectory and stay there forever. The most obvious Lyapunov function candidate is a sum of three squared errors:

$\begin{array}{l}\hfill V(\mathit{e})=\frac{k_y}{2}(e_x^2+e_y^2)+\frac{1}{2}{e}_{\phi }^2\end{array}$

which can be interpreted as a weighted sum of the squared error in distance and the squared error in orientation. A positive constant k_y has to be added because of different units, but it will later be shown that this constant plays an important role in the control law design. The time derivative of V is

$\begin{array}{l}\hfill \dot{V}=k_y{e}_x{ė}_x+k_y{e}_y{ė}_y+e_{\phi }{ė}_{\phi }\end{array}$

but this derivative has to be evaluated along the solutions of Eq. (3.38), which means that the error derivatives from Eq. (3.38) should be introduced

$\begin{array}{l}\hfill \begin{array}{cc}\dot{V}(\mathit{e})& =k_y{e}_x\left({\omega }_{\mathrm{ref}}{e}_y-v_{\mathrm{fb}}+e_y{\omega }_{\mathrm{fb}}\right)+k_y{e}_y\left(-{\omega }_{\mathrm{ref}}{e}_x+v_{\mathrm{ref}}sin{e}_{\phi }-e_x{\omega }_{\mathrm{fb}}\right)+e_{\phi }\left(-{\omega }_{\mathrm{fb}}\right)\\ & =-k_y{e}_x{v}_{\mathrm{fb}}+k_y{v}_{\mathrm{ref}}{e}_{y}sin{e}_{\phi }-e_{\phi }{\omega }_{\mathrm{fb}}\end{array}\end{array}$

The idea of Lyapunov-based control design is to make the derivative of the Lyapunov function negative by choosing the control laws properly. It is therefore obvious how the control could be constructed in this case. The linear velocity v_fb will make the first term on the right-hand side in Eq. (3.46) negative (by completing it to the negative square), while the angular velocity ω_fb will cancel the second term in Eq. (3.46) and will make the third one negative (by completing it to the negative square). The control law that achieves that is

$\begin{array}{l}\hfill \begin{array}{cc}{v}_{\mathrm{fb}}& =k_x{e}_x\\ {\omega }_{\mathrm{fb}}& =k_y{v}_{\mathrm{ref}}\frac{sin{e}_{\phi }}{e_{\phi }}{e}_y+k_{\phi }{e}_{\phi }\end{array}\end{array}$

This control law is a known and well-established control law from the literature [6, 10]. Now introducing the proposed control (3.47), $\dot{V}$ becomes

$\begin{array}{l}\hfill \dot{V}=-k_x{k}_y{e}_x^2-k_{\phi }{e}_{\phi }^2\end{array}$

Control gains are positive, and it will later be shown that k_x and k_φ can be arbitrary, uniformly continuous positive functions, while k_y is required to be a positive constant. The derivative of the Lyapunov function is clearly nonpositive, but since it is evaluated to zero at e_x = 0, e_φ = 0, irrespective of e_y, the derivative is negative semidefinite, and the equilibrium is stable. This result means that the error will stay bounded, but its convergence to 0 has not been proven.

The analysis of error convergence is considerably more difficult. In order to cope with it, we first need to introduce some additional mathematical tools. Signal norms will play an important role. The ${\mathcal{L}}_p$ norm of a function x(t) is defined as

$\begin{array}{l}\hfill {‖x‖}_p={\left({\int }_0^{\infty }\left|x(\tau )\right|{}^{p}d\tau \right)}^{1/p}\end{array}$

where |⋅| is the vector (scalar) length. If the above integral exists (is finite), the function x(t) is said to belong to ${\mathcal{L}}_p$. Limiting p toward infinity provides a very important class of functions ${\mathcal{L}}_{\infty }$: bounded functions.

Two very well-known lemmas will be used to prove the stability of control laws. The first one is Barbălat’s lemma and the other one is a derivation of Barbălat’s lemma. Both lemmas are taken from [11] and are given below for the sake of completeness.

Now, we are ready to address the problem of convergence of the errors in Eq. (3.38). Due to Eq. (3.48), $\dot{V}\le 0$, and therefore the Lyapunov function is nonincreasing and thus has the limit ${lim}_{t\to \infty }V(t)$. Consequently, the states of Eq. (3.38) are bounded:

$\begin{array}{l}\hfill e_x,e_y,e_{\phi }\in {\mathcal{L}}_{\infty }\end{array}$

Additionally, it follows from Eq. (3.47) that the control signals are bounded, and from Eq. (3.38) that the derivatives of the errors are bounded:

$\begin{array}{l}\hfill v_{\mathrm{fb}},{\omega }_{\mathrm{fb}},{ė}_x,{ė}_y,{ė}_{\phi }\in {\mathcal{L}}_{\infty }\end{array}$

where we also took into account that v_ref, ω_ref, k_x, and k_φ are bounded. The latter is true in the case of smooth reference trajectories (x_ref, y_ref, φ_ref).

In order to show asymptotic stability of Eq. (3.38), let us first calculate the following integral of $\dot{V}$ from Eq. (3.48):

$\begin{array}{l}\hfill {\int }_0^{\infty }\dot{V}dt=V(\infty )-V(0)=-{\int }_0^{\infty }{k}_x{k}_y{e}_x^{2}dt-{\int }_0^{\infty }{k}_{\phi }{e}_{\phi }^{2}dt\end{array}$

Since V is a positive definite function, the following inequality holds:

$\begin{array}{l}\hfill V(0)\ge {\int }_0^{\infty }{k}_x{k}_y{e}_x^{2}dt+{\int }_0^{\infty }{k}_{\phi }{e}_{\phi }^{2}dt\ge {\underset{\_}{k}}_x{k}_y{\int }_0^{\infty }{e}_x^{2}dt+{\underset{\_}{k}}_{\phi }{\int }_0^{\infty }{e}_{\phi }^{2}dt\end{array}$

where the lower bounds of functions k_x(t) and k_φ(t) have been introduced:

$\begin{array}{l}\hfill \begin{array}{c}{k}_x(t)\ge {\underset{\_}{k}}_x>0\\ k_{\phi }(t)\ge {\underset{\_}{k}}_{\phi }>0\end{array}\end{array}$

It follows from Eq. (3.53) that the errors e_x(t) and e_φ(t) belong to ${\mathcal{L}}_2$. Based on Lemma 3.2 it is easy to show that the errors e_x(t) and e_φ(t) converge to 0. Since the limit ${lim}_{t\to \infty }V(t)$ exists, then also ${lim}_{t\to \infty }{e}_y(t)$ exists.

We have seen that the convergence of the errors e_x(t) and e_φ(t) to 0 is relatively simple to show and the conditions for convergence are relatively mild—control gains and reference trajectories have to be bounded. Convergence of e_y to 0 is more difficult to show, and the requirements are much more difficult to achieve, as is shown next. Besides the control gains being uniformly continuous, the reference velocities have to be persistently exciting, which means that either v_ref or ω_ref should not limit to 0. Two cases will therefore be distinguished. In the first one, $v_{\mathrm{ref}}\nRightarrow 0$ will be assumed, and in the second, ${\omega }_{\mathrm{ref}}\nRightarrow 0$ will be assumed.

Now, it will be assumed that ${lim}_{t\to \infty }{v}_{\mathrm{ref}}(t)\ne 0$. Applying Lemma 3.1 on ${ė}_{\phi }(t)$ from Eq. (3.38) ensures that ${lim}_{t\to \infty }{ė}_{\phi }(t)=0$ since ${lim}_{t\to \infty }{e}_{\phi }(t)$ exists and is finite and ${ė}_{\phi }(t)$ is uniformly continuous. The latter is true due to Eq. (3.38) if ω_fb is uniformly continuous. The easiest way to check the uniform continuity of f(t) on $[0,\infty )$ is to see if $f,\dot{f}\in {\mathcal{L}}_{\infty }$. It has already been shown that e_y and e_φ are uniformly continuous, while the control gain k_φ and the reference velocity v_ref are uniformly continuous by the assumption, so it follows from Eq. (3.47) that ${ė}_{\phi }(t)$ is also uniformly continuous. The statement ${lim}_{t\to \infty }{ė}_y(t)=0$ (which is identical to ${lim}_{t\to \infty }{\omega }_{\mathrm{fb}}(t)=0$) has therefore been proven. The convergence of e_y to 0 follows from Eq. (3.47):

$\begin{array}{l}\hfill \begin{array}{cc}& e_{\phi }\to 0,k_{\phi }\in {\mathcal{L}}_{\infty },{\omega }_{\mathrm{fb}}\to 0\Rightarrow k_y{v}_{\mathrm{ref}}\frac{sin{e}_{\phi }}{e_{\phi }}{e}_y\to 0\\ & k_y{v}_{\mathrm{ref}}\frac{sin{e}_{\phi }}{e_{\phi }}{e}_y\to 0,\frac{sin{e}_{\phi }}{e_{\phi }}\to 1,k_y>0,v_{\mathrm{ref}}\to 0\Rightarrow e_y\to 0\end{array}\end{array}$

Now, it will be assumed that ${lim}_{t\to \infty }{\omega }_{\mathrm{ref}}(t)\ne 0$. Again one has to guarantee that ${lim}_{t\to \infty }{\omega }_{\mathrm{fb}}=0$. This is true if v_ref and k_φ are uniformly continuous, as shown before. Then Barbălat’s lemma (Lemma 3.1) is applied on ${ė}_x$ in Eq. (3.38). It has already been shown that e_x, e_y, and ω_fb are uniformly continuous; v_fb is uniformly continuous since k_x is uniformly continuous by the assumption; and ω_ref is also continuous from the assumption of this paragraph. This proves the statement ${lim}_{t\to \infty }{ė}_x(t)=0$. Like in the last paragraph, it can be concluded that the last two terms in Eq. (3.38) for ${ė}_x$ go to 0 as t goes to infinity. Consequently, the product ω_refe_y also goes to 0. Since ω_ref is persistently exciting and does not go to 0, e_y has to go to 0.

Once again it should be stressed that for convergence of e_x and e_φ, only boundedness of v_ref or ω_ref is required. A considerably more difficult task is to drive e_y to 0. This is achieved by persistent excitation from either v_ref or ω_ref. All the results are valid globally, which means that the convergence is guaranteed irrespective of the initial pose.

Example 3.10

A differentially driven vehicle is to be controlled to follow the reference trajectory $x_{\mathrm{ref}}=1.1+0.7sin(\frac{2\pi t}{30})$ and $y_{\mathrm{ref}}=0.9+0.7sin(\frac{4\pi t}{30})$. Sampling time is T_s = 0.033 s. The initial pose is [x(0), y(0), φ(0)] = [1.1, 0.8, 0]. Implement the control algorithm presented in this section in a Matlab code, experiment with control gains, and show the graphical results.

Solution

The code is given in Listing 3.9. Simulation results of Example 3.10 are shown in Figs. 3.24 and 3.25 where good tracking can be observed. Note that any positive functions can be used for k_x(t) and k_φ(t). Here the functions that are chosen make the linearized model of the system the same as in the case of the linear tracking controller (Example 3.9). Control laws are not the same, but they become the same in the limit case ($e_{\phi }\to 0$). The result of this choice is that the shape of transients is the same near the reference trajectory irrespective of the reference velocities.

Listing 3.9

1 Ts =  0.033;   %  Sampling time

2 t  =   0: Ts : 3 0 ;   %  Simulation time

3 q  =   [ 1 . 1 ;   0 . 8 ;   0 ] ;   %   Initial  robot pose

4

5 %  Reference

6 freq   =  2*pi/30;

7 xRef  =   1.1   +  0.7*sin( freq *t ) ;  yRef  =   0.9   +  0.7*sin(2* freq *t ) ;

8 dxRef  =   freq *0.7*cos( freq *t ) ;  dyRef  =  2* freq *0.7* cos(2* freq *t ) ;

9 ddxRef =− freq ˆ2*0.7* sin( freq *t ) ;  ddyRef  =−4* freq ˆ2*0.7*   sin (2* freq *t ) ;

10 qRef  =   [ xRef ;  yRef ;  atan2( dyRef ,  dxRef ) ] ;   %  Reference trajectory

11 vRef  =  sqrt( dxRef.ˆ2+dyRef .ˆ2) ;

12 wRef  =  ( dxRef .* ddyRef− dyRef .* ddxRef ) . / ( dxRef .ˆ2+dyRef .ˆ2) ;

13 uRef  =   [ vRef ;  wRef ] ;   %  Reference inputs

14

15 for k  =   1:length( t )

16 e  =   [cos(q (3) ) , sin(q (3) ) ,  0;   . . .

17 − sin ( q (3) ) ,  cos(q (3) ) ,  0;   . . .

18 0 , 0 ,  1]*( qRef ( : , k)  −   q ) ;   %  Error vector

19 e (3)   =  wrapToPi( e (3) ) ;   %  Correct angle

20

21  %  Current reference inputs

22 vRef  =  uRef (1 ,k) ;

23 wRef  =  uRef (2 ,k) ;

24

25  %  Control

26 zeta  =   0 . 9 ;   %  Experiment with this control design parameter

27 g  =   85;   %  Experiment with this control design parameter

28 Kx  =  2* zeta *sqrt(wRefˆ2  +  g*vRef ˆ2) ;

29 Kphi  =  Kx;

30 Ky  =  g ;

31  %  Gains Kx  and Kphi could also be constant.

32  %  This form  i s  used to have the same damping of the transient

33  %   irrespective  of ref.  velocities .

34

35  %  Control: feedforward and feedback

36 v  =  vRef*cos( e (3) )  +  Kx*e (1) ;

37  w   =  wRef  +  Ky*vRef* sinc ( e (3) /pi)*e (2)   +  Kphi*e (3) ;

38

39  %  Robot motion simulation

40 dq  =   [ v*cos(q (3) ) ;  v*sin(q (3) ) ;   w] ;

41  noise   =   0.00;   %  Set to experiment with noise (e.g. 0.001)

42 q  =  q  +  Ts*dq  +  randn(3 ,1) * noise ;   %  Euler integration

43 q (3)   =  wrapToPi(q (3) ) ;   %  Map  orientation angle to [− pi , pi]

44 end

Periodic Control Law Design

The problem of tracking is clearly periodic with respect to the orientation. This can be observed from the kinematic model by using an arbitrary control input and an arbitrary initial condition, resulting in a certain robot trajectory. If the same control input is applied to the robot and the initial condition only differs from the previous one by a multiple of 2π, the same response is obtained for x(t) and y(t), while φ(t) differs from the previous solution for the same multiple of 2π. The periodic nature should also be reflected in the control law used for the tracking. This should mean that one searches for a control law that is periodic with respect to the error in the orientation e_φ (the period is 2π) and ensures the convergence of the posture error e to one of the points ${\left[\begin{array}{lll}0& 0& 2k\pi \\ \end{array}\right]}^T(k\in \mathbb{Z})$. Thus, all the usual problems with orientation mapping to the (−π, π] are alleviated. These problems can become critical around ±180 degree in certain applications, for example, when using an observer to estimate the robot pose from the delayed measurements. Note that certain control laws are periodic in the sense discussed above, for example, feedback linearization control law given by Eqs. (3.25), (3.34) is periodic.

Obviously, the functions that are used in the section for the convergence analysis should also be periodic in e_φ. This means that these functions have multiple local minima and therefore do not satisfy the properties of the classic Lyapunov functions. Although the stability analysis resembles Lyapunov’s direct method (the second method of Lyapunov), the convergence is not proven by this stability theory because the convergence of e to zero is not needed in our approach. Nevertheless, the functions used in this section for the convergence analysis will still be referred to as “Lyapunov functions.”

Our goal is to bring the position error to zero, while the orientation error should converge to any multiple of 2π. In order to do this, a Lyapunov function that is periodic with respect to e_φ (with a natural period of 2π) will be used. First, the concept will be shown on one Lyapunov function, and later this will be extended to a more general case. The first Lyapunov-function candidate is chosen as

$\begin{array}{l}\hfill V=\frac{k_y}{2}\left(e_x^2+e_y^2\right)+\frac{1}{2}{\left(\frac{tan\frac{e_{\phi }}{2}}{\frac{1}{2}}\right)}^2\end{array}$

where k_y is a positive constant. Its derivative along the solutions of Eq. (3.38) is the following:

$\begin{array}{ll}\hfill \dot{V}& =k_y{e}_x\left({\omega }_{\mathrm{ref}}{e}_y-v_{\mathrm{fb}}+e_y{\omega }_{\mathrm{fb}}\right)\hfill \\ \hfill & +k_y{e}_y\left(-{\omega }_{\mathrm{ref}}{e}_x+v_{\mathrm{ref}}sin{e}_{\phi }-e_x{\omega }_{\mathrm{fb}}\right)-2\frac{tan\frac{e_{\phi }}{2}}{{cos}^2\frac{e_{\phi }}{2}}{\omega }_{\mathrm{fb}}\hfill \\ \hfill & =-k_y{e}_x{v}_{\mathrm{fb}}+k_y{v}_{\mathrm{ref}}{e}_{y}sin{e}_{\phi }-2\frac{tan\frac{e_{\phi }}{2}}{{cos}^2\frac{e_{\phi }}{2}}{\omega }_{\mathrm{fb}}\hfill \end{array}$

If the following control law is applied

$\begin{array}{l}\hfill \begin{array}{cc}{v}_{\mathrm{fb}}& =k_x{e}_x\\ {\omega }_{\mathrm{fb}}& =k_y{v}_{\mathrm{ref}}{e}_y{cos}^4\frac{e_{\phi }}{2}+k_{\phi }sin{e}_{\phi }\end{array}\end{array}$

where k_x and k_φ are positive bounded functions, the derivative $\dot{V}$ from Eq. (3.57) becomes

$\begin{array}{l}\hfill \dot{V}=-k_x{k}_y{e}_x^2-k_{\phi }{\left(\frac{tan\frac{e_{\phi }}{2}}{\frac{1}{2}}\right)}^2\end{array}$

Following the same lines as in the analysis of the control law (3.47) it can easily be concluded that e_x and $tan\frac{e_{\phi }}{2}$ converge to 0 (this means that $e_{\phi }\to 2k\pi$, $k\in \mathbb{Z}$) in the case of bounded control gains and a bounded trajectory. The convergence of e_y to 0 can also be concluded after a lengthy analysis if the same conditions are met as in the case of the control law (3.47).

Example 3.11

A differentially driven vehicle is to be controlled to follow the reference trajectory $x_{\mathrm{ref}}=1.1+0.7sin(\frac{2\pi t}{30})$ and $y_{\mathrm{ref}}=0.9+0.7sin(\frac{4\pi t}{30})$. Sampling time is T_s = 0.033 s. The initial pose is [x(0), y(0), φ(0)] = [1.1, 0.8, 0]. Implement the control algorithm presented in this section in a Matlab code, experiment with control gains, and show the graphical results.

Solution

The code is given in Listing 3.10. Simulation results are shown in Figs. 3.26 and 3.27, where good tracking can be observed.

Listing 3.10

1 Ts =  0.033;   %  Sampling time

2 t  =   0: Ts : 3 0 ;   %  Simulation time

3 q  =   [ 1 . 1 ;   0 . 8 ;   0 ] ;   %   Initial  robot pose

4

5 %  Reference

6 freq   =  2*pi/30;

7 xRef  =   1.1   +  0.7*sin( freq *t ) ;  yRef  =   0.9   +  0.7*sin(2* freq *t ) ;

8 dxRef  =   freq *0.7*cos( freq *t ) ;  dyRef  =  2* freq *0.7*cos(2* freq *t ) ;

9 ddxRef =− freq ˆ2*0.7* sin( freq *t ) ;  ddyRef  =−4* freq ˆ2*0.7* sin (2* freq *t ) ;

10 qRef  =   [ xRef ;  yRef ;  atan2( dyRef ,  dxRef ) ] ;   %  Reference trajectory

11 vRef  =  sqrt( dxRef.ˆ2+dyRef .ˆ2) ;

12 wRef  =  ( dxRef .* ddyRef− dyRef .* ddxRef ) . / ( dxRef .ˆ2+dyRef .ˆ2) ;

13 uRef  =   [ vRef ;  wRef ] ;   %  Reference inputs

14

15 for k  =   1:length( t )

16 e  =   [cos(q (3) ) , sin(q (3) ) ,  0;   . . .

17 − sin ( q (3) ) ,  cos(q (3) ) ,  0;   . . .

18 0 , 0 ,  1]*( qRef ( : , k)  −   q ) ;   %  Error vector

19 e (3)   =  wrapToPi( e (3) ) ;   %  Correct angle

20

21  %  Current reference inputs

22 vRef  =  uRef (1 ,k) ;

23 wRef  =  uRef (2 ,k) ;

24

25  %  Control

26 eX  =  e (1) ;  eY  =  e (2) ;  ePhi  =  e (3) ;

27 zeta  =   0 . 9 ;   %  Experiment with this control design parameter

28 g  =   85;   %  Experiment with this control design parameter

29 Kx  =  2* zeta *sqrt(wRefˆ2+g*vRef ˆ2) ;

30 Kphi  =  Kx;

31 Ky  =  g ;

32  %  Gains Kx  and Kphi could also be constant.

33  %  This form  i s  used to have the same damping of the transient

34  %   irrespective  of ref.  velocities .

35

36  %  Control: feedforward and feedback

37 v  =  vRef*cos( e (3) )  +  Kx*eX;

38  w   =  wRef  +  Ky*vRef *(cos( ePhi /2) ) ˆ4*eY  +  Kphi*sin( ePhi ) ;

39

40  %  Robot motion simulation

41 dq  =   [ v*cos(q (3) ) ;  v*sin(q (3) ) ;   w] ;

42  noise   =   0.00;   %  Set to experiment with noise (e.g. 0.001)

43 q  =  q  +  Ts*dq  +  randn(3 ,1) * noise ;   %  Euler integration

44 q (3)   =  wrapToPi(q (3) ) ;   %  Map  orientation angle to [− pi , pi]

45 end

A well-known control law by Kanayama et al. [9] can also be analyzed in the proposed framework. Choosing the Lyapunov function as

$\begin{array}{l}\hfill V=\frac{k_y}{2}\left(e_x^2+e_y^2\right)+\frac{1}{2}{\left(\frac{sin\frac{e_{\phi }}{2}}{\frac{1}{2}}\right)}^2\end{array}$

and using the control law proposed in [9] (there was also a third factor |v_ref| in the second term of ω_fb that can also be included in k_φ),

$\begin{array}{l}\hfill \begin{array}{cc}{v}_{\mathrm{fb}}& =k_x{e}_x\\ {\omega }_{\mathrm{fb}}& =k{v}_{\mathrm{ref}}{e}_y+k_{\phi }sin{e}_{\phi }\end{array}\end{array}$

results in a stable error-system where the convergence of all the errors can be shown under the same conditions as before. Note that beside stable equilibria at e_φ = 2kπ, $k\in \mathbb{Z}$, there is also an unstable or repelling equilibrium at e_φ = (2k + 1)π, $k\in \mathbb{Z}$.

A framework for periodic control law design is presented in [12]. Perhaps it is worth mentioning that it is quite simple to extend the proposed techniques to the control design for symmetric vehicles that can move in the forward and the backward direction during normal operation. In this case Lyapunov functions should be periodic with a period of π on e_φ.

Four-State Error Model of the System

In this section we will tackle the same problem as in the previous section, that is, from the control point of view we often want to track any robot pose that is different from the reference one for a multiple of 360 degree. The model (3.38) does not make this problem easier because the orientation error should be usually driven to 0 using Eq. (3.38). In this section a kinematic model of the system is presented where all the poses that differ in orientation for a multiple of 360 degree are presented as one. This can be achieved by extended the state vector for one element. The variable φ(t) from the original kinematic model (2.2) is exchanged by two new variables, $s(t)=sin(\phi (t))$ and $c(t)=cos(\phi (t))$. Their derivatives are

$\begin{array}{l}\hfill \begin{array}{cc}\dot{s}(t)& =cos(\phi (t))\dot{\phi }(t)=c(t)\omega (t)\\ ċ(t)& =-sin(\phi (t))\dot{\phi }(t)=-s(t)\omega (t)\\ \end{array}\end{array}$

The new kinematic model is then obtained:

$\begin{array}{l}\hfill \dot{\mathit{q}}=\left[\begin{array}{l}\dot{x}\\ \dot{y}\\ \dot{s}\\ ċ\\ \end{array}\right]=\left[\begin{array}{ll}c& 0\\ s& 0\\ 0& c\\ 0& -s\\ \end{array}\right]\left[\begin{array}{l}v\\ \omega \\ \end{array}\right]\end{array}$

The new error states are defined as follows:

$\begin{array}{l}\hfill \begin{array}{cc}{e}_x& =c(x_{\mathrm{ref}}-x)+s(y_{\mathrm{ref}}-y)\\ e_y& =-s(x_{\mathrm{ref}}-x)+c(y_{\mathrm{ref}}-y)\\ e_s& =sin({\phi }_{\mathrm{ref}}-\phi )=csin{\phi }_{\mathrm{ref}}-scos{\phi }_{\mathrm{ref}}\\ e_{\cos }& =cos({\phi }_{\mathrm{ref}}-\phi )=ccos{\phi }_{\mathrm{ref}}+ssin{\phi }_{\mathrm{ref}}\end{array}\end{array}$

After the differentiation of Eq. (3.64) and some manipulations, the following system is obtained:

$\begin{array}{l}\hfill \begin{array}{cc}{ė}_x& =v_{\mathrm{ref}}{e}_{\cos }-v+e_y\omega \\ {ė}_y& =v_{\mathrm{ref}}{e}_s-e_x\omega \\ {ė}_s& ={\omega }_{\mathrm{ref}}{e}_{\cos }-e_{\cos }\omega \\ {ė}_{\cos }& =-{\omega }_{\mathrm{ref}}{e}_s+e_s\omega \end{array}\end{array}$

Like in Eq. (3.37), v = v_refe_cos + v_fb and ω = ω_ref + ω_fb will be used in the control law. The control goal is to drive e_x, e_y, and e_s to 0. The variable e_cos is obtained as the cosine of the error in the orientation and should be driven to 1. This is why a new error will be defined as e_c = e_cos − 1 and the final error model of the system is now

$\begin{array}{l}\hfill \begin{array}{cc}{ė}_x& ={\omega }_{\mathrm{ref}}{e}_y-v_{\mathrm{fb}}+e_y{\omega }_{\mathrm{fb}}\\ {ė}_y& =-{\omega }_{\mathrm{ref}}{e}_x+v_{\mathrm{ref}}{e}_s-e_x{\omega }_{\mathrm{fb}}\\ {ė}_s& =-e_{\mathrm{c}}{\omega }_{\mathrm{fb}}-{\omega }_{\mathrm{fb}}\\ {ė}_{\mathrm{c}}& =e_s{\omega }_{\mathrm{fb}}\end{array}\end{array}$

A controller that achieves asymptotic stability of the error model (3.66) will be developed based on a Lyapunov approach. A very straightforward idea would be to use a Lyapunov function of the type

$\begin{array}{l}\hfill V_0=\frac{k}{2}\left(e_x^2+e_y^2\right)+\frac{1}{2}\left(e_s^2+e_{\mathrm{c}}^2\right)\end{array}$

Interestingly, this Lyapunov function results in the control law (3.61). However, a slightly more complex function will be proposed here, which also includes the function (3.67) as a special case. The following Lyapunov-function candidate is proposed to achieve the control goal:

$\begin{array}{l}\hfill V=\frac{k}{2}\left(e_x^2+e_y^2\right)+\frac{1}{2\left(1+\frac{e_{\mathrm{c}}}{a}\right)}\left(e_s^2+e_{\mathrm{c}}^2\right)\end{array}$

where k > 0 and a > 2 are constants. Note that the range of the function $e_{\mathrm{c}}=cos({\phi }_{\mathrm{ref}}-\phi )-1$ is [−2, 0], and therefore

$\begin{array}{l}\hfill \begin{array}{cc}0& <\frac{a-2}{a}\le 1+\frac{e_{\mathrm{c}}}{a}\le 1\\ 1& \le \frac{1}{1+\frac{e_{\mathrm{c}}}{a}}\le \frac{a}{a-2}\end{array}\end{array}$

Due to Eq. (3.69) the function V in Eq. (3.68) is lower-bounded by the function V₀ in Eq. (3.67), and V fulfills the conditions for the Lyapunov function. The role of $(1+\frac{e_{\mathrm{c}}}{a})$ will be explained later on. The function V can be simplified by using the following:

$\begin{array}{l}\hfill e_s^2+e_{\mathrm{c}}^2=e_s^2+{(e_{\cos }-1)}^2=2-2e_{\cos }=-2e_{\mathrm{c}}\end{array}$

Taking into account the equations of the error model (3.66), (3.70), the derivative of V in Eq. (3.68) is

$\begin{array}{ll}\hfill \dot{V}& =-k{e}_x{v}_{\mathrm{fb}}+k{v}_{\mathrm{ref}}{e}_y{e}_s+\frac{1}{2\left(1+\frac{e_{\mathrm{c}}}{a}\right)}(-2e_s{\omega }_{\mathrm{fb}})\hfill \\ \hfill & +\frac{-\frac{1}{a}{e}_s{\omega }_{\mathrm{fb}}(-2e_{\mathrm{c}})}{2{\left(1+\frac{e_{\mathrm{c}}}{a}\right)}^2}=-k{e}_x{v}_{\mathrm{fb}}+e_s\left(k{v}_{\mathrm{ref}}{e}_y-\frac{{\omega }_{\mathrm{fb}}}{{\left(1+\frac{e_{\mathrm{c}}}{a}\right)}^2}\right)\hfill \end{array}$

In order to make $\dot{V}$ negative semidefinite, the following control law is proposed:

$\begin{array}{l}\hfill \begin{array}{cc}{v}_{\mathrm{fb}}& =k_x{e}_x\\ {\omega }_{\mathrm{fb}}& =k{v}_{\mathrm{ref}}{e}_y{\left(1+\frac{e_{\mathrm{c}}}{a}\right)}^2+k_s{e}_s{\left[{\left(1+\frac{e_{\mathrm{c}}}{a}\right)}^2\right]}^n\end{array}\end{array}$

where k_x(t) and k_s(t) are positive functions, while $n\in \mathbb{Z}$. For practical reasons n is a small number (usually − 2, − 1, 0, 1, or 2 are good choices). By taking into account the control law (3.72), the function $\dot{V}$ becomes

$\begin{array}{l}\hfill \dot{V}=-k{k}_x{e}_x^2-k_s{e}_s^2{\left[{\left(1+\frac{e_{\mathrm{c}}}{a}\right)}^2\right]}^{n-1}\end{array}$

It is again very simple to show the convergence of e_x and e_s based on Eq. (3.73). The convergence of e_y and e_c is again a little more difficult [13].

Example 3.12

A differentially driven vehicle is to be controlled to follow the reference trajectory $x_{\mathrm{ref}}=1.1+0.7sin(\frac{2\pi t}{30})$ and $y_{\mathrm{ref}}=0.9+0.7sin(\frac{4\pi t}{30})$. Sampling time is T_s = 0.033 s. The initial pose is [x(0), y(0), φ(0)] = [1.1, 0.8, 0]. Implement the control algorithm presented in this section in a Matlab code. Experiment with control gains and additional control design parameters (a and n).

Solution

The code is given in Listing 3.11. Simulation results are shown in Figs. 3.28 and 3.29 where good tracking can be observed.

1 Ts =  0.033;   %  Sampling time

2 t  =   0: Ts : 3 0 ;   %  Simulation time

3 q  =   [ 1 . 1 ;   0 . 8 ;   0 ] ;   %   Initial  robot pose

4

5 %  Reference

6 freq   =  2*pi/30;

7   xRef  =   1.1   +  0.7*sin( freq *t ) ;  yRef  =   0.9   +  0.7*sin(2* freq *t ) ;

8  dxRef  =   freq *0.7*cos( freq *t ) ;  dyRef  =  2* freq *0.7*cos(2* freq *t ) ;

9 ddxRef =− freq ˆ2*0.7* sin( freq *t ) ;  ddyRef  =−4* freq ˆ2*0.7* sin (2* freq *t ) ;

10 qRef  =   [ xRef ;  yRef ;  atan2( dyRef ,  dxRef ) ] ;   %  Reference trajectory

11 vRef  =  sqrt( dxRef.ˆ2+dyRef .ˆ2) ;

12 wRef  =  ( dxRef .* ddyRef− dyRef .* ddxRef ) . / ( dxRef .ˆ2+dyRef .ˆ2) ;

13 uRef  =   [ vRef ;  wRef ] ;   %  Reference inputs

14

15 for k  =   1:length( t )

16     e  =   [cos(q (3) ) , sin(q (3) ) ,  0;   . . .

17       − sin ( q (3) ) ,  cos(q (3) ) ,  0;   . . .

18        0 , 0 ,  1]*( qRef ( : , k)  −   q ) ;   %  Error vector

19     eX  =  e (1) ;  eY  =  e (2) ;   %  Two  errors (due to distance)

20     eS  =  sin( e (3) ) ;  eCos  =  cos( e (3) ) ;  eC  =  eCos −   1;   %  Orientation errors

21

22     %  Current reference inputs

23     vRef  =  uRef (1 ,k) ;

24     wRef  =  uRef (2 ,k) ;

25

26     %  Control

27     zeta  =   0 . 9 ;   %  Experiment with this control design parameter

28     g  =   85;   %  Experiment with this control design parameter

29     a  =   10;   %  Experiment with this control design parameter

30     n  =   2;   %  Experiment with this parameter (integer)

31     Kx  =  2* zeta *sqrt(wRefˆ2+g*vRef ˆ2) ;

32     Ks  =  Kx;

33     K   =  g ;

34     %  Gains Kx  and Ks could also be constant.

35     %  This form  i s  used to have the same damping of the transient

36     %   irrespective  of ref.  velocities .

37

38     %  Control: feedforward and feedback

39     v  =  vRef*cos( e (3) )  +  Kx*eX;

40     w   =  wRef  +   K *vRef*eY*(1+eC/a) ˆ2  +  Ks*eS*(1+eC/a) ˆ(2*n) ;

41

42     %  Robot motion simulation

43     dq  =   [ v*cos(q (3) ) ;  v*sin(q (3) ) ;   w] ;

44     noise   =   0.00;   %  Set to experiment with noise (e.g. 0.001)

45     q  =  q  +  Ts*dq  +  randn(3 ,1) * noise ;   %  Euler integration

46     q (3)   =  wrapToPi(q (3) ) ;   %  Map  orientation angle to [− pi , pi]

47 end

Takagi-Sugeno Fuzzy Error Model of a Differentially Driven Wheeled Mobile Robot

The Takagi-Sugeno (or TS) models have their roots in fuzzy logic where the model is given in the form of if-then rules. A TS model can also be represented in a more compact polytopic form [14]:

$\begin{array}{l}\hfill \begin{array}{cc}\dot{\mathit{\xi }}\left(t\right)& =\sum _{i=1}^r{h}_i\left(\mathit{z}\left(t\right)\right)\left({\mathit{A}}_i\mathit{\xi }\left(t\right)+{\mathit{B}}_i\mathit{u}\left(t\right)\right)\\ \mathit{\upsilon }\left(t\right)& =\sum _{i=1}^r{h}_i\left(\mathit{z}\left(t\right)\right)\left({\mathit{C}}_i\mathit{\xi }\left(t\right)\right)\end{array}\end{array}$

where $\mathit{\xi }\left(t\right)\in {\mathbb{R}}^n$ is a state vector, $\mathit{\upsilon }\left(t\right)\in {\mathbb{R}}^p$ is an output vector, and $\mathit{z}\left(t\right)\in {\mathbb{R}}^q$ is the premise vector depending on the state vector (or some other quantity), A_i, B_i, and C_i are constant matrices. And finally, the nonlinear weighting functions $h_i\left(\mathit{z}\left(t\right)\right)$ are all nonnegative and such that ${\sum }_{i=1}^r{h}_i\left(\mathit{z}\left(t\right)\right)=1$ for an arbitrary z(t). For any nonlinear model, one can find an equivalent fuzzy TS model in a compact region of the state space variable using the sector nonlinearity approach, which consists of decomposing each bounded nonlinear term in a convex combination of its bounds [16]. The number of rules r is then related to the number of the nonlinearities of the model, as shown later.

In this section we will make use of the fact that in the wheeled robot error model the nonlinear functions are known a priori, which makes the use of the aforementioned concept possible. The nonlinear tracking error model (3.38) will therefore be rewritten in the equivalent matrix form:

$\begin{array}{l}\hfill \left[\begin{array}{l}{ė}_x\\ {ė}_y\\ {ė}_{\phi }\end{array}\right]=\left[\begin{array}{lll}0& {\omega }_{\mathrm{ref}}& 0\\ -{\omega }_{\mathrm{ref}}& 0& v_{\mathrm{ref}}\frac{sin{e}_{\phi }}{e_{\phi }}\\ 0& 0& 0\\ \end{array}\right]\left[\begin{array}{l}{e}_x\\ e_y\\ e_{\phi }\end{array}\right]+\left[\begin{array}{ll}-1& e_y\\ 0& -e_x\\ 0& -1\\ \end{array}\right]\left[\begin{array}{l}{v}_{\mathrm{fb}}\\ {\omega }_{\mathrm{fb}}\\ \end{array}\right]\end{array}$

Four bounded nonlinear functions appear in this model: ω_ref, $v_{\mathrm{ref}}\frac{sin{e}_{\phi }}{e_{\phi }}$ (or with different notation v_rsinc(φ)), e_y, and e_x. The premise vector is therefore:

$\begin{array}{l}\hfill \mathit{z}(t)=\left[\begin{array}{l}{\omega }_{\mathrm{ref}}\\ v_{\mathrm{ref}}(t)sinc\left(e_{\phi }(t)\right)\\ e_y(t)\\ e_x(t)\end{array}\right]\end{array}$

First, the controllability of Eq. (3.75) in the linearized sense will be analyzed. It is trivial to see that in the vicinity of the zero-error, the system (3.75) is controllable if v_ref is different from 0 and |e_φ| is different from π, or ω_ref is different from 0. In practical cases ω_ref often crosses 0, so v_ref cannot become 0, and |e_φ| cannot become π. The following assumptions are therefore needed to avoid loss of controllability and to restrict our attention to a certain compact region of the error space:

$\begin{array}{l}\hfill \begin{array}{cc}& {\underset{\_}{\omega }}_{\mathrm{ref}}\le {\omega }_{\mathrm{ref}}\le {\stackrel{-}{\omega }}_{\mathrm{ref}}\\ & \left|e_{\phi }\right|\le {ē}_{\phi }<\pi ,0<{\underset{\_}{v}}_{\mathrm{ref}}\le v_{\mathrm{ref}}\le {\stackrel{-}{v}}_{\mathrm{ref}}\Rightarrow {\underset{\_}{v}}_{\mathrm{ref}}sinc\left({ē}_{\phi }\right)\le v_{\mathrm{ref}}sinc\left(e_{\phi }\right)\le {\stackrel{-}{v}}_{\mathrm{ref}}\\ & \left|e_y\right|\le {ē}_y\\ & \left|e_x\right|\le {ē}_x\end{array}\end{array}$

The bounds on v_ref and ω_ref are obtained from the actual reference trajectory, while the bounds on the tracking error are selected on the basis of any a priori knowledge available. It is important that these bounds are lower than the error due to measurement noise, initial errors, etc. The bounds from Eq. (3.77) are denoted ${\underset{\_}{z}}_j$ and ${\stackrel{-}{z}}_j$, j = 1, 2, 3, 4. There are four nonlinearities in the system, so the number of fuzzy rules r is equal to 2⁴ = 16. The TS model of Eq. (3.75) is

$\begin{array}{l}\hfill \dot{\mathit{e}}\left(t\right)={\mathit{A}}_{\mathit{z}\left(t\right)}\mathit{e}\left(t\right)+{\mathit{B}}_{\mathit{z}\left(t\right)}{\mathit{u}}_{\mathrm{fb}}\left(t\right)\end{array}$

$\begin{array}{l}\hfill {\mathit{A}}_i=\left[\begin{array}{lll}0& {\epsilon }_i^1& 0\\ -{\epsilon }_i^1& 0& {\epsilon }_i^2\\ 0& 0& 0\end{array}\right],{\mathit{B}}_i=\left[\begin{array}{ll}-1& {\epsilon }_i^3\\ 0& -{\epsilon }_i^4\\ 0& -1\end{array}\right]\end{array}$

The index i runs in an arbitrary manner through all the vertices of the hypercube defined by Eq. (3.77). The most usual way is to use binary enumeration:

$\begin{array}{l}\hfill \begin{array}{cc}{i}_1& =\left\{\begin{array}{ll}0& i\le \frac{r}{2}\\ 1& \text{else}\end{array}\right.\\ i_2& =\left\{\begin{array}{ll}0& i-\frac{r}{2}{i}_1\le \frac{r}{4}\\ 1& \text{else}\end{array}\right.\\ i_3& =\left\{\begin{array}{ll}0& i-\frac{r}{2}{i}_1-\frac{r}{4}{i}_2\le \frac{r}{8}\\ 1& \text{else}\end{array}\right.\\ i_4& =\left\{\begin{array}{ll}0& i-\frac{r}{2}{i}_1-\frac{r}{4}{i}_2-\frac{r}{8}{i}_3\le \frac{r}{16}\\ 1& \text{else}\end{array}\right.\end{array}\end{array}$

Then ${\epsilon }_i^j$ in Eq. (3.79) is defined as:

$\begin{array}{l}\hfill {\epsilon }_i^j={\underset{\_}{z}}_j+i_j\left({\stackrel{-}{z}}_j-{\underset{\_}{z}}_j\right),i=1,2,\dots ,16,j=1,2,3,4\end{array}$

And finally the membership functions h_i need to be defined:

$\begin{array}{l}\hfill \begin{array}{cc}{h}_i(\mathit{z})& =w_{i_1}^1(z_1)w_{i_2}^2(z_2)w_{i_3}^3(z_3)w_{i_4}^4(z_4)i=1,2,\dots ,16\\ w_1^j(z_j)& =\frac{z_j-{\underset{\_}{z}}_j}{{\stackrel{-}{z}}_j-{\underset{\_}{z}}_j},w_0^j(z_j)=1-w_1^j(z_j)j=1,2,3,4\end{array}\end{array}$

The TS model (3.78) of the tracking-error model represents the exact model of the system (3.75), that is, in this approach the TS model does not serve as an approximator but takes into account all known nonlinearities that arise in the system. The form Eq. (3.78) is very suitable for the design and analysis tasks as it will be shown in the following.

PDC Control of a Differentially Driven Wheeled Mobile Robot

In order to stabilize the TS model (3.78), a PDC control law [15],

$\begin{array}{l}\hfill {\mathit{u}}_{\mathrm{fb}}\left(t\right)=-\sum _{i=1}^r{h}_i\left(\mathit{z}\left(t\right)\right){\mathit{F}}_i\mathit{e}\left(t\right)=-{\mathit{F}}_{\mathit{z}\left(t\right)}e\left(t\right)\end{array}$

is used. The stabilization problem by using PDC is a well-known one. Due to a specific structure where the plant model and the controller share the same membership functions it is possible to adapt certain tools for linear systems analysis and design to this nonlinear system. Particularly important is the possibility to treat system stability in a formal and straightforward manner. Roughly speaking, the system described by Eqs. (3.78), (3.83) is asymptotically stable if for each i and j the matrix (A_i −B_iF_j) is Hurwitz (a matrix is Hurwitz if all the eigen values lie in the open left half-plane of the complex plane s). The number of matrices to be analyzed grows very quickly. A systematic approach is therefore needed. It was soon realized that LMIs seem to be a perfect tool for the job [16]. Given the plant parameters in the form of matrices A_i and B_i, it is possible to find the set of controller parameters F_j that asymptotically stabilize the system.

The original approach is too conservative since it does not take into account specific properties of the system, for example, the shape of the membership functions. Many relaxations of the original LMI conditions exist. The adaptation of the result due to [17] is given below: