自动驾驶定位技术-马尔科夫定位_马尔可夫定位-CSDN博客

本文链接：https://blog.csdn.net/williamhyin/article/details/105133566

Localization

Github: https://github.com/williamhyin/CarND-Kidnapped-Vehicle

Email: williamhyin@outlook.com

Definition of Localization

汽车会收集当前环境的信息,并和已知地图对比,推断它在世界上的位置. 简而言之, 自动驾驶定位技术就是解决“我在哪儿”的问题, 并且对可靠性和安全性提出了非常高的要求.
自动驾驶中定位的要求: 精确度达到10 cm及以下, 车载传感器可用于估计本地测量值和给定地图之间的转换.
已有的定位解决方案:
- GPS 定位: 精度太低: 1-3 m; 更新速度慢: 10次/s; 容易受到遮挡物的影响, 检测到的卫星数量很少.
- 惯性传感器: 利用加速度计和陀螺仪, 根据上一时刻的位置和方位推断现在的位置和方位, 即航迹推测. 惯性导航和GPS可以结合起来使用, 惯性导航可以弥补GPS更新频率低的缺陷, GPS可以纠正惯性导航的运动误差. 但是, 如果是在地下隧道或者其他信号不好的地方, GPS可能无法及时纠正惯性导航的运动误差.
- 传感器融合: 利用RADAR 和LIDAR, 记录道路外观的图像, 测量车辆到静态物体如建筑物,电线杆,路缘的距离, 将传感器的点云数据与高精地图储存的特征进行匹配, 并实现车辆坐标系与世界坐标系之间的转换, 确定最有可能位于的位置.
- 摄像头: 利用摄像头, 记录道路外观的图像, 将摄像头的数据与地图进行匹配, 确定最有可能位于的位置
自动驾驶定位技术的组合方案：
- 基于 GPS 和惯性传感器的传感器融合
- 基于 LiDAR 点云与高精地图的匹配 (研究方向: 粒子滤波, 深度学习)
- 基于视觉的道路特征识别
应用实例:

首先根据GPS的数据（经纬高和航向）确定无人车大致处于哪条道路上, 这个位置的可能与真实位置有5~10米的差距. 然后根据车载传感器检测的车道线（虚、实线）及道路边缘（路沿或护栏）的距离与高精地图提供的车道线及道路边缘做比对, 然后修正无人车的横向定位. 根据车载传感器检测到的广告牌、红绿灯、墙上的标志、地上的标志（停止线、箭头等）, 与高精地图提供的同一道路特征（POI）进行匹配, 进而修正纵向定位和航向. 在没有检测到任何道路特征的情况下, 可以通过航位推算进行短时间的位置推算. 无人车的定位算法通常采用粒子滤波的方法, 需要多个计算周期后, 定位结果才会收敛, 进而提供一个相对稳定的定位结果.

基于视觉的道路特征识别, 由于容易受到天气的影响, 因此目前主要作为定位补充手段. 而且通常是将Lidar和视觉系统结合进行定位. 这种方法需要预先准备一幅激光雷达制造的3D地图, 用Ground-Plane Sufficient得到一个2D的纯地面模型地图, 用OpenGL将单目视觉图像与这个2D的纯地面模型地图经过坐标变换, 用归一化互信息配准. 然后用扩展卡尔曼滤波器（EKF）来实现定位.

Basic realization of Localization

马尔可夫定位或贝叶斯滤波是通常的定位滤波器. 我们通常认为我们的车辆位置作为一个概率分布,每次我们移动,我们的分布变得更分散(更广泛). 我们将变量(地图数据、观测数据和控制数据)传递到过滤器中,以便在每个时间步骤中集中(缩小)这种分布. 应用过滤器之前的每个状态代表我们的先验,而变窄的分布代表我们的贝叶斯后验.

1. 贝叶斯法则(Bayes Rule)

贝叶斯规则是马尔可夫局部化的基础.

我们可以将贝叶斯规则应用到车辆定位中, 通过在每个时间步骤中传递贝叶斯规则的变量来实现车辆的定位. 这就是所谓的本地化贝叶斯过滤器. 你可能认识到这类似于卡尔曼滤波器. 事实上, 许多定位滤波器,包括卡尔曼滤波器都是贝叶斯滤波器的特例.

P(location|observation): This is P(a|b), the normalized probability of a position given an observation (posterior).

P(observation|location): This is P(b|a), the probability of an observation given a position (likelihood)

P(location): This is P(a), the prior probability of a position (initialized by GPS + IMU)

P(observation): This is P(b), the total probability of an observation (Radar or Lidar)

两个重要的贝叶斯公式：

2. 后验概率(Localization Posterior)

假设我们出门堵车的可能因素有两个: 车辆太多和交通事故.

那么如果我们出门之前我们听到新闻说今天路上出了个交通事故,那么我们想算一下堵车的概率, 这个就叫做条件概率 .也就是P(堵车|交通事故).这是有因求果.

如果我们已经出了门, 然后遇到了堵车, 那么我们想算一下堵车时由交通事故引起的概率有多大,

那这个就叫做后验概率 (也是条件概率,但是通常习惯这么说).也就是P(交通事故|堵车).这是有果求因.
我们的目标是估计状态信度 bel (xt) ,不需要使用我们的整个观察历史. 我们将通过操作后验 *p(xt∣z1:t−1,μ1:t,m)得到一个递归状态估计量来实现这一点. 为此,我们必须证明我们当前的信度bel (xt)*可以用早一步的信度 *bel (xt-1)*来表示, 然后使用新的数据只更新当前的信度. 这种递归过滤器称为贝叶斯定位过滤器或马尔可夫定位过滤器, 使我们能够避免携带历史观测和运动数据. 我们将使用贝叶斯规则, 全概率公式和马尔可夫假设来实现这种递归状态估计器.

z_{1:t} represents the observation vector from time 0 to t (range measurements, bearing, images,etc.).

u_{1:t} represents the control vector from time 0 to t (yaw/pitch/roll rates and velocities).

m represents the map (grid maps, feature maps, landmarks)

xt represents the pose (position (x,y) + orientation \θ)
通过将观测向量 z1:t 分解为当前观测值 zt 和先前信息 z1:t−1, 我们向递归结构迈出了第一步.

现在, 我们应用贝叶斯规则:

值得注意的是, 当前观察都不包括在运动模型中. 我们将正规化因子定义为n, 1/n 为所有可能状态xt(i)上的观察和运动模型信度的总和. 也就是说我们只需要定义观察和运动模型来估算信度.