1.前言
由于所有的传感器都是带有噪声的，将机器人感知模型也就是测量模型同样用条件概率分布 p ( z t ∣ x t , m ) p(z_t|x_t,m) p(zt∣xt,m)进行表示已经成为约定俗成或者说水到渠成的事情了。其中 x t x_t xt代表了机器人的位姿， z t z_t zt代表传感器的测量值， m m m代表环境地图。
2.光束模型(Beam Models)
(1)四种测量模型分布
测量噪声
p
h
i
t
p_{hit}
phit
测量噪声被认为是一个高斯分布，由下面公式给出：
p
h
i
t
(
z
t
k
∣
x
t
,
m
)
=
{
η
N
(
z
t
k
;
z
t
k
∗
,
σ
h
i
t
2
)
,
0
≤
z
t
k
∗
≤
z
m
a
x
0
,
其
他
p_{hit}(z_t^k|x_t,m)=\begin{cases}\eta N(z_t^k;z_t^{k*},\sigma_{hit}^2),0\leq z_t^{k*}\leq z_{max}\\ 0,其他\end{cases}
phit(ztk∣xt,m)={ηN(ztk;ztk∗,σhit2),0≤ztk∗≤zmax0,其他其中，
z
t
k
∗
z_t^{k*}
ztk∗是传感器测量的均值，代表“真实”的距离。
N
(
z
t
k
;
z
t
k
∗
,
σ
h
i
t
2
)
N(z_t^k;z_t^{k*},\sigma_{hit}^2)
N(ztk;ztk∗,σhit2)是以
z
t
k
∗
z_t^{k*}
ztk∗为均值，
σ
h
i
t
2
\sigma_{hit}^2
σhit2为方差的高斯分布，归一化因子
η
=
(
∫
0
m
a
x
N
(
z
t
k
;
z
t
k
∗
,
σ
h
i
t
2
)
d
z
t
k
)
−
1
\eta=(\int_0^{max}N(z_t^k;z_t^{k*},\sigma_{hit}^2)dz_t^k)^{-1}
η=(∫0maxN(ztk;ztk∗,σhit2)dztk)−1
意外障碍物
意外障碍物的出现使得传感器对环境的测量失真，将意外障碍物作为传感器从误差是一种比较好的处理方式，当有障碍物时，传感器对目标的检测的可能性随着距离的增加而减小，换句话说，当传感器距离环境目标越近的时候，被障碍物阻挡的概率越低，检测成功的可能性就越大，因此用指数分布来描述这个误差。
p
s
h
o
r
t
(
z
t
k
∣
x
t
,
m
)
=
{
η
λ
s
h
o
r
t
e
−
λ
s
h
o
r
t
z
t
k
0
≤
z
t
k
≤
z
t
k
∗
0
,
其
他
p_{short}(z_t^k|x_t,m)=\begin{cases}\eta \lambda_{short}e^{-\lambda_{short}z_t^k} \,0\leq z_t^k\leq z_t^{k*}\\0,其他\end{cases}
pshort(ztk∣xt,m)={ηλshorte−λshortztk0≤ztk≤ztk∗0,其他其中，
λ
s
h
o
r
t
\lambda_{short}
λshort是测量模型的固有参数，归一化因子
η
=
1
1
−
e
−
λ
s
h
o
r
t
z
t
k
∗
\eta=\frac{1}{1-e^{-\lambda_{short}z_t^{k*}}}
η=1−e−λshortztk∗1
测量失败
传感器由于发生镜面反射或者超过最大测量距离导致检测失败，结果就是返回的测量值为最大测量距离。将
p
m
a
x
p_{max}
pmax用一个很窄的以
z
m
a
x
z_{max}
zmax为中心的均匀分布来模拟概率密度函数。
p
m
a
x
(
z
t
k
∣
x
t
,
m
)
=
{
1
,
z
=
z
m
a
x
0
,
其
他
p_{max}(z_t^k|x_t,m)=\begin{cases}1,z=z_{max}\\0,其他\end{cases}
pmax(ztk∣xt,m)={1,z=zmax0,其他
随机测量
用一个均匀分布来描述传感器的随机测量，即在传感器的测量范围内，任何一个点都是有可能被测量到的，而且概率是一样的，用来解释无法解释的传感器测量值。
p
r
a
n
d
(
z
t
k
∣
x
t
,
m
)
=
{
1
z
m
a
x
,
0
≤
z
t
k
≤
z
m
a
x
0
,
其
他
p_{rand}(z_t^k|x_t,m)=\begin{cases}\frac{1}{z_{max}},0\leq z_t^k\leq z_{max}\\0,其他\end{cases}
prand(ztk∣xt,m)={zmax1,0≤ztk≤zmax0,其他
(2)混合分布
通过四个加权参数
z
h
i
t
,
z
s
h
o
r
t
,
z
m
a
x
,
z
r
a
n
d
z_{hit},z_{short},z_{max},z_{rand}
zhit,zshort,zmax,zrand将上述四个误差模型进行加权：
p
(
z
t
k
∣
x
t
,
m
)
=
[
z
h
i
t
z
s
h
o
r
t
z
m
a
x
z
r
a
n
d
]
⋅
[
p
h
i
t
(
z
t
k
∣
x
t
,
m
)
p
s
h
o
r
t
(
z
t
k
∣
x
t
,
m
)
p
m
a
x
(
z
t
k
∣
x
t
,
m
)
p
r
a
n
d
(
z
t
k
∣
x
t
,
m
)
]
p(z_t^k|x_t,m)=\begin{bmatrix}z_{hit}\\z_{short}\\z_{max}\\z_{rand}\end{bmatrix}·\begin{bmatrix}p_{hit}(z_t^k|x_t,m)\\p_{short}(z_t^k|x_t,m)\\p_{max}(z_t^k|x_t,m)\\p_{rand}(z_t^k|x_t,m)\end{bmatrix}
p(ztk∣xt,m)=⎣⎢⎢⎡zhitzshortzmaxzrand⎦⎥⎥⎤⋅⎣⎢⎢⎡phit(ztk∣xt,m)pshort(ztk∣xt,m)pmax(ztk∣xt,m)prand(ztk∣xt,m)⎦⎥⎥⎤
(3)程序流程
由于光束模型缺乏光滑的过渡，在复杂的环境中的效果不是很好，因此这里仅仅当做了解，不再做深入的推导和补充，后续用到的话再回来补上。
3.似然场模型
似然场模型类似于对环境进行高斯平滑，从而避免光束模型的缺点。其他类似于光束模型。
(1)测量模型分布
测量噪声
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_{\sigma_{hit}}(dist)
phit(ztk∣xt,m)=εσhit(dist)其中，
d
i
s
t
dist
dist是传感器坐标
(
x
z
t
k
,
y
z
t
k
)
T
(x_{z_t^k},y_{z_t^k})^T
(xztk,yztk)T与地图上最近点之间的距离。
测量失败和随机测量前面已经叙述，这里不再重复。
(2)混合分布
似然场模型的混合分布也是上述三种测量模型分布的叠加：
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)程序流程
4：如果测量值达到了最大值，舍去
5，6：将障碍物在传感器中的局部坐标转换到
x
−
y
x-y
x−y全局坐标
7：计算
x
−
y
x-y
x−y空间中与最近障碍物之间的距离
8：将三种分布进行混合，其中，
p
r
o
b
(
d
i
s
t
,
σ
h
i
t
)
prob(dist,\sigma_{hit})
prob(dist,σhit)计算以0为中心，标准差为
σ
h
i
t
\sigma_{hit}
σhit的高斯分布关于
d
i
s
t
dist
dist的概率。
扫一扫
1596
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
