# 测距仪的似然域模型

## 模型建立

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，如图所示：

[

X

Y

]

T

begin{bmatrix}X^{'}&Y^{'}end{bmatrix}^T

[XY]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}

[XY]=[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]=[XY]+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(ztkxt,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

o1o2o3所在位置。

### 噪声二：失败噪声模型

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

pmax(ztkxt,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

prand(ztkxt,m)={zmax100ztk<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(ztkxt,m)=zhitzmaxzrandTphit(ztkxt,m)pmax(ztkxt,m)prand(ztkxt,m)

z

h

i

t

z

m

a

x

z

r

a

n

d

z_{hit}、z_{max}、z_{rand}

zhitzmaxzrand和为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

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{(XztkXd)2+(YztkYd)2

Xd,Ydoccupiedinm}q=q(zhitprob(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(ztkxt,m)存在较小的影响，模型具有光滑性；

• 未考虑环境异常噪声

p

s

h

o

r

t

p_{short}

，由此不能对人或动态清晰建模

• 射线投影被近邻函数代替，不能确定抵达某点路径是否被障碍物拦截
• 未考虑地图不确定性，不能处理地图上未知高度或未探测区域。

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(ztkxt,m)=zmax1

THE END