Example CLF-CBF For Differential Drive Robot

This blog is work in progress…

Introduction

This document presents the mathematical foundation of a control scheme that combines a Control Lyapunov Function (CLF) for stabilizing a 3-DOF differential drive robot towards a target position, and a Control Barrier Function (CBF) for ensuring safe obstacle avoidance. The control inputs are derived by solving a Quadratic Programming (QP) problem at each time step.

3-DOF Differential Drive Robot Model

The state of a differential drive robot is defined by:

𝑉 = [ x, y, θ ]^T

where x, y denote the position in the plane and θ is the heading angle of the robot. The control inputs are:

𝑂 = [ v, ω ]^T

where v is the linear velocity and ω is the angular velocity.

The dynamics of the robot can be written as:

𝑇₍𝑉₎ = [ v cos(θ), v sin(θ), ω ]

Control Lyapunov Function (CLF) and Stability

Lyapunov Function for Position and Heading Control

To design a controller that drives the robot towards the goal, we use a Lyapunov function. A Lyapunov function V(𝑉) is a scalar function that is positive definite and decreases over time. The key property of a Lyapunov function is that if 𝑇V(𝑉) ≤ 0, the system is stable, meaning that the robot will converge to the goal.

For this system, we can define the Lyapunov function based on the errors in position and heading:

V(𝑉) = 0.5 e²_pos + 0.5 e²_θ

where:

e_pos is the position error,
e_θ is the heading error.

These errors are defined as:

e_pos = √[(x_goal – x)² + (y_goal – y)²]
e_θ = wrapToPi(θ_goal – θ)

Control Law for Stability

To ensure stability, we choose control inputs 𝑂 = [ v, ω ]^T that make the Lyapunov function decrease over time. Specifically, we want 𝑇V(𝑉) ≤ 0, where:

𝑇V(𝑉) = e_pos 𝑇e_pos + e_θ 𝑇e_θ

For the position error, we use a proportional control law:

v_clf = k_v e_pos

where k_v > 0 is a constant gain that dictates how aggressively the robot moves towards the goal.

For the heading error, we use a proportional control law for angular velocity:

ω_clf = k_θ e_θ

where k_θ > 0 is a constant gain that adjusts the heading of the robot to align with the target.

Control Barrier Function (CBF) Design

Design of the Barrier Function

The Control Barrier Function (CBF) ensures that the robot avoids obstacles by defining a constraint that keeps the robot away from unsafe regions. Consider an obstacle located at:

𝑉_obs = [ x_obs, y_obs ]^T

with a safety radius r_obs. The barrier function is defined as:

h(𝑉) = (x – x_obs)² + (y – y_obs)² – r_obs²

This ensures that h(𝑉) > 0, meaning the robot stays outside the unsafe region.

Barrier Function Derivative and Safety Constraint

To maintain safety, the time derivative of the barrier function must satisfy:

𝑇h(𝑉) + α h(𝑉) ≥ 0

where α is a positive constant. The time derivative of h(𝑉) is:

𝑇h(𝑉) = 2 (x – x_obs) v cos(θ) + 2 (y – y_obs) v sin(θ)

The CBF constraint becomes:

2 (x – x_obs) v cos(θ) + 2 (y – y_obs) v sin(θ) + α h(𝑉) ≥ 0

Quadratic Programming Formulation

At each time step, the control inputs 𝑂 = [ v, ω ]^T are obtained by solving a Quadratic Program (QP) that balances the goal-reaching behavior (CLF) and the safety constraint (CBF). The QP is formulated as:

min 𝑂 &quad; 0.5 | 𝑂 – 𝑂_clf |²

subject to the CBF constraint:

2 (x – x_obs) v cos(θ) + 2 (y – y_obs) v sin(θ) + α h(𝑉) ≥ 0

Including a Slack Variable

In cases where the CBF constraint might conflict with the CLF, we include a slack variable ε to relax the CBF constraint:

2 (x – x_obs) v cos(θ) + 2 (y – y_obs) v sin(θ) + α h(𝑉) ≥ ε

This ensures that the optimization problem remains feasible, even when the robot is near the obstacle, allowing small safety violations in exchange for maintaining control stability.

Conclusion

The combined Control Lyapunov Function (CLF) and Control Barrier Function (CBF) framework ensures that the 3-DOF differential drive robot can both reach a target goal and avoid obstacles. The CLF ensures stability by guiding the robot towards the goal, while the CBF enforces safety by preventing the robot from entering unsafe regions. The use of a Quadratic Program (QP) allows the robot to balance these two objectives, and the inclusion of a slack variable ensures feasibility in cases of constraint conflicts.