Autoware-Pure_pursuit代码解析

最新推荐文章于 2025-11-08 01:00:31 发布

原创

最新推荐文章于 2025-11-08 01:00:31 发布 · 5.4k 阅读

16 ·

CC 4.0 BY-SA版权

本文详细解析了Autoware中的纯追踪（Pure Pursuit）算法实现，包括相对坐标计算、预瞄距离设置、曲率计算以及速度控制。通过计算机器人与目标点间的关系，动态调整预瞄距离和曲率，从而引导机器人沿着路径行驶。

// calculation relative coordinate of point from current_pose frame

计算全局坐标系相对于机器人坐标系中的位置,即point相对于current_pose坐标系的位置

公式参考，R=L^2/(2*y),论文--一种纯追踪模型改进算法

geometry_msgs::Point calcRelativeCoordinate(geometry_msgs::Point point_msg, geometry_msgs::Pose current_pose)
{
tf::Transform inverse;
tf::poseMsgToTF(current_pose, inverse);
tf::Transform transform = inverse.inverse();

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

return tf_point_msg;

}

例如：

    geometry_msgs::Point point_msg;

   point_msg.x=1;
   point_msg.y=2;
   point_msg.z=0;

   geometry_msgs::Pose current_pose;
   current_pose.position.x=4;
   current_pose.position.y=7;
   current_pose.position.z=0;
   current_pose.orientation.x=0;
   current_pose.orientation.y=0;
   current_pose.orientation.z=0;
   current_pose.orientation.w=1;

计算结果为

x: -3
y: -5i
z: 0