做机械臂开发的人十有八九第一次算正运动学时都被坐标系绕晕过。我头一回照着论文里的DH表建模对着一个六轴机械臂建了两天末端位置算出来差出十万八千里最后发现是Z轴方向画反了。今天这篇是机械臂速成系列的第八篇专门讲标准DH法实战从连杆坐标系的建立到运动学方程的组装一步一步带你把正运动学跑通。这套方法无论你是用AR3、UR5e、松灵Piper这类现成机械臂还是自己3D打印一个六轴来玩都是绕不开的第一道坎。文章不会有太复杂的数学推导但该有的细节和坑一个不少适合刚入门、想搞懂运动学方程到底怎么来的同学也适合已经会用现成库但没自己手推过一遍的老哥们。1. 标准DH法到底在算什么先把思路捋直1.1 一个让你事半功倍的视角连杆不是关节坐标系才是本体我先说一个大家容易犯的错误学DH法的时候满脑子都是“我这根杆多长、那个关节能转多少度”这是站在机械结构的角度看问题。但DH法真正的视角是“坐标系”。每个连杆都绑一个坐标系然后通过坐标系之间的相对变换来描述运动。什么是“绑一个坐标系”你可以把每个连杆想象成一块底板底板上画了三个互相垂直的箭头代表X轴、Y轴、Z轴。机械臂动起来的时候每个连杆的姿态变了底板上箭头指向也跟着变。运动学要回答的问题是已知每个关节转了多少度末端那块底板上的箭头相对于基座底板指向哪、原点在哪。所以机械臂正运动学本质上是一条“坐标系链”基座坐标系{0}到连杆1坐标系{1}{1}到{2}一直传到最后末端坐标系{n}。每个相邻变换只依赖四个参数也就是一张DH参数表就能把一个六轴机械臂的全部几何关系描述完。我以前总觉得连杆参数表已经够清楚了后来发现这是错觉。比如一根斜着的连杆你光知道长度和角度很难直观推出它在三维空间里的位姿但放到坐标系变换里只要按规则建系、填参数、矩阵连乘答案就很稳定。这种视角的优势是“模块化”。不管机械臂是四轴、六轴还是七轴代码里都是同一个变换函数只是参数表长度不同。当年我自己手写正运动学时靠的就是这个思路先把一根连杆的变换矩阵写对后面全是体力活。1.2 标准DH参数表四个参数说清所有几何关系标准DH法给每个相邻坐标系之间的变换准备了四个参数分别是θ_i、d_i、a_i、α_i。我习惯用“谁和谁的关系”来记而不是死背定义。以从坐标系{i-1}到坐标系{i}的变换为例θ_i绕Z_{i-1}轴从X_{i-1}转到X_i的角度通常是关节变量。d_i沿Z_{i-1}轴从X_{i-1}到X_i的距离也就是两条X轴在Z_{i-1}方向上隔了多远。a_i沿X_i轴从Z_{i-1}到Z_i的距离通常就是连杆长度。α_i绕X_i轴从Z_{i-1}转到Z_i的角度通常反映相邻两个关节轴线的扭转角。这四个参数里θ_i和d_i描述的是“绕Z轴转了多少、沿Z轴平移多少”a_i和α_i描述的是“下一个坐标系的X轴方向和旋转”。你可以把一根连杆想象成一根弯管两端各有一个轴承座a_i是弯管两头之间的直线距离α_i是两端轴承孔中心线之间的夹角d_i则是前一个关节轴方向上的错位量。为什么需要四个参数而不是两个因为三维空间里两个坐标系之间的相对关系一共有六个自由度但DH法通过精心选择坐标系的建立方式把其中两个自由度“藏”进了坐标系定义规则里。剩下的四个参数足以完整描述相邻连杆关系这也是DH法能被广泛使用的根本原因。1.3 标准DH和改进DH别等建完坐标系才发现表错了这里必须给新手提个醒。你搜DH法会看到标准DH也叫经典DH和改进DH也叫Craig DH两种长得像但坐标系约定不一样。我们这次实战用的是标准DH也就是变换顺序为“绕Z旋转 → 沿Z平移 → 沿X平移 → 绕X旋转”。改进DH的顺序则是“沿X平移 → 绕X旋转 → 沿Z平移 → 绕Z旋转”。为什么会有这种区别因为坐标系放的位置不一样。标准DH把坐标系{i-1}的Z轴放在第i个关节轴线上坐标系{i}的Z轴放在第i1个关节轴线上改进DH把坐标系{i}的Z轴放在第i个关节轴线上相当于坐标系整体“向后错了一位”。这就导致同一个机械臂用标准DH和用改进DH填出来的参数表可能完全不同。对比项标准DH本文用改进DH / Craig DH坐标系放置坐标系{i-1}的Z轴沿关节i轴线坐标系{i}的Z轴沿关节i轴线变换顺序Rot(z,θ) → Trans(z,d) → Trans(x,a) → Rot(x,α)Trans(x,a) → Rot(x,α) → Trans(z,d) → Rot(z,θ)常用下标第i行写θ_i, d_i, a_i, α_i第i行写θ_i, d_i, a_{i-1}, α_{i-1}常见场景经典教材、传统工业机器人资料Craig《机器人学导论》、ROS里不少URDF模型如果你只是照着别人的机器人抄DH表一定先确认对方用的是哪一种。我在实际项目里踩过一次坑从某个开源六轴机械臂工程里拿到一张看起来很像样的DH表直接用标准DH矩阵去套结果末端旋转方向怎么调都不对后来发现那份代码用的是改进DH。所以不是“哪个DH是对的”而是“你自己的程序按哪种DH写的参数表就要匹配哪种”。2. 动手建立连杆坐标系六轴机械臂一步一步来2.1 Z轴的摆放原则建坐标系的第一步是先把每个关节的Z轴方向定下来。对于第i个旋转关节Z_{i-1}轴就沿着第i个关节的旋转轴方向。这里的方向可以自己选但选完就不要再改不然后面所有参数都会跟着乱。基座坐标系{0}的Z0一般让机器人“立起来”的方向为正也就是第一个关节竖着的转轴方向向上。比如一台典型的六轴工业机械臂底座关节绕竖直方向旋转那Z0就竖直向上。后面每个关节轴方向要根据机械结构实际姿态来画。画图时我的习惯是把机械臂侧视图先画出来标好每一个关节轴线再在这个图上加坐标系这样不容易漏。有一个常见问题末端坐标系{n}的Z轴怎么放标准DH里Z_n理论上可以任意选择因为末端之后没有下一个关节了。实际中我建议先把最后一个关节的轴线方向作为Z_{n-1}然后末端坐标系Z_n沿工具方向比如法兰中心向外这样后续加装夹爪、视觉相机时可以直接用工具坐标系思维负担小一点。2.2 X轴和原点的确定定好Z轴后X轴按公垂线规则来X_i沿Z_{i-1}与Z_i的公垂线方向从Z_{i-1}指向Z_i。所谓公垂线就是一条同时垂直两条异面直线的线段它的方向就是两条Z轴之间最短距离的方向。这个方向定了X_i的方向也就定了。如果Z_{i-1}和Z_i是相交的公垂线长度为0X_i方向通常取与两者都垂直的方向让X_i、Z_{i-1}、Z_i满足右手定则。如果Z_{i-1}和Z_i是平行的公垂线有无数条你可以任选一条但选定之后往往会影响相邻的d_i或a_i数值所以最好选一条看起来最“自然”的例如过前一个坐标系原点的那条公垂线这样很多d_i会变成0计算省事。原点位置也顺带确定了坐标系{i}的原点就是X_i轴与Z_i轴的交点。注意不是X_i和Z_{i-1}的交点而是X_i这条公垂线和Z_i的交点。这个细节我当年搞反过导致原点位置全错了。如果你发现某一行参数填完相邻变换矩阵里的平移向量和几何结构怎么也对不上大概率就是原点取错地方了。2.3 实例一个6R垂直关节型机械臂的完整建模为了实战我建一个典型6R垂直关节型机械臂的结构底座旋转然后大臂俯仰、肘部俯仰再往后是腕部旋转、腕部俯仰、末端旋转。这种构型很常见很多六轴教学机械臂和工业机械臂都是这个路子。先按前面的规则把Z轴方向捋一遍Z0竖直向上Z1水平关节2轴线Z2水平且与Z1平行关节3轴线Z3为腕部关节4轴线Z4为关节5轴线Z5为关节6轴线。按标准DH规则建立坐标系并做一定的零位约定后得到的DH参数表如下iθ_i(变量)d_i(m)a_i(m)α_i(°)1θ10.10902θ200.403θ300.3504θ40.10905θ500-906θ60.100我来逐个解释参数对应的结构含义d10.1是基座到肩关节的高度a20.4是大臂长度a30.35是小臂长度d40.1是腕部到肘部的偏置距离d60.1是末端法兰中心到工具坐标系原点的距离。α190°表示Z0到Z1轴线相互垂直α2和α3为0说明关节2、3轴线平行α490°和α5-90°描述腕部两个轴线的扭转关系。这里要说明一下我给的这组参数是简化的理想模型不一定对应某台真实机械臂的完整CAD模型。真实机械臂的DH表往往还会有许多微小偏置比如关节2和3之间的坐标系原点不一定刚好在关节轴心上但这些偏置在运动学原理上完全一样只是参数稍多一点。学习阶段先用简化模型把流程跑通后面再填真实参数就顺了。2.4 新手最容易犯的三个错误第一个错误是平行关节的X轴方向乱取。像关节2和关节3Z1∥Z2公垂线不唯一。如果你把X2的方向取反对应a2的符号也会变同时相邻行的d和θ都可能受影响。只改一半坐标就会翻转。我的建议是画好图后每个X轴方向都用叉积验一遍X_i方向必须垂直于Z_{i-1}和Z_i而且从Z_{i-1}指向Z_i别凭感觉。第二个错误是Z轴方向正反搞错。Z轴方向反了α_i的正负号就会差180°矩阵模型会有非常明显的错误。比如某个关节明明应该顺时针转末端却往逆时针跑。遇到这种问题不要先怀疑代码先回头检查Z轴画的方向。第三个错误是基座坐标系和末端坐标系没有约定好。基座坐标系{0}的方向会影响最终读到的末端位置末端坐标系{n}的方向会影响姿态矩阵怎么解读。尤其是后面要加装夹爪、相机如果没分清楚“法兰坐标系”和“工具坐标系”手眼标定的时候会被绕到怀疑人生。建议在项目一开始就定义一个统一约定比如基座坐标系X指向前方Z向上末端工具坐标系Z沿工具伸出方向。3. 从坐标系到运动学方程相邻变换矩阵的推导3.1 四次基本变换的顺序为什么不能乱标准DH的相邻连杆变换矩阵T_i可以拆成四个基本变换T_i Rot(z, θ_i) * Trans(z, d_i) * Trans(x, a_i) * Rot(x, α_i)这四个变换是“在变换后的当前坐标系下”依次执行的先绕当前Z轴旋转θ再沿当前Z轴平移d再沿当前X轴平移a最后绕当前X轴旋转α。因为矩阵乘法是右乘所以每次新变换都作用在已经变换完的坐标系上。我说一个你们肯定遇到过的类比先向右转90度再往前走两步和先往前走两步再向右转90度结果完全不一样。DH变换顺序同理。你要是把Trans(x,a)和Rot(z,θ)调换位置得到的矩阵几乎肯定不对。所以我写代码时从不用记忆的矩阵公式而是每次都用四个基础矩阵相乘来生成一是能减少公式记忆错误二是以后换成改进DH也容易改。3.2 标准DH矩阵长什么样把上面四个基本变换乘起来就得到标准DH的4×4齐次变换矩阵[ \begin{bmatrix} \cos\theta_i -\sin\theta_i\cos\alpha_i \sin\theta_i\sin\alpha_i a_i\cos\theta_i \ \sin\theta_i \cos\theta_i\cos\alpha_i -\cos\theta_i\sin\alpha_i a_i\sin\theta_i \ 0 \sin\alpha_i \cos\alpha_i d_i \ 0 0 0 1 \end{bmatrix} ]这个矩阵看着复杂其实结构很清晰左上3×3部分是旋转矩阵右上3×1部分是平移向量最后一行固定为[0,0,0,1]。旋转矩阵描述坐标系{i}相对{i-1}的三个轴方向平移向量描述坐标系{i}原点在{i-1}里的位置。当α_i90°时矩阵里很多项会变成0计算量会小很多这也是为什么很多机械臂关节垂直相交时DH表看起来很“清爽”。我建议初学者不要直接背这个公式而是按一下步骤自己推一遍写出Rot(z,θ)、Trans(z,d)、Trans(x,a)、Rot(x,α)四个4×4矩阵然后依次相乘。推过一次之后你就能理解矩阵里每一项是怎么来的比如第一列其实就是X_i在X_{i-1}、Y_{i-1}、Z_{i-1}三个方向上的投影。3.3 六轴正运动学方程怎么组装六轴机械臂的正运动学就是六个相邻变换矩阵连乘T_06 T_1 * T_2 * T_3 * T_4 * T_5 * T_6这个T_06就是从基座坐标系{0}到末端坐标系{6}的总变换矩阵。T_06左上3×3是末端姿态矩阵右上3×1是末端原点在基座坐标系下的位置。因为每个T_i里只有θ_i是变量其余参数是常数所以T_06本质上是六个关节角的函数。而这个“函数”长什么样就是机械臂的运动学方程。在实际工程里我们很少真的把T_06完全展开成一大堆sin、cos的多项式那样又长又容易抄错。通常直接让程序做矩阵连乘需要某个位置的表达式时再单独提取。但你要理解展开式的来历后面学逆运动学、求雅可比矩阵、做轨迹规划时很多推导都要用到“每个T_i对θ_i的偏导”这类概念。这也是我一直强调要手推一遍正运动学的原因不是为了考试是为了让后面的路好走。4. 用Python把正运动学“跑”起来4.1 手写numpy版本的DH变换工具我推荐Python配numpy。先写一个标准DH变换函数输入当前关节角θ、d、a、α返回4×4矩阵。这里有一个细节DH参数表里的角度习惯用度但numpy的三角函数默认用弧度所以函数入口先把度和弧度转换统一。import numpy as np def dh_transform(theta, d, a, alpha): 标准DH相邻连杆变换矩阵角度单位度 theta np.deg2rad(theta) alpha np.deg2rad(alpha) 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公式完全一致核心就是那三个cos、sin计算。写完这个函数后无论是四轴还是六轴正运动学就只是循环调用它。主程序里先定义DH参数表然后按顺序乘起来。我用前面那张机械臂参数表if __name__ __main__: # 参数表theta_i, d_i, a_i, alpha_i params [ (0, 0.1, 0, 90), (0, 0.0, 0.4, 0), (0, 0.0, 0.35, 0), (0, 0.1, 0, 90), (0, 0.0, 0, -90), (0, 0.1, 0, 0), ] q [0, 0, 0, 0, 0, 0] # 六关节角度单位度 T np.eye(4) for i in range(6): theta_i, d_i, a_i, alpha_i params[i] T T dh_transform(theta_i q[i], d_i, a_i, alpha_i) print(末端位置:, T[:3, 3]) print(姿态矩阵:\n, T[:3, :3])注意这里的theta_i q[i]是为了处理“零点偏移”。params里第一列是DH表里的θ_i常量偏移如果机器人在零位时关节角不是0你把这个偏移写进去再在当前读数上叠加q[i]。我给的示例里偏移都是0所以直接用q[i]也没问题。4.2 代入真实参数并验证结果运行上面这段代码会得到类似这样的输出末端位置: [0.75 -0.2 0.1] 姿态矩阵: [[1. 0. 0.] [0. 0. -1.] [0. 1. 0.]]这个结果对应什么几何含义所有关节角为0时大臂和小臂沿X方向伸直末端位置在X方向0.75米处Y方向有-0.2米的偏置Z方向高度0.1米。姿态矩阵不是单位阵而是绕X轴转了90度的效果这是因为DH表里α1和α4这些扭转角导致腕部坐标系发生了翻转。看到姿态矩阵不是单位阵很多初学同学会慌觉得是不是哪里错了。其实只要对照坐标系定义能解释通就是对的。最笨也最有效的验证方法是把某个关节角度改成90°观察末端位置是否真的绕对应轴线转了90°。比如把q设为[90,0,0,0,0,0]如果模型正确末端位置应该沿基座Z0轴旋转90°X方向的位置跑到Y方向去。如果符号反了结果会跑到负方向这时候就要回去查α或θ的符号。4.3 怎么用末端位姿反过来检查DH表正运动学写完后如何确认自己建的DH表和真实机械臂一致我一般用三个方法。第一个是量零位坐标。把机械臂手动或通过示教器移到零位所有关节角为0用尺子量末端法兰中心在基座坐标系下的坐标再和代码输出对比。如果误差在几毫米内说明DH参数基本正确。这个方法最直接但前提是你能准确找到基座坐标系原点通常我会在基座中心贴一个标记。第二个是仿真对照。很多机械臂发布过URDF模型在Gazebo或PyBullet里可以把URDF的正运动学结果当成“标准答案”。你把自己手写的DH正运动学结果和仿真里读到的末端位姿对比不一致就说明坐标系某处定义有出入。我当年调自己的六轴时就是用PyBullet逐关节对了一遍才发现关节4的α符号写反了。第三个是运动轨迹一致性。让机械臂只动某个关节比如只动关节2观察末端的运动轨迹在基座坐标系里是不是一段圆弧。如果轨迹不是圆弧说明这个关节对应的a或α参数很可能错了。这个方法在机械臂还没完全装好、只能手动掰关节时也特别好用。5. 常见问题排查与实战心得5.1 位置错得离谱先查这四处正运动学写出来后如果末端位置完全对不上不要急着改代码先按下面这张表排查现象可能原因排查方向位置完全不对姿态矩阵也乱变换顺序用错或矩阵元素抄错先固定θ0、a1、d0、α0看矩阵是不是沿X轴平移1某个关节转动后方向反了Z轴方向取反或α正负号不对在图上标出Z和X轴用右手定则复核平行关节之间位置对但夹角不对X轴取法导致a和d分配不同重选公垂线方向同步检查相邻两行参数角度值看着对但结果是乱码角度/弧度混用代码里统一用弧度或确保传入deg前先转换排查时我的习惯是先构造最简单的关节值比如让所有θ0手算一下末端位置大概在哪里再用代码算一遍。如果这段对不上问题基本出在DH表或矩阵公式上如果这一段能对上但动某个关节后不对问题大概率出在那根杆对应的坐标系方向上。5.2 奇异点、关节限位和零点偏移对运动学方程的影响正运动学本身不受关节限位影响你给任何角度都能算出一个位姿哪怕这个位姿在实际机械臂上根本到不了。所以不要用“正运动学算出来的位置能不能到达”来判断模型对不对。能不能到是你后面要做轨迹规划和碰撞检测的事。真正影响正运动学工程精度的是零点偏移。大多数机械臂用编码器测关节角但编码器读数0不一定是DH表里的θ0。如果机器人示教器显示“当前角度为0”时其实大臂已经有一个初始倾角这时候直接拿显示角度代入DH表算出来的末端位置肯定偏。所以做样机时一定要做零点标定把每个关节在机械零位时的编码器读数记录下来作为偏移量。另外腕部奇异点值得提一句。很多六轴机械臂在关节5角度接近0度时逆运动学解会出现数值不稳定正运动学本身不会爆炸但会暴露在雅可比矩阵条件数上。这虽然不是DH法的锅但如果你发现某些位姿下手算的逆解跳变很大多半是奇异点问题不是模型错误。5.3 从正运动学走向逆运动学下一步怎么走正运动学通了下一步就是逆运动学给一个末端目标位姿反解六个关节角。逆运动学有解析法和数值法两条路。解析法快而且精确但需要针对具体机械臂结构推公式不是每种构型都有解析解。数值法通用常见的是基于雅可比矩阵的迭代法、阻尼最小二乘法还有优化法。我个人的建议是如果只是做应用先用现成的库比如Python的Robotics Toolbox、PyBullet、MoveIt里的KDL或Trac-IK把逆解跑通再去研究原理。如果是要做深入研究或者想搞懂机械臂控制底层逻辑回头再把数值逆解自己写一遍写的时候你会对雅可比矩阵的几何意义有更深理解。很多初学者容易陷入一个误区觉得正运动学太简单逆运动学才厉害于是跳过正运动学疯狂研究逆解。实际上逆运动学所有计算都建立在正运动学模型之上你正运动学里一个符号错误逆解可能在某些姿态下看起来正确在某些姿态下彻底发散。我做传统六轴机械臂项目时花在标定DH参数和验证正运动学上的时间比写逆解代码的时间多得多但最后调试反而更顺利。我自己现在写机械臂代码还是会把DH表从零手写一遍哪怕后面用现成库也要拿手写结果和库结果对一下确认模型没被某个隐晦的默认约定坑到。标准DH法看起来只是一张表和几个矩阵但它决定了你后续所有运动学、动力学、轨迹规划的地基。如果你能亲手从连杆坐标系一步步推到运动学方程再花半小时把numpy版本的正运动学跑通后面遇到再复杂的机械臂心里都会很有底。
标准DH法实战：从连杆坐标系到机械臂正运动学方程
做机械臂开发的人十有八九第一次算正运动学时都被坐标系绕晕过。我头一回照着论文里的DH表建模对着一个六轴机械臂建了两天末端位置算出来差出十万八千里最后发现是Z轴方向画反了。今天这篇是机械臂速成系列的第八篇专门讲标准DH法实战从连杆坐标系的建立到运动学方程的组装一步一步带你把正运动学跑通。这套方法无论你是用AR3、UR5e、松灵Piper这类现成机械臂还是自己3D打印一个六轴来玩都是绕不开的第一道坎。文章不会有太复杂的数学推导但该有的细节和坑一个不少适合刚入门、想搞懂运动学方程到底怎么来的同学也适合已经会用现成库但没自己手推过一遍的老哥们。1. 标准DH法到底在算什么先把思路捋直1.1 一个让你事半功倍的视角连杆不是关节坐标系才是本体我先说一个大家容易犯的错误学DH法的时候满脑子都是“我这根杆多长、那个关节能转多少度”这是站在机械结构的角度看问题。但DH法真正的视角是“坐标系”。每个连杆都绑一个坐标系然后通过坐标系之间的相对变换来描述运动。什么是“绑一个坐标系”你可以把每个连杆想象成一块底板底板上画了三个互相垂直的箭头代表X轴、Y轴、Z轴。机械臂动起来的时候每个连杆的姿态变了底板上箭头指向也跟着变。运动学要回答的问题是已知每个关节转了多少度末端那块底板上的箭头相对于基座底板指向哪、原点在哪。所以机械臂正运动学本质上是一条“坐标系链”基座坐标系{0}到连杆1坐标系{1}{1}到{2}一直传到最后末端坐标系{n}。每个相邻变换只依赖四个参数也就是一张DH参数表就能把一个六轴机械臂的全部几何关系描述完。我以前总觉得连杆参数表已经够清楚了后来发现这是错觉。比如一根斜着的连杆你光知道长度和角度很难直观推出它在三维空间里的位姿但放到坐标系变换里只要按规则建系、填参数、矩阵连乘答案就很稳定。这种视角的优势是“模块化”。不管机械臂是四轴、六轴还是七轴代码里都是同一个变换函数只是参数表长度不同。当年我自己手写正运动学时靠的就是这个思路先把一根连杆的变换矩阵写对后面全是体力活。1.2 标准DH参数表四个参数说清所有几何关系标准DH法给每个相邻坐标系之间的变换准备了四个参数分别是θ_i、d_i、a_i、α_i。我习惯用“谁和谁的关系”来记而不是死背定义。以从坐标系{i-1}到坐标系{i}的变换为例θ_i绕Z_{i-1}轴从X_{i-1}转到X_i的角度通常是关节变量。d_i沿Z_{i-1}轴从X_{i-1}到X_i的距离也就是两条X轴在Z_{i-1}方向上隔了多远。a_i沿X_i轴从Z_{i-1}到Z_i的距离通常就是连杆长度。α_i绕X_i轴从Z_{i-1}转到Z_i的角度通常反映相邻两个关节轴线的扭转角。这四个参数里θ_i和d_i描述的是“绕Z轴转了多少、沿Z轴平移多少”a_i和α_i描述的是“下一个坐标系的X轴方向和旋转”。你可以把一根连杆想象成一根弯管两端各有一个轴承座a_i是弯管两头之间的直线距离α_i是两端轴承孔中心线之间的夹角d_i则是前一个关节轴方向上的错位量。为什么需要四个参数而不是两个因为三维空间里两个坐标系之间的相对关系一共有六个自由度但DH法通过精心选择坐标系的建立方式把其中两个自由度“藏”进了坐标系定义规则里。剩下的四个参数足以完整描述相邻连杆关系这也是DH法能被广泛使用的根本原因。1.3 标准DH和改进DH别等建完坐标系才发现表错了这里必须给新手提个醒。你搜DH法会看到标准DH也叫经典DH和改进DH也叫Craig DH两种长得像但坐标系约定不一样。我们这次实战用的是标准DH也就是变换顺序为“绕Z旋转 → 沿Z平移 → 沿X平移 → 绕X旋转”。改进DH的顺序则是“沿X平移 → 绕X旋转 → 沿Z平移 → 绕Z旋转”。为什么会有这种区别因为坐标系放的位置不一样。标准DH把坐标系{i-1}的Z轴放在第i个关节轴线上坐标系{i}的Z轴放在第i1个关节轴线上改进DH把坐标系{i}的Z轴放在第i个关节轴线上相当于坐标系整体“向后错了一位”。这就导致同一个机械臂用标准DH和用改进DH填出来的参数表可能完全不同。对比项标准DH本文用改进DH / Craig DH坐标系放置坐标系{i-1}的Z轴沿关节i轴线坐标系{i}的Z轴沿关节i轴线变换顺序Rot(z,θ) → Trans(z,d) → Trans(x,a) → Rot(x,α)Trans(x,a) → Rot(x,α) → Trans(z,d) → Rot(z,θ)常用下标第i行写θ_i, d_i, a_i, α_i第i行写θ_i, d_i, a_{i-1}, α_{i-1}常见场景经典教材、传统工业机器人资料Craig《机器人学导论》、ROS里不少URDF模型如果你只是照着别人的机器人抄DH表一定先确认对方用的是哪一种。我在实际项目里踩过一次坑从某个开源六轴机械臂工程里拿到一张看起来很像样的DH表直接用标准DH矩阵去套结果末端旋转方向怎么调都不对后来发现那份代码用的是改进DH。所以不是“哪个DH是对的”而是“你自己的程序按哪种DH写的参数表就要匹配哪种”。2. 动手建立连杆坐标系六轴机械臂一步一步来2.1 Z轴的摆放原则建坐标系的第一步是先把每个关节的Z轴方向定下来。对于第i个旋转关节Z_{i-1}轴就沿着第i个关节的旋转轴方向。这里的方向可以自己选但选完就不要再改不然后面所有参数都会跟着乱。基座坐标系{0}的Z0一般让机器人“立起来”的方向为正也就是第一个关节竖着的转轴方向向上。比如一台典型的六轴工业机械臂底座关节绕竖直方向旋转那Z0就竖直向上。后面每个关节轴方向要根据机械结构实际姿态来画。画图时我的习惯是把机械臂侧视图先画出来标好每一个关节轴线再在这个图上加坐标系这样不容易漏。有一个常见问题末端坐标系{n}的Z轴怎么放标准DH里Z_n理论上可以任意选择因为末端之后没有下一个关节了。实际中我建议先把最后一个关节的轴线方向作为Z_{n-1}然后末端坐标系Z_n沿工具方向比如法兰中心向外这样后续加装夹爪、视觉相机时可以直接用工具坐标系思维负担小一点。2.2 X轴和原点的确定定好Z轴后X轴按公垂线规则来X_i沿Z_{i-1}与Z_i的公垂线方向从Z_{i-1}指向Z_i。所谓公垂线就是一条同时垂直两条异面直线的线段它的方向就是两条Z轴之间最短距离的方向。这个方向定了X_i的方向也就定了。如果Z_{i-1}和Z_i是相交的公垂线长度为0X_i方向通常取与两者都垂直的方向让X_i、Z_{i-1}、Z_i满足右手定则。如果Z_{i-1}和Z_i是平行的公垂线有无数条你可以任选一条但选定之后往往会影响相邻的d_i或a_i数值所以最好选一条看起来最“自然”的例如过前一个坐标系原点的那条公垂线这样很多d_i会变成0计算省事。原点位置也顺带确定了坐标系{i}的原点就是X_i轴与Z_i轴的交点。注意不是X_i和Z_{i-1}的交点而是X_i这条公垂线和Z_i的交点。这个细节我当年搞反过导致原点位置全错了。如果你发现某一行参数填完相邻变换矩阵里的平移向量和几何结构怎么也对不上大概率就是原点取错地方了。2.3 实例一个6R垂直关节型机械臂的完整建模为了实战我建一个典型6R垂直关节型机械臂的结构底座旋转然后大臂俯仰、肘部俯仰再往后是腕部旋转、腕部俯仰、末端旋转。这种构型很常见很多六轴教学机械臂和工业机械臂都是这个路子。先按前面的规则把Z轴方向捋一遍Z0竖直向上Z1水平关节2轴线Z2水平且与Z1平行关节3轴线Z3为腕部关节4轴线Z4为关节5轴线Z5为关节6轴线。按标准DH规则建立坐标系并做一定的零位约定后得到的DH参数表如下iθ_i(变量)d_i(m)a_i(m)α_i(°)1θ10.10902θ200.403θ300.3504θ40.10905θ500-906θ60.100我来逐个解释参数对应的结构含义d10.1是基座到肩关节的高度a20.4是大臂长度a30.35是小臂长度d40.1是腕部到肘部的偏置距离d60.1是末端法兰中心到工具坐标系原点的距离。α190°表示Z0到Z1轴线相互垂直α2和α3为0说明关节2、3轴线平行α490°和α5-90°描述腕部两个轴线的扭转关系。这里要说明一下我给的这组参数是简化的理想模型不一定对应某台真实机械臂的完整CAD模型。真实机械臂的DH表往往还会有许多微小偏置比如关节2和3之间的坐标系原点不一定刚好在关节轴心上但这些偏置在运动学原理上完全一样只是参数稍多一点。学习阶段先用简化模型把流程跑通后面再填真实参数就顺了。2.4 新手最容易犯的三个错误第一个错误是平行关节的X轴方向乱取。像关节2和关节3Z1∥Z2公垂线不唯一。如果你把X2的方向取反对应a2的符号也会变同时相邻行的d和θ都可能受影响。只改一半坐标就会翻转。我的建议是画好图后每个X轴方向都用叉积验一遍X_i方向必须垂直于Z_{i-1}和Z_i而且从Z_{i-1}指向Z_i别凭感觉。第二个错误是Z轴方向正反搞错。Z轴方向反了α_i的正负号就会差180°矩阵模型会有非常明显的错误。比如某个关节明明应该顺时针转末端却往逆时针跑。遇到这种问题不要先怀疑代码先回头检查Z轴画的方向。第三个错误是基座坐标系和末端坐标系没有约定好。基座坐标系{0}的方向会影响最终读到的末端位置末端坐标系{n}的方向会影响姿态矩阵怎么解读。尤其是后面要加装夹爪、相机如果没分清楚“法兰坐标系”和“工具坐标系”手眼标定的时候会被绕到怀疑人生。建议在项目一开始就定义一个统一约定比如基座坐标系X指向前方Z向上末端工具坐标系Z沿工具伸出方向。3. 从坐标系到运动学方程相邻变换矩阵的推导3.1 四次基本变换的顺序为什么不能乱标准DH的相邻连杆变换矩阵T_i可以拆成四个基本变换T_i Rot(z, θ_i) * Trans(z, d_i) * Trans(x, a_i) * Rot(x, α_i)这四个变换是“在变换后的当前坐标系下”依次执行的先绕当前Z轴旋转θ再沿当前Z轴平移d再沿当前X轴平移a最后绕当前X轴旋转α。因为矩阵乘法是右乘所以每次新变换都作用在已经变换完的坐标系上。我说一个你们肯定遇到过的类比先向右转90度再往前走两步和先往前走两步再向右转90度结果完全不一样。DH变换顺序同理。你要是把Trans(x,a)和Rot(z,θ)调换位置得到的矩阵几乎肯定不对。所以我写代码时从不用记忆的矩阵公式而是每次都用四个基础矩阵相乘来生成一是能减少公式记忆错误二是以后换成改进DH也容易改。3.2 标准DH矩阵长什么样把上面四个基本变换乘起来就得到标准DH的4×4齐次变换矩阵[ \begin{bmatrix} \cos\theta_i -\sin\theta_i\cos\alpha_i \sin\theta_i\sin\alpha_i a_i\cos\theta_i \ \sin\theta_i \cos\theta_i\cos\alpha_i -\cos\theta_i\sin\alpha_i a_i\sin\theta_i \ 0 \sin\alpha_i \cos\alpha_i d_i \ 0 0 0 1 \end{bmatrix} ]这个矩阵看着复杂其实结构很清晰左上3×3部分是旋转矩阵右上3×1部分是平移向量最后一行固定为[0,0,0,1]。旋转矩阵描述坐标系{i}相对{i-1}的三个轴方向平移向量描述坐标系{i}原点在{i-1}里的位置。当α_i90°时矩阵里很多项会变成0计算量会小很多这也是为什么很多机械臂关节垂直相交时DH表看起来很“清爽”。我建议初学者不要直接背这个公式而是按一下步骤自己推一遍写出Rot(z,θ)、Trans(z,d)、Trans(x,a)、Rot(x,α)四个4×4矩阵然后依次相乘。推过一次之后你就能理解矩阵里每一项是怎么来的比如第一列其实就是X_i在X_{i-1}、Y_{i-1}、Z_{i-1}三个方向上的投影。3.3 六轴正运动学方程怎么组装六轴机械臂的正运动学就是六个相邻变换矩阵连乘T_06 T_1 * T_2 * T_3 * T_4 * T_5 * T_6这个T_06就是从基座坐标系{0}到末端坐标系{6}的总变换矩阵。T_06左上3×3是末端姿态矩阵右上3×1是末端原点在基座坐标系下的位置。因为每个T_i里只有θ_i是变量其余参数是常数所以T_06本质上是六个关节角的函数。而这个“函数”长什么样就是机械臂的运动学方程。在实际工程里我们很少真的把T_06完全展开成一大堆sin、cos的多项式那样又长又容易抄错。通常直接让程序做矩阵连乘需要某个位置的表达式时再单独提取。但你要理解展开式的来历后面学逆运动学、求雅可比矩阵、做轨迹规划时很多推导都要用到“每个T_i对θ_i的偏导”这类概念。这也是我一直强调要手推一遍正运动学的原因不是为了考试是为了让后面的路好走。4. 用Python把正运动学“跑”起来4.1 手写numpy版本的DH变换工具我推荐Python配numpy。先写一个标准DH变换函数输入当前关节角θ、d、a、α返回4×4矩阵。这里有一个细节DH参数表里的角度习惯用度但numpy的三角函数默认用弧度所以函数入口先把度和弧度转换统一。import numpy as np def dh_transform(theta, d, a, alpha): 标准DH相邻连杆变换矩阵角度单位度 theta np.deg2rad(theta) alpha np.deg2rad(alpha) 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公式完全一致核心就是那三个cos、sin计算。写完这个函数后无论是四轴还是六轴正运动学就只是循环调用它。主程序里先定义DH参数表然后按顺序乘起来。我用前面那张机械臂参数表if __name__ __main__: # 参数表theta_i, d_i, a_i, alpha_i params [ (0, 0.1, 0, 90), (0, 0.0, 0.4, 0), (0, 0.0, 0.35, 0), (0, 0.1, 0, 90), (0, 0.0, 0, -90), (0, 0.1, 0, 0), ] q [0, 0, 0, 0, 0, 0] # 六关节角度单位度 T np.eye(4) for i in range(6): theta_i, d_i, a_i, alpha_i params[i] T T dh_transform(theta_i q[i], d_i, a_i, alpha_i) print(末端位置:, T[:3, 3]) print(姿态矩阵:\n, T[:3, :3])注意这里的theta_i q[i]是为了处理“零点偏移”。params里第一列是DH表里的θ_i常量偏移如果机器人在零位时关节角不是0你把这个偏移写进去再在当前读数上叠加q[i]。我给的示例里偏移都是0所以直接用q[i]也没问题。4.2 代入真实参数并验证结果运行上面这段代码会得到类似这样的输出末端位置: [0.75 -0.2 0.1] 姿态矩阵: [[1. 0. 0.] [0. 0. -1.] [0. 1. 0.]]这个结果对应什么几何含义所有关节角为0时大臂和小臂沿X方向伸直末端位置在X方向0.75米处Y方向有-0.2米的偏置Z方向高度0.1米。姿态矩阵不是单位阵而是绕X轴转了90度的效果这是因为DH表里α1和α4这些扭转角导致腕部坐标系发生了翻转。看到姿态矩阵不是单位阵很多初学同学会慌觉得是不是哪里错了。其实只要对照坐标系定义能解释通就是对的。最笨也最有效的验证方法是把某个关节角度改成90°观察末端位置是否真的绕对应轴线转了90°。比如把q设为[90,0,0,0,0,0]如果模型正确末端位置应该沿基座Z0轴旋转90°X方向的位置跑到Y方向去。如果符号反了结果会跑到负方向这时候就要回去查α或θ的符号。4.3 怎么用末端位姿反过来检查DH表正运动学写完后如何确认自己建的DH表和真实机械臂一致我一般用三个方法。第一个是量零位坐标。把机械臂手动或通过示教器移到零位所有关节角为0用尺子量末端法兰中心在基座坐标系下的坐标再和代码输出对比。如果误差在几毫米内说明DH参数基本正确。这个方法最直接但前提是你能准确找到基座坐标系原点通常我会在基座中心贴一个标记。第二个是仿真对照。很多机械臂发布过URDF模型在Gazebo或PyBullet里可以把URDF的正运动学结果当成“标准答案”。你把自己手写的DH正运动学结果和仿真里读到的末端位姿对比不一致就说明坐标系某处定义有出入。我当年调自己的六轴时就是用PyBullet逐关节对了一遍才发现关节4的α符号写反了。第三个是运动轨迹一致性。让机械臂只动某个关节比如只动关节2观察末端的运动轨迹在基座坐标系里是不是一段圆弧。如果轨迹不是圆弧说明这个关节对应的a或α参数很可能错了。这个方法在机械臂还没完全装好、只能手动掰关节时也特别好用。5. 常见问题排查与实战心得5.1 位置错得离谱先查这四处正运动学写出来后如果末端位置完全对不上不要急着改代码先按下面这张表排查现象可能原因排查方向位置完全不对姿态矩阵也乱变换顺序用错或矩阵元素抄错先固定θ0、a1、d0、α0看矩阵是不是沿X轴平移1某个关节转动后方向反了Z轴方向取反或α正负号不对在图上标出Z和X轴用右手定则复核平行关节之间位置对但夹角不对X轴取法导致a和d分配不同重选公垂线方向同步检查相邻两行参数角度值看着对但结果是乱码角度/弧度混用代码里统一用弧度或确保传入deg前先转换排查时我的习惯是先构造最简单的关节值比如让所有θ0手算一下末端位置大概在哪里再用代码算一遍。如果这段对不上问题基本出在DH表或矩阵公式上如果这一段能对上但动某个关节后不对问题大概率出在那根杆对应的坐标系方向上。5.2 奇异点、关节限位和零点偏移对运动学方程的影响正运动学本身不受关节限位影响你给任何角度都能算出一个位姿哪怕这个位姿在实际机械臂上根本到不了。所以不要用“正运动学算出来的位置能不能到达”来判断模型对不对。能不能到是你后面要做轨迹规划和碰撞检测的事。真正影响正运动学工程精度的是零点偏移。大多数机械臂用编码器测关节角但编码器读数0不一定是DH表里的θ0。如果机器人示教器显示“当前角度为0”时其实大臂已经有一个初始倾角这时候直接拿显示角度代入DH表算出来的末端位置肯定偏。所以做样机时一定要做零点标定把每个关节在机械零位时的编码器读数记录下来作为偏移量。另外腕部奇异点值得提一句。很多六轴机械臂在关节5角度接近0度时逆运动学解会出现数值不稳定正运动学本身不会爆炸但会暴露在雅可比矩阵条件数上。这虽然不是DH法的锅但如果你发现某些位姿下手算的逆解跳变很大多半是奇异点问题不是模型错误。5.3 从正运动学走向逆运动学下一步怎么走正运动学通了下一步就是逆运动学给一个末端目标位姿反解六个关节角。逆运动学有解析法和数值法两条路。解析法快而且精确但需要针对具体机械臂结构推公式不是每种构型都有解析解。数值法通用常见的是基于雅可比矩阵的迭代法、阻尼最小二乘法还有优化法。我个人的建议是如果只是做应用先用现成的库比如Python的Robotics Toolbox、PyBullet、MoveIt里的KDL或Trac-IK把逆解跑通再去研究原理。如果是要做深入研究或者想搞懂机械臂控制底层逻辑回头再把数值逆解自己写一遍写的时候你会对雅可比矩阵的几何意义有更深理解。很多初学者容易陷入一个误区觉得正运动学太简单逆运动学才厉害于是跳过正运动学疯狂研究逆解。实际上逆运动学所有计算都建立在正运动学模型之上你正运动学里一个符号错误逆解可能在某些姿态下看起来正确在某些姿态下彻底发散。我做传统六轴机械臂项目时花在标定DH参数和验证正运动学上的时间比写逆解代码的时间多得多但最后调试反而更顺利。我自己现在写机械臂代码还是会把DH表从零手写一遍哪怕后面用现成库也要拿手写结果和库结果对一下确认模型没被某个隐晦的默认约定坑到。标准DH法看起来只是一张表和几个矩阵但它决定了你后续所有运动学、动力学、轨迹规划的地基。如果你能亲手从连杆坐标系一步步推到运动学方程再花半小时把numpy版本的正运动学跑通后面遇到再复杂的机械臂心里都会很有底。