JP6757240B2 - 制御装置、制御方法、プログラム - Google Patents

Description

本発明は、機械装置を構成する関節機構の関節角度を油圧により制御する制御装置、制御方法、プログラムに関する。

油圧マニピュレータなどの油圧で制御される機械装置は、単位体積当たりのエネルギー効率が高いという利点がある。反面、油圧で制御される機械装置は高精度な位置決め性能のために必要な、微小動作の制御が不得手である。油圧で制御される機械装置の一例として作業機械の技術が特許文献１に開示されている。当該特許文献１の技術はバネの力により作業機構の重量を補償している。

特開２０１５−１０５５６０号公報

ところで上述のような機械装置においては油圧制御弁の開度に基づいて油圧量を調整し関節を中心に回転する２つのリンク間の角度である関節角度を決定するなどしている。このような機械装置において関節角度を制御して動作させるリンク自体の重さを打ち消す重力補償トルクを精度良く油圧制御のフィードバック系に適用することが求められていた。

そこでこの発明は、上述の課題を解決する制御装置、制御方法、プログラムを提供することを目的としている。

本発明の第１の態様によれば、駆動機構を有する機械装置の前記駆動機構を油圧により制御する制御装置が、前記駆動機構を制御して動作させる駆動機構構成部品の自重を打ち消す前記駆動機構を構成するリンク間の現在の角度またはその目標に対応する重力補償トルクを重力補償方程式を用いて算出する重力補償トルク算出部と、前記重力補償トルクと、前記駆動機構を構成するリンク間の角度を示す関節角度の現在と目標との偏差と、前記目標のみに基づいて、前記重力補償トルクを、前記油圧の流量を制御する制御弁の弁開度を示す重力補償弁開度に変換する重力補償弁開度算出部と、前記駆動機構の現在の駆動状態量と目標の駆動状態量とにより定まるフィードバック弁開度に前記重力補償弁開度を加算して、前記制御弁の弁開度制御指令を算出する弁開度制御部と、を備えることを特徴とする。

上述の制御装置において、前記駆動機構が、関節機構を介して接続された複数のリンクを含み、前記制御装置が、前記駆動機構を有する機械装置の前記関節機構の関節角度を前記油圧により制御し、前記重力補償トルク算出部が、前記関節角度を制御して動作させる前記駆動機構構成部品であるリンクの自重を打ち消す前記重力補償トルクを重力補償方程式を用いて算出し、前記弁開度制御部が、前記関節機構の現在の前記駆動状態量である関節角度と目標の前記駆動状態量である関節角度とにより定まる角度フィードバック弁開度と前記重力補償弁開度とに基づいて、前記制御弁の弁開度制御指令を算出してもよい。

また上述の制御装置において、前記重力補償弁開度算出部は、前記現在の関節角度を前記重力補償方程式に入力して算出された前記重力補償トルクと、前記目標の関節角度と、前記現在の関節角度と前記目標の関節角度との偏差と、を用いて重力補償弁開度を算出してよい。

また上述の制御装置において、前記重力補償弁開度算出部は、前記目標の関節角度を前記重力補償方程式に入力して算出された前記重力補償トルクと、前記目標の関節角度と、前記現在の関節角度と前記目標の関節角度との偏差と、を用いて重力補償弁開度を算出してよい。
また上述の制御装置において、前記偏差の変動周波数を抑制する変動抑制部を備えてよい。

また上述の制御装置において、前記重力補償弁開度算出部は、前記重力補償トルクを前記重力補償弁開度に変換する可変ゲイン係数を、一定角速度で変化させる前記関節角度の定常偏差を超える角度以上の場合には当該角度の大きさに比例させて所定の係数値より大きな所定の値まで増大させ、前記関節角度の定常偏差を超えない角度未満の場合には前記所定の係数値に保持してよい。

また上述の制御装置において、前記重力補償トルク算出部は、前記制御弁が算出された弁開度に達するまでの応答時間に基づいて決定された所定時間分先行した時刻における重力補償トルクを算出して出力してよい。

また上述の制御装置において、前記重力補償弁開度算出部は、前記重力補償トルクを前記重力補償弁開度に変換する可変ゲイン係数を前記関節角度が目標角度に移行するまでに最速となる可変ゲイン係数を学習してよい。

本発明の第２の態様によれば、制御方法は、駆動機構を有する機械装置の前記駆動機構を油圧により制御する制御装置の制御方法であって、前記駆動機構を制御して動作させる駆動機構構成部品の自重を打ち消す前記駆動機構を構成するリンク間の現在の角度またはその目標に対応する重力補償トルクを重力補償方程式を用いて算出し、前記重力補償トルクと、前記駆動機構を構成するリンク間の角度を示す関節角度の現在と目標との偏差と、前記目標のみに基づいて、前記重力補償トルクを、前記油圧の流量を制御する制御弁の弁開度を示す重力補償弁開度に変換し、前記駆動機構の現在の駆動状態量と目標の駆動状態量とにより定まるフィードバック弁開度に前記重力補償弁開度を加算して、前記制御弁の弁開度制御指令を算出することを特徴とする。

本発明の第３の態様によれば、プログラムは、駆動機構を有する機械装置の前記駆動機構を油圧により制御する制御装置のコンピュータを、前記駆動機構を制御して動作させる駆動機構構成部品の自重を打ち消す前記駆動機構を構成するリンク間の現在の角度またはその目標に対応する重力補償トルクを重力補償方程式を用いて算出する重力補償トルク算出手段、前記重力補償トルクと、前記駆動機構を構成するリンク間の角度を示す関節角度の現在と目標との偏差と、前記目標のみに基づいて、前記重力補償トルクを、前記油圧の流量を制御する制御弁の弁開度を示す重力補償弁開度に変換する重力補償弁開度算出手段、前記駆動機構の現在の駆動状態量と目標の駆動状態量とにより定まるフィードバック弁開度に前記重力補償弁開度を加算して、前記制御弁の弁開度制御指令を算出する弁開度制御手段、として機能させることを特徴とする。

本発明によれば、重力補償トルクτ_ｇｎ［Ｎｍ］を油圧の流量を制御する制御弁の弁開度を示す重力補償弁開度ａ_ｇｎ［％］に変換する構成だけで、動作させるリンクの自重を打ち消すための弁開度制御指令を算出することができる。よって複雑なロジックを構築する必要が無く動作させるリンクの自重を打ち消すための弁開度制御指令を算出することができる。

第一の実施形態による制御装置の構成を示すブロック図である。 第一の実施形態による制御装置の処理フローを示す図である。 第二の実施形態による制御装置の構成を示すブロック図である。 第二の実施形態による制御装置の処理フローを示す図である。 第三実施形態による制御装置の可変ゲイン係数の決定概要を示す図である。 第四の実施形態による制御装置の重力補償トルクの出力タイミングを説明する図である。

＜第一の実施形態＞
以下、第一の実施形態による制御装置を図面を参照して説明する。
図１は第一の実施形態による制御装置の構成を示すブロック図である。
この図で示すように制御システムは制御装置１と機械装置２とにより構成されている。制御装置１は関節機構を介して接続された複数のリンクを含む駆動機構を有する機械装置２を制御する。制御装置１は機械装置２における関節機構の関節角度を油圧により制御する。機械装置２は例えば油圧マニピュレータなどであってよい。

制御装置１はコンピュータであり、ＲＯＭ（Read Only Memory）、ＲＡＭ（Random Access Memory）、ＳＳＤ（Solid State Drive）などの記憶部、ＣＰＵ（Central Processing Unit）、通信インタフェースなどのハードウェアによって構成されてよい。

制御装置１のＣＰＵは制御装置１に電源が投入されることに基づいて起動し、記憶している制御プログラムを実行する。これにより、制御装置１には、重力補償トルク算出部１１、重力補償弁開度算出部１２、フィードバック制御部１３、弁開度制御部１４の各機能が備わる。

重力補償トルク算出部１１は、機械装置２に備わる関節機構の関節角度を制御して動作させるリンクの自重を打ち消す重力補償トルクτ_ｇｎ［Ｎｍ］を、重力補償方程式を用いて算出する。本実施形態において関節機構は機械装置２を構成する駆動機構の一態様である。またリンクは駆動機構を制御して動作させる駆動機構構成部品の一態様である。
重力補償弁開度算出部１２は、重力補償トルクτ_ｇｎ［Ｎｍ］を、油圧の流量を制御する制御弁の弁開度を示す重力補償弁開度ａ_ｇｎ［％］に変換する。具体的には、重力補償弁開度算出部１２は、重力補償トルクτ_ｇｎ［Ｎｍ］と、関節角度の目標角度と、当該目標角度と関節角度の現在角度の偏差とを用いて重力補償弁開度ａ_ｇｎ［％］を算出する。

フィードバック制御部１３は、駆動機構の現在の駆動状態量と目標の駆動状態量とにより定まるフィードバック弁開度と重力補償弁開度とに基づいて、制御弁の弁開度制御指令を算出する。具体的にはフィードバック制御部１３は、機械装置２に備わる関節機構の関節角度の目標角度θ_ｒｎと現在角度θ_ｎとを用いて、関節角度が目標角度θ_ｒｎとなるようにフィードバック制御を行う。関節角度は駆動機構の駆動状態量の一態様である。フィードバック制御部１３はフィードバック制御により算出した角度フィードバック弁開度ａ_ｆｎ［％］を出力する。
弁開度制御部１４は、重力補償弁開度算出部１２から取得した重力補償弁開度ａ_ｇｎ［％］と、フィードバック制御部１３から取得した角度フィードバック弁開度ａ_ｆｎ［％］とを用いて、制御弁の弁開度制御指令を算出する。

制御装置１の処理によれば、重力補償トルクτ_ｇｎ［Ｎｍ］を、油圧の流量を制御する制御弁の弁開度を示す重力補償弁開度ａ_ｇｎ［％］に変換することができる。したがって、動作させるリンクの自重を打ち消すための弁開度制御指令を簡易な構成で算出することができる。

図２は第一の実施形態による制御装置の処理フローを示す図である。
次に制御装置１の処理の詳細について説明する。
まずフィードバック制御部１３は一例として、二つのリンクが接続された関節機構の関節角度の目標角度θ_ｒｎと現在角度θ_ｎの偏差Δθ_ｎを取得する（ステップＳ１０１）。偏差はθ_ｒｎ−θ_ｎにより算出することができる。フィードバック制御部１３は偏差Δθ_ｎを用いて、当該Δθ_ｎが０となるような角度フィードバック弁開度ａ_ｆｎ［％］をフィードバック制御により算出する（ステップＳ１０２）。フィードバック制御部１３は角度フィードバック弁開度ａ_ｆｎ［％］を弁開度制御部１４へ出力する。

他方、重力補償トルク算出部１１は、機械装置２から関節機構の現在角度θ_ｎを取得する。重力補償トルク算出部１１は現在角度θ_ｎを重力補償方程式ｇ（θ_ｎ）に代入して、関節角度を制御して動作させるリンクの自重を打ち消す重力補償トルクτ_ｇｎ［Ｎｍ］を算出する（ステップＳ１０３）。重力補償トルク算出部１１は重力補償トルクτ_ｇｎ［Ｎｍ］を重力補償弁開度算出部１２へ出力する。

重力補償弁開度算出部１２は重力補償トルクτ_ｇｎ［Ｎｍ］と、目標角度θ_ｒｎと、偏差Δθ_ｎとを取得する。重力補償弁開度算出部１２はτ_ｇｎ［Ｎｍ］×θ_ｒｎの値を算出する（ステップＳ１０４）。重力補償弁開度算出部１２はτ_ｇｎ［Ｎｍ］×θ_ｒｎの値と偏差Δθ_ｎの値とに基づいて、式（１）で表される重力補償弁開度算出式の可変ゲイン係数Ｋ_ｇｎを決定する（ステップＳ１０５）。

重力補償弁開度算出部１２は可変ゲイン係数Ｋ_ｇｎを決定する場合に、τ_ｇｎ［Ｎｍ］×θ_ｒｎの値の正負の符号、偏差Δθ_ｎの正負の符号に基づいて、可変ゲイン係数Ｋ_ｇｎを増加させるか減少させるかを決定する。またτ_ｇｎ［Ｎｍ］×θ_ｒｎの値や偏差Δθ_ｎの値に基づいて所定の算出式により、可変ゲイン係数Ｋ_ｇｎの数値を特定してよい。

一例としては、ステップＳ１０５の処理において重力補償弁開度算出部１２は、τ_ｇｎ［Ｎｍ］×θ_ｒｎの値が＋を示し、偏差Δθ_ｎの値が＋を示す場合、リンクを重力と逆らう方向に駆動する状況であって目標に到達していない状況と判定する。この場合、重力補償弁開度算出部１２は重力補償が足りないと判定し、可変ゲイン係数Ｋ_ｇｎを増加させるようにその可変ゲイン係数Ｋ_ｇｎを決定する。
また重力補償弁開度算出部１２は、τ_ｇｎ［Ｎｍ］×θ_ｒｎの値が＋を示し、偏差Δθ_ｎの値が−を示す場合、リンクを重力と逆らう方向に駆動する状況であって目標を超えてリンクを作動させてしまった状況と判定する。この場合、重力補償弁開度算出部１２は重力補償が効きすぎであると判定し、可変ゲイン係数Ｋ_ｇｎを減少させるようにその可変ゲイン係数Ｋ_ｇｎを決定する。

また重力補償弁開度算出部１２は、τ_ｇｎ［Ｎｍ］×θ_ｒｎの値が−を示し、偏差Δθ_ｎの値が＋を示す場合、リンクを重力方向に駆動する状況であって目標に到達していない状況と判定する。この場合、重力補償弁開度算出部１２は重力補償が効きすぎであると判定し、可変ゲイン係数Ｋ_ｇｎを減少させるようにその可変ゲイン係数Ｋ_ｇｎを決定する。
また重力補償弁開度算出部１２は、τ_ｇｎ［Ｎｍ］×θ_ｒｎの値が−を示し、偏差Δθ_ｎの値が−を示す場合、リンクを重力方向に駆動する状況であって目標を超えてリンクを作動させてしまった状況と判定する。この場合、重力補償弁開度算出部１２は重力補償が足りないと判定し、可変ゲイン係数Ｋ_ｇｎを増加少させるようにその可変ゲイン係数Ｋ_ｇｎを決定する。

重力補償弁開度算出部１２は可変ゲイン係数Ｋ_ｇｎを決定すると、上記式（１）により重力補償弁開度ａ_ｇｎ［％］を算出する（ステップＳ１０６）。重力補償弁開度算出部１２は重力補償弁開度ａ_ｇｎ［％］を弁開度制御部１４へ出力する。

弁開度制御部１４はフィードバック制御部１３から取得した角度フィードバック弁開度ａ_ｆｎ［％］と、重力補償弁開度算出部１２から取得した重力補償弁開度ａ_ｇｎ［％］を加算して、弁開度制御指令を算出する（ステップＳ１０７）。弁開度制御部１４は弁開度制御指令を機械装置２の対象の制御弁へ出力する（ステップＳ１０８）。

以上の処理によれば、重力補償トルクτ_ｇｎ［Ｎｍ］を油圧の流量を制御する制御弁の弁開度を示す重力補償弁開度ａ_ｇｎ［％］に変換することができる。したがって、動作させるリンクの自重を打ち消すための弁開度制御指令を算出することができる。

また上述の処理によれば、重力補償トルクτ_ｇｎ［Ｎｍ］を、油圧の流量を制御する制御弁の弁開度を示す重力補償弁開度ａ_ｇｎ［％］に変換する構成だけで、動作させるリンクの自重を打ち消すための弁開度制御指令を算出することができる。よって複雑なロジックを構築する必要が無く動作させるリンクの自重を打ち消すための弁開度制御指令を算出することができる。
またτ_ｇｎ［Ｎｍ］×θ_ｒｎの値の正負の符号、偏差Δθ_ｎの正負の符号に基づいて、可変ゲイン係数Ｋ_ｇｎを増加させるか減少させるかを決定するので、重力補償トルクτ_ｇｎ［Ｎｍ］と目標角度の関係や、偏差Δθ_ｎの正負に応じた適切な重力補償弁開度ａ_ｇｎ［％］を算出することができる。

＜第二の実施形態＞
以下、第二の実施形態による制御装置を図面を参照して説明する。
図３は第二の実施形態による制御装置の構成を示すブロック図である。
第二の実施形態における制御装置１、機械装置２のハードウェア構成は第一の実施形態と同様である。
制御装置１のＣＰＵは制御装置１に電源が投入されることに基づいて起動し、記憶している制御プログラムを実行する。これにより、制御装置１には、図３に示すように重力補償トルク算出部１１、重力補償弁開度算出部１２、フィードバック制御部１３、弁開度制御部１４、変動抑制部１５の各機能が備わる。

重力補償トルク算出部１１は、機械装置２に備わる関節の角度を制御して動作させるリンクの自重を打ち消す重力補償トルクτ_ｇｎ［Ｎｍ］を、重力補償方程式を用いて算出する。
重力補償弁開度算出部１２は、重力補償トルクτ_ｇｎ［Ｎｍ］を、油圧の流量を制御する制御弁の弁開度を示す重力補償弁開度ａ_ｇｎ［％］に変換する。具体的には、重力補償弁開度算出部１２は、関節角度の目標角度θ_ｒｎを重力補償方程式に入力して算出された重力補償トルクτ_ｇｎ［Ｎｍ］と、関節角度の目標角度θ_ｒｎと、関節角度の現在角度θ_ｎと関節角度の目標角度θ_ｒｎの偏差Δθ_ｎと、を用いて重力補償弁開度を算出する。

フィードバック制御部１３は、機械装置２に備わる関節機構の関節角度の目標角度θ_ｒｎと現在角度θ_ｎとを用いて、関節角度が目標角度θ_ｒｎとなるようにフィードバック制御を行う。フィードバック制御部１３はフィードバック制御により算出した角度フィードバック弁開度ａ_ｆｎ［％］を出力する。
弁開度制御部１４は、重力補償弁開度算出部１２から取得した重力補償弁開度ａ_ｇｎ［％］にと、フィードバック制御部１３から取得した角度フィードバック弁開度ａ_ｆｎ［％］とを用いて、制御弁の弁開度制御指令を算出する。
変動抑制部１５は関節角度の現在角度θ_ｎと関節角度の目標角度θ_ｒｎの偏差Δθ_ｎの変動周波数を抑制する。例えば変動抑制部１５はローパスフィルタによって構成されてよい。変動抑制部１５の処理により、重力補償弁開度算出部１２に入力される関節角度の現在角度θ_ｎと関節角度の目標角度θ_ｒｎの偏差Δθ_ｎは、その値の増減の頻度が抑制される。

図４は第二の実施形態による制御装置の処理フローを示す図である。
次に第二の実施形態による制御装置１の処理の詳細について説明する。
まずフィードバック制御部１３は二つのリンクが接続された関節機構の関節角度の目標角度θ_ｒｎと現在角度θ_ｎの偏差Δθ_ｎを取得する（ステップＳ２０１）。偏差はθ_ｒｎ−θ_ｎにより算出することができる。フィードバック制御部１３は偏差Δθ_ｎを用いて、当該Δθ_ｎが０となるような角度フィードバック弁開度ａ_ｆｎ［％］をフィードバック制御により算出する（ステップＳ２０２）。フィードバック制御部１３は角度フィードバック弁開度ａ_ｆｎ［％］を弁開度制御部１４へ出力する。

他方、重力補償トルク算出部１１は、関節機構の目標角度θ_ｒｎを取得する。重力補償トルク算出部１１は目標角度θ_ｒｎを重力補償方程式ｇ（θ_ｒｎ）に代入して、関節角度を制御して動作させるリンクの自重を打ち消す重力補償トルクτ_ｇｎ［Ｎｍ］を算出する（ステップＳ２０３）。重力補償トルク算出部１１は重力補償トルクτ_ｇｎ［Ｎｍ］を重力補償弁開度算出部１２へ出力する。

変動抑制部１５は偏差Δθ_ｎを取得する。変動抑制部１５は取得した偏差Δθ_ｎの変動周波数を抑制して重力補償弁開度算出部１２へ出力する（ステップＳ２０４）。重力補償弁開度算出部１２は重力補償トルクτ_ｇｎ［Ｎｍ］と、目標角度θ_ｒｎと、変動周波数が抑制された偏差Δθ_ｎとを取得する。重力補償弁開度算出部１２はτ_ｇｎ［Ｎｍ］×θ_ｒｎの値を算出する（ステップＳ２０５）。重力補償弁開度算出部１２はτ_ｇｎ［Ｎｍ］×θ_ｒｎの値と偏差Δθ_ｎの値とに基づいて、式（１）で表される重力補償弁開度算出式の可変ゲイン係数Ｋ_ｇｎを決定する（ステップＳ２０６）。

第一の実施形態と同様に、第二の実施形態の重力補償弁開度算出部１２は可変ゲイン係数Ｋ_ｇｎを決定する場合に、τ_ｇｎ［Ｎｍ］×θ_ｒｎの値の正負の符号、偏差Δθ_ｎの正負の符号に基づいて、可変ゲイン係数Ｋ_ｇｎを増加させるか減少させるかを決定する。またτ_ｇｎ［Ｎｍ］×θ_ｒｎの値や偏差Δθ_ｎの値に基づいて所定の算出式により、可変ゲイン係数Ｋ_ｇｎの数値を特定してよい。

重力補償弁開度算出部１２は可変ゲイン係数Ｋ_ｇｎを決定すると、上記式（１）により重力補償弁開度ａ_ｇｎ［％］を算出する（ステップＳ２０７）。重力補償弁開度算出部１２は重力補償弁開度ａ_ｇｎ［％］を弁開度制御部１４へ出力する。

弁開度制御部１４はフィードバック制御部１３から取得した角度フィードバック弁開度ａ_ｆｎ［％］と、重力補償弁開度算出部１２から取得した重力補償弁開度ａ_ｇｎ［％］を加算して、弁開度制御指令を算出する（ステップＳ２０８）。弁開度制御部１４は弁開度制御指令を機械装置２の対象の制御弁へ出力する（ステップＳ２０９）。

以上の処理によれば、第一の実施形態と同様の効果に加え、変動抑制部１５が偏差の変動周波数を抑制するため、可変ゲイン係数Ｋ_ｇｎの変動を抑えることができる。自重補償の思想は静的な（動特性を持たない）重量負荷変化を補償するもので、細かく補償量を変える必要はない場合もある。また補償しないことによる外乱はフィードバックで賄うことができる。むしろ、急変することがかえって外乱になる可能性があり、これを防ぐべく変動の少ない入力信号となるようにしている。

＜第三の実施形態＞
以下、第三の実施形態による制御装置を図面を参照して説明する。
図５は第三実施形態による制御装置の可変ゲイン係数の決定概要を示す図である。
重力補償弁開度算出部１２は、重力補償トルクτ_ｇｎ［Ｎｍ］を重力補償弁開度ａ_ｇｎ［％］に変換する可変ゲイン係数Ｋ_ｇｎを偏差Δθ_ｎの値に基づいて変化させる。例えば重力補償弁開度算出部１２は、一定角速度で変化させる関節角度の定常偏差を超える角度以上に関節角度を動かす場合には、当該関節角度の大きさに比例させて所定の係数値Ｋ１より大きな所定の値Ｋ２まで可変ゲイン係数Ｋ_ｇｎを増大させる。

重力補償弁開度算出部１２は、一定角速度で変化させる関節角度の定常偏差を超えない角度未満で関節角度を動かす場合には、可変ゲイン係数Ｋ_ｇｎを所定の係数値Ｋ１に保持する。

重力補償弁開度算出部１２は、偏差Δθ_ｎが所定の大きさΔθ１となった場合に、当該偏差Δθ_ｎの大きさに応じて、可変ゲイン係数Ｋ_ｇｎを所定の係数値Ｋ１から比例的に増加させる。Δθ１は、一定角速度で行う関節角度の制御において発生する関節角度の定常偏差に基づいて決定する。可変ゲイン係数Ｋ_ｇｎの上限であるＫ２は、出力可能な弁開度飽和値などから現実的な値を設定する。係数値Ｋ１は関節機構を静止状態で制御している時、フィードバック制御出力が殆どなく重力補償のための制御出力が主となる状態における可変ゲイン係数Ｋ_ｇｎの値を目安として定める。

第三の実施形態による可変ゲイン係数Ｋ_ｇｎの決定手法によれば、その値を都度調整する必要が無く、また、下限値を明確に設定していることから可変ゲイン係数Ｋ_ｇｎを有意な範囲内で収束させることができる。
なお第三の実施形態による可変ゲイン係数Ｋ_ｇｎの決定手法を第一の実施形態や第二の実施系値に適用してよい。

＜第四の実施形態＞
以下、第四の実施形態による制御装置を図面を参照して説明する。
図６は第四の実施形態による制御装置の重力補償トルクの出力タイミングを説明する図である。
図６で示す波形は、関節機構の関節角度が目標角度θ_ｒｎに達するまでの当該角度の時間的な変化を示している。関節機構の関節角度が目標角度θ_ｒｎに達するまでには応答時間が必要である。重力補償トルク算出部１１は、目標軌道θ_ｒｎが予め与えられている場合、別途ステップ応答などで計測してある制御弁の応答時間λ分だけ先行して、リンクの自重を打ち消す重力補償トルクτ_ｇｎ［Ｎｍ］を重力補償方程式ｇ（θ_ｎ（ｔ＋λ））により算出して、その重力補償トルクτ_ｇｎ［Ｎｍ］を時刻ｔにおいて重力補償弁開度算出部１２へ出力する。

なお、第二の実施形態の場合には、重力補償トルク算出部１１は、目標軌道θ_ｒｎが予め与えられている場合、別途ステップ応答などで計測してある制御弁の応答時間λ分だけ先行して、リンクの自重を打ち消す重力補償トルクτ_ｇｎ［Ｎｍ］を重力補償方程式ｇ（θ_ｒｎ（ｔ＋λ））により算出して、その重力補償トルクτ_ｇｎ［Ｎｍ］を時刻ｔにおいて重力補償弁開度算出部１２へ出力する。

油圧系は微小開度の非線形が強いため、応答遅れが無視できない。そのためこれを無駄時間と捉え、先行してリンクの自重を打ち消す重力補償トルクτ_ｇｎ［Ｎｍ］を重力補償方程式ｇ（θ_ｒｎ）により算出して、その重力補償トルクτ_ｇｎ［Ｎｍ］を重力補償弁開度算出部１２へ出力する。これにより例えば第三の実施形態でΔθ_ｎが動き始めに大きくプラスになり、従って可変ゲイン係数Ｋ_ｇｎが大きくなるためΔθ_ｎがマイナスになり可変ゲイン係数Ｋ_ｇｎが小さくなるといった、可変ゲイン係数Ｋ_ｇｎのハンチングを防ぐことができる。

＜第五の実施形態＞
重力補償弁開度算出部１２は、重力補償トルクτ_ｇｎ［Ｎｍ］を重力補償弁開度ａ_ｇｎ［％］に変換する可変ゲイン係数Ｋ_ｇｎを関節角度が目標角度に移行するまでに最速となる可変ゲイン係数を学習するようにしてもよい。
例えば、重力補償弁開度算出部１２は可変ゲイン係数Ｋ_ｇｎを角速度を入力とした変数としてＫ_ｇｎ（θn’）として可変にする。可変ゲイン係数Ｋ_ｇｎ（θn’）はある角速度（θn’）の際について何度か様々な軌道で動かし学習させるようにしてよい。

上述の各実施形態においては、駆動機構が、関節機構を介して接続された複数のリンクを含み、制御装置１が、駆動機構を有する機械装置２の関節機構の関節角度を油圧により制御している。そして、重力補償トルク算出部１１が、関節角度を制御して動作させる駆動機構構成部品であるリンクの自重を打ち消す重力補償トルクを重力補償方程式を用いて算出する。また弁開度制御部１４が、関節機構の現在の駆動状態量である関節角度と目標の駆動状態量である関節角度とにより定まる角度フィードバック弁開度と重力補償弁開度とに基づいて、制御弁の弁開度制御指令を算出する。
しかしながら駆動機構は関節機構以外の他の機構であってよい。例えば駆動機構は伸縮機構などの他の機構であってもよい。そして駆動状態量は駆動機構が伸縮機構である場合には伸び量や縮み量を示すものであってもよい。

駆動機構が関節機構以外である場合も、重力補償弁開度算出部１２は、現在の駆動状態量を重力補償方程式に入力して算出された重力補償トルクと、目標の駆動状態量と、現在の駆動状態量と目標の駆動状態量との偏差と、を用いて重力補償弁開度を算出してよい。

また駆動機構が関節機構以外である場合も、重力補償弁開度算出部１２は、目標の駆動状態量を重力補償方程式に入力して算出された重力補償トルクと、目標の駆動状態量と、現在の駆動状態量と目標の駆動状態量との偏差と、を用いて重力補償弁開度を算出してよい。

また駆動機構が関節機構以外である場合も、偏差の変動周波数を抑制する変動抑制部を備えてよい。

また駆動機構が関節機構以外である場合も、重力補償弁開度算出部１２は、重力補償トルクを重力補償弁開度に変換する可変ゲイン係数を、一定度合で変化させる駆動状態量の定常偏差を超える駆動状態量以上の場合には当該駆動状態量の大きさに比例させて所定の係数値より大きな所定の値まで増大させ、駆動状態量の定常偏差を超えない駆動状態量未満の場合には所定の係数値に保持するようにしてよい。

また駆動機構が関節機構以外である場合も、重力補償トルク算出部１１は、制御弁が算出された弁開度に達するまでの応答時間に基づいて決定された所定時間分先行した時刻における重力補償トルクを算出して出力してよい。

また駆動機構が関節機構以外である場合も、重力補償弁開度算出部１２は、重力補償トルクを重力補償弁開度に変換する可変ゲイン係数を駆動状態量が目標の駆動状態量に移行するまでに最速となる可変ゲイン係数を学習するようにしてもよい。

上述の制御装置１は内部に、コンピュータシステムを有している。そして、制御装置１に上述した各処理を行わせるためのプログラムは、当該制御装置１のコンピュータ読み取り可能な記録媒体に記憶されており、このプログラムを制御装置１のコンピュータが読み出して実行することによって、上記処理が行われる。ここでコンピュータ読み取り可能な記録媒体とは、磁気ディスク、光磁気ディスク、ＣＤ−ＲＯＭ、ＤＶＤ−ＲＯＭ、半導体メモリ等をいう。また、このコンピュータプログラムを通信回線によってコンピュータに配信し、この配信を受けたコンピュータが当該プログラムを実行するようにしても良い。

また、上記プログラムは、前述した各処理部の機能の一部を実現するためのものであっても良い。さらに、前述した機能をコンピュータシステムにすでに記録されているプログラムとの組み合わせで実現できるもの、いわゆる差分ファイル（差分プログラム）であっても良い。

１・・・制御装置
２・・・機械装置
１１・・・重力補償トルク算出部
１２・・・重力補償弁開度算出部
１３・・・フィードバック制御部
１４・・・弁開度制御部
１５・・・変動抑制部

Claims (10)

1. 駆動機構を有する機械装置の前記駆動機構を油圧により制御する制御装置が、
   前記駆動機構を制御して動作させる駆動機構構成部品の自重を打ち消す前記駆動機構を構成するリンク間の現在の角度またはその目標に対応する重力補償トルクを重力補償方程式を用いて算出する重力補償トルク算出部と、
   前記重力補償トルクと、前記駆動機構を構成するリンク間の角度を示す関節角度の現在と目標との偏差と、前記目標のみに基づいて、前記重力補償トルクを、前記油圧の流量を制御する制御弁の弁開度を示す重力補償弁開度に変換する重力補償弁開度算出部と、
   前記駆動機構の現在の駆動状態量と目標の駆動状態量とにより定まるフィードバック弁開度に前記重力補償弁開度を加算して、前記制御弁の弁開度制御指令を算出する弁開度制御部と、
   を備える制御装置。
2. 前記駆動機構が、関節機構を介して接続された複数のリンクを含み、
   前記制御装置が、前記駆動機構を有する機械装置の前記関節機構の関節角度を前記油圧により制御し、
   前記重力補償トルク算出部が、前記関節角度を制御して動作させる前記駆動機構構成部品であるリンクの自重を打ち消す前記重力補償トルクを重力補償方程式を用いて算出し、
   前記弁開度制御部が、前記関節機構の現在の前記駆動状態量である関節角度と目標の前記駆動状態量である関節角度とにより定まる角度フィードバック弁開度と前記重力補償弁開度とに基づいて、前記制御弁の弁開度制御指令を算出する
   請求項１に記載の制御装置。
3. 前記重力補償弁開度算出部は、前記現在の関節角度を前記重力補償方程式に入力して算出された前記重力補償トルクと、前記目標の関節角度と、前記現在の関節角度と前記目標の関節角度との偏差と、を用いて重力補償弁開度を算出する
   請求項２に記載の制御装置。
4. 前記重力補償弁開度算出部は、前記目標の関節角度を前記重力補償方程式に入力して算出された前記重力補償トルクと、前記目標の関節角度と、前記現在の関節角度と前記目標の関節角度との偏差と、を用いて重力補償弁開度を算出する
   請求項２に記載の制御装置。
5. 前記偏差の変動周波数を抑制する変動抑制部を備える請求項３または請求項４に記載の制御装置。
6. 前記重力補償弁開度算出部は、前記重力補償トルクを前記重力補償弁開度に変換する可変ゲイン係数を、一定角速度で変化させる前記関節角度の定常偏差を超える角度以上の場合には当該角度の大きさに比例させて所定の係数値より大きな所定の値まで増大させ、前記関節角度の定常偏差を超えない角度未満の場合には前記所定の係数値に保持する
   請求項２から請求項５の何れか一項に記載の制御装置。
7. 前記重力補償トルク算出部は、前記制御弁が算出された弁開度に達するまでの応答時間に基づいて決定された所定時間分先行した時刻における重力補償トルクを算出して出力する
   請求項２から請求項６の何れか一項に記載の制御装置。
8. 前記重力補償弁開度算出部は、前記重力補償トルクを前記重力補償弁開度に変換する可変ゲイン係数を前記関節角度が目標角度に移行するまでに最速となる可変ゲイン係数を学習する
   請求項２から請求項７の何れか一項に記載の制御装置。
9. 駆動機構を有する機械装置の前記駆動機構を油圧により制御する制御装置の制御方法であって、
   前記駆動機構を制御して動作させる駆動機構構成部品の自重を打ち消す前記駆動機構を構成するリンク間の現在の角度またはその目標に対応する重力補償トルクを重力補償方程式を用いて算出し、
   前記重力補償トルクと、前記駆動機構を構成するリンク間の角度を示す関節角度の現在と目標との偏差と、前記目標のみに基づいて、前記重力補償トルクを、前記油圧の流量を制御する制御弁の弁開度を示す重力補償弁開度に変換し、
   前記駆動機構の現在の駆動状態量と目標の駆動状態量とにより定まるフィードバック弁開度に前記重力補償弁開度を加算して、前記制御弁の弁開度制御指令を算出する
   制御方法。
10. 駆動機構を有する機械装置の前記駆動機構を油圧により制御する制御装置のコンピュータを、
    前記駆動機構を制御して動作させる駆動機構構成部品の自重を打ち消す前記駆動機構を構成するリンク間の現在の角度またはその目標に対応する重力補償トルクを重力補償方程式を用いて算出する重力補償トルク算出手段、
    前記重力補償トルクと、前記駆動機構を構成するリンク間の角度を示す関節角度の現在と目標との偏差と、前記目標のみに基づいて、前記重力補償トルクを、前記油圧の流量を制御する制御弁の弁開度を示す重力補償弁開度に変換する重力補償弁開度算出手段、
    前記駆動機構の現在の駆動状態量と目標の駆動状態量とにより定まるフィードバック弁開度に前記重力補償弁開度を加算して、前記制御弁の弁開度制御指令を算出する弁開度制御手段、
    として機能させるプログラム。

Images