WO2016175157A1

WO2016175157A1 - 多関節アーム機構の動作制御装置及びロボット装置

Info

Publication number: WO2016175157A1
Application number: PCT/JP2016/062862
Authority: WO
Inventors: 尹　祐根; 順央川口
Original assignee: ライフロボティクス株式会社
Priority date: 2015-04-30
Filing date: 2016-04-24
Publication date: 2016-11-03
Also published as: JP6595206B2; TW201641233A; JP2016209937A

Abstract

ロボットアームの移動制御において位置誤差を解消又は減少させる。　ロボット装置の動作制御装置（１００）は、複数の関節部（Ｊ１－Ｊ６）にそれぞれ対応する現在の複数の関節変数から注目点の現在位置を計算する現在位置・姿勢計算部（１０４）と、現在位置から最終目標位置までの軌道を計算する軌道計算部（１０５）と、軌道上の単位時間ごとの複数の目標位置各々から次の目標位置までの距離と単位時間とに基づいて移動速度を計算し、計算された移動速度をロボットアーム機構（２００）の構造に応じたヤコビアン逆行列により複数の関節部（Ｊ１－Ｊ６）の複数の関節角速度に変換し、複数の関節角速度と単位時間とから次の目標位置に対応する複数の関節変数を複数の指令値として計算する指令値計算部（１０６）と、複数の指令値を複数のモータドライバ（２０１、２０３，２０５、２０７、２０９、２１１）にそれぞれ出力するドライバ制御部（１０３）とを具備する。

Description

多関節アーム機構の動作制御装置及びロボット装置

　本発明の実施形態は、多関節アーム機構の動作制御装置及びロボット装置に関する。

　近年ロボットがユーザと同一空間にいる環境が多くなってきている。介護用ロボットはもちろん産業用ロボットでも作業者と並んで作業を行なう状況が今後拡大していくものと考えられる。そのようなロボットの多くは垂直多関節アーム機構を備えている。垂直多関節アーム機構には位置に関して３自由度（ｘ，ｙ，ｚ）、姿勢に関して３自由度（α，β，γ）が要求され、一般的には根元３軸と呼ばれる回転関節部Ｊ１，Ｊ２，Ｊ３と手首３軸と呼ばれる回転関節部Ｊ４，Ｊ５，Ｊ６とからそれを実現している。例えば関節部Ｊ１，Ｊ４，Ｊ６にはねじり関節部、関節部Ｊ２，Ｊ３，Ｊ５には曲げ関節部が適用される。アーム先端にはハンド等の手先効果器（エンドエフェクタ）が装備される。

　その手先の位置、姿勢の動作制御としては様々な方式があるが、その一例として現在位置から最終目標位置までの軌道を計算し、その軌道上での１０ｍｓ等の単位時間（制御周期）ごとの個々の目標位置を求める。

　目標位置から複数の関節部それぞれの関節角度を同次変換行列を用いた逆運動学により求めるには代数的又は幾何学的解法が実時間制御では好ましいが、それを適用できるアーム機構が制限されており、近年ではそのような制限が少ないヤコビアン逆行列による方法が一般的によく用いられている。

　この方法では目標位置からヤコビアン逆行列により複数の関節部それぞれの関節角速度を計算し、その関節角速度を指令値としてモータドライバに出力する。モータドライバは各関節部の速度を目標値としてアクチュエータ（モータ）を駆動する。

　この方式では移動開始時や速度変化に伴う慣性誤差等により実際の手先の位置が計算上の位置から外れてしまう事があり、その誤差を修正するためにフィードバック制御が採用されることが多い。

　しかしフィードバック制御では修正動作が後追いとなりその位置の誤差修正は次の指令値に加えることとなるので位置誤差を完全に解消することはできず、また移動速度が不安定になる事態も生じる。

　目的は、ロボットアームの移動制御において位置誤差を解消又は減少させることにある。

　本実施形態に係る多関節アーム機構の動作制御装置は、多関節アーム機構を構成する複数の関節部のアクチュエータにそれぞれ対応する複数のモータドライバに複数の指令値をそれぞれ出力する。指令値の計算処理では、まず現在位置計算部で複数の関節部にそれぞれ対応する現在の複数の関節変数から多関節アーム機構上の注目点の現在位置を計算する。現在位置から最終目標位置までの注目点の軌道を軌道計算部で計算する。指令値計算部では、軌道上の単位時間ごとの複数の目標位置各々から次の目標位置までの距離と単位時間とに基づいて注目点の移動速度を計算し、計算された移動速度を多関節アーム機構の構造に応じたヤコビアン逆行列により複数の関節部にそれぞれ対応する複数の関節角速度に変換するとともに、計算された複数の関節角速度と単位時間とから次の目標位置に対応する複数の関節変数を複数の指令値として計算する。複数の指令値を複数のモータドライバに指令値出力部から出力する。

図１は、本実施形態に係るロボット装置のロボットアーム機構の外観斜視図である。図２は、図１のロボットアーム機構の内部構造を示す斜視図である。図３は、図１のロボットアーム機構を図記号表現により示す図である。図４は、本実施形態に係るロボット装置のブロック図である。図５は、図４の動作制御装置による動作制御の流れ図である。図６は、図５の工程Ｓ１４の流れ図である。図７は、図５の動作制御（位置指令）による効果を速度指令の場合と比較して説明するために第１関節部の関節角度の時間変化を示す図である。

　以下、図面を参照しながら本実施形態に係るロボット装置を説明する。本実施形態に係るロボット装置は動作制御装置を備える。動作制御装置は、ロボット装置に装備されている多関節アーム機構の各関節のモータドライバを制御する単独の装置として、多関節アーム機構を備えるロボット装置に組み込むことができる。本実施形態では、複数の関節部のうち一が直動伸縮関節の多関節アーム機構を有するロボット装置を説明する。以下の説明において、略同一の機能及び構成を有する構成要素については、同一符号を付し、重複説明は必要な場合にのみ行う。

　図１は、本実施形態に係るロボット装置のロボットアーム機構２００の外観斜視図である。ロボットアーム機構２００は、略円筒形状の基部１と基部１に接続されるアーム部２とアーム部２の先端に取り付けられる手首部４とを有する。手首部４には図示しないアダプタが設けられている。例えば、アダプタは後述の第６回転軸ＲＡ６の回転部に設けられる。手首部４に設けられたアダプタには、用途に応じたロボットハンドが取り付けられる。

　ロボットアーム機構２００は、複数、ここでは６つの関節部Ｊ１，Ｊ２，Ｊ３，Ｊ４，Ｊ５，Ｊ６を有する。複数の関節部Ｊ１，Ｊ２，Ｊ３，Ｊ４，Ｊ５，Ｊ６は基部１から順番に配設される。一般的に、第１、第２、第３関節部Ｊ１，Ｊ２，Ｊ３は根元３軸と呼ばれ、第４、第５、第６関節部Ｊ４，Ｊ５，Ｊ６はロボットハンド３の姿勢を変化させる手首３軸と呼ばれる。手首部４は第４、第５、第６関節部Ｊ４，Ｊ５，Ｊ６を有する。根元３軸を構成する関節部Ｊ１，Ｊ２，Ｊ３の少なくとも一つは直動伸縮関節である。ここでは第３関節部Ｊ３が直動伸縮関節部、特に伸縮距離の比較的長い関節部として構成される。アーム部２は直動伸縮関節部Ｊ３（第３関節部Ｊ３）の伸縮部分を表している。

　第１関節部Ｊ１は基台面に対して例えば垂直に支持される第１回転軸ＲＡ１を中心としたねじり関節である。第２関節部Ｊ２は第１回転軸ＲＡ１に対して垂直に配置される第２回転軸ＲＡ２を中心とした曲げ関節である。第３関節部Ｊ３は、第２回転軸ＲＡ２に対して垂直に配置される第３軸（移動軸）ＲＡ３を中心として直線的にアーム部２が伸縮する関節である。

　第４関節部Ｊ４は、第４回転軸ＲＡ４を中心としたねじり関節である。第４回転軸ＲＡ４は、後述の第７関節部Ｊ７が回転していないとき、つまりアーム部２の全体が直線形状にあるとき、第３移動軸ＲＡ３と略一致する。第５関節部Ｊ５は第４回転軸ＲＡ４に対して直交する第５回転軸ＲＡ５を中心とした曲げ関節である。第６関節部Ｊ６は第４回転軸ＲＡ４に対して直交し、第５回転軸ＲＡ５に対して垂直に配置される第６回転軸ＲＡ６を中心とした曲げ関節である。

　基部１を成すアーム支持体（第１支持体）１１ａは、第１関節部Ｊ１の第１回転軸ＲＡ１を中心に形成される円筒形状の中空構造を有する。第１関節部Ｊ１は図示しない固定台に取り付けられる。第１関節部Ｊ１が回転するとき、アーム部２は第１支持体１１ａの軸回転とともに左右に旋回する。なお、第１支持体１１ａが接地面に固定されていてもよい。その場合、第１支持体１１ａとは独立してアーム部２が旋回する構造に設けられる。第１支持体１１ａの上部には第２支持部１１ｂが接続される。

　第２支持部１１ｂは第１支持部１１ａに連続する中空構造を有する。第２支持部１１ｂの一端は第１関節部Ｊ１の回転部に取り付けられる。第２支持部１１ｂの他端は開放され、第３支持部１１ｃが第２関節部Ｊ２の第２回転軸ＲＡ２において回動自在に嵌め込まれる。第３支持部１１ｃは第１支持部１１ａ及び第２支持部に連通する鱗状の外装からなる中空構造を有する。第３支持部１１ｃは、第２関節部Ｊ２の曲げ回転に伴ってその後部が第２支持部１１ｂに収容され、また送出される。ロボットアーム機構２００の直動伸縮関節部Ｊ３（第３関節部Ｊ３）を構成するアーム部２の後部はその収縮により第１支持部１１ａと第２支持部１１ｂの連続する中空構造の内部に収納される。

　第３支持部１１ｃはその後端下部において第２支持部１１ｂの開放端下部に対して第２回転軸ＲＡ２を中心として回動自在に嵌め込まれる。それにより第２回転軸ＲＡ２を中心とした曲げ関節部としての第２関節部Ｊ２が構成される。第２関節部Ｊ２が回動するとき、アーム部２は第２回転軸ＲＡ２を中心に垂直方向に回動、つまり起伏動作をする。

　第４関節部Ｊ４は、アーム部２の伸縮方向に沿ったアーム中心軸、つまり第３関節部Ｊ３の第３移動軸ＲＡ３に典型的には接する第４回転軸ＲＡ４を有するねじり関節である。第４関節部Ｊ４が回転すると、手首部４及び手首部４に取り付けられたロボットハンドは第４回転軸ＲＡ４を中心に回転する。第５関節部Ｊ５は、第４関節部Ｊ４の第４回転軸ＲＡ４に対して直交する第５回転軸ＲＡ５を有する曲げ関節部である。第５関節部Ｊ５が回転すると、第５関節部Ｊ５から先端にかけてロボットハンドとともに上下（第５回転軸ＲＡ５を中心に垂直方向）に回動する。第６関節部Ｊ６は、第４関節部Ｊ４の第４回転軸ＲＡ４に直交し、第５関節部Ｊ５の第５回転軸ＲＡ５に垂直な第６回転軸ＲＡ６を有する曲げ関節である。第６関節部Ｊ６が回転すると、ロボットハンドは左右に旋回する。

　上記の通り手首部４のアダプタに取り付けられたロボットハンドは、第１、第２、第３関節部Ｊ１．Ｊ２．Ｊ３により任意位置に移動され、第４、第５、第６関節部Ｊ４、Ｊ５、Ｊ６により任意姿勢に配置される。特に第３関節部Ｊ３のアーム部２の伸縮距離の長さは、基部１の近接位置から遠隔位置までの広範囲の対象にロボットハンドを到達させることを可能にする。第３関節部Ｊ３はそれを構成する直動伸縮機構により実現される直線的な伸縮動作とその伸縮距離の長さとが特徴的である。

　（内部構造の説明）　
　図２は、図１のロボットアーム機構２００の内部構造を示す斜視図である。直動伸縮機構はアーム部２と射出部３０とを有する。アーム部２は第１連結コマ列２１と第２連結コマ列２２とを有する。第１連結コマ列２１は複数の第１連結コマ２３からなる。第１連結コマ２３は略平板形に構成される。前後の第１連結コマ２３は、互いの端部箇所においてピンにより屈曲自在に列状に連結される。第１連結コマ列２１は内側や外側に自在に屈曲できる。

　第２連結コマ列２２は複数の第２連結コマ２４からなる。第２連結コマ２４は横断面コ字形状の短溝状体に構成される。前後の第２連結コマ２４は、互いの底面端部箇所においてピンにより屈曲自在に列状に連結される。第２連結コマ列２２は内側に屈曲できる。第２連結コマ２４の断面はコ字形状であるので、第２連結コマ列２２は、隣り合う第２連結コマ２４の側板同士が衝突して、外側には屈曲しない。なお、第１、第２連結コマ２３、２４の第２回転軸ＲＡ２に向いた面を内面、その反対側の面を外面というものとする。第１連結コマ列２１のうち先頭の第１連結コマ２３と、第２連結コマ列２２のうち先頭の第２連結コマ２４とは結合コマ２７により接続される。例えば、結合コマ２７は第２連結コマ２４と第１連結コマ２３とを合成した形状を有している。

　射出部３０は、複数の上部ローラ３１と複数の下部ローラ３２とが角筒形状のフレーム３５に支持されてなる。例えば、複数の上部ローラ３１は第１連結コマ２３の長さと略等価な間隔を隔ててアーム中心軸に沿って配列される。同様に、複数の下部ローラ３２は第２連結コマ２４の長さと略等価な間隔を隔ててアーム中心軸に沿って配列される。射出部３０の後方には、ガイドローラ４０とドライブギア５０とが第１連結コマ列２１を挟んで対向するように設けられる。ドライブギア５０は図示しない減速器を介してモータ５５に接続される。第１連結コマ２３の内面には連結方向に沿ってリニアギアが形成されている。複数の第１連結コマ２３が直線状に整列されたときに互いのリニアギアは直線状につながって、長いリニアギアを構成する。ドライブギア５０は、直線状のリニアギアにかみ合わされる。直線状につながったリニアギアはドライブギア５０とともにラックアンドピニオン機構を構成する。

　（接合構造の説明）　
　アーム伸長時、モータ５５が駆動し、ドライブギア５０が順回転すると、第１連結コマ列２１はガイドローラ４０により、アーム中心軸と平行な姿勢となって、上部ローラ３１と下部ローラ３２との間に誘導される。第１連結コマ列２１の移動に伴い、第２連結コマ列２２は射出部３０の後方に配置された図示しないガイドレールにより射出部３０の上部ローラ３１と下部ローラ３２との間に誘導される。上部ローラ３１と下部ローラ３２との間に誘導された第１、第２連結コマ列２１，２２は互いに押圧される。これにより、第１、第２連結コマ列２１，２２による柱状体が構成される。射出部３０は、第１、第２連結コマ列２１，２２を接合して柱状体を構成するとともに、その柱状体を上下左右に支持する。第１、第２連結コマ列２１、２２の接合による柱状体が射出部３０により堅持されることで、第１、第２連結コマ列２１，２２の接合状態が保持される。第１、第２連結コマ列２１、２２の接合状態が維持されているとき、第１、第２連結コマ列２１，２２の屈曲は互いに拘束される。それにより第１、第２連結コマ列２１、２２は、一定の剛性を備えた柱状体を構成する。柱状体とは、第２連結コマ列２２に第１連結コマ列２１が接合されてなる柱状の棒体を言う。この柱状体は第２連結コマ２４が第１連結コマ２３とともに全体として様々な断面形状の筒状体に構成される。筒状体とは上下左右が天板、底板及び両側板で囲まれ、前端部と後端部とが開放された形状として定義される。第１、第２連結コマ列２１、２２の接合による柱状体は、結合コマ２７が始端となって、第３移動軸ＲＡ３に沿って直線的に第３支持部１１ｃの開口から外に向かって送り出される。

　アーム収縮時、モータ５５が駆動し、ドライブギア５０が逆回転されると、ドライブギア５０と係合している第１連結コマ列２１が第１支持体１１ａ内に引き戻される。第１連結コマ列の移動に伴って、柱状体が第３支持体１１ｃ内に引き戻される。引き戻された柱状体は射出部３０後方で分離される。例えば、柱状体を構成する第１連結コマ列２１はガイドローラ４０とドライブギア５０とに挟まれ、柱状体を構成する第２連結コマ列２２は重力により下方に引かれ、それにより第２連結コマ列２２と第１連結コマ列２１とは互いに離反される。離反された第１、第２連結コマ列２１，２２はそれぞれ屈曲可能な状態に復帰し、個々に屈曲され、第１支持体１１ａの内部に格納される。

　（座標系の定義）　
　図３は、図１のロボットアーム機構２００を図記号表現により示す図である。ロボットアーム機構２００において、根元３軸を構成する第１関節部Ｊ１と第２関節部Ｊ２と第３関節部Ｊ３とにより３つの位置自由度が実現される。また、手首３軸を構成する第４関節部Ｊ４と第５関節部Ｊ５と第６関節部Ｊ６とにより３つの姿勢自由度が実現される。

　第１関節部Ｊ１は、第１支持部１１ａと第２支持部１１ｂとの間に配設されており、回転軸ＲＡ１を中心としたねじり関節として構成されている。回転軸ＲＡ１は第１関節部Ｊ１の固定部が設置される基台の基準面ＢＰに垂直に配置される。回転軸ＲＡ１に平行にＺ軸を規定する。Ｚ軸を中心とした直交３軸のロボット座標系Σｂ（Ｘｂ,Ｙb,Ｚb）を規定する。

　第２関節部Ｊ２は回転軸ＲＡ２を中心とした曲げ関節として構成される。第２関節部Ｊ２の回転軸ＲＡ２は空間座標系上のＸb軸に平行に設けられる。第２関節部Ｊ２の回転軸ＲＡ２は第１関節部Ｊ１の回転軸ＲＡ１に対して垂直な向きに設けられる。さらに第２関節部Ｊ２は、第１関節部Ｊ１に対して、第１回転軸ＲＡ１の方向（Ｚb軸方向）と第１回転軸ＲＡ１に垂直なＹb軸方向との２方向に関してオフセットされる。第２関節部Ｊ２が第１関節部Ｊ１に対して上記２方向にオフセットされるように、第２支持体１１ｂは第１支持体１１ａに取り付けられる。第１関節部Ｊ１に第２関節部Ｊ２を接続する仮想的なアームロッド部分（リンク部分）は、先端が直角に曲がった２つの鈎形状体が組み合わされたクランク形状を有している。この仮想的なアームロッド部分は、中空構造を有する第１、第２支持体１１ａ、１１ｂにより構成される。

　第３関節部Ｊ３は移動軸ＲＡ３を中心とした直動伸縮関節として構成される。第３関節部Ｊ３の移動軸ＲＡ３は第２関節部Ｊ２の回転軸ＲＡ２に対して垂直な向きに設けられる。第２関節部Ｊ２の回転角がゼロ度、つまりアーム部２の起伏角がゼロ度であってアーム部２が水平な基準姿勢においては、第３関節部Ｊ３の移動軸ＲＡ３は、第２関節部Ｊ２の回転軸ＲＡ２とともに第１関節部Ｊ１の回転軸ＲＡ１にも垂直な方向に設けられる。空間座標系上では、第３関節部Ｊ３の移動軸ＲＡ３はＸb軸及びＺb軸に対して垂直なＹb軸に平行に設けられる。さらに、第３関節部Ｊ３は、第２関節部Ｊ２に対して、その回転軸ＲＡ２の方向（Ｙb軸方向）と、移動軸ＲＡ３に直交するＺb軸の方向との２方向に関してオフセットされる。第３関節部Ｊ３が第２関節部Ｊ２に対して上記２方向にオフセットされるように、第３支持体１１ｃは第２支持体１１ｂに取り付けられる。第２関節部Ｊ２に第３関節部Ｊ３を接続する仮想的なアームロッド部分（リンク部分）は、先端が垂直に曲がった鈎形状体を有している。この仮想的なアームロッド部分は、第２、第３支持体１１ｂ、１１ｃにより構成される。

　第４関節部Ｊ４は回転軸ＲＡ４を中心としたねじり関節として構成される。第４関節部Ｊ４の回転軸ＲＡ４は第３関節部Ｊ３の移動軸ＲＡ３に略一致するよう配置される。　
　第５関節部Ｊ５は回転軸ＲＡ５を中心とした曲げ関節として構成される。第５関節部Ｊ５の回転軸ＲＡ５は第３関節部Ｊ３の移動軸ＲＡ３及び第４関節部Ｊ４の回転軸ＲＡ４に略直交するよう配置される。　
　第６関節部Ｊ６は回転軸ＲＡ６を中心としたねじり関節として構成される。第６関節部Ｊ６の回転軸ＲＡ６は第４関節部Ｊ４の回転軸ＲＡ４及び第５関節部Ｊ５の回転軸ＲＡ５に略直交するよう配置される。第６関節部Ｊ６は手先効果器としてのロボットハンドを左右に旋回するために設けられている。なお、第６関節部Ｊ６は、その回転軸ＲＡ６が第４関節部Ｊ４の回転軸ＲＡ４及び第５関節部Ｊ５の回転軸ＲＡ５に略直交する曲げ関節として構成されてもよい。

　このように複数の関節部Ｊ１－Ｊ６の根元３軸のうちの一つの曲げ関節部を直動伸縮関節部に換装し、第１関節部Ｊ１に対して第２関節部Ｊ２を２方向にオフセットさせ、第２関節部Ｊ２に対して第３関節部Ｊ３を２方向にオフセットさせることにより、本実施形態に係るロボット装置のロボットアーム機構２００は、特異点姿勢を構造上解消している。

　（ブロック構成）　
　図４は、本実施形態に係るロボット装置のブロック図である。なお、本実施形態に係るロボットアーム機構２００の関節部Ｊ１，Ｊ２，Ｊ３，Ｊ４，Ｊ５、Ｊ６には、アクチュエータとして、例えばステッピングモータが設けられている。これらステッピングモータには、モータドライバ２０１、２０３，２０５、２０７，２０９、２１１がそれぞれ電気的に接続されている。また、これらステッピングモータ２０１、２０３，２０５、２０７，２０９、２１１のドライブシャフトには、一定の回転角ごとに出力されるパルスをカウンタで加減算して角位置を測定するためのロータリーエンコーダ２０２，２０４，２０６，２０８，２１０，２１２がそれぞれ接続されている。

　動作制御装置１００は、関節部Ｊ１－Ｊ６の動作制御することにより手先等の注目点の移動を制御するために設けられ、システム制御部１０１と、操作部インターフェース（Ｉ／Ｆ）１０２と、ドライバ制御部１０３と、現在位置・姿勢計算部１０４、軌道計算部１０５と、指令値計算部１０６とを有する。　
　システム制御部１０１は、ＣＰＵ（Ｃｅｎｔｒａｌ　Ｐｒｏｃｅｓｓｉｎｇ　Ｕｎｉｔ）と半導体メモリ等を有し、動作制御装置１００を統括して制御する。システム制御部１０１には、制御／データバス１０９を介して各部が接続されている。

　動作制御装置１００には、操作部インターフェース１０２を介して操作部３００が接続されている。操作部３００は、手首部４に取り付けられたロボットハンド（手先効果器）の移動や姿勢の変更をユーザが入力するための入力インターフェースとして機能する。動作制御装置１００では例えばロボットハンドの把持中心点に注目してその点（手先基準点）を制御点（手先座標系Σhの原点）として移動・姿勢変更等の計算処理を実行する。動作制御装置１００には、操作部３００を介して、手先の最終目標位置等が入力される。操作部３００は、例えば、スイッチ、マウス、キーボード、トラックボールおよびタッチパネル等の入力デバイスを有する。

　ドライバ制御部１０３は、モータドライバ２０１、２０３，２０５、２０７，２０９、２１１を統括して制御する。ドライバ制御部１０３は、後述の指令値計算部１０６で関節部Ｊ１－Ｊ６ごとに計算された指令値に応じた制御信号をモータドライバ２０１、２０３，２０５、２０７，２０９、２１１各々に対して送信する。

　現在位置・姿勢計算部１０４は、関節部Ｊ１，Ｊ２，Ｊ３，Ｊ４，Ｊ５、Ｊ６各々の関節変数に基づいて、アーム構造のリンクパラメータに応じて既定された同次変換行列に従って順運動学によりロボット座標系から見た手先基準点の位置と手先姿勢とを計算する。なお関節変数とは、関節部Ｊ１，Ｊ２，Ｊ４，Ｊ５、Ｊ６では基準位置からの正負の回転角度であり、関節部Ｊ３であれば最も収縮した状態からの伸張距離（直動変位）である。

　図３に示すように、ロボット座標系Σbは第1関節部Ｊ１の第１回転軸ＲＡ１上の任意位置を原点とした座標系である。ロボット座標系Σｂにおいて、直交３軸（Ｘｂ、Ｙｂ，Ｚｂ）が規定されている。Ｚｂ軸は第１回転軸ＲＡ１に平行な軸である。Ｘｂ軸とＹｂ軸とは互いに直交し、且つＺb軸に直交する軸である。手先座標系Σｈは、手首部４に取り付けられたロボットハンドの任意位置（手先基準点）を原点とした座標系である。例えば、ロボットハンドが２指ハンドのとき、手先基準点の位置は２指先間中央位置に規定される。手先座標系Σｈにおいて、直交３軸（Ｘｈ、Ｙｈ，Ｚｈ）が規定されている。Ｚｈ軸は第６回転軸ＲＡ６に平行な軸である。Ｘｈ軸とＹｈ軸とは互いに直交し、且つＺｈ軸に直交する軸である。例えば、Ｘｈ軸は、ロボットハンドの前後方向に平行な軸である。手先姿勢とは、手先座標系Σhのロボット座標系Σｂに対する直交３軸各々周りの回転角（Ｘｈ軸周りの回転角（ヨウ角）α、Ｙｈ軸周りの回転角（ピッチ角）β、Ｚｈ軸周りの回転角（ロール角）γとして与えられる。

　現在位置・姿勢計算部１０４は、エンコーダ２０２，２０４，２０６，２０８，２１０，２１２のエンコーダパルスに基づいて、ロボットアーム機構２００の関節変数ベクトル^－θを計算する。関節変数ベクトル^－θは回転関節部Ｊ１，Ｊ２，Ｊ４、Ｊ５，Ｊ６の関節角度θ_Ｊ１，θ_Ｊ２，θ_Ｊ４、θ_Ｊ５、θ_Ｊ６と直動伸縮関節部Ｊ３の伸縮長Ｌ_Ｊ３とで与えられる関節変数の組（θ_Ｊ１，θ_Ｊ２，Ｌ_Ｊ３、θ_Ｊ４、θ_Ｊ５、θ_Ｊ６）である。現在位置・姿勢計算部１０４は、同次変換行列Ｋ（パラメータ（θ_Ｊ１，θ_Ｊ２，Ｌ_Ｊ３、θ_Ｊ４、θ_Ｊ５、θ_Ｊ６））により、ロボット座標系Σｂから見た手先基準点の位置（ｘ、ｙ、ｚ）と手先姿勢（α、β、γ）とを計算する。同次変換行列Ｋは、手先座標系Σｈとロボット座標系Σbとの関係を定義する行列式である。同次変換行列Ｋは、ロボットアーム機構２００を構成するリンク間の関係（リンク長とリンクのねじれ角）と関節部の軸間の関係（リンク間距離とリンクの間角度）とで決まる。例えば、現在位置・姿勢計算部１０４は、現在のロボットアーム機構２００の関節変数ベクトル^－θ_０（θ_０－Ｊ１，θ_０－Ｊ２，Ｌ_０－Ｊ３、θ_０－Ｊ４、θ_０－Ｊ５、θ_０－Ｊ６）を同次変換行列Ｋに代入することにより、ロボット座標系Σｂから見た手先基準点の現在位置ｘ_０（ｘ_０、ｙ_０、ｚ_０）と手先姿勢φ_０（α_０、β_０、γ_０）とを計算する。

　軌道計算部１０５は、手先基準点（以下単に手先という）の現在位置・姿勢と手先の最終目標位置・姿勢とに基づいて、単位時間（制御周期、例えば１０ｍｓ）毎の手先の目標位置を計算する。目標位置を与えることで、目標位置で規定された位置と姿勢をとって、手先を移動させることができる。手先の現在位置・姿勢は、現在位置・姿勢計算部１０４による計算処理から得られる。手先の最終目標位置・姿勢は、例えば、操作部３００を介してユーザにより入力される。

　軌道計算部１０５は、予めプリセットされている、手先の現在位置ｘ_０、現在姿勢φ_０、手先の最終目標位置Ｘ_１、最終目標姿勢Φ_１をパラメータとする多項式に、各パラメータを代入することにより手先の軌道（以下、手先軌道という）を計算し、その手先軌道上に単位時間Δｔ毎の目標位置を計算する。軌道計算方法としては任意の方法が採用される。単位時間Δｔは制御周期として固定値であり、ユーザにより、例えば１０ｍｓに設定されている。軌道計算部１０５は、手先を現在位置から最終目標位置への移動に要する目標時間幅Ｔと単位時間Δｔとに基づいて、目標位置の数ｍ（＝Ｔ／Δｔ）を決定し、単位時間Δｔ毎の手先軌道上の手先の目標位置（ｘ_１、ｘ_２・・・ｘ_ｍ（ｍ＝ｔ１／Δｔ））を計算する。例えば、目標位置ｘ_１は、手先基準点の位置ｘ_１と手先姿勢φ_１との両方を与えるパラメータとする。目標時間Ｔは、例えば、ユーザにより直接入力されてもよいし、ユーザが操作部３００を操作している期間に応じて決定されてもよいし、手先の現在位置ｘ_０と手先の最終目標位置Ｘ_１との距離に応じて自動的に決定されてもよい。例えば、手先軌道は、手先の現在位置ｘ_０と手先の最終目標位置Ｘ_１とを結ぶ直線軌道として計算される。

　指令値計算部１０６は、手先の現在位置と手先の最終目標位置との間の複数の目標位置に関する複数の関節変数ベクトルを計算する。なお、関節変数ベクトルとは関節部Ｊ１－Ｊ６の６つの関節変数、つまり回転関節部Ｊ１、Ｊ２、Ｊ４－Ｊ６の回転角と直動伸縮関節部Ｊ３のアーム伸縮長とからなる。指令値計算部１０６の計算処理については後述する。図５は、図４の動作制御装置１００による動作制御の流れ図である。

　（工程Ｓ１１）現在の関節変数ベクトル^－θ_０を計算　
　システム制御部１０１の制御に従って、現在位置・姿勢計算部１０４により、エンコーダ２０２，２０４，２０６，２０８，２１０，２１２から入力されたエンコーダパルスに基づいて、現在の関節変数ベクトル^－θ_０（θ_０－Ｊ１、θ_０－Ｊ２、Ｌ_０－Ｊ３、θ_０－Ｊ４、θ_０－Ｊ５、θ_０－Ｊ６）が計算される。　
　（工程Ｓ１２）手先の現在位置ｘ_０を計算　
　システム制御部１０１の制御に従って、現在位置・姿勢計算部１０４により、現在の関節変数ベクトル^－θ_０に基づいて、ロボット座標系Σｂ上の手先の現在位置ｘ_０（ｘ_０、ｙ_０、ｚ_０）と手先姿勢φ_０（α_０、β_０、γ_０）が計算される。

　（工程Ｓ１３）手先の目標位置（ｘ_１、ｘ_２、・・・ｘ_ｍ（＝Ｘ_１））を計算　
　システム制御部１０１の制御に従って、軌道計算部１０５により、手先の現在位置ｘ_０・手先姿勢φ_０から手先の最終目標位置Ｘ_１・手先姿勢Φ_１までの手先軌道が計算され、その手先軌道上に単位時間Δｔ毎の手先の目標位置（ｘ_１、ｘ_２、・・・ｘ_ｍ（＝Ｘ_１））が計算される。　
　（工程Ｓ１４）目標の関節変数ベクトル（^－θ_１、^－θ_２、・・・^－θ_ｍ）を計算　
　システム制御部１０１の制御に従って、指令値計算部１０６により、手先の目標位置（ｘ_１、ｘ_２、・・・ｘ_ｍ（＝Ｘ_１））にそれぞれ対応する関節変数ベクトル（^－θ_１、^－θ_２、・・・^－θ_ｍ）が計算される。指令値計算部１０６による計算処理手順の詳細は後述する。　
　（工程Ｓ１５）目標の関節変数ベクトル（^－θ_１、^－θ_２、・・・^－θ_ｍ）を出力　
　ドライバ制御部１０３から、モータドライバ２０１、２０３、２０５、２０７、２０９、２１１に対して、指令値として目標の関節変数ベクトル（^－θ_１、^－θ_２、・・・^－θ_ｍ）が所定の制御周期Δｔ（例えば、１０ｍｓ）で順次出力される。

　次に、工程Ｓ１４の指令値計算部１０６による計算処理について図６を参照して説明する。図６は、図５の工程Ｓ１４の流れ図である。　
　（工程Ｓ１４１）初期化（ｎ＝０）　
　ここでは説明の便宜上、変数ｎを用いる。まずこの変数ｎがゼロ値に初期化される。　
　（工程Ｓ１４２）ヤコビアン逆行列Ｊ^－１（^－θ_ｎ）の計算　
　本実施形態のアーム機構はその構造上特異点が存在せず、従ってヤコビアン逆行列が常に存在する。ヤコビアン逆行列は、手先速度（手先位置・姿勢の微小変化）を関節角速度（関節角度・伸縮長の微小変化）に変換する行列である。ヤコビアン逆行列は、手先基準点の位置・手先姿勢を表すベクトルの関節変数による編微分で与えられる。指令値計算部１０６は、工程Ｓ１１で現在位置・姿勢計算部１０４により計算された現在の関節変数ベクトル^－θ_ｎ（θ_ｎ－Ｊ１、θ_ｎ－Ｊ２、Ｌ_ｎ－Ｊ３、θ_ｎ－Ｊ４、θ_ｎ－Ｊ５、θ_ｎ－Ｊ６）により、アーム構造のリンクパラメータによるヤコビアン逆行列Ｊ^－１（^－θ_ｎ）を計算する。

　（工程Ｓ１４３）手先速度^－ｘ_ｎ＋１の計算　
　現在の手先位置（現在の目標位置）ｘ_ｎ、次の手先位置（単位時間Δｔ後の次の目標位置）ｘ_ｎ＋１、単位時間Δｔに基づいて、手先速度^・ｘ_ｎ＋１が計算される。　
　（工程Ｓ１４４）関節角速度^－・θ_ｎ＋１の計算　
　工程Ｓ１４３で計算された手先速度^－ｘ_ｎ＋１がヤコビアン逆行列Ｊ^－１（^－θ_ｎ）により、関節角速度^－・θ_ｎ＋１に変換される。

　（工程Ｓ１４５）目標の関節変数ベクトル^－θ_ｎ＋１の計算　
　繰り返し回数が前の工程Ｓ１４５で計算された（変数ｎ＝０の場合は工程Ｓ１１で計算された）関節変数ベクトル^－θ_ｎ、工程Ｓ１４４で計算された関節角速度^－・θ_ｎ＋１、単位時間Δｔに基づいて、次の関節変数ベクトル^－θ_ｎ＋１が計算される。関節角速度^－・θ_ｎ＋１に単位時間Δｔを乗算することにより、単位時間Δｔの間の各関節の変位量が計算される。移動直前の関節変数ベクトル^－θ_ｎに関節の変位量を加算することにより、単位時間Δｔ経過後の関節変数ベクトル^－θ_ｎ＋１が計算される。

　（工程Ｓ１４６）処理継続の判定　
　変数ｎが繰り返し回数（ｍ－１）のとき、指令値計算部１０６による計算処理は終了される。一方、変数（ｎ）が繰り返し回数（ｍ－１）ではない場合、工程Ｓ１４７に処理が移行される。

　（工程Ｓ１４７）変数の繰り上げｎ←ｎ＋１　
　変数ｎを（ｎ＋１）に繰り上げ、工程Ｓ１４２に処理が戻る。

　以上説明した指令値計算部１０６による計算処理により、手先の目標位置（ｘ_１、ｘ_２、・・・ｘ_ｍ）にそれぞれ対応する関節変数ベクトル（^－θ_１、^－θ_２、・・・^－θ_ｍ）が計算される。

　次に、本実施形態に係るロボット装置の動作制御装置１００による動作制御の効果を図７を参照して説明する。本実施形態に係るロボット装置の動作制御装置１００は、手先の現在位置と手先の次の目標位置とに基づいて、手先の目標位置に対応する関節変数（関節角度、伸張距離）θ_Ｊ１，θ_Ｊ２，Ｌ_Ｊ３、θ_Ｊ４、θ_Ｊ５、θ_Ｊ６を計算し、その関節変数θ_Ｊ１，θ_Ｊ２，Ｌ_Ｊ３、θ_Ｊ４、θ_Ｊ５、θ_Ｊ６をそれぞれ指令値としてモータドライバ２０１、２０３，２０５、２０７，２０９、２１１に対して入力する。モータドライバ２０１、２０３，２０５、２０７，２０９、２１１は、各関節部の関節角度、伸張距離を目標値としてアクチュエータ（ステッピングモータ）を駆動する。　
　図５の動作制御（位置指令）による効果を速度指令の場合と比較して説明するために第１関節部Ｊ１の関節角度の時間変化を示す図である。図７では、一例として第１関節部Ｊ１の関節角度θ_Ｊ１に関して、モータドライバ２０１に対して位置（関節角度）を指令値としたときの時間変化を、速度（関節角速度）を指令値としたときの時間変化とともに示している。第１関節部Ｊ１のモータドライバ２０１は、ドライバ制御部１０３から時刻ｔ_０、ｔ_１、ｔ_２、・・・・ｔ_ｍ－１に、それぞれ指令値として関節角度θ_{１－Ｊ１、}θ_{２－Ｊ１、}θ_{３－Ｊ１、・・・}θ_ｍ－Ｊ１が与えられる。図７に示すように、モータドライバ２０１は、時刻ｔ_０から単位時間Δｔ経過後の時刻ｔ_１に、第１関節部Ｊ１の関節角度が関節角度θ_１－Ｊ１に到達するために必要なパルスをステッピングモータに供給する。これにより、第１関節部Ｊ１の関節角度は、時刻ｔ_０から単位時間Δｔ経過後の時刻ｔ_１に関節角度θ_１－Ｊ１に到達する。ここで重要なのは、何らかの要因で、第１関節部Ｊ１の関節角度が、時刻ｔ_０から単位時間Δｔ経過後の時刻ｔ_１に関節角度θ_１－Ｊ１に回転していなくても、計算上の予定した関節角度θ_１－Ｊ１と実際の関節角度との間の誤差がその後に累積しない点にある。すなわち、モータドライバ２０１は、時刻ｔ_０から単位時間Δｔ経過後の時刻ｔ_１に関節角度θ_１－Ｊ１に到達していなくても、次の時刻ｔ_１から単位時間Δｔ経過後の時刻ｔ_２に、第１関節部Ｊ１の関節角度が関節角度θ_２－Ｊ１に到達するために必要なパルスをステッピングモータに供給する。これにより、第１関節部Ｊ１の関節角度は、時刻ｔ_１に予定した関節角度θ_１－Ｊ１まで回転していなくても、時刻ｔ_１における実際の関節角度から、単位時間Δｔ経過後の時刻ｔ_２に関節角度θ_１－Ｊ１に到達する。

　一方、モータドライバ２０１に指令値として関節角速度を入力したとき、移動開始時や速度変化に伴う慣性誤差が累積し、第１関節部Ｊ１の実際の関節角度は、計算上の予定した関節角度から外れてしまうことがある。一方、本実施形態に係るロボット装置の動作制御装置１００のように、モータドライバ２０１に指令値として関節角度を入力することで、モータドライバ２０１は、単位時間Δｔで第１関節部Ｊ１の関節角度を指令された関節角度まで移動するようにステッピングモータを駆動する。これにより、第１関節部Ｊ１の動作制御の途中で位置誤差が発生した場合であっても、その位置誤差は累積しない。したがって、本実施形態に係るロボット装置は、ロボットアームの移動制御において位置誤差を解消又は減少させることができる。

　つまり、本実施形態では、目標位置間の手先速度を計算し、それをヤコビアン逆行列により関節角速度に変換し（従来ではこの関節角速度を指令値としていた）、その関節角速度から関節変数ベクトルを求め、関節変数ベクトルを指令値としてモータドライブに供給することにより、モータドライブでは、指令された関節変数ベクトルに単位時間で移動するようモータを駆動するので関節角速度を指令値とする場合と異なり位置誤差は原理的に生じない、又は非常に低下させることができ、フィードバック制御は不要になる。また位置誤差が生じないことから目標位置を順次進めながら次々と目標位置の関節変数ベクトルを計算し、つまり事前に全目標位置の関節変数ベクトルを計算させることも可能になる。また同次変換行列を用いた逆運動学により関節変数ベクトルを求めることができるが、その場合非線形演算であるがゆえに計算処理が非常に複雑になり、また解析的に解を一意に求める事ができないことがあるが、ヤコビアン逆行列は線形演算であり計算処理は非常に簡素であり、そのような問題は生じない。もっとも本実施形態の直動伸縮アーム機構には通常の回転関節を組み合わせたアーム構造とは異なり冗長性が存在しないので、無数解が生じる事がないのでヤコビアン逆行列で簡素に処理することができる。

　本発明のいくつかの実施形態を説明したが、これらの実施形態は、例として提示したものであり、発明の範囲を限定することは意図していない。これら実施形態は、その他の様々な形態で実施されることが可能であり、発明の要旨を逸脱しない範囲で、種々の省略、置き換え、変更を行うことができる。これら実施形態やその変形は、発明の範囲や要旨に含まれると同様に、特許請求の範囲に記載された発明とその均等の範囲に含まれるものである。

　１００…動作制御装置、２００…ロボットアーム機構、３００…操作部、１０１…システム制御部、１０２…操作部Ｉ／Ｆ、１０３…ドライバ制御部、１０４…現在位置・姿勢計算部、１０５…軌道計算部、１０６…指令値計算部、１０９…制御／データバス、２０１、２０３、２０５、２０７、２０９、２１１…モータドライバ、２０２，２０４，２０６，２０８，２１０，２１２…エンコーダ

Claims

　多関節アーム機構を構成する複数の関節部のアクチュエータにそれぞれ対応する複数のモータドライバに複数の指令値をそれぞれ出力する多関節アーム機構の動作制御装置において、
　前記複数の関節部にそれぞれ対応する現在の複数の関節変数から前記多関節アーム機構上の注目点の現在位置を計算する現在位置計算部と、
　前記現在位置から最終目標位置までの前記注目点の軌道を計算する軌道計算部と、
　前記軌道上の単位時間ごとの複数の目標位置各々から次の目標位置までの距離と前記単位時間とに基づいて前記注目点の移動速度を計算し、前記計算された移動速度を前記多関節アーム機構の構造に応じたヤコビアン逆行列により前記複数の関節部にそれぞれ対応する複数の関節角速度に変換するとともに、前記計算された複数の関節角速度と前記単位時間とから前記次の目標位置に対応する複数の関節変数を前記複数の指令値として計算する指令値計算部と、
　前記複数の指令値を前記複数のモータドライバにそれぞれ出力する指令値出力部とを具備する多関節アーム機構の動作制御装置。
　前記指令値計算部は、前記現在位置から前記最終目標位置に向かって順次連鎖しながら前記複数の目標位置各々について前記複数の関節変数を計算する請求項１記載の多関節アーム機構の動作制御装置。
　前記指令値出力部は、前記単位時間経過ごとに前記次の目標位置に関する前記複数の指令値を前記モータドライバに順次出力する請求項１記載の多関節アーム機構の動作制御装置。
　前記指令値出力部は、前記指令値計算部により前記複数の目標位置の全てに関する前記複数の関節変数の計算処理と並行して前記複数の指令値を前記モータドライバに順次出力する請求項３記載の多関節アーム機構の動作制御装置。
　前記複数の関節部は回転関節である請求項１記載の多関節アーム機構の動作制御装置。
　前記複数の関節部には肘関節部がなく前記複数の関節部の少なくとも一は直動伸縮関節部である請求項１記載の多関節アーム機構の動作制御装置。
　前記直動伸縮関節部は、
　屈曲可能に連結された断面コ字形状を有する複数の第1連結コマと、
　屈曲可能に連結された平板形状を有する複数の第２連結コマと、前記複数の第２連結コマのうち先端の第２連結コマは前記複数の第１連結コマのうち先端の第１連結コマと接続される、前記第１連結コマはその上部において前記第２連結コマに接合されることにより屈曲が拘束されて柱状体が構成される、前記第１、第２連結コマの分離により前記柱状体が解除される、
　前記第１連結コマを前記第２連結コマに接合し前記柱状体を構成するととともに前記柱状体を支持する射出部とを有する請求項６記載の多関節アーム機構の動作制御装置。
　複数の関節部を備える多関節アーム機構と、
　前記多関節アーム機構を構成する複数の関節部のアクチュエータにそれぞれ対応する複数のモータドライバに複数の指令値をそれぞれ出力する動作制御装置とを有するロボット装置において、
　前記動作制御装置は、
　前記複数の関節部にそれぞれ対応する現在の複数の関節変数から前記多関節アーム機構上の注目点の現在位置を計算する現在位置計算部と、
　前記現在位置から最終目標位置までの前記注目点の軌道を計算する軌道計算部と、
　前記軌道上の単位時間ごとの複数の目標位置各々から次の目標位置までの距離と前記単位時間とに基づいて前記注目点の移動速度を計算し、前記計算された移動速度を前記多関節アーム機構の構造に応じたヤコビアン逆行列により前記複数の関節部にそれぞれ対応する複数の関節角速度に変換するとともに、前記計算された複数の関節角速度と前記単位時間とから前記次の目標位置に対応する複数の関節変数を前記複数の指令値として計算する指令値計算部と、
　前記複数の指令値を前記複数のモータドライバにそれぞれ出力する指令値出力部とを有するロボット装置。