机器人轨迹跟踪控制——CLF-CBF-QP

本次使用MATLAB复现CLF-CBF-QP算法，以实现机器人轨迹跟踪同时保证安全性能

模型

使用自行车模型来进行模拟机器人的移动动态，具体的模型推导参考车辆运动学模型-自行车模型

采用偏差变量
$$\begin{gathered} \tilde{p}=p-p_{ref} \\ \tilde{u}=u-u_{ref} \end{gathered}$$
系统偏差模型写成状态方程形式为
$$\dot{\tilde{p}}=A\tilde{p}+B\tilde{u}=\left[\begin{array}{cccc}0& 0& -v\sin (\phi )& \cos (\phi )\\ 0& 0& v\cos (\phi )& \sin (\phi )\\ 0& 0& 0& \frac{\tan (\delta )}{l}\\ 0& 0& 0& 0\end{array}\right]\tilde{p}+\left[\begin{array}{cc}0& 0\\ 0& 0\\ 0& \frac{v}{l\cos (\delta {)}^2}\\ 1& 0\end{array}\right]\tilde{u}$$
其中，矩阵$A=\frac{\partial \tilde{f}(p,u)}{\partial p}$，$B=\frac{\partial \tilde{f}(p,u)}{\partial u}$

算法

算法使用控制障碍函数保证安全性，控制李雅普诺夫函数保证轨迹跟踪，具体可以参考基于控制障碍函数(Control Barrier Function)的二次规划（QP）控制
$$\begin{array}{c}\begin{array}{rl}& u^{\ast }(x)=\underset{u=(u,\delta )\in {\mathbb{R}}^m\times \mathbb{R}}{\mathrm{argmin}}\frac{1}{2}{u}^{T}H(x)u+F^T(x)u\\ & s.t.\{\begin{array}{l}{L}_{f}V(x)+L_{g}V(x)u+c_{3}V(x)-\delta \le 0\\ -L_{f}h(x)-L_{g}h(x)u-\alpha (h(x))\le 0\end{array}\end{array}\end{array}$$

CLF 选取

基于运动学偏差模型，选取CLF $V:R^n\to R$ 如下
$$V(\tilde{p})=\frac{1}{2}{\tilde{p}}^{T}S\tilde{p}$$
其中，矩阵$S$为 LQR 线性二次调节器过程中产生的 Algebraic Riccaci Equation 等式
$$A^{T}S+SA+Q-SB{R}^{-1}{B}^{T}S=0$$
的解，其中，矩阵$A$、$B$的取值见偏差模型状态方程，满足
V ˙ = d d t p ~ T S p ~ = − ( p ~ T Q p ~ +