Mobile robot chassis - four-wheel differential model (four independent wheels)

Mobile Robot Chassis - Four Wheel Differential Model

1. Principle of four-wheel differential model

An example of a four-wheel differential model chassis is shown in the figure below. For the front and rear wheels of the chassis, their speeds are synchronous, so under ideal conditions, the chassis motion can be regarded as a circular motion with the ICR as the center, and for the four wheels, the angular speed of the circular motion is consistent , the circular motion center ICR is always located on the y-axis extension line of the chassis geometric center COG, the distance between ICR and COG $d_c$Constrained, constrained and circular motion angular velocity ${oh}_c$Related, the speed of the entire chassis is located at the velocity center COM, and $v_c$Indicates that the instantaneous center velocity $v_c$By component $v_{cx}$和$v_{cy}$Synthesis, set the speed of the four wheels as $v_1$、$v_2$、$v_3$、$v_4$, which are determined by the preset target velocity $v_{ix}$and the lateral sliding velocity $v_{iy}$Composite $（i=1,2,3,4)$ , let the wheelbase between the left wheel and the right wheel be$c$ .

The angular velocity formula of circular motion is shown in formula 1.
$${oh}_c=\frac{v_c}{d_c}\text{\hspace{1em}( 1 )}$$
where${oh}_c$is the angular velocity of circular motion, and the linear velocity is $v_c$, the radius of circular motion is $d$ . let$d_c$The angle with the y-axis is $a_c$, by $v_c$The vertical relationship with ICR-COM can be $v_{c}cos{\alpha }_c=v_{cx}$from $v_{c}sin{\alpha }_c=v_{cy}$,One of the following 2-dimensional functions:
$$\begin{gathered} {oh}_c=\frac{v_c}{d_c}=\frac{v_{c}cos{\alpha }_c}{d_{c}cos{\alpha }_c}=\frac{v_{cx}}{d_{cy}} \\ {oh}_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}$$
From the condition that the angular velocities of the four wheels of the rotating rigid body are consistent, Equation 2 can be generalized to Equation 3:
$$\begin{gathered} {oh}_c=\frac{v_i}{d_i}=\frac{v_{i}cos{\alpha }_i}{d_{i}cos{\alpha }_i}=\frac{v_{ix}}{d_{iy}} \\ {oh}_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：
$${oh}_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 )}$$
At the same time,$d_i$(where $i=1,2,3,4$ ) with$d_c$The projection length on the x-axis and y-axis satisfies formula 5:
$$\begin{gathered} d_1=d_2=d_{cy}-\frac{c}{2} \\ {d}_3=d_{4y}=d_{cy}+\frac{c}{2}\text{\hspace{1em}( 5 )} \end{gathered}$$
The four-wheel differential chassis sets the speeds of the left wheel and the right wheel as$V_L$and $V_R$, and in the case that the speeds of the front and rear wheels are strictly synchronized, constraints such as formula 6 can be established:
$$\begin{gathered} V_L=v_{1x}=v_2 \\ {V}_R=v_{3x}=v_{4x}\text{\hspace{1em}( 6 )} \end{gathered}$$
combination of 4、5、6可得式7 shows conclusion：
$$\begin{gathered} V_L={oh}_c\cdot (d_{cy}-\frac{c}{2})={oh}_c{d}_{cy}-{oh}_c\cdot \frac{c}{2}=v_{cx}-{oh}_c\cdot \frac{c}{2} \\ {V}_R={oh}_c\cdot (d_{cy}+\frac{c}{2})={oh}_c{d}_{cy}+{oh}_c\cdot \frac{c}{2}=v_{cx}+{oh}_c\cdot \frac{c}{2}\text{\hspace{1em}( 7 )} \end{gathered}$$
Arrange Equation 7 and express it in the form of matrix multiplication to obtain the forward kinematics relationship of the four-wheel differential chassis, as shown in Equation 8: [ vcx ω c ] =
$$\left[\begin{array}{l}{v}_{cx}\\ {oh}_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 )}$$
Then the inverse kinematics formula can be obtained only by simple inverse transformation. The inverse kinematics model of the four-wheel differential is shown in formula 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}\\ {oh}_c\end{array}\right]\text{\hspace{1em}(9)}$$

2. Engineering practice

2.1 Python implementation

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++ implementation

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