CN110986953A - 路径规划方法、机器人及计算机可读存储介质 - Google Patents

Abstract

本发明涉及智能控制领域，公开了一种路径规划方法、机器人及计算机可读存储介质。路径规划方法，包括：构建自起始位置指向目标位置的约束向量；构建随机树；对随机树进行扩展，直至扩展至目标位置，连接根节点与目标位置形成规划路径；对随机树进行扩展，包括：获取随机树中的父节点，生成采样点，构建采样点指向目标位置的采样向量，构建父节点与采样点之间的运动路径；若小于预设阈值且不存在障碍物，则将采样点加入随机树。与现有技术相比，本发明实施方式所提供的路径规划方法、机器人及计算机可读存储介质具有减少机器人路径规划计算中的运算量、提升路径规划的效率的优点。

Description

路径规划方法、机器人及计算机可读存储介质

技术领域

本发明涉及智能控制领域，特别涉及一种路径规划方法、机器人及计算机可读存储介质。

背景技术

近年来，随着人工智能技术的进一步发展，自动控制技术应用越来越广泛，如自主型机器人、汽车自动驾驶等，其中、自主型机器人越来越成为科技界的研究热点。在机器人的自主控制中，无论是机器人的自主移动，还是机器人的手臂的控制，都需要对运动轨迹进行规划，并控制机器人和/或机器人手臂沿规划的运动轨迹移动，从而避免机器人与障碍物发生碰撞。现有技术中的机器人进行路径规划时，使用的规划算法主要是快速扩展随机树算法，通过对随机树的不断扩展，形成起始位置至目标位置的轨迹。

然而，本发明的发明人发现，现有技术中的快速扩展随机树算法在进行路径规划的过程中，运算量较大，路径规划的效率较低。

发明内容

本发明实施方式的目的在于提供一种路径规划方法、机器人及计算机可读存储介质，有效的减少路径规划计算中的运算量、提升路径规划的效率。

为解决上述技术问题，本发明的实施方式提供了一种路径规划方法，包含以下步骤：构建自起始位置指向目标位置的约束向量；以所述起始位置为根节点，构建随机树；对所述随机树进行扩展，直至所述随机树扩展至所述目标位置，连接所述根节点与所述目标位置之间的叶子节点、形成规划路径；其中，所述对所述随机树进行扩展，包括：获取所述随机树中的一个节点作为父节点，在所述父节点周围预设空间范围内随机生成采样点，构建所述采样点指向所述目标位置的采样向量，连接所述父节点与所述采样点、形成运动路径；判断所述采样向量与所述约束向量之间的向量夹角是否小于预设阈值、并判断所述运动路径上是否存在障碍物；若所述向量夹角小于预设阈值、且所述运动路径上不存在障碍物，则将所述采样点作为叶子节点加入所述随机树。

本发明的实施方式还提供了一种机器人，包括：路径规划装置，以及机械手臂；其中，所述路径规划装置包括：至少一个处理器；以及，与所述至少一个处理器通信连接的存储器；其中，所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行如前述的路径规划方法，以控制所述机械手臂的路径规划。

本发明的实施方式还提供了一种计算机可读存储介质，存储有计算机程序，所述计算机程序被处理器执行时实现如前述的路径规划方法。

本发明实施方式相对于现有技术而言，构建自起始位置指向目标位置的约束向量，在对随机树进行扩展的过程中，随机采集到采样点后，获取采样点指向目标位置的采样向量、并计算采样向量与约束向量的向量夹角，将向量夹角小于预设阈值、且运动路径上不存在障碍物的采样点作为叶子节点加入随机树中，从而有效的减少叶子节点的数量，减少路径规划过程中计算量，提升路径规划的效率。

另外，所述将所述采样点作为叶子节点加入所述随机树后，还包括：获取所述叶子节点与所述目标位置之间的目标路径；判断所述目标路径上是否存在障碍物；若所述目标路径上不存在障碍物，则将所述目标位置加入所述随机树。

另外，所述连接所述根节点与所述目标位置之间的叶子节点、形成规划路径后，还包括：获取所述规划路径上依次相连的第一节点、第二节点和第三节点；获取所述第一节点与所述第三节点之间的路径作为优化路径；若所述优化路径上没有障碍物，则从所述规划路径中去除所述第二节点，连接所述第一节点和所述第三节点、形成新的规划路径。连接第一节点和第三节点作为优化路径，若优化路径上没有障碍物，则去除第一节点和第三节点之间的第二节点，从而减少目标路径中的转折点，有效的对目标路径进行优化。

另外，应用于可移动装置；所述判断所述运动路径上是否存在障碍物，包括：模拟所述可移动装置沿所述运动路径运动，并与所述障碍物进行碰撞检测；若在所述碰撞检测中检测到所述可移动装置不与所述障碍物发生碰撞，则判定所述运动路径上不存在障碍物。模拟可移动装置沿运动路径运动，并在模拟运动的过程中进行碰撞检测，根据碰撞检测的结果判定运动路径上是否存在障碍物，增加运动路径的可靠性。

另外，所述模拟所述可移动装置沿所述运动路径运动前，还包括：构建包覆所述可移动装置的包络模型；所述模拟所述可移动装置沿所述运动路径运动，并与所述障碍物进行碰撞检测，具体为：模拟所述包络模型沿所述运动路径运动，并根据所述包络模型与所述障碍物进行碰撞检测。构建包覆可移动装置的包络模型，根据包络模型与障碍物进行碰撞检测，可以有效的减少可移动装置的不规则形状对碰撞检测计算过程的影响，提升运算效率。

另外，所述可移动装置为机械手臂，所述机械手臂包括多个运动关节；所述构建包覆所述可移动装置的包络模型，包括：根据预设比例放大各个所述运动关节，形成所述包络模型。

另外，所述与所述障碍物进行碰撞检测前，还包括：采集周围环境的深度数据；根据所述深度数据，获取所述障碍物的点云数据。

另外，所述获取所述障碍物的点云数据后，还包括：对所述点云数据进行聚类处理；根据聚类处理后的所述点云数据，生成碰撞检测库；根据所述碰撞检测库进行所述碰撞检测。

附图说明

图1是本发明第一实施方式所提供的路径规划方法的程序流程图；

图2是本发明第一实施方式所提供的路径规划方法所规划形成的路径示意图；

图3是本发明第一实施方式所提供的路径规划方法中对随机树进行扩展的程序流程图；

图4是本发明第二实施方式所提供的路径规划方法的程序流程图；

图5是本发明第二实施方式所提供的路径规划方法中形成新的规划路径的示意图；

图6是本发明第三实施方式所提供的机器人的结构示意图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明的各实施方式进行详细的阐述。然而，本领域的普通技术人员可以理解，在本发明各实施方式中，为了使读者更好地理解本申请而提出了许多技术细节。但是，即使没有这些技术细节和基于以下各实施方式的种种变化和修改，也可以实现本申请所要求保护的技术方案。

本发明的第一实施方式涉及一种路径规划方法，具体步骤如图1所示，包括以下步骤：

步骤S101：构建自起始位置指向目标位置的约束向量。

具体的，在本步骤中，确定起始位置和目标位置在同一空间坐标系中的坐标，根据起始位置的坐标和目标位置的坐标，计算得到自起始位置指向目标位置的约束向量。

步骤S102：以起始位置为根节点，构建随机树。

具体的，如图2所示，201和202均为障碍物，1号位置为起始位置，以1号位置为根节点，构建随机树。

步骤S103：对随机树进行扩展。

具体的，在本步骤中，对随机树进行扩展的具体步骤如图3所示，包括：

步骤S201：获取随机树中的一个节点作为父节点、并随机生成采样点。

具体的，如图2所示，以1号节点作为父节点，在1号节点周围随机生成多个采样点2。可理解的是，在本步骤中，采样点的个数可以是一个，也可以是多个，具体可以根据实际需要进行灵活的设定。

优选的，在本实施方式中，采样点与父节点的距离小于或等于预设距离，即在父节点周围的一定范围内生成采样点。在父节点周围的预设空间范围内生成采样点，可以避免采样点与父节点的距离过远，提升采样点的有效率，提升随机树的扩展效率。

步骤S202：构建采样点指向目标位置的采样向量、并构建父节点与采样点之间的运动路径。

具体的，在本步骤中，生成采样点后，获取采样点在空间坐标系中的坐标，从而计算采样点指向目标位置的采样向量。

进一步的，如图2所示，直接构建父节点(1号节点)与采样点(多个2号节点)之间的直线段作为运动路径。

步骤S203：判断采样向量与约束向量之间的向量夹角是否小于预设阈值，若是，则执行步骤S204，若否，则执行步骤S201。

具体的，在本实施方式中，预设阈值为90°。可以理解的是，预设阈值为90°仅为本实施方式中的一种具体的举例说明，并不构成限定，在本发明的其它实施方式中，预设阈值也可以是60°、120°等任意角度，具体可以根据实际需要进行灵活的设定。

需要说明的是，上述步骤S203仅为本实施方式中的一种具体的举例说明，并不构成限定，在本发明的其它实施方式中，也可以是计算采样向量与约束向量的方向一致性比例。例如，分别计算采样向量和约束向量与X轴的夹角角度，计算两个夹角角度的比值，得到方向一致性比例。判断方向一致性比例是否处于预设约束比例范围，如大于0且小于1，若是，则执行步骤S204，若否，则执行步骤S201。

步骤S204：判断父节点与采样点之间的运动路径上是否存在障碍物，若否，则执行步骤S205，若是，则执行步骤S201。

如图2所示，针对每个2号节点，其与1号节点之间均存在运动路径，部分运动路径上存在障碍物(如，障碍物201和202)，部分运动路径上不存在障碍物，当运动路径上不存在障碍物时，执行步骤S205，当运动路径上存在障碍物时，执行步骤S201。

具体的，在本步骤中，首先利用实感摄像头获取运动路径所在区域内空间的深度图像，并从深度图像中获取障碍物位置。构建空间障碍物模型，然后模拟可移动装置自父节点运动至采样点的运动过程。在模拟的运动过程中，对可移动装置与障碍物进行碰撞检测，若在碰撞检测中检测到可移动装置不与障碍物发生碰撞，则判定运动路径上不存在障碍物。可以理解的是，利用深度图像获取障碍物位置仅为本实施方式中的一种具体的举例说明，并不构成限定，在实际应用过程中，还可以是利用红外雷达、声波雷达等装置获取障碍物的位置，在此不进行一一列举，具体可以根据实际需要进行灵活的设定。

优选的，在本实施方式中，在模拟可移动装置自父节点运动至采样点的运动过程前，构建包覆可移动装置的包络模型，即可移动装置整体位于包络模型内部。模拟可移动装置自父节点运动至采样点的运动过程具体为，模拟包络模型沿运动路径运动，并根据包络模型与障碍物进行碰撞检测。构建包络模型包覆可移动装置，可以有效的简化碰撞检测的计算过程，提升碰撞检测的效率。

进一步的，在本实施方式中，根据预设比例放大可移动装置、以形成包络模型。例如，在本实施方式中，可移动装置为机械手臂，机械手臂包括多个运动关节，根据预设比例放大各个运动关节，形成包络模型。可以理解的是，上述仅为本实施方式中构建包络模型的一种具体方法的举例说明，并不构成限定，在本发明的其它实施方式中，也可以是其它方法，例如构建可移动装置的最小外接球作为包络模型等方法，在此不进行一一列举，具体可以根据实际需要进行灵活的设定。

进一步的，在本实施方式中，采集周围环境的深度数据后；根据深度数据，获取周围环境中的障碍物的点云数据，根据点云数据构建空间障碍物模型。

优选的，在本步骤中，根据点云数据构建空间障碍物模型前，还对点云数据进行聚类处理。完成聚类处理之后再依据聚类处理后的点云数据生成实时的碰撞检测库，根据彭转检测库进行碰撞检测。

步骤S205：将采样点作为叶子节点加入随机树。

步骤S104：判断随机树是否扩展至目标位置，若是，则执行步骤S105，若否，则执行步骤S103，继续对随机树进行扩展。

具体的，在本步骤中，将采样点作为叶子节点加入随机树后，获取采样点与目标位置之间的目标路径，并判断目标路径上是否存在障碍物，若目标路径上不存在障碍物，则将目标位置加入所述随机树，执行步骤S105，若目标路径上存在障碍物，则执行步骤S103。如图2所示，在图2中，8号节点为目标位置，当扩展到7号节点时，获取7号节点和目标位置(8号节点)之间的路径作为目标路径，判断目标路径上是否存在障碍物，在图2中，由于7号节点和8号节点之间的路径上不再存在障碍物，则执行步骤S105，反之，当扩展到6号节点时，由于6号节点与8号节点之间的路径上存在障碍物，则执行步骤S103。可以理解的是，上述仅为本实施方式中的一种具体的举例说明，并不构成限定，在本发明的其它实施方式中，也可以是设置目标位置周围、没有障碍物的区域作为目标区域，当叶子节点位于目标区域内时，则判定随机树扩展至目标位置，执行步骤S105，否则，执行步骤S103。或者也可以是其它方法，在此不进行一一列举，具体可以根据实际情况进行灵活的应用。

步骤S105：连接根节点与目标位置之间的叶子节点、形成规划路径。

与现有技术相比，本发明第一实施方式所提供的路径规划方法中，在将随机采集的采样点加入随机树前，根据自起始位置指向目标位置的约束向量对采样点进行判定，当采样向量与约束向量之间的向量夹角小于预设阈值时，才将采样点加入随机树中，从而减少随机树中的节点个数，有效的减少路径规划过程中的运算量，提升路径规划的效率。

本发明的第二实施方式涉及一种路径规划方法。第二实施方式是第一实施方式的进一步改进，具体步骤如图4所示，包括：

步骤S301：构建自起始位置指向目标位置的约束向量。

步骤S302：以起始位置为根节点，构建随机树。

步骤S303：对随机树进行扩展，并判断随机树是否扩展至目标位置，若是，则执行步骤S304，若否，则再次执行步骤S303。

步骤S304：判断随机树是否扩展至目标位置，若是，则执行步骤S305，若否，则执行步骤S303。

步骤S305：连接根节点与目标位置之间的叶子节点、形成规划路径。

可以理解的是，本实施方式中的步骤S201至步骤S205与第一实施方式中的步骤S101至步骤S105大致相同，具体可以参照第一实施方式的具体介绍，在此不再赘述。

步骤S306：获取规划路径上依次相连的第一节点、第二节点和第三节点，并获取第一节点与第三节点之间的路径作为优化路径。

可以理解的是，第一节点、第二节点和第三节点依次相连仅为本实施方式中的一种具体的举例说明，并不构成限定，在本发明的其它实施方式中，也可以是第一节点和第三节点之间包括多个节点，具体可以根据实际需要进行灵活的设定，在此不进行一一列举。

步骤S307：判断优化路径上是否有障碍物，若是，则执行步骤S306，若否，则执行步骤S308。

具体的，判断优化路径上是否有障碍物的具体方法与第一实施方式中判断运动路径上是否有障碍物的方法基本相同，具体可以参照第一实施方式的具体介绍，在此不再赘述。

步骤S308：从规划路径中去除第二节点，连接第一节点和第三节点、形成新的规划路径。

在本步骤中，如图5所示，501为障碍物，9号节点、10号节点和11号节点均为规划路径中的中间节点，设第一节点为9号节点，第二节点为10号节点，第三节点为11号节点，当9号节点和11号节点之间的路径上不存在障碍物501时，去除10号节点，直接连接9号节点和11号节点(图中虚线所示)，形成新的规划路径。

与现有技术相比，本发明第二实施方式所提供的路径规划方法在保留第一实施方式的全部技术效果的同时，减少规划路径中采样点的数量，有效的对规划路径进行优化，提升规划路径的质量。

上面各种方法的步骤划分，只是为了描述清楚，实现时可以合并为一个步骤或者对某些步骤进行拆分，分解为多个步骤，只要包含相同的逻辑关系，都在本专利的保护范围内；对算法中或者流程中添加无关紧要的修改或者引入无关紧要的设计，但不改变其算法和流程的核心设计都在该专利的保护范围内。

本发明第四实施方式涉及一种机器人，如图6所示，包括：路径规划装置601和机械手臂602，其中，路径规划装置601包括：至少一个处理器6011；以及，与至少一个处理器6011通信连接的存储器6012；其中，存储器6012存储有可被至少一个处理器6011执行的指令，指令被至少一个处理器6011执行，以使至少一个处理器6011能够执行如上述路径规划方法。

其中，存储器6012和处理器6011采用总线方式连接，总线可以包括任意数量的互联的总线和桥，总线将一个或多个处理器6011和存储器6012的各种电路连接在一起。总线还可以将诸如外围设备、稳压器和功率管理电路等之类的各种其他电路连接在一起，这些都是本领域所公知的，因此，本文不再对其进行进一步描述。总线接口在总线和收发机之间提供接口。收发机可以是一个元件，也可以是多个元件，比如多个接收器和发送器，提供用于在传输介质上与各种其他装置通信的单元。经处理器6011处理的数据通过天线在无线介质上进行传输，进一步，天线还接收数据并将数据传送给处理器6011。

处理器6011负责管理总线和通常的处理，还可以提供各种功能，包括定时，外围接口，电压调节、电源管理以及其他控制功能。而存储器6012可以被用于存储处理器6011在执行操作时所使用的数据。

本发明第五实施方式涉及一种计算机可读存储介质，存储有计算机程序。计算机程序被处理器执行时实现上述方法实施例。

即，本领域技术人员可以理解，实现上述实施例方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，该程序存储在一个存储介质中，包括若干指令用以使得一个设备(可以是单片机，芯片等)或处理器(processor)执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-OnlyMemory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。

本领域的普通技术人员可以理解，上述各实施方式是实现本发明的具体实施例，而在实际应用中，可以在形式上和细节上对其作各种改变，而不偏离本发明的精神和范围。

Claims (10)

1.一种路径规划方法，其特征在于，包括：

构建自起始位置指向目标位置的约束向量；

以所述起始位置为根节点，构建随机树；

对所述随机树进行扩展，直至所述随机树扩展至所述目标位置；

连接所述根节点与所述目标位置之间的叶子节点、形成规划路径；

其中，所述对所述随机树进行扩展，包括：

获取所述随机树中的一个节点作为父节点，在所述父节点周围预设空间范围内随机生成采样点，构建所述采样点指向所述目标位置的采样向量，连接所述父节点与所述采样点、形成运动路径；

判断所述采样向量与所述约束向量之间的向量夹角是否小于预设阈值、并判断所述运动路径上是否存在障碍物；

若所述向量夹角小于预设阈值、且所述运动路径上不存在障碍物，则将所述采样点作为叶子节点加入所述随机树。

2.根据权利要求1所述的路径规划方法，其特征在于，所述将所述采样点作为叶子节点加入所述随机树后，还包括：

获取所述叶子节点与所述目标位置之间的目标路径；

判断所述目标路径上是否存在障碍物；

若所述目标路径上不存在障碍物，则将所述目标位置加入所述随机树。

3.根据权利要求1所述的路径规划方法，其特征在于，所述连接所述根节点与所述目标位置之间的叶子节点、形成规划路径后，还包括：

获取所述规划路径上依次相连的第一节点、第二节点和第三节点；

获取所述第一节点与所述第三节点之间的路径作为优化路径；

若所述优化路径上没有障碍物，则从所述规划路径中去除所述第二节点，连接所述第一节点和所述第三节点、形成新的规划路径。

4.根据权利要求1所述的路径规划方法，其特征在于，应用于可移动装置；

所述判断所述运动路径上是否存在障碍物，包括：

模拟所述可移动装置沿所述运动路径运动，并与所述障碍物进行碰撞检测；

若在所述碰撞检测中检测到所述可移动装置不与所述障碍物发生碰撞，则判定所述运动路径上不存在障碍物。

5.根据权利要求4所述的路径规划方法，其特征在于，所述模拟所述可移动装置沿所述运动路径运动前，还包括：

构建包覆所述可移动装置的包络模型；

所述模拟所述可移动装置沿所述运动路径运动，并与所述障碍物进行碰撞检测，包括：

模拟所述包络模型沿所述运动路径运动，并根据所述包络模型与所述障碍物进行碰撞检测。

6.根据权利要求5所述的路径规划方法，其特征在于，所述可移动装置包括机械手臂，所述机械手臂包括多个运动关节；

所述构建包覆所述可移动装置的包络模型，包括：

根据预设比例放大各个所述运动关节，形成所述包络模型。

7.根据权利要求1所述的路径规划方法，其特征在于，所述与所述障碍物进行碰撞检测前，还包括：

采集周围环境的深度数据；

根据所述深度数据，获取所述障碍物的点云数据。

8.根据权利要求7所述的路径规划方法，其特征在于，所述获取所述障碍物的点云数据后，还包括：

对所述点云数据进行聚类处理；

根据聚类处理后的所述点云数据，生成碰撞检测库；

根据所述碰撞检测库进行所述碰撞检测。

9.一种机器人，包括：

路径规划装置；以及

机械手臂；

其中，所述路径规划装置包括：至少一个处理器；以及，

与所述至少一个处理器通信连接的存储器；其中，

所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行如权利要求1至8中任一所述的路径规划方法，以控制所述机械手臂的路径规划。

10.一种计算机可读存储介质，存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1至8中任一项所述的路径规划方法。

Images