一种机器人轨迹规划方法及系统
技术领域
本发明属于机器人轨迹规划领域,尤其涉及一种机器人轨迹规划方法及系统。
背景技术
本部分的陈述仅仅是提供了与本发明相关的背景技术信息,不必然构成在先技术。
在平面内规划出机器人的可行路径之后,需要对环境中的自由空间进行提取以形成用于后端轨迹规划的位置上的硬约束条件来保证轨迹的安全性。其中,将自由空间称作运动走廊。
运动走廊可以为轨迹提供位置上的硬约束条件,同时也决定了轨迹的段数。运动走廊数目的增多会导致轨迹分段的增多,而轨迹分段的增多又增加了需要优化的参数数量,进而加重了整体的计算负担。由于通过前端路径搜索得到的路径节点十分稠密,因此把每个路径点都进行运动走廊的生成并参与到最终的轨迹规划中是不可行的,有必要针对一些作用不大的运动走廊进行剪枝。
重复剪枝是在运动走廊进行初步扩展的同时删除掉具有相互包含关系的运动走廊。具体实现是在新运动走廊向外初步扩展一到两层后判断处于初步扩展中的新运动走廊是否被旧运动走廊包含,若没有包含再继续执行后续扩展,否则放弃扩展。重复剪枝可以对路径节点周围的自由空间较为充分地利用。但发明人发现,重复剪枝在栅格细密的高分辨率地图环境下运动走廊生成较多,计算代价相对较高。
发明内容
为了解决上述问题,本发明提供一种机器人轨迹规划方法及系统,其给每条边设置一个固定的时间代价,这样搜索出来的一系列运动走廊可以使轨迹规划的优化参数量最小,而且运动走廊数目小,降低了计算负担。
为了实现上述目的,本发明采用如下技术方案:
本发明的第一个方面提供一种机器人轨迹规划方法。
一种机器人轨迹规划方法,包括:
获取机器人可行路径;
重复剪枝可行路径的运动走廊;
基于重复剪枝后的运动走廊之间的连通关系形成无向图,其中每个运动走廊作为无向图的一个顶点,包含前端路径搜索路径点的运动走廊之间的相交区域作为连接顶点的边;
基于Dijkstra算法及每条边的时间代价,搜索一组最优运动走廊组合;
将最优运动走廊组合作为安全约束,基于Bernstein多项式基的分段曲线轨迹公式生成机器人规划轨迹。
本发明的第二个方面提供一种机器人轨迹规划系统。
一种机器人轨迹规划系统,包括:
可行路径获取模块,其用于获取机器人可行路径;
重复剪枝模块,其用于重复剪枝可行路径的运动走廊;
无向图形成模块,其用于基于重复剪枝后的运动走廊之间的连通关系形成无向图,其中每个运动走廊作为无向图的一个顶点,包含前端路径搜索路径点的运动走廊之间的相交区域作为连接顶点的边;
最优运动走廊搜索模块,其用于基于Dijkstra算法及每条边的时间代价,搜索一组最优运动走廊组合;
规划轨迹生成模块,其用于将最优运动走廊组合作为安全约束,基于Bernstein多项式基的分段曲线轨迹公式生成机器人规划轨迹。
本发明的第三个方面提供一种计算机可读存储介质。
一种计算机可读存储介质,其上存储有计算机程序,该程序被处理器执行时实现如上述所述的机器人轨迹规划方法中的步骤。
本发明的第四个方面提供一种计算机设备。
一种计算机设备,包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序,所述处理器执行所述程序时实现如上述所述的机器人轨迹规划方法中的步骤。
与现有技术相比,本发明的有益效果是:
本发明在获取机器人可行路径的基础上,首先,重复剪枝可行路径的运动走廊,再基于重复剪枝后的运动走廊之间的连通关系形成无向图,最后,基于Dijkstra算法及每条边的时间代价,搜索出一组最优运动走廊组合;将最优运动走廊组合作为安全约束,基于Bernstein多项式基的分段曲线轨迹公式生成机器人规划轨迹,搜索出来的一系列运动走廊使得轨迹规划的优化参数量最小,在减少运动走廊数目小的同时,降低了计算负担。
附图说明
构成本发明的一部分的说明书附图用来提供对本发明的进一步理解,本发明的示意性实施例及其说明用于解释本发明,并不构成对本发明的不当限定。
图1是本发明实施例的机器人轨迹规划方法流程图;
图2(a)是本发明实施例的运动走廊的初步扩展;
图2(b)是本发明实施例的经过轴向膨胀的运动走廊;
图3是本发明实施例的重复剪枝法;
图4是本发明实施例的运动走廊对应的无向图;
图5是本发明实施例的只有时间代价的剪枝效果;
图6是本发明实施例的改变无向图和边值代价的效果;
图7是本发明实施例的规划路径与重复减枝法得到的路径比较;
图8(a)是本发明实施例的一个三阶Bézier曲线;
图8(b)是本发明实施例的另一个三阶Bézier曲线。
具体实施方式
下面结合附图与实施例对本发明作进一步说明。
应该指出,以下详细说明都是例示性的,旨在对本发明提供进一步的说明。除非另有指明,本文使用的所有技术和科学术语具有与本发明所属技术领域的普通技术人员通常理解的相同含义。
需要注意的是,这里所使用的术语仅是为了描述具体实施方式,而非意图限制根据本发明的示例性实施方式。如在这里所使用的,除非上下文另外明确指出,否则单数形式也意图包括复数形式,此外,还应当理解的是,当在本说明书中使用术语“包含”和/或“包括”时,其指明存在特征、步骤、操作、器件、组件和/或它们的组合。
轨迹规划作为移动机器人运动规划的后端,需要利用在前端搜索得到的路径生成一条连续、平滑且带有时间属性的可执行轨迹。
实施例一
参照图1,本实施例的一种机器人轨迹规划方法,其包括:
步骤S101:获取机器人可行路径。
在具体实施中,可通过Hybrid A*算法获得一条可行路径。
此处需要说明的是,也可采用其他现有的算法获得可行路径,此处不再累述。
下面均以Hybrid A*算法获得一条可行路径为说明。
步骤S102:重复剪枝可行路径的运动走廊。
为了使每一个运动走廊利用到尽可能多的自由空间,需要对其执行初步扩展、轴向膨胀两个步骤的操作。初步扩展就是对路径中需要进行运动走廊生成的节点进行初步的自由空间拓展。由于路径中的每个节点都已经由Hybrid A*算法在前端搜索时存储在了路径数组中,因此可以实现方便地查询。在选定好需要膨胀的路径节点后,需要以路径节点为中心向外层层扩展,直至扩展区域成为以路径节点为圆心的自由空间中最大圆的内接正方形为止,如图2(a)所示。图中的黑色栅格代表环境中的障碍物,白色栅格代表环境中的自由空间,实心圆代表需要进行运动走廊初步扩展的路径节点,空心圆代表以路径节点为圆心生成的自由空间中的最大圆,灰色栅格代表一个完成初步扩展的运动走廊。
通过初步扩展得到的运动走廊还不能充分地利用自由空间,需要对其进行二次膨胀。膨胀的方法是依次查询运动走廊在x轴和y轴方向上相邻栅格的障碍信息,并将自由栅格加入到运动走廊范围内,遇到障碍栅格时停止。通过轴向膨胀后的运动走廊有效面积大大增加,如图2(b)所示。图中的箭头为运动走廊的膨胀方向,灰色栅格为完成初步扩展的运动走廊在经过轴向膨胀后获得的最终运动走廊。
重复剪枝是在运动走廊进行初步扩展的同时删除掉具有相互包含关系的运动走廊。具体实现是在新运动走廊向外初步扩展一到两层后判断处于初步扩展中的新运动走廊是否被旧运动走廊包含,若没有包含再继续执行后续扩展,否则放弃扩展。
重复剪枝法在真实地图中的运动走廊剪枝效果如图3所示,图中白色方框为运动走廊,浅黑线为Hybrid A*算法搜索得到的路径,黑线为生成轨迹,可以看到运动走廊较为充分地扩展了路径点周围的自由空间。
步骤S103:基于重复剪枝后的运动走廊之间的连通关系形成无向图,其中每个运动走廊作为无向图的一个顶点,运动走廊之间的相交区域作为连接顶点的边,如图4所示。
步骤S104:基于Dijkstra算法及每条边的时间代价,搜索一组最优运动走廊组合。
从运行效率上考虑,每增加一个运动走廊就会多引入一段轨迹的优化参数,因此可以给每条边设置一个固定的时间代价,这样搜索出来的一系列运动走廊可以使轨迹规划的优化参数量最小。
如图5所示,虽然运动走廊数目较少,但是运动走廊1和运动走廊2之间存在大量路径节点信息的缺失,导致轨迹形状接近折线,长度也有所增加。因此可以改变运动走廊的连接条件,把对运动走廊连接的判断从是否具有相交区域改为是否具有相同路径节点,这样可以保证搜索得到的运动走廊是没有遗漏路径节点信息的。此外考虑到不同的运动走廊连接之间存在差异,重叠区域更大的运动走廊连接可以给分段轨迹的时间分配提供更多的自由,因此可以将时间代价替换为下一步扩展运动走廊和当前运动走廊重叠面积的倒数S-1来引导算法选择重叠面积更大的运动走廊连接。这样边值代价V的计算公式如下:V=S-1。
在其他实施例中,时间代价也可为1或重叠区域包含路径点数,运动走廊面积及其他相关变量的单调下降函数形式等。
如图6所示,经过运动走廊连接关系及边值代价设置的调整,轨迹的生成效果也获得了提升。
为了对比两种不同运动走廊剪枝方法对后端轨迹规划的影响,选取了采用Cartographer建图方法获得的5张真实地图进行了算法实验,5张真实地图的分辨率均为0.05m/pix,大小分别为831×1148,848×1893,2019×3309,3091×3494,3043×4329。在除运动走廊剪枝方法外其他条件均相同的情况下进行轨迹生成,得到具有代表性的实验数据如表1和表2所示,表中时间为后端轨迹规划时间。
表1使用Dijkstra剪枝法的轨迹规划实验数据
表2使用重复剪枝法的轨迹规划实验数据
根据表中的数据可以计算出使用Dijkstra剪枝算法生成的轨迹在4张真实地图中的运动走廊数目减少了30.09%,轨迹规划效率提高了41.84%。但是重复剪枝法在轨迹长度和Jerk2积分值上更优,平均Jerk2积分值比Dijkstra剪枝算法低3.18%,轨迹长度缩短了0.06%。
步骤S105:将最优运动走廊组合作为安全约束,基于Bernstein多项式基的分段曲线轨迹公式生成机器人规划轨迹。
Bernstein多项式基
式中,n是Bernstein多项式基的阶数,
式中,
Bézier曲线具有以下几个特殊性质:
端点插值性质:Bézier曲线总是从第一个控制点开始,在最后一个控制点结束而不会经过任何其他的控制点。
固定间隔性质:Bézier曲线的定义域始终是[0,1]。
凸包性质:B(t)在[0,1]上的各点是特征多边形各顶点的凸形组合,即曲线落在特征多边形各顶点所构成的凸包之中。
Hodograph性质:Bézier曲线B(t)的导数B(1)(t)被称为Hodograph。Hodograph仍然是Bézier曲线并且其控制点具有如下形式:
式中,n为Bézier曲线的阶数。
对于Bézier曲线来说,基可以看作控制点的权值,控制点也可以看作基的权值。由于Bézier曲线是在区间[0,1]上定义的,而在实际的轨迹规划中所使用的时间通常不会为[0,1],因此对于轨迹的每一段都需要使用一个比例因子sj来将时间t缩放到该段的实际时间。设μ维度为x维度和y维度中的任意一个维度,则一个具有m段的分段Bézier曲线轨迹可以表达为:
式中,
完整轨迹的总时间为:
T=Tm-T0
轨迹规划代价函数的选取一般基于以下3种原则,运行时间最优,能量最优及Jerk最优。Jerk最优的轨迹在速度和加速度上都可以保持连续和光滑,因此具有良好的运动特性,可以使三轮全向移动机器人运动更加平稳产生较小的冲击。
将最小化Jerk作为最优化指标,代价函数选取为fμ(t)三阶导数的平方:
上式事实上是一个二次规划形式PTQ0P。P是一个包括x,y维度上所有控制点cij的向量,Q0是目标函数的Hessian矩阵。设
为了保证最终生成轨迹的光滑、安全和动力学可行性,必须要对分段轨迹施加一系列的约束条件,以限制其位置、速度和加速度都在允许的范围内。由于Bézier曲线的Hodograph性质,每段轨迹的高阶导数都可以表示为相应低阶控制点的线性组合:
式中,l是导数的阶数,n是Bernstein多项式基的阶数;
路径点约束体现在在轨迹上不仅包括对一个点位置的约束,还包括对轨迹在该点处的l(l≤n)阶导数的约束。这里起约束作用的路径点包含起点和终点。路径点约束是直接施加在控制点上的等式约束,其形式为:
在相邻两段轨迹的连接点处,轨迹的
为了保证轨迹的安全性可以把一段轨迹的所有控制点限制在该段轨迹对应的运动走廊范围内,这样由该段轨迹控制点所确定的凸包也必然被运动走廊包含,又因为Bézier曲线具有凸包性质,所以该段轨迹被完全限制在了安全区域中。安全约束是施加在控制点上的不等式约束,对于第j段轨迹,其形式为:
式中,
与安全性约束相似,根据Bézier曲线的凸包性质以及Hodograph性质,可以给整段轨迹在每个维度μ上的速度施加
安全约束、路径点约束、连续性约束和高阶动力学约束给优化问题提供了可行域(cj∈Ωj),线性等式约束条件Aeqc=beq和线性不等式约束条件Aeqc=beq,其中c=[c1,c2,...,cm]。结合代价函数,分段轨迹求解问题最终可以被描述成一个二次凸优化问题:
min cTQ0c
s.t.Aeqc=beq,
Aieqc≤bieq,
cj∈Ωj,j=1,2,...,m
使用了开源二次规划求解器OOQP(Object Oriented software for QuadraticProgramming)来完成上式所描述的二次凸优化问题的求解。
使用Dijkstra剪枝法处理后的运动走廊生成轨迹中的局部轨迹偏离斜向障碍物更远的效果,如图7所示,黑色线为采用本实施例的该方法输出的规划路径,灰色线为重复剪枝法得到的路径。局部轨迹偏离斜向障碍物更远的偏离对于移动机器人实际运动过程更加有利,能够避免机器人与障碍物碰撞。
实施例二
本实施例提供了一种机器人轨迹规划系统,其包括:
可行路径获取模块,其用于获取机器人可行路径;
重复剪枝模块,其用于重复剪枝可行路径的运动走廊;
无向图形成模块,其用于基于重复剪枝后的运动走廊之间的连通关系形成无向图,其中每个运动走廊作为无向图的一个顶点,包含前端路径搜索路径点的运动走廊之间的相交区域作为连接顶点的边;
最优运动走廊搜索模块,其用于基于Dijkstra算法及每条边的时间代价,搜索一组最优运动走廊组合;
规划轨迹生成模块,其用于将最优运动走廊组合作为安全约束,基于Bernstein多项式基的分段曲线轨迹公式生成机器人规划轨迹。
本实施例的一种机器人轨迹规划系统与实施例一所述的机器人轨迹规划方法中的步骤一一对应,其具体实施过程相同,此处不再累述。
实施例三
本实施例提供了一种计算机可读存储介质,其上存储有计算机程序,该程序被处理器执行时实现如上述实施例一所述的机器人轨迹规划方法中的步骤。
实施例四
本实施例提供了一种计算机设备,包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序,所述处理器执行所述程序时实现如上述实施例一所述的机器人轨迹规划方法中的步骤。
本领域内的技术人员应明白,本发明的实施例可提供为方法、系统、或计算机程序产品。因此,本发明可采用硬件实施例、软件实施例、或结合软件和硬件方面的实施例的形式。而且,本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器和光学存储器等)上实施的计算机程序产品的形式。
本发明是参照根据本发明实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器,使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中,使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品,该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上,使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理,从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程,是可以通过计算机程序来指令相关的硬件来完成,所述的程序可存储于一计算机可读取存储介质中,该程序在执行时,可包括如上述各方法的实施例的流程。其中,所述的存储介质可为磁碟、光盘、只读存储记忆体(Read-Only Memory,ROM)或随机存储记忆体(RandomAccessMemory,RAM)等。
以上所述仅为本发明的优选实施例而已,并不用于限制本发明,对于本领域的技术人员来说,本发明可以有各种更改和变化。凡在本发明的精神和原则之内,所作的任何修改、等同替换、改进等,均应包含在本发明的保护范围之内。