CN114995442B - 基于最佳观测点序列的移动机器人运动规划方法及装置 - Google Patents

Abstract

本发明属于移动机器人领域，是基于最佳观测点序列的移动机器人运动规划方法及装置。本发明以信息采集为任务背景的运动规划方法能够将信息存在于不同区域的概率与其他要素相结合，通过拓扑语义地图，判断空间中不同区域的搜索优先级，合理的对空间进行目标搜索与信息采集，该方法不同于以往机器人由近及远的遍历方式，提高了机器人搜索效率，大大减少了机器人的搜索能耗，并且由于移动机器人的可控性，机器人在工厂的复杂环境内可稳定运行，不仅保障了工厂设备运作的有序性，还保障了工人的人身安全，并提高了生产活动的便捷性。

Description

基于最佳观测点序列的移动机器人运动规划方法及装置

技术领域

本发明属于移动机器人领域，涉及一种基于最佳观测点序列的移动机器人运动规划方法及装置，具体是一种在拓扑语义地图上基于最佳观测点序列的移动机器人自主进行工厂内信息的搜索与采集，执行后机器人自主返回起始位置的运动规划方法。

背景技术

移动机器人对信息的采集通常是指利用机器人对工作环境内的目标进行搜索，当检测到目标时对其进行识别与采集信息。因工厂内环境复杂并且根据工厂性质的不同，多数工厂的内部环境对人并不友好，故工厂应用场景下实现移动机器人代替人进行所需信息的自主采集与自动返回是很有必要的；通过拓扑语义地图，机器人根据语义信息依次前往优先级高的观测区域进行信息的采集，并且由于移动机器人的可控性，机器人在工厂的复杂环境内可稳定运行，不仅保障了工厂设备运作的有序性，还保障了工人的人身安全，并提高了生产活动的便捷性。运动规划指在已知起点与终点的情况下，规划出一条合理的路径，“合理”不仅仅是指规划出的路径距离最短，根据实际情况还可能是机器人执行时间最短、规定时间内探索的区域最大等。

目前，自主移动机器人在执行信息采集任务方面大多按照距离机器人位置由近及远遍历空间的方式进行，这种方式虽然能够通过遍历空间中的所有位置来实现采集任务，但存在效率低下的问题，若要应用于工厂这种大空间场合中，很明显这种方法的可行性较低，且机器人的能耗问题也是一个考虑的重点。

发明内容

本发明的第一个目的是为了解决现有技术存在问题，提供一种基于最佳观测点序列的移动机器人运动规划方法，能够使得机器人在执行信息采取任务时合理规划最佳运动路线，提高采集任务效率，降低机器人的能耗。

本发明提出了一种在基于最佳观测点序列的移动机器人自主采集信息与返回的运动规划方法，使得机器人采集运动的期望时间较小。假定单个目标隐藏在若干个观测点其中一个的可视空间中，这时的一个搜索策略可以用观测点访问顺序的排列组合来表示，它表示在前一个观测点没有找到目标的情况下，下一次的搜索在哪进行。整个运动规划过程分为上下两层，上层是实现机器人访问所有观测点的路径规划，通过优化观测点顺序来减小机器人的运动期望时间就是上层路径规划的搜索策略；下层是实现机器人在观测点之间运动的运动规划。机器人采集到信息后返回初始位置。

一种基于最佳观测点序列的移动机器人的运动规划方法，包括：

步骤1、利用快速扩展随机树(RRT)对机器人所处空间覆盖，生成多个采样点；

步骤2、对采样点进行删减得到观测点集合，并构建观测点位置的相机探测模型；同时根据观测点构建拓扑语义地图；具体是：

2-1考虑到障碍物遮挡、相机检测距离等因素，将所述采样点进行筛选得到m个观测点；

2-2在观测点可视空间不重叠的情况下，若目标位于空间中的u点，位于第i个观测点w_i位置的相机探测模型表示为：

其中θ表示机器人的相机在静止时刻的水平探测弧度，Δrad表示在单位时间内机器人搜索观测点空间探测到新环境的水平弧度；K_i(u,w_i,time_i)表示在第i个观测点花费时长time_i探测到目标的概率；||u-w_i||表示在第i个观测点的相机与目标的无遮挡空间距离，time_i表示在整个空间实现覆盖搜索下每个观测点的搜索时长；

2-3根据观测点构建拓扑语义地图；具体是：根据以往机器人发现目标的时间点以及在各个观测点的可视空间出现顺序以及频次，计算出在当前时间目标存在于各个观测点的概率，从而得到拓扑语义地图。

步骤3、上层运动规划，寻找最佳观测点序列；

3-1、计算机器人起始点出发到采集到信息返回起始点的时间代价；

将拓扑语义地图上机器人的起始点q₀作为第0个观测点加入到观测点集合中，通过拓扑语义地图更新得到目标在每一观测点的概率P_i，目标存在于起始点可视空间内的概率为P₀，计算机器人从第i-1个观测点到第i个观测点然后返回起始点所要花费的总代价cost：

cost＝α(P_i-P_i-1)+d_i+O_i,i∈[1,m] (3)

其中d_i表示经过第i-1个观测点到达第i个观测点的机器人运动距离，O_i表示从第i个观测点返回起始点的机器人运动距离，α表示权重，α取值大于0；

机器人以速度v匀速运动，则机器人从第i-1个观测点到第i个观测点的时间代价为：

其中m表示观测点集合中除起始点q₀外的观测点总个数；

假设机器人在第s个观测点采集到信息，则机器人从起始点出发至采集到信息返回起始点的时间代价为：

上层运动规划的目标是寻求一个观测点序列，使得发现目标时的期望代价E(T)在所有序列中最小，故观测点序列的目标函数表示为：

3-2、生成最佳观测点序列；

3-2-1、为实现最优期望时间内机器人完成信息采集过程，根据拓扑语义地图找到机器人的待访问观测点q_i,i∈[0,m]，组建得到待访问观测点集合；计算待访问观测点q_i,i∈[1,m]与起始点q₀之间总代价cost，在待访问观测点q_i中挑选出所有观测点q_i,i＝1,…，m与起始点q₀之间总代价cost之和最大的待访问观测点q_b作为位于起始点q₀后的第二观测点，构成当前机器人的运动回路；

3-2-2、判断待访问观测点集合是否还存在没有插入到回路的观测点q_i，若是则遍历待访问观测点集合，计算集合中每个待访问观测点q_i与运动回路中所有观测点的总代价cost之和，筛选出总代价cost之和最大的待访问观测点q_i，将当前待访问观测点q_i作为当前机器人的运动回路的待插入观测点；

3-2-3、计算待插入观测点插入当前机器人的运动回路各个位置后的期望代价E(T)，寻找期望代价E(T)最小对应的待插入观测点插入运动回路位置，然后将当前待插入观测点插入运动回路的上述位置，构成当前机器人的最新运动回路；重复步骤3-2-2至3-2-3，直至待访问观测点集合为空；

3-2-4、由于步骤3-2-3得到的当前机器人的最新运动回路存在从起始点出发的顺时针运动路径和逆时针运动路径两种情况，故选取期望代价E(T)最小运动路径，即为最佳观测点序列；

步骤4、下层运动规划，生成运动规划结果；

根据拓扑语义地图和最佳观测点序列，生成机器人在最佳观测点序列上相邻观测点间的路径，最终实现对机器人所处空间进行覆盖搜索，实现机器人对信息的自主采集与返回。

本发明的第二个目的是提供一种基于最佳观测点序列的移动机器人的运动规划装置，包括：

采样点获取及预处理模块，用于获取通过快速扩展随机树RRT生成的采样点，然后将采样点经过删减处理获得观测点集；

上层运动规划模块，用于计算机器人起始点出发——采集信息——返回起始点的时间代价，并寻找到最佳观测点序列；

下层运动规划模块，用于根据拓扑语义地图和最佳观测点序列，生成机器人在最佳观测点序列上相邻观测点间的路径，最终实现对机器人所处空间进行覆盖搜索，实现机器人对信息的自主采集与返回。

本发明的第三个目的是提供一种计算机可读存储介质，其上存储有计算机程序，当所述计算机程序在计算机中执行时，令计算机执行所述的方法。

本发明的第四个目的是提供一种计算设备，包括存储器和处理器，所述存储器中存储有可执行代码，所述处理器执行所述可执行代码时，实现所述的方法。

本发明提出的在基于最佳观测点序列的移动机器人自主采集信息与返回的运动规划方法能够针对目标存在于不同位置的概率与其他要素相结合，判断空间中不同位置的搜索优先级，合理的对空间中的目标进行搜索，然后进行识别与信息采集，该方法不同于以往机器人由近及远的遍历方式，提高了机器人信息采集效率，大大减少了机器人的能耗。

附图说明

图1为本发明上层运动规划构成的观测点回路示例；

图2为本发明上层运动规划构成观测点序列的第一种情况；

图3为本发明上层运动规划构成观测点序列的第二种情况；

图4是为本发明流程图。

具体实施方式

下面结合附图对本发明做进一步的分析。

基于最佳观测点序列的移动机器人自主采集信息与返回的运动规划方法包括如下步骤：

步骤1、利用快速扩展随机树(RRT)算法，使其通过随机采样生成采样点，由于RRT算法的采样点不位于障碍物区域、父子节点之间连线不经过障碍物的特点，所以生成的每一个采样点都是机器人可到达的，当算法运行一定时间后，RRT生成的节点对机器人所处空间进行了覆盖，保存所有采样点；

步骤2、对采样点进行删减得到观测点集合，并构建观测点位置的相机探测模型；同时根据观测点构建拓扑语义地图；具体是：

2-1选取任一采样点x作为观测点，考虑到相机检测距离因素，根据目标物体大小以及相机检测能力，得到相机检测最远距离R，把与x之间距离小于R的采样点作为待删除点，由于考虑到环境中障碍物对相机检测的影响，将与x之间存在障碍物的采样点从待删除点中去除，其余点全部删除，重复以上步骤，直至地图中采样点一直保持为m个；

2-2在观测点可视空间不重叠的情况下，若目标位于空间中的u点，位于第i个观测点w_i位置的相机探测模型表示为：

其中θ表示机器人的相机在静止时刻的水平探测弧度，Δrad表示在单位时间内机器人搜索观测点空间探测到新环境的水平弧度；K_i(u,w_i,time_i)表示在第i个观测点花费时长time_i探测到目标的概率；||u-w_i||表示在第i个观测点的相机与目标的无遮挡空间距离，time_i表示在整个空间实现覆盖搜索下每个观测点的搜索时长；

2-3根据观测点构建拓扑语义地图；具体是：根据机器人此时的位置，将观测点集合中的所有观测点按照与机器人之间的距离由近及远排列，机器人按照此排列顺序依次前往观测点搜索目标，若在第i个观测点检测到目标，则记录此时时刻M_i，将观测点以及此时时刻进行对应，机器人遍历完观测点后继续按照此前的排列顺序从第一个观测点继续开始寻找目标直至机器人接收到结束指令，当机器人在一观测点检测到目标时，若此观测点已有对应的时刻，则用当前时刻对其进行更新，记录最后检测到目标的时刻M_e。

M₀为机器人开始上层运动规划的时刻，若对于第i个观测点存在有对应检测到目标时间的观测点，其概率P_i可以由计算得到，len_i为M₀减去第i个观测点对应的时间点M_i，i＝1,…,m，得到的一个时长，单位为秒；v的值与空间环境大小以及(M₀-M_e)成反比，根据实际情况下空间以及(M₀-M_e)的大小得到。

若某些观测点自始至终没有搜索到目标，但在真正搜索过程中目标仍有一定概率存在于其观测范围之中，故将这些观测点对应概率设定为此时得到地图上每个观测点可视范围对应的目标存在概率，从而生成拓扑语义地图。

步骤3、上层运动规划，寻找最佳观测点序列；

3-1、计算机器人起始点出发到采集到信息返回起始点的时间代价；

将拓扑语义地图上机器人的起始点q₀作为第0个观测点加入到观测点集合中，通过拓扑语义地图得到目标在每一观测点的概率P_i，目标存在于起始点可视空间内的概率为P₀，且计算机器人从第i-1个观测点到第i个观测点然后返回起始点所要花费的总代价cost：

cost＝α(P_i-P_i-1)+d_i+O_i,i∈[1,m] (3)

其中d_i表示经过第i-1个观测点到达第i个观测点的机器人运动距离，O_i表示从第i个观测点返回起始点的机器人运动距离，α表示权重，α取值大于0；

机器人以速度v匀速运动，则机器人从第i-1个观测点到第i个观测点的时间代价为：

其中m表示观测点集合中除起始点q₀外的观测点总个数；

假设机器人在第s个观测点采集到信息，则机器人从起始点出发至采集到信息返回起始点的时间代价为：

上层运动规划的目标是寻求一个观测点序列，使得发现目标时的期望代价E(T)在所有序列中最小，故观测点序列的目标函数表示为：

3-2、生成最佳观测点序列；

3-2-1、为实现最优期望时间内机器人完成信息采集过程，根据拓扑语义地图找到机器人的待访问观测点q_i,i∈[0,m]，组建得到待访问观测点集合；计算待访问观测点q_i,i∈[1,m]与起始点q₀之间总代价cost，在待访问观测点q_i中挑选出所有观测点q_i,i＝1,…，m与起始点q₀之间总代价cost之和最大的待访问观测点q_b作为位于起始点q₀后的第二观测点，构成当前机器人的运动回路；

3-2-2、判断待访问观测点集合是否还存在没有插入到回路的观测点q_i，若是则遍历待访问观测点集合，计算集合中每个待访问观测点q_i与运动回路中所有观测点的总代价cost之和，筛选出总代价cost之和最大的待访问观测点q_i，将当前待访问观测点q_i作为当前机器人的运动回路的待插入观测点；

3-2-3、计算待插入观测点插入当前机器人的运动回路各个位置后的期望代价E(T)，寻找期望代价E(T)最小对应的待插入观测点插入运动回路位置，然后将当前待插入观测点插入运动回路的上述位置，构成当前机器人的最新运动回路；重复步骤3-2-2至3-2-3，直至待访问观测点集合为空，此时得到机器人运动回路，运动回路如图1所示，图中是共有五个观测点的回路以及观测点之间的相对位置，q₀为起始点，此时回路可表示为q₀-q₃-q₄-q₁-q₂-q₀；

3-2-4、由于步骤3-2-3得到的当前机器人的最新运动回路存在从起始点出发的顺时针运动路径和逆时针运动路径两种情况，所以此回路会有两种机器人遍历所有观测点的序列，删除起始点的一条邻边会有两种情况(如图2与图3所示)，两种情况分别为q₀→q₂→q₁→q₄→q₃与q₀→q₃→q₄→q₁→q₂，故选取期望代价E(T)最小运动路径，即为最佳观测点序列；

步骤4、机器人按照观测点序列运动进行信息的采集，机器人于初始位置出发，利用A^*算法规划出与序列中下一观测点之间的全局路径，以及DWA算法进行局部路径规划前往序列中的下一个观测点。当机器人到达观测点时，机器人对观测点可视空间进行搜索，没有检测到目标则前往下一观测点，直至机器人检测到目标。机器人采集到信息后返回初始点位置。

Claims (5)

1.基于最佳观测点序列的移动机器人运动规划方法，其特征在于该方法包括以下步骤：

步骤1、利用快速扩展随机树RRT对机器人所处空间覆盖，生成多个采样点；

步骤2、对采样点进行删减得到观测点集合，并构建观测点位置的相机探测模型；同时根据观测点构建拓扑语义地图；

2-1选取任一采样点x作为观测点，考虑到相机检测距离因素，根据目标物体大小以及相机检测能力，得到相机检测最远距离R，把与x之间距离小于R的采样点作为待删除点，由于考虑到环境中障碍物对相机检测的影响，将与x之间存在障碍物的采样点从待删除点中去除，其余点全部删除，重复以上步骤，直至地图中采样点一直保持为m个；

2-2在观测点可视空间不重叠的情况下，若目标位于空间中的u点，位于第i个观测点w_i位置的相机探测模型表示为：

其中θ表示机器人的相机在静止时刻的水平探测弧度，Δrad表示在单位时间内机器人搜索观测点空间探测到新环境的水平弧度；K_i(u,w_i,time_i)表示在第i个观测点花费时长time_i探测到目标的概率；||u-w_i||表示在第i个观测点的相机与目标的无遮挡空间距离，time_i表示在整个空间实现覆盖搜索下每个观测点的搜索时长；

2-3根据观测点构建拓扑语义地图；

步骤3、上层运动规划，寻找最佳观测点序列；

3-1、计算机器人起始点出发——采集信息——返回起始点的时间代价；

将拓扑语义地图上机器人的起始点q₀作为第0个观测点加入到观测点集合中，通过拓扑语义地图更新得到目标在每一观测点的概率P_i，目标存在于起始点可视空间内的概率为P₀，计算机器人从第i-1个观测点到第i个观测点然后返回起始点所要花费的总代价cost：

cost＝α(P_i-P_i-1)+d_i+O_i,i∈[1,m] (3)

其中d_i表示经过第i-1个观测点到达第i个观测点的机器人运动距离，O_i表示从第i个观测点返回起始点的机器人运动距离，α表示权重；

机器人以速度v匀速运动，则机器人从第i-1个观测点到第i个观测点的时间代价为：

其中m表示观测点集合中除起始点q₀外的观测点总个数；

假设机器人在第s个观测点采集到信息，则机器人从起始点出发至采集到信息返回起始点的时间代价为：

上层运动规划的目标是寻求一个观测点序列，使得发现目标时的期望代价E(T)在所有序列中最小，故观测点序列的目标函数表示为：

3-2、生成最佳观测点序列；

3-2-1、为实现最优期望时间内机器人完成信息采集过程，根据拓扑语义地图找到机器人的待访问观测点q_i,i∈[0,m]，组建得到待访问观测点集合；计算待访问观测点q_i,i∈[1,m]与起始点q₀之间总代价cost，在待访问观测点q_i中挑选出所有观测点q_i,i＝1,…，m与起始点q₀之间总代价cost之和最大的待访问观测点q_b作为位于起始点q₀后的第二观测点，构成当前机器人的运动回路；

3-2-2、判断待访问观测点集合是否还存在没有插入到回路的观测点q_i，若是则遍历待访问观测点集合，计算集合中每个待访问观测点q_i与运动回路中所有观测点的总代价cost之和，筛选出总代价cost之和最大的待访问观测点q_i，将当前待访问观测点q_i作为当前机器人的运动回路的待插入观测点；

3-2-3、计算待插入观测点插入当前机器人的运动回路各个位置后的期望代价E(T)，寻找期望代价E(T)最小对应的待插入观测点插入运动回路位置，然后将当前待插入观测点插入运动回路的上述位置，构成当前机器人的最新运动回路；重复步骤3-2-2至3-2-3，直至待访问观测点集合为空；

3-2-4、由于步骤3-2-3得到的当前机器人的最新运动回路存在从起始点出发的顺时针运动路径和逆时针运动路径两种情况，故选取期望代价E(T)最小运动路径，即为最佳观测点序列；

步骤4、下层运动规划，生成运动规划结果；

根据拓扑语义地图和最佳观测点序列，生成机器人在最佳观测点序列上相邻观测点间的路径，最终实现对机器人所处空间进行覆盖搜索，实现机器人对信息的自主采集与返回。

2.根据权利要求1所述方法，其特征在于步骤2-3具体是：根据以往机器人发现目标的时间点以及在各个观测点的可视空间出现顺序以及频次，计算出在当前时间目标存在于各个观测点的概率，从而得到拓扑语义地图。

3.实现权利要求1-2任一项所述方法的基于最佳观测点序列的移动机器人的运动规划装置，其特征在于包括：

采样点获取及预处理模块，用于获取通过快速扩展随机树RRT生成的采样点，然后将采样点经过删减处理获得观测点集；

上层运动规划模块，用于计算机器人起始点出发——采集信息——返回起始点的时间代价，并寻找到最佳观测点序列；

下层运动规划模块，用于根据拓扑语义地图和最佳观测点序列，生成机器人在最佳观测点序列上相邻观测点间的路径，最终实现对机器人所处空间进行覆盖搜索，实现机器人对信息的自主采集与返回。

4.一种计算机可读存储介质，其上存储有计算机程序，当所述计算机程序在计算机中执行时，令计算机执行权利要求1-2中任一项所述的方法。

5.一种计算设备，包括存储器和处理器，所述存储器中存储有可执行代码，所述处理器执行所述可执行代码时，实现权利要求1-2中任一项所述的方法。