CBF Path Planning — Teaching Report

1. What Is a Control Barrier Function (CBF)?

A Control Barrier Function is a mathematical tool used in robotics and control theory to enforce safety constraints on a dynamical system.

Instead of designing a controller that simply tries to reach a goal, a CBF wraps around a nominal controller (the "ideal" motion plan) and modifies its output so that the robot never enters an unsafe region.

1.1 Barrier Function Definition

For a circular obstacle centered at p_obs with radius r:

h(x) = ||x - p_obs||² - r²

• If h(x) ≥ 0, the robot is safe (outside the enlarged obstacle).
• If h(x) < 0, the robot is inside the obstacle (unsafe).

1.2 CBF Condition

The safety condition must hold along trajectories:

dh/dt + α·h(x) ≥ 0

• α > 0 is the class-K gain.
• This ensures that if the robot is near the boundary (h ≈ 0), the barrier function does not decrease into the unsafe set.

For a single-integrator ẋ = u:

dh/dt = ∇h · u = 2(x - p_obs) · u

So the constraint becomes:

2(x - p_obs) · u + α·(||x - p_obs||² - r²) ≥ 0

1.3 Safety Filter (QP Formulation)

The safest control u_safe is found by solving a Quadratic Program:

minimize  ||u - u_des||²
subject to  2(x - p_obs) · u + α·h ≥ 0   (for every obstacle)

This finds the control closest to the desired one while satisfying all safety constraints.

---

2. Repository Structure

The repository contains 5 independent implementations of CBF-based path planning, each targeting a different scenario and increasing in complexity:

Folder	Scenario	Key Feature
cbf_2d/	Single agent, static obstacles	Introductory implementation
cbf_dynamic_obstacles/	Single agent, moving obstacles	Relative velocity CBF
cbf_modern/	Multi-agent, predictive warm-start	OSQP with sensitivity-based warm-start
cbf_multiagent/	Multi-agent, decentralized	Inter-agent collision avoidance
cbf_novel/	Advanced hierarchical CBF	Predictive barriers, adaptive safety margins, priority tiers

---

3. Implementation Deep-Dive

3.1 cbf_2d/ — Basic Static Obstacle Avoidance

Purpose: Teach the fundamental CBF loop with a point robot in a 2D world.

• Dynamics (dynamics.py): PointRobot with single integrator ẋ = u.
• Nominal Controller (controllers.py): GoalController computes an attractive force toward the goal and adds repulsive + tangential components when near obstacles to help the QP stay feasible.
• CBF Safety Filter (cbf.py): CBFController solves a small QP using cvxpy + OSQP to project u_des into the safe set.
• Environment (environment.py): Simple list of CircularObstacle objects.
• Visualization (visualization.py): Real-time matplotlib animation + GIF export.

Teaching point: This is the cleanest starting point. The QP has one constraint per obstacle. If the nominal controller pushes the robot directly into an obstacle, the CBF will block the motion (often causing the robot to freeze). This demonstrates why good nominal controllers matter.

---

3.2 cbf_dynamic_obstacles/ — Moving Obstacles

Purpose: Extend the basic CBF to handle obstacles with non-zero velocity.

• Dynamics: Same point robot, but obstacles now have velocity vectors.
• Environment: Dynamic obstacles bounce off walls and repel each other.
• CBF (cbf.py): The constraint changes from:

  grad_h · u + α·h ≥ 0
  

  to:

  grad_h · (u - v_obs) + α·h ≥ 0
  

  This is a relative velocity formulation. It treats the obstacle as moving and applies the CBF to the relative dynamics between robot and obstacle.

Teaching point: When obstacles move, the barrier gradient must be applied to the relative velocity (u - v_obs). If v_obs is ignored, the robot may miscalculate safety and collide with moving objects.

---

3.3 cbf_modern/ — Predictive Warm-Start & Multi-Agent

Purpose: Demonstrate advanced QP solving techniques for real-time performance and multi-agent decentralization.

• Solver: Uses osqp (Python interface) directly instead of cvxpy, giving lower overhead.
• DecentralizedCBFController (cbf.py):
  • Builds linear constraints A·u ≤ b for obstacles and inter-agent collisions.
  • Maintains a history of the last two optimal controls, dual variables, and active constraint sets.
  • Sensitivity predictor: Uses previous KKT conditions to predict the next dual solution, providing a high-quality warm-start for OSQP.
  • Parameter extrapolation: Predicts future A and b matrices using linear extrapolation, enabling the solver to "see around corners."
• Agent (agent.py): Wraps the controller, manages individual robot state and goal switching.
• Metrics: Logs solve time, iterations, and intervention magnitude.

Teaching point: Solving QPs fast enough for real-time control is hard. Warm-starting with history-based predictions can dramatically reduce iterations. The code also shows how to implement a decentralized multi-agent system where each agent its own CBF independently.

---

3.4 cbf_multiagent/ — Decentralized Multi-Agent Collision Avoidance

Purpose: Show how CBFs scale to N agents without a central planner.

• Agent (agent.py): Each agent maintains its own DecentralizedCBFController.
• CBF (cbf.py): Adds inter-agent constraints:

  h_ij = ||x_i - x_j||² - (r_i + r_j + margin)² ≥ 0
  grad_h_ij · (u_i - u_j) + α·h_ij ≥ 0
  

  Other agents' velocities are treated as exogenous inputs (assumed static or estimated).
• Controller: Also includes speed limits ||u||_∞ ≤ 2.0.
• Environment: Multiple agents, dynamic obstacles, and visualization showing all trajectories.

Teaching point: Multi-agent safety is achieved by making each agent a "selfish" optimizer that only cares about its own safety constraints. The system achieves emergent coordination without explicit communication (though in practice, sharing velocities improves performance).

---

3.5 cbf_novel/ — Hierarchical Predictive CBF (HCBF)

Purpose: The most advanced implementation, introducing adaptive margins, predictive constraints, hierarchical priorities, and robust fallbacks.

Key Concepts:

3.5.1 Constraint Priority System

Instead of treating all constraints equally, HCBF categorizes them:

Priority	Meaning	Treatment
CRITICAL	Walls, immovable objects	Hard (no slack, must always satisfy)
HIGH	Obstacles, aggressive threats	Slack allowed but heavily penalized
MEDIUM	Inter-agent, comfort	Slack allowed, lighter penalty
LOW	Smoothness, efficiency	Nice-to-have

3.5.2 Adaptive Safety Margins

The safety margin around obstacles is not fixed:

margin = base_margin * (0.9 + 0.4 * agent_speed_factor + 0.3 * obstacle_speed_factor)
margin += 0.05 * obstacle_uncertainty

Faster agents and more uncertain obstacles get larger margins.

3.5.3 Predictive Barrier Constraints

Instead of enforcing CBF only at the current state, the controller projects forward T steps:

x_pred(t) = x + v·t
h_pred(t) = ||x_pred(t) - x_obs_pred(t)||² - r²
grad_h_pred · (u - v_obs) + α(t)·h_pred(t) ≥ 0

This prevents last-second collisions by accounting for the robot's future trajectory.

3.5.4 Hierarchical QP Solving

The QP is solved in stages: add lower-priority constraints only if higher-priority ones remain feasible. If the final solve is infeasible, a conservative fallback returns a scaled-down u_des (capped at 0.5 m/s) rather than zero, preventing deadlock.

3.5.5 Reached-Agent Immovability

Finished agents are treated as CRITICAL static obstacles by their neighbors, ensuring completed tasks are not disrupted.

Teaching point: Real-world robot safety requires more than textbook CBFs. Adaptive margins handle sensor noise, predictive constraints handle latency, and priority tiers prevent the robot from freezing in complex scenes. The fallback mechanism ensures graceful degradation.

---

4. Mathematical Summary

4.1 Single Integrator CBF

System: ẋ = u Barrier: h(x) = ||x - x_obs||² - r² Gradient: ∇h = 2(x - x_obs) CBF condition: ∇h · u + α·h ≥ 0

4.2 Relative Velocity CBF (Moving Obstacles)

When obstacle moves at v_obs, use relative dynamics:

∇h · (u - v_obs) + α·h ≥ 0

4.3 Multi-Agent CBF

Agent i avoids agent j:

h_ij = ||x_i - x_j||² - (r_i + r_j)²
∇h_ij · (u_i - u_j) + α·h_ij ≥ 0

4.4 Hierarchical QP Objective

min  ||u - u_des||² + λ₁||u - u_prev||² + λ_slack·slack
s.t. hard_constraints
     high_priority_constraints + slack_high ≥ 0
     medium_priority_constraints + slack_med ≥ 0

---

5. Dependencies

All implementations share these core dependencies:

numpy
cvxpy
osqp          (used in cbf_modern)
scipy         (used in cbf_modern)
matplotlib    (visualization)
pillow        (GIF export)

Install via:

pip install -r requirements.txt

---

6. How to Run

Enter any folder and run:

python main.py

Results:

• Live simulation window
• Robot trajectory plot
• Control vectors
• Animated GIF saved to disk

---

7. Common Pitfalls for Students

Symptom	Likely Cause	Fix
Robot freezes near obstacle	Nominal controller pushes directly toward p_obs; CBF blocks all motion	Add tangential velocity or improve nominal controller
QP infeasible (OSQP returns non-optimal)	Too many constraints, too small safety margin, or impossible geometry	Reduce alpha, increase safety margin, use slack variables, or reduce constraint count
Jittery motion	High alpha makes CBF too aggressive	Lower alpha or add control smoothing
Moving obstacles cause collisions	Using static CBF instead of relative velocity CBF	Switch to grad_h · (u - v_obs) formulation
Multi-agent pushing	Agents treat each other as static	Share velocity info or use grad_h · (u_i - u_j) formulation

---

8. Learning Path

1. Start with cbf_2d/ — understand the single obstacle, single integrator loop.
2. Move to cbf_dynamic_obstacles/ — add moving obstacles and relative velocity.
3. Study cbf_modern/ — learn warm-starting, history-based prediction, and direct OSQP usage.
4. Explore cbf_multiagent/ — extend to N agents and decentralized decision making.
5. Master cbf_novel/ — implement robust, production-style HCBF with fallbacks and adaptive margins.

---

9. Advanced Topics & Extensions

• Differential-drive robots: Replace single integrator with unicycle dynamics; CBF becomes ∇h · f(x) + ∇h · g(x)·u + α·h ≥ 0.
• Learning-based nominal controllers: Replace GoalController with a neural network; CBF ensures the learned policy never violates safety.
• Safe MARL: Multiple learning agents sharing the same CBF shield.
• Higher relative degree: For systems where ḣ is a function of u indirectly, use Higher-Order CBFs (HOCBF).
• CLF-CBF-QP: Combine Control Lyapunov Functions (for goal reaching) with CBFs (for safety) in a single QP.

---