做机器人这些年我见过太多人一上来就调运动控制卡、发脉冲让机械臂动起来结果机械臂真的动起来之后却说不清末端执行器到底在哪个位置。有一次做视觉抓取相机给出一个目标点在像素坐标里需要换算到机器人基坐标系团队里一个新同事直接拿像素值当毫米值填进偏移量差点把夹爪怼进工件里。这件事让我意识到不管以后做轨迹规划、力控还是视觉引导机械臂运动学永远是绕不开的第一道坎。机械臂运动学回答的问题特别朴素每个关节转了一个角度之后末端到底在哪、姿态是什么。反过来想让末端到某个位姿各个关节应该转到多少度。前者叫正运动学后者叫逆运动学。本篇是“机械臂运动学”系列的第一篇先把正运动学这条主线讲清楚从空间变换基础到DH参数建模再到实际写代码验证。适合刚接触机器人的学生、正在做机械臂二次开发的工程师也适合那些想搞懂控制器里“坐标变换”到底做了什么的人。1. 从关节空间到笛卡尔空间运动学到底在算什么1.1 正运动学给角度求位姿像“看仪表盘猜动作”正运动学的输入是机械臂各个关节的角度输出是末端执行器在基坐标系下的位置和姿态。位置好理解就是三维坐标里那个点姿态指的是末端工具坐标系三个轴分别指向哪个方向。打个比方你坐在车里方向盘转了多少度、油门踩了多少这是输入车最终开到哪、车头朝哪个方向这是输出。对机械臂来说六个关节各转一个角度末端执行器到哪儿、抓爪朝哪儿有一套完全确定的映射关系。这个映射就是正运动学。这句话值得反复读正运动学是“完全确定”的。给定一组关节角度末端位姿有且只有一个解。所以正运动学在工程上特别好验证也特别适合做调试基础。如果一台机械臂说自己在某个姿态你通过正解算出来末端位置和实测位置对不上那一定是某个环节出了问题最大的嫌疑就是DH参数表抄错了。正运动学的计算方法不复杂本质就是坐标系一层一层往下乘。每个关节都对应一个坐标变换把上一个坐标系里的点变换到下一个坐标系里最后把所有变换连乘起来就得到末端位姿。这个“连乘”的思路会贯穿整个系列。1.2 逆运动学给位姿求角度麻烦从这儿开始逆运动学的输入是末端位姿输出是六个关节角。听起来像正运动学的“倒放”但实际上麻烦得多。麻烦主要来自三个方面。第一多解。同一个末端位姿机械臂可能有八组关节解比如肘部朝上还是朝下、腕部翻转还是不翻转。第二无解。目标点离机械臂太远或者姿态要求超出可达范围逆解会直接失败。第三奇异位形。在某些特殊姿态下关节速度会趋向无穷大机械臂表现为“某个方向怎么动都推不动”。逆运动学没有固定公式可以套求解方式高度依赖机械臂结构。不过咱们先不急着啃硬骨头因为逆解需要以正解为基础。如果正解没吃透逆解算出来的关节角你都不知道怎么验证对不对。1.3 为什么第一篇先把“地基”打牢这个系列我打算按这样的顺序写第一篇讲运动学整体框架和正运动学第二篇重点展开逆运动学的解析解法和数值解法第三篇再看速度运动学雅可比矩阵和奇异位形分析。选这个顺序是因为我在实际带人做项目时发现只要正运动学的基本功扎实后面学逆解、学路径规划都特别快。反过来如果连DH参数和齐次变换都没捋清楚后面每一步都会踩坑而且踩了坑还不知道怎么排查。所以这一篇宁可多花篇幅把基础概念和实操细节讲透也不急着往前赶。2. 空间变换基础旋转矩阵和齐次变换2.1 位姿不只是位置还有姿态很多新手写运动学代码最容易犯的一个错是只关注末端位置把姿态给丢了。位置是三维坐标姿态是另一个东西。想象你手里拿着一支笔笔尖点在某一个坐标点上但笔杆可以朝上、朝前、朝左、朝右偏。位置一样姿态完全不同。机械臂的末端法兰、吸盘、夹爪都有方向性姿态错了就算位置对了该撞该偏一样出问题。描述姿态最常见的工具是旋转矩阵。对于一个坐标系可以用三个互相垂直的单位向量表示它的三个轴方向。这三个向量在基坐标系下各有一个三维坐标把它们拼成三列就得到一个三乘三的旋转矩阵。这个矩阵看起来有九个数字但并不是九个独立变量。三个单位向量的长度都必须是1两两之间必须垂直这六个约束下来真正的独立变量只有三个。所以一个旋转矩阵本质上只描述三个自由度对应我们常说的横滚、俯仰、偏航。2.2 旋转矩阵为什么它是正交的又为什么好用旋转矩阵有一个非常好的性质它的逆矩阵等于它的转置。这意味着你不需要真的做矩阵求逆只要把矩阵转置一下就能把坐标从目标坐标系变回基坐标系。这个性质在做坐标变换时极其方便。你从机械臂基座算到末端得到的是末端坐标系相对基座的旋转矩阵反过来如果你想从末端坐标系的视角看基座在哪直接把矩阵转置就行。工程上还有一个常见的坑旋转矩阵在浮点运算中会慢慢失去正交性。矩阵本身有误差乘的次数多了三个轴不再严格垂直长度也不完全等于1。所以在写运动学程序的时候尤其是做循环计算、迭代求解时要定期用正交化手段修正旋转矩阵。这不是理论洁癖是实打实影响控制精度的细节。那为什么不直接用欧拉角欧拉角的直观性好界面显示方便但存在万向锁问题在某些姿态下两个旋转轴会重合丢失一个自由度导致姿态突变。四元数适合插值和滤波但不够直观调试时很难一眼看出问题。所以我的建议是内部计算用旋转矩阵或四元数人机交互显示用欧拉角不要在内部计算里全程用欧拉角硬扛。2.3 齐次变换矩阵把旋转和平移装进同一个框架旋转矩阵管姿态平移向量管位置。如果分开写计算一个点的完整变换就要同时做矩阵乘法和向量加法比较啰嗦。于是人们发明了齐次变换矩阵一个四乘四的矩阵把旋转矩阵和平移向量组合在一起。结构如下[ R t ] [ 0 1 ]左上角是三乘三旋转矩阵右上角是三乘一平移向量左下角是一乘三零向量右下角是1。坐标点也要写成齐次形式末尾补一个1。为什么要把矩阵扩成四维核心原因是把变换变成纯矩阵乘法。这样一来多个坐标系的连续变换可以直接连乘不用来回切换乘法和加法。机械臂每个关节就是一个变换矩阵从基座到末端一串矩阵乘下来就是正运动学。你可以把这件事想象成搭积木。每个关节的变换是一块积木积木之间按照固定的顺序拼在一起最终拼出的形状就是机械臂的姿态。DH参数法就是给这块“积木”规定了统一的制作标准。2.4 固定轴和动轴左乘与右乘的差别坐标变换里面最容易踩坑的一个细节是旋转是相对于固定轴还是相对于自身轴。如果每次旋转都相对于基坐标系固定轴那么旋转矩阵要左乘。假设当前姿态是M想绕基座x轴旋转90度新姿态就是R_x(90) * M。如果每次旋转都相对于当前坐标系动轴那么旋转矩阵要右乘。新姿态就是M * R_x(90)。这个区别在机械臂里极其重要。DH参数法里有些参数是绕z轴转、有些是绕x轴转如果搞不清是左乘还是右乘DH表就会写出一堆错值。我自己的经验是拿到一个变换序列先用最简情况手推一遍。比如坐标系绕x轴转90度再沿当前坐标系z轴平移一个单位看最终坐标落在哪。纸上推一遍比你对着代码猜半天有效得多。3. DH参数建模机械臂的通用“坐标系语言”3.1 为什么要用DH参数而不是直接堆旋转矩阵理论上你可以用前面说的旋转矩阵和平移向量给每个关节手写坐标变换。但真实机械臂有六个关节每个关节之间还有连杆长度、关节偏置、轴扭转这些几何关系直接堆矩阵非常容易出错维护起来也痛苦。DH参数法就是为了解决这个问题出现的。它用四个参数描述相邻两个关节坐标系之间的变换关系关节角θ、连杆偏距d、连杆长度a、连杆扭转α。只要把每个关节对应的四个参数列成一张表整条运动学链就完整确定了。这四个参数各有各的几何意义。θ是绕z轴转的角度对应关节转动d是沿z轴平移的距离对应相邻关节轴线之间的偏置a是沿x轴平移的距离对应连杆长度α是绕x轴转的角度对应相邻两个关节轴线之间的扭转角。用生活类比来解释你可以把每个关节看成一根“骨头”连接处的一块“筋”。θ决定这块筋怎么转d决定这根骨头伸出多远a决定下一块骨头多长α决定两根骨头是不是不在一个平面里。这四个信息凑齐坐标系的相对位置就唯一确定了。3.2 标准DH和改进DH先分清再动手DH参数法有两个版本标准DHSDH和改进DHMDH。初学者最容易栽的坑就在这里拿到的资料里DH表是哪种约定决定了公式和参数的含义完全不同混用必错。标准DH的变换顺序是绕z轴旋转θ沿z轴平移d沿x轴平移a绕x轴旋转α。变换矩阵如下T Rot(z, θ) · Trans(z, d) · Trans(x, a) · Rot(x, α)改进DH的变换顺序是绕x轴旋转α沿x轴平移a绕z轴旋转θ沿z轴平移d。两者都能描述机械臂但同一台机械臂在两种约定下的DH表数值不一样。尤其是α的正负号很多资料抄来抄去正负号已经失真直接套代码经常出现末端位姿旋转90度、位置偏移一个连杆长度之类的诡异结果。讨论完毕注意看小标题编号## 1. 章节名### 1.1 子主题名很好。表格对比标准DH和改进DH对比项标准DH (SDH)改进DH (MDH)变换顺序绕z转θ → 沿z移d → 沿x移a → 绕x转α绕x转α → 沿x移a → 绕z转θ → 沿z移d坐标系固定位置固定在关节i的远端固定在关节i的近端常见应用场景传统教学教材、部分控制器URDF模型、部分现代算法库混淆风险中高轴上参数定义容易反怎么确认手里的DH表是哪一种两种办法。第一查原始说明文档里的变换公式看定义的是“坐标系i相对坐标系i-1”还是“坐标系i-1相对坐标系i”。第二找一个已知机械臂零位姿态的实测值用两套公式分别手算哪一套对上就用哪一套。没有第三种捷径。3.3 一台真实机械臂的DH表是怎么来的以最常见的六轴垂直关节机械臂为例我给出一组适合学习用的标准DH参数。参数数值是我随意取的演示值不代表任何具体型号产品。关节iθ_id_ia_iα_i1θ10.30090°2θ200.300°3θ300.050°4θ40.20090°5θ500-90°6θ60.0500°这张表的几何含义是关节1垂直安装在基座上基座高度0.3米关节1的轴线与关节2的轴线呈90度关节2和关节3分别对应大臂和小臂长度轴线平行关节4引入腕部偏置0.2米轴线又转了90度关节5把腕部方向再折一下关节6就是工具法兰带0.05米工具长度。注意关节4、5、6的轴线设计成一个“球形腕”也就是三轴交于一点。这是工业六轴机械臂最常见的构型好处是逆运动学可以解耦先算腕心位置再解前三个关节和后三个关节。后面讲逆解的时候会回来说这件事。4. 正运动学实操从DH表到位姿矩阵4.1 标准DH单个连杆变换矩阵怎么写拿到了DH表下一步就是把它转成变换矩阵。标准DH约定下第i个关节的变换矩阵可以写成T_i [ cosθ -sinθ·cosα sinθ·sinα a·cosθ ] [ sinθ cosθ·cosα -cosθ·sinα a·sinθ ] [ 0 sinα cosα d ] [ 0 0 0 1 ]这个公式不用死背理解它的来源就有数了。它就是把“绕z转θ、沿z移d、沿x移a、绕x转α”这四次基本变换依次左乘组合起来的结果。写代码的时候我习惯于把每一步单独写成独立函数然后循环连乘。这样做的好处是哪一关节出问题可以直接打印中间变量定位。等代码稳定了再考虑矩阵优化、缓存之类的花活前期调试阶段千万别上来就搞复杂封装。4.2 先用两连杆平面臂建立直觉在直接上六轴之前我强烈建议你先亲手推一遍两连杆平面机械臂的正解。这不是浪费时间而是用最小模型建立足够的直觉。假设机械臂在一个平面内第一个连杆长度L1第二个连杆长度L2两个关节角分别是q1和q2。末端坐标可以很直观地写出来x L1·cos(q1) L2·cos(q1q2) y L1·sin(q1) L2·sin(q1q2)这个公式的本质是第一步沿着第一个连杆方向走L1第二步沿着当前方向再旋转q2之后走L2。用这个公式对照DH法的矩阵连乘你就能直观感受到“坐标系一层层变换”到底是什么含义。拿这个公式去做测试比如L10.3、L20.2q130度、q260度手算一遍末端坐标再套矩阵乘法算一遍两边一对比如果一致说明你对齐次矩阵和DH参数的理解基本到位了。4.3 用Python把六轴正解完整跑起来下面给一个可直接运行的六轴正解Python脚本用的是上一节那张标准DH参数表。import numpy as np def sdh_transform(theta, d, a, alpha): 标准DH单个连杆变换矩阵 ct, st np.cos(theta), np.sin(theta) ca, sa np.cos(alpha), np.sin(alpha) return np.array([ [ct, -st * ca, st * sa, a * ct], [st, ct * ca, -ct * sa, a * st], [0, sa, ca, d], [0, 0, 0, 1] ]) # 演示用DH参数请勿对应真实型号 d1, d4, d6 0.30, 0.20, 0.05 a2, a3 0.30, 0.05 # 六个关节角先全部置零 q np.deg2rad([0, 0, 0, 0, 0, 0]) params [ (q[0], d1, 0.0, np.deg2rad(90)), (q[1], 0.0, a2, 0.0), (q[2], 0.0, a3, 0.0), (q[3], d4, 0.0, np.deg2rad(90)), (q[4], 0.0, 0.0, np.deg2rad(-90)), (q[5], d6, 0.0, 0.0), ] T np.eye(4) for p in params: T T sdh_transform(*p) np.set_printoptions(precision4, suppressTrue) print(T)这段代码的逻辑很简单从单位矩阵出发依次乘上每个关节的变换矩阵最终得到的就是机械臂末端的齐次变换矩阵。4.4 用零位姿态验证代码正确性上面代码跑出来的结果应该是一个四乘四矩阵。因为所有关节角都设为零这个矩阵代表机械臂处于“零位姿态”时的末端位姿。按照我给的演示参数输出大致如下[[ 1. 0. 0. 0.35] [ 0. 0. -1. -0.25] [ 0. 1. 0. 0.3 ] [ 0. 0. 0. 1. ]]第四列的前三个数就是末端位置x0.35y-0.25z0.3。看到y是负的有人可能会觉得“是不是算错了”其实没算错这是腕部偏置d4和工具长度d6在零位姿态下带来的几何结果。换句话说零位姿态下末端并不在x-z平面内这本身就是DH表几何关系决定的。验证代码是否正确有几个更通用的检查点一是旋转部分的三个列向量长度应该等于1二是三个列向量两两正交三是整个矩阵的行列式应该等于1。你可以用代码自动检查R T[:3, :3] print(np.linalg.det(R)) # 应该非常接近1 print(np.linalg.norm(R[:, 0])) # 应该非常接近1 print(np.allclose(R R.T, np.eye(3))) # 应该输出True这组检查通过基本可以证明矩阵连乘的代码逻辑没有大问题。如果跑到这里一切正常恭喜你正运动学你已经上手了接下来要做的就是多换几组关节角度去对照实测值。5. 逆运动学入门方向比技巧重要5.1 逆解的本质非线性方程组与多解逆运动学从数学上看就是已知末端的齐次变换矩阵T反解六个关节角θ1到θ6。T矩阵里包含12个有效数但机械臂只有6个自由度所以这是一个求解六个未知数的非线性方程组问题。非线性方程组没有通用解析解法所以逆运动学成了机械臂领域一座绕不开的山。不过好消息是常见工业机械臂的结构是有规律的尤其是球形腕构型后三个关节轴线交于一点可以把问题大大简化。这个简化思路是这样的先不管姿态只找腕心位置。腕心是后三个关节轴线的交点它只跟前三个关节角有关。而末端位姿和工具长度给定之后腕心位置是可以直接算出来的。于是六自由度逆解被拆成两个三自由度问题先用位置解前三个关节再用姿态解后三个关节。5.2 解析解与数值解怎么选解析解的意思是通过几何关系或代数消元直接把关节角表达成关于末端位姿的公式。它快、稳定、可重复而且能一次性把多组解全部算出来。只要机械臂结构满足特定约束条件解析解永远是首选。数值解用迭代的方式逼近一个可行解比如拉格朗日法、牛顿-拉夫逊法或各种优化算法。它的优点是通用随便什么结构的机械臂都能算缺点是慢需要好的初值而且初值给不好可能收敛到错误的解上。工程上的常见做法是混合使用优先用解析解得不到或用不了的时候切换到数值解用解析解的结果作为数值解的初值。我见过不少纯做优化的团队跑很久才收敛一次换到解析解之后控制周期从几十毫秒降到几毫秒效果立竿见影。5.3 多解取舍、奇异位形、关节限位同样是抓一个点机械臂可以把肘部抬高也可以把肘部压低腕部可以正向翻转也可以反向翻转。这么多组解怎么选工程上主要看四条规则。一是避开关节限位选离各关节中点最近的那组解。二是路径连续性相邻两个控制周期之间关节角变化越小越好避免瞬移。三是避障某些解虽然关节角度可行但机械臂整体会撞到周边设备必须舍弃。四是优先级实时控制中多解选择还要避开奇异位形防止关节速度爆炸。奇异位形值得单独说。以球形腕机械臂为例最典型的是腕部奇异当关节4和关节6的轴线重合时腕部丢失一个自由度关节4和关节6会往相反方向高速转动速度趋向无穷大。还有肩部奇异腕心落在关节1轴线上关节1的解不唯一。遇到这类情况控制器通常会限速、停止或要求人为设置过渡路径。5.4 先从几何法入手的三条经验给准备学逆解的同学三条经验。第一条找个两连杆或三连杆平面机械臂用几何法手推一遍解析解感受一下求解过程。第二条先把工具长度、腕部偏置这些参数在纸上画清楚再进代码很多人最终解不对问题出在几何建模阶段。第三条不要指望一套代码通吃所有机械臂结构和DH表变了解析解的公式就要重推。正运动学的熟练度在逆运动学的调试阶段会成倍放大你的效率。因为你算出一组关节角之后必须用正解去验证末端位姿是否正确正解不熟这一步会非常痛苦。6. 实际工程里的运动学坑6.1 DH表约定不一致是最隐蔽的坑前面讲了标准DH和改进DH的区别这里再说一个真实场景同一台机械臂你从官方手册里拿到的DH表和从某个开源库里翻到的DH表符号却对不上。这种情况经常发生。你可能在官方资料里看到α190度在某个开源模型里看到α1-90度。它们可能都没错只是用了不同的约定。比如坐标系初始方向定义不同、零位标定位置不同、关节正方向不同都会导致DH表中的数值差一个符号。我的习惯是拿到任何一份DH表先做一次零位验证。把机械臂开到机械零点限位块或标记线对准的位置读当前关节角然后用自己写的正解脚本算末端位姿拿激光跟踪仪或至少一把钢卷尺实测末端位置。对得上这份DH表基本可信对不上先查约定后查参数。6.2 单位、正负、零位最隐蔽的三兄弟单位问题是最常见的低级错误。很多控制器内部用弧度上位机界面给人看的是度一旦角度从界面上读出来直接塞进正解函数末端位置会偏移到离谱。排查办法很简单把关节角从0度转到90度看末端位置的变化量是否接近对应连杆长度。如果是1e-9量级基本可以断定单位出了问题。正负方向是第二个常见坑。DH表里θ的正方向定义取决于每个关节的旋转方向定义有些控制器关节正转是顺时针有些是逆时针。这个问题只能靠实测确认手动把某个关节正转一点看正解末端的位移方向是否和视觉观察一致。零位标定是第三个坑。机械臂的零点不对DH表再正确也没用。很多调试事故——末端位置偏移、姿态跳变、甚至撞机——归根到底都是零点没校好。判断方法很简单把机械臂摆到机械零位看控制器里每个关节角读数是否和DH表完全一致。有偏差就要重新标定。6.3 一套我一直在用的正解校验流程我这里有一套个人常用的校验流程比较笨但可靠。第一步把机械臂开到机械零位记录所有关节角用正解算末端位姿和实测对一遍。第二步依次单关节运动比如只转关节2从0度到30度看末端位置变化的方向是否和该关节轴线的旋转方向一致。第三步开一个简单的矩形轨迹让机械臂末端在笛卡尔空间走四个角点用正解输出的位置画出来检查是否是规则矩形。第四步有条件的话用外部测量设备实测末端轨迹误差应该在毫米级。这套流程走下来DH表、单位、符号、零点基本都能确认清楚。6.4 调试心得别一上来就碰六轴通用求解器这些年带过不少人我发现上手最快的人都有一个共同习惯先拿两连杆、三连杆的小模型练手把坐标变换和DH参数吃透再碰真实机械臂。上来就想写一套“六轴万能逆解算法”的多半会在各种细节里挣扎很久。还有一个心得藏在调试细节里写正解代码时把每个关节的变换矩阵都保存在一个列表里。万一最终结果不对你可以从第一个关节开始逐级打印看哪一级的坐标系方向突变、平移量异常问题范围一下子就缩小了。如果不拆开调试只盯着最终的4x4矩阵看遇到问题只能干瞪眼。运动学这门功没有太多玄学最笨的办法往往就是最稳的办法手推一遍程序算一遍实测再对一遍。三遍对上了心里就有底了。后续进入逆解和雅可比矩阵的学习时你会发现证结走得稳的好处会陆续显现出来。我现在回头想想当年自己卡得最久的地方反而不是那些复杂的矩阵公式而是一个最简单的问题坐标系的方向到底怎么定义。把这个想明白之后整个运动学学习曲线一下就顺了。如果你现在也在这个位置纠结不妨放慢一点把前面第2节和第3节的内容动手多推几遍收益绝对比急着往后学要大得多。