CN107614209A - 机器人装置及步进马达控制装置 - Google Patents

机器人装置及步进马达控制装置 Download PDF

Info

Publication number
CN107614209A
CN107614209A CN201680030783.5A CN201680030783A CN107614209A CN 107614209 A CN107614209 A CN 107614209A CN 201680030783 A CN201680030783 A CN 201680030783A CN 107614209 A CN107614209 A CN 107614209A
Authority
CN
China
Prior art keywords
imbalance
track
focus
stepper motor
command value
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN201680030783.5A
Other languages
English (en)
Inventor
尹祐根
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Life Robotics Inc
Original Assignee
Life Robotics Inc
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Life Robotics Inc filed Critical Life Robotics Inc
Publication of CN107614209A publication Critical patent/CN107614209A/zh
Pending legal-status Critical Current

Links

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J13/00—Controls for manipulators
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J13/00—Controls for manipulators
    • B25J13/08—Controls for manipulators by means of sensing devices, e.g. viewing or touching devices
    • B25J13/087—Controls for manipulators by means of sensing devices, e.g. viewing or touching devices for sensing other physical parameters, e.g. electrical or chemical properties
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/40—Robotics, robotics mapping to robotics vision
    • G05B2219/40512—Real time path planning, trajectory generation
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/40—Robotics, robotics mapping to robotics vision
    • G05B2219/40516—Replanning
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/40—Robotics, robotics mapping to robotics vision
    • G05B2219/40519—Motion, trajectory planning
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y10—TECHNICAL SUBJECTS COVERED BY FORMER USPC
    • Y10S—TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y10S901/00—Robots
    • Y10S901/19—Drive system for arm
    • Y10S901/23—Electric motor
    • Y10S901/24—Stepper motor

Abstract

本发明是以步进马达产生失调为前提而提供产生失调时的较佳应对。机器人装置具备具有关节部的机械臂机构、产生使关节部运转的动力的步进马达310、驱动步进马达的马达驱动器210、计算机械臂机构的关注点从当前位置移动到最终目标位置的轨道的轨道计算部105、向马达驱动器输出和轨道计算部计算的轨道相应的指令值的指令值输出部103、以及检测步进马达的失调的失调检测部216。进而，机器人装置具备系统控制部101。系统控制部在检测到失调时，再次计算从因失调而移位的关注点的位置到最终目标位置的轨道，为了根据再次计算的轨道使关注点移动，控制轨道计算部及指令值输出部。

Description

机器人装置及步进马达控制装置
技术领域
本发明的实施方式涉及一种机器人装置及步进马达控制装置。
背景技术
由于步进马达的旋转角和脉冲信号的数量成比例，所以基本上不需要反馈电路，就可以开环控制，较AC马达、DC马达而言有利，但反过来，如果施加过负荷或脉冲频率过高，则会同步偏移，产生控制紊乱，即所谓的“失调”。如果产生失调，步进马达会停止。而且，即便重新开始，要到达目标位置必须返回到基准位置，由此，不可避免地会产生停工时间。失调现象在步进马达中是不可避免的，为了不引起步进马达失调，先前已经进行了各种尝试，但大多数不能解决根本问题。
发明内容
本发明的目的在于以步进马达产生失调为前提，提供产生失调时的较佳应对。
本实施方式所涉及的机器人装置，具备：机械臂机构，其具有关节部；步进马达，其产生使关节部运转的动力；马达驱动器，其驱动步进马达；轨道计算部，其计算机械臂机构的关注点从当前位置移动到最终目标位置的轨道；指令值输出部，其将轨道计算部计算的轨道的相应指令值输出到马达驱动器；以及失调检测部，其检测步进马达的失调。此外，机器人装置具备系统控制部。当系统控制部检测到失调时，再次计算从因失调而移位的关注点的位置到最终目标位置的轨道，为了根据再次计算出的轨道移动关注点，控制轨道计算部及指令值输出部。
附图说明
图1是本实施方式所涉及的机器人装置的机械臂机构的外观立体图；
图2是表示图1的机械臂机构的内部构造的侧视图；
图3是通过图中符号表现来表示图1的机械臂机构的构成的图；
图4是表示本实施方式所涉及的机器人装置的构成的框图；
图5是表示图4的动作控制装置失调时的处理次序的流程图；
图6是表示图5的失调后的多个轨道的图；
图7是表示图5的步骤S13的次序的流程图。
具体实施方式
以下，参照附图，对本实施方式所涉及的机器人装置进行说明。本实施方式所涉及的机器人装置具备动作控制装置。动作控制装置作为单独的装置发挥功能，控制装备在机器人装置的多关节臂机构的各个关节的马达驱动器。而且，动作控制装置也可以组入具备多关节臂机构的机器人装置。本实施方式所涉及的机器人装置的动作控制装置可以应用于多种形态的多关节臂机构。此处，以多个关节部之一是直动伸缩关节的多关节臂机构为一个例子进行说明。在以下的说明中，对具有大致相同的功能及构成的构成要素标注相同符号，而且仅在必要的情形时进行重复说明。
图1是本实施方式所涉及的机械臂机构的外观立体图。机械臂机构具有大致圆筒形状的基部10、连接于基部10的臂部2以及安装于臂部2的前端的腕部4。在腕部4设置有未图示的转接器。例如，转接器设置在下述第六旋转轴RA6的旋转部。设置在腕部4的转接器，安装有和用途相对应的机械手。
机械臂机构具有多个、此处为六个关节部J1、J2、J3、J4、J5、J6。多个关节部J1、J2、J3、J4、J5、J6从基部10依序配设。一般而言，第一、第二、第三关节部J1、J2、J3被称为根部三轴，第四、第五、第六关节部J4、J5、J6被称为改变机械手姿势的腕部三轴。腕部4具有第四、第五、第六关节部J4、J5、J6。构成根部三轴的关节部J1、J2、J3中的至少一个是直动伸缩关节。这里，第三关节部J3构成为直动伸缩关节部、特别是伸缩距离相对较长的关节部。臂部2表示直动伸缩关节部J3(第三关节部J3)的伸缩部分。
第一关节部J1是以例如被基座面垂直支撑的第一旋转轴RA1为中心的扭转关节。第二关节部J2是以和第一旋转轴RA1垂直配置的第二旋转轴RA2为中心的弯曲关节。第三关节部J3是以和第二旋转轴RA2垂直配置的第三轴(移动轴)RA3为中心，臂部2直线伸缩的关节。
第四关节部J4是以第四旋转轴RA4为中心的扭转关节。第四旋转轴RA4在下述第七关节部J7不旋转时，即臂部2的全体处于直线形状时，和第三移动轴RA3大致一致。第五关节部J5是以和第四旋转轴RA4正交的第五旋转轴RA5为中心的弯曲关节。第六关节部J6是以和第四旋转轴RA4正交、且和第五旋转轴RA5垂直配置的第六旋转轴RA6为中心的弯曲关节。
形成基部10的臂支撑体(第一支撑体)11a具有以第一关节部J1的第一旋转轴RA1为中心形成的圆筒形状的中空构造。第一关节部J1安装在未图示的固定台。当第一关节部J1旋转时，臂部2随着第一支撑体11a的轴旋转而左右旋转。另外，第一支撑体11a可以固定在接地层。在这种情况下，设置为和第一支撑体11a分开而臂部2独立旋转的构造。在第一支撑体11a的上部连接有第二支撑部11b。
第二支撑部11b具有和第一支撑部11a连续的中空构造。第二支撑部11b的一端安装在第一关节部J1的旋转部。第二支撑部11b的另一端开放，第三支撑部11c在第二关节部J2的第二旋转轴RA2自由旋转地嵌入。第三支撑部11c具有由和第一支撑部11a和第二支撑部连通的鳞片状的外装形成的中空构造。第三支撑部11c伴随第二关节部J2的弯曲旋转，其后部被收纳或送出至第2支撑部11b。构成机械臂机构的直动伸缩关节部J3(第三关节部J3)的臂部2的后部是通过其收缩而被收纳在第一支撑部11a和第二支撑部11b的连续的中空构造的内部。
第三支撑部11c是其后端下部以第二旋转轴RA2为中心自由旋转地嵌入到第二支撑部11b的开放端下部。由此，构成作为以第二旋转轴RA2为中心的弯曲关节部的第二关节部J2。当第二关节部J2转动时，臂部2以第二旋转轴RA2为中心朝垂直方向转动、即进行起伏动作。
第四关节部J4是具有和沿着臂部2的伸缩方向的臂中心轴、即第三关节部J3的第三移动轴RA3典型地相接的第四旋转轴RA4的扭转关节。当第四关节部J4旋转时，腕部4和安装于腕部4的机械手以第四旋转轴RA4为中心旋转。第五关节部J5是具有和第四关节部J4的第四旋转轴RA4正交的第五旋转轴RA5的弯曲关节部。当第五关节部J5旋转时，从第五关节部J5遍及前端地和机械手一起上下(以第五旋转轴RA5为中心朝垂直方向)转动。第六关节部J6是具有和第四关节部J4的第四旋转轴RA4正交、且和第五关节部J5的第五旋转轴RA5垂直的第六旋转轴RA6的弯曲关节。当第六关节部J6旋转时，机械手左右旋转。
如上所述，安装在腕部4的转接器的机械手是通过第一、第二、第三关节部J1、J2、J3移动到任意位置，且通过第四、第五、第六关节部J4、J5、J6配置成任意姿势。特别是，第三关节部J3的臂部2的伸缩距离的长度可以使机械手到达从基部10的附近位置到远距离位置的大范围的对象。第三关节部J3的特征是通过构成其的直动伸缩机构而实现的直线伸缩动作及其伸缩距离的长度。
(内部构造的说明)图2
图2是表示图1的机械臂机构的内部构造的立体图。直动伸缩机构具有臂部2及射出部30。臂部2具有第一连结链节排21及第二连结链节排22。第一连结链节排21包含多个第一连结链节23。第一连结链节23构成为大致平板形。前后的第一连结链节23在相互的端部部位通过销弯曲自如地呈排状连结。第一连结链节排21可以朝内侧或外侧自如地弯曲。
第二连结链节排22包含多个第二连结链节24。第二连结链节24构成为横剖面“コ”字形状的短槽状体。前后的第二连结链节24在相互的底面端部部位通过销弯曲自如地呈排状连结。第二连结链节排22可以朝内侧弯曲。第二连结链节24的剖面为“コ”字形状，所以第二连结链节排22和相邻的第二连结链节24的侧板彼此冲突，不会朝外侧弯曲。另外，将面向第一、第二连结链节23、24的第二旋转轴RA2的面设为内面，将其相反侧的面设为外面。第一连结链节排21中最前端的第一连结链节23、及第二连结链节排22中最前端的第二连结链节24通过结合链节27连接。例如，结合链节27具有第二连结链节24和第一连结链节23合成后的形状。
射出部30通过在角筒形状的框架35上支撑多个上部辊31和多个下部辊32而形成。例如，多个上部辊31隔开和第一连结链节23的长度大致相同的间隔，沿着臂中心轴排列。同样地，多个下部辊32隔开和第二连结链节24的长度大致相同的间隔，沿着臂中心轴排列。导辊40和驱动齿轮50以隔着第一连结链节排21相对的方式设置在射出部30的后方。驱动齿轮50通过未图示的减速器连接于步进马达330。线性齿轮沿着连结方向在第一连结链节23的内面形成。多个第一链接链节23呈直线状整齐排列时相互的线性齿轮呈直线状相连，而构成较长的线性齿轮。驱动齿轮50啮合于直线状的线性齿轮。直线状相连的线性齿轮和驱动齿轮50一起构成齿轮齿条机构。
(接合构造的说明)
当臂伸长时，马达55驱动，驱动齿轮50正向旋转，第一连结链节排21通过导辊40变成和臂中心轴平行的姿势，被引导至上部辊31和下部辊32之间。随着第一连结链节排21的移动，第二连结链节排22通过配置在射出部30后方的未图示的导轨被引导至射出部30的上部辊31和下部辊32之间。被引导至上部辊31和下部辊32之间的第一、第二连结链节排21、22相互挤压。由此，第一、第二连结链节排21、22构成柱状体。射出部30连接第一、第二连结链节排21、22构成柱状体，并上下左右地支撑柱状体。第一、第二连结链节排21、22的接合状态通过射出部30保持第一、第二连结链节排21、22接合形成的柱状体而被保持。当维持第一、第二连结链节排21、22的接合状态时，第一、第二连结链节排21、22的弯曲相互约束。由此，第一、第二连结链节排21、22构成具备一定刚性的柱状体。所谓柱状体是指将第二连结链节排22和第一连结链节排21接合而形成的柱状的棒体。在该柱状体中，第二连结链节24和第一连结链节23一起整体构成各种剖面形状的筒状体。筒状体被定义为上下左右被天板、底板和两侧板包围，前端部和后端部开放的形状。第一、第二连结链节排21、22接合而成的柱状体以结合链节27为始端，沿着第三移动轴RA3直线地从第三支撑体11c的开口朝外被送出。
当臂收缩时，马达55驱动，驱动齿轮50逆向旋转，和驱动齿轮50卡合的第一连结链节排21被拉回到第一支撑体11a中。随着第一连结链节排的移动，柱状体被拉回到第三支撑体11c中。拉回的柱状体在射出部30的后方分离。例如，构成柱状体的第一连结链节排21夹持在导辊40和驱动齿轮50之间，构成柱状体的第二连结链节排22因为重力被拉向下方，由此第二连结链节排22和第一连结链节排21相互远离。远离后的第一、第二连结链节排21、22分别恢复成可弯曲的状态，分别朝相同方向的内侧弯曲，大致平行地容纳在第一支撑体11a的内部。
(图中符号显示)图3
图3是通过图中符号表现来表示图1的机械臂机构的图。在机械臂机构中，通过构成根部三轴的第一关节部J1、第二关节部J2和第三关节部J3，实现三个位置自由度。通过构成腕部三轴的第四关节部J4、第五关节部J5及第六关节部J6实现三个姿势自由度。
机器人坐标系Σb是以第一关节部J1的第一旋转轴RA1上的任意位置为原点的坐标系。在机器人坐标系Σb中，规定有正交三轴(Xb、Yb、Zb)。Zb轴是和第一旋转轴RA1平行的轴。Xb轴和Yb轴相互正交、且和Zb轴正交的轴。末端坐标系Σh是以安装在腕部4的机械手5的任意位置(末端基准点)为原点的坐标系。例如，当机械手5是二指手时，末端基准点(以下仅称为末端)的位置被规定为二指尖间中央位置。在末端坐标系Σh中，规定了正交三轴(Xh、Yh、Zh)。Zh轴是和第六旋转轴RA6平行的轴。Xh轴和Yh轴相互正交、并且和Zh轴正交。例如，Xh轴是和机械手5的前后方向平行的轴。末端姿势是相对于末端坐标Σh的机器人坐标系Σb的绕各个正交三轴的旋转角(绕Xh轴的旋转角(侧倾角)α、绕Yh轴的旋转角(俯仰角)β、绕Zh轴的旋转角(摆动角)γ)。
第一关节部J1设置在第一支撑部11a和第二支撑部11b之间，构成以旋转轴RA1为中心的扭转关节。旋转轴RA1垂直地设置在设置有第一关节部J1的固定部的基座的基准面BP上。
第二关节部J2构成以旋转轴RA2为中心的弯曲关节。第二关节部J2的旋转轴RA2平行于空间坐标系上的Xb轴设置。第二关节部J2的旋转轴RA2设置在和第一关节部J1的旋转轴RA1垂直的方向。此外，第二关节部J2相对于第一关节部J1，沿第一旋转轴RA1的方向(Zb轴方向)和第一旋转轴RA1垂直的Yb轴方向两个方向偏移。以第二关节部J2相对于第一关节部J1在所述两个方向偏移的方式，将第二支撑部11b安装在第一支撑体11a。将第一关节部J1与第二关节部J2连接的虚设的臂杆部分(链路部分)具有前端垂直弯曲的两个钩状体组合形成的曲柄形状。该虚设的臂杆部分由具有中空构造的第一、第二支撑体11a、11b构成。
第三关节部J3构成以移动轴RA3为中心的直动伸缩关节。第三关节部J3的移动轴RA3设置在和第二关节部J2的旋转轴RA2垂直的方向。当第二关节部J2的旋转角是零度时、即臂部2的起伏角是零度时，臂部2在水平的基准姿势时，第三关节部J3的移动轴RA3设置在和第二关节部J2的旋转轴RA2以及第一关节部J1的旋转轴RA1垂直的方向。在空间坐标系上，第三关节部J3的移动轴RA3平行于相对于Xb轴及Zb轴垂直的Yb轴。此外，第三关节部J3相对于第二关节部J2在其旋转轴RA2的方向(Yb轴方向)和与移动轴RA3正交的Zb轴的方向的两个方向上偏移。以第三关节部J3相对于第二关节部J2在所述两个方向偏移的方式，将第三支撑体11c安装在第二支撑体11b上。第二关节部J2连接第三关节部J3的虚设的臂杆部分(链路部分)具有前端垂直弯曲的钩状体。该虚设的臂杆部分由第二、第三支撑体11b、11c构成。
第四关节部J4构成为以旋转轴RA4为中心的扭转关节。第四关节部J4的旋转轴RA4以和第三关节部J3的移动轴RA3大致相同的方式配置。
第五关节部J5构成为以旋转轴RA5为中心的弯曲关节。第五关节部J5的旋转轴RA5以和第三关节部J3的移动轴RA3和第四关节部J4的旋转轴RA4大致正交的方式配置。
第六关节部J6构成为以旋转轴RA6为中心的扭转关节。第六关节部J6的旋转轴RA6以和第四关节部J4的旋转轴RA4和第五关节部J5的旋转轴RA5大致正交的方式配置。第六关节部J6是为了使作为末端效应器的机械手5左右转动而设置。另外，第六关节部J6也可以构成为与其旋转轴RA6和第四关节部J4的旋转轴RA4与第五关节部J5的旋转轴RA5大致正交的弯曲关节。
这样，将多个关节部J1－J6的根部三轴中的一个弯曲关节部更换为直动伸缩关节部，使第二关节部J2在两个方向上相对于第一关节部J1偏移，而且使第三关节部J3在两个方向上相对于第二关节部J2偏移，由此，本实施方式所涉及的机器人装置的机械臂机构在构造上消除临界点姿势。
(框图)图4
图4是表示本实施方式所涉及的机器人装置的构成的框图。在本实施方式所涉及的机器人装置的机械臂机构的关节部J1、J2、J3、J4、J5、J6分别设置有步进马达310、320、330、340、350、360作为致动器。在步进马达310、320、330、340、350、360电性连接有驱动器单元210、220、230、240、250、260。通常，驱动器单元210、220、230、240、250、260和控制对象的步进马达310、320、330、340、350、360一起设置。这些驱动器单元210、220、230、240、250、260具有相同构成，根据动作控制装置100的控制信号，对控制对象步进马达执行相同的操作。这里，只对驱动器单元210进行说明，省略对其它驱动器单元220、230、240、250、260的说明。
驱动器单元210控制步进马达310的驱动和停止。驱动器单元210具有控制部211、电源电路212、脉冲信号产生部213、旋转编码器215、失调判定部216和计数器217。控制部211根据从动作控制装置100输入的控制信号，统括地控制驱动器单元210。
具体而言，从动作控制装置100向驱动器单元210中的控制部211输入和步进马达310的旋转角的经过单位时间Δt后的目标位置(区别于下述的最终目标位置)相关的位置指令信号。控制信号作为Δt之后的关节变量(关节角度，J3中为伸缩距离)而提供。控制部211基于步进马达310的旋转角的当前位置和目标位置，决定脉冲数Np。具体而言，控制部211基于步进马达310的旋转角的当前位置和目标位置计算旋转角度差，并以当前位置和目标位置的旋转角度差除以步进马达310的例如0.72度的步进角，由此决定脉冲信号的脉冲数Np。此外，控制部211基于所决定的脉冲数和单位时间Δt，决定脉冲频率fp。具体而言，控制部211在单位时间Δt上乘以脉冲数Np，并计算其倒数，由此决定脉冲频率fp。控制部211对脉冲信号产生部213输出和脉冲数相关的脉冲数指令信号及和脉冲频率相关的脉冲频率指令信号。脉冲信号产生部213对步进马达310的各相，按脉冲频率指令信号指令的频率、即该频率的倒数的周期，输出由脉冲数指令信号指令的脉冲数的脉冲信号。从脉冲信号产生部213输出的脉冲信号也被送入下述失调判定部216。
表示步进马达310的驱动电流值的驱动电流指令信号从动作控制装置100输入到控制部211。控制部211对电源电路212输出与驱动电流指令信号相应的控制信号。电源电路212通过使用从未图示的外部电源供给部提供的电力，产生具有指定的驱动电流值的驱动电流。将产生的驱动电流提供给步进马达310。步进马达310通过电源电路212提供的驱动电流驱动，并且根据从脉冲信号产生部213输出的脉冲信号旋转。
旋转编码器215连接在步进马达310的驱动轴，每隔例如0.18度的一定旋转角输出脉冲信号(编码器脉冲)。计数器217根据旋转方向对从旋转编码器215输出的编码器脉冲的数量进行加减计算，计算计数数。该计数数在步进马达310的驱动轴的基准位置(原点)被重设。根据重设次数和计数数可以求出关节部的关节角度(关节变量)。
失调判定部216比较和脉冲信号产生部213输出的脉冲信号相对的编码器脉冲的计数数，由此判定步进马达310是否产生失调。失调判定部216同步于脉冲信号重复地进行编码器脉冲的计数/重设。脉冲信号的一周期中旋转的步进角是固定的。对应于步进角的编码器脉冲的数量是固定的。如果编码器脉冲的计数数和对应于步进角的数字一致，那么步进马达310的旋转和脉冲信号同步、即没有产生失调。另一方面，如果编码器脉冲的计数数和步进角对应的数字不一致，那么步进马达310的旋转和脉冲信号不同步、即产生失调。当步进马达310产生失调时，失调判定部216产生失调检测信号。
动作控制装置100具有系统控制部101、操作部接口102、指令值输出部103、当前位置·姿势计算部104、轨道计算部105、指令值计算部106、动态计算部107、驱动电流决定部108和驱动器单元接口109。计数器217计数的计数数(表示从基准位置的旋转角)及重设次数(表示旋转次数)相关的数据以及从失调判定部216输出的失调检测信号从驱动器单元210通过驱动器单元接口109输入到动作控制装置100。本实施方式所涉及的机器人装置的动作控制装置100可以作为独立装置应用于使用步进马达的其它装置和机构。
(系统控制部)
系统控制部101具有CPU(Centra l Process i ng Un it)和半导体记忆体等，统括地控制动作控制装置100。各部通过控制/数据总线110连接到系统控制部101。
(操作部)
操作部50通过操作部接口102连接到动作控制装置100。操作部50用作输入接口，用于输入腕部4或机械手(末端效应器)的关注点的位置的变更、姿势的变更以及时间的变更。动作控制装置100将例如二指手的末端作为关注点(控制点)而执行移动·姿势变更等的计算处理。例如，操作部50具备指定使机械手移动的最终目标位置和移动时间的操纵杆等。例如，基于操纵杆的操作方向、操纵杆的倾斜角度、操纵杆的操作加速度，输入机械手的最终目标位置和移动时间。另外，构成操作部50的这些输入装置可以由其它装置、例如鼠标、键盘、轨迹球以及触控面板等代替。
(指令值输出部)
指令值输出部103根据系统控制部101的控制，将下述指令值计算部106计算出的各关节部J1－J6的指令值(Δt后的关节角度(关节变量))，分别输出到驱动器单元210、220、230、240、250、260。具体而言，指令值输出部103将下述指令值计算部106计算出的各关节部J1－J6的关节变量相应的位置指令信号，输出到驱动器单元210、220、230、240、250、260。此外，指令值输出部103将下述驱动电流决定部108决定的各驱动关节部J1－J6的驱动电流值相应的驱动电流指令信号，输出到驱动器单元210、220、230、240、250、260。
(当前位置·姿势计算部)
当前位置·姿势计算部104基于各关节部J1、J2、J3、J4、J5、J6的关节变量，根据臂构造的链路参数规定的同次变换矩阵，利用正向运动学计算从机器人坐标系观察的末端关注点的位置和姿势。另外，所谓关节变量，对于关节部J1、J2、J4、J5、J6而言是指从基准位置的正负的旋转角度，对于关节部J3则是指从最收缩状态的伸展距离(直动变位)。
当前位置·姿势计算部104基于由各驱动器单元210、220、230、240、250、260的计数器217计数的计数数，计算和关节部J1－J6相关的关节变量向量－θ。关节变量向量－θ是由旋转关节部J1、J2、J4、J5、J6的关节角度θJ1、θJ2、θJ4、θJ5、θJ6和直动伸缩关节部J3的伸缩长LJ3提供的关节变量组(θJ1、θJ2、LJ3、θJ4、θJ5、θJ6)。例如，当前位置·姿势计算部104保持在关节部J1的步进马达310启动前为止的累积计数数，并将启动中计数的计数数加至累积计数数，由此更新累积计数数。当前位置·姿势计算部104对累积计数数乘以和1计数对应的步进角度，由此计算从步进马达310的基准位置的旋转角度，从而指定关节部J1的关节变量。当前位置·姿势计算部104通过相同的方式指定其它关节部J2－J6的关节变量。
当前位置·姿势计算部104通过同次变换矩阵K(参数(θJ1、θJ2、LJ3、θJ4、θJ5、θJ6))，计算从机器人坐标系Σb观察的末端关注点的位置(x、y、z)和末端姿势(α、β、γ)。同次变换矩阵K是对末端坐标系Σh和机器人坐标系Σb关系进行定义的矩阵式。同次变换矩阵K由构成机械臂机构的链路间的关系(链路长和链路的扭转角)和关节部的轴间的关系(链路间距离和链路之间角度)决定。例如，当前位置·姿势计算部104通过将当前的机械臂机构的关节变量向量－θ0(θ0－J1、θ0－J2、L0－J3、θ0－J4、θ0－J5、θ0－J6)代入同次变换矩阵K，计算从机器人坐标系Σb观察的末端的当前位置P0(x0、y0、z0)和末端姿势φ0(α0、β0、γ0)。
(轨道计算部)
轨道计算部105基于末端的当前位置·姿势和末端的最终目标位置·姿势，计算连结这些的每隔单位时间Δt(控制周期，例如10ms)的末端的目标位置的点列。末端的当前位置·姿势从当前位置·姿势计算部104的计算处理获得。末端的最终目标位置·姿势和移动时间通过例如操作部50由使用者输入。轨道计算部105向事先预设的以末端的当前位置和当前姿势、末端的最终目标位置和最终目标姿势作为参数的函数代入各参数，由此计算末端的轨道(以下称为末端轨道)，并计算这个末端轨道上每隔单位时间Δt的目标位置。作为轨道计算方法，可以采用任意方法。单位时间Δt作为控制周期是固定值，被使用者设定为例如10ms。轨道计算部105基于从末端的当前位置移动到最终目标位置所需要的移动时间宽度T和单位时间Δt，决定目标位置的数m(＝T/Δt)，并且计算末端轨道上每隔单位时间Δt的目标位置(p1、p2···pm(m＝T/Δt))。这里，目标位置是提供末端的位置和姿势两者的参数。
详细说明在后文中叙述，轨道计算部105在检测到失调时，可以基于系统控制部101的控制再次计算五种轨道。第一至第三轨道是因为失调而从本来的轨道移位的手的关注点的位置(重新开始移动位置)经过不同路径到达最终目标位置的轨道。第四、第五轨道是从重新开始移动位置经过不同路径返回到移动开始时的位置的轨道。当检测到失调时，从这五个轨道中选择一个轨道，并沿着这个轨道进行移动控制。
另外，在轨道计算部105，轨道计算方法是任意的，而且可以事先默认多个函数。例如，轨道计算部105根据统控制部101的控制，变更用于末端轨道计算的函数。例如，轨道计算部105使用和通常的末端轨道的计算用函数不同的函数，再次计算失调检测后的末端轨道。例如，失调检测后再次计算的末端轨道是等价于将失调检测后的位置P0'和最终目标位置P1链接的最短路径。失调检测后再次计算的末端轨道具有从失调检测后的位置向失调检测时的位置将末端拉回特定距离的轨道。
(指令值计算部)
指令值计算部106计算和所计算出的轨道上以Δt周期排列的目标位置分别对应的多个关节变量向量。另外，所谓关节变量向量是指关节部J1－J6的六个关节变量、即旋转关节部J1、J2、J4－J6的旋转角和直动伸缩关节部J3的臂伸缩长的六个变量。关于指令值计算部106的计算处理在后文中进行叙述。
(动态计算部)
动态计算部107计算使末端从目标位置移动到Δt后的下一个目标位置所需要的分别设置在关节部J1－J6的各个步进马达310～360的扭矩(驱动扭矩)。此外，动态计算部107计算为使臂部2静止各关节部所需要的扭矩(静止扭矩)。各关节部所需要的静止扭矩是对根据较该关节部靠前的部分的重心质量、该关节部的旋转轴到重心的距离等所计算的该关节部的负荷扭矩，逆向施加等价扭矩、即自重导致的负荷扭矩来实现均衡的扭矩。
各个步进马达310～360的必要扭矩通过和关节部J1－J6相关的动态模型资料，连结机械臂机构的关节部J1－J6的各链路的重心位置、重心质量、链路长、惯性张量、关节部的关节变量(旋转角位置或直动变位)、关节角速度、关节角加速度等计算。另外，动态模型是指和马达的动态特性相关的模型。
(驱动电流决定部)
驱动电流决定部108是为了产生动态计算部107计算出的扭矩而决定各个步进马达310、320、330、340、350、360的驱动电流值。驱动电流决定部108保持将各个步进马达310、320、330、340、350、360的扭矩大小和驱动电流值关联而成的对应表的资料。驱动电流决定部108参照对应表，决定和动态计算部107计算出的各个关节部J1－J6的扭矩大小分别对应的驱动电流值。
(失调时的应对次序)图5、图6
以下，参照图5，对本实施方式所涉及的机器人装置的动作控制装置100的失调时的应对次序进行说明，并参照图6进行补充说明。图5是用于说明图4的动作控制装置100失调时的处理次序的流程图。图6是表示第一到第五轨道(1)－(5)的图。动作控制装置100的各部根据系统控制部101的控制按照图5的流程图操作。
(步骤S11)最终目标位置等的输入
在末端移动时首先通过操作部接口102，输入末端关注点的最终目标位置。通过步骤S11，如图6所示，输入最终目标位置P1。此外，在失调产生后判定是否等待操作者的手动指示重新开始移动(手动重新开始的接通/断开)，在手动重新开始设定为断开时进一步设定到失调产生后重新开始移动的待机时间tw。另外，待机时间tw可以设定为0以上。将待机时间tw设定为0小时，当然意味着允许立即重新开始移动。此外，失调产生后返回到移动开始时的位置(移动开始位置)P0时，设定是否由操作者手动选择与返回路径不同的轨道(4)、(5)中的任意一个。
(步骤S12)末端轨道的计算处理
轨道计算部105计算从末端关注点的当前位置(移动开始位置)到步骤S11中输入的最终目标位置的末端轨道。轨道计算部105从末端关注点的当前位置到最终目标位置的末端轨道上设定每隔单位时间Δt的多个目标位置。在图6的例子中，计算将当前位置P0和最终目标位置P1连结的末端轨道，并且在该末端轨道上设定每隔单位时间Δt的多个目标位置(p1、p2、···pm－1、pm(＝P1))。末端关注点的当前位置P0是从前位置·姿势计算部104的计算处理获得。具体而言，当前位置·姿势计算部104基于各个驱动器单元210、220、230、240、250、260的计数器217计数的计数数，计算当前的关节变量向量－θ0(θ0－J1、θ0－J2、L0－J3、θ0－J4、θ0－J5、θ0－J6)，并且基于该关节变量向量－θ0，利用正向运动学计算出末端的当前位置P0。
(步骤S13)关节部J1－J6的指令值(关节变量向量)的计算处理
指令值计算部106计算出与步骤S12计算出的多个目标位置分别对应的各关节的角度、表示伸展距离的多个关节变量向量。如图6所示，步骤S13计算出和多个目标位置(p1、p2、···pm－1、pm(＝P1))分别对应的多个关节变量向量(－θ1、－θ2、···－θm)。关于指令值计算部106的计算处理次序在下文中详细叙述。
(步骤S14)指令值的输出处理
和步骤S13计算出的和关节变量向量对应的位置指令信号、及和驱动电流决定部108决定的驱动电流值对应的驱动电流指令信号，从指令值输出部103以特定的控制周期Δt(例如，10ms)依次输出到驱动器单元210、220、230、240、250、260。通过步骤S14，末端从当前位置P0每隔控制周期Δt依次通过目标位置p1、p2、···pm－1，移动到最终目标位置P1。
(步骤S15)末端是否到达最终目标位置的判定处理
通过系统控制部101判定末端是否到达最终目标位置。具体而言，通过当前位置·姿势计算部104，将每隔控制周期Δt计算的末端的当前位置和图6所示的最终目标位置P1进行比较。在末端的当前位置和最终目标位置一致时，判定末端到达最终目标位置。此时，动作控制装置100的一系列控制完成。另一方面，在末端的当前位置和末端的最终目标位置不一致时，使处理移行到步骤S16。
(步骤S16)失调产有无的判定处理
通过系统控制部101判定是否产生失调。具体而言，从失调判定部216向动作控制装置100输入失调检测信号时，处理移行到步骤S17。在图6中，将失调检测时点的末端的位置设为Pso，因某种外因、例如作业者触碰机械臂机构的臂部2，末端移动到位置P0'。另一方面，如果没有从失调判定部216输出失调检测信号，那么在末端的当前位置到达末端的最终目标位置之前，以特定的控制周期Δt依次输出位置指令信号和驱动电流指令信号。
(步骤S17)失调产生时末端轨道的再次计算处理
在步骤S17中，系统控制部101基于由于失调而移位的距离Δd、即当初的轨道上的失调检测时点的末端的位置Pso和失调产生后移位的末端位置P0'的距离(移位距离)Δd、以及从失调检测到失调消除能够重新开始移动的经过时间(n·Δt1)，决定失调检测后重新开始末端移动时维持当初的最终目标位置P1作为到达位置、还是从当初的最终目标位置P1变更为移动开始时的位置P0。判定是否满足失调所导致的移位距离Δd没有达到特定的阈值THd、而且失调检测到重新开始移动的经过时间(n·Δt1)没有达到特定的阈值THt的条件。以下，将该经过时间(n·Δt1)称为失调所导致的停工时间。另外，失调检测后，动作控制部100重复输出用于控制驱动器单元210而重新开始移动的指令值，重试重新开始移动。将该重复周期(重试周期)设为Δt1、将重复次数(重试次数)设为n。脉冲信号产生部213的脉冲信号数和编码脉冲数匹配、即脉冲信号和旋转同步的状态是失调消除后的状态，在这种状态下可以重新开始移动。重复进行重新开始移动的重试控制，直到达成该状态为止。
在步骤S17中，在末端位置不会因为失调从本来的轨道大幅偏移(移位距离Δd相对较短)、进而失调产生到消除为止不会经过长时间(停工时间n·Δt1相对较短)时，重新开始移动并指向当初的最终目标位置，另一方面，在末端位置因为失调从本来的轨道大幅偏移(移位距离Δd相对较长)及/或失调产生到消除经过长时间(停工时间n·Δt1相对较长)时，解除当初的最终目标位置，返回到移动开始位置，等待下一任务。
另外，为了判定移位距离Δd的长短和移位距离Δd比较的阈值THd、为了判定停工时间(n·Δt1)的长短和停工时间(n·Δt1)比较的阈值THt通过系统控制部101动态地变动。例如，当失调检测时的位置Pso较最终目标位置P1靠近移动开始位置P0时，设定为相对较小的值，当失调检测时的位置Pso较移动开始位置P0靠近最终目标位置P1时，设定为相对较大的值。
当步骤S17中判定为“YES”时，即重新开始移动并指向当初的最终目标位置时，在步骤S18中，系统控制部101等待重新开始移动，直到输入操作者的重新开始移动指示。或者，当步骤S11中将操作者的重新开始移动的手动指示设定为断开时，从失调检测时点开始等待步骤S11中设定的待机时间tw。这些是为了确保重新开始移动时的安全性所设定的。
在步骤S18中操作者输入重新开始移动指示时，或经过待机时间tw时，步骤S19－S21根据选择的轨道进行指令值输出，重新开始移动。实际上，轨道再计算步骤S19和轨道选择步骤S20是和步骤S18同时执行。在步骤S19中，图6所示的三种轨道(1)－(3)由轨道计算部105计算。
第一轨道(1)从因失调而移位的手的关注点的位置(重新开始移动时的位置)P0’，经过因失调而从移动开始位置P0到最终目标位置P1的当初的轨道偏移的路径后，返回当初的轨道上的失调检测位置Pso，并沿着当初的轨道到达最终目标位置。
第二轨道(2)从因失调而移位的手的关注点的位置(重新开始移动时的位置)P0’，移动到失调检测时的位置Pso和最终目标位置P1之间的位置，并沿着当初的轨道到达最终目标位置。第三轨道(3)是从因失调而移位的关注点的位置P0’到最终目标位置P1为止的直线的最短轨道。
其次，在步骤S20中，从这三种轨道(1)－(3)通过轨道计算部105或系统控制部101选择一个轨道。这里，从安全性的观点出发，分别将第一轨道(1)设定为第一位的优先顺位，将第二轨道(2)设定为第二位的优先顺位，将第三轨道(3)设定为第三位的优先顺位。选择条件为从当初的移动开始位置P0到最终目标位置P1的移动中本来设定的任务时间是否能够维持在小于对末端关注点的移动速度规定的速度上限值的范围内。如果假定轨道长最长、安全性最高的第一轨道(1)满足该选择条件，则选择第一轨道(1)。如果第一轨道(1)不满足该选择条件，假定轨道长第二长、安全性第二高的第二轨道(2)满足该选择条件，则选择第二轨道(2)。当第一、第二轨道(1)、(2)都不满足该选择条件，到最终目标位置P1的最短路径第三轨道(3)满足该选择条件时，选择第三轨道(3)。另外，当第三轨道(3)不满足该选择条件时，动作被任意设定，典型而言，可以在这个重新开始移动位置P0’停住，或者也可以选择第四轨道(4)，并返回到移动开始位置P0等待下一任务。
在步骤21中，由指令值计算部106计算表示与步骤S20中选择的轨道上的多个目标位置分别对应的多个关节变量向量的指令值，并以特定的控制周期Δt的周期，依序和表示驱动电流决定部108决定的各个位置的驱动电流值的指令信号，一并从指令值输出部103输出到驱动器单元210、220、230、240、250、260。
当步骤S17中判定为“NO”时，即重新开始移动、沿着返回轨道(4)、(5)的任意一个返回到原本的移动开始位置P0时，在步骤S22中，系统控制部101等待重新开始移动，直到操作者输入重新开始移动指示，或者等待从失调检测时点开始在步骤S11中设定的待机时间tw。
当步骤S22中操作者输入重新开始移动指示时，或经过待机时间tw时，在步骤S23中，将步骤S11中设定的“返回到移动开始位置P0的返回轨道(4)、(5)的手动选择功能”设定为断开时，在步骤S24中，通过轨道计算部105计算从因失调而移位的手的关注点的位置(重新开始移动时的位置)P0’返回到原本的移动开始位置P0的返回轨道(4)。
返回轨道(4)从重新开始移动时的位置P0’，经过因失调而从移动开始位置P0到最终目标位置P1的当初的轨道偏移的路径，返回到失调检测位置Pso，沿着当初的轨道到达原本的移动开始位置P0。另一方面，第五轨道(5)是从因失调而移位的关注点的位置(重新开始移动位置)P0’到原本的移动开始位置P0的直线的最短返回轨道。由于返回轨道(4)是之前刚通过的路程，所以相较于新的第五轨道(5)，轨道上介置障碍物等的可能性较低，即安全性较高。因此，当操作者没有手动进行轨道选择时，相比返回轨道(5)优先选择返回轨道(4)。
在步骤25中，通过指令值计算部106计算表示和步骤S24所计算的轨道(4)上的多个目标位置分别对应的多个关节变量向量的指令值，并以特定的控制周期Δt的周期，依序和表示驱动电流决定部108决定的各个位置的驱动电流值的指令信号，一起从指令值输出部103输出到驱动器单元210、220、230、240、250、260。
在步骤S23中，将步骤S11中设定的“返回至移动开始位置P0的返回轨道(4)、(5)的手动选择功能”设定为接通时，在步骤S26中，通过系统控制部101在操作部50的显示器上显示用于提醒操作者选择返回轨道(4)和第五轨道(5)的任意一个画面。当操作者选择返回轨道(4)时，移行到步骤S24。当操作者选择返回轨道(5)时，在步骤S28中通过轨道计算部105计算从因失调而移位的手的关注点的位置(重新开始移动时的位置)P0’返回到原本的移动开始位置P0的直线且最短的返回轨道(5)，并且移行到步骤S25。
所述步骤S14－S28重复进行，直到手5的末端关注点通过步骤S15到达最终目标位置P1、或返回到移动开始位置P0。
这样，机械臂的致动器采用步进马达，因其失调而停止动作，所以难以采用，但是如果像所述一样，当产生失调时可以再次计算从此时的位置到最终目标位置为止的轨道，并重新开始沿着再次计算的轨道的末端移动，而且在无法维持任务时间时，可以重新开始返回到原本的移动开始位置的末端移动，等待下一个任务指令。这些作用效果可以促进对机械臂的致动器采用步进马达。
采用本实施方式的失调时重新开始移动的控制，意味着将能够活用步进马达固有的特性的失调提高安全性的新颖构思实际地实用化。先前当作业者接触臂时，需要通过加速度感测器等检测接触来停止动作等的安全机构。但是，当作业者接触采用步进马达的机械臂时，因其负荷产生失调而停止动作。因此，能够确保和先前相同的安全性。
(指令值计算部的处理的说明)图7
其次，参照图7对步骤S21、S24的指令值计算部106的指令值计算处理进行说明。
(步骤S131)初始化(n＝0)
这里，为了便于说明，使用变量n。首先，将该变量n初始化为零。
(步骤S132)雅可比逆矩阵J－1(－θn)的计算
本实施方式的机器人装置的机械臂机构在构造上不存在临界点，因此始终存在雅可比逆矩阵。雅可比逆矩阵是将末端速度(末端位置·姿势的微小变化)变换成关节角速度(关节角度·伸缩长的微小变化)的矩阵。雅可比逆矩阵通过对表示末端的位置·末端姿势的向量的关节变量进行偏微分得到。指令值计算部106通过步骤S12中当前位置·姿势计算部104计算的当前的关节变量向量－θn(θn－J1、θn－J2、Ln－J3、θn－J4、θn－J5、θn－J6)，利用臂构造的链路参数计算雅可比逆矩阵J－1(－θn)。
(步骤S133)末端速度－pn+1的计算
基于当前的末端位置(当前的目标位置)pn、下一末端位置(单位时间Δt后的下一目标位置)pn+1、单位时间Δt，计算末端速度·pn+1。
(步骤S134)关节角速度－·θn+1的计算
通过雅可比逆矩阵J－1(－θn)，将在步骤S133计算的末端速度－pn+1变换成关节角速度－·θn+1。
(步骤S135)目标的关节变量向量－θn+1的计算
基于重复次数前的步骤S135计算的(变量n＝0时为步骤S11所计算)关节变量向量－θn、步骤S134计算的关节角速度－·θn+1、单位时间Δt，计算下一关节变量向量－θn+1。关节角速度－·θn+1乘以单位时间Δt，计算单位时间Δt之间的各个关节的变位量。通过在即将移动前的关节变量向量－θn加上关节的变位量，计算经过单位时间Δt后的关节变量向量－θn+1。
(步骤S136)处理继续的判定
当变量n为重复次数(m－1)时，结束指令值计算部106的计算处理。另一方面，当变量(n)不是重复次数(m－1)时，将处理移行到步骤S137。
(步骤S137)变量的提升n←n+1
将变量n提升为(n+1)，使处理返回到步骤S142。
通过以上说明的指令值计算部106的计算处理，计算和末端的目标位置(p1、p2、···pm)分别对应的关节变量向量(－θ1、－θ2、···－θm)。
(效果)
如上所述，在本实施方式所涉及的机器人装置中，在构成机械臂机构的关节部J1－J6分别具备步进马达310～360。这些步进马达310～360的操作由动作控制装置100控制。步进马达一般不用于产业用机器人，因为对步进马达施加大于步进马达的扭矩的负荷的话，会产生失调。但是，本实施方式所涉及的机器人装置在构造上没有临界点，因此臂不会在意外的时刻为了避免临界点而进行较大的旋转移动等，所以，可以作为例如和作业者协动进行作业的机器人装置使用。即，本实施方式所涉及的机器人装置可以接近作业者配置。因此，即使作业者通过使用步进马达作为关节部的致动器接触机器人装置的臂，如果对步进马达施加步进马达的必要扭矩以上的负荷，就会产生失调，所以臂也不会以步进马达的必要扭矩以上的较大力伤害作业者。因此，通过对关节部的致动器使用步进马达，与使用其他马达、例如AC马达等情形相比，可以减小臂接触作业者引起的作业者受伤等风险。
这样，在本实施方式所涉及的机器人装置的设想配置环境中，预先设想了作业者接触臂而步进马达失调的情况。在这种情况下，有失调后的位置从预定的末端轨道上偏移的情形。但是，本实施方式的机器人装置的动作控制装置100的轨道计算部105在步进马达310～360产生失调时，可以再次计算从失调检测后的末端的当前位置到末端的最终目标位置的末端轨道。因此，即使产生失调，也可以继续进行末端的移动控制，使末端从失调检测后的位置再次移动到最终目标位置。由此，即使产生失调，也不会中断末端的移动控制，而且不需要使末端暂时返回到基准位置，所以可以缩短停工时间。因此，本实施方式所涉及的机器人装置以步进马达产生失调为前提可以提供产生失调时的较佳应对。
本实施方式中，是以机器人装置为例进行步进马达控制的说明，但是也可以对其他动作对象、例如输送机装置的线移动的致动器采用步进马达，也可以应用于步进马达控制。计算作为动作对象的线从当前状态(当前速度)变化(速度变化)到目标状态(目标速度)变化的过程，并根据这个过程输出指令值。这个过程中步进马达产生失调时，再次计算产生失调时点的线的速度到目标速度的过程，并根据再次计算的过程使线的速度阶段性变化。在本实施方式的思想范围内同样适用。
虽然对本发明的几个实施方式进行了说明，但是这些实施方式只是作为例子提出的，并非用于限定本发明的范围。对于这些新的实施方式，能够以其他各种方式进行实施，在不脱离本发明的要旨的范围内，能够进行各种省略、置换、变更。这些实施方式和其变形，包含于本发明的范围和要旨的同时，也包含于权利要求书中记载的发明及其均等范围内。
附图标记说明
50：操作部
100：动作控制装置
101：系统控制部
102：操作部I/F
103：指令值输出部
104：当前位置·姿势计算部
105：轨道计算部
106：指令值计算部
107：动态计算部
108：驱动电流决定部
109：驱动器单元I/F
110：控制/数据总线
210～260：驱动器单元
310：步进马达
211：控制部
212：电源电路
213：脉冲信号产生部
215：编码器
216：失调判定部
217：计数器

Claims (13)

1.一种机器人装置，其特征在于具备：
机械臂机构，其具有关节部；
步进马达，其产生使所述关节部运转的动力；
马达驱动器，其驱动所述步进马达；
轨道计算部，其计算所述机械臂机构的关注点从作为当前位置的移动开始位置移动到最终目标位置的轨道；
指令值输出部，其向所述马达驱动器输出和所述轨道计算部计算的轨道相应的指令值；
失调检测部，其检测所述步进马达的失调；以及
系统控制部，其在检测到所述失调时，再次计算从因所述失调而移位的所述关注点的位置到所述最终目标位置的轨道，并控制所述轨道计算部及所述指令值输出部，使所述关注点根据所述再次计算的轨道移动。
2.根据权利要求1所述的机器人装置，其中通过所述轨道计算部，再次计算第一轨道及第二轨道，所述第一轨道是从因所述失调而移位的所述关注点的位置，返回到所述移动开始位置和所述最终目标位置的当初的轨道，并到达所述最终目标位置，所述第二轨道是从因所述失调而移位的所述关注点的位置到所述最终目标位置的直线轨道。
3.根据权利要求2所述的机器人装置，其中所述系统控制部基于为维持根据所述当初的轨道预定的任务时间所必需的所述关注点的移动速度，选择所述第一、第二轨道中的一个。
4.根据权利要求3所述的机器人装置，其中所述系统控制部在所述第一轨道预定的所述关注点的移动速度小于特定的上限值时优先选择所述第一轨道，在所述第一轨道预定的所述关注点的移动速度等于或高于特定的上限值时选择所述第二轨道。
5.根据权利要求4所述的机器人装置，其中所述系统控制部在从检测到所述失调的时点经过特定的等待时间之后，为了根据所述第一轨道或所述第二轨道重新开始所述关注点的移动，控制所述指令值输出部。
6.根据权利要求4所述的机器人装置，其中所述系统控制部在检测到所述失调后等待使用者的重新开始移动指示，为了根据所述第一轨道或所述第二轨道重新开始所述关注点的移动，控制所述指令值输出部。
7.根据权利要求1所述的机器人装置，其中通过所述轨道计算部再次计算第一轨道、第二轨道及第三轨道，所述第一轨道是从因所述失调而移位的所述关注点的位置，经由因所述失调从所述移动开始位置到所述最终目标位置的当初的轨道偏移的路径，返回到所述当初的轨道，并到达所述最终目标位置，所述第二轨道是从因所述失调而移位的所述关注点的位置经过与因所述失调而从所述当初的轨道偏移的路径不同的路径，返回到所述当初的轨道，并到达所述最终目标位置，所述第三轨道是从因所述失调而移位的所述关注点的位置到所述最终目标位置的直线轨道。
8.一种机器人装置，其特征在于具备：
机械臂机构，其具有关节部；
步进马达，其产生使所述关节部运转的动力；
马达驱动器，其驱动所述步进马达；
轨道计算部，其计算所述机械臂机构的关注点从作为当前位置的移动开始位置移动到最终目标位置的轨道；
指令值输出部，其向所述马达驱动器输出和所述轨道计算部计算的轨道相应的指令值；
失调检测部，其检测所述步进马达的失调；以及
系统控制部，其在检测到所述失调时，计算从因所述失调而移位的所述关注点的位置返回到所述移动开始位置的轨道，为了根据所述计算的轨道使所述关注点返回到所述移动开始位置而控制所述轨道计算部及所述指令值输出部。
9.一种步进马达控制装置，其特征在于具备：
机械臂机构，其具有关节部；
步进马达，其产生使所述关节部运转的动力；
马达驱动器，其驱动所述步进马达；
轨道计算部，其计算所述机械臂机构的关注点从作为当前位置的移动开始位置移动到最终目标位置的轨道；
指令值输出部，其向所述马达驱动器输出和所述轨道计算部计算的轨道相应的指令值；
失调检测部，其检测所述步进马达的失调；以及
系统控制部，其在检测到所述失调时，当从所述轨道起因所述失调而移位的所述关注点的移位距离等于或小于特定距离、且从检测到所述失调的时点到所述失调恢复的时点的延迟时间等于或小于特定时间时，使所述关注点沿着从因所述失调而移位的所述关注点的位置到所述最终目标位置的轨道移动，另一方面，当所述移位距离大于所述特定距离、或所述延迟时间大于所述特定时间时，为了使所述关注点从因所述失调而移位的所述关注点的位置返回到所述移动开始位置，控制所述轨道计算部及所述指令值输出部。
10.一种步进马达控制装置，其控制产生用于装备在机械臂机构的关节部运转的动力的步进马达，其特征在于具备：
轨道计算部，其计算所述机械臂机构的关注点从当前位置移动到最终目标位置的轨道；
指令值输出部，其向所述马达驱动器输出和所述轨道计算部计算的轨道相应的指令值；
失调检测部，其检测所述步进马达的失调；以及
系统控制部，其在检测到所述失调时，再次计算从因所述失调而移位的所述关注点的位置到所述最终目标位置的轨道，为了根据所述再次计算的轨道使所述关注点移动，控制所述轨道计算部及所述指令值输出部。
11.一种机器人装置，其控制产生用于装备在机械臂机构的关节部运转的动力的步进马达，其特征在于具备：
轨道计算部，其计算所述机械臂机构的关注点从作为当前位置的移动开始位置移动到最终目标位置的轨道；
指令值输出部，其向所述马达驱动器输出和所述轨道计算部计算的轨道相应的指令值；
失调检测部，其检测所述步进马达的失调；以及
系统控制部，其于检测到所述失调时，计算从因所述失调而移位的所述关注点的位置返回到所述移动开始位置的轨道，为了根据所述计算的轨道使所述关注点返回到所述移动开始位置，控制所述轨道计算部及所述指令值输出部。
12.一种步进马达控制装置，其控制产生用于装备在机械臂机构的关节部运转的动力的步进马达，其特征在于具备：
轨道计算部，其计算所述机械臂机构的关注点从作为当前位置的移动开始位置移动到最终目标位置的轨道；
指令值输出部，其向所述马达驱动器输出和所述轨道计算部计算的轨道相应的指令值；
失调检测部，其检测所述步进马达的失调；以及
系统控制部，其在检测到所述失调时，计算从因所述失调而移位的所述关注点的位置返回到所述移动开始位置的轨道，为了根据所述计算的轨道使所述关注点返回到所述移动开始位置，控制所述轨道计算部和所述指令值输出部。
13.一种步进马达控制装置，其控制产生用于装备在机械臂机构的关节部运转的动力的步进马达，其特征在于具备：
轨道计算部，其计算所述机械臂机构的关注点从作为当前位置的移动开始位置移动到最终目标位置的轨道；
指令值输出部，其向所述马达驱动器输出和所述轨道计算部计算的轨道相应的指令值；
失调检测部，其检测所述步进马达的失调；以及
系统控制部，其在检测到所述失调时，当从所述轨道起从所述失调而移位的所述关注点的移位距离等于或小于特定距离、且从检测到所述失调的时点到所述失调恢复的时点的延迟时间等于或小于特定时间时，使所述关注点沿着从因所述失调而移位的所述关注点的位置到所述最终目标位置的轨道移动，另一方面，当所述移位距离大于所述特定距离、或所述延迟时间大于所述特定时间时，为了使所述关注点从因所述失调而移位的所述关注点的位置返回到所述移动开始位置，控制所述轨道计算部及所述指令值输出部。
CN201680030783.5A 2015-05-28 2016-05-15 机器人装置及步进马达控制装置 Pending CN107614209A (zh)

Applications Claiming Priority (3)

Application Number Priority Date Filing Date Title
JP2015-108688 2015-05-28
JP2015108688A JP6659238B2 (ja) 2015-05-28 2015-05-28 ロボット装置及びステッピングモータ制御装置
PCT/JP2016/064403 WO2016190140A1 (ja) 2015-05-28 2016-05-15 ロボット装置及びステッピングモータ制御装置

Publications (1)

Publication Number Publication Date
CN107614209A true CN107614209A (zh) 2018-01-19

Family

ID=57393896

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201680030783.5A Pending CN107614209A (zh) 2015-05-28 2016-05-15 机器人装置及步进马达控制装置

Country Status (6)

Country Link
US (1) US20180079077A1 (zh)
EP (1) EP3305478A4 (zh)
JP (1) JP6659238B2 (zh)
CN (1) CN107614209A (zh)
TW (1) TW201702030A (zh)
WO (1) WO2016190140A1 (zh)

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109861599A (zh) * 2019-03-25 2019-06-07 北京美联泰科生物技术有限公司 电机控制方法
CN110154089A (zh) * 2018-02-14 2019-08-23 发那科株式会社 机器人的关节的异常检测装置以及异常检测方法
CN113315425A (zh) * 2021-07-28 2021-08-27 深圳市永达电子信息股份有限公司 步进电机定位方法、系统、设备及存储介质
TWI766807B (zh) * 2021-09-17 2022-06-01 財團法人精密機械研究發展中心 機械手臂的安全復位方法
CN114845841A (zh) * 2019-10-09 2022-08-02 川崎重工业株式会社 控制方法、控制装置、机器人系统、程序及记录介质

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP6468804B2 (ja) * 2014-10-30 2019-02-13 ライフロボティクス株式会社 ロボットアーム機構
JP2018051635A (ja) * 2016-09-26 2018-04-05 セイコーエプソン株式会社 ロボット制御装置、ロボットおよびロボットシステム
JP7007839B2 (ja) * 2017-08-31 2022-01-25 川崎重工業株式会社 多関節ロボット
JP7310963B2 (ja) * 2019-03-22 2023-07-19 オムロン株式会社 ロボットハンド、ロボットハンド制御方法及びプログラム
WO2021101514A1 (en) * 2019-11-19 2021-05-27 Google Llc Optimization of motion paths of a robot using vision data

Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US5426722A (en) * 1993-09-09 1995-06-20 Stratasys, Inc. Method for optimizing the motion of a multi-axis robot

Family Cites Families (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPS63206197A (ja) * 1987-02-20 1988-08-25 Jeol Ltd ア−ム移動を司どるモ−タの制御機構
FR2671305B1 (fr) * 1991-01-09 1995-04-07 Commissariat Energie Atomique Procede de pilotage d'un bras de robot en definissant des trajets de substitution.
US6212968B1 (en) * 1998-07-29 2001-04-10 Janome Sewing Machine Co., Ltd, SCARA robot
JP2000047731A (ja) * 1998-07-29 2000-02-18 Janome Sewing Mach Co Ltd スカラロボット
US6278907B1 (en) * 1999-11-24 2001-08-21 Xerox Corporation Apparatus and method of distributing object handling
US6871115B2 (en) * 2002-10-11 2005-03-22 Taiwan Semiconductor Manufacturing Co., Ltd Method and apparatus for monitoring the operation of a wafer handling robot
US7792604B2 (en) * 2005-03-23 2010-09-07 Hurco Companies, Inc. Method of performing additive lookahead for adaptive cutting feedrate control
US7706920B2 (en) * 2007-09-19 2010-04-27 Disney Enterprises, Inc. Smooth clipping of data streams
KR101732902B1 (ko) * 2010-12-27 2017-05-24 삼성전자주식회사 로봇의 경로 계획 장치 및 그 방법
US8810187B2 (en) * 2011-08-19 2014-08-19 Oriental Motor Boston Technology Group Incorporated Method and apparatus for misstep detection and recovery in a stepper motor
JP2013132696A (ja) * 2011-12-26 2013-07-08 Seiko Epson Corp アーム制御装置、および、アーム制御方法
JP6399591B2 (ja) * 2014-10-31 2018-10-03 ライフロボティクス株式会社 ロボットアーム機構及びステッピングモータ制御装置

Patent Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US5426722A (en) * 1993-09-09 1995-06-20 Stratasys, Inc. Method for optimizing the motion of a multi-axis robot

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
D. SREEVIJAYAN ET AL: "Architectures for fault-tolerant mechanical systems", 《7TH MEDITERRANEAN ELECTROTECHNICAL CONFERENCE : PROCEEDINGS》 *

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110154089A (zh) * 2018-02-14 2019-08-23 发那科株式会社 机器人的关节的异常检测装置以及异常检测方法
CN109861599A (zh) * 2019-03-25 2019-06-07 北京美联泰科生物技术有限公司 电机控制方法
CN114845841A (zh) * 2019-10-09 2022-08-02 川崎重工业株式会社 控制方法、控制装置、机器人系统、程序及记录介质
CN114845841B (zh) * 2019-10-09 2024-02-06 川崎重工业株式会社 控制方法、控制装置、机器人系统、程序及记录介质
CN113315425A (zh) * 2021-07-28 2021-08-27 深圳市永达电子信息股份有限公司 步进电机定位方法、系统、设备及存储介质
TWI766807B (zh) * 2021-09-17 2022-06-01 財團法人精密機械研究發展中心 機械手臂的安全復位方法

Also Published As

Publication number Publication date
TW201702030A (zh) 2017-01-16
WO2016190140A1 (ja) 2016-12-01
US20180079077A1 (en) 2018-03-22
JP2016221606A (ja) 2016-12-28
EP3305478A1 (en) 2018-04-11
EP3305478A4 (en) 2019-01-23
JP6659238B2 (ja) 2020-03-04

Similar Documents

Publication Publication Date Title
CN107614209A (zh) 机器人装置及步进马达控制装置
CN107708943A (zh) 机器人装置及马达控制装置
JP6501921B2 (ja) 二足ロボットの歩行制御方法及び歩行制御装置
US10423227B2 (en) Hand exoskeleton force feedback system
US8423183B2 (en) Robot and method of controlling the same
CN103158162B (zh) 外构架式双向力反馈数据手套
CN109983510A (zh) 机器人控制系统、机械控制系统、机器人控制方法、机械控制方法和记录介质
CN107148329B (zh) 机械臂机构及步进马达控制装置
CN105382840B (zh) 机器人的控制装置以及控制方法
TW201634199A (zh) 雙臂機器人之教示系統及雙臂機器人之教示方法
US10118291B2 (en) Robotic systems for supporting a user
CN105034010B (zh) 进行机器人的灵活控制的控制装置
CN104589354A (zh) 机器人控制装置、机器人系统以及机器人
CN104181431A (zh) 一种电容式触屏测试机械手
CN102009708A (zh) 一种缩放滚动机构
CN107073707A (zh) 机器人和机器人系统
TW201641233A (zh) 多關節臂機構之動作控制裝置及機械手臂裝置
CN110520227A (zh) 冲压系统
CN104943760A (zh) 用于弹跳机器人的运动姿态调节装置
CN105992677A (zh) 紧凑型并联运动机器人
US20200269446A1 (en) Joint structure for robot
JP2007089716A (ja) マッサージ装置
TW201644182A (zh) 機器人裝置及步進馬達控制裝置
CN207432211U (zh) 并联连杆机器人
CN104669232A (zh) 中指力反馈装置

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
WD01 Invention patent application deemed withdrawn after publication

Application publication date: 20180119

WD01 Invention patent application deemed withdrawn after publication