Source-linked AI summary
Fast Direction-Conditioned Reachability for Motion Prediction Under Model Uncertainty
Hrishav Das, Melkior Ornik
TL;DR
Uncertain-model reachability can be costly when repeated replanning needs only an agent’s extent in a particular direction. The paper selects one admissible model for that direction and computes its reachable set; on an uncertain linearized bicycle model, it reports a roughly threefold speedup with directional extent within 5% of the full family in the reported directions.
Problem
Repeated online reachability with uncertain dynamics must account for every admissible model, even when planning needs only a particular directional extent.
Source: §I · p. 1Method
For a query direction, the method selects one admissible state and input matrix pair whose reachable set approaches the uncertain family’s extent, then computes that model’s reachable set.
Source: §I · p. 2, D. Reachable Set of the Selected Model · p. 3Results
On the uncertain linearized bicycle model, the method was about three times faster than full-family propagation and its queried-direction extent was within 5% of the uncertain-family extent in the reported directions.
Source: §VI · p. 7Takeaways & Limitations
In a closed-loop multi-vehicle simulation, the resulting predictions were fast enough for rapid replanning around moving vehicles.
Source: §VI · p. 7The approach is faster only when a few directions are needed: it must be rerun for each direction, whereas a computed full uncertain set can be queried in any number of directions.
Source: C. Computation Time · p. 5
Abstract
from arXiv · showhide
To avoid collisions, a robot must repeatedly predict where nearby agents may move, usually with an imperfect model of their dynamics. Reachable sets provide such predictions, but computing them when the system matrices themselves are uncertain can become computationally expensive and conservative for frequent replanning. Moreover, a planner often needs to know only how far an agent can move in one particular direction, for example toward the robot, rather than the complete reachable set. We propose a direction-conditioned reachability method for linear systems with uncertain state and input matrices. Given a query direction $d$, the method selects one admissible model $(A^\star,B^\star)$ whose reachable set extends nearly as far along $d$ as the reachable set of the entire uncertain model family, and then computes the reachable set of only this model with a standard reachability solver. On an uncertain linearized bicycle model, the complete selection-and-computation pipeline is about three times faster than computing the reachable set of the full uncertain family in the CORA toolbox, while its extent along $d$ is within $5\%$ of the full family's in the reported directions. We also use the method in a closed-loop multi-vehicle simulation in which the robot queries, at each replanning step, how far each nearby vehicle can move toward it, and replans to avoid the resulting sets.
I. INTRODUCTION
Reachable sets support motion prediction and planning, but accounting for uncertain dynamics can make repeated online computations costly. The paper instead selects an admissible model for a queried direction and computes that model’s reachable set.
I. INTRODUCTION
- Reachable sets describe states attainable from an initial set under admissible inputs and uncertainty, and are used in robotics and autonomous-driving planning and safety tasks.Hamilton–Jacobi methods handle nonlinear reachability but scale poorly with state dimension; set-propagation methods scale more favorably but can be conservative, particularly for nonlinear dynamics.Source: §I · p. 1
- When dynamics are uncertain, reachability must account for every admissible model, adding cost to computations that are repeated as new observations arrive.Planning often needs only a geometric question such as whether a candidate trajectory may collide, or how far another vehicle can move toward the robot.Source: §I · pp. 1–2
- The method selects one admissible state and input matrix pair for a query direction, then computes the reachable set of that fixed model.The selection targets the extent along the queried direction rather than the complete uncertain reachable set.Source: §I · p. 2, §II · p. 2
- For a single query direction, the selection-and-computation procedure is about three times faster than computing the reachable set of the full uncertain family.The paper also reports a closed-loop multi-vehicle simulation using these predictions for replanning around moving vehicles.Source: §I · p. 2
II. PROBLEM FORMULATION
- The uncertain state and input matrices are modeled as constant over the horizon and drawn from matrix sets; the initial state set is compact and convex, and the input set is a compact convex polytope.Each matrix-set generator represents a source of model error that may perturb several matrix entries together.Source: §II · p. 2
III. DIRECTION-CONDITIONED REACHABILITY
The proposed approximation separates direction-conditioned model selection into state-matrix and input-matrix choices, then propagates the selected model.
III. DIRECTION-CONDITIONED REACHABILITY
- The procedure first writes the fixed-model support function, then selects A⋆ using its initial-set contribution and B⋆ using its input contribution.It finally computes the reachable set for the selected pair.Source: §III · p. 2
- With A⋆ fixed, the method chooses B⋆ by maximizing the support-function contribution due to inputs.This follows the preceding selection of A⋆ from the initial-set contribution.Source: §III · p. 2
- After selecting A⋆ and B⋆, the method computes the reachable set of that fixed model.The four-step procedure is presented as an approximate solution to direction-conditioned selection.Source: §III · p. 2
A. Directional Support for a Fixed Model
For a fixed model, the directional support expression separates extent contributed by the initial set from extent contributed by inputs. The method approximates state-matrix selection by optimizing only the initial-set term.
A. Directional Support for a Fixed Model
- The fixed-model support expression adds the initial-set extent along d to the input contribution integrated over [0, T].The direction-conditioned selection problem maximizes this expression over admissible A and B.Source: A. Directional Support for a Fixed Model · p. 2
B. State-Matrix Selection
- The method selects A using only the initial-set term, although A also affects the input term.The state-matrix objective is nonconvex because the matrix exponential depends nonlinearly on the uncertainty parameters; experiments use dense-grid search followed by local refinement.Source: B. State-Matrix Selection · p. 3
- The state-matrix choice is an approximation, not a claimed global solution; it is expected to be most accurate when input contribution dominates drift over the horizon.Its accuracy is evaluated empirically.Source: B. State-Matrix Selection · p. 3
C. Input-Matrix Selection
With A⋆ fixed, the method selects the input matrix by maximizing the input contribution to directional extent. Convexity permits a search over extreme points of the matrix polytope.
C. Input-Matrix Selection
- Because the input set U is a polytope, the linear input contribution attains its maximum at a vertex of U.The input-matrix selection objective is evaluated numerically using midpoint samples over [0, T].Source: C. Input-Matrix Selection · p. 3
- For fixed A⋆, the input-matrix objective is convex in B, so at least one maximizer lies at an extreme point of the compact matrix polytope.The proof uses that each integrand is a pointwise maximum of functions linear in B, and integration preserves convexity.Source: C. Input-Matrix Selection · p. 3
- For matrix-zonotope B, it suffices to evaluate at most 2^nB candidates obtained from box vertices.The matrix zonotope is an affine image of the coefficient box, and its extreme points are images of box vertices.Source: C. Input-Matrix Selection · p. 3
D. Reachable Set of the Selected Model
The method selects a model tailored to a query direction, then computes that model’s reachable set with a standard linear reachability algorithm. Its evaluation uses an uncertain kinematic bicycle model with bounded perturbations to selected Jacobian and input-gain terms.
D. Reachable Set of the Selected Model
- The selected model depends on the query direction, and its reachable set is computed with a standard linear reachability algorithm after matrix uncertainty is removed.The method calls this output the direction-conditioned reachable set.Source: D. Reachable Set of the Selected Model · p. 3
IV. BICYCLE MODEL EVALUATION
- The evaluation compares the uncertain CORA outer approximation with the direction-conditioned set by their extents along the query direction and the computation required.Both computations use CORA.Source: §IV · p. 3
A. Vehicle Model and Uncertainty
- The vehicle model places bounded uncertainty on selected Jacobian and input-gain terms, while leaving the affine column unperturbed to isolate those uncertainties.The evaluation uses a kinematic bicycle model.Source: A. Vehicle Model and Uncertainty · pp. 3–4
B. Directional Accuracy
Across the evaluated directions, the selected model’s upper extent closely tracks the uncertain CORA outer approximation, while its lower extent can differ substantially. For representative directions, its upper extent is at most 4.8% below the outer approximation and matches it at 90°.
B. Directional Accuracy
- Across θ, the selected model’s upper extent closely tracks the uncertain CORA outer approximation, but their lower extents differ substantially.The selection targets reach along +d, not −d.Source: B. Directional Accuracy · p. 4, Figure 3 · p. 5
- At θ = 60°, the direction-conditioned set nearly reaches the uncertain CORA boundary along d despite being much smaller overall.The comparison illustrates the trade-off of retaining extent along the queried direction rather than reproducing the full uncertain set.Source: Figure 2 · p. 5, B. Directional Accuracy · p. 5
- 4.8% is the maximum shortfall in upper extent for the direction-conditioned set versus the uncertain CORA outer approximation; the values match at θ = 90°.The reported gap bounds the loss introduced by sequential model selection.Source: Table I · p. 5, B. Directional Accuracy · p. 5
APPROXIMATION (h+
Figure 4 separates the uncertain propagation, direction-conditioned propagation, matrix-selection time, and total direction-conditioned pipeline across query directions.
APPROXIMATION (h+
- The plot compares computation time across query direction θ.The green uncertain CORA propagation is computed once because it is independent of θ.Source: Figure 4 · p. 5
- The red curve represents CORA propagation of the direction-conditioned model.The remaining curves show matrix-selection time and the total direction-conditioned pipeline.Source: Figure 4 · p. 5
- The green curve represents uncertain CORA propagation, computed once independently of θ.This provides the direction-independent timing comparison in the figure.Source: Figure 4 · p. 5
C. Computation Time
The direction-conditioned pipeline reduces computation time for a small number of directional queries, but must be rerun for each direction.
C. Computation Time
- 0.154 s per direction versus 0.457 s for uncertain CORA propagation makes Algorithm 1 about three times faster for one direction.The comparison was measured in MATLAB R2026a with CORA on an AMD Ryzen 7 5800H CPU.Source: C. Computation Time · p. 5
- 0.0367 s for matrix selection plus 0.118 s for selected-model propagation yields the 0.154 s per-direction total.The selection-and-computation pipeline is evaluated separately from the full uncertain propagation.Source: C. Computation Time · p. 5
- The method remains faster than one uncertain propagation for up to two directional queries, but must be rerun for each direction.The full uncertain set, by contrast, can be queried in any number of directions once computed.Source: C. Computation Time · p. 5
V. CLOSED-LOOP MULTI-VEHICLE NAVIGATION
In closed loop, the ego vehicle uses direction-conditioned reachable sets to avoid two vehicles whose future motion is uncertain.
V. CLOSED-LOOP MULTI-VEHICLE NAVIGATION
- At each replanning step, the ego asks how far each nearby vehicle can move toward it and avoids the resulting reachable sets.The query direction points from each vehicle toward the ego.Source: §V · p. 5
- The simulation uses one direction per nearby vehicle, matching the setting where the method is faster than computing the full uncertain set.The paper relates this use of set-based predictions of traffic participants to prior work,,.Source: §V · pp. 5–6
- The ego vehicle must reach a goal while avoiding two other vehicles whose future motion is not known exactly.The method is used in closed loop, with replanning at each step.Source: §V · p. 5
A. Scenario and Online Prediction
The online prediction scenario uses an ego vehicle and two uncertain vehicles, relinearizes their prediction models at each update, and queries direction-conditioned sets toward the ego.
A. Scenario and Online Prediction
- The ego travels at 1.05 m/s through a 14 m × 9 m workspace toward a goal, avoiding two vehicles following unknown waypoint sequences.The other vehicles follow the nonlinear bicycle model, and the ego does not know their waypoints or inputs.Source: A. Scenario and Online Prediction · p. 6
- Vehicles move continuously, while the ego measures their states every 0.4 s and estimates acceleration and yaw rate by finite differences.The state is ξk = [xk yk ψk vk]⊤.Source: A. Scenario and Online Prediction · p. 6
- The steering estimate is obtained by inverting the yaw-rate equation, and the estimated inputs are clipped to actuator limits.The inversion uses the bicycle-model yaw-rate relation.Source: A. Scenario and Online Prediction · p. 6
- At each update, the measured state and estimated input define the operating point for rebuilding the uncertain linear prediction model.The prediction model is re-linearized at every update.Source: A. Scenario and Online Prediction · p. 6
- For each vehicle, the query is padded with zeros to the dimension of z and passed to Algorithm 1 with T = 1.65 s.Direction-conditioned reachable sets are computed only for vehicles within a 5.3 m sensing radius of the ego.Source: A. Scenario and Online Prediction · p. 6
- At a 0.4 s replanning period, predicting both vehicles takes about 0.31 s with Algorithm 1 versus about 0.91 s for full uncertain sets.The stated times are 2 × 0.154 s and 2 × 0.457 s, respectively.Source: A. Scenario and Online Prediction · p. 6
- Green regions show uncertain CORA outer approximations, while red regions show the direction-conditioned sets that the planner avoids.Figure 5 depicts the sets for Vehicle 1 at one update; its arrow points toward the ego.Source: Figure 5 · p. 6, A. Scenario and Online Prediction · p. 6
B. Reachability-Aware Path Planning
The planner uses direction-conditioned reachable sets as obstacles and recomputes its route as vehicles move. These sets target motion toward the ego rather than enclosing every possible vehicle motion.
B. Reachability-Aware Path Planning
- At each update, the ego projects nearby vehicles’ reachable sets at several horizon times, enlarges them by 0.28 m, and finds the shortest grid path avoiding their union.Source: B. Reachability-Aware Path Planning · p. 6
C. Closed-Loop Behavior
- When reachable sets block its A* route, the ego reroutes around them; it later proceeds toward the goal once no red set intersects the route.Source: C. Closed-Loop Behavior · p. 6
- Full uncertain obstacles would not fit the replanning period; direction-conditioned sets target toward-ego extent and leave more free space in unqueried directions.Source: C. Closed-Loop Behavior · pp. 6–7
VI. CONCLUSION
- About three times faster than full-family reachability, the method kept queried-direction extent within 5% and supported rapid replanning in multi-vehicle simulation.Source: §VI · p. 7
- Future work will seek stronger guarantees for state-matrix selection and extend the method to higher-dimensional models and hardware experiments.Source: §VI · p. 7