差速小车的里程计

现在我们已经有了一个差速小车DiffCart并且能够通过键盘控制它，为了实现DiffCart的自主导航，我们还将给他装上IMU和激光雷达等传感器，进行定位建图。实际的差速小车都会在两个轮子上安装编码器，一方面用于控制两个轮子的转速，另一方面给上层系统提供了一个粗略的定位，常被人们称为里程计。而小车的定位通常是多个传感器融合的结果。

本文我们通过Gazebo的API获取两轮的转速，并在此基础上构建里程计。我们并不模拟编码器的工作原理，并且假设两轮电机的速度环控制是完美的。

1. 里程计原理

里程计的工作方式基本上可以看做是差速驱动的逆。差速驱动是给定小车运动的线速度和角速度来解算左右轮的转速，里程计则是根据左右轮的转速来估计小车的线速度和角速度。此外，里程计还需要对小车的线速度和角速度进行积分，从而可以得到一个粗略的定位。由于测量误差，以及诸如轮子打滑这样的未建模因素的存在，使用里程计来估计位姿是很不准的，而且这个误差将随着积分的作用而不断累加。

如右图所示，我们沿用差速驱动的符号和示意图。差速小车在$O$系中运动，以两轮中点为原点建立跟随小车一起运动的局部坐标系$A$系。点$C$是质点$A$的瞬时转动中心。$l,r$分别是小车的轮间距和轮半径。假设左右两轮的转速分别为$n_L, n_R$弧度每秒，根据几何关系可以写出轮子转速与小车线速度$v$和角速度$\omega$之间的关系：

$$ \begin{array}{rcl} n_R = (v + 0.5 \omega l) / r \\ n_L = (v - 0.5 \omega l) / r \end{array} $$

对上式求逆，我们就可以根据两轮的转速估计小车的线速度$v$和角速度$\omega$：

$$ \begin{array}{rcl} v & = & \cfrac{n_R r + n_L r}{2} \\ \omega & = & \cfrac{n_R r - n_L r}{l} \end{array} $$

小车在世界坐标系下的位姿可以用$A$点在$O$系下的向量$\begin{bmatrix} x_A & y_A & \theta_A \end{bmatrix}^T$来表示。其中$x_A, y_A$分别是$A$点在$O$系下的坐标，方向角$\theta_A$则是小车前进方向相对于$O$系$x$轴逆时针转动的角度。由于我们假设小车的轮子只有纯滚动运动，不会出现侧滑的现象，所以小车在$A$系下的速度矢量可以写作$\begin{bmatrix} v & 0 \end{bmatrix}^T$，其在$O$系下的投影可以通过左乘一个旋转矩阵得到：

$$ \begin{bmatrix} v_{x,O} \\ v_{y, O} \end{bmatrix} = \begin{bmatrix} \cos{\theta} & -\sin{\theta} \\ \sin{\theta} & \cos{\theta} \end{bmatrix} \begin{bmatrix} v \\ 0 \end{bmatrix} = \begin{bmatrix} v \cos {\theta} \\ v \sin {\theta} \end{bmatrix} $$

由于里程计的更新频率一般比较高，我们可以近似的认为，在前后两次更新的时间间隔$\Delta t$内机器人以$\begin{bmatrix} v_{x,O} & v_{y, O} \end{bmatrix}^T$的速度匀速直线运动了一小段距离之后，以$\omega$的角速度原地转动了一个小角度。那么我们可以通过如下的形式更新机器人的位姿：

$$ \begin{bmatrix} x_{A, t+1} \\ y_{A, t+1} \\ \theta_{A, t+1} \end{bmatrix} = \begin{bmatrix} x_{A, t} \\ y_{A, t} \\ \theta_{A, t} \end{bmatrix} + \begin{bmatrix} v \cos {\theta} \\ v \sin {\theta} \\ \omega \end{bmatrix} \Delta t $$

2. DiffCart的里程计

现在我们给DiffCart添加里程计。我们在Gazebo的仿真更新结束事件的回调函数OnWorldUpdateEnd中调用函数UpdateOdometry来定时的完成里程计更新。下面的代码片段是该函数的实现，该函数有一个输入参数td记录了前后两次更新的时间差$\Delta t$。在函数的一开始，我们通过Gazebo的API获取了左右两个轮子的转速。

        private: void UpdateOdometry(common::Time td) {
            mLeftWheelVel = mLeftWheel->GetVelocity(0);
            mRightWheelVel = mRightWheel->GetVelocity(0);

接下来根据轮半径和轮间距计算$\Delta t$时间内的线位移ds和角位移da。

            double ds_left = mWheelRadius * mLeftWheelVel * td.Double();
            double ds_right = mWheelRadius * mRightWheelVel * td.Double();
            double ds = 0.5 * (ds_left + ds_right);
            double da = (ds_right - ds_left) / mWheelDiff;

然后累积位移，获得粗略的位姿估计：

            mXOdom += ds * std::cos(mAOdom);
            mYOdom += ds * std::sin(mAOdom);
            mAOdom += da;

至此，里程计的位姿更新就已经结束了。为了能够在ROS的rviz工具中可视化的看到它，我们通过tf工具发布了一个"base"到"odom"的坐标变换。这里的坐标系"base"是指机器人的局部坐标系，即刚刚的$A$系。 "odom"则是世界坐标系，即$O$系。

            double c = std::cos(mAOdom);
            double s = std::sin(mAOdom);
            tf2::Vector3 ori(mXOdom, mYOdom, 0);
            tf2::Matrix3x3 rot(c, -s, 0, s,  c, 0, 0,  0, 1);
            tf2::Transform trans(rot, ori);

            geometry_msgs::TransformStamped odom;
            odom.header.stamp = ros::Time(mSimTime.sec, mSimTime.nsec);
            odom.header.frame_id = "odom";
            odom.child_frame_id = "base";
            odom.transform = tf2::toMsg(trans);
            if (NULL != mOdomTfBr)
                mOdomTfBr->sendTransform(odom);
        }

如果把源码(tag:v0.1.1)下下来，并编译通过的话，我们就可以通过如下的第一行指令加载DiffCart的仿真环境，使用第二行指令控制DiffCart运动一段距离之后，就可以在上面右图所示的rviz截图中看到机器人的运动。

        $ roslaunch gazebo_demos diffcart.launch
        $ roslaunch gazebo_demos diffcart_keyteleop.launch

3. 完

本文中，我们分析了里程计的工作原理，介绍了如何根据左右轮的转速估计DiffCart的线速度和角速度，进而给出了机器人位姿的粗略估计方法。然后扩展了DiffCart的插件，为之提供里程计，并发布tf坐标变换，可以通过rviz查看机器人运动。