小车模型——双差速小车模型

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

双差速轮模型是一种常见的移动机器人配置，通过独立控制左右轮的转速实现运动和转向。以下是该模型的运动学正解、逆解和动力学模型的详细介绍。

---

1. 双差速轮模型的运动学正解

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

1.1 定义变量

• $(v_L,v_R)$：左轮和右轮的线速度（单位：米/秒）。
• $({\omega }_L,{\omega }_R)$：左轮和右轮的角速度（单位：弧度/秒）。
• $(r)$：车轮半径（单位：米）。
• $(L)$：左右轮之间的轴距（单位：米）。
• $(v)$：机器人的线速度（单位：米/秒）。
• $(\omega )$：机器人的角速度（单位：弧度/秒）。

1.2 计算公式

已知左右轮的角速度，可以计算机器人的线速度和角速度：

• 线速度 $(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}$$

---

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{drive}})$：驱动力（单位：牛顿）。
• $(T_{\text{drive}})$：驱动扭矩（单位：牛顿·米）。

3.2 动力学方程

动力学方程描述了机器人的加速度和角加速度：

• 线加速度 $(a)$：
  $$a=\frac{F_{\text{drive}}}{m}$$
• 角加速度 $(\alpha )$：
  $$\alpha =\frac{T_{\text{drive}}}{I}$$

---

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);
        updateAGVState(state, current_vel, delta_time);

        std::cout << "时间: " << (i + 1) * delta_time << " s, "
                  << "位置: (" << state.x << ", " << state.y << "), "
                  << "朝向: " << state.theta << " rad" << std::endl;
    }

    return 0;
}

5. 总结

双差速轮模型通过独立控制左右轮的转速，实现了机器人的运动和转向。运动学正解用于根据轮速计算机器人的运动状态，而逆解用于根据期望运动状态计算所需的轮速。动力学模型进一步描述了机器人在给定驱动力和驱动扭矩下的动态行为。这些模型为机器人的设计和控制提供了理论基础。