gmaping原理及代码解析（三）

（三）gmapping详细代码解析

---

A、里程计运动模型

我们先来看一下Gmapping里面实现的里程计模型代码：

/*
@desc 里程计运动模型
@p    表示粒子估计的最优位置(机器人上一个时刻的最优位置)
@pnew 表示里程计算出来的新的位置
@pold 表示里程计算出来的旧的位置(即上一个里程计的位置)
*/
OrientedPoint 
MotionModel::drawFromMotion(const OrientedPoint& p, const OrientedPoint& pnew, const OrientedPoint& pold) const{
	double sxy=0.3*srr;

	OrientedPoint delta=absoluteDifference(pnew, pold); // 计算新位姿相对于旧位姿的偏移量
	
	/*初始化一个点*/
	OrientedPoint noisypoint(delta);
	
	/*走过的X、y、z轴方向的距离加入噪声*/
	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);
}

• OrientPoint点的结构:

/*带方向的点  即除了位置之外，还有角度，可以理解为机器人的位姿*/
template <class T, class A>
struct orientedpoint: public point<T>
{
  inline orientedpoint() : point<T>(0,0), theta(0) {};
	inline orientedpoint(const point<T>& p);
	inline orientedpoint(T x, T y, A _theta): point<T>(x,y), theta(_theta){}
        inline void normalize();

    inline orientedpoint<T,A> rotate(A alpha)
    {
		T s=sin(alpha), c=cos(alpha);
		A a=alpha+theta;
		a=atan2(sin(a),cos(a));
		return orientedpoint(
			c*this->x-s*this->y,
			s*this->x+c*this->y, 
			a);
	}
	A theta;
};

• absoluteDifference函数：

absoluteDifference()这个函数是将t时刻与t-1时刻里程计测量数据之差转换到t-1时刻的车辆坐标系下，计算位姿差

/*
@desc 两个位姿的差值，算出来P1在以P2为原点的坐标系里面的坐标。
*/
template <class T, class A>
orientedpoint<T,A> absoluteDifference(const orientedpoint<T,A>& p1,const orientedpoint<T,A>& p2)
{
	orientedpoint<T,A> delta=p1-p2;
	delta.theta=atan2(sin(delta.theta), cos(delta.theta));
	double s=sin(p2.theta), c=cos(p2.theta);
	return orientedpoint<T,A>(c*delta.x+s*delta.y, 
	                         -s*delta.x+c*delta.y, delta.theta);
}

• sampleGaussian函数（这里我把源代码中的注释给删掉了）：

double sampleGaussian(double sigma, unsigned int S) {
	if (S!=0)
        {
                srand(S);
        }
	if (sigma==0)
		return 0;
	return pf_ran_gaussian (sigma);
}

sampleGaussian函数里面的pf_ran_gaussian()函数,从方差为sigma,均值为0的高斯分布中抽取随机数，

参考：https://www.taygeta.com/random/gaussian.html（没太懂）：

double pf_ran_gaussian(double sigma)
{
  double x1, x2, w;
  double r;

  do
  {
    do { r = drand48(); } while (r == 0.0);
    x1 = 2.0 * r - 1.0;
    do { r = drand48(); } while (r == 0.0);
    x2 = 2.0 * drand48() - 1.0;
    w = x1*x1 + x2*x2;
  } while(w > 1.0 || w==0.0);

  return(sigma * x2 * sqrt(-2.0*log(w)/w));
}

• fod(x,y)是返回x/y的余数，将角度限制到（-pi,pi）之间

gmapping的里程计采样模型采取的方法是《概率机器人》里的里程计运动模型，具体如下：

• 运动模型示意图：

伪代码：

（1）2-4行表示将t时刻数据转换到t-1时刻的车辆坐标系下

（2）5-6行给新的偏移量加入一个包含噪声的偏移值,噪声是一个高斯采样值

（3）8-9行把得到的新的偏移量转换到没有噪声的车辆坐标下，再叠加t-1时刻的位姿，得到t时刻的位姿估计

控制量ut就是里程计给出的信息，，其中

是里程计信息在t-1时刻的位姿，是里程计在t时刻位姿，因为里程计是机器人内部的传感器，它计算的是机器人一点都没噪声的理想状态时的行进距离，所以由于机器人在行进的时候的偏移和打滑，这个是不准确的，所以在5~6行我们加入一定的噪声来表示机器人实际行进的“真”值。

---

B、scan-match模型

先上代码，gmapping里的函数scanMatch()，函数位于gridslamprocessor.hxx中；

来看在scanMatch中调用第一个比较重要的函数---optimize函数，位于scanmatcher.cpp中：

double ScanMatcher::optimize(OrientedPoint& pnew, const ScanMatcherMap& map, const OrientedPoint& init, const double* readings) const
{
    double bestScore=-1;
    /*计算当前位置的得分*/
    OrientedPoint currentPose=init;
    double currentScore=score(map, currentPose, readings);
	
    /*所有时的步进增量*/
    double adelta=m_optAngularDelta, ldelta=m_optLinearDelta;
	
    /*精确搜索的次数*/
    unsigned int refinement=0;
	
    /*搜索的方向*/
    enum Move{Front, Back, Left, Right, TurnLeft, TurnRight, Done};
    //enum Move{Front, Back, Left, Right, TurnLeft, TurnRight, Done};
    int c_iterations=0;
    do
    {
        /*如果这一次(currentScore)算出来比上一次(bestScore)差，则有可能是走太多了，要减少搜索步长 这个策略跟LM有点像*/
        if (bestScore>=currentScore)