CN103344963B - 一种基于激光雷达的鲁棒航迹推算方法 - Google Patents

Abstract

一种基于激光雷达的鲁棒航迹推算方法，根据移动机器人系统故障模型以及运动学模型，构造不同故障式下的运动学模型；然后构造快速的激光雷达测量模型；最后，在粒子滤波器框架下，整合不同模式运动学模型与测量模型，同时实现故障诊断与航迹推算。

Description

一种基于激光雷达的鲁棒航迹推算方法

技术领域

本发明属于航迹推算系统测量领域，具体涉及一种基于激光雷达的鲁棒航迹推算方法。

背景技术

自主移动机器人已广泛应用于星球探测、军事搜救、灾难救援、家庭护理以及工业应用等领域。机器人航迹推算根据机器人传感器信息对其位姿进行估计，是移动机器人实现环境建模与自主导航的前提。鲁棒航迹推算是指在机器人出现故障的情况下(如传感器失效、传感器受强噪声干扰及车轮打滑等)仍能利用冗余信息对机器人位姿进行较为准确地估计的技术。

常规地，移动机器人利用部署在其上的内部传感器(如编码器、陀螺仪)和/或外部传感器(如激光雷达、CCD摄像头)进行位姿估计。在系统无故障情况下的航迹推算问题已有诸多成熟技术。然而，当传感器出现故障时，特别是内部传感器和外部传感器可能同时出现故障的情况下，如何利用冗余传感器实信息较为准确地估计移动机器人位姿(即鲁棒航迹推算)问题，目前尚未被系统研究。

实现鲁棒航迹推算的主要困难在于：必须同时对机器人实现故障诊断与位姿估计。因为，内部传感器故障会改变其运动学模型；外部传感器故障则要求建立新的感知模型。

由于故障诊断和位姿估计分别是离散和连续状态估计问题，而粒子滤波器是估计混合系统的一种非参数方法，适合与非线性、非高斯系统估计问题，因此，本技术采用粒子滤波器作为状态估计技术。

综上，本技术针对部分内部传感器故障、激光雷达部分异常等实际的故障情形，通过构造内部传感器故障情况下的运动学模型、激光雷达鲁棒感知模型，并在粒子滤波算法框架下结合上述两类模型，同时进行机器人故障诊断与位姿估计。

现有技术提出了一种利用光纤陀螺仪(Fiberopticgyroscopes，FOGs)以及间接反馈卡尔曼滤波(Kalmanfilter)方法提高航迹推算系统的精度。该方法首先对光纤陀螺仪进行精确的标定来减少其尺度因子非线性误差以及不同温度引起的误差。然后，利用间接反馈卡尔曼滤波融合光纤陀螺仪和编码器数据。从而提高航迹推算系统精度。

该方法中，光纤陀螺仪因速度以及温度变化导致的误差模型采用文献[2]提出的模型。测量尺度因子误差的基本方法是用已知的输入速度ω旋转陀螺仪，并与陀螺仪的输出测量值ω_g比较，两者之差是由于尺度因子非线性性引起的误差，误差可用三次多项式拟合：

$\mathrm{\ϵ}\left({\mathrm{\ω}}_g\right)={\mathrm{\ω}}_g-\mathrm{\ω}=a_0+a_1{\mathrm{\ω}}_g+a_2{\mathrm{\ω}}_g^2+a_3{\mathrm{\ω}}_g^3---\left(1\right)$

测量误差随温度的变化关系用二次多项式拟合：

ε(T)＝b₀+b₁T+b₂T²(2)

其中，ω_g为陀螺仪量测值，T为温度，a₀，a₁，a₂，a₃，b₀，b₁以及b₂为待定参数。

现有技术一的缺点：

1)对光纤陀螺仪的标定需要专门的高精度仪器，标定过程复杂，不易操作且成本高。且无论如何标定，都难以对陀螺以漂移误差进行精确建模。

2)如果编码器和/或陀螺仪出现故障，此方法则完全失效。

3)采用Kalman滤波器，无法处理非线性系统估计问题。

Verma等利用粒子滤波器(ParticleFilter，PF)来解决计算复杂度以及高斯假设问题，对漫游车(rover)进行状态检测和故障诊断。

该技术特点之一：提出了一种变分辨率粒子滤波器(VariableParticleFilter，VRPF)，并应用于漫游车故障诊断问题。VRPF通过将具有相似特征的状态抽象为一个状态。一个抽象状态可以表示单个状态或状态集合，当状态表示成抽象状态时，状态空间被缩小了。VRPF动态地改变状态空间的分辨率：在信度强时分辨率高，信度强的抽象状态被分解为多个更细的抽象状态；信度弱时分辨率低，此时将多个相似状态合并为一个抽象状态。

该技术特点之二：针对低发生概率的故障可能存在的漏检问题，提出了风险敏感粒子滤波器(RiskSensitiveParticleFilter，RSPF)，通过提高低概率故障的风险水平增加对低概率故障的采用概率，该方法的风险评价是主观的且不能完全避免漏检。

该技术只能诊断机器人内部传感器故障，却不能提供准确的航迹推算信息。

文献提出了一种航迹推算系统软故障诊断与补偿方法。软故障是指元件性能下降但没有完全失效。该技术考虑与航迹推算密切相关的两类软故障：(1)编码器和陀螺仪等内部传感器噪声和尺度因子故障；(2)移动机构异常，包括(a)车轮打滑，(b)车轮爆胎，以及(c)车轮受阻。

该方法首先构造了移动机器人运动学模型，然后根据机器人内部传感器之间的冗余关系构造残差特征。根据残差特征设计机器人内部传感器软故障检测的阈值逻辑。此外，残差特征也作为后续粒子滤波估计的噪声方差调整依据。

该技术没有对航迹推算传感器出现失效故障时加以考虑。

本发明针对航迹推算系统内部传感器可能发生失效故障、激光雷达测量也可能受大噪声干扰的情况，在诊断内部传感器故障的同时，利用激光雷达的有效测量，快速提供较为准确的航迹推算信息。

发明内容

该技术的整体思路为将故障诊断与航迹推算问题作为一个离散与连续状态的联合估计问题。首先，根据移动机器人系统故障模型以及运动学模型，构造不同故障式下的运动学模型；其次，构造快速的激光雷达测量模型；最后，在粒子滤波器框架下，整合不同模式运动学模型与测量模型，同时实现故障诊断与航迹推算。

设Г_t-1，Г_t分别表示移动机器人t-1，t时刻的坐标系(位姿)。移动机器人航迹推算系统是计算从Г_t-1到Г_t的齐次变换A_t。

$A_t=\left[\begin{array}{cc}{H}_t& p_t\\ 0& 1\end{array}\right]---\left(3\right)$

其中， $H_t=\left[\begin{array}{cc}{\cos \mathrm{\θ}}_t& {-\sin \mathrm{\θ}}_t\\ {\sin \mathrm{\θ}}_t& {\cos \mathrm{\θ}}_t\end{array}\right]$ 表示旋转矩阵，p_t＝(x_t，y_t)^T表示平移向量。令x_t＝(x_t，y_t，θ_t)^T，则航迹推算的实质是估计3维连续状态x_t。

适用性说明：本技术适用于平面环境下二轮差动驱动移动机器人，要求该机器人配备有编码器(左、右轮各一)、陀螺仪、激光雷达。

为了实现在传感器故障情况下对移动机器人进行较为快速准确的航迹推算，本技术方案主要包含步骤。

步骤l：建立故障模式及其运动学模型

移动机器人内部传感器故障模式如表1所示，不同的故障模式其运动学模型各不相同，分别表示为公式(6-9)。

表1移动机器人内部传感器故障模式

步骤2：激光雷达感知模型

该步骤通过对激光雷达读数进行分段分析、逆射线跟踪以及高通滤波三个子步骤构造激光雷达快速鲁棒的感知模型。

步骤2.1：通过分段分析方法构建激光雷达有效感知窗口

步骤2.2：利用逆射线跟踪方法确定误差

步骤2.3：利用高通滤波实现鲁棒感知模型

步骤3：利用粒子滤波方法实现故障诊断和鲁棒航迹推算。

1)初始化，设置粒子数目N以及离散故障集合S＝{1，2，3，4}

2)对于每一时间步t，执行3-15

3)对于前4个粒子(i从1至4)执行4-8

4) $S_t^{\left[i\right]}=i$

5) ${\mathrm{\θ}}_t^{\left[i\right]}=\mathrm{\τ}{\stackrel{\·}{\mathrm{\θ}}}_t^i$

6) $x_t^{\left[i\right]}=\mathrm{\τ}{\mathrm{\υ}}_t^i\cos \left({\mathrm{\θ}}_t^{\left[i\right]}\right)$

7) $y_t^{\left[i\right]}=\mathrm{\τ}{\mathrm{\υ}}_t^i\sin \left({\mathrm{\θ}}_t^{\left[i\right]}\right)$

8) $w_t^{\left[i\right]}=p\left(R_t\right|R_{t-1},x_t^{\left[i\right]})$

9)i从5至N循环，执行10-12

10)采样使得 $s_t^{\left[i\right]}=j\∝w_t^{\left[j\right]},j\∈[1..4]$

11)根据粒子i的离散状态确定的运动学模型，采样连续状态

$x_t^{\left[i\right]}~p\left(\left(x{)}_t\right|(x{)}_{t-1}^i,s_t^{\left[i\right]}\right)$

12)计算权重 $w_t^{\left[i\right]}=p\left(R_t\right|R_{t-1},x_t^{\left[i\right]})$

13)计算边缘分布 ${\hat{P}}_N\left(s_t)\right|z_{1..t})=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{w}_t{\mathrm{\δ}}_{s_t^{\left[i\right]}}\left(s_t\right)$

14)故障诊断， ${\hat{s}}_t^{\mathrm{MAP}}={\arg \max }_{s_t}{\hat{P}}_N\left(s_t\right|z_{1..t})$

15)航迹推算， ${\hat{x}}_t={\arg \max }_{x_t^{\left[i\right]}}{w}_t^{\left[i\right]}$

本发明针对航迹推算系统内部传感器可能发生失效故障、激光雷达测量也可能受大噪声干扰的情况，同时诊断内部传感器故障并利用激光雷达的有效测量数据快速提供较为准确的航迹推算信息。

附图说明

下面结合附图，通过非限定性的举例对本发明的优选实施方式作进一步说明，在附图中：图1逆射线跟踪原理图

具体实施方式

以下将对本发明的优选实施例进行详细的描述；应当理解，优选实施例仅为了说明本发明，而不是为了限制本发明的保护范围。

令υ_t，记分别表示移动机器人t时刻的线速度以及偏航率，则直接根据υ_t和可以计算 $x_t:\left\{\begin{array}{c}{\mathrm{\θ}}_t=\mathrm{\τ}{\stackrel{\·}{\mathrm{\θ}}}_t\\ x_t=\mathrm{\τ}{\mathrm{\υ}}_t\cos \left({\mathrm{\θ}}_t\right)\\ y_t={\mathrm{\τ\υ}}_t\sin \left({\mathrm{\θ}}_t\right)\end{array}\right.---\left(4\right)$

通常，υ_t，分别通过编码器和陀螺仪的读数确定。令g_t分别表示左轮编码器、右轮编码器、陀螺仪t时刻的读数，由于编码器测量和陀螺仪测量存在以下的冗余关系， ${\stackrel{\·}{\mathrm{\θ}}}_t=(e_t^R-e_t^L)/D---\left(5\right)$

其中，D表示两轮间的轴距，对4种故障模式分别构造运动学模型如下：

模式1(无故障模式)的运动学模型： $\left\{\begin{array}{c}{\mathrm{\υ}}_t^1=(e_t^L+e_t^R)/2\\ {\stackrel{\·}{\mathrm{\θ}}}_t^1=g_t\end{array}\right.---\left(6\right)$

模式2(左编码器故障)的运动学模型： $\left\{\begin{array}{c}{\mathrm{\υ}}_t^2=e_t^R-D\·g_t/2\\ {\stackrel{\·}{\mathrm{\θ}}}_t^2=g_t\end{array}\right.---\left(7\right)$

模式3(右编码器故障)的运动学模型： $\left\{\begin{array}{c}{\mathrm{\υ}}_t^3=e_t^L+D\·g_t/2\\ {\stackrel{\·}{\mathrm{\θ}}}_t^3=g_t\end{array}\right.---\left(8\right)$

模式4(陀螺仪故障)的运动学模型： $\left\{\begin{array}{c}{\mathrm{\υ}}_t^4=(e_t^L+e_t^R)/2\\ {\stackrel{\·}{\mathrm{\θ}}}_t^4=(e_t^R-e_t^L)/D\end{array}\right.---\left(9\right)$

所述的激光雷达感知模型构建方法为：

(1)通过分段分析方法构建激光雷达有效感知窗口，设激光雷达t时刻的获得的原始数据表示为R_t， $R_t=\{b_t^j=({\mathrm{\ρ}}_t^j,{\mathrm{\α}}_t^j)\},j\∈[1..L]---\left(10\right)$

其中，表示R_t的第j条射线，L为射线总数，表示第j条射线测量距离，表示第j条射线的方向， ${\mathrm{\α}}_t^j=\tilde{n}+(j-1)\mathrm{\ζ}---\left(11\right)$

其中，表示角度偏移，ζ表示角度分辨率；通过式(12)对R_t进行分段分析，

$R_t=\underset{k=1}{\overset{c_t}{\∪}}{G}_t^k---\left(12\right)$

其中，表示第k段，c_t表示分段数目， $f_t^{k+1}=r_t^k+1,$ $|{\mathrm{\&rho;}}_t^{i+1}-{\mathrm{\&rho;}}_t^i|<\mathrm{gap},$ $i+1,i\&Element;[f_t^k..r_t^k],$ $|{\mathrm{\&rho;}}_t^{f_t^{k+1}}-{\mathrm{\&rho;}}_t^{r_t^k}|\&GreaterEqual;\mathrm{gap},$ gap表示一个阈值，

设表示第k段射线数目，表示第k段射线平均长度，根据这两个特征，通过式(13)构造有效感知窗口， $W_t=\underset{k=1}{\overset{c_t}{\&cup;}}{G}_t^k,d_t^k>\mathrm{\&gamma;},{\mathrm{\&lambda;}}_2\&GreaterEqual;{\widetilde{\mathrm{\&rho;}}}_t^k\&GreaterEqual;{\mathrm{\&lambda;}}_1---\left(13\right)$

其中，W_t表示R_t的有效感知窗口，γ，λ₂，λ₁分别表示阈值。特别地，对于LMS291激光雷达，设置γ＝5，λ₂＝81.9m，λ₁＝.03m；

(2)利用逆射线跟踪方法确定误差

Γ_t-1，Γ_t分别表示移动机器人t-1，t时刻的坐标系(位姿)，这两个坐标系的原点分别用O_t-1，O_t表示，令表示射线击中的点，则在坐标系Γ_t中的坐标为， $B_t^i{}_{{\mathrm{\&Gamma;}}_t}=\left[\begin{array}{c}{\mathrm{\&rho;}}_t^i\cos {\mathrm{\&alpha;}}_t^i\\ {\mathrm{\&rho;}}_t^i\sin {\mathrm{\&alpha;}}_t^i\end{array}\right]---\left(14\right)$

如前所述，Γ_t分相对于Γ_t-1的齐次变换矩阵为A_t，如式(3)。则在坐标系Γ_t-1中的坐标为： $B_t^i{}_{{\mathrm{\&Gamma;}}_{t-1}}={H_t\&CenterDot;}^{{\mathrm{\&Gamma;}}_t}{B}_t^i+p_t---\left(15\right)$

其中， $B_t^i{}_x{}^{{\mathrm{\&Gamma;}}_{t-1}}=\cos {\mathrm{\&theta;}}_t{\mathrm{\&rho;}}_t^i\cos {\mathrm{\&alpha;}}_t^i-\sin {\mathrm{\&theta;}}_t{\mathrm{\&rho;}}_t^i\sin {\mathrm{\&alpha;}}_t^i+x_t---\left(16\right)$

$B_t^i{}_y{}^{{\mathrm{\&Gamma;}}_{t-1}}=\sin {\mathrm{\&theta;}}_t{\mathrm{\&rho;}}_t^i\cos {\mathrm{\&alpha;}}_t^i+\cos {\mathrm{\&theta;}}_t{\mathrm{\&rho;}}_t^i\sin {\mathrm{\&alpha;}}_t^i+y_t---\left(17\right)$

设和分别表示相对于的长度和方向，根据式(15-17)有：

${\mathrm{\&gamma;}}_t^i=\arctan \left(\frac{{\sin \mathrm{\&theta;}}_t{\mathrm{\&rho;}}_t^i\cos {\mathrm{\&alpha;}}_t^i+\cos {\mathrm{\&theta;}}_t{\mathrm{\&rho;}}_t^i\sin {\mathrm{\&alpha;}}_t^i+y_t}{\cos {\mathrm{\&theta;}}_t{\mathrm{\&rho;}}_t^i\cos {\mathrm{\&alpha;}}_t^i-\sin {\mathrm{\&theta;}}_t{\mathrm{\&rho;}}_t^i\sin {\mathrm{\&alpha;}}_t^i+x_t}\right)---\left(19\right)$

设齐次变换是准确的，且在R_t-1的视野内，则可用一种替代的方法计算和O_t-1之间的距离，设则R_t-1中方向射线的期望距离为， $d({\mathrm{\&gamma;}}_t^i,R_{t-1})={\mathrm{\&rho;}}_t^{j_{b_t^i}}+({\mathrm{\&gamma;}}_t^i-(j_{b_t^i}-1)\mathrm{\&zeta;}-\tilde{n})---\left(20\right)$

射线的期望与感知距离的差异为，

如果齐次变换是准确的，则将很小，否贝将非常大。设激光雷达的噪声服从方差为σ²的正态分布，则射线在给定x_t情况下的似然概率为，

$p\left(b_t^i\right|R_{t-1},x_t)=\frac{1}{\sqrt{2\mathrm{\&pi;}}\mathrm{\&sigma;}}{e}^{-\frac{e_t^i}{2{\mathrm{\&sigma;}}^2}}---\left(22\right)$

(3)利用高通滤波实现鲁棒感知模型

为实现上述目标，采用如下方法计算p(R_t|R_t-1，x_t)， $p\left(R_t\right|R_{t-1},x_t)=\underset{b_t^i\&Element;W_t}{\mathrm{\&Sigma;}}p\left(b_t^i\right|R_{t-1},x_t)---\left(23\right)$

所述利用粒子滤波方法实现故障诊断和鲁棒航迹推算具体过程：

16)初始化，设置粒子数目N以及离散故障集合S＝{1，2，3，4)

17)对于每一时间步t，执行3-15

18)对于前4个粒子(i从1至4)执行4-8

19) $s_t^{\left[i\right]}=i$

20) ${\mathrm{\&theta;}}_t^{\left[i\right]}=\mathrm{\&tau;}{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_t^i$

21) $x_t^{\left[i\right]}=\mathrm{\&tau;}{\mathrm{\&upsi;}}_t^i\cos \left({\mathrm{\&theta;}}_t^{\left[i\right]}\right)$

22) $y_t^{\left[i\right]}={\mathrm{\&tau;\&upsi;}}_t^i\sin \left({\mathrm{\&theta;}}_t^{\left[i\right]}\right)$

23) $w_t^{\left[i\right]}=p\left(R_t\right|R_{t-1},x_t^{\left[i\right]})$

24)i从5至N循环，执行10-12

25)采样使得 $s_t^{\left[i\right]}=j\&Proportional;w_t^{\left[j\right]},j\&Element;[1..4]$

26)根据粒子i的离散状态确定的运动学模型，采样连续状态

$x_t^{\left[i\right]}~p\left({\left(x\right)}_t\right|{\left(x\right)}_{t-1}^i,s_t^{\left[i\right]})$

27)计算权重 $w_t^{\left[i\right]}=p\left(R_t\right|R_{t-1},x_t^{\left[i\right]})$

28)计算边缘分布 ${\hat{P}}_N\left(s_t)\right|z_{1..t})=\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{w}_t{\mathrm{\&delta;}}_{s_t^{\left[i\right]}}\left(s_t\right)$

29)故障诊断， ${\hat{s}}_t^{\mathrm{MAP}}={\arg \max }_{s_t}{\hat{P}}_N\left(s_t\right|z_{1..t})$

30)航迹推算， ${\hat{x}}_t{=\arg \max }_{x_t^{\left[i\right]}}{w}_t^{\left[i\right]}$

第3-8步确保每一个离散状态均被采样，同时确定了离散故障模式的初始概率，第10步说明离散故障模式采样概率由步骤8确定，第11步采样连续的状态，第14步进行故障诊断，第15步进行航迹推算。

Claims (1)

1.一种基于激光雷达的鲁棒航迹推算方法，根据移动机器人系统故障模型以及运动学模型，构造不同故障模式下的运动学模型；然后构造快速的激光雷达感知模型；最后，在粒子滤波器框架下，整合不同模式运动学模型与感知模型，同时实现故障诊断与航迹推算，所述的不同故障模式下的运动学模型的构造方法为：不同的故障模式其运动学模型各不相同，分别表示为公式(6-9)：

令υ_t，分别表示移动机器人t时刻的线速度以及偏航率，则直接根据υ_t和可以计算x_t： $\left\{\begin{array}{c}{\mathrm{\&theta;}}_t=\mathrm{\&tau;}{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_t\\ x_t={\mathrm{\&tau;\&upsi;}}_{t}cos\left({\mathrm{\&theta;}}_t\right)\\ y_t={\mathrm{\&tau;\&upsi;}}_{t}sin\left({\mathrm{\&theta;}}_t\right)\end{array}\right.---\left(4\right)$ 其中x_t是x_t的第一分量，y_t是x_t的第二分量，υ_t，分别通过编码器和陀螺仪的读数确定，令g_t分别表示左轮编码器、右轮编码器、陀螺仪t时刻的读数，由于编码器测量和陀螺仪测量存在以下的冗余关系， ${\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_t=(e_t^R-e_t^L)/D---\left(5\right)$

其中，D表示两轮间的轴距，对4种故障模式分别构造运动学模型如下：

模式1无故障模式的运动学模型： $\left\{\begin{array}{c}{\mathrm{\&upsi;}}_t^1=(e_t^L+e_t^R)/2\\ {\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_t^1=g_t\end{array}\right.---\left(6\right)$

模式2左编码器故障的运动学模型： $\left\{\begin{array}{c}{\mathrm{\&upsi;}}_t^2=e_t^R-D\&CenterDot;g_t/2\\ {\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_t^2=g_t\end{array}\right.---\left(7\right)$

模式3右编码器故障的运动学模型： $\left\{\begin{array}{c}{\mathrm{\&upsi;}}_t^3=e_t^L+D\&CenterDot;g_t/2\\ {\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_t^3=g_t\end{array}\right.---\left(8\right)$

模式4陀螺仪故障的运动学模型： $\left\{\begin{array}{c}{\mathrm{\&upsi;}}_t^4=(e_t^L+e_t^R)/2\\ {\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_t^4=(e_t^R-e_t^L)/D\end{array}\right.---\left(9\right)$

所述的激光雷达感知模型构建方法为：

(1)通过分段分析方法构建激光雷达有效感知窗口，设激光雷达t时刻的获得的原始数据表示为R_t， $R_t=\{b_t^j=({\mathrm{\&rho;}}_t^j,{\mathrm{\&alpha;}}_t^j)\},j\&Element;\&lsqb;\mathrm{1..}L\&rsqb;---\left(10\right)$

其中，表示R_t的第j条射线，L为射线总数，表示第j条射线测量距离，表示第j条射线的方向，

其中，表示角度偏移，表示角度分辨率；通过式(12)对R_t进行分段分析，

$R_t=\underset{k=1}{\overset{c_t}{\&cup;}}{G}_t^k---\left(12\right)$

其中，表示第k段，表示的第一条射线编号，表示的最后一条射线编号，c_t表示分段数目，f_t ¹＝1， $|{\mathrm{\&rho;}}_t^{i+1}-{\mathrm{\&rho;}}_t^i|<gap,i+1,i\&Element;\&lsqb;f_t^k\mathrm{..}{r}_t^k\&rsqb;,$ $|{\mathrm{\&rho;}}_t^{f_t^{k+1}}-{\mathrm{\&rho;}}_t^{r_t^k}|\&GreaterEqual;gap,$ gap表示一个阈值，

设表示第k段射线数目，表示第k段射线平均长度，根据这两个特征，通过式(13)构造有效感知窗口，

其中，W_t表示R_t的有效感知窗口，γ，λ₂，λ₁分别表示阈值；对于LMS291激光雷达，设置γ＝5，λ₂＝81.9m，λ₁＝.03m；

(2)利用逆射线跟踪方法确定误差

Γ_t-1，Γ_t分别表示移动机器人t-1，t时刻的坐标系，这两个坐标系的原点分别用O_t-1，O_t表示，令表示射线击中的点，则在坐标系Γ_t中的坐标为，

Γ_t分相对于Γ_t-1的齐次变换矩阵为A_t，则在坐标系Γ_t-1中的坐标为：

$H_t=\left[\begin{array}{cc}{\mathrm{cos\&theta;}}_t& -{\mathrm{sin\&theta;}}_t\\ {\mathrm{sin\&theta;}}_t& {\mathrm{cos\&theta;}}_t\end{array}\right]$ 表示旋转矩阵，p_t＝(x_t，y_t)^T表示平移向量，

其中，

设和分别表示相对于的长度和方向，根据公式(15-17)有：

${\mathrm{\&gamma;}}_t^i=\arctan \left(\frac{{\mathrm{sin\&theta;}}_t{\mathrm{\&rho;}}_t^i{\mathrm{cos\&alpha;}}_t^i+{\mathrm{cos\&theta;}}_t{\mathrm{\&rho;}}_t^i{\mathrm{sin\&alpha;}}_t^i+y_t}{{\mathrm{cos\&theta;}}_t{\mathrm{\&rho;}}_t^i{\mathrm{cos\&alpha;}}_t^i-{\mathrm{sin\&theta;}}_t{\mathrm{\&rho;}}_t^i{\mathrm{sin\&alpha;}}_t^i+x_t}\right)---\left(19\right)$

设齐次变换是准确的，且在R_t-1的视野内，则可用一种替代的方法计算和O_t-1之间的距离，设则R_t-1中方向射线的期望距离为，

射线的感知与期望距离的差异为， $e_t^i=d_t^i-d({\mathrm{\&gamma;}}_t^i,R_{t-1})---\left(21\right)$

如果齐次变换是准确的，则将很小，否则将非常大；设激光雷达的噪声服从方差为σ²的正态分布，则射线在给定x_t情况下的似然概率为，

$p\left(b_t^i\right|R_{t-1},x_t)=\frac{1}{\sqrt{2\mathrm{\&pi;}}\mathrm{\&sigma;}}{e}^{-\frac{e_t^i}{2{\mathrm{\&sigma;}}^2}}---\left(22\right)$

(3)利用高通滤波实现鲁棒感知模型

采用如下方法计算p(R_t|R_t-1，x_t)， $p\left(R_t\right|R_{t-1},x_t)=\underset{b_t^i\&Element;W_t}{\&Sigma;}p\left(b_t^i\right|R_{t-1},x_t)---\left(23\right)$

利用粒子滤波方法实现故障诊断和鲁棒航迹推算具体过程：

1)初始化，设置粒子数目N以及离散故障集合S＝{1，2，3，4}

2)对于每一时间步t，执行3-15

3)对于前4个粒子，i从1至4，执行4)-8)

4)其中，s_t为t时刻的故障模式

5) ${\mathrm{\&theta;}}_t^{\&lsqb;i\&rsqb;}=\mathrm{\&tau;}{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_t^i$

6) $x_t^{\&lsqb;i\&rsqb;}={\mathrm{\&tau;\&upsi;}}_t^{i}cos\left({\mathrm{\&theta;}}_t^{\&lsqb;i\&rsqb;}\right)$

7) $y_t^{\&lsqb;i\&rsqb;}={\mathrm{\&tau;\&upsi;}}_t^{i}sin\left({\mathrm{\&theta;}}_t^{\&lsqb;i\&rsqb;}\right)$

8) $w_t^{\&lsqb;i\&rsqb;}=p\left(R_t\right|R_{t-1},x_t^{\&lsqb;i\&rsqb;})$

9)i从5至N循环，执行10-12

10)采样使得 $s_t^{\&lsqb;i\&rsqb;}=j\&Proportional;w_t^{\&lsqb;j\&rsqb;},j\&Element;\&lsqb;\mathrm{1..4}\&rsqb;$

11)根据粒子i的离散状态确定的运动学模型，采样连续状态

$x_t^{\&lsqb;i\&rsqb;}~p\left(x_t\right|x_{t-1}^i,s_t^{\&lsqb;i\&rsqb;})$

12)计算权重 $w_t^{\&lsqb;i\&rsqb;}=p\left(R_t\right|R_{t-1},x_t^{\&lsqb;i\&rsqb;})$

13)计算边缘分布 ${\hat{P}}_N\left(s_t\right|z_{\mathrm{1..}t})=\underset{i=1}{\overset{N}{\&Sigma;}}{w}_t{\mathrm{\&delta;}}_{s_t^{\&lsqb;i\&rsqb;}}\left(s_t\right)s_t,$ 表示t时刻的离散状态

14)故障诊断， ${\hat{s}}_t^{MAP}={\mathrm{argmax}}_{s_t}{\hat{P}}_N\left(s_t\right|z_{\mathrm{1..}t})$

15)航迹推算， ${\hat{x}}_t={\mathrm{argmax}}_{x_t^{\&lsqb;i\&rsqb;}}{w}_t^{\&lsqb;i\&rsqb;}$

第3-8步确保每一个离散状态均被采样，同时确定了离散故障模式的初始概率，第10步说明离散故障模式采样概率由步骤8确定，第11步采样连续的状态，第14步进行故障诊断，第15步进行航迹推算。