时间最优非对称S型加减速控制方法

时间最优非对称s型加减速控制方法
技术领域
1.本发明涉及机械臂运动控制技术领域，具体涉及一种时间最优非对称s型加减速控制方法。


背景技术：

2.基于机器人机械臂的协作设备，是一种高精度，多输入多输出、高度非线性、强耦合的复杂系统，冗余机械臂具有更高的操作灵活性、更强的鲁棒性，在任务执行过程中表现出良好的适应能力，而且便于维修维护，已在敏捷制造、危险环境作业、空间在轨装配等领域得到广泛应用。
3.机器人控制系统要求在一定的控制策略的作用下，使机器人能够快速准确地完成给定任务。对于具有严格路径要求的任务，需要规划机械臂构型空间的运动轨迹，通过逆运动学算法求解得到关节轨迹。机械臂末端执行器的运动决定了执行任务的效率及精度，平稳的运行速度能够减缓机械部件磨损，避免系统的振动和冲击。对于复杂曲线，恒定的末端运行速度将导致各关节轴速度、加速度的不断变化，速度过大或速度变化过快可能会造成较大的几何路径误差。因此，机器人在启停阶段、速度变化时都必须通过加减速控制算法实现速度的平滑过渡。
4.目前，现有的加减速控制方法主要包括梯形加减速、三角函数加减速、指数加减速等，但大多数机械臂仍然采用最简单的梯形加减速控制方法来进行速度规划。随着高速、高精度的任务需求，非对称s型加减速控制技术得到了广泛关注。完整的s型速度曲线包括七个阶段：加加速段、匀加速段、减加速段、匀速段、加减速段、匀减速段、减减速段，非对称s型速度曲线允许各阶段存在不同的加加速度，与其他方法相比，涉及到更多的参数，使得规划过程更加困难，特别是对于具有复杂运动学约束的冗余机械臂。


技术实现要素：

5.针对传统加减速控制算法的不足，本发明提供一种基于智能算法的时间最优非对称s型加减速控制方法，以解决现有技术存在的问题。为实现上述目的，本发明提供一种时间最优非对称s型加减速控制方法，包括以下步骤：
6.s1、获取待规划机械臂路径的给定路径总长、起始点速度和终止点速度信息，根据待规划s型速度曲线是否存在加速区与减速区确定问题类型；
7.s2、建立适应度函数；
8.s3、输入鲸鱼算法中的预设参数对种群进行初始化，所述预设参数包括问题类型、鲸鱼种群规模、最大迭代次数；
9.s4、判断所述鲸鱼算法中搜索代理是否符合运动学约束条件；
10.其中，所述运动学约束条件包括：
11.任意时刻，机械臂各关节的运动应满足构型空间的加速度、速度约束条件极值和速度极值；
12.任意时刻，所规划的机械臂末端执行器的运动应满足操作空间的加加速度、加速度及速度约束条件；
13.所述s型速度剖面加速区的加速度方向为正，和所述减速区的加速度方向为负；
14.s5、基于鲸鱼算法及其更新公式，在所述最大迭代次数内通过不断更新搜索代理的位置获取种群中最好的解，得到基于鲸鱼算法的最优时间参数；
15.s6、根据所述鲸鱼算法输出的最优时间参数，计算s型速度曲线各阶段的加加速度，逐次积分得到加速度、速度及位移曲线，从而确定机械臂末端执行器的运动。
16.进一步地，所述步骤s1具体包括：
17.s101、定义参考路径长度，所述参考路径长度表示为公式(1)：
18.l
ref
＝l
acc
+l
dec
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1)
19.其中，l
ref
表示参考路径长度，l
acc
表示加速区参考路径长度，l
dec
表示减速区参考路径长度，根据公式组(2)和公式组(3)计算：
[0020][0021][0022]
其中，vs表示路径起始点的速度，ve表示路径终点的速度，vc表示给定的机械臂末端执行器恒速运动时的切向速度值，a
max
表示末端执行器的最大加速度，j
max
表示末端执行器的最大加加速度；
[0023]
s102、根据给定路径总长与参考路径长度之间的关系，判断待规划s型速度曲线是否存在加速区和/或减速区；其中，分为如下情况：
[0024]
若l≥l
ref
时，则该路径为长路径，所述待规划s型速度曲线同时具有加速区与减速区；
[0025]
若l《l
ref
时，则该路径为短路径，此时通过判断始末速度之间的关系，确定所述待规划s型速度曲线是否包括加速区与减速区。
[0026]
进一步地，所述判断待规划s型速度曲线是否包括加速区与减速区，分为如下情况：
[0027]
当vs》ve时，所述待规划s型速度曲线不包括加速区；
[0028]
当vs《ve时，所述待规划s型速度曲线不包括减速区；
[0029]
当vs＝ve时，所述待规划s型速度曲线既不包括加速区，也不包括减速区；
[0030]
其中，l表示给定路径总长。
[0031]
进一步地，所述步骤s2中所述适应度函数如公式(4)所示：
[0032][0033]
其中，tm(m＝1,2,
…
,7)表示所述待规划曲线第m段的执行时间，ωm表示所述待规划s型速度曲线第m段执行时间对应的权重因子。
[0034]
进一步地，所述步骤s3中，初始化种群表示为公式(5)：
[0035][0036]
其中，ns表示鲸鱼种群规模；xn,n＝1,2,
…
,ns表示一个搜索代理；t
m,n
∈[0,t
m,max
],m＝1,2,
…
,7,t
m,max
表示第m段执行时间的最大值；
[0037]
若同时存在加速区与减速区，各阶段时间均不设置为0；
[0038]
若不存在加速区，t1,t2,t3设置为0；
[0039]
若不存在减速区，t5,t6,t7设置为0；
[0040]
若既不存在加速区，也不存在减速区，t1,t2,t3,t5,t6,t7设置为0。
[0041]
进一步地，所述步骤s4中的所述运动学约束条件表示为公式(6)：
[0042][0043]
其中，分别表示机械臂第i个关节的加速度极值和速度极值；分别表示机械臂第i个关节任意时刻的加速度和速度；j
lim
、a
lim
、v
lim
分别表示机械臂末端执行器的加加速度极值、加速度极值和速度极值；jk、ak、vk分别表示机械臂末端执行器任意时刻的加加速度、加速度及速度；a
acc
和a
dec
分别表示加速区和减速区的加速度。
[0044]
进一步地，所述步骤s5中，所述鲸鱼算法的数学模型为：公式(7)—(11)：
[0045][0046][0047][0048][0049]
[0050]
其中，表示鲸鱼即搜索代理的位置向量，表示猎物即全局最优解的位置向量，δ表示当前迭代次数，表示猎物与鲸鱼的距离，表示决定影响鲸鱼行动距离的权重值，为定义在[0,2]上的随机向量，表示第δ次迭代中影响变化的权重值，在迭代过程中由2线性递减至0，是定义在[0,1]上的随机向量；
[0051]
更新后搜索代理的位置，为公式(12)—(17)：
[0052]
在[0,1]上定义一个随机数p，
[0053]
若p《0.5且则按照公式(12)和(13)进行更新
[0054][0055][0056]
若p《0.5且则按照公式(14)和(15)进行更新
[0057][0058][0059]
其中，表示从当前种群中选取的任意一个搜索代理的位置向量；
[0060]
若p≥0.5，则按照公式(16)和(17)进行更新
[0061][0062][0063]
其中，b是对数螺旋形状常数，l为定义在[-1,1]上的随机数。
[0064]
进一步地，所述步骤s6中，获取时间最优的非对称s型速度曲线具体表示为公式(18)—(25)：
[0065]
所述s型速度曲线各阶段的加加速度，表示为公式(18)—(22)：
[0066]
若同时存在加速区与减速区，则加加速度表示为公式(18)和公式(19)：
[0067]
[0068][0069]
若不存在加速区，则加加速度表示为公式(20)：
[0070][0071]
若不存在减速区，则加加速度表示为公式(21)：
[0072][0073]
若既不存在加速区，也不存在减速区，则加加速度表示为公式(22)：
[0074]
j1＝j2＝j3＝j4＝j5＝j6＝j7＝0
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(22)
[0075]
所述s型速度曲线各阶段的加速度、速度、位移，分别表示为公式(23)—(25)：
[0076]
所述s型速度曲线各阶段的加速度，表示为公式(23)：
[0077][0078]
所述s型速度曲线各阶段的速度，表示为公式(24)：
[0079][0080]
所述s型速度曲线各阶段的位移，表示为公式(25)：
[0081][0082]
其中，tm(m＝1,2,
…
,7)表示s型速度曲线第m段的执行时间，jm(m＝1,2,
…
,7)表示s型速度曲线第m段的加加速度值，l表示路径总长度，vs表示路径起始点的速度，ve表示路径终止点的速度。
[0083]
与现有技术相比，本发明具有以下优点：
[0084]
1、本发明设计了一种完整的s型加减速控制算法，通过判断待规划s型速度曲线是否存在加速区、匀速区和减速区，将其归类为四种情况，包含了非对称s型速度曲线的所有可能性。与传统的s型加减速控制算法相比，本发明参数计算简单且不依赖于任何简化假设，所得s型速度分布可以是不对称的，路径起始点和终止点的速度可以是非零的，可行性与适用性更好。
[0085]
2、本发明基于智能算法求解s型速度曲线最优时间参数时，考虑更多的运动学约束条件，不仅是机械臂末端执行器沿路径的限制，还包括各关节的运动限制，避免了由于机械臂操作空间与构型空间的非线性导致的运动范围超限，保障了末端执行器的轨迹跟踪性能。
[0086]
3、本发明首次将鲸鱼算法应用于机械臂末端执行器的速度规划中，该算法是一种
基于群体的全局优化算法，计算效率高、进化收敛速度快且精度高、鲁棒性强、可实现性好，能够有效地得到时间最优非对称s型速度分布，在不违反运动学约束条件的情况下，充分开发了机械臂的运动能力，缩短了任务执行时间，避免了关节电机饱和导致的轨迹跟踪误差。
附图说明
[0087]
图1是本发明实施例时间最优非对称s型加减速控制方法的流程示意图；
[0088]
图2是本发明实施例时间最优非对称s型加减速控制方法中的机械臂闭环逆运动学算法的示意框图；
[0089]
图3是本发明实施例时间最优非对称s型加减速控制方法中基于鲸鱼算法计算s型速度曲线最优时间参数的流程图；
[0090]
图4是本发明实施例时间最优非对称s型加减速控制方法中s型加减速控制得到的运动曲线图。
具体实施方式
[0091]
下面结合附图和实施例对本发明的实施方式作进一步详细描述。需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互结合。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0092]
图1示出了本发明实施例时间最优非对称s型加减速控制方法的流程示意图。图2示出了本发明实施例时间最优非对称s型加减速控制方法中的机械臂闭环逆运动学算法的示意框图。图3示出了本发明实施例时间最优非对称s型加减速控制方法中基于鲸鱼算法计算s型速度曲线最优时间参数的流程图。图4示出了本发明实施例时间最优非对称s型加减速控制方法中s型加减速控制得到的运动曲线图。
[0093]
本发明实施例提供一种时间最优非对称s型加减速控制方法，如图1所示，包括以下步骤：
[0094]
s1、获取待规划机械臂路径的给定总长、路径起始点速度和路径终止点速度信息，根据待规划s型速度曲线是否存在加速区与减速区确定问题类型。
[0095]
s101、定义参考路径长度，所述参考路径长度表示为公式(1)：
[0096]
l
ref
＝l
acc
+l
dec
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1)
[0097]
其中，l
ref
表示参考路径长度，l
acc
表示加速区参考路径长度，l
dec
表示减速区参考路径长度，根据公式组(2)和公式组(3)计算：
[0098]
[0099][0100]
其中，vs表示路径起始点的速度，ve表示路径终点的速度，vc表示给定的机械臂末端执行器恒速运动时的切向速度值，a
max
表示末端执行器最大加速度，j
max
表示末端执行器的最大加加速度；
[0101]
s102、根据给定路径总长与参考路径长度之间的关系，判断待规划s型速度曲线是否存在加速区和/或减速区；其中，分为如下四种情况：
[0102]
若l≥l
ref
时，则该路径为长路径，所述待规划s型速度曲线同时具有加速区与减速区。为方便进一步描述本发明实施例将此种情况定义为：case＝1。
[0103]
若l《l
ref
时，则该路径为短路径，此时需要进一步通过判断始末速度之间的关系，确定所述待规划s型速度曲线是否包括加速区与减速区。
[0104]
当vs》ve时，所述待规划s型速度曲线不包括加速区。为方便进一步描述本发明实施例将此种情况定义为：case＝2。
[0105]
当vs《ve时，所述待规划s型速度曲线不包括减速区。为方便进一步描述本发明实施例将此种情况定义为：case＝3。
[0106]
当vs＝ve时，所述待规划s型速度曲线既不包括加速区，也不包括减速区。为方便进一步描述本发明实施例将此种情况定义为：case＝4。
[0107]
其中，l表示给定路径总长。
[0108]
s2、建立适应度函数。
[0109]
本发明实施例中，具体采用运动执行时间作为适应度评价指标。为使总运动执行时间最短，采用时间总和的倒数作为适应度函数，步骤s2中所建立的适应度函数如公式(4)所示：
[0110][0111]
其中，tm(m＝1,2,
…
,7)表示所述待规划s型速度曲线第m段的执行时间；ωm表示所述待规划s型速度曲线第m段执行时间对应的权重因子，ω1、ω2、ω3、ω5、ω6、ω7设为1且ω4设为0.5，以延长匀速段的执行时间。本发明实施例中待解决的问题是最大值优化问题，适应度值越大，代表搜索代理的位置越接近最优解。
[0112]
s3、输入鲸鱼算法中的预设参数对种群进行初始化，所述预设参数包括问题类型、鲸鱼种群规模、最大迭代次数。
[0113]
本发明实施例的步骤s3中，输入问题类型case，种群规模ns，最大迭代次数n
iter
，初始化种群x表示为公式(5)：
[0114]
[0115]
其中，ns表示鲸鱼种群规模；xn,n＝1,2,
…
,ns表示一个搜索代理；t
m,n
∈[0,t
m,max
],m＝1,2,
…
,7,t
m,max
表示第m段执行时间的最大值。
[0116]
若case＝1，则同时存在加速区与减速区，各阶段时间均不设置为0；
[0117]
若case＝2，则不存在加速区，t1,t2,t3设置为0；
[0118]
若case＝3，则不存在减速区，t5,t6,t7设置为0；
[0119]
若case＝4，则既不存在加速区，也不存在减速区，t1、t2、t3、t5、t6、t7设置为0。
[0120]
本发明实施例所提供的技术方案，设计了一种完整的s型加减速控制算法，通过判断待规划s型速度曲线是否存在加速区、匀速区和减速区，将其归类为四种情况，包含了非对称s型速度曲线的所有可能性。与传统的s型加减速控制算法相比，本发明实施例所提供的技术方案的参数计算简单且不依赖于任何简化假设，所得s型速度分布可以是不对称的，路径起始点和终止点的速度可以是非零的，可行性与适用性更好。
[0121]
本发明实施例中，s型速度曲线包括七个阶段，因此每个搜索代理维度为7，种群规模ns设置为30，最大迭代次数n
iter
设置为500，问题类型由步骤s1获得。
[0122]
s4、判断所述鲸鱼算法中搜索代理是否符合运动学约束条件。其中，所述运动学约束条件包括：任意时刻，机械臂各关节的运动应满足构型空间的加速度、速度约束条件极值和速度极值；任意时刻，所规划的机械臂末端执行器的运动应满足操作空间的加加速度、加速度及速度约束条件；所述s型速度剖面加速区的加速度方向为正，和所述减速区的加速度方向为负。步骤s4中的所述运动学约束条件表示为公式(6)：
[0123][0124]
其中，分别表示机械臂第i个关节的加速度极值和速度极值；分别表示机械臂第i个关节任意时刻的加速度和速度；j
lim
、a
lim
、v
lim
分别表示机械臂末端执行器的加加速度极值、加速度极值和速度极值；jk、ak、vk分别表示机械臂末端执行器任意时刻的加加速度、加速度及速度；a
acc
和a
dec
分别表示加速区和减速区的加速度，可通过以下公式(7)和(8)进行计算
[0125]aacc
＝j1t1＝j3t3ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(7)
[0126]adec
＝j5t5＝j7t7ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(8)
[0127]
根据末端执行器的速度剖面，结合其预定义几何路径，可得到末端执行器的期望轨迹。进而机械臂关节速度及加速度可通过闭环逆运动学算法迭代求解。闭环逆运动学算法框图如图2所示，控制规律可表示为公式(9)：
[0128][0129]
其中，表示雅可比矩阵，k
p
表示末端顶点的刚度矩阵，b表示关节阻尼系数，δx＝x-x
des
，x
des
为设计的末端期望位姿，将关节角度代入正运动学方程f(q)中计算可得到实际的末端位姿x；末端期望速度与雅可比伪逆的乘积作为前馈环节以提高系统性能；为梯度投影算子，梯度投影系数取负值；h为优化目标函数，设置为公式(10)：
[0130][0131]
其中，nq表示机械臂关节总数，i表示第i个关节，qi表示第i个关节的位置，q
i,lim
表示第i个关节的位置极值。
[0132]
在逆运动学求解过程中将优化目标函数投影至雅可比矩阵零空间，驱使关节向关节许可位移的中值移动，以实现关节限位。
[0133]
本发明实施例所提供的技术方案考虑了更多的运动学约束条件，不仅是机械臂末端执行器沿路径的限制，还包括各关节的运动限制，避免了由于机械臂操作空间与构型空间的非线性导致的运动范围超限，保障了末端执行器的轨迹跟踪性能。
[0134]
s5、本发明实施例中基于鲸鱼算法及其更新公式，在所述最大迭代次数内通过不断更新搜索代理的位置获取种群中最好的解，基于鲸鱼算法求解最优时间参数的流程图如图3所示。
[0135]
本发明实施例中鲸鱼算法的数学模型为：公式(11)—(15)：
[0136][0137][0138][0139][0140][0141]
其中，表示鲸鱼即搜索代理的位置向量，表示猎物即全局最优解的位置向量，δ表示当前迭代次数，表示猎物与鲸鱼的距离，表示决定影响鲸鱼行动距离的权重值，为定义在[0,2]上的随机向量，表示第δ次迭代中影响变化的权重值，在迭代过程中由2线性递减至0，是定义在[0,1]上的随机向量。
[0142]
更新后搜索代理的位置，为公式(16)—(17)：
[0143]
在[0,1]上定义一个随机数p，
[0144]
若p《0.5且则按照公式(16)和(17)进行更新，
[0145][0146][0147]
若p《0.5且则按照公式(18)和(19)进行更新，
[0148][0149][0150]
其中，表示从当前种群中选取的任意搜索代理的位置向量。
[0151]
若p≥0.5，则按照公式(20)和(21)进行更新，
[0152][0153][0154]
其中，b是对数螺旋形状常数，l为定义在[-1,1]上的随机数。
[0155]
s6、根据鲸鱼算法的最优参数，获取时间最优的曲线路径以及速度。
[0156]
本发明实施例中根据s曲线的最优执行时间计算各阶段的加加速度，进而得到机械臂末端执行器的加速度、速度及位移曲线。
[0157]
本发明实施例中s曲线各阶段的加加速度可根据鲸鱼算法所得最优参数按照如下公式(22)—公式(26)进行计算：
[0158]
若case＝1，同时存在加速区与减速区，则
[0159][0160]
其中，
[0161][0162]
若case＝2，不存在加速区，则
[0163][0164]
若case＝3，不存在减速区，则
[0165][0166]
若case＝4，既不存在加速区，也不存在减速区，则退化为恒速运动，只包括匀速区
[0167]
j1＝j2＝j3＝j4＝j5＝j6＝j7＝0
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(26)
[0168]
计算得到加加速度后，通过积分公式，可以得到如下加速度时间公式(27)、速度时间公式(28)、位移时间公式(29)：
[0169]
[0170]
[0171][0172]
其中，tm(m＝1,2,
…
,7)表示s型速度曲线第m段的执行时间，jm(m＝1,2,
…
,7)表示s型速度曲线第m段的加加速度值，l表示路径总长度，vs表示路径起始点的速度，ve表示路径终止点的速度。
[0173]
本发明实施例中通过基于鲸鱼算法的时间最优非对称s型加减速控制方法，可得到机械臂末端执行器的加加速度、加速度、速度、位移曲线，如图4所示。
[0174]
本发明实施例所提供的技术方案，首次将鲸鱼算法应用于机械臂末端执行器的速度规划中，该算法是一种基于群体的全局优化算法，计算效率高、进化收敛速度快且精度高、鲁棒性强、可实现性好，能够有效地得到时间最优非对称s型速度分布，在不违反运动学约束条件的情况下，充分开发了机械臂的运动能力，缩短了任务执行时间，避免了关节电机饱和导致的轨迹跟踪误差。
[0175]
在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必须针对的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任
一个或多个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。
[0176]
尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制。本领域的普通技术人员在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。
[0177]
以上本发明的具体实施方式，并不构成对本发明保护范围的限定。任何根据本发明的技术构思所做出的各种其他相应的改变与变形，均应包含在本发明权利要求的保护范围内。