JP3802697B2 - Attitude angle control device for work equipment - Google Patents

Description

【００１３】
バケット姿勢角を一定に保つためには、（１）式の両辺を時間微分して次式（４）で求める補正前のバケット回動速度Ｔv４に制御する。
【数４】
δv＝αv−Ｔv２−Ｔv３−Ｔv４＝０
∴Ｔv４＝αv−Ｔv２−Ｔv３ （４）
ただし：δvはバケット４の回動速度
【００１４】
５０１、５０２は加算点であり（４）式のバケットの回動速度Ｔv４を算出する。姿勢角偏差Δδおよびバケット回動速度Ｔv４は後述するバケット回動速度補正回路６００へ入力され、バケット回動速度Ｔv４は、姿勢角偏差Δδに基づいて補正されて回動速度Ｔv４kとして出力される。７０１は関数発生器であり、バケット角Ｔ４が入力されるとこのバケット角Ｔ４をリンク補正するための係数に変換する。７０２は乗算器であり、変換された係数と補正後のバケット回動速度Ｔv４k とを乗算して、バケットシリンダ速度を算出する。７０３は係数器であり、この７０３はシリンダ速度にバケットシリンダ８の受圧面積を乗算してバケットシリンダ流量制御値Ｑ４を次式（５）を用いて演算する。なお、シリンダ面積ａ４は実際にはロッド側とボア側では異なるので、伸縮に基づいて適宣切り換えて用いる。
【数５】
Ｑ４ ＝ａ4・ｆ（Ｔ４）・Ｔv４k （５）
【００１５】
図２は、第一の実施の形態によるバケット回動速度補正回路６００の詳細を示す図である。関数発生器６５０は、姿勢角偏差Δδに基づいた作業アタッチメントの回動速度Ｔv４dを出力する。この関数発生器６５０は、姿勢角偏差Δδが正の方向に大きくなるときは正の回動速度Ｔv４dを出力し、姿勢角偏差Δδが負の方向に大きくなるときは負の回動速度Ｔv４dを出力するフィードバック制御系を構成する。なお、この関数発生器６５０はその入出力特性に不感帯を持たない。また、関数発生器６５０の最大出力値は無制限ではなく、回路の構成により所定の値に制限される。加算点６５１は、上述した加算点５０１および５０２で算出されたバケットの回動速度Ｔv４と上記関数発生器６５０から出力される作業アタッチメントの姿勢角偏差Δδに基づいた回動速度Ｔv４dとを加算する。加算点６５１の出力は、制限回路６６０に入力される。制限回路６６０は、負リミッタ回路６６１と正リミッタ回路６６２および切換器６６３とで構成され、切換器６６３により正リミッタ回路６６１および負リミッタ回路６６２のいずれか一方で制限された信号が出力される。切換器６６３は符号器６０４からの出力により接点ａ、ｂが切換えられる。
【００１６】
符号器６０４は、上述した加算点５０１および５０２で算出された補正前のバケット回動速度Ｔv４の正負符号を判定し、判定した符号により切換器６６３の接点を切換える。すなわち、バケット回動速度Ｔv４の符号が正のとき切換器６６３の接点をａ側に切換え、負のときｂ側に切換える。負リミッタ回路６６１は入力信号が負のときは０を出力し、入力信号が０または正のときは入力信号に基づいた信号を出力する。正リミッタ回路６６２は入力信号が正のときは０を出力し、入力信号が０または負のときは入力信号に基づいた信号を出力する。
【００１７】
このような図２の回動速度補正回路６００は、符号器６０４の出力が正のとき、切換器６６３で負リミッタ回路６６１側に切換えられるので、加算点６５１から出力される値が０以上である場合のみ信号を出力する。反対に、符号器６０４の出力が負のとき、切換器６６３で正リミッタ回路６６２側に切換えられるので、加算点６５１から出力される値が０以下である場合のみ信号を出力する。また、Ｔv４が０のときは、Ｔv４が０となる以前に出力していた符号に基づいた信号を出力する（次式（６））。
【数６】

【００１８】
上述した第一の実施の形態による作業機の姿勢角制御についてさらに詳細に説明する。図示しない電源スイッチが投入されると、角度検出器１０１〜１０４で検出された角度α，Ｔ２，Ｔ３，Ｔ４に基づいて加算点２０１〜２０３でバケットの姿勢角δが演算される。アームの制御が開始されると、その開始時のバケット姿勢角δ０が記憶器３０１に保持され、以後、姿勢角偏差Δδが加算点３０２で演算される。一方、アーム回動速度演算回路４００で演算された各アームの回動速度αv ，Ｔv２ ，Ｔv３ に基づいて加算点５０１，５０２はバケット姿勢角を一定に保つために補正前のバケットの回動速度Ｔv４を演算する。バケット回動速度補正回路６００は、姿勢角偏差Δδに基づいてフィードバック量として関数発生器６５０から出力される作業アタッチメントの回動速度Ｔv４dとバケット回動速度Ｔv４とを加算点６５１で加算し、この加算結果Ｔv４d＋Ｔv４を正および負のリミッタ回路６６１、６６２に入力する。一方、符号器６０４は、入力されたバケット回動速度Ｔv４の符号を判定し、判定した符号に基づいて切換器６６３を切換える。この結果、加算点６５１から出力される加算値に対して切換器６６３で選択された負リミッタ回路６６１あるいは正リミッタ回路６６２が出力を制限し、補正後のバケット回動速度Ｔv４k を上式（６）のように出力する。
【００１９】
ここで、姿勢角偏差Δδと補正前のバケット回動速度Ｔv４の符号の関係を整理すると次のようになる。
▲１▼Ｔv４＞０でΔδ＞０のときは、バケットシリンダ伸び方向で偏差修正も伸び方向である（バケットの回動が遅れている）。
▲２▼Ｔv４＜０でΔδ＜０のときは、バケットシリンダ縮み方向で偏差修正も縮み方向である（バケットの回動が遅れている）。
▲３▼Ｔv４＞０でΔδ＜０のときは、バケットシリンダ伸び方向で偏差修正は縮み方向である（バケットの回動が進んでいる）。
▲４▼Ｔv４＜０でΔδ＞０のときは、バケットシリンダ縮み方向で偏差修正は伸び方向である（バケットの回動が進んでいる）。
Ｔv４＞０である▲１▼と▲３▼では、（Ｔv４d＋Ｔv４）＞０のときフィードバック制御とフィードフォワード制御によって姿勢角偏差を減少させるが、（Ｔv４d＋Ｔv４）＜０のとき補正値を０とする。一方、Ｔv４＜０である▲２▼と▲４▼では、（Ｔv４d＋Ｔv４）＜０のときフィードバック制御とフィードフォワード制御によって姿勢角偏差を減少させるが、（Ｔv４d＋Ｔv４）＞０のとき補正値を０とする。すなわち、制限回路６６０は、加算点６５１で加算された回動速度が補正前のバケット回動速度Ｔv４と同方向の場合は加算された回動速度を出力し、逆方向の場合は加算された回動速度を出力しないように制限する。
【００２０】
図３（ａ）は、補正前のバケットの回動速度Ｔv４＞０における姿勢角偏差Δδと補正後のバケット回動速度Ｔv４kの関係を表すグラフである。図中の細い線Ｌ１は補正前のバケットの回動速度Ｔv４を示す線であり、本実施の形態ではフィードフォワード量である。図中の太い線Ｌ２は本実施の形態による補正後のバケット回動速度Ｔv４kである。なお、参考のために第１の従来技術による補正後の回動速度と第２の従来技術による補正後の回動速度をそれぞれ図３（ｄ）および（ｅ）に示した。図３（ｄ）におけるフィードフォワード量は本実施の形態と同じＴv４であり、補正後の回動速度Ｔv４kを波形Ｌｄで示す。また、図３（ｅ）におけるフィードフォワード量は、後述する第二の実施の形態と同じＴv４に姿勢角偏差Δδの大きさと方向に基づいた係数をかけたものであり、補正後の回動速度Ｔv４kを波形Ｌｅで示す。
【００２１】
本実施の形態による図３（ａ）の波形Ｌ２を従来技術による図３（ｄ）および（ｅ）の波形ＬｄおよびＬｅと比較すると、波形Ｌ２ではフィードバック制御で不感帯を設けていないから波形Ｌ１とＬ２が一致する区間がなくなり、偏差Δδの全域で姿勢角精度が改善されることがわかる。また、波形Ｌ２が姿勢角偏差Δδに基づいて一様に増減するので、偏差Δδが大きくなるとより大きな制御量をが得られることがわかる。さらに、波形Ｌ２では、補正後の回動速度Ｔv４kの符号が変化しないことがわかる。
【００２２】
したがって、以上説明したように第一の実施の形態によれば、以下のような作用効果が得られる。
（１）姿勢角偏差Δδに基づいたフィードバック制御に不感帯を設けないから姿勢角と目標値との偏差Δδが小さいときにも姿勢角精度を改善することができ、姿勢角偏差全域にわたり姿勢角精度を改善できる。
（２）補正後の回動速度の符号が反転したときは、フィードフォワード制御と逆方向の回動速度を出力をしないようにしたからハンチングを防止できる。
【００２３】
−第二の実施の形態−
図４は、第二の実施の形態によるバケット回動速度補正回路６００Ａの詳細を示す図である。図４における関数発生器６５０および制限回路６６０については第一の実施の形態と同じであり、その説明を省略する。相違点は、加算点６５１で加算されるＴv４に姿勢角偏差Δδの大きさと方向に基づいた係数を掛けるようにしたものである。
【００２４】
図４において、６０１は絶対値変換器、６０２は符号反転器であり、入力された姿勢角偏差Δδをそれぞれ｜Δδ｜および−｜Δδ｜に変換する。６０３と６０４は、それぞれ姿勢角偏差Δδと補正前のバケット回動速度Ｔv４の符号を判定する符号器、６０５は、符号器６０３および６０４の判定結果を比較する排他的論理和回路である。６０６は｜Δδ｜および−｜Δδ｜のいずれか一方を選択する切換器であり、符号器６０３および６０４で判定された符号が不一致のとき排他的論理和回路６０５からの切換信号で切換器６０６の接点がｂ側に切換わり、または、符号が一致したときは切換器６０６の接点がａ側に切換わり、それぞれ｜Δδ｜、−｜Δδ｜を関数発生器６０７へ入力する。
【００２５】
この関数発生器６０７は、入力が正のときは出力が減少し、入力が負のときは出力が増加する係数ｃを出力する。なお、入力が０のとき、すなわち、姿勢角と目標値との偏差Δδ＝０のときは係数ｃ＝１を出力する。また、関数発生器６０７の最大出力値は無制限ではなく、回路の構成により所定の値に制限される。関数発生器６０７の出力と補正前のバケット回動速度Ｔv４とが乗算器６０８へ入力され、上述した係数ｃと補正前のバケット回動速度Ｔv４が乗算されて次式（７）による信号が乗算器６０８から出力される。
【数７】
ＯＵＴ（６０８）＝ｃ・Ｔv４ （７）
【００２６】
したがって、上式（６）のＴv４に代えて式（７）を代入すれば、第二の実施の形態によるバケット回動速度補正回路６００Ａの出力は次式（８）のように表すことができる。
【数８】

【００２７】
上述した第二の実施の形態による作業機の姿勢角制御についてさらに詳細に説明する。図示しない電源スイッチが投入され、バケット姿勢角を一定に保つために補正前のバケットの回動速度Ｔv４を演算するまでは第一の実施の形態と同じである。図４によるバケット回動速度補正回路６００Ａは、入力された姿勢角偏差Δδと補正前のバケット回動速度Ｔv４の符号から、バケットの回動が目標値に対して進んでいるか、あるいは遅れているかを排他的論理和回路６０５で判定する。バケットの回動が目標値より遅れていると判定されたとき、切換器６０６は−｜Δδ｜を選択し、補正前のバケット回動速度Ｔv４に対して１より大きな係数ｃをかけ、バケットの回動が目標値より進んでいると判定されたとき、切換器６０６は｜Δδ｜を選択し、補正前のバケット回動速度Ｔv４に対して１より小さな係数ｃをかけて上式（７）の出力値が乗算器６０８から出力される。すなわち、Ｔv４とΔδの符号が一致する上述した▲１▼と▲２▼の場合にはバケットの回動が遅れているので、切換器６０６をａ接点とすることにより、補正前のバケット回動速度Ｔv４を増加させる方向にフィードフォワード量を増加させて姿勢角偏差を減少させる。Ｔv４とΔδの符号が不一致である▲３▼と▲４▼の場合にはバケットの回動が進んでいるので、切換器６０６をｂ接点とすることにより、補正前のバケット回動速度Ｔv４を減少させる方向にフィードフォワード量を減少させて姿勢角偏差を減少させる。そして、姿勢角偏差Δδに基づいてフィードバック量として関数発生器６５０から出力される作業アタッチメントの回動速度Ｔv４dと、乗算器６０８の出力ＯＵＴ（６０８）との加算値を加算点６５１から制限回路６６０に入力する。制限回路６６０は、入力された加算値の出力を制限し、補正後のバケット回動速度Ｔv４kを上式（８）のように出力する。
【００２８】
図３（ｂ）は、補正前のバケットの回動速度Ｔv４＞０における姿勢角偏差Δδと補正後のバケット回動速度Ｔv４kの関係を表すグラフである。図中の細い線Ｌ１１は、乗算器６０８の出力であるフィードフォワード量である。すなわち、姿勢角偏差Δδの大きさと符号に基づいて乗算器６０８から出力されるｃ・Ｔv４を示す。太い線Ｌ１２は、本実施の形態による補正後のバケット回動速度Ｔv４kである。図３（ｄ）の第１の従来技術による補正後の回動速度Ｔv４kを示す波形Ｌｄおよび図３（ｅ）の第２の従来技術による補正後の回動速度Ｔv４kを示す波形Ｌｅと比較すると、図３（ｂ）に示す補正後の回動速度Ｔv４kの波形Ｌ１２は姿勢角偏差Δδに対する傾きが大きいので、偏差Δδの変化に対してより大きな制御量を得られることがわかる。
【００２９】
以上説明したように第二の実施の形態によれば、第一の実施の形態における作用効果に加えて、以下のような作用効果を得ることができる。
（１）姿勢角偏差Δδに基づいてフィードフォワード量を変化させて回動速度を補正するようにしたから、姿勢角偏差Δδが生じたときの姿勢角制御の追従性がよくなり、姿勢角精度が向上する。
【００３０】
−第三の実施の形態−
図５は、第三の実施の形態によるバケット回動速度補正回路６００Ｂの詳細を示す図である。第二の実施の形態との相違点は、不感帯付加回路６７０を追加した点であるから、主に不感帯付加回路６７０について説明し、その他の説明は省略する。図５において、不感帯付加回路６７０は、関数発生器６７１および６７２と、切換器６７３と、加算点６７４とから構成される。負の関数発生器６７１は、入力される姿勢角偏差Δδの値が所定値−ｐ以下のときは入力の大きさに基づいた負の値Ｔv４aを出力し、入力される偏差Δδが増加して０に近づくと、入力値Δδが所定の値−ｐより大きい領域では出力Ｔv４aを０に制限する。同様に正の関数発生器６７２は、入力される偏差Δδが所定値＋ｐ以上のときは入力の大きさに基づいた正の値Ｔv４aを出力し、入力される偏差Δδが減少して０に近づくと、入力値Δδが所定の値＋ｐより小さい領域では出力Ｔv４aを０に制限する。なお、関数発生器６７１および６７２の最大出力値は無制限ではなく、回路の構成により所定の値に制限される。
【００３１】
切換器６７３は、第一の実施の形態における切換器６６３と同様に切換わる。すなわち、バケット回動速度Ｔv４の符号が正のときは切換器６７３の接点はａ側に切換えられ、負のときはｂ側に切換えられる。加算点６７４は、制限回路６６０から出力される信号と切換器６７３で選択される信号とを加算する。
【００３２】
したがって、第二の実施の形態によるバケット回動速度補正回路６００Ｂの出力に不感帯付加回路６７０を追加することにより第三の実施の形態によるバケット回動速度補正回路６００Ｃが構成され、この回路の出力は次式（９）のように表すことができる。
【数９】

【００３３】
上述した第三の実施の形態による作業機の姿勢角制御についてさらに詳細に説明する。図示しない電源スイッチが投入され、制限回路６６０からの出力を得るまでは第二の実施の形態と同じである。第二の実施の形態による制御回路では、上述したようにＴv４とΔδの符号が不一致である▲３▼と▲４▼の場合にはバケットの回動が進んでいるので、補正前のバケット回動速度Ｔv４を減少させるようにフィードフォワード量を減少させて姿勢角偏差を減少させるようにした。これに対して、本実施の形態による制御回路６００Ｂでは、前述の▲３▼と▲４▼の場合、補正前のバケット回動速度Ｔv４と逆方向の姿勢角偏差Δδに基づいた回動速度Ｔv４aを不感帯付加回路６７０において加算するようにした。さらに、補正後の回動速度Ｔv４kの符号が反転する付近（ -ｐ＜Δδ＜ｐ）に不感帯を設け、姿勢角偏差Δδに基づいた作業アタッチメントの回動速度Ｔv４aを加算点６７４で加算するようにしたので、補正後のバケット回動速度Ｔv４kが上式（９）のように出力される。
【００３４】
図３（ｃ）は、補正前のバケットの回動速度Ｔv４＞０における姿勢角偏差Δδと補正後のバケット回動速度Ｔv４kの関係を表すグラフである。図中の細い線Ｌ１１は、乗算器６０８の出力であるフィードフォワード量である。すなわち、姿勢角偏差Δδの大きさと符号に基づいて乗算器６０８から出力されるｃ・Ｔv４を示す。太い線Ｌ２２は、本実施の形態による補正後のバケット回動速度Ｔv４kである。第二の実施の形態による制御量を示す図３（ｂ）の波形Ｌ１２と比較すると、図３（ｃ）の波形Ｌ２２では、姿勢角偏差Δδが所定値−ｐより小さいときは補正前の回動速度Ｔv４と逆方向の回動速度Ｔv４kが出力されることがわかる。さらに、従来技術による補正後の回動速度を示す図３（ｄ）の波形Ｌｄおよび図３（ｅ）の波形Ｌｅと比較すると、波形Ｌ２２には制御量Ｔv４kの符号が反転する領域において、Ｔv４kが所定の区間変化しないように不感帯が得られていることがわかる。
【００３５】
以上説明したように第三の実施の形態によれば、第一および第二の実施の形態における作用効果に加えて、以下のような作用効果を得ることができる。
（１）バケットの回動が目標値に対して進んだときは、フィードフォワード量と逆方向の姿勢角偏差Δδに基づいた制御を行うようにしたから、姿勢角制御の追従性がよくなるとともに姿勢角精度が向上する。
（２）補正後の回動速度の符号が反転する領域で、姿勢角偏差Δδが所定値−ｐ以内では不感帯を設けたからハンチングを防止することができる。
【００３６】
なお、第三の実施の形態の説明では第１の回動速度Ｔv４を補正する速度補正手段６０１〜６０８を含めて説明したが、これらは含めなくてもよい。
【００３７】
特許請求の範囲における各構成要素と、発明の実施の形態における各構成要素との対応について説明すると、角度検出器１０１〜１０４と加算点２０１〜２０３が姿勢角検出手段に、加算点３０２が偏差演算手段に、補正前のバケット回動速度Ｔv４が第１の回転速度に、アーム回動速度演算回路４００と加算点５０１および５０２が第１の回転速度演算手段に、姿勢角偏差Δδに基づいた作業アタッチメントの回動速度Ｔv４dが第２の回転速度に、関数発生器６５０が第２の回転速度演算手段に、加算点６５１が第１の加算手段に、制限回路６６０が制限手段に、作業アタッチメント回動速度補正回路６０１〜６０８が速度補正手段に、補正前のバケット回動速度Ｔv４と逆方向の姿勢角偏差Δδに基づいた回動速度Ｔv４aが第３の回転速度に、関数発生器６７１〜６７２および切換器６７３が第３の回転速度演算手段に、加算点６７４が第２の加算手段にそれぞれ対応する。
【００３８】
なお、本発明を適用するにあたっては以上の実施例の各構成要素を次のようにしてもよい。
イ）アームは３本に限定されない。
ロ）アーム回動速度演算回路４００を軌跡制御装置の一部としたが、他の制御装置、たとえば、手動制御装置としても良い。すなわち、手動操作レバーにより第１アーム１〜第３アーム３の速度指令値を出力するものでもよい。
ハ）アーム回動速度を演算値としたが、角度検出器１０１〜１０３からの出力を微分して用いても良い。
ニ）関数発生器６０７の偏差０に対する係数ｃを１としたが、他の数値をとることもできる。
ホ）係数ｃを関数発生器６０７により求めたが、演算器その他の手段によって演算して求めても良い。
ヘ）バケットを油圧シリンダで駆動したが、油圧に限定されず、また油圧モ−タ，油圧ロ−タリアクチュエータなどその他のアクチュエータを用いることができる。
ト）回転掘削バケットに適用するだけでなく、その他の各種作業アタッチメントにも使用できる。
チ）第１アーム１の角度を上部旋回体に対する相対角で検出し、作業機本体の傾斜角を検出して相対角を補正しても良い。
リ）アームを旋回体に置き換えることにより、旋回体の旋回角に応じて作業アタッチメントの方向を制御するような用途にも適用できる。
【００３９】
【発明の効果】
以上詳細に説明したように本発明によれば、次のような効果を奏する。
（１）請求項１の発明では、作業アタッチメントの第１の回転速度と作業アタッチメントの姿勢角偏差に基づいた第２の回転速度とを加算し、加算した回転速度が第１の回転速度と逆方向の場合は加算した回転速度を出力しないように制限したから、加算した回転速度を第１の回転速度の方向に出力したり逆方向に出力したりすることがなくなる。この結果、姿勢角制御のための制御量の符号が反転する姿勢角偏差付近のハンチングを防止する効果がある。
（２）請求項２の発明では、請求項１の構成に加えて、作業アタッチメントの姿勢角が目標値に対して遅れているときは第１の回転速度を姿勢角偏差に基づいてより大きくし、進んでいるときは姿勢角偏差に基づいてより小さくするように補正したから、請求項１と同様の効果を奏するとともに、より速く作業アタッチメントの姿勢角を目標値に近づけることができる。
（３）請求項３の発明では、請求項１および２の構成に加えて、作業アタッチメントの姿勢角が目標値に対して進み、姿勢角偏差が所定値を越える場合には、第１の回転速度と逆方向の姿勢角偏差に基づいた第３の回転速度を制限手段の出力に加算したから、請求項１および２と同様の効果を奏するとともに、所定値を越えて進んだ作業アタッチメントの姿勢角を、より速く目標値に近づけることができ、さらに、姿勢角偏差付近でのハンチングを防止する効果がある。
【図面の簡単な説明】
【図１】第一の実施の形態による作業アタッチメントの姿勢角制御装置を示す図である。
【図２】第一の実施の形態によるバケット回動速度補正回路６００の詳細図である。
【図３】補正前のバケットの回動速度Ｔv４＞０の場合の姿勢角偏差Δδと補正後のバケット回動速度Ｔv４kの関係を示すグラフである。
【図４】第二の実施の形態によるバケット回動速度補正回路６００Ａの詳細図である。
【図５】第三の実施の形態によるバケット回動速度補正回路６００Ｂの詳細図である。
【図６】多関節基礎用作業機の一例を示す図である。
【図７】作業機における各アームの対地角度および相対角度の定義を示す図である。
【符号の説明】
１〜３…アーム、４…回転掘削バケット、５〜８油圧シリンダ、９…電気油圧変換弁、１０１〜１０４…角度検出器、２０１〜２０３…加算点、３０１…記憶器、３０２…加算点、４００…アーム回動速度演算回路、５０１、５０２…加算点、６００、６００Ａ、６００Ｂ…作業アタッチメント回動速度補正回路、６０１…絶対値変換器、６０２…符号反転器、６０３、６０４…符号器、６０５…排他的論理和回路、６０６…切換器、６０７…関数発生器、６０８…乗算器、６５０…関数発生器、６５１…加算点、６６０…制限回路、６６１…負リミッタ回路、６６２…正リミッタ回路、６６３…切換器、６７０…不感帯付加回路、６７１、６７２…関数発生器、６７３…切換器、６７４…加算点、７０１…関数発生器、６０８…乗算器、７０１…関数発生器、７０２…乗算器、７０３…係数器[0001]
BACKGROUND OF THE INVENTION
The present invention relates to a control device that controls a posture angle of a work attachment that is rotatably mounted on a work machine.
[0002]
[Prior art]
2. Description of the Related Art Conventionally, a foundation work machine (hereinafter referred to as a work machine) in which a rotary excavation bucket is attached to the tip of an articulated arm is known. This working machine has a first arm, a second arm, and a third arm, each of which is driven by first to third hydraulic cylinders, on an upper swing body that is turnably provided on the lower traveling body. The rotary excavation bucket is attached to the tip of the third arm. The posture angle of the rotary excavation bucket is controlled by the fourth cylinder in various angular directions.
[0003]
In excavation with such a working machine, it is necessary to keep the attitude angle of the rotary excavation bucket constant. Especially for rotary excavation buckets, if the attitude angle error is large, the excavated hole will be broken, pipes and other buried objects will be damaged, and excavation resistance will increase and the rotation of the bucket will stop. Control is required. By the way, in a hydraulic power shovel having an excavator body, a boom, an arm, and a bucket attached to the tip of the arm, various control devices are known that keep the attitude angle of the bucket constant. For example, in Japanese Examined Patent Publication No. 61-45025 (first prior art), in order to maintain the attitude of the bucket, a value obtained by reversing the sign of the sum of the boom rotation speed and the arm rotation speed is used. The speed is obtained and the flow rate control of the bucket cylinder is performed. In Japanese Patent No. 2509368 (second prior art), the rotation speed of the work attachment is obtained to maintain the bucket posture, the deviation of the detected posture angle from the target value, and the obtained rotation. The rotation speed is corrected based on the direction of the speed, and the rotation speed of the work attachment is controlled by the corrected rotation speed.
[0004]
[Problems to be solved by the invention]
However, the first prior art has a drawback that if there is an error in the actual flow rate output value with respect to the flow rate control value of the bucket cylinder, an error occurs in the attitude angle and it is accumulated. Furthermore, when the boom and arm rotation speeds for obtaining the bucket rotation speed are very low, the differential value of the angle sensor output representing the actual rotation speed cannot be used. Since the obtained flow rate control value is used, an error in the actual flow rate output value with respect to the boom and arm cylinder flow rate control values is also added to the attitude angle error.
[0005]
In the second prior art, since a dead zone is provided in the vicinity of the control target value in the feedback characteristic of the deviation with respect to the target value of the posture angle, there is a problem that improvement in posture angle accuracy is limited to a case where the deviation is large. there were. Furthermore, if the feedforward amount corrected based on the posture angle deviation and the posture angle deviation feedback amount are added, the control direction of the posture angle is not always constant, so hunting is performed near the posture angle deviation where the sign of the control amount is reversed. There was a possibility of waking up.
[0006]
An object of the present invention is to provide a posture angle control device for a work machine that can perform control based on posture angle deviation even when the deviation of the posture angle from a target value is small. Another object of the present invention is to provide a posture angle control device for a work machine that suppresses hunting in the vicinity of a posture angle deviation where the sign of the control amount of posture angle control is reversed.
[0007]
[Means for Solving the Problems]
The present invention will be described with reference to FIGS. 2, 4 and 5 showing an embodiment.
(1) According to the first aspect of the present invention, posture angle detection means 101 to 104, 201 for detecting the posture angle of a work attachment that is mounted on a drive body rotatably attached to a base and whose posture angle is controlled by an actuator. ˜203, deviation calculation means 302 for calculating a deviation Δδ between the posture angle of the work attachment based on the values detected by the posture angle detection means 101 to 104, 201 to 203 and its target value, and the rotation of the driving body The first rotation speed calculation means 400, 501 to 502 for calculating and outputting the first rotation speed Tv4 of the work attachment necessary for maintaining the posture angle at the target value, and the work attachment based on the posture angle deviation Δδ Second rotation speed calculation means 650 for calculating and outputting the second rotation speed Tv4d, and addition means 6 for adding the calculated first and second rotation speeds Tv4 and Tv4d. 51 is applied to a posture angle control device for a work attachment. A limiting means 660 is provided for limiting the output so as not to output the added rotation speed when the added rotation speed is in the same direction as the first rotation speed Tv4, and to output the added rotation speed when the added rotation speed is in the reverse direction. And the above-mentioned object is achieved by driving the actuator with the output of the limiting means 660.
(2) According to the invention of claim 2, posture angle detection means 101 to 104, 201 for detecting the posture angle of a work attachment which is mounted on a drive body rotatably attached to a base and whose posture angle is controlled by an actuator. ˜203, deviation calculation means 302 for calculating a deviation Δδ between the posture angle of the work attachment based on the values detected by the posture angle detection means 101 to 104, 201 to 203 and its target value, and the rotation of the driving body The first rotation speed calculation means 400, 501 to 502 for calculating and outputting the first rotation speed Tv4 of the work attachment necessary for maintaining the posture angle at the target value, and the work attachment based on the posture angle deviation Δδ Second rotation speed calculation means 650 for calculating and outputting the second rotation speed Tv4d, and addition means 6 for adding the calculated first and second rotation speeds Tv4 and Tv4d. 51 is applied to a posture angle control device for a work attachment. When the posture angle of the work attachment is delayed with respect to the target value, the first rotation speed Tv4 is further increased based on the posture angle deviation Δδ, and the posture angle of the work attachment advances with respect to the target value. When there is, the speed correcting means 601 to 608 for correcting the first rotational speed Tv4 so as to reduce the first rotational speed Tv4 based on the attitude angle deviation Δδ, and the added rotational speed is the first rotational speed. In the same direction as Tv4, there is provided limiting means 660 for limiting the output so that the added rotational speed is not output, and in the reverse direction so as not to output the added rotational speed, and the actuator is driven by the output of this limiting means 660 This achieves the above-described object.
(3) According to the invention of claim 3, posture angle detection means 101 to 104, 201 for detecting the posture angle of a work attachment which is mounted on a drive body rotatably attached to a base and whose posture angle is controlled by an actuator. ˜203, deviation calculation means 302 for calculating a deviation Δδ between the posture angle of the work attachment based on the values detected by the posture angle detection means 101 to 104, 201 to 203 and its target value, and the rotation of the driving body The first rotation speed calculation means 400, 501 to 502 for calculating and outputting the first rotation speed Tv4 of the work attachment necessary for maintaining the posture angle at the target value, and the work attachment based on the posture angle deviation Δδ Second rotation speed calculation means 650 for calculating and outputting the second rotation speed Tv4d, and first addition for adding the calculated first and second rotation speeds Tv4 and Tv4d It is applied to a posture angle control device for a work attachment having means 651. And a limiting means 660 for limiting the output so as not to output the added rotation speed when the added rotation speed is in the same direction as the first rotation speed Tv4, and to output the added rotation speed when the added rotation speed is in the opposite direction. The posture angle of the work attachment is delayed from the target value 0 in case, The posture angle is advanced with respect to the target value, and the posture angle deviation Δδ is within a predetermined value. 0 in case, The posture angle is advanced with respect to the target value, and the posture angle deviation Δδ exceeds the predetermined value in case of Based on attitude angle deviation Δδ Was In the direction opposite to the first rotation speed Tv4 Calculated values for each Third rotation speed Tv4a As Third rotation speed calculation means 671 to 673 for outputting, and second addition means 674 for adding the third rotation speed Tv4a and the output of the limiting means 660 are provided. The output of the second addition means 674 is used as an actuator. The above-described object is achieved by driving the.
(4) According to a fourth aspect of the present invention, in the posture angle control device for a work implement according to the third aspect, when the posture angle of the work attachment is delayed with respect to the target value, the posture angle deviation Δδ is used. When the first rotation speed Tv4 is further increased and the posture angle of the work attachment is advanced with respect to the target value, the first rotation is performed so as to reduce the first rotation speed Tv4 based on the posture angle deviation Δδ. It further includes speed correction means 601 to 608 for correcting the speed Tv4.
[0008]
In the means for solving the above problems, the drawings are associated with the drawings of the embodiments for easy understanding, but the present invention is not limited to the embodiments.
[0009]
DETAILED DESCRIPTION OF THE INVENTION
Embodiments of the present invention will be described below with reference to the drawings.
-First embodiment-
FIG. 6 shows an example of a conventionally known working machine equipped with the attitude angle control device according to the present invention. This work machine is configured by turning an upper swing body US on the lower traveling body LT so as to be capable of swinging. The first arm 1, the second arm 2, the third arm 3 and the first arm for driving these are provided on the upper swing body US. A cylinder 5, a second cylinder 6 and a third cylinder 7 are mounted, and the rotary excavation bucket 4 is mounted on the tip of the third arm 3, and the posture angle of the fourth cylinder 8 is shown in FIGS. 6 (a), 6 (b), Control as in (c).
[0010]
FIG. 1 is a diagram showing a posture angle control device for a work attachment according to the first embodiment of the present invention. In the working machine shown in FIG. 6, the ground angle and relative angle of each arm are defined as shown in FIG. That is, in FIG. 7, the point O is the rotation fulcrum of the first arm 1, the ground angle of the first arm 1 is α, the relative angle of the second arm 2 to the first arm 1 is T2, and the third arm 3 is the third arm 3. When the relative angle with respect to the second arm 2 is T3 and the relative angle with respect to the third arm 3 of the bucket 4 is T4, the ground posture angle δ of the bucket 4 is expressed by the following equation (1).
[Expression 1]
δ = α-T2-T3-T4 (1)
[0011]
In FIG. 1, an angle detector 101 is mounted near the pivot point of the first arm 1 and detects the ground angle α of the first arm 1 by a known pendulum mechanism and potentiometer. The angle detectors 102, 103, and 104 are attached to the rotation fulcrums of the second arm 2, the third arm 3, and the bucket 4, respectively, and the first arm 1 and the second arm 2 by a known lever mechanism and potentiometer. Relative angle T2, relative angle T3 between second arm 2 and third arm 3, and relative angle T4 between third arm 3 and bucket 4 are detected. Reference numerals 201 to 203 denote addition points, respectively, and the posture angle δ of the bucket shown in the equation (1) is calculated at these addition points 201 to 203. Reference numeral 301 denotes a storage unit that holds a target bucket posture angle δ0, and in this embodiment, stores and holds the bucket posture angle at the start of control. Reference numeral 302 denotes an addition point, which calculates a deviation Δδ between the target bucket posture angle δ0 and the actual bucket posture angle δ as shown in the following equation (2).
[Expression 2]
Δδ = δ−δ0 (2)
[0012]
The arm rotation speed calculation circuit 400 is a part of a trajectory control device (not shown), and calculates the arm rotation speeds αv, Tv2, Tv3 calculated to control the trajectory of the bucket attachment point (PB in FIG. 6). Output. For example, the rotation speed of each arm when the first arm 1 is fixed and the trajectory is controlled by the second arm 2 and the third arm 3 can be calculated by the following equation (3).
[Equation 3]

[0013]
In order to keep the bucket posture angle constant, both sides of the equation (1) are time-differentiated to control the bucket rotation speed Tv4 before correction obtained by the following equation (4).
[Expression 4]
δv = αv−Tv2−Tv3−Tv4 = 0
∴Tv4 = αv−Tv2−Tv3 (4)
Where δv is the rotation speed of bucket 4
[0014]
Reference numerals 501 and 502 denote addition points, which calculate the bucket rotation speed Tv4 in equation (4). The attitude angle deviation Δδ and the bucket rotation speed Tv4 are input to a bucket rotation speed correction circuit 600 described later, and the bucket rotation speed Tv4 is corrected based on the attitude angle deviation Δδ and output as a rotation speed Tv4k. A function generator 701 receives a bucket angle T4 and converts the bucket angle T4 into a coefficient for link correction. A multiplier 702 multiplies the converted coefficient by the corrected bucket rotation speed Tv4k to calculate a bucket cylinder speed. Reference numeral 703 denotes a coefficient unit, which calculates the bucket cylinder flow rate control value Q4 using the following equation (5) by multiplying the cylinder speed by the pressure receiving area of the bucket cylinder 8. Since the cylinder area a4 is actually different between the rod side and the bore side, the cylinder area a4 is appropriately switched based on expansion and contraction.
[Equation 5]
Q4 = a4 · f (T4) · Tv4k (5)
[0015]
FIG. 2 is a diagram showing details of the bucket rotation speed correction circuit 600 according to the first embodiment. The function generator 650 outputs the rotation speed Tv4d of the work attachment based on the posture angle deviation Δδ. The function generator 650 outputs a positive rotation speed Tv4d when the posture angle deviation Δδ increases in the positive direction, and outputs a negative rotation speed Tv4d when the posture angle deviation Δδ increases in the negative direction. The output feedback control system is configured. This function generator 650 has no dead band in its input / output characteristics. Further, the maximum output value of the function generator 650 is not unlimited and is limited to a predetermined value depending on the circuit configuration. The addition point 651 adds the bucket rotation speed Tv4 calculated at the above-described addition points 501 and 502 and the rotation speed Tv4d based on the posture angle deviation Δδ of the work attachment output from the function generator 650. . The output of the addition point 651 is input to the limiting circuit 660. The limiting circuit 660 includes a negative limiter circuit 661, a positive limiter circuit 662, and a switching circuit. Change The switch 663 outputs a signal limited by one of the positive limiter circuit 661 and the negative limiter circuit 662. The switch 663 switches the contacts a and b by the output from the encoder 604.
[0016]
The encoder 604 determines the sign of the bucket rotation speed Tv4 before correction calculated at the addition points 501 and 502 described above, and switches the contact point of the switch 663 based on the determined code. That is, when the sign of the bucket rotation speed Tv4 is positive, the contact of the switch 663 is switched to the a side, and when the sign is negative, the contact is switched to the b side. The negative limiter circuit 661 outputs 0 when the input signal is negative, and outputs a signal based on the input signal when the input signal is 0 or positive. The positive limiter circuit 662 outputs 0 when the input signal is positive, and outputs a signal based on the input signal when the input signal is 0 or negative.
[0017]
2 is switched to the negative limiter circuit 661 side by the switch 663 when the output of the encoder 604 is positive, the value output from the addition point 651 is 0 or more. A signal is output only in some cases. On the contrary, when the output of the encoder 604 is negative, the switch 663 switches to the positive limiter circuit 662 side, so that a signal is output only when the value output from the addition point 651 is 0 or less. When Tv4 is 0, a signal based on the code output before Tv4 becomes 0 is output (the following equation (6)).
[Formula 6]

[0018]
The attitude angle control of the working machine according to the first embodiment described above will be described in more detail. When a power switch (not shown) is turned on, the bucket attitude angle δ is calculated at the addition points 201 to 203 based on the angles α, T2, T3, and T4 detected by the angle detectors 101 to 104. When the control of the arm is started, the bucket posture angle δ0 at the start is held in the storage device 301, and thereafter the posture angle deviation Δδ is calculated at the addition point 302. On the other hand, based on the arm rotation speeds αv, Tv2, and Tv3 calculated by the arm rotation speed calculation circuit 400, the addition points 501 and 502 are the rotation speeds of the buckets before correction in order to keep the bucket attitude angle constant. Tv4 is calculated. The bucket rotation speed correction circuit 600 adds the rotation speed Tv4d of the work attachment and the bucket rotation speed Tv4 output from the function generator 650 as a feedback amount based on the attitude angle deviation Δδ at the addition point 651. The addition result Tv4d + Tv4 is input to the positive and negative limiter circuits 661 and 662. On the other hand, the encoder 604 determines the sign of the input bucket rotation speed Tv4 and switches based on the determined sign. Change The device 663 is switched. As a result, the negative limiter circuit 661 or the positive limiter circuit 662 selected by the switch 663 limits the output with respect to the addition value output from the addition point 651, and the corrected bucket rotation speed Tv4k is expressed by the above equation (6 ) Is output.
[0019]
Here, the relationship between the sign of the attitude angle deviation Δδ and the bucket rotation speed Tv4 before correction is summarized as follows.
(1) When Tv4> 0 and Δδ> 0, deviation correction is also in the extension direction in the bucket cylinder extension direction (bucket rotation is delayed).
{Circle around (2)} When Tv4 <0 and Δδ <0, the deviation correction is also in the contraction direction in the bucket cylinder contraction direction (the rotation of the bucket is delayed).
(3) When Tv4> 0 and Δδ <0, the deviation correction is in the shrinking direction in the bucket cylinder extending direction (the bucket is rotating).
(4) When Tv4 <0 and Δδ> 0, the deviation correction is in the expansion direction in the bucket cylinder contraction direction (the rotation of the bucket is advanced).
In (1) and (3) where Tv4> 0, the attitude angle deviation is reduced by feedback control and feedforward control when (Tv4d + Tv4)> 0, but the correction value is set to 0 when (Tv4d + Tv4) <0. On the other hand, in (2) and (4) where Tv4 <0, the attitude angle deviation is reduced by feedback control and feedforward control when (Tv4d + Tv4) <0, but the correction value is 0 when (Tv4d + Tv4)> 0. To do. That is, the limiting circuit 660 outputs the added rotation speed when the rotation speed added at the addition point 651 is in the same direction as the bucket rotation speed Tv4 before correction, and is added in the reverse direction. Limit the rotation speed not to be output.
[0020]
FIG. 3A is a graph showing the relationship between the posture angle deviation Δδ and the corrected bucket rotation speed Tv4k at the bucket rotation speed Tv4> 0 before correction. A thin line L1 in the figure is a line indicating the rotation speed Tv4 of the bucket before correction, and is a feedforward amount in the present embodiment. A thick line L2 in the figure represents the bucket rotation speed Tv4k after correction according to the present embodiment. For reference, FIGS. 3D and 3E show the rotational speeds corrected by the first conventional technique and the rotational speeds corrected by the second conventional technique, respectively. The feedforward amount in FIG. 3D is the same Tv4 as in the present embodiment, and the corrected rotation speed Tv4k is indicated by a waveform Ld. Also, the feedforward amount in FIG. 3 (e) is obtained by multiplying the same Tv4 as in the second embodiment described later by a coefficient based on the magnitude and direction of the posture angle deviation Δδ, and the corrected rotation speed Tv4k is indicated by a waveform Le.
[0021]
Comparing the waveform L2 of FIG. 3A according to the present embodiment with the waveforms Ld and Le of FIG. 3D and FIG. 3E according to the prior art, the waveform L2 has no dead zone due to feedback control. It can be seen that there is no section in which L2 matches, and the attitude angle accuracy is improved over the entire range of the deviation Δδ. Further, since the waveform L2 is increased or decreased uniformly based on the posture angle deviation Δδ, it can be seen that a larger control amount can be obtained as the deviation Δδ increases. Furthermore, in the waveform L2, it can be seen that the sign of the corrected rotation speed Tv4k does not change.
[0022]
Therefore, as described above, according to the first embodiment, the following operational effects can be obtained.
(1) Since no dead zone is provided in the feedback control based on the posture angle deviation Δδ, the posture angle accuracy can be improved even when the deviation Δδ between the posture angle and the target value is small. Can be improved.
(2) When the sign of the rotational speed after correction is reversed, hunting can be prevented because the rotational speed in the reverse direction to the feedforward control is not output.
[0023]
-Second embodiment-
FIG. 4 is a diagram showing details of the bucket rotation speed correction circuit 600A according to the second embodiment. The function generator 650 and the limiting circuit 660 in FIG. 4 are the same as those in the first embodiment, and a description thereof will be omitted. The difference is that Tv4 added at the addition point 651 is multiplied by a coefficient based on the magnitude and direction of the posture angle deviation Δδ.
[0024]
In FIG. 4, reference numeral 601 denotes an absolute value converter, and 602 denotes a sign inverter, which converts the input attitude angle deviation Δδ into | Δδ | and − | Δδ |, respectively. Reference numerals 603 and 604 denote encoders that determine the signs of the attitude angle deviation Δδ and the bucket rotation speed Tv4 before correction, and reference numeral 605 denotes an exclusive OR circuit that compares the determination results of the encoders 603 and 604. Reference numeral 606 denotes a switch for selecting one of | Δδ | and − | Δδ |. When the codes determined by the encoders 603 and 604 do not match, the switch 606 is switched by a switching signal from the exclusive OR circuit 605. Is switched to the b side, or when the signs coincide, the contact of the switch 606 is switched to the a side, and | Δδ | and − | Δδ | are input to the function generator 607, respectively.
[0025]
The function generator 607 outputs a coefficient c that decreases when the input is positive and increases when the input is negative. When the input is 0, that is, when the deviation Δδ between the attitude angle and the target value is 0, the coefficient c = 1 is output. Further, the maximum output value of the function generator 607 is not unlimited and is limited to a predetermined value depending on the circuit configuration. The output of the function generator 607 and the bucket rotation speed Tv4 before correction are input to the multiplier 608, and the above-described coefficient c and the bucket rotation speed Tv4 before correction are multiplied to multiply the signal by the following equation (7). Output from the device 608.
[Expression 7]
OUT (608) = c · Tv4 (7)
[0026]
Therefore, if the equation (7) is substituted for Tv4 in the above equation (6), the output of the bucket rotation speed correction circuit 600A according to the second embodiment can be expressed as the following equation (8). .
[Equation 8]

[0027]
The attitude angle control of the working machine according to the second embodiment described above will be described in more detail. The operation is the same as that in the first embodiment until a power switch (not shown) is turned on and the bucket rotation speed Tv4 before correction is calculated in order to keep the bucket attitude angle constant. The bucket rotation speed correction circuit 600A shown in FIG. 4 determines whether the bucket rotation has advanced or delayed from the target value based on the input attitude angle deviation Δδ and the sign of the bucket rotation speed Tv4 before correction. Is determined by the exclusive OR circuit 605. When it is determined that the bucket rotation is delayed from the target value, the switch 606 selects-| Δδ |, multiplies the bucket rotation speed Tv4 before correction by a coefficient c greater than 1, and When it is determined that the rotation is ahead of the target value, the switcher 606 selects | Δδ |, and applies the coefficient c smaller than 1 to the bucket rotation speed Tv4 before correction, and the above equation (7) Are output from the multiplier 608. That is, in the case of (1) and (2) described above in which the signs of Tv4 and Δδ coincide with each other, the bucket rotation is delayed. The feedforward amount is increased in the direction in which the speed Tv4 is increased, and the attitude angle deviation is decreased. In the case of (3) and (4) where the signs of Tv4 and Δδ do not match, the rotation of the bucket is proceeding. The posture angle deviation is reduced by reducing the feedforward amount in the direction of reduction. Then, a limit value 660 is added from the addition point 651 to the limiting circuit 660 as an addition value of the rotation speed Tv4d of the work attachment output from the function generator 650 as a feedback amount based on the posture angle deviation Δδ To enter. The limit circuit 660 limits the output of the input addition value and outputs the corrected bucket rotation speed Tv4k as shown in the above equation (8).
[0028]
FIG. 3B is a graph showing the relationship between the posture angle deviation Δδ and the corrected bucket rotation speed Tv4k at the bucket rotation speed Tv4> 0 before correction. A thin line L11 in the figure is the feedforward amount that is the output of the multiplier 608. That is, c · Tv4 output from the multiplier 608 based on the magnitude and sign of the attitude angle deviation Δδ is shown. The thick line L12 is the bucket rotation speed Tv4k after correction according to the present embodiment. Compared with the waveform Ld showing the rotational speed Tv4k after correction according to the first prior art in FIG. 3 (d) and the waveform Le showing the rotational speed Tv4k after correction according to the second prior art in FIG. 3 (e). 3B shows that the waveform L12 of the corrected rotation speed Tv4k has a large inclination with respect to the attitude angle deviation Δδ, and therefore a larger control amount can be obtained with respect to the change in the deviation Δδ.
[0029]
As described above, according to the second embodiment, the following operational effects can be obtained in addition to the operational effects of the first embodiment.
(1) Since the rotation speed is corrected by changing the feedforward amount based on the attitude angle deviation Δδ, the attitude angle control is improved when the attitude angle deviation Δδ occurs, and the attitude angle accuracy is improved. Will improve.
[0030]
-Third embodiment-
FIG. 5 is a diagram showing details of the bucket rotation speed correction circuit 600B according to the third embodiment. The difference from the second embodiment is that a dead zone adding circuit 670 is added, and therefore the dead zone adding circuit 670 will be mainly described, and the other description will be omitted. In FIG. 5, the dead band adding circuit 670 includes function generators 671 and 672, a switch 673, and an addition point 674. The negative function generator 671 outputs a negative value Tv4a based on the magnitude of the input when the value of the input attitude angle deviation Δδ is equal to or less than a predetermined value −p, and the input deviation Δδ increases. When approaching 0, the output Tv4a is limited to 0 in the region where the input value Δδ is greater than the predetermined value −p. Similarly, the positive function generator 672 outputs a positive value Tv4a based on the magnitude of the input when the input deviation Δδ is equal to or greater than the predetermined value + p, and the input deviation Δδ decreases and approaches zero. In the region where the input value Δδ is smaller than the predetermined value + p, the output Tv4a is limited to zero. Note that the maximum output values of the function generators 671 and 672 are not unlimited and are limited to predetermined values depending on the circuit configuration.
[0031]
The switch 673 is the switch in the first embodiment. Change It is switched in the same manner as the device 663. That is, when the sign of the bucket rotation speed Tv4 is positive, the contact of the switch 673 is switched to the a side, and when it is negative, the contact is switched to the b side. The summing point 674 is connected to the signal output from the limiting circuit 660. Change The signal selected by the device 673 is added.
[0032]
Therefore, by adding the dead band adding circuit 670 to the output of the bucket rotation speed correction circuit 600B according to the second embodiment, a bucket rotation speed correction circuit 600C according to the third embodiment is configured, and the output of this circuit Can be expressed as the following equation (9).
[Equation 9]

[0033]
The attitude angle control of the work machine according to the third embodiment described above will be described in more detail. The process is the same as in the second embodiment until a power switch (not shown) is turned on and an output from the limiting circuit 660 is obtained. In the control circuit according to the second embodiment, as described above, in the cases of (3) and (4) where the signs of Tv4 and Δδ do not coincide with each other, the rotation of the bucket proceeds. The posture angle deviation is reduced by reducing the feedforward amount so as to reduce the dynamic speed Tv4. On the other hand, in the control circuit 600B according to the present embodiment, in the case of (3) and (4) described above, the rotation speed Tv4a based on the attitude angle deviation Δδ in the opposite direction to the bucket rotation speed Tv4 before correction. Are added in the dead zone adding circuit 670. Further, a dead zone is provided in the vicinity (−p <Δδ <p) where the sign of the corrected rotation speed Tv4k is reversed, and the rotation speed Tv4a of the work attachment based on the posture angle deviation Δδ is added at the addition point 674. Therefore, the corrected bucket rotation speed Tv4k is output as shown in the above equation (9).
[0034]
FIG. 3C is a graph showing the relationship between the posture angle deviation Δδ and the corrected bucket rotation speed Tv4k at the bucket rotation speed Tv4> 0 before correction. A thin line L11 in the figure is the feedforward amount that is the output of the multiplier 608. That is, c · Tv4 output from the multiplier 608 based on the magnitude and sign of the attitude angle deviation Δδ is shown. A thick line L22 is the bucket rotation speed Tv4k after correction according to the present embodiment. Compared to the waveform L12 of FIG. 3B showing the control amount according to the second embodiment, in the waveform L22 of FIG. 3C, when the posture angle deviation Δδ is smaller than the predetermined value −p, the rotation before correction is performed. It can be seen that a rotational speed Tv4k in the opposite direction to the moving speed Tv4 is output. Further, when compared with the waveform Ld in FIG. 3D and the waveform Le in FIG. 3E showing the rotation speed after correction according to the prior art, the waveform L22 has Tv4k in the region where the sign of the control amount Tv4k is inverted. It can be seen that the dead zone is obtained so that does not change for a predetermined interval.
[0035]
As described above, according to the third embodiment, the following operational effects can be obtained in addition to the operational effects of the first and second embodiments.
(1) When the rotation of the bucket advances with respect to the target value, the control based on the posture angle deviation Δδ in the direction opposite to the feedforward amount is performed. Angular accuracy is improved.
(2) In the region where the sign of the rotational speed after correction is reversed, hunting can be prevented because the dead zone is provided when the posture angle deviation Δδ is within a predetermined value −p.
[0036]
In the description of the third embodiment, the speed correction means 601 to 608 for correcting the first rotation speed Tv4 have been described. However, these may not be included.
[0037]
The correspondence between each component in the claims and each component in the embodiment of the invention will be described. Angle detectors 101 to 104 and addition points 201 to 203 are posture angle detection means, and addition point 302 is a deviation. Based on the attitude angle deviation Δδ, the bucket rotation speed Tv4 before correction is the first rotation speed, the arm rotation speed calculation circuit 400 and the addition points 501 and 502 are the first rotation speed calculation means. The rotation speed Tv4d of the work attachment is the second rotation speed, the function generator 650 is the second rotation speed calculation means, the addition point 651 is the first addition means, the limit circuit 660 is the limit means, and the work attachment. The rotation speed correction circuits 601 to 608 serve as speed correction means, and the rotation speed Tv4a based on the posture angle deviation Δδ in the opposite direction to the bucket rotation speed Tv4 before correction is the third rotation speed. Number generator 671-672 and Selector 673 corresponds to the third rotational speed calculation means, and the addition point 674 corresponds to the second addition means.
[0038]
In applying the present invention, each component of the above embodiment may be configured as follows.
B) The number of arms is not limited to three.
B) The arm rotation speed calculation circuit 400 is a part of the trajectory control device, but may be another control device such as a manual control device. That is, the speed command values of the first arm 1 to the third arm 3 may be output by a manual operation lever.
C) Although the arm rotation speed is the calculated value, the outputs from the angle detectors 101 to 103 may be differentiated and used.
D) Although the coefficient c for the deviation 0 of the function generator 607 is set to 1, other numerical values may be used.
E) Although the coefficient c is obtained by the function generator 607, it may be obtained by calculation by an arithmetic unit or other means.
F) The bucket is driven by a hydraulic cylinder, but is not limited to hydraulic pressure, and other actuators such as a hydraulic motor and a hydraulic rotary actuator can be used.
G) Not only can it be applied to rotating excavation buckets, it can also be used for other various work attachments.
H) The angle of the first arm 1 may be detected as a relative angle with respect to the upper swing body, and the inclination angle of the work implement main body may be detected to correct the relative angle.
B) By replacing the arm with a swivel body, the present invention can be applied to applications in which the direction of the work attachment is controlled according to the swivel angle of the swivel body.
[0039]
【The invention's effect】
As described above in detail, the present invention has the following effects.
(1) In the invention of claim 1, the first rotation speed of the work attachment and the second rotation speed based on the posture angle deviation of the work attachment are added, and the added rotation speed is opposite to the first rotation speed. In the case of the direction, since the added rotation speed is limited so as not to be output, the added rotation speed is not output in the direction of the first rotation speed or in the reverse direction. As a result, there is an effect of preventing hunting near the posture angle deviation in which the sign of the control amount for posture angle control is reversed.
(2) In the invention of claim 2, in addition to the configuration of claim 1, when the posture angle of the work attachment is delayed with respect to the target value, the first rotational speed is increased based on the posture angle deviation. Since the correction is made to make it smaller based on the posture angle deviation when traveling, the same effect as in claim 1 can be obtained and the posture angle of the work attachment can be brought closer to the target value faster.
(3) In the invention of claim 3, in addition to the configurations of claims 1 and 2, the posture angle of the work attachment advances with respect to the target value, and the posture angle deviation exceeds a predetermined value. in case of, The third rotation speed based on the posture angle deviation in the direction opposite to the first rotation speed is Add to limiter output Therefore, while having the same effect as the first and second aspects, The posture angle of the work attachment that has advanced beyond the specified value can be made faster. It can be brought close to the target value, and further has an effect of preventing hunting in the vicinity of the posture angle deviation.
[Brief description of the drawings]
FIG. 1 is a view showing a posture angle control device for a work attachment according to a first embodiment.
FIG. 2 is a detailed view of a bucket rotation speed correction circuit 600 according to the first embodiment.
FIG. 3 is a graph showing the relationship between the attitude angle deviation Δδ and the corrected bucket rotation speed Tv4k when the bucket rotation speed Tv4> 0 before correction;
FIG. 4 is a detailed diagram of a bucket rotation speed correction circuit 600A according to a second embodiment.
FIG. 5 is a detailed diagram of a bucket rotation speed correction circuit 600B according to a third embodiment.
FIG. 6 is a diagram illustrating an example of a multi-joint foundation working machine.
FIG. 7 is a diagram showing the definition of the ground angle and relative angle of each arm in the work machine.
[Explanation of symbols]
DESCRIPTION OF SYMBOLS 1-3 ... Arm, 4 ... Rotary excavation bucket, 5-8 hydraulic cylinder, 9 ... Electrohydraulic conversion valve, 101-104 ... Angle detector, 201-203 ... Addition point, 301 ... Memory | storage device, 302 ... Addition point, 400: arm rotation speed calculation circuit, 501 502: addition point, 600, 600A, 600B ... work attachment rotation speed correction circuit, 601 ... absolute value converter, 602 ... sign inverter, 603, 604 ... encoder, 605: Exclusive OR circuit, 606 ... Switcher, 607 ... Function generator, 608 ... Multiplier, 650 ... Function generator, 651 ... Addition point, 660 ... Limit circuit, 661 ... Negative limiter circuit, 662 ... Positive limiter Circuit, 663... Switch, 670 .. dead band adding circuit, 671, 672... Function generator, 673... Switch, 674 .. addition point, 701 .. function generator, 608. 1 ... function generator, 702 ... multiplier, 703 ... coefficient multiplier

Claims (4)

Posture angle detection means for detecting a posture angle of a work attachment that is mounted on a drive body rotatably attached to the base and whose posture angle is controlled by an actuator;
Deviation calculation means for calculating a deviation between the posture angle of the work attachment based on the value detected by the posture angle detection means and the target value;
First rotation speed calculation means for calculating and outputting a first rotation speed of the work attachment necessary for maintaining the posture angle at the target value regardless of rotation of the driving body;
Second rotation speed calculation means for calculating and outputting a second rotation speed of the work attachment based on the posture angle deviation;
In the posture angle control device for a work attachment having addition means for adding the calculated first and second rotation speeds,
Limiting means for limiting the output so that the added rotation speed is output when the added rotation speed is in the same direction as the first rotation speed, and the added rotation speed is not output when the added rotation speed is in the opposite direction. An attitude angle control device for a working machine, wherein the actuator is driven by the output of the limiting means. Posture angle detection means for detecting a posture angle of a work attachment that is mounted on a drive body rotatably attached to the base and whose posture angle is controlled by an actuator;
Deviation calculation means for calculating a deviation between the posture angle of the work attachment based on the value detected by the posture angle detection means and the target value;
First rotation speed calculation means for calculating and outputting a first rotation speed of the work attachment necessary for maintaining the posture angle at the target value regardless of rotation of the driving body;
Second rotation speed calculation means for calculating and outputting a second rotation speed of the work attachment based on the posture angle deviation;
In the posture angle control device for a work attachment having addition means for adding the calculated first and second rotation speeds,
When the posture angle of the work attachment is delayed with respect to the target value, the first rotation speed is increased based on the posture angle deviation, and the posture angle of the work attachment advances with respect to the target value. Speed correction means for correcting the first rotation speed so as to reduce the first rotation speed based on the posture angle deviation when
Limiting means for limiting the output so as not to output the added rotation speed when the added rotation speed is in the same direction as the first rotation speed, and not to output the added rotation speed when the added rotation speed is in the opposite direction. And a posture angle control device for a working machine, wherein the actuator is driven by the output of the limiting means. Posture angle detection means for detecting a posture angle of a work attachment that is mounted on a drive body rotatably attached to the base and whose posture angle is controlled by an actuator;
Deviation calculation means for calculating a deviation between the posture angle of the work attachment based on the value detected by the posture angle detection means and the target value;
First rotation speed calculation means for calculating and outputting a first rotation speed of the work attachment necessary for maintaining the posture angle at the target value regardless of rotation of the driving body;
Second rotation speed calculation means for calculating and outputting a second rotation speed of the work attachment based on the posture angle deviation;
In the posture angle control device for a work attachment having a first addition means for adding the calculated first and second rotation speeds,
Limiting means for limiting the output so as not to output the added rotational speed when the added rotational speed is in the same direction as the first rotational speed, and not to output the added rotational speed when in the reverse direction;
0 if the attitude angle of the working attachment is delayed with respect to the target value, 0 if the attitude angle deviation the attitude angle is not advanced with respect to the target value is within a predetermined value the calculated value of the first rotational speed and the reverse direction based on the attitude angle deviation when the attitude angle deviation the attitude angle is not advanced with respect to the target value exceeds a predetermined value, the respective Third rotation speed calculation means for outputting as a rotation speed of 3,
An attitude angle control device for a working machine, comprising: a second addition unit that adds the third rotation speed and the output of the limiting unit; . In the attitude angle control device for a work machine according to claim 3,
When the posture angle of the work attachment is delayed with respect to the target value, the first rotation speed is increased based on the posture angle deviation, and the posture angle of the work attachment advances with respect to the target value. An attitude angle control device for a working machine, further comprising speed correction means for correcting the first rotation speed so as to reduce the first rotation speed based on the attitude angle deviation. .

Images