teb算法可以参考论文:C. Rösmann, F. Hoffmann and T. Bertram: Integrated online trajectory planning and optimization in distinctive topologies, Robotics and Autonomous Systems, Vol. 88, 2017, pp. 142–153.
g2o可以参考论文:《g2o : A General Framework for Graph Optimization》
源码地址:https://github.com/RoboMaster/RoboRTS/tree/ros/roborts_planning
该节点包含两部分:全局路径规划和局部路径规划,全局路径规划使用A*算法,局部路径规划使用TEB算法,分别在
global_planner
和local_planner
中。全局路径规划节点代码结构详解
1.文件目录
2.代码结构
全局规划整个节点很简单,只有一个Actionlib server及A*算法的实现。
Actionlib server的实现见
global_planner_node.cpp
,当中单独开了一个线程PlanThread
用于路径规划,另外有一份测试代码实现了Actionlib client,在global_planner_test.cpp
中,给定目标位置,让server计算路径。启动全局路径规划的代码如下:
其中
global_planner_ptr_->Plan(current_start, current_goal, current_path)
这句话调用了a_star_planner.cpp
中的Plan
函数,该函数又调用SearchPath
函数进行路径规划。SearchPath
才是最终A算法的实现,关于该算法的讲解可以参考[Introduction to the A Algorithm](https://www.redblobgames.com/pathfinding/a-star/introduction.html),讲的浅显易懂。这里也贴上实现代码:
3.运行
正常比赛只需要运行
测试可以在此基础上再运行
另外全局规划节点还集成类RVIZ可视化,只需要打开RVIZ订阅相应的话题即可。
4.依赖
该节点依赖于
roborts_common
、robot_msgs
和roborts_costmap
,请在编译前确保有这三个库。局部路径规划节点代码结构详解
1.代码结构
2.代码结构
局部路径规划的代码的结构较为繁多,代码分为两部分,一部分是ROS节点的实现,建立了一个Actionlib server用于响应路径规划请求,建立一个线程用于局部路径规划。Actionlib server很简单,这里不做过多描述,具体参考
local_planner_node.cpp
。在路径规划线程中通过下面这句话执行规划:
这条语句调用了
teb_local_planner.cpp
,该文件的主要功能是更新机器人位置和路径规划的起点和终点,它初始化了一个g2o优化器optimal_
,并在ComputeVelocityCommands
函数中调用了它:这条语句调用了
teb_optimal.cpp
,这是teb算法的真正实现部分,该文件结合了timed_elastic_band/include
下的头文件和teb_vertex.console.cpp
文件,利用g2o框架实现teb算法。在阅读下面内容前,请先学习teb算法和g2o的基本操作。
teb算法可以参考论文:C. Rösmann, F. Hoffmann and T. Bertram: Integrated online trajectory planning and optimization in distinctive topologies, Robotics and Autonomous Systems, Vol. 88, 2017, pp. 142–153.
g2o可以参考论文:《g2o : A General Framework for Graph Optimization》
另外推荐两个博客:一个是讲teb的:Timed-Elastic-Band局部路径规划算法,一个是讲g2o的:graph slam tutorial : 从推导到应用1。
需要注意的是,代码中只实现了最简单的teb算法,并没有加入后面的拓扑结构分析实现绕过障碍物。
3.TEB算法的实现
这部分的一大特点是函数重载较多,因为有好几种不同的代价函数,也就是g2o超图的边。
整个算法的入口是
bool TebOptimal::Optimal
函数,从teb_optimal.h
中可以看到,这个函数有两个重载,不过都大同小异,先调用InitTEBtoGoal
初始化路径上的机器人位姿和两位姿之间的时间间隔,然后初始化起点速度和终点速度,最后调用OptimizeTeb
函数。OptimizeTeb
函数是真正的teb核心算法,其实现如下:上面的代码中最关键的就是建图和优化两个步骤,建图函数如下:
这一步就是将路径的一系列约束加进去,比如与障碍物保持一定距离,速度、加速度限制、时间最小约束、最小转弯半径约束、旋转方向约束等,这些都转换成了代价函数放在g2o中,g2o优化时会使这些代价函数达到最小。
具体的每一种约束的结构和实现见
local_planner/timed_elastic_band/include/timed_elastic_band/
下的头文件,可结合论文公式理解。优化函数如下:
完成路径优化后,就根据局部路径中的前两个位姿计算当前机器人的速度和加速度,使用
GetVelocity
函数。4.运行
正常比赛时运行
可以运行
然后打开rviz:
直观感受teb算法的强大。
5.依赖
该节点依赖于
roborts_common
、robot_msgs
和roborts_costmap
,请在编译前确保有这三个库。