机械臂的轨迹规划，说白了就是在“怎么走”和“走多快”之间做交易。生产线节拍压得紧，机械臂末端就得跑得更快，但速度一上来，关节的冲击和振动也跟着上来，时间久了螺丝都能给你抖松。我做过的一个很典型的项目——基于改进鲸鱼算法的六自由度机械臂353多项式时间最优轨迹规划，核心就是解决这个矛盾：让六自由度机械臂沿着一条经过预设定路点的路径运动，用3-5-3分段多项式来描述轨迹，再用改进的鲸鱼优化算法去搜索三个时间段的最佳配比，在总时间最短的同时，保证每个关节的速度、加速度、加加速度都不超过硬件极限。

这类项目不是学术圈自嗨，在工业点焊、搬运、涂胶这些场景里很常见。机械臂要走一条固定路径，路径上必须经过某些中间点（要绕过夹具，或者要在特定姿态下作业），每个点之间花多长时间是可调的，你当然希望整段跑得最快。但约束一大堆：关节电机转速上限、减速机承受的加速度极限、末端不允许的振动……单靠手调参数又慢又保守，所以我当时的思路很直接：先把轨迹数学化，再把约束写清楚，最后交给优化算法去搜。这篇内容就把这套完整流程拆开讲一遍，适合正在做机械臂路径规划、运动控制课题的同学参考，也适合想让仿真里的轨迹规划真正落到实机上的工程师。

1. 项目整体设计思路与方案选型

机械臂轨迹规划的项目，第一步从来不是写代码，而是把“路径”和“轨迹”这两件事分开。路径只关心空间几何关系，就是你从A点到B点要不要经过C点；轨迹则要给路径加上时间信息，也就是每个时刻机械臂各个关节的角度、角速度、角加速度是多少。本项目里路径是固定的：一条经过两个中间路点、包含起点和终点的典型折返路径，需要做的是在这个固定路径上规划时间信息，所以这个问题的自由度其实只在“时间”这一维。

1.1 核心痛点：快与稳之间的矛盾

先聊聊为什么这个问题值得做。一个六自由度机械臂在运动时，每个关节都由伺服电机驱动，电机的转速上限给出了关节速度约束，减速机和机械结构能承受的动态载荷给出了加速度约束，而加加速度（jerk）约束则直接关系到机械臂的振动水平和平稳性。如果只追求最快，优化算法很容易把时间压到极限，结果是速度贴着上限走，加减速过程极其生硬，动作末端明显点头，机械臂在停止位置反复振荡，这在真机上根本没法用。

我在实际调试中遇到过最典型的情况：仿真里跑得好好的轨迹，到了真机上在路点附近就抖个不停，检查下来发现加速度曲线在路点处出现了很大的跳变，加加速度早就爆表。所以说，时间最优问题里“最优”绝对不是单纯让时间最短，而是在所有物理约束都不越界的可行域内找最短时间。这就成了一个带约束的非线性优化问题，而且目标函数没有简单的解析梯度，传统方法很难啃得动。

1.2 轨迹方案选型：为什么偏偏是3-5-3多项式

轨迹的描述方式有很多，单段多项式最简单，但问题也很明显。三次多项式只能保证位置和速度连续，加速度在启停时刻是突变的，机械臂在起步和收尾时有明显冲击；五次多项式在端点处能同时约束位置、速度和加速度，轨迹柔顺很多，但要用单一的五次多项式去穿过多个指定路点，相当于在高次多项式上做多点插值，容易产生数值振荡，系数也可能非常不稳定。七次多项式更平滑，但求解和调节的计算量翻了好几倍，工程上“够用就好”的原则让我不想一上来就上高次。

3-5-3分段多项式是中间路线：整条轨迹分成三段，第一段和第三段用三次多项式，保证起点和终点的速度、加速度边界条件好处理；中间段用五次多项式，保证通过中间路点时的加速度连续。三段拼接后，位置、速度、加速度在整个时间轴上都是连续的。这里有个很关键的好处——分段之后，每段的时间长度 t1、t2、t3 是独立的变量，优化算法不需要去调多项式系数，只需要去调这三个时间，决策维度降到了3维，搜索空间大大缩小。对于六轴机械臂，6个关节各自独立构造3-5-3多项式，但三段时间是全局共享的，这样优化目标就统一了。

具体来说，第一段关节角位移 θ(t) 写作 a0 + a1·t + a2·t² + a3·t³，第二段是 b0 + b1·t + b2·t² + b3·t³ + b4·t⁴ + b5·t⁵，第三段又是三次多项式。三段总共 4 + 6 + 4 = 14 个待定系数，边界条件足够把它们全部解出来：起点处位置、速度、加速度已知（速度加速度通常设为0），终点处位置、速度、加速度已知，两个中间路点的位置已知，再加上路点之间速度、加速度连续条件。代码实现时一般是先构建系数矩阵，解线性方程组。这里需要提醒的是，求解时要注意矩阵条件数，尤其是某段时间过短时矩阵会接近奇异，后文会专门讲这个坑。

1.3 优化器选型：为什么是鲸鱼算法，以及它哪里不够用

时间最优优化的难点在于约束多，适应度曲面崎岖。当时我对比了几类方案。梯度下降类算法首先被排除，因为目标函数对时间参数的梯度写起来很麻烦，而且约束都是不等式形式，处理起来又要投影又要拉格朗日乘子，工程实现成本太高；遗传算法和粒子群都是可以的群体智能方案，但前者要调选择、交叉、变异一堆算子，后者需要调惯性权重和两个加速系数，调参环节多，烦。标准鲸鱼优化算法参数天然少，核心机制就三个：包围猎物、螺旋气泡网攻击、随机探索，实现几十行代码就能跑起来，所以在项目里选它作为基线。

但直接用标准WOA跑这个带约束问题，效果一般。最明显的问题是，标准WOA的初始解完全随机，在约束惩罚下大量个体一开始就落在不可行区域，浪费前半程的迭代；收敛因子线性递减，中后期探索能力衰减太快，种群容易早熟；加上多约束惩罚函数构造的适应度曲面坑坑洼洼，算法经常收敛到局部最优，算出来的总时间比理论期望值差一截。所以我在项目里做了三处针对性的改进：混沌映射初始化、非线性收敛因子、精英差分变异。这三处的设计细节和代码实现，后面专门用一章来说。

2. 3-5-3多项式轨迹的构建与约束体系

轨迹构建是整个方案的地基。这个环节如果做得不严谨，后面优化算法改得再好也白搭，因为优化搜索出来的时间参数最终都要落到轨迹系数和质量检验上。

2.1 轨迹的数学表达与边界条件

3-5-3多项式轨迹在关节空间里定义。对于每个关节，假设三段轨迹的时间分别是 t1、t2、t3。为了推导方便，通常把每一段的时间都归一化到 [0,1] 区间，也就是每段用无量纲的时间参数 u = (t - t_start) / T_segment 来表示。这样求导的时候需要额外乘上 1/T_segment，但数值稳定性会好很多。

第一段轨迹用三次多项式：θ1(u) = a10 + a11·u + a12·u² + a13·u³，u∈[0,1]。起点的位置、速度、加速度已知，速度加速度通常设为0，第一段末位置为第一个路点。三个条件对应三次多项式的4个系数还差一个——实际上三次多项式刚好可以由4个独立条件唯一确定：位置、速度、加速度、末端位置，正好4个。但这样第一段末速度、末加速度就被固定了，第二段五次多项式的起点速度、加速度就必须等于第一段末的值，这就是段间连续条件的来由。

第二段五次多项式：θ2(u) = a20 + a21·u + a22·u² + a23·u³ + a24·u⁴ + a25·u⁵，u∈[0,1]。它的边界条件是：起点位置等于第一个路点，起点速度等于第一段末速度，起点加速度等于第一段末加速度，末端位置等于第二个路点。这几个条件加起来只有4个，解决不了6个系数，还需要补充末端速度和末端加速度条件——但末端速度和加速度在第三段那里是连续的，不能直接随便指定，所以需要把三段联立起来求解。

在实际实现中，我用的方式是把三段的系数一起放进一个大的线性方程组里，把所有位置、速度、加速度的约束和段间连续条件都写进去，构建一个14阶的线性系统（14个未知系数，14个方程）。这样解出来的是全局满足连续条件的唯一轨迹，不会出现三段各自求解导致接口处对不上的问题。公式在文本里写开比较冗长，核心思路就是把角位移、角速度、角加速度的表达统一写成关于 u 的矩阵形式，然后根据边界条件填充系数矩阵。

2.2 从关节约束到适应度函数的转换

轨迹求出来之后，接下来最重要的一步是把机械臂的物理限制变成优化算法能用的惩罚项。一个完整的六自由度机械臂，每个关节都有限速、限加速度、限加加速度。以我的实验机械臂为例，关节1的角速度上限我设成 180°/s，加速度上限 400°/s²，加加速度上限 800°/s³；其余关节量级类似，但具体数值不同。这些参数通常能在电机和减速机的选型手册里查到，或者通过振动测试标定出来。

对某个给定的时间参数组合 (t1, t2, t3)，我先算出6个关节各自的三段多项式系数，然后在一个精细的时间网格上采样，比如每段分成200步，计算出每个采样时刻的角速度、角加速度、角加加速度，再分别除以对应关节的上限值，取超限比例的最大值。如果所有关节都满足约束，超限比例就等于0；如果有超限，就把超限比例累加起来作为惩罚项。这样设计出来的惩罚项能直观反映“超了多少”，而不是简单返回一个0/1标志，给优化算法提供方向信息，收敛会快很多。

2.3 目标函数与惩罚系数调节

适应度函数 = 总时间 T + λ·惩罚项。T 在这里就是 t1+t2+t3，单位用秒；惩罚项是超限比例的加权和。λ 的选择直接影响搜索行为，我扫过几组值，发现 λ 太小（比如0.1）时算法会肆无忌惮地往超限区域钻，结果虽然时间短但轨迹不可用；λ 太大（比如10）时，算法把几乎所有精力都用在满足约束上，总时间收敛得慢且偏保守。我用 λ=1.0 作为默认值，然后根据前50代的可行解比例微调：如果前期可行解占比过低，就把 λ 调大一点；如果可行解占比很高但时间停滞，就适当调小 λ 让搜索更激进一点。这个比例控制法我用了很久，比完全靠运气试参靠谱得多。

3. 改进鲸鱼算法的设计细节与实现

这一章是项目的重头戏。标准鲸鱼优化算法本身不难，难的是怎么把它改成用在这个具体问题上又好用又稳定的形态。

3.1 标准WOA的三个核心机制快速回顾

先说标准WOA。它模拟座头鲸用气泡网捕食的行为，一群鲸鱼在解空间里移动，每次迭代按概率选择三种更新方式之一。

第一种是包围猎物：X(t+1) = X*(t) - A·D，其中 D 是当前个体到最优个体的距离向量，A 是随收敛因子 a 变化的系数，A = 2a·r - a，r 是[0,1]随机数。第二种是螺旋气泡网更新：X(t+1) = D'·e^(b·l)·cos(2πl) + X*(t)，D' 是个体到最优个体的距离，b 是螺旋常数，l 是[-1,1]随机数。第三种是随机探索：当 |A| > 1 时，个体随机选一个同伴作为参考点移动：X(t+1) = X_rand - A·D''。

关键在于收敛因子 a 从2线性降到0，决定了算法的探索和开发节奏。标准WOA用 |A| 的大小控制包围和探索的切换，虽然实现简单，但“线性下降”放在复杂约束问题里很容易出现前期探索不足、后期过早收敛的问题。

3.2 第一处改进：混沌映射初始化种群

第一个改动是针对初始种群。标准WOA用均匀随机数初始化，在带约束的问题里，随机种子不好时初始种群会在不可行区域扎堆。我改成了混沌映射初始化。具体做法是使用帐篷映射（Tent Map）生成混沌序列：x(n+1) = μ·min(x(n), 1-x(n))，μ 取 1.999 附近（帐篷映射 μ 趋于2时混沌性最强），把生成的序列归一化到 t1、t2、t3 的可行范围，作为初始解。

和均匀随机数相比，混沌序列在三维空间里的分布更加均匀，个体之间的初始间距更大，不容易在搜索初期就挤在同片局部区域里。我当时用对比实验验证了这个改进的价值：纯随机初始化时前20代可行解占比经常不到一半，换成混沌初始化后这个比例能提到七八成，优化效率提升很直观。

3.3 第二处改进：非线性收敛因子收敛曲线

第二个改动是收敛因子 a。标准WOA里 a = 2 - 2·(iter/Tmax)，线性衰减。我把衰减曲线改成余弦形式：a = 2·cos(π·iter/Tmax)，这样在迭代前中期 a 的下降速度比线性慢，算法有更多时间去探索；到了后期急剧下降，专注局部精细搜索。

这个改动的效果在收敛曲线上看很明显：线性衰减版本的前半段收敛慢、后半段又容易被局部最优困住，余弦衰减版本整体收敛曲线更平稳，最终总时间也更短。需要说明的是，这个改进不增加任何计算开销，只是改一行公式，属于性价比极高的一处调整。

3.4 第三处改进：精英差分变异策略

第三处改动是引入差分变异，解决多约束问题里最头疼的“后期种群同质化”。标准WOA后期个体都聚集到最优值附近，适应度曲面稍微有个小坑就集体掉进去爬不出来。我在每次迭代结束后，按一定的变异概率（我用0.3）对适应度排名靠后的个体做差分扰动：X_new = X_best + F·(X_r1 - X_r2)，其中 F 取0.5，X_r1 和 X_r2 是从种群中随机挑的两个个体。

这个操作的本质是让表现差的个体朝着当前最优方向偏置，同时随机选两个个体提供方向扰动，保持一定的探索性。和完全全局随机扰动相比，这种变异利用了种群已有的分布信息，搜索更有目的性。我一开始用的是高斯扰动，效果一般，换成差分变异之后明显好很多，因为差分向量天然携带了种群的分布距离信息，能自适应地控制扰动步长。

3.5 算法流程与参数配置汇总

整个改进WOA的流程是这样的：

第一步，用帐篷映射生成初始种群，每个个体是一个三维向量 (t1, t2, t3)。 第二步，对每个个体调用轨迹求解函数，算出系数，采样检验约束，得到适应度值。 第三步，进入主循环：更新最优个体，按随机数选择螺旋更新或包围更新，|A|>1时执行随机探索。 第四步，计算变异概率，对排名靠后的部分个体执行精英差分变异。 第五步，检查终止条件：达到最大迭代次数，或最优适应度连续30代变化小于阈值。 第六步，输出最优 (t1, t2, t3) 和对应轨迹。

参数上我常用的配置是：种群规模50，最大迭代数200，变异概率0.3，差分缩放因子0.5，惩罚系数1.0。这个配置不是死参数，如果机械臂关节数更多、约束更复杂，种群规模可以适当上调到80~100，但不要无脑往上加，否则每次迭代的轨迹求解和约束采样会拖慢整体速度。

3.6 与遗传算法、粒子群算法的横向对比

在相同的约束条件和路点设置下，我把改进WOA、标准遗传算法、标准粒子群算法各跑了20次，统计可行解的最终总时间均值。改进WOA最后得到的总时间均值最短，而且多次运行的离散程度也最小。标准WOA虽然也能收敛，但结果波动明显，偶尔还出现陷入不可行解的情况。遗传算法在处理这个三维连续优化问题时显得有点笨重，交叉和变异算子需要频繁调参；粒子群参数少一点，但惯性权重和加速系数没有充分调优时，收敛速度中庸，后期也容易早熟。

当然这不代表改进WOA在所有问题上都碾压其他算法，只是在“低维、多约束、来自轨迹规划”这类问题上，它的综合稳定性最好。

4. 仿真验证结果与参数敏感性分析

做完算法设计，最关心的自然是结果。我搭了一个仿真环境，用一个典型的六自由度关节型工业机械臂模型来验证整个方案的可行性。这部分不会贴太多代码，重点讲验证思路和结果解读，因为这恰恰是很多人容易忽略的地方。

4.1 仿真模型与实验设定

仿真模型方面，我用了一个六自由度关节型机械臂的D-H参数模型，把运动学解算、轨迹生成和控制接口封装成独立的模块，整套逻辑都跑在MATLAB环境里用于快速验证。给定一条任务路径：起点关节角 [0, -45, 90, 0, 60, 0]（单位度），第一个路点 [-30, -30, 75, 15, 50, 10]，第二个路点 [30, -20, 60, -10, 45, -5]，终点 [0, 0, 30, 0, 30, 0]。每个关节的速度、加速度和加加速度上限按上一章给出的量级设定。这些数值本身不关键，关键是整个验证流程是通用的，换成你自己机械臂的参数也一样跑。

约束检查的采样密度也很重要。我每段轨迹采样200个点。采样点太少的话，超限峰值可能恰好落在采样点之间，导致“看似满足约束、实际已经超限”的误判；采样点太多又会拖慢单次适应度评估的速度。经过测试，200~500个点的区间是比较合理的平衡点。

4.2 收敛性能与轨迹优化结果

先看收敛曲线。改进WOA的适应度值在40代左右就已经下降到接近最终水平，而标准WOA要到120代附近才稳定，而且稳定后的总时间比改进版高约13%。20次独立运行中，改进WOA每次都能在200代内收敛到可行解，标准WOA有3次在迭代结束后仍然处于不可行区域。

再看三段时间的优化结果。优化前我按经验手工给了个初始时间分配，总耗时约12.8秒；优化后的最优时间分配大约是 t1=2.3s、t2=4.1s、t3=2.2s，总耗时降到8.6秒，缩短了约33%。其中第一段和第三段占用时间相近，中间段最长，符合直觉——因为中间段是五次多项式，要同时保证通过路点时的加速度连续，动态约束更紧张，需要留出更多时间。

各关节的约束校验结果也在预期范围内：关节1在 t=5.2s 附近达到速度最大值的92%，关节3的加加速度在路点2附近出现峰值，但都没有超过上限。最关键的加加速度曲线全程没有出现突变，说明3-5-3分段结构的平滑性设计是有效的。如果换成单段3次多项式，加速度在起点和终点处就会直接跳变；改成全程7次多项式虽然也能平滑，但系数求解复杂得多，收益不成比例。

4.3 关键参数的敏感性分析

我把几个关键参数扫了一遍，看看它们对最终总时间的影响，结果可以作为调参参考：

种群规模从30增到50，总时间改善明显；从50增到100，改善很小，但计算时间增加一倍。所以50左右是性价比拐点。最大迭代数200与300的结果几乎一致，说明200代在这个规模下已经足够收敛。变异概率从0.1提到0.3，跳出局部陷阱的能力提升，总体时间下降；高于0.5时反而把好解破坏得太多，平均时间上升。惩罚系数从0.5到2.0范围内，可行解率逐渐上升，但到2.0时总时间已经过于保守；λ=1.0是平衡点。这些结论是针对本问题规模的，换问题后数值会变，但趋势可以作为借鉴。

5. 工程落地中的常见问题与避坑经验

这一章是实际操作中踩坑的记录，没有这些经验，照着上面的步骤复现也不一定能顺利跑出结果。

5.1 多项式系数求解的病态问题

最坑的一个问题是系数矩阵的病态。3-5-3多项式的系数矩阵里包含大量时间的高次项，当某段时间特别短时（比如 t1 小于0.3秒），矩阵条件数会变得非常大，求解出的系数数量级巨大，轨迹在路点附近出现严重的数值振荡。一开始我以为是约束写错了，排查了半天才发现是数值问题。

解决思路有两个：一是把每段时间归一化到[0,1]再构建系数矩阵，让矩阵元素保持在相近数量级；二是在优化算法里给每段时间加下限，比如 t_min=0.5秒，既避免病态问题，也符合伺服电机加速能力的物理现实。两个措施叠加后，系数求解再也没出过问题。

5.2 惩罚系数和约束检查的调参技巧

适应度函数里的惩罚系数 λ 是最容易拍脑袋设置的参数。我的具体调试方法是：先跑30代，观察记录里可行解的比例。如果可行解比例为0，说明惩罚太轻，算法完全不在意约束，调大 λ；如果可行解比例接近100%，但总时间长时间不下降，说明惩罚过重，搜索被约束缚住了，调小 λ。经过两个来回基本就能定位到合适的量级。

约束检查还需要注意采样时刻的选择。多项式轨迹的速度、加速度、加加速度在时间上是光滑函数，超限点通常出现在启停瞬间和路点附近。采样网格如果均匀分布，很容易漏掉末端附近的峰值。我在采样时会在每段轨迹的末尾加密采样点，或者直接计算导数表达式在段内的极值点（因为多项式导数仍然是多项式，极值点可以通过令导数为0求出）。解析法更精确，但编码麻烦一些；我工程上偏向用加密采样，效果足够。

5.3 从仿真到真机的差异

仿真里轨迹完美，不代表真机也能完美复现。我在把轨迹下发到实际控制器时遇到过振动问题，后来找到原因：控制器对加加速度的前馈补偿有限，轨迹的加加速度越靠近上限，跟踪误差就越大，末端振动越明显。所以我在真机调试时都会把加加速度上限做折减处理，比如按硬件手册标称值的80%来约束优化，留20%的余量。这样虽然总时间会略有上升，但换来的是末端更稳定、执行更可靠，在工业场景里这个取舍是值得的。

另外，实际运行前最好做一次“干跑”验证：让机械臂在低速模式下走一遍优化后的轨迹，记录实际的速度和加速度曲线，和仿真结果对比。如果两条曲线趋势一致、偏差在合理范围内，再切换到生产节拍，能避免很多现场事故。

5.4 代码实现上的小细节

最后分享几个实现上的小细节。第一，所有角度统一用弧度制，内部计算不要混用角度制，否则求导后还要反复转换，很容易出错。第二，约束检查里每个关节的速度上限不一样，用一个数组存最大值，不要用一个全局统一值凑合。第三，时间参数 t1、t2、t3 在优化迭代过程中一定要做边界约束处理，我习惯把越界的个体直接置到边界上，而不是让它继续乱飞，这样能大幅减少无意义的适应度评估。

我做过类似方案之后，最大的体会是：一个轨迹规划项目能不能落地，往往不取决于优化算法多高级，而取决于你把约束条件建模得多细致、把数值细节处理得多干净。3-5-3多项式给了一个不错的平衡点，改进鲸鱼算法解决的是在约束下怎么找到好解，但真正决定工程可用性的，还是你对机械臂物理特性的理解深度。后续如果想继续扩展，可以考虑把关节力矩约束也纳入优化模型，或者把路点位置本身也作为优化变量做时间-路径联合优化，那又是另外一个深坑了。先把这个基础版本跑通、跑稳，后面很多事情都会顺很多。