CN102785244A - 一种多轴伺服机械手空间圆弧规划控制方法 - Google Patents

Abstract

一种多轴伺服机械手空间圆弧规划控制方法，包括以下步骤：1）特征点的提取，1.1）对空间阻隔物进行抽象，空间阻隔物设定为一个长方体区域；1.2）求出经过起始点和目标点，且垂直于XOY平面的平面表达式F(x,y,z)＝0；1.3）求平面F(x,y,z)＝0与长方体区域上棱的交点Q₁和Q₂；1.4）对所得交点Q₁和Q₂进行筛选确定最终所需的特征点Q；2）空间三点圆弧计算过程如下：2.1)给出已知的空间三点的坐标表示；2.2)分析问题，提炼出问题涉及到的隐含条件作为求解的约束条件；2.3)将已知条件代入求解约束条件进行整理、合并项并且转换为固定形式；2.4)求解所确定圆弧的圆心坐标值，进一步求出目标圆弧路径的半径。本发明提高工作效率、控制精度高。

Description

一种多轴伺服机械手空间圆弧规划控制方法

技术领域

本发明涉及多轴伺服机械手控制技术，尤其是一种多轴伺服机械手空间圆弧规划控制方法。

背景技术

近些年来，国内很多地区都出现用功短缺和劳动力成本上升的问题，为了缓减类似问题的进一步恶化，工业机械手必将担任起重要角色，对它的研究和应用也将成为今后的趋势，其中多轴机械手更是具备了高灵活度和高效的特点，能灵活完成多道工序，它的应用不仅能节省大量的人力劳动，同时也能提高生产效率，进而提升企业的核心竞争力，使企业立足于不败之地。

为了提高机械手的工作效率，对机械手动作路径进行合理规划就成为控制系统研究中非常重要的一部分。机械手的动作轨迹是衡量机械手性能的一个重要指标，它直接影响到机械手动作过程中的时间最优和能量最优。

发明内容

为了克服已有多轴伺服机械手动作路径规划方法的工作效率较低、控制精度较差的不足，本发明提供一种提高工作效率、控制精度高的多轴伺服机械手空间圆弧规划控制方法。

本发明解决其技术问题所采用的技术方案是：

一种多轴伺服机械手空间圆弧规划控制方法，所述控制方法包括以下步骤：

1）、特征点的提取，具体过程如下：

1.1）对空间阻隔物进行抽象，空间阻隔物设定为一个长方体区域；

1.2）求出经过起始点和目标点，且垂直于XOY平面的平面表达式F(x,y,z)=0；

1.3）求平面F(x,y,z)=0与长方体区域上棱的交点Q₁和Q₂；

1.4）对所得交点Q₁和Q₂进行筛选确定最终所需的特征点Q；

2）空间三点圆弧测量，具体过程如下：

2.1）给出已知的空间三点的坐标表示，即P_s(X_s,Y_s,Z_s),P_m(X_m,Y_m,Z_m),P_e(X_e,Y_e,Z_e)；

2.2）提炼出规划控制问题涉及到的隐含条件作为求解的约束条件，即给定三点P_s，P_m，P_e与圆心O_O共面，且它们到圆心O_O的距离都等于R。用空间向量的形式描述约束条件如式(2)和(3)所示：

$\left|\stackrel{\→}{O_O{P}_s}\right|=\left|\stackrel{\→}{O_O{P}_m}\right|=\left|\stackrel{\→}{O_O{P}_e}\right|=R---\left(2\right)$

$\stackrel{\→}{O_O{P}_s}\×\stackrel{\→}{P_s{P}_m}=\mathrm{\λ}(\stackrel{\→}{P_s{P}_m}\×\stackrel{\→}{P_m{P}_e})---\left(3\right)$

2.3)设X，Y，Z轴的单位向量分别为i，j，k，向量

和

如表达式(4)所示：

$\left\{\begin{array}{c}\stackrel{\→}{O_O{P}_s}=(X_s-X_{O_O},Y_s-Y_{O_O},Z_s-Z_{O_O})\\ \stackrel{\→}{P_s{P}_m}=(X_m-X_s,Y_m-Y_s,Z_m-Z_s)\\ \stackrel{\→}{P_m{P}_e}=(X_e-X_m,Y_e-Y_m,Z_e-Z_m)\end{array}\right.---\left(4\right)$

由表达式(2)可得具体坐标表达式(5)，如下：

$\left\{\begin{array}{c}{(X_s-X_{O_O})}^2+{(Y_s-Y_{O_O})}^2+{(Z_s-Z_{O_O})}^2=R^2\\ {(X_m-X_{O_O})}^2+{(Y_m-Y_{O_O})}^2+{(Z_m-Z_{O_O})}^2=R^2\\ {(X_e-X_{O_O})}^2+{(Y_e-Y_{O_O})}^2+{(Z_e-Z_{O_O})}^2=R^2\end{array}\right.---\left(5\right)$

根据向量叉乘定义可以得到式(6)和式(7)，具体如下：

$\stackrel{\→}{O_O{P}_s}\×\stackrel{\→}{P_s{P}_m}=\left|\begin{array}{ccc}i& j& k\\ X_s-X_{O_O}& Y_s-Y_{O_O}& Z_s-Z_{O_O}\\ X_m-X_s& Y_m-Y_s& Z_m-Z_s\end{array}\right|---\left(6\right)$

$\stackrel{\→}{P_s{P}_m}\×\stackrel{\→}{P_m{P}_e}=\left|\begin{array}{ccc}i& j& k\\ X_m-X_s& Y_m-Y_s& Z_m-Z_s\\ X_e-X_m& Y_e-Y_m& Z_e-Z_m\end{array}\right|---\left(7\right)$

2.4）将式(6)和式(7)代入式(3)并联合式(5)，转化为关于

和

的表达式(8)和(9)。

根据约束条件和叉乘计算公式可将等式转化为：

$A{\left[\begin{array}{ccc}{X}_{O_O}& Y_{O_O}& Z_{O_O}\end{array}\right]}^T=B---\left(8\right)$

$R=\sqrt{{(X_s-X_{O_O})}^2+{(Y_s-Y_{O_O})}^2+{(Z_s-Z_{O_O})}^2}---\left(9\right)$

然后，将由式(8)所求出来的圆心

的坐标代入式(9)求得所确定圆弧的半径R。

本发明的技术构思为：参照图1，将特征点提取的过程总结为如下四个步骤：

1)对空间阻隔物进行抽象，即表示为如图2中所示的一个长方体区域；

2)求出经过起始点和目标点，且垂直于XOY平面的平面表达式F(x,y,z)=0；

3)求平面F(x,y,z)=0与长方体上棱的交点Q₁和Q₂；

4)对所得交点Q₁和Q₂进行筛选确定最终所需的特征点Q；

空间三点圆弧计算过程如下：

1)给出已知的空间三点的坐标表示；

2)分析问题，提炼出问题涉及到的隐含条件作为求解的约束条件；

3)将已知条件代入求解约束条件进行整理、合并项并且转换为固定形式；

4)求解所确定圆弧的圆心坐标值，进一步求出目标圆弧路径的半径。

本发明的有益效果主要表现在：提高工作效率、控制精度高。

附图说明

图1为三点圆弧算法应用示意图；

图2为特征点求值示意图；

图3为特征点求值算法流程图；

图4为三点圆弧矢量图；

图5为空间三点求解圆弧算法流程图。

具体实施方式

下面结合附图对本发明作进一步描述。

参照图1～图5，一种多轴伺服机械手空间圆弧规划控制方法，所述控制方法包括以下步骤：

1）、特征点的提取，具体过程如下：

1.1）对空间阻隔物进行抽象，空间阻隔物设定为一个长方体区域；

1.2）求出经过起始点和目标点，且垂直于XOY平面的平面表达式F(x,y,z)=0；

1.3）求平面F(x,y,z)=0与长方体区域上棱的交点Q₁和Q₂；

1.4）对所得交点Q₁和Q₂进行筛选确定最终所需的特征点Q；

2）空间三点圆弧计算过程如下：

2.1)给出已知的空间三点的坐标表示；

2.2)分析问题，提炼出问题涉及到的隐含条件作为求解的约束条件；

2.3)将已知条件代入求解约束条件进行整理、合并项并且转换为固定形式；

2.4)求解所确定圆弧的圆心坐标值，进一步求出目标圆弧路径的半径。

本发明所研究的机械手属于横轴式机械手，一般应用在流水线生产作业过程中用来夹取成品、贴标以及对产品的有序摆放装箱等工序，这常常涉及到一些跨越某一阻隔而对所夹取的物品进行有序摆放，就是在这样的工作背景之下，本文提出了适用于横轴式机械手的三点圆弧算法，算法的提出和实现是的机械手动作路径简单、连贯而且非常平滑。图1就是一张从起始点跨越阻隔对物品进行有序摆放的示意图。为了确保算法有意义，这里的阻隔物宽度要远小于搬运距离且其高度也不能超过机械手运动的上限。

特征点求解：如图2所示，P_S为起始点，P_E为终点，O_O为圆弧的圆心。长方形边框是阻隔物抽象出来的空间长方体的切面，Q₁，Q₂，Q₁'，Q₂'，是过起始点P_S和终点P_E且垂直于平面XOY的空间平面与上述长方体上棱的交点，d表示Q₂点与圆心O_O之间的空间距离。

首先，我们对整个阻隔区域进行边界划定，将整个阻隔物划定在一个长方

$\left\{\begin{array}{c}{C}_0\≤x\≤C_1\\ C_2\≤y\≤C_3\\ C_4\≤z\≤C_5\end{array}\right.---\left(1\right)$

体区域内，这个区域可以用表达式(1)表示：

其中C_i(i=0,1...5)表示某常数。然后，我们求出经过起始点P_S和终点P_E且垂直于平面XOY的空间平面的表达式F(x,y,z)=0，同时求出该平面与限定空间的长方体上棱的交点Q₁和Q₂，接下来我们需要对所得到的特征点进行筛选，先假设Q₁点就是我们所要取得的特征点，那么我们利用P_S，Q₁和P_E三点可以确定一条目标空间圆弧（详细三点确定圆弧算法将在后面做详细说明），同时可以求得圆心O_O的坐标以及圆弧的半径R，最后通过计算O_O和Q₂之间的距离d并和R进行比较，我们就可以确定所需的特征点，如果d<R那么Q₁就是所求得特征点，如果d≥R那么Q₂是所求得的特征点，该类情况如图2中的虚线框所示。

空间三点圆弧的实现：根据图1中的实际问题，其中所涉及到的圆弧是通过已知的空间三点确定目标圆弧路径，这一节中我们就在空间三点已知的情况下对问题进行进一步的求解，对对应的英文下标说明如下：(S=start，m=middle,e=end)，如图4所示。

已知：P_s(X_s,Y_s,Z_s),P_m(X_m,Y_m,Z_m),P_e(X_e,Y_e,Z_e)，求空间圆弧半径R以及圆心坐标 $O_O(X_{O_O},Y_{O_O},Z_{O_O}).$

在问题求解之前，首先要挖掘出问题中所隐含的约束条件，即给定三点P_s，P_m，P_e与圆心O_O共面，且它们到圆心O_O的距离都等于R。用空间向量的形式描述约束条件如式(2)和(3)所示：

$\left|\stackrel{\&RightArrow;}{O_O{P}_s}\right|=\left|\stackrel{\&RightArrow;}{O_O{P}_m}\right|=\left|\stackrel{\&RightArrow;}{O_O{P}_e}\right|=R---\left(2\right)$

$\stackrel{\&RightArrow;}{O_O{P}_s}\&times;\stackrel{\&RightArrow;}{P_s{P}_m}=\mathrm{\&lambda;}(\stackrel{\&RightArrow;}{P_s{P}_m}\&times;\stackrel{\&RightArrow;}{P_m{P}_e})---\left(3\right)$

设X，Y，Z轴的单位向量分别为i，j，k，向量

和

如表达式(4)所示：

$\left\{\begin{array}{c}\stackrel{\&RightArrow;}{O_O{P}_s}=(X_s-X_{O_O},Y_s-Y_{O_O},Z_s-Z_{O_O})\\ \stackrel{\&RightArrow;}{P_s{P}_m}=(X_m-X_s,Y_m-Y_s,Z_m-Z_s)\\ \stackrel{\&RightArrow;}{P_m{P}_e}=(X_e-X_m,Y_e-Y_m,Z_e-Z_m)\end{array}\right.---\left(4\right)$

由表达式(2)可得具体坐标表达式(5)，如下：

$\left\{\begin{array}{c}{(X_s-X_{O_O})}^2+{(Y_s-Y_{O_O})}^2+{(Z_s-Z_{O_O})}^2=R^2\\ {(X_m-X_{O_O})}^2+{(Y_m-Y_{O_O})}^2+{(Z_m-Z_{O_O})}^2=R^2\\ {(X_e-X_{O_O})}^2+{(Y_e-Y_{O_O})}^2+{(Z_e-Z_{O_O})}^2=R^2\end{array}\right.---\left(5\right)$

根据向量叉乘定义可以得到式(6)和式(7)，具体如下：

$\stackrel{\&RightArrow;}{O_O{P}_s}\&times;\stackrel{\&RightArrow;}{P_s{P}_m}=\left|\begin{array}{ccc}i& j& k\\ X_s-X_{O_O}& Y_s-Y_{O_O}& Z_s-Z_{O_O}\\ X_m-X_s& Y_m-Y_s& Z_m-Z_s\end{array}\right|---\left(6\right)$

$\stackrel{\&RightArrow;}{P_s{P}_m}\&times;\stackrel{\&RightArrow;}{P_m{P}_e}=\left|\begin{array}{ccc}i& j& k\\ X_m-X_s& Y_m-Y_s& Z_m-Z_s\\ X_e-X_m& Y_e-Y_m& Z_e-Z_m\end{array}\right|---\left(7\right)$

将式(6)和式(7)代入式(3)并联合式(5)，可以转化为关于

和

的表达式(8)和(9)。

根据约束条件和叉乘计算公式可将等式转化为：

$A{\left[\begin{array}{ccc}{X}_{O_O}& Y_{O_O}& Z_{O_O}\end{array}\right]}^T=B---\left(8\right)$

$R=\sqrt{{(X_s-X_{O_O})}^2+{(Y_s-Y_{O_O})}^2+{(Z_s-Z_{O_O})}^2}---\left(9\right)$

然后，将由式(8)所求出来的圆心

的坐标代入式(9)就可以求得所确定圆弧的半径R。

在式(8)中，A是一个3×3的方阵，具体表达式如下：

其中，a₀₀=2(X_s-X_m)，a₀₁=2(Y_s-Y_m)，a₀₂=2(Z_s-Z_m)，a₁₀=2(X_s-X_m)，a₁₁=2(Y_s-Y_m)，a₁₂=2(Z_s-Z_m)

B=(b₀ b₁ b₂)^T是一个3×1的矩阵：

b₀=(X_s ²+Y_s ²+Z_s ²)-(X_m ²+Y_m ²+Z_m ²)，

b₁=(X_m ²+Y_m ²+Z_m ²)-(X_e ²+Y_e ²+Z_e ²)，

b₂=a₂₀X_s+a₂₁Y_s+a₂₂Z_s。

以上阐述的是本发明给出的是整个算法的推导和实现过程。需要指出，本发明不只限于横轴式机械手，本发明是针对工业控制中多轴系统的空间圆弧实现方法，所以适用于多轴数控系统以及多轴机械手系统。

Claims (1)

1.一种多轴伺服机械手空间圆弧规划控制方法，其特征在于：所述控制方法包括以下步骤：

1）、特征点的提取，具体过程如下：

1.1）对空间阻隔物进行抽象，空间阻隔物设定为一个长方体区域；

1.2）求出经过起始点和目标点，且垂直于XOY平面的平面表达式F(x,y,z)=0；

1.3）求平面F(x,y,z)=0与长方体区域上棱的交点Q₁和Q₂；

1.4）对所得交点Q₁和Q₂进行筛选确定最终所需的特征点Q；

2）空间三点圆弧测量，具体过程如下：

2.1）给出已知的空间三点的坐标表示，即P_s(X_s,Y_s,Z_s),P_m(X_m,Y_m,Z_m),P_e(X_e,Y_e,Z_e)；

2.2）提炼出规划控制问题涉及到的隐含条件作为求解的约束条件，即给定三点P_s，P_m，P_e与圆心O_O共面，且它们到圆心O_O的距离都等于R，用空间向量的形式描述约束条件如式(2)和(3)所示：

$\left|\stackrel{\&RightArrow;}{O_O{P}_s}\right|=\left|\stackrel{\&RightArrow;}{O_O{P}_m}\right|=\left|\stackrel{\&RightArrow;}{O_O{P}_e}\right|=R---\left(2\right)$

$\stackrel{\&RightArrow;}{O_O{P}_s}\&times;\stackrel{\&RightArrow;}{P_s{P}_m}=\mathrm{\&lambda;}(\stackrel{\&RightArrow;}{P_s{P}_m}\&times;\stackrel{\&RightArrow;}{P_m{P}_e})---\left(3\right)$

2.3)设X，Y，Z轴的单位向量分别为i，j，k，向量

和

如表达式(4)所示：

$\left\{\begin{array}{c}\stackrel{\&RightArrow;}{O_O{P}_s}=(X_s-X_{O_O},Y_s-Y_{O_O},Z_s-Z_{O_O})\\ \stackrel{\&RightArrow;}{P_s{P}_m}=(X_m-X_s,Y_m-Y_s,Z_m-Z_s)\\ \stackrel{\&RightArrow;}{P_m{P}_e}=(X_e-X_m,Y_e-Y_m,Z_e-Z_m)\end{array}\right.---\left(4\right)$

由表达式(2)可得具体坐标表达式(5)，如下：

$\left\{\begin{array}{c}{(X_s-X_{O_O})}^2+{(Y_s-Y_{O_O})}^2+{(Z_s-Z_{O_O})}^2=R^2\\ {(X_m-X_{O_O})}^2+{(Y_m-Y_{O_O})}^2+{(Z_m-Z_{O_O})}^2=R^2\\ {(X_e-X_{O_O})}^2+{(Y_e-Y_{O_O})}^2+{(Z_e-Z_{O_O})}^2=R^2\end{array}\right.---\left(5\right)$

根据向量叉乘定义可以得到式(6)和式(7)，具体如下：

$\stackrel{\&RightArrow;}{O_O{P}_s}\&times;\stackrel{\&RightArrow;}{P_s{P}_m}=\left|\begin{array}{ccc}i& j& k\\ X_s-X_{O_O}& Y_s-Y_{O_O}& Z_s-Z_{O_O}\\ X_m-X_s& Y_m-Y_s& Z_m-Z_s\end{array}\right|---\left(6\right)$

$\stackrel{\&RightArrow;}{P_s{P}_m}\&times;\stackrel{\&RightArrow;}{P_m{P}_e}=\left|\begin{array}{ccc}i& j& k\\ X_m-X_s& Y_m-Y_s& Z_m-Z_s\\ X_e-X_m& Y_e-Y_m& Z_e-Z_m\end{array}\right|---\left(7\right)$

2.4）将式(6)和式(7)代入式(3)并联合式(5)，转化为关于和

的表达式(8)和(9)；

根据约束条件和叉乘计算公式可将等式转化为：

$A{\left[\begin{array}{ccc}{X}_{O_O}& Y_{O_O}& Z_{O_O}\end{array}\right]}^T=B---\left(8\right)$

$R=\sqrt{{(X_s-X_{O_O})}^2+{(Y_s-Y_{O_O})}^2+{(Z_s-Z_{O_O})}^2}---\left(9\right)$

然后，将由式(8)所求出来的圆心

的坐标代入式(9)求得所确定圆弧的半径R。

Images