4、差速驱动AGV运动学：正逆解算与速度控制

差速驱动，说白了就是两个轮子各自独立转动，靠速度差来实现转向。这是AGV里最经典、最基础的结构。我最早接触AGV时，用的就是这种底盘。当时觉得挺简单，后来才发现——嗯，里面的坑还真不少。

今天咱们就把差速驱动的运动学彻底讲透。包括正解、逆解、线速度角速度控制，最后我会给出一套完整的C++代码。你跟着走一遍，基本就能上手了。

4.1 差速驱动模型长什么样？

先看结构。两个驱动轮，左右各一个，中间有个万向轮（或从动轮）撑着。两个轮子同轴，轴距记为L，轮子半径记为r。

控制输入是什么？左右轮转速 ω_L 和 ω_R，单位 rad/s。输出呢？车体的线速度 v 和角速度 ω。

这里有个关键点：差速驱动不能独立控制 v 和 ω 吗？其实可以。但它的控制自由度只有2，所以 v 和 ω 就是完全的状态描述。你想想看，给定 v 和 ω，就能唯一确定左右轮速度。反过来也一样。

我习惯把正运动学叫做“传感器解算”，逆运动学叫做“控制指令分配”。为什么？因为实际项目中，编码器测到的是轮速，你要算车体走了多远，就得用正解。而你要给电机发指令，就得用逆解。

4.2 正运动学解算

正运动学，就是已知左右轮转速 ω_L、ω_R，求车体线速度 v 和角速度 ω。

公式推导其实不复杂。左右轮线速度分别是：

v_L = ω_L * r
v_R = ω_R * r

车体线速度是左右轮速度的平均值：

v = (v_L + v_R) / 2

角速度呢？靠速度差。两个轮子速度不一样，车体就会绕中心旋转。角速度公式：

ω = (v_R - v_L) / L

这里 L 是轮距（两个轮子中心的距离）。

合起来就是：

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

我在项目中遇到过一个问题：编码器分辨率不够，导致 v_L 和 v_R 算出来抖动很大。后来加了低通滤波，才把速度曲线稳住。所以正解算虽然公式简单，但实际工程里滤波和标定才是大头。

4.3 逆运动学解算

逆运动学，就是已知目标线速度 v 和角速度 ω，求左右轮转速 ω_L、ω_R。

从正解公式反推：

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

这个公式在ROS里用得特别多。比如你发一个 cmd_vel 话题，里面包含 v 和 ω，底层驱动就要用逆解算出左右轮目标转速，然后发给电机控制器。

ω_L = clamp(ω_L, -ω_max, ω_max);
ω_R = clamp(ω_R, -ω_max, ω_max);

4.4 线速度与角速度控制

有了正逆解，控制就好办了。典型的控制流程是这样的：

1. 上位机下发目标 v_target 和 ω_target
2. 逆解算得到左右轮目标转速
3. 左右轮各自做PID闭环控制
4. 编码器反馈实际轮速
5. 正解算得到实际 v 和 ω，用于里程计

这里有个细节：线速度和角速度的控制周期可以不一样。我一般把线速度控制周期设在20ms，角速度设在10ms。为什么？因为角速度响应更快，转弯时如果控制周期太长，车体会晃。

你想想看，AGV在狭窄通道里转弯，角速度控制稍微慢一点，就可能撞墙。所以角速度的PID参数也要单独调，不能跟线速度共用一套。

4.5 实例代码实现

下面给出一套完整的C++实现。代码我实际在STM32和ROS上都跑过，可以直接移植。

// differential_drive_kinematics.h
#pragma once

class DifferentialDriveKinematics {
public:
    // 构造函数：传入轮距L和轮半径r
    DifferentialDriveKinematics(double wheel_distance, double wheel_radius)
        : L_(wheel_distance), r_(wheel_radius) {}

    // 正运动学：轮速 → 车体速度
    void forwardKinematics(double omega_L, double omega_R,
                           double& v, double& omega) {
        double v_L = omega_L * r_;
        double v_R = omega_R * r_;
        v = (v_L + v_R) / 2.0;
        omega = (v_R - v_L) / L_;
    }

    // 逆运动学：车体速度 → 轮速
    void inverseKinematics(double v, double omega,
                           double& omega_L, double& omega_R) {
        omega_R = (2.0 * v + omega * L_) / (2.0 * r_);
        omega_L = (2.0 * v - omega * L_) / (2.0 * r_);
    }

    // 带限幅的逆运动学
    void inverseKinematicsClamped(double v, double omega,
                                  double omega_max,
                                  double& omega_L, double& omega_R) {
        inverseKinematics(v, omega, omega_L, omega_R);
        omega_L = clamp(omega_L, -omega_max, omega_max);
        omega_R = clamp(omega_R, -omega_max, omega_max);
    }

private:
    double L_;  // 轮距 (m)
    double r_;  // 轮半径 (m)

    double clamp(double value, double min, double max) {
        if (value < min) return min;
        if (value > max) return max;
        return value;
    }
};

使用示例：

#include "differential_drive_kinematics.h"
#include <iostream>

int main() {
    // 假设轮距0.5m，轮半径0.1m
    DifferentialDriveKinematics kin(0.5, 0.1);

    // 逆解：目标线速度0.5m/s，角速度0.3rad/s
    double omega_L, omega_R;
    kin.inverseKinematicsClamped(0.5, 0.3, 10.0, omega_L, omega_R);
    std::cout << "左轮转速: " << omega_L << " rad/s" << std::endl;
    std::cout << "右轮转速: " << omega_R << " rad/s" << std::endl;

    // 正解：验证一下
    double v, omega;
    kin.forwardKinematics(omega_L, omega_R, v, omega);
    std::cout << "解算回线速度: " << v << " m/s" << std::endl;
    std::cout << "解算回角速度: " << omega << " rad/s" << std::endl;

    return 0;
}

这段代码我建议你直接复制到你的项目里。注意轮距和轮半径一定要标定准确，差1mm都会导致里程计漂移。我曾经吃过这个亏，AGV走直线走了10米，偏了30厘米，最后发现是轮距量错了5mm。

4.6 知识体系总览

下面这张图把差速驱动运动学的核心逻辑串起来了。你看一遍，应该能对整个流程有个全局认识。

这张图把整个流程分成了三块：逆解算（控制指令下发）、电机控制与反馈、正解算（里程计）。闭环反馈从里程计回到输入，形成一个完整的控制回路。你实际做项目时，这个框架可以直接套用。

好了，差速驱动的运动学就讲到这里。正解和逆解是基础，线速度角速度控制是核心，代码实现是落地。你把这几个点吃透了，差速AGV的运动控制基本就稳了。

公众号:蓝海资料掘金营，微信deep3321