Consider \( N \) planar robots with configuration (position) \( p_i(t) \in \mathbb{R}^2 \) and velocity \( v_i(t) \in \mathbb{R}^2 \) for \( i = 1,\dots,N \). We approximate each robot by a closed disk of radius \( r_i > 0 \). The robots are controlled at the velocity level (you may think of a low-level tracking controller that realizes commanded velocities subject to dynamics constraints learned in earlier courses):

\[ \dot p_i(t) = v_i(t), \quad v_i(t) \in \mathcal{V}_i \subset \mathbb{R}^2. \]

Safety requires that the disks do not overlap. Define the pairwise clearance constraint for each \( i \neq j \):

\[ \|p_i(t) - p_j(t)\|_2 \geq r_i + r_j + d_{\mathrm{margin}} \quad \forall t \geq 0, \]

where \( d_{\mathrm{margin}} \geq 0 \) is a design margin. The safe set of configurations in the global configuration space \( \mathcal{C} = \mathbb{R}^{2N} \) is

\[ \mathcal{C}_{\mathrm{safe}} = \Big\{ (p_1,\dots,p_N) \;\big|\; \|p_i - p_j\|_2 \geq r_i + r_j + d_{\mathrm{margin}} \;\forall i \neq j \Big\}. \]

Our goal is to design decentralized control laws \( v_i(t) = \pi_i(p_i(t), \mathcal{N}_i(t)) \), where \( \mathcal{N}_i(t) \) denotes local neighbor information (positions and velocities of nearby robots), such that:

• Each robot tracks a preferred velocity \( v_i^{\mathrm{pref}}(t) \) toward its task.
• The safety constraints above are satisfied for all \( t \geq 0 \).

Most practical multi-robot collision avoidance schemes operate in discrete time with sampling period \( \Delta t \), and update velocities at instants \( t_k = k\Delta t \).

flowchart TD
  K["Time step k"] --> SENSE["Perceive neighbors (p_j, v_j) in local range"]
  SENSE --> VO["Construct pairwise velocity constraints from geometry"]
  VO --> OPT["Solve local optimization for safe velocity v_i* close to v_i_pref"]
  OPT --> CMD["Send v_i* to low-level tracking controller"]
  CMD --> NEXT["Time step k+1"]
        

The core mathematical problem is to characterize the set of admissible velocities at each step that guarantee no future collision within a planning horizon.

We first derive an exact collision condition for two disk-shaped robots with constant velocities. Let robots \( i \) and \( j \) have positions \( p_i, p_j \in \mathbb{R}^2 \) and velocities \( v_i, v_j \in \mathbb{R}^2 \). Define

\[ p_{ij} = p_j - p_i, \quad v_{ij} = v_i - v_j, \quad R_{ij} = r_i + r_j + d_{\mathrm{margin}}. \]

Under constant velocities, relative motion is \( p_{ij}(t) = p_{ij} + t v_{ij} \). A collision occurs if there exists \( t \geq 0 \) such that

\[ \|p_{ij}(t)\|_2 \leq R_{ij}. \]

Squaring both sides yields a quadratic inequality in \( t \):

\[ \phi(t) = \|p_{ij} + t v_{ij}\|_2^2 - R_{ij}^2 \leq 0. \]

Expand \( \phi(t) \) as \( a t^2 + b t + c \), where

\[ a = \|v_{ij}\|_2^2, \quad b = 2\, p_{ij}^\top v_{ij}, \quad c = \|p_{ij}\|_2^2 - R_{ij}^2. \]

Proposition 1 (pairwise collision criterion). Assume \( a > 0 \). There exists \( t \geq 0 \) such that \( \phi(t) \leq 0 \) if and only if

• \( \Delta = b^2 - 4ac \geq 0 \) (real intersection), and
• the smaller root \( t_{\min} = \dfrac{-b - \sqrt{\Delta}}{2a} \) satisfies \( t_{\min} \geq 0 \).

Proof sketch.

The inequality \( \phi(t) \leq 0 \) defines an interval of \( t \) values where the distance is at most \( R_{ij} \). For a quadratic with \( a > 0 \), the set \( \{t \mid \phi(t) \leq 0\} \) is either empty or a closed interval \( [t_{\min}, t_{\max}] \) between the two real roots, provided \( \Delta \geq 0 \). Existence of a nonnegative \( t \) in that interval is equivalent to \( t_{\min} \geq 0 \). The explicit formula follows from the quadratic formula. \(\Box\)

A common diagnostic is the time-to-collision (TTC) when it exists:

\[ \mathrm{TTC}_{ij}(v_{ij}) = \begin{cases} +\infty, & \Delta < 0 \text{ or } t_{\min} < 0,\\[0.5em] t_{\min}, & \text{otherwise}. \end{cases} \]

This function will later allow us to define velocity sets that are safe up to a finite planning horizon \( T_h \), e.g. \( \mathrm{TTC}_{ij}(v_{ij}) \geq T_h \).

The velocity obstacle framework characterizes directly in velocity space which choices of \( v_i \) will lead to a collision with neighbor \( j \) under constant velocities. Define the closed disk

\[ \mathcal{D}(0,R_{ij}) = \{ x \in \mathbb{R}^2 \mid \|x\|_2 \leq R_{ij} \}. \]

For a fixed neighbor velocity \( v_j \) and relative position \( p_{ij} \), the velocity obstacle of \( j \) with respect to \( i \), over a (possibly infinite) horizon \( T_h \in (0,+\infty] \), is

\[ \mathrm{VO}_{ij} = \Big\{ v \in \mathbb{R}^2 \;\Big|\; \exists t \in [0,T_h]: p_{ij} + t (v - v_j) \in \mathcal{D}(0,R_{ij}) \Big\}. \]

Any commanded velocity \( v_i \in \mathrm{VO}_{ij} \) will eventually cause a collision with robot \( j \) within the horizon \( T_h \), assuming \( v_j \) remains constant.

From a geometric viewpoint, the set \( \mathrm{VO}_{ij} \) is a translated cone in velocity space:

• First consider the static case \( v_j = 0 \). Then \( \mathrm{VO}_{ij}^0 \) is the cone of velocities whose rays intersect the disk \( \mathcal{D}(-p_{ij},R_{ij}) \).
• For nonzero \( v_j \), we simply translate the cone: \( \mathrm{VO}_{ij} = v_j + \mathrm{VO}_{ij}^0 \).

Let \( \theta_{ij} \) denote the angle of \( p_{ij} \) and \( \alpha_{ij} \) the half-angle of the cone tangent to \( \mathcal{D}(0,R_{ij}) \) from \( p_{ij} \). Then the VO boundary is parameterized as

\[ \partial \mathrm{VO}_{ij}^0 = \left\{ \lambda \begin{bmatrix} \cos(\theta_{ij} \pm \alpha_{ij}) \\ \sin(\theta_{ij} \pm \alpha_{ij}) \end{bmatrix} \;\middle|\; \lambda \geq 0 \right\}, \quad \sin(\alpha_{ij}) = \frac{R_{ij}}{\|p_{ij}\|_2}. \]

The safe velocity set for robot \( i \) with respect to only neighbor \( j \) is the complement \( \mathbb{R}^2 \setminus \mathrm{VO}_{ij} \), and with respect to all neighbors \( \mathcal{N}_i \) (within sensing range) we obtain

\[ \mathcal{V}_i^{\mathrm{safe}} = \mathbb{R}^2 \setminus \bigcup_{j \in \mathcal{N}_i} \mathrm{VO}_{ij}. \]

In principle, robot \( i \) could choose any velocity in \( \mathcal{V}_i^{\mathrm{safe}} \). However, VO alone is not symmetric: if only one robot deviates while the other keeps its velocity, that robot may have to make large avoidance maneuvers. This motivates reciprocal constructions.

To distribute effort symmetrically among robots, we consider reciprocal collision avoidance. The key idea is that for each pair \( (i,j) \), both robots adjust their velocities such that the relative velocity moves just outside the corresponding VO cone.

Let current velocities at time step \( k \) be \( v_i, v_j \). Define relative velocity \( v_{ij} = v_i - v_j \). If \( v_{ij} \notin \mathrm{VO}_{ij}^0 \), no action is needed. If \( v_{ij} \in \mathrm{VO}_{ij}^0 \), we compute the smallest change \( u_{ij} \) that pushes \( v_{ij} \) to the closest point on the VO boundary.

In ORCA-like schemes, robot \( i \) is responsible for half of this correction:

\[ v_i^{\mathrm{new}} = v_i + \tfrac{1}{2} u_{ij}, \quad v_j^{\mathrm{new}} = v_j - \tfrac{1}{2} u_{ij}. \]

For decentralized controllers, we cannot directly coordinate \( v_j^{\mathrm{new}} \) but we can assume that every agent runs the same algorithm and thus enforces its half of the correction.

To derive a constraint on the next velocity of robot \( i \), let \( \hat{v}_i = v_i + \tfrac{1}{2} u_{ij} \), and let \( n_{ij} \in \mathbb{R}^2 \) be the outward normal of the VO boundary at the closest boundary point. Then we define the half-plane of admissible velocities:

\[ \mathrm{ORCA}_{ij} = \Big\{ v \in \mathbb{R}^2 \;\Big|\; (v - \hat{v}_i)^\top n_{ij} \geq 0 \Big\}. \]

Proposition 2 (local convex constraint). For fixed neighbor \( j \), the set \( \mathrm{ORCA}_{ij} \) is a half-space in \( \mathbb{R}^2 \), hence convex and closed.

With multiple neighbors \( j \in \mathcal{N}_i \), the intersection

\[ \mathcal{V}_i^{\mathrm{ORCA}} = \bigcap_{j \in \mathcal{N}_i} \mathrm{ORCA}_{ij} \]

is again convex (intersection of half-spaces). At each time step, each robot solves a small convex optimization problem:

\[ \begin{aligned} \min_{v \in \mathbb{R}^2} \quad & \| v - v_i^{\mathrm{pref}} \|_2^2 \\ \text{s.t.} \quad & v \in \mathcal{V}_i^{\mathrm{ORCA}}, \\ & v \in \mathcal{V}_i \text{ (robot kinematics limits)}. \end{aligned} \]

This is a quadratic program (QP) in two variables with linear constraints, solvable in closed form or via simple incremental algorithms.

flowchart TD
  A["Agent i at time step k"] --> B["For each neighbor j: compute p_ij, v_ij"]
  B --> C["If v_ij in velocity obstacle: find closest boundary, get u_ij, n_ij"]
  C --> D["Construct half-plane constraint (ORCA_ij)"]
  D --> E["Intersect constraints and solve small QP for v_i*"]
  E --> F["Apply v_i* as next desired velocity"]
        

Remark (invariance intuition). If every pair \( (i,j) \) satisfies the half-plane constraint above, then the relative velocity after update lies outside \( \mathrm{VO}_{ij}^0 \), guaranteeing no collision within horizon \( T_h \). The formal proof uses the geometry of the VO cone and is a standard result in the RVO/ORCA literature.

We now implement a simplified ORCA-like algorithm in Python for holonomic robots in 2D using numpy. This code omits some robustness details but is faithful to the mathematical structure above.

import numpy as np

class Agent:
    def __init__(self, position, velocity, radius, v_pref, max_speed):
        self.p = np.asarray(position, dtype=float)
        self.v = np.asarray(velocity, dtype=float)
        self.r = float(radius)
        self.v_pref = np.asarray(v_pref, dtype=float)
        self.max_speed = float(max_speed)

def time_to_collision(p_ij, v_ij, R, T_h=np.inf):
    # Solve ||p_ij + t v_ij||^2 <= R^2
    a = np.dot(v_ij, v_ij)
    c = np.dot(p_ij, p_ij) - R**2
    if c <= 0.0:
        # Already in collision (overlap)
        return 0.0
    if a == 0.0:
        return np.inf  # No relative motion
    b = 2.0 * np.dot(p_ij, v_ij)
    disc = b*b - 4.0*a*c
    if disc < 0.0:
        return np.inf
    t_min = (-b - np.sqrt(disc)) / (2.0*a)
    if t_min < 0.0 or t_min > T_h:
        return np.inf
    return t_min

def normalize(v):
    n = np.linalg.norm(v)
    if n == 0.0:
        return v
    return v / n

def orca_constraints_for_agent(i, agents, T_h=5.0, delta=0.1):
    """
    Compute ORCA half-planes for agent i.
    Each constraint is (n_ij, p_point) meaning:
        (v - p_point) . n_ij >= 0
    """
    ai = agents[i]
    constraints = []

    for j, aj in enumerate(agents):
        if j == i:
            continue
        p_ij = aj.p - ai.p
        v_ij = ai.v - aj.v
        R = ai.r + aj.r
        ttc = time_to_collision(p_ij, v_ij, R, T_h)
        if ttc == np.inf:
            continue  # No impending collision

        # Compute closest point on VO boundary
        # Approximate by projecting relative velocity onto
        # tangent circle at time t = ttc
        p_coll = p_ij + v_ij * ttc
        n_ij = normalize(p_coll)  # outward normal of disk at collision
        # Small epsilon shift to guarantee separation
        u_ij = (R * n_ij - p_coll) / max(ttc, delta)

        # Agent i assumes half the correction
        p_point = ai.v + 0.5 * u_ij
        constraints.append((n_ij, p_point))

    return constraints

def project_velocity(v_pref, constraints, max_speed):
    """
    Solve min ||v - v_pref||^2 s.t. (v - p_k) . n_k >= 0, ||v|| <= max_speed.
    Simple incremental projection onto violated half-planes.
    """
    v = v_pref.copy()

    # Speed clipping (ball constraint)
    nrm = np.linalg.norm(v)
    if nrm > max_speed:
        v *= max_speed / nrm

    for (n_k, p_k) in constraints:
        if np.dot(v - p_k, n_k) < 0.0:
            # Project v onto boundary of half-plane
            # Solve (v' - p_k) . n_k = 0
            v = v + (np.dot(p_k - v, n_k)) * n_k

            # Re-enforce speed constraint
            nrm = np.linalg.norm(v)
            if nrm > max_speed:
                v *= max_speed / nrm

    return v

def step_orca(agents, T_h=5.0, delta=0.1):
    new_vels = []
    for i, _ in enumerate(agents):
        cons = orca_constraints_for_agent(i, agents, T_h, delta)
        v_star = project_velocity(agents[i].v_pref, cons, agents[i].max_speed)
        new_vels.append(v_star)

    # Update all agents synchronously
    for agent, v_star in zip(agents, new_vels):
        agent.v = v_star
        agent.p = agent.p + agent.v * delta

# Example usage
if __name__ == "__main__":
    agents = [
        Agent(position=[-1.0, 0.0], velocity=[0.5, 0.0],
              radius=0.3, v_pref=[0.5, 0.0], max_speed=1.0),
        Agent(position=[1.0, 0.0], velocity=[-0.5, 0.0],
              radius=0.3, v_pref=[-0.5, 0.0], max_speed=1.0),
    ]

    for k in range(50):
        step_orca(agents, T_h=5.0, delta=0.1)
        print(f"Step {k}:",
              [f"p{i}={agents[i].p}, v{i}={agents[i].v}" for i in range(len(agents))])
      

This implementation keeps the quadratic program implicit by performing incremental projections onto half-planes. For a small number of neighbors, this method is efficient and easy to implement.

The following C++ code illustrates a similar structure using Eigen for vector operations. It focuses on the geometric computations; integration into a simulation loop is straightforward.

#include <vector>
#include <iostream>
#include <Eigen/Dense>

struct Agent {
    Eigen::Vector2d p;
    Eigen::Vector2d v;
    Eigen::Vector2d v_pref;
    double r;
    double max_speed;
};

double timeToCollision(const Eigen::Vector2d& p_ij,
                       const Eigen::Vector2d& v_ij,
                       double R, double T_h)
{
    double a = v_ij.squaredNorm();
    double c = p_ij.squaredNorm() - R*R;
    if (c <= 0.0) return 0.0;
    if (a == 0.0) return std::numeric_limits<double>::infinity();
    double b = 2.0 * p_ij.dot(v_ij);
    double disc = b*b - 4.0*a*c;
    if (disc < 0.0) return std::numeric_limits<double>::infinity();
    double t_min = (-b - std::sqrt(disc)) / (2.0*a);
    if (t_min < 0.0 || t_min > T_h)
        return std::numeric_limits<double>::infinity();
    return t_min;
}

Eigen::Vector2d normalize(const Eigen::Vector2d& v)
{
    double n = v.norm();
    if (n == 0.0) return v;
    return v / n;
}

struct HalfPlane {
    Eigen::Vector2d n;
    Eigen::Vector2d p0; // (v - p0) . n >= 0
};

std::vector<HalfPlane> orcaConstraintsForAgent(
    int i,
    const std::vector<Agent>& agents,
    double T_h,
    double delta)
{
    const Agent& ai = agents[i];
    std::vector<HalfPlane> cons;

    for (int j = 0; j < (int)agents.size(); ++j) {
        if (j == i) continue;
        const Agent& aj = agents[j];
        Eigen::Vector2d p_ij = aj.p - ai.p;
        Eigen::Vector2d v_ij = ai.v - aj.v;
        double R = ai.r + aj.r;
        double ttc = timeToCollision(p_ij, v_ij, R, T_h);
        if (ttc == std::numeric_limits<double>::infinity())
            continue;

        Eigen::Vector2d p_coll = p_ij + ttc * v_ij;
        Eigen::Vector2d n_ij = normalize(p_coll);
        Eigen::Vector2d u_ij = (R * n_ij - p_coll) / std::max(ttc, delta);
        Eigen::Vector2d p0 = ai.v + 0.5 * u_ij;
        cons.push_back({n_ij, p0});
    }
    return cons;
}

Eigen::Vector2d projectVelocity(
    const Eigen::Vector2d& v_pref,
    const std::vector<HalfPlane>& cons,
    double max_speed)
{
    Eigen::Vector2d v = v_pref;
    double nrm = v.norm();
    if (nrm > max_speed) v *= max_speed / nrm;

    for (const auto& hp : cons) {
        double val = (v - hp.p0).dot(hp.n);
        if (val < 0.0) {
            v = v + (hp.p0 - v).dot(hp.n) * hp.n;
            nrm = v.norm();
            if (nrm > max_speed) v *= max_speed / nrm;
        }
    }
    return v;
}

void stepORCA(std::vector<Agent>& agents,
              double T_h, double delta)
{
    std::vector<Eigen::Vector2d> new_v(agents.size());

    for (int i = 0; i < (int)agents.size(); ++i) {
        auto cons = orcaConstraintsForAgent(i, agents, T_h, delta);
        new_v[i] = projectVelocity(agents[i].v_pref, cons, agents[i].max_speed);
    }

    for (int i = 0; i < (int)agents.size(); ++i) {
        agents[i].v = new_v[i];
        agents[i].p += delta * agents[i].v;
    }
}
      

This C++ implementation closely mirrors the Python code. In a real system, one would integrate it with sensor data, controller interfaces, and possibly a real-time operating system.

Java can implement the same logic using simple vector helper methods. Below is a minimal ORCA-style update step for planar agents.

public class Agent {
    public double[] p = new double[2];
    public double[] v = new double[2];
    public double[] vPref = new double[2];
    public double r;
    public double maxSpeed;

    public Agent(double[] p0, double[] v0, double r, double[] vPref, double maxSpeed) {
        this.p[0] = p0[0]; this.p[1] = p0[1];
        this.v[0] = v0[0]; this.v[1] = v0[1];
        this.r = r;
        this.vPref[0] = vPref[0]; this.vPref[1] = vPref[1];
        this.maxSpeed = maxSpeed;
    }
}

class HalfPlane {
    public double[] n = new double[2];
    public double[] p0 = new double[2]; // (v - p0) . n >= 0
}

public class ORCA2D {

    private static double dot(double[] a, double[] b) {
        return a[0]*b[0] + a[1]*b[1];
    }

    private static double norm(double[] a) {
        return Math.sqrt(dot(a, a));
    }

    private static void sub(double[] out, double[] a, double[] b) {
        out[0] = a[0] - b[0];
        out[1] = a[1] - b[1];
    }

    private static void add(double[] out, double[] a, double[] b) {
        out[0] = a[0] + b[0];
        out[1] = a[1] + b[1];
    }

    private static void scale(double[] out, double[] a, double s) {
        out[0] = a[0] * s;
        out[1] = a[1] * s;
    }

    private static void normalize(double[] out, double[] a) {
        double n = norm(a);
        if (n > 1e-9) {
            out[0] = a[0] / n;
            out[1] = a[1] / n;
        } else {
            out[0] = a[0];
            out[1] = a[1];
        }
    }

    private static double timeToCollision(double[] p_ij,
                                          double[] v_ij,
                                          double R, double T_h)
    {
        double a = dot(v_ij, v_ij);
        double c = dot(p_ij, p_ij) - R*R;
        if (c <= 0.0) return 0.0;
        if (Math.abs(a) < 1e-9) return Double.POSITIVE_INFINITY;
        double b = 2.0 * dot(p_ij, v_ij);
        double disc = b*b - 4.0*a*c;
        if (disc < 0.0) return Double.POSITIVE_INFINITY;
        double tMin = (-b - Math.sqrt(disc)) / (2.0*a);
        if (tMin < 0.0 || tMin > T_h)
            return Double.POSITIVE_INFINITY;
        return tMin;
    }

    private static java.util.List<HalfPlane> orcaConstraintsForAgent(
            int i,
            java.util.List<Agent> agents,
            double T_h,
            double delta)
    {
        Agent ai = agents.get(i);
        java.util.List<HalfPlane> cons = new java.util.ArrayList<>();

        for (int j = 0; j < agents.size(); ++j) {
            if (j == i) continue;
            Agent aj = agents.get(j);

            double[] p_ij = new double[2];
            double[] v_ij = new double[2];
            sub(p_ij, aj.p, ai.p);
            sub(v_ij, ai.v, aj.v);

            double R = ai.r + aj.r;
            double ttc = timeToCollision(p_ij, v_ij, R, T_h);
            if (!Double.isFinite(ttc)) continue;

            double[] pColl = new double[2];
            double[] tmp = new double[2];
            scale(tmp, v_ij, ttc);
            add(pColl, p_ij, tmp);

            double[] n_ij = new double[2];
            normalize(n_ij, pColl);

            double[] u_ij = new double[2];
            scale(tmp, n_ij, R);
            sub(tmp, tmp, pColl); // R * n_ij - pColl
            double denom = Math.max(ttc, delta);
            scale(u_ij, tmp, 1.0 / denom);

            HalfPlane hp = new HalfPlane();
            hp.n[0] = n_ij[0];
            hp.n[1] = n_ij[1];
            hp.p0[0] = ai.v[0] + 0.5 * u_ij[0];
            hp.p0[1] = ai.v[1] + 0.5 * u_ij[1];
            cons.add(hp);
        }
        return cons;
    }

    private static double[] projectVelocity(double[] vPref,
                                            java.util.List<HalfPlane> cons,
                                            double maxSpeed)
    {
        double[] v = new double[]{vPref[0], vPref[1]};
        double nrm = norm(v);
        if (nrm > maxSpeed) {
            v[0] *= maxSpeed / nrm;
            v[1] *= maxSpeed / nrm;
        }

        for (HalfPlane hp : cons) {
            double[] diff = new double[]{v[0] - hp.p0[0], v[1] - hp.p0[1]};
            double val = dot(diff, hp.n);
            if (val < 0.0) {
                double alpha = dot(new double[]{hp.p0[0] - v[0], hp.p0[1] - v[1]}, hp.n);
                v[0] += alpha * hp.n[0];
                v[1] += alpha * hp.n[1];

                nrm = norm(v);
                if (nrm > maxSpeed) {
                    v[0] *= maxSpeed / nrm;
                    v[1] *= maxSpeed / nrm;
                }
            }
        }
        return v;
    }

    public static void stepORCA(java.util.List<Agent> agents,
                                double T_h, double delta)
    {
        java.util.List<double[]> newV = new java.util.ArrayList<>(agents.size());
        for (int i = 0; i < agents.size(); ++i) {
            java.util.List<HalfPlane> cons =
                    orcaConstraintsForAgent(i, agents, T_h, delta);
            double[] vStar = projectVelocity(agents.get(i).vPref, cons,
                                             agents.get(i).maxSpeed);
            newV.add(vStar);
        }
        // synchronous update
        for (int i = 0; i < agents.size(); ++i) {
            Agent a = agents.get(i);
            double[] vStar = newV.get(i);
            a.v[0] = vStar[0];
            a.v[1] = vStar[1];
            a.p[0] += delta * a.v[0];
            a.p[1] += delta * a.v[1];
        }
    }
}
      

The structure mirrors the Python and C++ implementations, illustrating that the core collision avoidance logic is language-agnostic once the geometry is understood.

MATLAB offers convenient matrix operations and direct integration with Simulink. Below is a function implementing one ORCA-like step for a set of agents, followed by a sketch of how to embed it into a Simulink model using a MATLAB Function block.

function agents = step_orca_matlab(agents, T_h, delta)
% agents: struct array with fields p (2x1), v (2x1), v_pref (2x1), r, max_speed

N = numel(agents);
new_v = zeros(2, N);

for i = 1:N
    cons = orca_constraints_for_agent(i, agents, T_h, delta); % cell array of structs
    v_star = project_velocity(agents(i).v_pref, cons, agents(i).max_speed);
    new_v(:, i) = v_star;
end

for i = 1:N
    agents(i).v = new_v(:, i);
    agents(i).p = agents(i).p + delta * agents(i).v;
end
end

function cons = orca_constraints_for_agent(i, agents, T_h, delta)
ai = agents(i);
cons = {};
for j = 1:numel(agents)
    if j == i, continue; end
    aj = agents(j);
    p_ij = aj.p - ai.p;
    v_ij = ai.v - aj.v;
    R = ai.r + aj.r;
    ttc = time_to_collision(p_ij, v_ij, R, T_h);
    if ~isfinite(ttc), continue; end

    p_coll = p_ij + ttc * v_ij;
    n_ij = normalize_vec(p_coll);
    u_ij = (R * n_ij - p_coll) / max(ttc, delta);
    hp.n = n_ij;
    hp.p0 = ai.v + 0.5 * u_ij;
    cons{end+1} = hp; %#ok<AGROW>
end
end

function ttc = time_to_collision(p_ij, v_ij, R, T_h)
a = dot(v_ij, v_ij);
c = dot(p_ij, p_ij) - R^2;
if c <= 0
    ttc = 0;
    return;
end
if a == 0
    ttc = inf;
    return;
end
b = 2 * dot(p_ij, v_ij);
disc = b^2 - 4*a*c;
if disc < 0
    ttc = inf;
    return;
end
t_min = (-b - sqrt(disc)) / (2*a);
if t_min < 0 || t_min > T_h
    ttc = inf;
else
    ttc = t_min;
end
end

function v = project_velocity(v_pref, cons, max_speed)
v = v_pref(:);
nrm = norm(v);
if nrm > max_speed
    v = (max_speed / nrm) * v;
end
for k = 1:numel(cons)
    hp = cons{k};
    if (v - hp.p0).' * hp.n < 0
        alpha = (hp.p0 - v).' * hp.n;
        v = v + alpha * hp.n;
        nrm = norm(v);
        if nrm > max_speed
            v = (max_speed / nrm) * v;
        end
    end
end
end

function v = normalize_vec(v)
n = norm(v);
if n > 1e-9
    v = v / n;
end
end
      

In Simulink, you can:

1. Represent each agent's position and velocity as signals in a vector of buses or as a concatenated state vector.
2. Use a MATLAB Function block that calls step_orca_matlab at each sample time.
3. Connect the output velocities to low-level dynamics models (e.g., double integrator or unicycle models) implemented using Simulink blocks.

Mathematica is well suited to expressing the half-space intersection as a small convex optimization problem and solving it with NMinimize.

ClearAll[timeToCollision, orcaConstraintsForAgent, projectVelocity, stepORCA];

timeToCollision[pij_, vij_, R_, Th_] := Module[
  {a, b, c, disc, tmin},
  a = vij.vij;
  c = pij.pij - R^2;
  If[c <= 0, Return[0.0]];
  If[a == 0, Return[Infinity]];
  b = 2 pij.vij;
  disc = b^2 - 4 a c;
  If[disc < 0, Return[Infinity]];
  tmin = (-b - Sqrt[disc])/(2 a);
  If[tmin < 0 || tmin > Th, Infinity, tmin]
];

normalize[v_] := If[Norm[v] > 10.^-9, v/Norm[v], v];

orcaConstraintsForAgent[i_, agents_, Th_, delta_] := Module[
  {ai = agents[[i]], cons = {}, Nagents = Length[agents], j, aj, pij, vij,
   R, ttc, pColl, nij, uij},
  For[j = 1, j <= Nagents, j++,
    If[j == i, Continue[]];
    aj = agents[[j]];
    pij = aj["p"] - ai["p"];
    vij = ai["v"] - aj["v"];
    R = ai["r"] + aj["r"];
    ttc = timeToCollision[pij, vij, R, Th];
    If[ttc === Infinity, Continue[]];
    pColl = pij + ttc vij;
    nij = normalize[pColl];
    uij = (R nij - pColl)/Max[ttc, delta];
    AppendTo[cons, <<"n" -> nij, "p0" -> (ai["v"] + 0.5 uij)>>];
  ];
  cons
];

projectVelocity[vPref_, cons_, maxSpeed_] := Module[
  {v = vPref, nrm, k, hp, diff, val, alpha},
  nrm = Norm[v];
  If[nrm > maxSpeed, v = (maxSpeed/nrm) v];
  For[k = 1, k <= Length[cons], k++,
    hp = cons[[k]];
    diff = v - hp["p0"];
    val = diff.hp["n"];
    If[val < 0,
      alpha = (hp["p0"] - v).hp["n"];
      v = v + alpha hp["n"];
      nrm = Norm[v];
      If[nrm > maxSpeed, v = (maxSpeed/nrm) v];
    ];
  ];
  v
];

stepORCA[agents_, Th_, delta_] := Module[
  {Nagents = Length[agents], consList, i, vStarList, newAgents},
  consList = Table[orcaConstraintsForAgent[i, agents, Th, delta],
                   {i, 1, Nagents}];
  vStarList = Table[
    projectVelocity[agents[[i]]["vPref"], consList[[i]],
                    agents[[i]]["maxSpeed"]],
    {i, 1, Nagents}
  ];
  newAgents = Table[
    With[{a = agents[[i]], vStar = vStarList[[i]]},
      <<"p" -> (a["p"] + delta vStar),
         "v" -> vStar,
         "vPref" -> a["vPref"],
         "r" -> a["r"],
         "maxSpeed" -> a["maxSpeed"]>>],
    {i, 1, Nagents}
  ];
  newAgents
];

(* Example initialization *)
agents0 = {
  <<"p" -> {-1., 0.}, "v" -> {0.5, 0.},
     "vPref" -> {0.5, 0.}, "r" -> 0.3, "maxSpeed" -> 1.>>,
  <<"p" -> {1., 0.}, "v" -> {-0.5, 0.},
     "vPref" -> {-0.5, 0.}, "r" -> 0.3, "maxSpeed" -> 1.>>
};

(* Run a few steps *)
FoldList[stepORCA[#1, 5.0, 0.1] &, agents0, Range[0, 20]];
      

More sophisticated formulations could use explicit quadratic programming with inequality constraints representing the half-planes.

Problem 1 (Quadratic Collision Condition). For two robots modeled as disks with relative position \( p_{ij} \) and relative velocity \( v_{ij} \), derive the quadratic coefficients \( a,b,c \) of the function \( \phi(t) = \|p_{ij} + t v_{ij}\|_2^2 - R_{ij}^2 \) and show that \( \phi(t) \leq 0 \) holds on an interval \( [t_{\min}, t_{\max}] \) if \( a > 0 \) and \( b^2 - 4ac \geq 0 \). Express \( t_{\min} \) and \( t_{\max} \).

Solution.

Expand the squared norm:

\[ \|p_{ij} + t v_{ij}\|_2^2 = (p_{ij} + t v_{ij})^\top (p_{ij} + t v_{ij}) = \|p_{ij}\|_2^2 + 2 t\, p_{ij}^\top v_{ij} + t^2 \|v_{ij}\|_2^2. \]

Hence

\[ \phi(t) = \|v_{ij}\|_2^2 t^2 + 2 p_{ij}^\top v_{ij} \, t + (\|p_{ij}\|_2^2 - R_{ij}^2). \]

Identify \( a = \|v_{ij}\|_2^2 \), \( b = 2 p_{ij}^\top v_{ij} \), \( c = \|p_{ij}\|_2^2 - R_{ij}^2 \). For \( a > 0 \) and discriminant \( \Delta = b^2 - 4ac \geq 0 \), the quadratic has two real roots,

\[ t_{\min} = \frac{-b - \sqrt{\Delta}}{2a}, \quad t_{\max} = \frac{-b + \sqrt{\Delta}}{2a}, \]

and the inequality \( \phi(t) \leq 0 \) holds exactly for \( t \in [t_{\min}, t_{\max}] \).

Problem 2 (VO as Minkowski Sum). Consider robot \( i \) as a point and robot \( j \) as a disk \( \mathcal{D}(p_j, R_{ij}) \). Show that collision between \( i \) and \( j \) is equivalent to a collision between a point robot at \( p_i \) and a static obstacle at the Minkowski sum \( \mathcal{D}(p_j,R_{ij}) \), and that \( \mathrm{VO}_{ij} \) can be interpreted as the set of velocities leading the point robot into that disk.

Solution.

A collision occurs if the distance between the centers satisfies \( \|p_i(t) - p_j(t)\|_2 \leq R_{ij} \). Equivalently, keeping robot \( i \) as a point and inflating robot \( j \) to radius \( R_{ij} \), we require that the point \( p_i(t) \) does not enter the disk \( \mathcal{D}(p_j(t), R_{ij}) \). This is exactly the Minkowski sum viewpoint: the configuration obstacle is the set of positions forbidden to the point robot.

If robot \( j \) moves with constant velocity \( v_j \), its inflated disk translates linearly, and we can pass to relative coordinates \( p_{ij}(t) = p_i(t) - p_j(t) \) with relative velocity \( v_{ij} = v_i - v_j \). Collision is then equivalent to the point at \( p_{ij}(t) \) entering a static disk \( \mathcal{D}(0,R_{ij}) \). The VO is precisely the set of \( v_i \) such that the relative ray \( p_{ij} + t(v_i - v_j) \) intersects \( \mathcal{D}(0,R_{ij}) \), which matches the definition.

Problem 3 (Convexity of ORCA Admissible Set). Let \( \mathrm{ORCA}_{ij} = \{ v \mid (v - \hat{v}_i)^\top n_{ij} \geq 0 \} \) be a half-space for each neighbor \( j \in \mathcal{N}_i \). Prove that the intersection \( \mathcal{V}_i^{\mathrm{ORCA}} = \bigcap_{j \in \mathcal{N}_i} \mathrm{ORCA}_{ij} \) is a convex set.

Solution.

Each set \( \mathrm{ORCA}_{ij} \) is defined by a single affine inequality in \( v \). Half-spaces of the form \( \{v \mid a^\top v + b \geq 0\} \) are convex because for any two points \( v_1, v_2 \) satisfying the inequality and any \( \lambda \in [0,1] \), the convex combination also satisfies it:

\[ a^\top (\lambda v_1 + (1-\lambda) v_2) + b = \lambda (a^\top v_1 + b) + (1-\lambda)(a^\top v_2 + b) \geq 0. \]

The intersection of an arbitrary collection of convex sets is convex. Therefore, \( \mathcal{V}_i^{\mathrm{ORCA}} \) is convex, and the local optimization problem in Section 4 is a convex QP.

Problem 4 (Symmetric Responsibility). Consider two robots \( i \) and \( j \) with current relative velocity \( v_{ij} \in \mathrm{VO}_{ij}^0 \). Suppose we compute the minimal correction \( u_{ij} \) that moves \( v_{ij} \) to the closest point on \( \partial \mathrm{VO}_{ij}^0 \). Show that if both robots update velocities as \( v_i^{\mathrm{new}} = v_i + \tfrac{1}{2} u_{ij} \), \( v_j^{\mathrm{new}} = v_j - \tfrac{1}{2} u_{ij} \), then the new relative velocity is outside \( \mathrm{VO}_{ij}^0 \).

Solution.

The new relative velocity is \( v_{ij}^{\mathrm{new}} = v_i^{\mathrm{new}} - v_j^{\mathrm{new}} = (v_i - v_j) + u_{ij} = v_{ij} + u_{ij} \).

By construction, \( u_{ij} \) is the minimal vector that moves \( v_{ij} \) to the closest boundary point \( v_{ij}^{\mathrm{proj}} \in \partial \mathrm{VO}_{ij}^0 \), i.e. \( v_{ij}^{\mathrm{proj}} = v_{ij} + u_{ij} \). Hence \( v_{ij}^{\mathrm{new}} = v_{ij}^{\mathrm{proj}} \) is exactly on the boundary, and a small outward perturbation (often enforced by adding a margin in the construction of \( u_{ij} \)) makes it strictly outside \( \mathrm{VO}_{ij}^0 \). Therefore, the new relative velocity is safe with respect to the VO constraint.

Problem 5 (Time-Horizon Safety). Let \( T_h > 0 \) be a planning horizon. Suppose that at time \( t_k \) the relative velocity \( v_{ij}(t_k) \) between robots \( i \) and \( j \) satisfies \( \mathrm{TTC}_{ij}(v_{ij}(t_k)) \geq T_h \). Show that, under constant velocities over \( [t_k, t_k + T_h] \), no collision occurs in that interval.

Solution.

By definition of the time-to-collision function, \( \mathrm{TTC}_{ij}(v_{ij}) \) is the smallest nonnegative time at which the distance between robots becomes at most \( R_{ij} \). The condition \( \mathrm{TTC}_{ij}(v_{ij}(t_k)) \geq T_h \) means either that there is no collision at all (TTC is infinite) or that the earliest collision time is at least \( T_h \) in the future. Under constant velocities in \( [t_k, t_k + T_h] \), the distance function is exactly the quadratic considered in Section 2, and it cannot reach \( R_{ij} \) before time \( T_h \). Therefore, the robots remain collision-free on the entire interval.

In this lesson we formulated collision avoidance in multi-robot teams as a geometric problem in velocity space. Starting from pairwise continuous collision conditions, we derived velocity obstacles as sets of unsafe velocities, then introduced reciprocal constructions (RVO/ORCA) that distribute responsibility among agents. These constructions yield local convex constraints (half-spaces) whose intersection defines a convex set of admissible velocities. We expressed the choice of a safe velocity as a small quadratic program and implemented simplified ORCA-like schemes in Python, C++, Java, MATLAB/Simulink, and Mathematica, preparing for more advanced multi-robot coordination topics in subsequent lessons.