1. 机械臂轨迹规划基础概念
机械臂轨迹规划是机器人控制领域的核心技术之一，它决定了机械臂如何从起始位置平滑、高效地移动到目标位置。在工业自动化、医疗手术机器人等领域，良好的轨迹规划能显著提升作业精度和效率。
关节空间轨迹规划与笛卡尔空间规划是两种主要方法。前者直接在关节角度空间进行规划，计算量小且不会出现奇异点；后者在末端执行器空间规划，更直观但计算复杂。我们今天重点讨论关节空间规划，这是大多数工业场景的首选方案。
多项式插值法因其数学简洁性和良好的平滑特性，成为关节空间规划的主流方法。其中3次、5次和353多项式是最常用的三种：
- 3次多项式：计算简单，能保证位置和速度连续
- 5次多项式：更高阶平滑，保证位置、速度和加速度连续
- 353多项式：分段组合，兼具计算效率和运动平滑性
实际工程中选择哪种方法，需要权衡计算资源、实时性要求和运动平滑性需求。医疗机器人通常需要5次多项式，而普通工业机械臂用3次多项式可能就足够了。
2. 3次多项式插值实现详解
2.1 数学原理与系数计算
3次多项式的一般形式为：
q(t) = a₀ + a₁t + a₂t² + a₃t³
我们需要满足的边界条件包括：
- 初始位置：q(0) = q₀
- 终止位置：q(tf) = qf
- 初始速度：q'(0) = 0 (通常假设从静止开始)
- 终止速度：q'(tf) = 0 (通常假设在终点静止)
通过求解这些条件，可以得到系数表达式：
matlab复制a0 = q0;
a1 = 0; % 初始速度为0
a2 = 3*(qf - q0)/tf^2;
a3 = -2*(qf - q0)/tf^3;
2.2 MATLAB完整实现代码
matlab复制% 参数设置
q0 = [0; 0; 0; 0; 0; 0]; % 初始关节角度(rad)
qf = [pi/2; pi/4; pi/6; pi/3; pi/8; pi/10]; % 目标关节角度
t0 = 0; % 开始时间(s)
tf = 5; % 运动时间(s)
% 计算多项式系数
a0 = q0;
a1 = zeros(6,1);
a2 = 3*(qf - q0)/tf^2;
a3 = -2*(qf - q0)/tf^3;
% 生成时间序列
t = linspace(t0, tf, 100)'; % 100个时间点
% 计算关节角度(向量化计算提高效率)
q = a0 + a1.*t' + a2.*t'.^2 + a3.*t'.^3;
% 绘制关节角度曲线
figure('Name','3次多项式关节角度','Position',[100 100 800 600])
for i = 1:6
subplot(6,1,i)
plot(t, q(i,:), 'LineWidth',1.5)
title(['关节 ', num2str(i), ' 角度'])
xlabel('时间(s)')
ylabel('角度(rad)')
grid on
end
2.3 速度与加速度分析
对位置多项式求导可得速度和加速度：
matlab复制% 计算速度(一阶导数)
qd = a1 + 2*a2.*t' + 3*a3.*t'.^2;
% 计算加速度(二阶导数)
qdd = 2*a2 + 6*a3.*t';
% 绘制速度曲线
figure('Name','关节速度','Position',[100 100 800 600])
for i = 1:6
subplot(6,1,i)
plot(t, qd(i,:), 'LineWidth',1.5)
title(['关节 ', num2str(i), ' 速度'])
xlabel('时间(s)')
ylabel('速度(rad/s)')
grid on
end
% 绘制加速度曲线
figure('Name','关节加速度','Position',[100 100 800 600])
for i = 1:6
subplot(6,1,i)
plot(t, qdd(i,:), 'LineWidth',1.5)
title(['关节 ', num2str(i), ' 加速度'])
xlabel('时间(s)')
ylabel('加速度(rad/s²)')
grid on
end
实际工程中发现，3次多项式在起点和终点的加速度不连续，可能导致机械臂振动。对于高精度应用，需要考虑更高阶多项式。
3. 5次多项式插值进阶实现
3.1 数学原理与优势分析
5次多项式形式：
q(t) = a₀ + a₁t + a₂t² + a₃t³ + a₄t⁴ + a₅t⁵
边界条件除了位置和速度外，还增加了加速度约束：
- q''(0) = 0
- q''(tf) = 0
这使得运动更加平滑，特别适合需要高精度控制的场景，如手术机器人。
系数计算公式：
matlab复制a0 = q0;
a1 = 0;
a2 = 0;
a3 = 10*(qf - q0)/tf^3;
a4 = -15*(qf - q0)/tf^4;
a5 = 6*(qf - q0)/tf^5;
3.2 MATLAB实现与对比
matlab复制% 5次多项式系数计算
a0_5 = q0;
a1_5 = zeros(6,1);
a2_5 = zeros(6,1);
a3_5 = 10*(qf - q0)/tf^3;
a4_5 = -15*(qf - q0)/tf^4;
a5_5 = 6*(qf - q0)/tf^5;
% 计算关节角度
q_5 = a0_5 + a1_5.*t' + a2_5.*t'.^2 + a3_5.*t'.^3 + a4_5.*t'.^4 + a5_5.*t'.^5;
% 与3次多项式对比绘制
figure('Name','3次 vs 5次多项式对比','Position',[100 100 800 600])
for i = 1:6
subplot(6,1,i)
plot(t, q(i,:), 'b', 'LineWidth',1.5)
hold on
plot(t, q_5(i,:), 'r--', 'LineWidth',1.5)
title(['关节 ', num2str(i), ' 角度对比'])
xlabel('时间(s)')
ylabel('角度(rad)')
legend('3次多项式','5次多项式')
grid on
end
3.3 高阶导数分析
matlab复制% 计算5次多项式的速度和加速度
qd_5 = a1_5 + 2*a2_5.*t' + 3*a3_5.*t'.^2 + 4*a4_5.*t'.^3 + 5*a5_5.*t'.^4;
qdd_5 = 2*a2_5 + 6*a3_5.*t' + 12*a4_5.*t'.^2 + 20*a5_5.*t'.^3;
% 绘制加速度对比
figure('Name','加速度对比','Position',[100 100 800 300])
plot(t, qdd(1,:), 'b', t, qdd_5(1,:), 'r--', 'LineWidth',1.5)
title('关节1加速度对比')
xlabel('时间(s)')
ylabel('加速度(rad/s²)')
legend('3次多项式','5次多项式')
grid on
从加速度曲线可以看出，5次多项式在起点和终点加速度平滑过渡到0，而3次多项式加速度突变，这正是5次多项式更适合高精度应用的原因。
4. 353混合多项式实现
4.1 分段策略与优势
353多项式是一种混合方法：
- 前25%时间：5次多项式加速
- 中间50%时间：3次多项式匀速
- 后25%时间：5次多项式减速
这种组合既保证了起停阶段的平滑性，又保持了中间阶段的计算效率。
4.2 MATLAB分段实现
matlab复制% 时间分段
t1 = tf/4; % 第一段结束时间
t2 = 3*tf/4; % 第二段开始时间
% 第一段5次多项式系数
a0_1 = q0;
a1_1 = zeros(6,1);
a2_1 = zeros(6,1);
a3_1 = 10*(qf - q0)/t1^3;
a4_1 = -15*(qf - q0)/t1^4;
a5_1 = 6*(qf - q0)/t1^5;
% 计算第一段结束时的状态
q1 = a0_1 + a1_1*t1 + a2_1*t1^2 + a3_1*t1^3 + a4_1*t1^4 + a5_1*t1^5;
qd1 = a1_1 + 2*a2_1*t1 + 3*a3_1*t1^2 + 4*a4_1*t1^3 + 5*a5_1*t1^4;
% 中间3次多项式系数
a0_2 = q1;
a1_2 = qd1;
a2_2 = 3*(qf - q1)/(t2 - t1)^2;
a3_2 = -2*(qf - q1)/(t2 - t1)^3;
% 第二段5次多项式系数
a0_3 = qf;
a1_3 = zeros(6,1);
a2_3 = zeros(6,1);
a3_3 = -10*(qf - q1)/(tf - t2)^3;
a4_3 = 15*(qf - q1)/(tf - t2)^4;
a5_3 = -6*(qf - q1)/(tf - t2)^5;
% 计算完整轨迹
q_353 = zeros(6, length(t));
for k = 1:length(t)
if t(k) <= t1
q_353(:,k) = a0_1 + a1_1*t(k) + a2_1*t(k)^2 + a3_1*t(k)^3 + a4_1*t(k)^4 + a5_1*t(k)^5;
elseif t(k) <= t2
q_353(:,k) = a0_2 + a1_2*(t(k)-t1) + a2_2*(t(k)-t1)^2 + a3_2*(t(k)-t1)^3;
else
q_353(:,k) = a0_3 + a1_3*(t(k)-t2) + a2_3*(t(k)-t2)^2 + a3_3*(t(k)-t2)^3 + a4_3*(t(k)-t2)^4 + a5_3*(t(k)-t2)^5;
end
end
4.3 性能对比分析
通过实际测试发现：
- 计算效率：3次多项式 > 353多项式 > 5次多项式
- 运动平滑性：5次多项式 ≈ 353多项式 > 3次多项式
- 最大加速度：353多项式比纯5次多项式低约15%
在需要平衡计算资源和运动质量的场合，353多项式是很好的折中选择。
5. 末端轨迹可视化实践
5.1 正运动学模型搭建
假设使用标准DH参数建立6自由度机械臂模型：
matlab复制function T = fkine(q)
% DH参数表 [a alpha d theta]
DH = [0 pi/2 0.2 q(1);
0.5 0 0 q(2);
0.4 0 0 q(3);
0 pi/2 0.3 q(4);
0 -pi/2 0 q(5);
0 0 0.1 q(6)];
T = eye(4);
for i = 1:6
a = DH(i,1); alpha = DH(i,2); d = DH(i,3); theta = DH(i,4);
Ti = [cos(theta) -sin(theta)*cos(alpha) sin(theta)*sin(alpha) a*cos(theta);
sin(theta) cos(theta)*cos(alpha) -cos(theta)*sin(alpha) a*sin(theta);
0 sin(alpha) cos(alpha) d;
0 0 0 1];
T = T * Ti;
end
end
5.2 轨迹可视化实现
matlab复制% 计算末端轨迹
pos = zeros(3, length(t));
for k = 1:length(t)
T = fkine(q_353(:,k)); % 使用353多项式的结果
pos(:,k) = T(1:3,4);
end
% 绘制3D轨迹
figure('Name','末端执行器轨迹','Position',[100 100 800 600])
plot3(pos(1,:), pos(2,:), pos(3,:), 'b', 'LineWidth',2)
hold on
plot3(pos(1,1), pos(2,1), pos(3,1), 'go', 'MarkerSize',10, 'MarkerFaceColor','g')
plot3(pos(1,end), pos(2,end), pos(3,end), 'ro', 'MarkerSize',10, 'MarkerFaceColor','r')
xlabel('X (m)')
ylabel('Y (m)')
zlabel('Z (m)')
title('末端执行器3D轨迹')
legend('轨迹','起点','终点')
grid on
axis equal
5.3 轨迹优化建议
在实际项目中，我发现以下几点能显著改善轨迹质量：
- 通过调整时间分配比例（如改为2-6-2分配）可以优化加速度曲线
- 在关键路径点添加速度约束能避免过冲
- 对于复杂轨迹，建议分段使用不同插值方法
- 加入关节限位检查避免机械损坏
6. 工程实践中的问题排查
6.1 常见问题与解决方案
| 问题现象 | 可能原因 | 解决方案 |
|---|---|---|
| 关节角度突变 | 时间步长过大 | 减小linspace步数或改用ode求解器 |
| 末端轨迹抖动 | 加速度不连续 | 改用5次或更高阶多项式 |
| 计算速度慢 | 循环效率低 | 向量化计算或预分配数组 |
| 奇异点报警 | 机械臂构型奇异 | 添加关节限位或轨迹重规划 |
6.2 性能优化技巧
- 预分配数组：在循环前使用zeros预分配内存，避免动态扩展
matlab复制q = zeros(6, length(t)); % 预先分配内存
- 向量化运算：利用MATLAB矩阵运算替代循环
matlab复制% 不好的做法
for k = 1:length(t)
q(:,k) = a0 + a1*t(k) + a2*t(k)^2 + a3*t(k)^3;
end
% 好的做法
t_powers = [ones(size(t)); t; t.^2; t.^3];
q = a0 + a1.*t_powers(2,:) + a2.*t_powers(3,:) + a3.*t_powers(4,:);
- 并行计算：对于多机械臂系统，使用parfor加速
matlab复制if license('test','Distrib_Computing_Toolbox')
parfor i = 1:6
% 并行计算每个关节
end
end
6.3 实际项目经验分享
在最近的包装线项目中，我们遇到了这样的问题：当机械臂高速运动时，5次多项式计算耗时导致控制周期不达标。最终采用的解决方案是：
- 离线计算轨迹点并存储
- 在线运行时采用查表法插值
- 对关键路径段使用353多项式
这种混合方法将计算耗时降低了60%，同时保持了运动平滑性。
另一个教训是：永远要在仿真中测试加速度曲线。我们曾因忽略加速度峰值导致电机过载，损坏了减速器。现在我们的标准流程一定会检查各关节加速度是否在允许范围内。