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

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∈Rⁿ代表机器人系统状态量，n代表机器人状态维数，向量v∈R^m,a∈R^m分别代表机器人电机的速度和加速度量，m代表机器人电机总数，则机器人系统的动态扩展模型描述如下

其中，J(q)∈R^m×n是向量q的雅克比矩阵。

沿着给定路径，机器人状态重新表示为q(s)，其中s代表路径位置。进而，公式(1)和(2)可重新表示为

其中，

机器人系统的物理约束表示如下

-v_max≤v≤v_max, (5)

-a_max≤a≤a_max, (6)

其中，常向量v_max∈R^m和a_max∈R^m分别是机器人电机速度和加速度的上限。

为满足机器人电机的加速度约束，将公式(4)带入公式(6)得

其中，

A(s)＝[(J(q(s))q_s)^T-(J(q(s))q_s)^T]^T,

B(s)＝[(J_sq_s+J(q(s))q_ss)^T-(J_sq_s+J(q(s))q_ss)^T]^T,

为满足机器人电机的速度约束，将公式(3)带入公式(5)得

其中，

根据公式(7)，计算最小路径加速度和最大路径加速度分别如下：

其中，标量A_i(s),B_i(s),C_i(s)分别是向量A(s),B(s),C(s)的元素。

根据公式(7),(8),(9),(10)可得路径位置和路径速二维空间内的可行域上边界，

MVC(s)＝min(MVC_V(s),MVC_A(s)),s∈[0,s_e], (11)

其中，s_e代表路径总长度，而代表电机速度约束的MVC_V(s)和代表电机加速度约束的MVC_A(s)表达式公式如下

MVC_V(s)＝min{-D_i(s)/A_i(s)|A_i(s)＞0,i∈[1,2m]}. (13)

在路径位置和路径速度二维空间的下边界为左右边界表达式分别为s＝0,s_e＝0。由这些边界围成的多边形就是路径位置和路径速度二维空间内的可行域。

第2步，为二维规划引入运动性能调节机制；

第2.1步，定义运动性能调节机制内的用户指定参数ε；

该用户指定参数定义为机器人系统路径速度的匀速上限，约束如下，

其中，函数Max(·)表示求取MVC(s)的函数最大值，标量分别表示初始和终止速度。

为了满足公式(14)的路径速度约束，另一个可行域的上边界描述为

M(s)＝ε,s∈[0,s_e]. (15)

引入用户指定参数ε后，可行域上边界重新描述为

MVC^*(s)＝min(MVC(s),M(s)),s∈[0,s_e]. (16)

第2.2步，调节用户指定参数ε以改变可行域形状；

用户指定的参数ε可以改变可行域形状，特别是其上边界的幅值和形状。通过改变可行域的形状，最优速度曲线对应的运动时间和巡航比例也随之发生改变。

当用户指定参数ε减小趋向时，可行域上边界的幅值在降低，这意味着最大路径速度在不断降低，可行速度曲线的最大值也在不断降低，那么最终生成的速度曲线对应的运动时间会不断增加。同时，可行域上边界的形状趋近于直线，这意味着可行速度曲线的巡航运动比例会不断提高。

当用户指定参数ε增大趋向Max(MVC*(s))时，可行域上边界的幅值在提高，这意味着最大路径速度在不断提高，可行速度曲线的最大值也在不断提高，那么最终生成的速度曲线对应的运动时间会不断降低。同时，可行域上边界的形状趋近于曲线MVC(s)，这意味着可行速度曲线的巡航比例会不断降低。

第3步，计算可行域边界上的匀速巡航部分；

本发明采用完备数值积分方法(参考文献[20])计算曲线MVC^*(s)下的可行速度曲线。该完备数值积分方法首先沿MVC^*(s)搜索加速度转换区域，即满足公式(7)的部分MVC^*(s)曲线段。然后，以加速度转换区域为起始，用公式(9)和(10)计算加速和减速曲线，并连接为可行速度曲线。

为了提高加速度转换区域的搜索效率，本发明描述匀速分界线概念L(s)，其数学定义如下

在该分界线上方公式成立。在该分界线下方公式成立。在该分界线上公式成立。

将已经得到的L(s)按照键值对存储到哈希表。针对不同的M(s)，在常量时间复杂度O(1)内查询哈希表得到

M＝{M(s)|M(s)＜L(s),s∈[0,s_e]}, (19)

其中，和M分别表示位于分界线L(s)上方和下方的部分M(s)曲线，两者的加速度均等于零，但是只有M满足公式(7)，可以作为MVC^*(s)上的加速度转换区域。

第4步，利用完备数值积分策略计算可行速度曲线；

首先，沿MVC^*(s)搜索所有加速度转换区域，当遇到第3步得到的M时，直接跳过，继续搜索剩余部分。然后，以这些加速度转换区域作为起点，用最大路径加速度正向积分加速曲线，用最小路径加速度反向积分减速曲线。最后，这些加速和减速曲线相交构成可行速度曲线。特别的，如果规划问题本身是无解的，该完备数值积分策略会在有限时间内输出无解信号以提示用户规划问题是无解的。

第5步，利用双向积分策略使第4步得到的速度曲线加速度连续；

在加速曲线和减速曲线交点p₁两侧选择点p₂和p₃，且点p₂和p₃分别位于加速曲线和减速曲线上。注意，点p₂和点p₁之间不存在其他交点，点p₃和点p₁之间也不存在其他交点。

以点p₂为起点，正向积分一条速度曲线l₁，其路径加速度为

以点p₃为起点，反向积分一条速度曲线l₂，其路径加速度为

其中，标量分别表示点p_i的路径位置，路径速度和路径加速度，而标量和的表达式如下

速度曲线l₁,l₂会在路径位置处连接，并且连接点的加速度是连续的。按照此法，处理第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所示)：

其中，q＝[x y θ]^T是机器人位姿，[xy]^T∈R²是机器人中心O_r在世界坐标系X_wO_wY_w下的位置，θ∈R是机器人的方向角，v,a∈R⁴分别代表主动轮的速度和加速度，矩阵J如下：

J(q)＝[J₁ J₂ J₃ J₄]^T,

给定路径选择k阶贝塞尔曲线，其数学表达式如下：

其中，在世界坐标系X_wO_wY_w下位置坐标P_i＝[x_i y_i]^T,i∈[0,n]是路径控制点。λ∈[0,1]是路径参数，与路径位置s存在非线性映射。由于路径是已知，可以提前建立λ与s的映射表。

沿该给定路径，机器人位姿重新表示为q(s)，其中s代表路径位置。进而，公式(1)和(2)可重新表示为

其中，

由于沿给定路径移动，每个主动轮偏转角η₁,η₂关于路径位置s如下：

主动轮的速度和加速度约束如下：

-v_max≤v≤v_max, (5)

-a_max≤a≤a_max, (6)

其中，常向量v_max∈R⁴和a_max∈R⁴分别是主动轮速度和加速度的上限。

为满足主动轮的加速度约束，将公式(6)带入公式(10)得

其中，

A(s)＝[(Jq_s)^T(-Jq_s)^T]^T,

为满足主动轮的速度约束，将公式(5)带入公式(9)得

其中，

根据公式(7),(8),(9),(10)可得路径位置和路径速度二维空间内的可行域上边界，

MVC(s)＝min(MVC_V(s),MVC_A(s)),s∈[0,s_e], (11)

其中，s_e代表路径总长度，而代表电机速度约束的MVC_V(s)曲线和代表电机加速度约束的MVC_A(s)曲线表达式公式如下

MVC_V(s)＝min{-D_i(s)/A_i(s)|A_i(s)＞0,i∈[1,8]}. (13)

在路径位置和路径速度二维空间的下边界为左右边界表达式分别为s＝0,s_e＝0。由这些边界围成的多边形就是路径位置和路径速度二维空间内的可行域。最终，如图2所示，全方位移动机器人的速度规划问题被转换为路径位置和路径速度的二维规划问题。其中，可行域由黑色点虚线MVC_A(s)，黑色虚线MVC_V(s)，下边界左边界s＝0和右边界s＝s_e围成(s_e代表路径总长度)。

第2步，为二维规划引入运动性能调节机制；

第2.1步，定义运动性能调节机制内的用户指定参数ε；

该用户指定参数定义为路径速度约束如下

其中，函数Max(·)表示求取MVC(s)的函数最大值，标量分别表示初始和终止速度。

为了满足公式(18)的路径速度约束，另一个可行域上边界描述为

M(s)＝ε,s∈[0,s_e]. (15)

引入用户参数ε后，可行域上边界重新描述为

MVC^*(s)＝min(MVC(s),M(s)),s∈[0,s_e]. (16)

第2.2步，调节用户指定参数ε以改变可行域形状；

用户指定参数ε可以改变可行域形状，特别是其上边界MVC^*(s)的幅值和形状。根据公式(14),(15),(16)可知，用户指定参数ε增大，则MVC^*(s)的幅值增大且形状趋于曲线MVC(s)；用户指定参数ε减小，则MVC^*(s)的幅值减小且形状趋于直线。因此，最优速度曲线对应的运动时间和巡航比例也随之发生改变。如图2所示，黑色虚线代表M(s)＝ε，随着用户调整ε参数大小，该虚线上下滑动以改变可行域上边界MVC^*(s)的幅值和形状。

当用户指定参数ε减小趋向时，可行域上边界的幅值在降低，这意味着最大路径速度在不断降低，可行速度曲线的最大值也在不断降低，那么最终生成的速度曲线对应的运动时间会不断增加。同时，可行域上边界的形状趋近于直线，这意味着可行速度曲线的巡航运动比例会不断提高。

当用户指定参数ε增大趋向Max(MVC^*(s))时，可行域上边界的幅值在提高，这意味着最大路径速度在不断提高，可行速度曲线的最大值也在不断提高，那么最终生成的速度曲线对应的运动时间会不断降低。同时，可行域上边界的形状趋近于曲线MVC(s)，这意味着可行速度曲线的巡航比例会不断降低。

第3步，计算可行域边界上的匀速巡航部分；

本发明采用完备数值积分方法[20]计算曲线MVC^*(s)下的可行速度曲线。该完备数值积分方法首先沿MVC^*(s)搜索加速度转换区域，即满足公式(11)的部分MVC^*(s)曲线段。然后，以加速度转换区域为起始，用公式(13)和(14)计算加速和减速曲线，并连接为可行速度曲线。

为了提高加速度转换区域的搜索效率，本发明描述匀速分界线概念，其数学定义如下

在该分界线上方公式成立。在该分界线下方公式成立。在该分界线上公式成立。

将已经得到的L(s)按照键值对存储到哈希表。针对不同的M(s)，在常量时间复杂度O(1)内查询哈希表得到

M＝{M(s)|M(s)＜L(s),s∈[0,s_e]}, (19)

其中，和M分别表示位于分界线L(s)上方和下方的部分M(s)曲线，两者的加速度均等于零，但是只有M满足公式(7)，可以作为MVC^*(s)上的加速度转换区域。如图2所示，黑色点点虚线代表匀速分界线，其将M(s)＝ε分为和M两部分。当用户指定参数ε增大时，比重增大，而M比重减小。当用户指定参数ε减小时，比重减小，而M比重增大。

第4步，利用完备数值积分策略计算可行速度曲线；

首先，沿MVC^*(s)搜索所有加速度转换区域，当遇到第3步得到的M时，直接跳过，继续搜索剩余部分。然后，以这些加速度转换区域作为起点，用最大路径加速度正向积分加速曲线，用最小路径加速度反向积分减速曲线。最后，这些加速和减速曲线相交构成可行速度曲线。如图3所示，黑色实线β₁,β₂代表加速曲线，黑色实线α₁,α₂代表减速曲线，与匀速运动M一起构成可行速度曲线，但是在β₁,β₂与α₁,α₂交点处加速度不连续。特别的，如果规划问题本身是无解的，该完备数值积分策略会在有限时间内输出无解信号以提示用户规划问题是无解的。

第5步，利用双向积分策略使第4步得到的速度曲线加速度连续；

在加速曲线和减速曲线交点p₁两侧选择点p₂和p₃，且点p₂和p₃位于加速曲线和减速曲线上。注意，点p₂和点p₁之间不存在其他交点，点p₃和点p₁之间也不存在其他交点。

以点p₂为起点，正向积分一条速度曲线l₁，其路径加速度为

以点p₃为起点，反向积分一条速度曲线l₂，其路径加速度为

其中，标量分别表示点p_i的路径位置，路径速度和路径加速度，而标量和的表达式如下

速度曲线l₁,l₂会在路径位置处连接，并且连接点的加速度是连续的。按照此法，处理第4步所得速度曲线内的所有交点，则最终生成的速度曲线的加速度是连续的。如图4所示，双向积分测量使得最终生成的速度曲线是加速度连续的，并且用户通过改变参数ε，既可以输出加速度连续的运动时间最优解，又可以输出拥有高巡航比例的可行解。

第6步，实验效果描述

为验证上述内嵌运动性能调节机制的速度规划方法的有效性，本发明方法在型号为“NK-OMNI I”的全方位移动机器人上进行了实验验证。给定路径选择三阶贝塞尔曲线，路径控制点为P₀＝[0.0 0.0]^T,P₁＝[1.3 2.2]^T,P₂＝[2.5 -1.7]^T,P₃＝[3.5 0.0]^T，单位m。

将主动轮的速度约束设定为v_max＝[18.0 18.0 18.0 18.0]^T，单位rad/s，主动轮的加速度约束设定为a_max＝[20.0 20.0 20.0 20.0]^T，单位rad/s²。如图5所示，用户指定参数ε＝0.6改变了可行域上边界MVC^*(s)。其中，根据公式(14)可知，用户指定参数ε的上限和下限分别等于Max(MVC(s))＝1.3和然后，利用完备数值积分策略[20]，在MVC^*(s)下生成一条加速度不连续的可行速度曲线。该曲线由黑色实线β₁,β₂,α₁,α₂以及M(s)构成。最后，利用双向积分策略修复加速度不连续的交点。如图6所示，黑色点线顺利将交点(β₁,β₂,α₁,α₂,M(s)之间的交点)两侧的速度曲线连接，并且保证连续的加速度。该实验结果说明了本发明方法所输出的速度曲线的加速度是连续的。

将主动轮的速度约束设定为v_max＝[8.0 8.0 8.0 8.0]^T，单位rad/s，主动轮的加速度约束设定为a_max＝[2.0 2.0 2.0 2.0]^T，单位rad/s²。为了凸显本发明方法的内嵌运动性能调节机制，给出与现有方法[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 (5)

1.一种内嵌运动性能调节机制的速度规划方法，该方法具体步骤如下：

第1步，将速度规划问题转换为路径位置和路径速度的二维规划问题，并计算其可行域；

第2步，为二维规划引入运动性能调节机制；

第2.1步，定义运动性能调节机制内的用户指定参数ε；该参数的物理含义是机器人系统路径速度的匀速上限；

第2.2步，调节用户指定参数ε以改变可行域形状；

第3步，计算可行域边界上的匀速巡航部分；

第4步，利用完备数值积分策略计算可行速度曲线；

第5步，利用双向积分策略使第4步得到的速度曲线加速度连续。

2.根据权利要求1所述的内嵌运动性能调节机制的速度规划方法，其特征在于，第2.1步所述的定义运动性能调节机制内的用户调节参数，具体步骤如下：

该用户指定参数ε的物理意义为机器人系统路径速度的匀速上限，即约束

其中，s∈[0,s_e]，s_e代表路径总长度，函数Max(·)表示求取MVC(s)的函数最大值，MVC(s)代表可行域上边界，标量分别表示初始和终止速度；

为了满足公式(14)的路径速度约束，另一个可行域上边界描述为

M(s)＝ε,s∈[0,s_e]. (15)

引入用户指定参数ε后，可行域上边界重新描述为

MVC^*(s)＝min(MVC(s),M(s)),s∈[0,s_e]. (16)。

3.根据权利要求1所述的内嵌运动性能调节机制的速度规划方法，其特征在于，第2.2步所述的调节用户指定参数ε以改变可行域形状，具体步骤如下：

用户指定参数ε可以改变可行域形状，特别是其上边界的幅值和形状；通过改变可行域的形状，最优速度曲线对应的运动时间和巡航比例也随之发生改变；

当用户指定参数ε减小趋向时，可行域上边界MVC^*(s)的幅值在降低，这意味着最大路径速度在不断降低，可行速度曲线的最大值也在不断降低，那么最终生成的速度曲线对应的运动时间会不断增加；同时，可行域上边界MVC^*(s)的形状趋近于直线，这意味着可行速度曲线的巡航运动比例会不断提高；

当用户指定参数ε增大趋向Max(MVC^*(s))时，可行域上边界MVC^*(s)的幅值在提高，这意味着最大路径速度在不断提高，可行速度曲线的最大值也在不断提高，那么最终生成的速度曲线对应的运动时间会不断降低；同时，可行域上边界MVC^*(s)的形状趋近于曲线MVC(s)，这意味着可行速度曲线的巡航比例会不断降低。

4.根据权利要求1所述的内嵌运动性能调节机制的速度规划方法，其特征在于，第3步所述的计算可行域边界上的匀速巡航部分，具体步骤如下：

采用完备数值积分方法计算曲线MVC^*(s)下的可行速度曲线；该完备数值积分方法首先沿MVC^*(s)搜索加速度转换区域；然后，以加速度转换区域为起始，计算加速和减速曲线，并连接为可行速度曲线；

为了提高加速度转换区域的搜索效率，描述匀速分界线概念L(s)的数学定义如下

在该分界线上方公式成立；在该分界线下方公式成立；在该分界线上公式成立；

将已经得到的L(s)按照键值对存储到哈希表。针对不同的M(s)，在常量时间复杂度O(1)内查询哈希表得到

其中，和M的加速度均等于零，但是只有M可以作为MVC^*(s)上的加速度转换区域。

5.根据权利要求1所述的内嵌运动性能调节机制的速度规划方法，其特征在于，第5步所述的利用双向积分策略使第4步生成的速度曲线加速度连续，具体步骤如下：

在加速曲线与减速曲线交点p₁两侧选择点p₂和p₃，且点p₂和p₃分别位于加速曲线或者减速曲线上；注意，点p₂和点p₁之间不存在其他交点，点p₃和点p₁之间也不存在其他交点；

以点p₂为起点，正向积分一条速度曲线l₁，其路径加速度为

以点p₃为起点，反向积分一条速度曲线l₂，其路径加速度为

其中，标量分别表示点p_i的路径位置、路径速度和路径加速度，而标量和的表达式如下

速度曲线l₁,l₂会在路径位置处连接，并且连接点的加速度是连续的；按照此法，处理所有的加速曲线与减速曲线交点，则最终生成的速度曲线的加速度是连续的。