**3. Control System and Model of Dynamics of the Vehicles**

*3.1. Structure of the Autonomous Vehicle's Control System*

The CT unit's motion results from the cooperation of the following three major system components:


The cooperation of the above system components has been illustrated in Figure 2.

**Figure 2.** Structure of the control system.

#### *3.2. Procedure of Determining the Control Signal Using a Fuzzy Logic Model*

The sudden intrusion of an obstacle onto the lane used by the autonomous vehicle, as analyzed herein, means that an obstacle (*y*<sup>0</sup> > 0) appears at a distance of *x*<sup>0</sup> ahead of the vehicle (Figure 1). A case is addressed where this distance may be shorter than the stopping distance *SZ* for the CT unit. Then, a safe solution may be to avoid the obstacle with using the adjacent road lane. In such a case, the method detecting the obstacle edge *K* by the environment perception system is very effective [30]. Based on an analysis of the current position of edge *K* relative to the *Rmin* lane edge, the predicted position of the target point *P* (Figure 1), i.e., the *yP* value, is calculated from Equation (1):

$$y\_P = y\_0 + 0.5b + y\_W \tag{1}$$

This makes it possible to calculate trajectory *yM*(*x*) in the control system (Figure 3); the trajectory is treated as the desired (planned) path of the center of mass *CA* within the *x*<sup>0</sup> road section [3]. The selection of an algorithm to generate this trajectory has been described in a subsequent part of this paper. The said trajectory is applied as an input to the anticipating model, where trajectory *yT*(*x*) is calculated based on equations:

$$y\_T(\mathbf{x}) = 0.5d \text{ for } \mathbf{x} \in \langle 0; L\_d \rangle$$

$$y\_T(\mathbf{x}) = y\_M \Big(\frac{\mathbf{x} - L\_d}{\bar{\mathbf{x}}\_0 - L\_x} \mathbf{x} \Big) \text{ for } \mathbf{x} \in \langle L\_d; \mathbf{x}\_0 \rangle \tag{2}$$

$$y\_T(\mathbf{x}) = y\_0 \text{ for } \mathbf{x} = \mathbf{x}\_0$$

**Figure 3.** Schematic diagram of the processing of trajectory *yM*(*x*) in order to determine the steering wheel angle *δ<sup>H</sup>* settings.

In Equation (2), the properties of the anticipating model and the value of the anticipation radius *La* are made use of. The determination of the anticipation radius value, together with the method of planning the vehicle trajectory for the critical situation under analysis, has been presented in Section 6. Trajectory *yT*(*x*) and path *yCA*(*x*) of the center of mass of the motor vehicle make a basis for determining the Δ*y* and Δ*β* values.

The Δ*y* and Δ*β* values describe the divergence between the trajectory planned and the actual vehicle path. They have been shown in Figure 4:

$$\begin{array}{l} \Delta y = y\_T(\mathbf{x}) - y\_{CA}(\mathbf{x})\\ \Delta \beta = \beta\_T(\mathbf{x}) - \beta\_{CA}(\mathbf{x}) \end{array} \tag{3}$$

The fuzzy logic model used in the control system minimizes the Δ*y* and Δ*β* values by immediate and ongoing adjustment of the steering wheel angle *δ<sup>H</sup>* in the model of vehicle dynamics. The fuzzy logic model is shown in Figure 5. It represents the connection between the input signals (Δ*y* and Δ*β*) and the steering wheel angle values *δ<sup>H</sup>* necessary for the obstacle to be avoided. The model includes inference rules based on associating the input signal values with possible logic states of these parameters. The inference rules are based on functions with trapezoidal profiles. In the model, the limit values of the input signals have been adopted as limitations *<sup>δ</sup>HMAX* and . *δHMAX*, determined by motor vehicle construction. To pass from the logic value of the input signal to the resultant value of the steering wheel angle *δH*, the analytical centrode model was used.

**Figure 4.** Illustration for determining Δ*y* and Δ*β* based on the course of the *yT*(*x*) and *yCA*(*x*) curves at point *xi*.

**Figure 5.** Structure of the fuzzy logic model used.

#### *3.3. Model of Dynamics of the CT Unit*

3.3.1. Generalized Coordinates; Equations of Motion of the CT Unit

The model of a CT unit is shown in Figure 6. It is generated in the PC-CRASH computer program, which is used to model the movement of vehicles in collision situations [31,32]. To describe and analyze the CT unit's motion, global and local coordinate systems have been used, pursuant to ISO 8855:


The vehicle bodies are solid with 6 degrees of freedom. Each of the wheels has a degree of freedom related to its rotational motion, which means that it has a moment of inertia relative to the axis of wheel rotation. Road wheels are linked with vehicle bodies by spring and damping elements with non-linear characteristics. They can move parallel to the *CsZs* axis relative to the vehicle body.

**Figure 6.** Model of a CT unit and the coordinate systems.

The interdependences between the coordinate systems may be described as follows:

$$
\begin{bmatrix} X \\ Y \\ Z \end{bmatrix} = N\_{\text{OCs}} \begin{bmatrix} X\_s \\ Y\_s \\ Z\_s \end{bmatrix} \tag{4}
$$

(global system {*O*} vs. local system {*Cs*} as defined above)

$$\mathbf{N}\_{\rm OCs} = \begin{bmatrix} \cos\psi\_{\rm s}\cos\theta\_{\rm s} & \cos\psi\_{\rm s}\sin\theta\_{\rm s}\sin\varphi\_{\rm s} - \sin\psi\_{\rm s}\cos\varphi\_{\rm s} & \cos\psi\_{\rm s}\sin\theta\_{\rm s}\cos\varphi\_{\rm s} + \sin\psi\_{\rm s}\sin\varphi\_{\rm s} \\ \sin\psi\_{\rm s}\cos\theta\_{\rm s} & \sin\psi\_{\rm s}\sin\theta\_{\rm s}\sin\varphi\_{\rm s} + \cos\psi\_{\rm s}\cos\varphi\_{\rm s} & \sin\psi\_{\rm s}\sin\theta\_{\rm s}\cos\varphi\_{\rm s} - \cos\psi\_{\rm s}\sin\varphi\_{\rm s} \\ -\sin\theta\_{\rm s} & \cos\theta\_{\rm s}\sin\varphi\_{\rm s} & \cos\theta\_{\rm s}\cos\varphi\_{\rm s} \end{bmatrix} \tag{5}$$

where:

*ψs*, *θs*, *ϕs*— quasi-Euler angles, defining the orientation of the local system {*Cs*} relative to the global system {*O*}, i.e.:

*ψs*— yaw angle; *θs*— pitch angle;

*ϕs*— roll angle;

$$
\begin{bmatrix} X\_s \\ Y\_s \\ Z\_s \end{bmatrix} = N\_{\text{CsOTu}} \begin{bmatrix} x\_{Th} \\ y\_{Th} \\ z\_{Th} \end{bmatrix} \tag{6}
$$

(local system {*Cs*} vs. local system {*OTu*}, as defined above);

$$N\_{\rm C\*OTu} = \begin{bmatrix} \cos \delta\_u & -\sin \delta\_u & 0\\ \sin \delta\_u & \cos \delta\_u & 0\\ 0 & 0 & 1 \end{bmatrix} \tag{7}$$

where:

*δu*—steering angle of the front left and front right wheel of the motor vehicle (*u* = [1,2]).

The steering angles of individual wheels are in conformity with the Ackermann model and are determined by the steering wheel angle *δH*. The values of this angle are calculated in the control system (Figure 3).

For the non-steerable wheels, the coordinate systems {*OT*3} and {*OT*4} are parallel to the {*CA*} system and the {*OT*5} and {*OT*6} systems are parallel to the {*CB*} system. The transforms describing the interdependence between these systems are unit matrices (e.g., Equation (7) for *δ<sup>u</sup>* = 0).

The physical model of the motorcar consists of a vehicle body and 4 road wheels; for the trailer, the physical model consists of a trailer body and 2 wheels. The models of dynamics of the car and the trailer, if treated separately, have 10 and 8 degrees of freedom, respectively. When the motorcar and the trailer are coupled together by means of a ball joint *OAB* (Figure 6), constraints are imposed on the trailer's motion and the number of the degrees of freedom of the CT unit is thus reduced to 15.

The set of the generalized coordinates, which completely define the car and trailer's positions, may be written as follows:

$$q\_A = \begin{bmatrix} \pounds\_A \y\_A z\_A \psi\_A \theta\_A \varphi\_A \omega\_1 \omega\_2 \omega\_3 \omega\_4 \end{bmatrix}^T \tag{8}$$

$$q\_B = \begin{bmatrix} \psi\_B \theta\_B \varphi\_B \omega\_{\mathbb{S}} \omega\_{\mathbb{G}} \end{bmatrix}^T \tag{9}$$

The CT unit's motion can be described by vectorial equations [30,31]:

$$m\_s \left(\ddot{r}\_s + \Omega\_s \times \dot{r}\_s\right) = \sum\_i^n F\_{sl} \tag{10}$$

$$T\_s \dot{\Omega}\_s + \Omega\_s \times T\_s \Omega\_s = \sum\_{j}^{k} \mathcal{M}\_{sj} \tag{11}$$

where:

*ms*—vehicle mass;

*rs*—vector from the origin of the global coordinate system to the center of mass *Cs* in the global coordinate system {*O*}; *rs* = *xs ys zs <sup>T</sup>* (cf. Figure 6); **.** *rs* = *vs*; **..** *rs* = *as*;

*Fsi*, *Msj*— generalized external forces and moments acting on vehicle *s*;

*Ts*—tensor of inertia of vehicle *s* relative to the vehicle center of mass in the local coordinate system {*Cs*};

**Ω***s*—vector of the yaw velocity of the body of vehicle *s* in the local coordinate system {*Cs*}; **Ω***<sup>s</sup>* =  . *ϕs* . *θs* . *ψs T* .
