天天看點
傳回

ROS筆記：gmapping源碼分析概述核心算法處理

概述

ROS筆記：gmapping源碼分析概述核心算法處理

源碼位址：

https://github.com/ros-perception/openslam_gmapping

https://github.com/ros-perception/slam_gmapping

程式入口：main.cpp 

int
main(int argc, char** argv)
{
  ros::init(argc, argv, "slam_gmapping");
 
  SlamGMapping gn;
  gn.startLiveSlam();
  ros::spin();
 
  return(0);
}

定義生成gn時，運作了SlamGMapping::init()這個成員函數：

void SlamGMapping::init()
{
  // 省略... ...
  gsp_ = new GMapping::GridSlamProcessor(); //構造GridSlamProcessor類 利用粒子濾波
  ROS_ASSERT(gsp_);

  tfB_ = new tf::TransformBroadcaster(); //建立TransformBroadcaster
  ROS_ASSERT(tfB_);
  
  // 省略... ...
   
  // Parameters used by our GMapping wrapper
  if(!private_nh_.getParam("throttle_scans", throttle_scans_))
    throttle_scans_ = 1;
  if(!private_nh_.getParam("base_frame", base_frame_))
    base_frame_ = "base_link";
  if(!private_nh_.getParam("map_frame", map_frame_))
    map_frame_ = "map";
  if(!private_nh_.getParam("odom_frame", odom_frame_))
    odom_frame_ = "odom";

  private_nh_.param("transform_publish_period", transform_publish_period_, 0.05);

  double tmp;
  if(!private_nh_.getParam("map_update_interval", tmp))
    tmp = 5.0;
  map_update_interval_.fromSec(tmp);
  
  // Parameters used by GMapping itself #GMapping 所用的各種參數 
  maxUrange_ = 0.0;  maxRange_ = 0.0; // preliminary default, will be set in initMapper()
 
  // 省略... ...
}

SlamGMapping::startLiveSlam()中

（1）先訂閱雷射雷達的資料

（2）通過boost::bind開啟兩個線程：laserCallback()和 publishLoop()，

在核心線程laserCallback中 ：

laserCallback()
{
  initMapper()  //進行參數初始化
                //gsp_->GridSlamProcessor::init()。
  addScan()     //調用openslam_gmappinig核心算法 gsp_->processScan（）

  updateMap()   //對地圖進行更新
}

initMapper函數處理第一次收到雷射資料時候地圖的初始化問題，被laserCallback函數調用。

updateMap函數每超過map_update_interval_時間就要被laserCallback函數調用調用更新地圖。

在addScan函數中：

SlamGMapping::addScan(const sensor_msgs::LaserScan& scan, GMapping::OrientedPoint& gmap_pose)
{

//省略的對雷射資料的處理 ... ...

  GMapping::RangeReading reading(scan.ranges.size(),
                                 ranges_double,
                                 gsp_laser_,
                                 scan.header.stamp.toSec());
  delete[] ranges_double;
  reading.setPose(gmap_pose);
  ROS_DEBUG("processing scan");
  return gsp_->processScan(reading); 
}

調用了openslam裡面的GridSlamProcessor::processScan函數，這個是gmapping核心的代碼。我們先來看看核心算法部分的代碼主要内容。

核心算法處理

ROS筆記：gmapping源碼分析概述核心算法處理
bool GridSlamProcessor::processScan(const RangeReading & reading, int adaptParticles){
    
    /**retireve the position from the reading, and compute the odometry*/
    /*從reading中擷取odometry資料*/
    OrientedPoint relPose=reading.getPose();
    /*第0次是特殊處理,将目前裡程計值指派給上一次的值。relPose 表示裡程計算出來的目前位置*/
    /*m_odoPose 表示裡程計上一次的位置 m_count 表示更新次數*/
    if (!m_count){
      m_lastPartPose=m_odoPose=relPose;
    }
    
    //write the state of the reading and update all the particles using the motion model
    /*對于每一個粒子，都從裡程計運動模型中采樣*/
    for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
      OrientedPoint& pose(it->pose);
      // Step.1※※裡程計運動模型問題※※
      pose=m_motionModel.drawFromMotion(it->pose, relPose, m_odoPose);
    }

    // update the output file
    if (m_outputStream.is_open()){
		//省略 。。。 。。。
    }
    
    //invoke the callback
    onOdometryUpdate();
    
    // accumulate the robot translation and rotation
    // 算出裡程計在這個周期内跑了多遠
    OrientedPoint move=relPose-m_odoPose;
    move.theta=atan2(sin(move.theta), cos(move.theta));
    m_linearDistance+=sqrt(move*move);
    m_angularDistance+=fabs(move.theta);
    
    // if the robot jumps throw a warning
    // 如果算出跑的距離超過了門檻值，就報警
    if (m_linearDistance>m_distanceThresholdCheck){
       //省略 。。。 。。。
    }   
     //下次疊代開始
    m_odoPose=relPose;
    bool processed=false;

    // process a scan only if the robot has traveled a given distance
    // 如果裡程計大于某個門檻值，就處理一批雷射資料。可參照視覺SLAM關鍵幀的内在含義   
    if (! m_count 
	|| m_linearDistance>m_linearThresholdDistance 
	|| m_angularDistance>m_angularThresholdDistance){
      
      //省略 。。。 。。。
     
      //this is for converting the reading in a scan-matcher feedable form
      //把雷射雷達資料轉換成openslam_gmapping可以讀懂的格式     
      assert(reading.size()==m_beams);
      double * plainReading = new double[m_beams];
      for(unsigned int i=0; i<m_beams; i++){
		plainReading[i]=reading[i];
      }
      m_infoStream << "m_count " << m_count << endl;
      if (m_count>0){ //如果不是第一幀雷射資料
	      scanMatch(plainReading); //Step.2 為每個粒子進行scanMatch，計算出每個粒子的最優位姿
       //省略 。。。 。。。
	
	onScanmatchUpdate();
	//Step.3 由于scanmatch之中對粒子的權重進行了更新，那麼這時候 各個粒子的軌迹上的累計權重都需要重新計算
	updateTreeWeights(false);
    //省略 。。。 。。。
    // Step.4 重采樣
	resample(plainReading, adaptParticles);
	
      } else { //如果是第一幀雷射資料，直接計算ActiveArea，此時機器人的位姿都是（0，0，0）
		for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){	
		  m_matcher.invalidateActiveArea();
		  m_matcher.computeActiveArea(it->map, it->pose, plainReading);
		  m_matcher.registerScan(it->map, it->pose, plainReading);
		  
		  // cyr: not needed anymore, particles refer to the root in the beginning!
	    // 這是軌迹的根，沒有父節點
		  TNode* node=new	TNode(it->pose, 0., it->node,  0);
		  node->reading=0;
		  it->node=node;
		}
      }
      updateTreeWeights(false);
      delete [] plainReading;
      m_lastPartPose=m_odoPose; //update the past pose for the next iteration
      m_linearDistance=0;
      m_angularDistance=0;
      m_count++;
      processed=true;
      
      //keep ready for the next step
      for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
		it->previousPose=it->pose;
      }
    }
    if (m_outputStream.is_open())
      m_outputStream << flush;
    m_readingCount++;
    //bool 型函數，處理完畢processed=true。
    return processed;
  }

core.1 運動模型

先來看一下 在motionmodel.cpp檔案中的drawFromMotion這個函數，它實作了裡程計運動模型，采用如下圖的公式（取自中文版《機率機器人》第103頁）。

ROS筆記：gmapping源碼分析概述核心算法處理

core.2 權重計算

接下來看一下GMapping::GridSlamProcessor::scanMatch這個函數。

scanMatch()：實作通過掃描比對每一個粒子，采樣出新的位姿，進行權重計算。利用最近一次的觀測來提高Proposal distribution進行優化。

rbpf-gmapping直接使用的是運動模型作為提議分布

score()：計算最優的粒子，原理就參考 《Probabilistic Robot》 一書的P143 頁 ， likelihood_field_range_finder_mode。

在score 函數裡，首先計算障礙物的坐标phit，然後将phit轉換成網格坐标iPhit

計算光束上與障礙物相鄰的非障礙物網格坐标pfree,pfrree由phit沿雷射束方向移動一個網格的距離得到，将pfree轉換成網格坐标ipfree（增量，并不是實際值）

在iphit 及其附近8個（m_kernelSize:default=1）栅格（pr,對應自由栅格為pf）搜尋最優可能是障礙物的栅格。

最優準則： pr 大于某一門檻值，pf小于該門檻值，且pr栅格的phit的平均坐标與phit的距離bestMu最小。

得分計算： s +=exp(-1.0/m_gaussianSigmabestMubesMu) 參考NDT算法,距離越大，分數越小，分數的較大值集中在距離最小值處，符合正态分布模型

至此 score 函數結束并傳回粒子（currentPose）得分，然後回到optimize函數

optimize() 幹的事就是 currentPose 的位姿進行微調，前、後、左、右、左轉、右轉 共6次，然後選取得分最高的位姿，傳回最終的得分

likelihoodAndScore()：重新計算權重。

/**Just scan match every single particle.
If the scan matching fails, the particle gets a default likelihood.*/
inline void GridSlamProcessor::scanMatch(const double* plainReading){
  // sample a new pose from each scan in the reference
  //  雷射資料以plainReading的封裝形式加入到函數當中來
  double sumScore=0;
  for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
    OrientedPoint corrected;
    double score, l, s;
    //optimize 函數傳回了bestScore，同時計算了corrected，即新位姿
    score=m_matcher.optimize(corrected, it->map, it->pose, plainReading);
    //    it->pose=corrected;
    // 當計算出的分數比較靠譜後，就用corrected值更新目前粒子的位姿
    if (score>m_minimumScore){
      it->pose=corrected;
    } else {
		 //省略。。。 。。。
    }
    //likelihoodAndSocre 傳回了l的數值用于計算粒子的權重
    m_matcher.likelihoodAndScore(s, l, it->map, it->pose, plainReading);
    sumScore+=score;
    it->weight+=l;
    it->weightSum+=l;

    //set up the selective copy of the active area
    //by detaching the areas that will be updated
    m_matcher.invalidateActiveArea(); // invalidateActiveArea是這個函數的開關
    //建圖的過程，生成地圖的函數，使用Bresenham算法
    // computeActiveArea函數的作用有兩個：一是調整地圖：把每個粒子所對應的掃描範圍給計算出來，然後調整地圖的大小使得上述掃描範圍不會超過地圖的大小。
    // 二是計算活動區域：調用gridLine() 函數将所有粒子與其所對應的phit(雷射打到障礙物上的終點)連接配接起來，進而形成一片activeArea。
    m_matcher.computeActiveArea(it->map, it->pose, plainReading);
  }
}

core.3 權重更新

由于scanmatch之中對粒子的權重進行了更新，那麼這時候 各個粒子的軌迹上的累計權重都需要重新計算, 是以我們需要用updateTreeWeights子函數進行粒子權重的更新。

void  GridSlamProcessor::updateTreeWeights(bool weightsAlreadyNormalized){

  if (!weightsAlreadyNormalized) {
    normalize();//歸一化粒子權重,并計算權重相似度Neff，用于判定是否需要進行重采樣
  }
  resetTree();//重置軌迹樹：周遊了所有的粒子，并沿着各個粒子的node節點向上追溯到根節點，
//并将周遊過程中各個節點的權重計數和通路計數器都設定為0。
  propagateWeights();//更新軌迹樹權重：從葉子節點向上追溯到根節點，更新沿途經過的各個節點的權重和累積權重
}

詳細可參考：https://gaoyichao.com/Xiaotu

core.4 選擇性重采樣

processScan 這個函數調用了位于gridslamprocessor.hxx中的resample子函數，其代碼如下：

registerScan()：配置設定記憶體，生成地圖

/**
頻繁的重采樣導緻粒子退化，曆史久遠的位姿喪失其自身的多樣性
plainReading 是雷射的原始資料；
adaptSize用于調節重采樣的粒子數目；
reading包含了帶時間戳的大量傳感器資訊
**/
inline bool GridSlamProcessor::resample(const double* plainReading, int adaptSize, const RangeReading* ){
  
  bool hasResampled = false; //條件開關，兼職傳回值
  
  TNodeVector oldGeneration; //軌迹樹向量的容器，記錄上一代粒子狀态
  for (unsigned int i=0; i<m_particles.size(); i++){
    oldGeneration.push_back(m_particles[i].node); //上一代粒子
  }
  /**
  * 門檻值由這個積定義，當函數updateTreeWeights中定義的Neff 小于此門檻值，則開始重采樣
  **/
  if (m_neff<m_resampleThreshold*m_particles.size()){		
   	//采取重采樣方法決定，哪些粒子會保留，保留的粒子會保留下标，有些粒子會重複采樣
	//而另外一些粒子會消失掉 
    //uniform_resampler是結構體模闆定義的重采樣器，在particle.h中定義，包含resampleIndexes等成員
    uniform_resampler<double, double> resampler;
    m_indexes=resampler.resampleIndexes(m_weights, adaptSize);
    //省略 。。。 。。。  
    onResampleUpdate(); //與算法無關
    // 開始對對粒子的更新處理
    //BEGIN: BUILDING TREE
    ParticleVector temp; //重采樣後的粒子集合
    unsigned int j=0;
    std::vector<unsigned int> deletedParticles;  		//this is for deleteing the particles which have been resampled away.
    //		不在選擇索引中的粒子被幹掉
    for (unsigned int i=0; i<m_indexes.size(); i++){
      while(j<m_indexes[i]){
	deletedParticles.push_back(j);
	j++;
			}
      if (j==m_indexes[i])
	j++;
// 更新軌迹樹中粒子的最新狀态
      Particle & p=m_particles[m_indexes[i]];
      TNode* node=0;
      TNode* oldNode=oldGeneration[m_indexes[i]];
      node=new	TNode(p.pose, 0, oldNode, 0);
      node->reading=0;
// 記錄到temp粒子集合中        
      temp.push_back(p);
      temp.back().node=node;
      temp.back().previousIndex=m_indexes[i];
    }
    while(j<m_indexes.size()){ //釋放記憶體
      deletedParticles.push_back(j);
      j++;
    }
    for (unsigned int i=0; i<deletedParticles.size(); i++){
      std::cerr <<" " << deletedParticles[i];
      delete m_particles[deletedParticles[i]].node;
      m_particles[deletedParticles[i]].node=0;
    }
    
    //END: BUILDING TREE
    // 重采樣後的最新粒子資料去更新比對器中的各種資料，如地圖、位姿、雷射等
    m_particles.clear();
    for (ParticleVector::iterator it=temp.begin(); it!=temp.end(); it++){
      it->setWeight(0);
      m_matcher.invalidateActiveArea();
      m_matcher.registerScan(it->map, it->pose, plainReading);
      m_particles.push_back(*it);
    }
    hasResampled = true;
  } else {
/** 
如果不滿足重采樣條件，則依舊需要更新 1.各個粒子軌迹樹；2.比對器中的各種資料，如地圖、位姿、雷射、活動區域等   
**/ 
    int index=0;
    TNodeVector::iterator node_it=oldGeneration.begin();
    for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
      //create a new node in the particle tree and add it to the old tree
      //BEGIN: BUILDING TREE  
      TNode* node=0;
      node=new TNode(it->pose, 0.0, *node_it, 0);
      
      node->reading=0;
      it->node=node;

      //END: BUILDING TREE
      m_matcher.invalidateActiveArea();
      m_matcher.registerScan(it->map, it->pose, plainReading);
      it->previousIndex=index;
      index++;
      node_it++;
      
    }
  }
  //END: BUILDING TREE
  
  return hasResampled;
}

頻繁的重采樣導緻粒子退化，曆史久遠的位姿喪失其自身的多樣性。GMapping使用 [公式] 這個名額來限制重采樣次數。

其定義為： [公式]

ROS筆記：gmapping源碼分析概述核心算法處理

當 [公式] 降低到某個門檻值以下，說明粒子的分布與真實分布差距很大，具體表現為某些粒子離真實值很近，而很多粒子離真實值較遠，重采樣就應該在此時進行。

重采樣的時間複雜度為 [公式] ，其中N為粒子數，M為地圖大小，是以采用用[公式] 這個名額來限制重采樣次數意義重大。

參考論文:

Improved Techniques for Grid Mapping with Rao-Blackwellized Particle Filters

參考文獻：

https://zhuanlan.zhihu.com

https://blog.csdn.net/

https://gaoyichao.com/Xiaotu/?

原理參考

http://blog.csdn.net/heyijia0327/article/details/40899819 pf原理講解

http://blog.csdn.net/u010545732/article/details/17462941 pf代碼實作

http://www.cnblogs.com/yhlx125/p/5634128.html gmapping分析

http://wenku.baidu.com/view/3a67461550e2524de4187e4d.html?from=search gmapping 分析

其他參考 ：

http://ishare.iask.sina.com.cn/f/24615049.html

雷射SLAM ros入門到精通 自動駕駛 機器學習 深度學習 SLAM
上一篇: SpringCloud(四)Ribbon自定義負載均衡
下一篇: SpringCloud系列八：自定義Ribbon配置

繼續閱讀