第四节：差速驱动运动学模型

好，咱们今天来聊聊差速驱动最核心的东西——运动学模型。

说白了，就是搞清楚一个问题：我给了左右轮不同的速度，机器人到底会怎么走？

我在项目里见过不少新手，调了半天PID，机器人还是走不直。为什么？其实就是没搞懂运动学模型。你想想看，如果连输入和输出之间的关系都没理清，那调参不就是瞎蒙吗？

4.1 双轮差速模型的基本假设

在推导之前，咱们得先明确几个前提。嗯，这里要注意，任何模型都有假设条件，脱离了这些假设，公式就不准了。

• 刚体假设：机器人车身是刚性的，不会变形。这个一般都能满足。
• 纯滚动假设：轮子与地面之间只有滚动，没有滑动。说白了就是不打滑。
• 轮距恒定：左右两个驱动轮之间的距离是固定的，记作 \( L \)。
• 轮径一致：左右轮半径相同，记作 \( r \)。

我在实际调试中遇到过一个问题：轮子磨损后直径变了，结果运动学模型算出来的位置和实际位置越差越大。所以，定期校准轮径参数，是个好习惯。

4.2 正运动学：从轮速到位姿

正运动学，就是已知左右轮转速 \( \omega_L \) 和 \( \omega_R \)，求机器人的线速度 \( v \) 和角速度 \( \omega \)。

先看线速度。机器人中心点的速度，其实就是左右轮速度的平均值：

v = (v_R + v_L) / 2

其中，\( v_R = \omega_R \cdot r \)，\( v_L = \omega_L \cdot r \)。

再看角速度。这个稍微绕一点。左右轮速度不一样，机器人就会转弯。角速度等于速度差除以轮距：

ω = (v_R - v_L) / L

合在一起，就是经典的差速驱动运动学方程：

v = r * (ω_R + ω_L) / 2
ω = r * (ω_R - ω_L) / L

这个公式，我建议你刻在脑子里。每次调PID之前，先拿这个算一算，看看你的控制量是不是合理。

4.3 逆运动学：从目标速度到轮速

逆运动学更常用。比如，你想让机器人以 \( v = 0.5 \, m/s \) 的速度直线前进，同时以 \( \omega = 0.2 \, rad/s \) 的速度转弯。那么左右轮应该给多少转速？

把上面的公式反过来就行：

ω_R = (2 * v + ω * L) / (2 * r)
ω_L = (2 * v - ω * L) / (2 * r)

举个例子。假设轮距 \( L = 0.4 \, m \)，轮径 \( r = 0.05 \, m \)。你想让机器人原地旋转（\( v = 0 \)，\( \omega = 1 \, rad/s \)）：

ω_R = (0 + 1 * 0.4) / (2 * 0.05) = 4 rad/s
ω_L = (0 - 1 * 0.4) / (2 * 0.05) = -4 rad/s

你看，左右轮速度大小相等、方向相反，机器人就会原地打转。这个在调PID的时候特别有用——你可以单独测试角速度环。

4.4 位姿更新：积分与累积误差

有了线速度和角速度，怎么得到机器人的位置和朝向？

答案是积分。假设机器人在 \( (x, y) \) 位置，朝向角为 \( \theta \)。经过一个极短的时间 \( \Delta t \)：

x_new = x + v * cos(θ) * Δt
y_new = y + v * sin(θ) * Δt
θ_new = θ + ω * Δt

这就是航迹推演（Dead Reckoning）的基本公式。

但是，这里有个大坑——累积误差。每次积分都会引入一点点误差，时间长了，位置估计就会漂移。我曾经做过一个项目，纯靠航迹推演跑了10分钟，结果位置误差超过了半米。

4.5 知识体系总览

为了让你看得更清楚，我画了一张图，把整个运动学模型的核心逻辑串起来：

这张图把整个流程串起来了。左边是逆运动学（控制时用），右边是正运动学（估计时用）。两者互为逆过程。

4.6 代码实现示例

最后，给你一个简单的Python实现。我在项目里就是这么写的：

class DifferentialDrive:
    def __init__(self, wheel_radius, wheel_base):
        self.r = wheel_radius      # 轮径
        self.L = wheel_base        # 轮距
        self.x = 0.0
        self.y = 0.0
        self.theta = 0.0
    
    def forward_kinematics(self, omega_L, omega_R):
        """正运动学：轮速 → 线速度、角速度"""
        v = self.r * (omega_R + omega_L) / 2.0
        omega = self.r * (omega_R - omega_L) / self.L
        return v, omega
    
    def inverse_kinematics(self, v, omega):
        """逆运动学：目标速度 → 轮速"""
        omega_R = (2.0 * v + omega * self.L) / (2.0 * self.r)
        omega_L = (2.0 * v - omega * self.L) / (2.0 * self.r)
        return omega_L, omega_R
    
    def update_pose(self, omega_L, omega_R, dt):
        """位姿更新"""
        v, omega = self.forward_kinematics(omega_L, omega_R)
        self.x += v * np.cos(self.theta) * dt
        self.y += v * np.sin(self.theta) * dt
        self.theta += omega * dt
        return self.x, self.y, self.theta

这段代码，你直接拿去用都没问题。唯一要注意的是，dt要足够小，否则积分误差会很大。我一般控制在10-20毫秒以内。