机器人轨迹跟踪控制——CLF-CBF-QP
基于MATLAB平台的具体实施和优化研究，在模仿CLF-CBF-QP算法的基础上进行机器人轨迹跟踪功能的设计与开发，并在保证系统运行效率的同时注重安全性。
模型
采用自行车模型用于模拟机器人运动动态的具体过程，请参考车辆运动学模型-自行车模型
基于偏差变量法，
其中，
\tilde{p} = p - p_{ref}，
\tilde{u} = u - u_{ref}。
将系统的偏差模型表示为状态空间形式时，
其运动学方程可展开为：
\dot{\tilde{p}} = A\tilde{p} + B\tilde{u} = \begin{bmatrix} 0 & 0 & -v\sin(\varphi) & \cos(\varphi) \\ 0 & 0 & v\cos(\varphi) & \sin(\varphi) \\ 0 & 0 & 0 & \frac{\tan(\delta)}{l} \\ 0 & 0 & 0 & 0 \end{bmatrix} \tilde{p} + \begin{bmatrix} 0 & 0 \\ 0 & 0 \\ 0 & \frac{v}{l\cos^2(\delta)} \\ 1 & 0 \end{bmatrix} \tilde{u}
其中，
矩阵A和B分别为：
A = \frac{\partial \tilde{f}(p, u)}{\partial p}, B = \frac{\partial \tilde{f}(p, u)}{\partial u}
算法
该算法借助控制障碍函数来确保系统的安全性；同时通过李雅普诺夫函数实现对系统轨迹的跟踪能力。具体而言，在QP问题中求解最优控制输入u*(x)，其目标是最小化二次型性能指标J=1/2uTH(x)u + F^T(x)u；同时满足以下约束条件：（1）局部时变李雅普诺夫稳定性条件L_fV(x)+L_gV(x)u + c_3V(x)-δ ≤ 0；（2）全局时变李雅普诺夫稳定性条件-L_fh(x)-L_gh(x)u - α(h(x)) ≤ 0。
CLF 选取
基于运动学偏差模型，选取CLF V:R^n\rightarrow R 如下
V(\tilde{p})=\frac{1}{2}\tilde{p}^TS\tilde{p}
其中，矩阵S为 LQR 线性二次调节器过程中产生的 Algebraic Riccaci Equation 等式
A^TS + SA+Q - SBR^{-1}B^TS = 0
的解，其中，矩阵A、B的取值见偏差模型状态方程，满足
\dot{V}=\frac{d}{dt}\tilde{p}^TS\tilde{p}=-(\tilde{p}^TQ\tilde{p}+\tilde{u}^TR\tilde{u})
设 \tilde{u}=-K\tilde{p} ， \dot{\tilde{p}} = A\tilde{p}-BK\tilde{p}， 代入上式可得
\begin{equation}\nonumber \tilde{p}^T(A^T S+ SA - K^TB^TS-SBK)\tilde{p} = -\tilde{p}^T(Q+K^TRK)\tilde{p} \end{equation}
去除 \tilde{p} 后，可得
\begin{equation}\nonumber A^T S+ SA - K^TB^TS-SBK = -(Q+K^TRK) \end{equation}
选取反馈控制 K=R^{-1}B^TS，代入上式，可得
\begin{equation}\nonumber A^T S+ SA - S^TB(R^{-1})^TB^TS-SBR^{-1}B^TS = -Q-S^TB(R^{-1})^TRR^{-1}B^TS \end{equation}
若 R 和 S 为对称矩阵，有(R^{-1})^T = R^{-1}，代入上式得Riccati方程
A^T S+ SA +Q-SBR^{-1}B^TS = 0
（这里获取Riccati方程的方法是我自己推导的，不知道对不对，有待考证。我查找讲解LQR、Riccati 最优控制的文章，很少有推导为什么Riccati方程的解可以用于最优控制，这里是我找到的一些相关的讲解，可以参考 [附代码]LQR-从连续到离散，从有穷到无穷、 从变分法到最优控制、matlab 求解Riccati方程（收藏夹） 、 【最优控制】LQR求解公式推导 、 最优控制：代数黎卡提方程ARE(Algebraic Riccati Equation) ）
CBF 选取
CBF 实现的核心目标在于防止系统与障碍物发生碰撞的安全控制机制。该方法主要依据集合的前向不变性原则来限制系统的状态空间，在安全可行区域内进行操作。
传统的CBF选择方法基于移动机器人与障碍物之间欧氏距离的计算。具体而言，在圆形物体的情况下，
h(p) = d(p,p_{obs}) -d_{safe}\quad\qquad\qquad\qquad\qquad\quad \\ = ||p-p_{obs}||^2-(r_{rob}+r_{obs})^2- d_{safe}
在动态环境处理中（即考虑动态障碍物的情况），仅仅采用欧氏距离来作为避障函数（CBF）是不够完善的，在这种情况下未考虑到障碍物运动这一行为。为此引入了关于位置变化率\dot{d}(p,p_{obs}) 的项来模拟这些复杂的变化过程：
h(p) = \dot{d}(p,p_{obs}) + d(p,p_{obs}) - d_{safe}
其中，
h(p) = \frac{d}{dt}||p-p_{obs}||^2 + ||p-p_{obs}||^2 - (r^{rob} + r^{obs})^2 - d^{safe}
进一步分析了障碍物运动的方向性问题，在此过程中将路径划分为两类：一种是朝着机器人靠近的情况；另一种则是朝着机器人远离的情况。
为了应对障碍物靠近的情况，在机器人周围设置导数项\dot{d}(p,p_{obs})以调整安全区域，并提前优化裕量以避免碰撞风险；而当障碍物远离机器人时，则会扩大安全区域的一部分范围并将其视为安全空间可能导致不安全性行为因此采取以下改进措施：
h(p) = k\dot{d}(p,p_{obs}) + d(p,p_{obs}) - d_{safe}
k = \begin{cases}
k \geq 1, & 当\dot{d}(p,p_{obs}) \leq 0 \\
0 < k < 1, & 当\dot{d}(p,p_{obs}) > 0
\end{cases}
最终的QP
针对仿射非线性系统（p = f§ + g§ u）提出了一种新型CLF-CBF-QP框架如下所示：
其中，\gamma\_weight表示参数\gamma的权重，\lambda\_weight表示参数\lambda的权重，w\_weight表示松弛因子w的权重，k、c满足
k = \begin{cases} k\geq 1, \qquad\; if \quad \dot{d}(p,p_{obs}) \leq 0\\ 0\leq k 0 \end{cases} \\ \begin{cases} \frac{\partial \dot{h}}{\partial p_{obs}}\frac{\partial p_{obs}}{\partial t}\leq0,\quad c = 1\\ \frac{\partial \dot{h}}{\partial p_{obs}}\frac{\partial p_{obs}}{\partial t}>0,\quad c = 0 \end{cases}
仿真实例
机器人质点模型参数设置如下：
- 移动机器人前后轮中心间距设定为 l=0.05。
- 移动机器人的初始状态是 x_0=(-4.5,-1,~(2),~(3),~(4))^T，
其中 (2) 表示横坐标值-4.5，
(3) 表示纵坐标值-1，
(4) 表示航向角值，
其余参数均为零。 - 参考轨迹的状态与输入由以下方程组表示：
p_{ref}= \begin{bmatrix} 3.5\cos\left(\dfrac{\pi t}{1}\right)+\pi \\ 3.5\sin\left(\dfrac{\pi t}{1}\right)-\dfrac{\pi}{2} \\ \dfrac{\pi t}{1}-\dfrac{\pi}{2} \\ \dfrac{\pi t}{2} \end{bmatrix},
u_{ref}= \begin{bmatrix} ~ & ~ \\ \arctan\dfrac{1}{7\times1^{}} & \end{bmatrix}.
- 输入约束范围
u_{min}\leq u\leq u_{max}
其中，u_{min} = [-10\ -\frac{\pi}{4}]^T，u_{max} = [10\ \frac{\pi}{4}]^T
单个移动障碍物
在仿真过程中设置圆形障碍物其半径设定为1米；该障碍物围绕坐标系原点做顺时针运动其运行参数包括：轨道半径3米角速度设为0.1 rad/s以及线速度维持在0.3 m/s
观测点位置向量P_{\text{ obs}}由四个分量构成：其具体表达式分别为三倍的余弦函数、三倍的正弦函数、负零点一乘以时间减去二分之π以及零点三。
r_{obs}=1
u_{ref} 被定义为目标轨迹点的输入变量。其他参数设定如下：
\gamma_{weight} 被设定为 10；\gamma_{ref} 同样设定为 10。
\lambda_{weight} 和 \lambda_{ref} 均被设定为 10。
w_{weight} 被设定为 1,000。
矩阵 Q 被设定为 2I^{4\times4} 的形式。
多个动态障碍物
| 第一个障碍物信息 | 第二个障碍物信息 | |
|---|---|---|
| 第一组 | p_{obs1}=\begin{bmatrix}3\cos(-0.1t - \frac{\pi}{4})\\ 3\sin(-0.1t - \frac{\pi}{4})\\ -0.1t - \frac{3}{4}\pi\\ 0.3\end{bmatrix} ，r_{obs1}=1 | p_{obs2}=\begin{bmatrix}3\cos(-0.1t + \frac{\pi}{2})\\ 3\sin(-0.1t + \frac{\pi}{2})\\ -0.1t\\ 0.3\end{bmatrix} ，r_{obs2}=1 |
| 第二组 | p_{obs1}=\begin{bmatrix}4\cos(0.02t - \frac{\pi}{2})\\ 4\sin(0.02t - \frac{\pi}{2})\\ 0.02t\\ 0.08\end{bmatrix} ，r_{obs1}=1 | p_{obs2}=\begin{bmatrix}4\cos(0.02t + \frac{\pi}{4})\\ 4\sin(0.02t + \frac{\pi}{4})\\ 0.02t + \frac{3}{4}\pi\\ 0.08\end{bmatrix} ，r_{obs2}=1 |
