CN113664815B - 教导位置与姿态的触发与补偿方法 - Google Patents

Abstract

本发明提供一种教导位置与姿态的触发与补偿方法。于姿态维持模式下，自动化机械系统每隔一周期计算并执行新的姿态维持命令以使其机械设备维持在第一姿态，并于新的姿态命令与第一姿态的参考命令具有力矩差异时，判定机械设备接受外力，决定外力的方向并切换至动力维持模式。于动力维持模式下，自动化机械系统依据补偿力及各马达的当前动力计算动力维持命令并经由马达驱动器执行动力维持命令以减轻朝外力的方向调整机械设备为第二姿态的阻力。本发明可供用户以便捷且省力的方式来对机械设备的位置姿态进行教导。

Description

教导位置与姿态的触发与补偿方法

技术领域

本发明是与有关机械设备的姿态教导，特别有关于教导位置与姿态的触发与补偿方法。

背景技术

于现有技术中，当要对机械设备(如多轴机构或机械手臂)的位置与姿态进行教导时，通常是由用户操作具有多个操作按钮的教导盒，前述多个操作按钮分别用来控制机械设备的多个马达来转动，以使机械设备到达用户期望的位置与姿态。

上述教导技术中，用户必须将期望的位置与姿态自空间坐标系(即XYZ轴坐标系)转换至机械设备的运动坐标系(由多个马达位置组成多维度坐标系)才能进行操作，进而顺利使机械设备到达用户期望的位置与姿态。上述教导技术不仅费时亦不够直觉。

为解决上述问题，目前另有一种位置与姿态教导技术被提出，前述位置与姿态教导技术是由用户先操作机械设备进入力矩模式，再以人力调整机械设备的姿态，借以直觉地使机械设备到达期望的位置与姿态。

然而，用户每次要进行位置与姿态教导时，都必须先手动操作机械设备切换至力矩模式，因而相当不便。

此外，于力矩模式下，机械设备的各马达是输出固定的力矩以于未受额外的外力的情况下维持固定姿态，当用户以人力调整机械设备的姿态时，必须克服机械设备的摩擦力与前述力矩，才能改变机械设备的姿态，而相当费力。

是以，现有姿态教导技术存在上述问题，而亟待更有效的方案被提出。

发明内容

本发明的主要目的，在于提供一种教导位置与姿态的触发与补偿方法，可自动侦测用户是否开始执行姿态教导。

为达上述目的，本发明提供一种教导位置与姿态的触发与补偿方法，用于包含一控制器、一马达驱动器及一机械设备的一自动化机械系统，该机械设备包括用以变换位置与姿态的多个马达，该教导位置与姿态的触发与补偿方法包括以下步骤：

a) 于一姿态维持模式下，该控制器每隔一周期计算并执行新的姿态维持命令以使该机械设备维持在一第一姿态，其中该姿态维持命令包括各该马达的一马达控制命令；

b) 于新的该姿态维持命令的输出力矩与该第一姿态所对应的一参考姿态命令的输出力矩之间具有差异时，判定该机械设备接受一外力，决定该外力的方向并切换至一动力维持模式；

c) 于该动力维持模式下，决定该机械设备的各该马达的当前动力；

d) 依据该外力的方向决定一补偿力；及

e) 依据该补偿力及各该马达的该当前动力计算一动力维持命令并经由该马达驱动器执行该动力维持命令以减轻朝该外力方向调整该机械设备为一第二姿态的阻力，其中该动力维持命令包括各该马达的该马达控制命令。

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

a1) 于该姿态维持模式下，该控制器每隔该周期取得该机械设备的一当前姿态；

a2) 依据该当前姿态与该第一姿态之间的一差异计算新的该姿态维持命令；及

a3) 依据新的该姿态维持命令的各该马达控制命令控制各该马达转动以使该机械设备为该第一姿态，其中各该马达控制命令包括一转动速度命令与一转动方向命令。

于一实施例中，该当前姿态包括各该马达的当前位置，该第一姿态包括各该马达的一第一位置；

该步骤a2)是计算各该马达的该当前位置与该第一位置之间的该差异，并依据该差异产生可使各该马达从该当前位置移回至该第一位置的各该马达控制命令，以做为新的该姿态维持命令。

于一实施例中，于该姿态维持模式下，新的该姿态维持命令是用以控制各该马达改变转动方式以抵消该机械设备所受该外力，该参考姿态命令是用以控制各该马达转动方式以使该机械设备于未受外力下摆出该第一姿态。

于一实施例中，该多个姿态维持命令是分别用以提供多个输出力矩；该步骤b)是计算新的该姿态维持命令所提供的该输出力矩及该参考姿态命令所提供的该输出力矩之间的一力矩差异，于该力矩差异大于一力矩临界值时，依据该力矩差异决定该外力的方向并切换至该动力维持模式。

于一实施例中，该步骤d)包括一步骤d1) 计算为了抵消或减轻该机械设备朝该外力的方向进行教导移动所生的摩擦力所需的一摩擦力补偿力。

于一实施例中，该多个马达分别用以带动多个传动机构移动以变换该机械设备的姿态，该步骤c)是取得各该传动机构的一移动速度；

该步骤d1)包括以下步骤：

d11) 于该马达未受外力时，设定该马达的该摩擦力补偿力为零；

d12) 于任一该传动机构的该移动速度为零时，设定对应的该马达的该摩擦力补偿力为一起始摩擦力；

d13) 于任一该传动机构的该移动速度大于零时，设定对应的该马达的该摩擦力补偿力为一动摩擦力；及

d14) 于任一该传动机构的该移动速度大于一安全速度时，调降该摩擦力补偿力。

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

d2) 依据各该马达的该当前动力计算为了抵消或减轻该机械设备的重力所需的一重力补偿力；及

d3) 依据该摩擦力补偿力及该重力补偿力决定最终的该补偿力。

于一实施例中，所述的教导位置与姿态的触发与补偿方法还包括以下步骤：

f) 于该动力维持模式下侦测到该机械设备停止变换位置与姿态时，记录各该马达的一当前位置，并作为该第二姿态的位置；及

g) 切换至该姿态维持模式。

于一实施例中，所述的教导位置与姿态的触发与补偿方法还包括以下步骤：

h) 于该自动化机械系统被操作来切换至一教导状态时，重复执行该步骤a)至该步骤g)；及

i) 于该自动化机械系统被操作来切换至一工作状态且收到要求该机械设备摆出该第二姿态的一工作控制命令时，控制各该马达转动至于该教导状态所记录的该第二姿态的位置。

本发明可供用户以便捷且省力的方式来对机械设备的姿态进行教导。

附图说明

图1为本发明一实施例的自动化机械系统的架构图。

图2为本发明一实施例的教导状态与工作状态的示意图。

图3为本发明一实施例的计算补偿力的示意图。

图4A为本发明一实施例的姿态教导的第一示意图。

图4B为本发明一实施例的姿态教导的第二示意图。

图4C为本发明一实施例的姿态教导的第三示意图。

图5为本发明的第一实施例的教导位置与姿态的触发与补偿方法的流程图。

图6为本发明的第二实施例的教导位置与姿态的触发与补偿方法的部分流程图。

图7为本发明的第三实施例的教导位置与姿态的触发与补偿方法的部分流程图。

图8为本发明的第四实施例的教导位置与姿态的触发与补偿方法的部分流程图。

图9为本发明的第五实施例的教导位置与姿态的触发与补偿方法的第一部分流程图。

图10为本发明的第五实施例的教导位置与姿态的触发与补偿方法的第二部分流程图。

符号说明：

1:机械设备；

11:马达；

12:传动机构；

2:马达驱动器；

3:控制器；

31:机构重力补偿模块；

32:摩擦力补偿模块；

33:安全模块；

40:教导状态；

400:姿态维持模式；

401:动力维持模式；

41:工作状态；

410:姿态维持模式；

D1:转向信息；

F1、F2、F2’、F3:力；

M1、M2:力矩；

P1:位置信息；

V1:转速信息；

S10-S18:第一触发与补偿步骤；

S20-S22:姿态维持步骤；

S30-S32:补偿步骤；

S400-S410:摩擦力补偿步骤；

S500-S515:第二触发与补偿步骤。

具体实施方式

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

首请参阅图1，为本发明一实施例的自动化机械系统的架构图。本发明公开一种教导位置与姿态的触发与补偿方法(下面简称为该方法)，该方法主要运用于如图1及图2所示的自动化机械系统。

如图1所示，自动化机械系统包括机械设备1、马达驱动器2及控制器3。

机械设备1(如机械手臂或其他多轴移动装置)包括用以变换位置与姿态的多个马达11及分别与该多个马达11连接的多个传动机构12(如机械肢体)。各马达11分别连接马达驱动器2，以接受马达驱动器2的控制而转动，并带动对应的各传动机构12进行运动。

马达驱动器2用以对各马达11进行控制，并可依据各马达11的状态取得各轴马达11的位置信息并回馈至控制器3。

控制器3用以计算各马达的马达控制命令，并经由马达驱动器2来执行马达控制命令。

于一实施例中，控制器3是依据各马达11的位置信息判断其转动方向及转动速度，借以计算进行移动或维持姿态所需的力矩(或者自设置于各马达11的力矩传感器读取力矩值)，并输出至马达驱动器2。马达驱动器2依据力矩值对各马达11进行对应的控制以移动机械设备1或使其维持姿态。

于一实施例中，于机械设备1接受用户的姿态教导时，控制器3可适时经由马达驱动器2来控制各马达11提供补偿力，以降低用户扳动机械设备1所遭受的阻力。

具体而言，控制器3包括用以补偿重力的机构重力补偿模块31、用以补偿摩擦力的摩擦力补偿模块32及用以避免补偿过度的安全模块33，其详细运作方式容后详述。

请一并参阅图2，图2为本发明一实施例的教导状态与工作状态的示意图。本发明的自动化机械系统可于两种状态间进行运作，分别为教导状态40与工作状态41。

当自动化机械系统处于教导状态40下，用户可对机械设备1的姿态进行调整，并且，自动化机械系统可对调整后的姿态进行记录。教导状态40包括两种模式，姿态维持模式400与动力维持模式401。

具体而言，当自动化机械系统处于姿态维持模式400下，是持续控制机械设备1摆出指定姿态，即各马达11会持续保持在相同位置或维持相同的移动速度，并且，当机械设备1遭受外力时，各马达会受控制来增加或减少输出力矩，以使机械设备1维持在指定姿态或维持相同的移动速度。

当自动化机械系统处于动力维持模式401下，是持续控制各马达11输出相同的动力(如相同力矩)，即未遭受外力时，机械设备1可以维持对应的姿态；遭受足以克服阻力(如摩擦力或输出动力)的外力时，机械设备1会朝往外力的方向移动来摆出新的姿态。借此，用户可调整机械设备1的姿态。

更进一步地，本发明的自动化机械系统是被设定来于进入教导状态40直接切换至姿态维持模式400以等待用户开始进行姿态教导。当侦测到外力介入时(如用户意图移动机械设备1)，自动切换至动力维持模式401以使用户可对机械设备1的姿态进行调整。

更进一步地，于动力维持模式401下，自动化机械系统可于侦测到用户停止变换机械设备1的姿态或是用户手动执行模式切换操作时，切换回姿态维持模式400以维持指定姿态(如调整后的姿态)或指定的移动速度。

当自动化机械系统处于工作状态41下(如经由自动侦测或用户手动操作)，自动化机械系统可进入姿态维持模式400，并依据用户设定的控制指令或排程来控制机械设备1摆出指定姿态，连续摆出不同姿态(如教导状态下所学习的姿态) 或以指定速度变换位置及/或姿态，借以达成自动化控制(如自动组装、自动检测或自动拾取等等)。

教导状态40的姿态维持模式400与工作状态41的姿态维持模式410可为相同或相似的运作方式，但不加以限定。

于一实施例中，自动化机械系统在教导状态40的姿态维持模式400下移动机械设备1的速度(即姿态变换速度)是小于在工作状态41的姿态维持模式400下移动机械设备1的速度。

具体而言，教导状态40通常是由用户协助，当进行姿态变化时，为避免以过快的姿态变换速度误碰撞用户或周围对象所带来的损害，而刻意降低姿态维持模式400中的姿态变换速度。工作状态41通常是机械设备1独立运作，周围没有用户或无关对象时，故姿态维持模式410可采用较快的姿态变换速度进行运作，以提升自动化处理的效率。

于一实施例中，控制器3可包括储存模块(图未标示)，前述储存模块可包括非瞬时计算机可读取媒体，并储存有计算机程序(如机械设备1的控制程序)，计算机程序记录有计算机可读取的程序代码。控制器3可执行计算机程序来经由马达驱动器2控制各马达11来实现本发明各实施例的该方法的各步骤。

请一并参阅图5，为本发明的第一实施例的教导位置与姿态的触发与补偿方法的流程图。该方法主要包括以下步骤。

步骤S10：控制器3进入姿态维持模式400。

步骤S11：于姿态维持模式400下，控制器3每隔一周期(如0.1毫秒、0.5毫秒、1秒或5秒)计算新的姿态维持命令，并执行新的姿态维持命令。

于一实施例中，姿态维持命令包括各马达11的马达控制命令。控制器3每隔一周期取得当前姿态(可包括当前位置)，并比较当前姿态是否与所指定的姿态(第一姿态)的姿态(可包括位置，如移动完成后的位置，可为空间坐标系的位置或机器人坐标系的位置)相符。若姿态相符，则控制器3可继续输出相同参数的姿态维持命令，即使各马达11维持当前的动力输出(如发出相同的马达控制命令)；若姿态(或位置)不符，则控制器3可计算当前姿态与指定姿态之间的差异及/或当前位置与指定位置之间的差异，并依据此差异计算新的姿态维持命令(如重新计算各马达11的移动方向、移动距离及/或输出力矩做为新的马达控制命令，而使该机械设备1于各马达11移动后可摆出指定姿态)及/或移动至指定位置，将新的姿态维持命令发送至马达驱动器2，以对各马达11逐一进行控制调整，而使机械设备的当前姿态修正为指定姿态。借此，机械设备1可适应所施加的外力，而持续维持在指定姿态(第一姿态)。

换句话说，前述新的姿态维持命令是用以控制各马达11改变转动方式(如改变输出力矩)以抵消机械设备1所受外力。

步骤S12：控制器3对新的姿态维持命令(如最新一个周期执行的姿态维持命令)与用以摆出第一姿态的参考姿态命令进行比较(如对姿态维持命令所提供的输出力矩与参考姿态命令所提供的输出力矩进行比较)，以侦测姿态维持命令是否发生变化(如输出力矩是否改变)，即判断机械设备1是否受到外力(即用户为了进行姿态教导而对机械设备1施力)，而使控制器3为了使机械设备1维持在指定姿态而产生用以抵消外力的新的姿态维持命令。

前述参考姿态命令是用以控制各马达11的转动方式以使机械设备1于未受外力下摆出指定的姿态(如第一姿态)。

步骤S13：控制器3判断新的姿态维持命令与参考姿态命令之间是否具有差异(如新的姿态维持命令所提供的输出力矩与参考姿态命令所提供的输出力矩之间具有差异)。

若控制器3判断新的姿态维持命令与参考姿态命令之间具有差异，则可决定外力的方向，并执行步骤S14。若控制器3判断新的姿态维持命令与参考姿态命令之间没有差异，则表示机械设备1未受到外力，并跳至步骤S11执行。

于一实施例中，各姿态维持命令是分别用以提供一组输出力矩。控制器3是于新的姿态维持命令所对应的新的输出力矩与参考姿态命令的输出力矩之间的力矩差异符合预设的力矩临界值时，判定新的姿态维持命令与参考姿态命令之间具有差异，并依据力矩变化决定外力的方向。

借此，本发明可经由姿态命令的变化有效侦测用户是否意图进行姿态教导，且不需额外设置感测装置。

步骤S14：于判定用户意图进行姿态教导时，控制器3切换至动力维持模式。

步骤S15：控制器3决定机械设备1的各马达的当前动力，如当前的力矩、转速及/或转动方向。

步骤S16：控制器3依据外力的方向决定补偿力。于一实施例中，补偿力的方向是与外力相同，即补偿力是用来抵销移动机械设备1的阻力，但不以此为限。

步骤S17：控制器3依据补偿力及各马达的当前动力计算一组动力维持命令，并经由马达驱动器2执行所计算的动力维持命令。前述动力维持命令包括各马达11的马达控制命令，当各马达11依据对应的马达控制命令运作时，可减轻朝外力的方向调整机械设备为另一姿态(第二姿态，即用户所期望的姿态)的阻力。

举例来说，补偿力的大小可等于(或接近)机械设备1的摩擦力(如最大静摩擦力或动摩擦力)的大小，但方向相反(即与外力相同方向)。由于补偿了摩擦力，用户仅需所施加少量外力(如小于摩擦力的外力)，即可朝期望方向推动机械设备1。

于另一例子中，补偿力的大小是略大于机械设备1的摩擦力的大小，并与外力相同方向，而使得机械设备1于未施下外力的情况下会缓慢朝先前施加的外力方向移动。借此，用户可更为轻松地使机械设备1朝期望方向移动。值得一提的是，于此状况下，当机械设备1移动至用户期望的位置时（即摆出用户期望的姿态），用户仅需稍微施加阻力，即可使机械设备1停止移动，而维持用户期望的姿态。

步骤S18：机械设备1接受用户的姿态教导操作，而摆出用户期望的姿态，接着，控制器3对最后的姿态(第二姿态)进行记录，(如记录各马达11的位置)。

本发明可供用户以便捷方式来触发姿态教导，并以省力方式来对机械设备的姿态进行教导，且不需额外设置传感器。

续请一并参阅图4A至图4C，图4A为本发明一实施例的姿态教导的第一示意图，图4B为本发明一实施例的姿态教导的第二示意图，图4C为本发明一实施例的姿态教导的第三示意图。图4A至图4C用以示例性说明本发明如何触发姿态教导，以及如何于姿态教导过程中进行力补偿。

如图4A所示，自动化机械系统于进入教导状态40后切换至姿态维持模式400，并维持第一姿态(如执行第一姿态的姿态参考命令)且为静止状态。当用户欲执行姿态教导时，可直接对机械设备1的传动机构12施加外力F1以改变机械设备1的第一姿态。于接受外力后，控制器3会依据外力F1计算新的姿态维持命令，并经由马达驱动器2控制多个马达11实现新的姿态维持命令(即增加了力矩M1来抵消外力F1)，以维持第一姿态。

接着，如图4B所示，当控制器3侦测到第一姿态的姿态参考命令与新的姿态维持命令具有差异(即增加了力矩M1)时，判定用户有姿态教导的意图，切换至动力维持模式401，并根据力矩M1计算出外力F1的方向。

于动力维持模式401下，控制器3依据外力F1的方向决定补偿力矩M2(如设定为最大静摩擦力)并产生对应的动力维持命令。于经由马达驱动器2执行动力维持命令后，由于马达11会朝外力F1的方向提供补偿力矩M2，用户可省力地移动传动机构12来使机械设备1摆出期望的第二姿态(如图4C所示)。

借此，本发明可供用户便捷地触发姿态教导，且省力地进行姿态教导。

续请一并参阅图5及图6，图6为本发明的第二实施例的教导位置与姿态的触发与补偿方法的部分流程图。于本实施例中，各姿态是以机械设备11摆出此姿态时各马达11的当时位置来做成记录，即前述当前姿态包括各马达11的当前位置，前述第一姿态包括各马达11的第一位置，前述第二姿态包括各马达11的第2位置。

相较于图5所示的该方法，本实施例的该方法的步骤S11还包括以下步骤。

步骤S20：于姿态维持模式400下，控制器3每隔一周期取得机械设备的当前姿态。

步骤S21：控制器3依据当前姿态与第一姿态之间的差异计算新的姿态维持命令。

于一实施例中，控制器3可计算各马达11的当前位置与第一位置之间的差异，并依据所算出的位置差异产生可使各马达11从当前位置移回至第一位置的各马达控制命令，以做为新的姿态维持命令。

步骤S22：控制器3依据新的姿态维持命令的各马达控制命令经由马达驱动器2控制各马达11转动以使机械设备成为第一姿态。

于一实施例中，前述各马达控制命令包括转动速度命令与转动方向命令。

借此，经由不断重复步骤S20-S22，本发明可是机械设备1于姿态维持模式下适应外力来维持指定的第一姿态。并且，因适应外力所改变的姿态维持命令的输出可用来侦测是否用户意图(即侦测用户是否开始姿态教导)。

续请一并参阅图5及图7，图7为本发明的第三实施例的教导位置与姿态的触发与补偿方法的部分流程图。相较于图5的该方法，本实施例的该方法的步骤S16包括以下步骤。

步骤S30：控制器3计算摩擦力补偿力，前述摩擦力补偿力是用来抵消或减轻机械设备1朝外力的方向进行教导移动所生的摩擦力。

于一实施例中，若机械设备1为移动中状态，摩擦力补偿力的量值可接近或等于动摩擦力的量值，但方向相反；若机械设备1为静止状态，摩擦力补偿力的量值可接近或等于最大静摩擦力的量值，但方向相反。

步骤S31：控制器3依据各马达的当前动力计算重力补偿力，前述重力补偿力是用来抵消或减轻机械设备的重力。

具体而言，由于传动机构12具有重量，于姿态教导过程中会因重力而垮掉或改变姿态，而导致用户必须克服传动机构12的重力才能移动机械设备1。故本发明针对机械设备1的重力进行补偿，可进一步节省用户进行姿态教导所需力量大小。

步骤S32：控制器3依据前述摩擦力补偿力及前述重力补偿力决定最终的补偿力。

于一实施例中，前述摩擦力补偿力与前述重力补偿力为力矩，控制器3计算前述摩擦力补偿力与前述重力补偿力的合力矩，作为最终的补偿力。

续请一并参阅图3，为本发明一实施例的计算补偿力的示意图。图3用以示例性说明本发明如何计算补偿力。

如图所示，马达驱动器2于每个通信周期取得所受外力的力矩F1，回馈力矩F1至机构重力补偿模块31及摩擦力补偿模块32。

具体而言，控制器3可由各马达11的位置信息P1、转向信息D1及/或转速信息V1来计算力矩F1，将位置信息P1回馈至机构重力补偿模块31，并将转向信息D1与转速信息V1回馈至摩擦力矩补偿模块32。

机构重力补偿模块31依据每个通信周期的位置信息P1可计算各马达11为负担机械设备1于当前姿态下的重力所需的重力补偿力矩F3。摩擦力矩补偿模块32可在每个通信周期依据转向信息D1及转速信息V1计算各马达11为消除上述摩擦力所需的摩擦力补偿力矩F2。

并且，为了避免摩擦力补偿力矩F2过大而使机械设备1移动过快，摩擦力补偿力矩F2会被回馈至安全模块33以进行调整并产生调整后的摩擦力补偿力矩F2’。

接着，控制器3综合重力补偿力矩F1及摩擦力补偿力矩F2’，计算并输出最终的控制力矩C1至马达驱动器2，并且马达驱动器2依据控制力矩C1对各马达11进行控制。由于控制力矩C1中包含了重力补偿力矩F3，因此当各马达11依据控制力矩C1转动时，可提供足够的支撑力而令机械设备1在该马达驱动器2的动力维持模式下维持稳定，不会因为支撑力不足而垮掉。再者，控制力矩C1中同时包含了调整后的摩擦力补偿力矩F2’，当使用者直接施力于机械设备1上且各马达11依据控制力矩C1转动时，各马达11可通过调整后的摩擦力补偿力矩F2’消除产生的摩擦力，而使得使用者可轻易地且安全地推动、拉动各传动机构12，进而以人力直接进行位置教导。

续请一并参阅图5、图7及图8，图8为本发明的第四实施例的教导位置与姿态的触发与补偿方法的部分流程图。相较于图5及图7所示的该方法，图8的实施例示对如何计算摩擦力补偿力(即步骤S30)做更详细的说明。本实施例的该方法是经由以下步骤来计算摩擦力补偿力。

步骤S400：控制器3判断是否各马达11受到外力而欲转动。

若在姿态维持模式下任一马达11未接受外力(例如此马达11的输出力矩未发生变化)，则控制器3执行步骤S401：判定用户没有直接施力于传动机构12上(即未开始进行姿态教导)或者虽施加外力，但外力没有作用于马达11，故此马达11不需进行摩擦力补偿，并设定马达的摩擦力补偿力为零。

若任一马达11接受外力，控制器3切换至动力维持模式，并执行步骤S402：判断各传动机构12的移动速度是否零(即传动机构12已克服最大静摩擦力而开始移动)。

若任一传动机构12的移动速度为零，则控制器3判定马达11未转动，或是虽转动，但提供动力未超过传动机构12的最大静摩擦力，而使传动机构12维持静止，并执行步骤S403：设定用以移动此传动机构12的马达的摩擦力补偿力为起始摩擦力(如接近或等于最大静摩擦力)。

若任一传动机构12的移动速度大于零，则控制器3判定马达11所提供动力曾经超过传动机构12的最大静摩擦力，而使传动机构12开始移动，并执行步骤S404：设定用以移动此传动机构12的马达的摩擦力补偿力为动摩擦力。前述动摩擦力小于前述起始摩擦力

于一实施例中，控制器3可依据各马达11的当前转动速度所对应的动摩擦系数与微动摩擦力计算前述动摩擦力。以力矩为例，可如下述式(一)计算：

动摩擦力矩＝微动摩擦力矩＋动摩擦系数×目前转动速度……式(一) 。

接着，控制器3执行步骤S405：判断是否任一传动机构12的移动速度大于安全速度。

若传动机构12的移动速度不大于安全速度，则控制器3不须执行安全功能，即执行步骤S406：直接输出所计算的摩擦力补偿力。

若任一传动机构12的移动速度大于安全速度，则控制器3执行安全功能，即步骤S407：控制器3调降摩擦力补偿力，以降低传动机构12的移动速度。

借此，本发明可依据机械设备1的当前移动状态来调整摩擦力补偿力，而提升用户进行姿态教导时的用户体验。

续请一并参阅图9及图10，图9为本发明的第五实施例的教导位置与姿态的触发与补偿方法的第一部分流程图，图10为本发明的第五实施例的教导位置与姿态的触发与补偿方法的第二部分流程图。本实施例的该方法包括以下步骤。

步骤S500：自动化机械系统接受操作(如经由人机接口接受进入教导状态操作)，并受控制器3控制来切换至教导状态40。

接着，控制器3执行步骤S501-S508以于姿态维持模式下侦测用户是否开始执行姿态教导，并于侦测到用户的开始意图时自动切换至动力维持模式来提供补偿力并接受姿态教导。图9的步骤S501-S508是与图5的步骤S10-S17相同或相似，其执行细节不再重复叙述。

接着，控制器3执行步骤S509：于动力维持模式下侦测到机械设备1是否停止变换位置与姿态，如经由各马达11的当前位置是否继续发生变化来侦测用户是否完成位置与姿态教导。

若机械设备1未停止变换位置与姿态，则控制器3再次执行步骤S509以持续侦测。

若机械设备1停止变换位置与姿态，则控制器3执行步骤S510：记录当前姿态(第二姿态)，如记录各马达11的当前位置，并作为第二姿态的位置。

接着，控制器3执行步骤S511：控制器3判断是否离开教导状态40，如用户操作人机接口来控制自动化机械系统离开教导状态40，或是自动化机械系统于完成姿态教导后自动离开教导状态40。

若未离开教导状态40，则控制器3控制自动化机械系统套至步骤S501，即切换回姿态维持模式400以对新的教导意图进行侦测。

若离开教导状态40，则控制器3执行步骤S512：控制自动化机械系统切换至工作状态，并进入姿态维持模式410。

于一实施例中，用户可操作人机接口来控制自动化机械系统进入工作状态41，或是自动化机械系统于完成姿态教导后自动进入工作状态41。

接着，控制器3执行步骤S513：控制器3判断是否收到任一工作控制命令。

于一实施例中，前述工作控制命令可包括各马达11的马达控制命令(如位置命令、转向命令及/或转速命令)。

接着，控制器3执行步骤S514：执行工作控制命令以控制各马达11转动至工作控制命令所指定的位置，并摆出工作控制命令所对应的姿态。

于一实施例中，工作控制命令的其中之一可以是要求机械设备1摆出第二姿态(即通过前述姿态教导所学习的姿态)的命令，而于步骤S514中控制器3是控制各马达11转动至于教导状态40所记录的第二姿态的位置。

步骤S515：控制器3判断是否结束工作，如用户可操作人机接口来控制自动化机械系统离开工作状态41，或是自动化机械系统于完成工作后自动离开工作状态41。

若判定结束工作，则控制器3结束该方法的执行。若判定未结束工作，则控制器3再次执行步骤S513。

借此，本发明可将教导状态40下所学习的姿态用于工作状态41。

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

Claims (9)

1.一种教导位置与姿态的触发与补偿方法，其特征在于，用于包含一控制器、一马达驱动器及一机械设备的一自动化机械系统，该机械设备包括用以变换位置与姿态的多个马达，该教导位置与姿态的触发与补偿方法包括以下步骤：

a) 于一姿态维持模式下，该控制器每隔一周期计算并执行新的姿态维持命令以使该机械设备维持在一第一姿态，其中该姿态维持命令包括各该马达的一马达控制命令，并且包括：

a1) 于该姿态维持模式下，该控制器每隔该周期取得该机械设备的一当前姿态；

a2) 依据该当前姿态与该第一姿态之间的一差异计算新的该姿态维持命令；及

a3) 依据新的该姿态维持命令的各该马达控制命令控制各该马达转动以使该机械设备为该第一姿态，其中各该马达控制命令包括一转动速度命令与一转动方向命令；

b) 于新的该姿态维持命令的输出力矩与该第一姿态所对应的一参考姿态命令的输出力矩之间具有差异时，判定该机械设备接受一外力，决定该外力的方向并切换至一动力维持模式；

c) 于该动力维持模式下，决定该机械设备的各该马达的当前动力；

d) 依据该外力的方向决定一补偿力；及

e) 依据该补偿力及各该马达的该当前动力计算一动力维持命令并经由该马达驱动器执行该动力维持命令以减轻朝该外力方向调整该机械设备为一第二姿态的阻力，其中该动力维持命令包括各该马达的该马达控制命令。

2.如权利要求1所述的教导位置与姿态的触发与补偿方法，其特征在于，该当前姿态包括各该马达的当前位置，该第一姿态包括各该马达的一第一位置；

该步骤a2)是计算各该马达的该当前位置与该第一位置之间的该差异，并依据该差异产生能够使各该马达从该当前位置移回至该第一位置的各该马达控制命令，以做为新的该姿态维持命令。

3.如权利要求1所述的教导位置与姿态的触发与补偿方法，其特征在于，于该姿态维持模式下，新的该姿态维持命令是用以控制各该马达改变转动方式以抵消该机械设备所受该外力，该参考姿态命令是用以控制各该马达转动方式以使该机械设备于未受外力下摆出该第一姿态。

4.如权利要求1所述的教导位置与姿态的触发与补偿方法，其特征在于，多个该姿态维持命令是分别用以提供多个输出力矩；该步骤b)是计算新的该姿态维持命令所提供的该输出力矩及该参考姿态命令所提供的该输出力矩之间的一力矩差异，于该力矩差异大于一力矩临界值时，依据该力矩差异决定该外力的方向并切换至该动力维持模式。

5.如权利要求1所述的教导位置与姿态的触发与补偿方法，其特征在于，该步骤d)包括一步骤d1) 计算为了抵消或减轻该机械设备朝该外力的方向进行教导移动所生的摩擦力所需的一摩擦力补偿力。

6.如权利要求5所述的教导位置与姿态的触发与补偿方法，其特征在于，该多个马达分别用以带动多个传动机构移动以变换该机械设备的姿态，该步骤c)是取得各该传动机构的一移动速度；

该步骤d1)包括以下步骤：

d11) 于该马达未受外力时，设定该马达的该摩擦力补偿力为零；

d12) 于任一该传动机构的该移动速度为零时，设定对应的该马达的该摩擦力补偿力为一起始摩擦力；

d13) 于任一该传动机构的该移动速度大于零时，设定对应的该马达的该摩擦力补偿力为一动摩擦力；及

d14) 于任一该传动机构的该移动速度大于一安全速度时，调降该摩擦力补偿力。

7.如权利要求5所述的教导位置与姿态的触发与补偿方法，其特征在于，该步骤d)包括以下步骤：

d2) 依据各该马达的该当前动力计算为了抵消或减轻该机械设备的重力所需的一重力补偿力；及

d3) 依据该摩擦力补偿力及该重力补偿力决定最终的该补偿力。

8.如权利要求1所述的教导位置与姿态的触发与补偿方法，其特征在于，还包括以下步骤：

f) 于该动力维持模式下侦测到该机械设备停止变换位置与姿态时，记录各该马达的一当前位置，并作为该第二姿态的位置；及

g) 切换至该姿态维持模式。

9.如权利要求8所述的教导位置与姿态的触发与补偿方法，其特征在于，还包括以下步骤：

h) 于该自动化机械系统被操作来切换至一教导状态时，重复执行该步骤a)至该步骤g)；及

i) 于该自动化机械系统被操作来切换至一工作状态且收到要求该机械设备摆出该第二姿态的一工作控制命令时，控制各该马达转动至于该教导状态所记录的该第二姿态的位置。

Images