In Chapter 15 you produced local motion commands. In dynamic environments, those commands must be filtered by a collision-avoidance layer that accounts for moving obstacles (Lesson 1) and chooses a velocity that is simultaneously (i) safe for a finite future window and (ii) close to a preferred velocity from a tracker (e.g., Pure Pursuit / Stanley / DWA / TEB outputs).

The key idea: instead of reasoning in configuration space, we reason in velocity space. Each moving obstacle induces a set of robot velocities that would lead to collision. Avoidance becomes: choose \( \mathbf{v} \) outside the union of those forbidden sets while satisfying actuation limits.

flowchart TD
  S["Perception: detect moving objects"] --> P["Prediction: constant-velocity over horizon T"]
  P --> R["Relative state: p_rel, v_rel"]
  R --> V["Build forbidden velocity sets (VO / reciprocal sets)"]
  V --> A["Intersect with admissible velocities (speed/accel/curvature limits)"]
  A --> O["Optimize: pick v closest to v_pref"]
  O --> C["Convert to base commands (v, omega)"]
  C --> U["Execute for dt; repeat"]
        

In this lesson we use a planar disc model (robot and obstacles approximated as discs). This is standard in crowd/AMR literature because it yields clean geometry and fast solvers; later lessons can add social costs and prediction-aware models.

Consider robot A and obstacle/agent B moving in the plane with constant velocities over a short horizon. Let positions be \( \mathbf{p}_A, \mathbf{p}_B \in \mathbb{R}^2 \) and velocities \( \mathbf{v}_A, \mathbf{v}_B \in \mathbb{R}^2 \). Define relative quantities:

\[ \mathbf{p}_{rel} \triangleq \mathbf{p}_B - \mathbf{p}_A,\qquad \mathbf{v}_{rel} \triangleq \mathbf{v}_A - \mathbf{v}_B. \]

Let the robot and obstacle be discs with radii \( r_A \) and \( r_B \), and define the combined radius \( R \triangleq r_A + r_B \). A collision occurs at time \( t \) if:

\[ \|\mathbf{p}_{rel} + t\,\mathbf{v}_{rel}\| \le R. \]

For a finite look-ahead horizon \( T \), the key predicate is: “Does there exist \( t \in [0,T] \) such that the inequality holds?”

Expand the squared distance:

\[ \|\mathbf{p}_{rel} + t\,\mathbf{v}_{rel}\|^2 = (\mathbf{v}_{rel}^\top\mathbf{v}_{rel})\,t^2 + 2(\mathbf{p}_{rel}^\top\mathbf{v}_{rel})\,t + (\mathbf{p}_{rel}^\top\mathbf{p}_{rel}). \]

Collision within the horizon is equivalent to the quadratic inequality:

\[ a t^2 + b t + c \le 0,\quad a=\mathbf{v}_{rel}^\top\mathbf{v}_{rel},\; b=2\mathbf{p}_{rel}^\top\mathbf{v}_{rel},\; c=\mathbf{p}_{rel}^\top\mathbf{p}_{rel} - R^2, \quad t \in [0,T]. \]

Lemma (Quadratic collision test). If \( a > 0 \) and the discriminant \( \Delta = b^2 - 4ac \) satisfies \( \Delta < 0 \), there is no collision at any time. If \( \Delta \ge 0 \), the earliest collision time is: \( t_{hit} = \frac{-b - \sqrt{\Delta}}{2a} \). A collision occurs within horizon iff \( 0 \le t_{hit} \le T \).

This predicate is the computational backbone of VO-based methods: it decides whether a candidate velocity is forbidden.

The Velocity Obstacle induced by B on A over horizon \( T \) is the set of A’s velocities that lead to collision (assuming B maintains \( \mathbf{v}_B \)).

\[ \mathrm{VO}_{A|B}(T) \triangleq \left\{ \mathbf{v}_A \in \mathbb{R}^2 \; \middle| \; \exists t \in (0,T] \text{ such that } \|\mathbf{p}_{rel} + t(\mathbf{v}_A-\mathbf{v}_B)\| \le R \right\}. \]

Define relative-velocity space set: \( \mathrm{VO}^{rel}_{A|B}(T) = \{ \mathbf{v}_{rel} \mid \exists t\in(0,T] : \|\mathbf{p}_{rel} + t\mathbf{v}_{rel}\| \le R\} \). Then: \( \mathrm{VO}_{A|B}(T) = \mathbf{v}_B + \mathrm{VO}^{rel}_{A|B}(T) \).

Geometric interpretation. For discs, \( \mathrm{VO}^{rel}_{A|B}(T) \) is a truncated cone in velocity space: the set of rays from the origin that intersect the disc of radius \( R/T \) centered at \( -\mathbf{p}_{rel}/T \). In practice, membership is tested with the quadratic predicate in Section 2 (more robust than geometry edge cases).

Safety criterion. If the chosen velocity satisfies \( \mathbf{v}_A \notin \mathrm{VO}_{A|B}(T) \) for all nearby B, then there is no collision with any of those obstacles within the horizon under the constant-velocity prediction model.

A pure VO policy can fail in symmetric encounters: both agents choose velocities that “wait” or oscillate because each assumes the other will maintain its velocity. Reciprocal methods address this by splitting responsibility.

A widely used reciprocity model is the Reciprocal Velocity Obstacle (RVO), which shifts the forbidden set so that the “do-nothing” solution corresponds to each agent taking half of the required relative-velocity change. One conceptual form is:

\[ \mathrm{RVO}_{A|B}(T) \triangleq \left\{ \mathbf{v}_A \;\middle|\; \mathbf{v}_A \in \frac{\mathbf{v}_A^{cur}+\mathbf{v}_B^{cur}}{2} + \mathrm{VO}^{rel}_{A|B}(T) \right\}. \]

This expression is interpreted operationally: the “apex” of the forbidden region is centered at the midpoint of current velocities, encouraging each agent to contribute roughly half of the velocity adjustment.

ORCA (Optimal Reciprocal Collision Avoidance). ORCA refines RVO into convex half-plane constraints in velocity space. For each neighbor B, ORCA constructs a half-plane: \( \mathcal{H}_{A|B} = \{\mathbf{v}\mid (\mathbf{v}-\mathbf{v}_{0})^\top\mathbf{n} \ge 0\} \), such that if both agents select velocities in their respective half-planes, collisions are avoided within the horizon. The resulting feasible set is an intersection of half-planes, enabling fast optimization (projection/LP in 2D).

In this lesson’s implementations we use a robust educational approach: sample admissible velocities and reject those that collide within horizon (the same collision predicate used to define VO). Synchronous updates naturally produce reciprocal behavior, and the method is transparent for learning. In production, ORCA/RVO2 libraries provide deterministic convex solvers.

VO/RVO are easiest for holonomic agents. An AMR base is typically nonholonomic (differential drive / Ackermann), so we do avoidance in a command space consistent with the base.

For a differential-drive base, commands are \( (v,\omega) \) with bounds \( |v| \le v_{max} \), \( |\omega| \le \omega_{max} \), and acceleration limits. Locally (small \( dt \)), map to body-frame velocity: \( \mathbf{v}_A = v[\cos\theta,\; \sin\theta]^\top \). Thus the admissible set in world-frame velocity space is approximately a disc (or annulus) clipped by curvature limits.

A generic admissible set model used by VO filters is: \( \mathcal{V}_{adm} = \{\mathbf{v}\in\mathbb{R}^2 \mid \|\mathbf{v}\| \le v_{max}\} \) (optionally refined by acceleration constraints between ticks). The avoidance problem becomes:

\[ \mathbf{v}^\star = \arg\min_{\mathbf{v}\in\mathcal{V}_{adm}} \|\mathbf{v}-\mathbf{v}_{pref}\|^2 \quad \text{s.t.}\quad \mathbf{v} \notin \bigcup_{B\in\mathcal{N}} \mathrm{VO}_{A|B}(T) \;\;(\text{or reciprocal constraints}). \]

The “preferred velocity” \( \mathbf{v}_{pref} \) comes from a tracker or local planner and encodes goal progress. The constraint set encodes safety.

In 2D, there are two common real-time strategies:

• Sampling + rejection (educational, robust): sample candidate velocities in \( \mathcal{V}_{adm} \), reject those that collide within \( [0,T] \), and pick the remaining one closest to \( \mathbf{v}_{pref} \) (optionally add a “time-to-collision” soft cost).
• Convex projection / LP (ORCA-style, deterministic): build half-plane constraints per neighbor and project \( \mathbf{v}_{pref} \) onto the intersection.

The sampling method uses the exact quadratic collision predicate, which is a direct computational realization of the VO definition. It is also easy to integrate with acceleration limits: only sample candidates within a “reachable” set from the current velocity.

The code below simulates multiple disc agents crossing through the origin. Each tick, every agent samples candidate velocities, rejects those that would collide with any neighbor within horizon \( T \), and selects the candidate closest to its goal-directed preferred velocity. Because updates are synchronous, the behavior is “reciprocal-emergent” in practice.

Code: Chapter16_Lesson2.py

"""
Chapter16_Lesson2.py
Autonomous Mobile Robots — Chapter 16, Lesson 2: Velocity Obstacles and Reciprocal Avoidance

Educational reference implementation:
- Time-horizon collision checking in relative motion
- Sampling-based velocity selection outside Velocity Obstacles (VO)
- Multi-agent simulation with synchronous (reciprocal-emergent) updates

This is NOT a full ORCA solver. For production, see RVO2 / ORCA libraries.
"""
from __future__ import annotations
from dataclasses import dataclass
import math
import random
from typing import List, Tuple

import numpy as np


@dataclass
class Agent:
    p: np.ndarray          # position (2,)
    v: np.ndarray          # velocity (2,)
    goal: np.ndarray       # goal position (2,)
    radius: float = 0.3
    v_max: float = 1.0


def norm(x: np.ndarray) -> float:
    return float(np.linalg.norm(x))


def clamp_norm(v: np.ndarray, v_max: float) -> np.ndarray:
    n = norm(v)
    if n <= v_max:
        return v
    return v * (v_max / (n + 1e-12))


def time_to_collision_in_horizon(p_rel: np.ndarray, v_rel: np.ndarray, R: float, T: float) -> Tuple[bool, float]:
    """
    Relative motion: p_rel(t) = p_rel + t v_rel.
    Collision if ||p_rel(t)|| <= R for some t in [0, T].

    Returns: (collides, t_hit)
      - collides: True if collision occurs within horizon
      - t_hit: smallest nonnegative collision time (approx; exact for quadratic), or +inf if none
    """
    # If already intersecting
    if norm(p_rel) <= R:
        return True, 0.0

    a = float(v_rel @ v_rel)
    b = 2.0 * float(p_rel @ v_rel)
    c = float(p_rel @ p_rel) - R * R

    if a < 1e-12:
        return False, float("inf")  # relative velocity ~ 0

    disc = b * b - 4.0 * a * c
    if disc < 0.0:
        return False, float("inf")

    sqrt_disc = math.sqrt(disc)
    t1 = (-b - sqrt_disc) / (2.0 * a)
    t2 = (-b + sqrt_disc) / (2.0 * a)

    # Earliest nonnegative intersection time
    t_hit = float("inf")
    if t1 >= 0.0:
        t_hit = t1
    elif t2 >= 0.0:
        t_hit = 0.0  # started outside, enters immediately in continuous-time view

    if 0.0 <= t_hit <= T:
        return True, t_hit
    return False, float("inf")


def sample_admissible_velocities(v_pref: np.ndarray, v_max: float, n: int = 250, spread: float = 1.2) -> List[np.ndarray]:
    """
    Samples candidate velocities in a disk of radius v_max, biased around v_pref.
    """
    cand: List[np.ndarray] = []
    # Always include v_pref clamped
    cand.append(clamp_norm(v_pref, v_max))

    for _ in range(n):
        # Gaussian around v_pref
        vx = random.gauss(float(v_pref[0]), spread * v_max / 2.0)
        vy = random.gauss(float(v_pref[1]), spread * v_max / 2.0)
        v = np.array([vx, vy], dtype=float)
        v = clamp_norm(v, v_max)
        cand.append(v)

    # Add some uniform samples
    for _ in range(max(50, n // 4)):
        ang = random.random() * 2.0 * math.pi
        r = math.sqrt(random.random()) * v_max
        cand.append(np.array([r * math.cos(ang), r * math.sin(ang)], dtype=float))

    return cand


def choose_velocity_vo(agent: Agent,
                       neighbors: List[Agent],
                       dt: float,
                       T: float,
                       w_pref: float = 1.0,
                       w_safety: float = 2.0,
                       n_samples: int = 250) -> np.ndarray:
    """
    Sampling-based VO avoidance:
    - candidate v is feasible if it does not collide with any neighbor within [0, T]
      under constant-velocity prediction.
    - objective prefers closeness to v_pref (toward goal) and larger time-to-collision.

    Reciprocal effect: all agents update synchronously each step (everyone "reacts").
    """
    # preferred velocity toward goal (simple proportional guidance)
    to_goal = agent.goal - agent.p
    dist = norm(to_goal)
    if dist < 1e-6:
        v_pref = np.zeros(2, dtype=float)
    else:
        v_pref = (to_goal / dist) * min(agent.v_max, dist / max(dt, 1e-3))

    candidates = sample_admissible_velocities(v_pref, agent.v_max, n=n_samples)

    best_v = np.zeros(2, dtype=float)
    best_cost = float("inf")

    for v in candidates:
        feasible = True
        # Safety score via minimum time-to-collision among neighbors
        min_ttc = float("inf")

        for nb in neighbors:
            p_rel = nb.p - agent.p
            v_rel = v - nb.v
            R = agent.radius + nb.radius
            coll, t_hit = time_to_collision_in_horizon(p_rel, v_rel, R, T)
            if coll:
                feasible = False
                break
            # approximate TTC (continuous-time) even if beyond T
            coll_any, t_any = time_to_collision_in_horizon(p_rel, v_rel, R, 1e6)
            if coll_any:
                min_ttc = min(min_ttc, t_any)

        if not feasible:
            continue

        # Cost: prefer v close to v_pref; also prefer larger TTC when relevant
        pref_cost = norm(v - v_pref) ** 2
        safety_cost = 0.0
        if min_ttc < float("inf"):
            safety_cost = 1.0 / (min_ttc + 1e-6)

        cost = w_pref * pref_cost + w_safety * safety_cost

        if cost < best_cost:
            best_cost = cost
            best_v = v

    # If no feasible sample, slow down (failsafe)
    if best_cost == float("inf"):
        return 0.2 * clamp_norm(agent.v, agent.v_max)

    return best_v


def simulate(n_agents: int = 8,
             steps: int = 400,
             dt: float = 0.05,
             T: float = 2.5,
             seed: int = 1):
    """
    Simple multi-agent crossing scenario:
    agents start on a circle, goals are opposite points.
    """
    random.seed(seed)
    np.random.seed(seed)

    agents = []
    R0 = 5.0
    for i in range(n_agents):
        ang = 2.0 * math.pi * i / n_agents
        p = np.array([R0 * math.cos(ang), R0 * math.sin(ang)], dtype=float)
        goal = -p
        agents.append(Agent(p=p, v=np.zeros(2, dtype=float), goal=goal, radius=0.35, v_max=1.2))

    P = np.zeros((steps + 1, n_agents, 2), dtype=float)

    for i, a in enumerate(agents):
        P[0, i] = a.p

    for k in range(steps):
        # Compute next velocities synchronously (reciprocal emergent)
        next_v = []
        for i, a in enumerate(agents):
            neighbors = [agents[j] for j in range(n_agents) if j != i]
            v_new = choose_velocity_vo(a, neighbors, dt=dt, T=T)
            next_v.append(v_new)

        # Update state
        for i, a in enumerate(agents):
            a.v = next_v[i]
            a.p = a.p + dt * a.v
            P[k + 1, i] = a.p

    return P


def main() -> None:
    P = simulate()
    # Save a lightweight CSV trajectory for plotting elsewhere
    # columns: step, agent, x, y
    rows = []
    for k in range(P.shape[0]):
        for i in range(P.shape[1]):
            rows.append((k, i, float(P[k, i, 0]), float(P[k, i, 1])))
    import csv
    with open("Chapter16_Lesson2_trajectories.csv", "w", newline="") as f:
        w = csv.writer(f)
        w.writerow(["step", "agent", "x", "y"])
        w.writerows(rows)
    print("Wrote Chapter16_Lesson2_trajectories.csv")


if __name__ == "__main__":
    main()
      

Libraries: NumPy for vector algebra; optional Matplotlib can be used for plotting trajectories from the exported CSV. For a full ORCA implementation, the community-standard library is “RVO2” (C++ core with language bindings).

The C++ code mirrors the Python logic: quadratic collision test and velocity sampling in a disc. It prints a few checkpoints and can be extended to log trajectories.

Code: Chapter16_Lesson2.cpp

// Chapter16_Lesson2.cpp
// Autonomous Mobile Robots — Chapter 16, Lesson 2: Velocity Obstacles (VO) Sampling Demo
//
// Educational reference implementation:
// - Time-horizon collision check in relative motion (quadratic)
// - Sampling-based velocity choice outside VO
//
// Build (example):
//   g++ -O2 -std=c++17 Chapter16_Lesson2.cpp -o vo_demo
//
// Note: This is a minimal educational demo, not a full ORCA solver.

#include <iostream>
#include <vector>
#include <cmath>
#include <random>
#include <limits>

struct Vec2 {
    double x{0.0}, y{0.0};
    Vec2() = default;
    Vec2(double x_, double y_) : x(x_), y(y_) {}
    Vec2 operator+(const Vec2& o) const { return Vec2{x+o.x, y+o.y}; }
    Vec2 operator-(const Vec2& o) const { return Vec2{x-o.x, y-o.y}; }
    Vec2 operator*(double s) const { return Vec2{x*s, y*s}; }
};

static inline double dot(const Vec2& a, const Vec2& b) { return a.x*b.x + a.y*b.y; }
static inline double norm2(const Vec2& a) { return dot(a,a); }
static inline double norm(const Vec2& a) { return std::sqrt(norm2(a)); }

static inline Vec2 clampNorm(const Vec2& v, double vmax) {
    double n = norm(v);
    if (n <= vmax) return v;
    double s = vmax / (n + 1e-12);
    return v * s;
}

struct Agent {
    Vec2 p;
    Vec2 v;
    Vec2 goal;
    double radius{0.35};
    double vmax{1.2};
};

struct TTCResult {
    bool collides;
    double t_hit;
};

TTCResult timeToCollisionInHorizon(const Vec2& p_rel, const Vec2& v_rel, double R, double T) {
    if (norm(p_rel) <= R) return {true, 0.0};

    double a = norm2(v_rel);
    double b = 2.0 * dot(p_rel, v_rel);
    double c = norm2(p_rel) - R*R;

    if (a < 1e-12) return {false, std::numeric_limits<double>::infinity()};

    double disc = b*b - 4.0*a*c;
    if (disc < 0.0) return {false, std::numeric_limits<double>::infinity()};

    double sdisc = std::sqrt(disc);
    double t1 = (-b - sdisc) / (2.0*a);
    double t2 = (-b + sdisc) / (2.0*a);

    double t_hit = std::numeric_limits<double>::infinity();
    if (t1 >= 0.0) t_hit = t1;
    else if (t2 >= 0.0) t_hit = 0.0;

    if (t_hit >= 0.0 && t_hit <= T) return {true, t_hit};
    return {false, std::numeric_limits<double>::infinity()};
}

std::vector<Vec2> sampleDisk(double vmax, int n, std::mt19937& rng) {
    std::uniform_real_distribution<double> uni(0.0, 1.0);
    std::vector<Vec2> out;
    out.reserve(n);
    for (int i=0;i<n;i++) {
        double ang = 2.0*M_PI*uni(rng);
        double r = std::sqrt(uni(rng))*vmax;
        out.emplace_back(r*std::cos(ang), r*std::sin(ang));
    }
    return out;
}

Vec2 chooseVelocityVO(const Agent& a, const std::vector<Agent>& neighbors, double dt, double T, std::mt19937& rng) {
    Vec2 toGoal = a.goal - a.p;
    double d = norm(toGoal);
    Vec2 v_pref{0.0, 0.0};
    if (d > 1e-9) {
        double spd = std::min(a.vmax, d / std::max(dt, 1e-3));
        v_pref = toGoal * (spd / (d + 1e-12));
    }

    auto cand = sampleDisk(a.vmax, 1200, rng);
    cand.push_back(clampNorm(v_pref, a.vmax));

    Vec2 best{0.0, 0.0};
    double bestCost = std::numeric_limits<double>::infinity();

    for (const auto& v : cand) {
        bool feasible = true;
        double min_ttc = std::numeric_limits<double>::infinity();

        for (const auto& nb : neighbors) {
            Vec2 p_rel = nb.p - a.p;
            Vec2 v_rel = v - nb.v;
            double R = a.radius + nb.radius;

            auto res = timeToCollisionInHorizon(p_rel, v_rel, R, T);
            if (res.collides) { feasible = false; break; }

            auto resAny = timeToCollisionInHorizon(p_rel, v_rel, R, 1e6);
            if (resAny.collides) min_ttc = std::min(min_ttc, resAny.t_hit);
        }

        if (!feasible) continue;

        double prefCost = norm2(v - v_pref);
        double safetyCost = 0.0;
        if (std::isfinite(min_ttc)) safetyCost = 1.0 / (min_ttc + 1e-6);

        double cost = 1.0*prefCost + 2.0*safetyCost;
        if (cost < bestCost) { bestCost = cost; best = v; }
    }

    if (!std::isfinite(bestCost)) return a.v * 0.2; // failsafe slow down
    return best;
}

int main() {
    std::mt19937 rng(1);

    const int N = 8;
    const int steps = 400;
    const double dt = 0.05;
    const double T = 2.5;
    const double R0 = 5.0;

    std::vector<Agent> agents;
    agents.reserve(N);

    for (int i=0;i<N;i++) {
        double ang = 2.0*M_PI*i/N;
        Vec2 p{R0*std::cos(ang), R0*std::sin(ang)};
        Agent a;
        a.p = p;
        a.v = Vec2{0.0, 0.0};
        a.goal = Vec2{-p.x, -p.y};
        agents.push_back(a);
    }

    for (int k=0;k<steps;k++) {
        std::vector<Vec2> nextV(N);
        for (int i=0;i<N;i++) {
            std::vector<Agent> neigh;
            neigh.reserve(N-1);
            for (int j=0;j<N;j++) if (j!=i) neigh.push_back(agents[j]);
            nextV[i] = chooseVelocityVO(agents[i], neigh, dt, T, rng);
        }
        for (int i=0;i<N;i++) {
            agents[i].v = nextV[i];
            agents[i].p = agents[i].p + agents[i].v * dt;
        }
        if (k % 50 == 0) {
            std::cout << "Step " << k << ": agent0 p=("
                      << agents[0].p.x << "," << agents[0].p.y << ")\n";
        }
    }

    std::cout << "Done.\n";
    return 0;
}
      

Java version of the same algorithmic core. It is structured to be easily adapted into ROS Java nodes or Android-based robots (though modern AMR stacks typically use C++).

Code: Chapter16_Lesson2.java

// Chapter16_Lesson2.java
// Autonomous Mobile Robots — Chapter 16, Lesson 2: Velocity Obstacles (VO) Sampling Demo
//
// Educational reference implementation (minimal):
// - Time-horizon collision check in relative motion
// - Sampling-based safe velocity selection
//
// Compile/run:
//   javac Chapter16_Lesson2.java
//   java Chapter16_Lesson2

import java.util.*;

public class Chapter16_Lesson2 {

    static class Vec2 {
        double x, y;
        Vec2(double x, double y) { this.x = x; this.y = y; }
        Vec2 add(Vec2 o) { return new Vec2(x + o.x, y + o.y); }
        Vec2 sub(Vec2 o) { return new Vec2(x - o.x, y - o.y); }
        Vec2 mul(double s) { return new Vec2(x * s, y * s); }
    }

    static double dot(Vec2 a, Vec2 b) { return a.x*b.x + a.y*b.y; }
    static double norm2(Vec2 a) { return dot(a,a); }
    static double norm(Vec2 a) { return Math.sqrt(norm2(a)); }

    static Vec2 clampNorm(Vec2 v, double vmax) {
        double n = norm(v);
        if (n <= vmax) return v;
        double s = vmax / (n + 1e-12);
        return v.mul(s);
    }

    static class Agent {
        Vec2 p, v, goal;
        double radius = 0.35;
        double vmax = 1.2;
        Agent(Vec2 p, Vec2 v, Vec2 goal) {
            this.p = p; this.v = v; this.goal = goal;
        }
    }

    static class TTC {
        boolean collides;
        double tHit;
        TTC(boolean c, double t) { collides=c; tHit=t; }
    }

    static TTC timeToCollisionInHorizon(Vec2 pRel, Vec2 vRel, double R, double T) {
        if (norm(pRel) <= R) return new TTC(true, 0.0);

        double a = norm2(vRel);
        double b = 2.0 * dot(pRel, vRel);
        double c = norm2(pRel) - R*R;

        if (a < 1e-12) return new TTC(false, Double.POSITIVE_INFINITY);

        double disc = b*b - 4.0*a*c;
        if (disc < 0.0) return new TTC(false, Double.POSITIVE_INFINITY);

        double sdisc = Math.sqrt(disc);
        double t1 = (-b - sdisc) / (2.0*a);
        double t2 = (-b + sdisc) / (2.0*a);

        double tHit = Double.POSITIVE_INFINITY;
        if (t1 >= 0.0) tHit = t1;
        else if (t2 >= 0.0) tHit = 0.0;

        if (tHit >= 0.0 && tHit <= T) return new TTC(true, tHit);
        return new TTC(false, Double.POSITIVE_INFINITY);
    }

    static List<Vec2> sampleDisk(double vmax, int n, Random rng) {
        ArrayList<Vec2> out = new ArrayList<>(n);
        for (int i=0;i<n;i++) {
            double ang = 2.0*Math.PI*rng.nextDouble();
            double r = Math.sqrt(rng.nextDouble())*vmax;
            out.add(new Vec2(r*Math.cos(ang), r*Math.sin(ang)));
        }
        return out;
    }

    static Vec2 chooseVelocityVO(Agent a, List<Agent> neighbors, double dt, double T, Random rng) {
        Vec2 toGoal = a.goal.sub(a.p);
        double d = norm(toGoal);
        Vec2 vPref = new Vec2(0.0, 0.0);
        if (d > 1e-9) {
            double spd = Math.min(a.vmax, d / Math.max(dt, 1e-3));
            vPref = toGoal.mul(spd / (d + 1e-12));
        }

        List<Vec2> cand = sampleDisk(a.vmax, 1200, rng);
        cand.add(clampNorm(vPref, a.vmax));

        Vec2 best = new Vec2(0.0, 0.0);
        double bestCost = Double.POSITIVE_INFINITY;

        for (Vec2 v : cand) {
            boolean feasible = true;
            double minTTC = Double.POSITIVE_INFINITY;

            for (Agent nb : neighbors) {
                Vec2 pRel = nb.p.sub(a.p);
                Vec2 vRel = v.sub(nb.v);
                double R = a.radius + nb.radius;

                TTC res = timeToCollisionInHorizon(pRel, vRel, R, T);
                if (res.collides) { feasible=false; break; }

                TTC resAny = timeToCollisionInHorizon(pRel, vRel, R, 1e6);
                if (resAny.collides) minTTC = Math.min(minTTC, resAny.tHit);
            }

            if (!feasible) continue;

            double prefCost = norm2(v.sub(vPref));
            double safetyCost = 0.0;
            if (Double.isFinite(minTTC)) safetyCost = 1.0 / (minTTC + 1e-6);

            double cost = 1.0*prefCost + 2.0*safetyCost;
            if (cost < bestCost) { bestCost = cost; best = v; }
        }

        if (!Double.isFinite(bestCost)) return a.v.mul(0.2);
        return best;
    }

    public static void main(String[] args) {
        Random rng = new Random(1);

        final int N = 8;
        final int steps = 400;
        final double dt = 0.05;
        final double T = 2.5;
        final double R0 = 5.0;

        ArrayList<Agent> agents = new ArrayList<>(N);
        for (int i=0;i<N;i++) {
            double ang = 2.0*Math.PI*i/N;
            Vec2 p = new Vec2(R0*Math.cos(ang), R0*Math.sin(ang));
            Vec2 goal = new Vec2(-p.x, -p.y);
            agents.add(new Agent(p, new Vec2(0.0,0.0), goal));
        }

        for (int k=0;k<steps;k++) {
            Vec2[] nextV = new Vec2[N];

            for (int i=0;i<N;i++) {
                ArrayList<Agent> neigh = new ArrayList<>(N-1);
                for (int j=0;j<N;j++) if (j!=i) neigh.add(agents.get(j));
                nextV[i] = chooseVelocityVO(agents.get(i), neigh, dt, T, rng);
            }

            for (int i=0;i<N;i++) {
                Agent a = agents.get(i);
                a.v = nextV[i];
                a.p = a.p.add(a.v.mul(dt));
            }

            if (k % 50 == 0) {
                Agent a0 = agents.get(0);
                System.out.printf("Step %d: agent0 p=(%.3f, %.3f)%n", k, a0.p.x, a0.p.y);
            }
        }
        System.out.println("Done.");
    }
}
      

MATLAB script version. For Simulink, the core function chooseVelocityVO can be placed into a MATLAB Function block, with neighbor states provided by buses (or a fixed maximum number of neighbors).

Code: Chapter16_Lesson2.m

% Chapter16_Lesson2.m
% Autonomous Mobile Robots — Chapter 16, Lesson 2: Velocity Obstacles (VO) Sampling Demo
%
% Educational reference implementation:
% - Finite-horizon collision check in relative motion
% - Sampling-based velocity selection outside VO
%
% Simulink note:
%   You can paste chooseVelocityVO() into a MATLAB Function block and feed
%   current state + neighbor states each control tick.

function Chapter16_Lesson2()
    rng(1);

    N = 8;
    steps = 400;
    dt = 0.05;
    T  = 2.5;
    R0 = 5.0;

    agents = struct();
    for i=1:N
        ang = 2*pi*(i-1)/N;
        p = [R0*cos(ang); R0*sin(ang)];
        agents(i).p = p;
        agents(i).v = [0;0];
        agents(i).goal = -p;
        agents(i).radius = 0.35;
        agents(i).vmax = 1.2;
    end

    for k=1:steps
        nextV = zeros(2,N);
        for i=1:N
            neigh = agents([1:i-1, i+1:N]);
            nextV(:,i) = chooseVelocityVO(agents(i), neigh, dt, T);
        end

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

        if mod(k,50)==0
            fprintf('Step %d: agent1 p=(%.3f, %.3f)\n', k, agents(1).p(1), agents(1).p(2));
        end
    end

    disp('Done.');
end

function v = chooseVelocityVO(a, neighbors, dt, T)
    % Preferred velocity to goal
    toGoal = a.goal - a.p;
    d = norm(toGoal);
    if d < 1e-9
        vPref = [0;0];
    else
        spd = min(a.vmax, d / max(dt, 1e-3));
        vPref = toGoal / (d + 1e-12) * spd;
    end

    % Candidate velocities
    cand = sampleDisk(a.vmax, 1200);
    cand = [cand, clampNorm(vPref, a.vmax)];

    bestCost = inf;
    v = [0;0];

    for c = 1:size(cand,2)
        vc = cand(:,c);
        feasible = true;
        minTTC = inf;

        for j = 1:numel(neighbors)
            nb = neighbors(j);
            pRel = nb.p - a.p;
            vRel = vc - nb.v;
            R = a.radius + nb.radius;

            [coll, ~] = ttcInHorizon(pRel, vRel, R, T);
            if coll
                feasible = false;
                break;
            end

            [collAny, tAny] = ttcInHorizon(pRel, vRel, R, 1e6);
            if collAny
                minTTC = min(minTTC, tAny);
            end
        end

        if ~feasible
            continue;
        end

        prefCost = norm(vc - vPref)^2;
        safetyCost = 0.0;
        if isfinite(minTTC)
            safetyCost = 1.0 / (minTTC + 1e-6);
        end

        cost = 1.0*prefCost + 2.0*safetyCost;
        if cost < bestCost
            bestCost = cost;
            v = vc;
        end
    end

    if isinf(bestCost)
        v = 0.2 * clampNorm(a.v, a.vmax); % failsafe slow down
    end
end

function [collides, tHit] = ttcInHorizon(pRel, vRel, R, T)
    if norm(pRel) <= R
        collides = true;
        tHit = 0.0;
        return;
    end

    a = dot(vRel,vRel);
    b = 2*dot(pRel,vRel);
    c = dot(pRel,pRel) - R^2;

    if a < 1e-12
        collides = false;
        tHit = inf;
        return;
    end

    disc = b^2 - 4*a*c;
    if disc < 0
        collides = false;
        tHit = inf;
        return;
    end

    sdisc = sqrt(disc);
    t1 = (-b - sdisc)/(2*a);
    t2 = (-b + sdisc)/(2*a);

    tHit = inf;
    if t1 >= 0
        tHit = t1;
    elseif t2 >= 0
        tHit = 0.0;
    end

    collides = (tHit >= 0) && (tHit <= T);
    if ~collides
        tHit = inf;
    end
end

function V = sampleDisk(vmax, n)
    V = zeros(2,n);
    for i=1:n
        ang = 2*pi*rand();
        r = sqrt(rand())*vmax;
        V(:,i) = [r*cos(ang); r*sin(ang)];
    end
end

function v = clampNorm(v, vmax)
    n = norm(v);
    if n <= vmax
        return;
    end
    v = v * (vmax/(n + 1e-12));
end
      

This code is written in Wolfram Language and can be pasted into a notebook. It implements the same core logic: collision predicate + velocity sampling + synchronous multi-agent update.

Code: Chapter16_Lesson2.nb

(* Chapter16_Lesson2.nb
   Autonomous Mobile Robots — Chapter 16, Lesson 2: Velocity Obstacles (VO) Sampling Demo

   This is Wolfram Language code that you can paste into a notebook.
*)

ClearAll[norm, ttcInHorizon, sampleDisk, clampNorm, chooseVelocityVO, simulate];

norm[v_] := Sqrt[v.v];

clampNorm[v_, vmax_] := Module[{n = norm[v]},
  If[n <= vmax, v, (vmax/(n + 10^-12)) v]
];

ttcInHorizon[pRel_, vRel_, R_, T_] := Module[
  {a, b, c, disc, t1, t2, tHit = Infinity},
  If[norm[pRel] <= R, Return[{True, 0.0}]];
  a = vRel.vRel;
  b = 2.0 (pRel.vRel);
  c = (pRel.pRel) - R^2;
  If[a < 10^-12, Return[{False, Infinity}]];
  disc = b^2 - 4 a c;
  If[disc < 0, Return[{False, Infinity}]];
  t1 = (-b - Sqrt[disc])/(2 a);
  t2 = (-b + Sqrt[disc])/(2 a);
  If[t1 >= 0, tHit = t1, If[t2 >= 0, tHit = 0.0]];
  If[0 <= tHit <= T, {True, tHit}, {False, Infinity}]
];

sampleDisk[vmax_, n_] := Table[
  Module[{ang = 2 Pi RandomReal[], r = Sqrt[RandomReal[]] vmax},
    {r Cos[ang], r Sin[ang]}
  ],
  {n}
];

chooseVelocityVO[a_Association, neighbors_List, dt_, T_] := Module[
  {toGoal, d, vmax, vPref, cand, best = {0., 0.}, bestCost = Infinity, vc,
   feasible, minTTC, pRel, vRel, R, coll, tHit, collAny, tAny, prefCost, safetyCost, cost},

  vmax = a["vmax"];
  toGoal = a["goal"] - a["p"];
  d = norm[toGoal];
  vPref = If[d < 10^-9, {0., 0.}, (Min[vmax, d/Max[dt, 10^-3]]/(d + 10^-12)) toGoal];

  cand = Join[sampleDisk[vmax, 1200], {clampNorm[vPref, vmax]}];

  Do[
    vc = cand[[i]];
    feasible = True;
    minTTC = Infinity;

    Do[
      pRel = neighbors[[j]]["p"] - a["p"];
      vRel = vc - neighbors[[j]]["v"];
      R = a["radius"] + neighbors[[j]]["radius"];

      {coll, tHit} = ttcInHorizon[pRel, vRel, R, T];
      If[coll, feasible = False; Break[]];

      {collAny, tAny} = ttcInHorizon[pRel, vRel, R, 10^6];
      If[collAny, minTTC = Min[minTTC, tAny]];
      ,
      {j, Length[neighbors]}
    ];

    If[!feasible, Continue[]];

    prefCost = norm[vc - vPref]^2;
    safetyCost = If[minTTC < Infinity, 1.0/(minTTC + 10^-6), 0.0];
    cost = 1.0 prefCost + 2.0 safetyCost;

    If[cost < bestCost, bestCost = cost; best = vc];
    ,
    {i, Length[cand]}
  ];

  If[bestCost === Infinity, 0.2 clampNorm[a["v"], vmax], best]
];

simulate[nAgents_: 8, steps_: 400, dt_: 0.05, T_: 2.5] := Module[
  {R0 = 5.0, agents, nextV, k, i, neighbors},

  agents = Table[
    Module[{ang = 2 Pi (i - 1)/nAgents, p},
      p = {R0 Cos[ang], R0 Sin[ang]};
      <|"p" -> p, "v" -> {0., 0.}, "goal" -> -p, "radius" -> 0.35, "vmax" -> 1.2|>
    ],
    {i, nAgents}
  ];

  Do[
    nextV = Table[
      neighbors = Delete[agents, i];
      chooseVelocityVO[agents[[i]], neighbors, dt, T],
      {i, nAgents}
    ];

    Do[
      agents[[i, "v"]] = nextV[[i]];
      agents[[i, "p"]] = agents[[i, "p"]] + dt agents[[i, "v"]];
      ,
      {i, nAgents}
    ];
    ,
    {k, steps}
  ];

  agents
];

(* Run a quick demo: *)
SeedRandom[1];
finalAgents = simulate[];
finalAgents[[1]]
      

Problem 1 (VO membership via quadratic test): Let \( \mathbf{p}_{rel} \) and \( \mathbf{v}_{rel} \) be constant. Show that collision within horizon \( T \) occurs iff the quadratic \( a t^2 + b t + c \le 0 \) has a solution in \( [0,T] \), where \( a=\mathbf{v}_{rel}^\top\mathbf{v}_{rel} \), \( b=2\mathbf{p}_{rel}^\top\mathbf{v}_{rel} \), \( c=\mathbf{p}_{rel}^\top\mathbf{p}_{rel}-R^2 \).

Solution: Start from \( \|\mathbf{p}_{rel}+t\mathbf{v}_{rel}\| \le R \). Squaring both sides (valid since norms are nonnegative) yields \( \|\mathbf{p}_{rel}+t\mathbf{v}_{rel}\|^2 \le R^2 \). Expanding gives \( a t^2 + b t + c \le 0 \). Therefore existence of collision time in \( [0,T] \) is exactly the existence of a feasible \( t \) solving the quadratic inequality on that interval. If \( \Delta=b^2-4ac < 0 \), the quadratic has no real roots so it never crosses zero: no collision. If \( \Delta \ge 0 \), the first contact time is the smallest nonnegative root \( t_{hit}=\frac{-b-\sqrt{\Delta}}{2a} \); collision within horizon holds iff \( 0 \le t_{hit} \le T \).

Problem 2 (Effect of horizon \(T\)): Consider fixed \( \mathbf{p}_{rel}\neq \mathbf{0} \) and radius \( R \). Compare the sets \( \mathrm{VO}_{A|B}(T_1) \) and \( \mathrm{VO}_{A|B}(T_2) \) for \( 0 < T_1 < T_2 \). Which one is larger?

Solution: If a velocity causes collision within \( T_1 \), it certainly causes collision within \( T_2 \) because \( [0,T_1] \subset [0,T_2] \). Hence:

\[ 0 < T_1 < T_2 \quad \Rightarrow \quad \mathrm{VO}_{A|B}(T_1) \subseteq \mathrm{VO}_{A|B}(T_2). \]

A longer horizon is safer but more restrictive and can induce conservatism or deadlocks if not paired with reciprocity or progress costs.

Problem 3 (Programming exercise: visualize unsafe velocities): Implement a function that samples velocities in a disc and marks them unsafe if the quadratic collision test indicates collision within horizon. Plot safe vs unsafe samples in velocity space for a moving obstacle.

Solution: Use the following reference script (produces a velocity-space scatter plot).

Code: Chapter16_Lesson2_Ex1.py

"""
Chapter16_Lesson2_Ex1.py
Exercise: Single agent + single moving obstacle — sample a safe velocity
and visualize unsafe velocities (in velocity space) under a finite horizon.
"""
from __future__ import annotations
import math
import random
import numpy as np

try:
    import matplotlib.pyplot as plt
except Exception:
    plt = None


def norm(x):
    return float(np.linalg.norm(x))


def time_to_collision(p_rel, v_rel, R, T):
    # Quadratic intersection with ||p_rel + t v_rel|| = R
    if norm(p_rel) <= R:
        return True, 0.0
    a = float(v_rel @ v_rel)
    b = 2.0 * float(p_rel @ v_rel)
    c = float(p_rel @ p_rel) - R * R
    if a < 1e-12:
        return False, float("inf")
    disc = b * b - 4.0 * a * c
    if disc < 0:
        return False, float("inf")
    t = (-b - math.sqrt(disc)) / (2.0 * a)
    return (0.0 <= t <= T), t


def sample_disk(v_max, n):
    out = []
    for _ in range(n):
        ang = random.random() * 2.0 * math.pi
        r = math.sqrt(random.random()) * v_max
        out.append(np.array([r*math.cos(ang), r*math.sin(ang)], dtype=float))
    return out


def main():
    random.seed(0)
    np.random.seed(0)

    # Agent and obstacle configuration
    p_a = np.array([0.0, 0.0])
    v_a = np.array([0.0, 0.0])
    p_b = np.array([2.0, 0.8])
    v_b = np.array([-0.4, -0.05])

    r_a, r_b = 0.35, 0.35
    R = r_a + r_b
    T = 3.0
    v_max = 1.2

    # Preferred velocity (toward a goal)
    goal = np.array([4.0, 0.0])
    v_pref = (goal - p_a)
    v_pref = v_pref / (norm(v_pref) + 1e-12) * 1.0

    candidates = sample_disk(v_max, 4000)
    safe = []
    unsafe = []
    for v in candidates:
        coll, t = time_to_collision(p_b - p_a, v - v_b, R, T)
        if coll:
            unsafe.append(v)
        else:
            safe.append(v)

    # Choose the safe velocity closest to v_pref
    safe = np.array(safe) if safe else np.zeros((0,2))
    unsafe = np.array(unsafe) if unsafe else np.zeros((0,2))
    if safe.shape[0] > 0:
        idx = np.argmin(np.sum((safe - v_pref[None,:])**2, axis=1))
        v_star = safe[idx]
    else:
        v_star = np.zeros(2)

    print("Preferred v:", v_pref)
    print("Chosen safe v*:", v_star)

    if plt is None:
        print("matplotlib not available; skipping plot.")
        return

    plt.figure(figsize=(7,6))
    if unsafe.shape[0] > 0:
        plt.scatter(unsafe[:,0], unsafe[:,1], s=5, alpha=0.15, label="Unsafe (inside VO_T)")
    if safe.shape[0] > 0:
        plt.scatter(safe[:,0], safe[:,1], s=6, alpha=0.25, label="Safe")
    plt.scatter([v_pref[0]], [v_pref[1]], marker="x", s=120, label="v_pref")
    plt.scatter([v_star[0]], [v_star[1]], marker="o", s=120, label="v*")
    plt.axis("equal")
    plt.xlabel("v_x")
    plt.ylabel("v_y")
    plt.title("Velocity-space safe/unsafe samples (finite horizon)")
    plt.legend()
    plt.grid(True)
    plt.show()


if __name__ == "__main__":
    main()
      

Problem 4 (Why ORCA becomes convex): Explain why replacing each forbidden region by a supporting half-plane constraint yields a convex feasible set. What is the computational advantage in 2D?

Solution: Each half-plane is convex. The intersection of convex sets is convex, hence the feasible set \( \mathcal{F} = \mathcal{V}_{adm} \cap \bigcap_{B\in\mathcal{N}} \mathcal{H}_{A|B} \) is convex when \( \mathcal{V}_{adm} \) is convex (e.g., a disc or polygon). In 2D, projecting \( \mathbf{v}_{pref} \) onto \( \mathcal{F} \) can be done with linear-time incremental constraint handling (specialized 2D LP / projection algorithms), enabling real-time performance with many neighbors.

flowchart TD
  P["v_pref"] --> K["Add constraint half-planes one by one"]
  K --> I["Maintain feasible intersection region (convex)"]
  I --> J["Project v_pref to nearest feasible v*"]
  J --> E["Execute (dt) and rebuild constraints"]
        

Velocity Obstacles convert dynamic collision avoidance into a velocity-space feasibility problem using relative motion. For disc approximations and finite horizons, collision checking reduces to a quadratic predicate, enabling fast rejection of unsafe velocities. Reciprocal approaches (RVO/ORCA) address symmetric deadlocks by sharing avoidance responsibility and, in ORCA, producing convex half-plane constraints that admit deterministic real-time solvers. The provided implementations emphasize transparency: a sampling-based VO filter that integrates naturally with AMR local-planning outputs via a preferred velocity.