在户外机器人上实现 GPS 航点导航

对机器人来说，度是错的单位

GPS 模块交给你的是以度表示的经纬度，而你手上的每一个控制器都想要米。换算是第一件活，而且比网上说的要容易。

在业余漫游车覆盖的那几百米里，地球足够平坦，等距圆柱近似的误差远小于接收机自身的噪声：

north (m) = (lat − lat₀) × 111 320
east  (m) = (lon − lon₀) × 111 320 × cos(lat₀)

原点 lat₀, lon₀ 一次选定——任务的起点——于是每个航点都变成一对以米为单位的 (east, north)，纯跟踪可以直接朝它转向，完全不需要知道一度是什么。

半正矢公式更正确，而你不需要它。它在几百公里上才要紧；在 500 m 上两种算法的一致程度在几毫米以内，比你的误差低三个数量级。

要记住的那个数：纬度一度是 111.3 km，所以五位小数是 1.1 m，六位是 11 cm。航点存六位。再往上存的就是噪声，而 AVR 上的 float 大约只能承载七个有效数字——够放 19.076123，不够放更多，这正是为什么你要换算成相对于原点的米，而不是直接对原始度数做算术。

一个定位解的三个毛病

**它滞后。**典型的 NEO-6M 以 1 Hz 报告。在 1.5 m/s 下，机器人每两个定位解之间走 1.5 m，而它给出的那个解描述的是解算时刻的位置，不是它现在的位置。

**它嘈杂。**消费级 GPS 在开阔、天空晴朗的条件下，也有一半时间落在离真值约 2.5 m 的范围内。在树下、楼边，或者卫星几何很差时，要糟得多——而且这种故障不是你能检测到的失锁，而是一个看起来很可信的错误答案。

**它没有航向。**一个 GPS 定位是一个位置。它没法告诉你机器人指着哪个方向，因为一台静止指着北的机器人和同一台指着南的机器人会产生完全相同的定位。对地航向是从连续位置推导出来的，只有在真正运动时才有意义——低于大约 1 m/s 时它基本上全是噪声。

这对路径跟踪器意味着什么

把同一个纯跟踪控制器放在同一条赛道上跑两遍，一遍用完美的里程计，一遍用 1 Hz、±1.5 m 且在更新之间保持不变的定位：

注意没有发生什么：机器人从来没有走丢。它大致待在赛道上并到达终点。只用 GPS 的导航是能用的——它只是比数据手册上看起来的差得多，而代价主要落在执行器上。

三个修法，按价值排序

**一——融合，而不是跟随。**以你的循环频率积分轮子里程计和某个航向源，把每一个 GPS 定位用来修正那个估计，而不是替换它。即使一个粗糙的互补混合也能把结果彻底改观：

// Between fixes: dead reckon. This runs at the loop rate.
x += cos(heading) * v * dt;
y += sin(heading) * v * dt;

// On a fix: pull the estimate toward it, gently. alpha near 0.1 trusts odometry
// over the second between fixes, which is exactly the right instinct.
if (newFix && fix.hdop < 2.5) {
  x += ALPHA * (fixX - x);
  y += ALPHA * (fixY - y);
}

这就是一个互补滤波在做它为倾角所做的同一件事：一个会漂移但平滑的信号，由一个嘈杂但有界的信号来修正。

**二——航向从别处拿。**磁力计能在静止时给出绝对航向，而且需要硬磁和软磁标定，还要离你的电机远一点。陀螺仪给出极好的短期航向，但会漂移。这两样一起用是每一台真实漫游车的做法；然后在运动速度足够快、值得信赖时，用 GPS 的对地航向去修正这一对。

**三——先提频率，再提精度。**一颗 10 Hz 的模块修掉了造成大部分摆动的那份陈旧感。它通常比去追厘米级精度更好的第一次升级，而且便宜得多。

读模块

GPS 模块通过串口以 9600 波特率讲 NMEA 0183：逗号分隔的句子，每行一句。你需要其中两种。

• $GPRMC——时间、有效性、纬度、经度、对地速度、对地航向。
• $GPGGA——定位质量、使用的卫星数，以及 HDOP，水平精度因子。

要解析定位，但要按质量设门限。两条规则能挡掉大部分户外灾难：

• **状态为 V（无效）或者定位质量为 0 的定位一律拒绝。**它里面仍然装着看起来很可信的数字。
• **HDOP 大于大约 2.5 的一律拒绝。**HDOP 描述的是卫星的几何，而糟糕的几何会给出一个偏差几十米的自信答案。

用一个解析库，别自己写——但要读足够多这些句子，好知道你在扔掉什么。还有：**冷启动在没有星历时需要 30 秒到几分钟。**不要把一台还没拿到定位的机器人当成一台在原点的机器人。要等，并且给出一个可见的“正在等待”指示。

测试之前先给它围上电子围栏

一台行为异常的户外车会走丢。第一次外场测试之前，在软件里加一条硬限制：离起点的最大距离，以及超过就停。

if (hypotf(x, y) > FENCE_RADIUS) { stop(); return; }   // before anything else

十行代码，而它就是“一个缺陷”和“一台在运河里的机器人”之间的区别。再配一个任务超时和一个你够得着的手动切断——顶甲板上装一个碰撞开关就管用，而且不需要任何无线电链路。

出问题时

在路径跟踪模拟器里用两种位置源跑同一条赛道，然后去做GPS 航点车——在那台车上，模块本身和上面的融合就是整个项目。