机械臂轨迹规划是机器人开发中的重头戏，今天咱们用Matlab的Robotics Toolbox带PUMA560走两步。先给机械臂充个电——初始化模型

matlab六自由度机械臂直线轨迹和圆弧轨迹。 puma560机器人仿真。

mdl_puma560 T0 = p560.fkine(qz); % 初始位姿 T1 = transl(0.4, 0.2, 0.3) * trotx(pi/3); % 目标位姿

直线轨迹的实现最省事，工具箱自带的jtraj能自动完成关节空间插值。不过想要笛卡尔空间直线运动，得用ctraj生成路径点：

t = linspace(0, 5, 100); % 5秒100个点 Ts = ctraj(T0, T1, t); % 生成轨迹序列 q_goal = p560.ikine6s(Ts); % 逆解计算 p560.plot(q_goal, 'trail', 'r') % 红色轨迹线

注意这里用了ikine6s这个专门为六轴机械臂优化的逆解算法。运行时会看到机械臂末端划出一道红色直线，过程中关节角度变化曲线用qplot(q_goal)就能可视化，能清晰看到各关节运动是否平滑。

matlab六自由度机械臂直线轨迹和圆弧轨迹。 puma560机器人仿真。

圆弧轨迹稍微费点劲。先确定圆心和半径，这里举个画半圆的例子：

theta = linspace(0, pi, 50); radius = 0.15; center = [0.3, 0, 0.2]; path = center + radius*[zeros(50,1) sin(theta') cos(theta')];

接下来得把每个路径点转换成旋转矩阵。这里有个小技巧——保持Z轴朝上：

Ts_arc = cell2mat(arrayfun(@(k) SE3(path(k,:)) * SE3.Rz(theta(k)),... 1:50, 'UniformOutput', false));

逆解计算时建议开启误差容忍，防止个别点位无解：

q_arc = p560.ikine(Ts_arc, 'tol', 0.05, 'ilimit', 1000); p560.plot(q_arc, 'trail','b','view',[60 30])

运行后会看到蓝色圆弧轨迹。如果出现关节突变，可以试试在轨迹点之间插入过渡点，或者改用五次多项式插值。实际项目中，轨迹速度规划可以加上lspb梯形速度曲线，避免机械臂启停时的冲击。

两种轨迹对比下，直线适合焊接、喷涂等场景，圆弧则在绕障碍作业时更实用。不过要注意PUMA560的工作空间限制，别让逆解计算时频繁报错。最后甩个组合拳——先走直线再画弧线：

q_total = [q_goal; q_arc]; p560.plot(q_total, 'trail','g','movie','demo.gif')

生成的gif动图直接贴报告里，比干巴巴的曲线图直观多了。轨迹规划的核心就是这些，剩下的参数调整得根据实际负载来微调了。