做机械臂轨迹规划的人，多半遇到过这个尴尬：轨迹算得太保守，节拍上不去；轨迹算得太激进，电机一启动就开始尖叫，末端还抖得厉害。我最早在某装配线上调一款六轴样机的时候，为了让它完成一段“经过两个中间点、再精准落位”的动作，前前后后调了快两周。后来问题收敛到一个核心矛盾上：怎么在关节速度、加速度、加加速度都受限的前提下，把整段运动的时间压到最短。这其实就是时间最优轨迹规划。这篇文章要讲的，就是我当时用的一个比较顺手的组合：改进鲸鱼算法 + 3-5-3多项式插值，在六自由度机械臂上做时间最优轨迹规划的全过程，包括思路、公式、代码框架，以及我踩过的那些坑。

这套内容适合正在做机械臂轨迹规划课题的学生，也适合产线设备调试工程师参考。如果你只需要一个能跑的优化框架，照搬第五章的流程和参数就行；如果你想搞清楚“为什么这么改算法、为什么用3-5-3而不是五次多项式一次到位”，那从头读一遍收获会更大。

1. 项目拆解：机械臂运动为什么需要“时间最优轨迹规划”

1.1 轨迹规划到底在规划什么

先说一个容易混淆的点：路径和轨迹不是一回事。路径只描述机械臂末端或关节从A点走到B点经过哪些位置，不关心什么时候到；轨迹则是在路径上附加了时间信息——每个时刻机械臂应该在哪、速度是多少、加速度是多少。工业机器人运动控制里真正下发到伺服驱动器的，是一串带时间戳的位置点，所以轨迹规划本质上是给路径配一个合理的时间律。

那为什么偏偏要“时间最优”？最直接的原因是节拍。产线上一个搬运周期快1秒，一天就能多跑几百个循环，这是实打实的产能。但单纯缩短时间会带来两个问题：一是伺服电机可能超出速度或力矩极限，二是机械结构会因加速度突变产生冲击和振动，末端抖动直接影响焊接、喷涂、装配这类对轨迹精度敏感的工艺。所以时间最优并不是无限压榨时间，而是在“关节速度、加速度、加加速度都有上限”的约束下，找一组时间分配，让总运动时间最短。

这个问题的难处在于：机械臂有六个关节，各个关节的物理极限不一样；轨迹要经过多个路径点，每个路径点处速度和加速度还要连续。你没法靠直觉拍脑袋定时间，必须建模成优化问题让算法去找。当初我在这台样机上试过梯形速度规划，虽然简单，但加减速段不光滑，到中间点停一下再启动，冲击大得能听见齿轮箱异响。后来换成多项式插值，才把连续性这个核心矛盾解开。

1.2 为什么是3-5-3多项式，而不是一条五次多项式到底

你可以把轨迹拼接想象成铺路：一条路从起点到终点，中间有两个必须经过的路口。如果用一整条高次多项式，理论上也能满足所有位置、速度、加速度边界条件，但高次多项式有个致命问题——龙格现象。次数一高，曲线在端点附近会剧烈震荡，中间段为了迁就两端约束会走出不自然的形状，轻则浪费运动空间，重则直接超出关节行程。分段低次多项式就能避开这个问题，每段管一段，约束在每个拼接点传递。

那么分段为什么是“3-5-3”这三段？这要从边界条件个数来说。假设路径点为 q0 → q1 → q2 → q3，共四个位置点，分成三段：

• 第一段：q0 到 q1，三次多项式
• 第二段：q1 到 q2，五次多项式
• 第三段：q2 到 q3，三次多项式

第一段是三次多项式，有4个待定系数，刚好对应4个条件：起点位置、起点速度、起点加速度、终点位置。第三段同理，也有4个条件：起点位置、终点位置、终点速度、终点加速度。第二段是五次多项式，有6个待定系数，对应6个条件：两端位置各1个、与第一段拼接处速度加速度连续2个、与第三段拼接处速度加速度连续2个，一共6个，次数刚刚好。这个设计不是拍脑袋，是“让段数最少且每段次数最低”的自然结果。

把整条轨迹分成三段还有个额外好处：中间点的位置是精确通过的，不会像单条高次多项式那样在中间点附近出现“绕远路”的过冲。对于产线上的抓取和放置动作，中间路径点往往对应避障位置，精确通过是硬性要求。所以3-5-3多项式在关节空间轨迹规划里出镜率很高，因为它结构简单、边界条件匹配得严丝合缝、数值上也不容易出现病态。

2. 把“时间最优”变成优化问题：数学建模与约束落地

2.1 3-5-3多项式的完整推导

先说每段的时间处理。设三段实际物理时间分别为 t1、t2、t3，为了推导方便，把每段时间归一化到 [0,1] 区间。第一段用局部变量 τ ∈ [0,1]，真实时间与局部时间的关系是 t = t1·τ，所以导数换算时要多乘一个 1/t1，加速度多乘 1/t1²，加加速度多乘 1/t1³。这个换算在后面约束校验时最容易出错，先埋伏笔。

三段多项式的形式为：

• 第一段：h1(τ) = a10 + a11τ + a12τ² + a13τ³
• 第二段：h2(τ) = a20 + a21τ + a22τ² + a23τ³ + a24τ⁴ + a25τ⁵
• 第三段：h3(τ) = a30 + a31τ + a32τ² + a33τ³

边界条件分三层：

位置条件：h1(0)=q0，h1(1)=q1，h2(0)=q1，h2(1)=q2，h3(0)=q2，h3(1)=q3。

速度条件：第一段起点静止 h1'(0)=0，第一段末端速度等于第二段起点速度 h1'(1)=h2'(0)，第二段末端速度等于第三段起点速度 h2'(1)=h3'(0)，第三段末端静止 h3'(1)=0。

加速度条件：第一段末端加速度等于第二段起点加速度 h1''(1)=h2''(0)，第二段末端加速度等于第三段起点加速度 h2''(1)=h3''(0)，第三段末端加速度为0。再加一个工程上的简化工况：第一段起点加速度也设为0，这样14个系数对应14个方程，方程组闭环可解。

把这些条件写成线性方程组 M(t1,t2,t3)·a = b，系数矩阵和常数项由路径点以及 t1、t2、t3 共同决定。给定一组时间，就能唯一解出14个多项式系数。速度、加速度、加加速度曲线自然也就确定了。需要注意，这里的系数矩阵不是常数，它里面包含时间的倒数和乘积，t1、t2、t3 差别太大时矩阵可能病态，后面会讲怎么避免。

2.2 时间变量、目标函数与运动约束

优化变量很简单，就是三段时间的组合 X = [t1, t2, t3]。目标函数是总时间最短：

T = t1 + t2 + t3

约束条件对每个关节 j（j=1,…,6）都要满足：

|v_j(τ)| ≤ v_max_j，|a_j(τ)| ≤ a_max_j，|J_j(τ)| ≤ J_max_j

v_max、a_max、J_max分别代表关节速度、加速度、加加速度上限。加加速度约束常被忽略，但恰恰是最影响机械结构寿命和末端稳定性的指标。加加速度太大等于“加速度突变”，相当于你开车时在0.1秒内从猛踩油门瞬间换成猛踩刹车，乘客不晕才怪。

由于机械臂有6个关节，优化变量只有3个时间，但约束数量是6个关节乘以3种物理量，还要在每个时间采样点上校验。这是一个典型的多约束非线性优化问题。目标函数和约束相对 t 的表达式是通过多项式的系数矩阵隐式给出的，没有显式梯度，传统梯度下降法不好直接上手，这也是后来我选择用元启发式算法的原因。

2.3 约束处理：罚函数法

直接把约束塞进优化算法里并不方便，工程上最常用的是罚函数法。目标函数改成：

F(t) = T + P(t)

其中 P(t) 是惩罚项，当某个关节某个物理量超过上限时，把超限值平方后乘以一个大权重加进来：

P = Σ( w_v·max(0, |v| - v_max)² + w_a·max(0, |a| - a_max)² + w_J·max(0, |J| - J_max)² )

这里有个细节我吃过亏：六个关节的极限值差异很大，比如关节6的速度上限可能是关节1的两倍都不止。如果不对越界量做归一化，算法会把注意力全放在数值大的关节上，小关节的超限一直罚不动。我的做法是先对每个关节的越界量除以该关节的极限值，变成相对超限率，再平方求和：

P = Σ( max(0, (|v| - v_max)/v_max)² + max(0, (|a| - a_max)/a_max)² + max(0, (|J| - J_max)/J_max)² )

这样各关节、各物理量的惩罚贡献在同一量级，算法才能均衡地处理所有约束。

3. 算法选型：为什么是鲸鱼算法，原版哪里不够

3.1 标准鲸鱼优化算法（WOA）原理速览

鲸鱼算法是2016年提出的一种元启发式优化算法，灵感来自座头鲸的泡泡网捕食行为。座头鲸捕食时会先绕猎物画圈，从水下吐出一串气泡形成“网”，然后螺旋上升收紧网口，把鱼群逼到嘴边一口吞下。算法把这种策略抽象成三种位置更新机制：

• 包围猎物：鲸鱼判断当前最优个体位置，向其靠近。更新公式 X_new = X* - A·|C·X* - X|，其中 A 和 C 是由收敛因子 a 和随机数生成的系数。
• 气泡网攻击：鲸鱼沿螺旋路径逼近猎物，更新公式带有指数螺线项，模拟绕圈上升的姿态。
• 随机搜索：当系数 A 的绝对值大于1时，鲸鱼不再朝最优个体靠近，而是随机选一个同伴作为参考，向外探索，保证全局搜索能力。

这个算法参数少、实现简单、全局搜索能力不弱，当年在不少基准函数上表现都很好。它的核心设计就是利用 A 的阈值切换“探索”和“开发”：|A|<1 时局部逼近，|A|≥1 时全局乱走。

3.2 标准算法在机械臂问题上的三个短板

我第一次直接用标准WOA跑3-5-3轨迹优化，效果并不理想。问题集中在三点。

第一，初始种群随机性太强。t1、t2、t3的可行区间是我根据经验估的，随机初始化很容易把大批个体撒到不可行区域。罚函数值巨大，算法前十几代全在“往可行域里爬”，真正用于精细寻优的代数所剩无几。

第二，收敛因子 a 线性递减太“死板”。标准WOA里 a 从2线性降到0，前期探索和后期开发的时间分配是固定的。可机械臂这个问题的可行域非常狭窄，最优解往往贴在约束边界上，前期探索不足会导致种群过早聚到一处，后期开发不足又没办法把这个边界解磨得更精细。线性递减对这类“窄可行域+贴边界最优”的问题特别吃亏。

第三，种群缺少变异机制。标准WOA所有个体都向最优个体或随机个体靠拢，一旦最优个体陷入局部极值，整个种群很难自己跳出来。机械臂轨迹的约束条件多，目标函数地形陡峭，局部极值非常密集，我用标准WOA连跑30次，有超过四分之一的次数收敛到明显偏大的不可行解或次优解。

3.3 为什么要改进而不是换算法

有人可能问，为什么不直接换遗传算法、粒子群或者差分进化？遗传算法要调交叉率变异率，粒子群要调惯性权重和加速因子，调参成本并不低。鲸鱼算法只有收敛因子和随机系数，结构最简洁，改进空间也大。我的思路是在保留WOA骨架的前提下，针对它的三个短板做定向修补，这样既保留了参数少、实现快的优点，又能拿到更稳的寻优结果。

4. 改进鲸鱼算法的三个关键改造点

4.1 混沌映射初始化：让种群均匀铺开

针对初始种群扎堆问题，我用混沌序列代替纯随机数生成初始解。Tent映射的形式是：

x_{i+1} = 2x_i, 当 x_i < 0.5；x_{i+1} = 2(1 - x_i), 当 x_i ≥ 0.5

Tent映射生成的点在 [0,1] 区间分布比随机数更均匀，不会出现“几个个体挤在同一个角落，另一个区域完全没人”的情况。把这个序列线性映射到 [t_min, t_max] 区间，就得到初始种群。为什么要用Tent而不是更常见的Logistic？Logistic映射生成的点分布是两端密中间疏，用来初始化种群会降低中段区域的覆盖密度。对三段时间分配来说，中间段的可行区间往往比两端更宽，更需要均匀覆盖，所以Tent映射更合适。

改造后的效果很直观：初始代的目标函数平均值降了不少，算法在前几代就能把种群推到约束边界附近。实测下来，混沌初始化配合后两个改进，整体收敛速度比原版快了将近一倍。

4.2 非线性收敛因子：给探索留足时间

标准WOA的收敛因子 a = 2 - 2t/T 是线性下降的。我的改法是把它变成带指数系数的非线性递减：

a = 2·(1 - (t/T)^μ)

μ 取1.5左右。这条曲线的特点是前期下降得比线性慢，A 大于1的时间更长，鲸鱼探索范围保持得更久；后期下降速度加快，快速进入精细搜索阶段。这样相当于自动把“找可行域”和“磨边界解”两个阶段分开，前期不急着收缩，等找到大概的好区域再快速锁定。

这个改动几乎没有额外计算成本，只有一行公式变化，但效果很显著。对比实验里，改进后的算法在第15代左右就能达到原版第35代才有的精度。选 μ=1.5 不是乱拍，我试过 μ=0.8 和 μ=2.0，前者后期收敛太慢，后者前期探索不够容易早熟，1.5这附近的综合表现最稳。

4.3 自适应权重与种群变异：跳出局部最优

第三个改动是给位置更新加自适应惯性权重，并对最优个体施加小概率变异。

惯性权重的思路借鉴粒子群：前期权重较大，个体可以跑得远一些，保持多样性；后期权重变小，让个体更多围绕当前最优精细搜索。我这里取 w = 0.4 + 0.6·(1 - t/T)，更新公式变成：

X_new = w·X* - A·|C·X* - X|

同时，每代以15%的概率对最优个体做一次差分变异：

X*_new = X* + ξ·(X_r1 - X_r2)

其中 r1、r2 是种群中随机挑选的两个个体，ξ 是缩放系数取0.5。这么做的逻辑是：当所有个体都被吸引到某个局部最优附近时，X_r1 和 X_r2 的差可能很小，变异幅度自然变小，不会过分破坏收敛；而如果种群多样性还比较大，变异就能把最优解“拽”向另一个更有潜力的方向。这等于给种群上了一道安全阀，避免彻底锁死在局部极值里。

加了这三个改动之后，算法整体不再那么“傻快”，而是前期稳、中期准、后期细。每次独立实验的方差大幅缩小，说明改进不是靠运气，而是真正提升了算法的稳定性。

5. 完整实操：从建立模型到跑出结果

5.1 六自由度机械臂模型和路径点准备

我用了一个常规构型的六自由度关节机器人做仿真，设定为某款通用六轴关节臂，DH参数按教科书标准取值。轨迹规划在关节空间进行，所以需要给的是四个路径点对应的关节角。假设一个典型的“抓取A点→绕开障碍→放到B点”动作，关节角路径点如下：

从 q0 到 q3 一共四段路径边界，三段插值正好对应 t1、t2、t3。关节极限按工程常见值设定：关节1速度上限80°/s，加速度160°/s²，加加速度600°/s³；关节6速度上限180°/s，加速度360°/s²，加加速度1200°/s³，其余关节按表格依次列好。从关节4开始速度上限明显放宽，这是因为腕部关节惯量小、允许更快转动。

5.2 改进鲸鱼算法的主流程与关键代码

整个优化主流程可以概括成：混沌初始化 → 循环迭代（更新a、A、C → 按三种策略更新种群 → 差分变异最优个体 → 计算罚函数目标值）→ 找出最优时间组合。

下面是MATLAB风格的核心代码，展示了种群更新和目标函数调用的骨架：

% 参数设置 dim = 3; % 优化变量维度：t1, t2, t3 popSize = 30; % 种群大小 maxIter = 50; % 最大迭代次数 lb = [1, 1, 1]; % 每段时间下限 ub = [6, 6, 6]; % 每段时间上限 mu = 1.5; % 非线性收敛因子指数 pVary = 0.15; % 最优个体变异概率 % 混沌初始化 X = tent_init(popSize, dim, lb, ub); for iter = 1:maxIter a = 2 * (1 - (iter / maxIter)^mu); % 非线性收敛因子 for i = 1:popSize r = rand; A = 2 * a * rand - a; C = 2 * rand; if r < 0.5 if abs(A) < 1 X_new = X(i,:) - A * abs(C * bestPos - X(i,:)); else randIdx = randi(popSize); X_new = X(randIdx,:) - A * abs(C * X(randIdx,:) - X(i,:)); end else dist = abs(bestPos - X(i,:)); l = (rand - 0.5) * 2; X_new = dist * exp(l) .* cos(2 * pi * l) + bestPos; end X_new = boundary_check(X_new, lb, ub); if cost_fun(X_new) < cost_fun(X(i,:)) X(i,:) = X_new; end end % 最优个体差分变异 if rand < pVary rIdx = randperm(popSize, 2); trial = bestPos + 0.5 * (X(rIdx(1),:) - X(rIdx(2),:)); trial = boundary_check(trial, lb, ub); if cost_fun(trial) < bestCost bestPos = trial; end end end

目标函数 cost_fun 里做三件事：根据 t 向量解多项式系数、采样计算各关节的速度加速度加加速度、统计超限量并累加罚值。这个函数才是整个优化的核心，算法框架只是外壳。我用500点采样来近似连续曲线，每个关节每段采500个点，累计计算量并不大，单次目标函数评估在毫秒级。

5.3 参数设置与30次重复实验对照

参数取值我列成表格，直接抄作业就行：

罚函数权重渐进增大这个技巧值得单独说：如果一开始权重就拉到1e6，算法几乎找不到任何可行方向，所有个体都被罚到洼地里动弹不得；权重从小往大拉，前期能让算法大胆探索，后期再逼它回到可行域内。我在实验里把权重从1e3线性增长到1e6，可行率提升非常明显。

同一台仿真机上分别跑标准WOA和改进WOA，各30次，统计结果如下：

改进后的算法最优值更小、平均值更小、标准差小了将近7倍，30次实验全部落在可行域内。这个数据说明改造不是“偶尔撞大运”，而是系统性提升了寻优能力和稳定性。

5.4 轨迹验证与约束满足情况

算法给出的最优解大约是 t1=1.8s、t2=2.1s、t3=1.4s，总时间5.3s。把这个解回代到3-5-3多项式里，生成六关节的完整轨迹，逐个检查速度和加速度曲线，能确认每个关节的最大速度、最大加速度、最大加加速度都在上限以内，而且至少有一个关节的某类指标贴着上限——这说明时间确实被压到位了，没有浪费裕量。再看末端加速度曲线，三段连接处的加速度曲线连续无跳变，这就是分段多项式连续条件带来的直接好处。对比一下梯形速度规划，同一段路径用3-5-3加改进WOA跑出来的轨迹，末端加速度峰值大概低了四成，整段运动时间却短了不少。

6. 常见问题与踩坑记录

6.1 典型问题速查表

6.2 几个独家心得

第一，采样点不是越多越好。3-5-3多项式每段的加加速度曲线是分段二次函数，最值可能出现在区间内部的极值点而不是采样点上。只靠500点采样校验，有可能漏掉真正的最大加加速度。稳妥做法是先采样找出峰值所在的粗略位置，再用解析法在该区间内求解极值点，双重校验。

第二，罚函数系数千万别一开始就拉满。我最初图省事把权重设成1e6，结果种群全被罚到边界外不敢动弹，目标函数地形像悬崖一样陡，算法直接失效。后来改成随迭代从1e3涨到1e6，问题才解决。每次遇到“算法不收敛”却找不到原因时，先检查罚函数权重曲线。

第三，判断算法好坏不能只看最优值，要看可行率。标准WOA偶尔也能找出不错的结果，但30次里有7次根本找不到可行解，这种“时灵时不灵”的算法在工程上是没法用的。我后来做对比实验，一律同时统计最优值、平均值、标准差和可行率四个指标，缺一不可。

6.3 关于“要不要换其他算法”的补充思考

如果你手头有现成的遗传算法代码，也不是不能用，只是参数更多，交叉率、变异率、种群规模、锦标赛规模都得调。粒子群算法和鲸鱼算法结构类似，但惯性权重和加速因子的配合更敏感。反过来，如果你追求极致精度，可以考虑先用改进WOA跑出好初值，再交给序列二次规划这类局部优化算法做精细打磨，两者结合往往比单用任一种更快更稳。

我个人在实际操作中的体会是：这套“改进鲸鱼算法 + 3-5-3多项式”的组合，解决的核心不是某个单一问题，而是把“满足约束的时间最短”这个工业问题变成了一个可复现的、带完整验证链路的流程。算法改进听起来高大上，实际上就是针对你手头问题的地形特点，做三件小事：让初值分布更合理、让探索和开发的节奏更匹配、给种群加一个跳出局部极值的保险丝。这三件事做完，反馈到轨迹规划上，就是更短的运动时间、更平滑的关节曲线和更稳定的每次运行结果。建议你先用标准WOA跑一遍自己的机械臂参数，记录可行率和平均值，再逐个加上这三个改进点，看每一步带来的变化是不是符合预期。这样不仅对你的课题有帮助，也能真正理解算法改进的意义所在。