简介面向机器人技术学习者的PUMA560六自由度机械臂轨迹规划画圆Matlab实现资料聚焦圆轨迹生成、逆运动学求解与关节空间映射。RAR压缩包内共4个文件包含3个.m脚本和1个Excel工作表整体仅13KB代码与数据分离便于逐行阅读与上机验证。目前已有近2000人学习使用适合自动化专业学生在课程设计、毕业设计中快速搭建仿真环境。资源采用五边形逼近方式规划圆形路径并基于逆运动学反解出各关节角度序列数据表可对照查看中间结果有助于理解六自由度机械臂的位姿解算与轨迹插补原理。附带脚本结构清晰可作为二次开发基础用于其他轨迹规划任务。1. 为什么画圆是检验六自由度PUMA560轨迹规划的最佳动作把六自由度PUMA560的末端画出一个正圆看起来只是让机械臂“绕一圈”实际上一旦动手做就会发现它同时把正运动学、逆运动学、雅可比、关节空间插值全部串了起来。画圆轨迹在笛卡尔空间是参数方程但落到每个关节上是时间序列而这部机器的六个轴又存在耦合和奇异位形稍微有一点姿态处理不当末端就会在圆弧中段突然抖一下。Matlab里跑通这个例子意味着你搞清了DH参数建模、逆解数值求解和五次多项式插值的衔接顺序也拿到了一个可以迁移到直线、椭圆、螺旋线轨迹的框架。如果你是刚接触六自由度机械臂轨迹规划算法或者正在做课程设计、毕设这个项目值得拆开看一遍。2. PUMA560的DH参数建模从关节角到位姿变换的第一步2.1 标准DH还是改进DH差一个字母矩阵差一列在画圆之前先把机器人的几何关系固定下来。PUMA560虽然是一台老机器人但直到今天仍然是机器人课程里讲解六自由度机械臂DH参数的标准对象。资料里同时出现过标准DH和改进DH两套写法刚开始我在这里吃过亏。标准DH把坐标系固定在连杆前端写矩阵时是Rotz(θ) * Transz(d) * Transx(a) * Rotx(α)改进DH则是Rotx(α) * Transx(a) * Rotz(θ) * Transz(d)坐标系的挂载位置和后置关节有关。用Matlab Robotics Toolbox建模时如果选择了Link(...,standard)就要在正运动学循环里用标准DH的乘法顺序否则后面的ikunc.m反解出来的关节角会有系统性偏差。我一般直接用Robotics Toolbox里的PUMA560参数既省事也方便和官方示例对照。下面的puma560_kin.m用的标准DH参数和RTB中puma560的默认值一致。2.2 PUMA560的DH参数表别把a和d记反关节θ_i (rad)d_i (m)a_i (m)α_i (rad)运动范围 (°)1000π/2-160 ~ 1602000.43180-225 ~ 45300.1500.0203-π/2-225 ~ 225400.43180π/2-110 ~ 1105000-π/2-100 ~ 10060000-200 ~ 200这几列不是随便填的。a_i与α_i描述的是相邻关节轴之间的空间关系d_i是沿关节轴方向的偏置。最容易搞混的是第三行和第四行的0.150与0.4318前者是肘部偏置后者是前臂长度。如果填反正运动学算出来的末端位置会完全偏离画圆轨迹自然不可能正确。2.3 用Matlab建立模型并验证正运动学% puma560_kin.m % 标准DH建模L(i)Link([θ d a α], standard) L1 Link([0 0 0 pi/2], standard); L2 Link([0 0 0.4318 0], standard); L3 Link([0 0.150 0.0203 -pi/2], standard); L4 Link([0 0.4318 0 pi/2], standard); L5 Link([0 0 0 -pi/2], standard); L6 Link([0 0 0 0], standard); robot SerialLink([L1 L2 L3 L4 L5 L6], name, PUMA560); q_test [0 -pi/4 pi/4 0 pi/4 0]; % 一组不奇异的手工设定关节角 T_test robot.fkine(q_test); % 末端位姿4x4齐次矩阵 disp(T_test);Link([θ d a α], standard)里四个元素对应标准DH的四个参数顺序必须和SerialLink的正运动学算法一致。q_test里第二、第三个关节取 -45° 和 45°目的是让机械臂从常见的折叠位形展开方便目视验证。fkine返回的是4x4齐次变换矩阵右上角3x1位置向量就是末端在基坐标系下的坐标。如果你用的是 Robotics Toolbox 10.xfkine默认返回SE3对象需要写成T_test robot.fkine(q_test).T;才能拿到纯矩阵。提示如果不想依赖 Robotics Toolbox也可以自己写一个dh_transform函数在循环里按标准DH乘法顺序逐连乘。只要保证ikunc.m里用的正解和这里是同一套函数即可。3. 从圆轨迹到关节角笛卡尔空间采样与逆运动学求解3.1 圆的参数方程与姿态固定画圆用极坐标参数方程最直接。圆的平面我放在z0.3m半径0.1m圆心(0.5, 0, 0.3)。这样机械臂在可达空间中部不容易碰到边界。末端姿态全程固定工具坐标系的 Z 轴垂直向下也就是旋转矩阵R rotx(pi)。为什么固定姿态因为画圆只要求位置走圆姿态如果跟着圆上的点一起旋转逆解会大幅变化容易越过关节限位。固定姿态也便于判断IK结果对不对。% circle_traj.m r 0.1; % 圆半径单位米 cx 0.5; cy 0.0; cz 0.3; % 圆心坐标 N 200; % 采样点数点数越多路径越平滑 theta linspace(0, 2*pi, N1); theta(end) []; % 去掉最后一个点避免和起点重复 x cx r*cos(theta); y cy r*sin(theta); z cz * ones(size(theta)); R_tool rotx(pi); % 固定末端姿态Z轴垂直向下 for i 1:N T_des(:,:,i) [R_tool, [x(i); y(i); z(i)]; 0 0 0 1]; endlinspace(0, 2*pi, N1)生成 N1 个点首尾都是角度 0。删除最后一个点保证整段轨迹是一个完整的圆而不是回到起点后重复采样。rotx(pi)让工具坐标系的 Z 轴指向基坐标系负 Z 方向对应画圆时末端执行器垂直向下压在桌面上。3.2 ikunc.m 的数值逆解用雅可比伪逆逼近目标位姿PUMA560 的腕部结构满足 Pieper 准则理论上可以用解析逆解。但ikunc.m这个文件从命名和实际用途看更可能是数值逆解函数。数值逆写的通用性更强换一个机械臂模型也能用代价是初始值不能给得太离谱。% ikunc.m function q ikunc(robot, T_des, q0, maxiter) q q0(:); lambda 1e-4; for k 1:maxiter T robot.fkine(q).T; e [T_des(1:3,4) - T(1:3,4); rot_error(T_des(1:3,1:3), T(1:3,1:3))]; if norm(e) 1e-8 break; end J numeric_jacobian(robot, q); dq (J*J lambda^2*eye(6)) \ J * e; % 阻尼最小二乘 q q dq; end end function er rot_error(Rd, R) % 用旋转矩阵的反对称部分近似姿态误差小角度下成立 E Rd * R; er 0.5 * [E(3,2)-E(2,3); E(1,3)-E(3,1); E(2,1)-E(1,2)]; end function J numeric_jacobian(robot, q) % 数值雅可比对每个关节加微小扰动计算位姿变化率 delta 1e-6; T0 robot.fkine(q).T; J zeros(6,6); for i 1:6 qp q; qp(i) q(i) delta; Tp robot.fkine(qp).T; dp Tp(1:3,4) - T0(1:3,4); dR Tp(1:3,1:3) * T0(1:3,1:3); dtheta 0.5 * [dR(3,2)-dR(2,3); dR(1,3)-dR(3,1); dR(2,1)-dR(1,2)]; J(:,i) [dp; dtheta] / delta; end endikunc接收四个参数机器人对象、目标位姿、初始关节角、最大迭代次数。误差向量e的前三维是位置误差后三维是小角度近似下的姿态误差。lambda是阻尼系数在关节接近奇异位形时防止dq暴涨。如果迭代以后norm(e)下降缓慢检查初始值是否离解太远或者把lambda降到1e-5再试。3.3 奇异点与雅可比条件数画圆到一半为什么会抖逆解算出所有采样点后不要急着插值。先看一下雅可比矩阵的条件数。条件数过大说明机械臂在这个位形附近接近奇异末端速度会被放大实际表现在圆轨迹中段出现一个明显的“抖点”。q_ik zeros(6,N); q_ik(:,1) [0 -pi/4 pi/4 0 pi/4 0]; % 手工选一个非奇异起始点 cond_list zeros(1,N); for i 1:N q_ik(:,i) ikunc(robot, T_des(:,:,i), q_ik(:,max(1,i-1)), 300); J numeric_jacobian(robot, q_ik(:,i)); cond_list(i) cond(J); end plot(cond_list); ylabel(Jacobian condition number); xlabel(sample index);这里把上一个采样点的逆解当作下一个点的初值能保证相邻两点角度不会跳变迭代速度也更快。如果cond_list里某些位置的条件数超过1e5说明圆弧经过了奇异点附近。常见处理是加大采样点数或者把圆心沿 X 轴方向偏移一点让整体轨迹离开奇异区域。4. 五次多项式插值让六个关节平滑通过所有采样点4.1 三次和五次差在哪速度连续不等于加速度连续逆解得到的q_ik只是一组离散角度直接用直线连接会出现速度突变。三次多项式只有四个系数只能约束起止位置和速度起止加速度不连续导致关节驱动力矩在切换点跳变。五次多项式多了两个系数可以把起止加速度也约束住运动曲线更柔和。对画圆这种连续轨迹尤其需要保证加速度连续否则末端的圆形轮廓会因为关节加减速冲击而出现局部变形。项目里的five_poly.m从命名习惯看是五次多项式插值函数。4.2 five_poly.m 的矩阵解法% five_poly.m function q five_poly(q0, qf, v0, vf, T, dt) % 五次多项式轨迹规划 % 输入起止角度、起止速度、总时间、采样间隔 % 输出从0到T的关节角序列 t 0:dt:T; M [1 0 0 0 0 0; 0 1 0 0 0 0; 0 0 2 0 0 0; 1 T T^2 T^3 T^4 T^5; 0 1 2*T 3*T^2 4*T^3 5*T^4; 0 0 2 6*T 12*T^2 20*T^3]; b [q0; v0; 0; qf; vf; 0]; a M \ b; q polyval(flipud(a), t); endM矩阵的前三行对应t0时的位置、速度、加速度约束后三行对应tT时的位置、速度、加速度约束。b里的两个0表示起止加速度为 0这是五次多项式最常用的边界条件。如果你希望末端在某个采样点不停顿可以把对应的速度约束改成非零值。polyval期望系数从高次到低次排列所以这里用flipud反转一下。4.3 把逆解序列接到 five_poly时间参数怎么定逆解序列是 6×N 的矩阵不能整块扔给一个五次多项式。常见做法是每两个相邻采样点之间调用一次five_poly并且用前后差分估计边界速度避免在中间点强制速度为零造成走走停停。% 假设 Q_ik 是 6xN 关节角序列 segT 5 / N; % 整个圆用5秒走完 dt 0.005; % 控制周期5ms仿真采样点 Q_smooth cell(1,6); for j 1:6 seg []; for i 1:N-1 q0 Q_ik(j,i); qf Q_ik(j,i1); % 边界速度用中心差分估计 if i 1 v0 (Q_ik(j,2) - Q_ik(j,1)) / segT; else v0 (Q_ik(j,i1) - Q_ik(j,i-1)) / (2*segT); end if i N-1 vf 0; else vf (Q_ik(j,i2) - Q_ik(j,i)) / (2*segT); end % 调用五次多项式生成一小段轨迹 q_seg five_poly(q0, qf, v0, vf, segT, dt); % 去除每段第一个点避免时间轴上出现重复点 if i 1 seg q_seg; else seg [seg(1:end-1), q_seg]; end end Q_smooth{j} seg; endsegT决定两个采样点之间的运动时间。想让整个圆 5 秒走完就用5/N如果想让动作更慢改成10/N即可。dt对应实际控制器周期一般取0.005或0.01。Q_smooth存成 cell 是因为每个关节的插值长度相同但分开存方便后续检查单个关节有没有超出限位。5. 主脚本流程与Excel数据导出把画圆结果固化成表5.1 把四个文件串成一个主脚本Copy_of_Untitled.m在项目里承担的就是主脚本角色。整体顺序是建立PUMA560模型生成圆形末端轨迹用ikunc逆解再用five_poly平滑最后画图或导出。其中容易漏掉的一步是逆解前要给所有笛卡尔采样点指定一个固定姿态否则IK会出现多解。% 主循环片段 robot puma560_kin(); % 自己封装的建模函数 [N, T_des] circle_traj(0.5, 0, 0.3, 0.1, 200); q_cur [0 -pi/4 pi/4 0 pi/4 0]; Q_ik zeros(6, N); for i 1:N q_cur ikunc(robot, T_des(:,:,i), q_cur, 300); Q_ik(:,i) q_cur; end5.2 把轨迹写入Excel注意单位和版本项目里的新建 XLSX 工作表.xlsx一般用来存关节角序列。推荐用writematrix代替老式的xlswrite避免在Python和Matlab之间来回倒腾时出现兼容问题。% 把平滑后的关节角整理成表第一列时间第2-7列为关节1-6 result [time(:), Q_smooth{1}(:), Q_smooth{2}(:), ... Q_smooth{3}(:), Q_smooth{4}(:), Q_smooth{5}(:), Q_smooth{6}(:)]; writematrix(result, 新建 XLSX 工作表.xlsx, Sheet, 1);需要特别注意的是关节角默认单位是弧度Excel表里最好加一行注释否则拿到其他设备上回放时会因为单位不一致出现轨迹错乱。如果后续要在Simulink里做控制建议把时间列单独存成double类型不要带datetime。5.3 验证画圆效果用正解还原末端位置并计算半径误差最后的验证方法是把平滑后的关节角重新做一遍正运动学看末端位置是否落在目标圆上。pos zeros(length(Q_smooth{1}), 3); for i 1:length(Q_smooth{1}) T_i robot.fkine([Q_smooth{1}(i), Q_smooth{2}(i), Q_smooth{3}(i), ... Q_smooth{4}(i), Q_smooth{5}(i), Q_smooth{6}(i)]).T; pos(i,:) T_i(1:3,4); end radius_err sqrt((pos(:,1)-cx).^2 (pos(:,2)-cy).^2) - r; max_abs_err max(abs(radius_err));这个误差如果小于 0.5mm说明IK和插值配合良好。如果出现周期性尖峰优先检查是否经过奇异点或者segT是否和采样周期不匹配。半径误差曲线画出来后还能直接看出圆弧的哪个角度段最容易偏离这是工程调试里最直观的反馈。本文还有配套的精品资源点击获取
