CN104793654A - 一种操作速度获取方法及装置 - Google Patents

Abstract

本发明的目的在于提供一种操作速度获取方法及装置，在预定的工件移动空间内建立坐标系，在所述坐标系内定义所述工件各边缘点且将其中至少一边缘点作为对齐点，并在工件定义第一操作点；在所述工件移动空间内定义外部控制点，所述外部控制点供所述对齐点移动至与其重合；所述操作装置在所述第一操作点操作所述工件移动，所述移动包括：工件平移至所述对齐点和外部控制点重合；或者，在所述对齐点和外部控制点重合的情况下，工件绕所述外部控制点进行转动或再平移；进行工件操作过程中的坐标移动量的计算以得到工件的移动距离，并根据所述移动距离及操作过程的时间得到工件相对外部控制点的移动速度；本发明简单易行。

Description

一种操作速度获取方法及装置

技术领域

本发明涉及机器人控制技术领域，特别是涉及一种操作速度获取方法及装置。

背景技术

在现有机器人应用的工业中，例如制衣，均应用了大量的机器人来代替人工，故机器人的控制如何达到准确高效成为一直以来厂商所追求的目标，在机器人控制中，机器人操作工件时工件相对于参考点的移动速度是一个重要的参考指标，但目前并没有比较好的速度获取方式。

发明内容

鉴于以上所述现有技术的缺点，本发明的目的在于提供一种操作速度获取方法及装置，解决现有技术中的各种缺陷。

为实现上述目标及其他相关目标，本发明提供一种操作速度获取方法，包括：在预定的工件移动空间内建立坐标系，在所述坐标系内定义所述工件各边缘点且将其中至少一边缘点作为对齐点，并在工件定义第一操作点；在所述工件移动空间内定义外部控制点，所述外部控制点供所述对齐点移动至与其重合；所述操作装置在所述第一操作点操作所述工件移动，所述移动包括：工件平移至所述对齐点和外部控制点重合；或者，在所述对齐点和外部控制点重合的情况下，工件绕所述外部控制点进行转动；根据平移前的对齐点坐标及外部控制点坐标获得平移矢量，在所述第一操作点通过所述平移到达第二操作点时，根据平移矢量及第一操作点坐标计算平移后的第二操作点坐标，据以计算所述第一操作点平移至第二操作点的第一坐标移动量；在所述第二操作点通过所述转动到达第三操作点时，结合转动角度计算所述第三操作点的坐标，据以计算第二操作点转动至第三操作点的第二坐标移动量；结合所述第一坐标移动量及第二坐标移动量以计算出总移动量；根据所述总移动量计算工件的移动距离；结合所述第一操作点移动至第三操作点的时间、及所计算的工件的移动距离，计算工件相对外部控制点的移动速度。

可选的，所述的操作速度获取方法，还包括：获取工件的各所述边缘点进行所述移动后的坐标。

可选的，所述的操作速度获取方法，所述外部控制点位于预设的法线上，所述工件上每两个边缘点构成一线段；每次的所述转动是将所述工件转动至令其边缘的一所述线段同所述法线重合，所述工件的转动角度是通过计算所述线段转动至同法线重合的转动角度而获得的。

可选的，所述转动角度等于：在第二操作点通过所述转动到达第三操作点时，第二操作点和外部控制点连线同第三操作点和外部控制点连线间的夹角。

可选的，所述坐标系包括：X轴、Y轴、Z轴及U轴，其中，所述U轴是转动轴，以角度为单位。

为实现上述目标及其他相关目标，本发明提供一种操作速度获取装置，包括：坐标定义单元及工件移动速度获取单元；所述坐标定义单元，用于在预定的工件移动空间内建立坐标系，在所述坐标系内定义所述工件各边缘点且将其中至少一边缘点作为对齐点，并在工件定义第一操作点；在所述工件移动空间内定义外部控制点，所述外部控制点供所述对齐点移动至与其重合；所述操作装置在所述第一操作点操作所述工件移动，所述移动包括：工件平移至所述对齐点和外部控制点重合；或者，在所述对齐点和外部控制点重合的情况下，工件绕所述外部控制点进行转动；所述工件移动速度获取单元，用于根据平移前的对齐点坐标及外部控制点坐标获得平移矢量，在所述第一操作点通过所述平移到达第二操作点时，根据平移矢量及第一操作点坐标计算平移后的第二操作点坐标，据以计算所述第一操作点平移至第二操作点的第一坐标移动量；在所述第二操作点通过所述转动到达第三操作点时，结合转动角度计算所述第三操作点的坐标，据以计算第二操作点转动至第三操作点的第二坐标移动量；结合所述第一坐标移动量及第二坐标移动量以计算出总移动量；根据所述总移动量计算工件的移动距离；结合所述第一操作点移动至第三操作点的时间、及所计算的工件的移动距离，计算工件相对外部控制点的移动速度。。

可选的，所述坐标转化单元，还用于获取工件的各所述边缘点进行所述移动后的坐标。

可选的，所述外部控制点位于预设的法线上，所述工件上每两个边缘点构成一线段；每次的所述转动是将所述工件转动至令其边缘的一所述线段同所述法线重合，所述工件的转动角度是通过计算所述线段转动至同法线重合的转动角度而获得的。

可选的，所述转动角度等于：在第二操作点通过所述转动到达第三操作点时，第二操作点和外部控制点连线同第三操作点和外部控制点连线间的夹角。

可选的，所述坐标系包括：X轴、Y轴、Z轴及U轴，其中，所述U轴是转动轴，以角度为单位。

如上所述，本发明的目的在于提供一种操作速度获取方法及装置，在预定的工件移动空间内建立坐标系，在所述坐标系内定义所述工件各边缘点且将其中至少一边缘点作为对齐点，并在工件定义第一操作点；在所述工件移动空间内定义外部控制点，所述外部控制点供所述对齐点移动至与其重合；所述操作装置在所述第一操作点操作所述工件移动，所述移动包括：工件平移至所述对齐点和外部控制点重合；或者，在所述对齐点和外部控制点重合的情况下，工件绕所述外部控制点进行转动或再平移；进行工件操作过程中的坐标移动量的计算以得到工件的移动距离，并根据所述移动距离及操作过程的时间得到工件相对外部控制点的移动速度；本发明简单易行。

附图说明

图1显示为本发明一实施例中操作速度获取方法的流程示意图。

图2a显示为本发明一实施例中平移操作的计算原理示意图。

图2b显示为本发明一实施例中转动操作的计算原理示意图。

图3显示为本发明一实施例中操作速度获取装置的结构示意图。

元件标号说明

1           操作速度获取装置

11          坐标定义单元

12          工件移动速度获取单元

S1～S6      方法步骤

具体实施方式

以下通过特定的具体实例说明本发明的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本发明的其他优点与功效。本发明还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本发明的精神下进行各种修饰或改变。需说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。

如图1所示，本发明提供一种操作速度获取方法，应用在例如缝纫作业中，所述工件例如为缝纫布料，所述操作装置例如为机械臂等。

所述方法包括：

步骤S1：在预定的工件移动空间内建立坐标系，在所述坐标系内定义所述工件各边缘点且将其中至少一边缘点作为对齐点，并在工件定义第一操作点；在所述工件移动空间内定义外部控制点。

在一实施例中，所述空间坐标系例如为四轴(如X横轴、Y纵轴、Z上下轴、U转动轴)的坐标系，操作装置所在的轴为U轴，实际上，空间坐标系可以只有三个轴，这个“U轴”是另外定义出来以角度为刻度的假想轴，用以提供角度参数。

在一实施例中，所述操作装置在所述第一操作点操作所述工件移动，所述移动包括：工件平移至所述对齐点和外部控制点重合；或者，在所述对齐点和外部控制点重合的情况下，工件绕所述外部控制点进行转动，当然在其他实施例中，若有其他需求还可再平移。

步骤S2：根据平移前的对齐点坐标及外部控制点坐标获得平移矢量，在所述第一操作点通过所述平移到达第二操作点时，根据平移矢量及第一操作点坐标计算平移后的第二操作点坐标，据以计算所述第一操作点平移至第二操作点的第一坐标移动量。

步骤S3：在所述第二操作点通过所述转动到达第三操作点时，结合转动角度计算所述第三操作点的坐标，据以计算第二操作点转动至第三操作点的第二坐标移动量。

步骤S4：结合所述第一坐标移动量及第二坐标移动量以计算出总移动量。

步骤S5：根据所述总移动量计算工件的移动距离。

步骤S6：结合所述第一操作点移动至第三操作点的时间、及所计算的工件的移动距离，计算工件相对外部控制点的移动速度。

通过结合所述平移操作轨迹及转动即可获得操作装置在操作工件平移及转动等运动过程中的移动轨迹。

在一实施例中，所述方法还包括：获取工件的各所述边缘点进行所述移动后的坐标在缝纫作业中，每次所述转动均是使得工件边缘的一段和预设法线重合以进行下一步缝纫作业，具体来说，所述外部控制点位于预设的法线上，所述工件上每两个边缘点构成一线段；每次的所述转动是将所述工件转动至令其边缘的一所述线段同所述法线重合，所述工件的转动角度是通过计算所述线段转动至同法线重合的转动角度而获得的，进一步的，所述转动角度等于：在第二操作点通过所述转动到达第三操作点时，第二操作点和外部控制点连线同第三操作点和外部控制点连线间的夹角。

以下将以一个详细实施例来说明上述原理，如图2a及2b所示；举例来说，所述坐标系涉及例如四轴X横轴、Y纵轴、Z上下轴、U转动轴；操作装置在CO1点抓取工件，虽然在该实施例中Z轴并未例示，但在本发明的教示下，结合空间几何及平面几何等现有技术应当可以毫无疑义地实现。

如图所示，设操作装置在X、Y直角坐标系内移动，U为操作装置的转动轴(逆时针为正方向)，工件为任意一闭合图形由例如P0、P1、P2、P3、P4、P5、P6、P7组成，定义P0为起始点，第一操作点CO坐标为(CO_0X，CO_0Y，CO_0U)，外部控制点A坐标为(A_X，A_Y)，法线为L。根据操作装置操作工件的方式不同进行不同的坐标转换并相结合：

方式1：平移

如图2a所示，操作装置控制工件将起始点P0按箭头所示平移至外部控制点A重合；未平移前设P0坐标为(P0_0X，P0_0Y)，可得平移向量为平移后操作装置的操作点CO1坐标(CO_1X，CO_1Y，CO_1U)的计算公式为：

[CO_1X，CO_1Y，CO_1U]＝[A_X-P0_0X，A_Y-P0_0Y，O]+[CO_0X，CO_0Y，CO_0U]

并可对应计算工件各边缘点P0-P7平移后坐标计算公式为：

[Pi_1X，Pi_1Y]＝[A_X-P0_0X，A_Y-P0_0Y]+[Pi_0X，Pi_0Y]，i为1以上的整数，本实施例中，i<8。

方式2：转动

如图2b所示，保持P0与外部控制点A重合的情况下，操作装置控制工件转动至使P0-P1所成线段与法线重合，也就是说，在缝纫作业中，每次转动之前必要的是保持P0点和外部控制点重合，那么在整个操作过程，是不断地平移重合后再转动，若两点有偏差了，则再平移重合后再转动的过程。

在一实施例中，操作装置转动前的操作点坐标为CO1(CO_1X，CO_1Y，CO_1U)，工件边缘点P0、P1的坐标为(P0_1X，P0_1Y)，(P1_1X，P1_1Y)，转动后的操作点坐标为CO2(CO_2X，CO_2Y，CO_2U)；P0连接P1所成线段与法线的夹角为θ；

第二操作点CO2的坐标计算即：

$[{\mathrm{CO}}_{2X},{\mathrm{CO}}_{2Y}]=[{\mathrm{CO}}_{1X}-A_x,{\mathrm{CO}}_{1Y}-A_Y]\&CenterDot;\left[\begin{array}{cc}\cos \left(\mathrm{\&theta;}\right)& -\sin \left(\mathrm{\&theta;}\right)\\ \sin \left(\mathrm{\&theta;}\right)& \cos \left(\mathrm{\&theta;}\right)\end{array}\right]+[A_X,A_Y];$

CO_2U＝θ+CO_1U。

工件边缘点P0-P7转动θ后的坐标：

$[{\mathrm{Pi}}_{1X},{\mathrm{Pi}}_{1Y}]=[A_X-{\mathrm{Pi}}_{1X},A_Y-{\mathrm{Pi}}_{1Y}]\&CenterDot;\left[\begin{array}{cc}\cos \mathrm{\&theta;}& -\sin \mathrm{\&theta;}\\ \sin \mathrm{\&theta;}& \cos \mathrm{\&theta;}\end{array}\right]+\left[A_X{A}_Y\right]$

结合上述平移及转动后的坐标计算，来继续说明本发明的速度计算：

具体来说，当操作装置动作时，设在Δt时间内，读取到操作装置的当前位置点为Di(i为1以上的整数)，其在X、Y、U轴的坐标值为[D_iX，D_iY，D_iU]，操作装置控制工件相对外部控制点的移动距离ΔLi。

设当前位置点Di与前一位置点Di-1的U轴角度差为θ，则：

θ＝D_iU-D_(i-1)U

在一实施例中，移动距离ΔLi由X、Y轴平移和U轴转动两个因素引起。

平移的坐标移动量：

[ΔD_X1,ΔD_Y1]＝[D_iX-D_(i-1)X，D_iY-D_(i-1)Y]

转动的坐标移动量：

$[{\mathrm{\&Delta;D}}_{X2},{\mathrm{\&Delta;D}}_{Y2}]=[A_X-D_{(i-1)X},A_Y-D_{(i-1)Y}]\&CenterDot;\left[\begin{array}{cc}\cos \mathrm{\&theta;}& -\sin \mathrm{\&theta;}\\ \sin \mathrm{\&theta;}& \cos \mathrm{\&theta;}\end{array}\right]+[A_X,A_Y]$

结合平移和转动的坐标移动量:

[ΔD_X,ΔD_Y]＝[ΔD_X1,ΔD_Y1]+[ΔD_X2,ΔD_Y2]

可得移动距离ΔLi：

$\mathrm{\&Delta;Li}=\sqrt{\mathrm{\&Delta;}{D_X}^2+\mathrm{\&Delta;}{D_Y}^2}$

因此工件相对外部控制点速度:

Vi＝ΔLi/Δt

通过跟踪检测操作装置运动轨迹，计算操作装置控制工件相对外部控制点的移动速度，此速度的计算对操作装置以外的外部设备的控制具有重要意义。

在其他实施例中，还可获取操作装置操作时的移动轨迹的重要参考数据，平移操作的话是直线，通过确定起点CO1及终点CO2即可确定；

转动的话，操作装置的移动轨迹是由多条上述平移操作的轨迹及转动操作的轨迹所构成；优选但非必须的是，采用插分方式进行精确转动轨迹的计算，记插分精度为Δθ，插值点数目N₁＝θ/Δθ，举例来说，若夹角为90度，插值精度为1度的话，则插值点数目为90个；CO1和CO2之间的各个插值点分别为CO₂₁.....CO_2n....CO_2N1；n，N1皆为整数。

设得到第1个插值点的转动角度为Δθ，得到向量计算CO₂₁的X轴和Y轴坐标：

$[{\mathrm{CO}}_{21X},{\mathrm{CO}}_{21Y}]=[{\mathrm{CO}}_{1X}-A_X,{\mathrm{CO}}_{1Y}-A_Y]\&CenterDot;\left[\begin{array}{cc}\cos \mathrm{\&Delta;\&theta;}& -\sin \mathrm{\&Delta;\&theta;}\\ \sin \mathrm{\&Delta;\&theta;}& \cos \mathrm{\&Delta;\&theta;}\end{array}\right]+[A_X,A_Y]$

转动时的第1个插值点即CO₂₁的U轴坐标：

CO_21U＝Δθ+CO_1U

同理，第n个插值点的转动角度为n*Δθ，得到向量CO_2n的X轴和Y轴坐标：

$[{\mathrm{CO}}_{2\mathrm{nX}},{\mathrm{CO}}_{2\mathrm{nY}}]=[{\mathrm{CO}}_{1X}-A_X,{\mathrm{CO}}_{1Y}-A_Y]\&CenterDot;\left[\begin{array}{cc}\cos (n*\mathrm{\&Delta;\&theta;})& -\sin (n*\mathrm{\&Delta;\&theta;})\\ \sin (n*\mathrm{\&Delta;\&theta;})& \cos (n*\mathrm{\&Delta;\&theta;})\end{array}\right]+\left[A_X{A}_Y\right]$

第n个点CO′_2n的U轴坐标：

CO_2n＝n*Δθ+CO_1U

得到插值点数据(即操作装置运动轨迹)如表1所示：

表1 转动插值点坐标

之后，重复上述平移、转动计算，最终得到操作装置的移动轨迹中的各个坐标点数据，并且亦可获得工件上各个边缘点的移动轨迹中的各个坐标点数据。

如图3所示，本发明还提供一种操作速度获取装置1，用于获取操作装置操作工件时的移动轨迹，其技术原理同上述实施例大致相同，因此在不冲突的情况下，上述实施例中的技术特征均可应用于以下实施例中，故不再作重复赘述。

所述操作速度获取装置，包括：坐标定义单元11及工件移动速度获取单元12；所述坐标定义单元11，用于在预定的工件移动空间内建立坐标系，在所述坐标系内定义所述工件各边缘点且将其中至少一边缘点作为对齐点，并在工件定义第一操作点；在所述工件移动空间内定义外部控制点，所述外部控制点供所述对齐点移动至与其重合；所述操作装置在所述第一操作点操作所述工件移动，所述移动包括：工件平移至所述对齐点和外部控制点重合；或者，在所述对齐点和外部控制点重合的情况下，工件绕所述外部控制点进行转动；所述工件移动速度获取单元12，用于根据平移前的对齐点坐标及外部控制点坐标获得平移矢量，在所述第一操作点通过所述平移到达第二操作点时，根据平移矢量及第一操作点坐标计算平移后的第二操作点坐标，据以计算所述第一操作点平移至第二操作点的第一坐标移动量；在所述第二操作点通过所述转动到达第三操作点时，结合转动角度计算所述第三操作点的坐标，据以计算第二操作点转动至第三操作点的第二坐标移动量；结合所述第一坐标移动量及第二坐标移动量以计算出总移动量；根据所述总移动量计算工件的移动距离；结合所述第一操作点移动至第三操作点的时间、及所计算的工件的移动距离，计算工件相对外部控制点的移动速度。

在一实施例中，所述坐标转化单元，还用于获取工件的各所述边缘点进行所述移动后的坐标。

在一实施例中，所述外部控制点位于预设的法线上，所述工件上每两个边缘点构成一线段；每次的所述转动是将所述工件转动至令其边缘的一所述线段同所述法线重合，所述工件的转动角度是通过计算所述线段转动至同法线重合的转动角度而获得的。

在一实施例中，所述转动角度等于：在第二操作点通过所述转动到达第三操作点时，第二操作点和外部控制点连线同第三操作点和外部控制点连线间的夹角。

在一实施例中，所述坐标系包括：X轴、Y轴、Z轴及U轴，其中，所述U轴是转动轴，以角度为单位。

综上所述，本发明的目的在于提供一种操作速度获取方法及装置，在预定的工件移动空间内建立坐标系，在所述坐标系内定义所述工件各边缘点且将其中至少一边缘点作为对齐点，并在工件定义第一操作点；在所述工件移动空间内定义外部控制点，所述外部控制点供所述对齐点移动至与其重合；所述操作装置在所述第一操作点操作所述工件移动，所述移动包括：工件平移至所述对齐点和外部控制点重合；或者，在所述对齐点和外部控制点重合的情况下，工件绕所述外部控制点进行转动或再平移；进行工件操作过程中的坐标移动量的计算以得到工件的移动距离，并根据所述移动距离及操作过程的时间得到工件相对外部控制点的移动速度；本发明简单易行。

上述实施例仅例示性说明本发明的原理及其功效，而非用于限制本发明。任何熟悉此技术的人士皆可在不违背本发明的精神及范畴下，对上述实施例进行修饰或改变。因此，举凡所属技术领域中具有通常知识者在未脱离本发明所揭示的精神与技术思想下所完成的一切等效修饰或改变，仍应由本发明的权利要求所涵盖。

Claims (10)

1.一种操作速度获取方法，其特征在于，所述方法包括：

在预定的工件移动空间内建立坐标系，在所述坐标系内定义所述工件各边缘点且将其中至少一边缘点作为对齐点，并在工件定义第一操作点；

在所述工件移动空间内定义外部控制点；

所述操作装置在所述第一操作点操作所述工件移动，所述移动包括：工件平移至所述对齐点和外部控制点重合；或者，在所述对齐点和外部控制点重合的情况下，工件绕所述外部控制点进行转动；

根据平移前的对齐点坐标及外部控制点坐标获得平移矢量，在所述第一操作点通过所述平移到达第二操作点时，根据平移矢量及第一操作点坐标计算平移后的第二操作点坐标，据以计算所述第一操作点平移至第二操作点的第一坐标移动量；

在所述第二操作点通过所述转动到达第三操作点时，结合转动角度计算所述第三操作点的坐标，据以计算第二操作点转动至第三操作点的第二坐标移动量；

结合所述第一坐标移动量及第二坐标移动量以计算出总移动量；

根据所述总移动量计算工件的移动距离；

结合所述第一操作点移动至第三操作点的时间、及所计算的工件的移动距离，计算工件相对外部控制点的移动速度。

2.根据权利要求1所述的操作速度获取方法，其特征在于，还包括：获取工件的各所述边缘点进行所述移动后的坐标。

3.根据权利要求2所述的操作速度获取方法，其特征在于，所述外部控制点位于预设的法线上，所述工件上每两个边缘点构成一线段；每次的所述转动是将所述工件转动至令其边缘的一所述线段同所述法线重合，所述工件的转动角度是通过计算所述线段转动至同法线重合的转动角度而获得的。

4.根据权利要求3所述的操作速度获取方法，其特征在于，所述转动角度等于：在第二操作点通过所述转动到达第三操作点时，第二操作点和外部控制点连线同第三操作点和外部控制点连线间的夹角。

5.根据权利要求1所述的操作速度获取方法，其特征在于，所述坐标系包括：X轴、Y轴、Z轴及U轴，其中，所述U轴是转动轴，以角度为单位。

6.一种操作速度获取装置，其特征在于，所述操作速度获取装置包括：坐标定义单元及工件移动速度获取单元；

所述坐标定义单元，用于在预定的工件移动空间内建立坐标系，在所述坐标系内定义所述工件各边缘点且将其中至少一边缘点作为对齐点，并在工件定义第一操作点；在所述工件移动空间内定义外部控制点；

所述操作装置在所述第一操作点操作所述工件移动，所述移动包括：工件平移至所述对齐点和外部控制点重合；或者，在所述对齐点和外部控制点重合的情况下，工件绕所述外部控制点进行转动；

所述工件移动速度获取单元，用于根据平移前的对齐点坐标及外部控制点坐标获得平移矢量，在所述第一操作点通过所述平移到达第二操作点时，根据平移矢量及第一操作点坐标计算平移后的第二操作点坐标，据以计算所述第一操作点平移至第二操作点的第一坐标移动量；在所述第二操作点通过所述转动到达第三操作点时，结合转动角度计算所述第三操作点的坐标，据以计算第二操作点转动至第三操作点的第二坐标移动量；结合所述第一坐标移动量及第二坐标移动量以计算出总移动量；根据所述总移动量计算工件的移动距离；结合所述第一操作点移动至第三操作点的时间、及所计算的工件的移动距离，计算工件相对外部控制点的移动速度。

7.根据权利要求6所述的操作速度获取装置，其特征在于，所述坐标转化单元，还用于获取工件的各所述边缘点进行所述移动后的坐标。

8.根据权利要求7所述的操作速度获取装置，其特征在于，所述外部控制点位于预设的法线上，所述工件上每两个边缘点构成一线段；每次的所述转动是将所述工件转动至令其边缘的一所述线段同所述法线重合，所述工件的转动角度是通过计算所述线段转动至同法线重合的转动角度而获得的。

9.根据权利要求8所述的操作速度获取装置，其特征在于，所述转动角度等于：在第二操作点通过所述转动到达第三操作点时，第二操作点和外部控制点连线同第三操作点和外部控制点连线间的夹角。

10.根据权利要求6所述的操作速度获取装置，其特征在于，所述坐标系包括：X轴、Y轴、Z轴及U轴，其中，所述U轴是转动轴，以角度为单位。