Trajectory Basics VI: Adaptive Tracking II

In previous articles of this series, we started from the foundations of trajectory, introduced the generation of smooth trajectories and naive implementations to track them, explained step by step why we need state-based traversal with re-parameterization, and finally the “smart pursuit” method to adaptively track the trajectories. We have arrived at the final piece of the puzzle, the adaptive speed planning, to reduce overshoot and deviation, while keeping the tracking time-efficient.

In Article IV, we have already showed how re-parameterization works, and it might seem straightforward to assign a speed to the tracker given its traversal state. This is much of an ideal case: with the curvatures and cusps in real trajectories, some tracker dynamics could not hold up to that speed without cutting corners or overshooting. This statement does not reckon the futility of this method, as in many cases it still stands for systems with linear constraints: prismatic actuators moving along lines or slight curves, joint motors rotating certain angles in configuration space. These systems can most likely achieve the assigned velocity without breaking bounds. However, for a 6-DoF robot arm end-effector weaving in Cartesian space, it can be very challenging. Slowing down at sharp turns is a solution, but that defeats the purpose of the pre-generated velocity profile. Naturally, there are two apparent ways to solve this problem: either generate a feasible velocity profile and let the tracker follow (open loop), or let the tracker operate at full capacity and dynamically adjust itself (closed loop). We will talk about the latter one first.

Before we start, we need to make a few things clear: first, we have to align on the tolerance of “cutting corners”, that is, the resolution \(\rho\) of the traversal. The resolution is defined as, from any path point, the maximal accumulated distance to the next goal point, so that the points in the middle are skipped. Given \(\rho\), at point \(P_i\), the next point \(P_j\) is

\[\arg\max_{j}(\sum_{k=i}^{j-1}d(P_k, P_{k+1})<\rho)\]

Since the points depict the shape of the trajectory, \(\rho\) is basically stating how many points it is allowed to skip while traversing the trajectory. For a uniformly sampled trajectory, this number is usually a multiple of the interval length \(d_i\) between path points; if \(\rho = d_i\), it has to traverse exactly through every single point and no skipping is allowed. The bigger \(\rho\), the less accurate the traversal, but it becomes faster by skipping corners and other shape details. Note this definition has no correlation with deviation at all; it only specifies that the tracker must go through certain points, and it may or may not deviate during the process. For non-holonomic systems, going through every path point may not be feasible, but even with deviation, the shape of the trajectory is obeyed. If the points are not uniformly distributed, with \(\rho > \min d(P_i,P_j)\), different starting point may give different goals, so the real traversal trajectory is not deterministic.

A similar concept that also describes the tracking quality is error tolerance \(\epsilon\), a distance threshold for the tracker to overshoot or undershoot a path point. Note that \(\epsilon\) is different from deviation in a sense that it is designed for an ideal condition: if the next path point is not on the straight line in the current direction of motion, the tracker will always needs additional space to perform a switch of direction. We will see how \(\epsilon\) is used soon.

Next, we have to take account of the technophysical limitations of the tracker. The sensor that samples tracker states works at a frequency \(f_s\), the controller that sends the high-level motion control signal to the tracker operates at frequency \(f_c\), and the program that reads and computes them has a specified loop frequency \(f_p\). As shown in the plot below, the sensor publishes at each red tick, the controller takes command at every blue tick, and the program takes input at magenta tick, sends output at green tick. The program’s read 0 is using the previous sensor tick and generates an output which is taken by the controller after out 0. Note when the controller gets the command, the sensor has generated 10 extra ticks, approximately 0.02 seconds. This means the controller is executing a command generated based on the input sampled 0.02 seconds ago:

It is quite clear when these ticks are projected onto a real trajectory. The exec 0 is computed based on sample 0, during which time the tracker has already moved to the blue dot. However, the original command is pointing back to the red dot, different from the ideal target position based on the tracker state at exec 0.

To clarify, when the latency \(\tau\) and velocity \(\bar{v}\) at sample 0 satisfy \(\bar{v}\tau\ll\rho\), this is not a problem, as the next goal is always much ahead on the trajectory. However, for real-time systems with low frequency, fine resolution, or high tracker speed such as autonomous driving, where the vehicle could operate at over 40m/s, 0.02s is a 0.8m difference which could cause potential danger near collision/dynamic boundaries, at the same time introducing zig-zag oscillation. The solution is to compute the next goal with predicted state \(\hat{s_i} = g(s_i, v_i\tau)\) at exec 0. If the trajectory is already generated, \(g = s_i + f'(u_i + v_i\tau/L)\) where \(f'\) is the prediction function evaluated by the trajectory at sample 0. This idea is called stitching, and it significantly reduces overshoot and oscillation issues.

Finally, we have to understand the behavior of the controller, and how it affects the tracker. Say there is a one meter long straight line, and the tracker is just going from one end to another. This seems to be a simple control problem—just give the controller the desired goal and let it take over, why bother using an interpolated trajectory? This is a deceptive idea, as in many cases the controller shows very little to no difference than interpolated trajectory. But we have to remember, during execution, there is not necessarily a constraint to enforce the controller to minimize its control error in the linear way as we expected. The controller could do whatever it wants to achieve its goal, and moving in a straight line is merely a coincidence. The purpose of a trajectory is to regulate the behavior of the tracker, that is to enforce a straight line motion. Now let’s consider the other extreme: what if the line is interpolated by \(10^6\) points with interval of only one micrometer? For tracker that is able to control speed, this should pose no difference with trajectory stitching; however, for tracker that has only position control and assumes zero velocity at goal, more points could severely reduce the speed, as it’s trying to decelerate before each overshoot. This example shows the importance of trajectory design, seeking balance between accuracy and speed, given performance requirements and controller capability.

With the points above cleared out, we can easily figure out how to adjust tracker’s speed within boundary dynamically. Generally speaking, the tracker should reach maximum speed when the path is straight, and minimum speed when approaching maximum curvature on the path. For convenience we set the limits on the path points. The minimum speed is usually set as a small value, or some limit from the applied scenario, where the only requirement is that the traversal resolution \(\rho\) is achievable. For the maximum speed, let’s illustrate by an example. Given a trajectory with uniformly spaced points, where each point is separated by \(\rho\). If the tracker needs to go from point 0 to point 4, it cannot skip point 2 and go directly to 3, because \(d(1, 2) + d(2, 3) > \rho\).

Consider the worst case where the program reads the tracker state just before reaching point 1, the stitching assumes the command output will be executed at the end of the red arrow. Since the next goal is point 2, if we want to ensure the smoothness of the tracking, the projection of commanded motion blue arrow on original \(\overrightarrow{01}\) direction should not have a horizontal component pointing back to point 1. The boundary condition is when there is no horizontal component at point P. The velocity magnitude at point 1 should thus remain under \(d(1, P) / \tau\). The maximum speed should not exceed this value. This is purely a task space constraint, determined by the trajectory and traversal resolution.

Another factor for the speed upper bound, on the other hand, is determined by control system’s constraint. As we have briefly mentioned in Article I, this is the process to map the limit in task space to control space, by solving \(\dot{x}_{\max}, \ddot{x}_{\max}...=\mathcal{M}(\dot{u}_{\max}, \ddot{u}_{\max}, ...)\). To solve for the velocity limit in task space, we need to introduce the error tolerance \(\epsilon\) mentioned above. Suppose the tracker is at speed \(v\) on interval [0, 1] and wants to maintain that after passing point 1. At point 1, it has to instantly spawn a vertical component \(v_y\) with direction \(\overrightarrow{P2}\) and reduce its horizontal component at \(\overrightarrow{01}\) to magnitude \(v_x\). The transient acceleration magnitude at this point, given angle \(\theta\) between \(\overrightarrow{01}\) and \(\overrightarrow{12}\), is

\[\begin{align} a &= \sqrt{(\frac{v-v_x}{t_{\epsilon}})^2 + (\frac{v_y}{t_\epsilon})^2} \\ &= \frac{\sqrt{2}v}{t_\epsilon}(1-\cos\theta)\end{align}\]

where \(t_\epsilon\) is the allowed time duration to accelerate, because no moving object is able to instantly reach a velocity. Substitute \(t_\epsilon=\frac{\epsilon}{v}\) and we arrive at

\[\begin{align}a &= \frac{\sqrt{2}(1-\cos\theta)}{\epsilon}v^2 \\ v &=(\frac{a\epsilon}{\sqrt{2}(1-\cos\theta)})^{\frac{1}{2}} \end{align}\]

Given limits in control space at predicted states of point 1, apply \(\mathcal{M}\) to get \(a\) in task space. Speed limit can then be derived by equation (4). As examples, for an articulated robot arm, joint velocity/acceleration limits can be converted to end-effector Cartesian space by forward kinematics, and for autonomous driving, this is done by the bicycle model, and the longitudinal and latitudinal limitations may be computed separately. We will leave the details of the formulation of \(\mathcal{M}\) and the computation of predicted states at point 1 to the next article.

If we perform this computation for the points within a certain look-ahead distance, we can safely adjust the velocity magnitude at run time: check ahead the predicted interval and its future neighbors, then compute \(v_{\max}\) and each point ahead accordingly. In the case with straight lines, since the overshoot will pass desired path points, there is no need to slow down. The tracker runs by this profile, and every loop it replans based on its current state. This is a simple representation of Model Predictive Control. Depending on the accuracy requirement and computation speed, one can choose a reasonable look-ahead distance to achieve desired behavior. You can see why we call this “closed loop”, because the plan is modified at every step by tracker’s actual state.

This closed loop approach is dynamically adjusting the tracker speed when it’s running forward, which works with not only pre-generated trajectory, but also real-time trajectory updating during traversal. The open loop approach takes a different stab by running backwards, starting from end conditions and rewind its way back to the origin, under specified system constraints. This way it’s generating a more sophisticated velocity profile based on the trajectory topology. We will leave this last part to the next article.