CN116829313A - 机器人控制装置、机器人控制方法以及机器人控制程序 - Google Patents

机器人控制装置、机器人控制方法以及机器人控制程序 Download PDF

Info

Publication number
CN116829313A
CN116829313A CN202280011668.9A CN202280011668A CN116829313A CN 116829313 A CN116829313 A CN 116829313A CN 202280011668 A CN202280011668 A CN 202280011668A CN 116829313 A CN116829313 A CN 116829313A
Authority
CN
China
Prior art keywords
state
operation object
particle
orientation
robot
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
CN202280011668.9A
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.)
Omron Corp
Original Assignee
Omron Corp
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 Omron Corp filed Critical Omron Corp
Publication of CN116829313A publication Critical patent/CN116829313A/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
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • 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/1679—Programme controls characterised by the tasks executed
    • B25J9/1687—Assembly, peg and hole, palletising, straight line, weaving pattern movement
    • 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/1694—Programme controls characterised by use of sensors other than normal servo-feedback from position, speed or acceleration sensors, perception control, multi-sensor controlled systems, sensor fusion
    • B25J9/1697—Vision controlled systems
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06Q—INFORMATION AND COMMUNICATION TECHNOLOGY [ICT] SPECIALLY ADAPTED FOR ADMINISTRATIVE, COMMERCIAL, FINANCIAL, MANAGERIAL OR SUPERVISORY PURPOSES; SYSTEMS OR METHODS SPECIALLY ADAPTED FOR ADMINISTRATIVE, COMMERCIAL, FINANCIAL, MANAGERIAL OR SUPERVISORY PURPOSES, NOT OTHERWISE PROVIDED FOR
    • G06Q10/00—Administration; Management
    • G06Q10/06—Resources, workflows, human or project management; Enterprise or organisation planning; Enterprise or organisation modelling
    • G06Q10/063—Operations research, analysis or management
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06Q—INFORMATION AND COMMUNICATION TECHNOLOGY [ICT] SPECIALLY ADAPTED FOR ADMINISTRATIVE, COMMERCIAL, FINANCIAL, MANAGERIAL OR SUPERVISORY PURPOSES; SYSTEMS OR METHODS SPECIALLY ADAPTED FOR ADMINISTRATIVE, COMMERCIAL, FINANCIAL, MANAGERIAL OR SUPERVISORY PURPOSES, NOT OTHERWISE PROVIDED FOR
    • G06Q50/00—Information and communication technology [ICT] specially adapted for implementation of business processes of specific business sectors, e.g. utilities or tourism
    • G06Q50/04—Manufacturing
    • 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/40034—Disassembly, for recycling
    • 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/40111—For assembly

Landscapes

  • Engineering & Computer Science (AREA)
  • Business, Economics & Management (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Human Resources & Organizations (AREA)
  • Strategic Management (AREA)
  • Economics (AREA)
  • Tourism & Hospitality (AREA)
  • General Business, Economics & Management (AREA)
  • Marketing (AREA)
  • Entrepreneurship & Innovation (AREA)
  • Theoretical Computer Science (AREA)
  • General Physics & Mathematics (AREA)
  • Physics & Mathematics (AREA)
  • Manufacturing & Machinery (AREA)
  • General Health & Medical Sciences (AREA)
  • Health & Medical Sciences (AREA)
  • Development Economics (AREA)
  • Educational Administration (AREA)
  • Primary Health Care (AREA)
  • Game Theory and Decision Science (AREA)
  • Operations Research (AREA)
  • Quality & Reliability (AREA)
  • Manipulator (AREA)
  • Management, Administration, Business Operations System, And Electronic Commerce (AREA)

Abstract

观测更新部(152)进行以下调整：越是表示接近观测结果所示的操作物体的位置姿势的位置姿势的粒子，使粒子的权重越大，并且，在观测结果表示产生了接触的情况下，在表示关注物体和操作物体的形状以及相对位置关系的假想空间中，越是接近关注物体和以各个粒子所表示的位置姿势配置的操作物体接触的状态，使对应的粒子的权重越大。状态推定部(154)计算基于调整了各粒子的权重的粒子的集合而推定出的操作物体的位置姿势的推定状态。行动计划部(144)计划用于将操作物体从推定状态移动到此次移动的目标状态的行动，指示转换部(146)指示机器人执行所计划的行动。反复判定部(156)反复进行目标状态的设定、观测结果的取得、粒子的集合的设定、粒子的集合的调整、推定状态的计算、行动的计划以及行动的执行，直到推定状态在规定的误差内与完成状态一致。

Description

机器人控制装置、机器人控制方法以及机器人控制程序
技术领域
本公开涉及机器人控制装置、机器人控制方法以及机器人控制程序。
背景技术
以往，在使用机器人操作物体的位置姿势的情况下，其条件是末端执行器与把持的物体的相对位置姿势是已知的，在操作中不变化，并且物体的目标位置姿势在机器人坐标系中的值是已知的。在上述条件下的机器人控制中，在控制中不注意把持的物体的位置姿势，机器人通过进行预定动作来使物体的位置姿势达到目标。
另外，作为现有技术，还存在通过利用照相机的视觉观测行为来识别物体的位置姿势，并以此为基础来控制机器人的方法。在采用这样的方法的情况下，也能够通过识别组装目标的物体的位置姿势来识别目标位置姿势在机器人坐标系中的值。
另外，还有与进行组装作业的机器人的动作生成相关的技术。例如，正在研究利用机器学习使机器人学习执行的作业的方法(参照Dmitry Kalashnikov,et al."QT-Opt:Scalable Deep Reinforcement Learning for Vision-Based Robotic Manipulation"arXiv preprint arXiv:1806.10293,2018.)。在该技术中，提出了基于从照相机得到的图像数据，通过强化学习使机器人学习把持对象物的动作的方法。根据该方法，能够使向机器人示教把持对象物的动作的一系列处理的至少一部分自动化。
发明内容
发明所要解决的技术问题
然而，在现实中，末端执行器与把持的物体的相对位置姿势是不确定的，并且在操作中可能变化。而且，物体的目标位置姿势在机器人坐标系中的值是不确定的。特别是，在未通过夹具高精度地确定待把持的物体或组装目标的物体的位置姿势的情况下，这些不确定性变大。因此，在现有技术中，存在以下问题：即使机器人本身按照预定动作进行动作，所把持的物体的位置姿势也不会达到目标位置姿势，因此有时把持物体并将其组装到其他物体上的操作失败。
另外，在利用照相机的识别方法中，由于成像器的量化，入射在一个像素上的光子数的波动、照明变动等因素，在传感器值中产生误差，因此识别出的位置姿势也包含误差。因此，即使以识别出的位置姿势为基础来控制机器人，也难以控制机器人以使物体的位置姿势达到目标状态。
本公开是鉴于上述情况而完成的，其目的在于，提供用于控制机器人以使操作物体高精度地操作的机器人控制装置、机器人控制方法以及机器人控制程序。
用于解决技术问题的技术方案
本公开的第一方式所涉及的机器人控制装置，所述机器人控制装置控制机器人对操作物体进行操作，使得所述操作物体从作为操作对象的操作物体远离位于环境中的关注物体的状态转变为所述操作物体以特定形式与所述关注物体接触的完成状态，所述机器人控制装置具备：目标状态设定部，将作为所述操作物体达到所述完成状态为止的移动过程中的目标状态的中间目标状态或所述完成状态设定为此次移动的目标状态；观测部，取得关于所述操作物体的位置姿势以及所述操作物体与所述关注物体之间是否存在接触的传感器的观测结果；粒子集合设定部，设定表示所述操作物体的位置姿势的不确定性的粒子的集合，所述粒子的集合中含有的各个所述粒子表示所述操作物体可能存在的位置姿势之一；粒子集合调整部，进行以下调整：越是所述粒子中表示接近所述观测结果所示的所述操作物体的位置姿势的位置姿势的粒子，使粒子的权重越大，并且，在所述观测结果表示发生了所述接触的情况下，在表示所述关注物体和所述操作物体的形状以及相对位置关系的假想空间中，越是接近所述关注物体和以各个所述粒子所表示的位置姿势配置的所述操作物体接触的状态，使对应的所述粒子的权重越大；状态推定部，计算基于调整了各所述粒子的权重的所述粒子的集合而推定出的所述操作物体的位置姿势的推定状态；行动计划部，计划用于将所述操作物体从所述推定状态移动到所述此次移动的目标状态的行动；行动部，指示所述机器人执行所计划的所述行动；以及处理控制部，反复进行所述目标状态的设定、所述观测结果的取得、所述粒子的集合的设定、所述粒子的集合的调整、所述推定状态的计算、所述行动的计划以及所述行动的执行，直到所述推定状态在规定的误差内与所述完成状态一致。
本公开的第二方式所涉及的机器人控制方法，所述机器人控制方法控制机器人对操作物体进行操作，使得所述操作物体从作为操作对象的操作物体远离位于环境中的关注物体的状态转变为所述操作物体以特定形式与所述关注物体接触的完成状态，所述机器人控制方法用于使计算机执行以下处理：将作为所述操作物体达到所述完成状态为止的移动过程中的目标状态的中间目标状态或所述完成状态设定为此次移动的目标状态；取得关于所述操作物体的位置姿势以及所述操作物体与所述关注物体之间是否存在接触的传感器的观测结果；设定表示所述操作物体的位置姿势的不确定性的粒子的集合，所述粒子的集合中含有的各个所述粒子表示所述操作物体可能存在的位置姿势之一；越是所述粒子中表示接近所述观测结果所示的所述操作物体的位置姿势的位置姿势的粒子，使粒子的权重越大，并且，在所述观测结果表示发生了所述接触的情况下，在表示所述关注物体和所述操作物体的形状以及相对位置关系的假想空间中，越是接近所述关注物体和以各个所述粒子所表示的位置姿势配置的所述操作物体接触的状态，使对应的所述粒子的权重越大；计算基于调整了各所述粒子的权重的所述粒子的集合而推定出的所述操作物体的位置姿势的推定状态；计划用于将所述操作物体从所述推定状态移动到所述此次移动的目标状态的行动；指示所述机器人执行所计划的所述行动；反复进行所述目标状态的设定、所述观测结果的取得、所述粒子的集合的设定、所述粒子的集合的调整、所述推定状态的计算、所述行动的计划以及所述行动的执行，直到所述推定状态在规定的误差内与所述完成状态一致。
本公开的第三方式所涉及的机器人控制程序，所述机器人控制程序用于控制机器人对操作物体进行操作，使得所述操作物体从作为操作对象的操作物体远离位于环境中的关注物体的状态转变为所述操作物体以特定形式与所述关注物体接触的完成状态，所述机器人控制程序用于使计算机执行以下处理：将作为所述操作物体达到所述完成状态为止的移动过程中的目标状态的中间目标状态或所述完成状态设定为此次移动的目标状态；取得关于所述操作物体的位置姿势以及所述操作物体与所述关注物体之间是否存在接触的传感器的观测结果；设定表示所述操作物体的位置姿势的不确定性的粒子的集合，所述粒子的集合中含有的各个所述粒子表示所述操作物体可能存在的位置姿势之一；越是所述粒子中表示接近所述观测结果所示的所述操作物体的位置姿势的位置姿势的粒子，使粒子的权重越大，并且，在所述观测结果表示发生了所述接触的情况下，在表示所述关注物体和所述操作物体的形状以及相对位置关系的假想空间中，越是接近所述关注物体和以各个所述粒子所表示的位置姿势配置的所述操作物体接触的状态，使对应的所述粒子的权重越大；计算基于调整了各所述粒子的权重的所述粒子的集合而推定出的所述操作物体的位置姿势的推定状态；计划用于将所述操作物体从所述推定状态移动到所述此次移动的目标状态的行动；指示所述机器人执行所计划的所述行动；反复进行所述目标状态的设定、所述观测结果的取得、所述粒子的集合的设定、所述粒子的集合的调整、所述推定状态的计算、所述行动的计划以及所述行动的执行，直到所述推定状态在规定的误差内与所述完成状态一致。
发明的效果
通过本发明的机器人控制装置、机器人控制方法以及机器人控制程序，能够控制机器人以高精度操作操作物体。
附图说明
图1是表示用于控制本实施方式所涉及的机器人的控制系统的结构的图。
图2是表示作为控制对象的一例的机器人的概略结构的图。
图3是表示动作计划装置以及控制装置的硬件结构的框图。
图4是表示动作计划装置以及控制装置的功能结构的例子的框图。
图5是表示使用了部件A～部件D的组装步骤的一例的图。
图6是表示把持数据的相对位置的一例的图。
图7是表示控制装置的执行部的功能结构的例子的框图。
图8是用于说明生成引导的方法的图。
图9是表示本实施方式的控制系统的处理流程的序列图。
图10是表示控制装置的控制处理的流程的流程图。
具体实施方式
以下，参照附图对本公开的实施方式的一例进行说明。需要说明的是，在各附图中，对相同或等效的构成要素以及部分赋予相同的参照附图标记。另外，为了便于说明，有时附图的尺寸比率被夸大，与实际的比率不同。
图1是表示用于控制本实施方式所涉及的机器人的控制系统的结构的图。如图1所示，控制系统1具有机器人10、状态观测传感器14、接触观测传感器16、动作计划装置20、状态转变控制部25和控制装置30。需要说明的是，状态转变控制部25可以是动作计划装置20的一部分，也可以是控制装置30的一部分，也可以如本实施方式那样构成为独立于动作计划装置20以及控制装置30中的任一个的装置。
(机器人)
图2以及图3是表示机器人10的概略结构的图。本实施方式中的机器人10是6轴垂直多关节机器人，在臂11的前端11a经由柔软部13设置有末端执行器12。机器人10通过末端执行器12把持部件来进行组装体的组装作业。在图3的例子中，末端执行器12构成为具有一组夹持部12a的手部，但也可以将末端执行器12作为吸附垫来吸附部件。在以下的说明中，将包括吸附部件的情况在内的情况称为把持部件。也可以换言之，将包括把持和吸附部件的情况称为保持部件。
如图2所示，机器人10具有具备关节J1～J6的6自由度的臂11。各关节J1～J6通过未图示的电机将连杆彼此以能够沿箭头C1～C6的方向旋转的方式连接。在臂11的前端连接有作为末端执行器12的夹爪。在此，以垂直多关节机器人为例，但也可以是水平多关节机器人(平面关节型机器人)。另外，以6轴机器人为例，但也可以是5轴或7轴等其他自由度的多关节型机器人，也可以是并联机器人。
(状态观测传感器)
状态观测传感器14观测机器人10的状态，将观测到的数据作为状态观测数据输出。作为状态观测传感器14，例如使用机器人10的关节的编码器、视觉传感器(照相机)、动作捕捉、力关联传感器等。作为机器人10的状态，可以根据各关节的角度确定臂11的前端11a的位置以及姿势，可以根据视觉传感器和/或力关联传感器推定部件(作业对象物)的姿势。在动作捕捉用的标记安装在末端执行器12上的情况下，作为机器人10的状态可以确定末端执行器12的位置以及姿势，可以根据末端执行器12的位置以及姿势来推定部件(作业对象物)的姿势。
所谓力关联传感器是力觉传感器以及转矩传感器的总称，进而是在将传感器设置在与部件接触的部位的情况下也包括触觉传感器的总称。力关联传感器也可以设置在末端执行器12把持部件的部分的表面或末端执行器12内的关节部分，以检测机器人10的末端执行器从部件受到的力。力关联传感器例如是将1个要素或多个要素的1轴、3轴或6轴的力作为机器人10的状态进行检测的传感器。通过使用力关联传感器，可以更高精度地掌握末端执行器12如何把持部件，即部件的姿势，能够进行适当的控制。
另外，通过视觉传感器也可以将末端执行器12本身或末端执行器12所把持的部件的位置以及姿势作为机器人10的状态来检测。
这样，通过作为状态观测传感器14的各种传感器，能够检测末端执行器12以及被把持的部件的状态。另外，能够取得各种传感器的检测结果作为状态观测数据。
(接触观测传感器)
接触观测传感器16是压力传感器、力觉传感器或触觉传感器。触觉传感器是检测压力分布的传感器，或者是可以检测正交三轴方向的力以及围绕正交三轴的力矩的传感器。压力传感器或触觉传感器例如设置在末端执行器12的与手指的把持对象物接触的部分。力觉传感器例如设置在机器人10的臂11与末端执行器12之间的手腕部分。
(动作计划装置/控制装置)
接着，对动作计划装置20以及控制装置30的结构进行说明。
图3是表示本实施方式所涉及的动作计划装置20以及控制装置30的硬件结构的框图。动作计划装置20以及控制装置30可以以相同的硬件结构实现。动作计划装置20具有CPU(Central Processing Unit：中央处理器)20A、ROM(Read Only Memory：只读存储器)20B、RAM(Random Access Memory：随机访问存储器)20C、存储器20D、输入部20E、显示部20F以及通信接口(I/F)20G。各结构经由总线20H可相互通信地连接。控制装置30具有CPU30A、ROM30B、RAM30C、存储器30D、输入部30E、显示部30F以及通信I/F30G。各结构经由总线30H可相互通信地连接。以下，对动作计划装置20的情况进行说明。
在本实施方式中，在ROM20B或存储器20D中存储有程序。CPU20A是中央运算处理单元，执行各种程序，或控制各结构。即，CPU20A从ROM20B或存储器20D读出程序，将RAM20C作为作业区域执行程序。CPU20A按照记录在ROM20B或存储器20D中的程序，进行上述各结构的控制以及各种运算处理。
ROM20B存储各种程序以及各种数据。RAM20C作为作业区域暂时存储程序或数据。存储器20D由HDD(Hard Disk Drive：硬盘驱动器)、SSD(Solid State Drive：固态驱动器)或闪存构成，存储包含操作系统的各种程序以及各种数据。
输入部20E包括键盘以及鼠标等指示设备，用于进行各种输入。显示部20F例如是液晶显示器，显示各种信息。显示部20F也可以采用触摸面板方式，作为输入部20E发挥功能。
通信接口(I/F)20G是用于与其他设备进行通信的接口，例如使用以太网(注册商标)、FDDI或Wi-Fi(注册商标)等标准。
图4是表示动作计划装置20以及控制装置30的功能结构的例子的框图。需要说明的是，也可以将动作计划装置20以及控制装置30构成为一体。
如图4所示，动作计划装置20作为功能结构具有转变生成部110和把持数据确定部112。动作计划装置20的各功能结构通过CPU20A读出存储在ROM20B或存储器20D中的程序，在RAM20C中展开并执行来实现。控制装置30作为功能结构具有取得部130和执行部132。控制装置30的各功能结构通过CPU30A读出存储在ROM30B或存储器30D中的程序，在RAM30C中展开并执行而实现。
在本实施方式中，用状态转变图表示基于机器人10的作业的组装工序。将部件或组装体分解为要素单位，将表示要素的转变的数据作为单位状态转变数据表示为整体的状态转变图。
在动作计划装置20中，作为动作计划生成各个单位状态转变数据，通过来自用户的输入来接收为了生成单位状态转变数据所需的登记信息。登记信息是部件的信息(部件的ID、种类等)、末端执行器12的不同种类的CAD信息、组装步骤(包括分解步骤)，以及部件的把持位置(包括姿势)等各种信息。需要说明的是，在组装步骤中包括在模拟上组装(或分解)时记录的部件的相对轨道。
转变生成部110生成包含各个单位状态转变数据的状态转变数据。各个单位状态转变数据根据登记信息中的组装步骤生成。
例如，转变生成部110生成单位状态转变数据，该单位状态转变数据表示从第一要素和第二要素独立存在的状态向组装由第一要素以及第二要素构成的组装体的第三要素的状态的转变。第一要素是部件或组装体。第二要素也是部件或组装体。
在此，在对把持数据确定部112进行说明之前，说明与状态转变数据对应的组装步骤的具体例。图5是表示使用了部件A～部件D的组装步骤的一例的图。当应用于状态转变时，(1)～(3)中的每一个与单位状态转变数据中包含的初始状态对应。当将组装(1)的部件A以及部件B的作业工序应用于该上述的单位状态转变数据时，部件A以及部件B分别与第一要素以及第二要素对应，组装了部件A以及部件B的(2)的组装体与第三要素对应。第三要素是以(1)为初始状态的单位状态转变数据中包含的目标状态。(2)以后也同样能够应用于单位状态转变数据。以下，以该部件A～部件D的组装步骤为例，对把持数据确定部112进行说明。
把持数据确定部112基于登记信息中的部件的把持位置，针对各个单位状态转变数据确定把持数据。把持数据确定部112确定为了组装而由机器人10的末端执行器12把持作为把持对象物的第一要素或第二要素时的把持数据。把持数据是末端执行器12把持把持对象物时的末端执行器12以及把持对象物的相对位置姿势的计划值。以下，对把持数据中的相对位置姿势进行说明。在此，相对位置姿势的计划值是末端执行器12把持要素时的把持位置以及把持姿势的计划值。另外，相对于作为把持对象物的要素，另一个要素成为组装目标对象物。另外，位置姿势用6自由度表示。
图6是表示把持数据的相对位置姿势的一例的图。在图6(A)中，在将末端执行器12作为吸附垫的情况下，末端执行器12相对于部件A的相对位置姿势R1用箭头表示。图6(A)示出了在将末端执行器12作为吸附垫的情况下末端执行器12相对于部件A的相对位置姿势R1。图6B示出了在末端执行器12是具备一对夹持部的夹爪(手部)的情况下末端执行器12相对于部件B的相对位置姿势R2。对于部件C以及部件D也同样地示出。
相对位置姿势通过下式求出。
(Tobj→tcp)-1＝Ttcp→obj
左边表示部件坐标系，右边表示来自末端执行器12的相对位置姿势(位置以及姿势)。把持数据例如作为把持时使用的末端执行器12的带识别ID的CAD数据和作为把持对象物的目标的相对位置姿势的计划值来计算。相对位置姿势的计划值可以是由作为管理员的用户输入的包括登记信息的部件的把持位置(部件表面上的应把持的位置)在内的信息，也可以通过把持计划等现有方法自动计算。
动作计划装置20向状态转变控制部25输出所生成的单位状态转变数据以及作为控制数据的把持数据。由单位状态转变数据表示的一个状态转变也称为任务。需要说明的是，也可以不是每次任务更新时都从动作计划装置20输出，而是将状态转变数据中包含的单位状态转变数据以及控制数据全部输出到状态转变控制部25，在状态转变控制部25侧管理输出哪个单位状态转变数据。另外，把持数据也可以不经由状态转变控制部25而直接输出到控制装置30，在控制装置30侧进行管理。
状态转变控制部25将状态转变数据中的与处理对象的任务对应的单位状态转变数据输出到控制装置30的执行部132。处理对象在组装作业开始时指定，在每次从控制装置30接收到任务完成的通知时更新。状态转变控制部25可以包含在动作计划装置20中，也可以包含在控制装置30中，也可以是与任意一个都不同的装置。另外，包括动作计划装置20、控制装置30以及状态转变控制部25的控制系统整体也可以是一个装置。
以下，对控制装置30的各处理部进行说明。
取得部130从状态转变控制部25取得处理对象的单位状态转变数据以及把持数据。另外，取得部130从状态观测传感器14取得观测了第一要素的位置以及第二要素的位置的状态观测数据。需要说明的是，第一要素的位置以及第二要素的位置包括姿势。
另外，取得部130取得从接触观测传感器16得到的传感器值即接触观测数据。
执行部132使用从状态转变控制部25输出的单位状态转变数据来执行任务。任务例如是完成在单位状态转变数据中组装了第三要素的状态的任务。执行部132通过执行任务，基于观测数据以及把持数据，使末端执行器12把持第一要素以及第二要素中的一方的把持对象物，使把持对象物沿着最终部分包含后述的引导的目标相对轨道相对于另一方的要素相对移动。
以下对执行部132的任务的具体执行方法进行说明。
在此，考虑通过机器人10的动作来操作两个物体的相对位置姿势的问题。
在此，将位于环境中的物体称为关注物体，将机器人10通过末端执行器12操作的物体称为操作物体。作为前提，给出了两个物体的CAD模型，两个物体是刚体，CAD模型与现实的物体形状没有误差。另外，以关注物体的位置姿势不会因某些外力而变化为前提。作为关注物体和操作物体的完成状态的相对位置姿势，作为任务的单位状态转变数据被给出。
在将把持的部件组装到另一部件的任务的情况下，关注物体是组装目标的部件，操作物体是把持的部件。完成状态是把持的部件被组装到组装目标的相对位置姿势。
在任务是将所把持的部件移动到环境中的另一位置的情况下，关注物体是环境中的地标，操作物体是把持的部件。完成状态是所需的移动目的地位置相对于地标的相对位置姿势。
关注物体和操作物体的相对位置姿势通过使用了状态观测传感器14的视觉观测来识别。例如，使用从作为状态观测传感器14的照相机得到的图像和各个CAD模型，通过CAD匹配来识别关注物体和操作物体各自在照相机坐标系中的位置姿势。使用两个位置姿势，将从关注物体的CAD原点观察到的操作物体的CAD原点的位置姿势作为相对位置姿势。需要说明的是，如上所述，由于各个物体在观测到的照相机坐标系中的位置姿势产生误差，因此在求出的相对位置姿势中也包含误差。将相对位置姿势的真值与观测值的背离称为系统的观测噪声。需要说明的是，由于没有得知相对位置姿势的真值的机构，因此观测噪声也是概念上的，不能求出该值。关注物体和操作物体的相对位置姿势的观测值除了通过处理由照相机拍摄的图像来取得之外，在能够与照相机的观测值之间进行校正的情况下，也可以在机器人10把持操作物体之后，通过基于由编码器测量的机器人10的各关节的角度进行运算来取得。
另外，通过作为接触观测传感器16的压力传感器或者力觉传感器、触觉传感器识别关注物体和操作物体的接触。在本实施方式中，以能够正确地观测接触观测传感器16的接触观测值是否没有错误地接触为前提。
当末端执行器12把持操作物体并且控制机器人10以通过将操作物体移动所指示的移动量来改变相对位置姿势时，操作物体的实际移动量的相对位置姿势的变化量与所指示的移动量不同。例如，由于机器人10的安装位置的误差或正向运动学的计算误差，TCP(工具中心点)的移动量本身可能产生误差。由于在现实的机器人10的使用场景下进行相应的事前校正，所以通常这些误差的大小较小，但即使如此，插入操作这样的需要高精度的操作即使存在微小的误差也会失败。或者，如果操作物体相对于末端执行器12的位置姿势在把持操作物体之后立即被观测到，并且此后未被再次观测到，则在观测操作物体之后将操作物体移动到引导起点之前的时段期间，由于把持操作物体的力不充分，操作物体相对于末端执行器12的位置姿势可能改变。此外，在使操作物体沿着后述的引导移动的期间，当操作物体与关注物体接触等对操作物体施加外力时，操作物体相对于末端执行器12的位置姿势可能发生变化。在任何情况下，如果存在这些误差，则不能将操作物体移动所指示的移动量。将实际的移动量与所指示的移动量的背离称为系统的系统噪声。
在此，将从关注物体看到的操作物体的相对位置姿势设为状态s，作为考虑离散时刻t＝0、1、……M的状态st的对象。将状态s的坐标系设为以关注物体为基准的相对坐标系的物体坐标系。另外，将时刻t的机器人坐标系中的TCP的位置姿势设为xt，将xt和st的位置姿势的齐次转换矩阵设为Tt。
将位置姿势的转换记为数学式(1)。st、Tt、xt是齐次坐标系。
st＝Ttxt (1)
从数学式(1)可知，齐次转换矩阵Tt不仅包含TCP的位置姿势从机器人坐标系向物体坐标系的转换，还包含从TCP的相对位置姿势向操作物体的相对位置姿势的转换。即，相对于xt是TCP的位置姿势，作为利用Tt转换xt的结果的状态st不是TCP而是操作物体的相对位置姿势。
需要说明的是，状态st和TCP的位置姿势xt是4行4列的矩阵，其左上3行3列的成分表示旋转矩阵，第4列的第1～3行作为位置矢量处理。第4列第4行是用于处理无限远的成分。由于旋转矩阵在性质上只具有三维的信息，因此这些矩阵实质上能够作为6维来处理。齐次转换矩阵Tt也是4行4列的矩阵。齐次坐标系表述的两个矩阵的积相当于矢量表述的位置姿势的加法运算。
现在考虑机器人10通过末端执行器12把持操作物体的情况。
考虑通过在某一时刻t将控制量ut施加到TCP的位置姿势xt而在时刻t+1将TCP的位置姿势转变为xt+1的系统。此时，由于操作物体被末端执行器12把持，因此随着动作从状态st转变到状态st+1。将根据控制量ut而变化的物体坐标系中的相对位置姿势的移动量设为at。另外，考虑在移动中操作物体的把持姿势有可能改变噪声vt，vt影响at的移动量是操作物体的实际移动量。
设vt为系统的系统噪声，系统噪声vt按照某概率分布Pv。需要说明的是，vt也是齐次坐标系。
另外，通过在动作中把持姿势变化vt，st和xt的齐次转换矩阵Tt变化vt。改变后的齐次转换矩阵为
系统的状态方程以矩阵运算的记述记为数学式(2)。vt、ut以及at是齐次坐标系。
根据数学式(1)和(2)，作为vtat和ut的关系得到数学式(4)。
接着，考虑状态观测传感器14以及接触观测传感器16的观测行为。假设通过观测行为得到观测值yt。观测值yt由状态的观测值st’和接触观测值ct’构成。
yt＝(st’,ct’)
st’是物体坐标系的相对位置姿势的观测值。另外，由于观测噪声ω，st’具有与真值st的误差。ct’是关注物体和操作物体的接触观测值。在ct’的观测中不产生误差。在关注物体和操作物体不接触的情况下，ct’＝0，在接触的情况下，ct’＝1。
设ωt为系统的观测噪声，观测噪声ωt遵循某个概率分布Pω。需要说明的是，ωt也是齐次坐标系。
将观测方程以齐次坐标系的形式记于数学式(5)。
s′t＝ωtst (5)
由此，将状态空间模型确定如下。
数学式(8)将状态空间模型记述为概率模型。
当时间转变到某一时刻t时，给出观测值的集合
y0：t＝{ym|m＝0，1，…，t}
给出y0:t时得到st的概率由以下的数学式(9)表示。
其中，
η＝1/p(yt|y0：t-1)
根据数学式(8)的状态空间模型，以及得到观测数学式(9)时的状态的概率，进行状态的控制。即，通过机器人10操作两个物体之间的相对位置姿势。
根据以上说明的任务的执行方法，执行部132控制机器人10操作操作物体，使得该任务从作为操作对象的操作物体远离位于环境中的关注物体的初始状态转变为操作物体以特定形式接触关注物体的完成状态。执行部132针对每个任务控制机器人10。
在此，任务是将把持的部件组装到其他部件的任务，将把持的部件转变到环境中的其他位置的任务，或者把持后述的部件的任务。
具体而言，如图7所示，执行部132具备引导设定部140、初始设定部142、行动计划部144、指令转换部146、观测部148、时间更新部150、观测更新部152、状态推定部154、反复判定部156以及目标设定部158。需要说明的是，目标设定部158是目标状态设定部的一例，时间更新部150是粒子集合设定部的一例，观测更新部152是粒子集合调整部的一例，指令转换部146是行动部的一例，反复判定部156是处理控制部的一例。
引导设定部140设定引导。引导是操作对象物体相对于关注物体的相对轨迹的最终部分，并且是与操作对象物体相对于关注物体的初始相对位置无关的共同的目标相对轨迹。引导包括完成状态以及达到完成状态为止的一系列的中间目标状态。在此，完成状态以及中间目标状态由物体坐标系表示。
例如，引导设定部140针对单位状态转变数据，基于登记信息中的组装步骤，生成引导。在此，引导是达到完成状态的离散的相对位置姿势的系列。完成状态是引导的终端。
引导设定部140也可以设置在动作计划装置20中。在该情况下，引导也与单位状态转变数据同样地由控制装置30取得。
以下，说明引导的生成方法的一例。
图8是从将部件D组装于部件A～C的组装体的状态开始，使部件D向不与部件A～C的组装体干涉而部件D能够移动的方向移动的情况下的部件D的位置姿势的轨迹的一例。将从该轨迹的终点开始使移动方向反转的轨迹作为引导G1生成。构成引导的状态的间隔不需要均匀，例如也可以在预想接触的区域中紧密地设定状态。
初始设定部142将相对于完成状态最远的、引导中的中间目标状态设定为此次移动的目标状态。
另外，初始设定部142对表示操作物体的位置姿势的不确定的粒子的集合，即，粒子的集合中包含的每个粒子表示操作物体的可能的位置姿势之一的粒子的集合进行初始设定。在此，粒子所表示的操作物体的位置姿势由物体坐标系表示。
行动计划部144计划用于将操作物体从初始状态或后述的推定状态移动到此次移动的目标状态的行动。在此，通过物体坐标系来表示推定状态以及行动。
指令转换部146将计划的行动转换为机器人10可执行的指令并输出，对机器人10进行指示。
观测部148从由取得部130取得的状态观测数据中取得关于关注物体和操作物体的相对位置姿势的观测结果。在此，观测部148取得关注物体和操作物体的相对位置姿势作为由物体坐标系表示的观测结果。
另外，观测部148从由取得部130取得的接触观测数据中取得关于操作物体和关注物体是否存在接触的观测结果。具体而言，从接触观测数据转换为与接触或非接触对应的二值，识别为接触观测值。将传感器值转换为二值的方法没有限定，可以使用现有的方法。
时间更新部150更新为下一步骤中的粒子的集合。
观测更新部152更新粒子集合中包含的各粒子的权重。此时，越是表示接近粒子中观测结果所示的操作物体的位置姿势的位置姿势的粒子，将粒子的权重更新得越大，并且，在观测结果表示发生了接触的情况下，在表示关注物体和操作物体的形状以及相对位置关系的假想空间中，越是接近关注物体和以各个粒子所表示的位置姿势配置的操作物体接触的状态，将对应的粒子的权重更新得越大。在此，假想空间是由物体坐标系记述的空间。
状态推定部154计算推定状态，该推定状态是基于调整了各粒子的权重的粒子的集合而推定出的操作物体的位置姿势。
反复判定部156判定计算出的推定状态是否在规定的误差内与完成状态一致。另外，反复判定部156反复进行目标设定部158、行动计划部144、指示转换部146、观测部148、时间更新部150、观测更新部152和状态推定部154的处理，直到推定状态在规定的误差内与完成状态一致。
目标设定部158相对于完成状态从远向近依次将引导中的中间目标状态设定为此次移动的目标状态，将完成状态设定为最后的此次移动的目标状态。
接着，对控制系统1的作用进行说明。
图9是表示本实施方式的控制系统1的处理流程的序列图。CPU20A通过作为动作计划装置20的各个部分发挥功能来进行动作计划处理，CPU30A通过作为控制装置30的各个部分发挥功能来进行控制处理。
在步骤S100中，动作计划装置20生成包含各个单位状态转变数据的状态转变数据以及控制数据。控制数据是与所生成的各个单位状态转变数据对应的各个把持数据。
在步骤S102中，动作计划装置20向状态转变控制部25输出状态转变数据以及控制数据。
在步骤S104中，状态转变控制部25接收一系列的任务开始指示，开始处理对象的任务。在接收到任务开始指示的时间点，作为处理对象开始最初的任务，根据任务的进行，依次更新任务。需要说明的是，任务开始指示也可以由控制装置30接收。
在步骤S108中，状态转变控制部25向控制装置30输出与处理对象的任务对应的单位状态转变数据以及与该单位状态转变数据对应的控制数据。需要说明的是，状态转变控制部25首先输出与反复处理的顺序的最初的任务对应的单位状态转变数据，根据任务的进行，输出与下一个任务对应的单位状态转变数据。
在步骤S114中，控制装置30的取得部130取得状态观测传感器14以及接触观测传感器16观测到的状态观测数据以及接触观测数据，并且执行部132执行处理对象的任务。由此，对于该任务，控制机器人10，使得作为操作对象的操作物体从远离位于环境中的关注物体的初始状态转变为操作物体以特定形式与关注物体接触的完成状态。
在步骤S118中，控制装置30的执行部132向状态转变控制部25通知任务完成。
在步骤S120中，状态转变控制部25判定处理是否在状态转变数据的最终状态结束。在判定为处理在最终状态结束的情况下，过渡到步骤S124。在判定为处理没有在最终状态结束的情况下，过渡到步骤S122。
在步骤S122中，状态转变控制部25更新处理对象的任务，返回步骤S108。任务的更新是指将处理对象的任务更新为下一个任务的处理。在任务更新后的步骤S108中，将与处理对象的任务对应的单位状态转变数据以及控制数据(把持数据)输出到控制装置30，重复以后的处理。
在步骤S124中，状态转变控制部25结束任务，结束控制系统1的处理。
图10是表示步骤S114的控制装置30的控制处理的流程的流程图。
在步骤S130中，引导设定部140针对单位状态转变数据，基于登记信息中的组装步骤，生成并设定引导。具体而言，确定相对于关注物体和操作物体的相对位置姿势的开始状态s0*、结束状态sM*，决定M+1个状态的集合
作为引导。
在此，状态集合s0:M*是引导，利用关注物体和操作物体的CAD模型来生成引导。例如，如参考图8所说明的那样，在CAD模型中，从操作物体被组装到处于结束状态的关注物体的状态开始，生成在不与关注物体发生干涉的情况下在操作物体可移动的方向上使操作物体经过M个阶段逐渐移动的情况下的操作物体的位置姿势的轨迹。然后，生成从该轨迹的终点开始使移动方向反转的轨迹作为引导。构成引导的状态的间隔不需要均匀，例如也可以在预想接触的区域中紧密地设定状态。
在步骤S132中，初始设定部142将目标状态的初始值设为引导的起点的状态s0*。
在步骤S134中，观测部148从由状态观测传感器14得到的状态观测数据中取得操作物体的相对位置姿势。
在步骤S136中，初始设定部142确定初始时刻t＝0时的任意的N个状态的集合，即粒子集合
的初始值。粒子的初始值s0 (n)可以通过任何事前知识来确定。例如，初始值s0 (n)可以被确定为均匀分布在最大误差范围内的区域中，该最大误差范围是针对操作物体的相对位置姿势的观测值而设想的。
在此，粒子集合表示操作物体的相对位置姿势的不确定性。各个粒子表示操作物体可能存在的相对位置姿势中的一个。
状态的推定值^st是代表粒子的集合的相对位置姿势。
另外，确定与N个粒子分别对应的N个粒子权重的集合
的初始值。在初始值中，将权重全部设为w(n)＝1/N。
最后，确定生成在粒子的时间更新中使用的粒子散布变量的概率分布/>
粒子散布变量相当于假想地设想的系统噪声。
在步骤S138中，当在构成引导的状态的集合的要素sm*中，将当前要素设为st*，将下一步骤的要素设为st+1*时，行动计划部144根据st+1*和当前状态的推定值^st来决定物体坐标系中的移动量at。
需要说明的是，将数学式中的X的上标“^(hat)”表示为^X。
状态st的推定值^st在后述的步骤S148中求出。^st的初始值是在上述步骤S134中取得的相对位置姿势。需要说明的是，也可以每次进行使用了CAD的路径计划，决定移动量。在不使用引导的情况下，以当前的状态为起点，如数学式(1)中所记载的那样，状态st和位置姿势xt具有st＝Ttxt的关系。根据该关系，如数学式(12)所示，得到转换推定的状态^st和机器人坐标系中的TCP的位置姿势的值xt的齐次转换矩阵^Tt。^Tt与Tt的推定值对应。
以决定的移动量at移动操作物体。当通过机器人10移动操作物体时，通过使用所推定的^Tt将移动量at转换为针对TCP的控制量^ut，通过^ut控制机器人10。
另外，实际上，由于产生了系统噪声vt，因此操作物体移动vtat的量。将实际的相对位置姿势的移动vtat转换为机器人坐标系的TCP的移动量后的量是实际的控制量ut，但由于vt未知而不能求出ut，所以使用^ut进行控制。
在步骤S140中，指令转换部146通过反向运动学将机器人坐标系中的TCP的移动量^ut转换为机器人10的各关节的旋转的指令量，对机器人10指示行动。
在步骤S142中，观测部148根据从状态观测传感器14以及接触观测传感器16得到的状态观测数据以及接触观测数据，得到将前述的物体坐标系的相对位置姿势、关注物体和操作物体的接触状态汇总为一个的观测值yt＝(st’,ct’)。
另外，使用由编码器得到的机器人10的各关节的角度的测量值，通过ForwardKinematics(正向运动学)得到xt作为机器人坐标系中的TCP的位置姿势的值。
在步骤S144中，时间更新部150在上述步骤S140中的行动后，根据at和粒子散布变量将粒子集合
Ξt
更新为
另外，将粒子权重标准化为1/N。
w(n)←1/N (15)
在步骤S146中，观测更新部152更新粒子集合中包含的各粒子的权重。
具体而言，首先如下进行干涉判定。在用于进行干涉判定的模拟空间(假想空间)内预先配置使用CAD的形状数据描绘的关注物体。此外，对于粒子集合中包含的各个粒子，使用CAD的形状数据将该粒子所表示的位置姿势的操作物体配置在模拟空间中。如果存在基于某个粒子配置的操作物体与关注物体重叠的部分，则判定为存在干涉，如果操作物体与关注物体分离并且不存在重叠部分，则判定为不存在干涉。这样，对每个粒子判定是否存在干涉。另外，还计算干涉距离，该干涉距离是在模拟上关注物体的表面与操作物体的表面之间的最近距离。
这样，使用粒子集合
在模拟上进行使用了CAD的干涉判定，将所得到的干涉判定的集合设为
将干涉距离的集合设为
干涉判定ct (n)是在模拟上第n个粒子的关注物体与操作物体是否存在干涉，干涉时ct (n)＝0，非干涉时ct (n)＝1。
干涉距离dt (n)是模拟上的第n个粒子的关注物体表面与操作物体表面的最短距离，干涉时dt (n)＜0，非干涉时dt (n)＞0。
与概率p(yt|st (n))对应的似然度L(yt|st (n))由数学式(16)决定。l(st (n)|st’)是粒子的状态和观测状态的距离。δ(·)是δ函数，τ和σ是超参数。
即，在由接触观测传感器16检测出的关注物体与操作物体不存在现实的接触(ct’＝0)的情况下，使用数学式(16)的上侧的式子，在存在现实的接触(ct’＝1)的情况下，使用下侧的式子。不存在现实的接触的情况下的式子中包含的δ函数，对于模拟中不存在干涉的粒子返回值1，对于存在干涉的粒子返回值0。
数学式(16)中包含的指数函数的部分是高斯函数。函数系可以不使用高斯函数，而使用距离近则似然高，距离远则似然低的函数。例如，可以使用指数函数的衰减等。
使用从似然度函数得到的似然度值，用数学式(17)更新粒子权重W。
即，在关注物体与操作物体之间不存在现实的接触的情况下，似然度函数将在模拟上存在干涉的粒子(与不接触的观测结果相反的粒子)的权重设为0，对于在模拟上不存在干涉的粒子，该粒子的状态st (n)越接近观测到操作物体的状态st'，权重越大。
另一方面，在关注物体与操作物体存在现实的接触的情况下，似然度函数具有以下作用：粒子的状态st (n)越接近操作物体被观测到的状态st’，越增大该粒子的权重；关注物体表面与操作物体表面之间的最短距离越小，越增大粒子的权重。其结果是，在存在现实的接触的情况下，在模拟上表示与关注物体和操作物体的表面彼此接触的状态接近的状态的粒子的权重被更新得较大，表示操作物体大幅远离关注物体或者操作物体深深侵入关注物体的状态的粒子的权重被更新得较小。
在步骤S148中，状态推定部154计算推定状态，该推定状态是基于调整了各粒子的权重的粒子的集合而推定出的操作物体的位置姿势。此时，如上所述，操作物体的状态的推定值^st被修正为接近操作物体被观测到的位置姿势，并且，在通过接触观测传感器16观测到现实的接触的情况下，在模拟上也被修正为接近操作物体和关注物体的表面彼此接触的状态。
具体而言，时刻t的状态的推定值^st作为针对粒子的集合的期望值由数学式(18)求出。数学式(18)中的log表示矩阵的对数、exp表示矩阵的指数。
以与更新后的粒子权重
成比例的概率从粒子集合
中随机选择粒子，进行Ξt的重新取样。
在步骤S150中，反复判定部156判定状态的推定值^st是否在规定的误差范围内与引导的结束状态sM*一致。如果状态的推定值^st在规定的误差范围内与引导的结束状态sM*一致，则结束控制处理。如果不一致则进入S152。
在步骤S152中，目标设定部158将目标状态设定为引导中的下一个状态。另外，将时刻前进1个步骤，将之前的t+1设为新的t。然后，返回步骤S138。
需要说明的是，上述图10的流程图所示的处理的顺序只要可以实现目的，也可以适当变更。例如，步骤S142的观测也可以在上述步骤S144之后进行。
另外，对于将操作物体从把持的场所移动到引导起点的部分，也可以如下控制机器人10。
首先，用直线连接从把持操作物体的场所到引导起点的移动路径，或者使用现有的Motion Planning的方法生成。另外，例如，操作物体从把持的场所到引导起点的移动例如如下进行。
首先，根据由状态观测传感器14的照相机得到的状态观测数据，观测关注物体在照相机坐标系中的位置姿势，展开具有以关注物体为基准的坐标系即物体坐标系的假想空间。由此，照相机坐标系和物体坐标系能够相互转换。
接着，将引导配置在假想空间内。在此，引导表示操作物体的轨道而不是末端执行器12。
接着，根据由照相机得到的状态观测数据，观测在照相机坐标系中的把持前的操作物体的位置姿势，将其转换为物体坐标系中的位置姿势，并且将操作物体配置在假想空间内。然后，根据由编码器得到的状态观测数据，取得机器人坐标系中的末端执行器12的位置姿势。
接着，根据由照相机得到的状态观测数据，观测照相机坐标系中的末端执行器12的位置姿势，转换为物体坐标系的位置姿势，将末端执行器12配置在假想空间中。由此，能够转换机器人坐标系和物体坐标系。如果预先校正了照相机坐标系与机器人坐标系的对应关系，则能够省略照相机对末端执行器12的观测。
接着，用末端执行器12把持操作物体。
根据在把持后由照相机得到的状态观测数据，观测照相机坐标系中的操作物体的位置姿势，并且修正操作物体在物体坐标系中的位置姿势。由此，消除把持误差。
然后，在假想空间内计划操作物体的初始移动路径，该初始移动路径用于从操作物体被把持时的位置姿势到引导起点的位置姿势的移动。
接着，将假想空间内的路径计划转换为机器人坐标系空间内的路径计划，按照路径计划使操作物体移动。需要说明的是，在移动中不能消除误差。
然后，从由照相机得到的状态观测数据中取得移动后的操作物体以及末端执行器12的位置姿势，使操作物体移动以与引导起点的位置姿势一致。修正照相机坐标系、物体坐标系、机器人坐标系相互的转换关系。由此，消除了操作物体在引导起点处的位置姿势误差。另外，坐标系相互的转换误差也变小。
如上所述，根据本实施方式的控制系统1，设想关于两个物体的相对位置姿势的粒子集合，根据观测信息和似然度函数求出各粒子的似然度，推定相对位置姿势。根据推定的相对位置姿势与下一时刻的目标相对位置姿势来决定物体的移动量。确定用于将所推定的相对位置姿势转换为机器人坐标系中的TCP的位置姿势的齐次转换矩阵，根据转换矩阵将物体的移动量转换为TCP的移动量，并且控制机器人10以使两个物体的相对位置姿势达到目标相对位置姿势。此时，使用操作物体与关注物体接触的信息，通过使用了物体间的接触检测结果和CAD的干涉判定模拟来推定两个物体的相对位置姿势，根据推定结果来决定操作物体的移动量。由此，能够在对观测或行动施加噪声的不确定的环境下使用机器人10操作物体，使两个物体的相对位置姿势高精度地达到目标状态。另外，即使在末端执行器12和操作物体的相对位置姿势不明确，并且在控制中变化的控制不确定的环境下，或者在状态观测传感器14的相对位置姿势的识别包含误差这样的观测不确定的环境下，也能够使两个物体间的相对位置姿势达到目标状态。
需要说明的是，也可以由CPU以外的各种处理器执行在上述实施方式中CPU读入软件(程序)并执行的动作计划处理或控制处理。作为该情况下的处理器，例示了专用电气电路等，该专用电路是具有为了执行特定的处理而专门设计的电路结构的处理器，如FPGA(Field-Programmable Gate Array：现场可编程门阵列)等在制造后能够变更电路结构的PLD(Programmable Logic Device：可编程逻辑器件)以及ASIC(Application SpecificIntegrated Circuit：专用集成电路)等。另外，也可以由这些各种处理器中的一个执行动作计划处理或控制处理，也可以由同种类或不同种类的两种以上的处理器的组合(例如，多个FPGA，以及CPU和FPGA的组合等)执行。另外，更具体而言，这些各种处理器的硬件结构是组合了半导体元件等电路元件的电气电路。
另外，在上述实施方式中，说明了将程序预先存储(安装)在ROM20B(30B)或存储器20D(30D)中的方式，但并不限定于此。程序也可以以记录在CD-ROM(Compact Disk ReadOnly Memory：光盘只读存储器)、DVD-ROM(Digital Versatile Disk Read Only Memory：数字多功能磁盘只读存储器)以及USB(Universal Serial Bus：通用串行总线)存储器等记录介质中的方式提供。另外，程序也可以采用经由网络从外部装置下载的方式。
另外，在上述实施方式中，以末端执行器12是具有夹持部12a的夹爪或吸附垫的情况为例进行了说明，但并不限定于此。只要是保持物体的结构，也可以是其他结构，例如末端执行器12也可以是真空吸附卡盘、磁力吸附卡盘、捞取对象物的刮铲等。
另外，以从单位状态转变数据的完成状态生成向导的情况为例进行了说明，但并不限定于此。作为单位状态转变数据，不仅可以给出完成状态，而且可以给出达到完成状态的离散相对位置姿势的系列。
另外，作为引导，以自动生成相对位置姿势的系列的情况为例进行了说明，但也可以不生成引导。在该情况下，在上述实施方式中，可以看作引导以单一的状态构成的特殊的事例。
另外，引导也可以是达到不是完成状态的状态的离散的相对位置姿势的系列。
另外，任务也可以是把持作为关注物体的部件的任务。在上述实施方式中，将位于环境中的物体作为关注物体，将末端执行器12把持的物体作为操作物体，但在把持部件的任务中，将为了由末端执行器12操作(把持)而置于环境中的物体作为关注物体，将末端执行器12本身作为操作物体。由于末端执行器12由机器人臂操作，所以末端执行器12也能够看作由机器人操作的物体。在把持部件的任务中，作为操作物体的末端执行器12把持放置在环境中的关注物体并且没有移动关注物体的状态是完成状态。
(附记)
关于以上的实施方式，进一步公开以下的附记。
(附记项1)
一种机器人控制装置，用于控制机器人对操作物体进行操作，使得所述操作物体从作为操作对象的操作物体远离位于环境中的关注物体的状态转变为所述操作物体以特定形式与所述关注物体接触的完成状态，
所述机器人控制装置包括：
存储器；以及
与所述存储器连接的至少一个处理器，
所述处理器执行以下处理：
将作为所述操作物体达到所述完成状态为止的移动过程中的目标状态的中间目标状态或所述完成状态设定为此次移动的目标状态；
取得关于所述操作物体的位置姿势以及所述操作物体与所述关注物体之间是否存在接触的传感器的观测结果；
设定表示所述操作物体的位置姿势的不确定性的粒子的集合，所述粒子的集合中含有的各个所述粒子表示所述操作物体可能存在的位置姿势之一；
越是所述粒子中表示接近所述观测结果所示的所述操作物体的位置姿势的位置姿势的粒子，使粒子的权重越大，并且，在所述观测结果表示发生了所述接触的情况下，在表示所述关注物体和所述操作物体的形状以及相对位置关系的假想空间中，越是接近所述关注物体和以各个所述粒子所表示的位置姿势配置的所述操作物体接触的状态，使对应的所述粒子的权重越大；
计算基于调整了各所述粒子的权重的所述粒子的集合而推定出的所述操作物体的位置姿势的推定状态；
计划用于将所述操作物体从所述推定状态移动到所述此次移动的目标状态的行动；
指示所述机器人执行所计划的所述行动；
反复进行所述目标状态的设定、所述观测结果的取得、所述粒子的集合的设定、所述粒子的集合的调整、所述推定状态的计算、所述行动的计划以及所述行动的执行，直到所述推定状态在规定的误差内与所述完成状态一致。
(附记项2)
一种非临时性存储介质，其存储有能够由计算机执行的程序，所述程序用于执行机器人控制处理，所述机器人控制处理用于控制机器人对操作物体进行操作，使得所述操作物体从作为操作对象的操作物体远离位于环境中的关注物体的状态转变为所述操作物体以特定形式与所述关注物体接触的完成状态，
所述机器人控制处理包括以下处理：
将作为所述操作物体达到所述完成状态为止的移动过程中的目标状态的中间目标状态或所述完成状态设定为此次移动的目标状态；
取得关于所述操作物体的位置姿势以及所述操作物体与所述关注物体之间是否存在接触的传感器的观测结果；
设定表示所述操作物体的位置姿势的不确定性的粒子的集合，所述粒子的集合中含有的各个所述粒子表示所述操作物体可能存在的位置姿势之一；
越是所述粒子中表示接近所述观测结果所示的所述操作物体的位置姿势的位置姿势的粒子，使粒子的权重越大，并且，在所述观测结果表示发生了所述接触的情况下，在表示所述关注物体和所述操作物体的形状以及相对位置关系的假想空间中，越是接近所述关注物体和以各个所述粒子所表示的位置姿势配置的所述操作物体接触的状态，使对应的所述粒子的权重越大；
计算基于调整了各所述粒子的权重的所述粒子的集合而推定出的所述操作物体的位置姿势的推定状态；
计划用于将所述操作物体从所述推定状态移动到所述此次移动的目标状态的行动；
指示所述机器人执行所计划的所述行动；
反复进行所述目标状态的设定、所述观测结果的取得、所述粒子的集合的设定、所述粒子的集合的调整、所述推定状态的计算、所述行动的计划以及所述行动的执行，直到所述推定状态在规定的误差内与所述完成状态一致。
日本申请2021-017687的公开通过参照其整体而并入本说明书中。
本说明书中记载的所有文献、专利申请以及技术规格，与具体且分别记载了通过参照而并入各个文献、专利申请以及技术规格的情况相同，在本说明书中通过参照而并入。

Claims (7)

1.一种机器人控制装置，
所述机器人控制装置控制机器人对操作物体进行操作，使得所述操作物体从作为操作对象的操作物体远离位于环境中的关注物体的状态转变为所述操作物体以特定形式与所述关注物体接触的完成状态，
所述机器人控制装置具备：
目标状态设定部，将作为所述操作物体达到所述完成状态为止的移动过程中的目标状态的中间目标状态或所述完成状态设定为此次移动的目标状态；
观测部，取得关于所述操作物体的位置姿势以及所述操作物体与所述关注物体之间是否存在接触的传感器的观测结果；
粒子集合设定部，设定表示所述操作物体的位置姿势的不确定性的粒子的集合，所述粒子的集合中含有的各个所述粒子表示所述操作物体可能存在的位置姿势之一；
粒子集合调整部，进行以下调整：越是所述粒子中表示接近所述观测结果所示的所述操作物体的位置姿势的位置姿势的粒子，使粒子的权重越大，并且，在所述观测结果表示发生了所述接触的情况下，在表示所述关注物体和所述操作物体的形状以及相对位置关系的假想空间中，越是接近所述关注物体和以各个所述粒子所表示的位置姿势配置的所述操作物体接触的状态，使对应的所述粒子的权重越大；
状态推定部，计算基于调整了各所述粒子的权重的所述粒子的集合而推定出的所述操作物体的位置姿势的推定状态；
行动计划部，计划用于将所述操作物体从所述推定状态移动到所述此次移动的目标状态的行动；
行动部，指示所述机器人执行所计划的所述行动；以及
处理控制部，反复进行所述目标状态的设定、所述观测结果的取得、所述粒子的集合的设定、所述粒子的集合的调整、所述推定状态的计算、所述行动的计划以及所述行动的执行，直到所述推定状态在规定的误差内与所述完成状态一致。
2.根据权利要求1所述的机器人控制装置，
还具备引导设定部，该引导设定部设定包含所述完成状态以及达到所述完成状态为止的一系列的所述中间目标状态的引导，
所述目标状态设定部相对于所述完成状态从远到近依次将所述引导中的所述中间目标状态设定为所述此次移动的目标状态，将所述完成状态设定为最后的所述此次移动的目标状态。
3.根据权利要求1或2所述的机器人控制装置，
所述完成状态、所述中间目标状态、所述粒子所表示的所述操作物体的位置姿势、所述推定状态以及所述行动由以所述关注物体为基准的相对坐标系的物体坐标系表示，
所述假想空间是由所述物体坐标系记述的空间，
所述观测部提供由所述传感器检测出的所述操作物体的位置姿势作为由所述物体坐标系表示的观测结果，
所述行动部将所述行动转换为所述机器人能够执行的指令并输出。
4.根据权利要求1至3中任一项所述的机器人控制装置，
所述操作物体是由所述机器人的末端执行器操作的物体。
5.根据权利要求1至3中任一项所述的机器人控制装置，
所述操作物体是安装在所述机器人的臂上的末端执行器，所述关注物体是在所述完成状态下由所述末端执行器完成保持的保持对象物体。
6.一种机器人控制方法，
所述机器人控制方法控制机器人对操作物体进行操作，使得所述操作物体从作为操作对象的操作物体远离位于环境中的关注物体的状态转变为所述操作物体以特定形式与所述关注物体接触的完成状态，
所述机器人控制方法用于使计算机执行以下处理：
将作为所述操作物体达到所述完成状态为止的移动过程中的目标状态的中间目标状态或所述完成状态设定为此次移动的目标状态；
取得关于所述操作物体的位置姿势以及所述操作物体与所述关注物体之间是否存在接触的传感器的观测结果；
设定表示所述操作物体的位置姿势的不确定性的粒子的集合，所述粒子的集合中含有的各个所述粒子表示所述操作物体可能存在的位置姿势之一；
越是所述粒子中表示接近所述观测结果所示的所述操作物体的位置姿势的位置姿势的粒子，使粒子的权重越大，并且，在所述观测结果表示发生了所述接触的情况下，在表示所述关注物体和所述操作物体的形状以及相对位置关系的假想空间中，越是接近所述关注物体和以各个所述粒子所表示的位置姿势配置的所述操作物体接触的状态，使对应的所述粒子的权重越大；
计算基于调整了各所述粒子的权重的所述粒子的集合而推定出的所述操作物体的位置姿势的推定状态；
计划用于将所述操作物体从所述推定状态移动到所述此次移动的目标状态的行动；
指示所述机器人执行所计划的所述行动；
反复进行所述目标状态的设定、所述观测结果的取得、所述粒子的集合的设定、所述粒子的集合的调整、所述推定状态的计算、所述行动的计划以及所述行动的执行，直到所述推定状态在规定的误差内与所述完成状态一致。
7.一种机器人控制程序，
所述机器人控制程序用于控制机器人对操作物体进行操作，使得所述操作物体从作为操作对象的操作物体远离位于环境中的关注物体的状态转变为所述操作物体以特定形式与所述关注物体接触的完成状态，
所述机器人控制程序用于使计算机执行以下处理：
将作为所述操作物体达到所述完成状态为止的移动过程中的目标状态的中间目标状态或所述完成状态设定为此次移动的目标状态；
取得关于所述操作物体的位置姿势以及所述操作物体与所述关注物体之间是否存在接触的传感器的观测结果；
设定表示所述操作物体的位置姿势的不确定性的粒子的集合，所述粒子的集合中含有的各个所述粒子表示所述操作物体可能存在的位置姿势之一；
越是所述粒子中表示接近所述观测结果所示的所述操作物体的位置姿势的位置姿势的粒子，使粒子的权重越大，并且，在所述观测结果表示发生了所述接触的情况下，在表示所述关注物体和所述操作物体的形状以及相对位置关系的假想空间中，越是接近所述关注物体和以各个所述粒子所表示的位置姿势配置的所述操作物体接触的状态，使对应的所述粒子的权重越大；
计算基于调整了各所述粒子的权重的所述粒子的集合而推定出的所述操作物体的位置姿势的推定状态；
计划用于将所述操作物体从所述推定状态移动到所述此次移动的目标状态的行动；
指示所述机器人执行所计划的所述行动；
反复进行所述目标状态的设定、所述观测结果的取得、所述粒子的集合的设定、所述粒子的集合的调整、所述推定状态的计算、所述行动的计划以及所述行动的执行，直到所述推定状态在规定的误差内与所述完成状态一致。
CN202280011668.9A 2021-02-05 2022-01-21 机器人控制装置、机器人控制方法以及机器人控制程序 Pending CN116829313A (zh)

Applications Claiming Priority (3)

Application Number Priority Date Filing Date Title
JP2021-017687 2021-02-05
JP2021017687A JP2022120650A (ja) 2021-02-05 2021-02-05 ロボット制御装置、ロボット制御方法、及びロボット制御プログラム
PCT/JP2022/002177 WO2022168634A1 (ja) 2021-02-05 2022-01-21 ロボット制御装置、ロボット制御方法、及びロボット制御プログラム

Publications (1)

Publication Number Publication Date
CN116829313A true CN116829313A (zh) 2023-09-29

Family

ID=82741747

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202280011668.9A Pending CN116829313A (zh) 2021-02-05 2022-01-21 机器人控制装置、机器人控制方法以及机器人控制程序

Country Status (5)

Country Link
US (1) US20240100698A1 (zh)
EP (1) EP4289564A1 (zh)
JP (1) JP2022120650A (zh)
CN (1) CN116829313A (zh)
WO (1) WO2022168634A1 (zh)

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2009169581A (ja) * 2008-01-15 2009-07-30 Toyota Motor Corp 移動体、移動体システム、及びその故障診断方法
JP5812940B2 (ja) * 2011-07-15 2015-11-17 三菱電機株式会社 複数の姿勢においてプローブを用いて物体をプロービングすることによって、物体に対するプローブの姿勢を求める方法およびシステム
EP3007867B1 (en) * 2013-06-11 2020-10-21 OnRobot A/S Systems and methods for sensing objects
JP7237483B2 (ja) * 2018-07-18 2023-03-13 キヤノン株式会社 ロボットシステムの制御方法、制御プログラム、記録媒体、制御装置、ロボットシステム、物品の製造方法
JP7299088B2 (ja) 2019-07-17 2023-06-27 中日本高速道路株式会社 床版取替工法

Also Published As

Publication number Publication date
EP4289564A1 (en) 2023-12-13
WO2022168634A1 (ja) 2022-08-11
JP2022120650A (ja) 2022-08-18
US20240100698A1 (en) 2024-03-28

Similar Documents

Publication Publication Date Title
CN110076772B (zh) 一种机械臂的抓取方法及装置
Chalon et al. Online in-hand object localization
JP6380828B2 (ja) ロボット、ロボットシステム、制御装置、及び制御方法
US20220258336A1 (en) Model generation apparatus, model generation method, control apparatus, and control method
US20220331964A1 (en) Device and method for controlling a robot to insert an object into an insertion
JP2022176917A (ja) ロボットデバイスを制御するための方法
Li et al. Manipulation skill acquisition for robotic assembly based on multi-modal information description
WO2021033471A1 (ja) 制御装置、制御方法、及び制御プログラム
Wang et al. Learning robotic insertion tasks from human demonstration
CN116829313A (zh) 机器人控制装置、机器人控制方法以及机器人控制程序
JP2022546733A (ja) 手の中の姿勢推定
JP2021035714A (ja) 制御装置、制御方法、及び制御プログラム
JP5812940B2 (ja) 複数の姿勢においてプローブを用いて物体をプロービングすることによって、物体に対するプローブの姿勢を求める方法およびシステム
JP7159525B2 (ja) ロボット制御装置、学習装置、及びロボット制御システム
Platt et al. Inferring hand-object configuration directly from tactile data
US20230278227A1 (en) Device and method for training a machine learning model to derive a movement vector for a robot from image data
US20230311331A1 (en) Device and method for controlling a robot to perform a task
WO2022168609A1 (ja) 制御システム、動作計画装置、制御装置、動作計画及び制御方法、動作計画方法、並びに制御方法
US20220274255A1 (en) Control apparatus, control method, and computer-readable storage medium storing a control program
Vladareanu et al. Robot control intelligent interfaces using the DSmT and the neutrosophic logic
US20220301209A1 (en) Device and method for training a neural network for controlling a robot
EP3978209B1 (en) Generative design techniques for soft robot manipulators
WO2022044615A1 (ja) 学習装置、学習方法、学習プログラム、制御装置、制御方法、及び制御プログラム
Nandikolla et al. Research Article Teleoperation Robot Control of a Hybrid EEG-Based BCI Arm Manipulator Using ROS
US20230278204A1 (en) Device and method for controlling a robot to perform a task

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