ROS2(二)- nav2_amcl 代码算法总结

我用的ros2-dashing版本，其他版本的没看过。navigation2代码可以git下载navigation2。

amcl算法全部可以从《概率机器人》书上找到：生成新粒子群(5.4节)、计算权重(6.4节)、重采样算法（8.3.5节）。有了算法看代码就容易很多。

1.粒子滤波概括

• amcl中的粒子样本是位置坐标数据。
• 初始化粒子群时，粒子群按照高斯分布围绕在给定的初始值周围。就是在riviz上点的初始位置。图上的小绿点。
• 计算新时刻的定位时，根据odom数据和上一时刻的粒子坐标，按照高斯分布采样，生成新的粒子群。
• 计算每个粒子的权重，权重是根据粒子当前坐标生成激光雷达数据的概率。
• 对当前粒子群的位置信息($x,y,\theta$)进行聚类，选权重和最大的类，计算均值。

2.程序逻辑

在计算粒子权重后，进行重采样，注入一些随机粒子，提升鲁棒性。

3.算法

3.1 粒子滤波基本算法

• 算法输入
  • $X_{t-1}$是t-1时刻粒子集
  • $u_t$是t-1到t时刻odom信息的变化量
  • $z_t$是雷达观测数据
• 算法首先通过处理粒子集$X_{t-1}$中的每个粒子$x_{t-1}^m$采样生成新粒子$x_t^m$，并计算权重$w_t^m$，构造一个暂时的粒子集${\overline{X}}_t$,表示置信度$\overline{bel}(x_t)$。再将${\overline{X}}_t$转换为粒子集$X_t$，近似用其后验分布$bel(x_t)$。
• 第3行，粒子集循环采样M个粒子
  • 第4行，在给定$t-1$时刻的粒子$x_{t-1}$及$odom$变换量$u_t$数据的条件下，生成$t$时刻粒子$x_t$的概率，按照这个概率采样生成新粒子$x_t$。（从状态转移分布$p(x_t|u_t,x_{t-1})$中采样）
  • 第5行，在$x_t$条件下产生雷达数据$z_t$的概率，作为粒子的权重$w_t$(重要性因子,important factor)
• 8-11行，重采样(resampling)/重要性采样(importance sampling)。
  • 从暂时粒子集${\overline{X}}_t$中抽取M个粒子，抽取的概率按照权值$w_t^m$决定。这样重采样后的粒子集分布发生变化。采样前按照$\overline{bel}(x_t)$分布，重采样后，近似按照后验$bel(x_t)=\eta p(z_t|x_t^m)\overline{bel}(x_t)$分布。
  • 重采样的好处：许多粒子是重复的，但不包含在数据集$X_t$中的粒子都是权重较低的。
  • 另一版本的粒子滤波：不进行重采样，每个粒子都维护一个权重$w_t^m=p(z_t|x_t^m)w_t^{m-1}$，这样算法结果也能近似后验，但其中一些粒子会以低后验概率结束。这样会需要更多的粒子。

3.2 根据odom和旧粒子采样得到新粒子

即$p(x_t|u_t,x_{t-1})$

3.2.1介绍里程计运动模型

在时间$(t-1,t]$内，机器人从位姿 x ‾ t − 1 = { x ‾ , y ‾ , θ ‾ } \overline{x}_{t-1}=\{\overline{x},\overline{y},\overline{\theta}\} xt−1​={x,y​,θ}到 x ‾ t = { x ‾ ′ , y ‾ ′ , θ ‾ ′ } \overline{x}_t=\{\overline{x}^\prime,\overline{y}^\prime,\overline{\theta}^\prime\} xt​={x′,y​′,θ′}（$\overline{x}$代表机器人内部的坐标，此坐标系与全局世界坐标的关系未知）。里程计的相对前进可以写作：
$$u_t=({\overline{x}}_{t-1},{\overline{x}}_t)$$

• $u_t$是${\overline{x}}_{t-1},{\overline{x}}_t$之间的相对差，也是对真实位姿$x_{t-1},x_t$之间差异的很好估计。
• 相对前进可以被转变为三个步骤的序列：
  • 初始旋转${\delta }_{rot1}$
  • 平移${\delta }_{trans}$
  • 第二次旋转${\delta }_{rot2}$

这三个参数一起可以组成由里程计编码的相对运动统计量。

3.2.2算法$p(x_t|u_t,x_{t-1})$

• ${\alpha }_1,{\alpha }_2,{\alpha }_3,{\alpha }_4$是指定机器人运动噪声的特定参数。
  

• 算法输入

  • 粒子的上一时刻位姿 x t − 1 = { x , y , θ } x_{t-1}=\{x,y,\theta\} xt−1​={x,y,θ}
  • 粒子的假定下一时刻姿态： x t = { x ′ , y ′ , θ ′ } x_{t}=\{x^\prime,y^\prime,\theta^\prime\} xt​={x′,y′,θ′}
  • 里程计两时刻的相对前进： u t = { x ‾ , y ‾ , θ ‾ ， x ‾ ′ , y ‾ ′ , θ ‾ ′ } u_t=\{\overline{x},\overline{y},\overline{\theta}，\overline{x}^\prime,\overline{y}^\prime,\overline{\theta}^\prime\} ut​={x,y​,θ，x′,y​′,θ′}

• 第2-4行：计算里程计数据的模型相对运动参数$({\delta }_{rot1},{\delta }_{trans},{\delta }_{rot2}{)}^T$

• 第5-7行：计算粒子数据给定位姿$x_{t-1},x_t$的相对运动模型参数$({\hat{\delta }}_{rot1},{\hat{\delta }}_{trans},{\hat{\delta }}_{rot2}{)}^T$

• 第8-9行：计算各自运动参数的误差概率。

  • 如$prob(a,b^2)$实现$a$上均值为0，方差为$b^2$的误差分布
  • 如$prob({\delta }_{rot1}-{\hat{\delta }}_{rot1},{\alpha }_1{\hat{\delta }}_{rot1}^2+{\alpha }_2{\hat{\delta }}_{trans}^2)$即里程计数据模型参数与粒子数据参数之差，均值为0，方差为${\alpha }_1{\hat{\delta }}_{rot1}^2+{\alpha }_2{\hat{\delta }}_{trans}^2$

• 第11行：由各自误差概率 $p_1,p_2,p_3$相乘得到的组合误差概率（假设不同误差源间相互独立）。

3.3.3采样算法(sample_motion_model_odometry)

将粒子初始姿态$x_{t-1}$和里程计读数$u_t$作为输入，输出一个随机$x_t$服从$p(x_t|u_t,x_{t-1})$的分布

• 算法输入：
  • 旧粒子： x t − 1 = { x , y , θ } x_{t-1}=\{x,y,\theta\} xt−1​={x,y,θ}
  • 里程计两时刻的相对前进： u t = { x ‾ , y ‾ , θ ‾ ， x ‾ ′ , y ‾ ′ , θ ‾ ′ } u_t=\{\overline{x},\overline{y},\overline{\theta}，\overline{x}^\prime,\overline{y}^\prime,\overline{\theta}^\prime\} ut​={x,y​,θ，x′,y​′,θ′}
• 输出：新粒子： x t = { x ′ , y ′ , θ ′ } x_{t}=\{x^\prime,y^\prime,\theta^\prime\} xt​={x′,y′,θ′}
• 第2-4行：计算里程计数据的参数
• 第5-7行：按照高斯采样，得到粒子数据的参数
• 第8-9行：根据采样得到的参数，计算新粒子数据
• 第11行：返回新粒子

3.3计算权重

将传感器光束$z_t^k$扫描到的障碍物, 映射到map坐标系。这需要了解：

• 机器相对map的坐标 x t = { x , y , θ } x_t=\{x,y,\theta\} xt​={x,y,θ}
• 传感器相对机器base_frame的坐标$(x_{k,sense},y_{k,sense})$
• ${\theta }_{k,sense}$表示传感器波束相对机器人航向的角度

那么光束$z_t^k$测量到障碍物在map中的坐标
$$\left[\begin{array}{c}{x}_{z_t^k}\\ y_{z_t^k}\end{array}\right]=\left[\begin{array}{c}x\\ y\end{array}\right]+\left[\begin{array}{cc}\cos \theta & -\sin \theta \\ \sin \theta & \cos \theta \end{array}\right]\left[\begin{array}{c}{x}_{k,sense}\\ y_{k,sense}\end{array}\right]+z_k\left[\begin{array}{c}\cos (\theta +{\theta }_{k,sense})\\ \sin (\theta +{\theta }_{k,sense})\end{array}\right]$$
这些坐标只有当传感器检测到一个障碍物时才是有意义的。如果测距传感器输出了最大值$z=z_{max}$,则这些坐标在物理世界没有任何意义（即使测量的确携带了信息）。似然域测量模型简单地将最大距离读数丢弃。

3.3.1测量过程的三种噪声

测量噪声
测量过程引起的噪声使用高斯进行建模。它涉及寻找地图上最近的障碍物。

• $dist$表示计算出的障碍物坐标 { x z t k , y z t k } \{x_{z_t^k}, y_{z_t^k}\} {xztk​​,yztk​​}与地图上最近障碍物的距离。
• 么传感器测量的概率可以由一个以 为中心的高斯函数给出，该高斯具有传感器噪声：
  $$p_{hit}(z_t^k|x_t,m)={\epsilon }_{{\delta }_{hit}}(dist)$$
  m代表地图。

测量失败

假定最大距离读数具有非常大的似然，可用点群分布$p_{max}$进行建模。

无法解释的随机测量

用一个均匀分布$p_{rand}$为感知中的随机噪声建立模型。

最终期望概率$p(z_t^k|x_t,m)$集成了三种分布
$$p(z_t^k|x_t,m)=z_{hit}{p}_{hit}+z_{rand}{p}_{rand}+z_{max}{p}_{max}$$

3.3.2计算权重算法

• 算法输入：
  • 激光数据$z_t$
  • 粒子数据$x_t$
  • 地图数据$m$
• 第3行：循环每一束激光
  • 第4行：抛弃最大数据
  • 第5-6行：计算map坐标系下激光探测到的障碍物坐标
  • 第7行：计算与最近障碍物的距离
  • 第8行: 得到似然概率结果

3.4 随机粒子

基于某些定位性能的评估增加一些随机粒子，提升鲁棒性。

• 参数${\alpha }_{slow},{\alpha }_{fast}$分别是估计长期和短期平均的指数滤波器的衰减率。
• 看不懂解释
  

每次更新粒子群，也会更新计算$w_{agv},w_{slow},w_{fast}$，并且重采样。重采样时一部分完全随机，另一部分按照权重作为概率抽样。

4.粒子滤波原理

粒子滤波原理推导