Kinodynamic SIPP-IP Planner

• The paper introduces Kinodynamic SIPP-IP, extending SIPP to incorporate waiting intervals and bounded acceleration, ensuring provable completeness and optimality.
• It employs interval projection through motion primitives to manage non-instantaneous starts and stops in environments with dynamic obstacles.
• Empirical evaluations demonstrate significantly reduced search space and faster runtimes compared to time-expanded A*, while maintaining optimal path solutions.

Kinodynamic SIPP-IP is a search-based planner designed for single-agent, graph-based pathfinding in environments with known dynamic obstacles, where the agent is subject to nontrivial kinodynamic constraints—in particular, bounded acceleration and deceleration, and non-instantaneous stopping and starting. By extending the classic Safe Interval Path Planning (SIPP) framework to explicitly handle waiting after deceleration, Kinodynamic SIPP-IP achieves provable completeness and optimality for a class of motion models where acceleration and deceleration limits are enforced, providing robust performance in challenging planning scenarios with timing-critical collision avoidance requirements (Ali et al., 2023).

1. Problem Setting and State Space Representation

Kinodynamic SIPP-IP operates over a directed graph $G = (V, E)$, where each vertex $v \in V$ represents a collision-free position, orientation, and a discrete velocity level $\mathit{vel}$. Only vertices with $\mathit{vel} = 0$ permit the agent to dwell (i.e., to "wait" by remaining stationary in both position and velocity). Edges $e = (v, u)$ correspond to precomputed kinodynamic motion primitives that encode transitions with specified durations $w(e)$, initial velocities $\mathit{vel}(v)$, final velocities $\mathit{vel}(u)$, and bounded acceleration magnitudes $a_{\max}$.

The agent's continuous-time dynamics between stops are expressed as: $\ddot x(t) = a, \quad |a| \le a_{\max}$

$v \in V$0

$v \in V$1

In practice, these are discretized, leading to a finite set of graph edges, each representing a motion segment (e.g., constant-acceleration primitives with $v \in V$2).

Dynamic obstacles are likewise projected onto the same graph. Each vertex and edge is annotated with a set of "safe intervals" $v \in V$3 and $v \in V$4, denoting time intervals during which they are not occupied by any obstacle. A timed path is feasible if each movement and waiting segment avoids obstacles, the agent never "jumps" (skips through the state space without a valid primitive), and any idle period implies that the agent decelerated to rest prior to waiting.

The planning objective is to discover a minimum-arrival-time trajectory from a start configuration to a designated goal, adhering strictly to kinodynamic and collision constraints.

2. Algorithmic Principles and Waiting-Interval Projection

Conventional SIPP labels search states by the tuple $v \in V$5, where $v \in V$6 is a vertex and $v \in V$7 is a safe interval; this is viable only under the assumption of instantaneous stopping/starting. Under non-instantaneous dynamics, this abstraction is incomplete because the agent with nonzero velocity cannot wait; thus, opportunities to leverage early arrival for deferred action are lost.

Kinodynamic SIPP-IP addresses this deficiency by representing each search node as $v \in V$8, where $v \in V$9 is a waiting interval—times at which an agent, after decelerating to zero, can wait and initiate outgoing motion. Expansion involves projecting waiting intervals through the kinematic and obstacle constraints of motion primitives:

• For each outgoing edge $\mathit{vel}$0, the algorithm computes all feasible successor intervals $\mathit{vel}$1, each corresponding to times at which the agent can reach $\mathit{vel}$2 (at a permitted velocity) without conflict, by sequential interval arithmetic and intersection along the primitive's swept cells and their safe intervals.
• For successor nodes with $\mathit{vel}$3, each interval $\mathit{vel}$4 is extended to the end of its parent safe interval in $\mathit{vel}$5, encoding further waiting opportunities.

The key operation is the projection of waiting intervals through edge and vertex safe intervals, ensuring that every feasible atomic time-step transition in a time-discretized A* graph is subsumed by a SIPP-IP node.

3. Formal Guarantees: Completeness and Optimality

Kinodynamic SIPP-IP is supported by formal proofs of completeness and optimality:

• Completeness: Every atomic A*-with-time-step successor is covered by a projected interval in SIPP-IP. If a collision-free, feasible solution exists in the underlying time-discretized A* graph, SIPP-IP will generate and expand this solution.
• Optimality: A SIPP-IP node $\mathit{vel}$6 guarantees that the A*-TS state $\mathit{vel}$7 exhibits the minimal $\mathit{vel}$8-value (arrival plus heuristic) among all reachable $\mathit{vel}$9 in the interval $\mathit{vel} = 0$0. Since SIPP-IP always expands the interval whose $\mathit{vel} = 0$1 is minimal and never prunes a potentially optimal state, the first expanded goal-interval yields the globally minimal-arrival-time solution.

The core insight is that SIPP-IP performs grouped expansions tightly coupled to admissible and consistent heuristics, preventing loss of optimal options due to premature pruning or over-aggregation (Ali et al., 2023).

4. Computational Complexity and Empirical Assessment

The principal computational load in SIPP-IP arises from projecting a waiting interval through each outgoing motion primitive, requiring per-cell interval arithmetic potentially over multiple safe intervals. If each primitive sweeps $\mathit{vel} = 0$2 cells and there are $\mathit{vel} = 0$3 safe intervals per cell, projection has complexity $\mathit{vel} = 0$4, with $\mathit{vel} = 0$5 usually 1.

Despite per-node operations being heavier than in vanilla SIPP, SIPP-IP substantially reduces the number of nodes compared to time-expanded A*, providing significant performance gains in practice.

Empirical evaluation spanned a suite of five grid maps of varying complexity and obstacle densities (from 1/25 to 1/3 free cells, 200 random instances per setting), employing primitives with ±0.5 cells/s² acceleration/deceleration, 2 cells/s cruising, and a time-step of 0.1 s. Comparative benchmarks included:

• A* (full time-expanded): Typically intractable on dense scenarios.
• SIPP1: Kinodynamically feasible SIPP without interval projection; incomplete under kinodynamic constraints.
• SIPP2: Allows re-expansion within safe intervals; also incomplete in certain cases.

Key performance outcomes:

Metric	SIPP-IP	SIPP1/SIPP2	A*
Success Rate	Highest (~100%)	Many missed (incomplete)	Often time-outs
Solution Suboptimality	Optimal	5–50% (max 42%) longer	N/A
Runtime	<<A*, <SIPP2, ~SIPP1	Faster, incomplete	2 orders slower

SIPP-IP solved nearly all instances except the hardest, yielding optimal paths, and achieving runtimes at least two orders of magnitude faster than full time-expanded A* (average <0.1 s excluding the largest map). SIPP1 and SIPP2 were faster in some settings but produced suboptimal plans and missed feasible solutions.

5. Applicability, Limitations, and Prospective Directions

Kinodynamic SIPP-IP is especially suited to discrete-graph planning settings where stopping and starting times are non-negligible, and agents must obey acceleration or deceleration limits—a context common in high-speed UAVs, nonholonomic mobile robots, and AI agents with inertia.

The algorithm is compelling when dynamic obstacles have known, predictable trajectories and fine-grained temporal resolution is required. However, projection overhead may be significant in cases where primitives are long (sweep many cells) or obstacle patterns yield highly fragmented safe intervals.

The method presently assumes pre-discretized primitives; extensions to continuous state spaces, or more elaborate kinodynamic models (such as bounded curvature), would necessitate additional projection mechanisms. Potential future developments include integrating SIPP-IP into multi-agent context solvers and enabling incremental replanning to handle changes in obstacle forecasts dynamically.

6. Summary

By incorporating waiting intervals and a projection-based expansion operator, Kinodynamic SIPP-IP restores completeness and optimality to SIPP under linear acceleration constraints, while retaining SIPP’s drastic search-space reduction relative to time-expanded A*. The approach is substantiated both theoretically and empirically, with demonstrated effectiveness in planning scenarios where agents cannot instantaneously adapt their speed, and environments exhibit complex, dynamic obstacles (Ali et al., 2023).