1. 引言：人类直觉中的"匀速"

在工业机器人、动画和航天领域，我们对"匀速运动"的直觉包含两个独立的诉求：

1. 平移要直：末端在世界坐标系中沿直线运动，速度恒定。
2. 旋转要匀：姿态绕固定轴以恒定角速度旋转，不忽快忽慢。

SLERP（旋转）+ LERP（平移） 正是同时满足这两个诉求的标准方案。本文用严格的公式说明它为什么"符合直觉"，并对比其他方案的差异。

---

2. SLERP + LERP 的完整公式

2.1 问题设定

给定初始位姿 $T_0=(R_0,p_0)$ 和末位姿 $T_1=(R_1,p_1)$，插值参数 $t\in [0,1]$。

2.2 旋转部分：四元数 SLERP

将旋转矩阵转为单位四元数 $q_0,q_1\in S^3$。定义相对旋转：

$$q_{\text{rel}}=q_0^{-1}\otimes q_1$$
将其写成指数形式：

$$q_{\text{rel}}=\exp \left(\frac{{\omega }_b}{2}\right),{\omega }_b\in {\mathbb{R}}^3$$
其中 ${\omega }_b$ 是相对旋转的轴角向量（体坐标系下）。

SLERP 插值公式：

$$\boxed{q(t)=q_0\otimes \exp \left(t\cdot \frac{{\omega }_b}{2}\right)}$$
展开指数：

$$q(t)=q_0\otimes \left(\cos \frac{\Vert {\omega }_b\Vert t}{2}+{\hat{\omega }}_b\sin \frac{\Vert {\omega }_b\Vert t}{2}\right)$$
其中 ${\hat{\omega }}_b={\omega }_b/\Vert {\omega }_b\Vert$ 是单位旋转轴。

2.3 平移部分：线性插值 LERP

$$\boxed{p(t)=(1-t)p_0+t{p}_1}$$

2.4 完整位姿

$$T(t)=\left[\begin{array}{cc}R(t)& p(t)\\ 0& 1\end{array}\right]$$
其中 $R(t)$ 由 $q(t)$ 转换得到。

---

3. 为什么 $\dot{p}$、${\omega }_b$、${\omega }_s$ 恒定？

3.1 末端点速度 $\dot{p}$ 恒定

对 LERP 求导：

$$\dot{p}(t)=\frac{d}{dt}\left[(1-t)p_0+t{p}_1\right]=p_1-p_0=\text{常数}$$
结论：$\dot{p}$ 是常向量，末端在世界坐标系中沿直线匀速运动。

3.2 体角速度 ${\omega }_b$ 恒定

四元数运动学方程：

$$\dot{q}=\frac{1}{2}q\otimes {\omega }_b$$
对 $q(t)=q_0\otimes \exp (t{\omega }_b/2)$ 求导：

$$\dot{q}(t)=q_0\otimes \exp \left(t\frac{{\omega }_b}{2}\right)\otimes \frac{{\omega }_b}{2}$$
于是：

$$q(t{)}^{-1}\dot{q}(t)=\exp \left(-t\frac{{\omega }_b}{2}\right)\otimes q_0^{-1}\otimes q_0\otimes \exp \left(t\frac{{\omega }_b}{2}\right)\otimes \frac{{\omega }_b}{2}=\frac{{\omega }_b}{2}$$
所以：

$$\boxed{{\omega }_b(t)=2q(t{)}^{-1}\dot{q}(t)={\omega }_b=\text{常数}}$$

3.3 空间角速度 ${\omega }_s$ 恒定

空间角速度的定义：

$${\omega }_s(t)=2\dot{q}(t)q(t{)}^{-1}$$
计算：

$$\dot{q}(t)q(t{)}^{-1}=q_0\otimes \exp \left(t\frac{{\omega }_b}{2}\right)\otimes \frac{{\omega }_b}{2}\otimes \exp \left(-t\frac{{\omega }_b}{2}\right)\otimes q_0^{-1}$$
因为 ${\omega }_b$ 与 $\exp (t{\omega }_b/2)$ 可交换：

$$=q_0\otimes \frac{{\omega }_b}{2}\otimes q_0^{-1}$$
所以：

$$\boxed{{\omega }_s(t)=q_0{\omega }_b{q}_0^{-1}=R_0{\omega }_b=\text{常数}}$$
几何解释：${\omega }_b$ 是旋转轴方向。绕轴旋转不改变轴本身，因此 ${\omega }_s=R(t){\omega }_b=R_0{\omega }_b$ 恒定。

3.4 恒定性总结

量	是否恒定	数学原因
$\dot{p}$	✅	LERP 是线性函数
${\omega }_b$	✅	指数映射保持生成元不变
${\omega }_s$	✅	绕固定轴旋转不改变轴方向

---

4. 对比：若要求空间或体线速度恒定

4.1 空间线速度 $v_s$ 恒定 → ScLERP

空间旋量的线速度分量为：

$$v_s=\dot{p}-{\omega }_s\times p$$
若要求 $v_s$ 恒定，则 $p(t)$ 不能再是直线插值。真实的运动为螺旋运动：

$$T(t)=T_0\exp (t{\xi }^{\wedge }),\xi =(\omega ,v)$$
此时末端轨迹为螺旋线，不再走直线。

4.2 体线速度 $v_b$ 恒定 → 同上

体线速度与空间线速度通过伴随变换关联：

$$v_b=R(t{)}^T(v_s-{\omega }_s\times p)=R(t{)}^T\dot{p}$$
若 $v_b$ 恒定，则 $\dot{p}$ 必须随 $R(t)$ 旋转，末端同样走螺旋线。

4.3 对比表

方案	$\dot{p}$	${\omega }_b$	${\omega }_s$	$v_s$	$v_b$	末端轨迹
SLERP+LERP	✅	✅	✅	❌	❌	直线
ScLERP	❌	✅	✅	✅	✅	螺旋线

核心权衡：末端走直线（满足工艺）与旋量线速度恒定（满足数学优美）不可兼得。

---

5. 为什么 SLERP 用四元数而不是旋转矩阵？

5.1 旋转矩阵插值的困难

若直接对 $R_0$ 和 $R_1$ 做线性插值：

$$R_{\text{lin}}(t)=(1-t)R_0+t{R}_1$$
结果不是旋转矩阵，因为：

• 正交性破坏：$R_{\text{lin}}^T{R}_{\text{lin}}\ne I$
• 行列式不等于 1

必须做正交化（如 SVD），成本极高。

5.2 四元数 SLERP 的优势

维度	四元数 SLERP	旋转矩阵插值
存储	4 个浮点数	9 个浮点数
插值公式	闭式解析	需要矩阵对数/指数
归一化	除以模长	SVD 或 Gram-Schmidt
每步运算	~16 次乘法	~27 次乘法 + 正交化
奇异性	无	无
角速度均匀性	✅ 天然保证	需额外计算

5.3 数学本质

四元数 SLERP 的核心是：

$$q(t)=q_0\otimes \exp \left(t\frac{{\omega }_b}{2}\right)$$
这是 $S^3$ 超球面上的测地线，天然对应等角速度旋转。而矩阵插值无法直接表达这种指数结构。

---

6. 如果直接对 $R$ 或 RPY 插值，角速度均匀吗？

6.1 对旋转矩阵直接插值

若对 $R_0$ 和 $R_1$ 的每个元素做线性插值：

$$R_{\text{naive}}(t)=(1-t)R_0+t{R}_1$$
结果：

• 不是旋转矩阵（正交性破坏）。
• 即使事后正交化，得到的角速度不均匀。
• 因为正交化过程是非线性的，会引入额外的角速度变化。

结论：角速度不均匀 ❌。

6.2 对欧拉角（RPY）插值

若将 $R_0,R_1$ 转为欧拉角 $({\varphi }_0,{\theta }_0,{\psi }_0)$ 和 $({\varphi }_1,{\theta }_1,{\psi }_1)$，然后线性插值：

$$\varphi (t)=(1-t){\varphi }_0+t{\varphi }_1,\text{类似地 }\theta (t),\psi (t)$$
问题：

1. 万向节锁：当 $\theta =\pm \pi /2$ 时，$\varphi$ 和 $\psi$ 耦合，插值结果不可预测。
2. 角速度不均匀：三个欧拉角的线性变化不对应刚体的恒定角速度旋转。因为欧拉角速度与体角速度的关系为：

$${\omega }_b=E(\varphi ,\theta ,\psi )\left[\begin{array}{c}\dot{\varphi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right]$$
其中 $E$ 是与角度相关的矩阵。即使 $\dot{\varphi },\dot{\theta },\dot{\psi }$ 是常数，${\omega }_b$ 也会随姿态变化。

3. 路径非测地线：欧拉角空间的直线在 $SO(3)$ 流形上不是测地线。

结论：角速度不均匀 ❌。

6.3 对比总结

插值方式	角速度均匀？	无奇异性？	结果合法？
四元数 SLERP	✅	✅	✅
旋转矩阵线性插值	❌	✅	❌（需正交化）
欧拉角线性插值	❌	❌（万向锁）	⚠️（一般情况合法）

---

7. 总结

SLERP+LERP 之所以符合人类认知的"匀速"，是因为它同时保证了三个量的恒定：

$$\boxed{\dot{p}=\text{常数},{\omega }_b=\text{常数},{\omega }_s=\text{常数}}$$

• $\dot{p}$ 恒定 → 末端走直线（视觉直观）。
• ${\omega }_b$ 恒定 → 绕体坐标系固定轴匀速旋转（体感直观）。
• ${\omega }_s$ 恒定 → 绕世界坐标系固定轴匀速旋转（空间直观）。

代价：两个旋量线速度 $v_s,v_b$ 不恒定。若追求旋量层面的均匀，则必须用 ScLERP，但末端将走螺旋线而非直线。

工程选择：工业机器人优先满足"末端走直线"（工艺需求），因此 SLERP+LERP 成为标准方案。它牺牲了 $SE(3)$ 流形上的测地线最优性，换来了物理空间中的路径可控性。

四元数的不可替代性：SLERP 必须用四元数实现，因为只有四元数（和旋转矩阵）能表示姿态，而四元数提供了最简洁、高效、无奇异的指数映射结构。欧拉角插值虽然直观，但角速度不均匀且存在万向锁；旋转矩阵插值需要昂贵的正交化，且角速度同样不均匀。