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

Abstract

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

Description

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

Ｂ、従来の技術 この種の多関節作業機をモデル化して示すと、第６図に
示すようになる。第１の７−ム１は関節５を介して本体
に連結され、第２のアーム２は関節６を介して第１のア
ームｌと連結され、第３のアーム３は関ｆＭ７を介して
第２のアーム２と連結されている、そして、各アームは
図示していない３本の油圧シリンダによりそれぞれ駆動
される。

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

Ｃ０発明が解決しようとする問題点 しかしながら、このような従来装置では、第６図に示す
ように、駆動される２本のアーム１．２のうち一方がス
ロークエンドに達すると、それ以上アーム先端を移動さ
せることができないので制御範囲が非常に狭かった。４
本以上のアームを有する多関節作業機でも同様である。

本発明の目的は、ある駆動アームがストロークエンドに
達すると残余のアームの２本を駆動して軌跡制御するこ
とにより上述した問題点を解消した多関節作業機の軌跡
制御装置を提供することにある。

Ｄ０問題点を解決するための手段 本発明は、軌跡制御対象部位の所定方向における速度を
指令し、速度指令信号を出力する指令手段と、各アーム
の相対角度を検出して角度信号を出力する角度検出手段
と、軌跡制御のために一対で駆動される２本のアームの
組の中から、予め定めた優先順位に従って、少なくとも
人力された角度信号に基づき２本のアームとも駆動可能
な組を選択し、前記速度指令信号および角度信号に基づ
いて、選択された２本のアームの角速度指令信号を演算
する角速度演算手段と、その角速度指令信号により各ア
ーム駆動用アクチュエータの駆動を制御する制御部への
入力信号を演算する入力信号演算手段とを具備する。

８０作用 角速度演算手段には、軌跡制御対象部位の所定方向の速
度指令信号が指令手段から入力されるとともに、角度検
出手段から各アームの相対角度を示す角度信号が入力さ
れる。そして、角速度演算手段は、一対で駆動される２
本のアームの組の中から、予め定めた優先順位に従って
、少なくとも入力された角度信号に基づき２本のアーム
とも駆動可能な組を選択し、速度指令信号および角度信
号に基づいて、選択された２本のアームの角速度指令信
号を演算して出力する。その角速度指令信号は入力信号
演算手段で入力信号に変換されて制御部に出力される。

制御部はその入力信号に相応してアクチュエータを駆動
し、これにより２本のアームが駆動されて軌跡制御が行
なわれる。

この際、まず第１優先順位の組の２本のアームが駆動さ
れ、その一方が例えばストロークエンドに達して駆動で
きなくなると、第２優先順位の組の２本のアームが駆動
され、以下、同様にして順次の組の２本のアームにより
軌跡制御が実行される。

Ｆ、実施例 第１図〜第６図は本発明を３本のアームを有する多関節
作業機に適用した場合の一実施例を示す。

第１図において、本体に固定された架台４にはピン５を
介して第１のアームｌが回動可能に連結されている。第
１のアームｌの他端にはピン６を介して第２のアーム２
が回動可能に連結され、第２のアーム２の他端にはピン
７を介して第３のアーム３が回動可イ艶に連結されてい
る。第１のアームｌと本体との間には油圧シリンダ８が
介装されている。また第１のアームｌには油圧シリンダ
９の基端が連結され、そのピストンロッド９１が、第１
のアームｌと第２のアーム２との間に取付けられたリン
ク機構１１に連結されている。また、油圧シリンダｌＯ
の基端が第２のアーム２に連結され、そのピストンロッ
ド１０１が、第２のアーム２と第３のアーム３との間に
取付けられたリンク機構１２に連結されている。そして
、各アーム１〜３の相対角度を検出する角度センサ４１
−１〜４１−３がアーム１〜３の関節となるビン５〜７
に関連して設けられている。

ここで第２図に基づいて各アーム１〜３の回動範囲につ
いて説明する。なお、以下では反時計方向回りを正の角
度としている。

第１のアームｌは、基線ｆＬｔを基準として時計方向に
一〇１１、反時計方向に０１０回動でき、第２のアーム
２は第１のアームｌの軸線ｆＬ２を基準として時計方向
に−０２１、反時計方向に０２０２０回動、第３のアー
ム３は第２のアーム２の軸線ｆＬｓを基準として時計方
向に−θ３１１反時計方向に０３θ回動できる。すなわ
ち、第１のアームｌが基線１１となす角度を０１、第２
のアーム２が軸線１２となす角度を０２）第３のアーム
３が軸線見３となす角度を０３とすれば、第１のアーム
１はθ１＝θ１ｏおよび−０１１でストロークエンドと
なり、第２のアーム２はθ２＝０２０および０２＝−０
２１でストロークエンドとなり、第３のアーム３は０３
＝θ３ｏおよび０３＝−０３１でストロークエンドとな
る。

再び第１図において、各油圧シリンダ８〜１０はそれぞ
れ電磁比例弁５１−１〜５１〜３によりその速度および
駆動方向が制御され、電磁比例弁５１−１〜５１−３に
は油圧ポンプ１３から吐出油が供給されている。そして
、電磁比例弁５１−１〜５１−３は制御部ｇ１２０から
の入力信号ｉ　ｌ　”　ｆ　３により制御される。制御
装置２０には速度指令装置１３０が接続され、その速度
指令装置３０から、軌跡制御対象部位である第３のアー
ム３先端の水平方向（Ｘ方向）および垂直方向（ｙ方向
）の速度を指令する速度指令信号Ｖｘ。

ｖｙが供給され、また、角度センサ４１−１〜４１−３
も接続されアーム角度信号０１〜θ３が入力されるよう
になっている。

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

角速度演算部２１は例えばマイクロコンピュータで構成
され、入力される角度信号０１〜θ３および速度指令信
号Ｖｘ　、Ｖｙから後述する角速度演算信号ｊ１４．ｊ
２ａ、（＞３ａを演算して出力するように構成されてい
る０周知の通り３本のアームにより軌跡制御する場合、
ある拘束条件を定めて２本のアームだけを駆動するが、
本発明では。

２木−組の駆動アームｔ、ｊ（ｉ＝１〜３゜ｊ＝１〜３
）の各組に優先順位を予め定め、その優先順位に従って
逐次２本の駆動アームｉｓＪが決定され、それに基づい
て上述の角速度演算信号／）　ｌａ”’　６３ａが演算
されて出力される。

すなわち、３本アームの場合、第１のアームｌと第２の
アーム２の第１組、第１のアーム１と第３のアーム３の
第２組、第２のアーム２と第３のアーム３の第３組の３
つの組があるが、この実施例では、第２のアーム２と第
３のアーム３の第３組をまず優先させて軌跡制御し、そ
のうちのいずれかのアームがストロークエンドに達した
ら、第１のアームｌと第２のアーム２の第１組により軌
跡制御し、そのいずれかがストロークエンドに達したら
第１のアームｌと第３のアーム３の第２組により軌跡制
御するようにしている。

次に、アーム１〜３の任意の姿勢においてどの組を用い
て軌跡制御するかについて第４図のフローチャートに従
って以下に説明する。

まず、手順Ｐ１において、角度信号０１〜θ３および速
度指令信号Ｖｘ、Ｖｙを読込み１手順Ｐ２において、各
組における角速度演算信号（６２１，６３１）、　（０
１２，１＞２２）−（１１１１３，６３３）を以下の（
１）弐〜（３）式により求める。但し１次式では各アー
ムの長さをそれぞれｆＬｓ　　、　１２．　ｕ３とし、
６ｔ２．４ｔａは第１のアームｌ、６２１．６２２は第
２のアーム２．４３ｔ、６ａ３は第３のアーム３の各組
における角速度演算信号を示す。

■第３組（（＞２１　、６ａｔ） ・・・（１） ■第１組（６１２，δ２２） ・・・（２） ■第２組（４１３、δ３３） ・・・（３） そして、次の条件イル二においては、■手順Ｐ３〜Ｐ６
．◎手順Ｐ３〜Ｐ５　、Ｐａ、Ｏ手順Ｐ３　、Ｐ７　、
Ｐ５　、Ｐａ、■手順Ｐ３　、　Ｐ７　。

Ｐ５　、Ｐａをそれぞれ経て手順Ｐ２４に進み、角速度
演算信号６１ａ”’　ｆ）　３ａとしてｊ）　ｌａ＝　
Ｏｒ０２ａ＝Ｇ２１，４３ａ＝４３１が出力される。

■　６３１＞Ｏ２Ｏ３＜Ｏ，ｊ２１＞０．δ２く０：第
３のアーム３および第２のアーム２が反時計方向に動け
るとき。

◎　（＞３１＞Ｏ，δ３　＜０．４２１＜Ｏ，ｆｅｚ　
＃−’２１”第３のアーム３が反時計方向に動ける場合
で、かつ、第２のアーム２が時計方向に動けるとき。

Ｑ　　４３１＜０．０３〜−０３１．６２１〉０、δ２
く０：第３のアーム３が時計方向に動ける場合で。

かつ、第２のアーム２が反時計方向に動けるとき。

■＜５３１＜０１０３　ｍ−０３１，４２１＜Ｏ１θ２
＃−δ２１：第３のアーム３および第２のアーム２が時
計方向に動けるとき。

また、これら各条件がいずれも満足しない場合には５次
の条件ｅ〜■において、＠手順Ｐ９〜Ｐ　ｔ　２．６手
順Ｐ９〜ｐＨ，Ｐ１４、ト手順Ｐ９　、Ｐｉ３　、Ｐｉ
　ｌ　、ＰＩ３、■手順Ｐ９゜Ｐｉ３．ｐＨ，Ｐｉ４を
それぞれ経て手順Ｐ２３に進み、角速度演算信号６１ａ
”’　６３ａとして、４１ａ＝δ１２．６２ａ＝　δ２
２．６３ａ＝　Ｏを出力する。

■　Ｊ２２＞Ｏ２Ｏ３＜０．δ１２〉０１０１４ｅ１ｏ
：第２のアーム２および第１のアームｌが反時計方向に
動けるとき。

Ｑ　　４２２＞０．０□＜０、（＞１２＜０１ＯＨ’ｓ
、−θ１１：第２のアーム２が反時計方向に動ける場合
で、かつ、第１のアーム１が時計方向に動けるとき。

■　４２２＜０、θ２−４−θ２ｏ、６１２〉０、δ１
””１０：第２のアーム２が時計方向に動ける場合で、
かつ、第１のアーム１が反時計方向に動けるとき。

■　ｊ２２＜Ｏ，θ２−−〇２１．６１２＜０、θ１鵜
−θ１１：第２のアーム２および第１のアーム１が時計
方向に動ける場合。

更に、これらの各条件が満足しない場合には、次の条件
Ｏ〜■において、■手順Ｐ１５〜Ｐ１８、■手順ＰＬ５
〜Ｐ１７．Ｐ２０、■手順Ｐ１５．Ｐ１９．Ｐ１７．Ｐ
１８、■手順Ｐ１５．Ｐ１９．Ｐ１７．Ｐ２０をそれぞ
れ経て手順Ｐ２２に進み、角速度演算信号θ１ａ〜０３
ａとして４１ａ−６１３−６２ａ＝０．６３ａ−４３１
が出力される。

＠　　（＞３３＞Ｏ２Ｏ３＜０．　＜５１３＞０１０１
＃０１０：第３のアーム３および第１のアーム１が反時
計方向に動けるとき。

［Ｆ］　６３３＞Ｏ２Ｏ３＜Ｏ，（５１３＜０．０□−
−δ１１：第３のアーム３が反時計方向に動ける場合で
、かつ、第１のアーム１が時計方向に動けるとき。

■　δ３３＜０１０３＃−０３１，６１３〉０、θ１＃
θ１ｏ：第３のアーム３が時計方向に動ける場合で、か
つ、第１のアームｌが反時計方向に動けるとき。

■　６３３＜０．０３　＃−０３１、（５１３＜　０、
Ｏ１＃−０１１：ｆ：ＪＩＪ３のアーム３および第１の
アーム１が時計方向に動けるとき。

これらいずれもの条件が満足されない場合には、手順Ｐ
２１において各角速度演算信号６１ａ。

６２ａ、６３ａは零とされる。なお、０３く０゜０２く
０の範囲でのみ動作するようにしたが、θ３＞０．０２
＞０の範囲でも条件を適切に定めれば同様に制御できる
。

再び第３図において、サーボ制御部２２は、角速度演算
信号（５Ｈａを積分して位置指令信号ｏ、ｂを出力する
積分器２２１−１〜２２１−３と、積分器２２１−１〜
２２１−３の出力ｏ、ｂと実際のアーム角度Ｏ１との偏
差Δθ１ｂをとる偏差発生器２２２−１〜２２２−３と
、偏差ΔＯＩｂに所定のゲインをかけるゲイン設定器２
２３−１〜２２３−３と、ゲイン設定器２２３−１〜２
２３−３の出力Δ（５Ｈｃに角速度演算信号（５Ｈａを
加え合わせて角速度指令信号δ、ｄを得る加算器２２４
−１〜２２４−３とを有する。

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

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

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

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

速度指令装Ｆ１１３０から、アーム３先端の水平方向（
Ｘ方向）および垂直方向（ｙ方向）の速度指令信号Ｖｘ
、Ｖｙが角速度演算部２１に入力される。更に、角度セ
ンサ４１−１〜４１−３から各アームの相対角度信号０
１〜θ３が角速度演算部２１に入力される。

このような各種信号が入力された角速度演算部２１は、
上述した第４図に示す手順によって角速度演算信号ａｌ
ａ−６３ａを出力する。なお、ここで出力値が零でない
一対の角速度演算信号をδｉ　ａ　＋　ＯＪ　ａ（ｔ　
＝　１〜３　＊　Ｊ　＝　１〜３　）とする。

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

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

Ｏｊｄはリンク補正部２３−蔽、２３−ｊに入力されて
積分器２２１−ｉ　、２２１−Ｊの位置指令信号０＝ｂ
、θｊｂ、すなわちアーム角度に応じた補正演算が実行
されてシリンダ速度信号力１゜之ｊが得られる。この補
正演算は、角速度指令信号６ｔｄ、６ｊｄに所定のゲイ
ンを与えることにより行われる。シリンダ速度信号之；
、之ｊは電流演算部２４−　ｔ　、　２４−　Ｊに入力
されて電磁比側弁５１−　Ｌ　、５１−　Ｊの入力信号
蔽ｉ＋ｉＪに変換される。そして、電磁比例弁５１−ｉ
〜５１−ｊがその入力信号ｉｆ　＊　ｉＪによって制御
され、これにより油圧シリンダ８〜ｌＯが制御されてア
ームｊ＋Ｊが駆動される。

第５図（ａ）、（ｂ）に第３のアーム３先端をＰｌ−Ｐ
２に沿ッテ水平押出しくＶｘ＞０、ｖｙ＝ｏ）とした場
合の各アームの動きを示す、なお、第１のアーム１の回
動支点を０１．第２のアーム２の回動支点を０２）第３
のアーム３の回動支点を０３、第３のアーム３先端を０
４で示し、各部の初期位置をそれぞれＯｌｌ　、　０２
１．０３１　、０４１で示す。

第５図（ａ）は第２のアーム２および第３のアーム３を
駆動して軌跡制御した場合を示し、アーム３先端０４が
０４１→０４２→０４３→０４４と移動するとき、各ア
ーム２．３の連結点は０３１→０３２→０３３→０３４
と移動し、第３のアーム３がストロークエンドに達した
状態、すなわち第２および第３のアーム２．３の第３組
ではもはや軌跡制御ができなくなった状態を示している
。この実施例では、第３組に次いで第１のアームｌおよ
び第２のアーム２の第１組に次の優先順位が与えられて
おり、第３のアーム３がストロークエンドに達しても引
き続き第１および第２のアーム１．２により軌跡制御が
行なわれる。第５図（ｂ）はその場合を示し、アーム３
先端０４が０４４→０４５→０４Ｂと移動するとき、各
アーム１．２の連結点は０２１→０２２呻０２３．アー
ム２，３の連結点は０３４→０３５→０３Ｂと移動し、
第２のアーム２がストロークエンドに達した状態、すな
わち各アーム１〜３が一直線になった状態を示している
。

このように本実施例では、第３のアーム３がストローク
エンドに達しても引き続き第１および第２のアーム１．
２により軌跡制御が行なわれるので、従来のように特定
の２本のアームのみにより軌跡制御する場合に比べて、
第５図（ａ）、（ｂ）におけるアーム３先端軌跡０４４
→０４５→０４８の分だけ作業範囲を広くできる。

以上では３本アームの作業機について説明したが４本以
上のｎ本のアームを有する作業機にも同様にこの発明を
適用できる。この場合、一対のアームは・Ｍ組（Ｍ−Σ
（ｎ−１））できるので、それらの組間に同様に優先順
位を与えて上述のように角速度演算信号６ｚａ、６ｊａ
を求めればよい。

また、アクチュエータを油圧シリンダとしたが空圧シリ
ンダでもよく、油圧、空圧または電動機の回転力を直線
運動に変換してアームを駆動するようなものでもよい。

Ｇ１発明の効果 本発明によれば、軌跡制御のために一対で駆動される２
本のアームの組の中から、予め定めた優先順位に従って
、少なくとも入力された角度信号に基づ８ｚ本の７−ム
とも駆動可俺な組を選択するようにしたので、従来のよ
うに特定の２本のアームのみにより軌跡制御する場合に
比べて作業Ｗｌ団を広くすることができる。

【図面の簡単な説明】

第１図は本発明が適用される多関節作業機の一例と本発
明の一実施例とを示す図、第２図はその多関節作業機の
各アームの回動範囲（ストロークエンド）を示す図、第
３図は第１図の制御装置の詳細ブロック図、第４図はそ
の角速度演算部での処理手順例を示すフローチャート、
第５図（ａ）。 （ｂ）は軌跡制御時に選択されたアームとその動きを説
明する図、第６図は３本アームによる従来の作業機のモ
デル図である。 １〜３：アーム　　　８〜１０：油圧シリンダ３０：指
令手段　　　　４０：角度検出手段５０：制御部　　　
　　８０：角速度演算手段７０：入力信号演算手段 ８０：７クチユエータ 出　願　人　　日立建機株式会社 代理人弁理士　　永　井　冬　紀 第１図 第２図

Claims (1)

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