Source-linked AI summary
Path Planning with Motion Primitives in Dynamic Environments: SIPP on Lattices
Marat Agranovskiy
TL;DR
Dynamic path planning must account for other agents’ known trajectories, while lattice motion primitives offer kinematically feasible paths but complicate collision checking. The paper combines state lattices with SIPP and rasterizes spatiotemporal swept volumes; it reports that enriched lattices can match highly connected grids’ routing metrics while producing smooth, executable paths.
Problem
Dynamic path planning must find safe, efficient, physically executable paths around other agents with known trajectories, while continuous collision checking for lattice primitives is difficult.
Source: §1 · p. 2Method
The method combines SIPP with state-lattice motion primitives and rasterizes the spatiotemporal swept volumes of obstacles and robot motions onto the grid.
Source: §1 · pp. 2–3Results
Enriching the lattice control set can match highly connected grids’ routing metrics while guaranteeing smooth, physically executable trajectories.
Source: §1 · p. 3, §4 · p. 14Takeaways & Limitations
Lattice primitives yield smooth, physically executable paths, while the expanded state space increases search time relative to grid variants.
Source: §3.3 · p. 11, §4 · p. 14The authors identify increased state-space dimensionality as a search-time cost and plan future closed-loop physical validation and MAPF extensions.
Source: §4 · p. 14
Abstract
from arXiv · showhide
Autonomous navigation in dynamic environments is a critical challenge, particularly when spaces are shared with other mobile agents whose future trajectories are known. While traditional grid-based planners efficiently find collision-free paths, their reliance on stop-and-turn mechanics over $2^k$-connected grids produces piecewise-linear trajectories that are kinodynamically highly sub-optimal for differentially constrained robots. In this paper, we present an adaptation of Safe Interval Path Planning (SIPP) that operates on state lattices, utilizing precomputed, kinodynamically smooth motion primitives. To efficiently handle dynamic environments, we rasterize the spatiotemporal swept volumes of moving obstacles directly onto the grid, treating grid cells as atomic units of space, whose resolution is typically dictated by inherent localization noise. We perform a comprehensive comparative analysis between our lattice-based approach and $2^k$-connected grid planners across diverse topological environments. Our evaluation considers a broad spectrum of performance metrics, including planning time, path angularity, cumulative heading change (angle-over-length), and bending energy. The results demonstrate that while the expanded state space of lattice-based search increases computational overhead, it yields trajectories with significantly superior kinodynamic properties. Specifically, our method achieves a reachability comparable to highly connected grids while ensuring smooth, continuous, and physically executable paths ready for real-world deployment.
1 Introduction
The paper addresses dynamic path planning for robots in shared environments, where known trajectories of other agents become moving obstacles. It combines lattice motion primitives with SIPP and grid-based spatiotemporal rasterization to seek smooth, executable paths without temporal sampling.
1 Introduction
- Known trajectories of other agents can be treated as dynamic obstacles, leaving a computationally demanding spatiotemporal search for one agent.This setting arises in shared workspaces such as automated warehouses and multi-robot logistics hubs.Source: §1 · p. 2
- State lattices use precomputed, kinematically compliant curves to connect discrete states, but moving obstacles make continuous collision verification difficult.Temporal sampling adds computational overhead and risks missing collisions between checks.Source: §1 · p. 2
- The method rasterizes the swept volumes of moving obstacles and robot primitives onto grid cells instead of continuously evaluating or sampling curves.The paper treats grid cells as atomic units, with resolution tied to sensor noise and localization uncertainty.Source: §1 · p. 2
- Lattice-based SIPP combines kinematically feasible state lattices with safe-interval search for dynamic path planning.The evaluation compares it with straight-line planners on 2^k-connected grids, focusing on search time, routing efficiency, bending energy, and trajectory angularity.Source: §1 · p. 3
- The authors report that an expressive lattice control set can match highly connected grids’ path lengths while producing smooth, physically executable trajectories.Their comparison includes bending energy and trajectory angularity alongside search time and spatial routing efficiency.Source: §1 · p. 3
2 The Main Part
The method represents robot configurations and obstacle occupancy on a grid, then searches over lattice motion primitives and safe time intervals. Its collision checks use rasterized primitive traces and bounded searches for safe departure times.
2.1 Problem Statement
- Static obstacles use a binary occupancy grid, while dynamic obstacles occupy cell-specific time intervals computed by dilating and rasterizing their swept volumes.Dynamic obstacles are dilated by the agent’s radius, allowing the circular agent to be treated as a point when querying dynamic occupancy.Source: §2.1 · p. 3
- Grid cells act as atomic space units; static safety checks account for the robot’s radius, while dynamic checks query the pre-dilated obstacle intervals at its center cell.This discretization assumes the agent’s center is localized at the cell center, with small discrepancies absorbed by conservative radius padding.Source: §2.1 · pp. 3–4
- Motion primitives are short, kinodynamically feasible transitions between discrete states, with execution time as their cost.The state and primitive space is regular under parallel translation, and the control set is finite and computed offline; Fig. 2 shows 112 base primitives and 384 in the extended set.Source: §2.1 · p. 4, Figure 2 · p. 8
- The problem is to find a collision-free sequence of motions, optionally with waits, from a start configuration and time to a goal, minimizing arrival time.The agent is represented by grid-cell position and a discretized heading.Source: §2.1 · pp. 3, 5
2.2 Safe Interval Path Planning with Motion Primitives
- A SIPP state pairs a discrete robot state with a safe interval, a maximal contiguous period when its cell is unoccupied by moving obstacles.The minimum arrival time is the search g-value, and the robot may wait within its current safe interval before departing.Source: §2.2 · p. 5
- For each primitive and target safe interval, successor generation searches a bounded departure window for the earliest collision-free departure.The search accounts for clearing the starting cell before its interval closes and arriving while the target interval remains open; occupied intervals along the primitive trace are used to find invalid departure blocks.Source: §2.2 · pp. 5–7
3 Results
The evaluation benchmarks six motion-primitive sets for a simulated differential-drive robot and compares search efficiency with trajectory geometry. The algorithm set spans smooth lattice primitives and straight-line grid neighborhoods.
3.1 Evaluated Algorithms
- The benchmark includes SIPP-Basic, SIPP-Extended, and SIPP-2^k variants for k = 2, 3, 4, and 5.The lattice sets use smooth transitions in (i, j, θ), while grid variants use straight-line transitions.Source: §3.1 · p. 8
- Grid-2^k variants search a 2D spatial state, whereas lattice variants search position and orientation in a 3D state.The grid planners assume movement to any valid neighbor regardless of current orientation.Source: §3.1 · p. 8
3.2 Evaluation Metrics
- The evaluation compares search efficiency and the geometric quality of resulting spatial trajectories.The stated computational-time metric measures the A* expansion loop’s time to find the optimal path.Source: §3.2 · p. 9
1. Search Time
This section defines search time as the computational cost of the A* expansion loop.
1. Search Time
- Search time is the A* expansion loop’s computational time to find the optimal path.Source: §1 · p. 9
2. Path Cost
In SIPP, total execution time includes both travel along motion primitives and waiting for dynamic obstacles.
2. Path Cost
- Total execution time combines traversal along primitives with the waiting needed to yield to dynamic obstacles.Source: §2 · p. 9
3. Trajectory Angularity (A)
Trajectory angularity measures path jaggedness by summing discrete curvature values computed from sampled point triplets.
3. Trajectory Angularity (A)
4. Angle-Over-Length (AOL)
Angle-over-length quantifies cumulative angular displacement, using curvature for smooth paths and segment-junction angles for grid paths.
4. Angle-Over-Length (AOL)
- For a smooth lattice path, cumulative angular displacement is the integral of absolute curvature along the path.The heading change per unit arc length is the signed curvature.Source: §4 · p. 9
- For piecewise-linear grid paths, angle-over-length is computed as the sum of absolute angles between consecutive segments.The heading derivative is zero along segments and concentrated at junctions.Source: §4 · p. 9
5. Bending Energy (Ebend)
Lattice primitives sharply reduce bending energy relative to grid variants, at the cost of higher search time; the evaluation uses a minimal-turn-radius model for grid paths.
5. Bending Energy (Ebend)
- Grid-path bending energy models each point-turn as a rotation at the minimal feasible radius, then sums the discrete turn penalties.The assumed maximal inscribed circle has Rmin = 0.5 for the stated grid resolution.Source: §5 · p. 10
3.3 Empirical Evaluation
- The Denver evaluation uses three map types and a circular robot of radius one grid cell; start and goal headings are relaxed for comparison.The maps are Denver, arena, and empty 64 × 64.Source: §3.3 · p. 10
- Dynamic obstacles are generated as randomized DFS walks over the state lattice, including overlapping, chaotic spatiotemporal constraints.The authors describe this as a deliberate worst-case stress test.Source: §3.3 · p. 10
- Lattice-based algorithms incur higher computational costs than Grid-2^k variants because search expands heading as well as spatial states.The lattice search operates over (i, j, θ), versus (i, j) for the grid planners.Source: §3.3 · p. 11
- Lattice algorithms show a massive reduction in bending energy compared with all Grid-2^k variants.The comparison is reported for the Denver 1 256 map in Table 1.Source: §3.3 · p. 11, Table 1 · p. 11
4 Conclusion
The lattice-based SIPP approach rasterizes moving obstacles’ spatiotemporal swept volumes for sampling-free collision checking. Enriching its motion-primitive control set matches highly connected grids’ routing metrics while producing smooth, executable trajectories, at the cost of increased search time.
4 Conclusion
- Rasterizing moving obstacles’ spatiotemporal swept volumes onto the grid enables efficient, sampling-free collision checking.Source: §4 · p. 14
- Lattice control-set enrichment matches highly connected grids’ routing metrics while guaranteeing smooth, physically executable trajectories.Highly connected grids perform exceptionally well on spatial metrics but produce kinematically unfeasible, jagged paths.Source: §4 · p. 14
- The state lattice’s increased dimensionality impacts search time; future work proposes primitive-bundle evaluation and closed-loop physical validation.The proposed validation would compare against planners such as TEB or MPC.Source: §4 · p. 14