第一次在实验室里调机械臂的时候我盯着示教器上那一排位姿数据看了很久——x、y、z、rx、ry、rz看着很简单但机械臂就是不走我想要的轨迹。后来自己动手写了一次正运动学代码从基座标系一路变换到末端执行器才真正意识到机器人干的所有事情本质上都是一台机器在不停地做坐标系变换。坐标系变换不是某个高深的分支它是整个机械臂运动学的底座。你如果想把末端执行器移动到某个空间点或者让视觉系统告诉机械臂“工件在这里”中间隔着的一定是一串坐标系的换算。不理解这一层后面学雅可比、动力学、轨迹规划、手眼标定都会觉得虚。这篇是我自己学习过程中整理的笔记围绕“坐标系变换”这个主题把旋转矩阵、齐次变换矩阵、DH参数、正运动学这些内容串起来讲一遍顺便把我踩过的坑也写出来希望对刚开始接触机械臂的读者有点帮助。1. 坐标系变换到底在解决什么问题1.1 机械臂里的“位置”和“姿态”是两件事很多人上手机械臂时第一次听到“位姿”这个词就以为它指的是位置。其实位姿是“位置 姿态”的合称英文叫PosePosition and Orientation。位置好理解就是三维空间里那个点用(x, y, z)描述姿态描述的是这个坐标系相对于参考坐标系“怎么摆”是正着放还是歪着放。举个生活化的例子。你去超市拿货架上的酱油瓶你的手要移动到酱油瓶的位置光有位置还不够你还得让手掌的朝向正好能握住瓶身该正握还是侧握这个“朝向”就是姿态。机械臂也一样焊接时焊枪要垂直于焊缝喷涂时喷头要对准曲面这都关系到姿态。在数学上描述一个坐标系相对于另一个坐标系的关系就是坐标系变换的核心任务。我们知道空间中任何一个刚体都可以用一个坐标系固定在上面然后用这个坐标系相对于参考坐标系的位置和姿态来完整描述刚体在空间中的状态。对于机械臂来说末端执行器就是一个刚体基座是另一个刚体通常固定在世界上它们之间就是基坐标系和末端坐标系之间的变换关系。一旦这个关系清楚了机械臂末端的位姿就是确定的。1.2 为什么所有上位机都藏着矩阵运算如果你看过机械臂的上位机软件或者读过一些开源的控制代码会发现里面密密麻麻都是矩阵。这不是程序员故意写得难懂而是矩阵天然适合做坐标变换。最直观的原因是三维空间中的一次旋转可以用一个3×3矩阵表示一次平移可以用一个3×1向量表示两者组合起来就是一次刚体变换。而机械臂有多个关节从基座到末端每个关节就是一个坐标系相邻两个坐标系之间都有一个变换。整条链上依次做变换就是多个矩阵连乘。矩阵乘法天然支持这种“链式”操作而且计算过程是线性的、确定的非常适合编程实现和实时计算。换句话说坐标系变换给了我们一套统一的语言不管机械臂有几个关节也不管关节类型是旋转还是移动都可以通过一系列坐标变换最终算出一个从基坐标系到末端坐标系的复合变换矩阵。这就是后面正运动学的数学根。2. 从旋转矩阵到齐次变换矩阵2.1 旋转矩阵是怎么来的先说姿态。如果有一个坐标系B相对于参考坐标系A发生了旋转那么我们想描述B的x轴、y轴、z轴在A坐标系下的方向。这九个方向余弦就组成了一个3×3的旋转矩阵。实际用的时候不需要去记那些复杂的推导记住旋转矩阵的几个性质写代码和做计算会顺手很多。旋转矩阵的每一列是单位向量所以它是正交矩阵即有R的转置等于R的逆。它的行列式等于1说明它只改变方向不改变长度也不产生镜像。旋转矩阵的自由度其实只有3个因为9个元素之间有关联约束这也是为什么工程上会用欧拉角、轴角和四元数来存储姿态本质都是为了压缩存储和方便插值。具体到绕单轴的旋转矩阵最重要的就是绕x轴、绕y轴、绕z轴三个基本矩阵。比如绕z轴旋转θ矩阵是[cosθ -sinθ 0] [sinθ cosθ 0] [ 0 0 1]这三个基础矩阵是构成任意旋转的积木。实际机械臂关节大多是旋转关节每个关节角θ对应一个绕当前关节轴旋转的矩阵这个基本变换会在DH参数模型中反复出现。2.2 把平移和旋转统一成齐次矩阵旋转矩阵只能描述方向变化但坐标系变换还有平移。平移就是一个三维向量。有人就想能不能把旋转和平移放在同一个框架里这样一次运算就能同时处理姿态和位置。于是就有了齐次变换矩阵。它是一个4×4矩阵左上角是3×3旋转矩阵右上角是3×1平移向量下面一行是[0 0 0 1]。这样可以用一个矩阵表示一次完整的刚体变换[R t] [0 1]使用齐次矩阵还有个额外的好处就是可以用一个4×1的齐次坐标点来表示空间中的点然后直接左乘变换矩阵得到变换后的点坐标。这样旋转和平移就统一成了矩阵乘法不需要分开写两步。这个思路在计算链式变换时特别方便因为多个关节就是多个4×4矩阵连乘。2.3 两个坐标系连续变换时乘法的顺序不能乱实际机械臂从基座到末端通常有6个关节意味着要做6次变换。每次变换对应一个齐次矩阵记为T_01、T_12、T_23……这里下标表示从前一个坐标系变换到后一个坐标系。那么从基坐标系0到末端坐标系6的总变换是T_06 T_01 × T_12 × T_23 × T_34 × T_45 × T_56整个式子是从左到右依次把坐标系一个个叠加上去。这里有一个特别容易犯错的地方矩阵乘法不满足交换律顺序一旦错了姿态就会完全不对。比如你先绕z轴转90度再沿x轴平移和先平移再绕z轴转90度结果往往不一样。很多初学者在写正运动学时总变换矩阵算出来不对大概率就是某个关节的变换顺序或者左右乘搞反了。我在实际写代码时有一个习惯每算一步变换就把当前坐标系相对于基坐标系的位姿打印出来沿着轨迹每个关节角都验证一遍确认无误后再继续下一步。这样做虽然麻烦一点但能非常快地定位是哪一个环节出了问题。3. 一张DH参数表串起整个机械臂3.1 为什么要用DH参数如果手动去定义每个关节坐标系之间的旋转和平移6个关节就要写6组矩阵而且这些矩阵的物理意义不够直观。1955年Denavit和Hartenberg提出了一种标准化的建模方法用4个参数来描述相邻两个关节坐标系之间的关系这就是DH参数。这4个参数分别是关节角θ、关节距离d、连杆长度a、连杆扭转角α。每一种机械臂都可以用一张DH参数表唯一描述。换句话讲只要这张表给出来机械臂的几何结构就是确定的。初次接触DH参数时最难的是理解这4个参数到底对应什么。我当时的理解方法是把它当作“从上一个坐标系经过两个旋转、两个平移到达下一个坐标系”的路线图。先绕当前z轴转θ再沿z轴平移d然后沿新x轴平移a最后绕新x轴转α。每一步都有明确的几何意义θ和d描述的是关节沿z轴的旋转和平移a和α描述的是连杆自身的长度和扭转。这样一来每一个关节的变换矩阵都有一个统一的公式形式把DH参数代进去就能得到相邻坐标系的齐次变换矩阵。3.2 标准DH和改进DH别混用做机械臂的人一定会遇到两种DH模型标准DHSDH和改进DHMDH。标准DH是把坐标系固定在连杆前端变换顺序是“先绕z转θ再沿z平移d再沿x平移a再绕x转α”改进DH则是把坐标系固定在连杆后端变换顺序是“先绕x转α再沿x平移a再绕z转θ再沿z平移d”。这两种模型各有各的适用范围。很多工业机械臂的官方文档用的是标准DH但一些开源库比如ROS的urdf在处理树状结构或多分支结构时更喜欢改进DH。同一个机械臂用标准DH和改进DH得到的DH参数表是不同的所以参考别人的数据时一定要注意对方用的是哪一套否则直接代入公式出来的运动学结果很容易是错的。UR5机械臂是比较经典的例子它的DH参数表在很多教材里都能找到。表中有六个关节的θ、d、a、α前三个参数相对固定θ是关节变量。实际计算时直接把每个关节当前的关节角代入矩阵公式把6个矩阵相乘就得到了末端相对于基座的位姿。这就是一个标准的正运动学过程。3.3 数值实例从关节角到位姿矩阵为了更直观我以UR5的一个简化示例来说。假设某个时刻六个关节角分别是q1、q2、q3、q4、q5、q6然后从DH参数表查出每个连杆对应的d和a以及α逐个关节构造变换矩阵。六个矩阵相乘之后得到T_06。这个4×4矩阵的左上角3×3就是末端姿态右上角3×1就是末端位置。把矩阵展开来看末端位置并不是某个关节角直接决定的而是所有关节角共同作用的结果。末端姿态则更复杂它受到各个旋转矩阵连乘的影响因此各个轴的朝向会随着机器人形态而变化。这也是为什么有时候你以为某个关节转90度末端就会朝某个方向结果完全不是那样——因为它是复合变换的结果。在学习阶段我强烈建议读者不要只看公式可以用Python或者MATLAB写一个简单的正运动学函数输入六个关节角输出末端位姿。这样你对公式的理解、对矩阵连乘顺序的理解都会深刻很多。我在自己的学习过程中就是用这种方法把UR5的正运动学从零写了一遍整个过程大约只花了一个下午但对坐标系变换的感觉完全不一样了。4. 正运动学与逆运动学坐标系变换的两种用法4.1 正运动学关节空间到笛卡尔空间的单向通道正运动学是已知各个关节角求末端位姿。这个问题在数学上是确定且唯一的给一组关节角末端的位姿是确定的。正运动学并不难难点在于建立正确的DH参数模型并把矩阵连乘写对。正运动学在实际中是干什么的呢最常见的就是在仿真环境中显示机械臂的状态。你在示教器上看到机械臂当前末端的位姿就是控制器实时做了一遍正运动学算出来的。另一个场景是轨迹规划规划器规划的是末端在笛卡尔空间中的轨迹但控制器需要知道每个时刻对应的关节角。这就要靠逆运动学把末端位姿换算成关节角。我在调试机械臂时经常用正运动学做代码验证。比如手动转动某个关节观察末端位置的变化方向是否与直觉一致或者在每次运动之前用正运动学算出当前末端的位姿然后再和目标位姿做差判断当前状态与目标状态的偏差。这比自己盯着关节角去想象末端位置要直观得多。4.2 逆运动学从末端位姿反推关节角逆运动学的任务正好相反已知末端期望的位姿求各个关节角。这是一个比正运动学复杂得多的问题。原因很好理解正运动学是一对一的映射而逆运动学往往是多对一的映射。同一个末端位姿机械臂可能有多个关节角组合都能达到极端情况下可能没有解或者因为关节限位导致某些数学解不可达。逆运动学的求解方法大体分两类解析法和数值法。对于6自由度的机械臂如果它的后三个关节轴交于一点球形腕就可以用解析法求出显式解。UR5就是这样所以能用piker等库求解。数值法则更通用思路是给定一个初始关节角猜测通过迭代优化比如雅可比矩阵的伪逆逐步逼近满足末端位姿的关节角。在学习坐标系变换的阶段逆运动学不用太早深入但需要理解它的基本逻辑先建立末端位姿与基坐标系之间的变换矩阵然后通过几何或数值方法反解。理解了正运动学的坐标系变换链再去学逆运动学就不会觉得公式来得莫名其妙了。4.3 工具坐标系机械臂真正干活的坐标系坐标系变换还有一个非常实用的扩展就是工具坐标系。基坐标系和末端坐标系描述的是机械臂本体的状态但机械臂末端通常还装着一个工具比如焊枪、夹爪、吸盘。这个工具的尖端才是真正与工件接触的位置。工具中心点TCP相对于末端坐标系有一个固定的偏移和旋转这就是工具坐标系。控制器在做运动规划时你可以选择让末端坐标系对齐目标也可以让工具坐标系对齐目标。默认情况下示教器上显示的位姿很多是TCP的位姿。这意味着如果你想调整工具的方向只要标定好TCP相对于末端坐标系的变换剩下的矩阵运算就会自动处理。实际操作中TCP标定是很常见的事。更换一个夹爪或焊枪都需要重新测量工具中心点相对于末端法兰的偏移然后输入控制器。控制器在内部会把TCP值作为固定偏移矩阵T_tool去参与运算。整个过程其实就是坐标系变换在工程中的一次典型应用。5. 实操中的坑和我的排查建议5.1 矩阵左乘右乘顺序之坑我说过矩阵顺序不能乱但实际踩坑时哪怕我理论上知道写代码时还是经常搞反。尤其是在拼接多个变换矩阵时究竟是应该左乘还是右乘需要结合自己定义的坐标系传递方向来定。我自己现则的方法是每写一行矩阵乘法就注释一行“当前坐标系相对于哪个坐标系的变换”。比如T_01表示“坐标系1相对于坐标系0”那么链式传递时就是T_01 × T_12下标中间的1是连接的桥梁。一旦下标顺序接不上就说明有地方写错了。这个方法帮助我至少避免了好几次低级错误。5.2 万向锁与姿态插值问题姿态除了用旋转矩阵表示还经常用欧拉角表示。但欧拉角有一个著名的缺陷万向锁。当某个轴旋转到±90度时会丢失一个自由度导致姿态出现奇异。如果在这种状态下对欧拉角做插值机械臂末端姿态就会变得很诡异甚至出现突然翻转。所以在做姿态插值或轨迹规划时我更推荐用四元数而不是欧拉角。四元数没有万向锁问题而且插值平滑。很多控制器内部也默认用四元数存储姿态。你在示教器上看到的是欧拉角但底层运算大概率是四元数或旋转矩阵。理解坐标系变换时要意识到姿态的表示方式有很多种关键是要知道它们之间如何互相转换。5.3 手眼标定里的坐标系串联另一个很常被问到的坑是手眼标定。视觉系统给机械臂一个目标位置但这个位置是在相机坐标系下描述的机械臂要动就必须把它转换到基坐标系下。这就涉及到相机固定在某处眼在手外还是相机装在机械臂末端眼在手上的区别。不管是哪种情况本质上都是在求解一系列坐标系变换矩阵之间的约束方程。比如眼在手外目标物体在相机坐标系中的坐标要变成基坐标系坐标中间需要知道相机在基座标系中的位姿。这个位姿未知所以要通过多次移动机械臂并拍摄标定板来求解。理解了坐标系变换的链式关系手眼标定的原理就清楚了一大半。剩下的就是标定采样时注意覆盖足够多的位姿避免算法退化。5.4 常见问题速查表问题现象可能原因排查建议正运动学算出的末端位置与实机示教器不一致DH参数选错标准DH与改进DH混用、d或a符号不对核对DH参数表来源打印每一步变换矩阵末端姿态与预期方向偏差较大矩阵乘法顺序写反检查下标顺序是否连续逐步验证欧拉角插值时末端姿态突然翻转万向锁奇异改用四元数插值路径规划时末端位置可达但姿态不可达逆运动学无解或关节限位检查目标姿态是否超出关节工作空间工具尖端实际位置与示教器显示不一致工具坐标系标定错误重新标定TCP检查偏移量正负号这张表是根据我自己调试经验和与同行交流整理的不代表覆盖所有情况但覆盖了我遇到的大部分问题。如果遇到类似现象可以优先按这个思路排查。6. 后续扩展这只是一个起点坐标系变换这个主题往下延伸至少有四个方向值得继续深入。第一个是雅可比矩阵它描述关节速度与末端速度之间的线性映射是分析机械臂奇异位形和做速度规划的基础。第二个是动力学机械臂的惯性力、科氏力、重力项都依赖坐标系变换建立运动方程。第三个是轨迹规划笛卡尔空间和关节空间的切换本质上就是坐标变换在时间维度上的扩展。第四个是视觉伺服把相机图像中的像素坐标转换为机械臂基坐标系中的三维坐标靠的还是变换链。我在学完坐标系变换之后最大的体会是机械臂控制再花哨底层都是这些矩阵在流转。你在上位机里看到的一个个数值内部都是经过层层变换算出来的。搞懂了坐标系变换很多看似复杂的功能其实都能拆成一串矩阵乘积。如果你正在学机械臂我建议先不要急着碰控制算法老老实实把坐标系变换这块啃下来。找一个公开的机械臂DH参数去网上找一个现成的正运动学代码自己敲一遍然后对着示教器验证一次。这个过程会刷新你对机械臂的认知。我个人在后期做项目时每次遇到奇怪的位姿问题第一反应就是回到坐标变换去检查是哪两个坐标系之间的关系搞错了是哪个参数标定偏了。坐标系变换就是这么一种基础而又常用的工具它不炫技但你真的离不开它。