Smooth Cubic Polynomial Trajectories for Human-Robot Interactions

Abstract

With the growing importance of Human-Robot Interaction (HRI), the movement of the robots requires more and more attention to address the issues related to safety, efficiency and ergonomics. Trajectories are excellent candidates in the making of desirable motions designed for collaborative robots, because they allow to simply and precisely describe the motions. Despite the large number of works available for Online Trajectory Generation (OTG), there was, to our knowledge, no complete solution capable to simultaneously meet all the requirements of these new applications. In this paper we present a complete trajectory generation algorithm that build trajectories from arbitrary initial and final conditions, subject to general asymmetric bounds on jerk, acceleration and velocity. A review of the state of the art exposes the limits of the previous OTG works and reveals the difficult problem of non-linearity related with short motions. We explain how these non-linearities introduce discontinuities and we propose a solution based on sequences of segment of third degree polynomial functions.

Appendices

Appendix A: Analytic Expression of the Parametric Curve

$\mathcal {C}(x_{f}(t_{n}), t_{f}(t_{n}))$

We consider here the particular case where the trajectory is defined by:

• Three non null jerk segments associated to parabolas in the phase diagram.

• The first and the last segments are associated to $J_{min}<0.$.

• The third segment begin with a positive acceleration $a_{2}$.

The first trajectory segment is entirely defined by the initial conditions $a_{i}$ and $v_{i}$ and the duration $t_{1}$ using the equations (3). The parabola associated to the second segment cross the abscissa axis at $v_{i}^{\prime }$ and the one associated to the third segment at $v_{f}^{\prime }$ with:

$$\begin{array}{@{}rcl@{}} v_{i}^{\prime} = v_{i} - \frac{{a_{i}}^{2}}{2 J_{max}} v_{f}^{\prime} = v_{f} - \frac{{a_{f}}^{2}}{2 J_{min}} \end{array} $$

And the acceleration at the intersection point of the two parabolas is $a_{2}$ defined by:

$$\begin{array}{@{}rcl@{}} a_{2} = \sqrt{ \frac{ 2(v_{i}^{\prime}-v_{f}^{\prime}) \times J_{max}\times J_{min}}{J_{max}-J_{min}} } \end{array} $$

We choose the positive solution as defined in the hypothesis and compute the durations $t_{2}$ and $t_{3}$ of the two last segments.

$$\begin{array}{@{}rcl@{}} t_{2} = \frac{(a_{2} - a_{1})}{ J_{max}} t_{3} = \frac{(a_{f} - a_{2})}{J_{min}} \end{array} $$

From the Eqs. 4 and 5 and using an algebraic calculator, the values of $x_{f}$ and $t_{f} = t_{1}{+}t_{2}{+}t_{3}$ can be easily computed. Then the zeros of the derivative of the $x_{f}$ function with respect to $t_{1}$ would define the points of discontinuity of the optimal curve $C_{opt}$. Unfortunately, we cannot manage to obtain these points in a usable form.

Appendix B: Solving the Three Segments Problems

1.1 Solving the AJA Problem

In this case the jerk $J_{a}$ of the first and last segments is null in the Eqs. 3 and 5 that respectively become Eqs. 6 and 7:

$$\begin{array}{@{}rcl@{}} a_{1} &=& \qquad a_{i} \\ v_{1} &=& \qquad a_{i}\times{t_{1}} {+} v_{i} \\ x_{1} &=& \qquad a_{i}\times{t_{1}}^{2}/2 \qquad + v_{i}{\times}t_{1} \end{array} $$

(6)

$$\begin{array}{@{}rcl@{}} a_{f} &=& \qquad a_{2} \\ v_{f} &= & \qquad a_{2}{\times}{t_{3}} {+} v_{2} \\ x_{f} &=& \qquad a_{2}{\times}{t_{3}}^{2}/2 {+} v_{2}{\times}t_{3}\qquad {+} x_{2} \end{array} $$

(7)

Using an algebra system, we obtain the results:

$$\begin{array}{@{}rcl@{}} k_{1} &=& a_{i}{-}a_{f} \\ k_{2} &=& {k_{1}}^{-1} \\ k_{3} &=& a_{i}{\times}(({-}J_{a}{\times}v_{i}){+}{a_{f}}^{2}/2{+}a_{i}{\times}({-}a_{f}{+}a_{i}/2)) \\ k_{4} &=& a_{f}{\times}J_{a}{\times}v_{i} \\ k_{5} &=& 12{\times}{J_{a}}^{2}{\times}{v_{i}}^{2} \\ k_{6} &=& a_{f}{-}a_{i} \\ k_{7} &=& (3{\times}(12{\times}a_{i}{\times}{J_{a}}^{2}{\times}(2{\times}a_{f}{\times}k_{6}{\times}x_{f}{+}k_{1}{\times}{v_{f}}^{2}) \\ &&{+}a_{f}{\times}(a_{i}{\times}(a_{i}{\times}(4{\times}{a_{f}}^{3}{+}a_{i}{\times}(a_{i}{\times}(4{\times}a_{f}{-}a_{i}) \\ &&{-}6{\times}{a_{f}}^{2})){-}(k_{5}{+}{a_{f}^{4}})){+}12{\times}a_{f}{\times}{J_{a}}^{2}{\times}{v_{i}}^{2})))^{0.5}\\ k_{8} &=& k_{6}{\times}{J_{a}}^{-1}\\ k_{9} &=& k_{1}{\times}a_{f} \\ k_{10} &=& 2{\times}J_{a}{\times}v_{f} \\ k_{11} &=& 1/\sqrt{3} \\ k_{12} &=& (k_{6}{\times}(12{\times}a_{i}{\times}{J_{a}}^{2}{\times}(2{\times}a_{f}{\times}x_{f}{-}{v_{f}}^{2}) \\ &&{+}a_{f}{\times}(k_{5}{+}a_{i}{\times}(a_{i}{\times}(3{\times}{a_{f}}^{2}{+}a_{i}{\times}(a_{i}{-}3{\times}a_{f})) \\ &&{-}{a_{f}}^{3}))))^{0.5} \end{array} $$

And finally the system has two solutions (t₁, $t_{2}$, $t_{3}$) and ($t_{1}^{\prime }$, $t_{2}$, $t_{3}^{\prime }$) with:

$$\begin{array}{@{}rcl@{}} t_{1} &=& {a_{i}}^{-1}{\times}k_{2}{\times}{J_{a}}^{-1}{\times}(({-}k_{7}/6){+}k_{4}{+}k_{3}) \\ t_{2} &=& k_{8} \end{array} $$

$$\begin{array}{@{}rcl@{}} t_{3} &=& ({a_{f}}^{-1}{\times}{J_{a}}^{-1}{\times}(k_{11}{\times}k_{2}{\times}k_{12}{+}k_{10}{+}k_{9}))/2 \\ t_{1}^{\prime} &=& {a_{i}}^{-1}{\times}k_{2}{\times}{J_{a}}^{-1}{\times}(k_{7}/6{+}k_{4}{+}k_{3}) \\ t_{3}^{\prime} &=& {a_{f}}^{-1}\times {J_{a}}^{-1}\times ((k_{10}{+}k_{9})/2{-}(k_{11}\times k_{2}\times k_{12})/2) \end{array} $$

1.2 Solving the JJA Problem

The system of polynomial equations to solve is defined by Eqs. 6, 4 and 5. Using an algebra system, we obtain the following result where $r_{i}$ is one solution of the quartic equation defined by the coefficients $c_{i}$ with $0\leq i \leq 4$:

$$\begin{array}{@{}rcl@{}} k_{1} &=& 1 {-} 2 {\times} a_{f} \\ k_{2} &=& 2 {\times} a_{f} \\ k_{3} &=& {a_{f}}^{2} {\times} J_{b} {+} k_{1} {\times} J_{a} \\ c_{4} &=& 3 {\times} {a_{f}}^{4} {\times} {J_{b}}^{4} \\ &&{+} J_{a} {\times} (2 {\times} k_{1} {\times} {a_{f}}^{2} {\times} J_{b} {+} (4 {\times} (a_{f} {-} 1 ) {\times} a_{f} {+} 1 ) {\times} J_{a} ) \!\!\left.\right) \\ c_{3} &=& 4 {\times} a_{f} {\times} J_{b} {\times} \left( \right.\!\! {a_{f}}^{3} {\times} {J_{b}}^{2} \\ &&{+} J_{a} {\times} ((3 {\times} a_{f} {-} 1 ) {\times} J_{a} {-} 3 {\times} {a_{f}}^{2} {\times} J_{b} ) \!\!\left.\right) \\ c_{2} &=& 6 {\times} J_{b} {\times} (2 {\times} J_{a} {\times} k_{3} {\times} v_{i} {+} ((k_{2} {-} 1 ) {\times} J_{a} {-} {a_{f}}^{2} {\times} J_{b} ) {\times} {a_{i}}^{2} ) \\ c_{1} &=& 12 {\times} a_{f} {\times} (2 {\times} J_{a} {\times} (a_{f} {\times} J_{b} {-} J_{a} ) {\times} v_{i} {+} (J_{a} {-} a_{f} {\times} J_{b} ) {\times} {a_{i}}^{2} ) \\ c_{0} &=& 3 {\times} (4 {\times} {J_{a}}^{2} {\times} {v_{i}}^{2} {+} a_{i} {\times} (4 {\times} J_{a} {\times} (k_{2} {-} a_{i} ) {\times} v_{i} {+} {a_{i}}^{3} ) ) \\ &&{+} 4 {\times} (3 {\times} {J_{a}}^{2} {\times} (2 {\times} a_{f} {\times} x_{f} {-} {v_{f}}^{2} ) {-} 2 {\times} a_{f} {\times} {a_{i}}^{3} ) \\ t1 &=& {-}{J_{a}}^{-1} {\times} (a_{i} {+} a_{f} {\times} J_{b} {\times} r_{i} ) \\ t2 &=& r_{i} \\ t3 &=\phantom{0}& {-}\frac{{Ja}^{-1} {\times} (2 {\times} Ja {\times} v_{i} - ({a_{i}}^{2} {+} 2 {\times} Ja {\times} v_{f} ) {+} J_{b} {\times} k_{3} {\times} {r_{i}}^{2} ) }{ 2 {\times} a_{f} } \end{array} $$

1.3 Solving the AJJ Problem

An AJJ system can be solved similarly as a JJA one or using a symmetry with respect to the acceleration axis to build an equivalent JJA system.

Appendix C: Derivative of the Cubic Polynomial Functions

The derivatives of the functions $x_{f}, v_{f}$ and $a_{f}$ relatively to the three times $t_{1}, t_{2}$ and $t_{3}$ can be grouped in a matrix. The newton method uses the inverse of this matrix to compute the times increments that, at the first oder, compensate the errors of the functions.

$$\begin{array}{@{}rcl@{}} \left( \begin{array}{ccc} {\frac{\partial x_{f}}{\partial t_{1}}} & \frac{\partial x_{f}}{\partial t_{2}} & \frac{\partial x_{f}}{\partial t_{3}} \\ \frac{\partial v_{f}}{\partial t_{1}} & \frac{\partial v_{f}}{\partial t_{2}} & \frac{\partial v_{f}}{\partial t_{3}} \\ \frac{\partial a_{f}}{\partial t_{1}} & \frac{\partial a_{f}}{\partial t_{2}} & \frac{\partial a_{f}}{\partial t_{3}} \end{array} \right) = \left( \begin{array}{ccc} lx_{1} & lv_{1} & la_{1} \\ lx_{2} & lv_{2} & la_{2} \\ lx_{3} & lv_{3} & la_{3} \end{array} \right)^{-1} \end{array} $$

In the JJJ case defined by the Eqs. 3, 4 and 5, using an algebraic calculator we obtain:

$$\begin{array}{@{}rcl@{}} k_{1} &=& J_{a}{\times}t_{1} \\ k_{2} &= &J_{b}{\times}t_{2} \\ k_{3} &=& J_{a}{\times}t_{3} \\ k_{4} &=& a_{i}{+}k_{3}{+}k_{2}{+}k_{1} \\ k_{5} &=& a_{i}{+}J_{b}{\times}(t_{3}{+}t_{2}){+}k_{1} \\ k_{6} &=& J_{a}{\times}k_{5}{-}J_{b}{\times}k_{4} \\ k_{7} &=& {t_{1}}^{2} \\ k_{8} &=& {t_{2}}^{2} \\ k_{9} &=& {t_{3}}^{2} \\ k_{10} &=& (J_{a}{\times}(k_{9}{+}k_{8}{+}k_{7}))/2 \\ k_{11} &=& t_{1}{\times}a_{i} \\ k_{12} &=& t_{2}{\times}(a_{i}{+}k_{1}) \\ k_{13} &=& t_{3}{\times}(a_{i}{+}J_{a}{\times}(t_{2}{+}t_{1})) \\ k_{14} &=& v_{i}{+}k_{13}{+}k_{12}{+}k_{11}{+}k_{10} \\ k_{15} &=& a_{i}{+}J_{a}{\times}(t_{3}{+}t_{2}{+}t_{1}) \\ k_{16} &=& J_{b}{\times}k_{15}{-}J_{a}{\times}k_{5} \\ k_{17} &=& J_{a}{\times}k_{7} \\ k_{18} &=& J_{a}{\times}k_{9}{+}J_{b}{\times}k_{8}{+}k_{17} \\ k_{19} &=& t_{3}{\times}(a_{i}{+}k_{2}{+}k_{1}) \\ k_{20} &=& v_{i}{+}k_{19}{+}k_{12}{+}k_{11}{+}k_{18}/2 \\ k_{21} &=& ({-}k_{15}){+}a_{i}{+}k_{3}{+}k_{2}{+}k_{1} \\ k_{22} &=& v_{i}{+}k_{19}{+}k_{12}{+}k_{11}{+}(J_{b}{\times}(k_{9}{+}k_{8}){+}k_{17})/2 \\ k_{23} &=& 1/(J_{a}{\times}k_{21}{\times}k_{22}{+}k_{16}{\times}k_{20}{+}k_{6}{\times}k_{14}) \\ lx_{1} &=& k_{6}{\times}k_{23} \\ lv_{1} &=& (J_{b}{\times}k_{20}{-}J_{a}{\times}k_{22}){\times}k_{23} \\ la_{1} &=& (k_{4}{\times}k_{22}{-}k_{5}{\times}k_{20}){\times}k_{23}\!\!\left.\right] \\ lx_{2} &=& J_{a}{\times}k_{21}{\times}k_{23} \\ lv_{2} &=& J_{a}{\times}({-}(k_{19}{+}k_{12}{+}k_{11} {-}({-}k_{18}/2))\\ &&{+}k_{13}{+}k_{12}{+}k_{11}{+}k_{10}){\times}k_{23} \\ la_{2} &=& (k_{15}{\times}k_{20}{-}k_{4}{\times}k_{14}){\times}k_{23}\!\!\left.\right] \\ lx_{3} &=& k_{16}{\times}k_{23} \\ lv_{3} &=& (J_{a}{\times}k_{22}{-}J_{b}{\times}k_{14}){\times}k_{23} \\ la_{3} &=& (k_{5}{\times}k_{14}{-}k_{15}{\times}k_{22}){\times}k_{23}\!\!\left.\right])) \end{array} $$

The computation is done similarly in the case of AJA, AJJ and JJA sequences.