移動(dòng)機(jī)器人底盤(pán)-四輪差速模型

1. 四輪差速模型原理

四輪差速模型底盤(pán)實(shí)例如下圖所示。對(duì)于底盤(pán)的前輪和后輪來(lái)說(shuō)，其速度是同步的，那么在理想條件下，可以將底盤(pán)運(yùn)動(dòng)視為以ICR為圓心做圓周運(yùn)動(dòng)，對(duì)于四個(gè)輪子來(lái)說(shuō)，圓周運(yùn)動(dòng)的角速度是一致的，圓周運(yùn)動(dòng)圓心ICR始終位于底盤(pán)幾何中心COG的y軸延長(zhǎng)線上，ICR與COG之間的距離$d_c$受約束，約束與圓周運(yùn)動(dòng)的角速度${\omega }_c$有關(guān)，整個(gè)底盤(pán)的速度位于速度瞬心COM處，用$v_c$表示，瞬心速度$v_c$由分量$v_{cx}$和$v_{cy}$合成，設(shè)四個(gè)輪子的速度分別為$v_1$、$v_2$、$v_3$、$v_4$，其均由預(yù)設(shè)目標(biāo)速度$v_{ix}$和側(cè)向滑動(dòng)速度$v_{iy}$合成$（i=1,2,3,4）$，設(shè)左輪和右輪之間的軸距為$c$。

圓周運(yùn)動(dòng)的角速度公式如式1所示。
$${\omega }_c=\frac{v_c}{d_c}\text{\hspace{1em}(1)}$$
其中${\omega }_c$為圓周運(yùn)動(dòng)角速度，線速度為$v_c$，圓周運(yùn)動(dòng)半徑為$d$。設(shè)$d_c$與y軸的夾角為${\alpha }_c$，由$v_c$與ICR-COM的垂直關(guān)系可得$v_{c}cos{\alpha }_c=v_{cx}$以及$v_{c}sin{\alpha }_c=v_{cy}$，那么綜上有式2的約束：
$$\begin{gathered} {\omega }_c=\frac{v_c}{d_c}=\frac{v_{c}cos{\alpha }_c}{d_{c}cos{\alpha }_c}=\frac{v_{cx}}{d_{cy}} \\ {\omega }_c=\frac{v_c}{d_c}=\frac{v_{c}sin{\alpha }_c}{d_{c}sin{\alpha }_c}=\frac{v_{cy}}{d_{cx}}\text{\hspace{1em}(2)} \end{gathered}$$
由旋轉(zhuǎn)剛體的四個(gè)輪子的角速度一致的條件，式2可以泛化為式3：
$$\begin{gathered} {\omega }_c=\frac{v_i}{d_i}=\frac{v_{i}cos{\alpha }_i}{d_{i}cos{\alpha }_i}=\frac{v_{ix}}{d_{iy}} \\ {\omega }_c=\frac{v_i}{d_i}=\frac{v_{i}sin{\alpha }_i}{d_{i}sin{\alpha }_i}=\frac{v_{iy}}{d_{ix}}\text{\hspace{1em}(3)} \end{gathered}$$
由式2和3得式4：
$${\omega }_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)\text{\hspace{1em}(4)}$$
同時(shí)，$d_i$（其中$i=1,2,3,4$）與$d_c$在x軸和y軸上得投影長(zhǎng)度滿(mǎn)足式5：
$$\begin{gathered} d_{1y}=d_{2y}=d_{cy}?\frac{c}{2} \\ {d}_{3y}=d_{4y}=d_{cy}+\frac{c}{2}\text{\hspace{1em}(5)} \end{gathered}$$
四輪差速底盤(pán)設(shè)定左輪、右輪得速度分別為$V_L$和$V_R$，且在前輪和后輪速度嚴(yán)格同步的情況下，可建立如式6的約束：
$$\begin{gathered} V_L=v_{1x}=v_{2x} \\ {V}_R=v_{3x}=v_{4x}\text{\hspace{1em}(6)} \end{gathered}$$
結(jié)合式4、5、6可得式7所示結(jié)論：
$$\begin{gathered} V_L={\omega }_c?(d_{cy}?\frac{c}{2})={\omega }_c{d}_{cy}?{\omega }_c?\frac{c}{2}=v_{cx}?{\omega }_c?\frac{c}{2} \\ {V}_R={\omega }_c?(d_{cy}+\frac{c}{2})={\omega }_c{d}_{cy}+{\omega }_c?\frac{c}{2}=v_{cx}+{\omega }_c?\frac{c}{2}\text{\hspace{1em}(7)} \end{gathered}$$
將式7整理，按照矩陣乘的形式表示就得到了四輪差速底盤(pán)的前向運(yùn)動(dòng)學(xué)關(guān)系，如式8所示：
$$\left[\begin{array}{l}{v}_{cx}\\ {\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]\text{\hspace{1em}(8)}$$
那么逆向運(yùn)動(dòng)學(xué)的公式只需要進(jìn)行簡(jiǎn)單的逆變換即可得到，四輪差速逆向運(yùn)動(dòng)學(xué)模型如式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}_{cx}\\ {\omega }_c\end{array}\right]\text{\hspace{1em}(9)}$$文章來(lái)源地址http://www.zghlxwxcb.cn/news/detail-493515.html

2. 工程實(shí)踐

2.1 Python實(shí)現(xiàn)

import numpy as np

def forward_kinematics(vl, vr, d):
    """
    正向運(yùn)動(dòng)學(xué)模型：根據(jù)輪子速度計(jì)算機(jī)器人的運(yùn)動(dòng)

    參數(shù)：
    vl: 左側(cè)輪子的速度
    vr: 右側(cè)輪子的速度
    d: 輪子間距

    返回：
    x: 機(jī)器人的x坐標(biāo)
    y: 機(jī)器人的y坐標(biāo)
    theta: 機(jī)器人的角度（弧度）
    """
    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):
    """
    逆向運(yùn)動(dòng)學(xué)模型：根據(jù)機(jī)器人的位置和角度計(jì)算輪子的速度

    參數(shù)：
    x: 機(jī)器人的x坐標(biāo)
    y: 機(jī)器人的y坐標(biāo)
    theta: 機(jī)器人的角度（弧度）
    d: 輪子間距

    返回：
    vl: 左側(cè)輪子的速度
    vr: 右側(cè)輪子的速度
    """
    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  # 輪子間距

# 正向運(yùn)動(dòng)學(xué)
x, y, theta = forward_kinematics(vl, vr, d)
print("機(jī)器人的位置：(x={}, y={}), 角度：{}".format(x, y, theta))

# 逆向運(yùn)動(dòng)學(xué)
vl, vr = inverse_kinematics(x, y, theta, d)
print("左前輪速度：{}, 右前輪速度：{}".format(vl, vr))

2.2 C++實(shí)現(xiàn)

#include <iostream>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/assignment.hpp>
#include <boost/numeric/ublas/operation.hpp>
#include <boost/numeric/ublas/io.hpp>

namespace ublas = boost::numeric::ublas;

// 正向運(yùn)動(dòng)學(xué)模型
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;
}

// 逆向運(yùn)動(dòng)學(xué)模型
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;  // 輪子間距

    // 正向運(yùn)動(dòng)學(xué)
    ublas::vector<double> pose = forward_kinematics(vl, vr, d);
    std::cout << "機(jī)器人的位置：(x=" << pose[0] << ", y=" << pose[1] << "), 角度：" << pose[2] << std::endl;

    // 逆向運(yùn)動(dòng)學(xué)
    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;
}

到了這里，關(guān)于移動(dòng)機(jī)器人底盤(pán)-四輪差速模型（四輪獨(dú)立）的文章就介紹完了。如果您還想了解更多內(nèi)容，請(qǐng)?jiān)谟疑辖撬阉鱐OY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！