CN101952087B - 机器人、机器人的控制装置及控制方法 - Google Patents

Abstract

本发明提供一种机器人、机器人的控制装置及控制方法、以及机器人的控制装置的控制程序。基于物体(3)及手(30)间的铅直方向的力、和手(30)与水平面所成角度，由搬运力估算部(14)估算人(2)在铅直方向上施加的搬运力，基于估算出的力进行力控制，以使得机器人系统(1)中的机械臂的铅直方向的力成为规定力。

Description

机器人、机器人的控制装置及控制方法

技术领域

本发明涉及一种人和机器人协作进行作业(协调作业)时的机器人、机器人的控制装置及控制方法、以及机器人的控制装置的控制程序。

背景技术

近年来，正积极进行看护机器人或家务支援机器人等家庭用机器人的研发。此外，与到目前为止与人隔离的情形不同，工业用的机器人也正增加进行与人的协作作业的功能。这些机器人由于靠近人进行作业，所以与人的物理接触是不可缺少的。为此，基于安全性方面，有必要使机器人在机构上柔软，在活动上也柔和。

作为机器人装置的一例，已提出了一种人和机械臂协作搬运物体的装置。在其中提出，一方面控制钳爪的旋转运动，以使得机械臂的钳爪的手的旋转力变为零，另一方面控制钳爪的铅直方向的平移运动，以便维持物体水平的技术(参照专利文献1)。

根据此专利文献1的技术，实现作为现有课题的物体的质量不明确，或物体的质量在中途发生变化的情形的协作搬运。

专利文献1：JP特开2000-343470号公报

发明内容

但是，在专利文献1这样的现有的技术中，如图13A所示，为了与作用于机械臂101上的力无关系地上下移动机械臂101，以便保持搬运的物体103A水平，而在搬运物体103A之下具有另一物体103B，此外在人102要降下搬运的物体103A的时候，即使物体103A、103B相互接触，也会产生机械臂101还要向下移动的现象。由此，如图13A、13B所示，存在物体103A、103B彼此相互挤压、破损，在机械臂101上产生过多的负载这样的课题。

鉴于这样的课题而进行本发明，其目的在于，提供一种机器人、机器人的控制装置及控制方法、以及机器人的控制装置的控制程序，其中，在人和机械臂协作搬运物体时，即使搬运的物体的重量不明确、或重量在中途变化也能搬运，并且即使是在机械臂或搬运的物体之下存在其它的物体的情况下，物体彼此也不会有所需以上的相互挤压，能实现安全的机器人控制。

为了实现上述目的，本发明结构如下。

根据本发明的第一方式，提供一种机器人，

该机器人具有机械臂、和配置在上述机械臂的钳爪上的手，按照由人和上述手来保持位于上述人和上述机器人之间的物体的状态，进行协作，搬运上述物体，

该机器人包括：

驱动部，其调整上述手的角度；

力检测部，其检测并输出作用在上述物体和上述手之间的力；

角度检测部，其检测并输出上述手的上述角度；以及

控制部，其进行以下力控制，即：基于上述角度检测部的输出和上述力检测部的输出来进行控制，以使得上述机械臂的铅直方向的力成为规定力。

根据这样的结构，无论搬运的物体的重量如何，都能估算人的搬运力，实现对机械臂和人进行协作来搬运物体的机器人的控制。

此外，根据本发明的第四方式，提供一种机器人的控制装置，

其中，上述机器人具有机械臂、和配置在上述机械臂的钳爪上的手，该机器人的控制装置控制上述机器人，以便按照由人和上述手来保持位于上述人和上述机器人之间的物体的状态，进行协作，搬运上述物体，

该机器人的控制装置包括：

输入部，其输入由力检测部检测出的力信息、以及由角度检测部检测出的角度信息，其中，上述力检测部检测作用在上述物体和上述手之间的力，上述角度检测部检测通过驱动部而被进行角度调整的上述手的角度；以及

控制部，其进行以下力控制，即：基于上述力检测部的上述力信息和上述角度检测部的上述角度信息来进行控制，以使得上述机械臂的铅直方向的力成为规定力。

通过此结构，在人将物体放置在例如桌子等上时，能实现可安全、水平地放置物体的机器人的控制。

根据本发明的第九方式，提供一种机器人的控制方法，

其中，上述机器人具有机械臂和配置在上述机械臂的钳爪上的手，在该机器人的控制方法中，控制上述机器人，以便按照由人和上述手来保持位于上述人和上述机器人之间的物体的状态，进行协作，搬运上述物体，

输入由力检测部检测出的力信息、以及由角度检测部检测出的角度信息，其中，上述力检测部检测作用在上述物体和上述手之间的力，上述角度检测部检测通过驱动部而被进行角度调整的上述手的角度，

通过控制部进行以下力控制，即：基于上述力检测部的上述力信息和上述角度检测部的上述角度信息来进行控制，以使得上述机械臂的铅直方向的力成为规定力。

根据本发明的第十方式，提供一种机器人的控制装置的控制程序，

其中，上述机器人具有机械臂和配置在上述机械臂的钳爪上的手，利用该机器人的控制装置的控制程序控制上述机器人，以便按照由人和上述手来保持位于上述人和上述机器人之间的物体的状态，进行协作，搬运上述物体，

该机器人的控制装置的控制程序用于使计算机实现如下功能：

输入由力检测部检测出的力信息、以及由角度检测部检测出的角度信息的功能，其中，上述力检测部检测作用在上述物体和上述手之间的力，上述角度检测部检测通过驱动部而被进行角度调整的上述手的角度；以及

通过控制部进行力控制的功能，其中，上述力控制为：基于上述力检测部的上述力信息和上述角度检测部的上述角度信息来进行控制，以使得上述机械臂的铅直方向的力成为规定力。

通过这些这样的结构，无论搬运的物体的重量如何，都能估算人的搬运力，实现对机械臂和人进行协作来搬运物体的机器人的控制。

发明效果

如上所述，根据本发明的机器人、机器人的控制装置及控制方法、以及机器人的控制装置的控制程序，在机器人的机械臂和人进行协作搬运物体的情况下，根据在机械臂的钳爪上产生的力和机械臂的钳爪的手的角度，一边援助物体的重量部分，一边由上述控制部估算人在铅直方向上对物体施加的力，并基于此估算的力由上述控制部进行铅直方向的力控制，所以即使是在物体的重量不明确的情况下或物体的重量在中途变化的情况下，也能不测量物体的重量就实现对机械臂和人能够进行协动搬运物体的机器人控制。

并且，由于在根据在机械臂的钳爪上产生的力和钳爪角度来估算人要搬运的力的同时，还基于在机械臂的钳爪上产生的力进行铅直方向的力控制，所以，在搬运的物体与处于其下的其它物体干扰时，没有进行无用地相互挤压，安全的机器人控制成为可能。

附图说明

本发明的这些以及其他目的、特征，基于与针对添加的附图的优选实施方式相关的如下记述会明确。在此附图中，

图1是表示本发明的1个实施方式中的机器人系统的结构的概况的图，

图2是表示本发明的上述实施方式中的构成上述机器人系统的机器人控制装置和作为控制对象的机械臂的详细结构的图，

图3A是本发明的上述实施方式中的上述机器人控制装置的控制方框图，

图3B是本发明的上述实施方式中的上述机器人控制装置的另一控制方框图，

图4A是用于说明本发明的上述实施方式中的由上述机器人控制装置控制的上述机械臂的钳爪的记号等的说明图，

图4B是用于说明本发明的上述实施方式中的由上述机器人控制装置控制的上述机械臂的钳爪的滚动角(roll)、俯仰角(pitch)、偏转角(yaw)的说明图，

图4C是用于说明本发明的上述实施方式中的由上述机器人控制装置控制的上述机械臂的钳爪的滚动角、俯仰角、偏转角的说明图，

图4D是用于说明本发明的上述实施方式中的由上述机器人控制装置控制的上述机械臂的钳爪的滚动角、俯仰角、偏转角的说明图，

图5是用于说明本发明的上述实施方式中的上述机械臂的钳爪中的记号等的说明图，

图6A是表示本发明的上述实施方式中的根据上述机械臂和人的协作而产生的物体搬运的状态的图，

图6B是表示本发明的上述实施方式中的根据上述机械臂和人的协作而产生的物体搬运的状态的图，

图6C是表示本发明的上述实施方式中的根据上述机械臂和人的协作而产生的物体搬运的状态的图，

图6D是表示本发明的上述实施方式中的根据上述机械臂和人的协作而产生的物体搬运的状态的图，

图7是以表的方式表示本发明的上述实施方式相关的上述机器人控制装置中的估算参数数据库的一例的图，

图8是表示本发明的上述实施方式中的用于进行上述机器人控制装置的运动控制部的物体搬运动作控制的工作步骤的流程图，

图9是表示本发明的上述实施方式中的上述机器人控制装置的上述运动控制部的搬运物承载判定工作步骤的流程图，

图10是表示本发明的上述实施方式中的根据上述机械臂和人的协作而产生的上述物体的承载时的状态的说明图，

图11是用于说明在本发明的上述实施方式中的上述机器人控制装置中，通过搬运物承载判定部的搬运物承载判定方法的一例的图(图11(a)是表示上述机械臂的钳爪z位置和时间t的关系的图表，图11(b)是表示上述机械臂的角度ω_p和时间t的关系的图表，图11(c)是表示上述机械臂的铅直方向的力F_z和时间t的关系的图表)，

图12是表示本发明的上述实施方式的变化例相关的机器人系统的说明图，

图13A是表示现有例中的机器人控制装置的概况的图，

图13B是用于说明现有例中的机器人控制装置的课题的图。

具体实施方式

下面，在参照附图详细地说明本发明的实施方式之前，说明本发明的各种方式。

根据本发明的第一方式，提供一种机器人，

该机器人具有机械臂、和配置在上述机械臂的钳爪上的手，按照由人和上述手来保持位于上述人和上述机器人之间的物体的状态，进行协作，搬运上述物体，

该机器人包括：

驱动部，其调整上述手的角度；

力检测部，其检测并输出作用在上述物体和上述手之间的力；

角度检测部，其检测并输出上述手的上述角度；以及

控制部，其进行以下力控制，即：基于上述角度检测部的输出和上述力检测部的输出来进行控制，以使得上述机械臂的铅直方向的力成为规定力。

根据本发明的第二方式，提供一种第一方式所述的机器人，其中，

上述控制部使用上述角度检测部的输出和上述力检测部的输出，估算上述人要在铅直方向上作用于上述机器人的力，并进行以下力控制，即：用估算出的力进行控制，以使得上述机械臂的铅直方向的力成为规定力。

根据本发明的第三方式，提供一种第一或第二方式所述的机器人，其中，

上述力检测部检测并输出作用在上述物体和上述手之间的铅直方向的力，

上述控制部基于上述角度检测部的输出和上述力检测部的输出，决定上述力控制的铅直方向的输入，当上述人提升上述物体且上述力控制的铅直方向的输入变为向上时，进行力控制，以使得上述机械臂上升，并且当上述人使上述物体下降且上述力控制的铅直方向的输入变为向下时，进行力控制，以使得上述机械臂下降，另一方面，在由上述力检测部检测出的上述铅直方向的力变大时，进行力控制，以使得上述机械臂的向上的力增加，并且在由上述力检测部检测出的上述铅直方向的力变小时，进行力控制，以使得上述机械臂的向上的力减少。

根据本发明第四方式，提供一种机器人的控制装置，

其中，上述机器人具有机械臂和配置在上述机械臂的钳爪上的手，该机器人的控制装置控制上述机器人，以便按照由人和上述手来保持位于上述人和上述机器人之间的物体的状态，进行协作，搬运上述物体，

该机器人的控制装置包括：

输入部，其输入由力检测部检测出的力信息、以及由角度检测部检测出的角度信息，其中，上述力检测部检测作用在上述物体和上述手之间的力，上述角度检测部检测通过驱动部而被进行角度调整的上述手的角度；以及

控制部，其进行以下力控制，即：基于上述力检测部的上述力信息和上述角度检测部的上述角度信息来进行控制，以使得上述机械臂的铅直方向的力成为规定力。

根据本发明第五方式，提供一种第四方式所述的机器人的控制装置，其中，

该机器人的控制装置包括搬运力估算部，该搬运力估算部基于上述角度检测部的输出和上述力检测部的输出，估算上述人要在铅直方向上作用于上述机器人的力，

该机器人的控制装置进行以下力控制，即：基于由上述搬运力估算部估算出的上述力来进行控制，以使得上述机械臂的铅直方向的力成为规定力。

根据本发明的第六方式，提供一种第五方式所述的机器人的控制装置，其中，

上述搬运力估算部使用将在上述角度检测部的输出上乘以系数后的值、和在上述力检测部的输出的铅直分量上乘以系数后的值进行相加后而得到的值，以作为上述力控制的铅直方向的输入。

根据本发明的第七方式，提供一种第四～六方式中任意一项所述的机器人的控制装置，其中，

该机器人的控制装置还包括钳爪位置检测部，该钳爪位置检测部检测上述机械臂的钳爪的上述手的位置，

并且包括搬运物承载判定部，该搬运物承载判定部根据上述钳爪位置检测部的输出、上述角度检测部的输出、和上述力检测部的输出，判定上述搬运的物体是否被承载在物体承载面上，

当上述搬运物承载判定部判定为上述物体被承载在上述物体承载面上时，将上述机械臂的铅直方向的控制从上述力控制切换为位置控制。

根据本发明的第八方式，提供一种第七方式所述的机器人的控制装置，其中，

在上述机械臂的上述手比水平面更向下，且上述钳爪位置检测部的输出从下降转变为停止，并且上述力检测部的输出中、铅直方向向下的力转变为减少时，上述搬运物承载判定部判定为上述搬运的物体被承载在上述物体承载面上。

根据本发明的第九方式，提供一种机器人的控制方法，

其中，上述机器人具有机械臂和配置在上述机械臂的钳爪上的手，在该机器人的控制方法中，控制上述机器人，以便按照由人和上述手来保持位于上述人和上述机器人之间的物体的状态，进行协作，搬运上述物体，

输入由力检测部检测出的力信息、以及由角度检测部检测出的角度信息，其中，上述力检测部检测作用在上述物体和上述手之间的力，上述角度检测部检测通过驱动部而被进行角度调整的上述手的角度，

通过控制部进行以下力控制，即：基于上述力检测部的上述力信息和上述角度检测部的上述角度信息来进行控制，以使得上述机械臂的铅直方向的力成为规定力。

根据本发明的第十方式，提供一种机器人的控制装置的控制程序，

其中，上述机器人具有机械臂和配置在上述机械臂的钳爪上的手，利用该机器人的控制装置的控制程序控制上述机器人，以便按照由人和上述手来保持位于上述人和上述机器人之间的物体的状态，进行协作，搬运上述物体，

该机器人的控制装置的控制程序用于使计算机实现如下功能：

输入由力检测部检测出的力信息、以及由角度检测部检测出的角度信息的功能，其中，上述力检测部检测作用在上述物体和上述手之间的力，上述角度检测部检测通过驱动部而被进行角度调整的上述手的角度；以及

通过控制部进行力控制的功能，其中，上述力控制为：基于上述力检测部的上述力信息和上述角度检测部的上述角度信息来进行控制，以使得上述机械臂的铅直方向的力成为规定力。

下面，使用附图详细地说明本发明的1个实施方式相关的机器人、机器人的控制装置及控制方法、以及机器人的控制装置的控制程序。

首先，说明本发明的1个实施方式相关的机器人系统1的结构。图1是表示本发明的上述实施方式中的机器人系统1的结构的概况的图。

如图1所示，本发明的上述实施方式的、包括具有机械臂5的机器人5A及其控制装置4的机器人系统1是如下系统，即，在厨房或桌子等作业台附近的壁面91上承载机械臂5的基端，机械臂5的钳爪的手30可保持例如把持搬运对象的物体3的一端3a，与机械臂5协作的人2可把持与物体3的一端3a相对的另一端3b，按照人2和机器人5A的机械臂5的钳爪的手30保持位于人2和机器人5A之间的物体3的状态，人2和机器人5A协作，搬运物体3。具体地，按照机械臂5的手30把持物体3的一端(位于人2和机器人5A之间的物体3中的、靠近机器人5A一侧的把持用部分，例如锅的一个把手3a)，且人2把持物体3的另一端(位于人2和机器人5A之间的物体3中的、靠近人2一侧的把持用部分，例如锅的另一个把手3b)的状态，通过人2在想要搬送物体3的方向上施加力，来移动机器人系统1的机械臂5，机械臂5和人2进行协作就能搬运物体3。

本发明的上述实施方式的物体3，概念上是包括加入水或食材的锅、或餐具、或家具等的重量物，并且还包含重量在中途变化的物体(例如在搬运空的锅的中途加入水或食材的锅)，是机械臂5和人2一边保持一边协作搬运，可承载在承载面上的对象物体。

此外，在本发明的上述实施方式中，虽然机械臂5的基端被配置在作业台附近的壁面91上，但在无壁面的岛式厨房的情况下，可以承载在顶棚面或岛式厨房的作业侧面等适于进行作业的场所。

图2是表示构成机器人系统1的控制装置4和作为控制对象的机械臂5的详细结构的图。并且，在图4A中放大示出机械臂5的前臂连杆32、手腕部31、手30。

控制装置4在此上述实施方式中，作为一例，由普通的个人电脑构成，控制装置4由估算参数数据库7、动作轨道程序用数据库37、和搬运控制部8构成。其中，如图3A及图3B所示，估算参数数据库7、动作轨道程序用数据库37、和搬运控制部8可作为控制程序6构成。

构成输入输出IF(接口)9，以便包括连接在个人电脑的PCI总线等的扩展槽(extended slot)上的例如D/A板、A/D板、计数器板等。

控制装置4经由作为输入部的一例的输入输出IF9与驱动机械臂5的各连杆操纵装置的马达驱动器10连接，向此马达驱动器10发送控制信号。

通过控制装置4执行控制机械臂5的工作，由机械臂5的各关节部的后述的编码器11输出的各关节角度信息、以及检测力信息且由后述的力检测部13输出的在机械臂5的钳爪(手30)上产生的力信息，通过输入输出IF9的计数器板被取入到控制装置4中，基于取入的各关节角度信息及力信息，由控制装置4计算各关节部的旋转动作中的控制指令值。计算出的各控制指令值通过输入输出IF9的D/A板，被给予用于驱动控制机械臂5的各关节部的马达驱动器10，按照由马达驱动器10送来的各控制指令值，来驱动机械臂5的各关节部的马达12。作为驱动部的一例，用此马达驱动器10和马达12发挥作用。此外，编码器11作为输出角度信息的角度检测部的一例起作用。

机械臂5作为一例是6自由度的多连杆操纵装置，包括：手30；在前端具有安装有手30的手腕部31的前臂连杆32；前端可旋转地连接在前臂连杆32的基端上的上臂连杆33；和可旋转地连接支持上臂连杆33的基端且固定在壁面91上的台部34。

手腕部31具有第4关节部27、第5关节部28、第6关节部29三个旋转轴，能使手30相对前臂连杆32的相对的姿势(方向)变化。即，在图2、图4A中，第4关节部27能使手30相对手腕部31的环绕Ψ_x轴(纵轴)的相对的姿势变化。第五关节部28能使手30相对手腕部31的、环绕与第4关节部27的Ψ_x轴正交的Ψ_y轴(横轴)的相对的姿势变化。第6关节部29能使手30相对手腕部31的、环绕与第四关节部27的Ψ_x轴及第5关节部28的Ψ_y轴分别正交的钳爪前端方向的轴Ψ_z(横轴)的相对的姿势变化。即，手30能够相对于前臂连杆32的一端32a在Ψ_x轴、Ψ_y轴、Ψ_z轴的三轴方向上分别独立地旋转。在此，Ψ_x轴、Ψ_y轴、Ψ_z轴是彼此正交的坐标轴。此外，Ψ_x轴、Ψ_y轴、Ψ_z轴不必与图2中的后述的钳爪坐标系36一致。

前臂连杆32的另一端32b能够相对上臂连杆33的一端33a环绕第3关节部26旋转。在上述实施方式中，能够环绕第3关节部26旋转意味着能够环绕与第5关节部28的Ψ_y轴平行的横轴旋转。上臂连杆33的另一端33b能够相对台部34环绕第2关节部25旋转。在上述实施方式中，能够环绕第2关节部25旋转意味着能够环绕与第5关节部28的Ψ_y轴(第3关节部26的横轴)平行的横轴旋转。台部34的上侧可动部34a能够相对台部34的下侧固定部34b环绕第1关节部24旋转。在上述实施方式中，能够环绕第1关节部24旋转意味着能够环绕绝对坐标系35的z轴旋转。

其结果，机械臂5能够分别独立地环绕第1关节部24至第6关节部29合计6个轴旋转，构成上述6自由度的多连杆操纵装置。

在构成各轴的旋转部分的各关节部中包括：作为配备在构成各关节部的一对构件(例如转动侧构件、支持该转动侧构件的支持侧构件)中的一个构件上、且通过后述的马达驱动器10进行驱动控制的旋转驱动装置的一例的马达12(实际上配置在机械臂5的各关节部的内部中)；和检测马达12的旋转轴的旋转相位角(即关节角)的编码器11(实际中，配置在机械臂5的各关节部的内部)。因此，通过将配置在构成各关节部的一个构件上的马达12的旋转轴连接在各关节部的另一构件上，并使上述旋转轴进行正逆旋转，就能使另一构件相对一个构件环绕各轴旋转。

35是相对台部34的下侧固定部34b的相对的位置关系固定的绝对坐标系，36是相对手30的相对的位置关系固定的钳爪坐标系。设从绝对坐标系35观察到的钳爪坐标系36的原点位置O_e(x，y，z)为机械臂5的手30的位置(即钳爪位置)。而且，使用滚动角、俯仰角、和偏转角用坐标(φ，θ，ψ)表现从绝对坐标系35观察到的钳爪坐标系36的姿势。设此坐标(φ，θ，ψ)为机械臂5的钳爪姿势(手30的姿势)，将钳爪位置及姿势向量定义为向量r＝[x，y，z，φ，θ，ψ]^T。使用图4B～图4D说明滚动角、俯仰角、偏转角。首先，以绝对坐标系35的Z轴为旋转轴，考虑使坐标系旋转角度φ后得到的坐标系(参照图4B)。设此时的坐标系为[X′，Y′，Z]。接着，以Y′为旋转轴使此坐标系旋转角度θ(参照图4C)。设此时的坐标轴为[X″，Y′，Z″]。最后，以X″为旋转轴使此坐标系旋转角度ψ(参照图4D)。此时的坐标系的姿势为滚动角度φ、俯仰角度θ、偏转角度ψ，此时的姿势向量成为(φ，θ，ψ)。在钳爪坐标系36与姿势(φ，θ，ψ)坐标系将原点位置平行移动到钳爪坐标系36的原点位置O_e(x，y，z)后得到的坐标系一致的时候，设钳爪坐标系的姿势向量为(φ，θ，ψ)。

在控制机械臂5的钳爪位置及姿势的情况下，通过搬运控制部8能使钳爪位置及姿势向量r追随由后述的目标轨道生成部18生成的钳爪位置及姿势目标向量r_d。

力检测部13检测或估算在机械臂5的手30和搬运物体3之间产生的力，并输出其信息。作为一例，虽然假设力检测部13为处于手30和手腕部31之间的6轴的力传感器，但在限定物体3的搬运方向的情况等中即使是6轴以下也无妨。但是，至少需要能够始终检测铅直方向的力的机构或装置。来自力检测部13的输出被输出给估算参数数据库7、搬运力估算部14、目标轨道生成部18、以及搬运物承载判定部(搬运物体承载判定部)19。

此外，作为力检测部13的另一例子，也可以不使用传感器，而构成基于马达电流值等进行估算的力估算部(作为一例，参照图3B的13A)。此情况下，力检测部13(力估算部13A)的外力的估算按如下的方法进行估算。马达驱动器10的电流传感器10a测量流过对机械臂5的各关节部进行驱动的马达12的电流值i＝[i₁，i₂，i₃，i₄，i₅，i₆]^T。由马达驱动器10的电流传感器测量出的电流值i＝[i₁，i₂，i₃，i₄，i₅，i₆]^T经由输入输出IF9的A/D板被取入到力检测部13(力估算部13A)中。此外，在关节角的现在值q经由输入输出IF9的计数器板被取入到力检测部13(力估算部13A)中的同时，还将来自后述的近似逆运动学(inverse kinematics)计算部22的关节角度误差补偿输出u_qe取入到力检测部13(力估算部13A)中。

力检测部13(力估算部13A)作为观测器起作用，根据上述电流值i、关节角的现在值q、关节角度误差补偿输出u_qe的各信息，计算由施加给机械臂5的外力在各关节部产生的扭矩τ_ext。而且，力检测部13(力估算部13A)进一步根据F_ext＝J_v(q)^-Tτ_ext将其换算成机械臂5的钳爪的等效钳爪外力F_ext，并将换算后的力F_ext输出给估算参数数据库7、搬运力估算部14、目标轨道生成部18、和搬运物承载判定部19。在此，J_v(q)是满足式(1)的雅可比矩阵。

[数1]

v＝Jv(q)q

···式(1)

其中，v＝[v_x，v_y，v_z，λ_x，λ_y，λ_z]^T，(v_x，v_y，v_z)是钳爪坐标系36中的机械臂5的钳爪的平移速度，(λ_x，λ_y，λ_z)是钳爪坐标系36中的机械臂5的钳爪的角速度。

图3A及图3B是表示搬运控制部8的详细结构的图。搬运控制部8结构为包括：搬运力估算部14、力控制部15、和搬运物承载判定部19。力控制部15结构为包括：阻抗控制部17、位置误差计算部40、位置控制部16、和目标轨道生成部18。并且，位置控制部16结构为包括：位置误差补偿部20、正运动学(forward kinematics)计算部21、和近似逆运动学计算部22，根据机械臂5的目标轨道、以及机械臂5的现在位置及姿势，由位置控制部16输出机械臂5的指令值。再有，力控制系统除上述方法外，还可以使用混合控制等其它的力控制方法。

从机械臂5输出由各个关节部的编码器11测量出的关节角的现在值(关节角度向量)向量q＝[q₁，q₂，q₃，q₄，q₅，q₆]^T，通过输入输出IF9的计数器板被取入到搬运控制部8中。其中，q₁、q₂、q₃、q₄、q₅、q₆分别是从编码器11检测出的第1关节部24、第2关节部25、第3关节部26、第4关节部27、第5关节部28、第6关节部29的关节角度。

目标轨道生成部18基于钳爪位置及姿势向量r、动作轨道程序用数据库37、来自力检测部13的力信息、和来自后述的搬运物承载判定部19的搬运物承载判定信息，输出用于实现设为目标的机械臂5的动作的钳爪位置及姿势目标r_d。在沿事先决定的轨道进行位置控制的情况下，设为目标的机械臂5的动作，被事先给予动作轨道程序，其中，该动作轨道程序按照设为目标的作业将各个时间(t＝0、t＝t₁、t＝t₂、…)中的每一点的位置(r_d0、r_d1、r_d2、…)存储在动作轨道程序用数据库37中；目标轨道生成部18基于来自动作轨道程序用数据库37的上述各个时间(t＝0、t＝t₁、t＝t₂、…)中的每一点的位置(r_d0、r_d1、r_d2、…)的信息、和钳爪位置及姿势向量r，使用多项式插值，补充各点间的轨道，生成钳爪位置及姿势目标向量r_d。此外，在进行力控制的时候，通过从力控制开始时起停止钳爪位置及姿势目标向量r_d的更新，就能由后述的钳爪位置及姿势目标修正输出r_dΔ实现力控制。另外，在想进行位置及姿势中仅一部分的力控制的情况下，通过停止想进行力控制的成分的更新，就能实现。

阻抗控制部17是发挥在机械臂5中实现向机械阻抗设定值的、上述机械臂5的机械阻抗的值的控制的功能的部分，输入后述的来自搬运物承载判定部19的搬运物承载判定信息，并且还根据通过实验事前设定的阻抗参数即惯性M、粘性D、刚性K和搬运力估算部14输出的搬运力F_carry，按以下的式(2)计算用于实现上述机械臂5的机械阻抗的值的、向机械阻抗设定值的控制的钳爪位置及姿势目标修正输出r_dΔ，并向位置控制部16输出。钳爪位置及姿势目标修正输出r_dΔ，通过位置误差计算部40与目标轨道生成部18输出的钳爪位置及姿势目标向量r_d相加，生成钳爪位置及姿势修正目标向量r_dm。

[数2]

$r_{\mathrm{d\Δ}}={(s^2\hat{M}+s\hat{D}+\hat{K})}^{-1}{F}_{\mathrm{carry}}$

····式(2)

其中，

[数3]

$\hat{M}=\left[\begin{array}{cccccc}M& 0& 0& 0& 0& 0\\ 0& M& 0& 0& 0& 0\\ 0& 0& M& 0& 0& 0\\ 0& 0& 0& M& 0& 0\\ 0& 0& 0& 0& M& 0\\ 0& 0& 0& 0& 0& M\end{array}\right]$

····式(3)

[数4]

$\hat{D}=\left[\begin{array}{cccccc}D& 0& 0& 0& 0& 0\\ 0& D& 0& 0& 0& 0\\ 0& 0& D& 0& 0& 0\\ 0& 0& 0& D& 0& 0\\ 0& 0& 0& 0& D& 0\\ 0& 0& 0& 0& 0& D\end{array}\right]$

····式(4)

[数5]

$\hat{K}=\left[\begin{array}{cccccc}K& 0& 0& 0& 0& 0\\ 0& K& 0& 0& 0& 0\\ 0& 0& K& 0& 0& 0\\ 0& 0& 0& K& 0& 0\\ 0& 0& 0& 0& K& 0\\ 0& 0& 0& 0& 0& K\end{array}\right]$

····式(5)

s是拉普拉斯算子。

经由输入输出IF9的计数器板，将由机械臂5的各关节部的编码器11测量出的关节角的现在值q即关节角度向量q输入给正运动学计算部21。然后，在正运动学计算部21中，进行从机械臂5的关节角度向量q向钳爪位置及姿势向量r的转换的几何科学计算。因此，正运动学计算部21作为检测机械臂5的钳爪位置的钳爪位置检测部的一例起作用。由正运动学计算部21计算出的钳爪位置及姿势向量r被输出给目标轨道生成部18、搬运物承载判定部19、位置误差补偿部20、和搬运力估算部14。

位置误差补偿部20输入根据在机械臂5中测量的关节角度向量q由正运动学计算部21计算出的钳爪位置及姿势向量r、和钳爪位置及姿势修正目标向量r_dm的误差r_e(＝r_dm-r)，从位置误差补偿部20向近似逆运动学计算部22输出位置误差补偿输出u_re。

在近似逆运动学计算部22中，基于从位置误差补偿部20输入的位置误差补偿输出u_re和在机械臂5中测量的关节角度向量q，通过近似式u_out＝J_r(q)^-1u_in进行逆运动学的近似计算。其中，J_r(q)是满足

[数6]

$\stackrel{\·}{r}=J_r\left(q\right)\stackrel{\·}{q}$

关系的雅可比矩阵，u_in是向近似逆运动学计算部22的输入，u_out是自近似逆运动学计算部22的输出，如果设输入u_in为关节角度误差q_e，则按照q_e＝J_r(q)^-1r_e，成为从钳爪位置及姿势误差r_e向关节角度误差q_e的转换式。因此，如果从位置误差补偿部20将位置误差补偿输出u_re输入到近似逆运动学计算部22，则作为来自近似逆运动学计算部22的输出，将用于补偿关节角度误差q_e的关节角度误差补偿输出u_qe从近似逆运动学计算部22向机器人5A的马达驱动器10输出。

关节角度误差补偿输出u_qe经由输入输出IF9的D/A板被给予机器人5A的马达驱动器10，以作为电压指令值，由各个马达12正逆旋转驱动各关节部，机械臂5工作。

关于以上这样构成的阻抗控制部17，说明机械臂5的阻抗控制工作的原理。

阻抗控制工作的根本是基于位置误差补偿部20的钳爪位置及姿势误差r_e的反馈控制(位置控制)，由图3A及图3B的虚线包围且由参照符号16表示的部分为位置控制部16。作为位置误差补偿部20，例如如果使用PID补偿器的话，则基于位置控制部16的控制进行工作，以便钳爪位置及姿势误差r_e收敛于0，能实现作为目标的机械臂5的阻抗控制工作。

搬运力估算部14根据来自估算参数数据库7的输出信息、来自力检测部13的输出信息、以及机械臂5的钳爪的姿势信息q即关节角度数据(关节变量向量或关节角度向量q)，估算机械臂5和人2的协作搬运所需的搬运力F_carry。使用图5、图6A～图6B说明一例。在图5中用参照符号23表示的ω_p是机械臂5的钳爪与水平面所成的角度，表示通过估算参数数据库7由钳爪位置及姿势向量r所决定的角度。在此，在此角度ω_p中，设图5的箭头标记的方向(逆时针旋转)为正。如图6A所示，设当人2的腕2a和机械臂5分别支撑物体3的两个把手3a、3b时，由力检测部13检测出的铅直方向的力为F_z(≤0)。铅直方向的力F_z的方向以图2的绝对坐标系35的z轴的方向为正。设此时的力控制的铅直方向的输入(从搬运力估算部14向力控制部15的力控制的输入中的铅直方向分量)为

[数7]

(-k_pF_z+k_ωω_p)    …式(7)

在此，k_p及k_ω作为一例，是由用力检测部13检测出的铅直方向的力F_z的值、和机械臂5的钳爪与水平面所成角度ω_p的值的正负所决定的参数，是由用力检测部13检测出的铅直方向的力F_z的值、和机械臂5的钳爪与水平面所成角度ω_p的值的正负所决定的参数(系数)。再有，k_p及k_ω也可以不根据角度ω_p的值的正负，而由角度ω_p的值更细地进行参数设定，也可以广义地定义为由铅直方向的力F_z和角度ω_p所唯一决定的参数。

如图6A所示，当-k_pF_zo+(-k_ωω_p0)＝0时，由于力控制的输入变为0，所以此状态成为平衡点，静止。在此，如图6A所示，ω_p0代表人2持有物体3时，在机械臂5和人2之间平衡保持物体3时的机械臂5的钳爪相对水平面的角度。此外，如图6A所示，F_z0是人2持有物体3时，在机械臂5和人2之间平衡保持物体3时的机械臂5的钳爪的铅直方向的力。

如图6B所示，从图6A的平衡状态开始，人2向上提升物体3的一个把手3b，变成ω_p＝ω_p+的状态(相对水平面人2提升物体3的把手3b为角度ω_p+的状态)。此时，为-k_pF_z+k_ωω_p+＞0，由于力控制的铅直方向的输入向上，所以机械臂5上升。相反，如图6C所示，从图6A的平衡状态开始，人2增加向下的力，变成ω_p＝ω_p-的状态(相对水平面人2降下物体3的把手3b为角度ω_p-的状态)。此时，为-k_pF_z+k_ωω_p-＜0，由于力控制的铅直方向的输入向下，所以机械臂5下降。

如此，在搬运力估算部14中，通过以上述式(7)为力控制的铅直方向的输入，人2就能在想要上下移动的方向上与机械臂5协作工作，就能实现机械臂5和人2的协作搬运物体3。此外，在此技术中，事前不需要搬运的物体3的质量信息。并且，在物体3的重量变化的情况下，由力检测部13检测出的铅直方向的力F_z变化。其结果，由于在物体3的重量增加的情况下，机械臂5增加向上的力，另一方面，在物体3的重量减少的情况下，机械臂5减少向上的力，所以即使物体3重量发生变化，也无任何障碍，能进行协作搬运工作。

如果在搬运的物体3的重量的变动比较少的情况下(例如在预先设定物体3的重量的变动幅度的阈值为5kg时，搬运的物体的重量为5kg以下的话)(换言之，如果是在人2侧可吸收的程度的物体3的重量变动的话)，则在上述式(7)中，设k_p＝0，仅使用k_ωω_p即可。通常，由于在力检测部13的输出中存在噪声，所以通过仅设为k_ωω_p(即通过获取噪声的原因)，就能减少因噪声而导致的振动原因。

如图6D所示，在搬运的物体3或机械臂5的手30之下存在与搬运的物体3相干扰的物体43的状况下，因在搬运的物体3和干扰的物体43之间产生的力

[数8]

N_z

而使得机械臂5所受到的铅直方向的向下的力F_z变得接近0(例如变为0.5N左右)。

在此，在无干扰的物体43的情况下，机械臂5所负担的物体3的重量始终作为铅直方向向下的力被检测。如果由力检测部13检测出的铅直方向的力F_z比0大的时候，则由搬运力估算部14判断为在搬运的物体3或机械臂5的手30之下存在干扰的物体43，为了安全，搬运力估算部14仍旧输出由力检测部13检测出的铅直方向的力不变。由此，能防止搬运的物体3或机械臂5的手30和干扰的物体43相互挤压。

基于这些情况，设由力检测部13检测出的铅直方向的力为F_z，此外，考虑力检测部13的噪声或误差的影响(但是，物体3和机械臂5的手30之间，由于通过手30的物体3的夹具等的把持而相对不动。)，如果设搬运力估算部14中的是否考虑噪声或误差的影响的切换的阈值为-ε，则铅直方向的搬运力

[数9]

被决定为

[数10]

$\left\{\begin{array}{ccc}\hat{F}={-k}_p{F}_z+k_{\mathrm{\ω}}{\mathrm{\ω}}_p& \mathrm{if}& F_z\≤-\mathrm{\ϵ}\\ \hat{F}=F_z& \mathrm{if}& F_z>-\mathrm{\ϵ}\end{array}\right.$

···式(8)

根据物体3的重量而变化的力由于是铅直方向的力及环绕俯仰轴的扭矩(参照图4B～图4D)，所以关于其之外的力，仍旧从搬运力估算部14输出由力检测部13检测出的力不变。如果由力检测部13检测出的6轴的力为

[数11]

F_ext＝[F_x F_y F_z F_φ F_θ F_ψ]^T

则搬运力估算部14的输出为

[数12]

$F_{\mathrm{carry}}={\left[\begin{array}{cccccc}{F}_x& F_y& \hat{F}& F_{\mathrm{\φ}}& l\·\hat{F}& F_{\mathrm{\ψ}}\end{array}\right]}^T$

···式(9)

在此，

[数13]

l

是从机械臂5的钳爪到搬运的物体3的重心的距离。

估算参数数据库7基于力检测部13的输出信息F_z及机械臂5的钳爪的姿势信息ω_p，分别适当选择参数k_p、k_ω，输出给搬运力估算部14。

在图7中示出估算参数数据库7的一例。在此估算参数数据库7中，对应在机械臂5的钳爪中预先产生的力的铅直分量F_z、和机械臂5的钳爪与水平面所成的角度ω_p的组合，分别预先设定参数k_p、k_ω的值并加以存储。因此，如果将上述力的铅直分量和角度ω_p的信息输入给估算参数数据库7，则从估算参数数据库7中分别输出参数k_p、k_ω的值。例如，F_z＝-10或ω_p＝0.1(即ω_p为正)时，从估算参数数据库7中输出k_p＝0.52、k_ω＝32。

接着，说明搬运物承载判定部19的详情。在上述实施方式的情况下，由于噪声或作为力检测部13的一例的力传感器的偏移(offset)偏差等，如图10所示，虽然人2将搬运的物体3放置在台90的上面(即物体承载面90a)之上，却因与机械臂5的手30的力的平衡，而会引起物体3成为倾斜的状态。具体地，例如物体3的一个把手3a仍旧被把持在机械臂5的手30上不变，通过降下持有物体3的另一把手3b的人2的腕2a，就会引起成为物体3向人2侧倾斜的状态。

因此，通过搬运物承载判定部19就能判定搬运中的物体3是否承载在物体承载面90a上。一面参照图11(a)～(c)的图表一面说明搬运物承载判定部19的搬运物承载判定方法的一例。

首先，在人2将搬运的物体3放置在台90的物体承载面90a之上前，通常将搬运的物体3从上向下移动，在某个位置静止。这样的动作，通过检测机械臂5的钳爪位置中的z位置(钳爪位置及姿势向量r中的z分量)的变动，就能够由搬运物承载判定部19进行判定。具体地，在图11中，如表示钳爪z位置和时间t之间的关系的图表(a)所示，当着眼于机械臂5的钳爪z位置(z方向的钳爪位置)时，在物体3的承载的瞬间即时间t₁以前的期间中的z位置持续下降，在比物体3的承载的瞬间即时间t₁往后的期间中的z位置既不下降也不上升，为恒定。

因此，如果搬运物承载判定部19检测出在从正运动学计算部21输入的钳爪z位置持续下降后，在比某一时间t₁往后的期间中的z位置既不下降也不上升而为恒定的话，则搬运物承载判定部19判定为承载了搬运物。z位置为恒定的判定，例如如图11(a)所示，是在一定时间的期间、z位置处于一定范围Δz内的时候判定为是恒定的。一定范围Δz通过事前的实验等被预先设定为例如10mm等。除此之外，计算z的时间微分，在z的时间微分的绝对值在阈值以下的情况下，即z的速度在阈值以下的情况下，也可以设z是恒定的等。

此外，在上述人2要将搬运的物体3放置在台90的物体承载面90a之上前，如图10所示，物体3的一个把手3a仍旧被机械臂5的手30所把持，持有物体3的另一把手3b的人2的腕2a会降下。因此，通过降下持有物体3的另一把手3b的人2的腕2a，在物体3向人2侧倾斜时，机械臂5的钳爪与水平面所成的角度ω_p就变得比水平面更向下，通过检测这种情况，就能判定为人2要开始将搬运的物体3承载在物体承载面90a上。具体地，在图11中，如表示角度ω_p和时间t之间的关系的图表(b)所示，时间t₁以后的期间，角度ω_p为负的值且为恒定。

因此，在搬运物承载判定部19内根据位置及姿势向量r计算角度ω_p，在某一时间t₁以后的期间中，当搬运物承载判定部19检测出角度ω_p为负的值且为恒定时，搬运物承载判定部19就判定为搬运物向人2侧倾斜，能判定为人2要开始将搬运的物体3承载在物体承载面90a上。

在与上述z位置同样的手法中能够判定在此所说的恒定。此外，从力检测部13输入到搬运物承载判定部19的、在机械臂5和物体3之间产生的铅直方向的力F_z，在人2向比水平面更向下移动持有物体3的把手3b的腕2a时，上述铅直方向的力F_z具有向下的值，当将物体3承载在上述承载面90a上时，上述铅直方向的力F_z的绝对值变小，且接近0。承载后，上述铅直方向的力F_z是负的值且为恒定状态。通过检测这种情况，就能设人2要将搬运的物体3承载在物体承载面90a上，并在此后判定为已承载。具体地，在图11中，如表示铅直方向的力F_z和时间t之间的关系的图表(c)所示，铅直方向的力F_z在物体3的承载的瞬间即时间t₁以前的期间中，上述铅直方向的力F_z具有向下的值，在物体3的承载的瞬间即时间t₁之前的期间中，上述铅直方向的力F_z上升，在比物体3的承载的瞬间即时间t₁往后的期间，上述铅直方向的力F_z为负的值且为恒定。

当从力检测部13输入的上述铅直方向的力F_z具有向下的值，且在某一时间t₁之前的期间中，上述铅直方向的力F_z减少，在比上述某一时间t₁往后的期间，由搬运物承载判定部19检测出上述铅直方向的力F_z为负的值且为恒定时，搬运物承载判定部19判定为已承载搬运物3。搬运物承载判定部19对上述铅直方向的力F_z减少了的判定，是在对上述铅直方向的力F_z进行时间微分后的值是某一阈值以上时，由搬运物承载判定部19判定为上述铅直方向的力F_z减少了。阈值通过事前的实验等，被预先设定为例如5N/s等。此外，对恒定的判定，在与上述z方向的位置相同的手法中，能够判定。

基于以上的情况，当搬运物承载判定部19根据机械臂5的钳爪z位置的信息、机械臂5的钳爪与水平面所成的角度ω_p的信息、力检测部13的输出中的铅直方向的力信息F_z，基于作为上述角度检测部的一例的编码器11的输出，判定为上述机械臂5的钳爪的上述手30比水平面更向下倾斜，并且，基于作为上述钳爪位置检测部的一例的上述正运动学计算部21的输出，判定为钳爪z位置的变动从下降转变为恒定，并且，基于上述力检测部13的输出，判定为铅直方向向下的力转变为减少时(在图11中，从承载物体3的瞬间即时间t₁开始经过一定时间Δt后的时间t₂时)，由搬运物承载判定部19认为“物体3被承载在承载面90a上”。一定时间Δt通过事前的实验等，被预先设定为例如1秒等。

接着，说明搬运物承载判定部19认为“物体3向物体承载面90a上的承载已解除”时的情况。通常，由于假设承载物体3的场所是水平面，所以认为机械臂5的钳爪角度处于从比水平方向更向下到水平方向之间。由此，在通过搬运物承载判定部19判定为“物体3向物体承载面90a上承载中”的期间，在持续输出代表搬运物承载的信号的同时，还持续从编码器11获取机械臂5的钳爪角度，在机械臂5的钳爪角度比水平面更向上的时刻，搬运物承载判定部19认为“物体3的承载已解除”，搬运物承载判定部19停止代表搬运物承载的信号的输出。此外，即便物体3已承载在承载面90a上，在机械臂5和物体3之间产生的铅直方向的力F_z，通常也处于从向下的力到0之间。因此，当通过力检测部13检测出此范围以外的上述铅直方向的力F_z(即，力检测部13检测出上述铅直方向的力F_z变为向上的力)，且此检测信息被输入给搬运物承载判定部19时，由于搬运物承载判定部19能够认清是“机械臂5的手30向承载面90a推压物体3的状态”，还是“人2向上方提升物体3的状态”，所以，此时搬运物承载判定部19也认为“物体3的承载已解除”。

基于图8、图9的流程图说明以上的搬运控制部8的工作步骤。这些工作步骤的功能，作为控制程序6(参照图2)可分别由计算机执行。

最初，基于图8的流程图说明搬运物承载判定部19不工作的情形。

由机械臂5的关节部的各自的编码器11测量出的关节角度数据(关节变量向量或关节角度向量q)被从编码器11取入给控制装置4(步骤S1)。

接着，基于取入到控制装置4中的关节角度数据(关节变量向量或关节角度向量q)，由近似逆运动学计算部22进行机械臂5的运动学计算所需的雅可比矩阵J_r等的计算(步骤S2)。

接着，由正运动学计算部21，根据来自机械臂5的各自的编码器11的关节角度数据(关节变量向量或关节角度向量q)，计算机械臂5的现在的钳爪位置及姿势向量r，输出给估算参数数据库7、位置误差计算部40、和目标轨道生成部18等(步骤S3)。

接着，力检测部13根据安装在机械臂5上的力传感器的输出，计算在钳爪产生的F_ext，并输出给估算参数数据库7、搬运力估算部14、和目标轨道生成部18等(步骤S4)。

接着，搬运力估算部14根据在步骤S3由正运动学计算部21计算求出的钳爪位置及姿势向量r，决定角度ω_p。在如图4B～图4D所示来定义钳爪姿势的情况下，角度ω_p＝θ。并且，搬运力估算部14将角度ω_p输出给估算参数数据库7(步骤S5)。

接着，根据力检测部13的输出即力F_ext及上述角度ω_p，由估算参数数据库7分别决定参数k_p及k_ω的值，并从估算参数数据库7分别向搬运力估算部14输出(步骤S6)。

接着，搬运力估算部14根据参数k_p及k_ω、在钳爪产生的力F_ext、角度ω_p、和关节角度数据(关节变量向量或关节角度向量q)，计算搬运力F_carry，并向阻抗控制部17输出(步骤S7)。

接着，基于预先存储的机械臂5的动作轨道程序用数据库37的动作轨道程序、和在步骤S3由正运动学计算部21计算求出的钳爪位置及姿势向量r，目标轨道生成部18计算机械臂5的钳爪位置及姿势目标向量r_d(步骤S8)。

接着，阻抗控制部17根据通过实验预先设定的机械阻抗参数的惯性M、粘性D、刚性K和由搬运力估算部14计算出并施加给机械臂5的等效钳爪外力F_carry，通过阻抗控制部17计算钳爪位置及姿势目标修正输出r_dΔ，并输出给位置误差计算部40(步骤S9)。

接着，位置误差计算部40计算钳爪位置及姿势修正目标向量r_dm，其中，钳爪位置及姿势修正目标向量r_dm作为来自目标轨道生成部18的钳爪位置及姿势目标向量r_d、与来自阻抗控制部17的钳爪位置及姿势目标修正输出r_dΔ之和(r_d+r_dΔ)。接着，位置误差计算部40计算钳爪位置及姿势的误差r_e，并输出给位置误差补偿部20，其中，该钳爪位置及姿势的误差r_e作为钳爪位置及姿势修正目标向量r_dm、与来自正运动学计算部21的现在的钳爪位置及姿势向量r之差(r_dm-r)。最后，位置误差补偿部20计算位置误差补偿输出u_re，并从位置误差补偿部20输出给近似逆运动学计算部22(步骤S10)，其中，位置误差补偿输出u_re是用于控制机械臂5以便将钳爪位置及姿势的误差r_e收敛在0的控制输入。作为位置误差补偿部20的具体例子，考虑PID补偿器。通过适当调整作为常数的对角矩阵的比例、微分、积分三个增益，进行位置误差补偿部20的控制以便将位置误差收敛在0。

接着，近似逆运动学计算部22通过将在步骤S2计算出的雅可比矩阵J_r的逆矩阵与位置误差补偿输出u_re相乘，从而将位置误差补偿输出u_re从与钳爪位置及姿势的误差相关的值转换为与关节角度的误差相关的值即关节角度误差补偿输出u_qe，并输出给机器人5A的马达驱动器10(步骤S11)。

接着，关节角度误差补偿输出u_qe，将来自近似逆运动学计算部22的信息通过输入输出IF9，被给予马达驱动器10，马达驱动器10基于关节角度误差补偿输出u_qe使流过关节部的各个马达12的电流量变化。根据此电流量的变化，产生机械臂5的各个关节部的旋转运动，机械臂5进行工作(步骤S12)。

接着，在持续上述搬运控制工作的步骤S1～步骤S12的情况下，返回步骤S1，另一方面，在此之外的情况下(不是断开电源或使机器人停止，而是仅结束上述搬运控制工作的情况)，结束上述搬运控制工作的步骤S1～步骤S12(步骤S13)。

通过重复执行以上的步骤S1～步骤S12作为控制的计算循环，就能实现机械臂5的动作的控制，即人2和机械臂5协作搬运物体3的搬运工作。

接着，基于图9的流程图说明搬运物承载判定部19进行搬运物的承载判定工作的情形。

由机械臂5的关节部的各个编码器11测量出的关节角度数据(关节变量向量或关节角度向量q)被从编码器11取入到控制装置4(步骤S21)。

接着，基于取入到控制装置4中的关节角度数据(关节变量向量或关节角度向量q)，由近似逆运动学计算部22进行机械臂5的运动学计算所需的雅可比矩阵J_r等的计算(步骤S22)。

接着，由正运动学计算部21根据来自机械臂5的各个编码器11的关节角度数据(关节变量向量或关节角度向量q)，计算机械臂5的现在的钳爪位置及姿势向量r，输出给估算参数数据库7、位置误差补偿部20、目标轨道生成部18、和搬运物承载判定部19(步骤S23)。

接着，力检测部13根据安装在机械臂5上的力传感器的输出，计算在钳爪产生的F_ext，并输出给估算参数数据库7、搬运力估算部14、目标轨道生成部18、和搬运物承载判定部19(步骤S24)。

接着，搬运力估算部14及搬运物承载判定部19根据在步骤S3由正运动学计算部21计算求出的钳爪位置及姿势向量r，决定角度ω_p。在如图4B～图4D所示来定义钳爪姿势的情况下，角度ω_p＝θ。并且，搬运力估算部14将角度ω_p输出给估算参数数据库7(步骤S25)。

接着，搬运物承载判定部19输入力检测部13的输出中的铅直方向的力信息F_z(步骤S26)。并且根据在步骤S3由正运动学计算部21计算求出的钳爪位置及姿势向量r，决定钳爪z位置(钳爪位置及姿势向量r中的z分量)。

接着，基于输入的信息，搬运物承载判定部19检测搬运的物体3是否被承载在桌子等台90的物体承载面90a上，进行此判定(步骤S27)。具体地，如前面所说明的，搬运物承载判定部19根据来自上述正运动学计算部21的机械臂5的钳爪z位置的信息、钳爪与水平面所成的角度ω_p的信息、和来自力检测部13的输出中的铅直方向的力信息F_z，判定“物体3是否被承载在承载面90a上”。

因此，例如，当搬运物承载判定部19基于根据由上述正运动学计算部21计算求出的钳爪位置及姿势向量r计算出的钳爪与水平面所成角度ω_p，判定为上述机械臂5的钳爪的上述手30比水平面更向下倾斜，并且，基于上述正运动学计算部21的输出，判定为钳爪z位置的变动从下降转变为恒定，并且，基于上述力检测部13的输出，判定为铅直方向向下的力转变为减少时(在图11中，从承载物体3的瞬间即t₁开始经过一定时间Δt后的时间t₂时)，由搬运物承载判定部19认为“物体3被承载在承载面90a上”。在此，一定时间Δt通过事前的实验等，被预先设定为例如1秒等。

相反，在此之外的情况下，由搬运物承载判定部19判定为“物体3没有被承载在承载面90a上”。

下面说明在步骤S27中，由搬运物承载判定部19判定为“物体3没有被承载在承载面90a上”的时候，进行力控制的情况。

估算参数数据库7根据力检测部13的输出即力F_ext及从搬运力估算部14输入的角度ω_p，由估算参数数据库7分别决定参数k_p及k_ω的值，并从估算参数数据库7分别向搬运力估算部14输出(步骤S28)。

接着，搬运力估算部14根据参数k_p及k_ω、在钳爪产生的力F_ext、角度ω_p、和关节角度数据(关节变量向量或关节角度向量q)，计算搬运力F_carry，并向阻抗控制部17输出(步骤S29)。

接着，基于预先存储的机械臂5的动作轨道程序用数据库37的动作轨道程序、在步骤S23由正运动学计算部21计算求出的钳爪位置及姿势向量r、和来自力检测部13的力信息，目标轨道生成部18计算机械臂5的钳爪位置及姿势目标向量r_d，并输出给位置误差计算部40(步骤S30)。

接着，阻抗控制部17根据通过实验预先设定的机械阻抗参数的惯性M、粘性D、刚性K和由搬运力估算部14计算出并施加给机械臂5的等效钳爪外力F_carry，通过阻抗控制部17计算钳爪位置及姿势目标修正输出r_dΔ，并输出给位置误差计算部40(步骤S31)。

接着，位置误差计算部40计算钳爪位置及姿势修正目标向量r_dm，其中，钳爪位置及姿势修正目标向量r_dm作为来自目标轨道生成部18的钳爪位置及姿势目标向量r_d、与来自阻抗控制部17的钳爪位置及姿势目标修正输出r_dΔ之和(r_d+r_dΔ)。接着，位置误差计算部40计算钳爪位置及姿势的误差r_e，其中，钳爪位置及姿势的误差r_e作为钳爪位置及姿势修正目标向量r_dm、与来自正运动学计算部21的现在的钳爪位置及姿势向量r之差(r_dm-r)，并输出给位置误差补偿部20。最后，位置误差补偿部20计算位置误差补偿输出u_re，并从位置误差补偿部20输出给近似逆运动学计算部22(步骤S32)，其中，位置误差补偿输出u_re是用于控制机械臂5以便将钳爪位置及姿势的误差r_e收敛在0的控制输入。作为位置误差补偿部20的具体例，考虑PID补偿器。通过适当调整作为常数的对角矩阵的比例、微分、积分三个增益，进行位置误差补偿部20的控制，以便将位置误差收敛在0。

接着，近似逆运动学计算部22，通过将在步骤S22计算出的雅可比矩阵J_r的逆矩阵与位置误差补偿输出u_re相乘，从而将位置误差补偿输出u_re从与钳爪位置及姿势的误差相关的值转换为与关节角度的误差相关的值即关节角度误差补偿输出u_qe，并输出给机器人5A的马达驱动器10(步骤S33)。

接着，关节角度误差补偿输出u_qe，将来自近似逆运动学计算部22的信息通过输入输出IF9，被给予马达驱动器10，马达驱动器10基于关节角度误差补偿输出u_qe使流过关节部的各个马达12的电流量变化。根据此电流量的变化，产生机械臂5的各个关节部的旋转运动，机械臂5进行工作(步骤S34)。

接着，在继续上述搬运控制工作的步骤S21～步骤S36的情况下，返回步骤S21，另一方面，在此之外的情况下(不是断开电源或使机器人停止，而是仅结束上述搬运控制工作的情况)，结束上述搬运控制工作的步骤S21～步骤S36(步骤S37)。

接着，使用图9说明在步骤S27中，在由搬运物承载判定部19判定为“物体3被承载在承载面90a上”的情况下进行位置控制的情形。

在动作轨道程序用数据库37中预先存储在判定为“物体3被承载在承载面90a上”时机械臂5进行工作的轨道。作为用于修正物体3的倾斜的轨道，在此作为一例，设以每秒10mm的速度向铅直方向向下移动。在由搬运物承载判定部19判定为“物体3被承载在承载面90a上”的情况下，将预先存储的承载时的动作轨道程序用数据库37的动作轨道程序输入给目标轨道生成部18。

当在机械臂5的钳爪产生的力中、铅直方向的力F_z为向下的力的时候，目标轨道生成部18基于机械臂5的动作轨道程序用数据库37的动作轨道程序、和在步骤S23由正运动学计算部21计算求出的钳爪位置及姿势向量r，计算机械臂5的钳爪位置及姿势目标向量r_d，并输出给位置误差计算部40。此外，当在机械臂5的钳爪产生的力中、铅直方向的力F_z变为0或向上的时候，目标轨道生成部18不更新机械臂5的钳爪位置及姿势目标向量r_d，而继续向位置误差计算部40输出相同的值(步骤S35)。由此，直到物体3不倾斜地承载在承载面90a上为止，机械臂5都持续下降，在物体3不倾斜地承载在承载面90a上且不给予机器人向下的力的地方，机械臂5停止。

接着，虽然位置误差计算部40计算钳爪位置及姿势修正目标向量r_dm，但由于来自阻抗控制部17的钳爪位置及姿势目标修正输出r_dΔ为0，所以变为r_d＝r_dm，其中，钳爪位置及姿势修正目标向量r_dm作为来自目标轨道生成部18的钳爪位置及姿势目标向量r_d、与来自阻抗控制部17的钳爪位置及姿势目标修正输出r_dΔ之和(r_d+r_dΔ)。接着，由位置误差计算部40计算钳爪位置及姿势的误差r_e，并输出给位置误差补偿部20，其中，钳爪位置及姿势的误差r_e作为钳爪位置及姿势修正目标向量r_dm、与来自正运动学计算部21的现在的钳爪位置及姿势向量r之差(r_dm-r)。最后，由位置误差补偿部20计算位置误差补偿输出u_re，并从位置误差补偿部20输出给近似逆运动学计算部22(步骤S36)，其中，位置误差补偿输出u_re是用于控制机械臂5以便将钳爪位置及姿势的误差r_e收敛在0的控制输入。作为位置误差补偿部20的具体例，考虑PID补偿器。通过适当调整作为常数的对角矩阵的比例、微分、积分三个增益，进行位置误差补偿部20的控制，以便将位置误差收敛在0。

接着，进行上述步骤S34。

通过重复执行以上的步骤S21～步骤S36作为控制的计算循环，就能实现机械臂5的搬运动作及搬运物的承载动作的控制，即就能实现机械臂5的以下动作：人2和机械臂5协作搬运物体3，在物体3的承载时可不使物体3相对水平面倾斜地被承载。

在上述实施方式中，在搬运物承载判定部19判定物体3的承载后，虽然基于承载判定结果，自动地切换机械臂5的力控制和位置控制，以便基于搬运物承载判定部19的判定结果来进行搬运物承载工作，但是为了安全，也可以例如设置按钮等，在搬运物承载判定部19判定物体3的承载且仅在人2按压按钮时，切换机械臂5的力控制和位置控制。由此，由于能仅在人2有意图时切换机械臂5的力控制和位置控制，在人2无意图时不进行机械臂5的力控制和位置控制的切换，所以能进一步提高安全性。

再有，在上述实施方式中，虽然设为相对6轴所有的方向进行力控制，但就铅直方向的移动及俯仰量以外而言，也可以不通过力控制，而通过位置控制进行工作。

如上所述，根据本发明的上述实施方式相关的控制装置4，能提供一种如下控制装置：在由人2和机械臂5协作搬运物体3时，即使在不知道物体3的重量的情况下或在物体3的重量在中途变化的情况下，也能平稳地搬运物体3，且物体3不挤压干扰物体43，能实现安全的机器人控制。

此外，如图12所示，不限于将机械臂5的基端固定在壁面9上，也可以为在固定在壁面9上的一对横杆38上，在沿横杆38可移动的、作为滑动单元的移动体39上支持机械臂5的基端，沿横杆38使机械臂5在横杆38上可在横方向例如水平方向上移动。移动体39，既可以内置能够在控制装置4的控制下自动推进的马达等，或者也可以为移动体39沿横杆38自由移动，并通过人2的力，在搬运方向上与人2一起自由移动。

再有，通过适当组合上述各种各样的实施方式中的任意的实施方式，就能发挥其各自具有的效果。

工业实用性

本发明作为家庭用机器人等进行与人协作作业的机器人、进行机器人的机械臂的动作的控制的机器人(臂)的控制装置及控制方法、以及机器人的控制装置的控制程序是有用的。此外，不限于家庭用机器人，也能作为以下来应用：进行与人协作作业的工业用机器人、或生产设备等中的可动机构的机械臂的控制装置及控制方法、具有机械臂的控制装置的机器人、以及机械臂的控制程序。

本发明虽然参照附图结合优选实施方式进行了充分论述，但对于本领域的普通技术人员而言，各种变形或修正应是明确的。只要不脱离根据附带的权利要求的本发明的范围，这种变形或修正就应该理解为包含在本发明中。

Claims (6)

1.一种机器人，其具有机械臂、和配置在上述机械臂的钳爪上的手，按照由人和上述手来保持位于上述人和上述机器人之间的物体的状态，进行协作，搬运上述物体，

该机器人包括：

驱动部，其调整上述手的角度；

力检测部，其检测并输出作用在上述物体和上述手之间的力；

角度检测部，其检测并输出上述手的上述角度；以及

控制部，其进行以下力控制，即：基于上述角度检测部的输出和上述力检测部的输出来进行控制，以使得上述机械臂的铅直方向的力成为规定力，

上述力检测部检测并输出作用在上述物体和上述手之间的铅直方向的力，

上述控制部基于上述角度检测部的输出和上述力检测部的输出，决定上述力控制的铅直方向的输入，当上述人提升上述物体且上述力控制的铅直方向的输入变为向上时，进行力控制，以使得上述机械臂上升，并且当上述人使上述物体下降且上述力控制的铅直方向的输入变为向下时，进行力控制，以使得上述机械臂下降。

2.一种机器人的控制装置，其中，上述机器人具有机械臂和配置在上述机械臂的钳爪上的手，该机器人的控制装置控制上述机器人，以便按照由人和上述手来保持位于上述人和上述机器人之间的物体的状态，进行协作，搬运上述物体，

该机器人的控制装置包括：

输入部，其输入由力检测部检测出的力信息、以及由角度检测部检测出的角度信息，其中，上述力检测部检测作用在上述物体和上述手之间的力，上述角度检测部检测通过驱动部而被进行角度调整的上述手的角度；

控制部，其进行以下力控制，即：基于上述力检测部的上述力信息和上述角度检测部的上述角度信息来进行控制，以使得上述机械臂的铅直方向的力成为规定力；以及

搬运力估算部，其使用将在上述角度检测部的输出上乘以第一系数后的值、和在上述力检测部的输出的铅直分量上乘以第二系数后的值进行相加而得到的值，以作为上述力控制的铅直方向的输入，来估算上述人要在铅直方向上作用于上述机器人的力，其中，上述第二系数和上述第一系数是由上述力检测部检测出的铅直方向的力的值、和上述钳爪与水平面所成角度的值的正负所决定的参数，

该机器人的控制装置由上述控制部进行以下力控制，即：基于用上述搬运力估算部估算出的上述力来进行修正并进行控制，以使得上述机械臂的铅直方向的力成为上述规定力。

3.一种机器人的控制装置，其中，上述机器人具有机械臂和配置在上述机械臂的钳爪上的手，该机器人的控制装置控制上述机器人，以便按照由人和上述手来保持位于上述人和上述机器人之间的物体的状态，进行协作，搬运上述物体，

该机器人的控制装置包括：

输入部，其输入由力检测部检测出的力信息、以及由角度检测部检测出的角度信息，其中，上述力检测部检测作用在上述物体和上述手之间的力，上述角度检测部检测通过驱动部而被进行角度调整的上述手的角度；

控制部，其进行以下力控制，即：基于上述力检测部的上述力信息和上述角度检测部的上述角度信息来进行控制，以使得上述机械臂的铅直方向的力成为规定力；

钳爪位置检测部，其检测上述机械臂的钳爪的上述手的位置；以及

搬运物承载判定部，其根据上述钳爪位置检测部的输出、上述角度检测部的输出、和上述力检测部的输出，判定上述搬运的物体是否被承载在物体承载面上，

当上述搬运物承载判定部判定为上述物体被承载在上述物体承载面上时，将上述机械臂的铅直方向的控制从上述力控制切换为位置控制。

4.根据权利要求3所述的机器人的控制装置，其中，

在根据上述角度检测部的输出，机械臂的上述手比水平面更向下，且根据上述钳爪位置检测部的输出，上述钳爪位置的变动从下降转变为恒定，且上述力检测部的输出中、铅直方向向下的力转变为减少时，上述搬运物承载判定部判定为上述搬运的物体被承载在上述物体承载面上。

5.一种机器人的控制方法，其中，上述机器人具有机械臂和配置在上述机械臂的钳爪上的手，在该机器人的控制方法中，控制上述机器人，以便按照由人和上述手来保持位于上述人和上述机器人之间的物体的状态，进行协作，搬运上述物体，

从输入部输入由力检测部检测出的力信息、以及由角度检测部检测出的角度信息，其中，上述力检测部检测作用在上述物体和上述手之间的力，上述角度检测部检测通过驱动部而被进行角度调整的上述手的角度，

通过控制部进行以下力控制，即：基于上述力检测部的上述力信息和上述角度检测部的上述角度信息来进行控制，以使得上述机械臂的铅直方向的力成为规定力，

使用将在上述角度检测部的输出上乘以第一系数后的值、和在上述力检测部的输出的铅直分量上乘以第二系数后的值进行相加而得到的值，以作为上述力控制的铅直方向的输入，通过搬运力估算部估算上述人要在铅直方向上作用于上述机器人的力，其中，上述第二系数和上述第一系数是由上述力检测部检测出的铅直方向的力的值、和上述钳爪与水平面所成角度的值的正负所决定的参数，

由上述控制部进行以下力控制，即：基于用上述搬运力估算部估算出的上述力来进行修正并进行控制，以使得上述机械臂的铅直方向的力成为上述规定力。

6.一种机器人的控制方法，其中，上述机器人具有机械臂和配置在上述机械臂的钳爪上的手，在该机器人的控制方法中，控制上述机器人，以便按照由人和上述手来保持位于上述人和上述机器人之间的物体的状态，进行协作，搬运上述物体，

从输入部输入由力检测部检测出的力信息、以及由角度检测部检测出的角度信息，其中，上述力检测部检测作用在上述物体和上述手之间的力，上述角度检测部检测通过驱动部而被进行角度调整的上述手的角度，

通过控制部进行以下力控制，其中，上述力控制为：基于上述力检测部的上述力信息和上述角度检测部的上述角度信息来进行控制，以使得上述机械臂的铅直方向的力成为规定力，

通过钳爪位置检测部检测上述机械臂的钳爪的上述手的位置，

根据上述钳爪位置检测部的输出、上述角度检测部的输出、和上述力检测部的输出，通过搬运物承载判定部判定上述搬运的物体是否被承载在物体承载面上，

当上述搬运物承载判定部判定为上述物体被承载在上述物体承载面上时，将上述机械臂的铅直方向的控制从上述力控制切换为位置控制。

Images