概率机器人：测距传感器的波束模型
机器人感知
测量模型
测量模型：用于描述客观世界中生成传感器测量数据的过程。
模型的特性取决于传感器：
- 成像传感器 ：通过投影几何学建立
- 声纳传感器 ：通过描述声波和声波在环境表面上的反射建立
测量模型定义为一个条件概率密度p(zt∣xt,m)p(z_t|x_t,m)p(zt∣xt,m)，表示在环境地图mmm和机器人位姿为xtx_txt的条件下，传感器测量得到ztz_tzt的概率密度。
传感器在进行测量时，将会产生一系列的测量值。如相机传感器返回一组完整的数据（亮度、饱和度、色彩）；同样，测距传感器将产生一些列测距值，其中每个ztkz_t^kztk表示一个独立的测距值：
zt={zt1,zt2,⋯,ztK}z_t=\{z_t^1,z_t^2,\cdots,z_t^K\} zt={zt1,zt2,⋯,ztK}
对每个独立的测量相乘可以得到测量模型：
p(zt∣xt,m)=∏K=1Kp(ztK∣xt,m)p(z_t|x_t,m)=\prod_{K=1}^Kp(z_t^K|x_t,m) p(zt∣xt,m)=K=1∏Kp(ztK∣xt,m)
理想条件下，每个独立测量束噪声之间相互独立，符合马尔可夫假设。
地图
环境的地图（map）给出了环境中物体的列表及其属性，包含特征地图和位置地图两大类。
基于特征的地图
在基于特征的地图（Feature-based Maps）中，用下标nnn表示特征索引，N表示环境中物体的总数：
m={m1,m2,⋯,mN}m=\{m_1,m_2,\cdots,m_N\} m={m1,m2,⋯,mN}
每个mn(1≤n≤N)m_n(1\le n\le N)mn(1≤n≤N)指定一个属性，包含了特征的属性及特征的笛卡尔位置。
基于特征的地图仅指明在指定位置（即地图中包含的对象的位置）的环境的形状，特征描述法使得调节对象的位置更为简单。
基于位置的地图
在基于位置的地图（Location-based Maps）中，用下标x,yx,yx,y表示平面地图中，该物体的坐标：
m={m0,0,m0,1,⋯,mx,y}m=\{m_{0,0},m_{0,1},\cdots,m_{x,y}\} m={m0,0,m0,1,⋯,mx,y}
每个mx,ym_{x,y}mx,y指定为平面坐标为(x,y)(x,y)(x,y)的属性。
基于位置的地图具有体积（volumetric），体积地图不仅包含环境物体的信息，也包含了对象没有物体的信息（即闲置区域)
占用栅格地图
占用栅格地图（Occupancy Grid Map）是一种经典的地图表示法，它是基于位置的地图。
占用栅格地图为每一个坐标(x,y)(x,y)(x,y)分配了一个二值占用值，用于指明该位置是否被某个对象占用。被占用的区域称为占用区域，未被占用的区域称为闲置区域。
占用栅格地图使得搜寻机器人通过闲置区域抵达目标点的路径更为便捷。
测距仪的波束模型
测距仪测量到附近物体的距离，可沿着一个波束测量（激光测距仪优选模型）或在一个测量锥内测量（超声波传感器优选模型）。
模型建立
测距仪的波束模型（Beam Model）p(zt∣xt,m)p(z_t|x_t,m)p(zt∣xt,m)可以看作是四种在机器人移动过程中存在的噪声的混合，每类噪声代表一种在实际运行中测距仪误差的产生。
噪声一：测量噪声模型
理想情况下，测距仪总是测量位于其测量范围内最近物体的实际距离ztk∗z_t^{k*}ztk∗。但由于测距传感器受到环境噪声影响，所得返回值一般为测量值ztkz_t^kztk。在不同的地图中，可采用不同方式得到实际距离：
- 基于位置的地图：射线投影法
- 基于特征的地图：在测量锥内寻找最近特征以获取
通常采用均值为ztk∗z_t^{k*}ztk∗，标准差为σhit\sigma_{hit}σhit的高斯分布来建模该噪声，如图所示采用phitp_{hit}phit表示此概率密度：
由于测距传感器存在测量最大值zmaxz_{max}zmax，同时测量值必定大于等于零，则给定测量噪声表达式：
phit(zt∣xt,m)={ηN(ztk;ztk∗,σhit2)0≤ztk≤zmax0其他p_{hit}(z_t|x_t,m)=\begin{cases}\eta \mathcal{N}(z_t^k;z_t^{k*},\sigma_{hit}^2)\quad&0\le z_t^k\le z_{max}\\ 0&其他\end{cases} phit(zt∣xt,m)={ηN(ztk;ztk∗,σhit2)00≤ztk≤zmax其他
式中，实际距离ztk∗z_t^{k*}ztk∗由射线投影计算得到；N(ztk;ztk∗,σhit2)\mathcal{N}(z_t^k;z_t^{k*},\sigma_{hit}^2)N(ztk;ztk∗,σhit2)表示均值为ztk∗z_t^{k*}ztk∗，方差为σhit2\sigma_{hit}^2σhit2的关于变量ztkz_t^kztk的正态分布：
N(ztk;ztk∗,σhit2)=12πσhit2exp(−12(ztk−ztk∗)2σhit2)\mathcal{N}(z_t^k;z_t^{k*},\sigma_{hit}^2)=\frac{1}{\sqrt{2\pi\sigma_{hit}^2}}exp(-\frac{1}{2}\frac{(z_t^k-z_t^{k*})^2}{\sigma_{hit}^2}) N(ztk;ztk∗,σhit2)=2πσhit21exp(−21σhit2(ztk−ztk∗)2)
η\etaη为归一化因子：
η=(∫0zmaxN(ztk;ztk∗,σhit2)dztk)−1\eta=(\int_0^{z_{max}} \mathcal{N}(z_t^k;z_t^{k*},\sigma_{hit}^2) \quad dz_t^k \quad)^{-1} η=(∫0zmaxN(ztk;ztk∗,σhit2)dztk)−1
噪声的标准差σhit\sigma_{hit}σhit为测量模型的固有参数。
噪声二：环境噪声模型
由于地图信息mmm为静态信息而机器人移动环境为动态环境，当实际环境内存在地图信息中不包含的对象时（如建图时，环境内不包含操控机器人的人物所处信息），测距仪将受到意外环境噪声影响。从而将导致实际距离ztk∗z_t^{k*}ztk∗更短。
此类意外对象造成测距仪噪声的可能性随着其距离而概率降低。如图所示，由于测距仪返回距离其最近的物体的距离，此时意外对象A和B出现在地图环境内，且A位于B前侧，则唯有A不出现时，测距仪能够感测到意外对象B的存在，否则仅传回意外对象A存在。
由此，采用指数分布来建模此类噪声现象，如图所示，采用pshortp_{short}pshort表示指数分布噪声：
在定义域范围内，模型如下所示：
pshort(zt∣xt,m)={ηλshortexp(−λshort⋅ztk)0≤ztk≤ztk,∗0其他p_{short}(z_t|x_t,m)=\begin{cases}\eta\lambda_{short}exp(-\lambda_{short}\cdot z_t^k)\quad&0\le z_t^k\le z_t^{k,*}\\ 0\quad&其他\end{cases} pshort(zt∣xt,m)={ηλshortexp(−λshort⋅ztk)00≤ztk≤ztk,∗其他
式中，参量λshort\lambda_{short}λshort为模型的固有参量；η\etaη为归一化因子：
η=(∫0ztk∗λshortexp(−λshort⋅ztk)dztk)−1=（−exp(−λshortztk∗)+exp(−λshort0)）−1=11−exp(−λshortztk∗)\begin{aligned} \eta&=(\int_0^{z_t^{k*}}\lambda_{short}exp(-\lambda_{short}\cdot z_t^k)\quad dz_t^k)^{-1}\\ &=（-exp(-\lambda_{short}z_t^{k*})+exp(-\lambda_{short}0)）^{-1}\\ &=\frac{1}{1-exp(-\lambda_{short}z_t^{k*})} \end{aligned} η=(∫0ztk∗λshortexp(−λshort⋅ztk)dztk)−1=（−exp(−λshortztk∗)+exp(−λshort0)）−1=1−exp(−λshortztk∗)1
噪声三：失败噪声模型
当所处环境存在镜面类容易反射激光束、声纳等信号的环境，或激光测距仪检测到黑色吸光现象、高光照下激光测量失败等情况时，环境存在测量失败噪声。
此时，测距仪将返回传感器的最大允许范围值zmaxz_{max}zmax。采用以zmaxz_{max}zmax为中心的点群分布建模，以pmaxp_{max}pmax表示该模型：
实际上，pmaxp_{max}pmax为一个离散分布，上图将其用一个很窄的以zmaxz_{max}zmax为中心的均匀分布给出，则模型的数学表达式如下所示：
pmax(ztk∣xt,m)=I(z=zmax)={1z=zmax0其他p_{max}(z_t^k|x_t,m)=I(z=z_{max})=\begin{cases}1\quad&z=z_{max}\\0\quad&其他\end{cases} pmax(ztk∣xt,m)=I(z=zmax)={10z=zmax其他
式中，函数I(z=zmax)I(z=z_{max})I(z=zmax)为指示函数，当括号内条件成立时返回1，其余返回0。
噪声四：随机噪声模型
此外，测距仪传感器可能偶尔存在无法解释的测量，将其简化为在测量区间[0,zmax)[0,z_{max})[0,zmax)中存在一个均匀分布，如图所示，定义为prandp_{rand}prand：
prand(ztk∣xt,m)={1zmax0≤ztk<zmax0其他p_{rand}(z_t^k|x_t,m)=\begin{cases}\frac{1}{z_{max}}\quad&0\le z_t^k<z_{max}\\ 0\quad&其他\end{cases} prand(ztk∣xt,m)={zmax100≤ztk<zmax其他
波束模型
将上述四类噪声模型通过参数zhit、zshort、zmax、zrandz_{hit}、z_{short}、z_{max}、z_{rand}zhit、zshort、zmax、zrand进行加权平均混合，得到波束模型p(ztk∣xt,m)p(z_t^k|x_t,m)p(ztk∣xt,m)
p(ztk∣xt,m)=[zhitzshortzmaxzrand]T⋅[phit(ztk∣xt,m)pshort(ztk∣xt,m)pmax(ztk∣xt,m)prand(ztk∣xt,m)]p(z_t^k|x_t,m)=\begin{bmatrix}z_{hit}\\z_{short}\\z_{max}\\z_{rand}\end{bmatrix}^T\cdot\begin{bmatrix}p_{hit}(z_t^k|x_t,m)\\p_{short}(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(ztk∣xt,m)=⎣⎢⎢⎡zhitzshortzmaxzrand⎦⎥⎥⎤T⋅⎣⎢⎢⎡phit(ztk∣xt,m)pshort(ztk∣xt,m)pmax(ztk∣xt,m)prand(ztk∣xt,m)⎦⎥⎥⎤
参数间满足和为1的条件：
zhit+zshort+zmax+zrand=1z_{hit}+z_{short}+z_{max}+z_{rand}=1 zhit+zshort+zmax+zrand=1
所得模型如图所示：
模型算法
针对测量模型，可设计如下算法实现该模型：
系统输入： 完整的一组测量数据ztz_tzt、机器人位姿xtx_txt、地图环境信息mmm
系统输出： 期望概率p(ztk∣xt,m)p(z_t^k|x_t,m)p(ztk∣xt,m)
Algorithmbeam_range_finder_model(zt,xt,m):1:q=12:fork=1tokdo3:computeztk∗forthemeasurementztkusingraycasting4:p=zhit⋅phit(ztk∣xt,m)+zshort⋅pshort(ztk∣xt,m)+zmax⋅pmax(ztk∣xt,m)+zrand⋅prand(ztk∣xt,m)5:q=q⋅p6:returnq\begin{aligned} &Algorithm\quad beam\_range\_finder\_model(z_t,x_t,m):\\ 1:&\qquad q=1\\ 2:&\qquad for\enspace k=1\enspace to\enspace k\enspace do\\ 3:&\qquad\qquad compute\enspace z_t^{k*}\enspace for\enspace the\enspace measurement\enspace z_t^k\enspace using\enspace ray\enspace casting\\ 4:&\qquad\qquad p=z_{hit}\cdot p_{hit}(z_t^k|x_t,m)+z_{short}\cdot p_{short}(z_t^k|x_t,m)+z_{max}\cdot p_{max}(z_t^k|x_t,m)+z_{rand}\cdot p_{rand}(z_t^k|x_t,m) \\ 5:&\qquad\qquad q=q\cdot p\\ 6:&\qquad return\quad q\\ \end{aligned} 1:2:3:4:5:6:Algorithmbeam_range_finder_model(zt,xt,m):q=1fork=1tokdocomputeztk∗forthemeasurementztkusingraycastingp=zhit⋅phit(ztk∣xt,m)+zshort⋅pshort(ztk∣xt,m)+zmax⋅pmax(ztk∣xt,m)+zrand⋅prand(ztk∣xt,m)q=q⋅preturnq
其中，首先定义变量qqq用于存储概率密度（第一行）。
随后，遍历所有测量数据（第二行），对其内每个数据ztkz_t^kztk采用射线投影法计算其实际距离ztk∗z_t^{k*}ztk∗（第三行）
再次，对每个实际距离ztk∗z_t^{k*}ztk∗采用波束模型计算其期望概率（第四行），并将其累乘至qqq中（第五行）
最终，完整遍历完所有数据后，得到测距仪得到的这一组距离值ztz_tzt的概率并返回（第六行）。
模型内参
传感器模型参数Θ\ThetaΘ包括了混合参数zhit、zshort、zmax、zrandz_{hit}、z_{short}、z_{max}、z_{rand}zhit、zshort、zmax、zrand以及参数σhit\sigma_{hit}σhit和参数λshort\lambda_{short}λshort。任何传感器测量的似然就是Θ\ThetaΘ的函数。
通常，可以通过实际数据得到模型的内参。假设参考数据集为Z={zi}Z=\{z_i\}Z={zi}、机器人位姿X={xi}X=\{x_i\}X={xi}、地图信息mmm，其中ziz_izi表示每个实际的测量数据；xix_ixi表示测量被采纳时机器人的位姿，则可对数据集ZZZ进行极大似然估计，从而得到模型的内参：
p(Z∣X,m,Θ)p(Z|X,m,\Theta) p(Z∣X,m,Θ)
为推导极大似然估计，定义一致性变量cic_ici用于表示一个测量值ziz_izi产生的可能途径。
波束模型具有四类噪声途径参数测量值ziz_izi，则cic_ici的取值有四种类型：hithithit、shortshortshort、maxmaxmax、randrandrand
对于模型的极大似然估计，首先假设已知一致性变量cic_ici情况下的求解，并由此推广至其未知情况下的求解。
假设已知cic_ici取值
此时，可将测试集ZZZ划分为四类不相交的数集，ZhitZ_{hit}Zhit、ZshortZ_{short}Zshort、ZmaxZ_{max}Zmax、ZrandZ_{rand}Zrand：
Zhit⋃Zshort⋃Zmax⋃Zrand=ZZhit⋂Zshort=Zhit⋂Zmax=Zhit⋂Zrand=∅Zshort⋂Zmax=Zshort⋂Zrand=Zmax⋂Zrand=∅Z_{hit}\bigcup Z_{short}\bigcup Z_{max}\bigcup Z_{rand}=Z\\ Z_{hit}\bigcap Z_{short}=Z_{hit}\bigcap Z_{max}=Z_{hit}\bigcap Z_{rand}=\varnothing\\ Z_{short}\bigcap Z_{max}=Z_{short}\bigcap Z_{rand}=Z_{max}\bigcap Z_{rand}=\varnothing Zhit⋃Zshort⋃Zmax⋃Zrand=ZZhit⋂Zshort=Zhit⋂Zmax=Zhit⋂Zrand=∅Zshort⋂Zmax=Zshort⋂Zrand=Zmax⋂Zrand=∅
混合参数的极大似然估计
对数据集进行归一化，可得混合参数的极大似然估计：
[zhitzshortzmaxzrand]=∣Z∣−1[∣Zhit∣∣Zshort∣∣Zmax∣∣Zrand∣]\begin{bmatrix}z_{hit}\\z_{short}\\z_{max}\\z_{rand}\end{bmatrix}=|Z|^{-1}\begin{bmatrix}|Z_{hit}|\\|Z_{short}|\\|Z_{max}|\\|Z_{rand}|\end{bmatrix} ⎣⎢⎢⎡zhitzshortzmaxzrand⎦⎥⎥⎤=∣Z∣−1⎣⎢⎢⎡∣Zhit∣∣Zshort∣∣Zmax∣∣Zrand∣⎦⎥⎥⎤
固有参数σhit\sigma_{hit}σhit的极大似然估计
首先，求解似然函数：
p(Zhit∣X,m,Θ)=∏zi∈Zhitphit(zi∣xi,m,Θ)=∏zi∈Zhit12πσhit2exp(−12(zi−zi∗)2σhit2)\begin{aligned} p(Z_{hit}|X,m,\Theta)=&\prod_{z_i\in Z_{hit}}p_{hit}(z_i|x_i,m,\Theta)\\ =&\prod_{z_i\in Z_{hit}}\frac{1}{\sqrt{2\pi\sigma_{hit}^2}}exp(-\frac{1}{2}\frac{(z_i-z_i^*)^2}{\sigma_{hit}^2}) \end{aligned} p(Zhit∣X,m,Θ)==zi∈Zhit∏phit(zi∣xi,m,Θ)zi∈Zhit∏2πσhit21exp(−21σhit2(zi−zi∗)2)
式中，zi∗z_i^{*}zi∗表示由机器人位姿xix_ixi及地图信息mmm计算得到的实际距离。
其次，求解对数似然函数表达式（两边取自然对数）：
lnp(Zhit∣X,m,Θ)=∑zi∈Zhit[−12ln(2πσhit2)−12(zi−zi∗)2σhit2]=−12∑z∈Zhit[ln(2πσhit2)+(zi−zi∗)2σhit2]=−12∑z∈Zhit[ln(2π)+2ln(σhit)+(zi−zi∗)2σhit2]=−12[∣Zhit∣⋅ln(2π)+∣Zhit∣⋅2ln(σhit)+∑z∈Zhit(zi−zi∗)2σhit2]=−∣Zhit∣2⋅ln(2π)−∣Zhit∣⋅ln(σhit)−12σhit2∑z∈Zhit(zi−zi∗)2\begin{aligned} \ln p(Z_{hit}|X,m,\Theta)=&\sum_{z_i\in Z_{hit}}\Bigl[-\frac{1}{2}\ln(2\pi\sigma_{hit}^2)-\frac{1}{2}\frac{(z_i-z_i^*)^2}{\sigma_{hit}^2}\Bigr]\\ =&-\frac{1}{2}\sum_{z\in Z_{hit}}\Bigl[\ln(2\pi\sigma_{hit}^2)+\frac{(z_i-z_i^*)^2}{\sigma_{hit}^2}\Bigr]\\ =&-\frac{1}{2}\sum_{z\in Z_{hit}}\Bigl[\ln(2\pi)+2\ln(\sigma_{hit})+\frac{(z_i-z_i^*)^2}{\sigma_{hit}^2}\Bigr]\\ =&-\frac{1}{2}\Bigl[\vert Z_{hit}\vert\cdot\ln(2\pi)+\vert Z_{hit}\vert\cdot2\ln(\sigma_{hit})+\sum_{z\in Z_{hit}}\frac{(z_i-z_i^*)^2}{\sigma_{hit}^2}\Bigr]\\ =&-\frac{\vert Z_{hit}\vert}{2}\cdot\ln(2\pi)-\vert Z_{hit}\vert\cdot\ln(\sigma_{hit})-\frac{1}{2\sigma_{hit}^2}\sum_{z\in Z_{hit}}(z_i-z_i^*)^2 \end{aligned} lnp(Zhit∣X,m,Θ)=====zi∈Zhit∑[−21ln(2πσhit2)−21σhit2(zi−zi∗)2]−21z∈Zhit∑[ln(2πσhit2)+σhit2(zi−zi∗)2]−21z∈Zhit∑[ln(2π)+2ln(σhit)+σhit2(zi−zi∗)2]−21[∣Zhit∣⋅ln(2π)+∣Zhit∣⋅2ln(σhit)+z∈Zhit∑σhit2(zi−zi∗)2]−2∣Zhit∣⋅ln(2π)−∣Zhit∣⋅ln(σhit)−2σhit21z∈Zhit∑(zi−zi∗)2
将对数似然函数对参数σhit\sigma_{hit}σhit求偏导并使其为0：
∂lnp(Zhit∣X,m,Θ)∂σhit=0−∣Zhit∣σhit+1σhit3∑zi∈Zhit(zi−zii∗)2=01σhit3∑zi∈Zhit(zi−zii∗)2=∣Zhit∣σhit∑zi∈Zhit(zi−zii∗)2=∣Zhit∣σhit2\begin{aligned} \frac{\partial \ln p(Z_{hit}|X,m,\Theta)}{\partial \sigma_{hit}}&=0\\ -\frac{\vert Z_{hit}\vert}{\sigma_{hit}}+\frac{1}{\sigma_{hit}^3}\sum_{z_i\in Z_{hit}}(z_i-z_i^{i*})^2&=0\\ \frac{1}{\sigma_{hit}^3}\sum_{z_i\in Z_{hit}}(z_i-z_i^{i*})^2&=\frac{\vert Z_{hit}\vert}{\sigma_{hit}}\\ \sum_{z_i\in Z_{hit}}(z_i-z_i^{i*})^2&=\vert Z_{hit}\vert\sigma_{hit}^2 \end{aligned} ∂σhit∂lnp(Zhit∣X,m,Θ)−σhit∣Zhit∣+σhit31zi∈Zhit∑(zi−zii∗)2σhit31zi∈Zhit∑(zi−zii∗)2zi∈Zhit∑(zi−zii∗)2=0=0=σhit∣Zhit∣=∣Zhit∣σhit2
由此，得到似然对数的极大值，也即参数σhit\sigma_{hit}σhit的极大似然估计：
σhit=1∣Zhit∣⋅∑zi∈Zhit(zi−zii∗)2\sigma_{hit}=\sqrt{\frac{1}{\vert Z_{hit}\vert}\cdot\sum_{z_i\in Z_{hit}}(z_i-z_i^{i*})^2} σhit=∣Zhit∣1⋅zi∈Zhit∑(zi−zii∗)2
固有参数λshort\lambda_{short}λshort的极大似然估计
同样，先计算固有参数λshort\lambda_{short}λshort的似然函数：
p(Zshort∣X,m,Θ)=∏zi∈Zshortpshort(zi∣xi,m,Θ)=∏zi∈Zshort[λshort⋅exp(−λshortzi)]\begin{aligned} p(Z_{short}|X,m,\Theta)=&\prod_{z_i\in Z_{short}}p_{short}(z_i|x_i,m,\Theta)\\ =&\prod_{z_i\in Z_{short}}\Bigl[\lambda_{short}\cdot exp(-\lambda_{short}z_i)\Bigr] \end{aligned} p(Zshort∣X,m,Θ)==zi∈Zshort∏pshort(zi∣xi,m,Θ)zi∈Zshort∏[λshort⋅exp(−λshortzi)]
两侧求自然对数，得到似然函数对数形式：
lnp(Zshort∣X,m,Θ)=∑zi∈Zshort[lnλshort−λshort⋅zi]=∣Zshort∣⋅lnλshort−λshort∑zi∈Zshortzi\begin{aligned} \ln p(Z_{short}|X,m,\Theta)=&\sum_{z_i\in Z_{short}}\Bigl[\ln\lambda_{short}-\lambda_{short}\cdot z_i\Bigr]\\ =&\vert Z_{short}\vert\cdot \ln \lambda_{short}-\lambda_{short}\sum_{z_i\in Z_{short}}z_i\\ \end{aligned} lnp(Zshort∣X,m,Θ)==zi∈Zshort∑[lnλshort−λshort⋅zi]∣Zshort∣⋅lnλshort−λshortzi∈Zshort∑zi
对其两侧求对参数λshort\lambda_{short}λshort的偏导，并使其为零：
∂lnp(Zshort∣X,m,Θ)∂λshort=0∣Zshort∣λshort−∑zi∈Zshortzi=0∣Zshort∣λshort=∑zi∈Zshortzi\begin{aligned} \frac{\partial \ln p(Z_{short}|X,m,\Theta)}{\partial \lambda_{short}}&=0\\ \frac{\vert Z_{short}\vert}{\lambda_{short}}-\sum_{z_i\in Z_{short}}z_i&=0\\ \frac{\vert Z_{short}\vert}{\lambda_{short}}&=\sum_{z_i\in Z_{short}}z_i\\ \end{aligned} ∂λshort∂lnp(Zshort∣X,m,Θ)λshort∣Zshort∣−zi∈Zshort∑ziλshort∣Zshort∣=0=0=zi∈Zshort∑zi
由此，得到似然对数的极大值，也即参数λshort\lambda_{short}λshort的极大似然估计：
λshort=∣Zshort∣∑zi∈Zshortzi\lambda_{short}=\frac{\vert Z_{short}\vert}{\sum_{z_i\in Z_{short}}z_i} λshort=∑zi∈Zshortzi∣Zshort∣
未知cic_ici取值
上述推导假设参数cic_ici已知，将其推广至参数cic_ici未知情况下。设计采用期望值最大算法（Expectation Maximization，EM）实现两步迭代：
- 计算cic_ici的期望值
- 在该期望下计算模型参数
期望计算
首先，定义数据集ZZZ的似然函数：
p(Z∣X,m,Θ)=∏zi∈Zp(zi∣xi,m,Θ)=∏zi∈Zhitphit(zi∣xi,m)⋅∏zi∈Zshortpshort(zi∣xi,m)⋅∏zi∈Zmaxpmax(zi∣xi,m)⋅∏zi∈Zrandprand(zi∣xi,m)\begin{aligned} p(Z|X,m,\Theta)=&\prod_{z_i\in Z}p(z_i|x_i,m,\Theta)\\ =& \prod_{z_i\in Z_{hit}}p_{hit}(z_i|x_i,m)\cdot\prod_{z_i\in Z_{short}}p_{short}(z_i|x_i,m)\cdot\\ &\prod_{z_i\in Z_{max}}p_{max}(z_i|x_i,m)\cdot\prod_{z_i\in Z_{rand}}p_{rand}(z_i|x_i,m) \end{aligned} p(Z∣X,m,Θ)==zi∈Z∏p(zi∣xi,m,Θ)zi∈Zhit∏phit(zi∣xi,m)⋅zi∈Zshort∏pshort(zi∣xi,m)⋅zi∈Zmax∏pmax(zi∣xi,m)⋅zi∈Zrand∏prand(zi∣xi,m)
对似然函数两侧进行自然对数：
lnp(Z∣X,m,Θ)=∑zi∈Zhitlnphit(zi∣xi,m)+∑zi∈Zshortlnpshort(zi∣xi,m)+∑zi∈Zmaxlnpmax(zi∣xi,m)+∑zi∈Zrandlnprand(zi∣xi,m)\begin{aligned} \ln p(Z|X,m,\Theta)=& \sum_{z_i\in Z_{hit}}\ln p_{hit}(z_i|x_i,m)+\sum_{z_i\in Z_{short}}\ln p_{short}(z_i|x_i,m)+\\ &\sum_{z_i\in Z_{max}}\ln p_{max}(z_i|x_i,m)+\sum_{z_i\in Z_{rand}}\ln p_{rand}(z_i|x_i,m) \end{aligned} lnp(Z∣X,m,Θ)=zi∈Zhit∑lnphit(zi∣xi,m)+zi∈Zshort∑lnpshort(zi∣xi,m)+zi∈Zmax∑lnpmax(zi∣xi,m)+zi∈Zrand∑lnprand(zi∣xi,m)
将变量cic_ici带入上式，可知数据集ZZZ内测量值ziz_izi将采用对应的噪声模型计算：
lnp(Z∣X,m,Θ)=∑zi∈Z[I(ci=hit)⋅lnphit(zi∣xi,m)+I(ci=short)⋅lnpshort(zi∣xi,m)+I(ci=max)⋅lnpmax(zi∣xi,m)+I(ci=rand)⋅lnprand(zi∣xi,m)]\begin{aligned} \ln p(Z|X,m,\Theta)=\sum_{z_i\in Z}\Bigl[&I(c_i=hit)\cdot\ln p_{hit}(z_i|x_i,m)\\ +&I(c_i=short)\cdot\ln p_{short}(z_i|x_i,m)\\ +&I(c_i=max)\cdot\ln p_{max}(z_i|x_i,m)\\ +&I(c_i=rand)\cdot\ln p_{rand}(z_i|x_i,m)\Bigl] \end{aligned} lnp(Z∣X,m,Θ)=zi∈Z∑[+++I(ci=hit)⋅lnphit(zi∣xi,m)I(ci=short)⋅lnpshort(zi∣xi,m)I(ci=max)⋅lnpmax(zi∣xi,m)I(ci=rand)⋅lnprand(zi∣xi,m)]
由于参数cic_ici未知，则次啊用期望最大算法，希望对上式计算最大期望：
1:E[lnp(Z∣X,m,Θ)]=∑i[p(ci=hit)⋅lnphit(zi∣xi,m)+p(ci=short)⋅lnpshort(zi∣xi,m)+p(ci=max)⋅lnpmax(zi∣xi,m)+p(ci=rand)⋅lnprand(zi∣xi,m)]2：E[lnp(Z∣X,m,Θ)]=:∑i[ei,hit⋅lnphit(zi∣xi,m)+ei,short⋅lnpshort(zi∣xi,m)+ei,max⋅lnpmax(zi∣xi,m)+ei,rand⋅lnprand(zi∣xi,m)]\begin{aligned} 1:E[\ln p(Z|X,m,\Theta)]=\sum_i\Bigl[&p(c_i=hit)\cdot\ln p_{hit}(z_i|x_i,m)+p(c_i=short)\cdot\ln p_{short}(z_i|x_i,m)+\\ &p(c_i=max)\cdot\ln p_{max}(z_i|x_i,m)+p(c_i=rand)\cdot\ln p_{rand}(z_i|x_i,m)\Bigr]\\ 2：E[\ln p(Z|X,m,\Theta)]=:\sum_i\Bigl[&e_{i,hit}\cdot\ln p_{hit}(z_i|x_i,m)+e_{i,short}\cdot\ln p_{short}(z_i|x_i,m)+\\ &e_{i,max}\cdot\ln p_{max}(z_i|x_i,m)+e_{i,rand}\cdot\ln p_{rand}(z_i|x_i,m)\Bigr]\\ \end{aligned} 1:E[lnp(Z∣X,m,Θ)]=i∑[2：E[lnp(Z∣X,m,Θ)]=:i∑[p(ci=hit)⋅lnphit(zi∣xi,m)+p(ci=short)⋅lnpshort(zi∣xi,m)+p(ci=max)⋅lnpmax(zi∣xi,m)+p(ci=rand)⋅lnprand(zi∣xi,m)]ei,hit⋅lnphit(zi∣xi,m)+ei,short⋅lnpshort(zi∣xi,m)+ei,max⋅lnpmax(zi∣xi,m)+ei,rand⋅lnprand(zi∣xi,m)]
式中，第二行符号=:=:=:含义为记作 ，其中参数ei,hit、ei,short、ei,max、ei,rande_{i,hit}、e_{i,short}、e_{i,max}、e_{i,rand}ei,hit、ei,short、ei,max、ei,rand表示有这四种模型引起的产生测量值ziz_izi的概率：
[ei,hitei,shortei,maxei,rand]:=[p(ci=hit)p(ci=short)p(ci=max)p(ci=rand)]=η[phit(zi∣xi,m)pshort(zi∣xi,m)pmax(zi∣xi,m)prand(zi∣xi,m)]\begin{bmatrix}e_{i,hit}\\e_{i,short}\\e_{i,max}\\e_{i,rand}\end{bmatrix} :=\begin{bmatrix}p(c_i=hit)\\p(c_i=short)\\p(c_i=max)\\p(c_i=rand)\end{bmatrix}=\eta\begin{bmatrix}p_{hit}(z_i|x_i,m)\\p_{short}(z_i|x_i,m)\\p_{max}(z_i|x_i,m)\\p_{rand}(z_i|x_i,m)\end{bmatrix} ⎣⎢⎢⎡ei,hitei,shortei,maxei,rand⎦⎥⎥⎤:=⎣⎢⎢⎡p(ci=hit)p(ci=short)p(ci=max)p(ci=rand)⎦⎥⎥⎤=η⎣⎢⎢⎡phit(zi∣xi,m)pshort(zi∣xi,m)pmax(zi∣xi,m)prand(zi∣xi,m)⎦⎥⎥⎤
式中，符号:=:=:=表示意为。η\etaη为归一化因子：
η=[phit(zi∣xi,m)+pshort(zi∣xi,m)+pmax(zi∣xi,m)+prand(zi∣xi,m)]−1\eta=\bigl[p_{hit}(z_i|x_i,m)+p_{short}(z_i|x_i,m)+p_{max}(z_i|x_i,m)+p_{rand}(z_i|x_i,m)\bigr]^{-1} η=[phit(zi∣xi,m)+pshort(zi∣xi,m)+pmax(zi∣xi,m)+prand(zi∣xi,m)]−1
极大似然估计
极大似然估计的混合参数为归一化的期望：
[zhitzshortzmaxzrand]=∣Z∣−1∑i[ei,hitei,shortei,maxei,rand]\begin{bmatrix}z_{hit}\\z_{short}\\z_{max}\\z_{rand}\end{bmatrix}=\vert Z \vert^{-1}\sum_i\begin{bmatrix}e_{i,hit}\\e_{i,short}\\e_{i,max}\\e_{i,rand}\end{bmatrix} ⎣⎢⎢⎡zhitzshortzmaxzrand⎦⎥⎥⎤=∣Z∣−1i∑⎣⎢⎢⎡ei,hitei,shortei,maxei,rand⎦⎥⎥⎤
极大似然估计的参数σhit\sigma_{hit}σhit及λshort\lambda_{short}λshort可将上述在已知cic_ici取值情况下，计算所得的表达式中的硬分配替换为用期望加权的软分配，从而推广至未知cic_ici取值下的表达式：
σhit=1∑zi∈Zei,hit⋅∑zi∈Zei,hit(zi−zii∗)2\sigma_{hit}=\sqrt{\frac{1}{\sum_{z_i\in Z}e_{i,hit}}\cdot\sum_{z_i\in Z}e_{i,hit}(z_i-z_i^{i*})^2} σhit=∑zi∈Zei,hit1⋅zi∈Z∑ei,hit(zi−zii∗)2
λshort=∑zi∈Zei,short∑zi∈Zei,shortzi\lambda_{short}=\frac{\sum_{z_i\in Z}e_{i,short}}{\sum_{z_i\in Z}e_{i,short}z_i} λshort=∑zi∈Zei,shortzi∑zi∈Zei,short
参数调节算法
系统输入： 测距仪测量数据集ZZZ、机器人位姿数据集XXX、地图信息mmm
系统输出： 模型内参Θ\ThetaΘ
Algorithmlearn_intrinsic_parameters(Z,X,m):1:repeatuntilconvergencecritensatisfied2:forallziinZdo3:η=[phit(zi∣xi,m)+pshort(zi∣xi,m)+pmax(zi∣xi,m)+prand(zi∣xi,m)]−14:calculatezi∗5:ei,hit=ηphit(zi∣xi,m)6:ei,short=ηpshort(zi∣xi,m)7:ei,max=ηpmax(zi∣xi,m)8:ei,rand=ηprand(zi∣xi,m)9:zhit=∣Z∣−1∑iei,hit10:zshort=∣Z∣−1∑iei,short11:zmax=∣Z∣−1∑iei,max12:zrand=∣Z∣−1∑iei,rand13:σhit=1∑iei,hit⋅∑iei,hit(zi−zii∗)214:λshort=∑iei,short∑iei,shortzi15:returnΘ={zhit,zshort,zmax,zrand,σhit,λshort}\begin{aligned} &Algorithm\quad learn\_intrinsic\_parameters(Z,X,m):\\ 1:&\qquad repeat\enspace until\enspace convergence\enspace criten\enspace satisfied\\ 2:&\qquad\qquad for\enspace all \enspace z_i\enspace in\enspace Z\enspace do\\ 3:&\qquad\qquad\qquad \eta=\bigl[p_{hit}(z_i|x_i,m)+p_{short}(z_i|x_i,m)+p_{max}(z_i|x_i,m)+p_{rand}(z_i|x_i,m)\bigr]^{-1}\\ 4:&\qquad\qquad\qquad calculate\enspace z_i^*\\ 5:&\qquad\qquad\qquad e_{i,hit}=\eta p_{hit}(z_i|x_i,m)\\ 6:&\qquad\qquad\qquad e_{i,short}=\eta p_{short}(z_i|x_i,m)\\ 7:&\qquad\qquad\qquad e_{i,max}=\eta p_{max}(z_i|x_i,m)\\ 8:&\qquad\qquad\qquad e_{i,rand}=\eta p_{rand}(z_i|x_i,m)\\ \\ 9:&\qquad\qquad z_{hit}=\vert Z\vert^{-1}\sum_ie_{i,hit}\\ 10:&\qquad\qquad z_{short}=\vert Z\vert^{-1}\sum_ie_{i,short}\\ 11:&\qquad\qquad z_{max}=\vert Z\vert^{-1}\sum_ie_{i,max}\\ 12:&\qquad\qquad z_{rand}=\vert Z\vert^{-1}\sum_ie_{i,rand}\\ 13:&\qquad\qquad \sigma_{hit}=\sqrt{\frac{1}{\sum_i e_{i,hit}}\cdot\sum_i e_{i,hit}(z_i-z_i^{i*})^2}\\ 14:&\qquad\qquad \lambda_{short}=\frac{\sum_i e_{i,short}}{\sum_i e_{i,short}z_i}\\ 15:&\qquad return\quad \Theta=\{z_{hit},z_{short},z_{max},z_{rand},\sigma_{hit},\lambda_{short}\} \\ \end{aligned} 1:2:3:4:5:6:7:8:9:10:11:12:13:14:15:Algorithmlearn_intrinsic_parameters(Z,X,m):repeatuntilconvergencecritensatisfiedforallziinZdoη=[phit(zi∣xi,m)+pshort(zi∣xi,m)+pmax(zi∣xi,m)+prand(zi∣xi,m)]−1calculatezi∗ei,hit=ηphit(zi∣xi,m)ei,short=ηpshort(zi∣xi,m)ei,max=ηpmax(zi∣xi,m)ei,rand=ηprand(zi∣xi,m)zhit=∣Z∣−1i∑ei,hitzshort=∣Z∣−1i∑ei,shortzmax=∣Z∣−1i∑ei,maxzrand=∣Z∣−1i∑ei,randσhit=∑iei,hit1⋅i∑ei,hit(zi−zii∗)2λshort=∑iei,shortzi∑iei,shortreturnΘ={zhit,zshort,zmax,zrand,σhit,λshort}
第三行，计算归一化因子
第四行，有机器人位姿及地图信息，计算测量值的实际值，也即在地图环境mmm下机器人处于位姿xix_ixi时提取到的测距值zi∗z_i^*zi∗
第五行~第八行，计算由四类噪声模型独立引起产生测量值ziz_izi的概率
第九行~第十四行，遍历完成后计算模型的六个内参
算法采用期望最大化算法实现极大似然估计参数，通过迭代数据实现对内参的迭代优化。
算法效果
如图为两组机器人在办公室环境下获取到的10 000个测量数据，左图为声纳传感器测量数据，右图为激光传感器测量数据。图中x轴为计数个数，y轴为传感器测量距离：
其中，测量的期望范围为3m，也即真实距离300cm，最大量程为500cm。用所设计的极大似然估计参数模型测试，得到如下效果图。
其中，左侧数据为采用上述测试集测试得到的图像（ztk∗=3z_t^{k*}=3ztk∗=3）；右侧一组则为对照数据的分布图像（ztk∗>3z_t^{k*}>3ztk∗>3），由图可以分析得到：
- 实际距离ztk∗z_t^{k*}ztk∗越小，模型测量的数据越精准
- 短距离测量的高斯分布（左侧）较长距离测量的高斯分布（右侧）更窄
- 激光传感器（b组）比超声波传感器（a组）测量更准确，（高斯分布更窄且最大值分布更少）
同时给出一组180°180\degree180°激光传感器的扫描环境图，该机器人被已经获得地图信息mmm的占用栅格地图中走廊位置处，扫描自身周围的环境：
则下图中，给出了地图信息与测量似然p(zt∣xt,m)p(z_t|x_t,m)p(zt∣xt,m)映射到x−yx-yx−y空间的平面图，其中显示颜色越深处，机器人处于该位置的可能性就越高。
上图显示，所有高似然区域处于走廊中，这和机器人的实际情况相符合。同时概率分散在整个走廊上也表明单一传感器难以确切确定机器人位姿。（走廊环境大致相同，无特殊特征，难区分）
同时，后验有规则地分布在两条水平带状区上是因为难以确定机器人的航向，由此形成两种可能的朝向对应两条区域。
局限性
波束模型需要耗费大量计算力，用于对每一个单一测量ztkz_t^kztk通过射线投影计算其实际测量ztk∗z_t^{k*}ztk∗。如激光雷达每次扫描将得到一组180°180\degree180°的数据，对这180个数据需要逐一进行计算，将耗费大量时间与计算量。
此外，该模型缺乏光滑性。在由许多小障碍的混乱环境中，概率分布p(ztk∣xt,m)p(z_t^k|x_t,m)p(ztk∣xt,m)在机器人位姿xtx_txt处很不光滑。机器人位姿的小变化将引起测距模型大的位移变化。
概率机器人：测距传感器的波束模型相关推荐
- 传感器怎么获取障碍物的宽度信息_机器人感知 -- 测距传感器
在自动驾驶中和移动机器人的应用中,感知是至关重要的一环,周边环境障碍物的确定对于之后的路径规划和移动有着中要的影响.在实践中,我们往往用激光雷达(lidar)来满足感知的要求,主要原理是通过雷达接收到 ...
- 距离感应音乐玩具计算机系统设计,可编程机器人玩具程小奔红外测距传感器测量距离案例...
文:邱老师 上次课程我们学习了用速度乘以时间计算程小奔移动距离的方法.那么,它能否测量还没有走过的路程距离呢?答案是可以的. 程小奔编程机器人集合了多种传感器,今天我们要使用程小奔机器人的红外测距传感 ...
- 清洁机器人--沿边测距传感器 sharp psd红外传感器的FOV角度分析
清洁机器人–沿边测距传感器 sharp psd红外传感器的FOV角度分析 文章目录 清洁机器人--沿边测距传感器 sharp psd红外传感器的FOV角度分析 1.LED灯珠的发光角度 1.1 理论基 ...
- 概率机器人（Probability Robotics）笔记 Chapter 9: 占据栅格建图（Occupancy Grid Mapping）
1. 简介 前两章讲了定位的概率方法,是一个低维度感知问题,假设地图已知. 但有些情况地图未知,或者不准确. 因此建图可以让部署机器人更容易,也会让机器人更能适应环境变化. 建图是真正的自动机器人的核 ...
- 概率机器人总结——占用栅格地图先实践再推导
概率机器人总结--占用栅格地图先实践再推导 概率机器人总结--占用栅格地图构建先实践再推导 实践过程 伪代码分析 真代码分析 推导过程 静态二值贝叶斯滤波 概率机器人总结--占用栅格地图构建先实践再推 ...
- 概率机器人总结——(扩展)卡尔曼滤波先实践再推导
概率机器人总结--卡尔曼滤波先实践再推导 概率机器人总结--(扩展)卡尔曼滤波先实践再推导 (1)卡尔曼.扩展卡尔曼.粒子滤波到底什么关系? (2)实践--扩展卡尔曼滤波 (3)推导--卡尔曼滤波 ( ...
- arduino学习笔记十--Arduino 读红外测距传感器
Arduino 读红外测距传感器GP2D12 实例,仅供大家参考! 器材:Arduino 开发板,GP2D12,1602 字符液晶,连接线若干. GP2D12 是日本SHARP 公司生产的红外距离传感 ...
- 无人驾驶之概率机器人，附加部分，及强化学习试卷
注:本卷为内部用卷,供大家检验学习成果用,如需商业化请私聊我,谢谢! 注:本卷可结合博文的考纲使用 一. 填空题(60分,定义,区别2分,其他1分) 概率机器人与传统机器人相比的优势在于_______ ...
- 《概率机器人》速度运动模型gmapping中代码解析
一个刚性移动机器人的构型通常用6个变量来描述:他的三维直角坐标系,以及相对外部坐标系的三个欧拉角(RPY 横滚 ,俯仰,偏航),所以那么在平面环境中一般用三个变量既可以描述,称之为位姿. 所以一般而平 ...
最新文章
- html缓存特效代码,HTML特效代码
- sys.dm_exec_query_stats的total_worker_time的单位是微秒还是毫秒
- Spring Boot异常处理
- 前端开发桌面终极工具（FastStone Capture）推荐（转）
- ssm上传文件获取路径_ssm框架实现图片上传显示并保存地址到数据库(示例代码)...
- 子类重写方法aop切不到_Spring-aop 全面解析（从应用到原理）
- 研磨设计模式——桥接模式
- opencv3+python3.5成语填字游戏（一）印刷体汉字的分割
- 今日头条视频去重复上传方法-网络营销推广教程 如何完美去除视频字幕和LOGO批量下载快手西瓜视频...
- FineUI 后台Grid中 某列添加背景色 AspCore MVC
- java实现鼠标宏编程_对键盘鼠标宏处理－－按键精灵让我们不要重复工作
- 记录对安卓开源项目【nodebb-webview】修改过程中遇见的问题以及解决办法
- 计算机自带拼图程序,电脑上比较好用的拼图软件？
- CSS 图片偏移技术以及坐标问题
- 语音 LMS 降噪的 C 语言源代码及其解释
- 昨天的《实话实说》周尚元造飞机
- Shell脚本编程－－cut命令
- 0基础快速入门WebPack（3）——图解详述plugins（插件）的安装及sourceMap的使用及WebpackDevServer正向代理和模块热更新等（附详细案例源码解析过程及版本迭代过程）
- BZOJ 5010: [Fjoi2017]矩阵填数
- 《被讨厌的勇气》读后感