Autoware中pure pursuit纯跟踪算法的代码分析（三）

calcCurvature函数详解

calcCurvature函数原型如下：

double PurePursuit::calcCurvature(const geometry_msgs::Point& target) const
{
  double kappa;
  const geometry_msgs::Point pt = calcRelativeCoordinate(target, current_pose_);
  const double denominator = pt.x * pt.x + pt.y * pt.y;
  const double numerator = 2.0 * pt.y;

  if (denominator != 0.0)
  {
    kappa = numerator / denominator;
  }
  else
  {
    kappa = numerator > 0.0 ? KAPPA_MIN_ : -KAPPA_MIN_;
  }

  return kappa;
}

其中calcRelativeCoordinate函数的返回值是目标规划路径点的位置相对于车辆当前位置的相对坐标。calcRelativeCoordinate函数详解见上一篇文章。

原理图如下：

算法参考：
Simple estimation of curvature given two points.

1. Convert the target point from map frame into the current pose frame,
   so it has a local coorinates of (pt.x, pt.y, pt.z).
2. If we think it is a cirle with a curvature kappa passing the two points,
   kappa = 2* pt.y / (pt.x* pt.x + pt.y * pt.y). For detailed derivation, please
   refer to “Integrated Mobile Robot Control” by Omead Amidi
   (CMU-RI-TR-90-17, Equation 3.10 on Page 21)

其中两点间的平面距离AB = L，BC = x，AC = y；利用三角形勾股定律可以求出圆弧半径，然后取倒数就可以求出曲率。

——————
2021.12.16
软件园