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

Abstract

(57)【要約】本公報は電子出願前の出願データであるた
め要約のデータは記録されません。

Description

【発明の詳細な説明】 Ａ、産業上の利用分野 本発明は、３本以上のアームが各関節を介して互いに回
動可能とされた多関節作業機の軌跡制御装置に関する。

Ｂ、従来の技術 この種の多関節作業機をモデル化して示すと。

第４図に示すようになる。第１のアーム１は関節７を介
して本体に連結され、第２のアーム２は関節８を介して
第１のアーム１と連結され、第３のアーム３は関節９を
介して第２のアーム２と連結されている。そして、各ア
ーム１〜３は例えば図示の如く設けられた第１−第３の
油圧シリンダ４〜６によりそれぞれ回動される。

ところで、この種の作業機で軌跡制御を行う際、軌跡制
御対象部位である第３のアーム３の先端の水平方向（Ｘ
方向）の速度Ｖｘと垂直方向（ｙ方向）の速度ｖｙとを
指令値として入力するが、可動アームが３本以上あるの
で、全てのアームを駆動させて軌跡制御することができ
ない、そこで、従来は、例えば拘束条件として第３のア
ーム３を固定し、残余の第１および第２のアーム１．２
を駆動して軌跡制御が行われている。

Ｃ１発明が解決しようとする問題点 上述した従来の軌跡制御では次のような問題がある。

■第４図において、地面（本体）と第１のアームｌのな
す角度を０１．第１のアームｌの延長線と第２のアーム
２とがなす角度を０２．第２のアーム２の延長線と第３
のアーム３のなす角度を０３、第１−第３のアーム１〜
３の長さをそれぞれ見、〜Ｊ１３を第１〜第３の油圧シ
リンダ４〜６の全長を２１〜Ｚ３、第１の油圧シリンダ
４の前固定端と関ｆｌｉ７までの距離を文ＬＺＭＩ”、
同様に第２．ｉ３の油圧シリンダ５，６についても見、
°１文２″、ｉ３°＋１３”、関ｍ７と第１（７）油圧
シリンダ４の地面側固定端とを結ぶ線と地面との角度を
入とする。

第３のアーム３の先端のＸ座標、ｙ座標は、ｘ＝　ｆＬ
ｌｃＯ３０１４２ｃａｓ　（θｌｌ０２）　　ｊ１３ｃ
ｏｓ　（Ｏｔ＋０２”０３）・・・（１） Ｙ　＝　ｎ　ｌ５ｉｎθ１−１２ｓｉｎ　（θ１十〇ｚ
）−ｆＬ３ｇｉｎ　（０，＋０２÷０３）・・・（２） と表わされる。また、アーム角度Ｏｆ　（ｉ＝１〜３）
に対する、各シリンダ速度之１と各アーム角速度６ｔの
比６【／之１は、 ６□／之１　＝ｆＬ１”ｆＬｌ”５ｉｎ（０１＋入）Ｘ
−局 （見１　÷見、　−２１，−見１−刀ｓ（０＋入））・
・・（３） ６ｚ／之２　、＝　ｊｈ’　・！Ｌ２”　ｓｉｎθ２Ｘ
−局 （ｆＬ２÷１ｚ−２１２・４１２″ｃｏｓθ２）・・・
（４） ６３／之！　＝Ｊ１３°ｕ３ｓｉｎθ３×−埼 （ｆＬ３＋ｆＬ３−２見３・文３″ｃｏｓ　０３）・・
・（５） となり、各アーム１３についてθｉとａ＝／２ｉとの関
係を示せば第５図（ａ）〜（Ｃ）のようになる、９の図
から、油圧シリンダ４〜６への流量を一定（之１一定）
としても、各アームの角速度６１はアーム角度０１によ
って変化しアーム１〜３の移動角度が大きくなったり小
さくなったりしてしまうことがわかる。

ここで、多関節作業機を例えば塗装作業に用いる場合に
ついて考えると、下塗り時には、軌跡精度ではなく、よ
り速い速度が要求され、仕上げ塗装時には、速度ではな
くより高い軌跡精度が要求される。

しかしながら、上述した従来の軌跡制御では、３本アー
ムの場合いずれかひとつのアームが固定され、例えば第
３のアーム３が固定される場合には、第５図（ａ）、（
ｂ）に示す特性が重ね合わされて得られる軌跡精度と速
度とにより画一的に第１および第２のアーム１．２が駆
動制御されるので、所望の精度、速度が得られないこと
があった。

■また。上述の従来の軌跡制御では、駆動される一方の
油圧シリンダがストロークエンドに達すると、それ以上
アーム先端を移動させることができないので、制御範囲
が非常に狭かった。この欠点は、先端アーム３の対地角
φ（Ｆ４図）が一定となるような拘束条件を用いれば、
上述した３本のアームを有する多関節作業機の場合には
解決できる、しかし、それぞれ回動可能な４本以上のア
ームをもつ多関節作業機においては、特定の２本のアー
ムまたは先端のアームがストロークエンドに達するとア
ーム先端をそれ以上移動できず、同様に制御範囲が狭い
という問題があった。

本発明の目的は、同峙に駆動する２つのアクチュエータ
の組合せをアームの角度に応じて変えることにより上述
した問題点を解決した多関節作業機の軌跡制御装置を提
供することにある。

Ｄ０問題点を解決するための手段 本発明は、軌跡制御対象部位の所定方向における速度を
指令し、速度指令信号を出力する指令手段と、軌跡制御
対象部位の動き量に関する少なくとも２つのモードを選
択するモード選択手段と、各アームの相対角度を検出し
て角度信号を出力する角度検出手段と、検出された角度
から各アームの回動角速度とアームを駆動する各アクチ
ュエータの速度との速度比を演算し、前記選択されたモ
ードに基づいて前記速度比のうちのいずれか２つを選択
し駆動すべき２本のアームを決定し、前記速度指令信号
および角度信号に基づいて、決定された２本の駆動アー
ムの角速度指令信号を演算する角速度演算手段と、その
角速度指令信号によりアクチュエータの駆動を制御する
制御部への入力信号を演算する入力信号演算手段とを具
備する。

８０作用 モード選択手段からあるモードを選択するとそのモード
を示す信号が角速度演算手段に入力される。また、その
角速度演算手段には、軌跡制御対象部位の所定方向の速
度指令信号が指令手段から入力されるとともに、角度検
出手段から各アームの相対角度を示す角度信号も入力さ
れる。そして、角度信号から各アームの回動角速度とア
ーム駆動用アクチュエータの速度との速度比を演算し、
入力されたモード信号に基づいて演算された速度比のう
ちいずれか２つを選択して駆動すべき２本のアームが決
定される０例えば、軌跡の精度を重視するモードが選択
されると速度比が小さい値の２本のアームが選択される
。そして、２本の駆動アームが決定されると、速度指令
信号と角度信号とにより駆動アームの角速度指令信号が
演算される。この角速度指令信号は入力信号演算手段に
入力されて、アクチュエータの駆動方向や駆動速度等を
制御する制御部への入力信号が演算される。その入力信
号に基づいて制御部が駆動され、これによりアクチュエ
ータが駆動される。

Ｆ、実施例 第１図〜第３図はこの発明の詳細な説明する図であり、
第４図に示したモデル図の如く３本のアームを有する多
関節作業機に適用するように構成されたものである。

第１図において、第３のアーム３先端の水平方向（Ｘ方
向）および垂直方向（ｙ方向）の速度を指令し速度指令
信号Ｖｘ、Ｖｙを出力する速度指令装置ｔｌｏは制御装
置２０と接続されている。また、軌跡制御の際にアーム
３先端の軌跡精度を重視した精度モードまたは速度を重
視した速度モードを選択し精度モード信号Ｍａおよび速
度モード信号Ｍｖを出力するモード選択スイッチ３０も
制御装置２０と接続されている。更に、第１〜第３のア
ーム１〜３の角度（０１〜０３）をそれぞれ検出して角
度信号０１〜０３を出力するポテンショメータの如き角
度センサ４１−１〜４１−３も制御装置２０に接続され
ている。制御装置２０には各油圧シリンダ４〜６への流
量および吐出油の方向を制御する電磁比例弁（す〒ボ弁
）５１−１〜５１−３が後続し、制御装置２０に入力さ
れる各種信号θ１〜θ３　、Ｖｘ、Ｖｙ。

Ｍａ、Ｍｙに基づいて後述の如き演算が行われて電磁比
例弁５１−１〜５１−３に入力信号１１〜１３が出力さ
れるように構成されている。なお、電磁比例弁５１−１
〜５１−３によって制御される各アーム１〜３の角度θ
！〜θ３が制御装置２０にフィードバックされている。

制御装置２０は、角速度演算部２１と、サーボ制御部２
２と、リンク補正部２３と、電流演算部２４とが順次に
接続されて構成されている。

角速度演算部２１は例えばマイクロコンピュータで構成
され、選択された精度モードまたは速度モードに応じて
、入力される角度信号θｌ〜０３および速度指令信号Ｖ
ｘ　、Ｖｙから以下に示す演算を行い角速度演算信号６
ｔａ、６ｊａ（ｉ、　＝　１〜３．Ｊ＝１〜３）を演算
して出力するように構成されている６周知の通り３本の
アームにより軌跡制御する場合、ある拘束条件を定めて
２本のアームだけを駆動するが、本発明ではアーム回動
角速度６ｉとアクチュエータ速度、すなわち本実施例で
は油圧シリンダ速度之１との速度比δＩ／之Ｉのいずれ
か２つを選択して駆動アームを２本決定する０本実施例
の如き、精度モードの場合には、上記速度比ｂＸ／之ｉ
が小さい２つのアームを常に選択し、速度モードの場合
には速度比δ５／之ｔが大きいアームを常に２本選択す
る０例えば、第５図（ａ）〜（Ｃ）において、各アーム
角度が０．＝Ａ、０□＝Ｂ　、０３＝Ｃであれば各速度
比は、 ａＳ／之１＝Ｄ＜（＞３／之ｓ＝Ｆ＜１）２／之２＝Ｅ
でアリ、この場合、精度モードでは第１のアーム１と第
３のアーム３とが選択駆動され、速度モードでは第２の
アーム２と第３のアーム３とが選択駆動される。

なお、アーム角度Ｏｚに対する速度比 ｏｘ／之ｉは各アーム毎に：ＪＸＪ５図（ａ）　〜（Ｃ
）　（７）ようになるが、逐次計算して求めたり、ある
いは予めアーム角度θｉ−速度比６ｉ／之ｉを二次元マ
ツプとして記憶しておき、そのマツプを久方されたアー
ム角度ＯＬでルックアップして速度比６ｔ／之Ｉを求め
てもよい。

このように２本の駆動アームが決定されたら下記の（８
）弐〜（８）式に従って角速度演算信号Ｏｉ　ａｏ　Ｏ
Ｊ　＆が演算される。

■第１のアームｌと第２のアーム２が選択されると次式
（８）により４ｘａ、６ｊａとしてＯＬ　＋０２が演算
される。

６３＝Ｏ・・・（８） ■第２のアーム２と第３のアーム３が選択されると次式
（７）により、δｔａ、ｊＪａとしてＯｚ＋０３が演算
される。

ｔ−０ ・・・（７） ■第１のアームｌと第３のアーム３が選択されると次式
（８）によりＯｉ　ａｒ　ＯＪ　＆として／）１　、ｅ
３が演算される。

δ２−０ ・・・（８） サーボ制御部２２は、角速度演算信号０１ａを積分して
位置指令信号Ｏ１ｂを出力する積分器２２１−１〜２２
１−３と、積分器２２１−１〜２２１−３の出力θ、ｂ
と実際のアーム角度θｉとの偏差Δθｔｂをとる偏差発
生器２２２−１〜２２２−３と、偏差ΔＯｉｂに所定の
ゲインをかけるゲイン設定器２２３−１〜２２３−３と
、ゲイン設定器２２３−１〜２２３−３の出力ΔＯｉＣ
に角速度演算信号６ｔａを加え合わせて角速度指令信号
＋ｉｄを得る加算器２２４−１〜２２４−３とを有する
。

そして、加算器２２４−１〜２２４−３に各リンク補正
部２３−１〜２３−３が後続している。

このリンク補正部２３−１〜２３−３にはアームの位置
信号θ、ｂが入力され、入力される角速度指令信号６Ｘ
ｄにアーム角度に応じたゲインを与えてシリンダ速度信
号之１を出力する。このリンク補正部２３−１〜２３−
３には電流演算部２４−１〜２４−３が後続し、それら
には電磁比例弁５１−１〜５１〜３が後続している。電
磁比例弁５１−１〜５１−３はその入力信号蓋」により
図示していない油圧ポンプからの吐出油の方向および流
量を制御する。

本実施例の各構成要素のうち、油圧シリンダ４〜６がア
クチュエータ８０を、電磁比例弁５１−１〜５１−３が
制御部５０を、速度指令装置１０が指令手段を、モード
選択スイッチ３０が選択手段を、角度センサ４１−１〜
４１−３が角度検出手段４０を、角速度演算部２１．サ
ーボ制御部２２が角速度演算手段６０を、リンク補正部
２３と電流演算部２４とが入力信号演算手段７０をそれ
ぞれ構成している。そして、軌跡制御対象部位が第３の
アーム３の最先端である。

このように構成された本実施例の作用について説明する
。

モード選択スイッチ３０により精度モードまたは速度モ
ードのいずれかが選択されるとモード信号ＭａまたはＭ
ｖが角速度演算部２１に入力され、速度指令袋！ｔｌＯ
から、アーム３先端の水平方向（Ｘ方向）および垂直方
向（ｙ方向）の速度指令信号Ｖｘ　、Ｖｙが角速度演算
部２１に入力される。更に、角度検出センサ４１−１〜
４１−３から各アームの相対角度信号０１〜θ３が角速
度演算部２１に入力される。

このような各種信号が入力された角速度演算部２１は、
第２図に示す手順によって角速度演算信号６直ａ、δｊ
ａを出力する。

手順Ｐｉ−ｔ’は、各信号ｖｘ＊　ｖＹ　＊　０１＋’
２＋０３およびＭａまたはＭｙを読み込む０手順Ｐ２で
は角度信号θ！〜θ３により各アーム毎に二次元マツプ
から速度比６ｔ／之【をルックアップして演算する０手
順Ｐ３では選択されたモードをモード信号により判別す
る。精度モードが選択されていると、手順Ｐ４において
、演算した速度比δｉ／之りのうち小さい値を示してい
る２本のアームを選択し１手順Ｐ５において、（Ｉｌｔ
）弐〜（８）式に基づいて一対の角速度演算信号６．ａ
。

６ｊａが出力される。速度モードが選択されていると１
手順Ｐ６において速度比δｉ／之！のうち大きい値を示
している２本のアームを選択し、手順Ｐ５において同様
に一対の角速度演算信号（＞ｔａ、（＞ｊａが出力され
る。

一対の角速度演算信号ｂ　ｘ　ａ　＋　ＯＪ　ａは積分
器２２１−ｉ　、２２１−ｊで積分されて位置指令信号
０．ｂ、ＯＪｂとなる。その位置指令信号θ、ｂ、Ｏｊ
ｂと実際の角度θ１．θｊとの偏差が偏差器２２２−ｉ
、２２２−ｊでとられてΔθｉｂ、Δθｊｂが得られる
。その偏差Δθｌｂ、ΔＯｊｂに対してゲイン設定器２
２３−ｉ　、２２３−ｊで所定のゲインが与えられてΔ
６【ｃ、Δ６ｊｃとされ、加算器２２４−ｉ　、２２４
−ｊで角速度演算信号Ｏｉ　ａ　＊　Ｏｊ　ｔＬとΔＯ
ｉＣ，Δ４ｊｃとが加算補正され、これにより角速度指
令信号ｏ　ｔ　ｄ　。

δｊｄが得られる。この角速度指令信号６ｚｄ。

θｊｄはリンク補正部２３　　ｉ、２３　　Ｊに入力さ
れて積分器２２１−、．２２１−Ｊの位置指令信号ｏ、
ｂ、θｊｂ、すなわちアーム角度に応じた補正演算が実
行されてシリンダ速度信号２友。

之ｊが得られる。この補正演算は、角速度指令信号６ｉ
ｄ、δｊｄに所定のゲインを与えることにより行われる
。シリンダ速度信号之り、之ｊは電流演算部２４−、．
２４−ｊに入力されて電磁比例弁５１−　＝　、　５１
−　Ｊの入力信号Ｉ直＋　１．　Ｊに変換される。そし
て、電磁比例弁５１−、。

５１−ｊがその入力信号ｔ　Ｈ、ｚ　３によって制御さ
れアームｉ＋Ｊが駆動される。駆動される２本のアーム
ｆ＋Ｊは、選択されたモードおよび逐次入力される角度
０１〜０３により決定されるが、その拠所となる速度比
ａｔ／之」は各アーム毎に第５図（ａ）〜（Ｃ）のよう
になっているので、同一モードにおいてもアーム角度に
より速度比δＸ／之２が変わると駆動アームが適宜変わ
ることになる。

第３図（ａ）、（ｂ）に、垂直方向をｙ〇一定とし、水
平方向でアーム３先端をｘｓ−＋ｘｔ＋ｘｓと移動させ
るように指令したときのアーム３先端の軌跡を示し、第
３図（ａ）が速度モード、第３図（ｂ）が精度モードの
場合を示す、この図かられかるように、精度モードで軌
跡制御すると略ｙ＝ｙｏでアーム３先端を移動できる。

また速度モードの場合には、垂直方向の位置精度は悪く
なるが、指令する速度Ｖｘ　、Ｖｙに対して精度モード
に比べて水平方向のｘｓ＋ｘｔ−＋ｘｓの移動時間が早
くなる。

従って、例えば多関節作業機を用いて下塗り、仕上げ塗
りを行うような場合、下塗り時に速度モードを選択し仕
上げ塗り時に精度モードを選択すれば効率よく作業がで
きる。また、精度だけが必要な作業に精度モードで軌跡
制御すれば、従来のように特定の２本のアームだけを駆
動する場合に比べて位置精度が向上する。

以上では３本アームの作業機にって説明したが４本以」
；のアームを有する作業機にも同様に適用でき、この場
合、軌跡モードとして精度と速度との中間的なモードを
設定し、速度比０１／之ｉの２つの中間値を有する２本
のアームを駆動するようにしてもよい、更に以上ではア
クチュエータを油圧シリンダとしたが空圧シリンダでも
よく、油圧、空圧モータまたは電動機の回転力を直線運
動に変換してアームを駆動するようなものでもよい。

Ｇ０発明の効果 本発明によれば、軌跡制御対象部位の軌跡の精度や速度
等を選択できるようにしたので、作業内容に応じた所望
の精度や速度で作業機を運転できる。また駆動される２
本のアームが固定されずアーム角度に応じて駆動アーム
の組合わせが変わり作業範囲が従来のものに比べて広く
なる。更にまた、実施例のように精度を重視したモード
で軌跡制御すれば、従来のものに比べてより高い精度で
軌跡が制御できる。

【図面の簡単な説明】

第１図は本発明の一実施例を示すブロック図。 第２図はその処理手順の一例を示すフローチャート、第
３図（ａ）、（ｂ）は実際の軌跡の２例を示す図、第４
図は３本アームのモデル図、第５図（ａ）〜（ｃ）はア
ーム角度に対する速度比６＝／之ｉを示すグラフである
。 １〜３：アーム　　　　４〜６：油圧シリンダｌＯ：指
令手段　　　　　３０：選択手段４０：角度検出手段　
　　５０：制御部６０：角速度演算手段 ７０：入力信号演算手段 ８０：アクチュエータ 出　願　人　　日立建機株式会社 代理人弁理士　　永　井　冬　紀 第２図 第３図 ａ４ Ｘ麿康 第４図 第５図 アー４４ｊ

Claims (1)

1. 【特許請求の範囲】 １）関節を介して回動可能に連結された３本以上のアー
   ムと、それらのアームを駆動するアクチュエータと、そ
   のアクチュエータによるアームの移動量を制御する制御
   部とを有する多関節作業機の軌跡制御装置において、 軌跡制御対象部位の所定方向における速度を指令し、速
   度指令信号を出力する指令手段と、前記軌跡制御対象部
   位の動き量に関する少なくとも２つのモードを選択する
   モード選択手段と、 各アームの相対角度を検出して角度信号を出力する角度
   検出手段と、 検出された角度から各アームの回動角速度と各アクチュ
   エータの速度との速度比を演算し、前記選択されたモー
   ドに基づいて前記速度比のうちのいずれか２つを選択し
   駆動すべき２本のアームを決定し、前記速度指令信号お
   よび角度信号に基づいて、決定された２本の駆動アーム
   の角速度指令信号を演算する角速度演算手段と、 その角速度指令信号により前記制御部への入力信号を演
   算する入力信号演算手段と、を具備したことを特徴とす
   る多関節作業機の軌跡制御装置。 ２）特許請求の範囲第１項に記載の装置において、前記
   アクチュエータを油圧シリンダとし、前記制御部を電磁
   比例弁としたことを特徴とする多関節作業機の軌跡制御
   装置。 ３）特許請求の範囲第１項または第２項に記載の装置に
   おいて、前記モードは精度モードと速度モードとし、精
   度モードが選択されたときは前記速度比のうち小さい２
   つの値をもつ２本のアームを常時駆動し、速度モードが
   選択されたときは前記速度比のうち大きい２つの値をも
   つ２本のアームを常時駆動することを特徴とする多関節
   作業機の軌跡制御装置。