Control of a Mobile Robot with a Trailer Based on Nilpotent Approximation

Abstract

We consider a kinematic model of a mobile robot with a trailer moving on a homogeneous plane. The robot can move back and forth and make a pivot turn. For this model, we pose the following optimal control problem: transfer the “robot–trailer” system from an arbitrarily given initial configuration into an arbitrarily given final configuration so that the amount of maneuvering is minimal. By a maneuver we mean a functional that defines a trade-off between the linear and angular robot motion. Depending on the trailer–robot coupling, this problem corresponds to a two-parameter family of optimal control problems in the 4-dimensional space with a 2-dimensional control.

We propose a nilpotent approximation method for the approximate solution of the problem. A number of iterative algorithms and programs have been developed that successfully solve the posed problem in the ideal case, namely, with no state constraints. Based on these algorithms, we propose a dedicated reparking algorithm that solves a particular case of the problem where the initial and final robot position coincide and takes into account a state constraint on the trailer’s turning angle occurring in real systems.

APPENDIX

Let $M$ be a smooth manifold of dimension $\dim {M} = n$.

Denote by $T_q M$ the tangent space to $M$ at a point $q \in M$.

Suppose that on $M$ we are given a family $\mathcal {F} = \{X_1, X_2\}$ of two smooth vector fields $X_1$, $X_2 \in \operatorname {Vec}\left (M\right )$ satisfying the full rank conditions

$$ \operatorname {Lie}_q \mathcal {F} = T_q M, \quad \forall q \in M, $$

where $\operatorname {Lie}_q \mathcal {F}$ denotes the Lie algebra generated by the system $\mathcal {F}$ at the point $q$,

$$ \operatorname {Lie}_q \mathcal {F} = \operatorname {span}\left (X_1(q), X_2(q), [X_1, X_2](q), \ldots , [X_i,[\ldots ,[X_1, X_2]\ldots ]](q) \; | \; X_i \in \mathcal {F}\right ). $$

Here the brackets designate the commutator (Lie bracket) of vector fields,

$$ [X_1, X_2](q) = \frac {d}{d t} \Big |_{t=0} \left (e^{-\sqrt {t}X_2} \circ e^{-\sqrt {t}X_1} \circ e^{\sqrt {t}X_2} \circ e^{\sqrt {t}X_1}(q)\right ) \in \operatorname {Vec}(M),$$

where $e^{t X_i}(q)$ stands for the flow of the vector field $X_i \in \operatorname {Vec}\left (M\right )$ from the point $q \in M$ in time $t$, i.e., the solution of the Cauchy problem $\dot {\gamma }(t) = X_i(\gamma (t))$, $\gamma (0) = q$.

By ${\mathbf {L}}^s(q),\thinspace s \in \mathbb {N}$, we denote the vector spaces generated by the values of the Lie brackets of the fields $X_1, X_2$ of length $\leq s$ at the point $q$ (the fields $X_i$ themselves are brackets of length 1):

$$ \begin {aligned} {\mathbf {L}}^1(q) = & \ {\operatorname {span}\nolimits }\big (X_1(q),X_2(q)\big ),\\ {\mathbf {L}}^2(q) = & \ {\operatorname {span}\nolimits }\big ({\mathbf {L}}^1(q) + [{\mathbf {L}}_1,{\mathbf {L}}_1](q)\big ),\\ &\qquad \ldots \ldots \ldots \ldots \ldots \\ {\mathbf {L}}^s(q) = & \ {\operatorname {span}\nolimits }\left ({\mathbf {L}}^{s-1}(q) +[{\mathbf {L}}^1, {\mathbf {L}}^{s-1}](q)\right ). \end {aligned}$$

The full rank condition guarantees that for any point $q \in M$ there exists a least integer $r = r(q)$ such that $\dim {\mathbf {L}}^r(q)=n$. In other words, the system $\mathcal {F}$ defines a distribution in the tangent space with the flag

$$ {\mathbf {L}}^1 (q)\subseteq {\mathbf {L}}^2 (q) \subseteq \dots \subseteq {\mathbf {L}}^{r-1}(q) \subset {\mathbf {L}}^r (q) = T_q M.$$

(A.1)

Definition 1.

A growth vector of the system $\mathcal {F}$ at a point $q$ is the vector

$$ \big ( \dim {\mathbf {L}}^1(q),\dots , \dim {\mathbf {L}}^r(q)\big ).$$

Fix the dimension $\dim M = 4$ and consider the control system

$$ \dot {q} = {\mathbf {u}}_1 X_1\big (q\big ) + {\mathbf {u}}_2 X_2\big (q\big ),$$

(A.2)

where the trajectory $q = q(t) \in M$, $t \geq 0$, is a piecewise smooth curve, the controls ${\mathbf {u}}_1$, ${\mathbf {u}}_2$ are real-valued piecewise continuous functions, and the smooth vector fields $X_1$, $X_2 \in \operatorname {Vec}\left (M\right )$ form a system with growth vector $(2,3,4)$,

$$ {\operatorname {span}\nolimits }(X_1(q), X_2(q), [X_1,X_2](q), [X_1, [X_1,X_2]](q)) = T_q M, \quad \forall q \in M.$$

Next, for system (A.2) we will describe the construction of the nilpotent approximation—in a certain sense, the simplest system with the growth vector $(2,3,4)$ —whose trajectories locally approximate the trajectories of the original system. Saying “the simplest,” we mean the following property: the vector fields of the approximate system form a nilpotent Lie algebra in which all Lie brackets are zeros starting from the third order. Such an approximate system is the easiest to construct in special coordinates describing the motion of the system in the directions of the kernel vector fields and their commutators, the so-called privileged coordinates. Before describing the construction itself, we introduce some definitions; see [27] for details.

Definition 2.

A change of coordinates for system (A.2) is a diffeomorphism $\sigma : M \to M : q \mapsto \sigma (q)$. The differential of this change will be denoted by $\sigma _*:T_q M \to T_{\sigma (q)}M : X_i \mapsto \sigma _*(X_i)$, $i= 1,\dots ,4$.

Definition 3.

For system (A.2), the order of the differential operator $X$ at the point $q^0$ is the minimum number $s \in {\mathbb {N}}$ such that for any function $\sigma$ having the order $p = \min \big \{p \in {\mathbb {N}} \mid X_{k_1} \ldots X_{k_p} (\sigma )(q^0) = 0,\ k_j \in \{1,2\} \big \}$, all derivatives of order $s+p$ along the fields $X_1$, $X_2$ of $X(\sigma )$ are zero at this point,

$$ X_{k_1} \ldots X_{k_{s+p}} X(\sigma )(q^0) = 0, \quad k_j \in \{1,2\}.$$

Definition 4.

The system of local coordinates $\tilde {q} = (\tilde {q}_1,\ldots , \tilde {q}_4)$ on $M$ with the center at a point $q^0$, defined by the change $\big ({\tilde {q}}_1(q), \ldots , {\tilde {q}}_4(q)\big ) = \big (\sigma _1(q), \ldots , \sigma _4(q)\big )$, is said to be linearly adapted at the point $q^0$ if the differentials $\operatorname {d}\nolimits {\tilde {q}}_1, \ldots , \operatorname {d}\nolimits {\tilde {q}}_4$ form a basis of $T^*_{q^0}M$ adapted to the flag $\{0\} = {\mathbf {L}}^0(q^0)\subset {\mathbf {L}}^1(q^0)\subset {\mathbf {L}}^2(q^0) \subset {\mathbf {L}}^3(q^0)$; i.e., ${\mathbf {L}}^i(q^0) = {\operatorname {span}\nolimits }(\frac {\partial }{\partial {{\tilde {q}}_1}}|_{q^0},\ldots ,\frac {\partial }{\partial {{\tilde {q}}_i}}|_{q^0}), \ i=1,2,3$. In this case, the order of the coordinate $\tilde {q}_i$ at the point $q^0$ is the minimum number $p\in {\mathbb {N}}$ such that all derivatives of order $p$ along the fields $X_{k_j}$ of $\sigma _i$ are zero at this point, $X_{k_1} \ldots X_{k_p} (\sigma _i)(q^0) = 0$, $k_j \in \{1,2\}$, where $X_{k_j}(f) = \langle \nabla f,X_{k_j} \rangle$ denotes the derivative of the function $f$ in the direction of the field $X_{k_j}$, the operation $\langle ,\rangle$ is the inner product, and $\nabla$ is the operation of taking the gradient.

Definition 5.

For system (A.2) written in linearly adapted coordinates $\tilde {q}$, the weight of the coordinate $\tilde {q}_i$ is the least number $\omega _i \in {\mathbb {N}}$ such that ${\mathbf {L}}^{\omega _i}(q^0)$ does not vanish identically.

Definition 6.

The system of local coordinates $\tilde {q} = (\tilde {q}_1,\ldots , \tilde {q}_4)$ centered at a point $q^0$ is called privileged for system (A.2) if

• – $(\tilde {q}_1,\ldots , \tilde {q}_4)$ are linearly adapted at the point $q^0$.

• – The order of the coordinate $\tilde {q}_i$ at the point $q^0$ equals the weight $\omega _i$.

Now that we have all the definitions needed, let us describe the construction of the nilpotent approximation. The nilpotent approximation for system (A.2) is constructed in the space ${\mathbb {R}}^4$ in the following manner:

• 1. System (A.2) is written in the privileged coordinates $\tilde {q}$,

  $$ \dot {{\tilde {q}}} = {\mathbf {u}}_1 X_1({\tilde {q}}) + {\mathbf {u}}_2 X_2({\tilde {q}}), \quad {\tilde {q}} \in M, \thinspace ({\mathbf {u}}_1,{\mathbf {u}}_2)\in {\mathbb {R}}^2.$$

  (A.3)

• 2. The vector fields $X_i({\tilde {q}})$ are expanded in a Maclaurin series with the subsequent grouping of terms of the same order,

  $$ X_i({\tilde {q}}) = X_i^{(-1)}({\tilde {q}}) + X_i^{(0)}({\tilde {q}}) + X_i^{(1)}({\tilde {q}})+ X_i^{(2)}({\tilde {q}}) + \ldots .$$

• 3. Terms starting with the zero order are dropped, and the remaining terms of order $-1$ form the kernel vector fields $\widehat {X}_i({\tilde {q}}) = X_i^{(-1)}({\tilde {q}})$ of the approximate system—the nilpotent approximation

  $$ \dot {{\hat {q}}} = {\mathbf {u}}_1 \widehat {X}_1({\hat {q}}) + {\mathbf {u}}_2 \widehat {X}_2({\hat {q}}), \quad {\hat {q}} \in {\mathbb {R}}^4, \thinspace ({\mathbf {u}}_1,{\mathbf {u}}_2)\in {\mathbb {R}}^2. $$

  (A.4)

The nilpotent approximation (A.4) for the original system (A.3) possesses the following key properties:

• 1. All commutators of order $\geq 3$ of the vector fields $\widehat {X}_1$, $\widehat {X}_2$ are zero.

• 2. The growth vector of system (A.4) is $(2,3,4)$.

• 3. Under the controls ${\mathbf {u}}_1(t)$ and ${\mathbf {u}}_2(t)$, the trajectory ${\hat {q}}(t)$ of system (A.4) locally (for small $t>0$) approximates the trajectory ${\tilde {q}}(t)$ of system (A.3).

Chapter 8 of Montgomery’s book [21] explains the relation between the original system and its nilpotentization (nilpotent approximation): it is given by the Gromov–Mitchell theorem (8.4.1). An estimate of the proximity of paths is given in Sec. 8.7. More details on the construction of the nilpotent approximation can be found in Bellaiche’s monograph [25]. Section 7 deals with estimates on distances; in particular, see Assertion 7.29 on the closeness of the trajectories of the original and approximating systems.

Remark 7.

In the general case of $\dim M = n$, for the original ${\tilde {q}}(t) = ({\tilde {q}}_1(t),...,{\tilde {q}}_n(t))$ and the approximating trajectory ${\hat {q}}(t) = ({\hat {q}}_1(t),...,{\hat {q}}_n(t))$ in privileged coordinates issuing from one point, one has the local estimate $|{\tilde {q}}_i(t) - {\hat {q}}_i(t)| \leq C t^{w_i+1}$, where $C$ is a constant and $w_i$ is the weight of the coordinate ${\tilde {q}}_i$ (the degree of nonholonomity in the direction ${\tilde {q}}_i$, which is calculated as the least depth of the flag of the distribution (A.1) that does not set to zero the $i$th direction).

In the present paper, for the trajectories ${\tilde {q}}(t)$ and ${\hat {q}}(t)$ of systems (A.3) and (A.4), one has the estimate

$$ |{\tilde {q}}_i(t) - {\hat {q}}_i(t)| \leq C t^{w_i+1}, \quad w = (1,1,2,3), $$

where $C$ is a constant defined by the form of the vector fields $X_i$ and the initial point $q^0$.