做机械臂调试的时候我经常被问到同一个问题我给机械臂发了一个目标位置位置明明是对的为什么末端姿态总差那么一点每次遇到这种情况十有八九不是电机没转到位而是我们对“位姿”这件事的数学表达不够严谨。所谓位姿就是位置加姿态位置告诉你末端在空间哪个点姿态告诉你末端朝着哪个方向、工具是正着拿还是斜着拿。机械臂运动学里所有的计算——正解、逆解、轨迹规划、力控全都要围着这套表达转。这篇文章是这个系列的第七篇我想把从旋转矩阵到齐次变换这条线一次讲透把坐标变换这个机器人学的地基打牢让后面再调试机械臂偏差、写轨迹规划、对接ROS的话题时心里能有个清晰的坐标系。1. 位姿是什么位置和姿态是两个层面的问题1.1 位置用坐标描述姿态需要“一个坐标系相对于另一个坐标系”很多刚接触机械臂的人会把“让末端到某个点”和“让末端以某个姿态到某个点”混在一起。位置问题很简单三维空间里一个点用三个坐标就能描述坐标系一确定位置就确定了。姿态问题要麻烦得多同一个位置末端法兰可以朝上、朝下、斜着45度、旋转90度这些状态的位置完全一样但“位姿”完全不同。你可以把机械臂末端想象成一个人站在门口位置是“他站在门口这一块地砖上”姿态是“他面朝屋里还是面朝屋外是抬头还是低头”。这两个信息必须分开记录最后再合并成一个数学对象。机械臂控制里常说的“末端位姿”本质就是“末端坐标系”相对于“基坐标系”的一个完整描述末端坐标系原点在哪位置末端坐标系的三个轴分别指向哪个方向姿态。在机器人学里表达姿态最核心的工具就是旋转矩阵表达完整位姿最核心的工具是从旋转矩阵扩展出来的齐次变换矩阵。你可以把旋转矩阵理解成“只描述方向的一张名片”而齐次变换矩阵是“把方向和生产地址一起写上去的完整档案”。后面所有运动学计算DH参数、正解、逆解归根结底都在操作这些东西。1.2 为什么姿态不能简单用“三个角度”草草表达有人会问姿态不就是绕X轴转多少、绕Y轴转多少、绕Z轴转多少吗存三个角度不就行了理论上可以实际上一堆坑。角度有顺序问题先绕X再绕Y和先绕Y再绕X得到的方向完全不同角度还存在一个著名的“万向节锁”问题在某些姿态下会丢失一个旋转自由度另外角度在插值和平滑处理时非常别扭直接对三个角度做线性插值末端走过的姿态轨迹会非常诡异。所以业界更愿意用旋转矩阵作为姿态的“底层真相”欧拉角只是给人看的、方便调试的“人机接口”。真正在计算、存储、传递位姿时旋转矩阵或者等价的四元数才是靠谱的载体。这也是为什么很多机械臂SDK和ROS话题里姿态数据要么是四元数要么是9个元素的旋转矩阵很少会直接给你三个欧拉角让你去算。要理解齐次变换必须先吃透旋转矩阵理解旋转矩阵的每一列、每一行到底在说什么。这样才能在拿到一个姿态数据时不用借助任何工具心里大概有数这个旋转大概是什么样的末端轴朝哪个方向工具坐标系有没有装反。1.3 全文统一使用的坐标系约定写这篇文章之前我必须先交代一下坐标系的记法不然下面的公式很容易看晕。我用 $T_{AB}$ 表示“坐标系 {B} 在坐标系 {A} 中的位姿”也就是把一个在 {B} 系里表达的向量变换到 {A} 系里表达需要用到的那个矩阵。换句话说$T_{AB}$ 是“从 B 到 A”的变换。举个例子$T_{01}$ 表示机械臂的第1个关节坐标系在基坐标系 {0} 里的位姿它的旋转部分 $R_{01}$ 的第1列就是 {1} 系的X轴在 {0} 系里的方向余弦第2列是 {1} 系的Y轴在 {0} 系里的方向余弦第3列同理是Z轴。这个约定一定要记牢因为后面讲链式变换时下标匹配不对左乘右乘全是错的。2. 旋转矩阵用9个数表达3个自由度2.1 旋转矩阵的几何含义三个轴在另一个坐标系里的投影旋转矩阵为什么是3×3因为它存放的是两个坐标系之间的方向关系。假设有两个坐标系 {A} 和 {B}把 {B} 系的三个单位坐标轴 $X_B, Y_B, Z_B$ 分别投影到 {A} 系的三个轴上得到的9个方向余弦按列排列就是旋转矩阵 $R_{AB}$。理解这个几何含义比死记公式重要得多。我调试机械臂时经常用这个直觉来检查旋转矩阵对不对拿到一个 $R$先看第3列也就是 {B} 系的Z轴在 {A} 系里的方向这直接对应机械臂末端法兰的朝向。很多手眼标定的结果对不对看一眼第3列的数值是否符合预期就能判断个大概。旋转矩阵还有一个重要性质它是正交矩阵满足 $R^T R I$而且行列式等于1。这意味着它的逆矩阵就是它的转置。为什么因为旋转不改变向量的长度和夹角把 {B} 系的轴转回 {A} 系的轴只需要把方向余弦矩阵“反过来”看也就是行列互换。这个性质会在后面求逆变换时省下大量计算。2.2 绕单轴旋转的基本矩阵与符号陷阱最基础的旋转矩阵是绕坐标轴的单轴旋转。绕Z轴旋转角度 $\theta$矩阵为$$ R_z(\theta) \begin{bmatrix} \cos\theta -\sin\theta 0 \ \sin\theta \cos\theta 0 \ 0 0 1 \end{bmatrix} $$绕X轴旋转$$ R_x(\theta) \begin{bmatrix} 1 0 0 \ 0 \cos\theta -\sin\theta \ 0 \sin\theta \cos\theta \end{bmatrix} $$绕Y轴旋转$$ R_y(\theta) \begin{bmatrix} \cos\theta 0 \sin\theta \ 0 1 0 \ -\sin\theta 0 \cos\theta \end{bmatrix} $$请注意绕Y轴的矩阵负号位置跟X轴、Z轴不一样。$R_x$ 和 $R_z$ 的负号在右上角而 $R_y$ 的负号在左下角。这个差异是右手定则决定的按右手定则绕Y轴正方向旋转时Z轴的正方向转向X轴的正方向跟绕X、绕Z的旋向不太一样。我见过太多人在这一步上栽跟头写出来的矩阵行列式变成-1那就是把右手系写成了左手系机械臂末端轨迹会整个反掉。2.3 欧拉角与旋转矩阵一张公式表说清ZYX顺序实际调试机械臂时几乎没人直接输入9个矩阵元素大家习惯用欧拉角。机械臂领域最常见的约定是“ZYX欧拉角”也叫偏航-俯仰-横滚yaw-pitch-roll。按这个顺序先绕Z轴转偏航角 $\psi$再绕新的Y轴转俯仰角 $\theta$最后绕新的X轴转横滚角 $\phi$合成的旋转矩阵为$$ R R_z(\psi) R_y(\theta) R_x(\phi) $$展开后就是那张网上流传很广的公式表$$ R \begin{bmatrix} c_\psi c_\theta c_\psi s_\theta s_\phi - s_\psi c_\phi c_\psi s_\theta c_\phi s_\psi s_\phi \ s_\psi c_\theta s_\psi s_\theta s_\phi c_\psi c_\phi s_\psi s_\theta c_\phi - c_\psi s_\phi \ -s_\theta c_\theta s_\phi c_\theta c_\phi \end{bmatrix} $$这里面每个元素都代表一个坐标轴方向。第3行第1列是 $-\sin\theta$意味着当俯仰角为90度时横滚和偏航会混在一起——这就是万向节锁的数学来源。你如果想让末端姿态在空间中平滑过渡直接用这三个角插值很容易在接近这个位置时出现姿态突变。我建议你直接把这张公式表存下来但更要理解它的来源矩阵乘法的顺序是从右往左读的先绕X轴转再绕Y轴转最后绕Z轴转。换一个顺序比如“XYZ欧拉角”公式完全不同。所以在和不同品牌的机械臂打交道时第一件事就是查它官方文档里欧拉角的定义顺序。UR、JAKA、睿尔曼这些品牌的SDK里几乎都提供“欧拉角转旋转矩阵”的接口但不保证顺序一致直接套公式会出大事。2.4 为什么旋转矩阵的三列之间必须互相垂直旋转矩阵每一列都是另一个坐标系的单位轴在当前坐标系中的投影单位轴之间天然两两垂直长度为1。这带来两个直接后果第一旋转矩阵的9个元素并不独立存在6个约束条件所以实际自由度为3第二当你手算或从算法里得到一个“疑似旋转矩阵”时可以立刻验证它是否合法。我在自研机械臂控制代码时会习惯性地做一个断言检查计算 $R^T R$看结果是否接近单位阵再算一下行列式看是否接近1。只要有一个不满足就说明数据源有问题可能是标定错了也可能是传感器噪声太大。尤其是从IMU姿态解算里拿到的旋转矩阵如果没做正交化修正时间一长就会漂移这也是有人反馈“基于IMU的位姿解算 yaw 仍会慢漂”的原因之一。旋转矩阵本身的数学性质不会漂漂的是没有持续修正它的实现。3. 齐次变换把旋转和平移塞进一个矩阵3.1 旋转和平移不能直接相加的真正原因当我们有了旋转矩阵 $R$ 和平移向量 $t$一个点从 {B} 系变换到 {A} 系的公式是$$ p_A R p_B t $$这个公式很简洁但在复合变换时不好用。你想把多个坐标系的变换连起来比如从基座到关节1从关节1到关节2从关节2到末端每一级都是“旋转加平移”连起来就得反复展开括号写出来的公式又长又容易错。根本原因是旋转是线性变换平移是仿射变换两者不是一个类型没法直接写成一次矩阵乘法。这就像你要把“转个方向”和“走两步”合并成一个指令绕来绕去总得多带一个附加项。解决办法就是升维把三维向量补一维变成四维把旋转和平移统一放进一个4×4矩阵里。3.2 4×4矩阵的结构与几何意义齐次变换矩阵的标准形式是$$ T \begin{bmatrix} R t \ 0_{1\times3} 1 \end{bmatrix} \begin{bmatrix} r_{11} r_{12} r_{13} t_x \ r_{21} r_{22} r_{23} t_y \ r_{31} r_{32} r_{33} t_z \ 0 0 0 1 \end{bmatrix} $$左上角3×3是旋转矩阵右上角3×1是平移向量左下角是三个0右下角是1。这个1就是“齐次坐标”的由来。对应的点变换公式变成$$ \begin{bmatrix} p_A \ 1 \end{bmatrix} T_{AB} \begin{bmatrix} p_B \ 1 \end{bmatrix} $$这下旋转和平移就统一成矩阵乘法了。复合变换也变得非常优雅如果我知道 {B} 在 {A} 里的位姿 $T_{AB}$又知道 {C} 在 {B} 里的位姿 $T_{BC}$那么 {C} 在 {A} 里的位姿直接是$$ T_{AC} T_{AB} T_{BC} $$这就是机械臂运动学正解的发动机。每两个相邻连杆之间的相对位姿都能写成这样一个齐次变换矩阵然后从基座开始一路右乘到末端得到的就是末端在基座坐标系下的完整位姿。市面上各种机械臂库比如Python的现代机器人学库、ROS里的tf库底层全是这个套路。3.3 逆变换怎么求别再傻傻去求4×4的逆矩阵有了 $T_{AB}$怎么求 $T_{BA}$也就是反向变换。最笨的办法是直接对4×4矩阵求逆数值上容易出精度问题而且没必要。利用旋转矩阵的正交性可以一步写出$$ T_{BA} T_{AB}^{-1} \begin{bmatrix} R^T -R^T t \ 0 1 \end{bmatrix} $$这个公式非常重要。实际场景里比如手眼标定已知相机相对于机械臂末端的位姿想求机械臂末端相对于相机的位姿就是一次这样的逆变换。很多新手在这里犯的错误是只把左上角旋转矩阵转置了忘了平移向量要经过 $ -R^T t $ 变换结果算出来的位置完全对不上。平移量也是相对于旋转之后的坐标系的直接取负号是不对的。我写代码时从来不直接np.linalg.inv(T)而是专门写一个inv_tf(T)函数用上面这个公式算。速度快、数值稳而且代码语义清晰别人一看就知道你在做坐标变换的逆运算。4. 链式变换机械臂运动学正解的发动机4.1 相邻连杆的位姿传递T_01、T_12、T_23是怎么来的机械臂本质上是一连串用关节连接起来的刚体。给每个连杆都固定一个坐标系基座是 {0} 系第1个连杆是 {1} 系第2个连杆是 {2} 系依此类推。每个关节的角度决定了相邻两个坐标系之间的相对位姿。用齐次变换矩阵写出来就是一系列 $T_{01}$、$T_{12}$、$T_{23}$……这些矩阵本身不复杂但确定它们的过程有一套标准方法也就是DH参数法。DH参数给每个连杆定义4个参数连杆长度、连杆转角、连杆偏距、关节角。前三个是机械结构固定的只有关节角随电机转动变化。把这些参数代进DH变换公式就能得到相邻坐标系的齐次变换矩阵。这套方法的好处是标准化不管什么构型的机械臂——UR那种六轴协作臂、Piper那种带轮子的复合机器人、自己3D打印的5自由度小臂——只要把DH参数表整理出来正解代码几乎可以通吃。我遇到过很多人问“为什么我照着DH表算出来的末端位姿和示教器上显示的不一样”十有八九是DH建系规则没统一。有的教材把X轴方向定为沿连杆方向有的定为垂直连杆方向有的参考坐标系建在前一个关节有的建在后一个关节。这些差异会直接影响DH参数的正负号和尺寸最终导致一个看似一模一样、实际差了一个坐标系变换的矩阵。所以拿到一台机械臂的DH表时别只抄参数一定要连它建系的规则一起抄。4.2 左乘和右乘这是最容易翻车的地方链式变换的规则是 $T_{AC} T_{AB} T_{BC}$按“从基座往末端”的顺序从右往左一路乘过去。但很多实际操作不是简单的链式而是要在当前位姿上“叠”一个变换这时候就要分清左乘还是右乘。我总结成一句话相对固定坐标系变就左乘相对自身坐标系变就右乘。举个例子机械臂末端当前位姿是 $T$相对世界系。如果想让末端沿世界坐标系的Z轴下降10厘米就左乘一个平移矩阵$T \text{Trans}(0,0,-0.1) \times T$。如果想让末端沿“自己的”Z轴前进10厘米也就是刀具前进方向就右乘$T T \times \text{Trans}(0,0,0.1)$。这个区别在轨迹规划里天天用。沿着世界系走的轨迹和沿着工具系走的轨迹写出来的代码就一个左乘右乘之差但末端走的路径完全不一样。如果你在调试时发现机械臂的末端位置跟预期差得越来越多先别怀疑电机丢步回来检查一下这一步的乘法和顺序。4.3 一个二连杆机械臂的完整计算例子纸上得来终觉浅我拿一个平面二连杆机械臂演示一下。假设两个关节都在同一平面内旋转连杆1长度 $L_1$连杆2长度 $L_2$关节角分别为 $\theta_1$ 和 $\theta_2$。很多教材会直接给末端位置公式我换一种方式用齐次变换推一遍。{0} 系在基座{1} 系在关节1{2} 系在关节2{3} 系在末端。关节1到关节2的变换本质是绕 {1} 系的Z轴转 $\theta_2$再沿 {1} 系的X轴平移 $L_1$所以$$ T_{01} \begin{bmatrix} \cos\theta_1 -\sin\theta_1 0 0 \ \sin\theta_1 \cos\theta_1 0 0 \ 0 0 1 0 \ 0 0 0 1 \end{bmatrix} $$同理$$ T_{12} \begin{bmatrix} \cos\theta_2 -\sin\theta_2 0 L_1 \ \sin\theta_2 \cos\theta_2 0 0 \ 0 0 1 0 \ 0 0 0 1 \end{bmatrix} $$末端坐标系 {3} 相对 {2} 只在X方向偏移 $L_2$$$ T_{23} \begin{bmatrix} 1 0 0 L_2 \ 0 1 0 0 \ 0 0 1 0 \ 0 0 0 1 \end{bmatrix} $$末端位姿直接链式相乘$$ T_{03} T_{01} T_{12} T_{23} $$展开后左上角的旋转矩阵是 $\cos(\theta_1\theta_2)$ 那一套右上角的平移部分就是经典的二连杆正解公式。用这个例子你可以很直观地看到齐次变换把“旋转累加”和“位置累加”自动处理好了你不用手动去展开三角函数的和差公式。5. 用代码把数学跑起来验证位姿别靠猜5.1 Pythonnumpy scipy 快速搭一套坐标变换工具我平时最快验证位姿猜想的方式是在Python里用numpy加scipy搭一套临时工具箱。numpy负责矩阵运算scipy负责欧拉角和四元数转换两者配合几天就能把一套坐标变换逻辑验证完。下面这段代码可以直接跑import numpy as np from scipy.spatial.transform import Rotation as R def rot_from_euler_zyx(yaw, pitch, roll): # 注意小写 zyx 表示绕固定坐标系的 Z、Y、X 轴依次旋转 r R.from_euler(zyx, [yaw, pitch, roll], degreesTrue) return r.as_matrix() def rot_to_euler_zyx(mat): r R.from_matrix(mat) # degreesTrue 让结果以角度输出方便人看 return r.as_euler(zyx, degreesTrue) def tf_inv(T): R_mat T[:3, :3] t T[:3, 3] T_inv np.eye(4) T_inv[:3, :3] R_mat.T T_inv[:3, 3] -R_mat.T t return T_inv # 示例绕Z轴转30度然后平移(1,2,3) yaw, pitch, roll 30, 0, 0 R_mat rot_from_euler_zyx(yaw, pitch, roll) T np.eye(4) T[:3, :3] R_mat T[:3, 3] [1, 2, 3] T_inv tf_inv(T) print(验证 T_inv T 是否接近单位阵:) print(T_inv T)这里有个坑值得提醒scipy的from_euler里小写字母zyx表示绕固定坐标系的Z、Y、X轴依次旋转大写ZYX表示绕自身坐标系的轴旋转。很多人没仔细看文档大小写混用结果同样的输入得到完全不同的旋转矩阵。这个坑我在给SolidWorks导出的模型做坐标对齐时踩过后来所有代码里都统一用一个小写字母的封装函数避免每次写错。5.2 从URSim/ROS话题里拿位姿时容易踩的坑在仿真环境里调机械臂比如URSim虚拟机、Gazebo加UR5e很多人会遇到一个问题宿主机上拿到的虚拟机器人位姿跟仿真界面里显示的对不上。这时候要检查的往往不是位姿数据本身而是坐标变换关系。ROS里的位姿话题通常给的是位置加四元数比如geometry_msgs/Pose里的position和orientation。四元数本身也是旋转矩阵的另一种表达只是更紧凑。拿到四元数后要先转成旋转矩阵再组装成齐次变换矩阵。这个环节常见错误有两个一是把四元数的实部虚部顺序搞反了ROS里四元数顺序是 (x, y, z, w)很多数学库里是 (w, x, y, z)不统一就全乱套二是话题里的坐标值到底是在哪个坐标系下表达的有的人拿到base_link位姿就直接当成世界坐标用没注意还有一个map到odom的变换。我个人习惯是凡是涉及跨话题的位姿数据一律先打印出它的坐标帧名称再在tf树里查一遍父子关系然后才进计算。这一步看着繁琐实际上能省掉后面排查“机械臂偏差”的大部分时间。5.3 自己造一个小工具齐次变换的复合与可视化在做一个稍微复杂的项目时我会建议你在项目里放一个tf_utils.py里面包含几个固定函数矩阵转欧拉角、欧拉角转矩阵、四元数转矩阵、求逆变换、链式复合。代码量不大但能保证全项目统一约定。每个函数旁边写清楚使用的欧拉角顺序和坐标系规则这样后面不管谁来维护都不会因为“我以为是XYZ顺序”而改出一堆bug。我见过不少团队把“绕固定轴左乘、绕自身轴右乘”这两条规则写进代码注释里但还是会在具体函数里用错。我的做法是给封装的函数命名时直接体现语义比如transform_by_fixed_frame(T, delta)和transform_by_local_frame(T, delta)让调用的人根本不需要想左乘还是右乘。这比任何注释都有效。6. 常见位姿翻车现场与排查清单6.1 欧拉角顺序约定混乱不同品牌机械臂、不同软件库的欧拉角顺序不统一。UR机器人示教器默认是“绕固定坐标系的Z、Y、X”有些国产机械臂SDK用的是“绕自身坐标系的Z、Y、X”还有一些视觉库默认是“X、Y、Z”。同一个 (30, 45, 60)在A平台和B平台解算出的旋转矩阵可能完全不同。排查方法是拿一个已知姿态做基准测试。把机械臂末端手动转到某个容易识别的姿态比如让工具轴垂直向下然后读取示教器和SDK返回的欧拉角反推它的顺序定义。不要想当然一定要实测。6.2 万向节锁角度表达在某些姿态下会“卡死”当俯仰角接近正负90度时偏航和横滚变得不可区分欧拉角表达会退化。具体表现是你连续改变末端姿态示教器上的某个角度却突然跳变或者你给的插值轨迹在某个点附近出现异常旋转。这不是机械臂坏了是欧拉角的数学缺陷。工程上的解决办法是计算和通信都用旋转矩阵或四元数只在显示和人工输入时转成欧拉角。千万别拿欧拉角做连续轨迹插值。如果你发现某个商用SDK只让你传欧拉角那你最好自己内部维护一套旋转矩阵每次要调用SDK时再转成它的约定。6.3 把旋转矩阵的逆写成转置把齐次变换的逆也写成转置旋转矩阵的逆等于转置这个性质很多人记住了但用错了地方。对齐次变换矩阵只转置不处理平移向量得到的结果完全不对。正确公式是$$ T^{-1} \begin{bmatrix} R^T -R^T t \ 0 1 \end{bmatrix} $$我建议你在自己的代码库里统一封装tf_inv并加一个测试用例随机生成一个旋转矩阵和平移向量验证tf_inv(T) T是否等于单位阵。这样以后再用就再也不会踩这个坑。6.4 弧度、角度混用三角函数库的参数默认是弧度但很多机械臂SDK的接口参数是角度。UR示教器上显示的是角度SDK里返回的关节角默认也是角度但如果你把角度直接传给math.cos算出来的结果会非常离谱。这个错误看起来很低级但在工程项目里极其常见尤其是多个语言混用时。我的经验是所有内部计算统一用弧度所有对外显示和人工输入统一用角度在系统边界做好转换。这样哪怕出了问题也只要检查边界。6.5 从仿真/传感器拿到的位姿对不上仿真环境里位姿对不上先查三件事第一坐标系帧名是否正确是不是拿错了参考系第二四元数转旋转矩阵时顺序是否正确第三数据的时间戳是否对齐机械臂动得快的时候一个周期的时间差也会造成明显位姿偏差。如果是IMU融合出来的位姿还要注意yaw方向通常是可观测性最差的。旋转矩阵的Roll和Pitch可以通过重力方向校正Yaw只能靠磁力计或其他外部参考漂移快是常态。你看到yaw慢慢漂不是旋转矩阵数学错了是传感器融合策略的局限。这种场景下如果机械臂末端还带有视觉或其他绝对位姿传感器可以让它定期修正yaw比单靠IMU稳得多。7. 一个我每次都会做的检查动作这篇文章写到这里该讲的数学和实操都讲完了最后分享一个我自己的习惯。每当我拿到一台新机械臂或者要写一段跟位姿相关的代码时第一件事不是写代码而是在纸上画出三个坐标系基座坐标系、关键关节坐标系、末端工具坐标系然后手动写出它们之间的坐标变换链。这一步看着土但真能救大命。画完之后我会在代码里打一行日志把变换链上的每一个 $T$ 打出来尤其是旋转矩阵的第三列也就是Z轴方向。机械臂末端工具的Z轴朝哪儿一般跟抓取、放置、视觉手眼标定高度相关看一眼数值就能发现坐标系是不是建反了、欧拉角顺序是不是有问题。这个习惯让我少熬了很多个排查的夜。这套从旋转矩阵到齐次变换的数学工具是机械臂运动学里绕不开的底层能力。往深了走后面还涉及DH参数、四元数、逆解、轨迹规划全都在这个地基上盖楼。如果你有兴趣我可以接着写这个系列里关于DH参数建模和正解实现的实操那个话题实操性更强代码也会更多。
机械臂位姿数学基础：从旋转矩阵到齐次变换一次讲透
做机械臂调试的时候我经常被问到同一个问题我给机械臂发了一个目标位置位置明明是对的为什么末端姿态总差那么一点每次遇到这种情况十有八九不是电机没转到位而是我们对“位姿”这件事的数学表达不够严谨。所谓位姿就是位置加姿态位置告诉你末端在空间哪个点姿态告诉你末端朝着哪个方向、工具是正着拿还是斜着拿。机械臂运动学里所有的计算——正解、逆解、轨迹规划、力控全都要围着这套表达转。这篇文章是这个系列的第七篇我想把从旋转矩阵到齐次变换这条线一次讲透把坐标变换这个机器人学的地基打牢让后面再调试机械臂偏差、写轨迹规划、对接ROS的话题时心里能有个清晰的坐标系。1. 位姿是什么位置和姿态是两个层面的问题1.1 位置用坐标描述姿态需要“一个坐标系相对于另一个坐标系”很多刚接触机械臂的人会把“让末端到某个点”和“让末端以某个姿态到某个点”混在一起。位置问题很简单三维空间里一个点用三个坐标就能描述坐标系一确定位置就确定了。姿态问题要麻烦得多同一个位置末端法兰可以朝上、朝下、斜着45度、旋转90度这些状态的位置完全一样但“位姿”完全不同。你可以把机械臂末端想象成一个人站在门口位置是“他站在门口这一块地砖上”姿态是“他面朝屋里还是面朝屋外是抬头还是低头”。这两个信息必须分开记录最后再合并成一个数学对象。机械臂控制里常说的“末端位姿”本质就是“末端坐标系”相对于“基坐标系”的一个完整描述末端坐标系原点在哪位置末端坐标系的三个轴分别指向哪个方向姿态。在机器人学里表达姿态最核心的工具就是旋转矩阵表达完整位姿最核心的工具是从旋转矩阵扩展出来的齐次变换矩阵。你可以把旋转矩阵理解成“只描述方向的一张名片”而齐次变换矩阵是“把方向和生产地址一起写上去的完整档案”。后面所有运动学计算DH参数、正解、逆解归根结底都在操作这些东西。1.2 为什么姿态不能简单用“三个角度”草草表达有人会问姿态不就是绕X轴转多少、绕Y轴转多少、绕Z轴转多少吗存三个角度不就行了理论上可以实际上一堆坑。角度有顺序问题先绕X再绕Y和先绕Y再绕X得到的方向完全不同角度还存在一个著名的“万向节锁”问题在某些姿态下会丢失一个旋转自由度另外角度在插值和平滑处理时非常别扭直接对三个角度做线性插值末端走过的姿态轨迹会非常诡异。所以业界更愿意用旋转矩阵作为姿态的“底层真相”欧拉角只是给人看的、方便调试的“人机接口”。真正在计算、存储、传递位姿时旋转矩阵或者等价的四元数才是靠谱的载体。这也是为什么很多机械臂SDK和ROS话题里姿态数据要么是四元数要么是9个元素的旋转矩阵很少会直接给你三个欧拉角让你去算。要理解齐次变换必须先吃透旋转矩阵理解旋转矩阵的每一列、每一行到底在说什么。这样才能在拿到一个姿态数据时不用借助任何工具心里大概有数这个旋转大概是什么样的末端轴朝哪个方向工具坐标系有没有装反。1.3 全文统一使用的坐标系约定写这篇文章之前我必须先交代一下坐标系的记法不然下面的公式很容易看晕。我用 $T_{AB}$ 表示“坐标系 {B} 在坐标系 {A} 中的位姿”也就是把一个在 {B} 系里表达的向量变换到 {A} 系里表达需要用到的那个矩阵。换句话说$T_{AB}$ 是“从 B 到 A”的变换。举个例子$T_{01}$ 表示机械臂的第1个关节坐标系在基坐标系 {0} 里的位姿它的旋转部分 $R_{01}$ 的第1列就是 {1} 系的X轴在 {0} 系里的方向余弦第2列是 {1} 系的Y轴在 {0} 系里的方向余弦第3列同理是Z轴。这个约定一定要记牢因为后面讲链式变换时下标匹配不对左乘右乘全是错的。2. 旋转矩阵用9个数表达3个自由度2.1 旋转矩阵的几何含义三个轴在另一个坐标系里的投影旋转矩阵为什么是3×3因为它存放的是两个坐标系之间的方向关系。假设有两个坐标系 {A} 和 {B}把 {B} 系的三个单位坐标轴 $X_B, Y_B, Z_B$ 分别投影到 {A} 系的三个轴上得到的9个方向余弦按列排列就是旋转矩阵 $R_{AB}$。理解这个几何含义比死记公式重要得多。我调试机械臂时经常用这个直觉来检查旋转矩阵对不对拿到一个 $R$先看第3列也就是 {B} 系的Z轴在 {A} 系里的方向这直接对应机械臂末端法兰的朝向。很多手眼标定的结果对不对看一眼第3列的数值是否符合预期就能判断个大概。旋转矩阵还有一个重要性质它是正交矩阵满足 $R^T R I$而且行列式等于1。这意味着它的逆矩阵就是它的转置。为什么因为旋转不改变向量的长度和夹角把 {B} 系的轴转回 {A} 系的轴只需要把方向余弦矩阵“反过来”看也就是行列互换。这个性质会在后面求逆变换时省下大量计算。2.2 绕单轴旋转的基本矩阵与符号陷阱最基础的旋转矩阵是绕坐标轴的单轴旋转。绕Z轴旋转角度 $\theta$矩阵为$$ R_z(\theta) \begin{bmatrix} \cos\theta -\sin\theta 0 \ \sin\theta \cos\theta 0 \ 0 0 1 \end{bmatrix} $$绕X轴旋转$$ R_x(\theta) \begin{bmatrix} 1 0 0 \ 0 \cos\theta -\sin\theta \ 0 \sin\theta \cos\theta \end{bmatrix} $$绕Y轴旋转$$ R_y(\theta) \begin{bmatrix} \cos\theta 0 \sin\theta \ 0 1 0 \ -\sin\theta 0 \cos\theta \end{bmatrix} $$请注意绕Y轴的矩阵负号位置跟X轴、Z轴不一样。$R_x$ 和 $R_z$ 的负号在右上角而 $R_y$ 的负号在左下角。这个差异是右手定则决定的按右手定则绕Y轴正方向旋转时Z轴的正方向转向X轴的正方向跟绕X、绕Z的旋向不太一样。我见过太多人在这一步上栽跟头写出来的矩阵行列式变成-1那就是把右手系写成了左手系机械臂末端轨迹会整个反掉。2.3 欧拉角与旋转矩阵一张公式表说清ZYX顺序实际调试机械臂时几乎没人直接输入9个矩阵元素大家习惯用欧拉角。机械臂领域最常见的约定是“ZYX欧拉角”也叫偏航-俯仰-横滚yaw-pitch-roll。按这个顺序先绕Z轴转偏航角 $\psi$再绕新的Y轴转俯仰角 $\theta$最后绕新的X轴转横滚角 $\phi$合成的旋转矩阵为$$ R R_z(\psi) R_y(\theta) R_x(\phi) $$展开后就是那张网上流传很广的公式表$$ R \begin{bmatrix} c_\psi c_\theta c_\psi s_\theta s_\phi - s_\psi c_\phi c_\psi s_\theta c_\phi s_\psi s_\phi \ s_\psi c_\theta s_\psi s_\theta s_\phi c_\psi c_\phi s_\psi s_\theta c_\phi - c_\psi s_\phi \ -s_\theta c_\theta s_\phi c_\theta c_\phi \end{bmatrix} $$这里面每个元素都代表一个坐标轴方向。第3行第1列是 $-\sin\theta$意味着当俯仰角为90度时横滚和偏航会混在一起——这就是万向节锁的数学来源。你如果想让末端姿态在空间中平滑过渡直接用这三个角插值很容易在接近这个位置时出现姿态突变。我建议你直接把这张公式表存下来但更要理解它的来源矩阵乘法的顺序是从右往左读的先绕X轴转再绕Y轴转最后绕Z轴转。换一个顺序比如“XYZ欧拉角”公式完全不同。所以在和不同品牌的机械臂打交道时第一件事就是查它官方文档里欧拉角的定义顺序。UR、JAKA、睿尔曼这些品牌的SDK里几乎都提供“欧拉角转旋转矩阵”的接口但不保证顺序一致直接套公式会出大事。2.4 为什么旋转矩阵的三列之间必须互相垂直旋转矩阵每一列都是另一个坐标系的单位轴在当前坐标系中的投影单位轴之间天然两两垂直长度为1。这带来两个直接后果第一旋转矩阵的9个元素并不独立存在6个约束条件所以实际自由度为3第二当你手算或从算法里得到一个“疑似旋转矩阵”时可以立刻验证它是否合法。我在自研机械臂控制代码时会习惯性地做一个断言检查计算 $R^T R$看结果是否接近单位阵再算一下行列式看是否接近1。只要有一个不满足就说明数据源有问题可能是标定错了也可能是传感器噪声太大。尤其是从IMU姿态解算里拿到的旋转矩阵如果没做正交化修正时间一长就会漂移这也是有人反馈“基于IMU的位姿解算 yaw 仍会慢漂”的原因之一。旋转矩阵本身的数学性质不会漂漂的是没有持续修正它的实现。3. 齐次变换把旋转和平移塞进一个矩阵3.1 旋转和平移不能直接相加的真正原因当我们有了旋转矩阵 $R$ 和平移向量 $t$一个点从 {B} 系变换到 {A} 系的公式是$$ p_A R p_B t $$这个公式很简洁但在复合变换时不好用。你想把多个坐标系的变换连起来比如从基座到关节1从关节1到关节2从关节2到末端每一级都是“旋转加平移”连起来就得反复展开括号写出来的公式又长又容易错。根本原因是旋转是线性变换平移是仿射变换两者不是一个类型没法直接写成一次矩阵乘法。这就像你要把“转个方向”和“走两步”合并成一个指令绕来绕去总得多带一个附加项。解决办法就是升维把三维向量补一维变成四维把旋转和平移统一放进一个4×4矩阵里。3.2 4×4矩阵的结构与几何意义齐次变换矩阵的标准形式是$$ T \begin{bmatrix} R t \ 0_{1\times3} 1 \end{bmatrix} \begin{bmatrix} r_{11} r_{12} r_{13} t_x \ r_{21} r_{22} r_{23} t_y \ r_{31} r_{32} r_{33} t_z \ 0 0 0 1 \end{bmatrix} $$左上角3×3是旋转矩阵右上角3×1是平移向量左下角是三个0右下角是1。这个1就是“齐次坐标”的由来。对应的点变换公式变成$$ \begin{bmatrix} p_A \ 1 \end{bmatrix} T_{AB} \begin{bmatrix} p_B \ 1 \end{bmatrix} $$这下旋转和平移就统一成矩阵乘法了。复合变换也变得非常优雅如果我知道 {B} 在 {A} 里的位姿 $T_{AB}$又知道 {C} 在 {B} 里的位姿 $T_{BC}$那么 {C} 在 {A} 里的位姿直接是$$ T_{AC} T_{AB} T_{BC} $$这就是机械臂运动学正解的发动机。每两个相邻连杆之间的相对位姿都能写成这样一个齐次变换矩阵然后从基座开始一路右乘到末端得到的就是末端在基座坐标系下的完整位姿。市面上各种机械臂库比如Python的现代机器人学库、ROS里的tf库底层全是这个套路。3.3 逆变换怎么求别再傻傻去求4×4的逆矩阵有了 $T_{AB}$怎么求 $T_{BA}$也就是反向变换。最笨的办法是直接对4×4矩阵求逆数值上容易出精度问题而且没必要。利用旋转矩阵的正交性可以一步写出$$ T_{BA} T_{AB}^{-1} \begin{bmatrix} R^T -R^T t \ 0 1 \end{bmatrix} $$这个公式非常重要。实际场景里比如手眼标定已知相机相对于机械臂末端的位姿想求机械臂末端相对于相机的位姿就是一次这样的逆变换。很多新手在这里犯的错误是只把左上角旋转矩阵转置了忘了平移向量要经过 $ -R^T t $ 变换结果算出来的位置完全对不上。平移量也是相对于旋转之后的坐标系的直接取负号是不对的。我写代码时从来不直接np.linalg.inv(T)而是专门写一个inv_tf(T)函数用上面这个公式算。速度快、数值稳而且代码语义清晰别人一看就知道你在做坐标变换的逆运算。4. 链式变换机械臂运动学正解的发动机4.1 相邻连杆的位姿传递T_01、T_12、T_23是怎么来的机械臂本质上是一连串用关节连接起来的刚体。给每个连杆都固定一个坐标系基座是 {0} 系第1个连杆是 {1} 系第2个连杆是 {2} 系依此类推。每个关节的角度决定了相邻两个坐标系之间的相对位姿。用齐次变换矩阵写出来就是一系列 $T_{01}$、$T_{12}$、$T_{23}$……这些矩阵本身不复杂但确定它们的过程有一套标准方法也就是DH参数法。DH参数给每个连杆定义4个参数连杆长度、连杆转角、连杆偏距、关节角。前三个是机械结构固定的只有关节角随电机转动变化。把这些参数代进DH变换公式就能得到相邻坐标系的齐次变换矩阵。这套方法的好处是标准化不管什么构型的机械臂——UR那种六轴协作臂、Piper那种带轮子的复合机器人、自己3D打印的5自由度小臂——只要把DH参数表整理出来正解代码几乎可以通吃。我遇到过很多人问“为什么我照着DH表算出来的末端位姿和示教器上显示的不一样”十有八九是DH建系规则没统一。有的教材把X轴方向定为沿连杆方向有的定为垂直连杆方向有的参考坐标系建在前一个关节有的建在后一个关节。这些差异会直接影响DH参数的正负号和尺寸最终导致一个看似一模一样、实际差了一个坐标系变换的矩阵。所以拿到一台机械臂的DH表时别只抄参数一定要连它建系的规则一起抄。4.2 左乘和右乘这是最容易翻车的地方链式变换的规则是 $T_{AC} T_{AB} T_{BC}$按“从基座往末端”的顺序从右往左一路乘过去。但很多实际操作不是简单的链式而是要在当前位姿上“叠”一个变换这时候就要分清左乘还是右乘。我总结成一句话相对固定坐标系变就左乘相对自身坐标系变就右乘。举个例子机械臂末端当前位姿是 $T$相对世界系。如果想让末端沿世界坐标系的Z轴下降10厘米就左乘一个平移矩阵$T \text{Trans}(0,0,-0.1) \times T$。如果想让末端沿“自己的”Z轴前进10厘米也就是刀具前进方向就右乘$T T \times \text{Trans}(0,0,0.1)$。这个区别在轨迹规划里天天用。沿着世界系走的轨迹和沿着工具系走的轨迹写出来的代码就一个左乘右乘之差但末端走的路径完全不一样。如果你在调试时发现机械臂的末端位置跟预期差得越来越多先别怀疑电机丢步回来检查一下这一步的乘法和顺序。4.3 一个二连杆机械臂的完整计算例子纸上得来终觉浅我拿一个平面二连杆机械臂演示一下。假设两个关节都在同一平面内旋转连杆1长度 $L_1$连杆2长度 $L_2$关节角分别为 $\theta_1$ 和 $\theta_2$。很多教材会直接给末端位置公式我换一种方式用齐次变换推一遍。{0} 系在基座{1} 系在关节1{2} 系在关节2{3} 系在末端。关节1到关节2的变换本质是绕 {1} 系的Z轴转 $\theta_2$再沿 {1} 系的X轴平移 $L_1$所以$$ T_{01} \begin{bmatrix} \cos\theta_1 -\sin\theta_1 0 0 \ \sin\theta_1 \cos\theta_1 0 0 \ 0 0 1 0 \ 0 0 0 1 \end{bmatrix} $$同理$$ T_{12} \begin{bmatrix} \cos\theta_2 -\sin\theta_2 0 L_1 \ \sin\theta_2 \cos\theta_2 0 0 \ 0 0 1 0 \ 0 0 0 1 \end{bmatrix} $$末端坐标系 {3} 相对 {2} 只在X方向偏移 $L_2$$$ T_{23} \begin{bmatrix} 1 0 0 L_2 \ 0 1 0 0 \ 0 0 1 0 \ 0 0 0 1 \end{bmatrix} $$末端位姿直接链式相乘$$ T_{03} T_{01} T_{12} T_{23} $$展开后左上角的旋转矩阵是 $\cos(\theta_1\theta_2)$ 那一套右上角的平移部分就是经典的二连杆正解公式。用这个例子你可以很直观地看到齐次变换把“旋转累加”和“位置累加”自动处理好了你不用手动去展开三角函数的和差公式。5. 用代码把数学跑起来验证位姿别靠猜5.1 Pythonnumpy scipy 快速搭一套坐标变换工具我平时最快验证位姿猜想的方式是在Python里用numpy加scipy搭一套临时工具箱。numpy负责矩阵运算scipy负责欧拉角和四元数转换两者配合几天就能把一套坐标变换逻辑验证完。下面这段代码可以直接跑import numpy as np from scipy.spatial.transform import Rotation as R def rot_from_euler_zyx(yaw, pitch, roll): # 注意小写 zyx 表示绕固定坐标系的 Z、Y、X 轴依次旋转 r R.from_euler(zyx, [yaw, pitch, roll], degreesTrue) return r.as_matrix() def rot_to_euler_zyx(mat): r R.from_matrix(mat) # degreesTrue 让结果以角度输出方便人看 return r.as_euler(zyx, degreesTrue) def tf_inv(T): R_mat T[:3, :3] t T[:3, 3] T_inv np.eye(4) T_inv[:3, :3] R_mat.T T_inv[:3, 3] -R_mat.T t return T_inv # 示例绕Z轴转30度然后平移(1,2,3) yaw, pitch, roll 30, 0, 0 R_mat rot_from_euler_zyx(yaw, pitch, roll) T np.eye(4) T[:3, :3] R_mat T[:3, 3] [1, 2, 3] T_inv tf_inv(T) print(验证 T_inv T 是否接近单位阵:) print(T_inv T)这里有个坑值得提醒scipy的from_euler里小写字母zyx表示绕固定坐标系的Z、Y、X轴依次旋转大写ZYX表示绕自身坐标系的轴旋转。很多人没仔细看文档大小写混用结果同样的输入得到完全不同的旋转矩阵。这个坑我在给SolidWorks导出的模型做坐标对齐时踩过后来所有代码里都统一用一个小写字母的封装函数避免每次写错。5.2 从URSim/ROS话题里拿位姿时容易踩的坑在仿真环境里调机械臂比如URSim虚拟机、Gazebo加UR5e很多人会遇到一个问题宿主机上拿到的虚拟机器人位姿跟仿真界面里显示的对不上。这时候要检查的往往不是位姿数据本身而是坐标变换关系。ROS里的位姿话题通常给的是位置加四元数比如geometry_msgs/Pose里的position和orientation。四元数本身也是旋转矩阵的另一种表达只是更紧凑。拿到四元数后要先转成旋转矩阵再组装成齐次变换矩阵。这个环节常见错误有两个一是把四元数的实部虚部顺序搞反了ROS里四元数顺序是 (x, y, z, w)很多数学库里是 (w, x, y, z)不统一就全乱套二是话题里的坐标值到底是在哪个坐标系下表达的有的人拿到base_link位姿就直接当成世界坐标用没注意还有一个map到odom的变换。我个人习惯是凡是涉及跨话题的位姿数据一律先打印出它的坐标帧名称再在tf树里查一遍父子关系然后才进计算。这一步看着繁琐实际上能省掉后面排查“机械臂偏差”的大部分时间。5.3 自己造一个小工具齐次变换的复合与可视化在做一个稍微复杂的项目时我会建议你在项目里放一个tf_utils.py里面包含几个固定函数矩阵转欧拉角、欧拉角转矩阵、四元数转矩阵、求逆变换、链式复合。代码量不大但能保证全项目统一约定。每个函数旁边写清楚使用的欧拉角顺序和坐标系规则这样后面不管谁来维护都不会因为“我以为是XYZ顺序”而改出一堆bug。我见过不少团队把“绕固定轴左乘、绕自身轴右乘”这两条规则写进代码注释里但还是会在具体函数里用错。我的做法是给封装的函数命名时直接体现语义比如transform_by_fixed_frame(T, delta)和transform_by_local_frame(T, delta)让调用的人根本不需要想左乘还是右乘。这比任何注释都有效。6. 常见位姿翻车现场与排查清单6.1 欧拉角顺序约定混乱不同品牌机械臂、不同软件库的欧拉角顺序不统一。UR机器人示教器默认是“绕固定坐标系的Z、Y、X”有些国产机械臂SDK用的是“绕自身坐标系的Z、Y、X”还有一些视觉库默认是“X、Y、Z”。同一个 (30, 45, 60)在A平台和B平台解算出的旋转矩阵可能完全不同。排查方法是拿一个已知姿态做基准测试。把机械臂末端手动转到某个容易识别的姿态比如让工具轴垂直向下然后读取示教器和SDK返回的欧拉角反推它的顺序定义。不要想当然一定要实测。6.2 万向节锁角度表达在某些姿态下会“卡死”当俯仰角接近正负90度时偏航和横滚变得不可区分欧拉角表达会退化。具体表现是你连续改变末端姿态示教器上的某个角度却突然跳变或者你给的插值轨迹在某个点附近出现异常旋转。这不是机械臂坏了是欧拉角的数学缺陷。工程上的解决办法是计算和通信都用旋转矩阵或四元数只在显示和人工输入时转成欧拉角。千万别拿欧拉角做连续轨迹插值。如果你发现某个商用SDK只让你传欧拉角那你最好自己内部维护一套旋转矩阵每次要调用SDK时再转成它的约定。6.3 把旋转矩阵的逆写成转置把齐次变换的逆也写成转置旋转矩阵的逆等于转置这个性质很多人记住了但用错了地方。对齐次变换矩阵只转置不处理平移向量得到的结果完全不对。正确公式是$$ T^{-1} \begin{bmatrix} R^T -R^T t \ 0 1 \end{bmatrix} $$我建议你在自己的代码库里统一封装tf_inv并加一个测试用例随机生成一个旋转矩阵和平移向量验证tf_inv(T) T是否等于单位阵。这样以后再用就再也不会踩这个坑。6.4 弧度、角度混用三角函数库的参数默认是弧度但很多机械臂SDK的接口参数是角度。UR示教器上显示的是角度SDK里返回的关节角默认也是角度但如果你把角度直接传给math.cos算出来的结果会非常离谱。这个错误看起来很低级但在工程项目里极其常见尤其是多个语言混用时。我的经验是所有内部计算统一用弧度所有对外显示和人工输入统一用角度在系统边界做好转换。这样哪怕出了问题也只要检查边界。6.5 从仿真/传感器拿到的位姿对不上仿真环境里位姿对不上先查三件事第一坐标系帧名是否正确是不是拿错了参考系第二四元数转旋转矩阵时顺序是否正确第三数据的时间戳是否对齐机械臂动得快的时候一个周期的时间差也会造成明显位姿偏差。如果是IMU融合出来的位姿还要注意yaw方向通常是可观测性最差的。旋转矩阵的Roll和Pitch可以通过重力方向校正Yaw只能靠磁力计或其他外部参考漂移快是常态。你看到yaw慢慢漂不是旋转矩阵数学错了是传感器融合策略的局限。这种场景下如果机械臂末端还带有视觉或其他绝对位姿传感器可以让它定期修正yaw比单靠IMU稳得多。7. 一个我每次都会做的检查动作这篇文章写到这里该讲的数学和实操都讲完了最后分享一个我自己的习惯。每当我拿到一台新机械臂或者要写一段跟位姿相关的代码时第一件事不是写代码而是在纸上画出三个坐标系基座坐标系、关键关节坐标系、末端工具坐标系然后手动写出它们之间的坐标变换链。这一步看着土但真能救大命。画完之后我会在代码里打一行日志把变换链上的每一个 $T$ 打出来尤其是旋转矩阵的第三列也就是Z轴方向。机械臂末端工具的Z轴朝哪儿一般跟抓取、放置、视觉手眼标定高度相关看一眼数值就能发现坐标系是不是建反了、欧拉角顺序是不是有问题。这个习惯让我少熬了很多个排查的夜。这套从旋转矩阵到齐次变换的数学工具是机械臂运动学里绕不开的底层能力。往深了走后面还涉及DH参数、四元数、逆解、轨迹规划全都在这个地基上盖楼。如果你有兴趣我可以接着写这个系列里关于DH参数建模和正解实现的实操那个话题实操性更强代码也会更多。
