JP3749319B2 - 建設機械の軌跡制御装置 - Google Patents
建設機械の軌跡制御装置 Download PDFInfo
- Publication number
- JP3749319B2 JP3749319B2 JP27493096A JP27493096A JP3749319B2 JP 3749319 B2 JP3749319 B2 JP 3749319B2 JP 27493096 A JP27493096 A JP 27493096A JP 27493096 A JP27493096 A JP 27493096A JP 3749319 B2 JP3749319 B2 JP 3749319B2
- Authority
- JP
- Japan
- Prior art keywords
- arm
- boom
- stroke end
- hydraulic actuator
- control
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Fee Related
Links
Images
Landscapes
- Operation Control Of Excavators (AREA)
Description
【発明の属する技術分野】
本発明は多関節型のフロント装置を備えた建設機械の軌跡制御装置に係わり、特に、ブーム、アーム、バケットなどのフロント装置を備えた油圧ショベルにおいて、そのフロント装置のバケットの目標速度ベクトルを指示し、ベクトル軌跡制御を行う建設機械の軌跡制御装置に関する。
【0002】
【従来の技術】
油圧ショベルのフロント装置の先端部、つまりアーム先端の目標速度ベクトルを指示し、ベクトル軌跡制御を行う軌跡制御装置が知られている。この軌跡制御装置では、油圧アクチュエータがストロークエンドに達すると制御が不可能となり、通常では停止させているが、この場合は動作が不連続となるため操作性が悪い。
【0003】
そこで、このような作業を容易にするため特開平7−229161号公報に記載のような提案がある。この提案では、フロント装置の姿勢と動作方向をパターン化し、油圧アクチュエータがストロークエンドに近づき、フロント装置が予め設定した軌跡制御可能領域の境界に近づいたと判別された場合、ベクトル軌跡制御を中止するとともに、メモリ手段のそのパターンでの記憶内容にしたがってブーム、アームの各腕を軌跡制御することでストロークエンドにおける動作を可能としている。
【0004】
【発明が解決しようとする課題】
しかしながら、上記従来技術には次のような問題がある。
特開平7−229161号公報に記載の従来技術では、油圧アクチュエータがストロークエンドに近づいた軌跡制御可能領域の境界において、ベクトル軌跡制御からメモリ手段の記憶内容による各腕の軌跡制御へとオンオフ的に切り換える。このため、制御の連続性に滑らかさを欠き、動作がギクシャクする。例えば、バケットの動きが急に速くなったり、遅くなったりする。また、メモリ手段の記憶内容としては固定値が記憶され、フロント装置の姿勢と動作方向のパターンに応じてその中の1つを選択し、各腕の軌跡制御へと移行するが、固定値は目標速度ベクトルとは直接関係のない値であるため、当該制御はシーケンス的な制御となり、目標速度ベクトルの指示内容が反映されにくい。例えば、目標速度ベクトルをオペレータが操作レバーで教示する場合、オペレータの意志が動作に比例的に反映されにくい。
【0005】
本発明の目的は、ベクトル軌跡制御による動作中、油圧アクチュエータのストロークエンド近傍においても目標速度ベクトルに応じた円滑な連続した動作を可能とし、操作性を良くできる建設機械の軌跡制御装置を提供することである。
【0006】
【課題を解決するための手段】
(1)上記の目的を達成するために、本発明による建設機械の軌跡制御装置は次の構成を採用する。すなわち、上下方向に回動可能な複数のフロント部材により構成される多関節型のフロント装置と、このフロント装置を移動する複数の油圧アクチュエータと、これら複数の油圧アクチュエータに供給される圧油の流量を制御する複数の流量制御弁とを備え、前記複数のフロント部材が1つのブームと1つのアームを含み、前記複数のアクチュエータが前記ブームを駆動するブームシリンダと前記アームを駆動するアームシリンダを含む建設機械の軌跡制御装置において、(a)前記アームの先端の目標速度ベクトルを指示する指示手段と、(b)前記ブーム及びアームの姿勢を検出する姿勢検出手段と、(c)前記姿勢検出手段で検出されたブーム及びアームの姿勢に基づき、前記目標速度ベクトルに応じた動作を前記アーム先端に行わせるベクトル軌跡制御のための指令信号を演算する第1の演算手段と、(d)前記ベクトル軌跡制御のための指令信号により前記ブーム及びアームが動かされるとき、前記ブームシリンダ及び前記アームシリンダのうちの一方である1つの油圧アクチュエータのストロークエンド近傍において、前記目標速度ベクトルに応じて前記ブームシリンダ及びアームシリンダによるベクトル軌跡制御から動き得る他の1つの油圧アクチュエータの単独動作制御へと連続して移行するように、前記指令信号を補正する第2の演算手段とを備え、前記第2の演算手段は、前記1つの油圧アクチュエータのストロークエンドまでの距離に応じて、ストロークエンドとストロークエンド近傍を除いた範囲では1で、ストロークエンドで0となり、ストロークエンド近傍ではストロークエンドに近づくにしたがって1から0に漸次小さくなるゲインを計算し、このゲインを前記1つの油圧アクチュエータに係わる指令信号と前記他の油圧アクチュエータに係わる指令信号とに乗ずることで前記指令信号を減少させる停止制御演算手段と、前記1つの油圧アクチュエータのストロークエンド近傍において、前記目標速度ベクトルに対する前記動き得る他の油圧アクチュエータの補正動作信号を演算するとともに、前記1つの油圧アクチュエータのストロークエンドまでの距離に応じて、ストロークエンドとストロークエンド近傍を除いた範囲では0でストロークエンドで1となり、ストロークエンド近傍ではストロークエンドに近づくにしたがって0から1に漸次大きくなるゲインを計算し、このゲインを前記他の油圧アクチュエータの補正動作信号に乗ずることで前記補正動作信号を増加させる連続制御演算手段と、前記停止制御演算手段からの信号と前記連続制御演算手段からの信号とを合成することにより前記1つの油圧アクチュエータに係わる指令信号と前記他の油圧アクチュエータに係わる指令信号を補正する信号合成手段とを有するものとする。
【0007】
以上のように構成した本発明においては、指示手段により目標速度ベクトルが指示されると、第1の演算手段は、目標速度ベクトルに応じた動作をアーム先端に行わせるベクトル軌跡制御のための指令信号を演算し、ブーム及びアームはこの指令信号により動かされ、ベクトル軌跡制御が行われる。このようなベクトル軌跡制御による動作中、1つの油圧アクチュエータがストロークエンド近傍に達すると、第2の演算手段は、目標速度ベクトルに応じてブームシリンダ及びアームシリンダによるベクトル軌跡制御から動き得る他の1つの油圧アクチュエータの単独動作制御へと連続して移行するように、当該指令信号を補正する。これにより、他の油圧アクチュエータの動作制御への移行は連続的なものとなり、しかもこの他の油圧アクチュエータの動作制御への移行は目標速度ベクトルに応じたものとなる。よって、油圧アクチュエータのストロークエンド近傍においても目標速度ベクトルに応じた円滑な連続した動作が可能となり、操作性が良くなる。
【0009】
また、ベクトル軌跡制御による動作中、1つの油圧アクチュエータがストロークエンド近傍に達する前は、停止制御演算手段は指令信号に乗じるゲインを1とし、連続制御演算手段は他の油圧アクチュエータの補正動作信号に乗じるゲインを0とし、通常の目標速度ベクトルによる軌跡制御が行われる。
【0010】
一方、1つの油圧アクチュエータがストロークエンド近傍に達すると、停止制御演算手段はストロークエンドまでの距離が小さくなるにしたがって指令信号に乗じるゲイン徐々に減じ、ストロークエンドでゲインを0とし、連続制御演算手段はストロークエンドまでの距離が小さくなるにしたがって、別途演算して得た目標速度ベクトルに対する他の油圧アクチュエータの補正動作信号に乗じるゲインを徐々に増加させ、ストロークエンドでゲインを1とする。信号合成手段では、このようにして得られた停止制御演算手段からの信号と連続制御演算手段からの信号とを合成することにより指令信号を補正する。これにより、上記(1)のように他の油圧アクチュエータの動作制御への移行は連続的なものとなり、しかもこの他の油圧アクチュエータの動作制御への移行は目標速度ベクトルに応じたものとなる。
【0011】
(2)また、上記(1)において、好ましくは、前記連続制御演算手段は、前記1つの油圧アクチュエータのストロークエンド近傍において、前記目標速度ベクトルのうち、前記動き得る他の油圧アクチュエータにより駆動されるフロント部材の動作方向への投影成分を求め、この目標速度ベクトルの投影成分に相当する速度が得られるよう前期補正動作信号を求める。
【0012】
これにより連続制御演算手段で演算される補正動作信号は目標速度ベクトルに応じた値となり、上記(1)のように他の油圧アクチュエータの動作制御への移行は目標速度ベクトルに応じたものとなる。
【0013】
【発明の実施の形態】
以下、本発明を油圧ショベルのフロント装置に適用した場合の実施形態を図1〜図8により説明する。
【0014】
図1において、本発明が適用される油圧ショベルのフロント装置は、油圧ポンプ2と、この油圧ポンプからの圧油により駆動されるブームシリンダ3a、アームシリンダ3b、バケットシリンダ3cを含む複数の油圧アクチュエータと、油圧アクチュエータ3a〜3cに供給される圧油の流量を制御する複数の流量制御弁4a〜4cとを有し、これらは油圧ショベルのフロント装置の被駆動部材を駆動する油圧駆動装置を構成している。
【0015】
また、油圧ショベルは、図2に示すように、上下方向にそれぞれ回動するブーム1a、アーム1b、およびバケット1cからなる多関節型のフロント装置1Aと、上部旋回体1dと、下部走行体1eからなる車体1Bとで構成され、フロント装置1Aのブーム1aの基端は上部旋回体の前部に支持されている。
【0016】
以上のような油圧ショベルのフロント装置に本実施例による軌跡制御装置が設けられている。この軌跡制御装置は、バケット1cの移動ベクトルと回転を教示する操作レバー装置5と、ブーム1a、アーム1bおよびバケット1cのそれぞれ回動支点に設けられ、フロント装置1Aの姿勢に関する状態量としてそれぞれの回動角を検出する角度検出器6a,6b,6cと、操作レバー装置5と角度検出器6a〜6cの信号を入力し、軌跡制御を行うための電気信号を出力する制御ユニット7とで構成されている。
【0017】
操作レバー装置5は図3に示すように1本の操作レバー5aを有し、この操作レバー5aを倒すことでバケット1cの移動ベクトルを教示し、操作レバー5aを回転させることでバケット1cの回転動作を教示する。操作レバー装置5の本体5b内には、例えば操作レバー5aのX方向の倒れ量、Y方向の倒れ量をそれぞれ検出するポテンショメータ及び操作レバーの回転角を検出するポテンショメータが内蔵され、操作レバー5aのX方向の倒れ量はバケット1cのX方向の教示速度成分VXとして検出され、操作レバー5aのY方向の倒れ量はバケット1cのY方向の教示速度成分VYとして検出され、操作レバーの回転角はバケット1cの回転動作の教示角速度VZとして検出される。教示速度成分VX,VYは両者で教示速度ベクトルVXYを構成する。これらの教示速度ベクトルVXY及び回転の教示角速度VZはそれぞれ目標速度ベクトル及び目標角速度として制御ユニット7に入力される。なお、目標速度ベクトルとしては教示速度ベクトルVXYを用いる代わりに制御ユニット7内に予め設定した値を用いてもよい。
【0018】
制御ユニット7における処理内容を図4〜図6により説明する。
【0019】
はじめに、図4において処理の全体の概要を説明する。
【0020】
制御ユニット7は、座標変換演算部7a、ブーム・アーム停止制御部7A、ブーム・アームの連続制御部7B、バケット制御部7C、流量制御弁の特性補正部7D,7E,7Fの各機能を有している。
【0021】
まず、座標変換演算部7aにおいて、通常の軌跡制御のように教示速度ベクトルVXYと角度検出器6a,6bからの角度信号θa,θbから求めたフロント装置1Aの姿勢に基づき、教示入力されたブーム・アームの角速度θ1id,θ2id(dは微分を意味する;以下同じ)を求める。
【0022】
次に、ブーム・アーム停止制御部7Aにおいて、ブームシリンダ3a又はアームシリンダ3bのストロークエンドにおいて動作を停止するように教示入力された角速度θ1id,θ2idを補正し、目標ブーム角速度θ1sd及び目標アーム角速度θ2sdを求める。
【0023】
次に、ブーム・アームの連続制御部7Bにおいて、ブームシリンダ3a及びアームシリンダ3bの一方がストロークエンドに達していても他方が動作可能な場合には、教示速度ベクトルVXYと角度検出器6a,6bの角度信号θa,θbとからブーム連続制御による目標ブーム角速度θ1cd及びアーム連続制御による目標アーム角速度θ2cdを求める。
【0024】
次に、和算部7q,7rにおいて、目標ブーム角速度θ1sdと目標ブーム角速度θ1cdとの和、及び目標アーム角速度θ2sdと目標アーム角速度θ2cdとの和より出力すべき目標ブーム角速度θ1od及び目標アーム角速度θ2odを求める。
【0025】
次に、バケット制御部7Cにおいて、教示バケット角速度VZ及びブーム・アームの目標角速度θ1od,θ2odに基づき出力すべき目標バケット角速度θ3oを求める。
【0026】
次に、流量制御弁の特性補正部7D,7E,7Fにおいて、それぞれの流量制御弁の特性に基づく補正を行い、流量制御弁への指令値u1,u2,u3を求める。
【0027】
次に、図5に基づきこれらの処理の詳細を説明する。
【0028】
まず、座標変換演算部7aにおいて、教示速度ベクトルVXYと角度検出器6a,6bからの角度信号θa,θbから求めたフロント装置1Aの姿勢に基づき、教示入力されたブーム・アームの角速度θ1id,θ2idを以下の式により求める。
【0029】
θ1id=−(X2*VX+Y2*VY)/(X2*Y−Y2*X) …(1)
θ2id= (X *VX+Y *VY)/(X2*Y−Y2*X) …(2)
ここで、
VX:教示速度ベクトルのX方向成分
VY:教示速度ベクトルのY方向成分
θ1:ブーム対地角(以下、単にブーム角度という:図2参照)
θ2:アームとブームがなす角度(以下、単にアーム角度という:図2参照)
L1:ブーム長さ(図2参照)
L2:アーム長さ(図2参照)
X=L1*cos(θ1)+L2*cos(θ1+θ2) …(3)
Y=L1*sin(θ1)+L2*sin(θ1+θ2) …(4)
X2=L2*cos(θ1+θ2) …(5)
Y2=L2*sin(θ1+θ2) …(6)
次に、図5の点線で囲んだブーム・アーム停止制御部7Aに関して説明する。
【0030】
まず、ブームストロークエンドでの停止制御ゲインの演算部7bにおいて、教示入力されたブーム角速度θ1idとブーム角度θ1に応じて、ブーム角速度θ1idが0より小さいときは、図6(a)のようにブームの角度θ1に応じて通常は1で最小角度においては0となり、かつ最小角度近傍では最小角度に近づくにしたがって1から0に漸次小さくなり、ブーム角速度θ1idが0より大きいときは、図6(b)のようにブームの角度θ1に応じて通常は1で最大角度において0となり、かつ最大角度近傍では最大角度に近づくにしたがって1から0に漸次小さくなるブームストロークエンドでの停止制御ゲインh1iを求める。
【0031】
次に、アームストロークエンドでの停止制御ゲインの演算部7cにおいても同様に、教示入力されたアーム角速度θ2idとアームの角度θ2に応じて図6(a)及び(b)のようなアームストロークエンドでの停止制御ゲインh2iを求める。
【0032】
次に、最小値の選択部7dにおいて、ブームストロークエンドでの停止制御ゲインh1iとアームストロークエンドでの停止制御ゲインh2iのうち小さい方を選択してブーム・アームの停止制御のゲインh12iとする。
【0033】
次に、ブーム停止制御ゲインの乗算部7eにおいて、教示入力されたブーム角速度θ1idにブーム・アームの停止制御ゲインh12iを乗ずることで、
θ1sd=h12i*θ1id …(7)
によりブーム停止制御による目標ブーム角速度θ1sdを求める。
【0034】
次に、アーム停止制御ゲインの演算部7fにおいても同様に、教示入力されたアーム角速度θ2idにブーム・アームの停止制御ゲインh12iを乗ずることで、
θ2sd=h12i*θ2id …(8)
によりアーム停止制御による目標アーム角速度θ2sdを求める。
【0035】
次に、図5の点線で囲んだブーム・アームの連続制御部7Bに関して説明する。
【0036】
まず、ブーム動作方向成分の演算部7gにおいて、図7の(a)及び図8の(a)のようにアームストロークエンド近傍において、教示入力された速度ベクトルVXYに対し動作可能なブームシリンダ3aによるブーム動作方向成分の角速度θ1rdを、
θs1=tan-1(VY/VX)−tan-1(Y/X) …(9)
Vs1=VXY*sin(θs1) …(10)
θ1rd=Vs1/Ls1 …(11)
ここで、
θs1:教示速度ベクトルVXYの方向と動き得るブームシリンダ3aによるブーム動作方向の垂線とのなす角度(ブーム連続制御角度)
X:アーム先端位置のX座標値
Y:アーム先端位置のY座標値
Vs1:教示速度ベクトルVXYのうち動き得るブームシリンダ3aによるブーム動作方向への投影成分(補正ベクトル)
Ls1:ブーム基端からアーム先端までの距離
により求める。
【0037】
次に、ブームストロークエンドでの停止制御ゲインの演算部7hにおいて、上記のように求めたブーム動作方向成分の角速度θ1rdとブーム角度θ1に応じて、ブーム動作方向成分の角速度θ1rdが0より小さいときは、図6(a)のようにブームの角度θ1に応じて通常は1で最小角度においては0となり、かつ最小角度近傍では最小角度に近づくにしたがって1から0に漸次小さくなり、ブーム動作方向成分の角速度θ1rdが0より大きいときは、図6(b)のようにブーム角度θ1に応じて通常は1で最大角度において0となり、かつ最大角度近傍では最大角度に近づくにしたがって1から0に漸次小さくなるブームストロークエンドでの停止制御ゲインh1sを求める。
【0038】
次に、アームストロークエンドでの連続制御ゲインの演算部7iにおいて、教示入力されたアーム角速度θ2idとアーム角度θ2に応じて、アーム角速度θ2idが0より小さいときは図6(c)のようにアーム角度θ2に応じて通常は0で最小角度においては1となり、かつ最小角度近傍では最小角度に近づくにしたがって0から1に漸次大きくなり、アーム角速度θ2idが0より大きいときは図6(d)のようにアーム角度θ2に応じて通常は0で最大角度において1となり、かつ最大角度近傍では最大角度に近づくにしたがって0から1に漸次大きくなるアームストロークエンドでの連続制御ゲインh1cを求める。
【0039】
次に、最小値の選択部7jにおいて、ブームストロークエンドでの停止制御ゲインh1sとアームストロークエンドでの連続制御ゲインh1cのうち小さい方を選択してブーム連続制御ゲインh1rを求める。
【0040】
次に、ブーム連続制御ゲインの乗算部7kにおいて、ブーム動作方向成分の角速度θ1rdにブーム連続制御ゲインh1rを乗ずることで、
θ1cd=h1r*θ1rd …(12)
によりブーム連続制御による目標ブーム角速度θ1cdを求める。
【0041】
次に、アームに関しても同様に、まず、アーム動作方向の成分の演算部7lにおいて、図7の(b)及び図8の(b)のようにブームストロークエンド近傍において、教示入力された速度ベクトルVXYに対し動作可能なアームシリンダ3bによるアーム動作方向成分の角速度θ2rdを、
θs2=tan-1(VY/VX)−(θ1+θ2) …(13)
Vs2=VXY*sin(θs2) …(14)
θ2dr=Vs2/Ls2 …(15)
ここで、
θs2:教示速度ベクトルVXYの方向と動き得るアームシリンダ3bによるアーム動作方向の垂線とのなす角度(アーム連続制御角度)
Vs2:教示速度ベクトルVXYのうち動き得るアームシリンダ3bによるアーム動作方向への投影成分(補正ベクトル)
Ls2:アーム基端からアーム先端までの距離
により求める。
【0042】
次に、アームストロークエンドでの停止制御ゲインの演算部7mにおいて、上記のように求めたアーム動作方向成分の角速度θ2rdとアーム角度θ2に応じて、アーム動作方向成分の角速度θ2rdが0より小さいときは、図6(a)の用にアーム角度θ2に応じて通常は1で最小角度においては0となり、かつ最小角度近傍では最小角度に近づくにしたがって1から0に漸次小さくなり、アーム動作方向成分の角速度θ2rdが0より大きいときは、図6(b)のようにアーム角度θ2に応じて通常は1で最大角度において0となり、かつ最大角度近傍では最大角度に近づくにしたがって1から0に漸次小さくなるアームストロークエンドでの停止制御ゲインh2sを求める。
【0043】
次に、ブームストロークエンドでの連続制御ゲインの演算部7nにおいて、教示入力されたブーム角速度θ1idとブーム角度θ1に応じて、ブーム角速度θ1idが0より小さいときは図6(c)のようにブーム角度θ1に応じて通常は0で最小角度においては1となり、かつ最小角度近傍では最小角度に近づくにしたがって0から1に漸次大きくなり、ブーム角速度θ1idが0より大きいときは図6(d)のようにブーム角度θ1に応じて通常は0で最大角度において1となり、かつ最大角度近傍では最大角度に近づくにしたがって0から1に漸次大きくなるブームストロークエンドでの連続制御ゲインh2cを求める。
【0044】
次に、最小値の選択部7oにおいて、アームストロークエンドでの停止制御ゲインh2sとブームストロークエンドでの連続制御ゲインh2cのうち小さい方を選択してアーム連続制御ゲインh2rを求める。
【0045】
次に、アーム連続制御ゲインの乗算部7pにおいて、アーム動作方向成分の角速度θ2rdにアーム連続制御ゲインh2rを乗ずることで、
θ2cd=h2r*θ2rd …(16)
によりアーム連続制御による目標アーム角速度θ2cdを求める。
【0046】
次に、以上で求めた停止制御による目標角速度と連続制御による目標角速度の和からブーム・アームの目標角速度を求める手順を説明する。
【0047】
まず、ブーム停止制御と連続制御の和の演算部7qにおいて、ブーム停止制御による目標ブーム角速度θ1sdとブーム連続制御による目標ブーム角速度θ1cdとの和をとることで、
θ1od=θ1sd+θ1cd …(17)
により出力される目標ブーム角速度θ1odを求める。
【0048】
次に、アーム停止制御と連続制御の和の演算部7rにおいて、アーム停止制御による目標アーム角速度θ2sdとアーム連続制御による目標アーム角速度θ2cdとの和をとることで、
θ2od=θ2sd+θ2cd …(18)
により出力される目標アーム角速度θ2odを求める。
【0049】
次に、図5の点線で囲んだバケット制御部7Cに関して説明する。
【0050】
まず、ブーム・アーム動作の補正部7sにおいて、教示入力されたバケット角速度VZと出力される目標ブーム角速度θ1od、目標アーム角速度θ2odとの差をとることで、
θ3rd=VZ−(θ1od+θ2od) …(19)
によりブーム・アーム動作の補正がされたバケット角速度θ3rdを求める。
【0051】
次に、バケットストロークエンドでの停止制御ゲインの演算部7tにおいて、図6のように、補正されたバケット角速度θ3rが0より小さいときは図6(a)のようにバケットの角度θ3に応じて通常は1で最小角度においては0となり、0より大きいときは図6(b)のように最大角度においては0となるバケットストロークエンドでの停止制御ゲインh3sを求める。
【0052】
次に、バケット停止制御ゲインの乗算部7tにおいて、補正されたバケット角速度θ3rdにバケット停止制御ゲインh3sを乗ずることで、
θ3od=h3s*θ3rd …(20)
によりバケット停止制御による目標バケット角速度θ3odを求め出力値とする。
【0053】
以上において、操作レバー装置5は、フロント装置1Aの先端部(アーム先端)の目標速度ベクトル(教示速度ベクトルVXY)を指示する指示手段を構成し、角度検出器6a〜6cはフロント装置1Aの姿勢を検出する姿勢検出手段を構成し、座標変換部7aは、前記姿勢検出手段で検出されたフロント装置1Aの姿勢に基づき、目標速度ベクトルに応じた動作をフロント装置1Aの先端部に行わせるベクトル軌跡制御のための指令信号を演算する第1の演算手段を構成し、ブーム・アーム停止制御部7A、ブーム・アーム連続制御部7B及び和の演算部7q,7rは、前記指令信号によりフロント装置が動かされるとき、1つの油圧アクチュエータのストロークエンド近傍において、前記目標速度ベクトルに応じて前記ベクトル軌跡制御から動き得る他の油圧アクチュエータの動作制御へと連続して移行するように、前記指令信号を補正する第2の演算手段を構成する。
【0054】
また、ブーム・アーム停止制御部7Aは、1つの油圧アクチュエータのストロークエンドまでの距離により、通常は1でストロークエンドで0となるゲインを乗ずることで前記指令信号を減少させる停止制御演算手段を構成し、ブーム・アーム連続制御部7Bは、前記1つの油圧アクチュエータのストロークエンド近傍において、前記目標速度ベクトルに対する前記動き得る他の油圧アクチュエータの補正動作信号を演算するとともに、前記1つの油圧アクチュエータのストロークエンドまでの距離により、通常は0でストロークエンドで1となるゲインを乗ずることで前記補正動作信号を増加させる連続制御演算手段を構成し、和の演算部7q,7rは、前記停止制御演算手段からの信号と前記連続制御演算手段からの信号とを合成することにより前記指令信号を補正する信号合成手段を構成する。
【0055】
ここで、以上述べた本実施形態の動作を図9を用いて簡単に説明する。動作例として、教示速度ベクトルとしては垂直上方に一定で、ブームがストロークエンドに近づきアームによる補正動作が行われる場合を考える。
【0056】
まず、図9(a)において、ブーム角度θ1がストロークエンドより十分離れている場合には、通常の軌跡制御が行われ、教示速度ベクトルVXYがそのまま出力ベクトルVXY’となる。
【0057】
VXY’=VXY …(21)
次に図9(b)において、ブーム角度θ1がストロークエンドに近い場合には、教示速度ベクトルVXYにブーム停止制御ゲインh1iを乗じたものと(14)式のアーム補正ベクトルVs2にアーム連続制御ゲインh2cを乗じたものとの和で出力ベクトルVXY’が求められる。
【0058】
VXY’=h1i*VXY+h2c*Vs2 …(22)
次に、図9(c)において、ブーム角度θ1がストロークエンドにある場合には 、通常の軌跡制御は行われず、アーム補正ベクトルVs2がそのまま出力ベクトルVXY’となる。
【0059】
VXY’=Vs2 …(23)
以上のように構成した本実施形態では、1つのアクチュエータのストロークエンド近傍で操作レバー装置5の操作により更に同じアクチュエータのストロークエンドの方向に教示入力された場合、従来のように教示入力されたベクトル軌跡制御に関して減速・停止制御を行い、更に他の動き得るアクチュエータに対しては、教示入力された速度ベクトルVXYの動作方向成分により連続した動作を行うため、他のアクチュエータの動作制御への移行は連続的なものとなり、しかもこの他のアクチュエータの動作制御への移行は教示入力速度ベクトルVXY(目標速度ベクトル)に応じたものとなる。よって、油圧アクチュエータのストロークエンド近傍においても教示入力速度ベクトルVXYに応じた円滑な連続した動作が可能となり、ギクシャクした動きのない、オペレータの意志を反映した軌跡制御が行え、操作性を良くできる。
【0060】
本発明の第2の実施形態を図10及び図11により説明する。上記実施形態では、教示入力された速度ベクトルVXY(目標速度ベクトル)をそのまま座標変換部7aでブーム・アームの角速度θ1id,θ2idに変換し、指令信号としたが、本実施形態はフィードバック制御により位置補正をした値を指令信号とするものである。図中、図4に示す機能と同等の機能には同じ符号を付している。
【0061】
図10において、本実施形態の制御ユニット7(図1参照)は、図4に示す第1の実施形態の座標変換演算部7a、ブーム・アーム停止制御部7A、ブーム・アームの連続制御部7B、バケット制御部7C、流量制御弁の特性補正部7D,7E,7Fの各機能に加えて、フィードバック制御演算部7Zの機能を有している。このフィードバック制御演算部7Zは、教示速度ベクトルVXYの位置補正制御演算部7z1及び和の演算部7z2と教示角速度VZの位置補正制御演算部z3及び和の演算部7z4とで構成されている。
【0062】
位置補正制御演算部7z1においては、図11に示すように、その時の教示速度ベクトルVXYの方向に直線を伸ばすことで目標軌跡rを求めると共に、前回の目標軌跡rとアーム先端の現在点との距離dを求め、この距離dにゲインKFBを乗じることで、アーム先端の現在点より目標軌跡r上に下ろした垂線に一致する補正ベクトルKFB・dを求める。
【0063】
演算部7z2では、教示速度ベクトルVXYと補正ベクトルKFB・dとの和をとることで、
VXYC=VXY+KFB・d …(24)
により出力される補正教示速度ベクトルVXYCを求める。
【0064】
また、位置補正制御演算部7z3においては、図11に示すように、バケット1cの現在角度ZLを求めると共に、教示角速度Vzを積分してバケット1cの目標角度Zrを求め、この目標角度Zrと現在角度ZLとの差を、
ΔZ=Zr−ZL …(25)
により求め、この角度差ΔZにゲインKFBを乗じることで、補正角速度KFB・ΔZを求める。
【0065】
演算部7z4では、教示角速度VZと補正角速度KFB・ΔZとの和をとることで、
VZC=VZ+KFB・ΔZ …(26)
により出力される補正教示角速度VZCを求める。
【0066】
その後の計算は、第1の実施形態と同様である。
【0067】
以上のように構成した本実施形態によれば、フィードバック制御演算部7Zで実際の位置及び角度と目標とのずれを小さくするよう教示速度ベクトルVXY及び教示角速度VZを補正し、この補正した値を用いて減速・停止制御及び増速・連続制御を行うので、操作レバーの操作でアーム先端の目標軌跡及びバケットの目標角度を自由に設定しながら軌跡制御を行うものにおいて、ストロークエンド近傍でも円滑な連続した動作を可能として操作性を良くできる。
【0068】
【発明の効果】
本発明によれば、ベクトル軌跡制御による動作中、1つの油圧アクチュエータがストロークエンド近傍に達すると、目標速度ベクトルに応じて、動き得る他の油圧アクチュエータの動作制御へと制御が連続的に移行するため、油圧アクチュエータのストロークエンド近傍においても教示入力速度ベクトルに応じた円滑な連続した動作が可能となり、ギクシャクした動きのない、目標速度ベクトルの内容を反映した軌跡制御が行え、操作性を良くできる。
【図面の簡単な説明】
【図1】本発明の一実施形態による油圧ショベルの軌跡制御装置をその油圧回路とともに示す図である。
【図2】本発明が適用される油圧ショベルの外観を示す図である。
【図3】操作レバー装置の機能を説明する図である。
【図4】制御ユニットの機能を説明する図である。
【図5】ブーム・アーム停止制御部、ブーム・アームの連続制御部、バケット制御部の詳細を示す図である。
【図6】関節角度と停止制御ゲイン及び連続制御ゲインの関係を示す図である。
【図7】ブーム・アーム停止制御部及び連続制御部での処理を説明する図である。
【図8】ブーム・アーム停止制御部及び連続制御部での処理を説明する図である。
【図9】動作の一例を説明する図である。
【図10】本発明の他の実施形態による油圧ショベルの軌跡制御装置における制御ユニットの機能を説明する図である。
【図11】フィードバック制御演算部の処理を説明する図である。
【符号の説明】
1A フロント装置
1B 車体
1a ブーム
1b アーム
1c バケット
1d 上部旋回体
1e 下部走行体
2 油圧ポンプ
3a〜3c 油圧アクチュエータ
4a〜4c 流量制御弁
5 操作レバー装置
6a〜6c 角度検出器
7 制御ユニット
7a 座標変換演算部
7A ブーム・アーム停止制御部
7B ブーム・アームの連続制御部
7C バケット制御部
7D,7E,7F 流量制御弁の特性補正部
7q,7r 和の演算部
Claims (2)
- 上下方向に回動可能な複数のフロント部材により構成される多関節型のフロント装置と、このフロント装置を移動する複数の油圧アクチュエータと、これら複数の油圧アクチュエータに供給される圧油の流量を制御する複数の流量制御弁とを備え、前記複数のフロント部材が1つのブームと1つのアームを含み、前記複数のアクチュエータが前記ブームを駆動するブームシリンダと前記アームを駆動するアームシリンダを含む建設機械の軌跡制御装置において、
(a)前記アームの先端の目標速度ベクトルを指示する指示手段と、
(b)前記ブーム及びアームの姿勢を検出する姿勢検出手段と、
(c)前記姿勢検出手段で検出されたブーム及びアームの姿勢に基づき、前記目標速度ベクトルに応じた動作を前記アーム先端に行わせるベクトル軌跡制御のための指令信号を演算する第1の演算手段と、
(d)前記ベクトル軌跡制御のための指令信号により前記ブーム及びアームが動かされるとき、前記ブームシリンダ及び前記アームシリンダのうちの一方である1つの油圧アクチュエータのストロークエンド近傍において、前記目標速度ベクトルに応じて前記ブームシリンダ及びアームシリンダによるベクトル軌跡制御から動き得る他の1つの油圧アクチュエータの単独動作制御へと連続して移行するように、前記指令信号を補正する第2の演算手段とを備え、前記第2演算手段は、
前記1つの油圧アクチュエータのストロークエンドまでの距離に応じて、ストロークエンドとストロークエンド近傍を除いた範囲では1で、ストロークエンドで0となり、ストロークエンド近傍ではストロークエンドに近づくにしたがって1から0に漸次小さくなるゲインを計算し、このゲインを前記1つの油圧アクチュエータに係わる指令信号と前記他の油圧アクチュエータに係わる指令信号とに乗ずることで前記指令信号を減少させる停止制御演算手段と、
前記1つの油圧アクチュエータのストロークエンド近傍において、前記目標速度ベクトルに対する前記動き得る他の油圧アクチュエータの補正動作信号を演算するとともに、前記1つの油圧アクチュエータのストロークエンドまでの距離に応じて、ストロークエンドとストロークエンド近傍を除いた範囲では0でストロークエンドで1となり、ストロークエンド近傍ではストロークエンドに近づくにしたがって0から1に漸次大きくなるゲインを計算し、このゲインを前記他の油圧アクチュエータの補正動作信号に乗ずることで前記補正動作信号を増加させる連続制御演算手段と、
前記停止制御演算手段からの信号と前記連続制御演算手段からの信号とを合成することにより前記1つの油圧アクチュエータに係わる指令信号と前記他の油圧アクチュエータに係わる指令信号を補正する信号合成手段とを有することを特徴とする建設機械の軌跡制御装置。 - 請求項1記載の建設機械の軌跡制御装置において、前記連続制御演算手段は、前記1つの油圧アクチュエータのストロークエンド近傍において、前記目標速度ベクトルのうち、前記動き得る他の油圧アクチュエータにより駆動されるフロント部材の動作方向への投影成分を求め、この目標速度ベクトルの投影成分に相当する速度が得られるよう前期補正動作信号を求めることを特徴とする建設機械の軌跡制御装置。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP27493096A JP3749319B2 (ja) | 1996-10-17 | 1996-10-17 | 建設機械の軌跡制御装置 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP27493096A JP3749319B2 (ja) | 1996-10-17 | 1996-10-17 | 建設機械の軌跡制御装置 |
Publications (2)
Publication Number | Publication Date |
---|---|
JPH10121507A JPH10121507A (ja) | 1998-05-12 |
JP3749319B2 true JP3749319B2 (ja) | 2006-02-22 |
Family
ID=17548531
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP27493096A Expired - Fee Related JP3749319B2 (ja) | 1996-10-17 | 1996-10-17 | 建設機械の軌跡制御装置 |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP3749319B2 (ja) |
Families Citing this family (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP6692568B2 (ja) * | 2015-01-06 | 2020-05-13 | 住友重機械工業株式会社 | 建設機械 |
CN110409528B (zh) * | 2019-07-25 | 2022-03-08 | 中国航空工业集团公司西安飞行自动控制研究所 | 一种挖掘机铲斗轨迹自动控制装置、方法和计算机可读存储介质 |
CN114109246B (zh) * | 2021-11-29 | 2024-02-02 | 江苏徐工工程机械研究院有限公司 | 岩土工程机械及其工作臂控制方法 |
-
1996
- 1996-10-17 JP JP27493096A patent/JP3749319B2/ja not_active Expired - Fee Related
Also Published As
Publication number | Publication date |
---|---|
JPH10121507A (ja) | 1998-05-12 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
US5799419A (en) | Method for controlling the operation of power excavator | |
JP2736569B2 (ja) | 油圧パワーショベルの操作方法 | |
JP4776640B2 (ja) | 油圧ショベルのフロント制御装置 | |
JP4215944B2 (ja) | 油圧ショベルのフロント制御装置 | |
JP3198249B2 (ja) | 建設機械の干渉防止装置 | |
JP3749319B2 (ja) | 建設機械の軌跡制御装置 | |
JP3822646B2 (ja) | 3関節型掘削機の操作制御装置 | |
JP3437348B2 (ja) | 建設機械の軌跡制御装置 | |
JP2000303492A (ja) | 建設機械のフロント制御装置 | |
JP3705661B2 (ja) | 建設機械の作業機の姿勢制御装置 | |
JPH06312392A (ja) | 多関節ロボットの制御装置 | |
JP3682352B2 (ja) | 建設機械のフロント制御装置 | |
JPH10121508A (ja) | 建設機械の軌跡制御装置 | |
KR100547203B1 (ko) | 건설기계의 자동 평탄작업장치 및 방법 | |
JP3749308B2 (ja) | 建設作業用マニュアルマニピュレータの操作制御機構 | |
JPH0823155B2 (ja) | 作業機の制御装置 | |
JP2574671B2 (ja) | パワ−シヨベルにおける作業機の制御装置 | |
JP3266714B2 (ja) | マニピュレータの姿勢角度制御装置 | |
JPH10292420A (ja) | 油圧ショベルの掘削軌跡制御装置 | |
JP2601865B2 (ja) | 作業機の軌跡制御装置 | |
JPH0720919A (ja) | マニプレータの制御装置 | |
JPH0336970B2 (ja) | ||
JPH04119203A (ja) | アクチュエータの駆動制御装置 | |
JP3679850B2 (ja) | 建設機械の干渉防止装置 | |
JPH0791844B2 (ja) | 作業機の制御装置 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20041228 |
|
A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20050208 |
|
A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20050411 |
|
A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20050906 |
|
A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20051101 |
|
TRDD | Decision of grant or rejection written | ||
A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20051129 |
|
A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20051201 |
|
R150 | Certificate of patent or registration of utility model |
Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20091209 Year of fee payment: 4 |
|
LAPS | Cancellation because of no payment of annual fees |