CN114063557B - 于弧形路径中控制端效器追踪对象的方法 - Google Patents

Abstract

本发明提出一种于弧形路径中控制端效器追踪对象的方法，运用于具有输送带及端效器的自动化作业系统。该方法是规画直线轨迹与姿态插补轨迹，计算移动速度曲线、位置补偿量、旋转速度曲线与姿态补偿量，依据上述资料产生并执行控制命令以控制端效器朝对象移动并旋转端效器的端效器姿态。并且，端效器于追上对象时，是与对象具有相同的移动速度与对应的姿态变化。本发明可有效控制端效器于弧形路径中追随对象以进行作业。

Description

于弧形路径中控制端效器追踪对象的方法

技术领域

本发明是与追踪对象位置与姿态的方法有关，特别是指于弧形路径中控制端效器追踪对象的方法。

背景技术

现有的自动作业系统包括输送带、机械设备的端效器与控制器。输送带是由电动机所驱动并用以输送对象。端效器(end-effector)用以对输送带上对象进行抓取、放置、加工或检测等自动化作业。控制器可依据电动机转动速度与方向来预测输送带上的对象于各时间点或各周期的预测位置，并控制端效器前往至预测位置来对对象进行自动化作业。

然而，端效器通常是由静止开始动作，且端效器的移动速度与对象的移动速度不一定相同，这使得当端效器抵达预测位置时，可能因两者速度不同步而造成抓取、放置、加工或检测等自动化作业失败(如对象未抵达预测位置或已离开预测位置)。

此外，现有技术由于是依据输送带速度预测对象于指定时间的预测位置，并控制端效器前往预测位置来进行自动作业，当输送带速度改变，对象便不会于指定时间抵达预测位置，而使端效器错过对象。

此外，输送带通常有直线路径与弧形路径(即弯角)两种形态，由于对象于弧形路径的实际移动距离与位移不同，控制端效器追上对象所需的轨迹规划算法过于复杂而不易实现，这导致现有的自动作业系统通常是在直线路径设置端效器来进行自动化作业，而浪费了弧形路径的空间。

此外，当对象于弧形路径被输送时，对象的姿态会不断改变，这使得端效器无法准确地对对象执行自动化作业。

发明内容

本发明的主要目的，是在于提供一种于弧形路径中控制端效器追踪对象的方法，可使端效器于弧形路径中随对象移动并改变姿态。

为达上述目的，本发明是提供一种于弧形路径中控制端效器追踪对象的方法，运用于具有一输送带及一端效器的一自动化作业系统，该输送带用以输送一对象并具有一弧形路径，该方法包括以下步骤：

步骤a: 取得进入该弧形路径的该对象的一对象进入姿态及一对象进入位置；

步骤b: 取得一对象移动速度及一对象旋转速度；

步骤c: 规画自一端效器起始位置到该对象进入位置的一直线轨迹；

步骤d: 依据该对象移动速度计算用以使该端效器追上该对象的一移动速度曲线及用于该直线轨迹的一位置补偿量；

步骤e: 依据该对象进入姿态及一端效器起始姿态规画一姿态插补轨迹；

步骤f: 依据该对象旋转速度计算用以使该端效器旋转至与该对象的姿态对应的一旋转速度曲线及用于该姿态插补轨迹的一姿态补偿量；

步骤g: 依据该直线轨迹、该移动速度曲线、该位置补偿量、该姿态插补轨迹及该姿态补偿量产生一控制命令；及

步骤h: 执行该控制命令来控制该端效器朝该对象移动并旋转姿态，其中于该端效器追上该对象后，该端效器与该对象的移动速度相同，且该端效器与该对象的姿态对应变化。

于一实施例中，该自动化作业系统还包括一触发装置，该方法于该步骤a之前还包括一步骤i: 于通过该触发装置侦测到该对象进入该弧形路径时，执行该步骤a至该步骤h。

于一实施例中，该对象的姿态是以一坐标系表示，步骤b是计算该输送带的该弧形路径的线速度作为该坐标系的该对象移动速度，依据该对象移动速度计算一对象移动角度，并依据该对象移动角度计算该坐标系的该对象旋转速度。

于一实施例中，该方法还包括一步骤j: 于该端效器追上该对象后，控制该端效器对该对象执行一自动化作业。

于一实施例中，该自动化作业包括对该对象执行抓取、放置、加工或检测的至少其中之一操作。

于一实施例中，该方法还包括一步骤k: 于侦测到该对象移动速度改变时，再次执行该步骤a至该步骤h。

于一实施例中，该步骤d包括以下步骤：

步骤d1: 依据该对象移动速度计算该移动速度曲线，其中该移动速度曲线是用以指示该端效器的一端效器移动速度的变化；及

步骤d2: 基于该移动速度曲线计算该端效器于各周期的一弧形移动距离及一弧形移动角度，依据各该弧形移动距离及各该弧形移动角度执行一弧形插补以获得各该周期的一弧形补偿位置，并依据该对象进入位置及各该周期的该弧形补偿位置计算各该周期的该位置补偿量。

于一实施例中，该步骤g包括一步骤g1:依据该端效器于该直线轨迹的各该周期的直线偏移量及各该周期的该位置补偿量计算该端效器于各该周期的一端效器位置，并依据各该周期的该端效器位置计算该端效器于各该周期的一移动控制命令。

于一实施例中，该步骤f包括：

步骤f1:依据该对象旋转速度计算该旋转速度曲线；及

步骤f2:依据该旋转速度曲线计算各周期的一对象旋转角度，依据该对象进入姿态与各该周期的该对象旋转角度执行一四元素插补以获得该对象于各该周期的一四元素旋转坐标作为各该周期的该姿态补偿量。

于一实施例中，该步骤g包括一步骤g2:依据各该周期的该四元素旋转坐标执行一姿态融合以获得该对象于各该周期的一旋转矩阵，并依据各该旋转矩阵产生各该周期的一旋转控制命令。

本发明可有效于弧形路径中追踪对象的轨迹与姿态，而使端效器可于弧形路径中对追随对象以进行作业。

附图说明

图1为于直线路径中执行自动化作业的示意图；

图2为于弧形路径中执行自动化作业的示意图；

图3为本发明一实施状态的自动化作业系统的架构图；

图4为本发明一实施状态的控制器的架构图；

图5为本发明一实施状态的移动速度曲线的示意图；

图6A为本发明一实施状态的自动化作业系统的移动轨迹的第一示意图；

图6B为本发明一实施状态的自动化作业系统的移动轨迹的第二示意图；

图6C为本发明一实施状态的自动化作业系统的移动轨迹的第三示意图；

图6D为本发明一实施状态的自动化作业系统的移动轨迹的第四示意图；

图6E为本发明一实施状态的自动化作业系统的移动轨迹的第五示意图；

图7为本发明一实施状态的自动化作业系统的姿态轨迹的示意图；

图8为本发明一实施状态的自动化作业系统的连续弧形路径的示意图；

图9为本发明第一实施例的于弧形路径中控制端效器追踪对象的方法的流程图；及

图10为本发明第二实施例的于弧形路径中控制端效器追踪对象的方法的流程图。

符号说明：

10：控制器；

11：端效器；

12：输送带；

130-132、140-142：位置；

2：自动化作业系统；

20：控制器；

21：触发装置；

22：编码器；

23：输送带；

24：端效器；

30：终端机；

31： 网络；

40：直线轨迹规划模块；

41：移动速度同步模块；

410：移动速度曲线模块；

411：弧形插补模块；

42：姿态轨迹规划模块；

43：旋转速度同步模块；

430：旋转速度曲线模块；

431：姿态插补模块；

501-505：弧形轨迹；

511-514：对象位移量；

511’-513’：位置补偿量；

520：直线轨迹；

521-523：弥补的直线偏移量；

600-604：对象弧形位置；

601’-603’：弧形补偿位置；

610-614：端效器位置；

70：输送带；

71-73：机械设备；

θ1-θ4：角度；

A1、A2：位移；

C1-C3：命令；

R1-R3：区域；

t0-t3：时间点；

Vo：对象移动速度；

Ve：端效器移动速度；

X、Y、Z：轴；

o：坐标原点；

S10-S17：第一追踪步骤；

S200-S214：第二追踪步骤。

具体实施方式

下面结合附图和具体实施例对本发明作进一步说明，以使本领域的技术人员可以更好的理解本发明并能予以实施，但所举实施例不作为对本发明的限定。

首请参阅图1及图2，图1为于直线路径中执行自动化作业的示意图，图2为于弧形路径中执行自动化作业的示意图。图1及图2是用以更为清楚地说明本发明所要解决的主要问题。自动化作业系统包括端效器11、输送带12及控制器10。

如图1所示，于直线路径中，同一对象于位置130、位置131及位置132的姿态(以XYZ轴的朝向表示)都是相同的，即对象于直线路径中并不会改变姿态，这使得端效器11只需依据对象当前姿态作为自身的目标姿态即可顺利夹取对象。

如图2所示，于弧形路径中，同一对象于位置140、位置141及位置142的姿态都不同，即对象于弧形路径中会不断改变姿态，这使得端效器11于弧形路径中无法只以对象当前姿态作为目标姿态对对象进行作业。

并且，于弧形路径中，对象的实际移动距离(即位置140与位置141之间的弧形轨迹的长度)是大于位移(即位置140与位置141之间的直线轨迹的长度)，这使得一般直线路径(实际移动距离等于位移)中所采用的端效器控制方式不适用于弧形路径。

请参阅图3，为本发明一实施状态的自动化作业系统的架构图。本发明提供一种于弧形路径中控制端效器追踪对象的方法(以下简称该方法)，该方法主要运用于一种自动化作业系统2。

自动化作业系统2可包括触发装置21、编码器22、输送带23、端效器24及控制器20。编码器22电性连接输送带23，控制器20电性连接编码器22及端效器24。触发装置21电性连接编码器22及控制器20。

于本发明中，输送带23具有弧形路径(如图6A至图8所示弧形路径)，本发明主要是公开如何于弧形路径中控制端效器追踪对象。

编码器22连接电动机(图未标示)或输送带23，并记录电动机或输送带23的各项编码器信息(例如电动机的转角度数或速度，或者输送带23的位置或速度)。输送带23通过前述电动机的转动而运作以输送一或多个对象。端效器24设置于机械设备上(如机器手臂或机器人，图未标示)，并受控制来朝向输送带23上的对象移动，以对对象进行取、放或其他加工程序。

于一实施例中，编码器22于被触发(如收到触发装置21的触发信号或经由计时器定时触发)时将编码器信息(如对象被触发时的输送带位置)传输至控制器20，控制器20可依据被触发时的编码器信息与当前的编码器信息计算输送带23上的各对象目前的(输送带)位置及/或速度。

触发装置21(如摄影机、红外线传感器或超声波传感器等等)主要是于侦测到待处理的对象出现在输送带23作业区域时发出触发信号，借以令控制器52自编码器22读取编码器信息。借此，控制器20可在触发装置21被触发时获得对象的当前位置。

于一实施例中，自动化作业系统2可经由网络31连接外部的终端机30。终端机30可提供控制接口(图未标示)，控制接口提供自动化作业系统2的管理者于线上对输送带23的速度进行调整。

请一并参阅图4，为本发明一实施状态的控制器的架构图。控制器20可包括用以控制端效器24追上对象的直线轨迹规划模块40及移动速度同步模块41，以及用以调整端效器24的姿态至适合对对对象进行作业的姿态轨迹规划模块42及旋转速度同步模块43。

图4所示的各模块可以硬件方式(如线路板、集成电路或SoC)或软件方式(如固件或应用程序等计算机程序)来加以实现，不加以限定。当以软件方式实现时，控制器20可包括非暂时性计算机可读取媒体，非暂时性计算机可读取媒体储存有前述计算机程序，当控制器20执行计算机程序后，可实现前述各模块的功能。

请一并参阅图3、图4及图9，图9为本发明第一实施例的于弧形路径中控制端效器追踪对象的方法的流程图。本实施例的该方法包括以下步骤。

步骤S10：控制器20判断对象是否进入弧形路径。

于一实施例中，触发装置21是被设置于(或朝向)弧形路径的入口，并可于侦测到对象进入弧形路径时(即对象抵达触发位置)对外发送触发信号。

若控制器20判断没有对象进入弧形路径，则再次执行步骤S10以持续侦测。

若控制器20判断任一对象进入弧形路径，则执行步骤S11：控制器20取得进入弧形路径的对象的姿态与位置。

步骤S11：控制器20取得进入弧形路径的对象所在的对象进入位置与所摆出的对象进入姿态。

于一实施例中，控制器20于收到触发信号时可将对象进入位置设定为触发位置，如触发装置21所在位置或所感测的位置。

于一实施例中，触发装置21为摄影机，控制器20于摄影机所拍摄的影像画面中辨识对象姿态。

于一实施例中，控制器20可依据对象进入位置与输送带23的速度(即对象移动速度)持续计算对象的对象当前位置，如每隔一周期计算一次对象当前位置，前述周期可为50毫秒、500毫秒、1秒、5秒、10秒等等，不加以限定。

于一实施例中，控制器20可依据对象进入姿态与对象移动速度持续计算对象于输送带23中的对象当前姿态，如每隔一周期计算一次对象当前姿态。

于一实施例中，如图2所示，各对象可被设定一组坐标系，并具有坐标系原点o(x,y,z)，此坐标系于空间坐标系(如输送带坐标系或现实空间坐标系)中的朝向与位置即可做为对象的姿态与位置。前述空间坐标系可为笛卡尔空间坐标系(Cartesian coordinates)。

步骤S12：控制器20取得对象移动速度，并可进一步取得对象旋转速度。

于一实施例中，控制器20可计算输送带23于弧形路径的线速度作为坐标系的移动速度。并且，控制器20可依据前述线速度计算对象的自体的旋转速度，如依据对象移动速度计算单位时间(如过去一周期)内的对象移动角度，并依据对象移动角度计算对象的对象旋转速度。

步骤S13：控制器20通过直线轨迹规画模块40依据对象进入位置及端效器24的端效器位置规画直线轨迹。

于一实施例中，直线轨迹规划模块40可取得对象进入位置及端效器起始位置，再依据对象进入位置与端效器起始位置规划直线轨迹。并且，直线轨迹规划模块40依据直线轨迹周期性地输出点位坐标(x,y,z)，以控制端效器24朝对象移动。

步骤S14：控制器20通过移动速度同步模块41依据对象的移动速度计算移动速度曲线及位置补偿量。

于一实施例中，移动速度同步模块41取得对象移动速度及端效器移动能力(如速度上限或加速度)，再依据两者规划一组速度曲线。并且，移动速度同步模块41可依据移动速度曲线周期性地输出偏移量(offset，如后述的落后偏移量)以作为加速或减速的参考。

前述移动速度曲线是用以指示端效器24于不同时间点或周期的端效器移动速度。并且，当端效器24依据移动速度曲线进行移动时，将可追上对象(即弥补完所有的落后偏移量)，并可于追上对象后维持与对象相同的移动速度。

请一并参阅图5，为本发明一实施状态的移动速度曲线的示意图。图5用以对移动速度曲线进行示例性说明。

于图5的例子中，输送带23的移动速度(对象移动速度)固定为Vo。于追踪程序下(即端效器24未追上对象)，控制器20是控制端效器24由静止开始加速。于t1时间点(如第一周期结束)时端效器移动速度Ve(t1)与对象移动速度Vo相同，于t2时间点(如第二周期结束)时端效器移动速度Ve(t2)达到最高值，并开始逐渐减速，而使得于t3时间点(如第三周期结束)时端效器移动速度Ve(t1)与对象移动速度Vo相同。

并且，于t3时间点时，端效器24已弥补完于t1时间点之前的落后偏移量，即区域A1的面积(即端效器24于t1-t3时间点之间比对象多增加的位移量)会等于区域A2的面积(即对象于t1时间点之前比端效器24多增加的位移量)。

接着，于已同步程序下(即端效器24已追上对象)，端效器24维持与对象相同的移动速度(如维持相同的位移量)，以持续跟随对象并对对象进行自动化作业。

请一并参阅图6A至图6E，图6A为本发明一实施状态的自动化作业系统的移动轨迹的第一示意图，图6B为本发明一实施状态的自动化作业系统的移动轨迹的第二示意图，图6C为本发明一实施状态的自动化作业系统的移动轨迹的第三示意图，图6D为本发明一实施状态的自动化作业系统的移动轨迹的第四示意图，图6E为本发明一实施状态的自动化作业系统的移动轨迹的第五示意图。于本发明中，移动速度同步模块41可对速度曲线的位移执行弧形插补以计算对应的补偿量。并且，于本例子中，是假设端效器24花费三周期追上对象。并且，于本例子中，端效器24对于对象的落后偏移量包括直线偏移量与弧形偏移量。

如图6A所示，当对象抵达弧形路径的对象进入位置600时会触发设置于此的触发装置21，而使得控制器20规划自端效器24起始位置610到对象进入位置600的直线轨迹520(直线轨迹520的长度即为端效器24落后对象的直线偏移量)与端效器24的移动速度曲线。

并且，控制器20基于移动速度曲线开始控制端效器24沿直线轨迹520朝对象进入位置600移动。

接着，如图6B所示，于第一周期中，对象从对象进入位置600沿弧形路径(弧形轨迹501)移动至对象弧形位置601(即对象位移量511)。控制器20依据端效器24于第一周期在直线轨迹520上所能弥补的直线偏移量521(可基于端效器24的移动速度曲线经计算获得)与第一周期的位置补偿量511’ (即弧形偏移量，表示端效器24于第一周期在弧形路径上可以弥补的偏移量，于本例子中为空间向量)计算端效器24于第一周期的移动目的地(即端效器位置611，可为空间坐标)。

更进一步地，移动速度同步模块41可依据移动速度曲线410计算出第一周期的弧形移动距离，依据此移动距离与弧形路径的半径计算弧形移动角度θ1。再依据对象进入位置600(空间坐标，如笛卡尔空间坐标)、弧形路径的半径与弧形移动角度θ1执行弧形插补模块411来计算出弧形补偿位置601’的坐标(空间坐标)。依据弧形补偿位置601’的坐标与对象进入位置600之间的位移量计算出第一周期的位置补偿量511’。

接着，如图6C所示，于第二周期中，对象从对象弧形位置601沿弧形路径(弧形轨迹502)移动至对象弧形位置602(即对象位移量512)。移动速度同步模块41依据端效器24于第二周期在直线轨迹520上所能弥补的直线偏移量522与第二周期的位置补偿量512’(即端效器24于第二周期在弧形路径上可以弥补的弧形偏移量)计算端效器24于第二周期的移动目的地(即端效器位置612)。

更进一步地，移动速度同步模块41可依据移动速度曲线410模块计算出弧形路径累积的移动距离，依据此移动距离与弧形路径的半径计算弧形移动角度(即对象进入位置600与弧形补偿位置602’之间的移动角度θ2)，依据对象进入位置600、弧形路径的半径与移动角度θ2执行弧形插补模块411来计算出弧形补偿位置602’的坐标，并依据弧形补偿位置602’的坐标与对象进入位置600之间的位移量计算第二周期的位置补偿量512’。

值得一提的是，由于移动速度曲线410的最高移动速度大于对象移动速度(才能从静止状态追上持续移动的对象)，于第二周期中，弧形路径补偿位置的移动速度可加速至大于对象于弧形路径的移动速度，而开始弥补落后的弧形偏移量。

接着，如图6D所示，于第三周期中，对象从对象弧形位置602沿弧形路径(弧形轨迹503)移动至对象弧形位置603(即对象位移量513)。控制器20依据端效器24于第三周期在直线轨迹520上所能弥补的直线偏移量523与第三周期的位置补偿量513’(即端效器24于第三周期在弧形路径上可以弥补的弧形偏移量)计算端效器24于第三周期的移动目的地(即端效器位置613)。于本例子中，第三周期结束时，端效器24弥补完所有落后偏移量而追上对象，如端效器位置613与对象弧形位置603相同。

更进一步地，移动速度同步模块41可依据移动速度曲线410计算出弧形路径累积的移动距离，由于第三周期结束已弥补完所有落后偏移量，故弧形路径累积的移动距离等于对象弧形轨迹501、502、503的长度和。依据此移动距离与弧形路径的半径计算弧形移动角度(即对象进入位置600与弧形补偿位置603’之间的移动角度θ3)，依据对象进入位置600、弧形路径的半径与移动角度θ3执行弧形插补411来计算出弧形补偿位置603’的坐标，并依据弧形补偿位置603’的坐标与对象进入位置600之间的位移量计算第三周期的位置补偿量513’。

接着，如图6E所示，于第四周期中(即端效器24追上对象后)，对象从对象弧形位置603沿弧形路径(弧形轨迹504)移动至对象弧形位置604。由于直线轨迹520已全部弥补完毕，移动速度同步模块41可直接将对象位移量514设定为第四周期的位置补偿量514 (即端效器24于第四周期以后在弧形路径上可以弥补的弧形偏移量等同于对象的位移量)，计算端效器24于第四周期的移动目的地(即端效器位置614)，以此类推。

更进一步地，移动速度同步模块41可依据对象进入位置600与对象移动速度计算出对象进入弧形路径后的累积的移动距离(即弧形轨迹501、502、503、504的长度和)，依据此移动距离与弧形路径的半径计算对象进入弧形路径后的对象移动角度(即对象进入位置600与对象弧形位置603之间的对象移动角度θ4)，依据对象进入位置600、弧形路径的半径与对象移动角度θ4执行弧形插补来计算出对象弧形位置604的坐标。并且，由于端效器24已追上对象且两者的速度已达成同步，移动速度同步模块41可将对象弧形位置604的坐标与对象进入位置600之间的对象位移量514直接设定为第四周期的位置补偿量514。

此外，于本例子中，第四周期开始，端效器24可以与对象相同的速度持续追踪对象，并开始对对象自动化处理。并且，当端效器24完成自动化作业后，端效器24可自动移回至端效器起始位置610，以等待下一对象的抵达，而对象继续沿着弧形轨迹505直到离开弧形路径。

借此，本发明可持续修正端效器24的轨迹来确保端效器24追上对象并于追上对象后持续追踪对象以进行自动化处理。

复请参阅图9，接着执行步骤S15：控制器20取得对象进入弧形路径时端效器24的端效器初始姿态，并通过姿态轨迹规划模块42依据对象进入姿态及端效器起始姿态规画一组姿态插补轨迹。前述姿态插补轨迹是用来使端效器24自端效器起始姿态转动至成为可对对象进行作业的姿态。

步骤S16：控制器20通过旋转速度同步模块43取得对象旋转速度，并依据对象旋转速度计算旋转速度曲线与姿态补偿量。

具体而言，由于对象在弧形路径中移动时，姿态会不断地改变，对此，本发明进一步提供姿态补偿量，并使端效器24于跟随对象的同时，依据此组姿态补偿量连续改变姿态，以持续保持可对对象进行作业的姿态。

于一实施例中，旋转速度同步模块43取得对象旋转速度及端效器旋转能力(如旋转角速度上限、角加速度或可旋转角度)，再依据两者规划一组旋转速度曲线。并且，旋转速度同步模块43可依据旋转速度曲线周期性地输出偏移量以作为加速或减速旋转的参考。

前述旋转速度曲线可用以指示端效器24于不同时间点的端效器旋转速度。并且，当端效器24依据旋转速度曲线进行旋转时，将可于追上对象后与对象保持相对应的姿态，而可对对象进行作业。

请一并参阅图6A至图6C与图7，图7为本发明一实施状态的自动化作业系统的姿态轨迹的示意图。

于对象抵达对象进入位置600时，姿态轨迹规划模块42依据对象于对象进入位置600的对象进入姿态及端效器24于端效器起始位置610的端效器起始姿态规画一组姿态插补轨迹，即自端效器起始位置610的端效器起始姿态改变为可对对象进入位置600的对象进行作业的端效器姿态。

于第一周期中，对象自对象进入位置600的姿态改变为对象弧形位置601的姿态。旋转速度同步模块43可(依据对象于对象进入位置600的姿态与对象旋转角度)计算姿态补偿量，如执行四元素插补来获得对象的于对象弧形位置601的四元素旋转坐标。接着，控制器20可依据姿态补偿量控制端效器24改变姿态(如于X-Y平面水平转动)来保持可对对象弧形位置601的对象进行作业的姿态。

于第二周期中，对象自对象弧形位置601的姿态改变为对象弧形位置602的姿态。旋转速度同步模块43可(依据对象于对象弧形位置601的姿态与第二周期的对象旋转角度)计算姿态补偿量，控制器20可依据姿态补偿量控制端效器24改变姿态，如改变为可对对象弧形位置602的对象进行作业的姿态。

于第三周期中，对象自对象弧形位置602的姿态改变为对象弧形位置603的姿态。旋转速度同步模块43可(依据对象于对象弧形位置602的姿态与第三周期的对象旋转角度)计算姿态补偿量，控制器20可依据姿态补偿量控制端效器24改变姿态，如改变为可对对象弧形位置603的对象进行作业的姿态，以此类推。

借此，本发明可使端效器的姿态对应对象当前姿态旋转改变，而可以于端效器追上对象后，顺利对对象进行自动化作业。

更进一步地，当对象移动至对象弧形位置604且端效器24移动至端效器位置614时，端效器24完成自动化作业，并可自动移回至端效器起始位置610来摆出端效器起始姿态，以等待下一对象的抵达。

于一实施例中，前述姿态补偿量可为对象当前姿态，并用以对姿态插补轨迹的对象进入姿态进行修正以获得可于追上对象后持续对对象进行作业的姿态变换轨迹，即修正后的姿态插补轨迹是表示各周期的端效器24与对象当前姿态的姿态差异。

并且，旋转速度同步模块43可周期性地计算姿态补偿量以持续修正姿态插补轨迹与端效器当前姿态。

复请参阅图9，接着执行步骤S17：控制器20依据直线轨迹、移动速度曲线、位置补偿量、姿态插补轨迹、旋转速度曲线及姿态补偿量产生控制命令。并且，控制器20依据所产生的控制命令控制端效器24朝对象移动并旋转端效器24的端效器姿态。

并且，于端效器24追上对象后，端效器24的移动速度是与对象的移动速度相同而可持续跟随对象，且端效器24的姿态是对应对象姿态进行变化而可持续对对象进行作业。

请参阅图4，于一实施例中，控制器20将端效器位置(如端效器起始位置或端效器当前位置)与对象弧形位置(如对象进入位置或对象当前位置)输入至直线轨迹规划模块40以获得“直线轨迹”的输出(步骤S13)；将对象移动速度与端效器移动能力输入至移动速度同步模块41以获得“移动速度曲线”及“位置补偿量”的输出(步骤S14)；将端效器姿态(如端效器起始姿态或端效器当前姿态)与对象姿态(如对象进入姿态或对象当前姿态)输入至姿态轨迹规划模块42以获得“姿态插补轨迹”的输出(步骤S15)；将对象旋转速度(可由对象的移动线速度计算获得)与端效器旋转能力输入至旋转速度同步模块43以获得“旋转速度曲线”与“姿态补偿量”的输出(步骤S16)。接着，控制器20可依据直线轨迹、移动速度曲线及位置补偿量计算用以控制端效器22移动的一组移动控制命令C1，并依据姿态插补轨迹、旋转速度曲线及姿态补偿量计算用以控制端效器22旋转的一组旋转控制命令C2。最后，控制器20将移动控制命令C1与旋转控制命令C2整合为控制命令C3，并经由执行控制命令C3来控制端效器24。

借此，本发明可有效于弧形路径中追踪对象的轨迹与姿态，而使端效器可于弧形路径中对追随对象以进行作业。

请一并参阅图4及图10，图10为本发明第二实施例的于弧形路径中控制端效器追踪对象的方法的流程图。于本实施例中，当侦测到对象速度(如输送带的速度)改变时，会依据最新的参数(如对象当前位置及/或对象移动速度)重新计算一次控制命令并执行，借以提升作业的准确性。

并且，如图4所示，于本实施例中移动速度曲线模块41包括移动速度曲线模块410与弧形插补模块411。旋转速度同步模块43包括旋转速度曲线模块430及姿态插补模块431。本实施例的该方法包括以下步骤。

步骤S200：控制器20 判断对象是否进入弧形路径。

若控制器20判断没有对象进入弧形路径，则再次执行步骤S200以持续侦测。

若控制器20判断任一对象进入弧形路径，则执行步骤S201：控制器20 取得进入弧形路径的对象所在的对象进入位置与所摆出的对象进入姿态。

步骤S202：控制器20取得进入弧形路径的对象当前的对象移动速度与对象旋转速度。前述对象旋转速度可由对象移动速度计算获得。

接着，控制器20可依序或同时执行步骤S203、步骤S204、步骤S207及步骤S208。

步骤S203：控制器20通过直线轨迹规划模块40规划从端效器起始位置至对象进入位置的直线轨迹。

步骤S204：控制器20通过移动速度曲线模块410依据对象的移动速度计算端效器24的移动速度曲线。前述移动速度曲线的最高速度是大于对象的移动速度，而使端效器24可于落后的情况下追上对象。

步骤S205：控制器20基于移动速度曲线计算端效器24于各周期的弧形移动距离，依据各周期的弧形移动距离与弧形路径的半径计算各周期的弧形移动角度，再依据对象进入位置、弧形路径的半径与各周期的弧形移动角度执行弧形插补来计算出各周期的弧形补偿位置，并依据将对象进入位置与各周期的弧形补偿位置之间的位移量作为计算各周期的位置补偿量。

步骤S206：控制器20依据端效器24各周期于直线轨迹的直线偏移量及各周期的位置补偿量计算各周期的端效器位置(如空间坐标)，并依据各周期的端效器位置计算各周期的移动控制命令。前述移动控制命令是用以控制端效器24自端效器当前位置前往指定的位置。

步骤S207：控制器20通过姿态轨迹规划模块42依据对象进入姿态及端效器起始姿态规画一组姿态插补轨迹。前述姿态插补轨迹是用来使端效器24自端效器起始姿态转动至成为可对处于对象进入姿态的对象进行作业的姿态。

步骤S208：控制器20通过旋转速度曲线430依据对象旋转速度计算端效器43的旋转速度曲线。并且，前述旋转速度曲线可以端效器43追上对象后，随对象转动而维持可对对象进行作业的姿态。

步骤S209：控制器20通过姿态插补模块431计算对象旋转角度，依据对象进入姿态与旋转角度计算对象的当前姿态来作为姿态补偿量，并依据姿态补偿量执行四元素插补以获得对象的四元素旋转坐标。前述四元素插补属于机械运动学的现有技术，其技术细节于此不再赘述。

步骤S210：控制器20依据对象的四元素旋转坐标及姿态插补轨迹执行姿态融合以获得对象的旋转矩阵，并依据旋转矩阵产生旋转控制命令。

步骤S211：控制器20判断端效器24是否追上对象。

若控制器20判断端效器24追上对象，则执行步骤S212：控制端效器24对对象执行预测的自动化作业(如抓取、放置、加工或检测等等)。

步骤S213：控制器20判断是否完成此对象的自动化作业。于一实施例中，控制器20是依据是否自动化作业的所有步骤都执行完毕，或者依据对象是否抵达预设的完成位置来判断是否完成此对象的自动化作业。

若控制器20判断完成自动化作业，则结束此对象的追踪控制；若控制器20判断自动化作业尚未完成，则执行步骤S214以继续追踪此对象。

值得一提的是，当执行自动化作业期间，输送带23仍持续输送对象(即对象会持续移动且姿态持续改变)，而端效器24会维持与对象相同的速度并随对象改变姿态，来于移动中完成自动化作业。

以图6A至图6E为例，当对象被输送至对象进入位置600时会触发前述触发装置21，而使端效器24开始追踪对象。当对象被移动至位置603时，端效器24追上对象，与对象维持相同的移动速度与对应姿态，并开始对对象执行自动化作业。当对象被移动至位置604时，自动化作业完成，端效器24停止跟随对象，并可进一步回到端效器起始位置610。

若控制器20判断端效器24未追上对象，则执行步骤S214：控制器20判断对象的移动速度(如输送带23的速度)是否改变。具体而言，控制器20可持续地自编码器22读取(如每隔一周期读取一次)编码器信息(如对象的位置或速度)，并依据编码器信息判断对象的速度是否改变。

若对象的移动速度没有改变，则控制器20再次执行步骤S211以判断是否追上对象。

若对象的移动速度改变，则控制器20再次执行步骤S201-S211以更新对象位置与对象姿态，决定并执行新的控制命令。

虽于前述实施例中，是以于单一弧形路径输送对象来进行说明，但不以此限定本发明的输送带23的路径。

请参阅图8，为本发明一实施状态的自动化作业系统的连续弧形路径的示意图。

图8的输送带70具有三个区域R1-R3，并分别设置有机械设备71-73。

区域R1与区域R2为弧形路径，故可直接应用本发明的于弧形路径中控制端效器追踪对象的方法来控制机械设备71、72对输送带70上的对象进行追踪与作业。

区域R3为直线路径，可直接应用现有的于直线路径中控制端效器追踪对象的方法(如图1所示) 来控制机械设备73对输送带70上的对象进行追踪与作业。

借此，本发明可有效利用弧形路径的空间。

以上所述实施例仅是为充分说明本发明而所举的较佳的实施例，本发明的保护范围不限于此。本技术领域的技术人员在本发明基础上所作的等同替代或变换，均在本发明的保护范围之内。本发明的保护范围以权利要求书为准。

Claims (10)

1.一种于弧形路径中控制端效器追踪对象的方法，运用于具有一输送带及一端效器的一自动化作业系统，该输送带用以输送一对象并具有一弧形路径，其特征在于，该方法包括以下步骤：

步骤a：取得进入该弧形路径的该对象的一对象进入姿态及一对象进入位置；

步骤b：取得一对象移动速度及一对象旋转速度；

步骤c：规画自一端效器起始位置到该对象进入位置的一直线轨迹；

步骤d：依据该对象移动速度计算用以使该端效器追上该对象的一移动速度曲线及用于该直线轨迹的一位置补偿量；

步骤e：依据该对象进入姿态及一端效器起始姿态规画一姿态插补轨迹；

步骤f：依据该对象旋转速度计算用以使该端效器旋转至与该对象的姿态对应的一旋转速度曲线及用于该姿态插补轨迹的一姿态补偿量；

步骤g：依据该直线轨迹、该移动速度曲线、该位置补偿量、该姿态插补轨迹及该姿态补偿量产生一控制命令；及

步骤h：执行该控制命令来控制该端效器朝该对象移动并旋转姿态，其中于该端效器追上该对象后，该端效器与该对象的移动速度相同，且该端效器与该对象的姿态对应变化。

2.如权利要求1所述的于弧形路径中控制端效器追踪对象的方法，其特征在于，该自动化作业系统还包括一触发装置，该方法于该步骤a之前还包括一步骤i：于通过该触发装置侦测到该对象进入该弧形路径时，执行该步骤a至该步骤h。

3.如权利要求1所述的于弧形路径中控制端效器追踪对象的方法，其特征在于，该对象的姿态是以一坐标系表示，步骤b是计算该输送带的该弧形路径的线速度作为该坐标系的该对象移动速度，依据该对象移动速度计算一对象移动角度，并依据该对象移动角度计算该坐标系的该对象旋转速度。

4.如权利要求1所述的于弧形路径中控制端效器追踪对象的方法，其特征在于，还包括一步骤j ：于该端效器追上该对象后，控制该端效器对该对象执行一自动化作业。

5.如权利要求4所述的于弧形路径中控制端效器追踪对象的方法，其特征在于，该自动化作业包括对该对象执行抓取、放置、加工或检测的至少其中之一操作。

6.如权利要求1所述的于弧形路径中控制端效器追踪对象的方法，其特征在于，还包括一步骤k：于侦测到该对象移动速度改变时，再次执行该步骤a至该步骤h。

7.如权利要求1所述的于弧形路径中控制端效器追踪对象的方法，其特征在于，该步骤d包括以下步骤：

步骤d1：依据该对象移动速度计算该移动速度曲线，其中该移动速度曲线是用以指示该端效器的一端效器移动速度的变化；及

步骤d2：基于该移动速度曲线计算该端效器于各周期的一弧形移动距离及一弧形移动角度，依据各该弧形移动距离及各该弧形移动角度执行一弧形插补以获得各该周期的一弧形补偿位置，并依据该对象进入位置及各该周期的该弧形补偿位置计算各该周期的该位置补偿量。

8.如权利要求7所述的于弧形路径中控制端效器追踪对象的方法，其特征在于，该步骤g包括一步骤g1：依据该端效器于该直线轨迹的各该周期的直线偏移量及各该周期的该位置补偿量计算该端效器于各该周期的一端效器位置，并依据各该周期的该端效器位置计算该端效器于各该周期的一移动控制命令。

9.如权利要求1所述的于弧形路径中控制端效器追踪对象的方法，其特征在于，该步骤f包括：

步骤f1：依据该对象旋转速度计算该旋转速度曲线；及

步骤f2：依据该旋转速度曲线计算各周期的一对象旋转角度，依据该对象进入姿态与各该周期的该对象旋转角度执行一四元素插补以获得该对象于各该周期的一四元素旋转坐标作为各该周期的该姿态补偿量。

10.如权利要求9所述的于弧形路径中控制端效器追踪对象的方法，其特征在于，该步骤g包括一步骤g2：依据各该周期的该四元素旋转坐标执行一姿态融合以获得该对象于各该周期的一旋转矩阵，并依据各该旋转矩阵产生各该周期的一旋转控制命令。