★ FEATURED ARTICLE 做过机器人轨迹规划的人应该都有过这种经历让机械臂沿一个矩形路径走到了拐角不降速末端就会明显“抽”一下轻则工件表面留下刀痕重则直接触发加速度报警。很多人会用控制器自带的拐角过渡功能但默认的zone/CNT方案大多只是“圆角减速”真正把机器人空间平滑过渡做成高阶曲线、让速度曲线和加速度曲线都连续的做法其实并不多。这篇文章想聊的就是我一直在用的方案6点5次贝塞尔曲线专门用来生成连接两段路径的过渡段。我会从为什么选5次、控制点怎么求、插补时有哪些坑、实测中怎么调这几个角度把整套思路还原出来给做运动控制、机器人集成调试或者正在研究轨迹规划的朋友做个参考。1. 从一段矩形路径讲起拐角为什么不敢不降速1.1 最原始的处理方式到拐角先刹停先回到最基本的问题。机械臂沿着一组示教点走如果控制器不做任何过渡处理就意味着它必须在每个拐角点达到速度为零然后换个方向重新加速。这种做法的好处是简单可靠路径精度完全贴合示教点坏处也很明显每过一个拐角就是一次“刹车—转向—起步”节拍被拖得很难看。假设一台机器人走一个300mm × 300mm的矩形目标TCP速度500mm/s如果四个角都要刹停再起步光加减速过程就要花掉大量时间。对这种场景很多人的第一反应是“那我用圆弧过渡不就行了”确实圆弧过渡可以让机器人不刹停直接拐过去但真正调到高速时你会发现圆弧方案并没有从根本上解决冲击问题。1.2 圆弧过渡的天花板切向连续不等于加速度连续圆弧过渡的经典做法是在两条直线段的交点附近取出两个切点用一段圆弧把它们连起来。几何上圆弧和前后直线段在切点处切线方向一致所以位置连续、切线连续都满足也就是常说的C0和C1连续。但问题在于机器人实际运动的速度和加速度不是只看几何还要看时间参数化。如果你的规划器让机器人在直线段上跑匀速500mm/s进入圆弧段后切向速度虽然大小不变但方向开始连续变化这时会产生向心加速度。圆弧的曲率半径一旦取得太小向心加速度就可能超出机械臂的允许范围而如果为了安全把过渡半径取得很大轮廓偏差又会超标。更麻烦的是在圆弧的两个切点处加速度从“零”突然变成“向心加速度”这个瞬间的加速度跳变会让机械臂硬件感受到一次冲击。我把这种现象称为“天花板”圆弧过渡只能保证几何平滑很难保证运动学平滑。想要在拐角处既不掉速太多、又不产生冲击需要的不只是“把角磨圆”而是要构造一条在两个端点都能同时约束位置、速度、加速度的过渡曲线。1.3 平滑的实质想消除冲击就得提高连续性阶数先说结论再解释原理。我们评估两段轨迹拼接得好不好通常会看三个连续性指标C0连续位置连续轨迹不会断开C1连续速度连续端点处速度方向一致不会“折”C2连续加速度连续端点处加速度也一致不会“顶”一下。如果用生活经验类比C0连续相当于开车压过一个“坎”C1连续相当于方向盘突然猛打一下C2连续相当于方向盘是逐渐打过去的。对工业机器人来说绝大多数抖动问题都出在加速度不连续也就是只做到C1、没有做到C2。而5次贝塞尔曲线之所以在这种场景里特别有价值就是因为它能同时锁定端点的位置、速度和加速度让过渡段和前后直线段在拼接点上实现完整的C2连续。这不是玄学而是由它的数学结构决定的接下来我把这块拆开讲。2. 6个控制点、5次多项式的数学必然性2.1 五次多项式为什么是“刚好够用”的那一档先看多项式本身。一条五次多项式曲线可以写成B(u) a5*u^5 a4*u^4 a3*u^3 a2*u^2 a1*u a0参数u从0到1一共有6个待定系数。现在看一下我们需要多少个边界条件在u0过渡段起点需要位置q0、速度v0、加速度a0在u1过渡段终点需要位置q1、速度v1、加速度a1。加起来正好6个条件。6个方程解6个未知数五个系数全部落定。这就是“5次”的数学必然性它是能同时约束位置、速度、加速度的最低次数。三次多项式只有4个系数只能约束位置和速度端点加速度没法控制七次多项式有8个系数就算约束jerk加加速度都绰绰有余但后面会说到盲目提高次数反而会带来新的问题。2.2 换成贝塞尔形式每个控制点都有运动学含义五次贝塞尔曲线有6个控制点P0到P5方程形式是B(u) C(5,0)*(1-u)^5*P0 C(5,1)*(1-u)^4*u*P1 C(5,2)*(1-u)^3*u^2*P2 C(5,3)*(1-u)^2*u^3*P3 C(5,4)*(1-u)*u^4*P4 C(5,5)*u^5*P5这看起来只是一串系数但贝塞尔曲线有一个非常漂亮的特性端点处的导数可以由相邻控制点直接算出来。对5次贝塞尔B(0) P0 B(1) P5 B(0) 5*(P1 - P0) B(1) 5*(P5 - P4) B(0) 20*(P2 - 2*P1 P0) B(1) 20*(P5 - 2*P4 P3)这些式子反过来解就能直接从边界运动状态求出控制点P0 q0 P1 q0 v0/5 P2 q0 2*v0/5 a0/20 P5 q1 P4 q1 - v1/5 P3 q1 - 2*v1/5 a1/20所以控制点不是随便拖出来的“调形手柄”每一个都有明确含义P0和P5定端点位置P1和P4由端点速度方向决定P2和P3由端点加速度决定。这也是为什么我特别喜欢这种参数化方式在工程里调试时你知道动哪个控制点会影响什么而不是像某些自由曲线一样全靠试。2.3 为什么不是3次也不是7次我把常用次数做了一个对比应该能说明问题次数控制点个数端点可约束的运动量主要短板3次4位置、速度加速度不可控拼接处仍有冲击5次6位置、速度、加速度端点的jerk会有跳变高刚性场景需注意7次8位置、速度、加速度、jerk控制点多容易出现龙格振荡数值不稳定这里单独说一下高次多项式的问题。很多人觉得次数越高越平滑实际不是。高阶曲线对控制点极其敏感某个控制点有少量数值误差曲线在端点附近可能会产生明显的波浪状摆动也就是计算数学里说的龙格振荡。对于实时插补系统来说这会导致轨迹抖动反而比5次更难调稳。5次贝塞尔处在“约束能力足够”和“数值特性稳定”的甜点区这是我推荐它的核心原因。补充一句5次贝塞尔虽然在端点能做到C2连续但在端点处的jerk加加速度仍然可能不连续。这一点先记着后面实测调参部分我会专门讲它带来的振动问题。3. 过渡段生成流程从原路径到6个控制点3.1 先在原路径上把过渡区间定出来有了公式第二步是在原始路径上选出一段“要被替换掉”的区域。假设现在有两条直线段交于拐角点O目标是用一段贝塞尔曲线替换掉靠近O的一小段。实际操作中我会在进入拐角的直线段上选一个点P_start在离开拐角的直线段上选一个点P_endP_start和P_end离拐角点的距离称为过渡距离d。曲线生成后机器人实际走的路径就是从P_start开始脱离原直线到P_end重新贴回原直线。这样做的意思是它天然保证了过渡段起点和终点的位置都落在原路径上和前后直线段拼接时只有速度、加速度需要额外对齐。过渡距离d怎么定有两个约束要同时满足。第一个是轮廓偏差。过渡段越贴近拐角路径越“紧”但允许通过的速度上限也越低。工程上可以先用一个粗略公式估算初始值如果两段直线的夹角为θ允许的最大轮廓偏差为ε那么过渡距离近似为d ≈ ε / sin(θ/2)。这只是几何初值实际还要考虑机器人动力学约束后面我会再讲校验方法。3.2 从运动状态求解控制点可以直接抄的代码这一节直接给可以用的Python代码。设过渡段起点位置为q0终点位置为q1起点速度、加速度为v0、a0终点速度、加速度为v1、a1六个控制点可以这样求from math import comb import numpy as np def solve_control_points(q0, v0, a0, q1, v1, a1): 根据边界运动状态求解6个控制点 q: 位置v: 速度a: 加速度 坐标单位要保持一致比如都用mm和mm/s q0 np.array(q0, dtypefloat) v0 np.array(v0, dtypefloat) a0 np.array(a0, dtypefloat) q1 np.array(q1, dtypefloat) v1 np.array(v1, dtypefloat) a1 np.array(a1, dtypefloat) P0 q0 P1 q0 v0 / 5.0 P2 q0 2.0 * v0 / 5.0 a0 / 20.0 P5 q1 P4 q1 - v1 / 5.0 P3 q1 - 2.0 * v1 / 5.0 a1 / 20.0 return np.array([P0, P1, P2, P3, P4, P5]) def bezier5(P, u): 根据控制点和参数u(0~1)求贝塞尔曲线上的点 n 5 result np.zeros_like(P[0]) for i in range(n 1): result comb(n, i) * (1 - u) ** (n - i) * u ** i * P[i] return result给一个具体的数字例子。假设机器人以500mm/s沿X方向进入过渡段在终点处速度方向偏转45度速度大小还是500mm/s起终点加速度都为0前后都是匀速直线段。起点q0取(0, 0, 0)终点q1取(80, 80, 0)那么v0 (500, 0, 0)v1 (353.55, 353.55, 0)计算出的控制点大概是控制点坐标 (mm)P0(0, 0, 0)P1(100, 0, 0)P2(200, 0, 0)P3(-61.42, -61.42, 0)P4(9.29, 9.29, 0)P5(80, 80, 0)第一次看到这个结果的人通常会问P3怎么跑到负坐标去了离终点那么远是不是算错了其实没算错。P3是终点侧的“加速度控制点”它的位置由末端速度方向反向推导出来不一定落在路径附近。这种现象恰恰是5次贝塞尔为了满足端点切向条件所必需的。理解这一点之后调参会容易很多否则很容易把正确结果当成bug去改。3.3 最容易犯的错误控制点“扭头”和单位混乱我见过不少初学贝塞尔过渡的人在这里翻车。第一个坑是控制点“扭头”。有些人拿到前后直线段上的几个采样点直接把它们填进P1、P4甚至P2、P3想着“反正控制点会拉出一条曲线来”。结果生成出来的曲线在起点先往反方向退一段再拐向终点看起来就像先倒车再转弯。原因很简单位置控制点和导数控制点语义完全不同。P1必须等于P0加上v0方向上的一个偏移P4必须等于P5减去v1方向上的一个偏移而不是随便一个原路径上的点。用上一节公式计算天然就能避开这个坑。第二个坑是单位。P1 q0 v0/5这个式子当坐标单位是毫米、速度单位是毫米每秒的时候P1到P0的距离直接等于“5毫秒内走过的距离”。如果速度用了m/s坐标用了mm那曲线长度会直接缩水一个量级机器人实际走的路径会严重偏离预期。所以在处理数据时务必保证位置、速度、加速度三者的单位体系一致。4. 从几何曲线到真实运动参数化与插补4.1 重要认知贝塞尔参数u不是时间这一步是几何曲线变成真实运动的关键很多人会在这里卡住。贝塞尔曲线的参数u只描述曲线形状u从0均匀变到1对应的并不是路径上的均匀位移更不是时间上的均匀变化。如果你的控制器直接每隔固定时间间隔取一个均匀的u值去插补末端速度会出现明显的波动。正确的做法是做弧长参数化。步骤如下在u轴上取一组密集的采样点比如200个均匀分布在[0,1]区间用贝塞尔曲线公式求出这200个点然后累加相邻点之间的距离得到累计弧长s建立一张(u, s)映射表插补时先由速度规划器给出当前时刻应走的弧长s(t)再在映射表里反查对应的u最后重新代入贝塞尔公式取点。这个过程本质上是在回答“走到第几厘米时参数u是多少”这个问题。对绝大多数工业应用来说用累计弦长代替真实弧长已经足够精确因为过渡段内部曲率变化不会特别极端。只有当机器人的重复定位精度小于0.02mm或者过渡段内部出现很大曲率时才需要考虑用数值积分做更精确的弧长计算。4.2 和S型速度规划对接推荐逐段处理再校验弧长参数化之后过渡段就变成了一条有明确长度的曲线。接下来要解决的是速度规划怎么和它衔接。这里有两种做法。第一种是把整条路径直线段过渡段当作一条连续曲线算出总弧长L然后对整个L做S型速度规划生成一条s(t)曲线。听起来很完美但工程实现比较复杂尤其是多条直线段和多个过渡段拼接时计算量和边界处理的复杂度都会上升。第二种更实用把过渡段当成一个独立的小段规划器预先算出过渡段允许的通过速度然后让前后直线段在这个速度下“接进来”。具体操作是在生成本节控制点之前先根据过渡段的曲率分布和各轴速度加速度限制倒推出过渡段的v_max然后把这个v_max作为前后直线段的目标速度。这样每个过渡段端点上的v0和v1都是已知常量控制点可以直接求S型规划器也只需要按“直线段速度—过渡段速度—直线段速度”做一次前瞻就行。如果你需要快速估算过渡段允许速度可以用向心加速度的经典公式先对贝塞尔曲线做一阶导、二阶导计算曲率最大值κ_max然后v_max ≈ sqrt(a_centripetal_max / κ_max)。需要说明的是这只是初值真实关节约束还要通过逆运动学逐点校验这个放到下一章讲。4.3 姿态部分不要拿欧拉角直接做贝塞尔到这一步位置已经可以平滑过渡了但还有一个容易忽略的问题姿态。六维空间的机器人运动不仅包含TCP位置还包含工具姿态。很多人处理姿态时直接把欧拉角当成三个分量和位置一起塞进控制点生成一条六维贝塞尔这么做会出两个问题。一是万向锁。欧拉角在某些角度附近会发生奇异性插补结果会突然产生姿态跳变。二是欧拉角的数值空间不是度量三维旋转的自然空间三个角度线性插值出来的中间姿态往往会“绕远路”看起来就是在空中画了一个不自然的弧线。实践经验是位置和姿态要分开处理位置部分使用本文的6点5次贝塞尔姿态部分如果转角较小可以先对姿态误差取对数映射成旋转矢量对旋转矢量做5次贝塞尔插值再指数映射回旋转矩阵或四元数如果转角较大或者要求连续多段平滑建议用四元数Squad插值它天然适合单位四元数流形上的平滑过渡。我见过不少视觉引导项目位置平滑做得很好但姿态没有处理末端工具在过渡段里猛地一偏在工件表面拉出一道斜槽。排查这个问题有个简单办法在仿真环境里给TCP画出一个坐标系然后观察过渡段内坐标轴三个方向的角速度曲线如果姿态角速度曲线出现尖峰就说明姿态插值方式需要改进。5. 实测中跑不稳的三个典型症状振动、超差、掉速5.1 过渡段两端有规律振动问题多半出在jerk跳变6点5次贝塞尔保证了端点C2连续但前面提过端点的jerk加加速度仍然可能不连续。在刚性较差、负载较重的设备上这个jerk跳变足以激励起机械共振。如何在现场判断这个问题我常用的办法是录波形。让机器人以同一程序重复跑某个过渡段同时抓取关节电机电流或者末端加速度计信号。如果波形在每个过渡段的入口或出口附近都出现固定位置的尖峰那基本可以断定是轨迹曲线本身的问题如果是机械共振尖峰应该是固定频率的和位置没有这种对应关系。针对这类问题的处理办法有三个按优先级排列降低过渡段的端点速度给jerk留出缓冲空间如果设备负载大工艺又要求高速度可以考虑只在问题轴或关键段升到7次贝塞尔显式约束端点jerk在控制器里加入加速度前馈补偿减小跟踪误差带来的冲击。我个人的原则是能通过调整过渡半径和速度解决就不要轻易上7次否则调试成本会明显增加。5.2 轮廓偏差超预期过渡距离不能拍脑袋定过渡距离d设得越大速度可以跑得越高但路径偏离原始拐角的量也越大。很多工艺对路径偏差有明确限制比如涂胶轨迹、焊接轨迹偏出0.5mm可能就算不合格。一个实用的做法是把原始路径和生成后的贝塞尔过渡段都离散成点云然后计算原路径点到贝塞尔曲线的最近距离取出最大值作为最大偏差。这个值必须进入设计闭环。实际调参的时候过渡距离往往是“先在纸上算一个初值再在线验证一版再微调”的循环过程。经验规律是过渡段离拐角越远最大偏移越大两段直线的夹角越尖锐相同过渡距离下的偏移也越大。比如90度拐角下过渡距离从30mm提到100mm最大偏差可能会从2mm量级涨到5mm量级。所以不要问“过渡距离设50mm行不行”必须结合允许偏差和机器人速度一起定。5.3 笛卡尔速度没超关节却报速度超限还有一种很隐蔽的情况过渡段的TCP速度明明没有超过设定值但运行时某个关节报速度超限错误。原因是笛卡尔速度恒定不代表关节速度恒定。尤其在过渡段姿态变化快、或者路径靠近奇异位形时雅可比矩阵会放大关节速度。处理办法是在生成过渡段后、正式下载到控制器之前做一次离线逆解校验沿贝塞尔曲线采样每隔几毫米取一个点对每个点做逆运动学得到六个关节角对关节角序列做数值微分检查角速度和角加速度是否在允许范围内如果超限优先增大过渡半径其次降低过渡段通过速度最后才考虑修改过渡段形状。这里要特别提醒不建议在实时插补循环里做临时限速那种做法虽然能避免报警但会引入新的加速度尖峰反而加剧抖动。离线算好边界是一条更稳妥的路。6. 商业控制器里的内置过渡用什么以及多拐角过渡怎么扩展6.1 常见控制器的zone/CNT方案本质是什么接触过几类主流商业控制器的人应该知道ABB有zone参数发那科有CNT参数本质上都是让机器人在到达示教点之前提前转弯。它们内部实现的过渡算法很大一部分是抛物线混合或者圆弧过渡再加速度重规划。这类方案的优点是参数少、实时性好、调试门槛低非常适合产线工人快速调轨迹。缺点也明显一是连续性通常只能保障到C1在高速大负载场景下仍会产生冲击二是轮廓偏差和速度上限之间的关系不够透明调节起来基本靠经验试。对于自研运动控制器、离线编程工具或者对节拍和精度有极致要求的项目用6点5次贝塞尔替代内置zone过渡是可以明显提升运动质量的。6.2 连续多拐角的过渡关键是要做速度前瞻前面讲的都是单个拐角的过渡但一条实际路径往往有连续好几个拐角。如果每个拐角独立生成过渡段问题就来了第一个过渡段的终点速度方向和大小可能和第二个过渡段的起点要求对不上。解决这个问题的最好方式是先把整条路径的拐角全部识别出来做一次速度前瞻规划。我目前的做法可以归纳成五步预扫描整条路径识别所有拐角并记录相邻拐角之间直线段的长度基于各关节的速度、加速度限制为每个拐角分配一个可行的通过速度根据每个拐角允许的通过速度、角度变化和轮廓偏差要求逐段计算过渡区间长度和控制点把所有直线段和过渡段拼接成一条完整路径做统一的弧长参数化进入实时插补按规划好的s(t)取点执行。第2步是整个方案的核心。如果跳过它每个拐角单独生成的曲线在拼接处可能会出现速度不匹配原本做的平滑工作就白费了。6.3 两个控制点微调技巧最后分享两个调参技巧都是我从实际项目里攒出来的。第一个技巧如果要微调过渡段形状优先动P2和P3不要动P1和P4。P1和P4负责端点速度方向改了它们等于改了和相邻直线段的衔接条件P2和P3只影响起始加速度和结束加速度向曲线内部的延伸方式动它们不会破坏C2连续性。实际微调时P2、P3沿某一方向偏移的量控制在两端点距离的5%以内一般不会引起曲线中段的异常波动。第二个技巧如果想让过渡更快“贴回”原直线可以适当减小过渡段两端的速度方向变化率也就是让P1更靠近P0、P4更靠近P5。这会让曲线在端点附近更快转向但要注意这同时会增大过渡段中部的曲率峰值所以需要同步校验关节速度。我一般会在调试记录里记下控制点坐标、最大偏差、最大关节速度这三组数据方便多组方案横向对比。这套6点5次贝塞尔方案我最早是在一个三轴平台上跑通的后来才逐步移植到六轴机械臂上。回头看最花时间的从来不是公式本身而是单位、坐标系和姿态处理这些容易被忽略的细节。如果你正准备在自己的控制器里实现类似功能我的建议是先在小负载低速设备上把公式链路验证一遍把控制点坐标打印出来对照公式手动核算一次确认没有单位问题再配上示波器看速度曲线和电流波形确认过渡段边界没有尖峰。等这两步都稳了再上高速产线能省下大量排查时间。
阅读完成 · 觉得有帮助？
