基于SLERP（Spherical Linear Interpolation） 进行旋转滤波
使用 SLERP 进行旋转滤波的方法详解
一、背景与概述
在三维空间中，旋转通常用 四元数（Quaternion） 来表示，因为它相比欧拉角能避免万向节死锁（Gimbal Lock），相比旋转矩阵更加稳定且存储紧凑。
在很多传感器应用（如 IMU 姿态估计、机器人关节角度测量、视觉 SLAM）中，采集到的旋转数据会带有噪声。如果直接使用原始数据，可能造成视觉抖动或控制不稳定。
旋转滤波 就是为了平滑噪声数据。对于欧氏空间的数值滤波，可以直接使用线性插值分别处理每个坐标分量；然而旋转是非线性的，直接分量插值会导致不准确甚至不稳定的结果。这时就要用到 SLERP（球面线性插值）。
二、SLERP 原理
SLERP 是 Ken Shoemake 在 1985 年提出的一种四元数插值方法，用于在单位球面上以恒定速度插值两个旋转。
核心思想
给定两个单位四元数 q1q_1q1 和 q2q_2q2，它们在四维空间中都位于单位超球面上，SLERP 会沿球面的大圆弧从 q1q_1q1 平滑移动到 q2q_2q2，而不是简单地在坐标上做线性插值。
数学公式：
SLERP(q1,q2,t)=sin((1−t)θ)sinθq1+sin(tθ)sinθq2
\text{SLERP}(q_1, q_2, t) = \frac{\sin((1-t)\theta)}{\sin\theta} q_1 + \frac{\sin(t\theta)}{\sin\theta} q_2
SLERP(q1,q2,t)=sinθsin((1−t)θ)q1+sinθsin(tθ)q2
其中：
- t∈[0,1]t \in [0,1]t∈[0,1] 为插值参数
- θ=cos−1(q1⋅q2)\theta = \cos^{-1}(q_1 \cdot q_2)θ=cos−1(q1⋅q2) 表示两个四元数的夹角（弧度）
- 点积 q1⋅q2q_1 \cdot q_2q1⋅q2用于计算夹角
- 插值结果也是单位四元数
特点：
- 姿态变化速度恒定
- 插值结果始终在单位球面上，不会丢失旋转约束
- 能正确处理大角度旋转
三、旋转滤波的思路
假设有一个旋转数据序列：
q0,q1,q2,…,qn
q_0, q_1, q_2, \dots, q_n
q0,q1,q2,…,qn
它可能来自 IMU 或视觉追踪，在时间序列上有抖动。
常见平滑方法
-
滑动平均（Moving Average）
- 在欧氏空间可直接对坐标平均
- 在旋转空间需要用 SLERP 代替分量平均
-
指数平滑（Exponential Smoothing）
- 对新数据做加权插值：
qfiltered=SLERP(qprev,qnew,α) q_{\text{filtered}} = \text{SLERP}(q_{\text{prev}}, q_{\text{new}}, \alpha) qfiltered=SLERP(qprev,qnew,α)
其中 α\alphaα 是平滑系数（0 表示保持原姿态，1 表示完全替换）
- 对新数据做加权插值：
-
卡尔曼滤波（扩展版）
- 状态变量使用四元数，需要额外定义旋转的状态转移和更新公式
- 插值部分同样可在更新步使用 SLERP
四、实现步骤
Step 1: 确保四元数是单位化的
旋转插值要求四元数长度为 1：
q←q∥q∥
q \leftarrow \frac{q}{\|q\|}
q←∥q∥q
Step 2: 计算夹角与点积
dot = np.dot(q1, q2)# 防止浮点误差导致 acos 域越界
dot = np.clip(dot, -1.0,1.0)
theta = np.arccos(dot)
Step 3: SLERP 插值实现（伪代码）
import numpy as npdef slerp(q1, q2, t):q1 = q1 / np.linalg.norm(q1)q2 = q2 / np.linalg.norm(q2)dot = np.dot(q1, q2)# 如果点积为负，反向 q2 以走短弧if dot < 0.0:q2 = -q2dot = -dotdot = np.clip(dot, -1.0, 1.0)theta = np.arccos(dot)if abs(theta) < 1e-10:return q1 # 旋转相差极小sin_theta = np.sin(theta)w1 = np.sin((1 - t) * theta) / sin_thetaw2 = np.sin(t * theta) / sin_thetareturn w1 * q1 + w2 * q2
Step 4: 应用到滤波
例如指数平滑：
alpha = 0.1 # 平滑参数
q_filtered = slerp(q_filtered_last, q_measurement, alpha)
五、注意事项
-
避免长弧插值
- 如果两个四元数的夹角大于 180°，通过反转其中一个四元数，可以选择较短的插值路径。
-
单位化
- 每次插值完成后最好重新单位化结果，消除浮点误差。
-
性能优化
- 如果夹角很小，可以退化为线性插值，减少计算量。
-
插值参数的选择
- 滤波中平滑系数 α 越小，平滑效果越明显，但响应速度越慢。
六、应用实例
- IMU 姿态稳定：将原始四元数姿态与上一时刻的平滑姿态进行 SLERP 以抑制抖动。
- 动画骨骼插值：游戏引擎中使用 SLERP 来平滑骨骼旋转过渡。
- 机器人路径规划：在关节运动或末端执行器姿态控制中进行平滑旋转更新。
七、总结
使用 SLERP 进行旋转滤波的核心优势在于：
- 能保持旋转的几何正确性
- 始终处于单位四元数空间，避免误差累积
- 姿态变化速度恒定，不会出现中途加速或减速的假象
对于需要稳定且准确的旋转数据的场景（传感器融合、三维动画、机器人控制），这是一种简单高效、可直接应用的滤波方法。