JP2912986B2 - 作業機の軌跡制御装置 - Google Patents
作業機の軌跡制御装置Info
- Publication number
- JP2912986B2 JP2912986B2 JP30391691A JP30391691A JP2912986B2 JP 2912986 B2 JP2912986 B2 JP 2912986B2 JP 30391691 A JP30391691 A JP 30391691A JP 30391691 A JP30391691 A JP 30391691A JP 2912986 B2 JP2912986 B2 JP 2912986B2
- Authority
- JP
- Japan
- Prior art keywords
- value
- trajectory
- boom
- arm
- bucket
- 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
Description
【０００１】
【産業上の利用分野】本発明は作業機のアームやバケッ
トの所定点を予め定められた軌跡上に移動させるための
作業機の軌跡制御装置に関する。
トの所定点を予め定められた軌跡上に移動させるための
作業機の軌跡制御装置に関する。
【０００２】
【従来の技術】一般に、作業機は、本体、およびこの本
体に可回動に順次連結された複数の作業部材で構成さ
れ、オペレータがこれら複数の作業部材を操作すること
により任意の作業が実施される。このような作業機の構
成を、油圧ショベルを例示して説明する。図５は油圧シ
ョベルの側面図である。図で、１は下部走行体、２は本
体である上部旋回体、３は上部旋回体２に可回動に連結
されたフロント機構を示す。フロント機構３は、上部旋
回体２に点Ｏにおいて可回動に連結されたブーム４とこ
のブーム４を駆動するブームシリンダ４Ｓ、ブーム４に
点Ａにおいて可回動に連結されたアーム５とこのアーム
５を駆動するアームシリンダ５Ｓ、およびアーム５に点
Ｂにおいて可回動に連結されたバケット６とこのバケッ
ト６を駆動するバケットシリンダ６Ｓで構成されてい
る。なお、アーム５には、バケット６に代え、作業の種
類に対応した作業部材が連結される場合がある。図示さ
れていないが、上部旋回体２には油圧ポンプが設置さ
れ、この油圧ポンプからの圧油によりブームシリンダ４
Ｓ、アームシリンダ５Ｓおよびバケットシリンダ６Ｓが
駆動され、それらの駆動速度は、それらの上流側に設け
られ、かつ、オペレータが操作する操作レバーの操作量
に応じて開口面積が制御される各流量制御弁により制御
される。
体に可回動に順次連結された複数の作業部材で構成さ
れ、オペレータがこれら複数の作業部材を操作すること
により任意の作業が実施される。このような作業機の構
成を、油圧ショベルを例示して説明する。図５は油圧シ
ョベルの側面図である。図で、１は下部走行体、２は本
体である上部旋回体、３は上部旋回体２に可回動に連結
されたフロント機構を示す。フロント機構３は、上部旋
回体２に点Ｏにおいて可回動に連結されたブーム４とこ
のブーム４を駆動するブームシリンダ４Ｓ、ブーム４に
点Ａにおいて可回動に連結されたアーム５とこのアーム
５を駆動するアームシリンダ５Ｓ、およびアーム５に点
Ｂにおいて可回動に連結されたバケット６とこのバケッ
ト６を駆動するバケットシリンダ６Ｓで構成されてい
る。なお、アーム５には、バケット６に代え、作業の種
類に対応した作業部材が連結される場合がある。図示さ
れていないが、上部旋回体２には油圧ポンプが設置さ
れ、この油圧ポンプからの圧油によりブームシリンダ４
Ｓ、アームシリンダ５Ｓおよびバケットシリンダ６Ｓが
駆動され、それらの駆動速度は、それらの上流側に設け
られ、かつ、オペレータが操作する操作レバーの操作量
に応じて開口面積が制御される各流量制御弁により制御
される。
【０００３】上記油圧ショベルによる作業は、オペレー
タがブームシリンダ４Ｓ、アームシリンダ５Ｓおよびバ
ケットシリンダ６Ｓを適切に駆動することにより実行さ
れるが、上記のようにフロント機構３がブーム４、アー
ム５、バケット６の３つのリンク機構で構成されている
ので、連結点Ｂまたはバケット６の先端Ｃをオペレータ
の意思どおりに動かすには当該オペレータに相当の熟練
が要求される。特に、地面を所定の軌跡、例えば直線に
沿って掘削する場合、ブーム４の上昇速度とアーム５の
巻込み速度との微妙な整合が必要となり、このような掘
削をオペレータの手動操作によって高精度で行うことは
極めて困難である。このため、従来、所定の軌跡に沿っ
た掘削を自動的に行うことができる軌跡制御装置が提案
されている。
タがブームシリンダ４Ｓ、アームシリンダ５Ｓおよびバ
ケットシリンダ６Ｓを適切に駆動することにより実行さ
れるが、上記のようにフロント機構３がブーム４、アー
ム５、バケット６の３つのリンク機構で構成されている
ので、連結点Ｂまたはバケット６の先端Ｃをオペレータ
の意思どおりに動かすには当該オペレータに相当の熟練
が要求される。特に、地面を所定の軌跡、例えば直線に
沿って掘削する場合、ブーム４の上昇速度とアーム５の
巻込み速度との微妙な整合が必要となり、このような掘
削をオペレータの手動操作によって高精度で行うことは
極めて困難である。このため、従来、所定の軌跡に沿っ
た掘削を自動的に行うことができる軌跡制御装置が提案
されている。
【０００４】図６は従来の作業機の軌跡制御装置のブロ
ック図である。図で、４はブーム、４Ｓはブームシリン
ダ（駆動手段）、５はアーム、５Ｓはアームシリンダ
（駆動手段）を示し、これらは図５に示すものと同一で
ある。４Ｄは上部旋回体２とブーム４との間の相対的回
動角αを検出するブーム回動角検出手段、５Ｄはブーム
４とアーム５との間の相対的回動角βを検出するアーム
回動角検出手段である。７はアーム５上のバケット６と
の連結点Ｂの軌跡を設定する軌跡設定手段であり、当該
軌跡が直線である場合、上部旋回体２とブーム４の連結
点ＯをＸ、Ｙ座標の原点としたときの点ＢのＹ軸方向の
高さｈ０ が設定される。８は点ＢのＸ軸方向の移動速
度Ｖｘ を設定する指令手段である。
ック図である。図で、４はブーム、４Ｓはブームシリン
ダ（駆動手段）、５はアーム、５Ｓはアームシリンダ
（駆動手段）を示し、これらは図５に示すものと同一で
ある。４Ｄは上部旋回体２とブーム４との間の相対的回
動角αを検出するブーム回動角検出手段、５Ｄはブーム
４とアーム５との間の相対的回動角βを検出するアーム
回動角検出手段である。７はアーム５上のバケット６と
の連結点Ｂの軌跡を設定する軌跡設定手段であり、当該
軌跡が直線である場合、上部旋回体２とブーム４の連結
点ＯをＸ、Ｙ座標の原点としたときの点ＢのＹ軸方向の
高さｈ０ が設定される。８は点ＢのＸ軸方向の移動速
度Ｖｘ を設定する指令手段である。
【０００５】１０は前記座標における点Ｂの位置ｘ、ｙ
を演算する位置演算手段、１１は軌跡設定手段６に設定
された高さｈ0 と点Ｂの実際の位置との偏差Δｙを演算
する位置偏差演算手段、１２は位置偏差Δｙを修正する
ための点ＢのＹ軸方向の移動速度Ｖy を演算する速度指
令値演算手段である。１３は当該移動速度Ｖy 、ブーム
回動角α、アーム回動角β、設定値ｈおよび移動速度指
令値Ｖx に基づいてブーム４の回動角速度ωA およびア
ーム５の回動角速度ωB を演算する回動角速度演算手段
である。１４は演算された回動角速度ωA および回動角
速度ωB に基づいて駆動手段４Ｓ、５Ｓを制御する駆動
制御手段であり、前記の各流量制御弁がこれに相当す
る。
を演算する位置演算手段、１１は軌跡設定手段６に設定
された高さｈ0 と点Ｂの実際の位置との偏差Δｙを演算
する位置偏差演算手段、１２は位置偏差Δｙを修正する
ための点ＢのＹ軸方向の移動速度Ｖy を演算する速度指
令値演算手段である。１３は当該移動速度Ｖy 、ブーム
回動角α、アーム回動角β、設定値ｈおよび移動速度指
令値Ｖx に基づいてブーム４の回動角速度ωA およびア
ーム５の回動角速度ωB を演算する回動角速度演算手段
である。１４は演算された回動角速度ωA および回動角
速度ωB に基づいて駆動手段４Ｓ、５Ｓを制御する駆動
制御手段であり、前記の各流量制御弁がこれに相当す
る。
【０００６】上記位置演算手段１０、位置偏差演算手段
１１、速度指令値演算手段１２および回動角速度演算手
段１３の演算を図７を参照して説明する。図７は上記点
Ｏを原点とするＸ、Ｙ座標を示す。Ｌ１ は点Ｏ、Ａ間
の距離、Ｌ２ は点Ａ、Ｂ間の距離、Ｋ０ は高さｈ０
の直線軌跡（Ｘ軸と平行な軌跡）、Ｂ（ｘ、ｙ）は、例
えば、アーム先端付近に設定される所定点であって、バ
ケットとの連結点Ｂの位置を示す。その他の符号は上述
の符号と同じ値を示す。
１１、速度指令値演算手段１２および回動角速度演算手
段１３の演算を図７を参照して説明する。図７は上記点
Ｏを原点とするＸ、Ｙ座標を示す。Ｌ１ は点Ｏ、Ａ間
の距離、Ｌ２ は点Ａ、Ｂ間の距離、Ｋ０ は高さｈ０
の直線軌跡（Ｘ軸と平行な軌跡）、Ｂ（ｘ、ｙ）は、例
えば、アーム先端付近に設定される所定点であって、バ
ケットとの連結点Ｂの位置を示す。その他の符号は上述
の符号と同じ値を示す。
【０００７】まず、位置演算手段１０はブーム回動角α
およびアーム回動角βに基づいて点Ｂの位置ｘ、ｙを次
の（１）、（２）式により演算する。
およびアーム回動角βに基づいて点Ｂの位置ｘ、ｙを次
の（１）、（２）式により演算する。
【０００８】
【数１】
【０００９】
【数２】
【００１０】位置偏差演算手段１１は、上記（１）、
（２）式により求められた点Ｂの位置と軌跡設定手段６
に設定された軌跡を入力して位置偏差Δｙを演算する。
この場合、設定された軌跡のＹ軸方向の距離は次式のよ
うに、
（２）式により求められた点Ｂの位置と軌跡設定手段６
に設定された軌跡を入力して位置偏差Δｙを演算する。
この場合、設定された軌跡のＹ軸方向の距離は次式のよ
うに、
【００１１】
【数３】
【００１２】であるから、位置偏差Δｙは、
【００１３】
【数４】
【００１４】となる。速度指令値演算手段１２は、上記
位置偏差Δｙを解消するため点Ｂが移動すべきＹ軸方向
の移動速度Ｖy を次式により求める。
位置偏差Δｙを解消するため点Ｂが移動すべきＹ軸方向
の移動速度Ｖy を次式により求める。
【００１５】
【数５】
【００１６】上記（５）式中、Ｋy は予め定められた正
の係数である。
の係数である。
【００１７】回動角速度演算手段１３は、速度指令値演
算手段１２の演算値Ｖy 、ブーム回動角検出手段４Ｄの
検出値α、アーム回動角検出手段５Ｄの検出値βおよび
指令手段７に設定された点ＢのＸ軸方向の移動速度Ｖx
を使用して、ブーム回動角速度ωA およびアーム回動角
速度ωB を演算する。まず、（１）式および（２）式を
それぞれ時間微分すると、
算手段１２の演算値Ｖy 、ブーム回動角検出手段４Ｄの
検出値α、アーム回動角検出手段５Ｄの検出値βおよび
指令手段７に設定された点ＢのＸ軸方向の移動速度Ｖx
を使用して、ブーム回動角速度ωA およびアーム回動角
速度ωB を演算する。まず、（１）式および（２）式を
それぞれ時間微分すると、
【００１８】
【数６】
【００１９】
【数７】
【００２０】が得られる。次いで、上記（６）、（７）
式を値ωA および値ωB について解くと、
式を値ωA および値ωB について解くと、
【００２１】
【数８】
【００２２】
【数９】
【００２３】となり、これによりブーム回動角速度ωA
およびアーム回動角速度ωB を得ることができる。
およびアーム回動角速度ωB を得ることができる。
【００２４】以上のように、常時、ブームシリンダ（駆
動手段）４Ｓの流量制御弁の開口を、演算されたブーム
回動角速度ωA に比例した大きさに制御し、同時に、ア
ームシリンダ（駆動手段）５Ｓの流量制御弁の開口を、
演算されたアーム回動角速度ωB に比例した大きさに制
御することにより、点Ｂは所定の軌跡Ｋ0 上を移動する
ことになり、何等高度の熟練技術を要することなく、油
圧ショベルによる直線状の掘削を行うことができる。な
お、この場合、バケット６のアーム５に対する相対角度
は掘削に都合の良い所定の角度に固定されている。
動手段）４Ｓの流量制御弁の開口を、演算されたブーム
回動角速度ωA に比例した大きさに制御し、同時に、ア
ームシリンダ（駆動手段）５Ｓの流量制御弁の開口を、
演算されたアーム回動角速度ωB に比例した大きさに制
御することにより、点Ｂは所定の軌跡Ｋ0 上を移動する
ことになり、何等高度の熟練技術を要することなく、油
圧ショベルによる直線状の掘削を行うことができる。な
お、この場合、バケット６のアーム５に対する相対角度
は掘削に都合の良い所定の角度に固定されている。
【００２５】
【発明が解決しようとする課題】上記油圧ショベルの軌
跡制御装置においては、ブーム回動角速度ωA およびア
ーム回動角速度ωB は直交座標系における点ＢのＹ軸方
向の位置偏差Δｙを解消するように演算されるため、ア
ームの姿勢が変化するとブームの回動角速度がこれに大
きく影響されて追随できなくなり、点Ｂを軌跡Ｋ0 に沿
って移動させることが困難となる。それ故、所定点の一
回の移動の全域にわたって精度の良い掘削を行うために
は、ゲインや作業速度を著しく低く設定しなければなら
ず、作業効率が大幅に損なわれていた。
跡制御装置においては、ブーム回動角速度ωA およびア
ーム回動角速度ωB は直交座標系における点ＢのＹ軸方
向の位置偏差Δｙを解消するように演算されるため、ア
ームの姿勢が変化するとブームの回動角速度がこれに大
きく影響されて追随できなくなり、点Ｂを軌跡Ｋ0 に沿
って移動させることが困難となる。それ故、所定点の一
回の移動の全域にわたって精度の良い掘削を行うために
は、ゲインや作業速度を著しく低く設定しなければなら
ず、作業効率が大幅に損なわれていた。
【００２６】本発明の目的は、上記従来技術における課
題を解決し、作業効率を向上させることができる作業機
の軌跡制御装置を提供するにある。
題を解決し、作業効率を向上させることができる作業機
の軌跡制御装置を提供するにある。
【００２７】
【課題を解決するための手段】上記の目的を達成するた
め、本発明は、少なくともブームおよびその駆動手段な
らびにアームおよびその駆動手段を有するフロント機構
と、前記ブームの回動角を検出する第１の検出手段と、
前記アームの回動角を検出する第２の検出手段と、前記
アームの所定点の軌跡を設定する軌跡設定手段と、前記
所定点の前記軌跡に沿う方向の速度を指令する速度指令
手段と、前記所定点を前記軌跡に沿って移動させるよう
に前記第１の検出手段の検出値、前記第２の検出手段の
検出値、前記軌跡設定手段の設定値および前記速度指令
手段の指令値に基づいて前記ブームの駆動手段および前
記アームの駆動手段に指令すべき回動角速度を演算する
演算部とを備えた作業機の軌跡制御装置において、当該
演算部を、前記第１の検出手段の検出値、前記第２の検
出手段の検出値および前記軌跡設定手段の設定値に基づ
いてぜきブームおよび前記アームの現在姿勢における前
記所定点が前記軌跡上に位置するときの前記ブームの回
動角と現在の前記ブームの回動角との偏差を演算する第
１の演算手段と、この第１の演算手段の演算値に基づい
て前記ブームの駆動手段に指令する回動角速度を演算す
る第２の演算手段と、前記第１の検出手段の検出値、前
記第２の検出手段の検出値、前記軌跡設定手段の設定
値、前記速度指令手段の指令値および前記第２の演算手
段の演算値に基づいて前記アームの駆動手段に指令する
回動角速度を演算する第３の演算手段とで構成すること
を特徴とする。
め、本発明は、少なくともブームおよびその駆動手段な
らびにアームおよびその駆動手段を有するフロント機構
と、前記ブームの回動角を検出する第１の検出手段と、
前記アームの回動角を検出する第２の検出手段と、前記
アームの所定点の軌跡を設定する軌跡設定手段と、前記
所定点の前記軌跡に沿う方向の速度を指令する速度指令
手段と、前記所定点を前記軌跡に沿って移動させるよう
に前記第１の検出手段の検出値、前記第２の検出手段の
検出値、前記軌跡設定手段の設定値および前記速度指令
手段の指令値に基づいて前記ブームの駆動手段および前
記アームの駆動手段に指令すべき回動角速度を演算する
演算部とを備えた作業機の軌跡制御装置において、当該
演算部を、前記第１の検出手段の検出値、前記第２の検
出手段の検出値および前記軌跡設定手段の設定値に基づ
いてぜきブームおよび前記アームの現在姿勢における前
記所定点が前記軌跡上に位置するときの前記ブームの回
動角と現在の前記ブームの回動角との偏差を演算する第
１の演算手段と、この第１の演算手段の演算値に基づい
て前記ブームの駆動手段に指令する回動角速度を演算す
る第２の演算手段と、前記第１の検出手段の検出値、前
記第２の検出手段の検出値、前記軌跡設定手段の設定
値、前記速度指令手段の指令値および前記第２の演算手
段の演算値に基づいて前記アームの駆動手段に指令する
回動角速度を演算する第３の演算手段とで構成すること
を特徴とする。
【００２８】さらに、本発明は、上記作業機の軌跡制御
装置において、前記アームに可回動に連結されるバケッ
トおよびその駆動手段と、前記バケットの回動角を検出
する第３の検出手段と、前記バケットに対する予め決め
られた角度を設定するバケット角度設定手段と、このバ
ケット角度設定手段の設定値に基づいて前記軌跡設定手
段の出力値を前記バケットの所定点の軌跡に関連した値
とする第４の演算手段と、前記第２の演算手段の演算
値、前記第３の演算手段の演算値、前記第１の検出手段
の検出値、前記第２の検出手段の検出値、前記第３の検
出手段の検出値および前記バケット角度設定手段の設定
値に基づいて前記バケットの駆動手段に指令する回動角
速度を演算する第５の演算手段とを設けたことを特徴と
する。
装置において、前記アームに可回動に連結されるバケッ
トおよびその駆動手段と、前記バケットの回動角を検出
する第３の検出手段と、前記バケットに対する予め決め
られた角度を設定するバケット角度設定手段と、このバ
ケット角度設定手段の設定値に基づいて前記軌跡設定手
段の出力値を前記バケットの所定点の軌跡に関連した値
とする第４の演算手段と、前記第２の演算手段の演算
値、前記第３の演算手段の演算値、前記第１の検出手段
の検出値、前記第２の検出手段の検出値、前記第３の検
出手段の検出値および前記バケット角度設定手段の設定
値に基づいて前記バケットの駆動手段に指令する回動角
速度を演算する第５の演算手段とを設けたことを特徴と
する。
【００２９】
【作用】第１の演算手段では、第１の検出手段で検出さ
れたブーム回動角と、第２の検出手段で検出されたアー
ム回動角とに基づいて、ブームの作業機本体との連結点
とアームの前記所定点とを結ぶ線の距離および当該線の
角度を算出し、これら距離、角度および軌跡設定手段の
設定値から、ブームおよびアームの現在姿勢において前
記所定点が軌跡上に位置するときのブームの回動角と現
在の前記ブームの回動角との偏差を演算する。第２の演
算手段は、この第１の演算手段の演算値に基づいてブー
ムの駆動手段に指令する回動角速度を演算する。第３の
演算手段は、第１の検出手段の検出値、第２の検出手段
の検出値、軌跡設定手段の設定値、速度指令手段の指令
値および第２の演算手段の演算値に基づいてアームの駆
動手段に指令する回動角速度を演算する。
れたブーム回動角と、第２の検出手段で検出されたアー
ム回動角とに基づいて、ブームの作業機本体との連結点
とアームの前記所定点とを結ぶ線の距離および当該線の
角度を算出し、これら距離、角度および軌跡設定手段の
設定値から、ブームおよびアームの現在姿勢において前
記所定点が軌跡上に位置するときのブームの回動角と現
在の前記ブームの回動角との偏差を演算する。第２の演
算手段は、この第１の演算手段の演算値に基づいてブー
ムの駆動手段に指令する回動角速度を演算する。第３の
演算手段は、第１の検出手段の検出値、第２の検出手段
の検出値、軌跡設定手段の設定値、速度指令手段の指令
値および第２の演算手段の演算値に基づいてアームの駆
動手段に指令する回動角速度を演算する。
【００３０】さらに、アームにバケットが連結されてい
る場合には、バケットに対して予め決められた角度を設
定し、この設定された角度に基づいて第４の演算手段に
より軌跡設定手段の出力値をバケットの所定点の軌跡に
関連した値とし、この値を上記第１の演算手段に入力す
るようにし、第５の演算手段により、第２の演算手段の
演算値、第３の演算手段の演算値、第１の検出手段の検
出値、第２の検出手段の検出値、バケット回動角および
バケット角度の設定値に基づいてバケットの駆動手段に
指令する回動角速度を演算する。
る場合には、バケットに対して予め決められた角度を設
定し、この設定された角度に基づいて第４の演算手段に
より軌跡設定手段の出力値をバケットの所定点の軌跡に
関連した値とし、この値を上記第１の演算手段に入力す
るようにし、第５の演算手段により、第２の演算手段の
演算値、第３の演算手段の演算値、第１の検出手段の検
出値、第２の検出手段の検出値、バケット回動角および
バケット角度の設定値に基づいてバケットの駆動手段に
指令する回動角速度を演算する。
【００３１】
【実施例】以下、本発明を図示の実施例に基づいて説明
する。図１は本発明の実施例に係る作業機の軌跡制御装
置のブロック図である。図で、図６に示す部分と同一又
は等価な部分には同一符号を付して説明を省略する。２
０は角度偏差演算手段であり、ブーム回動角α、アーム
回動角βおよび軌跡設定手段７の設定値ｙに基づいて、
現在のブーム４およびアーム５と同一姿勢において点Ｂ
が軌跡Ｋ0 に位置するときのブーム回動角と、現在のブ
ーム回動角との偏差Δαを演算する。２１は当該偏差Δ
αを解消するべきブームの回動角速度ωA を演算するブ
ーム回動角速度指令値演算手段である。２２はブームの
回動角速度ωA 、ブーム回動角α、アーム回動角β、軌
跡設定値ｙおよび速度指令値ＶX に基づいてアームの回
動角速度ωB を演算するアーム回動角速度指令値演算手
段である。
する。図１は本発明の実施例に係る作業機の軌跡制御装
置のブロック図である。図で、図６に示す部分と同一又
は等価な部分には同一符号を付して説明を省略する。２
０は角度偏差演算手段であり、ブーム回動角α、アーム
回動角βおよび軌跡設定手段７の設定値ｙに基づいて、
現在のブーム４およびアーム５と同一姿勢において点Ｂ
が軌跡Ｋ0 に位置するときのブーム回動角と、現在のブ
ーム回動角との偏差Δαを演算する。２１は当該偏差Δ
αを解消するべきブームの回動角速度ωA を演算するブ
ーム回動角速度指令値演算手段である。２２はブームの
回動角速度ωA 、ブーム回動角α、アーム回動角β、軌
跡設定値ｙおよび速度指令値ＶX に基づいてアームの回
動角速度ωB を演算するアーム回動角速度指令値演算手
段である。
【００３２】上記角度偏差演算手段２０、ブーム回動角
速度指令値演算手段２１およびアーム回動角速度指令値
演算手段２２の演算を図２を参照して説明する。図２で
図７と同一部分には同一符号が付してある。ＯＢは点Ｏ
と点Ｂとを結ぶ線、ＯＢ0 はブーム４およびアーム５が
図示の姿勢にあるとき点Ｂを軌跡Ｋ0 上に位置せしめた
場合の点Ｏと点Ｂとを結ぶ線、Ｌは線ＯＢ0 （＝ＯＢ）
の長さ、θは線ＯＢの角度、Δθは線ＯＢと線ＯＢ0と
の間の角度偏差を示す。
速度指令値演算手段２１およびアーム回動角速度指令値
演算手段２２の演算を図２を参照して説明する。図２で
図７と同一部分には同一符号が付してある。ＯＢは点Ｏ
と点Ｂとを結ぶ線、ＯＢ0 はブーム４およびアーム５が
図示の姿勢にあるとき点Ｂを軌跡Ｋ0 上に位置せしめた
場合の点Ｏと点Ｂとを結ぶ線、Ｌは線ＯＢ0 （＝ＯＢ）
の長さ、θは線ＯＢの角度、Δθは線ＯＢと線ＯＢ0と
の間の角度偏差を示す。
【００３３】角度偏差演算手段２０は次の演算により長
さＬと角度θを求め、これらからブーム角度偏差Δθを
演算する。まず、長さＬは（１）式および（２）式から
次式で求めることができる。
さＬと角度θを求め、これらからブーム角度偏差Δθを
演算する。まず、長さＬは（１）式および（２）式から
次式で求めることができる。
【００３４】
【数１０】
【００３５】又、角度θも（１）式および（２）式から
次式で求めることができる。
次式で求めることができる。
【００３６】
【数１１】
【００３７】上記（１０）、（１１）式からブーム角度
偏差Δαは、
偏差Δαは、
【００３８】
【数１２】
【００３９】となる。
【００４０】このようにして、現在時点におけるブーム
角度偏差Δαが求められると、次に、ブーム回動角速度
指令値演算手段２１は当該ブーム角度偏差Δαを解消す
るためのブーム４の回動角速度を次式により演算する。
角度偏差Δαが求められると、次に、ブーム回動角速度
指令値演算手段２１は当該ブーム角度偏差Δαを解消す
るためのブーム４の回動角速度を次式により演算する。
【００４１】
【数１３】
【００４２】上記（１３）式中、値ＫA は予め定められ
た正の係数である。（１３）式によりブーム４の姿勢を
修正する回動角速度ωA が求められると、アーム回動角
速度指令値演算手段２２は回動角速度ωA に応じるアー
ム回動角速度ωB を次式により演算する。
た正の係数である。（１３）式によりブーム４の姿勢を
修正する回動角速度ωA が求められると、アーム回動角
速度指令値演算手段２２は回動角速度ωA に応じるアー
ム回動角速度ωB を次式により演算する。
【００４３】
【数１４】
【００４４】（１３）、（１４）式により得られたブー
ム回動角速度ωA およびアーム回動角速度ωB を用いて
駆動制御手段１４が制御され、ブームシリンダ（駆動手
段）４Ｓの流量制御弁およびアームシリンダ（駆動手
段）５Ｓの流量制御弁の各開口面積が値ωA 、ωB に応
じて制御される。
ム回動角速度ωA およびアーム回動角速度ωB を用いて
駆動制御手段１４が制御され、ブームシリンダ（駆動手
段）４Ｓの流量制御弁およびアームシリンダ（駆動手
段）５Ｓの流量制御弁の各開口面積が値ωA 、ωB に応
じて制御される。
【００４５】このように本実施例では、ブームとアーム
の現在姿勢におけるブームの回動角偏差を解消するよう
にし、これに応じてアームの回動角速度を決定するよう
にしたので、ブームはアームの姿勢に影響されることは
なく、ゲインや作業速度を低く抑えなくても精度良く掘
削を行うことができ、作業効率を向上させることができ
る。又、内部の座標系を常に掘削面を基準として設定す
ることができるので、掘削面がいかなる傾斜角であって
も、点Ｂと軌跡との偏差をほぼ０とすることができ、精
度の良い斜面直線掘削を行うことができる。
の現在姿勢におけるブームの回動角偏差を解消するよう
にし、これに応じてアームの回動角速度を決定するよう
にしたので、ブームはアームの姿勢に影響されることは
なく、ゲインや作業速度を低く抑えなくても精度良く掘
削を行うことができ、作業効率を向上させることができ
る。又、内部の座標系を常に掘削面を基準として設定す
ることができるので、掘削面がいかなる傾斜角であって
も、点Ｂと軌跡との偏差をほぼ０とすることができ、精
度の良い斜面直線掘削を行うことができる。
【００４６】図３は本発明の他の実施例に係る作業機の
軌跡制御装置のブロック図である。図で、図１に示す部
分と同一又は等価な部分には同一符号を付して説明を省
略する。６はバケット、６Ｓはバケットシリンダ（駆動
手段）であり、図５に示すものと同じである。６Ｄはバ
ケット６の回動角を検出するバケット回動角検出手段、
９はバケット回動角を所定の角度に設定するバケット角
設定手段である。２３はブーム回動角速度指令値演算手
段２１の演算値ωA 、アーム回動角速度指令値演算手段
２２の演算値ωB 、ブーム回動角α、アーム回動角β、
およびバケット回動角γに基づいてバケット回動角速度
ωC を演算するバケット回動角速度指令値演算手段であ
る。
軌跡制御装置のブロック図である。図で、図１に示す部
分と同一又は等価な部分には同一符号を付して説明を省
略する。６はバケット、６Ｓはバケットシリンダ（駆動
手段）であり、図５に示すものと同じである。６Ｄはバ
ケット６の回動角を検出するバケット回動角検出手段、
９はバケット回動角を所定の角度に設定するバケット角
設定手段である。２３はブーム回動角速度指令値演算手
段２１の演算値ωA 、アーム回動角速度指令値演算手段
２２の演算値ωB 、ブーム回動角α、アーム回動角β、
およびバケット回動角γに基づいてバケット回動角速度
ωC を演算するバケット回動角速度指令値演算手段であ
る。
【００４７】本実施例の演算を図４を参照して説明す
る。図４で図２と同一部分には同一符号が付してある。
Ｃは、バケットの所定点であり、本実施例では図５に示
すバケット６の先端、Ｌ３ は点Ｂと点Ｃとの間の長
さ、Ｋ１ は点Ｃの軌跡、γはバケット角度、ψはＸ軸
方向に対する直線ＢＣの角度、Ｌ３ｙは直線ＢＣのＹ軸
方向の長さである。バケット６は角度ψに固定され、こ
の角度ψがバケット角設定手段９に設定される。角度ψ
が一定であるので、長さＬ３ｙは、
る。図４で図２と同一部分には同一符号が付してある。
Ｃは、バケットの所定点であり、本実施例では図５に示
すバケット６の先端、Ｌ３ は点Ｂと点Ｃとの間の長
さ、Ｋ１ は点Ｃの軌跡、γはバケット角度、ψはＸ軸
方向に対する直線ＢＣの角度、Ｌ３ｙは直線ＢＣのＹ軸
方向の長さである。バケット６は角度ψに固定され、こ
の角度ψがバケット角設定手段９に設定される。角度ψ
が一定であるので、長さＬ３ｙは、
【００４８】
【数１５】
【００４９】で表され、一定である。
【００５０】軌跡設定手段７は上記（１５）式とバケッ
ト先端Ｃの軌跡、
ト先端Ｃの軌跡、
【００５１】
【数１６】
【００５２】から、点Ｂの軌跡を次式の演算により、
【００５３】
【数１７】
【００５４】とする。角度偏差演算手段２０およびブー
ム回動角速度指令値演算手段２１では、（１７）式で得
られた軌跡ｙを用いてさきの実施例と同じくブーム角度
偏差Δαおよびブーム回動角速度ωA を算出する。
ム回動角速度指令値演算手段２１では、（１７）式で得
られた軌跡ｙを用いてさきの実施例と同じくブーム角度
偏差Δαおよびブーム回動角速度ωA を算出する。
【００５５】上記バケット回動角速度指令値演算手段２
３はバケット回動角速度ωC を次のように演算する。ま
ず、角度ψとブーム回動角速度α、アーム回動角速度β
およびバケット回動角速度γとの間には次の関係があ
る。
３はバケット回動角速度ωC を次のように演算する。ま
ず、角度ψとブーム回動角速度α、アーム回動角速度β
およびバケット回動角速度γとの間には次の関係があ
る。
【００５６】
【数１８】
【００５７】この式の両辺を時間微分すると、
【００５８】
【数１９】
【００５９】となる。ただし、ωC ＝ｄψ／ｄｔ であ
る。しかし、実際には掘削反力等の外乱が加わるため、
角度ψには次式のような角度偏差Δψが生じる。
る。しかし、実際には掘削反力等の外乱が加わるため、
角度ψには次式のような角度偏差Δψが生じる。
【００６０】
【数２０】
【００６１】正確なブーム回動角速度ωC はこのような
角度偏差Δψを補償した次式により得られる。
角度偏差Δψを補償した次式により得られる。
【００６２】
【数２１】
【００６３】上記（２０）式中、ＫC は正の係数であ
る。駆動制御手段１４には、ブーム回動角速度ωA 、ア
ーム回動角速度ωB およびバケット回動角速度ωC が入
力され、これに応じてブーム４、アーム５およびバケッ
ト６が駆動され、点Ｂが軌跡Ｋ0 上を、又は点Ｃが軌跡
Ｋ1 上を移動することとなる。
る。駆動制御手段１４には、ブーム回動角速度ωA 、ア
ーム回動角速度ωB およびバケット回動角速度ωC が入
力され、これに応じてブーム４、アーム５およびバケッ
ト６が駆動され、点Ｂが軌跡Ｋ0 上を、又は点Ｃが軌跡
Ｋ1 上を移動することとなる。
【００６４】本実施例の効果もさきの実施例の効果と同
じである。なお、上記各実施例の説明では、作業機とし
て油圧ショベルを例示して説明したが、他の作業機に適
用することができるのは当然である。
じである。なお、上記各実施例の説明では、作業機とし
て油圧ショベルを例示して説明したが、他の作業機に適
用することができるのは当然である。
【００６５】
【発明の効果】以上述べたように、本発明では、ブーム
とアーム（又はブーム、アームおよびバケット）の現在
姿勢におけるブームの回動角偏差を解消するようにし、
これに応じてアーム（又はアームとバケット）の回動角
速度を決定するようにしたので、ブームはアーム（又は
アームとバケット）の姿勢に影響されることはなく、ゲ
インや作業速度を低く抑えなくても精度良く掘削を行う
ことができ、作業効率を向上させることができる。
とアーム（又はブーム、アームおよびバケット）の現在
姿勢におけるブームの回動角偏差を解消するようにし、
これに応じてアーム（又はアームとバケット）の回動角
速度を決定するようにしたので、ブームはアーム（又は
アームとバケット）の姿勢に影響されることはなく、ゲ
インや作業速度を低く抑えなくても精度良く掘削を行う
ことができ、作業効率を向上させることができる。
【００６６】又、内部の座標系を常に軌跡を基準として
設定することができるので、軌跡がいかなる傾斜角をも
っていても、点Ｂと軌跡との偏差をほぼ０とすることが
でき、精度の良い軌跡制御を行うことができる。
設定することができるので、軌跡がいかなる傾斜角をも
っていても、点Ｂと軌跡との偏差をほぼ０とすることが
でき、精度の良い軌跡制御を行うことができる。
【図１】本発明の実施例に係る作業機の軌跡制御装置の
ブロック図である。
ブロック図である。
【図２】図１に示す装置の動作を説明する線図である。
【図３】本発明の他の実施例に係る作業機の軌跡制御装
置のブロック図である。
置のブロック図である。
【図４】図３に示す装置の動作を説明する線図である。
【図５】油圧ショベルの側面図である。
【図６】従来の作業機の軌跡制御装置のブロック図であ
る。
る。
【図７】図６に示す装置の動作を説明する線図である。
４ ブーム ５ アーム ６ バケット ４Ｓ、５Ｓ、６Ｓ 駆動手段 ４Ｄ ブーム回動角検出手段 ５Ｄ アーム回動角検出手段 ６Ｄ バケット回動角検出手段 ７ 軌跡設定手段 ８ 指令手段 ９ バケット角設定手段 ２０ 角度偏差演算手段 ２１ ブーム回動角速度指令値演算手段 ２２ アーム回動角速度指令値演算手段 ２３ バケット回動角速度指令値演算手段
Claims (2)
- 【請求項１】 少なくともブームおよびその駆動手段な
らびにアームおよびその駆動手段を有するフロント機構
と、前記ブームの回動角を検出する第１の検出手段と、
前記アームの回動角を検出する第２の検出手段と、前記
アームの所定点の軌跡を設定する軌跡設定手段と、前記
所定点の前記軌跡に沿う方向の速度を指令する速度指令
手段と、前記所定点を前記軌跡に沿って移動させるよう
に前記第１の検出手段の検出値、前記第２の検出手段の
検出値、前記軌跡設定手段の設定値および前記速度指令
手段の指令値に基づいて前記ブームの駆動手段および前
記アームの駆動手段に指令すべき回動角速度を演算する
演算部とを備えた作業機の軌跡制御装置において、当該
演算部を、前記第１の検出手段の検出値、前記第２の検
出手段の検出値および前記軌跡設定手段の設定値に基づ
いて前記ブームおよび前記アームの現在姿勢における前
記所定点が前記軌跡上に位置するときの前記ブームの回
動角と現在の前記ブームの回動角との偏差を演算する第
１の演算手段と、この第１の演算手段の演算値に基づい
て前記ブームの駆動手段に指令する回動角速度を演算す
る第２の演算手段と、前記第１の検出手段の検出値、前
記第２の検出手段の検出値、前記軌跡設定手段の設定
値、前記速度指令手段の指令値および前記第２の演算手
段の演算値に基づいて前記アームの駆動手段に指令する
回動角速度を演算する第３の演算手段とで構成すること
を特徴とする作業機の軌跡制御装置。 - 【請求項２】 請求項１記載の作業機の軌跡制御装置に
おいて、前記アームに可回動に連結されるバケットおよ
びその駆動手段と、前記バケットの回動角を検出する第
３の検出手段と、前記バケットに対する予め決められた
角度を設定するバケット角度設定手段と、このバケット
角度設定手段の設定値に基づいて前記軌跡設定手段の出
力値を前記バケットの所定点の軌跡に関連した値とする
第４の演算手段と、前記第２の演算手段の演算値、前記
第３の演算手段の演算値、前記第１の検出手段の検出
値、前記第２の検出手段の検出値、前記第３の検出手段
の検出値および前記バケット角度設定手段の設定値に基
づいて前記バケットの駆動手段に指令する回動角速度を
演算する第５の演算手段とを設けたことを特徴とする作
業機の軌跡制御装置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP30391691A JP2912986B2 (ja) | 1991-10-24 | 1991-10-24 | 作業機の軌跡制御装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP30391691A JP2912986B2 (ja) | 1991-10-24 | 1991-10-24 | 作業機の軌跡制御装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JPH05112971A JPH05112971A (ja) | 1993-05-07 |
| JP2912986B2 true JP2912986B2 (ja) | 1999-06-28 |
Family
ID=17926819
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP30391691A Expired - Fee Related JP2912986B2 (ja) | 1991-10-24 | 1991-10-24 | 作業機の軌跡制御装置 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP2912986B2 (ja) |
Families Citing this family (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH07197485A (ja) * | 1993-12-28 | 1995-08-01 | Komatsu Ltd | 建設機械の作業機制御装置 |
| CN102947513B (zh) * | 2010-06-23 | 2015-07-08 | 斗山英维高株式会社 | 建筑机械的作业轨迹控制装置及其方法 |
| JP7237792B2 (ja) * | 2019-10-03 | 2023-03-13 | 日立建機株式会社 | 建設機械 |
| CN112925329A (zh) * | 2021-02-01 | 2021-06-08 | 上海三一重机股份有限公司 | 挖掘机作业轨迹规划方法及装置 |
-
1991
- 1991-10-24 JP JP30391691A patent/JP2912986B2/ja not_active Expired - Fee Related
Also Published As
| Publication number | Publication date |
|---|---|
| JPH05112971A (ja) | 1993-05-07 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| KR910009255B1 (ko) | 파워샤벨의 제어장치 | |
| US4910673A (en) | Apparatus for controlling arm movement of industrial vehicle | |
| JP2002167794A (ja) | 油圧ショベルのフロント制御装置 | |
| JP2912986B2 (ja) | 作業機の軌跡制御装置 | |
| JPH076212B2 (ja) | パワ−シヨベルの位置制御装置 | |
| JPS6237183B2 (ja) | ||
| WO2022163168A1 (ja) | 作業機械 | |
| EP0293057B1 (en) | Apparatus for controlling arm movement of industrial vehicle | |
| JPH083187B2 (ja) | パワ−シヨベルのバケツト角度制御装置 | |
| JP2631757B2 (ja) | 建設機械の掘削制御方法 | |
| JPH02101229A (ja) | 作業機の制御装置 | |
| JPH0823155B2 (ja) | 作業機の制御装置 | |
| JPH076209B2 (ja) | パワ−シヨベルにおける作業機の制御装置 | |
| JPH0681579A (ja) | シールド掘進機の自動方向制御装置 | |
| JP2983783B2 (ja) | 作業機の定速軌跡制御装置 | |
| EP4187022A1 (en) | Work machine | |
| JPH0776453B2 (ja) | 作業機の軌跡制御装置 | |
| JP2601865B2 (ja) | 作業機の軌跡制御装置 | |
| JPS63194030A (ja) | パワ−シヨベルの作業機制御方法および装置 | |
| JPH0559745A (ja) | 腕式作業機の作業具軌跡制御装置 | |
| JPH0788674B2 (ja) | パワ−シヨベルの作業機制御装置 | |
| JPH07107276B2 (ja) | 作業機の制御装置 | |
| JPH0791844B2 (ja) | 作業機の制御装置 | |
| JP2699097B2 (ja) | 建設機械のバケット刃先深さ制御装置 | |
| JPS6145025B2 (ja) |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| LAPS | Cancellation because of no payment of annual fees |