简介欧拉角与四元数是机器人学中描述三维旋转的两种常用方式这里提供的是MATLAB实现将二者转换为机器手坐标系的旋转描述矩阵适合机器人控制、运动规划与三维仿真方向的开发者学习参考。资源包共2个文件均为.m脚本分别实现欧拉角模式与四元数模式的矩阵转换代码精简便于直接调用与二次修改也适合初学者对照学习矩阵推导。针对Z-Y-X等常见欧拉角顺序程序先分解各轴旋转矩阵再合成总旋转矩阵四元数部分则依据规范化公式生成对应的3×3旋转矩阵并附有齐次变换矩阵平移部分的说明便于理解完整坐标系描述。已有1734人学习适合机械臂位姿解算、虚拟现实场景构建等场景直接套用或扩展。1. 从“旋转怎么存”到“机械臂怎么动”先搞清楚这三套语言做机械臂控制、机器人仿真或者视觉抓取的人十有八九都绕不开一个问题同一个旋转为什么有人给你欧拉角有人给你四元数还有人直接甩你一个3x3的矩阵如果你刚接触这块大概率会对着roll, pitch, yaw和(w, x, y, z)发懵更别说把它们塞进机械臂的位姿描述矩阵里让真实的手臂按你期望的角度转起来。先明确一个核心概念机械手坐标系描述矩阵本质上是描述机械臂末端或者工具相对于基座坐标系通常会拆成“姿态矩阵 位置向量”的完整位姿。姿态部分就是3x3旋转矩阵位置部分是3x1平移向量合起来就是我们常说的4x4齐次变换矩阵R [[r11, r12, r13], [r21, r22, r23], [r31, r32, r33]] T [[r11, r12, r13, px], [r21, r22, r23, py], [r31, r32, r33, pz], [0, 0, 0, 1]]R就是旋转矩阵(px, py, pz)是末端原点在基座坐标系下的位置。我们说的“把欧拉角/四元数转化为描述矩阵”核心任务就是根据给定的欧拉角或四元数算出R再结合位置向量拼成最终的齐次矩阵然后喂给机械臂控制器或机器人库。那这篇内容适合谁如果你正在做机械臂运动学求解、机器人仿真、视觉标定、ROS/URDF 建模或者要手写一个角度转换函数库这篇文章就是给你准备的操作手册。我会把欧拉角、四元数、旋转矩阵三者的关系讲透再给出可直接复用的 Python/C 实现最后附上我在实际机械臂调试中踩过的坑。2. 欧拉角转旋转矩阵先搞懂“绕轴顺序”和“内旋/外旋”这两个坑2.1 欧拉角到底在描述什么旋转欧拉角的本质是把一个复杂的空间旋转拆成绕三个坐标轴的依次旋转。听起来简单但实际使用中至少有两个变量需要先定死旋转顺序比如ZYX、ZYZ、XYZ顺序不同最终姿态完全不同。内旋还是外旋内旋是每次绕旋转后的新坐标轴转外旋是绕固定的世界坐标轴转。同一组欧拉角内旋和外旋的结果差异非常大。以工业机械臂最常用的ZYX顺序为例roll绕X轴、pitch绕Y轴、yaw绕Z轴对应顺序是先绕 Z 轴转 yaw再绕新的 Y 轴转 pitch最后绕新的 X 轴转 roll。注意很多控制柜里的欧拉角约定并不是ZYX而是ZXZ、ZYZ或ZYX的某种变体。拿到角度值之前先确认你手上的欧拉角是哪套约定不然后面所有计算全是错的。2.2 ZYX 欧拉角转旋转矩阵的具体公式设yaw γpitch βroll α。先定义三个基础旋转矩阵R_z(γ) [[cosγ, -sinγ, 0], [sinγ, cosγ, 0], [0, 0, 1]] R_y(β) [[cosβ, 0, sinβ], [0, 1, 0], [-sinβ, 0, cosβ]] R_x(α) [[1, 0, 0], [0, cosα, -sinα], [0, sinα, cosα]]对于内旋ZYX顺序旋转矩阵R R_z(γ) * R_y(β) * R_x(α)。展开化简后得到R [[cosβ*cosγ, sinα*sinβ*cosγ - cosα*sinγ, cosα*sinβ*cosγ sinα*sinγ], [cosβ*sinγ, sinα*sinβ*sinγ cosα*cosγ, cosα*sinβ*sinγ - sinα*cosγ], [-sinβ, sinα*cosβ, cosα*cosβ]]如果你手头的是外旋ZYX结果矩阵是R R_x(α) * R_y(β) * R_z(γ)展开后矩阵数值会不同。核心口诀是内旋顺序是右乘外旋顺序是左乘。这一条记住了后面所有推导都好办。2.3 从旋转矩阵反解欧拉角的注意事项有些场景下你是从旋转矩阵反解欧拉角比如手眼标定后从矩阵里提取角度值。这时候要特别注意**万向锁Gimbal Lock**问题。当pitch角接近 ±90° 时R[0][2] cosα*sinβ*cosγ sinα*sinγ里面的roll和yaw会互相耦合出现同一个旋转矩阵对应多组欧拉角的歧义。实际项目中我碰到过不少次这种情况机械臂末端姿态看起来没问题但示教器上显示的角度值突然跳变就是因为反解时没处理好万向锁分支。3. 四元数转旋转矩阵为什么说它是“最稳”的表达方式3.1 四元数的四个数到底代表什么四元数写成q (w, x, y, z)其中w是实部(x, y, z)是虚部。在旋转表达里它等价于“绕一个单位向量(vx, vy, vz)旋转角度 θ”对应关系是w cos(θ/2) x vx * sin(θ/2) y vy * sin(θ/2) z vz * sin(θ/2)所以四元数天然满足w² x² y² z² 1。如果某个传感器给你的四元数加起来不等于 1先做归一化不然后面转出来的矩阵可能“不干净”。3.2 单位四元数转旋转矩阵公式给定单位四元数q (w, x, y, z)对应的旋转矩阵为R [[1 - 2(y² z²), 2(xy - wz), 2(xz wy) ], [2(xy wz), 1 - 2(x² z²), 2(yz - wx) ], [2(xz - wy), 2(yz wx), 1 - 2(x² y²) ]]这个公式一定要背熟它比欧拉角公式更稳定、没有万向锁问题。实际写代码时我建议用一个变量缓存2x²、2y²、2xy这类重复计算省得每次调三角函数性能在小控制器上也能好不少。3.3 四元数转欧拉角的三种常见公式如果你拿到的是四元数但机械臂控制接口偏偏只收欧拉角很多老款控制柜就是这样那就需要做二次转换。以ZYX约定为例已知四元数(w, x, y, z)roll atan2(2(wx yz), 1 - 2(x² y²)) pitch asin(2(wy - zx)) yaw atan2(2(wz xy), 1 - 2(y² z²))注意asin的参数如果落出[-1, 1]说明四元数精度有问题或归一化不到位需要先做钳位处理。实操心得我在验证四元数转欧拉角是否写对时会先用一组已知的欧拉角比如(30°, 60°, 90°)转成四元数再转回欧拉角对比误差。这个闭环验证步骤别省能帮你快速定位到底是公式写错还是数据约定不对。4. 实操用 Python 写一个完整的“角度转矩阵”工具4.1 基础库怎么选numpy 还是 scipy做机器人领域的人大多用 Python 搭配 numpy。如果你不想自己背公式scipy.spatial.transform里的Rotation类是最省事的但它对底层原理隐藏比较深不利于理解。我建议两个都掌握验证用 scipy实际工程自己写一份。import numpy as np from scipy.spatial.transform import Rotation as R # 欧拉角转四元数 r R.from_euler(ZYX, [yaw, pitch, roll], degreesTrue) quat r.as_quat() # 返回 [x, y, z, w] # 四元数转旋转矩阵 mat r.as_matrix() # 四元数转欧拉角 euler r.as_euler(ZYX, degreesTrue)要注意的是scipy 的as_quat()返回顺序是(x, y, z, w)而很多机械臂 SDK比如 ROS 的 geometry_msgs::Quaternion顺序是(w, x, y, z)。顺序弄反的话你会在调试时得到一个“看起来正常但实际乱转”的机械臂非常折磨。4.2 手写实现一份可直接复用的转换模块下面这份代码我做了精简只保留核心函数包含欧拉角ZYX 内旋转矩阵、四元数转矩阵、矩阵转四元数。实际项目里可以直接把rotation_utils.py拖进去。import numpy as np def euler_zyx_inner_to_matrix(roll, pitch, yaw): 内旋 ZYX 欧拉角转旋转矩阵角度单位弧度 cr, sr np.cos(roll), np.sin(roll) cp, sp np.cos(pitch), np.sin(pitch) cy, sy np.cos(yaw), np.sin(yaw) R np.array([ [cp * cy, sr * sp * cy - cr * sy, cr * sp * cy sr * sy], [cp * sy, sr * sp * sy cr * cy, cr * sp * sy - sr * cy], [-sp, sr * cp, cr * cp] ]) return R def quaternion_to_matrix(q): 单位四元数(qx, qy, qz, qw)转旋转矩阵 x, y, z, w q R np.array([ [1 - 2 * (y * y z * z), 2 * (x * y - z * w), 2 * (x * z y * w)], [2 * (x * y z * w), 1 - 2 * (x * x z * z), 2 * (y * z - x * w)], [2 * (x * z - y * w), 2 * (y * z x * w), 1 - 2 * (x * x y * y)] ]) return R def matrix_to_quaternion(mat): 旋转矩阵转四元数返回(qx, qy, qz, qw) tr np.trace(mat) if tr 0: S np.sqrt(tr 1.0) * 2 w 0.25 * S x (mat[2][1] - mat[1][2]) / S y (mat[0][2] - mat[2][0]) / S z (mat[1][0] - mat[0][1]) / S else: if mat[0][0] mat[1][1] and mat[0][0] mat[2][2]: S np.sqrt(1.0 mat[0][0] - mat[1][1] - mat[2][2]) * 2 w (mat[2][1] - mat[1][2]) / S x 0.25 * S y (mat[0][1] mat[1][0]) / S z (mat[0][2] mat[2][0]) / S elif mat[1][1] mat[2][2]: S np.sqrt(1.0 mat[1][1] - mat[0][0] - mat[2][2]) * 2 w (mat[0][2] - mat[2][0]) / S x (mat[0][1] mat[1][0]) / S y 0.25 * S z (mat[1][2] mat[2][1]) / S else: S np.sqrt(1.0 mat[2][2] - mat[0][0] - mat[1][1]) * 2 w (mat[1][0] - mat[0][1]) / S x (mat[0][2] mat[2][0]) / S y (mat[1][2] mat[2][1]) / S z 0.25 * S return np.array([x, y, z, w])这段代码我在 Ubuntu ROS Noetic 环境下用 numpy 跑过精度和 scipy 对齐到小数点后 6 位。唯一要注意的就是所有角度用弧度从示教器读出来的通常是度数得先np.deg2rad()转换。4.3 组装 4x4 齐次变换矩阵的通用函数有了旋转矩阵下一步就是把它和平移向量拼成最终送给机械臂的数据格式。以常见的机器人控制库为例def compose_homogeneous_matrix(mat_rot, trans_vec): 旋转矩阵 平移向量 - 4x4 齐次矩阵 T np.eye(4) T[:3, :3] mat_rot T[:3, 3] trans_vec return T机械臂 SDK 里常见的位姿描述就是这样的 4x4 数组只是不同厂家的数组存储顺序和索引方式不一定相同。有的接口要的是[r11, r12, r13, px, ...]按行展开的一维数组有的接口要的是嵌套列表。转换前先看一眼官方额定义别想当然。5. 坐标系进阶从相机到机械臂底座的矩阵链5.1 机械臂位姿描述为什么需要多个坐标系很多刚接触机器人的同学会问我直接给机械臂一个末端位姿不就行了吗为什么还有“基座坐标系”“工具坐标系”“工件坐标系”这么多概念原因是不同的坐标参考下同一个姿态的矩阵数值完全不同。机械臂出厂时控制器记录的末端位姿通常是以基座坐标系为参考的但你从相机或视觉系统拿到的目标点是以相机坐标系为参考的。要把一个相机坐标系下的位置换算成机械臂基座坐标系下的位置就是做矩阵链乘法T_base_to_object T_base_to_camera * T_camera_to_object这里面T_base_to_camera就是手眼标定的结果通常也是 4x4 齐次矩阵。你可以把每个“坐标系转换”想象成一个接力棒每一段变换矩阵负责把坐标从一个参考系换算到下一个参考系多个矩阵连续相乘最后拿到目标点在机械臂基座系下的坐标。5.2 相机坐标系中的“右手系”判断方法热门搜索词里有人问“相机坐标系的右手系怎么判断 xy 轴方向”这个问题我实际标定时也踩过。最简单的判断方法是先确认x轴方向通常水平向右再确认y轴方向通常竖直向下注意这个和很多机器人坐标系的 y 轴向上相反用右手定则伸出右手四指从x轴向y轴弯曲拇指指向的就是z轴方向。相机坐标系的z轴通常朝前指向场景所以x轴朝右、y轴朝下正是右手系。如果你发现机械臂抓取时位置总是偏了一个固定的旋转量大概率是相机坐标系和机械臂坐标系的轴方向约定没对齐这时候不是改算法而是检查转换矩阵里的旋转部分是否配错了。5.3 外参标定后的矩阵链乘法实例假设你在机械臂底座上装了一个相机标定得到T_base_to_camera视觉算法给出目标点在相机坐标系下的齐次坐标p_camera (x, y, z, 1)。那么目标点在机械臂基座坐标系下的坐标就是p_base T_base_to_camera p_camera如果还要进一步转换到工具坐标系下抓取就再左乘一个T_tool_to_base的逆矩阵。这也就是为什么从欧拉角/四元数转出来的旋转矩阵非常重要——它不止用来描述末端姿态也是整条坐标变换链里最基础的零件。6. 常见问题排查与性能调优实际项目中的避坑清单6.1 角度顺序不对导致机械臂姿态镜像我最早做机械臂视觉抓取时从相机标定拿到一个四元数转成矩阵后直接发给机械臂结果机械臂末端姿势和预期完全相反。排查了半天发现是因为相机标定给的坐标顺序是(x, y, z, w)而机械臂 SDK 读取四元数时按(w, x, y, z)解析才导致姿态反了。这个问题的排查思路其实很固定先打印原始四元数、归一化后的四元数、旋转矩阵再用一组已知角度的测试用例验证公式是否正确最后检查 SDK 接口要求的四元数顺序。建议每步都单独验证别一口气把整个流程跑通后再找问题。6.2 四元数没有归一化导致矩阵不正交万向锁和矩阵非正交的问题通常是相互关联的。如果你的四元数来自传感器融合算法原始数据可能会有轻微漂移w²x²y²z²不等于 1。转出来的旋转矩阵各列之间不再严格正交做逆矩阵时会出很大偏差。常规做法是使用前先归一化norm np.sqrt(qw*qw qx*qx qy*qy qz*qz) qw / norm qx / norm qy / norm qz / norm另外如果你的机械臂 SDK 库没有内置位姿合法性检查建议在发送前把旋转矩阵的行列式检查一下np.linalg.det(R)应该非常接近 1如果差太多说明中间某个环节的数据约定错了。6.3 万向锁导致的欧拉角跳变怎么处理实际调试时我对机械臂做连续轨迹运动发现它的末端姿态有时会突然从(178°, -89°, 179°)跳变到(-2°, -89°, -1°)从旋转矩阵看姿态几乎没变但欧拉角数值大变。这个就是典型的万向锁附近欧拉角不稳定。一种处理方式是统一用四元数做控制指令不发欧拉角如果控制柜只收欧拉角那就在插补路径上对角度做连续化处理比如检测相邻时刻角度差超过 90° 时做加减 360° 的补偿。这个问题的本质是“同一姿态的多种数学表示”不是真正的姿态突变。6.4 性能优化减少三角函数调用次数如果你需要在高频控制循环比如 100Hz~1000Hz里反复做欧拉角/四元数转换三角函数调用会占用不少 CPU。我的经验是在欧拉角转矩阵时先算好sin/cos并缓存在四元数转矩阵时把2*x*x这类乘方和2*x*y这类乘积缓存下来避免重复乘如果可能把角度归一化到[-π, π]再计算避免大角度下精度损失。性能对比上手写的四元数转矩阵大概比 scipy 的通用接口快 30%~50%在一些低配工控机上效果非常明显。7. 扩展场景在 ROS、URDF 和仿真里的实际应用7.1 ROS 里的坐标系发布与 TF 树ROS 中机器人模型和传感器位姿全靠 TF 树来维护坐标系变换关系。tf2里的核心数据结构就是四元数geometry_msgs/Quaternion。你发布一个静态变换时通常用四元数描述旋转from tf2_ros import StaticTransformBroadcaster from geometry_msgs.msg import TransformStamped t TransformStamped() t.header.stamp rospy.Time.now() t.header.frame_id base_link t.child_frame_id camera_link t.transform.translation.x 0.1 t.transform.translation.y 0.0 t.transform.translation.z 0.5 t.transform.rotation.x 0.0 t.transform.rotation.y 0.0 t.transform.rotation.z 0.0 t.transform.rotation.w 1.0 StaticTransformBroadcaster().sendTransform(t)这里的四元数如果是从某个标定工具导出的(x, y, z, w)顺序记得对应关系要一致。否则你在 RViz 里看到相机坐标系方向就完全是歪的。7.2 URDF 建模时坐标系怎么建立搜索词里提到“sw导出urdf坐标系的建立”这块实际很多人会卡住。SolidWorks 导出 URDF 插件会自动为每个 link 创建坐标系但这个坐标系的原点和轴方向是基于你在 SW 里定义的坐标系来的。如果你定义得不规范导出的 URDF 里关节轴方向容易乱。我的建议是在 SolidWorks 里为每个 link 建立基准坐标系时保持所有 link 的坐标轴方向尽量一致比如都让 X 轴指向关节旋转轴方向旋转顺序统一用ZYX。这样导出的 URDF 里各关节的origin会非常干净后续和欧拉角/四元数转换工具配合也少踩坑。7.3 仿真里从“角度”到“矩阵”的自动校验在 Gazebo 或 MoveIt 里做规划时经常要对比不同描述方式下的姿态是否一致。我常用一个简单方法先由欧拉角生成旋转矩阵再由四元数生成旋转矩阵求两者差矩阵的 Frobenius 范数小于1e-6就认为转换正确diff np.linalg.norm(mat_from_euler - mat_from_quat) print(diff:, diff)这个方法非常直观也适合写进自动化测试防止以后改动代码时引入转换 bug。8. 写在后面把这个工具扩展成你自己的惯用库所有核心内容讲完了最后说一点我个人的习惯。我在第一次把这套转换逻辑写进项目后就专门维护了一个robot_math_utils.py里面类化了欧拉角/四元数/矩阵互转、4x4 齐次矩阵合成和拆解、坐标系变换链的批量乘法。每次换项目只要把坐标顺序约定和 SDK 接口的参数格式核对一遍基本不用改核心代码。建议你也照着这个思路做先吃透公式原理再写一个自己的小函数库最后用已知角度做闭环验证。等这套流程跑顺了你会发现“欧拉角/四元数转描述矩阵”这个问题本身不复杂难的是搞清楚你手上的数据到底是哪种约定、要喂给哪一种接口。把约定对齐了剩下的事就是套公式。本文还有配套的精品资源点击获取