企业官网建设流程全解析

2021 年 11 月 11 日，我把这半个月的机械臂路径规划学习做了一次彻底复盘。之所以选这个时间点整理，是因为前一阶段我一直在追算法名词，RRT、RRT*、PRM、CHOMP 抄了一堆，仿真里也跑出过看着挺漂亮的轨迹，可一旦把速度提上去、把障碍物挪近 5 厘米，问题就全冒出来了：轨迹抖、逆解跳变、规划时间忽长忽短、偶尔还会出现末端直接穿过障碍的情况。机械臂路径规划这件事，表面上是“找一条从 A 到 B 的路”，实际做下来会发现它横跨运动学建模、构型空间采样、碰撞检测、时间参数化和轨迹平滑好几层，任何一层算错，最后都表现成“机械臂不听话”。

这篇进展记录写给我自己，也写给正在啃这块内容、手上有台六轴或者七轴臂、能在仿真里跑但总觉得不对劲的人。我不打算把它写成教科书，只讲我这半个月真实验证过的顺序、参数、算例和踩过的坑。看之前你最好已经能读懂 DH 参数表，知道正逆运动学是干什么的，会一点 Python，剩下的部分我会尽量把“为什么这么选”讲透，包括关键参数是怎么算出来的，而不是直接甩一组数给你抄。如果你是完全的新手，也可以先看第 2 节的建模和第 3 节的插值，那两节是后面所有内容的地基。

1. 先把学习地图摊开：这阶段到底要解决什么

我一开始的错误是把它当成一个纯算法问题，觉得只要找到一个足够强的采样规划器，路径自然就出来了。跑了大概三天我就意识到不对，因为一个“规划成功”的结果和一条“能下发给控制器执行”的轨迹，中间隔着一整套工程处理。这一节先把我理解的层次拆开，后面所有内容都挂在这个骨架上。

1.1 从“能动”到“动得对”的三层结构

我给自己划了三层。最上面是任务层：给定末端位姿序列、给定避障要求、给定节拍要求，决定要走直线、走圆弧还是随便走一条自由路径。中间是规划层：在关节空间或者笛卡尔空间里找一条无碰撞的几何路径，这一层输出的是“路径点”，本质上只是一个几何描述，没有时间概念。最下面是执行层：把路径点变成带时间戳的关节角度序列，包括速度规划、加速度限制、jerk 限制，最后才是下发给伺服。

这个分层非常重要，因为它解释了我早期遇到的一类困惑：为什么采样规划器给出的路径看起来没问题，但一执行就抖。原因很简单，采样规划器输出的是几何路径，点与点之间的间距是不均匀的，有的地方两个点靠得极近，有的地方隔得很远。如果把这种不均匀的路径直接按固定时间间隔下发，关节速度就会忽快忽慢，速度不连续，加速度就是无穷大，控制器只能靠自己的滤波器硬扛，表现出来就是抖动和异响。

提示：判断一个规划器输出的是不是几何路径，看它返回的数据结构里有没有时间戳。只有路径点没有时间的，一定还要过一遍时间参数化，这一步不能省。

所以我现在的工作流固定成：建模与运动学 → 几何路径规划 → 碰撞检测校验 → 时间参数化 → 平滑性检查 → 下发或仿真验证。任何一步跳过，后面都会以某种莫名其妙的形式还回来。

1.2 为什么我先啃关节空间而不是直线插补

很多人入门时第一反应是让末端走直线，因为直观。我一开始也是这么干的，在笛卡尔空间里对位置做线性插值，对姿态做四元数球面插值，然后每个插值点求一次逆解。跑了没多远就出问题了：末端路径确实是直的，但关节角度在某些位置突然跳了几十度，因为逆解在奇异位形附近会从一个分支跳到另一个分支。

这就是笛卡尔空间规划最典型的陷阱。末端走直线，在关节空间里往往是一条弯得离谱的曲线，而且这条曲线会撞上关节限位，或者穿过奇异区。工业场景里末端走直线是刚需，比如涂胶、焊接，但这些场景通常在工作空间中心区域做短距离直线运动，远离奇异位形和关节限位，所以问题不突出。一旦你的任务要求大范围运动，或者工作空间边缘作业，笛卡尔直线插补就很容易崩。

我现在的做法是：大范围转移用关节空间规划，末端精细作业段再用笛卡尔直线插补，两段之间用过渡点衔接。关节空间规划的好处是天然避开了逆解多解和奇异性问题，因为每个关节都是独立的一维变量，规划器在六维构型空间里搜索，找到的解直接就是关节角度序列，不需要再求逆解。代价是末端路径形状不可控，可能绕一个大圈。这个取舍在后面第 4 节会具体讲。

1.3 仿真闭环优先于真机试错

这一条是我用一次接近撞机换来的。当时我在真机上试一条新规划的路径，速度设得不高，但轨迹中间有一段关节加速度明显偏大，机械臂在接近某个位姿时突然一顿，末端夹具差点蹭到旁边的工装。事后回看数据，是时间参数化那一步没有做加速度约束，只是简单按固定速度走。

从那以后我的规矩是：任何新参数、新算法，先在仿真里跑够 200 次以上，记录成功率、规划时间、最大关节加速度，指标合格了再上真机，而且真机第一次跑必须降到 20% 速度并手动按住急停。仿真器不需要多高级，能算出正运动学、能做碰撞检测就够了。我用的仿真环境里，机械臂模型和真机是同一套 URDF，碰撞几何也是同一套，这样仿真里不撞，真机上基本也不会撞，误差主要来自标定偏差和柔性变形，留 3 到 5 厘米的安全余量足够。

2. 建模与运动学：所有规划的账本

规划器再聪明，它也只认你给它的模型。模型错了，规划出来的东西在真实机械臂上一定不对，而且这种错误往往很隐蔽，表现成“仿真里好好的，真机上差一点”。这一节讲我在这上面花的时间，以及几个具体的检查手段。

2.1 DH 表填错一个符号，后面全白干

我建的是标准 DH 参数的六轴串联臂，四个参数：连杆长度 a、连杆扭转角 alpha、关节偏距 d、关节角 theta。最容易错的地方不是数值，而是符号和零位定义。比如 alpha 的正负决定了下一个坐标系的 z 轴朝向，一旦反了，正运动学算出来的末端位姿在某些关节角下会镜像翻转，而零位附近可能看起来差不多，你就不容易发现。

我现在的检查流程是三步。第一步，把六个关节分别单独转到 0、+30 度、-30 度，观察末端位置变化方向是否符合直觉，比如绕基座旋转的关节，末端应该绕着竖直轴画圆弧而不是往上下跑。第二步，取三到四个已知位姿（比如机械臂竖直向上、水平前伸），用卷尺量末端实际位置，和正运动学计算结果对比，误差应该在 3 毫米以内，超过就说明 DH 参数有问题。第三步，检查相邻坐标系之间的变换矩阵，最后一列的平移分量应该和机械臂图纸上的连杆长度对得上。

注意：不同厂家的零位定义可能不同，有的是所有关节水平，有的是有一个关节带固定偏置。填 DH 表之前一定要拿到厂家的零位说明，不要凭正运动学结果反推，那是循环论证。

还有一个坑是关节方向和限位。有些臂的关节正方向和你直觉相反，DH 表里 theta 的符号要跟着改。限位也要按实际填写，规划器如果不知道限位，会规划出超过机械行程的解，仿真里能跑，真机上直接报错或者撞限位。

2.2 逆解多解怎么挑：代价函数写法

六轴臂的逆解通常有 8 组解（肩左/肩右、肘上/肘下、腕翻/腕不翻的组合）。解析解能全求出来，数值解一般只给你一组，取决于初值。我在这一块的体会是，不要用“离初值最近”作为唯一标准，那个标准在很多场景下会选出一条需要关节大范围翻转的解。

我用的代价函数是几项加权相加：关节空间距离（当前角度到解的欧氏距离，权重最高）、是否接近关节限位（越接近限位惩罚越大）、是否接近奇异位形、以及是否需要腕部翻转（翻转通常意味着要走很长一段，实际节拍上不划算）。权重怎么定没有理论最优，我是按实际调试出来的：距离项权重 1.0，限位惩罚用 1/(限位余量+0.1) 这种形式，奇异项用雅可比最小奇异值的倒数做一个软惩罚。

具体写法上，我习惯把每个候选解的打分都打印出来，看前两名差多少。如果差得很小，说明这条路径本来就处在解的切换边界附近，这时候要警惕：连续两个路径点的最优解如果来自不同分支，中间就会出现关节跳变。解决办法是在规划时把解的分支编号带上，强制整条路径用同一个分支，或者在切换点插入一段过渡，让关节平滑走过去。

2.3 奇异位形的识别与软规避

奇异位形我吃过一次亏，是腕部奇异。当时末端要走一条接近竖直的直线，路径中段腕部两个关节几乎共线，雅可比矩阵接近奇异，逆解算出来的关节速度突然放大到正常值的十几倍。虽然仿真里只是数值大，但真机上这就是一次报警。

识别方法不复杂，算雅可比矩阵的最小奇异值，或者用条件数。我设的阈值是最小奇异值低于 0.02 就认为进入奇异邻域，规划时对这一段加惩罚，让路径绕开。另一个实用手段是在奇异邻域内降低笛卡尔速度，因为奇异点本身不可怕，可怕的是在奇异点附近还要求较高的末端速度，那关节速度必然爆掉。实际调试里我把接近奇异区的末端速度限制到额定的 30%，抖动和报警都消失了。

还有一类是肩部和肘部的奇异，通常在机械臂完全伸直或者完全折叠的时候出现。这类奇异靠工作空间限制更有效，也就是在任务规划阶段就不要把目标点设在手臂完全伸直的边界上，留 5% 到 10% 的余量。

3. 轨迹插值与时间参数化

这一节是我这半个月花时间最多的地方，也是最容易被低估的地方。很多教程讲完规划算法就结束了，但真正决定机械臂跑起来顺不顺的，是插值和参数化这一段。

3.1 三次和五次多项式，差的到底是什么

三次多项式插值给定起点和终点的位置与速度，四个约束解四个系数，形式简单，计算量小。问题是它的加速度在两端是突变的，也就是说加加速度（jerk）在端点处是无穷大。机械臂经过减速器传到末端，这个突变会激起结构振动，轻则末端抖动，重则影响精度。

五次多项式多给了起点和终点的加速度约束，一共六个约束解六个系数，加速度连续，jerk 是有限值。代价是峰值速度比三次多项式高一些，而且计算稍微复杂。我现在的默认选择是五次，只有在计算资源极度受限或者运动时间很短（小于 0.2 秒）的情况下才退回三次。

系数可以直接解出来。设起点 q0、终点 qf、总时间 T，两端速度加速度都为 0，那么：

• a0 = q0
• a1 = 0
• a2 = 0
• a3 = 10(qf - q0) / T³
• a4 = -15(qf - q0) / T⁴
• a5 = 6(qf - q0) / T⁵

举个具体算例，q0 = 0，qf = 1.2 rad，T = 2 s，代入得 a3 = 1.5，a4 = -1.125，a5 = 0.225。这条曲线的峰值速度出现在 T/2 处，等于 1.875 × (qf - q0) / T = 1.125 rad/s；峰值加速度出现在约 0.211T 和 0.789T 处，约 5.77 × (qf - q0) / T² = 1.73 rad/s²。这两个数很关键，因为你要拿它们去和电机的额定速度、额定加速度对比，超了就说明这条轨迹下不去，要么延长时间，要么换更短的路径。

3.2 梯形曲线与 S 型曲线的参数手算

梯形速度曲线是最常见的时间参数化方式，分加速、匀速、减速三段，加速度是方波，所以 jerk 在三个切换点都是无穷大。工程上大家都用它，因为简单，而且实际系统有滤波器，能吃掉一部分冲击。但如果你追求平滑，就得用 S 型曲线，把加速度也做成梯形，jerk 变成有限值。

手算一遍梯形曲线，用同一组数据：行程 1.2 rad，最大速度 1.0 rad/s，最大加速度 2.0 rad/s²。先判断能不能达到最大速度：加速段时间 t_acc = v_max / a_max = 0.5 s，加速段位移是 0.5 × a × t² = 0.25 rad，加速加减速两段共 0.5 rad，小于总行程 1.2 rad，说明能达到匀速段。匀速段位移 1.2 - 0.5 = 0.7 rad，时间 0.7 s，总时间 0.5 + 0.7 + 0.5 = 1.7 s。

同样数据换成带 jerk 限制的 S 型曲线，设 jerk 上限 10 rad/s³。加速阶段先以 jerk 爬升到 a_max 用时 0.2 s，再保持 a_max 加速到 v_max 又用 0.5 s，整个加速段约 0.7 s，位移约 0.35 rad。两段加减速共 0.7 rad，匀速 0.5 s，总时间约 1.9 s。比梯形曲线慢了大约 12%，换来的是加速度连续、冲击更小。这个 12% 就是平滑性的价格，节拍要求紧的时候要不要付，得看具体工艺。

提示：判断一条轨迹该用哪种参数化，看工艺要求。搬运、码垛这类对末端精度要求不高的，梯形曲线足够；打磨、装配这类要求末端稳定的，老老实实上 S 型，那 12% 的时间换来的稳定性很值。

还有一个容易忽略的细节：多关节同时运动时，每个关节的行程不一样，如果各关节独立做时间参数化，它们会在不同时刻到达终点，末端路径就变形了。标准做法是同步化，也就是取所有关节里需要时间最长的那个作为公共总时间，其他关节按比例拉长时间，或者用统一的归一化时间变量 s ∈ [0,1] 来参数化整条路径。我一开始就是各关节独立算，结果末端画出来的轨迹和预期的差了一大截。

3.3 多段路径的连续性与过点处理

实际任务很少是单段点到点，通常是经过若干个中间点。分段做多项式插值最直接，但段与段之间如果速度不连续，就会在连接点出现“顿挫”。解决办法有三种：一是提高多项式阶数做全局插值，点多了计算量大且容易过冲；二是用样条（比如三次样条或者 B 样条），天然保证二阶连续；三是在连接点做速度匹配，前一段的终点速度等于后一段的起点速度。

我常用的是三次样条，因为它能保证位置、速度、加速度连续，而且可以用自然边界条件（两端加速度为零）快速求解。实现上是一个三对角方程组，用追赶法解，几十个点也是毫秒级。需要注意的是，样条插值会出现过冲，特别是在点间距变化很大的情况下，机械臂可能跑到两个中间点之外去。如果中间点是避障关键点，这个过冲就可能撞上障碍。我的处理办法是在关键点附近加密采样，把点间距控制在行程的 5% 以内，过冲量一般在 1% 以下，可以接受。

3.4 一个可以直接用的五次多项式实现

下面这段是我现在用的单段五次多项式插值，输入起点终点和总时间，输出带时间戳的角度序列。注意它只处理单关节，多关节要循环调用再做同步化。

import numpy as np def quintic_traj(q0, qf, T, dt=0.01, v0=0.0, vf=0.0, a0=0.0, af=0.0): """五次多项式插值，返回 (t, q, v, a) 四个数组""" 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], ], dtype=float) b = np.array([q0, v0, a0, qf, vf, af], dtype=float) c = np.linalg.solve(A, b) # c0..c5 t = np.arange(0, T + dt, dt) q = c[0] + c[1]*t + c[2]*t**2 + c[3]*t**3 + c[4]*t**4 + c[5]*t**5 v = c[1] + 2*c[2]*t + 3*c[3]*t**2 + 4*c[4]*t**3 + 5*c[5]*t**4 a = 2*c[2] + 6*c[3]*t + 12*c[4]*t**2 + 20*c[5]*t**3 return t, q, v, a # 校验：q0=0, qf=1.2, T=2 时峰值速度应约 1.125 rad/s t, q, v, a = quintic_traj(0.0, 1.2, 2.0) print("峰值速度 %.3f rad/s, 峰值加速度 %.3f rad/s^2" % (v.max(), np.abs(a).max()))

这段代码跑出来峰值速度 1.125、峰值加速度 1.732，和手算一致，说明系数没解错。我建议你也做这个校验，因为它能同时验证你的系数求解和物理直觉——如果算出来峰值速度是 5 rad/s，那多半是哪里符号错了。

4. 采样规划与图搜索的对比实测

前面都是准备工作，这一节讲规划器本身。我把几个主流的规划方式都在同一个场景下跑了一遍，场景是一台六轴臂从初始位姿运动到目标位姿，中间有两个圆柱形障碍物，工作空间大约 1 立方米。测的指标是规划时间、成功率、路径长度、最大关节加速度。

4.1 RRT-Connect、RRT*、PRM 各管什么场景

RRT-Connect 是从起点和终点各长一棵树，交替扩展，遇到就接上。它的特点是快，在空旷环境里几乎是一瞬间出解，但不保证路径质量，经常绕远路，而且因为随机性，每次跑出来的路径都不一样。适合什么场景？适合那些只要求“找到一条可行路径”的场合，比如大范围转移、示教点之间的过渡，路径长短无所谓，快就行。

RRT* 是在 RRT 基础上加了重连（rewire），每次新采样点插入后，会检查附近已有节点能不能通过这个新点获得更短的路径，能就改接。理论上采样趋于无穷时路径最优，实际跑下来路径确实比 RRT-Connect 短 15% 到 30%，但规划时间会翻几倍，而且在六维关节空间里，重连半径按公式算出来会非常小。原因是重连半径随维度升高衰减很快，公式大致是 (log n / n)^(1/d)，d = 6 时即使 n 到几千，半径也只有步长的零点几倍，重连几乎失效。

我的应对方式是：不严格按公式算半径，直接用步长的 1.5 到 2 倍作为固定半径，牺牲一点理论最优性换实际效果。实测这样能在规划时间只增加 60% 的情况下把路径长度压下来 20% 左右，性价比比纯公式好。

PRM 是先在构型空间里撒点建一张路网，然后查询时在这个路网上做图搜索。它的优势是可复用，环境不变的情况下，建一次图可以反复查询，每次查询只要几毫秒。适合固定工位、反复执行同类任务的场合。缺点是建图本身耗时长，而且如果障碍物会变，图就废了，得重建。

4.2 步长、目标偏置、rewire 半径的取值逻辑

这三个参数是采样规划器最常调的东西，我总结了一套自己的取值习惯。

步长：决定了每次扩展走多远。太小，树长得慢，规划时间长；太大，容易跳过狭窄通道，而且生成的路径锯齿感强。我的经验值是关节空间归一化之后取 0.15 到 0.25。所谓归一化，就是把每个关节的角度除以它的行程范围，让六个维度都在 [0,1] 之间，这样步长在六个维度上意义一致。如果不归一化，行程大的关节主导了距离度量，规划器会忽略行程小的关节，路径质量很差。

目标偏置：每次采样时，有一定概率直接朝目标点采样，而不是在整个空间里随机撒。这个概率一般取 0.05 到 0.1。取值太大会让树过早地朝目标方向长，遇到障碍时容易困在局部；取值太小则收敛慢。我试过 0.2，在窄通道场景下成功率反而下降了，因为树太贪心，一头撞在障碍上出不来。

rewire 半径：如上所说，别信公式，用步长的 1.5 到 2 倍。另外要注意重连时要做碰撞检测，新边如果撞了就不能接，这条检测是 RRT* 耗时的主要来源。

4.3 碰撞检测这一块的性能瓶颈

跑下来我发现，规划时间的 70% 以上都花在碰撞检测上，而不是树的操作。所以优化碰撞检测的收益比换规划器大得多。

我做的三件事。第一，用包围体做粗筛。每个连杆用几个球体或者胶囊体近似，先做球与球、球与圆柱的快速相交测试，通过了再进精细检测。这一层能把 80% 的采样点直接筛掉。第二，跳过相邻连杆的自碰撞检测。相邻连杆本来就挨着，检测它们纯属浪费，还会误报。第三，控制离散检测的步长。沿着一条新边做碰撞检测时，是按固定间隔取点检测的，间隔太大就会漏检，也就是常说的隧穿——机械臂从一个障碍的一侧瞬间到了另一侧，中间的点没检测到。

隧穿这个问题我专门算过。如果关节最大速度是 2 rad/s，检测间隔是 0.05 rad，那么在两个检测点之间机械臂转过 0.05 rad，末端移动距离取决于臂长。臂长 0.8 米时，末端移动约 4 厘米。如果障碍物厚度小于 4 厘米，就存在漏检风险。所以检测间隔要按“末端最大位移不超过最小障碍尺寸的三分之一”来定，我一般取 0.02 到 0.03 rad，兼顾精度和速度。

4.4 六自由度实测对比数据

同一场景、同一台模型、同样的起终点，各跑 100 次，统计如下。这些数只能作为量级参考，换台臂、换个场景都会变，但相对关系我觉得有普遍性。

几个观察值得说。RRT-Connect 加捷径优化（随机取路径上两点，尝试直连，能连就替换）是个性价比极高的操作，只要 0.13 秒的额外时间就能把路径缩短 22%，我现在的默认配置就是这个。A* 在网格分辨率 0.05 rad 时，六维网格的节点数是天文数字，实际是靠启发式和稀疏化才跑得动，规划时间 1.6 秒太慢，但它的路径完全可重复，这点在某些需要复现的场合很有价值。PRM 的查询时间几乎可以忽略，如果工位固定、任务重复，建一次图能用很久，这是最划算的方案。

5. 踩坑记录与排查速查表

这一节是我这半个月真正花时间的地方，每个坑都对应一段调试。我把现象、原因、解决方式整理出来，希望你能少走点弯路。

5.1 规划成功但执行抖动

最典型的组合是“路径看着没问题，速度一上去就抖”。我遇到的原因有三个。

第一个是路径点间距不均匀，直接按固定时间间隔下发，导致速度跳变。解决办法就是第 3 节说的时间参数化，不要偷懒。

第二个是各关节没同步，有的关节先到、有的后到，末端在最后阶段被“拽”了一下。解决办法是用统一归一化时间变量，或者取最长关节时间做同步。

第三个是逆解分支跳变，相邻两个路径点的解来自不同分支，中间某个关节角度突变。这个在数据上很好识别，把关节角度序列画出来，如果看到某个关节有一小段几乎是垂直的跳变，基本就是它。解决办法是在规划阶段绑定分支，或者在跳变点加密采样、插入过渡。

注意：这三种原因在末端轨迹上看起来都是“抖”，但排查顺序应该是先看关节角度曲线，再看速度曲线，最后看加速度曲线。从原始数据入手，比盯着末端轨迹猜快得多。

5.2 偶发规划超时与随机性

同一个起终点，有时候 0.05 秒出解，有时候跑满 5 秒超时。这是采样规划器的固有特性，但如果超时率超过 5%，就说明参数或者场景有问题。

我排查的顺序：先看随机种子是不是每次都变，如果是，把种子固定下来复现问题；然后看失败的那些次是不是都卡在同一个区域，如果是，说明那里有个窄通道，需要调整采样策略；最后看超时设置，我一般设 3 秒上限，超过就判定失败，退回上一个可行解或者请求人工介入。

窄通道的处理办法有几个：加大目标偏置不一定是好事，前面说过；更有效的是在通道附近做重点采样，也就是把采样分布从均匀改成带高斯扰动的分布，中心放在通道入口；另一个办法是换用双向搜索，RRT-Connect 天生就比单向的 RRT 更容易过窄通道。

5.3 碰撞漏检与隧穿

这个坑最危险，因为仿真里可能看不出来，真机上就是撞。除了前面说的检测间隔，还有一个隐蔽原因是碰撞模型和真实几何不一致。比如你的模型里连杆是个细圆柱，实际上末端还装着夹具、气管、相机，这些东西如果没加进碰撞模型，规划出的路径在真实环境里就会撞。

我的做法是给碰撞模型加一层“膨胀”，所有几何体半径加 2 到 3 厘米，宁可保守。另外末端工具一定要单独建模，别嫌麻烦，我见过太多因为漏了末端工具而撞的案例。

5.4 常见问题速查表

这张表我贴在工位旁边，遇到问题先对一遍，能省不少时间。里面每一条都是真实踩过的，不是编的。

6. 接下来打算怎么走

这半个月最大的收获不是学会了某个算法，而是把整条链路串起来了。以前我觉得规划就是调用一个函数，现在我知道那只是一个环节，前后还有建模、碰撞、参数化、平滑性检查一大堆事，而且每一件都会影响最终结果。

下一阶段我打算做三件事。一是把时间参数化这块做扎实，目前还是手写多项式加固定参数，想试试带约束的数值优化方式，把速度、加速度、jerk 的约束都显式写进去，看能不能在保证平滑的前提下再压一点节拍。二是把 PRM 的图复用做起来，我们的工位其实是固定的，如果能把建图时间摊薄到几十次任务里，单次规划时间几乎为零，这个收益比优化算法本身大。三是想验证一下在真实噪声条件下，规划出的路径对定位误差的敏感度，因为真机的重复定位精度做不到仿真那么理想，路径贴障碍太近的话，实际执行就有风险，可能需要给路径加一个安全裕度的后处理。

如果你也在啃这块，我给一个我自己的建议：不要一上来就追求算法先进，先把一条最简单的点到点轨迹跑通，从头到尾每个环节都亲手算一遍、打印出来看一遍。等你清楚每个数字是怎么来的，再去换算法，你会发现大部分问题根本不在算法上。我第一次把关节速度曲线打印出来的时候，才发现自己前面折腾的那些规划器参数，全被一个没做的同步化给毁了。