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)【要約】
【目的】 上部旋回体を備えた作業機械において、作業
機本体が傾いている場合にも作業軌跡を正確に制御す
る。 【構成】 本体傾斜角センサ１４，１５で作業機本体の
傾斜状態を検出し、この傾斜状態に基づき、目標位置等
設定部２１で設定された目標位置を目標位置等補正演算
部２２で補正する。この補正目標位置に基づいて目標角
度演算部２３で上部旋回体の目標旋回角度及び各作業部
材の相対回動角度を演算し、これらの目標角度と、角度
検出センサ１０〜１３で検出される検出角度との比較に
基づき、油圧モータ６及び各油圧シリンダ７〜９による
上記旋回駆動の制御及び相対回動駆動の制御を行う。
機本体が傾いている場合にも作業軌跡を正確に制御す
る。 【構成】 本体傾斜角センサ１４，１５で作業機本体の
傾斜状態を検出し、この傾斜状態に基づき、目標位置等
設定部２１で設定された目標位置を目標位置等補正演算
部２２で補正する。この補正目標位置に基づいて目標角
度演算部２３で上部旋回体の目標旋回角度及び各作業部
材の相対回動角度を演算し、これらの目標角度と、角度
検出センサ１０〜１３で検出される検出角度との比較に
基づき、油圧モータ６及び各油圧シリンダ７〜９による
上記旋回駆動の制御及び相対回動駆動の制御を行う。
Description
【０００１】
【産業上の利用分野】本発明は、油圧ショベルやクレー
ン等の作業機械において、その掘削部やブーム先端等の
作業軌跡を制御するための装置に関するものである。
ン等の作業機械において、その掘削部やブーム先端等の
作業軌跡を制御するための装置に関するものである。
【０００２】
【従来の技術】従来、油圧ショベル等のように複数のア
ームが相対回動可能に直列に連結された多関節作業機で
は、その所定部位（例えばショベル先端）の作業軌跡制
御を行うために、上記所定部位の目標位置を予め設定し
ておき、この目標位置を得るための各関節での目標相対
回動角度を演算し、この目標相対回動角度に実際の相対
回動角度を一致させるようにアクチュエータを制御する
といったことが一般に行われている。
ームが相対回動可能に直列に連結された多関節作業機で
は、その所定部位（例えばショベル先端）の作業軌跡制
御を行うために、上記所定部位の目標位置を予め設定し
ておき、この目標位置を得るための各関節での目標相対
回動角度を演算し、この目標相対回動角度に実際の相対
回動角度を一致させるようにアクチュエータを制御する
といったことが一般に行われている。
【０００３】さらに、特開平２−１０１２２８号公報に
は、作業機本体から見た掘削面の傾斜角θに基づき、こ
の作業機本体の位置を基準に設定される車体固定座標ｘ
−ｙを、掘削面を基準に設定される座標ｘ′−ｙ′に変
換し、この変換後の座標に基づいてアーム先端の作業軌
跡制御を行うことにより、その制御内容をより単純化す
るようにしたものが示されている。
は、作業機本体から見た掘削面の傾斜角θに基づき、こ
の作業機本体の位置を基準に設定される車体固定座標ｘ
−ｙを、掘削面を基準に設定される座標ｘ′−ｙ′に変
換し、この変換後の座標に基づいてアーム先端の作業軌
跡制御を行うことにより、その制御内容をより単純化す
るようにしたものが示されている。
【０００４】
【発明が解決しようとする課題】上記公報に示される装
置では、実際の作業機本体の傾斜状態を考慮せず、掘削
面に対して直交するある一つの基準面上での作業機本体
に対する掘削面の傾斜角θに基づいて、上記掘削面を基
準としたｘ′−ｙ′座標系への座標変換を行い、この座
標系で制御を行っているので、作業機本体に対して旋回
する上部旋回体に上記アームが連結されている作業機
械、すなわち上記基準面から逸脱した領域で作業が行わ
れる作業機械には、適用することができない。
置では、実際の作業機本体の傾斜状態を考慮せず、掘削
面に対して直交するある一つの基準面上での作業機本体
に対する掘削面の傾斜角θに基づいて、上記掘削面を基
準としたｘ′−ｙ′座標系への座標変換を行い、この座
標系で制御を行っているので、作業機本体に対して旋回
する上部旋回体に上記アームが連結されている作業機
械、すなわち上記基準面から逸脱した領域で作業が行わ
れる作業機械には、適用することができない。
【０００５】本発明は、このような事情に鑑み、作業機
本体に旋回可能に設置された旋回体に作業部材が連結さ
れている作業機械において、上記作業機本体の傾斜状態
にかかわらず所定部位の作業軌跡を正確に制御すること
ができる装置を提供することを目的とする。
本体に旋回可能に設置された旋回体に作業部材が連結さ
れている作業機械において、上記作業機本体の傾斜状態
にかかわらず所定部位の作業軌跡を正確に制御すること
ができる装置を提供することを目的とする。
【０００６】
【課題を解決するための手段】上記課題を解決するため
の手段として、本発明は、作業機本体と、この作業機本
体に旋回可能に設置された旋回体と、互いに相対回動可
能に直列に連結され、そのうちの一つが上記旋回体に相
対回動可能に連結される複数の作業部材と、上記旋回体
を旋回駆動する旋回駆動手段と、各相対回動部位を回動
駆動する回動駆動手段とを備えた作業機械の作業軌跡制
御装置において、上記作業部材における所定の軌跡制御
対象部位の目標位置を設定する目標位置設定手段と、互
いに異なる複数の方向について上記作業機本体の基準面
からの傾斜角を検出する傾斜検出手段と、上記旋回体の
旋回角度及び各相対回動部位における相対回動角度を検
出する角度検出手段と、上記傾斜検出手段で検出された
作業機本体の傾斜角に基づき上記目標位置の補正を行う
目標位置補正演算手段と、この目標位置補正演算手段に
より演算された補正目標位置に基づいて上記旋回体の目
標旋回角度及び各相対回動部位での目標相対回動角度を
演算する目標角度演算手段と、この目標相対角度演算手
段で演算された目標相対角度と上記角度検出手段で検出
された実際の角度とに基づいて上記旋回駆動手段及び回
動駆動手段による駆動を制御する駆動制御手段とを備え
たものである（請求項１）。
の手段として、本発明は、作業機本体と、この作業機本
体に旋回可能に設置された旋回体と、互いに相対回動可
能に直列に連結され、そのうちの一つが上記旋回体に相
対回動可能に連結される複数の作業部材と、上記旋回体
を旋回駆動する旋回駆動手段と、各相対回動部位を回動
駆動する回動駆動手段とを備えた作業機械の作業軌跡制
御装置において、上記作業部材における所定の軌跡制御
対象部位の目標位置を設定する目標位置設定手段と、互
いに異なる複数の方向について上記作業機本体の基準面
からの傾斜角を検出する傾斜検出手段と、上記旋回体の
旋回角度及び各相対回動部位における相対回動角度を検
出する角度検出手段と、上記傾斜検出手段で検出された
作業機本体の傾斜角に基づき上記目標位置の補正を行う
目標位置補正演算手段と、この目標位置補正演算手段に
より演算された補正目標位置に基づいて上記旋回体の目
標旋回角度及び各相対回動部位での目標相対回動角度を
演算する目標角度演算手段と、この目標相対角度演算手
段で演算された目標相対角度と上記角度検出手段で検出
された実際の角度とに基づいて上記旋回駆動手段及び回
動駆動手段による駆動を制御する駆動制御手段とを備え
たものである（請求項１）。
【０００７】ここで上記目標位置補正演算手段は、上記
作業機本体の検出傾斜角に基づき、目標位置設定手段で
設定された目標位置を補正目標位置に変換するための補
正用関数を演算し、これを少なくとも上記作業機本体の
位置が変化しない状態での制御動作中は記憶しておくよ
うに構成されていることが、より好ましい（請求項
２）。
作業機本体の検出傾斜角に基づき、目標位置設定手段で
設定された目標位置を補正目標位置に変換するための補
正用関数を演算し、これを少なくとも上記作業機本体の
位置が変化しない状態での制御動作中は記憶しておくよ
うに構成されていることが、より好ましい（請求項
２）。
【０００８】
【作用】請求項１記載の装置では、傾斜検出手段によっ
て基準面（一般には水平面）からの作業機本体の傾斜状
態が直接検出され、この作業機本体の傾斜状態に基づい
て設定目標位置の補正が行われ、この補正目標位置に基
づいて予め旋回駆動及び相対回動駆動が制御されるの
で、作業中に旋回体が旋回する場合でも常に軌跡制御対
象部位の位置を正確に制御することができる。また、先
に設定目標位置の補正が行われ、この補正後の目標位置
に基づいて目標旋回角度及び目標相対回動角度の演算が
行われるので、原目標位置に基づいて目標旋回角度及び
目標相対回動角度を演算してからこれらの角度を補正す
る場合に比べ、演算はより簡略化される。
て基準面（一般には水平面）からの作業機本体の傾斜状
態が直接検出され、この作業機本体の傾斜状態に基づい
て設定目標位置の補正が行われ、この補正目標位置に基
づいて予め旋回駆動及び相対回動駆動が制御されるの
で、作業中に旋回体が旋回する場合でも常に軌跡制御対
象部位の位置を正確に制御することができる。また、先
に設定目標位置の補正が行われ、この補正後の目標位置
に基づいて目標旋回角度及び目標相対回動角度の演算が
行われるので、原目標位置に基づいて目標旋回角度及び
目標相対回動角度を演算してからこれらの角度を補正す
る場合に比べ、演算はより簡略化される。
【０００９】さらに、請求項２記載の装置では、一旦目
標位置補正演算手段で目標位置補正用の補正用関数が演
算された後は、作業機本体の傾斜状態が変化するまでは
上記補正用関数が記憶され、使用されるので、目標位置
が変わる度に一から目標位置の補正演算を行う必要がな
い。このため、より迅速に目標位置の補正演算処理が実
行される。
標位置補正演算手段で目標位置補正用の補正用関数が演
算された後は、作業機本体の傾斜状態が変化するまでは
上記補正用関数が記憶され、使用されるので、目標位置
が変わる度に一から目標位置の補正演算を行う必要がな
い。このため、より迅速に目標位置の補正演算処理が実
行される。
【００１０】
【実施例】本発明の一実施例を図面に基づいて説明す
る。
る。
【００１１】図２は、本発明装置を備えた作業機械であ
る油圧ショベルを示している。この油圧ショベルは、作
業機本体である下部走行体１を備え、この下部走行体１
上に旋回可能に上部旋回体２が設置されている。この上
部旋回体２には、その旋回軸方向（図２では上下方向）
に直交する方向（図２では奥行き方向）の軸周りに相対
回動可能にブーム３の基端が連結されている。同様に、
このブーム３の先端にはアーム４の基端が回動可能に連
結されており、このアーム４の先端にはバケット５が相
対回動可能に連結されている。すなわち、これらのブー
ム３、アーム４、及びバケット５は、互いに相対回動可
能に直列に連結された複数の作業部材を構成している。
る油圧ショベルを示している。この油圧ショベルは、作
業機本体である下部走行体１を備え、この下部走行体１
上に旋回可能に上部旋回体２が設置されている。この上
部旋回体２には、その旋回軸方向（図２では上下方向）
に直交する方向（図２では奥行き方向）の軸周りに相対
回動可能にブーム３の基端が連結されている。同様に、
このブーム３の先端にはアーム４の基端が回動可能に連
結されており、このアーム４の先端にはバケット５が相
対回動可能に連結されている。すなわち、これらのブー
ム３、アーム４、及びバケット５は、互いに相対回動可
能に直列に連結された複数の作業部材を構成している。
【００１２】この油圧ショベルは、油圧モータ６、第１
油圧シリンダ７、第２油圧シリンダ８、及び第３油圧シ
リンダ９を備えている。
油圧シリンダ７、第２油圧シリンダ８、及び第３油圧シ
リンダ９を備えている。
【００１３】油圧モータ６は、その作動により、上記下
部走行体１に対して上部旋回体２を旋回駆動する旋回駆
動手段である。
部走行体１に対して上部旋回体２を旋回駆動する旋回駆
動手段である。
【００１４】第１油圧シリンダ７は、その両端が下部走
行体２とブーム３とに連結され、それ自身の伸縮によっ
て上部旋回体２に対してブーム３を相対回転駆動するも
のである。同様に、第２油圧シリンダ８及び第３油圧シ
リンダ９は、ブーム３とアーム４との間、アーム４とバ
ケット５との間にそれぞれ連結されており、これら油圧
シリンダ８，９自身の伸縮で、ブーム３に対するアーム
４の相対回動駆動、及びアーム４に対するバケット５の
相対回動駆動がそれぞれ行われるようになっている。す
なわち、これらの油圧シリンダ７〜９によって本発明に
おける回動駆動手段が構成されている。
行体２とブーム３とに連結され、それ自身の伸縮によっ
て上部旋回体２に対してブーム３を相対回転駆動するも
のである。同様に、第２油圧シリンダ８及び第３油圧シ
リンダ９は、ブーム３とアーム４との間、アーム４とバ
ケット５との間にそれぞれ連結されており、これら油圧
シリンダ８，９自身の伸縮で、ブーム３に対するアーム
４の相対回動駆動、及びアーム４に対するバケット５の
相対回動駆動がそれぞれ行われるようになっている。す
なわち、これらの油圧シリンダ７〜９によって本発明に
おける回動駆動手段が構成されている。
【００１５】図１に示すように、この油圧ショベルに
は、旋回角センサ１０、第１関節角センサ１１、第２関
節角センサ１２、第３関節角センサ１３、及び本体傾斜
角センサ１４，１５が配設されている。
は、旋回角センサ１０、第１関節角センサ１１、第２関
節角センサ１２、第３関節角センサ１３、及び本体傾斜
角センサ１４，１５が配設されている。
【００１６】上記旋回角センサ１０は、下部走行体１に
対する上部旋回体２の旋回角度θ１を検出するものであ
り、第１関節角センサ１１、第２関節角センサ１２、及
び第３関節角センサ１３は、上部旋回体２に対するブー
ム３の相対回動角度θ２、ブーム３に対するアーム４の
相対回動角度θ３、アーム４に対するバケット５の相対
回動角度θ４をそれぞれ検出するものである。すなわ
ち、これらのセンサ１０〜１３によって、本発明におけ
る角度検出手段が構成されている。
対する上部旋回体２の旋回角度θ１を検出するものであ
り、第１関節角センサ１１、第２関節角センサ１２、及
び第３関節角センサ１３は、上部旋回体２に対するブー
ム３の相対回動角度θ２、ブーム３に対するアーム４の
相対回動角度θ３、アーム４に対するバケット５の相対
回動角度θ４をそれぞれ検出するものである。すなわ
ち、これらのセンサ１０〜１３によって、本発明におけ
る角度検出手段が構成されている。
【００１７】本体傾斜角センサ１４，１５は、下部走行
体１に設置されている。本体傾斜角センサ１４は、図４
に示すように、水平面Ｈを基準とするＸｗ−Ｙｗ−Ｚｗ
座標系（より具体的には原点Ｏｗが上部旋回体２の旋回
中心であって水平面Ｈに対してＹｗ軸が直交する座標
系；以下、座標系Σｗと称する。）と、下部走行体１を
基準とするＸｏ−Ｙｏ−Ｚｏ座標系（より具体的には原
点Ｏｏが上部旋回体２の旋回中心であってその旋回軸に
Ｙｏ軸が平行な座標系；以下、座標系Σｏと称する。）
とを設定した場合に、Ｘｗ−Ｙｗ平面とＸｏ軸とのなす
角度αを検出するものであり、本体傾斜角センサ１５
は、上記Ｘｗ−Ｙｗ平面とＺｏ軸とのなす角度βを検出
するものである。すなわち、これらの本体傾斜角センサ
１４，１５により、互いに異なる２方向について基準面
（この実施例では水平面）に対する下部走行体１の傾斜
角度を検出する傾斜検出手段が構成されている。
体１に設置されている。本体傾斜角センサ１４は、図４
に示すように、水平面Ｈを基準とするＸｗ−Ｙｗ−Ｚｗ
座標系（より具体的には原点Ｏｗが上部旋回体２の旋回
中心であって水平面Ｈに対してＹｗ軸が直交する座標
系；以下、座標系Σｗと称する。）と、下部走行体１を
基準とするＸｏ−Ｙｏ−Ｚｏ座標系（より具体的には原
点Ｏｏが上部旋回体２の旋回中心であってその旋回軸に
Ｙｏ軸が平行な座標系；以下、座標系Σｏと称する。）
とを設定した場合に、Ｘｗ−Ｙｗ平面とＸｏ軸とのなす
角度αを検出するものであり、本体傾斜角センサ１５
は、上記Ｘｗ−Ｙｗ平面とＺｏ軸とのなす角度βを検出
するものである。すなわち、これらの本体傾斜角センサ
１４，１５により、互いに異なる２方向について基準面
（この実施例では水平面）に対する下部走行体１の傾斜
角度を検出する傾斜検出手段が構成されている。
【００１８】上記各センサ１０〜１５の検出信号はコン
トローラ２０に入力されるようになっている。このコン
トローラ２０は、各センサ１０〜１５の検出信号に基づ
き、上記油圧モータ６及び各油圧シリンダ７〜９の駆動
制御を行うように構成されている。
トローラ２０に入力されるようになっている。このコン
トローラ２０は、各センサ１０〜１５の検出信号に基づ
き、上記油圧モータ６及び各油圧シリンダ７〜９の駆動
制御を行うように構成されている。
【００１９】具体的に、このコントローラ２０は、目標
位置等設定部（目標位置設定手段）２１、目標位置等補
正演算部（目標位置補正演算手段）２２、目標角度演算
部（目標角度演算手段）２３、及びアクチュエータ制御
部（駆動制御手段）２４を備えている。
位置等設定部（目標位置設定手段）２１、目標位置等補
正演算部（目標位置補正演算手段）２２、目標角度演算
部（目標角度演算手段）２３、及びアクチュエータ制御
部（駆動制御手段）２４を備えている。
【００２０】目標位置等設定部２１は、上記油圧ショベ
ルにおける軌跡制御対象部位であるバケット５の先端部
５ａに所望の作業軌跡を描かせるための同先端部５ａの
目標位置（ｘwr，ｙwr，ｚwr）及び目標対地角度θｗｂ
を、上記座標系Σｏを基準に設定するものである。
ルにおける軌跡制御対象部位であるバケット５の先端部
５ａに所望の作業軌跡を描かせるための同先端部５ａの
目標位置（ｘwr，ｙwr，ｚwr）及び目標対地角度θｗｂ
を、上記座標系Σｏを基準に設定するものである。
【００２１】目標位置等補正演算部２２は、本体傾斜角
センサ１４，１５で検出された下部走行体１の傾斜状態
に基づき、上記設定目標位置（ｘwr，ｙwr，ｚwr）及び
設定目標対地角度θwbを上記座標系Σｏにおける目標位
置（ｘor，ｙor，ｚor）及び目標対地角度θobに補正す
るものである。
センサ１４，１５で検出された下部走行体１の傾斜状態
に基づき、上記設定目標位置（ｘwr，ｙwr，ｚwr）及び
設定目標対地角度θwbを上記座標系Σｏにおける目標位
置（ｘor，ｙor，ｚor）及び目標対地角度θobに補正す
るものである。
【００２２】目標角度演算部２３は、上記目標位置等補
正演算部２２で演算された補正目標位置及び補正目標対
地角度に基づき、図５に示すような、上記目標位置及び
目標対地角度を得るための上部旋回体２の目標旋回角度
θ1r、上部旋回体２に対するブーム３の目標相対回動角
度θ2r、ブーム３に対するアーム４の目標相対回動角度
θ3r、及びアーム４に対するバケット５の目標相対回動
角度θ4rをそれぞれ演算するものである。
正演算部２２で演算された補正目標位置及び補正目標対
地角度に基づき、図５に示すような、上記目標位置及び
目標対地角度を得るための上部旋回体２の目標旋回角度
θ1r、上部旋回体２に対するブーム３の目標相対回動角
度θ2r、ブーム３に対するアーム４の目標相対回動角度
θ3r、及びアーム４に対するバケット５の目標相対回動
角度θ4rをそれぞれ演算するものである。
【００２３】アクチュエータ制御部２４は、上記目標角
度演算部２３で演算された目標角度θ1r〜θ4rと、各セ
ンサ１０〜１３により検出された実際の検出角度θ１〜
θ４とに基づき、両者を合致させるように上記油圧モー
タ６及び各油圧シリンダ７〜９による旋回駆動及び回動
駆動のフィードバック制御を行うものである。
度演算部２３で演算された目標角度θ1r〜θ4rと、各セ
ンサ１０〜１３により検出された実際の検出角度θ１〜
θ４とに基づき、両者を合致させるように上記油圧モー
タ６及び各油圧シリンダ７〜９による旋回駆動及び回動
駆動のフィードバック制御を行うものである。
【００２４】次に、このコントローラ２０の行う演算制
御動作を図３のフローチャートを参照しながら説明す
る。
御動作を図３のフローチャートを参照しながら説明す
る。
【００２５】まず、目標位置等設定手段２１は、本体傾
斜角センサ１４，１５で検出された本体傾斜角α，βを
取込み（ステップＳ１）、これらに基づき、上記座標系
Σｗをもとに設定された目標位置及び目標対地角度を上
記座標系Σｏでの位置及び対地角度に補正するための補
正マトリックスＴowを演算し、これを記憶する（ステッ
プＳ２）。
斜角センサ１４，１５で検出された本体傾斜角α，βを
取込み（ステップＳ１）、これらに基づき、上記座標系
Σｗをもとに設定された目標位置及び目標対地角度を上
記座標系Σｏでの位置及び対地角度に補正するための補
正マトリックスＴowを演算し、これを記憶する（ステッ
プＳ２）。
【００２６】具体的には、図４に示すように、上記座標
系ΣｏがＸｗ−Ｚｗ平面上の単位ベクトルｗ＝（λ，
０，ν）周りにθｏだけ傾いているとすると、座標系Σ
ｗから座標系Σｏへの座標変換マトリックス（すなわち
補正マトリックス）Ｔowは次式で表される。
系ΣｏがＸｗ−Ｚｗ平面上の単位ベクトルｗ＝（λ，
０，ν）周りにθｏだけ傾いているとすると、座標系Σ
ｗから座標系Σｏへの座標変換マトリックス（すなわち
補正マトリックス）Ｔowは次式で表される。
【００２７】
【数１】
【００２８】このような補正マトリックスＴowの演算、
記憶が終了した後、実際の制御動作が開始される。すな
わち、座標系Σｗに基づき、目標位置等設定部２１によ
り目標位置（ｘwr，ｙwr，ｚwr）及び目標対地角度θwb
が設定され（ステップＳ３）、次いで、これら目標位置
（ｘwr，ｙwr，ｚwr）及び目標対地角度θwbに基づいて
目標位置等補正演算部２２により補正目標位置（ｘor，
ｙor，ｚor）及び補正目標対地角度θobが演算される
（ステップＳ４，Ｓ５）。
記憶が終了した後、実際の制御動作が開始される。すな
わち、座標系Σｗに基づき、目標位置等設定部２１によ
り目標位置（ｘwr，ｙwr，ｚwr）及び目標対地角度θwb
が設定され（ステップＳ３）、次いで、これら目標位置
（ｘwr，ｙwr，ｚwr）及び目標対地角度θwbに基づいて
目標位置等補正演算部２２により補正目標位置（ｘor，
ｙor，ｚor）及び補正目標対地角度θobが演算される
（ステップＳ４，Ｓ５）。
【００２９】まず、補正目標位置（ｘor，ｙor，ｚor）
については、既に記憶されている補正マトリックスＴow
を用いて次式により演算される。
については、既に記憶されている補正マトリックスＴow
を用いて次式により演算される。
【００３０】
【数２】
【００３１】また、補正目標対地角度θobには、次式
(6)，(7)の双方を同時に満たす値が採用される。
(6)，(7)の双方を同時に満たす値が採用される。
【００３２】
【数３】
【００３３】次に、目標角度演算部２３は、上記補正目
標位置（ｘor，ｙor，ｚor）と目標対地角度θobとに基
づき、上部旋回体２の目標旋回角度θ1r、上部旋回体２
に対するブーム３の目標相対回動角度θ2r、ブーム３に
対するアーム４の目標相対回動角度θ3r、及びアーム４
に対するバケット５の目標相対回動角度θ4rを演算する
（ステップＳ６）。
標位置（ｘor，ｙor，ｚor）と目標対地角度θobとに基
づき、上部旋回体２の目標旋回角度θ1r、上部旋回体２
に対するブーム３の目標相対回動角度θ2r、ブーム３に
対するアーム４の目標相対回動角度θ3r、及びアーム４
に対するバケット５の目標相対回動角度θ4rを演算する
（ステップＳ６）。
【００３４】その演算要領を図５を参照しながら説明す
る。いま、原点Ｏｏからのブーム３のオフセット量をＬ
1x，Ｌ1y，Ｌ1zとし、ブーム３、アーム４、及びバケッ
ト５のアーム長さをそれぞれＬ2，Ｌ3，Ｌ4とすると、
図５から次式が導かれる。
る。いま、原点Ｏｏからのブーム３のオフセット量をＬ
1x，Ｌ1y，Ｌ1zとし、ブーム３、アーム４、及びバケッ
ト５のアーム長さをそれぞれＬ2，Ｌ3，Ｌ4とすると、
図５から次式が導かれる。
【００３５】
【数４】
【００３６】ここで、上式を簡略化するため変数ａを導
入すると次式が得られる。
入すると次式が得られる。
【００３７】
【数５】
【００３８】（式(13))2＋(式(14))2により次式が得ら
れる。
れる。
【００３９】
【数６】
【００４０】ここで、上部旋回体２の目標旋回角度θ1r
は以下のようにして求められる。まず、上記式(13)によ
り、次式が得られる。
は以下のようにして求められる。まず、上記式(13)によ
り、次式が得られる。
【００４１】
【数７】
【００４２】この式を上記式(14)に代入すると次式が得
られる。
られる。
【００４３】
【数８】
【００４４】よってＣ1は次式で与えられる。
【００４５】
【数９】
【００４６】従って、目標旋回角度θ1rは次の２つの式
を同時に満たす角度となる。
を同時に満たす角度となる。
【００４７】
【数１０】
【００４８】次に、上記式(15)，(9)からそれぞれ次式
(17)，(18)が得られる。
(17)，(18)が得られる。
【００４９】
【数１１】
【００５０】これらの式(17)，(18)の右辺をそれぞれ
ｘ′，ｙ′とおくと、次式が得られる。
ｘ′，ｙ′とおくと、次式が得られる。
【００５１】
【数１２】
【００５２】（式(19))2＋(式(20))2により次式が得ら
れる。
れる。
【００５３】
【数１３】
【００５４】上記２つの式の双方を同時に満たす値が目
標相対角度θ3rとして採用される。また、上記式(19)，
(20)に三角関数の加法定理を適用すると、次式が得られ
る。
標相対角度θ3rとして採用される。また、上記式(19)，
(20)に三角関数の加法定理を適用すると、次式が得られ
る。
【００５５】
【数１４】
【００５６】これらの式(21)，(22)を整理すると次式が
得られる。
得られる。
【００５７】
【数１５】
【００５８】ここで、上記式(23)，(24)を同時に満たす
値が目標相対角度θ2rとして採用される。
値が目標相対角度θ2rとして採用される。
【００５９】一方、アクチュエータ制御部２４は、各角
度センサ１０〜１３による検出角度θ１〜θ４を取込み
（ステップＳ７）、これらの角度と、上述の要領で算出
された目標角度θ1r〜θ4rとを比較し、後者に前者が近
づくように油圧モータ６及び各油圧シリンダ７〜９の駆
動制御を行う（ステップＳ８）。以下、制御が終了する
まで、目標作業軌跡を得るための目標位置が一定の周期
で設定され（ステップＳ３）、その後、記憶済みの補正
マトリックスＴowを用いてステップＳ４以降の演算制御
動作が繰り返される（ステップＳ９でＮＯ）。
度センサ１０〜１３による検出角度θ１〜θ４を取込み
（ステップＳ７）、これらの角度と、上述の要領で算出
された目標角度θ1r〜θ4rとを比較し、後者に前者が近
づくように油圧モータ６及び各油圧シリンダ７〜９の駆
動制御を行う（ステップＳ８）。以下、制御が終了する
まで、目標作業軌跡を得るための目標位置が一定の周期
で設定され（ステップＳ３）、その後、記憶済みの補正
マトリックスＴowを用いてステップＳ４以降の演算制御
動作が繰り返される（ステップＳ９でＮＯ）。
【００６０】このような装置によれば、下部走行体１が
傾いており、また作業中に上部旋回体２が旋回駆動され
る場合でも、上記下部走行体１の実際の傾斜状態に基づ
く目標位置等の補正によって、バケット先端部５ａの作
業軌跡制御を正確に行うことができる。しかも、上記下
部走行体１の傾斜状態によってまず目標位置等を補正
し、この補正目標位置に基づいて目標旋回角度及び目標
相対回動角度の演算を行っているので、補正前の目標位
置等に基づいて目標旋回角度及び目標相対回動角度を演
算し、その後にこれらの角度を補正する場合に比べ、演
算内容は著しく簡略化される。
傾いており、また作業中に上部旋回体２が旋回駆動され
る場合でも、上記下部走行体１の実際の傾斜状態に基づ
く目標位置等の補正によって、バケット先端部５ａの作
業軌跡制御を正確に行うことができる。しかも、上記下
部走行体１の傾斜状態によってまず目標位置等を補正
し、この補正目標位置に基づいて目標旋回角度及び目標
相対回動角度の演算を行っているので、補正前の目標位
置等に基づいて目標旋回角度及び目標相対回動角度を演
算し、その後にこれらの角度を補正する場合に比べ、演
算内容は著しく簡略化される。
【００６１】さらに、この実施例では、上記補正を行う
ための補正マトリックス（補正用関数）Ｔowを一旦演算
した後は、これを記憶してしまい、下部走行体１の傾斜
状態が変わるまで（この実施例では制御終了まで）は目
標位置等が変わっても同じ記憶補正マトリックスＴowを
何度も用いるようにしているので、目標位置等が変わる
度に一から補正マトリックスＴowを演算する必要がな
く、従ってより迅速に演算処理を行って応答性を高める
ことができる。
ための補正マトリックス（補正用関数）Ｔowを一旦演算
した後は、これを記憶してしまい、下部走行体１の傾斜
状態が変わるまで（この実施例では制御終了まで）は目
標位置等が変わっても同じ記憶補正マトリックスＴowを
何度も用いるようにしているので、目標位置等が変わる
度に一から補正マトリックスＴowを演算する必要がな
く、従ってより迅速に演算処理を行って応答性を高める
ことができる。
【００６２】なお、本発明はこのような実施例に限定さ
れるものでなく、例として次のような態様をとることも
可能である。
れるものでなく、例として次のような態様をとることも
可能である。
【００６３】(1) 上記実施例では、３つの作業部材（ブ
ーム３、アーム４、バケット５）を備えた油圧ショベル
を示したが、本発明は、少なくとも２つの作業部材を備
えた作業機械について適用が可能である。
ーム３、アーム４、バケット５）を備えた油圧ショベル
を示したが、本発明は、少なくとも２つの作業部材を備
えた作業機械について適用が可能である。
【００６４】(2) 上記実施例では、作業機械として油圧
ショベルを示したが、本発明はこれに限らず、例えばク
レーンのブーム先端に所望の運搬軌跡を描かせる場合等
にも適用が可能である。
ショベルを示したが、本発明はこれに限らず、例えばク
レーンのブーム先端に所望の運搬軌跡を描かせる場合等
にも適用が可能である。
【００６５】(3) 上記実施例では、目標位置に加え、目
標対地角度も設定し、これら目標位置と目標対地角度の
双方を補正するものを示したが、本発明は目標位置のみ
に基づく軌跡制御を行う場合にも有効に適用することが
可能である。
標対地角度も設定し、これら目標位置と目標対地角度の
双方を補正するものを示したが、本発明は目標位置のみ
に基づく軌跡制御を行う場合にも有効に適用することが
可能である。
【００６６】
【発明の効果】以上のように本発明は、作業機本体の傾
斜状態を検出し、この傾斜状態に基づいて目標位置の補
正を行い、この補正後の目標位置に基づいて作業軌跡制
御を行うようにしたものであるので、実際に作業機本体
が傾いており、かつ作業中に旋回体が旋回駆動される場
合でも、軌跡制御対象部位の作業軌跡を正確に制御する
ことができる効果がある。しかも、上記作業機本体の傾
斜状態に基づいてまず目標位置を補正し、この補正目標
位置に基づいて目標旋回角度及び目標相対回動角度の演
算を行うようにしているので、補正前の目標位置に基づ
いて目標旋回角度及び目標相対回動角度を演算し、その
後にこれらの角度を補正する場合に比べ、演算内容を著
しく簡略化でき、制御の応答性も高めることができる。
斜状態を検出し、この傾斜状態に基づいて目標位置の補
正を行い、この補正後の目標位置に基づいて作業軌跡制
御を行うようにしたものであるので、実際に作業機本体
が傾いており、かつ作業中に旋回体が旋回駆動される場
合でも、軌跡制御対象部位の作業軌跡を正確に制御する
ことができる効果がある。しかも、上記作業機本体の傾
斜状態に基づいてまず目標位置を補正し、この補正目標
位置に基づいて目標旋回角度及び目標相対回動角度の演
算を行うようにしているので、補正前の目標位置に基づ
いて目標旋回角度及び目標相対回動角度を演算し、その
後にこれらの角度を補正する場合に比べ、演算内容を著
しく簡略化でき、制御の応答性も高めることができる。
【００６７】さらに、上記目標位置補正演算手段が、目
標位置設定手段で設定された目標位置を補正目標位置に
変換するための補正用関数を演算し、これを上記作業機
本体の位置が変化しない状態での制御動作中は記憶して
おくように構成されたものによれば、一旦補正用関数を
演算した後は、設定目標位置が変わっても、記憶された
補正用関数が何度も用いられるので、目標位置等が変わ
る度に一から補正演算処理を行う必要がなく、よって制
御の応答性をさらに高めることができる効果がある。
標位置設定手段で設定された目標位置を補正目標位置に
変換するための補正用関数を演算し、これを上記作業機
本体の位置が変化しない状態での制御動作中は記憶して
おくように構成されたものによれば、一旦補正用関数を
演算した後は、設定目標位置が変わっても、記憶された
補正用関数が何度も用いられるので、目標位置等が変わ
る度に一から補正演算処理を行う必要がなく、よって制
御の応答性をさらに高めることができる効果がある。
【図１】本発明の一実施例における油圧ショベルの作業
軌跡制御装置を示すブロック図である。
軌跡制御装置を示すブロック図である。
【図２】上記油圧ショベルの全体側面図である。
【図３】上記作業軌跡制御装置の演算制御動作を示すフ
ローチャートである。
ローチャートである。
【図４】上記油圧ショベルにおいて設定される座標系を
示す説明図である。
示す説明図である。
【図５】上記作業軌跡制御装置による目標旋回角度及び
目標相対回動角度の演算要領を示す説明図である。
目標相対回動角度の演算要領を示す説明図である。
１ 下部走行体（作業機本体） ２ 上部旋回体 ３ ブーム（作業部材） ４ アーム（作業部材） ５ バケット（作業部材） ５ａ バケット先端部（軌跡制御対象部位） ６ 油圧モータ（旋回駆動手段） ７〜９ 油圧シリンダ（回動駆動手段） １０ 旋回角センサ（角度検出手段） １１ 第１関節角センサ（角度検出手段） １２ 第２関節角センサ（角度検出手段） １３ 第３関節角センサ（角度検出手段） １４，１５ 本体傾斜角センサ ２０ コントローラ ２１ 目標位置等設定部（目標位置設定手段） ２２ 目標位置等補正演算部（目標位置補正演算手段） ２３ 目標角度演算部（目標角度演算手段） ２４ アクチュエータ制御部（駆動制御手段）
Claims (2)
- 【請求項１】 作業機本体と、この作業機本体に旋回可
能に設置された旋回体と、互いに相対回動可能に直列に
連結され、そのうちの一つが上記旋回体に相対回動可能
に連結される複数の作業部材と、上記旋回体を旋回駆動
する旋回駆動手段と、各相対回動部位を回動駆動する回
動駆動手段とを備えた作業機械の作業軌跡制御装置にお
いて、上記作業部材における所定の軌跡制御対象部位の
目標位置を設定する目標位置設定手段と、互いに異なる
複数の方向について上記作業機本体の基準面からの傾斜
角を検出する傾斜検出手段と、上記旋回体の旋回角度及
び各相対回動部位における相対回動角度を検出する角度
検出手段と、上記傾斜検出手段で検出された作業機本体
の傾斜角に基づき上記目標位置の補正を行う目標位置補
正演算手段と、この目標位置補正演算手段により演算さ
れた補正目標位置に基づいて上記旋回体の目標旋回角度
及び各相対回動部位での目標相対回動角度を演算する目
標角度演算手段と、この目標相対角度演算手段で演算さ
れた目標相対角度と上記角度検出手段で検出された実際
の角度とに基づいて上記旋回駆動手段及び回動駆動手段
による駆動を制御する駆動制御手段とを備えたことを特
徴とする作業機械の作業軌跡制御装置。 - 【請求項２】 請求項１記載の作業機械の作業軌跡制御
装置において、上記目標位置補正演算手段は、上記作業
機本体の検出傾斜角に基づき、目標位置設定手段で設定
された目標位置を補正目標位置に変換するための補正用
関数を演算し、これを少なくとも上記作業機本体の位置
が変化しない状態での制御動作中は記憶しておくように
構成されていることを特徴とする作業機械の作業軌跡制
御装置。
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) | 굴삭기의 조작장치 | |
| JP6969475B2 (ja) | 建設機械 | |
| EP3719212A1 (en) | System for remapping a control signal for excavator arm movement to a rotatory degree of freedom of a tool | |
| 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) | ロボットシステムおよび制御装置 | |
| JP6826234B2 (ja) | 作業機械 | |
| WO2023149104A1 (ja) | 作業機械および作業機械の制御方法 | |
| JPS6136426A (ja) | 腕式作業機の作業具軌跡制御装置 |