数学模型（核心思想）Math Model (Core Concept)

2.1 基本想法2.1 The Basic Idea

我们要做的事情很简单：What we're doing is simple:

1. 每隔很短的时间（5 毫秒），读一次传感器Every short interval (5 milliseconds), read the sensors
2. 算出这段时间里机器人移动了多少Calculate how much the robot moved during that time
3. 把移动量加到之前的位置上Add the movement to the previous position

反复执行这个过程，就能一直知道机器人在哪里。By repeating this process, we always know where the robot is.

2.2 直线情况（简单）2.2 Straight Line Case (Simple)

如果机器人走直线，没有转弯：If the robot moves in a straight line without turning:

       前 (y+)
       ↑
       |  竖轮走了 Δv
       |
  ← ──┼── → (x+)
横轮走了 Δh

• Δx = Δh（横向移动 = 横轮走的距离） (lateral movement = horizontal wheel distance)
• Δy = Δv（纵向移动 = 竖轮走的距离） (longitudinal movement = vertical wheel distance)

这很直觉。但机器人大部分时间不是走直线，而是在转弯。This is intuitive. But most of the time, the robot isn't going straight — it's turning.

2.3 从直线到弧线的过渡2.3 From Straight Lines to Arcs

想象一下：机器人不是走完美直线，而是一边走一边微微右转。Imagine: the robot isn't going perfectly straight, but slightly turning right as it moves.

• 如果转了 0.1 度 —— 几乎是直线，Δv 和 Δh 直接当位移用也差不多If it turned 0.1° — nearly straight, using Δv and Δh directly as displacement is close enough
• 如果转了 5 度 —— 有点弯了，直接用 Δv 当 y 位移就不太准了If it turned 5° — it's curving a bit, using Δv directly as y displacement is less accurate
• 如果转了 90 度 —— 明显是弧线，必须用更精确的方法If it turned 90° — clearly an arc, must use a more precise method

弧线越弯，弧长和位移的差距越大。所以我们需要一个方法，把弧长转换成位移。The more the arc curves, the bigger the difference between arc length and displacement. So we need a method to convert arc length into displacement.

2.4 转弯情况（圆弧模型）2.4 Turning Case (Circular Arc Model)

关键假设：在很短的时间内，机器人的运动轨迹可以看作一段圆弧。Key assumption: over a very short time, the robot's path can be approximated as a circular arc.

为什么？因为两边电机功率不同，机器人会绕一个点画弧线。时间越短，这个假设越准确。Why? Because different motor power on each side makes the robot curve around a point. The shorter the time interval, the more accurate this assumption.

         B (新位置)
        ╱
      ╱  弦长（我们要求的位移）
    ╱
  A (旧位置)
   \
    \  半径 R
     \
      O (圆心)

角度变化 = Δθ（陀螺仪告诉我们的）
弧长 = 定位轮走的距离

推导过程（不需要背，理解思路就好）：Derivation (no need to memorize, just understand the approach):

1. 陀螺仪告诉我们角度变了 ΔθThe gyroscope tells us the angle changed by Δθ
2. 竖轮告诉我们走了弧长 弧长vThe vertical wheel tells us the arc length arc_v
3. 圆弧公式：弧长 = 半径 × 角度，所以 半径 = 弧长v ÷ ΔθArc formula: arc length = radius × angle, so radius = arc_v ÷ Δθ
4. 弦长公式（几何）：弦长 = 2 × 半径 × sin(Δθ/2)Chord formula (geometry): chord = 2 × radius × sin(Δθ/2)
5. 弦长就是实际位移，再分解到 x 和 y 方向The chord is the actual displacement, then decomposed into x and y directions

第 5 步为什么要"分解"？ 因为机器人是斜着走的（一边前进一边转弯），位移不是纯 x 或纯 y 方向，需要用三角函数拆成两个分量。用的角度是这段时间的平均朝向 θ旧 + Δθ/2（起点和终点的中间值）。Why "decompose" in step 5? Because the robot moves diagonally (moving forward while turning), the displacement isn't purely in x or y — we need trig to split it into two components. The angle used is the average heading during the interval: θ_old + Δθ/2 (midpoint between start and end).

为什么是 sin 和 cos 这样搭配？Why this sin/cos pairing? 这其实是"旋转矩阵" —— 把机器人视角的前后左右，转换成场地视角的 x 和 y。车头朝前（y+方向）时，前进 = y 增加，所以竖轮对 y 的贡献用 cos（cos(0)=1）；前进不改变 x，所以竖轮对 x 的贡献用 sin（sin(0)=0）。转了角度之后，cos 和 sin 的值随之变化，自动完成了坐标转换。This is actually a "rotation matrix" — converting the robot's local forward/sideways into field-frame x and y. When heading forward (y+ direction), moving forward = y increases, so the vertical wheel's y contribution uses cos (cos(0)=1); forward movement doesn't change x, so the vertical wheel's x contribution uses sin (sin(0)=0). As the angle changes, cos and sin values change accordingly, automatically handling the coordinate conversion.

2.5 偏移补正2.5 Offset Correction

理想情况下，竖轮应该装在机器人的旋转中心上。但实际上很难做到，轮子往往偏了一点。Ideally, the vertical wheel should be mounted at the robot's center of rotation. But in practice that's hard to achieve — the wheel is usually offset a bit.

偏移会导致：机器人原地转圈时，定位轮也会走一段弧，程序误以为机器人移动了。This offset causes a problem: when the robot spins in place, the tracking wheel travels along an arc, making the program think the robot has moved.

补正方法：在计算半径时，加上定位轮到旋转中心的偏移距离。Correction method: add the tracking wheel's offset distance from the center of rotation when calculating the radius.

这个偏移距离需要量出来（后面会讲怎么测量）。This offset distance needs to be measured (we'll cover how to measure it later).