JPH0630007B2 - ロボツト位置検出装置 - Google Patents

Description

【発明の詳細な説明】 「産業上の利用分野」 本発明は、ロボット位置検出装置に関し、更に詳しく
は、２以上の自由度を有し且つそれら自由度の間に干渉
があるロボットの各自由度の位置を検出するロボット位
置検出装置に関する。

「従来技術およびその問題点」 第１図は一般的な多関節型ロボットシステムを示すもの
で、多関節型ロボット１と、制御装置２と、リモートコ
ントローラ３とから基本的になっている。

多関節型ロボット１の手首部は、第１のひねりと曲げと
第２のひねりの合わせて３つの自由度を有しており、そ
の動力伝達機構として例えば第２図に示す如き動力伝達
機構５が用いられている。

この動力伝達機構５において、第１のひねりの自由度θ
_４は、モータＭ_４，減速機Ｇ_４および駆動パイプＰ_４に
より、第１の軸Ｌ_１が軸方向を変える自由度である。

曲げの自由度θ_５は、モータＭ_５，減速機Ｇ_５，駆動パ
イプＰ_５，ベベルギアＢ_１，Ｂ_２およびフレームＦによ
り、第２の軸Ｌ_２が第１の軸Ｌ_１のまわりに旋回される
自由度である。

また、第２のひねりの自由度θ_６は、モータＭ_６，減速
機Ｇ_６，駆動パイプＰ_６，ベベルギアＢ_３，Ｂ_４，Ｂ_５
により、第２の軸Ｌ_２が軸回転される自由度である。

モータＭ_４，Ｍ_５，Ｍ_６の各軸には、速度を検出するた
めの速度検出器Ｒ_４，Ｒ_５，Ｒ_６および角度位置を検出
するための角度検出器Ｅ_４，Ｅ_５，Ｅ_６が取り付けられ
ている。

さて、モータＭ_５およびＭ_６を停止して、モータＭ_４だ
けを回転させる場合を考えると、前述のようにモータＭ
_４の回転に従い第１のひねりの自由度θ_４が変化する。
すなわち第１の軸Ｌ_１が軸方向を変える。そして、それ
に伴ってベベルギアＢ_２が移動する。ところが、そのベ
ベルギアＢ_２に噛合するベベルギアＢ_１は、モータＭ_５
が停止しているから停止しており、このためベベルギア
Ｂ_２が回転することになる。そうすると、ベベルギアＢ
_２の回転によって、フレームＦが第１の軸Ｌ_１のまわり
に回転する。これは曲げの自由度θ_５が変化することに
他ならない。

同様に、モータＭ_６の停止によりベベルギアＢ_３が停止
しているにもかかわらず、第１のひねりθ_４の変化と共
にベベルギアＢ_４が移動されるので、そのベベルギアＢ
_４が回転されることとなり、これに噛合するベベルギア
Ｂ_５が回転し、第２の軸Ｌ_２が軸回転する。これは第２
のひねりの自由度θ_６が変化したことに他ならない。

また同様に、モータＭ_５を駆動して曲げの自由度θ_５を
変化すると、モータＭ_６を停止していてもベベルギアＢ
_５が回転し、第２の軸Ｌ_２が軸回転する。これは第２の
ひねりの自由度θ_６が変化したことに他ならない。

このように、２以上の自由度を有するロボットにおい
て、一つの自由度を変化すると、他の自由度が干渉を受
けて変化してしまうことがある。

ところで、角度検出器Ｅ_４，Ｅ_５，Ｅ_６はインクリメン
タル式で、構造の簡単化のために、モータＭ_４，Ｍ_５，
Ｍ_６の各軸に取り付けられるのが一般的であるから、そ
れらの検出値はモータＭ_４，Ｍ_５，Ｍ_６の相対的な回転
角度位置を表している。然るに、上述のように干渉があ
るから、モータＭ_４，Ｍ_５，Ｍ_６の回転角度位置は必ず
しも各自由度の角度位置に対応しない。

従って、自由度の間に干渉のある場合には、角度検出器
の検出量が各自由度の角度を表さないという問題点があ
る。

「発明の目的」 本発明の目的とするところは、２以上の自由度を有し且
つそれら自由度の間に干渉のあるロボットにおいて、そ
の干渉を考慮した演算を行って、各自由度の位置検出器
の検出量から各自由度の実際の位置を得ることができる
ロボット位置検出装置を提供することにある。

「発明の構成」 本発明のロボット位置検出装置は、第１番目から第Ｎ
（Ｎは２以上の整数）番目までの各自由度に対応して設
けられ、各自由度を駆動する駆動源の相対的な位置を表
す位置検出量Ｄ_ｉをそれぞれ出力する位置検出器、第ｍ
番目の自由度以外の自由度の移動によって第ｍ番目の自
由度が受ける干渉量に基づいて第ｍ番目の自由度につい
ての位置検出量Ｄ_ｍを補正する演算を行い、第ｍ番目の
自由度についての絶対的な位置Θ_ｍを得る位置演算部、
および各自由度についての絶対的な位置Θ_ｉを各自由度
の位置データとして出力する出力部を具備してなること
を構成上の特徴とするものである。

「実施例」 以下、図に示す実施例に基づいて本発明を更に詳しく説
明する。ここに第１図は本発明のロボット位置検出装置
の一実施例を含むロボットシステムの外観図、第２図は
第１図に示す多関節型ロボットの手首部の駆動機構の模
式的機構図、第３図は第１図に示すロボットシステムの
制御系統のブロック図、第４図は第３図に示す制御系統
における干渉補正演算部の一構成例のブロック図、第５
図は第３図に示す制御系統の関節角度演算部の一構成例
のブロック図である。なお、図に示す実施例により本発
明が限定されるものではない。

第１図に示すロボットシステムは、多関節型ロボット１
と、制御装置２と、リモートコントローラ３とから基本
的になっている。

多関節型ロボット１はθ_１〜θ_６の６つの自由度を有
し、そのうちθ_１〜θ_３はアームの自由度で、θ_４〜θ
_６は手首部の自由度である。

その手首部の駆動機構は、第２図に示すようであって、
第１のひねりの自由度θ_４は他の自由度θ_５，θ_６から
干渉を受けず、曲げの自由度θ_５は第１のひねりの自由
度θ_４から干渉を受け、第２のひねりの自由度θ_６は第
１のひねりの自由度θ_４および曲げの自由度θ_５の両者
から干渉を受ける。なお、この駆動機構の説明は、先に
「従来技術およびその問題点」の欄で詳細に説明したの
と同じであるから省略する。

制御装置２は、コンピュータを内蔵するもので、干渉補
正を行ってロボット１を制御する機能を持つ点および本
発明に係る演算処理を行って角度検出器の検出量から各
自由度の関節角度を得る機能を持つ点で従来と異なる。
これらの点以外の構成は従来と同様である。

リモートコントローラ３は、従来と同様の構成である。

第３図は第１図に示すロボットシステムの制御系統図
で、二点鎖線の左側部分は制御装置２に設けられ、二点
鎖線の右側部分はロボット１に設けられる。

オペレータが、制御盤２_ａまたはリモートコントローラ
３を介し、手先部の位置（ｘ，ｙ，ｚ）および姿勢
（ｌ，ｍ，ｎ）を教示すると、これらの教示データは、
順変換演算部２１によって、自由度θ_１，θ_２，θ_３，
θ_４，θ_５，θ_６がそれぞれ達成すべき目標角度位置φ
_１，φ_２，φ_３，φ_４，φ_５，φ_６に変換される。

また再生時には記憶装置に記憶された記憶データ（ｘ，
ｙ，ｚ，ｌ，ｍ，ｎ）が同様に変換される。

かかる順変換演算部２１は、従来公知の構成と同様であ
る。

次に、目標角度位置φ_１，φ_２，φ_３，φ_４，φ_５，φ
_６は、角度／制御量変換部２２によって、サーボ機構に
与えるべき目標制御量ｓ_１，ｓ_２，ｓ_３，ｓ_４，ｓ_５，
ｓ_６にそれぞれ変換される。

かかる角度／制御量変換部２２は、干渉について考慮し
ないもので、例えば、各自由度についての変換係数をＣ
_１，Ｃ_２，Ｃ_３，Ｃ_４，Ｃ_５，Ｃ_６とするとき、 ｓ_ｉ＝φ_ｉ／Ｃ_ｉ＋ｓ_ｉｏ… （ｉ＝１，２，３，４，５，６） （ｓ_ｉｏは定数） なる演算を行うものである。

次に、これら目標制御量ｓ_１〜ｓ_６に対して、干渉補正
演算部２３は、干渉を打消すための補正を行い、実際に
各自由度のサーボ機構に与えるべき制御量ｄ_１〜ｄ_６を
算出する。

例えば手首部の３つの自由度θ_４，θ_５，θ_６について
説明すると、まず、第１のひねりの自由度θ_４は他の自
由度からの干渉をうけないので、補正量＝０であり、第
４図に示すように、ｓ_４＝ｄ_４とされる。

次に、曲げの自由度θ_５は、第１のひねりの自由度θ_４
による干渉をうけるから、その干渉量をθ_４５とすれ
ば、そのθ_４５を相殺するだけの補正量ｈ_４５がｓ_５か
ら差引かれて実際の制御量ｄ_５とされる。

例えば、干渉量θ_４５が第１のひねりの自由度θ_４の駆
動量（ｄ_４−ｓ_４０）に比例するとすれば、比例定数を
α_４５として、 θ_４５＝（ｄ_４−ｓ_４０）α_４５ となり、このθ_４５を打消すためにモータＭ_５を駆動す
べき補正量ｈ_４５は、比例定数をβ_４５として、ｈ_４５
＝θ_４５β_４５となるから、比例定数をｋ_４５としてま
とめれば、 ｈ_４５＝（ｄ_４−ｓ_４０）ｋ_４５ となる。

そこで、第４図に演算部２３_ａで示すように、 ｄ_５＝ｓ_５−（ｄ_４−ｓ_４０）ｋ_４５… とされる。

次に、第２のひねりの自由度θ_６は、第１のひねりの自
由度θ_４による干渉と曲げの自由度θ_５による干渉をう
けるから、それらによる干渉量θ_４６，θ_５６を相殺す
るだけの補正量ｈ_４６，ｈ_５６をｓ_６から差引かれて実
際の制御量ｄ_６とされる。

前述の場合と同様に、 ｈ_４６＝（ｄ_４−ｓ_４０）ｋ_４６ ｈ_５６＝（ｄ_５−ｓ_５０）ｋ_５６ となるから、第４図に演算部２３_ｂで示すように、 ｄ_６＝ｓ_６−（ｄ_４−ｓ_４０）ｋ_４６ −（ｄ_５−ｓ_５０）ｋ_５６… とされる。

こうして得られた実際の制御量ｄ_ｉは、帰還制御部２４
に出力される。

帰還制御部２４は、各自由度の角度検出器Ｅ_１〜Ｅ_６の
角度検出量Ｄ_１〜Ｄ_６と制御量ｄ_１〜ｄ_６の偏差に基づ
いて、各サーボアンプＡ_１〜Ａ_６に駆動信号を出力す
る。

各サーボアンプＡ_１〜Ａ_６は、各速度検出器Ｒ_１〜Ｒ_６
の帰還を受けながら、各サーボモータＭ_１〜Ｍ_６を駆動
する。

かくして、各自由度の間の干渉が補正された制御が行わ
れ、各自由度θ_１〜θ_６は、干渉に左右されずに、目標
角度位置φ_１〜φ_６をそれぞれ達成しうることとなる。

さて、角度検出器Ｅ_ｉが出力する角度検出量Ｄ_ｉは、サ
ーボモータＭ_ｉが実際に駆動された量をあらわすもので
あり、干渉があるために各自由度の角度位置を表してい
ない。これらは、関節角度演算部２５によって変換され
ることではじめて角度位置Θ_ｉとして検出される。

すなわち、関節角度演算部２５は、他の自由度からの干
渉をうけない自由度θ_ｑについては、前記式を変形す
ることにより、 Θ_ｑ＝Ｃ_ｑ（Ｄ_ｑ−ｓ_ｑ０）… （ｑは他から干渉をうけない自由度の番号） （ｓ_ｑ０は定数） なる演算を行い、その絶対的な角度位置Θ_ｑを検出す
る。

例えば、手首部の第１のひねりの自由度θ_４は、他の自
由度からの干渉をうけないから、第５図に演算部２５_ａ
で示すように、 Θ_４＝Ｃ_４（Ｄ_４−ｓ_４０）… として角度位置Θ_４が検出される。

この角度位置Θ_４は、サーボモータＭ_４の回転のみによ
り達成された角度位置である。

一方、他の自由度からの干渉をうける自由度θ_ｐについ
ては、干渉による角度変化分を加える演算を行って、角
度位置Θ_ｐを検出する。

例えば、手首部の曲げの自由度θ_５で達成された角度位
置Θ_５は、自己のサーボモータＭ_５により生じる角度位
置Θ_５５に、第１のひねりの自由度θ_４のサーボモータ
Ｍ_４の回転により干渉で生じる角度変化分Θ_４５を加え
たものとなる。

すなわち、 Θ_５＝Θ_５５＋Θ_４５… ここで、上記式により、 Θ_５５＝Ｃ_５（Ｄ_５−ｓ_５０）… であり、Θ_４５が第１のひねりの自由度θ_４の実際の駆
動量（Ｄ_４−ｓ_４０）に比例するとすれば、比例定数を
Ｋ_４５として、 Θ_４５＝Ｋ_４５（Ｄ_４−ｓ_４０）… となるから、結局、第５図に演算部２５_ｂで示すよう
に、 Θ_５＝Ｃ_５（Ｄ_５＋ｓ_５０） ＋Ｋ_４５（Ｄ_４−Ｓ_４０）… なる演算を行うことで、角度位置Θ_５が検出される。

次に、第２のひねりの自由度θ_６で達成された角度位置
Θ_６は、第１のひねりの自由度θ_４による干渉と曲げの
自由度θ_５による干渉をうけるから、それらの干渉によ
る角度変化分Θ_４６，Θ_５６を、自己の達成分Θ_６６に
加えたものとなる。

すなわち、 Θ_６＝Θ_６６＋Θ_４６＋Θ_５６… ここで、 Θ_６６＝Ｃ_６（Ｄ_６−ｓ_６０） Θ_４６＝Ｋ_４６（Ｄ_４−ｓ_４０） Θ_５６＝Ｋ_５６（Ｄ_５−ｓ_５０） であるから、結局、第５図に演算部２５_ｃで示すよう
に、 Θ_６＝Ｃ_６（Ｄ_６−ｓ_６０） ＋Ｋ_４６（Ｄ_４−ｓ_４０） ＋Ｋ_５６（Ｄ_５−ｓ_５０）… なる演算を行うことで、角度位置Θ_６が検出される。

かくして、角度検出器Ｅ_１〜Ｅ_６の出力する位置検出量
Ｄ_１〜Ｄ_６から、実際に達成された角度位置Θ_１〜Θ_６
が検出されることとなる。

検出された角度位置Θ_１〜Θ_６は、逆変換演算部２６に
よって、手先部の位置（ｘ，ｙ，ｚ）および姿勢（ｌ，
ｍ，ｎ）に変換され、メモリーに記憶される。

逆変換演算部２６は、従来公知の構成と同様の構成であ
る。

上記説明から理解されるように、本発明に係る関節角度
演算部２５により、角度検出器Ｅ_１〜Ｅ_６の出力する位
置検出量Ｄ_１〜Ｄ_６から、干渉に左右されずに、実際に
達成された角度位置Θ_１〜Θ_６を検出できる。

また、干渉補正演算部２３により、各自由度の間の干渉
に左右されずに、多関節型ロボット１を制御できる。

他の実施例としては、角度／制御量変換部２２と干渉補
正演算部２３とを、関節角度演算部２５のように、一つ
にまとめて、目標角度位置φ_１〜φ_６から直接に実際の
制御量ｄ_１〜ｄ_６を算出するようにしたものが挙げられ
る。

また他の実施例としては、関節角度演算部２５を、角度
／制御量変換部２２と干渉補正演算部２３のように、２
つに分けて、まず角度検出量Ｄ_１〜Ｄ_６を干渉がない場
合の角度位置に変換し、次いで干渉補正を行って角度位
置Θ_１〜Θ_６を算出するようにしたものが挙げられる。

「発明の効果」 本発明によれば、第１番目から第Ｎ（Ｎは２以上の整
数）番目までの各自由度に対応して設けられ、各自由度
を駆動する駆動源の相対的な位置を表す位置検出量Ｄ_ｉ
をそれぞれ出力する位置検出器、第ｍ番目の自由度以外
の自由度の移動によって第ｍ番目の自由度が受ける干渉
量に基づいて第ｍ番目の自由度についての位置検出量Ｄ
_ｍを補正する演算を行い、第ｍ番目の自由度についての
絶対的な位置Θ_ｍを得る位置演算部、および各自由度に
ついての絶対的な位置Θ_ｉを各自由度の位置データとし
て出力する出力部を具備してなることを特徴とするロボ
ット位置検出装置が提供され、これにより複数の自由度
を有し且つ各自由度の間に干渉があるロボットにおい
て、各自由度の位置を、干渉に左右されずに、好適に検
出可能となる。

【図面の簡単な説明】

第１図は本発明のロボット位置検出装置の一実施例を含
むロボットシステムの外観図、第２図は第１図に示す多
関節型ロボットの手首部の駆動機構の模式的機構図、第
３図は第１図に示すロボットシステムの制御系統のブロ
ック図、第４図は第３図に示す制御系統における干渉補
正演算部の一構成例のブロック図、第５図は第３図に示
す制御系統の関節角度演算部の一構成例のブロック図で
ある。 （符号の説明） １…多関節型ロボット ２…制御装置 ３…リモートコントローラ ５…手首部の駆動機構 ２３…干渉補正演算部 ２３ａ，２３ｂ…演算部 ２５…関節角度演算部 ２５ａ，２５ｂ，２５ｃ…演算部 θ_４…手首部の第１のひねりの自由度 θ_５…手首部の曲げの自由度 θ_６…手首部の第２のひねりの自由度 Ｍ_４，Ｍ_５，Ｍ_６…サーボモータ Ｅ_４，Ｅ_５，Ｅ_６…角度検出器。

Claims (1)

【特許請求の範囲】
1. 【請求項１】(a)第１番目から第Ｎ（Ｎは２以上の整
   数）番目までの各自由度に対応して設けられ、各自由度
   を駆動する駆動源の相対的な位置を表す位置検出量Ｄ_ｉ
   をそれぞれ出力する位置検出器、 (b)第ｍ番目の自由度以外の自由度の移動によって第ｍ
   番目の自由度が受ける干渉量に基づいて第ｍ番目の自由
   度についての位置検出量Ｄ_ｍを補正する演算を行い、第
   ｍ番目の自由度についての絶対的な位置Θ_ｍを得る位置
   演算部、および (c)各自由度についての絶対的な位置Θ_ｉを各自由度の
   位置データとして出力する出力部を具備してなることを
   特徴とするロボット位置検出装置。