SLAM总结（三）-传感器之各传感器特性

一、综述

1.传感器可分为主动式和被动式。主动式需要自己发射信号，然后通过接收信号测量，那么安装多个同类传感器、多机器人或一个传感器有多个发射器会出现相互干扰的情况。激光雷达、超声波、激光和红外属于主动式。被动式不会出现相互干扰的情况，相机、IMU、编码器属于被动式。另外，根据测量的目标不同又可分为内部传感器和外部传感器，IMU、编码器和GPS都是检测机体自身的运动，为内部传感器，相机和激光SLAM测量的是外部环境，为外部传感器
2.传感器比较重要的是测量范围、精度、内参、测量速度、测量之间的间隔、适用环境。不同传感器之间存在测量频率和时间戳之间匹配的问题。
3.距离运动会使相机（特别是卷帘快门）和激光雷达的每帧数据发生畸变（distortion），因为一帧数据由很多点组成，速度不够快，传感器一直在移动。帧率和速度差越大，畸变越大。
4.传感器都不可避免地存在噪声，在实际使用时可将传感器数据以图表的形式可视化显示，根据噪声类型选择合适的滤波方法，如一阶滤波、体素滤波等。
5. 环境退化（environmental degradation）会使传感器测量出错或跟踪失败，因为环境中没有包含充分信息了。如激光雷达面对的空旷环境、相机面对低纹理的白墙。
6. 根据定位方法不同可将传感器分为增量式定位和绝对定位的传感器。相机、激光雷达、IMU和编码器用于增量式定位，有累积误差，GPS、UWB等为绝对式定位，没有累积误差。

二、相机

1、单目相机

1.原理
针孔相机原理包括透镜成像原理和感光显像原理。透镜成像原理一般就是通过各种凹凸透镜组成镜头达到小孔成像的原理，本质还是小孔成像。感光显像是通过胶片、CCD和CMOS等感光元件将小孔成的像保存下来。这些和摄影知识密不可分。B站“黄门摄影”很好的介绍了相机的原理，如下图所示。


合理的曝光才能得到亮度合适的照片，低曝照片会很暗，过曝照片会太亮，甚至全变白。影响曝光的外因是光源的亮度及其照射物体角度，如太阳或灯光。影响曝光的内因有光圈（孔径）的大小、快门（曝光）时间和感光元件的感光度（ISO，感光元件对光的敏感度）。物体反射光源照来的光（不同颜色反射的不一样），当我们按下快门时物体反射的光从镜头摄入（曝光），感光元件感光后将图片保存下来。在SLAM中，为了适应不同环境，一般不会人为改变光源，光圈越大、快门时间越长和ISO越大图片会越亮。相机一般有自适应系统，会通过一个传感器感受外部光源，然后通过调节光圈、快门和ISO获得合适的曝光，但突然光线变化，相机会来不及反应，会出现短暂过曝和低爆情况，如突然从墙面转向玻璃。
景深和视角（视野）也是摄影的两个主要参数。景深是指焦点前后会比较清晰的一段，其他部分会模糊一些。摄影时一般为了把人拍清楚都会把人放在焦点处前后，这样背景就在景深之外也就模糊了。视角是指小孔射出去光线的最大角度。景深与光圈和焦距都有关。视角与焦距和感光元件尺寸有关。视角有水平视角、垂直视角和对角视角。
光圈和f值并不是等价关系，光圈大小和f值成反比，f=焦距/镜头孔径大小，焦距是镜头的中心点（小孔）到感光元件的距离，可认为光圈大小是指孔径大小。光圈越大通光量越大（孔径越大），景深越浅。
焦距是指成像平面到镜头光学中心的距离。镜头分为定焦镜头和变焦镜头，定焦镜头结构简单，镜片数少，光线折射扭曲率低，光线损失少，成像质量好。标准镜头感光元件对角线尺寸和焦距一致称为标准镜头，那么对角的视角是45°。焦距越长景深越浅。当感光元件尺度一定时，焦距越长视角也越小，相同物理尺度的物体占据感光元件的尺度变大，造成物体放大的效果。焦距可按如下划分，这也是SLAM中比较重要的概念，长焦镜头大于135mm，中长焦70-135mm，标准35-65，广角镜头小于35mm，鱼眼小于15mm，尼康一款6mm鱼眼视角可达到220°。

2.畸变
1）炫光是由于光在镜头里反射再反射到感光元件造成的，镜片越多反射越严重。光圈越小，反射越少。
2）运动模糊：理想情况下，相机和物体都完全静止，那么感光元件上的一个的点只会接收到现实世界中的一个固定点（颜色保持不变）反射的光，图片自然清晰，但剧烈运动时，感光元件上的一个的点会接收到现实世界中的多个点（颜色不同）反射的光，从而造成图片模糊。快门分为卷帘门（逐行曝光）和全局快门。这样卷帘门的曝光总时间时间一般长于全局门，而且每行曝光的时间还不一样，从而更容易造成模糊。
3）径向畸变：镜头的径向畸变产生的原因是放大率差异造成，屈光度越大差异越大。放大率随着光线与光轴所成角度变化而变化。当放大率随半径增大而减小时，会出现桶形畸变，当放大率随半径减小而增大时，会出现枕形畸变。想象一下相机和一张栅格图平行，光轴穿过栅格图中心。那么两种畸变如下，左图中，假设正方形内接圆放大率是1，那么随着半径增大，放大率变小，正方形角点在成像中缩短就出现桶形的效果。枕形刚好相反。

广角镜头容易发生桶形畸变

长焦镜头容易发生枕形畸变

4）切向畸变：由于感光元件装偏造成，如下图所示，想象一下相机和一个圆平行，光轴不穿过圆心，那么在圆弧上每个点的畸变都不一样。若感光元件没装偏，应该落在A₀上，装偏后落在了A₁上，发生畸变。

3.相机数学模型
相机数学模型是将针孔模型和去畸变模型用数学公式表达出来。理想针孔模型如下图所示，图中有4个坐标系：真实世界相机坐标系、归一化平面坐标系（一可理解为1m）、感光元件成像坐标系和像素坐标系。真实世界相机坐标系的原点在相机的光心（针孔），z轴与光轴重合，x轴水平向左（面朝z轴方向），y轴垂直向下。归一化平面坐标系和感光元件成像坐标系的原点是光轴与该平面的角点，坐标系方向与真实世界相机的坐标系一致。像素坐标系就是照片指照片的坐标系，它的原点在照片的左上角。真实世界坐标系下的点a₀，然后通过针孔模型投影到感光元件成像坐标系下的点a₂，此时说成的像是倒立的，最后通过离散采样得到像正立的照片。想象一下站着从像素坐标系左边看它就是我们拍摄得到的照片。

1）针孔成像原理的数学模型
已知a₀在相机坐标系下的3D坐标（X，Y，Z），待求量是相机在像素坐标系下的坐标（x，y）。首先将3D坐标系（X，Y，Z）投影到归一化平面得到（X/Z，Y/Z，1），然后使用投影矩阵K投影到像素坐标系,公式如下所示,先缩放后平移。f_x、f_y是x、y轴的缩放参数，理想情况下，该参数可通过相机的水平视角、垂直视角和相片分辨率求得，假设水平视角为α、 垂直视角为β，分辨率为m*n，则f_x=m/[2tan(α/2)],f_y=m/[2tan(β/2)],c_x、c_y是归一化平面坐标系原点投影到像素坐标系后在像素坐标系下的平移，理想情况下，该参数可通过相机的相片分辨率求得，c_x=m/2、c_y=n/2。实际肯定有所偏差，可以用该值作为参考。

2）去畸变的数学模型
在SLAM中，我们得到的是相机拍得的具有畸变的照片。一般首先在具有畸变的照片提取特征点，然后对这些特征点进行去畸变，去畸变后便能使用投影矩阵进行投影。因而，我们的已知量是具有畸变的点在像素坐标系的坐标（u，v），以及相机的投影矩阵和畸变参数。待求的是去畸变后的特征点在像素坐标系的坐标（u_undistorted，v_undistorted）。
一般去畸变主要是去径向畸变和切向畸变。去畸变在归一化平面坐标系进行。opencv中会先将像素坐标系具有畸变的点反向投影到归一化平面，得到在归一化平面具有畸变的坐标（x，y），然后去畸变得到（x_undistorted，y_undistorted），最后将其投影到像素坐标系得到（u_undistorted，v_undistorted）。
径向畸变的畸变公式为：
x=x_undistorted（1+k₁r²+k₂r⁴+k₃r⁶)
y=y_undistorted（1+k₁r²+k₂r⁴+k₃r⁶)
切向畸变的畸变公式为：
x=x_undistorted+2p₁xy+p₂(r²+2x²)
x=x_undistorted+p₁(r²+2y²)+2p₂xy
其中，k₁、k₂、p₁、p₂、k₃为畸变参数，r为点在归一化平面坐标系中的半径。
opencv中去畸变的公式如下,公式中先去切向畸变，再去径向畸变，可迭代去畸变：
x_undistorted={x-[2p₁xy+p₂(r²+2x²)]}/（1+k₁r²+k₂r⁴+k₃r⁶)
y_undistorted={y-[p₁(r²+2y²)+2p₂xy]}/（1+k₁r²+k₂r⁴+k₃r⁶)

4.标定
单目相机一般使用张正友标定方法，在ROS和Opencv中都实现了该算法。可用A4纸打印棋盘格。使用棋盘格越大，打印越清晰和精确，标定结果越精确。也有其他类似棋盘格的标定板能够实现更加精确的标定。ROS中标定方法如下：

# 打开相机，不同相机打开方法不一样，普通相机分为uvc_camera和usb_cam
sudo apt-get install ros-kinetic-uvc-camera
rosrun uvc_camera uvc_camera_node
# 8x6中间是字小写母x，8、6表示长宽内点数=正方形数-1；--square是方格边长，单位m；camera可不用
sudo apt-get install ros-kinetic-camera-calibration
rosrun camera_calibration cameracalibrator.py --size 8x6 --square 0.024 image:=/image_raw camera:=/camera

5.误差分析
相机精度一般很高，误差一般只有几个像素点。
6.优缺点
优点：
1）价格便宜：亲测笔记本自带、普通家用摄像头都能可以跑算法；
2）便于安装；
3）具有丰富的语义信息，可与目标检测、目标分割和目标跟踪等CV算法融合使用。

缺点：
1）单目具有尺度不确定性，在没有辅助信息的情况下无法获得物体的物理尺度信息；
2）光照变化对其影响较大：昼夜变换、突然变亮或变暗（门窗地方）
3）低纹理对齐影响较大：特征点少的地方无法跟踪，如白墙；
4）闭塞：如门口的地方
5）模糊：相机和物体剧烈运动都会造成模糊，不利于跟踪特征点。动态物体造成模糊可能是好事，可以
6）计算量大，占用内存大，图片数据比其它传感器数据多几个量级
7）单目SLAM初始化比较困难，map原点并不一定是最开始运行算法的地方。

7.品牌
普通相机一般分为uvc和usb类相机，普通的相机跑算法也够了。另外，也可以使用深度相机的RGB相机。

8.使用经验
使用树莓派带的便宜的64x480的相机跑ORB-SLAM2，图像太模糊了跑不动。使用笔记本摄像头和家用摄像头跑ORB-SALM2是没问题的，在家里跑还是比较容易跟丢的，特别是在门口的地方。单目ORB-SALM2还是比较适合在博物馆这种大厅跑。另外，相机的角度有一定的讲究，调整角度往特征点丰富、动态特征少的地方，向前平视时直走很难初始化。
深度相机一般价格比较贵，带的摄像头一般分辨率比较高，分辨率越高算法的计算量会增大，算法精度也更高，使用时640x480的分辨率已经够用。

2、深度相机

深度相机与单目相机相比的特点是相机可以输出RGD相机大多数像素点的深度，从而消除的单目相机的尺度不确定性。双目相机通过基线距离获得尺度，结构光和ToF相机通过主动发射光源获得尺度信息。几种深度相机的对比如下图。

深度相机品牌有：微软Kinect系列、Intel realsense系列、苹果、英飞凌、TI等为代表的消费级RGB-D；orbbec、pico、human+、爱观、图漾、艾芯智能、知微传感等；英特尔的RealSense、华硕的Xtion

双目相机

1.原理
双目相机的单目的成像原理和单目相机成像原理相同。双目相机的角度和光心的距离都是人为设计好的，它们有一定的共视区域。相机的高度也是一样的，可以通过快匹配匹配更多的像素点。在光心物理距离已知的情况下，可通过三角化计算出像素点的深度。与单目相机利用相邻时刻两帧不同的是，双目相机一个时刻就能得到两帧图像，即双目的左右相机得到的图像。左右相机如下图所示，双目基线O_lO_r的距离b（单位m）已知；双目安装在同一水平线上，且成像平面没有夹角。这样，对于空间中的点P，其在左右双目成像的y值是一样的，两成像平面没有夹角，那么左右光轴成像平面的交点的距离等于b。点P在左右成像平面的投影P_l、P_r的x值也是可以通过匹配算法对应得到的已知量u_L、u_R，它们的单位是m。这样在左右光轴组成的平面上就可以得到相似三角形O_lP^’O_r和p_lP^’p_r。焦距f（单位m）也是已知的，由相似三角形原理得到：
(Z-f）/ Z=(b-u_L+u_R) / b
由上式可得：
点P在相机坐标系下深度：Z=fb/（u_L-u_R),可令d=u_L-u_R
d可称之为视差，其与深度Z成反比，这也是为何说视差图可以展示图的深度的原因。d最小是一个像素对应的距离，在f一定时，要增大深度测量范围，则需增大b。当然，有时为了让左右相机共视区域更大，会使左右成像平面成一定角度，那么在计算深度时需要将这个角度考虑进去。

因为左右相机匹配点的y值相同，那么匹配时可以使用块匹配等方法加速匹配。双目相机需要计算所有像素点（如480*640）中大多数点的深度，需要先对所有点进行匹配，然后再计算每对匹配点深度。这些都是在相机的处理器计算，故计算量很大，需要使用FPGA或者GPU加速计算获得实时性。

2.畸变
左右的畸变和单目相机一样。还有就是左右相机的安装也可能出现误差，从而引起畸变。
3.优缺点
优点：
1）可以获得深度，消除了单目的尺度不确定性
2）更容易初始化
3）可以获得更多点的深度，可进行稠密建图
缺点：
1）价格贵
2）不太可靠，之前使用的双目相机空洞较多

4.品牌
曾经使用过两款国产的双目相机，使用中都发现存在时间戳逆序情况，好品牌可能不存在这种问题。另外，理想中相机的每个像素的深度都是可以得到的，但是实际上确存在较多空洞。

结构光相机

1.原理
该链接对结构光原理做了比较全面的论述
2.标定

1. 标定：标定彩色相机和红外相机畸变参数和相机内参，两个相机相对位姿（都是利用二维图像方法标定的，未利用深度值）
2. 深度校正：校正红外相机的深度值（得到相对位姿后，使用标定板以图像计算得到的深度校正深度相机测得深度值）
3. 配准：将彩色相机像素与红外相机配准，得到每个像素点的深度值。
   a) 纽带：彩色相机与红外相机观测到的是世界坐标系下的同一个点
   b) Libfreenect2配准存在的问题：彩色图像存在残缺，给基于特征的视觉SLAM带来不利影响
   3.使用经验
   乐视三合一的结构光相机200多块就能买到，而且室内建图想过还比较好，之前用它做过室内三维建图，效果还不错。尝试用ICP得到匹配深度点得到相对pose，但效果不佳。
   物件抓取可以用奥比中光、kinect for azure，可以用点云做，用传统的ppf算法，奥比中光深度相机拍零件的空洞（空洞一般是物体表面反光和透明导致的，相机打出去的光看不到，返回的深度值就变成了0）比较大，要换用结构光相机，TI有一款DLP，可以投射光，还提供了一些sdk，接上相机就可以跑一些demo。结构光打出去会有阴影，像自然光照射一样。

ToF相机

1.原理
ToF是Time of Flight的缩写，也就是通过光发出去和传回来的时间计算深度。相机一般由发射器、接收器和RGB相机组成，发射器和接收器通过发射光得到在深度相机坐标系下的各点深度，然后通过深度相机和RGB相机的相对位姿将3D点投影到RGB像素平面，从而获得点的颜色。激光雷达、深度相机等能获得点的3D坐标系的都可以通过这种方式获得点的颜色。
2.标定与结构光相机类似
3.优缺点
缺点：
1）比较黑色区域会造成近红外线反射的比较少；
2）玻璃等反射强的材料会导致传感器发出来的光回不去，从而无法测量距离。
3）如果两个摄像头互相对着放置，则什么数据也捕获不到

3.使用经验
Kinect 2.0得到的深度图也存在一些空洞，也会出现上一条缺点中存在的问题。

三、激光雷达

激光雷达一般是单线和多线，多线一般是16、32、64、128线。汽车级至少16线。室内简单很多，结构化物体多，探测距离也段，对传感器要求低，一般用单线就够了，如扫地机器人。
参考链接知道，按结构可分为机械式雷达、混合固态激光雷达和固态激光雷达。机械式激光雷达在工作时发射系统和接收系统会一直360度地旋转，而混合固态激光雷达工作时，单从外观上是看不到旋转的，巧妙之处是将机械旋转部件做得更加小巧并深深地隐藏在外壳之中。业内普遍认为，混合固态激光雷达指用半导体“微动”器件（如MEMS扫描镜）来代替宏观机械式扫描器，在微观尺度上实现雷达发射端的激光扫描方式。固态激光雷达有OPA（optical phased array，光学相控阵技术）与Flash固态激光雷达。相对于MEMS，OPA的电子化更加彻底，它完全取消了机械结构，通过调节发射阵列中每个发射单元的相位差来改变激光的出射角度。Flash原本的意思为快闪。而Flash激光雷达的原理也是快闪，不像MEMS或OPA的方案会去进行扫描，而是短时间直接发射出一大片覆盖探测区域的激光，再以高度灵敏的接收器，来完成对环境周围图像的绘制。相比于机械激光雷达，MEMS激光雷达体积更小，价格更低廉，更适合大规模应用；同时相比于OPA和Flash，MEMS在技术上更容易实现。因此，MEMS被各大主机厂商一致看好，近些年入局MEMS激光雷达研究的企业很多。业内人士认为，未来，中远距离激光雷达将是MEMS固态的天下。

国产品牌有：
1）速腾聚创（http://www.robosense.cn/rslidar/RS-LiDAR-16）：主要用自动驾驶，有16线
2）禾赛（https://www.hesaitech.com/zh）：主要用在室内，40线以上
3）镭神（https://www.leishen-lidar.com/index.html）：室内外都用，精度相对低些，价格便宜
4）思岚（http://www.slamtec.com/）：单线，主要用在扫地机器人
另外还有EAI等。
国外品牌有主要有velodyne（16线2w左右），占了无人驾驶市场一大半。

1、2D激光雷达

1.原理
以旋转式激光雷达为例，雷达中有一个激光测距器，一帧雷达数据由该测距器绕雷达中心旋转一周间隔固定角度测得距离值，其旋转一周的频率旋转频率在10-20HZ。激光雷达的原始数据一般是一帧中每束激光测得的距离d_i、角度θ_i和对应时间戳。因为采集一帧时间很短，可以假设机体是静止的，如下图右下圆，把它想象成一直转的激光雷达。那么以雷达旋转中心作为原点，一般以第一束激光方向作为x轴，然后根据右手坐标系原则便能得到雷达坐标系。通过下面公式可以将每个激光点转换到该坐标系。

2.畸变
旋转式激光雷达的畸变一般指运动带来的畸变。一帧数据中，没束光束发射的时间是递增的。发射器固连在激光雷达旋转，激光雷达固连在机体，当机体运动时发射器随着机体运动，发射器发射每束光束时在世界坐标系下位姿是变化的。假设机器人运动速度是0.5m/s，雷达帧率为10HZ，那么一帧数据中第一束与最后束雷达中心的距离是0.05m，这个距离也算比较大了，如上图所示，静止的假设其实就是假设光束i的坐标系i与第1束的坐标系0重合，那么两束光束坐标系相差T_0i。那么如何消除其影响呢，可以使用编码器或IMU这样高频（IMU一般100HZ-200HZ,编码器可以达到1000HZ）且短期漂移小的传感器来预测T_0i。去畸变后所有点可以看成以第一束光束坐标系为参考坐标系的点云，此时其与世界坐标系是全局一致的。在carto中使用时，匹配时会将点云以该参考坐标系旋转和平移。但在更新网格时，连线得从其真实中心开始连线，其实就是光速路径在世界坐标系的线位置，如果将点都和第一束光线坐标系原点相连，则又产生了新的畸变。

3.优缺点
优点：
1)精度高，数据量少，计算量小
2）价格相对3D便宜很多
缺点：
1）比较适合室内平地，不太适合室外；
2）不易做闭环检测、重定位和全局定位
3）走廊在走廊方向无法确定位置，想象一下用高斯图表示不确定性，那么当前位置走廊方向的不确定性为无穷大。也就是2D雷达偏爱有角度物体的地方。
4）无结构地方易丢失，如空旷的地方
5）结构相似的地方易定位出错
6）雪、雾、雨和扬尘环境对其影响很大

4.使用经验
本人使用过EAI、镭神的LS01B等单线激光雷达。运行cartographer单独使用EAI在室内建图飘的比较厉害，LS01B还能用一下，但也会飘。

2、3D激光雷达

1.原理
3D激光雷达，可以看成有n个2D激光雷达叠在一起。具体待补充。
2.畸变
3.数学模型
4.标定
5.误差分析
6.优缺点
7.品牌

四、IMU

1.原理
IMU一般包括角速度计和加速度计。以MPU6050为例，磁力计暂不讨论。加速度计物理模型如下图所示，其为z轴垂直于地面静止的情况。将一个小球至于盒子里，使用传感器测量盒壁的压力，便能得到各轴的加速度。

角速度则通过在3个轴分别使用3个陀螺仪测量角速度。

2.数学模型
IMU的加速度计和角速度计的测量值为：

a_t和w_t分别是小车真实的加速度和角速度。R_w^tg^w将世界坐标系下的重力向量转换到当前机体坐标系下。n_a和n_w分别是加速度测量和陀螺仪测量中的附加噪声。一般假设它们是高斯白噪声。b_at 和 b_wt分别为加速度偏置和陀螺仪偏置。它们被建模为随机变化噪声，其导数为高斯白噪声。

3.误差分析
IMU误差来源：
1）陀螺仪动态响应特性良好，但是会产生累计误差，其中累计误差主要为低频误差。R_C=R+u_L。R_C为真实值，R为测量值，u_L为低频噪声
2）加速度计没有累计误差，但是容易受到振动误差干扰，其中振动误差主要为高频误差。R_C=R+u_H。R_C为真实值，R为测量值，u_H为高频噪声。
因此，IMU可使用互补滤波器滤波：输出 = 陀螺仪x 高通滤波器 + 加速度计 x低通滤波器
4.优缺点
优点：
1）可靠性高，很少因为环境问题出故障；
2）高频：100-200HZ，短期内漂移小，可用于补偿视觉和激光SLAM剧烈运动带来的影响，补充在视觉或激光短期丢失轨迹
3）角度（使用角速度计）计算较准确，低成本IMU通过各种算法精度可以达到0.1°左右
4）可以观测重力向量，观测roll和pitch角

缺点：
1）长期漂移大
2）匀速时，加速度计读数为0
3）加速度计的偏差与温度有关，并且会随时间的推移而变化，位移（使用角速度计和加速度计）计算

5.品牌
invensense、法国的sbg、MTI（XSEN）、同样级别的西安精准测控（1w-2w）、adi的16488之类传感器（可用于卡尔曼组合导航)。

6.使用经验
使用MPU5050与相机跑VINS-Mono效果不佳，漂移很厉害。

六、编码器

1.原理
编码器安装于电机之上，只有轮式机器人才会有。根据检测原理，编码器可分为光学式、磁式、感应式和电容式，根据其刻度方法及信号输出形式，可分为增量式、绝对式以及混合式三种。对于增量式光电编码器，编码器上有一个圆形刻度盘，编码器的读数是脉冲数，脉冲数和刻度是对应，对于一个500线/圈的编码器，经过4倍频后，可增加到2000线。
编码器
2.数学模型
轮式机器人一般分为差速式、汽车式和全向式。机体坐标系原点取两轮的中点，x轴垂直轮轴向前，z轴垂直于地面，根据右手坐标系可获得y轴。编码器做差速模型的航迹推算，已知编码器两次读数差ΔE_r和ΔE_l，可求得左右轮速度：

r是车轮的半径。E为编码器每转的线数，I为减速机的减速比，一般电机和车轮之间会安装一个减速机，而编码器安装在电机尾部，不经过减速机。T_e为时间间隔
对于差速式轮式机器人，在短时间内可认为它是沿圆弧运动的，可得机体线速度v、角速度w:
v=(v_r+v_l)/2
w=（v_r-v_l）/D
已知(x_k,y_k)可求得得两帧之间的位移（x_k+1，y_k+1）和航相θ~k+1:

其中，由圆弧长度近似两帧之间距离Δs。

3.标定
需标定参数有车轮半径r、轮间距D。

4.误差分析
编码器存在一个脉冲读数的系统误差。因而，码盘上刻度线越多，结果越精确。
5.优缺点
优点：
1）可直接测量尺度
2）短期漂移小
3）高频
缺点：
1）精度低
2）长期存在较大漂移
3）车轮打滑对结果影响大

6.使用经验
使用中确实存在打滑现象，如地面凸起的立方体。

五、全球导航卫星系统(GNSS)

1.原理
目前的GNSS有美国的GPS、俄罗斯的GLONASS、我国的北斗和欧盟的伽利略。GNSS主要依靠天上的卫星定位。已知量是卫星在世界坐标系的位置和卫星与待测点的距离。由几何知识知道，将卫星看成已知位置的路标点，那么要确定一个待测点的位置，理论上只需观测到3个路标点即可。但实际中，由于卫星与待测点的距离存在误差，一般需要待测点观测到4颗卫星，各GNSS系统一般可以保证在地球上每个点可以观测到4颗卫星。卫星和待测点之间通过无线电波测距，原理类似ToF，距离=光速*时间。时间测量精度、光速在不同介质中速度差异等都会引起测量误差。DNSS一般有单点定位和差分定位，差分定位精度高于单点定位。差分定位可以通过基站校准误差达到更高精度参考链接

2.优缺点
优点：
1）相机、激光雷达、IMU和编码器用于增量式定位，而GNSS定位是绝对定位不存在累积误差

缺点
1）室内、隧道、桥下和树下等有遮挡的地方会出现定位丢失或定位不准；
2)测距假设光是沿直线传播的，但在一些地方，可能存在光路反射，从而使定位不准。

3.品牌
无人机用便宜模块：
1)Ublox：
2)NEO-M8N

4.使用经验
无人驾驶一般用差分定位中的RTK(Real - time kinematic，实时动态）载波相位差分技术，精度可以达到厘米级定位精度，价格达到好几万，而一般无人机、手机等上面用的GNSS模块一般使用单点定位，定位精度只有2.5m，价格只有几十块。定位精度虽然比较低，但它没有累积误差，可考虑与视觉和激光SLAM融合，提高GNSS的定位精度，减少SLAM的累积误差。

七、其他辅助传感器

1、毫米波雷达（Radar）：毫米波是无线电波，频率较低，波长较长，通常是4-12mm，与被测物体接触时，无线电波的吸收较少，因此，RADAR的有效工作距离相对更远，在雾、雨、雪和扬尘等所有天气条件下，均能稳定运行；激光雷达（lidar）发射的是激光脉冲，频率高，波长短，通常在900-1500nm之间，测量更清晰更准确。参考链接1参考链接2
2、激光测距仪器：激光测距精度2mm，但与光强关系较大，只在一定光强范围测得距离是准确的。测量玻璃距离时可能会偏小（实际2m，测得0.525m）；玻璃后有障碍物会测到障碍物距离；从障碍物到玻璃会出现测不到数据的情况；激光测到边缘时，测量值不会突变，会缓慢变化，5、6次左右。
3、红外测距仪器：易受光照影响
3、超声波测距仪器：超声波测距精度1.5cm；有发散角，可通过加挡板的方法减小发散角，四面都加会造成信号过强；超声波盲区8mm。
4、UWB（Ultra-Wide Band，超宽带）：相当于小范围的GNNS系统。该系统需在小范围内假设基站实现卫星的功能。淘宝上定位头盔htc vive也是基于这个原理实现的。
5、 路标：使用路标辅助定位，如使用二维码（apriltag）辅助视觉定位，二维码具有较准确的在世界坐标系下的位姿，从而减少累积误差。