基于地图的运动学模型
模型建立
实际运行下，在机器人运动时将给定地图 m m m，地图中包括机器人能够或不能够通过的空间信息。例如占用地图（Occupancy maps），其中分为占用区域（Occupied）和闲置区域（Free）。机器人在运动时，应保证位姿始终处于闲置区域内。
采用 p ( x t ∣ u t , x t − 1 , m ) p(x_t|u_t,x_{t-1},m) p(xt∣ut,xt−1,m)表示基于地图信息的运动学模型（Map-base motion model）,表示机器人放置于地图 m m m所示的环境下，在控制量 u t u_t ut作用下，由初始位姿 x t − 1 x_{t-1} xt−1行驶至当前位姿 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_{t-1} xt−1与当前位姿 x t x_t xt间距离很小（如小于机器人底盘半径）时，可将模型近似。
首先，对其进行贝叶斯准则：
p
(
x
t
∣
u
t
,
x
t
−
1
,
m
)
=
η
p
(
m
∣
x
t
,
y
t
,
x
t
−
1
)
p
(
x
t
∣
u
t
,
x
t
−
1
)
p(x_t|u_t,x_{t-1},m)=\eta p(m|x_t,y_t,x_{t-1})p(x_t|u_t,x_{t-1})
p(xt∣ut,xt−1,m)=ηp(m∣xt,yt,xt−1)p(xt∣ut,xt−1)
其次，用
p
(
m
∣
x
t
)
p(m|x_t)
p(m∣xt)近似
p
(
m
∣
x
t
,
u
t
,
x
t
−
1
)
p(m|x_t,u_t,x_{t-1})
p(m∣xt,ut,xt−1)。同时由于
p
(
m
)
p(m)
p(m)相对于所求后验为常数，归于归一化因子
η
\eta
η中：
p
(
x
t
∣
u
t
,
x
t
−
1
,
m
)
=
η
p
(
m
∣
x
t
)
p
(
x
t
∣
u
t
,
x
t
−
1
)
=
η
p
(
x
t
∣
m
)
p
(
m
)
p
(
x
t
)
p
(
x
t
∣
u
t
,
x
t
−
1
)
=
η
p
(
x
t
∣
m
)
p
(
x
t
∣
u
t
,
x
t
−
1
)
p
(
x
t
)
\begin{aligned} p(x_t|u_t,x_{t-1},m)&=\eta p(m|x_t)p(x_t|u_t,x_{t-1})\\ &=\eta\frac{p(x_t|m)p(m)}{p(x_t)}p(x_t|u_t,x_{t-1})\\ &=\eta\frac{p(x_t|m)p(x_t|u_t,x_{t-1})}{p(x_t)}\\ \end{aligned}
p(xt∣ut,xt−1,m)=ηp(m∣xt)p(xt∣ut,xt−1)=ηp(xt)p(xt∣m)p(m)p(xt∣ut,xt−1)=ηp(xt)p(xt∣m)p(xt∣ut,xt−1)
由此，得到基于地图的近似模型：
p
(
x
t
∣
u
t
,
x
t
−
1
,
m
)
=
η
p
(
x
t
∣
u
t
,
x
t
−
1
)
p
(
x
t
∣
m
)
p
(
x
t
)
p(x_t|u_t,x_{t-1},m)=\eta\frac{p(x_t|u_t,x_{t-1})p(x_t|m)}{p(x_t)}
p(xt∣ut,xt−1,m)=ηp(xt)p(xt∣ut,xt−1)p(xt∣m)
- η \eta η为归一化因子，分母 p ( x t ) p(x_t) p(xt)通常是均匀的，可归于 η \eta η中
- 分子第一项
p
(
x
t
∣
u
t
,
x
t
−
1
)
p(x_t|u_t,x_{t-1})
p(xt∣ut,xt−1)
- 为无地图估计运动学模型
- 可为速度运动模型或里程计运动模型
- 分子第二项
p
(
x
t
∣
m
)
p(x_t|m)
p(xt∣m)
- 表示位姿 x t x_t xt与地图 m m m的一致性
- 当机器人位于占用区域时， p ( x t ∣ m ) = 0 p(x_t|m)=0 p(xt∣m)=0
- 当机器人位于闲置区域时， p ( x t ∣ m ) p(x_t|m) p(xt∣m)假定为常数项
- 分子得到一个概率分布，用于将概率分配给与地图一致的姿态 x t x_t xt
闭式算法（近似）
**系统输入： ** 初始位姿 x t − 1 x_{t-1} xt−1、控制量 u t u_t ut、估计后继姿态 x t x_t xt、地图信息 m m m
系统输出： 基于地图的机器人当前位姿可能性
p
(
x
t
∣
u
t
,
x
t
−
1
)
⋅
p
(
x
t
∣
m
)
p(x_t | u_t,x_{t-1})\cdot p(x_t|m)
p(xt∣ut,xt−1)⋅p(xt∣m)
A
l
g
o
r
i
t
h
m
m
o
t
i
o
n
_
m
o
d
e
l
_
w
i
t
h
_
m
a
p
(
x
t
,
u
t
,
x
t
−
1
,
m
)
:
1
:
r
e
t
u
r
n
p
(
x
t
∣
u
t
,
x
t
−
1
)
⋅
p
(
x
t
∣
m
)
\begin{aligned} &Algorithm\quad motion\_model\_with\_map(x_t,u_t,x_{t-1},m): \\ 1:&\qquad return\quad p(x_t | u_t,x_{t-1})\cdot p(x_t|m) \end{aligned}
1:Algorithmmotion_model_with_map(xt,ut,xt−1,m):returnp(xt∣ut,xt−1)⋅p(xt∣m)
采样算法
系统输入： 初始位姿 x t − 1 x_{t-1} xt−1、控制量 u t u_t ut、地图信息 m m m
系统输出： 加权样本 ⟨ x t , π ⟩ \langle x_t,\pi\rangle ⟨xt,π⟩
算法根据粒子滤波算法，随机生成采样样本并计算该样本在地图信息上的概率。
A
l
g
o
r
i
t
h
m
s
a
m
p
l
e
m
o
t
i
o
n
_
m
o
d
e
l
_
w
i
t
h
_
m
a
p
(
u
t
,
x
t
−
1
,
m
)
:
1
:
d
o
2
:
x
t
=
s
a
m
p
l
e
_
m
o
t
i
o
n
_
m
o
d
e
l
(
u
t
,
x
t
−
1
)
3
:
π
=
p
(
x
t
∣
m
)
4
:
u
n
t
i
l
π
>
0
1
:
r
e
t
u
r
n
⟨
x
t
,
π
⟩
\begin{aligned} &Algorithm\quad sample_motion\_model\_with\_map(u_t,x_{t-1},m): \\ 1:&\qquad do\\ 2:&\qquad\qquad x_t=sample\_motion\_model(u_t,x_{t-1})\\ 3:&\qquad\qquad \pi=p(x_t|m)\\ 4:&\qquad until \quad\pi>0\\ 1:&\qquad return\quad \langle x_t,\pi\rangle \end{aligned}
1:2:3:4:1:Algorithmsamplemotion_model_with_map(ut,xt−1,m):doxt=sample_motion_model(ut,xt−1)π=p(xt∣m)untilπ>0return⟨xt,π⟩
针对随机生成的采样
x
t
x_t
xt，计算该采样点在地图环境
m
m
m中是否在闲置区域，当概率
π
>
0
\pi>0
π>0时返回加权样本；当概率
π
≤
0
\pi\leq0
π≤0时，重新生成新样本。
算法效果
以速度运动模型为例，假设机器人当前速度运动模型通过闭式算法得到如图所示情况：
实际在地图环境下，存在矩形障碍物，则在该障碍物周围以机器人半径进行膨胀，形成膨胀层。
基于地图的速度运动模型
p
(
x
t
∣
u
t
,
x
t
−
1
,
m
)
p(x_t|u_t,x_{t-1},m)
p(xt∣ut,xt−1,m)在膨胀层以内（包括障碍物层）概率为0，膨胀层以外与
p
(
x
t
∣
u
t
,
x
t
−
1
)
p(x_t | u_t,x_{t-1})
p(xt∣ut,xt−1)成比例。
非零似然现象
将障碍物上方部分，标记为 ( ∗ ) (*) (∗)称为非零似然部分，此部分虽然 p ( x t ∣ u t , x t − 1 ) ≠ 0 p(x_t | u_t,x_{t-1})\neq0 p(xt∣ut,xt−1)=0，但是机器人抵达此区域必须横穿障碍物，实际无法抵达。
此问题是因为算法中仅考虑了位姿 x t x_t xt处模型的连续性，而未考虑抵达目标点路径的连续性，也即近似认为 p ( m ∣ x t , u t , x t − 1 ) = p ( m ∣ x t ) p(m|x_t,u_t,x_{t-1})=p(m|x_t) p(m∣xt,ut,xt−1)=p(m∣xt)。从中，丢失过往机器人的路径相关信息。
此现象在实际情况下：
- 当更新频率足够高：假定运动模型中噪声有界，从而使得近似是严格的，此现象不会出现
- 当更新频率不够高：非零似然现象存在于机器人较大相对运动时
529
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
