迷宫机器人的沿墙行驶与精确转弯

引言

迷宫求解算法假设机器人处在它以为自己在的那个格子里。洪水填充算出一条完美的路线，然后机器人在第三个转弯上蹭了墙，此后每一个决策都建立在错误的格子上。

所以算法很少是失败的那个。失败的是运动：在走廊里保持居中，以及精确地转九十度。本文讲的就是这两件事，以及为什么每件事那个显而易见的做法都是错的那个。

为什么只靠航位推算不行

诱人的做法是纯靠编码器计数来驱动：一格是 N 个计数，四分之一转是 M 个计数，而迷宫是网格，那就数呗。

它失败是因为这些误差会累积而且永不自我纠正。轮子在加速时打滑。两台电机并不相同。你量出来的轴距差一毫米，于是每次转弯都是 88° 而不是 90°。每个误差都很小；没有一个会互相抵消。走出二十格之后，机器人正带着十足自信斜着开向墙。

解法是把迷宫本身当作参考。墙是直的、平行的、间距已知——这是一个免费、抗漂移的测量，而且连续可用。

把沿墙走写成一个 PID 环

两侧各一颗距离传感器之后，居中就变成你在循迹车里已经解决过一次的控制问题：算出误差，把它驱到零。

// Positive error = too close to the left wall = steer right.
float error = leftDistance - rightDistance;

float correction = Kp * error + Kd * (error - previousError);
previousError = error;

int base = 120;
setMotors(base + correction, base - correction);

修正量是差分施加的——加到一个轮子上，从另一个上减掉——于是机器人的前进速度大致保持恒定，而航向发生变化。这和循迹车是同一个结构；只有误差项不同。

先把 Kd 设为零，增大 Kp 直到机器人跟住走廊、同时还有一点轻微摆动，然后加 Kd 直到摆动被压掉。Ki 在这里几乎从不有用：迷宫走廊很短，而积分作用主要是在每个格子边界上增加延迟和超调。

朴素环会搞错的那种情况

迷宫里有开口。侧墙一结束，那颗传感器立刻读到某个很远的东西的距离——一个开格对面的墙，或者干脆什么都没有——于是 leftDistance - rightDistance 变成一个巨大的假误差。机器人会朝它本该开过去的那个缺口猛栽一跟头。

在信任这个差值之前先检查墙是否存在：

const int WALL_THRESHOLD = 100;   // mm — beyond this there is no wall

bool leftWall  = leftDistance  < WALL_THRESHOLD;
bool rightWall = rightDistance < WALL_THRESHOLD;

float error;
if (leftWall && rightWall) {
  error = leftDistance - rightDistance;        // centre between both
} else if (leftWall) {
  error = 2 * (leftDistance - IDEAL_SIDE);     // hold one wall at a set distance
} else if (rightWall) {
  error = 2 * (IDEAL_SIDE - rightDistance);
} else {
  error = 0;                                    // open cell — hold heading instead
}

单侧的情况把增益翻倍，因为一面墙只给你一半信息：两面墙时，向左漂 5 mm 会让差值变化 10 mm；一面墙时只变化 5。

在两面墙都没有的情况下，退回到 error = 0 会让机器人就电机而言直着开——而现实中那不是直。陀螺仪就是在这一步接手的。

精确地转 90 度

用编码器计数走完一次转弯是自然的第一尝试，而它不够准。原地转向比直行打滑更多，而每次转弯 2° 的误差在十次之后就是 20°——一台斜对着走廊的机器人。

改成积分陀螺仪。MPU6050的 Z 轴角率是对机器人转多快的直接测量，而且它不在乎轮子有没有打滑：

float turnDegrees(float target) {
  float heading = 0;
  unsigned long last = micros();

  while (fabs(heading) < fabs(target)) {
    unsigned long now = micros();
    float dt = (now - last) / 1000000.0;
    last = now;

    heading += (readGyroZ() - gyroBias) * dt;   // bias subtraction is essential

    int speed = (target > 0) ? TURN_SPEED : -TURN_SPEED;
    setMotors(speed, -speed);                   // pivot in place
  }
  setMotors(0, 0);
  return heading;                               // report the real angle achieved
}

有三件事决定成败：

**把陀螺偏置减掉。**一颗没标定的陀螺仪静止时会读每秒一两度，而你正在积分它。照倾角教程里的做法，静止时量出平均值，每次读数都减掉它。

**提前停，让它滑行。**机器人有转动动量，所以在正好 90° 时切电机必然过冲。在大约 85° 处停下让它滑进去——更好的做法是随着剩余角度变小把转弯速度降下来。

**对着前墙重新校方。**当一次转弯把一堵墙放到机器人正前方时，那堵墙就是一个完美的 90° 参考。慢慢开上去，让沿墙环把机器人贴正，这会免费地把累积的航向误差清零。做微型迷宫车的人一有机会就这么干，因为它把一台正在漂移的机器人变成一台会自我纠正的机器人。

知道一格什么时候过去了

格子计数是把运动和迷宫算法连起来的那一环。编码器距离能把你大致送到那儿；墙能精确地告诉你。

最可靠的触发方式是侧墙的下降沿。机器人越过格子边界时，一颗原本在读墙的侧传感器会突然读到很远的距离——这个跳变发生在格子里一个已知的点上，而且它是一个物理地标，而不是一个累积出来的估计。

把两者结合起来：用编码器预测边界，然后让墙沿去确认或修正它。两者不一致时，信墙。

在让它去求解之前先验证

四项测试，在真实迷宫上，按这个顺序：

1. **直走廊，十格。**机器人应当保持居中，到达时仍然居中。摆动说明 Kd 太低；偏向一侧说明传感器有偏移——把两颗侧传感器都对同一堵墙量，然后在软件里修正差值。
2. **单侧走廊。**拆掉一堵墙。机器人必须继续直走，而不是朝缺口猛栽。
3. **连续十次转弯。**转九十度十次，检查机器人最后是否面朝它原来的方向。差了几度以上，你的陀螺偏置或滑行补偿就需要再改。
4. **重新校方。**朝一堵墙开，让它贴正，然后确认之后航向误差已经消失。

只有这四项都过了，才值得在上面跑一个迷宫算法——一个拿到坏位置数据的规划器只会产出自信的胡说八道。先在迷宫求解模拟器里单独看规划层，然后把两者合起来放进微型迷宫车项目。