CN112454367A

CN112454367A - 一种轨迹规划方法、装置以及计算机存储介质

Info

Publication number: CN112454367A
Application number: CN202011455167.9A
Authority: CN
Inventors: 姚达琛; 何悦; 李�诚
Original assignee: Beijing Sensetime Technology Development Co Ltd
Current assignee: Beijing Sensetime Technology Development Co Ltd
Priority date: 2020-12-10
Filing date: 2020-12-10
Publication date: 2021-03-09
Anticipated expiration: 2040-12-10
Also published as: WO2022121207A1; KR20220083975A; JP2023508794A; CN112454367B

Abstract

本申请提供了一种轨迹规划方法、装置以及计算机存储介质。该轨迹导航方法包括：从建模获取轨迹信息和交通信息，其中，轨迹信息包括机器人的当前位置和目的地位置；基于交通信息在建模上设置多个关键点；基于交通信息在两两关键点之间形成连通段；采用预设算法搜索当前位置和目的地位置之间的所有连通段，按照搜索结果将连通段连接为机器人的目标轨迹路径。通过上述方式，本申请的轨迹规划方法能够实现机器人在建模上点对点之间的路径规划。

Description

一种轨迹规划方法、装置以及计算机存储介质

技术领域

本申请涉及机器人轨迹规划技术领域，特别是涉及一种轨迹规划方法、装置以及计算机存储介质。

背景技术

近年来，AI教育与自动驾驶逐渐变得火热，将二者结合在一起的智能机器人成为了大部分公司产品开发的重点。这种智能机器人通常在封闭沙盘运行，可供学生学习机器人控制策略以及路径规划等方面的知识。除此之外，这种具有自动驾驶功能的机器人作为展品的展示效果也是非常好的。

然而，智能机器人在沙盘内运行，只会靠工作人员直接控制，或通过输入清晰、直接的运行路径引导智能机器人移动。由于沙盘内环境复杂，可能会出现机器人偏离运行路径或出现一些机器人无法实现的转弯操作，导致机器人走走停停。

发明内容

本申请提供了一种轨迹规划方法、装置以及计算机存储介质，主要解决的技术问题是如何实现机器人在建模上点对点之间的路径规划的问题。

为解决上述技术问题，本申请提供了一种轨迹规划方法，所述轨迹规划方法包括：

从所述建模获取轨迹信息和交通信息，其中，所述轨迹信息包括机器人的当前位置和目的地位置；

基于所述交通信息在所述建模上设置多个关键点；

基于所述交通信息在两两关键点之间形成连通段；

采用预设算法搜索所述当前位置和所述目的地位置之间的所有连通段，按照搜索结果将所述连通段连接为所述机器人的目标轨迹路径。

所述交通信息包括交通道路分布情况；

所述基于所述交通信息在所述建模上设置多个关键点的步骤，包括：

基于所述交通道路分布情况获取每个路口的进出口处、每个弯道的进出口处以及每条道路的边缘和中间位置在所述建模中的关键坐标；

将所述关键坐标在所述建模对应的位置设置为所述关键点。

通过上述方式，提出了一种基于道路分布情况设置关键点的方法。

其中，所述基于所述交通信息在所述建模上设置多个关键点的步骤，还包括：

基于所述交通信息获取所述建模中允许停泊的位置坐标；

从所述允许停泊的位置坐标中获取所述机器人在所述建模中需要停泊的位置坐标；

将所述位置坐标在所述建模对应的位置设置为所述关键点。

通过上述方式，提出了一种基于机器人情况设置关键点的方法。

所述交通信息包括交通规则；

所述基于所述交通信息在两两关键点之间形成连通段的步骤包括：

将所述机器人的当前位置和目的地位置之间的关键点两两之间组成轨迹段；

将符合所述交通规则的轨迹段设置为所述连通段。

通过上述方式，引入交通规则判断两两关键点连成的轨迹段是否合理。

其中，所述按照搜索结果将所述连通段连接为所述机器人的目标轨迹路径的步骤之后，所述轨迹规划方法还包括：

获取所述机器人的动力学模型，其中，所述动力学模型包括所述机器人在每段所述连通段的加速度和最大速度；

获取所述机器人在所述目标轨迹路径中每段连通段的运动时间以及运动向量；

基于所述运动时间的大小，通过所述机器人在每段所述连通段的加速度和最大速度更新每段所述连通段的运动向量。

通过上述方式，引入机器人的动力学模型优化目标轨迹路径。

其中，所述获取所述机器人在所述目标轨迹路径中每段连通段的运动时间以及运动向量的步骤之后，所述轨迹规划方法还包括：

基于所述连通段的运动向量以及加速度段占所述连通段的比例获取所述连通段的速度系数；

获取所述连通段的起始时间和终止时间；

基于所述连通段的起始时间和速度系数确定第一标准时间；

基于所述连通段的第一标准时间、运动向量、速度系数和加速度段占所述连通段的比例确定第二标准时间；

所述基于所述运动时间的大小，通过所述机器人在每段所述连通段的加速度和最大速度更新每段所述连通段的运动向量的步骤，包括：

基于所述运动时间、第一标准时间和第二标准时间的大小，通过所述机器人在每段所述连通段的加速度和最大速度更新每段所述连通段的运动向量。

通过上述方式，计算目标轨迹路径优化的判断条件。

其中，所述基于所述运动时间、第一标准时间和第二标准时间的大小，通过所述机器人在每段所述连通段的加速度和最大速度更新每段所述连通段的运动向量的步骤，包括：

当所述运动时间在所述起始时间和所述第一标准时间之间时，基于所述起始时间以及所述机器人在每段所述连通段的加速度和最大速度将所述运动向量进行更新；

当所述运动时间在所述第一标准时间和所述第二标准时间之间时，基于所述第一标准时间以及所述机器人在每段所述连通段的加速度和最大速度将所述运动向量进行更新；

当所述运动时间在所述第二标准时间和所述终止时间之间时，基于所述第二标准时间以及所述机器人在每段所述连通段的加速度和最大速度将所述运动向量进行更新。

通过上述方式，根据机器人在目标轨迹路径中的运动时间与判断条件的比较结果对目标轨迹路径进行不同方式的优化。

其中，所述基于所述连通段的运动向量以及加速度段占所述连通段的比例获取所述连通段的速度系数的步骤，包括：

基于所述连通段的运动向量、所述机器人的加速度和最大速度确定所述加速度段占所述连通段的比例；

基于所述机器人的最大速度、所述连通段的运动向量以及所述加速度段占所述连通段的比例确定所述连通段的速度系数。

通过方式，提出一种计算每个轨迹段中加速度段的长度比例。

为解决上述技术问题，本申请还提供了一种轨迹规划装置，所述轨迹规划装置包括存储器和处理器，其中，所述存储器与所述处理器耦接；

其中，所述存储器用于存储程序数据，所述处理器用于执行所述程序数据以实现如上述的轨迹规划方法。

为解决上述技术问题，本申请还提供了一种计算机存储介质，所述计算机存储介质用于存储程序数据，所述程序数据在被处理器执行时，用以实现如上述的轨迹规划方法。

与现有技术相比，本申请的有益效果是：轨迹规划装置从建模获取轨迹信息和交通信息，其中，轨迹信息包括机器人的当前位置和目的地位置；基于交通信息在建模上设置多个关键点；基于交通信息在两两关键点之间形成连通段；采用预设算法搜索当前位置和目的地位置之间的所有连通段，按照搜索结果将连通段连接为机器人的目标轨迹路径。通过上述方式，本申请的轨迹规划方法通过在当前位置和目的地位置之间设置多个关键点，并通过多个关键点组合形成机器人的目标轨迹路径，由于轨迹规划装置基于交通信息设置关键点，即关键点的位置设置于建模中的合理位置，提高目标轨迹路径的灵活性和实用性，从而实现机器人在建模上点对点之间的路径规划。

附图说明

为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。其中：

图1是本申请提供的轨迹规划方法第一实施例的流程示意图；

图2是本申请提供的沙盘建模一实施例的结构示意图；

图3是本申请提供的多条轨迹路径在沙盘建模的示意图；

图4是本申请提供的目标轨迹路径在沙盘建模的示意图；

图5是本申请提供的轨迹规划方法第二实施例的流程示意图；

图6是本申请提供的轨迹规划方法第三实施例的流程示意图；

图7是本申请提供的优化后目标轨迹路径在沙盘建模的示意图；

图8是本申请提供的轨迹规划装置一实施例的结构示意图；

图9是本申请提供的计算机存储介质一实施例的结构示意图。

具体实施方式

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

为了机器人在建模上点对点之间的路径规划，本申请提出了一种轨迹规划方法，具体请参阅图1，图1是本申请提供的轨迹规划方法第一实施例的流程示意图。

本申请的轨迹规划方法应用于一种轨迹规划装置，其中，本申请的轨迹规划装置可以为服务器，也可以为终端设备，还可以为由服务器和终端设备相互配合的系统。相应地，电子设备包括的各个部分，例如各个单元、子单元、模块、子模块可以全部设置于服务器中，也可以全部设置于终端设备中，还可以分别设置于服务器和终端设备中。

进一步地，上述服务器可以是硬件，也可以是软件。当服务器为硬件时，可以实现成多个服务器组成的分布式服务器集群，也可以实现成单个服务器。当服务器为软件时，可以实现成多个软件或软件模块，例如用来提供分布式服务器的软件或软件模块，也可以实现成单个软件或软件模块，在此不做具体限定。

如图1所示，本实施例的轨迹规划方法具体包括以下步骤：

S101：从建模获取轨迹信息和交通信息，其中，轨迹信息包括机器人的当前位置和目的地位置。

其中，本申请的建模可以为一种用于模拟城市先进道路交通的沙盘，例如是基于212种中国城市道路交通场景设计的沙盘。具体请参阅图2，沙盘中可以包括车道、交通标志、交通信号灯以及道路闸机等元素，其中，车道、交通标志、交通信号灯以及道路闸机按1:10的微缩比例。沙盘通过控制其中元素的状态将道路交通事件可视化，提升沙盘的展示效果。

其中，轨迹规划装置接入沙盘建模，接入方式可以为通过USB接口连接，或无线通信连接。轨迹规划装置通过沙盘建模的定位系统定位获取轨迹信息和交通信息。具体地，轨迹信息包括机器人在沙盘建模中的当前位置和目的地位置，交通信息包括交通规则和交通道路分布情况。轨迹规划装置接入沙盘建模后，可获悉沙盘建模的三维模型以及机器人在沙盘建模中可运行的区域以及运行规则。机器人的运行规则由沙盘建模的交通规则决定，交通规则与现实生活中的交通规则一致，在此不再赘述。

S102：基于交通信息在建模上设置多个关键点。

其中，轨迹规划装置基于交通道路分布情况在沙盘建模上设置多个关键点，这些关键点根据交通道路的走向分布在道路上，关键点的位置均为机器人在正常运行过程中可以到达的位置，例如道路边缘、道路拐弯处以及道路交汇处等。

其中，轨迹规划装置预设关键点的目的是为轨迹规划提供合理的搜索空间，生成的轨迹路径一定是穿过若干关键点然后到达目的地位置。

S103：基于交通信息在两两关键点之间形成连通段。

其中，轨迹规划装置基于轨迹信息确定此次轨迹规划目标的目的地位置，以及通过沙盘建模的定位系统获取机器人的当前位置。

由于最终的目标轨迹路径是有向路径，轨迹规划装置需要进一步设计当前位置和目的地位置之间两两关键点之间的有向邻接矩阵以进行交通规则设计。两个关键点之间若在交通规则下能够进行连接，则在邻接矩阵中将该线路设置为1，即将两个关键点之间的线路设置为连通段。

由此，轨迹规划装置通过多个关键点在机器人的当前位置和目的地位置之间连接成多条连通段，组成每条连通段的临界矩阵均为1，即连接成的连通段为可循迹的可行连通段。

如图3所示，一般而言，由于关键点的数量较多，连接成的连通段数量也很多。因此，轨迹规划装置还需要从多条可行连通段中筛选中最近且合理的连通段连接成最终的目标轨迹路径。

S104：采用预设算法搜索当前位置和目的地位置之间的所有连通段，按照搜索结果将连通段连接为机器人的目标轨迹路径。

其中，轨迹规划装置可以采用预设的搜索算法，从多条轨迹路径中搜索出最佳的轨迹路径作为目标轨迹路径。其中，预设的搜索算法包括但不限于：Dijkstra算法、A*(A-Star)算法、RRT算法、人工势场法等。

以A-Star算法为例，A-Star算法是一种静态路网中求解最短路最有效的方法。公式表示为：f(n)＝g(n)+h(n)，其中f(n)是节点n从当前位置到目的地位置的估价函数，g(n)是在状态空间中从当前位置到节点n的实际代价，h(n)是从节点n到目的地位置最佳路径的估计代价。具体地，轨迹规划路径采用A-Star算法计算当前位置到目的地位置之间的所有连通段的估价函数，将组合估计函数值最小的多段连通段连接成机器人的目标轨迹路径。

在本实施例中，轨迹规划装置接入建模，并从建模获取轨迹信息和交通信息，其中，轨迹信息包括机器人的当前位置和目的地位置；基于交通信息在建模上设置多个关键点；基于交通信息在两两关键点之间形成连通段；采用预设算法搜索当前位置和目的地位置之间的所有连通段，按照搜索结果将连通段连接为机器人的目标轨迹路径。通过上述方式，本申请的轨迹规划方法通过在当前位置和目的地位置之间设置多个关键点，并通过多个关键点组合形成机器人的目标轨迹路径，由于轨迹规划装置基于交通信息设置关键点，即关键点的位置设置于建模中的合理位置，提高目标轨迹路径的灵活性和实用性，从而实现机器人在建模上点对点之间的路径规划。

为了机器人在建模上点对点之间的路径规划，本申请提出了另一种轨迹规划方法，具体请参阅图5，图5是本申请提供的轨迹规划方法第二实施例的流程示意图。

如图5所示，本实施例的轨迹规划方法具体包括以下步骤：

S201：从建模获取轨迹信息和交通信息，其中，轨迹信息包括机器人的当前位置和目的地位置。

S202：基于交通道路分布情况获取每个路口的进出口处、每个弯道的进出口处以及每条道路的边缘和中间位置在建模中的关键坐标。

其中，对于一般的交通道路来说，在道路上正常运行的车辆只能在路口、弯道、道路交汇处改变运动方向，因此，轨迹规划装置需要在路口、弯道、道路交汇处设置关键点。除此之外，对于一些较长的道路，在道路的中间设置关键点有利于适应车辆加速、减速的情况。

具体地，轨迹规划装置基于沙盘建模的交通道路分布情况获取每个路口的进出口处、每个弯道的进出口处以及每条道路的边缘和中间位置在沙盘建模中的关键坐标。

S203：将关键坐标在建模对应的位置设置为关键点。

其中，轨迹规划装置基于关键坐标在沙盘建模上设置多个关键点，生成如图2所示的沙盘建模。

进一步地，轨迹规划装置还可以基于交通规则对生成的关键点进行排序。例如，在一条道路上，轨迹规划装置根据该道路的可行驶方向对道路上的关键点进行排序。因此，关键点的排列顺序在一定程度也直接反映了沙盘建模的交通规则。

S204：将机器人的当前位置和目的地位置之间的关键点两两之间组成轨迹段。

S205：将符合交通规则的轨迹段设置为连通段。

其中，轨迹规划装置将关键点两两之间组成，且邻接矩阵数值为1的线路设置为轨迹段，轨迹段为有向轨迹段。在连接多条轨迹段之间，轨迹规划装置还需要结合运行时间、交通道路分布情况进一步判断轨迹段是否符合交通规则，其中，交通规则包括单双号限行、按照时间限行等规则。当轨迹点符合交通规则时，轨迹规划装置将符合交通规则的轨迹段设置为连通段。

为了机器人在建模上点对点之间的路径规划，本申请提出了又一种轨迹规划方法，具体请参阅图6，图6是本申请提供的轨迹规划方法第三实施例的流程示意图。

如图6所示，本实施例的轨迹规划方法具体包括以下步骤：

S301：从建模获取轨迹信息和交通信息，其中，轨迹信息包括机器人的当前位置和目的地位置。

S302：基于交通信息在建模上设置多个关键点。

S303：基于交通信息在两两关键点之间形成连通段。

S304：采用预设算法搜索当前位置和目的地位置之间的所有连通段，按照搜索结果将连通段连接为机器人的目标轨迹路径。

通过上述步骤得到的目标轨迹路径在逻辑上合理可行，但是在实际运行中，还需要进一步考虑机器人的动力学模型。如图4生成的目标轨迹路径中，轨迹段之间有一定的夹角；当夹角较小时，由于机器人的动力学模型约束，在实际运行中，机器人很难完成按照目标轨迹路径运行，可能会偏离目标轨迹路径，影响后续的运行情况。因此，轨迹规划装置需要针对机器人的动力学模型对目标轨迹路径进行优化。机器人的动力学模型约束主要包含纵轴加速度和转弯半径，转弯半径可以等价为横轴加速度，轨迹规划装置需要保证目标规划路径的任意一段加速度都要符合最大加速度约束。具体请继续参阅以下步骤：

S305：获取机器人的动力学模型，其中，动力学模型包括机器人在每段连通段的加速度和最大速度。

S306：获取机器人在目标轨迹路径中每段连通段的运动时间以及运动向量。

其中，轨迹规划装置基于连通段的运动向量、机器人的加速度和最大速度获取该连通段中加速度段所占的比例，具体计算公式如下：

其中，ω为加速度段占整个连通段的比例，P为连通段的运动向量，a为加速度，v为最大速度。

其中，连通段的运动向量包括连通段的方向以及连通段的长度，连通段的方向表示机器人在该连通段的可运动方向。

轨迹规划装置进一步基于连通段的运动向量以及加速度段占连通段的比例获取连通段的速度系数，具体计算公式如下：

其中，λ为连通段的速度系数。

进一步地，轨迹规划装置获取每一连通段的其预期起始时间和预期终止时间，并根据连通段的预期起始时间和速度系数获取连通段的第一标准时间，具体计算公式如下：

其中，t₀为轨迹段的预期起始时间，t₁为轨迹段的第一标准时间。

轨迹规划装置基于连通段的第一标准时间、运动向量、速度系数和加速度段占连通段的比例确定第二标准时间，具体计算公式如下：

其中，t₂为连通段的第二标准时间。

S307：基于运动时间、第一标准时间和第二标准时间的大小，通过机器人在每段连通段的加速度和最大速度更新每段连通段的运动向量。

其中，轨迹规划装置根据机器人的运动时间、第一标准时间和第二标准时间的大小，对每段连通段的运动向量进行优化，具体优化公式如下：

其中，Ti(P，t)为连通段的运动路径。

当运动时间在起始时间和第一标准时间之间时，机器人处于加速状态，轨迹规划装置基于起始时间更新运动向量。

当运动时间在第一标准时间和第二标准时间之间时，机器人处于匀速状态，轨迹规划装置基于第一标准时间更新运动向量。

当运动时间在第二标准时间和终止时间之间时，机器人处于加速状态，轨迹规划装置基于终止时间更新运动向量。

最后，轨迹规划装置将更新后的轨迹段进行合并，得到如图7所示的完整轨迹路径。至此，轨迹规划装置完成了起始点和目的地点之间的轨迹规划，且生成的轨迹考虑到当前机器人的位置、速度、姿态，生成一条满足曲率连续性的光滑路径，可以根据此路径直接解算机器人在任意时刻的速度及控制配置，让机器人按照指定的控制策略运动即可完美地按照规定轨迹运行。

为实现上述实施例的轨迹规划方法，本申请还提出了一种轨迹规划装置，具体请参阅图8，图8是本申请提供的轨迹规划装置一实施例的结构示意图。

本实施例的轨迹规划装置400包括处理器41、存储器42、输入输出设备43以及总线44。

该处理器41、存储器42、输入输出设备43分别与总线44相连，该存储器42中存储有程序数据，处理器41用于执行程序数据以实现上述实施例所述的轨迹规划方法。

在本实施例中，处理器41还可以称为CPU(Central Processing Unit，中央处理单元)。处理器41可能是一种集成电路芯片，具有信号的处理能力。处理器41还可以是通用处理器、数字信号处理器(DSP)、专用集成电路(ASIC)、现场可编程门阵列(FPGA)或者其它可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。通用处理器可以是微处理器或者该处理器41也可以是任何常规的处理器等。

本申请还提供一种计算机存储介质，如图9所示，计算机存储介质500用于存储程序数据51，程序数据51在被处理器执行时，用以实现如上述实施例所述的轨迹规划方法。

本申请上述实施例所述的轨迹规划方法，在实现时以软件功能单元的形式存在并作为独立的产品销售或使用时，可以存储在装置中，例如一个计算机可读取存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)或处理器(processor)执行本发明各个实施方式所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。

以上所述仅为本申请的实施方式，并非因此限制本申请的专利范围，凡是利用本申请说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本申请的专利保护范围内。

Claims

1.一种轨迹规划方法，其特征在于，所述轨迹规划方法包括：

从建模获取轨迹信息和交通信息，其中，所述轨迹信息包括机器人的当前位置和目的地位置；

基于所述交通信息在所述建模上设置多个关键点；

基于所述交通信息在两两关键点之间形成连通段；

2.根据权利要求1所述的轨迹规划方法，其特征在于，

所述交通信息包括交通道路分布情况；

将所述关键坐标在所述建模对应的位置设置为所述关键点。

3.根据权利要求2所述的轨迹规划方法，其特征在于，

所述基于所述交通信息在所述建模上设置多个关键点的步骤，还包括：

基于所述交通信息获取所述建模中允许停泊的位置坐标；

将所述位置坐标在所述建模对应的位置设置为所述关键点。

4.根据权利要求2所述的轨迹规划方法，其特征在于，

所述交通信息包括交通规则；

将符合所述交通规则的轨迹段设置为所述连通段。

5.根据权利要求1所述的轨迹规划方法，其特征在于，

所述按照搜索结果将所述连通段连接为所述机器人的目标轨迹路径的步骤之后，所述轨迹规划方法还包括：

6.根据权利要求5所述的轨迹规划方法，其特征在于，

所述获取所述机器人在所述目标轨迹路径中每段连通段的运动时间以及运动向量的步骤之后，所述轨迹规划方法还包括：

获取所述连通段的起始时间和终止时间；

基于所述连通段的起始时间和速度系数确定第一标准时间；

7.根据权利要求6所述的轨迹规划方法，其特征在于，

所述基于所述运动时间、第一标准时间和第二标准时间的大小，通过所述机器人在每段所述连通段的加速度和最大速度更新每段所述连通段的运动向量的步骤，包括：

8.根据权利要求6所述的轨迹规划方法，其特征在于，

所述基于所述连通段的运动向量以及加速度段占所述连通段的比例获取所述连通段的速度系数的步骤，包括：

9.一种轨迹规划装置，其特征在于，所述轨迹规划装置包括存储器和处理器，其中，所述存储器与所述处理器耦接；

其中，所述存储器用于存储程序数据，所述处理器用于执行所述程序数据以实现如权利要求1～8任一项所述的轨迹规划方法。

10.一种计算机存储介质，其特征在于，所述计算机存储介质用于存储程序数据，所述程序数据在被处理器执行时，用以实现如权利要求1～8中任一项所述的轨迹规划方法。