全局定位问题初总结

自主导航系统的开发是构建完全自动驾驶系统的主要挑战之一。完全自主导航不仅要求在GPS信号正常情况下完美工作，而且在GPS不可靠的情况下也能够具有可靠的导航能力。因此，基于里程计系统近年来备受关注。自给式（就是没有GPS也能计算出车辆自身的位姿）里程计方法分为五种主要类型，即车轮、惯性、激光、雷达（毫米波雷达？）和视觉，以上分类主要是依据传感器的类型进行分类的里程计的方法。

1.轮速里程计

轮速里程计（WO）是车辆自定位的最简单形式之一，已应用于许多转向机器人，如两轮和四轮机器人。在这些车辆中，左右两侧的车轮可以在不同的速度和方向上独立操作。轮速里程计的方法是基于安装在机器人上的车轮编码器来跟踪每个车轮的转数。转数被整合到一个车辆的运动模型中，以确定机器人相对于起始点的当前位置。

限制：它只适用于地面车辆。此外，它还存在位置漂移现象，其测量误差随着时间累积。

2.惯性里程计

惯性里程计（IO）或惯性导航系统（INS）是一种定位方法，它使用来自IMU传感器的测量值来确定车辆/机器人相对于给定起点的位置。

限制：由于不同来源的误差，例如陀螺仪测量和加速度计的恒定误差，这些系统存在漂移问题。

3.雷达-惯性里程计

为了获得精确的运动估计结果，一些方法将雷达数据与IMU测量数据以松耦合或紧耦合的方式融合。将雷达和IMU数据融合到扩展卡尔曼滤波器（EKF）中，以估计飞机的状态。也有提出了一种结合单个雷达数据和陀螺仪测量数据的方法，以获得地面平台的前进速度、侧滑速度和角速度，以克服在光滑路面上里程测量的挑战。

融合的类型：

1. 松耦合：在松耦合技术中，位置和方向是通过混合来自两个独立子系统（即视觉里程计模块和IMU模块）的姿态估计来确定的。估计数据在后续阶段进行融合，以精确车辆的位置和方向。
2. 紧耦合：将从捕获图像中提取的关键信息与IMU传感器的原始测量值融合在一起，以获得更好的精度。关键信息可以通过使用图像检测技术（即角点检测器）从图像中提取和跟踪特征点，或者通过使用of算法利用图像的像素强度来获得关键信息。紧密耦合的方法执行视觉和IMU测量的直接和系统的融合，通常比松耦合方法产生更好的结果。这是因为紧耦合框架将图像对齐的关键信息和IMU误差项合并为一个代价函数。
3. 滤波方法：基于过滤器的方法是解决VIO和SLAM问题的最早方法之一。它们包括两个主要部分：预测步骤和更新步骤。此外，它们可以被视为最大后验（MAP）方法。其中来自内部状态传感器（例如IMU传感器）的测量值用于计算系统姿态的先验分布，而来自外部状态传感器（例如摄像机或激光雷达）的测量值用于构建似然分布。在基于滤波器的视觉惯性里程计中，车辆的先验分布（动态模型）是通过使用来自IMU传感器的线速度和角速度来计算的。该动态模型用于预测步骤，以预测车辆的运动。此外，在更新步骤中，将关键信息（例如，从捕获的图像中提取的特征或像素强度）用作似然分布（测量模型）来更新预测。基于滤波框架的视觉惯性里程计可分为三类：扩展卡尔曼滤波器（EKF），多状态约束卡尔曼滤波器（MSCKF）和无迹卡尔曼滤波（UKF）

总结：
我们介绍了一个全面的文献综述包含了在非GPS环境的定位方法。这些方法可分为五大类：轮速里程计、惯性里程计、视觉里程计、激光里程计和雷达里程计。每种方法都有一些缺点，主要是在低纹理环境、低光照、阴影、尺度模糊和随时间漂移等条件下进行评估。因此，正如调查中所讨论的，已经提出了这些方法的各种组合和融合，例如可视激光里程计和视觉惯性里程计。车轮里程计是最早的独立定位系统之一，它是利用车轮编码器来估计相对于起点的位置。然而，车轮里程计存在一些缺点，如位置漂移和在不平坦的地形和光滑的路面上不准确。此外，它只能用于地面系统。惯性里程计通过使用加速度计和陀螺传感器的测量值来估计车辆的位置和方向来解决这些缺点。惯性里程计仍然存在漂移问题，因为陀螺仪或加速计中的恒定误差会导致速度的二次误差和位置的三次误差增长。雷达里程计使用天线发射无线电信号来测量车辆周围物体的速度和范围。雷达系统的主要优点是覆盖范围广，不受环境条件（如多云天气）的影响，并且可以在夜间方便地工作，这使得它成为户外应用的合适解决方案。但是，它只能用于目标检测，因为输出分辨率不够高，无法进行目标识别。另一方面，激光雷达发射激光脉冲来探测环境中的物体。与雷达系统相比，激光雷达系统的主要优点是，激光雷达可以使用短波长探测到小目标，并且可以生成周围物体的三维单色图像。然而，它在透明物体（如玻璃）和恶劣天气（如灰尘、雾、雨和雪）方面有局限性。视觉里程计通过从图像中提取关键信息来估计位置和方向。关键信息可以用直接或间接的方法提取。尽管与惯性和轮速里程计相比，视觉里程计提供了更精确的估计，但它仍然存在一些缺点。这些问题主要与计算复杂性和图像条件有关，例如低光照、阴影和低纹理环境。此外，由于视觉里程计是基于相对测量的，因此也存在误差累积引起的漂移问题。提出了视觉里程计的缺点和惯性里程计。基本上，VIO将单个或多个摄像头捕捉到的视觉数据与IMU传感器提供的惯性测量值进行融合，以确定车辆的位置和方向。最先进的VIO方法可以分为松耦合和紧耦合。

以上内容引用自论文：
文章：A Survey on Odometry for Autonomous Navigation Systems
作者：SHERIF A. S. MOHAMED , MOHAMMAD-HASHEM HAGHBAYAN, TOMI WESTERLUND

个人理解：
机器人的全局定位狭义上就是要实现机器人位姿（x,y,z,theta）的时刻确定。目前我们采用了AMCL自适应蒙特卡洛算法来构建我们的概率定位系统，主要依赖激光雷达来获取机器人周围的环境信息，然后把这些周围信息（一帧点云）传入AMCL算法节点来与粒子们的运动预测（期望值）作比较，根据差别大小来更新每个粒子的权重。通过激光雷达来定位建图的方法在自动驾驶中应用十分广泛，目前华为和小鹏的自动驾驶技术也是依赖于激光雷达的数据采集。另外，我们在定位的状态估计阶段也需要里程计和IMU的数据（我猜的）。特别的，我们希望通过哨岗识别来辅助解决重定位的我问题。

什么是粒子？
在粒子滤波中，后验分布的样本，我们称之为“粒子”，每一个粒子都是在时刻t的一个状态的实例化，这个实例化就是在t时刻的真实状态的假设。
我们可以把一个粒子理解为一个假设的机器人，随着机器人移动指令的发布，地图上所有的粒子都会这样移动。

粒子滤波主要步骤如下：
（1）初始化阶段：
规定粒子数量，将粒子平均的分布在规划区域，规划区域需要人为或者通过特征算法计算得出，比如人脸追踪，初始化阶段需要人为标出图片中人脸范围或者使用人脸识别算法识别出人脸区域。对于SLAM来说，规划区域一般为用来进行定位的地图，在初始化时，将需要设置的特定数量粒子均匀的撒满整张地图。
（2）状态转移阶段：
这个阶段所做的任务就是对每个粒子根据状态转移方程进行状态估计，每个粒子将会产生一个与之相对应的预测粒子。这一步同卡尔曼滤波方法相同，只是卡尔曼是对一个状态进行状态估计，粒子滤波是对大量样本（每个粒子即是一个样本）进行状态估计。
注意：里程计数据对粒子状态的更新十分重要，所以里程计的精度是很重要的。
（3）权重校正阶段：
在这一阶段中，算法需要对预测粒子进行评价，越接近于真实状态的粒子，其权重越大，反之，与真实值相差较大的粒子，其权重越小。此步骤是为重采样做准备。在SLAM中权重计算方式有很多，比如机器人行走过程中，激光雷达或者深度摄像头会返回周围位置信息，如果这些信息与期望值相差较大，亦或者在运动中某些粒子本应该没有碰到障碍或者边界，然而在运算中却到达甚至穿过了障碍点或边界，那么这种粒子就是坏点粒子，这样的粒子权重也就比较低一些。
（4）重采样阶段：
根据粒子权重对粒子进行筛选，筛选过程中，既要大量保留权重大的粒子，又要有一小部分权重小的粒子；权重小的粒子有些会被淘汰，为了保证粒子总数不变，一般会在权值较高的粒子附近加入一些新的粒子。
（5）滤波：
将重采样后的粒子带入状态转移方程得到新的预测粒子，然后将它们继续进行上述转移、校正、重采样过程，经过这种循环迭代，最终绝大部分粒子会聚集在与真实值最接近的区域内，从而得到机器人准确的位置，实现定位。
（6）地图生成：
每个粒子都携带一个路径地图，整个过程下来，我们选取最优的粒子，即可获得规划区域的栅格地图。

参考文章原文链接：https://blog.csdn.net/lqygame/article/details/71158422

AMCL包给出了base_frame（机器人）到map_frame（静态地图）的tf变换，在rviz中我们可以发现机器人所在位置有个base坐标系，而地图的原点处有个不动的map坐标系（静态的），这两个坐标系之间有线连接，这种tf变换也就实现了机器人在静态map中的定位。