(57)【特許請求の範囲】
【請求項１】
上下方向に回動可能な複数のフロント部材により構成される多関節型のフロント装置と、このフロント装置を移動する複数の油圧アクチュエータと、これら複数の油圧アクチュエータに供給される圧油の流量を制御する複数の流量制御弁とを備え、前記複数のフロント部材が１つのブームと１つのアームを含み、前記複数のアクチュエータが前記ブームを駆動するブームシリンダと前記アームを駆動するアームシリンダを含む建設機械の軌跡制御装置において、
（ａ）前記アームの先端の目標速度ベクトルを指示する指示手段と、
（ｂ）前記ブーム及びアームの姿勢を検出する姿勢検出手段と、
（ｃ）前記姿勢検出手段で検出されたブーム及びアームの姿勢に基づき、前記目標速度ベクトルに応じた動作を前記アーム先端に行わせるベクトル軌跡制御のための指令信号を演算する第１の演算手段と、
（ｄ）前記ベクトル軌跡制御のための指令信号により前記ブーム及びアームが動かされるとき、前記ブームシリンダ及び前記アームシリンダのうちの一方である１つの油圧アクチュエータのストロークエンド近傍において、前記目標速度ベクトルに応じて前記ブームシリンダ及びアームシリンダによるベクトル軌跡制御から動き得る他の１つの油圧アクチュエータの単独動作制御へと連続して移行するように、前記指令信号を補正する第２の演算手段とを備え、前記第２演算手段は、
前記１つの油圧アクチュエータのストロークエンドまでの距離に応じて、ストロークエンドとストロークエンド近傍を除いた範囲では１で、ストロークエンドで０となり、ストロークエンド近傍ではストロークエンドに近づくにしたがって１から０に漸次小さくなるゲインを計算し、このゲインを前記１つの油圧アクチュエータに係わる指令信号と前記他の油圧アクチュエータに係わる指令信号とに乗ずることで前記指令信号を減少させる停止制御演算手段と、
前記１つの油圧アクチュエータのストロークエンド近傍において、前記目標速度ベクトルに対する前記動き得る他の油圧アクチュエータの補正動作信号を演算するとともに、前記１つの油圧アクチュエータのストロークエンドまでの距離に応じて、ストロークエンドとストロークエンド近傍を除いた範囲では０でストロークエンドで１となり、ストロークエンド近傍ではストロークエンドに近づくにしたがって０から１に漸次大きくなるゲインを計算し、このゲインを前記他の油圧アクチュエータの補正動作信号に乗ずることで前記補正動作信号を増加させる連続制御演算手段と、
前記停止制御演算手段からの信号と前記連続制御演算手段からの信号とを合成することにより前記１つの油圧アクチュエータに係わる指令信号と前記他の油圧アクチュエータに係わる指令信号を補正する信号合成手段とを有することを特徴とする建設機械の軌跡制御装置。
【請求項２】
請求項１記載の建設機械の軌跡制御装置において、前記連続制御演算手段は、前記１つの油圧アクチュエータのストロークエンド近傍において、前記目標速度ベクトルのうち、前記動き得る他の油圧アクチュエータにより駆動されるフロント部材の動作方向への投影成分を求め、この目標速度ベクトルの投影成分に相当する速度が得られるよう前期補正動作信号を求めることを特徴とする建設機械の軌跡制御装置。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
本発明は多関節型のフロント装置を備えた建設機械の軌跡制御装置に係わり、特に、ブーム、アーム、バケットなどのフロント装置を備えた油圧ショベルにおいて、そのフロント装置のバケットの目標速度ベクトルを指示し、ベクトル軌跡制御を行う建設機械の軌跡制御装置に関する。
【０００２】
【従来の技術】
油圧ショベルのフロント装置の先端部、つまりアーム先端の目標速度ベクトルを指示し、ベクトル軌跡制御を行う軌跡制御装置が知られている。この軌跡制御装置では、油圧アクチュエータがストロークエンドに達すると制御が不可能となり、通常では停止させているが、この場合は動作が不連続となるため操作性が悪い。
【０００３】
そこで、このような作業を容易にするため特開平７−２２９１６１号公報に記載のような提案がある。この提案では、フロント装置の姿勢と動作方向をパターン化し、油圧アクチュエータがストロークエンドに近づき、フロント装置が予め設定した軌跡制御可能領域の境界に近づいたと判別された場合、ベクトル軌跡制御を中止するとともに、メモリ手段のそのパターンでの記憶内容にしたがってブーム、アームの各腕を軌跡制御することでストロークエンドにおける動作を可能としている。
【０００４】
【発明が解決しようとする課題】
しかしながら、上記従来技術には次のような問題がある。
特開平７−２２９１６１号公報に記載の従来技術では、油圧アクチュエータがストロークエンドに近づいた軌跡制御可能領域の境界において、ベクトル軌跡制御からメモリ手段の記憶内容による各腕の軌跡制御へとオンオフ的に切り換える。このため、制御の連続性に滑らかさを欠き、動作がギクシャクする。例えば、バケットの動きが急に速くなったり、遅くなったりする。また、メモリ手段の記憶内容としては固定値が記憶され、フロント装置の姿勢と動作方向のパターンに応じてその中の１つを選択し、各腕の軌跡制御へと移行するが、固定値は目標速度ベクトルとは直接関係のない値であるため、当該制御はシーケンス的な制御となり、目標速度ベクトルの指示内容が反映されにくい。例えば、目標速度ベクトルをオペレータが操作レバーで教示する場合、オペレータの意志が動作に比例的に反映されにくい。
【０００５】
本発明の目的は、ベクトル軌跡制御による動作中、油圧アクチュエータのストロークエンド近傍においても目標速度ベクトルに応じた円滑な連続した動作を可能とし、操作性を良くできる建設機械の軌跡制御装置を提供することである。
【０００６】
【課題を解決するための手段】
（１）上記の目的を達成するために、本発明による建設機械の軌跡制御装置は次の構成を採用する。すなわち、上下方向に回動可能な複数のフロント部材により構成される多関節型のフロント装置と、このフロント装置を移動する複数の油圧アクチュエータと、これら複数の油圧アクチュエータに供給される圧油の流量を制御する複数の流量制御弁とを備え、前記複数のフロント部材が１つのブームと１つのアームを含み、前記複数のアクチュエータが前記ブームを駆動するブームシリンダと前記アームを駆動するアームシリンダを含む建設機械の軌跡制御装置において、（ａ）前記アームの先端の目標速度ベクトルを指示する指示手段と、（ｂ）前記ブーム及びアームの姿勢を検出する姿勢検出手段と、（ｃ）前記姿勢検出手段で検出されたブーム及びアームの姿勢に基づき、前記目標速度ベクトルに応じた動作を前記アーム先端に行わせるベクトル軌跡制御のための指令信号を演算する第１の演算手段と、（ｄ）前記ベクトル軌跡制御のための指令信号により前記ブーム及びアームが動かされるとき、前記ブームシリンダ及び前記アームシリンダのうちの一方である１つの油圧アクチュエータのストロークエンド近傍において、前記目標速度ベクトルに応じて前記ブームシリンダ及びアームシリンダによるベクトル軌跡制御から動き得る他の１つの油圧アクチュエータの単独動作制御へと連続して移行するように、前記指令信号を補正する第２の演算手段とを備え、前記第２の演算手段は、前記１つの油圧アクチュエータのストロークエンドまでの距離に応じて、ストロークエンドとストロークエンド近傍を除いた範囲では１で、ストロークエンドで０となり、ストロークエンド近傍ではストロークエンドに近づくにしたがって１から０に漸次小さくなるゲインを計算し、このゲインを前記１つの油圧アクチュエータに係わる指令信号と前記他の油圧アクチュエータに係わる指令信号とに乗ずることで前記指令信号を減少させる停止制御演算手段と、前記１つの油圧アクチュエータのストロークエンド近傍において、前記目標速度ベクトルに対する前記動き得る他の油圧アクチュエータの補正動作信号を演算するとともに、前記１つの油圧アクチュエータのストロークエンドまでの距離に応じて、ストロークエンドとストロークエンド近傍を除いた範囲では０でストロークエンドで１となり、ストロークエンド近傍ではストロークエンドに近づくにしたがって０から１に漸次大きくなるゲインを計算し、このゲインを前記他の油圧アクチュエータの補正動作信号に乗ずることで前記補正動作信号を増加させる連続制御演算手段と、前記停止制御演算手段からの信号と前記連続制御演算手段からの信号とを合成することにより前記１つの油圧アクチュエータに係わる指令信号と前記他の油圧アクチュエータに係わる指令信号を補正する信号合成手段とを有するものとする。
【０００７】
以上のように構成した本発明においては、指示手段により目標速度ベクトルが指示されると、第１の演算手段は、目標速度ベクトルに応じた動作をアーム先端に行わせるベクトル軌跡制御のための指令信号を演算し、ブーム及びアームはこの指令信号により動かされ、ベクトル軌跡制御が行われる。このようなベクトル軌跡制御による動作中、１つの油圧アクチュエータがストロークエンド近傍に達すると、第２の演算手段は、目標速度ベクトルに応じてブームシリンダ及びアームシリンダによるベクトル軌跡制御から動き得る他の１つの油圧アクチュエータの単独動作制御へと連続して移行するように、当該指令信号を補正する。これにより、他の油圧アクチュエータの動作制御への移行は連続的なものとなり、しかもこの他の油圧アクチュエータの動作制御への移行は目標速度ベクトルに応じたものとなる。よって、油圧アクチュエータのストロークエンド近傍においても目標速度ベクトルに応じた円滑な連続した動作が可能となり、操作性が良くなる。
【０００９】
また、ベクトル軌跡制御による動作中、１つの油圧アクチュエータがストロークエンド近傍に達する前は、停止制御演算手段は指令信号に乗じるゲインを１とし、連続制御演算手段は他の油圧アクチュエータの補正動作信号に乗じるゲインを０とし、通常の目標速度ベクトルによる軌跡制御が行われる。
【００１０】
一方、１つの油圧アクチュエータがストロークエンド近傍に達すると、停止制御演算手段はストロークエンドまでの距離が小さくなるにしたがって指令信号に乗じるゲイン徐々に減じ、ストロークエンドでゲインを０とし、連続制御演算手段はストロークエンドまでの距離が小さくなるにしたがって、別途演算して得た目標速度ベクトルに対する他の油圧アクチュエータの補正動作信号に乗じるゲインを徐々に増加させ、ストロークエンドでゲインを１とする。信号合成手段では、このようにして得られた停止制御演算手段からの信号と連続制御演算手段からの信号とを合成することにより指令信号を補正する。これにより、上記（１）のように他の油圧アクチュエータの動作制御への移行は連続的なものとなり、しかもこの他の油圧アクチュエータの動作制御への移行は目標速度ベクトルに応じたものとなる。
【００１１】
（２）また、上記（１）において、好ましくは、前記連続制御演算手段は、前記１つの油圧アクチュエータのストロークエンド近傍において、前記目標速度ベクトルのうち、前記動き得る他の油圧アクチュエータにより駆動されるフロント部材の動作方向への投影成分を求め、この目標速度ベクトルの投影成分に相当する速度が得られるよう前期補正動作信号を求める。
【００１２】
これにより連続制御演算手段で演算される補正動作信号は目標速度ベクトルに応じた値となり、上記（１）のように他の油圧アクチュエータの動作制御への移行は目標速度ベクトルに応じたものとなる。
【００１３】
【発明の実施の形態】
以下、本発明を油圧ショベルのフロント装置に適用した場合の実施形態を図１〜図８により説明する。
【００１４】
図１において、本発明が適用される油圧ショベルのフロント装置は、油圧ポンプ２と、この油圧ポンプからの圧油により駆動されるブームシリンダ３ａ、アームシリンダ３ｂ、バケットシリンダ３ｃを含む複数の油圧アクチュエータと、油圧アクチュエータ３ａ〜３ｃに供給される圧油の流量を制御する複数の流量制御弁４ａ〜４ｃとを有し、これらは油圧ショベルのフロント装置の被駆動部材を駆動する油圧駆動装置を構成している。
【００１５】
また、油圧ショベルは、図２に示すように、上下方向にそれぞれ回動するブーム１ａ、アーム１ｂ、およびバケット１ｃからなる多関節型のフロント装置１Ａと、上部旋回体１ｄと、下部走行体１ｅからなる車体１Ｂとで構成され、フロント装置１Ａのブーム１ａの基端は上部旋回体の前部に支持されている。
【００１６】
以上のような油圧ショベルのフロント装置に本実施例による軌跡制御装置が設けられている。この軌跡制御装置は、バケット１ｃの移動ベクトルと回転を教示する操作レバー装置５と、ブーム１ａ、アーム１ｂおよびバケット１ｃのそれぞれ回動支点に設けられ、フロント装置１Ａの姿勢に関する状態量としてそれぞれの回動角を検出する角度検出器６ａ，６ｂ，６ｃと、操作レバー装置５と角度検出器６ａ〜６ｃの信号を入力し、軌跡制御を行うための電気信号を出力する制御ユニット７とで構成されている。
【００１７】
操作レバー装置５は図３に示すように１本の操作レバー５ａを有し、この操作レバー５ａを倒すことでバケット１ｃの移動ベクトルを教示し、操作レバー５ａを回転させることでバケット１ｃの回転動作を教示する。操作レバー装置５の本体５ｂ内には、例えば操作レバー５ａのＸ方向の倒れ量、Ｙ方向の倒れ量をそれぞれ検出するポテンショメータ及び操作レバーの回転角を検出するポテンショメータが内蔵され、操作レバー５ａのＸ方向の倒れ量はバケット１ｃのＸ方向の教示速度成分ＶXとして検出され、操作レバー５ａのＹ方向の倒れ量はバケット１ｃのＹ方向の教示速度成分ＶYとして検出され、操作レバーの回転角はバケット１ｃの回転動作の教示角速度ＶZとして検出される。教示速度成分ＶX，ＶYは両者で教示速度ベクトルＶXYを構成する。これらの教示速度ベクトルＶXY及び回転の教示角速度ＶZはそれぞれ目標速度ベクトル及び目標角速度として制御ユニット７に入力される。なお、目標速度ベクトルとしては教示速度ベクトルＶXYを用いる代わりに制御ユニット７内に予め設定した値を用いてもよい。
【００１８】
制御ユニット７における処理内容を図４〜図６により説明する。
【００１９】
はじめに、図４において処理の全体の概要を説明する。
【００２０】
制御ユニット７は、座標変換演算部７ａ、ブーム・アーム停止制御部７Ａ、ブーム・アームの連続制御部７Ｂ、バケット制御部７Ｃ、流量制御弁の特性補正部７Ｄ，７Ｅ，７Ｆの各機能を有している。
【００２１】
まず、座標変換演算部７ａにおいて、通常の軌跡制御のように教示速度ベクトルＶXYと角度検出器６ａ，６ｂからの角度信号θa，θbから求めたフロント装置１Ａの姿勢に基づき、教示入力されたブーム・アームの角速度θ1iｄ，θ2iｄ（ｄは微分を意味する；以下同じ）を求める。
【００２２】
次に、ブーム・アーム停止制御部７Ａにおいて、ブームシリンダ３ａ又はアームシリンダ３ｂのストロークエンドにおいて動作を停止するように教示入力された角速度θ1iｄ，θ2iｄを補正し、目標ブーム角速度θ1sｄ及び目標アーム角速度θ2sｄを求める。
【００２３】
次に、ブーム・アームの連続制御部７Ｂにおいて、ブームシリンダ３ａ及びアームシリンダ３ｂの一方がストロークエンドに達していても他方が動作可能な場合には、教示速度ベクトルＶXYと角度検出器６ａ，６ｂの角度信号θa，θbとからブーム連続制御による目標ブーム角速度θ1cｄ及びアーム連続制御による目標アーム角速度θ2cｄを求める。
【００２４】
次に、和算部７ｑ，７ｒにおいて、目標ブーム角速度θ1sｄと目標ブーム角速度θ1cｄとの和、及び目標アーム角速度θ2sｄと目標アーム角速度θ2cｄとの和より出力すべき目標ブーム角速度θ1oｄ及び目標アーム角速度θ2oｄを求める。
【００２５】
次に、バケット制御部７Ｃにおいて、教示バケット角速度ＶZ及びブーム・アームの目標角速度θ1oｄ，θ2oｄに基づき出力すべき目標バケット角速度θ3oを求める。
【００２６】
次に、流量制御弁の特性補正部７Ｄ，７Ｅ，７Ｆにおいて、それぞれの流量制御弁の特性に基づく補正を行い、流量制御弁への指令値ｕ1，ｕ2，ｕ3を求める。
【００２７】
次に、図５に基づきこれらの処理の詳細を説明する。
【００２８】
まず、座標変換演算部７ａにおいて、教示速度ベクトルＶXYと角度検出器６ａ，６ｂからの角度信号θa，θbから求めたフロント装置１Ａの姿勢に基づき、教示入力されたブーム・アームの角速度θ1iｄ，θ2iｄを以下の式により求める。
【００２９】
θ1iｄ＝−（Ｘ2＊ＶX＋Ｙ2＊ＶY）／（Ｘ2＊Ｙ−Ｙ2＊Ｘ）    …（１）
θ2iｄ＝  （Ｘ ＊ＶX＋Ｙ ＊ＶY）／（Ｘ2＊Ｙ−Ｙ2＊Ｘ）    …（２）
ここで、
ＶX：教示速度ベクトルのＸ方向成分
ＶY：教示速度ベクトルのＹ方向成分
θ1：ブーム対地角（以下、単にブーム角度という：図２参照）
θ2：アームとブームがなす角度（以下、単にアーム角度という：図２参照）
Ｌ1：ブーム長さ（図２参照）
Ｌ2：アーム長さ（図２参照）
Ｘ＝Ｌ1＊ｃｏｓ（θ1）＋Ｌ2＊ｃｏｓ（θ1＋θ2）       …（３）
Ｙ＝Ｌ1＊ｓｉｎ（θ1）＋Ｌ2＊ｓｉｎ（θ1＋θ2）       …（４）
Ｘ2＝Ｌ2＊ｃｏｓ（θ1＋θ2）                          …（５）
Ｙ2＝Ｌ2＊ｓｉｎ（θ1＋θ2）                          …（６）
次に、図５の点線で囲んだブーム・アーム停止制御部７Ａに関して説明する。
【００３０】
まず、ブームストロークエンドでの停止制御ゲインの演算部７ｂにおいて、教示入力されたブーム角速度θ1iｄとブーム角度θ1に応じて、ブーム角速度θ1iｄが０より小さいときは、図６（ａ）のようにブームの角度θ1に応じて通常は１で最小角度においては０となり、かつ最小角度近傍では最小角度に近づくにしたがって１から０に漸次小さくなり、ブーム角速度θ1iｄが０より大きいときは、図６（ｂ）のようにブームの角度θ1に応じて通常は１で最大角度において０となり、かつ最大角度近傍では最大角度に近づくにしたがって１から０に漸次小さくなるブームストロークエンドでの停止制御ゲインｈ1iを求める。
【００３１】
次に、アームストロークエンドでの停止制御ゲインの演算部７ｃにおいても同様に、教示入力されたアーム角速度θ2iｄとアームの角度θ2に応じて図６（ａ）及び（ｂ）のようなアームストロークエンドでの停止制御ゲインｈ2iを求める。
【００３２】
次に、最小値の選択部７ｄにおいて、ブームストロークエンドでの停止制御ゲインｈ1iとアームストロークエンドでの停止制御ゲインｈ2iのうち小さい方を選択してブーム・アームの停止制御のゲインｈ12iとする。
【００３３】
次に、ブーム停止制御ゲインの乗算部７ｅにおいて、教示入力されたブーム角速度θ1iｄにブーム・アームの停止制御ゲインｈ12iを乗ずることで、
θ1sｄ＝ｈ12i＊θ1iｄ                             …（７）
によりブーム停止制御による目標ブーム角速度θ1sｄを求める。
【００３４】
次に、アーム停止制御ゲインの演算部７ｆにおいても同様に、教示入力されたアーム角速度θ2iｄにブーム・アームの停止制御ゲインｈ12iを乗ずることで、
θ2sｄ＝ｈ12i＊θ2iｄ                             …（８）
によりアーム停止制御による目標アーム角速度θ2sｄを求める。
【００３５】
次に、図５の点線で囲んだブーム・アームの連続制御部７Ｂに関して説明する。
【００３６】
まず、ブーム動作方向成分の演算部７ｇにおいて、図７の（ａ）及び図８の（ａ）のようにアームストロークエンド近傍において、教示入力された速度ベクトルＶXYに対し動作可能なブームシリンダ３ａによるブーム動作方向成分の角速度θ1rｄを、
θs1＝ｔａｎ-1（ＶY／ＶX）−ｔａｎ-1（Ｙ／Ｘ）    …（９）
Ｖs1＝ＶXY＊ｓｉｎ（θs1）                        …（１０）
θ1rｄ＝Ｖs1／Ｌs1                                …（１１）
ここで、
θs1：教示速度ベクトルＶXYの方向と動き得るブームシリンダ３ａによるブーム動作方向の垂線とのなす角度（ブーム連続制御角度）
Ｘ：アーム先端位置のＸ座標値
Ｙ：アーム先端位置のＹ座標値
Ｖs1：教示速度ベクトルＶXYのうち動き得るブームシリンダ３ａによるブーム動作方向への投影成分（補正ベクトル）
Ｌs1：ブーム基端からアーム先端までの距離
により求める。
【００３７】
次に、ブームストロークエンドでの停止制御ゲインの演算部７ｈにおいて、上記のように求めたブーム動作方向成分の角速度θ1rｄとブーム角度θ1に応じて、ブーム動作方向成分の角速度θ1rｄが０より小さいときは、図６（ａ）のようにブームの角度θ1に応じて通常は１で最小角度においては０となり、かつ最小角度近傍では最小角度に近づくにしたがって１から０に漸次小さくなり、ブーム動作方向成分の角速度θ1rｄが０より大きいときは、図６（ｂ）のようにブーム角度θ1に応じて通常は１で最大角度において０となり、かつ最大角度近傍では最大角度に近づくにしたがって１から０に漸次小さくなるブームストロークエンドでの停止制御ゲインｈ1sを求める。
【００３８】
次に、アームストロークエンドでの連続制御ゲインの演算部７ｉにおいて、教示入力されたアーム角速度θ2iｄとアーム角度θ2に応じて、アーム角速度θ2iｄが０より小さいときは図６（ｃ）のようにアーム角度θ2に応じて通常は０で最小角度においては１となり、かつ最小角度近傍では最小角度に近づくにしたがって０から１に漸次大きくなり、アーム角速度θ2iｄが０より大きいときは図６（ｄ）のようにアーム角度θ2に応じて通常は０で最大角度において１となり、かつ最大角度近傍では最大角度に近づくにしたがって０から１に漸次大きくなるアームストロークエンドでの連続制御ゲインｈ1cを求める。
【００３９】
次に、最小値の選択部７ｊにおいて、ブームストロークエンドでの停止制御ゲインｈ1sとアームストロークエンドでの連続制御ゲインｈ1cのうち小さい方を選択してブーム連続制御ゲインｈ1rを求める。
【００４０】
次に、ブーム連続制御ゲインの乗算部７ｋにおいて、ブーム動作方向成分の角速度θ1rｄにブーム連続制御ゲインｈ1rを乗ずることで、
θ1cｄ＝ｈ1r＊θ1rｄ                              …（１２）
によりブーム連続制御による目標ブーム角速度θ1cｄを求める。
【００４１】
次に、アームに関しても同様に、まず、アーム動作方向の成分の演算部７ｌにおいて、図７の（ｂ）及び図８の（ｂ）のようにブームストロークエンド近傍において、教示入力された速度ベクトルＶXYに対し動作可能なアームシリンダ３ｂによるアーム動作方向成分の角速度θ2rｄを、
θs2＝ｔａｎ-1（ＶY／ＶX）−（θ1＋θ2）          …（１３）
Ｖs2＝ＶXY＊ｓｉｎ（θs2）                        …（１４）
θ2ｄr＝Ｖs2／Ｌs2                                …（１５）
ここで、
θs2：教示速度ベクトルＶXYの方向と動き得るアームシリンダ３ｂによるアーム動作方向の垂線とのなす角度（アーム連続制御角度）
Ｖs2：教示速度ベクトルＶXYのうち動き得るアームシリンダ３ｂによるアーム動作方向への投影成分（補正ベクトル）
Ｌs2：アーム基端からアーム先端までの距離
により求める。
【００４２】
次に、アームストロークエンドでの停止制御ゲインの演算部７ｍにおいて、上記のように求めたアーム動作方向成分の角速度θ2rｄとアーム角度θ2に応じて、アーム動作方向成分の角速度θ2rｄが０より小さいときは、図６（ａ）の用にアーム角度θ2に応じて通常は１で最小角度においては０となり、かつ最小角度近傍では最小角度に近づくにしたがって１から０に漸次小さくなり、アーム動作方向成分の角速度θ2rｄが０より大きいときは、図６（ｂ）のようにアーム角度θ2に応じて通常は１で最大角度において０となり、かつ最大角度近傍では最大角度に近づくにしたがって１から０に漸次小さくなるアームストロークエンドでの停止制御ゲインｈ2sを求める。
【００４３】
次に、ブームストロークエンドでの連続制御ゲインの演算部７ｎにおいて、教示入力されたブーム角速度θ1iｄとブーム角度θ1に応じて、ブーム角速度θ1iｄが０より小さいときは図６（ｃ）のようにブーム角度θ1に応じて通常は０で最小角度においては１となり、かつ最小角度近傍では最小角度に近づくにしたがって０から１に漸次大きくなり、ブーム角速度θ1iｄが０より大きいときは図６（ｄ）のようにブーム角度θ1に応じて通常は０で最大角度において１となり、かつ最大角度近傍では最大角度に近づくにしたがって０から１に漸次大きくなるブームストロークエンドでの連続制御ゲインｈ2cを求める。
【００４４】
次に、最小値の選択部７ｏにおいて、アームストロークエンドでの停止制御ゲインｈ2sとブームストロークエンドでの連続制御ゲインｈ2cのうち小さい方を選択してアーム連続制御ゲインｈ2rを求める。
【００４５】
次に、アーム連続制御ゲインの乗算部７ｐにおいて、アーム動作方向成分の角速度θ2rｄにアーム連続制御ゲインｈ2rを乗ずることで、
θ2cｄ＝ｈ2r＊θ2rｄ                              …（１６）
によりアーム連続制御による目標アーム角速度θ2cｄを求める。
【００４６】
次に、以上で求めた停止制御による目標角速度と連続制御による目標角速度の和からブーム・アームの目標角速度を求める手順を説明する。
【００４７】
まず、ブーム停止制御と連続制御の和の演算部７ｑにおいて、ブーム停止制御による目標ブーム角速度θ1sｄとブーム連続制御による目標ブーム角速度θ1cｄとの和をとることで、
θ1oｄ＝θ1sｄ＋θ1cｄ                            …（１７）
により出力される目標ブーム角速度θ1oｄを求める。
【００４８】
次に、アーム停止制御と連続制御の和の演算部７ｒにおいて、アーム停止制御による目標アーム角速度θ2sｄとアーム連続制御による目標アーム角速度θ2cｄとの和をとることで、
θ2oｄ＝θ2sｄ＋θ2cｄ                            …（１８）
により出力される目標アーム角速度θ2oｄを求める。
【００４９】
次に、図５の点線で囲んだバケット制御部７Ｃに関して説明する。
【００５０】
まず、ブーム・アーム動作の補正部７ｓにおいて、教示入力されたバケット角速度ＶZと出力される目標ブーム角速度θ1oｄ、目標アーム角速度θ2oｄとの差をとることで、
θ3rｄ＝ＶZ−（θ1oｄ＋θ2oｄ）                   …（１９）
によりブーム・アーム動作の補正がされたバケット角速度θ3rｄを求める。
【００５１】
次に、バケットストロークエンドでの停止制御ゲインの演算部７ｔにおいて、図６のように、補正されたバケット角速度θ3rが０より小さいときは図６（ａ）のようにバケットの角度θ３に応じて通常は１で最小角度においては０となり、０より大きいときは図６（ｂ）のように最大角度においては０となるバケットストロークエンドでの停止制御ゲインｈ３ｓを求める。
【００５２】
次に、バケット停止制御ゲインの乗算部７ｔにおいて、補正されたバケット角速度θ3rｄにバケット停止制御ゲインｈ3sを乗ずることで、
θ3oｄ＝ｈ3s＊θ3rｄ                              …（２０）
によりバケット停止制御による目標バケット角速度θ3oｄを求め出力値とする。
【００５３】
以上において、操作レバー装置５は、フロント装置１Ａの先端部（アーム先端）の目標速度ベクトル（教示速度ベクトルＶXY）を指示する指示手段を構成し、角度検出器６ａ〜６ｃはフロント装置１Ａの姿勢を検出する姿勢検出手段を構成し、座標変換部７ａは、前記姿勢検出手段で検出されたフロント装置１Ａの姿勢に基づき、目標速度ベクトルに応じた動作をフロント装置１Ａの先端部に行わせるベクトル軌跡制御のための指令信号を演算する第１の演算手段を構成し、ブーム・アーム停止制御部７Ａ、ブーム・アーム連続制御部７Ｂ及び和の演算部７ｑ，７ｒは、前記指令信号によりフロント装置が動かされるとき、１つの油圧アクチュエータのストロークエンド近傍において、前記目標速度ベクトルに応じて前記ベクトル軌跡制御から動き得る他の油圧アクチュエータの動作制御へと連続して移行するように、前記指令信号を補正する第２の演算手段を構成する。
【００５４】
また、ブーム・アーム停止制御部７Ａは、１つの油圧アクチュエータのストロークエンドまでの距離により、通常は１でストロークエンドで０となるゲインを乗ずることで前記指令信号を減少させる停止制御演算手段を構成し、ブーム・アーム連続制御部７Ｂは、前記１つの油圧アクチュエータのストロークエンド近傍において、前記目標速度ベクトルに対する前記動き得る他の油圧アクチュエータの補正動作信号を演算するとともに、前記１つの油圧アクチュエータのストロークエンドまでの距離により、通常は０でストロークエンドで１となるゲインを乗ずることで前記補正動作信号を増加させる連続制御演算手段を構成し、和の演算部７ｑ，７ｒは、前記停止制御演算手段からの信号と前記連続制御演算手段からの信号とを合成することにより前記指令信号を補正する信号合成手段を構成する。
【００５５】
ここで、以上述べた本実施形態の動作を図９を用いて簡単に説明する。動作例として、教示速度ベクトルとしては垂直上方に一定で、ブームがストロークエンドに近づきアームによる補正動作が行われる場合を考える。
【００５６】
まず、図９（ａ）において、ブーム角度θ1がストロークエンドより十分離れている場合には、通常の軌跡制御が行われ、教示速度ベクトルＶXYがそのまま出力ベクトルＶXY’となる。
【００５７】
ＶXY’＝ＶXY                                        …（２１）
次に図９（ｂ）において、ブーム角度θ1がストロークエンドに近い場合には、教示速度ベクトルＶXYにブーム停止制御ゲインｈ1iを乗じたものと（１４）式のアーム補正ベクトルＶs2にアーム連続制御ゲインｈ2cを乗じたものとの和で出力ベクトルＶXY’が求められる。
【００５８】
ＶXY’＝ｈ1i＊ＶXY＋ｈ2c＊Ｖs2                      …（２２）
次に、図９（ｃ）において、ブーム角度θ1がストロークエンドにある場合には 、通常の軌跡制御は行われず、アーム補正ベクトルＶs2がそのまま出力ベクトルＶXY’となる。
【００５９】
ＶXY’＝Ｖs2                                        …（２３）
以上のように構成した本実施形態では、１つのアクチュエータのストロークエンド近傍で操作レバー装置５の操作により更に同じアクチュエータのストロークエンドの方向に教示入力された場合、従来のように教示入力されたベクトル軌跡制御に関して減速・停止制御を行い、更に他の動き得るアクチュエータに対しては、教示入力された速度ベクトルＶXYの動作方向成分により連続した動作を行うため、他のアクチュエータの動作制御への移行は連続的なものとなり、しかもこの他のアクチュエータの動作制御への移行は教示入力速度ベクトルＶXY（目標速度ベクトル）に応じたものとなる。よって、油圧アクチュエータのストロークエンド近傍においても教示入力速度ベクトルＶXYに応じた円滑な連続した動作が可能となり、ギクシャクした動きのない、オペレータの意志を反映した軌跡制御が行え、操作性を良くできる。
【００６０】
本発明の第２の実施形態を図１０及び図１１により説明する。上記実施形態では、教示入力された速度ベクトルＶXY（目標速度ベクトル）をそのまま座標変換部７ａでブーム・アームの角速度θ1iｄ，θ2iｄに変換し、指令信号としたが、本実施形態はフィードバック制御により位置補正をした値を指令信号とするものである。図中、図４に示す機能と同等の機能には同じ符号を付している。
【００６１】
図１０において、本実施形態の制御ユニット７（図１参照）は、図４に示す第１の実施形態の座標変換演算部７ａ、ブーム・アーム停止制御部７Ａ、ブーム・アームの連続制御部７Ｂ、バケット制御部７Ｃ、流量制御弁の特性補正部７Ｄ，７Ｅ，７Ｆの各機能に加えて、フィードバック制御演算部７Ｚの機能を有している。このフィードバック制御演算部７Ｚは、教示速度ベクトルＶXYの位置補正制御演算部７z1及び和の演算部７z2と教示角速度ＶZの位置補正制御演算部z3及び和の演算部７z4とで構成されている。
【００６２】
位置補正制御演算部７z1においては、図１１に示すように、その時の教示速度ベクトルＶXYの方向に直線を伸ばすことで目標軌跡ｒを求めると共に、前回の目標軌跡ｒとアーム先端の現在点との距離ｄを求め、この距離ｄにゲインＫFBを乗じることで、アーム先端の現在点より目標軌跡ｒ上に下ろした垂線に一致する補正ベクトルＫFB・ｄを求める。
【００６３】
演算部７z2では、教示速度ベクトルＶXYと補正ベクトルＫFB・ｄとの和をとることで、
ＶXYC＝ＶXY＋ＫFB・ｄ                               …（２４）
により出力される補正教示速度ベクトルＶXYCを求める。
【００６４】
また、位置補正制御演算部７z3においては、図１１に示すように、バケット１ｃの現在角度ＺLを求めると共に、教示角速度Ｖzを積分してバケット１ｃの目標角度Ｚrを求め、この目標角度Ｚrと現在角度ＺLとの差を、
ΔＺ＝Ｚr−ＺL                                      …（２５）
により求め、この角度差ΔＺにゲインＫFBを乗じることで、補正角速度ＫFB・ΔＺを求める。
【００６５】
演算部７z4では、教示角速度ＶZと補正角速度ＫFB・ΔＺとの和をとることで、
ＶZC＝ＶZ＋ＫFB・ΔＺ                               …（２６）
により出力される補正教示角速度ＶZCを求める。
【００６６】
その後の計算は、第１の実施形態と同様である。
【００６７】
以上のように構成した本実施形態によれば、フィードバック制御演算部７Ｚで実際の位置及び角度と目標とのずれを小さくするよう教示速度ベクトルＶXY及び教示角速度ＶZを補正し、この補正した値を用いて減速・停止制御及び増速・連続制御を行うので、操作レバーの操作でアーム先端の目標軌跡及びバケットの目標角度を自由に設定しながら軌跡制御を行うものにおいて、ストロークエンド近傍でも円滑な連続した動作を可能として操作性を良くできる。
【００６８】
【発明の効果】
本発明によれば、ベクトル軌跡制御による動作中、１つの油圧アクチュエータがストロークエンド近傍に達すると、目標速度ベクトルに応じて、動き得る他の油圧アクチュエータの動作制御へと制御が連続的に移行するため、油圧アクチュエータのストロークエンド近傍においても教示入力速度ベクトルに応じた円滑な連続した動作が可能となり、ギクシャクした動きのない、目標速度ベクトルの内容を反映した軌跡制御が行え、操作性を良くできる。
【図面の簡単な説明】
【図１】本発明の一実施形態による油圧ショベルの軌跡制御装置をその油圧回路とともに示す図である。
【図２】本発明が適用される油圧ショベルの外観を示す図である。
【図３】操作レバー装置の機能を説明する図である。
【図４】制御ユニットの機能を説明する図である。
【図５】ブーム・アーム停止制御部、ブーム・アームの連続制御部、バケット制御部の詳細を示す図である。
【図６】関節角度と停止制御ゲイン及び連続制御ゲインの関係を示す図である。
【図７】ブーム・アーム停止制御部及び連続制御部での処理を説明する図である。
【図８】ブーム・アーム停止制御部及び連続制御部での処理を説明する図である。
【図９】動作の一例を説明する図である。
【図１０】本発明の他の実施形態による油圧ショベルの軌跡制御装置における制御ユニットの機能を説明する図である。
【図１１】フィードバック制御演算部の処理を説明する図である。
【符号の説明】
１Ａ  フロント装置
１Ｂ  車体
１ａ  ブーム
１ｂ  アーム
１ｃ  バケット
１ｄ  上部旋回体
１ｅ  下部走行体
２    油圧ポンプ
３ａ〜３ｃ  油圧アクチュエータ
４ａ〜４ｃ  流量制御弁
５    操作レバー装置
６ａ〜６ｃ  角度検出器
７    制御ユニット
７ａ  座標変換演算部
７Ａ  ブーム・アーム停止制御部
７Ｂ  ブーム・アームの連続制御部
７Ｃ  バケット制御部
７Ｄ，７Ｅ，７Ｆ  流量制御弁の特性補正部
７ｑ，７ｒ  和の演算部
【図１】
0003749319.tif 000002
【図２】
0003749319.tif 000003
【図３】
0003749319.tif 000004
【図４】
0003749319.tif 000005
【図５】
0003749319.tif 000006
【図６】
0003749319.tif 000007
【図７】
0003749319.tif 000008
【図８】
0003749319.tif 000009
【図９】
0003749319.tif 000010
【図１０】
0003749319.tif 000011
【図１１】
0003749319.tif 000012