(57)【要約】【課題】本発明は、安定した目標位置追随制御を実現可能な制御装置を提供する。【解決手段】駆動軸の検出位置を入力するための駆動軸位置入力手段１０１と、出力軸の検出位置を入力するための出力軸位置入力手段１０２と、出力軸の位置と駆動軸の位置との関係を示す干渉行列に関するデータを保存するための記憶手段４と、出力軸の目標位置と入力された出力軸の位置に対して、データを用いて駆動軸に対する位置補償を算出する位置補償算出手段１１と、入力された出力軸の位置及び入力された駆動軸の位置に対して、データを用いて駆動軸に対するトルク補償を算出するトルク補償算出手段１７と、位置補償及びトルク補償から、駆動源への指令値を算出する指令値算出部１５と、指令値に基づいて駆動軸を駆動するための信号を出力する出力手段１０３とを備える。【選択図】図１
2008299590.tif 000002
【特許請求の範囲】
【請求項１】
駆動軸、出力軸、前記駆動軸を駆動する駆動源、及び前記駆動軸から前記出力軸へ駆動力を伝達する駆動力伝達手段を有する制御対象を制御する制御装置であって、
前記駆動軸の検出位置を入力するための駆動軸位置入力手段と、
前記出力軸の検出位置を入力するための出力軸位置入力手段と、
前記出力軸の位置と前記駆動軸の位置との関係を示す干渉行列に関するデータを保存するための記憶手段と、
前記出力軸の目標位置と入力された前記出力軸の位置に対して、前記データを用いて前記駆動軸に対する位置補償を算出する位置補償算出手段と、
前記入力された前記出力軸の位置及び入力された前記駆動軸の位置に対して、前記データを用いて前記駆動軸に対するトルク補償を算出するトルク補償算出手段と、
前記位置補償及び前記トルク補償から、前記駆動源への指令値を算出する指令値算出部と、
前記指令値に基づいて前記駆動軸を駆動するための信号を出力する出力手段
とを備えることを特徴とする制御装置。
【請求項２】
前記位置補償算出手段が、
前記目標位置と前記入力された前記出力軸の位置との偏差を算出する出力軸偏差算出部と、
前記偏差に基づいて前記データを用いて駆動軸位置偏差を算出する駆動軸偏差算出部と、
前記入力された前記駆動軸の位置に基づいて駆動軸速度を算出する速度算出部と、
前記駆動軸位置偏差及び前記駆動軸速度から前記位置補償を算出する位置補償算出部
とを備えることを特徴とする請求項１に記載の制御装置。
【請求項３】
前記トルク補償算出手段が、
前記入力された前記出力軸の位置を前記データを用いて駆動軸推定位置を算出する推定位置算出部と、
前記駆動軸推定位置と前記入力された前記駆動軸の位置との差から前記トルク補償を算出するトルク補償算出部
とを備えることを特徴とする請求項１又は２に記載の制御装置。
【請求項４】
前記差に基づいて前記データを修正する修正部を更に備えることを特徴とする請求項３に記載の制御装置。
【請求項５】
前記修正部が、
前記差の時間差分を算出し、
前記時間差分に基づいて、指令値前記データを修正する
ことを特徴とする請求項４に記載の制御装置。
【請求項６】
前記目標位置を算出するための目標位置算出部をさらに有し、
前記修正部が、前記目標位置を算出する周期よりも長い周期で前記データを修正することを特徴とする請求項３又は４に記載の制御装置。
【請求項７】
前記出力軸へ付加される力を入力するための力入力手段と、
前記力に基づいて力補償を算出する力補償算出部
とを更に備えることを特徴とする請求項１〜６のいずれか１項に記載の制御装置。
【請求項８】
前記出力軸へ付加されるトルクを入力するためのトルク入力手段と、
前記トルクに基づいて、前記データを用いて力補償を算出する力補償算出部
とを更に備えることを特徴とする請求項１〜６のいずれか１項に記載の制御装置。
【請求項９】
前記指令値算出部が、前記力補償に基づいて前記指令値を算出することを特徴とする請求項７又は８に記載の制御装置。
【請求項１０】
駆動軸、出力軸、前記駆動軸を駆動する駆動源、及び前記駆動軸から前記出力軸へ駆動力を伝達する駆動力伝達手段を有する制御対象を制御する制御方法であって、
前記駆動軸の検出位置を入力するステップと、
前記出力軸の検出位置を入力するステップと、
前記出力軸の目標位置と入力された前記出力軸の位置に対して、前記出力軸の位置と前記駆動軸の位置との関係を示す干渉行列に関するデータを用いて前記駆動軸に対する位置補償を算出するステップと、
前記入力された前記出力軸の位置及び入力された前記駆動軸の位置に対して、前記データを用いて前記駆動軸に対するトルク補償を算出するステップと、
前記位置補償及び前記トルク補償から、前記駆動源への指令値を算出するステップと、
前記指令値に基づいて前記駆動軸を駆動するための信号を出力するステップ
とを含むことを特徴とする制御方法。
【請求項１１】
駆動軸、出力軸、前記駆動軸を駆動する駆動源、及び前記駆動軸から前記出力軸へ駆動力を伝達する駆動力伝達手段を有する制御対象を制御するプログラムであって、
前記出力軸の目標位置と入力された前記出力軸の位置に対して、前記出力軸の位置と前記駆動軸の位置との関係を示す干渉行列に関するデータを用いて前記駆動軸に対する位置補償を算出させる命令と、
前記入力された前記出力軸の位置及び入力された前記駆動軸の位置に対して、前記データを用いて前記駆動軸に対するトルク補償を算出させる命令と、
前記位置補償及び前記トルク補償から、前記駆動源への指令値を算出させる命令と、
前記指令値に基づいて前記駆動軸を駆動するための信号を出力させる命令
とを制御装置に実行させることを特徴とするプログラム。
【請求項１２】
請求項１〜９に記載の制御装置と、
前記駆動軸の検出位置を検出するための駆動軸位置検出器と、
前記出力軸の検出位置を検出するための出力軸位置検出器と、
前記出力手段から出力される信号に基づいて制御される駆動源
とを備えることを特徴とするロボット。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボットの目標位置決めを制御する制御装置、制御方法、プログラム及びロボットに関する。
【背景技術】
【０００２】
ロボット等の制御対象を目標位置へ移動させる目標位置追随制御の一つとして、制御対象を駆動する駆動源と制御対象の両方の位置を計測することで制御を行う「フルクローズド制御」が採用されている。フルクローズド制御によれば、制御対象を直接計測するため、高精度な位置決め制御が可能になるという利点を持つ反面、駆動源と制御対象間の駆動力伝達手段が、例えばモータのバックラッシ、軸のすべり、ワイヤによる伸び、リンクの曲げ等の誤差要素を含む場合には、制御誤差を起こす等、制御系が不安定になる可能性がある。
【０００３】
誤差要素に起因する制御誤差への対策として、位置補償及び駆動トルク補償が行われている。位置補償では、例えば、駆動源と制御対象それぞれの位置検出器の出力をフィルタ手段に通した結果をもとに目標位置への位置補償を算出する手法が知られている（例えば、特許文献１。）。この場合、駆動源と制御対象との位置関係を示す関係式（干渉行列）が一定であると仮定してその関係式を用いている。モータと減速機で構成されたリンク機構を例にとると、モータに取り付けたエンコーダ値と減速機を介した制御において、減速機の減速比が高い場合、ほとんど誤差がなく、上記関係式は行列等で一意に表現できる。しかし、例えばワイヤによる駆動力伝達手段を採用する場合、姿勢変化や負荷加重においてワイヤが伸びてしまい、干渉行列の想定した行列の要素がそれぞれ変化してしまうことが考えられる。また、干渉機構を採用している場合、その傾向はより顕著に現れ、また非線形要素であることから、正確なモデルによる同定は困難である。こうした非線形変動を特許文献１で用意されたフィルタで排除することは困難である。
【０００４】
また、目標位置偏差にて補償する場合、位置制御系に積分要素を導入させることで、定常偏差をなくし、制御性能を向上させることができる。しかし一般に積分要素は位相遅れを招き、制御不安定性を引き起こす要因となる。
【０００５】
一方、駆動トルク補償では、張力差動形トルクセンサにより、ワイヤにかかる張力を直接計測し、フィードバック制御する制御系が構築されている（例えば、特許文献２参照。）。しかしながら、特許文献２では張力差動形トルクセンサが駆動力伝達手段に直接触れているため、駆動力伝達手段自体の剛性が低くなってしまう。このように、安定した目標位置追随制御を行うことは困難であった。
【特許文献１】特開２００４−１７１３３３号公報
【特許文献２】特開平５−１６４６４８号公報
【発明の開示】
【発明が解決しようとする課題】
【０００６】
本発明は、安定した目標位置追随制御を実現可能な制御装置、制御方法、プログラム及びロボットを提供する。
【課題を解決するための手段】
【０００７】
本願発明の一態様によれば、駆動軸、出力軸、駆動軸を駆動する駆動源、及び駆動軸から出力軸へ駆動力を伝達する駆動力伝達手段を有する制御対象を制御する制御装置であって、（イ）駆動軸の検出位置を入力するための駆動軸位置入力手段と、（ロ）出力軸の検出位置を入力するための出力軸位置入力手段と、（ハ）出力軸の位置と駆動軸の位置との関係を示す干渉行列に関するデータを保存するための記憶手段と、（ニ）出力軸の目標位置と入力された出力軸の位置に対して、データを用いて駆動軸に対する位置補償を算出する位置補償算出手段と、（ホ）入力された出力軸の位置及び入力された駆動軸の位置に対して、データを用いて駆動軸に対するトルク補償を算出するトルク補償算出手段と、（ヘ）位置補償及びトルク補償から、駆動源への指令値を算出する指令値算出部と、（ト）指令値に基づいて駆動軸を駆動するための信号を出力する出力手段とを備える制御装置が提供される。
【０００８】
本願発明の他の態様によれば、駆動軸、出力軸、駆動軸を駆動する駆動源、及び駆動軸から出力軸へ駆動力を伝達する駆動力伝達手段を有する制御対象を制御する制御方法であって、（イ）駆動軸の検出位置を入力するステップと、（ロ）出力軸の検出位置を入力するステップと、（ハ）出力軸の目標位置と入力された出力軸の位置に対して、出力軸の位置と駆動軸の位置との関係を示す干渉行列に関するデータを用いて駆動軸に対する位置補償を算出するステップと、（ニ）入力された出力軸の位置及び入力された駆動軸の位置に対して、データを用いて駆動軸に対するトルク補償を算出するステップと、（ホ）位置補償及びトルク補償から、駆動源への指令値を算出するステップと、（ヘ）指令値に基づいて駆動軸を駆動するための信号を出力するステップとを含む制御方法が提供される。
【０００９】
本願発明の更に他の態様によれば、駆動軸、出力軸、駆動軸を駆動する駆動源、及び駆動軸から出力軸へ駆動力を伝達する駆動力伝達手段を有する制御対象を制御するプログラムであって、（イ）出力軸の目標位置と入力された出力軸の位置に対して、出力軸の位置と駆動軸の位置との関係を示す干渉行列に関するデータを用いて駆動軸に対する位置補償を算出させる命令と、（ロ）入力された出力軸の位置及び入力された駆動軸の位置に対して、データを用いて駆動軸に対するトルク補償を算出させる命令と、（ハ）位置補償及びトルク補償から、駆動源への指令値を算出させる命令と、（ニ）指令値に基づいて駆動軸を駆動するための信号を出力させる命令とを制御装置に実行させるプログラムが提供される。
【００１０】
本願発明の更に他の態様によれば、（イ）上述した制御装置と、（ロ）駆動軸の検出位置を検出するための駆動軸位置検出器と、（ハ）出力軸の検出位置を検出するための出力軸位置検出器と、（ニ）出力手段から出力される信号に基づいて制御され駆動軸を駆動する駆動源とを備えるロボットが提供される。
【発明の効果】
【００１１】
本発明によれば、安定した目標位置追随制御を実現可能な制御装置、制御方法、プログラム及びロボットを提供することができる。
【発明を実施するための最良の形態】
【００１２】
次に、図面を参照して、本発明の実施の形態を説明する。以下の図面の記載において、同一又は類似の部分には同一又は類似の符号を付している。ただし、図面は模式的なものであり、具体的な寸法等は以下の説明を参酌して判断すべきものである。又、図面相互間においても互いの寸法の関係や比率が異なる部分が含まれていることはもちろんである。
【００１３】
また、以下に示す実施の形態は、この発明の技術的思想を具体化するための装置や方法を例示するものであって、この発明の技術的思想は、構成部品の材質、形状、構造、配置等を下記のものに特定するものでない。この発明の技術的思想は、特許請求の範囲において、種々の変更を加えることができる。
【００１４】
本発明の実施の形態に係る制御装置は、図１に示すように、駆動軸、出力軸、及び駆動軸と出力軸とを連結する駆動力伝達手段を有する制御対象（ロボット）を制御する制御装置であって、出力軸の目標位置と検出された出力軸の位置に対して、出力軸の位置と駆動軸の位置との関係を示す関係式（干渉行列）を用いて位置補償を算出する位置補償算出手段１１と、検出された出力軸の位置及び検出された駆動軸の位置に対して、干渉行列を用いて駆動軸トルク補償を算出するトルク補償算出手段１７と、位置補償及び駆動軸トルク補償から、後述する駆動源への指令値を算出する指令値算出部１５と、指令値に基づく出力手段１０３からの出力信号によって制御される駆動源２１とを備える。駆動源２１は駆動軸を駆動する。
【００１５】
本発明の実施の形態における制御対象は、図２に模式的に示すように、本体３０と、本体３０に設けられた可動部からなる。可動部は、複数の節（リンク）３１，３２と、駆動プーリ３３及び従動プーリ３４等から構成される複数の関節（ジョイント）３６，３７と、駆動プーリ３３及び従動プーリ３４に巻きつけた駆動プーリ３３から従動プーリ３４へ駆動力を伝達するための駆動力伝達手段（ワイヤ）３８を有する。
【００１６】
図３に模式的に示すように、駆動プーリ３３、減速機３９、駆動源２１及び駆動軸位置検出器２２が、駆動軸位置入力手段１０１を介して駆動軸（関節）３６に取り付けられている。駆動源２１としては、サーボモータ等のアクチュエータが使用可能である。駆動源２１が回転駆動し、減速機３９が回転数を落としトルクを上げる。一方、従動プーリ３４及び出力軸位置検出器２３が出力軸位置入力手段１０２を介して出力軸（関節）３７に取り付けられている。駆動軸３６の回転により、駆動プーリ３３、駆動力伝達手段（ワイヤ）３８及び従動プーリ３４を介して出力軸３７が回転駆動する。本発明の実施の形態では、簡単のために、駆動軸３６及び出力軸３７に対して制御する場合について述べる。
【００１７】
駆動軸位置検出器２２及び出力軸位置検出器２３のそれぞれは、ロータリエンコーダ等の位置センサが使用可能であり、所定の周波数成分を除去するフィルタを備えていても良い。駆動軸位置検出器２２は、駆動軸３６の位置の変位量（駆動軸角）ΘMを検出する。出力軸位置検出器２３は、出力軸３７の位置の変位量（出力軸角）ΘJを検出する。
【００１８】
図１に示したデータメモリ（記憶手段）３及び干渉行列メモリ（記憶手段）４としては、例えば半導体メモリ、磁気ディスク、光ディスク、光磁気ディスクや磁気テープ等が採用可能である。データメモリ３は、目標位置に関するデータ列を記憶する。干渉行列メモリ４は、基準姿勢の出力軸−駆動軸変位量変換式（以下、単に「干渉行列」という。）MＴJを記憶する。干渉行列MＴJは、駆動軸位置の変位量ΘMと出力軸位置の変位量ΘJとを用いて式（１）のように表すことができる。
【００１９】
ΘJ＝MＴJΘM            …（１）
目標位置算出部１０、位置補償算出手段１１、トルク補償算出手段１７及び指令値算出部１５は、モジュールとして中央演算処理装置（ＣＰＵ）１に含まれる。目標位置算出部１０が、データメモリ３からデータ列を読み出して、各制御周期における出力軸３７の目標位置ΘJrを算出する。
【００２０】
位置補償算出手段１１は、出力軸偏差算出部１２、駆動軸偏差算出部１３、速度算出部１６及び位置補償算出部１４を備える。出力軸偏差算出部１２は、出力軸位置検出器２３により検出され、出力軸位置入力手段１０２により入力された出力軸位置の変位量ΘJと、目標位置算出部１０により算出した出力軸３７の目標位置ΘJrとの偏差ΔΘJ（＝ΘJ−ΘJr）を算出する。
【００２１】
駆動軸偏差算出部１３は、出力軸偏差算出部１２により算出した出力軸位置偏差ΔΘJを干渉行列MＴJを用いて変換し、式（２）のように駆動軸位置偏差ΔΘMを算出する。
【００２２】
ΔΘM＝MＴJΔΘJ …（２）
速度算出部１６が、駆動軸位置検出器２２により検出され、駆動軸位置入力手段１０１により入力された駆動軸位置の変位量ΘMに基づいて駆動軸速度を算出する。なお、速度算出部１６が、出力軸位置の変位量ΘJに基づいて駆動軸速度を算出することもできるが、駆動軸３６と出力軸３７では減速比だけ速度が異なっており、同じ分解能の位置検出器を取り付けていれば、出力軸位置の変位量ΘJに基づくよりも、駆動軸位置の変位量ΘMに基づいて駆動軸速度を算出する方が、より高い精度での速度を算出することができるため、好ましい。
【００２３】
位置補償算出部１４は、駆動軸偏差算出部１３により算出した駆動軸位置偏差ΔΘM、及び速度算出部１６により算出した駆動軸速度に基づいて、駆動軸位置補償を算出し、これを出力軸位置補償とする。このように、駆動軸速度をフィードバックした速度制御ループに、駆動軸位置偏差ΔΘMをフィードバックした位置制御ループを構築することができる。この位置制御ループにおいて積分要素を実装すると、指令値との定常偏差をなくすことができるが、位相を遅らせてしまうため、あまり大きなゲインを与えることができない。
【００２４】
トルク補償算出手段１７は、推定位置算出部１８及びトルク補償算出部１９を備える。推定位置算出部１８が、出力軸位置検出器２３により検出された出力軸位置の変位量ΘJを、基準姿勢の干渉行列MＴJを用いて変換することにより、式（３）のように駆動軸推定位置Θ'Mを算出する。
【００２５】
Θ'M＝MＴJΘJ            …（３）
ここで、干渉行列MＴJは、設計時の初期姿勢等のある姿勢（以下、「基準姿勢」という。）であって、外部から負荷が加わっていないときの幾何学的構造に基づいて予め算出したものを採用している。ロボットが基準姿勢より変化している場合、あるいは外部から負荷がかかっている場合においては、駆動力伝達手段３８（ワイヤ）の伸びや経路長変化が生じ、駆動力伝達手段３８において誤差が発生する。これにより干渉行列MＴJが変動し、干渉行列MＴJと異なる干渉行列MＴ'Jとなる場合がある。駆動軸位置検出器２２により検出される実際の駆動軸位置の変位量ΘMは、干渉行列MＴ'Jを用いて式（４）のようになる。
【００２６】
ΘM＝MＴ'JΘJ            …（４）
このため、駆動軸位置検出器２２により検出された駆動軸位置の変位量ΘMと、推定位置算出部１８により算出した駆動軸推定位置Θ'Mは必ずしも一致せず、基準姿勢での干渉行列MＴJからの変化量ΔMＴJ（＝MＴJ−MＴ'J）に対応する駆動軸位置の差ΔΘM（＝Θ'M−ΘM）が生じる場合がある。トルク補償算出部１９が、干渉行列の変化量ΔMＴJに相当する差ΔΘMを式（５）のように算出する。
【００２７】
ΔΘM＝ΔMＴJΘJ …（５）
更に、トルク補償算出部１９が、差ΔΘMに基づいて駆動軸トルク補償τMを算出する。例えば、差ΔΘMを線形補償する場合、係数をＫとして、式（６）のように算出される。
【００２８】
τM＝ＫΔΘM            …（６）
なお、線形補償する他にも、トルクオブザーバで実装する等してもよい。
【００２９】
指令値算出部１５が、トルク補償算出部１９により算出した駆動軸トルク補償τMと、位置補償算出部１４により算出した出力軸位置補償とを加算することにより指令値を算出する。
【００３０】
出力手段１０３は、指令値算出部１５により算出された指令値に基づいて駆動軸３６を駆動するための信号を出力する。駆動源２１は、出力手段１０３からの信号に基づいて駆動軸３６を駆動させる。
【００３１】
次に、本発明の実施の形態に係る制御方法を、図４のフローチャートを参照しながら説明する。
【００３２】
（イ）ステップＳ１において、目標位置算出部１０が、データメモリ３から目標位置データ列を読み出して、各制御周期における目標位置ΘJrを算出する。
【００３３】
（ロ）ステップＳ２において、出力軸位置検出器２３により検出された出力軸位置の変位量ΘJ及び目標位置算出部１０により算出した目標位置ΘJrに基づいて、出力軸位置補償を算出する。
【００３４】
（ハ）ステップＳ３において、出力軸位置検出器２３により検出された出力軸位置の変位量ΘJ及び駆動軸位置検出器２２により検出された駆動軸位置の変位量ΘMに基づいて駆動軸トルク補償τMを算出する。
【００３５】
（ニ）ステップＳ４において、ステップＳ２により算出した出力軸位置補償とステップＳ３により算出した駆動軸トルク補償τMとを加算して指令値を算出する。この指令値に基づいて、駆動源２１が駆動軸３６を駆動させる。
【００３６】
（ホ）ステップＳ５において、制御が終了したか確認し、終了していなければ再びステップＳ１に戻り、指令値を算出する。
【００３７】
次に、図４のステップＳ２における出力軸位置補償算出方法を、図５のフローチャートを参照しながら説明する。
【００３８】
（イ）ステップＳ１１において、出力軸位置検出器２３が、出力軸位置の変位量ΘJを検出し、出力軸位置入力手段１０２がＣＰＵ１に入力する。
【００３９】
（ロ）ステップＳ１２において、駆動軸偏差算出部１３が、出力軸位置の変位量ΘJと目標位置ΘJrとの出力軸位置偏差ΔΘJを算出する。
【００４０】
（ハ）ステップＳ１３において、駆動軸偏差算出部１３が、出力軸位置偏差ΔΘJを干渉行列MＴJを用いて変換し、駆動軸位置偏差ΔΘMを算出する。
【００４１】
（ニ）ステップＳ１４において、駆動軸位置検出器２２が駆動軸位置の変位量ΘMを検出し、駆動軸位置入力手段１０１がＣＰＵ１に入力する。
【００４２】
（ホ）ステップＳ１５において、速度算出部１６が、駆動軸位置検出器２２により検出された駆動軸位置の変位量ΘMに基づいて駆動軸速度を算出する。
【００４３】
（ヘ）ステップＳ１６において、位置補償算出部１４が、駆動軸位置偏差ΔΘMに基づいて駆動軸位置補償を算出し、これを出力軸位置補償とする。
【００４４】
次に、図４のステップＳ３における駆動軸トルク補償算出方法を、図６のフローチャートを参照しながら説明する。
【００４５】
（イ）ステップＳ２１において、出力軸位置検出器２３が、出力軸位置の変位量ΘJを検出する。
【００４６】
（ロ）ステップＳ２２において、推定位置算出部１８が、干渉行列メモリ４から基準姿勢の干渉行列MＴJを読み出して、出力軸位置検出器２３により検出された出力軸位置の変位量ΘJを、基準姿勢の干渉行列MＴJを用いた式（１）より変換し、駆動軸推定位置Θ'Mを算出する。
【００４７】
（ハ）ステップＳ２３において、駆動軸位置検出器２２が、駆動軸位置の変位量ΘMを検出する。
【００４８】
（ニ）ステップＳ２４において、トルク補償算出部１９が、干渉行列の変化量ΔMＴJに対応する駆動軸推定位置Θ'Mと駆動軸位置の変位量ΘMとの差ΔΘMを算出する。
【００４９】
（ホ）ステップＳ２５において、トルク補償算出部１９が、差ΔΘMに基づいて駆動軸トルク補償τMを算出する。
【００５０】
本発明の実施の形態に係る制御装置によれば、駆動力伝達手段（ワイヤ）３８のような非線形要素を含んだ制御対象において、干渉行列を一定とみなさずに、図２に示したリンク３１，３２の位置の変化等の制御対象の姿勢変化や負荷荷重に応じて生じる干渉行列の変化量ΔMＴJを考慮して駆動軸トルク補償を算出するので、安定化した制御を実現可能となる。また、駆動力伝達手段（ワイヤ）３８の張力を直接計測しなくてすむので、駆動力伝達手段（ワイヤ）３８の剛性を維持することができる。
【００５１】
図４に示した一連の手順、即ち：駆動軸位置の変位量ΘMを検出させる命令；出力軸位置の変位量ΘJを検出させる命令；出力軸の目標位置を算出させる命令；目標位置と検出・入力された出力軸位置に対して、干渉行列MＴJに関するデータを用いて駆動軸３６に対する位置補償を算出させる命令；検出・入力された出力軸位置の変位量ΘJ及び検出・入力された駆動軸位置の変位量ΘMに対して、干渉行列MＴJに関するデータを用いて駆動軸３６に対するトルク補償τMを算出させる命令；駆動軸３６に対するトルク補償τMに基づいて、駆動源２１への指令値を算出させる命令；及び指令値に基づいて駆動軸３６を駆動するための信号を出力手段１０３から駆動源２１へ出力させる命令；等は、図４と等価なアルゴリズムのプログラムにより、図１に示した制御装置を制御して実行出来る。このプログラムは、本発明の制御装置を構成するコンピュータシステムのデータメモリ３に記憶させればよい。また、このプログラムは、コンピュータ読取り可能な記録媒体に保存し、この記録媒体を制御装置のデータメモリ３に読み込ませることにより、本発明の実施の形態の一連の手順を実行することができる。ここで、「コンピュータ読取り可能な記録媒体」とは、例えばコンピュータの外部メモリ装置、半導体メモリ、磁気ディスク、光ディスク、光磁気ディスク、磁気テープなどのプログラムを記録することができるような媒体などを意味する。具体的には、フレキシブルディスク、ＣＤ−ＲＯＭ，ＭＯディスクなどが「コンピュータ読取り可能な記録媒体」に含まれる。例えば、制御装置の本体は、フレキシブルディスク装置（フレキシブルディスクドライブ）および光ディスク装置（光ディスクドライブ）を内蔵若しくは外部接続するように構成できる。
【００５２】
フレキシブルディスクドライブに対してはフレキシブルディスクを、また光ディスクドライブに対してはＣＤ−ＲＯＭをその挿入口から挿入し、所定の読み出し操作を行うことにより、これらの記録媒体に格納されたプログラムをデータメモリ３にインストールすることができる。また、所定のドライブ装置を接続することにより、例えばＲＯＭや、磁気テープ装置を用いることもできる。さらに、インターネット等の情報処理ネットワークを介して、このプログラムをデータメモリ３に格納することが可能である。
【００５３】
（第１の変形例）
第１の変形例として、干渉行列メモリ４に記憶している干渉行列MＴJを修正する場合を説明する。第１の変形例に係る制御装置は、図７に示すように、修正部４１を更に備える点が、図１に示した制御装置の構成と異なる。
【００５４】
修正部４１は、トルク補償算出部１９により算出した干渉行列の変化量ΔMＴJを用いて干渉行列MＴJを修正する。干渉行列の変化量ΔMＴJは、駆動軸推定位置Θ'Mと駆動軸位置の変位量ΘMとの差ΔΘMに依存しているので、式（７）のように差ΔΘMの時間差分ΔΘM（tn）／dtを算出する。
【００５５】
ΔΘM（tn）／dt＝（Θ'M（tn）-ΘM（tn））-（Θ'M（tn-1）-ΘM（tn-1））     …（７）
ここで、干渉行列MＴJは、出力軸位置ベクトルとの積を考慮すると、式（８）のように表現され、ΔΘMの成分ΔΘMiによって行ベクトルＴMiが修正される。
【００５６】
MＴJ＝［ＴM1，ＴM2，…ＴMm］t     …（８）
例えば、線形補償をする場合、ΔΘM（tn）を用いて、係数をＫTiとして、式（９）のように干渉行列MＴJの行ベクトルＴMiが修正され、新しい干渉行列が得られる。
【００５７】
ＴMi＝ＴMi＋ＫTiΔΘMi（tn）      …（９）
なお、ΔΘM（tn）／dtが大きいほど修正量が大きくなり、制御が不安定となる可能性が大きくなるので、ΔΘM（tn）／dtに対する閾値をデータメモリ３に記憶させておき、修正部４１が、ΔΘM（tn）／dtを閾値と比較して、ΔΘM（tn）／dtが閾値よりも大きい場合には丸め処理等の補正を行う等しても良い。
【００５８】
次に、第１の変形例に係る制御方法を、図８のフローチャートを参照しながら説明する。
【００５９】
（イ）ステップＳ３１において、目標位置算出部１０が目標位置ΘJrを算出する。
【００６０】
（ロ）ステップＳ３２において、修正部４１が、以前の修正からの経過時間等に基づいて、干渉行列MＴJを修正するか判断を行う。修正すると判断した場合はステップＳ３３に進む。
【００６１】
（ハ）ステップＳ３３において、修正部４１が、ΔΘM（tn）／dtを算出し、ΔΘM（tn）／dtを用いて干渉行列MＴJの要素を修正する。
【００６２】
（ニ）ステップＳ３３と並列して、ステップＳ３４において、出力軸位置検出器２３により検出された出力軸位置の変位量ΘJ及び目標位置算出部１０により算出した目標位置ΘJrに基づいて、修正した干渉行列MＴ'Jを用いて出力軸位置補償を算出する。
【００６３】
（ホ）ステップＳ３５において、出力軸位置検出器２３により検出された出力軸位置の変位量ΘJ及び駆動軸位置検出器２２により検出された駆動軸位置の変位量ΘMに基づいて、修正した干渉行列MＴ'Jを用いて駆動軸トルク補償τMを算出する。
【００６４】
（ヘ）ステップＳ３６及びＳ３７における手順は、図４のステップＳ４及びＳ５の手順と実質的に同様であるので、重複した説明を省略する。
【００６５】
次に、図８のステップＳ３３の干渉行列の修正方法を、図９のフローチャートを参照しながら説明する。
【００６６】
（イ）ステップＳ４１において、出力軸位置検出器２３が出力軸位置の変位量ΘJを検出する。
【００６７】
（ロ）ステップＳ４２において、推定位置算出部１８が、干渉行列メモリ４から基準姿勢の干渉行列MＴJを読み出して、出力軸位置検出器２３により検出された出力軸位置の変位量ΘJを、基準姿勢の干渉行列MＴJを用いて変換し、駆動軸推定位置Θ'Mを算出する。
【００６８】
（ハ）ステップＳ４３において、駆動軸位置検出器２２が駆動軸位置の変位量ΘMを検出する。
【００６９】
（ニ）ステップＳ４４において、トルク補償算出部１９が、駆動軸推定位置Θ'Mと駆動軸位置の変位量ΘMとの差ΔΘMを算出する。
【００７０】
（ホ）ステップＳ４５において、修正部４１が、時間差分ΔΘM（tn）／dtを算出し、干渉行列の修正量とする。
【００７１】
（ヘ）ステップＳ４６において、修正部４１が、干渉行列メモリ４に記憶している干渉行列MＴJの要素をΔΘM（tn）を用いて修正する。
【００７２】
第１の変形例によれば、干渉行列MＴJの要素を修正することで、駆動力伝達手段３８をより正確に表現することができる。この結果、駆動軸推定位置Θ'Mをより正確に算出することができ、駆動軸位置偏差ΔΘMを小さくすることができ、結果として駆動軸トルク補償τMをも小さくすることができる。
【００７３】
また、図８に示したステップＳ３２において、修正部４１が、ある一定間隔が経過するまでは修正しないと判断することにより、修正する頻度を調整することができる。ここで、干渉行列MＴJの修正は、制御周期程度の頻度で行うと、出力軸位置補償において振動を誘発する可能性がある。よって、干渉行列MＴJの修正を行う周期は、目標位置ΘJrを算出する周期と比べて長い周期で、数倍程度大きくなるようにすれば、制御安定性を確保することができる。
【００７４】
（第２の変形例）
第２の変形例として、図１０に示すように、力検出器（力センサ）４２及び力補償算出部４３を更に備える点が、図５に示した第１の変形例と異なる。
【００７５】
力検出器４２は、例えば力入力手段１０４を介して制御対象（ロボット）の先端部に取り付けられる。力検出器４２は、出力軸３７に負荷された負荷荷重を検出する。力検出器４２として、６軸の力・モーメントを計測できるものを用いてもよい。
【００７６】
力補償算出部４３は、力検出器４２により検出された負荷荷重Ｆに基づいて、ヤコビアンＪを用いて、式（１０）のように出力軸トルクτJを算出する。
【００７７】
τJ＝ＪtＦ      …（１０）
更に、力補償算出部４３は、仮想仕事の原理より、MＴJの逆行列である駆動軸−出力軸位置変換干渉行列JＴMを用いて、式（１１）のように出力軸トルクτJを各駆動軸３６に負荷された駆動軸トルクτMに変換する。
【００７８】
τM＝（MＴtJ）-1τJ＝JＴtMτJ              …（１１）
更に、力補償算出部４３が、駆動軸トルクτMを用いて線形補償等の力補償を算出する。指令値算出部１５は、出力軸位置補償及び駆動軸トルク補償に、力補償算出部４３により算出した力補償を加算して指令値を算出する。他の構成は、図７に示した制御装置の構成と実質的に同様であるので、重複した説明を省略する。
【００７９】
次に、第２の変形例に係る制御方法を、図１１のフローチャートを参照して説明する。
【００８０】
（イ）ステップＳ５１において、目標位置算出部１０が目標位置ΘJrを算出する。
【００８１】
（ロ）ステップＳ５２において、修正部４１が、以前の修正からの経過時間等に基づいて、干渉行列MＴJを修正するか判断を行う。修正すると判断した場合はステップＳ５３に進む。
【００８２】
（ハ）ステップＳ５３おいて、修正部４１が、ΔΘM（tn）を算出し、ΔΘM（tn）を用いて干渉行列MＴJの要素を修正する。
【００８３】
（ニ）ステップＳ５４において出力軸位置補償を算出し、ステップＳ５５において駆動軸トルク補償を算出するのに並列して、ステップＳ５６において力補償算出部４３が力補償を算出する。
【００８４】
（ホ）ステップＳ５７において、指令値算出部１５が、出力軸位置補償、駆動軸トルク補償及び力補償に基づいて指令値を算出する。この指令値に基づいて駆動源２１が駆動軸３６を制御する。
【００８５】
次に、図１１のステップＳ５６における力補償算出方法を、図１２のフローチャートを参照しながら説明する。
【００８６】
（イ）ステップＳ６１において、力検出器４２が、出力軸３７に対する負荷加重（力）を検出する。
【００８７】
（ロ）ステップＳ６２において、力補償算出部４３が、力検出器４２により検出された負荷加重に基づいて出力軸トルクτJを算出する。
【００８８】
（ハ）ステップＳ６３において、力補償算出部４３が、出力軸トルクτJを干渉行列を用いて変換し、駆動軸トルクτMを算出する。
【００８９】
（ニ）ステップＳ６４において、力補償算出部４３が、駆動軸トルクτMを用いて力補償を算出する。
【００９０】
また、図１０に示した力検出器４２の代わりに、出力軸３７のトルクを検出する出力軸トルク検出器（トルクセンサ）を備えていても良い。この場合の力補償算出方法を、図１３のフローチャートを参照しながら説明する。
【００９１】
（イ）ステップＳ７１において、出力軸トルク検出器が、出力軸トルクを検出する。この場合、図１１のステップＳ６２における力補償算出部４３により算出された出力軸トルクτJと同じ物理量が得られる。
【００９２】
（ロ）ステップＳ７２において、力補償算出部４３が、出力軸トルク検出器により検出された出力軸トルクを干渉行列を用いて変換し、駆動軸トルクτMを算出する。
【００９３】
（ハ）ステップＳ７３において、力補償算出部４３が、駆動軸トルクτMを用いて力補償を算出する。
【００９４】
第２の変形例によれば、先端部での負荷荷重を計測する力検出器４２や、トルク入力手段を介して設けられた各関節に作用する外部からのトルクを計測するトルク検出器により、駆動軸３６に作用するトルクを算出し、これを力補償、あるいはトルク補償として指令値へ加えることで、より正確な指令値を算出することができる。
【００９５】
（その他の実施の形態）
上記のように、本発明は実施の形態によって記載したが、この開示の一部をなす論述及び図面はこの発明を限定するものであると理解すべきではない。この開示から当業者には様々な代替実施の形態、実施例及び運用技術が明らかとなろう。
【００９６】
例えば、実施の形態の説明においては、ロボットの動力伝達系として、図２に示すような駆動プーリ３３と従動プーリ３４、及び駆動力伝達手段（ワイヤ）３８から構成される系を説明したが、他の動力伝達系であっても良い。また、駆動軸３６と出力軸３７が一対一対応しない干渉機構を有するものであっても良い。
【００９７】
更に、制御装置は、ＣＰＵ１が制御対象（ロボット）の内部に埋め込まれて一体化されていても良い。また、ＣＰＵ１は制御対象の外部にあり、有線或いは無線で遠隔制御することも可能である。
【００９８】
このように、本発明はここでは記載していない様々な実施の形態等を含むことは勿論である。したがって、本発明の技術的範囲は上記の説明から妥当な特許請求の範囲に係る発明特定事項によってのみ定められるものである。
【図面の簡単な説明】
【００９９】
【図１】本発明の実施の形態に係る制御装置の一例を示すブロック図である。
【図２】本発明の実施の形態に係る制御対象の一例を示す概略図である。
【図３】本発明の実施の形態に係る制御対象の一例の一部を示す斜視図である。
【図４】本発明の実施の形態に係る制御方法の一例を説明するためのフローチャートである。
【図５】本発明の実施の形態の出力軸位置補償算出方法の一例を説明するためのフローチャートである。
【図６】本発明の実施の形態のトルク補償算出方法の一例を説明するためのフローチャートである。
【図７】第１の変形例の制御装置の一例を示すブロック図である。
【図８】第１の変形例の制御方法の一例を説明するためのフローチャートである。
【図９】第１の変形例の干渉行列の修正方法の一例を説明するためのフローチャートである。
【図１０】第２の変形例の制御装置の一例を示すブロック図である。
【図１１】第２の変形例の制御方法の一例を説明するためのフローチャートである。
【図１２】第２の変形例の力補償算出方法の一例を説明するためのフローチャートである。
【図１３】第２の変形例のトルク補償算出方法の一例を説明するためのフローチャートである。
【符号の説明】
【０１００】
１…中央演算処理装置（ＣＰＵ）
３…データメモリ
４…干渉行列メモリ（記憶手段）
１０…目標位置算出部
１１…位置補償算出手段
１２…出力軸偏差算出部
１３…駆動軸偏差算出部
１４…位置補償算出部
１５…指令値算出部
１６…速度算出部
１７…トルク補償算出手段
１８…推定位置算出部
１９…トルク補償算出部
２１…駆動源
２２…駆動軸位置検出器
２３…出力軸位置検出器
３０…本体
３１，３２…節（リンク）
３３…駆動プーリ（関節）
３４…従動プーリ（関節）
３６…駆動軸（関節）
３７…出力軸（関節）
３８…駆動力伝達手段（ワイヤ）
３９…減速機
４１…修正部
４２…力検出器
４３…力補償算出部
１０１…駆動軸位置入力手段
１０２…出力軸位置入力手段
１０３…出力手段
１０４…力入力手段
【図１】
2008299590.tif 000003
【図２】
2008299590.tif 000004
【図３】
2008299590.tif 000005
【図４】
2008299590.tif 000006
【図５】
2008299590.tif 000007
【図６】
2008299590.tif 000008
【図７】
2008299590.tif 000009
【図８】
2008299590.tif 000010
【図９】
2008299590.tif 000011
【図１０】
2008299590.tif 000012
【図１１】
2008299590.tif 000013
【図１２】
2008299590.tif 000014
【図１３】
2008299590.tif 000015