M4-T6 — Hybrid A* and State Lattices

Learning objectives

After this lesson, you should be able to:

• Describe a search state as a pose and an edge as a motion primitive generated by the vehicle model.
• Explain how Hybrid A* keeps one node per cell of a discretised (x, y, θ) space while the states themselves stay continuous.
• Build a heuristic from the maximum of a non-holonomic and an obstacle-aware estimate, and say why it is admissible.
• State what Hybrid A* guarantees (drivability) and what it does not (completeness, optimality).

The intuition

Grid A* thinks in squares. Move from this square to a neighbour: the cost is 1 or √2, the heading is whatever the move happens to be. For a robot that spins on the spot this is fine. For a tug that needs 1.2 m to turn, the route “right, then up, then right” has two sharp corners that it can only cut or fail to reach.

The fix is to let the search move the way the vehicle moves. From a pose, try a few steering commands for a short distance, say hard left, straight, hard right, using the vehicle model of lesson M1-T3. Each gives a new pose with a new heading. The new poses are the neighbours. The sequence of poses that the search returns is a path that the vehicle can follow without corners, because it was generated by the vehicle.

The state is now three numbers, so there are many more of them. To keep the search finite, the space is divided into cells of (x, y, θ), and a cell holds at most one node: the cheapest pose found in it.

The mathematics

State, primitives, cost

A node is a continuous pose (x, y, θ) with a cost-to-come g and a parent. Its neighbours come from applying each steering angle δ in a small set (for example −δ_max, 0, +δ_max, and the same in reverse if allowed) for an arc length s, using the bicycle model with wheelbase L:

The cost of a step is its length s, plus penalties that shape the result: for reversing, for changing direction, for changing the steering angle, and for being near obstacles.

One node per cell

Hybrid A* (Dolgov et al., 2010) discretises (x, y, θ) into cells, for example 0.25 m and 5°. Each cell stores at most one node. When a new node falls into an occupied cell, it replaces the old one only if its cost-to-come is lower, otherwise it is dropped. This is the closed-set check of A*, done on cells, and it keeps the number of expanded nodes bounded by the number of cells. The nodes themselves keep their exact continuous poses, so errors do not accumulate through rounding to cell centres.

Heuristics

Hybrid A* uses the larger of two estimates, each of which ignores part of the problem and so never overestimates:

The maximum of two admissible heuristics is admissible and at least as informed as either. The curve estimate stops the search from approaching the goal at the wrong angle; the 2D estimate stops it from running into a dead end that the curve cannot see.

Analytic expansion

The probability that a primitive lands exactly in the goal cell with the right heading is small. So every so often (every node near the goal, or every n-th node) the search tries a Dubins or Reeds-Shepp curve from the current node to the goal pose. If it is collision-free, the search ends with that curve appended (lab M4-L4, stage 3).

State lattices

A state lattice (Pivtoraiko, Knepper and Kelly, 2009) chooses the lattice of states first, usually a regular grid of positions and a few headings, and then pre-computes a small set of motion primitives that connect each state to nearby states exactly, ending on lattice states with the right heading and curvature. A search over the lattice is ordinary graph search, with the guarantees of graph search at the lattice resolution. The cost is the offline design of the primitives; the benefit is that edge costs and swept areas can be tabulated in advance.

What is and is not guaranteed

Every Hybrid A* path is drivable (it is built from arcs of the vehicle model, up to the cell merging and the final curve). The search is neither complete nor optimal: pruning to one node per cell can discard the only node that leads to the goal, and the retained path need not be the cheapest. It usually gives a good path fast, and a smoothing step then improves its curvature.

A worked example

One primitive. A tug with wheelbase L = 1.0 m steers at δ = 30° and drives an arc of s = 0.8 m.

The straight primitive ends 0.8 m ahead with the same heading. The three neighbours are 0.77 m, 0.80 m and 0.77 m ahead, and about 0.18 m to the left, 0 and 0.18 m to the right of the straight line, rotated by +26.5°, 0° and −26.5°. With a cell of 0.25 m and 5° the left and right ends fall in different cells and different heading bins.

Which cell? A node at (3.37, 5.12) with θ = 0.52 rad (29.8°) is in the cell with column ⌊3.37 / 0.25⌋ = 13, row ⌊5.12 / 0.25⌋ = 20 and heading bin ⌊0.52 / 0.0873⌋ = ⌊5.96⌋ = 5. A second node in the same cell, with a lower g, replaces it.

How big is the space? A 40 m × 40 m yard with 0.25 m cells and 72 heading bins has 160 × 160 × 72 = 1 843 200 cells. A plain 2D grid for the same yard has 25 600. The heuristic must therefore keep the search focused: a blind search would visit a large fraction of 1.8 million cells.

Combining heuristics. From the node, the curve length to the goal pose ignoring obstacles is 8.92 m (the LSL example of lesson M4-T5). A wall in front of the goal forces the 2D path to detour: say 11.4 m. Then h = max(8.92, 11.4) = 11.4 m: the 2D estimate dominates because the curve estimate did not see the wall. In an open yard the curve estimate dominates, because the 2D distance (the straight 8.54 m) is smaller. Neither would be enough alone.

Engineering implications and limitations

Choose the discretisation

The cell size and the number of heading bins decide the cost and the quality. Finer cells keep more candidate nodes and find shorter paths; coarser ones are faster and prune more. The cell should be small compared with the primitive length, and the heading bins fine enough that a primitive changes the heading bin, otherwise a node and its child fall in the same cell and the search stalls. Tune on the tightest manoeuvre, such as the bay (lab M4-L4).

Penalties are the personality

The weights for reversing, switching direction and steering changes decide how the vehicle behaves. A cost of 1 per metre forward and 2 per metre in reverse gives a planner that prefers to go forward; a fixed penalty per cusp stops it from shuffling. Log them with the path.

The 2D heuristic is cheap and valuable

One Dijkstra run from the goal over the costmap, 2D, gives the holonomic estimate for every cell at once. Recompute it when the goal or the map changes (module 3).

Smooth afterwards

The path is made of arcs from the discrete steering set and joins at the cell-merged nodes, so its curvature is piecewise constant and not perfectly smooth. A gradient-based smoother that keeps the path collision-free and within curvature limits is standard (module 5).

When to prefer a lattice

If the robot always works in the same kind of place and the same manoeuvres, a precomputed lattice is predictable and fast, and its swept areas can be checked against a static map ahead of time. Hybrid A* is more flexible and needs no offline design.

Common misconceptions

Engineering takeaways

1. Search over poses (x, y, θ); edges are motion primitives from the vehicle model, so the path is drivable by construction.
2. Hybrid A* keeps one node per cell of the discretised pose space and prefers the lower cost-to-come, with continuous poses kept inside.
3. The heuristic is the maximum of the Dubins or Reeds-Shepp length and the 2D obstacle-aware distance; both are admissible.
4. An analytic expansion to the goal pose finishes the search exactly; the method is neither complete nor optimal.
5. A state lattice designs the primitives offline so that they connect lattice states exactly and graph search gives guarantees at the lattice resolution.

References and further reading

The Hybrid A* papers and the state lattice paper.

• D. Dolgov, S. Thrun, M. Montemerlo and J. Diebel, “Path Planning for Autonomous Vehicles in Unknown Semi-structured Environments”, The International Journal of Robotics Research, 29(5), 2010.
• M. Pivtoraiko, R. A. Knepper and A. Kelly, “Differentially Constrained Mobile Robot Motion Planning in State Lattices”, Journal of Field Robotics, 26(3), 2009.
• P. E. Hart, N. J. Nilsson and B. Raphael, “A Formal Basis for the Heuristic Determination of Minimum Cost Paths”, IEEE Transactions on Systems Science and Cybernetics, 4(2), 1968.

Knowledge check

Three conceptual questions. Write an answer, then reveal the explanation. Your answers stay in your browser.

What’s next

Next: from a path to motion in time. Module 5 turns paths into trajectories and the commands that follow them.

Related content

Nothing below is required to finish this lesson.

• Before
  M4-T5: Dubins and Reeds-Shepp Curves

  Build the shortest paths for a car that turns with a minimum radius, with and without reversing.

• Lab
  M4-L4: Park the Car-Like Robot

  Reverse a tug into a charging bay with a Dubins and Reeds-Shepp path.