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

1. 四轮差速模型原理

四轮差速模型底盘实例如下图所示。对于底盘的前轮和后轮来说，其速度是同步的，那么在理想条件下，可以将底盘运动视为以ICR为圆心做圆周运动，对于四个轮子来说，圆周运动的角速度是一致的，圆周运动圆心ICR始终位于底盘几何中心COG的y轴延长线上，ICR与COG之间的距离$d_c$受约束，约束与圆周运动的角速度${\omega }_c$有关，整个底盘的速度位于速度瞬心COM处，用$v_c$表示，瞬心速度$v_c$由分量$v_{cx}$和$v_{cy}$合成，设四个轮子的速度分别为$v_1$、$v_2$、$v_3$、$v_4$，其均由预设目标速度$v_{ix}$和侧向滑动速度$v_{iy}$合成$（i=1,2,3,4）$，设左轮和右轮之间的轴距为$c$。

圆周运动的角速度公式如式1所示。
$${\omega }_c=\frac{v_c}{d_c}\text{\hspace{1em}(1)}$$
其中${\omega }_c$为圆周运动角速度，线速度为$v_c$，圆周运动半径为$d$。设$d_c$与y轴的夹角为${\alpha }_c$，由$v_c$与ICR-COM的垂直关系可得$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}$$
由旋转刚体的四个轮子的角速度一致的条件，式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)}$$
同时，$d_i$（其中$i=1,2,3,4$）与$d_c$在x轴和y轴上得投影长度满足式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}$$
四轮差速底盘设定左轮、右轮得速度分别为$V_L$和$V_R$，且在前轮和后轮速度严格同步的情况下，可建立如式6的约束：
$$\begin{gathered} V_L=v_{1x}=v_{2x} \\ {V}_R=v_{3x}=v_{4x}\text{\hspace{1em}(6)} \end{gathered}$$
结合式4、5、6可得式7所示结论：
$$\begin{gathered} V_L={\omega }_c\cdot (d_{cy}-\frac{c}{2})={\omega }_c{d}_{cy}-{\omega }_c\cdot \frac{c}{2}=v_{cx}-{\omega }_c\cdot \frac{c}{2} \\ {V}_R={\omega }_c\cdot (d_{cy}+\frac{c}{2})={\omega }_c{d}_{cy}+{\omega }_c\cdot \frac{c}{2}=v_{cx}+{\omega }_c\cdot \frac{c}{2}\text{\hspace{1em}(7)} \end{gathered}$$
将式7整理，按照矩阵乘的形式表示就得到了四轮差速底盘的前向运动学关系，如式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)}$$
那么逆向运动学的公式只需要进行简单的逆变换即可得到，四轮差速逆向运动学模型如式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)}$$

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

// 正向运动学模型
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;
}