CN113791617A - 一种基于消防机器人物理尺寸的全局路径规划方法 - Google Patents

Abstract

本发明公开了一种基于消防机器人物理尺寸的全局路径规划方法，在消防机器人运动过程中实时对地图进行处理，改变栅格地图中栅格的膨胀设置，既符合机器人的物理模型，又满足机器人避障要求而膨胀出的额外距离。通过这样的处理，机器人的全局规划路径不会穿过狭窄的障碍物之间，也能通过比机器人物理尺寸较大的通道。

Description

一种基于消防机器人物理尺寸的全局路径规划方法

技术领域

本申请属于路径规划技术领域，具体涉及一种基于消防机器人物理尺寸的全局路径规划方法。

背景技术

移动机器人的路径规划是机器人在获取到当前环境地图后，规划出一条无碰撞的从起点到终点的最优路径。基于move_base的导航框架解决了机器人运行各个节点之间的通信问题，从全局规划器、局部规划器的创建，到静态地图、障碍物地图、膨胀地图、体素地图的维护，move_base一共开启了十一个进程来完成上述工作。全局路径规划依赖于代价地图的建立，机器人完成建图后，使用栅格地图保存地图的信息，可以使用一维数组或者是二维数组来保存地图中所有栅格点，通过下标的索引就能够找到每个栅格的代价值，再加上地图分辨率，就能完成栅格地图到世界地图的坐标转换，也就是机器人在地图中实际坐标和在栅格地图中的坐标存在一个可转换的关系。

而在move_base整体工作流程中，需要根据障碍物栅格信息，结合动态加载的参数膨胀半径，实现对障碍物周边栅格的膨胀。当障碍物的膨胀半径设置过大时，可能会出现距离较近的两个障碍物之间被膨胀区域填满，且相邻障碍物之间的膨胀区域重叠，这就导致在全局路径规划时无法从这两个障碍物之间通过，而是规划成绕行的方式，导致规划得到的全局路径并不是最佳路径。

发明内容

本申请的目的在于提供一种基于消防机器人物理尺寸的全局路径规划方法，提高全局路径规划的有效性。

为实现上述目的，本申请所采取的技术方案为：

一种基于消防机器人物理尺寸的全局路径规划方法，适用于正方形的消防机器人，该消防机器人上安装有激光雷达，所述基于消防机器人物理尺寸的全局路径规划方法，包括：

步骤1、获取障碍物地图和对应的栅格地图，基于栅格地图进行全局路径规划得到当前最优路径并执行步骤2；

步骤2、控制消防机器人根据当前最优路径运动，并在运动过程中通过激光雷达持续检测是否满足窄通道判断条件，若满足窄通道判断条件则执行步骤3；否则继续执行步骤2，所述窄通道判断条件为激光雷达扫描到两个障碍物且消防机器人当前位置与当前最优路径中位于消防机器人当前位置后的任一控制点之间不存在障碍物；

步骤3、根据消防机器人的物理尺寸判断激光雷达扫描到的两个障碍物之间的通道是否为窄通道，若为窄通道则执行步骤4；否则返回步骤2继续执行；

步骤4、计算通过窄通道的中间位置的离散坐标，将离散坐标转换为栅格地图中的栅格坐标并标记，继续执行步骤5；

步骤5、根据预设的膨胀半径对栅格地图中的障碍物进行膨胀，并在膨胀过程中忽略已标记过的栅格，得到膨胀地图，继续执行步骤6；

步骤6、基于消防机器人当前位置根据膨胀地图重新进行全局路径规划，更新消防机器人的当前最优路径并返回步骤2。

以下还提供了若干可选方式，但并不作为对上述总体方案的额外限定，仅仅是进一步的增补或优选，在没有技术或逻辑矛盾的前提下，各可选方式可单独针对上述总体方案进行组合，还可以是多个可选方式之间进行组合。

作为优选，所述根据消防机器人的物理尺寸判断激光雷达扫描到的两个障碍物之间的通道是否为窄通道，包括：

控制激光雷达获取两个障碍物距离消防机器人最近点的激光数据oA₀、oB₀，且激光数据oA₀对应的激光角度为α₀，激光数据oB₀对应的激光角度为β₀，其中o点为激光雷达的中心点；

以角度阈值递增和递减的方式分别控制激光雷达依次获取两个障碍物对应的激光数据和激光角度，直至激光雷达无法扫描到障碍物，获得激光数据oA₀、oA₁、…、oA_i、…、oA_n和oB₀、oB₁、…、oB_i、…、oB_n，和激光角度α₀、α₁、…、α_i、…、α_n和β₀、β₁、…、β_i、…、β_n，且i＝0,1,2,…,n；

根据对应的激光数据oA_i和oB_i计算得到两个障碍物在激光点A_i和B_i之间的间距d_i：

取消防机器人的边长为d1，对角线长为

消防机器人运动过程中与障碍物之间的最小距离为d2，则两个障碍物之间的距离d_i满足：

则认为两个障碍物之间的通道为窄通道；否则两个障碍物之间的通道不为窄通道。

作为优选，所述计算通过窄通道的中间位置的离散坐标，包括：

取激光数据oA_i和oB_i对应的激光点A_i和B_i之间的中点C_i，且C_i＝2/d_i；

根据相邻两个中点的坐标计算得到n-1条直线段；

基于预设的离散程度取n-1条直线段上的离散坐标。

作为优选，所述将离散坐标转换为栅格地图中的栅格坐标，包括：

由障碍物地图中的离散坐标转换为栅格地图中的栅格坐标的公式为：

式中，m_x、m_y代表栅格坐标，w_x、w_y代表离散坐标，origin_x、origin_y代表障碍物地图的原点的坐标，resolution代表障碍物地图的分辨率。

本申请提供的基于消防机器人物理尺寸的全局路径规划方法，在消防机器人运动过程中实时对地图进行处理，改变栅格地图中栅格的膨胀设置，既符合机器人的物理模型，又满足机器人避障要求而膨胀出的额外距离。通过这样的处理，机器人的全局规划路径不会穿过狭窄的障碍物之间，也能通过比机器人物理尺寸较大的通道。

附图说明

图1为本申请的消防机器人导航的物理仿真环境；

图2为本申请设置第一膨胀半径后膨胀地图的示意图；

图3为本申请设置第二膨胀半径后膨胀地图的示意图；

图4为本申请基于消防机器人物理尺寸的全局路径规划方法的流程图；

图5为本申请判断窄通道的一种实施例示意图。

图示中：1、消防机器人；2、障碍物；3、第一膨胀半径膨胀后的障碍物区域；4、第二膨胀半径膨胀后的障碍物区域。

具体实施方式

下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。

除非另有定义，本文所使用的所有的技术和科学术语与属于本申请的技术领域的技术人员通常理解的含义相同。本文中在本申请的说明书中所使用的术语只是为了描述具体的实施例的目的，不是在于限制本申请。

现有技术中，在进行全局路径规划时，例如使用A*算法计算全局路径的时候，会使用两个队列来处理栅格情况，在搜索的过程中，使用的四邻域计算，这样最终其中一个队列保存的是起点到终点会经过的栅格路径，这条栅格队列中的栅格代价是最小的，保证了本次搜索的全局路径是最短的。

然而在消防机器人的仿真导航中发现，基于如图1所示的物理仿真环境，封闭地图大小为15*6(米)，地图分辨率为0.05，消防机器人1尺寸大小为0.9*0.9(米)，机器人的膨胀半径设置成0.636米。静态的障碍物2大小统一设置为1*0.15(米)，第一列障碍物间距统一为0.5米，第二列障碍物间距统一为0.75米，第三列障碍物间距统一为1米，第四列障碍物间距统一为1.25米，每列障碍物之间的水平间距为2.5米。

在仿真导航的过程中发现：如图2所示，采用第一膨胀半径对栅格地图进行膨胀时，得到第一膨胀半径膨胀后的障碍物区域3，可见前三列障碍物因为膨胀半径设置的比较大，相邻障碍物之间的膨胀区域有重叠的部分，这样导致机器人的全局规划过程中，起点到终点之间的路径规划不会直接穿过这些障碍物，而是选择更加安全也更加合理的绕行。在第四列障碍物前，因为相邻障碍物之间的距离较大，膨胀区域不会重叠，因此在障碍物之间存在一条比价狭窄的通道，机器人全局规划会直接穿过这条窄通道。

如图3所示，采用比第一膨胀半径大的第二膨胀半径对栅格地图进行膨胀时，得到第二膨胀半径膨胀后的障碍物区域4，此时机器人的膨胀半径设置的更大一点，达到实验中方形机器人对角线长度时(这个长度满足机器人的原地旋转要求)，前三列障碍物之间还是被膨胀区域给填充，全局规划不会从中间穿过，此时第四列障碍物之间也被膨胀区域填满，且相邻障碍物之间的膨胀区域重叠，全局路径不再从通道之间通过，而是规划成绕行的方式，在本次全局规划中消防机器人不能穿过宽度为1.25米的障碍物，而是需要选择绕行的方式，显然这并不符合常理，不能作为最优的规划路径。

由于现有的全局路径规划方法在全局路径搜索的过程中并没有考虑到机器人的真实尺寸，只要栅格状态是FREE_SPACE，就是可通行的状态。当地图进行膨胀处理之后，原本状态为FREE_SPACE的栅格代价值发生了变化，修改为253，因此导致原本可以通过的通道变得不可通行。(栅格代价值254代表有障碍物，253代表是障碍物周边的膨胀栅格，小于253表示可行区域，255表示未知状态)。当障碍物进行膨胀之后，狭窄通道之间的栅格变得不可通过，因此全局路径必须绕行，前三列障碍物因为通道间距太小(0.5,0.75,1)，这样处理相对合适，对于第四列通道，通道宽度1.25米，对于机器人的尺寸0.9米有较大的余量，如果按照现有的全局路径规划方式进行处理，选择机器人对角线长度的膨胀半径，机器人无法通过部分原本能够通过的通道，导致全局路径规划得到的并不是最优的路径。

其中一个实施例中，为了解决上述由于膨胀半径设置不恰当造成全局路径规划无法得到最佳路径的情况，提供一种基于消防机器人物理尺寸的全局路径规划方法，本实施例中的规划方法适用于正方形的消防机器人，也就是可以原地转向且应用差速模型的机器人。

如图4所示，本实施例的基于消防机器人物理尺寸的全局路径规划方法，包括以下步骤：

步骤1、获取障碍物地图和对应的栅格地图，基于栅格地图进行全局路径规划得到当前最优路径并执行步骤2。

在本实施例中首先基于预设的膨胀半径对障碍物地图对应的栅格地图进行膨胀处理并全局路径规划(例如A*算法)，得到的当前最优路径中包含多个控制消防机器人运动的控制点，但此时全局路径规划得到的当前最优路径可能有忽略消防机器人能够通过的窄通道。

步骤2、控制消防机器人根据当前最优路径运动，并在运动过程中通过激光雷达持续检测是否满足窄通道判断条件，若满足窄通道判断条件则执行步骤3；否则继续执行步骤2。

本实施例为了优化消防机器人的运动路径，在消防机器人运行过程中实时监测是否遇到消防机器人能够通过的窄通道。本实施例基于激光雷达判断消防机器人位于当前位置时是否遇到需要进行窄通道判断的障碍物，本实施例设置窄通道判断条件为激光雷达扫描到两个障碍物且消防机器人当前位置与当前最优路径中位于消防机器人当前位置后的任一控制点之间不存在障碍物。

根据激光雷达得到的激光角度可判断激光雷达是否扫描到两个障碍物，并且可基于障碍物地图判断任意两点之间是否存在障碍物，该判断过程为现有的常规判断技术(例如以当前位置的坐标和当前位置后的任一控制点的坐标计算得到直线，然后根据障碍物地图获取障碍物的范围，判断直线是否穿过障碍物范围)，本实施例中不进行详述。

需要说明的是，在判断当前位置与当前位置之后的任一控制点之间是否存在障碍物时，以从终点开始回溯往前取控制点的方式进行判断。

步骤3、根据消防机器人的物理尺寸判断激光雷达扫描到的两个障碍物之间的通道是否为窄通道，若为窄通道则执行步骤4；否则返回步骤2继续执行。

为了适应规则和非规则障碍物之间的间距计算，本实施例提供一种窄通道判断方式如下：

如图5所示，控制激光雷达获取两个障碍物距离消防机器人最近点的激光数据oA₀、oB₀，且激光数据oA₀对应的激光角度为α₀，激光数据oB₀对应的激光角度为β₀，其中o点为激光雷达的中心点，A₀和B₀为激光雷达测距时的激光点。由于激光雷达扫描到障碍物的边界点时其激光数据和激光角度均会发生明显的突变，因此可以判断在对应方向上存在障碍物。激光角度为激光点与o点的连线与激光雷达坐标系中的y轴的夹角。

以角度阈值递增和递减的方式分别控制激光雷达依次获取两个障碍物对应的激光数据和激光角度，直至激光雷达无法扫描到障碍物，获得激光数据oA₀、oA₁、…、oA_i、…、oA_n和oB₀、oB₁、…、oB_i、…、oB_n，和激光角度α₀、α₁、…、α_i、…、α_n和β₀、β₁、…、β_i、…、β_n，且i＝0,1,2,…,n。

以图5为例，由于图5中激光点A₀相对而言更加接近激光雷达的坐标系的y轴，因此本实施例中在取激光数据oA₀、oA₁、…、oA_i、…、oA_n时，控制激光角度由α₀以角度阈值递增，在取激光数据oB₀、oB₁、…、oB_i、…、oB_n时，控制激光角度由β₀以角度阈值递减。

考虑到障碍物的大小以及数据处理压力，本实施例中角度阈值优选取1°，在其他实施例中可根据实际情况进行调整，例如2°、3°等。

根据对应的激光数据oA_i和oB_i计算得到两个障碍物在激光点A_i和B_i之间的间距d_i(d₀、d₁、…、d_n)：

取消防机器人的边长为d1，对角线长为

消防机器人运动过程中与障碍物之间的最小距离为d2，则两个障碍物之间的距离d_i满足：

即两个对应激光点之间间距的中点均满足上述条件，则认为两个障碍物之间的通道为窄通道；否则两个障碍物之间的通道不为窄通道。

需要说明的是，本实施例计算两个障碍物之间对应的激光点的间距作为两个障碍物之间的一个离散间距，以离散间距计算方式应对规则和不规则障碍物之间的通道判断，适应性高且简化计算。并且在激光以角度阈值递增或递减的方式获取激光数据后，若一个障碍物的激光数据的个数与另一个障碍物的激光数据的个数不相等，则以激光数据较少的个数匹配两个障碍物对应的激光数据。

图中坐标系xoy为激光雷达对应的坐标系，o点为激光雷达的中心点，同样也为消防机器人的中心位置，坐标系XOY为障碍物地图的坐标系，O点为原点，通常位于障碍物地图左下角。

步骤4、计算通过窄通道的中间位置的离散坐标，将离散坐标转换为栅格地图中的栅格坐标并标记，继续执行步骤5。

若两个障碍物之间的通道为消防机器人能够通过的窄通道，则需要避免在全局路径规划中对障碍物之间的栅格进行膨胀，使得在全局路径规划中允许消防机器人从两个障碍物之间通过。

本实施例首先取激光数据oA_i和oB_i对应的激光点A_i和B_i之间的中点C_i(C₀、C₁、C₂…C_n)，且C_i＝2/d_i；根据相邻两个中点的坐标计算得到n-1条直线段；基于预设的离散程度取n-1条直线段上的离散坐标。

中点C_i的坐标根据激光点A_i和B_i的坐标计算得到，其中激光点A_i和B_i的坐标可根据消防机器人当前位置的坐标计算得到。本实施例以计算激光点A_i和B_i的坐标为例进行简述：根据消防机器人上安装的里程计得到机器人当前坐标(X,Y)，并通过对里程计积分得到消防机器人坐标系中的y轴与障碍物地图坐标系中的X轴之间的夹角为θ，则激光点A_i的坐标

为

则激光点B_i的坐标

为

本实施例基于直线段取离散坐标时，使用bresenham算法，可以得到直线通过的栅格点。通过直线的斜率，自定义栅格之间的增量，也就是直线的离散程度，可以得到这条直线所经过的栅格。

本实施例中由障碍物地图中的离散坐标转换为栅格地图中的栅格坐标的公式为：

式中，m_x、m_y代表栅格坐标，w_x、w_y代表离散坐标，origin_x、origin_y代表障碍物地图的原点的坐标，resolution代表障碍物地图的分辨率。

步骤5、根据预设的膨胀半径对栅格地图中的障碍物进行膨胀，并在膨胀过程中忽略已标记过的栅格，得到膨胀地图，继续执行步骤6。

在膨胀过程中忽略已标记的栅格，使得在全局路径规划时将两个障碍物之间的通道识别为消防机器人能够通过的窄通道，避免进行避让而增加路径长度。

修改导航过程中维护的costmap，在对地图进行膨胀处理之前，将数组中的栅格状态修改成true，这样地图在膨胀的时候就不会对这些栅格进行膨胀，人为的在窄通道的中间开辟出一条可通行的路径。

步骤6、基于消防机器人当前位置根据膨胀地图重新进行全局路径规划，更新消防机器人的当前最优路径并返回步骤2。

本实施例的全局路径规划方法在消防机器人运动过程中实时判断窄通道并根据最优路径，从而保证消防机器人能够以最优、最快的方式由起点运动至终点，实现最短路径规划运动。

以上所述实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。

以上所述实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。因此，本申请专利的保护范围应以所附权利要求为准。

Claims (4)

1.一种基于消防机器人物理尺寸的全局路径规划方法，适用于正方形的消防机器人，该消防机器人上安装有激光雷达，其特征在于，所述基于消防机器人物理尺寸的全局路径规划方法，包括：

步骤1、获取障碍物地图和对应的栅格地图，基于栅格地图进行全局路径规划得到当前最优路径并执行步骤2；

步骤2、控制消防机器人根据当前最优路径运动，并在运动过程中通过激光雷达持续检测是否满足窄通道判断条件，若满足窄通道判断条件则执行步骤3；否则继续执行步骤2，所述窄通道判断条件为激光雷达扫描到两个障碍物且消防机器人当前位置与当前最优路径中位于消防机器人当前位置后的任一控制点之间不存在障碍物；

步骤3、根据消防机器人的物理尺寸判断激光雷达扫描到的两个障碍物之间的通道是否为窄通道，若为窄通道则执行步骤4；否则返回步骤2继续执行；

步骤4、计算通过窄通道的中间位置的离散坐标，将离散坐标转换为栅格地图中的栅格坐标并标记，继续执行步骤5；

步骤5、根据预设的膨胀半径对栅格地图中的障碍物进行膨胀，并在膨胀过程中忽略已标记过的栅格，得到膨胀地图，继续执行步骤6；

步骤6、基于消防机器人当前位置根据膨胀地图重新进行全局路径规划，更新消防机器人的当前最优路径并返回步骤2。

2.如权利要求1所述的基于消防机器人物理尺寸的全局路径规划方法，其特征在于，所述根据消防机器人的物理尺寸判断激光雷达扫描到的两个障碍物之间的通道是否为窄通道，包括：

控制激光雷达获取两个障碍物距离消防机器人最近点的激光数据oA₀、oB₀，且激光数据oA₀对应的激光角度为α₀，激光数据oB₀对应的激光角度为β₀，其中o点为激光雷达的中心点；

以角度阈值递增和递减的方式分别控制激光雷达依次获取两个障碍物对应的激光数据和激光角度，直至激光雷达无法扫描到障碍物，获得激光数据oA₀、oA₁、…、oA_i、…、oA_n和oB₀、oB₁、…、oB_i、…、oB_n，和激光角度α₀、α₁、…、α_i、…、α_n和β₀、β₁、…、β_i、…、β_n，且i＝0,1,2,…,n；

根据对应的激光数据oA_i和oB_i计算得到两个障碍物在激光点A_i和B_i之间的间距d_i：

取消防机器人的边长为d1，对角线长为

消防机器人运动过程中与障碍物之间的最小距离为d2，则两个障碍物之间的距离d_i满足：d1+2*d2≤d_i≤

则认为两个障碍物之间的通道为窄通道；否则两个障碍物之间的通道不为窄通道。

3.如权利要求2所述的基于消防机器人物理尺寸的全局路径规划方法，其特征在于，所述计算通过窄通道的中间位置的离散坐标，包括：

取激光数据oA_i和oB_i对应的激光点A_i和B_i之间的中点C_i，且C_i＝2/d_i；

根据相邻两个中点的坐标计算得到n-1条直线段；

基于预设的离散程度取n-1条直线段上的离散坐标。

4.如权利要求3所述的基于消防机器人物理尺寸的全局路径规划方法，其特征在于，所述将离散坐标转换为栅格地图中的栅格坐标，包括：

由障碍物地图中的离散坐标转换为栅格地图中的栅格坐标的公式为：

式中，m_x、m_y代表栅格坐标，w_x、w_y代表离散坐标，origin_x、origin_y代表障碍物地图的原点的坐标，resolution代表障碍物地图的分辨率。

Images