CN114115239A - 一种机器人路径规划方法、系统、设备及介质 - Google Patents

Abstract

本发明提供一种机器人路径规划方法、系统、设备及介质，该方法通过在当前位置与目标位置之间的基准连线上确定第一途经位置，获取当前位置的第一采样位置和第二采样位置，并确定第二途径位置，若第一途经位置和第二途径位置中至少存在一个满足预设碰撞条件，则将满足预设碰撞条件的一个位置作为下一位置，重复进行当前位置的确定，直到最新得到的当前位置与目标位置之间的当前距离小于预设距离阈值，完成各当前位置的确定，根据目标位置、各当前位置以及起始位置确定规划路径，在机器人路径规划时综合考虑第一采样位置和第二采样位置确定的第二途径位置，以及目标方向的第一途经位置来确定当前位置，提升目标搜索性，减少了机器人路径规划用时和冗余节点，效率高。

Description

一种机器人路径规划方法、系统、设备及介质

技术领域

本发明涉及路径规划技术领域，特别是涉及一种机器人路径规划方法、系统、设备及介质。

背景技术

机器人的运动规划是机器人研究领域的重要问题，通过在预设的初始位置和需要达到的目标位置之间，为该机器人规划得到一条没有障碍物的路径，以实现路径规划。

全局路径规划是结合已有的地图信息进行路径搜索的方式，其中RRT(Rapidly-exploring Random Trees，基于快速扩展随机树)算法是一种较为常用的路径的搜索方式，采用完全随机搜索的方式，路径搜索的方向随机，时间不定。有时采用该方法所生成的路径也并不是最优路径，可能存在“绕路”的风险，且该算法的目标搜索性差，耗时较多，存于较多的冗余节点，效率低。

发明内容

鉴于以上所述现有技术的缺点，本发明的目的在于提供一种机器人路径规划方法、系统、设备及介质，用于解决对于RRT算法目标搜索性差、时间长、冗余节点多、效率低的技术问题。

针对于上述问题，本发明提供了一种机器人路径规划方法，所述方法包括：

获取起始位置和目标位置；

在所述起始位置和目标位置之间确定若干个当前位置，并根据所述起始位置、各当前位置和目标位置确定规划路径；

所述当前位置的确定方式包括，

将所述起始位置作为当前位置，获取位于基准连线上的第一途经位置，所述基准连线为所述当前位置和目标位置之间的连线；

获取所述当前位置的第一采样位置和第二采样位置，并确定第一参考距离和第二参考距离，所述第一采样位置和第二采样位置分别位于基准连线的两侧，所述第一参考距离为所述第一采样位置与目标位置之间的距离，所述第二参考距离为所述第二采样位置与目标位置之间的距离，所述第一参考距离和所述第二参考距离均小于基准距离，所述基准距离为所述起始位置和目标位置之间的距离；

根据所述第一参考距离和第二参考距离从所述第一采样位置和第二采样位置中确定优选位置，并获取位于优选连线上的第二途经位置，所述优选连线为所述优选位置与所述当前位置之间的连线；

若目标途径位置满足预设碰撞条件，将所述目标途径位置作为所述当前位置对应的下一位置，所述目标途径位置包括所述第一途经位置或第二途经位置；

重复进行所述当前位置的确定，直到所述当前位置与目标位置之间的当前距离小于预设距离阈值，完成各所述当前位置的确定。

可选的，所述方法还包括以下任意之一：

若所述第一途经位置和第二途经位置均不满足预设碰撞条件，重新获取新的所述第一采样位置和第二采样位置，并确定新的所述第二途经位置，直到所述第二途径位置满足所述预设碰撞条件，将所述第二途径位置作为所述当前位置对应的下一位置；

若所述第一途经位置和第二途经位置均满足预设碰撞条件，将所述第一途经位置作为所述下一位置。

可选的，所述方法还包括：

若所述第一途经位置和第二途经位置均不满足预设碰撞条件，将所述优选位置在所述基准连线的一侧作为优选区域；

在所述优选区域确定新的优选采样位置，并确定新的所述第二途经位置；

直到所述第二途径位置满足所述预设碰撞条件，将所述第二途径位置作为所述当前位置对应的下一位置。

可选的，所述方法还包括：

获取预设目标偏置函数；

若所述预设目标偏置函数小于预设函数阈值，则根据所述当前位置的当前位置信息和预设目标偏置函数生成所述第一采样位置和第二采样位置；

若所述预设目标偏置函数大于或等于预设函数阈值，则生成所述第一途经位置。

可选的，所述第一途经位置的确定方式包括，分别获取当前位置信息和目标位置信息，并根据第一预设步长因子、所述当前位置信息和目标位置信息确定所述第一途经位置；

和/或，

所述第二途经位置的确定方式包括，分别获取当前位置信息和优选位置信息，并根据第二预设步长因子、所述当前位置信息和优选位置信息确定所述第二途经位置。

可选的，所述根据所述第一参考距离和第二参考距离从所述第一采样位置和第二采样位置中确定优选位置包括以下任意之一：

若所述第一参考距离小于第二参考距离，将所述第一采样位置作为优选位置；

若所述第二参考距离小于第一参考距离，将所述第二采样位置作为优选位置；

若所述第二参考距离等于第一参考距离，将所述第一采样位置和/或所述第二采样位置作为优选位置。

可选的，所述预设碰撞条件包括：

获取若干个障碍位置，以所述当前位置为起点，目标途径位置为终点，随机生成若干个检测位置，所述当前位置、目标途径位置和各检测位置生成的检测路径中不包括任一所述障碍位置。

本发明还提供了一种机器人路径规划系统，所述系统包括：

获取模块，用于获取起始位置和目标位置；

规划模块，用于在所述起始位置和目标位置之间确定若干个当前位置，并根据所述起始位置、各当前位置和目标位置确定规划路径；

所述当前位置的确定方式包括，

将所述起始位置作为当前位置，获取位于基准连线上的第一途经位置，所述基准连线为所述当前位置和目标位置之间的连线；

获取所述当前位置的第一采样位置和第二采样位置，并确定第一参考距离和第二参考距离，所述第一采样位置和第二采样位置分别位于基准连线的两侧，所述第一参考距离为所述第一采样位置与目标位置之间的距离，所述第二参考距离为所述第二采样位置与目标位置之间的距离，所述第一参考距离和所述第二参考距离均小于基准距离，所述基准距离为所述起始位置和目标位置之间的距离；

根据所述第一参考距离和第二参考距离从所述第一采样位置和第二采样位置中确定优选位置，并获取位于优选连线上的第二途经位置，所述优选连线为所述优选位置与所述当前位置之间的连线；

若目标途径位置满足预设碰撞条件，将所述目标途径位置作为所述当前位置对应的下一位置，所述目标途径位置包括所述第一途经位置或第二途经位置；

重复进行所述当前位置的确定，直到所述当前位置与目标位置之间的当前距离小于预设距离阈值，完成各所述当前位置的确定。

本发明还提供了一种电子设备，包括处理器、存储器和通信总线；

所述通信总线用于将所述处理器和存储器连接；

所述处理器用于执行所述存储器中存储的计算机程序，以实现如上述实施例中任一项所述的方法。

本发明还提供了一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序用于使所述计算机执行如上述实施例中任一项所述的方法。

如上所述，本发明提供的一种机器人路径规划方法、系统、设备及介质，具有以下有益效果：

本实施例提供了一种机器人路径规划方法，通过在当前位置与目标位置之间的基准连线上确定第一途经位置，获取当前位置的第一采样位置和第二采样位置，并确定第二途径位置，若第一途经位置和第二途径位置中至少存在一个满足预设碰撞条件，则将满足预设碰撞条件的一个位置作为下一位置，重复进行当前位置的确定，直到最新得到的当前位置与目标位置之间的当前距离小于预设距离阈值，完成各当前位置的确定，根据目标位置、各当前位置以及起始位置确定规划路径，在机器人路径规划时综合考虑第一采样位置和第二采样位置确定的第二途径位置，以及目标方向的第一途经位置来确定当前位置，提升了目标搜索性，减少了机器人路径规划的用时和冗余节点，效率更高。

附图说明

图1为本发明实施例一提供的机器人路径规划方法的一种流程示意图。

图2为本发明实施例一提供的当前位置确定方式的一种流程示意图。

图3为本发明实施例一提供的机器人路径规划方法的一种具体的流程示意图。

图4为本发明实施例一提供的封闭的四边形的结构示意图。

图5为本发明实施例一提供的在20次实验中，传统RRT算法每次实验运行的时间与本实施例提供的机器人路径规划方法所运行的时间的对比示意图。

图6为本发明实施例一提供的在20次实验中，传统RRT算法每次实验得到的节点数与实施例提供的机器人路径规划方法每次试验得到的节点数对比示意图。

图7为本发明实施例一提供的传统RRT算法的仿真实验的一种示意图。

图8为本发明实施例一提供的本实施例提供的机器人路径规划方法的仿真实验的一种示意图。

图9为本发明实施例一提供的传统RRT算法在增加障碍物以后的仿真实验的一种示意图。

图10为本发明实施例一提供的本实施例提供的机器人路径规划方法在增加障碍物以后的仿真实验的一种示意图。

图11为本发明实施例二提供的路线规划系统的一种结构示意图；

图12为一实施例提供的终端的一种结构示意图。

具体实施方式

以下通过特定的具体实例说明本发明的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本发明的其他优点与功效。本发明还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本发明的精神下进行各种修饰或改变。需说明的是，在不冲突的情况下，以下实施例及实施例中的特征可以相互组合。

需要说明的是，以下实施例中所提供的图示仅以示意方式说明本发明的基本构想，遂图式中仅显示与本发明中有关的组件而非按照实际实施时的组件数目、形状及尺寸绘制，其实际实施时各组件的型态、数量及比例可为一种随意的改变，且其组件布局型态也可能更为复杂。

实施例一

请参阅图1，本发明实施例提供的机器人路径规划方法，该方法包括：

S101：获取起始位置和目标位置。

起始位置可以是当前机器人或其他目标对象的出发点，目标位置可以是机器人或其他目标对象的目的地。

可选的，起始位置之前可能已经存在一定长度的路径，此时可以在已知路径上寻找与目标位置最接近的位置，并作为起始位置(当前位置)。

可选的，该起始位置和目标位置均为预设地图上的两个点。

S102：在起始位置和目标位置之间确定若干个当前位置，并根据起始位置、各当前位置和目标位置确定规划路径。

规划路径可以是自目标位置起，反向将各个当前位置进行连接，再与初始位置进行连接，并将得到的自起始位置到目标位置的路径，作为规划路径。

可选的，得到规划路径之后，该方法还包括：

获取预设地图的各障碍物的障碍位置信息，将各障碍位置分别与规划路径进行比对，若规划路径不包括障碍位置信息，则规划成功。否则，以与起始位置最近的障碍物周围的任意一个点为当前位置，重新进行机器人路径规划。

其中，参见图2，当前位置的确定方式包括，

S1021：将起始位置作为当前位置，获取位于基准连线上的第一途经位置。

其中，基准连线为当前位置和目标位置之间的连线，该基准连线为线段，故第一途经位置与目标位置的距离A小于起始位置与目标位置的基准距离B。

可选的，本实施例中的各种距离可以通过欧式距离等方式确定。

在一些实施例中，第一途经位置的确定方式包括：

分别获取当前位置信息和目标位置信息，并根据第一预设步长因子、当前位置信息和目标位置信息确定第一途经位置。

其中，当前位置信息为当前位置的位置信息。目标位置信息为目标位置的位置信息。该当前位置信息、目标位置信息以及本实施例中的其他位置信息均归一于某一预设坐标系，可以通过坐标位置加以标识。或者当前位置信息、目标位置信息以及本实施例中的其他位置信息也可以通过位姿来进行标识。

可选的，第一预设步长因子大于0且小于1，第一预设步长因子可以由本领域技术人员根据需要进行设定。

以当前位置信息为(a₀,b₀)，目标位置信息为(a₁,b₁)，第一预设步长因子Q₁为例，第一途经位置M₁的一种可选的确定方式包括：

其中a₀为当前位置的横坐标，b₀为当前位置的纵坐标，a₁为当前位置的横坐标，b₁为当前位置的纵坐标，Q₁为第一预设步长因子，M₁为第一途经位置的第一途经位置信息。

需要说明的是，上述仅是以二维坐标系为例示例性的说明第一途经位置的一种确定方式，对于多维表示的方式中第一途经位置的确定方式与上述方式类似，在此不再赘述。

在一些实施例中，第一途经位置为以当前位置为起点，沿当前位置向目标位置移动第一距离，其中第一距离根据第一预设步长因子和基准距离确定。

在一些实施例中，第一途经位置可以通过获取预设目标偏置函数，若该预设目标偏置函数小于预设函数阈值，则以当前位置为起点，沿当前位置向目标位置移动一定距离，得到第一途经位置。其中，预设目标偏置函数可以为Y＝rand(1)，预设函数阈值可以为0.5。

S1022：获取当前位置的第一采样位置和第二采样位置，并确定第一参考距离和第二参考距离。

其中，第一采样位置和第二采样位置分别位于基准连线的两侧，第一参考距离为第一采样位置与目标位置之间的距离，第二参考距离为第二采样位置与目标位置之间的距离，第一参考距离和第二参考距离均小于基准距离，基准距离为起始位置和目标位置之间的距离。

在一些实施例中，若规划的路径为二维路径，此时，可以将第一采样位置和第二采样位置分别位于基准连线的两侧理解为，第一采样位置在基准连线的一侧，第二采样位置在基础连线的另一侧。

在一些实施例中，若规划路径为三维路径，此时，基于多个维度分别获取第一采样位置和第二采样位置，并确定对应的若干组第一参考距离和第二参考距离，在此不再赘述。

在一些实施例中，第一采样位置和第二采样位置为随机在基准连线的两侧所生成的两个采样点。其中，该第一采样位置和第二采样位置的确定可以通过天牛须算法来实现，以当前位置作为天牛质心，以第一采样位置和第二采样位置分别作为天牛左须和天牛右须。

在一些实施例中，第一采样位置和第二采样位置可以通过获取预设目标偏置函数，若预设目标偏置函数小于预设函数阈值，则根据当前位置的当前位置信息和预设目标偏置函数生成第一采样位置和第二采样位置。其中，预设目标偏置函数可以为Y＝rand(1)，预设函数阈值可以为0.5。此时，第一采样位置和第二采样位置可以通过RRT算法来确定。

在一些实施例中，以第一采样位置的位置信息为(r₁，r₂)，第二采样位置的位置信息为(r₃，r₄)，目标位置的目标位置信息为(a₁,b₁)，当前位置的当前位置信息为(a₀,b₀)为例，第一参考距离L₁、第二参考距离L₂和基准距离L₀的确定方式包括：

其中，a₀为当前位置的横坐标，b₀为当前位置的纵坐标，a₁为当前位置的横坐标，b₁为当前位置的纵坐标，r₁为第一采样位置的横坐标，r₂为第一采样位置的纵坐标，r₃为第二采样位置的横坐标，r₄为第二采样位置的纵坐标，L₀为基准距离，L₁为第一参考距离，L₂为第二参考距离。

需要说明的是，上述仅是以二维坐标系为例示例性的说明第一途经位置的一种确定方式，对于多维表示的方式中第一途经位置的确定方式与上述方式类似，在此不再赘述。

S1023：根据第一参考距离和第二参考距离从第一采样位置和第二采样位置中确定优选位置，并获取位于优选连线上的第二途经位置。

其中，优选连线为优选位置与当前位置之间的连线，该优选连线为优选位置与当前位置之间的线段。

在一些实施例中，根据第一参考距离和第二参考距离从第一采样位置和第二采样位置中确定优选位置包括以下任意之一：

若第一参考距离小于第二参考距离，将第一采样位置作为优选位置；

若第二参考距离小于第一参考距离，将第二采样位置作为优选位置；

若第二参考距离等于第一参考距离，将第一采样位置和/或第二采样位置作为优选位置。

换句话说，优选位置为第一采样位置和第二采样位置中距离目标位置更近的一个位置。但当第一采样位置和第二采样位置同样近时，可以选择任意一个作为优选位置，也可以将两个均作为优选位置，待后续进一步进行判断。

可选的，若第二参考距离等于第一参考距离，也可以重新进行第一采样位置和第二采样位置的获取。

在一些实施例中，第二途径位置的确定方式包括：

分别获取当前位置信息和优选位置信息，并根据第二预设步长因子、当前位置信息和优选位置信息确定第二途经位置。

其中，第二预设步长因子可以与第一预设步长因子相等，也可以不相同。

可选的，第二预设步长因子大于0且小于1，第二预设步长因子可以由本领域技术人员根据需要进行设定。

以当前位置信息为(a₀,b₀)，优选位置信息为(p₁,p₂)，第二预设步长因子Q₂为例，第一途经位置M₂的一种可选的确定方式包括：

其中a₀为当前位置的横坐标，b₀为当前位置的纵坐标，p₁为优选位置的横坐标，p₂为优选位置的纵坐标，Q₂为第二预设步长因子，M₂为第二途经位置的第二途经位置信息。

需要说明的是，上述仅是以二维坐标系为例示例性的说明第二途经位置的一种确定方式，对于多维表示的方式中第二途经位置的确定方式与上述方式类似，在此不再赘述。

在一些实施例中，第二途经位置为以当前位置为起点，沿当前位置向优选位置移动第二距离，其中第二距离根据第二预设步长因子和优选距离确定。优选距离为优选位置与当前位置的距离。

S1024：若目标途径位置满足预设碰撞条件，将目标途径位置作为当前位置对应的下一位置。

其中，目标途径位置包括第一途经位置或第二途经位置。

在一些实施例中，预设碰撞条件包括：

获取若干个障碍位置，以当前位置为起点，目标途径位置为终点，随机生成若干个检测位置，当前位置、目标途径位置和各检测位置生成的检测路径中不包括任一障碍位置。

可选的，障碍位置为预设地图中各障碍物的位置信息，检测位置为目标途径位置到当前位置连线上的点。

目标途径位置是否满足预设碰撞条件的检测还可以通过本领域技术人员所知晓的其他方式进行判断，在此不做限定。

可选的，可以在当前位置和目标途径位置之间沿前后左右方向进行扩展，得到若干个检测位置，再对各检测位置之间所形成的路径进行碰撞检测，若存在碰撞的可能性，则不满足预设碰撞条件，否则满足预设碰撞条件。

可选的，可以通过collision-free函数来判定目标途径位置是否满足预设碰撞条件。

在一些实施例中，若第一途经位置和第二途经位置均满足预设碰撞条件，将第一途经位置作为下一位置。这样可以优先保证路径是沿最小距离的方向延伸。

在一些实施例中，对于目标途径位置是否满足预设碰撞条件的判断，是先判断第一途径位置是否满足预设碰撞条件后，若满足，则直接将该第一途经位置作为下一位置，不再对第二途径位置进行判断。若第一途经位置不满足预设碰撞条件，则判断第二途经位置是否满足预设碰撞条件，若满足，则将第二途径位置作为下一位置。

可选的，若第一参考距离等于第二参考距离，则将满足预设碰撞条件的一个第二途径位置作为下一位置。若两个第二途径位置均满足预设碰撞条件，此时任选一个第二途径位置作为下一位置。

在一些实施例中，若第一途经位置和第二途经位置均不满足预设碰撞条件，重新获取新的第一采样位置和第二采样位置，并确定新的第二途经位置，直到第二途径位置满足预设碰撞条件，将第二途径位置作为当前位置对应的下一位置。

此时，可以通过重新确定新的第一采样位置和第二采样位置，并重选优选位置，进而确定得到新的第二途径位置，直到得到的第二途径位置满足预设碰撞条件位置，将该第二途径位置作为下一位置。可选的，可以通过设定重复运算次数，当重新确定新的第一采样位置和第二采样位置的次数达到重复运算次数时，则放弃之前的路径规划，重新从起始位置开始规划路径。

在一些实施例中，由于之前已经确定得到优选位置，只不过该优选位置与当前位置之间存在障碍物，则可以单独在优选位置一侧重新生成对应的新的第二途径位置，具体的，若第一途经位置和第二途经位置均不满足预设碰撞条件，将优选位置在基准连线的一侧作为优选区域，在优选区域确定新的优选采样位置，并确定新的第二途经位置，直到第二途径位置满足预设碰撞条件，将第二途径位置作为当前位置对应的下一位置。此时，优选采样位置的确定可以采用RRT算法实现，也可以采样其他本领域技术人员所知晓的方式实现。

在一些实施例中，在将目标途径位置作为当前位置对应的下一位置之前，该方法还包括：

获取目标途径位置与目标位置的途径距离，将该途径距离与基准距离进行比对，若途径距离小于基准距离，则将该目标途径位置作为当前位置对应的下一位置，否则，重新获取新的第一采样位置和第二采样位置，重新进行目标途径位置的确定，直到途径距离小于基准距离。

S1025：重复进行当前位置的确定，直到当前位置与目标位置之间的当前距离小于预设距离阈值，完成各当前位置的确定。

其中，预设距离阈值为本领域技术人员所设定的值。

由于每一个当前位置实质上都可以确定得到一个下一位置，再将下一位置更新当前位置，将下一位置作为新的当前位置，以得到下一个新的当前位置，通过上述方式可以兼顾距离和效率以及采样的随机均匀。

本实施例提供了一种机器人路径规划方法，通过在当前位置与目标位置之间的基准连线上确定第一途经位置，获取当前位置的第一采样位置和第二采样位置，并确定第二途径位置，若第一途经位置和第二途径位置中至少存在一个满足预设碰撞条件，则将满足预设碰撞条件的一个位置作为下一位置，重复进行当前位置的确定，直到最新得到的当前位置与目标位置之间的当前距离小于预设距离阈值，完成各当前位置的确定，根据目标位置、各当前位置以及起始位置确定规划路径，在机器人路径规划时综合考虑第一采样位置和第二采样位置确定的第二途径位置，以及目标方向的第一途经位置来确定当前位置，提升了目标搜索性，减少了机器人路径规划的用时和冗余节点，效率更高。

下面以将上述实施例所述的机器人路径规划方法应用于机器人机器人路径规划为例，提供一个具体的实施例示例性的说明上述实施例中的机器人路径规划方法。

一种基于改进RRT算法的机器人机器人路径规划方法，基于起始点(起始位置)在预设目标偏置函数的作用下，生成两个向目标点扩展的采样点(第一采样位置和第二采样位置)，并通过比较目标点(目标位置)与所有叶子节点(第一采样位置和第二采样位置)的距离，确定第二途径位置X_near。然后借鉴天牛须算法分别计算目标位置X_goal与当前位置X_near和采样位置X_rand的距离，并生成三对触须，进一步决定树节点的扩展方向。

参见图3，已知起点位置X_init、终点位置X_goal、距离阈值Thr、扩展步长因子delta、重复运算次数，其中，重复运算次数为某一次机器人路径规划中，重新规划当前位置到目标位置之间的路径的次数，若机器人路径规划次数达到重复运算次数，则放弃本次循环，从起始点开始重新进行机器人路径规划，该具体的机器人路径规划方法包括：

S301：判断起点X_init与终点X_goal的距离小于等于步长且不与障碍物发生碰撞，若否执行步骤S102，若是则执行步骤S317。

这样，如果起点到终点的距离大于扩展步长或起始点到目标的距离小于扩展步长且与周围障碍物发生碰撞，则运用改进的RRT算法便可以得到一条从起点到终点的路径。

如果起点到终点的距离小于扩展步长且没有与周围障碍物发生碰撞，则连接起点与目标点便可以得到一条完美路径。

S302：设置扩展步长因子，并构建树。

S303：设置预设目标偏置函数Y＝rand(1)。

S304：判断预设目标偏置函数值，若Y≤0.5，则执行步骤S105,否则，执行步骤S106。

通过设置目标偏置函数Y＝rand(1)，当Y≤0.5时，将采样位置扩展向终点引导，加快RRT算法的扩展速度；当Y>0.5时，仍采用随机扩展的方式，保持随机树对环境随机探索的能力。最终生成两个采样点，第一采样位置X_rand₁和第二采样位置X_rand₂。

S305：确定第一采样位置。

其中，一种第一采样位置的确定方式为：

X_rand(1)＝400+rand(1)*350 公式(6)；

X_rand(2)＝400+rand(1)*350 公式(7)；

X_rand(3)＝400+rand(1)*350 公式(8)；

X_rand(4)＝400+rand(1)*350 公式(9)。

S106：确定第二采样位置。

其中，一种第二采样位置的确定方式为：

X_rand(1)＝rand(1)*750 公式(10)；

X_rand(2)＝rand(1)*750 公式(11)；

X_rand(3)＝rand(1)*750 公式(12)；

X_rand(4)＝rand(1)*750 公式(13)。

需要说明的是上述数字“350、400、750”均为一种示例性的说明，本领域技术人员也可根据需要选择合适的数值。

S307：分别确定第一采样位置的第一采样位置信息、第二采样位置的第二采样位置信息。

其中，第一采样位置信息：

X_rand1＝【X_rand(1)，X_rand(2)】 公式(14)。

第二采样位置信息

X_rand2＝【X_rand(3)，X_rand(4)】 公式(15)。

S308：从之前确定的若干当前位置中选取距离终点X_goal最近的一个当前位置作为新的当前位置X_near。

S309：分别确定第一参考距离、第二参考距离、基准距离。

选取距离X_goal最近的叶子节点为新的当前位置X_near，定义X_near是天牛触须的起点；定义第一参考距离tist为X_goal与X_rand₁之间的欧式距离；第二参考距离dist为X_rand₂与X_goal之间的欧式距离；基准距离sist为任意叶子节点(当前位置)与X_goal之间的欧式距离，此时选取新的当前位置X_near与终点X_goal之间的距离为基准距离sist。

一种第一参考距离tist、第二参考距离dist和基准距离sist的确定方式如下：

S310：判断第一参考距离是否小于或等于第二参考距离的大小，若是执行步骤S111，若否执行步骤S112。

S311：以X_near为天牛触须起点，delta为扩展步长因子朝向X_goal扩展生成天牛触须运动节点X_new₁(第一途径位置)；以X_near为天牛触须起点，delta为步长因子朝向X_rand₁扩展生成天牛触须运动节点X_new₂(第二途径位置)。

S312：以X_near为起点，delta为扩展步长因子朝向X_goal扩展生成天牛触须运动节点X_new₁(第一途径位置)；然后以X_near为天牛触须起点，delta为步长因子朝向X_rand₂扩展生成天牛触须运动节点X_new₂(第二途径位置)；

若X_new的坐标为

当tist≤dist时，以X_near为起点，delta为扩展步长朝向X_goal和X_rand₁扩展生成X_new₁和X_new₂，此时两对触须为X_near→X_goal和X_near→X_rand₁，X_new大小为：

当tist>dist时，以X_near为起点，delta为扩展步长朝向X_goal和X_rand₂扩展生成X_new₁和X_new₂，此时两对触须为X_near→X_goal和X_near→X_rand₂，X_new大小为：

S313：检查第一途经位置X_new₁和第二途径位置X_new₂是否满足预设碰撞条件，若是执行步骤S114，若否执行步骤S103。

S314：确定下一位置X_new。

一种其中，下一位置X_new的确定方式包括：

参见图4，连接天牛触须起点X_near，两个采样点(X_rand₁和X_rand₂)、目标点(X_goal)形成一个封闭的四边形，并计算侧边的欧式距离，设置天牛左须tist，天牛中须sist、天牛右须dist、寻找最佳的天牛触须运动节点(下一位置X_new)。

当tist≤dist时

(1)天牛触须运动节点X_new₁沿着天牛须起点X_near→X_goal运动，形成天牛中须，如果X_new₁符合collision-free无碰撞条件，就把X_new₁作为最佳的天牛触须运动节点(下一位置X_new)。

(2)天牛触须运动节点X_new₂沿着天牛须起点X_near→X_rand₁运动，形成天牛左须，如果X_new₂符合collision-free无碰撞条件，就把X_new₂作为最佳的天牛触须运动节点(下一位置X_new)。

当tist>dist时

(1)天牛触须运动节点X_new₁沿着天牛须起点X_near→X_goal运动，形成天牛中须，如果X_new₁符合collision-free无碰撞条件(也可以是本领域技术人员所知晓的其他能够确定无碰撞的算法)，就把X_new₁作为最佳的天牛触须运动节点(下一位置X_new)。

(2)天牛触须运动节点X_new₂沿着天牛须起点X_near→X_rand₂运动，形成天牛右须，如果X_new₂符合collision-free无碰撞条件，就把X_new₂作为最佳的天牛触须运动节点(下一位置X_new)。

S315：将下一位置X_new插入树。

此时该树有Count＝Count+1。

如果以X_near为天牛须起点，delta为扩展步长因子扩展至X_new，并与障碍物不发生碰撞，则把X_new插入树T；否则放弃X_new，重新采样生产X_rand₁和X_rand₂。

S316：检查当前位置X_new与终点X_goal之间的距离是否小于距离阈值Thr，若是执行步骤S117，否则执行步骤S103。

通过检查最新的当前位置是否到达终点附近。如果当前位置X_near与X_goal的欧式距离小于等于距离阈值Thr，则跳出当前循环；否则放弃当前位置，重新采样生产叶子节点第一采样位置和第二采样位置(X_rand₁和X_rand₂)。

S317:将X_near与X_new之间的路径画出来，并反向生成一条从起点到终点的路径。

执行完成S317后结束流程。

参见图5，图5为在20次实验中，传统RRT算法每次实验运行的时间与本实施例提供的机器人路径规划方法(基于天牛须算法改进后的方法)所运行的时间的对比的示意图，该图中，位于上侧的是传统RRT算法每次实验运行的时间，下侧的线条表征的是本实施例提供的机器人路径规划方法的时间。

参见图6，图6为在20次实验中，传统RRT算法每次实验得到的节点数与本实施例提供的机器人路径规划方法(基于天牛须算法改进后的方法)每次试验得到的节点数的对比示意图，该图中，位于下侧的线条为本实施例提供的机器人路径规划方法的数据，位于上侧的线条标识的是传统RRT算法每次实验得到的节点数。

参见图7，图7为传统RRT算法的仿真实验。

参见图8，图8为本实施例提供的机器人路径规划方法(基于天牛须算法改进的RRT算法)的仿真实验。

参见图9，图9为传统RRT算法在增加障碍物以后的仿真实验。

参见图10，图10为本实施例提供的机器人路径规划方法(基于天牛须算法改进的RRT算法)在增加障碍物以后的仿真实验。

结合图5-图10可以看出，本实施例中提供的一种移动机器人机器人路径规划方法，基于天牛须算法来改进RRT算法，改善了传统RRT算法目标搜索性差、时间长、冗余节点多、效率低等缺点，大大节省了RRT算法的运行时间。在借鉴天牛须算法和传统RRT算法的基础上提出了围绕目标点采样、生成新节点的方法，通过利用预设目标偏置函数生成两个向目标偏置的采样点(第一采样位置和第二采样位置)，缩短扩展时间。通过利用欧式距离加权函数找到一个距离目标点最近的叶子节点(当前节点)，把它定义为天牛触须的起点。让天牛触须起点分别连接两个采样点和目标点来生成三对天牛触须。连接天牛触须起点，两个采样点、目标点形成一个封闭的四边形，并计算侧边的欧式距离，寻找最佳的天牛触须运动节点。将传统RRT算法与改进后的RRT算法在Matlab中进行仿真实验，实验结果证明，该算法可以有效引导RRT树朝向目标点生长，并且提高了算法的收敛速度与运行效率。

实施例二

请参阅图11，本实施例提供了一种机器人路径规划系统1100，该系统包括：

获取模块1101，用于用于获取起始位置和目标位置；

规划模块1102，用于用于在起始位置和目标位置之间确定若干个当前位置，并根据起始位置、各当前位置和目标位置确定规划路径；

其中，当前位置的确定方式包括，

将起始位置作为当前位置，获取位于基准连线上的第一途经位置，基准连线为当前位置和目标位置之间的连线；

获取当前位置的第一采样位置和第二采样位置，并确定第一参考距离和第二参考距离，第一采样位置和第二采样位置分别位于基准连线的两侧，第一参考距离为第一采样位置与目标位置之间的距离，第二参考距离为第二采样位置与目标位置之间的距离，第一参考距离和第二参考距离均小于基准距离，基准距离为起始位置和目标位置之间的距离；

根据第一参考距离和第二参考距离从第一采样位置和第二采样位置中确定优选位置，并获取位于优选连线上的第二途经位置，优选连线为优选位置与当前位置之间的连线；

若目标途径位置满足预设碰撞条件，将目标途径位置作为当前位置对应的下一位置，目标途径位置包括第一途经位置或第二途经位置；

重复进行当前位置的确定，直到当前位置与目标位置之间的当前距离小于预设距离阈值，完成各当前位置的确定。

在本实施例中，该系统实质上是设置了多个模块用以执行上述任一实施例中的方法，具体功能和技术效果参照上述实施例一即可，此处不再赘述。

参见图12，本发明实施例还提供了一种电子设备1300，包括处理器1301、存储器1302和通信总线1303；

通信总线1303用于将处理器1301和存储器连接1302；

处理器1301用于执行存储器1302中存储的计算机程序，以实现如上述实施例一中的一个或多个所述的方法。

本发明实施例还提供了一种计算机可读存储介质，其特征在于，其上存储有计算机程序，

计算机程序用于使计算机执行如上述实施例一中的任一项所述的方法。

本申请实施例还提供了一种非易失性可读存储介质，该存储介质中存储有一个或多个模块(programs)，该一个或多个模块被应用在设备时，可以使得该设备执行本申请实施例的实施例一所包含步骤的指令(instructions)。

需要说明的是，本公开上述的计算机可读介质可以是计算机可读信号介质或者计算机可读存储介质或者是上述两者的任意组合。计算机可读存储介质例如可以是——但不限于——电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。计算机可读存储介质的更具体的例子可以包括但不限于：具有一个或多个导线的电连接、便携式计算机磁盘、硬盘、随机访问存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑磁盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。在本公开中，计算机可读存储介质可以是任何包含或存储程序的有形介质，该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。而在本公开中，计算机可读信号介质可以包括在基带中或者作为载波一部分传播的数据信号，其中承载了计算机可读的程序代码。这种传播的数据信号可以采用多种形式，包括但不限于电磁信号、光信号或上述的任意合适的组合。计算机可读信号介质还可以是计算机可读存储介质以外的任何计算机可读介质，该计算机可读信号介质可以发送、传播或者传输用于由指令执行系统、装置或者器件使用或者与其结合使用的程序。计算机可读介质上包含的程序代码可以用任何适当的介质传输，包括但不限于：电线、光缆、RF(射频)等等，或者上述的任意合适的组合。

上述计算机可读介质可以是上述电子设备中所包含的；也可以是单独存在，而未装配入该电子设备中。

可以以一种或多种程序设计语言或其组合来编写用于执行本公开的操作的计算机程序代码，上述程序设计语言包括面向对象的程序设计语言—诸如Java、Smalltalk、C++，还包括常规的过程式程序设计语言—诸如“C”语言或类似的程序设计语言。程序代码可以完全地在用户计算机上执行、部分地在用户计算机上执行、作为一个独立的软件包执行、部分在用户计算机上部分在远程计算机上执行、或者完全在远程计算机或服务器上执行。在涉及远程计算机的情形中，远程计算机可以通过任意种类的网络——包括局域网(LAN)或广域网(WAN)—连接到用户计算机，或者，可以连接到外部计算机(例如利用因特网服务提供商来通过因特网连接)。

附图中的流程图和框图，图示了按照本公开各种实施例的方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段、或代码的一部分，该模块、程序段、或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个接连地表示的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，依所涉及的功能而定。也要注意的是，框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合，可以用执行规定的功能或操作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。

上述实施例仅例示性说明本发明的原理及其功效，而非用于限制本发明。任何熟悉此技术的人士皆可在不违背本发明的精神及范畴下，对上述实施例进行修饰或改变。因此，举凡所属技术领域中具有通常知识者在未脱离本发明所揭示的精神与技术思想下所完成的一切等效修饰或改变，仍应由本发明的权利要求所涵盖。

Claims (10)

1.一种机器人路径规划方法，其特征在于，所述方法包括：

获取起始位置和目标位置；

在所述起始位置和目标位置之间确定若干个当前位置，并根据所述起始位置、各当前位置和目标位置确定规划路径；

所述当前位置的确定方式包括，

将所述起始位置作为当前位置，获取位于基准连线上的第一途经位置，所述基准连线为所述当前位置和目标位置之间的连线；

获取所述当前位置的第一采样位置和第二采样位置，并确定第一参考距离和第二参考距离，所述第一采样位置和第二采样位置分别位于基准连线的两侧，所述第一参考距离为所述第一采样位置与目标位置之间的距离，所述第二参考距离为所述第二采样位置与目标位置之间的距离，所述第一参考距离和所述第二参考距离均小于基准距离，所述基准距离为所述起始位置和目标位置之间的距离；

根据所述第一参考距离和第二参考距离从所述第一采样位置和第二采样位置中确定优选位置，并获取位于优选连线上的第二途经位置，所述优选连线为所述优选位置与所述当前位置之间的连线；

若目标途径位置满足预设碰撞条件，将所述目标途径位置作为所述当前位置对应的下一位置，所述目标途径位置包括所述第一途经位置或第二途经位置；

重复进行所述当前位置的确定，直到所述当前位置与目标位置之间的当前距离小于预设距离阈值，完成各所述当前位置的确定。

2.如权利要求1所述的机器人路径规划方法，其特征在于，所述方法还包括以下任意之一：

若所述第一途经位置和第二途经位置均不满足预设碰撞条件，重新获取新的所述第一采样位置和第二采样位置，并确定新的所述第二途经位置，直到所述第二途径位置满足所述预设碰撞条件，将所述第二途径位置作为所述当前位置对应的下一位置；

若所述第一途经位置和第二途经位置均满足预设碰撞条件，将所述第一途经位置作为所述下一位置。

3.如权利要求1所述的机器人路径规划方法，其特征在于，所述方法还包括：

若所述第一途经位置和第二途经位置均不满足预设碰撞条件，将所述优选位置在所述基准连线的一侧作为优选区域；

在所述优选区域确定新的优选采样位置，并确定新的所述第二途经位置；

直到所述第二途径位置满足所述预设碰撞条件，将所述第二途径位置作为所述当前位置对应的下一位置。

4.如权利要求1所述的机器人路径规划方法，其特征在于，所述方法还包括：

获取预设目标偏置函数；

若所述预设目标偏置函数小于预设函数阈值，则根据所述当前位置的当前位置信息和预设目标偏置函数生成所述第一采样位置和第二采样位置；

若所述预设目标偏置函数大于或等于预设函数阈值，则生成所述第一途经位置。

5.如权利要求1-4任一项所述的机器人路径规划方法，其特征在于，

所述第一途经位置的确定方式包括，分别获取当前位置信息和目标位置信息，并根据第一预设步长因子、所述当前位置信息和目标位置信息确定所述第一途经位置；

和/或，

所述第二途经位置的确定方式包括，分别获取当前位置信息和优选位置信息，并根据第二预设步长因子、所述当前位置信息和优选位置信息确定所述第二途经位置。

6.如权利要求1-4任一项所述的机器人路径规划方法，其特征在于，所述根据所述第一参考距离和第二参考距离从所述第一采样位置和第二采样位置中确定优选位置包括以下任意之一：

若所述第一参考距离小于第二参考距离，将所述第一采样位置作为优选位置；

若所述第二参考距离小于第一参考距离，将所述第二采样位置作为优选位置；

若所述第二参考距离等于第一参考距离，将所述第一采样位置和/或所述第二采样位置作为优选位置。

7.如权利要求1-4任一项所述的机器人路径规划方法，其特征在于，所述预设碰撞条件包括：

获取若干个障碍位置，以所述当前位置为起点，目标途径位置为终点，随机生成若干个检测位置，所述当前位置、目标途径位置和各检测位置生成的检测路径中不包括任一所述障碍位置。

8.一种机器人路径规划系统，其特征在于，所述系统包括：

获取模块，用于获取起始位置和目标位置；

规划模块，用于在所述起始位置和目标位置之间确定若干个当前位置，并根据所述起始位置、各当前位置和目标位置确定规划路径；

所述当前位置的确定方式包括，

将所述起始位置作为当前位置，获取位于基准连线上的第一途经位置，所述基准连线为所述当前位置和目标位置之间的连线；

获取所述当前位置的第一采样位置和第二采样位置，并确定第一参考距离和第二参考距离，所述第一采样位置和第二采样位置分别位于基准连线的两侧，所述第一参考距离为所述第一采样位置与目标位置之间的距离，所述第二参考距离为所述第二采样位置与目标位置之间的距离，所述第一参考距离和所述第二参考距离均小于基准距离，所述基准距离为所述起始位置和目标位置之间的距离；

根据所述第一参考距离和第二参考距离从所述第一采样位置和第二采样位置中确定优选位置，并获取位于优选连线上的第二途经位置，所述优选连线为所述优选位置与所述当前位置之间的连线；

若目标途径位置满足预设碰撞条件，将所述目标途径位置作为所述当前位置对应的下一位置，所述目标途径位置包括所述第一途经位置或第二途经位置；

重复进行所述当前位置的确定，直到所述当前位置与目标位置之间的当前距离小于预设距离阈值，完成各所述当前位置的确定。

9.一种电子设备，其特征在于，包括处理器、存储器和通信总线；

所述通信总线用于将所述处理器和存储器连接；

所述处理器用于执行所述存储器中存储的计算机程序，以实现如权利要求1-7中任一项所述的方法。

10.一种计算机可读存储介质，其特征在于，其上存储有计算机程序，

所述计算机程序用于使所述计算机执行如权利要求1-7中任一项所述的方法。

Images