Trajectory Basics II: Interpolation & Smoothing

In previous article we made an initial version of time-based trajectory tracker. There are two unresolved problems, 1. the spline trajectory generation, and 2. the tracker missing commanded goals. We will illustrate how a better understanding of 1 could solve 2.

Let us rewind to the point before we introduced spline. We are trying to navigate the tracker through a sequence of points. From a control perspective, the intention to control an object to go through a sequence of pre-defined points is not always doable. Fliess and Lévine coined the term “differential flatness”, meaning that for a system with states \(x\in\mathbb{R}^n\) and control inputs \(u \in\mathbb{R}^m\), one can find a set of outputs \(y \in\mathbb{R}^m\) that determines all states and inputs \(y = f(x, u, \dot{u}, ..., u^{(p)})\) without integration. This is an important property, because we can plan trajectories in the output space first, and map back to solve for the inputs. Most common vehicles, e.g. fully-actuated articulated robot, wheeled robot, planar rigid body, ideal fixed-wing aircraft, and quadrotors, are flat systems. Inverted double-pendulum and driving in water-logged roads are examples of non-flat system, where the state outputs depend also on previous states. Such systems are not flat, but may still be accessible, so that they can still be controlled.

With theoretical foundation, we can continue on path interpolation. There are many ways to perform the path interpolation, but why do we pick splines? Think about the ideal trajectory: accelerate at the start point, reach full speed in the middle, and decelerate till the end point. This rate change of velocity implies a minimal sum of acceleration along the trajectory, and we can further generalize this to the k-th order derivative of the states:

In the paper by R. Shadmehr and S. P. Wise, they used a method called “calculus of variations” to find the minimum of functional \(\mathcal{C}\). With a small variation function \(\eta\) that satisfies \(\eta^{(\alpha)}\vert_{\{0,\ 1\}} = 0\) , we can take derivative of the cost \(\frac{d\mathcal{C}(x+e\eta)}{de}\) with small perturbance \(e\), and use integral by parts to get the answer, \(\sigma^{(2k)} = 0\). This implies the trajectory is a \(2k - 1\) order polynomial function. As for our robot gripper example, as Flash and Hogan pointed out in their 1985 paper, we can mimic human arm to minimize the jerk (3rd order derivative) for trajectory tracking. This means the trajectory is a 5th order polynomial. Spline becomes handy in this case: an n-degree polynomial is (n-1) derivable, and it fits our optimization goal.

For convenience, we use \(f(t)\) to denote the set of polynomials, and \(f_{i}\) for the piece of polynomial connecting points \(p_i, p_{i+1}\). Up till now, we are still operating in the non-dimensional parameter space. The non-dimensionality is an important property for trajectory generation, but for our time-based trajectory tracker, we use \(t\) for trajectory parameter and continue in time domain. The temporal scaling is merely applying the scaling factor \(\alpha\) as \(f(t/\alpha)\), so that as \(\alpha\) increase, it takes longer to track the trajectory. The expressions for the position, velocity, and acceleration states for a 5th order polynomial are written out as

for interval \(i\). In one-dimensional case, coefficient matrix \(a\) has shape of \((N-1) \times 6\), and we have to solve for these unknowns. For path points of higher dimensions, we just need to solve the coefficients separately and stack them together:

For simplicity, we use \(p\) to represent output for all dimensions, and \(a_i^j\) to represent the \(j\)th polynomial coefficient of the \(i\)th interval. With the knots ensuring the continuity of the polynomials, the equations for the positional constraints can be written as

for \(i\in[0, N - 2]\). We also know that \(\dot{f}\) and \(\ddot{f}\) both equal \(0\) at \(t = \tau_0, t = \tau_{N-1}\):

Besides point positions, we have the set of derivatives as boundary conditions, \(\dot{f}_i(\tau_{i+1}) = \dot{f}_{i+1}(\tau_{i+1})\), \(\ddot{f}_i(\tau_{i+1}) = \ddot{f}_{i+1}(\tau_{i+1})\), …, up to \(f^{(4)}_i(\tau_{i+1}) = f^{(4)}_{i+1}(\tau_{i+1})\).

We can enforce velocity/acceleration limit on the knots, so that we are not going too fast if the path has sharp turns. These can be expressed as constraints, but tuning their magnitude is tricky. The spline generation is now formulated as an optimization problem on (1), given boundary conditions and continuity constraints from (5) to (8). This problem can be solved via quadratic programming. Take the trajectory below with four path points as an example,

the pipeline looks like this: firstly, estimate the total time required to traverse the entire trajectory, and plan the time assignment, or velocity profile, for each interval. This assigns \(\tau_i\) for each path point. At the sampling phase, locate interval \(i\) the query time point falls in, and compute the states (position, velocity, acceleration, etc) using solved \(a_i\). We can then write an improved version of tracking code for this trajectory. We added a self-explanatory API to the RobotInterface, GetState, which returns the current state of the robot.

// set initial chasing point 50ms ahead of current state
static constexpr double kPursuitLookAheadTimeS = 5e-2;
static constexpr double kPositionTolM = 1e-3;

struct CartesianState {
  // Skip the constructors etc
  ...
  Eigen::Vector3d pos, vel, acc;
  /**
   * check if two states match, using only positional info. Note the 
   * trajectory parameter check is required in case the traversed points 
   * have loop backs, so it does not skip part of trajectory
   */
  bool operator=(const CartesianState& other_state) const {
    // only checking position here, other derivatives are transient
    return (other_state.pos - pos).norm() < kPositionTolM; 
  }
  /**
   * to mark an undefined state, usually set as initial value.
   * The distance to any other state is infinity
   */
  static CartesianState NullState() {
    ...
  }
  
  // check if close to the other state, that the robot has moved
  bool InProximity(const CartesianState& other_state) const {
    ...
  }
};

template <typename State>
class Spline : public Trajectory<State> {
  // leave out constructors etc
  ...
  std::vector<double> knots;   
  /**
   * Initial given conditions of path points and vel, etc.
   * This is defined in the Trajectory base class, just showing here
   */
  std::vector<State> original_states; 
  /**
   * Compute the state on the trajectory, parameterized by u
   */
  State ComputeState(const double u) const override {
    ...
  }
};

template <typename State>
bool follow_path(const Trajectory<State>& t_traj, const RobotInterface<State>& robot, 
  const double control_frequency)
{ 
  AdaptiveTimer clk{control_frequency};
  double curr_ts = 0;  // important to keep track in the parameter space
  double next_ts = kPursuitLookAheadTimeS;
  const double loop_time = 1. / control_frequency;
  const double t_end = t_traj.knots.back();
  auto prev_state = CartesianState::NullState();
  clk.start();
  while (true) {
    if (clk.elapsed_time() > 2 * t_end) {
      // if spent too much time, bail out
      return false;
    }

    const auto& curr_state = robot.GetState();    
    // if the robot reaches desired end state; note the time stamp 
    // (trajectory parameter) check
    if (curr_ts >= t_end && curr_state == t_traj.original_states.back()) {
      break;
    }
    
    // if the robot is ready to go for the next state
    if (!curr_state.InProximity(prev_state)) {
      State next_state = t_traj.ComputeState(next_ts);
      // command to the next state (async motion)
      robot.MoveTo(next_state);
      curr_ts = next_ts;
      // the timer ensures each loop has the same time interval
      // always chasing a point loop time ahead on the trajectory
      next_ts = std::fmin(next_ts + loop_time, t_end);
    }
    /** 
     * if robot is not ready, just sleep and wait for the robot.
     * this will prevent accumulation of error when robot fail to 
     * catch a time stamp
     */
    clk.sleep();
    prev_state = curr_state;
  }
  return robot.HasReached(t_traj.back());
}

Comparing to the time-based robot gripper trajectory tracker in the previous article, the main difference is that here we are using time as an extra dimension to align the states. The program only increments \(t\) if the robot is close to the previous target, so if the robot is lagging behind, it will not send new targets. Once the robot catches up, the next target is also not going farther as world time elapses. Time is a dimensionality used to align the tracker’s own state. The end state is checked by both positional state of the robot and curr_ts, so that when trajectory has spatial loops, the tracker won’t skip the loop and jump to the end.

This is a much nicer version of a time-based tracker. However, when we are focusing on the smoothness and speed part of the trajectory, we omitted the “accurate” part—we do not want to deviate too much from the original linear path points, not only because we want to save travel distance, but also to avoid obstacles. Although obstacles can also be formulated as constraints for the optimization problem, they may change the convex nature of the problem, and/or make finding the solution non-viable. Another concern is that, we have not incorporated system kinematics/dynamics into the trajectory. The trajectory is merely a geometric shape, with smooth derivatives, their magnitudes can still be high at the turns. Naively enforcing magnitude constraints on the knots causes the spline to change its shape, and solving these constraints together with obstacle avoidance is a tough call for the optimizer, which requires huge amount of computational resource. We will discuss potential methods to solve this issue in the next article.