Motion planning of a mobile robot with a trailer via nilpotent approximation

We consider a two-driving wheel mobile robot with a trailer on a plane. Possible positions are given by $q = (x,y,\theta, \varphi) \in M$ , where $(x,y) \in \mathbb{R}^2$ is a midpoint of the robot and $\theta, \varphi \in S^1$ are angles of orientation of the robot and the trailer. The kinematic model reads as
$\dot{x} = u_1 \cos \theta, \quad \dot{y} = u_1 \sin \theta, \quad \dot{\theta} = u_2, \quad \dot{\varphi} = - u_1 \frac{\sin \varphi}{l_t} - u_2 \left(\frac {l_r \cos \varphi}{l_t} + 1\right), \quad \operatorname{(1)}�$
where $u_1$ and $u_2$ are, respectively, linear and angular velocities of the robot as controls; $l_t$ and $l_r$ are constants defining the geometry of the hooking up system. The motion planning problem is to find controls $u_1(t)$ , $u_2(t)$ that steer (1) from a given initial configuration $q_0 \in M$ to a given final one $q_1 \in M$ , i.e., to find a path $q(t)$ , s.t.
$�q (0)= q_0=(x_0, y_0, \theta_0, \varphi_0), \qquad q (t_1) = q_1=(x_1, y_1, \theta_1, \varphi_1). \quad \operatorname{(2)}$
The method of nipotent approximation is used. The corresponding nipotent problem is a sub-Riemannian (SR) problem on Engel group. We propose an iterative scheme to solve the motion planning problem (1)-(2), where on each iteration the optimal controls for the nilpotent system are applied to (2). The method converges if $q_0$ and $q_1$ are close enough. Moreover, the obtained trajectory is close to optimal in terms of $\int_0^{t_1} \sqrt{u_1^2 + \alpha^2 u_2^2} \ d t \to \min$ , with $\alpha>0$ .