JP2008299590A - 制御装置、制御方法、プログラム及びロボット - Google Patents

Abstract

【課題】本発明は、安定した目標位置追随制御を実現可能な制御装置を提供する。
【解決手段】駆動軸の検出位置を入力するための駆動軸位置入力手段１０１と、出力軸の検出位置を入力するための出力軸位置入力手段１０２と、出力軸の位置と駆動軸の位置との関係を示す干渉行列に関するデータを保存するための記憶手段４と、出力軸の目標位置と入力された出力軸の位置に対して、データを用いて駆動軸に対する位置補償を算出する位置補償算出手段１１と、入力された出力軸の位置及び入力された駆動軸の位置に対して、データを用いて駆動軸に対するトルク補償を算出するトルク補償算出手段１７と、位置補償及びトルク補償から、駆動源への指令値を算出する指令値算出部１５と、指令値に基づいて駆動軸を駆動するための信号を出力する出力手段１０３とを備える。
【選択図】図１

Description

本発明は、ロボットの目標位置決めを制御する制御装置、制御方法、プログラム及びロボットに関する。

ロボット等の制御対象を目標位置へ移動させる目標位置追随制御の一つとして、制御対象を駆動する駆動源と制御対象の両方の位置を計測することで制御を行う「フルクローズド制御」が採用されている。フルクローズド制御によれば、制御対象を直接計測するため、高精度な位置決め制御が可能になるという利点を持つ反面、駆動源と制御対象間の駆動力伝達手段が、例えばモータのバックラッシ、軸のすべり、ワイヤによる伸び、リンクの曲げ等の誤差要素を含む場合には、制御誤差を起こす等、制御系が不安定になる可能性がある。

誤差要素に起因する制御誤差への対策として、位置補償及び駆動トルク補償が行われている。位置補償では、例えば、駆動源と制御対象それぞれの位置検出器の出力をフィルタ手段に通した結果をもとに目標位置への位置補償を算出する手法が知られている（例えば、特許文献１。）。この場合、駆動源と制御対象との位置関係を示す関係式（干渉行列）が一定であると仮定してその関係式を用いている。モータと減速機で構成されたリンク機構を例にとると、モータに取り付けたエンコーダ値と減速機を介した制御において、減速機の減速比が高い場合、ほとんど誤差がなく、上記関係式は行列等で一意に表現できる。しかし、例えばワイヤによる駆動力伝達手段を採用する場合、姿勢変化や負荷加重においてワイヤが伸びてしまい、干渉行列の想定した行列の要素がそれぞれ変化してしまうことが考えられる。また、干渉機構を採用している場合、その傾向はより顕著に現れ、また非線形要素であることから、正確なモデルによる同定は困難である。こうした非線形変動を特許文献１で用意されたフィルタで排除することは困難である。

また、目標位置偏差にて補償する場合、位置制御系に積分要素を導入させることで、定常偏差をなくし、制御性能を向上させることができる。しかし一般に積分要素は位相遅れを招き、制御不安定性を引き起こす要因となる。

一方、駆動トルク補償では、張力差動形トルクセンサにより、ワイヤにかかる張力を直接計測し、フィードバック制御する制御系が構築されている（例えば、特許文献２参照。）。しかしながら、特許文献２では張力差動形トルクセンサが駆動力伝達手段に直接触れているため、駆動力伝達手段自体の剛性が低くなってしまう。このように、安定した目標位置追随制御を行うことは困難であった。
特開２００４−１７１３３３号公報 特開平５−１６４６４８号公報

本発明は、安定した目標位置追随制御を実現可能な制御装置、制御方法、プログラム及びロボットを提供する。

本願発明の一態様によれば、駆動軸、出力軸、駆動軸を駆動する駆動源、及び駆動軸から出力軸へ駆動力を伝達する駆動力伝達手段を有する制御対象を制御する制御装置であって、（イ）駆動軸の検出位置を入力するための駆動軸位置入力手段と、（ロ）出力軸の検出位置を入力するための出力軸位置入力手段と、（ハ）出力軸の位置と駆動軸の位置との関係を示す干渉行列に関するデータを保存するための記憶手段と、（ニ）出力軸の目標位置と入力された出力軸の位置に対して、データを用いて駆動軸に対する位置補償を算出する位置補償算出手段と、（ホ）入力された出力軸の位置及び入力された駆動軸の位置に対して、データを用いて駆動軸に対するトルク補償を算出するトルク補償算出手段と、（ヘ）位置補償及びトルク補償から、駆動源への指令値を算出する指令値算出部と、（ト）指令値に基づいて駆動軸を駆動するための信号を出力する出力手段とを備える制御装置が提供される。

本願発明の他の態様によれば、駆動軸、出力軸、駆動軸を駆動する駆動源、及び駆動軸から出力軸へ駆動力を伝達する駆動力伝達手段を有する制御対象を制御する制御方法であって、（イ）駆動軸の検出位置を入力するステップと、（ロ）出力軸の検出位置を入力するステップと、（ハ）出力軸の目標位置と入力された出力軸の位置に対して、出力軸の位置と駆動軸の位置との関係を示す干渉行列に関するデータを用いて駆動軸に対する位置補償を算出するステップと、（ニ）入力された出力軸の位置及び入力された駆動軸の位置に対して、データを用いて駆動軸に対するトルク補償を算出するステップと、（ホ）位置補償及びトルク補償から、駆動源への指令値を算出するステップと、（ヘ）指令値に基づいて駆動軸を駆動するための信号を出力するステップとを含む制御方法が提供される。

本願発明の更に他の態様によれば、駆動軸、出力軸、駆動軸を駆動する駆動源、及び駆動軸から出力軸へ駆動力を伝達する駆動力伝達手段を有する制御対象を制御するプログラムであって、（イ）出力軸の目標位置と入力された出力軸の位置に対して、出力軸の位置と駆動軸の位置との関係を示す干渉行列に関するデータを用いて駆動軸に対する位置補償を算出させる命令と、（ロ）入力された出力軸の位置及び入力された駆動軸の位置に対して、データを用いて駆動軸に対するトルク補償を算出させる命令と、（ハ）位置補償及びトルク補償から、駆動源への指令値を算出させる命令と、（ニ）指令値に基づいて駆動軸を駆動するための信号を出力させる命令とを制御装置に実行させるプログラムが提供される。

本願発明の更に他の態様によれば、（イ）上述した制御装置と、（ロ）駆動軸の検出位置を検出するための駆動軸位置検出器と、（ハ）出力軸の検出位置を検出するための出力軸位置検出器と、（ニ）出力手段から出力される信号に基づいて制御され駆動軸を駆動する駆動源とを備えるロボットが提供される。

本発明によれば、安定した目標位置追随制御を実現可能な制御装置、制御方法、プログラム及びロボットを提供することができる。

次に、図面を参照して、本発明の実施の形態を説明する。以下の図面の記載において、同一又は類似の部分には同一又は類似の符号を付している。ただし、図面は模式的なものであり、具体的な寸法等は以下の説明を参酌して判断すべきものである。又、図面相互間においても互いの寸法の関係や比率が異なる部分が含まれていることはもちろんである。

また、以下に示す実施の形態は、この発明の技術的思想を具体化するための装置や方法を例示するものであって、この発明の技術的思想は、構成部品の材質、形状、構造、配置等を下記のものに特定するものでない。この発明の技術的思想は、特許請求の範囲において、種々の変更を加えることができる。

本発明の実施の形態に係る制御装置は、図１に示すように、駆動軸、出力軸、及び駆動軸と出力軸とを連結する駆動力伝達手段を有する制御対象（ロボット）を制御する制御装置であって、出力軸の目標位置と検出された出力軸の位置に対して、出力軸の位置と駆動軸の位置との関係を示す関係式（干渉行列）を用いて位置補償を算出する位置補償算出手段１１と、検出された出力軸の位置及び検出された駆動軸の位置に対して、干渉行列を用いて駆動軸トルク補償を算出するトルク補償算出手段１７と、位置補償及び駆動軸トルク補償から、後述する駆動源への指令値を算出する指令値算出部１５と、指令値に基づく出力手段１０３からの出力信号によって制御される駆動源２１とを備える。駆動源２１は駆動軸を駆動する。

本発明の実施の形態における制御対象は、図２に模式的に示すように、本体３０と、本体３０に設けられた可動部からなる。可動部は、複数の節（リンク）３１，３２と、駆動プーリ３３及び従動プーリ３４等から構成される複数の関節（ジョイント）３６，３７と、駆動プーリ３３及び従動プーリ３４に巻きつけた駆動プーリ３３から従動プーリ３４へ駆動力を伝達するための駆動力伝達手段（ワイヤ）３８を有する。

図３に模式的に示すように、駆動プーリ３３、減速機３９、駆動源２１及び駆動軸位置検出器２２が、駆動軸位置入力手段１０１を介して駆動軸（関節）３６に取り付けられている。駆動源２１としては、サーボモータ等のアクチュエータが使用可能である。駆動源２１が回転駆動し、減速機３９が回転数を落としトルクを上げる。一方、従動プーリ３４及び出力軸位置検出器２３が出力軸位置入力手段１０２を介して出力軸（関節）３７に取り付けられている。駆動軸３６の回転により、駆動プーリ３３、駆動力伝達手段（ワイヤ）３８及び従動プーリ３４を介して出力軸３７が回転駆動する。本発明の実施の形態では、簡単のために、駆動軸３６及び出力軸３７に対して制御する場合について述べる。

駆動軸位置検出器２２及び出力軸位置検出器２３のそれぞれは、ロータリエンコーダ等の位置センサが使用可能であり、所定の周波数成分を除去するフィルタを備えていても良い。駆動軸位置検出器２２は、駆動軸３６の位置の変位量（駆動軸角）Θ_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（t_n）／dtを算出する。

ΔΘ_M（t_n）／dt＝（Θ'_M（t_n）-Θ_M（t_n））-（Θ'_M（t_n-1）-Θ_M（t_n-1）） …（７）

ここで、干渉行列^MＴ_Jは、出力軸位置ベクトルとの積を考慮すると、式（８）のように表現され、ΔΘ_Mの成分ΔΘ_Miによって行ベクトルＴ_Miが修正される。

^MＴ_J＝［Ｔ_M1，Ｔ_M2，…Ｔ_Mm］^t …（８）

例えば、線形補償をする場合、ΔΘ_M（t_n）を用いて、係数をＫ_Tiとして、式（９）のように干渉行列^MＴ_Jの行ベクトルＴ_Miが修正され、新しい干渉行列が得られる。

Ｔ_Mi＝Ｔ_Mi＋Ｋ_TiΔΘ_Mi（t_n） …（９）

なお、ΔΘ_M（t_n）／dtが大きいほど修正量が大きくなり、制御が不安定となる可能性が大きくなるので、ΔΘ_M（t_n）／dtに対する閾値をデータメモリ３に記憶させておき、修正部４１が、ΔΘ_M（t_n）／dtを閾値と比較して、ΔΘ_M（t_n）／dtが閾値よりも大きい場合には丸め処理等の補正を行う等しても良い。

次に、第１の変形例に係る制御方法を、図８のフローチャートを参照しながら説明する。

（イ）ステップＳ３１において、目標位置算出部１０が目標位置Θ_Jrを算出する。

（ロ）ステップＳ３２において、修正部４１が、以前の修正からの経過時間等に基づいて、干渉行列^MＴ_Jを修正するか判断を行う。修正すると判断した場合はステップＳ３３に進む。

（ハ）ステップＳ３３において、修正部４１が、ΔΘ_M（t_n）／dtを算出し、ΔΘ_M（t_n）／dtを用いて干渉行列^MＴ_Jの要素を修正する。

（ニ）ステップＳ３３と並列して、ステップＳ３４において、出力軸位置検出器２３により検出された出力軸位置の変位量Θ_J及び目標位置算出部１０により算出した目標位置Θ_Jrに基づいて、修正した干渉行列^MＴ'_Jを用いて出力軸位置補償を算出する。

（ホ）ステップＳ３５において、出力軸位置検出器２３により検出された出力軸位置の変位量Θ_J及び駆動軸位置検出器２２により検出された駆動軸位置の変位量Θ_Mに基づいて、修正した干渉行列^MＴ'_Jを用いて駆動軸トルク補償τ_Mを算出する。

（ヘ）ステップＳ３６及びＳ３７における手順は、図４のステップＳ４及びＳ５の手順と実質的に同様であるので、重複した説明を省略する。

次に、図８のステップＳ３３の干渉行列の修正方法を、図９のフローチャートを参照しながら説明する。

（イ）ステップＳ４１において、出力軸位置検出器２３が出力軸位置の変位量Θ_Jを検出する。

（ロ）ステップＳ４２において、推定位置算出部１８が、干渉行列メモリ４から基準姿勢の干渉行列^MＴ_Jを読み出して、出力軸位置検出器２３により検出された出力軸位置の変位量Θ_Jを、基準姿勢の干渉行列^MＴ_Jを用いて変換し、駆動軸推定位置Θ'_Mを算出する。

（ハ）ステップＳ４３において、駆動軸位置検出器２２が駆動軸位置の変位量Θ_Mを検出する。

（ニ）ステップＳ４４において、トルク補償算出部１９が、駆動軸推定位置Θ'_Mと駆動軸位置の変位量Θ_Mとの差ΔΘ_Mを算出する。

（ホ）ステップＳ４５において、修正部４１が、時間差分ΔΘ_M（t_n）／dtを算出し、干渉行列の修正量とする。

（ヘ）ステップＳ４６において、修正部４１が、干渉行列メモリ４に記憶している干渉行列^MＴ_Jの要素をΔΘ_M（t_n）を用いて修正する。

第１の変形例によれば、干渉行列^MＴ_Jの要素を修正することで、駆動力伝達手段３８をより正確に表現することができる。この結果、駆動軸推定位置Θ'_Mをより正確に算出することができ、駆動軸位置偏差ΔΘ_Mを小さくすることができ、結果として駆動軸トルク補償τ_Mをも小さくすることができる。

また、図８に示したステップＳ３２において、修正部４１が、ある一定間隔が経過するまでは修正しないと判断することにより、修正する頻度を調整することができる。ここで、干渉行列^MＴ_Jの修正は、制御周期程度の頻度で行うと、出力軸位置補償において振動を誘発する可能性がある。よって、干渉行列^MＴ_Jの修正を行う周期は、目標位置Θ_Jrを算出する周期と比べて長い周期で、数倍程度大きくなるようにすれば、制御安定性を確保することができる。

（第２の変形例）
第２の変形例として、図１０に示すように、力検出器（力センサ）４２及び力補償算出部４３を更に備える点が、図５に示した第１の変形例と異なる。

力検出器４２は、例えば力入力手段１０４を介して制御対象（ロボット）の先端部に取り付けられる。力検出器４２は、出力軸３７に負荷された負荷荷重を検出する。力検出器４２として、６軸の力・モーメントを計測できるものを用いてもよい。

力補償算出部４３は、力検出器４２により検出された負荷荷重Ｆに基づいて、ヤコビアンＪを用いて、式（１０）のように出力軸トルクτ_Jを算出する。

τ_J＝Ｊ^tＦ …（１０）

更に、力補償算出部４３は、仮想仕事の原理より、^MＴ_Jの逆行列である駆動軸−出力軸位置変換干渉行列^JＴ_Mを用いて、式（１１）のように出力軸トルクτ_Jを各駆動軸３６に負荷された駆動軸トルクτ_Mに変換する。

τ_M＝（^MＴ^t _J）^-1τ_J＝^JＴ^t _Mτ_J …（１１）

更に、力補償算出部４３が、駆動軸トルクτ_Mを用いて線形補償等の力補償を算出する。指令値算出部１５は、出力軸位置補償及び駆動軸トルク補償に、力補償算出部４３により算出した力補償を加算して指令値を算出する。他の構成は、図７に示した制御装置の構成と実質的に同様であるので、重複した説明を省略する。

次に、第２の変形例に係る制御方法を、図１１のフローチャートを参照して説明する。

（イ）ステップＳ５１において、目標位置算出部１０が目標位置Θ_Jrを算出する。

（ロ）ステップＳ５２において、修正部４１が、以前の修正からの経過時間等に基づいて、干渉行列^MＴ_Jを修正するか判断を行う。修正すると判断した場合はステップＳ５３に進む。

（ハ）ステップＳ５３おいて、修正部４１が、ΔΘ_M（t_n）を算出し、ΔΘ_M（t_n）を用いて干渉行列^MＴ_Jの要素を修正する。

（ニ）ステップＳ５４において出力軸位置補償を算出し、ステップＳ５５において駆動軸トルク補償を算出するのに並列して、ステップＳ５６において力補償算出部４３が力補償を算出する。

（ホ）ステップＳ５７において、指令値算出部１５が、出力軸位置補償、駆動軸トルク補償及び力補償に基づいて指令値を算出する。この指令値に基づいて駆動源２１が駆動軸３６を制御する。

次に、図１１のステップＳ５６における力補償算出方法を、図１２のフローチャートを参照しながら説明する。

（イ）ステップＳ６１において、力検出器４２が、出力軸３７に対する負荷加重（力）を検出する。

（ロ）ステップＳ６２において、力補償算出部４３が、力検出器４２により検出された負荷加重に基づいて出力軸トルクτ_Jを算出する。

（ハ）ステップＳ６３において、力補償算出部４３が、出力軸トルクτ_Jを干渉行列を用いて変換し、駆動軸トルクτ_Mを算出する。

（ニ）ステップＳ６４において、力補償算出部４３が、駆動軸トルクτ_Mを用いて力補償を算出する。

また、図１０に示した力検出器４２の代わりに、出力軸３７のトルクを検出する出力軸トルク検出器（トルクセンサ）を備えていても良い。この場合の力補償算出方法を、図１３のフローチャートを参照しながら説明する。

（イ）ステップＳ７１において、出力軸トルク検出器が、出力軸トルクを検出する。この場合、図１１のステップＳ６２における力補償算出部４３により算出された出力軸トルクτ_Jと同じ物理量が得られる。

（ロ）ステップＳ７２において、力補償算出部４３が、出力軸トルク検出器により検出された出力軸トルクを干渉行列を用いて変換し、駆動軸トルクτ_Mを算出する。

（ハ）ステップＳ７３において、力補償算出部４３が、駆動軸トルクτ_Mを用いて力補償を算出する。

第２の変形例によれば、先端部での負荷荷重を計測する力検出器４２や、トルク入力手段を介して設けられた各関節に作用する外部からのトルクを計測するトルク検出器により、駆動軸３６に作用するトルクを算出し、これを力補償、あるいはトルク補償として指令値へ加えることで、より正確な指令値を算出することができる。

（その他の実施の形態）
上記のように、本発明は実施の形態によって記載したが、この開示の一部をなす論述及び図面はこの発明を限定するものであると理解すべきではない。この開示から当業者には様々な代替実施の形態、実施例及び運用技術が明らかとなろう。

例えば、実施の形態の説明においては、ロボットの動力伝達系として、図２に示すような駆動プーリ３３と従動プーリ３４、及び駆動力伝達手段（ワイヤ）３８から構成される系を説明したが、他の動力伝達系であっても良い。また、駆動軸３６と出力軸３７が一対一対応しない干渉機構を有するものであっても良い。

更に、制御装置は、ＣＰＵ１が制御対象（ロボット）の内部に埋め込まれて一体化されていても良い。また、ＣＰＵ１は制御対象の外部にあり、有線或いは無線で遠隔制御することも可能である。

このように、本発明はここでは記載していない様々な実施の形態等を含むことは勿論である。したがって、本発明の技術的範囲は上記の説明から妥当な特許請求の範囲に係る発明特定事項によってのみ定められるものである。

本発明の実施の形態に係る制御装置の一例を示すブロック図である。 本発明の実施の形態に係る制御対象の一例を示す概略図である。 本発明の実施の形態に係る制御対象の一例の一部を示す斜視図である。 本発明の実施の形態に係る制御方法の一例を説明するためのフローチャートである。 本発明の実施の形態の出力軸位置補償算出方法の一例を説明するためのフローチャートである。 本発明の実施の形態のトルク補償算出方法の一例を説明するためのフローチャートである。 第１の変形例の制御装置の一例を示すブロック図である。 第１の変形例の制御方法の一例を説明するためのフローチャートである。 第１の変形例の干渉行列の修正方法の一例を説明するためのフローチャートである。 第２の変形例の制御装置の一例を示すブロック図である。 第２の変形例の制御方法の一例を説明するためのフローチャートである。 第２の変形例の力補償算出方法の一例を説明するためのフローチャートである。 第２の変形例のトルク補償算出方法の一例を説明するためのフローチャートである。

符号の説明

１…中央演算処理装置（ＣＰＵ）
３…データメモリ
４…干渉行列メモリ（記憶手段）
１０…目標位置算出部
１１…位置補償算出手段
１２…出力軸偏差算出部
１３…駆動軸偏差算出部
１４…位置補償算出部
１５…指令値算出部
１６…速度算出部
１７…トルク補償算出手段
１８…推定位置算出部
１９…トルク補償算出部
２１…駆動源
２２…駆動軸位置検出器
２３…出力軸位置検出器
３０…本体
３１，３２…節（リンク）
３３…駆動プーリ（関節）
３４…従動プーリ（関節）
３６…駆動軸（関節）
３７…出力軸（関節）
３８…駆動力伝達手段（ワイヤ）
３９…減速機
４１…修正部
４２…力検出器
４３…力補償算出部
１０１…駆動軸位置入力手段
１０２…出力軸位置入力手段
１０３…出力手段
１０４…力入力手段

Claims (12)

1. 駆動軸、出力軸、前記駆動軸を駆動する駆動源、及び前記駆動軸から前記出力軸へ駆動力を伝達する駆動力伝達手段を有する制御対象を制御する制御装置であって、
   前記駆動軸の検出位置を入力するための駆動軸位置入力手段と、
   前記出力軸の検出位置を入力するための出力軸位置入力手段と、
   前記出力軸の位置と前記駆動軸の位置との関係を示す干渉行列に関するデータを保存するための記憶手段と、
   前記出力軸の目標位置と入力された前記出力軸の位置に対して、前記データを用いて前記駆動軸に対する位置補償を算出する位置補償算出手段と、
   前記入力された前記出力軸の位置及び入力された前記駆動軸の位置に対して、前記データを用いて前記駆動軸に対するトルク補償を算出するトルク補償算出手段と、
   前記位置補償及び前記トルク補償から、前記駆動源への指令値を算出する指令値算出部と、
   前記指令値に基づいて前記駆動軸を駆動するための信号を出力する出力手段
   とを備えることを特徴とする制御装置。
2. 前記位置補償算出手段が、
   前記目標位置と前記入力された前記出力軸の位置との偏差を算出する出力軸偏差算出部と、
   前記偏差に基づいて前記データを用いて駆動軸位置偏差を算出する駆動軸偏差算出部と、
   前記入力された前記駆動軸の位置に基づいて駆動軸速度を算出する速度算出部と、
   前記駆動軸位置偏差及び前記駆動軸速度から前記位置補償を算出する位置補償算出部
   とを備えることを特徴とする請求項１に記載の制御装置。
3. 前記トルク補償算出手段が、
   前記入力された前記出力軸の位置を前記データを用いて駆動軸推定位置を算出する推定位置算出部と、
   前記駆動軸推定位置と前記入力された前記駆動軸の位置との差から前記トルク補償を算出するトルク補償算出部
   とを備えることを特徴とする請求項１又は２に記載の制御装置。
4. 前記差に基づいて前記データを修正する修正部を更に備えることを特徴とする請求項３に記載の制御装置。
5. 前記修正部が、
   前記差の時間差分を算出し、
   前記時間差分に基づいて、指令値前記データを修正する
   ことを特徴とする請求項４に記載の制御装置。
6. 前記目標位置を算出するための目標位置算出部をさらに有し、
   前記修正部が、前記目標位置を算出する周期よりも長い周期で前記データを修正することを特徴とする請求項３又は４に記載の制御装置。
7. 前記出力軸へ付加される力を入力するための力入力手段と、
   前記力に基づいて力補償を算出する力補償算出部
   とを更に備えることを特徴とする請求項１〜６のいずれか１項に記載の制御装置。
8. 前記出力軸へ付加されるトルクを入力するためのトルク入力手段と、
   前記トルクに基づいて、前記データを用いて力補償を算出する力補償算出部
   とを更に備えることを特徴とする請求項１〜６のいずれか１項に記載の制御装置。
9. 前記指令値算出部が、前記力補償に基づいて前記指令値を算出することを特徴とする請求項７又は８に記載の制御装置。
10. 駆動軸、出力軸、前記駆動軸を駆動する駆動源、及び前記駆動軸から前記出力軸へ駆動力を伝達する駆動力伝達手段を有する制御対象を制御する制御方法であって、
    前記駆動軸の検出位置を入力するステップと、
    前記出力軸の検出位置を入力するステップと、
    前記出力軸の目標位置と入力された前記出力軸の位置に対して、前記出力軸の位置と前記駆動軸の位置との関係を示す干渉行列に関するデータを用いて前記駆動軸に対する位置補償を算出するステップと、
    前記入力された前記出力軸の位置及び入力された前記駆動軸の位置に対して、前記データを用いて前記駆動軸に対するトルク補償を算出するステップと、
    前記位置補償及び前記トルク補償から、前記駆動源への指令値を算出するステップと、
    前記指令値に基づいて前記駆動軸を駆動するための信号を出力するステップ
    とを含むことを特徴とする制御方法。
11. 駆動軸、出力軸、前記駆動軸を駆動する駆動源、及び前記駆動軸から前記出力軸へ駆動力を伝達する駆動力伝達手段を有する制御対象を制御するプログラムであって、
    前記出力軸の目標位置と入力された前記出力軸の位置に対して、前記出力軸の位置と前記駆動軸の位置との関係を示す干渉行列に関するデータを用いて前記駆動軸に対する位置補償を算出させる命令と、
    前記入力された前記出力軸の位置及び入力された前記駆動軸の位置に対して、前記データを用いて前記駆動軸に対するトルク補償を算出させる命令と、
    前記位置補償及び前記トルク補償から、前記駆動源への指令値を算出させる命令と、
    前記指令値に基づいて前記駆動軸を駆動するための信号を出力させる命令
    とを制御装置に実行させることを特徴とするプログラム。
12. 請求項１〜９に記載の制御装置と、
    前記駆動軸の検出位置を検出するための駆動軸位置検出器と、
    前記出力軸の検出位置を検出するための出力軸位置検出器と、
    前記出力手段から出力される信号に基づいて制御される駆動源
    とを備えることを特徴とするロボット。

Images