high order control barrier functions with time varying output constraints

记录一下拖了一周的HOCBF设计过程。

问题描述

给定一个非线性系统：
\[ \dot{x} = f(x) + g(x)u \]

其中\(x\in\mathbb{R}^n\)是系统状态，\(u\in\mathbb{R}^m\)是控制输入，\(f(x)\)和\(g(x)\)是已知的连续函数。

CBF：
\[ h(x) \geq 0 \] 其约束为：
\[ L_fh(x) + L_gh(x)u \geq -\alpha(h(x)) \]

其中\(h(x)\)是已知的连续函数。

设h(t,x)是时间t的函数，其相关度为\(\lambda\)，则HOCBF约束为：
\[ L_f^{\lambda}h(t,x) + L_gL_f^{\lambda-1}h(t,x)u + \frac{\partial^{\lambda} h(t,x)}{\partial t^{\lambda}} \geq -K_{\alpha}(\mu(t,x)) \] 其中：
\[ K_{\alpha} = \left[\begin{matrix} \alpha_0 & \alpha_1 & \cdots & \alpha_{\lambda-1} \end{matrix} \right] \] \[ \mu(t,x) = \left[\begin{matrix} h(t,x) & L_fh(t,x)+\frac{\partial h(t,x)}{\partial t} & \cdots & L_f^{\lambda-1}h(t,x) +\frac{\partial^{\lambda-1} h(t,x)}{\partial t^{\lambda-1}} \end{matrix} \right]^T \]

确定\(\alpha_i\)的值，使得HOCBF约束成立。

\(\alpha_i\)应满足：
\[ s^{\lambda} + \alpha_{\lambda-1}s^{\lambda-1} + \cdots + \alpha_1s + \alpha_0 \] \[ \gamma_i < 0 \] 其中\(\gamma_i\)是系统的特征值。

设计过程

以三维空间的障碍物避障为例，系统状态为：
\[ x = \left[\begin{matrix} x & y & z \end{matrix}\right]^T \] 仿射系统：
\[ \dot{x} = f(x) + g(x)u \] 障碍物位置：
\[ p_{obs} = \left[\begin{matrix} x_{obs} & y_{obs} & z_{obs} \end{matrix}\right]^T \] 定义CBF：
\[ h(x) = (x-x_{obs})^T(x-x_{obs}) - r^2 \] 其中r是障碍物半径。

设相关度\(\lambda=2\)，则HOCBF约束为：
\[ L_f^{2}h(t,x) + L_gL_fh(t,x)u + \frac{\partial^{2} h(t,x)}{\partial t^{2}} \geq -K_{\alpha}(\mu(t,x)) \] 其中：
\[ K_{\alpha} = \left[\begin{matrix} \alpha_0 & \alpha_1 \end{matrix} \right] \] \[ \mu(t,x) = \left[\begin{matrix} h(t,x) & L_fh(t,x)+\frac{\partial h(t,x)}{\partial t} \end{matrix} \right]^T \]

分别计算\(L_f^{2}h(t,x), L_gL_fh(t,x), \frac{\partial^{2} h(t,x)}{\partial t^{2}}\)：
\[ L_fh(t,x) = \nabla h(x) \cdot f(x) = 2(x-x_{obs}) \cdot f(x) \] \[ L_f^{2}h(t,x) = \nabla L_fh(x) \cdot f(x) = 2f(x) \cdot f(x) + 2(x-x_{obs}) \cdot \nabla f(x) \cdot f(x) \] \[ L_gL_fh(t,x) = \nabla L_fh(x) \cdot g(x) = 2f(x) \cdot g(x) + 2(x-x_{obs}) \cdot \nabla f(x) \cdot g(x) \] \[ \frac{\partial h(t,x)}{\partial t} = 2(x-x_{obs}) \cdot \dot{x} \] \[ \frac{\partial^{2} h(t,x)}{\partial t^{2}} = 2(\dot{x} \cdot \dot{x} + (x-x_{obs}) \cdot \ddot{x}) \] 此处\(\cdot\)都是点乘。

代入HOCBF约束中，得到：
\[ 2f(x) \cdot f(x) + 2(x-x_{obs}) \cdot \nabla f(x) \cdot f(x) + 2f(x) \cdot g(x) + 2(x-x_{obs}) \cdot \nabla f(x) \cdot g(x) \cdot u + 2(\dot{x} \cdot \dot{x} + (x-x_{obs}) \cdot \ddot{x}) \geq -K_{\alpha}(\mu(t,x)) \]

QP问题

标准QP问题：
\[ x = argmin \frac{1}{2}x^THx + F^Tx \] \[ s.t. Ax \geq b \] 将HOCBF约束转化为QP问题：
\[ u = argmin \frac{1}{2} ||u - u_{ref}||^2 \] \[ s.t. L_f^{2}h(t,x) + L_gL_fh(t,x)u + \frac{\partial^{2} h(t,x)}{\partial t^{2}} \geq -K_{\alpha}(\mu(t,x)) \] 其中\(u_{ref}\)是控制输入的参考值。
对于正定矩阵H和常数项F，由于：
\[ ||u - u_{ref}||^2 = (u - u_{ref})^T(u - u_{ref}) = u^Tu - 2u^Tu_{ref} + u_{ref}^Tu_{ref} \] 忽略常数项，得到：
\[ H = I \] \[ F = -2u_{ref} \]

再对QP进行求解，得到控制输入u。