1. 机械臂轨迹规划基础概念
机械臂轨迹规划是机器人控制领域的核心技术之一，它决定了机械臂如何从初始位置平滑、高效地运动到目标位置。在关节空间轨迹规划中，我们需要为每个关节生成随时间变化的运动曲线，确保机械臂的运动满足以下基本要求：
- 位置连续性：关节角度必须连续变化
- 速度平滑性：避免速度突变导致的机械振动
- 加速度可控：防止过大加速度造成机械冲击
提示：关节空间规划与笛卡尔空间规划的主要区别在于，前者直接规划各关节运动，后者规划末端执行器在三维空间中的路径。
2. 多项式插值法原理与实现
2.1 三次多项式插值法
三次多项式是最基础的关节轨迹规划方法，其一般形式为：
q(t) = a₀ + a₁t + a₂t² + a₃t³
在Matlab中实现时，我们需要考虑以下关键点：
- 边界条件设置：
- 初始位置：q(0) = q₀
- 终止位置：q(tf) = qf
- 初始速度：q'(0) = 0
- 终止速度：q'(tf) = 0
matlab复制% 三次多项式系数计算（改进版）
function [a0, a1, a2, a3] = cubic_poly(q0, qf, t0, tf)
a0 = q0;
a1 = zeros(size(q0));
a2 = 3*(qf - q0)/(tf^2);
a3 = -2*(qf - q0)/(tf^3);
end
实际应用中的注意事项：
- 当关节运动范围较大时，建议分段使用三次多项式
- 对于旋转关节，注意角度归一化处理（-π到π之间）
- 时间参数tf的选择应考虑各关节最大速度限制
2.2 五次多项式插值法
五次多项式提供了更高阶的平滑性，其一般形式为：
q(t) = a₀ + a₁t + a₂t² + a₃t³ + a₄t⁴ + a₅t⁵
相比三次多项式，五次多项式可以额外控制起点和终点的加速度：
matlab复制% 五次多项式系数计算
function [a0, a1, a2, a3, a4, a5] = quintic_poly(q0, qf, t0, tf)
a0 = q0;
a1 = zeros(size(q0));
a2 = zeros(size(q0));
a3 = 10*(qf - q0)/(tf^3);
a4 = -15*(qf - q0)/(tf^4);
a5 = 6*(qf - q0)/(tf^5);
end
五次多项式的主要优势：
- 起点和终点加速度为零，运动更平滑
- 更适合高速运动的机械臂
- 减少机械振动和冲击
3. 混合多项式方法（353多项式）
3.1 353多项式原理
353多项式结合了三次和五次多项式的优点：
- 起始段（0-25%）：五次多项式
- 中间段（25-75%）：三次多项式
- 结束段（75-100%）：五次多项式
这种组合方式既保证了运动起始/结束时的平滑性，又保持了中间段的高效性。
3.2 Matlab实现关键点
matlab复制% 353多项式时间区间划分
t1 = tf/4; % 第一段结束时间
t2 = 3*tf/4; % 第二段开始时间
t_mid = t2 - t1; % 中间段持续时间
% 第一段五次多项式计算
[a0_1, a1_1, a2_1, a3_1, a4_1, a5_1] = quintic_poly(q0, q1, t0, t1);
% 中间段三次多项式计算
q1 = polyval([a5_1 a4_1 a3_1 a2_1 a1_1 a0_1], t1);
qd1 = polyval([5*a5_1 4*a4_1 3*a3_1 2*a2_1 a1_1], t1);
[a0_2, a1_2, a2_2, a3_2] = cubic_poly(q1, q2, 0, t_mid);
% 结束段五次多项式计算
q2 = polyval([a3_2 a2_2 a1_2 a0_2], t_mid);
qd2 = polyval([3*a3_2 2*a2_2 a1_2], t_mid);
[a0_3, a1_3, a2_3, a3_3, a4_3, a5_3] = quintic_poly(q2, qf, 0, tf-t2);
4. 完整轨迹规划实现
4.1 多关节同步规划
对于六自由度机械臂，需要为每个关节单独规划轨迹：
matlab复制% 初始化参数
dof = 6; % 自由度数量
q0 = zeros(dof, 1); % 初始关节角度
qf = [pi/2; pi/4; pi/6; pi/3; pi/8; pi/10]; % 目标关节角度
tf = 5; % 总运动时间
t_samples = 100; % 时间采样点数
% 预分配内存
t = linspace(0, tf, t_samples);
q = zeros(dof, t_samples);
qd = zeros(dof, t_samples);
qdd = zeros(dof, t_samples);
% 为每个关节计算轨迹
for j = 1:dof
% 计算多项式系数
[a0, a1, a2, a3] = cubic_poly(q0(j), qf(j), 0, tf);
% 计算位置、速度、加速度
q(j,:) = a0 + a1*t + a2*t.^2 + a3*t.^3;
qd(j,:) = a1 + 2*a2*t + 3*a3*t.^2;
qdd(j,:) = 2*a2 + 6*a3*t;
end
4.2 轨迹可视化
完整的可视化代码应包括：
matlab复制% 绘制关节角度曲线
figure('Name', 'Joint Angles', 'Position', [100 100 800 600]);
for j = 1:dof
subplot(dof,1,j);
plot(t, q(j,:), 'LineWidth', 1.5);
title(sprintf('Joint %d Angle', j));
xlabel('Time (s)');
ylabel('Angle (rad)');
grid on;
end
% 绘制关节速度曲线
figure('Name', 'Joint Velocities', 'Position', [100 100 800 600]);
for j = 1:dof
subplot(dof,1,j);
plot(t, qd(j,:), 'LineWidth', 1.5, 'Color', [0.85 0.33 0.10]);
title(sprintf('Joint %d Velocity', j));
xlabel('Time (s)');
ylabel('Velocity (rad/s)');
grid on;
end
% 绘制关节加速度曲线
figure('Name', 'Joint Accelerations', 'Position', [100 100 800 600]);
for j = 1:dof
subplot(dof,1,j);
plot(t, qdd(j,:), 'LineWidth', 1.5, 'Color', [0.49 0.18 0.56]);
title(sprintf('Joint %d Acceleration', j));
xlabel('Time (s)');
ylabel('Acceleration (rad/s²)');
grid on;
end
5. 末端轨迹绘制与验证
5.1 正运动学实现
要绘制末端轨迹，首先需要实现正运动学计算。以常见的UR5机械臂为例：
matlab复制function T = ur5_fkine(q)
% UR5机械臂DH参数
a = [0, -0.425, -0.39225, 0, 0, 0];
d = [0.089159, 0, 0, 0.10915, 0.09465, 0.0823];
alpha = [pi/2, 0, 0, pi/2, -pi/2, 0];
T = eye(4);
for i = 1:6
ct = cos(q(i));
st = sin(q(i));
ca = cos(alpha(i));
sa = sin(alpha(i));
Ti = [ct, -st*ca, st*sa, a(i)*ct;
st, ct*ca, -ct*sa, a(i)*st;
0, sa, ca, d(i);
0, 0, 0, 1];
T = T * Ti;
end
end
5.2 末端轨迹计算与可视化
matlab复制% 计算末端轨迹
ee_pos = zeros(3, t_samples);
for k = 1:t_samples
T = ur5_fkine(q(:,k));
ee_pos(:,k) = T(1:3,4);
end
% 绘制三维轨迹
figure('Name', 'End-Effector Trajectory');
plot3(ee_pos(1,:), ee_pos(2,:), ee_pos(3,:), 'LineWidth', 2, 'Color', 'b');
hold on;
plot3(ee_pos(1,1), ee_pos(2,1), ee_pos(3,1), 'ro', 'MarkerSize', 8, 'MarkerFaceColor', 'r');
plot3(ee_pos(1,end), ee_pos(2,end), ee_pos(3,end), 'go', 'MarkerSize', 8, 'MarkerFaceColor', 'g');
xlabel('X (m)');
ylabel('Y (m)');
zlabel('Z (m)');
title('End-Effector Trajectory in Cartesian Space');
legend('Trajectory', 'Start Point', 'End Point');
grid on;
axis equal;
6. 实际应用中的问题与解决方案
6.1 奇异点规避
在轨迹规划时需要注意机械臂的奇异位形：
matlab复制% 检查雅可比矩阵条件数
function [is_singular, cond_num] = check_singularity(q)
J = compute_jacobian(q); % 需要实现雅可比计算
cond_num = cond(J);
is_singular = cond_num > 1e6; % 阈值可根据实际情况调整
end
6.2 关节限位处理
实际机械臂都有关节运动范围限制：
matlab复制% 关节限位检查
function [is_valid, violated_joint] = check_joint_limits(q, q_min, q_max)
is_valid = all(q >= q_min) && all(q <= q_max);
violated_joint = find(q < q_min | q > q_max, 1);
end
6.3 轨迹优化建议
- 速度规划：采用S曲线速度规划减少冲击
- 时间最优：根据各关节最大速度/加速度自动优化运动时间
- 碰撞检测：结合环境模型进行实时碰撞检测
7. 性能评估与比较
7.1 不同方法的对比指标
| 指标 | 三次多项式 | 五次多项式 | 353多项式 |
|---|---|---|---|
| 计算复杂度 | 低 | 中 | 高 |
| 平滑性 | 一般 | 优秀 | 优秀 |
| 实时性 | 优秀 | 良好 | 一般 |
| 适用场景 | 低速运动 | 高速运动 | 精密控制 |
7.2 选择建议
- 对于简单、低速的应用，三次多项式足够
- 需要高平滑性的场合选择五次多项式
- 对运动过程有精细控制要求的考虑353多项式
在实际项目中，我通常会先使用五次多项式进行初步规划，然后在性能瓶颈处考虑使用353多项式优化特定区段的运动特性。