1 Introduction

In recent years, an interest has increased in applying the non-holonomic systems (mobile robots, tractor-trailer vehicles, orbiting satellites, space robot manipulators, etc.) to useful practical positioning tasks such as part transfer or an assembly. In order to accomplish the aforementioned tasks, the non-holonomic motion planning should be involved. It consists in obtaining (generating) a trajectory which joints a given initial posture (state) of the system with some desired final position in a workspace. For many of the repetitious processes, it is economically desirable to obtain the system motions that improve the performance of these processes with respect to some criteria. Reducing the energy required to transfer a part by the mobile robot from one location in the workspace to another constitutes a good example. In such a case, even a small energy decrease in one cycle of the process, multiplied by the total number of cycles in the lifetime of the manufacturing process, may result in essential savings in manufacturing costs. During the system motion in the workspace, some constraints are usually activated. They result from the existence of the control limits reflecting the physical abilities of the actuators and the fact that a system should not collide with obstacles. As is known, [5, 57], dynamic systems with non-holonomic constraints possess some peculiar characteristics which make motion planning for them challenging. Namely, non-holonomic velocity constraints are not integrable to equivalent constraints in configuration (posture) space. Consequently, the instantaneous velocity of a system subject to non-holonomic constraints is limited to certain directions. Constraints of this kind occur for any wheeled vehicle under the no-slip condition. Manoeuvres of a system with limited instantaneous velocity directions in a work space containing, e.g. obstacles, are considerably more complex than those accomplished by holonomic system. Moreover, there exists no continuous time-invariant stabilizing feedback of a point-to-point control problem (e.g. a parking task) for drift-less non-holonomic systems [5, 57]. Furthermore, the majority of the non-holonomic systems is globally controllable. However, their linearization about an equilibrium point is not controllable. Due to the aforementioned difficulties, motion planning techniques for holonomic systems are not directly applicable to non-holonomic motion planning. On account of the peculiar characteristics which possess systems subject to non-holonomic equality constraints, the solution of the motion planning problem with control and/or state-dependent inequality constraints becomes difficult from both theoretical and computational point of view. In such context, three representative approaches to the non-holonomic motion planning problem may be distinguished. The graph search-based methods including those offered in [13, 28, 29, 33, 42] generally involve some types of decomposition of the state space into cells (e.g. probabilistic road maps (PRM), rapidly exploring random trees (RRT) or uniform discrete grid representation). Then, a graph is constructed whose nodes are configurations (postures) and whose arcs are some types of paths (e.g. shortest, collision-free) connecting two configurations. The graph is searched applying the suitable algorithms. According to the discrete grid-based artificial potential field method, a global collision-free path planning of a cooperative cable parallel robot for multiple mobile cranes was presented in [58]. However, the graph search methods based on state space decomposition can be computationally both time-consuming (due to preprocessing of the continuous work space into a suitable graph) and complicated particularly for complex non-holonomic systems such as a tractor pulling the two or more trailers [11]. The second approach applying the Jacobian motion planning algorithms is based on so-called continuation method inspired by Wazewski [54]. It was introduced to robotics by Sussmann [48]. Jacobian algorithms have been developed theoretically in [8, 9] and applied to non-holonomic systems in [1, 11, 12, 26, 37, 49,50,51]. The augmented motion planning techniques such as the extended Jacobian [2, 4] or the configuration control proposed in [45] were adopted to non-holonomic systems in work [50]. Although motion planning algorithms based on the pseudo-inverse of the Jacobian are attractive and further investigated by many researchers, they possess some shortcomings. Namely, generated trajectories provide at most sub-optimal solutions. Moreover, pseudo-inverse motion planning strategies are not, in general, repeatable [43]. (only the extended Jacobian techniques [2, 4] may generate repeatable state trajectories provided that the corresponding Jacobian matrix is non-singular.) Consequently, an important class of cyclic technological operations cannot be accomplished by the pseudo-inverse approach. Furthermore, almost all those motion planning algorithms require explicitly pseudo-inverse of the Jacobian which can potentially contain kinematic singularities. In order to tackle the singular configurations, the use of damped least squares has been proposed in works [38] in lieu of the pseudo-inverses. Nevertheless, this technique suffers from the tracking errors due to a long-term numerical integration drift. The non-holonomic planners applying the inverse of the extended Jacobian, can potentially introduce algorithmic singularities [2] which cause the motion planning to stop even though the non-holonomic system is not in a kinematic singular configuration. An iterative learning discontinuous controller using the inverse kinematics has been proposed in [40] to accomplish repetitive tasks. The third approach of motion planning offered in works [7, 10, 15, 16, 24, 30, 32, 34, 36, 41, 55] is based on application of optimal control methods or the calculus of variations. Nevertheless, application of Pontryagin’s Maximum Principle in its classical form results in a two-point boundary value problem which is hard and sensitive to solve numerically. Moreover, maximizing the Hamiltonian (instead of directly minimizing the performance index) may result in stationary solutions which are not necessarily optimal ones. Using the calculus of variations and different kinds of parameterizations of controls, works [7, 15, 16, 24, 30, 56] convert the infinite-dimensional optimal control formulation into a finite-dimensional nonlinear programming problem. The result of projection is obtaining only near-optimal solutions. The approach in [10] formulates the planning problem as a problem of the calculus of variations and handles state inequality constraints as equality ones using slack variables which significantly increases the number of unknown functions to be found. Optimal trajectory generation using spline interpolation techniques was offered in works [21, 53] only for a simple class of mobile robots. This work also addresses the problem of optimal motion planning for the non-holonomic systems subject to both control and state-dependent constraints. In the case considered, it is very difficult to use Pontryagin’s Maximum Principle in its classical form [39], since it presents only a positive form of control. Therefore, it is natural to make an attempt to solve the aforementioned constrained optimal control problem by resorting to other techniques. A new method based on a non-classical formulation of the Pontryagin Maximum Principle [14, 18, 23] given in variational form, which makes it possible to handle state inequality constraints efficiently, is proposed here to determine optimal motions of the non-holonomic systems. This approach, which consists in transforming the state constraints into control-dependent ones, in contrast to the interior penalty function method, does not require an initial admissible system trajectory (whose determination may be very troublesome in practice). In contrast to optimization algorithms known from the literature [19, 20], which project infinite-dimensional control space into a finite-dimensional one and then apply techniques of liner programming problems, thus resulting only in near-optimal trajectories generated by discontinuous controls, the method proposed herein provides continuous solutions in an infinite-dimensional control space. As opposed to the most of the existing control algorithms, which provide only sub-optimal or near-optimal solutions and are based on maximizing the Hamiltonian [44, 47], the control schemes offered in our study directly minimize the performance index. As is known, the Hamiltonian-based approaches provide, in fact, only stationary solutions which may not necessarily be optimal ones. Moreover, in the case of classic (constraint-free) non-holonomic motion planning problems analysed and solved in the majority of works by means of the pseudo-inverses of Jacobian matrices [1, 11, 12, 26, 37, 49, 50], our control scheme is significantly simplified in such a way that it does not require computation of inverse or pseudo-inverse of the Jacobian what also results in numerical stability of the approach. The technique of convergence proof for the proposed algorithms is different from those known in the literature and is based on searching for limit trajectory whose existence is theoretically ensured. (Optimal control problems with state inequality constraints may not have limit controls in a class of measurable Lebesgue mappings [17, 25]). The paper is organized as follows. Section 2 presents the task of non-holonomic systems in terms of the optimal control problem. Section 3 offers new trajectory generation schemes based on negative formulation of the Pontryagin Maximum Principle to determine the optimal motions of the non-holonomic system. A numerically efficient procedure is proposed in Sect. 4 to determine an initial admissible control which is close (in the \(L^2\) norm) to an optimal one. Section 5 provides computer examples of planning the optimal motions in a workspace without and with obstacles for both a simple benchmark non-holonomic structure and the car pulling the two trailers.

2 Problem formulation

In order to express the task of a non-holonomic system in terms of an optimal control problem, the state vector \(\chi =\chi (t)=(\chi _1(t),\ldots ,\chi _n(t))^T \in \mathbb {R}^n\) is introduced, where \(t \in [0,\ T]\); T a fixed or final moment of performing the task; n stands for the amount of state variables. Then, the dynamic equations of the non-holonomic system can be written in a general state space form as [3]

$$\begin{aligned} \dot{\chi }(t)=f(\chi (t),v(t),t), \end{aligned}$$
(1)

where \(v(t) \in \mathbb {R}^m\) denotes the vector of controls; m stands for the number of controls; \(f: \mathbb {R}^n \times \mathbb {R}^m \times \mathbb {R}\longrightarrow \mathbb {R}^n\); \(f(\cdot , \cdot ,\cdot )\) is a smooth mapping with respect to \(\chi \), v and t, respectively. Due to non-holonomic system (1) considered herein, the following inequality holds true: \(m<n\). In further analysis, mapping \(\chi (t)\) is assumed to be absolutely continuous with respect to time. Moreover, control \(v(t)=(v_1(t),\ldots ,v_m(t))^T\) is an integrable Lebesgue mapping, on which some constraints resulting from the abilities of the physical actuators of system (1), are imposed

$$\begin{aligned} v_l \le v(t) \le v_u, \end{aligned}$$
(2)

where \(v_l=(v_{l,1},\ldots ,v_{l,m})^T\) and \(v_u=(v_{u,1},\ldots ,v_{u,m})^T\) are lower and upper limits on control v(t), respectively. If the kinematic non-holonomic constraints are only taken into account when accomplishing the task, then state vector \(\chi \) in (1) equals \(\chi =q(t)\), where q(t) is the posture of the non-holonomic system and control vector v(t) corresponds to auxiliary velocities \(\eta (t)\) [6], i.e. \(v(t)=\eta (t)\). In addition, if both kinematic non-holonomic constraints and system dynamic equations (second-order non-holonomic constraints) are analysed by the accomplishment of the task, then state vector \(\chi \) is equal to \(\chi =(q^T(t)\ \eta ^T(t))^T\) and v(t) in (1) presents torques acting in the wheels of the system. The task is to move system (1) from a given initial state \(\chi (0)=\chi _0\) to a final desired system position \(y_d\) in the \(\kappa \)-dimensional task space, where \(\kappa \le n\). Formally, the last condition may be described as follows

$$\begin{aligned} g(\chi (T))-y_d=0, \end{aligned}$$
(3)

where \(g: \mathbb {R}^n \longrightarrow \mathbb {R}^{\kappa }\); \(g(\cdot )\) denotes the output equations of system (1) which are assumed to be at least twice continuously differentiable; \(\chi (T)\) is an unknown final state to be found. In the particular case, i.e. for \(g(\cdot )=\hbox {id}(\cdot )\), where \(\hbox {id}(\cdot )\) denotes the identity mapping, \(\chi (t)\) is assumed to be known. During the system motion, collision avoidance constraints are induced. They result from the fact that non-holonomic system should not collide with the static and/or moving obstacles. The general form of these (inequality) constraints, in the case of moving obstacles, may be written in the following way:

$$\begin{aligned} \{c^i(\chi (t),t) \le 0:\ i=0: \mathcal {O}\}, \end{aligned}$$
(4)

where \(c^i=\gamma ^i_c-r_i(\chi ,t)\); \(\gamma ^i_c\) is a small positive number (a safety margin around the i-th obstacle); \(r_i(\cdot ,\cdot )\) denotes either a distance function between the non-holonomic system and the i-th obstacle [22] or an analytic description of the obstacle [31] and \(\mathcal {O}\) stands for the total number of (moving) obstacles in the work space, in which the system operates. Functions \(r_i\) are assumed to be continuously differentiable. For \(\mathcal {O}=0\), there are no obstacles in the work space. The motion of the non-holonomic system should be accomplished in such a way as to minimize the following performance index:

$$\begin{aligned} J(v)=\int _0^T\phi (\chi ,v,t)\hbox {d}t, \end{aligned}$$
(5)

where \(\phi (\cdot ,\cdot ,\cdot )\) is a smooth non-negative function of vector variables \(\chi \), v and time t. The next section will present an approach that renders it possible to solve optimization problem (1)–(5) making use of the non-classical formulation of Pontryagin’s Maximum Principle.

3 Generation of optimal trajectory of the non-holonomic system

In what follows, system (1) is assumed to be completely controllable for the pair \((\chi (\cdot ),v(\cdot ))\) [23]. This assumption means that both matrix \(\frac{\partial g(\chi (T))}{\partial \chi }\) is non-singular and the rows of matrix \(\left( \frac{\partial f(\chi (t),v(t),t)}{\partial v}\right) ^T\varPsi \) are linearly independent as mappings of time t, where \(\varPsi =\varPsi (t)\) is a solution of the fundamental differential equations \(\dot{\varPsi }(t)=-\left( \frac{\partial f(\chi (t),v(t),t)}{\partial \chi }\right) ^T\varPsi \); \(\varPsi (0)=\mathbb {I}_n\); \(\mathbb {I}_n\) denotes the \(n \times n\) identity matrix. Moreover, from the Euler–Lagrange equations of non-holonomic system [3], we can obtain the following upper estimation on the norm of the mapping \(f(\cdot , \cdot , \cdot )\): \(||f(\chi ,v,t)|| \le c_1||\chi ||+c_2||\chi ||^2+c_3||v||+c_4\), where \(c_i\) denotes some positive coefficients; \(i=1{:}4\); \(c_4\) is an upper estimation of the norm of the disturbance signal acting on the non-holonomic system. On account of the obvious inequalities \(\frac{\hbox {d}}{\hbox {d}t} ||\chi || \le ||\dot{\chi } ||\) and \(||v|| \le v_u\), we see that \(\frac{d(||\chi ||)}{c_5+c_1||\chi || +c_2 ||\chi ||^2} \le \hbox {d}t\), where \(c_5=c_3||v_u||+c_4\). Let us assume that \(\varDelta =c_1^2-4c_2c_5 < 0\) (the case in which \(\varDelta \ge 0\) is considered further on). Integrating both sides of the last differential inequality, we obtain the following bound on \(||\chi ||\): \(||\chi || \le \mathcal {A}_1\), where

$$\begin{aligned} \mathcal {A}_1= & {} \max _{t \in [0,\ T]} \left\{ \frac{1}{2c_2}\left[ \sqrt{-\varDelta } \tan \right. \right. \\&\times \left. \left. \left( \frac{\sqrt{-\varDelta }}{2}t-\arctan \frac{c_1}{\sqrt{-\varDelta }}\right) -c_1 \right] \right\} . \end{aligned}$$

Similarly, in the case of \(\varDelta \ge 0\), we can obtain the bound on \(||\chi ||\) in the form \(||\chi || \le \mathcal {A}_2\), where

$$\begin{aligned} \mathcal {A}_2= & {} \max _{t \in [0,\ T]} \left\{ \frac{|\sqrt{\varDelta }-c_1|}{c_2}\exp \right. \\&\times \left. \left[ \frac{1}{2}(c_1+\sqrt{\varDelta }-|\sqrt{\varDelta }-c_1|)t \right] \!+\!\frac{c_1+\sqrt{\varDelta }}{2c_2} \right\} \!. \end{aligned}$$

Consequently,

$$\begin{aligned} ||\chi || \le \mathcal {A}, \end{aligned}$$
(6)

where \( \mathcal {A}=\max \{\mathcal {A}_1,\ \mathcal {A}_2\}\). In further analysis, penalty functions are introduced to incorporate inequality constraints (2), (4) into our approach. Let \(\eta ^\mathrm{ex}_v(v_i)\) be an exterior penalty function defined for the i-th control variable \(v_i\), \(i=1,2,\ldots ,m\), in the following way:

$$\begin{aligned} \eta ^\mathrm{ex}_v(v_i)=\left\{ \begin{array}{ll} w^{\mathrm{ex},i}_v (v_i-v_{u,i})^2&{} \quad \hbox {for}\;v_i \ge v_{u,i},\\ w^{\mathrm{ex},i}_v (v_{l,i}-v_i)^2&{} \quad \hbox {for}\;v_i \le v_{l,i},\\ 0 &{} \quad \text {otherwise}, \end{array} \right. \end{aligned}$$
(7)

where \( w^{\mathrm{ex},i}_v\) denotes a constant positive coefficient (the strength of exterior penalty). Let us observe that application of formula (7) does not guarantee maintenance of control constraints (2). In order to force fulfilment of strong inequalities (2), interior penalty functions should be introduced. They take the following form for the i-th control variable:

$$\begin{aligned} \eta ^\mathrm{in}_v(v_i)=\left\{ \begin{array}{ll} w^{\mathrm{in},i}_v \left( \frac{1}{v_i-v_{l,i}}-\frac{1}{\gamma _i} \right) ^2 &{} \text { for } v_i \le v_{l,i}+\gamma _i,\\ w^{\mathrm{in},i}_v \left( \frac{1}{v_{u,i}-v_i}-\frac{1}{\gamma _i} \right) ^2 &{} \text { for } v_i \ge v_{u,i}-\gamma _i,\\ 0 &{}\text { otherwise}, \end{array} \right. \end{aligned}$$
(8)

where \(\gamma _i\) is a small positive number (a safety margin for the i-th control variable); \(w^{\mathrm{in},i}_v\) denotes a constant positive coefficient (the strength of interior penalty). Similarly, let \(\eta ^{\mathrm{ex}}_{\mathcal {O}}(c^i)\) be an exterior penalty function corresponding to \(\eta ^{\mathrm{ex}}_v\) and defined for the i-th obstacle, \(i=0,1,\ldots ,\mathcal {O}\), as follows

$$\begin{aligned} \eta ^{\mathrm{ex}}_{\mathcal {O}} (c^i)=\left\{ \begin{array}{ll} w^{\mathrm{ex},i}_{\mathcal {O}} (c^i)^2 &{} \text { for } c^i(\chi (t),t) \ge 0,\\ 0 &{}\text {otherwise}, \end{array} \right. \end{aligned}$$
(9)

where \( w^{\mathrm{ex},i}_{\mathcal {O}} > 0\) is a constant coefficient (the strength of the exterior penalty). Besides, we additionally introduce for the numerical computation purposes an exterior penalty function \(\eta ^{\mathrm{ex}}_{\mathcal {O}, in} \) corresponding to \(\eta ^{\mathrm{in}}_v\) and defined as

$$\begin{aligned} \eta ^{\mathrm{ex}}_{\mathcal {O},in} (c^i)=\left\{ \begin{array}{ll} w^{\mathrm{ex},i}_{\mathcal {O},in} (c^i)^2 &{}\text { for } c^i(\chi (t),t) \ge 0 ,\\ 0 &{}\text { otherwise}, \end{array} \right. \end{aligned}$$
(10)

where \(w^{\mathrm{ex},i}_{\mathcal {O},in}\) is a positive constant coefficient. Let \(\eta _v(v_i)\) denote either the exterior penalty function for \(v_i\), i.e. \(\eta _v(v_i)=\eta ^{\mathrm{ex}}_v(v_i)\) or interior penalty function, i.e. \(\eta _v(v_i)=\eta ^{\mathrm{in}}_v(v_i)\), where \(i=1,2,\ldots ,m\). Similarly, let \(\eta _{\mathcal {O}}(c^i) \) be either equal to \(\eta ^{\mathrm{ex}}_{\mathcal {O}} (c^i)\) or equal to \(\eta ^{\mathrm{ex}}_{\mathcal {O},in} (c^i)\), i.e. \(\eta _{\mathcal {O}}(c^i)=\eta ^{\mathrm{ex}}_{\mathcal {O}} (c^i)\) or \(\eta _{\mathcal {O}}(c^i)=\eta ^{\mathrm{ex}}_{\mathcal {O},in} (c^i)\), respectively. In order to maintain control and state-dependent inequality constraints (2), (4) when operating non-holonomic system (1) in the work space, we modify performance index (5) as follows

$$\begin{aligned} F_0(v)=\int _0^T\varPhi _0(\chi ,v,t)\hbox {d}t, \end{aligned}$$
(11)

where \(\varPhi _0=\phi +\sum _{i=1}^m \eta _v(v_i(t))+\sum _{i=0}^{\mathcal {O}}\eta _{\mathcal {O}}(c^i)\). Let us note that penalty terms (7)–(10) can cause a slight increase of the value of performance index (5). Nevertheless, application of interior penalty function (8) leaves a free control space for compensation of modelling uncertainties in the feedback control actions of the system. Moreover, let us note that equality constraints (3) may be equivalently expressed in an integrable form as

$$\begin{aligned} \int _0^T \varPhi _1(\chi ,v,t)\hbox {d}t=0, \end{aligned}$$
(12)

with \(\varPhi _1=\frac{\partial g}{\partial \chi } f(\chi ,v,t) + \frac{g(\chi _0)-y_d}{T}\). It should be noted that through Eq. (1), the state vector \(\chi \) is functionally dependent on control v. In this way, the left-hand side of vector equality (12) also depends on v. For further considerations, it will be reformulated in functional form which is equivalent to the previous one. Thus, constrain (12) assumes the following (functional) form:

$$\begin{aligned} F_1(v)=0, \end{aligned}$$
(13)

where \(F_1(v)=\int _0^T \varPhi _1(\chi ,v,t)\hbox {d}t\) with performance index (11). To use the negative formulation of Pontryagin’s Maximum Principle [14, 18, 23], we assume that a certain admissible control \(v^0=v^0(t)\), that is, satisfying relations (2) but not necessarily (3) and (4), is known. Such a control always exists, e.g. \(v^0(t)\equiv 0\). Moreover, we presume that this control does not minimize performance index (5). Next the increments of functionals given by the left-hand side of relation (13) and the right-hand side of (11) must be determined. Therefore, it is assumed that the control \(v^0=v^0(t)\), where \(t \in [0,\ T]\), is perturbed by a small function (variation) \(\delta v(t)=(\delta v_1(t),\ldots ,\delta v_m(t))^T\), where \(||\delta v||=\max _{t \in [0,\ T]} \{ \max _{1 \le i \le m} \{|\delta v_i(t)|\}\} \le O(\rho _0)\); \(O(\rho _0) \rightarrow 0\) as \(\rho _0 \rightarrow 0\); \(\rho _0\) is a given small number that guarantees the correctness of the presented method. According to the theory of small perturbations [39], it results in a small perturbation of state trajectory; that is, the trajectory \(\chi ^0(t)\) (which corresponds to the admissible control \(v^0(t)\) with initial condition \(\chi ^0(0)=\chi _0\)) is replaced by \(\chi ^0(t)+\delta \chi (t)\), where

$$\begin{aligned} \delta \dot{\chi }=f_{\chi }(t)\delta \chi +f_{v}(t)\delta v, \end{aligned}$$
(14)

with \(\delta \chi (0)=0\); \(f_{\chi }(t)=\left( \frac{\partial f}{\partial \chi } \right) |_{(\chi ^0(t),\ v^0(t))}\); \(f_{v}(t)=\left( \frac{\partial f}{\partial v} \right) |_{(\chi ^0(t),\ v^0(t))}\). More precisely, Eq. (14) should be written in the form \(\delta \dot{\chi }=f_{\chi }(t)\delta \chi +f_{v}(t)\delta v+o(\delta v)\), where \(\frac{||o(\delta v)||}{||\delta v||} \rightarrow 0\) as \(||\delta v|| \rightarrow 0\) and \(\delta \chi (t)\) should be treated as the exact difference between the perturbed and unperturbed trajectory. For simplicity, in further analysis, we neglect the term \(o(\delta v)\), bearing in mind that in this case, variation \(\delta \chi \) differs from the exact difference by \(o(\delta v)\). The value of the functional \(F_0(\cdot )\) for control \(v^0+\delta v\) can thus be expressed as (its derivation is presented in “Appendix A” section)

$$\begin{aligned} F_0(v^0+\delta v)=F_0(v^0)+\delta F_0(v^0,\delta v)+o(\delta v), \end{aligned}$$
(15)

where \(\delta F_0(v^0,\delta v)=\int _0^T \left\langle \frac{\partial F_0}{\partial v},\ \delta v \right\rangle \hbox {d}t\) is the Frechet differential of functional \(F_0(\cdot )\); \(\langle \cdot ,\ \cdot \rangle \) means the scalar product of vectors in the Euclidean space; \(\frac{\partial F_0}{\partial v}\) denotes the Frechet derivative of \(F_0(\cdot )\) equal to

$$\begin{aligned} \frac{\partial F_0}{\partial v}=\left( (f_{v}(t))^T\psi _0+\left( \frac{\partial \varPhi _0}{\partial v}\right) ^T\right) \Big |_{(\chi ^0(t),\ v^0(t))}; \end{aligned}$$

\(\psi _0(\cdot )\) stands for the conjugate mapping computed by solution of the following differential equation:

$$\begin{aligned} \dot{\psi }_0+(f_{\chi }(t))^T \psi _0=-\left( \frac{\partial \varPhi _0}{\partial \chi }\right) ^T\Big |_{(\chi ^0(t),\ v^0(t))} \end{aligned}$$

with boundary condition \(\psi _0(T)=0\). Similarly, the value of the vector functional determined by the left-hand side of Eq. (13) for control \(v^0+\delta v\) becomes (its derivation is also presented in “Appendix A” section)

$$\begin{aligned} F_1(v^0+\delta v)=F_1(v^0)+\delta F_1(v^0,\delta v)+o(\delta v), \end{aligned}$$
(16)

where \(\delta F_1(v^0,\delta v)=\int _0^T \frac{\partial F_1}{\partial v} \delta v \hbox {d}t\) is the Frechet differential of \(F_1(\cdot )\); \(\frac{\partial F_1}{\partial v}\) denotes the Frechet matrix functional derivative of \(F_1(\cdot )\) equal to

$$\begin{aligned} \frac{\partial F_1}{\partial v}=\left( (\psi _1)^Tf_{v}(t)+\frac{\partial \varPhi _1}{\partial v}\right) \Big |_{(\chi ^0(t),\ v^0(t))}; \end{aligned}$$

\(\psi _1(\cdot )\) stands for the conjugate mapping obtained by solving the following differential equation:

$$\begin{aligned} \dot{\psi }_1+(f_{\chi }(t))^T \psi _1=-\left( \frac{\partial \varPhi _1}{\partial \chi }\right) ^T\Big |_{(\chi ^0(t),\ v^0(t))} \end{aligned}$$

with boundary condition \(\psi _1(T)=0\). For properly selected variation \(\delta v\), Frechet’s differentials of functionals (11), (13) can approximate the exact increments of these functionals with any accuracy. Hence, the negative formulation of Pontryagin’s Maximum Principle (when neglecting the higher-order terms \(o(\delta v)\)) has the form

$$\begin{aligned} F_0(v^0+\delta v)=F_0(v^0)+\delta F_0(v^0,\delta v)\longrightarrow \min _{\delta v} \end{aligned}$$
(17)

subject to the vector equality constraint \(F_1(v^0)+\delta F_1(v^0,\delta v)=0\). On account of the fact that \(v^0\) does not necessarily satisfy (13), the norm \(||F_1(v^0)|| \ge 0\) can be large. Hence, in order to ensure required accuracy of the linear approximation, the aforementioned equality constraint is modified as follows (\(\delta F_1(v^0,\delta v)\) is a linear mapping with respect to \(\delta v\))

$$\begin{aligned} g^0\cdot F_1(v^0)+\delta F_1(v^0,\delta v^0)=0, \end{aligned}$$
(18)

where \(0< g^0<<1\); \(\delta v^0\) is now “a small” (algebraically) variation equal to \(\delta v^0=g^0 \delta v\). Minimization of (17) results in variation \(\delta v_0=-g_0 \frac{\partial F_0}{\partial v}\), where \(g_0\) denotes a positive small number, which does not necessarily satisfy equality constrain (18). Therefore, we have to solve the following auxiliary control problem (projection of variation \(\delta v_0\) onto linear functional constrains (18)):

$$\begin{aligned} \min _{\delta v^0} \left\{ \frac{1}{2} \int _0^T ||\delta v^0-\delta v_0||^2\hbox {d}t \right\} , \end{aligned}$$
(19)

subject to constraint (18)

$$\begin{aligned} \int _0^T \frac{\partial F_1}{\partial v} \delta v^0 \hbox {d}t=- g^0 F_1(v^0). \end{aligned}$$
(20)

Relations (19), (20) constitute the problem of the relative minima from the calculus of variations whose solution may be expressed as follows

$$\begin{aligned} \delta v^0= & {} \delta v_0 - \left( \frac{\partial F_1}{\partial v}\right) ^T\left[ \int _0^T\frac{\partial F_1}{\partial v}\left( \frac{\partial F_1}{\partial v}\right) ^T \hbox {d}t \right] ^{-1} \nonumber \\&\times \left( \int _0^T\frac{\partial F_1}{\partial v}\delta v_0 \hbox {d}t +g^0F_1(v^0)\right) . \end{aligned}$$
(21)

Taking into account (21), we obtain the new control \(v^1(t)=v^0(t)+\delta v^0(t)\), where \(t \in [0,\ T]\). The process of minimization is then repeated for \(v^1\). The sequence of controls \(v^{k+1}=v^{k}+\delta v^{k}\), where \(k=0,1,2,\ldots \);

$$\begin{aligned} \delta v^k= & {} \delta v_k -\left( \frac{\partial F_1}{\partial v}\right) ^T\left[ \int _0^T\frac{\partial F_1}{\partial v}\left( \frac{\partial F_1}{\partial v}\right) ^T \hbox {d}t \right] ^{-1}\nonumber \\&\times \left( \int _0^T\frac{\partial F_1}{\partial v}\delta v_k \hbox {d}t +g^k F_1(v^k)\right) ; \end{aligned}$$
(22)

\(\delta v_k=-g_k \frac{\partial F_0}{\partial v}\); \(g_k\), \(g^k\) are positive coefficients which fulfil the following inequalities: \(0< g_k,\ g^k<< 1\), and the corresponding (according to Eq. (1)) state trajectories \(\chi ^{k+1}\) are obtained as a result of solving iterative scheme (17)–(22). On account of the obvious relations \(\delta F_1(v^k,\delta v^k)=-g^k F_1(v^k)\), we obtain inequalities \(||F_1(v^k)|| > ||F_1(v^{k+1})|| \ge 0\) and consequently \(F_1(v^k) \rightarrow 0\) as \(k \rightarrow \infty \). If this is the case, from (22) we have

$$\begin{aligned}&\delta v^k \rightarrow \delta v^k_{*}= \delta v_k -\left( \frac{\partial F_1}{\partial v}\right) ^T \nonumber \\&\quad \times \left[ \int _0^T\frac{\partial F_1}{\partial v}\left( \frac{\partial F_1}{\partial v}\right) ^T \hbox {d}t \right] ^{-1} \int _0^T\frac{\partial F_1}{\partial v}\delta v_k \hbox {d}t . \end{aligned}$$

Observe that \(\delta F_1(v^k,\ \delta v^k_{*})=\int ^T_0 \frac{\partial F_1}{\partial v} \delta v^k_{*} \hbox {d}t=0\). Moreover, for \(\delta \tilde{v}=0\), one also obtains the equality \(\delta F_1(v^k,\ \delta \tilde{v})=\int ^T_0 \frac{\partial F_1}{\partial v} \delta \tilde{v} \hbox {d}t=0\). Consequently, from the obvious inequality (\(\delta v^k_{*}\) is the solution of optimal control problem (19), (20) for sufficiently large k)

$$\begin{aligned}&\frac{1}{2}\int _0^T ||\delta v^k_{*}-\delta v_k||^2 \hbox {d}t \le \frac{1}{2}\int _0^T ||\delta \tilde{v}-\delta v_k||^2\hbox {d}t\nonumber \\&\quad =\frac{1}{2}\int _0^T ||\delta v_k||^2 \hbox {d}t \end{aligned}$$

and the definition of \(\delta v_k\), we have

$$\begin{aligned}&\delta F_0(v^k,\ \delta v^k_{*})=\int _0^T \left\langle \frac{\partial F_0}{\partial v},\ \delta v^k_{*} \right\rangle \hbox {d}t \le \nonumber \\&\quad -\frac{1}{2 g^k} \int _0^T ||\delta v^k_{*}||^2 \hbox {d}t \le 0. \end{aligned}$$

Finally, we obtain that \(\delta F_0(v^k,\ \delta v^k_{*}) \le 0\) what implies decrease (not monotonous for algebraically small values of k) of sequence \(F_0(v^k)\). The natural question arises, under which conditions there exists a limit control for the sequence of \(v^k\) as \(k \rightarrow \infty \). Based on the work [17], we can establish the following result for \(\varPhi _0=\phi \).

Lemma 1

If

$$\begin{aligned}&\left\{ \left( \begin{array}{l} f(\chi ,v,t)\\ \phi (\chi ,v,t)\\ \varPhi _1(\chi ,v,t) \end{array} \right) :\ v_l \le v \le v_u \right\} =\mathrm{conv}\nonumber \\&\left\{ \left( \begin{array}{l} f(\chi ,v,t)\\ \phi (\chi ,v,t)\\ \varPhi _1(\chi ,v,t) \end{array} \right) :\ v_l \le v \le v_u \right\} , \end{aligned}$$
(23)

where \(\mathrm{conv}\{\cdot \}\) denotes the convex hull of the set \(\{\cdot \}\); then there exists Lebesgue measurable limit control being the solution of successive minimization problems (17)–(22).

Proof

The proof of Lemma 1 is similar to that given in [14]. Therefore, it is omitted. \(\square \)

However, in the applications of the control of non-holonomic systems, it is difficult to fulfil equality (23). Moreover, from the practical point of view, it is not essential to know a limit control (which may not exist theoretically for control problem (1)–(5) in a class of Lebesgue measurable mappings), and only the convergence of the trajectories \(\chi ^k\), \(k=0,1,2,\ldots \) is of great importance. We are now ready to offer the following Theorem.

Theorem 1

The sequence of trajectories \(\chi ^k\), \(k=0,1,2,\ldots \), generated by iterative scheme (17)–(22), has a convergent subsequence to an optimal (limit) trajectory.

Proof

The proof of Theorem 1 will be limited to the key moments of argumentations. First, observe that inequality (6) expresses the fact of the boundedness of \(\chi \). By solving successive minimization problems (17)–(22), the sequence of pairs \((v^k,\ \chi ^k)\), \(k=0,1,2,\ldots \) is generated. Note that

$$\begin{aligned} ||\chi ^k|| \le \mathcal {A} \end{aligned}$$
(24)

and

$$\begin{aligned} ||\dot{\chi }^k|| = ||f(\chi ^k,v^k,t)|| \le \mathcal {B}, \end{aligned}$$
(25)

where \(\mathcal {B}=\max _{||\chi || \le \mathcal {A},\ u_l \le v \le u_u,\ t\in [0,\ T]}\{ ||f(\chi ,\ v,\ t)|| \}\). Inequality (24) means that the set \(\{\chi ^k:\ k=0,1,2,\ldots \}\) is uniformly bounded, whereas inequality (25) implies that this set is equi-continuous. Thus, the Arzela–Ascoli theorem [35] holds for \(\chi ^k\). A convergent subsequence \(\chi ^{k_n}\), \(n=1,2,\ldots \), for which \(\lim _{n \rightarrow \infty } \chi ^{k_n}=\chi ^{*}\), may be chosen from the sequence \(\chi ^k\). From Filippov’s theorem [17], it follows that \(\chi ^{*}\) is an absolutely continuous mapping. Furthermore, the continuity of expression \(g(\chi (T))-y_d\) with respect to \(\chi \) yields the following relation: \(g(\chi ^{*}(T))-y_d=0\). We also know that for sufficiently large n, sequence \(F_0(v^{k_n})\) monotonous decreases what implies (in general, local) optimality of \(\chi ^{*}\). \(\square \)

In practice, however, the fact of satisfying Cauchy’s condition with a given accuracy by the components of sequence \((\chi ^k,\ F_0(v^k))\) may be taken as a solution to optimal control problem (1)–(5). Consequently, iterative scheme (17)–(22) may be stopped provided that the following inequalities are fulfilled: \(||g(\chi ^k(T))-y_d|| \le \epsilon \) and \(\frac{|F_0(v^{k+1})-F_0(v^k)|}{F_0(v^k)} \le \epsilon _c\), where \(\epsilon \), \(\epsilon _c\) are small numbers (accuracy of iterative solution). A few remarks may be made regarding successive minimization process (17)–(22) and Theorem 1.

  • Remark 1 Observe that iterative scheme (17)–(22) provides continuous solutions to variations \(\delta v^k\), \(k=0,1,2,\ldots \) which result in continuous controls \(v^k\). The optimization processes known from the literature [19, 20] project infinite-dimensional control space into a finite-dimensional one and then apply techniques of a linear programming problem to find \(\delta v^k\) what causes a drastic increase of the amount of numerical computations and results only in near-optimal solutions. Furthermore, the algorithms from [19, 20] search for the solution of control problem (1)–(5) in a class of quasi-constant (discontinuous) controls. Consequently, they are not suitable for non-holonomic systems particularly of the first order, in which the controls (system velocities) have to be continuous. Moreover, by transforming state inequality constraints (4) to equivalent scalar integral form \(\int _0^T \sum _{i=0}^{\mathcal {O}} \eta _{\mathcal {O}}(c^i(\chi ,\ t)) \hbox {d}t\), control scheme (17)–(22) avoids the troublesome process of computing the Gateaux differentials which were used in works [14, 19, 20]. To numerically find Gateaux differentials, one has to solve a number of Cauchy’s problems. This number depends on amount of time instances activating the state inequality constraints.

  • Remark 2 Let us note that in the particular case \(\varPhi _0=0\) and the lack of control and state-dependent inequality constraints (2), (4), we obtain a classic non-holonomic motion planning problem whose solution may be significantly simplified compared with formulas (22). For this purpose, equality constraint (3) is transformed into the scalar functional form as follows

    $$\begin{aligned} F(v^0)=h(\chi ^0(T)) \ge 0, \end{aligned}$$
    (26)

    where \(h=\frac{1}{2}||g(\chi ^0(T))-y_d||^2\). The value for functional \(F(v^0+\delta v)\) can be expressed as (its derivation is presented in “Appendix B” section)

    $$\begin{aligned} F(v^0+\delta v)=F(v^0)+\delta F(v^0,\ \delta v), \end{aligned}$$
    (27)

    where \(\delta F(v^0,\ \delta v)\) is the Frechet differential of functional F; \(\delta F(v^0,\ \delta v)=\int _0^T \left\langle \frac{\partial F}{\partial v},\ \delta v \right\rangle \hbox {d}t\); \(\frac{\partial F}{\partial v}\) denotes the Frechet derivative; \(\frac{\partial F}{\partial v}=(f_v(t))^T \psi \); \(\psi \) stands for the conjugate mapping determined by the solution of differential equation \(\dot{\psi }+(f_{\chi }(t))^T \psi =0\) with boundary condition \(\psi (T)=\left( \frac{\partial h}{\partial \chi } \big |_{\chi ^0(T)} \right) ^T\). Minimization of (27) results in variation \(\delta v^0=-g^0 \frac{\partial F}{\partial v}\). Taking into account \(\delta v^0\), we obtain new control \(v^{1}=v^0+\delta v^0\). The process of minimization is then iterated for \(v^1\). The sequence of controls \(v^{k+1}=v^k+\delta v^k\) with

    $$\begin{aligned} \delta v^k=-g^k \frac{\partial F}{\partial v}\big |_{v^k}. \end{aligned}$$
    (28)

    where \(k=0,1,2,\ldots \) is obtained as a result of solving iterative scheme (28). On account of obvious relation \(\delta F(v^k,\ \delta v^k) \le 0\), we obtain a sequence of inequalities \(F(v^k) \ge F(v^{k+1}) \ge 0\) what implies convergence of \(F(v^k)\) to 0 as \(k \rightarrow \infty \). Observe that expression (28) does not require computation of undesirable Jacobian pseudo-inverse what results also in numerical stability of our control scheme in neighbourhoods of singular configurations. The unconstrained non-holonomic motion planning with \(\varPhi _0=0\) has been also analysed in works [1, 11, 12, 49]. Although studies [26, 37] involve the performance index in trajectory planning, the gradient of the Hamiltonian in [26, 37] is projected onto the null-subspace of the Jacobian what does not even guarantee an instantaneous sub-optimal solution in the case of the conflict of the kinematic tasks. The non-holonomic motion planning algorithms proposed in works [1, 11, 12, 26, 37, 49] require a pseudo-inverse of the Jacobian which can potentially contain kinematic and/or algorithmic singularities (in the case of an extended Jacobian).

  • Remark 3 Observe that the trajectory from Theorem 1 maximizes (locally, in general) the Hamiltonian for an arbitrary \(\varPhi _0\). However, for the specific tasks encountered in practice and determined by state equality constraint (3), convex set (2) and performance index with cost function \(\varPhi _0=||v||^2\), the Hamiltonian becomes a quadratic form in control v (strictly concave) provided that non-holonomic system equations (1) are affine with respect to v. Hence, it does not have local maxima in (2), which are different from the global maximum of the Hamiltonian. In other words, the approach proposed here finds the optimal control \(v^{*}\) globally maximizing the Hamiltonian. In general, it is difficult to find globally optimal trajectory \(\chi ^{*}\) for the control problem with state inequality constraints (4). This is due to the fact that the optimal control may not exist at all for such problems, or there may be many local minima of functional (11). One way to overcome this difficulty is a suitable choice of the initial control \(v^0\) and then a search for an optimal trajectory whose existence is guaranteed by Theorem 1. The control \(v^0\) could be chosen applying trials and errors technique by e.g. random generation of constant admissible values with condition \(v^0 \ne 0\). As is easy to see, for many non-holonomic wheeled mobile robots (unicycle, differential drives, car-like wheeled mobile robots, cars with trailers), their corresponding Jacobian matrices become trivially singular for \(v(t)=0\), i.e. control \(v(t)=0\) is singular configuration for the aforementioned non-holonomic systems. Nevertheless, calculation by trial and error techniques of a nonzero admissible initial control which is relatively close to an optimal one seems to be both hard and time-consuming. Alternatively, it would be reasonable to take \(v^{0,0}(t)=0\), \(t \in [0,\ T]\), as the initial control (\(v^{0,0}=0\) is admissible) particularly when applying motion planner (17)–(22) in a model predictive control mode and/or in a complex work spaces including many obstacles. Moreover, as the numerical simulations carried out in Sect. 5 show, optimal solutions corresponding to \(v^{0,0}=0\) seem to be more energy-saving than those starting with \(v^{0,0}\ne 0\). As we know, iterative optimization scheme (17)–(22) converges relatively fast to an optimal solution provided that both initial control does not equal zero and it is in a sufficiently small neighbourhood of the optimal solution. The next section offers a numerically efficient algorithm providing an initial nonzero admissible control for iterative scheme (17)–(22). Moreover, this initial control is shown to be close (in the \(L^2_m[0,\ T]\) norm) to the optimal solution.

4 Computation of an initial admissible control

Determination of the initial control for iterative procedure (17)–(22) will also be based herein on a suitable modification of the negative formulation of the Pontryagin Maximum Principle. For this purpose, we modify performance index (5) as follows

$$\begin{aligned} F_3(v)=\int _0^T\varPhi _3(\chi ,v,t)\hbox {d}t, \end{aligned}$$
(29)

where \(\varPhi _3=\phi +\sum _{i=1}^m \eta _v(v_i(t))\). Moreover, equality constraint (3) and collision avoidance conditions (4) may be equivalently expressed as a scalar equality constrain of the form \(\frac{1}{2}||g(\chi (T))-y_d||^2+\int _0^T \sum _{i=0}^{\mathcal {O}} \eta _{\mathcal {O}}(c^i(\chi ,t))\hbox {d}t=0\), or finally in an equivalent integrable form

$$\begin{aligned} \int _0^T \varPhi _4(\chi ,v,t)\hbox {d}t=0, \end{aligned}$$
(30)

with \(\varPhi _4=\left\langle \left( \frac{\partial g}{\partial \chi }\right) ^T\left( g(\chi )-y_d\right) ,\ f(\chi ,v,t) \right\rangle + \sum _{i=0}^{\mathcal {O}} \eta _{\mathcal {O}}(c^i(\chi ,t))+\frac{\frac{1}{2}||g(\chi _0)-y_d||^2}{T}\). For further considerations, the left-hand side of (30) will be reformulated in functional scalar form as follows

$$\begin{aligned} F_4(v)=0, \end{aligned}$$
(31)

where \(F_4(v)=\int _0^T \varPhi _4(\chi ,v,t)\hbox {d}t\). Similarly as in the previous section, we take an arbitrary admissible control \(v^{0,0}(t)\) (including also \(v^{0,0}(t)= 0\)) as an initial guess for optimal control problem (1)–(5). Let \(\chi ^{0,0}(\cdot )\) denote the trajectory corresponding to control \(v^{0,0}\) with initial condition \(\chi ^{0,0}(0)=\chi _0\). The value of the functional \(F_3\) for control \(v^{0,0}+\delta v\) can thus be expressed as (its derivation is presented in “Appendix A” section)

$$\begin{aligned} F_3(v^{0,0}+\delta v)=F_3(v^{0,0})+\delta F_3(v^{0,0},\delta v)+o(\delta v),\nonumber \\ \end{aligned}$$
(32)

where \(\delta F_3(v^{0,0},\delta v)=\int _0^T \left\langle \frac{\partial F_3}{\partial v},\ \delta v \right\rangle \hbox {d}t\); \(\frac{\partial F_3}{\partial v}=\left( (f_{v}(t))^T\psi _3+\left( \frac{\partial \varPhi _3}{\partial v}\right) ^T\right) |_{(\chi ^{0,0},\ v^{0,0})}\); \(\dot{\psi }_3+(f_{\chi }(t))^T \psi _3=-\left( \frac{\partial \varPhi _3}{\partial \chi }\right) ^T|_{(\chi ^{0,0},\ v^{0,0})}\); \(\psi _3(T)=0\). Similarly, the value of the functional determined by the left-hand side of Eq. (31) for control \(v^{0,0}+\delta v\) becomes (its derivation is also presented in “Appendix A” section)

$$\begin{aligned} F_4(v^{0,0}+\delta v)=F_4(v^{0,0})+\delta F_4(v^{0,0},\delta v)+o(\delta v),\nonumber \\ \end{aligned}$$
(33)

where \(\delta F_4(v^{0,0},\delta v)=\int _0^T \left\langle \frac{\partial F_4}{\partial v},\ \delta v \right\rangle \hbox {d}t\); \(\frac{\partial F_4}{\partial v}=\left( (f_{v}(t))^T\psi _4+\left( \frac{\partial \varPhi _4}{\partial v}\right) ^T\right) |_{(\chi ^{0,0},\ v^{0,0})}\); \(\dot{\psi }_4+(f_{\chi }(t))^T \psi _4=-\left( \frac{\partial \varPhi _4}{\partial \chi }\right) ^T|_{(\chi ^{0,0},\ v^{0,0})}\); \(\psi _4(T)=0\). Hence, the modified version of the negative formulation of Pontryagin’s Maximum Principle takes the following form:

$$\begin{aligned} F_3(v^{0,0}+\delta v)=F_3(v^{0,0})+\delta F_3(v^{0,0},\delta v)\longrightarrow \min _{\delta v} \end{aligned}$$
(34)

subject to the scalar linear constraint

$$\begin{aligned} g^{0,0} F_4(v^{0,0})+\delta F_4(v^{0,0},\delta v^0)=0, \end{aligned}$$
(35)

where \(0< g^{0,0}<<1\); \(\delta v^0=g^{0,0} \delta v\). Minimization of (34) results in variation \(\delta v_0=-g_{0,0} \frac{\partial F_3}{\partial v}\), where \(g_{0,0}\) is a small positive coefficient, which does not necessarily satisfy equality constrain (35). Therefore, we have to solve the following auxiliary control problem (projection of variation \(\delta v_0\) onto linear functional constrain (35)):

$$\begin{aligned} \min _{\delta v^0} \left\{ \frac{1}{2} \int _0^T ||\delta v^0-\delta v_0||^2\hbox {d}t \right\} , \end{aligned}$$
(36)

subject to constraint (35)

$$\begin{aligned} \int _0^T \left\langle \frac{\partial F_4}{\partial v},\ \delta v^0 \right\rangle \hbox {d}t=- g^{0,0} F_4(v^{0,0}). \end{aligned}$$
(37)

Relations (36), (37) constitute the problem of the relative minima from the calculus of variations whose solution may be expressed as follows

$$\begin{aligned} \delta v^0=\delta v_0 - \frac{\partial F_4}{\partial v} \frac{\int _0^T \left\langle \frac{\partial F_4}{\partial v},\ \delta v_0 \right\rangle \hbox {d}t+g^{0,0} F_4(v^{0,0})}{\int _0^T \left\langle \frac{\partial F_4}{\partial v},\ \frac{\partial F_4}{\partial v} \right\rangle \hbox {d}t}. \end{aligned}$$
(38)

Taking into account (38), we obtain new control \(v^{0,1}(t)=v^{0,0}(t)+\delta v^0(t)\), where \(t \in [0,\ T]\). The process of minimization is then repeated for \(v^{0,1}\). The sequence of controls \(v^{0,k+1}=v^{0,k}+\delta v^{k}\), where \(k=0,1,2,\ldots \);

$$\begin{aligned} \delta v^k=\delta v_k - \frac{\partial F_4}{\partial v} \frac{\int _0^T \left\langle \frac{\partial F_4}{\partial v},\ \delta v_k \right\rangle \hbox {d}t+g^{0,k} F_4(v^{0,k})}{\int _0^T \left\langle \frac{\partial F_4}{\partial v},\ \frac{\partial F_4}{\partial v} \right\rangle \hbox {d}t}; \end{aligned}$$
(39)

\(\delta v_k=-g_{0,k} \frac{\partial F_3}{\partial v}\); \(g_{0,k}\), \(g^{0,k}\) are positive coefficients which fulfil the following inequalities: \(0<g_{0,k},\ g^{0,k}<< 1\), and the corresponding (according to Eq. (1)) state trajectories \(\chi ^{0,k+1}\) are obtained as a result of solving iterative scheme (34)–(39). On account of the obvious relation \(\delta F_4(v^{0,k},\delta v^k)=-g^{0,k} F_(v^{0,k}) \le 0\), we obtain inequalities \(F_4(v^{0,k}) > F_4(v^{0,k+1}) \ge 0\) and consequently \(F_4(v^{0,k}) \rightarrow 0\) as \(k \rightarrow \infty \). If this is the case, from (39) we have \(\delta v^k \rightarrow \delta v_k -\frac{\int _0^T \left\langle \frac{\partial F_4}{\partial v},\ \delta v_k \right\rangle \hbox {d}t}{\int _0^T \left\langle \frac{\partial F_4}{\partial v},\ \frac{\partial F_4}{\partial v} \right\rangle \hbox {d}t}\frac{\partial F_4}{\partial v}\). Using definition of \(\delta v_k\) and the integral Schwartz inequality, it is easy to show that \(\delta F_3\left( v^{0,k},\ \delta v_k -\frac{\int _0^T \left\langle \frac{\partial F_4}{\partial v},\ \delta v_k \right\rangle \hbox {d}t}{\int _0^T \left\langle \frac{\partial F_4}{\partial v},\ \frac{\partial F_4}{\partial v} \right\rangle \hbox {d}t}\frac{\partial F_4}{\partial v}\right) \le 0\) what implies decrease (not monotonous for algebraically small values of k) of sequence \(F_3(v^{0,k})\). Based on iterative minimization scheme (34)–(39), we can propose the following theorem.

Theorem 2

The sequence of trajectories \(\chi ^{0,k}\), \(k=0,1,2,\ldots \), generated by iterative scheme (34)–(39), has a convergent subsequence to an optimal (limit) trajectory.

Proof

Since functionals \(F_3(\cdot )\) and \(F_4(\cdot )\) are expressed in scalar forms, the proof of Theorem 2 becomes the particular case of the proof of Theorem 1, in which functional \(F_2(\cdot )\) was given in the vector form. Consequently, the proof of Theorem 2 may be omitted. \(\square \)

In practice, it suffices to take control \(v^{0,k}\) for k equal to several hundreds (two or three hundred) as the initial admissible solution \(v^0(t)\) for optimization scheme (17)–(22). Let us note that the scalar form of functional \(F_4\) makes it possible for many non-holonomic structures (unicycle, differential drives, car-like wheeled mobile robots, cars with trailers) to assume \(v^{0,0}=0\) as the initial control for iterative procedure (34)–(39). If this is the case, we have \(\frac{\partial F_4}{\partial v}=\left( \frac{\partial g}{\partial \chi }f_v(t) \right) ^T (g(\chi )-y_d)\) and assuming that \(g(\chi )-y_d \notin \ker \left( \left( \frac{\partial g}{\partial \chi }f_v(t) \right) ^T \right) \), one obtains \(\frac{\partial F_4}{\partial v}\ne 0\). Hence, \(\int _0^T \left\langle \frac{\partial F_4}{\partial v},\ \frac{\partial F_4}{\partial v} \right\rangle \big |_{v^{0,0}=0} \hbox {d}t >0\) and formula (38) may be applied to determine \(\delta v^0\). From definition of \(F_4(v)\), it follows that iterative scheme (34)–(39) seem to converge slower than that given by expressions (17)–(22). The reason is that \(F_4(v) < ||g(\chi (T))-y_d||\) for small norms \(||g(\chi (T))-y_d||\) provided that there are no collisions between system (1) and the obstacles. On the other hand, procedure (34)–(39) of determining the initial admissible solution should converge faster than that given by (17)–(22) for larger norms \(||g(\chi (T))-y_d||\) (>2).

5 Numerical examples

The aim of this section is to illustrate the efficiency of planning strategies (17)–(22), (28), (34)–(39) in the case of existence or non-existence of both control and/or state-dependent inequality constraints on a suitably chosen number of motion planning tasks. The two non-holonomic dynamic systems are assumed in this section to accomplish the corresponding point-to-point tasks. The first one is a simple benchmark non-holonomic system (unicycle or differential drive wheeled mobile robot [46, 52]) moving without slip on a horizontal plane. This non-holonomic system is used as a basis for many types of non-holonomic wheeled mobile robots. For this reason, this model has attracted much theoretical attention by nonlinear systems workers. Moreover, this simple non-holonomic structure was chosen herein to compare performance of control strategy (28) with representative motion planning algorithms known from the literature. The second plant, assumed in the simulations, is a car pulling the two trailers without slip which constitutes a complex non-holonomic dynamic system subject to control and state-dependent inequality constraints. As is known, both non-holonomic dynamic systems considered herein are controllable. Nevertheless, a collision-free manoeuvring of the car with trailers in a work space including obstacles does not even seem to be intuitively obvious due to both limited instantaneous velocity directions of the car and a strongly non-convex shape of the plant structure. Consequently, a point-to-point task to be accomplished by the car with two trailers makes a challenge for control strategies (17)–(22), (28), (34)–(39). In all the simulations, SI units are used. The output equation \(g(\cdot )\) from (3) is assumed in the whole section to be identity mapping, i.e. \(g(\chi (T))=\chi (T)\), (\(\kappa =n\)). Moreover, accuracy of iterative solutions is assumed to be less or equal to \(10^{-4}\), i.e. \(\epsilon \le 10^{-4}\) and \(\epsilon _c \le 10^{-4}\), respectively.

5.1 Numerical comparisons of the proposed control scheme with the Newton algorithm

Due to some analogies between our control strategy (28) which incorporates functional derivatives into iterative process and the Newton algorithms involving the Jacobian pseudo-inverse matrices, it seems natural to numerically compare these two different approaches on a chosen non-holonomic motion planning task. This task is assumed to be accomplished by the simple benchmark non-holonomic system represented herein by either unicycle or differential drive [46, 52]. On account of the fact that state constraints of the motion planning problem are directly expressed in task coordinates, the posture \(\chi \) of this system will also be represented in the same coordinates. As is known, the unicycle in the strict sense is a mobile robot with serious problem of balance in static conditions. (It can fall over.) However, there exist vehicles that are kinematically equivalent in the task coordinates to the unicycle but more stable from a mechanical viewpoint. Among these, the most important is the differential drive [46, 52]. This drive consists of two actuated fixed wheels mounted on the left and right side of the vehicle platform. Actuated fixed wheels are driven by motors mounted on fixed positions of the vehicle. Their axis of rotation has a fixed direction with respect to the platform’s coordinate frame. The two wheels of radiuses equal to R are independently driven. For the differential drive, denote \((\chi _1,\ \chi _2)^T \in \mathbb {R}^2\) the Cartesian coordinates of the midpoint of the segment of the length d joining the two wheel centres and by \(\chi _3 \in \mathbb {R}\) the common orientation of the actuated fixed wheels (hence, of the platform). Then, motion equations of the differential drive expressed in the task coordinates present an analytic drift-less dynamic system of the form [46]

$$\begin{aligned} \dot{\chi }=\left[ \begin{array}{ll} \cos (\chi _3) &{} 0\\ \sin (\chi _3) &{} 0\\ 0 &{} 1 \end{array} \right] v, \end{aligned}$$
(40)

where \(\chi =(\chi _1,\ \chi _2,\ \chi _3)^T\) denotes the posture of the differential drive; \(n=3\); \(v= ( v_1,\ v_2)^T\); \(v_1\) denotes linear velocity of the vehicle; and \(v_2\) is its angular velocity (\(m=2\)). If the wheels rotate at the same angular speed, the differential drive moves straightforward or backward (\(v_2=0\)). If one wheel is rotating faster than the other, the vehicle follows a curved path (\(v_1 \ne 0\) and \(v_2 \ne 0\)). If both wheels are rotating at the same velocity in opposite direction, the differential drive turns about the midpoint of the two driving wheels, i.e. \(v_1=0\). The linear and angular velocities \(v_1\), \(v_2\) in (40) are expressed as functions of the physical inputs, i.e. the angular speeds \(\omega _1\), \(\omega _2\) of the actuated fixed wheels. As is known, there is a one-to-one correspondence between v in (40) and \(\omega =(\omega _1,\ \omega _2)^T\) given by the following expression:

$$\begin{aligned} v=\left[ \begin{array}{cc} \frac{R}{2} &{} \frac{R}{2}\\ \frac{R}{d} &{} -\frac{R}{d} \end{array} \right] \omega . \end{aligned}$$
(41)

From linear Eq. (41), it follows that the upper and lower wheel speed limits \(v_u\), \(v_l\) are determined based on the physical upper and lower angular speed limits \(\omega _u\), \(\omega _l\), respectively, which depend on the physical abilities of the motors driving the both wheels of the differential drive. Motion equations (40) of the differential drive are most often used as a basis for many types of non-holonomic wheeled mobile robots. Hence, the numerical comparison of the motion planning algorithms will also be carried out in this section for the differential drive. There are many representative Newton algorithms of non-holonomic motion planning [1, 11, 12, 26, 37, 49]. Among them, we chose an improved version of the algorithm (originated in [11]) recently proposed in the work [49]. The control law offered in [49] is given by the following iterative formulas:

$$\begin{aligned} \begin{array}{l} \delta v^k=-\gamma R^{-1}(t)(f_v(\chi ^k,v^k))^T\psi _{k,22}(t)\\ \left( \psi _{k,22}(T)+(D_k(T))^{-1}\psi _{k,32}(T)\right) ^{-1}\\ \qquad (D_k(T))^{-1}(\chi ^k(T)-y_d), \end{array} \end{aligned}$$
(42)

where

$$\begin{aligned} \psi _k(t)= & {} \left[ \begin{array}{lll} \psi _{k,11} &{} \psi _{k,12} &{} \psi _{k,13}\\ \psi _{k,21} &{} \psi _{k,22} &{} \psi _{k,23}\\ \psi _{k,31} &{} \psi _{k,32} &{} \psi _{k,33} \end{array} \right] \ \text {with} \ \psi _k(0)=\mathbb {I}_{3\times n};\ n=3;\\ \dot{\psi }_k(t)= & {} \left[ \begin{array}{ccc} f_{\chi }(\chi ^k,v^k) &{} -f_v(\chi ^k,v^k)R^{-1}(t)(f_v(\chi ^k,v^k))^T &{} 0_{3\times 3}\\ -Q_k(t) &{} -(f_{\chi }(\chi ^k,v^k))^T &{} 0_{3\times 3}\\ D_k(t)Q_k(t) &{} 0_{3\times 3} &{} f_{\chi }(\chi ^k,v^k) \end{array} \right] \psi _k(t);\\ \dot{D}_k(t)= & {} f_v(\chi ^k,v^k)R^{-1}(t)(f_v(\chi ^k,v^k))^T+f_{\chi }(\chi ^k,v^k)D_k(t) +D_k(t)(f_{\chi }(\chi ^k,v^k))^T \end{aligned}$$

with \(D_k(0)=0\); \(R(t)=\mathbb {I}_2\); \(Q_k(t)=10\cdot V\cdot V^T\); \(V=\Gamma (-\frac{1}{\sqrt{2}},\ \frac{1}{\sqrt{2}},\ 0)^T\); \(\Gamma =\left[ \begin{array}{cc} \mathbb {I}_2 &{} 0\\ 0 &{} 0 \end{array} \right] \); \(\gamma =0.1\) and \(0_{3\times 3}\) denotes the \(3 \times 3\) zero matrix; \(k=0,1,2,\ldots \). The first task also analysed in [49] is to move the differential drive from initial posture \(\chi (0)=(0,\ 0,\ 0)^T\) to the final location \(y_d=(1,\ 1,\ 0)^T\) in the time horizon T equal to \(T=2\). The initial control \(v^0\) assumed in work [49] equals \(v^0=\left( 1,\ \sin (\frac{2 \pi t}{T})\right) ^T\). Our iterative scheme (28) with the same initial control and constant gain coefficients \(g^k\) equal to \(g^k=0.3\), \(k=0,1,2,\ldots ,\) has been applied to solve the aforementioned positioning task. The results of application of iterative schemes (28) and (42) are depicted in Figs. 1 and 2 which show accurate positioning of both schemes.

Nevertheless, energy E lost during the differential drive movement equals \(E=4.81\) for control scheme (42) and \(E=4.1\) for our iterative procedure, respectively, where \(E=\int _0^T ||v||^2 \hbox {d}t\). Inverse-free trajectory generator (28) has also been started with \(g^k=0.3\) and initial control \(v^0=0\) which is singular for iterative procedure (42). The convergence result for this simulation is given in Fig. 3.

Fig. 1
figure 1

Convergence to \(y_d\) corresponding to generator (28) with \(v^0 \ne 0\)

Full size image
Fig. 2
figure 2

Convergence to \(y_d\) corresponding to generator (42) with \(v^0 \ne 0\)

Full size image
Fig. 3
figure 3

Convergence to \(y_d\) corresponding to generator (28) with \(v^0 = 0\)

Full size image

Moreover, the lost energy by the differential drive equals \(E=3.81\) in this case. Consequently, iterative scheme (28) seems to be more energy-saving than that given by formula (42). In the last simulation of this section, we tried to find the most energy-saving solution for the analysed positioning task. For this purpose, iterative optimization scheme (17)–(22) has been applied with performance index (11) equal to \(\varPhi _0=||v||^2\). From Remark 3, it follows that our control scheme finds global solution in such a case. Gain coefficients \(g_k\), \(g^k\), \(k=0,1,2,\ldots ,\) are assumed to be constant for all k and equal to \(g_k=0.01\) and \(g^k=0.1\), respectively. Initial control \(v^0\) is the same as that given in work [49], i.e. \(v^0=\left( 1,\ \sin (\frac{2 \pi t}{T})\right) ^T\). The results of the simulation are presented in Figs. 4 and 5 which indicate accurate positioning of the differential drive (Fig. 4) and convergence to the global minimum (Fig. 5) which equals \(E=3.6\).

Fig. 4
figure 4

Convergence to \(y_d\) corresponding to generator (17)–(22) with \(v^0 \ne 0\) and \(\varPhi _0=||v||^2\)

Full size image
Fig. 5
figure 5

Convergence of \(F_0(v)\) corresponding to generator (17)–(22) with \(v^0 \ne 0\) and \(\varPhi _0=||v||^2\)

Full size image
Fig. 6
figure 6

Kinematic scheme of the car pulling the two trailers and the positioning task to be accomplished

Full size image

5.2 Non-holonomic motion planning for a car pulling the two trailers

The dynamic structure considered in this section is a car pulling the two trailers. The whole system is subject to the rolling of the wheels without sliding of both the car and the trailers. The car will be represented by two driving wheels connected by an axle. The posture of the system is given by two position coordinates of the last (second) trailer and three angles with respect to an absolute coordinate system. There are only two inputs, namely one linear velocity and one angular velocity which represent the actions of the car. The posture \(\chi \) of the whole non-holonomic system (shown schematically in Fig. 6) is expressed by five task coordinates (\(n=5\)) \(\chi =(\chi _1,\ldots ,\chi _5)^T\), where \((\chi _1,\ \chi _2)\) are the coordinates of the centre of the axle between the two wheels of the second trailer; \(\chi _5\) is the orientation angle with respect to the \(O\chi _1\) axis of the pulling car; \(\chi _i\) for \(3 \le i \le 4\) denotes the orientation angle of the trailer \(5-i\) with respect to the \(O\chi _1\) axis.

The motion equations of a car pulling the two trailers present dynamic system of the form [27]

$$\begin{aligned} \dot{\chi }=\left[ \begin{array}{cc} \cos (\chi _5-\chi _4) \cos (\chi _4-\chi _3) \cos (\chi _3) &{} \quad 0\\ \cos (\chi _5-\chi _4) \cos (\chi _4-\chi _3) \sin (\chi _3) &{} \quad 0\\ \frac{1}{l_1} \cos (\chi _5-\chi _4) \sin (\chi _4-\chi _3) &{} \quad 0\\ \frac{1}{l_2} \sin (\chi _5-\chi _4) &{} \quad 0\\ 0 &{} \quad 1 \end{array} \right] v, \end{aligned}$$
(43)

where \(l_1\) is the distance from the wheels of the last trailer to the wheels of the first trailer; \(l_2\) denotes the distance from the wheels of the first trailer to the wheels of the pulling car, 2w stands for the distance between the wheels of the car; l is the length of the car; \(v=(v_1,\ v_2)^T\); \(v_1\) denotes the linear velocity of the pulling car, and \(v_2\) is its angular velocity (\(m=2\)). Distances between the two wheels of the first and second trailers are assumed also to be equal to 2w. Let us note that the pulling car represents physically a differential drive with one or two passive castor wheels which are attached in the front of the car for balance and stability. Castor wheels are not actuated but they can also rotate freely about an axis perpendicular to their axis of rotation. The linear and angular velocities \(v_1\), \(v_2\) in Eq. (43) of the pulling car may be expressed as linear functions of the physical velocity inputs, i.e. angular speeds \(\omega _1\), \(\omega _2\) of the both actuated fixed wheels of the differential drive. The one-to-one correspondence between v in (43) and \(\omega =(\omega _1,\ \omega _2)^T\) is given by the following formula:

$$\begin{aligned} v=\left[ \begin{array}{cc} \frac{R}{2} &{} \frac{R}{2}\\ \frac{R}{2w} &{} -\frac{R}{2w} \end{array} \right] \omega . \end{aligned}$$
(44)

The drawbar of the first trailer is for simplicity attached to the midpoint of the segment joining the two wheel centres of the car. Similarly, the drawbar of the second trailer is attached to the midpoint of the segment joining the two wheel centres of the first trailer. All the wheels of the trailers are passive (not actuated). Moreover, the drawbars of the trailers are rigidly fixed to the trailer platforms.

Fig. 7
figure 7

Control \(v_1\) corresponding to generator (28) with \(v^0 = 0\) and non-trivial singular motion

Full size image
Fig. 8
figure 8

Algebraic difference of controls \(v_1-v_2\) corresponding to generator (28) with \(v^0 = 0\) and non-trivial singular motion

Full size image

5.2.1 Constraint-free singular motion of the car with two trailers

The second task is to move the car pulling the two trailers from its initial posture \(\chi (0)=(-4,\ -2,\ 0, 0,\ \pi /2)^T\) to desired location \(y_d=(-4,\ -2,\ 0,\ \pi /2, \pi )^T\). The final moment T of accomplishing the task equals \(T=12\). As is known [27], postures \(\chi (0)\) and \(y_d\) are singular for non-holonomic system (43). The initial control \(v^0\) equals \(v^0=0\). Our control scheme (28) has been applied to solve the third task with gain coefficients \(g^k\) equal to \(g^k=0.05\), \(k=0,1,\ldots \) The results of the simulation are presented in Figs. 7, 8, 9 and 10 which indicate that accurate positioning of the car with trailers is achieved (Fig. 9). Moreover, as is seen from Figs. 7 and 8, controls generating the system movement schematically shown in Fig. 10, are both continuous and non-trivially singular (\(v_1=v_2\)) according to the results of the work [27]. Consequently, iterative scheme (28) is also able to tackle non-trivial singular controls.

Fig. 9
figure 9

Convergence to \(y_d\) corresponding to generator (28) with \(v^0 = 0\) and non-trivial singular motion

Full size image
Fig. 10
figure 10

The singular motion of the car with two trailers corresponding to generator (28) for \(v^0 = 0\)

Full size image

5.2.2 Constraint optimal motions of the car pulling the trailers

The third task is to move non-holonomic system (43) from initial posture \(\chi (0)\!=\!(-4,\ -2,\ \pi /4, \pi /4,\ \pi /4)^T\) to desired (singular) location \(y_d=(0,\ 0,\ 0,\ 0,\ \pi /2)^T\). The final time of task accomplishment equals \(T=16\). During the movement of the car with trailers, control and state-dependent inequality constraints (2), (4) have to be maintained. The lower and upper limits on controls \(v_i\) are assumed to be equal to \(v_{u,i}=1\) and \(v_{l,i}=-1\), \(i=1,2\), respectively. The purpose of the third task is also to minimize performance index (11). There are two obstacles in the work space schematically shown in Fig. 6. The threshold values \(\gamma ^1_c\) and \(\gamma ^2_c\) taken for computations are equal to \(\gamma ^1_c=0.14\) and \(\gamma ^2_c=0.2\), respectively. Let us note that both initial and desired postures of non-holonomic system (43) are located in neighbourhoods of obstacles to make difficult the collision-free manoeuvring of the car pulling two trailers. In order to calculate the values of \(r_i\) which are the distance functions, both the length l of the car and its width (represented by the length 2w of the axle connecting the two driving wheels) have been discretized into 8 points. Analogous discretization process was carried out for the trailers. For simplicity of computations, all the wheels of non-holonomic system (43) are not taken into account in the collision avoidance. Depending on the initial control (nonzero or zero) and performance index \(\varPhi _0\), we have carried out two simulations with \(\varPhi _0=\sum _{i=1}^m \eta _v(v_i)+\sum _{j=0}^{\mathcal {O}}\eta _{\mathcal {O}}(c^j)\) (\(\phi =0\)) and the third one with \(\varPhi _0=\phi +\sum _{i=1}^m \eta _v(v_i)+\sum _{j=0}^{\mathcal {O}}\eta _{\mathcal {O}}(c^j)\), where \(\phi =||v||^2\), respectively. In this subsection, exterior penalty functions (7), (9) are assumed to be incorporated in iterative optimization schemes (17)–(22), (34)–(39). The following values for the exterior penalty function parameters \(w^{\mathrm{ex},i}_v\), \(w^{\mathrm{ex},i}_{\mathcal {O}}\) are taken in the first two simulations: \(w^{\mathrm{ex},i}_v=1.5\), \(w^{\mathrm{ex},i}_{\mathcal {O}}=7.5\), \(i=1,2\), respectively. The first two simulations depend on initial control. In the first simulation, initial control \(v^{0,0}\) is equal to zero, i.e. \(v^{0,0}=0\), \(t \in [0,\ 16]\). Consequently, iterative scheme (34)–(39) from Sect. 4 has to be applied to determine nonzero initial admissible control (close to the optimal one) for optimization procedure (17)–(22). Gain coefficients \(g_{0,k}\), \(g^{0,k}\), \(g_{k}\) \(g^{k}\), \(k=0,1,2,\ldots \) are assumed to be constant for all k, i.e. \(g_{0,k}= g^{0,k}=0.015\) and \(g_{k}= g^{k}=0.15\), respectively. In order to find nonzero initial control for optimization process (17)–(22), five hundred iterations of the algorithm given by expressions (34)–(39) were executed. Then, proper optimization process (17)–(22) has been started. The results of the first simulation are presented in Figs. 11, 12, 13, 14 and 15 which show accurate positioning of dynamic system (43) (Fig. 13).

Fig. 11
figure 11

Control \(v_1\) corresponding to combined generators (17)–(22) and (34)–(39) with exterior penalty functions (7), (9)

Full size image
Fig. 12
figure 12

Control \(v_2\) corresponding to combined generators (17)–(22) and (34)–(39) with exterior penalty functions (7), (9)

Full size image
Fig. 13
figure 13

Convergence to \(y_d\) corresponding to combined generators (17)–(22) and (34)–(39) with exterior penalty functions (7), (9)

Full size image
Fig. 14
figure 14

Convergence of \(F_0(v)\) corresponding to combined generators (17)–(22) and (34)–(39) with exterior penalty functions (7), (9)

Full size image
Fig. 15
figure 15

Optimal motion of the car with two trailers corresponding to combined generators (17)–(22) and (34)–(39) with exterior penalty functions (7), (9)

Full size image

As is seen from Figs. 11 and 12, control constraints (2) are not violated. Figure 14 presents convergence of performance index \(F_0(v)\) to the (global) minimum which guarantees collision-free movement of the car with two trailers (Fig. 15). The energy E lost during the movement equals \(E=8.76\). By applying the trial and error technique, we have found a nonzero initial admissible control for the second simulation. It equals \(v^0=\left( 0.4,\ -0.2 \sin (\frac{2 \pi t}{T})\right) ^T\). Figure 16 shows the movement of non-holonomic system (43) corresponding to control \(v^0\).

Fig. 16
figure 16

The motion of the car with trailers corresponding to initial control \(v^0 \ne 0\)

Full size image

Observe that \(v^0\) violates both state equality and inequality constraints (3), (4). Gain coefficients \(g_k\), \(g^k\) taken in this (second) simulation equal \(g_k=g^k=0.04\), \(k=0,1,2,\ldots \) Iterative optimization scheme (17)–(22) has been applied to find optimal solution. Figures 17, 18, 19, 20 and 21 present the results of the second simulation. As is seen from Figs. 17, 18, 20 and 21, control and state-dependent inequality constraints are maintained in the whole time horizon of the system movement. Furthermore, Fig. 19 presents convergence of system (43) to desired location \(y_d\). The energy E required to move the car in the second simulation equals \(E=9.47\). Similarly as in the case of the differential drive, nonzero initial admissible control provides more time-consuming optimal motion than that equal to zero.

Fig. 17
figure 17

Control \(v_1\) corresponding to generator (17)–(22) with exterior penalty functions (7), (9) and \(v^0 \ne 0\)

Full size image
Fig. 18
figure 18

Control \(v_2\) corresponding to generator (17)–(22) with exterior penalty functions (7), (9) and \(v^0 \ne 0\)

Full size image
Fig. 19
figure 19

Convergence to \(y_d\) corresponding to generator (17)–(22) with exterior penalty functions (7), (9) and \(v^0 \ne 0\)

Full size image
Fig. 20
figure 20

Convergence of \(F_0(v)\) corresponding to generator (17)–(22) with exterior penalty functions (7), (9) and \(v^0 \ne 0\)

Full size image
Fig. 21
figure 21

Optimal motion of the car with two trailers corresponding to generator (17)–(22) with exterior penalty functions (7), (9) and \(v^0 \ne 0\)

Full size image
Fig. 22
figure 22

Convergence to \(y_d\) corresponding to generator (17)–(22) with exterior penalty functions (7), (9), \(v^0 \ne 0\) and \(\phi \ne 0\)

Full size image
Fig. 23
figure 23

Convergence of \(F_0(v)-J(v)\) corresponding to generator (17)–(22) with exterior penalty functions (7), (9), \(v^0 \ne 0\) and \(\phi \ne 0\)

Full size image

In the third simulation, exterior penalty functions (7), (9) are applied with \(\varPhi _0=\phi +\sum _{i=1}^m \eta _v(v_i)+\sum _{j=0}^{\mathcal {O}}\eta _{\mathcal {O}}(c^j)\), where \(\phi =||v||^2\) and initial control \(v^0\) is equal to that from the previous simulation. The following values for the exterior penalty function parameters \(w^{\mathrm{ex},i}_v\), \(w^{\mathrm{ex},i}_{\mathcal {O}}\) were taken in the current simulation: \(w^{\mathrm{ex},i}_v=150\), \(w^{\mathrm{ex},i}_{\mathcal {O}}=1500\), respectively. Gain coefficients \(g_k\), \(g^k\) are assumed to be constant for all k, \(k=0,1,\ldots ,\) and equal to \(g_k=0.0005\), \(g^k=0.2\), respectively. The results of the current simulation are presented in Figs. 22, 23 and 24 which indicate accurate positioning of the car with trailers (Fig. 22). As is seen from Fig. 23, performance index \(F_0(v)-J(v)\) reflecting fulfilment of control and state inequality constraints (2), (4), converges to the (global) minimum what guarantees the collision-free movement of the car pulling the two trailers. Figure 24 presents the convergence of J(v) to the (global) minimum equal to \(J(v)=7.2\). Observe that energy E required to move the car with trailers in the first simulation for initial guess \(v^{0,0}=0\) differs slightly from that obtained in the current simulation and equals to \(E=7.2\).

Fig. 24
figure 24

Convergence of J(v) corresponding to generator (17)–(22) with exterior penalty functions (7), (9) and \(v^0 \ne 0\)

Full size image

6 Conclusions

A new method based on the non-classical (negative) formulation of the Pontryagin Maximum Principle to find optimal trajectories of non-holonomic systems is proposed in the paper. An important factor which affects the speed and accuracy of determining the minimizing sequence of controls, compared with works [19, 20] , is elimination of both the time-consuming linear programming problem and discontinuity of the controls. Moreover, proposed iterative control scheme (28) does not require explicit computing any inverse or pseudo-inverse of the (general) Jacobian matrix. This important feature of the offered generator for the classic (constraint-free) non-holonomic motion planning tasks does not cause to stop the algorithm if singular configurations appear when operating the non-holonomic system in the workspace, as the computer simulations carried out in Sect. 5 have shown. A case of state-dependent control constraints can be tackled by introducing a new control vector \(\omega \) such that \(\dot{v}=\omega \) for \(v_l(\chi ) \le v \le v_u(\chi )\), where \(v_l(\chi )\) and \(v_u(\chi )\) are state-dependent lower and upper limits imposed on vector v, respectively. Thus, a modified control problem is obtained with a new trajectory \((\chi ,\ v)\) and control \(\omega \). It is important to note that the method presented here does not call for the knowledge of an initial solution satisfying constraints (3) and (4). It is only required that admissible control should satisfy relations (2). Numerical simulations confirm the theoretical results obtained in Sects. 3 and 4. The problem formulation and given approach may be directly applicable to complex multiple non-holonomic systems interacting in a workspace with obstacles. This method is also useful in finding admissible trajectories (in the sense of satisfying state constraints (3) and (4)), which is often encountered in practice.