JP2704729B2 - 作業機の制御装置 - Google Patents
作業機の制御装置Info
- Publication number
- JP2704729B2 JP2704729B2 JP16663488A JP16663488A JP2704729B2 JP 2704729 B2 JP2704729 B2 JP 2704729B2 JP 16663488 A JP16663488 A JP 16663488A JP 16663488 A JP16663488 A JP 16663488A JP 2704729 B2 JP2704729 B2 JP 2704729B2
- Authority
- JP
- Japan
- Prior art keywords
- arm
- boom
- transfer function
- command
- axis
- 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
【発明の詳細な説明】 〔産業上の利用分野〕 この発明は、作業機としてブーム、アームおよびバケ
ットを具えた建設機械のパワーショベルあるいはロボッ
ト等において、直線軌跡精度を向上させるための技術に
関する。
ットを具えた建設機械のパワーショベルあるいはロボッ
ト等において、直線軌跡精度を向上させるための技術に
関する。
一般に油圧式パワーショベルは第８図に示すようにブ
ーム1,アーム2,バケット３及びこれらを駆動するブーム
シリンダ4,アームシリンダ5,パケットシリンダ６を具備
しており、各シリンダ４〜６は夫々運転室内に配備され
た操作レバー（図示せず）により手動操作される。そし
て、オペレータが手動操作により作業機の姿勢を制御し
てバケット位置を設定するようになっている。
ーム1,アーム2,バケット３及びこれらを駆動するブーム
シリンダ4,アームシリンダ5,パケットシリンダ６を具備
しており、各シリンダ４〜６は夫々運転室内に配備され
た操作レバー（図示せず）により手動操作される。そし
て、オペレータが手動操作により作業機の姿勢を制御し
てバケット位置を設定するようになっている。
かかる油圧式パワーショベルにおける作業には、バケ
ット３を固定し、ブーム１およびアーム２のみを回動さ
せてバケット３の刃先点に直線軌跡を描かせる直線掘削
作業がある。
ット３を固定し、ブーム１およびアーム２のみを回動さ
せてバケット３の刃先点に直線軌跡を描かせる直線掘削
作業がある。
この直線掘削作業での直線軌跡精度を向上させるため
に特願昭61−30240号では、第９図に示すように、Ｘ座
標と、Ｙ座標とを指令値にフィードバックするようにし
ている。
に特願昭61−30240号では、第９図に示すように、Ｘ座
標と、Ｙ座標とを指令値にフィードバックするようにし
ている。
すなわち、作業機各部の位置、角度等を以下のように
定義する（第10図参照）。
定義する（第10図参照）。
原点Ａ（0,0）：ブーム１の回動支点 点B:アーム２の回動支点 点Ｃ（Xa,Ya）：バケット３の回動支点（アーム先端
点） Ｘ軸：点A,B,C,Dを含む平面と点Ａを含む車輪旋回面
との交線 Ｙ軸：点Ａを通り、車輪旋回面に垂直な直線 l1 ：点A,B間の長さ l2 ：点B,C間の長さ α:Y軸に対する線分ABのなす角（ブーム角） β：線分ABに対する線分BCのなす角（アーム角） 第９図の指令位置入力装置10は、ブーム１、アーム２
およびバケット３を含む平面を座標平面としてアーム先
端点ＣのＸ軸方向およびＹ軸方向の位置指令（Xr,Yr）
を発生するものである。
点） Ｘ軸：点A,B,C,Dを含む平面と点Ａを含む車輪旋回面
との交線 Ｙ軸：点Ａを通り、車輪旋回面に垂直な直線 l1 ：点A,B間の長さ l2 ：点B,C間の長さ α:Y軸に対する線分ABのなす角（ブーム角） β：線分ABに対する線分BCのなす角（アーム角） 第９図の指令位置入力装置10は、ブーム１、アーム２
およびバケット３を含む平面を座標平面としてアーム先
端点ＣのＸ軸方向およびＹ軸方向の位置指令（Xr,Yr）
を発生するものである。
現在位置演算装置12は、角度検出器13,14でそれぞれ
検出したブーム角αおよびアーム角βとを基に下記第
（１）式によってアーム先端点の現在位置（Xa,Ya）を
算出するもので、その算出結果は速度ベクトル演算装置
15に入力される。
検出したブーム角αおよびアーム角βとを基に下記第
（１）式によってアーム先端点の現在位置（Xa,Ya）を
算出するもので、その算出結果は速度ベクトル演算装置
15に入力される。
速度ベクトル演算装置15は、指令位置入力装置10から
与えられるアーム先端点Ｃの位置指令（Xr,Yr）と現在
位置演算装置12から入力されるアーム先端点の現在位置
（Xa,Ya）とに基づきアーム先端点を指令位置（Xr,Yr）
へ移動させるための速度ベクトルeX,eYを次式（２）に
よって算出する。
与えられるアーム先端点Ｃの位置指令（Xr,Yr）と現在
位置演算装置12から入力されるアーム先端点の現在位置
（Xa,Ya）とに基づきアーム先端点を指令位置（Xr,Yr）
へ移動させるための速度ベクトルeX,eYを次式（２）に
よって算出する。
但し、ＫX,KYは正の係数である。
目標回動速度演算装置16は、アーム先端点の位置を指
令位置（Xr,Yr）に制御するためのブーム１およびアー
ム２の回動速度指令r,ｒを演算するもので、ここで
は前記速度ベクトルeX,eYとブーム角α，アーム角β，
ブーム１の長さl1,アームの長さl2とを基に次式（３）
によって算出する。
令位置（Xr,Yr）に制御するためのブーム１およびアー
ム２の回動速度指令r,ｒを演算するもので、ここで
は前記速度ベクトルeX,eYとブーム角α，アーム角β，
ブーム１の長さl1,アームの長さl2とを基に次式（３）
によって算出する。
Ａ＝−l2 sin（α＋β） Ｂ＝−l2 cos（α＋β） Ｃ＝l1 sin α＋l2 sin（α＋β） Ｄ＝l1 cos α＋l2 cos（α＋β） ブーム駆動系17およびアーム駆動系18は、以上のよう
にして算出されたブーム１の目標回動速度ｒおよびア
ーム２の目標回動速度ｒに対応してブームシリンダ４
およびアームシリンダ５に対する圧油供給給量を制御
し、これによって、ブーム１およびアーム２は目標回動
速度r,ｒに対応した速度で回動し、アーム２の先端
点は指令された位置Xr,Yrに制御される。
にして算出されたブーム１の目標回動速度ｒおよびア
ーム２の目標回動速度ｒに対応してブームシリンダ４
およびアームシリンダ５に対する圧油供給給量を制御
し、これによって、ブーム１およびアーム２は目標回動
速度r,ｒに対応した速度で回動し、アーム２の先端
点は指令された位置Xr,Yrに制御される。
第11図は、前記目標回動速度演算装置16から出力され
たブーム１の目標回動速度ｒおよびアーム２の目標回
動速度ｒによってアーム先端点が実際に位置（Xa,Y
a）に制御されるまでの制御ブロック図を示すもので、
変換系９で先の第（１）式と同じ変換が行なわれること
によりアーム先端点が位置制御される。
たブーム１の目標回動速度ｒおよびアーム２の目標回
動速度ｒによってアーム先端点が実際に位置（Xa,Y
a）に制御されるまでの制御ブロック図を示すもので、
変換系９で先の第（１）式と同じ変換が行なわれること
によりアーム先端点が位置制御される。
すなわち、第10図および第11図示す従来の制御系にお
いては、アーム先端点の位置指令Xr,Yrに対してアーム
先端点の現在位置Xa,Yaをフィードバックすることによ
り速度外乱等の影響を抑えて直線掘削精度を向上させる
ようにしているが、この従来技術ではブーム伝達関数Ｇ
B(S)およびアーム伝達関数ＧA(S)について考慮していな
いので、十分な軌跡精度が得られないという問題があ
る。すなわち、パワーショベルにおいては、ブーム回転
角αおよびアーム回転角βの変化によってアクチュエー
タ（ブームシリンダ、アームシリンダ）に対しての負荷
となる慣性と腕重量が大きく変化するため、これに伴っ
てブーム伝達関数ＧB(S)およびアーム伝達関数ＧA(S)も
常に変化しており、これらに均一な特性は望めない。
いては、アーム先端点の位置指令Xr,Yrに対してアーム
先端点の現在位置Xa,Yaをフィードバックすることによ
り速度外乱等の影響を抑えて直線掘削精度を向上させる
ようにしているが、この従来技術ではブーム伝達関数Ｇ
B(S)およびアーム伝達関数ＧA(S)について考慮していな
いので、十分な軌跡精度が得られないという問題があ
る。すなわち、パワーショベルにおいては、ブーム回転
角αおよびアーム回転角βの変化によってアクチュエー
タ（ブームシリンダ、アームシリンダ）に対しての負荷
となる慣性と腕重量が大きく変化するため、これに伴っ
てブーム伝達関数ＧB(S)およびアーム伝達関数ＧA(S)も
常に変化しており、これらに均一な特性は望めない。
また、この従来技術では、前記（１）式と（３）式の
変換式に示したように、Ｘ座標とＹ座標とが複合されて
いるために、制御系としてはＸ指令値XrがＸ座標のみな
らずＹ座標にも影響を与えるとともに、Ｙ指令値YrがＹ
座標のみならずＸ座標にも影響を与える系になってお
り、このため両座標系の相互干渉によって直線精度が今
一つ向上しないという問題もあった。
変換式に示したように、Ｘ座標とＹ座標とが複合されて
いるために、制御系としてはＸ指令値XrがＸ座標のみな
らずＹ座標にも影響を与えるとともに、Ｙ指令値YrがＹ
座標のみならずＸ座標にも影響を与える系になってお
り、このため両座標系の相互干渉によって直線精度が今
一つ向上しないという問題もあった。
この問題点を解決するためにブーム，アームの伝達関
数ＧB(S),GA(S)をＧB(S)＝ＧA(S)＝Ｇ(S)＝一定の方法
も本発明の中で最初に提案する。しかし、ＧB(S)＝ＧA
(S)＝Ｇ(S)＝一定にする方法によっても第12図に示すよ
うに、水平方向に直線掘削する場合には掘削面に垂直な
方向すなわちＹ方向の定常偏差はゼロになるが、第13図
に示すように、斜面を掘削する場合には掘削面に垂直な
方向の定常偏差は必ずしもゼロにならず斜面を正確に直
線掘削できないという問題もあった。すなわち、ブー
ム、アームの伝達関数ＧB(S),GA(S)をＧB(S)＝ＧA(S)＝
Ｇ(S)一定と仮定すると、 ζ :粘性係数 ωn:固有振動数 ｋ ：ゲイン となり、これによりＸ方向およびＹ方向の定常偏差を求
めると、 σX:X方向の定常偏差 σY:Y方向の定常偏差 vX:掘削速度のＸ成分 vY:掘削速度のＹ成分 となる。
数ＧB(S),GA(S)をＧB(S)＝ＧA(S)＝Ｇ(S)＝一定の方法
も本発明の中で最初に提案する。しかし、ＧB(S)＝ＧA
(S)＝Ｇ(S)＝一定にする方法によっても第12図に示すよ
うに、水平方向に直線掘削する場合には掘削面に垂直な
方向すなわちＹ方向の定常偏差はゼロになるが、第13図
に示すように、斜面を掘削する場合には掘削面に垂直な
方向の定常偏差は必ずしもゼロにならず斜面を正確に直
線掘削できないという問題もあった。すなわち、ブー
ム、アームの伝達関数ＧB(S),GA(S)をＧB(S)＝ＧA(S)＝
Ｇ(S)一定と仮定すると、 ζ :粘性係数 ωn:固有振動数 ｋ ：ゲイン となり、これによりＸ方向およびＹ方向の定常偏差を求
めると、 σX:X方向の定常偏差 σY:Y方向の定常偏差 vX:掘削速度のＸ成分 vY:掘削速度のＹ成分 となる。
したがって、第12図に示すような水平面の掘削のとき
には となるが、第13図に示すような斜面の掘削のときには となり、これにより、掘削面に垂直な方向の定常偏差σ
θは となり、ゼロにはならない。
には となるが、第13図に示すような斜面の掘削のときには となり、これにより、掘削面に垂直な方向の定常偏差σ
θは となり、ゼロにはならない。
この発明はこのような事情に鑑みてなされたもので、
ブームおよびアームの伝達関数を常に等しく一定に保持
することで、Ｘ座標およびＹ座標が干渉せず、高精度の
直線掘削をなし得る作業機の制御装置を提供しようとす
るものである。
ブームおよびアームの伝達関数を常に等しく一定に保持
することで、Ｘ座標およびＹ座標が干渉せず、高精度の
直線掘削をなし得る作業機の制御装置を提供しようとす
るものである。
また、この発明では、斜面掘削を高精度になし得る作
業機の制御装置を提供しようとするものである。
業機の制御装置を提供しようとするものである。
そこでこの発明では、ブーム伝達関数ＧB(S)とアーム
伝達関数ＧA(S)とを等しくかつ一定に保持する補償手段
を設けることにより、ＧA(S)＝ＧB(S)＝Ｇ(S)（一定）
とし、これによりＸ座標系とＹ座標系を完全に分離する
ようにする。
伝達関数ＧA(S)とを等しくかつ一定に保持する補償手段
を設けることにより、ＧA(S)＝ＧB(S)＝Ｇ(S)（一定）
とし、これによりＸ座標系とＹ座標系を完全に分離する
ようにする。
また、この発明では、角度θの斜面を掘削する場合上
記補償手段によってＧB(S)＝ＧA(S)＝Ｇ(S)（一定）と
するとともに、アーム先端点のＸ軸方向およびＹ軸方向
についての位置指令Xr,Yrを斜面の傾斜角θだけ回転し
た内部座標系の位置指令Xr′,Yr′に変換し、この変換
した位置指令Xr′,Yr′と前記θだけ回転した内部座標
系におけるアーム先端点のＸ軸およびＹ軸方向の現在位
置とに基づき算出したブームおよびアームの目標回動速
度（または回動角）に対応してブームシリンダおよびア
ームシリンダを駆動制御することにより、斜面直線掘削
の精度を向上させるようにする。
記補償手段によってＧB(S)＝ＧA(S)＝Ｇ(S)（一定）と
するとともに、アーム先端点のＸ軸方向およびＹ軸方向
についての位置指令Xr,Yrを斜面の傾斜角θだけ回転し
た内部座標系の位置指令Xr′,Yr′に変換し、この変換
した位置指令Xr′,Yr′と前記θだけ回転した内部座標
系におけるアーム先端点のＸ軸およびＹ軸方向の現在位
置とに基づき算出したブームおよびアームの目標回動速
度（または回動角）に対応してブームシリンダおよびア
ームシリンダを駆動制御することにより、斜面直線掘削
の精度を向上させるようにする。
以下この発明を添付図面に示す実施例にしたがって詳
細に説明する。
細に説明する。
第１図にこの発明の第１の実施例を示す。
この第１の実施例では、第９図に示した従来装置に対
し、アーム伝達関数補償器100を付加し、この補償器100
によるフィードバック補償によってアーム伝達関数ＧB
(S)とアーム伝達関数ＧA(S)を下式（７）に示すような
等しい一定値Ｇ(S)に保持するようにしている。
し、アーム伝達関数補償器100を付加し、この補償器100
によるフィードバック補償によってアーム伝達関数ＧB
(S)とアーム伝達関数ＧA(S)を下式（７）に示すような
等しい一定値Ｇ(S)に保持するようにしている。
Ｇ(B)＝ＧA(S)＝Ｇ(S)＝一定 …（７） すなわち、ブーム駆動系17におけるブーム伝達関数Ｇ
B(S)は下式で表示でき（第２図参照） ωn:固有振動数 ζ :粘性係数 また、アーム駆動系18におけるアーム伝達関数ＧA(S)
は式（10）のように近似できる。（第３図参照）。
B(S)は下式で表示でき（第２図参照） ωn:固有振動数 ζ :粘性係数 また、アーム駆動系18におけるアーム伝達関数ＧA(S)
は式（10）のように近似できる。（第３図参照）。
なお、（10）式においてＴは時定数であり、K1,K2,T
は既知で、k1,k2が未知である。
は既知で、k1,k2が未知である。
したがってＧA(S)をＧB(S)と同一値にするためには
（８）式と（10）式より、 が成立し、（11）式を変形すれば となる。
（８）式と（10）式より、 が成立し、（11）式を変形すれば となる。
したがってＧB(S)＝ＧA(S)とするにはアーム伝達関数
ＧA(S)の可変ゲインk1,k2を（12）（13）に従って変化
させればよい。
ＧA(S)の可変ゲインk1,k2を（12）（13）に従って変化
させればよい。
かかるゲイン可変制御を行なうための前記アーム伝達
関数補償器100の具体構成を第４図に示す。
関数補償器100の具体構成を第４図に示す。
アーム伝達関数補償器100にはブーム角度検出器13お
よびアーム角度検出器14からブーム角検出値αおよびア
ーム角検出値βが入力されており、これら検出値α，β
は補償器100内のメモリ20に入力される。メモリ20に
は、ブーム伝達関数ＧB(S)の特性値ωｎおよびζが各種
α，βの値に応じてマップ形式で予め記憶されており、
メモリ20は入力されたα，βに対するωn,ζを出力す
る。上記ωn,ζは例えば実車テストなどでα，βの各種
組合わせに応じて予め測定しておく。メモリ20から出力
されたωn,ζは演算器30に入力され、ここで前記（12）
および（13）式の演算が行なわれることによりアーム伝
達関数ＧA(S)をＧB(S)と等しくする可変ゲイン値k1,k2
がアーム駆動系18に出力される。これによりゲインk1,k
2が可変制御され、アーム伝達関数ＧA(S)はブーム伝達
関数ＧB(S)と等しい一定値Ｇ(S)に保持される。
よびアーム角度検出器14からブーム角検出値αおよびア
ーム角検出値βが入力されており、これら検出値α，β
は補償器100内のメモリ20に入力される。メモリ20に
は、ブーム伝達関数ＧB(S)の特性値ωｎおよびζが各種
α，βの値に応じてマップ形式で予め記憶されており、
メモリ20は入力されたα，βに対するωn,ζを出力す
る。上記ωn,ζは例えば実車テストなどでα，βの各種
組合わせに応じて予め測定しておく。メモリ20から出力
されたωn,ζは演算器30に入力され、ここで前記（12）
および（13）式の演算が行なわれることによりアーム伝
達関数ＧA(S)をＧB(S)と等しくする可変ゲイン値k1,k2
がアーム駆動系18に出力される。これによりゲインk1,k
2が可変制御され、アーム伝達関数ＧA(S)はブーム伝達
関数ＧB(S)と等しい一定値Ｇ(S)に保持される。
第５図は第１図の実施例構成に対応する制御ブロック
線図を示すものであり、位置指令入力装置10から出力さ
れたアーム先端点ＣのＸ軸方向およびＹ軸方向の位置指
令（Xr,Yr）は加算点31,32に入力される。加算点31,32
は第１図の現在位置演算装置12および速度ベクトル演算
装置15に対応し、位置指令（Xr,Yr）とフィードバック
されたアーム先端点Ｃの現在位置（Xa,Ya）との偏差eX
（＝Xr−Xa）、eY（＝Yr−Ya）をそれぞれ求め、これら
eX,eYを速度ベクトル演算装置15に入力する。速度ベク
トル演算装置15は先の第（３）式にしたがってeX,eYを
ブーム回動速度指令ｒ、アーム回動速度指令ｒに変
換し、これらの変換値r,ｒをそれぞれブーム駆動系
17、アーム駆動系18に入力する。
線図を示すものであり、位置指令入力装置10から出力さ
れたアーム先端点ＣのＸ軸方向およびＹ軸方向の位置指
令（Xr,Yr）は加算点31,32に入力される。加算点31,32
は第１図の現在位置演算装置12および速度ベクトル演算
装置15に対応し、位置指令（Xr,Yr）とフィードバック
されたアーム先端点Ｃの現在位置（Xa,Ya）との偏差eX
（＝Xr−Xa）、eY（＝Yr−Ya）をそれぞれ求め、これら
eX,eYを速度ベクトル演算装置15に入力する。速度ベク
トル演算装置15は先の第（３）式にしたがってeX,eYを
ブーム回動速度指令ｒ、アーム回動速度指令ｒに変
換し、これらの変換値r,ｒをそれぞれブーム駆動系
17、アーム駆動系18に入力する。
アーム駆動系18では前述したアーム伝達関数補償器10
0によりフィードバック補償が行なわれており、これに
よりブーム伝達関数ＧB(S)とアーム伝達関数ＧA(S)は、
ＧB(S)＝ＧA(S)＝Ｇ(S)＝一定となっている。その後、
ブーム角度αａ、アーム角度βａはブーム１、アーム２
のリンク駆動によって逆変換系32で先の（３）式の マトリックスと逆変換 が施され、これによりアーム先端点が位置（Xa,Ya）に
移動される。
0によりフィードバック補償が行なわれており、これに
よりブーム伝達関数ＧB(S)とアーム伝達関数ＧA(S)は、
ＧB(S)＝ＧA(S)＝Ｇ(S)＝一定となっている。その後、
ブーム角度αａ、アーム角度βａはブーム１、アーム２
のリンク駆動によって逆変換系32で先の（３）式の マトリックスと逆変換 が施され、これによりアーム先端点が位置（Xa,Ya）に
移動される。
かかる第５図に示す制御系においては、ＧA(S)＝ＧB
(S)＝Ｇ(S)（一定）となっているため、（Xa,Ya）と（X
r,Yr）との関係は下式（14）のようになる。
(S)＝Ｇ(S)（一定）となっているため、（Xa,Ya）と（X
r,Yr）との関係は下式（14）のようになる。
上記（14）式を変形すると となる。
これら（15）および（16）式からも判るように、この
第５図に示す制御系によれば、Ｘ指令値Xrは実際値Yaに
何ら影響を与えず、またＹ指令値Yrは実際値Xaに何ら影
響を与えない。よって、この制御系ではＸ座標とＹ座標
とを完全に分離することができる。
第５図に示す制御系によれば、Ｘ指令値Xrは実際値Yaに
何ら影響を与えず、またＹ指令値Yrは実際値Xaに何ら影
響を与えない。よって、この制御系ではＸ座標とＹ座標
とを完全に分離することができる。
したがって、アーム先端点ＣをＹ＝０（Ya＝０）でＸ
軸上を一定速度で動かす直線掘削を行なうと、上記（1
5）式から明らかなように、Ｘ方向への動きについては
何らかの動作おくれを生じるものの、Ｙ方向に関しては
Ya＝０であるため動的な動きは何ら生じずＹ＝０のまま
で直線移動する。即ち、理論的なＹ方向の軌跡誤差を零
とすることができる。
軸上を一定速度で動かす直線掘削を行なうと、上記（1
5）式から明らかなように、Ｘ方向への動きについては
何らかの動作おくれを生じるものの、Ｙ方向に関しては
Ya＝０であるため動的な動きは何ら生じずＹ＝０のまま
で直線移動する。即ち、理論的なＹ方向の軌跡誤差を零
とすることができる。
このようにこの実施例ではブーム傾角αａ、アーム傾
角βａに関係なくブーム伝達関数ＧB(S)およびアーム伝
達関数ＧA(S)を、ＧB(S)＝ＧA(S)＝一定としたので、Ｘ
座標とＹ座標とが完全に分離され、これにより、直線掘
削精度を向上させることができる。
角βａに関係なくブーム伝達関数ＧB(S)およびアーム伝
達関数ＧA(S)を、ＧB(S)＝ＧA(S)＝一定としたので、Ｘ
座標とＹ座標とが完全に分離され、これにより、直線掘
削精度を向上させることができる。
なお、上記実施例の補償器100では予め求めたブーム
伝達関数ＧB(S)のωn,ζを用いてアーム伝達関数ＧA(S)
のゲインk1,k2を可変制御するようにしたが、これとは
逆に、予め求めたアーム伝達関数ＧA(S)のωn,ζを用い
てブーム伝達関数ＧA(S)のゲインを可変制御するように
してもよい。
伝達関数ＧB(S)のωn,ζを用いてアーム伝達関数ＧA(S)
のゲインk1,k2を可変制御するようにしたが、これとは
逆に、予め求めたアーム伝達関数ＧA(S)のωn,ζを用い
てブーム伝達関数ＧA(S)のゲインを可変制御するように
してもよい。
また、上記実施例では、目標回動速度演算装置16は速
度指令r,ｒを出力するようにしたが、回動角指令α
r,βｒを出力し、この回動角指令αr,βｒに基づきブー
ムおよびアームの回動制御を行なうようにしてもよい。
度指令r,ｒを出力するようにしたが、回動角指令α
r,βｒを出力し、この回動角指令αr,βｒに基づきブー
ムおよびアームの回動制御を行なうようにしてもよい。
さらに、ＧB(S)＝ＧA(S)＝Ｇ(S)＝一定とするための
手法は前述したゲイン可変制御方式に限らず、他の任意
の手法を採用するようにしてもよい。
手法は前述したゲイン可変制御方式に限らず、他の任意
の手法を採用するようにしてもよい。
第６図はこの発明の第２の実施例を示すものであり、
この実施例は斜面掘削において掘削面に垂直方向の定常
偏差を零にしようとするものである。
この実施例は斜面掘削において掘削面に垂直方向の定常
偏差を零にしようとするものである。
第６図において、指令位置入力装置10から出力された
Ｘ−Ｙ絶対座標系におけるアーム先端点Ｃの位置指令X
r,Yrは指令位置演算装置40に入力される。指令位置演算
装置40では、位置指令入力装置10から入力されたＸ−Ｙ
絶対座標系における位置指令Xr,Yrを、Ｘ−Ｙ絶対座標
系をθだけ回転した内部座標系Xr′,Yr′に変換する下
式（17）に示す演算を行なう。
Ｘ−Ｙ絶対座標系におけるアーム先端点Ｃの位置指令X
r,Yrは指令位置演算装置40に入力される。指令位置演算
装置40では、位置指令入力装置10から入力されたＸ−Ｙ
絶対座標系における位置指令Xr,Yrを、Ｘ−Ｙ絶対座標
系をθだけ回転した内部座標系Xr′,Yr′に変換する下
式（17）に示す演算を行なう。
現在位置演算装置50は、角度検出器13,14でそれぞれ
検出したブーム角αおよびアーム角βとを基に下記（1
8）式によって前記内部座標系Ｘ′−Ｙ′におけるアー
ム先端点の現在位置（Xa′,Ya′）を算出するもので、
その算出結果は速度ベクトル演算装置60に入力される。
検出したブーム角αおよびアーム角βとを基に下記（1
8）式によって前記内部座標系Ｘ′−Ｙ′におけるアー
ム先端点の現在位置（Xa′,Ya′）を算出するもので、
その算出結果は速度ベクトル演算装置60に入力される。
速度ベクトル演算装置60は、指令位置演算装置40から
与えられるアーム先端点の位置指令（Xr′,Yr′）と現
在位置演算装置50から入力されるアーム先端点の現在位
置（Xa′,Ya′）とに基づきアーム先端点を指令位置（X
r,Yr）へ移動させるための速度ベクトルeX′,eY′を次
式（19）によって算出する。
与えられるアーム先端点の位置指令（Xr′,Yr′）と現
在位置演算装置50から入力されるアーム先端点の現在位
置（Xa′,Ya′）とに基づきアーム先端点を指令位置（X
r,Yr）へ移動させるための速度ベクトルeX′,eY′を次
式（19）によって算出する。
但し、ＫX′,kY′は正の係数である。
目標回動速度演算装置70は、アーム先端点の位置を指
令位置（Xr,Yr）に制御するためのブーム１およびアー
ム２の回動速度指令r,ｒを演算するもので、ここで
は前記速度ベクトルeX′,eY′とブーム角α，アーム角
β，ブーム１の長さl1,アーム２の長さl2とを基に次式
（20）によって算出する。
令位置（Xr,Yr）に制御するためのブーム１およびアー
ム２の回動速度指令r,ｒを演算するもので、ここで
は前記速度ベクトルeX′,eY′とブーム角α，アーム角
β，ブーム１の長さl1,アーム２の長さl2とを基に次式
（20）によって算出する。
ここで Ａ＝−l2 sin（α−θ＋β） Ｂ＝−l2 cos（α−θ＋β） Ｃ＝l1 sin（α−θ） ＋l2 sin（α−θ＋β） Ｄ＝l1 cos（α−θ） ＋l2 cos（α−θ＋β） アーム伝達関数補償器100は先の第１図に示したもの
と同様アーム伝達関数ＧA(S)のゲインk1,k2をブーム伝
達関数ＧB(S)のωn,ζに応じて可変制御することによ
り、ＧB(S)＝ＧA(S)＝Ｇ(S)＝一定とするフィードバッ
ク補償を行っている。
と同様アーム伝達関数ＧA(S)のゲインk1,k2をブーム伝
達関数ＧB(S)のωn,ζに応じて可変制御することによ
り、ＧB(S)＝ＧA(S)＝Ｇ(S)＝一定とするフィードバッ
ク補償を行っている。
かかる第６図に示す構成では、補償器100のフィード
バック補償によってＧA(S)＝G B(S)＝Ｇ(S)（一定）と
なっているので絶対座標系Ｘ−Ｙにおける定常偏差σX,
σＹは先の（４）（５）式で示したように となる。
バック補償によってＧA(S)＝G B(S)＝Ｇ(S)（一定）と
なっているので絶対座標系Ｘ−Ｙにおける定常偏差σX,
σＹは先の（４）（５）式で示したように となる。
しかし、第７図に示すように、傾斜角θの斜面に沿っ
た方向をＸ′軸、このＸ′軸に垂直な方向をＹ′軸とし
た、前記内部座標系Ｘ′−Ｙ′における掘削速度成分v
X′,VY′（vX′;X′軸に沿った方向の速度成分、vY′
＝;Y′軸に沿った方向の速度成分）は前記座標回転の結
果、 となり、したがって先の（４）（５）式により内部座標
系Ｘ′−Ｙ′における定常偏差σＸ′，σＹ′は となり、Ｙ′方向の定常偏差を０にすることができる。
た方向をＸ′軸、このＸ′軸に垂直な方向をＹ′軸とし
た、前記内部座標系Ｘ′−Ｙ′における掘削速度成分v
X′,VY′（vX′;X′軸に沿った方向の速度成分、vY′
＝;Y′軸に沿った方向の速度成分）は前記座標回転の結
果、 となり、したがって先の（４）（５）式により内部座標
系Ｘ′−Ｙ′における定常偏差σＸ′，σＹ′は となり、Ｙ′方向の定常偏差を０にすることができる。
このようにこの第２の実施例では、位置指令Xr,Yrと
現在位置Xa,Yaをそれぞれ絶対座標系を傾斜角θだけ回
転した内部座標系Ｘ′−Ｙ′の値に変換し、これらを用
いてブーム、アームを駆動するようにして斜面直線掘削
を行なうようにしたので、斜面に垂直なＹ′方向の定常
偏差を０にすることができ、斜面直線掘削精度を飛躍的
に向上させることができる。
現在位置Xa,Yaをそれぞれ絶対座標系を傾斜角θだけ回
転した内部座標系Ｘ′−Ｙ′の値に変換し、これらを用
いてブーム、アームを駆動するようにして斜面直線掘削
を行なうようにしたので、斜面に垂直なＹ′方向の定常
偏差を０にすることができ、斜面直線掘削精度を飛躍的
に向上させることができる。
以上説明したようにこの発明によれば、ブーム伝達関
数とアーム伝達関数を一定に維持するようにしたので、
Ｘ座標とＹ座標とが干渉しないようになり、直線軌跡精
度を向上させることができる。また、この発明では内部
座標系を斜面角に応じて回転し、この回転した座標系の
位置指令にしたがって直線掘削を行なうようにしたの
で、掘削面の傾斜角に関係なく掘削面に垂直な方向の定
常偏差を零にすることができ、斜面直線掘削の精度を向
上させることができる。
数とアーム伝達関数を一定に維持するようにしたので、
Ｘ座標とＹ座標とが干渉しないようになり、直線軌跡精
度を向上させることができる。また、この発明では内部
座標系を斜面角に応じて回転し、この回転した座標系の
位置指令にしたがって直線掘削を行なうようにしたの
で、掘削面の傾斜角に関係なく掘削面に垂直な方向の定
常偏差を零にすることができ、斜面直線掘削の精度を向
上させることができる。
第１図はこの発明の第１の実施例に示すブロック図、第
２図はブーム伝達関数を示すブロック図、第３図はアー
ム伝達関数を示すブロック図、第４図はアーム伝達関数
補償器の具体構成例を示すブロック図、第５図は上記第
１の実施例に対応する制御系ブロック線図、第６図はこ
の発明の第２の実施例を示すブロック図、第７図は上記
第２の実施例を説明するための説明図、第８図はパワー
ショベルの外観構成を示す側面図、第９図は従来の制御
系ブロック図、第10図はパワーショベルの各部位置、長
さ、角度等を定義するための説明図、第11図および第12
図は従来の直線掘削方式を説明するための説明図、第13
図は従来の斜面掘削の説明図である。 １……ブーム、２……アーム、３……バケット、４……
ブームシリンダ、５……アームシリンダ、６……バケッ
トシリンダ、10……指令位置入力装置、12,50……現在
位置演算装置、13……ブーム角度検出器、14……アーム
角度検出器、15,60……速度ベクトル演算装置、16,70…
…目標回動速度演算装置、17……ブーム駆動系、18……
アーム駆動系、20……メモリ、30……演算器、40……指
令位置演算装置、100……アーム伝達関係補償器。
２図はブーム伝達関数を示すブロック図、第３図はアー
ム伝達関数を示すブロック図、第４図はアーム伝達関数
補償器の具体構成例を示すブロック図、第５図は上記第
１の実施例に対応する制御系ブロック線図、第６図はこ
の発明の第２の実施例を示すブロック図、第７図は上記
第２の実施例を説明するための説明図、第８図はパワー
ショベルの外観構成を示す側面図、第９図は従来の制御
系ブロック図、第10図はパワーショベルの各部位置、長
さ、角度等を定義するための説明図、第11図および第12
図は従来の直線掘削方式を説明するための説明図、第13
図は従来の斜面掘削の説明図である。 １……ブーム、２……アーム、３……バケット、４……
ブームシリンダ、５……アームシリンダ、６……バケッ
トシリンダ、10……指令位置入力装置、12,50……現在
位置演算装置、13……ブーム角度検出器、14……アーム
角度検出器、15,60……速度ベクトル演算装置、16,70…
…目標回動速度演算装置、17……ブーム駆動系、18……
アーム駆動系、20……メモリ、30……演算器、40……指
令位置演算装置、100……アーム伝達関係補償器。
Claims (2)
- 【請求項１】ブーム、アームおよびバケットを有する作
業機と、 ブーム、アームおよびバケットを含む平面を座標平面と
してアーム先端点のＸ軸およびＹ軸方向についての位置
指令を発生する位置指令手段と、 前記アーム先端点のＸ軸およびＹ軸方向についての現在
位置を求める現在位置検出手段と、 前記位置指令手段の指令位置と現在位置検出手段で求め
た現在位置とに基づきアーム先端点が指令位置へ移動す
るためのブームおよびアームの目標回動速度または目標
回動角を算出する目標値演算手段と、 この目標値演算手段の算出目標値に基づきブームシリン
ダおよびアームシリンダを駆動制御する駆動手段と、 ブーム伝達関数およびアーム伝達関数とをそれぞれ等し
い一定値に保持する補償手段と を具える作業機の制御装置。 - 【請求項２】ブーム、アームおよびバケットを有する作
業機と、 ブーム、アームおよびバケットを含む平面を座標平面と
してアーム先端点のＸ軸およびＹ軸方向についての位置
指令を発生する位置指令手段と、 この位置指令手段の位置指令を前記Ｘ−Ｙ座標系を斜面
の傾斜角だけ回転した内部座標系の位置指令に変換する
座標変換手段と、 前記内部座標系における前記アーム先端点のＸ軸および
Ｙ軸方向についての現在位置を求める現在位置検出手段
と、 前記位置指令手段の指令位置と現在位置検出手段で求め
た現在位置とに基づきアーム先端点が指令位置へ移動す
るためのブームおよびアームの目標回動速度または目標
回動角を算出する目標値演算手段と、 この目標値演算手段の算出目標値に基づきブームシリン
ダおよびアームシリンダを駆動制御する駆動手段と、 ブーム伝達関数およびアーム伝達関数とをそれぞれ等し
い一定値に保持する補償手段と を具える作業機の制御装置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP16663488A JP2704729B2 (ja) | 1988-07-04 | 1988-07-04 | 作業機の制御装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP16663488A JP2704729B2 (ja) | 1988-07-04 | 1988-07-04 | 作業機の制御装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JPH02210508A JPH02210508A (ja) | 1990-08-21 |
| JP2704729B2 true JP2704729B2 (ja) | 1998-01-26 |
Family
ID=15834918
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP16663488A Expired - Fee Related JP2704729B2 (ja) | 1988-07-04 | 1988-07-04 | 作業機の制御装置 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP2704729B2 (ja) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP3926464B2 (ja) * | 1998-03-18 | 2007-06-06 | 日立建機株式会社 | 自動運転ショベル |
-
1988
- 1988-07-04 JP JP16663488A patent/JP2704729B2/ja not_active Expired - Fee Related
Also Published As
| Publication number | Publication date |
|---|---|
| JPH02210508A (ja) | 1990-08-21 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| KR100629469B1 (ko) | 로봇 제어장치 | |
| CA1154722A (en) | Control device for an earthwork machine | |
| WO1995018271A1 (fr) | Unite de commande de machines de terrassement | |
| JPH0138957B2 (ja) | ||
| CN113737885A (zh) | 挖掘机控制方法、装置、挖掘机及存储介质 | |
| JP2002167794A (ja) | 油圧ショベルのフロント制御装置 | |
| JP2704729B2 (ja) | 作業機の制御装置 | |
| JPS5815801B2 (ja) | 工業用ロボツトの軌跡制御方式 | |
| JPS61200226A (ja) | パワ−シヨベルの位置制御装置 | |
| JP4134369B2 (ja) | ロボットの制御装置 | |
| JP2631757B2 (ja) | 建設機械の掘削制御方法 | |
| JP2652167B2 (ja) | パワーショベルの作業機位置制御装置 | |
| JPH02101228A (ja) | 作業機の制御装置 | |
| JPH02101229A (ja) | 作業機の制御装置 | |
| JPH076209B2 (ja) | パワ−シヨベルにおける作業機の制御装置 | |
| JP2699097B2 (ja) | 建設機械のバケット刃先深さ制御装置 | |
| JP2960233B2 (ja) | ロボットの制御装置 | |
| JPH0823155B2 (ja) | 作業機の制御装置 | |
| JP3266714B2 (ja) | マニピュレータの姿勢角度制御装置 | |
| JPS5810197B2 (ja) | エンカツソウジユウソウチ | |
| JPH02221527A (ja) | 作業機の制御装置 | |
| JP2574671B2 (ja) | パワ−シヨベルにおける作業機の制御装置 | |
| JP2003211376A (ja) | ロボットの制御装置及び制御方法 | |
| JPH01312125A (ja) | 腕式作業機の制御方法 | |
| JPH0559745A (ja) | 腕式作業機の作業具軌跡制御装置 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| LAPS | Cancellation because of no payment of annual fees |