Skip to the lesson
FixTheRobot Explore Robotics / Navigation / M7 Safety, Uncertainty and Recovery / M7-T2

Navigation · Theory · M7

M7-T2 — Safety Filters: Control Barrier Functions for Navigation

Let the planner be ambitious, and put one small piece of code between it and the wheels that says no only when it has to.

  • Lesson 2 of 7
  • Module 7: Safety, Uncertainty and Recovery
  • Theory
  • About 10 minutes to read

Planners and local controllers optimise for progress and comfort. They also make mistakes: a stale costmap, an optimistic prediction, a controller that overshoots. A safety filter is a last, small, well-understood stage that takes whatever command the rest of the stack wants and changes it as little as possible if it would break a safety rule. This lesson explains the idea, works one correction by hand, and says where it helps and where it does not.

Learning objectives

After this lesson, you should be able to:

  • Describe a safety filter as a minimal correction of a nominal command, and place it in the navigation stack.
  • Write the control barrier function condition for a distance-to-obstacle barrier and what the parameter α means.
  • Compute the filtered velocity for a single constraint with the closed-form projection.
  • Name the limits of a filter: model errors, infeasibility, deadlock and the need for correct inputs.

The intuition

A road has a hard shoulder and a guard rail. A driver rarely touches the rail, because they steer well. The rail is there for the moment they do not. A safety filter is a guard rail in software: if the command stays inside the safe region, it passes through untouched. If it would leave, it is pushed back to the edge, and only in the direction that matters.

Two ideas make this precise. The first is a safe set: the states that are acceptable, written as h(x) ≥ 0. For a robot that must stay at least R from an obstacle, h is the distance to the obstacle minus R; it is positive when safe, zero at the edge and negative once the rule is broken. The second is a barrier condition: it is not enough to be inside the set, the command must not let h fall faster than a chosen rate. The farther the robot is from the edge, the faster it may approach; near the edge it may hardly approach at all.

The filter asks one question each control cycle: “is the nominal command compatible with that rule?” If yes, it passes. If not, it looks for the closest command that is. In the figure, the planner wants to move towards the obstacle at a speed that the rule forbids. The filter removes the excess along the line to the obstacle and leaves the sideways motion alone, so the robot still slides along the obstacle instead of stopping dead.

A safety filter corrects a command minimallyA robot approaches a circular obstacle. The planner asks for a velocity pointing towards the obstacle that would close the gap too fast. The filter moves the velocity to the boundary of the allowed set, along the direction of the obstacle only, and leaves the sideways part unchanged. obstacle distance 1.0 m, h = 0.6 m limit n·u = −α h = −0.6 u_nom u* Scout n (away from obstacle) the correction (0.32 m/s) is along n only
A velocity command and its filtered version. The green line is the boundary of the allowed velocities; the correction is the shortest move onto it.

The mathematics

The barrier condition

Take a robot whose state is x and whose input is u, with dynamics that are linear in the input, ẋ = f(x) + g(x) u. Let the safe set be C = { x : h(x) ≥ 0 }. A control barrier function (CBF) condition requires, at every state, an input that satisfies

CBF condition
Lf h(x)+Lg h(x)⋅u≥−α⋅h(x)L_{f}\,h(x)+L_{g}\,h(x)\cdot u\ge -\alpha \cdot h(x)
Lf h=∇h⋅f(x)L_{f}\,h=\nabla h\cdot f(x)
Lg h=∇h⋅g(x)L_{g}\,h=\nabla h\cdot g(x)

With α a positive constant (in general a class-K function), the condition lets h shrink no faster than ḣ = −α h, so h(t) ≥ h(0) e^(−α t): h never reaches zero in finite time, and the robot stays in the safe set. The constant 1/α is a time constant: the larger α is, the closer to the edge the robot may drive before it must stop approaching.

The filter as a small optimisation

Given a nominal command unom from the planner or controller, the filtered command solves a quadratic programme:

CBF quadratic programme
u∗=arg min⁡u 12∥u−unom ∥2u^{*}=\operatorname*{arg\,min}_{u}\,\tfrac{1}{2}\lVert u-u_{\text{nom}}\,\rVert ^{2}
subject to
Lf h+Lg h⋅u≥−α hL_{f}\,h+L_{g}\,h\cdot u\ge -\alpha \,h
other constraints: speed and acceleration limits
unom
what the rest of the stack wants to do.
h(x)
barrier function, ≥ 0 when safe. Here the distance to an obstacle minus a required clearance R.
α
how aggressively the robot may approach the boundary, in 1/s.
a, b
write the one constraint as aᵀu ≥ b, with a = Lg hᵀ and b = −α h − Lf h.

One constraint has a closed form

For a single linear constraint the closest point of the allowed half-space is a projection, and the correction is along a only:

Closed-form filter
if a⊤ unom≥b: u∗=unom\text{if}\,a^{\top}\,u_{\text{nom}}\ge b:\,u^{*}=u_{\text{nom}}
already safe
else
: u∗=unom+(b−a⊤ unoma⊤a)⋅a:\,u^{*}=u_{\text{nom}}+(\frac{b-a^{\top}\,u_{\text{nom}}}{a^{\top}a})\cdot a

With several obstacles there is one constraint for each, and a QP solver finds the closest command that satisfies them all. When the constraints contradict each other, or contradict the input limits, there is no solution (the QP is infeasible), and the filter needs a defined fallback, normally a stop.

Distance to a circular obstacle

For a robot at position p moving with velocity u (so ẋ = u) and an obstacle at po, take h = ‖p − po‖ − R. Then ∇h = n = (p − po) / ‖p − po‖, the unit vector pointing away from the obstacle, and the constraint is nᵀu ≥ −α h. Head-on, this is simply v ≤ α h: speed proportional to the room left. For a car-like robot or a unicycle whose position barrier depends only on the position, the constraint involves the forward speed alone, so the filter can slow the robot but cannot steer it; steering needs a barrier of higher order or a reference point offset from the axle.

PieceRoleTypical source
Nominal commandWhat the robot would do without the filterLocal planner or controller
Barrier h(x)Margin to the unsafe set, ≥ 0 if safeCostmap distance field, lidar ranges, tracked people
Constraint on uLimit on the closing rateα · h and the robot’s kinematics
FallbackWhat to do if no command satisfies the constraintsControlled stop, hand over to a hardware safety layer

A worked example

One correction, by hand. Scout is 1.0 m from the centre of a pillar, in the direction n = (0.8, 0.6) from the pillar to the robot. The pillar must be kept at R = 0.4 m, so h = 1.0 − 0.4 = 0.6 m. Take α = 1.0 s⁻¹. The local planner asks for unom = (−1.0, −0.2) m/s, which points mostly towards the pillar.

Worked filter correction
a=n=(0.8, 0.6)a=n=(0.8,\,0.6)
b=−α h=−0.6b=-\alpha \,h=-0.6
a⊤ unom=0.8⋅(−1.0)+0.6⋅(−0.2)=−0.92<−0.6→unsafea^{\top}\,u_{\text{nom}}=0.8\cdot (-1.0)+0.6\cdot (-0.2)=-0.92<-0.6\to \text{unsafe}
shift=b−a⊤unoma⊤a=−0.6+0.921=0.32\text{shift}=\frac{b-a^{\top}u_{\text{nom}}}{a^{\top}a}=\frac{-0.6+0.92}{1}=0.32
u∗=unom+0.32⋅n=(−1.0+0.256, −0.2+0.192)=(−0.744, −0.008)u^{*}=u_{\text{nom}}+0.32\cdot n=(-1.0+0.256,\,-0.2+0.192)=(-0.744,\,-0.008)

Check the result: nᵀu* = 0.8·(−0.744) + 0.6·(−0.008) = −0.600 exactly the limit, and the sideways part along t = (−0.6, 0.8) is unchanged at 0.44 m/s before and after. The speed has fallen from 1.02 to 0.74 m/s, and the motion along the pillar is intact.

What α does. For a head-on approach the allowed speed is α h. With h = 0.6 m: α = 0.5 gives 0.3 m/s, α = 1 gives 0.6 m/s, α = 2 gives 1.2 m/s and α = 4 gives 2.4 m/s. A small α is cautious and slow near obstacles; a large α lets the robot drive almost up to the clearance. Choose it so that the limit at the planner’s typical clearance matches a speed from which the robot can really stop (lesson M7-T1).

Making the barrier know about braking. A distance barrier ignores speed. A common improvement subtracts the stopping distance: h = d − R − v t − v²/(2a). Then h ≥ 0 says exactly “I can still stop in time”, and the filter keeps the speed at or below the limit of lesson M7-T1 at every distance. For Scout (a = 0.8, t = 0.3) at d − R = 1.5 m that limit is 1.33 m/s.

Engineering implications and limitations

Where it sits

The filter runs after the controller and before the base driver, at the control rate, and gets the freshest obstacle data available, not the costmap the planner used. Nav2’s collision monitor is the same idea in practical form: it checks the commanded velocity against polygons around the robot and stops, slows or limits it. A full CBF filter is more general because it corrects the direction as well as the speed.

Check what the filter cannot know

  • It trusts h. A glass wall that is not in the sensor data is not in the barrier, and the filter will drive into it as faithfully as the planner would (lesson M7-T4).
  • It trusts the model. The constraint holds if the robot responds to u as assumed. Latency, slip and a limited acceleration all break that assumption; add margin or use a barrier that includes them.
  • It can deadlock. A filter that only removes unsafe motion can leave a robot nose-to-nose with a person, each filtering the other. It does not plan; stuck detection and recovery (lessons M7-T5 and M7-T6) still apply.
  • It can be infeasible. Several obstacles plus acceleration limits can leave no safe command. Decide beforehand what happens: the usual answer is a controlled stop.

Software is not an independent safety layer

A filter that runs on the same computer, with the same sensors, as the planner shares its faults. It reduces the chance that a planning error becomes a collision; it does not make the robot a safety-rated machine. Where people could be hurt, an independent safety function with its own sensing and a defined stop is designed and verified to the standards that apply (for example IEC 61508 for the functional-safety process, ISO 13849 for machinery control systems, ISO 3691-4 for automated industrial vehicles). This track does not replace that work.

Common misconceptions

“A safety filter is a planner.”

It does not look ahead or find routes. It corrects one command at a time, so it can leave the robot parked in front of an obstacle that a planner would have routed around.

“If the filter exists, the controller can be careless.”

A careless controller makes the filter act constantly, and the robot crawls or jerks. The filter should rarely change the command.

“A larger α is always more efficient.”

It is more efficient only if the robot can stop from the speed it allows. A large α with a late sensor lets the robot reach the edge of the safe set too fast to stop.

“A CBF proves the robot is safe.”

It proves invariance of the set for the model and the data it was given. Wrong data or a wrong model make the proof irrelevant to the real robot.

Engineering takeaways

  1. A safety filter takes a nominal command and changes it as little as possible if it would leave the safe set.
  2. The CBF condition Lf h + Lg h u ≥ −α h lets the margin shrink no faster than exponentially at rate α.
  3. With one linear constraint the filtered command is unom plus a shift along the constraint normal: the sideways motion is kept.
  4. The filter depends on correct barrier inputs, an accurate model and a defined fallback when no safe command exists.
  5. It reduces risk in software; it is not an independent safety function and does not replace a safety case.

References and further reading

The CBF papers are the primary sources; the Nav2 page is the nearest widely used implementation of the same idea.

  • A. D. Ames, X. Xu, J. W. Grizzle and P. Tabuada, “Control Barrier Function Based Quadratic Programs for Safety Critical Systems”, IEEE Transactions on Automatic Control, 62(8), 2017.
  • A. D. Ames, S. Coogan, M. Egerstedt, G. Notomista, K. Sreenath and P. Tabuada, “Control Barrier Functions: Theory and Applications”, European Control Conference, 2019.
  • P. Wieland and F. Allgöwer, “Constructive Safety Using Control Barrier Functions”, IFAC Symposium on Nonlinear Control Systems, 2007.
  • Nav2 documentation, “Collision Monitor”, docs.nav2.org.

Knowledge check

Three conceptual questions. Write an answer, then reveal the explanation. Your answers stay in your browser.

Question 1

Scout is 1.2 m from an obstacle centre that must be kept 0.4 m away. With α = 1.5 s⁻¹, what is the largest speed head-on towards it?

Explanation

h = 1.2 − 0.4 = 0.8 m and v ≤ α h = 1.5 · 0.8 = 1.2 m/s.

Question 2

In the worked example the sideways part of the velocity is 0.44 m/s before and after the filter. Why does the filter leave it alone?

Explanation

The constraint involves only the component along n. The correction is the shortest move onto the allowed set, which is along n, so the part perpendicular to n is unchanged and the robot still slides along the obstacle.

Question 3

Two obstacles on either side of a narrow gap give constraints that no velocity can satisfy at once. What must a designer decide in advance?

Explanation

What the filter does when the QP is infeasible. The usual choice is a controlled stop (and a report), because applying one constraint and ignoring the other would silently break a safety rule.

What’s next

Next: what to do when the robot is not sure where it is, and the margins above are drawn around a position that may be wrong.

Next lesson

M7-T3 — Navigation Under Pose Uncertainty

Use pose covariance to inflate obstacles or tighten speed, and explain why a confident plan on a wrong pose is dangerous.

Nothing below is required to finish this lesson.