JPH0726577A - 多関節作業機の動作速度制御装置 - Google Patents

Abstract

(57)【要約】 【目的】 アクチュエータの最大動作速度の範囲内で適
正に軌跡制御を実行することができるようにする。 【構成】 動作速度制御装置Ｋは、各関節部の関節角を
検知する関節角センサ４１，４２，４３から入力された
関節角θ_iと、予め入力されたアクチュエータの最大動
作速度Ｖ_imin、Ｖ_imaxとからその関節角速度の最大値(d
θ_i/dt)_min、(dθ_i/dt)_maxを演算する角速度最大値演算
部６と、上記関節角センサ４１、４２、４３から入力さ
れた関節角θ_iと角速度最大値演算部６から入力された
関節角速度の最大値(dθ_i/dt)_min、(dθ_i/dt)_maxと予め
入力された最先端部腕体の動作方向とからこの最先端部
腕体の目標速度Ｖ_mと目標位置（ｘ_r，ｙ_r）とを演算す
る目標動作演算部７と、この目標動作演算部７から入力
される上記目標速度と目標位置とから各関節部の関節角
を演算する目標関節角演算部９とから構成されている。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は、油圧ショベル等で代表
される多関節作業機械の動作速度を制御する制御装置に
関するものである。

【０００２】

【従来の技術】従来、油圧ショベル等の多関節作業機械
は、複数のブームやアーム等がそれぞれ互いに軸回りに
回動自在に接合され、油圧機器からなるアクチュエータ
で自在に回動運動を行うように構成されている。このよ
うなアームの最先端には所定の作業を行うバケット等の
作業端が設けられており、この作業端が所定の作業を行
うようになっている。

【０００３】従来、上記作業端に所定の作業を行わせる
ためには、作業者が黙視しながら上記アクチュエータを
運転操作することによって行われていたが、近年、一定
の作業については、制御装置の制御の基に、自動的に作
業端が作業を行うように構成されていることが多い。

【０００４】このような作業機の自動制御については、
特開平２−１０１２２８号公報によって開示されたもの
が知られている。この公報に開示されている作業機の制
御装置は、ブームとアームとバケットとが直列に連結さ
れた作業機において、ブーム角、アーム角およびバケッ
ト角を検出する角度検出手段が設けられ、この角度検出
手段が検出した上記各角度からバケットの位置を逐一算
出するようにし、この算出されたバケットの現在位置と
予め入力されたバケットの目標軌跡とが一致しているか
否かを算出するようにし（具体的には上記両者間の偏差
を求めるようにし）、一致していないときには（偏差が
予め設定された値よりも大きいときには）ブーム、アー
ムおよびバケットを作動させる各々のアクチュエータへ
の流体の供給量を調節してバケットの実際の位置と目標
軌跡とを合致させるいわゆる軌跡制御が行われるように
構成されたものである。

【０００５】そして、このように構成することにより、
バケットの刃先の位置を精度よく目標軌跡に追随させる
ことができ、オペレータが操作レバーに手を触れずに高
精度の自動掘削を行うことができると説明されている。

【０００６】

【発明が解決しようとする課題】ところで、上記特開平
２−１０１２２８号公報によって開示された作業機の制
御装置においては、予め設定された目標軌跡にバケット
を沿わせることにのみ意が払われ、各所のアクチュエー
タの動作速度や各駆動部の構造特性については考慮され
ていないため、各所に形成されているリンク構造の姿勢
によっては、ブーム、アームおよびバケットを作動させ
る各々のアクチュエータの動作速度が追い付かず、結局
バケットは実質的に予め設定されている目標軌跡からは
ずれてしまうという問題点を有していた。

【０００７】本発明は、上記のような問題点を解決する
ためになされたものであり、アクチュエータの最大動作
速度の範囲内で適正に軌跡制御を実行することができる
多関節作業機の動作速度制御装置を提供することを目的
としている。

【０００８】

【課題を解決するための手段】本発明の請求項１記載の
多関節作業機の動作速度制御装置は、複数の腕体が関節
部を介して直列に連結され、かつ、各腕体を関節部の支
持軸回りに回動させる複数のアクチュエータが設けられ
てなる多関節作業機械の最先端部腕体の動作を制御する
多関節作業機の動作速度制御装置において、この動作速
度制御装置は、各関節部の関節角に応じたアクチュエー
タの限界作動速度を演算し、上記各アクチュエータがこ
の限界作動速度を超えない範囲で作動するように各アク
チュエータに動作信号を発信するように構成されている
ことを特徴とするものである。

【０００９】本発明の請求項２記載の多関節作業機の動
作速度制御装置は、請求項１記載の多関節作業機の動作
速度制御装置において、上記動作速度制御装置は、各関
節部の関節角を検知する関節角センサから入力された関
節角と予め入力されたアクチュエータの最大動作速度と
からその関節角速度の最大値を演算する角速度最大値演
算部と、上記関節角センサから入力された関節角と角速
度最大値演算部から入力された関節角速度の最大値と予
め入力された最先端部腕体の動作方向とからこの最先端
部腕体の目標速度と目標位置とを演算する目標動作演算
部と、この目標動作演算部から入力される上記目標速度
と目標位置とから各関節部の関節角を演算する目標関節
角演算部とからなり、この目標関節角演算部は各関節部
が上記演算された関節角になるように各アクチュエータ
に動作信号を発信するように構成されていることを特徴
とするものである。

【００１０】

【作用】上記請求項１記載の多関節作業機の動作速度制
御装置によれば、動作速度制御装置は、各関節部の関節
角に応じたアクチュエータの限界作動速度を演算し、上
記各アクチュエータがこの限界作動速度を超えない範囲
で作動するように各アクチュエータに動作信号を発信す
るようになっているため、この信号を受信した各アクチ
ュエータは無理のない状態で作動することができ、結果
として最先端腕体は予め設定された目標軌跡に確実に沿
う動作を確実に行うように制御することが可能になる。

【００１１】上記請求項２記載の多関節作業機の動作速
度制御装置によれば、まず動作速度制御装置の角速度最
大値演算部は、各関節部の関節角を検知する関節角セン
サから入力された関節角と、予め入力されたアクチュエ
ータの最大動作速度とからその関節角速度の最大値を演
算しその演算結果を目標動作演算部に伝達する。

【００１２】この関節角速度の最大値が入力された目標
動作演算部は、上記関節角センサから入力された関節角
と予め入力された最先端部腕体の動作方向とを合わせて
最先端部腕体の目標速度と目標位置とを演算しこの演算
結果を目標関節角演算部に伝達する。

【００１３】そして、上記最先端部腕体の目標速度と目
標位置とが入力された目標関節角演算部は各関節部の関
節角を演算し、この演算結果に基づいて各関節部が上記
演算された関節角になるように各アクチュエータに動作
信号を発信する。

【００１４】従って、この動作信号を受信した各アクチ
ュエータは、上記目標関節角演算部が演算した関節角に
なるように作動し、この作動は予め角速度最大値演算部
によって演算された関節角速度最大値以下になるように
設定することが可能であるため、各アクチュエータの作
動はその最大動作速度を超えないものになり、無理のな
い状態で最先端部腕体は予め設定された目標軌跡上を移
動する。

【００１５】

【実施例】図１は、本発明に係る動作速度制御装置が適
用された多関節作業機の一例を示す側面図である。この
図に示すように、本実施例においては、多関節作業機と
して油圧ショベル１が適用されている。従って、以下こ
の油圧ショベル１によってオペレータによる運転操作で
は非常に困難とされる直線状掘削作業を、非常に短い範
囲で行わせるようにし、この短い範囲の直線状掘削作業
の集積として任意の軌跡の掘削を行わせるようにした動
作速度制御装置について具体的に説明する。

【００１６】油圧ショベル１は、キャタピラ装置２と、
このキャタピラ装置２の上部に設けられた運転操作室３
と、この運転操作室３の前部から前方に突出するように
設けられた多関節腕体Ｌとから基本構成されている。上
記多関節腕体Ｌは、運転操作室３に第一水平軸３０１回
りに回動自在に直接連結された第一の腕体としてのブー
ムＬ１と、このブームＬ１の先端部に第二水平軸３０２
回りに回動自在に軸支された第二の腕体としてのアーム
Ｌ２と、さらにこのアームＬ２に先端部に第三水平軸３
０３回りに回動自在に設けられた第三の腕体（最先端部
腕体）としてのバケットＬ３とから構成されている。

【００１７】そして、上記ブームＬ１、アームＬ２およ
びバケットＬ３は多関節腕体Ｌに複数設けられた油圧で
作動するアクチュエータ３０によって各水平軸回りに回
動動作をするようになっている。具体的には、運転操作
室３とブームＬ１との間にはブーム用アクチュエータ３
１が付設され、ブームＬ１とアームＬ２との間にはアー
ム用アクチュエータ３２が付設され、アームＬ２とバケ
ットＬ３との間にはバケット用アクチュエータ３３が設
けられている。そして各アクチュエータ３０に所定の作
動をさせることによって、各多関節腕体Ｌは所定の動作
を行い、バケットＬ３の先端部３４が直線状の運動を行
うようになっている。

【００１８】図２は、図１に対応させて描いた作業機の
動作を解析するためのモデル図である。この図において
は、横軸にｘ軸を設定して水平方向の距離を表し、縦軸
にｙ軸を設定して垂直方向の距離を表すようにしてい
る。両軸の交点が上記ｘ−ｙ座標の原点であり、図１の
ブームＬ１が軸支されている運転操作室側の第一水平軸
３０１に相当する。このブームＬ１の有効長さ、すなわ
ち第一水平軸３０１と第二水平軸３０２との間の距離は
ｌ₁、アームＬ２の有効長さ、すなわち第二水平軸３０
２と第三水平軸３０３との間の距離はｌ₂、およびバケ
ットＬ３の有効長さ、すなわち第三水平軸３０３とバケ
ットＬ３の先端部３４との間の距離はｌ₃にそれぞれ設
定されている。

【００１９】そしてある時点において、図２に示すよう
に、ブームＬ１とｘ軸との間に形成される内角がθ₁、
ブームＬ１の延長線とアームＬ２との間に形成される外
角がθ₂、アームＬ２の延長線とバケットＬ３との間に
形成される外角がθであったとする。このような状態
で、非常に短い範囲でのバケットＬ３の先端部３４の作
動方向がｘ軸に対して角度β（以下操作角度という）だ
け傾いており、その動作速度がＶであったとする。

【００２０】そこでまず、このような状態のときの各関
節の角速度（第一水平軸３０１、第二水平軸３０２およ
び第三水平軸３０３における第一関節角θ₁、第二関節
角θ₂、第三関節角θ₃の角速度）と上記先端部３４の動
作速度との関係を求めることにする。

【００２１】図２におけるバケットＬ３の先端部３４の
座標を（ｘ、ｙ）とすると、

【００２２】

【数１】ｘ＝ｌ₃・ｃｏｓ（θ₁＋θ₂＋θ₃）＋ｌ₂・ｃ
ｏｓ（θ₁＋θ₂）＋Ｌ₁・ｃｏｓθ₁

【００２３】

【数２】ｙ＝ｌ₃・ｓｉｎ（θ₁＋θ₂＋θ₃）＋ｌ₂・ｓ
ｉｎ（θ₁＋θ₂）＋Ｌ₁・ｓｉｎθ_１ と表現することができる。

【００２４】そして、θ_１、θ₂、θ₃がどのような角度
になろうともある微小な範囲では先端部３４は直線状掘
削を行うように制御され、上記角度βは一定という前提
になっているため、

【００２５】

【数３】α＝θ₁＋θ₂＋θ₃＝一定 という関係が成立している。

【００２６】上記「数１」、「数２」および「数３」の
各式を時間ｔで微分することにより、

【００２７】

【数４】 (dx/dt)＝（−ｌ₂・ｓｉｎ（θ₁＋θ₂）−ｓｉｎθ₁）
・(dθ₁/dt)−ｌ₂・ｓｉｎ（θ₁＋θ₂）(dθ₂/dt)

【００２８】

【数５】 (dy/dt)＝（−ｌ₂・ｃｏｓ（θ₁＋θ₂）−ｃｏｓθ₁）
・(dθ₁/dt)−ｌ₂・ｃｏｓ（θ₁＋θ₂）(dθ₂/dt)

【００２９】

【数６】 (dα/dt)＝(dθ₁/dt)＋(dθ₂/dt)＋(dθ₃/dt)＝０ となる一方、(dx/dt)と(dy/dt)とは、バケット先端部３
４のｘ軸に対する作動方向の角度βとその動作速度Ｖと
の間の関係として、

【００３０】

【数７】(dx/dt)＝Ｖｃｏｓβ

【００３１】

【数８】(dy/dt)＝Ｖｓｉｎβ とおくことができる。そこで「数７」式および「数８」式の
右辺を「数３」式および「数４」式の左辺に代入するこ
とによって(dθ₁/dt)と(dθ₂/dt)とを未知数とした連立
方程式が得られるから、それを解くことによって(dθ₁/
dt)と(dθ₂/dt)とを、以下のようにＶ、θ₁、θ₂および
βをもって表現することができる。

【００３２】

【数９】(dθ₁/dt)＝（（ｃｏｓ（θ₁＋θ₂−β）／
（ｌ₁・ｓｉｎθ₁））・Ｖ

【００３３】

【数１０】(dθ₂/dt)＝｛−（ｌ₂・ｃｏｓ（θ₁＋θ₂−
β）＋ｌ₁・ｃｏｓ（θ₁−β））／（ｌ₁・ｌ₂・ｓｉｎ
θ₁）｝・Ｖ また、上記「数９」および「数１０」式の右辺を「数
６」式に代入して解くことにより、(dθ₃/dt)をＶ、θ₁
およびβによって表現することができる。

【００３４】

【数１１】(dθ₃/dt)＝（ｃｏｓ（θ₁−β）／（ｌ₂・
ｓｉｎθ₁））・Ｖ 以上の「数９」、「数１０」および「数１１」式に示す
通り、各関節角の角速度(dθ₁/dt)、(dθ₂/dt)および(d
θ₃/dt)は、いずれもバケット先端部３４の動作速度Ｖ
の関数とみなすことができる。

【００３５】ところで、上記の各関節角の角速度につい
ては、それぞれ構造上の制約によって正負の最大速度が
決まっており、それ以上速く動かすことはできない。そ
こでこのような制約条件を数式化すると、

【００３６】

【数１２】(dθ_i/dt)_min≦(dθ_i/dt)≦(dθ_i/dt)_max
（ｉ＝１，２，３） 但し上式において、(dθ_i/dt)_minは各関節角の角速度の
最小値（すなわち負の方向の最大値）であり、(dθ_i/d
t)_maxは最大値であり、具体的に特定の数値として表す
ことができる。

【００３７】この「数１２」式の(dθ_i/dt)に「数９」
〜「数１１」式の右辺をそれぞれ代入することによっ
て、各関節角の(dθ_i/dt)_minおよび(dθ_i/dt)_maxに対応
した、以下に示す六つのバケット先端部３４の動作速度
を算出することができる。

【００３８】 Ｖ_1min、Ｖ_2min、Ｖ_3min、Ｖ_1max、Ｖ_2max、Ｖ_3max そして以下の式で示すように、これらの六つの動作速度
の絶対値の最小値を選びだすことによって上記「数１
２」の制約条件からバケット先端部３４の目標動作速度
Ｖ_mを決めることができる。

【００３９】

【数１３】Ｖ_m＝ｍｉｎ（|Ｖ_1min|,|Ｖ_2min|,|Ｖ_3min|,
|Ｖ_1max|,|Ｖ_2max|,|Ｖ_3max|） このようにして得られたＶ_mの値を先の「数９」〜「数
１１」式に代入することによって各関節角の角速度(dθ
₁/dt)、(dθ₂/dt)および(dθ₃/dt)が得られる。従っ
て、これらの角速度が実現するように公知の制御手法で
各関節角の角速度制御を行えば、バケットＬ３の先端部
３４は、作動方向がｘ軸に対して角度βに傾いた状態を
保持して速度Ｖ_mで動作することになる。

【００４０】因に上記(dθ_i/dt)_min、および(dθ_i/dt)
_maxの値については、各アクチュエータ３１、３２、３
３の速度範囲が予め判明しておれば算出可能である。こ
の算出方法について、関節駆動機構を一般的に示した図
３を基に説明する。この図においてθは関節角、θ_L1は
ベクトルａ_L1とｘ軸との間に形成される角度、θ_L2はベ
クトルａ_L2とｙ軸との間に形成される角度である。これ
らの間には以下の式で示す関係が成立している。

【００４１】

【数１４】θ_L2＝θ＋θ_off2

【００４２】

【数１５】(dθ_L1/dt)＝０

【００４３】

【数１６】 θ_c＝arcｔａｎ｛（ａ_L1・ｓｉｎθ_L1＋ａ_L2・ｓｉｎθ
_L2）／（ａ_L1・ｃｏｓθ_L1＋ａ_L2・ｃｏｓ_L2）｝

【００４４】

【数１７】 (da_c/dt)＝ａ_L2・ｓｉｎ（θ_c＋θ_L2）・(dθ/dt) そして、アクチュエータを構成しているシリンダＣの最
小動作速度（すなわち負の方向の最大速度）Ｖ_cminと、
最大動作速度Ｖ_cmaxとの間にベクトルa_cの速度が入って
いなければならないため、「数１７」の条件式が成立す
る。

【００４５】

【数１８】Ｖ_cmin≦(da_c/dt)≦Ｖ_cmax 従って、「数１７」を「数１８」に代入して解くことに
より上記(dθ/dt)_min、および(dθ/dt)_maxの値を以下の
ように具体的に求めることができる。

【００４６】

【数１９】 (dθ/dt)_min＝Ｖ_cmin／｛ａ_L2・ｓｉｎ（θ_c＋θ_L2）｝

【００４７】

【数２０】 (dθ/dt)_max＝Ｖ_cmax／｛ａ_L2・ｓｉｎ（θ_c＋θ_L2）｝ となるのである。すなわち、各アクチュエータ３１、３
２、３３についてこのような計算を実行することによっ
て、各(dθ_i/dt)_min、および(dθ_i/dt)_maxの値を具体的
に求めることができるのである。

【００４８】つぎに、図２に示すバケットＬ３の先端部
３４が座標（ｘ，ｙ）の位置であったときから制御終期
である時間ｔが経過したときの座標（ｘ_r，ｙ_r）（目標
位置）については、以下の各式によって導出され、計算
される。まず座標（ｘ_r，ｙ_r）に移行してもバケットＬ
３の角度βは変わらない。この時のｘ成分およびｙ成分
の上記先端部３４の動作速度、Ｖ_xおよびＶ_yは、「数１
３」を用いて以下のように表現できる。

【００４９】

【数２１】Ｖ_x＝Ｖ_mｃｏｓβ、Ｖ_y＝Ｖ_mｓｉｎθ 従って、目標位置は以下の式で表わすことができる。

【００５０】

【数２２】ｘ_r＝ｘ＋Ｖ_x・ｔ、ｙ_r＝ｙ＋Ｖ_y・ｔ そして、目標位置に到達したときの各関節角の角度を、
ブームＬ１についてはθ_r1、アームＬ２については
θ_r2、バケットＬ３についてはθ_r3とおくと、

【００５１】

【数２３】ｘ_r＝ｌ₃・ｃｏｓ（θ_r1＋θ_r2＋θ_r3）＋ｌ
₂・ｃｏｓ（θ_r1＋θ_r2）＋ｌ₁・ｃｏｓθ_r1

【００５２】

【数２４】ｙ_r＝ｌ₃・ｓｉｎ（θ_r1＋θ_r2＋θ_r3）＋ｌ
₂・ｓｉｎ（θ_r1＋θ_r2）＋ｌ₁・ｓｉｎθ_r1 そして、ｘ’、ｙ’を「数２４」、「数２６」式のよう
に表し、

【００５３】

【数２５】 ｌ₂・ｃｏｓ（θ_r1＋θ_r2）＋ｌ₁・ｃｏｓθ_r1＝ｘ’

【００５４】

【数２６】 ｌ₂・ｓｉｎ（θ_r1＋θ_r2）＋ｌ₁・ｓｉｎθ_r1＝ｙ’ これら「数２５」、「数２６」式を用いて「数２３」、
「数２４」式を各種変形することにより、以下の各式が
得られる。

【００５５】

【数２７】ｃｏｓ（θ_r2）＝（ｘ’²＋ｙ’²−ｌ₁ ²−ｌ
₂ ²）／（２・ｌ₁・ｌ₂）

【００５６】

【数２８】 ｓｉｎ（θ_r2）＝±（１−ｃｏｓ²（θ_r2））

【００５７】

【数２９】 ｔａｎ（θ_r2）＝ｓｉｎ（θ_r2）／ｃｏｓ（θ_r2）

【００５８】

【数３０】ｃｏｓ（θ_r1）＝｛（ｌ₁＋ｌ₂・cos
（θ_r2））＋ｌ₂・sin（θ_r2）・ｙ’｝／｛（ｌ₁＋ｌ₂
・cos（θ_r2）²＋ｌ₂ ²・sin²（θ_r2）｝

【００５９】

【数３１】ｓｉｎ（θ_r1）＝｛−l₂・sin（θ_r2）・ｘ’
＋（l₁＋l₂・cos（θ_r2））・ｙ’｝／｛（ｌ₁＋ｌ₂・co
s（θ_r2）²＋ｌ₂ ²・sin²（θ_r2）｝

【００６０】

【数３２】 ｔａｎ（θ_r1）＝ｓｉｎ（θ_r1）／ｃｏｓ（θ_r1）

【００６１】

【数３３】θ_r3＝β−θ_r1−θ_r2 以上の「数２７」〜「数３３」式を解くことにより（例
えば、ｔａｎ（θ_r1）、ｔａｎ（θ_r2）を未知数として
方程式を解き、ｔａｎ（θ_r1）、ｔａｎ（θ_r2）の値か
らθ_r1、θ_r2を求め、「数３３」式からθ_r3を求めるよ
うにすればよい）、θ_r1、θ_r2およびθ_r3の値が得られ
る。

【００６２】このようにして得られたθ_r1、θ_r2および
θ_r3の値は、図に示すグラフにおいて、バケットＬ３の
先端部３４の座標が（ｘ，ｙ）であったものが、一制御
周期が経過した状態で（ｘ_r，ｙ_r）に変化したときの各
関節角を示しているのである。逆にいえば、このような
関節角（すなわち目標関節角）になるように各アクチュ
エータ３０を、先に説明した各関節角の角速度(dθ₁/d
t)、(dθ₂/dt)および(dθ₃/dt)でもって操作することに
より、バケットＬ３の先端部３４は無理なく確実に操作
角度βを保持した状態で移動し、座標（ｘ_r，ｙ_r）に到
達するということができる。

【００６３】従って、上記先端部３４が座標（ｘ_r，
ｙ_r）に到達した時点で、新たに操作角度βを与えてや
るとともに、この時点における(dθ_r1/dt)、(dθ_r2/dt)
および(dθ_r3/dt)を算出し、これらの新たな関節角の角
速度でブームＬ１、アームＬ２およびバケットＬ３を動
作させることによって多関節腕体Ｌはつぎの制御周期の
動作を行うことになる。

【００６４】上記制御周期を具体的妥当性を有する程度
に短いものにし、かつ、順次制御周期ごとに動作角度を
入力してやるとともに、各関節角の角速度を算出するよ
うにし、この角速度で多関節腕体Ｌを動作させるように
すれば、バケットＬ３の先端部３４に所望の動作を確実
に行わせることができる。

【００６５】以下、基本的に上記「数１」〜「数３３」
式で示した方式における多関節作業機の運転制御につい
て、図４の本発明に係る制御ブロックの一例を示すブロ
ック図を基に説明する。この図に示すように、本発明に
係る動作速度制御装置Ｋは、関節角度検出部４と、アク
チュエータ最大動作速度設定部５と、関節角速度最大値
演算部６と、目標動作演算部７と、動作方向設定部８
と、目標関節角演算部９とから基本構成されている。

【００６６】上記関節角度検出部４は、多関節腕体Ｌの
関節角を検出しこの検出結果を関節角速度最大値演算部
６に入力するためのものである。そのためにこの関節角
度検出部４には、ブームＬ１の関節角（第一関節角
θ₁）、アームＬ２の関節角（第二関節角θ₂）およびバ
ケットＬ３の関節角（第三関節角θ₃）を検出するため
の第一関節角センサ４１、第二関節角センサ４２および
第三関節角センサ４３が備えられている。この関節角度
検出部４は、これら関節角センサ４１、４２、４３から
入力されたアナログ信号をディジタル信号に変換して関
節角速度最大値演算部６に向かって出力するようになっ
ている。

【００６７】上記アクチュエータの最大動作速度設定部
５は、動作速度制御装置Ｋに対するオペレータによる入
力装置の一つである。そして、この最大動作速度設定部
５においては、各アクチュエータ３０（ブーム用アクチ
ュエータ３１、アーム用アクチュエータ３２、バケット
用アクチュエータ３３）のシリンダＣの最小動作速度Ｖ
_cminと、最大動作速度Ｖ_cmaxとが関節角速度最大値演算
部６にインプットされるようになっている。

【００６８】上記関節角速度最大値演算部６は、上記動
作速度制御装置Ｋから入力された各関節角θ_i（θ₁、θ
₂、θ₃）の値と、上記アクチュエータ最大動作速度設定
部５から入力されたシリンダＣの最小動作速度Ｖ
_cminと、最大動作速度Ｖ_cmaxとから予めプログラミング
されている「数１８」〜「数２０」式を用いて各(dθ_i/
dt)_min、および(dθ_i/dt)_maxの値（但しｉ＝１，２，
３）を演算するように構成されている。

【００６９】上記目標動作演算部７は、図２に示す現状
のバケットＬ３の先端部３４の座標（ｘ，ｙ）から制御
周期ｔが経過した後の目標位置に到達するための目標動
作速度を演算する目標速度演算部７１と、目標座標（ｘ
_r，ｙ_r）を演算する目標位置演算部７２とから構成され
ている。

【００７０】そして、上記目標速度演算部７１において
は、関節角度検出部４から入力された各関節角θ
_i（θ₁、θ₂、θ₃）の値と、動作方向設定部８から入力
された操作角度βとをもとに、「数１２」〜「数１３」
式に基づいて目標動作速度Ｖ_mが演算されるようになっ
ている。上記動作方向設定部８は動作速度制御装置Ｋに
対するオペレータによる入力装置の他の一つである。

【００７１】また、目標位置演算部７２においては、関
節角度検出部４から入力された各関節角θ_i（θ₁、
θ₂、θ₃）の値と、動作方向設定部８から入力された操
作角度βの値と、さらに目標速度演算部７１から入力さ
れた目標動作速度Ｖ_mの値とをもとに、「数２１」およ
び「数２２」式に基づいて制御周期ｔが経過後の目標座
標（ｘ_r，ｙ_r）が演算されるようになっている。

【００７２】上記目標関節角演算部９は、上記目標座標
（ｘ_r，ｙ_r）に到達するために現状の各関節角θ
_i（θ₁、θ₂、θ₃）をどのような角度に変化させればよ
いかを演算する演算部であり、上記目標動作演算部７の
目標位置演算部７２から入力される目標座標（ｘ_r，
ｙ_r）の値から、「数２７」〜「数３３」式に基づいて
演算され、具体的に目標関節角θ_ri（θ_r1、θ_r2、
θ_r3）が得られるようになっている。このようにして得
られた目標関節角θ_riの値は、目標関節角演算部９から
各アクチュエータ３０（ブーム用アクチュエータ３１、
アーム用アクチュエータ３２、バケット用アクチュエー
タ３３）の図示していないコントローラに伝達され、こ
れを受けたコントローラは各アクチュエータ３０が所定
の動作を行うように制御するようになっている。

【００７３】図５は、本発明に係る動作速度制御の一例
を示すフローチャートである。以下この図を基に本発明
の動作速度制御装置の作用について説明する。まず、ス
テップＳ１においては、各アクチュエータ３０の動作速
度の最大値が設定される。この設定は、オペレータがア
クチュエータ最大動作速度設定部５に各シリンダＣの固
有の最小動作速度Ｖ_cminと、最大動作速度Ｖ_cmaxとを入
力することによって行われる。

【００７４】つぎのステップＳ２においては、バケット
Ｌ３の先端部３４の動作方向の設定が行われる。この動
作方向の設定は、オペレータが操作角度βを動作方向設
定部８に入力することにより行われる。この操作角度β
は、通常複数の制御周期ｔ_iごとの値（β_i）が入力さ
れ、ｉが１からｎまで実行されることによってバケット
Ｌ３に所望の動作を確実に行わせることができる。

【００７５】そして、ステップＳ３においては、各関節
角センサ４１、４２、４３からの検出値が動作速度制御
装置Ｋの関節角度検出部４に入力される。そしてステッ
プＳ４において現状のバケットＬ３の先端部３４の座標
（ｘ，ｙ）が求められ、その後ステップＳ５において各
関節部の最大角速度の値（各(dθ_i/dt)_min、および(dθ
_i/dt)_maxの値（但しｉ＝１，２，３））が演算される。
このようなステップＳ４およびステップＳ５の処理は関
節角速度最大値演算部６において行われる。演算された
各関節部の最大角速度の値は目標動作演算部７に入力さ
れる。

【００７６】つぎにステップＳ６においては、制御周期
ｔが経過後のバケットＬ３の先端部３４の目標速度Ｖ_m
が求められ、ステップＳ７においては制御周期ｔが経過
後のバケットＬ３の先端部３４の目標座標（ｘ_r，ｙ_r）
が演算される。これらは目標動作演算部７の目標速度演
算部７１および目標位置演算部７２において実行され
る。

【００７７】そして、ステップＳ８においては、上記バ
ケットＬ３の先端部３４の目標座標（ｘ_r，ｙ_r）より関
節角の目標値（θ_r1、θ_r2、θ_r3）が演算される。この
演算は目標関節角演算部９において行われる。この演算
結果は、各アクチュエータ３０の図外のコントローラに
伝達され、このコントローラに制御されつつ、多関節腕
体Ｌ（ブームＬ１、アームＬ２、バケットＬ３）は、バ
ケットＬ３の先端部３４が目標座標（ｘ_r，ｙ_r）を目指
すように所定の動作を行ってその第一水平軸３０１、第
二水平軸３０２および第三水平軸３０３回りに回動す
る。

【００７８】最後に、ステップＳ９において制御が終了
したか否かが判断され、終了していないときはステップ
Ｓ３にフィードバックされ上記の各ステップの処理が繰
り返し実行される。

【００７９】本発明の多関節作業機の動作速度制御装置
は以上のように各アクチュエータ３０の最大動作速度か
ら各関節部の動作を決定するように構成されているの
で、従来のようにバケットＬ３の先端部３４の動作にの
み注目して制御するものではなく、その結果各アクチュ
エータ３０に無理な動作を強要することはない。従っ
て、無理から生じる動作遅れは起こらないため、上記先
端部３４が予め設定された制御軌跡に確実に沿った状態
で動作し、多関節作業機の動作精度が向上する。

【００８０】なお、本実施例においては、バケットＬ３
の先端部３４の速度Ｖは最大速度Ｖ_mを採用している
が、特に最大速度Ｖ_mに限定されるものではなく、この
速度を超えない速度を任意に設定することもできる。

【００８１】また、本実施例においては、多関節作業機
はブームＬ１、アームＬ２およびバケットＬ３からなる
三本の関節が採用されたもので例示したが、関節は三本
に限定されるものではなく、二本でもよいし四本以上で
もよい。

【００８２】さらに、関節駆動機構についても、図３に
示すようなシリンダＣを使用したものに限定されるもの
ではなく、電動機等を使用したものでもよい。

【００８３】

【発明の効果】以上詳述したように本発明の多関節作業
機の動作速度制御装置は、各関節部の関節角に応じたア
クチュエータの限界作動速度を演算し、上記各アクチュ
エータがこの限界作動速度を超えない範囲で作動するよ
うに各アクチュエータに動作信号を発信するようになっ
ているため、この信号を受信した各アクチュエータは無
理のない状態で作動することができ、結果として最先端
腕体は予め設定された目標軌跡に確実に沿う動作を行う
ように制御することが可能になる。

【００８４】上記動作速度制御装置を、各関節部の関節
角を検知する関節角センサから入力された関節角と予め
入力されたアクチュエータの最大動作速度とからその関
節角速度の最大値を演算する角速度最大値演算部と、上
記関節角センサから入力された関節角と角速度最大値演
算部から入力された関節角速度の最大値と予め入力され
た最先端部腕体の動作方向とからこの最先端部腕体の目
標速度と目標位置とを演算する目標動作演算部と、この
目標動作演算部から入力される上記目標速度と目標位置
とから各関節部の関節角を演算する目標関節角演算部と
から構成し、この目標関節角演算部は各関節部が上記演
算された関節角になるように各アクチュエータに動作信
号を発信するようにすれば、まず角速度最大値演算部に
よって各関節部の関節角を検知する関節角センサから入
力された関節角と、予め入力されたアクチュエータの最
大動作速度とからその関節角速度の最大値が演算され、
その演算結果は目標動作演算部に伝達される。

【００８５】この関節角速度の最大値が入力された目標
動作演算部においては、上記関節角センサから入力され
た関節角と予め入力された最先端部腕体の動作方向とを
合わせて最先端部腕体の目標速度と目標位置とが演算さ
れ、この演算結果は目標関節角演算部に伝達される。

【００８６】そして、上記最先端部腕体の目標速度と目
標位置とが入力された目標関節角演算部によって各関節
部の関節角が演算され、この演算結果に基づいて各関節
部が上記演算された関節角になるように各アクチュエー
タに動作信号が発信される。

【００８７】従って、この動作信号を受信した各アクチ
ュエータは、上記目標関節角演算部が演算した関節角に
なるように作動し、この作動は予め角速度最大値演算部
によって演算された関節角速度最大値以下になるように
設定することが可能であるため、各アクチュエータの作
動はその最大動作速度を超えないものになり、無理のな
い状態で最先端部腕体は予め設定された目標軌跡上を移
動する。

【００８８】以上のように、本発明においては、アクチ
ュエータの最大動作速度から腕体の関節の角速度の最大
値が逐一演算され、この演算結果に基づいて上記最大値
を超えないように各腕体の関節角を変化させるように操
作されるものであるため、常にアクチュエータは無理な
動作を強いられることはなく、適正かつ良好に動作する
ことができる。

【図面の簡単な説明】

【図１】本発明に係る動作速度制御装置が適用された多
関節作業機の一例を示す側面図である。

【図２】作業機の動作を解析するためのモデル図であ
る。

【図３】関節部のシリンダの動作と関節角との関係を示
す説明図である。

【図４】本発明に係る制御ブロックの一例を示すブロッ
ク図である。

【図５】本発明に係る動作速度制御の一例を示すフロー
チャートである。

【符号の説明】 １ 油圧ショベル ２ キャタピラ装置 ３ 運転操作室 ３０ アクチュエータ ３１ ブーム用アクチュエータ ３２ アーム用アクチュエータ ３３ バケット用アクチュエータ ４ 関節角度検出部 ４１ 第一関節角センサ ４２ 第二関節角センサ ４３ 第三関節角センサ ５ アクチュエータ最大動作速度設定部 ６ 関節角速度最大値演算部 ７ 目標動作演算部 ７１ 目標速度演算部 ７２ 目標位置演算部 ８ 動作方向設定部 ９ 目標関節角演算部

Claims (2)

【特許請求の範囲】
1. 【請求項１】 複数の腕体が関節部を介して直列に連結
   され、かつ、各腕体を関節部の支持軸回りに回動させる
   複数のアクチュエータが設けられてなる多関節作業機械
   の最先端部腕体の動作を制御する多関節作業機の動作速
   度制御装置において、この動作速度制御装置は、各関節
   部の関節角に応じたアクチュエータの限界作動速度を演
   算し、上記各アクチュエータがこの限界作動速度を超え
   ない範囲で作動するように各アクチュエータに動作信号
   を発信するように構成されていることを特徴とする多関
   節作業機の動作速度制御装置。
2. 【請求項２】 上記動作速度制御装置は、各関節部の関
   節角を検知する関節角センサから入力された関節角と予
   め入力されたアクチュエータの最大動作速度とからその
   関節角速度の最大値を演算する角速度最大値演算部と、
   上記関節角センサから入力された関節角と角速度最大値
   演算部から入力された関節角速度の最大値と予め入力さ
   れた最先端部腕体の動作方向とからこの最先端部腕体の
   目標速度と目標位置とを演算する目標動作演算部と、こ
   の目標動作演算部から入力される上記目標速度と目標位
   置とから各関節部の関節角を演算する目標関節角演算部
   とからなり、この目標関節角演算部は各関節部が上記演
   算された関節角になるように各アクチュエータに動作信
   号を発信するように構成されていることを特徴とする請
   求項１記載の多関節作業機の動作速度制御装置。