2.4.1 State-Space Representation of the Dynamic Model of a Mobile System With Constraints

In the sequel a state-space dynamic model for a system with m kinematic constraints is derived. Then partial linearization of the system state-space description is performed using a nonlinear feedback relation [6] that enables the second-order kinematic model to be obtained.

A dynamic system with m kinematic constraints is described by

$\begin{array}{l}\hfill \mathit{M}(\mathit{q})\ddot{\mathit{q}}+\mathit{V}(\mathit{q},\dot{\mathit{q}})+\mathit{F}(\dot{\mathit{q}})+\mathit{G}(\mathit{q})=\mathit{E}(\mathit{q})\mathit{u}-{\mathit{A}}^T(\mathit{q})\mathit{\lambda }\end{array}$

where the influence of unknown disturbances and unmodeled dynamics from Eq. (2.48) is left out. The kinematic motion model is given by

$\begin{array}{l}\hfill \dot{\mathit{q}}=\mathit{S}(\mathit{q})\mathit{v}(t)\end{array}$

The dynamic model (2.49) and kinematic model (2.50) can be joined to a single state-space description. A unified description of nonholonomic and holonomic constraints can be found in [16], where holonomic constraints are expressed in differential (with velocities) form like nonholonomic constraints.

Due to shorter notation the dependence on q is omitted in the following equations (e.g. M(q) or S(q) is written as M or S, respectively). A time derivative of Eq. (2.50) is

$\begin{array}{l}\hfill \ddot{\mathit{q}}=\dot{\mathit{S}}\mathit{v}+\mathit{S}\dot{\mathit{v}}\end{array}$

Inserting into Eq. (2.49) and expressing generalized coordinates q with pseudo velocities v results in

$\begin{array}{l}\hfill \mathit{M}\dot{\mathit{S}}\mathit{v}+\mathit{M}\mathit{S}\dot{\mathit{v}}+\mathit{V}+\mathit{F}+\mathit{G}=\mathit{E}\mathit{u}-{\mathit{A}}^T\mathit{\lambda }\end{array}$

The presence of Lagrangian multipliers λ due to motion constraints can be eliminated by considering relation AS = 0 and its transform S^TA^T = 0. Multiplying Eq. (2.52) by S^T therefore gives a reduced dynamical model:

$\begin{array}{l}\hfill {\mathit{S}}^T\mathit{M}\dot{\mathit{S}}\mathit{v}+{\mathit{S}}^T\mathit{M}\mathit{S}\dot{\mathit{v}}+{\mathit{S}}^T\mathit{V}+{\mathit{S}}^T\mathit{F}+{\mathit{S}}^T\mathit{G}={\mathit{S}}^T\mathit{E}\mathit{u}\end{array}$

where Lagrange multipliers λ are eliminated. Introducing $\tilde{\mathit{M}}={\mathit{S}}^T\mathit{M}\mathit{S}$, $\tilde{\mathit{V}}={\mathit{S}}^T\mathit{M}\dot{\mathit{S}}\mathit{v}+{\mathit{S}}^T(\mathit{V}+\mathit{F}+\mathit{G})$, and $\tilde{\mathit{E}}={\mathit{S}}^T\mathit{E}$ gives a more transparent notation,

$\begin{array}{l}\hfill \tilde{\mathit{M}}\dot{\mathit{v}}+\tilde{\mathit{V}}=\tilde{\mathit{E}}\mathit{u}\end{array}$

from where a vector of pseudo accelerations $\dot{\mathit{v}}$ is expressed:

$\begin{array}{l}\hfill \dot{\mathit{v}}={\tilde{\mathit{M}}}^{-1}\left(\tilde{\mathit{E}}\mathit{u}-\tilde{\mathit{V}}\right)\end{array}$

If condition $det{\mathit{S}}^T\mathit{E}\ne 0$ is true (which in most of realistic examples is) then from Eq. (2.54) the system input can be expressed as

$\begin{array}{l}\hfill \mathit{u}={\tilde{\mathit{E}}}^{-1}\left(\tilde{\mathit{M}}\dot{\mathit{v}}+\tilde{\mathit{V}}\right)\end{array}$

By extending the state vector with pseudo velocities x = [q^Tv^T]^T and presenting the system in nonlinear form $\dot{\mathit{x}}=f(\mathit{x})+g(\mathit{x})\mathit{u}$ (expression f(x) contains nonlinear dependence from states) the state-space representation of the system reads

$\begin{array}{l}\hfill \dot{\mathit{x}}=\left[\begin{array}{c}\mathit{S}\mathit{v}\\ -{\tilde{\mathit{M}}}^{-1}\tilde{\mathit{V}}\\ \end{array}\right]+\left[\begin{array}{c}{0}_{n\times r}\\ {\tilde{\mathit{M}}}^{-1}\tilde{\mathit{E}}\\ \end{array}\right]\mathit{u}\end{array}$

where r is the number of inputs in vector u and state vector x has the dimension (2n − m) × 1.

Using the inverse model (2.56) the required system input can be calculated for the desired pseudo accelerations of the system. Applying these inputs to Eq. (2.57) the resulting model becomes

$\begin{array}{l}\hfill \dot{\mathit{x}}=\left[\begin{array}{c}\mathit{S}\mathit{v}\\ 0_{(n-m)\times 1}\\ \end{array}\right]+\left[\begin{array}{c}{0}_{n\times (n-m)}\\ {\mathit{I}}_{(n-m)\times (n-m)}\\ \end{array}\right]{\mathit{u}}_z\end{array}$

where u_z are pseudo accelerations of the system. Relation (2.56) can therefore be used for calculating the required predicted system inputs. These inputs can be used to control the system without feedback (open-loop control) or they are better used as feedforward control in combination with some feedback control.

2.4.2 Kinematic and Dynamic Model of Differential Drive Robot

The differential drive vehicle in Fig. 2.3 has two wheels usually powered by electric motors. Let us assume that the vehicle mass center coincides with its geometric center. The mass of the vehicle without the wheels is m_v, and the mass of the wheels is m_w. The vehicle moves on the plane and its moment of inertia around the z-axis is J_v (z is in the normal direction to the plane), and the moment of inertia for the wheels is J_w.

In practice usually the mass of the wheels is much smaller than the mass of the casing; therefore a common mass m and inertia J can be used. The vehicle is described by three generalized coordinates q = [x, y, φ] and its input is the torque on the left and the right wheel, τ_l and τ_r, respectively (Fig. 2.18).

Dynamic Model

The dynamic model is derived by Lagrange formulation:

$\begin{array}{l}\hfill \frac{d}{dt}\left(\frac{\partial \mathcal{L}}{\partial {\dot{q}}_k}\right)-\frac{\partial \mathcal{L}}{\partial q_k}+\frac{\partial P}{\partial {\dot{q}}_k}=f_k-\sum _{j=1}^m{\lambda }_j{a}_{jk}\end{array}$

  (2.61)

where unknown disturbance ${\tau }_{d_k}$ from relation (2.47) is omitted. Similarly the forces and the torques due to the gravitation g_k are not present because the vehicle drives on the plane where the potential energy is constant (without loss of generality W_P = 0 can be assumed).

Overall, kinetic energy of the system reads

$\begin{array}{l}\hfill W_K=\frac{m}{2}\left({\dot{x}}^2+{\dot{y}}^2\right)+\frac{J}{2}{\dot{\phi }}^2\end{array}$

  (2.62)

The potential energy is W_P = 0, so the Lagrangian is

$\begin{array}{l}\hfill \mathcal{L}=W_K-W_P=\frac{m}{2}\left({\dot{x}}^2+{\dot{y}}^2\right)+\frac{J}{2}{\dot{\phi }}^2\end{array}$

  (2.63)

Additionally damping and friction at the wheels rotation can be neglected (P = 0). Forces and torques in Eq. (2.61) are

$\begin{array}{l}\hfill \begin{array}{c}\frac{d}{dt}\left(\frac{\partial \mathcal{L}}{\partial \dot{x}}\right)=m\ddot{x}\\ \frac{d}{dt}\left(\frac{\partial \mathcal{L}}{\partial \dot{y}}\right)=mÿ\\ \frac{d}{dt}\left(\frac{\partial \mathcal{L}}{\partial \dot{\phi }}\right)=J\ddot{\phi }\end{array}\end{array}$

  (2.64)

and

$\begin{array}{l}\hfill \begin{array}{c}\frac{\partial \mathcal{L}}{\partial x}=0\\ \frac{\partial \mathcal{L}}{\partial y}=0\\ \frac{\partial \mathcal{L}}{\partial \phi }=0\end{array}\end{array}$

  (2.65)

According to Lagrange formulation (2.61) the following differential equations are obtained:

$\begin{array}{l}\hfill \begin{array}{c}m\ddot{x}-{\lambda }_{1}sin\phi =F_x\\ mÿ+{\lambda }_{1}cos\phi =F_y\\ J\ddot{\phi }=M\end{array}\end{array}$

  (2.66)

where the resultant force on the left and the right wheel is $F=\frac{1}{r}({\tau }_{\mathrm{r}}+{\tau }_{\mathrm{l}})$ and r is the wheel radius. The force in the x-axis direction is $F_x=\frac{1}{r}({\tau }_{\mathrm{r}}+{\tau }_{\mathrm{l}})cos\phi$ and in the y-axis it is $F_y=\frac{1}{r}({\tau }_{\mathrm{r}}+{\tau }_{\mathrm{l}})sin\phi$. Torque on the vehicle is obtained by $M=\frac{L}{2r}({\tau }_{\mathrm{r}}-{\tau }_{\mathrm{l}})$, where L is the distance among the wheels. Therefore, the final model is

$\begin{array}{l}\hfill \begin{array}{c}m\ddot{x}-{\lambda }_{1}sin\phi -\frac{1}{r}({\tau }_{\mathrm{r}}+{\tau }_{\mathrm{l}})cos\phi =0\\ mÿ+{\lambda }_{1}cos\phi -\frac{1}{r}({\tau }_{\mathrm{r}}+{\tau }_{\mathrm{l}})sin\phi =0\\ J\ddot{\phi }-\frac{L}{2r}({\tau }_{\mathrm{r}}-{\tau }_{\mathrm{l}})=0\end{array}\end{array}$

  (2.67)

The obtained dynamic model written in matrix form is

$\begin{array}{l}\hfill \mathit{M}(\mathit{q})\ddot{\mathit{q}}+\mathit{V}(\mathit{q},\dot{\mathit{q}})+\mathit{F}(\dot{\mathit{q}})=\mathit{E}(\mathit{q})\mathit{u}-{\mathit{A}}^T(\mathit{q})\mathit{\lambda }\end{array}$

where matrices are

$\begin{array}{ll}\hfill \mathit{M}& =\left[\begin{array}{ccc}m& 0& 0\\ 0& m& 0\\ 0& 0& J\\ \end{array}\right]\hfill \\ \hfill \mathit{E}|& =\frac{1}{r}\left[\begin{array}{cc}cos\phi & cos\phi \\ sin\phi & sin\phi \\ \frac{L}{2}& -\frac{L}{2}\\ \end{array}\right]\hfill \\ \hfill \mathit{A}& =\left[\begin{array}{ccc}-sin\phi & cos\phi & 0\\ \end{array}\right]\hfill \\ \hfill \mathit{u}& =\left[\begin{array}{c}{\tau }_{\mathrm{r}}\\ {\tau }_{\mathrm{l}}\\ \end{array}\right]\hfill \end{array}$

and the remaining matrices are zero.

The common state-space model (2.57) that includes the kinematic and dynamic model is determined by matrices

$\begin{array}{ll}\hfill \tilde{\mathit{M}}=& \left[\begin{array}{cc}m& 0\\ 0& J\\ \end{array}\right]\hfill \\ \hfill \tilde{\mathit{V}}=& \left[\begin{array}{c}0\\ 0\\ \end{array}\right]\hfill \\ \hfill \tilde{\mathit{E}}=& \frac{1}{r}\left[\begin{array}{cc}1& 1\\ \frac{L}{2}& -\frac{L}{2}\\ \end{array}\right]\hfill \end{array}$

and according to Eq. (2.57) the system can be written in the state-space form $\dot{\mathit{x}=\mathit{f}(\mathit{x})+\mathit{g}(\mathit{x})\mathit{u}}$, where the state vector is x = [q^T, v^T]^T. The resulting model is

$\begin{array}{l}\hfill \left[\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\phi }\\ \dot{v}\\ \dot{\omega }\\ \end{array}\right]=\left[\begin{array}{c}vcos\phi \\ vsin\phi \\ \omega \\ 0\\ 0\\ \end{array}\right]+\left[\begin{array}{cc}0& 0\\ 0& 0\\ 0& 0\\ \frac{1}{mr}& \frac{1}{mr}\\ \frac{L}{2Jr}& \frac{-L}{2Jr}\\ \end{array}\right]\left[\begin{array}{c}{\tau }_{\mathrm{r}}\\ {\tau }_{\mathrm{l}}\\ \end{array}\right]\end{array}$

  (2.68)

The inverse system model is obtained by taking into account Eq. (2.56) as

$\begin{array}{l}\hfill \left[\begin{array}{c}{\tau }_{\mathrm{r}}\\ {\tau }_{\mathrm{l}}\\ \end{array}\right]=\left[\begin{array}{c}\frac{\dot{v}mr}{2}+\frac{\dot{\omega }Jr}{L}\\ \frac{\dot{v}mr}{2}-\frac{\dot{\omega }Jr}{L}\\ \end{array}\right]\end{array}$

  (2.69)

Using the inverse model the required input torques to the wheels can be obtained from the known robot velocities and accelerations. The obtained input torques can be applied in open-loop control or better as a feedforward part in a closed-loop control.

Example 2.9

Derive the kinematic and dynamic model for a vehicle with differential drive in Fig. 2.19. Express the models with the coordinates of the mass center (x_c, y_c), which are distance d away from the geometric center (x, y).

Solution

Considering the transformation between the geometric and mass center $x=x_{\mathrm{c}}-dcos\phi$ and $y=y_{\mathrm{c}}-dsin\phi$ and their time derivatives $\dot{x}={\dot{x}}_{\mathrm{c}}+d\dot{\phi }sin\phi$ and $\dot{y}={\dot{y}}_{\mathrm{c}}-d\dot{\phi }cos\phi$ and inserting the derivatives to kinematic model (Fig. 2.3), the final kinematic model and kinematic constraint for the mass center are obtained:

$\begin{array}{l}\hfill \left[\begin{array}{c}{\dot{x}}_{\mathrm{c}}\\ {\dot{y}}_{\mathrm{c}}\\ \dot{\phi }\\ \end{array}\right]=\left[\begin{array}{cc}cos(\phi )& -dsin(\phi )\\ sin(\phi )& dcos(\phi )\\ 0& 1\\ \end{array}\right]\left[\begin{array}{c}v\\ \omega \\ \end{array}\right]\end{array}$

$\begin{array}{l}\hfill -{\dot{x}}_{\mathrm{c}}sin\phi +{\dot{y}}_{\mathrm{c}}cos\phi -\dot{\phi }d=0\end{array}$

The Lagrangian for the mass center is $\mathcal{L}=\frac{m}{2}\left({\dot{x}}_{\mathrm{c}}^2+{\dot{y}}_{\mathrm{c}}^2\right)+\frac{J}{2}{\dot{\phi }}^2$. Considering relation (2.61) the dynamic model is

$\begin{array}{ll}\hfill m{\ddot{x}}_{\mathrm{c}}-{\lambda }_{1}sin\phi & =\frac{1}{r}({\tau }_{\mathrm{r}}+{\tau }_{\mathrm{l}})cos\phi \hfill \\ \hfill m{ÿ}_{\mathrm{c}}+{\lambda }_{1}cos\phi & =F_y=\frac{1}{r}({\tau }_{\mathrm{r}}+{\tau }_{\mathrm{l}})sin\phi \hfill \\ \hfill J\ddot{\phi }-{\lambda }_{1}d& =\frac{L}{2r}\hfill \end{array}$

and matrices for the model in matrix form are the following:

$\begin{array}{ll}\hfill \mathit{M}=& \left[\begin{array}{ccc}m& 0& 0\\ 0& m& 0\\ 0& 0& J\\ \end{array}\right]\hfill \\ \hfill \mathit{E}=& \frac{1}{r}\left[\begin{array}{cc}cos\phi & cos\phi \\ sin\phi & sin\phi \\ \frac{L}{2}& -\frac{L}{2}\\ \end{array}\right]\hfill \\ \hfill \mathit{A}=& \left[\begin{array}{ccc}-sin\phi & cos\phi & -d\\ \end{array}\right]\hfill \\ \hfill \mathit{S}=& \left[\begin{array}{cc}cos(\phi (t))& -dsin(\phi )\\ sin(\phi (t))& dcos(\phi )\\ 0& 1\\ \end{array}\right]\hfill \end{array}$

Considering Eq. (2.57) the overall state-space representation of the system reads

$\begin{array}{l}\hfill \left[\begin{array}{c}{\dot{x}}_{\mathrm{c}}\\ {\dot{y}}_{\mathrm{c}}\\ \dot{\phi }\\ \dot{v}\\ \dot{\omega }\\ \end{array}\right]=\left[\begin{array}{c}vcos\phi -\omega dsin\phi \\ vsin\phi +\omega dcos\phi \\ \omega \\ d{\omega }^2\\ \frac{-dv\omega m}{m{d}^2+J}\\ \end{array}\right]+\left[\begin{array}{cc}0& 0\\ 0& 0\\ 0& 0\\ \frac{1}{mr}& \frac{1}{mr}\\ \frac{L}{2r(d^{2}m+J)}& \frac{-L}{2r(d^{2}m+J)}\\ \end{array}\right]\left[\begin{array}{c}{\tau }_{\mathrm{r}}\\ {\tau }_{\mathrm{l}}\\ \end{array}\right]\end{array}$

  (2.70)

Example 2.10

Derive the kinematic and dynamic model for a vehicle in Example 2.9 where the mass center and geometric center are not the same. Express the models using coordinates of the geometric center (x, y), which could be more appropriate in case that the mass center of the vehicle is changing due to a varying load.

Solution

The kinematic model and kinematic constraint for the geometric center are obtained:

$\begin{array}{l}\hfill \left[\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\phi }\\ \end{array}\right]=\left[\begin{array}{cc}cos(\phi )& 0\\ sin(\phi )& 0\\ 0& 1\\ \end{array}\right]\left[\begin{array}{c}v\\ \omega \\ \end{array}\right]\\ \hfill -{\dot{x}}_{\mathrm{c}}sin\phi +{\dot{y}}_{\mathrm{c}}cos\phi =0\end{array}$

The Lagrangian is $\mathcal{L}=\frac{m}{2}\left({\dot{x}}_{\mathrm{c}}^2+{\dot{y}}_{\mathrm{c}}^2\right)+\frac{J}{2}{\dot{\phi }}^2$, where the mass center is expressed by the geometric center. The final state-space model is

$\begin{array}{ll}\hfill \left[\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\phi }\\ \dot{v}\\ \dot{\omega }\\ \end{array}\right]=& \left[\begin{array}{c}vcos\phi \\ vsin\phi \\ \omega \\ d{\omega }^2\\ \frac{-d\omega m(\dot{x}cos\phi +\dot{y}sin\phi -vcos(2\phi )-d\omega sin(2\phi ))}{m{d}^2+J}\\ \end{array}\right]\hfill \\ \hfill & +\left[\begin{array}{cc}0& 0\\ 0& 0\\ 0& 0\\ \frac{1}{mr}& \frac{1}{mr}\\ \frac{L-2dsin(2\phi )}{2r(d^{2}m+J)}& \frac{-L-2dsin(2\phi )}{2r(d^{2}m+J)}\\ \end{array}\right]\left[\begin{array}{c}{\tau }_{\mathrm{r}}\\ {\tau }_{\mathrm{l}}\\ \end{array}\right]\hfill \end{array}$

  (2.71)

Example 2.11

Using Matlab calculate the required torques so that the mobile robot from Example 2.9 will drive along the reference trajectory $x_{\mathrm{r}}=1.1+0.7sin(2\pi /30)$, $y_{\mathrm{r}}=0.9+0.7sin(4\pi /30)$ without using sensors (open-loop control). In the simulation calculate robot torques using the inverse dynamic model, and depict system trajectory. Robot parameters are m = 0.75 kg, J = 0.001 kg m², L = 0.075 m, r = 0.024 m, and d = 0.01 m.

Solution

The inverse robot model considering relation (2.56) is

$\begin{array}{l}\hfill \left[\begin{array}{c}{\tau }_{\mathrm{r}}\\ {\tau }_{\mathrm{l}}\\ \end{array}\right]=\left[\begin{array}{c}\frac{r(\dot{v}m-d{\omega }^{2}m)}{2}+\frac{r\left(\dot{\omega }\left(m{d}^2+J\right)+d{\omega }^{2}m\right)}{L}\\ \frac{r(\dot{v}m-d{\omega }^{2}m)}{2}-\frac{r\left(\dot{\omega }\left(m{d}^2+J\right)+d{\omega }^{2}m\right)}{L}\\ \end{array}\right]\end{array}$

From the reference, trajectory reference velocity v_r and reference angular velocity ω_r and their time derivatives ${\dot{v}}_{\mathrm{r}}$ and ${\dot{\omega }}_{\mathrm{r}}$ are calculated.

Obtained responses using Matlab simulation in Listing 2.3 are shown in Figs. 2.20 and 2.21. 6.88.8

Listing 2.3

Inverse robot dynamic model

1 Ts = 0.033; %  Sampling time

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

3 

4  %  Reference

5  freq =  2*pi/30;

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

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

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

9  dddxRef =− freq ˆ3*0.7* cos( freq *t ) ;  dddyRef =−8* freq ˆ3*0.7* cos (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 dvRef =  ( dxRef .* ddxRef + dyRef .* ddyRef ) ./ vRef ;

14 dwRef =  ( dxRef .* dddyRef− dyRef .* dddxRef ) ./ vRef .ˆ2 − 2.* wRef .* dvRef ./vRef ;

15

16 q = [ qRef ( : , 1 ) ;  vRef (1) ;  wRef (2) ] ; % I n i t a l  robot state

17 m = 0.75;  J = 0.001;  L = 0.075;  r = 0.024;  d = 0.01; %  Robot parameters

18

19 for k = 1:length( t )

20  %  Calculate torques from the trajectory and inverse model

21  v =  vRef (k) ; w =  wRef(k) ;  dv =  dvRef (k) ;  dw =  dwRef(k) ;

22  tau = [ ( r *(dv* m− d * w * m * w ) ) /2 + ( r *( dw *( m * d ˆ2+J) +  d* w * m *v) )/L; . . .

23   ( r *(dv* m− d * w * m * w ) ) /2  − ( r *( dw *( m * d ˆ2+ J ) +  d* w * m *v) )/L ] ;

24

25  %  Robot motion simulation using kinematic and dynamic model

26  phi =  q (3) ;  v =  q (4) ; w =  q (5) ;

27  F = [ v*cos( phi ) − d * w * sin ( phi ) ; . . .

28   v*sin( phi ) +  d* w *cos( phi ) ; . . .

29   w; . . .

30   d* wˆ2; . . .

31  −( d * w * v * m ) /( m * d ˆ2 + J ) ] ;

32 G = [0 ,   0; . . .

33   0 ,  0; . . .

34   0 ,  0; . . .

35   1/( m *r ) , 1/( m *r ) ; . . .

36   L/(2* r *( m *dˆ2 +  J) ) , − L /(2* r *( m * d ˆ2 + J) ) ] ;

37  dq =  F + G *tau ; %  State space model

38  q =  q +  dq*Ts ; %  Euler integration

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

40 end