Collision Cone

A Collision Cone is constructed by projecting the geometric shape of an obstacle from the perspective of a moving agent along all possible relative velocity vectors. The cone's apex is at the agent's position. If the agent's current relative velocity vector lies within this cone, a collision is predicted assuming constant velocity. The cone's angular width is determined by the combined radii of the agent and obstacle, creating a collision envelope.

The cone is intrinsically linked to Time-to-Collision (TTC). Points deeper within the cone correspond to shorter TTC values, indicating higher urgency. By analyzing the intersection of the velocity vector with the cone's boundaries, the algorithm can compute the exact predicted collision time. This allows for risk stratification, where velocities near the cone's edge represent near-misses with a larger TTC, while central vectors indicate imminent impact.

The Collision Cone is the foundational concept for the Velocity Obstacle (VO) family of algorithms. The VO is derived by translating the Collision Cone from positional space into the agent's velocity space. Specifically, the VO is the set of all the agent's absolute velocities that would cause the relative velocity to fall inside the Collision Cone. This transformation allows planners to directly select safe velocities from a feasible set, making avoidance a velocity selection problem.

• Core Insight: A velocity is unsafe if v_agent - v_obstacle ∈ Collision Cone.

In reactive or local planning systems, the Collision Cone is used for instantaneous risk assessment and evasive maneuver generation. The agent's planner samples candidate velocity commands from its dynamically feasible set. Each candidate is tested by checking if the resulting relative velocity vector lies outside all active Collision Cones from perceived obstacles. The safest candidate that also progresses toward the goal is selected. This enables real-time, sensor-driven avoidance without a full global path replan.

The cone's geometry adapts to uncertainty in perception and prediction:

• Obstacle Shape: For non-point obstacles, the cone is constructed using the Minkowski Sum of the agent and obstacle shapes, effectively inflating the obstacle.
• Prediction Uncertainty: If an obstacle's future trajectory is probabilistic, the cone can be replaced or augmented with a Probabilistic Collision Cone, representing a volume in velocity space where collision probability exceeds a threshold.
• Sensor Noise: The cone's boundaries can be expanded by a safety margin derived from estimated sensor error, ensuring robust avoidance.

While powerful, the classical Collision Cone has assumptions that must be managed in real systems:

• Constant Velocity Assumption: It assumes both agent and obstacle maintain current velocity. This breaks down with accelerating or maneuvering obstacles, requiring frequent recomputation or integration with trajectory prediction models.
• Non-Convex Obstacles: Complex obstacle shapes can create non-convex cones, complicating the 'inside/outside' test.
• Multi-Agent Scenarios: With many agents, the intersection of multiple cones (or VOs) can leave no viable velocity, causing reciprocal dance or deadlock. This is addressed by higher-order algorithms like Reciprocal Velocity Obstacle (RVO) and Optimal Reciprocal Collision Avoidance (ORCA).

The Velocity Obstacle (VO) is the direct geometric precursor to the Collision Cone. For a given agent and a moving obstacle, the VO is the set of all absolute velocities the agent could choose that would result in a collision within a specified time window (τ). The Collision Cone is often defined in the space of relative velocities. The key distinction is that the VO is used to prune an agent's admissible velocity space to find a safe, collision-free velocity.

• Core Concept: Defines a 'forbidden' region in velocity space.
• Application: Used in reactive, sampling-based planners where the agent selects a new velocity outside all VOs at each control cycle.

Optimal Reciprocal Collision Avoidance (ORCA) is a multi-agent, decentralized algorithm that builds upon the Velocity Obstacle concept. It assumes all agents share responsibility for avoidance. For each pair of agents, ORCA efficiently computes a half-plane of permitted velocities (the complement of the VO) that guarantees collision-free motion if both agents select a velocity from within their respective half-planes.

• Reciprocity: Enables smooth, cooperative maneuvers without oscillation.
• Efficiency: Uses linear programming to find the new velocity closest to the agent's preferred velocity, ensuring optimality.

Time to Collision (TTC) is a fundamental scalar risk metric derived from the geometry of the Collision Cone. It estimates the time remaining until a collision occurs if two agents continue on their current, constant relative velocity course. A TTC value is only valid if the relative velocity vector lies inside the Collision Cone.

• Calculation: For a straight-line approximation, TTC = (current separation distance) / (closing speed).
• Thresholding: Systems often define a critical TTC threshold (e.g., 3 seconds) to trigger warnings or automatic interventions.

Model Predictive Control (MPC) is an optimization-based control strategy that naturally incorporates Collision Cone constraints. At each time step, MPC solves a finite-horizon optimal control problem to find a sequence of control inputs that minimizes a cost function (e.g., deviation from goal) while satisfying dynamic constraints and collision avoidance constraints, which can be formulated using the Collision Cone or VO.

• Proactive: Optimizes over a future horizon, enabling smoother, more anticipatory avoidance.
• Constraint Handling: Can explicitly encode Collision Cone conditions as non-linear constraints in the optimization problem.

Trajectory Prediction is the process of forecasting the future states (position, velocity) of dynamic obstacles. Accurate prediction is critical for constructing a meaningful Collision Cone, as the cone is defined relative to the predicted future state of the obstacle. Without prediction, the cone is only valid for static obstacles or those moving with constant velocity.

• Methods: Ranges from simple constant velocity/acceleration models to complex neural networks.
• Uncertainty: Advanced methods output probabilistic predictions, leading to probabilistic or 'chance-constrained' Collision Cones.

A Control Barrier Function (CBF) is a mathematical tool used to provide formal safety guarantees for control systems. In collision avoidance, a CBF can be defined based on the geometry of the Collision Cone. The CBF constraint ensures that the derivative of a safety function (encoding distance to collision) is always above a certain threshold, guaranteeing the system state remains in the safe set (outside the Collision Cone).

• Formal Guarantee: Provides rigorous proofs of collision avoidance under defined dynamics.
• Real-Time Filter: Can be used as a safety filter to minimally modify an existing controller's command to ensure safety.