Safe Control Density

Non-Holonomic System

Nonholonomic navigation using unicycle model with forward speed and yaw-rate controls. The examples compare density feedback, density filters, density MPC, and CBF-style safety filters.

Run

Run the minimal nonholonomic feedback, filter, and MPC examples.

python examples/unicycle/static_single_obstacle/density_feedback.py
python examples/unicycle/static_single_obstacle/density_filter.py
python examples/unicycle/static_single_obstacle/density_mpc.py

System Dynamics

$$ \begin{aligned} x_{k+1} &= x_k+\Delta t\,v_k\cos\theta_k,\\ y_{k+1} &= y_k+\Delta t\,v_k\sin\theta_k,\\ \theta_{k+1} &= \theta_k+\Delta t\,\omega_k. \end{aligned} $$

Density Feedback Controller

The planar density gradient gives a desired direction in \(x,y\). The unicycle tracks that direction by choosing a forward speed and yaw-rate command that align the heading with the density-gradient direction.

$$ \begin{aligned} u_\rho(x_k,y_k) &= k_\rho\nabla\rho(x_k,y_k),\\ v_k &= \operatorname{sat}_{[0,v_{\max}]}\|u_\rho\|,\\ \theta_d &= \operatorname{atan2}(u_{\rho,y},u_{\rho,x}),\\ \omega_k &= \operatorname{sat}_{[-\omega_{\max},\omega_{\max}]} \left(k_\theta\operatorname{wrap}(\theta_d-\theta_k)\right). \end{aligned} $$

Single Obstacle

Multiple Obstacles

Local Sensing

Density-Feedback Sensing-Radius Sweep

Increasing the sensing radius makes the density-gradient direction react earlier to the obstacle, giving a more conservative path.

Density Safety Filter

The one-step density filter stays close to a nominal unicycle command \(u_{\mathrm{nom}}=[v_{\mathrm{nom}},\omega_{\mathrm{nom}}]^T\) while enforcing the next-step density condition.

$$ \begin{aligned} \min_{u_k,s}\quad &\frac{1}{2}\|u_k-u_{\mathrm{nom}}\|_W^2 +\frac{1}{2}w_s s^2\\ \mathrm{s.t.}\quad &z_{k+1}=F_d(z_k,u_k),\\ &\rho(z_{k+1})-\rho(z_k) +\Delta t\,\nabla\!\cdot(F_d\rho)(z_k,u_k)+s\ge0,\\ &u_k\in\mathcal U,\qquad s\ge0. \end{aligned} $$

Single Obstacle

Multiple Obstacles

Local Sensing

Density-Filter Sensing-Radius Sweep

The filter becomes more conservative as the sensing radius grows, because obstacle constraints shape the density condition earlier.

Density MPC

Density MPC enforces the density transport condition along a predicted horizon. This lets the controller choose steering and speed commands that account for future unicycle motion.

$$ \begin{aligned} \min_{z,u,c}\quad &\sum_{j=0}^{N-1}\ell(z_j,u_j,c_j)+\ell_f(z_N)\\ \mathrm{s.t.}\quad &z_0=z_k,\qquad z_{j+1}=F_d(z_j,u_j),\\ &\rho(z_{j+1})-\rho(z_j) +\Delta t\,\nabla\!\cdot(F_d\rho)(z_j,u_j) -\Delta t\,c_j\rho(z_j)\ge0,\\ &u_j\in\mathcal U,\qquad c_j\ge0. \end{aligned} $$

Single Obstacle

Multiple Obstacles

Local Sensing

Comparison with CLF-CBF filter

For the unicycle model, a vanilla one-step CBF filter encodes obstacle safety only through planar position. Let \(p_k=[x_k,y_k]^T\) be the robot position at time step \(k\), let \(p_o\) be the obstacle center, and let \(R\) be the inflated obstacle radius. The barrier does not depend on the heading state, so the one-step CBF condition can reduce forward speed near the unsafe set but does not directly constrain or reward the yaw-rate input needed to steer around the obstacle and toward the goal.

$$ \begin{aligned} p_k &= [x_k,y_k]^T,\\[2pt] h(z_k) &= \|p_k-p_o\|^2-R^2,\\[6pt] p_{k+1} &= p_k+\Delta t\,v_k \begin{bmatrix}\cos\theta_k\\ \sin\theta_k\end{bmatrix},\\ \theta_{k+1} &= \theta_k+\Delta t\,\omega_k,\\[6pt] h(z_{k+1}) &\ge (1-\gamma)h(z_k). \end{aligned} $$ $$ \begin{aligned} \left\| p_k+\Delta t\,v_k \begin{bmatrix}\cos\theta_k\\ \sin\theta_k\end{bmatrix} -p_o \right\|^2 - R^2 &\ge (1-\gamma)\left(\|p_k-p_o\|^2-R^2\right). \end{aligned} $$

After substitution, the one-step CBF inequality contains \(v_k\), but not \(\omega_k\). Steering changes \(\theta_{k+1}\), which affects position only in later steps. Therefore the CLF-CBF filter can remain safe by slowing or stopping, but the CBF part does not have direct authority to steer the robot away in the current step.

CLF-CBF Comparison