路径规划 | ROS中多个路径规划算法可视化与性能对比分析

目录

• 0 专栏介绍
• 1 引言
• 2 禁用局部规划器
• 3 路径规划定性对比实验
• • 3.1 加载路径规划器和可视化插件
  • 3.2 设置起点和终点
  • 3.3 选择规划器规划
  • 3.4 不同规划器对比
  • 3.5 路径保存和加载
• 4 路径规划定量对比实验
• • 4.1 计算规划耗时
  • 4.2 计算规划长度
  • 4.3 计算拓展节点数
  • 4.4 计算路径曲率
  • 4.5 计算路径转角

0 专栏介绍

🔥附C++/Python/Matlab全套代码🔥课程设计、毕业设计、创新竞赛必备！详细介绍全局规划(图搜索、采样法、智能算法等)；局部规划(DWA、APF等)；曲线优化(贝塞尔曲线、B样条曲线等)。

🚀详情：图解自动驾驶中的运动规划(Motion Planning)，附几十种规划算法

---

1 引言

在《运动规划实战精讲》系列中，我们介绍了非常多种路径规划算法，不同的算法有不同的工作原理及其适用场景。例如，A*算法通过启发式方法进行路径搜索，能够在较小的搜索空间内找到较优解，而RRT算法则通过随机采样快速扩展搜索树，适合高维空间的路径规划。

自然地，我们希望能够有一种方式定性、定量地对比不同算法的性能，比如计算效率、资源消耗等。本节通过设计一个可视化插件的形式，帮助大家进行规划算法的对比实验，并提供了常用的定量指标计算方法，最终效果如下所示

2 禁用局部规划器

在ROS的导航功能包集navigation中提供了move_base功能包，用于实现导航功能。move_base功能包提供了基于动作(action)的路径规划实现，move_base可以根据给定的目标点，控制机器人底盘运动至目标位置，并且在运动过程中会连续反馈机器人自身的姿态与目标点的状态信息。我们知道，move_base主要由全局路径规划与本地路径规划组成。

如果希望在给定的起点和终点对比不同的规划算法，就需要临时屏蔽local_planner的功能，防止机器人因为运动造成重规划。局部规划插件的详细设计流程在ROS从入门到精通5-5：局部路径规划插件开发案例(以DWA算法为例)，这里直接给出静态局部规划器的代码

bool StaticPlanner::setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan)
{
  if (!initialized_)
  {
    ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
    return false;
  }
  return true;
}

bool StaticPlanner::isGoalReached()
{
  if (!initialized_)
  {
    ROS_ERROR("Static planner has not been initialized");
    return false;
  }

  if (goal_reached_)
  {
    ROS_INFO("GOAL Reached!");
    return true;
  }
  return false;
}

bool StaticPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
{
  if (!initialized_)
  {
    ROS_ERROR("Static planner has not been initialized");
    return false;
  }

  cmd_vel.linear.x = 0.0;
  cmd_vel.angular.z = 0.0;

  return true;
}

3 路径规划定性对比实验

3.1 加载路径规划器和可视化插件

首先打开src/sim_env/config中的planner.yaml文件，配置可选的规划器属性，如下所示

planner:
  - a_star
  - dijkstra
  - gbfs
  - jps
  - d_star
  - lpa_star
  - d_star_lite
  - voronoi
  - theta_star
  - lazy_theta_star
  - rrt
  - rrt_star
  - rrt_connect
  - informed_rrt
  - aco
  - pso
  - ga

接着启动项目，在rviz中添加RMPV可视化插件

3.2 设置起点和终点

通过RMPV面板的Start Pose和Goal Pose选项设置起点和终点位姿

3.3 选择规划器规划

在RMPV的Planner选项中选择具体的规划器，点击Start Planning即可开始规划，左侧面板将显示规划器名称、起点、终点、路径长度、颜色等信息

3.4 不同规划器对比

重复3.3的步骤进行不同规划器的规划实验

3.5 路径保存和加载

点击Save Paths可以以json格式保存路径到本地，如图所示。再下一次实验中，可以通过Load Paths复现之前的实验

4 路径规划定量对比实验

4.1 计算规划耗时

耗时指标用cal_time.count()表示

#include <chrono>

auto start_time = std::chrono::high_resolution_clock::now();
g_planner_->plan(...);
auto finish_time = std::chrono::high_resolution_clock::now();
std::chrono::duration<double> cal_time = finish_time - start_time;
R_INFO << "Calculation Time: " << cal_time.count();

4.2 计算规划长度

规划长度用path_len表示

for (int i = 1; i < plan.size(); i++)
{
  const auto& pt1 = plan[i - 1];
  const auto& pt2 = plan[i];
  path_len += std::hypot(pt1.x() - pt2.x(), pt1.y() - pt2.y());
}
R_INFO << "Path Length: " << path_len;

4.3 计算拓展节点数

对于图搜索算法，拓展节点数是重要指标，用expand.size()表示

path_found = g_planner_->plan({ g_start_x, g_start_y, tf2::getYaw(start.pose.orientation) },
                                { g_goal_x, g_goal_y, tf2::getYaw(goal.pose.orientation) }, origin_path, expand);
R_INFO << "Expand Size: " << expand.size();

4.4 计算路径曲率

平均曲率和最大曲率是衡量路径平滑度的重要指标，计算如下

double max_curv = 0.0;
double avg_curv = 0.0;

auto calCurv = [](const PathPlanner::Point3d& pt1, const PathPlanner::Point3d& pt2,
                  const PathPlanner::Point3d& pt3) {
  double ax = pt1.x();
  double ay = pt1.y();
  double bx = pt2.x();
  double by = pt2.y();
  double cx = pt3.x();
  double cy = pt3.y();
  double a = std::hypot(bx - cx, by - cy);
  double b = std::hypot(cx - ax, cy - ay);
  double c = std::hypot(ax - bx, ay - by);

  double cosB = (a * a + c * c - b * b) / (2 * a * c);
  double sinB = std::sin(std::acos(cosB));

  double cross = (bx - ax) * (cy - ay) - (by - ay) * (cx - ax);
  return std::copysign(2 * sinB / b, cross);
};

for (int i = 2; i < plan.size(); i++)
{
  int n = i - 1;
  const auto& pt1 = plan[i - 2];
  const auto& pt2 = plan[i - 1];
  const auto& pt3 = plan[i];
  double curv = std::fabs(calCurv(pt1, pt2, pt3));
  max_curv = std::max(curv, max_curv);
  avg_curv = ((n - 1) * avg_curv + curv) / n;
}

4.5 计算路径转角

平均转角和最大转角是衡量路径平滑度的重要指标，计算如下

double max_angle = 0.0;
double avg_angle = 0.0;

auto calAngle = [](const PathPlanner::Point3d& pt1, const PathPlanner::Point3d& pt2,
                const PathPlanner::Point3d& pt3) {
rmp::common::geometry::Vec2d vec1(pt2.x() - pt1.x(), pt2.y() - pt1.y());
rmp::common::geometry::Vec2d vec2(pt3.x() - pt2.x(), pt3.y() - pt2.y());
return std::acos(vec1.innerProd(vec2) / (vec1.length() * vec2.length()));
};

for (int i = 2; i < plan.size(); i++)
{
  int n = i - 1;
  const auto& pt1 = plan[i - 2];
  const auto& pt2 = plan[i - 1];
  const auto& pt3 = plan[i];
  double angle = calAngle(pt1, pt2, pt3);
  max_angle = std::max(angle, max_angle);
  avg_angle = ((n - 1) * avg_angle + angle) / n;
}

---

🔥 更多精彩专栏：

• 《ROS从入门到精通》
• 《Pytorch深度学习实战》
• 《机器学习强基计划》
• 《运动规划实战精讲》
• …