JP3123859B2 - Robot position / posture recognition device - Google Patents

Description

【発明の詳細な説明】DETAILED DESCRIPTION OF THE INVENTION

【０００１】[0001]

【産業上の利用分野】本発明はロボットに関する。更に
詳述すると、本発明はロボットの位置・姿勢を認識する
方法及びその装置に関するものである。BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a robot. More specifically, the present invention relates to a method and apparatus for recognizing the position and orientation of a robot.

【０００２】[0002]

【従来の技術】一般にロボットには多種の型式のものが
提供されているが、いずれの型式のロボットでもロボー
トアーム先端の位置、姿勢を認識することは、ロボット
を駆動制御する上で非常に重要である。2. Description of the Related Art Generally, various types of robots are provided. However, in any type of robot, recognizing the position and posture of the tip of the robot arm is very difficult in controlling the driving of the robot. is important.

【０００３】従来、ロボットの位置、姿勢を認識する方
法としては、（１）センサもパターン認識能力もないも
の、（２）センサはあるがパターン認識能力のないも
の、（３）センサとパターン認識能力のあるものに大別
される。各方法は、それぞれ利点と欠点があり、その使
用の目的に応じて使い分けられているが、コンピュータ
の発達により、比較的安価にかつ確実に位置の推定がで
きる上記（１）の方法がよく採用されている。Conventionally, methods for recognizing the position and posture of a robot include (1) a method having neither a sensor nor pattern recognition ability, (2) a method having a sensor but no pattern recognition ability, and (3) a sensor and pattern recognition. They are roughly divided into those with the ability. Each method has advantages and disadvantages, and is used properly depending on the purpose of use. However, with the development of computers, the above method (1), which can estimate the position relatively inexpensively and reliably, is often adopted. Have been.

【０００４】これは、初期位置を与えておき、その後は
モータ等の回転数から現在の位置を推定する方法であっ
て、具体的にはモータの回転数をエンコーダで検出して
コンピュータ内に取り込み、コンピュータにより予め与
えられていた初期位置及びエンコーダからの検出パルス
を基に所定の計算をしてロボットの位置を推定するもの
である。This is a method in which an initial position is given, and then the current position is estimated from the rotation speed of a motor or the like. Specifically, the rotation speed of the motor is detected by an encoder and taken into a computer. The position of the robot is estimated by performing a predetermined calculation on the basis of the initial position given in advance by the computer and the detection pulse from the encoder.

【０００５】[0005]

【発明が解決しようとする課題】しかしながら、上述し
た従来のロボットの位置・姿勢認識方法にあっては、コ
ンピュータの計算周期毎にしか位置認識が行えなかった
ため、位置認識を行う際にはロボットアームを低速に移
動させたり、場合によってはロボットアームを停止させ
る必要があり、認識に時間がかかり過ぎるという欠点が
あった。However, in the above-described conventional method for recognizing the position and posture of a robot, the position can be recognized only at each calculation cycle of the computer. It is necessary to move the robot at a low speed or stop the robot arm in some cases, and there is a disadvantage that it takes too much time for recognition.

【０００６】本発明は、上述した従来技術の欠点を解消
し、位置、姿勢認識を短時間におこなうことができるロ
ボットの位置・姿勢認識装置を提供することを目的とす
る。SUMMARY OF THE INVENTION An object of the present invention is to provide a robot position / posture recognizing apparatus which can solve the above-mentioned disadvantages of the prior art and can perform position / posture recognition in a short time.

【０００７】[0007]

【０００８】[0008]

【課題を解決するための手段】かかる目的を達成するた
め、請求項１記載のロボットの位置・姿勢認識装置は、
アームの角度を検出して検出信号を出力するセンサと、
該センサからの検出信号を位相検出して検波信号を得る
位相検出回路、前記位相検出回路からの検波信号を可逆
計数する可逆カウンタ、ロボットアームの先端に設けら
れた視覚認識装置がロボット制御装置の計算周期とは関
係なく実際に撮像を行う撮像タイミングで発生するシャ
ッター閉信号により形成される位置認識信号により前記
可逆カウンタの計数値をセットするレジスタからなる検
出回路と、各アーム毎の検出回路における可逆カウンタ
の計数値を読み込みその計数値からアームの移動量を算
出するとともに、前記各アーム毎の検出回路におけるレ
ジスタにセットされた値を読み込みそのセット値及び撮
像データから得られるデータとロボットの座標系との関
係を求める演算処理装置とを備えるようにしている。In order to achieve the above object, a robot position / posture recognition apparatus according to claim 1 is provided.
A sensor that detects the angle of the arm and outputs a detection signal,
A phase detection circuit that detects a phase of a detection signal from the sensor to obtain a detection signal, a reversible counter that reversibly counts the detection signal from the phase detection circuit, and a visual recognition device provided at the tip of the robot arm is a robot control device. Sharps that occur at the timing of actual imaging regardless of the calculation cycle
A detection circuit consisting of a register for setting the count value of the reversible counter based on a position recognition signal formed by a shutter close signal, and reading the count value of the reversible counter in the detection circuit for each arm to determine the amount of movement of the arm from the count value. And an arithmetic processing unit that reads a value set in a register in a detection circuit of each arm and obtains a relationship between the set value and data obtained from imaging data and a coordinate system of the robot. .

【０００９】さらに、請求項２記載の発明に係るロボッ
トの位置・姿勢認識装置は、アームの角度を検出して検
出信号を出力するセンサと、前記センサからの検出信号
を位相検出して検波信号を得る位相検出回路、前記位相
検出回路からの検波信号を可逆計数する可逆カウンタ、
ロボットアームの先端に設けられた視覚認識装置がロボ
ット制御装置の計算周期とは関係なく実際に撮像を行う
撮像タイミングで発生するシャッター閉信号により形成
される認識信号により前記可逆カウンタの計数値をセッ
トするレジスタ、撮像タイミングで発生する認識信号に
よりセット状態になる判定信号保持部からなる検出回路
と、各アーム毎の検出回路における可逆カウンタの計数
値を読み込みその計数値からアームの移動量を算出する
とともに、前記判定信号保持部がセット状態のときに前
記レジスタにセットされた値を読み込み、そのセット値
及び撮像データから得られるデータとロボットの座標系
との関係を求める演算処理装置とを備えるようにしてい
る。A robot position / posture recognition apparatus according to a second aspect of the present invention includes a sensor for detecting an angle of an arm and outputting a detection signal, and a detection signal for detecting a phase of the detection signal from the sensor. Phase detection circuit, a reversible counter for reversibly counting the detection signal from the phase detection circuit,
The visual recognition device provided at the end of the robot arm actually forms an image regardless of the calculation cycle of the robot controller.
A detection circuit including a register for setting the count value of the reversible counter based on the recognition signal to be detected, a determination signal holding unit that is set to a set state by a recognition signal generated at an imaging timing, and a count value of the reversible counter in the detection circuit for each arm Is read from the count value, the amount of movement of the arm is calculated, and the value set in the register is read when the determination signal holding unit is in the set state, and the data obtained from the set value and the imaging data and the coordinates of the robot are read. And an arithmetic processing unit for obtaining a relationship with the system.

【００１０】[0010]

【００１１】[0011]

【作用】 請求項１記載の発明では、一つのセンサに対し
て、上記位相検出回路と、上記可逆カウンタと、上記レ
ジスタとからなる検出回路を設け、かつ各センサに対し
てそれぞれ検出回路を設け、かつ前記各検出回路におけ
る可逆カウンタの計数値を演算処理装置が読み込みその
計数値からアームの移動量を算出する。また、演算処理
装置は、前記各検出回路におけるレジスタにセットされ
た値を読み込みそのセット値及び撮像データとから得ら
れるデータとロボットの座標系との関係を求めることが
できる。 [Action] In the present invention of claim 1, wherein, for one single sensor, arranged and the phase detection circuit, and said reversible counter is provided with a detection circuit consisting of the register, and each detection circuit for each sensor The arithmetic processing unit reads the count value of the reversible counter in each of the detection circuits, and calculates the amount of movement of the arm from the count value. Further, the arithmetic processing unit can read a value set in a register in each of the detection circuits, and obtain a relationship between data obtained from the set value and the imaging data and a coordinate system of the robot.

【００１２】請求項２記載の発明では、請求項１記載の
発明における検出回路に、判定信号保持部を付加し、認
識信号が入力されたことをセットしておいて、演算処理
装置に認識信号が入力されたことを知らせるようにした
ものである。これにより、演算処理装置とは同期しない
認識信号でも確実に各レジスタのセット値及び撮像デー
タから得られるデータとロボットの座標系との関係を求
めることができる。According to a second aspect of the present invention, a determination signal holding unit is added to the detection circuit according to the first aspect of the present invention, and it is set that the recognition signal has been input, and the recognition signal is sent to the arithmetic processing unit. Is input to inform the user of the input. This makes it possible to reliably determine the relationship between the set value of each register and the data obtained from the imaging data and the coordinate system of the robot even with a recognition signal that is not synchronized with the arithmetic processing device.

【００１３】[0013]

【実施例】以下、本発明の構成を図面に示す実施例に基
づいて詳細に説明する。DESCRIPTION OF THE PREFERRED EMBODIMENTS The construction of the present invention will be described below in detail with reference to the embodiments shown in the drawings.

【００１４】図１に本発明のロボットの位置・姿勢認識
方法の実施例を実現する装置の一例を示す。該図におい
て、符号１はスカラ形ロボットである。スカラ形ロボッ
ト１は、主に、ベース２、第１アーム３、第２アーム
４、第１モータ５、第１減速機５Ａ、第２モータ６、第
２減速機６Ａ、第３モータ７、第４モータ８、第３減速
機８Ａを備えている。また、ベース２には第１アーム３
が回動可能に固定されており、この第１アーム３の先端
には第２アーム４が回動可能に固定されている。ベース
２には、第１モータ５及び第４モータ８が固定されてい
る。第１モータ５は、減速機５Ａを介して第１アーム３
を回転させるようになっている。第４モータ８は、プリ
ー９ａ，９ｂに掛け渡されたベルト１０ａと、プリー９
ｃ，９ｄに掛け渡されたベルト１０ｂとにより、スプラ
インシャフト先端把持部１１ａを所望角度だけ回転させ
られるようになっている。第２モータ６は第１アーム３
に固定されており、第２モータ６で第２アーム４を回転
できるようになっている。第２モータ６の先端内部には
第３モータ７が内蔵固定されており、第３モータ７はプ
リー１２ａ，１２ｂに掛け渡されたベルト１３によりボ
ールネジ１４を回転させることにより、スプラインシャ
フト１１ｂを図示上下動できるようになっている。ま
た、第１モータ５、第２モータ６、第３モータ７、及び
第４モータ８には、エンコーダ１５、１６、１７及び１
８がそれぞれ設けられており、各エンコーダ１５、１
６、１７及び１８は各モータ５、６、７及び８の回転数
を検出できるようになっている。これらエンコーダ１
５、１６は、第１アーム３及び第２アーム４の角度を検
出するセンサとして使用される。もちろん、エンコーダ
１７はスプラインシャフト先端把持部１１ａの上下動の
距離を、エンコーダ１８はスプラインシャフト先端把持
部１１ａの角度を測定するセンサとして使用される。FIG. 1 shows an example of an apparatus for realizing an embodiment of a method for recognizing the position and orientation of a robot according to the present invention. In the figure, reference numeral 1 denotes a scalar robot. The SCARA robot 1 mainly includes a base 2, a first arm 3, a second arm 4, a first motor 5, a first reduction gear 5A, a second motor 6, a second reduction gear 6A, a third motor 7, A four motor 8 and a third reduction gear 8A are provided. The base 2 has a first arm 3
Is rotatably fixed, and a second arm 4 is rotatably fixed to the end of the first arm 3. A first motor 5 and a fourth motor 8 are fixed to the base 2. The first motor 5 is connected to the first arm 3 via a speed reducer 5A.
Is to be rotated. The fourth motor 8 includes a belt 10a stretched around pulleys 9a and 9b,
The spline shaft tip grasping portion 11a can be rotated by a desired angle by the belt 10b stretched around c and 9d. The second motor 6 is the first arm 3
, And the second arm 4 can be rotated by the second motor 6. A third motor 7 is fixed inside the tip end of the second motor 6, and the spline shaft 11b is illustrated by rotating a ball screw 14 by a belt 13 stretched over pulleys 12a and 12b. It can be moved up and down. The first motor 5, the second motor 6, the third motor 7, and the fourth motor 8 include encoders 15, 16, 17, and 1 respectively.
8 are provided, and each encoder 15, 1
The motors 6, 17, and 18 can detect the rotation speeds of the motors 5, 6, 7, and 8, respectively. These encoders 1
Reference numerals 5 and 16 are used as sensors for detecting the angles of the first arm 3 and the second arm 4. Of course, the encoder 17 is used as a sensor for measuring the vertical movement distance of the spline shaft tip gripper 11a, and the encoder 18 is used as a sensor for measuring the angle of the spline shaft tip gripper 11a.

【００１５】このスカラ形ロボット１はロボット制御装
置２０に電気的に接続されており、ロボット制御装置２
０の制御下に、各モータ５、６、７及び８を回転させ、
かつ各エンコーダ１５、１６、１７及び１８からの検出
信号で各モータ５、６、７及び８の回転を駆動制御する
とともに、第２アーム４の先端位置を推定できるように
なっている。The SCARA type robot 1 is electrically connected to a robot controller 20 and a robot controller 2
Under the control of 0, the motors 5, 6, 7 and 8 are rotated,
In addition, the rotation of each of the motors 5, 6, 7, and 8 is controlled by the detection signals from the encoders 15, 16, 17, and 18, and the tip position of the second arm 4 can be estimated.

【００１６】ロボット制御装置２０は、各種の演算処理
や各種の制御を実行する演算処理装置（ＣＰＵ）２１
と、バス２２と、メモリー２３と、サーボモータコント
ロールインターフェース２４ａ，２４ｂ，２４ｃ，２４
ｄと、検出回路２５ａ，２５ｂ，２５ｃ，２５ｄとを備
えている。ＣＰＵ２１には、バス２２を介してメモリー
２３と、サーボモータコントロールインターフェース２
４ａ，２４ｂ，２４ｃ，２４ｄとが接続されるととも
に、検出回路２５ａ，２５ｂ，２５ｃ，２５ｄが接続さ
れている。サーボモータコントロールインターフェース
２４ａ，２４ｂ，２４ｃ及び２４ｄは、図示しないが、
各モータ５，６，７及び８に電気的にそれぞれ接続され
ており、各モータ５，６，７及び８の回転をＣＰＵ２１
の指令に基づいて回転駆動制御する。検出回路２５ａ，
２５ｂ，２５ｃ及び２５ｄは、全て同一回路構成となっ
ているので、検出回路２５ａのみの構成について説明す
る。検出回路２５ａは、エンコーダ１５からの検出信号
ＳＡ，ＳＢを取り込み、これを検波して検波信号Ｓａ，
Ｓｂの内の一つを出力できる位相検出回路２５１と、位
相検出回路２５１からの検波信号Ｓａ，Ｓｂの内の一つ
を差引計数する可逆カウンタ２５２と、この可逆カウン
タ２５２の計数値をＣＰＵ２１に読み込ませるスリース
テートバッファ２５３と、位置認識信号ＳＣが入力され
たときに前記位置認識信号ＳＣの微分信号Ｓｃを形成す
る微分回路２５４と、この微分信号Ｓｃが入力されたと
きに、その入力時点の可逆カウンタ２５２の計数値をセ
ットするレジスタ２５５と、このレジスタ２５５のセッ
ト値をＣＰＵ２１に読み込ませるスリーステートバッフ
ァ２５６と、微分信号Ｓｃが入力されるとセットされＣ
ＰＵ２１によりリセットされる判定信号保持部２５７と
から構成されている。The robot controller 20 includes an arithmetic processing unit (CPU) 21 for executing various arithmetic processes and various controls.
, Bus 22, memory 23, servo motor control interfaces 24a, 24b, 24c, 24
d and detection circuits 25a, 25b, 25c, and 25d. The CPU 21 has a memory 23 via a bus 22 and a servo motor control interface 2.
4a, 24b, 24c, and 24d are connected, and detection circuits 25a, 25b, 25c, and 25d are connected. Servo motor control interfaces 24a, 24b, 24c and 24d are not shown,
The motors 5, 6, 7, and 8 are electrically connected to each other, and the rotation of each motor 5, 6, 7, and 8 is controlled by the CPU 21.
The rotational drive is controlled based on the instruction. The detection circuit 25a,
Since 25b, 25c, and 25d all have the same circuit configuration, only the configuration of the detection circuit 25a will be described. The detection circuit 25a takes in the detection signals SA and SB from the encoder 15 and detects them to detect the detection signals Sa and SB.
A phase detection circuit 251 capable of outputting one of Sb, a reversible counter 252 for subtracting and counting one of the detection signals Sa and Sb from the phase detection circuit 251, and a count value of the reversible counter 252 to the CPU 21; A three-state buffer 253 to be read, a differentiating circuit 254 for forming a differential signal Sc of the position recognition signal SC when the position recognition signal SC is input, and a differential circuit Sc at the time of input when the differential signal Sc is input. A register 255 for setting the count value of the reversible counter 252, a three-state buffer 256 for reading the set value of the register 255 into the CPU 21, and a register C which is set when the differential signal Sc is input.
And a determination signal holding unit 257 that is reset by the PU 21.

【００１７】図２に、ロボット制御装置２０の検出回路
２５ａの具体的構成の一例を示す。この図は、特に、ス
リーステートバッファ２５３，２５６、微分回路２５４
及び判定信号保持部２５７の構成例について説明してい
る。FIG. 2 shows an example of a specific configuration of the detection circuit 25a of the robot controller 20. This diagram shows, in particular, the three-state buffers 253 and 256 and the differentiating circuit 254.
The configuration example of the determination signal holding unit 257 has been described.

【００１８】位相検出回路２５１は、エンコーダ１５か
らの検出信号ＳＡ，ＳＢ及びクロックφ₀ を取り込み、
検波信号Ｓａ又はＳｂを可逆カウンタ２５２に供給でき
るようになっている。可逆カウンタ２５２は、検波信号
Ｓａ又はＳｂを計数する回路である。可逆カウンタ２５
２の出力は、スリーステートバッファ２５３及びレジス
タ２５５に供給されるようになっている。スリーステー
トバッファ２５３は、ＣＰＵ２１からバス２２を介して
与えられるリード信号Ｒ₀ によって動作し、可逆カウン
タ２５２の計数値出力をバス２２のデータバスに供給す
る。これにより、ＣＰＵ２１は、可逆カウンタ２５２の
計数値を取り込むことができる。The phase detection circuit 251 takes in the detection signals SA and SB from the encoder 15 and the clock φ ₀ ,
The detection signal Sa or Sb can be supplied to the reversible counter 252. The reversible counter 252 is a circuit that counts the detection signal Sa or Sb. Reversible counter 25
2 is supplied to the three-state buffer 253 and the register 255. The three-state buffer 253 is operated by a read signal R ₀ given from the CPU 21 via the bus 22, and supplies the count value output of the reversible counter 252 to the data bus of the bus 22. Thus, the CPU 21 can capture the count value of the reversible counter 252.

【００１９】位置認識信号ＳＣは、微分回路２５４に入
力される。微分回路２５４は、二つのＤ型フリップフロ
ップＦＦ₁ ，ＦＦ₂ と、一つのナンド回路ＮＡとを備
え、位置認識信号ＳＣをフリップフロップＦＦ₁ のＤ入
力端子に入力し、フリップフロップＦＦ₁ のＱ出力端子
２５８をフリップフロップＦＦ₂ のＤ入力端子に接続
し、フリップフロップＦＦ₁ ，ＦＦ₂ のクロック端子に
クロックφ ₁ を与え、フリップフロップＦＦ ₁ の出力端
子２５８、クリップフロップＦＦ ₂ の反転出力端子２５
９からの信号をナンド回路ＮＡの両入力端子に供給し、
ナンド回路ＮＡの出力端子から、微分信号Ｓｃを得るよ
うになっている。この微分信号Ｓｃは、レジスタ２５５
と、判定信号保持部２５７に入力されるようになってい
る。The position recognition signal SC is input to a differentiating circuit 254. Differentiating circuit 254 includes two D-type flip-flop FF _1, FF _2, and a single NAND circuit NA, enter the position identification signals SC to the D input terminal of the flip-flop FF _1, the flip-flop FF ₁ Q Output terminal
Connect 258 to the D input terminal of the flip-flop FF _2, clocked phi ₁ to the clock terminal of the flip-flop FF _1, FF _2, the output of the flip-flop FF ₁
Child 258, the inverting output terminal of the clip flop FF ₂ 25
9 is supplied to both input terminals of the NAND circuit NA,
The differential signal Sc is obtained from the output terminal of the NAND circuit NA. This differential signal Sc is stored in the register 255
Is input to the determination signal holding unit 257.

【００２０】レジスタ２５５は、微分信号Ｓｃが入力さ
れた時点の可逆カウンタ２５２の計数値をセットし、こ
れをスリーステートバッファ２５６に供給している。ス
リーステートバッファ２５６は、ＣＰＵ２１からバス２
２を介して与えられるリード信号Ｒ₁ により動作し、レ
ジスタ２５５の値をバス２２のデータバスに供給できる
ようになっている。The register 255 sets the count value of the reversible counter 252 at the time when the differential signal Sc is input, and supplies this to the three-state buffer 256. The three-state buffer 256 is connected to the bus 2 from the CPU 21.
It operates in response to a read signal R ₁ provided through the bus 2, and can supply the value of the register 255 to the data bus of the bus 22.

【００２１】また、判定信号保持部２５７は、フリップ
フロップＦＦ₃ と、アンド回路ＡＤと、バッファＢＦと
を備え、微分信号ＳｃがフリップフロップＦＦ₃ に入力
されると、フリップフロップＦＦ₃ がセットされ、アン
ド回路ＡＤからの出力信号があるとリセットされるよう
になっている。フリップフロップＦＦ₃ の出力端子はバ
ッファＢＦを介してバス２２のデータバスに接続されて
おり、このバッファＢＦはＣＰＵ２１からのリード信号
Ｒ₂ により動作して、フリップフロップＦＦ₃の出力信
号をバス２２のデータバスに供給できるようになってい
る。また、このバッファＢＦの出力はアンド回路ＡＤの
一方の入力端子に供給されており、またＣＰＵ２１から
バス２２を介して与えられるライト信号Ｗ₀ はアンド回
路ＡＤの他方の入力端子に供給されている。そして、ア
ンド回路ＡＤは、バッファＢＦの出力と、ライト信号Ｗ
₀ との双方が論理積されたときに、クリア信号をフリッ
プフロップＦＦ₃のクリア端子に供給できるようになっ
ている。Further, the determination signal holding unit 257 includes a flip-flop FF _3, and an AND circuit AD, a buffer BF, the differential signal Sc is inputted to the flip-flop FF _3, is set flip-flop FF ₃ , Is reset when there is an output signal from the AND circuit AD. The output terminal of the flip-flop FF ₃ is connected to the data bus of the bus 22 through a buffer BF, this buffer BF is operated by the read signal R ₂ from the CPU 21, the output signal of the flip-flop FF ₃ bus 22 Data bus. The output of the buffer BF is supplied to one input terminal of the AND circuit AD, also write signal W ₀ supplied through the bus 22 from the CPU21 is supplied to the other input terminal of the AND circuit AD . The AND circuit AD outputs the output of the buffer BF and the write signal W
When both the ₀ is logical, so as to supply a clear signal to the clear terminal of the flip-flop FF _3.

【００２２】図３に上記位相検出回路２５１の具体的構
成の一例を示す。該図において、位相検出回路２５１
は、四つのフリップフロップＦＦ₂₁，ＦＦ₂₂，ＦＦ₂₃，
ＦＦ₂₄と、反転入力端子を少なくとも一つを有する四つ
のアンド回路ＡＤ₂₁，ＡＤ₂₂，ＡＤ₂₃，ＡＤ₂₄と、二つ
のオア回路ＯＲ₂₁，ＯＲ₂₂とを備えている。FIG. 3 shows an example of a specific configuration of the phase detection circuit 251. In the figure, the phase detection circuit 251
Includes four flip-flops _{FF 21, FF 22, FF 23} ,
And FF _24, and four AND circuits _{AD 21, AD 22, AD 23} , AD 24 having at least one inverting input terminal, and a two OR circuit OR _21, OR _22.

【００２３】エンコーダからの信号ＳＡは、フリップフ
ロップＦＦ₂₁のＤ入力端子に入力し、フリップフロップ
ＦＦ₂₁の出力端子をフリップフロップＦＦ₂₂のＤ入力端
子に接続し、フリップフロップＦＦ₂₁，ＦＦ₂₂のクロッ
ク端子にクロックφ₀ を与えられるようになっている。
フリップフロップＦＦ₂₁の出力端子からの信号は、アン
ド回路ＡＤ₂₁，ＡＤ₂₄，ＡＤ₂₇，ＡＤ₂₈の入力端子に供
給するとともに、アンド回路ＡＤ₂₂，ＡＤ₂₃，ＡＤ₂₅，
ＡＤ₂₆の反転入力端子に供給できるようになっている。
フリップフロップＦＦ₂₂の出力端子からの信号は、アン
ド回路ＡＤ₂₁，ＡＤ₂₂，ＡＤ₂₅，ＡＤ₂₇の入力端子に供
給するとともに、アンド回路ＡＤ₂₃，ＡＤ₂₄，ＡＤ₂₆，
ＡＤ₂₇の反転入力端子に供給できるようになっている。
エンコーダからの信号ＳＢは、フリップフロップＦＦ₂₃
のＤ入力端子に入力し、フリップフロップＦＦ₂₃の出力
端子をフリップフロップＦＦ₂₄のＤ入力端子に接続し、
フリップフロップＦＦ₂₃，ＦＦ₂₄のクロック端子にクロ
ックφ₀ を与えられるようになっている。フリップフロ
ップＦＦ₂₃の出力端子からの信号は、アンド回路Ａ
Ｄ₂₃，ＡＤ₂₄，ＡＤ₂₅，ＡＤ₂₈の入力端子に供給すると
ともに、アンド回路ＡＤ₂₁，ＡＤ₂₂，ＡＤ₂₆，ＡＤ₂₇の
反転入力端子に供給できるようになっている。フリップ
フロップＦＦ₂₄の出力端子からの信号は、アンド回路Ａ
Ｄ₂₁，ＡＤ₂₄，ＡＤ₂₅，ＡＤ₂₆の入力端子に供給すると
ともに、アンド回路ＡＤ₂₂，ＡＤ₂₃，ＡＤ₂₇，ＡＤ₂₈の
反転入力端子に供給できるようになっている。アンド回
路ＡＤ₂₁，ＡＤ₂₂，ＡＤ₂₃，ＡＤ₂₄の出力端子はオア回
路ＯＲ₂₁の入力端子に接続されており、このオア回路Ｏ
Ｒ₂₁は各入力端子に入力された信号の論理積がとられる
と検波信号Ｓａが出力端子から出力されるようになって
いる。アンド回路ＡＤ₂₅，ＡＤ₂₆，ＡＤ₂₇，ＡＤ₂₈の出
力端子はオア回路ＯＲ₂₂の入力端子に接続されており、
このオア回路ＯＲ₂₂は各入力端子に入力された信号の論
理積がとられると検波信号Ｓａが出力端子から出力され
るようになっている。The signal SA from the encoder is input to the D input terminal of the flip-flop FF _21, connects the output terminal of the flip-flop FF ₂₁ to the D input terminal of the flip-flop FF _22, the flip-flop FF _21, FF ₂₂ The clock φ ₀ can be supplied to the clock terminal.
Signal from the output terminal of the flip-flop FF ₂₁ supplies to the input terminal of the AND circuit _{AD 21, AD 24, AD 27} , AD 28, the AND circuit _{AD 22, AD 23, AD 25} ,
AD ₂₆ can be supplied to the inverting input terminal.
Together with the signal from the output terminal of the flip-flop FF ₂₂ is supplied to an input terminal of the AND circuit _{AD 21, AD 22, AD 25} , AD 27, the AND circuit _{AD 23, AD 24, AD 26} ,
AD ₂₇ can be supplied to the inverting input terminal.
The signal SB from the encoder is supplied to the flip-flop FF ₂₃
And the output terminal of the flip-flop FF ₂₃ is connected to the D input terminal of the flip-flop FF ₂₄ ,
The clock φ ₀ is supplied to the clock terminals of the flip-flops FF ₂₃ and FF ₂₄ . Signal from the output terminal of the flip-flop FF ₂₃ is the AND circuit A
Supplies to the input terminal of the _{D 23, AD 24, AD 25} , AD 28, can be supplied to the inverting input terminal of the AND circuit _{AD 21, AD 22, AD 26} , AD 27. Signal from the output terminal of the flip-flop FF ₂₄ is the AND circuit A
Supplies to the input terminal of the _{D 21, AD 24, AD 25} , AD 26, can be supplied to the inverting input terminal of the AND circuit _{AD 22, AD 23, AD 27} , AD 28. The output terminals of the AND circuits AD ₂₁ , AD ₂₂ , AD ₂₃ , and AD ₂₄ are connected to the input terminal of the OR circuit OR _21.
R ₂₁ is adapted to the detection signal Sa and the logical product of the signals input to the input terminal is taken is output from the output terminal. The output terminals of the AND circuits AD ₂₅ , AD ₂₆ , AD ₂₇ , and AD ₂₈ are connected to the input terminals of the OR circuit OR ₂₂ ,
The OR circuit OR ₂₂ is adapted to the detection signal Sa and the logical product of the signals input to the input terminal is taken is output from the output terminal.

【００２４】図４にロボットの位置・姿勢認識方法を説
明するための説明図を示す。尚、図４はロボット１を上
から見たものである。ベース２には第１アーム３が回動
可能に固定されており、この固定点を原点Ｏ₁ として当
該原点Ｏ₁ を通ってＸ軸とＹ軸とがとられている。ま
た、第１アーム３には第２アーム４が回動可能に固定さ
れており、この固定点を移動点Ｏ₂ とする。そして、第
１アーム３の長さを原点Ｏ₁ と移動点Ｏ₂ との間の長さ
Ｌ₁ とし、第２アーム４の長さを移動点Ｏ₂ と移動点Ｏ
₃ との間の長さＬ₂ とし、かつスプラインシャフト先端
把持部１１ａに固定した腕１１ｃの先端に取付けたカメ
ラ３０と移動点Ｏ₂までの距離をＬ₂’とする。また、ワ
ーク３５のＸ軸、Ｙ軸に対する位置をＸｔ、Ｙｔとし、
かつカメラ３０の座標におけるカメラ３０とワーク３５
との位置関係をＸｃ、Ｙｃとする。FIG. 4 is an explanatory diagram for explaining a method of recognizing the position and posture of the robot. FIG. 4 shows the robot 1 viewed from above. The base 2 has a first arm 3 is rotatably fixed, through the origin O ₁ and the X-axis and Y-axis are taken this fixed point as the origin O _1. Further, the first arm 3 and second arm 4 is rotatably fixed to the fixed point and the moving point O _2. Then, the length of the first arm 3 and the length L ₁ between the origin O ₁ and the mobile point O _2, moving point the length of the second arm 4 and the moving point O ₂ O
₃ and the length L ₂ between, and the distance between the camera 30 attached to the tip of the arm 11c fixed to the spline shaft distal clasps 11a to move point O ₂ and L ₂ '. The positions of the work 35 with respect to the X axis and the Y axis are Xt and Yt, respectively.
And the camera 30 and the workpiece 35 at the coordinates of the camera 30
And Xc and Yc.

【００２５】上述したようなロボットの位置・姿勢認識
装置の作用を図１ないし図４を基に図５及び図６を参照
しながら以下に説明する。なお、図５はＣＰＵ２１の処
理フローである。また、図６は位相検出回路２５１のタ
イミングチャートであり、横軸に時間を、縦軸に各信号
をそれぞれ示している。また、この実施例では、カメラ
３０は、本来、移動点Ｏ₃ を中心として回動可能であ
り、かつＸＹ平面に対して直交座標系方向に移動可能で
あるが、説明を簡単にするためにカメラ３０は第２アー
ム４に固定した状態で説明することにする。したがっ
て、ロボット制御装置２０には、エンコーダ１５及びエ
ンコーダ１６からの各検出信号ＳＡ，ＳＢを処理するも
のとする。The operation of the above-described robot position / posture recognition apparatus will be described below with reference to FIGS. 1 to 4 and FIGS. 5 and 6. FIG. 5 is a processing flow of the CPU 21. FIG. 6 is a timing chart of the phase detection circuit 251. The horizontal axis indicates time, and the vertical axis indicates each signal. Further, in this embodiment, the camera 30 is essentially a rotatable about the moving point O _3, and it is movable in the orthogonal coordinate system direction to the XY plane, in order to simplify the description The camera 30 will be described as being fixed to the second arm 4. Therefore, the robot control device 20 processes the detection signals SA and SB from the encoder 15 and the encoder 16.

【００２６】まず、ロボット制御装置２０のＣＰＵ２１
からの指令に基づき、サーボモータコントロールインタ
ーフェース２４ａ及び２４ｂは、第１モータ５及び第２
モータ６を回転駆動する。これにより、第１モータ５が
回転して第１減速機５Ａを介して第１アーム３を第１モ
ータ５の回転方向に応じた方向に移動させる。また、第
２モータ６が回転して第２減速機６Ａを介して第２アー
ム４を第２モータ６の回転方向に応じた方向に移動させ
る。これら第１モータ５及び第２モータ６の回転数及び
回転方向はエンコーダ１５及びエンコーダ１６により検
出される。エンコーダ１５からの検出信号ＳＡ，ＳＢ
は、検出回路２５ａに入力される。また、エンコーダ１
６からの検出信号ＳＡ，ＳＢは、検出回路２５ｂに入力
される。First, the CPU 21 of the robot controller 20
, The servo motor control interfaces 24a and 24b connect the first motor 5 and the second
The motor 6 is driven to rotate. As a result, the first motor 5 rotates to move the first arm 3 via the first reduction gear 5A in a direction corresponding to the rotation direction of the first motor 5. Further, the second motor 6 rotates to move the second arm 4 in a direction corresponding to the rotation direction of the second motor 6 via the second reduction gear 6A. The rotation speed and the rotation direction of the first motor 5 and the second motor 6 are detected by the encoder 15 and the encoder 16. Detection signals SA and SB from encoder 15
Is input to the detection circuit 25a. Also, encoder 1
6 are input to the detection circuit 25b.

【００２７】この検出信号ＳＡ，ＳＢは、検出回路２５
ａの位相検出回路２５１により位相検出される。すなわ
ち、図６に示すように、クロックφ₀ が位相検出回路２
５１の各フリップフロップＦＦ₂₁，ＦＦ_22, ＦＦ₂₃，Ｆ
Ｆ₂₄のクロック端子に入力されているときに、期間Ｔｐ
において図示のようなタイミングの検出信号ＳＡ，ＳＢ
が入力されると、フリップフロップＦＦ₂₁，ＦＦ_22, Ｆ
Ｆ₂₃，ＦＦ₂₄、アンド回路ＡＤ₂₁〜ＡＤ₂₈の作用により
オア回路ＯＲ₂₁から検波信号Ｓａが得られ、また、期間
Ｔｍにおいて図示のようなタイミングの検出信号ＳＡ，
ＳＢが入力されると、フリップフロップＦＦ₂₁，ＦＦ
_22, ＦＦ₂₃，ＦＦ₂₄、アンド回路ＡＤ₂₁〜ＡＤ₂₈の作用
によりオア回路ＯＲ₂₂から検波信号Ｓｂが得られる（図
６）。The detection signals SA and SB are supplied to the detection circuit 25
The phase is detected by the phase detection circuit 251 of FIG. That is, as shown in FIG. 6, the clock phi ₀ is the phase detector circuit 2
Each flip-flop FF ₂₁ of _{51, FF 22, FF 23,} F
When it is input to the clock terminal of the F _24, the period Tp
, The detection signals SA, SB at the timings as shown
When it is inputted, the flip-flop FF _21, FF _22, F
F _23, FF _24, the AND circuit AD ₂₁ acts detection signal Sa is obtained from the OR circuit OR ₂₁ by the to AD _28, The detection signal SA of the timing as shown in the period Tm,
When SB is input, flip-flops FF ₂₁ , FF _21
22, FF _23, FF _24, the detection signal Sb is obtained from the OR circuit OR ₂₂ by the action of the AND circuit AD ₂₁ to AD ₂₈ (FIG. 6).

【００２８】このような検波信号Ｓａ，Ｓｂは、可逆カ
ウンタ２５２に入力される。可逆カウンタ２５２では、
例えば検波信号Ｓａが入力されたときに加算し、検波信
号Ｓｂが入力されたときに減算する。The detection signals Sa and Sb are input to the reversible counter 252. In the reversible counter 252,
For example, the addition is performed when the detection signal Sa is input, and the addition is performed when the detection signal Sb is input.

【００２９】同様に、エンコーダ１６からの検出信号Ｓ
Ａ，ＳＢは、検出回路２５ｂにおいて、検出回路２５ａ
と同様に処理される。Similarly, the detection signal S from the encoder 16
A and SB are detected by the detection circuit 25a in the detection circuit 25b.
Is processed in the same way as

【００３０】ここで、ＣＰＵ２１はタイマー割込みがか
かると、図５に示すフローチャートの処理に移行する。
ＣＰＵ２１は、まず、検出回路２５ａ、２５ｂの可逆カ
ウンタ２５２の値を読む処理をする（ステップ５０
１）。これは、ＣＰＵ２１が、まず、ｎ＝１としてリー
ド信号Ｒ₀ をバス２２を介してスリーステートバッファ
２５３ａに与えると、スリーステートバッファ２５３ａ
がアクティブになって、可逆カウンタ２５２ａの計数値
Ｒｎをバス２２のデータバスに乗せるので、ＣＰＵ２１
はこれを（計数値Ｒｎ）読み込むことで読み込み処理が
終了する。次に、ＣＰＵ２１は、ｎ＝２としてリード信
号Ｒ₁ をバス２２を介してスリーステートバッファ（図
示せず）に与えると、スリーステートバッファがアクテ
ィブになって、可逆カウンタ（図示せず）の計数値Ｒ₂
をバス２２のデータバスに乗せるので、ＣＰＵ２１はこ
れを（計数値Ｒ₂ ）読み込むことで読み込み処理が終了
する。このように、必要な外部データを読み込む処理が
終了すると、次に計算処理に移行する。Here, when a timer interrupt occurs, the CPU 21 shifts to the processing of the flowchart shown in FIG.
First, the CPU 21 reads the value of the reversible counter 252 of the detection circuits 25a and 25b (step 50).
1). This is because the CPU 21 first supplies the read signal _R0 to the three-state buffer 253a via the bus 22 with n = 1, and the three-state buffer 253a
Becomes active and puts the count value Rn of the reversible counter 252a on the data bus of the bus 22, so that the CPU 21
Is read (count value Rn), and the reading process ends. Then, CPU 21, given a read signal R ₁ as n = 2 in the three-state buffer through the bus 22 (not shown), the three-state buffer is activated, a total of reversible counter (not shown) Number R ₂
Is loaded on the data bus of the bus 22, so that the CPU 21 reads this (count value R ₂ ), thereby completing the reading process. As described above, when the process of reading necessary external data is completed, the process proceeds to a calculation process.

【００３１】つぎに、ＣＰＵ２１は、第１アーム３の移
動量の変化分だけ取り出す処理を実行する（ステップ５
０２）。これは、今回読み込んだ計数値Ｒ₁ からメモリ
ー２３に記憶されている前回の可逆カウンタ２５２の計
数値Ｚ₁ を差引き、その演算結果をＤ₁ とすることによ
り、第１アーム３の移動量（変化分）が求まる。同様
に、Ｄ₂ ＝Ｒ₂ −Ｚ₂ の計算をすることにより、第２ア
ーム４の移動量（変化分）が求まる。Next, the CPU 21 executes a process of taking out a change in the amount of movement of the first arm 3 (step 5).
02). The movement amount of the first arm 3 is calculated by subtracting the previous count value Z ₁ of the reversible counter 252 stored in the memory 23 from the currently read count value R ₁ , and calculating the result as D _1. (Change) is obtained. Similarly, by calculating D ₂ = R ₂ −Z ₂ , the movement amount (change amount) of the second arm 4 is obtained.

【００３２】ついで、ＣＰＵ２１は、移動量を求める処
理をする（ステップ５０３）。まず、メモリー２３の領
域Ｐ₁に記憶されているデータに演算結果Ｄ₁を加算する
（Ｐ₁ ＝Ｐ₁ ＋Ｄ₁ ）。同様に、Ｐ₂ ＝Ｐ₂ ＋Ｄ₂ の計
算をする。これにより、Ｐ₁及びＰ₂ を求めることがで
きる。Next, the CPU 21 performs a process for obtaining the movement amount (step 503). First, the operation result to the data stored in the area P ₁ memory 23 adds _{D 1 (P 1 = P 1} + D 1). Similarly, the calculation of P ₂ = P ₂ + D ₂ is performed. Thereby, P ₁ and P ₂ can be obtained.

【００３３】その後、ＣＰＵ２１は、今回読み込んだ計
数値Ｒ₁ 、Ｒ₂ をＺ₁ 、Ｚ₂ としてメモリー２３の各エ
リアに記憶させる（ステップ５０４）。Thereafter, the CPU 21 stores the currently read count values R ₁ and R ₂ as Z ₁ and Z ₂ in each area of the memory 23 (step 504).

【００３４】この処理が終了すると、次に、ＣＰＵ２１
は、第１アーム３の角度θ₁ 及び第２アーム４と第１ア
ーム３のなす角度θ₂ を算出する処理を実行する（ステ
ップ５０５）。これは、第１減速機５Ａの減速比ｊａか
らθ₁ ＝Ｐ₁ ／ｊａの計算をすることにより第１アーム
３のＸ軸になす角度θ₁ を求めるとともに、第２減速機
６Ａの減速比ｊｂからθ₂ ＝Ｐ₂ ／ｊａの計算をするこ
とにより第２アーム４が第１アーム３に対してなす角度
θ₁ を求めている。When this processing is completed, the CPU 21
Performs a process of calculating the angle theta ₁ and the second arm 4 of the first arm 3 the angle theta ₂ of the first arm 3 (step 505). This is because the angle θ ₁ formed by the first arm 3 with respect to the X axis is obtained by calculating θ ₁ = P ₁ / ja from the reduction ratio ja of the first reduction gear 5A, and the reduction ratio of the second reduction gear 6A. By calculating θ ₂ = P ₂ / ja from jb, the angle θ ₁ formed by the second arm 4 with respect to the first arm 3 is obtained.

【００３５】このようにして各角度θ₁ 、θ₂ が算出さ
れたら、ＣＰＵ２１は、次の数式１を使用して第２アー
ム４の先端の移動点Ｏ₃ の位置の計算をする（ステップ
５０６）。When the angles θ ₁ and θ ₂ have been calculated in this way, the CPU 21 calculates the position of the moving point O ₃ of the tip of the second arm 4 using the following equation (step 506). ).

【００３６】[0036]

【数１】 このようにしてＸ，Ｙを算出した後に、ＣＰＵ２１は、
判定信号保持部２５７の状態を読みにゆく（ステップ５
０７）。これは、ＣＰＵ２１からバス２２を介してバッ
ファＢＦにリード信号Ｒ₂ を与えることにより、バッフ
ァＢＦをアクティブ状態にし、フリップフロップＦＦ₂
の出力端子の信号をバス２２のデータバスを介してＣＰ
Ｕ２１に取り込むことにより得られる。この時点では、
フリップフロップＦＦ₃ の値が論理“０”であるので
（ステップ５０７；Ｎ）、ＣＰＵ２１はタイマールーチ
ンを抜けて、他の処理に移行する。(Equation 1) After calculating X and Y in this way, the CPU 21
The state of the determination signal holding unit 257 is read (step 5).
07). This is because the buffer BF is activated by applying a read signal R ₂ to the buffer BF from the CPU 21 via the bus 22, and the flip-flop FF ₂
Of the output terminal of CP via the data bus of bus 22
It is obtained by taking it into U21. At this point,
Since the value of the flip-flop FF ₃ is a logic "0" (step 507; N), CPU 21 is exits the timer routine proceeds to other processing.

【００３７】上述のような処理をタイマーからの指令に
より一定時間毎に処理することにより、第１アーム３及
び第２アーム４のおおまかな位置、姿勢を求めることが
できる。By performing the above-described processing at regular intervals in accordance with a command from a timer, the approximate position and posture of the first arm 3 and the second arm 4 can be obtained.

【００３８】ここで、第１アーム３及び第２アーム４の
大まかな位置、姿勢が規定された領域（カメラ３０によ
る撮像可能な領域）に入った時点で、ＣＰＵ２１はカメ
ラ３０に対してシャッター閉の指令を出力する。このシ
ャッター閉の指令によりカメラ３０のシャッターが閉じ
ると同時に、シャッター閉信号により位置認識信号ＳＣ
が形成される。この位置認識信号ＳＣは、検出回路２５
ａ，２５ｂ，２５ｃ，２５ｄに入力される。例えば、検
出回路２５ａにおいて、微分回路２５４では、二つのフ
リップフロップＦＦ₁ ，ＦＦ₂ の各クロック端子に常時
クロックφ₁ が供給されており、位置認識信号ＳＣがフ
リップフロップＦＦ₁ に入力されると、二つのフリップ
フロップＦＦ₁ ，ＦＦ₂ とナンド回路ＮＡの作用により
位置認識信号ＳＣの立ち上がり部分が微分されてナンド
回路ＮＡの出力端子から微分信号Ｓｃが出力される。こ
の微分信号Ｓｃがレジスタ２５５に供給されると、レジ
スタ２５５は可逆カウンタ２５２のその時点の計数値を
セットする。これにより、レジスタ２５５には、その時
点の値を保持したままとなる。At this point, the CPU 21 closes the shutter of the camera 30 when the first arm 3 and the second arm 4 enter an area where the rough position and posture are roughly defined (an area where the camera 30 can capture an image). Is output. At the same time that the shutter of the camera 30 is closed by the shutter close command, the position recognition signal SC is obtained by the shutter close signal.
Is formed. This position recognition signal SC is supplied to the detection circuit 25
a, 25b, 25c and 25d. For example, in the detection circuit 25a, the differential circuit 254, always clock phi ₁ to the respective clock terminals of the two flip-flops FF _1, FF ₂ are supplied, the position identification signals SC are input to the flip-flop FF ₁ The rising portion of the position recognition signal SC is differentiated by the operation of the two flip-flops FF ₁ and FF ₂ and the NAND circuit NA, and the differentiated signal Sc is output from the output terminal of the NAND circuit NA. When the differential signal Sc is supplied to the register 255, the register 255 sets the current count value of the reversible counter 252. As a result, the value at that time is held in the register 255.

【００３９】一方、微分信号Ｓｃは判定信号保持部２５
７にも供給される。判定信号保持部２５７では、微分信
号Ｓｃが入力されると、フリップフロップＦＦ₃ がセッ
トされることになる。On the other hand, the differential signal Sc is stored in the judgment signal
7 as well. The decision signal holding unit 257, the differential signal Sc is input, so that the flip-flop FF ₃ is set.

【００４０】このような状態でタイマー割込みがかかる
と、ＣＰＵ２１は、図５に示すフローチャートを上述の
ように処理し（ステップ５０１〜５０６）、ついでフリ
ップフロップＦＦ₃ の出力が論理“１”になっているか
を判定する（ステップ５０７）。すなわち、ＣＰＵ２１
は、判定信号保持部２５７のバッファＢＦにリード信号
Ｒ₂ を与え、バッファＢＦをアクティブにしてフリップ
フロップＦＦ₃ の内容をバス２２を介してＣＰＵ２１に
読み込む。この場合にフリップフロップＦＦ₃がセット
されているから、フリップフロップＦＦ₃ の出力端子の
データは論理“１”となっているので（ステップ５０
７；Ｙ）、ＣＰＵ２１は、検出回路２５ａのレジスタ２
５５の値Ｃ₁ と、検出回路２５ｂのレジスタの値Ｃ₂ と
を読み込むとともに、先に処理したステップ５０１〜５
０６で得たデータを使用して数式２及び数式３の計算を
する（ステップ５０８）。[0040] When the timer interrupt is applied in this state, CPU 21 processes a flowchart shown in FIG. 5 as described above (step 501 through 506), then the output of the flip-flop FF ₃ becomes logic "1" Is determined (step 507). That is, the CPU 21
Gives a read signal R ₂ in the buffer BF of the determination signal holding unit 257, it reads the contents of the flip-flop FF ₃ in CPU21 through the bus 22 and activates the buffer BF. Since the flip-flop FF ₃ is set in this case, the data output terminal of the flip-flop FF ₃ is a logic "1" (step 50
7; Y), the CPU 21 operates the register 2 of the detection circuit 25a.
Reads in the value C ₁ of 55, and a value C ₂ of the register of the detection circuit 25b, Step treated previously 501-5
Formulas 2 and 3 are calculated using the data obtained in step 06 (step 508).

【００４１】[0041]

【数２】 (Equation 2)

【数３】 このようにしてＸ’，Ｙ’を求めることができる。その
後に、ＣＰＵ２１はバス２２を介して判定信号保持部２
５７のアンド回路ＡＤにラント信号Ｗ₀ を出力すると、
アンド回路ＡＤで論理積がとられてフリップフロップＦ
Ｆ₃ をリセットする（ステップ５０８）。(Equation 3) In this way, X 'and Y' can be obtained. After that, the CPU 21 sends the determination signal holding unit 2 via the bus 22.
When the runt signal W ₀ is output to the AND circuit AD 57,
AND operation is performed by the AND circuit AD, and the flip-flop F
The F ₃ is reset (step 508).

【００４２】このように、例えば画像データによりロボ
ット１が把持するべき位置が画像の座標系内で求まった
場合、メインルーチンにデータ取得を知らせるため、ハ
ンドシェークビットの操作を行う（ステップ５０９）。
そして、ＣＰＵ２１は、数式４を使用してカメラ座標の
位置、姿勢の情報をロボット座標に変換する。As described above, for example, when the position to be held by the robot 1 is determined in the coordinate system of the image from the image data, the handshake bit is operated to notify the main routine of the data acquisition (step 509).
Then, the CPU 21 converts the information of the position and the posture of the camera coordinates into the robot coordinates by using Expression 4.

【００４３】[0043]

【数４】 上述したように数式４により演算することでロボット１
の把持位置を算出することができる。したがって、本実
施例では、撮像点上を通過するだけで撮像から目標位置
の算出をおこなうことが可能なため、作業が高速化でき
る。(Equation 4) As described above, the robot 1
Can be calculated. Therefore, in the present embodiment, since the target position can be calculated from the imaging simply by passing over the imaging point, the operation can be speeded up.

【００４４】なお、上記実施例では、位置認識信号ＳＣ
が入力されたことを確実に検知するため、判定信号保持
部２５７を設け、位置認識信号ＳＣが入力されたときに
判定信号保持部２５７をセットしておき、ＣＰＵ２１の
タイマー割込み時の処理フローの処理で判定信号保持部
２５７のセット／リセットを判定し、その後の処理を実
行しているが、ＣＰＵ２１側でシャッター閉指令（撮像
指令）を出したことが分かっているので、それを基にレ
ジスタ２５５の値を読みにゆくようにしてもよい。この
ように構成する場合には、判定信号保持部２５７は不要
となる。In the above embodiment, the position recognition signal SC
In order to reliably detect the input of the position recognition signal SC, a determination signal holding unit 257 is provided, and when the position recognition signal SC is input, the determination signal holding unit 257 is set. In the processing, the setting / resetting of the determination signal holding unit 257 is determined, and the subsequent processing is executed. However, since it is known that the CPU 21 has issued the shutter close command (imaging command), the register is set based on the command. The value of 255 may be read. In the case of such a configuration, the determination signal holding unit 257 becomes unnecessary.

【００４５】図７に本発明の応用例を示す。この応用例
は、ロボット１のスプラインシャフト１１ｂの先端側に
レーザセンサ４０を固定してワーク３５までの距離を測
定できるようにしたものであり、スプラインシャフト１
１ｂが下降してワーク３５との間で一定の距離に達した
ところで、位置認識信号ＳＣを出力できるようにしたも
のである。FIG. 7 shows an application example of the present invention. In this application example, the laser sensor 40 is fixed to the tip side of the spline shaft 11b of the robot 1 so that the distance to the workpiece 35 can be measured.
When 1b descends and reaches a certain distance from the work 35, the position recognition signal SC can be output.

【００４６】即ち、レーザセンサ４０にはセンサ処理装
置４１が接続されている。このセンサ処理装置４１は、
レーザセンサ４０を駆動するとともに、レーザセンサ４
０からの検出信号を処理してレーザセンサ４０とワーク
３５との距離を、デジタルデータあるいは距離に応じた
距離検出電圧信号Ｖ_L として出力できるようになってい
る。センサ処理装置４１からの距離検出電圧信号Ｖ
_L は、コンパレータ４２の一方の入力端子に入力され
る。コンパレータ４２の他方の入力端子は、比較用電圧
Ｖ_REF を形成する可変抵抗機４３の可変端子に接続され
ている。可変抵抗機４３は、比較用電圧Ｖ_REF を可変で
きる。また、コンパレータ４２は、比較用電圧Ｖ_REF よ
り大きな距離検出電圧信号Ｖ_L が入力されたときにはそ
の出力端子から“Ｈ”を出力し、前記以外の距離検出電
圧信号Ｖ_L が入力されたときにはその出力端子から
“Ｌ”を出力するようになっている。That is, a sensor processing device 41 is connected to the laser sensor 40. This sensor processing device 41
While driving the laser sensor 40, the laser sensor 4
By processing the detection signal from 0, the distance between the laser sensor 40 and the work 35 can be output as digital data or a distance detection voltage signal _VL corresponding to the distance. Distance detection voltage signal V from sensor processing device 41
_L is input to one input terminal of the comparator 42. The other input terminal of the comparator 42 is connected to a variable terminal of a variable resistor 43 forming a comparison voltage _VREF . The variable resistor 43 can change the comparison voltage V _REF . Further, the comparator 42 outputs “H” from its output terminal when a distance detection voltage signal _VL larger than the comparison voltage V _REF is input, and outputs the “H” when a distance detection voltage signal _VL other than the above is input. "L" is output from the output terminal.

【００４７】いま、レーザセンサ４０とワーク３５が一
定の距離だけ近づいたときに、センサ処理装置４１から
一定距離に応じた距離検出電圧信号Ｖ_L が出力される。
この距離検出電圧信号Ｖ_L がコンパレータ４２の一方の
入力端子に入力されると、予め設定しておいた比較用電
圧Ｖ_REF より大きくなるため、コンパレータ４２の出力
端子から“Ｈ”が出力される。このコンパレータ４２の
出力端子から出力された“Ｈ”は、位置認識信号ＳＣと
して検出回路２５ａ，２５ｂ，２５ｃ，２５ｄに入力さ
れる。これにより、各検出回路２５ａ，２５ｂ，２５
ｃ，２５ｄのレジスタ２５５には、その時点の可逆カウ
ンタ２５２の値がセットされる。また、各検出回路２５
ａ，２５ｂ，２５ｃ，２５ｄの判定信号保持部２５７
は、“１”にセットされることになる。したがって、移
動前に判定信号保持部２５７をリセットしておけば、上
記ロボット１を移動中に正確な位置が認識できることに
なる。When the laser sensor 40 and the work 35 approach each other by a certain distance, the sensor processing device 41 outputs a distance detection voltage signal _VL corresponding to the certain distance.
When the distance detection voltage signal _VL is input to one input terminal of the comparator 42, the voltage becomes higher than a preset comparison voltage _VREF , so that "H" is output from the output terminal of the comparator 42. . "H" output from the output terminal of the comparator 42 is input to the detection circuits 25a, 25b, 25c, and 25d as the position recognition signal SC. Thereby, each detection circuit 25a, 25b, 25
The values of the reversible counter 252 at that time are set in the registers 255 of c and 25d. Further, each detection circuit 25
a, 25b, 25c, 25d determination signal holding unit 257
Is set to "1". Therefore, if the determination signal holding unit 257 is reset before the movement, the accurate position can be recognized while the robot 1 is moving.

【００４８】もし、レーザセンサ４０によりワーク３５
の位置が認識されなかった場合には、センサ処理装置４
１から距離検出電圧信号Ｖ_L は比較用電圧Ｖ_REF より低
いため、コンパレータ４２の出力（位置認識信号ＳＣ）
は“Ｌ”のままであり、第１アーム３及び第２アーム４
の移動完了後も判定信号保持部２５７はクリアされたま
まとなる。したがって、第１アーム３及び第２アーム４
の予定移動範囲を過ぎてもなお各検出回路２５ａ，２５
ｂ，２５ｃ，２５ｄの判定信号保持部２５７がリセット
状態であったときに、ＣＰＵ２１がこれらを読み取るこ
とにより、認識に異常があったことを知ることができ
る。なお、この応用例の場合には、位置認識信号ＳＣが
入力されたことを保持しておくため、判定信号保持部２
５７を必ず設けておく必要がある。If the work 35 is detected by the laser sensor 40,
Is not recognized, the sensor processing device 4
Since the distance detection voltage signal _VL from 1 is lower than the comparison voltage _VREF , the output of the comparator 42 (position recognition signal SC)
Remains at “L”, and the first arm 3 and the second arm 4
After the completion of the movement, the determination signal holding unit 257 remains cleared. Therefore, the first arm 3 and the second arm 4
Of each detection circuit 25a, 25
When the determination signal holding units 257 of b, 25c, and 25d are in the reset state, the CPU 21 reads these, so that it is possible to know that the recognition is abnormal. In the case of this application example, in order to hold that the position recognition signal SC has been input, the determination signal holding unit 2
57 must be provided.

【００４９】[0049]

【００５０】[0050]

【発明の効果】 以上の説明より明らかなように、 請求項
１記載の発明によれば、一つのセンサに対して、位相検
出回路、可逆カウンタ、レジスタからなる検出回路を設
け、かつ各センサに対してそれぞれ検出回路を設け、か
つ前記各検出回路における可逆カウンタの計数値を演算
処理装置が読み込みその計数値からアームの移動量を算
出し、かつ撮像タイミングの可逆カウンタの計数値をレ
ジスタにセットし、そのセット値及び撮像データから得
られるデータとロボットの座標系との関係とを求めるよ
うにしたので、ロボットを停止させることなくロボット
の位置、姿勢を精度良くかつ高速に求めることが可能な
ロボットの位置・姿勢認識装置を得ることができる。 As is apparent from the above description, the claims
According to the invention 1, wherein, for one single sensor, the phase detection circuit, the reversible counter is provided with a detection circuit comprising a register, and providing respective detector for each sensor, and the reversible counter in the detection circuits The arithmetic processing unit reads the count value of (1), calculates the amount of movement of the arm from the count value, and sets the count value of the reversible counter of the imaging timing in a register. The data obtained from the set value and the imaging data and the coordinates of the robot Since the relationship with the system is obtained, it is possible to obtain a robot position / posture recognition device capable of accurately and quickly obtaining the position and posture of the robot without stopping the robot.

【００５１】さらに、請求項２記載の発明では、請求項
１記載の発明における検出回路に、判定信号保持部を付
加し、認識信号が入力されたことをセットしておいて、
演算処理装置に認識信号が入力されたことを知らせるよ
うにしたので、演算処理装置とは同期しない認識信号で
も確実に各レジスタのセット値を演算処理装置に取り込
むことができる。Further, according to the second aspect of the present invention,
A detection signal holding unit is added to the detection circuit according to the first aspect of the invention, and it is set that a recognition signal has been input.
Since the recognition signal is input to the arithmetic processing unit, the set value of each register can be reliably taken into the arithmetic processing unit even with a recognition signal that is not synchronized with the arithmetic processing unit.

【図面の簡単な説明】[Brief description of the drawings]

【図１】本発明のロボットの位置・姿勢認識方法を実現
するロボットの位置・姿勢認識装置の実施例を示すブロ
ック図である。FIG. 1 is a block diagram showing an embodiment of a robot position / posture recognition apparatus for realizing a robot position / posture recognition method according to the present invention.

【図２】同ロボットの位置・姿勢認識装置の具体的構成
例を示すブロック図である。FIG. 2 is a block diagram showing a specific configuration example of a position / posture recognition device of the robot.

【図３】同位相検出回路の具体的構成例を示す回路図で
ある。FIG. 3 is a circuit diagram showing a specific configuration example of the phase detection circuit.

【図４】同ロボットの位置・姿勢認識方法を説明するた
めの説明図である。FIG. 4 is an explanatory diagram for explaining a position / posture recognition method of the robot.

【図５】同ロボットの位置・姿勢認識装置の動作を示す
フローチャートである。FIG. 5 is a flowchart showing an operation of the position / posture recognition device of the robot.

【図６】同位相検出回路の動作を説明するためのタイミ
ングチャートである。FIG. 6 is a timing chart for explaining the operation of the phase detection circuit.

【図７】同応用例を示すブロック図である。FIG. 7 is a block diagram showing the application example.

【符号の説明】[Explanation of symbols]

１ ロボット ２ ベース ３ 第１アーム ４ 第２アーム ５ 第１モータ ６ 第２モータ ７ 第３モータ ８ 第４モータ １５ エンコーダ（センサ） １６ エンコーダ（センサ） １７ エンコーダ（センサ） １８ エンコーダ（センサ） ２０ ロボット制御装置 ２１ 演算処理装置（ＣＰＵ） ２５ａ，２５ｂ，２５ｃ，２５ｄ 検出回路 ２５１ 位相検出回路 ２５２ 可逆カウンタ ２５５ レジスタ ２５７ 判定信号保持部 Reference Signs List 1 robot 2 base 3 first arm 4 second arm 5 first motor 6 second motor 7 third motor 8 fourth motor 15 encoder (sensor) 16 encoder (sensor) 17 encoder (sensor) 18 encoder (sensor) 20 robot Control device 21 Arithmetic processing unit (CPU) 25a, 25b, 25c, 25d Detection circuit 251 Phase detection circuit 252 Reversible counter 255 Register 257 Judgment signal holding unit

フロントページの続き (58)調査した分野(Int.Cl.⁷，ＤＢ名) B25J 13/08 B25J 13/00 G05B 19/19 G05D 3/12 Continuation of the front page (58) Field surveyed (Int. Cl. ⁷ , DB name) B25J 13/08 B25J 13/00 G05B 19/19 G05D 3/12

Claims (2)

(57)【特許請求の範囲】(57) [Claims] 【請求項１】 アームの角度を検出して検出信号を出力
するセンサと、該センサからの検出信号を位相検出して
検波信号を得る位相検出回路、前記位相検出回路からの
検波信号を可逆計数する可逆カウンタ、ロボットアーム
の先端に設けられた視覚認識装置がロボット制御装置の
計算周期とは関係なく実際に撮像を行う撮像タイミング
で発生するシャッター閉信号により形成される位置認識
信号により前記可逆カウンタの計数値をセットするレジ
スタからなる検出回路と、各アーム毎の検出回路におけ
る可逆カウンタの計数値を読み込みその計数値からアー
ムの移動量を算出するとともに、前記各アーム毎の検出
回路におけるレジスタにセットされた値を読み込みその
セット値及び撮像データから得られるデータとロボット
の座標系との関係を求める演算処理装置とを備えたこと
を特徴とするロボットの位置・姿勢認識装置。1. A sensor that detects an angle of an arm and outputs a detection signal, a phase detection circuit that detects a phase of the detection signal from the sensor to obtain a detection signal, and a reversible count of the detection signal from the phase detection circuit. A reversible counter, a visual recognition device provided at the tip of the robot arm, the reversible counter based on a position recognition signal formed by a shutter closing signal generated at an imaging timing that actually performs imaging regardless of the calculation cycle of the robot controller. A detection circuit consisting of a register for setting the count value of each arm, and reading the count value of a reversible counter in the detection circuit for each arm, calculating the amount of movement of the arm from the count value, and setting a register in the detection circuit for each arm. The set values are read and the relationship between the set values and the data obtained from the imaging data and the robot coordinate system is read. A position / posture recognition device for a robot, comprising: 【請求項２】 アームの角度を検出して検出信号を出力
するセンサと、該センサからの検出信号を位相検出して
検波信号を得る位相検出回路、前記位相検出回路からの
検波信号を可逆計数する可逆カウンタ、ロボットアーム
の先端に設けられた視覚認識装置がロボット制御装置の
計算周期とは関係なく実際に撮像を行う撮像タイミング
で発生するシャッター閉信号により形成される認識信号
により前記可逆カウンタの計数値をセットするレジス
タ、撮像タイミングで発生する認識信号によりセット状
態になる判定信号保持部からなる検出回路と、各アーム
毎の検出回路における可逆カウンタの計数値を読み込み
その計数値からアームの移動量を算出するとともに、前
記判定信号保持部がセット状態のときに前記レジスタに
セットされた値を読み込み、そのセット値及び撮像デー
タから得られるデータとロボットの座標系との関係を求
める演算処理装置とを備えたことを特徴とするロボット
の位置・姿勢認識装置。2. A sensor for detecting an angle of an arm and outputting a detection signal, a phase detection circuit for detecting a phase of the detection signal from the sensor to obtain a detection signal, and reversibly counting the detection signal from the phase detection circuit. The reversible counter, a visual recognition device provided at the tip of the robot arm, performs the actual imaging independently of the calculation cycle of the robot control device. A detection circuit including a register for setting a count value, a determination signal holding unit that is set by a recognition signal generated at an imaging timing, and reading a count value of a reversible counter in a detection circuit for each arm, and moving the arm from the count value. Calculate the amount and read the value set in the register when the judgment signal holding unit is in the set state. A position and posture recognition device for a robot, comprising: a calculation processing device for determining a relationship between data obtained from the set value and the imaging data and a coordinate system of the robot.