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

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 npdef forward_kinematics(vl, vr, d):"""正向运动学模型：根据轮子速度计算机器人的运动参数：vl: 左侧轮子的速度vr: 右侧轮子的速度d: 轮子间距返回：x: 机器人的x坐标y: 机器人的y坐标theta: 机器人的角度（弧度）"""R = d / 2.0omega = (vr - vl) / (2.0 * R)v = (vl + vr) / 2.0x = 0.0y = 0.0theta = 0.0if 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_xy = np.sin(omega) * (x - ICC_x) + np.cos(omega) * (y - ICC_y) + ICC_ytheta = theta + omegareturn x, y, thetadef inverse_kinematics(x, y, theta, d):"""逆向运动学模型：根据机器人的位置和角度计算轮子的速度参数：x: 机器人的x坐标y: 机器人的y坐标theta: 机器人的角度（弧度）d: 轮子间距返回：vl: 左侧轮子的速度vr: 右侧轮子的速度"""R = d / 2.0vl = (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;  // xpose[1] = 0.0;  // ypose[2] = 0.0;  // thetaif (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;
}