CN111813117A - 一种机器人巡线优先导航方法、装置和设备 - Google Patents

Abstract

本发明的实施例提供了一种机器人巡线优先导航方法、装置和设备，方法包括根据输入参数读取代价地图、机器人的主线路径及在所述代价地图中的位置，并预设所述主线路径的道路边界；计算出所述机器人从当前位置到主线路径上的目标点的规划路径，并将规划路径离散成初始点集；将所述初始点集输入非线性优化器进行轨迹优化，得到优化后的点集，并计算优化后的点集的执行速度；按照所述优化后的点集和执行速度执行导航。以此方式，能够在机器人进行导航过程中遵循既定路线行驶，在绕障的同时加以限定，不会偏离道路的预设宽度，使机器人在绕障的时候移动范围可控。

Description

一种机器人巡线优先导航方法、装置和设备

技术领域

本发明的实施例一般涉及机器人导航领域，并且更具体地，涉及一种机器人巡线优先导航方法、装置和设备。

背景技术

目前市场上现有的给定预设轨迹的机器人导航方案大部分都是严格按照轨迹行驶，即轨迹上出现障碍物则停车，待障碍物移开后继续前进，看起来很死板，不能随机应变，对使用场景要求相当苛刻，效率大打折扣。

对于预设轨迹中遇到障碍物的情况，常常采用局部避障算法，进行避障处理。在机器人实际避障过程中，由于环境的复杂性和不可预期性，机器人往往会额外偏离很多距离来实现绕障，此时很容易发生路径偏离或者对机器人失去控制的情况。

发明内容

根据本发明的实施例，提供了一种机器人巡线优先导航方案。

在本发明的第一方面，提供了一种机器人巡线优先导航方法。该方法包括：

根据输入参数读取代价地图、机器人的主线路径及在所述代价地图中的位置，并预设所述主线路径的道路边界；

计算出所述机器人从当前位置到主线路径上的目标点的规划路径，并将规划路径离散成初始点集；

将所述初始点集输入非线性优化器进行轨迹优化，得到优化后的点集，并计算优化后的点集的执行速度；

按照所述优化后的点集和执行速度执行导航。

进一步地，所述计算出所述机器人到主线路径上的目标点的规划路径，并将规划路径离散成初始点集，包括：

步骤1：判断所述机器人是否到达预设终点，如果是，则结束导航，否则将预设的道路边界的离散点集映射到所述代价地图，在所述代价地图上绘制道路边界；所述道路边界为在所述主线路径两侧，到所述主线路径距离相等的若干个离散点连接成的线段；

步骤2：判断所述机器人是否在所述道路边界内，如果是，则执行步骤3；否则发出机器人脱离道路提示；

步骤3：判断所述机器人是否在主线路径上，如果是，则以所述机器人当前位置为起点，将沿主线路径方向延伸前瞻距离后的点作为目标点；否则以所述机器人当前位置为起点，将沿路径方向延伸前瞻距离后的点在主线路径上的投影作为目标点；

步骤4：判断起点与目标点之间是否有障碍物，如果是，则执行步骤5；否则将起点到目标点之间路径作为所述规划路径，并将规划路径离散成初始点集；

步骤5：判断所述障碍物的宽度是否不小于两道路边界之间的距离，如果是，则发出越障失败提示；否则在所述道路边界内，通过A*算法规划出一条可通行路径，并离散成初始点集。

进一步地，还包括：

如果所述目标点与障碍物重合，则在主线路径上沿路径方向查找第一个非障碍物的点作为目标点，并在所述道路边界内，通过路径规划算法规划出一条可通行路径，并离散成初始点集。

进一步地，还包括：

在将所述初始点集输入非线性优化器进行轨迹优化之前，调整所述初始点集中的点与点之间的距离，使调整后每两个点之间的速度不超过速度阈值，将调整后的点作为待优化的点集。

进一步地，所述将所述初始点集输入非线性优化器进行轨迹优化包括：

根据优化规则建立信息矩阵，通过最小二乘法计算得出优化后的点集。

进一步地，所述优化规则包括：

速度约束：

取相邻两个点作为一组待优化数据，经过两点的时间为采样周期T，根据两点间距离S和采样时间T计算得到速度V，再根据速度的上限Vmax和下限Vmin，将速度代价设置为：

其中，V为速度；Vmin为速度的下限；Vmax为速度的上限；errorV为速度代价；

加速度约束：

取相邻三个点作为一组待优化数据，经过每两个点的时间为采样周期T，根据每两个点之间的距离S和采样时间T求出待优化加速度a，再根据最大加速度acc和减速度dec，将加速度代价设置为：

其中，a为待优化加速度；dec为减速度；acc为最大加速度；errora为加速度代价；

时间最短约束：

取相邻两个点的时间T为待优化数据，将时间代价设置为：

errorT＝T；T≠0

其中，T为相邻两个点的时间；

转向偏好约束：

取相邻两个点作为一组待优化数据，两个点的航向角yaw2-yaw1差为dyaw，将航向角代价设置为：

其中，dyaw为相邻两个点的航向角的差；errordyaw为航向角代价；

机器人运动模型约束：

error0＝|(cos yaw1+cos yaw2)×dy-(sin yaw1+sin yaw2)×dx|

其中，代价error的维度为2，其两个分量分别为error0和error1；pos1和pos2分别是两个相邻的路径点；yaw1和yaw2是相邻的两个路径点的航向角；dx和dy分别是两个相邻的路径点的横坐标差和纵坐标差。

进一步地，所述计算优化后的点集的执行速度，包括：

其中，Vx为下一个控制周期的线速度；Vw为下一个控制周期的角速度；r为弧的半径；θ为弧的圆心角；所述弧是以两个相邻点的两个姿态切线方向交点为圆心，两个点之间轨迹；T为采样周期。

在本发明的第二方面，提供了一种机器人巡线优先导航装置。该装置包括：

输出参数读取模块，用于根据输入参数读取代价地图、机器人的主线路径及在所述代价地图中的位置；

规划路径计算模块，用于计算所述机器人从当前位置到主线路径上的目标点的规划路径，并将规划路径离散成初始点集；

轨迹优化模块，用于将所述初始点集输入非线性优化器进行轨迹优化，得到优化后的点集，并计算优化后的点集的执行速度；

执行模块，用于按照所述优化后的点集和执行速度执行导航。

在本发明的第三方面，提供了一种电子设备。该电子设备包括：存储器和处理器，所述存储器上存储有计算机程序，所述处理器执行所述程序时实现如以上所述的方法。

在本发明的第四方面，提供了一种计算机可读存储介质，其上存储有计算机程序，所述程序被处理器执行时实现如根据本发明的第一方面的方法。

应当理解，发明内容部分中所描述的内容并非旨在限定本发明的实施例的关键或重要特征，亦非用于限制本发明的范围。本发明的其它特征将通过以下的描述变得容易理解。

本发明能够在机器人进行导航过程中遵循既定路线行驶，在绕障的同时加以限定，不会偏离道路的预设宽度，使机器人在绕障的时候移动范围可控。

附图说明

结合附图并参考以下详细说明，本发明各实施例的上述和其他特征、优点及方面将变得更加明显。在附图中，相同或相似的附图标记表示相同或相似的元素，其中：

图1示出了现有技术中机器人巡线路径示意图；

图2示出了现有技术中机器人绕障示意图；

图3示出了根据本发明的实施例的机器人巡线优先导航方法的流程图；

图4示出了根据本发明的实施例的带有预设边界的机器人巡线示意图；

图5示出了根据本发明的实施例的机器人巡线优先导航装置结构图；

图6示出了能够实施本发明的实施例的示例性电子设备的方框图。

具体实施方式

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

另外，本文中术语“和/或”，仅仅是一种描述关联对象的关联关系，表示可以存在三种关系，例如，A和/或B，可以表示：单独存在A，同时存在A和B，单独存在B这三种情况。另外，本文中字符“/”，一般表示前后关联对象是一种“或”的关系。

目前市场上现有的给定预设轨迹的机器人导航方案大部分都是严格按照轨迹行驶，即轨迹上出现障碍物则停车，待障碍物移开后继续前进。如图1所示，给定机器人一条路线从A到C，机器人会严格的按照这条路径进行巡线，如果路径上出现了障碍物，则机器人会停车，等待障碍物移开后继续前进，直至终点。这种方式不能随机应变，导致机器人的效率很低。

故，在实际应用中，对于预设轨迹中遇到障碍物的情况，常常采用局部避障算法，进行避障处理。如图2所示，给定机器人一条路线从A到C，机器人在路径上无障碍物时，严格按照预设轨迹进行巡线，当遇到障碍物时，根据障碍物的情况，采用局部避障算法重新规划绕障路径进行绕障处理。但在机器人实际避障过程中，由于环境的复杂性和不可预期性，机器人往往会额外偏离很多距离来实现绕障，此时很容易发生路径偏离或者对机器人失去控制的情况。

本发明中，提出一种优化后的巡线优先导航方法，能够在机器人进行导航过程中遵循既定路线行驶，在绕障的同时加以限定，不会偏离道路的预设宽度，使机器人在绕障的时候移动范围可控。

图3示出了本发明实施例的机器人巡线优先导航方法的流程图。

该方法包括：

S301、根据输入参数读取代价地图、机器人的主线路径及在所述代价地图中的位置，并预设所述主线路径的道路边界。

在一些实施例中，首先通过输入costmap参数对costmap进行定义，获取costmap。然后根据输入机器人定位参数，获取当前机器人的位置和姿态信息；并根据输入路线参数确定机器人的主线路径。所述主线路径为从预设起点开始到预设终点为止，并连接预设起点与预设终点之间离散点的有向线段。路线参数中至少包括起点坐标、终点坐标以及在起点坐标和终点坐标中间的离散点坐标。

S302、计算出所述机器人从当前位置到主线路径上的目标点的规划路径，并将规划路径离散成初始点集。

在本发明的实施例中，S302可以进一步包括：

S302-1：判断所述机器人是否到达预设终点，如果是，则结束导航，否则将预设的道路边界的离散点集映射到所述代价地图，在所述代价地图上绘制道路边界；所述道路边界为在所述主线路径两侧，到所述主线路径距离相等的若干个离散点连接成的线段。道路边界的代价与障碍物代价相同。

判断机器人是否达到预设终点，通过机器人的位置判断是否与终点位置相同，如果机器人的位置与终点位置相同，则认为机器人已经到达预设终点位置，此时结束导航。如果机器人的位置与终点位置不同，则判断机器人当前未达到预设终点，此时将预设的道路边界的离散点集映射到所述代价地图，在所述代价地图上绘制道路边界。

本发明的实施例中，在主线道路的基础上提出了道路边界的概念，如图4所示，所述道路边界为在所述主线路径两侧，到所述主线路径距离相等的若干个离散点连接成的线段。

S302-2：判断所述机器人是否在所述道路边界内，如果是，则执行S302-3；否则发出机器人脱离道路提示。

在本发明的实施例中，判断机器人是否在道路边界内，是基于机器人当前的位置坐标的纵坐标是否大于预设边界的最大值做出的判断，如果机器人当前位置坐标的纵坐标不超过预设边界的最大值，则认为机器人在道路边界内。一旦超出边界，则认为机器人已经脱离道路，发出机器人脱离道路提示。

S302-3：判断所述机器人是否在主线路径上，如果是，则以所述机器人当前位置为起点start_pos，将沿主线路径方向延伸前瞻距离后的点作为目标点goal_pos；前瞻距离为机器人预设的距离，用于探测机器人前方的障碍物。如果机器人不在主线路径上，需要先规划回归路径使机器人回归到主线路径上。以所述机器人当前位置为起点start_pos，将沿路径方向延伸前瞻距离后的点在主线路径上的投影作为目标点goal_pos。

在本实施例中，如果所述目标点goal_pos与障碍物重合，则在主线路径上沿着该方向向终点方向查找最近的非障碍物的点作为goal_pos。可以保证规划的目标点始终在主线上。在所述道路边界内，以start_pos为起点，goal_pos为终点，通过路径规划算法在costmap中规划出一条可通行路径，并离散成初始点集。

S302-4：判断起点start_pos与目标点goal_pos之间是否有障碍物，如果是，则执行S302-5；否则将起点start_pos到目标点goal_pos之间路径作为所述规划路径，并将规划路径离散成初始点集；

S302-5：判断所述障碍物的宽度是否不小于两道路边界之间的距离，如果是，如图4所示，机器人在预设道路边界内无法绕行，则发出越障失败提示；否则在所述道路边界内，通过A*算法规划出一条可通行路径，并离散成初始点集。

通过在原有路径规划算法(A*算法)基础上，规定道路边界概念，使机器人遇到障碍后在道路边界内通过路径规划算法进行路径规划，从而绕障，使机器人在绕障的同时不会发生路径偏离或者失控的情况。

作为本发明的一种实施例，在将所述初始点集输入非线性优化器进行轨迹优化之前，调整所述初始点集中的点与点之间的距离，使调整后每两个点之间的速度不超过速度阈值，将调整后的点作为待优化的点集。调整后通过两点之间的距离和采样时间T计算出速度，调整后的速度不超过设定的速度阈值。

S303、将所述初始点集输入非线性优化器进行轨迹优化，得到优化后的点集，并计算优化后的点集的执行速度。

在本发明的实施例中，实际的优化过程时通过非线性优化器实现的，将所有规则通过信息矩阵(为每一条规则加权)将问题转化为最小二乘的求解问题，最后通过最小二乘求解给出最优轨迹。

作为本发明的一种实施例，待优化变量是一个点的位置，误差是这个点到另一个点的距离；例如：

E＝P-P₀；

|E|²＝(x-x₀)²+(y-y₀)²＝EΩE^T

其中，p为待优化变量；p₀是另一个点；E为误差，即两个点之间的距离；Ω为信息矩阵，实施例中为单位阵。

当我们期望x方向的误差和y方向的误差权重一样时，可以使用单位阵作为信息矩阵，但是当我们偏好某个方向更小的误差时，我们要适当升高这个方向上的权重，比如我们希望x方向优先逼近时，信息矩阵Ω就变成了：

如此，优化后的变量就会向x方向优先逼近。

在本发明的一种实施例中，所述优化规则包括速度约束、加速度约束、时间最短约束、转向偏好约束和机器人运动模型约束。

速度约束：

取相邻两个点作为一组待优化数据，经过两点的时间为采样周期T，根据两点间距离S和采样时间T计算得到速度V，再根据速度的上限Vmax和下限Vmin，将速度代价设置为：

其中，V为速度；Vmin为速度的下限；Vmax为速度的上限；errorV为速度代价；

加速度约束：

取相邻三个点作为一组待优化数据，经过每两个点的时间为采样周期T，根据每两个点之间的距离S和采样时间T求出待优化加速度a，再根据最大加速度acc和减速度dec，将加速度代价设置为：

其中，a为待优化加速度；dec为减速度；acc为最大加速度；errora为加速度代价；

时间最短约束：

取相邻两个点的时间T为待优化数据，将时间代价设置为：

errorT＝T；T≠0

其中，T为相邻两个点的时间；

转向偏好约束：

取相邻两个点作为一组待优化数据，两个点的航向角yaw2-yaw1差为dyaw，将航向角代价设置为：

其中，dyaw为相邻两个点的航向角的差；errordyaw为航向角代价；

机器人运动模型约束：

error0＝|(cos yaw1+cos yaw2)×dy-(sin yaw1+sin yaw2)×dx|

其中，代价error的维度为2，两个分量分别为error0和error1；pos1和pos2分别是两个相邻的路径点；yaw1和yaw2是相邻的两个路径点的航向角；dx和dy分别是两个相邻的路径点的横纵坐标差；

根据优化出的轨迹中前两个点，求解下一控制周期的线速度Vx，角速度Vw，在采样周期T内认为机器人的速度是恒定的，pos1的方向指向pos2，找到两个姿态切线方向交点为圆心，由此可以拟定出，两个点之间的小段轨迹为一条弧线，求出圆弧的半径r和圆心角θ。

所述计算优化后的点集的执行速度，包括：

其中，Vx为下一个控制周期的线速度；Vw为下一个控制周期的角速度；r为弧的半径；θ为弧的圆心角；所述弧是以两个相邻点的两个姿态切线方向交点为圆心，两个点之间轨迹；T为采样周期。

将局部的路径(一系列点集)传给规划器，规划器拿到这些点之后，会根据机器人的运动学限制、要求的速度、加速度限制、以及一些预设的偏好设置(距离障碍物远近、转向偏好等)对这些点的位置做一些微调，这样做的目的是让路径比较平滑，更加符合机器人的运动学特性，以及符合使用者的偏好。

调整这些点的具体做法是：建立数学模型，这个模型中，这些点除了按原有的空间位置排列，还要明确自身的朝向(有相应的规则初始化这些朝向，不做具体介绍)。另外，在每两个点之间设置一个时间，这个时间的实际意义是机器人经过这两个点的过程所消耗的时间，这样以来，每几个点、外加几个时间就可以组合成一定的“约束”，这个约束和我们得预期有一个偏差，我们得目的是减小这个偏差(最小二乘问题求解)，例如：两个点的空间位置和经过两个点所用的时间就可以描述机器人的速度和加速度模型，我们在调整的过程中，会尝试给这两个点的空间位置和经过这两个点的时间一个微小的调整量，这样以来计算出的速度就和我们希望的速度有了一个误差，我们利用梯度下降法计算出这个微小的调整量，保证误差呈下降趋势，并在有限次的迭代后，得到一个最优的调整，使得这些约束更加符合我们得期望。在得到这些调整后的点之后，我们取前两个点，以及经过这两个点之间所需的时间，根据机器人运动学公式计算出下一个控制周期的线速度和角速度。

S304、按照所述优化后的点集和执行速度执行导航。

本发明能够在机器人进行导航过程中遵循既定路线行驶，在绕障的同时加以限定，不会偏离道路的预设宽度，使机器人在绕障的时候移动范围可控。

需要说明的是，对于前述的各方法实施例，为了简单描述，故将其都表述为一系列的动作组合，但是本领域技术人员应该知悉，本发明并不受所描述的动作顺序的限制，因为依据本发明，某些步骤可以采用其他顺序或者同时进行。其次，本领域技术人员也应该知悉，说明书中所描述的实施例均属于可选实施例，所涉及的动作和模块并不一定是本发明所必须的。

以上是关于方法实施例的介绍，以下通过装置实施例，对本发明所述方案进行进一步说明。

如图5所示，装置500包括：

输出参数读取模块501，用于根据输入参数读取代价地图、机器人的主线路径及在所述代价地图中的位置。

在一些实施例中，输出参数读取模块501首先通过输入costmap参数对costmap进行定义，获取costmap。然后根据输入机器人定位参数，获取当前机器人的位置和姿态信息；并根据输入路线参数确定机器人的主线路径。所述主线路径为从预设起点开始到预设终点为止，并连接预设起点与预设终点之间离散点的有向线段。路线参数中至少包括起点坐标、终点坐标以及在起点坐标和终点坐标中间的离散点坐标。

规划路径计算模块502，用于计算所述机器人从当前位置到主线路径上的目标点的规划路径，并将规划路径离散成初始点集。

所述规划路径计算模块502，还包括：

第一判断模块，用于判断所述机器人是否到达预设终点，如果是，则结束导航，否则将预设的道路边界的离散点集映射到所述代价地图，在所述代价地图上绘制道路边界；所述道路边界为在所述主线路径两侧，到所述主线路径距离相等的若干个离散点连接成的线段。

第二判断模块，用于判断所述机器人是否在所述道路边界内，如果是，则通过第三判断模块继续执行；否则发出机器人脱离道路提示。

第三判断模块，用于判断所述机器人是否在主线路径上，如果是，则以所述机器人当前位置为起点，将沿主线路径方向延伸前瞻距离后的点作为目标点；否则以所述机器人当前位置为起点，将沿路径方向延伸前瞻距离后的点在主线路径上的投影作为目标点。

第四判断模块，用于判断起点与目标点之间是否有障碍物，如果是，则通过第五判断模块继续执行；否则将起点到目标点之间路径作为所述规划路径，并将规划路径离散成初始点集。

第五判断模块，用于判断所述障碍物的宽度是否不小于两道路边界之间的距离，如果是，则发出越障失败提示；否则在所述道路边界内，通过A*算法规划出一条可通行路径，并离散成初始点集。

还包括，调整模块，用于在将所述初始点集输入非线性优化器进行轨迹优化之前，调整所述初始点集中的点与点之间的距离，使调整后每两个点之间的速度不超过速度阈值，将调整后的点作为待优化的点集。

轨迹优化模块503，用于将所述初始点集输入非线性优化器进行轨迹优化，得到优化后的点集，并计算优化后的点集的执行速度；

执行模块504，用于按照所述优化后的点集和执行速度执行导航。

所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，所述描述的模块的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。

如图6所示，设备包括中央处理单元(CPU)，其可以根据存储在只读存储器(ROM)中的计算机程序指令或者从存储单元加载到随机访问存储器(RAM)中的计算机程序指令，来执行各种适当的动作和处理。在RAM中，还可以存储设备操作所需的各种程序和数据。CPU、ROM以及RAM通过总线彼此相连。输入/输出(I/O)接口也连接至总线。

设备中的多个部件连接至I/O接口，包括：输入单元，例如键盘、鼠标等；输出单元，例如各种类型的显示器、扬声器等；存储单元，例如磁盘、光盘等；以及通信单元，例如网卡、调制解调器、无线通信收发机等。通信单元允许设备通过诸如因特网的计算机网络和/或各种电信网络与其他设备交换信息/数据。

处理单元执行上文所描述的各个方法和处理，例如方法S301～S304。例如，在一些实施例中，方法S301～S304可被实现为计算机软件程序，其被有形地包含于机器可读介质，例如存储单元。在一些实施例中，计算机程序的部分或者全部可以经由ROM和/或通信单元而被载入和/或安装到设备上。当计算机程序加载到RAM并由CPU执行时，可以执行上文描述的方法S301～S304的一个或多个步骤。备选地，在其他实施例中，CPU可以通过其他任何适当的方式(例如，借助于固件)而被配置为执行方法S301～S304。

本文中以上描述的功能可以至少部分地由一个或多个硬件逻辑部件来执行。例如，非限制性地，可以使用的示范类型的硬件逻辑部件包括：场可编程门阵列(FPGA)、专用集成电路(ASIC)、专用标准产品(ASSP)、芯片上系统的系统(SOC)、负载可编程逻辑设备(CPLD)等等。

用于实施本发明的方法的程序代码可以采用一个或多个编程语言的任何组合来编写。这些程序代码可以提供给通用计算机、专用计算机或其他可编程数据处理装置的处理器或控制器，使得程序代码当由处理器或控制器执行时使流程图和/或框图中所规定的功能/操作被实施。程序代码可以完全在机器上执行、部分地在机器上执行，作为独立软件包部分地在机器上执行且部分地在远程机器上执行或完全在远程机器或服务器上执行。

在本发明的上下文中，机器可读介质可以是有形的介质，其可以包含或存储以供指令执行系统、装置或设备使用或与指令执行系统、装置或设备结合地使用的程序。机器可读介质可以是机器可读信号介质或机器可读储存介质。机器可读介质可以包括但不限于电子的、磁性的、光学的、电磁的、红外的、或半导体系统、装置或设备，或者上述内容的任何合适组合。机器可读存储介质的更具体示例会包括基于一个或多个线的电气连接、便携式计算机盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦除可编程只读存储器(EPROM或快闪存储器)、光纤、便捷式紧凑盘只读存储器(CD-ROM)、光学储存设备、磁储存设备、或上述内容的任何合适组合。

此外，虽然采用特定次序描绘了各操作，但是这应当理解为要求这样操作以所示出的特定次序或以顺序次序执行，或者要求所有图示的操作应被执行以取得期望的结果。在一定环境下，多任务和并行处理可能是有利的。同样地，虽然在上面论述中包含了若干具体实现细节，但是这些不应当被解释为对本发明的范围的限制。在单独的实施例的上下文中描述的某些特征还可以组合地实现在单个实现中。相反地，在单个实现的上下文中描述的各种特征也可以单独地或以任何合适的子组合的方式实现在多个实现中。

尽管已经采用特定于结构特征和/或方法逻辑动作的语言描述了本主题，但是应当理解所附权利要求书中所限定的主题未必局限于上面描述的特定特征或动作。相反，上面所描述的特定特征和动作仅仅是实现权利要求书的示例形式。

Claims (10)

1.一种机器人巡线优先导航方法，其特征在于，包括：

根据输入参数读取代价地图、机器人的主线路径及在所述代价地图中的位置，并预设所述主线路径的道路边界；

计算出所述机器人从当前位置到主线路径上的目标点的规划路径，并将规划路径离散成初始点集；

将所述初始点集输入非线性优化器进行轨迹优化，得到优化后的点集，并计算优化后的点集的执行速度；

按照所述优化后的点集和执行速度执行导航。

2.根据权利要求1所述的方法，其特征在于，所述计算出所述机器人到主线路径上的目标点的规划路径，并将规划路径离散成初始点集，包括：

步骤1：判断所述机器人是否到达预设终点，如果是，则结束导航，否则将预设的道路边界的离散点集映射到所述代价地图，在所述代价地图上绘制道路边界；所述道路边界为在所述主线路径两侧，到所述主线路径距离相等的若干个离散点连接成的线段；

步骤2：判断所述机器人是否在所述道路边界内，如果是，则执行步骤3；否则发出机器人脱离道路提示；

步骤3：判断所述机器人是否在主线路径上，如果是，则以所述机器人当前位置为起点，将沿主线路径方向延伸前瞻距离后的点作为目标点；否则以所述机器人当前位置为起点，将沿路径方向延伸前瞻距离后的点在主线路径上的投影作为目标点；

步骤4：判断起点与目标点之间是否有障碍物，如果是，则执行步骤5；否则将起点到目标点之间路径作为所述规划路径，并将规划路径离散成初始点集；

步骤5：判断所述障碍物的宽度是否不小于两道路边界之间的距离，如果是，则发出越障失败提示；否则在所述道路边界内，通过A*算法规划出一条可通行路径，并离散成初始点集。

3.根据权利要求2所述的方法，其特征在于，还包括：

如果所述目标点与障碍物重合，则在主线路径上沿路径方向查找第一个非障碍物的点作为目标点，并在所述道路边界内，通过路径规划算法规划出一条可通行路径，并离散成初始点集。

4.根据权利要求1所述的方法，其特征在于，还包括：

在将所述初始点集输入非线性优化器进行轨迹优化之前，调整所述初始点集中的点与点之间的距离，使调整后每两个点之间的速度不超过速度阈值，将调整后的点作为待优化的点集。

5.根据权利要求1所述的方法，其特征在于，所述将所述初始点集输入非线性优化器进行轨迹优化包括：

根据优化规则建立信息矩阵，通过最小二乘法计算得出优化后的点集。

6.根据权利要求5所述的方法，其特征在于，所述优化规则包括：

速度约束：

取相邻两个点作为一组待优化数据，经过两点的时间为采样周期T，根据两点间距离S和采样时间T计算得到速度V，再根据速度的上限Vmax和下限Vmin，将速度代价设置为：

其中，V为速度；Vmin为速度的下限；Vmax为速度的上限；errorV为速度代价；

加速度约束：

取相邻三个点作为一组待优化数据，经过每两个点的时间为采样周期T，根据每两个点之间的距离S和采样时间T求出待优化加速度a，再根据最大加速度acc和减速度dec，将加速度代价设置为：

其中，a为待优化加速度；dec为减速度；acc为最大加速度；errora为加速度代价；

时间最短约束：

取相邻两个点的时间T为待优化数据，将时间代价设置为：

errorT＝T；T≠0

其中，T为相邻两个点的时间；

转向偏好约束：

取相邻两个点作为一组待优化数据，两个点的航向角yaw2-yaw1差为dyaw，将航向角代价设置为：

其中，dyaw为相邻两个点的航向角的差；errordyaw为航向角代价；

机器人运动模型约束：

error0＝|(cosyaw1+cosyaw2)×dy-(sinyaw1+sinyaw2)×dx|

其中，代价error的维度为2，其两个分量分别为error0和error1；pos1和pos2分别是两个相邻的路径点；yaw1和yaw2是相邻的两个路径点的航向角；dx和dy分别是两个相邻的路径点的横坐标差和纵坐标差。

7.根据权利要求1所述的方法，其特征在于，所述计算优化后的点集的执行速度，包括：

其中，Vx为下一个控制周期的线速度；Vw为下一个控制周期的角速度；r为弧的半径；θ为弧的圆心角；所述弧是以两个相邻点的两个姿态切线方向交点为圆心，两个点之间轨迹；T为采样周期。

8.一种机器人巡线优先导航装置，其特征在于，包括：

输出参数读取模块，用于根据输入参数读取代价地图、机器人的主线路径及在所述代价地图中的位置；

规划路径计算模块，用于计算所述机器人从当前位置到主线路径上的目标点的规划路径，并将规划路径离散成初始点集；

轨迹优化模块，用于将所述初始点集输入非线性优化器进行轨迹优化，得到优化后的点集，并计算优化后的点集的执行速度；

执行模块，用于按照所述优化后的点集和执行速度执行导航。

9.一种电子设备，包括存储器和处理器，所述存储器上存储有计算机程序，其特征在于，所述处理器执行所述程序时实现如权利要求1～7中任一项所述的方法。

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

Images