Trajectory Basics V: Adaptive Tracking I

In the previous two articles, we mentioned other methods of goal-fetching, and raised the question on how to determine velocity based on tracker’s state. This is trying to achieve 1. accuracy, where the tracker minimizes its deviation from the trajectory and 2. speed, where the tracker reaches the goal with its maximum velocity. These two notions are highly correlated and are namely adaptive path tracking. In this article, we will be focusing on the first part, finding goal position. Recall the goal-fetching formulation in article III:

\[\begin{equation}g(s_i, u_i) = h(s_{i},\ \sigma(u_{i+1}))= h(s_i, s_{i+1})\end{equation}\]

The new interpretation here for \(s\) is that, both the input and output states do not have to be on the trajectory, as long as they ensure the tracker has minimal deviation traversing the entire trajectory. The interpretation of \(u\) should not change, as the tracker still keeps track of where it is on the trajectory.

\[\begin{equation}\hat{s}_{i+1} = g(\hat{s}_i, u_i) = h(\hat{s}_{i},\ \sigma(u_{i+1}))=h(\hat{s}_i, s_{i+1})\end{equation}\]

The previous mentions of pure pursuit are very handwaving, throwing conclusions that the goal is just the next state \(\sigma(u_{i+1})\) on the trajectory. There are many questions unanswered: what if the tracker has kinematic constraints to reach the goal, as in the case of a non-holonomic system? What if the tracker is already overshooting its goal, should it go backwards to the goal, or continue to the next one? What if the tracker deviates too much from the trajectory, so that the distance-based method for finding \(u_i\) is already not accurate? To answer these questions, we have to start from the details of this seemingly straightforward pure pursuit method.

The plot above shows a tracker moving along the trajectory in the order of point A, B, C, D. The end point of the red arrow is the tracker’s position, and the red circle represents the search radius (look-ahead distance \(r\)) parameter for the pure pursuit.

Case 1. The tracker lies perfectly on the trajectory, and the search circle has intersections with the trajectory. This is the simplest and the most ideal case, where the tracker’s actual position is aligned with the trajectory. If the tracker has always been staying on the trajectory, it can easily find its \(u\) by directly using its traversed distance and find the next goal. This is the assumption in our previous discussions. Note: this case shows one disadvantage of pure pursuit: as one can see, if the look-ahead distance increases, the tracker would cut corner and its true path will deviate from the trajectory. The bigger the search radius, the worse the problem becomes.

Case 2. The tracker overshoots on reaching point B, but the search circle still has intersections with the trajectory. This time the traversed distance does not reflect the actual status, but the tracker has memory of its last goal, which is point B on the trajectory. One option is to start searching from point B and ask the tracker to go back. This is not ideal for the smoothness of the tracking, but for applications that have extreme accuracy requirement, or when the overshoot is small such that looking ahead from point B causes no drawback, it is often a solution. The other option is to find the intersections directly from its current position and shoot for the one along traversing direction. However, it would require a more sophisticated implementation of the trajectory in order to perform the search efficiently, as a simple set of trajectory points would require multiple passes and distance checks.

Case 3. The tracker is overshooting on reaching point C, or it could not be overshooting, but the search circle has four intersections on the trajectory. Same as case 2, it could use search results either from point C, or from its current position. For both options, the tracker should pick the intersection that has the minimal \(u\) that is ahead of tracker’s current \(u_i\), so that it won’t jump directly to the next segment of the trajectory.

Case 4. The tracker overshoots again on reaching point D, but this time it went so far that there is no intersection with the original trajectory. This is especially bad because fallback on point D would definitely cause a bump, so in this case we project tracker’s position onto the trajectory and start searching on that point. This projection is represented by a line segment stemming from the tracker, vertically intersects a tangential line on the trajectory, where the segment length is the minimum distance between the tracker and the trajectory. In case there are multiple such intersections, pick the one that is closest from point D. Again, this would require some design in trajectory’s data structure, but this method can also be applied to case 2 and 3, whenever there is deviation from the original trajectory.

These conclude the most common cases while traversing a trajectory, and the different solutions for finding the next goal. Once the goal is confirmed, the tracker is commanded directly to the target, hence the name “pure pursuit”. In practice, if the system dynamics is good enough to avoid major deviation, the tracker only needs to keep track of the index of the last point on the trajectory it has traversed, and use that instead of its actual position to find the next index. This could save work for implementation, but it’s not always the case, as the requirements for “good dynamics” is usually so high that only well-designed articulated robot arms meet the standard.

Moreover, as one may see from the plot, sometimes sending the tracker directly back to point \(p\) generates a velocity with too much normal component to the trajectory, potentially causing another overshoot and results in oscillation behavior. In such cases, although \(q\) is not on the trajectory, it may be a better choice as it gives more tolerance along the direction of the motion, making the traversal smoother.

In case of non-holonomic systems, shooting for the goal is a bit more involved due to constrained kinematics. We will save that for later and talk about a specific implementation of pure pursuit, referred to as “smart pursuit”. As shown in the plots above, in many practical cases where the trajectory is represented by a sequence of points, the smoothness of the traversal is largely determined by the resolution, namely the discretization, of the trajectory. The smart pursuit uses the trajectory as ground truth, and always try to pull back the tracker once it deviates. It has a look-ahead distance that is a multiple of the resolution, which simplifies the tracking into advancing by discrete intervals. In to the plot below, suppose we pick interval of four points, there are a few cases:

Case I.

1. Current tracker interval is \([S_0, S_1]\), and the tracker is at \(q_0\). There is no correction needed as it’s right on track, and the interval advances to \([S_1, S_2]\).
2. Current tracker interval is \([S_0, S_1]\), and the tracker is at \(T_0\). The next goal would be \(p_0\), the projection of \(T_0\) onto line segment \(S_0S_1\), and the interval advances to \([S_1, S_2]\).

Case II. Current tracker interval is \([S_1, S_2]\), and the tracker is at \(T_0\). The projection on the interval is now \(p_1\), behind \(S_1\). The next goal would be \(S_1\), and the interval does not advance.

Case III. Current tracker interval is \([S_1, S_2]\), and the tracker is at \(T_1\). The projection \(p_2\) is now ahead of the interval. Advance the interval to \([S_2, S_3]\) and check above cases again.

When reaching the last interval, simply set the final target as the trajectory’s end point. The implementation is also relatively friendly with a linear pass of the intervals for the projection and interpolation calculations. Theoretically, if the look-ahead distance is the same as the resolution, it should never miss a corner on the trajectory! And as noted, regardless of lagging, overshooting, or deviation, the tracker always keeps moving forward along the path.

There is still plenty of room for improvement on smart pursuit: instead of pulling tracker right back onto the trajectory, we can make some smoother interpolation along tracker’s moving direction; the look-ahead distance can also be adaptive, based on the curvature of the trajectory… to name but a few. In the next article, we will look into such improvements, together with how to adaptively set the desired speed for the tracker.