机器人碰撞检测————能量动量观测器的降阶ESO形式

一、问题描述

考虑这样一个系统：总能量或总动量记为 $V$，可以通过测量或计算得到；内部受到的能量或冲量记为 $ V_{inn} $ ，也可以测得；外部受到的功率或力记为$ \dot{V}_{ext}$，这是我们真正关心但无法直接测量的量。

系统的物理关系非常简单：

$$\dot{V} = \dot{V}\_{inn} + \dot{V}\_{ext}$$

换句话说，总能量的变化率等于内部作用与外部作用之和。问题是：已知 $V$（可测）和 $\dot{V}_{inn}$（可测），如何估计 $\dot{V}_{ext}$？

这个问题看起来很简单——既然 $\dot{V}_{ext} = \dot{V} - \dot{V}_{inn}$，那直接对 $V$ 求导再减掉 $\dot{V}_{inn}$ 不就行了？

问题在于：实际系统中 $V$ 的测量总是带有噪声的，直接对噪声信号做数值微分会把噪声放大到不可接受的程度。我们需要的是动态观测器——用一个动态系统来“过滤”出我们想要的信号，而不是简单地对测量值求导。

二、扩张状态观测器（ESO）的基本思想

扩张状态观测器的核心思想非常直接：把系统中不确定的扰动项当作一个新的状态变量，与原系统状态一起构成增广状态空间，然后对这个增广系统设计观测器。

对于我们的问题，设状态向量为：

$$\mathbf{x} = \begin{bmatrix} x_1 \\ x_2 \end{bmatrix} = \begin{bmatrix} V \\ \dot{V}\_{ext} \end{bmatrix}$$

其中 $x2 = \dot{V}\{ext}$ 就是我们要观测的外部量。系统的状态方程为：

$$\begin{bmatrix} \dot{x}_1 \\ \dot{x}_2 \end{bmatrix} = \begin{bmatrix} 0 & 1 \\ 0 & 0 \end{bmatrix} \begin{bmatrix} x_1 \\ x_2 \end{bmatrix} + \begin{bmatrix} 1 \\ 0 \end{bmatrix} u$$

其中 $u = \dot{V}_{inn}$ 是已知输入。输出方程为：

$$y = \begin{bmatrix} 1 & 0 \end{bmatrix} \mathbf{x} = x_1 = V$$

这里假设 $\dot{V}_{ext}$ 的变化率（即二阶导数）为零——也就是说，我们认为外部作用在短时间内是常数或缓慢变化的。这是一个典型的常值扰动模型。如果外部作用变化较快，可以把 $x_2$ 的导数建模为某个有界噪声，这会在后面卡尔曼滤波的框架中得到更自然的处理。

ESO 的标准形式是：

$$\dot{\hat{\mathbf{x}}} = A\hat{\mathbf{x}} + Bu + L(y - \hat{y})$$

其中 $\hat{y} = C\hat{\mathbf{x}}$，$L$ 是观测器增益矩阵。展开写就是：

$$\dot{\hat{x}}_1 = \hat{x}_2 + u + l_1(V - \hat{x}_1)$$ $$\dot{\hat{x}}_2 = l_2(V - \hat{x}_1)$$

这就是一个最简单的二阶扩张状态观测器。增益 $l_1$ 和 $l_2$ 的选择决定了观测器的收敛速度和抗噪性能。

三、龙伯格观测器：从极点配置到观测器设计

把上面的 ESO 写成更紧凑的矩阵形式，就是标准的龙伯格观测器（Luenberger Observer）。

对于系统：

$$\dot{\mathbf{x}} = A\mathbf{x} + Bu$$ $$y = C\mathbf{x}$$

龙伯格观测器的形式为：

$$\dot{\hat{\mathbf{x}}} = A\hat{\mathbf{x}} + Bu + L(y - C\hat{\mathbf{x}})$$

其中 $L$ 是观测器增益矩阵。观测误差 $\tilde{\mathbf{x}} = \mathbf{x} - \hat{\mathbf{x}}$ 的动态为：

$$\dot{\tilde{\mathbf{x}}} = (A - LC)\tilde{\mathbf{x}}$$

只要 $(A - LC)$ 是 Hurwitz 稳定的（即所有特征值具有负实部），观测误差就会指数收敛到零。

对于我们的二阶系统：

$$A = \begin{bmatrix} 0 & 1 \\ 0 & 0 \end{bmatrix}, \quad C = \begin{bmatrix} 1 & 0 \end{bmatrix}$$

设 $L = [l_1, l_2]^T$，则：

$$A - LC = \begin{bmatrix} -l_1 & 1 \\ -l_2 & 0 \end{bmatrix}$$

特征多项式为：

$$\det(sI - (A - LC)) = s^2 + l_1 s + l_2$$

如果我们希望观测器的极点位于 $- \omega$（临界阻尼，无振荡），则令：

$$s^2 + l_1 s + l_2 = (s + \omega)^2 = s^2 + 2\omega s + \omega^2$$

因此：

$$l_1 = 2\omega, \quad l_2 = \omega^2$$

这样，通过调节一个参数 $\omega$，我们就可以控制观测器的响应速度。$\omega$ 越大，响应越快，但对噪声越敏感。

龙伯格观测器的设计是确定性的——它假设模型是精确的，噪声可以忽略。增益 $L$ 是通过极点配置人为确定的，完全不考虑系统中存在的随机噪声。

四、降阶观测器：只估计未知的部分

在上面的 ESO/龙伯格观测器中，我们同时估计了 $x1 = V$ 和 $x_2 = \dot{V}\{ext}$ 两个状态。但 $V$ 本身是可以直接测量的，为什么要浪费计算资源去“估计”一个已知量呢？

降阶观测器（Reduced-Order Observer）的思想就是：只估计那些不能直接测量的状态。

对于我们的系统，$x1 = V$ 已知，只需要估计 $x_2 = \dot{V}\{ext}$。为了推导降阶观测器，我们引入一个辅助变量来避免对 $V$ 的直接微分。

设 $\hat{x}_2$ 是 $x_2$ 的估计值。引入辅助变量：

$$z = \hat{x}_2 - l \cdot V$$

其中 $l > 0$ 是观测器增益。对 $z$ 求导：

$$\dot{z} = \dot{\hat{x}}_2 - l\dot{V}$$

由系统方程，$\dot{V} = \dot{V}_{inn} + x_2 = u + x_2$。而观测器的设计使得 $\dot{\hat{x}}_2 = l(V - \hat{x}_1)$，其中 $\hat{x}_1 = V$（因为 $V$ 可直接测量，所以 $\hat{x}_1 = V$）。所以：

$$\dot{\hat{x}}_2 = l(V - V) + l(x_2 - \hat{x}_2) = l(x_2 - \hat{x}_2)$$

但这里出现了 $x_2$ 这个未知量。换一种推导方式：降阶观测器的标准设计方法是把可测状态 $x_1$ 当作已知输入来处理。

更直接的推导如下。由系统方程：

$$\dot{x}_1 = x_2 + u$$

如果 $x_1$ 可直接测量，那么 $x_2 = \dot{x}_1 - u$。但直接对 $x_1$ 求导会放大噪声。我们用辅助变量 $z = \hat{x}_2 - l x_1$ 来回避微分。

对 $z$ 求导：

$$\dot{z} = \dot{\hat{x}}_2 - l\dot{x}_1 = \dot{\hat{x}}_2 - l(x_2 + u)$$

降阶观测器的设计使得 $\dot{\hat{x}}_2 = -l\hat{x}_2 + l\dot{x}_1 - lu$（这个形式可以从上一节的全阶观测器通过代数变换得到）。代入：

$$\dot{z} = (-l\hat{x}_2 + l\dot{x}_1 - lu) - l(x_2 + u)$$

这个推导有些繁琐。更简洁的方式是直接写出降阶观测器的标准形式。对于可测状态 $x_1$ 和待估状态 $x_2$，降阶观测器为：

$$\dot{z} = -l z - l^2 x_1 - l u$$ $$\hat{x}_2 = z + l x_1$$

验证一下：对 $\hat{x}_2 = z + l x_1$ 求导：

$$\dot{\hat{x}}_2 = \dot{z} + l\dot{x}_1 = (-lz - l^2 x_1 - lu) + l(x_2 + u)$$ $$= -l(z + l x_1) + l x_2 = -l\hat{x}_2 + l x_2 = l(x_2 - \hat{x}_2)$$

所以：

$$\dot{\hat{x}}_2 = -l\hat{x}_2 + l x_2$$

在拉普拉斯域：

$$\hat{x}_2(s) = \frac{l}{s + l} x_2(s)$$

这是一个一阶低通滤波器。降阶观测器只有一个可调参数 $l$，它决定了滤波器的截止频率。与全阶观测器相比，降阶观测器阶次更低、计算量更小、相位滞后也更小。

将 $\hat{x}_2 = z + l x_1$ 和 $\dot{z} = -lz - l^2 x_1 - lu$ 合并，还可以得到降阶观测器的一个非常优雅的积分形式：

$$\hat{x}_2 = l\left(x_1 - \int (u + \hat{x}_2) dt\right)$$

这个形式揭示了一个重要的物理直觉：括号里的积分项 $\int (u + \hat{x}_2) dt$ 是利用“已知内部作用”和“当前估计的外部作用”重构出的 $x_1$ 的预测值。观测器本质上是在做模型预测误差的比例反馈——预测值低于测量值时增大估计，高于测量值时减小估计。

五、离散化实现

在实际的数字控制系统中，观测器需要在离散时间下实现。最直接的离散化方法是前向欧拉法（Forward Euler），其映射关系为 $s = (z - 1)/T_s$，其中 $T_s$ 是采样周期。

对于降阶观测器的积分形式：

$$\hat{x}_2 = l\left(x_1 - \int (u + \hat{x}_2) dt\right)$$

离散化后的迭代步骤为：

第一步：更新积分器（预测值）：

$$V_{pred}(k+1) = V_{pred}(k) + T_s \cdot (u(k) + \hat{x}_2(k))$$

第二步：计算当前时刻的估计值：

$$\hat{x}_2(k+1) = l \cdot (V_{meas}(k+1) - V_{pred}(k+1))$$

其中 $V{meas}$ 是测量/计算得到的 $V$，$u = \dot{V}\{inn}$ 是已知的内部变化率。

这种离散化方式属于显式欧拉法，计算简单直接，没有代数环问题。但需要注意稳定性条件：离散系统的极点为 $z_p = 1 - lT_s$，为了保证稳定（极点必须在单位圆内），必须满足 $0 < lT_s < 2$。工程上通常取 $lT_s \leq 0.1 \sim 0.2$ 以保证单调收敛。

六、DLR碰撞检测中的能量观测器与动量观测器

De Luca、Albu-Schäffer、Haddadin 等人在 2006 年提出了基于能量和广义动量的碰撞检测方法，后来在 2017 年的综述中做了系统性的总结和扩展。这些方法的核心思想与上面推导的降阶观测器完全一致，但针对机器人动力学做了具体的物理实现。

能量观测器

对于机器人系统，总能量 $E$ 为动能与势能之和：

$$E = T + U_g = \frac{1}{2}\dot{q}^T M(q)\dot{q} + U_g(q)$$

在无碰撞情况下，能量变化率等于电机输入功率：

$$\dot{E} = \dot{q}^T \tau$$

当发生碰撞时，外部碰撞力矩 $\tau_{ext}$ 会额外做功：

$$\dot{E} = \dot{q}^T(\tau + \tau_{ext})$$

能量观测器的标量残差信号定义为：

$$r(t) = k_O\left(E(t) - \int_0^t (\dot{q}^T\tau + r) ds - E(0)\right)$$

其动态方程为：

$$\dot{r} = k_O(\dot{q}^T\tau_{ext} - r) = k_O(P_{ext} - r)$$

这是一个一阶低通滤波器，由碰撞功率 $P{ext} = \dot{q}^T\tau{ext}$ 驱动。在自由运动时 $r = 0$，碰撞时 $r$ 以时间常数 $1/k_O$ 指数上升。

能量观测器的局限性在于：当机器人静止时（$\dot{q} = 0$），碰撞力矩不会产生功率，因此无法被检测到；此外，当碰撞力与接触点速度方向垂直时（$P_{ext} = 0$），也无法检测。

动量观测器

为了克服能量观测器的局限性，De Luca 等人提出了基于广义动量的观测器。

机器人的广义动量定义为：

$$p = M(q)\dot{q}$$

其动态方程为：

$$\dot{p} = \tau + \tau_{ext} - \alpha(q,\dot{q})$$

其中 $\alpha(q,\dot{q}) = C^T(q,\dot{q})\dot{q} - g(q)$ 是可以根据模型计算出的已知项。

动量观测器的残差向量定义为：

$$r(t) = K_O\left(p(t) - \int_0^t (\tau - \alpha(q,\dot{q}) + r) ds - p(0)\right)$$

在理想条件下，残差的动态为：

$$\dot{r} = K_O(\tau_{ext} - r)$$

写成传递函数形式（分量-wise）：

$$r_i(s) = \frac{k_{O,i}}{s + k_{O,i}} \tau_{ext,i}(s)$$

关键优势：动量观测器与能量观测器有本质区别。

第一，动量观测器是向量形式的，每个关节的残差分量只对应对应关节的外部力矩，实现了完全解耦——这是广义动量动态方程的核心性质。

第二，动量观测器不需要计算惯性矩阵的逆，也不需要加速度信息，避免了数值微分带来的噪声放大。

第三，动量观测器在机器人静止时也能检测碰撞——因为广义动量 $p$ 的动态方程中包含了重力项和科氏力项，即使 $\dot{q} = 0$，外部力矩也会直接反映在 $\dot{p}$ 中。

第四，动量观测器的残差具有隔离能力——如果碰撞发生在第 $i$ 个连杆上，那么残差向量 $r$ 的最后 $n-i$ 个分量将恒为零，从而可以定位碰撞发生的连杆。

正是由于这些优势，动量观测器被广泛应用于实际机器人系统中，包括 KUKA LBR iiwa 和 FRANKA EMIKA 等商业产品。

从我们前面推导的通用框架来看，DLR 的动量观测器正是将我们的通用变量做了如下映射：

通用变量	动量观测器
$V$（可测总量）	广义动量 $p = M\dot{q}$
$\dot{V}_{inn}$（已知内部输入）	$\tau - \alpha(q,\dot{q})$
$\dot{V}_{ext}$（待观测外部量）	碰撞力矩 $\tau_{ext}$
增益 $l$	对角增益矩阵 $K_O$

代入通用公式 $\hat{x}_2 = l(V - \int(u + \hat{x}_2)dt)$，得到的就是 DLR 的动量观测器：

$$r = K_O\left(p - \int(\tau - \alpha + r)dt\right)$$

七、卡尔曼滤波：从确定到随机的最优扩展

上面讨论的所有观测器——ESO、龙伯格观测器、降阶观测器、DLR 的动量和能量观测器——都是确定性的。它们假设模型是精确的，噪声是可以忽略的，增益是通过极点配置或经验确定的。

卡尔曼滤波（Kalman Filter）则完全不同。它将噪声 explicitly 纳入模型，并通过最小均方误差准则自动计算最优增益。

对于我们的系统，加入过程噪声 $w$ 和量测噪声 $v$：

$$\begin{bmatrix} \dot{x}_1 \\ \dot{x}_2 \end{bmatrix} = \begin{bmatrix} 0 & 1 \\ 0 & 0 \end{bmatrix} \begin{bmatrix} x_1 \\ x_2 \end{bmatrix} + \begin{bmatrix} 1 \\ 0 \end{bmatrix} u + \begin{bmatrix} 0 \\ 1 \end{bmatrix} w$$ $$y = \begin{bmatrix} 1 & 0 \end{bmatrix} \mathbf{x} + v$$

其中 $w$ 代表 $\dot{V}_{ext}$ 的随机波动（过程噪声），$v$ 代表 $V$ 测量中的噪声。

离散化后（采样周期 $T_s$）：

$$\mathbf{x}_k = A_d \mathbf{x}_{k-1} + B_d u_{k-1} + \mathbf{w}_{k-1}$$ $$y_k = H\mathbf{x}_k + v_k$$

其中：

$$A_d = \begin{bmatrix} 1 & T_s \\ 0 & 1 \end{bmatrix}, \quad B_d = \begin{bmatrix} T_s \\ 0 \end{bmatrix}, \quad H = \begin{bmatrix} 1 & 0 \end{bmatrix}$$

卡尔曼滤波的递推公式分为两步。

预测步：

$$\hat{\mathbf{x}}_{k|k-1} = A_d \hat{\mathbf{x}}_{k-1|k-1} + B_d u_{k-1}$$ $$P_{k|k-1} = A_d P_{k-1|k-1} A_d^T + Q$$

更新步：

$$K_k = P_{k|k-1} H^T (H P_{k|k-1} H^T + R)^{-1}$$ $$\hat{\mathbf{x}}_{k|k} = \hat{\mathbf{x}}_{k|k-1} + K_k (y_k - H\hat{\mathbf{x}}_{k|k-1})$$ $$P_{k|k} = (I - K_k H) P_{k|k-1}$$

其中 $Q = E[\mathbf{w}\mathbf{w}^T]$ 是过程噪声协方差，$R = E[v^2]$ 是量测噪声协方差。

最终，我们要观测的外部量就是：

$$\hat{\dot{V}}_{ext}(k) = \hat{x}_{2}(k|k)$$

卡尔曼滤波与龙伯格观测器的本质区别

从代数结构上看，在稳态条件下（$Pk$ 收敛到常数），卡尔曼增益 $K_k$ 会收敛到常值 $K\infty$，此时卡尔曼滤波与龙伯格观测器在形式上完全一致——都是 $\hat{x}_{k+1} = A\hat{x}_k + Bu_k + L(y_k - H\hat{x}_k)$。唯一的区别在于增益矩阵 $L$ 的计算方式。

但这个“唯一的区别”带来了本质的不同：

维度	龙伯格观测器	卡尔曼滤波
增益计算	极点配置（人为设定）	由 $Q/R$ 自动计算（最优）
增益特性	固定常数	时变（启动快、稳态平滑）
噪声处理	被动抑制	显式建模与最优权衡
设计依据	期望响应速度	噪声统计特性
输出信息	仅状态估计	状态估计 + 估计协方差

龙伯格观测器的增益是固定的，由设计者通过极点配置人为确定。而卡尔曼滤波的增益是时变的——启动时协方差 $P$ 较大，增益自动增大（快速跟踪）；稳态后 $P$ 收敛，增益自动减小（平滑滤波）。这种“先快后慢”的智能调节是固定增益观测器无法实现的。

更重要的是，卡尔曼滤波显式地建模了噪声的统计特性，并通过 $Q/R$ 的比值在响应速度和噪声抑制之间做出数学最优的权衡。而龙伯格观测器只是靠低通特性被动地抑制噪声。

降阶卡尔曼滤波

既然降阶观测器比全阶观测器更高效，那么自然也存在降阶卡尔曼滤波器。

对于我们的系统，$x1 = V$ 可直接测量，只需要估计 $x_2 = \dot{V}\{ext}$。降阶卡尔曼滤波将状态方程缩减为一维：

$$\dot{x}_2 = w, \quad E[w^2] = Q$$

伪测量方程为：

$$z = \dot{y} - u = x_2 + \text{噪声}$$

但为了避免直接对 $y$ 求导，引入辅助变量 $z = \hat{x}_2 - l y$。

降阶卡尔曼滤波的递推公式为：

协方差与增益更新：

$$\dot{P} = Q - \frac{P^2}{R}, \quad l(t) = \frac{P(t)}{R}$$

辅助变量更新：

$$\dot{z} = -l(t)z - l^2(t)y - l(t)u$$

状态重构：

$$\hat{x}_2(t) = z(t) + l(t)y(t)$$

离散化后（前向欧拉）：

$$P_{k+1} = P_k + T_s\left(Q - \frac{P_k^2}{R}\right)$$ $$l_{k+1} = \frac{P_{k+1}}{R}$$ $$z_{k+1} = z_k + T_s(-l_k z_k - l_k^2 y_k - l_k u_k)$$ $$\hat{x}_2(k+1) = z_{k+1} + l_{k+1} y_{k+1}$$

降阶卡尔曼滤波的计算量远小于全阶卡尔曼滤波（协方差从 $2\times2$ 矩阵退化为了标量），且物理意义清晰：$Q/R$ 的比值决定了滤波器的带宽——比值越大响应越快，比值越小滤波越平滑。稳态时，$l \to \sqrt{Q/R}$，这与龙伯格降阶观测器的固定增益 $l$ 形成对应，但前者是由噪声统计自动算出的最优值。

八、总结

从最开始的简单问题——已知总能量 $V$ 和内部输入 $\dot{V}_{inn}$，估计外部作用 $\dot{V}_{ext}$——我们一步步构建了完整的观测器理论框架：

1. 扩张状态观测器：把待估计的外部量增广为系统状态，设计观测器同时估计所有状态。

2. 龙伯格观测器：ESO 的矩阵形式，通过极点配置确定增益。

3. 降阶观测器：既然 $V$ 可直接测量，只估计未知的 $\dot{V}_{ext}$，减少计算量。

4. 离散化实现：前向欧拉法把连续观测器变为可编程的迭代公式。

5. DLR 能量/动量观测器：将通用框架应用到机器人动力学中，能量观测器提供标量检测，动量观测器提供解耦的向量估计和碰撞隔离能力。

6. 卡尔曼滤波：将噪声 explicitly 纳入模型，自动计算最优时变增益。

7. 降阶卡尔曼滤波：结合降阶观测器的高效和卡尔曼滤波的最优性。

这一系列方法的演进脉络清晰：从确定性的固定增益观测器到随机最优的时变增益滤波器，从全阶估计到降阶估计，核心始终围绕着如何从含噪测量中提取出我们真正关心的信号。DLR 的动量观测器之所以成为工业标准，正是因为它巧妙地利用了广义动量的物理特性，在无需加速度信息和惯性矩阵逆运算的前提下，实现了解耦、快速、可隔离的碰撞检测——而这背后，正是我们最初那个简单物理关系 $\dot{V} = \dot{V}_{inn} + \dot{V}_{ext}$ 的不断深化与拓展。

参考资料

Robot Collisions - A Survey

Collision Detection with DLR Arm