Build a CBF safety filter for a mobile robot
One obstacle. The maths is given; you turn it into a working filter.
The problem
A differential-drive robot follows a go-to-goal controller. The controller is fast and simple, and it ignores obstacles. Press Run without filter to watch it crash.
Your job is to write cbf_filter, a safety filter that sits between the controller and the motors. It should leave safe commands untouched and change unsafe ones as little as possible.
Your task
- Compute the barrier value h for the obstacle.
- Write the CBF condition as a·ν ≥ b.
- Leave the command unchanged when it is already safe; otherwise apply the smallest correction.
Submitting runs your filter on 20 hidden, randomly generated scenarios. You pass with zero safety violations, the goal reached in at least 18 tests, and a score of 80 or more.
Theory, one step at a time
1. What does “safe” mean?
We describe safety with a function h(x) that is positive when the robot is safe and zero on the edge of danger. The safe set is every state with h(x) ≥ 0. A safety filter's only job is to keep h(x) ≥ 0 for all time.
2. Why a look-ahead point?
For the robot centre p, the distance to an obstacle changes only when the robot drives (v), never when it turns (ω). A filter built on p can brake but cannot steer, so the robot stops in front of obstacles.
Instead we watch a point q a distance l in front of the robot:
det J = l, so J is invertible: any velocity ν of q can be produced by some (v, ω). We keep q at least D = r + robot_radius + l from the obstacle centre; the triangle inequality then keeps the body itself clear.
3. The barrier h(x)
h is positive outside the keep-out circle of radius D around the obstacle centre o, zero on it and negative inside. The dashed orange circles in the simulation are the boundaries h = 0.
4. The CBF condition
Far from the obstacle h is large, so h may decrease quickly. Near the boundary the allowed decrease shrinks to zero. On the boundary h cannot decrease at all. The robot may approach, but never cross.
5. The safety filter
Among all velocities that satisfy the condition, pick the one closest to what the nominal controller asked for:
With one constraint this has a closed form: keep ν_nom if it satisfies a·ν ≥ b, otherwise project it onto the line a·ν = b. With several obstacles it becomes a small quadratic program.
6. Why the robot stays safe
If dh/dt ≥ −α·h holds at every instant, the comparison lemma gives h(t) ≥ h(0)·e^(−αt). Starting safe (h(0) ≥ 0) means staying safe. The simulator runs the filter at 20 Hz, so tests allow 1 cm of discretisation slack.
7. Tradeoffs
Larger α lets the robot approach obstacles faster; smaller α is more conservative. A larger l makes steering easier but inflates every obstacle by l. A CBF guarantees safety, not progress: badly placed obstacles can still trap the robot.
Function signature
| x | (px, py, θ): pose in m, m, rad |
| u_nom | (v, ω): nominal command in m/s, rad/s |
| obstacles | list of (ox, oy, r) circles in m |
| params | alpha, l, robot_radius, v_min, v_max, omega_max, arena |
| return | (v, ω), the filtered command |