(57)【特許請求の範囲】
【請求項１】
基台に回転可能に取り付けられた駆動体に装着され、姿勢角をアクチュエータにより制御される作業アタッチメントの姿勢角を検出する姿勢角検出手段と、
前記姿勢角検出手段で検出された値に基づく前記作業アタッチメントの姿勢角とその目標値との偏差を演算する偏差演算手段と、
前記駆動体の回転に拘らず前記姿勢角を前記目標値に維持するために必要な前記作業アタッチメントの第１の回転速度を演算出力する第１の回転速度演算手段と、
前記姿勢角偏差に基づいた前記作業アタッチメントの第２の回転速度を演算出力する第２の回転速度演算手段と、
前記演算された第１と第２の回転速度を加算する加算手段とを有する作業アタッチメントの姿勢角制御装置において、
前記加算された回転速度が第１の回転速度と同方向の場合は前記加算された回転速度を出力し、逆方向の場合は前記加算された回転速度を出力しないように制限する制限手段を備え、この制限手段の出力で前記アクチュエータを駆動することを特徴とする作業機の姿勢角制御装置。
【請求項２】
基台に回転可能に取り付けられた駆動体に装着され、姿勢角をアクチュエータにより制御される作業アタッチメントの姿勢角を検出する姿勢角検出手段と、
前記姿勢角検出手段で検出された値に基づく前記作業アタッチメントの姿勢角とその目標値との偏差を演算する偏差演算手段と、
前記駆動体の回転に拘らず前記姿勢角を前記目標値に維持するために必要な前記作業アタッチメントの第１の回転速度を演算出力する第１の回転速度演算手段と、
前記姿勢角偏差に基づいた前記作業アタッチメントの第２の回転速度を演算出力する第２の回転速度演算手段と、
前記演算された第１と第２の回転速度を加算する加算手段とを有する作業アタッチメントの姿勢角制御装置において、
前記作業アタッチメントの姿勢角が前記目標値に対して遅れているときは前記姿勢角偏差に基づいて前記第１の回転速度をより大きくし、前記作業アタッチメントの姿勢角が前記目標値に対して進んでいるときは前記姿勢角偏差に基づいて前記第１の回転速度をより小さくするように前記第１の回転速度を補正する速度補正手段と、
前記加算された回転速度が第１の回転速度と同方向の場合は前記加算された回転速度を出力し、逆方向の場合は前記加算された回転速度を出力しないように制限する制限手段とを備え、この制限手段の出力で前記アクチュエータを駆動することを特徴とする作業機の姿勢角制御装置。
【請求項３】
基台に回転可能に取り付けられた駆動体に装着され、姿勢角をアクチュエータにより制御される作業アタッチメントの姿勢角を検出する姿勢角検出手段と、
前記姿勢角検出手段で検出された値に基づく前記作業アタッチメントの姿勢角とその目標値との偏差を演算する偏差演算手段と、
前記駆動体の回転に拘らず前記姿勢角を前記目標値に維持するために必要な前記作業アタッチメントの第１の回転速度を演算出力する第１の回転速度演算手段と、
前記姿勢角偏差に基づいた前記作業アタッチメントの第２の回転速度を演算出力する第２の回転速度演算手段と、
前記演算された第１と第２の回転速度を加算する第１の加算手段とを有する作業アタッチメントの姿勢角制御装置において、
前記加算された回転速度が第１の回転速度と同方向の場合は前記加算された回転速度を出力し、逆方向の場合は前記加算された回転速度を出力しないように制限する制限手段と、
前記作業アタッチメントの姿勢角が前記目標値に対して遅れている場合には０を、前記姿勢角が前記目標値に対して進んでいて前記姿勢角偏差が所定値以内にある場合には０を、前記姿勢角が前記目標値に対して進んでいて前記姿勢角偏差が所定値を越える場合には前記姿勢角偏差に基づいた前記第１の回転速度と逆方向の演算値を、それぞれ第３の回転速度として出力する第３の回転速度演算手段と、
前記第３の回転速度と前記制限手段の出力を加算する第２の加算手段とを備え、この第２の加算手段の出力で前記アクチュエータを駆動することを特徴とする作業機の姿勢角制御装置。
【請求項４】
請求項３に記載した作業機の姿勢角制御装置において、
前記作業アタッチメントの姿勢角が前記目標値に対して遅れているときは前記姿勢角偏差に基づいて前記第１の回転速度をより大きくし、前記作業アタッチメントの姿勢角が前記目標値に対して進んでいるときは前記姿勢角偏差に基づいて前記第１の回転速度をより小さくするように前記第１の回転速度を補正する速度補正手段をさらに備えることを特徴とする作業機の姿勢角制御装置。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
本発明は、作業機に回転可能に装着された作業アタッチメントの姿勢角を一定に制御する制御装置に関する。
【０００２】
【従来の技術】
従来から多関節アームの先端に回転掘削バケットを装着した基礎用作業機（以下、作業機とする）が知られている。この作業機は、下部走行体上に旋回可能に設けられた上部旋回体上に、各々が第１〜第３の油圧シリンダで駆動される第１アーム、第２アーム、第３アームを有し、第３アームの先端に回転掘削バケットを装着している。そして、回転掘削バケットは、第４シリンダでその姿勢角を種々の角度方向に制御される。
【０００３】
このような作業機による掘削では、回転掘削バケットの姿勢角を一定に保つ必要がある。特に回転掘削バケットでは、姿勢角誤差が大きいと掘削した孔をくずしたり、配管等の埋設物を損傷したり、掘削抵抗が増大してバケットの回転が停止したりするので、高精度な姿勢角制御が要求される。ところで、ショベル本体と、ブームと、アームと、アーム先端に取付けられたバケットとを有する油圧式パワ−ショベルにおいては、バケットの姿勢角を一定に保つ制御装置が種々知られている。たとえば、特公昭６１−４５０２５号公報（第１の従来技術とする）においては、バケットの姿勢を保持するため、ブーム回動速度とアーム回動速度の和の符号を反転した値をバケット回動速度として求め、これによりバケットシリンダの流量制御を行っている。また、特許第２５０９３６８号（第２の従来技術とする）においては、バケットの姿勢を保持するため、作業アタッチメントの回転速度を求め、検出された姿勢角の目標値に対する偏差と、求められた回転速度の方向とに基づいてその回転速度を補正し、この補正後の回転速度で作業アタッチメントの回転速度を制御するようにしている。
【０００４】
【発明が解決しようとする課題】
しかしながら、上記第１の従来技術においては、バケットシリンダの流量制御値に対する実際の流量出力値に誤差があると、姿勢角に誤差が生じて累積されてゆくという欠点がある。さらに、バケット回動速度を求めるためのブームおよびアーム回動速度が非常に低速になる場合には、実際の回動速度を表す角度センサ出力の微分値を用いることができないため、速度指令値から求めた流量制御値を用いるので、ブームおよびアームシリンダの流量制御値に対する実際の流量出力値の誤差も姿勢角の誤差に加算されることになる。
【０００５】
また、上記第２の従来技術においては、姿勢角の目標値に対する偏差のフィードバック特性において制御目標値付近に不感帯を設けてあるので、姿勢角精度の改善が偏差の大きい場合に限られるという問題があった。さらに、姿勢角偏差に基づいて補正したフィードフォワード量と姿勢角偏差フィードバック量とを加算すると、姿勢角の制御方向が常に一定とならないので、制御量の符号が反転する姿勢角偏差付近でハンチングを起こす可能性があった。
【０００６】
本発明の目的は、姿勢角の目標値に対する偏差が小さなときにも、姿勢角偏差に基づいた制御を行うことができる作業機の姿勢角制御装置を提供することにある。また、本発明の他の目的は、姿勢角制御の制御量の符号が反転する姿勢角偏差付近でのハンチングを抑えた作業機の姿勢角制御装置を提供することにある。
【０００７】
【課題を解決するための手段】
一実施の形態を示す図２、図４および図５を参照して本発明を説明する。
（１）請求項１の発明は、基台に回転可能に取り付けられた駆動体に装着され、姿勢角をアクチュエータにより制御される作業アタッチメントの姿勢角を検出する姿勢角検出手段１０１〜１０４、２０１〜２０３と、姿勢角検出手段１０１〜１０４、２０１〜２０３で検出された値に基づく作業アタッチメントの姿勢角とその目標値との偏差Δδを演算する偏差演算手段３０２と、駆動体の回転に拘らず姿勢角を目標値に維持するために必要な作業アタッチメントの第１の回転速度Ｔv４を演算出力する第１の回転速度演算手段４００、５０１〜５０２と、姿勢角偏差Δδに基づいた作業アタッチメントの第２の回転速度Ｔv４dを演算出力する第２の回転速度演算手段６５０と、演算された第１と第２の回転速度Ｔv４,Ｔv４d を加算する加算手段６５１とを有する作業アタッチメントの姿勢角制御装置に適用される。そして、加算された回転速度が第１の回転速度Ｔv４と同方向の場合は加算された回転速度を出力し、逆方向の場合は加算された回転速度を出力しないように制限する制限手段６６０を備え、この制限手段６６０の出力でアクチュエータを駆動することにより上述した目的を達成する。
（２）請求項２の発明は、基台に回転可能に取り付けられた駆動体に装着され、姿勢角をアクチュエータにより制御される作業アタッチメントの姿勢角を検出する姿勢角検出手段１０１〜１０４、２０１〜２０３と、姿勢角検出手段１０１〜１０４、２０１〜２０３で検出された値に基づく作業アタッチメントの姿勢角とその目標値との偏差Δδを演算する偏差演算手段３０２と、駆動体の回転に拘らず姿勢角を目標値に維持するために必要な作業アタッチメントの第１の回転速度Ｔv４を演算出力する第１の回転速度演算手段４００、５０１〜５０２と、姿勢角偏差Δδに基づいた作業アタッチメントの第２の回転速度Ｔv４dを演算出力する第２の回転速度演算手段６５０と、演算された第１と第２の回転速度Ｔv４,Ｔv４d を加算する加算手段６５１とを有する作業アタッチメントの姿勢角制御装置に適用される。そして、作業アタッチメントの姿勢角が前記目標値に対して遅れているときは姿勢角偏差Δδに基づいて第１の回転速度Ｔv４をより大きくし、作業アタッチメントの姿勢角が目標値に対して進んでいるときは姿勢角偏差Δδに基づいて第１の回転速度Ｔv４をより小さくするように第１の回転速度Ｔv４を補正する速度補正手段６０１〜６０８と、加算された回転速度が第１の回転速度Ｔv４と同方向の場合は加算された回転速度を出力し、逆方向の場合は加算された回転速度を出力しないように制限する制限手段６６０とを備え、この制限手段６６０の出力でアクチュエータを駆動することにより上述した目的を達成する。
（３）請求項３の発明は、基台に回転可能に取り付けられた駆動体に装着され、姿勢角をアクチュエータにより制御される作業アタッチメントの姿勢角を検出する姿勢角検出手段１０１〜１０４、２０１〜２０３と、姿勢角検出手段１０１〜１０４、２０１〜２０３で検出された値に基づく作業アタッチメントの姿勢角とその目標値との偏差Δδを演算する偏差演算手段３０２と、駆動体の回転に拘らず姿勢角を目標値に維持するために必要な作業アタッチメントの第１の回転速度Ｔv４を演算出力する第１の回転速度演算手段４００、５０１〜５０２と、姿勢角偏差Δδに基づいた作業アタッチメントの第２の回転速度Ｔv４dを演算出力する第２の回転速度演算手段６５０と、演算された第１と第２の回転速度Ｔv４,Ｔv４d を加算する第１の加算手段６５１とを有する作業アタッチメントの姿勢角制御装置に適用される。そして、加算された回転速度が第１の回転速度Ｔv４と同方向の場合は加算された回転速度を出力し、逆方向の場合は加算された回転速度を出力しないように制限する制限手段６６０と、作業アタッチメントの姿勢角が目標値に対して遅れている場合には０を、姿勢角が目標値に対して進んでいて姿勢角偏差Δδが所定値以内にある場合には０を、姿勢角が目標値に対して進んでいて姿勢角偏差Δδが所定値を越える場合には姿勢角偏差Δδに基づいた第１の回転速度Ｔv４と逆方向の演算値を、それぞれ第３の回転速度Ｔv４aとして出力する第３の回転速度演算手段６７１〜６７３と、第３の回転速度Ｔv４aと制限手段６６０の出力を加算する第２の加算手段６７４とを備え、この第２の加算手段６７４の出力でアクチュエータを駆動することにより上述した目的を達成する。
（４）請求項４に記載の発明は、請求項３に記載の作業機の姿勢角制御装置において、作業アタッチメントの姿勢角が目標値に対して遅れているときは姿勢角偏差Δδに基づいて第１の回転速度Ｔv４をより大きくし、作業アタッチメントの姿勢角が目標値に対して進んでいるときは姿勢角偏差Δδに基づいて第１の回転速度Ｔv４をより小さくするように第１の回転速度Ｔv４を補正する速度補正手段６０１〜６０８をさらに備えることを特徴とする。
【０００８】
なお、上記課題を解決するための手段では、わかりやすく説明するために実施の形態の図と対応づけたが、これにより本発明が実施の形態に限定されるものではない。
【０００９】
【発明の実施の形態】
以下、図面を参照して本発明の実施の形態を説明する。
−第一の実施の形態−
図６は本発明による姿勢角制御装置が搭載される従来から知られている作業機の一例を示す。この作業機は、下部走行体ＬＴ上に上部旋回体ＵＳを旋回可能に設けて成り、上部旋回体ＵＳ上に第１アーム１、第２アーム２、第３アーム３およびこれらを駆動する第１シリンダ５、第２シリンダ６、第３シリンダ７を有し、第３アーム３の先端に回転掘削バケット４を装着し、第４シリンダ８でその姿勢角を図６（ａ）、（ｂ）、（ｃ）のように制御する。
【００１０】
図１は、本発明の第一の実施の形態による作業アタッチメントの姿勢角制御装置を示す図である。図６に示した作業機において、各アームの対地角度および相対角度を図７のように定義する。すなわち、図７において、点Ｏを第１アーム１の回動支点とし、第１アーム１の対地角度をα，第２アーム２の第１アーム１に対する相対角度をＴ２，第３アーム３の第２アーム２に対する相対角度をＴ３，バケット４の第３アーム３に対する相対角度をＴ４とすると、バケット４の対地姿勢角δは次式（１）となる。
【数１】
δ＝α−Ｔ2 −Ｔ3 −Ｔ4             （１）
【００１１】
図１において、角度検出器１０１は第１アーム１の回動支点付近に取付けられ、周知の振り子機構とポテンショメ−タにより第１アーム１の対地角αを検出する。角度検出器１０２，１０３，１０４はそれぞれ第２アーム２，第３アーム３，バケット４の回動支点に取付けられ、周知のレバ−機構とポテンショメ−タにより第１アーム１と第２アーム２の相対角Ｔ２，第２アーム２と第３アーム３との相対角Ｔ３、第３アーム３とバケット４との相対角Ｔ４を検出する。２０１〜２０３はそれぞれ加算点であり、（１）式に示したバケットの姿勢角δはこれらの加算点２０１〜２０３で演算される。３０１は目標となるバケット姿勢角δ０を保持する記憶器で、本実施の形態では制御開始時のバケット姿勢角を記憶保持する。３０２は加算点で、目標となるバケット姿勢角δ０と実際のバケット姿勢角δとの偏差Δδを次式（２）のように演算する。
【数２】
Δδ＝δ−δ0                       （２）
【００１２】
アーム回動速度演算回路４００は図示しない軌跡制御装置の一部であり、バケット取り付け点（図６のＰＢ）の軌跡を制御するように演算された各アームの回動速度αv ，Ｔv２，Ｔv３を出力する。たとえば、第１アーム１を固定して、第２アーム２、第３アーム３で軌跡制御する場合の各アームの回動速度は次式（３）で演算することができる。
【数３】
0003802697.tif 000002
【００１３】
バケット姿勢角を一定に保つためには、（１）式の両辺を時間微分して次式（４）で求める補正前のバケット回動速度Ｔ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４が０となる以前に出力していた符号に基づいた信号を出力する（次式（６））。
【数６】
0003802697.tif 000003
【００１８】
上述した第一の実施の形態による作業機の姿勢角制御についてさらに詳細に説明する。図示しない電源スイッチが投入されると、角度検出器１０１〜１０４で検出された角度α，Ｔ２，Ｔ３，Ｔ４に基づいて加算点２０１〜２０３でバケットの姿勢角δが演算される。アームの制御が開始されると、その開始時のバケット姿勢角δ０が記憶器３０１に保持され、以後、姿勢角偏差Δδが加算点３０２で演算される。一方、アーム回動速度演算回路４００で演算された各アームの回動速度α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４に代えて式（７）を代入すれば、第二の実施の形態によるバケット回動速度補正回路６００Ａの出力は次式（８）のように表すことができる。
【数８】
0003802697.tif 000004
【００２７】
上述した第二の実施の形態による作業機の姿勢角制御についてさらに詳細に説明する。図示しない電源スイッチが投入され、バケット姿勢角を一定に保つために補正前のバケットの回動速度Ｔ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４の符号が正のときは切換器６７３の接点はａ側に切換えられ、負のときはｂ側に切換えられる。加算点６７４は、制限回路６６０から出力される信号と切換器６７３で選択される信号とを加算する。
【００３２】
したがって、第二の実施の形態によるバケット回動速度補正回路６００Ｂの出力に不感帯付加回路６７０を追加することにより第三の実施の形態によるバケット回動速度補正回路６００Ｃが構成され、この回路の出力は次式（９）のように表すことができる。
【数９】
0003802697.tif 000005
【００３３】
上述した第三の実施の形態による作業機の姿勢角制御についてさらに詳細に説明する。図示しない電源スイッチが投入され、制限回路６６０からの出力を得るまでは第二の実施の形態と同じである。第二の実施の形態による制御回路では、上述したようにＴ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の関係を示すグラフである。
【図４】第二の実施の形態によるバケット回動速度補正回路６００Ａの詳細図である。
【図５】第三の実施の形態によるバケット回動速度補正回路６００Ｂの詳細図である。
【図６】多関節基礎用作業機の一例を示す図である。
【図７】作業機における各アームの対地角度および相対角度の定義を示す図である。
【符号の説明】
１〜３…アーム、４…回転掘削バケット、５〜８油圧シリンダ、９…電気油圧変換弁、１０１〜１０４…角度検出器、２０１〜２０３…加算点、３０１…記憶器、３０２…加算点、４００…アーム回動速度演算回路、５０１、５０２…加算点、６００、６００Ａ、６００Ｂ…作業アタッチメント回動速度補正回路、６０１…絶対値変換器、６０２…符号反転器、６０３、６０４…符号器、６０５…排他的論理和回路、６０６…切換器、６０７…関数発生器、６０８…乗算器、６５０…関数発生器、６５１…加算点、６６０…制限回路、６６１…負リミッタ回路、６６２…正リミッタ回路、６６３…切換器、６７０…不感帯付加回路、６７１、６７２…関数発生器、６７３…切換器、６７４…加算点、７０１…関数発生器、６０８…乗算器、７０１…関数発生器、７０２…乗算器、７０３…係数器
【図１】
0003802697.tif 000006
【図２】
0003802697.tif 000007
【図３】
0003802697.tif 000008
【図４】
0003802697.tif 000009
【図５】
0003802697.tif 000010
【図６】
0003802697.tif 000011
【図７】
0003802697.tif 000012