Path Planning

Abstract

A cubic path in joint space for the joint variable qi(t) between two points q _i (t ₀) and q _i (t _f ) is

$$ q_i (t) = a_0 + a_1 t + a_2 t^2 + a_3 t^3 $$

(13.1)

where

$$ \begin{gathered} a_0 = - \frac{{q_1 \left( {t_0^3 - 3t_0^2 t_f } \right) + q_0 \left( {3t_0 t_f^2 - t_f^3 } \right)}} {{\left( {t_f - t_0 } \right)^3 }} \hfill \\ - \frac{{q'_0 \left( {t_0 t_f^3 - t_0^2 t_f^2 } \right) + q'_1 \left( {t_0^2 t_f^2 - t_0^3 t_f } \right)}} {{\left( {t_f - t_0 } \right)^3 }} \hfill \\ \end{gathered} $$

(13.2)

and

$$ \begin{gathered} a_1 = \frac{{6q_0 t_0 t_f - 6q_1 t_0 t_f }} {{\left( {t_f - t_0 } \right)^3 }} \hfill \\ + \frac{{q'_0 \left( {t_f^3 + t_0 t_f^2 - 2t_0^2 t_f } \right) + q'_1 \left( {2t_0 t_f^2 - t_0^3 - t_0^2 t_f } \right)}} {{\left( {t_f - t_0 } \right)}} \hfill \\ \end{gathered} $$

(13.3)

$$ \begin{gathered} a_2 = - \frac{{q_0 \left( {3t_0 + 3t_f } \right) + q_1 \left( { - 3t_0 - 3t_f } \right)}} {{\left( {t_f - t_0 } \right)^3 }} \hfill \\ - \frac{{q'_1 \left( {t_0 t_f - 2t_0^2 + t_f^2 } \right) + q'_0 \left( {2t_f^2 - t_0^2 - t_0 t_f } \right)}} {{\left( {t_f - t_0 } \right)^3 }} \hfill \\ \end{gathered} $$

(13.4)

$$ a_3 = \frac{{2q_0 - 2q_1 + q'_0 \left( {t_f - t_0 } \right) + q'_1 \left( {t_f - t_0 } \right)}} {{\left( {t_f - t_0 } \right)^3 }} $$

(13.5)

and

$$ \begin{gathered} q_i \left( {t_0 } \right) = q_0 \hfill \\ \dot q_i \left( {t_0 } \right) = q'_0 \hfill \\ q_i \left( {t_f } \right) = q_f \hfill \\ \dot q_i \left( {t_f } \right) = q'_f . \hfill \\ \end{gathered} $$

(13.6)