深度剖析自动驾驶和机器人定位技术

通过对机器人定位的概念、方法、要解决的问题及常用的手段做了介绍，希望大家读完能有所收获。

▍定位问题的引入

我们所熟知的机器人，可以大致分为以下四类：

这四种类型的机器人可以做组合，如有智能行为的移动机器人，机器人可以移动并做一些操作行为。 前两类机器人发展的比较成熟，已经大量应用在工业领域，商业和家庭领域的应用比例在近些年也在增长。其中移动机器人是我们目前关心的重点，自动驾驶也可以归结为移动机器人。移动机器人包含无人机、水下机器人、轮式机器人、履带式机器人、仿生足大狗等。 

移动机器人关心的问题有三个：

移动机器人的核心问题就是定位问题， 后两个问题都是以定位问题为前提才能达成的。

▍定位的基本概念

了解定位，我们首先要先了解位置。位置这个术语其实不是很准确，应该叫做位姿，包含位置和目标体的朝向（姿态），我们习惯于用笛卡尔坐标系来表征位姿：

2D平面的位姿

三个自由度(x,y,rotation)，x,y 是2D平面的坐标位置，rotation 是偏向角。

3D世界的位姿

六个自由度（x,y,z, yaw,pitch,roll)。（x,y,z） 是3D立体坐标系中的位置坐标， yaw(航向角）、pitch(俯仰角）、roll(倾斜角）分别代表目标刚体绕z,y,x轴按照顺序旋转后的朝向姿态。

具体到自动驾驶和机器人，它们都是在一个平面上进行移动，虽然中间有颠簸，但是我们关注的是水平面上的位姿，上下的颠簸引起的位姿变更，可以投影到水平面上，这样定位问题其实是2D位姿的估计问题，与之相对的如无人机就是3D定位问题。

理解定位，必须理解的另外一个概念就是参考基准，通俗讲就是相对谁的位置和姿态。我们举生活中的例子，坐公交汽车，我们相对公交车位置几乎是不变的，相对于马路上的某个站牌是一直在变化的。更加严谨的定义是参考帧， 这个参考帧具化成视觉效果，是一个坐标系，遵守右手法则，规定了在空间的零点位置和三叉戟坐标轴方向。

在这里有一个非常有意思的地方，可以细细琢磨，定位是研究参考帧与参考帧的相对位置关系。当我们向别人描述“我在哪里”的时候，其实是描述我作为主体的参考帧相对于某个地标（如***），甚至地球这个主体参考帧的相对位姿描述，放到太阳系就不对了。虽然我们描述位置的时候，常常是忽略掉这个地球参考帧的，但那是建立在大家都有这个地球参考帧的共识下，去讨论位置。 这里还有一个引申的概念就是刚体，组成这个刚体的所有参考帧互相之间的相对位姿在任何时刻都是不变的。 我们与***的相对位置，在任何时刻都可以使用一个固定的运算，因为***这个地标隶属于地球这个刚体。但如果使用这个运算去推算我们与月亮的相对位置，由于月亮与地球不在一个刚体上，且月亮绕着地球转，所以任何时刻这个转换关系都在变。

了解了位置的表征后，我们再来考虑机器人和自动驾驶的定位问题，其实它是要估计运动主体（机器人本身或者车辆）这个参考帧，相对于周遭静止环境的位姿或者位姿变化， 这个周遭静止的环境，我们可以统称为世界坐标系。移动定位问题，可以简化为跟重力方向垂直的水平面上的2D位姿估计。 2D坐标系的(0,0)点, 及x,y轴的朝向其实可以是任意的，只要基准定好了，后面的参考不变即可。 

对于自动驾驶，有一些细节需要补充， 我们熟知的定位（GPS）是经纬度坐标，如何对应平面笛卡尔坐标系呢？ 

经纬度坐标可以通过墨卡托投影系统（Universal Transverse Mercator，UTM）投影到UTM 的一个区块中， 区块中再细的位置可以看成一个2D平面使用笛卡尔坐标进行表征， 这样球面的经纬度坐标和平面坐标是可以转换的。

▍定位需要的传感器及定位方法

运动主体想要了解自己的位置，必须借助传感器。传感器可以分为以下两种：

内传感器

1. 原理

内传感器通过感知自身的运动变化，计量累计位姿变化。

2. 分类

轮子码盘（又叫轮式里程计），里程计通过事先知道车轮的直径，计数车轮转速，得出速度和位移。 以差分两轮为例子，根据两个轮子的转圈数差，可以推算出运动主体角度的变化。

惯性传感器（加速度计、陀螺仪），惯性传感器可以测量出线性加速度和角速度，通过积分可以推算出累计位移和角度变化。

内传感器的定位， 都依赖于一个起始位姿，通过不停积分，在起始位姿基础上，合并相对位姿，进而推算出新时刻的位姿。

3. 优点

不依赖于外部环境，不对外部环境做先验假设。

4. 缺点

它推算的是累计位姿变化，所以绝对定位需要一个准确的起始位姿，起始不对，后面定位都不准。既然是累计变化，如果每一步累计引入误差，不管误差多小，在后续长时间的积累下，都会是一个很大的误差。见下面的点模拟概率分布图：

外传感器

1. 原理

外传感器通过感知周遭环境，来辅助定位自己的位姿。

2. 分类

GPS接收

2D单目摄像头

双目摄像头

深度摄像头

激光雷达

……

下面分别描述每种外传感器的原理和优缺点：

GPS接收

优点是接收到全局位置锚定的定位信息，不会担心误差累计。

缺点有两个：

1）获取位置信息的频率不会很快，大概10Hz的样子。对于运动速度很慢的运动主体，还凑合能用，对于自动驾驶这个高速行驶的运动主体，需要更高的频率获取位姿。

2）GPS信号很容易被遮挡， 室内定位基本用不上GPS，汽车过隧道的时候，会有相当长的时间无GPS信号。对室外定位要求比较高的场景，或者室内定位场景， GPS无法单独解决定位问题。

2D单目摄像头

2D单目摄像头可以将3D世界中的物体，投影到2D像素平面。它有一个特点，像素平面中的物体大小和物体的远近可以等比例放大缩小，投射到成像平面是一样的，也就是说在没有物体大小先验知识情况下，是无法分辨远近的。

单目摄像头定位最大的优点是便宜，定位有多种思路，比较典型的有两种：

1）间接定位

运动主体可以通过识别一些不动的物体作为参照，间接进行定位。但这个对于机器人来说非常难。因为外界的环境通过摄像头传入机器人的都是数字化的信息，具体就是像素点，数字化的信息通过识别物体的方式进行定位，是一件非常吃力的事情。                                                            

有一个变种的方案很方便实施，就是二维码方案，机器人识别二维码是非常容易的，通过知晓二维码的宽度（只有知晓先验大小，才能判断远近），与不同二维码所代表的地标位置，机器人可以仅仅通过单目2D摄像头方便的推算出自己的绝对位姿。以前的VR设备，如HTC VIVE，多采用这种方式实现定位。这个方案不方便之处在于要提前部署二维码。

2）单目SLAM

单目SLAM要做到比较鲁棒的定位，需要两个阶段， 分为前端的视觉里程计和后端的回环机制。该方案的问题包含初始化尺度问题和实际工程实施中计算量实时性问题。              

尺度问题，是由2D成像理论内在特点决定的，因为没有深度信息，大小和远近是可以成比例缩放的， 反映到单目SLAM, 在初始化阶段，必须运动主体有平移的动作，来确定一个尺度基准，这个尺度基准是无法与测量单位“m,cm,mm”对应的，只是自己的一个单位， 后续的建图和定位都是基于这个单位来进行， 建立的地图和定位理论上可以等比例缩放。                                                                                      

计算量问题，在于SLAM算法本身的复杂性，勉强在嵌入式设备上跑，计算资源基本耗尽。在实践中可以从算法并行计算方面进行探索，或者选择高性能的计算平台。

单目SLAM 分为特征点法和直接法，考虑到对周遭环境的鲁棒性，一般采用特征点法，就是采用人工设计的角点，作为连续帧进行地标匹配。角点肯定是稀疏的，所以建图只是作为定位的辅助地标来做使用， 不能指导避障和导航。                                                    

主动光深度摄像头(3D）

这种摄像头在2D摄像头的基础上，增加红外发射和接收装置，在2D RGB像素上增加深度（距离）信息。比较有名的产品有微软的Kinect，苹果最新的手机产品IphoneX 会配备这种摄像头，提供给用户VR使用体验和增强人脸识别FaceId的准确度。

深度摄像头在定位方面主要是深度SLAM方案。 相对于单目SLAM, 因为每个像素有了深度信息， 这样尺度问题就不存在了，不需要运动主体做平移运动来做初始化动作。有了深度信息，理论上建图是可以做稠密图，可以做三维建模。

它的缺点也很明显，除了跟2D一样算法计算量偏大，红外主动光非常容易受到其它强光的影响， 这样在室外基本就退化成一个2D摄像头了。 对于一些透光介质，如玻璃，深度信息是无法得到的。

双目摄像头

两个摄像头，光心距离固定。双目摄像头通过视差，可以间接得到两个摄像头共视像素的深度信息。双目摄像头比较像人的眼睛, 人可以通过双眼，直接估计出眼前物体的相对远近。对于计算机来说，通过同一时刻两个摄像头分别拍的两帧图像，根据视差的几何关系，可以算出像素的深度，达到跟主动光深度摄像头一样的效果。

相比主动光深度摄像头， 它的优点是受环境光线的影响比较小，可以在室外自动驾驶汽车上应用。缺点是像素的深度信息不能直接获取，需要不小的运算量通过视差几何关系计算获得。

双目摄像头本质上就是一个深度摄像头，只是获取深度信息的手段不同，所以定位也可以应用深度SLAM方案。

激光雷达

激光雷达是目前定位选择的主流传感器，带自主导航的室内扫地机的商用产品，一般都会配备激光雷达。在自动驾驶领域，高精地图的采集及定位应用， 使用的是多线激光雷达方案。

激光雷达分为单线和多线， 单线雷达只能扫描一个平面的障碍，所以直接出来的是一个2D地图。 多线雷达（有16线，32线，64线）产品，通过多个扫描面的组合，可以给出丰富的环境3D点云。

激光雷达定位， 主要是激光SLAM算法，跟视觉SLAM一样，也分前端雷达里程计和后端回环检测矫正。

激光SLAM 对CPU的消耗，是远远低于视觉SLAM的，鲁棒性更好，更加稳定。以2D激光SLAM为例，它可以在任意时刻得到某个特定高度水平面的2D障碍轮廓，所以在做前端里程计的时候，连续两帧，计算局部的地图轮廓匹配，可以使用相对比较少的计算量获取相对位移。

激光扫描出的点有准确度很高的深度信息，这样在做后端回环优化的时候，不需要优化某个位姿下的观测值（扫描的点云）， 而直接优化位姿。

对于视觉SLAM， 不论是单目SLAM 通过三角测量算出的点云深度，还是深度SLAM中获取到的点深度， 有很大噪声在里面，所以优化要对观测点和位姿一起优化调整。

激光做定位的缺点是受环境如雨、雾的影响比较大，对于透明介质也无法得到准确的深度信息。

▍目前定位应用的主要方式

上面我们描述了用于定位的主流传感器，可以看到单一传感器在解决定位问题上都有自己的优缺点。在实际应用中，需要结合多个传感器联合解决定位问题。 以下针对几个典型场景，描述一下多传感器融合的情况：

1. 自动驾驶 GPS+IMU+里程计

GPS 给出的全局锚定，可以消除累计误差问题，不过它的更新频率低，并且信号容易被遮挡。 IMU和轮盘里程计更新频率高，不过有累计误差问题， 最容易想到的是收到GPS定位，使用GPS位置信息，误差就是GPS的精度，在下一次收到GPS定位间隔中，使用IMU（角度累加）和里程计（位移累加）进行位姿累加，中间的位姿误差是初值GPS定位误差和中间累加误差的积累。 

改进的方法是使用非线性卡尔曼滤波，在收到GPS位置信息的时候，要结合IMU和里程计的积累预测值和GPS观测值，算出一个误差收敛的更优的位置估算值。

2. 自动驾驶 GPS+ 多线雷达+高精地图匹配

GPS 给出全局锚定，中间使用雷达SLAM 前端里程计做累加，可以配合高精地图的图匹配，做类似后端回环优化的方式，将GPS、激光雷达及已知地图进行融合定位。

3. 自动驾驶多对双目视觉摄像头SLAM方案

这种方案成本低，更加考究的是算法，有很少的自动驾驶公司宣称自己主攻纯视觉方案，现在不是主流。

4. 单线雷达+IMU+里程计融合

满足室内定位的要求， 个人理解可以分为浅融合和深融合。 浅融合使用IMU+里程计的累加值作为推算雷达里程计的初值，在这个初值基础上进行连续帧的扫描匹配，会大大加速匹配速度。 深融合会结合IMU和里程计的值作为约束条件，应用到后端回环约束矫正中。

5. 深度摄像头+ IMU 融合

目前在手机的VR应用中已经初见端倪，如苹果公司的IphoneX以及Google 已经发布一段时间的Tango项目。 深度视觉SLAM 与 IMU 进行深浅融合，达到一个比较不错的VR体验。

以上组合只是市面上能看到的一些产品采用的定位手段，当然还可以结合单目SLAM, 双目SLAＭ与一些内传感器，进行随意组合。融合方案的定位精度会优于单一传感器，一个传感器在某种环境失效，补充传感器能顶上。