Trajectory Basics III: State-Based Traversal

In the last article, we used time to parameterize the path and generated a spline trajectory. We had a code snippet of a tracker running on a minimum-jerk trajectory that reaches for a point fixed time ahead, robust to occasional misses. However, it is hard to enforce the speed/acceleration limits at the knots without affecting the shape of the spline. More importantly, it is not easy to create a velocity profile for different task configuration, and usually the longer the trajectory, the more deviation between the planned trajectory and the tracker execution. The time-based tracker we created last time tried to resolve the deviation issue by using tracker’s own timer, and always trying to move directly to the next goal based on its own timer. This goal-fetching method described as

is called pure pursuit, and is trivial for systems with no dynamic constraints—that is, the system is non-holonomic. The major issue of pure pursuit is overshooting, if the tracker is poorly conditioned and given a far-fetched goal. Overshooting brings more deviation to the tracker.

To tackle these disadvantages brought by the time-based spline, we have to find a better approach than conjecturing the speed capability of the tracker. Just like leading a team, where the best approach is to let members fully explore their own potentials rather than to plan everything out for them, the program should let the tracker to reach for its own limit rather than assigning a velocity profile for it. The tracker should be able to find its goal on the trajectory based on its current state by certain guiding principle \(g(s)\), just like a lead gives general dev guidelines to the team. We can generalize equation 1 as

where the time-based tracker is a special case with \(u:= t\) and \(h(s_i, s_{i+1}) = s_{i+1}\).

This generalization opens up two important possibilities for tracking: 1. time-agnostic parameterization of the trajectory; 2. more sophisticated goal chasing methods other than pure pursuit. With this formulation, we can compute the goal state from any given positional path, either analytically generated splines, or numerically sampled coordinate sequences, based on the tracker’s current state. We can base off from any shape of positional trajectory and generate desired goal state, or control signal, during execution time. It also enables us to re-parameterize the trajectory for better tracking behavior. The state-based traversal is formulated as:

Given a trajectory \(\sigma\), find the target-guiding function \(g\), so that for any given state \(s_i\) at trajectory parameter \(u_i\), \(g(s_i, u_i)\) will guide the tracker to traverse the trajectory in the fastest fashion with minimal deviation.

Remember the case of the two players? During practice, player A soon found himself out of breath and not able to follow the planned velocity, so he changed his strategy to only follow the lines, while adjusting his pace based on the breath and foreseeable turns—a perfect example of state-based traversal. In our case, we can start simple with a pre-generated spline \(f(u)\), where no derivative bounds are enforced at the knots. Now, how do we compute this trajectory parameter \(u_i\), if it’s not tangible as time? The trajectory parameter represents a certain form of progress on the trajectory. In time-based case, this progress is how much time has elapsed; in the state-based scenario, this typically represents the percentage of trajectory traversed. The word “typically” is emphasized because there could be a lot of variations to this definition, e.g., longitudinal distance, if lateral error is tolerable, or other kinds of distance calculation with specific requirements of projection/transform. We proceed with the simplest, cumulative Euclidean distance. The tracker keeps track of the total distance \(d_i\) it traveled at every iteration \(i\), and computes \(u_i = d_i / L\), where the total length of the pre-generated spline could be calculated by summing up arc lengths between each knots as

We can then add a small and adjustable step \(\Delta u\) to \(u_i\) and get the next state \(f(u_i + \Delta u)\). This can be easily extended to non-spline trajectories, such as trajectories represented by sampled points, where \(L\) is computed by summing up the distances between all of the points. The implementation of a state-based tracker is similar to that of the time-based tracker: compare the similar use between traversed_dist and curr_ts from the previous article.

// set chasing point 1% of traj length ahead of current state
static constexpr double kPursuitLookAheadP = 1e-2;

// leave out implementation, shown in the time-based tracker code
struct CartesianState { };

template <typename State>
class Spline : public Trajectory<State> { };

template <typename State>
bool follow_path(const Trajectory<State>& s_traj, const RobotInterface<State>& robot, 
  const double control_frequency)
{ 
  AdaptiveTimer clk{control_frequency};
  const double traj_len = ComputeSplineArcLen(s_traj);
  double traveled_dist = 0;  // important to keep track in the parameter space
  double next_u = kPursuitLookAheadP;
  auto prev_state = CartesianState::NullState();
  auto next_state = s_traj.ComputeState(next_u);
  clk.start();
  while (true) {
    const auto& curr_state = robot.GetState();
    // if the robot reaches desired end state. Note the trajectory parameter check
    if (traveled_dist >= traj_len && curr_state == s_traj.original_states.back()) {
      break;
    }
    // place holder for no-motion check, in case of singularity/local optimum
    ...
    // If the robot has moved away
    if (!curr_state.InProximity(prev_state)) {
      // command to the next state (async motion)
      robot.MoveTo(next_state);
      traveled_dist += (curr_state - prev_state).dist();
      next_u = std::fmin(traveled_dist / traj_len + kPursuitLookAheadP, 1.0); 
      next_state = s_traj.ComputeState(next_u);
    }
    // Still ensures consistent loop freq by the timer
    clk.sleep();
    prev_state = curr_state;
  }
  return robot.HasReached(s_traj.back());
}

The state-based traversal prepares us for attaching adaptive speed profiles to arbitrary trajectories. In this example, you might have realized that this in nature is the same as the previous time-based tracker. The generalization to use next_u instead of next_ts, however, is foundational to the non-dimensionalization of the trajectory tracking, and to avoid notation confusion for future steps, since “time” as a trajectory parameter is different from its original meaning. The use of a fixed \(\Delta u\) also implies constant velocity. How to adaptively achieve fastest velocity, and how do other goal-fetching methods come into play? We will answer those questions in the next article.