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

1.本发明涉及机器人路径规划技术领域，特别是涉及一种并联机器人动态拾放轨迹规划方法及系统。


背景技术：

2.当今时代越来越多的工作都是通过机器人来取代传统人工操作从而提升产业效率，尤其是在重复性工作占比高的场景如货物分拣、堆放码垛等，机器人效率优势更为明显。在各种机器人种类中，delta并联机器人由其结构简单、容易求得解析正解等优点得以广泛应用。
3.在delta机器人进行抓取、搬运操作时一般是根据门字型路径进行运动，而在实际工作场景中，delta机器人在运动路径中往往存在障碍或者与其他物体存在碰撞的可能，而目前关于delta并联机器人避障的轨迹规划方法往往强调几何问题，计算实时性较差。


技术实现要素：

4.本发明的目的是提供一种并联机器人动态拾放轨迹规划方法及系统，以提高避障轨迹规划的计算效率。
5.为实现上述目的，本发明提供了如下方案：
6.一种并联机器人动态拾放轨迹规划方法，所述方法包括：
7.以最短避障距离为几何约束，对ph曲线进行求解，获得ph曲线多项式方程以及ph曲线与门字形拾放轨迹的第一交点和第二交点；
8.将第一交点和第二交点之间的ph曲线替换门字形拾放轨迹中第一交点和第二交点之间的直角过渡段；
9.以替换前的门字形拾放轨迹的中轴线为对称轴，将另一个直角过渡段替换为第一交点和第二交点之间的ph曲线的对称ph曲线，获得替换后的门字形拾放轨迹；
10.基于替换后的门字形拾放轨迹的对称性，并联机器人依据运动规律按照替换后的门字形拾放轨迹进行运动。
11.可选的，所述以最短避障距离为几何约束，对ph曲线进行求解，获得ph曲线多项式方程，具体包括：
12.构建ph曲线多项式方程的系数与ph曲线所要替换的门字形拾放轨迹中直角过渡段的直角边长的关系方程；
13.根据最短避障距离，利用公式d＝0.17064m，确定ph曲线所要替换的门字形拾放轨迹中直角过渡段的直角边长；其中，d为最短避障距离，m为ph曲线所要替换的门字形拾放轨迹中直角过渡段的直角边长；
14.将ph曲线所要替换的门字形拾放轨迹中直角过渡段的直角边长代入所述关系方程，获得ph曲线多项式方程的系数。
15.可选的，构建的ph曲线多项式方程的系数与ph曲线所要替换的门字形拾放轨迹中
直角过渡段的直角边长的关系方程为
[0016][0017]
其中，u0、u2和v2分别为ph曲线多项式方程的第一、第二、第三系数。
[0018]
可选的，所述ph曲线多项式方程为
[0019][0020]
其中，x(γ)、y(γ)分别为ph曲线的横坐标、纵坐标，γ为ph曲线的参数。
[0021]
可选的，所述基于替换后的门字形拾放轨迹的对称性，并联机器人依据运动规律按照替换后的门字形拾放轨迹进行运动，具体包括：
[0022]
依据第一多项式运动规律按照并联机器人拾取操作点到第一交点之间的第一直线轨迹进行运动；
[0023]
依据ph曲线参数依赖的运动规律按照第一交点和第二交点之间的ph曲线轨迹进行运动；
[0024]
依据第二多项式运动规律按照第二交点到对称分界点之间的第二直线轨迹进行运动；
[0025]
根据替换后的门字形拾放轨迹的对称性，依据第二多项式运动规律、ph曲线参数依赖的运动规律和第一多项式运动规律按照对称分界点到并联机器人放置操作点之间的门字形拾放轨迹进行运动。
[0026]
可选的，所述依据第一多项式运动规律按照并联机器人拾取操作点到第一交点之间的第一直线轨迹进行运动，之前还包括：
[0027]
获取第一多项式运动规律表达式为v1(φ1)＝v
10
v
11
φ1 v
12
φ
12
v
13
φ
13
v
14
φ
14
v
15
φ
15
；其中，v1(φ1)为按照第一直线轨迹运动的速度，φ1为第一变量，φ1＝t1/t1，φ1∈[0,1]，t1为并联机器人从拾取操作点到第一交点所需的总时间，t1为实时时间，v
10
为第一常数项，v
11
、v
12
、v
13
、v
14
和v
15
分别为第一多项式运动规律中多项式的系数；
[0028]
根据v1(0)＝0,v1(1)＝v
b
,v
′1(0)＝0,v
′1(1)＝0,v
″1(0)＝0,v
″1(1)＝0的边界条件，对第一多项式运动规律表达式进行求解，获得第一直线轨迹的多项式运动规律为v1(φ1)＝10v
b
φ
13
‑
15v
b
φ
14
6v
b
φ
15
；其中，v
b
为并联机器人运动到第一交点时的速度，v1(0)、
v
′1(0)、v
″1(0)分别为并联机器人在拾取操作点的速度、加速度和加速度变化率，v1(1)、v
′1(1)、v
″1(1)分别为并联机器人在第一交点的速度、加速度和加速度变化率。
[0029]
可选的，所述依据ph曲线参数依赖的运动规律按照第一交点和第二交点之间的ph曲线轨迹进行运动，之前还包括：
[0030]
将ph曲线轨迹均匀划分为多个时间段，并确定按照ph曲线轨迹进行运动时ph曲线参数依赖的运动规律表达式为v2(γ
i
)＝v
b
16(v
n
‑
v
b
)γ
i2
32(v
b
‑
v
n
)γ
i3
16(v
n
‑
v
b
)γ
i4
；其中，v2(γ
i
)为在第i个时间段运动的速度，γ
i
为第i个时间段的ph曲线参数的值，v
b
为并联机器人运动到第一交点时的速度，v
n
为并联机器人按照ph曲线轨迹运动的最小速度；
[0031]
结合ph曲线参数依赖的运动规律表达式和ph曲线中参数和时间的关系式，获得参数求解方程为其中，f(γ
i
)为参数求解函数，ζ、β、δ、η分别为第三、第四、第五、第六、第七多项式，m为ph曲线所要替换的门字形拾放轨迹中直角过渡段的直角边长，δt为时间间隔；
[0032]
根据公式利用牛顿迭代法确定每个时间段的参数的值；其中，为初始参数值，分别为r
‑
1次、r次迭代在第i个时间段的参数的值，γ
i
‑1为在第i
‑
1个时间段的参数的值，v2(γ
i
‑1)为在第i个时间段沿ph曲线轨迹运动的速度，σ(γ
i
‑1)为ph曲线弧长对参数γ
i
‑1的导数，为r
‑
1次迭代时的参数求解函数，为对时间的导数；
[0033]
将每个时间段的参数的值代入ph曲线参数依赖的运动规律表达式中，获得ph曲线参数依赖的运动规律。
[0034]
可选的，所述依据第二多项式运动规律按照第二交点到对称分界点之间的第二直线轨迹进行运动，之前还包括：
[0035]
确定第二直线轨迹的第二多项式运动规律为v3(φ3)＝v
30
v
31
φ3 v
32
φ
32
v
33
φ
33
；其中，v3(φ3)为第二直线轨迹的速度，φ3为第二变量，φ3＝t3/t3，φ3∈[0,1]，t3为并联机器人从第二交点到对称分界点所需的总时间，t3为按照第二直线轨迹运动的实时时间，v
30
为第三常数项，v
31
、v
32
、v
33
分别为第二多项式运动规律中多项式的系数；
[0036]
根据v3(0)＝v
b
,v3(1)＝v
max
,v3'(0)＝0,v3'(1)＝0,s3(0)＝0的边界条件，对第二直线轨迹的第二多项式运动规律进行求解，获得第二直线轨迹的多项式运动规律为v3(φ3)＝v
b
3(v
max
‑
v
b
)φ
32
2(v
b
‑
v
max
)φ
33
；其中，v
b
为并联机器人运动到第一交点时的速度，v
max
为按照第二直线轨迹运动的最大速度，v3(0)、v3'(0)、s3(0)分别为并联机器人在第二交点的速度、加速度和位移，v3(1)、v3'(1)分别为并联机器人在对称分界点的速度和加速度。
[0037]
一种并联机器人动态拾放轨迹规划系统，所述系统包括：
[0038]
ph曲线求解模块，用于以最短避障距离为几何约束，对ph曲线进行求解，获得ph曲线多项式方程以及ph曲线与门字形拾放轨迹的第一交点和第二交点；
[0039]
替换模块，用于将第一交点和第二交点之间的ph曲线替换门字形拾放轨迹中第一交点和第二交点之间的直角过渡段；
[0040]
替换后的门字形拾放轨迹获得模块，用于以替换前的门字形拾放轨迹的中轴线为对称轴，将另一个直角过渡段替换为第一交点和第二交点之间的ph曲线的对称ph曲线，获得替换后的门字形拾放轨迹；
[0041]
执行模块，用于基于替换后的门字形拾放轨迹的对称性，并联机器人依据运动规律按照替换后的门字形拾放轨迹进行运动。
[0042]
可选的，所述ph曲线求解模块，具体包括：
[0043]
关系方程构建子模块，用于构建ph曲线多项式方程的系数与ph曲线所要替换的门字形拾放轨迹中直角过渡段的直角边长的关系方程；
[0044]
直角边长确定子模块，用于根据最短避障距离，利用公式d＝0.17064m，确定ph曲线所要替换的门字形拾放轨迹中直角过渡段的直角边长；其中，d为最短避障距离，m为ph曲线所要替换的门字形拾放轨迹中直角过渡段的直角边长；
[0045]
系数获得子模块，用于将ph曲线所要替换的门字形拾放轨迹中直角过渡段的直角边长代入所述关系方程，获得ph曲线多项式方程的系数。
[0046]
根据本发明提供的具体实施例，本发明公开了以下技术效果：
[0047]
本发明公开了一种并联机器人动态拾放轨迹规划方法及系统，首先以最短避障距离为几何约束求解ph曲线，然后将门字形拾放轨迹的直角过渡替换为ph曲线过渡，最后基于替换后的门字形拾放轨迹的对称性，并联机器人依据运动规律按照替换后的门字形拾放轨迹进行运动。本发明以最短避障距离作为求解ph曲线的几何约束，使得在尽可能减少冲击振荡的基本要求下实现避障功能，并且利用了门字形拾放轨迹的对称性只需要进行一半路径的计算，从而提高了避障轨迹规划的计算效率。
附图说明
[0048]
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
[0049]
图1为本发明实施例一提供的一种并联机器人动态拾放轨迹规划方法的流程图；
[0050]
图2为本发明实施例二提供的拾放操作的几何路径图；
[0051]
图3为本发明实施例二提供的ppo路径中的ph曲线图；
[0052]
图4为本发明实施例二提供的坐标点计算原理图；
[0053]
图5为本发明实施例二提供的delta并联机器人示意图。
具体实施方式
[0054]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于
本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0055]
本发明的目的是提供一种并联机器人动态拾放轨迹规划方法及系统，以提高避障轨迹规划的计算效率。
[0056]
为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。
[0057]
实施例一
[0058]
本发明提供了一种并联机器人动态拾放轨迹规划方法，如图1所示，方法包括：
[0059]
步骤101，以最短避障距离为几何约束，对ph曲线进行求解，获得ph曲线多项式方程以及ph曲线与门字形拾放轨迹的第一交点和第二交点；
[0060]
步骤102，将第一交点和第二交点之间的ph曲线替换门字形拾放轨迹中第一交点和第二交点之间的直角过渡段；
[0061]
步骤103，以替换前的门字形拾放轨迹的中轴线为对称轴，将另一个直角过渡段替换为第一交点和第二交点之间的ph曲线的对称ph曲线，获得替换后的门字形拾放轨迹；
[0062]
步骤104，基于替换后的门字形拾放轨迹的对称性，并联机器人依据运动规律按照替换后的门字形拾放轨迹进行运动。
[0063]
具体实现过程如下：
[0064]
步骤101，以最短避障距离为几何约束，对ph曲线进行求解，获得ph曲线多项式方程，具体包括：
[0065]
构建ph曲线多项式方程的系数与ph曲线所要替换的门字形拾放轨迹中直角过渡段的直角边长的关系方程；
[0066]
根据最短避障距离，利用公式d＝0.17064m，确定ph曲线所要替换的门字形拾放轨迹中直角过渡段的直角边长；其中，d为最短避障距离，m为ph曲线所要替换的门字形拾放轨迹中直角过渡段的直角边长；
[0067]
将ph曲线所要替换的门字形拾放轨迹中直角过渡段的直角边长代入关系方程，获得ph曲线多项式方程的系数。
[0068]
在替换前的门字形拾放轨迹中，直角边的长度均是已知的，因此，在直角坐标系中直角边所在直线的方程也就能确定得到。所以，在同一直角坐标系中求解ph曲线多项式方程与替换前的门字形拾放轨迹的直角边的方程的交点，就可以得到替换前的门字形拾放轨迹中一个直角的两个直角边与ph曲线的交点。
[0069]
其中，构建的ph曲线多项式方程的系数与ph曲线所要替换的门字形拾放轨迹中直角过渡段的直角边长的关系方程为
[0070][0071]
其中，u0、u2和v2分别为ph曲线多项式方程的第一、第二、第三系数。
[0072]
ph曲线多项式方程为
[0073][0074]
其中，x(γ)、y(γ)分别为ph曲线的横坐标、纵坐标，γ为ph曲线的参数。
[0075]
步骤102，该直角用上述的ph曲线替换，使得直角过渡替换为ph曲线过渡。
[0076]
步骤103，由于替换前的门字形拾放轨迹对称，并且有两个直角，所以直接利用一个直角替换的ph曲线，基于对称性的原理，用对称ph曲线替换另一个直角，使得替换后的门字形拾放轨迹仍为对称结构。
[0077]
步骤104，基于替换后的门字形拾放轨迹的对称性，并联机器人依据运动规律按照替换后的门字形拾放轨迹进行运动，具体包括：
[0078]
依据第一多项式运动规律按照并联机器人拾取操作点到第一交点之间的第一直线轨迹进行运动；
[0079]
依据ph曲线参数依赖的运动规律按照第一交点和第二交点之间的ph曲线轨迹进行运动；
[0080]
依据第二多项式运动规律按照第二交点到对称分界点之间的第二直线轨迹进行运动；
[0081]
根据替换后的门字形拾放轨迹的对称性，依据第二多项式运动规律、ph曲线参数依赖的运动规律和第一多项式运动规律按照对称分界点到并联机器人放置操作点之间的门字形拾放轨迹进行运动。
[0082]
确定第一多项式运动规律的步骤为：
[0083]
获取第一多项式运动规律表达式为v1(φ1)＝v
10
v
11
φ1 v
12
φ
12
v
13
φ
13
v
14
φ
14
v
15
φ
15
；其中，v1(φ1)为按照第一直线轨迹运动的速度，φ1为第一变量，φ1＝t1/t1，φ1∈[0,1]，t1为并联机器人从拾取操作点到第一交点所需的总时间，t1为实时时间，v
10
为第一常数项，v
11
、v
12
、v
13
、v
14
和v
15
分别为第一多项式运动规律中多项式的系数；
[0084]
根据v1(0)＝0,v1(1)＝v
b
,v
′1(0)＝0,v
′1(1)＝0,v
″1(0)＝0,v
″1(1)＝0的边界条
件，对第一多项式运动规律表达式进行求解，获得第一直线轨迹的多项式运动规律为v1(φ1)＝10v
b
φ
13
‑
15v
b
φ
14
6v
b
φ
15
；其中，v
b
为并联机器人运动到第一交点时的速度，v1(0)、v
′1(0)、v
″1(0)分别为并联机器人在拾取操作点的速度、加速度和加速度变化率，v1(1)、v
′1(1)、v
″1(1)分别为并联机器人在第一交点的速度、加速度和加速度变化率。
[0085]
确定ph曲线参数依赖的运动规律的步骤为：
[0086]
将ph曲线轨迹均匀划分为多个时间段，并确定按照ph曲线轨迹进行运动时ph曲线参数依赖的运动规律表达式为v2(γ
i
)＝v
b
16(v
n
‑
v
b
)γ
i2
32(v
b
‑
v
n
)γ
i3
16(v
n
‑
v
b
)γ
i4
；其中，v2(γ
i
)为在第i个时间段运动的速度，γ
i
为第i个时间段的ph曲线参数的值，v
b
为并联机器人运动到第一交点时的速度，v
n
为并联机器人按照ph曲线轨迹运动的最小速度；
[0087]
结合ph曲线参数依赖的运动规律表达式和ph曲线中参数和时间的关系式，获得参数求解方程为其中，f(γ
i
)为参数求解函数，ζ、β、δ、η分别为第三、第四、第五、第六、第七多项式，m为ph曲线所要替换的门字形拾放轨迹中直角过渡段的直角边长，δt为时间间隔；
[0088]
根据公式利用牛顿迭代法确定每个时间段的参数的值；其中，为初始参数值，分别为r
‑
1次、r次迭代在第i个时间段的参数的值，γ
i
‑1为在第i
‑
1个时间段的参数的值，v2(γ
i
‑1)为在第i个时间段沿ph曲线轨迹运动的速度，σ(γ
i
‑1)为ph曲线弧长对参数γ
i
‑1的导数，为r
‑
1次迭代时的参数求解函数，为对时间的导数；
[0089]
将每个时间段的参数的值代入ph曲线参数依赖的运动规律表达式中，获得ph曲线参数依赖的运动规律。
[0090]
确定第二多项式运动规律的步骤为：
[0091]
确定第二直线轨迹的第二多项式运动规律为v3(φ3)＝v
30
v
31
φ3 v
32
φ
32
v
33
φ
33
；其中，v3(φ3)为第二直线轨迹的速度，φ3为第二变量，φ3＝t3/t3，φ3∈[0,1]，t3为并联机器人从第二交点到对称分界点所需的总时间，t3为按照第二直线轨迹运动的实时时间，v
30
为第三常数项，v
31
、v
32
、v
33
分别为第二多项式运动规律中多项式的系数；
[0092]
根据v3(0)＝v
b
,v3(1)＝v
max
,v3'(0)＝0,v3'(1)＝0,s3(0)＝0的边界条件，对第二直线轨迹的第二多项式运动规律进行求解，获得第二直线轨迹的多项式运动规律为v3(φ3)＝v
b
3(v
max
‑
v
b
)φ
32
2(v
b
‑
v
max
)φ
33
；其中，v
b
为并联机器人运动到第一交点时的速度，v
max
为按照第二直线轨迹运动的最大速度，v3(0)、v3'(0)、s3(0)分别为并联机器人在第二交点的速度、加速度和位移，v3(1)、v3'(1)分别为并联机器人在对称分界点的速度和加速度。
[0093]
本发明公开的一种新型含约束的拾放delta并联机器人的轨迹规划方法，在尽可
能减少冲击振荡的基本要求下实现避障功能，并且相较现有方法提高计算效率、降低计算时间、提高系统实时性。
[0094]
实施例二
[0095]
结合图2
‑
5进一步详细说明本发明的轨迹规划方法。
[0096]
术语解释
[0097]
ph曲线：即pythagorean
‑
hodograph曲线，是指一类具有参数速度的多项式曲线，可用于解决曲线弧长及等距线的计算问题。
[0098]
ppo路径：即pick
‑
and
‑
place operation路径，指机器人在进行拾取
‑
放置这一套动作时行经的路径。
[0099]
delta机器人：机器人结构如图5所示，包括定平台、动平台、主动杆和从动杆，三条支链对称分布。
[0100]
一种并联机器人动态拾取轨迹规划方法的基本思路为：
[0101]
1.构建基本拾取动作门字型路径；
[0102]
2.在满足几何约束的情况下计算ph曲线；
[0103]
3.在不同阶段运用不同的运动规律规划轨迹，即多项式运动规律和依赖于ph曲线参数的运动规律，以获得笛卡尔空间和关节空间中的平滑性能。
[0104]
具体步骤如下：
[0105]
步骤1：为实现并联机器人含约束复杂环境的拾放操作，对delta机器人进行门字形轨迹拾放操作轨迹规划，其中拾放操作轨迹的平滑曲线(如图2的bc弧线)与直角顶点(如图2的bhc直角)的偏离不超过指定的几何约束d，即图2中的参数d表示ppo路径中直角与ph曲线的最大偏差，称为规定几何约束，实际情况中d通常为已知，d可以理解为障碍物的直径。
[0106]
步骤2：步骤1中所述的门字形轨迹如图2所示，delta机器人首先在a点抓取物体，沿着abcdefg的顺序达到目标点g。其中ab，cd，de，fg段为直线，bc和ef为五次ph曲线。h、i点为无过渡情况下门轨迹的顶点，为了避障以及避免振荡，从b到c点走曲线bc而不走bh、hc的直线路径。各段直线的长度分别表示为|ag|＝w，|hc|＝|ei|＝k，|bh|＝|fi|＝m，|ab|＝|gf|＝j，|ah|＝|gi|＝h。
[0107]
在一般的拾放操作中，轨迹路径中|ag|和|ah|的值即w和h的值通常为已知。
[0108]
步骤3：此步骤将确定d与m的关系从而确定ph曲线。把过渡段的曲线表示出来如图3所示。
[0109]
其中步骤1中的弧线段bc和ef均采用五次ph曲线，ph曲线公式如下：
[0110][0111]
公式(1)展开为：
[0112][0113]
其中，γ是曲线的参数，由于本发明采用5次ph曲线构成路径的过渡段，因此这里n＝5；p
i
＝(x
i
,y
i
)表示曲线的控制点，i＝0,
…
,5。
[0114]
根据公式(1)能计算出曲线的x、y坐标，用含γ的式子表示为：
[0115][0116]
根据ph曲线定义可知u0、u2、v2均为多项式，具体如下：
[0117][0118]
根据计算，如图3所示门字型轨迹中的直角顶点h与曲线bc距离最大时，取曲线上的点为n，此时γ＝0.5。在ppo路径上，为了简化计算以及满足实时性要求，本专利将假设m＝k，因此公式3可以化为：
[0119][0120]
根据公式(4)可知，
[0121]
接下来计算d，经计算n点坐标可表示为：
[0122][0123]
根据图4，通过简单的几何关系可得h点y坐标值为n点的横坐标值与n点的纵坐标值相加，h点由于在y轴上所以横坐标为0，即：
[0124][0125]
根据h和n点坐标可求得步骤1中所述的几何约束d，即
[0126][0127]
为使拾放操作满足步骤1中所述的几何约束d，只要设计拾放操作轨迹中的m使其满足m＝d/0.17064，则满足了拾放操作轨迹的平滑曲线(如图2的bc弧线)与直角顶点(如图2的bhc直角)的偏离不会超过指定的几何约束d的要求，至此ph曲线得以确定。
[0128]
步骤4：步骤1中的拾放操作中的ab段中，机器人末端执行器的运动规律采用
[0129]
v1(φ)＝v
10
v
11
φ v
12
φ2 v
13
φ3 v
14
φ4 v
15
φ5ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(8)
[0130]
其中v1表示末端执行器在ab段的速度，v
10
到v
15
为系数，φ定义为φ＝t1/t1，φ∈[0,1]，t1表示为末端执行器从a点运动到b点所需的总时间，t1表示为实时时间；同时用v
b
表示末端执行器运动到b点时的速度。接下来根据初始条件计算各个系数。根据边界条件可知在初始时刻时，末端执行器速度、加速度、加速度变化率均为0，而在t1时刻，速度变为v
b
，加速度、加速度变化率也为0，即v1(0)＝0,v1(1)＝v
b
,v
′1(0)＝0,v
′1(1)＝0,v
″1(0)＝0,v
″1(1)＝0，所以公式8可化为
[0131]
v1(φ)＝10v
b
φ3‑
15v
b
φ4 6v
b
φ5ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(9)
[0132]
由速度可求得位移，进而可求得对应时间对应位置，这样ab段的轨迹规划即完成。
[0133]
步骤5：步骤1中的拾放操作中的bc段中，采用的是步骤3所示的ph曲线，其参数依赖的运动规律采用公式10，即：
[0134]
v2(γ)＝v
20
v
21
γ v
22
γ2 v
23
γ3 v
24
γ4 v
25
γ5ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(10)
[0135]
其中v2表示末端执行器在bc段的速度，v
20
到v
25
为系数。接下来根据初始条件计算各个系数。根据步骤4中可得到v2(0)＝v
b
，由于ppo轨迹是对称的，所以可以得到v2(1)＝v
b
，为了保证加速度的连续性，有v2'(0)＝0,v2'(1)＝0，点n处速度应为最小，这样公式10化为：
[0136]
v2(γ)＝v
b
16(v
n
‑
v
b
)γ2 32(v
b
‑
v
n
)γ3 16(v
n
‑
v
b
)γ4ꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(11)
[0137]
为了得知参数γ
i
与时间的关系，定义在bc段运动的实时时间t2＝iδt，δt为时间间隔，i＝1,2,3,...n,t2表示bc段运动的总时间。ph曲线中参数和时间的关系为：
[0138][0139]
其中，s表示曲线弧长，σ(γ)为弧长s对参数γ求导后的结果；
[0140][0141]
由公式12推导至公式14可求得每个时刻对应的参数值：
[0142][0143]
其中其中
[0144]
对公式14整理为函数f(γ
i
)：
[0145][0146]
分析f(γ
i
)可知该函数下ph曲线参数γ
i
有唯一解，然后根据牛顿迭代法通过公式16求解ph曲线参数γ
i
：
[0147][0148]
利用牛顿迭代法来求解γ
i
，迭代法的思想是不停计算，这个计算次数较大只能由计算机完成，表示初始值，由公式(16)的第二行给出，右上角的(r)表示进行了r次的计算后得到的第r个γ
i
。因为i表示分段后的第几段运动，所以γ0＝0。利用公式16可以算出然后再根据公式16可以不停地得到新的γ
i
的值，而迭代法的意义就在于，当进行了足够多次的运算后，前一个和后一个之间的差距会越来越来小最终误差小到可以忽略不计，这样稳定下来的值就是γ
i
的值。
[0149]
由公式16解得γ
i
即可求得当前时刻的速度值，即公式10得以确定。同步骤4，由速度可求得位移，进而可求得对应时间对应位置，这样bc段的轨迹规划即完成。
[0150]
步骤6：步骤1中的拾放操作中的cd段中，机器人末端执行器的运动规律采用
[0151]
v3(φ)＝v
30
v
31
φ v
32
φ2 v
33
φ3ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(17)
[0152]
其中v3表示末端执行器在cd段的速度，v
30
到v
33
为系数，φ定义为φ＝t3/t3，φ∈
[0,1]，t3表示为末端执行器从c点运动到d点所需的总时间，t3表示为实时时间。接下来根据初始条件计算各个系数。根据边界条件确定各个系数，v3(0)＝v
b
,v3(1)＝v
max
,v3'(0)＝0,v3'(1)＝0,s3(0)＝0，s3表示cd段的实时位移，所以公式17可化为
[0153]
v3(φ)＝v
b
3(v
max
‑
v
b
)φ2 2(v
b
‑
v
max
)φ3ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(18)
[0154]
其中s3(1)＝(w
‑
2k)/2，所以可得t3＝(w
‑
2k)/(v
b
v
max
)，且d点速度达到最大，即v
d
＝v
max
.由速度可求得位移，进而可求得对应时间对应位置，这样cd段的轨迹规划即完成。
[0155]
步骤7：步骤1中的拾放操作中的de段中，计算方法与cd段相似。
[0156]
步骤8：步骤1中的拾放操作中的ef段中，计算方法与bc段相似。
[0157]
步骤9：步骤1中的拾放操作中的fg段中，计算方法与ab段相似。
[0158]
本发明相对于现有技术的优点为：
[0159]
本发明有助于避免机器人在执行过程中与周围环境发生碰撞，从而提高机器人系统的安全性；
[0160]
在步骤7、8、9可以提高计算效率，这有助于实现实时插值。如果有两条ppo轨迹需要满足相同的规定几何约束，它们在图3所示的xoy坐标系中的ph曲线是相同的，因此，不必重新求解酸碱度曲线，节省了时间，有利于提高系统的实时性；
[0161]
由于ppo轨迹和ph曲线都是对称的，可以利用对称性简化计算以节省时间。
[0162]
实施例三
[0163]
本发明还提供了一种并联机器人动态拾放轨迹规划系统，系统包括：
[0164]
ph曲线求解模块，用于以最短避障距离为几何约束，对ph曲线进行求解，获得ph曲线多项式方程以及ph曲线与门字形拾放轨迹的第一交点和第二交点；
[0165]
替换模块，用于将第一交点和第二交点之间的ph曲线替换门字形拾放轨迹中第一交点和第二交点之间的直角过渡段；
[0166]
替换后的门字形拾放轨迹获得模块，用于以替换前的门字形拾放轨迹的中轴线为对称轴，将另一个直角过渡段替换为第一交点和第二交点之间的ph曲线的对称ph曲线，获得替换后的门字形拾放轨迹；
[0167]
执行模块，用于基于替换后的门字形拾放轨迹的对称性，并联机器人依据运动规律按照替换后的门字形拾放轨迹进行运动。
[0168]
ph曲线求解模块，具体包括：
[0169]
关系方程构建子模块，用于构建ph曲线多项式方程的系数与ph曲线所要替换的门字形拾放轨迹中直角过渡段的直角边长的关系方程；
[0170]
直角边长确定子模块，用于根据最短避障距离，利用公式d＝0.17064m，确定ph曲线所要替换的门字形拾放轨迹中直角过渡段的直角边长；其中，d为最短避障距离，m为ph曲线所要替换的门字形拾放轨迹中直角过渡段的直角边长；
[0171]
系数获得子模块，用于将ph曲线所要替换的门字形拾放轨迹中直角过渡段的直角边长代入关系方程，获得ph曲线多项式方程的系数。
[0172]
本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
[0173]
本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。