做运动控制的人应该都有同感：轨迹规划是决定一台设备“上限”的东西，而大多数工程里真正卡脖子的不是轨迹算不出来，而是“算不出来真正好的轨迹”。我前阵子把一个直线搬运任务从三次B样条换成七次B样条，再把时间、能量、冲击三个目标扔进NSGA2里做多目标轨迹规划，跑出来的结果比之前所有加权单目标方案都让人舒服。这篇就把整个项目的思路、代码逻辑和踩坑过程完整拆开，包括七次B样条怎么选型、NSGA2怎么和轨迹编码捏在一起、三个目标函数怎么算、参数怎么调，以及最容易被忽略的那几个细节。适合正在做机械臂轨迹优化、运动控制离线编程或者刚接触多目标遗传算法、想用它来解决实际轨迹规划问题的工程师和研究生。
1. 为什么我盯上了“时间+能量+冲击”这个组合
1.1 三个目标各管哪方面
先说清楚三个目标到底是什么。时间最优很好理解，就是从起点到终点花的时间最短，直接影响生产节拍。能量最优对应的是电机驱动功率的积分，节省能耗、降低发热，在移动设备或者频繁启停的场景里尤其重要。冲击最优对应的是加加速度（jerk），也就是加速度的变化率，它决定了机构在运动过程中受到的柔性冲击大小，直接影响机械寿命、末端抖动和噪声。
这三个目标放在同一个运动任务里，天然是矛盾的。想省时间，就需要更大的速度和加速度，加速度变化一定更剧烈，冲击上去了；想冲击小，就得把加减速过程拉长，时间就上去了；能量反而是个“中间派”，它跟速度曲线形状的关系更复杂，有时为了既快又稳，能量消耗反而偏高。所以这不是一个“找到唯一最优解”的问题，而是一个“在一堆各有取舍的可行解里做权衡”的问题。
1.2 换一个视角看矛盾，加权法为什么不够用
很多项目一开始都会考虑加权单目标：把时间、能量、冲击各自乘一个权重系数，加在一起变成一个标量，再用普通优化方法去解。这个做法我做过很多次，最后都被权重系数折磨得够呛。权重怎么定？纯靠拍脑袋。对某个工况S形加减速需求高的时候冲击权重调大，对另一个追求节拍的时候时间权重调大，每次改权重都要重新跑几轮，而且两个不同权重跑出来的结果之间还没法直观比较优劣。
更麻烦的是，加权法在数学上只能找到一个凸前沿上的点。如果真实Pareto前沿是非凸的，加权法会直接丢掉一些其实很合理的折衷解。对轨迹规划这种工程问题来说，丢解意味着你丢掉了一种“也许还能接受”的方案选项。这也是我后来彻底转投多目标遗传算法的原因——一次性把所有权衡方案都摆出来，比对着一个标量数值反复猜测强得多。
1.3 一组Pareto解比一个最优解更值钱
多目标优化里，Pareto最优解集指的是这样一组方案：你没法在不恶化至少一个目标的前提下改进另一个目标。比如一个解耗时3秒、耗能40焦耳，另一个解耗时3.5秒、耗能30焦耳，这俩没法说谁绝对更好，得看工况需求。NSGA2跑完之后给的不是一个答案，而是一整条Pareto前沿，上面有几十上百个候选轨迹。
这在实际项目里极其有用。同一套设备，白天要求高节拍，我可以直接从Pareto集里挑时间短的那个；夜间测试要求低振动，我换成冲击小的那个；偶尔有特殊工件怕抖，我再挑能量和冲击折衷的那个。不需要重新跑一遍算法，只要从解集里换一个点出来就行。这就是为什么我说“一组解比一个解更值钱”。
2. 七次B样条：从曲线性质看轨迹规划选型
2.1 B样条的骨架：控制点、节点向量、基函数
B样条曲线的核心构成是三样东西：控制点、节点向量和基函数。控制点决定了曲线的大致走向，但曲线一般不穿过控制点；节点向量则决定了每一段参数区间如何划分，基函数负责把控制点“加权合成”成实际曲线。它天然的优势是局部支撑性和凸包性——改动一个控制点只影响附近一段曲线，这给局部修形提供了极大方便；而曲线始终被约束在控制点构成的凸包内，也方便做安全工作空间的判断。
在轨迹规划里，我们通常把位置曲线写成时间参数下的B样条，也就是x(t)、y(t)、z(t)分别是三个B样条，或者用一个三维控制点的B样条统一表示。对于七次B样条，阶数p=7，意味着基函数的次数是7次，曲线的连续性是7减1，也就是C6连续。
2.2 阶次、连续性、冲击：七次带来的实际差异
为什么选七次而不是更常见的三次或五次？核心在于阶次决定了位置曲线能有多“光滑”。位置曲线连续不一定速度连续，速度连续不一定加速度连续，加速度连续了加加速度（jerk）可能还是断的。三次B样条大约能保证速度和加速度的连续，C2级别，对很多轻负载定位任务够用；五次B样条可以保证加速度的导数连续，适合伺服性能要求高一点的场景；而七次B样条能保证到C6连续，也就是说速度、加速度、加加速度甚至加加加速度在整条轨迹上都是平滑变化的。
冲击最优本身就是优化目标之一，而冲击是位置的三阶导。如果位置曲线本身连三阶导都不连续，那你压根没法形成“平滑的冲击曲线”，优化空间也会被结构性地限制。七次B样条给了足够高的本征光滑性，让冲击曲线的形状完全由控制点分布去塑造，而不是被阶数天花板卡住。
2.3 导数用控制点差分算，这是B样条最爽的地方
B样条在计算导数时有一个特别实用的性质：低一次的导数仍然是B样条，而且它的控制点可以由原控制点做差分得到。位置的一阶导（速度）的六个控制点，可以由原始七次控制点相减再除以节点区间长度得到；再算一次差分就能得到加速度的控制点，再差分一次就是冲击的控制点。也就是说，整个求导过程只需要做控制点层面的简单代数运算，然后在采样点处复用同一套基函数求值逻辑即可。
这意味着我们的适应度函数里可以非常方便地采样出速度、加速度、加加速度曲线：先对控制点做1到3阶差分，生成速度/加速度/加加速度曲线的控制点，然后在参数空间里均匀采样求值，再乘以时间缩放系数的幂次。我在实现里把这条导数链封装成一个函数，输入原始控制点，输出六个数组：位置、速度、加速度、加加速度的采样值。这个封装在后续多目标评估里被反复调用，是整个代码计算性能的关键骨架。
3. NSGA2如何嵌入轨迹规划
3.1 把轨迹“编码”成种群个体
用NSGA2做轨迹规划，第一步就是定义染色体。我的做法是：把B样条控制点序列直接展平当作基因主体，再额外拼接一个时间缩放系数T。单个控制点在二维平面里是(x, y)，如果做三维空间轨迹就是(x, y, z)。假设一条轨迹有nC个内部控制点，那么染色体长度就是 nC乘以空间维度，再加上末端的1个时间变量。
为什么额外加一个T？因为B样条参数u本身只是归一化的曲线参数，不是秒，需要靠T把参数区间映射到实际时间。位置等于P(u)，速度等于P'(u)/T，加速度等于P''(u)/T²，冲击等于P'''(u)/T³。这种设计的好处是，轨迹形状和时间尺度被拆成两组变量：控制点负责“走什么形状”，T负责“多快走完”，两者互相配合，NSGA2可以同时优化形状和时间尺度，得到的时间最优解往往比先定路径再单独优化时间律要好。
3.2 三个适应度函数与约束违反量的计算
一个个体解码成轨迹后，我会离散采样数百个点，然后计算三件事：
时间目标 f1 = T，直接就是时间缩放系数，越小越好。
能量目标 f2 = ∫ a(t)² dt，用加速度平方对时间积分近似。这里用梯形积分，把采样点上的加速度值结合起来乘时间步长。之所以用加速度平方而不是力矩平方，是因为力矩需要动力学模型和负载参数，受力模型不准确时反而会引入偏差；加速度平方偏差小，而且与冲击目标有明确的导数关系，更适合作为第一版优化目标。
冲击目标 f3 = ∫ j(t)³ dt，用加加速度平方积分近似。同样用采样点梯形积分。
对于约束，我检查的是运动学硬限制：采样点上的速度幅值、加速度幅值、加加速度幅值分别不能超过vMax、aMax、jMax，另外轨迹不能穿出工作空间的边界。对每一个约束，我都计算超限量的绝对值之和，得到一个总约束违反量violation。这个violation会随三个目标一起参与后续非支配排序。违反约束的个体天然被设有更高等级的惩罚规则压到“更差”的方向，尽可能保证最终解集是可行解。
3.3 非支配排序、拥挤度、精英保留
NSGA2的流程说穿了就是四步。第一步快速非支配排序：把种群里的个体按“支配关系”分层，第一层是没人能支配的个体，第二层是除去第一层后剩下个体里没人能支配的，以此类推。第二步计算拥挤度距离：每个非支配层内部，按每个目标排序后，根据相邻个体的目标差值来估计个体周围解的密度，用来给同一层个体分高下。第三步是选择与进化：用锦标赛法选出父代，做模拟二进制交叉和多项式变异生成子代。第四步是精英保留：父代和子代合并成一个大种群，重新排序，先按非支配层序号从低到高取，直到达到种群规模N的那一层，再按拥挤度大的先取，补齐N个，进入下一代。
这套机制决定了它在轨迹规划上的独特价值：非支配排序保证了搜索方向是“朝整个Pareto前沿推进”而不是朝单一方向收敛；拥挤度保证了前沿各个区域都有解，避免所有个体挤在一个局部折衷区。我的实测是，在经过大约60到80代后，种群就能铺出一条形状稳定的前沿带，后段主要在修细节。
3.4 这套流程的收敛压力与搜索偏好
虽然NSGA2是“多目标”算法，但它一样有搜索偏好。这个偏好首先来自变异算子和交叉算子对变量空间的结构性探索，其次来自拥挤度带来的均匀分布压力。如果初始种群采样不够散，后代很容易在前沿的中段堆出一堆相似解，两端位置却空着。所以初始种群的设计很重要，我一般会先做一个从起点到终点的直线B样条作为种子，然后对这个种子做随机扰动生成一批初始个体，再混入完全随机生成的个体。种子个体保证至少有一条可行基线，随机个体保证探索性。
还有一个容易被忽略的点，NSGA2本身没有“梯度”概念，对所有变量的尺度不敏感，但交叉和变异的分布指数决定了搜索步长的分布。轨迹规划里，控制点是米级或毫米级尺度，而T是秒级尺度，两者量级不一样没关系，因为哪怕同一个变量，变异也是按百分比式的扰动来做。但如果把所有变量混在一起做多项式变异，实际扰动幅度需要按每个变量的取值范围归一化。我会在变异前对每个基因单独算归一化域，这一步能明显改善解集的均匀性。
4. 代码实现与参数配置的细节
4.1 七次B样条基函数与时间缩放
代码的第一块是B样条基函数求值。经典的de Boor递推是稳定选择，我把它封装成一个函数：输入控制点、节点向量、阶数p、参数u，返回曲线上对应的点。这里的坑在于节点向量的构造和区间的查找判断。七次B样条要求节点向量必须有足够的长度，而且内部节点不能有重复超过p的度，不然分母会变成零。
def find_span(knots, p, u): n = len(knots) - p - 1 if u >= knots[n]: return n - 1 if u <= knots[p]: return p low, high = p, n while low != high - 1: mid = (low + high) // 2 if u < knots[mid]: high = mid else: low = mid return low def deboor(ctrl, knots, p, u): span = find_span(knots, p, u) d = [ctrl[span - p + k] for k in range(p + 1)] for r in range(1, p + 1): for k in range(p, r - 1, -1): denom = knots[span + k + 1] - knots[span - p + k] if abs(denom) < 1e-12: alpha = 0.0 else: alpha = (u - knots[span - p + k]) / denom d[k] = (1.0 - alpha) * d[k - 1] + alpha * d[k] return d[p]时间缩放的实现藏在采样循环里。假如当前个体解码出的时间系数是T，那么速度采样值是deboor计算出的控制点一阶差分曲线在u处的结果除以T，加速度是二阶差分曲线结果除以T²，冲击是三阶差分除以T³。注意这里三个除法的幂次容易写错，写错直接导致约束检查失真，后面我还会单独讲这个坑。
4.2 适应度与约束检查的伪代码思路
评估一个个体的时候，用统一的接口处理三个目标和一个约束违反量，在实现里大概是这个逻辑：
def evaluate(individual): ctrl_pts, T = decode(individual) pts = compute_curve(ctrl_pts, knots, 7, N_SAMPLES) vel_pts = compute_curve(ctrl_pts_1diff, knots, 6, N_SAMPLES) / T acc_pts = compute_curve(ctrl_pts_2diff, knots, 5, N_SAMPLES) / T**2 jerk_pts = compute_curve(ctrl_pts_3diff, knots, 4, N_SAMPLES) / T**3 f1 = T f2 = trapz(acc_pts**2) * T / N_SAMPLES f3 = trapz(jerk_pts**2) * T / N_SAMPLES violation = 0.0 violation += max(0.0, max(abs(vel_pts)) - V_MAX) violation += max(0.0, max(abs(acc_pts)) - A_MAX) violation += max(0.0, max(abs(jerk_pts)) - J_MAX) violation += bound_check(pts) return f1, f2, f3, violation能量和冲击的积分我用了简单的矩形或梯形近似，采样点数N_SAMPLES选500到1000。点数太少积分精度差，目标函数评估会带噪声，导致NSGA2收敛出的前沿不干净；点数太多又会拖慢每一代的计算。实测500点在PC上单次评估约几十毫秒，一整个种群跑200代大概几分钟，属于完全可以接受的量级。
4.3 种群参数与遗传算子的一套可复现配置
我最终固定下来的一套参数如表：
| 参数 | 取值 | 说明 |
|---|---|---|
| 种群规模 | 150 | 太小易早熟，太大会拖慢迭代 |
| 最大代数 | 250 | 观察目标基本稳定以80-120代居多 |
| 交叉概率 | 0.9 | SBX模拟二进制交叉 |
| 交叉分布指数 | 20 | 控制子代离父代的远近 |
| 变异概率 | 1/n_var | 每个基因位独立判断 |
| 变异分布指数 | 20 | 多项式变异参数 |
| 锦标赛规模 | 2 | 保留足够选择性压力 |
| 采样点数 | 500 | 目标函数积分用 |
这里要特别说明一下变异概率。一个常见的失误是把变异概率设成一个固定值比如0.1，但轨迹规划的基因数量可能有三四十个甚至更多，每个变量仅以0.1的概率变异的话，平均每次只有三四个基因变化，搜索效率太低。按1/n_var设置，平均每次期望有1个变量变化，配合交叉算子一起工作，效果更稳定。
4.4 迭代收敛与解的多样性评价
迭代过程中我一般同时观察两个东西：一个是三个目标各自的最小值和平均值是否还在显著下降，另一个是Pareto前沿的分布是否在变宽。如果前沿已经稳定，但连续几十代都没有新解加入，说明收敛得差不多了；如果前沿还在变宽，说明还有更好的区域没被挖出来，可以适当增加代数或多跑几个随机种子。
多样性方面，除了看前沿上的解数量，我还会检查解是否在目标空间里分布均匀。如果所有解都挤在时间短能量高的一端，说明交叉和变异的探索性还不够；这时我会检查分布指数是不是设得过大，导致子代都围绕父代附近产生，缺乏大步长跳变。把变异分布指数从20降到10，通常就能让前端和后端都长出解来。
5. 结果的读法：Pareto前沿的实用处置
5.1 快、省、稳三组典型解的差距
跑完后Pareto前沿大致呈现出一条从左下向右上延伸的弧带。以我做的一个典型点到点搬运实验为例，距离大约0.5米，允许最大速度1 m/s、最大加速度2 m/s²，从解集里取三个代表性方案对比：
| 方案 | 时间（s） | 能量积分（相对值） | 冲击积分（相对值） | 特点 |
|---|---|---|---|---|
| 快方案 | 2.1 | 1.8 | 3.6 | 节拍优先 |
| 折衷方案 | 2.8 | 1.0 | 1.2 | 综合最优 |
| 稳方案 | 4.2 | 1.5 | 0.4 | 低冲击优先 |
这个表格里最直观的一点是：快方案和稳方案在时间上差了一倍，但冲击积分差了接近一个数量级。这就是为什么“单目标最优”很难回答实际工程问题——你到底省时间还是护设备？答案在NSGA2给出的前端上一目了然。
5.2 前沿稀疏或扭曲时说明什么
如果跑出来的Pareto前沿在某一段特别稀疏甚至断档，通常有两个原因。第一是目标函数本身在该区域没有可行解：比如约束卡得很紧时，时间短和高冲击之间就存在不可行带，解集会被推到一起。第二是搜索过程没有充分覆盖该区域，也就是种群多样性不足：这时我会用刚才说的调分布指数、加大变异、补随机种子等方法重新跑。
前沿形状扭成L形、凹形也都正常。时间最短那一端往往能量不是最低，能量最低点在时间稍长一点的位置，这是因为非常急促加速到高速虽然省了时间但电流平方积分很高，而稍微拉长一点时间却能让速度曲线更平滑、有效降低电流峰值，这部分物理直觉和算法结果对得上，可以作为验证代码正确性的一个信号。
5.3 从解集里挑选工程方案的三种方式
从Pareto前沿上挑解，有三种我在工程里常用且都验证过可行的方式。
第一种是“最靠近理想点法”。先计算三个目标在解集中的理论最小值，组成一个理想点，然后从所有Pareto解里找欧氏距离离理想点最近的那个。优点是操作简单，缺点是对目标量纲敏感，如果三个目标没有归一化，会被量纲大的目标主导。所以实际使用前我会把三个目标各自减去最小值再除以该目标的极差，先做归一化再算距离。
第二种是“TOPSIS法”。构造正理想解和负理想解，计算每个解与两者的相对接近度，接近度最大的作为最终选择。它比最靠近理想点稍微优雅，考虑了与负理想解的距离，适合需要体现“不能太差”的场景。
第三种最粗暴但也最贴合工程：直接按项目需求排序。比如当前节拍紧张，就先按时间目标升序排列Pareto解，然后从前几个里挑冲击最小的；或者反过来，振动敏感就先按冲击排序，再挑时间较短的那个。反正解集都在桌面上，用Excel筛、用脚本筛都行，完全不需要再跑算法。
6. 踩坑记录：这套组合最容易翻车的几个点
6.1 节点向量和分母零除问题
七次B样条实现里第一个坑就是节点向量的边界条件。内部节点不能重复超过p次，否则基函数的分母项会有零，de Boor递推直接除零报错。我的做法是在构造节点向量时固定两端节点的重复度为p加1，中间全部均匀分布，这在开放均匀B样条里是标准做法。哪怕这样，在浮点数求值接近节点边界时，某个区间长度也可能小到接近零，所以我上面那段代码里加了denom绝对值小于1e-12就置零的容错逻辑。实测这个容错能避免大概百分之几的随机求值崩溃，而置零本身对结果影响很小。
还容易出错的点是 find_span 的边界判断。当 u 取到最后一个节点值时，如果不做 u >= knots[n] 的判断，数组索引会越界；如果只写大于等于，又会导致把最后一个区间判断成比实际多一区。这个问题我建议所有从零手写B样条的人，都在写完 find_span 后单独测试一组边界值：u=0、u=1、u=中间任意节点。
6.2 目标量纲差异把搜索方向带偏
决策变量和控制点没有量纲统一要求，但目标值之间的量纲差异会让部分决策机制产生问题。比如时间T是几秒，能量积分是几十，冲击积分可能是几百甚至上千。拥挤度距离计算里，如果某个目标的值域天然比其他目标大很多，排序后相邻距离主要由那个目标主导，拥挤度对分布的作用就失真，最终前沿会在量纲大的目标方向上被拉得特别均匀，另一个方向上却很稀疏。
解决方式有两个。一是在计算拥挤度前对每个目标做线性归一化，二是给三个目标分别加权再算距离。我更推荐前者，因为加权引入的主观参数越少越好。归一化只影响算法内部的“排序度量”，最终输出的三个目标值仍然是真实物理量，不会改变Pareto解集的客观支配关系。
6.3 初始种群和约束边界配合不当，前几代全被罚没
最让人头疼的一次实验是前三十代绝大多数个体约束违反量都很大，导致种群几乎在大量罚函数指导下爬行。原因是我用了完全随机初始化，从起点到终点随机撒控制点时，不少个体生成的曲线中间段严重偏离路径，速度、加速度、冲击全都超限。虽然后面也能慢慢爬回来，但浪费了大量代数在“排除不可行解”上，得不偿失。
我的修正方案是前面提过的：用一条直线或三次B样条拟合的起点到终点路径做种子个体，对它做有限幅度的随机扰动。具体做法是把每个控制点位置加上一个高斯噪声，噪声的方差设成任务空间尺度的一小部分，比如0.1米的任务里方差取0.02米。这样初始种群大部分个体都落在可行域附近，约束违反量从第一代就不会失控，搜索资源的绝大多数都用在“优化目标值”而不是“找可行解”上。
6.4 性能消耗与离线/在线应用怎么平衡
最后说一下性能问题。150个种群、250代、每代评估要计算500个采样点上的位置速度和加速度加加速度，整个流程跑一次大概几分钟到十几分钟，取决于控制点数量和采样点数。这个量级做离线轨迹规划完全没问题，轨迹规划本来就属于离线缓存再下发的场景。
但如果想把它往在线重规划上推，主要瓶颈是每代评估里的采样计算。优化空间有三个：一是把采样点数从500降低到200，用更好的自适应积分代替等间隔采样；二是把导数控制点的差分结果缓存起来，避免每个个体重复计算基函数；三是把种群规模减到100、代数减到120，先跑一个粗糙前沿，再从解集里挑几个候选做局部细化。我自己的经验是，这三步加起来能把一次完整优化从几分钟压到几十秒，对绝大多数半在线场景已经够用。真要毫秒级实时重规划，那需要的不是调参，而是换代理模型或者干脆离线预计算加查表，那是另一个话题了。
这套七次B样条加NSGA2的方案，我在不同的运动任务上反复验证过，三个目标的Pareto前沿都能稳定收敛，代码注释也写得比较全，照着实现一遍基本能跑通。最后分享一个小技巧：如果你也遇到前沿总有一段空着的情况，先别急着调参数，把初始种群里的随机种子数从一套改成三五套，跑完后把多次结果合并再取非支配集，往往比任何参数调整都更立竿见影。算法永远是在替你做权衡，而你要做的，是把权衡的视野尽量摊开。
