[发明专利]基于粒子滤波的远程定位方法

权利要求书

1.一种基于粒子滤波的远程定位方法，其特征在于，包括以下步骤：

基于视觉位姿进行粒子群的状态初始化；

基于优化后轮式里程计进行粒子位姿预测；

基于激光匹配进行预测粒子状态的微调；

基于激光观测信息进行粒子状态更新；

基于视觉位姿和机器人系统当前定位信息判断机器人是否被绑架，如果判断得到机器人被绑架，重新开始执行基于视觉位姿进行粒子群的状态初始化；如果机器人没有被绑架，判断视觉位姿是否可信，如果视觉位姿可信，结合视觉位姿、激光观测信息和二维激光似然域地图进行粒子权重更新；如果视觉位姿信息不可信，不结合视觉权重进行粒子权重更新，仅仅基于激光观测信息和二维激光似然域地图进行粒子权重更新；

根据更新的粒子权重对粒子状态进行加权平均，计算机器人基于多传感器融合算法得出的最终位姿；

基于视觉位姿进行粒子群状态初始化具体为：

基于视觉SLAM获得环境的视觉地图，加载视觉地图，远程操控机器人移动，当机器人在运动过程中视觉回环成功后，返回当前相机位姿x_visual，以x_visual为均值，基于服从式的高斯概率分布来采样粒子{x⁽ⁱ⁾}，对粒子群进行初始化，粒子分布服从高斯分布，每个粒子都携带有机器人的位姿信息，粒子群的位姿均值为系统粒子滤波的最后输出机器人位姿x_PF；

基于优化后轮式里程计进行粒子位姿预测具体为：

基于实现的系统粒子群{x⁽ⁱ⁾}的初始化，移动机器人在运动的过程中粒子群也需实时移动，将优化后的轮式里程计作为机器人的控制输入信息u_t-1，假设当前时刻为t，机器人在t-1时刻的粒子群为基于式其中代表基于里程计的运动更新，预测下一时刻粒子群的位置。

2.如权利要求1所述的基于粒子滤波的远程定位方法，其特征在于，基于激光匹配进行粒子状态微调具体为：

根据预测得到的粒子群在t时刻的状态由于轮式里程计存在误差，需要对粒子群中的每一个粒子进行位置微调，在移动过程中基于激光似然域模型，对当前激光观测信息和粒子所处的地图进行匹配，计算得到当前粒子的置信度；以每个粒子以预测值为初值，在一定范围内对粒子进行移动，计算各个位置的粒子置信度，保留置信度最高的粒子，替换预测得到的粒子群，得到微调的粒子群

3.如权利要求2所述的基于粒子滤波的远程定位方法，其特征在于，基于激光观测信息进行粒子状态更新具体为：

对于粒子群中每个粒子在其周围一定范围的区域内有规则地选取K个粒子，对每个粒子点求取其置信度其中基于机器人的概率运动模型计算得到，基于激光地图的似然域模型计算得到，基于这K个点的置信度计算近似的后验分布，最终计算得到粒子状态的后验分布从该后验分布中重新采样得到t时刻最终的粒子群

4.如权利要求1至3任一所述的基于粒子滤波的远程定位方法，其特征在于，判断机器人是否被绑架具体为：

基于当前的激光传感器信息z_t地图m，计算基于多传感器融合算法得到的机器人位姿x_PF的置信度和基于视觉SLAM得到的机器人位姿x_visual的置信度

若则认为此时机器人已经被绑架，对粒子群基于视觉位姿重新初始化；

若则认为此时视觉位姿信息失效，进行粒子权重更新；

若则认为此时视觉位姿信息有效，进行粒子权重更新。

5.如权利要求4所述的基于粒子滤波的远程定位方法，其特征在于，若则认为此时视觉位姿信息失效，进行粒子权重更新具体为：

6.如权利要求5所述的基于粒子滤波的远程定位方法，其特征在于，若则认为此时视觉位姿信息有效，进行粒子权重更新具体为

其中为视觉权重，x_visual为视觉SLAM系统输出的位姿，按照如下公计算：