概率机器人：基于地图的运动学模型
基于地图的运动学模型
模型建立
实际运行下，在机器人运动时将给定地图 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)。从中，丢失过往机器人的路径相关信息。
此现象在实际情况下：
- 当更新频率足够高：假定运动模型中噪声有界，从而使得近似是严格的，此现象不会出现
- 当更新频率不够高：非零似然现象存在于机器人较大相对运动时