JPH06322789A - 作業機械の作業軌跡制御装置 - Google Patents
作業機械の作業軌跡制御装置Info
- Publication number
- JPH06322789A JPH06322789A JP11481193A JP11481193A JPH06322789A JP H06322789 A JPH06322789 A JP H06322789A JP 11481193 A JP11481193 A JP 11481193A JP 11481193 A JP11481193 A JP 11481193A JP H06322789 A JPH06322789 A JP H06322789A
- Authority
- JP
- Japan
- Prior art keywords
- angle
- target
- target position
- working machine
- work
- 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.)
- Pending
Links
Landscapes
- Operation Control Of Excavators (AREA)
Abstract
(57)【要約】
【目的】 上部旋回体を備えた作業機械において、作業
機本体が傾いている場合にも作業軌跡を正確に制御す
る。 【構成】 本体傾斜角センサ14,15で作業機本体の
傾斜状態を検出し、この傾斜状態に基づき、目標位置等
設定部21で設定された目標位置を目標位置等補正演算
部22で補正する。この補正目標位置に基づいて目標角
度演算部23で上部旋回体の目標旋回角度及び各作業部
材の相対回動角度を演算し、これらの目標角度と、角度
検出センサ10〜13で検出される検出角度との比較に
基づき、油圧モータ6及び各油圧シリンダ7〜9による
上記旋回駆動の制御及び相対回動駆動の制御を行う。
機本体が傾いている場合にも作業軌跡を正確に制御す
る。 【構成】 本体傾斜角センサ14,15で作業機本体の
傾斜状態を検出し、この傾斜状態に基づき、目標位置等
設定部21で設定された目標位置を目標位置等補正演算
部22で補正する。この補正目標位置に基づいて目標角
度演算部23で上部旋回体の目標旋回角度及び各作業部
材の相対回動角度を演算し、これらの目標角度と、角度
検出センサ10〜13で検出される検出角度との比較に
基づき、油圧モータ6及び各油圧シリンダ7〜9による
上記旋回駆動の制御及び相対回動駆動の制御を行う。
Description
【0001】
【産業上の利用分野】本発明は、油圧ショベルやクレー
ン等の作業機械において、その掘削部やブーム先端等の
作業軌跡を制御するための装置に関するものである。
ン等の作業機械において、その掘削部やブーム先端等の
作業軌跡を制御するための装置に関するものである。
【0002】
【従来の技術】従来、油圧ショベル等のように複数のア
ームが相対回動可能に直列に連結された多関節作業機で
は、その所定部位(例えばショベル先端)の作業軌跡制
御を行うために、上記所定部位の目標位置を予め設定し
ておき、この目標位置を得るための各関節での目標相対
回動角度を演算し、この目標相対回動角度に実際の相対
回動角度を一致させるようにアクチュエータを制御する
といったことが一般に行われている。
ームが相対回動可能に直列に連結された多関節作業機で
は、その所定部位(例えばショベル先端)の作業軌跡制
御を行うために、上記所定部位の目標位置を予め設定し
ておき、この目標位置を得るための各関節での目標相対
回動角度を演算し、この目標相対回動角度に実際の相対
回動角度を一致させるようにアクチュエータを制御する
といったことが一般に行われている。
【0003】さらに、特開平2−101228号公報に
は、作業機本体から見た掘削面の傾斜角θに基づき、こ
の作業機本体の位置を基準に設定される車体固定座標x
−yを、掘削面を基準に設定される座標x′−y′に変
換し、この変換後の座標に基づいてアーム先端の作業軌
跡制御を行うことにより、その制御内容をより単純化す
るようにしたものが示されている。
は、作業機本体から見た掘削面の傾斜角θに基づき、こ
の作業機本体の位置を基準に設定される車体固定座標x
−yを、掘削面を基準に設定される座標x′−y′に変
換し、この変換後の座標に基づいてアーム先端の作業軌
跡制御を行うことにより、その制御内容をより単純化す
るようにしたものが示されている。
【0004】
【発明が解決しようとする課題】上記公報に示される装
置では、実際の作業機本体の傾斜状態を考慮せず、掘削
面に対して直交するある一つの基準面上での作業機本体
に対する掘削面の傾斜角θに基づいて、上記掘削面を基
準としたx′−y′座標系への座標変換を行い、この座
標系で制御を行っているので、作業機本体に対して旋回
する上部旋回体に上記アームが連結されている作業機
械、すなわち上記基準面から逸脱した領域で作業が行わ
れる作業機械には、適用することができない。
置では、実際の作業機本体の傾斜状態を考慮せず、掘削
面に対して直交するある一つの基準面上での作業機本体
に対する掘削面の傾斜角θに基づいて、上記掘削面を基
準としたx′−y′座標系への座標変換を行い、この座
標系で制御を行っているので、作業機本体に対して旋回
する上部旋回体に上記アームが連結されている作業機
械、すなわち上記基準面から逸脱した領域で作業が行わ
れる作業機械には、適用することができない。
【0005】本発明は、このような事情に鑑み、作業機
本体に旋回可能に設置された旋回体に作業部材が連結さ
れている作業機械において、上記作業機本体の傾斜状態
にかかわらず所定部位の作業軌跡を正確に制御すること
ができる装置を提供することを目的とする。
本体に旋回可能に設置された旋回体に作業部材が連結さ
れている作業機械において、上記作業機本体の傾斜状態
にかかわらず所定部位の作業軌跡を正確に制御すること
ができる装置を提供することを目的とする。
【0006】
【課題を解決するための手段】上記課題を解決するため
の手段として、本発明は、作業機本体と、この作業機本
体に旋回可能に設置された旋回体と、互いに相対回動可
能に直列に連結され、そのうちの一つが上記旋回体に相
対回動可能に連結される複数の作業部材と、上記旋回体
を旋回駆動する旋回駆動手段と、各相対回動部位を回動
駆動する回動駆動手段とを備えた作業機械の作業軌跡制
御装置において、上記作業部材における所定の軌跡制御
対象部位の目標位置を設定する目標位置設定手段と、互
いに異なる複数の方向について上記作業機本体の基準面
からの傾斜角を検出する傾斜検出手段と、上記旋回体の
旋回角度及び各相対回動部位における相対回動角度を検
出する角度検出手段と、上記傾斜検出手段で検出された
作業機本体の傾斜角に基づき上記目標位置の補正を行う
目標位置補正演算手段と、この目標位置補正演算手段に
より演算された補正目標位置に基づいて上記旋回体の目
標旋回角度及び各相対回動部位での目標相対回動角度を
演算する目標角度演算手段と、この目標相対角度演算手
段で演算された目標相対角度と上記角度検出手段で検出
された実際の角度とに基づいて上記旋回駆動手段及び回
動駆動手段による駆動を制御する駆動制御手段とを備え
たものである(請求項1)。
の手段として、本発明は、作業機本体と、この作業機本
体に旋回可能に設置された旋回体と、互いに相対回動可
能に直列に連結され、そのうちの一つが上記旋回体に相
対回動可能に連結される複数の作業部材と、上記旋回体
を旋回駆動する旋回駆動手段と、各相対回動部位を回動
駆動する回動駆動手段とを備えた作業機械の作業軌跡制
御装置において、上記作業部材における所定の軌跡制御
対象部位の目標位置を設定する目標位置設定手段と、互
いに異なる複数の方向について上記作業機本体の基準面
からの傾斜角を検出する傾斜検出手段と、上記旋回体の
旋回角度及び各相対回動部位における相対回動角度を検
出する角度検出手段と、上記傾斜検出手段で検出された
作業機本体の傾斜角に基づき上記目標位置の補正を行う
目標位置補正演算手段と、この目標位置補正演算手段に
より演算された補正目標位置に基づいて上記旋回体の目
標旋回角度及び各相対回動部位での目標相対回動角度を
演算する目標角度演算手段と、この目標相対角度演算手
段で演算された目標相対角度と上記角度検出手段で検出
された実際の角度とに基づいて上記旋回駆動手段及び回
動駆動手段による駆動を制御する駆動制御手段とを備え
たものである(請求項1)。
【0007】ここで上記目標位置補正演算手段は、上記
作業機本体の検出傾斜角に基づき、目標位置設定手段で
設定された目標位置を補正目標位置に変換するための補
正用関数を演算し、これを少なくとも上記作業機本体の
位置が変化しない状態での制御動作中は記憶しておくよ
うに構成されていることが、より好ましい(請求項
2)。
作業機本体の検出傾斜角に基づき、目標位置設定手段で
設定された目標位置を補正目標位置に変換するための補
正用関数を演算し、これを少なくとも上記作業機本体の
位置が変化しない状態での制御動作中は記憶しておくよ
うに構成されていることが、より好ましい(請求項
2)。
【0008】
【作用】請求項1記載の装置では、傾斜検出手段によっ
て基準面(一般には水平面)からの作業機本体の傾斜状
態が直接検出され、この作業機本体の傾斜状態に基づい
て設定目標位置の補正が行われ、この補正目標位置に基
づいて予め旋回駆動及び相対回動駆動が制御されるの
で、作業中に旋回体が旋回する場合でも常に軌跡制御対
象部位の位置を正確に制御することができる。また、先
に設定目標位置の補正が行われ、この補正後の目標位置
に基づいて目標旋回角度及び目標相対回動角度の演算が
行われるので、原目標位置に基づいて目標旋回角度及び
目標相対回動角度を演算してからこれらの角度を補正す
る場合に比べ、演算はより簡略化される。
て基準面(一般には水平面)からの作業機本体の傾斜状
態が直接検出され、この作業機本体の傾斜状態に基づい
て設定目標位置の補正が行われ、この補正目標位置に基
づいて予め旋回駆動及び相対回動駆動が制御されるの
で、作業中に旋回体が旋回する場合でも常に軌跡制御対
象部位の位置を正確に制御することができる。また、先
に設定目標位置の補正が行われ、この補正後の目標位置
に基づいて目標旋回角度及び目標相対回動角度の演算が
行われるので、原目標位置に基づいて目標旋回角度及び
目標相対回動角度を演算してからこれらの角度を補正す
る場合に比べ、演算はより簡略化される。
【0009】さらに、請求項2記載の装置では、一旦目
標位置補正演算手段で目標位置補正用の補正用関数が演
算された後は、作業機本体の傾斜状態が変化するまでは
上記補正用関数が記憶され、使用されるので、目標位置
が変わる度に一から目標位置の補正演算を行う必要がな
い。このため、より迅速に目標位置の補正演算処理が実
行される。
標位置補正演算手段で目標位置補正用の補正用関数が演
算された後は、作業機本体の傾斜状態が変化するまでは
上記補正用関数が記憶され、使用されるので、目標位置
が変わる度に一から目標位置の補正演算を行う必要がな
い。このため、より迅速に目標位置の補正演算処理が実
行される。
【0010】
【実施例】本発明の一実施例を図面に基づいて説明す
る。
る。
【0011】図2は、本発明装置を備えた作業機械であ
る油圧ショベルを示している。この油圧ショベルは、作
業機本体である下部走行体1を備え、この下部走行体1
上に旋回可能に上部旋回体2が設置されている。この上
部旋回体2には、その旋回軸方向(図2では上下方向)
に直交する方向(図2では奥行き方向)の軸周りに相対
回動可能にブーム3の基端が連結されている。同様に、
このブーム3の先端にはアーム4の基端が回動可能に連
結されており、このアーム4の先端にはバケット5が相
対回動可能に連結されている。すなわち、これらのブー
ム3、アーム4、及びバケット5は、互いに相対回動可
能に直列に連結された複数の作業部材を構成している。
る油圧ショベルを示している。この油圧ショベルは、作
業機本体である下部走行体1を備え、この下部走行体1
上に旋回可能に上部旋回体2が設置されている。この上
部旋回体2には、その旋回軸方向(図2では上下方向)
に直交する方向(図2では奥行き方向)の軸周りに相対
回動可能にブーム3の基端が連結されている。同様に、
このブーム3の先端にはアーム4の基端が回動可能に連
結されており、このアーム4の先端にはバケット5が相
対回動可能に連結されている。すなわち、これらのブー
ム3、アーム4、及びバケット5は、互いに相対回動可
能に直列に連結された複数の作業部材を構成している。
【0012】この油圧ショベルは、油圧モータ6、第1
油圧シリンダ7、第2油圧シリンダ8、及び第3油圧シ
リンダ9を備えている。
油圧シリンダ7、第2油圧シリンダ8、及び第3油圧シ
リンダ9を備えている。
【0013】油圧モータ6は、その作動により、上記下
部走行体1に対して上部旋回体2を旋回駆動する旋回駆
動手段である。
部走行体1に対して上部旋回体2を旋回駆動する旋回駆
動手段である。
【0014】第1油圧シリンダ7は、その両端が下部走
行体2とブーム3とに連結され、それ自身の伸縮によっ
て上部旋回体2に対してブーム3を相対回転駆動するも
のである。同様に、第2油圧シリンダ8及び第3油圧シ
リンダ9は、ブーム3とアーム4との間、アーム4とバ
ケット5との間にそれぞれ連結されており、これら油圧
シリンダ8,9自身の伸縮で、ブーム3に対するアーム
4の相対回動駆動、及びアーム4に対するバケット5の
相対回動駆動がそれぞれ行われるようになっている。す
なわち、これらの油圧シリンダ7〜9によって本発明に
おける回動駆動手段が構成されている。
行体2とブーム3とに連結され、それ自身の伸縮によっ
て上部旋回体2に対してブーム3を相対回転駆動するも
のである。同様に、第2油圧シリンダ8及び第3油圧シ
リンダ9は、ブーム3とアーム4との間、アーム4とバ
ケット5との間にそれぞれ連結されており、これら油圧
シリンダ8,9自身の伸縮で、ブーム3に対するアーム
4の相対回動駆動、及びアーム4に対するバケット5の
相対回動駆動がそれぞれ行われるようになっている。す
なわち、これらの油圧シリンダ7〜9によって本発明に
おける回動駆動手段が構成されている。
【0015】図1に示すように、この油圧ショベルに
は、旋回角センサ10、第1関節角センサ11、第2関
節角センサ12、第3関節角センサ13、及び本体傾斜
角センサ14,15が配設されている。
は、旋回角センサ10、第1関節角センサ11、第2関
節角センサ12、第3関節角センサ13、及び本体傾斜
角センサ14,15が配設されている。
【0016】上記旋回角センサ10は、下部走行体1に
対する上部旋回体2の旋回角度θ1を検出するものであ
り、第1関節角センサ11、第2関節角センサ12、及
び第3関節角センサ13は、上部旋回体2に対するブー
ム3の相対回動角度θ2、ブーム3に対するアーム4の
相対回動角度θ3、アーム4に対するバケット5の相対
回動角度θ4をそれぞれ検出するものである。すなわ
ち、これらのセンサ10〜13によって、本発明におけ
る角度検出手段が構成されている。
対する上部旋回体2の旋回角度θ1を検出するものであ
り、第1関節角センサ11、第2関節角センサ12、及
び第3関節角センサ13は、上部旋回体2に対するブー
ム3の相対回動角度θ2、ブーム3に対するアーム4の
相対回動角度θ3、アーム4に対するバケット5の相対
回動角度θ4をそれぞれ検出するものである。すなわ
ち、これらのセンサ10〜13によって、本発明におけ
る角度検出手段が構成されている。
【0017】本体傾斜角センサ14,15は、下部走行
体1に設置されている。本体傾斜角センサ14は、図4
に示すように、水平面Hを基準とするXw−Yw−Zw
座標系(より具体的には原点Owが上部旋回体2の旋回
中心であって水平面Hに対してYw軸が直交する座標
系;以下、座標系Σwと称する。)と、下部走行体1を
基準とするXo−Yo−Zo座標系(より具体的には原
点Ooが上部旋回体2の旋回中心であってその旋回軸に
Yo軸が平行な座標系;以下、座標系Σoと称する。)
とを設定した場合に、Xw−Yw平面とXo軸とのなす
角度αを検出するものであり、本体傾斜角センサ15
は、上記Xw−Yw平面とZo軸とのなす角度βを検出
するものである。すなわち、これらの本体傾斜角センサ
14,15により、互いに異なる2方向について基準面
(この実施例では水平面)に対する下部走行体1の傾斜
角度を検出する傾斜検出手段が構成されている。
体1に設置されている。本体傾斜角センサ14は、図4
に示すように、水平面Hを基準とするXw−Yw−Zw
座標系(より具体的には原点Owが上部旋回体2の旋回
中心であって水平面Hに対してYw軸が直交する座標
系;以下、座標系Σwと称する。)と、下部走行体1を
基準とするXo−Yo−Zo座標系(より具体的には原
点Ooが上部旋回体2の旋回中心であってその旋回軸に
Yo軸が平行な座標系;以下、座標系Σoと称する。)
とを設定した場合に、Xw−Yw平面とXo軸とのなす
角度αを検出するものであり、本体傾斜角センサ15
は、上記Xw−Yw平面とZo軸とのなす角度βを検出
するものである。すなわち、これらの本体傾斜角センサ
14,15により、互いに異なる2方向について基準面
(この実施例では水平面)に対する下部走行体1の傾斜
角度を検出する傾斜検出手段が構成されている。
【0018】上記各センサ10〜15の検出信号はコン
トローラ20に入力されるようになっている。このコン
トローラ20は、各センサ10〜15の検出信号に基づ
き、上記油圧モータ6及び各油圧シリンダ7〜9の駆動
制御を行うように構成されている。
トローラ20に入力されるようになっている。このコン
トローラ20は、各センサ10〜15の検出信号に基づ
き、上記油圧モータ6及び各油圧シリンダ7〜9の駆動
制御を行うように構成されている。
【0019】具体的に、このコントローラ20は、目標
位置等設定部(目標位置設定手段)21、目標位置等補
正演算部(目標位置補正演算手段)22、目標角度演算
部(目標角度演算手段)23、及びアクチュエータ制御
部(駆動制御手段)24を備えている。
位置等設定部(目標位置設定手段)21、目標位置等補
正演算部(目標位置補正演算手段)22、目標角度演算
部(目標角度演算手段)23、及びアクチュエータ制御
部(駆動制御手段)24を備えている。
【0020】目標位置等設定部21は、上記油圧ショベ
ルにおける軌跡制御対象部位であるバケット5の先端部
5aに所望の作業軌跡を描かせるための同先端部5aの
目標位置(xwr,ywr,zwr)及び目標対地角度θwb
を、上記座標系Σoを基準に設定するものである。
ルにおける軌跡制御対象部位であるバケット5の先端部
5aに所望の作業軌跡を描かせるための同先端部5aの
目標位置(xwr,ywr,zwr)及び目標対地角度θwb
を、上記座標系Σoを基準に設定するものである。
【0021】目標位置等補正演算部22は、本体傾斜角
センサ14,15で検出された下部走行体1の傾斜状態
に基づき、上記設定目標位置(xwr,ywr,zwr)及び
設定目標対地角度θwbを上記座標系Σoにおける目標位
置(xor,yor,zor)及び目標対地角度θobに補正す
るものである。
センサ14,15で検出された下部走行体1の傾斜状態
に基づき、上記設定目標位置(xwr,ywr,zwr)及び
設定目標対地角度θwbを上記座標系Σoにおける目標位
置(xor,yor,zor)及び目標対地角度θobに補正す
るものである。
【0022】目標角度演算部23は、上記目標位置等補
正演算部22で演算された補正目標位置及び補正目標対
地角度に基づき、図5に示すような、上記目標位置及び
目標対地角度を得るための上部旋回体2の目標旋回角度
θ1r、上部旋回体2に対するブーム3の目標相対回動角
度θ2r、ブーム3に対するアーム4の目標相対回動角度
θ3r、及びアーム4に対するバケット5の目標相対回動
角度θ4rをそれぞれ演算するものである。
正演算部22で演算された補正目標位置及び補正目標対
地角度に基づき、図5に示すような、上記目標位置及び
目標対地角度を得るための上部旋回体2の目標旋回角度
θ1r、上部旋回体2に対するブーム3の目標相対回動角
度θ2r、ブーム3に対するアーム4の目標相対回動角度
θ3r、及びアーム4に対するバケット5の目標相対回動
角度θ4rをそれぞれ演算するものである。
【0023】アクチュエータ制御部24は、上記目標角
度演算部23で演算された目標角度θ1r〜θ4rと、各セ
ンサ10〜13により検出された実際の検出角度θ1〜
θ4とに基づき、両者を合致させるように上記油圧モー
タ6及び各油圧シリンダ7〜9による旋回駆動及び回動
駆動のフィードバック制御を行うものである。
度演算部23で演算された目標角度θ1r〜θ4rと、各セ
ンサ10〜13により検出された実際の検出角度θ1〜
θ4とに基づき、両者を合致させるように上記油圧モー
タ6及び各油圧シリンダ7〜9による旋回駆動及び回動
駆動のフィードバック制御を行うものである。
【0024】次に、このコントローラ20の行う演算制
御動作を図3のフローチャートを参照しながら説明す
る。
御動作を図3のフローチャートを参照しながら説明す
る。
【0025】まず、目標位置等設定手段21は、本体傾
斜角センサ14,15で検出された本体傾斜角α,βを
取込み(ステップS1)、これらに基づき、上記座標系
Σwをもとに設定された目標位置及び目標対地角度を上
記座標系Σoでの位置及び対地角度に補正するための補
正マトリックスTowを演算し、これを記憶する(ステッ
プS2)。
斜角センサ14,15で検出された本体傾斜角α,βを
取込み(ステップS1)、これらに基づき、上記座標系
Σwをもとに設定された目標位置及び目標対地角度を上
記座標系Σoでの位置及び対地角度に補正するための補
正マトリックスTowを演算し、これを記憶する(ステッ
プS2)。
【0026】具体的には、図4に示すように、上記座標
系ΣoがXw−Zw平面上の単位ベクトルw=(λ,
0,ν)周りにθoだけ傾いているとすると、座標系Σ
wから座標系Σoへの座標変換マトリックス(すなわち
補正マトリックス)Towは次式で表される。
系ΣoがXw−Zw平面上の単位ベクトルw=(λ,
0,ν)周りにθoだけ傾いているとすると、座標系Σ
wから座標系Σoへの座標変換マトリックス(すなわち
補正マトリックス)Towは次式で表される。
【0027】
【数1】
【0028】このような補正マトリックスTowの演算、
記憶が終了した後、実際の制御動作が開始される。すな
わち、座標系Σwに基づき、目標位置等設定部21によ
り目標位置(xwr,ywr,zwr)及び目標対地角度θwb
が設定され(ステップS3)、次いで、これら目標位置
(xwr,ywr,zwr)及び目標対地角度θwbに基づいて
目標位置等補正演算部22により補正目標位置(xor,
yor,zor)及び補正目標対地角度θobが演算される
(ステップS4,S5)。
記憶が終了した後、実際の制御動作が開始される。すな
わち、座標系Σwに基づき、目標位置等設定部21によ
り目標位置(xwr,ywr,zwr)及び目標対地角度θwb
が設定され(ステップS3)、次いで、これら目標位置
(xwr,ywr,zwr)及び目標対地角度θwbに基づいて
目標位置等補正演算部22により補正目標位置(xor,
yor,zor)及び補正目標対地角度θobが演算される
(ステップS4,S5)。
【0029】まず、補正目標位置(xor,yor,zor)
については、既に記憶されている補正マトリックスTow
を用いて次式により演算される。
については、既に記憶されている補正マトリックスTow
を用いて次式により演算される。
【0030】
【数2】
【0031】また、補正目標対地角度θobには、次式
(6),(7)の双方を同時に満たす値が採用される。
(6),(7)の双方を同時に満たす値が採用される。
【0032】
【数3】
【0033】次に、目標角度演算部23は、上記補正目
標位置(xor,yor,zor)と目標対地角度θobとに基
づき、上部旋回体2の目標旋回角度θ1r、上部旋回体2
に対するブーム3の目標相対回動角度θ2r、ブーム3に
対するアーム4の目標相対回動角度θ3r、及びアーム4
に対するバケット5の目標相対回動角度θ4rを演算する
(ステップS6)。
標位置(xor,yor,zor)と目標対地角度θobとに基
づき、上部旋回体2の目標旋回角度θ1r、上部旋回体2
に対するブーム3の目標相対回動角度θ2r、ブーム3に
対するアーム4の目標相対回動角度θ3r、及びアーム4
に対するバケット5の目標相対回動角度θ4rを演算する
(ステップS6)。
【0034】その演算要領を図5を参照しながら説明す
る。いま、原点Ooからのブーム3のオフセット量をL
1x,L1y,L1zとし、ブーム3、アーム4、及びバケッ
ト5のアーム長さをそれぞれL2,L3,L4とすると、
図5から次式が導かれる。
る。いま、原点Ooからのブーム3のオフセット量をL
1x,L1y,L1zとし、ブーム3、アーム4、及びバケッ
ト5のアーム長さをそれぞれL2,L3,L4とすると、
図5から次式が導かれる。
【0035】
【数4】
【0036】ここで、上式を簡略化するため変数aを導
入すると次式が得られる。
入すると次式が得られる。
【0037】
【数5】
【0038】(式(13))2+(式(14))2により次式が得ら
れる。
れる。
【0039】
【数6】
【0040】ここで、上部旋回体2の目標旋回角度θ1r
は以下のようにして求められる。まず、上記式(13)によ
り、次式が得られる。
は以下のようにして求められる。まず、上記式(13)によ
り、次式が得られる。
【0041】
【数7】
【0042】この式を上記式(14)に代入すると次式が得
られる。
られる。
【0043】
【数8】
【0044】よってC1は次式で与えられる。
【0045】
【数9】
【0046】従って、目標旋回角度θ1rは次の2つの式
を同時に満たす角度となる。
を同時に満たす角度となる。
【0047】
【数10】
【0048】次に、上記式(15),(9)からそれぞれ次式
(17),(18)が得られる。
(17),(18)が得られる。
【0049】
【数11】
【0050】これらの式(17),(18)の右辺をそれぞれ
x′,y′とおくと、次式が得られる。
x′,y′とおくと、次式が得られる。
【0051】
【数12】
【0052】(式(19))2+(式(20))2により次式が得ら
れる。
れる。
【0053】
【数13】
【0054】上記2つの式の双方を同時に満たす値が目
標相対角度θ3rとして採用される。また、上記式(19),
(20)に三角関数の加法定理を適用すると、次式が得られ
る。
標相対角度θ3rとして採用される。また、上記式(19),
(20)に三角関数の加法定理を適用すると、次式が得られ
る。
【0055】
【数14】
【0056】これらの式(21),(22)を整理すると次式が
得られる。
得られる。
【0057】
【数15】
【0058】ここで、上記式(23),(24)を同時に満たす
値が目標相対角度θ2rとして採用される。
値が目標相対角度θ2rとして採用される。
【0059】一方、アクチュエータ制御部24は、各角
度センサ10〜13による検出角度θ1〜θ4を取込み
(ステップS7)、これらの角度と、上述の要領で算出
された目標角度θ1r〜θ4rとを比較し、後者に前者が近
づくように油圧モータ6及び各油圧シリンダ7〜9の駆
動制御を行う(ステップS8)。以下、制御が終了する
まで、目標作業軌跡を得るための目標位置が一定の周期
で設定され(ステップS3)、その後、記憶済みの補正
マトリックスTowを用いてステップS4以降の演算制御
動作が繰り返される(ステップS9でNO)。
度センサ10〜13による検出角度θ1〜θ4を取込み
(ステップS7)、これらの角度と、上述の要領で算出
された目標角度θ1r〜θ4rとを比較し、後者に前者が近
づくように油圧モータ6及び各油圧シリンダ7〜9の駆
動制御を行う(ステップS8)。以下、制御が終了する
まで、目標作業軌跡を得るための目標位置が一定の周期
で設定され(ステップS3)、その後、記憶済みの補正
マトリックスTowを用いてステップS4以降の演算制御
動作が繰り返される(ステップS9でNO)。
【0060】このような装置によれば、下部走行体1が
傾いており、また作業中に上部旋回体2が旋回駆動され
る場合でも、上記下部走行体1の実際の傾斜状態に基づ
く目標位置等の補正によって、バケット先端部5aの作
業軌跡制御を正確に行うことができる。しかも、上記下
部走行体1の傾斜状態によってまず目標位置等を補正
し、この補正目標位置に基づいて目標旋回角度及び目標
相対回動角度の演算を行っているので、補正前の目標位
置等に基づいて目標旋回角度及び目標相対回動角度を演
算し、その後にこれらの角度を補正する場合に比べ、演
算内容は著しく簡略化される。
傾いており、また作業中に上部旋回体2が旋回駆動され
る場合でも、上記下部走行体1の実際の傾斜状態に基づ
く目標位置等の補正によって、バケット先端部5aの作
業軌跡制御を正確に行うことができる。しかも、上記下
部走行体1の傾斜状態によってまず目標位置等を補正
し、この補正目標位置に基づいて目標旋回角度及び目標
相対回動角度の演算を行っているので、補正前の目標位
置等に基づいて目標旋回角度及び目標相対回動角度を演
算し、その後にこれらの角度を補正する場合に比べ、演
算内容は著しく簡略化される。
【0061】さらに、この実施例では、上記補正を行う
ための補正マトリックス(補正用関数)Towを一旦演算
した後は、これを記憶してしまい、下部走行体1の傾斜
状態が変わるまで(この実施例では制御終了まで)は目
標位置等が変わっても同じ記憶補正マトリックスTowを
何度も用いるようにしているので、目標位置等が変わる
度に一から補正マトリックスTowを演算する必要がな
く、従ってより迅速に演算処理を行って応答性を高める
ことができる。
ための補正マトリックス(補正用関数)Towを一旦演算
した後は、これを記憶してしまい、下部走行体1の傾斜
状態が変わるまで(この実施例では制御終了まで)は目
標位置等が変わっても同じ記憶補正マトリックスTowを
何度も用いるようにしているので、目標位置等が変わる
度に一から補正マトリックスTowを演算する必要がな
く、従ってより迅速に演算処理を行って応答性を高める
ことができる。
【0062】なお、本発明はこのような実施例に限定さ
れるものでなく、例として次のような態様をとることも
可能である。
れるものでなく、例として次のような態様をとることも
可能である。
【0063】(1) 上記実施例では、3つの作業部材(ブ
ーム3、アーム4、バケット5)を備えた油圧ショベル
を示したが、本発明は、少なくとも2つの作業部材を備
えた作業機械について適用が可能である。
ーム3、アーム4、バケット5)を備えた油圧ショベル
を示したが、本発明は、少なくとも2つの作業部材を備
えた作業機械について適用が可能である。
【0064】(2) 上記実施例では、作業機械として油圧
ショベルを示したが、本発明はこれに限らず、例えばク
レーンのブーム先端に所望の運搬軌跡を描かせる場合等
にも適用が可能である。
ショベルを示したが、本発明はこれに限らず、例えばク
レーンのブーム先端に所望の運搬軌跡を描かせる場合等
にも適用が可能である。
【0065】(3) 上記実施例では、目標位置に加え、目
標対地角度も設定し、これら目標位置と目標対地角度の
双方を補正するものを示したが、本発明は目標位置のみ
に基づく軌跡制御を行う場合にも有効に適用することが
可能である。
標対地角度も設定し、これら目標位置と目標対地角度の
双方を補正するものを示したが、本発明は目標位置のみ
に基づく軌跡制御を行う場合にも有効に適用することが
可能である。
【0066】
【発明の効果】以上のように本発明は、作業機本体の傾
斜状態を検出し、この傾斜状態に基づいて目標位置の補
正を行い、この補正後の目標位置に基づいて作業軌跡制
御を行うようにしたものであるので、実際に作業機本体
が傾いており、かつ作業中に旋回体が旋回駆動される場
合でも、軌跡制御対象部位の作業軌跡を正確に制御する
ことができる効果がある。しかも、上記作業機本体の傾
斜状態に基づいてまず目標位置を補正し、この補正目標
位置に基づいて目標旋回角度及び目標相対回動角度の演
算を行うようにしているので、補正前の目標位置に基づ
いて目標旋回角度及び目標相対回動角度を演算し、その
後にこれらの角度を補正する場合に比べ、演算内容を著
しく簡略化でき、制御の応答性も高めることができる。
斜状態を検出し、この傾斜状態に基づいて目標位置の補
正を行い、この補正後の目標位置に基づいて作業軌跡制
御を行うようにしたものであるので、実際に作業機本体
が傾いており、かつ作業中に旋回体が旋回駆動される場
合でも、軌跡制御対象部位の作業軌跡を正確に制御する
ことができる効果がある。しかも、上記作業機本体の傾
斜状態に基づいてまず目標位置を補正し、この補正目標
位置に基づいて目標旋回角度及び目標相対回動角度の演
算を行うようにしているので、補正前の目標位置に基づ
いて目標旋回角度及び目標相対回動角度を演算し、その
後にこれらの角度を補正する場合に比べ、演算内容を著
しく簡略化でき、制御の応答性も高めることができる。
【0067】さらに、上記目標位置補正演算手段が、目
標位置設定手段で設定された目標位置を補正目標位置に
変換するための補正用関数を演算し、これを上記作業機
本体の位置が変化しない状態での制御動作中は記憶して
おくように構成されたものによれば、一旦補正用関数を
演算した後は、設定目標位置が変わっても、記憶された
補正用関数が何度も用いられるので、目標位置等が変わ
る度に一から補正演算処理を行う必要がなく、よって制
御の応答性をさらに高めることができる効果がある。
標位置設定手段で設定された目標位置を補正目標位置に
変換するための補正用関数を演算し、これを上記作業機
本体の位置が変化しない状態での制御動作中は記憶して
おくように構成されたものによれば、一旦補正用関数を
演算した後は、設定目標位置が変わっても、記憶された
補正用関数が何度も用いられるので、目標位置等が変わ
る度に一から補正演算処理を行う必要がなく、よって制
御の応答性をさらに高めることができる効果がある。
【図1】本発明の一実施例における油圧ショベルの作業
軌跡制御装置を示すブロック図である。
軌跡制御装置を示すブロック図である。
【図2】上記油圧ショベルの全体側面図である。
【図3】上記作業軌跡制御装置の演算制御動作を示すフ
ローチャートである。
ローチャートである。
【図4】上記油圧ショベルにおいて設定される座標系を
示す説明図である。
示す説明図である。
【図5】上記作業軌跡制御装置による目標旋回角度及び
目標相対回動角度の演算要領を示す説明図である。
目標相対回動角度の演算要領を示す説明図である。
1 下部走行体(作業機本体) 2 上部旋回体 3 ブーム(作業部材) 4 アーム(作業部材) 5 バケット(作業部材) 5a バケット先端部(軌跡制御対象部位) 6 油圧モータ(旋回駆動手段) 7〜9 油圧シリンダ(回動駆動手段) 10 旋回角センサ(角度検出手段) 11 第1関節角センサ(角度検出手段) 12 第2関節角センサ(角度検出手段) 13 第3関節角センサ(角度検出手段) 14,15 本体傾斜角センサ 20 コントローラ 21 目標位置等設定部(目標位置設定手段) 22 目標位置等補正演算部(目標位置補正演算手段) 23 目標角度演算部(目標角度演算手段) 24 アクチュエータ制御部(駆動制御手段)
Claims (2)
- 【請求項1】 作業機本体と、この作業機本体に旋回可
能に設置された旋回体と、互いに相対回動可能に直列に
連結され、そのうちの一つが上記旋回体に相対回動可能
に連結される複数の作業部材と、上記旋回体を旋回駆動
する旋回駆動手段と、各相対回動部位を回動駆動する回
動駆動手段とを備えた作業機械の作業軌跡制御装置にお
いて、上記作業部材における所定の軌跡制御対象部位の
目標位置を設定する目標位置設定手段と、互いに異なる
複数の方向について上記作業機本体の基準面からの傾斜
角を検出する傾斜検出手段と、上記旋回体の旋回角度及
び各相対回動部位における相対回動角度を検出する角度
検出手段と、上記傾斜検出手段で検出された作業機本体
の傾斜角に基づき上記目標位置の補正を行う目標位置補
正演算手段と、この目標位置補正演算手段により演算さ
れた補正目標位置に基づいて上記旋回体の目標旋回角度
及び各相対回動部位での目標相対回動角度を演算する目
標角度演算手段と、この目標相対角度演算手段で演算さ
れた目標相対角度と上記角度検出手段で検出された実際
の角度とに基づいて上記旋回駆動手段及び回動駆動手段
による駆動を制御する駆動制御手段とを備えたことを特
徴とする作業機械の作業軌跡制御装置。 - 【請求項2】 請求項1記載の作業機械の作業軌跡制御
装置において、上記目標位置補正演算手段は、上記作業
機本体の検出傾斜角に基づき、目標位置設定手段で設定
された目標位置を補正目標位置に変換するための補正用
関数を演算し、これを少なくとも上記作業機本体の位置
が変化しない状態での制御動作中は記憶しておくように
構成されていることを特徴とする作業機械の作業軌跡制
御装置。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP11481193A JPH06322789A (ja) | 1993-05-17 | 1993-05-17 | 作業機械の作業軌跡制御装置 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP11481193A JPH06322789A (ja) | 1993-05-17 | 1993-05-17 | 作業機械の作業軌跡制御装置 |
Publications (1)
Publication Number | Publication Date |
---|---|
JPH06322789A true JPH06322789A (ja) | 1994-11-22 |
Family
ID=14647280
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP11481193A Pending JPH06322789A (ja) | 1993-05-17 | 1993-05-17 | 作業機械の作業軌跡制御装置 |
Country Status (1)
Country | Link |
---|---|
JP (1) | JPH06322789A (ja) |
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH10183675A (ja) * | 1996-12-04 | 1998-07-14 | Caterpillar Inc | モービル機械に取付けられ、該モービル機械に対し可動な作業具上の点の位置を求めるための装置と方法 |
CN102459766A (zh) * | 2009-04-20 | 2012-05-16 | 罗伯特·博世有限公司 | 具有工作臂的位置控制装置的移动工作机械和用于控制移动工作机械的工作臂的位置的方法 |
CN103906879A (zh) * | 2012-10-05 | 2014-07-02 | 株式会社小松制作所 | 挖掘机械的显示***、挖掘机械及挖掘机械的显示用计算机程序 |
-
1993
- 1993-05-17 JP JP11481193A patent/JPH06322789A/ja active Pending
Cited By (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH10183675A (ja) * | 1996-12-04 | 1998-07-14 | Caterpillar Inc | モービル機械に取付けられ、該モービル機械に対し可動な作業具上の点の位置を求めるための装置と方法 |
CN102459766A (zh) * | 2009-04-20 | 2012-05-16 | 罗伯特·博世有限公司 | 具有工作臂的位置控制装置的移动工作机械和用于控制移动工作机械的工作臂的位置的方法 |
CN103906879A (zh) * | 2012-10-05 | 2014-07-02 | 株式会社小松制作所 | 挖掘机械的显示***、挖掘机械及挖掘机械的显示用计算机程序 |
US9650755B2 (en) | 2012-10-05 | 2017-05-16 | Komatsu Ltd. | Display system of excavating machine, excavating machine, and display computer program of excavating machine |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP2682891B2 (ja) | パワーショベルの掘削制御装置 | |
JP2566745B2 (ja) | 電子制御油圧掘削機の自動平坦作業方法 | |
KR100240085B1 (ko) | 굴삭기의 조작장치 | |
WO2018051511A1 (ja) | 作業機械 | |
JP6969475B2 (ja) | 建設機械 | |
JP2018135679A (ja) | 作業車両および作業車両の制御方法 | |
JP2002167794A (ja) | 油圧ショベルのフロント制御装置 | |
JPH06322789A (ja) | 作業機械の作業軌跡制御装置 | |
KR100324292B1 (ko) | 3관절형 굴삭기의 조작 제어장치 | |
JP6155873B2 (ja) | 建設機械 | |
JP7314429B2 (ja) | 作業機械 | |
JP6871946B2 (ja) | 作業車両および作業車両の制御方法 | |
JPH0823155B2 (ja) | 作業機の制御装置 | |
WO2020194914A1 (ja) | 作業機械 | |
JP2000303492A (ja) | 建設機械のフロント制御装置 | |
JP3437348B2 (ja) | 建設機械の軌跡制御装置 | |
JP3682352B2 (ja) | 建設機械のフロント制御装置 | |
JP3338540B2 (ja) | 多関節作業機の軌跡制御装置 | |
JP3749319B2 (ja) | 建設機械の軌跡制御装置 | |
JP3705661B2 (ja) | 建設機械の作業機の姿勢制御装置 | |
WO2021070577A1 (ja) | 作業機械および作業機械の制御方法 | |
WO2023153202A1 (ja) | 作業エリア設定システム | |
JP7388870B2 (ja) | ロボットシステムおよび制御装置 | |
JP2721325B2 (ja) | 多関節型エレクタ装置の操作方法とその装置 | |
JP6826234B2 (ja) | 作業機械 |