实际运行下,在机器人运动时将给定地图 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)
**系统输入: ** 初始位姿 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)。从中,丢失过往机器人的路径相关信息。
此现象在实际情况下: