最近在调机械臂关节空间的轨迹生成模块项目需求很典型末端要依次经过八个目标点路径必须平滑速度、加速度都要卡上限否则电机扭矩一上去就开始抖甚至触发运动学保护。最后落地的一套方案就是标题里这套——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个控制点决定。节点向量两端采用重复度p1的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(i1) N 1; else N 0; end return; end N 0; den1 U(ik) - U(i); den2 U(ik1) - U(i1); 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(ik1) - u) / den2 * bspline_basis(i1, k-1, u, U); end end实际使用时不要用递归去算整个曲线因为递归调用在工程代码里性能不行。更推荐的办法是把所有基函数在一组参数点上一次算完存成矩阵。后面控制点反算和轨迹求值都用矩阵运算。2.2 节点向量怎么生成节点向量的生成直接决定了曲线段的分布。对于p7的clamped节点向量前8个节点是起点值后8个节点是终点值。如果控制点数量是n1内部节点数量是n-p。控制点越多内部节点越多曲线局部调整能力越强。这个项目里我分两种场景处理第一种是八个路点直接对应八个控制点的情况。p7n7内部节点数量为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ⱼ其中j0,...,7。写成矩阵形式就是A·PQA是8×8的采样矩阵A(j1,i1)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ᵀHPs.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样条曲线定义域内的采样点。如果采用八个控制点方案节点向量只有两端重复内部节点数量为0u_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:n1 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(ip1) - U(i1); if den 1e-12 D(:, i) p * (P(:, i1) - 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(ip-11) - U2(i1); if den 1e-12 E(:, i) (p-1) * (D(:, i1) - 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ⱼ₋₁ Δⱼ, j1,...,7然后把这个时间节点映射到[0,1]区间作为B样条的内部节点。这一步是整个优化方案的精髓时间分配一变节点向量就变B样条控制点也跟着变最终轨迹的速度曲线和加速度曲线随之改变。编码时我直接把Δ作为NSGA-II的个体变量这样能天然保证t₀t₁...t₇不需要额外处理单调性约束。个体就是一个7维向量每一维是正数。4.3 目标函数和约束处理NSGA-II多目标优化的两个目标我选的是目标1总时间T ΣΔⱼ最小化目标2加速度均方根RMS(a)最小化这两个目标的物理意义很明确一个追求效率一个追求平稳。帕累托前沿上的每组解都代表了效率和平稳的一种权衡。约束条件就是速度上限和加速度上限。在目标函数里我对每个个体做以下操作根据Δ生成时间节点归一化后构建节点向量U。用quadprog在八个路点插值约束下求最优控制点P。在[0,1]内密集采样计算速度和加速度曲线。检查速度最大值是否超过v_max加速度最大值是否超过a_max。计算约束违反量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 基函数在u1处求值为零这是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个关节单纯每个关节独立规划会忽略协同问题。比如某个时刻所有关节同时达到速度峰值机器人整体能量消耗和机械冲击都会很大。改进方向是在目标函数里加上关节速度的加权范数或者加关节力矩约束。至少在做末端轨迹跟踪时要对所有关节的速度模长和加速度模长做联合约束检查。常见问题现象解决方案基函数端点求值异常轨迹终点偏移或速度不为预期u1时特殊处理最后一个基函数插值矩阵病态控制点反算后轨迹振荡累积弦长参数化加1e-10正则项quadprog无解NSGA-II部分个体评估崩溃try-catch兜底无效个体直接淘汰约束采样稀疏优化通过但实机超限采样点500以上优化后密集复验时间分配过激某段时间过短导致加速度尖峰在目标里加时间平滑项或限制Δ下限6. 一些后续可以扩展的方向七次B样条加NSGA-II这套框架跑通之后扩展性其实很强。如果项目要求考虑关节力矩约束可以在目标函数里加入机械臂动力学模型计算关节力矩并作为新的约束项。如果想要轨迹更平滑可以把加加速度jerk也纳入约束。如果要做避障可以用人工势场或碰撞距离函数作为额外的目标函数项。我个人在实际操作中的体会是轨迹规划这个领域公式看懂不难难的是把每个细节都做对。B样条的端点处理、控制点反算的数值稳定性、约束校验的采样密度、优化算法的参数选择任何一个环节出问题最后跑出来的轨迹都是废的。希望这篇能把你的MATLAB调试时间从几天压缩到几小时。