概述
源码地址:
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核心的代码。我们先来看看核心算法部分的代码主要内容。
核心算法处理
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页)。
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使用 [公式] 这个指标来限制重采样次数。
其定义为: [公式]
当 [公式] 降低到某个阈值以下,说明粒子的分布与真实分布差距很大,具体表现为某些粒子离真实值很近,而很多粒子离真实值较远,重采样就应该在此时进行。
重采样的时间复杂度为 [公式] ,其中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