Trajectory Basics IV: Re-Parametrization & Velocity Profiling

In the last article, we achieved the non-dimensionalization of the trajectory traversal, but cannot control tracker velocity with a constant step size \(\Delta u\). To get back the notion of velocity, the involvement of time dimension seems inevitable, but with the trajectory parameter \(u\) that represents the traversed distance, we will compute \(v\) differently. Let us first revisit the regular definition of a speed profile on a spline \(f\), parameterized by \(t\):

\[\begin{align}v(t) & = f^\prime(t) \\ L &= \int_{t_0}^{t_{\max}}\lvert v(\tau)\rvert d\tau\end{align}\]

If the trajectory is not a spline \(f(t)\), instead is a set of points represented by \(X(t)\) (0th order derivatives), the parameterization by \(t\) would look like

\[\begin{align}v(t) = \frac{dX}{dt} \end{align}\]

Now, we want to re-parameterize the trajectory by \(u\), so that we can assign a different velocity profile \(w(u) = \frac{dY}{du}\). The relation between these two set of parameterization is

\[\begin{equation}\frac{dX}{dt} = \frac{dY}{du}\frac{du}{dt} \end{equation}\]

Take a closer look at the magnitudes of the variables,

\[\begin{equation}\lvert\frac{dX}{dt}\rvert = \lvert\frac{dY}{du}\cdot\frac{du}{dt}\rvert=\lvert\frac{dY}{du}\rvert\cdot\lvert\frac{du}{dt}\rvert\end{equation}\]

Since \(Y\) and \(u\) are both parameterization over positions, \(\lvert\frac{dY}{du}\rvert = 1\). \(\frac{du}{dt}\) is a scalar, so we arrive at

\[\begin{align}\lvert\frac{dX}{dt}\rvert & = \frac{du}{dt} \\ u &= \phi(t) \end{align}\]

To solve for the relation between \(u\) and \(t\), take integral for both sides of the equation and get

\[\begin{equation}u = \phi(t) = \int_{t_0}^{t}\lvert X^\prime(\tau)\rvert d\tau\end{equation}\]

We can see from this equation, when \(t = t_0\), \(u = 0\), and when \(t = t_{\max}\), \(u = 1\). This passes the sanity check as at \(t_0\), traversed distance is 0, and at \(t_{\max}\), the full length is traversed. Intuitively speaking, on a v-t graph, this shows the relation \(\phi\) between \(t\)-axis and the area under the curve. The final velocity profile parameterized on \(u\) is

\[w(u) = v(\phi^{-1}(u))\]

where we need to find the inverse mapping from \(t\) to \(u\), in order to get the corresponding \(v(t)\) that is equivalent to speed at traversed distance \(u\). From this formulation, we can see that the time dimension appears only during the initial construction of \(v(t)\) profile. Unfortunately there is hardly any analytical solution to this relation mapping, but with parameters such as acceleration, max velocity, deceleration assigned for the profile, we can compute the traversed distance and find the corresponding velocity at that point. Assuming constant velocity within each loop interval \(t_i\), the adjusted \(\Delta u\) is then

\[\Delta u =\frac{w(u)t_i}{L}\]

Let’s walk through a concrete example of a trapezoidal speed profile, where the tracker accelerates, reaches and maintains at maximum speed (or not, depending on the acceleration magnitude and trajectory length), and decelerates.

Based on the limitations of the tracker, we assign motion profile to the tracker: \(v_{\max}\) for maximum velocity, \(a_0\) for maximum acceleration, and \(a_1\) for maximum deceleration (assuming magnitude only, so it is a positive scalar). \(L\) denotes the trajectory length, and for simplicity, we have \(t_0 = 0, t_e=t_{\max}\). To make the formulation more generic, we assign an initial velocity \(v_0\) and a final velocity \(v_e\). We need to check if, given acceleration bounds and velocity initial values, the maximum velocity is reachable. There are two cases for the tracker: 1. accelerates, reaches \(v_{\max}\) and maintains, then decelerates; 2. accelerates, then decelerates before reaching \(v_{\max}\), otherwise it would exceed \(L\). To figure out which case the tracker falls in, we need to first compute a theoretical \(\hat{v}_{\max}\), that is the highest velocity the tracker can reach given acceleration and deceleration bounds, within \(L\):

\[\begin{align}t_m & = \frac{\hat{v}_{\max} - v_0}{a_0} \\ t_e - t_m & = \frac{\hat{v}_{\max} - v_e}{a_1} \\ L &= \frac{1}{2}(v_0 + \hat{v}_{\max})t_m + \frac{1}{2}(\hat{v}_{\max} + v_e)(t_e - t_m) \end{align}\]

Substitute \(t_m, t_e - t_m\) and solve for \(\hat{v}_{\max}\):

\[\begin{align}L &= \frac{(v_0 + \hat{v}_{\max})(\hat{v}_{\max} - v_0)}{2a_0} + \frac{(\hat{v}_{\max} + v_e)(\hat{v}_{\max} - v_e)}{2a_1} \\ \hat{v}_{\max} &= \sqrt{\frac{2a_0a_1L + a_1v_0^2 + a_0v_e^2}{a_0 + a_1}}\end{align}\]

Compare \(\hat{v}_{\max}\) with given bound \(v_{\max}\):

\[\begin{cases}\hat{v}_{\max}>v_{\max} & \text{case 1, compute $t_1$, $t_2$, $t_e$ additionally} \\ \hat{v}_{\max}\leq v_{\max} & \text{case 2, } t_m = \frac{v_{\max} - v_0}{a_0}, t_e = t_m + \frac{v_{\max}-v_e}{a_1}\end{cases}\]

Refer to the graphs on the left column, it is obvious that for case 1,

\[\begin{align}t_1 & = \frac{v_{\max} -v _0}{a_0} \\ t_e - t_2 & = \frac{v_{\max} - v_e}{a_1} \\ L &= \frac{1}{2}(v_0 + v_{\max})t_1 + (t_2 - t_1)v_{\max} + \frac{1}{2}(v_{\max} + v_e)(t_e - t_2) \\ \end{align}\]

which arrives at

\[\begin{equation}t_2 - t_1 = \frac{L}{v_{\max}}-\frac{a_1(v_{\max}^2 - v_0^2) + a_0(v_{\max}^2 -v_e^2)}{2a_0a_1v_{\max}}\end{equation}\]

If we take default setting with \(v_0 = v_e = 0\), this can be simplified as

\[\begin{align}t_1&=\frac{v_{\max}}{a_0}\\t_2&=\frac{L}{v_{\max}}+\frac{v_{\max}(a_1-a_0)}{2a_0a_1}\\t_e&=\frac{L}{v_{\max}}+\frac{v_{\max}(a_0+a_1)}{2a_0a_1}\end{align}\]

After getting time stamps \(t_1, t_2, t_e\) for case 1 or \(t_m\) for case 2, we can easily compute \(s_1, s_2\), or \(s_m\), and based on the distance traversed, locate the corresponding interval and assign velocity to the tracker. Using the formula for constant acceleration, \(s = \frac{v^2 - v_0^2}{2a}\), we can solve \(v\) for case 1:

\[\begin{equation}v = \begin{cases} \sqrt{v_0^2 + 2a_0s} & s \leq s_1 \\v_{\max} & s_1 < s \leq s_2 \\ \sqrt{v_{\max}^2-2a_1s} & s > s_2\end{cases}\end{equation}\]

and similarly for case 2, there are two intervals, separated by \(s_m\). We can perform a sanity check by plotting the v-s graph:

Pick a point \((s_i, v_i)\) on the curve, where \(s_i < s_1\), we have the derivative vector \(p_i\) at \(s_i\). The projection of \(p_i\) on \(s\) axis is thus motion rate of change w.r.t. distance, which is just \(v(s_i) = \sqrt{v_0^2+2a_0s_i}\); the projection on \(v\) axis is rate of change w.r.t. velocity, which is acceleration at \(s_i.\) Interestingly, the derivative projections are endowed with physical meanings and mapped back to the original function axis. To verify this, simply apply chain rule to compute the derivative:

\[v'(s_i) = (\sqrt{v_0^2+2a_0s_i})' = \frac{a_0}{\sqrt{v_0^2 + 2a_0s}}\]

This is a nice example as it’s already in \(\frac{dv}{ds}\) form, where we clearly see \(dv = a_0\) and \(ds = \sqrt{v_0^2+2a_0s}\). This visualization also introduces the concept of feasible motion cone, which describes a range of possible motion given current velocity and acceleration bounds, but we will skip that for now.

The trapezoidal velocity profile is a very practical design, simple but ensures smooth acceleration and deceleration. There exist many other more sophisticated profiles for different applications, such as with constant jerk profile that provides smooth acceleration and deceleration. We can even deploy the original minimum jerk velocity profile here, and the computation of \(v\) would then rely on numerical methods to search for the area under the v-t curve. In practice, it usually takes some pre-computation for faster query at run time.

In addition to the single profile above, we can also combine profiles of multiple pieces of trajectories together to generate a full profile with different initial/final velocities and accelerations, by applying the same approach for each piece. The catch is matching the final velocity of the previous profile with the initial velocity of the current profile. It requires a linear scan at the beginning to ensure the final velocities are clipped to match the magnitude of the initial velocities of the next piece, as well as the assigned max velocities for each interval.

Now there come new problems: what if the direct evaluation of \(f(u)\) is not viable, because the trajectory is a set of points, not a spline? What if the tracker is incapable of staying close to the trajectory (in some cases of non-holonomic systems, or effects from controller parameters), and always deviates, so its traversed distance does not reflect actual \(s\)? The answer to both questions relies on the original trajectory. We can project the current tracker position \(\hat{x_i}\) onto the trajectory and find the closest point \(x_i\), use its index \(i\) as the trajectory parameter. The next goal \(x_{i+1}\) is the first point outside of the look-ahead distance \(w(\frac{s_i}{L})t_i\) on the trajectory. The velocity mapping issue of the tracker’s deviated trajectory is also solved implicitly in this way. However, as an approximation, the accuracy of this method depends on the discretization resolution of the trajectory. We will discuss the projection methods in detail in the next article, as it needs to be done carefully, otherwise the tracker would easily stuck in some local optimum.

So far we have dived into how to assign a velocity profile on a path, but we still have to design the profile by estimating tracker’s capability, which could be off for different configurations. With this background information, in the next article, we will continue from article III and talk about how to use the profile merely as a guidance, to let the tracker decide its own velocity parameters at run time.