机械臂轨迹规划实战：从五次多项式插值到B样条避障

做机械臂这些年我越来越觉得轨迹规划是真正拉开水平差距的地方。同一个机械臂运动规划写得好的动作丝滑、定位利落写得糙的动作一顿一顿末端还乱抖。说白了机械臂轨迹规划就是回答一个问题从A点到B点每个关节在每一秒到底该转到哪个角度、以什么速度转、加速度多大。最经典的入门套路是从五次多项式插值开始等遇到避障、多段拼接就得请出B样条曲线。这篇文章把我从五次多项式插值到B样条曲线避障的一整套实现思路、Python代码和真实踩坑经历写出来正在入门机械臂控制、或者做过分运动学但没怎么碰过轨迹规划的朋友应该能直接拿去用。1. 轨迹规划到底在解决什么问题1.1 一次点对点运动里藏着三个量很多人拿到机械臂第一反应是目标关节角算出来了让电机转过去不就完了问题恰恰出在这个“转过去”上。你要是直接给驱动器一个阶跃目标电机会以最大能力加速到目标点附近再急刹车末端在那一瞬间加速度大得吓人整条手臂都跟着抖。更麻烦的是多个关节如果各自按自己的节奏冲末端就会走出一条莫名其妙的曲线该避的障碍一点都避不开。所以我习惯把一次点对点运动拆成三层来看位置、速度、加速度。位置决定末端走什么形状速度决定运动节奏加速度直接决定惯性力——而惯性力跟机械臂的抖动、电机的过载、末端手爪里抓的东西稳不稳全都挂钩。轨迹规划的本质就是同时约束这三层。只有位置没有速度规划等于给电机出了一道没头没尾的题只有路径没有时间执行层照样不知道该跑多快。1.2 三次多项式不够用在哪里初学者最常接触的是三次多项式插值。三次多项式只有四个系数刚好能约束起点和终点的位置、速度但加速度完全控制不了。这么做的后果是在每一段运动的边界处加速度会从某个值瞬间跳到另一个值。在公式里这只是一个数学上的“尖角”但落到真实机械臂上就是实打实的冲击载荷。我印象很深的一次是调试一个小型四轴机械臂。电机不发烫、结构也没有明显松动但末端装的摄像头画面总是一顿一顿。后来把速度曲线和加速度曲线拉出来一看问题全在加速度突变上。换成五次多项式之后起止时刻的加速度可以显式约束成0整个运动过程一下子就顺了。所以只要你对运动平稳性有要求别犹豫直接用五次多项式。1.3 五次多项式插值的约束方程五次多项式的形式是 θ(t) a0 a1t a2t² a3t³ a4t⁴ a5*t⁵一共六个未知系数正好对应六个边界条件t0 时位置 q0速度 v0加速度 acc0tT 时位置 q1速度 v1加速度 acc1把这六个条件代进去就能得到关于 a0 到 a5 的线性方程组。这里有个小技巧不用费劲去背闭式公式直接用线性方程组建个 6×6 矩阵交给 numpy 的np.linalg.solve一次搞定。搞嵌入式、要写C代码的时候再把这个矩阵解析化简成闭式公式也不迟。2. 五次多项式插值的Python实现2.1 用线性方程组求解比背公式更稳下面这段代码是我平时最常用的版本。把六个边界条件写成矩阵q0/q1是起止位置v0/v1是起止速度acc0/acc1是起止加速度T是总运动时间。返回的c就是 a0 到 a5 六个系数。import numpy as np def quintic_traj(q0, q1, v0, v1, acc0, acc1, T): 五次多项式系数求解 返回 [a0, a1, a2, a3, a4, a5] A np.array([ [1, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0], [0, 0, 2, 0, 0, 0], [1, T, T**2, T**3, T**4, T**5], [0, 1, 2*T, 3*T**2, 4*T**3, 5*T**4], [0, 0, 2, 6*T, 12*T**2, 20*T**3] ]) b np.array([q0, v0, acc0, q1, v1, acc1]) return np.linalg.solve(A, b) def quintic_pos(c, t): return c[0] c[1]*t c[2]*t**2 c[3]*t**3 c[4]*t**4 c[5]*t**5 def quintic_vel(c, t): return c[1] 2*c[2]*t 3*c[3]*t**2 4*c[4]*t**3 5*c[5]*t**4 def quintic_acc(c, t): return 2*c[2] 6*c[3]*t 12*c[4]*t**2 20*c[5]*t**3这里有个特别容易踩的坑矩阵第二行是速度条件不是位置条件所以系数是[0, 1, 0, 0, 0, 0]加速度条件那行才是[0, 0, 2, 0, 0, 0]。我见过好几份代码把这两个条件顺序写反结果算出来的位置倒是碰巧对的速度加速度全乱套。写完之后一定要用边界值验算一遍等于用 np.allclose 检查 θ(0)、θ(T) 是否满足。2.2 最常用的特例静止到静止实际工程里有一个出现频率极高的特例起点和终点的速度、加速度全部为0。比如机械臂从静止开始精确停到某一点再去抓取。这时候系数会大幅化简教科书里经常写成a0 q0a1 0a2 0a3 10 * (q1 - q0) / T³a4 -15 * (q1 - q0) / T⁴a5 6 * (q1 - q0) / T⁵这个公式很好记调试的时候可以直接拿它当 sanity check。我用一般线性方程组解出来的结果跟这个闭式公式对过很多次分毫不差。但要注意单位统一——角度全部用弧度时间全部用秒。之前有朋友把角度值直接填进去系数算出来小数点后一长串一看就是单位出了问题。2.3 把位置、速度、加速度曲线画出来写完函数第一件事永远是画图验证。别急着上真机也别急着进仿真先在本地把三条曲线画出来import matplotlib.pyplot as plt T 3.0 # 总时间 3 秒 q0, q1 np.deg2rad(15), np.deg2rad(75) # 从 15° 转到 75° c quintic_traj(q0, q1, 0, 0, 0, 0, T) ts np.linspace(0, T, 200) pos np.array([quintic_pos(c, t) for t in ts]) vel np.array([quintic_vel(c, t) for t in ts]) acc np.array([quintic_acc(c, t) for t in ts]) plt.figure(figsize(9, 3)) plt.subplot(1, 3, 1); plt.plot(ts, np.rad2deg(pos)); plt.title(position (deg)) plt.subplot(1, 3, 2); plt.plot(ts, np.rad2deg(vel)); plt.title(velocity (deg/s)) plt.subplot(1, 3, 3); plt.plot(ts, np.rad2deg(acc)); plt.title(acceleration (deg/s^2)) plt.tight_layout() plt.show()跑完你会看到位置曲线是一条平滑的 S 形速度从0开始中间到峰值再回0加速度首尾都是0中间有一个平稳的起伏。这就是五次多项式最舒服的状态。如果加速度曲线在端点出现跳变不用查别的先看边界条件是不是被填成0了。3. 从多项式到B样条换工具的真正原因3.1 五次多项式的三个痛点五次多项式虽然香但它应付不了复杂场景。第一个痛点是全局性整条曲线由两端边界条件完全决定中间没有可以“动手脚”的余地。想让路径中间稍微绕开一个障碍你只能改起点终点或者加路径点分段处理一改全改非常僵硬。第二个痛点是多段拼接麻烦。一段段五次多项式拼接起来每段的起点条件要跟上一段终点的位置、速度、加速度对齐段数一多约束方程越堆越复杂。我见过有人用12段五次多项式拼一条复杂路径光排约束就排了一个通宵。第三个痛点是overshoot。五次多项式本质是全局多项式运动时间一旦拉长或者起点终点挨得近中间往往会出现超出端点范围的过冲。关节角还好说要是用来规划末端笛卡尔位置过冲可能直接让机械臂撞到工作台边缘。所以在避障和复杂路径面前需要一种“既有平滑性又能局部调整”的工具这就是B样条登场的原因。3.2 B样条曲线的三要素B样条曲线有三个核心概念控制点Control Points、节点向量Knot Vector、次数Degree。控制点是定义在平面或空间里的一系列点曲线并不一定穿过它们更像是被它们“吸引”过去。节点向量决定曲线在哪一段被哪些控制点影响次数决定每小段曲线的多项式阶数。三者之间的关系我常用一个不太严谨但很好懂的说法控制点像一排磁铁B样条曲线像一根被磁铁拉着的软铁丝磁铁越近曲线越贴过去但不会完全吸死。节点向量则是软铁丝上的分段标记标记越多铁丝能弯的地方越多。工程上B样条次数通常取3这样曲线在整个定义域上具有 C2 连续——位置、速度、加速度都是连续的。3.3 为什么说B样条天然适合避障B样条有两个对避障至关重要的性质。第一个是局部支撑性第 i 个控制点只影响参数 u 在区间 [u_i, u_{ip1}) 内的那一段曲线移动一个控制点远处的曲线纹丝不动。这跟五次多项式“牵一发动全身”完全不同。第二个是凸包性整条曲线都落在控制点构成的凸包范围内。这个性质在避障里特别值钱——如果所有控制点都在障碍物同一侧曲线就一定不会绕到障碍物另一侧去。做碰撞检测的时候可以先拿凸包快速筛选再对曲线采样精确检测省下大量计算时间。基于这两点把避障问题转化成“移动控制点”或者“优化控制点位置”就顺理成章了。4. B样条曲线的Python实现与避障思路4.1 Cox-de Boor递推实现基函数B样条的理论核心是基函数最经典的计算方法是 Cox-de Boor 递推。实现起来代码不长但有几个细节必须处理到位不然曲线端点会不贴合控制点。def bspline_basis(i, p, u, knots): 第 i 个 p 次B样条基函数在参数 u 处的值 if p 0: if knots[i] u knots[i 1]: return 1.0 # 处理右端点u 等于最后一个节点时令最后一个基函数为 1 if abs(u - knots[-1]) 1e-12 and i len(knots) - 2: return 1.0 return 0.0 left 0.0 if knots[i p] - knots[i] 1e-12: left (u - knots[i]) / (knots[i p] - knots[i]) * bspline_basis(i, p - 1, u, knots) right 0.0 if knots[i p 1] - knots[i 1] 1e-12: right (knots[i p 1] - u) / (knots[i p 1] - knots[i 1]) * bspline_basis(i 1, p - 1, u, knots) return left right然后生成 clamped 均匀节点向量并采样整条曲线def make_clamped_knots(n, p): n 个控制点、p 次曲线生成 clamped 均匀节点向量 knots np.zeros(n p 1) m n - p - 1 for j in range(m): knots[p 1 j] (j 1) / (m 1) knots[-(p 1):] 1.0 return knots def bspline_curve(control_points, p, num200): 根据控制点生成B样条曲线采样点 n len(control_points) knots make_clamped_knots(n, p) curve [] for u in np.linspace(0.0, knots[n], num): pt np.zeros(control_points.shape[1]) for i in range(n): pt pt bspline_basis(i, p, u, knots) * control_points[i] curve.append(pt) return np.array(curve), knots你注意看bspline_basis里 p0 的分支我特意加了端点处理。很多初版实现会在这里栽跟头参数 u 取到最右端 1.0 时按数学定义最后一个基函数应该是1但常规开区间写法会返回0导致曲线端点差一截。这个小细节是我对比曲线首尾点和控制点时才发现的。4.2 控制点推离法最简单的避障思路拿到B样条之后避障就变成了一个“移动控制点”的问题。最简单的办法是控制点推离法先采样曲线检测哪些曲线点进入了障碍物范围对每一个碰撞点找出当前参数下权重最大的控制点把它沿着远离障碍物圆心的方向推一步重复这个过程直到整条曲线都不再碰撞。def avoid_obstacle(control_points, p, cx, cy, r, step0.3, max_iter50): 把障碍物近似成圆心(cx,cy)、半径r的圆通过推离控制点实现避障 cps control_points.copy().astype(float) obstacle np.array([cx, cy]) for _ in range(max_iter): curve, knots bspline_curve(cps, p, num300) us np.linspace(0.0, knots[len(cps)], len(curve)) hit False for pt, u in zip(curve, us): if np.linalg.norm(pt - obstacle) r: hit True # 当前参数下权重最大的控制点 weights np.array([bspline_basis(i, p, u, knots) for i in range(len(cps))]) i_max int(np.argmax(weights)) vec cps[i_max] - obstacle norm np.linalg.norm(vec) if norm 1e-9: vec np.array([1.0, 0.0]) else: vec vec / norm cps[i_max] cps[i_max] vec * step break # 一次只处理一个碰撞点防止多个点互相干扰 if not hit: break return cps这套思路在障碍物是单个圆形时非常有效而且代码量少。它的缺点是只能“推开”不保证推出来的路径最优所以更适合快速原型验证和中小型项目。真要上复杂障碍环境建议把目标函数换成“路径长度 曲率惩罚 碰撞惩罚”然后用优化器去调整控制点位置思路一致但工程量大不少。4.3 从路径到轨迹用五次多项式调度参数uB样条只解决了“路径”问题还没解决“轨迹”问题——曲线上没有时间信息。我的做法是用一条五次多项式把时间 t 映射到参数 u也就是让 u 从 0 平滑爬到 1再用 u 去采样B样条这样既保留了B样条形状又继承了五次多项式起止速度加速度为零的优点。T 3.0 c_u quintic_traj(0.0, 1.0, 0, 0, 0, 0, T) # u: 0 - 1 ts np.linspace(0, T, 200) path_points [] path_vels [] u_prev 0.0 for t in ts: u quintic_pos(c_u, t) pt np.zeros(2) for i in range(len(cps_new)): pt pt bspline_basis(i, 3, u, knots) * cps_new[i] path_points.append(pt) path_vels.append((pt - path_points[-2]) / (ts[1] - ts[0]) if len(path_points) 1 else np.zeros(2))这样得到的末端位置、速度就是定义良好的轨迹。实际跑机械臂时把速度再放大或者压缩相当于调整整体节奏B样条形状不受影响。用五次多项式调度 u是我试过几种方案里最省心的一种它保证了整个过程中关节不会因为参数变化太快而突然抖动。5. 完整示例五次多项式B样条避障一次跑通5.1 整条管线的流程设计把前面的模块拼成一个完整流程我习惯分四步走用五次多项式生成单关节的起停轨迹验证边界速度加速度为0。在笛卡尔空间布置控制点生成初始B样条路径。对障碍物做碰撞检测用控制点推离法迭代修改路径。再用五次多项式把参数 u 关于时间调度好得到最终可执行的轨迹。这四步每一步都有独立的函数出了问题能单独排查。我强烈建议不要写成一个几百行的脚本至少拆成函数否则调试避障参数的时候会非常痛苦。5.2 核心代码与运行结果这里的场景是末端从 (0, 0) 走到 (10, 0)中间有个圆心在 (5, 1.2)、半径 1.5 的圆形障碍物初始路径正好会撞上去。cps np.array([ [0.0, 0.0], [2.5, 0.3], [5.0, 0.4], [7.5, -0.2], [10.0, 0.0] ]) # 避障前 curve0, _ bspline_curve(cps, 3, 200) # 避障后 cps_new avoid_obstacle(cps, 3, cx5.0, cy1.2, r1.5, step0.3, max_iter50) curve1, knots bspline_curve(cps_new, 3, 200) plt.figure(figsize(6, 4)) plt.plot(curve0[:, 0], curve0[:, 1], b--, labelbefore avoidance) plt.plot(cps[:, 0], cps[:, 1], bo--, alpha0.4) plt.plot(curve1[:, 0], curve1[:, 1], r-, labelafter avoidance) plt.plot(cps_new[:, 0], cps_new[:, 1], ro-, alpha0.4) circle plt.Circle((5.0, 1.2), 1.5, colorgray, alpha0.4) plt.gca().add_patch(circle) plt.legend() plt.axis(equal) plt.grid(True) plt.show()跑出来的现象是蓝色虚线从障碍物圆内穿过去红色实线被“顶”到圆上方曲线整体平滑没有尖角。注意看红色控制点被推离最远的是接近 x5 附近的那个点这正是局部支撑性在起作用——远处控制点几乎没动。5.3 参数调整的节奏和原则避障参数里最影响结果的是step和max_iter。step太小收敛慢可能要跑几百次迭代step太大控制点会被推过头路径出现明显弯折。我建议把step初始设为障碍物半径的 0.1~0.2 倍比如半径1.5就用0.2左右然后观察曲线形态再微调。max_iter从50起步大多数单障碍场景50次内都能收敛。另一个容易忽略的点是碰撞检测的采样密度。num300对我来说是够用的但如果障碍物很小或者路径很长采样点太少会“漏检”——明明穿了障碍物采样点却全在圆外。稳妥做法是把采样密度跟路径长度挂钩路径越长、障碍越小采样点就要越多。6. 踩坑记录与排查建议6.1 常见问题速查表现象原因处理办法曲线末端不贴合最后一个控制点B样条基函数在 u1 处端点处理不对在 p0 分支里对右端点做特殊判断令最后一个基函数为1加速度曲线端点仍然突变边界条件里的速度或加速度没传0检查 quintic_traj 的 v0/v1/acc0/acc1 参数避障迭代不收敛step 太小或 max_iter 太小调大 step或者每次碰撞推离多个权重较大的控制点路径绕过障碍后出现尖角step 太大控制点被过度推离减小 step增加迭代次数曲线总体形态偏移过大一次推一个控制点累计偏移控制点数量过多限制被修改的控制点数量必要时加入平滑惩罚速度曲线在执行端仍然不光滑u(t) 调度方式太粗暴比如直接用线性映射用五次多项式调度参数 u保证速度加速度连续6.2 几条实际项目里的建议第一理论代码跑通之后一定要先在自己的仿真环境里跑动态过程别急着上真机。B样条路径计算是纯几何问题但真机一跑关节限位、奇异位形、力矩饱和这些问题全冒出来了。我习惯把规划出的轨迹先在仿真里把每个关节的角度曲线画出来跟限位表逐项比对。第二避障只规划末端位置是不够的。机械臂是个多连杆系统末端路径躲开了障碍中间关节杆件可能照样撞上去。工程上更严谨的做法是做全链路碰撞检测把每个连杆的包围盒跟障碍物一起做检测。实在没有条件至少要在规划阶段加一个“预留安全距离”把障碍物半径外扩10%到20%给控制误差留余地。第三代码里所有几何运算尽量用 numpy 的数组运算不要写一堆 for 循环。上面给的 bspline_curve 为了可读性用的是循环实际项目里我会用矩阵化改写速度能差几十倍。等你看懂原理之后优化是很自然的事。最后想分享一条经验轨迹规划看着是数学问题实际调试中大部分时间花在“让参数合理”上。五次多项式的 T 选多长B样条的控制点放几个、放在哪step 取多少这些没有一个通用的黄金值全靠根据机械臂的物理限制去定。我现在每次调新设备都会先跑一遍单关节五次多项式摸清楚它的实际速度和加速度极限再去定 B 样条避障的规划参数。设备的脾气摸透了轨迹自然顺。