CN107943034A - 移动机器人沿给定路径的完备且最短时间轨迹规划方法 - Google Patents
移动机器人沿给定路径的完备且最短时间轨迹规划方法 Download PDFInfo
- Publication number
- CN107943034A CN107943034A CN201711177997.8A CN201711177997A CN107943034A CN 107943034 A CN107943034 A CN 107943034A CN 201711177997 A CN201711177997 A CN 201711177997A CN 107943034 A CN107943034 A CN 107943034A
- Authority
- CN
- China
- Prior art keywords
- mrow
- path
- msub
- acceleration
- mfrac
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 38
- 230000001133 acceleration Effects 0.000 claims abstract description 122
- 230000010354 integration Effects 0.000 claims abstract description 23
- 239000011159 matrix material Substances 0.000 claims description 6
- 230000007935 neutral effect Effects 0.000 claims description 4
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 claims description 3
- 239000007787 solid Substances 0.000 description 5
- 238000010586 diagram Methods 0.000 description 3
- 238000002474 experimental method Methods 0.000 description 3
- 238000005457 optimization Methods 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 2
- 238000004519 manufacturing process Methods 0.000 description 2
- 238000012360 testing method Methods 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000010304 firing Methods 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 230000008092 positive effect Effects 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0217—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with energy consumption, time reduction or distance reduction criteria
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Manipulator (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
一种移动机器人沿给定路径的完备且最短时间轨迹规划方法,包括:1.将多维轨迹规划问题转换为路径位置和路径速度的两维问题;1.1以路径位置s,路径速度和路径加速度表示机器人系统;1.2机器人系统的运动学和动力学约束转为路径速度和路径加速度约束;1.3计算最大和最小路径加速度以及最大速度限制曲线MVC*(s);2.以最大路径加速度正向积分加速曲线;3.从加速曲线与MVC*(s)相交点开始,沿MVC*(s)寻找路径加速度切换区域;4.以最小路径加速度反向积分减速曲线;5.当遍历到路径终点时,结束;本发明所提的规划方法能够为给定路径实时输出时间最短轨迹,且该规划方法具有完备性。
Description
技术领域
本发明属于工业自动化领域,特别是涉及轮式移动机器人沿给定路径的完备且最短时间轨迹生成方法。
背景技术
众所周知,最短时间轨迹规划在工业自动化领域应用广泛,是提高工业机器人生产效率的重要工具。为了快速地生成最短时间轨迹,具有较低时间复杂度的解耦策略获得许多关注[1]。解耦策略共分为两步:第一,路径规划输出满足高层任务要求(避障,曲率连续等)的可行路径;第二,沿给定路径的最短时间轨迹规划输出满足运动学和动力学约束(电机的转速,加速度,力矩等)的最优轨迹。沿给定路径的最短时间轨迹规划方法是本发明的核心内容。
目前,沿给定路径的最短时间轨迹规划方法可以保证规划算法的最优性和实时性,但是,规划算法的完备性依然缺少理论证明和实验分析。规划算法的完备性是指如果规划问题有解,则该规划算法能够在有限时间内给出可行解,否则输出无解[2]。如果规划算法缺少完备性,那么该算法可能无法为有解的规划问题输出有效解,或者无法在有限时间内为无解的规划问题输出无解提示。因此,不完备的规划算法会降低机器人的生产效率,甚至危及到生产安全[3]。
为了使得机器人高效安全地工作,需要设计同时具有完备性、最优性和实时性的沿给定路径的轨迹规划方法。针对该问题,E.Barnett等为绳力驱动的平行机构提出一种满足绳子张力和电机力矩的最短时间轨迹规划方法,但是其计算时间复杂度高。动态规划技术(Dynamic Programming)将给定路径离散化,并迭代寻找每个离散路径点上满足运动学和动力学约束的最大路径速度[4-6],然而轨迹时间和计算时间由离散化程度决定。凸优化技术(Convex Optimization)在离散的路径和速度空间内沿合适的梯度方向搜索满足运动学和动力学约束的时间最优轨迹[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步,机器人系统路径参数化,用路径位置、路径速度和路径加速度重新表达机器人系统;
以基于主动偏心万向轮的全方位移动机器人为例,机器人系统运动学模型:
其中,ξ=[x y θ]T代表机器人位姿,[x y]T∈R2是机器人的位置,θ∈R是机器人的方向角,ω,a∈R4分别代表机器人主动轮的速度和加速度,矩阵J如下:
J=[J1 J2 J3 J4]T,
其中,ηi,i∈[1,2]是轮子偏转角,r,L,d分别是轮子半径,机器人本体半径以及机器人中心点到轮子转向轴距离。
沿着给定路径,机器人位姿重新表示为ξ(s),其中s代表路径位置。对ξ(s) 做关于时间t的一阶导可得
ξs=[xs ys θ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代表给定路径总长度。如果与最大速度限制曲线MVC*(s)相交,转步骤3;如果与路径终点界线s=se相交,转步骤4;如果与运动静止界线相交,返回无解提示。
第3步,从加速曲线与最大速度限制曲线MVC*(s)相交点开始,沿MVC*(s) 寻找路径加速度切换区域;
路径加速度切换区域总共分为两类:切换点和切换弧;
a)切换点包含以下三种类别:
①正切点:从该点开始,正反向积分的加减速曲线与最大速度限制曲线
MVC*(s)相切;
②断点:在该点上,最大速度限制曲线MVC*(s)是不连续的;
③奇点:在该点上,关于路径位置,路径速度和路径加速度不等式约束的第一特定向量包含至少一个零元素,即公式(11)的向量A(s)包含至少一个零元素。
b)切换弧定义:位于曲线V(s)上满足路径加速度约束(13)和(14)的连续弧线段;
如果沿MVC*(s)寻找到第一个路径加速度切换区域,那么转第4步并从路径加速度切换区域开始以最小路径加速度反向积分减速曲线(第4步中的情况②或③)。否则,转第4步并从路径终点开始以最小路径加速度反向积分减速曲线 (第4步的情况①)。
第4步,以最小路径加速度反向积分减速曲线
该步总共分为三种情况:
①从给定路径终点开始,以最小路径加速度反向积分减速曲线;
②从切换点开始,以最小路径加速度反向积分减速曲线;
③从切换弧左端开始,以最小路径加速度反向积分减速曲线。
对减速曲线反向积分,直到与运动静止界线或由第2步生成的加速曲线相交。如果与相交,则返回规划问题无解提示。如果与由第2步生成的加速曲线相交,则对于上述的情况②和③,分别转第2步的情况②和③,对于上述的情况①,直接返回最短时间轨迹。
本发明的优点和积极效果:
本发明提供了一种完备且最短时间的沿给定路径轨迹规划方法。在理论上,该方法同时具有最优性,实时性以及完备性。在应用上,该方法适用于多种不同机器人平台,可以为其在线输出时间最短轨迹。实验结果充分证明了本发明算法的有效性。
附图说明
图1是基于主动偏心万向轮的全方位移动机器人图;
图2是三阶贝塞尔给定路径图;
图3是Shin等[13]提出的轨迹规划方法输出的不可行轨迹图;
图4是本发明提出的轨迹规划方法输出的时间最优轨迹图;
图5是跟踪误差图;
图6是主动轮的速度曲线图;
图7是主动轮的加速度曲线图;
图8是本发明提出算法的完整流程图。
具体实施方式
为了使本技术领域的人员更好地理解本发明方案,下面结合附图和实施方式对本发明作进一步的详细说明。
实施例1
第1步,将多维轨迹规划问题转换为路径位置和路径速度的两维轨迹规划问题;
第1.1步,机器人系统路径参数化
以基于主动偏心万向轮的全方位移动机器人为例,其运动学模型(如图1 所示):
其中,ξ=[x y θ]T是机器人位姿,[x y]T∈R2是机器人中心Or在世界坐标系XwOwYw下的位置,θ∈R是机器人的方向角,ω,a∈R4分别代表主动轮的速度和加速度,矩阵J如下:
J=[J1 J2 J3 J4]T,
给定路径选择n阶贝塞尔曲线,其数学表达式如下:
其中,在世界坐标系XwOwYw下位置坐标Pi=[xi yi]T,i∈[0,n]是路径控制点。λ∈[0,1]是路径参数,与路径位置s存在非线性映射。由于路径是已知,可以提前建立λ与s的映射表。
沿该给定路径,机器人位姿重新表示为ξ(s),其中s代表路径位置。对ξ(s) 做关于时间t的一阶导可得
ξs=[xs ys θs]T, (4)
其中,xs=dx/ds,ys=dy/ds,θs=dθ/ds。
将公式(3)和(4)带入公式(1)和(2)可得
其中,矩阵向量
由于沿给定路径移动,每个主动轮偏转角η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步,以最大路径加速度正向积分加速曲线
该步总共分为三种情况:
①从给定路径起点开始,以最大路径加速度正向积分加速曲线,如图3和图4中黑色实线β0;
②从位于速度限制曲线MVC*(s)的切换点开始,以最大路径加速度正向积分加速曲线;
③从位于速度限制曲线V(s)的切换弧右端开始,以最大路径加速度正向积分加速曲线;如图4所示,从q2开始,正向积分加速曲线立即击穿速度限制曲线V(s);
以最大路径加速度正向积分加速曲线的具体步骤如下:
a)将给定路径等分为N份,每份路径长度为ds=se/N,其中se是给定路径总长度。
b)从起点开始,利用如下公式
si+1=si+ds,
正向数值积分加速曲线,直到与曲线MVC*(s),或s=se相交为止。如果与MVC*(s)相交,转步骤3;如果与s=se相交,转步骤4;如果与相交,返回无解提示。
第3步,从加速曲线与MVC*(s)相交点开始,沿MVC*(s)寻找路径加速度切换区域
路径加速度切换区域总共分为两类:切换点和切换弧。
切换点包含以下三种类别:
①正切点:从该点开始,正反积分的加减速曲线与MVC*(s)相切;
②断点:在该点上,MVC*(s)是不连续的;
③奇点:在该点上,公式(11)的向量A(s)包含至少一个零元素。
切换弧定义:位于速度限制曲线V(s)上满足约束路径加速度约束(13)和(14) 的连续弧线段,如图4中黑色实线q1q2;
如果沿MVC*(s)寻找到第一个路径加速度切换区域,那么转步骤4的情况②或③。否则,转步骤4的情况①。
第4步,以最小路径加速度反向积分减速曲线
该步总共分为三种情况:
①从给定路径终点开始,以最小路径加速度反向积分减速曲线,如图3和图4中黑色实线αe;
②从位于速度限制曲线MVC*(s)的切换点开始,以最小路径加速度反向积分减速曲线;
③从位于速度限制曲线V(s)的切换弧左端开始,以最小路径加速度反向积分减速曲线。如图4所示,从q1开始,反向积分减速曲线立即击穿速度限制曲线;
以最小路径加速度反向积分减速曲线的具体步骤如下:
a)将给定路径等分为N份,每份路径长度为ds=se/N,其中se是给定路径总长度。
b)从起点开始,利用如下公式
si-1=si-ds,
反向数值积分减速曲线,直到与或由步骤2生成的加速曲线相交。如果与相交,则返回规划问题无解提示。如果与由步骤2生成的加速曲线相交,则对于情况②和③,分别转步骤2的情况②和③,对于情况①,直接返回最短时间轨迹。
第5步,实验效果描述
为验证上述完备且最短时间沿给定路径轨迹规划方法的有效性,本发明在型号为“NK-OMNI I”的全方位移动机器人上进行了实验验证。给定路径选择三阶贝塞尔曲线(图2),路径控制点为P0=[0.0 0.0]T,P1=[1.0 1.7]T,P2=[4.0 1.7]T, P3=[5.0 0.0]T,单位m。主动轮的速度约束设定为ωmax=[6.0 6.0 6.0 6.0]T,单位rad/s。主动轮的加速度约束设定为amax=[1.0 1.0 1.0 1.0]T,单位rad/s2。另外,在第2步和第4步中,给定路径的等分数设定为N=10000。
参考图3,由Shin等人提出的轨迹规划方法[13]无法生成可行轨迹。与之相比,如图4所示,本发明所提轨迹规划方法仅耗费39毫秒输出一条满足主动轮速度和加速度约束的时间最优轨迹。其中,黑色虚线代表满足主动轮加速度约束的限制曲线MVC(s),黑色点虚线代表满足主动轮速度约束的限制曲线V(s)。黑色实线β0和αe分别代表由步骤2和4生成的加速和减速曲线。黑色实线q1q2代表路径加速度切换弧。该实验结果验证了本发明所提轨迹规划方法的完备性以及实时性。
实验利用一个简单的PID控制器跟踪图4内的时间最优轨迹。参考图5,跟踪误差收敛于零。参考图6和图7,主动轮的四个电机不仅满足主动轮速度和加速度约束,而且在整个跟踪过程中总会有一个电机的速度或加速度达到饱和。该实验结果表明本发明所提轨迹规划方法的最优性。
参考文献
[1]E.Barnett,C.Gosselin.Time-optimal trajectory planning of cable-driven parallel mechanisms for fully-specified paths withG1discontinuities.Journal of Dynamic Systems Measurement and Control,2013,137(7):603-617.
[2]K.Goldberg.Completeness in robot motion planning.WorkshopAlgorithmic Foundations ofRobotics(WAFR),San Francisco,1994.
[3]M.Dogar,A.Spielberg,S.Baker,D.Rus.Multi-robot grasp planning forsequential assembly operations.Proceedings of 2015IEEE InternationalConference on Robotics andAutomation,2015:193-200.
[4]K.Shin,N.McKay.A dynamic programming approach to trajectoryplanning of robotic manipulators.IEEE Transactions Automatic Control,1986,31(6): 491-500.
[5]K.Shin,N.McKay.Selection of near-minimum time geometric paths forrobotic manipulators.IEEE Transactions on Automatic Control,1986,31(6):501-511.
[6]S.Singh,M.Leu.Optimal trajectory generation for roboticmanipulators using dynamic programming.Journal ofDynamic Systems Measurementand Control, 1987,109(2):88-96.
[7]K.Hauser.Fast interpolation and time-optimization withcontact.International Journal ofRobotics Research,2014,33(9):1231-1250.
[8]D.Verscheure,B.Demeulenaere,J.Swevers,J.Schutter,M.Diehl.Time-optimal path tracking for robots:A convex optimization approach.IEEETransactions on Automatic Control,2009,54(10):2318-2327.
[9]F.Debrouwere,W.Loock,G.Pipeleers,Q.Dinh,M.Diehl,J.Schutter,J.Swevers.Time-optimal path following for robots with convex-concaveconstraints using sequential convex programming.IEEE Transactions onRobotics,2013,29(6):1485-1495.
[10]A.Singh,K.Krishna.A class of non-linear time scaling functionsfor smooth time optimal control along specified paths.Proceedings of2015IEEE/RSJ International Conference on IntelligentRobots andSystems,2015:5809-5816.
[11]D.Constantinescu,E.Croft.Smooth and time-optimal trajectoryplanning for industrial manipulators along specified paths.Journal ofRoboticSystems,2000, 17(5):233-249.
[12]S.Macfarlane,E.Croft.Jerk-bounded manipulator trajectoryplanning:Design for real-time applications.IEEE Transactions on RoboticsandAutomation,2003, 19(1):42-52.
[13]K.Shin,N.Mckay.Minimum-time control of robotic manipulators withgeometric path constraints.IEEE Transactions on Automatic Control,1985,30(6):531-541.
[14]Q.Pham.A general,fast,and robust implementation of the time-optimal path parameterization algorithm.IEEE Transactions on Robotics,2014,30(6): 1533-1540.
[15]P.Shen,X.Zhang,Y.Fang.Essential properties of numericalintegration for time-optimal trajectory planning along a specified path.IEEERobotics and Automation Letters,2017,2(2):888-895.
[16]L.Zlajpah.On time optimal path control of manipulators withbounded joint velocities and torques.Proceedings of IEEE InternationalConference on Robotics andAutomation,1996:1572-1577.
[17]F.Lamiraux,J.Laumond.From paths to trajectories for multibodymobile robots.Proceedings of the 5th International Symposium on ExperimentalRobotics,1998:301-309。
Claims (7)
1.一种移动机器人沿给定路径的完备且最短时间轨迹规划方法,该方法具体步骤如下:
第1步,将多维轨迹规划问题转换为路径位置和路径速度的两维轨迹规划问题;
第1.1步,机器人系统路径参数化,用路径位置,路径速度和路径加速度重新表达机器人系统;
第1.2步,机器人系统的运动学和动力学约束转换为关于路径位置,路径速度和路径加速度的不等式约束;
第1.3步,计算最大、最小路径加速度以及在路径位置和路径速度组成的二维平面内的最大速度限制曲线;
第2步,从路径起点或路径加速度切换区域开始,以最大路径加速度正向积分加速曲线;路径加速度切换区域包括两类:切换点和切换弧,其中切换弧是位于最大速度限制曲线上满足路径加速度约束的连续弧线段;
第3步,从加速曲线与最大速度限制曲线相交点开始,沿最大速度限制曲线寻找路径加速度切换区域,直到路径终点为止。
第4步,从路径终点或路径加速度切换区域开始,以最小路径加速度反向积分减速曲线;反向积分的减速曲线和正向积分的加速曲线相交,共同构成所规划的最短时间轨迹。
2.根据权利要求1所述的移动机器人沿给定路径的完备且最短时间轨迹规划方法,其特征在于,第1.1步所述的机器人系统路径参数化,具体步骤如下:
基于主动偏心万向轮的全方位移动机器人的运动学模型:
<mrow>
<mi>&omega;</mi>
<mo>=</mo>
<mi>J</mi>
<mover>
<mi>&xi;</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<mi>a</mi>
<mo>=</mo>
<mover>
<mi>&omega;</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>2</mn>
<mo>)</mo>
</mrow>
</mrow>
其中,ξ=[x y θ]T代表机器人位姿,[x y]T∈R2是机器人的位置,θ∈R是机器人的方向角,ω,a∈R4分别代表主动轮的速度和加速度,符号R代表实数集合,R右上角数字代表维度,矩阵J如下:
J=[J1 J2 J3 J4]T
<mrow>
<msub>
<mi>J</mi>
<mn>1</mn>
</msub>
<mo>=</mo>
<msup>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mfrac>
<mrow>
<mo>-</mo>
<mi>c</mi>
<mi>o</mi>
<mi>s</mi>
<mrow>
<mo>(</mo>
<mi>&theta;</mi>
<mo>+</mo>
<msub>
<mi>&eta;</mi>
<mn>1</mn>
</msub>
<mo>+</mo>
<mn>2</mn>
<mi>&pi;</mi>
<mo>/</mo>
<mn>3</mn>
<mo>)</mo>
</mrow>
</mrow>
<mi>r</mi>
</mfrac>
</mtd>
<mtd>
<mfrac>
<mrow>
<mo>-</mo>
<mi>s</mi>
<mi>i</mi>
<mi>n</mi>
<mrow>
<mo>(</mo>
<mi>&theta;</mi>
<mo>+</mo>
<msub>
<mi>&eta;</mi>
<mn>1</mn>
</msub>
<mo>+</mo>
<mn>2</mn>
<mi>&pi;</mi>
<mo>/</mo>
<mn>3</mn>
<mo>)</mo>
</mrow>
</mrow>
<mi>r</mi>
</mfrac>
</mtd>
<mtd>
<mfrac>
<mrow>
<mo>-</mo>
<mi>L</mi>
<mi>s</mi>
<mi>i</mi>
<mi>n</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>&eta;</mi>
<mn>1</mn>
</msub>
<mo>)</mo>
</mrow>
</mrow>
<mi>r</mi>
</mfrac>
</mtd>
</mtr>
</mtable>
</mfenced>
<mi>T</mi>
</msup>
</mrow>
<mrow>
<msub>
<mi>J</mi>
<mn>2</mn>
</msub>
<mo>=</mo>
<msup>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mfrac>
<mrow>
<mi>s</mi>
<mi>i</mi>
<mi>n</mi>
<mrow>
<mo>(</mo>
<mi>&theta;</mi>
<mo>+</mo>
<msub>
<mi>&eta;</mi>
<mn>1</mn>
</msub>
<mo>+</mo>
<mn>2</mn>
<mi>&pi;</mi>
<mo>/</mo>
<mn>3</mn>
<mo>)</mo>
</mrow>
</mrow>
<mi>d</mi>
</mfrac>
</mtd>
<mtd>
<mfrac>
<mrow>
<mo>-</mo>
<mi>c</mi>
<mi>o</mi>
<mi>s</mi>
<mrow>
<mo>(</mo>
<mi>&theta;</mi>
<mo>+</mo>
<msub>
<mi>&eta;</mi>
<mn>1</mn>
</msub>
<mo>+</mo>
<mn>2</mn>
<mi>&pi;</mi>
<mo>/</mo>
<mn>3</mn>
<mo>)</mo>
</mrow>
</mrow>
<mi>d</mi>
</mfrac>
</mtd>
<mtd>
<mfrac>
<mrow>
<mo>-</mo>
<mi>d</mi>
<mo>-</mo>
<mi>L</mi>
<mi>c</mi>
<mi>o</mi>
<mi>s</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>&eta;</mi>
<mn>1</mn>
</msub>
<mo>)</mo>
</mrow>
</mrow>
<mi>d</mi>
</mfrac>
</mtd>
</mtr>
</mtable>
</mfenced>
<mi>T</mi>
</msup>
</mrow>
<mrow>
<msub>
<mi>J</mi>
<mn>3</mn>
</msub>
<mo>=</mo>
<msup>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mfrac>
<mrow>
<mo>-</mo>
<mi>c</mi>
<mi>o</mi>
<mi>s</mi>
<mrow>
<mo>(</mo>
<mi>&theta;</mi>
<mo>+</mo>
<msub>
<mi>&eta;</mi>
<mn>2</mn>
</msub>
<mo>-</mo>
<mn>2</mn>
<mi>&pi;</mi>
<mo>/</mo>
<mn>3</mn>
<mo>)</mo>
</mrow>
</mrow>
<mi>r</mi>
</mfrac>
</mtd>
<mtd>
<mfrac>
<mrow>
<mo>-</mo>
<mi>s</mi>
<mi>i</mi>
<mi>n</mi>
<mrow>
<mo>(</mo>
<mi>&theta;</mi>
<mo>+</mo>
<msub>
<mi>&eta;</mi>
<mn>2</mn>
</msub>
<mo>-</mo>
<mn>2</mn>
<mi>&pi;</mi>
<mo>/</mo>
<mn>3</mn>
<mo>)</mo>
</mrow>
</mrow>
<mi>r</mi>
</mfrac>
</mtd>
<mtd>
<mfrac>
<mrow>
<mo>-</mo>
<mi>L</mi>
<mi>s</mi>
<mi>i</mi>
<mi>n</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>&eta;</mi>
<mn>2</mn>
</msub>
<mo>)</mo>
</mrow>
</mrow>
<mi>r</mi>
</mfrac>
</mtd>
</mtr>
</mtable>
</mfenced>
<mi>T</mi>
</msup>
</mrow>
<mrow>
<msub>
<mi>J</mi>
<mn>4</mn>
</msub>
<mo>=</mo>
<msup>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mfrac>
<mrow>
<mi>s</mi>
<mi>i</mi>
<mi>n</mi>
<mrow>
<mo>(</mo>
<mi>&theta;</mi>
<mo>+</mo>
<msub>
<mi>&eta;</mi>
<mn>2</mn>
</msub>
<mo>-</mo>
<mn>2</mn>
<mi>&pi;</mi>
<mo>/</mo>
<mn>3</mn>
<mo>)</mo>
</mrow>
</mrow>
<mi>d</mi>
</mfrac>
</mtd>
<mtd>
<mfrac>
<mrow>
<mo>-</mo>
<mi>c</mi>
<mi>o</mi>
<mi>s</mi>
<mrow>
<mo>(</mo>
<mi>&theta;</mi>
<mo>+</mo>
<msub>
<mi>&eta;</mi>
<mn>2</mn>
</msub>
<mo>-</mo>
<mn>2</mn>
<mi>&pi;</mi>
<mo>/</mo>
<mn>3</mn>
<mo>)</mo>
</mrow>
</mrow>
<mi>d</mi>
</mfrac>
</mtd>
<mtd>
<mfrac>
<mrow>
<mo>-</mo>
<mi>d</mi>
<mo>-</mo>
<mi>L</mi>
<mi>c</mi>
<mi>o</mi>
<mi>s</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>&eta;</mi>
<mn>2</mn>
</msub>
<mo>)</mo>
</mrow>
</mrow>
<mi>d</mi>
</mfrac>
</mtd>
</mtr>
</mtable>
</mfenced>
<mi>T</mi>
</msup>
</mrow>
其中,ηi,i∈[1,2]是轮子的偏转角,r,L,d分别是轮子半径,机器人本体半径以及机器人中心点到轮子转向轴距离;
沿着给定路径,机器人位姿被路径参数化为ξ(s),其中s代表路径位置;对ξ(s)做关于时间t的一阶导可得
<mrow>
<mover>
<mi>&xi;</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>=</mo>
<msub>
<mi>&xi;</mi>
<mi>s</mi>
</msub>
<mover>
<mi>s</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>3</mn>
<mo>)</mo>
</mrow>
</mrow>
ξs=[xs ys θs]T (4)
其中,xs=dx/ds,ys=dy/ds,θs=dθ/ds;
沿给定路径,重新表达机器人运动学模型:将公式(4)带入(3),再将公式(3)带入公式(1)得到公式(5);将公式(5)带入公式(2)得到公式(6);
<mrow>
<mi>&omega;</mi>
<mo>=</mo>
<msub>
<mi>J&xi;</mi>
<mi>s</mi>
</msub>
<mover>
<mi>s</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>5</mn>
<mo>)</mo>
</mrow>
</mrow>
其中,矩阵向量
由于沿给定路径移动,主动轮偏转角η1,η2关于路径位置s的表达如下:
<mrow>
<msub>
<mi>&eta;</mi>
<mn>1</mn>
</msub>
<mrow>
<mo>(</mo>
<mi>s</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mi>a</mi>
<mi>t</mi>
<mi>a</mi>
<mi>n</mi>
<mn>2</mn>
<mrow>
<mo>(</mo>
<mo>-</mo>
<msub>
<mi>y</mi>
<mi>s</mi>
</msub>
<mo>-</mo>
<msub>
<mi>L&theta;</mi>
<mi>s</mi>
</msub>
<mi>c</mi>
<mi>o</mi>
<mi>s</mi>
<mo>(</mo>
<mrow>
<mi>&theta;</mi>
<mo>+</mo>
<mfrac>
<mrow>
<mn>2</mn>
<mi>&pi;</mi>
</mrow>
<mn>3</mn>
</mfrac>
</mrow>
<mo>)</mo>
<mo>,</mo>
<mo>-</mo>
<msub>
<mi>x</mi>
<mi>s</mi>
</msub>
<mo>+</mo>
<msub>
<mi>L&theta;</mi>
<mi>s</mi>
</msub>
<mi>sin</mi>
<mo>(</mo>
<mrow>
<mi>&theta;</mi>
<mo>+</mo>
<mfrac>
<mrow>
<mn>2</mn>
<mi>&pi;</mi>
</mrow>
<mn>3</mn>
</mfrac>
</mrow>
<mo>)</mo>
<mo>)</mo>
</mrow>
<mo>-</mo>
<mi>&theta;</mi>
<mo>-</mo>
<mfrac>
<mrow>
<mn>2</mn>
<mi>&pi;</mi>
</mrow>
<mn>3</mn>
</mfrac>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>7</mn>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<msub>
<mi>&eta;</mi>
<mn>2</mn>
</msub>
<mrow>
<mo>(</mo>
<mi>s</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mi>a</mi>
<mi>t</mi>
<mi>a</mi>
<mi>n</mi>
<mn>2</mn>
<mrow>
<mo>(</mo>
<mo>-</mo>
<msub>
<mi>y</mi>
<mi>s</mi>
</msub>
<mo>-</mo>
<msub>
<mi>L&theta;</mi>
<mi>s</mi>
</msub>
<mi>c</mi>
<mi>o</mi>
<mi>s</mi>
<mo>(</mo>
<mrow>
<mi>&theta;</mi>
<mo>-</mo>
<mfrac>
<mrow>
<mn>2</mn>
<mi>&pi;</mi>
</mrow>
<mn>3</mn>
</mfrac>
</mrow>
<mo>)</mo>
<mo>,</mo>
<mo>-</mo>
<msub>
<mi>x</mi>
<mi>s</mi>
</msub>
<mo>+</mo>
<msub>
<mi>L&theta;</mi>
<mi>s</mi>
</msub>
<mi>s</mi>
<mi>i</mi>
<mi>n</mi>
<mo>(</mo>
<mrow>
<mi>&theta;</mi>
<mo>-</mo>
<mfrac>
<mrow>
<mn>2</mn>
<mi>&pi;</mi>
</mrow>
<mn>3</mn>
</mfrac>
</mrow>
<mo>)</mo>
<mo>)</mo>
</mrow>
<mo>-</mo>
<mi>&theta;</mi>
<mo>+</mo>
<mfrac>
<mrow>
<mn>2</mn>
<mi>&pi;</mi>
</mrow>
<mn>3</mn>
</mfrac>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>8</mn>
<mo>)</mo>
</mrow>
<mo>.</mo>
</mrow>
3.根据权利要求2所述的移动机器人沿给定路径的完备且最短时间轨迹规划方法,其特征在于,第1.2步所述的机器人系统的运动学和动力学约束转换为关于路径位置,路径速度和路径加速度的不等式约束,具体步骤如下:
主动轮的速度和加速度约束如下:
-ωmax≤ω≤ωmax (9)
-amax≤a≤amax (10)
其中,常向量ωmax∈R4和amax∈R4分别是主动轮速度和加速度的上限,符号R代表实数集合;
为满足主动轮的加速度约束,将公式(6)带入公式(10),合并同类项与移项得
<mrow>
<mi>A</mi>
<mrow>
<mo>(</mo>
<mi>s</mi>
<mo>)</mo>
</mrow>
<mover>
<mi>s</mi>
<mo>&CenterDot;&CenterDot;</mo>
</mover>
<mo>+</mo>
<mi>B</mi>
<mrow>
<mo>(</mo>
<mi>s</mi>
<mo>)</mo>
</mrow>
<msup>
<mover>
<mi>s</mi>
<mo>&CenterDot;</mo>
</mover>
<mn>2</mn>
</msup>
<mo>+</mo>
<mi>C</mi>
<mrow>
<mo>(</mo>
<mi>s</mi>
<mo>)</mo>
</mrow>
<mo>&le;</mo>
<mn>0</mn>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>11</mn>
<mo>)</mo>
</mrow>
</mrow>
其中,
A(s)=[(Jξs)T (-Jξs)T]T
<mrow>
<mi>C</mi>
<mrow>
<mo>(</mo>
<mi>s</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<msup>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<mo>-</mo>
<msubsup>
<mi>a</mi>
<mrow>
<mi>m</mi>
<mi>a</mi>
<mi>x</mi>
</mrow>
<mi>T</mi>
</msubsup>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msubsup>
<mi>a</mi>
<mrow>
<mi>m</mi>
<mi>a</mi>
<mi>x</mi>
</mrow>
<mi>T</mi>
</msubsup>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mi>T</mi>
</msup>
</mrow>
为满足主动轮的速度约束,将公式(5)带入公式(9),合并同类项与移项得
<mrow>
<mi>A</mi>
<mrow>
<mo>(</mo>
<mi>s</mi>
<mo>)</mo>
</mrow>
<mover>
<mi>s</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>+</mo>
<mi>D</mi>
<mrow>
<mo>(</mo>
<mi>s</mi>
<mo>)</mo>
</mrow>
<mo>&le;</mo>
<mn>0</mn>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>12</mn>
<mo>)</mo>
</mrow>
</mrow>
其中,
4.根据权利要求3所述的移动机器人沿给定路径的完备且最短时间轨迹规划方法,其特征在于,第1.3步所述的最大最小路径加速度和最大速度限制曲线的计算,具体步骤如下:
将公式(11)的第2和3项移动至不等号右边,且不等号两边同除以A(s),得最小和最大路径加速度,分别如下:
<mrow>
<mi>&alpha;</mi>
<mrow>
<mo>(</mo>
<mi>s</mi>
<mo>,</mo>
<mover>
<mi>s</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mi>m</mi>
<mi>a</mi>
<mi>x</mi>
<mo>{</mo>
<mfrac>
<mrow>
<mo>-</mo>
<msub>
<mi>B</mi>
<mi>i</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>s</mi>
<mo>)</mo>
</mrow>
<msup>
<mover>
<mi>s</mi>
<mo>&CenterDot;</mo>
</mover>
<mn>2</mn>
</msup>
<mo>-</mo>
<msub>
<mi>C</mi>
<mi>i</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>s</mi>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<msub>
<mi>A</mi>
<mi>i</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>s</mi>
<mo>)</mo>
</mrow>
</mrow>
</mfrac>
<mo>|</mo>
<msub>
<mi>A</mi>
<mi>i</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>s</mi>
<mo>)</mo>
</mrow>
<mo><</mo>
<mn>0</mn>
<mo>,</mo>
<mi>i</mi>
<mo>&Element;</mo>
<mo>&lsqb;</mo>
<mn>1</mn>
<mo>,</mo>
<mn>8</mn>
<mo>&rsqb;</mo>
<mo>}</mo>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>13</mn>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<mi>&beta;</mi>
<mrow>
<mo>(</mo>
<mi>s</mi>
<mo>,</mo>
<mover>
<mi>s</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mi>min</mi>
<mo>{</mo>
<mfrac>
<mrow>
<mo>-</mo>
<msub>
<mi>B</mi>
<mi>i</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>s</mi>
<mo>)</mo>
</mrow>
<msup>
<mover>
<mi>s</mi>
<mo>&CenterDot;</mo>
</mover>
<mn>2</mn>
</msup>
<mo>-</mo>
<msub>
<mi>C</mi>
<mi>i</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>s</mi>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<msub>
<mi>A</mi>
<mi>i</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>s</mi>
<mo>)</mo>
</mrow>
</mrow>
</mfrac>
<mo>|</mo>
<msub>
<mi>A</mi>
<mi>i</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>s</mi>
<mo>)</mo>
</mrow>
<mo>></mo>
<mn>0</mn>
<mo>,</mo>
<mi>i</mi>
<mo>&Element;</mo>
<mo>&lsqb;</mo>
<mn>1</mn>
<mo>,</mo>
<mn>8</mn>
<mo>&rsqb;</mo>
<mo>}</mo>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>14</mn>
<mo>)</mo>
</mrow>
</mrow>
其中,标量Ai(s),Bi(s),Ci(s)分别是向量A(s),B(s),C(s)的元素;
将公式(13)和(14)组成等式,该等式的解构成满足加速度约束(10)的速度限制曲线
<mrow>
<mi>M</mi>
<mi>V</mi>
<mi>C</mi>
<mrow>
<mo>(</mo>
<mi>s</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mi>m</mi>
<mi>i</mi>
<mi>n</mi>
<mo>{</mo>
<mover>
<mi>s</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>&GreaterEqual;</mo>
<mn>0</mn>
<mo>|</mo>
<mi>&alpha;</mi>
<mrow>
<mo>(</mo>
<mi>s</mi>
<mo>,</mo>
<mover>
<mi>s</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mi>&beta;</mi>
<mrow>
<mo>(</mo>
<mi>s</mi>
<mo>,</mo>
<mover>
<mi>s</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>)</mo>
</mrow>
<mo>}</mo>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>15</mn>
<mo>)</mo>
</mrow>
</mrow>
接着,将公式(12)的第2项移至不等式右边,且不等号两边同除以A(s),得到满足速度约束(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)。
5.根据权利要求1所述的移动机器人沿给定路径的完备且最短时间轨迹规划方法,其特征在于,第2步所述的以最大路径加速度正向积分加速曲线,具体步骤如下:
该步总共分为三种情况:
①从给定路径起点开始,以最大路径加速度正向积分加速曲线;
②从位于最大速度限制曲线的切换点开始,以最大路径加速度正向积分加速曲线;
③从位于最大速度限制曲线的切换弧右端开始,以最大路径加速度正向积分加速曲线;
对加速曲线正向积分,直到与最大速度限制曲线、运动静止界线或路径终点界线相交为止,如果与最大速度限制曲线相交,转第3步;如果与路径终点界线相交,转第4步;如果与运动静止界线相交,返回无解提示。
6.根据权利要求5所述的移动机器人沿给定路径的完备且最短时间轨迹规划方法,其特征在于,
第3步所述的沿最大速度限制曲线寻找路径加速度切换区域,具体步骤如下:
路径加速度切换区域总共分为两类:切换点和切换弧;
a)切换点包含以下三种类别:
①正切点:从该点开始,正反积分的加减速曲线与最大速度限制曲线相切;
②断点:在该点上,最大速度限制曲线是不连续的;
③奇点:在该点上,关于路径位置,路径速度和路径加速度不等式约束的第一特定向量包含至少一个零元素;
b)切换弧定义:位于最大速度限制曲线上满足路径加速度约束的连续弧线段;
如果沿最大速度限制曲线寻找到第一个路径加速度切换区域,那么转第4步并从路径加速度切换区域开始以最小路径加速度反向积分减速曲线;否则,转第4步并从路径终点开始以最小路径加速度反向积分减速曲线。
7.根据权利要求5所述的移动机器人沿给定路径的完备且最短时间轨迹规划方法,其特征在于,第4步所述的以最小路径加速度反向积分减速曲线,具体步骤如下:
该步总共分为三种情况:
①从给定路径终点开始,以最小路径加速度反向积分减速曲线;
②从切换点开始,以最小路径加速度反向积分减速曲线;
③从切换弧左端开始,以最小路径加速度反向积分减速曲线;
对减速曲线反向积分,直到与运动静止界线或由第2步生成的加速曲线相交;
如果与运动静止界线相交,则返回规划问题无解提示;如果与由第2步生成的加速曲线相交,则对于上述情况②和③,分别转第2步的情况②和③,对于上述情况①,直接返回最短时间轨迹。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201711177997.8A CN107943034B (zh) | 2017-11-23 | 2017-11-23 | 移动机器人沿给定路径的完备且最短时间轨迹规划方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201711177997.8A CN107943034B (zh) | 2017-11-23 | 2017-11-23 | 移动机器人沿给定路径的完备且最短时间轨迹规划方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN107943034A true CN107943034A (zh) | 2018-04-20 |
CN107943034B CN107943034B (zh) | 2020-08-04 |
Family
ID=61929981
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201711177997.8A Active CN107943034B (zh) | 2017-11-23 | 2017-11-23 | 移动机器人沿给定路径的完备且最短时间轨迹规划方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN107943034B (zh) |
Cited By (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108873892A (zh) * | 2018-05-31 | 2018-11-23 | 杭州晶智能科技有限公司 | 一种基于路径密度分析的自动吸尘机器人最优路径规划方法 |
CN109188915A (zh) * | 2018-11-05 | 2019-01-11 | 南开大学 | 内嵌运动性能调节机制的速度规划方法 |
CN111487959A (zh) * | 2019-01-28 | 2020-08-04 | 通用汽车环球科技运作有限责任公司 | 一种自主车辆运动计划系统和方法 |
CN111539345A (zh) * | 2020-04-27 | 2020-08-14 | 北京百度网讯科技有限公司 | 用于确定变道动作的方法、装置、设备及可读存储介质 |
CN111650805A (zh) * | 2020-05-21 | 2020-09-11 | 中国科学院西安光学精密机械研究所 | 一种基于旋转电磁铁切换机构的双视场快速切换装置及方法 |
CN113916247A (zh) * | 2021-10-13 | 2022-01-11 | 苏州科技大学 | 智能网联游览车辆道路选择及路径规划装置和方法 |
CN114466730A (zh) * | 2019-08-23 | 2022-05-10 | 实时机器人有限公司 | 用于使机器人优化速度同时保持对加速度和加加速度的限制的运动规划 |
CN116701821A (zh) * | 2023-08-03 | 2023-09-05 | 季华实验室 | 机器人速度计算方法、装置、电子设备及存储介质 |
Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN1399762A (zh) * | 1999-05-28 | 2003-02-26 | 布鲁克斯自动化公司 | 基于预先规定的时间上最优的轨迹形状的机器人操纵器 |
CN101493687A (zh) * | 2009-03-02 | 2009-07-29 | 广西大学 | 实时前瞻全程加减速控制的nurbs曲线自适应分段插补方法 |
CN102236269A (zh) * | 2011-07-21 | 2011-11-09 | 中国科学院光电技术研究所 | 一种对投影光刻机步进及扫描运动轨迹的规划方法 |
CN102298391A (zh) * | 2011-04-27 | 2011-12-28 | 哈尔滨工业大学 | 一种重载工业机器人操作空间内运动轨迹规划方法 |
CN102566511A (zh) * | 2010-12-09 | 2012-07-11 | 中国科学院沈阳计算技术研究所有限公司 | 五轴数控系统刀心点插补路径插值方法 |
CN104062902A (zh) * | 2014-05-15 | 2014-09-24 | 江南大学 | Delta机器人时间最优轨迹规划方法 |
CN105867376A (zh) * | 2016-04-12 | 2016-08-17 | 南京工程学院 | 基于三角函数的工业机器人轨迹规划升降速控制方法 |
EP3220226A1 (en) * | 2016-03-14 | 2017-09-20 | Volkswagen Aktiengesellschaft | Automated vehicle having optimized driving speed profiles |
-
2017
- 2017-11-23 CN CN201711177997.8A patent/CN107943034B/zh active Active
Patent Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN1399762A (zh) * | 1999-05-28 | 2003-02-26 | 布鲁克斯自动化公司 | 基于预先规定的时间上最优的轨迹形状的机器人操纵器 |
CN101493687A (zh) * | 2009-03-02 | 2009-07-29 | 广西大学 | 实时前瞻全程加减速控制的nurbs曲线自适应分段插补方法 |
CN102566511A (zh) * | 2010-12-09 | 2012-07-11 | 中国科学院沈阳计算技术研究所有限公司 | 五轴数控系统刀心点插补路径插值方法 |
CN102298391A (zh) * | 2011-04-27 | 2011-12-28 | 哈尔滨工业大学 | 一种重载工业机器人操作空间内运动轨迹规划方法 |
CN102236269A (zh) * | 2011-07-21 | 2011-11-09 | 中国科学院光电技术研究所 | 一种对投影光刻机步进及扫描运动轨迹的规划方法 |
CN104062902A (zh) * | 2014-05-15 | 2014-09-24 | 江南大学 | Delta机器人时间最优轨迹规划方法 |
EP3220226A1 (en) * | 2016-03-14 | 2017-09-20 | Volkswagen Aktiengesellschaft | Automated vehicle having optimized driving speed profiles |
CN105867376A (zh) * | 2016-04-12 | 2016-08-17 | 南京工程学院 | 基于三角函数的工业机器人轨迹规划升降速控制方法 |
Non-Patent Citations (9)
Title |
---|
ALESSANDRO GASPARETTO: "A technique for time-jerk optimal planning of robot trajectories", 《ROBOTICS AND COMPUTER-INTEGRATED MANUFACTURING》 * |
PEIYAO SHEN: "Trajectory planning of omnidirectional mobile robots", 《CONTROL AND INTELLIGENT SYSTEMS》 * |
QUANG-CUONG PHAM: "A General, Fast, and Robust Implementation of the Time-Optimal Path Parameterization Algorithm", 《ROBOTICS》 * |
TOBIAS KUNZ: "Time-Optimal Trajectory Generation for Path Following with Bounded Acceleration and Velocity", 《ROBOTICS: SCIENCE AND SYSTEMS》 * |
WANG HUI-FANG: "Time-optimal Trajectories for a Car-like Robot", 《ACTA AUTOMATIC SINICA》 * |
刘小磊: "R-Theta 型硅片传输机器人轨迹规划", 《中国优秀硕士学位论文全文数据库》 * |
张强: "切向加速度与弦误差约束的CNC系统时间最优轨迹规划", 《系统工程理论与实践》 * |
沈佩尧: "Essential Properties of Numerical Integration for", 《IEEE ROBOTICS AND AUTOMATION LETTERS》 * |
钱东海: "多约束条件下的机器人时间最优轨迹规划", 《制造业自动化》 * |
Cited By (15)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108873892A (zh) * | 2018-05-31 | 2018-11-23 | 杭州晶智能科技有限公司 | 一种基于路径密度分析的自动吸尘机器人最优路径规划方法 |
CN108873892B (zh) * | 2018-05-31 | 2022-02-01 | 广东乐生智能科技有限公司 | 一种基于路径密度分析的自动吸尘机器人最优路径规划方法 |
CN109188915B (zh) * | 2018-11-05 | 2021-10-29 | 南开大学 | 内嵌运动性能调节机制的速度规划方法 |
CN109188915A (zh) * | 2018-11-05 | 2019-01-11 | 南开大学 | 内嵌运动性能调节机制的速度规划方法 |
CN111487959A (zh) * | 2019-01-28 | 2020-08-04 | 通用汽车环球科技运作有限责任公司 | 一种自主车辆运动计划系统和方法 |
CN114466730A (zh) * | 2019-08-23 | 2022-05-10 | 实时机器人有限公司 | 用于使机器人优化速度同时保持对加速度和加加速度的限制的运动规划 |
CN114466730B (zh) * | 2019-08-23 | 2023-05-23 | 实时机器人有限公司 | 用于使机器人优化速度同时保持对加速度和加加速度的限制的运动规划 |
CN111539345A (zh) * | 2020-04-27 | 2020-08-14 | 北京百度网讯科技有限公司 | 用于确定变道动作的方法、装置、设备及可读存储介质 |
CN111539345B (zh) * | 2020-04-27 | 2023-09-26 | 阿波罗智能技术(北京)有限公司 | 用于确定变道动作的方法、装置、设备及可读存储介质 |
CN111650805B (zh) * | 2020-05-21 | 2021-03-19 | 中国科学院西安光学精密机械研究所 | 一种基于旋转电磁铁切换机构的双视场快速切换装置及方法 |
CN111650805A (zh) * | 2020-05-21 | 2020-09-11 | 中国科学院西安光学精密机械研究所 | 一种基于旋转电磁铁切换机构的双视场快速切换装置及方法 |
CN113916247A (zh) * | 2021-10-13 | 2022-01-11 | 苏州科技大学 | 智能网联游览车辆道路选择及路径规划装置和方法 |
CN113916247B (zh) * | 2021-10-13 | 2023-10-13 | 苏州科技大学 | 智能网联游览车辆道路选择及路径规划装置和方法 |
CN116701821A (zh) * | 2023-08-03 | 2023-09-05 | 季华实验室 | 机器人速度计算方法、装置、电子设备及存储介质 |
CN116701821B (zh) * | 2023-08-03 | 2024-03-19 | 季华实验室 | 机器人速度计算方法、装置、电子设备及存储介质 |
Also Published As
Publication number | Publication date |
---|---|
CN107943034B (zh) | 2020-08-04 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN107943034A (zh) | 移动机器人沿给定路径的完备且最短时间轨迹规划方法 | |
Shen et al. | Complete and time-optimal path-constrained trajectory planning with torque and velocity constraints: Theory and applications | |
Haddadin et al. | Real-time reactive motion generation based on variable attractor dynamics and shaped velocities | |
US9043030B2 (en) | Manipulator and path generation method thereof | |
Broquere et al. | From motion planning to trajectory control with bounded jerk for service manipulator robots | |
CN104965517A (zh) | 一种机器人笛卡尔空间轨迹的规划方法 | |
Kabir et al. | Generation of synchronized configuration space trajectories of multi-robot systems | |
Oftadeh et al. | A novel time optimal path following controller with bounded velocities for mobile robots with independently steerable wheels | |
JPWO2002066210A1 (ja) | ロボット制御装置 | |
Owen et al. | A robocentric motion planner for dynamic environments using the velocity space | |
Ismaiel et al. | A simulation-based study to calculate all the possible trajectories of differential drive mobile robot | |
Deray et al. | Timed-elastic smooth curve optimization for mobile-base motion planning | |
Iannitti et al. | Minimum control-switch motions for the snakeboard: A case study in kinematically controllable underactuated systems | |
Sang et al. | Hybrid Time-Energy Optimal Trajectory Planning for Robot Manipulators with Path and Uniform Velocity Constraints | |
Jia et al. | Coordination of a nonholonomic mobile platform and an on-board manipulator | |
Bazaz et al. | 3-cubic spline for online Cartesian space trajectory planning of an industrial manipulator | |
Tika et al. | Model predictive control based cooperative robot manipulation and collision avoidance in shared workspaces | |
Zhao et al. | Online via-points trajectory generation for reactive manipulations | |
Everett et al. | Model-based variable position mapping for telerobotic assistance in a cylindrical environment | |
Pajak et al. | Collision-free trajectory planning for mobile manipulators subject to control constraints | |
Wang et al. | Time-optimal and smooth trajectory planning based on continuous pseudo-acceleration | |
Wei et al. | Motion tracking and object manipulation of a hyper-redundant MDAMS under joint limits based on task-priority & relative Jacobian | |
Tu et al. | Whole-body control for velocity-controlled mobile collaborative robots using coupling dynamic movement primitives | |
Canfield et al. | Developing Metrics for Comparison of Mobile Robots Performing Welding Tasks | |
Li et al. | Trajectory planning of parallel mechanism for pouring robot |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |