移动机器人沿给定路径的完备且最短时间轨迹规划方法技术

一种移动机器人沿给定路径的完备且最短时间轨迹规划方法，包括：1.将多维轨迹规划问题转换为路径位置和路径速度的两维问题；1.1以路径位置s，路径速度

【技术实现步骤摘要】
移动机器人沿给定路径的完备且最短时间轨迹规划方法
本专利技术属于工业自动化领域，特别是涉及轮式移动机器人沿给定路径的完备且最短时间轨迹生成方法。
技术介绍
众所周知，最短时间轨迹规划在工业自动化领域应用广泛，是提高工业机器人生产效率的重要工具。为了快速地生成最短时间轨迹，具有较低时间复杂度的解耦策略获得许多关注[1]。解耦策略共分为两步：第一，路径规划输出满足高层任务要求(避障，曲率连续等)的可行路径；第二，沿给定路径的最短时间轨迹规划输出满足运动学和动力学约束(电机的转速，加速度，力矩等)的最优轨迹。沿给定路径的最短时间轨迹规划方法是本专利技术的核心内容。目前，沿给定路径的最短时间轨迹规划方法可以保证规划算法的最优性和实时性，但是，规划算法的完备性依然缺少理论证明和实验分析。规划算法的完备性是指如果规划问题有解，则该规划算法能够在有限时间内给出可行解，否则输出无解[2]。如果规划算法缺少完备性，那么该算法可能无法为有解的规划问题输出有效解，或者无法在有限时间内为无解的规划问题输出无解提示。因此，不完备的规划算法会降低机器人的生产效率，甚至危及到生产安全[3]。为了使得机器人高效安全地工作，需要设计同时具有完备性、最优性和实时性的沿给定路径的轨迹规划方法。针对该问题，E.Barnett等为绳力驱动的平行机构提出一种满足绳子张力和电机力矩的最短时间轨迹规划方法，但是其计算时间复杂度高。动态规划技术(DynamicProgramming)将给定路径离散化，并迭代寻找每个离散路径点上满足运动学和动力学约束的最大路径速度[4-6]，然而轨迹时间和计算时间由离散化程度决定。凸优化技术(ConvexOptimization)在离散的路径和速度空间内沿合适的梯度方向搜索满足运动学和动力学约束的时间最优轨迹[7-10]，因而存在收敛到局部最优解的可能性。D.Constantinescu等人为工业机械臂提出一种满足力矩和力矩一阶导约束的光滑且时间最优轨迹规划方法[11]。S.Macfarlane等以五次多项式拟合沿给定路径的速度曲线，从而快速输出满足加加速度约束的轨迹[12]，但是轨迹时间不是全局最优的。基于庞得里亚金极大值原理，K.Shin等提出沿给定路径计算路径加速度切换点，然后从切换点出发以最大和最小路径加速度进行数值积分，生成满足力矩约束的时间最优速度曲线[13]，但该算法不是完备的且计算时间复杂度高。Q.Pham给出该数值积分方法的C++/Python开源代码并与凸优化方法进行对比，证明其开源代码的实时性[14]。当同时考虑速度和力矩约束时，P.Shen等发现该数值积分方法[13]是不完备的，无法为有解的规划问题输出可行解，并给出具体的条件和证明[15]。文献[16],[17]虽然给出数值积分方法在速度和力矩约束下的改进策略，但是缺少严格的完备性证明。总结文献可知，现有沿给定路径轨迹方法虽然能够保证最优性和实时性，但是缺少规划算法的完备性及其证明。
技术实现思路
本专利技术的目的是提供一种移动机器人沿给定路径的完备且最短时间轨迹规划方法，能够同时确保规划算法的完备性和最优性，为轮式移动机器人实时生成时间最优轨迹。为了实现上述目的，本专利技术首先将多维轨迹规划问题转换为由路径位置和路径速度组成的两维轨迹规划问题。在此基础上，机器人系统的运动学和动力学约束转换为关于路径速度和路径加速度约束。接着，计算一条满足路径速度和路径加速度约束的最大速度限制曲线。沿着该最大速度限制曲线，搜寻所有的路径加速度切换区域(包括“切换点”和“切换弧”)。这些区域均满足路径速度和加速度约束。从这些切换区域出发，以最小路径加速度积分的减速曲线和以最大路径加速度积分的加速曲线相互交叉，共同连接成一条最短时间轨迹。本专利技术在前人基础(“切换点”)上补充了一类具有重要作用的路径加速度切换区域，称为“切换弧”。该切换弧的存在不仅使得本专利技术所提出的轨迹规划方法输出满足运动学和动力学约束的最短时间轨迹，而且保证了该规划方法的完备性。实验结果表明本专利技术算法同时具有时间最优、计算实时和规划完备的优点。本专利技术提供的移动机器人沿给定路径的完备且最短时间轨迹规划方法包括：第1步，将多维轨迹规划问题转换为路径位置和路径速度的两维轨迹规划问题；第1.1步，机器人系统路径参数化，用路径位置、路径速度和路径加速度重新表达机器人系统；以基于主动偏心万向轮的全方位移动机器人为例，机器人系统运动学模型：其中，ξ＝[xyθ]T代表机器人位姿，[xy]T∈R2是机器人的位置，θ∈R是机器人的方向角,ω,a∈R4分别代表机器人主动轮的速度和加速度，矩阵J如下：J＝[J1J2J3J4]T,其中，ηi,i∈[1,2]是轮子偏转角，r,L,d分别是轮子半径，机器人本体半径以及机器人中心点到轮子转向轴距离。沿着给定路径，机器人位姿重新表示为ξ(s)，其中s代表路径位置。对ξ(s)做关于时间t的一阶导可得ξs＝[xsysθs]T,(4)其中，xs＝dx/ds,ys＝dy/ds,θs＝dθ/ds。沿给定路径，重新表达机器人运动学模型：将公式(4)带入(3)，再将公式(3)带入公式(1)得到公式(5)；将公式(5)带入公式(2)得到公式(6)；其中，矩阵向量由于沿给定路径移动，每个主动轮偏转角η1,η2关于路径位置s如下：第1.2步，机器人系统的运动学和动力学约束转换为路径位置，路径速度和路径加速度约束；主动轮的速度和加速度约束如下：-ωmax≤ω≤ωmax,(9)-amax≤a≤amax.(10)其中，常向量ωmax∈R4和amax∈R4分别是主动轮速度和加速度的上限。为满足主动轮的加速度约束，将公式(6)带入公式(10)得其中，A(s)＝[(Jξs)T(-Jξs)T]T,为满足主动轮的速度约束，将公式(5)带入公式(9)得其中，第1.3步，计算最小和最大路径加速度以及在路径位置和路径速度组成的二维平面内的最大速度限制曲线；根据公式(11)，计算最小和最大路径加速度分别如下：其中，标量Ai(s),Bi(s),Ci(s)分别是向量A(s),B(s),C(s)的元素。利用公式(13)和(14)，得到满足加速度约束(10)的速度限制曲线接着，根据公式(12)，得到满足速度约束(9)的速度限制曲线V(s)＝min{-Di(s)/Ai(s)|Ai(s)＞0,i∈[1,8]},(16)其中，标量Di(s)是向量D(s)的元素。综合公式(15)和(16)，得到同时满足约束(9)和(10)的最大速度限制曲线MVC*(s)＝min(MVC(s),V(s)).(17)第2步，从路径起点或路径加速度切换区域开始，以最大路径加速度正向积分加速曲线；路径加速度切换区域包括两类：切换点和切换弧，其中切换弧是位于最大速度限制曲线上满足路径加速度约束的连续弧线段；即，该步总共分为三种情况：①从给定路径起点开始，以最大路径加速度正向积分加速曲线；②从位于最大速度限制曲线MVC*(s)的切换点开始，以最大路径加速度正向积分加速曲线；③从位于最大速度限制曲线V(s)的切换弧右端开始，以最大路径加速度正向积分加速曲线；对加速曲线正向积分，直到与最大速度限制曲线MVC*(s)、运动静止界线或路径终点界线s＝se相交为止。其中，se代表给定本文档来自技高网...

【技术保护点】
一种移动机器人沿给定路径的完备且最短时间轨迹规划方法，该方法具体步骤如下：第1步，将多维轨迹规划问题转换为路径位置和路径速度的两维轨迹规划问题；第1.1步，机器人系统路径参数化，用路径位置，路径速度和路径加速度重新表达机器人系统；第1.2步，机器人系统的运动学和动力学约束转换为关于路径位置，路径速度和路径加速度的不等式约束；第1.3步，计算最大、最小路径加速度以及在路径位置和路径速度组成的二维平面内的最大速度限制曲线；第2步，从路径起点或路径加速度切换区域开始，以最大路径加速度正向积分加速曲线；路径加速度切换区域包括两类：切换点和切换弧，其中切换弧是位于最大速度限制曲线上满足路径加速度约束的连续弧线段；第3步，从加速曲线与最大速度限制曲线相交点开始，沿最大速度限制曲线寻找路径加速度切换区域，直到路径终点为止。第4步，从路径终点或路径加速度切换区域开始，以最小路径加速度反向积分减速曲线；反向积分的减速曲线和正向积分的加速曲线相交，共同构成所规划的最短时间轨迹。

【技术特征摘要】
1.一种移动机器人沿给定路径的完备且最短时间轨迹规划方法，该方法具体步骤如下：第1步，将多维轨迹规划问题转换为路径位置和路径速度的两维轨迹规划问题；第1.1步，机器人系统路径参数化，用路径位置，路径速度和路径加速度重新表达机器人系统；第1.2步，机器人系统的运动学和动力学约束转换为关于路径位置，路径速度和路径加速度的不等式约束；第1.3步，计算最大、最小路径加速度以及在路径位置和路径速度组成的二维平面内的最大速度限制曲线；第2步，从路径起点或路径加速度切换区域开始，以最大路径加速度正向积分加速曲线；路径加速度切换区域包括两类：切换点和切换弧，其中切换弧是位于最大速度限制曲线上满足路径加速度约束的连续弧线段；第3步，从加速曲线与最大速度限制曲线相交点开始，沿最大速度限制曲线寻找路径加速度切换区域，直到路径终点为止。第4步，从路径终点或路径加速度切换区域开始，以最小路径加速度反向积分减速曲线；反向积分的减速曲线和正向积分的加速曲线相交，共同构成所规划的最短时间轨迹。2.根据权利要求1所述的移动机器人沿给定路径的完备且最短时间轨迹规划方法，其特征在于，第1.1步所述的机器人系统路径参数化，具体步骤如下：基于主动偏心万向轮的全方位移动机器人的运动学模型：其中，ξ＝[xyθ]T代表机器人位姿，[xy]T∈R2是机器人的位置，θ∈R是机器人的方向角，ω,a∈R4分别代表主动轮的速度和加速度，符号R代表实数集合，R右上角数字代表维度，矩阵J如下：J＝[J1J2J3J4]T其中，ηi,i∈[1,2]是轮子的偏转角，r,L,d分别是轮子半径，机器人本体半径以及机器人中心点到轮子转向轴距离；沿着给定路径，机器人位姿被路径参数化为ξ(s)，其中s代表路径位置；对ξ(s)做关于时间t的一阶导可得ξs＝[xsysθs]T(4)其中，xs＝dx/ds,ys＝dy/ds,θs＝dθ/ds；沿给定路径，重新表达机器人运动学模型：将公式(4)带入(3)，再将公式(3)带入公式(1)得到公式(5)；将公式(5)带入公式(2)得到公式(6)；其中，矩阵向量由于沿给定路径移动，主动轮偏转角η1,η2关于路径位置s的表达如下：3.根据权利要求2所述的移动机器人沿给定路径的完备且最短时间轨迹规划方法，其特征在于，第1.2步所述的机器人系统的运动学和动力学约束转换为关于路径位置，路径速度和路径加速度的不等式约束，具体步骤如下：主动轮的速度和加速度约束如下：-ωmax≤ω≤ωmax(9)-amax≤a≤amax(10)其中，...

【专利技术属性】
技术研发人员：沈佩尧，张雪波，方勇纯，
申请(专利权)人：南开大学，
类型：发明
国别省市：天津,12