系列文章目录
提示：这里可以添加系列文章的所有文章的目录，目录需要自己手动添加
TODO:写完再整理
文章目录
前言
认知有限，望大家多多包涵，有什么问题也希望能够与大家多交流，共同成长！在项目和平时的学习中，我对机器人/无人驾驶的决策规划模块进行了划分，当然划分的方法有很多，我的划分方式仅供参考
（1）动态障碍物行为预测模块（Behavior prediction）–结合感知和高精度地图信息，估计周围障碍物未来运动状态
（2）执行机构的轨迹规划模块（Trajectory_planning）–执行机构如机器人载体上的机械臂、【防盗标记–盒子君hzj】串行云台等的运动轨迹规划
（3）任务决策模块（Mission_planning）–任务决策模块比较偏业务层了，处理机器人/无人驾驶的各种任务，【防盗标记–盒子君hzj】主要分为三个方面：车底盘航线业务决策（交规、横向换道等等）、执行应用机构业务决策（机械臂、人机交互等等）、不同场景的导航方案切换决策（组合导航、融合导航）
（4）前端路径探索模块（path_finding）–全局路径规划算法难度不算复杂，找到一条可通行的（必须满足）、考虑动力学的（尽可能满足）、可以是稀疏的路径base_waypoints【由于其只考虑了环境几何信息，往往忽略了无人机本身的运动学与动力学模型。【防盗标记–盒子君hzj】因此，其得到的轨迹往往显得比较“突兀”，并不适合直接作为无人机的控制指令】
（5）后端轨迹处理模块（motion_planning）–我主要归纳整理为三个方向：（1）对base_waypoints进行简单处理及生成方向、（2）对base_waypoints轨迹优化方向【一般是二次优化，这里用的较多的事优化方面的知识】、（3）进行对应功能的replan方向（replan之前的预处理、进入replan的条件、停障replan、避障replan、纠偏replan、换道replan、自动泊车replan、穿过狭窄道路replan等等），【防盗标记–盒子君hzj】这部分内容使用的方法比较专
（6）路径跟踪模块（trajectory_following）–这个模块就得针对机器人载体了，如无人驾驶使用得阿克曼模型可以采用几何的pure pursuit纯追踪算法，更好的可以用模型预测控制MPC方法，还有强化学习做的（效果怎样我就没验证过了）；【防盗标记–盒子君hzj】当然也可能事麦克纳姆轮车、差速车PID、还有无人机的三维轨迹跟踪等等
（7）碰撞检测模块
（8）集群多机器人规划模块
当然，这种划分方式是我权衡了原理和功能粗略划分的，在实际产品研发过程中，需要理解了各个算法的功能和定位的基础上融汇贯通，不能生搬硬套，如全段通过hybrid A探索出来的路径与A、RRT*探索出来的路径更平滑，后端轨迹优化的任务就不用这么重了；又如，机器人/无人驾驶项目研发的需求业务还没发展到能响应很多功能阶段，【防盗标记–盒子君hzj】任务决策使用简单的状态机（fsm）就可以对现有任务进行状态转移了
本文先对costmap_2d代价地图生成原理做个简单的介绍，具体内容后续再更，其他模块可以参考去我其他文章
提示：以下是本篇文章正文内容
（1）【代价地图】costmap_2d功能包源码解读
costmap_2d包提供了一种2D代价地图的实现方案，该方案从实际环境中获取传感器数据，构建2D或3D占用栅格地图，以及基于占用栅格和用户定义膨胀半径的2D代价地图的膨胀代价【防盗标记–盒子君hzj】。该包也支持基于map_server初始化代价地图，基于滚动窗口的代价地图，以及基于参数化订阅和配置传感器主题
（1）代价地图生成的理论
（1）构型空间（ Configuration Space）定义
把障碍物进行膨胀，以忽略障碍物的棱角，把移动机器人看成一个质点，然后把障碍物和机器人人放入栅格地图中，就形成了构型空间（ Configuration Space）
（2）栅格地图转换为有权图
（2）代价地图的类型
（1）全局代价地图（global_costmap)：负责全局环境的路径规划，得到整体低阶路线
（2）局部代价地图（local_costmap）：负责局部环境的路径规划，实时避障
.
（3）costmap_2d功能包原理
大致的原理是通过建立不同的图层Layer然后叠在一起，【防盗标记–盒子君hzj】被填充的栅格点表示有障碍物
###（4）代价地图的层级结构及建立图层原理
layer类中还使用到了LayeredCostmap，【防盗标记–盒子君hzj】用来将各个层聚合在一起。LayeredCostmap类中主要的更新函数是updateMap
（1）staticLayer静态地图层
通常都是使用SLAM建立完成的静态地图，SLAM构建的地图在导航中是不可以直接使用的，因为SLAM构建的地图没有代价因子，【防盗标记–盒子君hzj】静态地图需要转换成静态代价地图，静态地图层代表代价地图的一个很大的不变部分
静态地图层只是简单地将灰度图中的像素值换算成ros代价地图中的代价值
incomingMap：接收到地图就更新staticlayer的costmap
incomingUpdate：updateCosts才真正反应到结果上
（2）障碍地图层obstacleLayer
用于动态记录传感器感得知道的障碍物信息，它需要根据雷达信息实时更新
障碍地图层得更新过程：
obstacleLayer用了4个函数来处理不同的信息类型：laserScanCallback laserScanValidInfCallback pointCloudCallback pointCloud2Callback，【防盗标记–盒子君hzj】这些函数在初始化的时候跟扫描数据直接绑定，所以一拿到数据地图就可以更新，等到updateCost的时候反映到结果上。所有的信息都会被处理变成ObservationBuffer类型，也就是一系列三维点云：std::list observation_list_;obstacle之后的所有计算都是基于这个类型的。
（3）膨胀层inflationLayer
该层的目的：
膨胀层是对致命障碍（lethal obstacles）周围边界的扩展（即是障碍物膨胀），以便代价地图更清晰显示机器人的可利用空间，避免机器人的外壳或者突出物装上障碍物【防盗标记–盒子君hzj】，使得运动具有一定的裕量
膨胀的过程：
膨胀过程在每个致命障碍周围插入缓冲区。增加到costmap的值取决于离最近的障碍的距离。缓冲区根据离圆心的距离存了一个cost，一个dist，一格格往外膨胀的时候直接查就行了
（4）voxelLayer
对付三维情况的，这里没用到,这一层具有与障碍层相同的功能，【防盗标记–盒子君hzj】但在三维空间中跟踪传感器数据。引入的三维像素网格允许更智能地清除障碍物，以反映可以看到的多个高度
（5）其他层（Other layer）
自己通过插件的形式实现自己的代价地图，以实现不同的功能
Social Costmap Layer
Range Sensor Layer
range_sensor_layer是costmap_2d中LayeredCostmap的一个插件，使用消息类型是 sensor_msgs/Range ，适用于声呐和红外传感器数据传输。
Range消息通过使用概率模型整合到代价图中。
在主代价地图（master costmap）中，【防盗标记–盒子君hzj】高于mark_threshold的概率的单元格会被标记为致命障碍（lethal obstacles），低于clear_threshold的概率的单元格会被标记为自由空间。
（5）计算代价值的方法
横轴是距离机器人中心的距离，纵轴是代价地图中栅格的灰度值
（1）致命障碍:栅格值为254：此时障碍物与机器人中心重叠，必然发生碰撞；
（2）内切障碍:栅格值为253：此时障碍物处于机器人的内切圆内，必然发生碰撞；
（3）外切障碍:栅格值为[128,252]：【防盗标记–盒子君hzj】此时障碍物处于其机器人的外切圆内，处于碰撞临界，不一定发生碰撞
（4）非自由空间:栅格值为(0,127]：此时机器人处于障碍物附近，属于危险警戒区，进入此区域，将来可能会发生碰撞；
（5）自由区域:栅格值为0：此处机器人可以自由通过；
（6）未知区域:栅格值为255：还没探明是否有障碍物
（6）聚合各层代价值，更新代价地图的方法
（1）地图维护的过程
代价地图以参数update_frequency 指定的周期进行地图更新。每个周期获取传感器数据后，代价地图底层占用结构都会执行标记making和清除cleaning操作，【防盗标记–盒子君hzj】并且将该结构投影到代价图且附上相应代价值。然后，每个代价值为costmap_2d::LETHAL_OBSTACLE的单元格都会执行障碍物膨胀操作，即从每个占用单元格向外传播代价值，直到用户定义的膨胀半径为止。膨胀代价值随着机器人离障碍物距离增加而减少
（2）tf变换
为了把来自传感器源的数据插入到代价地图，costmap_2d::Costmap2DROS要大量使用tf。 具体来说，它假定由global_frame参数，robot_base_frame参数和传感器源指定的坐标系之间的所有变换都是有联系的且是最新的。transfor_tolerance 参数设置了这些变换之间的最大延迟量（amount of latencylatency）。【防盗标记–盒子君hzj】如果 tf 树没有以期望速度来更新，那么导航功能包将会让机器人停止
（3）updateBounds函数【解决更新区域的问题】
功能：确定代价图中要更新的边界框（仅仅在边界框中进行代价值的更新）
过程：顺序遍历，【防盗标记–盒子君hzj】依次为每个层提供前层需要更新的边界框(最初是一个空框)。它决定了需要更新多少主成本图
（4）updateCosts函数【Marking and Clearing【标记操作和清除操作】】
功能：将代价值写入到master costmap
过程：代价地图订阅ROS上的传感器话题，并相应进行更新。每个传感器可以执行标记操作（将障碍物信息插入到成本图中），清除操作（从成本图中删除障碍物信息）或两者都同时执行
（1）Marking标记操作的含义
将障碍物状态信息插入到成本图中，【防盗标记–盒子君hzj】标记操作通过索引到数组内修改单元格的代价Occupied、Free、Unknown Space
每个状态在投影到代价图中后都会有其特定的代价值。
1）具有一定数量的占用单元格的列（参阅参数）会被分配了costmap_2d :: LETHAL_OBSTACLE 代价值;
2）具有一定数量的未知单元格的列（参见参数）被分配了costmap_2d :: NO_INFORMATION 代价值；
3）剩余的其他列分配了costmap_2d :: FREE_SPACE 代价值。
（2）Clearing清除操作的含义
从代价中删除障碍物信息，每次观测报告都需要传感器源向外发射线。如果存储的障碍物信息是3D的，需要将每一列的障碍物信息投影成2D后才能放入到代价地图
（7）创建一层代价地图层的方法
（1）使用map server获取静态地图层
代价地图可以选择使用用户定义的静态地图来进行初始化，【防盗标记–盒子君hzj】如果该选项被选择，代价地图将使用service call从 map_server 获取静态地图
（2）自定义一层代价地图
实现一个层很容易。首先，必须创建一个扩展costmap 2d::层类的新类
步骤
1）在启动文件中创建静态转换发布器
<node name="static_tf0" pkg="tf" type="static_transform_publisher" args="2 0 0 0 0 0 /map /base_link 100"/>
2）创建一个将被加载到参数空间中的minimal.yaml文件，用于指定costmap的行为
3）如果要指定图层，其需要将字典存储在插件数组中，
需要扩展参数文件来指定障碍层的内容
plugins:
- {name: static_map, type: "costmap_2d::StaticLayer"}
- {name: obstacles, type: "costmap_2d::VoxelLayer"}
publish_frequency: 1.0
obstacles:
observation_sources: base_scan
base_scan: {data_type: LaserScan, sensor_frame: /base_laser_link, clearing: true, marking: true, topic: /base_scan}
（2）costmap_2d源码解读
（1）源码的框架
（1）【老板-公关剪彩的】costmap2dROS向外提供简洁的接口
void start();#订阅传感器话题，启动代价地图更新
void stop();#停止代价地图更新，并停止订阅传感器话题
void pause();#暂停代价地图的更新，但是不会关闭传感器话题的订阅
void resume();#恢复代价地图更新
void resetLayers();#重置每一个代价地图层
.
.
（2）【包工头-管理各层地图调用的】LayeredCostmap实例化不同的层插件并将它们聚合为一个代价值
声明一个插件集合std::vector<boost::shared_ptr > plugins_;在每个函数【防盗标记–盒子君hzj】里面都是遍历这个插件集合然后调用每一层的具体函数
void LayeredCostmap::updateMap(x,y,yaw)
updateBounds（）#LayeredCostmap调用此插件，以轮询需要更新的costmap的数量。每一层都可以增加这个边界的大小。
updateCosts（） #实际更新基础costmap，仅在UpdateBounds（）期间计算的范围内。
（3）【工人-干活的】以插件的形式创建不同的代价地图层plugins，并实现功能的
1）static_layer.cpp 【静态障碍物层】
静态地图层只是简单地将SLAM得到的灰度图中的像素值换算成ros代价地图中的代价值【防盗标记–盒子君hzj】
2）obstacle_layer.cpp 【动态障碍物层】
障碍物层是将传感器检测到的点云投影到地图中，不同的传感器来源会分别存到Observation类中。只要遍历存储Obsrvation的容器，【防盗标记–盒子君hzj】将检测到的点云的每个点分别投影到地图中，将对应的网格的代价值设为LETHAL_OBSTACLE，其实现函数在void ObstacleLayer::updateBounds(…)中
3）inflation_layer.cpp 【膨胀层】
遍历地图中所有障碍物的网格点，根据膨胀代价矩阵，修改障碍物网格点周围的网格点的代价值，最终实现膨胀的效果
4）voxel_layer.cpp 【将动态障碍物层分成三维障碍物层】
（2）源码程序流程
（1）【老板层】costmap的初始化流程【Costmap2DROS(const std::string& name, tf2_ros::Buffer& tf)中】
步骤：
（1）Costmap初始化首先获得全局坐标系和机器人坐标系的转换
（2）加载并初始化各个Layer【StaticLayer，ObstacleLayer，InflationLayer】
在move_base刚启动时就建立了两个costmap，而这两个costmap都加载了三个Layer插件
（3）设置机器人的轮廓
（4）实例化了一个Costmap2DPublisher来发布可视化数据。
（5）通过一个movementCB函数不断检测机器人是否在运动
（6）开启动态参数配置服务，服务启动了更新mapUpdateLoop的线程。
（2）【包工头+工人层】Costmap总图整体更新流程【void Costmap2DROS::mapUpdateLoop(double frequency)】
Costmap的更新在mapUpdateLoop线程中实现，以参数update_frequency 指定的周期进行更新
（阶段一）UpdateBounds：这个阶段会更新每个Layer的更新区域
【void Costmap2DROS::updateMap()中updateBounds（）】
通过每层的代价地图插件plugin进行遍历操作
（1）步骤一：每个周期传感器数据进来后，【防盗标记–盒子君hzj】都要在代价地图底层占用结构上执行标记和清除障碍操作，marking操作就是索引到数组内修改cell的代价，clearing操作清除该cell对应的诸多层中对应cell的状态
1)StaticLayer更新工作流程
updateBounds阶段将更新的界限设置为整张地图，updateCosts阶段根据rolling参数（是否采用滚动窗口）设置的值，如果是，那静态地图会随着机器人移动而移动，【防盗标记–盒子君hzj】则首先要获取静态地图坐标系到全局坐标系的转换，再更新静态地图层到master map里
2)ObstacleLayer更新工作流程
updateBounds阶段将获取传感器传来的障碍物信息经过处理后放入一个观察队列中，updateCosts阶段则将障碍物的信息更新到master map。
3)inflationLayer更新工作流程
updateBounds阶段由于本层没有维护的map，所以维持上一层地图调用的Bounds值（处理区域）。updateCosts阶段用了一个CellData结构存储master map中每个grid点的信息，其中包括这个点的二维索引和这个点附近最近的障碍物的二维索引。【防盗标记–盒子君hzj】改变每个障碍物CELL附近前后左右四个CELL的cost值，更新到master map就完成了障碍物的膨胀
（2）步骤二：对代价赋值为costmap_2d::LETHAL_OBSTACLE的每个cell执行障碍物的膨胀操作
（阶段二）UpdateCosts：这个阶段将各层数据逐一拷贝聚合到Master Map
【void Costmap2DROS::updateMap()中的updateCosts（）】【虚方法的应用】
通过每层的代价地图插件plugin进行遍历操作
这种结构会被投影到代价地图附上相应代价值
（阶段三）publishCostmap()：发布更新好的代价地图Master Map
.
.
.
总结
一张地图是包括很多图层叠加起来的：如slam建出来的静态点云图层、占据图层、代价图层、膨胀图层、欧氏距离场距离与梯度图层、可视化图层等等
这里仅仅根据ROS提供的costmap_2d功能包简单介绍一下生成代价图层等的原理，【防盗标记–盒子君hzj】SLAM也是一个大方向，这里直接用了slam结果提供的静态图做全局图
要不有空我开一个地图专栏相互学习学习~