Planning non-holonomic stable trajectories on uneven terrain through non-linear time scaling

Abstract

In this paper, we present a framework for generating smooth and stable trajectories for wheeled mobile robots moving on uneven terrains. Instead of relying on static stability measures, the paper incorporates velocity and acceleration based constraints like no-slip and permanent wheel ground contact conditions in the planning framework. The paper solves this complicated problem in a computationally efficient manner with full 3D dynamics of the robot. The two major aspects of the proposed work are: Firstly, closed form functional relationships are derived which describes the 6 dof evolution of the robot’s state on an arbitrary 2.5D uneven terrain. This enables us to have a fast evaluation of robot’s dynamics along any candidate trajectory. Secondly, a novel concept of non-linear time scaling is introduced through which simple algebraic relations defining the bounds on velocities and accelerations are obtained. Moreover, non-linear time scaling also provides a new approach for manipulating velocities and accelerations along given geometric paths. It is thus exploited to obtain stable velocity and acceleration profiles. Extensive simulation results are presented to demonstrate the efficacy of the proposed methodology.

Appendices

Appendix 1

$$\begin{aligned} \eta _{i}= & {} k_{1}l_{i}\sin \alpha -k_{2}l_{i}\cos \alpha \end{aligned}$$

(59)

$$\begin{aligned} \nu _{i}= & {} k_{1}l_{i}\cos \alpha +k_{2}l_{i}\sin \alpha \end{aligned}$$

(60)

$$\begin{aligned} H_{1}= & {} k_{1}x-k_{1}w\sin \alpha -k_{1}h\cos \alpha \nonumber \\&+k_{2}y-k_{2}h\sin \alpha +k_{2}w\cos \alpha +k_{3}+l_{1} \end{aligned}$$

(61)

$$\begin{aligned} H_{2}= & {} k_{1}x+w(k_{2}\cos \alpha -k_{1}\sin \alpha )\nonumber \\&+h(k_{1}\cos \alpha +k_{2}\sin \alpha )+ k_{3}+l_{2}+k_{2}y \end{aligned}$$

(62)

$$\begin{aligned} H_{3}= & {} k_{1}x+w(k_{1}\sin \alpha -k_{2}\cos \alpha )\nonumber \\&+h(k_{1}\cos \alpha +k_{2}\sin \alpha ) +k_{3}+l_{3}+k_{2}y \end{aligned}$$

(63)

$$\begin{aligned} H_{4}= & {} k_{1}x+w(k_{1}\sin \alpha -k_{2}\sin \alpha )\nonumber \\&+h(-k_{1}\cos \alpha -k_{2}\sin \alpha )+ k_{3}+l_{4}+k_{2}y \end{aligned}$$

(64)

Appendix 2

Inverting the coefficient matrix in (18), and using the relations in (59–64) we get

$$\begin{aligned} \gamma= & {} k_2\cos \alpha -k_1\sin \alpha \end{aligned}$$

(65)

$$\begin{aligned} \beta= & {} -k_1\cos \alpha -k_2\sin \alpha \end{aligned}$$

(66)

Differentiating (65) twice we get

$$\begin{aligned}&\frac{d^2\gamma }{dt^2}= \frac{d^2k_2}{dy^2}(\frac{dy}{dt})^2\cos \alpha +\frac{dk_2}{dy}\frac{d^2y}{dt^2}\cos \alpha - \frac{dk_2}{dy}\frac{dy}{dt}\sin \alpha \frac{d\alpha }{dt}\nonumber \\&\quad +\frac{dk_2}{dy}\frac{dy}{dt}\sin \alpha \frac{d\alpha }{dt}\nonumber \\&\quad +k_2\cos \alpha (\frac{d\alpha }{dt})^2 -k_2\sin \alpha \frac{d^2\alpha }{dt^2}-\frac{d^2k_1}{dx^2}(\frac{dx}{dt})^2\sin \alpha \nonumber \\&\quad -\frac{dk_1}{dx}\frac{d^2x}{dt^2}\sin \alpha \nonumber \\&\quad - \frac{dk_1}{dx}\frac{dx}{dt}\cos \alpha \frac{d\alpha }{dt} -\frac{dk_1}{dx}\cos \alpha \frac{dx}{dt}\frac{d\alpha }{dt} +k_1\sin \alpha (\frac{d\alpha }{dt})^2\nonumber \\&\quad -k_1\cos \alpha \frac{d^2\alpha }{dt^2} \end{aligned}$$

(67)

By putting $\dot{x}(t)={x}^{'}(u)s_u,\ddot{x}(t)={x}^{''}(u)s_u^2$, $\dot{y}(t)={y}^{'}(u)s_u,\ddot{y}(t)={y}^{''}(u)s_u^2$, $\dot{\alpha }(t)={\alpha }^{'}(u)s_u,\ddot{\alpha }(t)={\alpha }^{''}(u)s_u^2$, as mentioned in Sect. 4 it can be shown that

$$\begin{aligned} \ddot{\gamma }(t)=\ddot{\gamma }(u)s_u^2 \end{aligned}$$

(68)

Similarly

$$\begin{aligned} \ddot{\beta }(t)=\ddot{\beta }(u)s_u^2 \end{aligned}$$

(69)

Using (68) and (69) it is straightforward to observe that . The derivation of $\ddot{z}(t)$ in terms of $\ddot{z}(u)$ proceeds along similar lines and is left to the reader.

Appendix 3

For deriving the equations of motion of the vehicle, the wheel ground contact normal and traction force unit vector needs to be calculated. Wheel ground contact normal can be calculated based on the wheel ground contact point information derived in Sect. 3 as:

$$\begin{aligned} \left[ \begin{array}{ccc} n_{xi}&n_{yi}&n_{zi} \end{array}\right] ^T =\left[ \begin{array}{c} {\frac{f_{x}}{\root 2 \of {f_{x}^2+f_{y}^2+f_{z}^2}}}\\ {\frac{f_{y}}{\root 2 \of {f_{x}^2+f_{y}^2+f_{z}^2}}}\\ -{\frac{1}{\root 2 \of {f_{x}^2+f_{y}^2+f_{z}^2}}} \end{array}\right] \end{aligned}$$

(70)

$f_{x}={\frac{\partial (a-f(b,c))}{\partial b}},b = x_{ci},c = y_{ci},a = z_{ci}$ $f_{y}={\frac{\partial (a-f(b,c))}{\partial c}},b = x_{ci},c = y_{ci},a = z_{ci}$ Once the unit normal vectors are calculated the traction force unit vector can be derived with the help of wheel axis unit vector which in our case has been taken as

$$\begin{aligned}&\hat{\mu }_{i} = R\left[ \begin{array}{ccc} 0&1&0 \end{array}\right] ^T \end{aligned}$$

(71)

$$\begin{aligned}&\hat{t}_{i}= {\frac{\hat{\mu }_{i}\times \hat{n}_{i}}{|(\hat{\mu }_{i}\times \hat{n}_{i})|}} \end{aligned}$$

(72)

With the above information the equations of motion can be written as

$$\begin{aligned}&\sum _{i=1}^4{N}_{i}\hat{n}_{i}+\sum _{i=1}^4{T_{i}}\hat{t}_{i}=\left[ \begin{array}{ccc} F_{x}&F_{y}&F_{z} \end{array}\right] ^T \end{aligned}$$

(73)

$$\begin{aligned}&\sum _{i=1}^4 r_i\times {N}_{i}\hat{n}_{i}+\sum _{i=1}^4 r_i\times {T_{i}}\hat{t}_{i}=\left[ \begin{array}{ccc} M_{x}&M_{y}&M_{z} \end{array}\right] ^T \end{aligned}$$

(74)

$$\begin{aligned}&r_{i} = \overrightarrow{P}_{gci} \end{aligned}$$

(75)

$$\begin{aligned}&F_{x}=ma_{x} \end{aligned}$$

(76)

$$\begin{aligned}&F_{y}=ma_{y} \end{aligned}$$

(77)

$$\begin{aligned}&F_{z} = ma_{z}+mg \end{aligned}$$

(78)

$$\begin{aligned}&M_{x}= I_{xx}{\dot{{\varOmega }}}_{x}+I_{zz}{\varOmega } _y{\varOmega } _z-I_{yy}{\varOmega } _y{\varOmega } _z \end{aligned}$$

(79)

$$\begin{aligned}&M_{y}= I_{yy}{\dot{{\varOmega }}}_{y}+I_{xx}{\varOmega } _x{\varOmega } _z-I_{zz}{\varOmega } _x{\varOmega } _z \end{aligned}$$

(80)

$$\begin{aligned}&M_{z}= I_{zz}{\dot{{\varOmega }}}_{z}+I_{yy}{\varOmega } _x{\varOmega } _y-I_{xx}{\varOmega } _x{\varOmega } _y \end{aligned}$$

(81)

$I_{xx}$, $I_{yy}$, $I_{zz}$ are the moment of inertia of the chassis and here a diagonal Inertia matrix has been taken.

Equations (73) and (74) can be written in the matrix form $A*C=D$ as mentioned in Sect. 3