Trajectory Generator

In this post we will be creating a simplified example of the trajectory generation method outlined in "Optimal rough terrain trajectory generation for wheeled mobile robots" by Thomas M. Howard and Alonzo Kelly.

Overview

The objective of trajectory generation is to generate a set of controls u to satisfy some constraints on our vehicle C subject to a set of governing differential equations describing system dynamics.

The full process of the algorithm is highlighted in light blue in the diagram below. The algorithm takes in your robot's initial and final state, control parameters and vehicle model as input. It then outputs the trajectory that the robot should follow as well as the control parameters required to achieve that trajectory.

The algorithm iteratively optimizes the control parameters until the robot until the cost (generally defined as the difference between the output trajectory's final state and the robot's desired final state) is below a certain threshold.

In-depth Example

Inputs

Let us assume that we have an Ackerman steering robot with a constant velocity of 3m/s. The initial and final state will be given by and .

We approximate our steer k over time t with a quadratic function where a, b and c are coefficients that will be determined. We wish find a feasible trajectory from our start to the goal using a quadratic time varying steer.

Our optimizer will be given an initial steer and an initial guess of the following variables stored in the TrajectoryParam struct: middle steer , final steer and the arc length of the trajectory .


struct TrajectoryParam {
    float s; // path length
    float km; // steer in the middle
    float kf;// final steer
};

We will be using the bicycle model as our vehicle model. So our motion model will be constrained to

is a fixed parameter that determines the number of fixed points along our trajectory to sample in generating a trajectory.

Trajectory Generation

The trajectory generation function is defined in Pose2DTrajectory TrajectoryOptimizer::generateTrajectory(TrajectoryParam& params, float k0).

The time horizon our initial trajectory will be given by s divided by our velocity.

Using the three pairs of points:

we can recover the coefficients that represent .


std::array<float, 3> xk = {0.0f, time_horizon/2.0f, time_horizon};
std::array<float, 3> yk = {k0, params.km, params.kf};
std::array<float, 3> coeff = quadraticCoefficients(xk, yk);

Using the coefficients of we can then roll out the trajectory generated by this steer command over discretized steps.


for (float t = 0.f; t < time_horizon; t += dt) {
        float steer = quadraticInterpolation(coeff, t);
        state.update(0.0f, steer, dt); // no acc.
        traj.x.push_back(state.x);
        traj.y.push_back(state.y);
        traj.theta.push_back(state.yaw);
}

Compute Cost

The cost calculation is done in Eigen::Vector3f TrajectoryOptimizer::calculatePoseErrorVector(const Pose2D& p1, const Pose2D p2). It is the sum of squared difference between each dimension of trajectory's final pose and our traget pose. If this value is below our threshold, then our optimization is complete. If not, we adjust parameters to try to get a better trajectory.

Adjust Parameters

The trajectory's final state can be modelled as the output of a nonlinear function :

Our parameter update step is done via the Gauss-Newton method with line search. To obtain the Jacobian we apply the centered difference approach which computes the error vector based on small perturbations to the parameters of


Eigen::MatrixXf TrajectoryOptimizer::calculateJacobian(Pose2D& target_pose, TrajectoryParam& params, float k0) {
    // perturb each param, use centered diff to approx jacobian
    TrajectoryParam params_pert;
    Eigen::Vector3f dp, dn;
    Pose2D pose_pos, pose_neg; // pose from positive and negative perturbation

    // perturb s, first col of jacobian
    params_pert = {params.s + delta_params_.s, params.km, params.kf};
    pose_pos = generateFinalState(params_pert, k0);
    dp = calculatePoseErrorVector(target_pose, pose_pos);

    params_pert = {params.s - delta_params_.s, params.km, params.kf};
    pose_neg = generateFinalState(params_pert, k0);
    dn = calculatePoseErrorVector(target_pose, pose_neg);

    Eigen::Vector3f d1 = (dp - dn) / (2.0f * delta_params_.s);

    // perturb km, second col
    params_pert = {params.s, params.km + delta_params_.km, params.kf};
    pose_pos = generateFinalState(params_pert, k0);
    dp = calculatePoseErrorVector(target_pose, pose_pos);

    params_pert = {params.s, params.km - delta_params_.km, params.kf};
    pose_neg = generateFinalState(params_pert, k0);
    dn = calculatePoseErrorVector(target_pose, pose_neg);

    Eigen::Vector3f d2 = (dp - dn) / (2.0f * delta_params_.km);

    // perturb kf, third col of jacobian
    params_pert = {params.s, params.km, params.kf + delta_params_.kf};
    pose_pos = generateFinalState(params_pert, k0);
    dp = calculatePoseErrorVector(target_pose, pose_pos);

    params_pert = {params.s, params.km, params.kf - delta_params_.kf};
    pose_neg = generateFinalState(params_pert, k0);
    dn = calculatePoseErrorVector(target_pose, pose_neg);

    Eigen::Vector3f d3 = (dp - dn) / (2.0f * delta_params_.kf);

    // return matrix
    Eigen::Matrix3f J;
    J << d1, d2, d3;
    return J;
}

In our line search, we apply a series of predefined learning rates to our parameter vector and compute the error in the trajectory generated by the updated parameter vector. We update our parameter vector with the learning rate that generated the lowest cost.


float TrajectoryOptimizer::selectLearningParam(Pose2D& target_pose, TrajectoryParam params, float k0, Eigen::Vector3f& dp) {
    float min_cost = std::numeric_limits<float>::max();
    float min_a = min_alpha_;
    for (float a = min_alpha_; a < max_alpha_; a += d_alpha_) {
        TrajectoryParam new_params = params;
        new_params.s += a * dp(0);
        new_params.km += a * dp(1);
        new_params.kf += a * dp(2);
        Pose2D last_pose = generateFinalState(new_params, k0);
        Eigen::Vector3f dstate = calculatePoseErrorVector(target_pose, last_pose);

        float cost = dstate.norm();
        if (cost <= min_cost && std::abs(cost) > std::numeric_limits<float>::epsilon()) {
            min_a = a;
            min_cost = cost;
        }

    }
    return min_a;
}

We repeat the trajectory generation->compute cost-> adjust parameter cycle until we have reached a maximum number of iterations or our computer trajectory is sufficiently close to our goal.