无人驾驶8： 粒子滤波定位(优达学城项目)

优达学城无人车定位的项目实现：

粒子滤波算法流程图

粒子滤波的伪代码：

step1:初始化

理论上说，当粒子数量足够多时，能准确地呈现贝叶斯后验分布，如果粒子太少，可能漏掉准确位置，粒子数量太多，会拖慢滤波器，无法实时定位无人车位置。

粒子初始化有两个方法;

1.在状态空间均匀取样；空间太大时(比如全球)，不易实现；

2.在某个初始估算值周围取样；对于无人车，可以用GPS获取估算位置；

这里采用GPS得到一个粗略初始坐标，然后用高斯分布取样，初始化粒子；

代码实现：

void printSamples(double gps_x, double gps_y, double theta) {
  std::default_random_engine gen;
  double std_x, std_y, std_theta;  // Standard deviations for x, y, and theta

  // TODO: Set standard deviations for x, y, and theta
    std_x = 2;
    std_y = 2;
    std_theta = 0.5;

  // This line creates a normal (Gaussian) distribution for x
  normal_distribution<double> dist_x(gps_x, std_x);
  
  // TODO: Create normal distributions for y and theta
    normal_distribution<double> dist_y(gps_y, std_y);
    normal_distribution<double> dist_theta(theta, std_theta);

  for (int i = 0; i < 3; ++i) {
    double sample_x, sample_y, sample_theta;
    
    // TODO: Sample from these normal distributions like this: 
    //   sample_x = dist_x(gen);
    //   where "gen" is the random engine initialized earlier.
    sample_x = dist_x(gen);
    sample_y = dist_y(gen);
    sample_theta = dist_theta(gen);
    
    // Print your samples to the terminal.
    std::cout << "Sample " << i + 1 << " " << sample_x << " " << sample_y << " " 
              << sample_theta << std::endl;
  }

  return;
}

step2: prediction

运用自行车模型，预测车辆在下一个时间步所处的位置；

对于每个粒子，根据其速度和位置值更新粒子位置，需要考虑控制输入中的不确定性，向速度和角速度添加高斯噪声；

自行车模型计算公式：
$x_f = x_0 + \dfrac{v}{ \dot{\theta}}[sin(\theta_0 + \dot(\theta)(dt)) - sin(\theta_0)]$xf​=x0​+θ˙v​[sin(θ0​+(˙​θ)(dt))−sin(θ0​)]

$y_f = y_0 + \dfrac{v}{ \dot{\theta}}[cos(\theta_0) - cos(\theta_0 + \dot{\theta}(dt))]$yf​=y0​+θ˙v​[cos(θ0​)−cos(θ0​+θ˙(dt))]

$\theta_f = \theta_0 + \dot{\theta}(dt)$θf​=θ0​+θ˙(dt)

step3: 测量更新

数据关联

使用周围物体的地标测量值，更新位置信仰之前，必须解决数据关联问题，即测量方法。

数据关联，就是将地标测量值和真实世界的对象相匹配；

最紧邻法:把最近的测量值，作为正确的对应值，优缺点对比

step4 准确性评估

两种方法：

方法一：取所有粒子的加权平均误差；

方法二：取权重最高的粒子；求其方差平方根即可

计算公式：
where:
$Position\_RMSE = \sqrt{(x_p - x_g)^2 + (y_p - y_g)^2}$Position_RMSE=(xp​−xg​)2+(yp​−yg​)2​

$Theta\_RMSE = \sqrt{(\theta_p - \theta_g)^2}$Theta_RMSE=(θp​−θg​)2​

Transformations and Associatio

传感器测量，是以无人车为原点坐标，在车辆坐标系统的值，要做数据管理，必须转化为地图坐标系通。

如图，点OBS1(2,2),OBS2(3,-2),OBS3(0,-4)是无人车传感器测量到的三个地标，而蓝色点P(4,5)是无人车在地图上的坐标。

Homogenous Transformation

$\left[ \begin{array}{c} \text{x}_m \\ \text{y}_m \\ 1 \end{array} \right] = \begin{bmatrix} \cos\theta & -\sin\theta & \text{x}_p \\ \sin\theta & \cos\theta & \text{y}_p \\ 0 & 0 & 1 \end{bmatrix} \times \left[ \begin{array}{c} \text{x}_c \\ \text{y}_c \\ 1 \end{array} \right]$⎣⎡​xm​ym​1​⎦⎤​=⎣⎡​cosθsinθ0​−sinθcosθ0​xp​yp​1​⎦⎤​×⎣⎡​xc​yc​1​⎦⎤​

Matrix multiplication results in:

$\text{x}_m= \text{x}_p + (\cos\theta \times \text{x}_c) - (\sin\theta \times \text{y}_c)$xm​=xp​+(cosθ×xc​)−(sinθ×yc​)

$\text{y}_m= \text{y}_p + (\sin\theta \times \text{x}_c) + (\cos\theta \times \text{y}_c)$ym​=yp​+(sinθ×xc​)+(cosθ×yc​)

根据公式，计算三个点对应的地图坐标：

OBS1(2,2): (6,3)

OBS2(3,-2):(2,2)

OBS3(0,-4) (0,5)

代码实现：

#include <cmath>
#include <iostream>

int main() {
  // define coordinates and theta
  double x_part, y_part, x_obs, y_obs, theta;
  x_part = 4;
  y_part = 5;
  x_obs = 2;
  y_obs = 2;
  theta = -M_PI/2; // -90 degrees

  // transform to map x coordinate
  double x_map;
  x_map = x_part + (cos(theta) * x_obs) - (sin(theta) * y_obs);

  // transform to map y coordinate
  double y_map;
  y_map = y_part + (sin(theta) * x_obs) + (cos(theta) * y_obs);

  // (6,3)
  std::cout << int(round(x_map)) << ", " << int(round((y_map)) << std::endl;

  return 0;
}

Associatio

根据三个观测点的地图坐标OBS1(6,3)，OBS2(2,2)，OBS3：(0,5)，关联最近的地标分别为L1,L2，L2

Calculating the Particle’s Final Weight

对每个粒子，遍历计算每个匹配的地标点，
$P(x,y)= \frac{1}{2\pi \sigma_x\sigma_y}e^{-(\frac{(x-\mu_x)^2}{2\sigma_x^2} + \frac{(y-\mu_y)^2}{2\sigma_y^2})}$P(x,y)=2πσx​σy​1​e−(2σx2​(x−μx​)2​+2σy2​(y−μy​)2​)

得到每个粒子的存在概率，概率值越大，权重越大。

代码实现：

#include <iostream>
#include "multiv_gauss.h"

int main() {
  // define inputs
  double sig_x, sig_y, x_obs, y_obs, mu_x, mu_y;
  // define outputs for observations
  double weight1, weight2, weight3;
  // final weight
  double final_weight;
    
  // OBS1 values
  sig_x = 0.3;
  sig_y = 0.3;
  x_obs = 6;
  y_obs = 3;
  mu_x = 5;
  mu_y = 3;
  // Calculate OBS1 weight
  weight1 = multiv_prob(sig_x, sig_y, x_obs, y_obs, mu_x, mu_y);
  // should be around 0.00683644777551 rounding to 6.84E-3
  std::cout << "Weight1: " << weight1 << std::endl;
    
  // OBS2 values
  sig_x = 0.3;
  sig_y = 0.3;
  x_obs = 2;
  y_obs = 2;
  mu_x = 2;
  mu_y = 1;
  // Calculate OBS2 weight
  weight2 = multiv_prob(sig_x, sig_y, x_obs, y_obs, mu_x, mu_y);
  // should be around 0.00683644777551 rounding to 6.84E-3
  std::cout << "Weight2: " << weight2 << std::endl;
    
  // OBS3 values
  sig_x = 0.3;
  sig_y = 0.3;
  x_obs = 0;
  y_obs = 5;
  mu_x = 2;
  mu_y = 1;
  // Calculate OBS3 weight
  weight3 = multiv_prob(sig_x, sig_y, x_obs, y_obs, mu_x, mu_y);
  // should be around 9.83184874151e-49 rounding to 9.83E-49
  std::cout << "Weight3: " << weight3 << std::endl;
    
  // Output final weight
  final_weight = weight1 * weight2 * weight3;
  // 4.60E-53
  std::cout << "Final weight: " << final_weight << std::endl;
    
  return 0;
}

Kidnapped-Vehicle project

优达学城的无人驾驶第六个项目，粒子滤波器定位，实现代码：

https://github.com/luteresa/P6-Kidnapped-Vehicle.git

运行结果：

Additional Resources on Localization

Simultaneous Localization and Mapping (SLAM)

The below papers cover Simultaneous Localization and Mapping (SLAM) - which as the name suggests, combines localization and mapping into a single algorithm without a map created beforehand.

Past, Present, and Future of Simultaneous Localization And Mapping: Towards the Robust-Perception Age by C. Cadena, et. al.

Abstract: Simultaneous Localization and Mapping (SLAM) consists in the concurrent construction of a model of the environment (the map), and the estimation of the state of the robot moving within it. The SLAM community has made astonishing progress over the last 30 years, enabling large-scale real-world applications, and witnessing a steady transition of this technology to industry. We survey the current state of SLAM. We start by presenting what is now the de-facto standard formulation for SLAM. We then review related work, covering a broad set of topics including robustness and scalability in long-term mapping, metric and semantic representations for mapping, theoretical performance guarantees, active SLAM and exploration, and other new frontiers. […]

Navigating the Landscape for Real-time Localisation and Mapping for Robotics and Virtual and Augmented Reality by S. Saeedi, et. al.

Abstract: Visual understanding of 3D environments in real-time, at low power, is a huge computational challenge. Often referred to as SLAM (Simultaneous Localisation and Mapping), it is central to applications spanning domestic and industrial robotics, autonomous vehicles, virtual and augmented reality. This paper describes the results of a major research effort to assemble the algorithms, architectures, tools, and systems software needed to enable delivery of SLAM, by supporting applications specialists in selecting and configuring the appropriate algorithm and the appropriate hardware, and compilation pathway, to meet their performance, accuracy, and energy consumption goals. […]

Other Methods

The below paper from Udacity’s founder Sebastian Thrun, while from 2002, is still relevant for many different methods of mapping used today in robotics.

Robotic Mapping: A Survey by S. Thrun

Abstract: This article provides a comprehensive introduction into the field of robotic mapping, with a focus on indoor mapping. It describes and compares various probabilistic techniques, as they are presently being applied to a vast array of mobile robot mapping problems. The history of robotic mapping is also described, along with an extensive list of open research problems.