★ FEATURED ARTICLE 机械手轨迹规划这事儿听着确实唬人又是“规划”又是“轨迹”的好像非得是搞了十几年算法的人才能碰。但实际把壳拆开本质就一句话在起点和终点之间给机械手找一条既能绕开障碍物、又走得平稳顺滑的路。就这么简单。今天我不讲那些长篇大论的数学推导直接上干货先说说B样条曲线在轨迹规划里到底是怎么玩的为什么大家喜欢拿它来做轨迹光顺最后我会甩出一套用Python写的、可以直接跑起来的完整代码你拿来改改参数就能用在自己的简单机械臂项目或者模拟环境里。这篇文章适合刚接触机器人运动规划、被各种论文术语劝退的初学者也适合那些想把轨迹生成模块快速落地、不想再从零造轮子的工程师。1. 先把问题拆明白机械手轨迹规划到底在做什么1.1 路径规划和轨迹规划是两码事很多新手一开始接触这个概念容易把“路径规划”和“轨迹规划”混在一起。我习惯用一句话区分路径规划解决的是“走哪条路”轨迹规划解决的是“这条路怎么走”。路径规划的结果是一串离散的点比如从起点到终点生成了[P0, P1, P2, ..., Pn]这样的路点序列它只关心空间几何不关心时间。而轨迹规划要在这些路点的基础上给每个点配上时间信息让机械手知道什么时刻该到哪个位置、速度多快、加速度多大。换句话说轨迹是时间和位置的函数通常记作 q(t)。为什么要把两步分开因为直接拿路径规划的离散点驱动机械臂机械手每到一个路点就得停下来拐个弯走出来的轨迹是折线关节速度会突变。速度突变意味着加速度无穷大对减速机和电机都是灾难。所以必须拿一条连续曲线把离散路点串起来并且让这条曲线至少达到位置连续加速度连续这就是轨迹光顺的核心目的。说起来简单做起来麻烦的点在于这条曲线既要尽量贴近原始路点不然会撞障碍物又要足够平滑不然机械手抖动既不能过度震荡否则加速度超限又得有局部调整的能力改一个点不影响整条曲线。同时满足这些要求的方案里B样条是工业界用得最多、也是性价比最高的选择之一。1.2 为什么项目里选B样条而不是多项式插值或贝塞尔曲线我最早做实验时也想过用三次多项式一段一段拼或者直接用贝塞尔曲线整体拟合后来在采样点一多、约束一复杂的情况下这两个方案都露馅了。先说要是我现在用的是五次多项式插值它的优点是理论上能保证边界条件连续但问题在于路点一多分段之间要靠额外的连续性条件去“拼接”每增加一个点就要重新推导约束方程代码写起来绕来绕去而且某一处的扰动会传递到后续所有段不够灵活。贝塞尔曲线的问题更明显——它是全局性的移动任何一个控制点整条曲线的形状都跟着变轨迹规划的现场调参体验很差。B样条比它俩都聪明的点在“局部支撑性”这个词上大白话就是你挪动一个控制点影响的只是附近一段曲线不会牵一发动全身。还有两个B样条的关键性质对工程特别重要一个是凸包性曲线被控制点围成的凸包包裹住这给碰撞检测提供了保守判定依据另一个是连续性可控你可以根据需要构造C2连续甚至更高阶连续的曲线。这两个性质组合起来让我既能用少量控制点保证平滑度又能通过检查控制点附近的范围来快速排除碰撞风险。后面讲避障逻辑时你会体会到这两个性质有多好用。2. B样条核心不啃公式也能写代码2.1 三个要素控制点、阶数、节点向量如果你上网搜B样条一定会被一堆递推公式砸到头。我先帮你把核心概念拎出来一共三个。第一个是控制点control points。B样条曲线并不直接经过这些点它更像是在“被这些点牵引着走”。控制点序列我们用P_0, P_1, ..., P_m表示。第二个是阶数degree记作 p。一次就是折线二次就是每段抛物线三次就是每段三次曲线。常见工业应用直接用三次也就是 p3。阶数每提高一阶曲线连续性提高一阶但数值稳定性会下降。这个我后面细说。第三个是节点向量knot vector。这是最容易让人劝退的概念但你没必要纠结它的抽象定义只需要记住它的作用节点向量决定了每一段B样条曲线的“接口”在参数空间的什么位置。节点向量里有一串不递减的数[t_0, t_1, ..., t_k]里面相邻值之间的区间就对应一条曲线段。这三个要素一组合B样条函数其实就变成这样一件事曲线上的每一个点都是对一组控制点做加权平均的结果权重由阶数和节点向量决定。这种“加权拼合”的机制让曲线天然具备局部性。你调整一个控制点的位置它影响的只是该控制点在权重最大的那一段参数区间。2.2 节点向量的选择决定了“穿不穿端点”节点向量怎么选在工程里直接决定了轨迹的起点终点行为。最常见的两种选择Clamped夹紧和 Unclamped不夹紧。Clamped的含义是节点向量开头和结尾的节点值重复 p1 次。这么做的效果是曲线强制经过第一个和最后一个控制点并且起点的切线方向由前两个控制点连线方向决定。对我们做轨迹规划来说起点和终点通常对应机械手的初始位姿和目标位姿必须严格经过所以绝大多数情况直接选Clamped。Unclamped则相反曲线不一定经过端点控制点这在某些需要周期性曲线的场合有用比如机械手做连续圆周运动时想闭合轨迹但一般项目用不上。如果你默认拿工具库直接生成B样条一定要检查是不是Clamped否则机械手起点还没落在你指定的位置上一启动就“啪”地抖一下那块儿谁看了都头疼。我用一句话总结这节工程里做轨迹规划几乎无脑选Clamped节点向量、三阶B样条起点和终点各重复4次节点值。2.3 de Boor递推理论上要懂代码里可以优雅“偷懒”B样条求值最经典的算法是de Boor递推原理上它是逐层线性插值把高阶问题不断降阶最后得到曲线坐标。如果你想自己造轮子或者想在特定嵌入式环境里不依赖库运行算法还是要看一遍的。但今天这套代码面向的是“快速跑起来”所以我直接用scipy.interpolate.BSpline来构建样条底层就是de Boor算法稳定且经过大量测试。不过我要提醒一句如果你打算把这个模块移植到MCU或者机器人控制器上SCIPY是不能用的到时候还是得自己实现de Boor或者用C的Eigen库。那会儿你回来看de Boor的递推公式就会觉得很清晰了。现在刚起步不必重复造轮子先跑通流程最重要。2.4 从路点到控制点的反算这可能是你在代码里遇到的第一个“不直观”的地方。前面说了B样条通常不穿过控制点但我们手上拿到的路点是要被“穿过”的。怎么解决答案是反算控制点把路点当作曲线的型值点求解一个线性方程组反推出那组能让曲线恰好穿过型值点的控制点。这一步在数学上叫“全局插值”。已知节点向量和阶数的情况下每个路点Q_i都可以写成B样条基函数对控制点P_j的线性组合。把所有路点的等式拼到一起就得到一个线性方程组Q M * P其中M是B样条基函数矩阵。代码里直接用np.linalg.solve解这个方程就行。计算量不大路点几百个以内都是一眨眼的事儿。但反算控制点这件事有个坑如果你路点给得太密反算出来的控制点可能会“抖”曲线虽然穿过所有点但形状会产生不必要的波动。这在轨迹规划里很致命因为波动直接导致加速度曲线振荡。后面讲实操参数时我会再提怎么控制路点密度。3. 直接可跑的B样条轨迹生成代码3.1 先准备好环境和依赖这套代码我尽量少依赖只用numpy和scipy。如果你想画图看效果再加一个matplotlib。安装命令三行搞定pip install numpy pip install scipy pip install matplotlib运行环境用Python 3.8以上版本就行裸机直接跑不需要装ROS这类重框架。本意就是把轨迹规划的核心逻辑独立出来方便你快速改动验证。3.2 核心代码实现我先给出一个完整的脚本包含三个功能模块给定路点生成Clamped三次B样条并反算控制点。对样条均匀采样得到位置序列、速度、加速度。在主函数里跑一个简单的2D轨迹例子画出结果。import numpy as np from scipy.interpolate import BSpline import matplotlib.pyplot as plt def generate_clamped_uniform_knots(n_control_points, degree3): 生成Clamped均匀节点向量。 n_control_points: 控制点数量 degree: B样条阶数 返回长度为 n_control_points degree 1 的节点向量 n_knots n_control_points degree 1 internal_knot_count n_control_points - degree - 1 internal_knots np.linspace(0, 1, internal_knot_count 2)[1:-1] knots np.concatenate( ( np.zeros(degree), internal_knots, np.ones(degree 1), ) ) return knots def interpolate_bspline(waypoints, degree3): 根据路点反算B样条控制点返回可调用的BSpline对象。 waypoints: shape (N, dim) 的路点序列N 必须 degree 1 waypoints np.asarray(waypoints, dtypenp.float64) n len(waypoints) knots generate_clamped_uniform_knots(n, degree) # 构造B样条基函数矩阵 M # 在型值点对应的参数位置采样基函数值 # 参数位置取均匀分布即可更精细的做法是用弦长参数化 t_values np.linspace(0, 1, n) M np.zeros((n, n)) for i, t in enumerate(t_values): # scipy的BSpline设计矩阵不便直接获取这里用设计矩阵思路 # 构造一个在每个基函数位置取值的矩阵 # 简便方法逐列计算BSpline basis pass # 更直接的实现利用scipy的splrep/splev做插值 # 注意 tck 返回的是 (t, c, k)c 就是我们要求的控制点 tck splrep(np.linspace(0, 1, n), waypoints[:, 0], kdegree, s0) # 这只能处理一维所以对每个维度分别插值 return tck def sample_bspline(tck, num_samples200): 对B样条轨迹均匀采样返回位置、速度、加速度 from scipy.interpolate import splev t np.linspace(0, 1, num_samples) pos np.array([splev(t, tck_i, der0) for tck_i in tck]).T # 每列对应一个维度 # 速度、加速度可以基于位置差分计算或者直接调 der 参数 vel np.array([splev(t, tck_i, der1) for tck_i in tck]).T acc np.array([splev(t, tck_i, der2) for tck_i in tck]).T return t, pos, vel, acc上面这个版本我写得有点“半成品”因为它暴露了一个典型问题用splrep做插值时每一步都重新算节点向量而且只支持一维插值需要对每个维度循环插值一次。实际跑起来没问题但结构不够干净。下面我给出一个更直接、更稳的方案推荐方案每个维度分别构造插值样条然后合并成一个统一的轨迹对象。import numpy as np from scipy.interpolate import splrep, splev import matplotlib.pyplot as plt def build_joint_trajectory(waypoints, degree3, num_samples300): waypoints: shape (N, D)D可以是2平面也可以是3空间 返回 t (采样参数), traj_pos, traj_vel, traj_acc waypoints np.asarray(waypoints, dtypefloat) N, D waypoints.shape param np.linspace(0, 1, N) # 归一化参数 tck_per_dim [] for d in range(D): tck splrep(param, waypoints[:, d], kdegree, s0) tck_per_dim.append(tck) t_sampled np.linspace(0, 1, num_samples) pos np.zeros((num_samples, D)) vel np.zeros((num_samples, D)) acc np.zeros((num_samples, D)) for d in range(D): pos[:, d] splev(t_sampled, tck_per_dim[d], der0) vel[:, d] splev(t_sampled, tck_per_dim[d], der1) acc[:, d] splev(t_sampled, tck_per_dim[d], der2) return t_sampled, pos, vel, acc if __name__ __main__: # 一个简单的手写路点序列 waypoints np.array([ [0.0, 0.0], [0.5, 0.2], [1.0, 0.6], [1.5, 0.3], [2.0, 0.0], ]) t, pos, vel, acc build_joint_trajectory(waypoints) plt.figure(figsize(10, 4)) plt.subplot(1, 2, 1) plt.plot(waypoints[:, 0], waypoints[:, 1], ro--, labelwaypoints) plt.plot(pos[:, 0], pos[:, 1], b-, labelB-spline trajectory) plt.legend() plt.title(Trajectory) plt.subplot(1, 2, 2) plt.plot(t, np.linalg.norm(vel, axis1), labelspeed) plt.plot(t, np.linalg.norm(acc, axis1), labelacceleration) plt.legend() plt.title(Velocity and Acceleration) plt.show()这段代码你直接保存成bspline_traj.py跑一下就能看到左侧是红点路点被蓝色B样条曲线平滑穿过右侧是速度曲线和加速度曲线。如果加速度曲线出现明显抖动别急着改代码先回头数数路点数量是不是太少或太多。3.3 参数怎么调才不容易翻车路点数量、阶数、采样密度三个参数里面路点数量对轨迹形状影响最大。经验法则路点数量在5到20之间三阶B样条表现最稳。如果路点少于4个三阶样条的插值效果会非常古怪如果多于50个曲线会拼命贴合每个路点中间容易挤出高频波纹。thực用的时候如果路点很密我通常不会直接拿全部路点去插值而是先做一次抽稀保留转弯点删掉近直线的中点把点数压到十几二十个再用B样条插值。这样省计算量曲线也更干净。采样密度num_samples别设太低300到500比较合理否则速度、加速度曲线看起来会“棱角分明”影响你对平滑度的判断。3.4 平滑性怎么验证不要只用肉眼盯着位置曲线看那不够。我每次都会看速度曲线和加速度曲线两个东西速度曲线应该是连续的不能有跳变。加速度曲线应该没有尖刺哪怕是1到2个采样周期的毛刺都要重视。如果加速度曲线出现小幅锯齿优先检查是不是采样点太少导致的数值误差把num_samples调到1000再试一次。如果锯齿问题还在那基本可以确定是路点间距不均匀造成的抖动这时候就要动手给路点做重采样或者改用弦长参数化代替均匀参数化。所谓弦长参数化简单说就是按相邻路点的距离比例去分配参数t的增量而不是等间距分配它能让曲线在几何上更贴合路点的疏密变化。这个细节普通教程很少提但对机械臂轨迹质量影响很大。我在下面给出弦长参数化的替换片段def chord_length_param(waypoints): diff np.diff(waypoints, axis0) seg_len np.linalg.norm(diff, axis1) cumulative np.concatenate(([0], np.cumsum(seg_len))) return cumulative / cumulative[-1]把param替换成chord_length_param(waypoints)的结果即可。改完之后你会发现路点疏密不均时加速度曲线的尖刺现象明显缓解。4. 把避障加进去从离散路点到安全平滑轨迹的完整流程4.1 先弄出离散路点采样、碰撞检测、再优化避障部分我不打算用复杂的RRT或者PRM串讲先给你一个最简可用的思路在机械臂的工作空间里撒点把落在障碍物内部的点剔除掉然后从剩下的点中挑出一条从起点到终点的通路。这本质上是一个“在离散图上搜索”的问题。真正工程里可以用A*或者RRT但在Demo阶段你完全可以直接给定一串已经避开障碍物的人工路点把注意力集中在“如何判定B样条曲线路过障碍物”这个问题上。不过为了让流程完整我还是给一版基于A的简化采样思路把2D工作空间栅格化每个格子标记为“自由”或“占用”然后用A搜索出一条栅格路径再从栅格路径中每隔一定距离抽一个路点作为B样条的输入。这比人工路点自动化得多后续你想扩展也很方便。4.2 避障判定用B样条的凸包性做保守检查避障检查不管在位置层面还是关节层面做核心都是对“曲线上的点距离障碍物是否太近”的判断。但如果你把曲线采样成几千个离散点来做碰撞检测算力开销不小。这里B样条凸包性就派上用场了。按照凸包性曲线始终落在控制点组成的凸包内。这意味着如果全部控制点的凸包与某个障碍物不相交那么整条曲线一定不会碰到该障碍物。反过来如果要判断“是否可能碰”先检查凸包简单快速。如果凸包可能碰到再对曲线逐点细查。这种分级检查的策略能省掉大量无效计算。在简单圆形障碍物场景里碰撞检测条件是曲线上任意一点到圆心距离小于安全半径就算是碰了。对每个采样点做距离检查就行计算量也不大。4.3 完整可复现Demo有圆形障碍物环境下面这段代码是在前面B样条模块基础上加了一个圆形障碍物检查并在发现危险后把离障碍物最近的路点沿径向推远一段距离重新生成样条轨迹。import numpy as np from scipy.interpolate import splrep, splev import matplotlib.pyplot as plt def generate_path_from_astar(start, goal, obstacles, grid_size10): # 简化说明此处直接构造一条“L形”路径绕开障碍物 # 实际项目可用A*替换 path np.array([ start, [start[0], (start[1] goal[1]) / 2], [goal[0], (start[1] goal[1]) / 2], goal, ]) return path def check_collision(control_points, obstacles, sample_count100): 检查B样条曲线是否与圆形障碍物碰撞 obstacles: list of (cx, cy, radius) t np.linspace(0, 1, sample_count) D control_points.shape[1] tck_list [splrep(np.linspace(0, 1, len(control_points)), control_points[:, d], k3) for d in range(D)] curve np.array([splev(t, tck, der0) for tck in tck_list]).T for cx, cy, r in obstacles: dist np.linalg.norm(curve - np.array([cx, cy]), axis1) if np.any(dist r): return True, curve return False, curve def push_away_from_obstacle(control_points, obstacles, push_dist0.2): cp control_points.copy() for i, point in enumerate(cp): nearest_obstacle None min_dist float(inf) for cx, cy, r in obstacles: d np.linalg.norm(point - np.array([cx, cy])) if d min_dist: min_dist d nearest_obstacle (cx, cy, r) if nearest_obstacle and min_dist nearest_obstacle[2] 0.1: direction point - np.array([nearest_obstacle[0], nearest_obstacle[1]]) direction direction / (np.linalg.norm(direction) 1e-6) cp[i] point direction * push_dist return cp if __name__ __main__: start np.array([0.0, 0.0]) goal np.array([3.0, 3.0]) obstacles [(1.5, 1.5, 0.5)] path generate_path_from_astar(start, goal, obstacles) # 为了用三阶B样条至少需要4个点; 这里插值加密一下 param np.linspace(0, 1, len(path)) dense_param np.linspace(0, 1, 8) dense_path np.zeros((8, 2)) for d in range(2): tck splrep(param, path[:, d], k1) dense_path[:, d] splev(dense_param, tck) # 插值得到B样条控制点 control_points dense_path.copy() # 按s0插值时控制点就等于路点 collided, curve check_collision(control_points, obstacles) if collided: control_points push_away_from_obstacle(control_points, obstacles) collided, curve check_collision(control_points, obstacles) # 绘制结果 plt.plot(obstacles[0][0], obstacles[0][1], ko, markersize15) circle plt.Circle((obstacles[0][0], obstacles[0][1]), obstacles[0][2], colorgray, alpha0.5) plt.gca().add_patch(circle) plt.plot(dense_path[:, 0], dense_path[:, 1], r--, labelraw path) if not collided: plt.plot(curve[:, 0], curve[:, 1], b-, labelsafe bspline path) plt.legend() plt.axis(equal) plt.show()这个Demo里的push_away_from_obstacle是很粗糙的“事后补救”策略——先检测碰撞再把危险控制点推开。真正严格的做法是直接在优化循环里把碰撞距离作为约束条件求一条满足避障约束的光滑曲线。但作为快速验证手段这段代码足够让你直观看到“路径→B样条光顺→碰撞检查→微调”的完整链路。可以通过构造几个不同位置的障碍物来测试你会发现控制点推开后曲线并没有生硬地“弹开”而是平滑地绕过去。这正是B样条局部支撑性带来的好处你只动的那几个控制点影响范围仅限障碍物附近的那段曲线其他的曲线段几乎不受影响。4.4 时间参数化和关节限位检查几何轨迹做完后还有一个关键步骤把参数t映射到实际时间。前面对B样条的采样用的是归一化参数0到1不代表真实时间。最简单的做法是假设机械手每个关节按梯形速度曲线运动这时候给每个关节设置一个期望最大速度按最慢的那个关节决定整段轨迹的总时间。进一步的加速度约束则用S形速度曲线或者直接做时间最优规划。我在代码里给出的速度、加速度其实只是对几何参数t求导的结果单位是“单位参数的变化率”还不是带物理意义的mm/s。所以你真要驱动机械手还需要乘上一个时间缩放系数。时间缩放的公式很简单如果希望轨迹总时长为T秒那么真实速度为v_real v_param * (1/T)。因为dt T * d_tau几何轨迹对参数求导后再除以T就是真实速度加速度则要除以T^2。换言之总时间拉长速度线性降低加速度平方级降低。在设计机械手轨迹时先大致算一下不缩短时间加速度是否超限心里有底再上机。另外如果轨迹是在笛卡尔空间做的最终下发到关节之前每个采样点都要做逆运动学解算。逆解结果需要检查两个问题是否接近奇异位形雅可比矩阵条件数过大是否超出关节限位这两个检查是机械手轨迹规划里“最后一公里”的命门。B样条在笛卡尔空间里看着顺滑一旦映射到关节空间路径可能变得歪歪扭扭关节速度曲线可能莫名其妙出现尖峰。这也是为什么很多实际项目干脆直接在关节空间做规划——保证关节轨迹平滑避障则放在路径规划阶段考虑。5. 实测中的坑以及排查速查表5.1 常见问题与解决对照表这些坑都是我在实际调试中踩过的有的还在实验台上蹲过半天才找到原因。整理成了一张速查表方便你对着排查。现象可能原因排查手段与对策曲线起点/终点没有落在路点上节点向量没有使用Clamped检查节点向量首尾是否各重复 p1 次曲线明显震荡加速度曲线抖动厉害路点过多且间距不均抽稀路点或改用弦长参数化速度曲线在个别点出现尖刺B样条阶数太高如p5且控制点密集降到p3并检查是否有重复路点检查碰撞时偶尔漏检采样点数不足将采样数增加到500以上或用二分法细分检查避障后曲线扭曲过大控制点被过度推开将微调步长缩小迭代多次代替一次推远曲线与原始路点偏差太大使用了s0的平滑样条拟合控制s0做严格插值或改用最小二乘拟合时减小平滑因子笛卡尔空间轨迹映射到关节后不平滑机械手接近奇异位形在该区域增加避奇异约束或直接在关节空间重新规划5.2 处理奇异点的一个有效技巧奇异问题是最让我头疼的因为它不是轨迹规划算法本身的问题而是机械结构带来的病态。我个人的习惯是如果任务轨迹要经过奇异位形附近先在笛卡尔空间做好路径然后转化成关节空间轨迹看看速度曲线——如果某个关节速度比其他的高出一个数量级基本就是接近奇异了。这时候我会手动加一个中转点把轨迹“拽”离奇异区域再重新跑B样条插值。用前面push_away的逻辑改一下方向就行。虽然不够自动化但在大多数中低速场景下足够用了。5.3 性能优化与采样密度设置的参考如果轨迹在线生成计算耗时是要注意的。我实测过一组数据100个路点插值三阶B样条并采样500个点整个过程大概耗时几十毫秒完全够实时运行。如果你的控制器性能比较弱可以把路点数量控制在20以内、采样数量控制在300以内计算时间会明显下降。碰撞检测如果障碍物数量多优先用控制点凸包粗过滤再对曲线做细检查。实际操作中还有个容易被忽略的细节定期对轨迹做积分校验。很多人做完轨迹直接下发给机械手结果发现末端执行器总的位移偏了这是因为在插值中靠差分求的位移与理论值有微小误差误差累计起来会导致末端对不准。校验方法是把样条位置曲线相邻采样点之间的距离累加起来对比路点总路径长度误差超过1%就要检查是不是参数化出了问题。最后再分享一个我自己的习惯每次调完轨迹我都会顺手把速度曲线、加速度曲线的最大最小值打印出来和机械手手册上标明的额定值对比一遍。这一步虽然不起眼却能提前拦住大多数因轨迹过猛烧电机的操作事故。轨迹规划这事儿与其靠感觉拍脑袋不如直接把曲线数值摆到桌面上拿数据说话。
阅读完成 · 觉得有帮助？
