我用的ros2-dashing版本,其他版本的没看过。navigation2代码可以git下载navigation2。
amcl算法全部可以从《概率机器人》书上找到:生成新粒子群(5.4节)、计算权重(6.4节)、重采样算法(8.3.5节)。有了算法看代码就容易很多。
文章目录
1.粒子滤波概括
- amcl中的粒子样本是位置坐标数据。
- 初始化粒子群时,粒子群按照高斯分布围绕在给定的初始值周围。就是在riviz上点的初始位置。图上的小绿点。
- 计算新时刻的定位时,根据odom数据和上一时刻的粒子坐标,按照高斯分布采样,生成新的粒子群。
- 计算每个粒子的权重,权重是根据粒子当前坐标生成激光雷达数据的概率。
- 对当前粒子群的位置信息( x , y , θ x,y,\theta x,y,θ)进行聚类,选权重和最大的类,计算均值。
![](https://i-blog.csdnimg.cn/blog_migrate/0f116c6bf6f8de5014751029b742e3b1.png)
2.程序逻辑
在计算粒子权重后,进行重采样,注入一些随机粒子,提升鲁棒性。
![](https://i-blog.csdnimg.cn/blog_migrate/ef4ea505f70112da76c7f4fddba984f8.png)
3.算法
3.1 粒子滤波基本算法
![](https://i-blog.csdnimg.cn/blog_migrate/4683dd8397d5318fa573e4b25e5872f8.png)
- 算法输入
- X t − 1 X_{t-1} Xt−1是t-1时刻粒子集
- u t u_t ut是t-1到t时刻odom信息的变化量
- z t z_t zt是雷达观测数据
- 算法首先通过处理粒子集 X t − 1 X_{t-1} Xt−1中的每个粒子 x t − 1 m x_{t-1}^m xt−1m采样生成新粒子 x t m x_t^m xtm,并计算权重 w t m w_t^m wtm,构造一个暂时的粒子集 X ‾ t \overline{X}_t Xt,表示置信度 b e l ‾ ( x t ) \overline{bel}(x_t) bel(xt)。再将 X ‾ t \overline{X}_t Xt转换为粒子集 X t X_t Xt,近似用其后验分布 b e l ( x t ) bel(x_t) bel(xt)。
- 第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。(从状态转移分布 p ( x t ∣ u t , x t − 1 ) p(x_t|u_t,x_{t-1}) p(xt∣ut,xt−1)中采样)
- 第5行,在 x t x_t xt条件下产生雷达数据 z t z_t zt的概率,作为粒子的权重 w t w_t wt(重要性因子,important factor)
- 8-11行,重采样(resampling)/重要性采样(importance sampling)。
- 从暂时粒子集 X ‾ t \overline{X}_t Xt中抽取M个粒子,抽取的概率按照权值 w t m w_t^m wtm决定。这样重采样后的粒子集分布发生变化。采样前按照 b e l ‾ ( x t ) \overline{bel}(x_t) bel(xt)分布,重采样后,近似按照后验 b e l ( x t ) = η p ( z t ∣ x t m ) b e l ‾ ( x t ) bel(x_t)=\eta p(z_t|x_t^m)\overline{bel}(x_t) bel(xt)=ηp(zt∣xtm)bel(xt)分布。
- 重采样的好处:许多粒子是重复的,但不包含在数据集 X t X_t Xt中的粒子都是权重较低的。
- 另一版本的粒子滤波:不进行重采样,每个粒子都维护一个权重 w t m = p ( z t ∣ x t m ) w t m − 1 w_t^m = p(z_t|x_t^m)w_t^{m-1} wtm=p(zt∣xtm)wtm−1,这样算法结果也能近似后验,但其中一些粒子会以低后验概率结束。这样会需要更多的粒子。
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′,θ′}(
x
‾
\overline{x}
x代表机器人内部的坐标,此坐标系与全局世界坐标的关系未知)。里程计的相对前进可以写作:
u
t
=
(
x
‾
t
−
1
,
x
‾
t
)
u_t=(\overline{x}_{t-1},\overline{x}_t)
ut=(xt−1,xt)
- u t u_t ut是 x ‾ t − 1 , x ‾ t \overline{x}_{t-1},\overline{x}_t xt−1,xt之间的相对差,也是对真实位姿 x t − 1 , x t x_{t-1},x_t 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
![](https://i-blog.csdnimg.cn/blog_migrate/b11d98bc0dfc42f45fd3b99516cba232.png)
这三个参数一起可以组成由里程计编码的相对运动统计量。
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是指定机器人运动噪声的特定参数。
-
算法输入
- 粒子的上一时刻位姿 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行:计算里程计数据的模型相对运动参数 ( δ r o t 1 , δ t r a n s , δ r o t 2 ) T (\delta_{rot1},\delta_{trans},\delta_{rot2})^T (δrot1,δtrans,δrot2)T
-
第5-7行:计算粒子数据给定位姿 x t − 1 , x t x_{t-1},x_t xt−1,xt的相对运动模型参数 ( δ ^ r o t 1 , δ ^ t r a n s , δ ^ r o t 2 ) T (\hat{\delta}_{rot1},\hat{\delta}_{trans},\hat{\delta}_{rot2})^T (δ^rot1,δ^trans,δ^rot2)T
-
第8-9行:计算各自运动参数的误差概率。
- 如 p r o b ( a , b 2 ) prob(a,b^2) prob(a,b2)实现 a a a上均值为0,方差为 b 2 b^2 b2的误差分布
- 如 p r o b ( δ r o t 1 − δ ^ r o t 1 , α 1 δ ^ r o t 1 2 + α 2 δ ^ t r a n s 2 ) prob(\delta_{rot1} -\hat{\delta}_{rot1},\alpha_1\hat{\delta}_{rot1}^2 + \alpha_2\hat{\delta}_{trans}^2) prob(δrot1−δ^rot1,α1δ^rot12+α2δ^trans2)即里程计数据模型参数与粒子数据参数之差,均值为0,方差为 α 1 δ ^ r o t 1 2 + α 2 δ ^ t r a n s 2 \alpha_1\hat{\delta}_{rot1}^2 + \alpha_2\hat{\delta}_{trans}^2 α1δ^rot12+α2δ^trans2
-
第11行:由各自误差概率 p 1 , p 2 , p 3 p_1,p_2,p_3 p1,p2,p3相乘得到的组合误差概率(假设不同误差源间相互独立)。
3.3.3采样算法(sample_motion_model_odometry)
将粒子初始姿态
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计算权重算法
![](https://i-blog.csdnimg.cn/blog_migrate/108488f24bfa291377bba0203ee8cbb2.png)
- 算法输入:
- 激光数据 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,并且重采样。重采样时一部分完全随机,另一部分按照权重作为概率抽样。