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

pure pursuit纯跟踪算法的简介

pure pursuit纯跟踪算法在汽车智能驾驶领域目前的应用很广泛，主要用于汽车的循迹。这种算法比较基础，利用的是数学几何知识。在已知当前点坐标和目标循迹点坐标后，通过计算两个点之前的曲率来获得汽车前轮的转向角，以此达到控制汽车循迹的目的。

源码分析

文件简介

Autoware中的pure pursuit纯跟踪算法主要pure_pursuit_note.cpp,pure_pursuit_core.cpp,pure_pursuit.cpp和pure_pursuit_viz.cpp这四个文件组成。
pure_pursuit_note.cpp：主函数main函数入口。
pure_pursuit_core.cpp：负责rosTopic的收发，回调函数的实现。
pure_pursuit.cpp：规划路径点有效性的判定，曲率的计算。
pure_pursuit_viz.cpp：Rviz的显示。

涉及到的两个类

PurePursuitNode 框架定义
PurePursuit 算法分析与计算

框架性函数介绍

void PurePursuitNode::run()   //算法的入口函数

void PurePursuitNode::callbackFromConfig(const autoware_config_msgs::ConfigWaypointFollowerConstPtr& config)  //回调函数：订阅相关车辆配置参数，包含预瞄距离，车辆速度和最小预瞄距离
void PurePursuitNode::callbackFromCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg) //回调函数：获取当前汽车的位资信息，包含坐标和四元数
void PurePursuitNode::callbackFromCurrentVelocity(const geometry_msgs::TwistStampedConstPtr& msg) //回调函数：获取当前汽车的车速信息
void PurePursuitNode::callbackFromWayPoints(const autoware_msgs::LaneConstPtr& msg) //回调函数：订阅规划的路径点

void PurePursuitNode::publishCtrlCmdStamped(const bool& can_get_curvature, const double& kappa) const //publish车辆速度，加速度以及转角
void PurePursuitNode::publishTwistStamped(const bool& can_get_curvature, const double& kappa) const //publish x轴线速度和z轴角速度
1
2
3
4
5
6
7
8
9

算法实现函数（按顺序介绍）

computeLookaheadDistance

computeLookaheadDistance函数主要是用于计算预瞄距离。

double PurePursuitNode::computeLookaheadDistance() const
{
  if (velocity_source_ == enumToInteger(Mode::dialog))
  {
    return const_lookahead_distance_;
  }

  const double maximum_lookahead_distance = current_linear_velocity_ * 10; //最大预瞄距离 = 当前速度×10
  const double ld = current_linear_velocity_ * lookahead_distance_ratio_; //预瞄距离 = 当前速度×K值(可配置，可初始化)

  return ld < minimum_lookahead_distance_ ? minimum_lookahead_distance_ :
                                            ld > maximum_lookahead_distance ? maximum_lookahead_distance : ld;
}
1
2
3
4
5
6
7
8
9
10
11
12
13

getNextWaypoint

getNextWaypoint函数主要是用于获取有效的目标规划路径点。

void PurePursuit::getNextWaypoint()
{
  //获取规划路径点的个数
  const int path_size = static_cast<int>(current_waypoints_.size());

  // if waypoints are not given, do nothing.
  if (path_size == 0)
  {
    next_waypoint_number_ = -1;
    return;
  }

  //遍历规划的路径点
  // look for the next waypoint.
  for (int i = 0; i < path_size; i++)
  {
    //如果是最后一个路径点，则返回
    // if search waypoint is the last
    if (i == (path_size - 1))
    {
      ROS_INFO("search waypoint is the last");
      next_waypoint_number_ = i;
      return;
    }

    //只有汽车的当前位置点与当前规划路径点之间的距离大于预瞄距离点时才算有效
    // if there exists an effective waypoint
    if (getPlaneDistance(current_waypoints_.at(i).pose.pose.position, current_pose_.position) > lookahead_distance_)
    {
      next_waypoint_number_ = i;
      return;
    }
  }

  // if this program reaches here , it means we lost the waypoint!
  next_waypoint_number_ = -1;
  return;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38

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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

calcRelativeCoordinate

calcRelativeCoordinate是用来计算汽车当前点与下一目标规划点之前的相对坐标的。

// calculation relative coordinate of point from current_pose frame
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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

canGetCurvature

canGetCurvature是用来获取曲率的。

bool PurePursuit::canGetCurvature(double* output_kappa)
{
  //判断规划路径点的有效性，赋值->next_waypoint_number_
  // search next waypoint
  getNextWaypoint();
  if (next_waypoint_number_ == -1)
  {
    ROS_INFO("lost next waypoint");
    return false;
  }
  //判断曲率是否有效
  // check whether curvature is valid or not
  bool is_valid_curve = false;
  for (const auto& el : current_waypoints_)
  {
    //只有汽车的当前位置点与当前规划路径点之间的距离大于最小预瞄距离点时，曲率才算有效
    if (getPlaneDistance(el.pose.pose.position, current_pose_.position) > minimum_lookahead_distance_)
    {
      is_valid_curve = true;
      break;
    }
  }
  if (!is_valid_curve)
  {
    return false;
  }
  // if is_linear_interpolation_ is false or next waypoint is first or last
  if (!is_linear_interpolation_ || next_waypoint_number_ == 0 ||
      next_waypoint_number_ == (static_cast<int>(current_waypoints_.size() - 1)))
  {
    //获取规划中的下一路径点的位置
    next_target_position_ = current_waypoints_.at(next_waypoint_number_).pose.pose.position;
    //计算下一路径点与汽车当前位置之间的圆弧曲率
    *output_kappa = calcCurvature(next_target_position_);
    return true;
  }

  // linear interpolation and calculate angular velocity
  const bool interpolation = interpolateNextTarget(next_waypoint_number_, &next_target_position_);

  if (!interpolation)
  {
    ROS_INFO("lost target!");
    return false;
  }

  *output_kappa = calcCurvature(next_target_position_);
  return true;

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49

convertCurvatureToSteeringAngle

convertCurvatureToSteeringAngle是用来转换曲率为转角。

double convertCurvatureToSteeringAngle(const double& wheel_base, const double& kappa)
{
  //转向角 = 反正切（轴距×曲率）
  return atan(wheel_base * kappa);
}
1
2
3
4
5

后面会有文章来分析计算曲率涉及到的数学几何知识。

——————
2021.12.13
软件园