定位问题的引入

我们所熟知的机器人，可以大致分为以下四类：
        1. 操作机器人： 模仿人的手和手臂的动作，特点是有多个关节自由度。
        2. 移动机器人：通过移动，空间变换完成特定任务（运输，覆盖，上下料，到达某地）。
        3. 信息机器人：计算机系统的智能行为装置，人工智能。
        4. 人机结合： 机器人与真人之间的控制反馈闭合，如假肢机器人。
这四种类型的机器人可以做组合，如有智能行为的移动机器人，机器人可以移动并且做一些操作行为。 前两个类型的机器人，是发展比较成熟的，已经大量应用在工业领域，商业和家庭领域在近些年应用比例也在增长。其中移动机器人是我们目前关心的重点，运动控制平台与自动驾驶都能归结为移动机器人，移动机器人包含无人机，水下机器人,轮式机器人，履带式机器人，仿生足大狗等。 移动机器人关心的问题包含：
        1.  where am I (定位问题）
        2.  where am I going(导航问题，调度问题）
        3.  How do I get there（避障问题， 动作规划）
移动机器人的核心问题就是定位问题， 上面列的后面两个问题，都是以定位问题为前提，才能达成。

定位的基本概念

了解定位，我们首先要先了解位置。位置这个术语其实不是很准确，应该叫做位姿，包含不仅位置，还有目标体的朝向（姿态）。我们习惯于用笛卡尔坐标系来表征位姿。 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轴按照顺序旋转后的朝向姿态。表示旋转有很多种方法，这里介绍的yaw,pitch,roll叫做欧拉角表示法，是比较容易理解的表示法，这里要注意的是旋转的顺序及之后是按照新位姿的坐标轴进行旋转，这是常规的表征方法，不同的顺序，及坐标轴的选取标准不同，出来的位姿将是不同的结果。具体到自动驾驶和运动控制平台，都是在一个平面上进行移动，虽然中间有颠簸，但是我们关注的是在水平面上的位姿，上下的颠簸引起的位姿变更，可以投影到水平面上，这样定位问题其实是2D位姿的估计问题。与之相对的如无人机就是3D定位问题。

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

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

了解了位置的表征后，我们再来考虑运动平台和自动驾驶的定位问题，其实是要估计的是运动主体（运动平台本身或者车辆）这个参考帧，相对于周遭静止环境（其实这个静止大多数情况是指的隶属于地球这个刚体的相对位置静止）的位姿或者位姿变化， 这个周遭静止的环境，我们可以统称为世界坐标系，移动定位问题，可以简化为跟重力方向垂直的水平面上的2D位姿估计。 2D坐标系的(0,0)点, 及x,y轴的朝向其实可以是任意的，只要基准定好了，后面的参考不变即可。 对于自动驾驶，有一些细节需要补充， 我们熟知的定位（GPS）是经纬度坐标，如何对应平面笛卡尔坐标系呢？ 经纬度坐标可以通过墨卡托投影系统（Universal Transverse Mercator，UTM）投影到UTM 的一个区块中， 区块中再细的位置的可以看成一个2D平面使用笛卡尔坐标进行表征。 这样球面的经纬度坐标和平面坐标是可以转换的。

定位需要的传感器及相应定位方法

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

内传感器通过感知自身的运动变化，计量累计位姿变化。内传感器包含轮子码盘（又叫轮式里程计），惯性传感器（加速度计，陀螺仪）。里程计通过事先知道车轮的直径，计数车轮转速，得出速度和位移。 以差分两轮为例子，根据两个轮子的转圈数差，可以推算出运动主体角度的变化。惯性传感器可以测量出线性加速度和角速度，通过积分可以推算出累计位移和角度变化。内传感器的定位， 都依赖于一个起始位姿，通过不停积分，在起始位姿基础上，合并相对位姿，进而推算出新时刻的位姿。内传感器的优点是不依赖于外部环境，不对外部环境做先验假设。缺点也非常明显， 首先它推算的是累计位姿变化，所以绝对定位需要一个准确的起始位姿，起始不对，后面定位都不准。既然是累计变化，如果每一步累计引入误差，不管误差多小，在后续足够长的积累下，都会是一个很大的误差。见下图（点模拟概率分布）：

外传感器，通过感知周遭环境，来辅助定位自己的位姿。外传感器包含GPS接收，2D单目摄像头，双目摄像头， 深度摄像头， 激光雷达等。

GPS接收的优点是接收到全局位置锚定的定位信息，不会担心误差累计， 缺点有两个，第一是它获取位置信息的频率不会很快，大概10hz的样子。对于运动速度很慢的运动主体，还凑合能用，对于自动驾驶这个高速行驶的运动主体，需要更高的频率位姿获取。第二个缺点是GPS信号很容易被遮挡， 室内定位基本用不上GPS，汽车过隧道的时候，会有相当的时间无GPS信号。对室外对定位要求比较高的场景，或者室内定位场景， GPS无法单独解决定位问题。

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

单目摄像头定位最大的优点是便宜。定位有多种思路，比较典型的以下两种：
       1.  人如何通过眼睛知道自己在哪？ 人是通过识别一些静止的物体（我们统一称为地标），来大致估计自己的位置。如在家中， 我们通过看到门，屋子里的摆设，从这个屋子可以轻松的去到那屋。 在室外，我们通过楼房，车站，树木等静止不变的地标，来确定方位。借鉴这个思路，如果运动主体可以通过识别一些不动的物体作为参照，就可以间接进行定位了。不幸的是，这个对于机器人来说，非常难。外界的环境通过摄像头传入机器人的都是数字化的信息，具体就是像素点，数字化的信息通过识别物体的方式进行定位，是一件非常吃力的事情。有一个变种的方案，是很方便实施的，就是二维码方案，机器人识别二维码，是非常容易的，通过知晓二维码的宽度（只有知晓先验大小，才能判断远近），不同二维码所代表的地标位置，机器人可以仅仅通过单目2D摄像头方便的推算出自己的绝对位姿。以前的VR设备，如HTC VIVE，多采用这种方式实现定位。这个方案不方便之处在于要提前部署二维码。
        2. 单目SLAM(simultaneous localization and mapping)， 做到比较鲁棒的定位，需要两个阶段， 分为前端的视觉里程计和后端的回环机制。单目SLAM的问题包含初始化尺度问题和实际工程实施中计算量实时性问题。 尺度问题是2D成像理论内在特点决定的，因为没有深度信息，大小和远近是可以成比例缩放的， 反映到单目SLAM, 在初始化阶段，必须运动主体有平移的动作，来确定一个尺度基准，这个尺度基准是无法与测量单位m,cm,mm对应的，只是自己的一个单位， 后续的建图和定位都是基于这个单位来进行， 建立的地图和定位理论上可以等比例缩放。计算量问题，在于SLAM算法本身的复杂性，勉强在嵌入式设备上跑，计算资源基本耗尽。在实践中可以从算法并行计算方面进行探索，或者选择高性能的计算平台。单目SLAM 分为特征点法和直接法，考虑到对周遭环境（光线变化）鲁棒性，一般采用特征点法，就是采用人工设计的角点，作为连续帧进行匹配的地标。角点肯定是稀疏的，所以建图只是作为定位的辅助地标来做使用， 不能指导避障和导航。

主动光深度摄像头(3D）摄像头，这种摄像头在2D摄像头的基础上，增加红外发射和接收装置。这种主动光装置目前分结构光三角测距或者TOF(Time of flight)，可以在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中获取到的点深度， 有很大噪声在里面，所以优化是观测点和位姿都要一起优化调整的。 激光做定位的缺点是受环境如雨，雾影响比较大，对于透明介质也无法得到准确深度信息。

目前定位应用的主要方式

上一章节，我们描述了用于定位的主流传感器，及对使用这个传感器进行定位的主流方法进行了简单介绍。可以看到单一传感器在解决定位问题上，都有自己的缺点和优点， 在实际应用中，都是要结合多个传感器，联合解决定位问题， 以下针对几个典型场景，描述一下传感器融合情况。

自动驾驶 GPS+IMU+里程计场景。GPS 给出的全局锚定，可以消除累计误差问题，不过它的更新频率低，并且信号容易被遮挡。 IMU ，轮盘里程计更新频率高，不过有累计误差问题， 最容易想到的是收到GPS定位，使用GPS位置信息，误差就是GPS的精度，在下一次收到GPS定位间隔中，使用IMU（角度累加）和里程计（位移累加）进行位姿累加，中间的位姿误差是初值GPS定位误差和中间累加误差的积累。 改进的方法是使用非线性卡尔曼滤波，在收到GPS位置信息的时候，要结合IMU和里程计的积累预测值，和GPS观测值，算出一个误差收敛的，更优的位置估算值。

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

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

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

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

以上的组合只是能看到的一些产品的定位手段评估，当然可以结合单目SLAM, 双目SLAＭ与一些内传感器，进行随意组合。　融合是解决两个问题：定位精度会优于单一传感器，一个传感器在某种环境失效，补充传感器能顶上。

总结

以上，通过对于定位概念和方法的简单介绍，让普通读者了解定位要解决的问题及常用手段。里面涉及的算法，只做了一些简单描述和优缺点总结，留待后续可以展开描述。