上个月调一台六轴机器人示教器上显示的末端坐标和仿真软件里算出来的结果差了将近80毫米。一开始怀疑机械装配有问题量了各处连杆长度都对又怀疑编码器零点跑偏重新校了一遍折腾到下午才反应过来问题出在正解——控制器里用的DH参数表和我仿真用的根本不是同一套约定。这类问题在六轴机器人开发里太常见了。正解看着简单就是把六个关节角代进去算出末端执行器的位置和姿态。但真正落地的时候坐标系定义、参数下标、零位约定、姿态表达方式每一个细节都容易翻车。这篇文章我打算把六轴机器人运动学正解从原理到代码再到验证完整梳理一遍给正在做机器人控制、离线编程或者上位机坐标显示的朋友做个参考。不管你是刚接触运动学还是已经写过正解但被各种坑折磨过这篇应该都能帮上忙。1. 正解到底解决什么问题1.1 示教器上的坐标从哪来要让六轴机器人正常工作控制器需要实时知道末端执行器在空间中的位置。每个关节都装了编码器但编码器只能告诉你“这个关节相对于零位转了多少度”不能直接告诉你“焊枪尖在空间哪个点”。正解就是这中间那座桥输入六个关节角度输出末端在基坐标系下的位姿。实际项目里正解至少承担三件事。第一示教器上的XYZ坐标显示靠的就是实时正解。操作者在示教器上移动机器人看到的位置数据不是某个传感器测出来的而是控制器根据编码器角度实时算出来的。如果正解写错示教器上显示的位置就是错的操作者对着错误坐标示教后果可想而知。第二直线和圆弧插补依赖正解。做直线运动时规划器要把目标直线在笛卡尔空间细分反解成各个关节的角度指令。正解在这个过程里负责确认当前点在笛卡尔空间中的实际位姿保证插补的连续性。第三离线编程和仿真验证离不开正解。在软件里拖动机器人末端软件需要根据关节角实时算出末端位置才能在三维场景里正确渲染机械臂的姿态。仿真里看着好好的程序到了真机上点位全偏十有八九是仿真用的正解和控制器用的正解不一致。所以正解不是运动学教材里一个可有可无的公式推导它是整个机器人控制系统的地基。地基歪了上面盖多少层楼都白搭。1.2 位置和姿态是两回事三个坐标不够很多刚接触机器人的朋友会问末端位置不是有x、y、z三个坐标就够了吗如果只关心“机器人末端到了哪个点”三个坐标确实够了。但机器人执行任务时不仅要“到达”一个点还要用正确的姿态到达。比如拧螺丝螺丝刀必须垂直于螺丝头顶着拧位置对了但姿态偏了照样拧不进去。所以要完整描述末端的状态需要两个部分位置末端在基坐标系下的x、y、z坐标姿态末端坐标系相对于基坐标系的旋转关系把位置和姿态合在一起称为“位姿”。工程上最常用的表达方式就是4×4齐次变换矩阵| R11 R12 R13 px | T | R21 R22 R23 py | | R31 R32 R33 pz | | 0 0 0 1 |左上角3×3的旋转矩阵R表示末端坐标系在基坐标系中的朝向右上角3×1的平移向量p表示末端坐标系原点在基坐标系中的位置。最后一行固定是0001这是齐次坐标的规矩为了把旋转和平移统一成一次矩阵乘法。齐次变换矩阵的好处是多个坐标系之间的变换可以通过矩阵连乘直接串联起来。比如从基座到关节1的变换乘以从关节1到关节2的变换得到的就是基座到关节2的变换。这个性质是后面推导六轴正解的核心工具。1.3 正解和逆解的边界机器人运动学里还有另一个概念叫逆解也就是给定目标位姿反推六个关节角。正解和逆解的关系经常被人搞混简单对比一下对比项正解逆解输入六个关节角末端目标位姿输出末端位姿六个关节角解的数量唯一可能多组解或无数解或无解计算难度简单固定流程连乘复杂需要解方程组或数值迭代正解是确定性的给定一组关节角输出的位姿是唯一的没有任何歧义。逆解就麻烦得多同一个末端位姿六轴机器人可能对应好几组关节角组合需要根据能量最小、避障、限位等条件去筛选。虽然逆解听起来更高级更复杂但正解是基础中的基础。正解的模型如果不准确逆解的输入就错了后面所有规划和控制全是空中楼阁。2. DH参数把机械结构翻译成数学语言2.1 四个参数背后的物理含义要用数学描述六轴机器人的运动学关系需要一个标准化的方法把机械结构“翻译”成参数。最常用的就是DH参数法由Denavit和Hartenberg在1955年提出。DH参数法的核心思路是在每一个连杆上固定一个坐标系然后用四个参数描述相邻两个坐标系之间的变换关系。四个参数分别是a连杆长度沿x轴方向从z轴到下一个z轴的平移距离。可以理解为相邻两个关节轴线之间的公垂线长度。alpha连杆扭转角绕x轴从z轴旋转到下一个z轴的角度。描述相邻关节轴线在空间中的夹角。d连杆偏距沿z轴方向从x轴到下一个x轴的平移距离可以理解为两个连杆沿关节轴线方向错开的距离。theta关节角绕z轴从x轴旋转到下一个x轴的角度。对于旋转关节theta是变量其他三个参数是常量。用生活化的方式理解想象两扇门装在同一面墙上a就是两扇门轴之间的水平距离alpha描述两扇门轴是否平行如果门轴都竖直alpha就是0d描述两扇门轴在竖直方向上的错位theta就是每扇门实际打开的角度。实际建模时从机器人的机械图纸或三维模型上可以直接量出这些尺寸。但要注意测量之前必须先明确坐标系怎么建否则量出来的数值没有意义。2.2 标准DH和改进DH一念之差天壤之别DH参数法有两个版本标准DHStandard DH和改进DHModified DH。这两个版本常被混用也是我在开头说的那个80毫米误差的罪魁祸首。它们的共同点是都用四个参数但坐标系放置的位置不同。标准DH把坐标系i固定在连杆i的末端改进DH把坐标系i固定在关节i1上也就是靠近下一个关节的位置。这个差别导致参数下标含义完全不同标准DH用的是a_i和alpha_i改进DH用的是a_{i-1}和alpha_{i-1}。邻接变换的推导顺序也不一样。标准DH是从当前z轴绕到下一个z轴改进DH是先绕z转theta、沿z平移d再沿x平移a、绕x转alpha。对比项标准DH改进DH坐标系固定位置连杆远端连杆近端a和alpha下标ii-1变换顺序Rot(z,θ) → Trans(z,d) → Trans(x,a) → Rot(x,α)相同常见应用场景传统教材、部分老控制器URDF、多数现代仿真软件麻烦在于很多资料拿到手根本不写用的是哪个版本。你看一张参数表只有a、alpha、d、theta四列数字如果不知道是标准DH还是改进DH算出来的结果可能完全对不上。这两个版本之间其实可以互相转换但转换规则并不简单实际开发中建议直接确认参数来源而不是靠猜。一个实用的建议拿到机器人的URDF文件或者厂商标注的DH表先搞清楚对方用的是什么约定。如果是自己建模型统一用改进DH因为现在主流的仿真软件和开源运动学库对改进DH支持更好。2.3 本文演示用的参数表下面给出我这台垂直六关节测试样机的改进DH参数表后面所有推导、代码和验证都以这张表为准ia_{i-1} (mm)alpha_{i-1} (°)d_i (mm)theta_i 初始 (°)10040002200-900-9036000004120-90650050900060-901000注意这里的“theta_i 初始”指的是机械臂处于机械零位时该关节的theta角度值。比如关节2在零位时theta2的初始值是-90度。实际运行中输入给正解的theta theta初始 编码器相对零位的角度变化。这一点很重要很多人在这一步把初始偏移漏掉了结果整个位姿全偏。3. 一步一步推导从关节角到位姿矩阵3.1 相邻连杆的通用变换矩阵有了DH参数表就可以写出相邻两个坐标系之间的齐次变换矩阵。以改进DH为例从坐标系i-1到坐标系i的变换矩阵A_i可以拆成四步绕当前z轴旋转theta_i沿当前z轴平移d_i沿新的x轴平移a_{i-1}绕新的x轴旋转alpha_{i-1}把四步写成矩阵并连乘得到通用的A_i矩阵| cosθ -sinθ·cosα sinθ·sinα a·cosθ | A | sinθ cosθ·cosα -cosθ·sinα a·sinθ | | 0 sinα cosα d | | 0 0 0 1 |其中θ、d、a、α分别对应DH表中的theta_i、d_i、a_{i-1}、alpha_{i-1}。这一步的推导过程本身并不复杂但很容易在细节上出错。比如矩阵第三行第二列到底是sinα还是负的sinα取决于你的旋转方向和变换顺序。不同资料里可能出现符号上的差异建议自己动手完整推导一遍再对照后面的代码验证确认自己用的是哪一版公式。实际工作中我不太推荐手写这个通式因为太容易抄错。用代码实现时直接按照旋转平移的四个步骤依次乘出来逻辑更清晰也不容易出错。3.2 六次连乘完整正解六轴机器人有六个关节从基座到末端法兰一共要经过六次相邻坐标系的变换。正解就是把六个A矩阵依次连乘起来T_0^6 A1 · A2 · A3 · A4 · A5 · A6这里的T_0^6就是末端法兰坐标系相对于基座坐标系的位姿。为了不过度抽象拿上一节的参数表演示一个中间过程。当机械臂处于零位也就是关节角为[0, -90°, 0, 0, 0, 0]时前两个关节的变换矩阵相乘之后得到从基座到关节2末端的位姿| 0 0 1 0 | T | -1 0 0 -200 | | 0 -1 0 400 | | 0 0 0 1 |这个中间矩阵的含义是关节2末端坐标系的原点在基座坐标系下位于(0, -200, 400)并且坐标系的朝向已经发生了明显旋转。后面每乘一个A矩阵末端坐标系就沿着机械臂的结构往前推进一段。把这六个矩阵全部乘完就得到最终末端法兰的位姿矩阵。这一步连乘的维度不大4×4矩阵相乘而已完全可以手算但比较繁琐。工程上当然是用程序来做也就是下一章要讲的代码实现。3.3 从旋转矩阵到欧拉角旋转矩阵不是给人类看的正解输出的是4×4齐次变换矩阵但矩阵里的数字对人类来说太不直观了。你在示教器上看到的通常是XYZ坐标加上RPY姿态角也就是绕固定坐标系的rollX轴旋转、pitchY轴旋转、yawZ轴旋转三个角度。从旋转矩阵提取RPY角的标准公式如下pitch atan2(-R[2][0], sqrt(R[0][0]^2 R[1][0]^2)) roll atan2(R[2][1], R[2][2]) yaw atan2(R[1][0], R[0][0])需要注意的是这个公式对应的是“先绕X轴转roll再绕Y轴转pitch最后绕Z轴转yaw”的固定轴约定等价于ZYX顺序的欧拉角内旋。不同控制器可能采用不同的约定有的用ZYZ有的用ZYX提取公式也会不一样。写代码之前先确认示教器上显示的角度是按什么约定算出来的。拿前面的零位位姿矩阵验证一下。零位时的旋转矩阵是| 0 -1 0 | R | -1 0 0 | | 0 0 -1 |代入提取公式可以得到RPY约为[180°, 0°, -90°]。你会发现这个角度和[ -180°, 0°, 90°]描述的是同一个姿态。这就是欧拉角的等价性问题同一个姿态可以对应多种角度组合。后面在对比示教器显示和自算结果时如果差180度或者差360度先别急着觉得代码错了很可能是等价角度表达的问题。4. 代码落地用Python实现正解4.1 核心函数运动学正解的代码实现非常简洁数学原理清楚了代码就是翻译一遍。下面是我调试用的Python版本依赖numpy做矩阵运算。核心就两个函数一个构造单连杆的变换矩阵一个把六个变换矩阵连乘起来。import numpy as np def dh_transform(theta_deg, d, a, alpha_deg): 构造改进DH的相邻连杆变换矩阵。 theta_deg和alpha_deg输入角度制内部转弧度。 theta np.deg2rad(theta_deg) alpha np.deg2rad(alpha_deg) ct np.cos(theta) st np.sin(theta) ca np.cos(alpha) sa 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] ]) def forward_kinematics(joints_deg, dh_params): 六轴正解。dh_params是[(d, a, alpha_deg), ...]按关节1到6排列。 T np.eye(4) for i in range(6): T T dh_transform(joints_deg[i], dh_params[i][0], dh_params[i][1], dh_params[i][2]) return T # 按之前的参数表定义 dh_params [ (400, 0, 0), # i1, a00, alpha00, d1400 ( 0, 200, -90), # i2, a1200, alpha1-90, d20 ( 0, 600, 0), # i3, a2600, alpha20, d30 (650, 120, -90), # i4, a3120, alpha3-90, d4650 ( 0, 0, 90), # i5, a40, alpha490, d50 (100, 0, -90), # i6, a50, alpha5-90, d6100 ] joints [0, -90, 0, 0, 0, 0] T forward_kinematics(joints, dh_params) print(T)运行结果[[ 0. -1. 0. 750.] [ -1. 0. 0. -920.] [ 0. 0. -1. 400.] [ 0. 0. 0. 1.]]这个结果和我前面手工推导的中间过程、以及最终位姿是完全吻合的。平移向量(750, -920, 400)就是零位时末端法兰在基坐标系下的位置。代码里有个容易被忽视的细节dh_params的排列顺序必须跟关节顺序一一对应。如果你拿到一张DH表里面每一行的i是从1到6那直接按顺序填就行。但有些表格的排列顺序是从基座往外有的厂商喜欢把某些行拆开标注要仔细核对。4.2 验证一零位计算写完正解函数第一件事就是验证它算得对不对。最快的验证方法就是零位测试把机器人打到机械零位记录此时的编码器值换算成关节角代入正解。对上面这个参数表零位时关节角是[0, -90, 0, 0, 0, 0]输出位置是(750, -920, 400)。这个结果应该和实际机器人在零位时用量具直接量出的法兰中心位置一致。三坐标测量仪或者激光跟踪仪精度最高但日常调试不一定有这条件。我的做法是把机器人在零位姿势下停住用卷尺量末端法兰中心相对于基座坐标系三个方向的距离。基座坐标系的原点通常定义在底座安装面中心x轴朝前y轴朝左z轴朝上。量出来的数值和正解输出的位置对得上就说明至少模型的几何尺寸是对的。4.3 验证二关节一旋转90度零位测试只能验证参数表的大方向还不能完全确认旋转轴方向定义正确。我一般再做一步单轴旋转验证在零位基础上只让关节1转动90度其他关节不动。关节角变为[90, -90, 0, 0, 0, 0]。从几何直觉上关节1是绕基座的z轴旋转的所以末端位置应该从(750, -920, 400)绕z轴转90度。按右手定则z轴正方向朝上绕z转90度后x变为-yy变为x所以新位置应该是(920, 750, 400)。把代码里joints改成[90, -90, 0, 0, 0, 0]如果输出的位置是(920, 750, 400)说明关节1的旋转方向和z轴正方向的约定一致。如果输出的是(-920, -750, 400)说明旋转方向反了可能需要把z轴方向取反或者把theta1的定义改成取反。这个验证方法非常实用。六个关节可以挨个测一遍每次只转一个关节观察末端位置的变化是否符合绕该关节z轴的圆弧运动。这个测试能在半小时内把整个正解模型的轴方向、参数符号全部验证完比直接上复杂位姿靠谱得多。5. 那些写代码时最容易翻车的细节5.1 标准DH和改进DH参数混用这是我在开头说的那个80毫米误差的直接原因。当时我从一个供应商资料里拿到一份参数表看起来规规矩矩但没有标注是标准DH还是改进DH。我按照常用的改进DH公式写好了正解在仿真软件里验证通过结果一上真机就偏了。排查过程是这样的先怀疑编码器零点重新标定了好几遍没用。又用激光跟踪仪量了末端位置发现不管怎么动误差都在固定方向而且大小跟某个关节的角度强相关。后来把参数表拿到另一个同事面前他一眼看出里面的alpha符号排列跟常见改进DH表不一样这才发现供应商用的是标准DH。标准DH和改进DH的关键差异我已经在第二章详述了。这里给出排查建议拿到任何DH表先在零位用直尺卷尺量一下末端位置再单轴旋转验证方向。如果方向和位置都对不上优先怀疑DH版本搞错了。还有一个更隐蔽的问题即便明确了DH版本不同文献对alpha的正负号定义也可能不同。机械臂的关节旋转方向控制器里往往有自己的约定。多花半小时做单轴验证绝对值得。5.2 零点偏移与theta_offset另一个高频翻车点是theta的零点偏移。前面参数表里的“theta_i初始”列已经体现了这个问题机械臂机械零位时不是所有关节的theta都是0。比如我这个样机的关节2零位时theta2等于-90度。如果你把这个初始偏移忘了加直接用编码器读数当成theta那么末端位置会整体旋转一个大角度误差大得离谱。控制系统里通常这样处理theta实际 theta初始 编码器相对零位的角度增量编码器的绝对位置也好相对位置也好最终都要映射到这样一个theta实际值上再送去正解计算。这里建议在代码层面对six个关节统一做这个映射不要散落在各个调用点。我在调试中还遇到过更隐蔽的情况某个关节的theta初始值其实不是固定常数而是跟安装方式有关。比如关节3和关节5在某些机器人上存在联动或者耦合结构运动学模型里要做额外的修正。碰到这类结构光看DH表不够还得多留意厂家技术文档里的补充说明。5.3 姿态提取的奇异和跳变前面提到过从旋转矩阵提取RPY角会遇到等价角的问题。还有个更麻烦的情况是奇异位形当pitch接近±90度时roll和yaw的计算会出现退化表现为角度值发生剧烈跳变。物理上这个现象不难理解。想象一架飞机当机头垂直朝上时机身绕垂直轴的旋转和绕自身纵轴的旋转产生了耦合你没法区分到底是哪个角在变。机器人腕部也有类似的位置姿态解算在这个位置会出现数值不稳定。这不是正解算错了而是欧拉角表示方法的固有问题。处理办法有三个第一在内部计算和存储中尽量使用4×4矩阵或四元数只在显示界面转换成欧拉角。这样即使临时显示跳变也不会影响控制逻辑。第二如果一定要用欧拉角可以在上层做角度连续化处理比如把相邻两次采样的角度差限制在±180度以内超过就加上或减去360度。第三判断pitch是否进入奇异区间比如cos(pitch)的绝对值小于某个阈值直接跳到等价的非奇异角度组合去显示。实际调试中我发现姿态跳变最容易出现在示教器显示环节让人误以为机器人突然抖了一下。其实机器人运动是正常的只是角度表示在跳变。所以你要是看到示教器上姿态角突然从90度跳到-270度先别慌先检查是不是奇异区间的问题。5.4 工具坐标系你算的是法兰还是针尖最后说一个概念性的大坑正解算出来的位姿默认是指末端法兰盘中心的位姿。但实际干活的时候你关心的是焊丝尖、吸盘口、夹爪指尖在哪里。法兰中心和工具尖之间通常存在几十到几百毫米的偏移如果直接把法兰位姿当成工具位姿用误差是必然的。正确做法是在正解的结果上再乘一个工具坐标变换T_tool T_flange · T_flange_tool其中T_flange_tool是工具坐标系相对于法兰坐标系的齐次变换矩阵描述了工具尖点在法兰坐标系下的位置和姿态。这个矩阵可以通过TCP标定得到也可以用游标卡尺直接量尺寸后计算。调试现场最典型的翻车场景就是示教器显示的坐标系一直是法兰中心程序里却拿这个坐标去和工件模型比对结果整个程序点位全偏移。你以为是正解算错了其实模型和输出的对象不匹配。我自己现在的习惯是正解函数输出统一停在法兰坐标系工具坐标变换单独做一个接口。这样逻辑清晰换工具时只需要改一个矩阵不需要动正解本体。先把这个分层做好再谈精度和其他模块。实际用下来正解这个模块在整个机器人控制系统里看着不起眼但几乎所有上层功能都建立在它上面。每次换一台新机型、换一套DH参数我都建议把零位验证和单轴旋转验证完整跑一遍半小时能省下后面几天的排错时间。这些小习惯比背一百个公式都管用。
六轴机器人运动学正解完整指南：DH参数、原理到代码实现与验证
上个月调一台六轴机器人示教器上显示的末端坐标和仿真软件里算出来的结果差了将近80毫米。一开始怀疑机械装配有问题量了各处连杆长度都对又怀疑编码器零点跑偏重新校了一遍折腾到下午才反应过来问题出在正解——控制器里用的DH参数表和我仿真用的根本不是同一套约定。这类问题在六轴机器人开发里太常见了。正解看着简单就是把六个关节角代进去算出末端执行器的位置和姿态。但真正落地的时候坐标系定义、参数下标、零位约定、姿态表达方式每一个细节都容易翻车。这篇文章我打算把六轴机器人运动学正解从原理到代码再到验证完整梳理一遍给正在做机器人控制、离线编程或者上位机坐标显示的朋友做个参考。不管你是刚接触运动学还是已经写过正解但被各种坑折磨过这篇应该都能帮上忙。1. 正解到底解决什么问题1.1 示教器上的坐标从哪来要让六轴机器人正常工作控制器需要实时知道末端执行器在空间中的位置。每个关节都装了编码器但编码器只能告诉你“这个关节相对于零位转了多少度”不能直接告诉你“焊枪尖在空间哪个点”。正解就是这中间那座桥输入六个关节角度输出末端在基坐标系下的位姿。实际项目里正解至少承担三件事。第一示教器上的XYZ坐标显示靠的就是实时正解。操作者在示教器上移动机器人看到的位置数据不是某个传感器测出来的而是控制器根据编码器角度实时算出来的。如果正解写错示教器上显示的位置就是错的操作者对着错误坐标示教后果可想而知。第二直线和圆弧插补依赖正解。做直线运动时规划器要把目标直线在笛卡尔空间细分反解成各个关节的角度指令。正解在这个过程里负责确认当前点在笛卡尔空间中的实际位姿保证插补的连续性。第三离线编程和仿真验证离不开正解。在软件里拖动机器人末端软件需要根据关节角实时算出末端位置才能在三维场景里正确渲染机械臂的姿态。仿真里看着好好的程序到了真机上点位全偏十有八九是仿真用的正解和控制器用的正解不一致。所以正解不是运动学教材里一个可有可无的公式推导它是整个机器人控制系统的地基。地基歪了上面盖多少层楼都白搭。1.2 位置和姿态是两回事三个坐标不够很多刚接触机器人的朋友会问末端位置不是有x、y、z三个坐标就够了吗如果只关心“机器人末端到了哪个点”三个坐标确实够了。但机器人执行任务时不仅要“到达”一个点还要用正确的姿态到达。比如拧螺丝螺丝刀必须垂直于螺丝头顶着拧位置对了但姿态偏了照样拧不进去。所以要完整描述末端的状态需要两个部分位置末端在基坐标系下的x、y、z坐标姿态末端坐标系相对于基坐标系的旋转关系把位置和姿态合在一起称为“位姿”。工程上最常用的表达方式就是4×4齐次变换矩阵| R11 R12 R13 px | T | R21 R22 R23 py | | R31 R32 R33 pz | | 0 0 0 1 |左上角3×3的旋转矩阵R表示末端坐标系在基坐标系中的朝向右上角3×1的平移向量p表示末端坐标系原点在基坐标系中的位置。最后一行固定是0001这是齐次坐标的规矩为了把旋转和平移统一成一次矩阵乘法。齐次变换矩阵的好处是多个坐标系之间的变换可以通过矩阵连乘直接串联起来。比如从基座到关节1的变换乘以从关节1到关节2的变换得到的就是基座到关节2的变换。这个性质是后面推导六轴正解的核心工具。1.3 正解和逆解的边界机器人运动学里还有另一个概念叫逆解也就是给定目标位姿反推六个关节角。正解和逆解的关系经常被人搞混简单对比一下对比项正解逆解输入六个关节角末端目标位姿输出末端位姿六个关节角解的数量唯一可能多组解或无数解或无解计算难度简单固定流程连乘复杂需要解方程组或数值迭代正解是确定性的给定一组关节角输出的位姿是唯一的没有任何歧义。逆解就麻烦得多同一个末端位姿六轴机器人可能对应好几组关节角组合需要根据能量最小、避障、限位等条件去筛选。虽然逆解听起来更高级更复杂但正解是基础中的基础。正解的模型如果不准确逆解的输入就错了后面所有规划和控制全是空中楼阁。2. DH参数把机械结构翻译成数学语言2.1 四个参数背后的物理含义要用数学描述六轴机器人的运动学关系需要一个标准化的方法把机械结构“翻译”成参数。最常用的就是DH参数法由Denavit和Hartenberg在1955年提出。DH参数法的核心思路是在每一个连杆上固定一个坐标系然后用四个参数描述相邻两个坐标系之间的变换关系。四个参数分别是a连杆长度沿x轴方向从z轴到下一个z轴的平移距离。可以理解为相邻两个关节轴线之间的公垂线长度。alpha连杆扭转角绕x轴从z轴旋转到下一个z轴的角度。描述相邻关节轴线在空间中的夹角。d连杆偏距沿z轴方向从x轴到下一个x轴的平移距离可以理解为两个连杆沿关节轴线方向错开的距离。theta关节角绕z轴从x轴旋转到下一个x轴的角度。对于旋转关节theta是变量其他三个参数是常量。用生活化的方式理解想象两扇门装在同一面墙上a就是两扇门轴之间的水平距离alpha描述两扇门轴是否平行如果门轴都竖直alpha就是0d描述两扇门轴在竖直方向上的错位theta就是每扇门实际打开的角度。实际建模时从机器人的机械图纸或三维模型上可以直接量出这些尺寸。但要注意测量之前必须先明确坐标系怎么建否则量出来的数值没有意义。2.2 标准DH和改进DH一念之差天壤之别DH参数法有两个版本标准DHStandard DH和改进DHModified DH。这两个版本常被混用也是我在开头说的那个80毫米误差的罪魁祸首。它们的共同点是都用四个参数但坐标系放置的位置不同。标准DH把坐标系i固定在连杆i的末端改进DH把坐标系i固定在关节i1上也就是靠近下一个关节的位置。这个差别导致参数下标含义完全不同标准DH用的是a_i和alpha_i改进DH用的是a_{i-1}和alpha_{i-1}。邻接变换的推导顺序也不一样。标准DH是从当前z轴绕到下一个z轴改进DH是先绕z转theta、沿z平移d再沿x平移a、绕x转alpha。对比项标准DH改进DH坐标系固定位置连杆远端连杆近端a和alpha下标ii-1变换顺序Rot(z,θ) → Trans(z,d) → Trans(x,a) → Rot(x,α)相同常见应用场景传统教材、部分老控制器URDF、多数现代仿真软件麻烦在于很多资料拿到手根本不写用的是哪个版本。你看一张参数表只有a、alpha、d、theta四列数字如果不知道是标准DH还是改进DH算出来的结果可能完全对不上。这两个版本之间其实可以互相转换但转换规则并不简单实际开发中建议直接确认参数来源而不是靠猜。一个实用的建议拿到机器人的URDF文件或者厂商标注的DH表先搞清楚对方用的是什么约定。如果是自己建模型统一用改进DH因为现在主流的仿真软件和开源运动学库对改进DH支持更好。2.3 本文演示用的参数表下面给出我这台垂直六关节测试样机的改进DH参数表后面所有推导、代码和验证都以这张表为准ia_{i-1} (mm)alpha_{i-1} (°)d_i (mm)theta_i 初始 (°)10040002200-900-9036000004120-90650050900060-901000注意这里的“theta_i 初始”指的是机械臂处于机械零位时该关节的theta角度值。比如关节2在零位时theta2的初始值是-90度。实际运行中输入给正解的theta theta初始 编码器相对零位的角度变化。这一点很重要很多人在这一步把初始偏移漏掉了结果整个位姿全偏。3. 一步一步推导从关节角到位姿矩阵3.1 相邻连杆的通用变换矩阵有了DH参数表就可以写出相邻两个坐标系之间的齐次变换矩阵。以改进DH为例从坐标系i-1到坐标系i的变换矩阵A_i可以拆成四步绕当前z轴旋转theta_i沿当前z轴平移d_i沿新的x轴平移a_{i-1}绕新的x轴旋转alpha_{i-1}把四步写成矩阵并连乘得到通用的A_i矩阵| cosθ -sinθ·cosα sinθ·sinα a·cosθ | A | sinθ cosθ·cosα -cosθ·sinα a·sinθ | | 0 sinα cosα d | | 0 0 0 1 |其中θ、d、a、α分别对应DH表中的theta_i、d_i、a_{i-1}、alpha_{i-1}。这一步的推导过程本身并不复杂但很容易在细节上出错。比如矩阵第三行第二列到底是sinα还是负的sinα取决于你的旋转方向和变换顺序。不同资料里可能出现符号上的差异建议自己动手完整推导一遍再对照后面的代码验证确认自己用的是哪一版公式。实际工作中我不太推荐手写这个通式因为太容易抄错。用代码实现时直接按照旋转平移的四个步骤依次乘出来逻辑更清晰也不容易出错。3.2 六次连乘完整正解六轴机器人有六个关节从基座到末端法兰一共要经过六次相邻坐标系的变换。正解就是把六个A矩阵依次连乘起来T_0^6 A1 · A2 · A3 · A4 · A5 · A6这里的T_0^6就是末端法兰坐标系相对于基座坐标系的位姿。为了不过度抽象拿上一节的参数表演示一个中间过程。当机械臂处于零位也就是关节角为[0, -90°, 0, 0, 0, 0]时前两个关节的变换矩阵相乘之后得到从基座到关节2末端的位姿| 0 0 1 0 | T | -1 0 0 -200 | | 0 -1 0 400 | | 0 0 0 1 |这个中间矩阵的含义是关节2末端坐标系的原点在基座坐标系下位于(0, -200, 400)并且坐标系的朝向已经发生了明显旋转。后面每乘一个A矩阵末端坐标系就沿着机械臂的结构往前推进一段。把这六个矩阵全部乘完就得到最终末端法兰的位姿矩阵。这一步连乘的维度不大4×4矩阵相乘而已完全可以手算但比较繁琐。工程上当然是用程序来做也就是下一章要讲的代码实现。3.3 从旋转矩阵到欧拉角旋转矩阵不是给人类看的正解输出的是4×4齐次变换矩阵但矩阵里的数字对人类来说太不直观了。你在示教器上看到的通常是XYZ坐标加上RPY姿态角也就是绕固定坐标系的rollX轴旋转、pitchY轴旋转、yawZ轴旋转三个角度。从旋转矩阵提取RPY角的标准公式如下pitch atan2(-R[2][0], sqrt(R[0][0]^2 R[1][0]^2)) roll atan2(R[2][1], R[2][2]) yaw atan2(R[1][0], R[0][0])需要注意的是这个公式对应的是“先绕X轴转roll再绕Y轴转pitch最后绕Z轴转yaw”的固定轴约定等价于ZYX顺序的欧拉角内旋。不同控制器可能采用不同的约定有的用ZYZ有的用ZYX提取公式也会不一样。写代码之前先确认示教器上显示的角度是按什么约定算出来的。拿前面的零位位姿矩阵验证一下。零位时的旋转矩阵是| 0 -1 0 | R | -1 0 0 | | 0 0 -1 |代入提取公式可以得到RPY约为[180°, 0°, -90°]。你会发现这个角度和[ -180°, 0°, 90°]描述的是同一个姿态。这就是欧拉角的等价性问题同一个姿态可以对应多种角度组合。后面在对比示教器显示和自算结果时如果差180度或者差360度先别急着觉得代码错了很可能是等价角度表达的问题。4. 代码落地用Python实现正解4.1 核心函数运动学正解的代码实现非常简洁数学原理清楚了代码就是翻译一遍。下面是我调试用的Python版本依赖numpy做矩阵运算。核心就两个函数一个构造单连杆的变换矩阵一个把六个变换矩阵连乘起来。import numpy as np def dh_transform(theta_deg, d, a, alpha_deg): 构造改进DH的相邻连杆变换矩阵。 theta_deg和alpha_deg输入角度制内部转弧度。 theta np.deg2rad(theta_deg) alpha np.deg2rad(alpha_deg) ct np.cos(theta) st np.sin(theta) ca np.cos(alpha) sa 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] ]) def forward_kinematics(joints_deg, dh_params): 六轴正解。dh_params是[(d, a, alpha_deg), ...]按关节1到6排列。 T np.eye(4) for i in range(6): T T dh_transform(joints_deg[i], dh_params[i][0], dh_params[i][1], dh_params[i][2]) return T # 按之前的参数表定义 dh_params [ (400, 0, 0), # i1, a00, alpha00, d1400 ( 0, 200, -90), # i2, a1200, alpha1-90, d20 ( 0, 600, 0), # i3, a2600, alpha20, d30 (650, 120, -90), # i4, a3120, alpha3-90, d4650 ( 0, 0, 90), # i5, a40, alpha490, d50 (100, 0, -90), # i6, a50, alpha5-90, d6100 ] joints [0, -90, 0, 0, 0, 0] T forward_kinematics(joints, dh_params) print(T)运行结果[[ 0. -1. 0. 750.] [ -1. 0. 0. -920.] [ 0. 0. -1. 400.] [ 0. 0. 0. 1.]]这个结果和我前面手工推导的中间过程、以及最终位姿是完全吻合的。平移向量(750, -920, 400)就是零位时末端法兰在基坐标系下的位置。代码里有个容易被忽视的细节dh_params的排列顺序必须跟关节顺序一一对应。如果你拿到一张DH表里面每一行的i是从1到6那直接按顺序填就行。但有些表格的排列顺序是从基座往外有的厂商喜欢把某些行拆开标注要仔细核对。4.2 验证一零位计算写完正解函数第一件事就是验证它算得对不对。最快的验证方法就是零位测试把机器人打到机械零位记录此时的编码器值换算成关节角代入正解。对上面这个参数表零位时关节角是[0, -90, 0, 0, 0, 0]输出位置是(750, -920, 400)。这个结果应该和实际机器人在零位时用量具直接量出的法兰中心位置一致。三坐标测量仪或者激光跟踪仪精度最高但日常调试不一定有这条件。我的做法是把机器人在零位姿势下停住用卷尺量末端法兰中心相对于基座坐标系三个方向的距离。基座坐标系的原点通常定义在底座安装面中心x轴朝前y轴朝左z轴朝上。量出来的数值和正解输出的位置对得上就说明至少模型的几何尺寸是对的。4.3 验证二关节一旋转90度零位测试只能验证参数表的大方向还不能完全确认旋转轴方向定义正确。我一般再做一步单轴旋转验证在零位基础上只让关节1转动90度其他关节不动。关节角变为[90, -90, 0, 0, 0, 0]。从几何直觉上关节1是绕基座的z轴旋转的所以末端位置应该从(750, -920, 400)绕z轴转90度。按右手定则z轴正方向朝上绕z转90度后x变为-yy变为x所以新位置应该是(920, 750, 400)。把代码里joints改成[90, -90, 0, 0, 0, 0]如果输出的位置是(920, 750, 400)说明关节1的旋转方向和z轴正方向的约定一致。如果输出的是(-920, -750, 400)说明旋转方向反了可能需要把z轴方向取反或者把theta1的定义改成取反。这个验证方法非常实用。六个关节可以挨个测一遍每次只转一个关节观察末端位置的变化是否符合绕该关节z轴的圆弧运动。这个测试能在半小时内把整个正解模型的轴方向、参数符号全部验证完比直接上复杂位姿靠谱得多。5. 那些写代码时最容易翻车的细节5.1 标准DH和改进DH参数混用这是我在开头说的那个80毫米误差的直接原因。当时我从一个供应商资料里拿到一份参数表看起来规规矩矩但没有标注是标准DH还是改进DH。我按照常用的改进DH公式写好了正解在仿真软件里验证通过结果一上真机就偏了。排查过程是这样的先怀疑编码器零点重新标定了好几遍没用。又用激光跟踪仪量了末端位置发现不管怎么动误差都在固定方向而且大小跟某个关节的角度强相关。后来把参数表拿到另一个同事面前他一眼看出里面的alpha符号排列跟常见改进DH表不一样这才发现供应商用的是标准DH。标准DH和改进DH的关键差异我已经在第二章详述了。这里给出排查建议拿到任何DH表先在零位用直尺卷尺量一下末端位置再单轴旋转验证方向。如果方向和位置都对不上优先怀疑DH版本搞错了。还有一个更隐蔽的问题即便明确了DH版本不同文献对alpha的正负号定义也可能不同。机械臂的关节旋转方向控制器里往往有自己的约定。多花半小时做单轴验证绝对值得。5.2 零点偏移与theta_offset另一个高频翻车点是theta的零点偏移。前面参数表里的“theta_i初始”列已经体现了这个问题机械臂机械零位时不是所有关节的theta都是0。比如我这个样机的关节2零位时theta2等于-90度。如果你把这个初始偏移忘了加直接用编码器读数当成theta那么末端位置会整体旋转一个大角度误差大得离谱。控制系统里通常这样处理theta实际 theta初始 编码器相对零位的角度增量编码器的绝对位置也好相对位置也好最终都要映射到这样一个theta实际值上再送去正解计算。这里建议在代码层面对six个关节统一做这个映射不要散落在各个调用点。我在调试中还遇到过更隐蔽的情况某个关节的theta初始值其实不是固定常数而是跟安装方式有关。比如关节3和关节5在某些机器人上存在联动或者耦合结构运动学模型里要做额外的修正。碰到这类结构光看DH表不够还得多留意厂家技术文档里的补充说明。5.3 姿态提取的奇异和跳变前面提到过从旋转矩阵提取RPY角会遇到等价角的问题。还有个更麻烦的情况是奇异位形当pitch接近±90度时roll和yaw的计算会出现退化表现为角度值发生剧烈跳变。物理上这个现象不难理解。想象一架飞机当机头垂直朝上时机身绕垂直轴的旋转和绕自身纵轴的旋转产生了耦合你没法区分到底是哪个角在变。机器人腕部也有类似的位置姿态解算在这个位置会出现数值不稳定。这不是正解算错了而是欧拉角表示方法的固有问题。处理办法有三个第一在内部计算和存储中尽量使用4×4矩阵或四元数只在显示界面转换成欧拉角。这样即使临时显示跳变也不会影响控制逻辑。第二如果一定要用欧拉角可以在上层做角度连续化处理比如把相邻两次采样的角度差限制在±180度以内超过就加上或减去360度。第三判断pitch是否进入奇异区间比如cos(pitch)的绝对值小于某个阈值直接跳到等价的非奇异角度组合去显示。实际调试中我发现姿态跳变最容易出现在示教器显示环节让人误以为机器人突然抖了一下。其实机器人运动是正常的只是角度表示在跳变。所以你要是看到示教器上姿态角突然从90度跳到-270度先别慌先检查是不是奇异区间的问题。5.4 工具坐标系你算的是法兰还是针尖最后说一个概念性的大坑正解算出来的位姿默认是指末端法兰盘中心的位姿。但实际干活的时候你关心的是焊丝尖、吸盘口、夹爪指尖在哪里。法兰中心和工具尖之间通常存在几十到几百毫米的偏移如果直接把法兰位姿当成工具位姿用误差是必然的。正确做法是在正解的结果上再乘一个工具坐标变换T_tool T_flange · T_flange_tool其中T_flange_tool是工具坐标系相对于法兰坐标系的齐次变换矩阵描述了工具尖点在法兰坐标系下的位置和姿态。这个矩阵可以通过TCP标定得到也可以用游标卡尺直接量尺寸后计算。调试现场最典型的翻车场景就是示教器显示的坐标系一直是法兰中心程序里却拿这个坐标去和工件模型比对结果整个程序点位全偏移。你以为是正解算错了其实模型和输出的对象不匹配。我自己现在的习惯是正解函数输出统一停在法兰坐标系工具坐标变换单独做一个接口。这样逻辑清晰换工具时只需要改一个矩阵不需要动正解本体。先把这个分层做好再谈精度和其他模块。实际用下来正解这个模块在整个机器人控制系统里看着不起眼但几乎所有上层功能都建立在它上面。每次换一台新机型、换一套DH参数我都建议把零位验证和单轴旋转验证完整跑一遍半小时能省下后面几天的排错时间。这些小习惯比背一百个公式都管用。
