控制障碍函数(Control Barrier Function,CBF) 二、示例

二、示例

2.1、系统模型

这里我们举一个CBF作者给出的经典示例

我们定义控制输入 $u$ 为蓝色车的推力。$p$ 为蓝色车的位置，$v$ 为蓝色车的速度，$z$ 为蓝色车与黄色车之间的距离，$v_0$ 为黄色车的速度。我们定义系统的状态为
x = [ p v z ] ∈ R 3 x = \begin{bmatrix} p \\ v \\ z \end{bmatrix} \in \mathbb{R}^3 x= ​pvz​ ​∈R3
对其求导为
x ˙ = [ v − 1 m F r ( v ) v 0 − v ] + [ 0 1 m 0 ] u \dot x = \begin{bmatrix} v \\ -\frac{1}{m} F_r(v) \\ v_0 - v \end{bmatrix} + \begin{bmatrix} 0 \\ \frac{1}{m} \\ 0 \end{bmatrix} u x˙= ​v−m1​Fr​(v)v0​−v​ ​+ ​0m1​0​ ​u
其中 $F_r(v)=f_0+f_{1}v+f_2{v}^2$ 为滚动阻力

为了保证系统安全，对这个系统做出一些约束

输入约束
$$-m{c}_{d}g\le u\le m{c}_{a}g$$
稳定性目标，其中 $v_d$ 为期望速度
$$v\to v_d$$
安全目标，其中 $T_h$ 为时间前瞻量
$$z\ge T_{h}v$$

2.2、设计CLF

系统的稳定目标为 $v\to v_d$ ，因此系统的原点 $x_e=[\cdot ,v_d,\cdot {]}^T$ 。设计李雅普诺夫函数为
$$V(x)=(v-v_d{)}^2$$
对其求梯度
∇ V ( x ) = [ 0 2 ( v − v d ) 0 ] \nabla V(x) = \begin{bmatrix} 0 \\ 2(v-v_d) \\ 0 \end{bmatrix} ∇V(x)= ​02(v−vd​)0​ ​
那么
$$\begin{array}{rl}{L}_{f}V(x)& =-\frac{2}{m}{F}_r(v)(v-v_d)\\ L_{g}V(x)& =-\frac{2}{m}(v-v_d)\end{array}$$
所以CLF约束可以表示为
$$\dot{V}(x,u)+\lambda V(x)=L_{f}V(x)+L_{g}V(x)u+\lambda V(x)=(v-v_d)\left\{\frac{2}{m}(u-F_r)+\lambda (v-v_d)\right\}\le 0$$

2.3、设计CBF

安全目标为 $z\ge T_{h}v$ ，即两车间距大于一定值

先以直观的方式定义CBF函数 $B(x)=z-T_{h}v$

对其求梯度 $\nabla B(x)=\left[\begin{array}{ccc}0& -T_h& 1\end{array}\right]$

那么
$$\begin{array}{rl}{L}_{f}V(x)& =\frac{T_h}{m}{F}_r(v)+(v_0-v)\\ L_{g}V(x)& =-\frac{T_h}{m}\end{array}$$
所以有CBF函数的导数
$$\dot{B}(x,u)+\gamma B(x)=\frac{T_h}{m}(F_r(v)-u)+(v_0-v)-\frac{T_h}{m}+\gamma (z-T_{h}v)\ge 0$$
如果忽视滚动阻力的影响，并采用最大的控制量 $u=-c_{d}mg$
$$\dot{B}(x,u)+\gamma B(x)=T_h{c}_{d}g+v_0+\gamma z-(1+T_h\gamma )v$$
当 $v$ 相对于 $T_h{c}_{d}g+v_0$ 比较大时，那么约束可能不会被满足，即 $\dot{B}(x,u)\le 0$ 当 $v$ 比较大时。

为了解决这个问题，我们多加一项，多加的这一项其实我们看等式的话，相当于将安全区域范围缩小了，但是能消除一部分 $B(x)$ 中的多项式
$$B(x)=z-T_{h}v-\frac{1}{2}\frac{(v-v_0{)}^2}{c_{d}g}$$
对其求导
$$\dot{B}(x,u)=\frac{1}{m}(T_h+\frac{v-v_0}{c_{d}g})(F_r(v)-u)+(v_0-v)$$
当 $u = -c_d m g $ 时
$$\dot{B}(x,u)=\frac{1}{m}{T}_h{F}_r(v)+T_h{c}_{d}g>0$$
所以CBF可以表示为
$$\dot{B}(x,u)+\gamma B(x)=\frac{1}{m}(T_h+\frac{v-v_0}{c_{d}g})(F_r(v)-u)+(v_0-v)+\gamma (z-T_{h}v)\ge 0$$

2.4、规划目标

最终，我们得到的是这样一个规划问题
$$\begin{array}{rl}\text{argmin}& u^{T}Hu\\ \text{subject to}& (v-v_d)\left\{\frac{2}{m}(u-F_r)+\lambda (v-v_d)\right\}\le 0\\ & \frac{1}{m}(T_h+\frac{v-v_0}{c_{d}g})(F_r(v)-u)+(v_0-v)+\gamma (z-T_{h}v)\ge 0\\ & -m{c}_{d}g\le u\le m{c}_{a}g\end{array}$$
从上到下分别为CLF约束，CBF约束，控制输入约束。

下一部分我们讲解这里推导的公式怎么用代码实现