移动机器人底盘-四轮差速模型
文章来源：https://www.toymoban.com/news/detail-493515.html
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所示。
ω
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=dcy−2cd3y=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⋅(dcy−2c)=ωcdcy−ωc⋅2c=vcx−ωc⋅2cVR=ωc⋅(dcy+2c)=ωcdcy+ωc⋅2c=vcx+ωc⋅2c(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]=[21−c121−c1][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]=[11−2c2c][vcxωc](9)文章来源地址https://www.toymoban.com/news/detail-493515.html
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;
}
到了这里，关于移动机器人底盘-四轮差速模型（四轮独立）的文章就介绍完了。如果您还想了解更多内容，请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章，希望大家以后多多支持TOY模板网！
