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算法可以填充由激光雷達的位置到障礙物之間的柵格概率了。