JP6275345B1 - Conversion coefficient calculation device, conversion coefficient calculation method, and conversion coefficient calculation program - Google Patents

Abstract

変換係数算出部（１３）が、ロボットハンド（１）の位置又は姿勢が切り替えられる毎に、位置姿勢情報取得部（１１）により特定されたロボット座標系での位置とセンサ情報取得部（１２）により取得されたセンサ情報が示すセンサ座標系での位置との組を保持し、保持している複数の組から、センサ座標系での位置をロボット座標系での位置に変換する際に用いる変換係数を算出する。The position and sensor information acquisition unit (12) in the robot coordinate system specified by the position / orientation information acquisition unit (11) each time the conversion coefficient calculation unit (13) switches the position or orientation of the robot hand (1). Is used to convert a position in the sensor coordinate system to a position in the robot coordinate system from a plurality of held pairs. Calculate the coefficient.

Description

この発明は、センサ座標系での位置をロボット座標系での位置に変換する際に用いる変換係数を算出する変換係数算出装置、変換係数算出方法及び変換係数算出プログラムに関するものである。   The present invention relates to a conversion coefficient calculation device, a conversion coefficient calculation method, and a conversion coefficient calculation program for calculating a conversion coefficient used when converting a position in a sensor coordinate system to a position in a robot coordinate system.

ロボットシステムは、例えば、加工対象等の物体を把持するロボットハンドが取り付けられているロボットアームなどを備えているシステムである。
また、ロボットシステムは、物体及びロボットハンドの周辺を計測するセンサを備えており、センサにより計測されたセンサ情報を用いて、ロボットアーム及びロボットハンドの位置及び姿勢を制御するようにしている。The robot system is, for example, a system including a robot arm to which a robot hand that holds an object such as a processing target is attached.
The robot system also includes a sensor that measures the periphery of the object and the robot hand, and controls the position and orientation of the robot arm and the robot hand using sensor information measured by the sensor.

ただし、センサにより計測されたセンサ情報は、物体等のセンサ座標系での３次元位置を示す情報である。このため、ロボットシステムが、ロボットアーム等の位置及び姿勢の制御を行う際、センサ情報を用いることができるようにするには、センサ情報が示す３次元位置をセンサ座標系からロボット座標系に変換する必要がある。
センサ座標系は、センサを中心とする座標系であり、ロボット座標系は、ロボットを中心とするロボット座標系である。However, the sensor information measured by the sensor is information indicating a three-dimensional position of the object or the like in the sensor coordinate system. Therefore, in order for the robot system to be able to use sensor information when controlling the position and orientation of the robot arm or the like, the three-dimensional position indicated by the sensor information is converted from the sensor coordinate system to the robot coordinate system. There is a need to.
The sensor coordinate system is a coordinate system centered on the sensor, and the robot coordinate system is a robot coordinate system centered on the robot.

以下の特許文献１には、ロボットハンドが画像認識用のマーカを把持し、センサであるカメラがマーカを撮影することで、座標の変換係数を算出する変換係数算出装置が開示されている。
この変換係数算出装置は、カメラにより撮影された映像内のマーカのセンサ座標系（カメラ座標系）での位置と、ロボットハンドにより把持されているマーカのロボット座標系での位置とから、センサであるカメラとロボットとの相対的な位置関係を示すパラメータを算出する。
センサであるカメラとロボットとの相対的な位置関係を示すパラメータは、センサ座標系での３次元位置をロボット座標系での３次元位置に変換する際に用いる座標の変換係数に相当する。
この変換係数算出装置は、カメラとロボットとの相対的な位置関係を示すパラメータを算出すると、以降、相対的な位置関係を示すパラメータを用いて、センサにより計測されたセンサ情報が示す物体等の３次元位置をロボット座標系の３次元位置に変換する。Patent Document 1 below discloses a conversion coefficient calculation apparatus that calculates a conversion coefficient of coordinates by a robot hand holding a marker for image recognition and a camera as a sensor photographing the marker.
This conversion coefficient calculation device uses a sensor based on the position of a marker in an image captured by a camera in the sensor coordinate system (camera coordinate system) and the position of a marker held by a robot hand in the robot coordinate system. A parameter indicating a relative positional relationship between a certain camera and a robot is calculated.
The parameter indicating the relative positional relationship between the camera as a sensor and the robot corresponds to a coordinate conversion coefficient used when converting a three-dimensional position in the sensor coordinate system to a three-dimensional position in the robot coordinate system.
After calculating the parameter indicating the relative positional relationship between the camera and the robot, the conversion coefficient calculating apparatus uses the parameter indicating the relative positional relationship to calculate the object or the like indicated by the sensor information measured by the sensor. The three-dimensional position is converted into a three-dimensional position in the robot coordinate system.

特開２０１４−１８０７２０号公報JP 2014-180720 A

従来の変換係数算出装置は以上のように構成されているので、センサがカメラであり、ロボットハンドがマーカを把持するようにすれば、センサであるカメラとロボットとの相対的な位置関係を示すパラメータを算出することができる。しかし、センサが、例えば、レーザスキャナなどの３次元センサである場合、マーカを撮影することができないため、センサとロボットとの相対的な位置関係を示すパラメータを算出することができないという課題があった。   Since the conventional conversion coefficient calculation apparatus is configured as described above, if the sensor is a camera and the robot hand grasps the marker, it indicates the relative positional relationship between the sensor camera and the robot. Parameters can be calculated. However, when the sensor is a three-dimensional sensor such as a laser scanner, for example, there is a problem that a parameter indicating the relative positional relationship between the sensor and the robot cannot be calculated because the marker cannot be photographed. It was.

この発明は上記のような課題を解決するためになされたもので、センサがレーザスキャナなどの３次元センサであっても、センサ座標系での位置をロボット座標系での位置に変換する際に用いる変換係数を算出することができる変換係数算出装置、変換係数算出方法及び変換係数算出プログラムを得ることを目的とする。   The present invention has been made to solve the above-described problem. Even when the sensor is a three-dimensional sensor such as a laser scanner, the position in the sensor coordinate system is converted to the position in the robot coordinate system. It is an object to obtain a conversion coefficient calculation device, a conversion coefficient calculation method, and a conversion coefficient calculation program that can calculate a conversion coefficient to be used.

この発明に係る変換係数算出装置は、物体が固定されているロボットハンドの位置及び姿勢を示す位置姿勢情報を取得し、位置姿勢情報から物体のロボット座標系での位置又は物体のロボット座標系での平面のパラメータである平面パラメータを特定する位置姿勢情報取得部と、ロボットハンドに固定されている物体を計測するセンサから、物体のセンサ座標系での位置を示すセンサ情報又は物体のセンサ座標系での平面のパラメータである平面パラメータを示すセンサ情報を取得するセンサ情報取得部とを設け、変換係数算出部が、ロボットハンドの位置又は姿勢が切り替えられる毎に、位置姿勢情報取得部により特定されたロボット座標系での位置又は平面パラメータとセンサ情報取得部により取得されたセンサ情報が示すセンサ座標系での位置又は平面パラメータとの組を保持し、保持している複数の組から、センサ座標系での位置をロボット座標系での位置に変換する際に用いる変換係数を算出し、センサ情報取得部により取得されたセンサ情報が、物体の表面における複数の位置又は複数の平面パラメータを示す３次元点群データであれば、物体の形状を示す形状データを用いて、３次元点群データから物体のセンサ座標系での位置又は平面パラメータを算出するようにしたものである。 The conversion coefficient calculation apparatus according to the present invention acquires position and orientation information indicating the position and orientation of a robot hand to which an object is fixed, and uses the position and orientation information of the object in the robot coordinate system or the object in the robot coordinate system. Sensor information indicating the position of the object in the sensor coordinate system or the sensor coordinate system of the object from the position and orientation information acquisition unit for specifying the plane parameter that is a plane parameter of the sensor and the sensor for measuring the object fixed to the robot hand And a sensor information acquisition unit that acquires sensor information indicating a plane parameter that is a plane parameter at each time, and the conversion coefficient calculation unit is specified by the position and orientation information acquisition unit every time the position or orientation of the robot hand is switched. Sensor coordinate system indicated by the sensor information acquired by the position or plane parameter in the robot coordinate system and the sensor information acquisition unit And a conversion coefficient used when converting a position in the sensor coordinate system to a position in the robot coordinate system from a plurality of held sets, and a sensor information acquisition unit Is the 3D point cloud data indicating a plurality of positions or a plurality of plane parameters on the surface of the object, the shape information indicating the shape of the object is used to obtain the object information from the 3D point cloud data. A position or plane parameter in the sensor coordinate system is calculated.

この発明によれば、変換係数算出部が、ロボットハンドの位置又は姿勢が切り替えられる毎に、位置姿勢情報取得部により特定されたロボット座標系での位置又は平面パラメータとセンサ情報取得部により取得されたセンサ情報が示すセンサ座標系での位置又は平面パラメータとの組を保持し、保持している複数の組から、センサ座標系での位置をロボット座標系での位置に変換する際に用いる変換係数を算出し、センサ情報取得部により取得されたセンサ情報が、物体の表面における複数の位置又は複数の平面パラメータを示す３次元点群データであれば、物体の形状を示す形状データを用いて、３次元点群データから物体のセンサ座標系での位置又は平面パラメータを算出するように構成したので、センサがレーザスキャナなどの３次元センサであっても、センサ座標系での位置をロボット座標系での位置に変換する際に用いる変換係数を算出することができる効果がある。 According to this invention, every time the position or orientation of the robot hand is switched, the conversion coefficient calculation unit is acquired by the position or plane parameter in the robot coordinate system specified by the position and orientation information acquisition unit and the sensor information acquisition unit. This is a conversion that is used when converting the position in the sensor coordinate system to the position in the robot coordinate system from the plurality of stored groups. If the sensor information obtained by calculating the coefficient and the sensor information acquisition unit is three-dimensional point cloud data indicating a plurality of positions or a plurality of plane parameters on the surface of the object, the shape data indicating the shape of the object is used. , since it is configured so as to calculate the position or plane parameters of the object sensor coordinate system from 3D point group data, three-dimensional sensor such as a laser scanner Even capacitors, there is an effect that it is possible to calculate the transform coefficients to be used for converting the position of the sensor coordinate system to the position in the robot coordinate system.

この発明の実施の形態１による変換係数算出装置４を実装しているロボットシステムを示す構成図である。It is a block diagram which shows the robot system which mounts the conversion coefficient calculation apparatus 4 by Embodiment 1 of this invention. この発明の実施の形態１による変換係数算出装置４を示すハードウェア構成図である。It is a hardware block diagram which shows the conversion coefficient calculation apparatus 4 by Embodiment 1 of this invention. 変換係数算出装置４がソフトウェア又はファームウェアなどで実現される場合のコンピュータのハードウェア構成図である。It is a hardware block diagram of a computer in case the conversion coefficient calculation apparatus 4 is implement | achieved by software or firmware. 変換係数算出装置４がソフトウェア又はファームウェアなどで実現される場合の処理手順に相当する変換係数算出方法を示すフローチャートである。It is a flowchart which shows the conversion coefficient calculation method equivalent to the process sequence in case the conversion coefficient calculation apparatus 4 is implement | achieved by software or firmware. この発明の実施の形態２による変換係数算出装置６を実装しているロボットシステムを示す構成図である。It is a block diagram which shows the robot system which mounts the conversion coefficient calculation apparatus 6 by Embodiment 2 of this invention. この発明の実施の形態２による変換係数算出装置６を示すハードウェア構成図である。It is a hardware block diagram which shows the conversion coefficient calculation apparatus 6 by Embodiment 2 of this invention. 変換係数算出装置６がソフトウェア又はファームウェアなどで実現される場合の処理手順に相当する変換係数算出方法を示すフローチャートである。It is a flowchart which shows the conversion coefficient calculation method corresponded to the process sequence in case the conversion coefficient calculation apparatus 6 is implement | achieved by software or firmware. この発明の実施の形態４による変換係数算出装置６を実装しているロボットシステムを示す構成図である。It is a block diagram which shows the robot system which mounts the conversion coefficient calculation apparatus 6 by Embodiment 4 of this invention. この発明の実施の形態４による変換係数算出装置６を示すハードウェア構成図である。It is a hardware block diagram which shows the conversion coefficient calculation apparatus 6 by Embodiment 4 of this invention.

以下、この発明をより詳細に説明するために、この発明を実施するための形態について、添付の図面に従って説明する。   Hereinafter, in order to explain the present invention in more detail, modes for carrying out the present invention will be described with reference to the accompanying drawings.

実施の形態１．
図１はこの発明の実施の形態１による変換係数算出装置４を実装しているロボットシステムを示す構成図である。
図１において、ロボットハンド１はロボットアームの先端等に取り付けられており、例えば、加工対象等の物体１０が固定されている。
ロボットアームは、少なくとも１つ以上の関節を備えており、１つ以上の関節が制御されることで、ロボットハンド１の位置又は姿勢が切り替えられる。
ロボットハンド１は、物体１０を把持する機能を有している。ただし、ロボットハンド１は、物体１０を固定することができればよく、物体１０を把持する方式は問わない。例えば、多数の指を開閉することで物体１０を挟み込む方式、磁力又は吸引によって物体１０を把持する方式などがある。Embodiment 1 FIG.
FIG. 1 is a block diagram showing a robot system in which a conversion coefficient calculation device 4 according to Embodiment 1 of the present invention is mounted.
In FIG. 1, a robot hand 1 is attached to the tip of a robot arm and the like, for example, an object 10 such as a processing target is fixed.
The robot arm includes at least one joint, and the position or posture of the robot hand 1 is switched by controlling the one or more joints.
The robot hand 1 has a function of gripping the object 10. However, the robot hand 1 only needs to be able to fix the object 10, and any method for gripping the object 10 may be used. For example, there are a method of sandwiching the object 10 by opening and closing many fingers, a method of gripping the object 10 by magnetic force or suction, and the like.

ロボット制御部２はロボットアーム又はロボットハンド１の関節を制御することで、ロボットハンド１の位置又は姿勢を切り替える制御装置である。ロボットハンド１の姿勢は、例えば、ロボットが置かれている面に対する角度などが該当する。
センサ３はロボットハンド１により把持されている物体１０を計測するレーザスキャナなどの３次元センサであり、物体１０の計測結果を示すセンサ情報を出力する。
センサ３から出力されるセンサ情報は、空間中の物体１０の表面の３次元座標（Ｘ, Ｙ, Ｚ）の集合である。センサ３による物体１０の計測方法として、ステレオビジョンに基づく方法、Ｔｉｍｅ−ｏｆ−Ｆｌｉｇｈｔ法などが知られている。The robot control unit 2 is a control device that switches the position or posture of the robot hand 1 by controlling the joints of the robot arm or the robot hand 1. The posture of the robot hand 1 corresponds to, for example, an angle with respect to the surface on which the robot is placed.
The sensor 3 is a three-dimensional sensor such as a laser scanner that measures the object 10 held by the robot hand 1, and outputs sensor information indicating the measurement result of the object 10.
The sensor information output from the sensor 3 is a set of three-dimensional coordinates (X, Y, Z) of the surface of the object 10 in the space. As a method for measuring the object 10 by the sensor 3, a method based on stereo vision, a Time-of-Flight method, and the like are known.

変換係数算出装置４は、位置姿勢情報取得部１１、センサ情報取得部１２、変換係数算出部１３及び座標変換部１４を備えている。
図２はこの発明の実施の形態１による変換係数算出装置４を示すハードウェア構成図である。
位置姿勢情報取得部１１は例えば図２に示す位置姿勢情報取得回路２１で実現されるものであり、ロボットハンド１の位置及び姿勢を示す位置姿勢情報を取得し、位置姿勢情報から物体１０のロボット座標系での３次元位置を特定する処理を実施する。
センサ情報取得部１２は例えば図２に示すセンサ情報取得回路２２で実現されるものであり、センサ３から物体１０のセンサ座標系での３次元位置を示すセンサ情報を取得する処理を実施する。
なお、センサ座標系は、センサ３を中心とする座標系であり、ロボット座標系は、ロボットハンド１を中心とするロボット座標系である。The conversion coefficient calculation device 4 includes a position / orientation information acquisition unit 11, a sensor information acquisition unit 12, a conversion coefficient calculation unit 13, and a coordinate conversion unit 14.
FIG. 2 is a hardware configuration diagram showing the conversion coefficient calculation device 4 according to Embodiment 1 of the present invention.
The position / orientation information acquisition unit 11 is realized by, for example, the position / orientation information acquisition circuit 21 illustrated in FIG. 2, acquires position / orientation information indicating the position and orientation of the robot hand 1, and the robot of the object 10 from the position / orientation information. A process of specifying a three-dimensional position in the coordinate system is performed.
The sensor information acquisition unit 12 is realized by, for example, the sensor information acquisition circuit 22 illustrated in FIG. 2, and performs a process of acquiring sensor information indicating a three-dimensional position of the object 10 in the sensor coordinate system from the sensor 3.
The sensor coordinate system is a coordinate system centered on the sensor 3, and the robot coordinate system is a robot coordinate system centered on the robot hand 1.

変換係数算出部１３は例えば図２に示す変換係数算出回路２３で実現される。
変換係数算出部１３はロボット制御部２によりロボットハンド１の位置又は姿勢が切り替えられる毎に、位置姿勢情報取得部１１により特定されたロボット座標系での３次元位置とセンサ情報取得部１２により取得されたセンサ情報が示すセンサ座標系での３次元位置との組を保持する記憶部１３ａを備えている。
変換係数算出部１３は、記憶部１３ａに保持されている複数の組から、センサ座標系での３次元位置をロボット座標系での３次元位置に変換する際に用いる変換係数として、剛体変換における回転行列Ｒ及び並進ベクトルｔを算出する処理を実施する。
また、変換係数算出部１３は、ロボット座標系での３次元位置とセンサ座標系での３次元位置との組を保持する毎に、ロボットハンド１の位置又は姿勢の切り替えを指示する制御信号をロボット制御部２に出力する処理を実施する。The conversion coefficient calculation unit 13 is realized by, for example, a conversion coefficient calculation circuit 23 illustrated in FIG.
The conversion coefficient calculation unit 13 is acquired by the three-dimensional position and sensor information acquisition unit 12 in the robot coordinate system specified by the position / orientation information acquisition unit 11 every time the position or posture of the robot hand 1 is switched by the robot control unit 2. The storage unit 13a holds a set with a three-dimensional position in the sensor coordinate system indicated by the sensor information.
The conversion coefficient calculation unit 13 uses, as a conversion coefficient used when converting a three-dimensional position in the sensor coordinate system into a three-dimensional position in the robot coordinate system from a plurality of sets held in the storage unit 13a. A process of calculating the rotation matrix R and the translation vector t is performed.
In addition, every time the conversion coefficient calculation unit 13 holds a set of a three-dimensional position in the robot coordinate system and a three-dimensional position in the sensor coordinate system, the conversion coefficient calculation unit 13 sends a control signal instructing switching of the position or orientation of the robot hand 1. A process of outputting to the robot control unit 2 is performed.

座標変換部１４は例えば図２に示す座標変換回路２４で実現される。
座標変換部１４は変換係数算出部１３により算出された変換係数である回転行列Ｒ及び並進ベクトルｔを用いて、センサ情報取得部１２により取得されたセンサ情報が示すセンサ座標系での３次元位置をロボット座標系での３次元位置に変換する処理を実施する。The coordinate conversion unit 14 is realized by, for example, a coordinate conversion circuit 24 shown in FIG.
The coordinate conversion unit 14 uses the rotation matrix R and the translation vector t that are the conversion coefficients calculated by the conversion coefficient calculation unit 13 to use the three-dimensional position in the sensor coordinate system indicated by the sensor information acquired by the sensor information acquisition unit 12. Is converted into a three-dimensional position in the robot coordinate system.

図１では、変換係数算出装置４の構成要素である位置姿勢情報取得部１１、センサ情報取得部１２、変換係数算出部１３及び座標変換部１４のそれぞれが、図２に示すような専用のハードウェア、即ち、位置姿勢情報取得回路２１、センサ情報取得回路２２、変換係数算出回路２３及び座標変換回路２４で実現されるものを想定している。
ここで、変換係数算出部１３が備えている記憶部１３ａは、例えば記憶処理回路で実現される。
この記憶処理回路は、例えば、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、フラッシュメモリ、ＥＰＲＯＭ（Ｅｒａｓａｂｌｅ Ｐｒｏｇｒａｍｍａｂｌｅ Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、ＥＥＰＲＯＭ（Ｅｌｅｃｔｒｉｃａｌｌｙ Ｅｒａｓａｂｌｅ Ｐｒｏｇｒａｍｍａｂｌｅ Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒy）などの不揮発性又は揮発性の半導体メモリや、磁気ディスク、フレキシブルディスク、光ディスク、コンパクトディスク、ミニディスク、ＤＶＤ（Ｄｉｇｉｔａｌ Ｖｅｒｓａｔｉｌｅ Ｄｉｓｃ）などが該当する。
また、位置姿勢情報取得回路２１、センサ情報取得回路２２、変換係数算出回路２３及び座標変換回路２４は、例えば、単一回路、複合回路、プログラム化したプロセッサ、並列プログラム化したプロセッサ、ＡＳＩＣ（Ａｐｐｌｉｃａｔｉｏｎ Ｓｐｅｃｉｆｉｃ Ｉｎｔｅｇｒａｔｅｄ Ｃｉｒｃｕｉｔ）、ＦＰＧＡ（Ｆｉｅｌｄ−Ｐｒｏｇｒａｍｍａｂｌｅ Ｇａｔｅ Ａｒｒａｙ）、または、これらを組み合わせたものが該当する。In FIG. 1, each of the position / orientation information acquisition unit 11, the sensor information acquisition unit 12, the conversion coefficient calculation unit 13, and the coordinate conversion unit 14, which is a component of the conversion coefficient calculation device 4, has dedicated hardware as illustrated in FIG. 2. Hardware, that is, a position and orientation information acquisition circuit 21, a sensor information acquisition circuit 22, a conversion coefficient calculation circuit 23, and a coordinate conversion circuit 24 are assumed.
Here, the storage unit 13a included in the conversion coefficient calculation unit 13 is realized by, for example, a storage processing circuit.
This memory processing circuit is, for example, a RAM (Random Access Memory), a ROM (Read Only Memory), a flash memory, an EPROM (Erasable Programmable Read Only Memory), or an EEPROM (Electrically Erasable Programmable Memory). And a semiconductor disk, magnetic disk, flexible disk, optical disk, compact disk, mini disk, DVD (Digital Versatile Disc), and the like.
The position / orientation information acquisition circuit 21, the sensor information acquisition circuit 22, the conversion coefficient calculation circuit 23, and the coordinate conversion circuit 24 are, for example, a single circuit, a composite circuit, a programmed processor, a parallel programmed processor, an ASIC (Application), or the like. A specific integrated circuit (FPGA), a field-programmable gate array (FPGA), or a combination thereof is applicable.

変換係数算出装置４の構成要素は、専用のハードウェアで実現されるものに限るものではなく、変換係数算出装置４がソフトウェア、ファームウェア、または、ソフトウェアとファームウェアとの組み合わせで実現されるものであってもよい。
ソフトウェア又はファームウェアはプログラムとして、コンピュータのメモリに格納される。コンピュータは、プログラムを実行するハードウェアを意味し、例えば、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）、中央処理装置、処理装置、演算装置、マイクロプロセッサ、マイクロコンピュータ、プロセッサ、ＤＳＰ（Ｄｉｇｉｔａｌ Ｓｉｇｎａｌ Ｐｒｏｃｅｓｓｏｒ）などが該当する。
図３は変換係数算出装置４がソフトウェア又はファームウェアなどで実現される場合のコンピュータのハードウェア構成図である。
変換係数算出装置４がソフトウェア又はファームウェアなどで実現される場合、変換係数算出部１３が備えている記憶部１３ａをコンピュータのメモリ３１上に構成するとともに、位置姿勢情報取得部１１の位置姿勢情報取得処理手順、センサ情報取得部１２のセンサ情報取得処理手順、変換係数算出部１３の変換係数算出処理手順及び座標変換部１４の座標変換処理手順をコンピュータに実行させるための変換係数算出プログラムをメモリ３１に格納し、コンピュータのプロセッサ３２がメモリ３１に格納されているプログラムを実行するようにすればよい。The components of the conversion coefficient calculation device 4 are not limited to those realized by dedicated hardware, but the conversion coefficient calculation device 4 is realized by software, firmware, or a combination of software and firmware. May be.
Software or firmware is stored as a program in the memory of a computer. The computer means hardware that executes a program, and includes, for example, a CPU (Central Processing Unit), a central processing unit, a processing unit, an arithmetic unit, a microprocessor, a microcomputer, a processor, a DSP (Digital Signal Processor), and the like. .
FIG. 3 is a hardware configuration diagram of a computer when the conversion coefficient calculation device 4 is realized by software or firmware.
When the conversion coefficient calculation device 4 is realized by software, firmware, or the like, the storage unit 13a included in the conversion coefficient calculation unit 13 is configured on the memory 31 of the computer and the position / orientation information acquisition unit 11 acquires the position / orientation information. A memory 31 stores a conversion coefficient calculation program for causing a computer to execute the processing procedure, the sensor information acquisition processing procedure of the sensor information acquisition unit 12, the conversion coefficient calculation processing procedure of the conversion coefficient calculation unit 13, and the coordinate conversion processing procedure of the coordinate conversion unit 14. And the computer processor 32 may execute the program stored in the memory 31.

図３において、通信回路３３はロボットハンド１から位置姿勢情報を受信するとともに、センサ３からセンサ情報を受信し、また、制御信号をロボット制御部２に出力する回路である。
表示装置３４は例えば液晶ディスプレイで実現されるものであり、プロセッサ３２の処理の様子などを表示する。
図４は変換係数算出装置４がソフトウェア又はファームウェアなどで実現される場合の処理手順に相当する変換係数算出方法を示すフローチャートである。In FIG. 3, a communication circuit 33 is a circuit that receives position and orientation information from the robot hand 1, receives sensor information from the sensor 3, and outputs a control signal to the robot control unit 2.
The display device 34 is realized by, for example, a liquid crystal display, and displays the processing state of the processor 32 and the like.
FIG. 4 is a flowchart showing a conversion coefficient calculation method corresponding to a processing procedure when the conversion coefficient calculation device 4 is realized by software or firmware.

次に動作について説明する。
ロボットハンド１は、物体１０を把持している。この実施の形態１では、一例として、球体の物体１０を把持しているものとする。
ロボット制御部２は、ロボットアーム又はロボットハンド１の関節を制御することで、ロボットハンド１の位置又は姿勢を切り替える。
この実施の形態１では、ロボット制御部２が、変換係数算出部１３から出力される制御信号に従ってロボットアーム又はロボットハンド１の関節の角度を変えることで、ロボットハンド１の位置又は姿勢を切り替えるものとする。
また、この実施の形態１では、説明の便宜上、Ｎ（Ｎは３以上の整数）回、ロボットアーム又はロボットハンド１の関節を制御するものとする。Next, the operation will be described.
The robot hand 1 holds the object 10. In the first embodiment, as an example, it is assumed that a spherical object 10 is held.
The robot control unit 2 switches the position or posture of the robot hand 1 by controlling the joints of the robot arm or the robot hand 1.
In the first embodiment, the robot control unit 2 switches the position or posture of the robot hand 1 by changing the angle of the joint of the robot arm or the robot hand 1 according to the control signal output from the conversion coefficient calculation unit 13. And
In the first embodiment, for convenience of explanation, it is assumed that the robot arm or the joint of the robot hand 1 is controlled N (N is an integer of 3 or more) times.

ロボットハンド１により把持されている物体１０は、ロボット制御部２によってロボットアーム又はロボットハンド１の関節が制御されることで、ｎ（ｎ＝１，２，・・・，Ｎ）回目の制御では、ロボット座標系でＰ_ｒ ^ｎの位置に移動される（図４のステップＳＴ１）。この段階では、ｎ＝１であるため、ロボット座標系でＰ_ｒ ^１の位置に移動される。
位置Ｐ_ｒ ^ｎは、以下の式（１）に示すように、ロボット座標系のＸ軸における座標Ｘ_ｒ、Ｙ軸における座標Ｙ_ｒ、Ｚ軸における座標Ｚ_ｒで表される。

式（１）において、Ｔは転置を表す記号であり、例えば、ｘ^Ｔはベクトルｘの転置である。The object 10 held by the robot hand 1 is controlled by the robot control unit 2 by controlling the robot arm or the joint of the robot hand 1, so that the n (n = 1, 2,..., N) -th control is performed. It is moved to the position of _P ^{r n} in the robot coordinate system (step ST1 in FIG. 4). At this stage, since n = 1, it is moved to the position of P _r ^{1 in} the robot coordinate system.
The position P _r ⁿ is represented by a coordinate X _{r on} the X axis, a coordinate Y _{r on} the Y axis, and a coordinate Z _{r on the} Z axis in the robot coordinate system, as shown in the following formula (1).

In the formula (1), T is the symbol denoting a transposed, for example, x ^T is the transpose of vector x.

位置姿勢情報取得部１１は、ロボットハンド１の位置及び姿勢を示す位置姿勢情報を取得し、位置姿勢情報から物体１０のロボット座標系での３次元位置Ｐ_ｒ ^ｎを特定する（図４のステップＳＴ２）。
位置姿勢情報からロボット座標系での３次元位置Ｐ_ｒ ^ｎを特定する処理は公知の技術であるため詳細な説明を省略するが、例えば、以下に示すような処理で３次元位置Ｐ_ｒ ^ｎを特定することができる。
例えば、ロボットハンド１の位置及び姿勢と、ロボットハンド１により把持されている物体１０の３次元位置Ｐ_ｒ ^ｎとの対応関係を示すテーブルを事前に用意し、位置姿勢情報取得部１１が、テーブルを参照することで、位置姿勢情報が示す位置及び姿勢に対応する３次元位置Ｐ_ｒ ^ｎを特定する。Position and orientation information acquisition unit 11, the step of acquiring position and orientation information indicating the position and orientation of the robot hand 1, identifies a three-dimensional position P _r ⁿ in the robot coordinate system of the object 10 from the position and orientation information (FIG. 4 ST2).
Processing of the position and orientation information for specifying the three-dimensional position P _r ⁿ in the robot coordinate system and a detailed description thereof will be omitted because it is a known technique, for example, a three-dimensional position P _r ⁿ in the process as shown below Can be identified.
For example, the position and orientation of the robot hand 1, and preparing a table showing the correspondence between the three-dimensional position P _r ⁿ of the object 10 which is gripped by the robot hand 1 in advance, the position and orientation information acquisition unit 11, the table by reference to the, to identify the three-dimensional position P _r ⁿ corresponding to the position and orientation indicated by the position and orientation information.

センサ３は、ロボット制御部２によって、ロボット座標系でＰ_ｒ ^ｎ（ｎ＝１，２，・・・，Ｎ）の位置に移動された物体１０を計測し、物体１０の計測結果として、物体１０のセンサ座標系での３次元位置Ｐ_ｓ ^ｎ（ｎ＝１，２，・・・，Ｎ）を示すセンサ情報を出力する。

この実施の形態１では、センサ３から出力されるセンサ情報は、物体１０の表面におけるＭ点の位置を示す３次元点群データｐ_１ ^ｎ，ｐ_２ ^ｎ，・・・，ｐ_Ｍ ^ｎである。Sensor 3, the robot control unit 2, _P ^r n in the robot coordinate system (n = 1,2, ···, N ) measures the object 10 which has been moved to the position of the measurement result of the object 10, the object Sensor information indicating a three-dimensional position P _s ⁿ (n = 1, 2,..., N) in 10 sensor coordinate systems is output.

In the first embodiment, the sensor information output from the sensor 3 is three-dimensional point group data p ₁ ⁿ , p ₂ ⁿ ,..., P _M ⁿ indicating the position of the M point on the surface of the object 10. .

センサ情報取得部１２は、センサ３から出力されたセンサ情報を取得し、センサ情報から物体１０のセンサ座標系での３次元位置Ｐ_ｓ ^ｎ（ｎ＝１，２，・・・，Ｎ）を特定する（図４のステップＳＴ３）。
Ｍは１以上の整数であり、Ｍ＝１の場合、Ｐ_ｓ ^ｎ＝ｐ_１ ^ｎである。
Ｍ≧２の場合、センサ情報取得部１２は、物体１０の形状を示す形状データを用いて、３次元点群データｐ_１ ^ｎ，ｐ_２ ^ｎ，・・・，ｐ_Ｍ ^ｎから物体１０のセンサ座標系での３次元位置Ｐ_ｓ ^ｎを算出する。
この実施の形態１では、ロボットハンド１が球体の物体１０を把持しているため、３次元点群データｐ_１ ^ｎ，ｐ_２ ^ｎ，・・・，ｐ_Ｍ ^ｎは、球体の表面上のいずれかの点である。
このため、センサ情報取得部１２は、物体１０の形状を示す形状データとして、球体の形状を表す球の方程式を使用し、３次元点群データｐ_１ ^ｎ，ｐ_２ ^ｎ，・・・，ｐ_Ｍ ^ｎに球の方程式をフィッティングすることで、物体１０の中心の位置（Ｘ_０，Ｙ_０，Ｚ_０）として、３次元位置Ｐ_ｓ ^ｎを得ることができる。The sensor information acquisition unit 12 acquires sensor information output from the sensor 3, three-dimensional position of the sensor coordinate system of the object 10 from the sensor information ^{_{P s n (n = 1,2,}} ···, N) and It is specified (step ST3 in FIG. 4).
M is an integer greater than or equal to 1, and when M = 1, P _s ⁿ = p ₁ ⁿ .
For M ≧ 2, the sensor information acquisition unit 12 uses the shape data indicating the shape of the object 10, 3D point group data ^{_{p 1 n, p 2 n,}} ···, sensors of the object 10 from _p ^{M n} A three-dimensional position P _s ⁿ in the coordinate system is calculated.
In the first embodiment, since the robot hand 1 holds the spherical object 10, the three-dimensional point cloud data p ₁ ⁿ , p ₂ ⁿ ,..., P _M ⁿ are stored on the surface of the sphere. That is the point.
For this reason, the sensor information acquisition part 12 uses the equation of the sphere showing the shape of the sphere as the shape data indicating the shape of the object 10, and uses the three-dimensional point cloud data p ₁ ⁿ , p ₂ ⁿ ,. By fitting a sphere equation to _M ⁿ , the three-dimensional position P _s ⁿ can be obtained as the center position (X ₀ , Y ₀ , Z ₀ ) of the object 10.

３次元点群データｐ_１ ^ｎ，ｐ_２ ^ｎ，・・・，ｐ_Ｍ ^ｎに球の方程式をフィッティングする方法としては、複数の方法が考えられるが、ここでは、一例として、最小二乗法によるフィッティング方法を説明する。
まず、球の方程式は、以下の式（３）で表されるものとする。

式（３）において、ｒは球体の半径である。
このとき、Ｍ点の３次元点群データｐ_１ ^ｎ，ｐ_２ ^ｎ，・・・，ｐ_Ｍ ^ｎの二乗誤差は、以下の式（４）で表される。

式（４）において、||ｘ||はベクトルｘのノルムを表す記号である。
センサ情報取得部１２は、式（４）で表される二乗誤差が最小となるＰ_ｓ ^ｎをセンサ座標系での３次元位置Ｐ_ｓ ^ｎとして算出する。As a method of fitting the spherical equation to the three-dimensional point cloud data p ₁ ⁿ , p ₂ ⁿ ,..., P _M ⁿ , a plurality of methods can be considered, but here, as an example, fitting by the least square method A method will be described.
First, the equation of the sphere is represented by the following formula (3).

In equation (3), r is the radius of the sphere.
At this time, the square error of M point three-dimensional point group data p ₁ ⁿ , p ₂ ⁿ ,..., P _M ⁿ is expressed by the following equation (4).

In Equation (4), || x || is a symbol representing the norm of the vector x.
The sensor information acquisition unit 12 calculates the P _s ⁿ squared error expressed by the formula (4) is minimum as the three-dimensional position P _s ⁿ of the sensor coordinate system.

式（４）で表される二乗誤差が最小となるＰ_ｓ ^ｎを算出する方法も複数存在するが、例えば、非特許文献１に開示されている「Direct Least Squares Algorithm」などの方法を用いることができる。
この実施の形態１では、球体が真球である例を説明しているが、物体１０が真球ではない楕円体であってもよく、この場合は、楕円体の形状を表す方程式を使用するようにすればよい。
［非特許文献１］
David Eberly著、“Least Squares Fitting of Data,”1999年7月15日発行、https://www.geometrictools.com/Documentation/LeastSquaresFitting.pdfThere are a plurality of methods for calculating P _s ⁿ that minimizes the square error represented by Equation (4). For example, a method such as “Direct Least Squares Algorithm” disclosed in Non-Patent Document 1 is used. Can do.
In the first embodiment, an example in which the sphere is a true sphere has been described. However, the object 10 may be an ellipsoid that is not a true sphere, and in this case, an equation representing the shape of the ellipsoid is used. What should I do?
[Non-Patent Document 1]
David Eberly, “Least Squares Fitting of Data,” published July 15, 1999, https://www.geometrictools.com/Documentation/LeastSquaresFitting.pdf

なお、Ｍ点の３次元点群データｐ_１ ^ｎ，ｐ_２ ^ｎ，・・・，ｐ_Ｍ ^ｎの中には、ロボットハンド１により把持されている物体１０の３次元位置以外の不要な位置の計測結果も含まれる場合がある。
この場合には、Ｍ点の３次元点群データｐ_１ ^ｎ，ｐ_２ ^ｎ，・・・，ｐ_Ｍ ^ｎの中から、不要な位置の計測結果を除去する必要がある。不要な位置の計測結果の除去は、人手によって除去するようにしてもよいが、例えば、以下の非特許文献２に記載されている方法を用いて、除去するようにしてもよい。
非特許文献２には、Ｍ点の３次元点群データｐ_１ ^ｎ，ｐ_２ ^ｎ，・・・，ｐ_Ｍ ^ｎの中から、ロボットハンド１により把持されている物体１０の領域の計測結果を自動的に抽出する方法が記載されている。
［非特許文献２］
Anas Abuzainaら著、“Sphere Detection in Kinect Point Clouds via the 3D Hough Transform,” Computer Analysis of Images and Patterns, vol. 8048, pp. 290〜297, Springer, 2013年発行It should be noted that in the three-dimensional point group data p ₁ ⁿ , p ₂ ⁿ ,..., P _M ⁿ of M points, unnecessary positions other than the three-dimensional position of the object 10 held by the robot hand 1 are included. Measurement results may also be included.
In this case, it is necessary to remove measurement results at unnecessary positions from the three-dimensional point group data p ₁ ⁿ , p ₂ ⁿ ,..., P _M ⁿ of M points. The measurement results at unnecessary positions may be removed manually, but may be removed using, for example, the method described in Non-Patent Document 2 below.
Non-Patent Document 2, the three-dimensional point group data _p ¹ n of M _points, ^p 2 n, · · _·, from among _p ^{M n,} the measurement results of the region of the object 10 which is gripped by the robot hand 1 A method for automatic extraction is described.
[Non-Patent Document 2]
Published by Anas Abuzaina et al., “Sphere Detection in Kinect Point Clouds via the 3D Hough Transform,” Computer Analysis of Images and Patterns, vol. 8048, pp. 290-297, Springer, 2013

変換係数算出部１３は、位置姿勢情報取得部１１により特定された物体１０のロボット座標系での３次元位置Ｐ_ｒ ^ｎと、センサ情報取得部１２により特定された物体１０のセンサ座標系での３次元位置Ｐ_ｓ ^ｎとを取得し、ロボット座標系での３次元位置Ｐ_ｒ ^ｎとセンサ座標系での３次元位置Ｐ_ｓ ^ｎとの組を記憶部１３ａに保存する（図４のステップＳＴ４）。
変換係数算出部１３は、ロボット座標系での３次元位置Ｐ_ｒ ^ｎとセンサ座標系での３次元位置Ｐ_ｓ ^ｎとの組を記憶部１３ａに保存すると、ロボットアーム又はロボットハンド１の関節を制御した回数ｎがＮ回に到達したか否かを判定する（図４のステップＳＴ５）。
この実施の形態１では、Ｎは３以上の整数であり、この段階では、ｎ＝１であるため、ロボットアーム又はロボットハンド１の関節を制御した回数ｎがＮ回に到達していないと判定される。Conversion coefficient calculation section 13, in the three-dimensional position P _r ⁿ and the sensor coordinate system of the object 10 identified by the sensor information acquisition unit 12 in the robot coordinate system of the object 10 specified by the position and orientation information acquisition unit 11 obtains the three-dimensional position _P ^{s n,} it stores a set of the three-dimensional position _P ^{s n} in the three-dimensional position _P ^{r n} and the sensor coordinate system in the robot coordinate system in the storage unit 13a (step ST4 in FIG. 4 ).
Conversion coefficient calculation section 13, save a set of the three-dimensional position P _s ⁿ in the three-dimensional position P _r ⁿ and the sensor coordinate system in the robot coordinate system in the storage unit 13a, the joint robot arm or robot hand 1 It is determined whether or not the controlled number n has reached N times (step ST5 in FIG. 4).
In the first embodiment, N is an integer equal to or greater than 3. At this stage, n = 1, and therefore, it is determined that the number n of controlling the joint of the robot arm or the robot hand 1 has not reached N times. Is done.

変換係数算出部１３は、ロボットアーム又はロボットハンド１の関節を制御した回数ｎがＮ回に到達していないと判定すると（図４のステップＳＴ５：ＮＯの場合）、ロボットハンド１の位置又は姿勢の切り替えを指示する制御信号をロボット制御部２に出力する（図４のステップＳＴ６）。
変換係数算出部１３からロボットハンド１の位置又は姿勢の切り替えを指示する制御信号が出力されると、ステップＳＴ１〜ＳＴ５の処理が繰り返される。When the conversion coefficient calculation unit 13 determines that the number n of times when the robot arm or the joint of the robot hand 1 is controlled has not reached N times (in the case of step ST5: NO in FIG. 4), the position or posture of the robot hand 1 Is output to the robot controller 2 (step ST6 in FIG. 4).
When a control signal instructing switching of the position or posture of the robot hand 1 is output from the conversion coefficient calculation unit 13, the processes of steps ST1 to ST5 are repeated.

即ち、ロボット制御部２は、変換係数算出部１３から制御信号を受けると、その制御信号に従ってロボットアーム又はロボットハンド１の関節の角度を変えることで、ロボットハンド１の位置及び姿勢を切り替える制御を行う。
これにより、ロボットハンド１により把持されている物体１０は、ロボット座標系でＰ_ｒ ^ｎの位置に移動される（図４のステップＳＴ１）。例えば、ロボットアーム又はロボットハンド１の関節を制御した回数ｎが２であれば、ロボット座標系でＰ_ｒ ^２の位置に移動される。例えば、ロボットアーム又はロボットハンド１の関節を制御した回数ｎが３であれば、ロボット座標系でＰ_ｒ ^３の位置に移動される。
位置姿勢情報取得部１１は、ロボットハンド１の位置及び姿勢を示す位置姿勢情報を取得し、位置姿勢情報から物体１０のロボット座標系での３次元位置Ｐ_ｒ ^ｎを特定する（図４のステップＳＴ２）。That is, when the robot control unit 2 receives a control signal from the conversion coefficient calculation unit 13, the robot control unit 2 performs control for switching the position and posture of the robot hand 1 by changing the angle of the joint of the robot arm or the robot hand 1 according to the control signal. Do.
Thus, the object 10 which is gripped by the robot hand 1 is moved to the position of P _r ⁿ in the robot coordinate system (step ST1 in FIG. 4). For example, if the number n of times when the joint of the robot arm or the robot hand 1 is controlled is 2, it is moved to the position of P _r ^{2 in} the robot coordinate system. For example, if the number n of controlling the joint of the robot arm or the robot hand 1 is 3, the robot arm is moved to the position of P _r ^{3 in} the robot coordinate system.
Position and orientation information acquisition unit 11, the step of acquiring position and orientation information indicating the position and orientation of the robot hand 1, identifies a three-dimensional position P _r ⁿ in the robot coordinate system of the object 10 from the position and orientation information (FIG. 4 ST2).

センサ３は、ロボット制御部２によって、ロボット座標系でＰ_ｒ ^ｎの位置に移動された物体１０を計測し、物体１０の計測結果として、物体１０のセンサ座標系での３次元位置Ｐ_ｓ ^ｎを示すセンサ情報を出力する。
センサ情報取得部１２は、センサ３から出力されたセンサ情報を取得し、センサ情報から物体１０のセンサ座標系での３次元位置Ｐ_ｓ ^ｎを特定する（図４のステップＳＴ３）。
変換係数算出部１３は、位置姿勢情報取得部１１により特定された物体１０のロボット座標系での３次元位置Ｐ_ｒ ^ｎと、センサ情報取得部１２により特定された物体１０のセンサ座標系での３次元位置Ｐ_ｓ ^ｎとを取得し、ロボット座標系での３次元位置Ｐ_ｒ ^ｎとセンサ座標系での３次元位置Ｐ_ｓ ^ｎとの組を記憶部１３ａに保存する（図４のステップＳＴ４）。
変換係数算出部１３は、ロボット座標系での３次元位置Ｐ_ｒ ^ｎとセンサ座標系での３次元位置Ｐ_ｓ ^ｎとの組を記憶部１３ａに保存すると、ロボットアーム又はロボットハンド１の関節を制御した回数ｎがＮ回に到達したか否かを判定する（図４のステップＳＴ５）。The sensor 3 measures the object 10 moved to the position of P _r ^{n in} the robot coordinate system by the robot control unit 2, and as a measurement result of the object 10, the three-dimensional position P _s ⁿ of the object 10 in the sensor coordinate system. The sensor information indicating is output.
The sensor information acquisition unit 12 acquires sensor information output from the sensor 3, to identify the three-dimensional position P _s ⁿ in the sensor coordinate system of the object 10 from the sensor information (step ST3 in FIG. 4).
Conversion coefficient calculation section 13, in the three-dimensional position P _r ⁿ and the sensor coordinate system of the object 10 identified by the sensor information acquisition unit 12 in the robot coordinate system of the object 10 specified by the position and orientation information acquisition unit 11 obtains the three-dimensional position _P ^{s n,} it stores a set of the three-dimensional position _P ^{s n} in the three-dimensional position _P ^{r n} and the sensor coordinate system in the robot coordinate system in the storage unit 13a (step ST4 in FIG. 4 ).
Conversion coefficient calculation section 13, save a set of the three-dimensional position P _s ⁿ in the three-dimensional position P _r ⁿ and the sensor coordinate system in the robot coordinate system in the storage unit 13a, the joint robot arm or robot hand 1 It is determined whether or not the controlled number n has reached N times (step ST5 in FIG. 4).

変換係数算出部１３は、ロボットアーム又はロボットハンド１の関節を制御した回数ｎがＮ回に到達していると判定すると（図４のステップＳＴ５：ＹＥＳの場合）、変換係数として回転行列Ｒ及び並進ベクトルｔを算出する（図４のステップＳＴ７）。
センサ座標系での３次元位置Ｐ_ｓ ^ｎをロボット座標系での３次元位置Ｐ_ｒ ^ｎに変換する際の剛体変換は、回転行列Ｒ及び並進ベクトルｔを用いると、以下の式（５）で表される。

以下、変換係数算出部１３による回転行列Ｒ及び並進ベクトルｔの算出処理を具体的に説明する。If the conversion coefficient calculation unit 13 determines that the number n of times when the joint of the robot arm or the robot hand 1 is controlled has reached N times (in the case of YES in step ST5 in FIG. 4), the conversion matrix R and the rotation matrix R and A translation vector t is calculated (step ST7 in FIG. 4).
Rigid transformation in converting the three-dimensional position P _s ⁿ of the sensor coordinate system into three-dimensional position P _r ⁿ of the robot coordinate system, using the rotation matrix R and translation vector t, the following equation (5) expressed.

Hereinafter, the calculation process of the rotation matrix R and the translation vector t by the conversion coefficient calculation unit 13 will be specifically described.

変換係数算出部１３は、記憶部１３ａに保持されているＮ個の組を取得する。
即ち、変換係数算出部１３は、以下に示すように、ロボット座標系での３次元位置Ｐ_ｒ ^ｎとセンサ座標系での３次元位置Ｐ_ｓ ^ｎとの組をＮ個取得する。ｎ＝１，２，・・・，Ｎである。
Ｐ_ｒ ^１−Ｐ_ｓ ^１の組
Ｐ_ｒ ^２−Ｐ_ｓ ^２の組
：
Ｐ_ｒ ^Ｎ−Ｐ_ｓ ^Ｎの組The conversion coefficient calculation unit 13 acquires N sets stored in the storage unit 13a.
That is, conversion coefficient calculation section 13, as shown below, a set of the three-dimensional position P _s ⁿ in the three-dimensional position P _r ⁿ and the sensor coordinate system in the robot coordinate system to the N acquisition. n = 1, 2,..., N.
P _r ¹ -P _s ¹ of the set P _r ² -P _s ² of the set:
Set of P _r ^N -P _s ^N

変換係数算出部１３は、ロボット座標系での３次元位置Ｐ_ｒ ^ｎとセンサ座標系での３次元位置Ｐ_ｓ ^ｎとの組をＮ個取得すると、以下に示すように、Ｎ個の組から回転行列Ｒ及び並進ベクトルｔを算出する。
Ｎ個の組から回転行列Ｒを算出する方法として、例えば、以下の非特許文献３に記載されている方法を用いることができる。
非特許文献３に記載されている方法は、Ｎ個の組における３次元位置Ｐ_ｒ ^ｎと３次元位置Ｐ_ｓ ^ｎを出来る限り近づける方法である。
即ち、非特許文献３に記載されている方法は、以下の式（６）を最小にする回転行列Ｒを算出する方法である。

［非特許文献３］
八木康史ら編、「コンピュータビジョン最先端ガイド３」、アドコム・メディア株式会社、2010年12月8日発行、ｐ．36〜37Conversion coefficient calculation section 13, a set of the three-dimensional position P _s ⁿ in the three-dimensional position P _r ⁿ and the sensor coordinate system in the robot coordinate system when the N to obtain, as shown below, from the N pairs A rotation matrix R and a translation vector t are calculated.
As a method for calculating the rotation matrix R from the N sets, for example, the method described in Non-Patent Document 3 below can be used.
Method described in Non-Patent Document 3 is a three-dimensional position P _r ⁿ and how close as possible a three-dimensional position P _s ⁿ in the N pairs.
That is, the method described in Non-Patent Document 3 is a method for calculating a rotation matrix R that minimizes the following equation (6).

[Non-Patent Document 3]
Edited by Yasushi Yagi et al., “Computer Vision Cutting-Edge Guide 3”, Adcom Media Co., Ltd., issued December 8, 2010, p. 36-37

変換係数算出部１３は、式（６）を最小にする回転行列Ｒを算出するため、まず、以下の式（７）で表される共分散行列Ａを算出する。

式（７）において、μ_ｒはロボット座標系での物体１０の観測点の重心座標であり、以下の式（８）で表される。
μ_ｓはセンサ座標系での物体１０の観測点の重心座標であり、以下の式（９）で表される。

In order to calculate the rotation matrix R that minimizes the equation (6), the conversion coefficient calculation unit 13 first calculates a covariance matrix A represented by the following equation (7).

In the formula (7), mu _r is the barycentric coordinates of the observation point of the object 10 in the robot coordinate system is expressed by the following equation (8).
μ _s is the barycentric coordinate of the observation point of the object 10 in the sensor coordinate system, and is expressed by the following equation (9).

変換係数算出部１３は、共分散行列Ａを算出すると、以下の式（１０）に示すように、共分散行列Ａを特異値分解する。

式（１０）において、Ｕ，Ｖはユニタリ行列であり、ユニタリ行列Ｕ，Ｖは共分散行列Ａを特異値分解することで得られる。
変換係数算出部１３は、以下の式（１１）に示すように、ユニタリ行列Ｕ，Ｖを用いて、回転行列Ｒを算出する。

式（１１）において、ｄｅｔ（Ｘ）は行列Ｘの行列式を表している。
変換係数算出部１３は、回転行列Ｒを算出すると、以下の式（１２）に示すように、回転行列Ｒと、物体１０の観測点の重心座標μ_ｒ，μ_ｓとを用いて、並進ベクトルｔを算出する。

After calculating the covariance matrix A, the conversion coefficient calculation unit 13 performs singular value decomposition on the covariance matrix A as shown in the following equation (10).

In Expression (10), U and V are unitary matrices, and the unitary matrices U and V are obtained by singular value decomposition of the covariance matrix A.
The conversion coefficient calculation unit 13 calculates the rotation matrix R using the unitary matrices U and V as shown in the following equation (11).

In equation (11), det (X) represents the determinant of the matrix X.
After calculating the rotation matrix R, the conversion coefficient calculation unit 13 uses the rotation matrix R and the barycentric coordinates μ _r and μ _s of the observation points of the object 10 as shown in the following equation (12), to obtain a translation vector. t is calculated.

座標変換部１４は、変換係数算出部１３により算出された回転行列Ｒ及び並進ベクトルｔを取得し、回転行列Ｒ及び並進ベクトルｔを記憶する。
その後、座標変換部１４は、センサ情報取得部１２からセンサ情報を受けると、記憶している回転行列Ｒ及び並進ベクトルｔと、そのセンサ情報が示す物体１０のセンサ座標系での３次元位置Ｐ_ｓを以下の式（１３）に代入することで、物体１０のセンサ座標系での３次元位置Ｐ_ｓをロボット座標系での３次元位置Ｐ_ｒに変換する（図４のステップＳＴ８）。

The coordinate conversion unit 14 acquires the rotation matrix R and the translation vector t calculated by the conversion coefficient calculation unit 13, and stores the rotation matrix R and the translation vector t.
Thereafter, when the coordinate conversion unit 14 receives the sensor information from the sensor information acquisition unit 12, the stored rotation matrix R and translation vector t and the three-dimensional position P in the sensor coordinate system of the object 10 indicated by the sensor information. _s to by substituting the equation (13) below, to convert the three-dimensional position P _s of the sensor coordinate system of the object 10 to the three-dimensional position P _r of the robot coordinate system (step ST8 in FIG. 4).

以上で明らかなように、この実施の形態１によれば、変換係数算出部１３が、ロボットハンド１の位置又は姿勢が切り替えられる毎に、位置姿勢情報取得部１１により特定されたロボット座標系での位置とセンサ情報取得部１２により取得されたセンサ情報が示すセンサ座標系での位置との組を保持し、保持している複数の組から、センサ座標系での位置をロボット座標系での位置に変換する際に用いる変換係数を算出するように構成したので、センサ３がレーザスキャナなどの３次元センサであっても、センサ座標系での位置をロボット座標系での位置に変換する際に用いる変換係数を算出することができる効果を奏する。   As is apparent from the above, according to the first embodiment, the conversion coefficient calculation unit 13 uses the robot coordinate system specified by the position / orientation information acquisition unit 11 every time the position or orientation of the robot hand 1 is switched. And the position in the sensor coordinate system indicated by the sensor information acquired by the sensor information acquisition unit 12 is held, and the position in the sensor coordinate system is determined from the plurality of held sets in the robot coordinate system. Since the conversion coefficient used when converting to the position is calculated, even when the sensor 3 is a three-dimensional sensor such as a laser scanner, the position in the sensor coordinate system is converted to the position in the robot coordinate system. There is an effect that the conversion coefficient used in the calculation can be calculated.

実施の形態２．
上記実施の形態１では、ロボットハンド１により把持されている物体１０が球体である例を示したが、この実施の形態２では、ロボットハンド１により把持されている物体１０が平板である例を説明する。Embodiment 2. FIG.
In the first embodiment, the example in which the object 10 gripped by the robot hand 1 is a sphere is shown, but in the second embodiment, the object 10 gripped by the robot hand 1 is a flat plate. explain.

図５はこの発明の実施の形態２による変換係数算出装置６を実装しているロボットシステムを示す構成図である。図５において、図１と同一符号は同一または相当部分を示すので説明を省略する。
センサ５はロボットハンド１により把持されている物体１０を計測するレーザスキャナなどの３次元センサであり、物体１０の計測結果を示すセンサ情報を出力する。
センサ５から出力されるセンサ情報は、空間中の物体１０の表面である平面のパラメータ（以下、「平面パラメータ」と称する）の集合である。センサ５による物体１０の計測方法として、ステレオビジョンに基づく方法、Ｔｉｍｅ−ｏｆ−Ｆｌｉｇｈｔ法などが知られている。
なお、センサ５は、平面パラメータの集合であるセンサ情報を出力するほか、図１のセンサ３と同様に、物体１０の表面の３次元座標（Ｘ, Ｙ, Ｚ）の集合であるセンサ情報を出力する。FIG. 5 is a block diagram showing a robot system in which the conversion coefficient calculation apparatus 6 according to Embodiment 2 of the present invention is mounted. In FIG. 5, the same reference numerals as those in FIG.
The sensor 5 is a three-dimensional sensor such as a laser scanner that measures the object 10 held by the robot hand 1, and outputs sensor information indicating the measurement result of the object 10.
The sensor information output from the sensor 5 is a set of plane parameters (hereinafter referred to as “plane parameters”) that are the surface of the object 10 in the space. As a method for measuring the object 10 by the sensor 5, a method based on stereo vision, a Time-of-Flight method, and the like are known.
The sensor 5 outputs sensor information that is a set of plane parameters, and also outputs sensor information that is a set of three-dimensional coordinates (X, Y, Z) of the surface of the object 10 in the same manner as the sensor 3 of FIG. Output.

変換係数算出装置６は、位置姿勢情報取得部４１、センサ情報取得部４２、変換係数算出部４３及び座標変換部４４を備えている。
図６はこの発明の実施の形態２による変換係数算出装置６を示すハードウェア構成図である。
位置姿勢情報取得部４１は例えば図６に示す位置姿勢情報取得回路５１で実現されるものであり、ロボットハンド１の位置及び姿勢を示す位置姿勢情報を取得し、位置姿勢情報から物体１０のロボット座標系での平面パラメータを特定する処理を実施する。
センサ情報取得部４２は例えば図６に示すセンサ情報取得回路５２で実現されるものであり、センサ５から物体１０のセンサ座標系での平面パラメータを示すセンサ情報を取得する処理を実施する。The conversion coefficient calculation device 6 includes a position / orientation information acquisition unit 41, a sensor information acquisition unit 42, a conversion coefficient calculation unit 43, and a coordinate conversion unit 44.
FIG. 6 is a hardware configuration diagram showing a conversion coefficient calculation apparatus 6 according to Embodiment 2 of the present invention.
The position / orientation information acquisition unit 41 is realized by, for example, the position / orientation information acquisition circuit 51 illustrated in FIG. 6. The position / orientation information acquisition unit 41 acquires position / orientation information indicating the position and orientation of the robot hand 1. A process for specifying a plane parameter in the coordinate system is performed.
The sensor information acquisition unit 42 is realized by, for example, the sensor information acquisition circuit 52 illustrated in FIG. 6, and performs a process of acquiring sensor information indicating a plane parameter in the sensor coordinate system of the object 10 from the sensor 5.

変換係数算出部４３は例えば図６に示す変換係数算出回路５３で実現される。
変換係数算出部４３はロボット制御部２によりロボットハンド１の位置又は姿勢が切り替えられる毎に、位置姿勢情報取得部４１により特定されたロボット座標系での平面パラメータとセンサ情報取得部４２により取得されたセンサ情報が示すセンサ座標系での平面パラメータとの組を保持する記憶部４３ａを備えている。
変換係数算出部４３は、記憶部４３ａに保持されている複数の組から、センサ座標系での３次元位置をロボット座標系での３次元位置に変換する際に用いる変換係数として、剛体変換における回転行列Ｒ及び並進ベクトルｔを算出する処理を実施する。
また、変換係数算出部４３は、ロボット座標系での平面パラメータとセンサ座標系での平面パラメータとの組を保持する毎に、ロボットハンド１の位置又は姿勢の切り替えを指示する制御信号をロボット制御部２に出力する処理を実施する。The conversion coefficient calculation unit 43 is realized by, for example, a conversion coefficient calculation circuit 53 illustrated in FIG.
The conversion coefficient calculation unit 43 is acquired by the plane parameter and sensor information acquisition unit 42 in the robot coordinate system specified by the position / orientation information acquisition unit 41 every time the position or posture of the robot hand 1 is switched by the robot control unit 2. A storage unit 43a that holds a set of plane parameters in the sensor coordinate system indicated by the sensor information.
The conversion coefficient calculation unit 43 uses, as a conversion coefficient used when converting a three-dimensional position in the sensor coordinate system into a three-dimensional position in the robot coordinate system from a plurality of sets held in the storage unit 43a. A process of calculating the rotation matrix R and the translation vector t is performed.
In addition, every time the conversion coefficient calculation unit 43 holds a set of a plane parameter in the robot coordinate system and a plane parameter in the sensor coordinate system, a control signal that instructs switching of the position or orientation of the robot hand 1 is controlled by the robot. The process of outputting to the unit 2 is performed.

座標変換部４４は例えば図６に示す座標変換回路５４で実現される。
座標変換部４４は変換係数算出部４３により算出された変換係数である回転行列Ｒ及び並進ベクトルｔを用いて、センサ情報取得部４２により取得されたセンサ情報が示すセンサ座標系での３次元位置をロボット座標系での３次元位置に変換する処理を実施する。The coordinate conversion unit 44 is realized by, for example, a coordinate conversion circuit 54 shown in FIG.
The coordinate conversion unit 44 uses the rotation matrix R and the translation vector t, which are the conversion coefficients calculated by the conversion coefficient calculation unit 43, to provide a three-dimensional position in the sensor coordinate system indicated by the sensor information acquired by the sensor information acquisition unit 42. Is converted into a three-dimensional position in the robot coordinate system.

図５では、変換係数算出装置６の構成要素が専用のハードウェアで実現されるものを想定している。
しかし、変換係数算出装置６の構成要素は、専用のハードウェアで実現されるものに限るものではなく、変換係数算出装置６がソフトウェア、ファームウェア、または、ソフトウェアとファームウェアとの組み合わせで実現されるものであってもよい。
変換係数算出装置６がソフトウェア又はファームウェアなどで実現される場合、変換係数算出部４５が備えている記憶部４５ａを図３に示すコンピュータのメモリ３１上に構成するとともに、位置姿勢情報取得部４１の位置姿勢情報取得処理手順、センサ情報取得部４２のセンサ情報取得処理手順、変換係数算出部４３の変換係数算出処理手順及び座標変換部４４の座標変換処理手順をコンピュータに実行させるための変換係数算出プログラムをメモリ３１に格納し、コンピュータのプロセッサ３２がメモリ３１に格納されているプログラムを実行するようにすればよい。
図７は変換係数算出装置４がソフトウェア又はファームウェアなどで実現される場合の処理手順に相当する変換係数算出方法を示すフローチャートである。In FIG. 5, it is assumed that the components of the conversion coefficient calculation device 6 are realized by dedicated hardware.
However, the components of the conversion coefficient calculation device 6 are not limited to those realized by dedicated hardware, and the conversion coefficient calculation device 6 is realized by software, firmware, or a combination of software and firmware. It may be.
When the conversion coefficient calculation device 6 is realized by software or firmware, the storage unit 45a included in the conversion coefficient calculation unit 45 is configured on the memory 31 of the computer shown in FIG. Conversion coefficient calculation for causing a computer to execute a position and orientation information acquisition processing procedure, a sensor information acquisition processing procedure of the sensor information acquisition unit 42, a conversion coefficient calculation processing procedure of the conversion coefficient calculation unit 43, and a coordinate conversion processing procedure of the coordinate conversion unit 44 The program may be stored in the memory 31, and the computer processor 32 may execute the program stored in the memory 31.
FIG. 7 is a flowchart showing a conversion coefficient calculation method corresponding to a processing procedure when the conversion coefficient calculation device 4 is realized by software or firmware.

次に動作について説明する。
この実施の形態２では、物体１０である平板は、ロボット座標系で無限に広がる平面であるものと仮定し、平板の平面は、以下の式（１４）が示す平面パラメータπ_ｒで表されるものとする。

式（１４）において、ｎ_ｒは平面の法線ベクトル、ｄ_ｒはロボット座標系の原点から平面までの距離である。ロボット座標系の原点は、例えばロボットハンド１の中心位置である。Next, the operation will be described.
In the second embodiment, it is assumed that the flat plate that is the object 10 is a plane that extends infinitely in the robot coordinate system, and the plane of the flat plate is represented by a plane parameter π _r represented by the following equation (14). Shall.

In the formula (14), n _r is the normal vector of the plane, is d _r is the distance from the origin of the robot coordinate system to the plane. The origin of the robot coordinate system is, for example, the center position of the robot hand 1.

ロボットハンド１は、平板の物体１０を把持している。
ロボット制御部２は、ロボットアーム又はロボットハンド１の関節を制御することで、ロボットハンド１の位置又は姿勢を切り替える。
この実施の形態２でも、上記実施の形態１と同様に、ロボット制御部２が、制御信号に従ってロボットアーム又はロボットハンド１の関節の角度を変えることで、ロボットハンド１の位置又は姿勢を切り替えるものとする。
また、この実施の形態２では、説明の便宜上、Ｎ（Ｎは３以上の整数）回、ロボットアーム又はロボットハンド１の関節を制御するものとする。The robot hand 1 holds a flat object 10.
The robot control unit 2 switches the position or posture of the robot hand 1 by controlling the joints of the robot arm or the robot hand 1.
Also in the second embodiment, as in the first embodiment, the robot control unit 2 switches the position or posture of the robot hand 1 by changing the angle of the joint of the robot arm or the robot hand 1 according to the control signal. And
In the second embodiment, for convenience of explanation, the joint of the robot arm or the robot hand 1 is controlled N (N is an integer of 3 or more) times.

ロボットハンド１により把持されている物体１０の平面は、ロボット制御部２によってロボットアーム又はロボットハンド１の関節が制御されることで、ｎ（ｎ＝１，２，・・・，Ｎ）回目の制御では、以下の式（１５）に示す平面パラメータπ_ｒ ^ｎで表される平面となる（図７のステップＳＴ１１）。この段階では、ｎ＝１であるため、物体１０の平面が平面パラメータπ_ｒ ^１で表される平面となる。

The plane of the object 10 held by the robot hand 1 is controlled by the robot control unit 2 by controlling the robot arm or the joint of the robot hand 1 so that the nth (n = 1, 2,..., N) times. In the control, the plane is represented by the plane parameter π _r ⁿ shown in the following equation (15) (step ST11 in FIG. 7). At this stage, since n = 1, the plane of the object 10 is a plane represented by the plane parameter π _r ¹ .

位置姿勢情報取得部４１は、ロボットハンド１の位置及び姿勢を示す位置姿勢情報を取得し、位置姿勢情報から物体１０のロボット座標系での平面パラメータπ_ｒ ^ｎを特定する（図７のステップＳＴ１２）。
位置姿勢情報からロボット座標系での平面パラメータπ_ｒ ^ｎを特定する処理は公知の技術であるため詳細な説明を省略するが、例えば、以下に示すような処理で平面パラメータπ_ｒ ^ｎを特定することができる。
例えば、ロボットハンド１の位置及び姿勢と、ロボットハンド１により把持されている物体１０の平面パラメータπ_ｒ ^ｎとの対応関係を示すテーブルを事前に用意し、位置姿勢情報取得部４１が、テーブルを参照することで、位置姿勢情報が示す位置及び姿勢に対応する平面パラメータπ_ｒ ^ｎを特定する。The position / orientation information acquisition unit 41 acquires position / orientation information indicating the position and orientation of the robot hand 1 and specifies a plane parameter π _r ⁿ in the robot coordinate system of the object 10 from the position / orientation information (step ST12 in FIG. 7). ).
Processing the position and orientation information for specifying the plane parameters [pi _r ⁿ of the robot coordinate system is a detailed description thereof will be omitted because it is a known technique, for example, to identify the plane parameters [pi _r ⁿ in the process as shown below be able to.
For example, the position and orientation of the robot hand 1, and preparing a table showing the correspondence between the plane parameter [pi _r ⁿ of the object 10 which is gripped by the robot hand 1 in advance, the position and orientation information acquisition unit 41, the table by referring to identify the plane parameters [pi _r ⁿ corresponding to the position and orientation indicated by the position and orientation information.

センサ５は、ロボット制御部２によって、位置又は姿勢が切り替えられたロボットハンド１に把持されている物体１０のセンサ座標系での平面パラメータπ_ｓ ^ｎ（ｎ＝１，２，・・・，Ｎ）を示すセンサ情報を出力する。

この実施の形態２では、センサ５から出力されるセンサ情報は、物体１０の表面におけるＭ点の平面パラメータを示す３次元点群データｐ_１ ^ｎ，ｐ_２ ^ｎ，・・・，ｐ_Ｍ ^ｎである。The sensor 5 has a plane parameter π _s ⁿ (n = 1, 2,..., N) in the sensor coordinate system of the object 10 held by the robot hand 1 whose position or orientation has been switched by the robot controller 2. ) Is output.

In the second embodiment, sensor information output from the sensor 5 is three-dimensional point group data p ₁ ⁿ , p ₂ ⁿ ,..., P _M ⁿ indicating M plane parameters on the surface of the object 10. is there.

センサ情報取得部４２は、センサ５から出力されたセンサ情報を取得し、センサ情報から物体１０のセンサ座標系での平面パラメータπ_ｓ ^ｎ（ｎ＝１，２，・・・，Ｎ）を特定する（図７のステップＳＴ１３）。
Ｍは１以上の整数であり、Ｍ＝１の場合、π_ｓ ^ｎ＝ｐ_１ ^ｎである。
Ｍ≧２の場合、センサ情報取得部４２は、物体１０の形状を示す形状データを用いて、３次元点群データｐ_１ ^ｎ，ｐ_２ ^ｎ，・・・，ｐ_Ｍ ^ｎから物体１０のセンサ座標系での平面パラメータπ_ｓ ^ｎを算出する。
この実施の形態２では、ロボットハンド１が平板の物体１０を把持しているため、３次元点群データｐ_１ ^ｎ，ｐ_２ ^ｎ，・・・，ｐ_Ｍ ^ｎは、平板の表面上のいずれかの点である。
このため、センサ情報取得部４２は、物体１０の形状を示す形状データとして、平板の形状を表す平面の方程式を使用し、３次元点群データｐ_１ ^ｎ，ｐ_２ ^ｎ，・・・，ｐ_Ｍ ^ｎに平面の方程式をフィッティングすることで、物体１０のセンサ座標系での平面パラメータπ_ｓ ^ｎを得ることができる。The sensor information acquisition unit 42 acquires the sensor information output from the sensor 5 and specifies the plane parameter π _s ⁿ (n = 1, 2,..., N) in the sensor coordinate system of the object 10 from the sensor information. (Step ST13 in FIG. 7).
M is an integer of 1 or more, and when M = 1, π _s ⁿ = p ₁ ⁿ .
For M ≧ 2, the sensor information acquisition unit 42 uses the shape data indicating the shape of the object 10, 3D point group data ^{_{p 1 n, p 2 n,}} ···, sensors of the object 10 from _p ^{M n} The plane parameter π _s ⁿ in the coordinate system is calculated.
In the second embodiment, since the robot hand 1 is grasping the object 10 of a flat plate, 3D point group data ^{_{p 1 n, p 2 n,}} ···, p M n are all on the surface of the plate That is the point.
For this reason, the sensor information acquisition part 42 uses the equation of the plane showing the shape of a flat plate as the shape data indicating the shape of the object 10, and uses the three-dimensional point group data p ₁ ⁿ , p ₂ ⁿ ,. By fitting a plane equation to _M ⁿ , the plane parameter π _s ⁿ in the sensor coordinate system of the object 10 can be obtained.

３次元点群データｐ_１ ^ｎ，ｐ_２ ^ｎ，・・・，ｐ_Ｍ ^ｎに平面の方程式をフィッティングする方法としては、複数の方法が考えられるが、上記実施の形態１と同様に、最小二乗法によるフィッティング方法を用いることができる。平面の方程式は、例えば、以下の式（１７）で表される。

式（１７）において、（ａ，ｂ，ｃ）は平面の法線ベクトルである。As a method of fitting a plane equation to the three-dimensional point cloud data p ₁ ⁿ , p ₂ ⁿ ,..., P _M ⁿ , a plurality of methods can be considered. A multiplicative fitting method can be used. The plane equation is expressed by the following equation (17), for example.

In Expression (17), (a, b, c) is a plane normal vector.

変換係数算出部４３は、位置姿勢情報取得部４１により特定された物体１０のロボット座標系での平面パラメータπ_ｒ ^ｎと、センサ情報取得部４２により特定された物体１０のセンサ座標系での平面パラメータπ_ｓ ^ｎとを取得し、ロボット座標系での平面パラメータπ_ｒ ^ｎとセンサ座標系での平面パラメータπ_ｓ ^ｎとの組を記憶部４３ａに保存する（図７のステップＳＴ１４）。
変換係数算出部４３は、ロボット座標系での平面パラメータπ_ｒ ^ｎとセンサ座標系での平面パラメータπ_ｓ ^ｎとの組を記憶部４３ａに保存すると、ロボットアーム又はロボットハンド１の関節を制御した回数ｎがＮ回に到達したか否かを判定する（図７のステップＳＴ１５）。
この実施の形態２では、Ｎは３以上の整数であり、この段階では、ｎ＝１であるため、ロボットアーム又はロボットハンド１の関節を制御した回数ｎがＮ回に到達していないと判定される。Conversion coefficient calculation unit 43, plane in the plane parameters [pi _r ⁿ in the robot coordinate system of the object 10 is identified, the sensor coordinate system of the object 10 identified by the sensor information obtaining unit 42 by the position and orientation information acquisition unit 41 obtains the parameter [pi _s ^n, stores a set of the plane parameters [pi _s ⁿ at plane parameters [pi _r ⁿ and the sensor coordinate system in the robot coordinate system in the storage unit 43a (step ST14 in FIG. 7).
Conversion coefficient calculation unit 43, save a set of the plane parameters [pi _s ⁿ at plane parameters [pi _r ⁿ and the sensor coordinate system in the robot coordinate system in the storage unit 43a, and controls the joint robot arm or robot hand 1 It is determined whether or not the number n has reached N times (step ST15 in FIG. 7).
In the second embodiment, N is an integer equal to or greater than 3, and at this stage, n = 1, and therefore, it is determined that the number n of controlling the robot arm or the joint of the robot hand 1 has not reached N times. Is done.

変換係数算出部４３は、ロボットアーム又はロボットハンド１の関節を制御した回数ｎがＮ回に到達していないと判定すると（図７のステップＳＴ１５：ＮＯの場合）、ロボットハンド１の位置又は姿勢の切り替えを指示する制御信号をロボット制御部２に出力する（図７のステップＳＴ１６）。
変換係数算出部４３からロボットハンド１の位置又は姿勢の切り替えを指示する制御信号が出力されると、ステップＳＴ１１〜ＳＴ１５の処理が繰り返される。If the conversion coefficient calculation unit 43 determines that the number n of times when the robot arm or the joint of the robot hand 1 is controlled has not reached N times (in the case of step ST15 in FIG. 7: NO), the position or orientation of the robot hand 1 Is output to the robot controller 2 (step ST16 in FIG. 7).
When a control signal instructing switching of the position or posture of the robot hand 1 is output from the conversion coefficient calculation unit 43, the processes of steps ST11 to ST15 are repeated.

即ち、ロボット制御部２は、変換係数算出部４３から制御信号を受けると、その制御信号に従ってロボットアーム又はロボットハンド１の関節の角度を変えることで、ロボットハンド１の位置及び姿勢を切り替える制御を行う。
これにより、ロボットハンド１により把持されている物体１０の平面は、平面パラメータπ_ｒ ^ｎで表される平面となる（図７のステップＳＴ１１）。例えば、ロボットアーム又はロボットハンド１の関節を制御した回数ｎが２であれば、物体１０の平面が平面パラメータπ_ｒ ^２で表される平面となる、例えば、ロボットアーム又はロボットハンド１の関節を制御した回数ｎが３であれば、物体１０の平面が平面パラメータπ_ｒ ^３で表される平面となる。
位置姿勢情報取得部４１は、ロボットハンド１の位置及び姿勢を示す位置姿勢情報を取得し、位置姿勢情報から物体１０のロボット座標系での平面パラメータπ_ｒ ^ｎを特定する（図７のステップＳＴ１２）。That is, upon receiving a control signal from the conversion coefficient calculation unit 43, the robot control unit 2 performs control for switching the position and posture of the robot hand 1 by changing the angle of the robot arm or the joint of the robot hand 1 according to the control signal. Do.
Thereby, the plane of the object 10 held by the robot hand 1 becomes a plane represented by the plane parameter π _r ⁿ (step ST11 in FIG. 7). For example, if the number n of times when the joint of the robot arm or robot hand 1 is controlled is 2, the plane of the object 10 becomes a plane represented by the plane parameter π _r ² , for example, the joint of the robot arm or robot hand 1 is If the controlled number of times n is 3, the plane of the object 10 is a plane represented by the plane parameter π _r ³ .
The position / orientation information acquisition unit 41 acquires position / orientation information indicating the position and orientation of the robot hand 1 and specifies a plane parameter π _r ⁿ in the robot coordinate system of the object 10 from the position / orientation information (step ST12 in FIG. 7). ).

センサ５は、ロボット制御部２によって、位置又は姿勢が切り替えられたロボットハンド１に把持されている物体１０のセンサ座標系での平面パラメータπ_ｓ ^ｎ（ｎ＝１，２，・・・，Ｎ）を示すセンサ情報を出力する。
センサ情報取得部４２は、センサ５から出力されたセンサ情報を取得し、センサ情報から物体１０のセンサ座標系での平面パラメータπ_ｓ ^ｎを特定する（図７のステップＳＴ１３）。
変換係数算出部４３は、位置姿勢情報取得部４１により特定された物体１０のロボット座標系での平面パラメータπ_ｒ ^ｎと、センサ情報取得部４２により特定された物体１０のセンサ座標系での平面パラメータπ_ｓ ^ｎとを取得し、ロボット座標系での平面パラメータπ_ｒ ^ｎとセンサ座標系での平面パラメータπ_ｓ ^ｎとの組を記憶部４３ａに保存する（図７のステップＳＴ１４）。
変換係数算出部４３は、ロボット座標系での平面パラメータπ_ｒ ^ｎとセンサ座標系での平面パラメータπ_ｓ ^ｎとの組を記憶部４３ａに保存すると、ロボットアーム又はロボットハンド１の関節を制御した回数ｎがＮ回に到達したか否かを判定する（図７のステップＳＴ１５）。The sensor 5 has a plane parameter π _s ⁿ (n = 1, 2,..., N) in the sensor coordinate system of the object 10 held by the robot hand 1 whose position or orientation has been switched by the robot controller 2. ) Is output.
The sensor information acquisition unit 42 acquires the sensor information output from the sensor 5, and specifies the plane parameter π _s ⁿ in the sensor coordinate system of the object 10 from the sensor information (step ST13 in FIG. 7).
Conversion coefficient calculation unit 43, plane in the plane parameters [pi _r ⁿ in the robot coordinate system of the object 10 is identified, the sensor coordinate system of the object 10 identified by the sensor information obtaining unit 42 by the position and orientation information acquisition unit 41 obtains the parameter [pi _s ^n, stores a set of the plane parameters [pi _s ⁿ at plane parameters [pi _r ⁿ and the sensor coordinate system in the robot coordinate system in the storage unit 43a (step ST14 in FIG. 7).
Conversion coefficient calculation unit 43, save a set of the plane parameters [pi _s ⁿ at plane parameters [pi _r ⁿ and the sensor coordinate system in the robot coordinate system in the storage unit 43a, and controls the joint robot arm or robot hand 1 It is determined whether or not the number n has reached N times (step ST15 in FIG. 7).

変換係数算出部４３は、ロボットアーム又はロボットハンド１の関節を制御した回数ｎがＮ回に到達していると判定すると（図７のステップＳＴ１５：ＹＥＳの場合）、変換係数として回転行列Ｒ及び並進ベクトルｔを算出する（図７のステップＳＴ１７）。
ロボット座標系での平面パラメータπ_ｒ ^ｎとセンサ座標系での平面パラメータπ_ｓ ^ｎとの関係は、回転行列Ｒ及び並進ベクトルｔを用いると、以下の式（１８）で表される。

以下、変換係数算出部１３による回転行列Ｒ及び並進ベクトルｔの算出処理を具体的に説明する。If the conversion coefficient calculation unit 43 determines that the number n of times when the robot arm or the joint of the robot hand 1 is controlled has reached N times (in the case of step ST15 in FIG. 7: YES), the conversion coefficient calculation unit 43 converts the rotation matrix R and A translation vector t is calculated (step ST17 in FIG. 7).
Relationship between the plane parameter [pi _s ⁿ at plane parameters [pi _r ⁿ and the sensor coordinate system in the robot coordinate system, using the rotation matrix R and translation vector t, is expressed by the following equation (18).

Hereinafter, the calculation process of the rotation matrix R and the translation vector t by the conversion coefficient calculation unit 13 will be specifically described.

変換係数算出部４３は、記憶部４３ａに保持されているＮ個の組を取得する。
即ち、変換係数算出部４３は、以下に示すように、ロボット座標系での平面パラメータπ_ｒ ^ｎとセンサ座標系での平面パラメータπ_ｓ ^ｎとの組をＮ個取得する。ｎ＝１，２，・・・，Ｎである。
π_ｒ ^１−π_ｓ ^１の組
π_ｒ ^２−π_ｓ ^２の組
：
π_ｒ ^Ｎ−π_ｓ ^Ｎの組The conversion coefficient calculation unit 43 acquires N sets held in the storage unit 43a.
That is, conversion coefficient calculation unit 43, as shown below, a set of the plane parameters [pi _s ⁿ at plane parameters [pi _r ⁿ and the sensor coordinate system in the robot coordinate system to the N acquisition. n = 1, 2,..., N.
π _r ¹ -π _s ¹ set π _r ² -π _s ² set:
π _r ^N −π _s ^N

変換係数算出部４３は、ロボット座標系での平面パラメータπ_ｒ ^ｎとセンサ座標系での平面パラメータπ_ｓ ^ｎとの組をＮ個取得すると、以下に示すように、Ｎ個の組から回転行列Ｒ及び並進ベクトルｔを算出する。
Ｎ個の組から回転行列Ｒを算出する方法として、例えば、以下の非特許文献４に記載されている方法を用いることができる。
非特許文献４に記載されている方法では、以下の式（１９）を最小にする回転行列Ｒを算出する方法である。

［非特許文献４］
Yuichi Taguchiら著、“Point-Plane SLAM for Hand-Held 3D Sensors,” Proceedings of IEEE International Conference on Robotics and Automation, pp. 5182-5189, 2013年発行Conversion coefficient calculation unit 43, when a set of the plane parameters [pi _s ⁿ at plane parameters [pi _r ⁿ and the sensor coordinate system in the robot coordinate system of N to obtain, as shown below, the rotation matrix from the N pairs R and translation vector t are calculated.
As a method for calculating the rotation matrix R from the N sets, for example, the method described in Non-Patent Document 4 below can be used.
The method described in Non-Patent Document 4 is a method for calculating a rotation matrix R that minimizes the following equation (19).

[Non-Patent Document 4]
Published by Yuichi Taguchi et al., “Point-Plane SLAM for Hand-Held 3D Sensors,” Proceedings of IEEE International Conference on Robotics and Automation, pp. 5182-5189, 2013

変換係数算出部４３は、回転行列Ｒを算出すると、以下の式（２０）に示す連立１次方程式を解くことで、並進ベクトルｔを求める。

After calculating the rotation matrix R, the conversion coefficient calculation unit 43 calculates a translation vector t by solving simultaneous linear equations shown in the following equation (20).

座標変換部４４は、変換係数算出部４３により算出された回転行列Ｒ及び並進ベクトルｔを取得し、回転行列Ｒ及び並進ベクトルｔを記憶する。
その後、座標変換部４４は、センサ情報取得部１２から物体１０のセンサ座標系での３次元位置Ｐ_ｓを示すセンサ情報を受けると、記憶している回転行列Ｒ及び並進ベクトルｔと、センサ情報が示す３次元位置Ｐ_ｓとを以下の式（２１）に代入することで、物体１０のセンサ座標系での３次元位置Ｐ_ｓをロボット座標系での３次元位置Ｐ_ｒに変換する（図７のステップＳＴ１８）。

The coordinate conversion unit 44 acquires the rotation matrix R and the translation vector t calculated by the conversion coefficient calculation unit 43, and stores the rotation matrix R and the translation vector t.
Then, the coordinate transformation unit 44 receives the sensor information indicating the three-dimensional position P _s of the sensor coordinate system of the object 10 from the sensor information acquiring unit 12, and the rotation matrix R and translation vector t stores sensor information by substituting the equation (21) below the three-dimensional position P _s indicated converts the three-dimensional position P _s of the sensor coordinate system of the object 10 to the three-dimensional position P _r of the robot coordinate system (FIG. 7 step ST18).

以上で明らかなように、この実施の形態２によれば、変換係数算出部４３が、ロボットハンド１の位置又は姿勢が切り替えられる毎に、位置姿勢情報取得部４１により特定されたロボット座標系での平面パラメータとセンサ情報取得部４２により取得されたセンサ情報が示すセンサ座標系での平面パラメータとの組を保持し、保持している複数の組から、センサ座標系での位置をロボット座標系での位置に変換する際に用いる変換係数を算出するように構成したので、センサ５がレーザスキャナなどの３次元センサであっても、センサ座標系での位置をロボット座標系での位置に変換する際に用いる変換係数を算出することができる効果を奏する。   As is apparent from the above, according to the second embodiment, the conversion coefficient calculation unit 43 uses the robot coordinate system specified by the position / orientation information acquisition unit 41 every time the position or orientation of the robot hand 1 is switched. The plane parameter in the sensor coordinate system indicated by the sensor information acquired by the sensor information acquisition unit 42 is held, and the position in the sensor coordinate system is determined from the plurality of held sets in the robot coordinate system. Since the conversion coefficient used for the conversion to the position in the camera is calculated, even if the sensor 5 is a three-dimensional sensor such as a laser scanner, the position in the sensor coordinate system is converted into the position in the robot coordinate system. There is an effect that it is possible to calculate a conversion coefficient to be used.

実施の形態３．
上記実施の形態１では、ロボットハンド１により把持されている物体１０が球体である例を示し、上記実施の形態２では、ロボットハンド１により把持されている物体１０が平板である例をしている。ロボットハンド１により把持されている物体１０は、球体や平板に限るものではない。Embodiment 3 FIG.
In the first embodiment, an example in which the object 10 gripped by the robot hand 1 is a sphere is shown, and in the second embodiment, the object 10 gripped by the robot hand 1 is a flat plate. Yes. The object 10 held by the robot hand 1 is not limited to a sphere or a flat plate.

この実施の形態３では、物体１０の形状が３次元モデルで表される形状であれば、物体１０の形状を示す形状データとして、３次元モデルを用いる。
例えば、角などを有する立方体のように、形状に特徴がある物体１０は、形状を３次元モデルで表すことができる。また、センサ情報が示す３次元点群データから、角などの特徴がある点を抽出することができる。
３次元モデルとしては、ロボットハンド１により把持される物体１０のＣＡＤモデル又はポリゴンデータなどを使用することができる。In the third embodiment, if the shape of the object 10 is a shape represented by a three-dimensional model, the three-dimensional model is used as shape data indicating the shape of the object 10.
For example, the object 10 having a feature such as a cube having corners can be represented by a three-dimensional model. In addition, points having features such as corners can be extracted from the three-dimensional point cloud data indicated by the sensor information.
As the three-dimensional model, a CAD model or polygon data of the object 10 held by the robot hand 1 can be used.

上記実施の形態１では、センサ情報取得部１２が、物体１０のセンサ座標系での３次元位置Ｐ_ｓ ^ｎを算出する際、物体１０の形状を示す形状データとして、球体の形状を表す球の方程式を使用するのに対して、この実施の形態３では、物体１０の３次元モデルを使用する点で相違している。
したがって、この実施の形態３では、センサ情報取得部１２は、物体１０の形状を示す形状データとして、物体１０の３次元モデルを使用し、物体１０の角などの特徴がある点の位置を示す３次元点群データｐ_１ ^ｎ，ｐ_２ ^ｎ，・・・，ｐ_Ｍ ^ｎに物体１０の３次元モデルをフィッティングすることで、物体１０の中心の位置として、３次元位置Ｐ_ｓ ^ｎを得ることができる。
その他は、上記実施の形態１と同様であるため詳細な説明を省略する。In the first embodiment, the sensor information acquisition unit 12, when calculating the three-dimensional position P _s ⁿ in the sensor coordinate system of the object 10, the shape data representing the shape of the object 10, the sphere representing the shape of a sphere In contrast to using equations, the third embodiment is different in that a three-dimensional model of the object 10 is used.
Therefore, in the third embodiment, the sensor information acquisition unit 12 uses the three-dimensional model of the object 10 as the shape data indicating the shape of the object 10, and indicates the position of a point having a feature such as a corner of the object 10. By fitting the three-dimensional model of the object 10 to the three-dimensional point cloud data p ₁ ⁿ , p ₂ ⁿ ,..., P _M ⁿ , the three-dimensional position P _s ⁿ is obtained as the center position of the object 10. Can do.
Since others are the same as that of the said Embodiment 1, detailed description is abbreviate | omitted.

実施の形態４．
上記実施の形態１，２では、変換係数算出部１３又は変換係数算出部４３が、センサ座標系での３次元位置をロボット座標系での３次元位置に変換する際に用いる変換係数を算出する例を示している。
しかし、ロボットハンド１により把持される物体１０の姿勢によっては、センサ３又はセンサ５の計測結果に大きな誤差が含まれてしまうことがある。センサ３又はセンサ５の計測結果に大きな誤差が含まれていれば、センサ３又はセンサ５から大きな誤差を含むセンサ情報が変換係数算出部１３又は変換係数算出部４３に出力されるため、変換係数算出部１３又は変換係数算出部４３により算出される変換係数の精度が劣化する。
例えば、物体１０が平板である場合、平板がセンサ５と正対している状況と、平板がセンサ５と正対していない状況とを比べてみると、正対していない状況で計測されたセンサ情報の方が大きな誤差を含んでいる。
ただし、形状や材質などの物体１０の特性によっては、物体１０の姿勢が変わっても、センサ情報に含まれる誤差に変化がない場合もある。
この実施の形態４では、物体１０の姿勢によって、変換係数算出部１３又は変換係数算出部４３により算出される変換係数の精度が劣化する状況を回避することができる例を説明する。Embodiment 4 FIG.
In the first and second embodiments, the conversion coefficient calculation unit 13 or the conversion coefficient calculation unit 43 calculates a conversion coefficient used when converting the three-dimensional position in the sensor coordinate system to the three-dimensional position in the robot coordinate system. An example is shown.
However, depending on the posture of the object 10 held by the robot hand 1, a large error may be included in the measurement result of the sensor 3 or the sensor 5. If the measurement result of the sensor 3 or sensor 5 includes a large error, the sensor information including the large error is output from the sensor 3 or sensor 5 to the conversion coefficient calculation unit 13 or the conversion coefficient calculation unit 43. The accuracy of the conversion coefficient calculated by the calculation unit 13 or the conversion coefficient calculation unit 43 deteriorates.
For example, when the object 10 is a flat plate, comparing the situation where the flat plate faces the sensor 5 and the situation where the flat plate does not face the sensor 5, sensor information measured in a situation where the flat plate does not face the sensor 5 Contains a larger error.
However, depending on the characteristics of the object 10 such as the shape and material, the error included in the sensor information may not change even if the posture of the object 10 changes.
In the fourth embodiment, an example in which the situation in which the accuracy of the conversion coefficient calculated by the conversion coefficient calculation unit 13 or the conversion coefficient calculation unit 43 deteriorates depending on the posture of the object 10 will be described.

図８はこの発明の実施の形態４による変換係数算出装置６を実装しているロボットシステムを示す構成図である。図８において、図５と同一符号は同一または相当部分を示している。
図９はこの発明の実施の形態４による変換係数算出装置６を示すハードウェア構成図である。図９において、図６と同一符号は同一または相当部分を示している。
変換係数算出装置６は、位置姿勢情報取得部４１、センサ情報取得部４２、変換係数算出部４５、座標変換部４４及び位置姿勢調整部４６を備えている。FIG. 8 is a block diagram showing a robot system equipped with a conversion coefficient calculation device 6 according to Embodiment 4 of the present invention. 8, the same reference numerals as those in FIG. 5 denote the same or corresponding parts.
FIG. 9 is a hardware configuration diagram showing a conversion coefficient calculation apparatus 6 according to Embodiment 4 of the present invention. 9, the same reference numerals as those in FIG. 6 denote the same or corresponding parts.
The conversion coefficient calculation device 6 includes a position / orientation information acquisition unit 41, a sensor information acquisition unit 42, a conversion coefficient calculation unit 45, a coordinate conversion unit 44, and a position / orientation adjustment unit 46.

変換係数算出部４５は例えば図９に示す変換係数算出回路５５で実現される。
変換係数算出部４５は図５の変換係数算出部４３と同様に、ロボット制御部２によりロボットハンド１の位置又は姿勢が切り替えられる毎に、位置姿勢情報取得部４１により特定されたロボット座標系での平面パラメータとセンサ情報取得部４２により取得されたセンサ情報が示すセンサ座標系での平面パラメータとの組を保持する記憶部４５ａを備えている。
変換係数算出部４５は図５の変換係数算出部４３と同様に、記憶部４５ａに保持されている複数の組から、センサ座標系での３次元位置をロボット座標系での３次元位置に変換する際に用いる変換係数として、剛体変換における回転行列Ｒ及び並進ベクトルｔを算出する処理を実施する。
また、変換係数算出部４５は図５の変換係数算出部４３と同様に、ロボット座標系での平面パラメータとセンサ座標系での平面パラメータとの組を保持する毎に、ロボットハンド１の位置又は姿勢の切り替えを指示する制御信号をロボット制御部２に出力する処理を実施する。The conversion coefficient calculation unit 45 is realized by, for example, a conversion coefficient calculation circuit 55 shown in FIG.
Similar to the conversion coefficient calculation unit 43 in FIG. 5, the conversion coefficient calculation unit 45 uses the robot coordinate system specified by the position / orientation information acquisition unit 41 every time the position or posture of the robot hand 1 is switched by the robot control unit 2. The storage unit 45a holds a set of the plane parameter and the plane parameter in the sensor coordinate system indicated by the sensor information acquired by the sensor information acquisition unit 42.
Similarly to the conversion coefficient calculation unit 43 in FIG. 5, the conversion coefficient calculation unit 45 converts a three-dimensional position in the sensor coordinate system into a three-dimensional position in the robot coordinate system from a plurality of sets held in the storage unit 45a. As a conversion coefficient used in the process, a process of calculating a rotation matrix R and a translation vector t in the rigid transformation is performed.
Similarly to the conversion coefficient calculation unit 43 in FIG. 5, the conversion coefficient calculation unit 45 stores the position of the robot hand 1 or the position of the robot hand 1 each time a set of a plane parameter in the robot coordinate system and a plane parameter in the sensor coordinate system is held. Processing for outputting a control signal for instructing switching of the posture to the robot controller 2 is performed.

変換係数算出部４５は図５の変換係数算出部４３と異なり、位置姿勢調整部４６によりロボットハンド１の位置又は姿勢が調整されると、その後、ロボット制御部２によりロボットハンド１の位置又は姿勢が切り替えられる毎に、ロボット座標系での平面パラメータとセンサ座標系での平面パラメータとの組を記憶部４５ａに保持する。
また、変換係数算出部４５は、記憶部４５ａに保持されている複数の組から、センサ座標系での３次元位置をロボット座標系での３次元位置に変換する際に用いる変換係数として、剛体変換における回転行列Ｒ及び並進ベクトルｔを再度算出する処理を実施する。
位置姿勢調整部４６は例えば図９に示す位置姿勢調整回路５６で実現される。
位置姿勢調整部４６は変換係数算出部４５により最初に変換係数が算出された後、ロボット制御部２を制御することで、ロボットハンド１の位置又は姿勢を調整する。Unlike the conversion coefficient calculation unit 43 in FIG. 5, the conversion coefficient calculation unit 45 adjusts the position or posture of the robot hand 1 by the position / orientation adjustment unit 46, and then the position or posture of the robot hand 1 by the robot control unit 2. Each time is switched, a set of a plane parameter in the robot coordinate system and a plane parameter in the sensor coordinate system is held in the storage unit 45a.
The conversion coefficient calculation unit 45 uses a rigid body as a conversion coefficient used when converting a three-dimensional position in the sensor coordinate system to a three-dimensional position in the robot coordinate system from a plurality of sets held in the storage unit 45a. A process of calculating again the rotation matrix R and the translation vector t in the transformation is performed.
The position / orientation adjustment unit 46 is realized by, for example, a position / orientation adjustment circuit 56 shown in FIG.
The position / orientation adjustment unit 46 adjusts the position or orientation of the robot hand 1 by controlling the robot control unit 2 after the conversion coefficient is first calculated by the conversion coefficient calculation unit 45.

この実施の形態４では、図５の変換係数算出装置６に変換係数算出部４５及び位置姿勢調整部４６が適用されている例を説明するが、図１の変換係数算出装置４に適用されているものであってもよい。
図１の変換係数算出装置４に適用される場合、変換係数算出部４５は、図１の変換係数算出部１３と同様に、位置姿勢情報取得部１１により特定されたロボット座標系での３次元位置とセンサ情報取得部１２により取得されたセンサ情報が示すセンサ座標系での３次元位置との組を保持する。
また、変換係数算出部４５は、図１の変換係数算出部１３と同様に、記憶部４５ａに保持されている複数の組から、センサ座標系での３次元位置をロボット座標系での３次元位置に変換する際に用いる変換係数として、剛体変換における回転行列Ｒ及び並進ベクトルｔを算出する。In the fourth embodiment, an example in which the conversion coefficient calculation unit 45 and the position / orientation adjustment unit 46 are applied to the conversion coefficient calculation device 6 of FIG. 5 will be described. However, the fourth embodiment is applied to the conversion coefficient calculation device 4 of FIG. It may be.
When applied to the conversion coefficient calculation device 4 in FIG. 1, the conversion coefficient calculation unit 45 is a three-dimensional robot coordinate system specified by the position and orientation information acquisition unit 11 as in the conversion coefficient calculation unit 13 in FIG. 1. A set of the position and the three-dimensional position in the sensor coordinate system indicated by the sensor information acquired by the sensor information acquisition unit 12 is held.
Similarly to the conversion coefficient calculation unit 13 in FIG. 1, the conversion coefficient calculation unit 45 calculates a three-dimensional position in the sensor coordinate system from a plurality of sets held in the storage unit 45a. A rotation matrix R and a translation vector t in rigid body conversion are calculated as conversion coefficients used when converting into positions.

図８では、変換係数算出装置６の構成要素が専用のハードウェアで実現されるものを想定している。
しかし、変換係数算出装置６の構成要素は、専用のハードウェアで実現されるものに限るものではなく、変換係数算出装置６がソフトウェア、ファームウェア、または、ソフトウェアとファームウェアとの組み合わせで実現されるものであってもよい。
変換係数算出装置６がソフトウェア又はファームウェアなどで実現される場合、変換係数算出部４５が備えている記憶部４５ａを図３に示すコンピュータのメモリ３１上に構成するとともに、位置姿勢情報取得部４１の位置姿勢情報取得処理手順、センサ情報取得部４２のセンサ情報取得処理手順、変換係数算出部４５の変換係数算出処理手順、座標変換部４４の座標変換処理手順及び位置姿勢調整部４６の処理手順をコンピュータに実行させるための変換係数算出プログラムをメモリ３１に格納し、コンピュータのプロセッサ３２がメモリ３１に格納されているプログラムを実行するようにすればよい。In FIG. 8, it is assumed that the components of the transform coefficient calculation device 6 are realized by dedicated hardware.
However, the components of the conversion coefficient calculation device 6 are not limited to those realized by dedicated hardware, and the conversion coefficient calculation device 6 is realized by software, firmware, or a combination of software and firmware. It may be.
When the conversion coefficient calculation device 6 is realized by software or firmware, the storage unit 45a included in the conversion coefficient calculation unit 45 is configured on the memory 31 of the computer shown in FIG. Position and orientation information acquisition processing procedure, sensor information acquisition processing procedure of the sensor information acquisition unit 42, conversion coefficient calculation processing procedure of the conversion coefficient calculation unit 45, coordinate conversion processing procedure of the coordinate conversion unit 44 and processing procedure of the position and orientation adjustment unit 46 A conversion coefficient calculation program to be executed by the computer may be stored in the memory 31, and the computer processor 32 may execute the program stored in the memory 31.

次に動作について説明する。
この実施の形態４では、物体１０が平板である例を説明する。
変換係数算出部４５は、上記実施の形態２における図５の変換係数算出部４３と同様に、図７のステップＳＴ１１〜ＳＴ１７を実行することにより、変換係数として、回転行列Ｒ及び並進ベクトルｔを算出する。Next, the operation will be described.
In the fourth embodiment, an example in which the object 10 is a flat plate will be described.
Similarly to the conversion coefficient calculation unit 43 in FIG. 5 in the second embodiment, the conversion coefficient calculation unit 45 performs steps ST11 to ST17 in FIG. 7 to obtain the rotation matrix R and the translation vector t as conversion coefficients. calculate.

変換係数算出部４５は、回転行列Ｒ及び並進ベクトルｔを算出した後、平板が概ねセンサ５と正対している状況となる平面パラメータπ_ｓ＝（ｎ_ｓ，ｄ_ｓ）を設定する。
即ち、変換係数算出部４５は、センサ５の光軸を表すベクトルｎ_ａと、平板の物体１０における平面の法線ベクトルｎ_ｒとのなす角が例えば５度以内になり、かつ、ロボット座標系の原点から平面までの距離ｄ_ｒがセンサ５の測距可能範囲内になるようなセンサ座標系での平面パラメータπ_ｓ＝（ｎ_ｓ，ｄ_ｓ）を設定する。After calculating the rotation matrix R and the translation vector t, the conversion coefficient calculation unit 45 sets a plane parameter π _s = (n _s , d _s ) in which the flat plate is generally facing the sensor 5.
That is, conversion coefficient calculation unit 45, a vector n _a representative of the optical axis of the sensor 5 becomes within an angle, for example 5 ° with the normal vector n _r of the plane in the object 10 of a flat plate, and the robot coordinate system distance from the origin to the plane d _r plane parameter _{π s = (n s, d} s) of the sensor coordinate system such that the distance measurement range of the sensor 5 is set to.

変換係数算出部４５は、既に算出している回転行列Ｒ及び並進ベクトルｔを用いて、設定したセンサ座標系での平面パラメータπ_ｓ＝（ｎ_ｓ，ｄ_ｓ）をロボット座標系での平面パラメータπ_ｒ＝（ｎ_ｒ，ｄ_ｒ）に変換する。
位置姿勢調整部４６は、物体１０の平面が、変換係数算出部４５により変換された平面パラメータπ_ｒ＝（ｎ_ｒ，ｄ_ｒ）で表される平面となるように、ロボット制御部２を制御することで、ロボットハンド１の位置又は姿勢を調整する。Using the rotation matrix R and the translation vector t that have already been calculated, the conversion coefficient calculation unit 45 converts the plane parameter π _s = (n _s , d _s ) in the set sensor coordinate system into the plane parameter in the robot coordinate system. Convert to π _r = (n _r , d _r ).
The position / orientation adjustment unit 46 controls the robot control unit 2 so that the plane of the object 10 is a plane represented by the plane parameter π _r = (n _r , d _r ) converted by the conversion coefficient calculation unit 45. As a result, the position or posture of the robot hand 1 is adjusted.

変換係数算出部４５は、位置姿勢調整部４６によりロボットハンド１の位置又は姿勢が調整されると、その後、図７のステップＳＴ１１〜ＳＴ１５をＮ回実行することにより、ロボット座標系での平面パラメータπ_ｒ ^ｎとセンサ座標系での平面パラメータπ_ｓ ^ｎとの組をそれぞれ記憶部４５ａに保持する。
変換係数算出部４５は、記憶部４５ａからロボット座標系での平面パラメータπ_ｒ ^ｎとセンサ座標系での平面パラメータπ_ｓ ^ｎとの組をＮ個取得し、Ｎ個の組から回転行列Ｒを再度算出する。
変換係数算出部４５は、回転行列Ｒを再度算出すると、その回転行列Ｒを用いて、並進ベクトルｔを再度算出する。When the position or orientation of the robot hand 1 is adjusted by the position / orientation adjustment unit 46, the conversion coefficient calculation unit 45 then executes Steps ST11 to ST15 of FIG. 7 N times to obtain a plane parameter in the robot coordinate system. hold [pi _r ⁿ and a set of the plane parameters [pi _s ⁿ of the sensor coordinate system to the respective storage unit 45a.
Conversion coefficient calculation unit 45, a set of the plane parameters [pi _s ⁿ at plane parameters [pi _r ⁿ and the sensor coordinate system in the robot coordinate system and N pieces from the storage unit 45a, the rotation matrix R from the N pairs Calculate again.
When the conversion coefficient calculation unit 45 calculates the rotation matrix R again, the conversion coefficient calculation unit 45 calculates the translation vector t again using the rotation matrix R.

以上で明らかなように、この実施の形態４によれば、変換係数算出部４５により変換係数が算出された後、ロボットハンド１の位置又は姿勢を調整する位置姿勢調整部４６を備え、変換係数算出部４５は、位置姿勢調整部４６によりロボットハンド１の位置又は姿勢が調整された後、再度、ロボットハンド１の位置又は姿勢が切り替えられる毎に、位置姿勢情報取得部４１により特定されたロボット座標系での平面パラメータとセンサ情報取得部４２により取得されたセンサ情報が示すセンサ座標系での平面パラメータとの組を保持し、保持している複数の組から、センサ座標系での位置をロボット座標系での位置に変換する際に用いる変換係数を算出するように構成したので、物体１０の姿勢によって、変換係数算出部４５により算出される変換係数の精度が劣化する状況を回避することができる効果を奏する。   As apparent from the above, according to the fourth embodiment, after the conversion coefficient is calculated by the conversion coefficient calculation unit 45, the position / orientation adjustment unit 46 that adjusts the position or posture of the robot hand 1 is provided, and the conversion coefficient The calculation unit 45 adjusts the position or posture of the robot hand 1 by the position / orientation adjustment unit 46, and then the robot identified by the position / orientation information acquisition unit 41 every time the position or posture of the robot hand 1 is switched again. A set of the plane parameter in the coordinate system and the plane parameter in the sensor coordinate system indicated by the sensor information acquired by the sensor information acquisition unit 42 is held, and the position in the sensor coordinate system is determined from the plurality of held sets. Since the conversion coefficient used for conversion to the position in the robot coordinate system is calculated, the conversion coefficient calculation unit 45 calculates the conversion coefficient according to the posture of the object 10. An effect that it is possible to avoid a situation where the precision of the transform coefficients is degraded.

なお、本願発明はその発明の範囲内において、各実施の形態の自由な組み合わせ、あるいは各実施の形態の任意の構成要素の変形、もしくは各実施の形態において任意の構成要素の省略が可能である。   In the present invention, within the scope of the invention, any combination of the embodiments, or any modification of any component in each embodiment, or omission of any component in each embodiment is possible. .

この発明は、センサ座標系での位置をロボット座標系での位置に変換する際に用いる変換係数を算出する変換係数算出装置、変換係数算出方法及び変換係数算出プログラムに適している。   The present invention is suitable for a conversion coefficient calculation device, a conversion coefficient calculation method, and a conversion coefficient calculation program for calculating a conversion coefficient used when converting a position in a sensor coordinate system to a position in a robot coordinate system.

１ ロボットハンド、２ ロボット制御部、３，５ センサ、４，６ 変換係数算出装置、１０ 物体、１１ 位置姿勢情報取得部、１２ センサ情報取得部、１３ 変換係数算出部、１３ａ 記憶部、１４ 座標変換部、２１ 位置姿勢情報取得回路、２２ センサ情報取得回路、２３ 変換係数算出回路、２４ 座標変換回路、３１ メモリ、３２ プロセッサ、３３ 通信回路、３４ 表示装置、４１ 位置姿勢情報取得部、４２ センサ情報取得部、４３ 変換係数算出部、４３ａ 記憶部、４４ 座標変換部、４５ 変換係数算出部、４５ａ 記憶部、４６ 位置姿勢調整部、５１ 位置姿勢情報取得回路、５２ センサ情報取得回路、５３ 変換係数算出回路、５４ 座標変換回路、５５ 変換係数算出回路、５６ 位置姿勢調整回路。   DESCRIPTION OF SYMBOLS 1 Robot hand, 2 Robot control part, 3, 5 Sensor, 4, 6 Conversion coefficient calculation apparatus, 10 Object, 11 Position and orientation information acquisition part, 12 Sensor information acquisition part, 13 Conversion coefficient calculation part, 13a Storage part, 14 coordinate Conversion unit, 21 position and orientation information acquisition circuit, 22 sensor information acquisition circuit, 23 conversion coefficient calculation circuit, 24 coordinate conversion circuit, 31 memory, 32 processor, 33 communication circuit, 34 display device, 41 position and orientation information acquisition unit, 42 sensor Information acquisition unit, 43 conversion coefficient calculation unit, 43a storage unit, 44 coordinate conversion unit, 45 conversion coefficient calculation unit, 45a storage unit, 46 position and orientation adjustment unit, 51 position and orientation information acquisition circuit, 52 sensor information acquisition circuit, 53 conversion Coefficient calculation circuit, 54 coordinate conversion circuit, 55 conversion coefficient calculation circuit, 56 position and orientation adjustment circuit.

Claims (8)

物体が固定されているロボットハンドの位置及び姿勢を示す位置姿勢情報を取得し、前記位置姿勢情報から前記物体のロボット座標系での位置又は前記物体のロボット座標系での平面のパラメータである平面パラメータを特定する位置姿勢情報取得部と、
前記ロボットハンドに固定されている物体を計測するセンサから、前記物体のセンサ座標系での位置を示すセンサ情報又は前記物体のセンサ座標系での平面のパラメータである平面パラメータを示すセンサ情報を取得するセンサ情報取得部と、
前記ロボットハンドの位置又は姿勢が切り替えられる毎に、前記位置姿勢情報取得部により特定されたロボット座標系での位置又は平面パラメータと、前記センサ情報取得部により取得されたセンサ情報が示すセンサ座標系での位置又は平面パラメータとの組を保持し、前記保持している複数の組から、前記センサ座標系での位置を前記ロボット座標系での位置に変換する際に用いる変換係数を算出する変換係数算出部とを備え、
前記変換係数算出部は、前記センサ情報取得部により取得されたセンサ情報が、前記物体の表面における複数の位置又は複数の平面パラメータを示す３次元点群データであれば、前記物体の形状を示す形状データを用いて、前記３次元点群データから前記物体のセンサ座標系での位置又は平面パラメータを算出する変換係数算出装置。 Position / orientation information indicating the position and orientation of the robot hand to which the object is fixed is acquired, and the position of the object in the robot coordinate system or the plane parameter of the object in the robot coordinate system is obtained from the position / orientation information. A position and orientation information acquisition unit for specifying parameters;
Sensor information indicating the position of the object in the sensor coordinate system or sensor information indicating a plane parameter that is a plane parameter in the sensor coordinate system of the object is acquired from a sensor that measures the object fixed to the robot hand. A sensor information acquisition unit,
Each time the position or orientation of the robot hand is switched, the position or plane parameter in the robot coordinate system specified by the position and orientation information acquisition unit, and the sensor coordinate system indicated by the sensor information acquired by the sensor information acquisition unit Conversion for calculating a conversion coefficient used when converting a position in the sensor coordinate system to a position in the robot coordinate system from the plurality of held sets A coefficient calculation unit,
The conversion coefficient calculation unit indicates the shape of the object if the sensor information acquired by the sensor information acquisition unit is three-dimensional point cloud data indicating a plurality of positions or a plurality of plane parameters on the surface of the object. A conversion coefficient calculation device that calculates a position or a plane parameter of the object in a sensor coordinate system from the three-dimensional point cloud data using shape data. 前記変換係数算出部により算出された変換係数を用いて、前記センサ情報取得部により取得されたセンサ情報が示すセンサ座標系での位置を前記ロボット座標系での位置に変換する座標変換部を備えたことを特徴とする請求項１記載の変換係数算出装置。   A coordinate conversion unit that converts a position in the sensor coordinate system indicated by the sensor information acquired by the sensor information acquisition unit into a position in the robot coordinate system using the conversion coefficient calculated by the conversion coefficient calculation unit. The conversion coefficient calculation apparatus according to claim 1, wherein: 前記変換係数算出部は、前記物体が球体であれば、前記物体の形状を示す形状データとして、前記球体の形状を表す球の方程式を用いることを特徴とする請求項１記載の変換係数算出装置。   The conversion coefficient calculation device according to claim 1, wherein if the object is a sphere, the conversion coefficient calculation unit uses a sphere equation representing the shape of the sphere as shape data indicating the shape of the object. . 前記変換係数算出部は、前記物体が平板であれば、前記物体の形状を示す形状データとして、前記平板の形状を表す平面の方程式を用いることを特徴とする請求項１記載の変換係数算出装置。   The conversion coefficient calculation device according to claim 1, wherein if the object is a flat plate, the conversion coefficient calculation unit uses a plane equation representing the shape of the flat plate as shape data indicating the shape of the object. . 前記変換係数算出部は、前記物体の形状が３次元モデルで表される形状であれば、前記物体の形状を示す形状データとして、前記３次元モデルを用いることを特徴とする請求項１記載の変換係数算出装置。   The said conversion coefficient calculation part uses the said three-dimensional model as shape data which shows the shape of the said object, if the shape of the said object is a shape represented with a three-dimensional model. Conversion coefficient calculation device. 前記変換係数算出部により変換係数が算出された後、前記ロボットハンドの位置又は姿勢を調整する位置姿勢調整部を備え、
前記変換係数算出部は、前記位置姿勢調整部により前記ロボットハンドの位置又は姿勢が調整された後、再度、前記ロボットハンドの位置又は姿勢が切り替えられる毎に、前記位置姿勢情報取得部により特定されたロボット座標系での位置又は平面パラメータと前記センサ情報取得部により取得されたセンサ情報が示すセンサ座標系での位置又は平面パラメータとの組を保持し、前記保持している複数の組から、前記センサ座標系での位置を前記ロボット座標系での位置に変換する際に用いる変換係数を算出することを特徴とする請求項１記載の変換係数算出装置。 A position / orientation adjustment unit that adjusts the position or orientation of the robot hand after the conversion coefficient is calculated by the conversion coefficient calculation unit;
The conversion coefficient calculation unit is specified by the position / orientation information acquisition unit every time the position or orientation of the robot hand is switched again after the position / orientation of the robot hand is adjusted by the position / orientation adjustment unit. Holding a set of the position or plane parameter in the robot coordinate system and the position or plane parameter in the sensor coordinate system indicated by the sensor information acquired by the sensor information acquisition unit, from the plurality of held sets, The conversion coefficient calculation apparatus according to claim 1, wherein a conversion coefficient used for converting a position in the sensor coordinate system to a position in the robot coordinate system is calculated. 位置姿勢情報取得部が、物体が固定されているロボットハンドの位置及び姿勢を示す位置姿勢情報を取得し、前記位置姿勢情報から前記物体のロボット座標系での位置又は前記物体のロボット座標系での平面のパラメータである平面パラメータを特定し、
センサ情報取得部が、前記ロボットハンドに固定されている物体を計測するセンサから、前記物体のセンサ座標系での位置を示すセンサ情報又は前記物体のセンサ座標系での平面のパラメータである平面パラメータを示すセンサ情報を取得し、
変換係数算出部が、前記ロボットハンドの位置又は姿勢が切り替えられる毎に、前記位置姿勢情報取得部により特定されたロボット座標系での位置又は平面パラメータと前記センサ情報取得部により取得されたセンサ情報が示すセンサ座標系での位置又は平面パラメータとの組を保持し、前記保持している複数の組から、前記センサ座標系での位置を前記ロボット座標系での位置に変換する際に用いる変換係数を算出し、前記センサ情報取得部により取得されたセンサ情報が、前記物体の表面における複数の位置又は複数の平面パラメータを示す３次元点群データであれば、前記物体の形状を示す形状データを用いて、前記３次元点群データから前記物体のセンサ座標系での位置又は平面パラメータを算出する変換係数算出方法。 A position and orientation information acquisition unit acquires position and orientation information indicating the position and orientation of the robot hand to which the object is fixed, and from the position and orientation information, the position of the object in the robot coordinate system or the robot coordinate system of the object Identify the plane parameters that are the plane parameters of
A sensor information acquisition unit, from a sensor that measures an object fixed to the robot hand, sensor information indicating the position of the object in the sensor coordinate system or a plane parameter that is a parameter of a plane in the sensor coordinate system of the object Sensor information indicating
The position or plane parameter in the robot coordinate system specified by the position / orientation information acquisition unit and the sensor information acquired by the sensor information acquisition unit each time the conversion coefficient calculation unit switches the position or orientation of the robot hand. Is used to convert the position in the sensor coordinate system to the position in the robot coordinate system from the plurality of held sets. If the sensor information obtained by calculating a coefficient and the sensor information acquisition unit is three-dimensional point group data indicating a plurality of positions or a plurality of plane parameters on the surface of the object, shape data indicating the shape of the object A conversion coefficient calculation method for calculating a position or a plane parameter of the object in the sensor coordinate system from the three-dimensional point cloud data. 物体が固定されているロボットハンドの位置及び姿勢を示す位置姿勢情報を取得し、前記位置姿勢情報から前記物体のロボット座標系での位置又は前記物体のロボット座標系での平面のパラメータである平面パラメータを特定する位置姿勢情報取得処理手順と、
前記ロボットハンドに固定されている物体を計測するセンサから、前記物体のセンサ座標系での位置を示すセンサ情報又は前記物体のセンサ座標系での平面のパラメータである平面パラメータを示すセンサ情報を取得するセンサ情報取得処理手順と、
前記ロボットハンドの位置又は姿勢が切り替えられる毎に、前記位置姿勢情報取得処理手順により特定されたロボット座標系での位置又は平面パラメータと前記センサ情報取得処理手順により取得されたセンサ情報が示すセンサ座標系での位置又は平面パラメータとの組を保持し、前記保持している複数の組から、前記センサ座標系での位置を前記ロボット座標系での位置に変換する際に用いる変換係数を算出し、前記センサ情報取得処理手順により取得されたセンサ情報が、前記物体の表面における複数の位置又は複数の平面パラメータを示す３次元点群データであれば、前記物体の形状を示す形状データを用いて、前記３次元点群データから前記物体のセンサ座標系での位置又は平面パラメータを算出する変換係数算出処理手順とをコンピュータに実行させるための変換係数算出プログラム。 Position / orientation information indicating the position and orientation of the robot hand to which the object is fixed is acquired, and the position of the object in the robot coordinate system or the plane parameter of the object in the robot coordinate system is obtained from the position / orientation information. Position and orientation information acquisition processing procedure for specifying parameters;
Sensor information indicating the position of the object in the sensor coordinate system or sensor information indicating a plane parameter that is a plane parameter in the sensor coordinate system of the object is acquired from a sensor that measures the object fixed to the robot hand. Sensor information acquisition processing procedure to
Each time the position or orientation of the robot hand is switched, the position or plane parameter in the robot coordinate system specified by the position and orientation information acquisition processing procedure and the sensor coordinates indicated by the sensor information acquired by the sensor information acquisition processing procedure Holding a set of positions or plane parameters in the system, and calculating a conversion coefficient to be used when converting the position in the sensor coordinate system to the position in the robot coordinate system from the plurality of held sets. If the sensor information acquired by the sensor information acquisition processing procedure is three-dimensional point cloud data indicating a plurality of positions or a plurality of plane parameters on the surface of the object, shape data indicating the shape of the object is used. A conversion coefficient calculation processing procedure for calculating a position or plane parameter of the object in the sensor coordinate system from the three-dimensional point cloud data. Conversion coefficient calculation program to be executed by the Yuta.

Images