概率机器人：测距仪的似然域模型
测距仪的似然域模型
似然域模型（Likelihood field model）克服了波束模型的局限性，能够在混乱小空间内，得到较为光滑的后验并大幅提高计算效率。
似然域模型是一种特设（ad hoc）算法，无合适的物理解释。
将障碍物检测的似然描述为地图坐标的函数，称为似然域。
模型建立
首先，模型将传感器扫描终点
z
t
z_t
zt映射至地图的全局坐标系（Map坐标系）。
假设机器人在时刻
t
t
t的位姿
x
t
=
[
X
Y
θ
]
T
x_t=begin{bmatrix}X&Y&thetaend{bmatrix}^T
xt=[XYθ]T，传感器相对于机器人坐标系（Footprint坐标系）的坐标为
[
X
k
,
s
e
n
s
Y
k
,
s
e
n
s
]
T
begin{bmatrix}X_{k,sens}&Y_{k,sens}end{bmatrix}^T
[Xk,sensYk,sens]T，传感器波束相对机器人航向角的偏角为
θ
k
,
s
e
n
s
theta_{k,sens}
θk,sens，传感器的测量终点为
z
t
k
z_t^k
ztk，如图所示：
则通过2D平面坐标变换公式，可以得到传感器相对地图坐标系的坐标
[
X
′
Y
′
]
T
begin{bmatrix}X^{'}&Y^{'}end{bmatrix}^T
[X′Y′]T表达式
[
X
′
Y
′
]
=
[
X
Y
]
+
[
cos
θ
−
sin
θ
sin
θ
cos
θ
]
[
X
k
,
s
e
n
s
Y
k
,
s
e
n
s
]
begin{bmatrix}X^{'}\Y^{'}end{bmatrix}=begin{bmatrix}X\Yend{bmatrix}+begin{bmatrix}costheta&-sintheta\sintheta&costhetaend{bmatrix}begin{bmatrix}X_{k,sens}\Y_{k,sens}end{bmatrix}
[X′Y′]=[XY]+[cosθsinθ−sinθcosθ][Xk,sensYk,sens]
随后，考虑测量终点
z
t
k
z_t^k
ztk在地图坐标系中的坐标
[
X
z
t
k
Y
z
t
k
]
T
begin{bmatrix}X_{z_t^k}&Y_{z_t^k}end{bmatrix}^T
[XztkYztk]T：
[
X
z
t
k
Y
z
t
k
]
=
[
X
′
Y
′
]
+
z
t
k
[
cos
(
θ
+
θ
k
,
s
e
n
s
)
sin
(
θ
+
θ
k
,
s
e
n
s
)
]
begin{bmatrix}X_{z_t^k}\Y_{z_t^k}end{bmatrix}=begin{bmatrix}X^{'}\Y^{'}end{bmatrix}+z_t^kbegin{bmatrix}cos(theta+theta_{k,sens})\sin(theta+theta_{k,sens})end{bmatrix}
[XztkYztk]=[X′Y′]+ztk[cos(θ+θk,sens)sin(θ+θk,sens)]
带入传感器坐标，可以得到：
[
X
z
t
k
Y
z
t
k
]
=
[
X
Y
]
+
[
cos
θ
−
sin
θ
sin
θ
cos
θ
]
[
X
k
,
s
e
n
s
Y
k
,
s
e
n
s
]
+
z
t
k
[
cos
(
θ
+
θ
k
,
s
e
n
s
)
sin
(
θ
+
θ
k
,
s
e
n
s
)
]
begin{bmatrix}X_{z_t^k}\Y_{z_t^k}end{bmatrix}=begin{bmatrix}X\Yend{bmatrix}+begin{bmatrix}costheta&-sintheta\sintheta&costhetaend{bmatrix}begin{bmatrix}X_{k,sens}\Y_{k,sens}end{bmatrix}+z_t^kbegin{bmatrix}cos(theta+theta_{k,sens})\sin(theta+theta_{k,sens})end{bmatrix}
[XztkYztk]=[XY]+[cosθsinθ−sinθcosθ][Xk,sensYk,sens]+ztk[cos(θ+θk,sens)sin(θ+θk,sens)]
需注意，当测距仪传回的测量值
z
t
k
=
z
m
a
x
z_t^k=z_{max}
ztk=zmax时，表明存在异常噪声，该坐标在物理世界无意义，似然域模型将丢弃该读数。同波束模型一样，似然域模型考虑了三种噪声模型：
噪声一：测量噪声模型
对于测量过程中引起的噪声采用高斯分布进行建模，设在地图环境
m
m
m中与测量坐标
[
X
z
t
k
Y
z
t
k
]
T
begin{bmatrix}X_{z_t^k}&Y_{z_t^k}end{bmatrix}^T
[XztkYztk]T最近的障碍物之间的欧氏距离为
d
i
s
t
dist
dist，则测量噪声可建模为均值为0，方差为
σ
h
i
t
2
sigma_{hit}^2
σhit2的高斯分布：
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)
式中，标准差
σ
h
i
t
sigma_{hit}
σhit为模型的固有参数。
如下左图，机器人在包含三个障碍物（灰色）的地图环境下进行测量，得到的测量点高斯似然如右图所示，图中越黑的地方存在障碍物的可能性越小；虚线表示传感器轴方向：
概率密度
p
h
i
t
p_{hit}
phit可由传感器轴正交归一化的似然域得到，如下所示为概率
p
h
i
t
(
z
t
k
)
p_{hit}(z_t^k)
phit(ztk)的曲线。该曲线表明传感器测量到三个障碍物对应曲线上
o
1
、
o
2
、
o
3
o_1、o_2、o_3
o1、o2、o3所在位置。
噪声二：失败噪声模型
当传感器返回
z
t
k
=
z
m
a
x
z_t^k=z_{max}
ztk=zmax时，认为传感器测量失败，返回最大量程值
z
m
a
x
z_{max}
zmax。假定此时具有非常大的似然，则可用点群
p
m
a
x
p_{max}
pmax进行建模：
p
m
a
x
(
z
t
k
∣
x
t
,
m
)
=
I
(
z
=
z
m
a
x
)
=
{
1
z
=
z
m
a
x
0
其
他
p_{max}(z_t^k|x_t,m)=I(z=z_{max})=begin{cases}1quad&z=z_{max}\0quad&其他end{cases}
pmax(ztk∣xt,m)=I(z=zmax)={10z=zmax其他
噪声三：随机噪声模型
由于传感器在测量过程中，可能会出现无法解释的噪声现象，用均匀分布
p
r
a
n
d
p_{rand}
prand对其进行建模：
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}}quad&0le z_t^k<z_{max}\ 0quad&其他end{cases}
prand(ztk∣xt,m)={zmax100≤ztk<zmax其他
似然域模型
由此，同波束模型一样，采用混合参数混合三种噪声，得到似然域模型如图所示：
其表达式如下：
p
(
z
t
k
∣
x
t
,
m
)
=
[
z
h
i
t
z
m
a
x
z
r
a
n
d
]
T
⋅
[
p
h
i
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_{max}\z_{rand}end{bmatrix}^Tcdotbegin{bmatrix}p_{hit}(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)=⎣⎡zhitzmaxzrand⎦⎤T⋅⎣⎡phit(ztk∣xt,m)pmax(ztk∣xt,m)prand(ztk∣xt,m)⎦⎤
式中，混合权数
z
h
i
t
、
z
m
a
x
、
z
r
a
n
d
z_{hit}、z_{max}、z_{rand}
zhit、zmax、zrand和为1：
z
h
i
t
+
z
m
a
x
+
z
r
a
n
d
=
1
z_{hit}+z_{max}+z_{rand}=1
zhit+zmax+zrand=1
模型算法
系统输入： 完整的一组测量数据
z
t
z_t
zt、机器人位姿
x
t
x_t
xt、地图信息
m
m
m
系统输出： 测量数据的可能性
q
q
q
A
l
g
o
r
i
t
h
m
l
i
k
e
l
i
h
o
o
d
_
f
i
e
l
d
_
r
a
n
g
e
_
f
i
n
d
e
r
_
m
o
d
e
l
(
z
t
,
x
t
,
m
)
:
1
:
q
=
1
2
:
f
o
r
a
l
l
k
d
o
3
:
i
f
z
t
k
≠
z
m
a
x
4
:
X
z
t
k
=
X
+
X
k
,
s
e
n
s
cos
θ
−
Y
k
,
s
n
e
s
sin
θ
+
z
t
k
cos
(
θ
+
θ
k
,
s
e
n
s
)
5
:
Y
z
t
k
=
Y
+
Y
k
,
s
e
n
s
cos
θ
+
X
k
,
s
n
e
s
sin
θ
+
z
t
k
sin
(
θ
+
θ
k
,
s
e
n
s
)
6
:
d
i
s
t
=
min
X
d
,
Y
d
{
(
X
z
t
k
−
X
d
)
2
+
(
Y
z
t
k
−
Y
d
)
2
∣
⟨
X
d
,
Y
d
⟩
o
c
c
u
p
i
e
d
i
n
m
}
7
:
q
=
q
⋅
(
z
h
i
t
⋅
p
r
o
b
(
d
i
s
t
,
σ
h
i
t
)
+
z
r
a
n
d
z
m
a
x
)
8
:
r
e
t
u
r
n
q
begin{aligned} &Algorithmquad likelihood_field_range_finder_model(z_t,x_t,m):\ 1:&qquad q=1 \ 2:&qquad forenspace allenspace kenspace do \ 3:&qquadqquad if z_t^k neq z_{max}\ 4:&qquadqquadqquad X_{z_t^k}=X+X_{k,sens}costheta-Y_{k,snes}sintheta+z_t^kcos(theta+theta_{k,sens})\ 5:&qquadqquadqquad Y_{z_t^k}=Y+Y_{k,sens}costheta+ X_{k,snes}sintheta+z_t^ksin(theta+theta_{k,sens})\ 6:&qquadqquadqquad dist=min_{X_d,Y_d}Biggl{sqrt{(X_{z_t^k}-X_d)^2+(Y_{z_t^k}-Y_d)^2}Biggvert langle X_d,Y_drangleenspace occupiedenspace inenspace mBiggr}\ 7:&qquadqquadqquad q=qcdot Bigl(z_{hit}cdot prob(dist,sigma_{hit})+frac{z_{rand}}{z_{max}}Bigr)\ 8:&qquad returnquad q end{aligned}
1:2:3:4:5:6:7:8:Algorithmlikelihood_field_range_finder_model(zt,xt,m):q=1forallkdoifztk=zmaxXztk=X+Xk,senscosθ−Yk,snessinθ+ztkcos(θ+θk,sens)Yztk=Y+Yk,senscosθ+Xk,snessinθ+ztksin(θ+θk,sens)dist=Xd,Ydmin{(Xztk−Xd)2+(Yztk−Yd)2
∣∣∣∣∣⟨Xd,Yd⟩occupiedinm}q=q⋅(zhit⋅prob(dist,σhit)+zmaxzrand)returnq
第一行，定义变量
q
q
q存储整组数据
z
t
z_t
zt的概率
第二行，循环检测组内每个测量值
z
t
k
z_t^k
ztk
第三行，判断是否为最大值，并抛弃最大输出
第四行~第五行，计算测量点在地图坐标系上的坐标值
第六行，计算地图信息上距离该测量点最近的障碍物的欧氏距离
第七行，混合高斯分布与均匀分布的噪声模型
最终，返回整组数据的准确性，模型的内参
Θ
=
{
z
h
i
t
,
z
m
a
x
,
z
r
a
n
d
,
σ
h
i
t
}
Theta={z_{hit},z_{max},z_{rand},sigma_{hit}}
Θ={zhit,zmax,zrand,σhit}可用极大似然估计进行标定。
似然域模型的优缺点
欧氏距离的光滑性，致使机器人位姿
x
t
x_t
xt的微小改动将仅对分布结果
p
(
z
t
k
∣
x
t
,
m
)
p(z_t^k|x_t,m)
p(ztk∣xt,m)存在较小的影响，模型具有光滑性；
同时，模型的与计算采用二维空间代替了波束模型中的三维空间，降低了计算量。
与此同时，似然域模型存在如下缺点：
- 未考虑环境异常噪声
p
s
h
o
r
t
p_{short}
pshort，由此不能对人或动态清晰建模 - 射线投影被近邻函数代替，不能确定抵达某点路径是否被障碍物拦截
- 未考虑地图不确定性，不能处理地图上未知高度或未探测区域。
针对上述缺点，可将地图占用值扩展分为三类：占用、闲置、未知。当测量位置
z
t
k
z_t^k
ztk处于地图未知区域时，假定其概率用均匀分布建模：
p
(
z
t
k
∣
x
t
,
m
)
=
1
z
m
a
x
p(z_t^k|x_t,m)=frac{1}{z_{max}}
p(ztk∣xt,m)=zmax1