机器人装置及马达控制装置的制作方法

本发明的实施方式涉及一种机器人装置及马达控制装置。

背景技术：

近年来，机器人与作业者位于同一空间的环境不断变多。可以认为今后看护用机器人、产业用机器人与作业者一起协同进行作业的状况会不断扩大。在这种状况下重要的是安全性，归因于以机械臂对作业者等的接触、特别是冲突为代表的扰乱因素的动作异常时的迅速停止或退避动作。

因此，在大多机器人装置中，必须在臂的前端或侧面等多个部位配设接触传感器。

然而，接触传感器的安装位置有限制，要在可能接触的部位全部安装并不现实，无法全面地应对接触。

技术实现要素：

本发明的目的在于全面地检测因机械臂接触等扰乱因素所导致的动作异常。

本实施方式所涉及的机器人装置具备多关节臂机构，其特征在于具备：多个链路；多个关节部，其连接多个链路间；多个马达，其产生用于驱动多个关节部的动力；传递机构，其将多个关节部中的至少一个关节部的马达的驱动轴的旋转传递到所述至少一个关节部的旋转轴；第一编码器，其检测至少一个关节部的马达的驱动轴的旋转；第二编码器，其检测至少一个关节部的旋转轴的旋转；以及判定部，其基于从第一编码器输出的编码脉冲及从所述第二编码器输出的编码脉冲，进行动作异常的判定。

附图说明

图1是本实施方式所涉及的机器人装置的机械臂机构的外观立体图；

图2是表示图1的机械臂机构的内部构造的侧视图；

图3是通过图中符号表现来表示图1的机械臂机构的构成的图；

图4是表示图1的直动伸缩关节部j3的驱动机构的构造的立体图；

图5是表示本实施方式所涉及的机器人装置的构成的框图；

图6是表示图5的冲突判定部105的冲突判定处理顺序的流程图。

【附图标记说明】

41：显示部；60：操作部；100：控制装置；101：系统控制部；102：操作部i/f；103：存储部；104：计算处理部；105：冲突判定部；106：显示控制部；107：驱动器单元i/f；108：输出部；120：数据/控制总线；210～260：驱动器单元；231：控制部；232：电源电路；233：脉冲信号产生部；235：驱动轴编码器；235：输出轴编码器；236、237：计数器；310～360：步进马达。

具体实施方式

以下，参照附图对本实施方式所涉及的机器人装置进行说明。机器人装置具备具有以步进马达作为致动器的关节部的机械臂机构。该机器人装置以垂直多关节臂机构、特别是多个关节部之一具备直动伸缩关节的垂直多关节臂机构为例子进行说明。在下面的说明中，对具有大致相同功能及构成的构成要素标注相同符号，而且只在必要情形时进行重复说明。

图1是本实施方式所涉及的机器人装置的外观立体图。构成机器人装置的机械臂机构具有大致圆筒形状的基部1、连接于基部1的臂部2及安装在臂部2前端的腕部4。在腕部4设置有未图示的转接器。例如，转接器设置在下述第六旋转轴ra6的旋转部。设置在腕部4的转接器，安装有与用途相对应的机械手。

机械臂机构具有多个、此处为六个关节部j1、j2、j3、j4、j5、j6。多个关节部j1、j2、j3、j4、j5、j6从基部1依序配设。一般而言，第一、第二、第三关节部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为中心的弯曲关节。

形成基部1的臂支撑体(第一支撑体)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的伸缩距离的长度可以使机械手到达从基部1的附近位置到远距离位置的大范围的对象。第三关节部j3的特征是通过构成其的直动伸缩机构而实现的直线伸缩动作及其伸缩距离的长度。

图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的长度大致等价的间隔沿着臂中心轴排列。在射出部30的后方，导辊40与驱动齿轮50以隔着第一连结链节排21相对的方式设置。驱动齿轮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分别恢复成可弯曲的状态。在收纳时，第二连结链节排22从射出部30朝内侧弯曲地被搬送到第一支撑体11a(基部1)内部的收纳部，第一连结链节排21也朝与第二连结链节排22相同的方向(内侧)弯曲被搬送。第一连结链节排21以与第二连结链节排22大致平行的状态被收纳。

图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

下面，对各个关节部j1－j6的驱动机构进行说明。这里，以第三关节部j3的驱动机构为例子进行说明，但其它各个关节部j1－j2、j4－j6的驱动机构也相同。

图4是表示图1的第三关节部j3的驱动机构的构造的立体图。如图4所示，第三关节部j3的驱动机构具有步进马达330。在步进马达330的驱动轴(驱动轴、输入轴)的一端卡合有驱动轴编码器235。每当驱动轴旋转特定的微小角时，驱动轴编码器235输出脉冲(编码脉冲)。在驱动轴的另一端直接连接有驱动滑轮。该驱动滑轮与从动滑轮之间架设有传动带。传动带可以采用平带、v带、附齿带等任意构造，其材料也可以采用合成橡胶、聚胺基甲酸酯等任意材料。从动滑轮的直径设计为相对于驱动滑轮的直径实现特定的减速比α。驱动滑轮、从动滑轮及传动带构成用于将步进马达330的旋转传递到驱动齿轮331的传递机构。另外，传递机构也可以是其它构成、例如由具有不同齿轮比的多个齿轮构成。从动滑轮的轴(输出轴)直接连接驱动齿轮331。从动滑轮的轴(输出轴、从动轴)卡合有输出轴编码器236。每当从动滑轮的轴旋转例如所述步进角时，输出轴编码器236输出编码脉冲。

驱动齿轮331与形成于直动伸缩关节部j3的臂部2的线性齿轮一起构成齿轮齿条机构。第三关节部j3的臂部2送出与驱动齿轮331的旋转角度相应的距离(伸缩距离)。在驱动齿轮331的旋转轴(驱动齿轮轴、从动轴、输出轴)通过传递机构连接有步进马达330的旋转轴(驱动轴、驱动轴)。传递机构典型而言为了使步进马达330的旋转速度减慢、增大步进马达330的扭矩，构成减速比α的减速机构。例如，当驱动齿轮331使用减速比2的减速机构时，驱动齿轮331以步进马达330的旋转角速度的1/2倍的旋转角速度旋转，产生步进马达330的扭矩的2倍的扭矩。

驱动轴编码器(第一编码器)235与输出轴编码器(第二编码器)236分别与计数器237、238一起构成增量型旋转编码器。也可以是光学式旋转编码器或磁式旋转编码器等任意方式。每当驱动轴旋转一定旋转角(分辨限度、步进角)时，驱动轴编码器235以两相、例如具有90度的相位差输出脉冲信号(a相信号及b相信号)。另外，每当驱动轴进行一次旋转时，驱动轴编码器235输出原点用的脉冲信号(z相信号)。输出轴编码器236典型地具备与驱动轴编码器235相同的功能及性能，每当驱动齿轮轴旋转所述相同步进角(分辨限度)时，以两相、例如具有90度的相位差输出脉冲信号(a相信号及b相信号)，而且每当驱动齿轮轴进行一次旋转时输出原点用的脉冲信号(z相信号)。

(构成的说明)图5

图5是表示本实施方式所涉及的机器人装置的构成的框图。本实施方式所涉及的机器人装置的机械臂机构的关节部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具有相同的构成，根据来自外部的控制装置100的控制信号(指令值)，驱动控制对象的步进马达310、320、330、340、350、360。这里，对驱动器单元230的构成及动作进行说明。其它驱动器单元210、220、240、250、260的构成及动作与驱动器单元230的构成及动作相同，所以省略说明。

步进马达330由在转子的周围配置多个定子线圈形成。定子线圈通过开关组件与电源电路232连接。这些开关组件通过从脉冲信号产生部233供给的脉冲信号接通。通过依序接通这些开关组件，步进马达330(转子)以特定的步进角依序旋转。可以通过改变脉冲信号的频率(脉冲频率)来改变步进马达330的旋转速度。

驱动器单元230控制步进马达330的驱动及停止。驱动器单元230具有控制部231、电源电路232、脉冲信号产生部233、驱动轴编码器235、输出轴编码器236、计数器237、及计数器238。控制部231根据从控制装置100输入的指令值，统括地控制驱动器单元230。

表示步进马达330的励磁电流值的指令值(电流脚本)从控制装置100被提供给控制部231。控制部231根据电流脚本，对电源电路232执行用于产生电流的控制。电源电路232是电流可变的ac/dc变换方式电源电路，产生由电流脚本指定的励磁电流值的电流。产生的励磁电流提供给步进马达330的定子线圈。

另外，表示下一伸缩距离的指令值(位置脚本)从控制装置100被提供给控制部231。下一伸缩长是指特定的控制周期δt(例如10ms)后的伸缩距离。另外，伸缩距离表示与臂部2最收缩状态的距离。另外，作为指令值，如果是关节部j1、j2、j4、j5、j6的驱动器单元的控制部，则作为关节角度被提供。关节角度是指从基准位置(原点)的正负旋转角度。伸缩距离及关节角度统称为关节变量。

控制部231对脉冲信号产生部233输出与位置脚本相应的脉冲控制信号。具体而言，控制部231通过将为使当前伸缩距离与从控制装置100输入的特定的控制周期δt后的伸缩距离的距离差变位所必需的驱动齿轮50的旋转角度除以步进长，决定脉冲数，并将控制周期δt除以脉冲数，根据其倒数决定脉冲频率。控制部231将与决定的脉冲条件(脉冲数及脉冲频率)对应的脉冲控制信号输出到脉冲信号产生部233。

脉冲信号产生部233将从控制部231输出的脉冲控制信号提供的脉冲数的脉冲，一边依序切换一边提供给定子线圈与电源电路232之间的开关组件。由此，在周期δt，步进马达330以使所述距离差变位所需的驱动齿轮50的旋转角度旋转，由此臂部2伸缩到指令的伸缩距离。同样，在与关节部j1、j2、j4、j5、j6对应的各个驱动器单元210、220、240、250、260，从控制装置100输入表示下一关节角度的位置脚本，由此各个关节部j1、j2、j4、j5、j6旋转到指令的关节角度。

用于使关节部j3在当前位置停止的动作停止信号从控制装置100输入到控制部231。步进马达330通过向与当前位置对应的相的定子线圈持续地提供电流而在这个位置停止。所以，在控制部231输入例如表示关节部j3的当前伸缩距离(关节变量)的编码作为动作停止信号。同样，与关节部j1、j2、j4、j5、j6对应的各个驱动器单元210、220、240、250、260，从控制装置100输入包含表示当前关节角度编码的动作停止信号。由此，机械臂机构的动作停止。

每当驱动轴旋转一定的旋转角(分辨限度、编码步进角)，驱动轴编码器235输出脉冲。实际上为了识别顺/逆的旋转方向，输出有a相/b相两种脉冲。每当驱动轴进行一次旋转时，驱动轴编码器235将原点用的脉冲(z相脉冲)输出到计数器237。计数器237将从驱动轴编码器235输出的a相(或b相)脉冲根据旋转方向进行加减运算。如果从驱动轴编码器235输出z相脉冲，计数器237重置计数值。因此，编码脉冲的计数值表示驱动轴的从原点的旋转变位(旋转角度)。计数器237也对从驱动轴编码器235输出的z相脉冲进行计数。z相脉冲的计数值表示“旋转数”。下面，仅称为“计数值”时，表示旋转角度的计数值与表示旋转数的计数值的总称，在区分两者时称为“表示旋转角度的计数值”与“表示旋转数的计数值”。每隔一定周期δt从计数器237输出计数值。该周期δt可以与所述控制周期δt相同，也可以不同。从计数器237输出的驱动轴的旋转相关的计数值的数据是根据控制装置100的系统控制部101的控制，通过驱动器单元接口107被控制装置100获取。

输出轴编码器236与驱动轴编码器235相同，每当驱动齿轮331的旋转轴(驱动齿轮轴、输出轴)旋转一定的旋转角时，便将脉冲信号(a相/b相)输出到计数器238，而且每当驱动齿轮轴进行一次旋转时，便将原点用的脉冲信号(z相信号)输出到计数器238。计数器238对从输出轴编码器236输出的a相脉冲(或b相脉冲)进行计数，而且对z相脉冲进行计数。每隔一定周期δt，输出表示驱动齿轮轴的旋转数的计数值与表示驱动齿轮轴的旋转数的计数值。

控制装置100具有系统控制部101、操作部接口102、存储部103、计算处理部104、冲突判定部105、显示控制部106、驱动器单元接口107、及输出部。

根据系统控制部101的控制，控制装置100从各个驱动器单元210－260，通过驱动器单元107每隔特定的控制周期δt(例如，每隔10ms)，供给表示驱动轴旋转角度的计数值及表示旋转数的计数值相关的数据、以及表示驱动齿轮轴旋转角度的计数值及表示旋转数的计数值相关的资料。

控制装置100通过操作部接口102连接操作部60。操作部60用作操作者输入腕部4或机械手(末端效应器)的关注点的位置的变更、姿势的变更及变更时间的输入接口。例如，操作部60具备用于指定使机械手移动的最终目标位置及移动时间的操纵杆等。例如，基于操纵杆的操作方向、操纵杆的倾斜角度、操纵杆的操作加速度，输入机械手的最终目标位置及移动时间。操作部60具备用于停止机械臂机构的动作的动作停止按钮。另外，构成操作部60的输入设备可以由其它装置、例如鼠标、键盘、轨迹球及触控面板等代替。例如，作为示教时的挂件的操作部60典型地采用触控面板等。

系统控制部101具有cpu(centralprocessingunit)及半导体存储器等，统括地控制控制装置100。系统控制部101通过控制/数据总线120连接各部。

当驱动轴(输入轴)的旋转角度及旋转数以减速比α换算后，驱动齿轮331的轴(输出轴)的旋转角度及旋转数产生差异(旋转偏移、角度差)时，有产生冲突等扰乱的可能。但是，也有因为传递机构的传动带伸缩或齿隙等的碰撞引起差异的情形，为了排除这种情况，这里对所述“旋转偏移”设定阈值，当所述“旋转偏移”超过阈值时，判定此偏移是由冲突等扰乱引起的。存储部103存储该阈值θth的数据。同样，存储部103存储阈值数据，该阈值数据用于判定与各个关节部j1、j2、j4、j5、j6对应的旋转偏移的原因是否排除由传递机构的碰撞引起的情况，而是由冲突等扰乱引起。阈值是根据各个传递机构的碰撞而设定，也可以通过操作部60根据操作者指示任意地变更。另外，存储部103存储具有用于通知操作者干扰物与机器人装置冲突的讯息的通知图像的数据。

计算处理部104计算作为指令值提供给各个驱动器单元210―260的、经过控制周期δt后的关节变量向量。另外，关节变量向量是指关节部j1－j6的六个关节变量、即旋转关节部j1、j2、j4－j6的关节角度与直动伸缩关节部j3的直动移位的6变量。

首先，计算处理部104为了计算经过控制周期δt后的关节变量向量，计算末端关注点的当前位置·姿势。具体而言，计算处理部104根据由各个驱动器单元210－260的计数器计数的驱动轴的旋转角度及旋转数，计算从编码原点的累积计数值，并在累积计数值上乘以相对于1计数的步进角，由此计算各个关节部j1－j6的当前的关节变量。计算处理部104将各个关节部j1－j6的当前的关节变量作为参数代入齐次变换矩阵k，由此计算从机器人坐标系σb观察的末端关注点的当前位置·姿势。齐次变换矩阵k是定义末端坐标系σh与机器人坐标系σb关系的矩阵式。齐次变换矩阵k由构成机械臂机构的链路间的关系(链路长与链路的扭转角)与关节部的轴间的关系(链路间距离与链路之间角度)决定。计算处理部104每隔控制周期δt反复进行所述计算处理，计算每隔控制周期δt的末端关注点的当前位置·姿势。

其次，计算处理部104基于计算出的各个关节部j1－j6的当前的关节变量、及通过操作部50、由操作者输入的末端的最终目标位置·姿势，计算将这些连接的每隔单位时间δt(控制周期、例如10ms)的末端关注点的目标位置的点列。具体而言，计算处理部104将末端关注点的当前位置·姿势及末端关注点的最终目标位置·姿势作为参数代入事先预设的末端关注点的轨道计算式，由此计算末端关注点的轨道(以下称为末端轨道)，并在该末端轨道上每隔单位时间δt计算目标位置的点列。轨道计算方法可以采用任意方法。

最后，计算处理部104计算与计算出的多个目标位置分别对应的多个关节变量向量。具体而言，计算处理部104基于末端关注点的当前位置·姿势、经过控制周期δt后的下一目标位置·姿势、及控制周期δt，计算末端速度，并用雅可比逆矩阵将计算出的末端速度变换成关节角速度。雅可比逆矩阵通过对表示末端关注点的位置·末端姿势的向量的关节变量进行偏微分得到，是将末端速度(末端位置·姿势的微小变化)变换成关节角速度(关节角度·伸缩长的微小变化)的矩阵。雅可比逆矩阵通过当前的关节变量向量及臂构造的链路参数计算。计算处理部104通过关节角速度乘以单位时间δt，计算单位时间δt之间的各个关节部的变位量，并将计算出的各个关节部的变位量加到即将移动(当前)的关节变量向量，由此计算经过单位时间δt后的关节变量向量。

输出部108根据系统控制部101的控制，将计算处理部104计算出的各个关节部j1－j6的指令值(关节变量)输出到各个驱动器单元210－260。另外，输出部108根据系统控制部101的控制将动作停止信号输出到各个驱动器单元210－260。

冲突判定部105基于驱动轴编码器235的输出及输出轴编码器236的输出，实际上基于计数器237、238的计数值判定是否产生因干扰物与机械臂部2冲突等扰乱因素引起的动作异常。这里，作为动作异常以干扰物的冲突为例子进行说明。冲突判定部105判定“干扰物与机器人装置冲突”时，输出冲突判定信号。关于冲突判定部105将在下文中详细叙述。

当冲突判定部105输出冲突判定信号时，系统控制部101从存储部103读出通知图像，并写入到显示控制部106的帧存储器。显示控制部106读出帧存储器内储存的图像的数据，并将其在显示部70显示。显示部70，典型而言，可以列举例如crt显示器或液晶显示器、有机el显示器、电浆显示器等。

(冲突判定处理)图6

本实施方式所涉及的机器人装置具有用于判定干扰物与机器人装置是否冲突的冲突判定功能。与冲突判定功能相关的处理被称为冲突判定处理。冲突判定部105的冲突判定处理在每个冲突判定周期(例如与控制周期(10ms)等价)反复进行。图6是表示图5的冲突判定部105的冲突判定处理顺序的流程图。这里，对关节部j3的冲突判定处理进行说明。另外，冲突判定部105能够以与图6相同的顺序进行其它各个关节部j1、j2、j4、j5、j6的冲突判定处理。

(步骤s11)

根据系统控制部101的控制，控制装置100从计数器237获取表示步进马达330的驱动轴(输入轴)的旋转角度及旋转数的计数值。同样，根据系统控制部101的控制，计数器238将表示驱动齿轮331的驱动齿轮轴(输出轴)的旋转角度及旋转数的计数值，以一定的周期δt反复地提供给控制装置100。

(步骤s12)

基于在步骤s11中输入的驱动轴的旋转相关的计数值，冲突判定部105依序计算与驱动轴的旋转相关的累计计数值并存储。冲突判定部105计算当前的累计计数值、与相对于一个周期前(δt)的累计计数值的计数值的差(变位)nin。

同样，基于驱动齿轮轴的旋转相关的计数值，冲突判定部105依序计算与驱动齿轮轴的旋转相关的累计计数值并存储。冲突判定部105计算当前的累计计数值、与一个周期前的累计计数值的差(变位)nout。

(步骤s13)

通过在步骤s12中计算出的与驱动轴的旋转相关的计数值的变位nin，乘以驱动轴编码器235的分辨限度(步进角)δθin，计算一个周期δt之间的步进马达330的旋转角度变位θin。同样，在步骤s12中计算出的驱动齿轮轴的旋转相关的计数值的变位nout，乘以输出轴编码器236的分辨限度(步进角)δθout，计算一个周期之间的驱动齿轮331的旋转角度变位θout。

(步骤s14)

将在步骤s13中计算出的一个周期δt之间的驱动齿轮331的旋转角度的变位θout相对于在步骤s13中计算出的一个周期δt之间的步进马达330的旋转角度的变位θin，利用减速比α进行换算，即计算变位θout乘以减速比α的倒数的角度的差、即输入轴与输出轴之间的“旋转偏移”。当计算出的角度差的绝对值等于或小于为了排除因传递机构的传动带伸缩或齿隙等碰撞的旋转偏移而既定的阈值θth时(步骤s14：no)，冲突判定部105判定“干扰物不与机器人装置冲突”，处理返回到步骤s11。另一方面，当计算出的角度差的绝对值大于阈值θth时(步骤s14：yes)，冲突判定部105判定“干扰物与机器人装置冲突”，并输出冲突判定信号。

另外，虽然在所述说明中驱动轴与驱动齿轮轴的输入输出轴间的旋转偏移在冲突判定中被利用，但是在步进马达中，是以指示其旋转的指令值的旋转角度与实际驱动轴的旋转角度在原则上等价为前提。因此，在冲突判定中，也可以比较所述指令值指令的旋转角度、与根据自步进马达330观察卡合于作为被驱动部的驱动齿轮的轴的输出轴编码器238的脉冲的计数值求出的旋转角度，来判定冲突等动作异常。

(步骤s15)

当在步骤s14中通过冲突判定部105输出冲突判定信号时，系统控制部101从存储部103读出通知干扰物与机器人装置冲突的通知图像的数据，并将其写入到显示控制部106的帧存储器。

显示控制部106读出帧存储器内储存的通知图像的数据，并将其显示在显示部70。由此，显示部70上显示通知干扰物与机器人装置冲突的讯息。

(步骤s16)

当在步骤s14中通过冲突判定部105输出冲突判定信号时，在输出所述讯息的同时，通过系统控制部101停止输出部108的指令值输出，停止机械臂机构的动作(紧急停止)，中断分配给机器人装置的作业(任务)，并重置任务进度自身。

另外，当冲突判定部105中判定关节部j1－j6中的至少一个产生冲突时，输出冲突判定信号。换句话说，冲突判定部105将关于关节部j1－j6的六个系统的步骤s14的比较输出(yes标记“1”、no标记“0”)导入or电路，当其中一个的旋转偏移超过阈值出现标记“1”时，输出标记“1”，并据此输出冲突判定信号。

即，在本实施方式中，无论作业员等从哪个方向、哪个部位接触或冲突臂部2或手部，必定对关节部j1－j6中的至少一个进行冲突判定。

(步骤s17、s18)

任务时间管理因冲突产生而被破坏，由于任务动作也需要例如与线的搬送机器等外部机器的协作，所以并不限定于臂动作重新开始，而是必须使任务动作自身重新开始。另外，也需要消除冲突的原因。所以，在步骤s16的动作停止后，系统控制部101等待操作者的任务重新开始指示，重新开始任务。

例如，即使从步骤s16动作停止时点经过了一定时间，如果操作者仍然没有指示任务重新开始，则臂动作结束。

(效果)

根据上面说明的关节部j3的冲突判定处理，可以对较机器人装置的关节部j3更前端的部分，检测具有与关节部j3的第三移动轴ra3平行的成分的如产生外部扭矩的干扰物的冲突。该冲突判定处理根据设置在关节部j3的驱动轴编码器235的旋转角度与输出轴编码器236的旋转角度之间是否产生特定的角度差来进行。原理是，当在关节部j3的臂部2的伸缩方向(与第三移动轴平行的方向)上，因为干扰物的冲突产生外部扭矩时，步进马达330的旋转角度与驱动齿轮331(关节部)的实际旋转角度之间会产生角度差。但是，也有这样的情形：由于步进马达330与驱动齿轮331之间的传递机构会弹性变形等，所以由于动作开始时或动作停止时对关节部作用的惯性等冲突之外的理由，步进马达330的旋转角度与驱动齿轮331的实际旋转角度之间产生角度差。为了从冲突判定中排除这种情形，对角度差设置阈值，由此可以仅在角度差超过阈值时，判定“干扰物与机器人装置冲突”。

另外，在关节部j3的冲突判定处理中，不能对较机器人装置的关节部j3更前端的部分，检测具有与关节部j3的第三移动轴ra3平行的成分的如产生外部扭矩的干扰物的冲突。但是，对于其它关节部j1、j2、j4、j5、j6，通过进行与关节部j3相同的冲突判定处理，可以更全面地检测干扰物对机械臂机构的冲突。例如，根据关节部j1的冲突判定处理，可以对较机器人装置的关节部j1更前端的部分，检测具有与关节部j1的第一旋转轴ra1正交的成分的如产生外部扭矩的干扰物的冲突。根据关节部j2的冲突判定处理，可以对较机器人装置的关节部j2更前端的部分，检测具有与关节部j2的第一旋转轴ra2正交的成分的如产生外部扭矩的干扰物的冲突。根据以上说明的本实施方式所涉及的机器人装置，通过对机械臂机构的各个关节部j1－j6的冲突判定处理，可以全面地检测干扰物对机械臂机构的接触。即，在本实施方式中，无论作业员等从哪个方向、哪个部位接触或冲突臂部2或手部，必定对关节部j1－j6中的至少一个进行冲突判定。

在所述说明中，从步进马达330的马达驱动器230观察，冲突等动作异常判定是由外部的控制装置100的冲突判定部105进行，但是也可以在步进马达330的马达驱动器230的控制部231(马达控制装置)中比较计数器237、238的输出，或比较外部的控制装置100的指令值与输出轴编码器236的计数器238的计数值，进行冲突判定。

虽然对本发明的几个实施方式进行了说明，但是这些实施方式只是作为例子提出的，并非用于限定本发明的范围。对于这些新的实施方式，能够以其他各种方式进行实施，在不脱离本发明的要旨的范围内，能够进行各种省略、置换、及变更。这些实施方式和其变形，包含于本发明的范围和要旨的同时，也包含于权利要求书中记载的发明及其均等范围内。