JPH11165287A - 姿勢検出装置および姿勢制御装置並びにロボット制御装置 - Google Patents

Abstract

(57)【要約】 【課題】 簡素なシステムで高精度な姿勢検出を迅速に
行うことのできる姿勢検出装置を実現し、これを用いた
構成の簡素な姿勢制御装置およびロボット制御装置を提
供する。 【解決手段】 ロボットハンド１８について、直交する
水平軸Ｘ、Ｙの回りにおけるそれぞれの回転角を検出す
る傾斜センサ３１、３２と、ハンド１８に設けられた３
個の画像認識点Ｌ1、Ｌ2、Ｌ3と、これら画像認識点を
画像認識するＣＣＤカメラ４５と、このカメラ４５によ
る認識画像に基づいて水平軸Ｘ、Ｙと直交する鉛直なＺ
軸回りにおける、３個の画像認識点Ｌ1〜Ｌ3を含む平面
ＳＬの回転角Δｎを算出する回転角算出手段４７と、を
有しており、その算出手段４７の算出結果と、傾斜セン
サ３１、３２の検出回転角とから、ハンド１８の姿勢を
検出する。

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】本発明は、画像処理系を含む
姿勢検出装置とこれを用いた姿勢制御装置およびロボッ
ト制御装置に係り、より詳しくはロボットのハンド部の
姿勢および先端位置制御に好適な装置に関する。

【０００２】

【従来の技術】従来、３次元空間内で任意の方向に姿勢
変化する制御対象、例えばロボットのハンド部のような
対象物の位置制御を行う場合、対象物を支持するアーム
の撓みやこれを駆動する減速ギヤ等のバックラッシュに
よって精密位置決めが難しく、そのために、ロボットの
関節部に設けたポテンショメータやエンコーダ等の内界
センサのみならず、ロボットの外部から手先部をモニタ
する外界センサが併用されている。さらに、そのような
外界センサとして、例えばロボットの手先部をカメラを
通して画像認識するものが知られており、そのような画
像処理系を有するロボットとしては、マスタースレーブ
方式のロボット等のように人手による作業負荷を軽減し
たり、遠隔操作によって作業者の安全を確保したりする
作業支援ロボットがある。この種のロボットは、特に医
療福祉分野において今後の開発が期待されている。さら
に、ＣＣＤカメラを２台使用し、その三角測量の原理に
従って対象物の３点を画像認識し、ハンド先端部の位置
を検出する等の提案がなされている。

【０００３】

【発明が解決しようとする課題】しかしながら、従来の
姿勢検出装置およびロボット制御装置にあっては、上述
したような画像処理系を有する場合、例えば画像認識点
付近の背景の明るさが変化し易い等の理由で対象物を常
時明確に画像認識することができず、また、画像処理も
複雑であった。そのため、わずかな姿勢変化や変位を迅
速かつ正確に検出することができず、例えば遠隔操作に
よって作業者の安全を確保するような場合、簡素なシス
テムで制御対象の姿勢を高精度に検出し、精度に優れた
作業を行うというようなことは困難であった。また、従
来作業者の微妙な手作業に頼っているような作業を代行
し得るロボットが期待されてはいるものの、実際には上
記の困難性が顕著なために実用化できていない。しかし
ながら、高精度な姿勢検出を可能にし、高精度の作業支
援ロボットを開発すれば、人手作業によっては到底不可
能な高精度と作業速度（作業時間の短縮）が十分期待で
きることから、上記のような問題の解消が希求されてい
る。

【０００４】また、対象物のわずかな姿勢変化を迅速か
つ正確に検出できれば、作業支援ロボットに限らず、各
種の生産機械、例えば繊維加工機のように部分的に複雑
で細かい動きをするものにおいても、人手作業によって
は到底不可能な高精度で迅速な作業が期待できる。そこ
で本発明は、簡素なシステムで高精度な姿勢検出を迅速
に行うことのできる姿勢検出装置を実現し、これを用い
た構成の簡素な姿勢制御装置およびロボット制御装置を
提供することを目的とする。

【０００５】

【課題を解決するための手段】上記目的達成のため、本
発明の発明者は、対象物の僅かな姿勢変化をも明確に画
像認識可能にすべく鋭意研究を重ね、次のような発明に
到達した。すなわち、請求項１に記載の発明は、姿勢変
化可能な対象物について、互いに直交する第１および第
２の水平軸の回りにおけるそれぞれの回転角を検出する
第１および第２の傾斜センサと、前記対象物に設けられ
た３個の画像認識点（Ｌ1、Ｌ2、Ｌ3）と、該３個の画
像認識点を画像認識する画像認識手段と、該画像認識手
段による認識情報に基づいて、前記第１および第２の水
平軸と直交する鉛直軸の回りにおける、前記３個の画像
認識点を含む平面の回転角を算出する回転角算出手段
と、を備え、該回転角算出手段の算出結果と前記第１お
よび第２の傾斜センサにより検出された回転角とに基づ
いて、前記鉛直軸並びに前記第１および第２の水平軸を
直交３軸とする座標系における前記制御対象物の姿勢を
検出するものである。

【０００６】この請求項１に係る発明では、３個の画像
認識点を含む平面について第１および第２の水平軸と直
交する鉛直軸の回りの回転角を算出し、その算出結果と
第１および第２の傾斜センサにより検出された回転角と
に基づいて、対象物の姿勢が検出される。したがって、
画像データの処理は３個の画像認識点を含む平面の前記
鉛直軸回りの回転角を算出するだけでよく、迅速な姿勢
検出が可能になる。

【０００７】また、請求項２に記載のように、前記画像
認識点が前記対象物に取り付けられ発光する発光素子で
構成されていれば、焦点を絞って３個の画像認識点を明
確に画像認識することができ、請求項３に記載のよう
に、前記第１および第２の傾斜検出手段が、前記対象物
にそれぞれ取り付けられ前記第１および第２の水平軸の
回りにおける前記対象物のそれぞれの回転角を検出する
２個の傾斜センサを有すると、水平面に対する傾斜方向
の回転を簡素な構成で直接高精度に検出することがで
き、検出精度をも高めることができる。

【０００８】請求項４に記載の発明は、姿勢制御可能な
制御対象物と、該制御対象物の姿勢を制御して該制御対
象物の先端位置を制御する制御手段と、前記制御対象物
の姿勢を検出し制御手段にフィードバックする姿勢検出
手段と、を備えた姿勢制御装置において、前記姿勢検出
手段が、互いに直交する第１および第２の水平軸の回り
における前記制御対象物のそれぞれの回転角を検出する
第１および第２の傾斜センサと、前記制御対象物に設け
られた３個の画像認識点（Ｌ1、Ｌ2、Ｌ3）と、該３個
の画像認識点を画像認識する画像認識手段と、該画像認
識手段による認識情報に基づいて、前記第１および第２
の水平軸と直交する鉛直軸回りにおける該３個の画像認
識点を含む平面の回転角を算出する回転角算出手段と、
を有し、該回転角算出手段の算出結果と前記第１および
第２の傾斜センサにより検出された回転角とに基づい
て、前記鉛直軸並びに前記第１および第２の水平軸を直
交３軸とする座標系における前記制御対象物の姿勢を検
出するものである。

【０００９】この請求項４に係る発明では、第１および
第２の傾斜センサにより直接的に検出された傾斜方向の
回転角と、回転角算出手段の算出結果とを、制御手段へ
のフィードバック情報とすることで、画像データの処理
は３個の画像認識点を含む平面の前記鉛直軸回りの回転
角を算出するだけでよくなり、水平面に対する傾斜方向
の回転も第１および第２の傾斜センサからなる簡素な構
成で直接高精度に検出することができる。したがって、
迅速で高精度の姿勢制御ができる。

【００１０】さらに、請求項５に記載の発明は、アーム
および該アームに支持されたハンドの姿勢を制御し、該
ハンドの先端部を目標位置に移動させるとともに該ハン
ドを目標姿勢に制御する姿勢制御手段と、前記ハンドの
先端部の姿勢を検出し制御手段にフィードバックする姿
勢検出手段と、を備えたロボット制御装置において、前
記姿勢検出手段が、互いに直交する第１および第２の水
平軸の回りにおける前記ハンドのそれぞれの回転角を検
出する第１および第２の傾斜センサと、前記ハンドに取
り付けられ発光する３個の発光素子と、該３個の発光素
子の発光点（Ｌ1、Ｌ2、Ｌ3）を画像認識する画像認識
手段と、該画像認識手段による認識情報に基づいて、前
記第１および第２の水平軸と直交する鉛直軸回りにおけ
る該３個の発光点を含む平面の回転角を算出する回転角
算出手段と、を有し、該回転角算出手段の算出結果と前
記第１および第２の傾斜センサにより検出された回転角
とに基づいて、前記鉛直軸並びに前記第１および第２の
水平軸を直交３軸とする座標系における前記制御対象物
の姿勢を検出するものである。

【００１１】この請求項５に係る発明では、３個の発光
素子の発光点を正確に画像認識することで、これら発光
点を含む平面の鉛直軸回りにおける回転角が正確に算出
され、その算出結果と、第１および第２の傾斜センサに
より直接的に検出された傾斜方向の回転角とが、制御手
段にフィードバックされる。したがって、画像データの
処理は３個の画像認識点を含む平面の前記鉛直軸回りの
回転角を算出するだけでよくなり、水平面に対する傾斜
方向の回転も第１および第２の傾斜センサからなる簡素
な構成で直接高精度に検出することができるから、迅速
で高精度の姿勢制御ができる。さらに、水平面に対する
傾斜方向の回転を傾斜センサによって直接検出すること
で、アームの撓みや駆動系のバックラッシュ等に対する
補償をも可能にし、より精度に優れた姿勢制御を行うこ
とができる。また、請求項６に記載のように、前記第１
および第２の傾斜センサが前記ハンドにそれぞれ取り付
けられると、複雑な外界センサが必要ない簡素なロボッ
ト制御装置となる。

【００１２】請求項７に記載の発明は、アームおよび該
アームに支持されたハンドの姿勢を制御し、該ハンドの
先端部を目標位置に移動させるとともに該ハンドを目標
姿勢に制御する姿勢制御手段と、前記ハンドの先端部の
位置および姿勢を検出し前記姿勢制御手段にフィードバ
ックする検出手段と、を備えたロボット制御装置におい
て、前記検出手段が、前記ハンドに取り付けられ発光す
る３個の発光素子と、該３個の発光素子の発光点（Ｌ
1、Ｌ2、Ｌ3）を所定方向から画像認識する画像認識手
段と、該画像認識手段の認識画像に基づいて、前記３個
の発光点によって形成される三角形の認識画像と、前記
所定方向における中間基準位置で前記３個の発光点に対
応して形成される三角形の認識画像とを比較して、前記
所定方向における前記ハンドの先端部の位置を検出する
ものである。

【００１３】この請求項７に係る発明では、３個の発光
素子の発光点を所定方向から正確に画像認識でき、現在
の位置で３個の発光点によって形成される三角形の認識
画像と、ハンドの中間基準位置で形成される三角形の認
識画像とがほぼ相似関係にあることから、これらの大き
さの比と、画像認識手段から中間基準位置までの既知の
距離とから、前記中間基準位置に対する前記ハンドの位
置を容易に検出することができる。

【００１４】

【発明の実施の形態】以下、本発明の実施の形態につい
て添付図面を参照しつつ説明する。図１〜図１１は本発
明に係る姿勢検出装置およびロボット制御装置の一実施
形態を示す図であり、本発明を双腕多関節型ロボットに
適用した例を示している。まず、その構成について説明
すると、図１および図２において、１０は作業支援ロボ
ットとして使用される双腕多関節型ロボットの片腕側で
あり、この片腕側の多関節型ロボット１０は、基台部１
１と、基台１１から腕状に延びるよう互いに連結して設
けられた複数の可動アーム１２、１３、１４、１５、１
６および１７と、先端側の可動アーム１７に支持された
ハンド１８と、を具備している。多関節型ロボット１０
は、アーム揺動方向の旋回について４自由度、アーム捩
り方向の回転について３自由度、ハンド１８の把持動作
方向について１自由度の計８自由度を有しており、図２
に示すように、可動アーム１２、１３、１５および１７
がそれぞれアーム長さ方向の中心軸（以下、アーム中心
軸という）と直交する旋回中心軸を中心にしてアーム揺
動方向に旋回し、可動アーム１４、１６およびハンド１
８がアーム長さ方向のそれぞれの回転中心軸回りに回転
するようになっている。また、ハンド１８は相互に接近
および離隔するよう平行移動可能な左右一対の把持指部
２１、２２を有しており、これら把持指部２１、２２は
所定の把持対象物（例えば縫合針）を把持および解放す
るように往復動可能になっている。

【００１５】基台部１１から可動アーム１３までの第
１、第２関節は、それらの旋回中心軸１５、１６を互い
に直交しつつ離間させることによって、食い違いジンバ
ル機構を構成している。また、詳細は図示しないが、可
動アーム１５、１７の基端側の旋回中心軸１５ａ、１７
ａを関節軸とする第４および第６関節は、それぞれ平歯
車で減速した後にウォームホイールを用いて旋回駆動す
るものであり、可動アーム１４、１６およびハンド１８
の基端側の第３、第５および第７関節は、それぞれイン
ターナルギヤ（大径内歯車とそれに内接するピニオン）
によって可動アーム１４、１６およびハンド１８を回転
駆動するものである。

【００１６】なお、旋回部をウォームホイールで構成し
た理由は、簡素な機構で大きな減速比を得られること、
回転部をインターナルギヤで構成した理由はロボットの
回転中心を駆動歯車と同軸にとることができ、負荷負担
の面で有利だからである。また、通常のジンバル機構は
２軸が同一面上で直交するが、本実施形態における第
１、第２関節部に用いた食い違いジンバル機構は、直交
する２軸間の距離を目的とする可動領域が得られるまで
広げたものである。このジンバル機構の部分はリンクで
構成することもできるが、リンク機構では可動領域を３
０度以上とることが困難であり、剛性および可動領域に
おいて優れたジンバル機構の方が好ましい。前記直交す
る２軸間の距離を０とすると、２方向について互いに円
運動を行うので、この距離は短い方がよく、本実施形態
においては例えば５０mmとする。アーム１２〜１７の胴
部にはＣＦＲＰ（Carbon Fiber Reinforced Plastic
s）、すなわち、カーボンファイバーをアクリル樹脂で
硬化させた、アクリル−炭素繊維複合材を用いて軽量化
を図るとともに、曲げ圧縮強度を確保している。

【００１７】図２において、Ｐ１、Ｐ２、Ｒ３、Ｐ４、
Ｒ５、Ｐ６およびＲ７は、第１〜第７関節を駆動する駆
動用ＤＣサーボモータであり、これらは図３に示す左腕
側モータドライバ２５および右腕側モータドライバ２６
のうち一方を介し、コンピュータ２７（制御手段、回転
角算出手段）によって駆動制御される。コンピュータ２
７は、内部のメモリデバイスに格納された所定のプログ
ラムに従ってハンド１８の目標の位置および姿勢を決定
し、これらサーボモータＰ１、Ｐ２、Ｒ３、Ｐ４、Ｒ
５、Ｐ６およびＲ７からのそれぞれのフィードバック情
報と前記目標の位置および姿勢の情報に基づいて各モー
タの駆動量を演算し、各モータの駆動量を特定する命令
（指令信号）を出力する。そして、この命令に従って例
えばモータドライバ２５がＤＣサーボモータＰ１、Ｐ
２、Ｒ３、Ｐ４、Ｒ５、Ｐ６およびＲ７をＰＷＭ（ Pul
se Width Modulation；パルス幅変調）駆動し、これら
のモータＰ１、Ｐ２、Ｒ３、Ｐ４、Ｒ５、Ｐ６およびＲ
７が正転および逆転駆動されることで、図示しない減速
機を介して可動アーム１２〜１７およびハンド１８がそ
れぞれ旋回又は回転するようになっている。

【００１８】コンピュータ２７による各モータ駆動量の
演算に際しては、多関節型ロボットのアームの駆動や位
置決めに関する公知の制御と同様にモータＰ１、Ｐ２、
Ｒ３、Ｐ４、Ｒ５、Ｐ６およびＲ７についての基本的な
駆動量を求め、更に、以下に説明するような特定の姿勢
検出およびその検出結果をフィードバック情報とする姿
勢制御によって、所定のモータ、例えばモータＰ１、Ｐ
２およびＲ３のそれぞれの微小回転量に相当する角度補
正値（目標値からのずれに対する補正値）をれぞれ求め
て、ハンド１８の細かな姿勢制御を伴う先端位置制御を
行い、把持対象物の先端を高精度に位置決めするように
なっている。したがって、アームの駆動や位置決めに関
する基本的な制御については、これ以上の詳細説明を省
略し、以下にハンド１８の姿勢制御とそのための角度補
正の仕方、ハンド１８の把持制御等について説明する。

【００１９】まず、ハンド１８の姿勢制御のための構成
について説明すると、図４に示すように、ハンド１８に
は２個のトルク平衡型の傾斜センサ３１、３２が取り付
けられており、これら傾斜センサ３１、３２は絶対座標
３軸のうち鉛直軸を除く２軸について（図４においてハ
ンド１８の中心点Ｏを通って直交する水平な軸Ｘ、Ｙの
うち各軸回りで）、ハンド１８の回転角を検出できるよ
うになっている。また、ハンド部の姿勢を知るための残
りの１軸、すなわち鉛直軸について（図４においてハン
ド１８の中心点Ｏを通って水平軸Ｘ、Ｙと直交する鉛直
軸Ｚの軸回りで）の回転角は、傾斜センサでは検出でき
ないので、次に説明する画像処理系３３によって検出す
る。

【００２０】画像処理系３３は、ハンド１８に取り付け
られた位置および姿勢検出用の３個のＬＥＤ（Light-em
itting Diode；発光ダイオード）４１、４２、４３と、
ハンド１８に対するＬＥＤ４１〜４３の取付面側でハン
ド１８から所定距離を隔てた所定位置に固定設置された
ＣＣＤ（Charge Coupled Device）カメラ４５と、を含
んで構成されている。このＣＣＤカメラ４５は、画像処
理ボード（Image Processing Board）４６を介してコン
ピュータ４７に接続されており、ＣＣＤカメラ４５の出
力データを画像処理ボード４６により画像処理してコン
ピュータ４７に取り込む画像認識手段が構成されてい
る。また、コンピュータ４７は、内部のメモリデバイス
に格納された所定のプログラムに従い、コンピュータ２
７と協働して、予め準備された３次元ＣＡＤデータに基
づいてハンド１８の目標姿勢を決定し、そのイメージを
図５に仮想線で示すようにモニタ４８に表示させるとと
もに、これにＣＣＤカメラ４５の認識画像（同図に実線
で示すハンド１８のイメージ）を重ねて表示させる。こ
こで、絶対座標におけるハンド１８のＸ軸回りの回転角
をローリング角、Ｙ軸回りの回転角をピッチング角、ハ
ンド１８の鉛直軸回りの回転角をヨーイング角とする
と、ローリング角およびピッチング角は、それぞれ傾斜
センサ３１、３２によって検出され、Ａ／Ｄ（Analog-t
o-Digital；アナログ・デジタル）コンバータ４９を介し
てコンピュータ４７に常時フィードバックされる。ま
た、ハンド１８の目標姿勢からのずれ、すなわちＸ軸、
Ｙ軸、Ｚ軸回りで補正すべきハンド１８の回転角を、そ
れぞれ補正値Δｌ、Δｍ、Δｎとすると、補正値Δｌ、
Δｍはそれぞれ傾斜センサ３１、３２の検出値と目標回
転角の差としてコンピュータ２７又は４７（回転角算出
手段）によって即座に算出することができる。

【００２１】ハンド１８のヨーイング角については、Ｌ
ＥＤ４１〜４３の各発光点Ｌ1、Ｌ2、Ｌ3を含む平面ＳL
の絶対座標における傾斜姿勢から、例えば平面ＳLと絶
対座標における既知の基準平面ＳＴとがなす角ΔＡ（図
６参照）を用いて、計算により求めることができるが、
実際には目標姿勢に対応する平面を設定し、姿勢制御に
必要な補正値Δｎの計算処理を行えば足りる。

【００２２】本実施形態においては、ハンド１８の先端
位置決め制御において、ハンド１８の位置を検出すると
ともに、各発光点Ｌ1、Ｌ2、Ｌ3を含む平面ＳLの絶対座
標における傾斜姿勢を目標姿勢に近付けるように補正す
る制御を実行する。まず、図７に示すように、ＬＥＤ４
１〜４３の発光点Ｌ1、Ｌ2、Ｌ3によって形成される三
角形Ｌ1Ｌ2Ｌ3の認識画像に対し、ＣＣＤカメラ４５の
向き方向（所定方向）における既知の中間基準位置に前
記ＣＡＤデータを基にハンド１８が目標姿勢のときにＣ
ＣＤカメラ４５によって認識される発光点Ｌ1、Ｌ2、Ｌ
3の認識画像として三角形Ｐ1Ｐ2Ｐ3を設定し、この仮想
の三角形Ｐ1Ｐ2Ｐ3と三角形Ｌ1Ｌ2Ｌ3とを比較する。ま
た、このとき、ハンド１８の傾斜方向の姿勢の誤差は傾
斜センサ３１、３２の検出値に基づいて常時目標値に追
従させるフィードバック制御を行うことにより、三角形
Ｌ1Ｌ2Ｌ3と三角形Ｐ1Ｐ2Ｐ3は鉛直軸回りの回転（ヨー
イング）角の誤差をＣＣＤカメラ４５の認識画像情報に
基づいて補正すればよい状態にする。

【００２３】この補正について説明すると、まず、ロボ
ット１０の先端座標をＲ（ｘi、ｙi、ｚi）とすると、
第１〜第３関節の関節角度（モータＰ１、Ｐ２、Ｒ３が
回転する関節の角度）より、幾何学的に次のように表す
ことができる。 Ｒ（ｘi）＝（Ｃ₂・Ｃ₃・Ｃ₄＋Ｓ₂・Ｃ₄）Ｌ＋Ｓ₂・ｌ₃ ・・・（１） Ｒ（ｙi）＝｛−（Ｃ₁・Ｓ₂・Ｓ₃＋Ｓ₁・Ｃ₃）Ｓ₄＋Ｃ₁・Ｃ₂・Ｃ₄｝Ｌ ＋Ｃ₁・Ｃ₂・ｌ₃ ・・・（２） Ｒ（ｚi）＝｛−（Ｃ₁・Ｓ₂・Ｓ₃＋Ｓ₂・Ｃ₃）Ｓ₄ ＋Ｃ₁・Ｃ₂・Ｃ₄｝Ｌ ＋Ｃ₁・Ｃ₂・ｌ₃ ・・・（３） ここで、Ｃはcosθ、Ｓはsinθ、Ｌは可動アーム１４か
らハンド１８の先端までの全長（図２中にｌ₄ 〜ｌ₈ ）
であり、ＣおよびＳの添字はモータＰ１、Ｐ２、Ｒ３、
Ｐ４、Ｒ５、Ｐ６、Ｒ７に対応する関節番号を表すため
のもの、ｌの添字は図２に示す可動アーム１４〜１７の
各リンク長さｌ₄ 〜ｌ₇ 又はハンド１８の手首から指先
まで長さｌ₈ を表すためのものである。

【００２４】そして、これを用いて、目標姿勢に対する
ハンド１８のローリング角Δｌ、ピッチング角Δｍ、ヨ
ーイング角Δｎを、補正値として次の式から得ることが
できる。 Δｌ＝cos^-1（ｚ／ｌ₈ ）− cos^-1（Ｒ(ｚi)／ｌ₈ ） ・・・（４） Δｍ＝cos^-1（ｙ／ｌ₈ ）− cos^-1（Ｒ(ｙi)／ｌ₈ ） ・・・（５） Δｎ＝cos^-1（ｘ／ｌ₈ ）− cos^-1（Ｒ(ｘi)／ｌ₈ ） ・・・（６） ここで、ｘ、ｙ、ｚは、それぞれＣＣＤカメラ４５の認
識画像と前記ＣＡＤデータから特定されるハンド１８の
先端座標の各座標成分であり、ｌ₈ はハンド１８の手首
から手先までの長さである。

【００２５】上述のようにハンド１８の傾斜方向の回転
角Δｌ、Δｍについては傾斜センサ３１、３２からのフ
ィードバック情報に基づいて常時目標値に補正するか
ら、上記補正値のうち（４）、（５）式を用いた計算は
不要となる。また、負荷重量（これによるアームの撓
み）や駆動系のバックラッシュ等によってもハンド１８
の先端部位置は変化するが、傾斜センサ３１、３２の検
出情報に基づくフィードバック制御を実行することで、
これらアームの撓みや駆動系のバックラッシュ等による
影響をなくすことができる。

【００２６】ハンド１８の目標姿勢に対するヨーイング
角のずれについては、コンピュータ２７又は４７によっ
て（６）式から求めた補正値Δｎを使って、上記画像処
理による補正を繰り返し実行することで、モニタ座標系
とロボット１０の先端座標とを迅速にかつ正確に一致さ
せる。これにより、ハンド１８が目標姿勢をとることに
なる。因みに、傾斜センサ３１、３２の検出精度がそれ
ぞれ例えば±０．０８８度、前記三角形Ｌ1Ｌ2Ｌ3の回
転角についてのそれは±０．４度であるとすると、姿勢
制御の精度としては±０．８度が実現できる。

【００２７】また、ハンド１８のヨーイング角の補正に
より、現在の位置で３個の発光点によって形成される三
角形Ｌ1Ｌ2Ｌ3の認識画像と、ハンド１８の目標姿勢に
対応する三角形Ｐ1Ｐ2Ｐ3の認識画像とがほぼ相似形に
なることから、これらの大きさの比、例えば各辺の長さ
の比と、ＣＣＤカメラ４５から前記中間基準位置までの
既知の距離Ｓ１とによって、前記中間基準位置に対する
ハンド１８の位置、例えばＣＣＤカメラ４５から発光点
Ｌ1までの離間距離を正確に検出することができる。し
たがって、上述した姿勢制御を継続しながらハンド１８
の位置を目標位置に近づけることで、所要の先端位置の
位置決めが達成できる。

【００２８】次に、図８および図９を参照して把持制御
系について説明する。把持指部２１、２２の把持制御系
６０は、メインコンピュータ２７から独立して把持制御
を行うようになっており、図９（ａ）に示すように把持
指部２１、２２の左右両側面に貼り付けられた二対のス
トレーンゲージ８１、８２、８３、８４を有している。
これらストレーンゲージ８１〜８４は、図９（ｂ）に示
すように、電源電池６２と共に４ゲージ法ブリッジ回路
６１を形成するよう相互に接続されており、このブリッ
ジ回路６１の出力電圧はストレーンゲージアンプ６３に
より約２０００倍の増幅率で増幅されて歪み信号とな
り、この歪み信号が高速８ビットＡ／Ｄ（Analog-to-Di
gital；アナログ・デジタル）コンバータ６４によりデジ
タル信号変換され、インターフェース回路６６を介して
マイクロプロセッサからなるＣＰＵ（Central Processi
ng Unit）６７に取り込まれる。このＣＰＵ６７は、歪
み信号に基づいて把持指部２１、２２の把持力を演算す
るとともに、予めプログラミングされた設定把持力と演
算結果を比較し、把持力を加減するようモータドライバ
６８に命令（指令信号）を出す。このモータドライバ６
８は、ＣＰＵ６７からの命令に従ってマイクロモータで
あるＤＣサーボモータ６９をＰＷＭ駆動して正転および
逆転させ、減速機構７１を介して把持指部２１、２２を
開閉させる。そして、このような制御プログラムを高速
でループさせることにより、把持指部２１、２２に例え
ば図１０に示すような一定の把持力で把持対象物（縫合
針）を把持させる。なお、図１０は、縦軸を把持力
（Ｎ）、横軸を時間（秒）として、直径３．０（mm）の
針を把持力設定値６．０（Ｎ）および３．０（Ｎ）で把
持制御した場合の特性を示している。また、図１１に、
直径０．５（mm）の針を同様に把持制御した場合の特性
を示す。

【００２９】上述の装置においては、３個の画像認識点
Ｌ1、Ｌ2、Ｌ3を含む平面ＳＬについてＺ軸（鉛直軸）
の回りの回転角（ヨーイング角）が算出され、その算出
結果と傾斜センサ３１、３２により検出された回転角
（ローリング角、ピッチング角）とに基づいて、ハンド
１８の姿勢が検出される。したがって、 ＣＣＤカメラ
４５の認識画像データの処理は、主に３個の画像認識点
Ｌ1、Ｌ2、Ｌ3を含む平面ＳＬの、Ｚ軸回りにおける目
標姿勢からのずれ角（補正すべき値）を算出するために
行えばよく、迅速な算出によって姿勢検出速度を高める
ことができる。また、画像認識点が３個のＬＥＤ４１、
４２、４３によって構成されるので、背景色や使用環境
の明るさ等が変化しても、ＣＣＤカメラ４５の焦点を絞
る等調整して３個の画像認識点Ｌ1、Ｌ2、Ｌ3を常時明
確に画像認識することができる。さらに、ハンド１８に
傾斜センサ３１、３２がそれぞれ取り付けられているの
で、水平面に対するハンド１８の傾斜方向の回転角を簡
素な構成で直接高精度に検出することができ、複雑な外
界センサが必要ない簡素で高精度なロボット制御装置が
実現できる。

【００３０】さらに、本実施形態においては、第１およ
び第２の傾斜センサ３１、３２により直接的に検出され
た傾斜方向の回転角と、コンピュータ４７（回転角算出
手段）によるΔｎの算出結果とを、コンピュータ４７
（制御手段）へのフィードバック情報とすることで、第
１および第２の傾斜センサ３１、３２の検出値と目標姿
勢に対応する回転角とから補正値Δｌ、Δｍを求めるだ
けで、上述のように迅速な画像処理と高精度の姿勢検出
が可能になるから迅速で高精度の姿勢制御ができる。し
かも、水平面に対する傾斜方向の回転角を傾斜センサに
よって直接検出することで、アームの撓みや駆動系のバ
ックラッシュ等に対する補償をも可能にすることができ
る。また、上記姿勢制御に際する画像情報を利用して３
個の発光点で形成される三角形の認識画像と、ハンド１
８の目標姿勢に対応する三角形の認識画像とを比較し
て、ハンド１８の位置をも容易に検出することができ、
迅速で高精度な先端位置制御を実行することのできるロ
ボット制御装置を提供することができる。

【００３１】

【発明の効果】請求項１に係る発明によれば、画像デー
タの処理は前記画像認識点を含む平面の鉛直軸回りの回
転角を算出するための処理で済み、迅速な姿勢検出を行
うことができる。請求項２に係る発明によれば、発光素
子で構成された３個の画像認識点を明確に画像認識する
ことができる。

【００３２】請求項３に係る発明によれば、水平面に対
する傾斜方向の回転を簡素な構成で直接高精度に検出す
ることができ、検出精度をも高めることができる。請求
項４に係る発明によれば、第１および第２の傾斜センサ
により直接的に検出された傾斜方向の回転角と、回転角
算出手段の算出結果とを、制御手段へのフィードバック
情報とするので、画像データの処理は３個の画像認識点
を含む平面の前記鉛直軸回りの回転角を算出するだけで
よく、水平面に対する傾斜方向の回転も第１および第２
の傾斜センサからなる簡素な構成で直接高精度に検出す
ることができる。その結果、迅速で高精度の姿勢制御が
できる。

【００３３】請求項５に係る発明によれば、３個の発光
素子の発光点を正確に画像認識することで、これら発光
点を含む平面の鉛直軸回りにおける回転角が正確に算出
され、その算出結果と、第１および第２の傾斜センサに
より直接的に検出された傾斜方向の回転角とが制御手段
にフィードバックされるので、迅速で高精度の姿勢制御
ができ、さらに、アームの撓みや駆動系のバックラッシ
ュ等に対する補償もできる。

【００３４】請求項６に係る発明によれば、複雑な外界
センサが必要ない簡素なロボット制御装置となる。請求
項７に係る発明によれば、現在の位置で３個の発光点に
よって形成される三角形の認識画像と、ハンドの中間基
準位置で３個の発光点に対応して形成される三角形の認
識画像とを比較して、前記中間基準位置に対するハンド
の位置を容易に検出することができる。

【図面の簡単な説明】

【図１】本発明に係る姿勢検出装置、姿勢制御装置およ
びロボット制御装置の実施形態を示すその多関節ロボッ
ト全体の概略斜視図である。

【図２】図１の多関節ロボットのリンク結合および関節
動作方向を示す説明図である。

【図３】図１のロボットを制御するロボット制御装置の
全体構成図である。

【図４】図１のロボットのハンド部の姿勢検出を行う姿
勢検出装置の斜視図である。

【図５】図４に示す姿勢検出装置の作用説明図である。

【図６】図４に示す姿勢検出装置の他の作用説明図であ
る。

【図７】図１のロボットのハンド部の姿勢と先端位置を
検出する装置の斜視図である。

【図８】図１の多関節ロボットの把持制御系にのブロッ
ク構成図である。

【図９】図１の多関節ロボットの手先部の把持指部の斜
視図である。

【図１０】図９の把持指部に装着された把持力検出回路
の回路図である。

【図１１】図９の手先部の把持制御時の特性を示す特性
図である。

【符号の説明】

１０ 双腕多関節型ロボット（作業支援ロボット） １２、１３、１４、１５、１６、１７ 可動アーム １８ ハンド ２１、２２ 把持指 ２７ コンピュータ（制御手段、回転角算出手段） ３１、３２ 傾斜センサ ４１、４２、４３ ＬＥＤ（発光素子） ４５ ＣＣＤカメラ（画像認識手段） ４６ 画像処理ボード（画像認識手段） ４７ コンピュータ（画像認識手段、回転角算出手
段） Ｌ1、Ｌ2、Ｌ3 発光点（画像認識点） Ｐ１、Ｐ２、Ｒ３、Ｐ４、Ｒ５、Ｐ６、Ｒ７ 駆動用
ＤＣサーボモータ

Claims (7)

【特許請求の範囲】
1. 【請求項１】姿勢変化可能な対象物について、互いに直
   交する第１および第２の水平軸の回りにおけるそれぞれ
   の回転角を検出する第１および第２の傾斜センサと、 前記対象物に設けられた３個の画像認識点（Ｌ1、Ｌ2、
   Ｌ3）と、 該３個の画像認識点を画像認識する画像認識手段と、 該画像認識手段による認識情報に基づいて、前記第１お
   よび第２の水平軸と直交する鉛直軸の回りにおける、前
   記３個の画像認識点を含む平面の回転角を算出する回転
   角算出手段と、を有し、 該回転角算出手段の算出結果と前記第１および第２の傾
   斜センサにより検出された回転角とに基づいて、前記鉛
   直軸並びに前記第１および第２の水平軸を直交３軸とす
   る座標系における前記対象物の姿勢を検出することを特
   徴とする姿勢検出装置。
2. 【請求項２】前記画像認識点が前記対象物に取り付けら
   れ発光する発光素子で構成されたことを特徴とする請求
   項１に記載の姿勢検出装置。
3. 【請求項３】前記第１および第２の傾斜検出手段が、前
   記対象物にそれぞれ取り付けられ前記第１および第２の
   水平軸の回りにおける前記対象物のそれぞれの回転角を
   検出する２個の傾斜センサを有することを特徴とする請
   求項１に記載の姿勢検出装置。
4. 【請求項４】姿勢制御可能な対象物と、該対象物の姿勢
   を制御して該対象物の先端位置を制御する制御手段と、
   前記対象物の姿勢を検出し制御手段にフィードバックす
   る姿勢検出手段と、を備えた姿勢制御装置において、 前記姿勢検出手段が、 互いに直交する第１および第２の水平軸の回りにおける
   前記対象物のそれぞれの回転角を検出する第１および第
   ２の傾斜センサと、 前記対象物に設けられた３個の画像認識点（Ｌ1、Ｌ2、
   Ｌ3）と、 該３個の画像認識点を画像認識する画像認識手段と、 該画像認識手段による認識情報に基づいて、前記第１お
   よび第２の水平軸と直交する鉛直軸回りにおける該３個
   の画像認識点を含む平面の回転角を算出する回転角算出
   手段と、を有し、 該回転角算出手段の算出結果と前記第１および第２の傾
   斜センサにより検出された回転角とに基づいて、前記鉛
   直軸並びに前記第１および第２の水平軸を直交３軸とす
   る座標系における前記対象物の姿勢を検出することを特
   徴とする姿勢制御装置。
5. 【請求項５】アームおよび該アームに支持されたハンド
   の姿勢を制御し、該ハンドの先端部を目標位置に移動さ
   せるとともに該ハンドを目標姿勢に制御する姿勢制御手
   段と、前記ハンドの先端部の姿勢を検出し制御手段にフ
   ィードバックする姿勢検出手段と、を備えたロボット制
   御装置において、 前記姿勢検出手段が、 互いに直交する第１および第２の水平軸の回りにおける
   前記ハンドのそれぞれの回転角を検出する第１および第
   ２の傾斜センサと、 前記ハンドに取り付けられ発光する３個の発光素子と、 該３個の発光素子の発光点（Ｌ1、Ｌ2、Ｌ3）を画像認
   識する画像認識手段と、 該画像認識手段による認識情報に基づいて、前記第１お
   よび第２の水平軸と直交する鉛直軸回りにおける該３個
   の発光点を含む平面の回転角を算出する回転角算出手段
   と、を有し、 該回転角算出手段の算出結果と前記第１および第２の傾
   斜センサにより検出された回転角とに基づいて、前記鉛
   直軸並びに前記第１および第２の水平軸を直交３軸とす
   る座標系における前記ハンドの姿勢を検出することを特
   徴とするロボット制御装置。
6. 【請求項６】前記第１および第２の傾斜センサが、前記
   ハンドにそれぞれ取り付けられたことを特徴とする請求
   項５に記載のロボット制御装置。
7. 【請求項７】アームおよび該アームに支持されたハンド
   の姿勢を制御し、該ハンドの先端部を目標位置に移動さ
   せるとともに該ハンドを目標姿勢に制御する姿勢制御手
   段と、前記ハンドの先端部の位置および姿勢を検出し前
   記姿勢制御手段にフィードバックする検出手段と、を備
   えたロボット制御装置において、 前記検出手段が、 前記ハンドに取り付けられ発光する３個の発光素子と、 該３個の発光素子の発光点（Ｌ1、Ｌ2、Ｌ3）を所定方
   向から画像認識する画像認識手段と、 該画像認識手段の認識画像に基づいて、前記３個の発光
   点によって形成される三角形の認識画像と、前記所定方
   向における中間基準位置で前記３個の発光点に対応して
   形成される三角形の認識画像とを比較して、前記所定方
   向における前記ハンドの先端部の位置を検出することを
   特徴とするロボット制御装置。