CN105676642A - 一种六自由度机器人工位布局与运动时间协同优化方法 - Google Patents

Abstract

本发明涉及一种六自由度机器人工位布局与运动时间协同优化方法，包括以下步骤：S1：对六自由度机器人进行运动学建模；S2：根据六自由度机器人、起点设备和目标设备的物理场景建立工位布局与运动时间的协同优化数学模型；S3：在空间直角坐标系内，以六自由度机器人的末端执行器从起点设备运动到目标设备所用时间为PSO算法的适应度，以起点设备和目标设备的工位布局为PSO算法的粒子，基于PSO算法得到协同优化数学模型的最优解以及最优解对应的工位布局。与现有技术相比，本发明实现六自由度机器人工位的优化，大大提高了机器人工作效率。

Description

一种六自由度机器人工位布局与运动时间协同优化方法

技术领域

本发明涉及机器人技术与生产线规划领域，尤其是涉及一种六自由度机器人工位布局与运动时间协同优化方法。

背景技术

在生产线规划完毕后，需要对工位进行设计。机器人工位设计作为工位设计之中一类难度较高的设计，其除了要考虑到设备布局之外还要考虑到机器人轨迹规划。传统的机器人工位设计步骤是：一、工位工艺确定与设备选定。二、设备及机器人布局三、机器人轨迹规划。在企业中一般的机器人工位均可以采用上述方法进行机器人工位的设计，而且在轨迹规划过程中仅靠现场示教编程就可以完成。但是，随着机器人工位的复杂化以及设备布局与机器人轨迹之间的相互影响，高效的机器人工位设计越来越困难。所以，有必要将机器人动作复杂，可能成为生产线中瓶颈的机器人工位做进一步优化。传统的串行设计方法将设备布局与机器人运动规划独立考虑，实际现实中设备的布局与机器人的运动轨迹息息相关，其影响着机器人的运动总时间。故本发明主要解决的问题就是如何通过同时优化设备布局与机器人运动轨迹来尽量达到工位所花时间最少。

涂胶机器人工位属于机器人工位中的一种，随着涂胶规划的复杂，或者有些企业为了节约成本以及提高设备利用率将一台涂胶机器人共用于两个工位，这就会存在工位操作总时间进一步优化的需要。

中国专利CN204338390U公开了一种涂胶机器人工作站，主要讲述涂胶机器人工作站的一类形式，具有结构简单，效率高，能完成曲面涂胶的优点。中国专利CN102513268A公开了一种涂胶机器人及其涂胶方法，描述一类新的涂胶机器人构成，能够识别车型，进行自动涂胶。中国专利CN203816833U公开了一种涂胶机器人，提出一种新颖的涂胶机器人结构，能很好地保证出料的清洁度从而不会因为出料口的余胶而导致余胶胶型不合格影响装配质量。中国专利CN201380143公开了小型智能涂胶机器人装置，介绍一种自动涂胶装置，利用图像处理技术，得到各关节的运动轨迹，从而实现了自动涂胶。以上专利采用六自由度机器人手臂实现涂胶，但未考虑工位布局与涂胶机器人运动轨迹的协同优化。

中国专利CN101387888B公开了一种基于工进制量子粒子群算法的移动机器人路径规划方法，特征是包含如下步骤：步骤一：把机器人简化成一个点，并在二维空间内运动，通过视觉系统能感知自己目前的位姿和障碍物的位置：步骤二：将机器人视觉系统感知到的所有障碍物处理成凸多边形：步骤三：将三维空间离散化为一系列的栅格，并对移动机器人在每一个栅格处的八个可能运动方向进行二进制编码：步骤四：定义从起点到目标点的路径的长短为该方法需要求解的目标函数：步骤五：针对机器人路径规划问题的离散特征，利用工进制量子粒子群算法对步骤四中的目标函数进行全军局优化以得到最优的移动机器人路径。该专利采用工进制量子粒子群算法对移动机器人的路径进行规划，不涉及机械臂轨迹与工位布局的内容。

中国专利CN102662399A公开了一种面向多目标的水果采摘机器人的路径优化方法，包括以下步骤：(1)采用三点三步法对六自由度水果采摘机器人末端执行器的坐标系进行标定；(2)通过六自由度水果采摘机器人的机械手末端执行器摄像头动态采集整个果树树冠的图像，并得到多个水果中心位置的三维坐标；(3)将所述水果中心位置的三维坐标视为树冠图像的顶点，并构造出一个无方向的连接图，采用哈密尔顿图形法实现六自由度水果采摘机器人路径的优化；(4)快速搜索出哈密尔顿图中的一个关于所有所述顶点的排列。该专利优化六自由度水果采摘机器人路径，使六自由度水果采摘机器人在最短的时间内，采摘到最多的水果，但水果位置是不确定的，与工作站的工位可调整设计的情况不同，因此该专利方法并不适用于工位布局优化。

发明内容

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种六自由度机器人工位布局与运动时间协同优化方法，实现六自由度机器人工位的优化，大大提高了机器人工作效率。

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

一种六自由度机器人工位布局与运动时间协同优化方法，用于控制六自由度机器人的末端执行器从起点设备运动到目标设备，包括以下步骤：

S1：建立以六自由度机器人为原点的空间直角坐标系，同时对六自由度机器人进行运动学建模，得到机器人运动学方程；

S2：根据六自由度机器人、起点设备和目标设备的物理场景建立工位布局与运动时间的协同优化数学模型，所述协同优化数学模型包括目标函数和约束条件，所述目标函数Z满足Z＝mint_z，t_z表示六自由度机器人的末端执行器从起点设备运动到目标设备所用时间，所述时间t_z通过基于运动时间最短的轨迹规划和机器人运动学方程由起点设备和目标设备的坐标获得，所述约束条件由物理场景决定；

S3：在空间直角坐标系内，以时间t_z为PSO(ParticleSwarmOptimization，粒子群优化)算法的适应度，以起点设备和目标设备的工位布局为PSO算法的粒子，基于PSO算法得到协同优化数学模型的最优解以及最优解对应的工位布局。

所述步骤S1中采用D-H参数法对六自由度机器人进行运动学建模。

所述约束条件包括设备不重叠约束、可达性约束和工位约束；

所述设备不重叠约束规定起点设备与目标设备不重叠，且起点设备与目标设备之间保持设定的最小间距；

所述可达性约束保证六自由度机器人能到达起点设备、目标设备以及从起点设备到目标设备的整个轨迹；

所述工位约束规定工位布局满足预设的工位设计规则。

满足设备不重叠约束的公式如下：

A_ij·B_ij＝0

$A_{ij}=max\{(\frac{l_i+l_j}{2}+d_{x,ij})-|x_i-x_j|,0\}$

$B_{ij}=max\{(\frac{w_i+w_j}{2}+d_{y,ij})-|y_i-y_j|,0\}$

式中，下标i表示起点设备，下标j表示目标设备，d_x,ij表示起点设备与目标设备在X轴方向上的最小间距，d_y,ij表示起点设备与目标设备在Y轴方向上的最小间距，l_i表示起点设备在X轴方向上的长度，w_i表示起点设备在Y轴方向上的长度，l_j表示目标设备在X轴方向上的长度，w_j表示目标设备在Y轴方向上的长度，x_i表示起点设备的中心在空间直角坐标系中的X轴坐标，y_i表示起点设备的中心在空间直角坐标系中的Y轴坐标，x_j表示目标设备的中心在空间直角坐标系中的X轴坐标，y_j表示目标设备的中心在空间直角坐标系中的Y轴坐标。

所述步骤S3具体为：

31：随机生成N个粒子，并初始化个体极值pbest_n、全局极值gbest和最大迭代次数，生成的粒子满足以下公式：

$P_n^k=\{(x_{n,i}^k,y_{n,i}^k),(x_{n,j}^k,y_{n,j}^k)\}$

$V_n^k=\{(v_{n,x,i}^k,v_{n,y,i}^k),(v_{n,x,j}^k,v_{n,y,j}^k)\}$

式中，下标n表示第n个粒子，下标i表示起点设备，下标j表示目标设备，表示第k次迭代中第n个粒子，表示第k次迭代中第n个粒子对应起点设备的X轴坐标，表示第k次迭代中第n个粒子对应起点设备的Y轴坐标，表示第k次迭代中第n个粒子对应目标设备的X轴坐标，表示第k次迭代中第n个粒子对应目标设备的Y轴坐标，表示第k次迭代中第n个粒子的速度，表示第k次迭代中第n个粒子对应起点设备在X轴方向上的移动速度，表示第k次迭代中第n个粒子对应起点设备在Y轴方向上的移动速度，表示第k次迭代中第n个粒子对应目标设备在X轴方向上的移动速度，表示第k次迭代中第n个粒子对应目标设备在Y轴方向上的移动速度；

32：每个粒子以各自对应的六自由度机器人的末端执行器从起点设备运动到目标设备所用时间t_z为自身的适应度，通过迭代，每个粒子跟踪个体极值pbest_n和全局极值gbest来更新自身；

33：输出全局极值gbest对应的粒子为最优工位布局，最优工位布局下的时间t_z为最短运动时间。

所述步骤32具体为：

321：在第k次迭代中，验证第n个粒子是否满足约束条件，若是，执行步骤322，若否，执行步骤323；

322：获取每个粒子对应六自由度机器人的末端执行器从起点设备运动到目标设备所用时间t_z，令t_z,n＝S_n，t_z,n表示第n个粒子对应的时间t_z，S_n表示第n个粒子的适应度，执行步骤324，

323：直接赋值第n个粒子有极大的适应度，即S_n＝∞，执行步骤324；

324：判断S_n是否小于pbest_n和gbest的数值，若是，则更新pbest_n和gbest的数值，执行步骤325，若否，直接执行步骤325；

325：根据pbest_n和gbest的数值更新第n个粒子的位置与速度；

326：判断迭代次数k是否达到最大迭代数，若是，结束迭代，若否，跳转步骤321。

所述步骤325中第n个粒子的位置与速度的更新满足以下公式：

$v_{n,x,i}^{k+1}={\mathrm{wv}}_{n,x,i}^k+c_1{r}_1({\mathrm{pbest}}_n-x_{n,i}^k)+c_2{r}_2(gbest-x_{n,i}^k)$

$v_{n,y,i}^{k+1}={\mathrm{wv}}_{n,y,i}^k+c_1{r}_1({\mathrm{pbest}}_n-y_{n,i}^k)+c_2{r}_2(gbest-y_{n,i}^k)$

$v_{n,x,j}^{k+1}={\mathrm{wv}}_{n,x,j}^k+c_1{r}_1({\mathrm{pbest}}_n-x_{n,j}^k)+c_2{r}_2(gbest-x_{n,j}^k)$

$v_{n,y,j}^{k+1}={\mathrm{wv}}_{n,y,j}^k+c_1{r}_1({\mathrm{pbest}}_n-y_{n,j}^k)+c_2{r}_2(gbest-y_{n,j}^k)$

$x_{n,i}^{k+1}=x_{n,i}^k+v_{n,x,i}^{k+1}$

$x_{n,j}^{k+1}=x_{n,j}^k+v_{n,x,j}^{k+1}$

$y_{n,i}^{k+1}=y_{n,i}^k+v_{n,y,i}^{k+1}$

$y_{n,j}^{k+1}=y_{n,j}^k+v_{n,y,j}^{k+1}$

式中，w为惯性权重，c₁、c₂为学习因子，r₁、r₂为介于(0、1)之间的随机数。

基于运动时间最短的轨迹规划和机器人运动学方程由起点设备和目标设备的坐标获得时间t_z的方法包括以下步骤：

1)以正弦函数拟合六自由度机器人的三角波运动控制模型和梯形波运动控制模型，获取三角波运动控制模型和梯形波运动控制模型相互转换的临界关节运动角度θ_g0，满足以下公式：

${\mathrm{\θ}}_{g0}=\frac{{\mathrm{\π\ω}}_{g,max}^2}{2{\mathrm{\α}}_{g,max}}$

式中，α_g,max表示第g个关节的最大加速度，ω_g,max表示第g个关节的最大速度；

2)基于机器人运动学方程，根据起点设备和目标设备获得六自由度机器人第g个关节的起点关节角度θ_g,i和目标关节角度θ_g,j的所有实数解，g＝1,2,...，6，下标i表示起点设备，下标j表示目标设备；

3)根据临界关节运动角度θ_g0、起点关节角度θ_g,i和目标关节角度θ_g,j获得第..个关节在六自由度机器人的末端执行器从起点设备运动到目标设备时最短运动时间t_ming，满足以下公式：

t_min,g＝min{t_min(θ_g,i,p,θ_g,j,q)}

$t_{\min }({\mathrm{\&theta;}}_{g,i,p},{\mathrm{\&theta;}}_{g,j,q})=\left\{\begin{array}{cc}\sqrt{\frac{2\mathrm{\&pi;}({\mathrm{\&theta;}}_{g,j,q}-{\mathrm{\&theta;}}_{g,j,q})}{{\mathrm{\&alpha;}}_{g,max}}},& ({\mathrm{\&theta;}}_{g,j,q}-{\mathrm{\&theta;}}_{g,j,q})<{\mathrm{\&theta;}}_{g0}\\ \frac{({\mathrm{\&theta;}}_{g,j,q}-{\mathrm{\&theta;}}_{g,j,q})}{{\mathrm{\&omega;}}_{g,\max }}+\frac{{\mathrm{\&pi;\&omega;}}_{g,max}}{2{\mathrm{\&alpha;}}_{g,max}},& ({\mathrm{\&theta;}}_{g,j,q}-{\mathrm{\&theta;}}_{g,j,q})\&GreaterEqual;{\mathrm{\&theta;}}_{g0}\end{array}\right.$

式中，θ_g,i,p表示起点关节角度θ_g,i的第p个实数解，θ_g,j,q表示目标关节角度θ_g,j的第q个实数解，t_min(θ_g,i,p,θ_g,j,q)表示第g个关节从θ_g,i,p运动到θ_g,j,q所用最短时间，α_g,max表示第g个关节的最大加速度，ω_g,max表示第g个关节的最大速度；

4)获得时间t_z，满足以下公式：

t_z＝max(t_min,g),g＝1,2,...，6。

所述目标设备包括两个相同的目标子设备时，采用双工位设置，两个目标子设备以六自由度机器人到起点设备的直线为对称轴对称设置。

与现有技术相比，本发明具有以下优点：

1)本发明根据物理场景建立工位布局与运动时间的协同优化数学模型，以获取最少的六自由度机器人的末端执行器从起点设备运动到目标设备所用时间为目标，通过PSO算法和基于运动时间最短的轨迹规划实现工位布局与涂胶机器人运动轨迹的协同优化，大大提高六自由度机器人的工作效率。

2)本发明以六自由度机器人的末端执行器从起点设备运动到目标设备所用时间为PSO算法的适应度，以起点设备和目标设备的工位布局为PSO算法的粒子，并设置设备不重叠约束、可达性约束和工位约束，算法简单有效，快速高效地求解协同优化数学模型的最优值。

3)本发明设置的设备不重叠约束、可达性约束和工位约束等充分反映了工作站中真实物理场景设计要求，保证最终获得的工位布局真实有效。

4)本发明设计了基于运动时间最短的轨迹规划，随着PSO算法不断地生成新的工位布局方案的同时，轨迹规划也在不断地寻找当前布局下最优轨迹，最终获得一种理想的工位布局方案，同时获得了最优布局下的最佳的轨迹方案。

5)本发明适用于涂胶工作站中六自由度机器人涂胶布局与运动时间的优化，将涂胶时间的优化问题等价成机器人从运动原点(起点设备)到涂胶起点(目标设备)运动时间的优化问题，可以提高涂胶工位的效率，相较于传统的涂胶机器人工位设计方法，本发明方法具有更佳的涂胶效率。

附图说明

图1为本发明方法流程示意图；

图2为涂胶工作站中物理场景示意图；

图3为涂胶工作站中物理场景数学模型示意图；

图4为库卡机器人KR270R2700ultra的D-H建模示意图；

图5为机器人关节梯形波运动控制模型示意图；

图6为机器人关节三角波运动控制模型示意图；

图7为POS算法适应度收敛曲线示意图；

图8为优化结果布局图；

图9为机器人关节速度曲线示意图；

其中，(9a)为第1个关节的速度-时间曲线，(9b)为第2个关节的速度-时间曲线，(9c)为第3个关节的速度-时间曲线，(9d)为第4个关节的速度-时间曲线，(9e)为第5个关节的速度-时间曲线，(9f)为第6个关节的速度-时间曲线；

图10为机器人于硅胶杯上方起点的位姿图；

图11为机器人于托盘上方终点的位姿图。

图中：1、硅胶杯，2、转台，3、托盘，4、涂胶机器人。

具体实施方式

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

如图1所示，一种六自由度机器人工位布局与运动时间协同优化方法，用于控制六自由度机器人的末端执行器从起点设备运动到目标设备，包括以下步骤：

S1：建立以六自由度机器人为原点的空间直角坐标系，同时采用D-H参数法对六自由度机器人进行运动学建模，得到机器人运动学方程。其中，D-H参数法为Denavit和Hartenberg于1995年提出的一种为关节链中的每一杆件建立坐标系的矩阵方法，为机器人运动学建模的经典方法，通过D-H参数法可以得到机器人的正运动学θ→T方程和逆运动学T→θ方程，在正逆运动学公式推导过程中主要依据以下两式：

$\begin{array}{c}{T_n}_{n+1}=T_{RZ}\left({\mathrm{\&theta;}}_{n+1}\right)T_Z\left(d_{n+1}\right)T_X\left(a_{n+1}\right)T_{RX}\left({\mathrm{\&alpha;}}_{n+1}\right)=\\ \left[\begin{array}{cccc}{\mathrm{cos\&theta;}}_{n+1}& -{\mathrm{sin\&theta;}}_{n+1}{\mathrm{cos\&alpha;}}_{n+1}& {\mathrm{sin\&theta;}}_{n+1}{\mathrm{sin\&alpha;}}_{n+1}& a_{n+1}{\mathrm{cos\&theta;}}_{n+1}\\ {\mathrm{sin\&theta;}}_{n+1}& {\mathrm{cos\&theta;}}_{n+1}{\mathrm{cos\&alpha;}}_{n+1}& -{\mathrm{cos\&theta;}}_{n+1}{\mathrm{sin\&alpha;}}_{n+1}& a_{n+1}{\mathrm{sin\&theta;}}_{n+1}\\ 0& {\mathrm{sin\&alpha;}}_{n+1}& {\mathrm{cos\&alpha;}}_{n+1}& d_{n+1}\\ 0& 0& 0& 1\end{array}\right]\end{array}$

${T_0}_6=\left[\begin{array}{cccc}{n}_x& o_x& a_x& p_x\\ n_y& o_y& a_y& p_y\\ n_z& o_z& a_z& p_z\\ 0& 0& 0& 1\end{array}\right]={T_0}_1{T_1}_2{T_2}_3{T_3}_4{T_4}_5{T_5}_6$

式中，ⁿT_n+1表示从坐标系n变换到坐标系n+1的坐标变换矩阵，由4个齐次变换矩阵相乘得到。⁰T₆表示六自由度机器人从机座坐标系变换到末端执行器的坐标变换矩阵。接近矢量表示夹持器进入物体的方向，方向矢量表示指尖互相指向，法线矢量表示指尖互相指向，以上三个矢量分别为末端执行器的Z轴、Y轴和X轴方向。θ、d、a、α是D-H参数法关节变量。

S2：根据六自由度机器人、起点设备和目标设备的物理场景建立工位布局与运动时间的协同优化数学模型，协同优化数学模型包括目标函数和约束条件，目标函数Z满足Z＝mint_z，t_z表示六自由度机器人的末端执行器从起点设备运动到目标设备所用时间，时间t_z通过基于运动时间最短的轨迹规划和机器人运动学方程由起点设备和目标设备的坐标获得，约束条件由物理场景决定。

约束条件包括设备不重叠约束、可达性约束和工位约束；

设备不重叠约束规定起点设备与目标设备不重叠，且起点设备与目标设备之间保持设定的最小间距，满足设备不重叠约束的公式如下：

A_ij·B_ij＝0

$A_{ij}=max\{(\frac{l_i+l_j}{2}+d_{x,ij})-|x_i-x_j|,0\}$

$B_{ij}=max\{(\frac{w_i+w_j}{2}+d_{y,ij})-|y_i-y_j|,0\}$

式中，下标i表示起点设备，下标j表示目标设备，d_x,ij表示起点设备与目标设备在X轴方向上的最小间距，d_y,ij表示起点设备与目标设备在Y轴方向上的最小间距，l_i表示起点设备在X轴方向上的长度，w_i表示起点设备在Y轴方向上的长度，l_j表示目标设备在X轴方向上的长度，w_j表示目标设备在Y轴方向上的长度，x_i表示起点设备的中心在空间直角坐标系中的X轴坐标，y_i表示起点设备的中心在空间直角坐标系中的Y轴坐标，x_j表示目标设备的中心在空间直角坐标系中的X轴坐标，y_j表示目标设备的中心在空间直角坐标系中的Y轴坐标；

可达性约束保证六自由度机器人能到达起点设备、目标设备以及从起点设备到目标设备的整个轨迹，验证可达性只需要将要到达位置点代入机器人逆运动学方程求解，若实数解存在则表示可达，反之不可达；

工位约束规定工位布局满足预设的工位设计规则，工位设计规则中人为事先确定了设备坐标系的运动范围。

基于运动时间最短的轨迹规划，是指根据机器人关节运动时间长短分别以两类运动控制模型去生成机器人在相邻路径点之间的轨迹，使机器人关节在最短时间内到达指定位置，这两类运动控制模型分别是梯形波与三角波运动控制模型，梯形波与三角波运动控制模型中设计了六自由度机器人关节速度与时间的曲线，如图5、6所示。

梯形波与三角波运动控制模型的原理如下：设第g个关节(g＝1,2,...，6)的角度从角度θ_g1运动到角度θ_g2，始末位置速度为0。当运动时间较短时，运动控制模型为准三角波；当运动时间较长时，运动控制模型为准梯形波。由于关节运动角度与运动时间相关，故可记θ_g0为第g个关节两种运动控制模型相互转换的临界关节运动角度。以下分两种情况进行计算：

考虑到正弦函数具有光滑可导的特性且在图5、图6加速段与减速段均能满足拟合的需求，因此当θ_g2-θ_g1≥θ_g0时，图3中时间t≤t₁部分函数可以用正弦函数去拟合运动控制模型曲线，关节速度可设为：

则关节角度：

关节加速度：

A、B、C、ω、均为待定系数。α_g,max表示第g个关节的最大加速度，ω_g,max表示第g个关节的最大速度，两者均由机器人说明书给定。边界条件：(1)(3)ω(0)＝0，(4)ω(t₁)＝ω_g,max，(5)α(0)＝0，(6)α(t₁)＝0，(7)最大加速度条件：Aω＝α_g,max。

将7个边界条件代入ω(t)、α(t)中，得到：

$t_1=\frac{{\mathrm{\&pi;\&omega;}}_{g,max}}{2{\mathrm{\&alpha;}}_{g,max}}$

$t_{spg}=\frac{{\mathrm{\&theta;}}_{g2}-{\mathrm{\&theta;}}_{g1}}{{\mathrm{\&omega;}}_{g,max}}+\frac{{\mathrm{\&pi;\&omega;}}_{g,max}}{2{\mathrm{\&alpha;}}_{g,max}}$

当θ_g2-θ_g1＜θ_i0时，图4中部分速度曲线可设为：

则关节角度：

关节加速度：

A、B、C、ω、均为待定系数。边界条件： $\left(2\right){\&Integral;}_0^{t_{spg}/2}\mathrm{\&omega;}\left(t\right)dt=\frac{{\mathrm{\&theta;}}_{g2}-{\mathrm{\&theta;}}_{g1}}{2},$ (3)ω(0)＝0，(4)α(0)＝0， $\left(5\right)\mathrm{\&alpha;}\left(\frac{t_{spg}}{2}\right)=0,$ (6)最大加速度条件：Aω＝α_g,max。

将6个边界条件代入ω(t)、α(t)中，得到：

$t_{spg}=\sqrt{\frac{2\mathrm{\&pi;}({\mathrm{\&theta;}}_{g2}-{\mathrm{\&theta;}}_{g1})}{{\mathrm{\&alpha;}}_{g,max}}}$

t_spg即为单个关节从角度θ_g1运动到角度θ_g2所用最短时间(简称单关节最短运动时间)，机器人在运动过程中的时间为六个关节运动时间中最长的单关节最短运动时间。

根据临界关节运动角度的定义，可求得：

${\mathrm{\&theta;}}_{g0}=\frac{{\mathrm{\&pi;\&omega;}}_{g,max}^2}{2{\mathrm{\&alpha;}}_{g,max}}$

由上述的公式推导可知，基于运动时间最短的轨迹规划和机器人运动学方程由起点设备和目标设备的坐标获得时间t_z的方法包括以下步骤：

1)以正弦函数拟合六自由度机器人的三角波运动控制模型和梯形波运动控制模型，获取三角波运动控制模型和梯形波运动控制模型相互转换的临界关节运动角度θ_g0，满足以下公式：

${\mathrm{\&theta;}}_{g0}=\frac{{\mathrm{\&pi;\&omega;}}_{g,max}^2}{2{\mathrm{\&alpha;}}_{g,max}}$

式中，α_g,max表示第g个关节的最大加速度，ω_g,max表示第g个关节的最大速度；

2)基于机器人运动学方程，根据机器人始末所要到达的点进行机器人位置反解，即根据起点设备和目标设备获得六自由度机器人第g个关节的起点关节角度θ_g,i和目标关节角度θ_g,j的所有实数解，g＝1,2,...，6，下标i表示起点设备，下标j表示目标设备；

3)根据临界关节运动角度θ_g0、起点关节角度θ_g,i和目标关节角度θ_g,j获得第g个关节在六自由度机器人的末端执行器从起点设备运动到目标设备时最短运动时间t_min,g，满足以下公式：

t_min,g＝min{t_min(θ_g,i,p,θ_g,j,q)}

$t_{\min }({\mathrm{\&theta;}}_{g,i,p},{\mathrm{\&theta;}}_{g,j,q})=\left\{\begin{array}{cc}\sqrt{\frac{2\mathrm{\&pi;}({\mathrm{\&theta;}}_{g,j,q}-{\mathrm{\&theta;}}_{g,j,q})}{{\mathrm{\&alpha;}}_{g,max}}},& ({\mathrm{\&theta;}}_{g,j,q}-{\mathrm{\&theta;}}_{g,j,q})<{\mathrm{\&theta;}}_{g0}\\ \frac{({\mathrm{\&theta;}}_{g,j,q}-{\mathrm{\&theta;}}_{g,j,q})}{{\mathrm{\&omega;}}_{g,\max }}+\frac{{\mathrm{\&pi;\&omega;}}_{g,max}}{2{\mathrm{\&alpha;}}_{g,max}},& ({\mathrm{\&theta;}}_{g,j,q}-{\mathrm{\&theta;}}_{g,j,q})\&GreaterEqual;{\mathrm{\&theta;}}_{g0}\end{array}\right.$

式中，θ_g,i,p表示起点关节角度θ_g,i的第p个实数解，θ_g,j,q表示目标关节角度θ_g,j的第q个实数解，需从(p·q)个机器人运动轨迹中选取单关节最短运动时间，t_min(θ_g,i,p,θ_g,j,q)表示第g个关节从θ_g,i,p运动到θ_g,j,q所用最短时间，即t_min(θ_g,i,p,θ_g,j,q)＝t_spg，α_g,max表示第g个关节的最大加速度，ω_g,max表示第g个关节的最大速度；

4)获得从θ_g,i,p对应位姿运动到θ_g,j,q对应位姿的时间t_z，满足以下公式：

t_z＝max(t_min,g),g＝1,2,...，6。

S3：在空间直角坐标系内，以时间t_z为PSO算法的适应度，以起点设备和目标设备的工位布局为PSO算法的粒子，基于PSO算法得到协同优化数学模型的最优解以及最优解对应的工位布局，即在优化工位布局的同时进行机器人运动轨迹的寻优，得到尽可能小的机器人运动时间。PSO算法中假设共有D台设备，每个粒子都是一种布局方案，粒子的编码是一个2D维向量，前、后D维分别表示每个设备的X与Y坐标。每个粒子的飞行方向和速率也是一个2D维向量，前、后D维分别表示设备在X与Y方向的移动速度。则步骤S3具体为：

31：随机生成N个粒子，并初始化个体极值pbest_n、全局极值gbest和最大迭代次数，生成的粒子满足以下公式：

$P_n^k=\{(x_{n,i}^k,y_{n,i}^k),(x_{n,j}^k,y_{n,j}^y)\}$

$V_n^k=\{(v_{n,x,i}^k,v_{n,y,i}^k),(v_{n,x,j}^k,v_{n,y,j}^k)\}$

式中，下标n表示第n个粒子，下标i表示起点设备，下标j表示目标设备，表示第k次迭代中第n个粒子，表示第k次迭代中第n个粒子对应起点设备的X轴坐标，表示第k次迭代中第n个粒子对应起点设备的Y轴坐标，表示第k次迭代中第n个粒子对应目标设备的X轴坐标，表示第k次迭代中第n个粒子对应目标设备的Y轴坐标，表示第k次迭代中第n个粒子的速度，表示第k次迭代中第n个粒子对应起点设备在X轴方向上的移动速度，表示第k次迭代中第n个粒子对应起点设备在Y轴方向上的移动速度，表示第k次迭代中第n个粒子对应目标设备在X轴方向上的移动速度，表示第k次迭代中第n个粒子对应目标设备在Y轴方向上的移动速度；

32：每个粒子以各自对应的六自由度机器人的末端执行器从起点设备运动到目标设备所用时间t_z为自身的适应度，通过迭代每个粒子，跟踪个体极值pbest_n和全局极值gbest来更新粒子自身；

33：输出全局极值gbest对应的粒子为最优工位布局，最优工位布局下的时间t_z为最短运动时间。

上述步骤32具体为：

321：在第k次迭代中，验证第n个粒子是否满足约束条件，若是，执行步骤322，若否，执行步骤323；

322：根据步骤S2中基于运动时间最短的轨迹规划获取每个粒子对应六自由度机器人的末端执行器从起点设备运动到目标设备所用时间t_z，令t_z,n＝S_n，t_z,n表示第n个粒子对应的时间t_z，S_n表示第n个粒子的适应度，执行步骤324，

323：直接赋值第n个粒子有极大的适应度，即S_n＝∞，执行步骤324；

324：判断S_n是否小于pbest_n和gbest的数值，若是，则更新pbest_n和gbest的数值，执行步骤325，若否，直接执行步骤325；

325：根据pbest_n和gbest的数值更新第n个粒子的位置与速度，更新公式满足以下公式：

$v_{n,x,i}^{k+1}={\mathrm{wv}}_{n,x,i}^k+c_1{r}_1({\mathrm{pbest}}_n-x_{n,i}^k)+c_2{r}_2(gbest-x_{n,i}^k)$

$v_{n,y,i}^{k+1}={\mathrm{wv}}_{n,y,i}^k+c_1{r}_1({\mathrm{pbest}}_n-y_{n,i}^k)+c_2{r}_2(gbest-y_{n,i}^k)$

$v_{n,x,j}^{k+1}={\mathrm{wv}}_{n,x,j}^k+c_1{r}_1({\mathrm{pbest}}_n-x_{n,j}^k)+c_2{r}_2(gbest-x_{n,j}^k)$

$v_{n,y,j}^{k+1}={\mathrm{wv}}_{n,y,j}^k+c_1{r}_1({\mathrm{pbest}}_n-y_{n,j}^k)+c_2{r}_2(gbest-y_{n,j}^k)$

$x_{n,i}^{k+1}=x_{n,i}^k+v_{n,x,i}^{k+1}$

$x_{n,j}^{k+1}=x_{n,j}^k+v_{n,x,j}^{k+1}$

$y_{n,i}^{k+1}=y_{n,i}^k+v_{n,y,i}^{k+1}$

$y_{n,j}^{k+1}=y_{n,j}^k+v_{n,y,j}^{k+1}$

式中，w为惯性权重，c₁、c₂为学习因子，r₁、r₂为介于(0、1)之间的随机数；

326：判断迭代次数k是否达到最大迭代数，若是，结束迭代，若否，跳转步骤321。

以如图2所示的涂胶工作站为例，涂胶机器人采用型号为KR270R2700ultra的六自由度机器人，硅胶杯(即起点设备)高0.5m，转台(即目标设备)直径1m，高0.818m(涂胶平面至底座平面)，同时采用双工位设置，两个转台沿涂胶机器人到硅胶杯的纵向平面对称摆放，根据本发明方法，进行工位布局和运动时间优化时，涂胶机器人的涂胶线速度是一定的，所以优化工位机器人运动总时间可以等价于优化机器人从硅胶杯运动到涂胶起点的时间，进而简化了问题。

首先，仅考虑一个转台的情况，可以得到如图3所示的六自由度机器人坐标系XOY，含x_i、y_i的坐标系表示硅胶杯的起点坐标系，含x_j、y_j的坐标系表示转台的目标坐标系，图3中矩形长宽均为1m，用Δ_x,i、Δ_y,i、Δ_x,j、Δ_y,j分别表示起点坐标系和目标坐标系偏离自身设备几何中心的距离。故原始硅胶杯与转台在机器人空间直角坐标系的位置分别为：F_i(0,-1.127,0.5)、F_j(-1.3437,-0.5416,0.818)，按⁰T₆及图3中机器人末端执行器所要达到的坐标方位，求得机器人末端执行器在起点坐标系与终点目标坐标系所对应的位姿矩阵T_Fi和T_Fj：

$T_{F_i}=\left[\begin{array}{cccc}0& 1& 0& 0\\ 1& 0& 0& -1.127\\ 0& 0& -1& 0.5\\ 0& 0& 0& 1\end{array}\right]$

$T_{F_j}=\left[\begin{array}{cccc}1& 0& 0& -1.3437\\ 0& -1& 0& -0.5416\\ 0& 0& -1& 0.8180\\ 0& 0& 0& 1\end{array}\right]$

根据D-H参数法进行机器人运动学建模，图4为库卡机器人KR270R2700ultra的D-H参数模型示意图，模型中参数如表1所示。

表1KR270R2700ultra的D-H参数

⁰T₆的左右矩阵元素分别相等，完成正运动学θ→T方程的推导。

$\left\{\begin{array}{c}{n}_x=S_6\&lsqb;C_1{S}_4{C}_{2+3}+S_1{C}_4\&rsqb;+C_6\&lsqb;C_5(C_1{C}_4{C}_{2+3}-S_1{S}_4)+C_1{S}_5{S}_{2+3}\&rsqb;\\ n_y=C_6\&lsqb;C_5(S_1{C}_4{C}_{2+3}+C_1{S}_4)+S_1{S}_5{S}_{2+3}\&rsqb;S_6(S_1{S}_4{C}_{2+3}-C_1{C}_4)\\ n_z=C_6(S_5{C}_{2+3}-C_4{C}_5{S}_{2+3})-S_4{S}_6{S}_{2+3}\\ o_x=S_6\&lsqb;C_5(S_1{S}_4-C_1{C}_4{C}_{2+3})-C_1{S}_5{S}_{2+3}\&rsqb;+C_6(C_1{S}_4{C}_{2+3}+S_1{C}_4)\\ o_y=C_6(S_1{S}_4{C}_{2+3}-C_1{C}_4)-S_6\&lsqb;C_5(S_1{C}_4{C}_{2+3}+C_1{S}_4)+S_1{S}_5{S}_{2+3}\&rsqb;\\ o_z=S_6(C_4{C}_5{S}_{2+3}-S_5{C}_{2+3})-S_4{C}_6{S}_{2+3}\\ a_x=-C_1{C}_5{S}_{2+3}+S_5(C_1{C}_4{C}_{2+3}-S_1{S}_4)\\ a_y=S_5(S_1{C}_4{C}_{2+3}+C_1{S}_4)-S_1{C}_5{S}_{2+3}\\ a_z=-C_5{C}_{2+3}-C_4{S}_5{S}_{2+3}\\ p_x=a_1{C}_1-d_6\&lsqb;C_1{C}_5{S}_{2+3}+S_5(S_1{S}_4-C_1{C}_4{C}_{2+3})\&rsqb;+d_4{C}_1{S}_{2+3}+a_2{C}_1{C}_2+a_3{C}_1{C}_{2+3}\\ p_y=a_1{S}_1+d_6\&lsqb;S_5(S_1{C}_4{C}_{2+3}+C_1{S}_4)-S_1{C}_5{S}_{2+3}\&rsqb;+d_4{S}_1{S}_{2+3}+a_2{S}_1{C}_2+a_3{S}_1{C}_{2+3}\\ p_z=d_1+d_4{C}_{2+3}-a_2{S}_2-d_6(C_5{C}_{2+3}+C_4{S}_5{S}_{2+3})-a_3{S}_{2+3}\end{array}\right.$

式中，C与S分别为Cos与Sin的简写；C₁,C₂,C₃,C₄,C₅,C₆分别表示Cos(θ₁),Cos(θ₂),Cos(θ₃),Cos(θ₄),Cos(θ₅),Cos(θ₆)；S₁,S₂,S₃,S₄,S₅,S₆分别表示Sin(θ₁),Sin(θ₂),Sin(θ₃),Sin(θ₄),Sin(θ₅),Sin(θ₆)；

再进行逆运动学T→θ方程的推导：

(1)记Y₁＝A₂A₃A₄A₅。由X₁(3,4)＝Y₁(3,4)可以求出机器人第一个关节角度

${\mathrm{\&theta;}}_1=arctan\left(\frac{p_y-a_y{d}_6}{p_x-a_x{d}_6}\right)$

(2)记 $X_2=A_1^{-1}\&times;{T_0}_6\&times;A_6^{-1},$ Y₂＝A₂A₃A₄A₅。由 $\left\{\begin{array}{c}{X}_2(1,4)=Y_2(1,4)\\ X_2(2,4)=Y_2(2,4)\end{array}\right.,$ 可以求得：

${\mathrm{\&theta;}}_3=\&PlusMinus;arctan\left(\frac{k}{\sqrt{d_4^2+a_3^2-k^2}}\right)-arctan\left(\frac{a_3}{d_4}\right)$

其中：k₁＝(p_x-a_xd₆)C₁+(p_y-a_yd₆)S₁-a₁，k₂＝a_zd₆-p_z+d₁。

(3)记 $X_3=A_2^{-1}\&times;A_1^{-1}\&times;{T_0}_6\&times;A_6^{-1},$ Y₃＝A₃A₄A₅。由 $\left\{\begin{array}{c}{X}_3(1,4)=Y_3(1,4)\\ X_3(2,4)=Y_3(2,4)\end{array}\right.,$ 求得：

${\mathrm{\&theta;}}_2=arctan\left(\frac{k_2(a_3{C}_3+d_4{S}_3+a_2)-k_1(a_3{S}_3-d_4{C}_3)}{k_2(a_3{S}_3-d_4{C}_3)+k_1(a_3{C}_3+d_4{S}_3+a_2)}\right)$

(4)记 $X_4=A_3^{-1}\&times;A_2^{-1}\&times;A_1^{-1}\&times;{T_0}_6\&times;A_6^{-1},$ Y₄＝A₄A₅。由 $\left\{\begin{array}{c}{X}_4(1,3)=Y_4(1,3)\\ X_4(2,3)=Y_4(2,3)\end{array}\right.,$ 求得：

若S₅≠0，则

${\mathrm{\&theta;}}_4=arctan\left(\frac{X_4(2,3)}{X_4(1,3)}\right)=arctan\left(\frac{a_y{C}_1-a_x{S}_1}{C_{2+3}(a_x{C}_1+a_y{S}_1)-a_z{S}_{2+3}}\right)$

若S₅＝0，则机械手为奇异形位，关节轴4和6重合，此时，可任取θ₄的值，再计算θ₆的值。

(5)记 $X_5=A_4^{-1}\&times;A_3^{-1}\&times;A_2^{-1}\&times;A_1^{-1}\&times;{T_0}_6\&times;A_6^{-1},$ Y₅＝A₅。由 $\left\{\begin{array}{c}{X}_5(1,3)=Y_5(1,3)\\ X_5(2,3)=Y_5(2,3)\end{array}\right.,$ 求得：

${\mathrm{\&theta;}}_5=arctan\left(\frac{S_5}{C_5}\right)$

(6)记 $X_6=A_5^{-1}\&times;A_4^{-1}\&times;A_3^{-1}\&times;A_2^{-1}\&times;A_1^{-1}\&times;{T_0}_6,$ Y₆＝A₆。由 $\left\{\begin{array}{c}{X}_6(2,1)=Y_6(2,1)\\ X_6(2,2)=Y_6(2,2)\end{array}\right.,$ 可得：

${\mathrm{\&theta;}}_6=arctan\left(\frac{S_6}{C_6}\right)$

然后，针对图3建立协同优化数学模型，确定目标函数，求解最优的机器手从硅胶杯口运动到零件上空涂胶起始点所用时间，其中：

设备不重叠约束中，转台与机器人以及硅胶杯三者之间不重叠，取设备间最小间距为d_x,ij＝d_y,ij＝0.1000m。硅胶杯和转台尺寸分别为(l_i,w_i)＝(0.050,0.050)、(l_j,w_j)＝(1,1)。硅胶杯和转台初始坐标分别为(x_0,i,y_0,i)＝(0,-1.127)、(x_0,j,y_0,j)＝(-1.5951,-0.5416)。

可达性约束中，需要验证机器人可以达到硅胶杯以及所有的涂胶轨迹，为了方便计算，检验涂胶轨迹转化成检验托盘边界点，本案例中转台上的托盘类型有2种，边界点坐标分别为(-1.4837,-0.7316)、(-1.4841,-0.7716)。

工位约束中，预先选定机器人朝向朝下，从而确定转台在XOY系第三象限中移动即横向、纵向坐标均小于0；硅胶杯对中放置，横向坐标恒为0，根据硅胶杯与机器人的相对位置，预判得硅胶杯纵向坐标恒小于0，实现缩小转台与硅胶杯位置搜索空间，提高计算机算法运算效率。

最后，采用基于运动时间最短的轨迹规划方法及基于PSO算法的布局优化进行模型求解，以机器人为中心优化转台与硅胶杯的位置及机器人轨迹，实现尽量缩少机器人运动时间的目标。

此时采用基于运动时间最短的轨迹规划的时候，需要测量硅胶杯口空间坐标以及涂胶起点的坐标，以这两点去反求机器人可能的位姿。由于机器人在两路径点之间的运动可以等效成两种姿态之间的变换，所以在逆解出该两点所有可能位姿之后便可以确定所有可能的运动轨迹方案，亦可以计算相应的运动时间。随着PSO算法不断地生成新的工位布局方案的同时，轨迹规划也在不断地寻找当前布局下最优轨迹。最终获得一种理想的工位布局方案即转台以及硅胶杯的放置位置最佳，同时获得了最优布局下的最佳的轨迹方案。

PSO算法中各参数分别为：种群规模N＝50，迭代上限为200，加速度系数(学习因子)c₁＝1.5，c₂＝1.5，惯性权重w＝1，机器人运动学参数按表2设置。为提高算法效率，初始种群取为原始布局方案。在Matlab中计算结果如表3、表4及图7-11，得到gbest由初始的2.1796s下降到最终的1.4472s。图8中大圆表示机器人运动包络水平投影，小圆表示机器人所在位置。图10、11是基于Matlabroboticstoolbox得到的验算结果，验证了所提算法与运动学模型的正确性，对应位置机器人的各关节角度如表4所示。

表2KR270R2700ultra运动学参数

表3优化前后设备布局情况

表4机器人在运动始末最佳位姿所对应关节值

上述的实施例描述是针对涂胶机器人工位，显然该领域的普通技术人员能够理解和应用本发明。熟悉本领域的技术人员可以将本发明专利中提出的协同优化方法运用到其他类型的机器人工位，例如搬运机器人工位。

Claims (9)

1.一种六自由度机器人工位布局与运动时间协同优化方法，用于控制六自由度机器人的末端执行器从起点设备运动到目标设备，其特征在于，包括以下步骤：

S1：建立以六自由度机器人为原点的空间直角坐标系，同时对六自由度机器人进行运动学建模，得到机器人运动学方程；

S2：根据六自由度机器人、起点设备和目标设备的物理场景建立工位布局与运动时间的协同优化数学模型，所述协同优化数学模型包括目标函数和约束条件，所述目标函数Z满足Z＝mint_z，t_z表示六自由度机器人的末端执行器从起点设备运动到目标设备所用时间，所述时间t_z通过基于运动时间最短的轨迹规划和机器人运动学方程由起点设备和目标设备的坐标获得，所述约束条件由物理场景决定；

S3：在空间直角坐标系内，以时间t_z为PSO算法的适应度，以起点设备和目标设备的工位布局为PSO算法的粒子，基于PSO算法得到协同优化数学模型的最优解以及最优解对应的工位布局。

2.根据权利要求1所述的一种六自由度机器人工位布局与运动时间协同优化方法，其特征在于，所述步骤S1中采用D-H参数法对六自由度机器人进行运动学建模。

3.根据权利要求1所述的一种六自由度机器人工位布局与运动时间协同优化方法，其特征在于，所述约束条件包括设备不重叠约束、可达性约束和工位约束；

所述设备不重叠约束规定起点设备与目标设备不重叠，且起点设备与目标设备之间保持设定的最小间距；

所述可达性约束保证六自由度机器人能到达起点设备、目标设备以及从起点设备到目标设备的整个轨迹；

所述工位约束规定工位布局满足预设的工位设计规则。

4.根据权利要求3所述的一种六自由度机器人工位布局与运动时间协同优化方法，其特征在于，满足设备不重叠约束的公式如下：

A_ij·B_ij＝0

$A_{ij}=max\{(\frac{l_i+l_j}{2}+d_{x,ij})-|x_i-x_j|,0\}$

$B_{ij}=max\{(\frac{w_i+w_j}{2}+d_{y,ij})-|y_i-y_j|,0\}$

式中，下标i表示起点设备，下标j表示目标设备，d_x,ij表示起点设备与目标设备在X轴方向上的最小间距，d_y,ij表示起点设备与目标设备在Y轴方向上的最小间距，l_i表示起点设备在X轴方向上的长度，w_i表示起点设备在Y轴方向上的长度，l_j表示目标设备在X轴方向上的长度，w_j表示目标设备在Y轴方向上的长度，x_i表示起点设备的中心在空间直角坐标系中的X轴坐标，y_i表示起点设备的中心在空间直角坐标系中的Y轴坐标，x_j表示目标设备的中心在空间直角坐标系中的X轴坐标，y_j表示目标设备的中心在空间直角坐标系中的Y轴坐标。

5.根据权利要求1所述的一种六自由度机器人工位布局与运动时间协同优化方法，其特征在于，所述步骤S3具体为：

31：随机生成N个粒子，并初始化个体极值pbest_n、全局极值gbest和最大迭代次数，生成的粒子满足以下公式：

$P_n^k=\{(x_{n,i}^k,y_{n,i}^k),(x_{n,j}^k,y_{n,j}^k)\}$

$V_n^k=\{(v_{n,x,i}^k,v_{n,y,i}^k),(v_{n,x,j}^k,v_{n,y,j}^k)\}$

式中，下标n表示第n个粒子，下标i表示起点设备，下标j表示目标设备，表示第k次迭代中第n个粒子，表示第k次迭代中第n个粒子对应起点设备的X轴坐标，表示第k次迭代中第n个粒子对应起点设备的Y轴坐标，表示第k次迭代中第n个粒子对应目标设备的X轴坐标，表示第k次迭代中第n个粒子对应目标设备的Y轴坐标，表示第k次迭代中第n个粒子的速度，表示第k次迭代中第n个粒子对应起点设备在X轴方向上的移动速度，表示第k次迭代中第n个粒子对应起点设备在Y轴方向上的移动速度，表示第k次迭代中第n个粒子对应目标设备在X轴方向上的移动速度，表示第k次迭代中第n个粒子对应目标设备在Y轴方向上的移动速度；

32：每个粒子以各自对应的六自由度机器人的末端执行器从起点设备运动到目标设备所用时间t_z为自身的适应度，通过迭代，每个粒子跟踪个体极值pbest_n和全局极值gbest来更新自身；

33：输出全局极值gbest对应的粒子为最优工位布局，最优工位布局下的时间t_z为最短运动时间。

6.根据权利要求5所述的一种六自由度机器人工位布局与运动时间协同优化方法，其特征在于，所述步骤32具体为：

321：在第k次迭代中，验证第n个粒子是否满足约束条件，若是，执行步骤322，若否，执行步骤323；

322：获取每个粒子对应六自由度机器人的末端执行器从起点设备运动到目标设备所用时间t_z，令t_z,n＝S_n，t_z,n表示第n个粒子对应的时间t_z，S_n表示第n个粒子的适应度，执行步骤324，

323：直接赋值第n个粒子有极大的适应度，即S_n＝∞，执行步骤324；

324：判断S_n是否小于pbest_n和gbest的数值，若是，则更新pbest_n和gbest的数值，执行步骤325，若否，直接执行步骤325；

325：根据pbest_n和gbest的数值更新第n个粒子的位置与速度；

326：判断迭代次数k是否达到最大迭代数，若是，结束迭代，若否，跳转步骤321。

7.根据权利要求6所述的一种六自由度机器人工位布局与运动时间协同优化方法，其特征在于，所述步骤325中第n个粒子的位置与速度的更新满足以下公式：

$v_{n,x,i}^{k+1}={\mathrm{wv}}_{n,x,i}^k+c_1{r}_1({\mathrm{pbest}}_n-x_{n,i}^k)+c_2{r}_2(gbest-x_{n,i}^k)$

$v_{n,y,i}^{k+1}={\mathrm{wv}}_{n,y,i}^k+c_1{r}_1({\mathrm{pbest}}_n-y_{n,i}^k)+c_2{r}_2(gbest-y_{n,i}^k)$

$v_{n,x,j}^{k+1}={\mathrm{wv}}_{n,x,j}^k+c_1{r}_1({\mathrm{pbest}}_n-x_{n,j}^k)+c_2{r}_2(gbest-x_{n,j}^k)$

$v_{n,y,j}^{k+1}={\mathrm{wv}}_{n,y,j}^k+c_1{r}_1({\mathrm{pbest}}_n-y_{n,j}^k)+c_2{r}_2(gbest-y_{n,j}^k)$

$x_{n,i}^{k+1}=x_{n,i}^k+v_{n,x,i}^{k+1}$

$x_{n,j}^{k+1}=x_{n,j}^k+v_{n,x,j}^{k+1}$

$y_{n,i}^{k+1}=y_{n,i}^k+v_{n,y,i}^{k+1}$

$y_{n,j}^{k+1}=y_{n,j}^k+v_{n,y,j}^{k+1}$

式中，w为惯性权重，c₁、c₂为学习因子，r₁、r₂为介于(0、1)之间的随机数。

8.根据权利要求1所述的一种六自由度机器人工位布局与运动时间协同优化方法，其特征在于，基于运动时间最短的轨迹规划和机器人运动学方程由起点设备和目标设备的坐标获得时间t_z的方法包括以下步骤：

1)以正弦函数拟合六自由度机器人的三角波运动控制模型和梯形波运动控制模型，获取三角波运动控制模型和梯形波运动控制模型相互转换的临界关节运动角度θ_g0，满足以下公式：

${\mathrm{\&theta;}}_{g0}=\frac{{\mathrm{\&pi;\&omega;}}_{g,max}^2}{2{\mathrm{\&alpha;}}_{g,max}}$

式中，α_g,max表示第g个关节的最大加速度，ω_g,max表示第g个关节的最大速度；

2)基于机器人运动学方程，根据起点设备和目标设备获得六自由度机器人第g个关节的起点关节角度θ_g,i和目标关节角度θ_g,j的所有实数解，g＝1,2,...，6，下标i表示起点设备，下标j表示目标设备；

3)根据临界关节运动角度θ_g0、起点关节角度θ_g,i和目标关节角度θ_g,j获得第..个关节在六自由度机器人的末端执行器从起点设备运动到目标设备时最短运动时间t_min,g，满足以下公式：

t_min,g＝min{t_min(θ_g,i,p,θ_g,j,q)}

$t_{\min }({\mathrm{\&theta;}}_{g,i,p},{\mathrm{\&theta;}}_{g,j,q})=\left\{\begin{array}{cc}\sqrt{\frac{2\mathrm{\&pi;}({\mathrm{\&theta;}}_{g,j,q}-{\mathrm{\&theta;}}_{g,j,q})}{{\mathrm{\&alpha;}}_{g,max}}},& ({\mathrm{\&theta;}}_{g,j,q}-{\mathrm{\&theta;}}_{g,j,q})<{\mathrm{\&theta;}}_{g0}\\ \frac{({\mathrm{\&theta;}}_{g,j,q}-{\mathrm{\&theta;}}_{g,j,q})}{{\mathrm{\&omega;}}_{g,\max }}+\frac{{\mathrm{\&pi;\&omega;}}_{g,max}}{2{\mathrm{\&alpha;}}_{g,max}},& ({\mathrm{\&theta;}}_{g,j,q}-{\mathrm{\&theta;}}_{g,j,q})\&GreaterEqual;{\mathrm{\&theta;}}_{g0}\end{array}\right.$

式中，θ_g,i,p表示起点关节角度θ_g,i的第p个实数解，θ_g,j,q表示目标关节角度θ_g,j的第q个实数解，t_min(θ_g,i,p,θ_g,j,q)表示第g个关节从θ_g,i,p运动到θ_g,j,q所用最短时间，α_g,max表示第g个关节的最大加速度，ω_g,max表示第g个关节的最大速度；

4)获得时间t_z，满足以下公式：

t_z＝max(t_min,g),g＝1,2,...，6。

9.根据权利要求1所述的一种六自由度机器人工位布局与运动时间协同优化方法，其特征在于，所述目标设备包括两个相同的目标子设备时，采用双工位设置，两个目标子设备以六自由度机器人到起点设备的直线为对称轴对称设置。