JPH0731531B2 - 多関節作業機の軌跡制御装置 - Google Patents

Description

【発明の詳細な説明】 A.産業上の利用分野 本発明は、３本以上のアームを有する多関節作業機の軌
跡制御装置に関する。

B.従来の技術 ３本以上のアームを有する多関節作業機で軌跡制御を行
う場合、駆動アームを２本選択し、残りのアームは固定
する方式がとられる。駆動すべき２本のアームを選択す
る方式として次の（１）〜（３）が知られている。

（１）特願昭61−83357号公報には、一対で駆動される
２本のアーム組に対して予め優先順位を定めておき、入
力された角度信号に基づいて２本のアームの組を選択す
るという方式が開示されている。

（２）特願昭61−83358号公報には、各アームの回動角
速度と各アクチュエータの速度との速度比を演算し、精
度モードでは速度比の小さい値をもつ２本のアームを選
択し、速度モードでは速度比の大きい値をもつ２本のア
ームを選択するという方式が開示されている。

（３）特願昭61−210486号公報には、各アームのアクチ
ュエータの負荷Lnを検出し、上記特願昭61−83358号公
報と同様の速度比Dnを演算し、低燃費モードが選択され
たとき、ΣLn/Dnが最も小さくなる拘束条件のアームの
組を選択するという方式が開示されている。

C.発明が解決しようとする課題 しかしながら上記（１）〜（３）の方式には次のような
問題点がある。

（１）特願昭61−83357号公報の方式おいては、優先順
位が予め固定されているため、必ずしも軌跡制御に最適
なアームの組が選択されるわけではなく、また、選択す
る際に制御方向とアームのストロークエンドを判定しな
ければならず、このため予めすべての組のアームに対し
て角度速制御値を演算する必要があり、アーム数が増え
た場合の処理時間は膨大なものとなり、実現が難しい。

（２）特願昭61−83358号公報の方式おいては、各アー
ムのアクチュエータの速度に対するアームの回動速度に
着目しているのみであり、先端軌跡の制御に適したアー
ムが選択されているわけではない。すなわち選択された
駆動アームの姿勢によっては、先端軌跡を制御方向に制
御することができない場合がある。

（３）特願昭61−210486号公報の方式おいても、各アー
ムの負荷と速度比に着目しているのみなので、上記
（２）の方式と同様の欠点を有している。

本発明の技術的課題は、先端軌跡制御に最適な一対のア
ームを選択して軌跡制御することにある。

D.課題を解決するための手段 一実施例である第１図により本発明を説明すると、本発
明は、関節を介して回動可能に連結された３本以上のア
ームと、それらのアームを駆動するアクチュエータと、
入力信号に応じてそのアクチュエータによるアームの移
動量を制御する制御手段500とを有する多関節作業機の
軌跡制御装置に適用される。

上記技術的課題は、次の構成で解決される。

軌跡制御対象部位のX,Y方向における速度を指令し、速
度指令信号を出力する指令手段200と、各アームの相対
角度を検出して角度信号を出力する角度検出手段100
と、速度指令信号が指令する方向に各アームを回動した
場合に軌跡制御対象部位の移動量がＸ方向には最大とな
りＹ方向には最小となるアーム、およびＸ方向には最小
となりＹ方向には最大となるアームをそれぞれ選択し、
選択したアームに応じた拘束条件を出力する拘束条件選
択手段300と、速度指令信号と相対角度に基づいて、ア
ームの角速度制御値を拘束条件に応じて演算し、その角
速度制御値を制御部500へ供給する演算手段400とを具備
する。

また、先端アームの姿勢角が所定の条件になると自動的
に姿勢角一定制御を行うモードを選択するモード選択手
段301を設け、拘束条件選択手段300により、先端アーム
の姿勢角が上記所定角になると、姿勢角一定制御を含む
拘束条件を優先選択するようにしてもよい。

E.作用 拘束条件選択手段300は、軌跡制御に最適な一対のアー
ムを選択してその拘束条件を演算手段400に入力する。
演算手段400の演算結果が制御手段500に入力されて一対
のアームは演算された角速度で駆動される。

請求項２によれば、姿勢角一定制御のモードが選択され
ているときには、姿勢角が所定値になると自動的に姿勢
角一定制御を含む拘束条件を優先して選び一対のアーム
が駆動される。

F.実施例 −第１の実施例− 第１図〜第５図により、第４図（ａ）に示す４本アーム
式作業機に本発明を適用する場合について説明する。

第４図（ａ）において、下部走行体LT上に旋回可能に上
部旋回体USが設けられ、これらにより作業機本体CMが構
成される。上部旋回体USには第１アームが回動可能に設
けられ、その先端に第２アーム２が回動可能に設けら
れ、その先端に第３アーム３が回動可能に設けられ、そ
の先端に第４アーム４が回動可能に設けられており、各
アーム１〜４はそれぞれ油圧シリンダ５〜８により駆動
される。第４アーム４の先端には作業用アタッチメン
ト、例えばバイブロハンマユニット９がピン結合され
る。第４図（ｂ）に示すアースオーガ掘削ユニット10を
用いても良い。また、第５図に示すように、第４アーム
４に代えて例えば掘削バケット等の作業用アタッチメン
トを取付け、油圧シリンダ８により回動可能としても良
い。この場合は作業用アタッチメントの姿勢角一定制御
が行われる。

第２図（ａ）のようにこの作業機の座標を定義し、以下
の説明はこの座標に従う。第２図（ａ）に示すように、 点Ｏを第１アーム１の回動支点とし、第１〜第４のアー
ム１〜４の長さをL₁〜L₄、相対角度をT₁〜T₄、各アーム
先端の座標を（X₁,Y₁）、（X₂,Y₂）、（X₃,Y₃）、（X₄,
Y₄）とするとき、各座標はそれぞれ、 と表すことができる。したがって、第４のアーム４先端
の速度₄,_４は、_４ ＝−_１・L₁sinT₁ −（_１−_２）・L₂sin（T₁−T₂） −（_１−_２−_３）・L₃sin（T₁−T₂−T₃） −（_１−_２−_３−_４）・L₄sin（T₁−T₂−T₃−T
₄） …（３）_４ ＝_１・L₁cosT₁ ＋（_１・_２）・L₂cos（T₁−T₂） ＋（_１−_２−_３）・L₃cos（T₁−T₂−T₃） ＋（_１−_２−_３−_４）・L₄cos（T₁−T₂−T₃−T
₄） …（４） と表すことができる。

周知のとおり４本のアームを有する多関節作業機におい
ては、アーム先端を軌跡制御するには拘束条件を定めて
２本のアームを駆動するが、その組合せは次に示すよう
に６通りある。

（１）第3,第４のアーム3,4を固定（第1,第２のアーム
1,2で駆動） （２）第2,第４のアーム2,4を固定（第1,第３のアーム
1,3で駆動） （３）第2,第３のアーム3,3を固定（第1,第４のアーム
1,4で駆動） （４）第1,第４のアーム1,4を固定（第2,第３のアーム
2,3で駆動） （５）第1,第３のアーム1,3を固定（第2,第４のアーム
2,4で駆動） （６）第1,第２とアーム1,2を固定（第3,第４のアーム
3,4で駆動） さらに第４アーム４を姿勢角一定制御した場合、次の３
通りがある。

（７）第３のアーム３を固定（第1,第２のアーム1,2で
軌跡制御） （８）第２のアーム２を固定（第1,第３のアーム1,3で
軌跡制御） （９）第１のアーム１を固定（第2,第３のアーム2,3で
軌跡制御） 以上の９通りの拘束条件を式で表すと次のとおりであ
る。

イ］（１）の場合：_３＝０、_４＝０ ロ］（２）の場合：_２＝０、_４＝０ ハ］（３）の場合：_２＝０、_３＝０ ニ］（４）の場合：_１＝０、_４＝０ ホ］（５）の場合：_１＝０、_３＝０ ヘ］（６）の場合：_１＝０、_２＝０ ト］（７）の場合：_３＝０、_４＝_１−_２ チ］（８）の場合：_２＝０、_４＝_１−_３ リ］（９）の場合：_１＝０、_４＝−（_２＋_３） ここで、（３），（４）式の₄,_４を速度指令信号
，とし、各拘束条件毎に上記条件を式（３），
（４）に代入して軌跡制御のために駆動する２本のアー
ムの角速度ｎを求めると、 ｎ＝fn（，,T₁,T₂,T₃,T₄） …（５） となる。ただし、拘束条件（７）〜（９）では軌跡制御
対象点は第３のアームの先端であるので L₄＝０ …（６） として求める。その詳細な式を次表１に示す。

次に制御装置を第１図に基づいて説明する。

制御装置は，角度検出部100と、速度指令部200と、拘束
条件選択部300と、角速度制御値演算部400と、駆動制御
値演算部500とが図のように接続されて構成されてい
る。

角度検出部100を構成する角度検出器101〜104は第１〜
第４のアーム１〜４の回動支点に取付けられ、周知のレ
バー機構とポテンショメータによりそれぞれのアームの
相対角T₁〜T₄を検出し、各相対角T₁〜T₄を拘束条件選択
部300、角速度制御値演算部400、および駆動制御値演算
部500へ入力する。

速度指令部200は、水平（Ｘ）方向の操作レバー201と垂
直（Ｙ）方向の操作レバー202とで構成され、例えばレ
バー機構とポテンショメータによりレバー操作角度に相
応した信号を出力し、この信号を第４アーム４または第
３アーム３の先端の水平および垂直方向の速度指令値
，として角速度制御値演算部400へ入力するととも
に、符号器203,204へも入力する。

拘束条件選択部300は、角度T₁〜T₄,符号器203,204から
出力される水平，垂直方向の速度指令値の符号，および
モード設定器301の出力から、後述する処理手順によっ
て拘束条件（１）〜（９）を選択出力し、これを角速度
制御値演算部400へ入力する。ここで、モード設定器301
は、オペレータの操作により、第４アーム４の相対角度
が所定値になると自動的に姿勢角一定制御を行うモード
を選択するものである。その姿勢角を一定制御モードの
選択時、第４のアーム４の姿勢角が所定角、例えば−90
゜になると、上記９つの拘束条件の中から姿勢角一定制
御を含む拘束条件を優先して選択する。

角速度制御値演算部400は、拘束条件によって演算ブロ
ック401〜409の中から対応するいずれか１つの演算ブロ
ックを選択し、角度T₁〜T₄および速度指令値，か、
拘束条件によって選択されたアームの角速度制御値ｎ
を演算し、これらを駆動制御値演算部500へ入力する。
なお、各演算ブロック401〜409の出力は同一アームの制
御値毎に各アームの加算点410〜422で加算され出力され
る。

駆動制御値演算部500は、次の式に基づいて角速度制御
値ｎおよび角度T₁〜T₄から、シリンダ５〜８の流量制
御値Q₁〜Q₄を演算し、それを電気油圧変換弁11〜14に入
力する。

Qn＝ｎ・gn（Tn）・An …（14） ここで、gn（Tn）はリンク補正係数 Anはシリンダ受圧面積 これから電気油圧変換弁11〜14には油圧源から圧油が導
かれており、入力される流量制御値Q₁〜Q₄に応じた流量
および方向で圧油を第１〜第４アーム１〜４用のシリン
ダ５〜８に供給し、軌跡制御が行われる。

拘束条件選択部300において、制御すべきアームの選択
は第３図に示す処理手順により行われる。

ステップS20においてモード設定器301の出力を読み込
み、第４アーム４の姿勢角一定制御モードか否かを判定
する。姿勢角一定制御モードが選択されていなければス
テップS2を、選択されているとステップS1へ進む。ステ
ップS1では、第４アーム４の相対角度が予め定めた所定
角か否かを判定し、所定角に達していなければステップ
S2に進み、所定角に達していればステップS3へ進む。

ステップS2では、第２図（ａ）に示す各アームの回動支
点と第４アーム４先端を結ぶ線分がＸ軸となす角（制御
角と呼ぶ）A₁₄,A₂₄,A₃₄,A₄₄を計算する。前述の各アー
ム先端の座標を用いると、 と表すことができる。

同様にステップS3では第２図（ｂ）に示す各アームの回
動支点と第３アーム３先端を結ぶ線分がＸ軸となす各
（制御角）A₁₃,A₂₃,A₃₃を計算する。

次にステップS4では、各アームの制御角を比べ、最も０
゜に近い制御角のアームを選択する。こうして選択され
たアームは、その回動によって生じる軌跡制御対象部位
の移動量がＸ軸方向には最小となりＹ軸方向には最大と
なるアームであり、言い換えればＹ軸方向の制御に最も
適したアームである。

ステップS5では、ステップS4で選択したアームが制御方
向でストロークエンドとなっていないかを判定する。こ
の判定は表２に示すように、例えば第１アームの場合速
度指令値が負のとき は当該アームの相対角が予め定めた最小値（min）より
小さいか否か、あるいはが正のときは予め定めた最大
値（max）より大きいか否かを判定して行われる。スト
ロークエンドの場合はステップS6に進み、未選択のアー
ム数が１本か否かを判定する。１本の場合はアームを２
本選択することができないのでステップS13へ進み制御
不可とする。すなわち拘束条件を出力しない。未選択の
アーム数が２本以上の場合はステップS17へ進み、ステ
ップS5でストロークエンドとなったアームを除いた中か
ら制御角が最も０゜に近いアームを選択し、同様にスト
ロークエンドを判定するためステップS5へ戻る。

ステップS5でストロークエンドでない場合はステップS8
へ進み、各アームの制御角を比べ、最も90゜に近い制御
角のアームを選択する。ここで選択されたアームは、そ
の回動によって生じる軌跡制御対象部位の移動量がＸ軸
方向に最大となりＹ軸方向には最小となるアームであ
り、言い換えればＸ軸方向の制御に最も適したアームで
ある。

ステップS9では、ステップS8で選択したアームが制御方
向でストロークエンドとなっていないかを上述と同様に
判定する。ただし、この場合Ｘに対するアーム角度の正
負により、表２に示すように判定を行う。ストロークエ
ンドでない場合は、アームが２本選択されたことにな
り、ステップS12へ進んで選択したアームに対応する拘
束条件（１）〜（９）のいずれかを出力する。

ステップS9でストロークエンドと判定された場合はステ
ップS10で未選択のアーム数を調べ、残っていない場合
はアームを２本選択することができないのでステップS1
3へ進み制御不可とする。未選択のアームが残っている
場合はステップS11へ進み、ステップS9でストロークエ
ンドとなったアームを除いた中から制御角が最も90゜に
近いアームを選択し、同様にストロークエンドを判定す
るためステップS9へ戻る。

次に本装置の動作について説明する。

図示しない電源スイッチを投入するとこの装置が起動
し、角度検出器101〜104で検出された角度T₁〜T₄,速度
レバー201,202で指令された速度指令値，の符号お
よびモード設定器301で設定されたモードに基づいて、
拘束条件選択部300は、第３図の手順により軌跡制御に
最も適した２本のアームを選択し、選択されたアームに
対応する拘束条件を出力する。角速度制御値演算部400
では、選択された拘束条件に対応するいずれかの演算ブ
ロックが選択され、角度T₁〜T₄および速度指令値，
から、選択されたアームの角速度制御値ｎが演算され
る。選択されていない演算ブロックの角速度制御値ｎ
は零なので、各演算ブロック出力のうち同一アームごと
にそのアーム角速度制御値を加算点410〜420で加算すれ
ば、選択された一対のアームの角速度制御値の切換えが
行われる。

これらの角速度制御値_１〜_４は駆動制御値演算部50
0でリンク補正され、各アームの流量制御値Q₁〜Q₄に変
換される。これらの流量制御値Q₁〜Q₄は電気油圧変換弁
11〜14に供給され、油圧源からの圧油が所定方向、所定
流量にて第１〜第４シリンダ５〜８に供給される。これ
により選択されたアームが回動して第４アーム４先端の
軌跡が速度指令レバーの操作に応じて制御される。ある
いは、第３アーム３先端の軌跡が速度指令レバーの操作
に応じて制御され、第４アーム４の姿勢角が一定に制御
される。

なお、拘束条件選択部300をソフトウエアの形態で構成
したが、予め角度T₁〜T₄,速度指令値，の符号およ
びモード設定信号の種々の組合せに対する拘束条件を計
算しておき、それらの組合せに対する拘束条件をテーブ
ル化してもよい。この場合、入力データでテーブルを参
照すれば直ちに拘束条件が求まり第３図の処理時間の短
縮が可能である。また、拘束条件に対応する演算ブロッ
クを選択して角速度制御値を演算すれば良いので、すべ
てのアームの組合せに対して角速度制御値を演算する必
要がなく、処理時間はアーム数が増えても変わらない。

さらに、モード設定器301を設けることにより、容易に
姿勢角一定制御への切換えを行うことができる。

また、各演算ブロックの角速度制御値を同一アーム毎に
加算しているので、拘束条件の切換えが自動的に行われ
る。なお、この切換に際して、旧データを漸減し新デー
タを漸増して切換時のショックを緩和することもでき
る。

また、姿勢角一定制御モードを選択すれば、第４のアー
ム４が予め定めた所定値、例えば−90゜になると自動的
に姿勢角一定制御が行われるので、小径の垂直孔などへ
作業用アタッチメントを移動させる際などに孔と先端ア
ームとの接触を防止できる。

なお、本発明を適用するにあたっては以上の実施例の各
構成要素を次のようにしても良い。

アーム数は４本に限定されない。

各アームを油圧シリンダで駆動したが、油圧に限定さ
れず、また油圧モータ、油圧ロータリアクチなどその他
のアクチュエータを用いることができる。

バイブロハンマ，アースオーガや掘削バケット等に使
用できる旨述べたが、その他の各種作業用アタッチメン
トにも使用できる。

第１アーム１の角度を上部旋回体に対する相対角で検
出したが、第１アームの対地角を検出しても良く、ま
た、作業機本体の傾斜角を検出して相対角を補正しても
良い。

速度指令をX,Yの２方向としたが、どちらか一方向と
しても良く、その場合は、本出願人による特願昭63−10
8099号に開示したように他の方向を軌跡偏差の修正のた
めの速度指令とすることができる。

角速度制御値を積分するとともに、制御開始時のアー
ム角度初期値と現在の角度との偏差を求め、積分値が偏
差に等しくなるように目標角度を求めるようなサーボ制
御を行うフィードバックを追加してもよい。

角度検出器としては、磁気抵抗素子を用いたもの、差
動コイルを用いたもの、光学式、磁気式のロータリエン
コーダを用いたものなどポテンショメータに限定されな
い。

G.発明の効果 本発明によれば、作業機のアーム数や姿勢によらず軌跡
制御に最も適した２本のアームの組を自動的に選択する
ことができ、作業範囲を最大限に広げることが可能であ
る。また、先端アームの姿勢角一定制御を自動的に選択
することができ、狭い場所での作業を安全に能率よく行
うことが可能である。

【図面の簡単な説明】

第１図は本発明の一実施例の構成図、第２図は座標を定
義する図、第３図は拘束条件選択部の処理手順を示す
図、第４図および第５図は作業機の側面図である。 １〜4:アーム、５〜8:油圧シリンダ 11〜14:電気油圧変換弁、100:角度検出手段 101〜104:角度検出器、200:速度指令部 201,202:速度指令レバー 300:拘束条件選択部、301:モード設定器 400:角速度制御値演算部 401〜409:演算ブロック 500:駆動制御値演算部

Claims (2)

【特許請求の範囲】
1. 【請求項１】関節を介して回動可能に連結された３本以
   上のアームと、それらのアームを駆動するアクチュエー
   タと、入力信号に応じてそのアクチュエータによるアー
   ムの移動量を制御する制御手段とを有する多関節作業機
   の軌跡制御装置において、 軌跡制御対象部位のX,Y方向における速度を指令し、速
   度指令信号を出力する指令手段と、 各アームの相対角度を検出して角度信号を出力する角度
   検出手段と、 前記速度指令信号が指示する方向に各アームを回動する
   場合に生じる軌跡制御対象部位の移動量がＸ方向には最
   大となりＹ方向には最小となるアーム、およびＸ方向に
   は最小となりＹ方向には最大となるアームをそれぞれ選
   択し、選択したアームに応じた拘束条件を出力する拘束
   条件選択手段と、 前記速度指令信号と前記相対角度に基づいて、アームの
   角速度制御値を前記拘束条件に応じて演算し、その角速
   度制御値を前記制御手段へ供給する演算手段とを具備す
   ることを特徴とする多関節作業機の軌跡制御装置。
2. 【請求項２】前記先端アームの姿勢角が所定値になると
   自動的に姿勢角一定制御を行うモードを選択するモード
   選択手段を有し、このモードが選択されている場合、前
   記拘束条件選択手段は、先端アームの姿勢角が前記所定
   値になると姿勢角一定制御を含む拘束条件を優先選択す
   ることを特徴とする請求項１に記載の多関節作業機の軌
   跡制御装置。