SLAM从入门到精通（里程计的计算）

【 声明：版权所有，欢迎转载，请勿用于商业用途。 联系信箱：feixiaoxing @163.com】

        曾经有一段时间，对里程计没有一点概念。一直以为它是通过某种传感器直接给出来的裸数据。等到后来接触多了，才知道里程计也是算出来的。狭义的里程计一般就是利用底盘上面的编码器直接计算出来的。而广义的里程计则多了，可以是激光里程计，也可以是图像里程计，只要通过传感器的数据差能获得当前机器人的位姿信息，都可以称作里程计。

1、差速轮的里程计

        所有底盘结构中，差速轮是具有代表性的。当然一方面，也是因为他比较简单。一般就左右一个动力轮，前面有一个万向轮。万向轮是没有动力的。

2、差速轮的运动学分析

        要了解差速轮，那么我们先预设一些变量。差速轮的间距是l，左轮的速度是vl，右轮的速度是vr。其中l是机械参数，vl和vr则可以通过电机编码器计算获得。假设这三个变量是目前为止我们可以获得的已知量。

        假设经过一段时间t，小车发生了旋转，那么这个时候应该如何分析呢。通过上图，我们可以知道这样一个事实，

theta1 = theta2 = theta3

        那么这个时候，因为发生了旋转，所以其实右轮比左轮多走了一段距离，也就是d，

d = vr * t - vl * t

        另外，根据三角形的定理，且d比较小，可以认为

tan(theta) = d / l

        又因为采样的速度很快，d要比l小很多，所以小角度转向的情况下，tan(theta) 近似等于theta，这里theta是弧度的概念，所以，

w * t = theta = tan(theta) = d / l = (vr - vl) * t /l

        两边同除以t，也就得到了

w = (vr - vl)/ l

        另外我们知道，小车中心点的速度是左右轮速度的一半，

v = (vr + vl) / 2

        这样既然v和w都知道了，那么转弯半径就容易求解了，

r = v/w = l * (vr + vl) / (2 * (vr-vl))

3、继续求解小车的位姿信息

        前面我们通过公式获得了小车的w、v、r信息，有的同学也许会说，这里面好像没有得到小车的位姿信息。别着急，我们可以进一步进行推断求解。假设小车当前的位姿是x、y、theta，那么t时间之后，小车的位姿应该是这样的，

x = x + delta_x = x + vx * cos(theta) * t - vy * sin(theta) * t
y = x + delta_y = y + vx * sin(theta) * t + vy * cos(theta) * t
theta = theta + delta_theta = theta + w * t

        因为不会侧滑，所以vy=0，所以公式变成

x = x + delta_x = x + vx * cos(theta) * t 
y = x + delta_y = y + vx * sin(theta) * t 
theta = theta + delta_theta = theta + w * t

        这个时候其实也就是v，所以可以进一步修改成

x = x + delta_x = x + v * cos(theta) * t 
y = x + delta_y = y + v * sin(theta) * t
theta = theta + delta_theta = theta + w * t

        从最终的结果来看，转弯半径r似乎没有起到作用。但是实际情况是，在规划小车执行局部路径，保证小车可以能够连续精确进行底盘方面的控制方面，还是很有作用的。当然，机器人的底盘很多种，有独轮的、两轮的、三轮的、四轮的、六轮的、八轮的、多轮的，实际使用哪一种模型，需要我们具体问题具体分析，当然分析方法都是很类似的，基本以几何分析为主。

4、后续的问题

        看到这里，有的同学也许有疑问了。既然底盘可以求解出车的位姿信息，那么还要lidar、imu、camera这些外部的设备做什么。只能说，理想很丰满，现实很骨感。从上面公式来说，本身v、w的求解就是一种近似求解，这是存在误差的。另外一方面，传感器本身的采样也是有误差的。就算上面两种情况都不存在，现在生活中路面也是坑坑洼洼、凹凸不平的，单纯利用里程计来计算小车的位姿，不现实也不可能的。

        另外，有一点同学门要注意的，就是这里odom求解出来的pose，并不是robot的pose，而是两轮中间点的pose。如果odom要转成robot的pose，还要通过matrix转一下的。如下图所示，这个robot就很能说明问题，本身robot的高度大于宽度，所以整个车的重心，相比轴距中心点其实是要高一点的。文章一开始的那个图可能会给大家一个错觉，以为odom和robot的pose是一回事，其实不是这样的。这里特此说明下。