CN106949893B - 一种三维避障的室内机器人导航方法和系统 - Google Patents

Abstract

本发明公开了一种三维避障的室内机器人导航方法和系统，其中方法的实现包括：采集机器人的三维模型、初始位置和终点位置，全局三维地图并进行全局路径规划，生成一系列全局路径点，在每一全局路径点处进行三维障碍物碰撞检测，得到最优的全局路径规划；机器人按最优的全局路径规划行进，在行进过程中对下一全局路径点进行三维障碍物碰撞检测，若会发生碰撞，则以实时三维地图为全局三维地图，以实时位置为初始位置重新进行全局路径规划，若不会发生碰撞，则继续行进直至达到终点位置，实现了机器人的自主导航与三维避障。本发明在拥挤的室内环境下也能顺利求解出可行的机器人路径，完成了机器人从初始位置到终点位置的自主导航和三维避障任务。

Description

一种三维避障的室内机器人导航方法和系统

技术领域

本发明属于计算机视觉领域，更具体地，涉及一种三维避障的室内机器人导航方法和系统。

背景技术

近年来，室内移动机器人因其广泛的用途受到研究者们越来越多的关注。室内移动机器人是一个环境感知、动态决策与规划、行为控制与执行等多种功能于一体的综合系统。在移动机器人导航过程中关键的一个问题是能否实时完成动态避障进而全局导航任务。目前比较流行的避障方法是利用激光雷达或者惯性测量原件与视觉传感器结合来完成这一任务，但是所需成本较高。随着相机的普及与计算机视觉理论的发展，使用基于纯视觉的方法来完成这一任务成为主流的研究方向。

传统的机器人导航中针对动态障碍物有三种主要的处理方法，一种是使用人工势场法，该方法假设环境中存在的障碍物对物体存在斥力，模仿力学方程求解出可行的无碰撞路径，但该方法一般在二维地图中使用。第二种方法是使用距离传感器，当检测到有障碍物出现时根据距离传感器的反馈控制机器人做出动作规避障碍物，然后检测当前路径相对于规划路径的偏移并校正。第三种方法则是使用一种动态的路径规划算法，该算法在每个时刻都根据当前环境和最新地图进行路径规划。对于我们的方法来说，第一种方法不适用于三维情况，第二种方法则需要附加的传感器，第三种方法计算资源消耗较大。且以上三种方法都不能对三维空间的碰撞进行检测，在拥挤的室内环境下很难成功进行导航。

一般首先使用静态地图进行路径检索并选取合适路径进行规划，但由于场景中可能存在移动的物体导致机器人需要进行动态避障的处理，

目前，机器人导航避障技术已引起机器人研究领域的广泛关注，成为移动机器人研究热点之一。但是由于室内场景越来越复杂导致在场景中很容易出现实时的障碍物，但是由于机器人计算资源的限制，使得机器人难以在导航的同时直接处理实时的障碍物，同时由于机器人需要随时处理出现的动态障碍物，在障碍物出现数量较多的时候难以保证信息获取的准确性。因此解决动态的机器人导航避障问题必须同时平衡导航的策略与障碍物检测。

发明内容

针对现有技术的以上缺陷或改进需求，本发明提供了一种三维避障的室内机器人导航方法和系统，其目的在于使得机器人即使在拥挤的室内条件下也能顺利规划出可行路径，并随着实时更新的实时三维地图进行动态避障，该方法在无人驾驶、室内定位导航等领域有广泛的应用前景。

为实现上述目的，按照本发明的一个方面，提供了一种三维避障的室内机器人导航方法，包括：

(1)采集机器人的三维模型、机器人行进的初始位置和终点位置，以及包含初始位置和终点位置的全局三维地图；

(2)根据初始位置、终点位置和全局三维地图进行全局路径规划，生成一系列全局路径点，在每一全局路径点处进行三维障碍物碰撞检测，若不会发生碰撞，则得到最优的全局路径规划，若会发生碰撞，则根据初始位置、终点位置和全局三维地图重新进行全局路径规划直至避开障碍物，得到最优的全局路径规划；

(3)机器人按照最优的全局路径规划行进，采集机器人的实时位置，拍摄包含实时位置和下一全局路径点的局部三维地图，利用局部三维地图更新全局三维地图得到实时三维地图；

(4)在行进过程中对下一全局路径点进行三维障碍物碰撞检测，若会发生碰撞，则以实时三维地图为全局三维地图，以机器人实时位置为初始位置，返回步骤(2)，若不会发生碰撞，则继续行进直至达到终点位置，实现了机器人的自主导航与三维避障。

进一步的，利用局部三维地图更新全局三维地图得到实时三维地图是指利用局部三维地图更新全局三维地图得到实时的全局三维地图。

进一步的，三维障碍物碰撞检测的具体实现方式为：

利用机器人的三维模型构建模型二叉树结构，利用全局三维地图构建全局二叉树结构，计算全局路径点处模型二叉树结构的球体和全局二叉树结构的球体之间的球心距离，若该距离大于等于模型二叉树结构和全局二叉树结构之间的距离，则三维模型和全局三维地图没有相交，不会发生碰撞，否则会发生碰撞。

更进一步的，进行三维障碍物碰撞检测之前所述方法还包括将机器人的三维模型划分成二维模型，对二维模型进行二维障碍物碰撞检测。

按照本发明的另一方面，提供了一种三维避障的室内机器人导航系统，包括：

第一模块，用于采集机器人的三维模型、机器人行进的初始位置和终点位置，以及包含初始位置和终点位置的全局三维地图；

第二模块，用于根据初始位置、终点位置和全局三维地图进行全局路径规划，生成一系列全局路径点，在每一全局路径点处进行三维障碍物碰撞检测，若不会发生碰撞，则得到最优的全局路径规划，若会发生碰撞，则根据初始位置、终点位置和全局三维地图重新进行全局路径规划直至避开障碍物，得到最优的全局路径规划；

第三模块，用于使机器人按照最优的全局路径规划行进，采集机器人的实时位置，拍摄包含实时位置和下一全局路径点的局部三维地图，利用局部三维地图更新全局三维地图得到实时三维地图；

第四模块，用于在行进过程中对下一全局路径点进行三维障碍物碰撞检测，若会发生碰撞，则以实时三维地图为全局三维地图，以机器人实时位置为初始位置，执行第二模块，若不会发生碰撞，则继续行进直至达到终点位置，实现了机器人的自主导航与三维避障。

进一步的，三维障碍物碰撞检测的具体实现方式为：

利用机器人的三维模型构建模型二叉树结构，利用全局三维地图构建全局二叉树结构，计算全局路径点处模型二叉树结构的球体和全局二叉树结构的球体之间的球心距离，若该距离大于等于模型二叉树结构和全局二叉树结构之间的距离，则三维模型和全局三维地图没有相交，不会发生碰撞，否则会发生碰撞。

更进一步的，进行三维障碍物碰撞检测之前所述方法还包括将机器人的三维模型划分成二维模型，对二维模型进行二维障碍物碰撞检测。

总体而言，通过本发明所构思的以上技术方案与现有技术相比，具有以下优点：

1、采集机器人的三维模型、初始位置和终点位置，全局三维地图，局部三维地图和机器人的实时位置，本发明所需要的数据形式单一、采集方便；

2、本发明在所有全局路径点处进行三维障碍物碰撞检测，得到最优的全局路径规划，即使在拥挤的室内环境下也能顺利求解出可行的机器人路径，完成了机器人从初始位置到终点位置的自主导航和三维避障任务。

附图说明

图1是一种三维避障的室内机器人导航方法的流程图；

图2是三维障碍物碰撞检测的流程图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

如图1所示，本发明提供的一种三维避障的室内机器人导航方法，包括以下步骤：

(1)采集机器人的三维模型、机器人行进的初始位置和终点位置，以及包含初始位置和终点位置的全局三维地图；

(2)根据初始位置、终点位置和全局三维地图进行全局路径规划，生成一系列全局路径点，在每一全局路径点处进行三维障碍物碰撞检测，若不会发生碰撞，则得到最优的全局路径规划，若会发生碰撞，则根据初始位置、终点位置和全局三维地图重新进行全局路径规划直至避开障碍物，得到最优的全局路径规划；

(3)机器人按照最优的全局路径规划行进，采集机器人的实时位置，拍摄包含实时位置和下一全局路径点的局部三维地图，利用局部三维地图更新全局三维地图得到实时三维地图；

(4)在行进过程中对下一全局路径点进行三维障碍物碰撞检测，若会发生碰撞，则以实时三维地图为全局三维地图，以机器人实时位置为初始位置，返回步骤(2)，若不会发生碰撞，则继续行进直至达到终点位置，实现了机器人的自主导航与三维避障。

优选地，使用RGB-D相机结合SLAM算法获取局部三维地图和机器人的实时位置。

优选的，在本发明的第一个实施例中，使用A*算法对机器人进行全局路径规划。

优选地，在本发明的第二个实施例中，使用ARA*算法机器人进行全局路径规划；ARA*算法是A*算法的改进版，与A*算法相比，它计算速度更快，所耗计算资源更少。

如图2所示，三维障碍物碰撞检测的具体实现方式为：

利用机器人的三维模型构建模型二叉树结构，利用全局三维地图构建全局二叉树结构，计算全局路径点处模型二叉树结构的球体和全局二叉树结构的球体之间的球心距离，若该距离大于等于模型二叉树结构和全局二叉树结构之间的距离，则三维模型和全局三维地图没有相交，不会发生碰撞，否则会发生碰撞。具体包括以下步骤：

(2-1)机器人在每一全局路径点处的三维模型表面由N个二维多边形组成，遍历机器人三维模型表面的N个多边形，对N个多边形使用启发式的算法得到N个基元球体；

(2-2)使用启发式的算法得到一个能将N个基元球体包围的大球体；

(2-3)使用宽度优先算法构建一个模型二叉树结构，使用大球体作为模型二叉树结构的根节点，父节点表示一个能将n个基元球体包围的父球体，子节点表示一个能将n/2个基元球体包围的子球体，最底层节点表示一个能将1个基元球体包围的最底层球体，n≤N；

(2-4)全局三维地图表面由N个全局二维多边形组成，遍历全局三维地图表面的N个全局多边形，对N个全局多边形使用启发式的算法得到N个全局基元球体；使用启发式的算法得到一个能将N个全局基元球体包围的全局大球体；

(2-5)使用宽度优先算法构建一个全局二叉树结构，使用全局大球体作为全局二叉树结构的全局根节点，全局父节点表示一个能将n个全局基元球体包围的全局父球体，全局子节点表示一个能将n/2个全局基元球体包围的全局子球体，全局最底层节点表示一个能将1个全局基元球体包围的全局最底层球体，n≤N；

(2-6)设置模型二叉树结构和全局二叉树结构之间的距离为无穷大，从模型二叉树结构和全局二叉树结构中分别选取节点；

(2-7)计算选取节点对应的球体之间的球心距离，若该距离大于等于模型二叉树结构和全局二叉树结构之间的距离，则三维模型和全局三维地图没有相交，机器人不会发生碰撞，流程结束。否则检查两个节点是否都是最底层节点，若是，则进入步骤(2-8)，若不是，则进入步骤(2-10)；

(2-8)检查选定两个节点代表的球体所包覆的多边形是否在三维空间相交，若是，则确认机器人和全局三维地图将发生碰撞，否则设置两个节点代表的球体所包覆的多边形之间的距离为机器人模型二叉树结构和全局二叉树结构之间的距离；

(2-9)从机器人模型二叉树结构和全局二叉树结构中未检索过的节点中各取一个节点并选定，返回步骤(2-7)。

(2-10)比较两个节点在模型二叉树结构和全局二叉树结构中的层级，若相同，则随机选取一个节点，否则选取层级较高的节点，选取节点的两个子节点并比较两个子节点与另一选定节点代表的球体之间的距离，选取距离较近的子节点代替其父节点，返回步骤(2-7)。

更进一步的，进行三维障碍物碰撞检测之前所述方法还包括将机器人的三维模型划分成二维模型，对二维模型进行二维障碍物碰撞检测。

按照本发明的另一方面，提供了一种三维避障的室内机器人导航系统，包括：

第一模块，用于采集机器人的三维模型、机器人行进的初始位置和终点位置，以及包含初始位置和终点位置的全局三维地图；

第二模块，用于根据初始位置、终点位置和全局三维地图进行全局路径规划，生成一系列全局路径点，在每一全局路径点处进行三维障碍物碰撞检测，若不会发生碰撞，则得到最优的全局路径规划，若会发生碰撞，则根据初始位置、终点位置和全局三维地图重新进行全局路径规划直至避开障碍物，得到最优的全局路径规划；

第三模块，用于使机器人按照最优的全局路径规划行进，采集机器人的实时位置，拍摄包含实时位置和下一全局路径点的局部三维地图，利用局部三维地图更新全局三维地图得到实时三维地图；

第四模块，用于在行进过程中对下一全局路径点进行三维障碍物碰撞检测，若会发生碰撞，则以实时三维地图为全局三维地图，以机器人实时位置为初始位置，执行第二模块，若不会发生碰撞，则继续行进直至达到终点位置，实现了机器人的自主导航与三维避障。

进一步的，三维障碍物碰撞检测的具体实现方式为：

利用机器人的三维模型构建模型二叉树结构，利用全局三维地图构建全局二叉树结构，计算全局路径点处模型二叉树结构的球体和全局二叉树结构的球体之间的球心距离，若该距离大于等于模型二叉树结构和全局二叉树结构之间的距离，则三维模型和全局三维地图没有相交，不会发生碰撞，否则会发生碰撞。

更进一步的，进行三维障碍物碰撞检测之前所述方法还包括将机器人的三维模型划分成二维模型，对二维模型进行二维障碍物碰撞检测。

本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (2)

1.一种三维避障的室内机器人导航方法，其特征在于，包括：

(1)采集机器人的三维模型、机器人行进的初始位置和终点位置，以及包含初始位置和终点位置的全局三维地图；

(2)根据初始位置、终点位置和全局三维地图进行全局路径规划，生成一系列全局路径点，在每一全局路径点处进行三维障碍物碰撞检测，若不会发生碰撞，则得到最优的全局路径规划，若会发生碰撞，则根据初始位置、终点位置和全局三维地图重新进行全局路径规划直至避开障碍物，得到最优的全局路径规划；

(3)机器人按照最优的全局路径规划行进，采集机器人的实时位置，拍摄包含实时位置和下一全局路径点的局部三维地图，利用局部三维地图更新全局三维地图得到实时三维地图；

(4)在行进过程中对下一全局路径点进行三维障碍物碰撞检测，若会发生碰撞，则以实时三维地图为全局三维地图，以机器人实时位置为初始位置，返回步骤(2)，若不会发生碰撞，则继续行进直至达到终点位置，实现了机器人的自主导航与三维避障；

所述三维障碍物碰撞检测的具体实现方式为：

(2-1)机器人在每一全局路径点处的三维模型表面由N个二维多边形组成，遍历机器人三维模型表面的N个多边形，对N个多边形使用启发式的算法得到N个基元球体；

(2-2)使用启发式的算法得到一个能将N个基元球体包围的大球体；

(2-3)使用宽度优先算法构建一个模型二叉树结构，使用大球体作为模型二叉树结构的根节点，父节点表示一个能将n个基元球体包围的父球体，子节点表示一个能将n/2个基元球体包围的子球体，最底层节点表示一个能将1个基元球体包围的最底层球体，n≤N；

(2-4)全局三维地图表面由N个全局二维多边形组成，遍历全局三维地图表面的N个全局多边形，对N个全局多边形使用启发式的算法得到N个全局基元球体；使用启发式的算法得到一个能将N个全局基元球体包围的全局大球体；

(2-5)使用宽度优先算法构建一个全局二叉树结构，使用全局大球体作为全局二叉树结构的全局根节点，全局父节点表示一个能将n个全局基元球体包围的全局父球体，全局子节点表示一个能将n/2个全局基元球体包围的全局子球体，全局最底层节点表示一个能将1个全局基元球体包围的全局最底层球体，n≤N；

(2-6)设置模型二叉树结构和全局二叉树结构之间的距离为无穷大，从模型二叉树结构和全局二叉树结构中分别选取节点；

(2-7)计算选取节点对应的球体之间的球心距离，若该距离大于等于模型二叉树结构和全局二叉树结构之间的距离，则三维模型和全局三维地图没有相交，机器人不会发生碰撞，流程结束，否则检查两个节点是否都是最底层节点，若是，则进入步骤(2-8)，若不是，则进入步骤(2-10)；

(2-8)检查选定两个节点代表的球体所包覆的多边形是否在三维空间相交，若是，则确认机器人和全局三维地图将发生碰撞，否则设置两个节点代表的球体所包覆的多边形之间的距离为机器人模型二叉树结构和全局二叉树结构之间的距离；

(2-9)从机器人模型二叉树结构和全局二叉树结构中未检索过的节点中各取一个节点并选定，返回步骤(2-7)；

(2-10)比较两个节点在模型二叉树结构和全局二叉树结构中的层级，若相同，则随机选取一个节点，否则选取层级较高的节点，选取节点的两个子节点并比较两个子节点与另一选定节点代表的球体之间的距离，选取距离较近的子节点代替其父节点，返回步骤(2-7)。

2.一种三维避障的室内机器人导航系统，其特征在于，包括：

第一模块，用于采集机器人的三维模型、机器人行进的初始位置和终点位置，以及包含初始位置和终点位置的全局三维地图；

第二模块，用于根据初始位置、终点位置和全局三维地图进行全局路径规划，生成一系列全局路径点，在每一全局路径点处进行三维障碍物碰撞检测，若不会发生碰撞，则得到最优的全局路径规划，若会发生碰撞，则根据初始位置、终点位置和全局三维地图重新进行全局路径规划直至避开障碍物，得到最优的全局路径规划；

第三模块，用于使机器人按照最优的全局路径规划行进，采集机器人的实时位置，拍摄包含实时位置和下一全局路径点的局部三维地图，利用局部三维地图更新全局三维地图得到实时三维地图；

第四模块，用于在行进过程中对下一全局路径点进行三维障碍物碰撞检测，若会发生碰撞，则以实时三维地图为全局三维地图，以机器人实时位置为初始位置，执行第二模块，若不会发生碰撞，则继续行进直至达到终点位置，实现了机器人的自主导航与三维避障；

所述三维障碍物碰撞检测的具体实现方式为：

(2-1)机器人在每一全局路径点处的三维模型表面由N个二维多边形组成，遍历机器人三维模型表面的N个多边形，对N个多边形使用启发式的算法得到N个基元球体；

(2-2)使用启发式的算法得到一个能将N个基元球体包围的大球体；

(2-3)使用宽度优先算法构建一个模型二叉树结构，使用大球体作为模型二叉树结构的根节点，父节点表示一个能将n个基元球体包围的父球体，子节点表示一个能将n/2个基元球体包围的子球体，最底层节点表示一个能将1个基元球体包围的最底层球体，n≤N；

(2-4)全局三维地图表面由N个全局二维多边形组成，遍历全局三维地图表面的N个全局多边形，对N个全局多边形使用启发式的算法得到N 个全局基元球体；使用启发式的算法得到一个能将N个全局基元球体包围的全局大球体；

(2-5)使用宽度优先算法构建一个全局二叉树结构，使用全局大球体作为全局二叉树结构的全局根节点，全局父节点表示一个能将n个全局基元球体包围的全局父球体，全局子节点表示一个能将n/2个全局基元球体包围的全局子球体，全局最底层节点表示一个能将1个全局基元球体包围的全局最底层球体，n≤N；

(2-6)设置模型二叉树结构和全局二叉树结构之间的距离为无穷大，从模型二叉树结构和全局二叉树结构中分别选取节点；

(2-7)计算选取节点对应的球体之间的球心距离，若该距离大于等于模型二叉树结构和全局二叉树结构之间的距离，则三维模型和全局三维地图没有相交，机器人不会发生碰撞，流程结束，否则检查两个节点是否都是最底层节点，若是，则进入步骤(2-8)，若不是，则进入步骤(2-10)；

(2-8)检查选定两个节点代表的球体所包覆的多边形是否在三维空间相交，若是，则确认机器人和全局三维地图将发生碰撞，否则设置两个节点代表的球体所包覆的多边形之间的距离为机器人模型二叉树结构和全局二叉树结构之间的距离；

(2-9)从机器人模型二叉树结构和全局二叉树结构中未检索过的节点中各取一个节点并选定，返回步骤(2-7)；

(2-10)比较两个节点在模型二叉树结构和全局二叉树结构中的层级，若相同，则随机选取一个节点，否则选取层级较高的节点，选取节点的两个子节点并比较两个子节点与另一选定节点代表的球体之间的距离，选取距离较近的子节点代替其父节点，返回步骤(2-7)。