**8. A Computational Example**

Let us consider the optimal control problem for two mobile robots. They have to exchange its position on the plane with obstacles.

Mathematical models of mobile robots [45] are given

$$\begin{array}{rcl} \dot{x}^{j} &=& 0.5(\boldsymbol{u}\_{1}^{j} + \boldsymbol{u}\_{2}^{j})\cos(\theta^{j}),\\ \dot{y}^{j} &=& 0.5(\boldsymbol{u}\_{1}^{j} + \boldsymbol{u}\_{2}^{j})\sin(\theta^{j}),\\ \dot{\theta}^{j} &=& 0.5(\boldsymbol{u}\_{1}^{j} - \boldsymbol{u}\_{2}^{j}),\end{array} \tag{64}$$

where **u***<sup>j</sup>* = [*u<sup>j</sup>* 1 *uj* <sup>2</sup>] is a vector of control, *j* = 1, 2.

Control is restricted

$$1 - 10 = u\_i^- \le u\_i^j \le u\_i^+ = 10, \; j = 1, 2, \; i = 1, 2. \tag{65}$$

The initial conditions are set

$$\text{tr}^1(0) = 0, \, y^1(0) = 0, \, \theta^1(0) = 0, \, \text{x}^2(0) = 10, \, y^2(0) = 10, \, \theta^2(0) = 0. \tag{66}$$

The terminal conditions are set

$$\mathbf{x}^1(t\_f) = 10, \ y^1(t\_f) = 10, \ \theta^1(t\_f) = 0, \mathbf{x}^2(t\_f) = 0, \ y^2(t\_f) = 0, \ \theta^2(t\_f) = 0,\tag{67}$$

where

$$t\_f = \begin{cases} \ t \ \text{, if } t < t^+ \text{and } \Delta\_f(t) \le \varepsilon \\\ t^+ \text{, otherwise} \end{cases} \tag{68}$$

$$\Delta\_f(t) = \sqrt{(10 - \mathbf{x}^1(t))^2 + (10 - y^1(t))^2 + (\theta^1(t))^2 + (\mathbf{x}^2(t))^2 + (y^2(t))^2 + (\theta^2(t))^2} \tag{69}$$
  $t^+ = 2.4$  s,  $\varepsilon = 0.01$ .

The quality functional includes the time to reach the terminal state and penalty functions for violation of the accuracy of reaching the terminal state and for violation of static and dynamic phase constraints

$$J\_{\mathfrak{c}} = t\_f + w\_1 \Delta\_f(t\_f) + w\_2 \int\_0^{t\_f} \sum\_{i=1}^2 \sum\_{j=1}^2 \theta(\varphi\_{i,j}(t)) dt + \mathcal{O}(\delta)$$

$$w\_3 \int\_0^{t\_f} \theta(d^2 - (\mathbf{x}^1(t) - \mathbf{x}^2(t))^2 - (y^1(t) - y^2(t))^2) dt \to \min\_{\mathbf{u}^1, \mathbf{u}^2} \tag{70}$$

where *w*<sup>1</sup> = 2.5, *w*<sup>2</sup> = 3, *w*<sup>3</sup> = 3,

$$\theta(\alpha) = \begin{cases} 1, \text{if } \alpha > 0 \\ 0, \text{otherwise} \end{cases} \tag{71}$$

$$\varphi\_{i,j}(t) = r\_i - \sqrt{(\mathbf{x}\_i - \mathbf{x}^j(t))^2 + (y\_i - y^j(t))^2}, \ i = 1, 2, \ j = 1, 2,\tag{72}$$

*r*<sup>1</sup> = 3, *r*<sup>2</sup> = 3, *x*<sup>1</sup> = 5, *x*<sup>2</sup> = 5, *y*<sup>1</sup> = 9, *y*<sup>2</sup> = 1, *d* = 2.

It is necessary to find such a control to move all robots from its initial conditions (66) to the terminal conditions (67) with the minimal value of the quality criterion (70).

To solve the optimal control problem (64)–(72) by the proposed synthesized optimal control method it is necessary to initially solve the control synthesis problem (45)–(48) for each robot. Since robots are similar, it is enough to solve the control synthesis problem once for one robot. For the solution of this problem, the symbolic regression method of Cartesian genetic programming is used.

In the result, the following control function was obtained:

$$u\_i^j = \begin{cases} u\_i^+ = 10, \text{if } u\_i^+ \le \bar{u}\_i^j \\\ u\_i^- = -10, \text{if } \bar{u}\_i^j \le u\_i^- \\\ \bar{u}\_{i'}^j, \text{otherwise} \end{cases}, i = 1, 2, \ j = 1, 2,\tag{73}$$

where

$$\bar{u}\_1^j = A + B + \rho\_\#(A), \; j = 1, 2,\tag{74}$$

$$\bar{u}\_1^j = B - A - \rho\_\#(A), \; j = 1, 2,\tag{75}$$

$$A = c\_1(\theta^\* - \theta^j) + \sigma\_\theta((\mathbf{x}^\* - \mathbf{x}^l)(\underline{y}^\* - \underline{y}^l)),\tag{76}$$

$$B = 2(\mathbf{x}^\* - \mathbf{x}^{\bar{j}}) + \text{sgn}(\mathbf{x}^\* - \mathbf{x}^{\bar{j}})c\_{2,\prime} \tag{77}$$

$$\rho\_{\theta}(a) = \begin{cases} \text{sgn}(a)B^{+}, \text{if } |a| > -\log(\delta^{-})\\ \text{sgn}(a)(\exp(|a|) - 1) \end{cases}, \; \sigma\_{\theta}(a) = \text{sgn}(a)\sqrt{|a|}. \tag{78}$$

*c*<sup>1</sup> = 3.1094, *c*<sup>2</sup> = 3.6289, *B*<sup>+</sup> = 108, *δ*<sup>−</sup> = 10−8.

For solution of the synthesis problem eight initial conditions were used and the quality criterion took into account the speed and the accuracy of terminal position achievement

$$\mathbf{x}^\* = \begin{bmatrix} x^\* \ y^\* \ \theta^\* \end{bmatrix}^T. \tag{79}$$

In the result of the solution of control synthesis problem a stable equilibrium point in the state space is appeared. Position of the equilibrium point depends on the terminal vector (79).

In the second stage the set of four points (79) were searched for each robot on criterion (70)

$$\mathbf{X}^\* = \{ \mathbf{x}^{\*,1,1}, \dots, \mathbf{x}^{\*,1,4}, \mathbf{x}^{\*,2,1}, \dots, \mathbf{x}^{\*,2,4} \}. \tag{80}$$

These points were switching in some time interval Δ*t* = 0.6 s for control function (73) of each robot.

To search for the points the evolutionary algorithm of Grey wolf optimizer [46,47] was used. In result, after more than one hundred tests the following best points were found:

$$\begin{array}{rcl} \mathbf{x}^{\*,1,1} &=& [4.0159 \, 1.8954 \, 1.2397]^T, & \mathbf{x}^{\*,1,2} &=& [7.0890 \, 4.2341 \, 0.5270]^T, \\ \mathbf{x}^{\*,1,3} &=& [7.2194 - 0.4480 \, 1.3042]^T, & \mathbf{x}^{\*,1,4} &=& [11.9722 \, 9.4663 \, 0.1866]^T, \\ \mathbf{x}^{\*,2,1} &=& [5.3899 \, 4.0791 - 0.1208]^T, & \mathbf{x}^{\*,2,2} &=& [-0.6401 \, 4.3126 - 0.0176]^T, \\ \mathbf{x}^{\*,2,3} &=& [0.3103 \, 0.8955 \, 0.6335]^T, & \mathbf{x}^{\*,2,4} &=& [-0.0791 - 0.1518 \, 0.0195]^T. \end{array} \tag{81}$$

The algorithm simulated the system (64) with the control (73) for calculation of criterion values (70) in one test more than 500,000 times.

When searching for points, the following constraints were used

$$-2 \le \mathbf{x}^\* \le 12, \ -2 \le \mathbf{y}^\* \le 12, \ -\pi/2 \le \theta^\* \le \pi/2. \tag{82}$$

In the Figure 1 the projections of optimal trajectories on the plane {*x*, *y*} are presented. The trajectories are black lines, red circles are obstacles, small black squares are projections of found points (81).

**Figure 1.** Optimal trajectories of robots on the plane {*x*, *y*} for synthesized optimal control.

The quality criterion (70) for found control was *Je* = 2.8914.

For comparative study of the obtained solution, the same optimal control problem was solved by a direct method. For this purpose control functions of robots were approximated by piece-wise linear functions of time. The interval of approximation was Δ*dt* = 0.4 s, therefore a number of intervals was

$$K = \left\lfloor \frac{t^+}{\Delta\_d t} \right\rfloor = \left\lfloor \frac{2.4}{0.4} \right\rfloor = 6.\tag{83}$$

For the approximation of control function, the values of parameters on the boundaries of intervals were searched. For each one control function it was necessary to find *K* + 1 = 7 parameters. Total vector of parameters had twenty eight components.

$$\mathbf{q} = [q\_1 \dots q\_{28}]^T.\tag{84}$$

The direct control has the following form

$$u\_i^j = \begin{cases} 10 = u\_i^+ \text{, if } u\_i^+ \le \bar{u}\_i^j \\\ -10 = u\_i^- \text{, if } \bar{u}\_i^j \le u\_i^- \text{, } i = 1, 2, \ j = 1, 2, \\\ \bar{u}\_{i'}^j \text{, otherwise} \end{cases} \tag{85}$$

where

$$
\mu\_1^1 = q\_s + (q\_{s+1} - q\_s) \frac{(t - s\Delta\_d t)}{\Delta\_d t},
\tag{86}
$$

$$
\overline{u}\_2^1 = q\_{s+L} + (q\_{s+L+1} - q\_{s+L}) \frac{(t - s\Delta\_d t)}{\Delta\_d t} \,\tag{87}
$$

$$
\overline{a}\_1^2 = q\_{s+2K} + (q\_{s+2L+1} - q\_{s+2L}) \frac{(t - s\Delta\_d t)}{\Delta\_d t},
\tag{88}
$$

$$
\Delta \overline{u}\_2^2 = q\_{s+3L} + (q\_{s+3L+1} - q\_{s+3L}) \frac{(t - s\Delta\_d t)}{\Delta\_d t},\tag{89}
$$

*s*Δ*dt* ≤ *t* ≤ (*s* + 1)Δ*dt*, *s* ∈ {1, . . . , 6}, *L* = *K* + 1 = 7.

To search for optimal parameters the same evolutionary algorithm of Grey wolf optimizer was used. In the result of more than one hundred tests the following best values of parameters were found:

$$\begin{array}{rcl} \mathbf{q} & = & [19.6125 \ 5.4318 \ 7.5921 \ 19.4020 \ 2.3928 \ 2.1627 \ 1.6976 \\ & 1.4941 \ 5.1828 \ 16.9087 \ 11.2478 & -2.4499 \ 17.7201 \ -0.6297 \\ & -0.9093 \ -1.6815 \ -19.5283 \ -16.4979 \ -0.2321 \ -11.4719 \ -17.7372 \\ & -1.4218 \ -18.0214 \ -3.7942 \ -3.0899 \ -13.3196 \ -9.7212 \ -0.3233]^T \\ \end{array} \tag{90}$$

The process of searching the parameters had restrictions

$$-20 = q^- \le q\_i \le q^+ = 20, \ i = 1, \dots, 28. \tag{91}$$

In one test, the algorithm simulated the system (64) with the control (85) for calculation of criterion values (70) more than 500,000 times. A value of quality criterion (70) for found control was *Je* = 2.5134.

In Figure 2, the projection of optimal trajectories of mobile robots on the horizontal plane {*x*, *y*} is presented.

**Figure 2.** Optimal trajectories of robots on the plane {*x*, *y*} for direct control.

To check the obtained solutions of sensitivity to perturbations, we included random functions of uncertainty into the model (64)

$$\begin{array}{rcl} \dot{x}^j &=& 0.5(\boldsymbol{\mu}\_1^j + \boldsymbol{\mu}\_2^j)\cos(\theta^j) + B\tilde{\xi}(t),\\ \dot{y}^j &=& 0.5(\boldsymbol{\mu}\_1^j + \boldsymbol{\mu}\_2^j)\sin(\theta^j) + B\tilde{\xi}(t),\\ \dot{\theta}^j &=& 0.5(\boldsymbol{\mu}\_1^j - \boldsymbol{\mu}\_2^j) + B\tilde{\xi}(t),\end{array} \tag{92}$$

where *j* = 1, 2, *ξ*(*t*) generates new random value in interval from −1 to 1 at every call.

Results of simulations with the found optimal controls and different levels of perturbations of the model are presented in the Table 1. The Table 1 includes average values of functional (70) on ten tests. As we can see, the synthesized optimal control is less sensitive to the perturbation of model. For the synthesized control with the level of perturbation *B* = 1.5, the average value of the functional is changed by no more than 30% and, for the direct control with the same level of perturbations, the functional is changed by more than 200%.


**Table 1.** The average values of functional (70).

In Figure 3, the trajectories for synthesized optimal control with model perturbations of level *B* = 1.5 are presented. In Figure 4, the trajectories for the direct control with the same level of perturbation *B* = 1.5 are presented.

As can be seen from Figures 3 and 4, the synthesized control does not change the nature of the motion of objects under large disturbances, and direct control first of all violates the accuracy of achieving the terminal conditions.

**Figure 3.** Optimal trajectories of robots on the plane {*x*, *y*} for synthesized control with *B* = 1.5.

**Figure 4.** Optimal trajectories of robots on the plane {*x*, *y*} for direct control with *B* = 1.5.
