机器人控制：Slotine-Li 自适应控制公式推导与实现

前言：为什么要搞自适应控制？

常规的 PID 或计算力矩法（Computed Torque）非常依赖精确的物理参数（质量、惯量）。
现实很残酷：

机械臂抓了不同重量的物体，总质量变了。
长期运行后，摩擦系数变了。

如果模型参数不准，刚性控制器的轨迹跟踪就会产生静差甚至振荡。
Slotine-Li 自适应控制的核心思想是：在线实时调整参数估计值（$\hat{\theta}$），让控制器自己“学会”真实物理参数。

第一部分：核心数学推导

我们以刚性机械臂为例（柔性关节机器人需要先做 QFA 变换，但自适应的逻辑完全一致）。

第 1 步：定义“综合误差”滑模面 $s$

我们定义位置误差：

$\tilde{x} = x - x_d$

我们不看单一的位置误差或速度误差，而是将两者结合成一个新变量 $s$（滑模面）：

$s = \dot{\tilde{x}} + \Lambda \tilde{x}$

这里的 $\Lambda$ 是一个正数（比如 5）。
物理直觉：如果我们能把 $s$ 控制到 0，那么 $\dot{\tilde{x}} = -\Lambda \tilde{x}$，这是一个一阶微分方程，其解为指数衰减。这就把二阶的跟踪问题，简化为了一阶的收敛问题。

第 2 步：引入“参考速度”

我们不敢直接测加速度（噪声太大），所以定义一个虚拟参考速度 $\dot{x}_r$：

$\dot{x}_r = \dot{x}_d - \Lambda \tilde{x}$

你会发现一个极其优美的性质：

$s = \dot{x} - \dot{x}_r$

也就是说，滑模面 $s$ 就是真实速度与参考速度的差值。

第 3 步：机器人动力学的“线性参数化”技巧

真实机器人的动力学（牛顿-欧拉方程）可以写成标准形式：

$M(x)\ddot{x} + C(x, \dot{x})\dot{x} + G(x) = \tau$

Slotine 发现，无论机械臂多复杂，上面等式左边的前两项（惯性力+科氏力+重力）永远可以写成“已知矩阵 $Y$”乘以“未知常数向量 $\theta$”：

$M\ddot{x}_r + C\dot{x}_r + G = Y(x, \dot{x}, x_r, \dot{x}_r) \cdot \theta$

$\theta$：未知物理参数集（如质量、转动惯量），最好是最小惯性参数集。我们就是要在线估计它。
$Y$：回归矩阵（Regressor），它只包含位置、速度、参考速度和参考加速度。全是已知数或可测量！

第 4 步：写出带估计值的控制律

因为真实参数 $\theta$ 未知，我们使用在线估计值 $\hat{\theta}$ 来计算控制力矩 $\tau$：

$\tau = Y \hat{\theta} - K_d s \tag{1}$

前项 $Y\hat{\theta}$：利用估计参数进行前馈补偿。
后项 $-K_d s$：阻尼反馈项（$K_d$ 是正定增益），负责将 $s$ 拉向 0。

第 5 步：推导误差动态方程

我们将控制律 (1) 代入真实动力学方程，经过化简（把 $\dot{x} = s + \dot{x}_r$ 代入），得到闭环误差动态方程：

$M \dot{s} + C s = -K_d s + Y(\hat{\theta} - \theta)$

定义参数估计误差 $\tilde{\theta} = \hat{\theta} - \theta$，得到终极误差方程：

$M \dot{s} + C s = -K_d s + Y \tilde{\theta} \tag{2}$

解读：方程左边是我们想要控制的误差变化，右边第一项是“稳定项”，第二项 $Y\tilde{\theta}$ 是“参数没估准带来的扰动项”。

第二部分：如何保证稳定性？（李雅普诺夫方法）

我们需要找到一个规则来更新 $\hat{\theta}$，让系统稳定。我们构造一个“虚拟总能量” $V$：

$V = \frac{1}{2} s^T M s + \frac{1}{2} \tilde{\theta}^T \Gamma^{-1} \tilde{\theta} \tag{3}$

第一项：实际运动的动能。
第二项：参数估计误差的“虚拟势能”（$\Gamma$ 是正定的自适应学习率矩阵）。

如果 $V$ 一直在减小，系统就绝对稳定。对 (3) 求导：

$\dot{V} = s^T M \dot{s} + \frac{1}{2} s^T \dot{M} s + \dot{\tilde{\theta}}^T \Gamma^{-1} \tilde{\theta}$

把误差方程 (2) 代入，并利用机器人动力学的斜对称性质（$s^T(\dot{M} - 2C)s = 0$，这是物理能量守恒决定的），可以消去麻烦的 $\dot{M}$ 项，化简得：

$\dot{V} = -s^T K_d s + s^T Y \tilde{\theta} + \dot{\tilde{\theta}}^T \Gamma^{-1} \tilde{\theta}$

为了确保 $\dot{V} \le 0$，我们强制让后两项等于 0，解得参数更新法则（自适应律）：

$\dot{\hat{\theta}} = -\Gamma \cdot Y^T \cdot s \tag{4}$

此时，$\dot{V} = -s^T K_d s \le 0$。根据 Barbalat 引理，当 $t \to \infty$ 时，$s \to 0$，因此位置误差 $\tilde{x} \to 0$。控制器设计完毕！

第三部分：在控制器中如何迭代？

1. 初始化

设定学习率 $\Gamma$（比如对角矩阵，数值 0.1）。
设定初始参数猜测值 $\hat{\theta}_{0}$（可以随便猜，比如全 0）。

2. 主循环（每 1ms 执行一次）

步骤	计算机动作	具体计算公式
Step 1	传感器读取	获取当前实际位置 $x_k$ 和速度 $\dot{x}_k$。
Step 2	轨迹插值	根据当前时刻，查表得到期望位置 $x_{d,k}$，速度 $\dot{x}_{d,k}$，加速度 $\ddot{x}_{d,k}$。
Step 3	计算误差与 $s$	$\tilde{x}_k = x_k - x_{d,k}$ $s_k = (\dot{x}_k - \dot{x}_{d,k}) + \Lambda \tilde{x}_k$
Step 4	计算参考量	$\dot{x}_{r,k} = \dot{x}_{d,k} - \Lambda \tilde{x}_k$ $\ddot{x}_{r,k} = \ddot{x}_{d,k} - \Lambda \dot{\tilde{x}}_k$
Step 5	组装 $Y$（重点！）	将当前已知的状态填入回归矩阵。例：$Y_k = [\ddot{x}_{r,k}, \dot{x}_{r,k}]$。注意：$Y$ 不是“更新”出来的，是“赋值”出来的！
Step 6	计算控制力矩（给电机）	$\tau_k = Y_k \cdot \hat{\theta}_{k-1} - K_d \cdot s_k$ （这里用的是上一时刻更新出来的参数）
Step 7	更新参数（存起来）	$\hat{\theta}_{k} = \hat{\theta}_{k-1} - \Gamma \cdot Y_k^T \cdot s_k \cdot \Delta t$ （$\Delta t$ 是控制周期，如 0.001s）

3. 总结迭代本质

$Y$ 每一拍都在变，但它是由传感器读数直接算出来的，不需要积分。
$\hat{\theta}$ 每一拍都在累加修正。如果误差 $s$ 大，$\hat{\theta}$ 就跳变得快；如果误差已经很小了，$\hat{\theta}$ 就几乎不动了。

第四部分：注意事项

1. 必须使用“最小惯性参数集”（基参数）

千万不要把“质量 $m$”和“质心 $l$”作为两个独立的参数去估计！因为在重力项中它们永远以乘积 $m \times l$ 出现。如果你硬要分开估计，回归矩阵 $Y$ 的列会线性相关，导致 $\hat{\theta}$ 发生参数漂移（一个变+1万，一个变-1万），力矩饱和振荡。必须先将冗余参数合并成一个基参数。

2. 持续激励条件（PE Condition）

如果机器人只是静止或匀速直线运动，$Y$ 矩阵里的某些列可能全是 0。此时 $\dot{\hat{\theta}} = 0$，参数根本不会更新。必须让机器人做持续晃动（正弦扫频），才能让参数收敛到真值。

3. 积分器防饱和

虽然公式里是纯积分，但如果长期存在外界干扰，$\hat{\theta}$ 可能会一直积分到很大的数值。工程上通常要加一个死区（Dead-zone）：当误差 $s$ 小于某个阈值时，强制停止参数更新（$\dot{\hat{\theta}} = 0$），防止噪声污染积分器。

结语

Slotine-Li 自适应控制的美妙之处在于，它将一个复杂的非线性跟踪问题，通过“滑模面 $s$”和“线性参数化 $Y\theta$”巧妙地转化为了一个简单的梯度下降问题。

控制律：$\tau = Y\hat{\theta} - K_d s$（用猜测的参数去补偿力）。
自适应律：$\dot{\hat{\theta}} = -\Gamma Y^T s$（根据当前误差反向修正参数）。
迭代：$Y$ 每拍重算，$\hat{\theta}$ 每拍累加。