建设网站天河区,网页qq怎么登录界面,做电影网站怎么挣钱,百度搜索引擎网址目录 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 专栏介绍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::vectorgeometry_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 chronoauto start_time std::chrono::high_resolution_clock::now();
g_planner_-plan(...);
auto finish_time std::chrono::high_resolution_clock::now();
std::chrono::durationdouble 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深度学习实战》《机器学习强基计划》《运动规划实战精讲》… 源码获取 · 技术交流 · 抱团学习 · 咨询分享 请联系
