1. 前言
在前面我们主要集中在TEB算法的学习。并且,在学习中,我已经把TEB算法从ROS的框架中移植出来,使用IPC中的shm替代ROS的socket通信,替代的过程可以查看我的队友lxz的blog。
其实,TEB的移植本身很好做,但是,因为它本身对ROS没有太严重的依赖,算法所需要的变量类型在TEB本身中已经定义完备,不需要依赖ROS所定义的类型。
但是,TEB算法所规划的路径是软约束,有可能产生很不可能通行的路径,因此,不能直接使用TEB算法所产生的路径。
需要对路径进行一定的判断和检验后,才能使用。
这里可以参考TEB算法中的teb_local_planner_ros.cpp的computeVelocityCommands
函数。
在函数中,有很重要的一个检验:判断路径是否碰撞到障碍物。这里需要用到ROS的Navigation
中的costmap
。
但因为我的模块需要脱离ROS环境,因此,我这里选择使用了功能更加强大、方便和通用的grid map进行替代。
之后的所有路径可通行性判断,TEB算法的障碍物获取等功能的使用,都将基于Grid Map实现。
Grid Map项目地址为:https://github.com/ANYbotics/grid_map
2. Grid Map 简介
这是一个带有ROS接口的C++库,用于管理具有多个数据层的二维网格地图。它是为移动机器人测绘设计的,用于存储数据,如高程、方差、颜色、摩擦系数、立足点质量、表面法线、可穿越性等。
具体的介绍可以查看官网:http://wiki.ros.org/grid_map
3. Grid Map 使用
对于本项目,我只需要使用 Grid Map
的 Grid map core
,而这一部分,完全脱离ROS实现,可以很方便的移植到自己的项目。
本次项目中,Grid Map
是一个很方便用来存储地图的容器。它可以无拷贝高效地实现地图中心跟随着小车移动,从而实现只记录小车周围障碍物地图的作用。
3.1 写入Grid Map
这一部分由我的队友yxs完成。但我这里可以大致说一下流程:
- 输入:
- 世界坐标系下的lidar点云数据
- 小车当前位置的精确IMU信息
- 使用滤波器筛选数据,滤除不关心的数据,比如:地面,超过小车6m的点。
- 只考虑每个点的x, y坐标信息,使用占据栅格地图构建算法,往Grid Map中写入数据。通过这样的方法可以只保留静止障碍物的信息。点云地图内的行人走过的痕迹,会随着时间在地图中被逐渐擦除。
- 使用
move
操作,更新Grid Map
中心在世界坐标系下的位置,让地图紧跟小车当前在世界坐标系中的位置。
3.2 使用 Grid Map
当我们获取到通过占据栅格构建算法得到的 Grid Map
后,在本项目中,主要可以用来识别障碍物和判断TEB算法生成的轨迹是否碰到障碍物。
3.2.1 识别障碍物
识别障碍物的工作我是参考了costmap_convertor。然后基于 Grid Map
写了一个 convertor
用来从 Grid Map
识别障碍物并转换到 TEB 的障碍物类别。
具体流程如下:
- 使用
DBSCAN
算法进行聚类。从而获得到一个个的点集。 - 使用
monotoneChain
对每一个点集,做凸包,得到对应的多边形的顶点的有序点集。 - 将第2个步骤得到的有序点集转换成TEB算法定义的障碍物。
3.2.2 判断轨迹可通行性
这个部分主要是为了复现 TEB
算法的 ROS
接口中判断轨迹是否是可通行的函数 isTrajectoryFeasible
。
在原本的TEB算法中, isTrajectoryFeasible
是通过调用 navigation
中的一个模块 costamap
,让 costmap
判断给定的位姿下,小车是否有碰到障碍物。
我这里采用的方法和 costmap
类似。对于判断路径上的一个点是否有碰到障碍物,我选择通过使用 Grid Map
的迭代器,去查询在当前位姿点下,小车的足迹模型内是否存在障碍物。