ROS导航之地图costmap_2d与bresenham算法

读者可以参读http://wiki.ros.org/costmap_2d

 

costmap_2d：

无论是激光雷达还是如kinect 或xtion pro深度相机作为传感器跑出的2D或3D SLAM地图，都不能直接用于实际的导航，必须将地图转化为costmap(代价地图),ROS的costmap通常采用grid(网格)形式。以前一直没有搞明白每个栅格的概率是如何算出来的，原因是之前一直忽略了内存的存储结构，栅格地图一个栅格占1个字节，也就是八位，可以存0-255中数据，也就是每个cell cost（网格的值）从0~255我们只需要三种情况：Occupied被占用（有障碍）, Free自由区域（无障碍）,  Unknown Space未知区域。笔者根据以下第二个图进行形象的讲解。这个值究竟是什么，接下来会讲到。

bresenham算法：

在介绍costmap_2d之前，要先介绍个算法bresenham算法，Bresenham直线算法是用来描绘由两点所决定的直线的算法，它会算出一条线段在n维光栅上最接近的点。这个算法只会用到较为快速的整数加法、减法和位元移位，常用于绘制电脑画面中的直线。是计算机图形学中最先发展出来的算法。

 

过各行各列象素中心构造一组虚拟网格线如上图。按直线从起点到终点的顺序计算直线与各垂直网格线的交点，然后根据误差项的符号确定该列象素中与此交点最近的象素。

 

核心思想：

假设：k=dy/dx。因为直线的起始点在象素中心，所以误差项d的初值d0＝0。X下标每增加1，d的值相应递增直线的斜率值k，即d＝d＋k。一旦d≥1，就把它减去1，这样保证d在0、1之间。当d≥0.5时，最接近于当前象素的右上方象素（x+1,y+1）而当d<0.5时，更接近于右方象素(x+1,y）为方便计算，令e＝d-0.5，e的初值为-0.5，增量为k。当e≥0时，取当前象素（xi，yi）的右上方象素（x+1,y+1）而当e<0时，更接近于右方象素(x+1,y）可以改用整数以避免除法。由于算法中只用到误差项的符号，因此可作如下替换：e1 = 2*e*dx

 

算法推导：

 

假设我们需要由(x₀,y₀)这一点，绘画一直线至右下角的另一点(x₁,y₁)，x,y分别代表其水平及垂直座标，并且x₁-x₀>y₁-y₀。在此我们使用计算机视觉常用坐标系即，x座标值沿x轴向右增长，y座标值沿y轴向下增长。

 

因此x及y之值分别向右及向下增加，而两点之水平距离为 且垂直距离为y₁-y₀。由此得之，该线的斜率必定介乎于1至0之间。而此算法之目的，就是找出在与之间，第x行相对应的第y列，从而得出一像素点，使得该像素点的位置最接近原本的线。

 

对于由(x₀,y₀)及(x₁,y₁)两点所组成之直线，公式如下：

 

 

 因此，对于每一点的x，其y的值是

 

 

因为x及y皆为整数，但并非每一点x所对应的y皆为整数，故此没有必要去计算每一点x所对应之y值。反之由于此线之斜率介乎于1至0之间，故此我们只需要找出当x到达那一个数值时，会使y上升1，若x尚未到此值，则y不变。至于如何找出相关的x值，则需依靠斜率。斜率之计算方法为。由于此值不变，故可于运算前预先计算，减少运算次数。

 

要实行此算法，我们需计算每一像素点与该线之间的误差。于上述例子中，误差应为每一点x中，其相对的像素点之y值与该线实际之y值的差距。每当x的值增加1，误差的值就会增加m。每当误差的值超出0.5，线就会比较靠近下一个映像点，因此y的值便会加1，且误差减1。

下列伪代码是这算法的简单表达（其中的plot(x,y)绘画该点，abs返回的是绝对值）。虽然用了代价较高的浮点运算，但很容易就可以改用整数运算。

function line(x0, x1, y0, y1)
     int deltax := x1 - x0
     int deltay := y1 - y0
     real error := 0
     real deltaerr := deltay / deltax    // 假設deltax != 0（非垂直線），
           // 注意：需保留除法運算結果的小數部份
     int y := y0
     for x from x0 to x1
         plot(x,y)
         error := error + deltaerr
         if abs (error) ≥ 0.5 then
             y := y + 1
             error := error - 1.0

 

　　

costmap_2d：

 

以激光雷达为传感器（或者kinect之类的深度相机的伪激光雷达），根据激光测量的障碍物距离机器人中心的距离，结合机器人的内切和外切半径，搞一个映射，例如，机器人内切半径为0.5m，外切半径为0.7m，当激光返回障碍距离在机器人中心附近，叫致命障碍，机器人一定能碰到障碍物，比如说0m,直接贴着机器人，或会取小于栅格的边长，比如小于0.1m范围内,则这个栅格值就设为254；当返回来的数值在0.1-0.5m之间，就设253；当在0.5-0.7之间，则可以设128，或者在252-128找个比例值（程序中可以控制），这些是被占用（有障碍）区域；当0.7-膨胀半径之间，设1-127之间的映射值，可能碰撞障碍物；当大于膨胀距离，则设为0，称为freespace。Unknown -- 意味着给定的单元没有相应的信息。

3d的结合z轴信息，进行设置，把这些区域归一化，就得到了每个栅格的概率。

 

costmap是分层的：比如静态地图是一层，障碍物是一层，我自己定义的障碍物是一层（假如我不想让机器人通过一个freespace就可以自己插入个障碍物主要的接口是costmap_2d::Costmap2DROS，每一层中使用pluginlib实例化Costmap2DROS和每一层都被添加到LayeredCostmap）各个层可以被独立的编译。

 

利用bresenham算法可以填充由激光雷达的位置到障碍物之间的栅格概率了。