15.5.2.1 Steering the nonholonomic integrator

The main idea of the method can be clearly illustrated for the nonholonomic integrator,

$$\begin{split}{\dot x}_1 & = u_1 \\ {\dot x}_2 & = u_2 \\ {\dot x}_3 & = x_1 u_2 - x_2 u_1 , \end{split}$$ (15.146)

which was considered throughout Section 15.5.1. This case will be explained in detail, and the methods obtained by generalizing the principles will subsequently be stated. The presentation given here is based on [727,846].

As was previously indicated, growing independent vector fields as quickly as possible is important. For the nonholonomic integrator, $[h_1,h_2]$, is linearly independent of $h_1$ and $h_2$, as observed in Example 15.12; thus, it satisfies this property. Consider steering the system from some ${x_{I}}= x(0)$ to some ${x_{G}} = x(1)$ while optimizing the cost functional

$$\int_0^1 \big( u_1(t)^2 + u_2(t)^2 \big) dt .$$ (15.147)

The problem can be solved by using the constrained Lagrangian formulation, which was given in Section 13.4.3. The first step is to eliminate the $u$ variables. From (15.146), the cost can be expressed in terms of ${\dot x}_1$ and ${\dot x}_2$ by using ${\dot x}_1 = u_1$ and ${\dot x}_2 = u_2$. The third equation in (15.146) can be written as

$${\dot x}_3 = x_1 {\dot x}_2 - x_2 {\dot x}_1$$ (15.148)

and will be interpreted as a constraint on the Lagrangian, which is combined using a (scalar) Lagrange multiplier as explained in Section 13.4.3. Define the Lagrangian as

$$L(x,{\dot x}) = ({\dot x}_1^2 + {\dot x}_2^2) + \lambda \big({\dot x}_3 - x_1 {\dot x}_2 + x_2 {\dot x}_1 \big),$$ (15.149)

in which the first term comes from the integrand of (15.147), and the second term comes from (15.148).

The Euler-Lagrange equation (13.118) yields

$$\begin{split}{\ddot x}_1 + \lambda {\dot x}_2 & = 0 \\ {\ddot... ...lambda {\dot x}_1 & = 0 \\ {\dot \lambda}& = 0 . \\ \end{split}$$ (15.150)

Note that ${\dot \lambda}= 0$ implies that $\lambda(t)$ is constant for all time. To obtain a differential equation that characterizes the optimal action trajectory, use the fact that for $i = 1, 2$, ${\dot x}_i = u_i$ and ${\ddot x}_i = {\dot u}_i$. This yields the equations ${\dot u}_1 = - \lambda {\dot u}_2$ and ${\dot u}_2 = \lambda {\dot u}_1$. These can be represented as second-order linear differential equations. Based on its roots, the solution is

$$\begin{split}u_1(t) & = u_1(0) \cos\lambda t - u_2(0) \sin\la... ... = u_1(0) \sin\lambda t + u_2(0) \cos\lambda t . \\ \end{split}$$ (15.151)

Given initial and goal states, the optimal action trajectory is found by determining $u_1(0)$, $u_2(0)$, and $\lambda$. Suppose that ${x_{I}}= x(0) = (0,0,0)$ and ${x_{G}}= x(1) = (0,0,a)$ for some $a \in {\mathbb{R}}$. Other cases can be obtained by applying transformations in $SE(3)$ to the solution.

The state trajectories for $x_1$ and $x_2$ can be obtained by integration of (15.151) because $u_i = {\dot x}_i$ for $i=1$ and $i=2$. Starting from $x_1(0) = x_2(0) = 0$, this yields

$$\begin{split}x_1(t) & = \frac{1}{\lambda} \big(u_1(0) \sin\la... ...cos\lambda t + u_2(0) \sin\lambda t + u_1(0)\big) . \end{split}$$ (15.152)

To maintain the constraint that $x_1(1) = x_2(1) = 0$, $\lambda$ must be chosen as $\lambda = 2 k \pi$ for some integer $n$. Integration of ${\dot x}_3$ yields

$$x_3(t) = \int_0^1 \big(x_1 u_2 - x_2 u_1\big) dt = \frac{1}{\lambda} \big(u_1^2(0) + u_2^2(0)\big) = a.$$ (15.153)

The cost is

$$\int_0^1 \big(u_1^2(t) + u^2_2(t)\big) dt = u_1^2(0) + u_2^2(0) = \lambda a .$$ (15.154)

The minimum cost is therefore achieved for $k = -1$, which yields $\lambda = 2 \pi$ and $\Vert u\Vert = 2 \pi a$. This fixes the magnitude of $u(0)$, but any direction may be chosen.

The steering problem can be solved in two phases:

1. Apply any action trajectory to steer $x_1$ and $x_2$ to their desired values while neglecting to consider $x_3$.
2. Apply the solution just developed to steer $x_3$ to the goal while $x_1$ and $x_2$ return to their values obtained in the first phase.

This idea can be generalized to other systems.