In previous lessons we treated planning as graph search over an abstract discrete state space with edges representing feasible transitions and costs. For high-DOF robots with nonholonomic or kinodynamic constraints, however, arbitrary edges are not allowed: motions must be dynamically feasible trajectories of the underlying system \( \dot{\mathbf{x}} = f(\mathbf{x}, \mathbf{u}) \).

State lattices address this by imposing a regular structure on the discrete state space (like a grid in configuration/state space) and by using motion primitives as edges. Each motion primitive is a short, pre-computed, dynamically feasible trajectory that can be reused from many lattice states by exploiting symmetry (e.g. translation invariance in workspace).

Formally, let the continuous state space be \( X \subset \mathbb{R}^n \) or a manifold such as \( \mathrm{SE}(2) \). A state lattice is a directed graph \( \mathcal{L} = (V, E) \) with \( V \subset X \) arranged in a regular pattern and each edge \( e = (v_i, v_j) \in E \) corresponding to a feasible trajectory segment of the robot.

A typical pipeline for lattice planning is:

flowchart TD
  A["Continuous model: x_dot = f(x,u)"] --> B["Design discrete state set (lattice)"]
  B --> C["Synthesize motion primitives from dynamics"]
  C --> D["Instantiate lattice graph (states + edges)"]
  D --> E["Run graph search (Dijkstra / A*)"]
  E --> F["Concatenate primitives to trajectory"]
  F --> G["Send reference trajectory to low-level controller"]
        

The key questions we study are:

• How to construct a lattice in configuration or state space?
• How to design dynamically feasible motion primitives?
• What completeness or optimality guarantees can we give with finite resolution?
• How to implement lattice planners efficiently for high-DOF robots?

Let the state space be a smooth manifold \( X \) (for example, configuration space or state space including velocities). A state lattice is defined by:

• A discrete state set \( V = \{ \mathbf{x}_k \}_{k=1}^N \subset X \), obtained by a regular discretization in some coordinate chart (e.g., uniform grid in \( x,y,\theta \)).
• A finite set of control templates \( \mathcal{U}_p = \{u^1(\cdot),\dots,u^m(\cdot)\} \) on a fixed horizon \( [0,T_p] \).
• A transition rule that integrates \( \dot{\mathbf{x}} = f(\mathbf{x},\mathbf{u}(t)) \) from each vertex \( \mathbf{x}_k \) with each primitive \( u^j(\cdot) \) to generate an outgoing edge \( e = (\mathbf{x}_k, \mathbf{x}_\ell) \).

More concretely, for each vertex \( \mathbf{x}_k \) and primitive \( u^j(\cdot) \), define the trajectory

\[ \mathbf{x}(t; \mathbf{x}_k, u^j) \text{ solves } \dot{\mathbf{x}}(t) = f(\mathbf{x}(t), u^j(t)), \quad \mathbf{x}(0) = \mathbf{x}_k, \quad t \in [0, T_p]. \]

Then, if the end state \( \mathbf{x}(T_p; \mathbf{x}_k, u^j) \) lies close to some lattice vertex \( \mathbf{x}_\ell \) (within a snapping tolerance), we create a directed edge \( e = (\mathbf{x}_k, \mathbf{x}_\ell) \) with cost

\[ c(e) = \int_0^{T_p} \ell(\mathbf{x}(t), u^j(t)) \, dt, \]

where \( \ell \) is a running cost, often simply path length or time. The graph \( \mathcal{L} = (V,E) \) thus encodes both geometry and dynamics.

Regularity of \( V \) (e.g., a grid in \( x,y,\theta \)) implies that each vertex has the same pattern of outgoing edges; this “shift-invariance” enables efficient pre-computation and reuse of motion primitives.

Motion primitives are short trajectories that satisfy the robot’s differential constraints and often bounds on control inputs. For a car-like robot with wheelbase \( L \), state \( \mathbf{x} = (x,y,\theta) \), forward speed \( v \), and steering angle \( \phi \), the kinematic model is

\[ \dot{x} = v \cos\theta, \quad \dot{y} = v \sin\theta, \quad \dot{\theta} = \frac{v}{L} \tan\phi. \]

A simple family of motion primitives fixes \( v \) and \( \phi \) over a short time window \( [0,T_p] \). Integrating the dynamics yields explicit formulas:

• For \( \phi = 0 \) (straight motion):

  \[ x(T_p) = x(0) + v T_p \cos\theta(0), \quad y(T_p) = y(0) + v T_p \sin\theta(0), \quad \theta(T_p) = \theta(0). \]

• For \( \phi \neq 0 \) (constant curvature): let \( \kappa = \frac{\tan\phi}{L} \) be curvature. Then

  \[ \theta(T_p) = \theta(0) + v \kappa T_p, \quad x(T_p) = x(0) + \frac{1}{\kappa}\big(\sin(\theta(T_p)) - \sin(\theta(0))\big), \]

  \[ y(T_p) = y(0) - \frac{1}{\kappa}\big(\cos(\theta(T_p)) - \cos(\theta(0))\big). \]

Thus each primitive is parameterized by a choice of discrete curvature \( \kappa_j \) (or steering angle \( \phi_j \)) and time horizon \( T_p \). By discretizing these parameters, we obtain a finite set of primitives that can be reused from any lattice pose \( (x_i,y_i,\theta_i) \).

In higher-DOF manipulators, the state lattice is more naturally defined in joint space, and motion primitives may be constant-velocity or spline-based joint motions respecting velocity/acceleration bounds. The basic idea is the same: precompute dynamically feasible short motions with controllable cost.

Consider a planar car-like robot with pose \( \mathbf{x} = (x,y,\theta) \). A common lattice construction is:

• Discretize position with grid steps \( \Delta x, \Delta y \).
• Discretize orientation into \( N_\theta \) equally spaced headings:

  \[ \theta_k = \frac{2\pi k}{N_\theta}, \quad k = 0,\dots,N_\theta-1. \]

The lattice vertices are then

\[ V = \left\{ (i\Delta x, j\Delta y, \theta_k) \;\middle|\; i,j \in \mathbb{Z}, \; k \in \{0,\dots,N_\theta-1\} \right\}. \]

For each orientation index \( k \) we attach a set of reference primitives (defined at the origin with heading zero), then transform them by rotation and translation to obtain outgoing transitions from any lattice pose. This exploits the symmetry of the kinematic model under rigid motions.

Let a reference primitive start at \( (0,0,0) \) and end at \( (\bar{x},\bar{y},\bar{\theta}) \) after integrating the dynamics for \( T_p \). From a generic state \( (x,y,\theta_k) \), the corresponding primitive’s end pose is

\[ \begin{aligned} x' &= x + \cos\theta_k \,\bar{x} - \sin\theta_k \,\bar{y}, \\ y' &= y + \sin\theta_k \,\bar{x} + \cos\theta_k \,\bar{y}, \\ \theta' &= \theta_k + \bar{\theta} \pmod{2\pi}. \end{aligned} \]

Finally, \( (x',y',\theta') \) is snapped to the nearest lattice vertex \( (i'\Delta x, j'\Delta y, \theta_{k'}) \). Collision checking for the primitive is done by sampling intermediate points along the trajectory and verifying that the robot’s footprint does not intersect configuration-space obstacles (which students saw in Chapter 1).

Once the lattice graph \( \mathcal{L} = (V,E) \) is instantiated (or lazily generated during search), we can run standard algorithms such as Dijkstra or A*. From Lesson 2, recall that a heuristic \( h \) is admissible if it never overestimates the true cost-to-go and consistent if it satisfies the triangle inequality relative to edge costs.

A simple admissible heuristic for a car-like lattice with cost equal to Euclidean path length is the Euclidean distance in the workspace (ignoring heading):

\[ h((x,y,\theta), (x_g,y_g,\theta_g)) = \sqrt{(x - x_g)^2 + (y - y_g)^2}. \]

If each primitive has cost at least the Euclidean distance between its endpoints (for instance, if we use arc length of the trajectory as cost), then this heuristic is admissible and, in fact, consistent:

\[ h(v) \leq c(v,v') + h(v') \quad \text{for all } (v,v') \in E. \]

Hence A* on the lattice yields an optimal path in the discrete graph, which corresponds to a sequence of dynamically feasible primitives in continuous space.

Because lattices discretize both state and controls (via motion primitives), they cannot be complete for the continuous planning problem. Instead, they often enjoy resolution completeness:

Definition (Resolution Completeness). A lattice planner is resolution-complete if there exists a resolution parameter \( \delta > 0 \) such that, whenever there exists a feasible continuous trajectory with clearance \( > \delta \) from obstacles, the planner finds a solution when the lattice spacing and primitive lengths are sufficiently small (bounded by a function of \( \delta \)).

Under mild assumptions on \( f(\mathbf{x},\mathbf{u}) \) (Lipschitz continuity) and controllability, one can show:

• For sufficiently small spatial and angular discretization and a rich enough set of motion primitives, any feasible trajectory can be approximated by concatenating lattice primitives.
• The cost of the lattice plan differs from the continuous optimum by at most \( \varepsilon(\delta) \), where \( \varepsilon(\delta) \to 0 \) as \( \delta \to 0 \).

A standard proof sketch partitions an optimal continuous trajectory into short segments of bounded length (depending on the discretization). Each segment is approximated by a primitive sequence starting and ending near lattice vertices, and Lipschitz continuity of the dynamics and running cost bounds the approximation error.

In practice, the resolution is chosen to balance accuracy and computational cost: the number of vertices scales roughly as \( O\big((1/\Delta x)(1/\Delta y)(1/\Delta\theta)\big) \) in a planar lattice, and the branching factor is the number of motion primitives.

flowchart TD
  R["Choose resolution (dx, dy, dth)"] --> L["Instantiate lattice (size grows as 1/(dx dy dth))"]
  L --> P["Search with A* using primitives"]
  P --> Q["Solution quality: cost gap epsilon(delta)"]
  Q --> T["Trade-off: finer resolution vs runtime"]
        

We implement a small-footprint lattice planner for a car-like robot in Python using A*. For simplicity, we work in a bounded grid and discretize orientation into a small number of headings. We assume a binary occupancy grid for collision checking.

import heapq
import math
import numpy as np

# Discretization parameters
DX = 1.0
DY = 1.0
N_THETA = 16  # number of discrete headings
DT = 1.0      # primitive duration
V = 1.0       # forward speed
L = 2.0       # wheelbase

# Occupancy grid: 0 = free, 1 = obstacle
# Here we just create a small empty grid as an example.
NX, NY = 50, 30
occ_grid = np.zeros((NX, NY), dtype=np.uint8)

def in_bounds(ix, iy):
    return 0 <= ix < NX and 0 <= iy < NY

def is_free(ix, iy):
    return in_bounds(ix, iy) and occ_grid[ix, iy] == 0

def angle_index(theta):
    """Map continuous angle to discrete index."""
    theta = (theta + 2.0 * math.pi) % (2.0 * math.pi)
    k = int(round(theta / (2.0 * math.pi) * N_THETA)) % N_THETA
    return k

def angle_from_index(k):
    return 2.0 * math.pi * k / N_THETA

# Precompute reference motion primitives at pose (0,0,0)
STEER_ANGLES = [-0.4, 0.0, 0.4]  # radians, simple left/straight/right
ref_primitives = []  # each entry: (dx, dy, dtheta, length)

for phi in STEER_ANGLES:
    kappa = math.tan(phi) / L  # curvature
    if abs(kappa) < 1e-6:
        # straight-line primitive
        dx = V * DT
        dy = 0.0
        dtheta = 0.0
        length = V * DT
    else:
        # circular-arc primitive
        dtheta = V * kappa * DT
        th_end = dtheta  # from 0 to dtheta
        dx = (1.0 / kappa) * math.sin(th_end)
        dy = -(1.0 / kappa) * (math.cos(th_end) - 1.0)
        length = abs(dtheta / kappa)
    ref_primitives.append((dx, dy, dtheta, length))

def successors(state):
    """
    state = (ix, iy, kth)
    Returns list of (next_state, cost).
    """
    ix, iy, kth = state
    theta = angle_from_index(kth)
    succs = []
    for dx_ref, dy_ref, dth_ref, length in ref_primitives:
        # transform reference primitive to global frame
        gx = ix * DX
        gy = iy * DY
        dx = math.cos(theta) * dx_ref - math.sin(theta) * dy_ref
        dy = math.sin(theta) * dx_ref + math.cos(theta) * dy_ref
        gx2 = gx + dx
        gy2 = gy + dy
        kth2 = angle_index(theta + dth_ref)
        ix2 = int(round(gx2 / DX))
        iy2 = int(round(gy2 / DY))

        if not is_free(ix2, iy2):
            continue

        cost = length  # use arc length as cost
        succs.append(((ix2, iy2, kth2), cost))
    return succs

def heuristic(state, goal_state):
    ix, iy, _ = state
    gx, gy, _ = goal_state
    x = ix * DX
    y = iy * DY
    xg = gx * DX
    yg = gy * DY
    return math.hypot(x - xg, y - yg)

def astar(start_state, goal_state):
    """
    A* search on the lattice.
    state = (ix, iy, kth)
    """
    open_heap = []
    g = {start_state: 0.0}
    parent = {start_state: None}
    f_start = heuristic(start_state, goal_state)
    heapq.heappush(open_heap, (f_start, start_state))

    closed = set()

    while open_heap:
        f_curr, curr = heapq.heappop(open_heap)
        if curr in closed:
            continue
        closed.add(curr)

        if (curr[0], curr[1]) == (goal_state[0], goal_state[1]):
            # reached goal cell (ignoring exact orientation)
            path = []
            s = curr
            while s is not None:
                path.append(s)
                s = parent[s]
            path.reverse()
            return path, g[curr]

        for nbr, cost in successors(curr):
            if nbr in closed:
                continue
            new_g = g[curr] + cost
            if new_g < g.get(nbr, float("inf")):
                g[nbr] = new_g
                parent[nbr] = curr
                f_nbr = new_g + heuristic(nbr, goal_state)
                heapq.heappush(open_heap, (f_nbr, nbr))

    return None, float("inf")

# Example usage
start = (2, 2, angle_index(0.0))
goal = (40, 20, angle_index(0.0))
path, cost = astar(start, goal)
print("Path length:", len(path), "Total cost:", cost)
      

This implementation demonstrates the basic pattern: local motion primitives provide successors, a geometric heuristic guides A*, and the result is a sequence of states, each linked by a dynamically feasible primitive.

The same logic can be implemented efficiently in C++ using the STL priority_queue. Below is a condensed version focusing on data structures and the A* loop.

#include <cmath>
#include <queue>
#include <vector>
#include <limits>
#include <unordered_map>
#include <tuple>

struct State {
  int ix, iy, kth;
};

struct Node {
  State s;
  double g;
  double f;
};

struct NodeCmp {
  bool operator()(const Node& a, const Node& b) const {
    return a.f > b.f; // min-heap via >
  }
};

struct StateHash {
  std::size_t operator()(State const& s) const noexcept {
    return ((s.ix * 73856093) ^ (s.iy * 19349663) ^ (s.kth * 83492791));
  }
};

struct StateEq {
  bool operator()(State const& a, State const& b) const noexcept {
    return a.ix == b.ix && a.iy == b.iy && a.kth == b.kth;
  }
};

static const int NX = 50;
static const int NY = 30;
static const int N_THETA = 16;
static const double DX = 1.0;
static const double DY = 1.0;
static const double L = 2.0;
static const double V = 1.0;
static const double DT = 1.0;

// A tiny occupancy grid for demo
int occ_grid[NX][NY] = {0};

inline bool in_bounds(int ix, int iy) {
  return 0 <= ix && ix < NX && 0 <= iy && iy < NY;
}

inline bool is_free(int ix, int iy) {
  return in_bounds(ix, iy) && occ_grid[ix][iy] == 0;
}

int angle_index(double theta) {
  double two_pi = 2.0 * M_PI;
  theta = fmod(theta + two_pi, two_pi);
  int k = static_cast<int>(std::round(theta / two_pi * N_THETA)) % N_THETA;
  return k;
}

double angle_from_index(int k) {
  return 2.0 * M_PI * k / N_THETA;
}

struct Primitive {
  double dx_ref;
  double dy_ref;
  double dth_ref;
  double length;
};

std::vector<Primitive> make_primitives() {
  std::vector<Primitive> prims;
  double steer[3] = {-0.4, 0.0, 0.4};
  for (int i = 0; i < 3; ++i) {
    double phi = steer[i];
    double kappa = std::tan(phi) / L;
    Primitive p;
    if (std::fabs(kappa) < 1e-6) {
      p.dx_ref = V * DT;
      p.dy_ref = 0.0;
      p.dth_ref = 0.0;
      p.length = V * DT;
    } else {
      p.dth_ref = V * kappa * DT;
      double th_end = p.dth_ref;
      p.dx_ref = (1.0 / kappa) * std::sin(th_end);
      p.dy_ref = -(1.0 / kappa) * (std::cos(th_end) - 1.0);
      p.length = std::fabs(p.dth_ref / kappa);
    }
    prims.push_back(p);
  }
  return prims;
}

double heuristic(State const& s, State const& goal) {
  double x = s.ix * DX;
  double y = s.iy * DY;
  double xg = goal.ix * DX;
  double yg = goal.iy * DY;
  return std::hypot(x - xg, y - yg);
}

int main() {
  std::vector<Primitive> prims = make_primitives();

  State start{2, 2, angle_index(0.0)};
  State goal{40, 20, angle_index(0.0)};

  std::priority_queue<Node, std::vector<Node>, NodeCmp> open;
  std::unordered_map<State, double, StateHash, StateEq> g;
  std::unordered_map<State, State, StateHash, StateEq> parent;

  g[start] = 0.0;
  parent[start] = start;
  open.push(Node{start, 0.0, heuristic(start, goal)});

  std::unordered_map<State, bool, StateHash, StateEq> closed;

  while (!open.empty()) {
    Node cur = open.top();
    open.pop();

    if (closed[cur.s]) continue;
    closed[cur.s] = true;

    if (cur.s.ix == goal.ix && cur.s.iy == goal.iy) {
      // Reconstruct and output path if desired
      break;
    }

    double theta = angle_from_index(cur.s.kth);
    for (auto const& p : prims) {
      double gx = cur.s.ix * DX;
      double gy = cur.s.iy * DY;
      double dx = std::cos(theta) * p.dx_ref - std::sin(theta) * p.dy_ref;
      double dy = std::sin(theta) * p.dx_ref + std::cos(theta) * p.dy_ref;
      double gx2 = gx + dx;
      double gy2 = gy + dy;
      double th2 = theta + p.dth_ref;

      State s2;
      s2.ix = static_cast<int>(std::round(gx2 / DX));
      s2.iy = static_cast<int>(std::round(gy2 / DY));
      s2.kth = angle_index(th2);

      if (!is_free(s2.ix, s2.iy)) continue;

      double new_g = g[cur.s] + p.length;
      auto it = g.find(s2);
      if (it == g.end() || new_g < it->second) {
        g[s2] = new_g;
        parent[s2] = cur.s;
        double f = new_g + heuristic(s2, goal);
        open.push(Node{s2, new_g, f});
      }
    }
  }

  return 0;
}
      

This code omits explicit path reconstruction and collision sampling along primitives, but it illustrates how the structured lattice and motion primitives fit naturally into the A* framework.

In Java, we can encapsulate the lattice planner into a class with an inner Node representing states and using a PriorityQueue for the open set.

import java.util.*;
import static java.lang.Math.*;

public class LatticePlanner {

    static final int NX = 50, NY = 30, N_THETA = 16;
    static final double DX = 1.0, DY = 1.0, L = 2.0, V = 1.0, DT = 1.0;

    static int[][] occGrid = new int[NX][NY];

    static class State {
        int ix, iy, kth;
        State(int ix, int iy, int kth) {
            this.ix = ix; this.iy = iy; this.kth = kth;
        }
        @Override public boolean equals(Object o) {
            if (!(o instanceof State)) return false;
            State s = (State)o;
            return ix == s.ix && iy == s.iy && kth == s.kth;
        }
        @Override public int hashCode() {
            return ix * 73856093 ^ iy * 19349663 ^ kth * 83492791;
        }
    }

    static class Node implements Comparable<Node> {
        State s;
        double g, f;
        Node(State s, double g, double f) {
            this.s = s; this.g = g; this.f = f;
        }
        public int compareTo(Node other) {
            return Double.compare(this.f, other.f);
        }
    }

    static class Primitive {
        double dxRef, dyRef, dthRef, length;
        Primitive(double dx, double dy, double dth, double len) {
            dxRef = dx; dyRef = dy; dthRef = dth; length = len;
        }
    }

    static boolean inBounds(int ix, int iy) {
        return ix >= 0 && ix < NX && iy >= 0 && iy < NY;
    }

    static boolean isFree(int ix, int iy) {
        return inBounds(ix, iy) && occGrid[ix][iy] == 0;
    }

    static int angleIndex(double theta) {
        double twoPi = 2.0 * PI;
        theta = (theta + twoPi) % twoPi;
        int k = (int) Math.round(theta / twoPi * N_THETA) % N_THETA;
        return k;
    }

    static double angleFromIndex(int k) {
        return 2.0 * PI * k / N_THETA;
    }

    static List<Primitive> makePrimitives() {
        double[] steer = {-0.4, 0.0, 0.4};
        List<Primitive> prims = new ArrayList<>();
        for (double phi : steer) {
            double kappa = Math.tan(phi) / L;
            if (Math.abs(kappa) < 1e-6) {
                prims.add(new Primitive(V * DT, 0.0, 0.0, V * DT));
            } else {
                double dth = V * kappa * DT;
                double thEnd = dth;
                double dx = (1.0 / kappa) * Math.sin(thEnd);
                double dy = -(1.0 / kappa) * (Math.cos(thEnd) - 1.0);
                double length = Math.abs(dth / kappa);
                prims.add(new Primitive(dx, dy, dth, length));
            }
        }
        return prims;
    }

    static double heuristic(State s, State goal) {
        double x = s.ix * DX;
        double y = s.iy * DY;
        double xg = goal.ix * DX;
        double yg = goal.iy * DY;
        return Math.hypot(x - xg, y - yg);
    }

    public static List<State> astar(State start, State goal) {
        List<Primitive> prims = makePrimitives();
        PriorityQueue<Node> open = new PriorityQueue<>();
        Map<State, Double> g = new HashMap<>();
        Map<State, State> parent = new HashMap<>();
        Set<State> closed = new HashSet<>();

        g.put(start, 0.0);
        parent.put(start, null);
        open.add(new Node(start, 0.0, heuristic(start, goal)));

        while (!open.isEmpty()) {
            Node cur = open.poll();
            if (closed.contains(cur.s)) continue;
            closed.add(cur.s);

            if (cur.s.ix == goal.ix && cur.s.iy == goal.iy) {
                List<State> path = new ArrayList<>();
                State s = cur.s;
                while (s != null) {
                    path.add(s);
                    s = parent.get(s);
                }
                Collections.reverse(path);
                return path;
            }

            double theta = angleFromIndex(cur.s.kth);
            for (Primitive p : prims) {
                double gx = cur.s.ix * DX;
                double gy = cur.s.iy * DY;
                double dx = Math.cos(theta) * p.dxRef - Math.sin(theta) * p.dyRef;
                double dy = Math.sin(theta) * p.dxRef + Math.cos(theta) * p.dyRef;
                double gx2 = gx + dx;
                double gy2 = gy + dy;
                double th2 = theta + p.dthRef;

                int ix2 = (int) Math.round(gx2 / DX);
                int iy2 = (int) Math.round(gy2 / DY);
                int kth2 = angleIndex(th2);
                if (!isFree(ix2, iy2)) continue;

                State s2 = new State(ix2, iy2, kth2);
                double newG = g.get(cur.s) + p.length;
                if (!g.containsKey(s2) || newG < g.get(s2)) {
                    g.put(s2, newG);
                    parent.put(s2, cur.s);
                    double f = newG + heuristic(s2, goal);
                    open.add(new Node(s2, newG, f));
                }
            }
        }
        return null;
    }

    public static void main(String[] args) {
        State start = new State(2, 2, angleIndex(0.0));
        State goal = new State(40, 20, angleIndex(0.0));
        List<State> path = astar(start, goal);
        if (path != null) {
            System.out.println("Found path of length " + path.size());
        } else {
            System.out.println("No path found.");
        }
    }
}
      

This Java code shows a direct translation of the lattice planning logic, suitable for integration into larger robotics frameworks.

MATLAB is frequently used in robotics for rapid prototyping. Below we show a core MATLAB implementation of a lattice planner and outline how to connect it to Simulink for executing the resulting trajectory.

function demo_lattice_planner
    % Grid parameters
    DX = 1.0; DY = 1.0; N_THETA = 16;
    NX = 50; NY = 30;
    L = 2.0; V = 1.0; DT = 1.0;

    occ = zeros(NX, NY);  % binary occupancy

    start = [2, 2, angle_index(0.0, N_THETA)];
    goal  = [40, 20, angle_index(0.0, N_THETA)];

    prims = make_primitives(L, V, DT);

    [path, cost] = astar_lattice(start, goal, prims, occ, DX, DY, N_THETA);
    fprintf('Path length = %d, cost = %.3f\n', size(path,1), cost);

    % To connect with Simulink:
    % 1) Export the sequence of continuous poses for each primitive.
    % 2) Use a 'From Workspace' block feeding a trajectory-following controller.

end

function kth = angle_index(theta, N_THETA)
    theta = mod(theta + 2*pi, 2*pi);
    kth = round(theta / (2*pi) * N_THETA);
    kth = mod(kth, N_THETA);
end

function theta = angle_from_index(kth, N_THETA)
    theta = 2*pi * kth / N_THETA;
end

function prims = make_primitives(L, V, DT)
    steer = [-0.4, 0.0, 0.4];
    prims = struct('dx_ref', {}, 'dy_ref', {}, 'dth_ref', {}, 'length', {});
    for i = 1:numel(steer)
        phi = steer(i);
        kappa = tan(phi) / L;
        if abs(kappa) < 1e-6
            dx = V * DT; dy = 0.0; dth = 0.0;
            len = V * DT;
        else
            dth = V * kappa * DT;
            thEnd = dth;
            dx = (1.0 / kappa) * sin(thEnd);
            dy = -(1.0 / kappa) * (cos(thEnd) - 1.0);
            len = abs(dth / kappa);
        end
        prims(i).dx_ref = dx;
        prims(i).dy_ref = dy;
        prims(i).dth_ref = dth;
        prims(i).length = len;
    end
end

function [path, total_cost] = astar_lattice(start, goal, prims, occ, DX, DY, N_THETA)
    NX = size(occ,1); NY = size(occ,2);

    function b = is_free(ix, iy)
        b = (ix >= 1) && (ix <= NX) && (iy >= 1) && (iy <= NY) ...
            && (occ(ix,iy) == 0);
    end

    function h = heuristic(s)
        x = s(1) * DX; y = s(2) * DY;
        xg = goal(1) * DX; yg = goal(2) * DY;
        h = hypot(x - xg, y - yg);
    end

    % open set: columns [ix, iy, kth, g, f]
    open = [start, 0.0, heuristic(start)];
    gmap = containers.Map();
    parent = containers.Map('KeyType','char','ValueType','char');

    key_start = key_of(start);
    gmap(key_start) = 0.0;

    closed = containers.Map('KeyType','char','ValueType','logical');

    while ~isempty(open)
        % pick node with smallest f
        [~, idx] = min(open(:,5));
        curr = open(idx, 1:3);
        gcurr = open(idx, 4);
        open(idx,:) = [];

        kcurr = key_of(curr);
        if isKey(closed, kcurr), continue; end
        closed(kcurr) = true;

        if curr(1) == goal(1) && curr(2) == goal(2)
            % reconstruct path
            path = curr;
            while ~strcmp(kcurr, key_start)
                p = str2num(parent(kcurr)); %#ok<ST2NM>
                path = [p; path]; %#ok<AGROW>
                kcurr = key_of(p);
            end
            total_cost = gcurr;
            return;
        end

        theta = angle_from_index(curr(3), N_THETA);
        for i = 1:numel(prims)
            p = prims(i);
            gx = curr(1) * DX; gy = curr(2) * DY;
            dx = cos(theta)*p.dx_ref - sin(theta)*p.dy_ref;
            dy = sin(theta)*p.dx_ref + cos(theta)*p.dy_ref;
            gx2 = gx + dx; gy2 = gy + dy;
            th2 = theta + p.dth_ref;
            ix2 = round(gx2 / DX);
            iy2 = round(gy2 / DY);
            kth2 = angle_index(th2, N_THETA);
            if ~is_free(ix2, iy2), continue; end
            s2 = [ix2, iy2, kth2];

            key2 = key_of(s2);
            new_g = gcurr + p.length;
            if ~isKey(gmap, key2) || new_g < gmap(key2)
                gmap(key2) = new_g;
                parent(key2) = kcurr;
                f = new_g + heuristic(s2);
                open = [open; s2, new_g, f]; %#ok<AGROW>
            end
        end
    end

    path = [];
    total_cost = inf;
end

function k = key_of(s)
    % string key for map
    k = sprintf('%d,%d,%d', s(1), s(2), s(3));
end
      

In Simulink, one would typically:

1. Generate a time-parameterized trajectory from the sequence of lattice states.
2. Feed it to a trajectory-tracking controller in Simulink using a From Workspace block.
3. Use the car-like kinematic model as the plant for closed-loop simulation.

Mathematica is convenient for symbolic manipulation of motion primitives and quick prototyping of planning algorithms using its graph library.

(* Discretization parameters *)
dx = 1.0; dy = 1.0; nTheta = 16;
v = 1.0; L = 2.0; dt = 1.0;

angleIndex[theta_] := Module[{th = Mod[theta + 2. Pi, 2. Pi]},
  Mod[Round[th/(2. Pi) nTheta], nTheta]
];

angleFromIndex[k_] := 2. Pi k/nTheta;

(* Reference motion primitives *)
steerAngles = {-0.4, 0., 0.4};

makePrimitive[phi_] := Module[{kappa, dth, thEnd, dxRef, dyRef, len},
  kappa = Tan[phi]/L;
  If[Abs[kappa] < 10^-6,
    dxRef = v dt; dyRef = 0.; dth = 0.; len = v dt,
    dth = v kappa dt;
    thEnd = dth;
    dxRef = (1./kappa) Sin[thEnd];
    dyRef = -(1./kappa) (Cos[thEnd] - 1.);
    len = Abs[dth/kappa];
  ];
  <<|"dxRef" -> dxRef, "dyRef" -> dyRef, "dthRef" -> dth, "len" -> len|>>
];

primitives = makePrimitive /@ steerAngles;

(* Successor generation from state {ix, iy, kth} *)
successors[{ix_, iy_, kth_}] := Module[
  {theta, gx, gy, succs},
  theta = angleFromIndex[kth];
  gx = ix dx; gy = iy dy;
  succs = Table[
    With[{p = primitives[[i]]},
      Module[{dxg, dyg, gx2, gy2, th2, ix2, iy2, kth2},
        dxg = Cos[theta] p["dxRef"] - Sin[theta] p["dyRef"];
        dyg = Sin[theta] p["dxRef"] + Cos[theta] p["dyRef"];
        gx2 = gx + dxg; gy2 = gy + dyg;
        th2 = theta + p["dthRef"];
        ix2 = Round[gx2/dx];
        iy2 = Round[gy2/dy];
        kth2 = angleIndex[th2];
        { {ix2, iy2, kth2}, p["len"] }
      ]
    ],
    {i, Length[primitives]}
  ];
  succs
];

heuristic[{ix_, iy_, _}, {gx_, gy_, _}] :=
  Module[{x, y, xg, yg},
    x = ix dx; y = iy dy;
    xg = gx dx; yg = gy dy;
    Sqrt[(x - xg)^2 + (y - yg)^2]
  ];

(* A* implemented with a simple loop and Association-based priority queue. *)
astar[start_, goal_] := Module[
  {open, g, parent, closed, popMin, pushNode, kStart, path},
  open = Association[];
  g = Association[];
  parent = Association[];
  closed = Association[];

  kStart = start;
  g[kStart] = 0.;
  parent[kStart] = None;
  open[kStart] = heuristic[start, goal];

  popMin[] := Module[{kmin},
    If[Length[open] == 0, Return[None]];
    kmin = First@First@SortBy[Normal[open], Last];
    open = KeyDrop[open, kmin];
    kmin
  ];

  pushNode[s_, f_] := (open[s] = f);

  While[Length[open] > 0,
    Module[{cur = popMin[], gcurr, s, th, succ},
      If[cur === None, Break[]];
      If[KeyExistsQ[closed, cur], Continue[]];
      closed[cur] = True;
      If[cur[[1 ;; 2]] === goal[[1 ;; 2]],
        (* reconstruct path *)
        path = {cur};
        s = cur;
        While[parent[s] =!= None,
          s = parent[s];
          path = Prepend[path, s];
        ];
        Return[{path, g[cur]}];
      ];
      gcurr = g[cur];
      succ = successors[cur];
      Do[
        Module[{s2 = succ[[i, 1]], cost = succ[[i, 2]], key2, newg, f},
          key2 = s2;
          newg = gcurr + cost;
          If[!KeyExistsQ[g, key2] || newg < g[key2],
            g[key2] = newg;
            parent[key2] = cur;
            f = newg + heuristic[s2, goal];
            pushNode[key2, f];
          ];
        ],
        {i, Length[succ]}
      ];
    ];
  ];
  { {}, Infinity }
];

start = {2, 2, angleIndex[0., nTheta]};
goal = {40, 20, angleIndex[0., nTheta]};
{path, cost} = astar[start, goal];
Print["Path length: ", Length[path], "  Cost: ", cost];
      

Mathematica’s Graph functions can also be used by explicitly constructing the lattice as a weighted graph and applying built-in shortest path algorithms, but the explicit A* implementation above mirrors the algorithms from previous lessons.

Problem 1 (Constant-Curvature Primitive Derivation). For the car-like robot model in Section 3 with constant speed \( v \) and constant steering angle \( \phi \), derive the endpoint formulas for \( x(T_p), y(T_p), \theta(T_p) \) starting from \( (x(0), y(0), \theta(0)) \).

Solution. We have curvature \( \kappa = \frac{\tan\phi}{L} \). The heading evolves as

\[ \dot{\theta}(t) = v \kappa \;\Rightarrow\; \theta(t) = \theta(0) + v\kappa t, \]

so \( \theta(T_p) = \theta(0) + v\kappa T_p \). For position:

\[ \dot{x}(t) = v\cos\theta(t), \quad \dot{y}(t) = v\sin\theta(t). \]

Substitute \( \theta(t) = \theta(0) + v\kappa t \) and integrate:

\[ x(T_p) - x(0) = \int_0^{T_p} v\cos(\theta(0) + v\kappa t)\, dt = \frac{1}{\kappa}\big(\sin(\theta(0) + v\kappa T_p) - \sin(\theta(0))\big), \]

\[ y(T_p) - y(0) = \int_0^{T_p} v\sin(\theta(0) + v\kappa t)\, dt = -\frac{1}{\kappa}\big(\cos(\theta(0) + v\kappa T_p) - \cos(\theta(0))\big). \]

Thus:

\[ \begin{aligned} x(T_p) &= x(0) + \frac{1}{\kappa}\big(\sin(\theta(0) + v\kappa T_p) - \sin(\theta(0))\big), \\ y(T_p) &= y(0) - \frac{1}{\kappa}\big(\cos(\theta(0) + v\kappa T_p) - \cos(\theta(0))\big), \\ \theta(T_p) &= \theta(0) + v\kappa T_p, \end{aligned} \]

which matches the formulas used in the code implementations.

Problem 2 (Admissibility of Euclidean Heuristic). Assume each motion primitive in the lattice has cost equal to its Euclidean length in the workspace, i.e., \( c(e) = \|\Delta(x,y)\|_2 \) for edge \( e \). Show that the heuristic \( h(v) = \) Euclidean distance in (x,y) to the goal is admissible and consistent for A* on the lattice.

Solution. Denote by \( d^*(v) \) the optimal discrete path cost from vertex \( v \) to the goal. Any path from \( v \) to the goal is a sequence of edges whose Euclidean displacements sum to a vector whose norm is at least the straight-line distance from \( v \) to goal. Thus

\[ d^*(v) \geq \text{EuclideanDistance}(v, \text{goal}) = h(v), \]

so \( h \) is admissible. For consistency, take any edge \( (v,v') \) with cost \( c(v,v') = \|\Delta(x,y)\| \). By the triangle inequality for the Euclidean norm:

\[ h(v) \leq c(v,v') + h(v'), \]

because the straight-line segment from \( v \) to the goal cannot be longer than the segment from \( v \) to \( v' \) plus the segment from \( v' \) to the goal. Hence \( h \) is consistent.

Problem 3 (Resolution Completeness Sketch). Suppose the system dynamics are Lipschitz in state and control, and there exists a feasible continuous trajectory from start to goal with clearance at least \( \delta > 0 \) from obstacles. Argue informally that a lattice planner with sufficiently fine discretization in space and heading will find a solution.

Solution. Partition the continuous trajectory into segments of length at most \( \ell \) such that within each segment the curvature and heading variation are bounded (possible because of Lipschitz continuity). Choose lattice spacings \( \Delta x, \Delta y, \Delta\theta \) sufficiently small so that for any state along the trajectory there exists a lattice vertex within distance < \( \delta/2 \), and the set of motion primitives is rich enough to approximate the local motion over one segment up to error < \( \delta/2 \).

Then for each segment we can construct a primitive sequence starting and ending at nearby lattice vertices that stays within \( \delta \) of the original trajectory, thus remaining collision-free. Concatenating these primitive sequences yields a discrete lattice path from a vertex near the start to one near the goal, proving resolution completeness at that discretization.

Problem 4 (Complexity Estimate for Lattice Search). Consider a rectangular region of size \( W \times H \) with position grid steps \( \Delta x, \Delta y \) and \( N_\theta \) heading discretization. Assume each lattice state has exactly \( b \) outgoing motion primitives. Estimate the worst-case number of nodes expanded by Dijkstra’s algorithm in terms of these parameters.

Solution. The number of grid cells is approximately

\[ N_{\text{cells}} \approx \frac{W}{\Delta x} \cdot \frac{H}{\Delta y}. \]

For each cell there are \( N_\theta \) heading states, so the total number of vertices is

\[ |V| \approx N_{\text{cells}} \cdot N_\theta = \frac{W H}{\Delta x \Delta y} N_\theta. \]

In the worst case (no early termination), Dijkstra will insert each vertex into the priority queue and relax all its outgoing edges at least once, resulting in \( O(|V|) \) expansions and \( O(b|V|) \) edge relaxations. Using a binary heap, operations on the queue cost \( O(\log |V|) \), so the total worst-case complexity is

\[ O\big(|V|\log|V| + b|V|\log|V|\big) = O\big(b|V|\log|V|\big), \]

which scales linearly with \( N_\theta \) and inversely with \( \Delta x \Delta y \).

In this lesson we systematically constructed lattice planners that embed robot kinematics and simple dynamics into graph search via motion primitives. We defined state lattices, derived dynamically feasible primitives for car-like robots, connected lattice search with heuristic search theory, and discussed resolution completeness and complexity. Multi-language implementations (Python, C++, Java, MATLAB/Simulink, and Mathematica) demonstrated how these concepts translate into practical planners that can be integrated with low-level controllers for high-DOF robots.