[发明专利]一种移动机器人初始位姿获取方法和系统

权利要求书

1.一种移动机器人初始位姿获取方法，其特征在于，包括步骤：

S100获取初始信息，包括环境地图信息和移动机器人的朝向角度位置信息；

S200根据所述初始信息，采用粒子群算法获得当前所述朝向角度位置下的最优匹配度；

S300判断当前所述朝向角度位置下的最优匹配度是否大于等于匹配阈值，若是则执行步骤S500；否则，执行步骤S400；

S400原地旋转一定角度，获得所述移动机器人的旋转后朝向角度位置下的最优匹配度，判断所述旋转后朝向角度位置下的最优匹配度是否大于等于匹配阈值，若是则执行步骤S500；否则，执行步骤S600；

S500获取最优匹配度对应粒子的位置和朝向角度，得到所述移动机器人的初始位姿；

S600移动机器人的初始位姿获取失败。

2.根据权利要求1所述的移动机器人初始位姿获取方法，其特征在于，所述步骤S400还包括步骤：

S410发布第一旋转指令，控制所述移动机器人旋转；

S420判断旋转过程中是否碰到障碍物，若是则执行步骤S440；否则，执行步骤S430；

S430判断所述移动机器人是否旋转到第一预设角度；若是则执行步骤S431；否则，返回步骤S410；

S431获得所述移动机器人旋转第一预设角度朝向下的最优匹配度；

S432判断所述第一预设角度朝向下的最优匹配度是否大于等于匹配阈值，若是则执行步骤S500；否则，执行步骤S600；

S440所述移动机器人停止旋转，获得所述移动机器人停止旋转后角度朝向下的最优匹配度；

S441判断所述停止旋转后朝向角度位置下的最优匹配度是否大于等于匹配阈值，若是则执行步骤S500；否则，执行步骤S450；

S450控制所述移动机器人旋转返回至初始状态；

S460发布第二旋转指令，控制所述移动机器人相对于所述第一旋转指令旋转方向的相反方向进行旋转；

S461判断所述移动机器人是否旋转到第二预设角度，若是则执行步骤S462；否则，返回步骤S460；

S462获得所述移动机器人旋转第二预设角度朝向下的最优匹配度；

S463判断所述第二预设角度朝向下的最优匹配度是否大于等于匹配阈值，若是则执行步骤S500；否则，执行步骤S600。

3.根据权利要求2所述的移动机器人初始位姿获取方法，其特征在于：所述旋转指令包括旋转速度指令、旋转方向指令和旋转角度指令，所述旋转角度为0-180°角度范围。

4.根据权利要求1所述的移动机器人初始位姿获取方法，其特征在于，所述步骤S100还包括步骤：

S110发射探测信号，并接收从障碍物反射回来的反射信号；

S120根据所述探测信号和发射信号得到移动机器人所在的环境地图信息；

S130读取移动机器人在所述环境地图上的朝向角度位置信息；

S140发送所述初始信息，包括环境地图信息和移动机器人的朝向角度位置信息。

5.根据权利要求1所述的移动机器人初始位姿获取方法，其特征在于，所述步骤S200还包括步骤：

S210对粒子群体进行初始化运算，根据状态变量的转移概率密度函数采样多个粒子；

S220采集所述移动机器人的所述初始信息；

S230根据所述初始信息，得到每个粒子的自身适应度值，对比获得粒子个体历史最佳适应度值和粒子群体历史最佳适应度值；

S240获得每个粒子的权重；

S250对比获得最优权重，并更新粒子的速度和位置；

S260根据所述更新的粒子速度和位置，更新每个粒子的权重；

S270对所有粒子的权重进行归一化运算；

S280重新进行采样，再次得到重采样后每个粒子的权重；

S290对比获得重采样后的最优权重作为最优匹配度输出。