GMapping的运动模型

在本系列的粒子滤波器一文中介绍过，我们需要状态方程和观测方程来描述和分析一个动态系统。其中状态方程描述的是系统状态在控制量的作用下，随着时间的演变过程，可以写成如下的形式：

$$ \begin{equation}\label{f1} x_t = g_t(u_t, x_{t-1}) + \varepsilon_t \end{equation} $$

其中，$x_t$表示系统状态向量。$u_t$为控制向量，相互之间是独立的，在不存在控制的场景下该值为0。$g(·)$是状态转移函数，描述了系统在$t-1$时刻下的状态，经控制量$u_t$作用后，转变为$t$时刻的状态。$\{\varepsilon_t, t \in \mathbb{N}\}$是服从独立同分布的系统噪声序列。这个方程就是所谓的运动模型，它是粒子滤波器中构建建议分布的依据之一。

在Probabilistic Robotics的第五章中介绍了速度和里程计两个运动学模型。 GMapping中声称他们使用的是高斯近似的里程计模型。但是根据给出的参考文献和源码来看，他们好像使用的是速度运动模型，只是速度控制量的来源是从里程计获得的。本文就结合代码来分析速度运动模型。

1. 运动模型定义

在GMapping的建图引擎一文中，我们提到在每个更新过程中，都会使用成员变量m_motionmodel对各个粒子中机器人位置估计进行预估。我们可以在建图引擎的头文件gridslamprocessor.h中找到该成员变量的定义：

            /**the motion model*/
        MotionModel m_motionModel;

MotionModel是一个结构体，其定义如下面代码片段所示，有四个double类型的成员变量srr, str, srt, stt，它们用于描述运动模型中的噪声项。另有成员函数drawFromMotion用于根据运动模型生成运动后的预测位置。

        struct MotionModel{
            OrientedPoint drawFromMotion(const OrientedPoint& p, double linearMove, double angularMove) const;
            OrientedPoint drawFromMotion(const OrientedPoint& p, const OrientedPoint& pnew, const OrientedPoint& pold) const;
            Covariance3 gaussianApproximation(const OrientedPoint& pnew, const OrientedPoint& pold) const;
            double srr, str, srt, stt;
        };

实际上MotionModel中定义的四个成员变量与在Probabilistic Robotics的第五章中介绍的速度和里程计两个运动学模型，一点关系都没有。

2. 运动估计

在建图引擎的processScan函数中，通过如下的形式调用drawFromMotion。其中参数it->pose是上一时刻粒子的位姿，relPose则是里程计记录的最新位姿，而m_odoPose则是建图引擎记录的上一时刻的位姿。遍历每个粒子，调用该函数之后，就完成了对各个粒子的位姿预估。

        for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
            OrientedPoint& pose(it->pose);
            pose=m_motionModel.drawFromMotion(it->pose, relPose, m_odoPose);
        }

下面是drawFromMotion函数的实现，其参数列表的意义在上文中已经介绍过了。在函数一开始，通过对最新里程计位姿读书pnew与上一时刻的系统位姿pold做差得到控制量。实际上里程计的度数是一种传感器的反馈数据，根据《Probabilistic Robotics》的说法，人们常常将里程计的反馈数据用作控制量，得到相对简单的运动模型。

        OrientedPoint MotionModel::drawFromMotion(const OrientedPoint& p, const OrientedPoint& pnew, const OrientedPoint& pold) const{
            double sxy=0.3*srr;
            OrientedPoint delta=absoluteDifference(pnew, pold);

然后我们构建noisypoint，并通过辅助函数sampleGaussian为控制量添加上噪声项。根据函数的名称，我们可以认为MotionModel通过对正态分布进行采样，来为控制量添加噪声。至于如何保证样本服从正态分布，是一个很大的主题，这里不再展开。

            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;

最后，我们将附加了噪声项的控制量添加到粒子的位姿向量上，并返回。可以认为该函数的返回值，就是对粒子位姿的预估。

3. 完 & 一些实际的考虑

实际上，整个运动模型都是在实现式($\ref{f1}$)。对于不同的机器人系统，函数$g_t(u_t, x_{t-1})$是不一样的，而且$u_t$的获取方式不同系统也会有很大的差异。从控制的角度来看，$u_t$应当是一个控制变量，比如说电机的控制电流，伺服转速等。但是从这些控制量得到整个系统的状态方程通常会比较复杂，有时甚至不能写出$g_t$的解析形式。所以人们选择了一种简化的方法，使用里程计的计数来估计一个控制量。如此得到的状态方程实际上就是一个简单的求和操作，形式上十分简洁，工程上也很容易实现。

这里的MotionModel是一个十分粗糙的运动模型，只是简单的矢量加减运算。相比于《Probabilistic Robotics》中提到的速度模型和里程计模型而言，有很多方面都没有考虑，精度上可能有折扣。但是计算简单，计算量小。有时为了提高系统的实时性，这种以牺牲精度来换取计算效率的方式是很常用的手段。模型不必十分精确，够用就行。