双轮差速运动模型

双轮差速运动模型

#include <stdio.h>
#include <math.h>

// 定义车轮参数
#define WHEEL_DISTANCE 0.5  // 轮间距
#define WHEEL_RADIUS 0.1    // 轮半径

// 定义小车状态结构体
typedef struct {
    double x;  // x 坐标
    double y;  // y 坐标
    double theta;  // 方向角度，以弧度表示
} RobotState;

// 计算左右轮子的速度
void computeWheelVelocities(double v, double omega, double *vl, double *vr) {
    // 计算左右轮子速度
    double r = WHEEL_DISTANCE / 2.0;
    *vl = v - omega * r;
    *vr = v + omega * r;
}

// 计算小车下一时刻状态
void computeNextState(RobotState *state, double v, double omega, double dt) {
    // 计算左右轮子速度
    double vl, vr;
    computeWheelVelocities(v, omega, &vl, &vr);
    
    // 计算小车下一时刻状态
    double x = state->x;
    double y = state->y;
    double theta = state->theta;
    double vr_dt = vr * dt;
    double vl_dt = vl * dt;
    double v_avg = (vr + vl) / 2.0;
    double omega_avg = (vr - vl) / WHEEL_DISTANCE;
    double delta_theta = omega_avg * dt;
    
    x += v_avg * cos(theta + delta_theta / 2.0) * dt;
    y += v_avg * sin(theta + delta_theta / 2.0) * dt;
    theta += delta_theta;

    // 更新小车状态
    state->x = x;
    state->y = y;
    state->theta = theta;
}

int main() {
    RobotState state = {0.0, 0.0, 0.0};  // 初始状态
    double v = 0.2;  // 线速度
    double omega = M_PI / 4.0;  // 角速度，以弧度表示
    double dt = 0.1;  // 时间间隔，单位秒

    // 模拟小车运动
    for (int i = 0; i < 40; i++) {
        computeNextState(&state, v, omega, dt);
        printf("x=%f, y=%f, theta=%f\n", state.x, state.y, state.theta);
    }

    return 0;
}

这个代码实现了一个简单的运动模型，使用了基本的运动学原理计算小车的下一时刻状态。在 main 函数中，定义了一个初始状态，以及线速度、角速度和时间间隔等参数，然后模拟小车的运动并输出小车的状态。可以通过调整这些参数，模拟不同的运动轨迹。