双差速轮AGV的运动学正解和逆解

青瓜先生

已于 2024-10-26 16:25:16 修改

阅读量1.3k

点赞数 1

分类专栏：机器人规划控制算法案例文章标签：机器人运动学正解运动学逆解运动控制差速轮

于 2024-10-26 12:35:36 首次发布

本文链接：https://blog.csdn.net/xiaoc100200/article/details/143251559

版权

机器人规划控制算法案例专栏收录该内容

11 篇文章 ¥79.90 ¥99.00

订阅专栏

超级会员免费看

简介：

双差速轮AGV（Automated Guided Vehicle）是一种常见的移动机器人结构，采用两侧独立驱动的车轮，通过控制左右轮的转速差实现运动和转向。理解其运动学正解（由轮速求车辆运动）和逆解（由期望运动求轮速）对于AGV的设计和控制至关重要。

运动学模型建立：

定义变量

𝑣：AGV的线速度（质心速度），单位：米/秒（m/s）
𝜔：AGV的角速度（绕质心的旋转速度），单位：弧度/秒（rad/s）
𝑣L、𝑣R ：左、右轮的线速度，单位：米/秒（m/s）
𝜔L、𝜔R：左、右轮的角速度，单位：弧度/秒（rad/s）
r：车轮半径，单位：米（m）
𝐿：左右车轮之间的轴距（轮间距），单位：米（m）
(x,y,θ)：AGV在全局坐标系中的位置和朝向

运动学正解（Forward Kinematics）：

已知左右轮的角速度，求AGV的线速度、角速度和位姿变化。

步骤：

1、计算左右轮的线速度：
2、计算AGV的线速度和角速度：
3、更新AGV的位姿（在小时间间隔 Δt 内）：

示例：

假设：

车轮半径 r=0.1 m
轴距 L=0.5 m
左轮角速度 𝜔L=10 rad/s
右轮角速度 𝜔R=15 rad/s
初始位姿 (x,y,θ)=(0,0,0)
时间间隔 Δt=1s

计算：

1、左右轮线速度：
2、AGV的线速度和角速度：
3、更新位姿：

运动学逆解（Inverse Kinematics）：

已知AGV的线速度和角速度，求左右轮的线速度、角速度。

步骤：

1、计算左右轮的线速度：

在这里插入图片描述

2、计算左右轮的角速度：

在这里插入图片描述

示例：

假设：

期望AGV线速度 v=1 m/s
期望AGV角速度 ω=0.5 rad/s
车轮半径 r=0.1 m
轴距 L=0.5 m

计算：

1、左右轮线速度：
2、左右轮角速度：

注意事项：

速度限制：确保计算得到的轮速不超过电机和驱动系统的最大能力。
轮滑和打滑：理论模型假设轮子与地面无滑动，实际应用中应考虑轮滑对控制的影响。
控制周期：在实际控制中，选择合适的控制周期 Δt 以平衡控制精度和计算负荷。
单位一致性：确保所有计算中的单位一致，避免因单位不一致导致的计算错误。

实现代码

以下是双差速轮AGV运动学正解和逆解的C++实现代码。代码包括了计算AGV的线速度和角速度（正解），以及根据期望的线速度和角速度计算左右轮的角速度（逆解）。

#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;

    // 计算AGV的线速度和角速度
    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;

    // 确保theta在[-pi, pi]范围内
    if (state.theta > PI) {
        state.theta -= 2 * PI;
    } else if (state.theta < -PI) {
        state.theta += 2 * PI;
    }
}

int main() {
    // 初始化AGV参数
    AGVParameters params;
    params.wheel_radius = 0.1;   // 车轮半径为0.1米
    params.axle_length = 0.5;    // 轴距为0.5米

    // 初始化AGV状态
    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);

        // 更新AGV状态
        updateAGVState(state, current_vel, delta_time);

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

    return 0;
}