六自由度机械臂的轨迹规划，说简单也简单，说复杂那是真复杂。我刚入行那会儿，以为给机械臂写个路径点，让它从A点走到B点就算完事。结果第一次实测就翻车——末端执行器到了指定位置，但整个臂身抖得跟筛糠似的，电机嗡嗡作响，吓得我赶紧拍下急停。后来才明白，光有路径远远不够，还得有“轨迹”，也就是每个关节在每个时刻的位置、速度、加速度。而轨迹怎么生成，就是多项式插值和B样条曲线这些数学工具的主场。这篇文章，我就把这两个方案的思路、选型、实操细节和踩坑记录完整分享一下，希望能给正在搞机械臂运动控制的同行一些参考。
1. 轨迹规划到底在解决什么问题
1.1 轨迹规划在机械臂控制链路中的位置
机械臂的控制链路大致可以拆成四层：任务规划、轨迹规划、轨迹跟踪控制、伺服执行。任务规划决定“做什么”，比如抓取某个零件；轨迹规划决定“怎么动”，即生成各关节随时间变化的位置序列；轨迹跟踪控制负责让实际关节角跟随目标值；伺服执行则是底层电机驱动器的工作。
很多新手把大部分精力花在底层控制上，PID调参调到飞起，结果发现机械臂动起来还是又慢又抖。其实问题往往出在轨迹规划这一层——你给了一个不连续、不平滑的期望轨迹，底层的PID再厉害也追不上。轨迹规划输出的是一系列带时间戳的关节位置指令，本质上是在给控制层“出题”。如果题目本身就是跳跃的、突变的，那控制层再努力也是事倍功半。
所以轨迹规划的核心任务，是在满足运动约束的前提下，生成一条尽可能平滑、可执行的关节空间轨迹。这里的“平滑”不是单纯看着顺眼，而是指速度和加速度连续，甚至加加速度也要有界。机械臂的真实物理世界是有惯性和弹性的，不连续的加速度会导致机构冲击和振动。
1.2 关节空间与笛卡尔空间：先决定在哪里规划
规划轨迹前，第一步要决定在哪个空间里规划。关节空间规划针对每个关节单独生成角度轨迹，计算简单，天然避开奇异性问题；笛卡尔空间规划则直接控制末端执行器的直线或圆弧路径，直观但需要实时做逆运动学解算，且容易遇到奇异点导致关节速度爆炸。
我个人的习惯是：凡是需要末端走直线、走圆弧的工艺场景（比如焊接、涂胶、3D打印），优先考虑笛卡尔空间规划；凡是点到点搬运、抓取，直接关节空间规划，又稳又快。两种空间下都可以用多项式或B样条，只是表达的对象不同——前者对关节角插值，后者对末端位姿插值。
1.3 评判轨迹好坏的四个维度
在深入具体算法前，先聊聊怎么评判一条轨迹好不好。我总结了四个维度：平滑性、精确性、时间最优性和实现复杂度。
平滑性看速度和加速度曲线是否连续，有没有突变；精确性看轨迹是否精确经过中间路径点，有没有偏差；时间最优性看同样的运动距离能不能用更短的时间完成；实现复杂度则看算法在嵌入式控制器上能否跑得动。这四个维度是互相博弈的，比如追求时间最优往往会牺牲平滑性，追求平滑又会增加计算量。实际项目中，咱们得根据应用场景找平衡点。搬运码垛任务对时间敏感，平滑性可以稍微放宽；精密装配对平滑和精确要求极高，时间反而可以妥协。
2. 多项式插值轨迹的核心逻辑与实战参数
2.1 三次多项式：入门最快，但小心加速度跳变
三次多项式轨迹是几乎所有机械臂运动控制教材的开篇案例，公式很简单：q(t) = a0 + a1·t + a2·t² + a3·t³。四个未知系数，正好对应四个约束条件：起点位置、终点位置、起点速度、终点速度。把这四个约束代进去，解一个四元一次方程组就得到系数。
三次多项式最大的优点就是简洁直观，代码量小，计算开销低，每个轨迹周期只需要带入一个三次函数求值。在很多老式PLC和低端MCU上，这点计算量非常友好。
但它的缺点同样明显：加速度曲线是一条直线，且起终点处的加速度通常是跳变的。也就是说，机械臂启动的一瞬间，加速度从0瞬间跳到一个非零值，对应的加加速度是无穷大。在高速运动场景下，这个突变会造成末端振动，对减速机和法兰盘都是持续的冲击。所以三次多项式入个门可以，实际工程中如果要跑高速高精度，我建议直接用五次多项式或B样条。
2.2 五次多项式：平滑性拉满，代价是计算量
五次多项式 q(t) = a0 + a1·t + a2·t² + a3·t³ + a4·t⁴ + a5·t⁵ 就有六个系数，对应的约束条件也扩展为六个：位置、速度、加速度在起终点的值。恰好满足“起止加速度为0”的约束，加速度曲线连续无跳变，整条轨迹的平稳性比三次好了一个档次。
我在一个搬运项目里同时用三次和五次多项式做了对比测试，同样的路径和时间，三次多项式方案下末端最大残余振动幅度大约是五次方案的三倍。用加速度计贴在末端法兰上测，数据差异非常直观。
代价方面，五次多项式需要解一个6×6的线性方程组，看起来比三次复杂，但对现代控制器来说完全不是负担。我建议如果你决定用多项式方案，直接上五次，别用三次，因为代码复杂度差别不大，但效果差异明显。不过五次也不是没缺点——它容易在中间段产生过冲，尤其是起始和终止位置距离短但时间也短的时候，位置曲线会在中间有一段反向摆动。这个后面在排查章节详细说。
2.3 多项式轨迹的边界条件与时间缩放技巧
多项式轨迹的边界条件决定了轨迹的动态特性，整条轨迹的形状完全由这些边界条件决定。实际项目中，起点速度通常设为0（机械臂从静止开始），但终点速度不一定是0——比如在连续生产线上的跟踪抓取，末端到达抓取点时需要保持与传送带同步的速度，这时候终点速度就必须设为非零值。
时间参数也对轨迹形状有直接影响。假设原本设定3秒走完的轨迹，因为工艺节拍加快需要2秒完成，最简单的办法是对时间轴整体缩放——所有时间变量乘以系数即可，因为多项式插值满足仿射不变性。但要注意，时间缩短后速度和加速度都会等比放大，可能超过电机额定转速和最大扭矩。所以在实际使用中，不能只顾着压缩时间，一定要回头校验缩放后的速度峰值和加速度峰值是否在允许范围内。
我在实操中会先算出轨迹的峰值速度、峰值加速度和峰值加加速度，再拿关节电机的额定参数去对比，留足安全余量。很多新手忽略这一步，结果轨迹规划在仿真里看着完美，一上真机就过流报警，大概率就是峰值加速度超了电机能力。
3. B样条曲线：连续性和局部性的最佳平衡
3.1 B样条的本质：一段一段拼起来的光滑曲线
如果说多项式插值是把整条轨迹当作一个整体来构造，那B样条就是把它拆成若干小段，每一段都是低阶多项式，段与段之间满足高阶连续条件，拼成一条整体光滑的大曲线。这个思路很像用几段圆弧拼一个平滑拐角的道路设计——每条圆弧本身简单，但拼接处保证切线方向一致甚至曲率连续。
B样条的数学定义是控制点、节点向量和基函数的加权组合，但实际工程中你不需要每次都从基函数算起，绝大多数时候是拿现成的库（比如Eigen、GSL或者机器人工具箱）来解。真正需要理解的是：k阶B样条曲线具有C^(k-2)连续阶。换句话说，三次B样条是C²连续，也就是位置、速度、加速度全部连续，这一点直接契合机械臂轨迹规划的平滑性需求。
3.2 节点向量、控制点、阶数怎么选
B样条的自由度主要来自三个部分：控制点、节点向量和阶数。控制点就是曲线外的那几根“牵引绳”，改变控制点位置曲线会被“拉”向或“推”离该点；节点向量决定了每段曲线的长度和形状自由度；阶数决定了曲线的全局平滑程度。
我的经验是，实际项目里优先用三次B样条（k=4），不要一上来就上高阶。五次B样条虽然连续阶更高，但控制点增多后，数值稳定性和实时计算量都会变差，而且对轨迹形状的调节也更不直觉。三次B样条已经能保证加速度连续，工程上够用。
节点向量的选择直接影响轨迹的起始和终止行为。为了让轨迹精确穿过起点和终点，通常采用Clamped B样条，即节点向量前后的重复度等于阶数。这样曲线在两端会被“钉死”，而中间区域保持B样条的柔性和局部性。控制点数量根据中间路径点的数量来定，我一般让控制点数量等于路径点数量加2，这样既能保证穿过多径点，又有足够的形状自由度。
3.3 位置路径加时间参数化才是完整轨迹
B样条本质上描述的是路径（位置与形状），不带时间信息。而机械臂执行器需要的是随时间变化的位置序列，所以必须做时间参数化：把B样条路径分配出一个时间轴，通常以弧长为中间变量，再根据速度、加速度约束建立弧长与时间的映射关系。
实际操作中，我先对B样条路径计算总弧长，然后将弧长按梯形速度规划或S形速度规划映射到时间轴，最后在每一个控制周期内求解该时刻对应的弧长，再反查B样条上的位置点。这一套流程看起来绕，但好处是路径形状和平滑性由B样条保证，速度曲线由速度规划器控制，两者解耦，特别适合需要严格限制速度和加速度的场景。
4. 实操过程：从仿真验证到真机部署
4.1 先用仿真环境把逻辑跑通
在碰真机之前，我习惯先在仿真环境里把轨迹算法完整验证一遍。这里推荐用基于物理引擎的仿真环境，比如带碰撞检测的动力学仿真平台。虽然仿真永远不会完全复现真机的摩擦、间隙和柔性效应，但用来验证轨迹生成的逻辑正确性和平滑性、检查速度加速度峰值、排除奇异性问题，已经非常够用。
建好六自由度机械臂模型后，我会把多项式轨迹和B样条轨迹分别编写成两个独立的轨迹生成模块，接口保持一致，方便切换对比。输入是一组关节角度路径点，输出是一串带时间戳的关节位置序列。然后我会把速度曲线和加速度曲线画出来，肉眼检查有没有突变和毛刺。
在实际测试中，我发现B样条生成的加速度曲线在轨迹中段会比五次多项式更平滑，尤其是在路径点比较多、路径变化剧烈的时候。因为B样条的局部修改特性，中间某个路径点的调整不会影响到远处轨迹段；而多项式插值是全局的，改一个边界条件整条曲线都会跟着动。这个特性在我们反复调整机械臂避障路径时特别有用。
4.2 多项式方案在真实控制周期里的落地细节
真机部署和仿真最大的区别在于控制周期和时间同步。常见的位置控制模式，控制周期是1毫秒或2毫秒，轨迹生成模块必须在这个周期内算出一个期望关节位置。我做的时候是把轨迹生成做成一个状态机：进入轨迹后，按控制周期递增时间，再根据当前所处的时间段计算各关节的目标位置，然后通过实时总线发给伺服驱动器。
这里有个很关键的坑：一定要确认轨迹输出时间戳和底层控制器的时钟是同步的。如果不加处理地直接用系统时钟计时，长时间运行后可能会发生时间漂移，导致机械臂实际轨迹和理论轨迹错开几个毫秒。虽然几毫秒的偏差看起来微不足道，但在连续高速运动场景下，位置误差会被放大，末端路径走得越来越不准确。
我的做法是在轨迹开始前记录启动时刻，之后每次插补都用“当前时刻-启动时刻”作为轨迹时间参数，而不是直接调用系统时间。这样即使控制器调度有轻微抖动，轨迹本身的时间基准依然稳定。另外，每个控制周期结束时，把当前的期望速度一并传给驱动器，可以显著改善跟踪效果，避免位置模式下速度滞后带来的路径轮廓误差。
4.3 切换B样条时要注意的坑
从多项式切到B样条，并不是换一个轨迹生成库那么简单。最大的变化在于你需要提前准备好控制点和节点向量，也就是你得先有一组“路径骨架”，再让B样条在这个骨架上生成光滑轨迹。这比直接给定起终点要多一步预处理。
我最初在项目中引入B样条时，遇到过一个棘手的现象：路径形状完全正确，但速度曲线在路径的某些区段出现大幅波动，甚至速度会反向。排查了好久，最后发现是时间参数化出了问题——按弧长等比例分配时间后，在弧长变化缓慢的区域（即曲率较大的弯道），如果速度规划没有充分考虑离心力约束，实际合成速度就会在弯道处骤然加大或减小，看起来像速度“抽搐”。
解决办法是在时间参数化阶段，把每个路径点的曲率半径和关节速度极限一起考虑进去，对速度曲线做基于曲率的限速。弯道急的地方自动降速，弯道缓的地方再提回来。这样B样条轨迹才真正实用，否则它生成的路径看着漂亮，实际跑起来可能比多项式轨迹还要难受。
5. 常见问题与排查技巧实录
5.1 轨迹抖动：先看加速度曲线再动PID
机械臂轨迹运行中抖动，是绝大多数新手首先会怀疑PID参数没调好。但我的经验是，轨迹的加速度连续性对抖动的影响远大于PID参数的细微差异。排查顺序非常重要：先把轨迹的加速度曲线拉出来看，若起终点或中间路径点存在跳变，百分之百是轨迹规划的问题，调PID只会越调越乱。
我接手过一个振动明显的定位任务项目，第一反应是伺服增益太低，于是把位置环增益一路往上调，结果机械臂开始发出高频啸叫，振动反而更剧烈。后来冷静下来查轨迹，发现原方案用的是三次多项式，加速度在起终点直接跳变。换成五次多项式后，同样的PID参数，振动基本消失。那次之后我立了个规矩：任何抖动问题都必须先排查轨迹，再做控制参数调整。
5.2 奇异点附近速度爆炸
六自由度机械臂在做笛卡尔空间规划时，接近奇异位形时关节速度会趋近无穷大。即使你在关节空间规划，如果中途经过奇异点附近的构型，也会出现某个关节瞬时速度极大的情况，很容易触发驱动器过速报警甚至损坏减速机。
我的排查经验是：在轨迹生成后增加一个前置校验步骤，遍历轨迹上的每一帧关节位置，用雅可比矩阵的条件数来判断距离奇异点的程度。条件数超过设定阈值时，要么在路径上绕开奇异区域，要么在该区域切换成关节空间的过渡轨迹。这一招虽然增加了计算负载，但换来的安全性非常值得。
5.3 末端路径偏移超差
当机械臂沿笛卡尔直线轨迹运动时，如果实际路径与理想直线出现明显偏离，先别急着归咎于机械臂刚度。轨迹规划层的一个常见原因是：笛卡尔空间插值得到的位置序列，没有经过逆运动学后验证。也就是说，你把规划好的末端位姿点反解成关节角度时，如果某些点在奇异点附近或精度不够，逆解结果会产生偏差。
我常用的校验方法是：把规划出的关节角度正向运动学映射回笛卡尔空间，再和原始规划位置做对比，计算每个点上的末端位置误差。如果误差超出工艺要求，就要加密插补点，或者对中间点直接使用反向解算得到的精确关节位置，而不是纯笛卡尔插补。
5.4 中间段过冲与“走弯路”现象
五次多项式虽然在起终点平滑，但两个路径点之间距离较短、速度要求较快时，容易在中间段产生过冲回摆。这本质上是多项式对边界条件过度“响应”带来的惯性。B样条通过控制点布局可以规避这个问题，方法是把控制点放在路径点附近而不是精确落在路径点上，给曲线留出适当的张力调节空间。
如果你必须用多项式方案，可以用分段多项式加中间速度约束的方式抑制过冲：在路径中点处强制设定一个中间速度值（一般取前后段速度的平均），把一条五次多项式拆成两段五次多项式拼起来，每段只负责一半路径。这样处理简单，实际效果也不错。不过这个方法就借用了B样条的“分段”思想，我发现很多工程问题用多项式解决到一半，最终都会导向B样条这个更灵活的框架。
我在实际项目中经历了几次从“多项式落地”到“B样条重构”的往复之后，形成了一个习惯：如果轨迹路径点少、要求简单，优先用五次多项式快速产出一个可跑的轨迹；如果路径点多、有实时避障调整需求，或者对路径平滑度要求极高、对加速度连续性要求苛刻，那就一步到位用B样条。这两个工具不存在谁完全替代谁的关系，适合的才是最好的。还有个细节，无论用哪个方案，都一定要在轨迹执行前多做一次峰值速度和峰值加速度校验，多留一点余量，真机能少出很多事。
