[发明专利]一种新的排爆机器人隧进定位方法

摘要

本发明公开了一种新的排爆机器人隧进定位方法，步骤如下S1，采样并初始化；S2.根据UKF算法更新粒子；S3，根据EKF算法再次更新粒子；S4，计算粒子的均值和均方差，根据建议分布函数随机抽取粒子并对每个粒子赋以权值S5，对进行归一化处理；S6.再采样，删除无效粒子，得到目标的位置。本发明在进行坐标估算过程中，进行两次粒子优化更新，降低了每个粒子坐标的误差，提高了滤波精度。在初始化阶段使用粒子滤波抽样方法，粒子数量有限，避免了UKF算法受粒子数影响而程序变慢的缺点，提高了运算速度。本发明应用于对过程噪声和量测噪声分布要求不高的非高斯噪声非线性系统，滤波精度高，定位误差小，而且算法简单，非常适合地下未知环境隧进系统钻头坐标定位。

主权项

一种新的排爆机器人隧进定位方法，其特征在于，步骤如下：S1，采样并初始化；S2，根据UKF算法更新粒子；S3，根据EKF算法再次更新粒子；S4，根据步骤S3中的粒子，计算粒子的均值和均方差，并根据粒子均值和均方差得到近似服从高斯分布的建议分布函数，根据建议分布函数随机抽取粒子并对每个粒子赋以权值具体公式为：wki=p(Zk|Xk(i))p(Xk(i)|Xk-1(i))q(Xk(i)|Xk-1(i),Zk)·wk-1(i)---(24);]]>其中，表示k时刻第i个粒子的权重；表示k时刻第i粒子量测传递概率密度函数；表示k时刻第i个粒子状态传递概率密度函数；表示第i个粒子的重要性密度函数；表示k‑1时刻第i个粒子的权重；S5，对步骤S4中的进行归一化处理；计算公式如下：w^ki=wki/Σj=1:Nwkj---(25);]]>S6.再采样，删除无效粒子，得到目标的位置。