CN103517789B

CN103517789B - 运动预测控制装置和方法

Info

Publication number: CN103517789B
Application number: CN201280022935.9A
Authority: CN
Inventors: 江本周平; 林俊宽; 坂野肇; 藤井正和; 曾根原光治
Original assignee: IHI Corp
Current assignee: IHI Corp
Priority date: 2011-05-12
Filing date: 2012-04-24
Publication date: 2015-11-25
Anticipated expiration: 2032-04-24
Also published as: US9108321B2; EP2708334A4; EP2708334B1; WO2012153629A1; US20140074291A1; EP2708334A1; CN103517789A

Abstract

（A）通过用计测装置（12a、12b）计测对象物和机器人（2）的一方或双方而取得传感器信息。（B）基于传感器信息，通过状态推定装置（14）预测对象物和机器人（2）的一方或双方的内部状态来进行更新。（C）通过数据存储装置（16）存储内部状态。（D）通过机器人控制装置（20）控制机器人（2）。在（B）中，通过状态推定装置（14）在不依赖于机器人的控制周期的任意的时间点更新内部状态。在（D）中，通过机器人控制装置以控制周期基于存储在数据存储装置（16）的最新的内部状态，算出机器人的控制所需的预测值。

Description

运动预测控制装置和方法

技术领域

本发明涉及使用了机器人等的自动装置中的预测工件（work）或机器人等的运动的运动预测控制装置和方法。

背景技术

（计测机器人的位置来控制机器人的情况）

在使用设置在机器人上或机器人的外部的传感器信息算出机器人的位置、姿势而对机器人进行驱动控制的控制装置中，作为在机器人的位置和姿势的计测中使用的传感器、计测方法，一般如下。

（1）　使用机器人上的内置传感器的方法。

例如，用设置在机器人上的陀螺仪（gyrosensor）或车轮的编码器（encoder）等计测机器人的速度、加速度、角速度等。用设置在机器人上的操纵（steering）部的编码器等计测机器人的操向角。

（2）　使用机器人上的外置传感器的方法。

例如，用设置在机器人上的测距传感器（激光测距仪（laserrangefinder）（LRF）、激光雷达（laserradar）等）、照相机等计测机器人周围的参照物（landmark）的位置、姿势。用设置在机器人上的GPS罗盘（compass）等计测机器人的当前的位置和姿势中的一个或两个。

（3）　用机器人外的外部传感器进行计测的方法。

用设置在机器人外部的测距传感器、照相机等对计测范围内的机器人的位置、姿势进行计测。

用单一的传感器高精度地求出机器人的位置、姿势是困难的，因此，有时使用多种上述的传感器、计测方法，根据得到的多个计测结果求出位置、姿势。

但是，由于如下的理由，不能简单地比较多个计测结果。

用（2）（3）计测的位置和姿势是用以进行计测的传感器、特定的点为基点的坐标系表示的相对的位置和姿势。计测坐标的基点各不相同，而且，在机器人上的传感器的情况下，成为基点的机器人本身的位置并不清楚，因此，难以单纯地用坐标变换等来比较这些计测结果。

因为（1）不直接计测位置、姿势，所以不能直接与（2）（3）进行比较。

此外，如以下所述，同步地计测所有的传感器信息是困难的。因此，存在得到的计测结果的计测时刻各不相同的情况。因为机器人的位置和姿势时时刻刻地变化，所以不能单纯地比较时刻不一致的计测结果。特别是，在机器人的速度快的情况下等，因为位置和姿势的变化量大，所以容易引起这样的问题。

a　一般来说，同一机器人内的内置传感器的传感器信息被同步地取得。但是，例如，难以在不同的机器人之间共用一个同步信号（需要有线连接）。因此，在多个机器人之间使内置传感器的信息同步是困难的。

b　在像（3）那样使用外部传感器的情况下，也与a同样地，在需要在多个装置之间收发同步信号的情况下，外部传感器与机器人上的传感器的同步是困难的。

c　在（2）或（3）中使用的传感器中存在由传感器其本身的计测时间、处理从传感器得到的传感器信息的时间等造成的计测延迟。因此，用传感器得到的计测结果会与计测延迟的量相应地成为旧的传感器信息，因此，计测时刻与像（1）那样的内置传感器的传感器信息不同。此外，因为存在计测对象从传感器的视野脱离的情况，所以不一定能始终得到指定的时刻的计测结果。

（操纵（handling）移动的工件的情况）

在用机器人操纵移动的工件的自动装置中，需要用设置在机器人上或机器人的外部的照相机等视觉传感器、激光测距仪（LRF）等测距传感器计测工件的位置，基于该计测结果对机器人臂进行跟随控制。

在机器人本身移动的移动机器人的情况下，也需要用设置在机器人上或机器人的外部的传感器计测移动机器人的位置，基于计测结果对机器人进行控制，使用同样的技术。关于计测机器人的位置的方法，如上所述，有用机器人上的传感器计测与参照物的相对位置的方法、用设置在机器人的外部的传感器直接计测机器人的位置的方法。

以下，也将移动的工件、移动机器人仅称为“移动体”。

在上述的机器人的运动预测控制中，以固定周期（例如：30fps的照相机）计测时时刻刻变化的对象物（工件、参照物）或机器人之间的相对位置，基于计测结果，例如对机器人输出使机器人靠近工件那样的移动指令。

但是，即使以计测的工件的位置为目标使机器人动作，有时也会由于传感器的计测延迟、数据的取得延迟、机器人的动作延迟等的控制延迟而造成机器人不能跟随移动的工件。

此外，在视觉传感器的情况下，因为一般来说传感器的计测周期比机器人的控制周期（例如：4ms）长，所以机器人并不是按每个控制周期得到最新的计测结果，而是与该量相应地成为计测延迟。特别是，当在图像处理中花费时间或者工件从照相机的视野脱离时，计测结果被更新的周期会变得更长，而且变得不固定。像这样，在操纵移动的工件或移动机器人的装置中，存在由于控制延迟而造成机器人的跟随性能降低的问题。

为了解决关于上述的背景技术的问题，已经提出了各种各样的控制方案（例如，专利文献1～4）。

专利文献1的“三维运动预测装置”根据进行单振动的对象物的位置数据来推定其运动参数，预测其将来的位置，基于该位置信息利用操纵器（manipulator）把持对象物。

专利文献2的“状态推定方案”基于通过观测以时序方式输入的观测信号，推定进行该观测的系统的内部状态。所谓内部状态，指的是对象物的位置、姿势、摆动角等的状态变量。

专利文献3的“运动预测装置”并用背景（background）处理和前景（foreground）处理进行自动跟踪和运动预测。

在专利文献4的“机器人装置及其控制方法”中，提出了如下的方法，即，首先使臂跟随对象物，利用跟随时的臂的运动状态与对象物的运动状态一致的情况，预测对象物的移动目的地。像这样，通过考虑臂的运动状态来预测对象物的移动目的地，从而可实现移动体的把持。

现有技术文献

专利文献

专利文献1：特开平07-019818号公报；

专利文献2：专利第4072017号公报；

专利文献3：专利第4153625号公报；

专利文献4：专利第4265088号公报。

发明内容

发明要解决的课题

根据上述的专利文献1，根据对对象物（被计测体或工件）进行计测的观测历史来推定对象物的运动状态，由此，能对未得到计测值的期间的数据进行补充，或者能预测用机器人把持对象物时的把持地点。

在运动状态的推定中，一般来说使用卡尔曼滤波器（Kalmanfilter）或粒子滤波器（particlefilter）等贝叶斯滤波器（Bayesfilter）。在贝叶斯滤波器中，像在专利文献2中公开的那样，将运动的状态（对象物的速度、加速度等）作为“内部状态”，事先定义表示内部状态如何与时间一起变化的“状态变迁模式”，定义“观测方程式”。

关于贝叶斯滤波器，大致划分，可用以下的两个处理来实现。

（1）预测任意时刻的内部状态的“预测处理”。

（2）根据内部状态预测计测值，与实际的计测值进行比较来对内部状态进行修正的“更新处理”（在专利文献2中，记述为“滤波推定方案”）。

在机器人在控制中使用状态推定结果的情况下，可考虑如下的两个结构。

A　在机器人控制装置内安装有上述的算法，按每个机器人控制周期进行（1）的预测处理，在得到传感器的计测值时，进行（2）的更新处理。

B　像专利文献1那样，做成由状态推定装置和机器人构成的系统，机器人从推定装置读出预测值，在控制中使用。

在对机器人等自动机械的运动进行控制时，要求以固定的控制周期（例如：4ms周期）算出控制指令值。与此相对地，在现有技术中，未提出将使用运动状态的推定的系统以考虑时间限制的方式构成的具体的方法。

即，在机器人控制装置中，因为受到更新运动状态（内部状态）的处理所需的时间的影响，所以以确定的控制周期算出控制指令值来控制机器人会变得困难。例如，存在如下的问题。

在上述的A的方案中，存在在机器人的控制周期内更新处理未结束的可能性。关于状态推定器的更新处理，一般来说要花费时间，特别是，在计测值的种类多的情况下，在使用粒子滤波器或其它复杂的处理的情况下，特别花费时间。

此外，在专利文献2的方案中，通过根据观测值、状态变量的种类划分预测处理、更新处理，从而能根据目的而减少计算量，但是，在想对复杂的运动、所得到的计测值全部进行反映的情况下，最终需要连结所有的处理，因此，推定所需的计算量不会减少。

此外，在上述的B的方法中，虽然机器人控制装置无需进行更新处理，但是，从机器人要求预测值开始，推定装置必须在固定时间内结束预测处理，返回预测值。想要设计满足它的状态推定装置，例如，需要考虑如下的问题。

（1）在机器人增加为多个时或更新处理的负荷高时，推定装置的运算处理能力会被分散。即使在这样的状况下，预测处理也必须在固定时间内结束。

（2）因为机器人的控制周期按每个机器人而有所不同，所以，从机器人侧的要求开始到返回预测值为止的时间限制不能一致地确定。

（3）机器人控制装置与状态推定装置之间的通信需要花费固定的时间。该通信的所需时间根据通信方式（有线、无线等）而有所不同。

由于如上所述的问题，像上述的那样，以确定的控制周期算出控制指令值来控制机器人会变得困难。

因此，本发明的目的在于，提供一种如下的运动预测控制装置和方法，即，在推定对象物与机器人的一方或双方的内部状态，基于该内部状态生成机器人的控制所需的预测值（控制指令值）的情况下，能在不受内部状态的更新处理所花费的时间的影响的情况下以确定的控制周期生成控制指令值。

用于解决课题的方案

根据本发明，提供一种运动预测控制装置，其特征在于，具备：通过对对象物和机器人的一方或双方进行计测而取得传感器信息的计测装置；基于传感器信息预测对象物和机器人的一方或双方的内部状态来进行更新的状态推定装置；存储所述内部状态的数据存储装置；以及控制所述机器人的机器人控制装置，状态推定装置在不依赖于机器人的控制周期的任意的时间点更新所述内部状态，机器人控制装置以所述控制周期基于存储在数据存储装置的最新的内部状态算出机器人的控制所需的预测值。

根据本发明的第一实施方式，计测装置计测对象物、机器人之间的相对关系作为所述传感器信息，状态推定装置预测对象物、机器人（对象物和机器人的一方或双方）的内部状态来进行更新，所述状态推定装置具有预测所述内部状态的整体预测部和对存储在数据存储装置的内部状态进行更新的更新部，所述机器人控制装置具有以所述机器人的控制周期预测该控制所需的预测值的机器人上预测部，

（A）在不依赖于所述控制周期的任意的时间点，通过所述计测装置、状态推定装置及数据存储装置计测对象物、机器人之间的相对关系，预测对象物、机器人的内部状态来进行更新，存储更新的内部状态和在预测中使用的状态变迁方程式，

（B）通过所述机器人控制装置，基于存储在数据存储装置的最新的内部状态，以所述控制周期预测所述预测值，基于预测值实时地控制机器人。

另外，所谓对象物、机器人之间的相对关系，指的是对象物与机器人控制装置所控制的机器人的相对关系（例如，相对位置、相对姿势、相对速度、相对加速度或相对姿势变化）或机器人控制装置所控制机器人与其它机器人的相对关系（例如，相对位置、相对姿势、相对速度、相对加速度或相对姿势变化）（以下，相同）。

根据本发明的第二实施方式，计测装置在不依赖于机器人的控制周期的任意的时间点取得对象物或多个机器人（即，对象物和多个机器人、对象物、多个机器人、或多个机器人中的一部分）的传感器信息，状态推定装置在取得所述传感器信息时对与取得传感器信息的时刻同时刻的各机器人的内部状态进行预测，将预测的内部状态（例如，通过观测方程式变换为传感器信息）与计测的传感器信息进行比较而进行更新，数据存储装置在进行所述更新时存储更新的内部状态和在预测中使用的状态变迁方程式，多个机器人控制装置基于存储在数据存储装置的最新的内部状态预测各机器人所需的预测值。

根据本发明的第三实施方式，所述机器人是把持相对地移动的对象物的机器人，计测装置计测对象物的位置或姿势作为所述传感器信息，状态推定装置根据对对象物进行计测的计测结果，基于状态变迁模式和观测模式对包括精度指标值的对象物的内部状态进行更新，而且根据精度指标值判定能否把持对象物，在能把持的情况下，机器人控制装置以基于内部状态的更新结果预测对象物的移动目的地而进行把持动作的方式控制机器人。

此外，根据本发明，提供一种运动预测控制方法，其特征在于，（A）通过用计测装置对对象物和机器人的一方或双方进行计测而取得传感器信息，（B）基于传感器信息，通过状态推定装置预测对象物和机器人的一方或双方的内部状态来进行更新，（C）通过数据存储装置存储所述内部状态，（D）通过机器人控制装置控制所述机器人，在所述（B）中，通过状态推定装置在不依赖于机器人的控制周期的任意的时间点更新所述内部状态，在所述（D）中，通过机器人控制装置以所述控制周期基于存储在数据存储装置的最新的内部状态算出机器人的控制所需的预测值。

根据本发明的第一实施方式，所述状态推定装置具有预测所述内部状态的整体预测部和对存储在数据存储装置的内部状态进行更新的更新部，在所述（A）～（C）中，在不依赖于所述控制周期的任意的时间点，通过所述计测装置、状态推定装置及数据存储装置计测对象物、机器人之间的相对关系作为所述传感器信息，预测对象物、机器人的内部状态来进行更新，存储更新的内部状态和在预测中使用的状态变迁方程式，在所述（D）中，通过所述机器人控制装置基于存储在数据存储装置的最新的内部状态，以所述控制周期预测所述预测值，实时地控制机器人。

根据本发明的第二实施方式，在不依赖于机器人的控制周期的任意的时间点，在所述（A）中，通过计测装置取得对象物或多个机器人的传感器信息，在所述（B）中，通过状态推定装置对与取得传感器信息的时刻同时刻的各机器人的内部状态进行预测，将预测的内部状态与传感器信息进行比较来进行更新，在所述（C）中，通过数据存储装置存储更新的内部状态和在预测中使用的状态变迁方程式，在所述（D）中，通过多个机器人控制装置基于存储在数据存储装置的最新的内部状态预测各机器人所需的预测值。

根据本发明的第三实施方式，所述机器人是把持相对地移动的对象物的机器人，在所述（A）中，通过计测装置计测对象物的位置或姿势作为所述传感器信息，在所述（B）中，根据计测的计测结果通过状态推定装置基于状态变迁模式和观测模式对包括精度指标值的对象物的内部状态进行更新，而且根据精度指标值判定能否把持对象物，在所述（D）中，在能把持的情况下，通过机器人控制装置基于内部状态的更新结果预测对象物的移动目的地而使机器人进行把持动作，在不能把持的情况下，重复所述（A）和（B），直到变得能把持为止。

发明效果

根据上述的本发明，在不依赖于机器人的控制周期的任意的时间点，取得对象物和机器人的一方或双方的传感器信息，基于传感器信息预测对象物和机器人的一方或双方的内部状态来进行更新，因此，无需在机器人的控制周期内对内部状态进行更新。

此外，基于存储在数据存储装置的最新的内部状态，以所述控制周期预测所述预测值而实时地控制机器人，因此，能在不受状态推定的计算量的影响的情况下以确定的控制周期算出机器人的控制所需的预测值（控制指令值）来控制机器人。

因此，机器人控制装置能在不受内部状态的更新处理所花费的时间的影响的情况下以确定的控制周期生成控制指令值。

附图说明

图1是具备根据本发明的第一实施方式的运动预测控制装置的机器人系统的第一结构例。

图2是根据本发明的第一实施方式的运动预测控制装置的第一结构例。

图3是根据本发明的第一实施方式的运动预测控制装置的第二结构例。

图4是具备根据本发明的第一实施方式的运动预测控制装置的机器人系统的第二结构例。

图5是具备根据本发明的第一实施方式的运动预测控制装置的机器人系统的第三结构例。

图6是具备根据本发明的第二实施方式的运动预测控制装置的机器人系统的示意图。

图7是根据本发明的第二实施方式的运动预测控制装置的第一结构例。

图8是根据本发明的第二实施方式的运动预测控制装置的第二结构例。

图9是根据本发明的第二实施方式的运动预测控制装置的第三结构例。

图10是根据本发明的第二实施方式的运动预测控制装置的第四结构例。

图11是具有根据本发明的第三实施方式的运动预测控制装置的机器人系统的结构例。

图12是根据本发明的第三实施方式的运动预测控制装置的结构例。

图13是根据本发明的第三实施方式的运动预测控制方法的整体流程图。

图14是示出本发明的第三实施方式的实施例中的经过时间与精度指标值E的关系的图。

具体实施方式

以下，基于附图详细地说明本发明的优选实施方式。另外，在各图中，对共同的部分标注相同的附图标记，省略重复的说明。

[本发明的第一实施方式]

在该图中，1是工件，2是能移动的机器人，3是机器人臂，4是机器手，5a是固定在机器手4的第一照相机，5b是固定在外部的定位置的第二照相机，6是被固定的参照物，10是第一实施方式的运动预测控制装置。以下，将工件1和参照物6综合起来作为“对象物”。

该机器人系统以如下方式构成，即，用照相机5a、5b对一边进行摆运动一边移动的工件1或被固定的参照物6进行计测，用照相机5b对机器人2进行计测，使机器人2跟随工件1地进行控制，通过机器手4把持工件1。另外，在该例子中，无需知道参照物6的位置。

在该图中，第一实施方式的运动预测控制装置10具备计测装置12、状态推定装置14、数据存储装置16及机器人控制装置20。装置12、14、20与数据存储装置16连接。

计测装置12计测对象物、机器人2之间的相对关系。将用计测装置12计测的对象物、机器人之间的相对位置、姿势等的计测结果设为计测值Y。计测值Y根据计测装置的种类、所计测的对象而分别进行定义。根据所计测的对象（是工件1，还是参照物6，还是移动机器人2）、计测装置的种类，计测值Y的内容不同。

例如，所谓相对关系是位置、姿势、速度、角速度、加速度、角加速度等的信息。

在该例子中，计测装置12由分别与第一照相机5a、第二照相机5b、机器人控制装置20连接的三台计测装置（第一计测装置12a、第二计测装置12b、第三计测装置12c）构成。

即，在该例子中，分别使用两台照相机和计测装置，从第一照相机5a和第二照相机5b导入对象物的图像，通过图像处理求出对象物的计测值Y。此外，计测装置12b还导入机器人2的图像，通过图像处理，还求出机器人2的计测值Y。计测装置12c用编码器计测机器人2的轮胎的移动量、臂关节的旋转量而求出机器人2的计测值Y。求出的对象物、机器人2的计测值Y存储在数据存储装置16。

另外，计测装置12不限定于该结构，也可以使用能计测对象物、机器人2之间的相对关系的装置，例如可以使用激光雷达、加速度传感器、陀螺仪、速度传感器等。

在状态推定装置14安装有基于卡尔曼滤波器的算法的状态推定算法。在该算法中，将吊有工件1的摆的摆动角θ、Δθ、支点位置x、y、z、参照物6的位置Xl、Yl、Zl、姿势RXl、RYl、RZl、机器人2的位置Xr、Yr、Zr、姿势RXr、RYr、RZr等的状态变量综合起来设为内部状态X。

此外，定义示出当时刻经过时内部状态X是如何变化的状态变迁方程式X（t+Δt）＝f（X（t））和将内部状态X和计测值Y对应起来的观测方程式Y＝h（X）。观测方程式按计测值Y的每个种类分别进行定义。

此外，该算法对内部状态X的误差分布C_ovX、计测值的误差分布C_ovY等进行管理，能根据这些数据求出状态推定的精度。另外，在以下的例子中，将这些精度指标值综合起来设为E。

状态推定装置14具有预测对象物、机器人2的内部状态的整体预测部15a和对存储在数据存储装置16的内部状态进行更新的更新部15b，预测对象物、机器人2的内部状态来进行更新。

在整体预测部15a中，使用状态变迁方程式f（X）直到指定的时刻为止进行使记录在数据存储装置16的内部状态X变迁的计算。

在更新部15b中，使用数据存储装置16中的内部状态X和观测方程式h（X）算出对象物、机器人2的计测值Y的预测值。将所输入的计测值Y与预测值进行比较，更新内部状态X，记录在数据存储装置16。此时，还同时更新状态推定的精度指标值E，记录在数据存储装置16。

上述的计测装置12、状态推定装置14及数据存储装置16是在与机器人2的控制周期不同的时间点被执行的非实时处理系统。该非实时处理系统的处理间隔可以比机器人2的控制周期长，而且周期也可以不固定。该处理间隔例如是30～80ms。

因此，根据上述的结构，在与机器人2的控制周期不同的时间点，通过计测装置12、状态推定装置14及数据存储装置16计测对象物、机器人之间的相对关系（例如，相对位置和姿势），预测对象物、机器人2的内部状态来进行更新，存储更新的内部状态和在预测中使用的状态变迁方程式。

在该例子中，机器人控制装置20具有接收部21、指令值算出部22、发送部23及机器人上预测部24。

接收部21从机器人2接收其位置信息（编码器信息等）。

指令值算出部22根据由机器人上预测部24得到的预测值算出对机器人2的指令值。预测值例如是工件1或机器人2的当前位置或未来位置。

发送部23将算出的指令值发送给机器人2。

机器人上预测部24以规定的控制周期预测机器人2的控制所需的预测值。

即，机器人控制装置20具有进行用于得到机器人2的控制所需的预测值的预测处理（prediction）的机器人上预测部24。

另外，机器人上预测部24只要能预测与机器人2的控制关联的内部量（预测值）即可，无需预测所有的内部量。

机器人控制装置20对机器人2以固定周期（例如：4ms）发送动作指令，接收机器人2的编码器信息等。接收的位置信息由前述的计测装置12c取得，成为移动机器人2的计测值Y。

使用机器人上预测部24预测对象物、机器人2的当前及未来的运动状态，用于指令值的算出。

机器人上预测部24使用状态变迁方程式f（X）直到指定的时刻为止进行使记录在数据存储装置16的内部状态X中的与机器人控制关联的状态量变迁的计算。

数据存储装置16保持有上述的内部状态X、内部状态的模式时刻tx、计测值Y、计测的时刻ty、状态变迁方程式f、观测方程式h、状态推定的精度指标值E等的数据。

上述的机器人控制装置20是以机器人2的控制周期进行控制的实时处理系统。该控制周期是比上述的非实时处理系统的处理间隔短的固定周期。该控制周期例如是3～4ms。

因此，根据上述的结构，通过机器人控制装置20基于存储在数据存储装置16的最新的内部状态，以机器人2的控制周期预测该控制所需的预测值，实时地控制机器人2。

以下，对根据本发明的第一实施方式的运动预测控制装置10的动作进行说明。

计测装置12在任意的时间点对对象物、机器人2进行计测，将计测结果作为传感器信息记录在数据存储装置16。

当在数据存储装置16记录有新的计测结果时，状态推定装置14通过预测处理、更新处理（Correction）推定内部状态。算出的内部状态、在推定中使用的状态变迁方程式等记录在数据存储装置16。

机器人控制装置20以固定的控制周期控制机器人2。此时，使用机器人上预测部24算出任意的时刻的对象物、机器人2的预测位置，在控制中使用。机器人上预测部24使用记录在数据存储装置16的最新的内部状态、状态变迁模式进行预测处理。

机器人控制装置20作为以固定的控制周期进行处理的实时处理系统进行动作。除此以外的各装置，是不具有固定周期的时间限制的非实时处理系统即可。

以下，对上述的第一实施方式的运动预测控制装置10的作用进行说明。

两台计测装置（第一计测装置12a和第二计测装置12b）在任意的时间点重复以下的（1）～（3）。

（1）在任意的时间点对照相机5a、5b发送快门信号，取得所拍摄的图像。

（2）根据得到的图像，通过图像处理取得对象物、机器人2的位置、姿势信息（计测值Y）。例如，提取图像内的白色区域，求出重心。

（3）将算出的计测值Y和快门时刻（被计测的时刻ty）、计测值Y的误差分布C_ovY记录在数据存储装置16。

此外，计测装置12c在任意的时间点重复以下的（a1）～（a3）。

（a1）在任意的时间点取得机器人控制装置20的编码器信息。

（a2）根据得到的编码器信息，求出轮胎的移动量或关节角度，取得移动机器人2的位置、姿势信息（计测值Y）。

（a3）将算出的计测值Y和编码器的计测时刻、计测值Y的误差分布C_ovY记录在数据存储装置16。

每当计测值Y被更新时，状态推定装置14实施以下的（4）～（6）。

（4）监视数据存储装置16，如果记录有新的计测值Y，就从数据存储装置16读出Y、ty、X、tx、f、h、E。

（5）内部状态X示出时刻tx时的内部状态，因此，使用整体预测部15a预测时刻ty时的内部状态。

（6）使用更新部15b预测时刻ty时的计测值，比较预测值与计测值Y，修正内部状态X。将修正的内部状态X和新的模式时刻tx（＝ty）记录在数据存储装置16。此外，更新部15b算出内部状态X的误差分布C_ovX、计测值与预测值之差等的精度指标值E，记录在数据存储装置16。

机器人控制装置20以任意的周期重复以下的（7）～（10）。

（7）从机器人2接收编码器信息等。关于与机器人2的通信，例如以4ms周期进行的方式由机器人2来管理。因此，机器人控制部20待机到接收数据为止，从接收数据起必须在4ms以内结束（10）的发送处理。

（8）使用机器人上预测部24算出工件、机器人的当前位置、未来的位置。在进行该计算时，机器人上预测部24参照数据存储装置16中的最新的内部状态等。

（9）以使臂的指尖靠近预测的工件1的位置的方式算出目标轨道。此外，对状态推定的精度指标值E进行评价，在高精度地进行状态推定的情况下，以向未来的工件的位置进行把持动作的方式，算出臂的目标轨道、机器手开闭指令值。

（10）将算出的目标轨道、机器手开闭指令值发送给机器人2。

该例子适合机器人控制装置20与数据存储装置16之间的通信所需时间长的情况。

即，在该例子中，数据存储装置16具有如下功能，即，在不依赖于来自机器人2的要求的时间点将存储在数据存储装置16的最新的内部状态转发给机器人上存储部26。

根据该结构，数据存储装置16将记录在数据存储装置16的内部状态、状态变迁模式转发给机器人上存储部26。机器人上预测部24参照机器人上存储部26中的数据进行处理。

优选进行转发的时间点是数据存储装置16的内部状态X被更新时（（6）之后紧接着）。

另外，在检测到在数据存储装置16记录有新的数据时（（6）之后紧接着）或按每个固定的时间进行数据转发。或者，可以在前述的两个时间点进行。

根据该结构，将数据存储装置16的内容逐次转发给机器人上存储部26，因为机器人上预测部24参照机器人上存储部26，所以在上述的（8）的计算中无需进行通信。因此，即使在使用了无线通信等通信所需时间长的通信的情况下，也能避免通信时间的影响。

图4是具备根据第一实施方式的运动预测控制装置的机器人系统的第二结构例。

该例子与机器人系统的第一结构例不同的点在于，机器人2被固定且没有参照物6。其它结构与第一结构例相同。

即，该例子与机器人系统的第一结构例不同的点在于，是用被固定的机器人2的臂3把持移动的工件1的情形，可以不进行关于机器人2的推定。

图5是具备根据第一实施方式的运动预测控制装置的机器人系统的第三结构例。

该例子与第一结构例不同的点在于，是机器人2根据已知位置的参照物6推定自身位置的情形，可以不进行关于工件的推定、参照物6的位置和姿势的推定。

该例子是移动机器人2对已知位置的参照物6进行计测来推定机器人2的自身位置的情况。在该例子中，所推定的内部量X是机器人2的原点位置（X_r、Y_r、Z_r）、机器人2的姿势（R_xr、R_yr、R_zr）。除此以外，也可以推定车轮的操向角、速度、角速度等。

关于在更新处理中使用的计测值，使用照相机5a计测参照物6的结果、照相机5b计测机器人2的位置的结果、机器人控制装置20记录的车轮的旋转量（θ）等。

在机器人系统的第三结构例中，其它方面可以与机器人系统的第一结构例相同。

另外，本发明的第一实施方式不限定于上述的结构例。

例如，在存在多个机器人和工件的情况下，会变成工件的运动状态推定和机器人的运动状态推定的组合，因此，通过上述的第一实施方式能实现考虑了机器人控制的时间限制的系统。

在上述的实施例中，虽然示出了进行摆运动的工件、车轮型的移动机器人，但是，各自移动的形态不限定于此。例如，工件可以是在传送装置（conveyer）上移动的工件，也可以是在水上浮游的工件、飞着的工件。机器人可以是爬行式、在轨道上移动的方式等。此外，即使是固定的臂，“移动”中也包括地基晃动或被动地活动的情形。

此外，各装置无需划分为各自的处理装置。例如，也可以是作为计测程序、状态推定程序在一个PC上并行处理多个程序的结构。

但是，使机器人控制装置（程序）为在固定周期的限制下进行处理的实时处理系统。另外，在实时OS中，能使非实时处理的程序与实时处理的程序并行。

数据存储装置16只要是多个装置、程序对相同的数据进行读写的共用存储器即可。因此，无需做成独立的装置，只要准备在状态推定装置14或计测装置12等的存储器空间上即可。

根据上述的第一实施方式的装置和方法，通过计测装置12、状态推定装置14及数据存储装置16计测对象物、机器人之间的相对关系（例如，相对位置），预测对象物、机器人之间的内部状态来进行更新，存储更新的内部状态和在预测中使用的状态变迁方程式，因此，能基于对象物（工件、参照物）或机器人的相对关系的计测结果（例如，位置、姿势）进行移动体（工件、移动机器人）的运动预测。

此外，机器人控制装置20具有以机器人2的控制周期预测机器人2的控制所需的预测值的机器人上预测部24，基于存储在数据存储装置16的最新的内部状态，以所述控制周期预测所述预测值，实时地控制机器人2，因此，能在不受状态推定的计算量的影响的情况下，按每个机器人以确定的控制周期算出控制指令值来控制机器人2。

即，机器人控制装置20能在不受状态推定的更新处理所花费的时间的影响的情况下进行动作。

此外，在计测装置12或机器人控制装置20增加而使状态推定的计算量增加的情况下，各机器人控制装置20也是独立地进行预测计算，因此，预测处理所花费的时间不会变长。因此，在系统变更时，无序重新考虑运算处理能力等的设计。

进而，能按每个机器人控制装置20设定预测处理的时间限制。此外，算出的预测值的精度、预测值的种类等也能按每个机器人控制装置20进行设定。因此，能考虑预测处理所花费的时间或控制周期等，在每个机器人2中实现改变预测计算的精度或者只计算内部状态中的所需的变量的想法。

此外，数据存储装置16在不依赖于来自机器人2的要求的时间点将存储在数据存储装置16的最新的内部状态转发给机器人上存储部26，由此，即使在机器人控制装置20与状态推定装置14之间的通信花费时间的情况下，或在通信所需时间不固定的情况下，也能立刻参照预测处理所需的数据。在该情况下，虽然在机器人2上进行参照的数据由于通信延迟的影响而未必是最新值，但是，预测处理能在固定时间内结束。

另外，第一实施方式不限定于上述的结构例，由权利要求书的记载示出，进而包括与权利要求书的记载均等的意思及范围内的所有的变更。

[本发明的第二实施方式]

在该图中，101是移动机器人、102是固定在移动机器人101的外部的参照物、103是固定在移动机器人101的内部传感器、104是固定在移动机器人101的外部的外部传感器。

在该系统中，多个移动机器人101能分别自主地移动。

以下，在第二实施方式中，将移动机器人仅称为“机器人”。

在该图中，第二实施方式的运动预测控制装置110具备计测装置112、状态推定装置114、数据存储装置116及多个机器人控制装置120。装置112、114、120分别与数据存储装置116直接连接。

在该例子中，多个机器人控制装置120由第一机器人控制装置120A、第二机器人控制装置120B及第三机器人控制装置120C构成。

另外，在该例子中，虽然机器人控制装置120是三台，但是，可以是两台，也可以是四台以上。

计测装置112与对应的外部传感器104连接，导入外部传感器104的传感器信息（观测值）。作为外部传感器104，可举出照相机、激光测距仪（LRF）、三维激光雷达、陀螺仪、GPS罗盘、脉冲编码器等。

计测装置112在任意的时间点从外部传感器104读入传感器信息，进行后述的计测处理（作用的（1）～（3））。

机器人控制装置120上的计测部124具有与计测装置112同样的功能。此外，在该例子中，将后述的机器人控制装置120的结构中的没有移动机构、用于驱动控制的处理部的称为“计测装置112”。

在状态推定装置114安装有基于卡尔曼滤波器的算法的状态推定算法。在该算法中，将机器人101的位置x、y、z、速度vx、vy、vz、角速度drx、dry、drz等状态变量综合起来设为内部状态X。

此外，定义示出当时刻经过时内部状态X是如何变化的状态变迁方程式X（t+△t）＝f（X（t））和将内部状态X与观测值Y对应起来的观测方程式Y＝h（X）。观测方程式按观测值Y（传感器信息）的每个种类分别定义。

此外，该算法对内部状态X的误差分布C_ovX、观测值Y的误差分布C_ovY等进行管理，能根据这些数据求出状态推定的精度。另外，在以下的例子中，将这些精度指标值综合起来设为E。

在状态推定装置114安装有整体预测部115a和更新部115b这两个功能。

整体预测部115a使用状态变迁方程式f（X）直到指定的时刻为止进行使记录在数据存储装置116的内部状态X变迁的计算。

更新部115b使用到计测时刻为止使用整体预测部115a进行变迁的内部状态X和观测方程式h（X），算出对象物（机器人101或参照物102）的观测值Y的预测值。比较输入的观测值Y与预测值，更新内部状态X，记录在数据存储装置116。此时，还同时更新状态推定的精度指标值E，记录在数据存储装置116。

图7的机器人控制装置120示出了在一般的机器人101中使用的结构。

在该图中，机器人控制装置120由用于移动的驱动部121、控制驱动部121的车辆控制部122、算出机器人101的目标轨道的行动计划部123、进行各传感器103的传感器处理的计测部124构成。

作为传感器103使用的主要是计测机器人101的角速度或加速度的陀螺仪103a、计测周围的形状的测距传感器103b（LRF）、计测设置在机器人101的车轮的旋转量的车轮用编码器103c。

图8是根据本发明的第二实施方式的运动预测控制装置的第二结构例。另外，在该图中，只示出了机器人控制装置120。

在第二实施方式中，不限定传感器103的种类。即，例如如图8所示，可以追加照相机103d、GPS罗盘103e、设置在机器人101的车轮的操向角计测用的操向角编码器103f等传感器，也可以是不使用一部分的传感器的结构。在运动预测控制装置的第二结构例中，其它方面可以与运动预测控制装置的第一结构例相同。

在图7、图8中，行动计划部123是以固定的控制周期动作的实时处理系统，使用机器人上预测部123a预测本机器人101的当前或未来的位置、姿势，在由轨道生成部123b进行的机器人101的目标轨道的算出中使用。此外，在避免碰撞的情况下等，根据需要，预测其它机器人101的当前或未来的位置、姿势。

机器人上预测部123a使用状态变迁方程式f（X）直到指定的时刻为止进行使记录在数据存储装置116的内部状态X中的与机器人控制关联的状态量变迁的计算。机器人上预测部123a以能在实时控制的时间限制下进行预测处理的方式设计。

车辆控制部122是以固定的控制周期动作的实时处理系统，以机器人101沿目标轨道运动的方式，在指令值算出部122a算出指令值来控制驱动部121。

通信部125具有机器人101（即，设置在机器人101的机器人控制装置120）与数据存储装置116之间的数据通信功能。具体地说，使用以太网（Ethernet)）（注册商标）或USB等进行有线通信或蓝牙（Bluetooth）（注册商标）、无线LAN等进行无线通信的设备。

数据存储装置116保持有内部状态X、内部状态的模式时刻tx、观测值Y、计测的时刻ty、状态变迁方程式f、观测方程式h、状态推定的精度指标值E等的数据。

以下，对上述的本发明的第二实施方式的运动预测控制装置110的作用进行说明。

计测装置112或计测部124在任意的时间点重复以下的（1）～（3）。

（1）在任意的时间点取得传感器信息。例如，在照相机的情况下，对照相机发送快门信号，取得拍摄的图像。

（2）处理取得的传感器信息，求出机器人101、参照物102的位置和姿势、机器人101的速度或角速度等的观测值Y。观测值Y按每个传感器而有所不同，因此，按每个传感器进行定义。此外，在此进行的传感器信息的处理也根据传感器的种类而有所不同。例如，在照相机的情况下，根据得到的图像通过图像处理取得对象物（机器人101或参照物102）的位置和姿势的传感器信息（例如：提取图像内的白色区域，求出重心）。此外，例如，在脉冲编码器的情况下，可考虑脉冲波的计数处理等。

（3）将算出的观测值Y和计测的时刻ty（快门时刻等）、观测值Y的误差分布C_ovY记录在数据存储装置116。

此外，每当观测值Y被更新时，状态推定装置114实施以下的（4）～（6）。

（4）监视数据存储装置116，如果记录有新的观测值Y，就从数据存储装置116读出Y、ty、X、tx、f、h、E。

（5）因为内部状态X示出时刻tx时的内部状态，所以，使用整体预测部115a预测时刻ty时的内部状态。

（6）使用更新部115b，根据在（5）中预测的内部状态预测时刻ty时的观测值，比较预测值与观测值Y，对内部状态X进行修正（更新）。将修正的内部状态X与新的模式时刻tx（＝ty）记录在数据存储装置116。此外，更新部115b算出内部状态X的误差分布C_ovX、观测值与预测值的偏差的大小等精度指标值E，记录在数据存储装置116。

此外，机器人控制装置120上的行动计划部123以固定的周期重复以下的（7）～（8）。

（7）使用机器人上预测部123a算出本机器人101的当前位置、未来的位置。此外，根据避免碰撞等的需要，算出其它机器人101的当前位置、未来的位置。

（8）生成像本机器人101移动到目标位置那样的目标轨道。

此外，机器人控制装置120上的车辆控制部122以固定的周期重复以下的（9）～（11）。

（9）从车轮用编码器103c求出本机器人101的当前速度。

（10）算出像沿在行动计划部123生成的目标轨道运动的那样的控制指令值。

（11）对驱动部121发送算出的控制指令值。

想要精密地控制驱动部121，需要车辆控制部122以固定周期算出控制指令值。此外，需要在行动计划部123事先生成在车辆控制部122进行参照的目标轨道。因此，行动计划部123、车辆控制部122的处理是以固定的周期重复的实时处理。

一般来说，行动计划部123的控制周期比车辆控制部122的控制周期长（根据一次生成的目标轨道的长度等）。此外，关于目标轨道，除了车辆控制部122参照的最近的移动路径以外，还有到目标地点的粗略的移动路径等。因此，像局部的路径生成、大区域的路径生成、更大区域的路径生成那样，有时也将行动计划部123进一步划分为两个以上的处理。在该情况下，生成局部的路径的处理的控制周期更短。关于大区域，即使是控制周期长的处理或周期不固定的非实时的处理也没有关系。

图9是根据本发明的第二实施方式的运动预测控制装置的第三结构例。在该例子中，机器人控制装置120还具备机器人上存储部126。

进行上述的（7）的计算时，机器人上预测部123a参照数据存储装置116中的最新的内部状态X。此时，由于需要用于通信的时间，有时在固定的时间限制下求不出预测值。因此，在该例子中，将数据存储装置116的内容逐次转发给机器人上存储部126，机器人上预测部123a参照机器人上存储部126。使进行转发的时间点为数据存储装置116的内部状态X被更新时（上述的（6）之后紧接着）。

第二机器人控制装置120B、第三机器人控制装置120C也与第一机器人控制装置120A同样地具有机器人上预测部123a和机器人上存储部126等。

另外，在该例子中，虽然机器人控制装置120为三台，但是可以是两台，也可以是四台以上。

在运动预测控制装置的第三结构例中，其它方面可以与运动预测控制装置的第一结构例或第二结构例相同。

图10是根据本发明的第二实施方式的运动预测控制装置的第四结构例。另外，在该图中，只示出了机器人控制装置120。

在上述的例子中，虽然在说明上使用了“计测装置112”的表达方式，但是计测装置112是设置在不具有移动机构的机器人控制装置120（即，机器人101）的计测装置，作为机器人控制装置120的一种来对待。

因此，本发明的第二实施方式中的系统结构做成为状态推定装置114和机器人控制装置120这两种，使机器人控制装置120（机器人101）包括不具有移动机构的装置。

数据存储装置116只要是多个装置、程序对相同的数据进行读写的共用存储器即可。因此，无需做成为独立的装置，如图10所示，只要准备在通信部125、状态推定装置114等的存储器空间上即可。

此外，状态推定装置114设置在任一个的机器人101上，如图10所示，可以是与机器人控制装置120为一体的结构。例如，能作为状态推定程序、机器人控制程序在一个PC上并行处理多个程序。

在运动预测控制装置的第四结构例中，其它方面可以与运动预测控制装置的第一结构例、第二结构例或第三结构例相同。

在上述的例子中，虽然将在机器人上预测部123a算出的预测值使用于机器人101的控制，但是，也能用于由机器人101对周围的人或物体进行计测，在机器人上预测部123a预测其位置、运动的信息而对人进行提示等提示实时地变化的信息的服务。

根据上述的本发明的第二实施方式的方法和装置，通过计测装置112、状态推定装置114及数据存储装置116在不依赖于机器人101的控制周期的任意的时间点取得对象物、多个机器人101的传感器信息，预测与取得传感器信息的时刻同时刻的各机器人101的内部状态X，将预测的内部状态与传感器信息进行比较来进行更新，存储更新的内部状态X和在预测中使用的状态变迁方程式f（X），因此，能基于对象物（例如，参照物102）或机器人101的传感器信息（例如，位置、姿势）进行各机器人101的运动预测。

此外，多个机器人控制装置120基于存储在数据存储装置116的最新的内部状态X预测各机器人101所需的预测值，因此，能在不受状态推定的计算量的影响的情况下，按每个机器人101以确定的控制周期算出控制指令值，控制多个机器人101。

即，各机器人控制装置120能在不受状态推定的更新处理所花费的时间的影响的情况下进行动作。

此外，在计测装置112或机器人控制装置120增加而使状态推定的计算量增加的情况下，各机器人控制装置120也是独立地进行预测计算，因此，预测处理所花费的时间不会变长。因此，在系统变更时无需重新考虑运算处理能力等的设计。

进而，能按每个机器人控制装置120设定预测处理的时间限制。此外，算出的预测值的精度、预测值的种类等也能按每个机器人控制装置120进行设定。因此，能考虑预测处理所花费的时间、控制周期等，在每个机器人101中实现改变预测计算的精度或者只计算内部状态X中的所需的变量的想法。

此外，数据存储装置116在不依赖于来自机器人101的要求的时间点将存储在数据存储装置116的最新的内部状态X转发给机器人上存储部126，由此，即使在机器人控制装置120与状态推定装置114之间的通信花费时间的情况下，或在通信所需时间不固定的情况下，也能立刻参照预测处理所需的数据。在该情况下，虽然在机器人上参照的数据由于通信延迟的影响而未必是最新值，但是，预测处理能在固定时间内结束。

另外，本发明的第二实施方式不限定于上述的结构例，由权利要求书的记载示出，进而包括与权利要求书的记载均等的意思及范围内的所有的变更。

[本发明的第三实施方式]

在该图中，201是对象物（工件）、202是机器人、203是机器人臂、204是机器手、205a是固定在机器手204的第一照相机、205b是固定在外部的定位置的第二照相机、210是运动预测控制装置。

该机器人系统以如下方式构成，即，用照相机205a、205b对一边进行摆运动一边移动的工件201进行计测，使机器人202跟随工件201地进行控制，通过机器手204把持工件201。

在该图中，运动预测控制装置210具备计测装置212、状态推定装置214及机器人控制装置220。状态推定装置214具备数据存储装置216。

计测装置212计测对象物201的位置和姿势。

在该例子中，计测装置212由分别与第一照相机205a和第二照相机205b连接的两台计测装置（第一计测装置212a和第二计测装置212b）构成。

即，在该例子中，分别使用两台照相机和计测装置，从第一照相机205a和第二照相机205b导入对象物201的图像，通过图像处理求出对象物201的位置和姿势。求出的对象物201的位置和姿势存储在数据存储装置216。

另外，计测装置212不限定于该结构，也可以是能用一台对对象物201的位置进行三维计测的计测装置，例如，可以使用激光雷达。

状态推定装置214具有如下功能，即，根据对对象物201进行计测的计测结果，基于状态变迁模式和观测模式，推定（更新）包括精度指标值E（后述）的对象物201的内部状态，而且根据精度指标值E判定能否把持对象物201。

在该例子中，在状态推定装置214安装有基于卡尔曼滤波器的算法的状态推定算法。

在该状态推定算法中，作为“状态变迁模式”，定义了以下的四个。

时刻t＝0时的内部状态：X₀

初始条件X₀的共离散：C_ovX₀

状态变迁方程式：X_t+△t＝f（X_t，△t）…（1）

单位时间平均的过程噪声（processnoise）（共离散）：Q。

在该算法中，将吊有对象物201的摆的摆动角θ、角速度△θ、支点位置x、y、z等状态变量综合起来设为内部状态X。在定义模式时，未必正确地知道初始条件。因此，定义推测初始条件与实物之差有多大的共离散C_ovX₀。

状态变迁方程式（1）示出当时刻经过时内部状态X是如何变化的。在该例子的情况下，根据摆的运动方程式、支点位置进行匀速直线运动等定义状态变迁方程式。

定义的状态变迁方程式（1）未必与实际的对象物201的运动一致。因此，定义共离散Q，该共离散Q示出在进行单位时间（△t＝1）的时间宽度的状态变迁计算时与实际的运动产生了哪种程度的差别。

此外，卡尔曼滤波器将在计测装置212算出的对象物201的位置和姿势设为计测值Y，并定义式（2）、（3）的“观测模式”（观测方程式和观测噪声）。

观测方程式：Y_t＝g（X_t）…（2）

观测噪声（共离散）：C_ovS_t…（3）。

观测方程式（2）是将内部状态X与计测值Y对应起来的式子。此外，观测噪声（3）是表示在时刻t时的计测中包括哪种程度的计测误差的共离散，由照相机的分辨率、视点的朝向来确定。观测噪声（3）不是固定值，由计测装置212按每个图像算出，与计测值一同交给状态推定装置214。

机器人控制装置220具有如下功能，即，在能把持的情况下，以基于内部状态的推定结果（更新结果）预测对象物201的移动目的地而进行把持动作的方式控制机器人202。

机器人控制装置220从机器人202接收指尖位置信息等，并发送指尖速度指令值、机器手开闭指令值。这些收发以固定的控制周期（4ms周期）进行。

以下，基于图12对根据本发明的第三实施方式的运动预测控制方法进行说明。

在第三实施方式的方法中，（A）开始，通过计测装置212计测对象物201的位置和姿势作为传感器信息。

（B）接下来，根据计测的计测结果，通过状态推定装置214基于状态变迁模式推定（更新）包括精度指标值E的对象物201的内部状态，而且（C）根据精度指标值E判定能否把持对象物201。

（D）接着，在能把持的情况下，通过机器人控制装置220基于内部状态的推定结果预测对象物201的移动目的地，使机器人202进行把持动作，（E）在不能把持的情况下，通过机器人控制装置220以能继续计测对象物201的位置和姿势的方式使机器人202移动。

精度指标值E由预测对象物201的位置和姿势的预测值与其实测值的误差和基于状态变迁模式和观测模式推测所述误差的离散的共离散矩阵构成。

在所述（C）中，优选是，在用所述共离散矩阵分割所述误差的马哈拉诺比斯距离(Mahalanobis'Distance)M_t的平方比第一阈值小而且共离散矩阵的行列式比第二阈值小时，判定为能把持。

第一阈值和第二阈值是预先设定的任意的阈值。

此外，在所述（C）中，也可以在用所述共离散矩阵分割所述误差的马哈拉诺比斯距离M_t的平方比第三阈值小而且共离散矩阵的迹（trace）比第四阈值小时，判定为能把持。

第三阈值和第四阈值是预先设定的任意的阈值。

以下，对第三实施方式的运动预测控制装置的动作进行说明。

（a1）计测装置212（第一计测装置212a和第二计测装置212b）在任意的时间点对两台照相机（第一照相机205a和第二照相机205b）分别发送快门信号，取得拍摄的图像。

（a2）由计测装置212根据得到的图像通过图像处理取得对象物201的位置和姿势的信息（计测值Y）。例如，提取图像内的白色区域，求出重心。

（a3）将从计测装置212发送快门信号的时刻设为ty。此外，根据照相机205a、205b的朝向、分辨率等算出观测噪声C_ovS_t。

（a4）状态推定装置214将计测值Y与通过状态变迁模式和观测模式预测的Y的预测值Ym进行比较，对内部状态X进行修正。此外，算出表示预测对象物201的位置的精度的精度指标值E。内部状态的更新在不依赖于机器人控制装置220的控制周期的任意的时间点进行。

（a5）机器人控制装置220按每个控制周期参照状态推定装置214的结果对精度指标值E进行评价，进行是否开始把持动作的判定。

（a6）在开始进行把持动作的情况下，预测对象物201的未来的位置，以预测的把持位置为目标算出指尖速度指令值。在不进行把持动作的情况下，以使照相机205a、205b能继续捕捉对象物201的方式算出指尖速度指令值。

以下对上述（a4）中的状态推定装置214的处理内容的细节进行说明。以下的处理内容是利用了卡尔曼滤波器的一般的状态推定方案。

（b1）根据计测时刻ty和当前的模式时刻t算出进行状态变迁的时间宽度△t。模式时刻t的初始值设为开始进行计测的时刻。此外，也可以将计测时刻定义为以开始时刻为基点的经过时间，将模式时刻的初始值设为0。

进行状态变迁的时间宽度△t用△t＝ty-t…（4）表示。

（b2）预测计测时刻ty的内部状态。

内部状态的预测值X_t+△t用在上述的状态变迁模式中定义的X_t+△t＝f（X_t，△t）…（1）表示。

（b3）算出计测时刻的内部状态预测值的共离散。在此，矩阵A是对状态变迁方程式f进行偏微分的矩阵。

更新（b9）前的内部状态的共离散C_ovX_t+△t｜_t用式（5）表示。

…（5）。

状态变迁方程式的偏微分矩阵A_t（△t）用式（6）表示。

…（6）。

（b4）使用内部状态预测值和观测方程式，用式（7）预测计测值Y的预测值Ym_t+Δt。

…（7）。

（b5）基于内部状态预测值的共离散，用式（8）求出预测值Ym的共离散C_ovY_t+Δt。在此，矩阵H是对观测方程式g进行偏微分的矩阵。

…（8）。

观测方程式的偏微分矩阵H_t用式（9）表示。

…（9）。

（b6）除了预测值Ym的共离散C_ovY以外，还用式（10）求出考虑了照相机的计测误差的共离散V_t+Δt。C_ovS_t+Δt是时刻ty（＝t+Δt）时的观测噪声，由计测装置212算出。

共离散V_t+Δt在预测值Ym的共离散中加上了照相机的观测噪声，因此，变成示出了预测值Ym与观测值Y的差的大小的共离散。

…（10）。

（b7）用式（11）算出卡尔曼增益（Kalmangain）K_t+Δt。

…（11）。

（b8）用式（12）、（13）更新内部状态X_t+Δt。另外，将更新前的内部状态标记为X_t+Δt｜_t。另外，Y是用传感器计测的观测值。

…（12）

…（13）。

（b9）用式（14）算出更新后的内部状态的共离散C_ovX_t+Δt。

…（14）。

通过从上述的（b1）到（b9）的处理，基于时刻ty的观测值，对内部状态进行修正（即，更新）。内部状态的更新在不依赖于机器人控制装置220的控制周期的任意的时间点进行。因此，通过系统整体重复上述（a1）～（a6）的处理，从而内部状态慢慢地接近真实值（对象物的实际速度等）。

要判断内部状态接近真实值到哪种程度，就要对用上述（b5）、（b6）求出的预测值的共离散C_ovY、V进行评价。这些共离散表示使用当前的推定结果预测对象物的位置和姿势的情况下的预测误差的大小。

在事先定义的状态变迁模式和观测模式正确的情况下，观测值与预测值之差eY成为按照共离散V的分布。因此，在实际的差eY与V相比变为大的值的情况下，就意味着事先确定的模式不正确。在这样的评价中使用式（15）所示的马哈拉诺比斯距离M_t。

…（15）。

因此，在状态推定装置214中，在从（b1）到（b9）的处理后，像以下那样算出表示当前的状态推定的精度的精度指标值E。

（b10）通过以下的（c1）、（c2）、（c3）的任一个算出精度指标值E。

（c1）…（16）

在此，m、Y、Ym、eY用相同数量的变量来表示。

（c2）…（17）

（c3）将式（16）或（17）的E对过去的全部计测结果进行总计的E。例如，根据式（18）的E。在此，p是内部状态的变量的数量。

[数学式1]

…（18）。

另外，（c1）的式（16）是正态分布的模式匹配度的评价式，（c3）的式（18）是信息量标准（AIC）。

（c2）的式（17）特征点在于，在分母中使用共离散C_ovY的迹。

在式（16）中共离散的行列式的平方根表示eY的分布的体积。另一方面，在式（17）中迹的平方根表示包住分布的最小的球的半径。

接着，在以下示出上述（a5）、（a6）中的机器人控制装置220的处理内容。

图13是根据本发明的第三实施方式的运动预测控制方法的整体流程图。如该图所示，运动预测控制方法由S1～S12的各步骤（工序）构成。

在S1中，从机器人202接收指尖位置信息等。与机器人202的通信，由机器人202以例如以4ms的控制周期进行的方式进行管理。因此，机器人控制装置220待机至接收数据为止，从接收数据起必须在例如4ms的控制周期以内结束S12的发送处理。

在S2中，判定示出把持动作过程中的指针（flag）F。指针F在程序开始时被初始化为“false”。

在S3中，在不是把持动作过程中的情况下，进行从上述（a1）到（a4）的处理。关于该处理，也可以是如下的形式，即，由其它处理系统以任意的执行周期实施，机器人控制装置220参照最新的推定结果和精度指标值E。

在S4中，根据当前的状态推定的精度指标值E判定是进行把持动作还是继续跟随动作。判定手段使用以下的（d1）～（d3）的任一种。

（d1）判定上述的（c1）的精度指标值E是否超过某个阈值（第一阈值）。在式（16）中，在马哈拉诺比斯距离M_t大的情况下，指数函数部接近于0。此外，在共离散V的体积大的情况下，分母变大。因此，只有在马哈拉诺比斯距离M_t小且共离散V的体积小时，才开始把持动作。

在像该例子那样使用两台照相机（第一照相机205a和第二照相机205b）的情况下，对象物201有可能从单方的照相机的视野脱离，发生只用一台照相机继续计测的状况。此时，因为只能用单方的视点进行计测，所以共离散C_ovY、V变成在照相机的视线方向上示出细长的分布的共离散矩阵。在（d1）的方案的情况下，即使在V细长的情况下体积也会变小，因此，存在开始把持动作的可能性。

（d2）判定上述的（c2）的精度指标值E是否超过某个阈值（第三阈值）。在C_ovY表示细长的分布的情况下，式（17）的精度指标值E变成小的值。因此，只有在两台照相机双方都捕捉到对象物201时才开始把持动作。

（d3）上述的（d1）、（d2）都用最新的计测结果来判定，但是，也可以考虑过去几个点的量的精度指标值进行判定。例如，可举出像上述的（c3）那样取过去算出的精度指标值的对数来进行总计的方法、取过去固定时间的量的平均值的方法等。

在S5中，在不进行把持动作的情况下，把持动作指针F设为“false”。

在S6中，在不进行把持动作的情况下，例如，像以下的式（19）、（20）、（21）那样设定臂速度Vx、Vy、Vz。

…（19）

…（20）

…（21）。

该控制是PD控制，mx、my、mz是所计测的对象物的当前位置[mm]、rx、ry、rz是跟随控制点（照相机的视线上的点）的当前位置[mm]、pr＿x、pr＿y、pr＿z是在上一次的步骤中算出的位置偏差（my-ry、mz-rz等）、Kp是位置控制增益、Kd是微分控制增益。

在S7中，在S4中判定为进行把持动作的情况下，使用状态变迁模式和观测模式预测未来的对象物201的位置和姿势，设为把持位置A。

在S8中，在把持动作开始时将把持动作指针F设为“true”。

在S9中，在把持动作过程中，以使机器人臂203移动到把持位置A的方式算出机器人臂203的目标速度。

在S10中，判定把持动作是否结束。例如，在机器人臂203到达把持位置A，关闭机器手204的动作结束的情况下，判定为把持动作结束。

在S11中，在结束把持动作的情况下，将把持动作指针F设为“false”。在未结束把持动作的情况下，把持动作指针F保持“true”的状态，因此，通过S2的分岔，在下次以后仍继续把持动作。

在S12中，将算出的目标速度、机器手开闭指令值发送给机器人202。

另外，在上述的例子中，虽然使用了卡尔曼滤波器作为推定对象物201的运动状态的方法，但是，只要是满足以下那样的条件的状态推定方法，也可以使用其它方法。例如，能应用粒子滤波器、最小平方法等。

（e1）要能推定对象物201的运动状态来预测未来的位置。

（e2）要能算出表示预测了未来的位置时的预测误差的离散的共离散矩阵。

在上述的c1中，虽然示出了使用了V的行列式的判定的例子，但是，无论使用V、C_ovY中的哪一个行列式都没有关系。此外，在上述的c2中，虽然示出了使用了C_ovY的迹的判定的例子，但是，无论使用V、C_ovY中的哪一个的迹都没有关系。

即，“推测了误差的离散的共离散矩阵”无论使用V、C_ovY中的哪一个都可以。

实施例1

图14是示出实施例中的经过时间与精度指标值E的关系的图。

该例子是用上述的（c2）的式（17）算出精度指标值E的结果。

根据该图可知，精度指标值E从计测开始时起慢慢地变大，在从计测开始起经过时间为两秒以后稳定在80以上。

因此，只要将式（17）的精度指标值E为80以上作为把持动作开始条件，就可以说很难把持失败。

此外，在该图中，像在经过时间为一秒附近可观察到的那样，存在精度指标值暂时变高的情况。因此，优选像上述的（c3）那样，考虑过去几个点的量的精度指标值进行判定。

例如，考虑过去几个点的平均值超过80、连续固定时间以上超过80等。

例如，优选将“精度指标值连续120ms以上超过80”作为把持动作开始条件。

根据上述的本发明的第三实施方式的装置和方法，通过计测装置212及状态推定装置214计测对象物201的位置或姿势，推定对象物201的内部状态，因此，即使是匀速直线运动、匀速圆周运动、匀加速直线运动以外的运动，也能基于对象物201（例如，工件）的内部状态（例如，位置、姿势、速度、角速度）进行对象物201的运动预测。

此外，通过状态推定装置214基于状态变迁模式和观测模式推定包括精度指标值E的对象物201的内部状态，根据精度指标值E判定能否把持对象物201，因此，在不能把持的情况下，通过机器人控制装置220例如以能继续对象物201的位置和姿势的计测的方式使机器人202移动，由此，即使在预测精度降低的情况下也能避免把持的失败于未然。

进而，在能把持的情况下，通过机器人控制装置220基于内部状态的推定结果预测对象物201的移动目的地来对机器人202进行把持动作，因此，能使机器人202跟随对象物201而可靠地进行把持。

在上述的例子中，虽然对象物201移动而机器人202被固定，但是，本发明的第三实施方式不限定于该例子，可以是对象物不动而机器人移动的情况，也可以是对象物和机器人这两者都移动的情况。

因此，本发明的第三实施方式中的“对象物的移动”指的是从机器人观察的对象物的相对的移动。

另外，本发明的第三实施方式不限定于上述的结构例，由权利要求书的记载所示出，进而包括与权利要求书的记载均等的意思及范围内的所有的变更。

附图标记说明

1：工件（对象物）；

2：机器人；

3：机器人臂；

4：机器手；

5a：第一照相机；

5b：第二照相机；

6：参照物；

10：运动预测控制装置；

12：计测装置；

12a：第一计测装置；

12b：第二计测装置；

12c：第三计测装置；

14：状态推定装置；

15a：整体预测部；

15b：更新部；

16：数据存储装置；

20：机器人控制装置；

21：接收部；

22：指令值算出部；

23：发送部；

24：机器人上预测部；

26：机器人上存储部；

101：机器人（本机器人、其它机器人）；

102：参照物；

103：内部传感器；

103a：陀螺仪；

103b：测距传感器（LRF）；

103c：车轮用编码器；

103d：照相机；

103e：GPS罗盘；

103f：操向角编码器；

104：外部传感器；

110：运动预测控制装置；

112：计测装置；

114：状态推定装置；

115a：整体预测部；

115b：更新部；

116：数据存储装置；

120：机器人控制装置；

120A：第一机器人控制装置；

120B：第二机器人控制装置；

120C：第三机器人控制装置；

121：驱动部；

122：车辆控制部；

122a：指令值算出部；

123：行动计划部；

123a：机器人上预测部；

123b：轨道生成部；

124：计测部；

125：通信部；

126：机器人上存储部；

201：工件（对象物）；

202：机器人；

203：机器人臂；

204：机器手；

205a：第一照相机；

205b：第二照相机；

210：运动预测控制装置；

212：计测装置；

212a：第一计测装置；

212b：第二计测装置；

214：状态推定装置；

216：数据存储装置；

220：机器人控制装置。

Claims

1.一种运动预测控制装置，其特征在于，具备：

计测装置，通过计测对象物和机器人的一方或双方而取得传感器信息；

状态推定装置，基于传感器信息，预测对象物和机器人的一方或双方的内部状态来进行更新；

数据存储装置，存储所述内部状态；以及

机器人控制装置，控制所述机器人，

状态推定装置在不依赖于机器人的控制周期的任意的时间点更新所述内部状态，

机器人控制装置以所述控制周期基于存储在数据存储装置的最新的内部状态算出机器人的控制所需的预测值。

2.根据权利要求1所述的运动预测控制装置，其特征在于，

计测装置计测对象物、机器人之间的相对关系作为所述传感器信息，状态推定装置预测对象物、机器人的内部状态来进行更新，

所述状态推定装置具有：

整体预测部，预测所述内部状态；以及

更新部，更新存储在数据存储装置的内部状态，

所述机器人控制装置具有机器人上预测部，以所述机器人的控制周期预测该控制所需的预测值，

3.根据权利要求2所述的运动预测控制装置，其特征在于，

所述机器人控制装置具有机器人上存储部，对存储在数据存储装置的最新的内部状态进行存储，

所述数据存储装置在不依赖于来自机器人的要求的时间点，将存储在数据存储装置的最新的内部状态转发给机器人上存储部。

4.根据权利要求2所述的运动预测控制装置，其特征在于，

所述内部状态是示出对象物、机器人的位置或姿势或者吊有对象物的摆的摆动角或摆的支点位置的状态变量。

5.根据权利要求2所述的运动预测控制装置，其特征在于，

计测的所述相对关系是对象物、机器人之间的相对位置或姿势。

6.根据权利要求2所述的运动预测控制装置，其特征在于，

计测的所述相对关系是对象物、机器人之间的相对速度或姿势变化。

7.根据权利要求1所述的运动预测控制装置，其特征在于，

计测装置在不依赖于机器人的控制周期的任意的时间点，取得对象物、多个机器人的传感器信息，

状态推定装置在取得了所述传感器信息时，预测与取得传感器信息的时刻同时刻的各机器人的内部状态，将预测的内部状态与传感器信息进行比较来进行更新，

数据存储装置在进行了所述更新时，存储更新的内部状态和在预测中使用的状态变迁方程式，

多个机器人控制装置基于存储在数据存储装置的最新的内部状态预测各机器人所需的预测值。

8.根据权利要求7所述的运动预测控制装置，其特征在于，

所述多个机器人控制装置以各机器人的控制周期预测各机器人的控制所需的预测值，实时地控制多个机器人。

9.根据权利要求7所述的运动预测控制装置，其特征在于，

状态推定装置具有：

整体预测部，预测所述内部状态；以及

更新部，对整体预测部预测的内部状态进行更新，

各机器人控制装置具有机器人上预测部，以各机器人的控制周期预测该控制所需的预测值。

10.根据权利要求7所述的运动预测控制装置，其特征在于，

各机器人控制装置具有机器人上存储部，对存储在数据存储装置的最新的内部状态进行存储，

数据存储装置在不依赖于来自机器人的要求的时间点，将存储在数据存储装置的最新的内部状态转发给机器人上存储部。

11.根据权利要求7所述的运动预测控制装置，其特征在于，

所述内部状态是示出机器人的位置、姿势、速度或角速度的状态变量。

12.根据权利要求1所述的运动预测控制装置，其特征在于，

所述机器人把持相对地移动的对象物，

计测装置计测对象物的位置或姿势作为所述传感器信息，

状态推定装置根据对对象物进行计测的计测结果，基于状态变迁模式和观测模式，更新包括精度指标值的对象物的内部状态，而且根据精度指标值判定能否把持对象物，

在能把持的情况下，机器人控制装置以基于内部状态的更新结果预测对象物的移动目的地来进行把持动作的方式控制机器人。

13.一种运动预测控制方法，其特征在于，

（A）通过用计测装置计测对象物和机器人的一方或双方而取得传感器信息，

（B）基于传感器信息，通过状态推定装置预测对象物和机器人的一方或双方的内部状态来进行更新，

（C）通过数据存储装置存储所述内部状态，

（D）通过机器人控制装置控制所述机器人，

在所述（B）中，通过状态推定装置在不依赖于机器人的控制周期的任意的时间点更新所述内部状态，

在所述（D）中，通过机器人控制装置以所述控制周期基于存储在数据存储装置的最新的内部状态，算出机器人的控制所需的预测值。

14.根据权利要求13所述的运动预测控制方法，其特征在于，

所述状态推定装置具有：

整体预测部，预测所述内部状态；以及

更新部，对存储在数据存储装置的内部状态进行更新，

在所述（A）～（C）中，在不依赖于所述控制周期的任意的时间点，通过所述计测装置、状态推定装置及数据存储装置计测对象物、机器人之间的相对关系作为所述传感器信息，预测对象物、机器人的内部状态来进行更新，存储更新的内部状态和在预测中使用的状态变迁方程式，

在所述（D）中，通过所述机器人控制装置，基于存储在数据存储装置的最新的内部状态，以所述控制周期预测所述预测值，实时地控制机器人。

15.根据权利要求13所述的运动预测控制方法，其特征在于，

在不依赖于机器人的控制周期的任意的时间点，在所述（A）中，通过计测装置取得对象物、多个机器人的传感器信息，在所述（B）中，通过状态推定装置预测与取得传感器信息的时刻同时刻的各机器人的内部状态，将预测的内部状态与传感器信息进行比较来进行更新，在所述（C）中，通过数据存储装置存储更新的内部状态和在预测中使用的状态变迁方程式，

在所述（D）中，通过多个机器人控制装置，基于存储在数据存储装置的最新的内部状态，预测各机器人所需的预测值。

16.根据权利要求15所述的运动预测控制方法，其特征在于，

在所述（D）中，以各机器人的控制周期预测各机器人的控制所需的预测值，实时地控制多个机器人。

17.根据权利要求14或15所述的运动预测控制方法，其特征在于，

所述控制周期是比计测装置的处理间隔短的固定周期。

18.根据权利要求13所述的运动预测控制方法，其特征在于，

所述机器人把持相对地移动的对象物，

在所述（A）中，通过计测装置计测对象物的位置或姿势作为所述传感器信息，

在所述（B）中，根据计测的计测结果，通过状态推定装置基于状态变迁模式和观测模式，更新包括精度指标值的对象物的内部状态，而且根据精度指标值判定能否把持对象物，

在所述（D）中，在能把持的情况下，通过机器人控制装置基于内部状态的更新结果预测对象物的移动目的地，使机器人进行把持动作，

在不能把持的情况下，重复所述（A）和（B），直到变得能把持为止。