Orientation Control for Differential Drive

Differential drive kinematics is given by Eq. (2.2). The third equation in Eq. (2.2) is the orientation equation of the kinematic model and is given by

$\begin{array}{l}\hfill \dot{\phi }(t)=\omega (t)\end{array}$

From the control point of view, Eq. (3.1) describes the system with control variable ω(t) and integral nature (its pole lies in the origin of the complex plane s). It is very well known that a simple proportional controller is able to drive the control error of an integral process to 0. The control law is then given by

$\begin{array}{l}\hfill \omega (t)=K({\phi }_{\mathrm{ref}}(t)-\phi (t))\end{array}$

where the control gain K is an arbitrary positive constant. The interpretation of the control law (3.3) is that the angular velocity ω(t) of the platform is set proportionally to the robot orientation error. Combining Eqs. (3.1), (3.2), the dynamics of the orientation control loop can be rewritten as:

$\begin{array}{l}\hfill \dot{\phi }(t)=K\left({\phi }_{\mathrm{ref}}(t)-\phi (t)\right)\end{array}$

from where closed-loop transfer function of the controlled system can be obtained:

$\begin{array}{l}\hfill G_{\mathrm{cl}}(s)=\frac{\varphi (s)}{{\varphi }_{\mathrm{ref}}(s)}=\frac{1}{\frac{1}{K}s+1}\end{array}$

with ϕ(s) and ϕ_ref(s) being the Laplace transforms of φ(t) and φ_ref(t), respectively. The transfer function G_cl(s) is of the first order, which means that in the case of a constant reference the orientation exponentially approaches the reference (with time constant $T=\frac{1}{K}$). The closed-loop transfer function also has unity gain, so there is no orientation error in steady state.

It has been shown that the controller (3.3) made the closed-loop transfer function behave as the first-order system. Sometimes the second-order transfer function $G_{\mathrm{cl}}(s)=\frac{\varphi (s)}{{\varphi }_{\mathrm{ref}}(s)}$ is desired because it gives more degrees of freedom to the designer in terms of the transients shape. We begin the construction by setting the angular acceleration $\dot{\omega }(t)$ of the platform proportional to the robot orientation error:

$\begin{array}{l}\hfill \dot{\omega }(t)=K({\phi }_{\mathrm{ref}}(t)-\phi (t))\end{array}$

The obtained controlled system

$\begin{array}{l}\hfill \dot{\omega }=\ddot{\phi }(t)=K({\phi }_{\mathrm{ref}}(t)-\phi (t))\end{array}$

with the transfer function

$\begin{array}{l}\hfill G_{\mathrm{cl}}(s)=\frac{\varphi (s)}{{\varphi }_{\mathrm{ref}}(s)}=\frac{K}{s^2+K}\end{array}$

is the second-order system with natural frequency ${\omega }_n=\sqrt{K}$ and damping coefficient ζ = 0. Such a system is marginally stable and its oscillating responses are unacceptable. Damping is achieved by the inclusion of an additional term to the controller (3.3):

$\begin{array}{l}\hfill \dot{\omega }(t)=K_1({\phi }_{\mathrm{ref}}(t)-\phi (t))-K_2\dot{\phi }(t)\end{array}$

where K₁ and K₂ are arbitrary positive control gains. Combining Eqs. (3.1), (3.4) the closed-loop transfer function becomes,

$\begin{array}{l}\hfill G_{\mathrm{cl}}(s)=\frac{\varphi (s)}{{\varphi }_{\mathrm{ref}}(s)}=\frac{K_1}{s^2+K_{2}s+K_1}\end{array}$

where natural frequency is ${\omega }_n=\sqrt{K_1}$, damping coefficient $\zeta =\frac{K_2}{2\sqrt{K_1}}$, and closed-loop poles $s_{1,2}=-\zeta {\omega }_n\pm j{\omega }_n\sqrt{1-{\zeta }^2}$.

Orientation Control for the Ackermann Drive

Controller design in the case of the Ackermann drive is very similar to the differential drive controller design; the only difference is due to a different kinematic model (2.19) for orientation that reads

$\begin{array}{l}\hfill \dot{\phi }=\frac{v_r(t)}{d}tan\left(\alpha (t)\right)\end{array}$

The control variable is α, which can again be chosen proportionally to the orientation error:

$\begin{array}{l}\hfill \alpha (t)=K({\phi }_{\mathrm{ref}}(t)-\phi (t))\end{array}$

The dynamics of the orientation error can be described by the following differential equation obtained by combining Eqs. (3.5), (3.6):

$\begin{array}{l}\hfill \dot{\phi }(t)=\frac{v_r(t)}{d}tan\left(K({\phi }_{\mathrm{ref}}(t)-\phi (t))\right)\end{array}$

The system is therefore nonlinear. For small angles α(t) and a constant velocity of the rear wheels v_r(t) = V, a linear model approximation is obtained,

$\begin{array}{l}\hfill \dot{\phi }(t)=\frac{V}{d}K\left({\phi }_{\mathrm{ref}}(t)-\phi (t)\right)\end{array}$

which can be converted to the transfer function form:

$\begin{array}{l}\hfill G_{\mathrm{cl}}(s)=\frac{\varphi (s)}{{\varphi }_{\mathrm{ref}}(s)}=\frac{1}{\frac{d}{VK}s+1}\end{array}$

Similarly as in the case of differential drive, the orientation error converges to 0 exponentially (for constant reference orientation) and the decay time constant is $T=\frac{d}{VK}$.

If the desired transfer function of the orientation feedback loop is of the second order, we follow a similar approach as in the case of the differential drive. Control law given by Eq. (3.4) can be adapted to the Ackermann drive as well:

$\begin{array}{l}\hfill \dot{\alpha }(t)=K_1({\phi }_{\mathrm{ref}}(t)-\phi (t))-K_2\dot{\phi }(t)\end{array}$

Assuming again a constant velocity v_r(t) = V and small angles α, a linear approximation of Eq. (3.5) is obtained:

$\begin{array}{l}\hfill \dot{\phi }(t)=\frac{V}{d}\alpha (t)\end{array}$

Deriving α(t) from Eq. (3.8) and introducing it into Eq. (3.7) yields

$\begin{array}{l}\hfill \ddot{\phi }(t)=K_1\frac{V}{d}\left({\phi }_{\mathrm{ref}}(t)-\phi (t)\right)-K_2\frac{V}{d}\dot{\phi }(t)\end{array}$

The closed-loop transfer function therefore becomes

$\begin{array}{l}\hfill G_{\mathrm{c}}(s)=\frac{\varphi (s)}{{\varphi }_{\mathrm{ref}}(s)}=\frac{K_1\frac{V}{d}}{s^2+K_2\frac{V}{d}s+K_1\frac{V}{d}}\end{array}$

The obtained response of the robot orientation is damped with the damping coefficient $\zeta =\frac{K_2}{2}\sqrt{\frac{V}{d{K}_1}}$ and has a natural frequency ${\omega }_n=\sqrt{K_1\frac{V}{d}}$.

Control to Reference Position

In this case the robot is required to arrive at a reference (final) position where the final orientation is not prescribed, so it can be arbitrary. In order to arrive to the reference point, the robot’s orientation is controlled continuously in the direction of the reference point. This direction is denoted by φ_r (see Fig. 3.3), which can be obtained easily using geometrical relations:

$\begin{array}{l}\hfill {\phi }_r(t)=arctan\frac{y_{\mathrm{ref}}-y(t)}{x_{\mathrm{ref}}-x(t)}\end{array}$

Angular velocity control ω(t) is therefore commanded as

$\begin{array}{l}\hfill \omega (t)=K_1({\phi }_r(t)-\phi (t))\end{array}$

where K₁ is a positive controller gain. The basic control principle is similar as in Example 3.1 and is illustrated in Fig. 3.3.

Translational robot velocity is first commanded as proposed by Eq. (3.9):

$\begin{array}{l}\hfill v(t)=K_2\sqrt{{(x_{\mathrm{ref}}(t)-x(t))}^2+{(y_{\mathrm{ref}}(t)-y(t))}^2}\end{array}$

We have already mentioned that the maximum velocity should be limited in the starting phase due to many practical constraints. Specifically, velocity and acceleration limits should be taken into account.

The control law (3.12) also hides a potential danger when the robot approaches the target pose. The velocity command (3.12) is always positive, and when decelerating toward the final position the robot can accidentally drive over it. The problem is that after crossing the reference pose the velocity command will start increasing because the distance between the robot and the reference starts increasing. The other problem is that crossing the reference also makes the reference orientation opposite, which leads to fast rotation of the robot. Some simple solutions to this problem exist:

• When the robot drives over the reference point, the orientation error abruptly changes (± 180 degrees). The algorithm will therefore check if the absolute value of orientation error exceeds 90 degree. The orientation error will then be either increased or decreased by 180 degree (so that it is in the interval [−180 degree, 180 degree]) before it enters the controller. Moreover, the control output (3.12) changes its sign in this case. The mentioned problems are therefore circumvented by an upgraded versions of control laws (3.11), (3.12):

$\begin{array}{l}\hfill \begin{array}{cc}{e}_{\phi }(t)& ={\phi }_r(t)-\phi (t)\\ \omega (t)& =K_{1}arctan(tan(e_{\phi }(t)))\\ v(t)& =K_2\sqrt{{(x_{\mathrm{ref}}(t)-x(t))}^2+{(y_{\mathrm{ref}}(t)-y(t))}^2}\cdot \text{sgn}(cos(e_{\phi }(t)))\end{array}\end{array}$

• When the robot reaches a certain neighborhood of the reference point, the approach phase is finished and zero velocity commands are sent. This mechanism to fully stop the vehicle needs to be implemented even in the case of the modified control law (3.13), especially in case of noisy measurements.

Control to Reference Pose Using an Intermediate Point

This control algorithm is easy to implement because we use a simple controller given by Eqs. (3.11), (3.12), which drives the robot to the desired reference point. But in this case not only the reference point (x_ref(t), y_ref(t)) but also the reference orientation φ_ref is required in the reference point. The idea of this approach is to add an intermediate point that will shape the trajectory in a way that the correct final orientation is obtained. The intermediate point (x_t, y_t) is placed on the distance r from the reference point such that the direction from the intermediate point toward the reference point coincides with the reference orientation (as shown in Fig. 3.4).

The intermediate point is determined by

$\begin{array}{l}\hfill \begin{array}{cc}{x}_t& =x_{\mathrm{ref}}-rcos{\phi }_{\mathrm{ref}}\\ y_t& =y_{\mathrm{ref}}-rsin{\phi }_{\mathrm{ref}}\\ \end{array}\end{array}$

The control algorithm consists of two phases. In the first phase the robot is driven toward the intermediate point. When the distance to the intermediate point becomes sufficiently low (checked by condition $\sqrt{{(x-x_t)}^2+{(y-y_t)}^2}<d_{\mathrm{tol}}$), the algorithm switches to the second phase where the robot is controlled to the reference point. This procedure ensures that the robot arrives to the reference position with the required orientation (a very small orientation error is possible in the reference pose). It is possible to make many variations of this algorithm and also make more intermediate points to improve the operation.

The described algorithm is very simple and applicable in many areas of use. According to the application, appropriate selection of parameters r and d_tol should be made.

Example 3.3

Write a control algorithm for a differential type robot to achieve the reference pose [x_ref, y_ref, φ_ref] = [4 m, 4 m, 0 degree] using an intermediate point. Find appropriate values for parameters r and d_tol. The initial pose of the vehicle is [x(0), y(0), φ(0)] = [1 m, 0 m, 100 degree].

Test the control algorithm by means of simulation of the differential drive vehicle kinematics.

Solution

M-script code of a possible solution is given in Listing 3.2. Simulation results are shown in Figs. 3.5 and 3.6.

Listing 3.2

1  Ts =  0.03;   %  Sampling time

2  t  =   0: Ts : 1 5 ;   %  Simulation time

3  r  =   0 . 5 ;   %  Distance parameter for the intermediate point

4  dTol  =   0.05;   %  Tolerance distance (to the intermediate point) for switch

5  qRef  =   [ 4 ;   4;   0 ] ;   %  Reference pose

6  q  =   [ 1 ;   0;  100/180*pi ] ;   %   Initial  pose

7

8  %  Intermediate point

9  xT  =  qRef (1)  −   r * cos ( qRef (3) ) ;

10 yT  =  qRef (2)  −   r * sin ( qRef (3) ) ;

11

12 state   =   0;   %  State: 0 −   go to intermediate point , 1 −   go to reference point

13 for k  =   1:length( t )

14    D   =  sqrt(( qRef (1)− q (1) ) ˆ2   +   ( qRef (2)− q (2) ) ˆ2) ;

15    if   D < dTol  %  Stop when close to the goal

16      v  =   0;

17      w   =   0;

18    else

19      if state==0

20       d  =  sqrt((xT− q (1) ) ˆ2+( yT− q (2) ) ˆ2) ;

21      if  d < dTol ,   state   =   1;  end

22

23       phiT  =  atan2(yT− q (2) ,   xT− q (1) ) ;

24      ePhi   =   phiT −   q (3) ;

25   else

26       ePhi  =  qRef (3)  −   q (3) ;

27    end

28     ePhi  =  wrapToPi( ePhi ) ;

29

30     %  Controller

31     v  =   D*0.8;

32    w   =  ePhi *5;

33   end

34

35   %  Robot motion simulation

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

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

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

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

40 end

Control to Reference Pose Using an Intermediate Direction

A robot needs to arrive from its initial pose to the reference pose where the position (x_ref, y_ref) and the orientation φ_ref are given. The idea of the algorithm with an intermediate direction is illustrated in Fig. 3.7 [2]. We begin with the construction of a right-angled triangle. The reference point is put to the vertex with the right angle. One cathetus connects the current robot position with the reference one and has the length $D=+\sqrt{{(x_{\mathrm{ref}}(t)-x(t))}^2+{(y_{\mathrm{ref}}(t)-y(t))}^2}$. This cathetus defines the orientation φ_r that is facing from the robot toward the target. The other cathetus has a fixed length of r > 0, which is a designer parameter of this method. The angle between cathetus D and the hypotenuse is denoted by β(t). Two angles play a crucial role in this approach, and they are defined as follows:

$\begin{array}{l}\hfill \begin{array}{cc}\alpha (t)& ={\phi }_r(t)-{\phi }_{\mathrm{ref}}\\ \beta (t)& =\left\{\begin{array}{ll}arctan\frac{+r}{D}& \alpha (t)>0\\ -arctan\frac{r}{D}& \text{otherwise}\end{array}\right.\end{array}\end{array}$

Note that the angles α(t) and β(t) are always of the same sign (in the case depicted in Fig. 3.7 they are both positive). If α becomes 0 at a certain time, β becomes irrelevant, so its sign can be arbitrary. Large values (in the absolute sense) of α mean that driving straight to the reference is not a good idea because the orientation error will be large in the reference pose. The angle α should therefore be reduced (in the absolute sense). This is achieved by defining an intermediate direction that is shifted from φ_r, and the shift is always away from the reference orientation φ_ref (if the reference orientation is seen pointing to the right from the robot’s perspective, the robot should approach the reference position from the left side, and vice versa). While α(t) normally decreases (in the absolute sense) when approaching the target, β(t) increases (in the absolute sense) as the distance to the reference decreases. We will make use of this when constructing the control algorithm.

The algorithm again has two phases. In the first phase (when |α(t)| is large), the robot’s orientation is controlled toward the intermediate direction φ_t(t) = φ_r(t) + β(t) (this case is depicted in Fig. 3.7). When the angles α and β become the same, the current reference orientation switches to φ_t(t) = φ_r(t) + α(t). Note that this switch does not cause any bump because both directions are the same at the time of the switch. The control law for orientation is therefore defined as follows:

$\begin{array}{l}\hfill \begin{array}{cc}{e}_{\phi }(t)& ={\phi }_r(t)-\phi (t)+\left\{\begin{array}{ll}\alpha (t)& |\alpha (t)|<|\beta (t)|\\ \beta (t)& \text{otherwise}\end{array}\right.\\ \omega (t)& =K{e}_{\phi }(t)\end{array}\end{array}$

Note that the current reference heading is never toward the reference point but is always slightly shifted. The shift is chosen so that the angle α(t) is driven toward 0. This in turn implies that the reference heading is forced toward the reference point and the robot will arrive there with the correct reference orientation. Note that this algorithm is also valid for negative values of angles α and β. We only need to ensure that all the angles are in the (−π, π] interval.

Robot translational velocity can be defined similarly as in the previous sections.

Example 3.4

Write a control algorithm for a differential type robot to achieve the reference pose [x_ref, y_ref, φ_ref] = [4 m, 4 m, 0 degree] using the presented control to the reference pose using an intermediate direction. Find an appropriate value for parameter r. The initial pose of the vehicle is [x(0), y(0), φ(0)] = [1 m, 0 m, 100 degree]. Test the algorithm on a simulated kinematic model.

Solution

The M-script code of a possible solution is in Listing 3.3. Simulation results are shown in Figs. 3.8 and 3.9.

Listing 3.3

1 Ts =  0.03;   %  Sampling time

2 t  =   0: Ts : 1 5 ;   %  Simulation time

3 r  =   0 . 2 ;   %  Distance parameter

4 qRef  =   [ 4 ;   4;   0 ] ;   %  Reference pose

5 q  =   [ 1 ;   0;  100/180*pi ] ;   %   Initial  pose

6

7 for k  =   1:length( t )

8   %  Compute intermidieate direction  shift

9   phiR  =  atan2( qRef (2)− q (2) ,   qRef (1)− q (1) ) ;

10  D = sqrt(( qRef (1)− q (1) ) ˆ2   +   ( qRef (2)− q (2) ) ˆ2) ;

11

12  alpha  =  wrapToPi(phiR −   qRef (3) ) ;

13  beta  =  atan( r/ D) ;

14  if  alpha <0, beta  =  − beta ;   end

15

16  %  Controller

17  if  abs( alpha )  <  abs(beta)

18   ePhi  =  wrapToPi(phiR −   q (3)   +   alpha ) ;   %  The second part

19  else

20   ePhi  =  wrapToPi(phiR −   q (3)   +   beta) ;   %  The  first  part

21   int . setPose (q) ;

22  end

23  v  =   D*0.8;

24  w   =  ePhi *5;

25

26  %  Robot motion simulation

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

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

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

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

31 end

Control on a Segmented Continuous Path Determined by a Line and a Circle Arc

The path composed from straight line segments and circular arcs is known to be the shortest possible path for Ackermann drive robots as shown in [3–5], where the circle’s radius is the minimal turning radius of the vehicle. Such a path is also the shortest possible for a differential robot where the minimum circle radius limits to zero, which means that the robot can turn on the spot.

The basic idea of the algorithm can be described with the illustration in Fig. 3.10. First, a circle of an appropriate (this will be discussed later) radius R is drawn through the reference point tangentially to the reference orientation. Note that two solutions exist, and a circle whose center is closer to the robot is chosen. This circle, more precisely a certain arc belonging to this circle, represents the second part of the planned path. The first part will run on a straight line tangent to the circle and crossing the current robot’s position. Again, two tangents exist, and we select the one that gives proper direction of driving along the circular arc. The direction of driving along the arc is defined with the reference orientation (in the case illustrated in Fig. 3.10 the robot will drive along the circle in the clockwise direction). The solution can be chosen easily by checking the sign of the cross products between the radius vectors and the tangential vectors.

In the first part of the algorithm the goal is to drive toward the point (x_t, y_t) where the straight segment meets the arc segment. The orientation controller is a very simple one:

$\begin{array}{l}\hfill \omega (t)=K({\phi }_t(t)-\phi (t))\end{array}$

where ${\phi }_t(t)=arctan\frac{y_t-y(t)}{x_t-x(t)}$. When the distance to the intermediate point is small enough, the second phase begins. This includes driving along the trajectory, and the controller changes to

$\begin{array}{l}\hfill \omega (t)=\frac{v(t)}{R}+K({\phi }_{\mathrm{tang}}(t)-\phi (t))\end{array}$

where R is the circle radius, v(t) is the desired translational velocity, and φ_tang(t) is the direction of the tangent to the arc in the current robot position. Note that the first term in the control is a feedforward part that ensures the robot will drive along the circular arc with radius R, while the second term is a feedback control that corrects the control errors.

To achieve higher robustness the reference path is calculated in each iteration, which ensures that the robot is always on the reference straight line or on the reference circle. The obtained path slightly differs from the ideal one composed of a straight line segment and a circular arc. The mentioned difference is due to inial robot orientation that does not coincide with the constructed tangent. Other reasons for this difference are noise and disturbances (wheels slipping and the like).

This reference path is relatively easy to determine in real time. The path itself is continuous, but the required inputs are not. Due to transition from the straight line to the circle the robot angular velocity instantly changes from zero to $\frac{v(t)}{R}$. In reality this is not achievable (limited robot acceleration), and therefore some tracking error appears in this transition.

Such a control is appropriate for situations where a robot needs to arrive to the reference pose quickly and the shape of the path is not prescribed but the length should be as short as possible (e.g., robot soccer). Especially if we deal with the robots with a limited turning radius (e.g., Ackermann drive), such a path is the shortest possible to the goal pose if the minimal robot turning radius is used for parameter R. But of course, this leads to high values of radial accelerations, so perhaps a larger value of R is desired.

Example 3.5

Write a control algorithm for a differential type robot to achieve the reference pose [x_ref, y_ref, φ_ref] = [0 m, 0 m, 0 degree] using the control algorithm proposed in this section. The radius of the circle should be R = 0.4 m. The initial pose of the vehicle is [x(0), y(0), φ(0)] = [−3 m, −3 m, 100 degree]. Test the algorithm on a simulated kinematic model.

Solution

Although the basic idea of the control algorithm is quite simple, the implementation becomes a bit more complicated as it needs to calculate appropriate circle centers, tangent points on the circle, and some other parameters. A possible solution is given in Listing 3.4, and the simulation results are shown in Figs. 3.11 and 3.12.

Listing 3.4

1 Ts =  0.03;   %  Sampling time

2 t  =   0: Ts : 1 5 ;   %  Simulation time

3 r  =   0 . 2 ;   %  Distance parameter

4 qRef  =   [ 4 ;   4;   0 ] ;   %  Reference pose

5 q  =   [ 1 ;   0;  100/180*pi ] ;   %   Initial  pose

6

7 aMax  =   5;   %  Maximum  acceleration

8 vMax  =   0 . 4 ;   %  Maximum  speed

9 accuracy  =  vMax*Ts ;   %  Accuracy

10 curveZone  =   0 . 6 ;   %  Radius

11 Rr  =  0.99* curveZone /2;   %  Radius

12 slowDown  =   false ;  v  =   0;  vDir  =   1;   w   =   0;   %   Initial  states

13 X = [0 ,   1;  −1,   0 ] ;   %  Support matrix: a. ’*X *b =  a(1)*b(2) −   a (2)*b(1)

14

15 for k  =   1:length( t )

16   fin   =   [cos( qRef (3) ) ;  sin( qRef (3) ) ] ;

17   D   =  qRef (1:2) ;   %  Destination point

18   S  =  q (1:2) ;   %  Robot position

19   M   =  (D   +  S) /2;

20   Ov  =   [cos(q (3) ) ;  sin(q (3) ) ] ;   %  Orientation vector

21   SDv  =   D  −   S ;   %  SD  vector

22   l2   =  norm(SDv) ;   %  Distance

23

24    if  slowDown

25     v  =  v −   aMax * Ts ;   if  v  <  0 , v  =   0;  end

26     w   =   0;

27    else

28     if   fin . ’*X *SDv  >  SDv. ’*X * fin

29       Ps  =   D  −   Rr * X . ’* f i n ;   %  Circle centre

30  else

31   Ps  =   D  −   Rr * X * f i n ;   %  Circle centre

32  end

33

34  l   =  norm(Ps− S ) ;

35  if   l   <   curveZone/2

36   Dv  =   fin ;

37  else

38   d  =  sqrt(sum((S− Ps ) .ˆ2)  −   Rr ˆ2) ;

39   alpha  =  atan(Rr/d) ;

40   phi  =  wrapTo2Pi(atan2(Ps (2)− S (2) ,   Ps (1)− S (1) ) ) ;

41   U1  =  S  +  d*[cos( phi+alpha ) ;  sin( phi+alpha ) ] ;

42   U2  =  S  +  d*[cos( phi− alpha ) ;   sin ( phi− alpha ) ] ;

43   i f   ((U1 −   S ) . ’* X *( Ps  −   U1 ) )   *   ( f i n . ’* X *(Ps  −   D ) )   >=   0

44     D   =  U1;

45   else

46     D   =  U2;

47   end

48   M   =  (D   +  S) /2;

49   SDv  =   D  −   S ;

50   Dv   =  SDv/(norm(SDv)+ eps) ;

51 end

52

53  if   l2   >  accuracy  %  if  the position  i s  not reached

54   v  =  v  +  aMax*Ts ;  if  v  >  vMax,  v  =  vMax;  end

55

56   Ev  =   X*(D− S ) ;

57   DTv   =   X * Dv ;

58   if  abs(DTv. ’*X *Ev)  <  0.000001  %  Go  on a straight  l i n e

59     gamma   =   0;

60     Sv  =  SDv/(norm(SDv)+ eps) ;

61  else   %  Go  on a  circle

62    C   =   DTv  * Ev. ’*X*(D   −   M ) /( DTv . ’* X * Ev )   +   D;   %

63     Circle   centre

64    if  SDv. ’*X *Dv  >  0 , a  =   1;   else  a  =  −1;   end

65   Sv   =  a* X*(C − S ) ;

66   Sv   =   Sv/(norm(Sv)+ eps) ;

67   gamma   =  a*acos(Dv. ’* Sv) ;

68  if  a*Sv . ’*X *Dv  <  0 , gamma   =  a*2*pi −   gamma ;   end

69  l   =  abs(gamma *norm(S− C ) ) ;   %  Curve length

70 end

71

72    if  v  >  eps

73     if  Ov. ’* Sv  <  0 , vDir  =   −1;   else   vDir   =   1;   end  %

74        Direction

75   ePhi  =  acos( vDir*Sv . ’*Ov) ;   %  Angular error

76    if  vDir*Ov. ’*X *Sv  <  0 , ePhi  =  − ePhi ;   end

77    dt  =   l /v ;  if  dt  <  0.00001 , dt  =   0.00001;  end

78    w   =  gamma /dt  +  ePhi/dt *10*(1− exp(− l2 /0.1) ) ;   %

79     Angular speed

80  else

81    w   =   0;

82 end

83  else

84   slowDown  =  true ;

85  end

86 end

87 u  =   [ vDir*v ;   w] ;   %  Tangential and angular velocity

88

89  %  Robot motion simulation

90  dq  =   [ u(1) *cos(q (3) ) ;  u(1) *sin(q (3) ) ;  u(2) ] ;

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

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

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

94 end

From Reference Pose Control to Reference Path Control

Often the control goal is given with a sequence of reference points that a robot should drive through. In these cases we no longer speak about reference pose control but rather a reference path is constructed through these points. Often straight line segments are used between the individual points, and the control goal is to get to each reference point with the proper orientation, and then automatically switch to the next reference point. This approach is easy to implement and usually sufficient for use in practice. Its drawback is the nonsmooth transition between neighboring line segments, which causes jumps of control error in these discontinuities.

The path is defined by a sequence of points T_i = [x_i, y_i]^T, where i ∈ 1, 2, …, n, and n is the number of points. In the beginning the robot should follow the first line segment (between points T₁ and T₂), and it should arrive to T₂ with orientation defined by the vector $\overrightarrow{{\mathit{T}}_2{\mathit{T}}_2}$. When it reaches the end of this segment it starts to follow the next line segment (between the points T₂ and T₃), and so on. Fig. 3.13 shows actual line segment between points T_i and T_i+1 with variables marked. Vector v =T_i+1 −T_i = [Δx, Δy]^T defines the segment direction, and vector r = R −T_i is the direction from point T_i to the robot center R. The vector v_n = [Δy, −Δx] is orthogonal to the vector v.

The robot must follow the current line segment while the projection of vector r to vector v is inside the interval defined by points T_i in T_i+1. This condition can be expressed as follows:

$\begin{array}{l}\hfill \left\{\begin{array}{lll}\text{Follow the current segment}({\mathit{T}}_i,{\mathit{T}}_{i+1})& \text{if}& 0<u<1\\ \text{Follow the next segment}({\mathit{T}}_{i+1},{\mathit{T}}_{i+2})& \text{if}& u>1\\ \end{array}\right.\end{array}$

where u is the dot product

$\begin{array}{l}\hfill u=\frac{{\mathit{v}}^T\mathit{r}}{{\mathit{v}}^T\mathit{v}}\end{array}$

Variable u is used to check if the current line segment is still valid or the switch to the next line segment is necessary.

Orthogonal distance of the robot from the line segment is defined using the normal vector v_n:

$\begin{array}{l}\hfill d=\frac{{\mathit{v}}_n^T\mathit{r}}{\sqrt{{\mathit{v}}_n^T{\mathit{v}}_n}}\end{array}$

If we normalize distance d by the distance length we obtain normalized orthogonal distance d_n between the robot and the straight line:

$\begin{array}{l}\hfill d_n=\frac{{\mathit{v}}_n^T\mathit{r}}{{\mathit{v}}_n^T{\mathit{v}}_n}\end{array}$

Normal distance d_n is zero if the robot is on the line segment and is positive if the robot is on the right side of the segment (according to vector v) and vice versa. Normal distance d_n is used to define desired direction or robot motion. If the robot is on the line segment or very close to it then it needs to follow the line segment. If, however, the robot is far away from the line segment then it needs to drive perpendicularly to line segment in order to reach the segment faster. The reference orientation of driving in a certain moment can be defined as follows:

$\begin{array}{l}\hfill {\phi }_{\mathrm{ref}}={\phi }_{\mathrm{lin}}+{\phi }_{\mathrm{rot}}\end{array}$

where φ_lin = arctan2(Δy, Δx) (note that the function arctan2 is the four-quadrant inverse tangent function) is the orientation of the line segment and ${\phi }_{\mathrm{rot}}=arctan(K_1{d}_n)$ is an additional reference rotation correction that enables the robot to reach the line segment. Gain K₁ changes the sensitivity of φ_rot according to d_n. Because φ_ref is obtained by addition of two angles it needs to be checked to be in the valid range [−π, π].

So far we define a reference orientation that the robot needs to follow using some control law. The control error is defined as

$\begin{array}{l}\hfill e_{\phi }={\phi }_{\mathrm{ref}}-\phi \end{array}$

where φ is the robot orientation. From orientation error the robot’s angular velocity is calculated using a proportional controller:

$\begin{array}{l}\hfill \omega =K_2{e}_{\phi }\end{array}$

where K₂ is a proportional gain. Similarly, also a PID controller can be implemented, where the integral part speeds up the decay of e_φ while the differential part damps oscillations due to the integral part. Translational robot velocity can be controlled by basic approaches discussed in the preceding sections.

Example 3.6

Write a control algorithm for a differentially driven robot to drive on the sequence of line segments defined by points T₁ = [3, 0], T₂ = [6, 4], T₃ = [3, 4], T₄ = [3, 1], and T₅ = [0, 3]. Find appropriate values for parameters K₁ and K₂. The initial pose of the vehicle is [x(0), y(0), φ(0)] = [5 m, 1 m, 108 degree]. Test the algorithm on a simulated kinematic model.

Solution

An M-script code of a possible solution is given in Listing 3.5. The reference path and the actual robot motion curve are shown in Fig. 3.14, and the control signals are shown in Fig. 3.15.

Listing 3.5

1 Ts =  0.03;   %  Sampling time

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

3 T   =   [3 ,   0;  6 ,  4;  3 ,  4;  3 ,  1;  0 ,  3 ] . ’ ;   %  Reference points of  l i n e  segments

4 q  =   [ 5 ;   1;  0.6*pi ] ;   %   Initial  pose

5

6 i   =   1;   %  Index of the  first  point

7 for k  =   1:length( t )

8   %  Reference segment determination

9   dx  =   T(1 , i +1) −   T (1 , i ) ;

10  dy   =   T(2 , i +1) −   T (2 , i ) ;

11

12  v   =   [ dx ;  dy ] ;   %  Direction vector of the current segment

13  vN  =   [ dy ;  − dx ] ;   %  Orthogonal direction vector of the current segment

14  r  =  q (1:2)  −   T ( : , i ) ;

15  u   =   v. ’* r /(v . ’* v) ;

16

17  if  u>1  &&  i< s i z e (T,2)−1   %  Switch to the next  l i n e  segment

18    i   =   i   +   1;

19   dx  =   T(1 , i +1) −   T (1 , i ) ;

20   dy  =   T(2 , i +1) −   T (2 , i ) ;

21   v   =   [ dx ;  dy ] ;

22   vN  =   [ dy ;  − dx ] ;

23   r   =   q(1:2)  −   T ( : , i ) ;

24 end

25

26 dn  =  vN. ’* r /(vN. ’*vN) ;   %  Normalized orthogonal distance

27

28 phiLin  =  atan2(v (2) , v (1) ) ;   %  Orientation of the  l i n e  segment

29 phiRot  =  atan(5*dn) ;   %  if  we are far from the  l i n e  then we need

30 %  additional rotation to face towards the  l i n e .  I f  we are on the  l e f t

31 %  side of the  l i n e  we turn clockwise , otherwise counterclock wise.

32 %  Gain 5 increases the  s e n s i t i v i t y   . . .

33

34  phiRef  =  wrapToPi( phiLin  +  phiRot ) ;

35

36  %  Orientation error for control

37  ePhi  =  wrapToPi( phiRef −   q (3) ) ;

38

39  %  Controller

40  v  =  0.4*cos( ePhi ) ;

41  w   =  3*ePhi ;

42

43  %  Robot motion simulation

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

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

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

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

48 end

Try to extend the code given in the solution so that the robot could also drive in reverse if $|e_{\phi }|>\frac{\pi }{2}$.