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 q0a1 0a2 0a3 10(qf - q0) / T³a4 -15(qf - q0) / T⁴a5 6(qf - q0) / T⁵举个具体算例q0 0qf 1.2 radT 2 s代入得 a3 1.5a4 -1.125a5 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, dt0.01, v00.0, vf0.0, a00.0, af0.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], ], dtypefloat) b np.array([q0, v0, a0, qf, vf, af], dtypefloat) 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 # 校验q00, qf1.2, T2 时峰值速度应约 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-Connect0.08 s96%基准 1.00差每次不同RRT-Connect 捷径优化0.21 s96%0.78中等RRT*固定重连半径0.35 s98%0.71中等PRM建图 2.4 s 后查询0.006 s99%0.83好关节空间 A*网格 0.05 rad1.6 s100%0.92完全可重复几个观察值得说。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 常见问题速查表现象最可能的原因快速验证方式处理方式轨迹抖动、有异响速度或加速度不连续画关节速度曲线看有没有台阶做时间参数化上 S 型曲线某个关节突然大幅跳变逆解分支切换画关节角度曲线找垂直段绑定分支或加密过渡点规划时间忽长忽短采样随机性固定随机种子复现设超时上限加重点采样仿真能过真机撞碰撞模型缺件或检测间隔太大检查 URDF 是否含末端工具加膨胀层缩小检测间隔末端路径偏离预期各关节未同步检查各关节到达终点时刻统一归一化时间变量接近某些位姿时报错奇异位形算雅可比最小奇异值加惩罚绕开或降速通过逆解无解或超限位目标点在工作空间外检查目标位姿可达性调整目标点或换构型这张表我贴在工位旁边遇到问题先对一遍能省不少时间。里面每一条都是真实踩过的不是编的。6. 接下来打算怎么走这半个月最大的收获不是学会了某个算法而是把整条链路串起来了。以前我觉得规划就是调用一个函数现在我知道那只是一个环节前后还有建模、碰撞、参数化、平滑性检查一大堆事而且每一件都会影响最终结果。下一阶段我打算做三件事。一是把时间参数化这块做扎实目前还是手写多项式加固定参数想试试带约束的数值优化方式把速度、加速度、jerk 的约束都显式写进去看能不能在保证平滑的前提下再压一点节拍。二是把 PRM 的图复用做起来我们的工位其实是固定的如果能把建图时间摊薄到几十次任务里单次规划时间几乎为零这个收益比优化算法本身大。三是想验证一下在真实噪声条件下规划出的路径对定位误差的敏感度因为真机的重复定位精度做不到仿真那么理想路径贴障碍太近的话实际执行就有风险可能需要给路径加一个安全裕度的后处理。如果你也在啃这块我给一个我自己的建议不要一上来就追求算法先进先把一条最简单的点到点轨迹跑通从头到尾每个环节都亲手算一遍、打印出来看一遍。等你清楚每个数字是怎么来的再去换算法你会发现大部分问题根本不在算法上。我第一次把关节速度曲线打印出来的时候才发现自己前面折腾的那些规划器参数全被一个没做的同步化给毁了。
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 q0a1 0a2 0a3 10(qf - q0) / T³a4 -15(qf - q0) / T⁴a5 6(qf - q0) / T⁵举个具体算例q0 0qf 1.2 radT 2 s代入得 a3 1.5a4 -1.125a5 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, dt0.01, v00.0, vf0.0, a00.0, af0.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], ], dtypefloat) b np.array([q0, v0, a0, qf, vf, af], dtypefloat) 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 # 校验q00, qf1.2, T2 时峰值速度应约 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-Connect0.08 s96%基准 1.00差每次不同RRT-Connect 捷径优化0.21 s96%0.78中等RRT*固定重连半径0.35 s98%0.71中等PRM建图 2.4 s 后查询0.006 s99%0.83好关节空间 A*网格 0.05 rad1.6 s100%0.92完全可重复几个观察值得说。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 常见问题速查表现象最可能的原因快速验证方式处理方式轨迹抖动、有异响速度或加速度不连续画关节速度曲线看有没有台阶做时间参数化上 S 型曲线某个关节突然大幅跳变逆解分支切换画关节角度曲线找垂直段绑定分支或加密过渡点规划时间忽长忽短采样随机性固定随机种子复现设超时上限加重点采样仿真能过真机撞碰撞模型缺件或检测间隔太大检查 URDF 是否含末端工具加膨胀层缩小检测间隔末端路径偏离预期各关节未同步检查各关节到达终点时刻统一归一化时间变量接近某些位姿时报错奇异位形算雅可比最小奇异值加惩罚绕开或降速通过逆解无解或超限位目标点在工作空间外检查目标位姿可达性调整目标点或换构型这张表我贴在工位旁边遇到问题先对一遍能省不少时间。里面每一条都是真实踩过的不是编的。6. 接下来打算怎么走这半个月最大的收获不是学会了某个算法而是把整条链路串起来了。以前我觉得规划就是调用一个函数现在我知道那只是一个环节前后还有建模、碰撞、参数化、平滑性检查一大堆事而且每一件都会影响最终结果。下一阶段我打算做三件事。一是把时间参数化这块做扎实目前还是手写多项式加固定参数想试试带约束的数值优化方式把速度、加速度、jerk 的约束都显式写进去看能不能在保证平滑的前提下再压一点节拍。二是把 PRM 的图复用做起来我们的工位其实是固定的如果能把建图时间摊薄到几十次任务里单次规划时间几乎为零这个收益比优化算法本身大。三是想验证一下在真实噪声条件下规划出的路径对定位误差的敏感度因为真机的重复定位精度做不到仿真那么理想路径贴障碍太近的话实际执行就有风险可能需要给路径加一个安全裕度的后处理。如果你也在啃这块我给一个我自己的建议不要一上来就追求算法先进先把一条最简单的点到点轨迹跑通从头到尾每个环节都亲手算一遍、打印出来看一遍。等你清楚每个数字是怎么来的再去换算法你会发现大部分问题根本不在算法上。我第一次把关节速度曲线打印出来的时候才发现自己前面折腾的那些规划器参数全被一个没做的同步化给毁了。
知识建站
内容驱动
阅读友好版式
SEO 精细化
线索转化