Autoware 纯跟踪控制中的数学知识点

目录

1.求阿克曼转向角

2.路径虚拟延长

3.求曲率

---

Autoware 纯跟踪控制（pure_pursuit）用到的一些数学方法（如求曲率、求参考曲线上距离小车最最近的点等）,觉得很有参考价值。因此做一些总结记录，方便使用的时候寻找。pure_pursuit源码在autoware中的路径是：autoware.ai/src/autoware/core_planning/pure_pursuit。

1.求阿克曼转向角

如下图1所示，箭头所指的是纯跟踪求解的圆弧，纯跟踪潜在的假设是小车到达目标点Goal point 是按该圆弧轨迹走的。根据输入参考轨迹、当前位置、速度和L的长度，可以确定轨迹上的Goal point ，进而可以求解出圆弧的曲率(kappa)。

图1 纯跟踪控制示意图

如图2所示，是简化后的阿克曼转向示意图。图中是转向角，r为转弯半径，L是轴距，其中r = 1/kappa。根据几何关系可以知道

因此

源码中对应的代码是：

double convertCurvatureToSteeringAngle(
  const double& wheel_base, const double& kappa)
{
  return atan(wheel_base * kappa);
}

图2 阿克曼转向示意图标题

2.路径虚拟延长

纯跟踪控制是实时跟踪参考轨迹上距离小车L的一个目标点。正常情况图3中A所示，算法可以找到跟踪目标点。当遇到B中的情况时，由于小车距离导航目标点很近，距离小于L,这时就无法找到跟踪目标点，因此算法失效。解决这个问题可以在参考路径末端延长一段距离（如C所示），这样就能保证小车在整个参考路径上都有效。延长的路径只是为保证算法不失效，实际上小车不会走这段路径，因为检测到小车到达导航目标点时，小车已经停下。

图3

 延长的路径以路径终点为起点，方向也是终点的方向。源码中是将坐标系转换到终点，在终点坐标系上进行插入点。终点坐标系中的x轴就是我门要延长的路径的方向，根据插入间距增加x坐标，然后转换回map坐标系，就得到对应的插入点。插值与坐标转换的源码如下：

  for (double dist = minimum_lookahead_distance_; dist > 0.0; dist -= interval)
  {
    virtual_last_point_rlt.x += interval * sgn;
    virtual_last_waypoint.pose.pose.position =
      calcAbsoluteCoordinate(virtual_last_point_rlt, pn);
    lane->waypoints.emplace_back(virtual_last_waypoint);
  }

// calculation absolute coordinate of point on current_pose frame
geometry_msgs::Point calcAbsoluteCoordinate(geometry_msgs::Point point_msg, geometry_msgs::Pose current_pose)
{
  tf::Transform inverse;
  tf::poseMsgToTF(current_pose, inverse);

  tf::Point p;
  pointMsgToTF(point_msg, p);
  tf::Point tf_p = inverse * p;
  geometry_msgs::Point tf_point_msg;
  pointTFToMsg(tf_p, tf_point_msg);
  return tf_point_msg;
}

3.求曲率

根据纯跟踪曲率公式,当获取小车当前点坐标，和跟踪目标点时就可以求出。L是两个点的距离。将目标点转换到车体坐标系，就可以求出.相应的代码如下：

double calcRadius(const geometry_msgs::Point &target, const geometry_msgs::Pose &current_pose)
{
  constexpr double RADIUS_MAX = 1e9;
  const double denominator = 2.0 * transformToRelativeCoordinate2D(target, current_pose).y;
  const double numerator = calcDistSquared2D(target, current_pose.position);

  if (std::fabs(denominator) > 0)
    return numerator / denominator;
  else
    return RADIUS_MAX;
}