控制障碍函数CBF详解（附带案例实现）
控制障碍函数CBF详解（附带案例实现）
1. Control Affine System
一个控制仿射系统的典型形式是
$$
\dot{x} = F(x,u)
$$
其中，$x\in \mathbb{R}^n$是系统的状态，$u\in\mathbb{R}^m$是系统的控制输入，$F$是Lipschitz连续的，这样就能保证给定一个初始状态$x(t_0)=x_0$的时候，动态系统的轨迹$x(t)$存在且唯一。
我们通常处理的是非线性系统，那么我们可以将非线性的仿射系统写成如下的形式
$$
\dot{x} = f(x) + g(x) u
$$
其中$f:\mathbb{R}^n \to \mathbb{R}^n$是系统的漂移向量场，它描述了系统在没有控制输入时的动态行为，$g:\mathbb{R}^n\to\mathbb{R}^{n\times m}$，是系统的控制向量场，它描述了系统的控制输入$u$是如何影响系统的。
2. Lyapunov Theory, Nagumo’s Theory, Invariance Principle
Lyapunov Theory
对于系统$\dot{x}=f(x)$而言，$x\in \mathbb{R}^n$是系统的状态，$f:\mathbb{R}^n \to \mathbb{R}^n$是一个系统状态的映射函数。如果
$$
\exists V(x) \
\text{s.t. } V(x_e) = 0, V(x)>0 \text{ for } x \ne x_e, \
\dot{V}(x) = \frac{\partial V}{\partial x} f(x) < 0 \text{ for } x\ne x_e
$$
那么系统则是稳定的。并且状态$x$所构成的集合，可以被称为是一个不变集（Invariance set）。
不变集合： 集合$\mathcal{C}$被称为不变的，如果系统从$\mathcal{C}$内的任意一点开始演化，那么系统的轨迹始终停留在$\mathcal{C}$内。
Nagumo’s Theory
对于系统$\dot{x} = f(x)$而言，$x\in \mathbb{R}^n$是系统的状态，$\mathcal{C}={x|h(x)\ge0}$是映射$h:\mathbb{R}^n \to \mathbb{R}$的一个上水平集，如果
$$
\dot{h}(x) \ge 0 \
\forall x\in\partial\mathcal{C}
$$
那么$\mathcal{C}$是一个不变集。符号的具体含义如下：
$$
\begin{align*}
\mathcal{C} & = { x\in D\sub \mathbb{R}^n: h(x) \ge 0 }, \
\partial\mathcal{C} & = { x\in D\sub \mathbb{R}^n: h(x) = 0 }, \
\text{Int}(\mathcal{C}) & = { x\in D\sub \mathbb{R}^n: h(x) > 0 },
\end{align*}
$$
集合$\mathcal{C}$是安全集，$\partial\mathcal{C}$是安全集的边界，$\text{Int}(\mathcal{C})$是安全集的内部点。
3. Control Lyapunov Function (CLF) and CLF-QP
Control Lyapunov Function
设$V(x):\mathbb{R}^n \to \mathbb{R}$是一个连续可微的函数，如果这里存在一个常量$c>0$使得
$$
\begin{align*}
& \text{1) }\Omega_c := { x\in\mathbb{R}^n: V(x) \le c}, \text{ a sublevel set of } V(x) \text{ is bounded} \
& \text{2) }V(x) > 0, \forall s \in\mathbb{R}^n \backslash {x_e}, V(x_e) = 0 \
& \text{3) }\inf_{u\in U} \dot{V}(x,u) < 0, \forall x \in \Omega_c \backslash {x_e}
\end{align*}
$$
1）存在一个子集，使得$V(x)\le c$是有界的
2）Lyapunov函数不在原点时大于零，在原点时等于零
3）对于控制量和系统状态来说，总使得$V(x)$的导数$\dot{V}(x)$小于零。
其中，$V(x)$可以被称为局部控制李雅普诺夫函数，$\Omega_c$是一个引力区（Region of Attraction, ROA），$\Omega_c$中的每一个点都会收敛到$x_e$，整个轨迹就如下图所示，会一直保持在这个区域内，并且最终收敛到$x_e$，这其实就是一种渐进稳定。
我们可以将导数显示地写出来
$$
\begin{align*}
\dot{V}(x) & = \nabla V(x) \dot{x} \
& = \nabla V(x)f(x) + \nabla V(x)g(x)u \
& = L_fV(x) + L_g V(x) u
\end{align*}
$$
其中$L_p q(x) = \nabla q(x) \cdot p(x)$是李导数算子，使得公式更加简洁。
为了使收敛更加迅速，我们需要考虑收敛的时间限制，指数收敛是一种快速的方式，所以我们希望最终的结果能够按照指数的方式进行收敛。
我们可以增加一个判断条件，设$V(x):\mathbb{R}^n \to \mathbb{R}$是连续可微、正定、有界的函数，如果$\exists \lambda>0$使得
$$
\text{4) }\inf_{u\in U} \dot{V}(x,u) + \lambda V(x) \le 0
$$
或者写成
$$
\inf_{u\in U} [L_f V(x) + L_gV(x)u] + \lambda V(x) \le 0
$$
那么$V(x)$就是指数稳定的控制李雅普诺夫函数（exponentially stabilizing CLF，ES-CLF），其中$\lambda$是$V(x(t))$上界的衰减率。
Control Lyapunov Function Quadratic Program
CLF约束对于$u$是线性的，因此用最小范数控制器，二次规划的目标位最小化控制量，受限制为，满足李雅普诺夫函数收敛的上界以及控制量 $u$在解集内。由于这个优化为一个凸优化问题，因此其实时性是可以被保证的，CLF约束通常用松弛变量来保证问题的可行性，如果没有松弛变量，控制器将指数稳定到系统原点$x_e$
4. Control Barrier Function (CBF) and CBF-CLF-QP
设$B(x):\mathbb{R}^n \to \mathbb{R}$是连续、可微的函数，$\mathcal{C}={x|B(x)\ge 0}$是该函数的零上水平集，并且$\nabla B(x)\ne 0,\forall x\in\partial\mathcal{C}$，如果$\exists \gamma$使得$\forall x\in\mathcal{C}$
$$
\sup_{u \in U}[L_fB(x) + L_g B(x)u] + \gamma B(x) \ge 0
$$
那么$B(x)$就被称作Control Barrier Function
因此这个二次规划问题就变成了
5. A Toy Example
我们考虑一个简单的例子，在这个例子中有两辆车辆，分别是lead vehicle和ego vehicle，如下图所示
下面我们先推导一下，然后再进行仿真验证。
5.1 Derivation
设计我们的状态量为：$x = [p, v, z]^T \in \mathbb{R}^3$，系统的微分方程为：
$$
\begin{align*}
\dot{p} & = v \
\dot{v} & = \frac{u - F_r(v)}{m}, \quad F_r(v) = f_0 + f_1 v + f_2 v^2 \
\dot{z} & = v_0 - v
\end{align*}
$$
其中F_r(v)是摩擦力，将微分方程写成矩阵的形式
$$
\dot{x} =
\begin{bmatrix}
\dot{p} \
\dot{v} \
\dot{z} \
\end{bmatrix} =
\begin{bmatrix}
v \
-\frac{1}{m} F_r(v) \
v_0 - v
\end{bmatrix} +
\begin{bmatrix}
0 \
\frac{1}{m} \
0
\end{bmatrix} u
$$
为了保障系统的安全性，给系统的输入做如下的限制
$$
-mc_dg \le u \le mc_ag
$$
ego vehicle的目标速度为
$$
v \to v_d
$$
最小安全距离必须满足时间前瞻量的限制$T_h$
$$
z \ge T_h v
$$
系统的平衡点$x_e = [\cdot, v_d, \cdot]^T$设计Lyapunov Function $V(x)$
$$
\begin{align*}
V(x) & = (v - v_d)^2 \
\dot{V}(x) & = \nabla V(x) \dot{x} \
\end{align*}
$$
其中
$$
\nabla V(x) = [0, 2(v-v_d), 0] \
\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
$$
然后我们可以获得
$$
L_f V(x) = -\frac{2}{m} F_r(v)(v-v_d) \
L_g V(x) = \frac{2}{m} (v-v_d)
$$
所以CLF的约束$\inf_{u\in U} \quad[L_f V(x) + L_g V(x)u] + \lambda V(x) \le 0$可以表示为
$$
\inf_{u\in U} \quad(v-v_d) [\frac{2}{m} (u-F_r) + \lambda (v-v_d)] \le 0
$$
保障安全性的目标是$z\ge T_h V$，则设计CBF
$$
B(x) = z - T_h V
$$
则有
$$
\nabla B(x) = [0, -T_h, 1] \
L_f B(x) = \frac{T_h}{m} F_r + (v_0 - v), \quad L_g B(x) = -\frac{T_h}{m}
$$
所以CBF的约束$\sup_{u\in U} \quad [L_f B(x) + L_g B(x)u] + \gamma B(x) \ge 0$ 可以表示为
$$
\frac{T_h}{m}(F_r - u) + (v_0 - v) + \gamma (z - T_h v) \ge 0
$$
忽略$F_r$，当$u=-mc_dg$的时候，CBF的约束可以表示为
$$
T_h c_d g + v_0 + \gamma z - (1+T_h \gamma)v
$$
当$v$足够大的时候，可能会导致CBF的约束小于0，所以我们需要重新设计CBF
$$
B(x) = z - T_hv - \textcolor{blue}{\frac{1}{2} \frac{(v-v_0)^2}{c_dg}}
$$
则有
$$
\dot{B}(x,u) = \frac{1}{m} (T_h + \textcolor{blue}{\frac{v-v_0}{c_dg}})(F_r(v) - u) + (v_0 - v)
$$
当$u=-mc_dg$的时候，$\dot{B}(x,u) = \frac{1}{m}T_h F_r + T_h c_dg > 0$，所以此CBF是一个可行的CBF，那么我们最终获得的CLF-CBF-QP为
$$
\begin{align*}
\arg\min & \quad u^T Hu \
\text{s.t.} & \quad (v-v_d)[\frac{2}{m}(u-F_r) + \lambda(v-v_d)] \le 0 \
& \quad \frac{1}{m}(T_h + \frac{(v-v_0)}{c_d g})(F_r - u) + (v_0 - v) + \gamma (z - T_h v) \ge 0 \
& \quad -m c_d g \le u \le m c_a g
\end{align*}
$$
5.2 Simulation
使用python来编写脚本代码，使用cvxopt凸优化的库来进行求解，具体可以参考cvxopt的wiki官网。
1 | import numpy as np |
最后的结果如下
Reference
[1]Jason Choi – Introduction to Control Lyapunov Functions and Control Barrier Functions
[2]CBF-CLF-Helper
