JP6632507B2 - リンク機構の制御装置、マニピュレータ、及びリンク機構の制御方法 - Google Patents

Description

本発明は、リンク機構の制御装置、マニピュレータ、及びリンク機構の制御方法に関する。

狭隘部に進入可能であり、高出力で高位置決め精度を有するマニピュレータの開発が要望されている。このようなマニピュレータとして、油圧を駆動源とする油圧マニピュレータの採用が有力視されている。生産現場の知能化及びロボット化が推進される場合、高出力で高位置決め精度を有する油圧マニピュレータのニーズは高まると予想される。

油圧マニピュレータとして、マスタースレーブのような遠隔操作タイプの小型の油圧マニピュレータが存在する。しかし、この油圧マニピュレータは高位置決め精度を要求されていない。また、リンク機構の一種として油圧ショベルの作業機が知られている。しかし、油圧ショベルの作業機は、油圧ショベルのコックピットに搭乗した運転者によって操作されるものであり、マニピュレータのように関節角度を制御されるものではない。また、油圧ショベルの作業機の位置決め精度は十分なものではない。

このように、現状においては、高出力で高位置決め精度を有する油圧マニピュレータは存在しないと考えられる。

特許第５４５５７３７号公報

本発明の態様は、高位置決め精度を実現できるリンク機構の制御装置、マニピュレータ、及びリンク機構の制御方法を提供することを目的とする。

本発明の第１の態様は、第１基端部及び第１先端部を有する第１ノードと、前記第１ノードの第１先端部にリンク部を介して回転可能に連結される第２基端部を有する第２ノードと、前記第１ノードの前記第１基端部及び前記第２ノードの第２先端部に連結される油圧シリンダとを有するリンク機構の制御装置であって、ストロークセンサで検出される前記油圧シリンダの検出ストローク量データを取得する検出ストローク量データ取得部と、角度センサで検出される前記第１ノードの検出回転角度データを取得する検出回転角度データ取得部と、前記第１ノードの目標回転角度データを取得する目標回転角度データ取得部と、前記検出回転角度と前記目標回転角度とに基づいて、前記油圧シリンダの目標シリンダ速度を算出する第１データ算出部と、前記検出ストローク量に基づいて、前記油圧シリンダの検出シリンダ速度を算出する第２データ算出部と、前記目標シリンダ速度及び前記検出シリンダ速度に基づいて、前記油圧シリンダの制御信号を出力するシリンダ制御部と、を備えるリンク機構の制御装置を提供する。

本発明の第１の態様において、前記目標回転角度データ取得部は、前記第１ノードの目標停止角度データを取得し、前記第２データ算出部は、前記目標回転角度と前記目標停止角度との差の絶対値が閾値以下になったとき、前記検出ストローク量のフィードバックを停止する第１スイッチ部を有し、前記シリンダ制御部は、前記絶対値が前記閾値以下になったとき、前記検出シリンダ速度のフィードバックゲインを前記油圧シリンダのシリンダ受圧面積に設定する第２スイッチ部を有してもよい。

本発明の第１の態様において、前記第２データ算出部は、前記目標回転速度が前記目標回転速度についての閾値以下になったとき、前記検出ストローク量のフィードバックを停止する第１スイッチ部を有し、前記シリンダ制御部は、前記目標回転速度が前記閾値以下になったとき、前記検出シリンダ速度のフィードバックゲインを前記油圧シリンダのシリンダ受圧面積に設定する第２スイッチ部を有してもよい。

本発明の第１の態様において、前記目標回転角度データ取得部は、前記第１ノードの目標停止角度データを取得し、前記第２データ算出部は、前記目標回転角度と前記目標停止角度との差の絶対値が閾値以下になったとき、前記検出ストローク量のフィードバックを停止して、前記検出回転角度に基づいて算出された前記油圧シリンダの算出ストローク量に基づいて、前記検出シリンダ速度を算出してもよい。

本発明の第１の態様において、前記第２データ算出部は、前記目標回転速度が前記目標回転速度についての閾値以下になったとき、前記検出ストローク量のフィードバックを停止して、前記検出回転角度に基づいて算出された前記油圧シリンダの算出ストローク量に基づいて、前記検出シリンダ速度を算出してもよい。

本発明の第２の態様は、第１の態様のリンク機構の制御装置と、前記制御装置によって制御されるリンク機構と、を備えるマニピュレータを提供する。

本発明の第３の態様は、第１基端部及び第１先端部を有する第１ノードと、前記第１ノードの第１先端部にリンク部を介して回転可能に連結される第２基端部を有する第２ノードと、前記第１ノードの前記第１基端部及び前記第２ノードの第２先端部に連結される油圧シリンダとを有するリンク機構の制御方法であって、ストロークセンサで検出される前記油圧シリンダの検出ストローク量データを取得する工程と、角度センサで検出される前記第１ノードの検出回転角度データを取得する工程と、前記第１ノードの目標回転角度データを取得する工程と、前記検出回転角度と前記目標回転角度とに基づいて、前記油圧シリンダの目標シリンダ速度を算出する工程と、前記検出ストローク量に基づいて、前記油圧シリンダの検出シリンダ速度を算出する工程と、前記目標シリンダ速度及び前記検出シリンダ速度に基づいて、前記油圧シリンダの制御信号を出力する工程と、を含むリンク機構の制御方法を提供する。

本発明の態様によれば、高位置決め精度を実現できるリンク機構の制御装置、マニピュレータ、及びリンク機構の制御方法が提供される。

図１は、第１実施形態に係るリンク機構の一例を模式的に示す図である。 図２は、第１実施形態に係る制御装置の一例を示す機能ブロック図である。 図３は、第１実施形態に係る制御装置の一例を示す制御ブロック図である。 図４は、第２実施形態に係る制御装置の一例を示す制御ブロック図である。 図５は、第２実施形態に係るリンク機構の制御方法を模式的に示す図である。 図６は、第２実施形態に係る目標回転角度の一例を示す図である。 図７は、第４実施形態に係る制御装置の一例を示す制御ブロック図である。

以下、本発明に係る実施形態について説明するが、本発明はこれに限定されない。実施形態に係る構成要素は適宜組み合わせることができるし、一部の構成要素を使用しないこともできる。

第１実施形態．
［制御対象］
第１実施形態について説明する。図１は、本実施形態に係るリンク機構１００の一例を模式的に示す図である。リンク機構１００は、第１ノード１と、第１ノード１に連結される第２ノード２と、第１ノード１及び第２ノード２に連結される油圧シリンダ３とを有する。

第１ノード１は、第１基端部１Ａ及び第１先端部１Ｂを有する。第２ノード２は、第２基端部２Ａ及び第２先端部２Ｂを有する。

第１ノード１の第１基端部１Ａは、支持部材４に第１リンク部５を介して回転可能に連結される。第２ノード２の第２基端部２Ａは、第１ノード１の第１先端部１Ｂに第２リンク部６を介して回転可能に連結される。

油圧シリンダ３は、第１ノード１の第１基端部１Ａ及び第２ノード２の第２先端部２Ｂに連結される。油圧シリンダ３は、第１リンク部５を介して第１ノード１の第１基端部１Ａに回転可能に連結される。油圧シリンダ３は、第３リンク部７を介して第２ノード２の第２先端部２Ｂに回転可能に連結される。

油圧シリンダ３は、シリンダ部３Ａと、シリンダ部３Ａと相対移動するロッド部３Ｂと、シリンダ３Ａの内部において移動可能なピストン部３Ｃとを有する。ロッド部３Ｂは、ピストン部３Ｃと接続される。シリンダ部３Ａのキャップ側空間Ｈａに作動油が供給されると、油圧シリンダ３は伸長する。シリンダ部３Ａのロッド側空間Ｈｂに作動油が供給されると、油圧シリンダ３は縮退する。

リンク機構１００は、油圧シリンダ３に供給される単位時間当たりの作動油の供給量Ｑを調整する調整弁１０１と、作動油を吐出する油圧ポンプ１０２とを有する。油圧ポンプ１０２から吐出された作動油は、調整弁１０１を介して油圧シリンダ３に供給される。調整弁１０１は、弁開度Δを調整して、油圧シリンダ３に供給される作動油の供給量Ｑを調整する。

調整弁１０１は、キャップ側空間Ｈａに供給される単位時間当たりの作動油の供給量Ｑ１を調整する第１調整弁１０１Ａと、ロッド側空間Ｈｂに供給される単位時間当たりの作動油の供給量Ｑ２を調整する第２調整弁１０１Ｂとを含む。油圧ポンプ１０２から吐出された作動油は、第１調整弁１０１Ａを介してキャップ側空間Ｈａに供給される。油圧ポンプ１０３から吐出された作動油は、第２調整弁１０１Ｂを介してロッド側空間Ｈｂに供給される。

第１調整弁１０１Ａは、弁開度Δ１を調整して、キャップ側空間Ｈａに供給される作動油の供給量Ｑ１を調整する。第２調整弁１０１Ｂは、弁開度Δ２を調整して、ロッド側空間Ｈｂに供給される作動油の供給量Ｑ２を調整する。

リンク機構１００は、油圧シリンダ３の検出ストローク量ｘを検出するストロークセンサ８と、第１ノード１の検出回転角度θを検出する角度センサ９とを有する。ストロークセンサ８は、例えばリニアエンコーダを含む。角度センサ９は、例えばレゾルバを含む。

リンク機構１００は、制御装置１０に制御される。制御装置１０は、調整弁１０１を制御するための制御信号を出力する。制御装置１０は、ストロークセンサ８で検出される油圧シリンダ３の検出ストローク量ｘ及び角度センサ９で検出される油圧シリンダ３の第１ノード１の検出回転角度θに基づいて、調整弁１０１の弁開度Δを調整して、グローバル座標系における第２ノード２の第２先端部２Ｂの位置を制御する。

［制御装置の機能構成］
図２は、本実施形態に係る制御装置１０の一例を示す機能ブロック図である。制御装置１０は、コンピュータシステムを含む。制御装置１０は、ＣＰＵ（Central Processing Unit）のようなプロセッサを含む演算処理装置と、ＲＯＭ（Read Only Memory）のような不揮発性メモリ及びＲＡＭ（Random Access Memory）のような揮発性メモリを含む記憶装置とを有する。記憶装置には、演算処理装置の演算処理において実行されるコンピュータプログラムが記憶される。コンピュータプログラムは、演算処理装置に読み込まれて実行される。

制御装置１０は、ストロークセンサ８、角度センサ９、及び入力装置２０と接続される。入力装置２０は、操作者に操作されることにより入力データを生成する。入力装置２０で生成された入力データは、制御装置１０に出力される。入力装置２０は、例えばコンピュータ用キーボード、マウス、及びタッチパネルの少なくとも一つを含む。

図２に示すように、制御装置１０は、検出ストローク量データ取得部１１と、検出回転角度データ取得部１２と、目標回転角度データ取得部１３と、第１データ算出部１４と、第２データ算出部１５と、シリンダ制御部１６とを有する。

検出ストローク量データ取得部１１は、ストロークセンサ８で検出される油圧シリンダ３の検出ストローク量ｘを示す検出ストローク量データを取得する。油圧シリンダ３の検出ストローク量ｘは、シリンダ部３Ａに対するロッド部３Ｂの移動量である。

検出回転角度データ取得部１２は、角度センサ９で検出される第１ノード１の検出回転角度θを示す検出回転角度データを取得する。第１ノード１の検出回転角度θは、支持部材４に対する第１ノード１の回転角度である。

目標回転角度データ取得部１３は、第１ノード１の目標回転角度θｒを示す目標回転角度データを取得する。第１ノード１の目標回転角度θｒは、支持部材４に対する第１ノード１の目標回転角度である。本実施形態において、目標回転角度データは、入力装置２０により入力される。

第１データ算出部１４は、検出回転角度θと目標回転角度θｒとに基づいて、油圧シリンダ３の目標シリンダ速度ｘｒ´を算出する。油圧シリンダ３の目標シリンダ速度ｘｒ´は、シリンダ部３Ａに対するロッド部３Ｂの目標移動速度である。

第２データ算出部１５は、検出ストローク量ｘに基づいて、油圧シリンダ３の検出シリンダ速度ｘ´を算出する。油圧シリンダ３の検出シリンダ速度ｘ´は、シリンダ部３Ａに対するロッド部３Ｂの移動速度である。

シリンダ制御部１６は、目標シリンダ速度ｘｒ´及び検出シリンダ速度ｘ´に基づいて、油圧シリンダ３の制御信号を出力する。本実施形態において、シリンダ制御部１６は、調整弁１０１に制御信号を出力する。

［制御ブロック］
図３は、本実施形態に係る制御装置１０の一例を示す制御ブロック図である。本実施形態において、制御装置１０は、３次元直交座標系であるグローバル直交座標系と、リンク機構１００に規定されるローカル座標系とに基づいて、リンク機構１００を制御する。

図３において、Ｘｒは、グローバル座標系における第２ノード２の第２先端部２Ｂの目標位置を示す。

ｘｒは、ローカル座標系における油圧シリンダ３の目標ストローク量を示す。ｘｒ´は、ローカル座標系における油圧シリンダ３の目標シリンダ速度を示す。

ｘは、ストロークセンサ８によって検出される油圧シリンダ３の検出ストローク量を示す。ｘ´は、検出ストローク量ｘを微分することにより算出される検出シリンダ速度である。

θｒは、ローカル座標系における第１ノード１の目標回転角度を示す。θｒ´は、ローカル座標系における第１ノード１の目標回転速度を示す。

θは、角度センサ９によって検出される第１ノード１の検出回転角度を示す。

ｆは、グローバル座標系における第２先端部２Ｂの目標位置をローカル座標系における第１ノード１の目標回転角度に変換する変換式であり、
ｆ（θｒ）＝Ｘｒ、
θｒ＝ｆ^-1（Ｘｒ）、
の関係が成立する。変換式ｆは、第１ノード１の寸法データ及び第２ノードの寸法データを含む幾何学的拘束式により導出される。

Ｊ（θ）は、検出回転角度θと目標回転速度θｒ´とから目標シリンダ速度ｘｒ´を導出するヤコビアンであり、第１ノード１の回転速度と油圧シリンダ３のストローク量との間の幾何学的拘束式により導出される。なお、本実施形態においては、ヤコビアンはスカラである。

Ｋθは及びＫΔは、フィードバックゲインを示す。

第２先端部２Ｂの位置Ｘを制御するためには、第１ノード１の回転速度を高精度に制御する必要がある。そのため、本実施形態において、制御装置１０は、検出回転角度θをアウターループによりフィードバックする。検出ストローク量ｘをアウターループによりフィードバックする場合、例えば第２リンク部６又は第３リンク部７のガタツキやバックラッシュの影響が検出ストローク量ｘに含まれない。そのため、そのようなノイズ成分を補償する制御ができず、第２先端部２Ｂの位置決め精度が低下する可能性がある。本実施形態によれば、検出回転角度θがアウターループによりフィードバックされることにより、例えば第２リンク部６又は第３リンク部７のガタツキやバックラッシュの影響が抑制され、第２先端部２Ｂの位置決め精度の低下が抑制される。

変換式ｆにより、第２先端部２Ｂの目標位置Ｘｒが目標回転角度θｒに変換される。第１データ算出部１４は、検出回転角度θと目標回転角度θｒとの角度偏差を算出し、フィードバックゲインＫθに基づいて、角度偏差から目標シリンダ速度θｒ´を算出する。

本実施形態において、最終的な制御対象は、第２先端部２Ｂの位置Ｘであるものの、直接的な制御対象は、油圧シリンダ３の調整弁１０１である。そのため、第１データ算出部１４は、ヤコビアンＪを使って、検出回転角度θと目標回転速度θｒ´とから、油圧シリンダ３の目標シリンダ速度ｘｒ´を算出する。

第２データ算出部１５は、ストロークセンサ８で検出される検出ストローク量ｘを１階微分することにより、油圧シリンダ３の検出シリンダ速度ｘ´を算出する。

シリンダ制御部１６は、目標シリンダ速度ｘｒ´と検出シリンダ速度ｘ´との偏差である速度偏差を算出し、フィードバックゲインＫΔに基づいて、制御弁１０１の弁開度Δを制御する制御信号を出力する。弁開度Δが調整されることにより、油圧シリンダ３に供給される作動油の供給量Ｑが調整される。

本実施形態において、制御装置１０は、検出シリンダ速度ｘ´をインナーループによりフィードバックする。本実施形態によれば、フィードバックゲインＫΔを大きくすることができ、高い応答性を得ることができる。

［作用及び効果］
以上説明したように、本実施形態によれば、検出ストローク量ｘ及び検出回転角度θが検出され、それら検出ストローク量ｘ及び検出回転角度θの両方がフィードバックされることにより、高位置決め精度を実現できるリンク機構１００が提供される。

例えば、検出回転角度θがフィードバックされ、検出ストローク量ｘがフィードバックされない場合、油圧シリンダ３を直接制御することができない。この場合には、幾何学的拘束から間接的に油圧シリンダ３のストローク量を算出し、算出されたストローク量に基づいて油圧シリンダ３を制御することとなる。しかし、この場合、機器接続間のバックラッシュやガタの影響が含まれた対象の制御を一つの制御ループでおこなうことになるためフィードバックゲインを十分に上げることができず応答性が不十分となる可能性がある。

一方、検出ストローク量ｘがフィードバックされ、検出回転角度θがフィードバックされない場合においても、第２リンク部６又は第３リンク部６のガタツキやバックラッシュの影響により、高位置決め精度を得ることができない可能性がある。

本実施形態においては、検出ストローク量ｘ及び検出回転角度θが検出され、それら検出ストローク量ｘ及び検出回転角度θの両方がフィードバックされることにより、高位置決め精度を実現できるリンク機構１００が提供される。以上により，ロボットのような多リンク系の制御と，油柱共振周波数などに代表される油圧系の制御を同時に満たすことができる。

本実施形態のリンク機構１００及び制御装置１０は、マニピュレータの少なくとも一部に適用可能である。マニピュレータのリンク機構１００を駆動させるためのアクチュエータとして並進機構を含む油圧シリンダ３を採用した場合、内界センサであるストロークセンサ８と外界センサである角度センサ９とが使用される。ストロークセンサ８及び角度センサ９の両方が併用されることにより、高位置決め精度を実現することができる。

なお、本実施形態において、検出ストローク量ｘのフィードバック制御は比例制御であるが、ＰＩＤや特性改善フィルタなど，油圧制御を考慮した制御装置１０により構成されてもよい。また、検出回転角度データを利用してガタツキなどの影響を検出し，適正な非線形制御を施してもよい。

第２実施形態．
第２実施形態について説明する。以下の説明において、上述の実施形態と同一又は同等の構成要素については、同一の符号を付し、その説明を簡略又は省略する。

図４は、本実施形態に係る制御装置１０の一例を示す制御ブロック図である。本実施形態の特徴部分は、所定条件のときに制御装置１０がインナーループを切り替える点にある。

図４において、Ａは、油圧シリンダ３のシリンダ受圧面積を示す。本実施形態において、制御装置１０は、第２先端部２Ｂが目標停止位置に近付いたときに、ストロークセンサ８の検出ストローク量ｘのフィードバックを停止し、検出シリンダ速度ｘ´のフィードバックゲインを油圧シリンダ３のシリンダ受圧面積Ａに設定する。

目標回転角度θｒは、時間ｔについての関数である（θｒ（ｔ））。本実施形態においては、制御装置１０は、第２先端部２Ｂが目標停止位置で停止するように、油圧シリンダ３を制御する。

図５は、本実施形態に係るリンク機構１００の制御方法を模式的に示す図である。本実施形態において、制御装置１０は、第２先端部２Ｂが目標停止位置から閾値Ｗだけ離れた制御切換位置に配置されたとき、ストロークセンサ８の検出ストローク量ｘのフィードバックを停止し、検出シリンダ速度ｘ´のフィードバックゲインをフィードバックゲインＫΔから油圧シリンダ３のシリンダ受圧面積Ａに切り替える。

制御装置１０は、制御切換位置と目標停止位置との間の停止制御区間において、第２先端部２Ｂの移動速度を徐々に低下させ、第２先端部２Ｂが目標停止位置で停止するように、制御を実施する。なお、図５において、通常制御区間とは、第１実施形態で説明した制御方法により制御される区間である。

第２先端部２Ｂが制御切換位置に配置されるとき、及び第２先端部２Ｂが目標停止位置に配置されるときの第１ノード１の目標回転角度θｒは、一義的に定められる。本実施形態において、第２先端部２Ｂが目標停止位置に配置されるときの第１ノード１の目標停止角度θｅｎｄは、一義的に定められる。また、第２先端部２Ｂが制御切換位置に配置されるときの第１ノード１の目標回転角度｜θｅｎｄ＋ε１｜は、一義的に定められる。なお、閾値ε１は、目標回転角度θｒについての閾値であり、第２先端部２Ｂの移動距離についての閾値Ｗを、第１ノード１の目標回転角度θｒについての閾値ε１に変換した値である。

図６は、本実施形態に係る目標回転角度θｒ（ｔ）の一例を示す図である。本実施形態においては、第２先端部２Ｂが目標停止位置に近付き、θｒ（ｔ）≦｜θｅｎｄ＋ε１｜の条件が成立したとき、制御装置１０は、ストロークセンサ８の検出ストローク量ｘのフィードバックを停止し、検出シリンダ速度ｘ´のフィードバックゲインを油圧シリンダ３のシリンダ受圧面積Ａに設定する。

本実施形態において、目標回転角度データ取得部１３は、第１ノード１の目標停止角度θｅｎｄを示す目標停止角度データを取得する。第２データ算出部１５は、目標回転角度θｒと目標停止角度θｅｎｄとの差の絶対値が閾値ε１以下になったとき、すなわち、
｜θｒ（ｔ）‐θｅｎｄ｜≦ε１の条件が成立したとき、検出ストローク量ｘのフィードバックを停止する第１スイッチ部３１を有する。シリンダ制御部１６は、目標回転角度θｒと目標停止角度θｅｎｄとの差の絶対値が閾値ε１以下になったとき、検出シリンダ速度ｘ´のフィードバックゲインを油圧シリンダ３のシリンダ受圧面積Ａに設定する第２スイッチ部３２を有する。

図４に示すように、通常制御区間においては、第１スイッチ部３１は、ストロークセンサ８の検出ストローク量ｘから算出される検出シリンダ速度ｘ´がフィードバックされるようにスイッチングする。また、通常制御区間においては、第２スイッチ部３２は、速度偏差がフィードバックゲインＫΔに基づいて変換されるようにスイッチングする。

一方、停止制御区間において、第１スイッチ部３１は、検出ストローク量ｘから算出される検出シリンダ速度ｘ´がフィードバックされないように、インナーループをオフに設定する。また、停止制御区間において、第２スイッチ部３２は、フィードバックゲインをシリンダ受圧面積Ａに設定する。

インナーループがオフに設定され、フィードバックゲインがシリンダ受圧面積Ａに設定されることにより、供給量Ｑと目標シリンダ速度ｘｒ´との間に、Ｑ＝Ａ×ｘｒ´の関係が成立する。制御装置１０は、流量Ｑ（＝Ａ×ｘｒ´）で油圧シリンダ３に作動油が供給されるように、調整弁１０１を弁開度Δにするための制御信号を出力する。

以上説明したように、本実施形態においても、検出回転角度θをアウターループでフィードバックされることにより、ガタツキなどの影響が抑制される。また、検出ストローク量ｘがインナーループでフィードバックされることにより、油圧シリンダ３の制御性向上、特に応答性向上が図られる。

リンク機構１０が作動中においては、高い応答性が要求される。リンク機構１０の作動中においては、一方向に慣性力が加わっているので，ガタツキの影響は非常に小さくなっている。そのためインナーループが支配的であってもよい。

一方，リンク機構１００の第２先端部２Ｂが目標停止位置に近付いてくると、高い位置決め精度が要求される。そのため、アウターループが非常に重要になる。

本実施形態においては、リンク機構１００の第２先端部２Ｂが目標停止位置に近付いたとき、インナーループが切られ、アウターループのみによる制御が実施される。これにより、ガタツキの影響が抑制され、関節角度の高位置決め精度が実現される。

また、通常制御区間においては、油圧シリンダ３の応答性を高めるために、フィードバックゲインＫΔは大きい値に設定されている。リンク機構１００の第２先端部２Ｂが目標停止位置に近付いたとき、大きい値のフィードバックゲインＫΔを使用し続けると、オーバーシュートし、リンク機構１００の第２先端部２Ｂが目標停止位置で停止することが困難となる。本実施形態においては、リンク機構１００の第２先端部２Ｂが目標停止位置に近付いたとき、フィードバックゲインがシリンダ受圧面積Ａに設定されるため、高い位置決め精度が得られる。

第３実施形態．
第３実施形態について説明する。以下の説明において、上述の実施形態と同一又は同等の構成要素については、同一の符号を付し、その説明を簡略又は省略する。

上述の第２実施形態においては、｜θｒ（ｔ）‐θｅｎｄ｜≦ε１の条件が成立したとき、第１スイッチ部３１及び第２スイッチ部３２においてスイッチングが実施されることとした。

本実施形態においては、θｒ´（ｔ）≦｜ε２｜の条件が成立したとき、第１スイッチ部３１及び第２スイッチ部３２においてスイッチングが実施される例について説明する。

すなわち、図６に示した例において、第２データ算出部１５の第１スイッチ部３１は、目標回転速度θｒ´が目標回転速度θｒ´についての閾値ε２以下になったとき、検出ストローク量ｘのフィードバックを停止する。シリンダ制御部１６の第２スイッチ部３２は、目標回転速度θｒ´が閾値ε２以下になったとき、検出シリンダ速度ｘ´のフィードバックゲインを油圧シリンダ３のシリンダ受圧面積Ａに設定する。

すなわち、本実施形態においては、第１ノード１の目標回転角度θｒ´が閾値ε２以下の低速回転状態になったとき、第１スイッチ部３１及び第２スイッチ部３２においてスイッチングが実施される。

以上説明したように、本実施形態においても、検出回転角度θをアウターループでフィードバックされることにより、ガタツキなどの影響が抑制される。また、検出ストローク量ｘがインナーループでフィードバックされることにより、油圧シリンダ３の制御性向上、特に応答性向上が図られる。

リンク機構１０が作動中においては、高い応答性が要求される。リンク機構１０の作動中においては、一方向に慣性力が加わっているので，ガタツキの影響は非常に小さくなっている。そのためインナーループが支配的であってもよい。

例えば、リンク機構１００の第２先端部２Ｂにレーザセンサを装着して、ある部位の面形状を計測する場合、目標停止位置のみならず、停止制御区間の全部において、高位置決め精度が要求される。本実施形態によれば、リンク機構１０の低速動作時をトリガとして第１スイッチ部３１及び第２スイッチ部３２のスイッチングが実施される。これにより、ガタツキの影響が抑制され，関節角度の高位置決め精度が実現される。

また、本実施形態においても、通常制御区間においては、油圧シリンダ３の応答性を高めるために、フィードバックゲインＫΔは大きい値に設定されている。リンク機構１００の第２先端部２Ｂが目標停止位置に近付いたとき、大きい値のフィードバックゲインＫΔを使用し続けると、オーバーシュートし、リンク機構１００の第２先端部２Ｂが目標停止位置で停止することが困難となる。本実施形態においては、リンク機構１００の第２先端部２Ｂが目標停止位置に近付いたとき、フィードバックゲインがシリンダ受圧面積Ａに設定されるため、高い位置決め精度が得られる。

第４実施形態．
第４実施形態について説明する。以下の説明において、上述の実施形態と同一又は同等の構成要素については、同一の符号を付し、その説明を簡略又は省略する。

本実施形態においては、｜θｒ（ｔ）‐θｅｎｄ｜≦ε１の条件が成立したとき、ストロークセンサ８で検出された検出ストローク量ｘを使用せず、角度センサ９の検出回転角度θに基づいて油圧シリンダ３の検出ストローク量ｘを幾何学的に算出し、その検出回転角度θから算出された算出ストローク量ｈ（θ）をインナーループでフィードバックする例について説明する。

図７は、本実施形態に係る制御装置１０の一例を示す制御ブロック図である。図７に示すように、第２データ算出部１５は、スイッチ部３３を有する。

上述の第２実施形態と同様、目標回転角度データ取得部１３は、第１ノード１の目標停止角度θｅｎｄを示す目標停止角度データを取得する。

本実施形態において、第２データ算出部は、目標回転角度θｒと目標停止角度θｅｎｄとの差の絶対値が閾値ε１以下になったとき、すなわち、｜θｒ（ｔ）‐θｅｎｄ｜≦ε１の条件が成立したとき、検出ストローク量ｘのフィードバックを停止して、検出回転角度θに基づいて算出された油圧シリンダ３の算出ストローク量ｈ（θ）に基づいて、検出シリンダ速度ｘ´を算出する。スイッチ部３３は、｜θｒ（ｔ）‐θｅｎｄ｜≦ε１の条件が成立したとき、検出ストローク量ｘのフィードバックが停止され、算出ストローク量ｈ（θ）がフィードバックされるように、インナーループをスイッチングする。

以上説明したように、本実施形態においては、外界センサである角度センサ９で検出される検出回転角度θがフィードバックされるとともに、検出回転角度θから幾何学的に算出ストローク量ｈ（θ）が算出されてフィードバックされる。すなわち、本実施形態においては、インナーループのフィードバックデータとしてアウターループと同等のデータが利用される。本実施形態においても、高位置決め精度が実現される。

第５実施形態．
第５実施形態について説明する。以下の説明において、上述の実施形態と同一又は同等の構成要素については、同一の符号を付し、その説明を簡略又は省略する。

本実施形態においては、θｒ´（ｔ）≦｜ε２｜の条件が成立したとき、ストロークセンサ８で検出された検出ストローク量ｘを使用せず、角度センサ９の検出回転角度θに基づいて油圧シリンダ３の検出ストローク量ｘを幾何学的に算出し、その検出回転角度θから算出された算出ストローク量ｈ（θ）をインナーループでフィードバックする例について説明する。

すなわち、図７に示した例において、第２データ算出部１５は、目標回転速度θｒ´が目標回転速度θｒ´についての閾値ε２以下になったとき、すなわち、θｒ´（ｔ）≦｜ε２｜の条件が成立したとき、検出ストローク量ｘのフィードバックを停止して、検出回転角度θに基づいて算出された油圧シリンダ３の算出ストローク量ｈ（θ）に基づいて、検出シリンダ速度ｘ´を算出する。

以上説明したように、本実施形態においても、高い位置決め精度が実現される。

なお、上述の各実施形態において、第１ノード１が支持部材４に固定されてもよい。第１ノード１が支持部材４に固定され回転しない場合、角度センサ９は、第２ノード２の検出回転角度θを検出し、検出回転角度データ取得部１２は、角度センサ９で検出される第２ノード２の検出回転角度θを示す検出回転角度データを取得する。目標回転角度データ取得部１３は、第２ノード２の目標回転角度θｒを示す目標回転角度データを取得する。

１…第１ノード、１Ａ…第１基端部、１Ｂ…第１先端部、２…第２ノード、２Ａ…第２基端部、２Ｂ…第２先端部、３…油圧シリンダ、３Ａ…シリンダ部、３Ｂ…ロッド部、３Ｃ…ピストン部、４…支持部材、５…第１リンク部、６…第２リンク部、７…第３リンク部、８…ストロークセンサ、９…角度センサ、１０…制御装置、１１…検出ストローク量データ取得部、１２…検出回転角度データ取得部、１３…目標回転角度データ取得部、１４…第１データ算出部、１５…第２データ算出部、１６…シリンダ制御部、２０…入力装置、３１…第１スイッチ部、３２…第２スイッチ部、３３…スイッチ部、１００…油圧リンク機構、１０１…調整弁、１０１Ａ…第１調整弁、１０１Ｂ…第２調整弁、１０２…油圧ポンプ、Ｈａ…キャップ側空間、Ｈｂ…ロッド側空間。

Claims (7)

1. 支持部材と、前記支持部材に第１リンク部を介して回転可能に連結される第１基端部及び第１先端部を有する第１ノードと、前記第１ノードの第１先端部に第２リンク部を介して回転可能に連結される第２基端部を有する第２ノードと、前記第１ノードの前記第１基端部に前記第１リンク部を介して連結され前記第２ノードの第２先端部に第３リンク部を介して連結される油圧シリンダとを有するリンク機構の制御装置であって、
   ストロークセンサで検出される前記油圧シリンダの検出ストローク量を示す検出ストローク量データを取得する検出ストローク量データ取得部と、
   角度センサで検出される前記支持部材に対する前記第１ノードの検出回転角度を示す検出回転角度データを取得する検出回転角度データ取得部と、
   前記支持部材に対する前記第１ノードの目標回転角度を示す目標回転角度データを取得する目標回転角度データ取得部と、
   前記検出回転角度と前記目標回転角度とに基づいて、前記油圧シリンダの目標シリンダ速度を算出する第１データ算出部と、
   前記検出ストローク量に基づいて、前記油圧シリンダの検出シリンダ速度を算出する第２データ算出部と、
   前記目標シリンダ速度及び前記検出シリンダ速度に基づいて、前記油圧シリンダの制御信号を出力するシリンダ制御部と、
   を備えるリンク機構の制御装置。
2. 前記目標回転角度データ取得部は、前記第１ノードの目標停止角度を示す目標停止角度データを取得し、
   前記第２データ算出部は、前記目標回転角度と前記目標停止角度との差の絶対値が閾値以下になったとき、前記検出ストローク量のフィードバックを停止する第１スイッチ部を有し、
   前記シリンダ制御部は、前記絶対値が前記閾値以下になったとき、前記検出シリンダ速度のフィードバックゲインを前記油圧シリンダのシリンダ受圧面積に設定する第２スイッチ部を有する、
   請求項１に記載のリンク機構の制御装置。
3. 前記第２データ算出部は、前記第１ノードの目標回転速度が前記目標回転速度についての閾値以下になったとき、前記検出ストローク量のフィードバックを停止する第１スイッチ部を有し、
   前記シリンダ制御部は、前記目標回転速度が前記閾値以下になったとき、前記検出シリンダ速度のフィードバックゲインを前記油圧シリンダのシリンダ受圧面積に設定する第２スイッチ部を有する、
   請求項１に記載のリンク機構の制御装置。
4. 前記目標回転角度データ取得部は、前記第１ノードの目標停止角度を示す目標停止角度データを取得し、
   前記第２データ算出部は、前記目標回転角度と前記目標停止角度との差の絶対値が閾値以下になったとき、前記検出ストローク量のフィードバックを停止して、前記検出回転角度に基づいて算出された前記油圧シリンダの算出ストローク量に基づいて、前記検出シリンダ速度を算出する、
   請求項１に記載のリンク機構の制御装置。
5. 前記第２データ算出部は、前記第１ノードの目標回転速度が前記目標回転速度についての閾値以下になったとき、前記検出ストローク量のフィードバックを停止して、前記検出回転角度に基づいて算出された前記油圧シリンダの算出ストローク量に基づいて、前記検出シリンダ速度を算出する、
   請求項１に記載のリンク機構の制御装置。
6. 請求項１から請求項５のいずれか一項に記載のリンク機構の制御装置と、
   前記制御装置によって制御されるリンク機構と、
   を備えるマニピュレータ。
7. 支持部材と、前記支持部材に第１リンク部を介して回転可能に連結される第１基端部及び第１先端部を有する第１ノードと、前記第１ノードの第１先端部に第２リンク部を介して回転可能に連結される第２基端部を有する第２ノードと、前記第１ノードの前記第１基端部に前記第１リンク部を介して連結され前記第２ノードの第２先端部に第３リンク部を介して連結される油圧シリンダとを有するリンク機構の制御方法であって、
   ストロークセンサで検出される前記油圧シリンダの検出ストローク量を示す検出ストローク量データを取得する工程と、
   角度センサで検出される前記支持部材に対する前記第１ノードの検出回転角度を示す検出回転角度データを取得する工程と、
   前記支持部材に対する前記第１ノードの目標回転角度を示す目標回転角度データを取得する工程と、
   前記検出回転角度と前記目標回転角度とに基づいて、前記油圧シリンダの目標シリンダ速度を算出する工程と、
   前記検出ストローク量に基づいて、前記油圧シリンダの検出シリンダ速度を算出する工程と、
   前記目標シリンダ速度及び前記検出シリンダ速度に基づいて、前記油圧シリンダの制御信号を出力する工程と、
   を含むリンク機構の制御方法。

Images