简介这套六自由度PUMA560轨迹规划画圆资源面向机器人方向学生与工程师围绕工业机器人圆形轨迹规划难题提供了从期望圆轨迹到各关节角度的完整Matlab实现。压缩包共4个文件含3个.m脚本与1个xlsx表格总大小仅13KB便于直接运行和二次修改。ikunc.m实现逆运动学求解将笛卡尔坐标反解为关节角five_poly.m通过五边形逼近算法生成平滑路径Copy_of_Untitled.m作为主控脚本串联各函数xlsx文件记录关节角度序列与时间戳方便对照验证。资源已有1999人学习适合研究六自由度机器人轨迹规划、逆运动学或完成课程设计的读者。借助这套代码可快速吃透PUMA560画圆的完整流程获得可直接运行的算法脚本与配套数据并迁移到搬运、焊接等复杂工业场景。1. 让六自由度 PUMA560 在笛卡尔空间画出一个真正的圆用六自由度 PUMA560 做轨迹规划画圆很多初学者第一步就栽在“圆”的定义上他们以为给每个关节单独规划正弦或圆弧运动末端就能走出圆形。实际运行结果往往是一个扭曲的椭圆。原因在于六自由度机械臂的末端位姿由关节空间到笛卡尔空间是高度非线性的映射关节空间的简单函数对应不到笛卡尔空间的圆形轨迹。正确的做法是先在笛卡尔空间生成圆的轨迹点再通过逆运动学逐点解算关节角最后在关节空间做平滑插补。这个流程覆盖了运动学正解、逆解、轨迹插补、速度规划四个核心模块也是工业机械臂做弧焊、涂胶、打磨时的通用套路。本文以 PUMA560 为对象用 MATLAB Robotics Toolbox 给出从建模到画圆仿真的完整闭环中途会解释每个参数的含义和踩坑点适合正在做机械臂轨迹规划课程设计或入门机器人仿真的工程师。2. PUMA560 的运动学建模与逆解选型2.1 标准 DH 参数与正运动学计算PUMA560 是六自由度关节型工业机械臂的经典样机其连杆坐标系采用标准 DHDenavit-Hartenberg参数描述。标准 DH 的四个参数依次是连杆长度 a、连杆转角 alpha、连杆偏距 d、关节角 theta。PUMA560 的常用标准 DH 参数表如下关节 ia(i-1) (mm)alpha(i-1) (deg)d(i) (mm)theta(i) 范围 (deg)1000-160 ~ 1602431.8-900-225 ~ 45320.30149.09-45 ~ 22540-90433.07-110 ~ 17050900-100 ~ 10060-9056.25-266 ~ 266在 MATLAB Robotics Toolbox 中可以用Link和SerialLink直接建出模型% 建立 PUMA560 连杆对象standard DH 参数 L1 Link(d, 0, a, 0, alpha, 0, standard); L2 Link(d, 0, a, 0.4318, alpha, -pi/2, standard); L3 Link(d, 0.14909, a, 0.0203, alpha, 0, standard); L4 Link(d, 0.43307, a, 0, alpha, -pi/2, standard); L5 Link(d, 0, a, 0, alpha, pi/2, standard); L6 Link(d, 0.05625, a, 0, alpha, -pi/2, standard); puma560 SerialLink([L1 L2 L3 L4 L5 L6], name, PUMA560);这里的alpha为负值是因为标准 DH 中绕 x 轴的旋转方向需要和坐标系定义一致。PUMA560 第 2、3、4 轴的参数分别对应大臂、小臂和腕部回转这几个参数直接决定工作空间形状调参时不要随意改动。建好模型后正运动学由puma560.fkine(q)完成输入 1x6 的关节角向量输出 4x4 齐次变换矩阵。2.1.1 正运动学的验证方法正运动学算得对不对最直接的方法是让所有关节角置零检查末端坐标是否落在预期位置。零位时 PUMA560 大臂水平、小臂竖直向上末端位置大约在 x0.4521m、z0.5822m 附近。如果对不上优先检查 d 值和 alpha 的符号。2.2 逆运动学的迭代解与解析解权衡六自由度机械臂的逆运动学有解析解和数值解两条路。PUMA560 的腕部三个轴交于一点满足 Pieper 准则因此存在封闭解析解求解速度快且精度高。Robotics Toolbox 自带的ikine是数值迭代方法依赖初始猜测值在奇异位形附近容易发散而ikine6s是专门针对六自由度球形腕机械臂的解析解函数直接返回 8 组候选解。画圆轨迹规划中需要逐点求解逆运动学每两个相邻轨迹点对应的关节角变化应该连续。用解析解的话需要手动选取一组最平滑的解通常做法是保证每个关节角的变化量最小。数值解则天然具有连续性但计算量大实时性受限制。我的做法是首点用ikine6s选一组远离关节限位的解后续点以上一点的解为初值调用ikine迭代速度和稳定性都能兼顾。对应的调用代码% 初始关节角选一组远离限位的姿态 q0 [0 0 0 0 0 0]; T_target puma560.fkine(q0); % 目标位姿 q_sol puma560.ikine6s(T_target); % 解析解返回多组 q_init q_sol(1, :); % 取第一组作为初值ikine6s返回的是 8x6 矩阵每一行代表一组关节角。不同组的差异主要体现在腕部翻转和肘部上下选择时除了考虑关节限位还要考虑后续轨迹的连续性。画圆场景中末端姿态通常保持不变所以姿态部分变化小逆解选取的难度主要在于避免关节跳变。2.3 工作空间边界对画圆半径的约束PUMA560 的最大工作半径约 0.9m但画圆时圆的半径不能直接取到这个值因为在圆的某些点上机械臂可能处于奇异位形。简单估算设定圆心的 x 坐标在 0.4m 到 0.6m 之间圆的半径取 0.1m 到 0.2m 比较安全。圆心太靠近基座或太靠近工作空间边缘逆解都可能失败。判定逆解是否有效可以检查两点一是解是否在关节限位内二是puma560.ikine返回的收敛标志。Robotics Toolbox 中ikine不直接返回标志需要手动检查重投影误差T_real puma560.fkine(q_sol); err norm(T_real.t - T_target.t); % 平移误差 if err 1e-4 error(逆解失败误差过大); end这里的T.t提取的是齐次变换矩阵的平移部分也就是末端位置。误差阈值取 1e-4 米对应 0.1mm 级别对轨迹规划足够。如果误差偏大优先考虑是否是初始点选在了奇异位形附近。3. 笛卡尔空间圆弧轨迹插补的两种实现方式3.1 圆弧插补的最小实现离散点直接拟合画圆最朴素的做法是直接按角度生成圆周上的离散点然后用puma560.ikine逐点解算。这种方法适合验证整个链路是否通畅代码量很小。圆心、半径、法向量确定后圆的参数方程为p(theta) c r * (u * cos(theta) v * sin(theta))其中 c 是圆心向量u 和 v 是圆平面内的两个正交单位向量r 是半径。问题在于从一个任意姿态的法向量推导 u 和 v 需要用到叉积和归一化。如果圆的平面恰好是水平面u 可以直接取 x 轴方向v 取 y 轴方向。代码如下% 定义圆参数圆心(0.4, 0, 0.3)半径0.12平面法向量(0,0,1) center [0.4, 0, 0.3]; r 0.12; n [0, 0, 1]; % 圆的法向量此处为水平面 u [1, 0, 0]; % 圆平面内一个正交方向 v cross(n, u); % 圆平面内另一个正交方向 theta linspace(0, 2*pi, 100); % 100个离散点 points zeros(length(theta), 3); for i 1:length(theta) points(i, :) center r * (u * cos(theta(i)) v * sin(theta(i))); end这段代码生成的points是圆周上均匀分布的坐标序列。linspace(0, 2*pi, 100)表示把 0 到 2π 分成 99 段共 100 个点点数越多轨迹越平滑但逆解次数也越多实时性下降。100 个点对静态仿真合适对实时控制建议用 20 到 50 个点。3.1.1 姿态插值的处理方式画圆过程中末端姿态要保持恒定比如始终竖直向下这对应一个固定的 3x3 旋转矩阵。把每个轨迹点的位置部分替换为圆上的点组成完整的齐次变换矩阵再送逆解。代码实现% 设定末端姿态为竖直向下 R eul2rotm([0, pi, 0], ZYZ); % 绕Z轴0度绕Y轴180度使末端朝下 for i 1:size(points, 1) T_i transl(points(i, :)) * rpy2tr(0, pi, 0); % 位置姿态 q_i(i, :) puma560.ikine(T_i, q_init); % 以上一点为初值 q_init q_i(i, :); endrpy2tr(0, pi, 0)生成末端竖直向下的姿态矩阵transl生成纯平移矩阵两者相乘得到完整的位姿。这种固定姿态的画圆方式对应工业中的水平弧焊末端工具方向始终垂直于工件表面。3.2 三点定圆与任意平面圆弧插补离散点直接拟合受限于圆平面的方向遇到任意倾斜平面就要用更通用的方法。已知空间三个不共线点 p1、p2、p3可以唯一确定一个圆。这在真实工程中非常常见现场示教三个点机器人自动生成圆弧路径。圆心和半径的计算公式% 三个非共线点 p1 [0.30, -0.05, 0.25]; p2 [0.40, 0.00, 0.25]; p3 [0.30, 0.05, 0.25]; % 计算圆心 v1 p2 - p1; v2 p3 - p1; n cross(v1, v2); % 平面法向量 n n / norm(n); % 圆心在平面内用垂径定理联立方程 A [2*v1; 2*v2]; b [dot(v1, v1); dot(v2, v2)]; center p1 A \ b; % 求解线性方程组得到圆心偏移这里的核心思路是圆心到 p1、p2、p3 的距离相等因此满足 2 * v1 · (c - p1) |v1|^2 和 2 * v2 · (c - p1) |v2|^2联立求解线性方程组即可。A \ b是 MATLAB 的矩阵左除比直接求逆更稳定。有了圆心、半径和法向量再按照第 3.1 节的方法生成圆上的点。u 和 v 的选取可以这样做% 构建圆平面内的正交坐标系 u (p1 - center) / norm(p1 - center); % 从圆心指向p1作为u方向 v cross(n, u); % 保证u, v, n构成右手系 for i 1:100 ang (i-1) / 99 * 2*pi; points(i, :) center r * (u * cos(ang) v * sin(ang)); end3.2.1 不共线判断与数值稳定性三点几乎共线时叉积cross(v1, v2)的模接近零导致圆心计算发散。实际操作中先判断norm(n)是否大于一个阈值比如 1e-6小于阈值就换点重试。如果三点之间有微小扰动圆心计算结果的波动也会放大所以示教点之间的间距不要太小至少大于期望半径的十分之一。3.3 圆弧与直线混合轨迹的衔接轨迹规划中经常遇到直线接圆弧的场景比如先走一段直线再画圆。两条轨迹的衔接处需要保证位置连续和切线方向连续否则机械臂在衔接点会停顿甚至抖动。处理方法是在直线末端预留一段减速区进入圆弧前把速度降到允许值圆弧起始点的切线方向和直线段的末端方向一致。衔接实现上直线段的轨迹点数量由速度和插补周期决定圆弧段的起点就是直线段的终点。这样位置自然连续切线方向通过直线方向向量与圆弧起始点的切线方向对齐来保证。4. 关节空间速度规划与逆解连续性处理4.1 梯形速度规划的参数选取笛卡尔空间的轨迹点确定后关节空间的时间分配决定机械臂的运动快慢。常见做法是对整个轨迹做梯形速度规划加速段、匀速段、减速段。每段的时间占比直接影响轨迹的平滑度。加速时间太短加速度过大冲击明显加速时间太长总运动时间拉长效率降低。参数设置的参考值如下参数参考值说明总插补点数100点数越多轨迹越平滑但逆解计算量线性增长加速段时间比例10% ~ 20%占总时间的比例比例越大启动越平缓最大关节速度1.0 rad/sPUMA560 关节速度上限超过会触发驱动器报警最大关节加速度5.0 rad/s^2加速度过大导致末端轨迹抖动和机械振动末端运动线速度0.1 ~ 0.2 m/s画圆场景建议低速便于观察轨迹精度梯形速度规划在 MATLAB 中的实现% 梯形速度规划生成时间序列 total_time 5; % 总运动时间秒 t_seg [1, 3, 1]; % 加速、匀速、减速段时间 t_total sum(t_seg); dt 0.05; % 插补周期50ms t 0:dt:t_total; % 计算每个时间点的归一化位置 s(t) s zeros(size(t)); a_max 2 / (t_seg(1) * (t_seg(1) 2*t_seg(2) t_seg(3))); % 加速度 for i 1:length(t) if t(i) t_seg(1) s(i) 0.5 * a_max * t(i)^2; elseif t(i) t_seg(1) t_seg(2) s(i) 0.5 * a_max * t_seg(1)^2 a_max * t_seg(1) * (t(i) - t_seg(1)); else t_rem t(i) - t_seg(1) - t_seg(2); s(i) 1 - 0.5 * a_max * (t_seg(3) - t_rem)^2; end endt_seg(1)为加速段时间 1 秒t_seg(2)为匀速段时间 3 秒t_seg(3)为减速段时间 1 秒。归一化位置 s 从 0 到 1对应整条轨迹的完成度。有了 s(t) 序列后圆弧轨迹点直接从原来的等角度间隔改为按 s 的分布取点靠近首尾的位置加速度小、中间速度快整体运动更平稳。4.1.1 对逆解序列的重采样逆解是逐点进行的如果轨迹点间距不均匀关节角度序列也会出现疏密变化。把梯形规划的 s 序列映射到原始 100 个轨迹点上用interp1做重采样即可% 原始轨迹按角度均匀分布重采样为按s分布 s_orig linspace(0, 1, 100); q_resampled interp1(s_orig, q_sol, s, spline);spline插值比默认的linear更平滑但可能出现 overshoot 导致关节角超出限位用完后检查一下极值。q_sol是之前逆解得到的 100x6 关节角矩阵插值后q_resampled的行数等于 t 的长度确保每个时间点都有对应的关节角。4.2 关节角连续性与跳变检测逆解得到的关节角序列如果在某一帧发生突然跳变通常原因是选解策略改变了解分支。比如前 50 个点选的是肘部向上的解第 51 个点因为这个分支越界而切到了肘部向下的解关节 2 和关节 3 的角度瞬间变化几十度。检测跳变用相邻关节角差值的绝对值diff_q diff(q_resampled); % 相邻关节角之差 max_jump max(abs(diff_q), [], 2); % 每个时刻的最大关节变化 if max(max_jump) 0.5 % 超过0.5弧度视为跳变 warning(关节角跳变过大请检查选解策略); end0.5 弧度约 28.6 度相邻两个插补周期内超过这个值说明运动不连续。出现跳变后的处理方式是回退到跳变点之前改用另一组逆解分支重新计算。实际工程中好的选解策略是每次比较当前点所有候选解与上一关节角的距离选最近的那组这样能最大限度保证连续性。4.3 奇异位形规避的具体策略PUMA560 在腕部中心正好位于关节 1 轴线上时关节 1 和关节 4 的旋转轴重合出现腕部奇异。此时关节 4 和关节 6 的角度解不唯一逆解结果可能剧烈变化。画圆时圆的平面如果经过关节 1 轴线附近腕部奇异就难以避免。规避策略是规划前先检查整条圆轨迹上的点是否靠近奇异区域。奇异判定条件是腕部中心到关节 1 轴线的距离小于某个阈值实践中用雅可比矩阵的条件数更通用。Robotics Toolbox 中计算雅可比矩阵条件数的方式for i 1:size(q_resampled, 1) J puma560.jacob0(q_resampled(i, :)); cond_num(i) cond(J); end if max(cond_num) 1000 disp(轨迹中存在病态位形建议调整圆心位置); end条件数超过 1000 表示雅可比矩阵接近奇异末端微小的速度变化需要极大的关节速度来响应。此时调整圆心位置让圆的轨迹避开奇异区域或者在奇异点附近降低速度二选一。降速实现简单但会拉长总运动时间调整圆心则更本质。5. 仿真验证与误差分析5.1 完整画圆仿真代码与轨迹可视化把前文的模块串起来写一个完整脚本% 定义PUMA560 L1 Link(d, 0, a, 0, alpha, 0, standard); L2 Link(d, 0, a, 0.4318, alpha, -pi/2, standard); L3 Link(d, 0.14909, a, 0.0203, alpha, 0, standard); L4 Link(d, 0.43307, a, 0, alpha, -pi/2, standard); L5 Link(d, 0, a, 0, alpha, pi/2, standard); L6 Link(d, 0.05625, a, 0, alpha, -pi/2, standard); puma560 SerialLink([L1 L2 L3 L4 L5 L6], name, PUMA560); % 圆参数 center [0.45, 0, 0.30]; r 0.12; n [0, 0, 1]; u [1, 0, 0]; v [0, 1, 0]; % 生成轨迹点 theta linspace(0, 2*pi, 80); points center r * (u*cos(theta) v*sin(theta)); % 逆解首点用解析解后续用数值迭代 T_start transl(points(1,:)) * rpy2tr(0, pi, 0); q_init puma560.ikine6s(T_start); q_traj zeros(size(points, 1), 6); q_traj(1, :) q_init(1, :); for i 2:size(points, 1) T_i transl(points(i,:)) * rpy2tr(0, pi, 0); q_traj(i, :) puma560.ikine(T_i, q_traj(i-1, :)); end % 验证末端轨迹 T_actual puma560.fkine(q_traj); pos_actual transl(T_actual); plot3(points(:,1), points(:,2), points(:,3), b-); hold on; plot3(pos_actual(:,1), pos_actual(:,2), pos_actual(:,3), r--);transl(T_actual)从齐次变换矩阵批量提取位置坐标。蓝色实线是理想圆红色虚线是实际末端轨迹两者应该几乎重合。rpy2tr(0, pi, 0)指定的姿态如果在某个轨迹点上超出了 PUMA560 的腕部可达范围逆解会发散表现为红色轨迹突然偏离蓝色轨迹。5.1.1 逆解失败时的定位方法逆解失败时先打印出失败点的位置坐标检查是否在工作空间内。然后尝试减小半径或调整圆心位置。最后一个原因是姿态设置不合理手腕翻转角度过大导致关节 5 超出限位。针对这三个原因逐个排查就能定位问题。5.2 画圆精度评估与误差指标轨迹规划的质量用实际轨迹与理想圆的偏差评估。最直接的是径向误差把实际位置投影到圆平面计算到圆心的距离与半径之差% 计算径向距离误差 dist vecnorm(pos_actual - center, 2, 2); radial_err dist - r; figure; plot(radial_err, LineWidth, 1.5); ylabel(径向误差 (m)); xlabel(轨迹点序号); grid on;径向误差应该在毫米量级以内。如果误差呈现周期性震荡大概率是插补点数量不足导致包络线内切如果误差呈现单调偏移说明圆心或半径计算有误。检查一下center和r的定义即可定位。除了径向误差还可以画关节角曲线来评估运动平滑度。关节速度和加速度可以由相邻点位差分得到如果速度曲线有明显突变说明梯形规划的时间分配和实际轨迹不匹配。5.3 从静态仿真到实时控制的差异仿真环境和真实控制器有三处关键差异插补周期、动力学模型、通信延迟。仿真里插补周期可以设为 50ms但真实控制器通常要求 1ms 到 8ms 的插补周期。在仿真中验证通过的轨迹点序列放到实时系统里需要预先离线计算所有逆解结果运行时只做查表和插值不能在控制周期内逐点做逆解运算。动力学层面的差异更隐蔽仿真只验证了几何可达性和运动学连续性没有验证关节力矩是否超限。快速画圆时关节 2 的负载力矩可能超过电机额定扭矩导致实际轨迹滞后于规划轨迹。这是六自由度机械臂轨迹规划从仿真到实物最常遇到的坑。解决办法有两个方向一是降低末端速度二是预先用动力学仿真验证力矩曲线确保所有关节力矩都在额定范围内。6. 提高画圆轨迹精度的三个实战技巧6.1 首末点闭合处的平滑处理整圆轨迹首尾相接首点和末点是同一个位置但按照 0 到 2π 采样时首点的关节速度和末点的关节速度通常不同导致闭环瞬间出现速度突变。解决办法是在梯形速度规划中把首尾速度设为零给出一小段加减速缓冲。具体做法是在 θ0 和 θ2π 之间插入过渡点让机械臂从静止开始加速在闭合点之前减速到零再进入下一圈。这个技巧对连续画多个圆的场景特别重要否则每一圈衔接处都会出现顿挫。构造连续多圈轨迹时第二圈的起点就是第一圈的终点但速度不为零。处理方式是去掉两圈之间的零速段把速度规划首尾相接只保留第一圈的加速段和最后一圈的减速段。MATLAB 中可以把两圈的轨迹点拼接后再做速度规划而不是每圈各做一次。6.2 末端姿态抖动抑制画圆时如果末端的姿态是固定方向逆解过程中腕关节会承担姿态补偿任务。当圆的平面法向量与末端工具的 z 轴方向不一致时腕关节角度会出现高频小幅抖动原因是逆解数值迭代在姿态部分收敛精度不足。抑制方法是把姿态误差阈值调小即提高逆解的收敛精度。Robotics Toolbox 的ikine可以通过选项设置容差q_i puma560.ikine(T_i, q_init, tol, 1e-10, ilimit, 500);tol是迭代收敛容差默认值可能为 1e-6对姿态要求高的轨迹需要收紧到 1e-10。ilimit是最大迭代次数容差变小后可能需要更多迭代才能收敛。代价是单点逆解耗时增加实时控制场景必须离线算好不能在线逐点调用。6.3 用前馈补偿减小跟踪误差关节空间的 PID 跟踪控制存在固有滞后末端画圆时轨迹曲率恒定滞后会导致实际的圆比规划的圆小一圈。常用对策是在速度规划阶段加入前馈项提前补偿滞后量。前馈的基本形式是速度前馈加上加速度前馈加速度项对圆弧轨迹尤为重要因为它提供向心力补偿。工业控制器中的前馈系数通常在调试模式下在线整定先设速度前馈为 0只调加速度前馈观察径向误差的变化。当径向误差的幅值减小、相位不再滞后时前馈系数基本到位。这个技巧在仿真中体现不出来但确实是现场调轨迹精度时最常用、最有效的手段。本文还有配套的精品资源点击获取
