移动机器人底盘-四轮差速模型(四轮独立)

移动机器人底盘-四轮差速模型

在这里插入图片描述

文章目录

  • 移动机器人底盘-四轮差速模型
    • 1. 四轮差速模型原理
    • 2. 工程实践
      • 2.1 Python实现
      • 2.2 C++实现

1. 四轮差速模型原理

四轮差速模型底盘实例如下图所示。对于底盘的前轮和后轮来说,其速度是同步的,那么在理想条件下,可以将底盘运动视为以ICR为圆心做圆周运动,对于四个轮子来说,圆周运动的角速度是一致的,圆周运动圆心ICR始终位于底盘几何中心COG的y轴延长线上,ICR与COG之间的距离 d c d_c dc受约束,约束与圆周运动的角速度 ω c \omega_c ωc有关,整个底盘的速度位于速度瞬心COM处,用 v c v_c vc表示,瞬心速度 v c v_c vc由分量 v c x v_{cx} vcx v c y v_{cy} vcy合成,设四个轮子的速度分别为 v 1 v_1 v1 v 2 v_2 v2 v 3 v_3 v3 v 4 v_4 v4,其均由预设目标速度 v i x v_{ix} vix和侧向滑动速度 v i y v_{iy} viy合成 ( i = 1 , 2 , 3 , 4 ) (i=1,2,3,4) i=1,2,3,4,设左轮和右轮之间的轴距为 c c c
移动机器人底盘-四轮差速模型(四轮独立)_第1张图片
圆周运动的角速度公式如式1所示。
ω c = v c d c (1) ω_c=\frac{v_c}{d_c}\tag{1} ωc=dcvc(1)
其中 ω c \omega_c ωc为圆周运动角速度,线速度为 v c v_c vc,圆周运动半径为 d d d。设 d c d_c dc与y轴的夹角为 α c \alpha_c αc,由 v c v_c vc与ICR-COM的垂直关系可得 v c c o s α c = v c x v_ccos\alpha_c=v_{cx} vccosαc=vcx以及 v c s i n α c = v c y v_csin\alpha_c=v_{cy} vcsinαc=vcy,那么综上有式2的约束:
ω c = v c d c = v c c o s α c d c c o s α c = v c x d c y ω c = v c d c = v c s i n α c d c s i n α c = v c y d c x (2) \omega_c=\frac{v_c}{d_c}=\frac{v_ccos\alpha_c}{d_ccos\alpha_c}=\frac{v_{cx}}{d_{cy}}\\ ω_c=\frac{v_c}{d_c}=\frac{v_csinα_c}{d_csinα_c}=\frac{v_{cy}}{d_{cx}}\tag{2} ωc=dcvc=dccosαcvccosαc=dcyvcxωc=dcvc=dcsinαcvcsinαc=dcxvcy(2)
由旋转刚体的四个轮子的角速度一致的条件,式2可以泛化为式3:
ω c = v i d i = v i c o s α i d i c o s α i = v i x d i y ω c = v i d i = v i s i n α i d i s i n α i = v i y d i x (3) \omega_c=\frac{v_i}{d_i}=\frac{v_icos\alpha_i}{d_icos\alpha_i}=\frac{v_{ix}}{d_{iy}}\\ ω_c=\frac{v_i}{d_i}=\frac{v_isinα_i}{d_isinα_i}=\frac{v_{iy}}{d_{ix}}\tag{3} ωc=divi=dicosαivicosαi=diyvixωc=divi=disinαivisinαi=dixviy(3)
由式2和3得式4:
ω c = v c d c = v c x d c y = c c y d c x = v i x d i y = v i y d i x , ( i = 1 , 2 , 3 , 4 ) (4) ω_c=\frac{v_c}{d_c}=\frac{v_{cx}}{d_{cy}}=\frac{c_{cy}}{d_{cx}}=\frac{v_{ix}}{d_{iy}}=\frac{v_{iy}}{d_{ix}},(i=1,2,3,4)\tag{4} ωc=dcvc=dcyvcx=dcxccy=diyvix=dixviy,(i=1,2,3,4)(4)
同时, d i d_i di(其中 i = 1 , 2 , 3 , 4 i=1,2,3,4 i=1,2,3,4)与 d c d_c dc在x轴和y轴上得投影长度满足式5:
d 1 y = d 2 y = d c y − c 2 d 3 y = d 4 y = d c y + c 2 (5) d_{1y}=d_{2y}=d_{cy}-\frac{c}{2}\\ d_{3y}=d_{4y}=d_{cy}+\frac{c}{2}\tag{5} d1y=d2y=dcy2cd3y=d4y=dcy+2c(5)
四轮差速底盘设定左轮、右轮得速度分别为 V L V_L VL V R V_R VR,且在前轮和后轮速度严格同步的情况下,可建立如式6的约束:
V L = v 1 x = v 2 x V R = v 3 x = v 4 x (6) V_L=v_{1x}=v_{2x}\\ V_R=v_{3x}=v_{4x}\tag{6} VL=v1x=v2xVR=v3x=v4x(6)
结合式4、5、6可得式7所示结论:
V L = ω c ⋅ ( d c y − c 2 ) = ω c d c y − ω c ⋅ c 2 = v c x − ω c ⋅ c 2 V R = ω c ⋅ ( d c y + c 2 ) = ω c d c y + ω c ⋅ c 2 = v c x + ω c ⋅ c 2 (7) V_L=\omega_c\cdot(d_{cy}-\frac{c}{2})=\omega_cd_{cy}-\omega_c\cdot\frac{c}{2}=v_{cx}-\omega_c\cdot\frac{c}{2}\\ V_R=ω_c⋅(d_{cy}+\frac{c}{2})=ω_cd_{cy}+ω_c⋅\frac{c}{2}=v_{cx}+ω_c⋅\frac{c}{2}\tag{7} VL=ωc(dcy2c)=ωcdcyωc2c=vcxωc2cVR=ωc(dcy+2c)=ωcdcy+ωc2c=vcx+ωc2c(7)
将式7整理,按照矩阵乘的形式表示就得到了四轮差速底盘的前向运动学关系,如式8所示:
[ v c x ω c ] = [ 1 2 1 2 − 1 c − 1 c ] [ V L V R ] (8) \left[\begin{array}{l} v_{c x} \\ \omega_{c} \end{array}\right]=\left[\begin{array}{cc} \frac{1}{2} & \frac{1}{2} \\ -\frac{1}{c} & -\frac{1}{c} \end{array}\right]\left[\begin{array}{l} V_{L} \\ V_{R} \end{array}\right]\tag{8} [vcxωc]=[21c121c1][VLVR](8)
那么逆向运动学的公式只需要进行简单的逆变换即可得到,四轮差速逆向运动学模型如式9所示:
[ V L V R ] = [ 1 − c 2 1 c 2 ] [ v c x ω c ] (9) \left[\begin{array}{l} V_{L} \\ V_{R} \end{array}\right]=\left[\begin{array}{cc} 1 & -\frac{c}{2} \\ 1 & \frac{c}{2} \end{array}\right]\left[\begin{array}{l} v_{c x} \\ \omega_{c} \end{array}\right]\tag{9} [VLVR]=[112c2c][vcxωc](9)

2. 工程实践

2.1 Python实现

import numpy as np

def forward_kinematics(vl, vr, d):
    """
    正向运动学模型:根据轮子速度计算机器人的运动

    参数:
    vl: 左侧轮子的速度
    vr: 右侧轮子的速度
    d: 轮子间距

    返回:
    x: 机器人的x坐标
    y: 机器人的y坐标
    theta: 机器人的角度(弧度)
    """
    R = d / 2.0
    omega = (vr - vl) / (2.0 * R)
    v = (vl + vr) / 2.0

    x = 0.0
    y = 0.0
    theta = 0.0

    if abs(omega) < 1e-10:
        x = v * np.cos(theta)
        y = v * np.sin(theta)
    else:
        ICC_x = x - R * np.sin(theta)
        ICC_y = y + R * np.cos(theta)
        x = np.cos(omega) * (x - ICC_x) - np.sin(omega) * (y - ICC_y) + ICC_x
        y = np.sin(omega) * (x - ICC_x) + np.cos(omega) * (y - ICC_y) + ICC_y
        theta = theta + omega

    return x, y, theta


def inverse_kinematics(x, y, theta, d):
    """
    逆向运动学模型:根据机器人的位置和角度计算轮子的速度

    参数:
    x: 机器人的x坐标
    y: 机器人的y坐标
    theta: 机器人的角度(弧度)
    d: 轮子间距

    返回:
    vl: 左侧轮子的速度
    vr: 右侧轮子的速度
    """
    R = d / 2.0

    vl = (2 * x - theta * d) / (2 * R)
    vr = (2 * x + theta * d) / (2 * R)

    return vl, vr


# 示例使用
vl = 2.0  # 左前轮速度
vr = 3.0  # 右前轮速度
vlr = -1.0  # 左后轮速度
vrr = 2.5  # 右后轮速度
d = 0.5  # 轮子间距

# 正向运动学
x, y, theta = forward_kinematics(vl, vr, d)
print("机器人的位置:(x={}, y={}), 角度:{}".format(x, y, theta))

# 逆向运动学
vl, vr = inverse_kinematics(x, y, theta, d)
print("左前轮速度:{}, 右前轮速度:{}".format(vl, vr))

2.2 C++实现

#include 
#include 
#include 
#include 
#include 
#include 

namespace ublas = boost::numeric::ublas;

// 正向运动学模型
ublas::vector<double> forward_kinematics(double vl, double vr, double d) {
    double R = d / 2.0;
    double omega = (vr - vl) / (2.0 * R);
    double v = (vl + vr) / 2.0;

    ublas::vector<double> pose(3);
    pose[0] = 0.0;  // x
    pose[1] = 0.0;  // y
    pose[2] = 0.0;  // theta

    if (std::abs(omega) < 1e-10) {
        pose[0] = v * std::cos(pose[2]);
        pose[1] = v * std::sin(pose[2]);
    } else {
        double ICC_x = pose[0] - R * std::sin(pose[2]);
        double ICC_y = pose[1] + R * std::cos(pose[2]);
        pose[0] = std::cos(omega) * (pose[0] - ICC_x) - std::sin(omega) * (pose[1] - ICC_y) + ICC_x;
        pose[1] = std::sin(omega) * (pose[0] - ICC_x) + std::cos(omega) * (pose[1] - ICC_y) + ICC_y;
        pose[2] += omega;
    }

    return pose;
}

// 逆向运动学模型
ublas::vector<double> inverse_kinematics(double x, double y, double theta, double d) {
    double R = d / 2.0;

    ublas::vector<double> wheel_velocities(2);
    wheel_velocities[0] = (2 * x - theta * d) / (2 * R);
    wheel_velocities[1] = (2 * x + theta * d) / (2 * R);

    return wheel_velocities;
}

int main() {
    double vl = 2.0;  // 左前轮速度
    double vr = 3.0;  // 右前轮速度
    double vlr = -1.0;  // 左后轮速度
    double vrr = 2.5;  // 右后轮速度
    double d = 0.5;  // 轮子间距

    // 正向运动学
    ublas::vector<double> pose = forward_kinematics(vl, vr, d);
    std::cout << "机器人的位置:(x=" << pose[0] << ", y=" << pose[1] << "), 角度:" << pose[2] << std::endl;

    // 逆向运动学
    ublas::vector<double> wheel_velocities = inverse_kinematics(pose[0], pose[1], pose[2], d);
    std::cout << "左前轮速度:" << wheel_velocities[0] << ", 右前轮速度:" << wheel_velocities[1] << std::endl;

    return 0;
}

你可能感兴趣的:(算法,算法,线性代数,机器人,运动学,人工智能)