Autoware中pure pursuit纯跟踪算法的代码分析（三） |
您所在的位置：网站首页 › qq邮箱已过期文件恢复 › Autoware中pure pursuit纯跟踪算法的代码分析（三） |
Autoware中pure pursuit纯跟踪算法的代码分析（三）
|
目录
calcCurvature函数详解
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. 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).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 软件园 |
【本文地址】
今日新闻 |
推荐新闻 |