CN108621158B - 一种关于机械臂的时间最优轨迹规划控制方法及装置 - Google Patents

Abstract

本发明涉及一种关于机械臂的时间最优轨迹规划控制方法及装置，其中方法包括：设置初始条件并通过三次样条插值算法计算得到关于时间的机械臂关节角度位置描述；基于机械臂关节角度位置描述结合动力学模型，得到力矩关于时间的连续函数；定义机械臂运动轨迹总时间为目标函数，并以此建立以时间最优轨迹规划模型；结合机械臂关节角度位置描述、力矩关于时间的连续函数设置机械臂时间最优轨迹规划控制模型的约束条件；基于所述最优轨迹规划模型，通过输入约束，采用序列二次规划算法迭代求解机械臂时间最短的运动轨迹，得到满足约束的时间最优运动轨迹。与现有技术相比，本发明无需大量复杂的计算，具有较好的实时性、利用时间最少。

Description

一种关于机械臂的时间最优轨迹规划控制方法及装置

技术领域

本发明涉及一种机器人控制技术，尤其是涉及一种关于机械臂的时间最优轨迹规划控制方法及装置。

背景技术

随着科学技术的发展，工业机器人在工业中的作用越来越大，时间和能量最优轨迹规划是工业机器人中重要的研究方向。能量最优可以减小机械臂执行器及操作臂上的应力，同时可以节约能量和降低成本。工业机器人的时间最优轨迹规划指的是在各种约束条件都能满足的情况下，把时间的最短作为指标来优化机器人的运动轨迹的过程，即机器人的手用最短的时间完成在预定的轨迹上或两点之间的运动，进行时间最优轨迹规划的实际意义是能够提高机器人的工作效率。做好机器人的最优时间轨迹规划是实现机器人最优控制的重要前提。

在实际应用中，时间最优是一个非常重要的性能指标。在工业当中，机械臂可以不分昼夜可靠准确的搬运重物，以时间最优轨迹进行研究，不仅能在有限性能充分发挥出关节电机的性能，还可以提高产品的生产效率，实现产品统一化，大大降低工人们的劳动强度。

发明内容

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种关于机械臂的时间最优轨迹规划控制方法及装置。

本发明的目的可以通过以下技术方案来实现：

一种关于机械臂的时间最优轨迹规划控制方法，包括：

设置初始条件并通过三次样条插值算法计算得到关于时间的机械臂关节角度位置描述；

基于机械臂关节角度位置描述结合动力学模型，得到力矩关于时间的连续函数；

定义机械臂运动轨迹总时间为目标函数，并以此建立以时间最优轨迹规划模型；

结合机械臂关节角度位置描述、力矩关于时间的连续函数设置机械臂时间最优轨迹规划控制模型的约束条件；

基于所述最优轨迹规划模型，通过输入约束，采用序列二次规划算法迭代求解机械臂时间最短的运动轨迹，得到满足约束的时间最优运动轨迹。

所述设置初始条件并通过三次样条插值算法计算得到关于时间的机械臂关节角度位置描述，包括：

获取机械臂运动轨迹的关键参数变量；

设计离散网格并将离散的点通过三次样条插值算法获取关节连续的角度加速函数；

对角度加速函数进行积分分别得到角速度和角度位置的描述。

所述力矩关于时间的连续函数为：

其中：τ为关节力矩，

θ分别为角加速度，角速度和角度；M(θ)是n*n的正定对称矩阵，是θ的函数，C(θ,ω)是n*1的离心力和科式力矢量；g(θ)是n*1的重力矢量。

所述目标函数为

其中：T为机器人沿最优轨迹运动的总体时间，n为关键点数量，t_i为第i-1段所用的时间。

所述约束条件为运动轨迹的起点、关键点和终点加速度。

一种关于机械臂的时间最优轨迹规划控制装置，包括：

加速度求解模块，用于设置初始条件并通过三次样条插值算法计算得到关于时间的机械臂关节角度位置描述；

动力学模型求解器，与加速度求解模块连接，用于基于机械臂关节角度位置描述结合动力学模型，得到力矩关于时间的连续函数；

目标函数求解器，与加速度求解模块和动力学模型求解器连接，用于定义机械臂运动轨迹总时间为目标函数，并以此建立以时间最优轨迹规划模型，并结合机械臂关节角度位置描述、力矩关于时间的连续函数设置机械臂时间最优轨迹规划控制模型的约束条件；

时间最优求解器，与目标函数求解器连接，用于基于所述最优轨迹规划模型，通过输入约束，采用序列二次规划算法迭代求解机械臂时间最短的运动轨迹，得到满足约束的时间最优运动轨迹。

所述运动轨迹求解模块包括：

用于获取机械臂运动轨迹的关键参数变量的第一单元；

用于设计离散网格并将离散的点通过三次样条插值算法获取关节连续的角度加速函数的第二单元，与第一单元连接；

用于对角度加速函数进行积分分别得到角速度和角度位置的描述的第三单元，与第二单元连接。

所述力矩关于时间的连续函数为：

其中：τ为关节力矩，

θ分别为角加速度，角速度和角度；M(θ)是n*n的正定对称矩阵，是θ的函数，C(θ,ω)是n*1的离心力和科式力矢量；g(θ)是n*1的重力矢量。

所述目标函数为

其中：T为机器人沿最优轨迹运动的总体时间，n为关键点数量，t_i为第i-1段所用的时间。

所述约束条件为运动轨迹的起点、关键点和终点加速度。

与现有技术相比，本发明具有以下有益效果：

1)本发明所涉及的时间最优轨迹规划算法简单，无需大量复杂的计算，具有较好的实时性、利用时间最少。

2)本发明通过设计连散网格可以将离散的角加速度拟合成比较精确的连续函数，能够更加有效的研究机器人的运动状态。

3)本发明充分发挥伺服关节电机的性能，为机器人关节体积设计越来越小，性能越来越好功能提供方法。

4)本发明能够充分水平多关节机器人电机性能，具有较强的鲁棒性，提高机器人的工作效率降低人们的工作强度。

附图说明

图1为方法的流程示意图；

图2为本发明的控制流程示意图；

图3为线性插值方法获取角加速度示意图；

图4(a)为大臂小臂同时摆动30度运动轨迹示意图；

图4(b)为大臂小臂同时摆动30度运动状态示意图；

图5(a)为大臂小臂同时摆动-30度运动轨迹示意图；

图5(b)为大臂小臂同时摆动-30度运动状态示意图。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。本实施例以本发明技术方案为前提进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述的实施例。

本发明提出关于机械臂的时间最优轨迹规划控制方法，以机器人运动时间总和作为目标函数的非线性约束规划问题，以达到在有限动力下充分提高机器人工作效率的作用。并设计离散网格对各个关节角加速度进行描述，进而再利用微分和动力学模型分别获取相关参数。

具体的，如图1和图2所示，包括：

设置初始条件并通过三次样条插值算法计算得到关于时间的机械臂关节角度位置描述；具体包括：

获取机械臂运动轨迹的关键参数变量；

设计离散网格并将离散的点通过三次样条插值算法获取关节连续的角度加速函数；

对角度加速函数进行积分分别得到角速度和角度位置的描述。

如图3所示，定义机械臂运动轨迹总时间为t_n，假设该时间被平均分为n段t₁,t₂,...,t_n，其中d_i＝t_i+1-t_i＝1/(n-1)，t₁＝0，则取a₁,a₂,...,a_n为关节空间中的关键点的角度加速度，选取网格点上的n个变量a_j(t＝t₁),a_j(t＝t₂),...,a_j(t＝t_n)为第j关节变量，利用线性插值算法构造机械臂连续的角度加速度函数，通过积分分别得到角速度和角度位置的描述。

基于机械臂关节角度位置描述结合动力学模型，得到力矩关于时间的连续函数；力矩关于时间的连续函数为：

其中：τ为关节力矩，

θ分别为角加速度，角速度和角度；M(θ)是n*n的正定对称矩阵，是θ的函数，C(θ,ω)是n*1的离心力和科式力矢量；g(θ)是n*1的重力矢量。

定义机械臂运动轨迹总时间为目标函数，并以此建立以时间最优轨迹规划模型，为充分发挥机械臂关节电机性能，提高机械臂的工作效率降低人们的劳动强度，将机器人沿最优轨迹运动所用的总时间作为优化目标，定义时间最优控制方法的目标函数为

其中：T为机器人沿最优轨迹运动的总体时间，n为关键点数量，t_i为第i-1段所用的时间。

结合机械臂关节角度位置描述、力矩关于时间的连续函数设置机械臂时间最优轨迹规划控制模型的约束条件，其中，约束条件为运动轨迹的起点、关键点和终点加速度；

基于最优轨迹规划模型，通过输入约束，采用序列二次规划算法迭代求解机械臂时间最短的运动轨迹，得到满足约束的时间最优运动轨迹，具体的，基于以机械臂时间最短为优化目标的轨迹规划模型，通过输入运动轨迹的起点、关键点和终点加速度，采用序列二次规划算法(SQP)迭代求解机械臂时间最短的运动轨迹，从而获得满足约束的时间最优运动轨迹。

实施例一

本实施例以水平多关节SCARA工业机器人为实施例进行关于机械臂的时间最优轨迹规划控制方法演示。具体步骤如下：

第一步，设置大臂和小臂的初始角度都为零，让大臂小臂同时向右摆动30度。通过加速度求解器进行水平多关节SCARA工业机器人获取关节角度加速度的连续函数。

第二步，根据第一步中获取关节角度加速度的连续函数代入到动力学模型求解器中计算可得力矩关于时间的函数；

第三步，通过第一步和第二步获取的加速度以及力矩等函数表达式建立约束条件。

第四步，将约束条件与建立以机器人运动轨迹消耗的总时间作为时间的目标函数代入时间最优求解器，将目标求解器中的目标函数和设置的关节状态的约束条件带入到规划求解器中，通过输入运动轨迹的起点、关键点和终点的加速度，采用序列二次规划算法(SQP)进行求解最优轨迹规划，最终计算得到运动时间的最小值为0.26s。关于以SCARA工业机器人时间最优轨迹规划方法为实施例示意图如附件图4(a)和(b)所示。

实施例二

本实施例以水平多关节SCARA工业机器人为实施例进行关于机械臂的时间最优轨迹规划控制方法演示。具体步骤如下：

第一步，设置大臂和小臂的初始角度都为零，让大臂小臂同时向左摆动30度。通过加速度求解器进行水平多关节SCARA工业机器人获取关节角度加速度的连续函数。

第二步，根据第一步中获取关节角度加速度的连续函数代入到动力学模型求解器中计算可得力矩关于时间的函数；

第三步，通过第一步和第二步获取的加速度以及力矩等函数表达式建立约束条件。

第四步，将约束条件与建立以机器人运动轨迹消耗的总时间作为时间的目标函数代入时间最优求解器，将目标求解器中的目标函数和设置的关节状态的约束条件带入到规划求解器中，通过输入运动轨迹的起点、关键点和终点的加速度，采用序列二次规划算法(SQP)进行求解最优轨迹规划，最终计算得到运动时间的最小值为0.26s。关于以SCARA工业机器人时间最优轨迹规划方法为实施例示意图如附件图5(a)和(b)所示。

实施例一和实施例二，讨论了针对水平多关节SCARA工业机器人为实施例进行介绍关于机械臂的时间最优轨迹规划控制方法。

水平多关节SCARA工业机器人大臂小臂同时顺时针摆动30度示意图如附件图4(a)和(b)所示。大臂小臂摆动角度相同，加速度速度曲线以及度曲线都趋于重合，但大臂的力矩曲线高于小臂的力矩曲线。其主要原因是大臂关节电机不仅承受大臂关节重量，同时需要承受小臂的关节重量，故大臂所需的力矩输出明显高于小臂的力矩输出。在同种情况下，水平多关节SCARA工业机器人大臂小臂同时逆时针摆动30度示意图如附件图5(a)和(b)所示。可以看出逆时针摆动所大臂小臂呈现的运动状态与顺时针摆动的运动状态大小相同方向相反，最小运动时间相同都为0.26s。本方法实验了关于时间最优轨迹控制方法的控制方法，并观察了机械臂的具体运动状态，求解运动轨迹时间的最小值。实现了提供机械臂的工作效率，降低工人们的劳动强度。

Claims (4)

1.一种关于机械臂的时间最优轨迹规划控制方法，其特征在于，无需大量复杂的计算，具有较好的实时性、利用时间少，包括：

设置初始条件并通过三次样条插值算法计算得到关于时间的机械臂关节角度位置描述，具体包括：获取机械臂运动轨迹的关键参数变量，设计离散网格，定义机械臂运动轨迹总时间为t_n，假设该时间被平均分为n段t₁,t₂,...,t_n，其中d_i＝t_i+1-t_i＝1/(n-1)，t₁＝0，则取a₁,a₂,...,a_n为关节空间中的关键点的角度加速度，选取网格点上的n个变量a_j(t＝t₁),a_j(t＝t₂),...,a_j(t＝t_n)为第j关节变量，并将离散的点通过三次样条插值算法获取关节连续的角度加速函数，对角度加速函数进行积分分别得到角速度和角度位置的描述；

基于机械臂关节角度位置描述结合动力学模型，得到力矩关于时间的连续函数，其中力矩关于时间的连续函数为：

其中：τ为关节力矩，

θ分别为角加速度，角速度和角度；M(θ)是n*n的正定对称矩阵，是θ的函数，C(θ,ω)是n*1的离心力和科式力矢量；g(θ)是n*1的重力矢量；

定义机械臂运动轨迹总时间为目标函数，并以此建立以时间最优轨迹规划模型；

结合机械臂关节角度位置描述、力矩关于时间的连续函数设置机械臂时间最优轨迹规划控制模型的约束条件，所述约束条件为运动轨迹的起点、关键点和终点加速度；

基于所述最优轨迹规划模型，通过输入约束，采用序列二次规划算法迭代求解机械臂时间最短的运动轨迹，得到满足约束的时间最优运动轨迹。

2.根据权利要求1所述的一种关于机械臂的时间最优轨迹规划控制方法，其特征在于，所述目标函数为

其中：T为机器人沿最优轨迹运动的总体时间，n为关键点数量，t_i为第i-1段所用的时间。

3.一种关于机械臂的时间最优轨迹规划控制装置，其特征在于，无需大量复杂的计算，具有较好的实时性、利用时间少，包括：

加速度求解模块，用于设置初始条件并通过三次样条插值算法计算得到关于时间的机械臂关节角度位置描述，

动力学模型求解器，与加速度求解模块连接，用于基于机械臂关节角度位置描述结合动力学模型，得到力矩关于时间的连续函数，

目标函数求解器，与加速度求解模块和动力学模型求解器连接，用于定义机械臂运动轨迹总时间为目标函数，并以此建立以时间最优轨迹规划模型，并结合机械臂关节角度位置描述、力矩关于时间的连续函数设置机械臂时间最优轨迹规划控制模型的约束条件，所述约束条件为运动轨迹的起点、关键点和终点加速度；

时间最优求解器，与目标函数求解器连接，用于基于所述最优轨迹规划模型，通过输入约束，采用序列二次规划算法迭代求解机械臂时间最短的运动轨迹，得到满足约束的时间最优运动轨迹；

所述运动轨迹求解模块包括：

用于获取机械臂运动轨迹的关键参数变量的第一单元，

用于设计离散网格，定义机械臂运动轨迹总时间为t_n，假设该时间被平均分为n段t₁,t₂,...,t_n，其中d_i＝t_i+1-t_i＝1/(n-1)，t₁＝0，则取a₁,a₂,...,a_n为关节空间中的关键点的角度加速度，选取网格点上的n个变量a_j(t＝t₁),a_j(t＝t₂),...,a_j(t＝t_n)为第j关节变量，并将离散的点通过三次样条插值算法获取关节连续的角度加速函数的第二单元，与第一单元连接，

用于对角度加速函数进行积分分别得到角速度和角度位置的描述的第三单元，与第二单元连接；

所述力矩关于时间的连续函数为：

其中：τ为关节力矩，

θ分别为角加速度，角速度和角度；M(θ)是n*n的正定对称矩阵，是θ的函数，C(θ,ω)是n*1的离心力和科式力矢量；g(θ)是n*1的重力矢量。

4.根据权利要求3所述的一种关于机械臂的时间最优轨迹规划控制装置，其特征在于，所述目标函数为

其中：T为机器人沿最优轨迹运动的总体时间，n为关键点数量，t_i为第i-1段所用的时间。

Images