机械臂轨迹优化：五次多项式插值与粒子群算法实践

1. 项目背景与核心思路

去年调试六轴机械臂时遇到一个典型问题：示教点位间直线运动导致末端抖动剧烈，工件加工表面出现明显振纹。传统解决方案要么增加过渡点位（降低效率），要么调低运动速度（影响产能）。于是尝试用多项式插值生成平滑轨迹，再结合粒子群算法优化运动参数，最终在保证精度的前提下将节拍时间缩短了23%。

这种轨迹规划方法特别适合需要连续平滑运动的场景，比如激光切割、弧焊、涂胶等工艺。核心思路分两步走：先用五次多项式插值生成候选轨迹，再用改进粒子群算法从能耗、时间、平滑性等多目标中寻找帕累托最优解。

2. 多项式插值实现细节

2.1 为什么选择五次多项式

三次多项式虽然计算量小，但加速度不连续会导致机械臂关节冲击。七次以上多项式又容易产生超调。经过实测对比，五次多项式在x(t)=a0+a1t+a2t²+a3t³+a4t⁴+a5t⁵这个经典形式下，既能保证加加速度（jerk）连续，又不会引入过多震荡。

关键约束条件包括：

• 起点/终点位置、速度、加速度匹配
• 中间路径点位置连续
• 关节角度、角速度不超过限值

2.2 系数求解的数值技巧

构建方程组时发现，直接求逆矩阵在路径点较多时会出现病态问题。后来改用分段拼接法：

1. 将整个轨迹按via points分成若干段
2. 每段单独用五次多项式表示
3. 在连接点施加C²连续性约束（位置、速度、加速度相等）

实测用QR分解求解比常规高斯消元稳定，特别是在处理20个以上路径点时，残差能控制在1e-6以内。MATLAB代码如下：

matlab复制% 构建约束矩阵A
A = zeros(6*n, 6*n); 
for i = 1:n
    t = [0; via_time(i)]; % 分段起止时间
    A(6*i-5:6*i, 6*i-5:6*i) = [ones(2,1), t, t.^2, t.^3, t.^4, t.^5;
                                zeros(1,6); 
                                1, 2*t(2), 3*t(2)^2, 4*t(2)^3, 5*t(2)^4;
                                0, 2, 6*t(2), 12*t(2)^2, 20*t(2)^3];
end

% 添加连续性约束
for i = 1:n-1
    A(6*n+i, 6*i-5:6*i) = [0 0 0 0 0 1]; % 前段末项
    A(6*n+i, 6*i+1:6*i+6) = -[1 via_time(i+1) via_time(i+1)^2 via_time(i+1)^3 via_time(i+1)^4 via_time(i+1)^5]; % 后段首项
end

3. 粒子群优化算法改进

3.1 适应度函数设计

需要同时优化三个目标：

1. 运动时间T（越小越好）
2. 能量消耗E=∫τ²dt（τ为关节力矩）
3. 平滑性指标J=∫jerk²dt

通过加权法转化为单目标：

math复制fitness = w1*T + w2*E + w3*J

权重系数建议初始设为w1=0.6, w2=0.3, w3=0.1，后期可根据实际需求调整。

3.2 参数编码与更新策略

每个粒子代表一组轨迹参数：

• 位置变量：各路径点的时间分配（归一化到[0,1]）
• 速度变量：时间调整步长

创新点在于引入动态惯性权重：

python复制w = w_max - (w_max - w_min) * (iter / max_iter)

实测w_max=0.9, w_min=0.4时收敛速度最快。同时采用约束处理技术，当粒子飞出可行域时，将其拉回边界并反转速度分量。

4. 工程实现关键点

4.1 实时性保障技巧

1. 预计算+缓存：提前离线计算常用轨迹模板
2. 并行计算：用OpenMP加速粒子群评估
3. 降维处理：对对称性路径合并优化变量

4.2 安全保护机制

必须实现的硬性约束：

c++复制if(任何关节角度 > 限值 || 任何关节速度 > 限值)
    适应度 = INF; // 直接淘汰该粒子

建议增加软约束惩罚项：

math复制fitness += 1000 * max(0, qdot - qdot_max)^2

5. 实测效果对比

在UR5机械臂上进行雕刻作业测试：

振动数据用IMU采集，采样频率1kHz。可见在保证精度的前提下，各项指标均有显著提升。

6. 常见问题排查

Q1：轨迹出现尖峰怎么办？

• 检查路径点是否过于密集
• 尝试增加平滑性权重w3
• 确认动力学参数（质量、惯量）输入正确

Q2：优化时间过长？

• 减少粒子数量（建议30-50个）
• 限制最大迭代次数（100-200次）
• 先用粗粒度优化，再用结果作为细优化的初值

Q3：实际运动与仿真不一致？

• 检查传动间隙补偿参数
• 确认摩擦力矩模型准确性
• 尝试在适应度函数中加入实际编码器反馈

这个方案最让我惊喜的是对老旧设备的兼容性——在某台服役8年的SCARA机械臂上实施后，居然将重复定位精度从±0.1mm提升到了±0.06mm。后来发现是因为平滑轨迹减少了传动部件的反向间隙影响。如果你们也在用传统示教方式，强烈建议试试这种优化方法。