概率机器人:测距仪的似然域模型

测距仪的似然域模型

似然域模型(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&\theta\end{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,如图所示:

概率机器人:测距仪的似然域模型_第1张图片

则通过2D平面坐标变换公式,可以得到传感器相对地图坐标系的坐标 [ 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\\Y\end{bmatrix}+\begin{bmatrix}\cos\theta&-\sin\theta\\\sin\theta&\cos\theta\end{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^k\begin{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\\Y\end{bmatrix}+\begin{bmatrix}\cos\theta&-\sin\theta\\\sin\theta&\cos\theta\end{bmatrix}\begin{bmatrix}X_{k,sens}\\Y_{k,sens}\end{bmatrix}+z_t^k\begin{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为模型的固有参数。

如下左图,机器人在包含三个障碍物(灰色)的地图环境下进行测量,得到的测量点高斯似然如右图所示,图中越黑的地方存在障碍物的可能性越小;虚线表示传感器轴方向:

概率机器人:测距仪的似然域模型_第2张图片

概率密度 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所在位置。

概率机器人:测距仪的似然域模型_第3张图片

噪声二:失败噪声模型

当传感器返回 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}1\quad&z=z_{max}\\0\quad&其他\end{cases} 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 其 他 p_{rand}(z_t^k|x_t,m)=\begin{cases}\frac{1}{z_{max}}\quad&0\le z_t^kprand(ztkxt,m)={zmax100ztk<zmax

似然域模型

由此,同波束模型一样,采用混合参数混合三种噪声,得到似然域模型如图所示:

概率机器人:测距仪的似然域模型_第4张图片
其表达式如下:
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}^T\cdot\begin{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 \begin{aligned} &Algorithm\quad likelihood\_field\_range\_finder\_model(z_t,x_t,m):\\ 1:&\qquad q=1 \\ 2:&\qquad for\enspace all\enspace k\enspace do \\ 3:&\qquad\qquad if z_t^k \neq z_{max}\\ 4:&\qquad\qquad\qquad X_{z_t^k}=X+X_{k,sens}\cos\theta-Y_{k,snes}\sin\theta+z_t^k\cos(\theta+\theta_{k,sens})\\ 5:&\qquad\qquad\qquad Y_{z_t^k}=Y+Y_{k,sens}\cos\theta+ X_{k,snes}\sin\theta+z_t^k\sin(\theta+\theta_{k,sens})\\ 6:&\qquad\qquad\qquad dist=\min_{X_d,Y_d}\Biggl\{\sqrt{(X_{z_t^k}-X_d)^2+(Y_{z_t^k}-Y_d)^2}\Bigg\vert \langle X_d,Y_d\rangle\enspace occupied\enspace in\enspace m\Biggr\}\\ 7:&\qquad\qquad\qquad q=q\cdot \Bigl(z_{hit}\cdot prob(dist,\sigma_{hit})+\frac{z_{rand}}{z_{max}}\Bigr)\\ 8:&\qquad return\quad 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{(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} 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(ztkxt,m)=zmax1

你可能感兴趣的:(概率机器人笔记,slam,自动驾驶)