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
算法推导:
假设我们需要由(x0,y0)这一点,绘画一直线至右下角的另一点(x1,y1),x,y分别代表其水平及垂直座标,并且x1-x0>y1-y0。在此我们使用计算机视觉常用坐标系即,x座标值沿x轴向右增长,y座标值沿y轴向下增长。
因此x及y之值分别向右及向下增加,而两点之水平距离为 且垂直距离为y1-y0。由此得之,该线的斜率必定介乎于1至0之间。而此算法之目的,就是找出在
与
之间,第x行相对应的第y列,从而得出一像素点,使得该像素点的位置最接近原本的线。
对于由(x0,y0)及(x1,y1)两点所组成之直线,公式如下:
因此,对于每一点的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算法可以填充由激光雷达的位置到障碍物之间的栅格概率了。