CN116540748A - 一种在导航路径上规划机器人绕行路径的方法及系统 - Google Patents

Abstract

本发明提供了一种在导航路径上规划机器人绕行路径的方法及系统，该方法步骤包括：步骤S100建立绕行规则判断程序，其中绕行规则包括：碰撞预测判断、车辆运动学约束判断、障碍物安全距离判断规则；设置机器人在行进方向上的预警视距；步骤S200当触发任一绕行规则时，以预警视距两端在当前行驶路径上的位置点为绕行起止点，使用A*算法获取绕行的初始路径；步骤S300对初始路径进行包括离障距离、路径平滑、绕行起止点连续性中至少一种优化后，进行拟合，生成绕行路径，替换预警视距内的行驶路径。籍此以弥补A*算法不考虑车辆运动学约束的缺点，使其能规划出适于机器人行驶的避障绕行路径。

Description

一种在导航路径上规划机器人绕行路径的方法及系统

技术领域

本发明涉及机器人路径规划导航技术，尤其涉及一种在导航路径上规划机器人绕行路径的方法及系统。

背景技术

在移动机器人导航过程中，特别是执行一些搬运任务的工业移动机器人(AGV)的导航过程中，首先需要根据感知到的周围环境，规划出移动机器人可以执行任务的路径。同时在机器人沿着规划出的路径行驶时，由于周围可能会存在障碍物，并且障碍物的位置也可能是实时变化的。因此为了完成移动机器人的任务，并且不撞到障碍物，则需要移动机器人能够根据原始执行任务的信息，生成可以绕开障碍物的新路径。

为此现有技术曾提供了多种绕障路径的规划技术，例如：TEB(Time ElasticBand)路径规划技术、DWA(Dynamic Window Approach)路径规划技术、A*（A-star）路径搜索技术。其中A*方案相较其他两种方案，计算较为简单，并可以在很短的时间内，搜索出一条绕开障碍物的路径，因此更适用于实时避障的绕行路径规划。

然而A*算法的缺陷在于，其搜索出的路径，有时会是一条不考虑车辆运动学约束的离散路径，因此该方案实际规划出的绕障路径，实践中有一定几率可能会导致机器人失控。

发明内容

为此，本发明的主要目的在于提供一种在导航路径上规划机器人绕行路径的方法及系统，以弥补A*算法不考虑车辆运动学约束的缺点，使其能规划出适于机器人行驶的避障绕行路径。

为了实现上述目的，根据本发明的一个方面，提供了一种在导航路径上规划机器人绕行路径的方法，步骤包括：

步骤S100建立绕行规则判断程序，其中绕行规则包括：碰撞预测判断、车辆运动学约束判断、障碍物安全距离判断规则；设置机器人在行进方向上的预警视距；

步骤S200当触发任一绕行规则时，以预警视距两端在当前行驶路径上的位置点为绕行起止点，使用A*算法获取绕行的初始路径；

步骤S300对初始路径进行包括离障距离、路径平滑、绕行起止点连续性中至少一种优化后，进行拟合，生成绕行路径，替换预警视距内的行驶路径。

在可能的优选实施方式中，所述在导航路径上规划机器人绕行路径的方法，其中步骤还包括：

步骤S400每间隔预设时间，重复执行步骤S200-S300获取新绕行路径P2；

步骤S500计算新绕行路径P2与原绕行路径P1的代价函数；

步骤S600当判断时，将新绕行路径替换原绕行路径， 其中q为预设的路径质量系数；

其中代价函数的计算步骤包括：

步骤S510分别计算P1、P2的曲率波动值1和2、最大曲率值和、平均曲率值和、路径上的点与障碍物之间的最小 距离和；

步骤S520计算P1的代价函数

计算P2的代价函数

其中分别为最大曲率系数、平均曲率系数、曲率波动系数、障碍 物系数。

在可能的优选实施方式中，其中所述预警视距包括：前、中、后三段连续视距，所述碰撞预测判断步骤包括：

步骤S110判断行驶路径上预警视距中，是否仅中段存在障碍物，若为是则视为触发绕行规则。

在可能的优选实施方式中，其中车辆运动学约束判断步骤包括：

步骤S120计算预警视距内的当前行驶路径中，每个路径点的曲率和曲率半径；

步骤S130遍历整条路径，获取最大曲率和最小曲率半径，当判断最大曲率超过阈值、或最小曲率半径小于机器人允许的最小转弯半径中任一时，视为触发绕行规则。

在可能的优选实施方式中，其中障碍物安全距离判断步骤包括：

步骤S140计算预警视距内的当前行驶路径中，每个路径点与障碍物的距离；

步骤S150遍历整条路径，获取最小距离，当判断最小距离超过阈值时，视为触发绕行规则。

在可能的优选实施方式中，其中步骤S300中路径平滑优化步骤包括：

步骤S310设初始路径上第个路径点的坐标为其中为 路径点的个数，对初始路径上的路径点，设计以下路径平滑优化代价函数并求解：

。

在可能的优选实施方式中，其中步骤S300中离障距离优化步骤包括：

步骤S320设初始路径周围第个障碍物的信息为其中为障碍物的个数，对初始路径上的路径点，设计以下离障距离优化代价函数并求解：

。

在可能的优选实施方式中，其中步骤S300中绕行起止点连续性优化步骤包括：

步骤S330分别在行驶路径上靠近绕行起止点之后，设置个靠近点；

步骤S340设第个靠近点的信息为其中为靠近点的 个数，对初始路径上的路径点，设计以下绕行起止点连续性优化代价函数并求解：

。

在可能的优选实施方式中，其中步骤S300中进行离障距离、路径平滑及绕行起止点连续性优化的步骤包括：

步骤S350对初始路径上的路径点，设计以下综合优化代价函数并求解：

；

其中∝为平滑系数，β为障碍物系数，γ为靠近系数。

为了实现上述目的，对应上述方法，本发明另一方面，还提供了一种在导航路径上规划机器人绕行路径的系统，其包括：

存储单元，用于存储包括如上述中任一所述在导航路径上规划机器人绕行路径的方法步骤的程序，以供检测单元，处理单元，导航单元适时调取执行；

检测单元，用于检测及获取机器人行驶路径上的障碍物信息，并判断其是否存在于预警视距内；

导航单元，用于提供行驶路径信息；

处理单元，用于根据绕行规则以预警视距两端在当前行驶路径上的位置点为绕行起止点，使用A*算法获取绕行的初始路径，然后对初始路径进行包括离障距离、路径平滑、绕行起止点连续性的综合优化后，进行拟合，生成绕行路径，以令导航单元替换预警视距内的行驶路径；

处理单元，还用于每间隔预设时间，循环计算获取新绕行路径P2，并计算新绕行路 径P2与原绕行路径P1的代价函数，当判断时，令 导航单元将新绕行路径替换原绕行路径。

通过本发明提供的该在导航路径上规划机器人绕行路径的方法及系统，能够持续对导航路径以及规划后的避障绕行路径进行包括：碰撞预测、车辆运动学约束、障碍物安全距离在内的判断，从而确保A*算法能够自我修正，以规划出适于机器人行驶的避障绕行路径，避免机器人失控。

此外考虑到即便是已经规划出的绕行路径，有时也未必是最优路径，因此在对应示例中，本案特别提出了路径代价这一评价路径质量的方案，并通过预设时间间隔来进行新绕行路径规划，以与原绕行路径进行路径代价评价，从而根据路径质量来决定是否将原绕行路径与新绕行路径进行替换，从而提升机器人的行驶效率。

附图说明

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

图1为本发明的在导航路径上规划机器人绕行路径的方法的步骤示意图；

图2为本发明的在导航路径上规划机器人绕行路径的方法的逻辑示意图；

图3为本发明的在导航路径上规划机器人绕行路径的方法中预警视距的示意图；

图4为本发明的在导航路径上规划机器人绕行路径的方法中绕行路径起止点的示意图；

图5为本发明的在导航路径上规划机器人绕行路径的方法中A*算法计算的初始路径的示意图；

图6为本发明的在导航路径上规划机器人绕行路径的方法中初始路径经过综合优化后的示意图；

图7为本发明的在导航路径上规划机器人绕行路径的方法中优化后的初始路径进行拟合后的示意图；

图8为本发明的在导航路径上规划机器人绕行路径的方法中碰撞预测判断的示意图；

图9为本发明的在导航路径上规划机器人绕行路径的方法中车辆运动学约束判断的示意图；

图10为本发明的在导航路径上规划机器人绕行路径的方法中障碍物安全距离判断的示意图；

图11为本发明的在导航路径上规划机器人绕行路径的方法中路径质量评价的示意图；

图12为本发明的在导航路径上规划机器人绕行路径的系统的结构示意图。

具体实施方式

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

此外本发明的说明书和权利要求书及附图中的术语“第一”、“第二”、“S100”、“S200”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本发明的实施例能够以除了在这里描述的那些以外的顺序实施。同时本发明中的术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含。除非另有明确的规定和限定，术语“设置”、“布设”、“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以根据具体情况，结合现有技术来理解上述术语在本案中的具体含义。

请参阅图1至图11所示，本发明提供的该在导航路径上规划机器人绕行路径的方法，其步骤包括：

步骤S100建立绕行规则判断程序，其中绕行规则包括：碰撞预测判断、车辆运动学约束判断、障碍物安全距离判断规则；设置机器人在行进方向上的预警视距。

具体的，如图3所示，本示例中该预警视距设计为包括：前L1、中L2、后L3三段连续视距，其作用在于，通过该视距划分出对应的路径段长度，例如L1段路径的作用是给机器人平滑安全的过渡到绕行路径预留空间，L2段路径较L1段路径和L3段路径长，可以有效的检测障碍物，并预留路径的绕行规划空间，L3段路径的作用是为了保证机器人可以按原始路径到达绕行路径的终点。通过该预警视距的设计，可为后续绕行规则的判断提供依据。

进一步的，其中该碰撞预测判断步骤包括：

步骤S110当判断预警视距中，L1段路径和L3段路径不存在障碍物，但L2段路径存在障碍物时，则视为触发绕行规则。

进一步的，其中该车辆运动学约束判断步骤包括：

步骤S120计算预警视距内的当前行驶路径中，每个路径点的曲率和曲率半径。

步骤S130遍历整条路径，获取最大曲率和最小曲率半径，当判断最大曲率超过阈值、或最小曲率半径小于机器人允许的最小转弯半径中任一时，视为触发绕行规则。

例如，设规划生成的初始路径点信息可用参数方程，据此可以计算 出每一路径点对应的一阶导数，二阶导数,曲率,和曲率半径的信息：

因此遍历整条路径，即可得到该路径的最大曲率，和最小曲率半径。

具体的，由于多数移动机器人的运动底盘，符合车辆运动学模型，因此对于不能原地旋转的车辆模型，存在一个最小的车辆转弯半径，从而需要对路径上的最大曲率进行约束。同时车辆最小转弯半径也是车辆运动学的基本特性。

然而如果当前行驶路径也是由A*算法计算而来，则有几率会遇到背景技术中提到的问题，因此此时需要判断，L1段路径、L2段路径和L3段路径上的最大曲率是否超出预设阈值、或最小曲率半径，是否小于机器人允许的最小转弯半径，如果存在任一不符时，则说明当前行驶路径并不适合于当前机器人行驶，可能有一定几率会导致机器人失控，为此则必须重新生成绕行路径来进行替换。这样才能有机会弥补A*算法不考虑车辆运动学约束的缺点。

进一步的，其中障碍物安全距离判断步骤包括：

步骤S140计算预警视距内的当前行驶路径中，每个路径点与障碍物的距离。

步骤S150遍历整条路径，获取最小距离，当判断最小距离超过阈值时，视为触发绕行规则。

例如：设初始路径上的路径点坐标信息其中为路径点 的个数，和障碍物的信息，其为障碍物的个数的基础上，可以 遍历查找最小距离：

最小可为0，代表机器人会与障碍物碰撞，最大只能为，的值需要比机器人最大直径/外形略大，因为超过这个范围的障碍物一般是没危险 的。

具体的，对于多数移动机器人而言，是无法分辨出障碍物的运动行为的，因此为了有效避障，当障碍物与L1段路径、L2段路径和L3段路径之间的最小距离小于安全阈值时，则说明可能会发生碰撞造成安全隐患，因此需要重新生成绕行路径，以进行规避。

步骤S200当触发任一绕行规则时，如图4至图5所示，以预警视距两端在当前行驶路径上的位置点为绕行起止点，使用A*算法获取绕行的初始路径，其中由于A*算法是一种根据起止点的信息，在已知空间中搜索可行路径的一种常用方法，其应用广泛且成熟，因此此处不再赘述。

步骤S300对初始路径进行包括离障距离、路径平滑、绕行起止点连续性中至少一种优化后，进行拟合，生成绕行路径，替换预警视距内的行驶路径。

具体的，由于前述提到A*算法获得的初始路径，可能并不符合车辆运动学约束，因此在该步骤中，除了要考虑这个因素以外，同时本案也对初始路径提出了多个优化目标，以尽量提高规划出的绕行路径质量，从而提升机器人的行驶效率。

为此，本示例中，为了尽可能使规划出的路径平滑，该步骤S300中的路径平滑优化的步骤包括：

步骤S310设初始路径上第个路径点的坐标为其中为 路径点的个数，对初始路径上的路径点，设计以下路径平滑优化代价函数并求解：

。

其中，为了尽可能的让机器人远离障碍物，该步骤S300中离障距离优化的步骤包括：

步骤S320设初始路径周围第个障碍物的信息为其中为障碍物的个数，对初始路径上的路径点，设计以下离障距离优化代价函数并求解：

。

其中，为了使绕行路径和原行驶路径（或原绕行路径）在规划起止点处尽可能的保持坐标连续、斜率连续、曲率连续。则需要让起止点处的路径与原路径尽可能靠近，因此如图6所示，本示例中优选在原行驶路径上尽量靠近规划起点后，设置至少两个靠近点，同理也在尽量靠近规划终点后，设置至少两个靠近点。

为此，该步骤S300中绕行起止点连续性优化步骤包括：

步骤S330分别在行驶路径上靠近绕行起止点之后，设置个靠近点，本示例中该优选为4个，即起点后2个，终点后2个；

步骤S340设第个靠近点的信息为其中为靠近点的 个数，对初始路径上的路径点，设计以下绕行起止点连续性优化代价函数并求解：

。

此外，在优选示例中，该步骤S300中也可以进行包括：离障距离、路径平滑及绕行起止点连续性优化在内的综合优化，其示例步骤包括：

步骤S350对初始路径上的路径点，设计以下综合优化代价函数并求解：

；

其中∝为平滑系数，β为障碍物系数，γ为靠近系数。此外因为绕行路径的起止点即为原始行进路径上的路径点，因此不需要被优化。同时根据上述公式，使用梯度下降法即可完成求解。

最后，如图7所示，对该优化后的初始路径进行拟合后，便可得到机器人可以运行的绕行路径。其中对离散路径点进行拟合的方法有多种，如：贝塞尔拟合，B样条拟合，多项式拟合，由于属于成熟的现有技术，在此不再赘述。

另一方面，由于现有的绕障决策有的是依据是否会碰撞障碍物，来决定是否生成绕障路径，但考虑到即便是已经规划出的绕行路径，有时也未必是最优路径，此时一般情况下，由于该绕行路径上已经不存在障碍物，因此现有技术不会再生成新绕行路径，即便是质量很差的绕行路径也必须走下去，但这样会影响路径跟随的精度。

虽然也可以采用实时规划的方式来解决，但这样也会造成大量的计算浪费。为此本发明构思上，特别提出了路径代价这一评价路径质量的方案，并通过预设时间间隔来进行新绕行路径规划，以与原绕行路径进行路径代价评价，从而根据路径质量来决定是否将原绕行路径与新绕行路径进行替换，籍此可相对节省计算资源，同时也为机器人提供了一种修正绕行路径的手段。

为此，所述在导航路径上规划机器人绕行路径的方法中，步骤还包括：

步骤S400每间隔预设时间，重复执行步骤S200-S300获取新绕行路径P2；

步骤S500计算新绕行路径P2与原绕行路径P1的代价函数；

步骤S600当判断时，将新绕行路径替换原绕行路径， 其中q为预设的路径质量系数。

具体来说，对于根据“固定时间间隔判断”生成的绕行路径，需要分别计算新生成的绕行路径与原路径的代价值，其代价值越小，路径质量越好，只有当新生成的绕行路径的质量比原路径的质量好，且好于一定阈值的时候，才将原路径更新为绕障路径。

例如，设绕行路径，可以根据“车辆运动学约束判断步骤”来计算出每个 点的曲率值，并表示为包含每个点的曲率的数组，其 中代表第i个点的曲率值。使用代表曲率数组中的最大曲率值，将 原绕行路径P1和新绕行路径P2的最大曲率值可分别计算出表示为和。 使用代表曲率数组中的平均曲率，将原绕行路径P1和新绕行路径P2的平 均曲率值记为和。使用代表曲率波动值：

将原绕行路径P1和新绕行路径P2的曲率波动值，分别表示为和。

根据“障碍物安全距离判断步骤”来计算原绕行路径P1和新绕行路径P2上的点分 别与障碍物之间的最小距离，并记为和。

建立原绕行路径的代价函数设计为：

同理建立新绕行路径的代价函数设计为：

其中，分别为最大曲率系数、平均曲率系数、曲率波动系数、障碍物系 数，可由使用者根据对路径质量的不同要求调整大小。

最后判断，当时，将原路径替换为绕障路径，其中q为 人为指定的路径质量系数，可根据实际情况进行调整。

由此可见，与实时生成绕行路径不同，固定时间间隔生成绕行路径的时间周期可以更长，消耗的计算资源更少。因为随着机器人往前行走，其视野内探测到的L1段路径、L2段路径和L3段路径是实时变化的，因此在固定时间间隔内生成绕行路径，能够找到一条比原路径质量更好的绕行路径。

实验例1：假设机器人存在一条原始路径，并触发了绕障路径规划功能。预设平滑 系数、障碍物系数、靠近系数均设为1。L1、L2、L3分别为1m，3m，2m。

如图4所示，LM1坐标为(-0.188m，-0.778m),LM2坐标为(9.205m，-0.886m)，机器人在从任务点LM1去往任务点LM2的路径上遇到了障碍物，因此需要以当前位置点(2.36m,-0.8m)为规划起始点，(8.36m,-0.8m)为规划终点进行绕行路径规划。

首先使用A*算法进行初始路径的搜索，所得初始路径如图5所示。显然此时的路径点之间存在折线，并且路径起始点和终点处与原路径之间的过渡也并不平滑，不适合机器人行驶。因此需要根据步骤S350进行路径平滑综合优优化，所得平滑路径如图6所示。

可以看出优化后的路径远离障碍物，在路径起始点和终止点处与原路径平滑连接，且路径点之间分布均匀。最后经B样条拟合后，如图7所示，所得路径即可直接下发给机器人执行。

下述分别根据“碰撞预测判断”，“车辆运动学约束判断”，“障碍物安全距离判断”触发情况进行说明。

其中设障碍物系数 均设为1，L1=1m、L2=3m、L3=2m，车辆允许最小转弯半径为0.2m，允许最小安全距离为0.4m， 固定时间间隔为2s。并假设机器人存在一条原始路径。

其中“碰撞预测判断”触发时，如图8所示，L1段路径和L3段路径不存在障碍物但是L2段路径存在障碍物，此时进行绕行路径规划并使用绕行路径替换原路径。

其中“车辆运动学约束判断”触发时，如图9所示，经计算原路径在最大曲率处的最小曲率半径为0.183m，小于车辆允许最小转弯半径0.2m，因此会进行绕行路径规划并使用绕行路径替换原路径。

其中“障碍物安全距离判断”触发时，如图10所示，机器人遇到障碍物1时，进行了绕行路径规划，并生成了绕行路径1，但是由于在进行绕行路径1优化的时候希望始末点处的路径与原路径尽可能靠近，因此会使得绕行路径1和障碍物2之间的安全距离较小。当机器人行驶一段路径后，检测到在(L1+L2+L3)长度内的路径与障碍物的最小距离过小为0.36m，小于了预设的安全距离0.4m，此时进行绕行路径规划并使用绕行路径替换原路径。

另一方面，触发“固定时间间隔判断”时，示例为每间隔2s，执行一次绕行路径规划，如图11所示。机器人在路径1上行驶，到固定时间间隔后，开始绕行路径规划，此时需要分别计算原来的路径1，和新生成的绕行路径2的代价值，以根据路径质量评价方案计算出：

，，，，，，1.5926，

最后判断。q是路径质量系数可人为调节，若q设为0.85,则不 使用绕行路径替换原路径，若q设为0.9,则使用绕行路径替换原路径。

另一方面，对应上述方法，如图12所示，本发明还提供了一种在导航路径上规划机器人绕行路径的系统，其包括：

存储单元，用于存储包括如上述中任一所述在导航路径上规划机器人绕行路径的方法步骤的程序，以供检测单元，处理单元，导航单元适时调取执行；

检测单元，用于检测及获取机器人行驶路径上的障碍物信息，并判断其是否存在于预警视距内；

导航单元，用于提供行驶路径信息；

处理单元，用于根据绕行规则以预警视距两端在当前行驶路径上的位置点为绕行起止点，使用A*算法获取绕行的初始路径，然后对初始路径进行包括离障距离、路径平滑、绕行起止点连续性的综合优化后，进行拟合，生成绕行路径，以令导航单元替换预警视距内的行驶路径；

处理单元，还用于每间隔预设时间，循环计算获取新绕行路径P2，并计算新绕行路 径P2与原绕行路径P1的代价函数，当判断时，令 导航单元将新绕行路径替换原绕行路径。

综上所述，通过本发明提供的该在导航路径上规划机器人绕行路径的方法及系统，能够持续对导航路径以及规划后的避障绕行路径进行包括：碰撞预测、车辆运动学约束、障碍物安全距离在内的判断，从而确保了A*算法能够自我修正，以规划出适于机器人行驶的避障绕行路径，避免机器人失控。

以上公开的本发明优选实施例只是用于帮助阐述本发明。优选实施例并没有详尽叙述所有的细节，也不限制该发明仅为所述的具体实施方式。显然，根据本说明书的内容，可作很多的修改和变化。本说明书选取并具体描述这些实施例，是为了更好地解释本发明的原理和实际应用，从而使所属技术领域技术人员能很好地理解和利用本发明。本发明仅受权利要求书及其全部范围和等效物的限制，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

本领域技术人员可以理解，除了以纯计算机可读程序代码方式实现本发明提供的系统、装置及其各个单元模块以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统、装置及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同程序。所以，本发明提供的系统、装置及其各个单元模块可以被认为是一种硬件部件，而对其内包括的用于实现各种程序的单元模块也可以视为硬件部件内的结构；也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。

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

此外，本发明实施例的各种不同的实施方式之间也可以进行任意组合，只要其不违背本发明实施例的思想，其同样应当视为本发明实施例所公开的内容。

Claims (10)

1.一种在导航路径上规划机器人绕行路径的方法，步骤包括：

步骤S100建立绕行规则判断程序，其中绕行规则包括：碰撞预测判断、车辆运动学约束判断、障碍物安全距离判断规则；设置机器人在行进方向上的预警视距；

步骤S200当触发任一绕行规则时，以预警视距两端在当前行驶路径上的位置点为绕行起止点，使用A*算法获取绕行的初始路径；

步骤S300对初始路径进行包括离障距离、路径平滑、绕行起止点连续性中至少一种优化后，进行拟合，生成绕行路径，替换预警视距内的行驶路径。

2.根据权利要求1所述的方法，其中步骤还包括：

步骤S400每间隔预设时间，重复执行步骤S200-S300获取新绕行路径P2；

步骤S500计算新绕行路径P2与原绕行路径P1的代价函数；

步骤S600当判断时，将新绕行路径替换原绕行路径，其中q为预设的路径质量系数；

其中代价函数的计算步骤包括：

步骤S510分别计算P1、P2的曲率波动值1和/>2、最大曲率值/>和、平均曲率值/>和/>、路径上的点与障碍物之间的最小距离和/>；

步骤S520计算P1的代价函数

；

计算P2的代价函数

；

其中分别为最大曲率系数、平均曲率系数、曲率波动系数、障碍物系数。

3.根据权利要求1所述的方法，其中所述预警视距包括：前、中、后三段连续视距，所述碰撞预测判断步骤包括：

步骤S110判断行驶路径上预警视距中，是否仅中段存在障碍物，若为是则视为触发绕行规则。

4.根据权利要求1所述的方法，其中车辆运动学约束判断步骤包括：

步骤S120计算预警视距内的当前行驶路径中，每个路径点的曲率和曲率半径；

步骤S130遍历整条路径，获取最大曲率和最小曲率半径，当判断最大曲率超过阈值、或最小曲率半径小于机器人允许的最小转弯半径中任一时，视为触发绕行规则。

5.根据权利要求1所述的方法，其中障碍物安全距离判断步骤包括：

步骤S140计算预警视距内的当前行驶路径中，每个路径点与障碍物的距离；

步骤S150遍历整条路径，获取最小距离，当判断最小距离超过阈值时，视为触发绕行规则。

6.根据权利要求1所述的方法，其中步骤S300中路径平滑优化步骤包括：

步骤S310设初始路径上第个路径点的坐标为/>其中/>为路径点的个数，对初始路径上的路径点/>，设计以下路径平滑优化代价函数并求解：

。

7.根据权利要求1所述的方法，其中步骤S300中离障距离优化步骤包括：

步骤S320设初始路径周围第个障碍物的信息为/>其中/>为障碍物的个数，对初始路径上的路径点/>，设计以下离障距离优化代价函数并求解：

。

8.根据权利要求1所述的方法，其中步骤S300中绕行起止点连续性优化步骤包括：

步骤S330分别在行驶路径上靠近绕行起止点之后，设置个靠近点；

步骤S340设第个靠近点的信息为/>其中/>为靠近点的个数，对初始路径上的路径点/>，设计以下绕行起止点连续性优化代价函数并求解：

。

9.根据权利要求1所述的方法，其中步骤S300中进行离障距离、路径平滑及绕行起止点连续性优化的步骤包括：

步骤S350对初始路径上的路径点，设计以下综合优化代价函数并求解：

；

其中∝为平滑系数，β为障碍物系数，γ为靠近系数。

10.一种在导航路径上规划机器人绕行路径的系统，其包括：

存储单元，用于存储包括如权利要求1至9中任一所述在导航路径上规划机器人绕行路径的方法步骤的程序，以供检测单元，处理单元，导航单元适时调取执行；

检测单元，用于检测及获取机器人行驶路径上的障碍物信息，并判断其是否存在于预警视距内；

导航单元，用于提供行驶路径信息；

处理单元，用于根据绕行规则以预警视距两端在当前行驶路径上的位置点为绕行起止点，使用A*算法获取绕行的初始路径，然后对初始路径进行包括离障距离、路径平滑、绕行起止点连续性的综合优化后，进行拟合，生成绕行路径，以令导航单元替换预警视距内的行驶路径；

处理单元，还用于每间隔预设时间，循环计算获取新绕行路径P2，并计算新绕行路径P2与原绕行路径P1的代价函数，当判断/>时，令导航单元将新绕行路径替换原绕行路径。