基于七次B样条与NSGA-II的机械臂轨迹规划MATLAB实现

最近在调机械臂关节空间的轨迹生成模块，项目需求很典型：末端要依次经过八个目标点，路径必须平滑，速度、加速度都要卡上限，否则电机扭矩一上去就开始抖，甚至触发运动学保护。最后落地的一套方案就是标题里这套——MATLAB环境下，用七次B样条做机械臂轨迹规划，以八个点作为必经路点，再配上速度加速度约束，用NSGA-II把时间分配和轨迹质量一起优化出来。整个过程做下来，踩了不少数值坑，也总结出一套可以直接抄作业的流程，写出来给正在搞轨迹规划的朋友参考。

这套方案适合谁看？如果你是做机器人控制、自动化设备轨迹生成的工程师，或者是在校做机械臂路径规划相关课题的学生，想把“B样条轨迹”“约束优化”这些概念落到代码里，这篇应该能帮你省掉不少查资料的功夫。我不打算把每个公式都推到天荒地老，重点讲清楚七次B样条在机械臂轨迹规划里怎么用、为什么用、以及MATLAB代码怎么一次跑通。

1. 先拆项目需求：八个点、七次B样条、速度加速度约束到底在解决什么

1.1 机械臂轨迹规划不是简单连点成线

机械臂从起点运动到终点，最简单的做法是在关节空间做线性插值，或者笛卡尔空间画一条直线。但真实机械臂对轨迹有两层要求：一是轨迹必须经过指定的路径点，这是工艺需求；二是关节的速度、加速度、甚至加加速度必须连续且受限，这是硬件约束。

直线插值带来的问题是速度曲线不连续，在拐点处加速度突变，机械臂电机会受到冲击，末端会有明显振动。所以工程上一般不直接连线，而是用样条曲线把离散路点“圆滑”过去。B样条就是这个场景下最常用的数学工具，因为它既能保证曲线通过路点足够接近，又能通过次数控制连续阶数。

项目里的“八个点”在这里指的是八个必经路径点。机械臂在运动过程中必须依次经过这些位形或位置，但点与点之间的路径可以任意光滑过渡。这就是轨迹规划与普通插值最大的区别：我们规划的是一整条带时间信息的连续运动曲线，而不是简单拟合。

1.2 为什么偏偏选七次B样条

B样条的次数决定了轨迹的连续阶数。三次B样条能做到C2连续，即位置、速度、加速度连续，绝大多数工业场景够用。为什么这个项目要上七次？

因为七次B样条可以提供C6连续，也就是说从位置一路到加加速度的五阶导数都是连续的。对于高速高精度的机械臂运动，高阶连续意味着电机电流变化更平缓，机械振动激励更小。另外七次B样条允许我们在轨迹端点同时指定位置、速度、加速度、加加速度等边界条件，这对轨迹两端需要精确停靠的场景非常有用。

代价当然也有。次数越高，曲线振荡的可能性越大，数值计算的条件数越差，对参数设置也更敏感。七次在工程里属于“够用但需要小心处理”的级别，不是越高越好。

1.3 速度加速度约束和NSGA-II是怎么凑到一起的

轨迹规划如果只做几何路径，那叫路径规划；一旦引入时间，变成“什么时刻到哪里、速度多大”，才叫轨迹规划。速度加速度约束是从电机和减速器来的，最大速度受限于电机转速，最大加速度受限于输出扭矩。轨迹的数学表达式再光滑，如果速度超限或者加速度超限，机械臂照样报错。

约束条件一多，再加上我们还想在满足约束的前提下让运动时间尽量短、轨迹尽量平滑，这就变成一个典型的多目标优化问题。目标之间互相冲突：时间越短，速度和加速度往往越大。这时候需要多目标优化算法去搜索帕累托前沿，NSGA-II是非支配排序遗传算法的经典代表，在这个场景下非常合适。

2. 七次B样条背后的数学基础：基函数、节点向量、控制点反算

2.1 从B样条基函数说起

B样条曲线的基本形式是

q(u) = Σ PᵢNᵢₚ(u)

其中 Pᵢ是控制点，Nᵢₚ(u)是p次B样条基函数，u是曲线参数。基函数由节点向量U决定，用Cox-de Boor递推公式计算。

节点向量是B样条和Bezier曲线最大的区别。B样条曲线被节点向量划分成多个曲线段，每一段由相邻若干个控制点共同影响。七次B样条的每个曲线段由8个控制点决定。节点向量两端采用重复度p+1的clamped形式，可以让曲线精确经过首尾控制点，这也是轨迹规划里几乎必用的形式。

我在MATLAB里实现的基函数是标准的递归版本：

function N = bspline_basis(i, k, u, U) % 计算第i个k次B样条基函数在u处的值 % i: 控制点索引, 从1开始 % k: B样条次数 % u: 参数值 % U: 节点向量 if k == 0 if u >= U(i) && u < U(i+1) N = 1; else N = 0; end return; end N = 0; den1 = U(i+k) - U(i); den2 = U(i+k+1) - U(i+1); if den1 > 1e-12 N = N + (u - U(i)) / den1 * bspline_basis(i, k-1, u, U); end if den2 > 1e-12 N = N + (U(i+k+1) - u) / den2 * bspline_basis(i+1, k-1, u, U); end end

实际使用时不要用递归去算整个曲线，因为递归调用在工程代码里性能不行。更推荐的办法是把所有基函数在一组参数点上一次算完，存成矩阵。后面控制点反算和轨迹求值都用矩阵运算。

2.2 节点向量怎么生成

节点向量的生成直接决定了曲线段的分布。对于p=7的clamped节点向量，前8个节点是起点值，后8个节点是终点值。如果控制点数量是n+1，内部节点数量是n-p。控制点越多，内部节点越多，曲线局部调整能力越强。

这个项目里我分两种场景处理：

第一种是八个路点直接对应八个控制点的情况。p=7，n=7，内部节点数量为0，节点向量退化成

U = [0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1]

这种情况下七次B样条实际上退化成一条七次Bezier曲线，只有一段，控制点近似等于路点，调整自由度很小。对“过八个点”这个需求来说能用，但对后续速度时间优化来说不够灵活。

第二种是真正发挥B样条优势的场景：路点是八个，但控制点扩展到14个，内部节点6个。节点向量变成

U = [0,0,0,0,0,0,0,0, u₁,u₂,u₃,u₄,u₅,u₆, 1,1,1,1,1,1,1,1]

其中u₁到u₆是内部节点，对应各段时间分配。这个形式才是后续接NSGA-II的正确姿势。控制点比路点多，意思是有6个自由变量来优化曲线形状，同时八个路点作为硬约束必须被满足。

节点向量生成代码：

function U = clamped_knot_vector(n, p, t_inner) % n: 控制点数量-1 % p: 次数 % t_inner: 内部节点向量, 长度为 n-p U = [zeros(1, p), t_inner, ones(1, p)]; end

如果不需要指定内部节点，直接用linspace生成均匀内部节点即可。

2.3 控制点反算：从路点到控制点

路点是轨迹必须经过的位置，但B样条本身并不保证经过控制点。要让轨迹过路点，需要反算控制点。

假设八个路点Q₀到Q₇对应的参数值为u₀到u₇，插值条件写成线性方程组

Σ PᵢNᵢₚ(uⱼ) = Qⱼ

其中j=0,...,7。写成矩阵形式就是A·P=Q，A是8×8的采样矩阵，A(j+1,i+1)=Nᵢₚ(uⱼ)。求解这个线性方程组就能得到控制点P。

对于八个控制点的情况，直接用矩阵左除：

A = zeros(8, 8); for j = 1:8 for i = 1:8 A(j, i) = bspline_basis(i, 7, u_param(j), U); end end P = A \ Q;

这里有个细节：参数值uⱼ不能取均匀分布就完事。如果路点间隔差异很大，均匀参数会导致曲线在密集段抖动。工程上应该用累积弦长参数化，把路点之间的欧氏距离累加并归一化，这样曲线对路点间距更鲁棒。我实测下来，累积弦长参数化能让插值矩阵条件数明显下降。

对于14个控制点的情况，8个方程解14个未知数，欠定。需要补充优化准则。常用的准则是“在满足插值约束的条件下最小化加速度能量”，这样轨迹最平稳。这是一个带等式约束的二次规划：

min ½PᵀHP

s.t. A·P = Q

其中H是加速度能量矩阵，可以用离散二阶导近似。MATLAB里直接调quadprog就能解：

Aeq = zeros(8, 14); for j = 1:8 for i = 1:14 Aeq(j, i) = bspline_basis(i, 7, u_param(j), U); end end beq = Q; H = build_energy_matrix(U, n, p); % 离散加速度能量矩阵 P_opt = quadprog(H + 1e-10*eye(14), [], [], [], Aeq, beq);

这里加1e-10正则项是为了防止H奇异，实际运行中很有用。

2.4 速度和加速度怎么从B样条里求出来

B样条求导有现成的递推公式。p次B样条的导数是一个p-1次B样条，新的控制点由原控制点差分得到：

Dᵢ = p × (Pᵢ₊₁ - Pᵢ) / (Uᵢ₊ₚ₊₁ - Uᵢ₊₁)

注意节点向量也要相应地去掉首尾节点。速度曲线和加速度曲线就分别是对位置曲线的一次、二次求导结果。

这个导数递推公式在编程时要特别注意分母为零的情况。clamped节点向量两端大量重复节点，某些控制点的分母会是0，对应控制点不应该参与计算。代码里要加保护，分母绝对值小于阈值就跳过。

有了速度和加速度表达式，约束校验就非常直接：对整条轨迹密集采样，计算每个采样点上的速度模长和加速度模长，取其最大值与限制值比较。如果有任何一点超限，这条轨迹就直接判为不可行。

3. MATLAB实操：从八个点生成可用的七次B样条轨迹

3.1 完整的数据准备流程

我实际跑的时候，八个路点先用的是笛卡尔空间里的二维坐标，验证没问题后再扩展到六关节。为了把原理讲清楚，这里用二维路径演示。

八个路点设为：

Q = [0 0.5 1.0 1.5 2.0 2.5 3.0 3.5; 0 0.1 0.5 0.8 0.7 1.0 1.2 1.0];

这八个点从左到右分布，带点起伏，比较能看出B样条的光滑效果。

路点对应的参数值用累积弦长：

% 累积弦长参数化 seg_len = sqrt(sum(diff(Q, 1, 2).^2, 1)); cum_len = [0, cumsum(seg_len)]; u_param = cum_len / cum_len(end);

得到的u_param就是八个路点在[0,1]区间上的参数位置。这个参数序列后面要作为B样条曲线定义域内的采样点。

如果采用八个控制点方案，节点向量只有两端重复，内部节点数量为0，u_param和节点向量之间没有对应关系，直接用就行。如果采用14控制点方案，内部节点可以选择把u_param里除首尾之外的值作为内部节点，这样路点和曲线段的对应关系最自然。

3.2 轨迹求值函数

我写了一个采样求值函数，在参数轴上均匀取N个点，一次性算出位置、速度、加速度：

function [q, v, a] = evaluate_trajectory(U, P, p, u_vec) % 在u_vec参数点上计算B样条轨迹的位置、速度、加速度 % U: 节点向量 % P: 控制点矩阵, 每一列是一个控制点坐标 % p: B样条次数 n = size(P, 2) - 1; q = zeros(length(u_vec), size(P, 1)); v = zeros(length(u_vec), size(P, 1)); a = zeros(length(u_vec), size(P, 1)); % 位置采样 for k = 1:length(u_vec) u = u_vec(k); for i = 1:n+1 q(k, :) = q(k, :) + bspline_basis(i, p, u, U) * P(:, i); end end % 速度：p-1次B样条, 控制点D D = zeros(size(P, 1), n); U2 = U(2:end-1); % 去掉首尾后的节点向量 for i = 1:n den = U(i+p+1) - U(i+1); if den > 1e-12 D(:, i) = p * (P(:, i+1) - P(:, i)) / den; end end for k = 1:length(u_vec) u = u_vec(k); for i = 1:n v(k, :) = v(k, :) + bspline_basis(i, p-1, u, U2) * D(:, i); end end % 加速度：对速度再做一次求导 E = zeros(size(P, 1), n-1); U3 = U2(2:end-1); for i = 1:n-1 den = U2(i+p-1+1) - U2(i+1); if den > 1e-12 E(:, i) = (p-1) * (D(:, i+1) - D(:, i)) / den; end end for k = 1:length(u_vec) u = u_vec(k); for i = 1:n-1 a(k, :) = a(k, :) + bspline_basis(i, p-2, u, U3) * E(:, i); end end end

这个函数的核心思想是位置、速度、加速度都表达成B样条基函数的线性组合，只是控制点和节点向量不同。这也是B样条比普通多项式优越的地方：求导之后依然是B样条，结构统一，计算方便。

3.3 可视化验证轨迹质量

轨迹算完之后一定要画图验证，不能只看数值。我一般画三张图：位置曲线、速度曲线、加速度曲线。

位置图上把八个路点标出来，确认轨迹确实经过路点且没有明显突变。速度曲线检查是否连续，加速度曲线检查是否超限。

u_plot = linspace(0, 1, 300); [q, v, a] = evaluate_trajectory(U, P, 7, u_plot); figure(1); plot(Q(1,:), Q(2,:), 'ro', 'LineWidth', 2); hold on; plot(q(:,1), q(:,2), 'b-', 'LineWidth', 1.5); xlabel('X'); ylabel('Y'); legend('路点', 'B样条轨迹'); grid on; figure(2); subplot(2,1,1); plot(u_plot, v(:,1), 'r-', u_plot, v(:,2), 'b-'); grid on; ylabel('速度'); subplot(2,1,2); plot(u_plot, a(:,1), 'r-', u_plot, a(:,2), 'b-'); grid on; ylabel('加速度');

第一次跑的时候最容易出现的问题是轨迹在路点附近出现过冲。如果出现过冲，优先检查参数化方式，均匀参数改成累积弦长参数后往往立刻改善。另外可以检查控制点反算矩阵的条件数，cond(A)如果超过1e6，就得考虑换参数化或者增加正则化。

4. 把NSGA-II接进来：在速度加速度约束下优化时间分配

4.1 为什么这里不能只用简单的时间缩放

很多初学轨迹规划的人第一反应是：既然速度加速度超限，那把总时间乘一个大于1的系数不就行了？确实，线性时间缩放能解决一部分约束问题，但这只是“整体放慢”，不会改变速度曲线的形态。

真正的问题在于，机械臂经过八个路点时，各段轨迹的几何复杂程度不一样。某一段路径拐弯很急，需要更多时间；另一段路径平直，可以快速通过。线性缩放无法针对性地调整各段时间分配，最后要么某一段仍然超限，要么整体慢得不经济。

所以需要优化的不是总时间这一个数，而是七段时间分配的相对比例。这才是NSGA-II发挥作用的地方。

4.2 决策变量和节点向量的联动

我采用的决策变量是七段时间间隔Δ₁到Δ₇，对应机械臂从路点Qⱼ运动到Qⱼ₊₁所用的时间。总时间T就是Δ₁到Δ₇之和。

七段时间间隔确定之后，时间节点就是

t₀ = 0, tⱼ = tⱼ₋₁ + Δⱼ, j=1,...,7

然后把这个时间节点映射到[0,1]区间作为B样条的内部节点。这一步是整个优化方案的精髓：时间分配一变，节点向量就变，B样条控制点也跟着变，最终轨迹的速度曲线和加速度曲线随之改变。

编码时我直接把Δ作为NSGA-II的个体变量，这样能天然保证t₀<t₁<...<t₇，不需要额外处理单调性约束。个体就是一个7维向量，每一维是正数。

4.3 目标函数和约束处理

NSGA-II多目标优化的两个目标我选的是：

• 目标1：总时间T = ΣΔⱼ，最小化
• 目标2：加速度均方根RMS(a)，最小化

这两个目标的物理意义很明确：一个追求效率，一个追求平稳。帕累托前沿上的每组解都代表了效率和平稳的一种权衡。

约束条件就是速度上限和加速度上限。在目标函数里，我对每个个体做以下操作：

1. 根据Δ生成时间节点，归一化后构建节点向量U。
2. 用quadprog在八个路点插值约束下求最优控制点P。
3. 在[0,1]内密集采样，计算速度和加速度曲线。
4. 检查速度最大值是否超过v_max，加速度最大值是否超过a_max。
5. 计算约束违反量g = [max(v)-v_max; max(a)-a_max]，违反量为正说明不可行。

约束处理上我推荐用NSGA-II的约束支配规则，而不是简单加罚函数。罚函数对罚系数太敏感，系数小了约束形同虚设，系数大了多目标变成单目标。约束支配的思路是：可行的个体永远优于不可行的个体；两个不可行个体比较约束违反总量；两个可行个体再比较帕累托支配关系。

目标函数的MATLAB骨架：

function [f, g] = traj_objective(delta, Q, v_max, a_max) % delta: 七段时间, 7维向量 % Q: 八个路点, 二维矩阵, 每列一个点 delta = delta(:)'; T = sum(delta); % 时间节点归一化到[0,1] t_knot = cumsum([0, delta]) / T; t_inner = t_knot(2:end-1); % 六个内部节点 U = [zeros(1,7), t_inner, ones(1,8)]; % 14控制点, 7次B样条 n = 13; p = 7; % 路点参数采用累积弦长 seg_len = sqrt(sum(diff(Q,1,2).^2, 1)); u_param = [0, cumsum(seg_len)] / cumsum(seg_len(end)); % 小心end写法 % 对每个个体重新反算控制点 Aeq = zeros(8, 14); for j = 1:8 for i = 1:14 Aeq(j, i) = bspline_basis(i, p, u_param(j), U); end end beq = Q(1, :).'; % 这里以单个坐标为例, 实际上要对每个维度求解 H = build_energy_matrix(U, n, p); P = quadprog(H + 1e-10*eye(14), [], [], [], Aeq, beq); P = reshape(P, 1, []); % 采样计算速度和加速度 u_plot = linspace(0, 1, 500); [q, v, a] = evaluate_trajectory(U, P, p, u_plot); % 目标 f1 = T; f2 = rms(a(:)); f = [f1, f2]; % 约束违反量 g = [max(abs(v(:))) - v_max; max(abs(a(:))) - a_max]; end

上面代码为了方便展示只写了单坐标维度，实际多关节时要把每个维度分别反算控制点，然后统一算速度模长、加速度模长来做约束判断。目标函数写成返回实现可行性的形式，NSGA-II主程序里再根据约束违反量进行非支配排序。

4.4 优化参数与结果选取

NSGA-II的具体实现我建议直接用成熟的MATLAB代码，不需要自己从头写非支配排序。种群大小设100到200，迭代100到200代，对七维决策变量来说足够收敛。

跑完之后会得到一组帕累托前沿。决策上我通常不直接选总时间最小的那个解，因为这个解往往刚好贴着约束边界，工程上太危险。我会在帕累托前沿上选一个距离约束边界有5%到10%安全裕度的解，也就是最大速度不超过v_max的90%到95%，最大加速度不超过a_max的90%到95%。机械臂实际运行中自重负载、摩擦、温度都会影响动态特性，贴着极限值跑迟早出问题。

选好解之后，再把对应的Δ代回去生成最终B样条控制点，重新在非常密的采样点上验证一遍轨迹约束，然后导出为关节位置序列下发到控制器。

5. 实操中的坑：这些问题我每个都踩过

5.1 基函数在u=1处求值为零

这是B样条编程最常见的坑。递归定义的基函数在u等于最后一个节点值时，所有基函数都可能返回0，导致轨迹在终点处掉到零。

处理办法是特殊判断：当u接近U(end)时，强制让最后一个基函数的值为1，其余为0。或者把采样区间稍微收缩一点，比如linspace(0,1-1e-6,N)，但如果终点也要精确经过路点，还是加特殊判断更可靠。

5.2 高次B样条的矩阵条件数问题

七次B样条基函数本身就容易产生病态矩阵，特别是路点分布不均的时候。我实测过均匀参数化的条件数可以到1e8，控制点反算出来的结果完全没法用，曲线到处乱抖。

解决方法是累积弦长参数化加正则化。quadprog里加1e-10的正则项能有效抑制高频振荡。如果问题仍然严重，考虑降低次数到五次或者调整路点参数化方式。

5.3 NSGA-II目标函数里quadprog报错

quadprog在部分时间分配下可能无解，因为插值约束加上最优性目标存在数值问题。我在目标函数里加了try-catch，一旦quadprog报错，直接给个体赋极大的目标值和约束违反量，让NSGA-II自然淘汰它。这比让程序崩溃强得多。

另外quadprog的求解速度在种群评估中会被放大几百倍，尽量用离散化后的稀疏矩阵保存H，避免每次重复计算。

5.4 约束采样点数不能太少

速度加速度约束的校验本质上是采样检查，采样点太少会漏掉峰值。我一开始用100个采样点，优化结果通过了校验，但放到机器人上跑的时候加速度超了5%。

原因就是加速度峰值刚好落在两个采样点之间。把采样点加到500到1000之后，这个问题基本消失。优化完成后一定要用更高密度的采样重新验证一次。

5.5 多关节问题别忘协调约束

如果机械臂有6个关节，单纯每个关节独立规划会忽略协同问题。比如某个时刻所有关节同时达到速度峰值，机器人整体能量消耗和机械冲击都会很大。改进方向是在目标函数里加上关节速度的加权范数，或者加关节力矩约束。至少在做末端轨迹跟踪时，要对所有关节的速度模长和加速度模长做联合约束检查。

6. 一些后续可以扩展的方向

七次B样条加NSGA-II这套框架跑通之后，扩展性其实很强。如果项目要求考虑关节力矩约束，可以在目标函数里加入机械臂动力学模型，计算关节力矩并作为新的约束项。如果想要轨迹更平滑，可以把加加速度jerk也纳入约束。如果要做避障，可以用人工势场或碰撞距离函数作为额外的目标函数项。

我个人在实际操作中的体会是，轨迹规划这个领域，公式看懂不难，难的是把每个细节都做对。B样条的端点处理、控制点反算的数值稳定性、约束校验的采样密度、优化算法的参数选择，任何一个环节出问题，最后跑出来的轨迹都是废的。希望这篇能把你的MATLAB调试时间从几天压缩到几小时。