nav2_amcl 代码算法总结
我用的ros2-dashing版本，其他版本的没看过。navigation2代码可以git下载navigation2。
amcl算法全部可以从《概率机器人》书上找到：生成新粒子群(5.4节)、计算权重(6.4节)、重采样算法（8.3.5节）。有了算法看代码就容易很多。
文章目录
- 1.粒子滤波概括
- 2.程序逻辑
- 3.算法
- 3.1 粒子滤波基本算法
- 3.2 根据odom和旧粒子采样得到新粒子
- 3.2.1介绍里程计运动模型
- 3.2.2算法 p ( x t ∣ u t , x t − 1 ) p(x_t|u_t,x_{t-1}) p(xt∣ut,xt−1)
- 3.3.3采样算法
- 3.3计算权重
- 3.3.1测量过程的三种噪声
- 3.3.2计算权重算法
- 3.4 随机粒子
- 4.粒子滤波原理
1.粒子滤波概括
- amcl中的粒子样本是位置坐标数据。
- 初始化粒子群时，粒子群按照高斯分布围绕在给定的初始值周围。就是在riviz上点的初始位置。图上的小绿点。
- 计算新时刻的定位时，根据odom数据和上一时刻的粒子坐标，按照高斯分布采样，生成新的粒子群。
- 计算每个粒子的权重，权重是根据粒子当前坐标生成激光雷达数据的概率。
- 对当前粒子群的位置信息( x , y , θ x,y,\theta x,y,θ)进行聚类，选权重和最大的类，计算均值。
2.程序逻辑
在计算粒子权重后，进行重采样，注入一些随机粒子，提升鲁棒性。
3.算法
3.1 粒子滤波基本算法
- 算法输入
- X t − 1 X_{t-1} Xt−1是t-1时刻粒子集
- u t u_t ut是t-1到t时刻odom信息的变化量
- z t z_t zt是雷达观测数据
- 第3行，粒子集循环采样M个粒子
- 第4行，在给定 t − 1 t-1 t−1时刻的粒子 x t − 1 x_{t-1} xt−1及 o d o m odom odom变换量 u t u_t ut数据的条件下，生成 t t t时刻粒子 x t x_t xt的概率，按照这个概率采样生成新粒子 x t x_t xt
- 第5行，在 x t x_t xt条件下产生雷达数据 z t z_t zt的概率，作为粒子的权重 w t w_t wt
3.2 根据odom和旧粒子采样得到新粒子
即 p ( x t ∣ u t , x t − 1 ) p(x_t|u_t,x_{t-1}) p(xt∣ut,xt−1)
3.2.1介绍里程计运动模型
在时间 ( t − 1 , t ] (t-1,t] (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′,θ′}。里程计的相对前进可以写作：
u t = ( x ‾ t − 1 , x ‾ t ) u_t=(\overline{x}_{t-1},\overline{x}_t) ut=(xt−1,xt)
相对前进可以被转变为三个步骤的序列：
- 初始旋转 δ r o t 1 \delta_{rot1} δrot1
- 平移 δ t r a n s \delta_{trans} δtrans
- 第二次旋转 δ r o t 2 \delta_{rot2} δrot2
这三个参数一起可以组成由里程计编码的相对运动统计量。
利用里程计的相对前进可以估计两时刻粒子 x t − 1 x_{t-1} xt−1和 x t x_{t} xt之间的相对差。
3.2.2算法 p ( x t ∣ u t , x t − 1 ) p(x_t|u_t,x_{t-1}) p(xt∣ut,xt−1)
-
α 1 , α 2 , α 3 , α 4 \alpha_1,\alpha_2,\alpha_3,\alpha_4 α1,α2,α3,α4是指定机器人运动噪声的特定参数。
-
p r o b ( a , b 2 ) prob(a,b^2) prob(a,b2)是均值为0，方差为 b 2 b^2 b2的高斯分布，值为a的概率。
-
算法输入
- 粒子的上一时刻位姿 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行：计算里程计数据的模型参数
-
第5-7行：计算粒子数据的模型参数
-
第8-9行：计算各自运动参数的误差概率。
-
第11行：由各自误差概率 p 1 , p 2 , p 3 p_1,p_2,p_3 p1,p2,p3相乘得到的组合误差概率。
3.3.3采样算法
将粒子初始姿态 x t − 1 x_{t-1} xt−1和里程计读数 u t u_t ut作为输入，输出一个随机 x t x_t xt服从 p ( x t ∣ u t , x t − 1 ) p(x_t|u_t,x_{t-1}) p(xt∣ut,xt−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 z_t^k ztk扫描到的障碍物, 映射到map坐标系。这需要了解：
- 机器相对map的坐标 x t = { x , y , θ } x_t=\{x,y,\theta\} xt={x,y,θ}
- 传感器相对机器base_frame的坐标 ( x k , s e n s e , y k , s e n s e ) (x_{k,sense},y_{k,sense}) (xk,sense,yk,sense)
- θ k , s e n s e \theta_{k,sense} θk,sense表示传感器波束相对机器人航向的角度
那么光束 z t k z_t^k ztk测量到障碍物在map中的坐标
[ x z t k y z t k ] = [ x y ] + [ cos θ − sin θ sin θ cos θ ] [ x k , s e n s e y k , s e n s e ] + z k [ cos ( θ + θ k , s e n s e ) sin ( θ + θ k , s e n s e ) ] \begin{bmatrix}x_{z_t^k} \\ y_{z_t^k} \end{bmatrix} = \begin{bmatrix}x \\ y \end{bmatrix} + \begin{bmatrix}\cos\theta & -\sin\theta \\ \sin\theta & \cos\theta \end{bmatrix} \begin{bmatrix}x_{k,sense} \\ y_{k,sense} \end{bmatrix}+ z_k\begin{bmatrix}\cos(\theta + \theta_{k,sense}) \\ \sin(\theta + \theta_{k,sense}) \end{bmatrix} [xztkyztk]=[xy]+[cosθsinθ−sinθcosθ][xk,senseyk,sense]+zk[cos(θ+θk,sense)sin(θ+θk,sense)]
这些坐标只有当传感器检测到一个障碍物时才是有意义的。如果测距传感器输出了最大值 z = z m a x z= z_{max} z=zmax,则这些坐标在物理世界没有任何意义（即使测量的确携带了信息）。似然域测量模型简单地将最大距离读数丢弃。
3.3.1测量过程的三种噪声
测量噪声
测量过程引起的噪声使用高斯进行建模。它涉及寻找地图上最近的障碍物。
- d i s t dist dist表示计算出的障碍物坐标 { x z t k , y z t k } \{x_{z_t^k}, y_{z_t^k}\} {xztk,yztk}与地图上最近障碍物的距离。
- 么传感器测量的概率可以由一个以 为中心的高斯函数给出，该高斯具有传感器噪声：
p h i t ( z t k ∣ x t , m ) = ε δ h i t ( d i s t ) p_{hit}(z_t^k|x_t,m)=\varepsilon_{\delta_{hit}}(dist) phit(ztk∣xt,m)=εδhit(dist)
m代表地图。
测量失败
假定最大距离读数具有非常大的似然，可用点群分布 p m a x p_{max} pmax进行建模。
无法解释的随机测量
用一个均匀分布 p r a n d p_{rand} prand为感知中的随机噪声建立模型。
最终期望概率 p ( z t k ∣ x t , m ) p(z_t^k|x_t,m) p(ztk∣xt,m)集成了三种分布
p ( z t k ∣ x t , m ) = z h i t p h i t + z r a n d p r a n d + z m a x p m a x p(z_t^k|x_t,m)=z_{hit}p_{hit} +z_{rand}p_{rand} +z_{max}p_{max} p(ztk∣xt,m)=zhitphit+zrandprand+zmaxpmax
3.3.2计算权重算法
- 算法输入：
- 激光数据 z t z_{t} zt
- 粒子数据 x t x_t xt
- 地图数据 m m m
- 第3行：循环每一束激光
- 第4行：抛弃最大数据
- 第5-6行：计算map坐标系下激光探测到的障碍物坐标
- 第7行：计算与最近障碍物的距离
- 第8行: 得到似然概率结果
3.4 随机粒子
基于某些定位性能的评估增加一些随机粒子，提升鲁棒性。
- 参数 α s l o w , α f a s t \alpha_{slow},\alpha_{fast} αslow,αfast分别是估计长期和短期平均的指数滤波器的衰减率。
- 看不懂解释
每次更新粒子群，也会更新计算 w a g v , w s l o w , w f a s t w_{agv},w_{slow},w_{fast} wagv,wslow,wfast，并且重采样。重采样时一部分完全随机，另一部分按照权重作为概率抽样。
4.粒子滤波原理
粒子滤波原理推导