SLAM总结（三）-传感器之各传感器特性_3d激光雷达和2d激光雷达区别

一、综述

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.标定
（1）时间同步
其它传感器也可从这两方面考虑
一般相机与MCU搭配使用，不集成在一起
1）考虑相机时钟源：相机是否可以对外输出相机曝光过程的事件信号(多数，MCU则记录曝光开始和曝光结束的两个时间，并取均值，作为最终的相机图像采样时间，这就实现了相机图像数据的时间同步)？是否支持外部触发（多数）？MCU是否有内部硬件时钟？MCU是否支持PPS时间同步？MCU是否支持PTP同步？
2）打时间戳的时刻：是相机模块打还是MCU打？打触发时刻、曝光开始、曝光中间、曝光结束时刻还是接收时刻？
相机从触发到成像的过程包括：延迟时间、曝光时间、读出时间，如下图所示，为了保持固定频率，曝光时间之前延迟时长不定。相机理想的时间戳为相机曝光采样的时刻，由于曝光需要一定时间，特别是卷帘门，可选取曝光中间时间作为时间戳，如曝光开始和曝光结束的均值，但实际较难获取这个时刻。一般相机有外部触发线，可以通过外部信号触发相机，可将触发时间作为相机时间戳；由于相机曝光之前延迟时间，触发时间可能离曝光时间较短，曝光结束时刻或周期结束时刻离曝光时刻更近。最差的可能是接收到图像再打时间戳，中间还包含了不稳定的时间延迟。

（2）空间标定
单目相机一般使用张正友标定方法，在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
1
2
3
4
5
6

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

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

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）时间同步
激光雷达一般比较贵，MCU和采集模块一般集成在一起，与相机有所不同。例如，Velodyne的VLP-16激光雷达对外提供1PPS信号的输入接口和一路消息输入串口，分别连接GNSS接收机的1PPS信号和定位输出接口（输出GPRMC或GPGGA格式的NMEA消息）。而部分Livox激光雷达硬件上仅有1PPS信号输入接口，需要在上位机将额外的GPRMC消息发送给激光雷达。激光雷达通过接收的1PPS信号及其对应的GNSS定位消息中的GNSS绝对时标，再配合其内部的时间系统，即可实现激光点云数据的时间同步。
是否有内部硬件时钟？是否支持PPS时间同步？是否支持PTP同步？是否可输出脉冲信号，是否可以在固定角度输出脉冲信号，可用于触发相机？

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激光雷达叠在一起。具体待补充。
禾赛：禾赛pandar 128量产车用的较多，价格20w+，线束较密
速腾：
ouster：ouster REV7 OS1 128，价格30w左右，30米外地面线束稀疏，存在较多无效点(30%左右)
镭神：
2.畸变
3.数学模型
4.标定
5.误差分析
6.优缺点
优点：
1)精度高
2）具有3D信息
缺点：
1）价格昂贵；
2）不易做闭环检测、重定位和全局定位
3）走廊在走廊方向无法确定位置，想象一下用高斯图表示不确定性，那么当前位置走廊方向的不确定性为无穷大。也就是2D雷达偏爱有角度物体的地方。
4）无结构地方易丢失，如空旷的地方
5）结构相似的地方易定位出错
6）雪、雾、雨和扬尘环境对其影响很大
7）一些激光雷达存在较多无效点
7.品牌

四、IMU

参考：P. D. Groves, “Principles of GNSS, Inertial, and Multi-sensor Integrated Navigation Systems”, 2nd ed., vol. 39. Artech House, 2013.
1.原理
IMU一般包括角速度计和加速度计。加速度计主要分为摆式和振梁式，角速度计(陀螺仪)分为光学陀螺、振动陀螺和质量转子陀螺(大多被前两种替代)。

2.误差分析
参考：武汉大学多源智能导航实验-如何区分惯性器件的零偏误差、武汉大学多源智能导航实验-Allan方差分析方法的直观理解、严恭敏-Allan方差分析的使用要点、
IMU原理介绍及误差分析
加速度计和陀螺仪均存在零偏误差、比例因子误差、交叉耦合误差、随机噪声，以及高阶的误差项、角速率/比力交叉敏感误差。每一种系统性误差都包含四个部分:常值项、随温度变化项、逐次启动项和工作期间变化项。常值项每次开机都会存在， IMU 处理器根据 内场标定结果，对其进行修正 随温度变化项也可以结合内场标定的温度参数进行补偿 如果不补偿温度变化项，传感器在开机之后的头几分钟，一般都会表现出系统误差这是因为传感器需要预热过程，以使其达到正常工作温度。每一个误差源的逐次启动项变化都影响总体误差，而且每次开机都不同，但在一次开机过程中，保持不变。虽然 IMU 处理器不能修正逐次启动项误差，但可以在初始对准和 或组合导航过程中对其进行标校。工作期间变化项，在一次开机工作过程中会缓慢变化， IMU 处理器和对准过程均不能补偿工作期间的变化项误差。理论上，惯性导航的工作期间变化误差可以通过与其他导航传感器的组合予以修正，但实际上难以实现。
真正决定惯性导航系统精度、影响组合导航系统设计的，是补偿了常值项误差之后惯性传感器的性能 经过 IMU 常值补偿之后，剩余的误差源还有逐次启动项、工作期间变化项误差，还包括常值项误差修正后的残余误差、温度相关误差等。
零偏误差项是惯性仪表所有误差的主要成分。将零偏分成静态分量 和动态分量 部分。零偏的静态分量，也称为固定零偏、启动零偏或者零偏重复性，包含逐次启动零偏和经标定补偿之后的剩余常值项零偏 零偏的静态分量在一次启动的整个工作过程中都保持不变，但逐次启动则会有变化。零偏的动态分量，也称为工作期间零偏变化量或零偏不稳定性，在数分钟的工作时间内就会有变化。零的动态分量包含经标定补偿之后的温变零偏剩余项 一般地，动态零偏占静态零偏的 10%
如果只用一个指标来代表一款IMU的精度的话，那毫无疑问是陀螺零偏。这是因为：1) 惯导系统的精度主要取决于IMU中的陀螺器件精度，而不是加速度计精度，直线上差1%，还在一条线上，但角度差1%，转一圈就是3.6度；2) 陀螺的精度指标中最重要的又是零偏误差，它基本上决定了该惯导长时间独立工作时的误差发散速度。但是，这里需要特别注意的是，陀螺零偏有好几种，看产品指标时一定要弄清楚是哪一种陀螺零偏指标。
加速度计和陀螺仪主要存在如下误差：

确定性误差：
大多数惯性传感器的比例因子和交叉藕合误差在 10 ^-4-10 ^-3(100-1000ppm)之间。但存在两个主要的特例:一些未经标定的消费级 MEMS 陀螺的比例因子误差可以达到 0.1 (10%) ，交叉藕合误差达到 0.02(2%) ;而激光陀螺的该项误差可以小至 10^-6 -10 ^-4 (1 - 100ppm)。比例因子和交叉耦合误差组成Ma和Mg：

（1）比例因子误差：由加速度计比例因子误差导致的加速度计输出误差，与沿敏感轴方向的真实比力成正比;同样，由陀螺比例因子误差导致的陀螺输出误差，与绕敏感轴的真实转动角速率成正比 。另外，比例因子非线性误差，有时简称为非线性 非线性误差是指比例因子随着比力或角速率的大小不同而变化 比例因子非线性导致的误差，可以建模为传感器测量值的幕级数形式 幕级数包含真实角速率或比力的平方项、三次方项 四次方项。在一些仪表的技术规范中，提供了仪表的非线性幕级数系数的平方或方的标准偏差(比如，振梁式加速度计的K2和K3几项)
（2）交叉耦合误差：由于惯性传感器的敏感轴与载体坐标系的正交轴之间不对准造成的(非正交误差)。。轴与轴之间不对准，导致每个加速度计会测量到与其敏感轴向正交方向上的比力分量;同样，每个陀螺会测量到与其敏感轴向正交方向上的角速率分量 轴间的不对准误差也会产生附加的比例因子误差，典型地，这部分误差比交叉糯合误差小 -4 个数量级 在振动型惯性传感器中，各个单独的传感器之间的串扰，也可能导致交叉藕合误差。;大多数惯性传感器的非线性误差在10^-4- 10^-3 之间;某些 MEMS 陀螺则达10^-2 。

随机误差：
（1）随机噪声(随机游走，wa和wg)：受多种误差源的影响，所有的惯性传感器都表现出随机噪声 电气方面的噪声限制了惯性传感器的分辨率，特别地，对输出信号非常微弱的 MEMS 传感器而言更是如此 由于机械结构不稳定，导致摆式加速度计会输出噪声;为了克服闭锁效应，机抖激光陀螺在补偿了随机抖动之后的残余项也会产生噪声振梁式加速度计(VBA) 和振动陀螺也表现出高频共振 此外，机抖激光陀螺的抖轮振动、转子陀螺的转子振动都会导致加速度计产生测量噪声。随机噪声的功率谱密度方根 (PSD 方根) ，变化范围从航空级 IMU的大概 20 ug//Hz，到战术级 IMU 中摆式或石英振梁式加速度计的 100ug//HZ
左右，再到 MEMS 传感器的 80 -1000ug//HZ的量级,环形激光陀螺的随机噪声在 0.001 -0. 02° //h的范围，取决于精度级别 采用光纤陀螺或石英振动陀螺的战术级 IMU ，其陀螺随机噪声的内PSD方根在 0.03 -0.1°//h的范围 MEMS硅质振动陀螺随机噪声的 PSD 方根一般为 0.06 _2 °//h而且会随着输入角速率的增大而增大。
IMU 数据总线输出采样值时，量化过程是另一个噪声来源量。战术级16bit为10^-4m.s^-1和210^-6rad，消费级8-12bit为10^-3m.s^-1和210^-5rad
（2）零偏：ba和bg
1）常值零偏：常值零偏：这个比较好理解，也就是这只陀螺生产出来后就一直固定不变的零偏值。对于传统的高性能惯性器件来说，该误差在出厂标定时往往就被补偿干净了，因此不会标注这个指标；但对于低端MEMS IMU芯片来说（例如用在手机中的价格不到10块钱的芯片），则不可能做逐个的标定和补偿，因此常会存在deg/s（也就是几千deg/h）量级的常值零偏。这看上去巨大无比，但我们在实际使用中很容易对付，例如在初始启动过程中利用几秒钟的静态数据求平均即可扣掉大部分。
2）全温零偏误差：英文是bias error over temperature，或是零偏的温度敏感系数（或称温漂系数），反映器件参数的温度敏感性。就是指陀螺零偏在其额定工作温度范围内相对于室温零偏值的变化量。对于传统惯性器件，一般是逐个做了温漂补偿的，因此这个全温零偏误差就是温补后的残差；而对于低端MEMS芯片，不可能逐个做温漂标定和补偿，如果给出这个全温零偏误差可能会很吓人，因此厂家往往给个零偏的温度敏感系数（例如0.01 deg/s/℃）。
3）零偏重复性：全称是零偏逐次上电重复性（run-to-run repeatability），这是传统惯性器件的经典指标，是指惯性器件不同次上电运行时的零偏的不重复程度。具体测量方法是在常温下将器件多次上电，测量和记录每次上电的零偏数值，然后统计其差异。很多新手会好奇，一个传感器的零偏值每次上电的变化能有多大？这么细微的误差因素都需要考虑？这是因为传统惯性器件在出厂环节就把常值误差和温漂误差等主要误差都已仔细补偿掉了，因此逐次上电重复性这个次要误差才成为了一个不可忽视的主要误差。
4）零偏不稳定性：严格来说应该称为零偏不稳定性（in-run instability），反映器件上电稳定后其零偏随时间变化的情况。根据具体测算方法又分为两种：
a)我国的国军标定义的零偏不稳定性：采集几个小时的静态数据，每10秒或100秒求平均（以便抑制器件白噪声的影响），然后统计这些平均值的标准差。
b)Allan方差给出的零偏不稳定性：采集足够长时间的静态数据（一般大于10小时，越高等级的器件所需时间越长），画Allan方差曲线，取其谷底值。
前者对惯导的实际表现有比较直接的影响，有现实指导意义；而后者则只是反映器件在极端理想条件下的性能极限，缺乏现实意义。从具体数值来看，前者也比后者大几倍甚至高一个量级(5-10倍)。
有些国内同行将国军标指标称为“零偏稳定性”，而将Allan方差指标称为“零偏不稳定性”，以示区别，但我觉得这在文字表述上会造成困扰，因此更倾向于称它们是两种零偏不稳定性指标。
5）零偏的加速度敏感性：英文是g-sensitivity或linear acceleration effect。振动陀螺、质量转子陀螺，以及某些光纤陀螺，表现出与比力相关的误差，也就是所谓的g相关零偏，单位°/h/g。陀螺的输出本来应该对加速度完全不敏感，但由于其敏感结构的加工误差等因素，多少还是会受到线加速度影响的，我们就用零偏的加速度敏感性来描述。显然，这种零偏误差只有在强动态载体上才会造成显著影响，而对于常见车载、船载低动态载体往往可以忽略。需要注意的是，越是高灵敏度的MEMS陀螺，其微机械结构的敏感质量越大，因此其加速度敏感性往往会比较大，例如某高端MEMS陀螺的加速度敏感性为18 deg/h/g。因而对 个陀螺组合的 IMU 而言，** 相关零偏是一个3x3 的矩阵，记为Gg**
所有零偏误差的总和，或者是所有零偏指标中最大的那个。但毫不意外的，厂家最喜欢提Allan方差零偏（尤其是MEMS厂商），因为它的数值最小。而某些不负责任的商家甚至都不明确标注这是Allan方差给出的零偏，以此误导用户。因此大家一定要擦亮眼睛，认真理解具体指标及其测算方法，尤其是对MEMS器件。我一般会重点关注MEMS零偏的温度敏感系数这个指标，因为MEMS器件的温漂是其内在的关键误差！对温度变化不敏感的话，其它指标通常都不会太差。我们调研选型惯性器件时一定要确保是把相同零偏指标的相同统计值来进行对比。
峰峰值 = 2倍最大值= 6倍RMS或σ值
不要因为MEMS器件巨大的常值零偏和全温零偏误差而轻易否定它、拒绝它。反正MEMS惯导不会长时间单独工作，一般都是与GNSS构成组合导航系统，那么常值零偏和缓慢零偏变化（包括温漂）以及零偏重复性一般都能被组合导航算法（例如增广Kalman滤波）有效地进行在线估计和补偿，此时我们真正在乎和需要关心的是零偏不稳定性中的中短期变化成份（例如，10s~1000s时间尺度上的波动变化），而MEMS器件的这种中短期零偏不稳定性与传统惯性器件相比并不会差得太远。千万别因为我们的偏执而错过了一个物美价廉的好器件！
陀螺仪动态响应特性良好，但是会产生累计误差，其中累计误差主要为低频误差。R_C=R+u_L。R_C为真实值，R为测量值，u_L为低频噪声
加速度计没有累计误差，但是容易受到振动误差干扰，其中振动误差主要为高频误差。R_C=R+u_H。R_C为真实值，R为测量值，u_H为高频噪声。
因此，IMU可使用互补滤波器滤波：输出 = 陀螺仪x 高通滤波器 + 加速度计 x低通滤波器
（3）深层误差：由于惯性传感器的设计方式不同，加速度计和陀螺仪还表现出某些深层次的误差特性。
在测量值超过工作量程时， MEMS 传感器往往出现误差。因此，选取与应用需求相匹配的传感器很重要。
测量过程超越传感器的带宽，也会引起误差。因此，传感器的带宽必须与将要测量的动态性相匹配。
不同类型的传感器，还表现出不同的高阶系统误差 摆式加速度计存在滞后误差，且与测量的比力是增加还是减小有关。振动陀螺，会敏感到绕输入轴和输出轴的角加速度、以及绕输入轴和输出轴的角速率的平方，振动陀螺还存在g相关比例因子误差;在启动头几秒的工作时间内，也会表现出较大误差
（4）振动导致的误差
在振动环境中，运动与传感器比例因子和交叉藕合误差相互作用，产生振荡传感器误差。

3.误差模型
测量值为(这些值是载体坐标系相对于惯性空间的运动，在载体系中的投影表示。运动的测量并不相对于地球)：

式中，f为比力，是加速度真实值，包括重力加速度和其他加速度。
IMU的加速度计和角速度计的测量值为：

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

4.标定
（1）时间同步
参考：武汉大学多源智能导航实验-MEMS IMU原始数据采集和时间同步的那些坑
从GNSS/INS组合导航的常见使用需求来看，MEMS IMU数据与GNSS的时间同步误差一般应小于10毫秒，最好是小于1毫秒。高精度的硬件时间同步（更确切地说是“时标统一”，精度在微秒量级）。然而，很多应用系统不具备硬件同步的条件（即无法连接1PPS脉冲信号或IMU采样信号），那么我们只能被动地采用软件同步的方式（精度只有几十毫秒量级）。
1）硬件同步：ICM20602有中断输出引脚（INT），可配置为采样完成（Data Ready）中断信号输出；ADIS16465则直接有一个“采样完成”信号的输出接口。MCU接收GNSS的PPS信号对本地系统时钟进行授时，修正时钟偏差，MCU接收IMU的采样完成中断信号打上时间戳，1PPS信号的误差一般为几十纳秒，可忽略不计；本地晶振时间一般采用微秒级别的计数器，那么最终GNSS数据和IMU数据的时间同步精度即为本地时间的精度，即微秒级。
（2）软件同步：许多GNSS/INS系统在开发过程中，没有条件连接GNSS接收机的1PPS信号或者IMU根本不具备数据采集相关的硬件信号（诸如“触发采样”、“采样完成”等信号管脚）。使用IMU的接收时间作为采样时间，因为每条IMU数据量很少而使得延迟较小。通过软件的方式实现的时间同步具有诸多不可控因素，导致实际上通过软件时间同步的精度低且稳定性差。
（2）内参标定
参考：多传感器融合定位理论基础（四）：IMU内参模型及标定
内场标定时，可标定零偏、比例因子和交叉藕合误差Ma/g、陀螺的与g相关误差Gg等误差项,基于转台或其它传感器可计算真实比力f和角速度w，通过解析法、最小二乘法可求解下式得到内参：


村田不标内参误差有千分之2-10，标了可明显减少。
5.优缺点
优点：
1）可靠性高，很少因为环境问题出故障；
2）高频：100-200HZ，短期内漂移小，可用于补偿视觉和激光SLAM剧烈运动带来的影响，补充在视觉或激光短期丢失轨迹
3）角度（使用角速度计）计算较准确，低成本IMU通过各种算法精度可以达到0.1°左右
4）可以观测重力向量，观测roll和pitch角

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

6.品牌
选型：武汉大学多源智能导航实验-如何读懂MEMS惯性器件的精度指标？、询价_资料-云汉芯城
航海级( marine - grade )：714万、100万美元(或 80 万欧元)，
航空级( aviation - grade)：71.4万、10 万美元(或8万欧元)，
中等精度：14-35万、2 -5 万美元(或1.6 -4 万欧元)，
工业/战术级级：1万-20万、 2000-30000美元(1600 -25000 欧元)，EPSON的M-G366(1w、北云组合导航使用，抗震较好)、法国的sbg、MTI（XSEN）、西安精准测控（1w-2w）、adi的ADIS16488、ADIS16465(7000)之类传感器、村田
消费级：10元、1美元左右，invensense的ICM20602（25）。

7.使用经验
使用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.误差分析
参考：武汉大学多源智能导航实验-Allan方差分析方法的直观理解
编码器存在一个脉冲读数的系统误差。因而，码盘上刻度线越多，结果越精确。
5.优缺点
优点：
1）可直接测量尺度
2）短期漂移小
3）高频
缺点：
1）精度低
2）长期存在较大漂移
3）车轮打滑对结果影响大

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

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

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

2.标定
一般的，杆臂初值10cm，角度10度，1.5公里完成标定，组合导航5hz，卫导10hz，没有GNSS信号情况下30s精度10cm左右，接轮速维持1、2分钟。晶振守时准，10-20min没问题。

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

缺点
1）室内、隧道、桥下和树下等有遮挡的地方会出现定位丢失或定位不准；
2)多路径反射：测距假设光是沿直线传播的，但在一些地方，可能存在光路反射，从而使定位不准。
3）受天气影响，受不同时间电离层和大气层变化影响

3.品牌
参考：自动驾驶系统入门（1）GNSS与高精度定位 - 3
GNSS组合导航厂商：国内有北云(M系列，pboxX1（0 点几度每小时）和X36D，X26单天线(停产)，模组M20-22（单天线），M20D-21D（双天线），芯片Alice)、导远(570系列)、北斗星通、华测等，国外Novatel。
性能
差分服务：国内有千寻位置、六分科技、中移智行等公司，国外有Trimble、Hexagon、Sapcorda等公司。
DTU及SIM卡：https://www.zhiqwl.com/hangye/863.html,https://www.zhiqwl.com/hangye/1066.html
无人机用便宜模块：
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）辅助视觉定位，二维码具有较准确的在世界坐标系下的位姿，从而减少累积误差。