CN115302520B

CN115302520B - 机器人路径的优化方法及装置、电子设备

Info

Publication number: CN115302520B
Application number: CN202211246426.6A
Authority: CN
Inventors: 张亮; 雷龙; 周心其; 熊伟成
Original assignee: Shenzhen Smart Mapping Tech Co ltd
Current assignee: Shenzhen Smart Mapping Tech Co ltd
Priority date: 2022-10-12
Filing date: 2022-10-12
Publication date: 2023-01-03
Anticipated expiration: 2042-10-12
Also published as: CN115302520A

Abstract

本发明公开了一种机器人路径的优化方法及装置、电子设备，其中，该方法包括：确定前端可行路径，并划分所述前端可行路径的待求解空间，其中，所述待求解空间为部分约束的轨迹路径范围；计算所述轨迹路径范围的预通行时间；采用预通行时间构建所述待求解空间的优化目标矩阵和约束矩阵，其中，所述优化目标矩阵用于表征轨迹优化的最佳指标，所述约束矩阵用于表征轨迹优化满足的约束条件；根据所述优化目标矩阵和所述约束矩阵在所述待求解空间中求解最优轨迹，并将所述最优轨迹确定为机器人的导航轨迹。通过本发明，解决了相关技术在复杂场景中求解光滑路径的效率低的技术问题，提高了特殊应用场景下的适用性和鲁棒性。

Description

机器人路径的优化方法及装置、电子设备

技术领域

本发明涉及计算机领域，具体而言，涉及一种机器人路径的优化方法及装置、电子设备。

背景技术

相关技术中，智能机器人，尤其是可移动智能机器人，其定位与导航功能是不可或缺基础且核心的功能。导航功能为机器人提供了移动的方案，使得机器人具备了快速到达目标点执行相应任务的能力。但该功能同时又会受到运动动力学，传动结构，电机能力的约束，因此机器人的移动方案，即轨迹规划，需要综合考虑到实际的物理和业务限制，才能为电机等执行机构提供合适的指令，让机器人具有优秀的行动表现。例如几乎在所有的实际使用中，机器人的避障功能都是至关重要的，机器人需要具备自然平滑的避开障碍物继续工作的能力，这主要包含两个要求：快速与满足运动学要求。机器人需要能够在确定障碍物后，快速的计算出新的轨迹规划，同时要考虑到此时机器人的朝向，速度，自身加减速能力与转向能力等约束，这在许多场景下都具有挑战性。又比如对于一些有最小转向半径的机器人来说，轨迹的曲率不能过大，否则机器人将会极大的偏离预定轨道，不仅对控制和定位等功能带来极大负担，也有较高的安全隐患。轨迹规划的目的即为规划出一条满足机器人动力学限制的可行移动方案，指导机器人在某时刻应当具备的姿态，速度，角速度等要求，这样才能够合理的到达目标点或者完成某种业务目的。

相关技术中，方法大致可以分为两种，一种是将轨迹光滑与速度规划分为两步处理，另一种则是按照机器人的运动学能力将机器人的控制命令离散化，通过不断迭代得到最终的轨迹方案。将轨迹规划分为两步的做法首先使用各种样条曲线拟合前端生成的可行路径，确定一种尽量满足物理学约束的光滑的路径，当路径已知后，路径在空间中的特性也被确定，基于这些特性，该做法确定在不同位置需要限制的速度，角速度等运动学参数，加之其他业务和实现的约束，得到适配该路径的速度规划，两者结合即为完整的轨迹规划。该方法的逻辑清晰，易于理解，但分为两步的做法先天具备求解速度不快的缺点。此外，对前端路径的光滑处理，很难预先考虑到整体速度的变化情况，往往只能考虑障碍物和曲率的限制，这极可能导致后期的速度规划难度上升，甚至出现无解的情况。尤其许多光滑路径的算法采用的是插值拟合的方法，只能在前端路径周围小范围内调整，而大多数前端路径仅考虑可通行性而不考虑可执行性，使得光滑处理对路径优化的效果十分有限。而速度规划如果基于已经固定的路径计算个轨迹点的时刻与合适的速度等参数，计算量较高和难度也不低。整体最终得到的轨迹规划效果却不一定良好。将机器人的控制指令离散化的方式，根据机器人的运动学能力，将机器人的转向和加减速的指令离散化为有限的个数，在整条路径构成的状态空间中确定合适的一套指令策略，使得机器人可以较为光滑且合适的到达目标点。但是显而易见的是，随着路径的到目标点距离的增加，求解的难度与运算量也会成指数级的上升，这是离散化的指令在整个状态空间中不断迭代的结果。即使如一些算法一般，采取在线计算的方式，即走一步算一步，也对硬件提出了更高的要求，且实时性不高。

针对相关技术中存在的上述问题，目前尚未发现有效的解决方案。

发明内容

本发明实施例提供了一种机器人路径的优化方法及装置、电子设备。

根据本申请实施例的一个方面，提供了一种机器人路径的优化方法，包括：确定前端可行路径，并划分所述前端可行路径的待求解空间，其中，所述待求解空间为部分约束的轨迹路径范围；计算所述轨迹路径范围的预通行时间；采用预通行时间构建所述待求解空间的优化目标矩阵和约束矩阵，其中，所述优化目标矩阵用于表征轨迹优化的最佳指标，所述约束矩阵用于表征轨迹优化满足的约束条件；根据所述优化目标矩阵和所述约束矩阵在所述待求解空间中求解最优轨迹，并将所述最优轨迹确定为机器人的导航轨迹。

根据本申请实施例的另一个方面，还提供了一种机器人路径的优化装置，包括：划分模块，用于确定前端可行路径，并划分所述前端可行路径的待求解空间，其中，所述待求解空间为部分约束的轨迹路径范围；计算模块，用于计算所述轨迹路径范围的预通行时间；构建模块，用于采用预通行时间构建所述待求解空间的优化目标矩阵和约束矩阵，其中，所述优化目标矩阵用于表征轨迹优化的最佳指标，所述约束矩阵用于表征轨迹优化满足的约束条件；确定模块，用于根据所述优化目标矩阵和所述约束矩阵在所述待求解空间中求解最优轨迹，并将所述最优轨迹确定为机器人的导航轨迹。

根据本申请实施例的另一方面，还提供了一种存储介质，该存储介质包括存储的程序，程序运行时执行上述的步骤。

根据本申请实施例的另一方面，还提供了一种电子设备，包括处理器、通信接口、存储器和通信总线，其中，处理器，通信接口，存储器通过通信总线完成相互间的通信；其中：存储器，用于存放计算机程序；处理器，用于通过运行存储器上所存放的程序来执行上述方法中的步骤。

本申请实施例还提供了一种包含指令的计算机程序产品，当其在计算机上运行时，使得计算机执行上述方法中的步骤。

通过本发明，确定前端可行路径，并划分前端可行路径的待求解空间，其中，待求解空间为部分约束的轨迹路径范围，计算轨迹路径范围的预通行时间，采用预通行时间构建待求解空间的优化目标矩阵和约束矩阵，其中，优化目标矩阵用于表征轨迹优化的最佳指标，约束矩阵用于表征轨迹优化满足的约束条件，根据优化目标矩阵和约束矩阵在待求解空间中求解最优轨迹，通过先划分部分约束的轨迹路径范围，然后基于预通行时间构建待求解空间的优化目标矩阵和约束矩阵，最后在在待求解空间中求解最优轨迹，并将最优轨迹确定为机器人的导航轨迹，划定待求解空间，并对最终轨迹的各物理属性直接进行限制，通过合理且灵活的空间划分方式和求解约束条件的构建，解决了相关技术在复杂场景中求解光滑路径的效率低的技术问题，提高了特殊应用场景下的适用性和鲁棒性。

附图说明

此处所说明的附图用来提供对本发明的进一步理解，构成本申请的一部分，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：

图1是本发明实施例的一种机器人的硬件结构框图；

图2是根据本发明实施例的一种机器人路径的优化方法的流程图；

图3是本发明实施例中离散路径点的示意图；

图4是本发明实施例生成新的可行路径的示意图；

图5是本发明实施例中第一矩形队列的示意图；

图6是本发明实施例中待求解空间的范围示意图；

图7是本发明实施例采用梯形估计法的速度时间图；

图8是本发明实施例中求解生成的最优轨迹的示意图；

图9是根据本发明实施例的一种机器人路径的优化装置的结构框图。

具体实施方式

为了使本技术领域的人员更好地理解本申请方案，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分的实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本申请保护的范围。需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。

需要说明的是，本申请的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本申请的实施例能够以除了在这里图示或描述的那些以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。

实施例1

本申请实施例一所提供的方法实施例可以在服务器、计算机、机器人或者类似的运算装置中执行。以运行在机器人上为例，图1是本发明实施例的一种机器人的硬件结构框图。如图1所示，机器人10可以包括一个或多个（图1中仅示出一个）处理器102（处理器102可以包括但不限于微处理器MCU或可编程逻辑器件FPGA等的处理装置）和用于存储数据的存储器104，可选地，上述机器人还可以包括用于通信功能的传输设备106以及输入输出设备108。本领域普通技术人员可以理解，图1所示的结构仅为示意，其并不对上述机器人的结构造成限定。例如，机器人10还可包括比图1中所示更多或者更少的组件，或者具有与图1所示不同的配置。

存储器104可用于存储机器人程序，例如，应用软件的软件程序以及模块，如本发明实施例中的一种机器人路径的优化方法对应的机器人程序，处理器102通过运行存储在存储器104内的机器人程序，从而执行各种功能应用以及数据处理，即实现上述的方法。存储器104可包括高速随机存储器，还可包括非易失性存储器，如一个或者多个磁性存储装置、闪存、或者其他非易失性固态存储器。在一些实例中，存储器104可进一步包括相对于处理器102远程设置的存储器，这些远程存储器可以通过网络连接至机器人10。上述网络的实例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。

传输装置106用于经由一个网络接收或者发送数据。上述的网络具体实例可包括机器人10的通信供应商提供的无线网络。在一个实例中，传输装置106包括一个网络适配器（Network Interface Controller，简称为NIC），其可通过基站与其他网络设备相连从而可与互联网进行通讯。在一个实例中，传输装置106可以为射频（Radio Frequency，简称为RF）模块，其用于通过无线方式与互联网进行通讯。

在本实施例中提供了一种机器人路径的优化方法，图2是根据本发明实施例的一种机器人路径的优化方法的流程图，如图2所示，该流程包括如下步骤：

步骤S202，确定前端可行路径，并划分前端可行路径的待求解空间，其中，待求解空间为部分约束的轨迹路径范围；

本实施例的前端可行路径是机器人理论可通行的区域，前端可行路径包括待求解空间。

步骤S204，计算轨迹路径范围的预通行时间；

可选的，轨迹路径范围包括多个路径段，每个路径段对应一个预通行时间。

步骤S206，采用预通行时间构建待求解空间的优化目标矩阵和约束矩阵，其中，优化目标矩阵用于表征轨迹优化的最佳指标，约束矩阵用于表征轨迹优化满足的约束条件；

步骤S208，根据优化目标矩阵和约束矩阵在待求解空间中求解最优轨迹，并将最优轨迹确定为机器人的导航轨迹。

本实施例的机器人可以是扫地机器人，工业机器人（如仓库，流水线等），在得到最优轨迹最后，机器人按照最优轨迹进行移动。

通过上述步骤，确定前端可行路径，并划分前端可行路径的待求解空间，其中，待求解空间为部分约束的轨迹路径范围，计算轨迹路径范围的预通行时间，采用预通行时间构建待求解空间的优化目标矩阵和约束矩阵，其中，优化目标矩阵用于表征轨迹优化的最佳指标，约束矩阵用于表征轨迹优化满足的约束条件，根据优化目标矩阵和约束矩阵在待求解空间中求解最优轨迹，通过先划分部分约束的轨迹路径范围，然后基于预通行时间构建待求解空间的优化目标矩阵和约束矩阵，最后在在待求解空间中求解最优轨迹，并将最优轨迹确定为机器人的导航轨迹，划定待求解空间，并对最终轨迹的各物理属性直接进行限制，通过合理且灵活的空间划分方式和求解约束条件的构建，解决了相关技术在复杂场景中求解光滑路径的效率低的技术问题，提高了特殊应用场景下的适用性和鲁棒性。

由于潜在轨迹与可行路径间具有相似性，两者状态空间近似，但考虑到机器人的运动学特性等约束，轨迹可能在可行路径周围较大范围内波动，因此前端的可行路径仅具备启发的作用，用于指导划分潜在的求解空间，并用于估计机器人经过该空间所需要的时间。

本实施例为了能将空间划分为较小的数量，且不损失较多的待求解空间，首先以正方形构建原空间，随后对这些正方形进行精简，膨胀与合并。划分求解空间的优势在于，可以通过调整求解空间几何形状直接约束最终轨迹的形状，控制最终轨迹的分布趋势。因此当一些场景具有特别的要求时，比如要求机器人在一定区域必须沿某一直线运动时，对求解空间的划分满足这些要求。

在这样的情形中，特定区域内的矩形将收缩为一条直线，这意味着在该处机器人仅能沿该直线运动。最终求得的轨迹的动力学性质也将满足这一要求。

在本实施例的一个实施方式中，划分前端可行路径的待求解空间包括：对前端可行路径进离散化处理，得到连续的离散路径点；分别生成以离散路径点为圆心的外接圆的正方形，其中，每个正方向对应一个路径段；将多个正方形占用的空间确定为前端可行路径的待求解空间。

可选的，在得到连续的离散路径点之后，还可以根据预设的必要约束条件（如是否允许碰撞，必须经过的位置，必须绕过的位置等）对离散路径点进行调整。

在本实施例的一个实施场景中，划分前端可行路径的待求解空间包括：

S11，对前端可行路径进离散化处理，得到连续的离散路径点；

在一个实例中，机器人需要尽量贴合一条划定的直线运动（比如某段墙面），目标段直线为预先已知。

首先前端算法将提供一条从起点到终点的可行路径，即前端可行路径，但往往它仅满足可通行性而不满足动力学约束和特殊要求的。本实施例先对该路径做离散化处理，等间距取路径点，如图3所示，图3是本发明实施例中离散路径点的示意图。

S12，遍历离散路径点上的每个路径点,将在目标段直线外侧且投影距离低于预设值的路径点投影到目标段上，以确定沿目标段运行的中间路径，其中，目标段直线为机器人的必要约束路径；

S13，在离散路径点的起点到中间路径起点之间生成第一路径，在中间路径的终点到离散路径点的终点之间生成第二路径；

本实施例对路径的离散化处理是便于后续的贴墙与生成矩形操作。随后遍历路径上的各点,将在墙面直线外侧且投影距离低于某值的路径点投影到墙面上，确定了需要沿墙面运行的一段路径。然后在起点到墙面路径（对应上述的中间路径）起点，墙面路径终点到终点之间生成新的路径。图4是本发明实施例生成新的可行路径的示意图。

S14，采用中间路径，第一路径和第二路径生成前端可行路径的待求解空间。

在一个实施方式中，采用中间路径，第一路径和第二路径生成前端可行路径的待求解空间包括：对第一路径和第二路径进行离散化处理，并生成以每个离散点为圆心，预定半径的圆为外接圆的正方形，得到第一矩形队列，沿中间路径生成小于第二预设值的目标矩形；去除第一矩形队列中重复连接的矩形，得到第二矩形队列；检测第二矩形队列中是否存在互相不连接且相邻的矩形对：若存在互相不连接且相邻的矩形对，向外膨胀矩形对中的两个矩形，直至互相连接，并采用膨胀后的矩形将第二矩形队列更新为第三矩形队列；检测目标矩形是否超出第三矩形队列的矩形块范围，若存在，修剪目标矩形的超出部分，将修剪后的目标矩形和第三矩形队列占用的轨迹路径范围确定为待求解空间。

将新的路径非贴墙段离散化处理，并生成以离散点为圆心，以到障碍物的距离或者某设定值中的较小值为半径的圆为外接圆的正方形。而将贴墙段生成宽度极窄的矩形，此为待求解空间的初始划分，图5是本发明实施例中第一矩形队列的示意图。

此时的待求解空间出现多个重叠，这将影响后续求解得到结果的简洁性，因此对现在这系列矩形做简化处理，去除重复连接的矩形。在去除重复连接的矩形后，检测是否存在可以合并为一个矩形的互相连接的两个矩形，若存在则将其合并。检测是否存在互相不连接的且在矩形队列中相邻的两个矩形：若存在，则两个矩形向彼此膨胀扩展，直至互相连接，若膨胀时由于障碍物不能连接，则生成一个新的矩形连接该矩形对。检测贴墙路径段是否超出相邻的矩形块范围，超出则进行修剪，最后待求解空间如图6所示，图6是本发明实施例中待求解空间的范围示意图，大部分矩形块与地图坐标系平行，而贴墙直线与地图坐标系不平行。

为了在后续的最优化问题中计算轨迹的速度与加速度值，仅有空间的参数而缺乏时间时无法做到的，因此需要对各轨迹所用的时间进行估计。本方法使用梯形估计法对每段轨迹的时间进行估算。图7是本发明实施例采用梯形估计法的速度时间图，如图7所示在梯形时间估计中，将速度的变化视为由起点从零匀加速到某个值，然后以该值匀速运动一段距离，随后匀减速到到零，抵达路径终点。而路径的长度则设定为待求解空间，即相应矩形块的对角线长度。

在本实施例的一个实施方式中，计算轨迹路径范围的预通行时间包括：

S21，针对轨迹路径范围的每个路径段，计算路径段的路径长度L，其中，轨迹路径范围包括若干条依次邻接的路径段；

S22，确定机器人的限制速度v_max和限制加速度a_max；

S23，采用以下公式分别计算机器人的最大预估速度v_max ^*和最大预估加速度a_max ^*：v_max ^*=m*v_max，a_max ^*=n*a_max，其中，0＜m＜1，0＜n＜1；

S24，采用最大预估速度和最大预估加速度计算在路径段上加速到最大预估速度所用的加速时间t₁；

S25，采用加速时间计算机器人的最大加速距离；

S26，根据最大加速距离和路径长度计算路径段的预通行时间。

可选的，m=0.7， n=0.7，按造机器人的物理属性，设定时间估计中的最大预估加速度与最大预估速度；

计算过程如下：v_max ^*=0.7*v_max；

a_max ^*=0.7*a_max

v_max ^*为时间估计计算中的最大预估速度，v_max为机器人实际的速度限制。

a_max ^*为时间估计计算中的最大预估加速度，a_max为机器人实际的加速度限制。

t₁为加速到最大预估速度所用的时间。

s1为机器人由静止加速到最大预估速度需要的距离。

在一个示例中，根据最大加速距离和路径长度计算路径段的预通行时间包括：判断路径长度L是否小于2*最大加速距离s₁；

若路径长度L小于2*最大加速距离s₁，采用以下公式计算路径段的预通行时间t_sum:

若路径长度L大于或等于2*最大加速距离s₁，采用以下公式计算路径段的预通行时间t_sum:

。

在本实施例中，将轨迹规划问题转化为最优化问题，将待求解的轨迹以贝塞尔曲线的形式描述，通过求解贝塞尔曲线的相关参数，确定满足约束条件的轨迹规划。整个过程大致可以分为两个部分：构建优化目标矩阵，即轨迹需要满足的一个最佳指标；构建约束矩阵，即在最优化求解过程中，各变量需要满足的诸多限制条件，比如连续性约束，速度加速度约束等。在此先介绍贝塞尔曲线在本实施例中的应用和性质。

本实施例采用的贝塞尔曲线为样条曲线的一种，可以由如下的公式所表述。在二维的轨迹规划中，需要分别在x轴与y轴方向构建两条贝塞尔曲线，两者结合即为最终的二维贝塞尔曲线。

贝塞尔曲线由数个控制点确定，这些控制点确定了贝塞尔曲线的各项性质。对于一个n阶贝塞尔曲线，其拥有n+1个控制点，以{P₀,P₁,…,P_n}表示。

则对该n阶贝塞尔曲线上的任意一个点B(u):

其中，

为多项式系数公式，u∈[0,1],u为一个无量纲变量。

可以发现，贝塞尔曲线一定经过第一和最后一个控制点。

贝塞尔曲线的导数依然是贝塞尔曲线，但阶数将下降一阶。因此对于以n阶贝塞尔曲线描述位置的轨迹，其速度与加速度分别是n-1阶与n-2阶贝塞尔曲线。当已知整条轨迹的时间T时，位置，速度与加速度的控制点可由下表达：

贝塞尔曲线还具备凸包的性质，即贝塞尔曲线一定处与控制点构成的几何中，而不会超出控制点的限制。这为后续的最优化求解提供了方便，计算得的解一定是全局最优解，而非局部最优。

此外，各阶次的贝塞尔曲线可以转换为相应阶次的多项式曲线，贝塞尔曲线的控制点{P₀,P₁,…,P_n}与多项式曲线的各阶系数{D₀,D₁,…,D_n}的关系可以由如下公式转换：

其中D为多项式曲线的各阶系数向量，P为贝塞尔曲线控制点向量，M为（n+1,n+1）的转换矩阵，其元素为：

其中， 0<=j<=i<=n+1;

如此可实现多项式曲线各项系数与贝塞尔曲线控制点间相互转化。

机器人的最优化目标可以是单一目标，比如在自动驾驶技术中为了使得驾驶舒适度最佳，通常希望加加速度的积分最小。而在一些光滑算法中，则会综合考虑到障碍物的距离，光滑程度等多个优化目标，对不同的参数使用不同的权重以调整该参数在整体优化目标中的比重。

在本实施例的一方面，采用预通行时间构建待求解空间的优化目标矩阵包括：确定轨迹路径范围的每个路径段的目标优化指标，并采用若干个目标优化指标构建系数矩阵，其中，系数矩阵的阶数与目标优化指标的项数相同；基于系数矩阵构建系数矩阵的多项式曲线表达式；计算多项式曲线表达式的k阶导数，其中，k为大于0的整数；采用多项式曲线表达式和k阶导数计算得到优化目标函数；将预通行时间确定为多项式曲线的总时间，基于总时间将优化目标函数中多项式曲线转换为相应阶次的贝塞尔曲线，得到贝塞尔控制点矩阵和转换矩阵，其中，贝塞尔控制点矩阵中的控制点为最优轨迹的待求解变量，贝塞尔曲线的轨迹与最优轨迹相同；采用贝塞尔控制点矩阵和转换矩阵构建优化目标矩阵。

本实施例包括多段贝塞尔曲线，且一个路径段对应的矩形空间对应一段贝塞尔曲线。

在此仅以单一目标为例，多优化目标可以很容易的由多个单一目标组合而成。以一条n阶多项式曲线为例，其系数矩阵为{D₀,D₁,…,D_n}。

则多项式曲线表达为:

其k阶导数表达为：

则优化目标函数，和优化目标函数的矩阵表达式分别为：

（i,l）∈[0,n]

T为该多项式曲线总时间;

为系数矩阵的转置矩阵。

上式可简化表达为：

引入贝塞尔控制点矩阵P，与转换矩阵M。

本实施例使用加速度的积分为最优化目标，则取求导阶数为2,得到的轨迹的加速度积分为全局最小。使用贝尔曲线的控制点坐标为待求解变量，构建相应的优化目标矩阵。

本实施例以贝塞尔曲线的控制点为待求解变量，则参照上述的速度与加速度公式可以使用待求解变量表达相应的位置，速度与加速度贝塞尔曲线的控制点，根据凸包性质，若这些控制点不超出相应的物理限制，实际的位置，速度与加速度也会满足这些限制。

在本实施例的另一方面，采用预通行时间构建待求解空间的约束矩阵包括：为轨迹路径范围的每个路径段构建位置矩阵，速度矩阵，以及加速度矩阵；分别设置位置矩阵，速度矩阵，以及加速度矩阵的上界值和下界值，其中，上界值和下界值用于计算最优轨迹的位置参数，速度参数，加速度参数；为轨迹路径范围的每个路径段构建位置连续性矩阵，速度连续性矩阵，以及加速度连续性矩阵，其中，连续性矩阵用于描述相邻两个路径段在连接点处的位置，速度，加速度的参数相等；将位置矩阵，速度矩阵，加速度矩阵，位置连续性矩阵，速度连续性矩阵，加速度连续性矩阵确定为优化目标矩阵的约束矩阵。

则某条贝塞尔的位置，各参数可用如下矩阵表达为：

[pos]为位置矩阵，[P]为控制点矩阵,T为该段轨迹的估计时间，R为该矩形块的旋转矩阵。旋转矩阵根据矩形块与坐标轴的角度来构建，以矩形块与坐标轴的角度为θ为例，旋转矩阵如下：

[vel]为速度矩阵：

[acc]为加速度矩阵：

分别为各段轨迹的位置速度加速度矩阵设置上界与下界则可以限制最终求得的轨迹的位置，速度与加速度参数。

为了满足各贝塞尔曲线在连接点处的连续性，两条贝塞尔曲线在连接点处的位置，速度，加速度的参数也必须相等。则存在位置连续性矩阵如下：

其中，T_j为第j段轨迹的时间估计，p_j|n为第j段轨迹的第n个控制点,R_j为第j段轨迹的旋转矩阵。

速度连续性矩阵如下：

加速度连续性矩阵如下：

在本实施例中，根据所述优化目标矩阵和所述约束矩阵在所述待求解空间中求解最优轨迹包括：将所述优化目标矩阵和所述约束矩阵代入求解器中进行求解。使用规划问题求解器求解，根据前文描述的数学关系，整体的最优化问题的求解式表述为：

s.t[矩阵1]_min≤[矩阵2] [矩阵3] ≤[矩阵4]_max。

其中，[矩阵1]如下：

其中，[矩阵2]如下：

其中，[矩阵3]如下：

其中，[矩阵4]如下：

优化目标为贝塞尔曲线二阶导数的积分，约束条件为位置，速度与加速度的限制，待求解变量为贝塞尔曲线的控制点系数。利用OSQP（Operator Splitting QuadraticProgram，二次规划的算子分裂）求解器或其他求解器求解，即可得到满足各约束的轨迹。

图8是本发明实施例中求解生成的最优轨迹的示意图，可以发现最终轨迹满足贴墙的特殊要求。

本实施例提出了一种在复杂环境中通过划分不同的待求解空间，使用贝塞尔曲线描述潜在轨迹并建立相应数学模型，最后使用最优化问题直接求解得到轨迹规划的方法。本实施例对于最优轨迹包括：使用待求解空间划分算法划分环境；随后估计各条路径的时间；接着利用贝塞尔曲线构建最优化问题和约束条件；最后求解该最优化问题，得到最优的轨迹规划。

提出了一种划分待求解空间的方法，以将待求解空间划分为数目最少，互相连接，形状合理的数个矩形块为目标，使用这些不同方向与大小的矩形块构建后续最优化问题的部分约束，以此满足实际的限制。提出一种以贝塞尔曲线描述最终轨迹，构建最优化的方法。该方法以轨迹的一种或多种属性为最优化目标，得到可以描述最终轨迹的贝塞尔曲线的相关系数。提出一种构建最优化问题限制的描述方法。该方法可以限制轨迹点处于不同方向的矩形块中，也可以限制轨迹点处于不同的直线上，以生成满足如空间限制等一些特别要求的轨迹。

在本实施例中，对于待求解空间的划分也可以使用其他思路，比如不对所有的离散点生成正方形，而是先对某一个点生成正方形，随后向下一个点膨胀，当不能继续膨胀时，在矩形外的下一个路径点重复这个过程，最终得到一系列矩形作为待求解空间。只要是沿着前端生成的可行路径划分空间的方法，均可以替代上述实施例列举的方法，只是效率上有所不同。此外，也可以通过将轨迹描述由贝塞尔曲线转换为多项式曲线的形式，将最优化问题中的待求解变量由贝塞尔曲线的控制点转换为多项式曲线的系数。这个方法的底层数学逻辑是一致的，两个最优化问题是等价的。但是以多项式系数作为待求解变量，会导致在最优化问题的描述中引入两次矩阵的求逆操作，同时在构建限制矩阵时，需要增加额外的转化过程。最后，通过修改限制矩阵，可以引入其他的限制条件，或者改同一种限制的表达形式。比如限制贝塞尔曲线控制点在某一条射线上滑动，这其实也是例子中贴墙的一种变换，实际上是一样的。

一些轨迹规划将速度规划与轨迹光滑分开处理，这会降低求解的速度。而一些轨迹规划将可行的动作空间离散为有限的数量，却会导致整体运算量的指数上升，一方面速度较慢，一方面对硬件有额外的要求，但最终的轨迹结果却无法保证满足各项物理要求。同时上述的轨迹规划，一般只针对于得到可行轨迹，并不具备适配其他业务需求的拓展性。

本实施例的方案可以通过合理且灵活的空间划分方式和对规划问题相应数学模型的构建，解决了在复杂场景中同时得到光滑路径与速度规划的问题，同时该发明对一些有着特殊需求的应用场景下有着适用性。

本实施例将速度规划于轨迹光滑同时处理，提高了求解的速度，同时将待求解空间划分为有数的连续矩形块，降低了运算量和算法对于硬件的需求，显著提高了运算速度。此外，因为可以人为划定待求解空间，并通过限制矩阵的方式对最终轨迹的各物理属性直接进行限制，使得本方法对于其他业务的适应力强，实用性也更强。

通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到根据上述实施例的方法可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件，但很多情况下前者是更佳的实施方式。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质（如ROM/RAM、磁碟、光盘）中，包括若干指令用以使得一台终端设备（可以是手机，计算机，服务器，或者网络设备等）执行本发明各个实施例所述的方法。

实施例2

在本实施例中还提供了一种机器人路径的优化装置，用于实现上述实施例及优选实施方式，已经进行过说明的不再赘述。如以下所使用的，术语“模块”可以实现预定功能的软件和/或硬件的组合。尽管以下实施例所描述的装置较佳地以软件来实现，但是硬件，或者软件和硬件的组合的实现也是可能并被构想的。

图9是根据本发明实施例的一种机器人路径的优化装置的结构框图，如图9所示，该装置包括：分割模块90，确定模块92，生成模块94，连接模块96，其中，

划分模块90，用于确定前端可行路径，并划分所述前端可行路径的待求解空间，其中，所述待求解空间为部分约束的轨迹路径范围；

计算模块92，用于计算所述轨迹路径范围的预通行时间；

构建模块94，用于采用预通行时间构建所述待求解空间的优化目标矩阵和约束矩阵，其中，所述优化目标矩阵用于表征轨迹优化的最佳指标，所述约束矩阵用于表征轨迹优化满足的约束条件；

确定模块96，用于根据所述优化目标矩阵和所述约束矩阵在所述待求解空间中求解最优轨迹，并将所述最优轨迹确定为机器人的导航轨迹。

需要说明的是，上述各个模块是可以通过软件或硬件来实现的，对于后者，可以通过以下方式实现，但不限于此：上述模块均位于同一处理器中；或者，上述各个模块以任意组合的形式分别位于不同的处理器中。

实施例3

本发明的实施例还提供了一种存储介质，该存储介质中存储有计算机程序，其中，该计算机程序被设置为运行时执行上述任一项方法实施例中的步骤。

可选地，在本实施例中，上述存储介质可以被设置为存储用于执行以下步骤的计算机程序：

S1，确定前端可行路径，并划分所述前端可行路径的待求解空间，其中，所述待求解空间为部分约束的轨迹路径范围；

S2，计算所述轨迹路径范围的预通行时间；

S3，采用预通行时间构建所述待求解空间的优化目标矩阵和约束矩阵，其中，所述优化目标矩阵用于表征轨迹优化的最佳指标，所述约束矩阵用于表征轨迹优化满足的约束条件；

S4，根据所述优化目标矩阵和所述约束矩阵在所述待求解空间中求解最优轨迹，并将所述最优轨迹确定为机器人的导航轨迹。

可选地，在本实施例中，上述存储介质可以包括但不限于：U盘、只读存储器（Read-Only Memory，简称为ROM）、随机存取存储器（Random Access Memory，简称为RAM）、移动硬盘、磁碟或者光盘等各种可以存储计算机程序的介质。

本发明的实施例还提供了一种电子设备，包括存储器和处理器，该存储器中存储有计算机程序，该处理器被设置为运行计算机程序以执行上述任一项方法实施例中的步骤。

可选地，上述电子设备还可以包括传输设备以及输入输出设备，其中，该传输设备和上述处理器连接，该输入输出设备和上述处理器连接。

可选地，在本实施例中，上述处理器可以被设置为通过计算机程序执行以下步骤：

S2，计算所述轨迹路径范围的预通行时间；

可选地，本实施例中的具体示例可以参考上述实施例及可选实施方式中所描述的示例，本实施例在此不再赘述。

上述本申请实施例序号仅仅为了描述，不代表实施例的优劣。

以上所述仅是本申请的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本申请原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本申请的保护范围。

Claims

1.一种机器人路径的优化方法，其特征在于，包括：

确定前端可行路径，并划分所述前端可行路径的待求解空间，其中，所述待求解空间为部分约束的轨迹路径范围；

计算所述轨迹路径范围的预通行时间；

采用预通行时间构建所述待求解空间的优化目标矩阵和约束矩阵，其中，所述优化目标矩阵用于表征轨迹优化的最佳指标，所述约束矩阵用于表征轨迹优化满足的约束条件；

根据所述优化目标矩阵和所述约束矩阵在所述待求解空间中求解最优轨迹，并将所述最优轨迹确定为机器人的导航轨迹。

2.根据权利要求1所述的方法，其特征在于，划分所述前端可行路径的待求解空间包括：

对所述前端可行路径进离散化处理，得到连续的离散路径点；

分别生成以所述离散路径点为圆心的外接圆的正方形，其中，每个正方形对应一个路径段；

将多个正方形占用的空间确定为所述前端可行路径的待求解空间。

3.根据权利要求1所述的方法，其特征在于，划分所述前端可行路径的待求解空间包括：

遍历所述离散路径点上的每个路径点,将在目标段直线外侧且投影距离低于预设值的路径点投影到所述目标段直线上，以确定沿所述目标段直线运行的中间路径，其中，所述目标段直线为所述机器人的必要约束路径；

在所述离散路径点的起点到所述中间路径起点之间生成第一路径，在所述中间路径的终点到所述离散路径点的终点之间生成第二路径；

采用所述中间路径，所述第一路径和所述第二路径生成所述前端可行路径的待求解空间。

4.根据权利要求3所述的方法，其特征在于，采用所述中间路径，所述第一路径和所述第二路径生成所述前端可行路径的待求解空间包括：

对所述第一路径和所述第二路径进行离散化处理，并生成以每个离散点为圆心，预定半径的圆为外接圆的正方形，得到第一矩形队列，沿所述中间路径生成小于第二预设值的目标矩形；

去除所述第一矩形队列中重复连接的矩形，得到第二矩形队列；

检测所述第二矩形队列中是否存在互相不连接且相邻的矩形对：若存在互相不连接且相邻的矩形对，向外膨胀所述矩形对中的两个矩形，直至互相连接，并采用膨胀后的矩形将所述第二矩形队列更新为第三矩形队列；

检测所述目标矩形是否超出所述第三矩形队列的矩形块范围，若存在，修剪所述目标矩形的超出部分，将修剪后的目标矩形和所述第三矩形队列占用的轨迹路径范围确定为所述待求解空间。

5.根据权利要求1所述的方法，其特征在于，计算所述轨迹路径范围的预通行时间包括：

针对轨迹路径范围的每个路径段，计算路径段的路径长度L，其中，所述轨迹路径范围包括若干条依次邻接的路径段；

确定所述机器人的限制速度v_max和限制加速度a_max；

采用以下公式分别计算所述机器人的最大预估速度v_max ^*和最大预估加速度a_max ^*：v_max ^*=m*v_max，a_max ^*=n*a_max，其中，0＜m＜1，0＜n＜1；

采用所述最大预估速度和所述最大预估加速度计算在所述路径段上加速到所述最大预估速度所用的加速时间t₁；

采用所述加速时间计算所述机器人的最大加速距离；

根据所述最大加速距离和所述路径长度计算所述路径段的预通行时间。

6.根据权利要求5所述的方法，其特征在于，根据所述最大加速距离和所述路径长度计算所述路径段的预通行时间包括：

判断所述路径长度L是否小于2*最大加速距离s₁；

若所述路径长度L小于2*最大加速距离s₁，采用以下公式计算所述路径段的预通行时间t_sum:

若所述路径长度L大于或等于2*最大加速距离s1，采用以下公式计算所述路径段的预通行时间t_sum:

。

7.根据权利要求1所述的方法，其特征在于，采用预通行时间构建所述待求解空间的优化目标矩阵包括：

确定所述轨迹路径范围的每个路径段的目标优化指标，并采用若干个所述目标优化指标构建系数矩阵，其中，所述系数矩阵的阶数与所述目标优化指标的项数相同；

基于所述系数矩阵构建所述系数矩阵的多项式曲线表达式；

计算所述多项式曲线表达式的k阶导数，其中，k为大于0的整数；

采用所述多项式曲线表达式和所述k阶导数计算得到优化目标函数；

将所述预通行时间确定为所述多项式曲线的总时间，基于所述总时间将所述优化目标函数中多项式曲线转换为相应阶次的贝塞尔曲线，得到贝塞尔控制点矩阵和转换矩阵，其中，所述贝塞尔控制点矩阵中的控制点为最优轨迹的待求解变量，所述贝塞尔曲线的轨迹与所述最优轨迹相同；

采用所述贝塞尔控制点矩阵和所述转换矩阵构建优化目标矩阵。

8.根据权利要求1所述的方法，其特征在于，采用预通行时间构建所述待求解空间的约束矩阵包括：

为所述轨迹路径范围的每个路径段构建位置矩阵，速度矩阵，以及加速度矩阵；

分别设置所述位置矩阵，所述速度矩阵，以及所述加速度矩阵的上界值和下界值，其中，所述上界值和所述下界值用于计算所述最优轨迹的位置参数，速度参数，加速度参数；

为所述轨迹路径范围的每个路径段构建位置连续性矩阵，速度连续性矩阵，以及加速度连续性矩阵，其中，所述连续性矩阵用于描述相邻两个路径段在连接点处的位置，速度，加速度的参数相等；

将所述位置矩阵，所述速度矩阵，所述加速度矩阵，所述位置连续性矩阵，所述速度连续性矩阵，所述加速度连续性矩阵确定为所述优化目标矩阵的约束矩阵。

9.一种机器人路径的优化装置，其特征在于，包括：

划分模块，用于确定前端可行路径，并划分所述前端可行路径的待求解空间，其中，所述待求解空间为部分约束的轨迹路径范围；

计算模块，用于计算所述轨迹路径范围的预通行时间；

构建模块，用于采用预通行时间构建所述待求解空间的优化目标矩阵和约束矩阵，其中，所述优化目标矩阵用于表征轨迹优化的最佳指标，所述约束矩阵用于表征轨迹优化满足的约束条件；

确定模块，用于根据所述优化目标矩阵和所述约束矩阵在所述待求解空间中求解最优轨迹，并将所述最优轨迹确定为机器人的导航轨迹。

10.一种电子设备，包括处理器、通信接口、存储器和通信总线，其中，处理器，通信接口，存储器通过通信总线完成相互间的通信；其中：

存储器，用于存放计算机程序；

处理器，用于通过运行存储器上所存放的计算机程序来执行权利要求1至8中任一项所述的方法步骤。