**5. An Example**

Consider a control problem for two similar mobile robots. Mathematical model of control objects has the following form

$$\begin{aligned} \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{aligned} \tag{33}$$

where *j* = 1, 2.

The control is limited

$$\begin{array}{rcl} \mathbf{u}\_{i}^{-} & \lessapprox \mathbf{u}\_{i}^{j} & \lessapprox \mathbf{u}\_{i}^{+} \\ \end{array} \tag{34}$$

where *j* = 1, 2, *u*− *<sup>i</sup>* , *<sup>u</sup>*<sup>+</sup> *<sup>i</sup>* are the given constraints, *i* = 1, 2. For the system (33) , the initial conditions are

$$\mathbf{x}^{j}(0) = \mathbf{x}\_{0^{\prime}}^{j} \ y^{j}(0) = y\_{0^{\prime}}^{j} \ \theta^{j}(0) = \theta\_{0}^{j}. \tag{35}$$

The terminal conditions are

$$\mathbf{x}^{j}(t) = \mathbf{x}\_{f'}^{j}, \ y^{j}(t) = \mathbf{y}\_{f'}^{j}, \boldsymbol{\theta}^{j}(t) = \boldsymbol{\theta}\_{f}^{j}. \tag{36}$$

The static phase constraints are

$$\varrho\_i(\mathbf{x}^j, \mathbf{y}^j) = r\_i^2 - (\mathbf{x}\_i^\* - \mathbf{x}^j)^2 - (y\_i^\* - \mathbf{y}^j) \lessapprox 0,\tag{37}$$

where *j* = 1, 2, *ri*, *x*<sup>∗</sup> *<sup>i</sup>* , *y*<sup>∗</sup> *<sup>i</sup>* are the given parameters of constraints, *i* = 1, ... ,*r*, *r* is a number of static phase constraints. For two robots, we have only one dynamic phase constraint

$$
\chi(\mathbf{x}^1, \mathbf{x}^2) = d^2 - (\mathbf{x}^1 - \mathbf{x}^2)^2 - (y^1 - y^2)^2 \lessapprox 0,\tag{38}
$$

where *d* is a given minimal distance between robots.

A quality functional is

$$J = \mathbf{t}\_f + \sum\_{j=1}^{2} \delta\_j(\mathbf{t}\_f),\tag{39}$$

where

$$t\_f = \begin{cases} \ t\_\prime \text{ if } t < t^+ \text{ and } \sum\_{j=1}^2 \delta\_j(t) < \varepsilon\\ t^+ \text{ otherwise} \end{cases},\tag{40}$$

$$\boldsymbol{\delta}\_{\boldsymbol{\hat{y}}}(t) = \sqrt{(\boldsymbol{\Delta}\_{\mathbf{x}}^{\boldsymbol{\hat{j}}})^2(t) + (\boldsymbol{\Delta}\_{\boldsymbol{\mathcal{Y}}}^{\boldsymbol{\hat{j}}})^2(t) + (\boldsymbol{\Delta}\_{\boldsymbol{\theta}}^{\boldsymbol{\hat{j}}})^2(t)},\tag{41}$$

$$\boldsymbol{\Delta}\_{\mathbf{x}}^{\boldsymbol{\hat{j}}}(t) = \mathbf{x}^{\boldsymbol{\hat{j}}}(t) - \mathbf{x}\_{f'}^{\boldsymbol{\hat{j}}}$$

$$\boldsymbol{\Delta}\_{\boldsymbol{\mathcal{Y}}}^{\boldsymbol{\hat{j}}}(t) = \mathbf{y}^{\boldsymbol{\hat{j}}}(t) - \mathbf{y}\_{f'}^{\boldsymbol{\hat{j}}}$$

$$\boldsymbol{\Delta}\_{\boldsymbol{\theta}}^{\boldsymbol{\hat{j}}}(t) = \boldsymbol{\theta}^{\boldsymbol{\hat{j}}}(t) - \boldsymbol{\theta}\_{f'}^{\boldsymbol{\hat{j}}}$$

*j* = 1, 2, *ε* is a small positive value.

To obtain equations for conjugate variables, all constraints are included in the quality criterion, and terminal conditions are excluded as follows

$$J = t\_f + a \int\_0^{t\_f} \sum\_{j=1}^2 \mu^2(\wp\_i(\mathbf{x}^j, y^j))dt + b \int\_0^{t\_f} \mu^2(\chi(\mathbf{x}^1, \mathbf{x}^2))dt. \tag{42}$$

Suggesting that the problem is not abnormal, let us write down the Hamilton function in the following form

$$H(\mathbf{x}, \mathbf{u}, \boldsymbol{\upmu}) = -1 - \sum\_{j=1}^{2} \mu^2(q\_i(\mathbf{x}^j, \mathbf{y}^j)) - a\mu^2(\chi(\mathbf{x}^1, \mathbf{x}^2)) + 0.5\sum\_{j=1}^{2} \Psi\_{1+3(j-1)}(\boldsymbol{u}\_1^j + \boldsymbol{u}\_2^j)\cos\theta^j + \cdots$$

$$0.5\sum\_{j=1}^{2} \Psi\_{2+3(j-1)}(\boldsymbol{u}\_1^j + \boldsymbol{u}\_2^j)\sin\theta^j + 0.5\sum\_{j=1}^{2} \Psi\_{3j}(\boldsymbol{u}\_1^j - \boldsymbol{u}\_2^j). \tag{43}$$

As a result, the differential equations for conjugate variables are

*ψ*˙ <sup>1</sup>+3(*j*−1) <sup>=</sup> <sup>−</sup>*∂H*(**x**, **<sup>u</sup>**,*ψ*) *<sup>∂</sup>x<sup>j</sup>* <sup>=</sup> <sup>4</sup>*μ*(*<sup>r</sup>* 2 <sup>1</sup> − (*x*<sup>∗</sup> <sup>1</sup> <sup>−</sup> *<sup>x</sup><sup>j</sup>* )<sup>2</sup> <sup>−</sup> (*y*<sup>∗</sup> <sup>1</sup> <sup>−</sup> *<sup>y</sup><sup>j</sup>* ))(*x*∗ <sup>1</sup> <sup>−</sup> *<sup>x</sup><sup>j</sup>* )+ 4*μ*(*r* 2 <sup>1</sup> − (*x*<sup>∗</sup> <sup>2</sup> <sup>−</sup> *<sup>x</sup><sup>j</sup>* )<sup>2</sup> <sup>−</sup> (*y*<sup>∗</sup> <sup>2</sup> <sup>−</sup> *<sup>y</sup><sup>j</sup>* ))(*x*∗ <sup>2</sup> <sup>−</sup> *<sup>x</sup><sup>j</sup>* ) <sup>−</sup> <sup>4</sup>*μ*(*d*<sup>2</sup> <sup>−</sup> (*x*<sup>1</sup> <sup>−</sup> *<sup>x</sup>*2)<sup>2</sup> <sup>−</sup> (*y*<sup>1</sup> <sup>−</sup> *<sup>y</sup>*2))(*x*<sup>1</sup> <sup>−</sup> *<sup>x</sup>*2), *ψ*˙ <sup>2</sup>+3(*j*−1) <sup>=</sup> <sup>−</sup>*∂H*(**x**, **<sup>u</sup>**,*ψ*) *<sup>∂</sup>y<sup>j</sup>* <sup>=</sup> <sup>4</sup>*μ*(*<sup>r</sup>* 2 <sup>1</sup> − (*x*<sup>∗</sup> <sup>1</sup> <sup>−</sup> *<sup>x</sup><sup>j</sup>* )<sup>2</sup> <sup>−</sup> (*y*<sup>∗</sup> <sup>1</sup> <sup>−</sup> *<sup>y</sup><sup>j</sup>* ))(*y*∗ <sup>1</sup> <sup>−</sup> *<sup>y</sup><sup>j</sup>* )+ (44) 4*μ*(*r* 2 <sup>1</sup> − (*x*<sup>∗</sup> <sup>2</sup> <sup>−</sup> *<sup>x</sup><sup>j</sup>* )<sup>2</sup> <sup>−</sup> (*y*<sup>∗</sup> <sup>2</sup> <sup>−</sup> *<sup>y</sup><sup>j</sup>* ))(*y*∗ <sup>2</sup> <sup>−</sup> *<sup>y</sup><sup>j</sup>* ) + <sup>4</sup>*μ*(*d*<sup>2</sup> <sup>−</sup> (*x*<sup>1</sup> <sup>−</sup> *<sup>x</sup>*2)<sup>2</sup> <sup>−</sup> (*y*<sup>1</sup> <sup>−</sup> *<sup>y</sup>*2))(*y*<sup>1</sup> <sup>−</sup> *<sup>y</sup>*2), *ψ*˙ <sup>3</sup>*<sup>j</sup>* <sup>=</sup> <sup>−</sup>*∂H*(**x**, **<sup>u</sup>**,*ψ*) *∂θ<sup>j</sup>* <sup>=</sup> 0.5*ψ*1+3(*j*−<sup>1</sup>)(*u<sup>j</sup>* <sup>1</sup> <sup>+</sup> *<sup>u</sup><sup>j</sup>* <sup>2</sup>) sin *<sup>θ</sup><sup>j</sup>* <sup>−</sup> 0.5*ψ*2+3(*j*−<sup>1</sup>)(*u<sup>j</sup>* <sup>1</sup> <sup>+</sup> *<sup>u</sup><sup>j</sup>* <sup>2</sup>) cos *<sup>θ</sup><sup>j</sup>* ,

where *j* = 1, 2. Optimal control is calculated from equations

$$\boldsymbol{\mu}\_{1}^{j} = \begin{cases} \boldsymbol{u}\_{1}^{j+}, \text{ if } \boldsymbol{\mathcal{W}}\_{j} + \boldsymbol{\psi}\_{3j} > 0 \\ \boldsymbol{u}\_{1}^{j-}, \text{ if } \boldsymbol{\mathcal{W}}\_{j} + \boldsymbol{\psi}\_{3j} < 0 \\ \text{ special control mode, if } \boldsymbol{\mathcal{W}}\_{j} + \boldsymbol{\psi}\_{3j} = 0 \end{cases} \tag{45}$$

$$\boldsymbol{\mu}\_{2}^{j} = \begin{cases} \boldsymbol{\mu}\_{2}^{j+}, \text{ if } \mathcal{W}\_{j} - \boldsymbol{\psi}\_{3j} > 0 \\ \boldsymbol{\mu}\_{2}^{j-}, \text{ if } \mathcal{W}\_{j} - \boldsymbol{\psi}\_{3j} < 0 \\ \text{ special control mode, if } \mathcal{W}\_{j} - \boldsymbol{\psi}\_{3j} = 0 \end{cases} \tag{46}$$

where

$$\mathcal{W}\_{\dot{\jmath}} = \psi\_{1+3(j-1)} \sin \theta^{\dot{\jmath}} + \psi\_{2+3(j-1)} \cos \theta^{\dot{\jmath}},\tag{47}$$

*<sup>u</sup>j*<sup>+</sup> <sup>1</sup> , *<sup>u</sup>j*<sup>+</sup> <sup>2</sup> , *<sup>u</sup>j*<sup>−</sup> <sup>1</sup> , *<sup>u</sup>j*<sup>−</sup> <sup>2</sup> are upper and lower values of control for robot *j*, *j* = 1, 2.

The nonlinear programming problem consists of finding initial conditions for conjugate variables

$$
\eta\_i = \psi\_i(0), \ i = 1, \ldots, 6,\tag{48}
$$

so that initial conditions have to allocate on a sphere with a unit radius

$$\sqrt{\sum\_{i=1}^{6} q\_i^2} = 1,\tag{49}$$

as well as terminal time

$$\mathbf{t}\_f = \mathbf{t}^+ + q\_{7'} \tag{50}$$

and special control modes

$$\|u\_1^1 = q\_{8\prime}\text{ if }|\mathcal{W}\_1 + \Psi\_3| < \varepsilon\_{0\prime} \tag{51}$$

$$\mu\_2^1 = q\_{9\prime} \text{ if } |\mathcal{W}\_1 - \psi\_3| < \varepsilon\_{0\prime} \tag{52}$$

$$\|u\_1^2 = q\_{10\prime}\text{ if }|\mathcal{W}\_2 + \psi\_6| < \varepsilon\_{0\prime} \tag{53}$$

$$
\mu\_2^2 = q\_{11\prime} \text{ if } |\mathcal{W}\_1 - \mathfrak{y}\mathfrak{k}| < \varepsilon \mathfrak{o}\_{\prime} \tag{54}
$$

where *ε*<sup>0</sup> is a small positive value.

A goal function for the nonlinear programming has the following form

$$F = \sum\_{j=1}^{2} \delta\_j(t\_f) + a \int\_0^{t\_f} \sum\_{j=1}^{2} \mu^2(\varrho\_i(\mathbf{x}^j, y^j))dt + b \int\_0^{t\_f} \mu^2(\chi(\mathbf{x}^1, \mathbf{x}^2))dt. \tag{55}$$

#### **6. Computational Experiment**

The problem had the following parameters: a number of objects *M* = 2, dimensions of objects *n*<sup>1</sup> = 3, *n*<sup>2</sup> = 3, dimension of state space vector *n* = 3 + 3 = 6, dimensions of control *m*<sup>1</sup> = 2, *m*<sup>2</sup> = 2, dimension of control space *m* = 2 + 2 = 4 dimension of parameter vector *n* + 1 + *m* = 11.

The following values of parameters of the algorithm were used: *x*<sup>1</sup> <sup>0</sup> = 0, *<sup>y</sup>*<sup>1</sup> <sup>0</sup> = 0, *<sup>θ</sup>*<sup>1</sup> <sup>0</sup> = 0, *<sup>x</sup>*<sup>2</sup> <sup>0</sup> = 10, *y*2 <sup>0</sup> = 10, *<sup>θ</sup>*<sup>2</sup> <sup>0</sup> = 0, *<sup>x</sup>*<sup>1</sup> *<sup>f</sup>* = 10, *<sup>y</sup>*<sup>1</sup> *<sup>f</sup>* = 10, *<sup>θ</sup>*<sup>1</sup> *<sup>f</sup>* = 0, *<sup>x</sup>*<sup>2</sup> *<sup>f</sup>* = 0, *<sup>y</sup>*<sup>2</sup> *<sup>f</sup>* = 0, *<sup>θ</sup>*<sup>2</sup> *<sup>f</sup>* <sup>=</sup> 0, *<sup>u</sup>*<sup>+</sup> *<sup>i</sup>* = 10, *u*<sup>−</sup> *<sup>i</sup>* = −10, *i* = 1, 2, *t* <sup>+</sup> = 3.5 s, *x*∗ <sup>1</sup> = 5, *y*<sup>∗</sup> <sup>1</sup> = 8, *r*<sup>1</sup> = 2, *x*<sup>∗</sup> <sup>2</sup> = 5, *y*<sup>∗</sup> <sup>2</sup> = 2, *r*<sup>2</sup> = 2, *d* = 2, *ε*<sup>0</sup> = 0.1, *a* = 2, *b* = 2, *ε* = 0.01, *H* = 32, *P* = 256, *Plength* = 8, *Prt* = 0.33, Δ = 0.22, *β* = 0.2, *α* = 0.3, *K* = 7, constraints on parameter values are *q*<sup>−</sup> *<sup>i</sup>* = −1, *q*+ *<sup>i</sup>* = 1, *i* = 1, . . . , 6, *q*<sup>−</sup> <sup>7</sup> <sup>=</sup> <sup>−</sup>2, *<sup>q</sup>*<sup>+</sup> <sup>7</sup> = 1, *q*<sup>−</sup> *<sup>i</sup>* <sup>=</sup> <sup>−</sup>10, *<sup>q</sup>*<sup>+</sup> *<sup>i</sup>* = 10, *i* = 8, . . . , 11.

An obtained optimal vector of parameters was

$$\mathbf{q} = \begin{bmatrix} 0.7722 & 0.2142 \ -0.2825 & -0.2968 & -0.23 & 0.3702 \ -1.5112 & 0.0179 & 3.3965 \ -2.96 & -0.5945 \end{bmatrix}^T$$

This solution provided the value of the goal function (55) *F* = 0.1238.

To obtain the solution of optimal control problem for (33) and (44) by using the Pontryagin maximum principle, the complexity of search was the following: *H* = 32, *P* = 256, *R* = *H*, *Plength* = 8, *step* = 0.22, *n* = 8/0.22 = 36, *H* + *nRP* = 32+36×32×256 = 294, 944, i.e., the functional was calculated 294,944 times. Simulation was performed in Lazarus, an open-source Free Pascal-based software, on PC with Intel Core i7, 2.8 GHz, OS Win 7. Series of 10 runs was implemented. The CPU time for 10 runs was approx. 20 min, i.e., 1 run was approx. 2 min.

The trajectories of the robots are shown in Figure 1. On Figure 1, red circles present the static constraints. Plots of obtained control are presented on Figures 2–5. Figures 2–5 show that optimal control includes sectors of special control modes [8]. The controls *u*<sup>1</sup> <sup>2</sup> and *<sup>u</sup>*<sup>2</sup> <sup>1</sup> have sliding modes.

**Figure 3.** Control *u*<sup>1</sup> 2.

**Figure 5.** Control *u*<sup>2</sup> 2.
