JPH071368A - ロボットの位置・姿勢認識方法及び装置 - Google Patents

Abstract

(57)【要約】 【目的】 ロボットの位置・姿勢認識を短時間におこな
うことができるようにする。 【構成】 ロボットの位置・姿勢認識装置は、アーム
３、４の角度を検出し検出信号を出力するエンコーダ１
５、１６を含むロボット１と、各検出信号を処理する検
出回路２５ａ，２５ｂと、検出回路２５ａ，２５ｂから
のデータとロボットの座標系との関係を求めるＣＰＵ２
１とからなる。検出回路２５ａ，２５ｂは同一構成であ
り、検出回路２５ａは、検波信号を得る位相検出回路２
５１、その検波信号を可逆計数する可逆カウンタ２５
２、撮像タイミングで発生する位置認識信号ＳＣにより
可逆カウンタ２５２の計数値をセットするレジスタ２５
５からなる。レジスタ２５５は撮像タイミングのアーム
３、４の角度データを保持しているため、ＣＰＵ２１は
ロボット１を停止させることなく角度データを使用でき
る。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明はロボットに関する。更に
詳述すると、本発明はロボットの位置・姿勢を認識する
方法及びその装置に関するものである。

【０００２】

【従来の技術】一般にロボットには多種の型式のものが
提供されているが、いずれの型式のロボットでもロボー
トアーム先端の位置、姿勢を認識することは、ロボット
を駆動制御する上で非常に重要である。

【０００３】従来、ロボットの位置、姿勢を認識する方
法としては、（１）センサもパターン認識能力もないも
の、（２）センサはあるがパターン認識能力のないも
の、（３）センサとパターン認識能力のあるものに大別
される。各方法は、それぞれ利点と欠点があり、その使
用の目的に応じて使い分けられているが、コンピュータ
の発達により、比較的安価にかつ確実に位置の推定がで
きる上記（１）の方法がよく採用されている。

【０００４】これは、初期位置を与えておき、その後は
モータ等の回転数から現在の位置を推定する方法であっ
て、具体的にはモータの回転数をエンコーダで検出して
コンピュータ内に取り込み、コンピュータにより予め与
えられていた初期位置及びエンコーダからの検出パルス
を基に所定の計算をしてロボットの位置を推定するもの
である。

【０００５】

【発明が解決しようとする課題】しかしながら、上述し
た従来のロボットの位置・姿勢認識方法にあっては、コ
ンピュータの計算周期毎にしか位置認識が行えなかった
ため、位置認識を行う際にはロボットアームを低速に移
動させたり、場合によってはロボットアームを停止させ
る必要があり、認識に時間がかかり過ぎるという欠点が
あった。

【０００６】本発明は、上述した従来技術の欠点を解消
し、位置、姿勢認識を短時間におこなうことができきる
ロボットの位置・姿勢認識方法及び装置を提供すること
を目的とする。

【０００７】

【課題を解決するための手段】かかる目的を達成するた
め、請求項１記載の発明に係るロボットの位置・姿勢認
識方法は、各アームの角度を検出するセンサからの検出
信号を可逆カウンタで計数し、これら可逆カウンタの計
数値から各アームの移動量を算出することにより各アー
ムの大まかな位置・姿勢を求め、かつロボットアームの
先端に設けられた視覚認識装置の撮像タイミングで位置
認識信号を発生し、この撮像タイミングでの位置認識信
号で可逆カウンタの計数値をレジスタに記憶させ、前記
可逆カウンタ、レジスタの値及び撮像データとから得ら
れるデータとロボットの座標系との関係を求めるように
している。

【０００８】また、請求項２記載のロボットの位置・姿
勢認識装置は、アームの角度を検出して検出信号を出力
するセンサと、該センサからの検出信号を位相検出して
検波信号を得る位相検出回路、前記位相検出回路からの
検波信号を可逆計数する可逆カウンタ、撮像タイミング
で発生する位置認識信号により前記可逆カウンタの計数
値をセットするレジスタからなる検出回路と、各アーム
毎の検出回路における可逆カウンタの計数値を読み込み
その計数値からアームの移動量を算出するとともに、前
記各アーム毎の検出回路におけるレジスタにセットされ
た値を読み込みそのセット値及び撮像データから得られ
るデータとロボットの座標系との関係を求める演算処理
装置とを備えるようにしている。

【０００９】さらに、請求項３記載に発明に係るロボッ
トの位置・姿勢認識装置は、アームの角度を検出して検
出信号を出力するセンサと、前記センサからの検出信号
を位相検出して検波信号を得る位相検出回路、前記位相
検出回路からの検波信号を可逆計数する可逆カウンタ、
撮像タイミングで発生する認識信号により前記可逆カウ
ンタの計数値をセットするレジスタ、撮像タイミングで
発生する認識信号によりセット状態になる判定信号保持
部からなる検出回路と、各アーム毎の検出回路における
可逆カウンタの計数値を読み込みその計数値からアーム
の移動量を算出するとともに、前記判定信号保持部がセ
ット状態のときに前記レジスタにセットされた値を読み
込み、そのセット値及び撮像データから得られるデータ
とロボットの座標系との関係を求める演算処理装置とを
備えるようにしている。

【００１０】

【作用】請求項１記載の発明では、各アームの角度を検
出するセンサからの検出信号を可逆カウンタで計数し、
これら可逆カウンタの計数値から各アームの移動量を算
出することにより各アームの大まかな位置・姿勢を求め
ることができる。また、大まかな位置・姿勢を求めてゆ
き、その値が規定の領域に入ったときに、ロボットアー
ムの先端に設けられた視覚認識装置を撮像する。この撮
像タイミングで位置認識信号を発生させる。この撮像タ
イミングでの認識信号で可逆カウンタの計数値はレジス
タに記憶される。すなわち、撮像タイミングのときの各
アームの角度が可逆カウンタにあるので、これらをレジ
スタに記憶させてしまう。これにより、従来なら、撮像
タイミングで各アームの移動を停止し、前記各可逆カウ
ンタの値を演算処理装置が読み取り、読み取り終了後に
再びアームを移動させる過程が必要であったが、本発明
では移動中でも、当該撮像タイミングでの可逆カウンタ
の計数値がレジスタに記憶されているため、各アームを
停止させる必要がまったくなくなる。これにより、ロボ
ットを停止させることなく、前記可逆カウンタ、レジス
タの値及び撮像データから得られるデータとロボットの
座標系との関係を求めることができる。

【００１１】請求項２記載の発明では、請求項１記載の
発明を実現するため、一つのセンサに対して、上記位相
検出回路と、上記可逆カウンタと、上記レジスタとから
なる検出回路を設け、かつ各センサに対してそれぞれ検
出回路を設け、かつ前記各検出回路における可逆カウン
タの計数値を演算処理装置が読み込みその計数値からア
ームの移動量を算出する。また、演算処理装置は、前記
各検出回路におけるレジスタにセットされた値を読み込
みそのセット値及び撮像データとから得られるデータと
ロボットの座標系との関係を求めることができる。

【００１２】請求項３記載の発明では、請求項２記載の
発明における検出回路に、判定信号保持部を付加し、認
識信号が入力されたことをセットしておいて、演算処理
装置に認識信号が入力されたことを知らせるようにした
ものである。これにより、演算処理装置とは同期しない
認識信号でも確実に各レジスタのセット値及び撮像デー
タから得られるデータとロボットの座標系との関係を求
めることができる。

【００１３】

【実施例】以下、本発明の構成を図面に示す実施例に基
づいて詳細に説明する。

【００１４】図１に本発明のロボットの位置・姿勢認識
方法の実施例を実現する装置の一例を示す。該図におい
て、符号１はスカラ形ロボットである。スカラ形ロボッ
ト１は、主に、ベース２、第１アーム３、第２アーム
４、第１モータ５、第１減速機５Ａ、第２モータ６、第
２減速機６Ａ、第３モータ７、第４モータ８、第３減速
機８Ａを備えている。また、ベース２には第１アーム３
が回動可能に固定されており、この第１アーム３の先端
には第２アーム４が回動可能に固定されている。ベース
２には、第１モータ５及び第４モータ８が固定されてい
る。第１モータ５は、減速機５Ａを介して第１アーム３
を回転させるようになっている。第４モータ８は、プリ
ー９ａ，９ｂに掛け渡されたペルト１０ａと、プリー９
ｃ，９ｄに掛け渡されたペルト１０ｂとにより、スプラ
インシャフト先端把持部１１ａを所望角度だけ回転させ
られるようになっている。第２モータ６は第１アーム３
に固定されており、第２モータ６で第２アーム４を回転
できるようになっている。第２モータ６の先端内部には
第３モータ７が内蔵固定されており、第３モータ７はプ
リー１２ａ，１２ｂに掛け渡されたペルト１３によりボ
ールネジ１４を回転させることにより、スプラインシャ
フト１１ｂを図示上下動できるようになっている。ま
た、第１モータ５、第２モータ６、第３モータ７、及び
第４モータ８には、エンコーダ１５、１６、１７及び１
８がそれぞれ設けられており、各エンコーダ１５、１
６、１７及び１８は各モータ５、６、７及び８の回転数
を検出できるようになっている。これらエンコーダ１
５、１６は、第１アーム３及び第２アーム４の角度を検
出するセンサとして使用される。もちろん、エンコーダ
１７はスプラインシャフト先端把持部１１ａの上下動の
距離を、エンコーダ１８はスプラインシャフト先端把持
部１１ａの角度を測定するセンサとして使用される。

【００１５】このスカラ形ロボット１はロボット制御装
置２０に電気的に接続されており、ロボット制御装置２
０の制御下に、各モータ５、６、７及び８を回転させ、
かつ各エンコーダ１５、１６、１７及び１８からの検出
信号で各モータ５、６、７及び８の回転を駆動制御する
とともに、第２アーム４の先端位置を推定できるように
なっている。

【００１６】ロボット制御装置２０は、各種の演算処理
や各種の制御を実行する演算処理装置（ＣＰＵ）２１
と、バス２２と、メモリー２３と、サーボモータコント
ロールインターフェース２４ａ，２４ｂ，２４ｃ，２４
ｄと、検出回路２５ａ，２５ｂ，２５ｃ，２５ｄとを備
えている。ＣＰＵ２１には、バス２２を介してメモリー
２３と、サーボモータコントロールインターフェース２
４ａ，２４ｂ，２４ｃ，２４ｄとが接続されるととも
に、検出回路２５ａ，２５ｂ，２５ｃ，２５ｄが接続さ
れている。サーボモータコントロールインターフェース
２４ａ，２４ｂ，２４ｃ及び２４ｄは、図示しないが、
各モータ５，６，７及び８に電気的にそれぞれ接続され
ており、各モータ５，６，７及び８の回転をＣＰＵ２１
の指令に基づいて回転駆動制御する。検出回路２５ａ，
２５ｂ，２５ｃ及び２５ｄは、全て同一回路構成となっ
ているので、検出回路２５ａのみの構成について説明す
る。検出回路２５ａは、エンコーダ１５からの検出信号
ＳＡ，ＳＢを取り込み、これを検波して検波信号Ｓａ，
Ｓｂの内の一つを出力できる位相検出回路２５１と、位
相検出回路２５１からの検波信号Ｓａ，Ｓｂの内の一つ
を差引計数する可逆カウンタ２５２と、この可逆カウン
タ２５２の計数値をＣＰＵ２１に読み込ませるスリース
テートバッファ２５３と、位置認識信号ＳＣが入力され
たときに前記位置認識信号ＳＣの微分信号Ｓｃを形成す
る微分回路２５４と、この微分信号Ｓｃが入力されたと
きに、その入力時点の可逆カウンタ２５２の計数値をセ
ットするレジスタ２５５と、このレジスタ２５５のセッ
ト値をＣＰＵ２１に読み込ませるスリーステートバッフ
ァ２５６と、微分信号Ｓｃが入力されるとセットされＣ
ＰＵ２１によりリセットされる判定信号保持部２５７と
から構成されている。

【００１７】図２に、ロボット制御装置２０の検出回路
２５ａの具体的構成の一例を示す。この図は、特に、ス
リーステートバッファ２５３，２５６、微分回路２５４
及び判定信号保持部２５７の構成例について説明してい
る。

【００１８】位相検出回路２５１は、エンコーダ１５か
らの検出信号ＳＡ，ＳＢ及びクロックφ₀ を取り込み、
検波信号Ｓａ又はＳｂを可逆カウンタ２５２に供給でき
るようになっている。可逆カウンタ２５２は、検波信号
Ｓａ又はＳｂを計数する回路である。可逆カウンタ２５
２の出力は、スリーステートバッファ２５３及びレジス
タ２５５に供給されるようになっている。スリーステー
トバッファ２５３は、ＣＰＵ２１からバス２２を介して
与えられるリード信号Ｒ₀ によって動作し、可逆カウン
タ２５２の計数値出力をバス２２のデータバスに供給す
る。これにより、ＣＰＵ２１は、可逆カウンタ２５２の
計数値を取り込むことができる。

【００１９】位置認識信号ＳＣは、微分回路２５４に入
力される。微分回路２５４は、二つのＤ型フリップフロ
ップＦＦ₁ ，ＦＦ₂ と、一つのナンド回路ＮＡとを備
え、位置認識信号ＳＣをフリップフロップＦＦ₁ のＤ入
力端子に入力し、フリップフロップＦＦ₁ の出力端子を
フリップフロップＦＦ₂ のＤ入力端子に接続し、フリッ
プフロップＦＦ₁ ，ＦＦ₂ のクロック端子にクロックφ
₀ を与え、フリップフロップＦＦ₁ ，ＦＦ₂ の各出力端
子からの信号をナンド回路ＮＡの両入力端子に供給し、
ナンド回路ＮＡの出力端子から、微分信号Ｓｃを得るよ
うになっている。この微分信号Ｓｃは、レジスタ２５５
と、判定信号保持部２５７に入力されるようになってい
る。

【００２０】レジスタ２５５は、微分信号Ｓｃが入力さ
れた時点の可逆カウンタ２５２の計数値をセットし、こ
れをスリーステートバッファ２５６に供給している。ス
リーステートバッファ２５６は、ＣＰＵ２１からバス２
２を介して与えられるリード信号Ｒ₁ により動作し、レ
ジスタ２５５の値をバス２２のデータバスに供給できる
ようになっている。

【００２１】また、判定信号保持部２５７は、フリップ
フロップＦＦ₃ と、アンド回路ＡＤと、バッファＢＦと
を備え、微分信号ＳｃがフリップフロップＦＦ₃ に入力
されると、フリップフロップＦＦ₃ がセットされ、アン
ド回路ＡＤからの出力信号があるとリセットされるよう
になっている。フリップフロップＦＦ₃ の出力端子はバ
ッファＢＦを介してバス２２のデータバスに接続されて
おり、このバッファＢＦはＣＰＵ２１からのリード信号
Ｒ₂ により動作して、フリップフロップＦＦ₃の出力信
号をバス２２のデータバスに供給できるようになってい
る。また、このバッファＢＦの出力はアンド回路ＡＤの
一方の入力端子に供給されており、またＣＰＵ２１から
バス２２を介して与えられるライト信号Ｗ₀ はアンド回
路ＡＤの他方の入力端子に供給されている。そして、ア
ンド回路ＡＤは、バッファＢＦの出力と、ライト信号Ｗ
₀ との双方が論理積されたときに、クリア信号をフリッ
プフロップＦＦ₃のクリア端子に供給できるようになっ
ている。

【００２２】図３に上記位相検出回路２５１の具体的構
成の一例を示す。該図において、位相検出回路２５１
は、四つのフリップフロップＦＦ₂₁，ＦＦ₂₂，ＦＦ₂₃，
ＦＦ₂₄と、反転入力端子を少なくとも一つを有する四つ
のアンド回路ＡＤ₂₁，ＡＤ₂₂，ＡＤ₂₃，ＡＤ₂₄と、二つ
のオア回路ＯＲ₂₁，ＯＲ₂₂とを備えている。

【００２３】エンコーダからの信号ＳＡは、フリップフ
ロップＦＦ₂₁のＤ入力端子に入力し、フリップフロップ
ＦＦ₂₁の出力端子をフリップフロップＦＦ₂₂のＤ入力端
子に接続し、フリップフロップＦＦ₂₁，ＦＦ₂₂のクロッ
ク端子にクロックφ₀ を与えられるようになっている。
フリップフロップＦＦ₂₁の出力端子からの信号は、アン
ド回路ＡＤ₂₁，ＡＤ₂₄，ＡＤ₂₇，ＡＤ₂₈の入力端子に供
給するとともに、アンド回路ＡＤ₂₂，ＡＤ₂₃，ＡＤ₂₅，
ＡＤ₂₆の反転入力端子に供給できるようになっている。
フリップフロップＦＦ₂₂の出力端子からの信号は、アン
ド回路ＡＤ₂₁，ＡＤ₂₂，ＡＤ₂₅，ＡＤ₂₇の入力端子に供
給するとともに、アンド回路ＡＤ₂₃，ＡＤ₂₄，ＡＤ₂₆，
ＡＤ₂₇の反転入力端子に供給できるようになっている。
エンコーダからの信号ＳＢは、フリップフロップＦＦ₂₃
のＤ入力端子に入力し、フリップフロップＦＦ₂₃の出力
端子をフリップフロップＦＦ₂₄のＤ入力端子に接続し、
フリップフロップＦＦ₂₃，ＦＦ₂₄のクロック端子にクロ
ックφ₀ を与えられるようになっている。フリップフロ
ップＦＦ₂₃の出力端子からの信号は、アンド回路Ａ
Ｄ₂₃，ＡＤ₂₄，ＡＤ₂₅，ＡＤ₂₈の入力端子に供給すると
ともに、アンド回路ＡＤ₂₁，ＡＤ₂₂，ＡＤ₂₆，ＡＤ₂₇の
反転入力端子に供給できるようになっている。フリップ
フロップＦＦ₂₄の出力端子からの信号は、アンド回路Ａ
Ｄ₂₁，ＡＤ₂₄，ＡＤ₂₅，ＡＤ₂₆の入力端子に供給すると
ともに、アンド回路ＡＤ₂₂，ＡＤ₂₃，ＡＤ₂₇，ＡＤ₂₈の
反転入力端子に供給できるようになっている。アンド回
路ＡＤ₂₁，ＡＤ₂₂，ＡＤ₂₃，ＡＤ₂₄の出力端子はオア回
路ＯＲ₂₁の入力端子に接続されており、このオア回路Ｏ
Ｒ₂₁は各入力端子に入力された信号の論理積がとられる
と検波信号Ｓａが出力端子から出力されるようになって
いる。アンド回路ＡＤ₂₅，ＡＤ₂₆，ＡＤ₂₇，ＡＤ₂₈の出
力端子はオア回路ＯＲ₂₂の入力端子に接続されており、
このオア回路ＯＲ₂₂は各入力端子に入力された信号の論
理積がとられると検波信号Ｓａが出力端子から出力され
るようになっている。

【００２４】図４にロボットの位置・姿勢認識方法を説
明するための説明図を示す。尚、図４はロボット１を上
から見たものである。ベース２には第１アーム３が回動
可能に固定されており、この固定点を原点Ｏ₁ として当
該原点Ｏ₁ を通ってＸ軸とＹ軸とがとられている。ま
た、第１アーム３には第２アーム４が回動可能に固定さ
れており、この固定点を移動点Ｏ₂ とする。そして、第
１アーム３の長さを原点Ｏ₁ と移動点Ｏ₂ との間の長さ
Ｌ₁ とし、第２アーム４の長さを移動点Ｏ₂ と移動点Ｏ
₃ との間の長さＬ₂ とし、かつスプラインシャフト先端
把持部１１ａに固定した腕１１ｃの先端に取付けたカメ
ラ３０と移動点Ｏ₂までの距離をＬ₂’とする。また、ワ
ーク３５のＸ軸、Ｙ軸に対する位置をＸｔ、Ｙｔとし、
かつカメラ３０の座標におけるカメラ３０とワーク３５
との位置関係をＸｃ、Ｙｃとする。

【００２５】上述したようなロボットの位置・姿勢認識
装置の作用を図１ないし図４を基に図５及び図６を参照
しながら以下に説明する。なお、図５はＣＰＵ２１の処
理フローである。また、図６は位相検出回路２５１のタ
イミングチャートであり、横軸に時間を、縦軸に各信号
をそれぞれ示している。また、この実施例では、カメラ
３０は、本来、移動点Ｏ₃ を中心として回動可能であ
り、かつＸＹ平面に対して直交座標系方向に移動可能で
あるが、説明を簡単にするためにカメラ３０は第２アー
ム４に固定した状態で説明することにする。したがっ
て、ロボット制御装置２０には、エンコーダ１５及びエ
ンコーダ１６からの各検出信号ＳＡ，ＳＢを処理するも
のとする。

【００２６】まず、ロボット制御装置２０のＣＰＵ２１
からの指令に基づき、サーボモータコントロールインタ
ーフェース２４ａ及び２４ｂは、第１モータ５及び第２
モータ６を回転駆動する。これにより、第１モータ５が
回転して第１減速機５Ａを介して第１アーム３を第１モ
ータ５の回転方向に応じた方向に移動させる。また、第
２モータ６が回転して第２減速機６Ａを介して第２アー
ム４を第２モータ６の回転方向に応じた方向に移動させ
る。これら第１モータ５及び第２モータ６の回転数及び
回転方向はエンコーダ１５及びエンコーダ１６により検
出される。エンコーダ１５からの検出信号ＳＡ，ＳＢ
は、検出回路２５ａに入力される。また、エンコーダ１
６からの検出信号ＳＡ，ＳＢは、検出回路２５ｂに入力
される。

【００２７】この検出信号ＳＡ，ＳＢは、検出回路２５
ａの位相検出回路２５１により位相検出される。すなわ
ち、図６に示すように、クロックφ₀ が位相検出回路２
５１の各フリップフロップＦＦ₂₁，ＦＦ_22, ＦＦ₂₃，Ｆ
Ｆ₂₄のクロック端子に入力されているときに、期間Ｔｐ
において図示のようなタイミングの検出信号ＳＡ，ＳＢ
が入力されると、フリップフロップＦＦ₂₁，ＦＦ_22, Ｆ
Ｆ₂₃，ＦＦ₂₄、アンド回路ＡＤ₂₁〜ＡＤ₂₈の作用により
オア回路ＯＲ₂₁から検波信号Ｓａが得られ、また、期間
Ｔｍにおいて図示のようなタイミングの検出信号ＳＡ，
ＳＢが入力されると、フリップフロップＦＦ₂₁，ＦＦ
_22, ＦＦ₂₃，ＦＦ₂₄、アンド回路ＡＤ₂₁〜ＡＤ₂₈の作用
によりオア回路ＯＲ₂₂から検波信号Ｓｂが得られる（図
６）。

【００２８】このような検波信号Ｓａ，Ｓｂは、可逆カ
ウンタ２５２に入力される。可逆カウンタ２５２では、
例えば検波信号Ｓａが入力されたときに加算し、検波信
号Ｓｂが入力されたときに減算する。

【００２９】同様に、エンコーダ１６からの検出信号Ｓ
Ａ，ＳＢは、検出回路２５ｂにおいて、検出回路２５ａ
と同様に処理される。

【００３０】ここで、ＣＰＵ２１はタイマー割込みがか
かると、図５に示すフローチャートの処理に移行する。
ＣＰＵ２１は、まず、検出回路２５ａ、２５ｂの可逆カ
ウンタ２５２の値を読む処理をする（ステップ５０
１）。これは、ＣＰＵ２１が、まず、ｎ＝１としてリー
ド信号Ｒ₀ をバス２２を介してスリーステートバッファ
２５３ａに与えると、スリーステートバッファ２５３ａ
がアクティブになって、可逆カウンタ２５２ａの計数値
Ｒｎをバス２２のデータバスに乗せるので、ＣＰＵ２１
はこれを（計数値Ｒｎ）読み込むことで読み込み処理が
終了する。次に、ＣＰＵ２１は、ｎ＝２としてリード信
号Ｒ₁ をバス２２を介してスリーステートバッファ（図
示せず）に与えると、スリーステートバッファがアクテ
ィブになって、可逆カウンタ（図示せず）の計数値Ｒ₂
をバス２２のデータバスに乗せるので、ＣＰＵ２１はこ
れを（計数値Ｒ₂ ）読み込むことで読み込み処理が終了
する。このように、必要な外部データを読み込む処理が
終了すると、次に計算処理に移行する。

【００３１】つぎに、ＣＰＵ２１は、第１アーム３の移
動量の変化分だけ取り出す処理を実行する（ステップ５
０２）。これは、今回読み込んだ計数値Ｒ₁ からメモリ
ー２３に記憶されている前回の可逆カウンタ２５２の計
数値Ｚ₁ を差引き、その演算結果をＤ₁ とすることによ
り、第１アーム３の移動量（変化分）が求まる。同様
に、Ｄ₂ ＝Ｒ₂ −Ｚ₂ の計算をすることにより、第２ア
ーム４の移動量（変化分）が求まる。

【００３２】ついで、ＣＰＵ２１は、移動量を求める処
理をする（ステップ５０３）。まず、メモリー２３の領
域Ｐ₁に記憶されているデータに演算結果Ｄ₁を加算する
（Ｐ₁ ＝Ｐ₁ ＋Ｄ₁ ）。同様に、Ｐ₂ ＝Ｐ₂ ＋Ｄ₂ の計
算をする。これにより、Ｐ₁及びＰ₂ を求めることがで
きる。

【００３３】その後、ＣＰＵ２１は、今回読み込んだ計
数値Ｒ₁ 、Ｒ₂ をＺ₁ 、Ｚ₂ としてメモリー２３の各エ
リアに記憶させる（ステップ５０４）。

【００３４】この処理が終了すると、次に、ＣＰＵ２１
は、第１アーム３の角度θ₁ 及び第２アーム４と第１ア
ーム３のなす角度θ₂ を算出する処理を実行する（ステ
ップ５０５）。これは、第１減速機５Ａの減速比ｊａか
らθ₁ ＝Ｐ₁ ／ｊａの計算をすることにより第１アーム
３のＸ軸になす角度θ₁ を求めるとともに、第２減速機
６Ａの減速比ｊｂからθ₂ ＝Ｐ₂ ／ｊａの計算をするこ
とにより第２アーム４が第１アーム３に対してなす角度
θ₁ を求めている。

【００３５】このようにして各角度θ₁ 、θ₂ が算出さ
れたら、ＣＰＵ２１は、次の数式１を使用して第２アー
ム４の先端の移動点Ｏ₃ の位置の計算をする（ステップ
５０６）。

【００３６】

【数１】 このようにしてＸ，Ｙを算出した後に、ＣＰＵ２１は、
判定信号保持部２５７の状態を読みにゆく（ステップ５
０７）。これは、ＣＰＵ２１からバス２２を介してバッ
ファＢＦにリード信号Ｒ₂ を与えることにより、バッフ
ァＢＦをアクティブ状態にし、フリップフロップＦＦ₂
の出力端子の信号をバス２２のデータバスを介してＣＰ
Ｕ２１に取り込むことにより得られる。この時点では、
フリップフロップＦＦ₃ の値が論理“０”であるので
（ステップ５０７；Ｎ）、ＣＰＵ２１はタイマールーチ
ンを抜けて、他の処理に移行する。

【００３７】上述のような処理をタイマーからの指令に
より一定時間毎に処理することにより、第１アーム３及
び第２アーム４のおおまかな位置、姿勢を求めることが
できる。

【００３８】ここで、第１アーム３及び第２アーム４の
大まかな位置、姿勢が規定された領域（カメラ３０によ
る撮像可能な領域）に入った時点で、ＣＰＵ２１はカメ
ラ３０に対してシャッター閉の指令を出力する。このシ
ャッター閉の指令によりカメラ３０のシャッターが閉じ
ると同時に、シャッター閉信号により位置認識信号ＳＣ
が形成される。この位置認識信号ＳＣは、検出回路２５
ａ，２５ｂ，２５ｃ，２５ｄに入力される。例えば、検
出回路２５ａにおいて、微分回路２５４では、二つのフ
リップフロップＦＦ₁ ，ＦＦ₂ の各クロック端子に常時
クロックφ₁ が供給されており、位置認識信号ＳＣがフ
リップフロップＦＦ₁ に入力されると、二つのフリップ
フロップＦＦ₁ ，ＦＦ₂ とナンド回路ＮＡの作用により
位置認識信号ＳＣの立ち上がり部分が微分されてナンド
回路ＮＡの出力端子から微分信号Ｓｃが出力される。こ
の微分信号Ｓｃがレジスタ２５５に供給されると、レジ
スタ２５５は可逆カウンタ２５２のその時点の計数値を
セットする。これにより、レジスタ２５５には、その時
点の値を保持したままとなる。

【００３９】一方、微分信号Ｓｃは判定信号保持部２５
７にも供給される。判定信号保持部２５７では、微分信
号Ｓｃが入力されると、フリップフロップＦＦ₃ がセッ
トされることになる。

【００４０】このような状態でタイマー割込みがかかる
と、ＣＰＵ２１は、図５に示すフローチャートを上述の
ように処理し（ステップ５０１〜５０６）、ついでフリ
ップフロップＦＦ₃ の出力が論理“１”になっているか
を判定する（ステップ５０７）。すなわち、ＣＰＵ２１
は、判定信号保持部２５７のバッファＢＦにリード信号
Ｒ₂ を与え、バッファＢＦをアクティブにしてフリップ
フロップＦＦ₃ の内容をバス２２を介してＣＰＵ２１に
読み込む。この場合にフリップフロップＦＦ₃がセット
されているから、フリップフロップＦＦ₃ の出力端子の
データは論理“１”となっているので（ステップ５０
７；Ｙ）、ＣＰＵ２１は、検出回路２５ａのレジスタ２
５５の値Ｃ₁ と、検出回路２５ｂのレジスタの値Ｃ₂ と
を読み込むとともに、先に処理したステップ５０１〜５
０６で得たデータを使用して数式２及び数式３の計算を
する（ステップ５０８）。

【００４１】

【数２】

【数３】 このようにしてＸ’，Ｙ’を求めることができる。その
後に、ＣＰＵ２１はバス２２を介して判定信号保持部２
５７のアンド回路ＡＤにラント信号Ｗ₀ を出力すると、
アンド回路ＡＤで論理積がとられてフリップフロップＦ
Ｆ₃ をリセットする（ステップ５０８）。

【００４２】このように、例えば画像データによりロボ
ット１が把持するべき位置が画像の座標系内で求まった
場合、メインルーチンにデータ取得を知らせるため、ハ
ンドシェークビットの操作を行う（ステップ５０９）。
そして、ＣＰＵ２１は、数式４を使用してカメラ座標の
位置、姿勢の情報をロボット座標に変換する。

【００４３】

【数４】 上述したように数式４により演算することでロボット１
の把持位置を算出することができる。したがって、本実
施例では、撮像点上を通過するだけで撮像から目標位置
の算出をおこなうことが可能なため、作業が高速化でき
る。

【００４４】なお、上記実施例では、位置認識信号ＳＣ
が入力されたことを確実に検知するため、判定信号保持
部２５７を設け、位置認識信号ＳＣが入力されたときに
判定信号保持部２５７をセットしておき、ＣＰＵ２１の
タイマー割込み時の処理フローの処理で判定信号保持部
２５７のセット／リセットを判定し、その後の処理を実
行しているが、ＣＰＵ２１側でシャッター閉指令（撮像
指令）を出したことが分かっているので、それを基にレ
ジスタ２５５の値を読みにゆくようにしてもよい。この
ように構成する場合には、判定信号保持部２５７は不要
となる。

【００４５】図７に本発明の応用例を示す。この応用例
は、ロボット１のスプラインシャフト１１ｂの先端側に
レーザセンサ４０を固定してワーク３５までの距離を測
定できるようにしたものであり、スプラインシャフト１
１ｂが下降してワーク３５との間で一定の距離に達した
ところで、位置認識信号ＳＣを出力できるようにしたも
のである。

【００４６】即ち、レーザセンサ４０にはセンサ処理装
置４１が接続されている。このセンサ処理装置４１は、
レーザセンサ４０を駆動するとともに、レーザセンサ４
０からの検出信号を処理してレーザセンサ４０とワーク
３５との距離を、デジタルデータあるいは距離に応じた
距離検出電圧信号Ｖ_L として出力できるようになってい
る。センサ処理装置４１からの距離検出電圧信号Ｖ
_L は、コンパレータ４２の一方の入力端子に入力され
る。コンパレータ４２の他方の入力端子は、比較用電圧
Ｖ_REF を形成する可変抵抗機４３の可変端子に接続され
ている。可変抵抗機４３は、比較用電圧Ｖ_REF を可変で
きる。また、コンパレータ４２は、比較用電圧Ｖ_REF よ
り大きな距離検出電圧信号Ｖ_L が入力されたときにはそ
の出力端子から“Ｈ”を出力し、前記以外の距離検出電
圧信号Ｖ_L が入力されたときにはその出力端子から
“Ｌ”を出力するようになっている。

【００４７】いま、レーザセンサ４０とワーク３５が一
定の距離だけ近づいたときに、センサ処理装置４１から
一定距離に応じた距離検出電圧信号Ｖ_L が出力される。
この距離検出電圧信号Ｖ_L がコンパレータ４２の一方の
入力端子に入力されると、予め設定しておいた比較用電
圧Ｖ_REF より大きくなるため、コンパレータ４２の出力
端子から“Ｈ”が出力される。このコンパレータ４２の
出力端子から出力された“Ｈ”は、位置認識信号ＳＣと
して検出回路２５ａ，２５ｂ，２５ｃ，２５ｄに入力さ
れる。これにより、各検出回路２５ａ，２５ｂ，２５
ｃ，２５ｄのレジスタ２５５には、その時点の可逆カウ
ンタ２５２の値がセットされる。また、各検出回路２５
ａ，２５ｂ，２５ｃ，２５ｄの判定信号保持部２５７
は、“１”にセットされることになる。したがって、移
動前に判定信号保持部２５７をリセットしておけば、上
記ロボット１を移動中に正確な位置が認識できることに
なる。

【００４８】もし、レーザセンサ４０によりワーク３５
の位置が認識されなかった場合には、センサ処理装置４
１から距離検出電圧信号Ｖ_L は比較用電圧Ｖ_REF より低
いため、コンパレータ４２の出力（位置認識信号ＳＣ）
は“Ｌ”のままであり、第１アーム３及び第２アーム４
の移動完了後も判定信号保持部２５７はクリアされたま
まとなる。したがって、第１アーム３及び第２アーム４
の予定移動範囲を過ぎてもなお各検出回路２５ａ，２５
ｂ，２５ｃ，２５ｄの判定信号保持部２５７がリセット
状態であったときに、ＣＰＵ２１がこれらを読み取るこ
とにより、認識に異常があったことを知ることができ
る。なお、この応用例の場合には、位置認識信号ＳＣが
入力されたことを保持しておくため、判定信号保持部２
５７を必ず設けておく必要がある。

【００４９】

【発明の効果】以上の説明より明らかなように、請求項
１記載の発明によれば、各アームの角度を検出するセン
サからの検出信号を可逆カウンタで計数し、これら可逆
カウンタの計数値から各アームの移動量を算出すること
により各アームの大まかな位置・姿勢を求め、また、撮
像手段に撮像指令を出した時点における可逆カウンタの
値をレジスタに記憶させ、撮像時の各アームの角度及び
撮像データから得られるデータと、ロボットの座標系の
関係とが求められるので、ロボットを停止させることな
くロボットの位置、姿勢を精度良くかつ高速に求めるこ
とができる。

【００５０】また、請求項２記載の発明によれば、請求
項１記載の発明を実現するため、一つのセンサに対し
て、位相検出回路、可逆カウンタ、レジスタからなる検
出回路を設け、かつ各センサに対してそれぞれ検出回路
を設け、かつ前記各検出回路における可逆カウンタの計
数値を演算処理装置が読み込みその計数値からアームの
移動量を算出し、かつ撮像タイミングの可逆カウンタの
計数値をレジスタにセットし、そのセット値及び撮像デ
ータから得られるデータとロボットの座標系との関係と
を求めるようにしたので、ロボットを停止させることな
くロボットの位置、姿勢を精度良くかつ高速に求めるこ
とが可能なロボットの位置・姿勢認識装置を得ることが
できる。

【００５１】さらに、請求項３記載の発明では、請求項
２記載の発明における検出回路に、判定信号保持部を付
加し、認識信号が入力されたことをセットしておいて、
演算処理装置に認識信号が入力されたことを知らせるよ
うにしたので、演算処理装置とは同期しない認識信号で
も確実に各レジスタのセット値を演算処理装置に取り込
むことができる。

【図面の簡単な説明】

【図１】本発明のロボットの位置・姿勢認識方法を実現
するロボットの位置・姿勢認識装置の実施例を示すブロ
ック図である。

【図２】同ロボットの位置・姿勢認識装置の具体的構成
例を示すブロック図である。

【図３】同位相検出回路の具体的構成例を示す回路図で
ある。

【図４】同ロボットの位置・姿勢認識方法を説明するた
めの説明図である。

【図５】同ロボットの位置・姿勢認識装置の動作を示す
フローチャートである。

【図６】同位相検出回路の動作を説明するためのタイミ
ングチャートである。

【図７】同応用例を示すブロック図である。

【符号の説明】

１ ロボット ２ ベース ３ 第１アーム ４ 第２アーム ５ 第１モータ ６ 第２モータ ７ 第３モータ ８ 第４モータ １５ エンコーダ（センサ） １６ エンコーダ（センサ） １７ エンコーダ（センサ） １８ エンコーダ（センサ） ２０ ロボット制御装置 ２１ 演算処理装置（ＣＰＵ） ２５ａ，２５ｂ，２５ｃ，２５ｄ 検出回路 ２５１ 位相検出回路 ２５２ 可逆カウンタ ２５５ レジスタ ２５７ 判定信号保持部

Claims (3)

【特許請求の範囲】
1. 【請求項１】 各アームの角度を検出するセンサからの
   検出信号を可逆カウンタで計数し、これら可逆カウンタ
   の計数値から各アームの移動量を算出することにより各
   アームの大まかな位置・姿勢を求め、かつロボットアー
   ムの先端に設けられた視覚認識装置の撮像タイミングで
   位置認識信号を発生し、この撮像タイミングでの位置認
   識信号で可逆カウンタの計数値をレジスタに記憶させ、
   前記可逆カウンタ、レジスタの値及び撮像データとから
   得られるデータとロボットの座標系との関係を求めるこ
   とできることを特徴とするロボットの位置・姿勢認識方
   法。
2. 【請求項２】 アームの角度を検出して検出信号を出力
   するセンサと、該センサからの検出信号を位相検出して
   検波信号を得る位相検出回路、前記位相検出回路からの
   検波信号を可逆計数する可逆カウンタ、撮像タイミング
   で発生する位置認識信号により前記可逆カウンタの計数
   値をセットするレジスタからなる検出回路と、各アーム
   毎の検出回路における可逆カウンタの計数値を読み込み
   その計数値からアームの移動量を算出するとともに、前
   記各アーム毎の検出回路におけるレジスタにセットされ
   た値を読み込みそのセット値及び撮像データから得られ
   るデータとロボットの座標系との関係を求める演算処理
   装置とを備えたことを特徴とするロボットの位置・姿勢
   認識装置。
3. 【請求項３】 アームの角度を検出して検出信号を出力
   するセンサと、該センサからの検出信号を位相検出して
   検波信号を得る位相検出回路、前記位相検出回路からの
   検波信号を可逆計数する可逆カウンタ、撮像タイミング
   で発生する認識信号により前記可逆カウンタの計数値を
   セットするレジスタ、撮像タイミングで発生する認識信
   号によりセット状態になる判定信号保持部からなる検出
   回路と、各アーム毎の検出回路における可逆カウンタの
   計数値を読み込みその計数値からアームの移動量を算出
   するとともに、前記判定信号保持部がセット状態のとき
   に前記レジスタにセットされた値を読み込み、そのセッ
   ト値及び撮像データから得られるデータとロボットの座
   標系との関係を求める演算処理装置とを備えたことを特
   徴とするロボットの位置・姿勢認識装置。