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

二、示例

2.1、系统模型

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

控制障碍函数(Control Barrier Function,CBF) 二、示例_第1张图片

我们定义控制输入 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˙= vm1Fr(v)v0v + 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 mcdgumcag
稳定性目标,其中 v d v_d vd 为期望速度
v → v d v \to v_d vvd
安全目标,其中 T h T_h Th 为时间前瞻量
z ≥ T h v z \ge T_h v zThv

2.2、设计CLF

系统的稳定目标为 v → v d v \to v_d vvd ,因此系统的原点 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)=(vvd)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(vvd)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)(vvd)=m2(vvd)
所以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)=(vvd){m2(uFr)+λ(vvd)}0

2.3、设计CBF

安全目标为 z ≥ T h v z \ge T_h v zThv ,即两车间距大于一定值

先以直观的方式定义CBF函数 B ( x ) = z − T h v B(x) = z - T_h v B(x)=zThv

对其求梯度 ∇ B ( x ) = [ 0 − T h 1 ] \nabla B(x) = \begin{bmatrix} 0 & -T_h & 1 \end{bmatrix} B(x)=[0Th1]

那么
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)+(v0v)=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)+(v0v)mTh+γ(zThv)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)=zThv21cdg(vv0)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+cdgvv0)(Fr(v)u)+(v0v)
当 $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+cdgvv0)(Fr(v)u)+(v0v)+γ(zThv)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(vvd){m2(uFr)+λ(vvd)}0m1(Th+cdgvv0)(Fr(v)u)+(v0v)+γ(zThv)0mcdgumcag
从上到下分别为CLF约束,CBF约束,控制输入约束。

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

你可能感兴趣的:(控制理论,概率论,线性代数,机器学习)