基于NSGA-II与七次B样条的机械臂轨迹多目标优化方法

机械臂轨迹规划做到后期，大家都会遇到同一个瓶颈：单目标优化怎么调都不对劲。你压低了运行时间，关节冲击立刻飙上去，机械振动、减速器异响接踵而至；你想着那干脆把冲击做小，轨迹又慢得像个老太太过马路，节拍流水线又拖不起。我前前后后试过五次B样条加加权和、试过梯形速度规划硬拼接、试过单独做时间最优播送，最后在某公司的七轴机械臂实测项目里敲定了一套方案：7次B样条参数化轨迹，配上NSGA-II多目标遗传算法，同时优化时间、能量和冲击三个目标，在一轮一轮的非支配排序中给出帕累托前沿，再从前沿里挑出工程上最顺眼的那条轨迹。这套组合跑下来的效果比我预想的稳很多，代码全程带注释，也顺手传给了实验室的师弟们做底盘，后面陆续有几个人拿着它改成AGV路径平滑和无人机航迹。

这篇文章就把这套方案的完整思路、数学原理、代码结构和调参经验一次讲透。适合正在做机械臂轨迹规划、运动控制或者多目标优化相关课题的工程师和学生。不管你之前用的是三次B样条还是五次B样条，看完应该能直接把手上的方案升级到七次，并且对NSGA-II的落地细节有一个完整的认识。

1. 整体方案与核心设计思路

1.1 为什么非要是7次B样条？连续性说了算

在机械臂轨迹规划里，决定我们能不能把轨迹直接甩给伺服控制的，从来不只是位置本身，而是位置对时间的高阶导数。关节位置连续，速度不一定连续；速度连续，加速度不一定连续；就算加速度连续了，冲击（jerk，也就是位置的三阶导数）不连续，照样会在电机力矩里形成高频分量，齿轮箱里咔咔响。

三次B样条只有C2连续，冲击曲线是一条分段折线，拐点处剧烈跳变，跑高速时很容易触发机械共振。五次B样条能把冲击做成连续，但冲击的导数不连续，追求极致的轨迹平滑和低残余振动时也不够用。这次用到7次B样条，从数学上说连续阶数达到C6，前六阶导数全部连续，冲击曲线本身光滑可导。这对高速高精度的应用来说是非常关键的——伺服系统接收到的是一个高阶平滑的位置指令，控制的跟踪误差会小一个量级。

当然，7次B样条不是没有代价。控制点数量、节点向量、基函数递推的代码都比低次复杂得多，计算量也明显上升。网上现成的开源代码大多只做到三次或者五次，要往上推到7次，很多坑只能自己趟。后面我专门列了一节讲踩过的坑。

1.2 多目标优化是必须的：时间、能量与冲击的三角困局

为什么不用加权和？我最早用五次B样条加线性加权做的时候，每次都要试一组权重，重量这头压下去、能量那头翘起来。时间、能量、冲击本质上是三个互相打架的目标：你希望时间短，关节速度就得大，加速度和冲击必然上抬；你希望冲击小，轨迹得圆滑，总时间就会拉长；能量则跟速度平方和加速度平方相关，时间和冲击对它都有牵扯。

这三个目标构成一个典型的三角形矛盾关系，在数学上无法找到一个解让三个目标同时达到各自的最优值。你只能找“折中解”——也就是帕累托最优解。

如果硬要把三个目标压成一个标量再做单目标优化，问题就出在：第一，权重怎么选都不够地道，你根本不知道这一组权重意味着帕累托前沿上的哪个点；第二，权重一变轨迹风格就全变了，调试过程极其痛苦；第三，不同量纲的目标直接线性叠加，在数学上缺乏说服力——时间单位是秒，能量单位是焦耳，冲击单位是每秒立方，这三个量加在一起算什么？

NSGA-II恰好在这一点上立住了。它用非支配排序把所有个体划分成一层层帕累托等级，相同等级的个体再用拥挤距离保持多样性，最后输出的是一整条帕累托前沿，而不是单个最优解。我们不需要在优化前拍脑袋定权重，而是等优化结束后把前沿呈现在眼前，再根据现场的节拍要求、耗能预算、舒适度标准去选定一条实际执行的轨迹。工程上讲，这叫“把决策从优化阶段挪到后处理阶段”，逻辑顺太多了。

1.3 为什么在众多多目标算法里选NSGA-II

当时我也对比过其他几个方案。MOPSO对惯性权重和局部最优解的依赖比较敏感，在轨迹规划这种高维连续问题上容易过早收敛；MOEA/D需要预先分配权重向量，目标个数只有3个但决策变量达到十几个的情况下反而显得繁琐；纯遗传算法（单目标）又回到了加权和的老路。NSGA-II的优势在于：它是进化算法家族里最成熟的通用方案，非支配排序加拥挤距离的组合理论完备、代码实现路线清晰、出问题好排查；它在几十到几百个决策变量的连续优化问题上表现稳定，轨迹规划的控制点数量正好落在它的舒适区；再加上大量现成的工程实现可以参考，能把验证周期压缩不少。

1.4 整体方案的技术栈和模块划分

这套方案的代码结构分成了四个模块：B样条参数化模块、目标函数与约束模块、NSGA-II进化模块、结果可视化与分析模块。B样条模块负责曲线生成和高阶导数计算，目标函数模块负责时间、能量、冲击的计算以及约束检验，NSGA-II模块负责整个进化流程的调度，可视化模块负责帕累托前沿和轨迹曲线的绘制。模块化的好处是后面换场景时不需要动核心算法，只改目标函数接口的解析式就能适配AGV、无人机等其他运动体。

2. 7次B样条参数化与三目标函数构建

2.1 基函数、节点向量与控制点的关系

B样条的定义核心是Cox-de Boor递推式。一条p次B样条曲线由控制点Pi（i=0..n）和节点向量U确定。节点向量的数量m+1满足m = n + p + 1。基函数的递推形式是：

N_{i,0}(u) = 1，当u_i ≤ u < u_{i+1}，否则为0

N_{i,p}(u) = (u - u_i)/(u_{i+p} - u_i) * N_{i,p-1}(u) + (u_{i+p+1} - u)/(u_{i+p+1} - u_{i+1}) * N_{i+1,p-1}(u)

7次B样条就是把p设为7。为了让轨迹的首末端位置、速度、加速度、冲击都有约束可加，节点向量两端取重节点（clamped），让曲线首末经过控制点。重节点连续个数直接决定曲线在端点处的连续阶数。我在代码里把两端各设了p+1个重复节点，这样边界条件自然被“钉住”——位置、速度、加速度、冲击的初末值都能被显式控制。

控制点个数我取了12个，节点向量内节点按均匀分布。可能有人会问：为什么不是8个或者20个？太少的话曲线自由度不足，很难同时让冲击小、时间短；太多的话NSGA-II的搜索维度暴涨，收敛困难。实测下来12到16个控制点对机械臂这种8到10秒执行周期的轨迹是最经济的区间。

这里放一段B样条生成的骨架代码，注释比较全：

# 7次B样条基函数计算（Cox-de Boor 递推） def b_spline_basis(u, degree, knots, i): """ 计算第 i 个 p 次 B 样条基函数在参数 u 处的值 u: 参数值，范围 [0,1] degree: B样条次数 p = 7 knots: 节点向量（升序） i: 基函数序号 """ if degree == 0: # 零次基函数：落在半开区间 [knots[i], knots[i+1]) 内取1，否则取0 return 1.0 if knots[i] <= u < knots[i+1] else 0.0 # 两个非零系数项，分母为零时该项取0（防止除零异常） left = (u - knots[i]) / (knots[i+degree] - knots[i]) if knots[i+degree] != knots[i] else 0.0 right = (knots[i+degree+1] - u) / (knots[i+degree+1] - knots[i+1]) if knots[i+degree+1] != knots[i+1] else 0.0 return left * b_spline_basis(u, degree-1, knots, i) + right * b_spline_basis(u, degree-1, knots, i+1)

2.2 三个目标函数怎么定义才算“地道”

设计变量就是B样条的控制点位置加时间缩放系数，目标函数有三个：

目标一（时间最优）：直接把末端节点值映射为执行时间。我在代码里做了归一化处理，节点向量定义在[0,1]之间，实际时间通过缩放系数乘到秒单位，缩放系数本身也作为优化变量。这样做的优势是控制点位置和时间缩放解耦，搜索空间形态更干净——你改时间系数的时候不会影响曲线形状，反过来也一样。

目标二（能量最优）：关节能量消耗严格说要积分力矩乘角速度，但做完整逆动力学计算重、又要精确的动力学参数，在优化初期完全不必要。工程近似上用关节速度平方积分加上关节加速度平方积分作为能量成本的替代指标。速度项代表动能损耗，加速度项间接反映力矩大小，两个一叠，贴合度已经足够。实测下来用它选出来的轨迹，再套上真实逆动力学验算，能量消耗排名和近似模型的排名基本一致。

目标三（冲击最优）：冲击是位置的3阶导数，定义成每个关节冲击平方在时间上的积分。为什么用平方积分？因为冲击可正可负，如果直接积分，正负会抵消，最后算出来可能接近零，完全无法反映真实的冲击波动。平方后把波动幅度显式放大，优化器会主动去压冲击的幅值而不是偶然的均值。

三个目标函数在代码里的评价入口大概长这样：

def evaluate_individual(ctrl_points, time_scale, config): """ 对单个个体进行完整评价 返回 [总时间, 能量指标, 冲击指标, 约束违规量] """ # 1. 由控制点生成 7 次 B 样条关节轨迹 traj = build_trajectory(ctrl_points, time_scale, config) # 2. 按离散步长采样轨迹及各阶导数 samples = traj.sample(config.dt) # 3. 计算目标函数 total_time = time_scale # 时间目标 energy = energy_cost(samples) # 速度平方+加速度平方积分 jerk = jerk_cost(samples) # 冲击平方积分 # 4. 约束检验，返回违规量 violation = constraint_violation(samples, config.joint_limits) return [total_time, energy, jerk, violation]

2.3 约束的嵌入方式：硬约束不能只靠罚函数

约束包括四组：关节位置上下限、速度上限、加速度上限、冲击上限。位置约束在B样条控制点上直接实现比较粗暴，但更稳妥的是再加一道插值检验——每0.01秒采样关节轨迹，超出限位的个体直接标为不可行。速度上限在B样条的一阶导曲线上采样检验，加速度和冲击上限分别在二阶、三阶导曲线上采样检验。

不可行个体的处理我采取的是“罚函数加可行性优先排序”。罚函数值大的个体在非支配排序中自动落到最底层，这样既不影响帕累托前沿形状，又能保证最终选出的一定是满足硬约束的方案。千万注意：如果你的约束违反特别严重，比如从关节位置上限直接冲到了三倍距离，这种个体即使被罚到底层，它在种群里的数量如果太多也会拖慢收敛。所以我另外加了一个比例截断，违反量超过阈值就直接丢弃，不参与进化。

3. NSGA-II的算法实现与代码结构

3.1 染色体编码与初始化：用种子个体加速收敛

设计变量的编码方式是整个算法最需要谨慎的地方。我这里用的是实数编码，每个个体由三部分组成：若干个控制点坐标、时间缩放系数、节点向量分布参数。控制点坐标不是关节绝对位置，而是在位置插值基上的偏移量，偏移量范围根据关节限位做归一化，这样即使变异步长较大，也不会瞬间跳出可行区域太远。时间缩放系数用对数编码，避免数量级差异过大导致搜索不平衡。

种群初始化不是全随机，而是先给出一条经过三次B样条拟合的可行轨迹，再把控制点加上高斯噪声生成初始种群。用一条可行解做“种子”的好处是，优化前期就能有一批满足硬约束的个体参与进化，不会前几十代全是罚函数堆里的废柴。这个细节对收敛速度的影响非常大，强烈建议动手实现时保留。我刚开始用全随机初始化的时候，前30代几乎没有个体能通过位置约束检验，NSGA-II在水里瞎扑腾，后来换成种子初始化，第10代就能看到像样的帕累托前沿。

3.2 快速非支配排序与拥挤距离：NSGA-II的骨架

非支配排序用的经典快速非支配排序算法，两层循环统计每个个体被谁支配、支配谁，时间复杂度O(MN2)，M是目标个数，N是种群规模。3个目标、100个个体规模下完全够用。

拥挤距离在目标空间里逐目标排序后计算相邻个体的归一化距离，作为同一帕累托前沿内部选择时的多样性指标。它的作用是防止进化过程中种群全都挤到帕累托前沿的某一个角落——如果没有拥挤距离的约束，进化算法很容易因为选择压力集中在某个区域而丢失前沿的其他部分，导致最后给出的选择集非常狭窄。

核心排序逻辑用一段伪代码表示：

def fast_non_dominated_sort(population): """ 快速非支配排序：返回每个个体的帕累托等级 """ for p in population: p.dominated_set = [] # p 支配的解集合 p.dominated_count = 0 # 支配 p 的解的数量 for q in population: if dominates(p.obj, q.obj): # p 在所有目标上不劣于 q，且至少一个严格优于 q p.dominated_set.append(q) elif dominates(q.obj, p.obj): p.dominated_count += 1 if p.dominated_count == 0: p.rank = 1 front[1].append(p) rank = 1 while front[rank] is not empty: next_front = [] for p in front[rank]: for q in p.dominated_set: q.dominated_count -= 1 if q.dominated_count == 0: q.rank = rank + 1 next_front.append(q) rank += 1 front[rank] = next_front

3.3 遗传算子：SBX交叉与多项式变异

交叉算子我用了模拟二进制交叉（SBX），变异用多项式变异。这两个是实数编码进化算法里最常用的组合，本质上是把二元编码的位交叉、位变异思想迁移到连续空间。

SBX交叉的核心是分布式指数ηc。ηc越大，子代越接近两个父代的均值；ηc越小，子代越可能远离父代做全局探索。我取ηc=20，这个数值能让子代在有较高概率贴近父代的同时保留一定的探索性。变异方面，多项式变异用分布指数ηm控制扰动范围，ηm=20，变异后个体与父代的差别不会太剧烈，保证局部精细搜索。交叉概率0.9，变异概率按染色体长度倒数来设，这样平均每个个体变异的基因数稳定在1个左右，避免变异太猛导致后代结构被破坏。

我踩过的一个坑是：交叉和变异的作用对象不只是控制点，也包括时间缩放系数。控制点的变异步长可以稍微大一些，但时间系数如果变异太大，会让轨迹执行时间从6秒突变到12秒，前几代整个种群的时间目标分布就会特别散，后面要靠很多代才能拉回来。后来我把时间系数的变异幅值单独调成控制点变异幅值的五分之一，这个问题才消停。

3.4 精英保留与主循环：为什么最终解这么稳

NSGA-II最大的特点之一就是精英保留：每一代把父代和子代合并成一个规模为2N的种群，统一做非支配排序，再按帕累托等级从优到劣依次取到N个个体进入下一代。这样做的好处是优秀个体不会因为随机采样而被淘汰，收敛性和稳定性比无精英策略的普通遗传算法强很多。实测下来，同样的参数设置，有精英保留的方案跑二十次的波动幅度只有无精英方案的三分之一。

主循环的终止条件我设了两道：一是迭代代数达到上限（这里取200代），二是连续20代帕累托前沿面积变化小于阈值就提前收敛。跑一个完整的优化流程在普通笔记本上大概需要30到60秒，大部分时间花在B样条采样和目标计算上，NSGA-II本身的排序开销只占很小一部分。主循环的骨架长这样：

def evolve(self, max_gen=200, patience=20): population = self.seeded_init() for gen in range(max_gen): offspring = [] while len(offspring) < pop_size: p1 = tournament_select(population) p2 = tournament_select(population) c1, c2 = sbx_crossover(p1, p2, eta_c=20) c1 = polynomial_mutation(c1, eta_m=20) c2 = polynomial_mutation(c2, eta_m=20) offspring.extend([c1, c2]) combined = population + offspring fronts = fast_non_dominated_sort(combined) new_pop = [] for front in fronts: if len(new_pop) + len(front) <= pop_size: for ind in front: ind.crowding = crowding_distance(front, ind) new_pop.extend(front) else: # 按拥挤距离降序补足剩余名额 front.sort(key=lambda x: x.crowding, reverse=True) new_pop.extend(front[:pop_size - len(new_pop)]) break population = new_pop if self.check_convergence(patience): break return extract_pareto_front(population)

4. 实操过程、调参与避坑记录

4.1 第一次跑崩：7次B样条的过冲问题

第一次把代码跑起来，优化的结果惨不忍睹——轨迹曲线在关节空间出现明显的波浪，中间段直接冲出了关节位置限位。排查发现这是7次B样条的高阶振荡问题：控制点数量太多、节点分布过于均匀一致的话，曲线会在某些区段过冲。

解决办法有三个。第一，给控制点的搜索范围加约束，不让它离初始解太远。第二，把节点向量的分布从纯均匀改成非均匀，靠近首尾段适当加密，这样曲线在端点的边界层里不容易出现吉布斯式的波动。第三，给时间目标加了一个软性下限，不允它低于物理上能完成的极限时间——因为过冲问题也和“时间最优”的压力有关，时间缩放系数如果被压得太低，曲线必然为了追时间而剧烈波动。

另外要提一句：过冲不是7次B样条独有的，三次、五次样条节点取得密了一样会有。但次数越高，过冲越容易被放大，所以高阶样条的过冲控制必须是代码层面的一等公民，不能指望优化器自己绕过去。

4.2 帕累托前沿的形态与选点策略

优化跑完，把帕累托前沿画出来，能看到一个很典型的三维形状。时间短的个体能量和冲击都高；能量低的个体时间偏长、冲击中等；冲击极低的个体通常时间更长、能量也可能更高。它不是一条简单的曲线，而是一个三维曲面。

这时候如果只盯着某一个目标，很容易选出一个局部最优但全局很别扭的点。我的经验是：先按工程需要把时间目标定一个硬阈值，比如节拍要求必须在7秒以内，在前沿上切一刀，再在剩余范围内选冲击较小的个体，最后用真实逆动力学仿真验证能量消耗。

帕累托前沿上不存在一个严格意义上的最佳点，它是权衡的集合。哪条轨迹最好，取决于产线的节拍、减速器的寿命预算和负载的振动敏感度，这些是算法外部的工程信息，不该预先写死进优化目标里。我常跟师弟们说一句话：NSGA-II给你的是选择题答案，不是判断题答案——别指望它告诉你唯一正确答案，它把选项给你列整齐了，选哪个是你工程师的事。

4.3 代码实现里的三个关键细节

第一个细节是B样条求导的实现。很多人用数值差分法去求一阶、二阶、三阶导数，因为代码短。但数值差分在离散采样步长下会引入截断误差，三阶导数更是噪声放大镜——优化器会利用这些伪梯度钻空子，在数值噪声的缝隙里找出“看起来冲击很小”实则完全错误的解。正确做法是直接用Cox-de Boor递推对控制点做差分提升，解析式求导，速度和精度都高一个量级。

第二个细节是约束采样的步长。0.01秒采样对8秒轨迹意味着每个个体要算800个点，三阶导数要乘3，每个个体评价要处理几千个采样点。可以采用松弛策略：前50代每0.02秒采样一次，代际收敛后再加密到0.01，实测能省接近一半的优化时间，最终解的质量几乎不变。这个技巧在维度高的时候特别有用。

第三个细节是随机种子的管理。NSGA-II是随机算法，同一组参数不同随机种子跑出来的前沿有细微差异。调试阶段一定要固定随机种子，否则你无法判断某个参数的改动到底是有效还是只是随机波动。代码里默认种子设为42，线上部署时再放开。

4.4 从机械臂到AGV和无人机：这套框架的复用路径

这套“B样条+NSGA-II”的框架不止能用在关节空间轨迹规划。我后续把它改到过两个场景：AGV的路径平滑和无人机航迹规划。

AGV场景把关节位置换成位姿变量，目标函数改成路径长度、路径曲率、时间，冲击目标换成曲率变化率；无人机场景把控制点换成三维航点，目标函数加上威胁区避让的罚函数。因为核心框架——B样条参数化、多目标排序、精英保留——都是通用的，换场景时只需要改目标函数的解析式和约束条件，算法层完全不用动。这也是当初我为什么坚持把代码写模块化：目标函数接口留出来，后面接什么场景都是改改接口的事。

如果你也想复用到自己的场景，建议先做一件事：把你的运动对象的可行域画出来，看看B样条控制点数量在这个可行域里能不能表达出足够丰富的轨迹形状。如果轨迹形状有限，先加控制点；如果控制点太多导致NSGA-II搜不动，先减控制点再用种子初始化。这个基础工程量不大，但决定了后面整个优化过程的质量。

4.5 调试期最容易忽略的三个问题

除了上面说的过冲和随机种子，还有三个调试期非常容易忽略的问题。

第一，目标函数数值范围不均衡。时间大概在几秒这个量级，能量可能跑到几十到几百，冲击可能上千甚至过万。如果不做归一化，非支配排序里冲击目标会主导所有个体之间的支配关系，时间和能量目标几乎不起作用。解决办法是在评价函数内部对每个目标做极值归一化，用当代种群的最大最小值把三个目标统一映射到[0,1]区间。

第二，初始种子的可行轨迹必须是真可行的，不能是“大概可行”。如果种子轨迹本身就轻微违反了某个速度约束，后续进化会一直带着这个违规基因，很难完全洗掉。我在代码里专门留了一个种子校验函数，初始化之后先把种子自身严格检验一遍，不合格就微调初始控制点再校验，直到确认干净了才进入主循环。

第三，约束采样步长和优化目标采样步长可以分开。约束检验需要密采样才能抓准峰值，但目标函数里的能量和冲击积分理论上用较稀疏的采样也不会差太多。分开设置的好处是可以在不牺牲约束安全性的前提下加速评价。

这个“B样条细化+NSGA-II进化”的组合，我个人在实际里还发现一个有意思的延伸：把帕累托前沿上几个典型的折中解拿出来，用随机扰动加插值的方式生成一条新的偏好轨迹库，相当于把离线优化结果变成在线轨迹库。机械臂现场换产线时，不需要重跑优化，只要在库里查表就能快速切换轨迹风格，这在节拍频繁变化的生产线上很实用。后来我甚至给这个轨迹库加了一层基于现场实时负载扭矩的反馈修正——离线前沿负责选型，在线修正负责微调，两层配合下来，比单纯离线优化的方案对工况波动的适应性强不少。