Theory about Controllers

Problem definition

The Comparator (Cp) is an element of the Autonomous Car (AC) control loop which connects three elements. The Cp receives inputs from the Path Planning and from the Filter and outputs data to the Controller. Its name reveals its task: compare the desired positions of the AC (computed with the Path Planning element) with the AC current position (estimated with the Filter element). The result of this operation, named the error, is outputted to the Controller.

We will remind that the desired position of the AC is a smooth trajectory which links the start point with the target point. In fact, the trajectory is defined like a set of m points.

$\begin{displaymath} T_{j}=\{P_{i}|P_{i}=[x_{i},y_{i}]^T,i=1,...,m\} \end{displaymath}$

(1)

Usually, for a close loop system, a comparator computes the error by subtracting the estimated value from the desired value. This solution is not possible for the AC comparator. Here the goal is to be as close as possible to the trajectory, which links the mentioned points and not to jump from one point to another.

CoCt_1

Figure 1. Comparing the estimated position (red dot) with the desired positions (black dots)

This observation suggests that it is more practical to relate the robot estimated position with the segment defined by two successive points (see figure 1). This means that, in a particular moment of locomotion, from all of m-1 segments, one segment must be selected. We will name this segment the active segment.

The Controller element (Ct) receives the input from the comparator (Co) and output the command signal to the AC. For our close loop system model the command signal generates the AC steering angle. Because there are many possibilities to define the Ct transfer function and to compute its gains, the Ct problem consist on choosing a solution from these possibilities.

In conclusion the Cp, Ct problems consist on:

For the Cp is necessary to define :
- A procedure for the active segments selection;
- The procedure to compute the error;
For the Ct is necessary to select:
- A suitable transfer function;
- A method of gains computation.

The errors computation

The error is defined like the distance from the AC current position to the active segment. In figure 2 we have illustrated this concept for two positions of the AC.

CoCt_2

Figure 2. The error definition

Figure 2 illustrates two possible position of the AC which are symmetric relative to the vector defined from point i to point i+1. Even the length of the errors, illustrated in figure 2, is equal, the active segment orientation gives us the possibility to distinct them by defining the sign of the error

$\begin{displaymath} \mathbf{u}=\left[\begin{array}{cccc} x_{i-1}-x\\ y_{i-1}-y\\ 0 \end{array}\right] \end{displaymath}$

$\begin{displaymath} \mathbf{v}=\left[\begin{array}{cccc} x_{i-1}-x_{i}\\ y_{i-1}-y_{i}\\ 0 \end{array}\right] \end{displaymath}$

(2)

$\begin{displaymath} \mathbf{w}= \mathbf{v}\times\mathbf{u} \end{displaymath}$

(3)

$\begin{displaymath} e=sgn(\mathbf{w})\sqrt{|\mathbf{u}|^2-\left(\frac{\mathbf{u}^T\mathbf{v}}{|\mathbf{v}|\right)^2}} \end{displaymath}$

(4)

The active segment selection depends on the angel between vectors u and v i.e. if the angel is bigger than 90^o then the active segment will be replaced from $P_{i}P_{i+1}$ to $P_{i+1}P_{i+2}$ .

CoCt_3

Figure 3. The active segment selection

$\begin{displaymath} v=\Bigg\{\begin{array}{cccc} P_{i}P_{i+1}\:if\:P_{i}P_{i+1}\cdot\mathbf{u}\>0\\ P_{i+1}P_{i+2}\:if\:P_{i}P_{i+1}\cdot\mathbf{u}\le0 \end{array} \end{displaymath}$

(5)

Another method used in active segment selection is to compare the projection of vector v on the support of vector u with the length of vector u.

$\begin{displaymath} v=\Bigg\{\begin{array}{cccc} P_{i}P_{i+1}\:if\:P_{i}P_{i+1}\cdot\mathbf{u}<|\mathbf{u}|\\ P_{i+1}P_{i+2}\:if\:P_{i}P_{i+1}\cdot\mathbf{u}\ge|\mathbf{u}| \end{array} \end{displaymath}$

In conclusion the error computation procedure consists on:

Select the first segment like the active segment;
Until condition (5) allows it compute the error with (4);
Change the active segment accordingly to (5) and go to step 2, until the last segment is achieved

The command signal computation

The chosen controller is the PID. The controller transfer function is given by:

$\begin{displaymath} u=K_{P}e+K_{V}\dot{e}+K_{I}\int_{0}^{t}e(\tau)d(\tau) \end{displaymath}$

(6)

where: u is the command signal, e is the error computed with (4) and K_P,V,I are the controller gains.

In order to compute the gains several methods can be used. We have selected the twiddle method which consists on searching for a particular combination of the gains values which minimize the value of the Total Error (TE).The TE is a cumulated error of tracking an imposed trajectory. Shortly, the twiddle is an iterative algorithm and can be describe by the following steps:

Impose a number of total iterations N_max, and an error tolerance T_ol;
Choose a trajectory; This can be a straight line; an open or an closed contour;
Propose (guess) an initial set of values for the gains Gains=[K_P,K_V,K_I];
Impose an initial adjustment increment for each gain d_Gains=[dK_P,dK_V,dK_I];
Run a simulation of the AC which follow the desire trajectory; Compute the TE (7);
Modify the gains by increasing or decreasing them accordingly to the mentioned increments Gains=Gains+d_Gains; The modifications are made in the following order: the proportional gain; the derivative gain and the integrative gain;
After each modifications of the gain, a simulation is made and the actual value of the TE is computed;
If the actual value of the TE is smaller than the error tolerance or if the number of iterations is equal to the total number of the iterations stop the searching loop;
Compare the actual value of the TE with its previous value; Accordingly modify the increments d_Gains and go to 6; increase the number of iteration with 1;

$\begin{displaymath} TE=\sum_{P_{1}}^{P_{m}}|e_{i}| \end{displaymath}$

(7)

Diag

Figure 4. The twiddle algorithm flowchart

Figure 4 contain the previous algorithm flowchart. The flowchart is composed from an initialization block and continues with an iterative block which modifies the controller parameters. The iterative block is composed from three blocks which have an identical structure but refers to different gains.

CoCt_5

Figure 5. The close loop diagram used in twiddle algorithm

Several observations are necessary.

In order to compute the TE simulations are made. The simulations consist on controlling the AC to track the chosen trajectory. The diagram from figure 5 illustrates the close loop system used to simulate the trajectory tracking.
The computed command u is related to the front AC wheel steering angle γ (see figure 6):
- in an proportional way

$\begin{displaymath} \gamma=ku \end{displaymath}$

(8)

or, in order to saturate the value of the steering angel to

$\begin{displaymath} \gamma=\gamma_{max}\Bigg(\frac{2}{1+e^{-frac{u}{2}}}-1\Bigg) \end{displaymath}$

(9)

gama

Figure 6. The steering angle function

Figure 7 illustrate the connection between the error sign and the steering angle sign. For more details of AC models consult the Model section of this site.

CoCt_7

Figure 6. The steering angle for two possible situations