[go: up one dir, main page]
More Web Proxy on the site http://driver.im/

CN109188915B - 内嵌运动性能调节机制的速度规划方法 - Google Patents

内嵌运动性能调节机制的速度规划方法 Download PDF

Info

Publication number
CN109188915B
CN109188915B CN201811306985.5A CN201811306985A CN109188915B CN 109188915 B CN109188915 B CN 109188915B CN 201811306985 A CN201811306985 A CN 201811306985A CN 109188915 B CN109188915 B CN 109188915B
Authority
CN
China
Prior art keywords
speed
acceleration
curve
feasible
path
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.)
Active
Application number
CN201811306985.5A
Other languages
English (en)
Other versions
CN109188915A (zh
Inventor
张雪波
沈佩尧
方勇纯
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Nankai University
Original Assignee
Nankai University
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Nankai University filed Critical Nankai University
Priority to CN201811306985.5A priority Critical patent/CN109188915B/zh
Publication of CN109188915A publication Critical patent/CN109188915A/zh
Application granted granted Critical
Publication of CN109188915B publication Critical patent/CN109188915B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
    • G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
    • G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
    • G05B13/042—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators in which a parameter or coefficient is automatically adjusted to optimise the performance

Landscapes

  • Engineering & Computer Science (AREA)
  • Health & Medical Sciences (AREA)
  • Artificial Intelligence (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Evolutionary Computation (AREA)
  • Medical Informatics (AREA)
  • Software Systems (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Feedback Control In General (AREA)

Abstract

一种内嵌运动性能调节机制的速度规划方法，包括：1.将速度规划问题转换为路径位置和路径速度的二维规划问题；将机器人系统速度和加速度约束转换为二维空间内可行域边界；2.为二维规划引入运动性能调节机制；2.1定义运动调节机制的用户指定参数；2.2调节该参数以改变可行域形状；3.计算可行域边界上的匀速巡航部分；以哈希表存储查询匀速巡航部分；4.利用完备数值积分策略计算可行域内的可行速度曲线；5.用双向积分策略使第4步的速度曲线加速度连续，并输出该速度曲线。该规划方法能够根据用户需求输出不同运动性能的可行速度曲线，兼顾规划完备性。

Description

内嵌运动性能调节机制的速度规划方法
技术领域
本发明属于工业自动化领域，特别是涉及内嵌运动性能调节机制的速度规划方法。
背景技术
众所周知，速度规划在工业机器人自动化领域有着举足轻重的地位，决定着机器人系统的安全性和高效性[1]。根据用户指定的性能指标，将机器人系统的物理约束和起止状态作为输入，速度规划方法在有限时间内输出满足物理约束的最优速度曲线或者无解提示。主要性能指标包括运动时间，能量消耗，运动光滑度等[2]。
为了提高机器人系统的生产效率，现有的速度规划方法将机器人的运动时间作为目标函数以生成满足物理约束的最短时间速度曲线[3],[4]。然而，这些速度曲线对应的加速度控制量属于砰-砰控制(Bang-Bang Control)，即加速度是非连续的且饱和的。这会导致跟踪控制精度的降低和误差收敛时间的延长，尤其是存在外部扰动的情况下[1]。于是，考虑光滑度的速度规划方法被提出以提高跟踪控制效果[5-9]。速度曲线用分段的参数多项式表示以保证连续的加速度。然后，利用最优化工具，计算参数空间下的最优速度曲线。但是，这些速度曲线不是全局最优的，并且非线性最优化工具通常是离线的[10],[11]。另外，由分段多项式构成的速度曲线使机器人主要处在加速或减速状态，缺少高比例的匀速巡航状态。对于城市交通系统，这是造成事故的潜在因素[12]。上述的速度规划方法均缺少完备性，即对于有解规划问题输出可行解，否则输出无解提示。因此，现有的速度规划方法无法实时地输出加速度连续且运动时间全局最优的速度曲线，同时保证完备性和调整巡航比例。
具体而言，K.Shin和J.Bobrow等利用庞特里雅金极大原理为工业机械臂提出一种运动时间全局最优的速度规划方法，但是加速度是饱和且非连续的[3],[4]。Q.Pham提供了该方法的C++/Python开源版本，并移植应用到航天飞行器[13]。另外，凸优化技术和动态规划技术也被用来计算运动时间全局最优的速度曲线[14],[15]。然而，这些方法在生产和生活场景下存在重要隐患。跟踪加速度饱和且不连续的速度曲线会降低位姿误差的收敛效果，而且可能导致机器人机械结构受损，影响机器人整体运动质量和安全。为了输出加速度连续的速度曲线，光滑速度规划方法采用分段多项式插值策略来表示可行速度曲线，然后借助最优化工具计算最优速度曲线，包括序列二次优化(Sequential QuadraticProgramming，SQP)[16]，可变容差法(Flexible Tolerance Method，FTM)[17]，粒子群算法(Particle Swarm Optimization，PSO)[18]和有效集法(Active-Set Optimization)[19]。在给定运动时间条件下，A.Piazzi等用分段三阶样条曲线表达速度曲线，以加速度一阶导的平方积分作为目标函数来计算最优速度曲线[6]。C.Bianco等深入研究基于分段多项式插值策略的速度规划方法，并给出生成可行解的前提条件及相关数学证明[5]。S.Kucuk先采用分段三阶样条表示速度曲线并进行优化计算，然后使用七阶多项式平滑分段连接处以保证连续的加速度[18]。S.Macfarlane和D.Constantinescu等通过限制速度曲线的加加速度来保证光滑度(连续的加速度)[10],[17]。A.Gasparetto和V.Zanotto等将带权重的运动时间和加加速度积分作为目标函数[1]。通过调整权重，该方法能输出更光滑或者更快速的速度曲线。总结文献可知，这些方法无法输出全局空间的运动时间最优解，而且计算效率不可调控，甚至缺少规划完备性。
发明内容
本发明的目的是克服现有技术存在的上述不足，提供一种内嵌运动性能调节机制的速度规划方法，能够实时地输出加速度连续的速度曲线，同时内嵌的调节机制可以根据用户指定参数成比例地改变速度曲线的巡航比例和运动时间，同时该方法具有重要的规划完备性，能在有限时间内为有解问题输出可行解，否则输出无解提示。
为了实现上述目的，本发明方法首先将速度规划问题转换为路径位置和路径速度的二维空间规划问题。首先，将速度和加速度约束转换为二维空间内可行区域的边界。然后，利用完备数值积分策略计算运动时间全局最优的速度曲线，但是该曲线的加速度是不连续的。接着，利用双向积分策略修复加速度不连续区域。经过双向积分策略的后处理，本发明方法最终输出一条加速度连续的可行速度曲线。在此基础上，内嵌的调节机制提供一个用户可调节的功能参数。通过改变该参数，二维空间内可行区域边界发生变化，从而影响最终生成的速度曲线的巡航比例和运动时间，使其在运动时间全局最优解和高巡航比例解之间变化。另外，该内嵌运动调节机制带来更高效的计算能力。随着解的巡航比例增大，得到解所需的计算时间不断减小，这与可行解比最优解更容易被得到的日常认知相符。
本发明提供的内嵌运动性能调节机制的速度规划方法包括：
第1步，将速度规划问题转换为路径位置和路径速度的二维规划问题，并计算其可行域；
向量q∈Rn代表机器人系统状态量，n代表机器人状态维数，向量v∈Rm,a∈Rm分别代表机器人电机的速度和加速度量，m代表机器人电机总数，则机器人系统的动态扩展模型描述如下
Figure BDA0001853884910000031
Figure BDA0001853884910000032
其中，J(q)∈Rm×n是向量q的雅克比矩阵。
沿着给定路径，机器人状态重新表示为q(s)，其中s代表路径位置。进而，公式(1)和(2)可重新表示为
Figure BDA0001853884910000033
Figure BDA0001853884910000034
其中，
Figure BDA0001853884910000035
机器人系统的物理约束表示如下
-vmax≤v≤vmax, (5)
-amax≤a≤amax, (6)
其中，常向量vmax∈Rm和amax∈Rm分别是机器人电机速度和加速度的上限。
为满足机器人电机的加速度约束，将公式(4)带入公式(6)得
Figure BDA0001853884910000036
其中，
A(s)＝[(J(q(s))qs)T-(J(q(s))qs)T]T,
B(s)＝[(Jsqs+J(q(s))qss)T-(Jsqs+J(q(s))qss)T]T,
Figure BDA0001853884910000037
为满足机器人电机的速度约束，将公式(3)带入公式(5)得
Figure BDA0001853884910000038
其中，
Figure BDA0001853884910000039
根据公式(7)，计算最小路径加速度
Figure BDA00018538849100000310
和最大路径加速度
Figure BDA00018538849100000311
分别如下：
Figure BDA00018538849100000312
Figure BDA0001853884910000041
其中，标量Ai(s),Bi(s),Ci(s)分别是向量A(s),B(s),C(s)的元素。
根据公式(7),(8),(9),(10)可得路径位置和路径速二维空间内的可行域上边界，
MVC(s)＝min(MVCV(s),MVCA(s)),s∈[0,se], (11)
其中，se代表路径总长度，而代表电机速度约束的MVCV(s)和代表电机加速度约束的MVCA(s)表达式公式如下
Figure BDA0001853884910000042
MVCV(s)＝min{-Di(s)/Ai(s)|Ai(s)＞0,i∈[1,2m]}. (13)
在路径位置和路径速度二维空间的下边界为
Figure BDA0001853884910000043
左右边界表达式分别为s＝0,se＝0。由这些边界围成的多边形就是路径位置和路径速度二维空间内的可行域。
第2步，为二维规划引入运动性能调节机制；
第2.1步，定义运动性能调节机制内的用户指定参数ε；
该用户指定参数定义为机器人系统路径速度的匀速上限，约束如下，
Figure BDA0001853884910000044
其中，
Figure BDA0001853884910000045
函数Max(·)表示求取MVC(s)的函数最大值，标量
Figure BDA0001853884910000046
分别表示初始和终止速度。
为了满足公式(14)的路径速度约束，另一个可行域的上边界描述为
M(s)＝ε,s∈[0,se]. (15)
引入用户指定参数ε后，可行域上边界重新描述为
MVC*(s)＝min(MVC(s),M(s)),s∈[0,se]. (16)
第2.2步，调节用户指定参数ε以改变可行域形状；
用户指定的参数ε可以改变可行域形状，特别是其上边界的幅值和形状。通过改变可行域的形状，最优速度曲线对应的运动时间和巡航比例也随之发生改变。
当用户指定参数ε减小趋向
Figure BDA0001853884910000047
时，可行域上边界的幅值在降低，这意味着最大路径速度在不断降低，可行速度曲线的最大值也在不断降低，那么最终生成的速度曲线对应的运动时间会不断增加。同时，可行域上边界的形状趋近于直线，这意味着可行速度曲线的巡航运动比例会不断提高。
当用户指定参数ε增大趋向Max(MVC*(s))时，可行域上边界的幅值在提高，这意味着最大路径速度在不断提高，可行速度曲线的最大值也在不断提高，那么最终生成的速度曲线对应的运动时间会不断降低。同时，可行域上边界的形状趋近于曲线MVC(s)，这意味着可行速度曲线的巡航比例会不断降低。
第3步，计算可行域边界上的匀速巡航部分；
本发明采用完备数值积分方法(参考文献[20])计算曲线MVC*(s)下的可行速度曲线。该完备数值积分方法首先沿MVC*(s)搜索加速度转换区域，即满足公式(7)的部分MVC*(s)曲线段。然后，以加速度转换区域为起始，用公式(9)和(10)计算加速和减速曲线，并连接为可行速度曲线。
为了提高加速度转换区域的搜索效率，本发明描述匀速分界线概念L(s)，其数学定义如下
Figure BDA0001853884910000051
在该分界线上方
Figure BDA0001853884910000052
公式
Figure BDA0001853884910000053
成立。在该分界线下方
Figure BDA0001853884910000054
公式
Figure BDA0001853884910000055
成立。在该分界线上
Figure BDA0001853884910000056
公式
Figure BDA0001853884910000057
成立。
将已经得到的L(s)按照键值对
Figure BDA0001853884910000058
存储到哈希表。针对不同的M(s)，在常量时间复杂度O(1)内查询哈希表得到
Figure BDA0001853884910000059
M＝{M(s)|M(s)＜L(s),s∈[0,se]}, (19)
其中，
Figure BDA00018538849100000510
和M分别表示位于分界线L(s)上方和下方的部分M(s)曲线，两者的加速度均等于零，但是只有M满足公式(7)，可以作为MVC*(s)上的加速度转换区域。
第4步，利用完备数值积分策略计算可行速度曲线；
首先，沿MVC*(s)搜索所有加速度转换区域，当遇到第3步得到的M时，直接跳过，继续搜索剩余部分。然后，以这些加速度转换区域作为起点，用最大路径加速度
Figure BDA00018538849100000511
正向积分加速曲线，用最小路径加速度
Figure BDA00018538849100000512
反向积分减速曲线。最后，这些加速和减速曲线相交构成可行速度曲线。特别的，如果规划问题本身是无解的，该完备数值积分策略会在有限时间内输出无解信号以提示用户规划问题是无解的。
第5步，利用双向积分策略使第4步得到的速度曲线加速度连续；
在加速曲线和减速曲线交点p1两侧选择点p2和p3，且点p2和p3分别位于加速曲线和减速曲线上。注意，点p2和点p1之间不存在其他交点，点p3和点p1之间也不存在其他交点。
以点p2为起点，正向积分一条速度曲线l1，其路径加速度为
Figure BDA0001853884910000061
以点p3为起点，反向积分一条速度曲线l2，其路径加速度为
Figure BDA0001853884910000062
其中，标量
Figure BDA0001853884910000063
分别表示点pi的路径位置，路径速度和路径加速度，而标量
Figure BDA0001853884910000064
和
Figure BDA0001853884910000065
的表达式如下
Figure BDA0001853884910000066
Figure BDA0001853884910000067
Figure BDA0001853884910000068
Figure BDA0001853884910000069
速度曲线l1,l2会在
Figure BDA00018538849100000610
路径位置处连接，并且连接点的加速度是连续的。按照此法，处理第4步所得速度曲线内的所有交点，则最终生成的速度曲线的加速度是连续的。
本发明的优点和积极效果：
本发明提供了一种内嵌运动性能调节机制的速度规划方法。在加速度连续约束下，该方法输出全局空间内的运动时间最优速度曲线，并且保证完备特性。同时，该方法内嵌了高效的运动性能调节机制。根据用户指定参数，该方法既能输出较快且光滑的速度曲线以提高生产效率，又能输出拥有高巡航比例的可行速度曲线以提高机器人的运动稳定性和跟踪精度。实验结果充分证明了本发明算法的有效性。
附图说明
图1是基于主动偏心万向轮的全方位移动机器人运动学模型图；
图2是速度规划转为二维规划示意图；
图3是完备数值积分方法[20]示意图；
图4子图A表示当用户指定参数增大时本发明算法输出运动时间趋向最优的速度曲线，图B表示当用户指定参数减小时本发明算法输出拥有较高巡航运动比例的速度曲线；
图5是用户指定参数ε＝0.6实验结果示意图；
图6是双向积分策实验结果示意图；
图7是与参考文献[17]所提方法的对比实验结果图；
图8是本发明方法的用户指定参数ε＝0.26的跟踪误差图。
图9是参考文献[17]提出方法的跟踪误差图。
图10是本发明方法的用户指定参数ε＝0.63的主动轮速度曲线图。
图11是参考文献[17]方法的主动轮速度曲线图。
图12是本发明方法的用户指定参数ε＝0.26的主动轮速度曲线图。
图13是本发明方法的用户指定参数ε＝0.63的主动轮加速度曲线图。
图14是参考文献[17]方法的主动轮加速度曲线图。
图15是本发明方法的用户指定参数ε＝0.26的主动轮加速度曲线图。
图16是本发明提出方法的完整流程图。
具体实施方式
为了使本技术领域的人员更好地理解本发明方案，下面结合附图和实施方式对本发明作进一步的详细说明。
实施例1
第1步，将速度规划问题转换为路径位置和路径速度的二维规划问题；
以基于主动偏心万向轮的全方位移动机器人为例，其运动学模型(如图1所示)：
Figure BDA0001853884910000071
Figure BDA0001853884910000072
其中，q＝[x y θ]T是机器人位姿，[xy]T∈R2是机器人中心Or在世界坐标系XwOwYw下的位置，θ∈R是机器人的方向角，v,a∈R4分别代表主动轮的速度和加速度，矩阵J如下：
J(q)＝[J1 J2 J3 J4]T,
Figure BDA0001853884910000081
Figure BDA0001853884910000082
Figure BDA0001853884910000083
Figure BDA0001853884910000084
给定路径选择k阶贝塞尔曲线，其数学表达式如下：
Figure BDA0001853884910000085
其中，在世界坐标系XwOwYw下位置坐标Pi＝[xi yi]T,i∈[0,n]是路径控制点。λ∈[0,1]是路径参数，与路径位置s存在非线性映射。由于路径是已知，可以提前建立λ与s的映射表。
沿该给定路径，机器人位姿重新表示为q(s)，其中s代表路径位置。进而，公式(1)和(2)可重新表示为
Figure BDA0001853884910000086
Figure BDA0001853884910000087
其中，
Figure BDA0001853884910000088
由于沿给定路径移动，每个主动轮偏转角η1,η2关于路径位置s如下：
Figure BDA0001853884910000089
Figure BDA00018538849100000810
主动轮的速度和加速度约束如下：
-vmax≤v≤vmax, (5)
-amax≤a≤amax, (6)
其中，常向量vmax∈R4和amax∈R4分别是主动轮速度和加速度的上限。
为满足主动轮的加速度约束，将公式(6)带入公式(10)得
Figure BDA0001853884910000091
其中，
A(s)＝[(Jqs)T(-Jqs)T]T,
Figure BDA0001853884910000092
Figure BDA0001853884910000093
为满足主动轮的速度约束，将公式(5)带入公式(9)得
Figure BDA0001853884910000094
其中，
Figure BDA0001853884910000095
根据公式(7),(8),(9),(10)可得路径位置和路径速度二维空间内的可行域上边界，
MVC(s)＝min(MVCV(s),MVCA(s)),s∈[0,se], (11)
其中，se代表路径总长度，而代表电机速度约束的MVCV(s)曲线和代表电机加速度约束的MVCA(s)曲线表达式公式如下
Figure BDA0001853884910000096
MVCV(s)＝min{-Di(s)/Ai(s)|Ai(s)＞0,i∈[1,8]}. (13)
在路径位置和路径速度二维空间的下边界为
Figure BDA0001853884910000097
左右边界表达式分别为s＝0,se＝0。由这些边界围成的多边形就是路径位置和路径速度二维空间内的可行域。最终，如图2所示，全方位移动机器人的速度规划问题被转换为路径位置和路径速度的二维规划问题。其中，可行域由黑色点虚线MVCA(s)，黑色虚线MVCV(s)，下边界
Figure BDA00018538849100000911
左边界s＝0和右边界s＝se围成(se代表路径总长度)。
第2步，为二维规划引入运动性能调节机制；
第2.1步，定义运动性能调节机制内的用户指定参数ε；
该用户指定参数定义为路径速度约束如下
Figure BDA0001853884910000098
其中，
Figure BDA0001853884910000099
函数Max(·)表示求取MVC(s)的函数最大值，标量
Figure BDA00018538849100000910
分别表示初始和终止速度。
为了满足公式(18)的路径速度约束，另一个可行域上边界描述为
M(s)＝ε,s∈[0,se]. (15)
引入用户参数ε后，可行域上边界重新描述为
MVC*(s)＝min(MVC(s),M(s)),s∈[0,se]. (16)
第2.2步，调节用户指定参数ε以改变可行域形状；
用户指定参数ε可以改变可行域形状，特别是其上边界MVC*(s)的幅值和形状。根据公式(14),(15),(16)可知，用户指定参数ε增大，则MVC*(s)的幅值增大且形状趋于曲线MVC(s)；用户指定参数ε减小，则MVC*(s)的幅值减小且形状趋于直线。因此，最优速度曲线对应的运动时间和巡航比例也随之发生改变。如图2所示，黑色虚线代表M(s)＝ε，随着用户调整ε参数大小，该虚线上下滑动以改变可行域上边界MVC*(s)的幅值和形状。
当用户指定参数ε减小趋向
Figure BDA0001853884910000102
时，可行域上边界的幅值在降低，这意味着最大路径速度在不断降低，可行速度曲线的最大值也在不断降低，那么最终生成的速度曲线对应的运动时间会不断增加。同时，可行域上边界的形状趋近于直线，这意味着可行速度曲线的巡航运动比例会不断提高。
当用户指定参数ε增大趋向Max(MVC*(s))时，可行域上边界的幅值在提高，这意味着最大路径速度在不断提高，可行速度曲线的最大值也在不断提高，那么最终生成的速度曲线对应的运动时间会不断降低。同时，可行域上边界的形状趋近于曲线MVC(s)，这意味着可行速度曲线的巡航比例会不断降低。
第3步，计算可行域边界上的匀速巡航部分；
本发明采用完备数值积分方法[20]计算曲线MVC*(s)下的可行速度曲线。该完备数值积分方法首先沿MVC*(s)搜索加速度转换区域，即满足公式(11)的部分MVC*(s)曲线段。然后，以加速度转换区域为起始，用公式(13)和(14)计算加速和减速曲线，并连接为可行速度曲线。
为了提高加速度转换区域的搜索效率，本发明描述匀速分界线概念，其数学定义如下
Figure BDA0001853884910000101
在该分界线上方
Figure BDA0001853884910000111
公式
Figure BDA0001853884910000112
成立。在该分界线下方
Figure BDA0001853884910000113
公式
Figure BDA0001853884910000114
成立。在该分界线上
Figure BDA0001853884910000115
公式
Figure BDA0001853884910000116
成立。
将已经得到的L(s)按照键值对
Figure BDA0001853884910000117
存储到哈希表。针对不同的M(s)，在常量时间复杂度O(1)内查询哈希表得到
Figure BDA0001853884910000118
M＝{M(s)|M(s)＜L(s),s∈[0,se]}, (19)
其中，
Figure BDA0001853884910000119
和M分别表示位于分界线L(s)上方和下方的部分M(s)曲线，两者的加速度均等于零，但是只有M满足公式(7)，可以作为MVC*(s)上的加速度转换区域。如图2所示，黑色点点虚线代表匀速分界线，其将M(s)＝ε分为
Figure BDA00018538849100001110
和M两部分。当用户指定参数ε增大时，
Figure BDA00018538849100001111
比重增大，而M比重减小。当用户指定参数ε减小时，
Figure BDA00018538849100001112
比重减小，而M比重增大。
第4步，利用完备数值积分策略计算可行速度曲线；
首先，沿MVC*(s)搜索所有加速度转换区域，当遇到第3步得到的M时，直接跳过，继续搜索剩余部分。然后，以这些加速度转换区域作为起点，用最大路径加速度
Figure BDA00018538849100001113
正向积分加速曲线，用最小路径加速度
Figure BDA00018538849100001114
反向积分减速曲线。最后，这些加速和减速曲线相交构成可行速度曲线。如图3所示，黑色实线β1,β2代表加速曲线，黑色实线α1,α2代表减速曲线，与匀速运动M一起构成可行速度曲线，但是在β1,β2与α1,α2交点处加速度不连续。特别的，如果规划问题本身是无解的，该完备数值积分策略会在有限时间内输出无解信号以提示用户规划问题是无解的。
第5步，利用双向积分策略使第4步得到的速度曲线加速度连续；
在加速曲线和减速曲线交点p1两侧选择点p2和p3，且点p2和p3位于加速曲线和减速曲线上。注意，点p2和点p1之间不存在其他交点，点p3和点p1之间也不存在其他交点。
以点p2为起点，正向积分一条速度曲线l1，其路径加速度为
Figure BDA00018538849100001115
以点p3为起点，反向积分一条速度曲线l2，其路径加速度为
Figure BDA0001853884910000121
其中，标量
Figure BDA0001853884910000122
分别表示点pi的路径位置，路径速度和路径加速度，而标量
Figure BDA0001853884910000123
和
Figure BDA0001853884910000124
的表达式如下
Figure BDA0001853884910000125
Figure BDA0001853884910000126
Figure BDA0001853884910000127
Figure BDA0001853884910000128
速度曲线l1,l2会在
Figure BDA0001853884910000129
路径位置处连接，并且连接点的加速度是连续的。按照此法，处理第4步所得速度曲线内的所有交点，则最终生成的速度曲线的加速度是连续的。如图4所示，双向积分测量使得最终生成的速度曲线是加速度连续的，并且用户通过改变参数ε，既可以输出加速度连续的运动时间最优解，又可以输出拥有高巡航比例的可行解。
第6步，实验效果描述
为验证上述内嵌运动性能调节机制的速度规划方法的有效性，本发明方法在型号为“NK-OMNI I”的全方位移动机器人上进行了实验验证。给定路径选择三阶贝塞尔曲线，路径控制点为P0＝[0.0 0.0]T,P1＝[1.3 2.2]T,P2＝[2.5 -1.7]T,P3＝[3.5 0.0]T，单位m。
将主动轮的速度约束设定为vmax＝[18.0 18.0 18.0 18.0]T，单位rad/s，主动轮的加速度约束设定为amax＝[20.0 20.0 20.0 20.0]T，单位rad/s2。如图5所示，用户指定参数ε＝0.6改变了可行域上边界MVC*(s)。其中，根据公式(14)可知，用户指定参数ε的上限和下限分别等于Max(MVC(s))＝1.3和
Figure BDA00018538849100001210
然后，利用完备数值积分策略[20]，在MVC*(s)下生成一条加速度不连续的可行速度曲线。该曲线由黑色实线β1,β2,α1,α2以及M(s)构成。最后，利用双向积分策略修复加速度不连续的交点。如图6所示，黑色点线顺利将交点(β1,β2,α1,α2,M(s)之间的交点)两侧的速度曲线连接，并且保证连续的加速度。该实验结果说明了本发明方法所输出的速度曲线的加速度是连续的。
将主动轮的速度约束设定为vmax＝[8.0 8.0 8.0 8.0]T，单位rad/s，主动轮的加速度约束设定为amax＝[2.0 2.0 2.0 2.0]T，单位rad/s2。为了凸显本发明方法的内嵌运动性能调节机制，给出与现有方法[17]的实验对比结果。[17]的核心思路是将规划问题转化为非线性规划问题，然后利用数值优化工具，例如FTM或者SQP，来完成最优解的求解。如图7所示，当用户指定参数ε设置为最大值Max(MVC(s))＝0.63时，本发明所提方法耗时40毫秒输出运动时间全局最优速度曲线。与[17]方法输出的速度曲线相比，本发明方法所输出的速度曲线对应的运动时间更短。当用户将参数降低到ε＝0.26，本发明方法耗时2毫秒输出可行速度曲线。该速度曲线的运动时间与[17]方法输出的速度曲线相同，但是其拥有较高的巡航比例。如图8到图15所示，相对[17]方法，本发明方法输出的速度曲线能带来更低的跟踪误差，并且机器人电机的速度和加速度曲线更光滑。
参考文献
[1]A.Gasparetto,V.Zanotto.A new method for smooth trajectory planningof robot manipulators.Mechanism and Machine Theory,2007,42(4):455-471.
[2]L.Jaillet,J.Cortés,T.Siméon.Sampling-based path planning onconfiguration-space costmaps.IEEE Transactions on Robotics,2010,26(4):635-646.
[3]K.Shin,N.Mckay.Minimum-time control of robotic manipulators withgeometric path constraints.IEEE Transactions on Automatic Control,1985,30(6):531-541.
[4]J.Bobrow,S.Dubowsky,J.Gibson.Time-optimal control of roboticmanipulators along specified paths.International Journal of RoboticsResearch,1985,4(3):3-17.
[5]C.Bianco.Minimum-jerk velocity planning for mobile robotapplications.IEEE Transactions on Robotics,2013,29(5):1317-1326.
[6]A.Piazzi,A.Visioli.Global minimum-jerk trajectory planning ofrobot manipulators.IEEE Transactions on Industrial Electronics,2000,47(1):140-149.
[7]B.Cao,G.Doods,G.Irwin.Time-optimal and smooth constrained pathplanning for robot manipulators.Proceedings of 1994IEEE InternationalConference on Robotics and Automation,1994:1853-1858.
[8]V.Zanotto,A.Gasparetto,A.Lanzutti,P.Boscariol,R.Vidoni.Experimental validation of minimum time-jerk algorithms forindustrial robots.Journal of Intelligent and Robotic Systems,2011,64(2):197-219.
[9]D.Ortiz,S.Westerberg,P.Hera,U.Mettin,L.Freidovich.Increasing thelevel of automation in the forestry logging process with crane trajectoryplanning and control.Journal of Field Robotics,2014,31(3):343-363.
[10]S.Macfarlane,E.Croft.Jerk-bounded manipulator trajectoryplanning:Design for real-time applications.IEEE Transactions on Robotics andAutomation,2003,19(1):42-52.
[11]L.Liu,C.Chen,X.Zhao,Y.Li.Smooth trajectory planning for aparallel manipulator with joint friction and jerk constraints.InternationalJournal of Control Automation and Systems,2016,14(4):1022-1036.
[12]D.González,J.Pérez,V.Milanés,F.Nashashibi.A review of motionplanning techniques for automated vehicles.IEEE Transactions on IntelligentTransportation Systems,2016,17(4):1135-1145.
[13]H.Nguyen,Q.-C.Pham.Time-optimal path parameterization of rigid-body motions:applications to spacecraft reorientation.Journal of Guidance,Control,and Dynamics,2016,39(7):1665-1669.
[14]S.Singh,M.Leu.Optimal trajectory generation for roboticmanipulators using dynamic programming.Journal of Dynamic Systems Measurementand Control,1987,109(2):88-96.
[15]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.
[16]H.Liu,X.Lai,W.Wu.Time-optimal and jerk-continuous trajectoryplanning for robot manipulators with kinematic constraints.Robotics andComputer-Integrated Manufacturing,2013,29(2):309-317.
[17]D.Constantinescu,E.Croft.Smooth and time-optimal trajectoryplanning for industrial manipulators along specified paths.Journal of RoboticSystems,2000,17(5):233-249.
[18]S.Kucuk.Optimal trajectory generation algorithm for serial andparallel manipulators.Robotics and Computer-Integrated Manufacturing,2017,48:219-232.
[19]S.Baraldo,A.Valente.Smooth joint motion planning for highprecision reconfigurable robot manipulators.Proceedings of 2017 IEEEInternational Conference on Robotics and Automation,2017:845-850.
[20]P.Shen,X.Zhang,Y.Fang.Complete and time-optimal path-constrainedtrajectory planning with torque and velocity constraints:Theory andApplications.IEEE/ASME Transactions on Mechatronics,2018,23(2):735-746.

Claims (2)

1.一种内嵌运动性能调节机制的速度规划方法，该方法具体步骤如下：
第1步，将速度规划问题转换为路径位置和路径速度的二维规划问题，并计算其可行域；
第2步，为二维规划引入运动性能调节机制；
第2.1步，定义运动性能调节机制内的用户指定参数ε，具体步骤如下：
该用户指定参数ε的物理意义为机器人系统路径速度的匀速上限，即约束
Figure FDA0003214669640000011
其中，
Figure FDA0003214669640000012
se代表路径总长度，函数Max(·)表示求取MVC(s)的函数最大值，MVC(s)代表可行域上边界，标量
Figure FDA0003214669640000013
分别表示初始和终止速度；
为了满足公式(14)的路径速度约束，另一个可行域上边界描述为
M(s)＝ε,s∈[0,se]； (15)
引入用户指定参数ε后，可行域上边界重新描述为
MVC*(s)＝min(MVC(s),M(s)),s∈[0,se]； (16)；
第2.2步，调节用户指定参数ε以改变可行域形状；
第3步，计算可行域边界上的匀速巡航部分，即机器人速度上限中的常量部分；
第4步，利用完备数值积分策略计算曲线MVC*(s)下的可行速度曲线，具体步骤如下：
首先沿MVC*(s)搜索加速度转换区域；然后，以加速度转换区域为起始，计算加速和减速曲线，并连接为可行速度曲线；
为了提高加速度转换区域的搜索效率，描述匀速分界线概念L(s)的数学定义如下
Figure FDA0003214669640000014
其中，Ai(s),Bi(s),Ci(s)是关于路径位置s的非线性函数，通过加速度约束联立等式方程获得；在该分界线上方
Figure FDA0003214669640000015
公式
Figure FDA0003214669640000016
成立；在该分界线下方
Figure FDA0003214669640000017
公式
Figure FDA0003214669640000018
成立；在该分界线上
Figure FDA0003214669640000019
公式
Figure FDA00032146696400000110
成立，其中，
Figure FDA00032146696400000111
代表最大路径加速度，
Figure FDA00032146696400000112
代表最小路径加速度；
将已经得到的L(s)按照键值对
Figure FDA0003214669640000021
存储到哈希表；针对不同的M(s)，在常量时间复杂度O(1)内查询哈希表得到
Figure FDA0003214669640000022
M＝{M(s)|M(s)＜L(s),s∈[0,se]}, (19)
其中，
Figure FDA0003214669640000023
和M的加速度均等于零，但是只有M可以作为MVC*(s)上的加速度转换区域，其中，M(s)是用户指定参数ε决定的常量函数；
第5步，利用双向积分策略使第4步得到的速度曲线加速度连续，具体步骤如下：
在加速曲线与减速曲线交点p1两侧选择点p2和p3，且点p2和p3分别位于加速曲线或者减速曲线上；点p2和点p1之间不存在其他交点，点p3和点p1之间也不存在其他交点；
以点p2为起点，正向积分一条速度曲线l1，其路径加速度为
Figure FDA0003214669640000024
以点p3为起点，反向积分一条速度曲线l2，其路径加速度为
Figure FDA0003214669640000025
其中，标量
Figure FDA0003214669640000026
分别表示点pi的路径位置、路径速度和路径加速度，而标量
Figure FDA0003214669640000027
和
Figure FDA0003214669640000028
的表达式如下
Figure FDA0003214669640000029
Figure FDA00032146696400000210
Figure FDA00032146696400000211
Figure FDA00032146696400000212
速度曲线l1,l2会在
Figure FDA00032146696400000213
路径位置处连接，并且连接点的加速度是连续的；按照此法，处理所有的加速曲线与减速曲线交点，则最终生成的速度曲线的加速度是连续的。
2.根据权利要求1所述的内嵌运动性能调节机制的速度规划方法，其特征在于，第2.2步所述的调节用户指定参数ε以改变可行域形状，具体步骤如下：
用户指定参数ε可以改变可行域形状上边界的幅值和形状；通过改变可行域的形状，最优速度曲线对应的运动时间和巡航比例也随之发生改变；
当用户指定参数ε减小趋向
Figure FDA0003214669640000031
时，可行域上边界MVC*(s)的幅值在降低，其中标量
Figure FDA0003214669640000032
分别表示初始和终止速度，这意味着最大路径速度在不断降低，可行速度曲线的最大值也在不断降低，那么最终生成的速度曲线对应的运动时间会不断增加；同时，可行域上边界MVC*(s)的形状趋近于直线，这意味着可行速度曲线的巡航运动比例会不断提高；
当用户指定参数ε增大趋向Max(MVC*(s))时，可行域上边界MVC*(s)的幅值在提高，这意味着最大路径速度在不断提高，可行速度曲线的最大值也在不断提高，那么最终生成的速度曲线对应的运动时间会不断降低；同时，可行域上边界MVC*(s)的形状趋近于曲线MVC(s)，这意味着可行速度曲线的巡航比例会不断降低。
CN201811306985.5A 2018-11-05 2018-11-05 内嵌运动性能调节机制的速度规划方法 Active CN109188915B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201811306985.5A CN109188915B (zh) 2018-11-05 2018-11-05 内嵌运动性能调节机制的速度规划方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201811306985.5A CN109188915B (zh) 2018-11-05 2018-11-05 内嵌运动性能调节机制的速度规划方法

Publications (2)

Publication Number Publication Date
CN109188915A CN109188915A (zh) 2019-01-11
CN109188915B true CN109188915B (zh) 2021-10-29

Family

ID=64941804

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201811306985.5A Active CN109188915B (zh) 2018-11-05 2018-11-05 内嵌运动性能调节机制的速度规划方法

Country Status (1)

Country Link
CN (1) CN109188915B (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113703433B (zh) * 2020-05-21 2024-05-14 北京配天技术有限公司 机器人运动轨迹的速度规划方法及装置
CN114237047B (zh) 2021-12-10 2022-09-23 广东工业大学 一种基于约束分类的时间最优速度规划方法及系统

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP2219092A1 (en) * 2009-02-04 2010-08-18 Magneti Marelli S.p.A. Method for controlling the speed of a vehicle
WO2012044881A2 (en) * 2010-09-30 2012-04-05 Potens Ip Holdings Llc System for simulating manual transmission operation in a vehicle
EP2997426A1 (en) * 2013-05-15 2016-03-23 ABB Technology AG Electrical drive system with model predictive control of a mechanical variable
CN106695787A (zh) * 2016-12-17 2017-05-24 上海新时达电气股份有限公司 一种速度规划方法
CN107490965A (zh) * 2017-08-21 2017-12-19 西北工业大学 一种空间自由漂浮机械臂的多约束轨迹规划方法
CN107844058A (zh) * 2017-11-24 2018-03-27 北京特种机械研究所 一种运动曲线离散动态规划方法
CN107943034A (zh) * 2017-11-23 2018-04-20 南开大学 移动机器人沿给定路径的完备且最短时间轨迹规划方法

Family Cites Families (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9469029B2 (en) * 2014-07-31 2016-10-18 Siemens Industry Software Ltd. Method and apparatus for saving energy and reducing cycle time by optimal ordering of the industrial robotic path
US9524647B2 (en) * 2015-01-19 2016-12-20 The Aerospace Corporation Autonomous Nap-Of-the-Earth (ANOE) flight path planning for manned and unmanned rotorcraft
CN105883616B (zh) * 2016-06-13 2017-06-16 南开大学 桥式吊车最短时间防摆轨迹实时生成方法
US10480947B2 (en) * 2016-12-21 2019-11-19 X Development Llc Boolean satisfiability (SAT) reduction for geometry and kinematics agnostic multi-agent planning
CN106647282B (zh) * 2017-01-19 2020-01-03 北京工业大学 一种考虑末端运动误差的六自由度机器人轨迹规划方法
CN107826978B (zh) * 2017-03-15 2019-10-18 南京工业大学 一种双摆桥式起重机的速度轨迹规划消摆方法
WO2018185522A1 (en) * 2017-04-04 2018-10-11 Graf Plessen Mogens Coordination of harvesting and transport units for area coverage
CN108180914B (zh) * 2018-01-09 2021-06-18 昆明理工大学 一种基于蚁群改进和尖峰平滑的移动机器人路径规划方法
CN108549328B (zh) * 2018-03-22 2020-05-26 汇川技术（东莞）有限公司 自适应速度规划方法及系统
CN108681787B (zh) * 2018-04-28 2021-11-16 南京航空航天大学 基于改进双向快速扩展随机树算法的无人机路径优化方法
CN108594757B (zh) * 2018-05-15 2021-01-01 南京旭上数控技术有限公司 一种基于位置和姿态约束的机器人小线段前瞻规划方法
CN108621165B (zh) * 2018-05-28 2021-06-15 兰州理工大学 障碍环境下的工业机器人动力学性能最优轨迹规划方法

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP2219092A1 (en) * 2009-02-04 2010-08-18 Magneti Marelli S.p.A. Method for controlling the speed of a vehicle
WO2012044881A2 (en) * 2010-09-30 2012-04-05 Potens Ip Holdings Llc System for simulating manual transmission operation in a vehicle
EP2997426A1 (en) * 2013-05-15 2016-03-23 ABB Technology AG Electrical drive system with model predictive control of a mechanical variable
CN106695787A (zh) * 2016-12-17 2017-05-24 上海新时达电气股份有限公司 一种速度规划方法
CN107490965A (zh) * 2017-08-21 2017-12-19 西北工业大学 一种空间自由漂浮机械臂的多约束轨迹规划方法
CN107943034A (zh) * 2017-11-23 2018-04-20 南开大学 移动机器人沿给定路径的完备且最短时间轨迹规划方法
CN107844058A (zh) * 2017-11-24 2018-03-27 北京特种机械研究所 一种运动曲线离散动态规划方法

Also Published As

Publication number Publication date
CN109188915A (zh) 2019-01-11

Similar Documents

Publication Publication Date Title
Hu et al. An efficient RRT-based framework for planning short and smooth wheeled robot motion under kinodynamic constraints
CN104834309B (zh) 基于目标跟踪控制策略的单移动机器人最优巡回控制方法
Wen et al. Elman fuzzy adaptive control for obstacle avoidance of mobile robots using hybrid force/position incorporation
Jin et al. A fine-interpolation-based parametric interpolation method with a novel real-time look-ahead algorithm
Shen et al. Real-time acceleration-continuous path-constrained trajectory planning with built-in tradeoff between cruise and time-optimal motions
Pham et al. On the structure of the time-optimal path parameterization problem with third-order constraints
Wen et al. A novel 3D path following control framework for robots performing surface finishing tasks
CN114043480A (zh) 一种基于模糊控制的自适应阻抗控制算法
CN109188915B (zh) 内嵌运动性能调节机制的速度规划方法
Matin et al. Design Intelligent PID like Fuzzy Sliding Mode Controller for Spherical Motor
Xu et al. Model predictive control-based path tracking control for automatic guided vehicles
Chen et al. An adaptive path tracking controller based on reinforcement learning with urban driving application
CN113848905A (zh) 基于神经网络和自适应控制的移动机器人轨迹跟踪方法
Xu et al. Trajectory planning with Bezier curve in Cartesian space for industrial gluing robot
Liu et al. Extended model predictive control scheme for smooth path following of autonomous vehicles
Bai et al. Relative navigation control of articulated vehicle based on LTV-MPC
Ning et al. Time-optimal point stabilization control for WIP vehicles using quasi-convex optimization and B-spline adaptive interpolation techniques
CN113867157B (zh) 一种控制补偿的最优轨迹规划方法、设备及存储设备
Muller et al. Trajectory generation and feedforward control for parking a car
CN114800521A (zh) 一种带约束的三自由度机械臂固定路径点运动控制系统
Sidobre et al. Online task space trajectory generation
Huo et al. Design and validation of a novel adaptive motion control for a pendulum spherical robot
CN114840947A (zh) 一种带约束的三自由度机械臂动力学模型
Huang et al. Overview of trajectory planning methods for robot systems
Shen et al. Real-time acceleration-continuous path-constrained trajectory planning with built-in tradability between cruise and time-optimal motions

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