★ FEATURED ARTICLE 简介面向正在学习机器人学、需要通过编程实践理解正解与逆解原理的学生以及希望快速上手机器人建模的工程师和研究人员这份资料围绕D-H参数法在六自由度机器人运动学中的应用是一套可直接运行的配套学习包。压缩包共3个文件、约2.27MB包含两个MATLAB程序分别实现正解与逆解计算和一份Word程序说明文档兼顾代码演示与理论注释。程序中以六自由度机器人为对象演示了由关节角度求解末端执行器位置姿态以及由目标位姿反推关节变量的全过程Word说明对D-H四个参数关节角、连杆长度、关节偏置、偏转角的设定、坐标变换思路和程序使用步骤进行了梳理能够帮助读者在动手运行代码的同时理解连杆链建模与坐标变换的核心逻辑。已有1019人下载学习特别适合用作机器人学课程作业的参考或自学入门配套材料。1. 先立住D-H法的骨架四参数表定生死六自由度机械臂拿到手最容易翻车的环节往往不是写代码而是D-H参数表这一关。同一根关节轴把θ、d、a、α这四个参数的样子弄混或者把标准D-H和修正D-H混着用正运动学算出来的末端位置很容易偏出十几厘米逆解更是直接无解。这套资料把D-H法的路径一次讲清楚标准与修正D-H的取舍、参数表怎么从机构图纸里抠出来、正解与逆解的MATLAB和Python程序怎么落地以及建模过程里常踩的坑。适合正在搭课设、毕设或仿真环境的开发者——目标是把一张机械图纸变成能跑通的运动学模型。2. 标准D-H与修正D-H差一个符号末端偏十几厘米2.1 四个参数的物理含义先弄干净六自由度串联机械臂的每个关节自由度都可以用四个参数唯一定义。这四个参数分别是关节角θ、连杆偏置d、连杆长度a、连杆扭角α。表格里看着简单实际填表时很多人卡在“这个角到底绕哪根轴转”上。参数符号物理含义约定说明关节角θ_i从x_{i-1}绕z_{i-1}轴旋转到x_i轴的角度关节变量角度单位统一用度或弧度连杆偏置d_i沿z_{i-1}轴方向从x_{i-1}到x_i的距离相邻关节轴平行时为0连杆长度a_i沿x_i轴方向从z_{i-1}到z_i的公垂线距离一般写非负值连杆扭角α_i从z_{i-1}绕x_i轴旋转到z_i轴的角度正负号决定相邻关节轴的朝向关系标准D-HSDH与修正D-HMDH最大的区别是坐标系固定位置不同。标准D-H把坐标系i固定在连杆i的远端也就是关节i1的位置修正D-H把坐标系i固定在关节i的位置。落到参数上标准D-H用的是a_i、α_i、d_i、θ_i而修正D-H用的是a_{i-1}、α_{i-1}、d_i、θ_i。很多人从网上下了一份资料又抄了另一段代码结果连α取的是哪个下标都没注意正解矩阵自然对不上。我一般建议优先用修正D-H。原因是市面上的六轴机械臂大多是球形腕结构后三根轴交于一点修正D-H的参数表能直接对应机械图纸上的距离标注不容易把d4和a4搞混。资料包里如果两种都有建议以修正D-H作为主实现标准D-H拿来对照原理。2.2 从六轴结构图纸到参数表一条完整提取流程拿到一张六轴机械臂的结构图纸不要直接往上填数。先把关节轴画出来再按下面的顺序走一遍。第一步标出所有关节轴的方向给坐标系0到6编号。基座坐标系0的z0沿J1轴线朝上x0方向随便选但后面所有角度都要以这个x0为基准。第二步从图纸上读出相邻轴的几何关系。重点是z_{i-1}到z_i的夹角α、公垂线距离a以及沿z_{i-1}方向的偏移d。第三步把零位姿态定下来。令所有θ_i0时机械臂处于图纸的默认姿态。很多图纸不给零位标记这时候就按“前臂水平前伸”的姿态去对齐千万不能随手填一个角度。第四步按修正D-H格式填入参数表。下面是一个典型的六轴教学臂参数表单位是毫米角度单位是度。这张表不是我凭空编的是很多实验室样机都常用的构型参数拿来跑仿真和课设完全够用。关节θdaα1θ11350902θ2017503θ3017004θ4850905θ500-906θ64500这张表里d1是底座到J2轴线的高度a2和a3分别是肩关节到肘关节、肘关节到腕关节的长度d4是腕关节偏置d6是末端法兰到工具坐标系的偏移。α的正负号是重点——第4关节α90°第5关节α-90°这两个符号一旦写反末端姿态会整个翻转。2.3 参数表填完后的三个自检点参数表填完先别急着写程序用三个自检点看一眼。第一个自检点把θ1到θ6都设成0度正运动学算出的末端位置应该大致落在x轴正方向z值接近d1d4d6。如果z值明显不对说明d列有填错的。第二个自检点看J4和J5的α符号。球形腕构型中J4轴和J5轴是垂直的α90°和α-90°对应完全不同的腕部弯曲方向。拿不准时可以在纸上画坐标轴用右手定则比划一下。第三个自检点让θ290°、其他关节为0末端位置应该明显向上抬起。如果反而是向下垂说明θ2的零位定义和图纸对不上需要调整θ2的初始角度而不是去改D-H表。3. 从参数表到正运动学MATLAB与Python矩阵连乘实现3.1 单关节齐次变换矩阵的构造正运动学的本质是把相邻关节坐标系之间的齐次变换矩阵连乘起来。单个关节的变换矩阵在修正D-H约定下由四步组成绕x_{i-1}转α_{i-1}沿x_{i-1}平移a_{i-1}绕z_i转θ_i沿z_i平移d_i。下面这段MATLAB代码写了单个关节的变换矩阵。参数里角度统一传弧度因为MATLAB的cos和sin默认接收弧度值。function T mdh_transform(theta, d, a, alpha) % 修正D-H单关节齐次变换矩阵 % theta: 关节角单位弧度 % d: 连杆偏置 % a: 连杆长度 % alpha: 连杆扭角单位弧度 T [cos(theta), -sin(theta), 0, a; sin(theta)*cos(alpha), cos(theta)*cos(alpha), -sin(alpha), -d*sin(alpha); sin(theta)*sin(alpha), cos(theta)*sin(alpha), cos(alpha), d*cos(alpha); 0, 0, 0, 1]; end这段代码的矩阵布局和大多数资料里的MDH矩阵一致。注意第二行第四列是-d*sin(alpha)第三行第四列是d*cos(alpha)这个负号和标准D-H不一样。如果读者拿到的资料用的是标准D-H矩阵会是另一种布局不能混着抄。调用这个函数时建议把D-H表按行存入一个六行四列的矩阵dh_table每行分别是[theta_deg, d, a, alpha_deg]。这里theta和alpha用角度存进函数时先转弧度避免在主程序里到处写deg2rad。3.2 六轴连乘顺序不能换位置的矩阵乘法六自由度机械臂的总变换矩阵就是六个单关节矩阵按顺序连乘T06 T01 * T12 * T23 * T34 * T45 * T56。矩阵乘法不满足交换律所以关节顺序不能乱。% 六自由度机械臂正运动学计算 dh [0, 135, 0, 90; % 关节1 0, 0, 175, 0; % 关节2 0, 0, 170, 0; % 关节3 0, 85, 0, 90; % 关节4 0, 0, 0, -90; % 关节5 0, 45, 0, 0]; % 关节6 T06 eye(4); for i 1:6 theta_deg dh(i, 1); d dh(i, 2); a dh(i, 3); alpha_deg dh(i, 4); T06 T06 * mdh_transform(deg2rad(theta_deg), d, a, deg2rad(alpha_deg)); end % 提取末端位置和姿态 pos T06(1:3, 4); % 末端位置 R06 T06(1:3, 1:3); % 末端姿态旋转矩阵 disp(pos);这段代码的逻辑很直接从单位矩阵开始逐个乘上每个关节的变换矩阵。循环结束后T06的第四列前三行就是末端位置前三行前三列是末端姿态。实际使用中我会额外加一句判断计算R06的行列式是否接近1如果偏差大说明某个α的符号填错了。3.3 用Python再实现一遍适合接仿真和视觉MATLAB适合推导和验证但很多机器人项目的通信、视觉部分用Python写所以正运动学最好有一份Python版本。下面是numpy实现。import numpy as np def fkine(q_deg, dh_table): 六自由度机械臂正运动学 q_deg: 关节角度列表单位度 dh_table: 六行四列每行为[d, a, alpha_deg] T06 np.eye(4) for i in range(6): theta_rad np.deg2rad(q_deg[i]) d, a, alpha_deg dh_table[i] alpha_rad np.deg2rad(alpha_deg) A np.array([ [np.cos(theta_rad), -np.sin(theta_rad), 0, a], [np.sin(theta_rad)*np.cos(alpha_rad), np.cos(theta_rad)*np.cos(alpha_rad), -np.sin(alpha_rad), -d*np.sin(alpha_rad)], [np.sin(theta_rad)*np.sin(alpha_rad), np.cos(theta_rad)*np.sin(alpha_rad), np.cos(alpha_rad), d*np.cos(alpha_rad)], [0, 0, 0, 1] ]) T06 T06 A return T06 # 示例全部关节在0度查看末端位置 dh [ [135, 0, 90], [0, 175, 0], [0, 170, 0], [85, 0, 90], [0, 0, -90], [45, 0, 0] ] q_home [0, 0, 0, 0, 0, 0] T fkine(q_home, dh) print(T[0:3, 3])这里我把dh_table每行定义成[d, a, alpha_deg]和前面MATLAB版本保持一致。跑出来的位置大概在x345毫米、z265毫米附近不同零位定义会有差异如果差很多先检查a2、a3、d1、d4、d6几项是否填入正确。Python版本的好处是后面接机器人操作系统、做轨迹规划、画误差曲线都顺手。4. 六轴逆运动学解析解球形腕构型的求解顺序4.1 球形腕的判断为什么后三轴交于一点六轴机械臂的逆运动学没有通用闭式解能解析求解的前提是结构满足特定几何约束。最常见的可解构型就是球形腕J4、J5、J6三根轴相交于同一点这个点叫腕心。对应到D-H表上特征是a40、a50并且J4和J5、J5和J6之间的公垂线距离为0只有d4和d6这种沿关节轴方向的偏置。判断方法很简单用正运动学分别算T04和T06如果J4到J6的变换矩阵在位置部分只有沿z轴方向的偏移说明腕心存在。球形腕的意义在于前三关节决定腕心在空间中的位置后三关节决定末端姿态位置和姿态解耦逆解就可以分成两段算。4.2 前三轴解位置把空间问题压成平面二连杆求位置时先把末端工具中心TCP沿z6轴反推d6距离得到腕心坐标。这一步是因为正运动学给出的末端位置包含工具偏移而逆解第一步处理的腕心不含这个偏移。p_tcp np.array([0.3, 0.1, 0.4]) # 目标TCP位置 R_des np.eye(3) # 目标姿态矩阵 z6 R_des[:, 2] # 末端z轴方向 w p_tcp - d6 * z6 # 反推腕心位置 wx, wy, wz w[0], w[1], w[2]拿到腕心坐标后前三轴可以简化成平面几何问题。θ1通过腕心在x-y平面的投影求角度θ3用肩关节到腕心的距离和a2、a3组成的三角形余弦定理求夹角θ2再根据三角形的角度补出来。# 求θ1腕心在x-y平面上的投影角 import math rxy math.hypot(wx, wy) theta1_1 math.atan2(wy, wx) math.acos(d4 / rxy) theta1_2 math.atan2(wy, wx) - math.acos(d4 / rxy) # 求θ3余弦定理a2175, a3170 r math.hypot(wx, wy) - d4 # 简化后的肩关节水平投影距离 z wz - d1 # 腕心相对肩关节的高度差 L math.sqrt(r*r z*z) cos_theta3 (L*L - a2*a2 - a3*a3) / (2*a2*a3) theta3 math.acos(cos_theta3) # 注意存在正负两个解这段代码里有个关键保护acos的参数如果绝对值大于1说明目标点不在可达空间内解不存在。跑代码之前应该加一个判断防止数学计算崩溃。θ1有两组解θ3也有正负两种取法所以前三轴组合下来已经有四组解后面再乘上腕部的多解总共会有八组甚至更多。4.3 后三轴解姿态从末端姿态矩阵里剥出腕部角度位置解出来后前三轴的旋转矩阵R30已经确定末端姿态R60已知那么腕部坐标系相对于第三轴坐标系的旋转矩阵就是R36 R30的转置乘以R60。R36本质上是四、五、六三个关节角的旋转组合按Z-Y-Z欧拉角公式反推就行。R36 R30.T R_des # 按Z-Y-Z内旋约定提取腕部角度 theta4 math.atan2(R36[1, 2], R36[0, 2]) theta5 math.acos(R36[2, 2]) theta6 math.atan2(R36[2, 1], -R36[2, 0])这里的欧拉角约定必须和正运动学连乘时的旋转顺序完全一致。资料里如果用的是XYZ约定θ4、θ5、θ6的公式会不一样不能凭记忆直接套。实际调试时我会把求解出的关节角代回正运动学看末端位置和姿态是否还原这一步比任何公式都可靠。4.4 奇异位形与多解取舍别让角度在±180度之间跳解析解算出来是一堆候选角度不能直接用。第一步把每个角归一化到-180度到180度之间防止出现170度和-190度其实是同一个位置的假跳变。第二步按关节限位剔除不可达解。第三步才是正经的多解选择——连续轨迹运动时用上一时刻的关节角作为参考选和它最接近的一组解避免关节角突变。奇异位形要单独处理。腕部奇异发生在θ5接近0度或180度时此时θ4和θ6有无穷多组解肩部奇异发生在腕心位于z0轴上时θ1不稳定。工程上的处理方式是设置一个奇异阈值比如sinθ5小于0.01就锁定上一帧的θ4只解θ6。5. 常见问题与排查五条高频D-H事故记录5.1 正解和仿真模型位置对不上现象正运动学算出的末端位置和三维仿真模型里显示的末端位置偏差很大甚至超过几十毫米。原因绝大多数情况是标准D-H和修正D-H混用了。矩阵用的是标准D-H公式参数表却按修正D-H填或者反过来。另一种常见原因是α的正负号没核对导致某个坐标系旋转方向反了。解决先把参数表标注清楚用的是哪套约定。我自己的习惯是给每个矩阵加注释说明来自MDH或SDH。如果是MDH矩阵第二行第四列一定是-d*sin(alpha)如果是SDH第三行第四列没有这个负号结构。核对无误后把全部关节角设成0手动验证末端位置是否落在合理位置。5.2 逆解算出来但轨迹运行时角度剧烈跳动现象连续轨迹跟踪时某个关节角突然从-170度跳变到170度机械臂剧烈抖动。原因atan2返回的角度范围是-180度到180度关节实际允许范围和它不一致。如果关节限位是0到360度170度和-190度是同一个位置但控制器不知道就会产生大角度差值导致插补器误判。解决逆解输出前做一个角度归一化和邻域选择。先算出所有候选解再和上一帧关节角做差值选绝对差值最小的那组解。归一化函数不能只对同一个角取模还要考虑差值的对称性比如当差值大于180度时减去360度。def nearest_angle(target, reference): diff (target - reference 180) % 360 - 180 return reference diff5.3 代码里角度单位混用仿真结果忽对忽错现象同一套正解代码改几个关节角度值结果有时正确有时错得离谱。原因写代码时没有统一角度单位。正运动学函数内部用弧度但调用时传了度数或者D-H表里的α用度数存矩阵里却直接当弧度用。这类问题最隐蔽因为0度、90度这些特殊角度在度和弧度下数值恰好重叠单测几个点是看不出来的。解决在D-H表入口处统一做一次deg2rad转换后续所有计算一律用弧度。避免在代码中散落数字常量角度统一用变量传递。每次改代码后先跑一遍θ[0,0,0,0,0,0]和θ[90,0,0,0,0,0]两个固定测试点。5.4 逆解报错acos参数超出[-1,1]现象跑轨迹规划时某个目标点突然报数学错误程序退出。原因目标点超出工作空间。余弦定理中acos的参数由L、a2、a3算出当目标点距离肩关节太远时L的平方大于(a2a3)的平方cos值超出[-1,1]acos返回NaN。解决在求解前先算一下腕心到基座的距离范围。最小可达距离是|a2-a3|最大可达距离是a2a3超出范围直接跳过这个目标点或者把轨迹生成时的半径和高度限制在工作空间内。工程上我会在逆解函数开头加一个可达性判断返回空列表而不是报错。5.5 腕部奇异时θ4和θ6乱跳现象末端姿态有小幅连续变化但θ4和θ6的波形出现尖峰、毛刺。原因θ5接近0度时R36矩阵退化成只依赖θ4θ6的组合单个角度解不出来。这是结构决定的不是代码bug。解决在手控制前判断abs(sin(theta5))是否小于阈值。小于阈值时直接沿用上一帧的θ4值然后从R36反解θ6。只要θ4和θ6之和正确末端姿态误差就不会大。阈值一般取0.005到0.01太大会损失精度太小则起不到稳定作用。6. 进阶验证用一条圆轨迹闭合正逆解的半小时自检拿到D-H参数表后我建议做一次正逆解联合验证生成一条目标圆轨迹用逆解算关节角再用正解算回末端位置对比误差。这条闭环能暴露参数表、矩阵公式、角度单位、多解筛选几乎所有问题。圆轨迹选得很有讲究。固定姿态、只让末端在水平面画圆会同时激励J1、J2、J3三个大关节和J4、J5、J6三个小关节既覆盖工作空间中的典型区域又不会逼到奇异边界。半径一般取工作空间半径的百分之二十左右太大会碰上不可达点太小又看不出问题。import numpy as np import matplotlib.pyplot as plt # 这里dh_table、fkine、ikine沿用前面的定义 # ikine返回与参考角最接近的一组解 radius 40.0 center np.array([0.3, 0.0, 0.35]) point_num 200 err np.zeros(point_num) q_ref np.zeros(6) for k in range(point_num): t 2 * np.pi * k / point_num target_pos center np.array([radius*np.cos(t), radius*np.sin(t), 0]) target_pose np.eye(4) target_pose[0:3, 3] target_pos q_sol ikine(target_pose, q_ref) if len(q_sol) 0: err[k] np.nan continue T_actual fkine(q_sol, dh_table) err[k] np.linalg.norm(T_actual[0:3, 3] - target_pos) q_ref q_sol plt.plot(err) plt.xlabel(采样点) plt.ylabel(末端位置误差(mm)) plt.show()跑完这个脚本看两个东西。第一误差整体应该平稳数值在小数点级别或毫米级第二误差曲线上不能有尖峰。尖峰意味着某个采样点逆解跳变了常见原因是多解筛选失败或奇异位形未处理。从第一次做六轴机械臂建模开始我就养成了一个习惯任何新拿到的D-H表先用这条圆轨迹流程跑一遍。十分钟左右能看到结果能省下后面在实物调试时的大量排查时间。这套资料把D-H表提取、正解代码、逆解推导和验证脚本串成了一条完整链路照着跑一遍基本能把六自由度机器人的运动学思路打通。希望帮到你。本文还有配套的精品资源点击获取
阅读完成 · 觉得有帮助？
