沉默杀手HomeArchivesTagsCategories
(六) 源码分析4 processScan函数
|激光SLAMgmapping|
Word count: 2k|Reading time: 7 min

当前位姿与上一次位姿做差，计算做累计角度偏差和位移偏差。利用激光雷达测得距离做得分处理。非首帧调用scanMatch，upDateTreeWeight,resample。首帧则调用invalidActiveArea,computeActiveArea,registerScan。

第1部分

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
bool GridSlamProcessor::processScan(const RangeReading & reading, int adaptParticles)
{
    /* retireve the position from the reading, and compute the odometry */
    /*当前雷达在里程计坐标系的位置  addScan最后的setPose */
    OrientedPoint relPose = reading.getPose();

    /*m_count表示这个函数被调用的次数，开始为0,如果是第0次调用,则所有的位姿都是一样的*/
    if (!m_count)
	   m_lastPartPose = m_odoPose = relPose;
    
    /*对于每一个粒子，都从里程计运动模型中采样，得到车子的初步估计位置  这一步对应里程计的更新 */
    int tmp_size = m_particles.size();

    //这个for循环显然可以用 OpenMP 进行并行化
    for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++)
    { 
        OrientedPoint& pose(it->pose);      // 上一时刻粒子的位姿
        // relPose是里程计记录的最新位姿， m_odoPose是建图引擎记录的上一时刻的位姿
        // pose就是it->pose加了噪声，由于上面是引用，所以m_particles容器的成员pose全更新为pose
        pose=m_motionModel.drawFromMotion(it->pose, relPose, m_odoPose);
    }
    onOdometryUpdate();  /*回调函数，什么都没做，可以自己修改*/
    
    // 根据两次里程计的数据,计算机器人的线性位移和角度位移的累积值
    // m_odoPose表示上一次的里程计位姿   relPose表示新的里程计的位姿
    OrientedPoint move = relPose - m_odoPose;
    move.theta=atan2(sin(move.theta), cos(move.theta));

    // 统计机器人在进行激光雷达更新之前， 走了多远的距离  以及　平移了多少的角度
    // 这两个变量最后还是要清零
    m_linearDistance+=sqrt(move*move);   // x²+y²的开方
    m_angularDistance+=fabs(move.theta);

    // if the robot jumps throw a warning
    /*
     如果机器人在走了m_distanceThresholdCheck这么远的距离都没有进行激光雷达的更新
     则需要进行报警。这个误差很可能是里程计或者激光雷达的BUG造成的。
     例如里程计数据出错 或者 激光雷达很久没有数据等等
     每次进行激光雷达的更新之后 m_linearDistance这个参数就会清零
      m_distanceThresholdCheck在开头定义为5 */
    if (m_linearDistance > m_distanceThresholdCheck)
    {
			/*------- 一堆报警内容 -------*/
    }
    //更新:把当前的位置赋值给旧的位置
    m_odoPose = relPose;
     //先声明为false，最后如果成功就赋值 true
    bool processed=false;
    // 只有当机器人走过一定的距离或者旋转过一定的角度,或者过一段指定的时间才处理激光数据
    // 否则太低效了，period_被构造函数写死成5秒，可以考虑修改
    if ( ! m_count  ||   m_linearDistance >=m_linearThresholdDistance
            || m_angularDistance >=m_angularThresholdDistance
            || (period_ >= 0.0 && (reading.getTime() - last_update_time_) > period_) )
    {
   			/*第2部分*/
	}
    m_readingCount++;
    return processed;
}

ScanMatcher 构造函数

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
ScanMatcher::ScanMatcher(): m_laserPose(0,0,0)
{
    line_angle = 0.0;
    m_laserBeams=0;
    m_optRecursiveIterations=9;
    m_activeAreaComputed=false;

    // 5cm解析度的默认参数
    // 这个参数是计算似然位姿的时候使用的，实际的gmapping中没有用到
    m_llsamplerange=0.01;
    m_llsamplestep=0.01;
    m_lasamplerange=0.005;
    m_lasamplestep=0.005;

    //地图进行拓展的大小
    m_enlargeStep=10.;
    m_fullnessThreshold=0.1;

    //指示里程计和陀螺仪是否可靠
    //如果可靠的话，那么进行score计算的时候，就需要对离里程计数据比较远的位姿增加惩罚
    //对于我们的应用来说，陀螺仪在短期内还是很可靠的。
    m_angularOdometryReliability=0.;
    m_linearOdometryReliability=0.;

    //理论上的离激光点的空闲距离 也就是说沿着激光束方向离激光点这么远距离的栅格一定是空闲的。
    m_freeCellRatio=sqrt(2.);

    //跳过一帧激光数据的开始几束激光
    m_initialBeamsSkip=0;   
    m_linePoints = new IntPoint[20000];
}

全是参数赋值，但是全写死，不能通过ROS修改

drawFromMotion 里程计运动模型

  • p 表示粒子估计的最优位置(机器人上一个时刻的最优位置)
  • pnew 表示里程计算出来的新的位置
  • pold 表示建图引擎记录的上一时刻的位姿(即上一个里程计的位置)

不是执行圆加运算，pnew和pold只是用于产生噪声控制量，然后加到p上

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
OrientedPoint 
MotionModel::drawFromMotion(const OrientedPoint& p, const OrientedPoint& pnew, const OrientedPoint& pold) const
{
    double sxy=0.3*srr;
     // 计算出pnew 相对于 pold走了多少距离，得到控制量
     // 这里的距离表达是相对于车身坐标系来说的
    OrientedPoint delta=absoluteDifference(pnew, pold);
    
    /*初始化一个点*/
    OrientedPoint noisypoint(delta);
    
    /*走过的三个方向的距离，为控制量添加上噪声项*/
    noisypoint.x+=sampleGaussian(srr*fabs(delta.x)+str*fabs(delta.theta)+sxy*fabs(delta.y));
    noisypoint.y+=sampleGaussian(srr*fabs(delta.y)+str*fabs(delta.theta)+sxy*fabs(delta.x));
    noisypoint.theta+=sampleGaussian(stt*fabs(delta.theta)+srt*sqrt(delta.x*delta.x+delta.y*delta.y));
    
    /*限制角度的范围为 -π～π  */
    noisypoint.theta=fmod(noisypoint.theta, 2*M_PI);
    if (noisypoint.theta>M_PI)
        noisypoint.theta-=2*M_PI;
    
    /*把加入了噪声的控制量 加到粒子估计的最优的位置上，得到新的粒子位姿的预估(根据运动模型推算)*/
    return absoluteSum(p,noisypoint);
}

这里的MotionModel是一个十分粗糙的运动模型，只是简单的矢量加减运算。 相比于《Probabilistic Robotics》中提到的速度模型和里程计模型而言，有很多方面都没有考虑，精度上可能有折扣。

第2部分 if中的处理激光数据

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
last_update_time_ = reading.getTime();
/*复制一帧数据 把激光数据转换为scan-match需要的格式*/
int beam_number = reading.getSize();
double * plainReading = new double[beam_number];
for(unsigned int i=0; i<beam_number; i++)
{
    plainReading[i]=reading.m_dists[i];
}
/*如果不是第一帧数据*/
if (m_count>0)
{
    /*
	  为每个粒子进行scanMatch，计算出来每个粒子的最优位姿，同时计算该最优位姿的得分和似然  对应于gmapping论文中的用最近的一次测量计算proposal的算法
	  除了进行scanMatch之外，还对粒子进行了权重的计算，并计算了粒子的有效区域，但不进行内存分配 内存分配在resample()函数中
	  这个函数在gridslamprocessor.hxx里
    */
    scanMatch(plainReading);

    //至此 关于proposal的更新完毕了，接下来是计算权重
    onScanmatchUpdate();
    /*
	  由于scanMatch中对粒子的权重进行了更新，那么这个时候各个粒子的轨迹上的累计权重都需要重新计算
	  这个函数即更新各个粒子的轨迹上的累计权重是更新
	  GridSlamProcessor::updateTreeWeights(bool weightsAlreadyNormalized) 函数在gridslamprocessor_tree.cpp里面实现
    */
    updateTreeWeights(false);
    /*
     粒子重采样  根据neff的大小来进行重采样  不但进行了重采样，也对地图进行更新
     GridSlamProcessor::resample 函数在gridslamprocessor.hxx里面实现
   */
    std::cerr<<"plainReading:"<<m_beams<<std::endl;
    resample(plainReading, adaptParticles, reading_copy);
}
/*如果是第一帧激光数据*/
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);

        //为每个粒子创建路径的第一个节点。该节点的权重为0,父节点为it->node(这个时候为NULL)。
        //因为第一个节点就是轨迹的根，所以没有父节点
        TNode* node=new	TNode(it->pose, 0., it->node,  0);
        node->reading = reading_copy;
        it->node=node;
    }
}
//	"Tree: normalizing, resetting and propagating weights at the end..." ;
//进行重采样之后，粒子的权重又会发生变化，因此需要再次更新粒子轨迹的累计权重
//GridSlamProcessor::updateTreeWeights(bool weightsAlreadyNormalized) 函数在gridslamprocessor_tree.cpp里面实现
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;
}