占据栅格图octomap
在传统分阶段(感知-建图-规划-优化)任务中,环境建图是非常重要的一环。octomap是一个开源的三维环境建模库,其基于八叉树octree实现,可以将三维点云数据转换为三维地图
以Ubuntu 20.04 + ROS noetic为例,安装过程如下
1 | sudo apt-get update |
确保机器人有能提供三维点云数据的传感器
octomap在使用时是作为一个ROS节点而存在,其输入为点云+定位信息,输出占据栅格+可视化标记。最基本的启动方式为roslaunch octomap_server octomap_mapping.launch,可以启动rviz添加octomap,选择/octomap_full进行查看
一般可以通过launch文件启动,便于配置参数
1 | <launch> |
使用D435 的点云
d435虽然本身只能输出RGB-D图像,但是其内置的驱动可以通过计算输出点云数据,且该点云可以附带颜色,操作方式如下
1 | roslaunch realsense2_camera rs_camera.launch filters:=pointcloud |
但是显然,由深度相机所得到的点云缺点很明显,首先是距离近,距离如果太远了相机的精度下降,其内容不可信,如果强行纳入计算则原处的点可能波动很大;其次是频率低,该点云的输出频率依赖于相机图像的频率;最后,由于依赖于深度图像,当面对如纯白墙面或者昏暗环境等深度相机不起作用的情况,也无法输出点云。可见,通过这种方式得到点云肯定是比不上由激光雷达得到点云。
占据栅格图的使用
除了直接在rviz中订阅地图来查看生成的占据栅格图,要如何使用地图来进行路径规划?
二维地图
数据结构
二维占据地图projected_map,表示二维地图上障碍物的位置,是octomap_server默认发布的话题,在CPP中需要#include <nav_msgs/OccupancyGrid.h>。
其数据类型为nav_msgs::OccupancyGrid,其具体数据结构有以下三部分
- header
- info
- data
其中:
- header.frame_id
表示地图坐标系,通常这里是 map。你工程里 octomap_server 也把 frame_id 设成了 map,见 src/astar_nav/launch/octomap.launch:5。 - info.resolution
每个栅格边长,单位米/格。 - info.width, info.height
地图宽高,单位是格子数,不是米。 - info.origin
栅格 (0,0) 在世界坐标里的原点位姿。通常可理解为地图左下角对应的世界坐标。 - data
一个一维 int8[] 数组,长度等于 width * height。
数组索引通常按行展开,访问方式就是: index = y * width + x。其中值为-1~100的整数,-1代表未知,0代表空闲,100表示占据,数字越大表示该位置被占用的概率越大
数据使用
- 订阅话题并设置回调函数
首先需要订阅话题/projected_map,并使用回调函数将数据传给同类型的类内变量raw_map;同时在回调函数中进行地图膨胀,而膨胀后的地图可以用std::vector<int8_t> inflated_data_表示。 - 地图膨胀
为了让无人机与障碍物保持安全距离,需要对地图进行膨胀。参考代码如下1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33void Planner::inflateMap(){
int w = raw_map_.info.width;
int h = raw_map_.info.height;
double res = raw_map_.info.resolution;
int radius = static_cast<int>(std::ceil(safety_dist_ / res));
inflated_data_ = raw_map_.data;
for (int y = 0; y < h; ++y)
{
for (int x = 0; x < w; ++x)
{
// 原始地图中该格为占据
if (raw_map_.data[y * w + x] > 50)
{
// 将周围 radius 范围内的格子都标记为占据
for (int dy = -radius; dy <= radius; ++dy)
{
for (int dx = -radius; dx <= radius; ++dx)
{
int nx = x + dx;
int ny = y + dy;
if (nx >= 0 && nx < w && ny >= 0 && ny < h)
{
if (dx * dx + dy * dy <= radius * radius)
inflated_data_[ny * w + nx] = 100;
}
}
}
}
}
}
} - 路径规划
使用得到的膨胀后的二维地图进行路径规划
三维地图
参考
ROS中的三维地图表示:OctoMap详解及应用-CSDN博客
nav_msgs::OccupancyGrid数据格式详解-CSDN博客