机械臂正运动学：DH参数、矩阵变换与工程验证-北京尧图网络

在机器人学的学习路上正运动学几乎是我遇到的第一道绕不过去的坎。它干的事情很朴素给你机械臂每个关节的角度告诉你末端执行器在空间里到底在哪、朝什么方向。但真到用的时候很多人会发现光会背公式远远不够——坐标系怎么建、DH表怎么填、矩阵怎么乘、算出来的结果怎么验证每一步都有讲究。这篇笔记是我反复推导、写代码、调真实机械臂之后沉淀下来的内容适合刚接触机构学的学生也适合已经接手实际机器人项目、却总被各种细节卡住的工程师。1. 正运动学解决的是哪一步问题从关节空间到末端的位姿映射1.1 为什么明明有电机角度反馈还要费劲算这个很多刚接触机器人的人会问一句机械臂每个关节的电机都装了编码器角度随时能读出来那我还算正运动学干什么这个问题的答案恰恰是理解正运动学价值的入口。编码器告诉你的是每个关节转了多少度但控制任务关心的是末端执行器在世界坐标里的位置和朝向。比如视觉识别给的目标通常是相机坐标系下的坐标而你要让机械臂末端去抓这个目标就必须把目标坐标和机械臂末端坐标放在同一个参考系里比。两个坐标能不能对齐靠的就是正运动学这层映射。换个更日常的说法你只知道左胳膊肘弯了30度、手腕转了45度但手在哪里、掌心朝向哪边你是靠大脑的肌肉反馈感觉出来的。正运动学就是让机器也拥有这种感觉而且比大脑更精确——给定一组关节角末端位姿是一组确定的、可复现的数字。1.2 位姿两个字拆开是位置和姿态两件事正运动学的输出专业叫法叫末端位姿。这个词拆开看一半是位置一半是姿态。位置好理解就是末端在三维空间里的坐标(x, y, z)相对机械臂的基座标系来度量。姿态则复杂一点它描述末端坐标系相对基座坐标系转成了什么角度。同样是末端指到空间同一个点手腕可以朝上、朝下、朝左、朝右抓取效果完全不一样。工程上描述姿态的常见方式有欧拉角、旋转矩阵、四元数三种。旋转矩阵是数学推导中最常用的形式四元数是工程实现和插值中最常用的形式欧拉角则更直观但容易遇到万向锁问题。正运动学的底层严格来说是在计算末端坐标系的旋转矩阵加平移向量最后看你习惯用哪种形式把结果展示出来。换句话说正运动学就是一个函数输入是n个关节角输出是末端坐标系相对基坐标系的完整描述。2. 数学地基先打牢旋转矩阵、齐次变换矩阵和它们的乘法习惯2.1 旋转矩阵的本质坐标系各个轴在参考系里的投影旋转矩阵看起来是一堆数字其实含义很直白一个3x3的旋转矩阵它的每一列就是旋转后那个坐标系的x轴、y轴、z轴在参考坐标系下的方向余弦。比如绕z轴旋转θ角旋转后的x轴在原来的xy平面上转过了θ角所以它的方向余弦是(cosθ, sinθ, 0)。三列合在一起恰好就是那三根轴在参考系里的分量。这就是为什么旋转矩阵一定是正交矩阵——一个坐标系的三个轴天然互相垂直模长都是1。三种基本旋转矩阵是推导所有复杂旋转的积木绕x轴的旋转、绕y轴的旋转、绕z轴的旋转各有各的矩阵形式。复杂的末端姿态无非是这三种基本旋转按某种顺序的组合。我刚开始学的时候总觉得这些矩阵得死记硬背后来发现根本不用背——你只要记住每一列是坐标系某根轴在参考系里的投影然后画个三角形用三角函数一推就出来了。提示旋转矩阵的乘法不满足交换律。先绕x轴转90度再绕y轴转90度和先绕y轴再绕x轴得到的是两个完全不同的姿态。这一点跟普通数字乘法完全不同是新手最容易犯的错误。2.2 为什么非要搞一个4x4的变换矩阵机械臂是一个多连杆链我们要的末端位姿是N个连杆变换叠加的结果。如果每一步都用3x3旋转矩阵加3x1平移向量分开表示连续变换写起来非常啰嗦而且还有一个致命问题旋转和平移没法用统一的乘法连在一起算。解决的办法就是齐次变换矩阵。它是一个4x4矩阵左上角是3x3的旋转矩阵右上角是3x1的平移向量最下面一行是[0, 0, 0, 1]。空间里的一个点直接用齐次坐标表示平移就用矩阵右上角的向量加进去旋转就用左上角的矩阵转过去四维乘法把旋转和平移统一成了一个操作。用4x4矩阵最大的好处有两个。第一多个连杆的位姿变换可以不停地做矩阵乘法乘完还是一个4x4矩阵末端的位姿被干干净净地封装在最终矩阵里。第二矩阵求逆变得方便这在后面反向运动学、空间速度分析里都会反复用到。很多教材一上来就扔出这个矩阵学生往往一头雾水。我的理解方式是它就是一张盖了章的便条把某两个坐标系之间怎么转、怎么挪一次性写清楚还能叠加。2.3 连续变换的乘法顺序是从左到右还是从右到左齐次变换矩阵的乘法顺序是正运动学里第一个大坑。假设有三个坐标系基座标系A、中间坐标系B、末端坐标系C。已知T_AB表示B相对于A的位姿T_BC表示C相对于B的位姿那么C相对于A的位姿是T_AC T_AB × T_BC注意B在中间所以它被夹在两个矩阵势力范围的正中间。这个公式本身不难难的是理解矩阵乘法里左乘和右乘分别对应什么物理动作。我自己的记忆方式是这样的矩阵从右往左读最右边的变换最先作用在坐标系上。如果你站在当前坐标系下想做一次自身运动就要右乘一个变换矩阵如果你想在世界坐标系下做一次运动就要左乘一个变换矩阵。正运动学里每个关节的变换都是沿着当前连杆坐标系发生的所以用连续右乘的方式把变换矩阵连成一条链乘出来的结果就是从基座标系到末端的完整位姿描述。这句乘的顺序不能反是后来调代码时救了我不止一次的经验。3. DH参数是建模的骨架两种约定背后是完全不同的坐标系放置逻辑3.1 为什么非得弄一套人为的参数来描述连杆机械臂的几何结构看起来很直观一个底座、几个臂管、几个关节。但如果让你用数学把它们精确描述出来你会发现连杆之间并不存在天然的坐标系。DH参数就是一套人为规定的标准它规定了你把坐标系放在哪里、定义哪些参数让全世界的人描述同一台机械臂时用的都是同一套语言。DH参数一共四个θ绕z轴的关节角通常对应旋转关节的变量d沿z轴的连杆偏移距离通常对应移动关节的变量a沿x轴的连杆长度即相邻两个z轴之间的公垂线长度α绕x轴的连杆扭转角即相邻两个z轴之间的夹角这个编排的精妙之处在于任何相邻两个关节坐标系之间的变换都可以拆成上述四个参数的组合。你只要把一台机械臂的每个依次相邻的关节对上这四个参数就得到了一张DH参数表。这张表本身就是机械臂运动学的骨架。3.2 标准DH和修正DH别等代码出错再回来查DH参数有个令人又爱又恨的地方它存在两个版本——标准DHSDH和修正DHMDH。这两者的数学定义几乎一样但坐标系放置的位置和变换顺序不同。标准DH中坐标系i固定在连杆i的远端也就是第i1个关节处两条相邻z轴的公垂线指向x轴。变换顺序是沿z轴平移d绕z轴旋转θ沿x轴平移a绕x轴旋转α。修正DH中坐标系i固定在连杆i的近端也就是第i个关节处而且相邻连杆的扭转角α和长度a被划到了前一个连杆的参数里。变换顺序相应的变成绕x轴旋转α沿x轴平移a绕z轴旋转θ沿z轴平移d。两种约定在同类型机械臂上得到的DH参数表不仅数值不一样矩阵形式也不一样甚至同一套关节角度算出来的中间变换矩阵都不同。我这里给一个最实用的判断方法查资料或读开源代码时先看一眼它的变换函数里是先处理绕z轴还是先处理绕x轴就知道它用的是哪套约定。我见过不少人在移植别人代码时把标准DH的矩阵公式套到修正DH的参数表上结果末端位置凭空偏了十几厘米查了一整天才发现是约定不匹配。提示Standard DH和Modified DH没有高下之分但混用会翻车。选哪套取决于你的首篇参考资料和机械臂厂家的说明书但一旦选定整套代码必须自洽。3.3 坐标系建在连杆哪一端直接决定参数表的符号标准DH和修正DH的差别会造成一个很隐蔽的符号问题同一个零位姿态在两套约定下DH参数表中的a或d甚至α的正负都可能不一样。我自己做过的一个小项目里六轴机械臂的肩关节和肘关节附近有两个偏置结构也就是z轴和z轴之间的公垂线不在同一平面里。用标准DH建出来的表第二个关节的α是正的90度换用修正DH同一段结构却要求α取负的90度。如果不注意按修正DH的坐标系规则去填标准DH的表最终结果会差出一个镜面对称的姿态。所以我的建议是在一张纸上完整画一遍坐标系变换图标清楚每个z轴的位置再决定用哪套约定。这一步在工程上从来不省时间画图十分钟省的是后面至少两天查bug的时间。4. 二连杆入门手算与完整公式推导正解到底怎么“解”出来4.1 一个最简单但完整的例子平面二连杆末端坐标开始接触公式不要直接看六轴机械臂那吓人的4x4矩阵链先拿平面二连杆练手。假设一个二连杆机械臂第一根杆长L1第二根杆长L2关节角分别是θ1和θ2。这相当于平面上一只手臂肩膀关节转θ1肘关节相对上臂转θ2。用标准DH来建表每根连杆的d和α都是0a就是杆长DH参数表长得非常清爽连杆1θθ1, d0, aL1, α0连杆2θθ2, d0, aL2, α0把这两个变换矩阵用第2节的方法乘起来得到末端坐标。数学结果并不复杂x L1 * cos(θ1) L2 * cos(θ1 θ2) y L1 * sin(θ1) L2 * sin(θ1 θ2)第一次推导出这个结果时我挺有成就感的。因为它非常直觉末端的x坐标等于上臂末端投影到x轴的距离加上下臂相对水平方向转过(θ1θ2)后在x轴上的投影。这个式子虽然简单但完整展示了正运动学从关节角到笛卡尔坐标的全部逻辑也是后面积累复杂机械臂公式的最小单元。4.2 从二连杆到任意连杆矩阵链乘将具化为通式二连杆只有两个变换手算还能应付。但真实的六轴机械臂有六个变换手算矩阵会变成灾难片现场。好在我们并不需要把最终矩阵的每个元素都展开成三角函数表达式只需交给计算机做矩阵连乘即可。通式写出来其实是干净的T_0N T_01 × T_12 × T_23 × ... × T_(N-1)N其中T_0N就是从基座标系(0)到末端坐标系(N)的齐次变换矩阵。T_(i-1)i是第i个关节的DH变换矩阵它由第i行DH参数计算得出。矩阵维度始终是4x4N个矩阵乘法需要的计算量完全可控这也是为什么几乎所有的机器人运动学库最终都在做同一件事循环读取DH表连续相乘。注意这里的0到N下标不同教材可能写成T_N基座系或T_GN但本质相同。读代码时先确认下标顺序避免把变换方向搞反。4.3 二连杆公式的深层模式叠加的三角函数角度二连杆公式里的cos(θ1θ2)其实暗示了一个规律后续连杆的关节角叠加在之前所有关节角的总和上。这个叠加就是正运动学的本质——每个关节的转动都对下游所有连杆有贡献。这也是为什么正运动学即使面对6个甚至7个关节理论上也不存在歧义你给一组确定的关节角末端位姿是唯一确定的。对比之后要学的逆运动学已知末端位姿反解关节角往往有多组解甚至无解正运动学这种一对一的确定性让它可以成为所有后续算法的可信基线——不管逆运动学怎么算算出来的关节角都可以再通过正运动学来验证对不对。5. 工程落地六轴机械臂的Python正运动学实现与结果解读5.1 自己写一个干净的DH变换函数现在进入代码环节。下面这个函数是标准DH约定下的变换矩阵实现输入theta, d, a, alpha四个参数输出一个4x4矩阵。代码刻意保持短小方便你直接检查每一步import numpy as np def dh_transform(theta, d, a, alpha): 标准DH约定下的相邻连杆变换矩阵。 theta, alpha 使用弧度 d, a 使用米。 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] ])有件事值得展开说一下上式中第一行第二列是-st * ca而不是-sin(theta) * cos(alpha)的简写这个细节和上一节的变换顺序严格对应。如果你用的是修正DH这个矩阵第一行第二列的位置要换成更复杂的组合。每次从一个来源复制代码我都要先花一分钟对照这个位置检查它到底是标准DH还是修正DH。5.2 一组合理的六轴机械臂DH表下面这组DH参数是我在某个教学项目中用于示范的数据参数数值的量级接近常见的桌面级六轴机械臂但请理解这不是任何厂家的真实参数只用于展示方法dh_params [ # theta_offset, d, a, alpha (0, 0.33, 0.04, np.pi / 2), (0, 0.00, 0.30, 0.0), (0, 0.00, 0.04, np.pi / 2), (0, 0.32, 0.00, -np.pi / 2), (0, 0.00, 0.00, np.pi / 2), (0, 0.06, 0.00, 0.0), ]每个元组对应一个关节的DH参数第一个元素theta_offset是关节角为零时的角度偏置。真正计算时实际关节角q要加上这个偏置。比如写代码时def forward_kinematics(q, dh_params): T np.eye(4) for (offset, d, a, alpha), q_i in zip(dh_params, q): theta q_i offset T T dh_transform(theta, d, a, alpha) return T这段代码会输出一个4x4的矩阵T。T[:3, 3]就是末端位置T[:3, :3]就是末端姿态的旋转矩阵。整个流程不会超过三十行但它就是一台六轴机械臂正运动学的完整核心。5.3 结果怎么解读位置向量、姿态矩阵与欧拉角拿到T矩阵之后第一件事是读出末端位置position T[:3, 3]这一步非常直观。姿态部分如果直接输出3x3旋转矩阵人类很难凭肉眼判断这个姿态是不是我想象的样子。工程上一般会把旋转矩阵转成欧拉角或者四元数再输出。从旋转矩阵提取欧拉角按ZYX顺序可以这样写def rotation_to_zyx_euler(R): pitch np.arcsin(-R[2, 0]) if abs(np.cos(pitch)) 1e-6: roll np.arctan2(R[2, 1], R[2, 2]) yaw np.arctan2(R[1, 0], R[0, 0]) else: # 万向锁附近退化处理 roll np.arctan2(-R[1, 2], R[1, 1]) yaw 0.0 return np.array([roll, pitch, yaw])这里特别提一下万向锁。当pitch接近正负90度时这个提取公式会退化roll和yaw无法唯一确定。这个不是代码bug而是欧拉角表示法的固有缺陷。工程上处理这种情况的标准做法是改用四元数输出或者保留原始旋转矩阵别在欧拉角上死磕。我自己调试机械臂的时候就吃过这个亏姿态看起来在跳变以为代码写错了查了半天才知道是因为末端姿态逼近奇异区域。6. 验证与避坑怎么确认你的正解是对的以及常见的翻车现场6.1 零位校验机械臂的出厂姿态就是照妖镜正运动学代码写完最怕的是运行了个寂寞——程序没报错结果却是错的。第一个高效验证手段就是让所有关节角度置零运行正运动学然后对比机械臂实际零位姿态下的末端位姿。机械臂出厂通常都有一个home位或零位姿态这个姿态下末端的位置和姿态在厂家文档里可以查到或者在机器人仿真环境里摆出来量一下。如果你的DH参数表中该有偏置值的地方都写对了那么零位下算出的位置应当和实际零位位置在毫米级误差内吻合。如果偏差很大优先检查这几点坐标系建立放反了、α的正负反了、或是a和d的单位搞错了。我自己经历过的一次真翻车是这样的第一版DH表零位校验算出来的末端z坐标凭空高了0.3米查来查去发现是第三个关节的d值该填0.32我不小心填成了0.64等于把两个关节的偏移重复叠加了一次。这个错误在程序里毫无痕迹只有和真实位置对照时才能暴露。6.2 单关节运动测试末端轨迹的形状不会说谎零位校验通过之后第二个验证方法是单关节运动测试。让其他关节保持零位只让某一个关节从0转到90度观察末端位置的变化轨迹。如果动的是第一个关节末端位置应该扫过一个圆弧圆弧的圆心大致在基座关节的转轴上半径等于末端到该转轴的水平距离。如果动的是第二个关节末端应该绕第二个关节的轴扫过一个类似的圆弧。这个测试有一个隐藏价值它能帮你判断DH表中d和a是否填反。假如你动了第三个关节末端却同时出现了不该有的竖直方向大位移那就说明该关节的d值多半有误。这个测试不需要高精度仪器你只需要在屏幕上打印出一组角度下的末端xy坐标目测是否落在圆弧上即可。机械臂的运动学是连续可微的任何位置跳变或者轨迹曲线不圆的现象都暗示参数或者坐标系方向出了问题。6.3 和仿真环境交叉对比找一个独立的计算源手算验证到底还是用自己的代码万一自己的实现逻辑整个错了手算可能被带偏。更可靠的做法是找一个独立的第三方计算源做交叉对比比如某商业数值计算软件的机器人工具箱或者开源的机器人仿真环境。具体操作方式是在仿真环境里搭一台和你DH表参数一致的机械臂然后取几组关节角度同时用你的正运动学代码和仿真环境计算末端位姿对比两者输出的xyz和姿态矩阵。一般只要在零位测试通过的前提下交叉对比再取三五个点就能确保万无一失。这里有个小心得对比时不要只看位置姿态矩阵的每个元素也逐个数位对一遍。位置对而姿态错的情况并不罕见多半是α的符号或旋转顺序不对。矩阵元素逐一对齐能很快定位到具体是哪个连杆的哪个参数出错。6.4 角度与弧度混用、数值累计误差两个最隐蔽的坑最后说说两个很隐蔽、也让我反复吃亏的问题。第一个是角度制和弧度制混用。这个问题低级但杀伤力极大。三角函数在Python里默认用弧度而机械臂厂家给的关节角度数据常常是度数。如果你在代码里直接拿度数喂给np.cos和np.sin结果当然是错的而且错得毫无规律。解决方法是建立一个约定所有算法核心统一用弧度只有在人机交互界面才转成度数。我在项目里甚至在DH变换函数内部加了一个断言检查传入的theta是否在合理弧度范围内宁可多花一次转换的代码量也不要让bug静默存在。第二个是数值累计误差。DH矩阵连乘时每次乘完的元素会经过浮点运算几十次连乘下来旋转矩阵会略微失去正交性。当你的正运动学函数被上层逆运动学算法反复调用时这个问题会被放大最终导致姿态漂移。工程上的常规对策是定期把旋转矩阵做正交化处理用SVD或者简单的Gram-Schmidt过程恢复它的正交性。我在写工具库时会在每轮连乘结束后检查一下det(R)是否仍接近1必要时做一次重投影。这种细节平时感觉不到但到了精度要求严苛的机器人标定任务里可能就是决定成败的那一步。最后留一个我个人的使用习惯。每接触一台新的机械臂我先不急着写控制逻辑而是先花一下午时间把DH表手工填一遍做零位校验和单关节圆弧测试彻底确认运动学模型正确再往上层叠算法。这一步看似浪费了时间实际是后面所有调试工作的防火墙。做运动学的人都知道模型错了后面的一切算法都只是在一个错误的世界里精心编织幻觉——把正解这个地基打牢后面不管做轨迹规划、逆解还是力控心里都会很有底。

机械臂正运动学：DH参数、矩阵变换与工程验证

相关推荐

STM32双轴硬实时同步：基于TIM1/TIM8主从模式的直线插补实现

用PLC玩转医药洁净室空调五种模式：从生产到紧急状态的控制策略

补充论述：拓扑冻结与稳定宇宙的形成——从科普性说明到数学物理论证

更多资讯

嵌入式C与标准C的差异：内存、运行时与编译器行为解析

UWB定位锚点三合一实测：中继、定位、信标如何兼顾部署效率

PLC工程师实战错题本：90条现场踩坑经验与避坑指南

从零理解电机旋转电压矢量：Clarke/Park变换与SVPWM仿真实践

从零搭建步兵机器人：硬件选型、供电与通信全解析

读懂HCU控制模块说明书：状态机、标定与故障码排查

今日更新

微信云开发旅游小程序实战：LBS打卡+UGC社交全栈方案

金融AI代理平台核心拆解：从多代理协作到财报分析实战

基于大数据的智慧旅游推荐与可视化平台架构实践

本周精选

金融AI落地指南：从调研报告到技术选型与工程实践

大模型上下文成本与注意力优化技术深度解析——为何O(n²)平方复杂度无法被常规优化消除｜TaoToken 统一 Key 通道实测

基于GMM的语音识别系统原理与MATLAB实现详解

本月热门

我发现了一个新思路：用 Remotion + Claude Code 像写代码一样自动化生成短视频

Windows下 Codex 中 Chrome 和 Computer Use 插件不可用问题排查及解决参考方式：TaoToken 统一 Key 配置与验证

2026 大模型集体涨价：用 Python 做企业 Token 成本测算与选型避坑（附配置）

读完文章，想聊聊您的网站？