[发明专利]基于粒子滤波算法的巡检机器人动态路径规划方法及装置

权利要求书

1.一种基于粒子滤波算法的巡检机器人动态路径规划方法，其特征在于，包括：

S101：对巡检机器人任务场景创建地图，建立路径滚动窗口；

S102：利用传感器获取当前时刻的环境信息，所述环境信息包括所述任务场景中障碍物和所述巡检机器人的位置与移动速度；

S103：根据所述当前时刻的环境信息，采用粒子滤波算法预测所述任务场景中动态障碍物和所述巡检机器人下一时刻的位置和移动速度，得到更新后的环境信息；

S104：根据所述巡检机器人的当前位置及任务终点，确定所述巡检机器人在所述路径滚动窗口内的局部路径起止点；

S105：在所述路径滚动窗口内建立局部坐标系，根据所述更新后的环境信息和所述局部路径起止点，在所述局部坐标系中生成所述巡检机器人的初始路径样本集；

S106：在所述路径滚动窗口内迭代执行粒子滤波算法估计路径样本，直至迭代次数达到预设次数，输出目标局部路径，以便所述巡检机器人根据所述目标局部路径前进一步；

S107：判断所述巡检机器人是否到达所述任务终点，若否，则将所述路径滚动窗口向前推进一步，循环执行S102-S107直至所述巡检机器人到达所述任务终点；

所述根据所述巡检机器人的当前位置及任务终点，确定所述巡检机器人在所述路径滚动窗口内的局部路径起止点包括：

根据所述巡检机器人的当前位置与任务终点，确定所述路径巡检窗口内的基准点；

根据所述基准点，确定所述巡检机器人的局部任务点，从而确定所述巡检机器人在所述路径滚动窗口内的局部路径起止点；

所述根据所述巡检机器人的当前位置与任务终点，确定所述路径巡检窗口内的基准点包括：

确定所述巡检机器人的当前位置与所述任务终点的连线，选取所述连线与所述路径滚动窗口的交汇点作为所述基准点；

所述根据所述基准点，确定所述巡检机器人的局部任务点包括：

以所述基准点为中心，在预设生成范围内选取所述巡检机器人的局部任务点；

其中，所述预设生成范围服从标准差为的高斯分布，L₀为全局路径长度，L_k为当前时刻的剩余待规划路径长度，δ_max为所述预设生成范围的最大标准差。

2.如权利要求1所述的方法，其特征在于，所述在所述路径滚动窗口内建立局部坐标系包括：

以所述巡检机器人的当前位置与所述基准点的连线作为坐标系横轴，在所述路径滚动窗口内建立局部坐标系，以便采用一组N维向量表示一条路径样本X＝[x₀，x₁，…，x_D]，其中，D＝L/s为一次滚动规划的步数，L为所述路径滚动窗口的大小，s为规划步长，N＝D+1，x₀为所述巡检机器人的当前位置，x₁为所述巡检机器人的局部路径点，x_D为所述巡检机器人的局部路径终点。

3.如权利要求2所述的方法，其特征在于，所述根据所述更新后的环境信息和所述局部路径起止点，在所述局部坐标系中生成所述巡检机器人的初始路径样本集包括：

根据所述更新后的环境信息和所述局部路径起止点，采用正交化算法在所述局部坐标系中生成所述巡检机器人的初始路径样本集。

4.如权利要求1所述的方法，其特征在于，所述在所述路径滚动窗口内迭代执行粒子滤波算法估计路径样本，直至迭代次数达到预设次数，输出目标局部路径，以便所述巡检机器人根据所述目标局部路径前进一步包括：

S701：将所述初始路径样本集输入至粒子滤波算法估计路径样本，并记录所述粒子滤波算法的当前迭代次数；

S702：根据路径评价函数计算每个路径样本的权值，并根据所述每个路径样本的权值估计当前最优路径样本；

S703：判断所述当前迭代次数是否达到预设次数；

S704：若所述当前迭代次数未达到所述预设次数，则在所述路径滚动窗口内对所述巡检机器人的局部路径点进行重采样，得到更新后的路径样本集与当前迭代次数，返回执行S702；

S705：若所述当前迭代次数达到所述预设次数，则输出所述当前最优路径样本作为目标局部路径，以便所述巡检机器人根据所述目标局部路径前进一步。