一种并联机器人拾放操作轨迹规划方法及系统与流程

本发明涉及机器人智能控制技术领域，特别是涉及一种并联机器人拾放操作轨迹规划方法及系统。

背景技术：

高速delta并联机器人具有运动速度快、定位精确、成本低等优点，目前被广泛应用于医药、食品和电子产品等生产线的快速分拣、抓取和装配中。由于机器人末端执行器在运动过程中的运动轨迹和运动规律会影响机器人的动态特性，所以合理有效的轨迹规划很关键。高速delta并联机器人常采用拾放操作，但很少有学者对并联机器人的拾放操作轨迹规划方法进行研究，因此研究delta并联机器人的拾放操作轨迹规划方法非常重要。拾放操作常采用门字形轨迹，包括竖直、水平、竖直三个阶段。门字形轨迹的竖直方向与水平方向连接处存在直角，因此容易产生冲击、振动，这对delta机器人的高速运行非常不利。

技术实现要素：

本发明的目的是提供一种并联机器人拾放操作轨迹规划方法及系统，以较短的运动周期，提高并联机器人的工作效率。

为实现上述目的，本发明提供了一种并联机器人拾放操作轨迹规划方法，所述方法包括：

将门字形拾放操作轨迹中的直角过渡替换为ph曲线过渡，对所述ph曲线求解，得到ph曲线多项式方程、ph曲线弧长计算式和初始拾放操作轨迹；

采用非对称多项式运动规律确定并联机器人按照所述初始拾放操作轨迹运动的运动周期，所述运动周期为带有不等式约束条件的多项式函数；

以所述运动周期最小为目标函数，采用内点法优化所述初始拾放操作轨迹，得到第一优化拾放操作轨迹；

在预设空间范围内采用人工势场法对所述第一优化拾放操作轨迹进行优化，得到第二优化拾放操作轨迹；所述第二优化拾放操作轨迹为在所述预设空间范围内的拾放操作轨迹。

可选的，所述对所述ph曲线求解，具体包括：

由平面上一条n次bezier曲线，确定n次ph曲线；所述n次bezier曲线为其中，pi＝(xi,yi)是ph曲线的控制点，i＝0,...,n，n为曲线多项式中最高次项的次数，τ为曲线参数，τ∈[0,1]；存在多项式σ(τ)使得则n次ph曲线为p(τ)＝(x(τ),y(τ))；

确定所述初始拾放操作轨迹中的ph曲线为五次ph曲线，根据所述五次ph曲线的控制点的约束条件计算所述五次ph曲线的曲率和六个控制点的坐标；

根据所述五次ph曲线的曲率和六个控制点的坐标推理运算得到五次ph曲线多项式方程和ph曲线弧长计算式；

根据所述五次ph曲线多项式方程和所述ph曲线弧长计算式确定初始拾放操作轨迹。

可选的，所述ph曲线多项式方程为其中，p0，p1，p2，p3，p4，p5为五次ph曲线的控制点；τ为曲线参数，τ∈[0,1]；i表示多项式次数，i＝0,...,5；所述ph曲线弧长计算式为其中u0、u2分别为多项式u(τ)＝u0(1-τ)²+2u1(1-τ)τ+u2τ²的系数，v2为多项式v(τ)＝v0(1-τ)²+2v1(1-τ)τ+v2τ²的系数。

可选的，所述采用非对称多项式运动规律确定并联机器人按照所述初始拾放操作轨迹运动的运动周期，具体包括：

将加速段与减速段的时间比设为4:6，结合边界条件，得到满足要求的3-4-5-6次多项式p(γ)＝lall(-10γ⁶+36γ⁵-45γ⁴+20γ³)；其中,γ＝t/t，γ∈[0,1]，t为运动时间，t为单次拾放操作运动周期；lall为单次拾放操作运动的路径总长，为单侧ph曲线的长度，所述初始拾放操作轨迹左右对称，u0、u2分别为多项式u(τ)＝u0(1-τ)²+2u1(1-τ)τ+u2τ²的系数，v2为多项式v(τ)＝v0(1-τ)²+2v1(1-τ)τ+v2τ²的系数，n为与所述单侧ph曲线连接的竖直方向的拾放操作轨迹长度，m为拾放操作轨迹在水平方向的投影长度，k为所述单侧ph曲线在水平方向的投影长度；

根据一维位移运动规律确定s(t)＝p(γ)，进而确定运动周期为：

其中，0≤l≤h,0≤k≤m/2，amax为最大加速度。

可选的，所述以所述运动周期最小为目标函数，采用内点法优化所述初始拾放操作轨迹，得到第一优化拾放操作轨迹，具体包括：

以最小化运动周期为目标函数，采用内点法优化所述初始拾放操作轨迹，得到ph曲线轨迹；

计算所述ph曲线轨迹的轨迹坐标，得到第一优化拾放操作轨迹。

可选的，所述计算所述ph曲线轨迹的轨迹坐标，具体包括：

根据最小的运动周期值计算单侧所述ph曲线的弧长；

根据所述弧长和ph曲线弧长计算式，求解参数ε；

根据参数ε和

计算所述ph曲线的轨迹坐标。

可选的，所述在预设空间范围内采用人工势场法对所述第一优化拾放操作轨迹进行优化，得到第二优化拾放操作轨迹，具体包括：

确定当拾取点或者放置点的人工势场为：utotal(p)＝ud(p)+uc(p)；其中p代表当前位置；用来跟踪目标位置，kd是正常数，pi是期望的拾取点或者放置点；uc(p)＝kc/d²，用来避免向超出工作空间的范围运动，kc是正常数，d是当前位置点p到工作空间边界的距离；

以期望的拾取点或者放置点pi做势场的吸引力源，所述工作空间边界做势场的排斥力源；

利用势场的负梯度确定力ftotal，ftotal＝-▽ud-▽uc，

通过力ftotal控制所述当前位置点p，使得所述当前位置点p距离期望的拾取点或者放置点在预设距离范围内且又在预设空间范围内，确定所述当前位置点p为新的拾取点或者放置点；

根据新的拾取点或者放置点确定第二优化拾放操作轨迹；

当第二优化拾放操作轨迹的过渡曲线部分超出受限空间时，重新设定初始拾放操作轨迹的竖直方向的投影长度，直至第二优化拾放操作轨迹位于所述预设空间范围内。

本发明还提供了一种并联机器人拾放操作轨迹规划系统，所述系统包括：

曲线求解单元，用于将门字形拾放操作轨迹中的直角过渡替换为ph曲线过渡，对所述ph曲线求解，得到ph曲线多项式方程、ph曲线弧长计算式和初始拾放操作轨迹；

运动周期确定单元，用于采用非对称多项式运动规律确定并联机器人按照所述初始拾放操作轨迹运动的运动周期，所述运动周期为带有不等式约束条件的多项式函数；

第一优化单元，用于以所述运动周期最小为目标函数，采用内点法优化所述初始拾放操作轨迹，得到第一优化拾放操作轨迹；

第二优化单元，用于在预设空间范围内采用人工势场法对所述第一优化拾放操作轨迹进行优化，得到第二优化拾放操作轨迹；所述第二优化拾放操作轨迹为在所述预设空间范围内的拾放操作轨迹。

可选的，所述曲线求解单元包括：

曲线方程确定子单元，用于由平面上一条n次bezier曲线，确定n次ph曲线；所述n次bezier曲线为其中，pi＝(xi,yi)是ph曲线的控制点，i＝0,...,n，n为曲线多项式中最高次项的次数，τ为曲线参数，τ∈[0,1]；存在多项式σ(τ)使得则n次ph曲线为p(τ)＝(x(τ),y(τ))；

曲率计算子单元，用于确定所述初始拾放操作轨迹中的ph曲线为五次ph曲线，根据所述五次ph曲线的控制点的约束条件计算所述五次ph曲线的曲率和六个控制点的坐标；

运算子单元，用于根据所述五次ph曲线的曲率和六个控制点的坐标推理运算得到五次ph曲线多项式方程和ph曲线弧长计算式；

初始拾放操作轨迹确定子单元，用于根据所述五次ph曲线多项式方程和所述ph曲线弧长计算式确定初始拾放操作轨迹。

可选的，所述第二优化单元包括：

人工势场确定子单元，用于确定当拾取点或者放置点的人工势场为：utotal(p)＝ud(p)+uc(p)；其中p代表当前位置；用来跟踪目标位置，kd是正常数，pi是期望的拾取点或者放置点；uc(p)＝kc/d²，用来避免向超出工作空间的范围运动，kc是正常数，d是当前位置点p到工作空间边界的距离；

引斥力源确定子单元，用于以期望的拾取点或者放置点pi做势场的吸引力源，所述工作空间边界做势场的排斥力源；

控制力确定子单元，用于利用势场的负梯度确定控制力ftotal，ftotal＝-▽ud-▽uc，

新拾取点或者放置点确定子单元，用于通过控制力ftotal控制所述当前位置点p，使得所述当前位置点p距离期望的拾取点或者放置点在预设距离范围内且又在预设空间范围内，确定所述当前位置点p为新的拾取点或者放置点；

第二优化拾放操作轨迹确定子单元，用于根据新的拾取点或者放置点确定第二优化拾放操作轨迹；

竖直方向的投影长度调整子单元，用于当第二优化拾放操作轨迹的过渡曲线部分超出受限空间时，重新设定初始拾放操作轨迹的竖直方向的投影长度，直至第二优化拾放操作轨迹位于所述预设空间范围内。

根据本发明提供的具体实施例，本发明公开了以下技术效果：

(1)本发明实现了基于ph曲线的并联机器人的轨迹规划，该轨迹规划方法和系统具有较短的运动周期，提高了并联机器人的工作效率。

(2)本发明具有良好的运动特性，使得本发明具有实用性。

(3)本发明实现了预设空间范围内的轨迹自优化，保证了机器人在工作空间内运动，使得本发明具有可靠性和安全性。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例提供的并联机器人拾放操作轨迹规划方法的流程图；

图2为本发明实施例提供的拾放操作轨迹示意图；

图3为本发明实施例提供的五次ph曲线在uov坐标系下的示意图；

图4为本发明实施例提供的并联机器人拾放操作轨迹规划系统的系统框图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明的目的是提供并联机器人拾放操作轨迹规划方法及系统，以较短的运动周期，提高并联机器人的工作效率。

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

如图1所示，本实施例提供的并联机器人拾放操作轨迹规划方法包括：

步骤101：将门字形拾放操作轨迹中的直角过渡替换为ph曲线过渡，对所述ph曲线求解，得到ph曲线多项式方程、ph曲线弧长计算式和初始拾放操作轨迹。

本实施例中的拾放操作轨迹如图2所示，其中曲线abcdefg为并联机器人的拾放操作轨迹，水平运动距离|ag|＝m，即拾放操作轨迹在水平方向的投影长度，竖直方向距离|ah|＝|gi|＝h，即拾放操作轨迹在竖直方向的投影长度，|ab|＝|gf|＝n，即与单侧ph曲线连接的竖直方向的拾放操作轨迹长度，|bh|＝|fi|＝l，即单侧ph曲线在竖直方向的投影长度，|hc|＝|ei|＝k，即单侧ph曲线在水平方向的投影长度。由图2可知，本申请将现有的门字形拾放操作轨迹中的直角过渡部分替换成了bc曲线和ef曲线，即五次ph曲线。

ph曲线是一类特殊的bezier曲线，具有弧长可表示为参数的多项式函数等优点。

设p(τ)表示平面上一条n次bezier曲线，其定义如下：

其中，pi＝(xi,yi)是ph曲线的控制点，i＝0,...,n，n为ph曲线多项式中最高次项的次数，τ为曲线参数。如果存在多项式σ(τ)使得：

则称p(τ)＝(x(τ),y(τ))为n次ph曲线。

x'(τ)，y'(τ)分别是x(τ),y(τ)关于τ的导数，将x'(τ)，y'(τ)，σ(τ)构成勾股三角形(x'(τ),y'(τ),σ(τ))，设x'(τ)和y'(τ)可表示为

一般取实系数多项式w(τ)＝1，保证曲线没有尖点，此时ph曲线都是奇次的。则此时有σ(τ)＝u²(τ)+v²(τ)(4)

因此构造ph曲线的关键是确定多项式u(τ)和v(τ)，对于五次ph曲线

本实施例中采用的五次ph曲线为：

图3为五次ph曲线在uov坐标系下的示意图，即拾放操作轨迹中的过渡部分bc，采用五次ph曲线。

则五次ph曲线的控制点应满足：

构造ph曲线的过程，就是求解上述系数的过程。为了简化系数的计算过程，使p0p1在y轴上，则v0＝0，化简得：

曲率为ph曲线上的曲率计算公式为：

ph曲线不仅实现了切向的连续变化，同时也实现了曲率的连续变化。

由曲率公式k(0)＝0k(1)＝0可得，

v1＝0

u1＝0

进一步简化得p1＝p2，p3＝p4，

由五次ph曲线性质可得以下结论：

则参数化表达式为：

ph曲线经过第一个控制点和第六个控制点，即：

由式(10)结合轨迹竖直方向与水平方向为直角，得图3的uov坐标系下各个控制点的坐标为：

由坐标系关系得：

得：

得到u0,u2,v2，进而可求得式(6)，即ph曲线，可得：

由式(2)得：

由ph曲线弧长计算公式得：

因此得弧长p0p5：

ph曲线求解之后，可以得到图2所示的轨迹。

步骤102：采用非对称多项式运动规律确定并联机器人按照所述初始拾放操作轨迹运动的运动周期，所述运动周期为带有不等式约束条件的多项式函数。

并联机器人在拾放操作轨迹中采用多项式运动规律，并采用非对称的加速度规律，将加速段与减速段的时间比设为4:6，结合边界条件，得到满足要求的3-4-5-6次多项式：

p(γ)＝lall(-10γ⁶+36γ⁵-45γ⁴+20γ³)(20)

其中,γ＝t/t，γ∈[0,1]，t为运动时间，t为单次拾放操作运动周期；lall为单次拾放操作运动的路径总长，如图2和3所示，为单侧ph曲线的长度，所述初始拾放操作轨迹左右对称，u0、u2分别为多项式u(τ)＝u0(1-τ)²+2u1(1-τ)τ+u2τ²的系数，v2为多项式v(τ)＝v0(1-τ)²+2v1(1-τ)τ+v2τ²的系数，n为与所述单侧ph曲线连接的竖直方向的拾放操作轨迹长度，m为拾放操作轨迹在水平方向的投影长度，k为所述单侧ph曲线在水平方向的投影长度；

在拾放操作运动中采用上述运动规律，即将式(17)设为一维位移运动规律：

根据一维位移运动规律确定s(t)＝p(γ)

进而确定运动周期为：

其中，0≤l≤h,0≤k≤m/2，amax为最大加速度。

不同的参数l和k会得到不同的ph曲线，考虑高速分拣物体的需求，应使时间最短，即需要最小化t。

步骤103：以所述运动周期最小为目标函数，采用内点法优化所述初始拾放操作轨迹，得到第一优化拾放操作轨迹。

该步骤103具体可以包括以下步骤：

以最小化运动周期为目标函数，采用内点法优化所述初始拾放操作轨迹，得到ph曲线轨迹；

具体的，上述优化问题可以通过内点法解决带不等式约束的最小化优化问题的方法进行优化，优化可得：|ab|＝|gf|＝0，|ce|＝0。因此可得运动有以下阶段：

(1)当并联机器人末端执行器从点a向点d运动时，即机器人沿着五次ph曲线运动。

(2)当并联机器人末端执行器从点d向点g运动时，即机器人沿着五次ph曲线运动，该五次ph曲线与阶段(1)的ph曲线关于竖直方向对称，因此与阶段(1)的轨迹坐标计算方法类似。

计算所述ph曲线轨迹的轨迹坐标，得到第一优化拾放操作轨迹。

具体的，根据最小的运动周期值计算单侧所述ph曲线的弧长，即由上式(21)求s(t)；

根据所述弧长s(t)和ph曲线弧长计算式(18)，求解参数ε；

根据参数ε，将τ(自变量)用ε(已知量)代入式(16)计算得到所述ph曲线在uov坐标系下的坐标；

经过坐标变换将uov坐标系下坐标转换到机器人坐标系下的轨迹坐标。

步骤104：在预设空间范围内采用人工势场法对所述第一优化拾放操作轨迹进行优化，得到第二优化拾放操作轨迹；所述第二优化拾放操作轨迹为在所述预设空间范围内的拾放操作轨迹。

该步骤104具体可以包括以下步骤：

确定当拾取点或者放置点的人工势场为：utotal(p)＝ud(p)+uc(p)；其中p代表当前位置；用来跟踪目标位置，kd是正常数，pi是期望的拾取点或者放置点；uc(p)＝kc/d²，用来避免向超出工作空间的范围运动，kc是正常数，d是当前位置点p到工作空间边界的距离；

以期望的拾取点或者放置点pi做势场的吸引力源，所述工作空间边界做势场的排斥力源；

利用势场的负梯度确定力ftotal，ftotal＝-▽ud-▽uc，

通过力ftotal控制所述当前位置点p，使得所述当前位置点p距离期望的拾取点或者放置点在预设距离范围内且又在预设空间范围内，确定所述当前位置点p为新的拾取点或者放置点；

根据新的拾取点或者放置点确定第二优化拾放操作轨迹；

当第二优化拾放操作轨迹的过渡曲线部分超出受限空间时，重新设定初始拾放操作轨迹的竖直方向的投影长度，直至第二优化拾放操作轨迹位于所述预设空间范围内。

如图4所示，本实施例还提供了一种与上述方法相应的并联机器人拾放操作轨迹规划系统，该系统包括：

曲线求解单元401，用于将门字形拾放操作轨迹中的直角过渡替换为ph曲线过渡，对所述ph曲线求解，得到ph曲线多项式方程、ph曲线弧长计算式和初始拾放操作轨迹；

运动周期确定单元402，用于采用非对称多项式运动规律确定并联机器人按照所述初始拾放操作轨迹运动的运动周期，所述运动周期为带有不等式约束条件的多项式函数；

第一优化单元403，用于以所述运动周期最小为目标函数，采用内点法优化所述初始拾放操作轨迹，得到第一优化拾放操作轨迹；

第二优化单元404，用于在预设空间范围内采用人工势场法对所述第一优化拾放操作轨迹进行优化，得到第二优化拾放操作轨迹；所述第二优化拾放操作轨迹为在所述预设空间范围内的拾放操作轨迹。

其中，所述曲线求解单元401包括：

曲线方程确定子单元，用于由平面上一条n次bezier曲线，确定n次ph曲线；所述n次bezier曲线为其中，pi＝(xi,yi)是ph曲线的控制点，i＝0,...,n，n为曲线多项式中最高次项的次数，τ为曲线参数，τ∈[0,1]；存在多项式σ(τ)使得则n次ph曲线为p(τ)＝(x(τ),y(τ))；

曲率计算子单元，用于确定所述初始拾放操作轨迹中的ph曲线为五次ph曲线，根据所述五次ph曲线的控制点的约束条件计算所述五次ph曲线的曲率和六个控制点的坐标；

运算子单元，用于根据所述五次ph曲线的曲率和六个控制点的坐标推理运算得到五次ph曲线多项式方程和ph曲线弧长计算式；

初始拾放操作轨迹确定子单元，用于根据所述五次ph曲线多项式方程和所述ph曲线弧长计算式确定初始拾放操作轨迹。

所述第二优化单元404包括：

人工势场确定子单元，用于确定当拾取点或者放置点的人工势场为：utotal(p)＝ud(p)+uc(p)；其中p代表当前位置；用来跟踪目标位置，kd是正常数，pi是期望的拾取点或者放置点；uc(p)＝kc/d²，用来避免向超出工作空间的范围运动，kc是正常数，d是当前位置点p到工作空间边界的距离；

引斥力源确定子单元，用于以期望的拾取点或者放置点pi做势场的吸引力源，所述工作空间边界做势场的排斥力源；

控制力确定子单元，用于利用势场的负梯度确定控制力ftotal，ftotal＝-▽ud-▽uc，

新拾取点或者放置点确定子单元，用于通过控制力ftotal控制所述当前位置点p，使得所述当前位置点p距离期望的拾取点或者放置点在预设距离范围内且又在预设空间范围内，确定所述当前位置点p为新的拾取点或者放置点；

第二优化拾放操作轨迹确定子单元，用于根据新的拾取点或者放置点确定第二优化拾放操作轨迹；

竖直方向的投影长度调整子单元，用于当第二优化拾放操作轨迹的过渡曲线部分超出受限空间时，重新设定初始拾放操作轨迹的竖直方向的投影长度，直至第二优化拾放操作轨迹位于所述预设空间范围内。

对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。

本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。