差速轮模型，阿克曼模型的运动模型，轨迹预测（附代码演示）

lmrcat

已于 2023-12-16 17:42:10 修改

阅读量2.3k

点赞数 21

分类专栏：算法文章标签：机器人算法自动驾驶

于 2023-12-16 17:39:21 首次发布

本文链接：https://blog.csdn.net/jucat/article/details/135034782

版权

算法专栏收录该内容

20 篇文章 2 订阅

订阅专栏

差速轮模型

图 fig.1 中，差速轮机器以独轮车模型表示。其中点 Robot 是差速轮的驱动轮中心，点 H 是机器车头，表示了机器的朝向。当机器以固定的角速度，线速度运行时候，会产生圆形轨迹 C，圆心为 O，转弯半径 r。Robot' ，H' 表示机器在圆形运动中某个时刻的位姿。

差速轮机器以固定角速度，线速度运行时候，轨迹表示如下：

差速轮轨迹

差速轮运动模型

在图 fig.2 中，机器 Robot 以固定角速度 w，线速度 v 运行了一段时间 dt 后来到了 Robot' 的位置。以终点机器 Robot' 朝向作直线，交起点机器 Robot 的驱动轮轴线于点 D。因为 Robot 和 Robot' 都在以点 O 为圆心，r 为半径的圆 C 上，所以机器的朝向都是与圆 C 相切。过点 Robot' 作垂线垂直于 Robot 的驱动轮轴线于点 T。Robot 的朝向直线和 Robot' 的朝向直线相交于点 A（图中未标明）。有直角三角形 Robot' - O - T 相似于直角三角形 A - D - Robot。

终点机器比起点机器的航向角 yaw 增加了 w * dt = α 角度。

通过相似直角三角形知道此时运动圆弧所对的圆心角 γ = α = w * dt。

角 ε = π/2 - γ。

根据线速度，角速度，知道转弯半径 r = v / w。

设线段 Robot' - T 为 dx，线段 T - Robot 为 dy，那么有：

$r=\frac{v}{w}$

$r^2=dx^2+(r-dy)^2$

$\gamma =w*dt$

$r=dy+r*cos(\gamma )$

联立求得

$\alpha =\gamma=w*dt$

$dy=\frac{v}{w}*(1-cos(\gamma))$

$dx=\sqrt{(\frac{v}{w})^2-(\frac{v}{w}-dy)^2}$ ，当 fabs(α) < π 的时候 dx 为正，当 fabs(α) > π 的时候 dx 为负

dx dy 就是机器在线速度 v，角速度 w 下运行 dt 时间后的相对坐标，α 角是相对角度。

注意：

在这里转弯半径 r 是有方向的。

顺时针角速度 w 为负，逆时针角速度 w 为正。前进线速度 v 为正，后退线速度 v 为负。转弯半径方向由 v / w 得到。

阿克曼模型

在图 fig.4 中，给差速轮模型的车头 H 位置装上一个转向轮 S，构成阿克曼的单车模型。假设该车的转向轮范围是 [-45°, 45°]，转向轮轴线交驱动轮轴线于点 Oal，该点就是阿克曼模型的转弯的圆心，线段 ral 就是阿克曼的转弯半径，圆形轨迹 cal 是机器以固定转向轮角度，线速度运行产生的运动轨迹。

阿克曼机器以固定转向轮角度，线速度运行时候，轨迹表示如下（演示前轮转角 [0, 45°] 的转弯轨迹）：

阿克曼运动模型

可以看出阿克曼的运动模型其实与差速轮模型比较类似，我们只需要求出阿克曼模型的角速度 w 和转弯半径 r，之后就可以套用差速轮运动模型了。

前轮转轴轴线与运动轨迹圆 cal 交于点 L，点 L 所在圆 cal 的切线交机器 Robot 的驱动轮轴线于点 M。三角形 Oal - L - M 与三角形 Oal - H - Robot 相似。角 ζ 等于前轮转角 steering。

设线段 Robot - H 为 wheelbase，也就是轴距，前后轮之间的距离，有：

$r=\frac{wheelbase}{tan(\zeta )}$ ，ζ 不能取 n * π/2，n 为整数

$w=v/r$

注意：

转向轮向左转为正，右转为负。参考图 fig.3 的符号位置，当“转向轮左转 - 机器前进”和“转向轮右转 - 机器后退” 这两种情况，机器是逆时针运动，角速度为正；当 “转向轮左转 - 机器后退” 和 “转向轮右转 - 机器前进” 这两种情况，机器是顺时针运动，角速度为负；

C++ ROS 代码

下面函数功能是输入当前机器位姿，线速度，角速度（前轮角度）后，计算 dt 时间后的机器位姿

/**
 * @brief 差速轮和阿克曼运动模型预测位姿
 * @param  pose             当前位姿
 * @param  v                当前线速度
 * @param  ws               当前角速度或阿克曼的转向轮角度
 * @param  dt               运动所长时间
 * @param  wheel_base       阿克曼的轴距
 * @param  use_steering     是否将 ws 参数作为阿克曼转向轮角度
 * @return geometry_msgs::PoseStamped 返回运动 dt 时间后的位姿
 */
geometry_msgs::PoseStamped predict(const geometry_msgs::PoseStamped pose,
                                   const double v, const double ws,
                                   const double dt, const double wheel_base,
                                   const bool use_steering = false) {
  double dx = 0, dy = 0, alpha = 0;
  double epsilon = 0.001;
  if (fabs(ws) < epsilon) { // 认为是直线
    dx = v * dt;
  } else {
    double r, w = ws;
    if (use_steering) {
      r = fabs(wheel_base / tan(ws));
      r *= (ws > 0) ^ (v >= 0) ? -1 : 1;
      w = v / r;
    }
    double angle = fabs(w * dt);
    while (angle > 2*M_PI) angle -= 2*M_PI;
    int dx_sign = (angle > M_PI ? -1 : 1) * (v > 0 ? 1 : -1);
    alpha = angles::normalize_angle(w * dt);
    dy = v/w*(1-cos(w * dt));
    dx = std::sqrt(std::pow(v/w, 2) - std::pow(v/w-dy, 2)) * dx_sign;
  }

  geometry_msgs::PoseStamped next_pose = pose;
  double yaw = tf::getYaw(next_pose.pose.orientation);
  next_pose.pose.position.x += cos(yaw)*dx - sin(yaw)*dy;
  next_pose.pose.position.y += sin(yaw)*dx + cos(yaw)*dy;
  double next_yaw = angles::normalize_angle(yaw + alpha);
  next_pose.pose.orientation = tf::createQuaternionMsgFromYaw(next_yaw);
  return next_pose;
}