机器人的路径跟踪：一份学习路线图
能把一份航点清单交给机器人，让它把这些航点跑完，并且能以米为单位说出它跑得有多好。
规划和跟踪是两个不同的问题，而大多数机器人材料只讲第一个。A* 给你一条路线；而其中没有任何东西会告诉轮子该做什么。这条路线补的就是那一级台阶——一份航点清单和一台真的把它们走出来的机器人之间的所有东西。
按顺序走完这些节点。每一个都是下一个节点所假定的东西，而跳过其中任何一个，都会造出一台漫无目的地乱走、且原因无法隔离的机器人。
开始之前
你需要一台已经能指挥的差速驱动机器人——两个电机、一个驱动器，以及一个以稳定速率运行的回路。电机控制路线涵盖了全部这些内容，而本路线假定你已完成它。不需要控制理论；纯跟踪是几何问题，不是 PID 问题。
你将会做到什么
- 把轮子编码器计数转换成位姿，并说出在你自己家地板上每行驶一米它会漂多远。
- 把陀螺仪与一个绝对参考融合成一个不会在一次任务中逐渐跑掉的航向。
- 用曲率公式在十几行里实现纯跟踪，并解释其中每一行。
- 依据你实测的控制滞后、而不是靠试错来选择前视距离，并让它随速度调度。
- 用真值测量横向误差，报告均方根值、最坏情况，以及误差集中在路径的哪一段。
- 说明为什么同一个控制器在原始 GPS 定位上会差十四倍，并在不改动控制器的前提下修好它。
大家卡在哪里
**估计有问题的时候去调控制器。**如果机器人以为自己就在路径上，它会报告零误差， 然后自信地一头扎进树篱里。在动任何一个增益之前，先对着外部的东西测量——卷尺就算。 **相信一个没有滞后的仿真。**一个完美的运动学纯跟踪会随着前视距离缩小而变好， 所以一个无滞后的模型推荐的恰恰是那个在真实硬件上会振荡的设置。在任何你据以 整定的模型里，都加上转向时间常数和感知延迟。 **做全局最近点搜索。**这是显而易见的实现方式，而且在任何折返的路径上都会 无声地失败——它会匹配到机器人一分钟前刚走过的那一段。要跟踪弧长，只在它附近 一个窗口内搜索，并且绝不让进度倒退。 **把顶点的归属解决到入边那一段。**过了拐角之后，两条相邻线段会投影到同一个 点、且距离相同。如果留下了错误的那一个，一个按航向和横向误差转向的控制器 看到的这两者都约等于零，然后径直开离赛道。这是整页上最安静的一个 bug。
把它做成GPS 航点车，那个项目严格按这个顺序来做—— 先在室内用编码器，再到户外用融合定位。
你需要什么
| 什么 | 这条路线为什么需要它 | 说明 |
|---|---|---|
| 两个轮子上的编码器 | 其他一切都要消费的位姿估计 | 在这里不是可选项——这条路线讲的就是知道自己在哪里 |
| 陀螺仪或 IMU | 轮子测不出自己打滑穿过的那些旋转 | MPU-6050 就够 |
| 一个稳定的回路 | 估计器和控制器都要对时间积分 | 固定间隔，或者把真实经过的时间传进去 |
| 某个可以对照测量的东西 | 没有真值就无法整定跟踪 | 一条粉笔线加一把卷尺，胜过任何数量的遥测 |
| GPS 模块（只用后半段） | 最后两个节点 | 在室内它根本定不到位 |
为什么按这个顺序
这条路线不太寻常，因为**前五个节点根本不涉及转向。**它们 是在建立位姿估计，因为一个作用在错误位姿上的路径控制器，其失败方式 看起来和整定糟糕的控制器一模一样——机器人一边报告零误差，一边开进树篱里。 先把估计弄对，意味着当你终于去整定一个增益时，你能相信自己测到的东西。
**先编码器，再里程计，再打滑，按这个顺序。**每一个都在纠正前一个的 乐观。编码器告诉你轮子转了；里程计把两个轮子的运动变成一个位姿；打滑则告诉你 那个位姿里有多少是虚构的——具体来说，系统性误差随距离线性增长， 而随机误差只按其平方根增长。
IMU 在控制器之前加入，因为航向是位姿中对路径跟踪器最重要的部分， 也正是轮子最不擅长的部分。一次原地转弯几乎按定义就在刮擦两个轮胎， 所以那个最能改变航向的动作，恰恰是里程计测得最差的动作。互补滤波把 陀螺仪优秀的短期角速度与绝对参考优秀的长期平均融合在一起，得到一个 不会跑掉的航向。
运动曲线排在纯跟踪之前，因为路径跟踪器是连续地指令速度的， 而一个阶跃式的速度指令会造出一台猛冲并打滑的机器人——把你刚花了三个 节点控住的里程计误差又请回来。
**纯跟踪，然后是实验台，然后是横向误差。**控制器只有十几行、几何也很简单； 难的是选前视距离，以及判断结果好不好。实验台的存在是为了让你看着前视 距离的这笔交易实际发生，而横向误差这个节点的存在，是为了让“开得好” 变成一个带均方根值和最坏情况的数字。
**GPS 排在最后，而且只在最后。**它对控制器毫无改变，却对估计改变一切。 把它放在最后，意味着当跟踪精度差了十四倍时，你已经百分之百确定 控制器不是原因。
检查点：如何判断一个阶段已经落地
| 阶段 | 检查内容 | 通过是什么样 |
|---|---|---|
| 编码器 | 沿一条粉笔线用手把机器人推 2 m | 两个轮子的计数都能在 2% 以内换算成 2000 mm |
| 里程计 | 走一个 1 m 的正方形并回到起点 | 报告的位姿在几厘米内闭合，而且你能说出每米的误差 |
| 打滑 | 做一次原地转弯，把陀螺仪航向与里程计航向作比较 | 两者不一致，而且你能说出差多少——那道差就是刮擦量 |
| IMU 融合 | 慢慢把机器人转 360°，然后让它静止两分钟 | 航回到起始值，而且静止时不再漂移 |
| 运动曲线 | 先指令一次阶跃速度变化，再指令一次带曲线的 | 带曲线的那次移动不会让轮胎尖叫；阶跃那次会 |
| 纯跟踪 | 从偏出 0.5 m 的起点跟随一条直线 | 它收敛且不带超调，而且你能用米说出你的前视距离 |
| 前视距离整定 | 刻意把前视距离减半 | 它会振荡——现在你知道失效模式了，也知道无滞后的仿真为什么会掩盖它 |
| 横向误差 | 走一个 8 字，并对照真值记录误差 | 你能报告均方根值和最坏误差，并说出最坏处位于路径的哪一段 |
| GPS | 静止不动记录十分钟定位 | 你能看出控制器原本会去追的那种漂移，以米计 |
这条路线到哪里为止
这条路线是拿一条给定的路径去把它走完。它不产出路径——那是 路径规划与建图，而两者合起来 才是完整的导航栈。
它同样在三件事之前停下。跟踪时顺带避障——对出现在你已经承诺要走 的路径上的东西作出反应——需要在跟踪器下面再跑一个局部规划器， 而这确实比单独的任何一半都更难。阿克曼转向和全向转向会改变几何： 纯跟踪很容易适配汽车式机器人，但这里从曲率到轮速的那一步是差速驱动专用的—— 对于能横移的底盘，请从全向驱动开始，并且注意 全向机器人能严格优于这里两个控制器中的任何一个，因为它不必花掉航向就能修正位置。 而 RTK GPS 把户外定位从米级带到厘米级，是让户外路径跟踪真正精确的那个东西—— 在你得出“户外导航就是不准”这个结论之前，值得知道它存在。
学习路线
路线
按顺序完成各个节点——完成一个就会解锁下一个。你的进度会保存在这台设备上。
已完成 0 / 11
常见问题
常见问题解答
路径规划和路径跟踪有什么区别？
规划产出一条路线；跟踪让车轮把它开出来。A* 交给你一份航点清单，却完全不涉及转向、速度，也不管机器人最后偏到了本想去的位置左边半米时该怎么办。路径跟踪正是缺失的那一半：给定机器人认为自己所在的位置和路径的走向，决定车轮此刻应该做什么。大多数机器人学资料把规划讲得很透彻，对跟踪却几乎只字不提，这就是为什么一台规划正确的机器人常常还是会到处乱走。
纯跟踪的前视距离该怎么选？
要根据你的机器人实测的控制滞后来选，而不是靠反复试错。前视距离必须足够远，让机器人在转向响应所需的时间内在物理上能够到达那里——太短会振荡，太长会切弯。测出从下达转向指令到机器人真正开始转向之间的时间，乘以你的速度，从这个值起步。然后让它随速度调度：同一台机器人在 1 m/s 时需要的前视距离比 0.2 m/s 时更长，因为同样的滞后会走过更长的距离。
为什么我的路径跟踪器在仿真里正常，到了机器人上却会振荡？
几乎可以肯定是因为仿真里没有滞后。纯运动学的纯跟踪模型会随着前视距离缩短而单调变好，所以对着它整定，得到的推荐就是最短的前视距离——而这恰恰是在硬件上会振荡的设置。任何你据以整定的模型，都必须包含转向时间常数和感知延迟，否则它会自信地教给你与真实机器人所需正好相反的东西。
纯跟踪还是 Stanley——该用哪种路径控制器？
差速驱动机器人和平滑路径用纯跟踪；当前轴处的跟踪精度比平顺性更重要时用 Stanley。纯跟踪朝路径前方的一个点转向，这使它宽容且天然平顺，代价是会切弯。Stanley 直接把航向误差和横向误差结合起来，跟踪得更紧，但对有噪声的位姿估计容忍度更低。对于小型室内机器人，纯跟踪几乎总是正确的首选。
为什么我的机器人到了户外用 GPS 时，跟踪效果差了这么多？
因为控制器没问题，位姿估计有问题。消费级 GPS 定位的精度只有几米，更新频率为 1–10 Hz，而车轮里程计精确到厘米级，你读多快它就更新多快。给同一个控制器喂一个每次更新都跳两米的定位，它就会去追这些跳变。解决办法不在控制器里——而在于把 GPS 与里程计和一个航向来源融合，让控制器看到的位姿是平滑的，并让 GPS 去修正缓慢的漂移，而不是驱动快速回路。
为什么我的机器人刚过一个拐角就偏离了路线？
这是顶点平局问题，也是整条路径上最隐蔽的缺陷。刚过拐角时，两条相邻的路径线段会以相同的距离投影到同一个点上，所以一个朴素的最近点搜索可能会匹配到机器人已经走完的那一段。一个按航向误差和横向误差转向的控制器，这时看到的两者都约等于零，却径直开离了路线，而且全程报告跟踪完美。要沿路径跟踪弧长，只在上一个位置附近的窗口内搜索，并且绝不让进度倒退。