概率機器人總結——占用柵格地圖先實踐再推導

• 概率機器人總結——占用柵格地圖構建先實踐再推導
• • 實踐過程
  • • 偽代碼分析
    • 真代碼分析
  • 推導過程
  • • 靜態二值貝葉斯濾波

概率機器人總結——占用柵格地圖構建先實踐再推導

當我將概率機器人看到這里的時候，越發覺將數學理論轉到實際應用是一件非常有意思的事情，像我的話很早之前就用過gmapping和amcl這些ros自帶的功能包了，但是知其然不知其所以然，看起來很炫酷的操作卻不明白其背后的原理是什么，通過這一段時間的學習總結，對此有了一個更加深入的認識，很爽的。

實踐過程

偽代碼分析

這里我先放上來《概率機器人》中的偽代碼

其中$m_i$就是我們見到的柵格，$l_{t,i}$這個東西叫對數占用概率，由后驗概率表示如下：$$l_{t,i}=\log \frac{p\left(m_i|z_{1,t},x_{1,t}\right)}{1?p\left(m_i|z_{1,t},x_{1,t}\right)}$$當然反過來，當我們更新對數占用概率后可以用其恢復后驗概率，如下：$$p\left(m_i|z_{1,t},x_{1,t}\right)=1?\frac{1}{1+\exp \left\{l_{t,i}\right\}}$$這個讓我聯想到在視覺SLAM里面用到的李群和李代數的關系，我們回到偽代碼，第3-7行表示只對機器人感知范圍內的柵格進行更新對數占用概率的更新，不在范圍內的不進行跟新，在更新的過程中有一個inverse_sensor_model函數，這個是什么呢？看下面偽代碼：

看起來會稍微復雜一些，但其實很簡單，結合下面這張圖解釋：

首先我們通過觀測獲得的數據的坐標系肯定是基于我們傳感器的，第2-5行可以理解為將其轉到世界坐標系下，給定一個傳感器觀測的范圍，也就是上圖中的白色區域，第6-12行表示的是，如果我們給定的一個柵格$m_i$位于白色區域就返回一個固定值$l_{free}$，如果位于黑色區域就返回一個固定值$l_{occu}$，如果位于灰色區域就返回一個值$l_0$，再下面實際的程序里氛圍設置為了0.4，0.6和0.5。

真代碼分析

PythonRobotics里面關于mapping的Demo太過簡單，因此我在知乎上搜到了一個北航小哥寫的文章和代碼，推導清晰，代碼也很簡單，一看就懂，先附上鏈接Grid Mapping 占用柵格地圖構建實現，然后粘幾段代碼過來看一下：

void GridMapper::updateMap ( const sensor_msgs::LaserScanConstPtr& scan, Pose2d& robot_pose ) {/* 獲取激光的信息 */const double& ang_min = scan->angle_min;const double& ang_max = scan->angle_max;const double& ang_inc = scan->angle_increment;const double& range_max = scan->range_max;const double& range_min = scan->range_min;/* 設置遍歷的步長，沿著一條激光線遍歷 */const double& cell_size = map_->getCellSize();const double inc_step = 1.0 * cell_size;/* for every laser beam */for(size_t i = 0; i < scan->ranges.size(); i ++){/* 獲取當前beam的距離 */double R = scan->ranges.at(i); if(R > range_max || R < range_min)continue;/* 沿著激光射線以inc_step步進，更新地圖*/double angle = ang_inc * i + ang_min;double cangle = cos(angle);double sangle = sin(angle);Eigen::Vector2d last_grid(Eigen::Infinity, Eigen::Infinity); //上一步更新的grid位置，防止重復更新for(double r = 0; r < R + cell_size; r += inc_step){Eigen::Vector2d p_l(r * cangle,r * sangle); //在激光雷達坐標系下的坐標/* 轉換到世界坐標系下 */Pose2d laser_pose = robot_pose * T_r_l_;Eigen::Vector2d p_w = laser_pose * p_l;/* 更新這個grid */if(p_w == last_grid) //避免重復更新continue;updateGrid(p_w, laserInvModel(r, R, cell_size));last_grid = p_w;}//for each step}// for each beam }

這個是最主要的代碼段，這個函數就是從激光的數據結構中遍歷每一條激光線，然后以柵格長度的步長去遍歷這條激光線上的柵格，然后將各個柵格的坐標從雷達坐標系下轉到世界坐標系下，然后逐個更新柵格被占用的后驗概率，跟新概率這兩個函數如下：

void GridMapper::updateGrid ( const Eigen::Vector2d& grid, const double& pmzx ) {/* TODO 這個過程寫的太低效了 */double log_bel;if( ! map_->getGridLogBel( grid(0), grid(1), log_bel ) ) //獲取log的belreturn;log_bel += log( pmzx / (1.0 - pmzx) ); //更新map_->setGridLogBel( grid(0), grid(1), log_bel ); //設置回地圖 } double GridMapper::laserInvModel ( const double& r, const double& R, const double& cell_size ) {if(r < ( R - 0.5*cell_size) )return P_free_;if(r > ( R + 0.5*cell_size) )return P_prior_;return P_occ_; }

這個laserInvModel函數相當于上面偽代碼里面的inverse_sensor_model，但是要簡單很多，就是判定該柵格距離雷達中心所在位置，而updateGrid函數運行的就是第一段偽代碼所示的步驟，對應著看就好了，代碼在我電腦上運行效果如下（看上去和gmapping差不多了，gmapping也是不帶回環檢測的）：

推導過程

上面的效果看到了，接下來需要通過推導來看看為什么這么做是好使的，占用柵格地圖的構建是基于靜態二值貝葉斯濾波實現的。

靜態二值貝葉斯濾波

這里基本的貝葉斯濾波方程入手：$$\begin{array}{rl}p\left(x|z_{1,t}\right)& =\frac{p\left(z_t|x,z_{1,t?1}\right)p\left(x|z_{1,t?1}\right)}{p\left(z_t|z_{1,t?1}\right)}\\ & =\frac{p\left(z_t|x\right)p\left(x|z_{1,t?1}\right)}{p\left(z_t|z_{1,t?1}\right)}\end{array}$$這里$p\left(z_t|x\right)$可以稱作觀測方程，也可以成為似然函數， 而$p\left(x|z_{1,t?1}\right)$可以成為運動方程，也可以成為先驗值，因為是靜態的所以我們省去了$u_t$，$p\left(z_t|z_{1,t?1}\right)$我們一般視為一個常數。現在我們對測量方程再運用一次貝葉斯法則有：$$p\left(x|z_{1,t}\right)=\frac{p\left(x|z_t\right)p\left(z_t\right)p\left(x|z_{1,t?1}\right)}{p(x)p\left(z_t|z_{1,t?1}\right)}$$這里出現了一個關鍵的分布$p\left(x|z_t\right)$，稱為反向觀測模型，注意和我們的后驗$p\left(x|z_{1,t}\right)$是不同的，那么前向觀測模型當然指的是$p\left(z_t|x\right)$，那么反向觀測模型和前向觀測模型有什么區別呢？為什么要這樣做？反向觀測模型可以理解為設計一個函數根據相機圖像來計算門為關著的概率，而前向觀測模型指的是描述所有相機圖像中顯示門為關著的分布更容易些。前者當然要簡單些，那為什么普通的貝葉斯濾波不這么做呢? 因為$p\left(z_t\right)$我們不知道呀～
然后同理求其對立事件$?x$的分布$$p\left(?x|z_{1,t}\right)=\frac{p\left(?x|z_t\right)p\left(z_t\right)p\left(?x|z_{1,t?1}\right)}{p(?x)p\left(z_t|z_{1,t?1}\right)}$$相除得$$\begin{array}{rl}\frac{p\left(x|z_{1,t}\right)}{p\left(?x|z_{1;t}\right)}& =\frac{p\left(x|z_t\right)}{p\left(?x|z_t\right)}\frac{p\left(x|z_{1,t?1}\right)}{p\left(?x|z_{1;t?1}\right)}\frac{p(?x)}{p(x)}\\ & =\frac{p\left(x|z_t\right)}{1?p\left(x|z_t\right)}\frac{p\left(x|z_{1,t?1}\right)}{1?p\left(x|z_{1,t?1}\right)}\frac{1?p(x)}{p(x)}\end{array}$$因為是二值所以上式才成立，然后我們取對數得$$\begin{array}{rl}{l}_t(x)& =\log \frac{p\left(x|z_t\right)}{1?p\left(x|z_t\right)}+\log \frac{p\left(x|z_{1;t?1}\right)}{1?p\left(x|z_{1,t?1}\right)}+\log \frac{1?p(x)}{p(x)}\\ & =\log \frac{p\left(x|z_t\right)}{1?p\left(x|z_t\right)}?\log \frac{p(x)}{1?p(x)}+l_{t?1}(x)\end{array}$$其中$l_t(x)$就是我們定義的概率對數表達式，是我們遞歸二值后驗的一個中間量：$$l(x):=\log \frac{p(x)}{1?p(x)}$$然后我們的結論是，二值靜態貝葉斯濾波按照如下方式遞歸：$$l_t=l_{t?1}+\log \frac{p\left(x|z_t\right)}{1?p\left(x|z_t\right)}?\log \frac{p(x)}{1?p(x)}$$
接下來我們將靜態二值貝葉斯濾波應用到占用柵格地圖的構建中來：所謂地圖構建問題就是根據位置和觀測建立地圖：$$p\left(m|z_{1:t},x_{1:t}\right)$$標準的占用柵格方法是按照柵格將其分為一些獨立問題：$$p\left(m|z_{1,t},x_{1,t}\right)=\prod _{i}p\left(m_i|z_{1,t},x_{1,t}\right)$$然后我們在柵格上建立對數占用概率表達方式：$$l_{t,i}=\log \frac{p\left(m_i|z_{1,t},x_{1,t}\right)}{1?p\left(m_i|z_{1,t},x_{1,t}\right)}$$然后就利用上述的靜態二值貝葉斯濾波進行遞歸就好了，帶有反向觀測模型的部分就變成了：$$inverse?sensor?model\left(m_i,x_t,z_t\right)=\log \frac{p\left(m_i|z_t,x_t\right)}{1?p\left(m_i|z_t,x_t\right)}$$其實就是根據當前的位置和所觀測到的情況給柵格賦值，對應了上述的操作，最后我們每迭代一次恢復成將對數占用概率恢復成后驗就知道當前柵格是否被占用的概率。

總結完畢，歡迎交流～

此外，對SLAM算法感興趣的同學可以看考我的博客SLAM算法總結——經典SLAM算法框架總結

總結

以上是生活随笔為你收集整理的概率机器人总结——占用栅格地图先实践再推导的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。