gmapping-运动模型

1,运动模型
（整个过程与《概率机器人》中的逻辑一致。

//p表示上图中的x，即机器人当前位姿 pnew 表示里程计算出来的当前位置， pold 表示里程计上一次的位置

OrientedPoint MotionModel::drawFromMotion(const OrientedPoint& p, const OrientedPoint& pnew, const OrientedPoint& pold) const
{
        double sxy=0.3*srr;//srr 作为平移函数的平移误差（rho / rho） 0.1
        OrientedPoint delta=absoluteDifference(pnew, pold);//得到无噪声条件下的机器人位姿
          //增加高斯噪声 利用sampleGaussian函数产生高斯噪声加载到偏移量上，
        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));

//通过fmod函数限制偏移角度在（-pi,pi）
        noisypoint.theta=fmod(noisypoint.theta, 2*M_PI);//计算noisypoint.theta/2*M_PI的余数。 因为要把角度差限定在[−π π][−π π]之间。 
        if (noisypoint.theta>M_PI)
                noisypoint.theta-=2*M_PI;
//最后是返回累计的噪声 
//将带有噪声的偏移量加载到t-1时刻粒子的位姿值上 通过absoluteSum实现，此处涉及一个带噪声的偏移量转换到t-1时刻的base_link坐标系的坐标变换），得到t时刻新的粒子值。
        return absoluteSum(p,noisypoint);
}

解释：
（1）OrientedPoint delta=absoluteDifference(pnew, pold);
absoluteDifference()这个函数是将t时刻与t-1时刻里程计测量数据之差转换到t-1时刻的base_link坐标系下，符合公式2-4（源代码中用偏差分量替代了理论中的模长），
参考常用算法函数

delta = p1 -p2=(x1-x2,y1-y2,theta1-theta2)

atan2(y, x)是4象限反正切，它的取值不仅取决于正切值y/x，还取决于点 (x, y) 落入哪个象限：

当点(x, y) 落入第一象限时，atan2(y, x)的范围是 0 ~ pi/2;
当点(x, y) 落入第二象限时，atan2(y, x)的范围是 pi/2 ~ pi;
当点(x, y) 落入第三象限时，atan2(y, x)的范围是 －pi～－pi/2;
当点(x, y) 落入第四象限时，atan2(y, x)的范围是 -pi/2～0.

这个OrientedPoint delta的计算结果对应的理论公式的结果就是 两个位姿的差值转换到t-1时刻的base_link坐标系下。
（2）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));

以下四个参数是运动模型的噪声参数
double srr_;//作为平移函数的平移误差（rho / rho） 0.1 对应里程计运动模型中的 a3
double srt_;//作为旋转的函数的平移误差（rho / theta）0.2 对应里程计运动模型中的 a2
double str_;//作为平移函数的旋转中的测量误差（θ/ rho）0.1 对应里程计运动模型中的 a4
double stt_;//作为旋转的函数的旋转中的测量误差（θ/θ）0.2 对应里程计运动模型中的 a1
double sxy;

（3）absoluteSum(p,noisypoint)
参考常用算法函数

template <class T, class A>
orientedpoint<T,A> absoluteSum(const orientedpoint<T,A>& p1,const orientedpoint<T,A>& p2){
    double s=sin(p1.theta), c=cos(p1.theta);
    return orientedpoint<T,A>(c*p2.x-s*p2.y,
                  s*p2.x+c*p2.y, p2.theta) + p1;

p1是当前机器人的位姿,p2是带有噪声的t-1时刻坐标下的差值。
，前面的cp2.x-sp2.y, sp2.x+cp2.y, p2.theta是分别在p1的位姿上加上各自的噪声分量。

参考
gmaping原理及代码解析（三）