JPS62189223A - 作業機の制御装置 - Google Patents

Abstract

(57)【要約】本公報は電子出願前の出願データであるた
め要約のデータは記録されません。

Description

【発明の詳細な説明】 ［産業上の利用分野１ 本発明はパワーショベル等の作業機の制御装置に関する
ものである。

［従来の技術］ 一般に油圧式パワーショベルは第７図に示すようにブー
ム１．アーム２．バケット３及びこれらを駆動するブー
ムシリンダ４．アームシリンダ５゜バケットシリンダ６
を具備しており、各シリンダ４〜６は夫々運転室内に配
備された操作レバー（図示Ｕず）により手動操作される
。そして、オペレータが手動操作により作業機の姿勢を
制御してバケット位置を設定するようになっている。こ
のため、オペレータに高度な技術が要求される。

特に、バケットによる直線掘削作業すなわち、アームの
平行移動制御は極めて困難である。

そこで、第８図に示すように、アームの先端点のＸ軸方
向及びｙ軸方向の速度ベクトル〈「およびｙｒを指令速
度入力装置１０で設定し、この速度ペルトルｘｒ、ｙｒ
と角度検出器１１．１２でそれぞれ検出したブーム角α
およびアーム角βとに基づいてブーム１およびアーム２
の目標回動速度ｃｉｒ、ｊｒを目標回動速度演算装置１
３で求め、この目標回動速度ｃｉｒ、ｊｒをブーム駆動
系１４およびアーム駆動系１５に入力し、これら駆動系
１４．１５によってブームシリンダ４およびアームシリ
ンダ５への圧油供給油量を制御することにより、アーム
先端点の軌跡を平行移動させるようにしたものがある（
特開昭５５−３００３８号）。

すなわち、作業別各部の位置を第９図に示すように定義
する。

原点Ａ　（０，０）：ブーム１の回動支点点８　（ｘｂ
、ｙｂ　）　：アーム２の回動支点点Ｃ（ｘｃ、ｙｃ　
）　：バケット３の回動支点点Ｄ　（ｘｄ、ｙｄ　）　
：バケット３の刃先点とする。ここで、 Ｘ軸：点Ａ、Ｂ、Ｃ，Ｄを含む平面と点Ａを含む車輪旋
回面との交線 ｙ軸：点Ａを通り、車輪旋回面に垂直な直線である。

また、第９図において ［１：点Ａ、Ｂ間の長さ Ｌ２：点８．０間の長さ Ｌ３：点Ｃ，Ｄ間の長さ α：ｙ軸に対する線分ＡＳのなす角（ブーム角）β：ｙ
軸に対する線分ＢＣのなす角（アーム角）γ：ｙ＠に対
する線分ＣＤのなす角（刃先角）である。

以上のように定義すると、アーム２の先端点Ｃの座標（
ｘｃ、ｙｃ）は ｘ　ｃ　＝　ｌ　１ｓｉｎａ　＋　ｌ　２ｓｉｎ　（α
＋β）ｙ（＝１１ｃｏｓα＋Ｌ　２ｃｏｓ　（α＋β）
・・・（１）となる。

これを時間について微分すると、 ・・・（２） となる。

但し、ｄ＝αの時間的変化（ブームの回動速度）ｂ＝β
の時間的変化（アームの回ａ速度）である。

よって、アーム２の先端点ＣのＸ軸方向の速度成分ｘｃ
、ｙ軸方向の速度成分９Ｃを指令値とした場合のブーム
１およびアーム２の目標回動速度ｔｆｘｃ　、　４ｃは
前記第（２）式を逆変換することにより、 ・・・　（３） 但し、Ａ　＝　−Ｌ２ｓｉｎ（α　十β）３−−Ｌ２ｃ
ｏｓ（α　十β） Ｃ＝−Ｌ１ｓｉｎα＋　Ｌ２ｓｉｎ（α　＋β）［）＝
　１１ｃｏｓ　　α＋Ｌ２ｃｏｓ（α　十β　）となる
。従って、この演算をΩＣ＝Ｏとして目標口ｖＪ速度演
ＲＨＩ？ｆ１３によって行うことにより、アーム先端点
の軌跡を平行移動させることができる。

［発明が解決しようとする問題点］ しかしながら、上記速度ベクトルｘｃ、ｙｃに対応した
回動速度指令αＣ１βＣによってアーム先端あるいはバ
ケット先端を速度制御する場合、ブーム、アーム、バケ
ットの各種作業の応答ぼれ、負倚外乱、制御系の定常偏
差等の速度外乱によって位置婦差が生じ、速度ベクトル
で指令した目標軌跡からアーム先端あるいはバケット先
端がずれるといった問題があった。

例えば、第１０図に示すように、水平の直線掘削を行う
為の速度ベクトルＶを与えると、この速度ベクトル■に
対応した回ｖＪ速度指令によって、ブーム１は反時計回
り方向、アーム２は時計回り方向に回動するが、いま前
述の速度外乱がアーム２に発生し、アーム回動速度がそ
の指令値まで達しなかったとすると、アーム先端の軌跡
はＣＯ２のように上方に向かう。そして、Ｃ′に達した
俊、アーム２も指令値通りにｆｌＪＪ　’ｄＯされると
すると、ｃ　’　Ｃ”のように目標軌跡と平行な軌跡を
描くことになる。

このように、各秒速度外乱が発生する毎に、速度ベクト
ル指令による目標軌跡と実機の軌跡がずれてしまうので
、掘削や運搬など各種作業が思うように実施できなくな
り、またオペレータの意図しない方向へ作′Ｍ機が動く
可能性もあり、安全面についても問題があった。

本発明は上記のような問題点に鑑み、各種の外乱が発生
しても作業機の軌跡を指令値通りに制御することができ
る作呆叔の制御装置を提供することを目的とするもので
ある。

［問題点を解決するための手段］ 本発明は、ブーム、アームおよびバケットを含む平面を
座標平面としてアームの先端点のＸ軸方向およびｙ軸方
向の位置を指令する位置指令手段と、ブーム角およびア
ーム角をそれぞれ検出する角度検出手段と、この角度検
出手段で検出したブーム角およびアーム角に基づいてア
ームの先端点の現在位置を算出する現在位置演算手段と
、前記位置指令手段により設定された指令位置と前記現
在位置演算手段で算出されたアーム先端点の現在位置と
に基づきアーム先端点が指令位置へ移動するための速度
ベクトルを専用づる速度ベクトル演算手段と、算出され
た速度ベルクルと前記角度検出手段で検出したブーム角
およびアーム角に基づいてブーム目標回１１１Ｊ速度お
よびアーム目標回ｖＪ速度を算出する回動速度演算手段
と、算出された回動速度に基づいてブームシリンダおよ
びアームシリンダへの圧油供給流量を制御する流量制御
手段とを設けたものである。

〔作用］ 速度ベルモル演痺手段は、位置指令手段によって設定さ
れたアーム先端点の指令位置と現在位置演算手段で算出
されたアーム先端点の現在位置とに基づいてアーム先端
点を指令位置へ移動させるための速度ベルクルを算出す
る。そして、回動速度演算手段は算出された速度ベクト
ル値とブーム角およびアーム角の検出値に基づいてブー
ムおよびアームの目標回ｖＪ速度をそれぞれｐ出し、ブ
ームシリンダおよびアームシリンダへの圧油供給但を制
鶴りする。

従って、ブームおよびアームの回ｖＪ速度がアーム先端
点の現在位置を監視しながら制御されるため、速度外乱
が発生してもアームの軌跡が指定した軌跡からずれるこ
とはなくなる。

［実施例］ 第１図は本発明の一実施例を示すブロック図であり、第
８図の従来＠成と異なる点は指令位置入力装置１６と、
現在位置演算手段１７と、速度ベクトル演算装置１Ｂと
を設（づ、目標回動速度演忰装置１３に対しアーム２の
先端点の位置を指令位置へ移動させるための速度ベクト
ル；ｃｒｏ、　ｙｒ゛を入力し、ブーム１およびアーム
２の回０速度を制御するようにしたものである。

指令位置入力装ｅ１１６は、ブーム、アームおよびバケ
ットを含む平面を座標平面としてアーム先端点のＸ軸方
向およびｙ軸方向の位置（ｘｒ、ｙ「）を指令するもの
で、第２図に示すように、作業機と相似形のマスクレバ
ー１６ａと、このマスタレバー１６ａのブームに対応す
る部分の操作角αＦ　（ブーム角αに対応）を検出する
角度検出器１６ｂと、マスクレバー１６ａのアームに対
応する部分の操作角β［（アーム角βに対応）を検出す
る角度検出器１６ｃと、これら角度検出器１６ｂ、ｉ５
ｃ出検出した操作角α［、β［と作業機のブーム１およ
びアーム２の長さ１１　、Ｌ２とを基に次式（４）によ
ってアーム先端点の指令位置（Ｘｒ、Ｖｒ）を算出する
指令位置入力装置１６ｄとから構成されている。

ｘｒ　＝　Ｌｌｓｉｎαｌ　＋Ｌ２ｓｉｎ（ａＬ十β［
）ｙｒ　　＝　Ｌ２ＣＯ３（２Ｌ　　＋　Ｌ２ｃｏｓ（
αＬ　　＋β［）・・・　（４） 現在位置演算装置１７は、角度検出器１１，１２でそれ
ぞれ検出したブーム角αおよびアーム角βとを基に前述
した第（１）式によってアーム先端点の現在位置（ｘ、
ｙ）を算出するもので、その締出結果は速度ペルトル演
算装置１８に入力される。

速度ベクトル演停装＠１８は、指令位置入力袋＠１６か
ら与えられるアーム先端点の指令位置（ｘｒ、ｙｒ）と
現在位置演算装置１７がら入力されるアーム先端点の現
在位１ｆｆ（ｘ、ｙ）とに基づきアーム先端点を指令位
置（ｘｒ　、　ｙｒ　）へ移動させるための速度ベクト
ルｘｒ°、　ｙｒ”を次式（５）によって算出する。

ｘｒ’＝Ｋｘ　（ｘｒ　−Ｘ） ｙｒ’＝Ｋｙ　（ｙｒ　−Ｙ）　　　　　・・・（５）
但し、ＫＸ、ｋｙは正の係数である。

目標回動速度演算装置１３は、アーム先端点の位置を指
令位置（ｘｒ、ｙｒ）に制御するためのブーム１および
アーム２の回動速度を演算するもので、ここではｆｌｕ
ベクトルｉｒ゛、　ｙｒ゛とブーム角α、アーム角β、
プーム１の長さＬｌ、アーム２の長さＬ２とを塁に次式
（６）によって算出する。

但し、　Ａ　＝　−１２ｓｉｎ（α　十β　）３　＝　
−Ｌ２ｃｏｓ（α　十β） Ｃ＝　　　Ｌｌｓｉｎ　　α＋Ｌ２ｓｉｎ（ａ　　十β
　）［）＝　　　Ｌｌｃｏｓ　　α＋Ｌ２ｃｏｓ　（α
　十β）・・・（６） ブーム駆動系１４およびアーム駆動系１５は、以上のよ
うにして算出されたブーム１の目標回動速度ｄ「および
アーム２の目標圏ｖＪ速度／３ｒに対応してブームシリ
ンダ４およびアームシリンダ５に対する圧油供給給量を
制御する。

これによって、ブーム１およびアーム２は目標回動速度
ｉｒ、ｊｒに対応した速度で回動し、アーム２の先端点
は指令された位置ｘｒ、ｙｒに制御される。

第３図は、ブーム１およびアーム２の目標回動速度ｃｃ
ｒ、ｊｒを別の方法で算出するようにした本発明の他の
実施例を示すブロック図であり、第１図の構成に対して
目標角度演算装Ｗ１１９が新たに付加されている。

この実施例は、ブーム角αのの初期値α０．アーム角の
初期値β０に対する１サンプリング時間後のブーム目標
角度αｒ、アーム目標角度ｂ「を順次算出しながらブー
ム目標回動速度ｄｒ、アーム目標回初速度βｒを求めて
いくものである。

すなわち、ブーム１およびアーム２が停止している時の
ブーム角αの初期値をα０．アーム角βの初期値をβＯ
とすると、速度ベクトル（ｘ　ｒ’。

ｙｒ’）およびα０．βＯよりブーム目標回動速度ｄ「
、アーム目標回動速度ｊ３ｒは次式げ）によって算出さ
れる。

但し、Ａ　＝−Ｌｌｓｉｎ（α０＋β０）Ｂ　　＝　　
−Ｌ２ｃｏｓ（α０＋β　０）Ｃ＝　　Ｌｌｓｉｎ　　
ａ　Ｏ＋Ｌ２ｓｉｎ（αＯ＋β　０）Ｄ　　＝　　Ｌｌ
ｃｏｓ　　ａ　Ｏ＋Ｌ２ＣＯ３（（Ｚ　Ｏ＋β　０）・
・・　（７） ざらに、サンプリング時間を八ＴすればΔＴ秒後のブー
ム目標角度αｒ、アーム目標角度βｒは、次式（８）の
ように近似できる。

αｒ　＝ｃｉｒ　ｘΔＴ十αＯ βｒ　＝ｊｒ　ｘＡＴ＋βＯ・（８） そして、次のサンプリング時間の目標回動速度は、それ
ぞれ１サンプリング時間前のブーム目標角度αｒ゛、ア
ーム目標角度βｒ゛を用いて次式（９）で算出すること
ができる。

但し、Ａ　−−Ｌ２Ｓｉｎ（αｒ’＋β「゛）Ｂ　＝　
−Ｌ２ＣＯ３（（Ｚ　ｒ’−＋−βｒ’）Ｃ＝　　　Ｌ
ｌｓｉｎ　　αｒ’　　＋１２ｓｉｎ（ｃｒｒ’十　β
　ｒ’）Ｄ＝　　Ｌｌｃｏｓ　αｒ’　＋Ｌ２ｃｏｓ（
αｒ’＋βｒ’）・・・（９） さらにαｒ、β「は次のく１０）式で近似できる。

αｒ　＝ｃｉｒ　ｘ△Ｔ＋αｒ゛ Ｂｒ　＝ｊｒ　ｘΔＴ＋βｒ′ ・・・（１０） 従つ′で、（８）式および（９）式を目標角度演算装置
１９で実行し、その実行結果を目標回！ＩＩ速度演算装
置１３に入力し、この演算装置１３で第〈９）式を実行
することにより、アームの先端点を指令位置（ｘｒ、ｙ
ｒ）へ移動するための回動速ｒｆＸｊｒ、ｃｉｒを求め
ることができる。

なお、演算精度をさらに向上させるためには台形則等を
用いればよい。

第４図はブーム駆動系１４およびアーム駆動系１５の詳
細構成例を示ずブロック図であり、ブーム駆動系１４お
よびアーム駆動系１５のいずれがもブーム角αおよびア
ーム角βをそれぞれ検出する角度検出器１４ａ、’１５
ａと、検出したブーム角αお、よびアーム角αを微分演
算して実回動速度ｄおよびｂに変換する角速度演算器１
４ｂ、１５ｂと、目標回動速度ｄｒ、β「と実回動速度
ｄ。

泊との偏差ｃａｒ　−ａ、　ｊｒ　−ｆｉを増幅する補
償器１４ｃ、　１５ｃ、！：、増幅すｈ　タａ　差Ｕ　
ｒ　　（２、Ｂｒ−６に対応した圧油流量をブームシリ
ンダ４　ｄ３よびアームシリンダ５にそれぞれ供給する
流帛制曲弁１４ｄ、１５ｄとから構成されている。

第５図はブーム駆動系１４およびアーム駆動系１５の他
の実施例を示すブロック図である。この実施例は、第４
図の実施例がブーム角αおよびアーム角βを大同ｖＪ速
度に変換して目標回動速度ｄｒ、ｊｒとの偏差を求めて
いたのに対し、目標回動速度ｃｊｒ、ｊｒを目標角度演
算装置１４ｅ、１５ｅによって単位時間当りに変化すべ
き目標角度αｒ、β「に変換し、この目標角度αｒ、β
ｒと現在のブーム角α、アーム角βとの偏差αｒ−α。

βｒ−βを求め、これによってブームシリンダ４および
アームシリンダ５への圧油供給流量を単位時間毎に繰返
し制御するようにしたものである。

この場合、目標角度演算装置１４ｅ、１５ｅは第３図の
目標角度演算装置１９と同じ礪能のものである。

第６図は指令位置入力装置１６の他の実施例を示すブロ
ック図であり、 ジョイスティック型の操縦レバー１６ｅを用い、このレ
バー１６ｅの倒し角と方向とによってＸ軸方向およびｙ
軸方向の速度ｘｒ、ｙｒを指令するようにし、このレバ
ー１６ｅで指令された速度くｒ、ｙｒを指令位置演算装
置１６ｆに入力すると共に、ブーム角αとアーム角βと
に基づいてアーム先端点の現在位置ｘ、ｙを現在位置演
算装置１６ｇで算出し、この演算結果ｘ、ｙを指令位置
演算装置１６ｆに入力することにより、指令速度λｒ、
ｙｒに対し次の単位時間に変化すべき目標位置ｘｒ、ｙ
ｒを順次に粋出し、これを指令位置の情報として速度ベ
クトル演算装置１８に入力するようにしたものである。

すなわち、アーム先端点の初期位置を（ｘＯ２ｙＯ）と
すると、へＴ秒後の指令位置ｘｒ、ｙｒは ｘｒ＝交ｒ×ΔＴ＋Ｘ０ ｙｒ　＝ｙｒ　ｘΔＴ＋ｙＯ・＝＜１１）となるので、
これを最初の指令位置ｘｒ、ｙｒとして算出する。そし
て、次のΔＴ秒後の指令位置については、第（１１）式
の算出結果をｘｒ’、ｙ「°として ｘｒ　＝ｘｒ　ｘΔＴ十ｘ　ｒ’ ｙｒ＝シ「×Δｌ＋ｙｒ’　　　　・・・（１２）とし
で求める。以後、第（１２）式の演算結果Ｘ「、ｙ「を
新たなｘｒ’、　ｙｒ”として次のへＴ秒後の指令位置
を求める。

すなわち、単位時間当りの指令位置の変化量Δｘｒ、Δ
ｙ「を順次に積分して行き、その積分値を最新の指令位
置Ｘｒ、ｙｊの情報として出力するものである。

なお、ブーム駆動系、アーム駆動系で使用している角度
検出器１４ａ、１５ａはアーム先端点の現在位置を算出
するのに用いている角度検出器１１．１２と共用するこ
とができる。また、ブーム角およびアーム角をフィード
バックしてアーム先端点の現在位置を算出しているため
、ブーム駆動系およびアーム駆動系におけるブーム角お
よびアーム角のフィールパックループは省略し、オーブ
ンループ制御としてもよい。

［光切の効果］ 以上説明したように本発明によれば、アーム先端点の目
標位置を指令し、この目標位置に達するのに必要なブー
ムおよびアームの回ｖＪ速度を算出してその算出結果に
基づいてブームおよびアームの回動速度を制御している
ため、速度外乱が発生してもアームの先端点が目標位置
からずれてしまうようなことはなくなり、アーム先端点
を目標とする軌跡に従って正確に移動させることができ
る。

【図面の簡単な説明】

第１図は本発明の一実施例を示すブロック図、第２図は
指令位置入力装置の詳細構成例を示すブロック図、第３
図は本発明の他の実施例を示すブロック図、第４図はブ
ーム駆動系およびアーム駆動系の一実施例を示すブロッ
ク図、第５図はブーム駆動系およびアーム駆動系の他の
実施例を示すブロック図、第６図は指令位置入力装置の
他の実施例を示すブロック図、第７図は作業はの一例で
あるパワーショベルの一般的憫成を示す図、第８図は従
来の制御ＩＩ装置の構成を示すブロック図、第１図およ
び第１０図は従来の制御装置の動作を説明するための説
明図である。 １・・・ブーム、２・・・アーム、３・・・バケット、
４・・・ブームシリンダ、５・・・アームシリンダ、１
１，１２・・・角度検出器、１３・・・目標回ｖＪ速度
演算装置、１４・・・ブーム駆動系、１５・・・アーム
駆動系、１６・・・指令位置入力装置、１７・・・現在
位置演算装置、１８・・・速度ベクトル演等装置、１９
・・・目標角度演算装置。 第２図 ｙ 第８図

Claims (1)

1. 【特許請求の範囲】 車体の一点に回動可能に取付けられたブームと、ブーム
   の先端に回動可能に取付けられたアームと、アームの先
   端に回動可能に取付けられたバケットとを有する作業機
   において、 ブーム、アームおよびバケットを含む平面を座標平面と
   してアームの先端点のｘ軸方向およびｙ軸方向の位置を
   指令する位置指令手段と、 ブーム角およびアーム角をそれぞれ検出する角度検出手
   段と、 この角度検出手段で検出したブーム角およびアーム角に
   基づいてアームの先端点の現在位置を算出する現在位置
   演算手段と、 前記位置指令手段により設定された指令位置と前記現在
   位置演算手段で算出されたアーム先端点の現在位置とに
   基づきアーム先端点が指令位置へ移動するための速度ベ
   クトルを算出する速度ベクトル演算手段と、 算出された速度ベルクルと前記角度検出手段で検出した
   ブーム角およびアーム角に基づいてブーム目標回動速度
   およびアーム目標回動速度を算出する回動速度演算手段
   と、 算出された回動速度に基づいてブームシリンダおよびア
   ームシリンダへの圧油供給流量を制御する流量制御手段 とを備え、アーム先端点の位置を指令された位置に制御
   する作業機の制御装置。