导语在三维旋转处理中四元数、万向锁、Lerp和Slerp是四个紧密关联的概念。通过一个具体的机器人关节旋转示例将这四个概念串联起来深入浅出地解析其原理。最后以问答形式巩固理解。一、 场景机器人肩关节的旋转任务假设我们有一个网球AI教练系统中的机器人其肩关节需要从初始姿态AAA旋转到目标姿态BBB以完成一个发球动作。姿态 A机器人手臂自然下垂朝向正前方。姿态 B手臂抬起同时向外旋转一定角度。我们需要解决三个问题如何用数学描述这两个姿态引入欧拉角与四元数为什么直接对角度进行插值会导致动作突变揭示万向锁如何实现平滑、恒定的旋转动画对比Lerp与Slerp如图所示的三轴陀螺仪若旋转红圈过程中恰巧旋转到了图二所示状态然后进行步骤3时会发现旋转蓝圈与绿圈效果一样。也就是这种状态下旋转Y或Z轴效果一样了都只能使飞机左右倾斜而不能再俯视或仰视它只能头朝天这下坏了飞机操作失灵了只能向上直冲。这就所谓的万向节死锁gimbal lock二、 欧拉角Euler Angles直观的旋转表示1. 什么是欧拉角欧拉角是描述三维刚体取向的一种直观方法它将任意旋转分解为绕三个坐标轴的一系列旋转。通常这三个轴对应偏航Yaw绕垂直轴通常为Y轴或Z轴的旋转如摇头。俯仰Pitch绕水平横向轴通常为X轴的旋转如点头。滚转Roll绕前后纵深轴通常为Z轴或X轴的旋转如翻滚。2. 旋转顺序与约定由于三维旋转不满足交换律先绕X转再绕Y转 ≠ 先绕Y转再绕X转必须约定旋转顺序。常见的约定有航空约定ZYX先偏航Z再俯仰Y最后滚转X。机器人学常用XYZ 或 ZYZ根据具体关节设计而定。在示例中我们假设使用ZYX 顺序偏航-俯仰-滚转。姿态 A 对应(0∘,0∘,0∘)(0^\circ, 0^\circ, 0^\circ)(0∘,0∘,0∘)姿态 B 对应(45∘,90∘,45∘)(45^\circ, 90^\circ, 45^\circ)(45∘,90∘,45∘)。3. 欧拉角的优点与隐患优点极其直观人类容易理解和调试“俯仰角30度”一听就懂。隐患当中间轴如俯仰轴旋转到特定角度如±90∘\pm 90^\circ±90∘时会触发万向锁Gimbal Lock导致自由度丢失。三、 万向锁Gimbal Lock欧拉角的阿喀琉斯之踵1. 通俗理解三轴陀螺仪想象一个机械三轴陀螺仪有三个嵌套的金属环分别控制偏航、俯仰、滚转。正常情况下三个环互不干扰提供三个自由度。但是当中间的俯仰环旋转到90°时最外层的偏航环和最内层的滚转环会共面它们的旋转轴完全重合。此时转动偏航或滚转手臂只能在同一个平面上画圈系统丢失了一个自由度——这就是万向锁。2. 数学本质万向锁的本质是参数化奇异性。在俯仰角为90∘90^\circ90∘时旋转矩阵退化为RRz(α)Ry(90∘)Rx(γ)Rz(αγ)Ry(90∘) R R_z(\alpha) R_y(90^\circ) R_x(\gamma) R_z(\alpha\gamma) R_y(90^\circ)RRz​(α)Ry​(90∘)Rx​(γ)Rz​(αγ)Ry​(90∘)此时α\alphaα和γ\gammaγ无法被独立解耦它们被“捆绑”成一个角度。数学上雅可比矩阵在该点秩亏无法求逆。3. 在示例中的危害如果我们的目标姿态 B 是(45∘,90∘,45∘)(45^\circ, 90^\circ, 45^\circ)(45∘,90∘,45∘)俯仰角正好90∘90^\circ90∘。若使用欧拉角插值从0∘0^\circ0∘到90∘90^\circ90∘逐渐增加俯仰当俯仰接近90∘90^\circ90∘时偏航和滚转会瞬间耦合机械臂末端轨迹会出现非预期的剧烈抖动甚至触发电机保护。四、 四元数Quaternion无奇异的旋转表示1. 为什么需要四元数为了彻底避开万向锁数学家发明了四元数。它是复数的四维扩展形式为qwxiyjzk q w xi yj zkqwxiyjzk其中i2j2k2ijk−1i^2 j^2 k^2 ijk -1i2j2k2ijk−1。在机器人学中我们使用单位四元数模长为1来表示旋转。2. 几何意义轴角表示任意一个三维旋转都可以理解为绕某个单位轴(x,y,z)(x, y, z)(x,y,z)旋转θ\thetaθ角度。四元数完美封装了这两者wcos⁡(θ/2)w \cos(\theta/2)wcos(θ/2)(x,y,z)sin⁡(θ/2)⋅旋转轴(x, y, z) \sin(\theta/2) \cdot \text{旋转轴}(x,y,z)sin(θ/2)⋅旋转轴为什么是θ/2\theta/2θ/2因为四元数旋转通过两次乘法实现q⋅p⋅q−1q \cdot p \cdot q^{-1}q⋅p⋅q−1一次乘法贡献一半旋转两次刚好完成θ\thetaθ旋转。3. 四元数如何解决万向锁四元数直接在一个四维超球面上描述旋转不存在“依次旋转的三个轴”因此天然没有奇点。无论旋转路径如何它都能平滑过渡。在示例中我们将姿态 A 和 B 转换为四元数qAq_AqA​和qBq_BqB​完全避开了欧拉角的陷阱。五、 Slerp 与 Lerp 插值平滑旋转的保证现在我们需要在qAq_AqA​和qBq_BqB​之间生成中间帧让机器人手臂平滑运动。1. Lerp线性插值Lerp 在两点间沿直线插值v(t)(1−t)v0tv1v(t) (1-t)v_0 t v_1v(t)(1−t)v0​tv1​。问题1角速度不均匀。直线切割单位球面导致在起点和终点附近角速度慢中间快。手臂动作会“一顿一顿”。问题2向量缩水。插值结果模长小于1需要额外归一化Nlerp且当qAq_AqA​与qBq_BqB​夹角接近180∘180^\circ180∘时中间向量趋近零数值不稳定。2. Slerp球面线性插值Slerp 在单位球面上沿**最短圆弧大圆**插值Slerp(q0,q1,t)sin⁡((1−t)Ω)sin⁡Ωq0sin⁡(tΩ)sin⁡Ωq1 \text{Slerp}(q_0, q_1, t) \frac{\sin((1-t)\Omega)}{\sin\Omega} q_0 \frac{\sin(t\Omega)}{\sin\Omega} q_1Slerp(q0​,q1​,t)sinΩsin((1−t)Ω)​q0​sinΩsin(tΩ)​q1​其中Ωarccos⁡(q0⋅q1)\Omega \arccos(q_0 \cdot q_1)Ωarccos(q0​⋅q1​)是两四元数的夹角。Slerp 的优势恒定角速度参数ttt线性对应旋转角度运动丝滑。天然单位长度结果始终在球面上无需归一化。数值稳定只要q0q_0q0​和q1q_1q1​不互为反向公式始终有效。在示例中使用 Slerp 能让机器人手臂以恒定速度从 A 旋转到 B完美完成发球动作。importnumpyasnp# 1. 欧拉角转四元数示例ZYX顺序defeuler_to_quat(yaw,pitch,roll):cynp.cos(yaw*0.5)synp.sin(yaw*0.5)cpnp.cos(pitch*0.5)spnp.sin(pitch*0.5)crnp.cos(roll*0.5)srnp.sin(roll*0.5)wcr*cp*cysr*sp*sy xcr*sp*sy-sr*cp*cy ycr*sp*cysr*cp*sy zsr*sp*cy-cr*cp*syreturnnp.array([w,x,y,z])# 2. 定义姿态q_Aeuler_to_quat(0,0,0)# 初始姿态q_Beuler_to_quat(np.pi/4,np.pi/2,np.pi/4)# 目标姿态含90°俯仰# 3. Slerp 插值defslerp(q0,q1,t):dotnp.dot(q0,q1)ifdot0:# 走最短路径q1-q1 dot-dot dotnp.clip(dot,-1,1)Omeganp.arccos(dot)ifOmega1e-6:return(1-t)*q0t*q1 sin_Omeganp.sin(Omega)return(np.sin((1-t)*Omega)/sin_Omega)*q0(np.sin(t*Omega)/sin_Omega)*q1# 4. 生成平滑轨迹trajectory[]fortinnp.linspace(0,1,30):q_interpslerp(q_A,q_B,t)trajectory.append(q_interp)七、 原理解析Q1: 欧拉角为什么需要约定旋转顺序A:三维空间中的旋转是不可交换的。先绕X轴转30°再绕Y轴转30°与先绕Y轴转30°再绕X轴转30°最终姿态完全不同。因此必须严格约定顺序如ZYX否则同一个(α,β,γ)(α, β, γ)(α,β,γ)会对应无数种姿态。Q2: 万向锁是机械结构的故障吗A:不是。它是欧拉角数学表示的固有缺陷。当中间轴旋转到90°时第一轴和第三轴的旋转轴在数学上重合导致自由度丢失。即使是最完美的机械陀螺仪只要使用欧拉角描述就会遇到这个问题。Q3: 四元数旋转为什么要用θ/2\theta/2θ/2A:四元数旋转操作是q⋅p⋅q−1q \cdot p \cdot q^{-1}q⋅p⋅q−1这是一个共轭乘法。代数上可以证明这种双重乘法会将旋转角度加倍。因此为了最终得到θ\thetaθ的旋转四元数本身必须存储θ/2\theta/2θ/2。Q4: 为什么 Slerp 能保证恒定角速度A:Slerp 的公式是从球面几何推导出来的。它使得插值参数ttt与球面上的弧长成严格正比而弧长对应旋转角度。因此匀速变化的ttt直接对应匀速旋转角速度自然恒定。Q5: 在什么情况下可以用 Lerp 代替 SlerpA:当两个四元数夹角非常小如小于 5°时直线弦和圆弧大圆几乎重合此时 Lerp 归一化Nlerp的计算量远小于 Slerp且视觉上难以区分。但在大角度或高精度机器人控制中必须使用 Slerp。八、 总结通过机器人肩关节旋转的示例我们串联了四个核心概念欧拉角直观但存在万向锁隐患。万向锁欧拉角在中间轴90°时的自由度丢失是数学奇异性。四元数用四维向量无奇点地表示旋转彻底避开万向锁。Slerp/LerpSlerp 在单位球面上沿圆弧插值保证恒定角速度Lerp 走直线仅适用于小角度。理解这些概念及其内在联系是掌握三维旋转处理、动作捕捉与机器人轨迹规划的关键。