[发明专利]基于粒子滤波的机器人定位方法

权利要求书

1.基于粒子滤波的机器人定位方法，其特征在于，该方法包括如下步骤：

步骤1：在地面上设置J个RFID标签作为标志点；在机器人上安装传感器，用于探测机器人与每个标志点之间的距离；

步骤2：机器人根据工作人员发送的控制指令，从当前位置移动到实际位置D；令工作人员需要机器人移动到的理想位置为u；以理想位置u为中心，令采样次数为t，进行第一次采样，即t＝1，采样n个位置坐标，并作为样本集，将这n个位置坐标作为样本粒子；t＝1,2,…,max，max为总的采样次数；

具体采样方法为：随机生成n个整数，根据采样公式生成与n个整数相对应的n个位置坐标，所述采样公式为：

其中x_q为第q个整数，X为与x_q对应的位置坐标，Σ为控制采样的范围，q＝1,2,…,n；

步骤3：根据样本集中每个样本粒子与每个标志点之间的距离和机器人与每个标志点之间的距离，计算每个样本粒子的匹配度，将匹配度归一化处理，从而获得每个样本粒子的权值；

步骤4：计算样本集的有效粒子数，判断样本集的有效粒子数是否小于等于预设好的有效粒子数阈值，若是，则转步骤9；否则转步骤5：

步骤5：删除权值小于预设好的权值阈值的样本粒子；则剩余的粒子个数为n-y个，其中y为权值小于权值阈值的样本粒子的个数；t+1；

步骤6：进行第t次采样：以机器人的理想位置u为中心，利用采样公式，采样y个位置坐标，从而补充样本粒子集；

步骤7：在上一轮采样中剩余的粒子中随机选择一个粒子，基于随机选择的粒子和步骤6中采样的y个位置坐标，进行交叉计算，从而得到y个新的位置坐标，并将该y个新的位置坐标替换原y个位置坐标；得到新的样本集；

步骤8：判断t是否小于max，若是，将新的样本集作为用于下次计算的样本集，并转步骤3；否则，转步骤9；

步骤9：计算新的样本集中所有样本粒子的坐标的加权平均值；并将该加权平均值作为机器人的实际位置D的坐标；

所述步骤3中计算每个样本粒子的匹配度的具体方法为：

其中M_i为第i个样本粒子的匹配度，j为第j个标志点；d_ij为第i个样本粒子与第j个标志点的距离，为机器人传感器测得机器人与第j个标志点的实际距离；i＝1,2,…,n；j＝1,2,…,J；

所述步骤4中计算样本集的有效粒子数，采用如下公式计算样本集的有效粒子数：

2.根据权利要求1所述的基于粒子滤波的机器人定位方法，其特征在于，所述步骤7中得到y个新的位置坐标的具体方法为：将y个位置坐标中的第w个位置坐标的横坐标作为第w个新的位置坐标的横坐标，将随机选择的粒子的纵坐标作为第w个新的位置坐标的纵坐标；w＝1,2,…,y。