差速轮的运动学正解、逆解及动力学模型

差速轮机器人（如 AGV）通过控制左右轮的转速差实现运动和转向。理解其运动学正解（由轮速求车辆运动）和逆解（由期望运动求轮速）对于机器人的设计和控制至关重要。

---

1. 差速轮运动学正解

运动学正解是指根据左右轮的角速度计算机器人整体的线速度和角速度。

1.1 定义变量

• $(v)$：机器人的线速度（单位：米/秒）
• $(\omega )$：机器人的角速度（单位：弧度/秒）
• $(v_L,v_R)$：左轮和右轮的线速度（单位：米/秒）
• $({\omega }_L,{\omega }_R)$：左轮和右轮的角速度（单位：弧度/秒）
• $(r)$：车轮半径（单位：米）
• $(L)$：左右轮之间的轴距（单位：米）
• $((x,y,\theta ))$：机器人在全局坐标系中的位置和朝向

1.2 计算公式

已知左右轮的角速度 $({\omega }_L)$ 和 $({\omega }_R)$，可以计算机器人的线速度和角速度：

• 线速度 $(v)$：
  $$v=\frac{v_L+v_R}{2}=\frac{r({\omega }_L+{\omega }_R)}{2}$$
• 角速度 $(\omega )$：
  $$\omega =\frac{v_R-v_L}{L}=\frac{r({\omega }_R-{\omega }_L)}{L}$$

1.3 位姿更新

根据线速度和角速度，可以更新机器人在全局坐标系中的位置和朝向：

• 位置更新：
  $$x_{\text{new}}=x+v\cos (\theta )\Delta t$$
  $$y_{\text{new}}=y+v\sin (\theta )\Delta t$$
• 朝向更新：
  $${\theta }_{\text{new}}=\theta +\omega \Delta t$$

---

2. 差速轮运动学逆解

运动学逆解是指根据机器人的期望线速度和角速度，计算左右轮的角速度。

2.1 定义变量

• 期望线速度 $(v)$：机器人期望的线速度（单位：米/秒）
• 期望角速度 $(\omega )$：机器人期望的角速度（单位：弧度/秒）

2.2 计算公式

根据期望的线速度和角速度，可以计算左右轮的线速度和角速度：

• 左轮线速度 $(v_L)$：
  $$v_L=v-\frac{L\omega }{2}$$
• 右轮线速度 $(v_R)$：
  $$v_R=v+\frac{L\omega }{2}$$
• 左轮角速度 $({\omega }_L)$：
  $${\omega }_L=\frac{v_L}{r}$$
• 右轮角速度 $({\omega }_R)$：
  $${\omega }_R=\frac{v_R}{r}$$

---

3. 差速轮动力学模型

动力学模型描述了机器人在给定控制输入（如轮速）下的运动状态变化。动力学模型考虑了机器人的质量和惯性，以及轮子与地面之间的摩擦力。

3.1 定义变量

• $(m)$：机器人的质量（单位：千克）
• $(I)$：机器人的转动惯量（单位：千克·米²）
• $(F_{\text{friction}})$：轮子与地面之间的摩擦力（单位：牛顿）

3.2 动力学方程

动力学方程描述了机器人在给定轮速下的加速度和角加速度：

• 线加速度 $(a)$：
  $$a=\frac{F_{\text{total}}}{m}$$
  其中 $(F_{\text{total}})$ 是左右轮的驱动力之和。
• 角加速度 $(\alpha )$：
  $$\alpha =\frac{T_{\text{total}}}{I}$$
  其中 $(T_{\text{total}})$ 是左右轮的驱动力矩之和。

---

4. 实现代码示例

以下是一个简单的 C++ 实现，展示了如何计算运动学正解和逆解：

#include <iostream>
#include <cmath>

const double PI = 3.14159265358979323846;

// AGV 参数
struct AGVParameters {
    double wheel_radius;  // 车轮半径（单位：米）
    double axle_length;   // 轴距（单位：米）
};

// AGV 状态
struct AGVState {
    double x;      // X 位置（单位：米）
    double y;      // Y 位置（单位：米）
    double theta;  // 朝向角（单位：弧度）
};

// 轮速
struct WheelSpeeds {
    double left;   // 左轮角速度（单位：弧度/秒）
    double right;  // 右轮角速度（单位：弧度/秒）
};

// 线速度和角速度
struct Velocity {
    double linear;      // 线速度（单位：米/秒）
    double angular;     // 角速度（单位：弧度/秒）
};

// 运动学正解：由轮速计算 AGV 速度
Velocity forwardKinematics(const WheelSpeeds& wheel_speeds, const AGVParameters& params) {
    double v_left = params.wheel_radius * wheel_speeds.left;
    double v_right = params.wheel_radius * wheel_speeds.right;

    Velocity vel;
    vel.linear = (v_left + v_right) / 2.0;
    vel.angular = (v_right - v_left) / params.axle_length;

    return vel;
}

// 运动学逆解：由 AGV 速度计算轮速
WheelSpeeds inverseKinematics(const Velocity& vel, const AGVParameters& params) {
    double v_left = vel.linear - (params.axle_length / 2.0) * vel.angular;
    double v_right = vel.linear + (params.axle_length / 2.0) * vel.angular;

    WheelSpeeds wheel_speeds;
    wheel_speeds.left = v_left / params.wheel_radius;
    wheel_speeds.right = v_right / params.wheel_radius;

    return wheel_speeds;
}

// 更新 AGV 的位姿
void updateAGVState(AGVState& state, const Velocity& vel, double delta_time) {
    state.x += vel.linear * cos(state.theta) * delta_time;
    state.y += vel.linear * sin(state.theta) * delta_time;
    state.theta += vel.angular * delta_time;

    if (state.theta > PI) {
        state.theta -= 2 * PI;
    } else if (state.theta < -PI) {
        state.theta += 2 * PI;
    }
}

int main() {
    AGVParameters params;
    params.wheel_radius = 0.1;   // 车轮半径为 0.1 米
    params.axle_length = 0.5;    // 轴距为 0.5 米

    AGVState state;
    state.x = 0.0;
    state.y = 0.0;
    state.theta = 0.0;

    double delta_time = 0.1;  // 时间步长为 0.1 秒

    // 示例：运动学逆解
    Velocity desired_vel;
    desired_vel.linear = 1.0;     // 期望线速度 1.0 m/s
    desired_vel.angular = 0.5;    // 期望角速度 0.5 rad/s

    WheelSpeeds wheel_speeds = inverseKinematics(desired_vel, params);

    std::cout << "需要的左轮角速度: " << wheel_speeds.left << " rad/s" << std::endl;
    std::cout << "需要的右轮角速度: " << wheel_speeds.right << " rad/s" << std::endl;

    // 模拟运动一段时间
    for (int i = 0; i < 50; ++i) {
        Velocity current_vel = forwardKinematics(wheel_speeds, params);