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

当差速轮不在车体中心时，运动学和动力学模型需要考虑车轮位置对车辆运动的影响。以下是详细的运动学正解、逆解和动力学模型的介绍。

---

1. 差速轮不在车体中心模型的运动学正解

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

1.1 定义变量

• $(v_L,v_R)$：左轮和右轮的线速度（单位：米/秒）。
• $({\omega }_L,{\omega }_R)$：左轮和右轮的角速度（单位：弧度/秒）。
• $(r)$：车轮半径（单位：米）。
• $(L)$：左右轮之间的轴距（单位：米）。
• $(d)$：车轮到车体中心的距离（单位：米）。
• $(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;   // 轴距（单位：米）
    double offset;        // 车轮到车体中心的距离（单位：米）
};

// 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 米
    params.offset = 0.1;         // 车轮到车体中心的距离为 0.1 米

    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