二、示例
2.1、系统模型
这里我们举一个CBF作者给出的经典示例
我们定义控制输入
u
u
u 为蓝色车的推力。
p
p
p 为蓝色车的位置，
v
v
v 为蓝色车的速度，
z
z
z 为蓝色车与黄色车之间的距离，
v
0
v_0
v0 为黄色车的速度。我们定义系统的状态为
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−m1Fr(v)v0−v
+
0m10
u
其中
F
r
(
v
)
=
f
0
+
f
1
v
+
f
2
v
2
F_r(v) = f_0 + f_1 v + f_2 v^2
Fr(v)=f0+f1v+f2v2 为滚动阻力
为了保证系统安全，对这个系统做出一些约束
输入约束
−
m
c
d
g
≤
u
≤
m
c
a
g
-m c_d g \le u \le m c_a g
−mcdg≤u≤mcag
稳定性目标，其中
v
d
v_d
vd 为期望速度
v
→
v
d
v \to v_d
v→vd
安全目标，其中
T
h
T_h
Th 为时间前瞻量
z
≥
T
h
v
z \ge T_h v
z≥Thv
2.2、设计CLF
系统的稳定目标为
v
→
v
d
v \to v_d
v→vd ，因此系统的原点
x
e
=
[
⋅
,
v
d
,
⋅
]
T
x_e = [\cdot,v_d,\cdot]^T
xe=[⋅,vd,⋅]T 。设计李雅普诺夫函数为
V
(
x
)
=
(
v
−
v
d
)
2
V(x) = (v - v_d)^2
V(x)=(v−vd)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
那么
L
f
V
(
x
)
=
−
2
m
F
r
(
v
)
(
v
−
v
d
)
L
g
V
(
x
)
=
−
2
m
(
v
−
v
d
)
\begin{aligned} L_f V(x) &= -\frac{2}{m} F_r(v) (v-v_d) \\ L_g V(x) &= -\frac{2}{m} (v-v_d) \\ \end{aligned}
LfV(x)LgV(x)=−m2Fr(v)(v−vd)=−m2(v−vd)
所以CLF约束可以表示为
V
˙
(
x
,
u
)
+
λ
V
(
x
)
=
L
f
V
(
x
)
+
L
g
V
(
x
)
u
+
λ
V
(
x
)
=
(
v
−
v
d
)
{
2
m
(
u
−
F
r
)
+
λ
(
v
−
v
d
)
}
≤
0
\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
V˙(x,u)+λV(x)=LfV(x)+LgV(x)u+λV(x)=(v−vd){m2(u−Fr)+λ(v−vd)}≤0
2.3、设计CBF
安全目标为 z ≥ T h v z \ge T_h v z≥Thv ，即两车间距大于一定值
先以直观的方式定义CBF函数 B ( x ) = z − T h v B(x) = z - T_h v B(x)=z−Thv
对其求梯度 ∇ B ( x ) = [ 0 − T h 1 ] \nabla B(x) = \begin{bmatrix} 0 & -T_h & 1 \end{bmatrix} ∇B(x)=[0−Th1]
那么
L
f
V
(
x
)
=
T
h
m
F
r
(
v
)
+
(
v
0
−
v
)
L
g
V
(
x
)
=
−
T
h
m
\begin{aligned} L_f V(x) &= \frac{T_h}{m} F_r(v) + (v_0 - v) \\ L_g V(x) &= -\frac{T_h}{m} \\ \end{aligned}
LfV(x)LgV(x)=mThFr(v)+(v0−v)=−mTh
所以有CBF函数的导数
B
˙
(
x
,
u
)
+
γ
B
(
x
)
=
T
h
m
(
F
r
(
v
)
−
u
)
+
(
v
0
−
v
)
−
T
h
m
+
γ
(
z
−
T
h
v
)
≥
0
\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
B˙(x,u)+γB(x)=mTh(Fr(v)−u)+(v0−v)−mTh+γ(z−Thv)≥0
如果忽视滚动阻力的影响，并采用最大的控制量
u
=
−
c
d
m
g
u = -c_d mg
u=−cdmg
B
˙
(
x
,
u
)
+
γ
B
(
x
)
=
T
h
c
d
g
+
v
0
+
γ
z
−
(
1
+
T
h
γ
)
v
\dot B(x,u) + \gamma B(x) =T_h c_d g + v_0 + \gamma z - (1+T_h \gamma) v
B˙(x,u)+γB(x)=Thcdg+v0+γz−(1+Thγ)v
当
v
v
v 相对于
T
h
c
d
g
+
v
0
T_h c_d g + v_0
Thcdg+v0 比较大时，那么约束可能不会被满足，即
B
˙
(
x
,
u
)
≤
0
\dot B(x,u) \le 0
B˙(x,u)≤0 当
v
v
v 比较大时。
为了解决这个问题，我们多加一项，多加的这一项其实我们看等式的话，相当于将安全区域范围缩小了，但是能消除一部分
B
(
x
)
B(x)
B(x) 中的多项式
B
(
x
)
=
z
−
T
h
v
−
1
2
(
v
−
v
0
)
2
c
d
g
B(x) = z - T_h v - \frac{1}{2} \frac{(v - v_0)^2}{c_d g}
B(x)=z−Thv−21cdg(v−v0)2
对其求导
B
˙
(
x
,
u
)
=
1
m
(
T
h
+
v
−
v
0
c
d
g
)
(
F
r
(
v
)
−
u
)
+
(
v
0
−
v
)
\dot B(x,u) = \frac{1}{m} (T_h + \frac{v - v_0}{c_d g}) (F_r(v) - u) + (v_0 - v)
B˙(x,u)=m1(Th+cdgv−v0)(Fr(v)−u)+(v0−v)
当 $u = -c_d m g $ 时
B
˙
(
x
,
u
)
=
1
m
T
h
F
r
(
v
)
+
T
h
c
d
g
>
0
\dot B(x,u) = \frac{1}{m} T_h F_r(v) + T_h c_d g > 0
B˙(x,u)=m1ThFr(v)+Thcdg>0
所以CBF可以表示为
B
˙
(
x
,
u
)
+
γ
B
(
x
)
=
1
m
(
T
h
+
v
−
v
0
c
d
g
)
(
F
r
(
v
)
−
u
)
+
(
v
0
−
v
)
+
γ
(
z
−
T
h
v
)
≥
0
\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
B˙(x,u)+γB(x)=m1(Th+cdgv−v0)(Fr(v)−u)+(v0−v)+γ(z−Thv)≥0
2.4、规划目标
最终，我们得到的是这样一个规划问题
argmin
u
T
H
u
subject to
(
v
−
v
d
)
{
2
m
(
u
−
F
r
)
+
λ
(
v
−
v
d
)
}
≤
0
1
m
(
T
h
+
v
−
v
0
c
d
g
)
(
F
r
(
v
)
−
u
)
+
(
v
0
−
v
)
+
γ
(
z
−
T
h
v
)
≥
0
−
m
c
d
g
≤
u
≤
m
c
a
g
\begin{aligned} \text{argmin} \ \ &u^T H u \\ \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{aligned}
argmin subject to uTHu(v−vd){m2(u−Fr)+λ(v−vd)}≤0m1(Th+cdgv−v0)(Fr(v)−u)+(v0−v)+γ(z−Thv)≥0−mcdg≤u≤mcag
从上到下分别为CLF约束，CBF约束，控制输入约束。
下一部分我们讲解这里推导的公式怎么用代码实现
