(57)【要約】【課題】リンク自体の重さを打ち消す重力補償トルクを精度良く油圧制御のフィードバック系に適用する。【解決手段】関節機構を介して接続された複数のリンクを含む駆動機構を有する機械装置の関節機構の関節角度を油圧により制御する。関節角度を制御して動作させるリンクの自重を打ち消す重力補償トルクを算出する。重力補償トルクを、油圧の流量を制御する制御弁の弁開度を示す重力補償弁開度に変換する。関節の現在の関節角度と目標の関節角度とにより定まる角度フィードバック弁開度と重力補償弁開度とに基づいて、制御弁の弁開度制御指令を算出する。【選択図】図１
2018097722.tif 000002
【特許請求の範囲】
【請求項１】
駆動機構を有する機械装置の前記駆動機構を油圧により制御する制御装置が、
前記駆動機構を制御して動作させる駆動機構構成部品の自重を打ち消す重力補償トルクを重力補償方程式を用いて算出する重力補償トルク算出部と、
前記重力補償トルクを前記油圧の流量を制御する制御弁の弁開度を示す重力補償弁開度に変換する重力補償弁開度算出部と、
前記駆動機構の現在の駆動状態量と目標の駆動状態量とにより定まるフィードバック弁開度と前記重力補償弁開度とに基づいて、前記制御弁の弁開度制御指令を算出する弁開度制御部と、
を備える制御装置。
【請求項２】
前記駆動機構が、関節機構を介して接続された複数のリンクを含み、
前記制御装置が、前記駆動機構を有する機械装置の前記関節機構の関節角度を前記油圧により制御し、
前記重力補償トルク算出部が、前記関節角度を制御して動作させる前記駆動機構構成部品であるリンクの自重を打ち消す前記重力補償トルクを重力補償方程式を用いて算出し、
前記弁開度制御部が、前記関節機構の現在の前記駆動状態量である関節角度と目標の前記駆動状態量である関節角度とにより定まる角度フィードバック弁開度と前記重力補償弁開度とに基づいて、前記制御弁の弁開度制御指令を算出する
請求項１に記載の制御装置。
【請求項３】
前記重力補償弁開度算出部は、前記現在の関節角度を前記重力補償方程式に入力して算出された前記重力補償トルクと、前記目標の関節角度と、前記現在の関節角度と前記目標の関節角度との偏差と、を用いて重力補償弁開度を算出する
請求項２に記載の制御装置。
【請求項４】
前記重力補償弁開度算出部は、前記目標の関節角度を前記重力補償方程式に入力して算出された前記重力補償トルクと、前記目標の関節角度と、前記現在の関節角度と前記目標の関節角度との偏差と、を用いて重力補償弁開度を算出する
請求項２に記載の制御装置。
【請求項５】
前記偏差の変動周波数を抑制する変動抑制部を備える請求項３または請求項４に記載の制御装置。
【請求項６】
前記重力補償弁開度算出部は、前記重力補償トルクを前記重力補償弁開度に変換する可変ゲイン係数を、一定角速度で変化させる前記関節角度の定常偏差を超える角度以上の場合には当該角度の大きさに比例させて所定の係数値より大きな所定の値まで増大させ、前記関節角度の定常偏差を超えない角度未満の場合には前記所定の係数値に保持する
請求項２から請求項５の何れか一項に記載の制御装置。
【請求項７】
前記重力補償トルク算出部は、前記制御弁が算出された弁開度に達するまでの応答時間に基づいて決定された所定時間分先行した時刻における重力補償トルクを算出して出力する
請求項２から請求項６の何れか一項に記載の制御装置。
【請求項８】
前記重力補償弁開度算出部は、前記重力補償トルクを前記重力補償弁開度に変換する可変ゲイン係数を前記関節角度が目標角度に移行するまでに最速となる可変ゲイン係数を学習する
請求項２から請求項７の何れか一項に記載の制御装置。
【請求項９】
駆動機構を有する機械装置の前記駆動機構を油圧により制御する制御装置の制御方法であって、
前記駆動機構を制御して動作させる駆動機構構成部品の自重を打ち消す重力補償トルクを重力補償方程式を用いて算出し、
前記重力補償トルクを前記油圧の流量を制御する制御弁の弁開度を示す重力補償弁開度に変換し、
前記駆動機構の現在の駆動状態量と目標の駆動状態量とにより定まるフィードバック弁開度と前記重力補償弁開度とに基づいて、前記制御弁の弁開度制御指令を算出する
制御方法。
【請求項１０】
駆動機構を有する機械装置の前記駆動機構を油圧により制御する制御装置のコンピュータを、
前記駆動機構を制御して動作させる駆動機構構成部品の自重を打ち消す重力補償トルクを重力補償方程式を用いて算出する重力補償トルク算出手段、
前記重力補償トルクを前記油圧の流量を制御する制御弁の弁開度を示す重力補償弁開度に変換する重力補償弁開度算出手段、
前記駆動機構の現在の駆動状態量と目標の駆動状態量とにより定まるフィードバック弁開度と前記重力補償弁開度とに基づいて、前記制御弁の弁開度制御指令を算出する弁開度制御手段、
として機能させるプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、機械装置を構成する関節機構の関節角度を油圧により制御する制御装置、制御方法、プログラムに関する。
【背景技術】
【０００２】
油圧マニピュレータなどの油圧で制御される機械装置は、単位体積当たりのエネルギー効率が高いという利点がある。反面、油圧で制御される機械装置は高精度な位置決め性能のために必要な、微小動作の制御が不得手である。油圧で制御される機械装置の一例として作業機械の技術が特許文献１に開示されている。当該特許文献１の技術はバネの力により作業機構の重量を補償している。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２０１５−１０５５６０号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
ところで上述のような機械装置においては油圧制御弁の開度に基づいて油圧量を調整し関節を中心に回転する２つのリンク間の角度である関節角度を決定するなどしている。このような機械装置において関節角度を制御して動作させるリンク自体の重さを打ち消す重力補償トルクを精度良く油圧制御のフィードバック系に適用することが求められていた。
【０００５】
そこでこの発明は、上述の課題を解決する制御装置、制御方法、プログラムを提供することを目的としている。
【課題を解決するための手段】
【０００６】
本発明の第１の態様によれば、駆動機構を有する機械装置の前記駆動機構を油圧により制御する制御装置が、前記駆動機構を制御して動作させる駆動機構構成部品の自重を打ち消す重力補償トルクを重力補償方程式を用いて算出する重力補償トルク算出部と、前記重力補償トルクを前記油圧の流量を制御する制御弁の弁開度を示す重力補償弁開度に変換する重力補償弁開度算出部と、前記駆動機構の現在の駆動状態量と目標の駆動状態量とにより定まるフィードバック弁開度と前記重力補償弁開度とに基づいて、前記制御弁の弁開度制御指令を算出する弁開度制御部と、を備えることを特徴とする。
【０００７】
上述の制御装置において、前記駆動機構が、関節機構を介して接続された複数のリンクを含み、前記制御装置が、前記駆動機構を有する機械装置の前記関節機構の関節角度を前記油圧により制御し、前記重力補償トルク算出部が、前記関節角度を制御して動作させる前記駆動機構構成部品であるリンクの自重を打ち消す前記重力補償トルクを重力補償方程式を用いて算出し、前記弁開度制御部が、前記関節機構の現在の前記駆動状態量である関節角度と目標の前記駆動状態量である関節角度とにより定まる角度フィードバック弁開度と前記重力補償弁開度とに基づいて、前記制御弁の弁開度制御指令を算出してもよい。
【０００８】
また上述の制御装置において、前記重力補償弁開度算出部は、前記現在の関節角度を前記重力補償方程式に入力して算出された前記重力補償トルクと、前記目標の関節角度と、前記現在の関節角度と前記目標の関節角度との偏差と、を用いて重力補償弁開度を算出してよい。
【０００９】
また上述の制御装置において、前記重力補償弁開度算出部は、前記目標の関節角度を前記重力補償方程式に入力して算出された前記重力補償トルクと、前記目標の関節角度と、前記現在の関節角度と前記目標の関節角度との偏差と、を用いて重力補償弁開度を算出してよい。
また上述の制御装置において、前記偏差の変動周波数を抑制する変動抑制部を備えてよい。
【００１０】
また上述の制御装置において、前記重力補償弁開度算出部は、前記重力補償トルクを前記重力補償弁開度に変換する可変ゲイン係数を、一定角速度で変化させる前記関節角度の定常偏差を超える角度以上の場合には当該角度の大きさに比例させて所定の係数値より大きな所定の値まで増大させ、前記関節角度の定常偏差を超えない角度未満の場合には前記所定の係数値に保持してよい。
【００１１】
また上述の制御装置において、前記重力補償トルク算出部は、前記制御弁が算出された弁開度に達するまでの応答時間に基づいて決定された所定時間分先行した時刻における重力補償トルクを算出して出力してよい。
【００１２】
また上述の制御装置において、前記重力補償弁開度算出部は、前記重力補償トルクを前記重力補償弁開度に変換する可変ゲイン係数を前記関節角度が目標角度に移行するまでに最速となる可変ゲイン係数を学習してよい。
【００１３】
本発明の第２の態様によれば、制御方法は、駆動機構を有する機械装置の前記駆動機構を油圧により制御する制御装置の制御方法であって、前記駆動機構を制御して動作させる駆動機構構成部品の自重を打ち消す重力補償トルクを重力補償方程式を用いて算出し、前記重力補償トルクを前記油圧の流量を制御する制御弁の弁開度を示す重力補償弁開度に変換し、前記駆動機構の現在の駆動状態量と目標の駆動状態量とにより定まるフィードバック弁開度と前記重力補償弁開度とに基づいて、前記制御弁の弁開度制御指令を算出することを特徴とする。
【００１４】
本発明の第３の態様によれば、プログラムは、駆動機構を有する機械装置の前記駆動機構を油圧により制御する制御装置のコンピュータを、前記駆動機構を制御して動作させる駆動機構構成部品の自重を打ち消す重力補償トルクを重力補償方程式を用いて算出する重力補償トルク算出手段、前記重力補償トルクを前記油圧の流量を制御する制御弁の弁開度を示す重力補償弁開度に変換する重力補償弁開度算出手段、前記駆動機構の現在の駆動状態量と目標の駆動状態量とにより定まるフィードバック弁開度と前記重力補償弁開度とに基づいて、前記制御弁の弁開度制御指令を算出する弁開度制御手段、として機能させることを特徴とする。
【発明の効果】
【００１５】
本発明によれば、重力補償トルクτｇｎ［Ｎｍ］を油圧の流量を制御する制御弁の弁開度を示す重力補償弁開度ａｇｎ［％］に変換する構成だけで、動作させるリンクの自重を打ち消すための弁開度制御指令を算出することができる。よって複雑なロジックを構築する必要が無く動作させるリンクの自重を打ち消すための弁開度制御指令を算出することができる。
【図面の簡単な説明】
【００１６】
【図１】第一の実施形態による制御装置の構成を示すブロック図である。
【図２】第一の実施形態による制御装置の処理フローを示す図である。
【図３】第二の実施形態による制御装置の構成を示すブロック図である。
【図４】第二の実施形態による制御装置の処理フローを示す図である。
【図５】第三実施形態による制御装置の可変ゲイン係数の決定概要を示す図である。
【図６】第四の実施形態による制御装置の重力補償トルクの出力タイミングを説明する図である。
【発明を実施するための形態】
【００１７】
＜第一の実施形態＞
以下、第一の実施形態による制御装置を図面を参照して説明する。
図１は第一の実施形態による制御装置の構成を示すブロック図である。
この図で示すように制御システムは制御装置１と機械装置２とにより構成されている。制御装置１は関節機構を介して接続された複数のリンクを含む駆動機構を有する機械装置２を制御する。制御装置１は機械装置２における関節機構の関節角度を油圧により制御する。機械装置２は例えば油圧マニピュレータなどであってよい。
【００１８】
制御装置１はコンピュータであり、ＲＯＭ（Read Only Memory）、ＲＡＭ（Random Access Memory）、ＳＳＤ（Solid State Drive）などの記憶部、ＣＰＵ（Central Processing Unit）、通信インタフェースなどのハードウェアによって構成されてよい。
【００１９】
制御装置１のＣＰＵは制御装置１に電源が投入されることに基づいて起動し、記憶している制御プログラムを実行する。これにより、制御装置１には、重力補償トルク算出部１１、重力補償弁開度算出部１２、フィードバック制御部１３、弁開度制御部１４の各機能が備わる。
【００２０】
重力補償トルク算出部１１は、機械装置２に備わる関節機構の関節角度を制御して動作させるリンクの自重を打ち消す重力補償トルクτｇｎ［Ｎｍ］を、重力補償方程式を用いて算出する。本実施形態において関節機構は機械装置２を構成する駆動機構の一態様である。またリンクは駆動機構を制御して動作させる駆動機構構成部品の一態様である。
重力補償弁開度算出部１２は、重力補償トルクτｇｎ［Ｎｍ］を、油圧の流量を制御する制御弁の弁開度を示す重力補償弁開度ａｇｎ［％］に変換する。具体的には、重力補償弁開度算出部１２は、重力補償トルクτｇｎ［Ｎｍ］と、関節角度の目標角度と、当該目標角度と関節角度の現在角度の偏差とを用いて重力補償弁開度ａｇｎ［％］を算出する。
【００２１】
フィードバック制御部１３は、駆動機構の現在の駆動状態量と目標の駆動状態量とにより定まるフィードバック弁開度と重力補償弁開度とに基づいて、制御弁の弁開度制御指令を算出する。具体的にはフィードバック制御部１３は、機械装置２に備わる関節機構の関節角度の目標角度θｒｎと現在角度θｎとを用いて、関節角度が目標角度θｒｎとなるようにフィードバック制御を行う。関節角度は駆動機構の駆動状態量の一態様である。フィードバック制御部１３はフィードバック制御により算出した角度フィードバック弁開度ａｆｎ［％］を出力する。
弁開度制御部１４は、重力補償弁開度算出部１２から取得した重力補償弁開度ａｇｎ［％］と、フィードバック制御部１３から取得した角度フィードバック弁開度ａｆｎ［％］とを用いて、制御弁の弁開度制御指令を算出する。
【００２２】
制御装置１の処理によれば、重力補償トルクτｇｎ［Ｎｍ］を、油圧の流量を制御する制御弁の弁開度を示す重力補償弁開度ａｇｎ［％］に変換することができる。したがって、動作させるリンクの自重を打ち消すための弁開度制御指令を簡易な構成で算出することができる。
【００２３】
図２は第一の実施形態による制御装置の処理フローを示す図である。
次に制御装置１の処理の詳細について説明する。
まずフィードバック制御部１３は一例として、二つのリンクが接続された関節機構の関節角度の目標角度θｒｎと現在角度θｎの偏差Δθｎを取得する（ステップＳ１０１）。偏差はθｒｎ−θｎにより算出することができる。フィードバック制御部１３は偏差Δθｎを用いて、当該Δθｎが０となるような角度フィードバック弁開度ａｆｎ［％］をフィードバック制御により算出する（ステップＳ１０２）。フィードバック制御部１３は角度フィードバック弁開度ａｆｎ［％］を弁開度制御部１４へ出力する。
【００２４】
他方、重力補償トルク算出部１１は、機械装置２から関節機構の現在角度θｎを取得する。重力補償トルク算出部１１は現在角度θｎを重力補償方程式ｇ（θｎ）に代入して、関節角度を制御して動作させるリンクの自重を打ち消す重力補償トルクτｇｎ［Ｎｍ］を算出する（ステップＳ１０３）。重力補償トルク算出部１１は重力補償トルクτｇｎ［Ｎｍ］を重力補償弁開度算出部１２へ出力する。
【００２５】
重力補償弁開度算出部１２は重力補償トルクτｇｎ［Ｎｍ］と、目標角度θｒｎと、偏差Δθｎとを取得する。重力補償弁開度算出部１２はτｇｎ［Ｎｍ］×θｒｎの値を算出する（ステップＳ１０４）。重力補償弁開度算出部１２はτｇｎ［Ｎｍ］×θｒｎの値と偏差Δθｎの値とに基づいて、式（１）で表される重力補償弁開度算出式の可変ゲイン係数Ｋｇｎを決定する（ステップＳ１０５）。
【００２６】
【数１】
2018097722.tif 000003
【００２７】
重力補償弁開度算出部１２は可変ゲイン係数Ｋｇｎを決定する場合に、τｇｎ［Ｎｍ］×θｒｎの値の正負の符号、偏差Δθｎの正負の符号に基づいて、可変ゲイン係数Ｋｇｎを増加させるか減少させるかを決定する。またτｇｎ［Ｎｍ］×θｒｎの値や偏差Δθｎの値に基づいて所定の算出式により、可変ゲイン係数Ｋｇｎの数値を特定してよい。
【００２８】
一例としては、ステップＳ１０５の処理において重力補償弁開度算出部１２は、τｇｎ［Ｎｍ］×θｒｎの値が＋を示し、偏差Δθｎの値が＋を示す場合、リンクを重力と逆らう方向に駆動する状況であって目標に到達していない状況と判定する。この場合、重力補償弁開度算出部１２は重力補償が足りないと判定し、可変ゲイン係数Ｋｇｎを増加させるようにその可変ゲイン係数Ｋｇｎを決定する。
また重力補償弁開度算出部１２は、τｇｎ［Ｎｍ］×θｒｎの値が＋を示し、偏差Δθｎの値が−を示す場合、リンクを重力と逆らう方向に駆動する状況であって目標を超えてリンクを作動させてしまった状況と判定する。この場合、重力補償弁開度算出部１２は重力補償が効きすぎであると判定し、可変ゲイン係数Ｋｇｎを減少させるようにその可変ゲイン係数Ｋｇｎを決定する。
【００２９】
また重力補償弁開度算出部１２は、τｇｎ［Ｎｍ］×θｒｎの値が−を示し、偏差Δθｎの値が＋を示す場合、リンクを重力方向に駆動する状況であって目標に到達していない状況と判定する。この場合、重力補償弁開度算出部１２は重力補償が効きすぎであると判定し、可変ゲイン係数Ｋｇｎを減少させるようにその可変ゲイン係数Ｋｇｎを決定する。
また重力補償弁開度算出部１２は、τｇｎ［Ｎｍ］×θｒｎの値が−を示し、偏差Δθｎの値が−を示す場合、リンクを重力方向に駆動する状況であって目標を超えてリンクを作動させてしまった状況と判定する。この場合、重力補償弁開度算出部１２は重力補償が足りないと判定し、可変ゲイン係数Ｋｇｎを増加少させるようにその可変ゲイン係数Ｋｇｎを決定する。
【００３０】
重力補償弁開度算出部１２は可変ゲイン係数Ｋｇｎを決定すると、上記式（１）により重力補償弁開度ａｇｎ［％］を算出する（ステップＳ１０６）。重力補償弁開度算出部１２は重力補償弁開度ａｇｎ［％］を弁開度制御部１４へ出力する。
【００３１】
弁開度制御部１４はフィードバック制御部１３から取得した角度フィードバック弁開度ａｆｎ［％］と、重力補償弁開度算出部１２から取得した重力補償弁開度ａｇｎ［％］を加算して、弁開度制御指令を算出する（ステップＳ１０７）。弁開度制御部１４は弁開度制御指令を機械装置２の対象の制御弁へ出力する（ステップＳ１０８）。
【００３２】
以上の処理によれば、重力補償トルクτｇｎ［Ｎｍ］を油圧の流量を制御する制御弁の弁開度を示す重力補償弁開度ａｇｎ［％］に変換することができる。したがって、動作させるリンクの自重を打ち消すための弁開度制御指令を算出することができる。
【００３３】
また上述の処理によれば、重力補償トルクτｇｎ［Ｎｍ］を、油圧の流量を制御する制御弁の弁開度を示す重力補償弁開度ａｇｎ［％］に変換する構成だけで、動作させるリンクの自重を打ち消すための弁開度制御指令を算出することができる。よって複雑なロジックを構築する必要が無く動作させるリンクの自重を打ち消すための弁開度制御指令を算出することができる。
またτｇｎ［Ｎｍ］×θｒｎの値の正負の符号、偏差Δθｎの正負の符号に基づいて、可変ゲイン係数Ｋｇｎを増加させるか減少させるかを決定するので、重力補償トルクτｇｎ［Ｎｍ］と目標角度の関係や、偏差Δθｎの正負に応じた適切な重力補償弁開度ａｇｎ［％］を算出することができる。
【００３４】
＜第二の実施形態＞
以下、第二の実施形態による制御装置を図面を参照して説明する。
図３は第二の実施形態による制御装置の構成を示すブロック図である。
第二の実施形態における制御装置１、機械装置２のハードウェア構成は第一の実施形態と同様である。
制御装置１のＣＰＵは制御装置１に電源が投入されることに基づいて起動し、記憶している制御プログラムを実行する。これにより、制御装置１には、図３に示すように重力補償トルク算出部１１、重力補償弁開度算出部１２、フィードバック制御部１３、弁開度制御部１４、変動抑制部１５の各機能が備わる。
【００３５】
重力補償トルク算出部１１は、機械装置２に備わる関節の角度を制御して動作させるリンクの自重を打ち消す重力補償トルクτｇｎ［Ｎｍ］を、重力補償方程式を用いて算出する。
重力補償弁開度算出部１２は、重力補償トルクτｇｎ［Ｎｍ］を、油圧の流量を制御する制御弁の弁開度を示す重力補償弁開度ａｇｎ［％］に変換する。具体的には、重力補償弁開度算出部１２は、関節角度の目標角度θｒｎを重力補償方程式に入力して算出された重力補償トルクτｇｎ［Ｎｍ］と、関節角度の目標角度θｒｎと、関節角度の現在角度θｎと関節角度の目標角度θｒｎの偏差Δθｎと、を用いて重力補償弁開度を算出する。
【００３６】
フィードバック制御部１３は、機械装置２に備わる関節機構の関節角度の目標角度θｒｎと現在角度θｎとを用いて、関節角度が目標角度θｒｎとなるようにフィードバック制御を行う。フィードバック制御部１３はフィードバック制御により算出した角度フィードバック弁開度ａｆｎ［％］を出力する。
弁開度制御部１４は、重力補償弁開度算出部１２から取得した重力補償弁開度ａｇｎ［％］にと、フィードバック制御部１３から取得した角度フィードバック弁開度ａｆｎ［％］とを用いて、制御弁の弁開度制御指令を算出する。
変動抑制部１５は関節角度の現在角度θｎと関節角度の目標角度θｒｎの偏差Δθｎの変動周波数を抑制する。例えば変動抑制部１５はローパスフィルタによって構成されてよい。変動抑制部１５の処理により、重力補償弁開度算出部１２に入力される関節角度の現在角度θｎと関節角度の目標角度θｒｎの偏差Δθｎは、その値の増減の頻度が抑制される。
【００３７】
図４は第二の実施形態による制御装置の処理フローを示す図である。
次に第二の実施形態による制御装置１の処理の詳細について説明する。
まずフィードバック制御部１３は二つのリンクが接続された関節機構の関節角度の目標角度θｒｎと現在角度θｎの偏差Δθｎを取得する（ステップＳ２０１）。偏差はθｒｎ−θｎにより算出することができる。フィードバック制御部１３は偏差Δθｎを用いて、当該Δθｎが０となるような角度フィードバック弁開度ａｆｎ［％］をフィードバック制御により算出する（ステップＳ２０２）。フィードバック制御部１３は角度フィードバック弁開度ａｆｎ［％］を弁開度制御部１４へ出力する。
【００３８】
他方、重力補償トルク算出部１１は、関節機構の目標角度θｒｎを取得する。重力補償トルク算出部１１は目標角度θｒｎを重力補償方程式ｇ（θｒｎ）に代入して、関節角度を制御して動作させるリンクの自重を打ち消す重力補償トルクτｇｎ［Ｎｍ］を算出する（ステップＳ２０３）。重力補償トルク算出部１１は重力補償トルクτｇｎ［Ｎｍ］を重力補償弁開度算出部１２へ出力する。
【００３９】
変動抑制部１５は偏差Δθｎを取得する。変動抑制部１５は取得した偏差Δθｎの変動周波数を抑制して重力補償弁開度算出部１２へ出力する（ステップＳ２０４）。重力補償弁開度算出部１２は重力補償トルクτｇｎ［Ｎｍ］と、目標角度θｒｎと、変動周波数が抑制された偏差Δθｎとを取得する。重力補償弁開度算出部１２はτｇｎ［Ｎｍ］×θｒｎの値を算出する（ステップＳ２０５）。重力補償弁開度算出部１２はτｇｎ［Ｎｍ］×θｒｎの値と偏差Δθｎの値とに基づいて、式（１）で表される重力補償弁開度算出式の可変ゲイン係数Ｋｇｎを決定する（ステップＳ２０６）。
【００４０】
第一の実施形態と同様に、第二の実施形態の重力補償弁開度算出部１２は可変ゲイン係数Ｋｇｎを決定する場合に、τｇｎ［Ｎｍ］×θｒｎの値の正負の符号、偏差Δθｎの正負の符号に基づいて、可変ゲイン係数Ｋｇｎを増加させるか減少させるかを決定する。またτｇｎ［Ｎｍ］×θｒｎの値や偏差Δθｎの値に基づいて所定の算出式により、可変ゲイン係数Ｋｇｎの数値を特定してよい。
【００４１】
重力補償弁開度算出部１２は可変ゲイン係数Ｋｇｎを決定すると、上記式（１）により重力補償弁開度ａｇｎ［％］を算出する（ステップＳ２０７）。重力補償弁開度算出部１２は重力補償弁開度ａｇｎ［％］を弁開度制御部１４へ出力する。
【００４２】
弁開度制御部１４はフィードバック制御部１３から取得した角度フィードバック弁開度ａｆｎ［％］と、重力補償弁開度算出部１２から取得した重力補償弁開度ａｇｎ［％］を加算して、弁開度制御指令を算出する（ステップＳ２０８）。弁開度制御部１４は弁開度制御指令を機械装置２の対象の制御弁へ出力する（ステップＳ２０９）。
【００４３】
以上の処理によれば、第一の実施形態と同様の効果に加え、変動抑制部１５が偏差の変動周波数を抑制するため、可変ゲイン係数Ｋｇｎの変動を抑えることができる。自重補償の思想は静的な（動特性を持たない）重量負荷変化を補償するもので、細かく補償量を変える必要はない場合もある。また補償しないことによる外乱はフィードバックで賄うことができる。むしろ、急変することがかえって外乱になる可能性があり、これを防ぐべく変動の少ない入力信号となるようにしている。
【００４４】
＜第三の実施形態＞
以下、第三の実施形態による制御装置を図面を参照して説明する。
図５は第三実施形態による制御装置の可変ゲイン係数の決定概要を示す図である。
重力補償弁開度算出部１２は、重力補償トルクτｇｎ［Ｎｍ］を重力補償弁開度ａｇｎ［％］に変換する可変ゲイン係数Ｋｇｎを偏差Δθｎの値に基づいて変化させる。例えば重力補償弁開度算出部１２は、一定角速度で変化させる関節角度の定常偏差を超える角度以上に関節角度を動かす場合には、当該関節角度の大きさに比例させて所定の係数値Ｋ１より大きな所定の値Ｋ２まで可変ゲイン係数Ｋｇｎを増大させる。
【００４５】
重力補償弁開度算出部１２は、一定角速度で変化させる関節角度の定常偏差を超えない角度未満で関節角度を動かす場合には、可変ゲイン係数Ｋｇｎを所定の係数値Ｋ１に保持する。
【００４６】
重力補償弁開度算出部１２は、偏差Δθｎが所定の大きさΔθ１となった場合に、当該偏差Δθｎの大きさに応じて、可変ゲイン係数Ｋｇｎを所定の係数値Ｋ１から比例的に増加させる。Δθ１は、一定角速度で行う関節角度の制御において発生する関節角度の定常偏差に基づいて決定する。可変ゲイン係数Ｋｇｎの上限であるＫ２は、出力可能な弁開度飽和値などから現実的な値を設定する。係数値Ｋ１は関節機構を静止状態で制御している時、フィードバック制御出力が殆どなく重力補償のための制御出力が主となる状態における可変ゲイン係数Ｋｇｎの値を目安として定める。
【００４７】
第三の実施形態による可変ゲイン係数Ｋｇｎの決定手法によれば、その値を都度調整する必要が無く、また、下限値を明確に設定していることから可変ゲイン係数Ｋｇｎを有意な範囲内で収束させることができる。
なお第三の実施形態による可変ゲイン係数Ｋｇｎの決定手法を第一の実施形態や第二の実施系値に適用してよい。
【００４８】
＜第四の実施形態＞
以下、第四の実施形態による制御装置を図面を参照して説明する。
図６は第四の実施形態による制御装置の重力補償トルクの出力タイミングを説明する図である。
図６で示す波形は、関節機構の関節角度が目標角度θｒｎに達するまでの当該角度の時間的な変化を示している。関節機構の関節角度が目標角度θｒｎに達するまでには応答時間が必要である。重力補償トルク算出部１１は、目標軌道θｒｎが予め与えられている場合、別途ステップ応答などで計測してある制御弁の応答時間λ分だけ先行して、リンクの自重を打ち消す重力補償トルクτｇｎ［Ｎｍ］を重力補償方程式ｇ（θｎ（ｔ＋λ））により算出して、その重力補償トルクτｇｎ［Ｎｍ］を時刻ｔにおいて重力補償弁開度算出部１２へ出力する。
【００４９】
なお、第二の実施形態の場合には、重力補償トルク算出部１１は、目標軌道θｒｎが予め与えられている場合、別途ステップ応答などで計測してある制御弁の応答時間λ分だけ先行して、リンクの自重を打ち消す重力補償トルクτｇｎ［Ｎｍ］を重力補償方程式ｇ（θｒｎ（ｔ＋λ））により算出して、その重力補償トルクτｇｎ［Ｎｍ］を時刻ｔにおいて重力補償弁開度算出部１２へ出力する。
【００５０】
油圧系は微小開度の非線形が強いため、応答遅れが無視できない。そのためこれを無駄時間と捉え、先行してリンクの自重を打ち消す重力補償トルクτｇｎ［Ｎｍ］を重力補償方程式ｇ（θｒｎ）により算出して、その重力補償トルクτｇｎ［Ｎｍ］を重力補償弁開度算出部１２へ出力する。これにより例えば第三の実施形態でΔθｎが動き始めに大きくプラスになり、従って可変ゲイン係数Ｋｇｎが大きくなるためΔθｎがマイナスになり可変ゲイン係数Ｋｇｎが小さくなるといった、可変ゲイン係数Ｋｇｎのハンチングを防ぐことができる。
【００５１】
＜第五の実施形態＞
重力補償弁開度算出部１２は、重力補償トルクτｇｎ［Ｎｍ］を重力補償弁開度ａｇｎ［％］に変換する可変ゲイン係数Ｋｇｎを関節角度が目標角度に移行するまでに最速となる可変ゲイン係数を学習するようにしてもよい。
例えば、重力補償弁開度算出部１２は可変ゲイン係数Ｋｇｎを角速度を入力とした変数としてＫｇｎ（θn’）として可変にする。可変ゲイン係数Ｋｇｎ（θn’）はある角速度（θn’）の際について何度か様々な軌道で動かし学習させるようにしてよい。
【００５２】
上述の各実施形態においては、駆動機構が、関節機構を介して接続された複数のリンクを含み、制御装置１が、駆動機構を有する機械装置２の関節機構の関節角度を油圧により制御している。そして、重力補償トルク算出部１１が、関節角度を制御して動作させる駆動機構構成部品であるリンクの自重を打ち消す重力補償トルクを重力補償方程式を用いて算出する。また弁開度制御部１４が、関節機構の現在の駆動状態量である関節角度と目標の駆動状態量である関節角度とにより定まる角度フィードバック弁開度と重力補償弁開度とに基づいて、制御弁の弁開度制御指令を算出する。
しかしながら駆動機構は関節機構以外の他の機構であってよい。例えば駆動機構は伸縮機構などの他の機構であってもよい。そして駆動状態量は駆動機構が伸縮機構である場合には伸び量や縮み量を示すものであってもよい。
【００５３】
駆動機構が関節機構以外である場合も、重力補償弁開度算出部１２は、現在の駆動状態量を重力補償方程式に入力して算出された重力補償トルクと、目標の駆動状態量と、現在の駆動状態量と目標の駆動状態量との偏差と、を用いて重力補償弁開度を算出してよい。
【００５４】
また駆動機構が関節機構以外である場合も、重力補償弁開度算出部１２は、目標の駆動状態量を重力補償方程式に入力して算出された重力補償トルクと、目標の駆動状態量と、現在の駆動状態量と目標の駆動状態量との偏差と、を用いて重力補償弁開度を算出してよい。
【００５５】
また駆動機構が関節機構以外である場合も、偏差の変動周波数を抑制する変動抑制部を備えてよい。
【００５６】
また駆動機構が関節機構以外である場合も、重力補償弁開度算出部１２は、重力補償トルクを重力補償弁開度に変換する可変ゲイン係数を、一定度合で変化させる駆動状態量の定常偏差を超える駆動状態量以上の場合には当該駆動状態量の大きさに比例させて所定の係数値より大きな所定の値まで増大させ、駆動状態量の定常偏差を超えない駆動状態量未満の場合には所定の係数値に保持するようにしてよい。
【００５７】
また駆動機構が関節機構以外である場合も、重力補償トルク算出部１１は、制御弁が算出された弁開度に達するまでの応答時間に基づいて決定された所定時間分先行した時刻における重力補償トルクを算出して出力してよい。
【００５８】
また駆動機構が関節機構以外である場合も、重力補償弁開度算出部１２は、重力補償トルクを重力補償弁開度に変換する可変ゲイン係数を駆動状態量が目標の駆動状態量に移行するまでに最速となる可変ゲイン係数を学習するようにしてもよい。
【００５９】
上述の制御装置１は内部に、コンピュータシステムを有している。そして、制御装置１に上述した各処理を行わせるためのプログラムは、当該制御装置１のコンピュータ読み取り可能な記録媒体に記憶されており、このプログラムを制御装置１のコンピュータが読み出して実行することによって、上記処理が行われる。ここでコンピュータ読み取り可能な記録媒体とは、磁気ディスク、光磁気ディスク、ＣＤ−ＲＯＭ、ＤＶＤ−ＲＯＭ、半導体メモリ等をいう。また、このコンピュータプログラムを通信回線によってコンピュータに配信し、この配信を受けたコンピュータが当該プログラムを実行するようにしても良い。
【００６０】
また、上記プログラムは、前述した各処理部の機能の一部を実現するためのものであっても良い。さらに、前述した機能をコンピュータシステムにすでに記録されているプログラムとの組み合わせで実現できるもの、いわゆる差分ファイル（差分プログラム）であっても良い。
【符号の説明】
【００６１】
１・・・制御装置
２・・・機械装置
１１・・・重力補償トルク算出部
１２・・・重力補償弁開度算出部
１３・・・フィードバック制御部
１４・・・弁開度制御部
１５・・・変動抑制部
【図１】
2018097722.tif 000004
【図２】
2018097722.tif 000005
【図３】
2018097722.tif 000006
【図４】
2018097722.tif 000007
【図５】
2018097722.tif 000008
【図６】
2018097722.tif 000009