State Lattice

The methods outlined in this post are based on the following paper:

State Space Sampling of Feasible Motions for High-Performance Mobile Robot Navigation in Complex Environments

State Lattice Methods

State lattice methods involve precalculating a set of feasible trajectories for your robot using a given shape parametrization. At runtime, the best trajectory can be calculated quickly based on your cost function (e.g. closeness to obstacles). The three shape parametrizations identified in the paper are uniform, biased and lane state sampling. In this post we will be consideration polar uniform and biased sampling.

In all of the sampling methods below each trajectory is represented with a start pose and a goal pose. Using the lookup table for goal poses made using the trajectory generator in our previous post, we can use the as an initial guess in our optimization for a new trajectory.

Uniform State Sampling

Uniform state sampling involves uniformly sampling goal poses in a cone in front of the robot.

is the number samples in the terminal state position and represents the number of points along the arc to sample from.

is the terminal heading and represents the number of final orientations to sample from at each sample point.

is the terminal position horizon and represents the radius of the cone.

and represent the angular range to sample for terminal positions.

and represent the angular range to sample for terminal headings.

In calculateUniformPolarStates, we first generate a series of angles uniformly and then generate our target trajectory using some simple geometry in sampleStates.


// NOTE nxy = np in this code
std::vector<Pose2D> calculateUniformPolarStates(size_t nxy, size_t nh, float d,
                        float a_min, float a_max, float p_min, float p_max) {

    std::vector<float> angle_samples(nxy);
    for (int i = 0; i < nxy; ++i)
        angle_samples[i] = i / (nxy - 1.f);

    std::vector<Pose2D> states = sampleStates(angle_samples, a_min, a_max, d, p_max, p_min, nh);

    return states;
}

std::vector<Pose2D> sampleStates(std::vector<float>& angle_samples, float a_min,
                 float a_max, float d, float p_max, float p_min, size_t nh) {
    std::vector<Pose2D> states;

    float xf, yf, yawf;
    for (float i : angle_samples) {
        float a = a_min + (a_max - a_min) * i;
        for (int j = 0; j < nh; ++j) {
            xf = d * std::cos(a);
            yf = d * std::sin(a);
            if (nh == 1)
                yawf = (p_max - p_min) / 2.f + a;
            else
                yawf = p_min + (p_max - p_min) * j / (nh - 1.f) + a;
            Pose2D final_state {xf, yf, yawf};
            states.push_back(final_state);
        }
    }

    return states;
}

Biased State Sampling

The parametrization for biased state sampling is similar to uniform state sample except we add an extra term which is the number of navigation function samples. This new parameter biases our sampling at points where the global cost function is lower. So more dots appear near our goal angle.

Below is heavily commented version of the calculateBiasedPolarStates code.


std::vector<Pose2D> calculateBiasedPolarStates(float goal_angle, int ns, int nxy,
                                   int nh, int d,
                                   float a_min, float a_max,
                                   float p_min, float p_max){
    std::vector<float> as(ns-1);
    std::vector<float> cnav(ns-1);
    for (size_t i = 0; i < ns - 1; ++i) {
        as[i] = a_min + (a_max - a_min) * i / (ns - 1.f); // this is the angle used in uniform sampling
        cnav[i] = M_PI - std::abs(as[i] - goal_angle); // angle difference between uniformly sampled node and goal angle
    }

    // sum up all the angle diffs and find max element
    float cnav_sum = std::accumulate(cnav.begin(), cnav.end(), 0.f);
    float cnav_max = *std::max_element(cnav.begin(), cnav.end());

    // normalise the distribution of cnav
    for (float& cnavi : cnav) {
        cnavi = (cnav_max - cnavi) / (cnav_max * ns - cnav_sum);
    }

    // cumalative sum
    std::vector<float> cumsum_cnav(cnav.size());
    std::partial_sum(cnav.begin(), cnav.end(), cumsum_cnav.begin());

    // output angles based on cumulative sume
    std::vector<float> di;
    int li = 0;
    for (int i = 0; i < nxy; ++i ) {
        for (int ii = li; ii < ns-1; ++ii) {
            if ( ii*(1.f/ ns) >= i / (nxy - 1.f) ) {
                di.push_back(cumsum_cnav[ii]);
                li = ii - 1;
                break;
            }
        }
    }

    // use sample states as before
    auto states = sampleStates(di, a_min, a_max, d, p_max, p_min, nh);
    return states;
}

Lane Sampling

Lane sampling samples based on the geometry of a road lane.

is the lane centerline.

is the lane heading.

is the lane width.

is the vehicle width.

is the lane horizon.

is the number of lateral offsets to sample from along the center position.

is the number of lanes. In this example we kept this to one.

Recall a 2D rotation matrix is parametrized as follows.

The function below illustrates how each of these variables are used:


std::vector<Pose2D> calculateLaneStates(
    float l_center, float l_heading, float l_width,
    float v_width, float d, size_t nxy) {
    // apply rotation matrix to the points
    float xc = d * std::cos(l_heading) - l_center * std::sin(l_heading);
    float yc = d * std::sin(l_heading) + l_center * std::cos(l_heading);

    // move the center point by a small delta to the left or right
    // depending on how much "wiggle room" is left in the lane
    std::vector<Pose2D> states; states.reserve(nxy);
    for (size_t i = 0; i < nxy; ++i) {
        float delta = -0.5*(l_width-v_width) + (l_width-v_width) * (i / (nxy - 1.f));
        float xf = xc - delta * std::sin(l_heading);
        float yf = yc + delta * std::cos(l_heading);
        float yawf = l_heading;
        Pose2D target_state{xf, yf, yawf};
        states.push_back(target_state);
    }

    return states;
}