上个月调一套直角坐标机械臂的视觉抓取程序抓取轨迹是一条折线先沿X方向进再转Y方向出。第一版轨迹直接在拐点处拼接两个直线段看起来也没什么问题。结果一跑起来末端在拐角处明显顿了一下电机还嗡嗡响。查了很久不是伺服参数没调好也不是负载惯量算错真正的原因很简单轨迹生成只保证了位置连续速度方向却在拐点处瞬间突变。这个问题的标准解法就是给轨迹做空间平滑过渡。而我这次用的方案是6点5次贝塞尔曲线。说白了就是用一条5次贝塞尔曲线把原来那个尖角过渡成一条光滑曲线让机器人在拐弯时速度方向能连续变化加速度也不会突然跳变。这篇文章就把这条曲线的原理、控制点配置、代码实现、参数调整和现场问题排查全部讲清楚。做工业机器人、移动机器人、AGV、数控路径规划的朋友都可以直接参考。1. 空间平滑过渡到底解决什么问题1.1 一段让人头疼的“折线轨迹”先回到现象本身。我们常见的机器人路径很多都是折线视觉引导抓取时的进出段、码垛路径的层间切换、AGV地图里的直角弯本质上都是一堆直线段首尾相接。如果对这些直线段不做任何处理直接让机器人沿折线跑从运动学角度看会出什么事我们来拆一下。第一位置是连续的。机器人肯定能从一个点走到下一个点不会凭空跳过去。第二速度矢量不连续。假设前一段沿X正方向以200mm/s运动后一段沿Y正方向也以200mm/s运动那么在拐点那一瞬间速度方向从X轴突然变成Y轴。速度大小虽然没变但方向变了相当于在拐点处施加了一个无穷大的速度变化率。现实中的电机当然无法承受无穷大的加速度所以控制器会强制减速等机器人差不多停在拐点附近再重新加速往新方向走。这就是我们看到的“顿一下”。用数学语言说这样的拼接轨迹只在连接点处做到了C0连续也就是位置连续但C1速度都不连续。对机器人来说C0连续是底线C1连续是及格线C2连续加速度连续才是大多数场景下真正能平稳运行的要求。再往上还有C3连续加加速度连续那是高速高精加工才追求的。1.2 平滑过渡的数学本质很多人一听到“平滑过渡”就以为是“加个圆弧”其实圆弧只是一种特例。平滑过渡的数学本质是把原来连接点处的速度方向突变变成一段有限时间内的连续变化同时最好让加速度也连续变化。机器人轨迹的连续性等级和运动冲击的关系可以这样看连续性等级数学含义运动学表现实际影响C0位置连续轨迹无断点最基本要求但可能出现折角C1速度连续速度大小和方向无突变无明显停顿但加速度可能突变C2加速度连续加速度无跳变惯性力连续机械冲击显著降低C3加加速度连续加加速度无跳变适合高速高精应用轨迹更柔和对大多数工业机器人场景做到C2连续已经能解决绝大多数的抖动、顿挫、异响问题。C3以上当然更好但实现成本和调参难度都会上升而且对机械本体刚度不够好的设备C2和C3的区别其实很难体感出来。所以空间平滑过渡的核心任务就是在两条轨迹段之间插入一段新轨迹使得整体在连接点处至少满足C2连续。也就是说位置、速度、加速度三个量都要在过渡段起点和终点处与前后轨迹段对齐。1.3 为什么选中“6点5次贝塞尔曲线”贝塞尔曲线是图形学和轨迹规划里最经典的参数曲线。它的特点是n次贝塞尔曲线由n1个控制点定义。所以6个控制点对应的就是5次贝塞尔曲线这就是标题里“6点5次”的由来。那为什么不用更低阶的比如3次贝塞尔曲线只有4个控制点在端点位置和端点速度都确定的情况下自由度已经全部用完没有余量再去约束端点加速度。也就是说用3次贝塞尔做过渡能做C1连续但做不到C2连续。而5次贝塞尔的6个控制点刚好可以在固定端点位置、端点速度、端点加速度这6个约束之后把曲线形状唯一的确定下来。如果用更高阶比如7次、9次也不是不行但控制点多了调参难度和过冲风险都会增加工程上不划算。贝塞尔曲线还有一个很重要的性质凸包性。整条曲线一定落在所有控制点围成的凸包内。这意味着只要控制点摆得离原始轨迹不远过渡曲线就绝对不会飞出离谱的距离。这个性质在工程上非常有用后面调轮廓误差的时候就是靠它兜底。2. 6点5次贝塞尔曲线控制点怎么摆原理是什么2.1 从4点3次到6点5次多出的两个点用来干什么先看3次贝塞尔曲线它由P0、P1、P2、P3四个控制点组成曲线从P0出发到P3结束。P0到P1的方向决定了曲线起点的切线方向P3到P2的方向决定了曲线终点的切线方向。端点处的速度大小与P1相对P0的距离成正比具体是3倍的P0P1向量终点是3倍的P3P2向量。这带来一个问题如果你已经确定了P0、P3并且要求起点速度是某个大小和方向终点速度是另一个大小和方向那么P1和P2的位置就已经被锁死了。加速度怎么办没有自由度去管了。所以3次贝塞尔曲线做两段直线之间的过渡只能保证速度连续加速度大概率在端点处跳变。现场表现就是机器人过拐角不再停顿了但会在过渡段两端各抖一下。5次贝塞尔曲线则在P0和P3之间多了P2和P4两个控制点六个控制点是P0、P1、P2、P3、P4、P5。多出来的这两个点的核心作用就是用来匹配端点加速度。一个典型的配置方式是P0、P1、P2三个点控制起点的位置、速度和加速度P3、P4、P5三个点控制终点的位置、速度和加速度。2.2 端点速度、加速度与控制点的对应关系5次贝塞尔曲线的标准形式是B(t) (1-t)^5 * P0 5t(1-t)^4 * P1 10t^2(1-t)^3 * P2 10t^3(1-t)^2 * P3 5t^4(1-t) * P4 t^5 * P5其中t是曲线参数取值范围是0到1。对t求一阶导得到B(t) 5(1-t)^4 * (P1-P0) 20t(1-t)^3 * (P2-P1) 30t^2(1-t)^2 * (P3-P2) 20t^3(1-t) * (P4-P3) 5t^4 * (P5-P4)继续求二阶导B(t) 20(1-t)^3 * (P2-2P1P0) 60t(1-t)^2 * (P3-2P2P1) 60t^2(1-t) * (P4-2P3P2) 20t^3 * (P5-2P4P3)把t0和t1代入会得到几个非常重要的端点性质位置B(0) P0B(1) P5一阶导B(0) 5(P1-P0)B(1) 5(P5-P4)二阶导B(0) 20(P2-2P1P0)B(1) 20(P3-2P4P5)这组公式怎么看一句话总结起点的速度由P1相对P0的位置决定起点的加速度由P0、P1、P2三点的相对位置决定终点对称。特别注意第二个式子如果P2-2P1P0 0也就是P1正好是P0和P2的中点那么起点加速度就是0。反过来看当P0、P1、P2三个点等距排列在一条直线上时曲线在起点的加速度为0同时在起点处速度方向也正好沿着这三个点所在的直线。这对直线段过渡来说是再理想不过的边界条件了。因为实际机器人轨迹是有时间概念的贝塞尔曲线本身只定义了路径形状并不包含速度规划。假设实际时间τ从0到T轨迹写成Q(τ) B(τ/T)那么实际端点速度就不是B(0)而是B(0)/T。实际端点加速度也不是B(0)而是B(0)/T^2。把比例系数换算过去用真实速度v和真实加速度a来表示控制点公式就变成起点端P1 P0 v0 * T / 5P2 2P1 - P0 a0 * T^2 / 20终点端P4 P5 - v1 * T / 5P3 2P4 - P5 a1 * T^2 / 20这里的v0、a0是过渡段起点的实际速度和加速度v1、a1是终点的实际速度和加速度。这组公式是通用的不管前后轨迹段是直线还是圆弧只要能给出端点速度和加速度就能算出控制点。2.3 直线-直线过渡的控制点配置公式最常见的机器人拐角场景是两条直线段拼接。比如从A点直线走到B点再直线走到C点B是拐点。做平滑过渡时要保证过渡段起点P0在前一段直线AB上终点P5在后一段直线BC上。先定义几个量u是AB方向的单位向量w是BC方向的单位向量v是机器人沿直线段运动的速度大小T是过渡段总时间d是P0到B点的距离同时也是P5到B点的距离。在对称设计下P0和P5离B点的距离相同。控制点的摆法如下P0 B - d * uP1 P0 (v*T/5) * uP2 P1 (v*T/5) * uP5 B d * wP4 P5 - (v*T/5) * wP3 P4 - (v*T/5) * w看出来了吗P0、P1、P2三个点在AB直线段上等距排列P3、P4、P5在BC直线段上等距排列。这样的布置刚好满足起点和终点的加速度都为0并且端点速度大小都等于v方向与前后直线段一致。因此过渡段和前后直线段之间位置连续、速度连续、加速度连续也就是完整的C2连续。这里有一个硬性条件vT/5必须严格小于d。否则P1会越过B点跑到拐角的另一侧去控制点分布就不再落在原始路径段上了。工程上一般建议vT/5取值在d的20%到50%之间这样曲线既能自然弯曲又不会过于贴近拐角。3. 实操步骤从控制点到可执行轨迹3.1 第一步提取原始轨迹信息动手写代码之前先把过渡需要的信息整理清楚。对于一个直角拐角需要知道的原始信息有拐点B的坐标这是两条直线段的交点进入方向单位向量u即上一段直线末端指向B的方向离开方向单位向量w即B指向下一段直线末端的方向直线段上的运动速度v单位要和坐标一致期望的过渡时间T过渡距离d这几个量里方向单位向量通常由路径点序列的差分得到。比如路径点A、B、C是三个三维坐标u normalize(B-A)w normalize(C-B)。注意路径段如果太短归一化时会出现除零问题后面会专门讲这个坑。v可以直接取工艺速度比如码垛速度200mm/s或者视觉引导的TCP速度150mm/s。T和d则需要人工根据节拍和轮廓误差要求来定通常是先估算一个初值再仿真调优。3.2 第二步计算6个控制点用Python写一个控制点计算函数非常直接import numpy as np def build_transition_control_points(B, u, w, speed, T, d): step speed * T / 5.0 if step d: raise ValueError(speed*T/5 必须小于 d请减小速度或过渡时间或增大过渡距离) P0 B - d * u P1 P0 step * u P2 P1 step * u P5 B d * w P4 P5 - step * w P3 P4 - step * w return np.array([P0, P1, P2, P3, P4, P5])这里的B、u、w都是numpy数组支持二维或三维坐标。speed是速度大小T是过渡总时间d是过渡距离。实际算一个例子。假设路径是A(0,0)经过B(100,100)到达C(200,0)这是一个90度折角的路径。取速度200mm/s过渡时间0.25秒过渡距离40mm。u normalize(B-A) (0.7071, 0.7071)w normalize(C-B) (0.7071, -0.7071)。speed * T / 5 200 * 0.25 / 5 10mm。这个值是10小于d的40mm满足条件。逐点计算P0 (100,100) - 40*(0.7071,0.7071) (71.7157, 71.7157)P1 (71.7157,71.7157) 10*(0.7071,0.7071) (78.7868, 78.7868)P2 (78.7868,78.7868) 10*(0.7071,0.7071) (85.8579, 85.8579)P5 (100,100) 40*(0.7071,-0.7071) (128.2843, 71.7157)P4 (128.2843,71.7157) - 10*(0.7071,-0.7071) (121.2132, 78.7868)P3 (121.2132,78.7868) - 10*(0.7071,-0.7071) (114.1421, 85.8579)可以看到六个控制点分两组各自由等距点组成一组在AB线段方向上一组在BC线段方向上。把这条曲线画出来会发现它从AB段上某个点平滑地转到BC段上某个点拐角处不再有尖角。3.3 第三步采样生成轨迹点控制点拿到之后下一步就是按照实际控制周期采样。5次贝塞尔曲线的求值直接用标准公式即可def eval_bezier5(ctrl, t): P0, P1, P2, P3, P4, P5 ctrl mt 1.0 - t return (mt**5)*P0 5*t*(mt**4)*P1 10*(t**2)*(mt**3)*P2 \ 10*(t**3)*(mt**2)*P3 5*(t**4)*mt*P4 (t**5)*P5如果还想看速度、加速度曲线就对t求导def eval_bezier5_vel(ctrl, t): P0, P1, P2, P3, P4, P5 ctrl mt 1.0 - t return 5*(mt**4)*(P1-P0) 20*t*(mt**3)*(P2-P1) \ 30*(t**2)*(mt**2)*(P3-P2) 20*(t**3)*mt*(P4-P3) 5*(t**4)*(P5-P4)注意这两个函数返回的都是曲线参数t下的位置和导数。如果采样时t是等间隔的0到1并且假设实际时间τ从0均匀增加到T那么实际速度要除以T实际加速度要除以T^2。这一点非常容易搞错很多人直接在控制器里用等间隔t发送位置结果发现实际速度完全对不上理论值。对多数控制器来说位置指令是按固定周期下发的。假设周期是4ms过渡总时间T是0.25秒那么采样点数就是0.25/0.00462.5取63个点。每个点的实际时间τ对应到一个tt τ/T。然后把对应的B(t)位置发给控制器。如果T取得不合适和控制器周期不是整数倍关系插补点会多一个或少一个这个问题可以直接通过计算采样点数的取整来处理。3.4 第四步仿真验证连续性控制点配置和采样都做完之后不要急着上机先在仿真里看三条曲线位置曲线、速度曲线、加速度曲线。位置曲线平滑只能说明样子好看真正反映运动平稳性的是速度和加速度曲线。检验点有这么几个过渡段起点和终点的速度是否等于前后直线段的速度。如果不等说明控制点计算和实际速度不匹配。加速度曲线在起点和终点是否连续。如果端点加速度不等于前后直线段的加速度那过渡段和直线段拼接处还是会抖。加速度曲线内部有没有尖峰。贝塞尔曲线本身是光滑的但如果控制点布局太差加速度曲线会出现非常大的峰值导致电机过流。在我这个例子里直线段运动时加速度为0过渡段起点和终点加速度也都是0所以加速度曲线会从0平滑地变化再回到0中间没有跳变。这种曲线放到控制器上跑机械冲击会比折线拼接小很多。4. 关键参数怎么调过渡距离、时间与轮廓误差4.1 过渡距离d平滑性和贴线性的天平过渡距离d是P0点到拐点B的距离。这个参数直观上决定了过渡曲线距离原始路径“有多远”。d越大过渡段越长曲线在拐角处的曲率越小机器人转弯越从容加速度峰值越低运动越平稳。但代价是曲线偏离原始路径的距离也越大。如果机器人的工作空间里正好有障碍物或者视觉系统要求末端严格经过某个定位点过渡距离太大就可能撞东西或者定位偏差超标。d越小曲线越贴近原始折线轮廓误差小但转弯越急促加速度峰值高电机负载大。实际调参时我一般先用路径段长度的5%~15%作为初值。比如一段500mm的直线段d取25mm到75mm左右。然后再根据加速度峰值和轮廓误差去微调。4.2 过渡时间T与速度匹配过渡时间T是一个容易被忽视但极其重要的参数。它直接决定了过渡段的实际速度。回到控制点公式P1相对P0的偏移量是vT/5。如果T太小P1、P2会非常靠近P0曲线起点处速度方向虽然还是沿AB方向但整段过渡会非常急促等效于在很短时间内完成转弯加速度自然就大。如果T太大速度匹配出问题甚至会让vT/5超过d直接报错。T的初值可以用期望的过渡段平均速度来估。简单的方法是让T大约等于2d/v左右也就是大致让机器人以和直线段差不多的速度走完这段过渡。比如d40mmv200mm/sT可以取0.4秒左右。这样v*T/516mm小于d同时P1点还算靠近P0曲线不会甩得太远。如果工艺上对节拍敏感可以适当减小T但要注意加速度峰值。估算加速度峰值有个粗糙的办法把过渡段近似看成一个圆弧半径约为d乘以某个比例系数向心加速度约等于v^2/r。如果算出来加速度超过设备允许值就说明T太小或者d太小得往回调。4.3 如何估算轮廓误差轮廓误差是说过渡曲线偏离原始折线多远。贝塞尔曲线的凸包性质给我们一个上界过渡曲线一定在所有控制点的凸包内。因此控制点离原始路径越近轮廓误差上界越小。但实际最大误差点一般在曲线中点附近。对于对称布置的六点控制最大误差大致发生在t0.5附近。仿真时可以直接取t从0到1扫一遍计算每个采样点到直线段AB或BC的最小距离再取最大值。如果轮廓误差超差有两条路可以走。第一条减小d这是最直接的。第二条调整step相对d的比例也就是改变v*T/5和d的关系。step越接近d控制点越靠近B点曲线越“贴角”轮廓误差越小但转弯越急促。step越小控制点离B越远曲线越圆弧化轮廓误差越大。实际调节时要反复权衡。4.4 不同场景的推荐参数区间不同机器人应用对过渡参数的要求差别很大应用场景过渡距离d过渡时间T主要关注点工业机械臂搬运5~50mm0.1~0.4sTCP速度稳定节拍移动机器人AGV0.1~1m0.5~2s最小转弯半径乘客舒适度数控加工0.05~0.5mm毫秒级轮廓误差进给速度视觉引导抓取几mm~几十mm0.1~0.3s定位精度视觉拍照时序数控加工对轮廓误差要求极其严苛通常不会用单纯贝塞尔过渡而是用NURBS样条。贝塞尔曲线更适合对轮廓要求不是极致精密的机器人运动场景。5. 常见问题与排查技巧实录5.1 常见问题速查表现象可能原因排查方向过渡段起点和终点速度不匹配控制点step算错或T取得不对检查v*T/5是否与P1P0距离一致过渡段轮廓误差偏大d太大或step相对d太小减小d或增大step比例过渡段末端仍抖动端点加速度不匹配前一段不是匀速用通用公式代入a0、a1而不是简单等距控制器上轨迹速度与仿真不一致采样t映射到时间时没除以T检查实际速度是否等于B(t)/T速度提前减速节拍变慢对速度方向变化预估过于保守放宽速度限制优化过渡参数实时生成时偶发位置跳变相反方向控制点越过拐点B检查v*T/5 d是否恒成立5.2 一个真实排查案例为什么过渡段末端总会抖一下有次调试六轴机械臂过渡段起点很平稳速度曲线也光滑但就是在过渡段快结束时机械臂末端明显抖一下。位置曲线看不出任何问题但加速度曲线在末端有一个微小的尖峰。排查过程是这样的先看控制点终点端的P3、P4、P5看起来都落在BC直线方向上似乎没问题。再看速度匹配发现实际控制器是按固定插补周期发位置指令的而我把t映射成时间时想当然地用了等间隔t没有做弧长参数化。贝塞尔曲线本身参数速度和实际弧长速度并不严格成正比尽管曲线从视觉上看很均匀参数速度在接近终点时已经有了偏差最终导致实际速度在末端有一个很小的波动。解决办法是在上位机先做一次弧长参数化也就是把曲线采样成密集点序列计算累计弧长再按期望速度把累计弧长映射到t上。经过这一步之后加速度曲线末端的尖峰消失了。这个案例说明位置曲线再好看也不代表速度曲线和加速度曲线没问题。调运动平稳性一定要把速度曲线和加速度曲线画出来看。5.3 在线实时生成时的几个坑如果过渡段不是离线算好而是在机器人运行过程中实时生成有几个细节需要特别注意。第一方向单位向量归一化时路径段长度可能很短甚至接近零导致u或w出现异常值。在线生成前一定要判断路径段长度短于一定阈值就跳过过渡处理或改用直线拼接。第二控制器插补周期是固定的过渡时间T不一定能被周期整除采样点数和最后一段位置可能对不上。处理办法是记录累计时间按实际时间计算t而不是用固定的索引步长。第三实时计算时尽量用double类型避免用float累积误差。过渡距离只有几十毫米的时候float的精度在大坐标下可能不够用位置指令会出现肉眼可见的抖动。还有一个隐蔽的坑多轴机器人下发位置指令时如果每个轴走的是独立控制周期没有统一时间戳会出现轻微的不同步。运动控制器通常会自己处理这个问题但如果自己做上位机要把同一个时间戳下的六个关节角度打包一次性下发不要分轴发送。5.4 与控制器内置过渡功能的取舍很多商业机器人控制器本身带拐角过渡功能比如ABB的Corner、KUKA的Smooth、FANUC的Corner Zone。这些内置功能用起来很方便只要设一个半径值就行。那为什么还要自己造轮子内置过渡的优点是省事缺点是参数不透明。你永远不知道它在内部用的是圆弧还是样条也不知道它实际轮廓误差是多少。对普通搬运、码垛场景内置过渡完全够用。但如果你需要精确控制轮廓误差、需要C2连续保证、需要在开源ROS控制器里实现自定义轨迹或者需要把路径结果导出给其他模块做碰撞检测那自己用贝塞尔曲线生成过渡段就更可控。我的一般建议是快速验证用内置功能精细控制和自己搭建运动系统时用本文这套方法。二者可以并存不冲突。个人体会是做机器人轨迹平滑这事核心不在于把算法做得多么花哨而在于把连续性的概念真正落实到每一个控制点、每一个采样周期上。我现在调试运动系统第一反应已经不再是调伺服增益而是先看轨迹的速度曲线和加速度曲线。只要轨迹本身做到了C2连续大部分现场抖动和异响都会明显减轻。最后分享一个小技巧上机之前把过渡段的加速度曲线打印出来贴在示教器旁边一旦现场异常先对照曲线检查轨迹参数再动伺服参数排查效率会高很多。