JPS63114730A - 多関節構造機械の制御装置 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 〔産業上の利用分野〕 本発明は多関節アーム′ｆ！：備える多関節構造機械の
制御装置、さらに詳しくは多関節構造機械のアームの移
動軌跡？制御するのに好適な制御装置に関する。

〔従来の技術〕

多関節構造機械は一般に２ないし３本以上の関節から構
成されておシ、それぞれの部材は円弧軌跡を描いて動作
する。したがって、建屋内への資材の差し込みのように
アームの先端が直線軌跡を描くように動作される場合に
は、複数のレバーを同時に操作しなければならず、相当
の熟練を必要とするばかりでなく作業能率が良好でない
。

多関節構造機械の軌跡制御装置の一例として、油圧ショ
ベルの直朦掘削制御装＃（特公昭５４−３７４０６号）
がある。この直線掘削制御装置ではアーム先端の移動速
度を与える操作レバーと掘削面の角度及びパケット姿勢
角度を設定するダイヤル類、及び自動手動切換えスイッ
チ等から構成される操作盤からの指令に基づいて、演算
制御装置にて所望の掘削軌跡を実現するためのブーム、
アーム及びパケットの動作角度を演算し、その演算結果
に対して油圧サーボ機構によシ、ブーム、アーム及びパ
ケットの動作角度を追従させるものである。

〔発明が解決しようとする問題点〕

上記従来技術はアーム先端を移動させる入力として、操
作レバーでアーム先端の移動速度を与えている。従来技
術を多関節構造機械に適用し、ある物体を一定の１所へ
位置決めする場合１位置決めする場所の近傍へ大まかに
移動する時は移動速度を入力する方法が操作性が良いが
、最終的な位置に位置決めする時は操作性が悪いという
欠点があった。その理由は以下の通シである。多関節構
造機械は一般に大リーチであシ、その長さは数メートル
から数十メートルにも及ぶ。また、各アームの駆動部は
ほとんどアームの支点の近傍となっている。このような
構造の物を用いて、位置決めするために数センチメート
ルの微動を速度入力で行うのは非常にむずかしいからで
ある。

本発明の目的は４−４的な位置決めを容易に出来る多関
節構造機械の制御装置を提供することを目的とする。

〔問題点を解決するための手段〕

上記目的は、軌跡制＃装置を速度指令値を与える速度指
令手段と、位置指令値を与える位置指令手段と、位置指
令による位置指令制御を開始させる位置指令制御開始手
段と、位置−速度変換手段と、これらの手段からの条件
と各アームの相対角度とによってアーム先端が所望の軌
跡を移動するための各シリンダへの圧油流量全演算する
６に算制御手段と、各シリンダへの圧油流量を制御する
流ｉ制御手段とを備えることによシ達成される。

〔作用〕

位置指令手段はダイヤルあるいはディジタルスイッチ等
で現在のアーム位置からの相対移動距離を与える。位置
−速度変換手段は位置指令制御開始手段からの信号と信
号が入力された時のアーム位置から絶対的な目標位置を
演算すると共に、指令速度を演算する。また、位置指令
制御開始手段から一旦信号が入力されると、位置指令制
御が完了するまで速度指令手段からの１号は受は付けな
いようになっている。本発明は以上説明したような動作
をするので、速度指令と位置指令による制御動作を簡単
に切換えることができるので最終的な位置決めを容易に
行うことができる。

〔実施列〕

以下、本発明の一実施例を図面を参照して説明する。

第１図は本発明の制御装置の一例ｔ−１えた多関節構造
機械の構成を示すもので、図において１は旋回体、２は
旋回体１上に設けたブラケット、３はブラケット２に設
けた第１アーム、５は第１アーム３の先端に取付けた第
２アーム、７は第２アーム５の先端に取付けた第３アー
ムである。これらの第１アーム３．第２アーム５及び第
３アーム７＃ｉそれぞれ第１シリンダ４．第２シリンダ
６及び第３シリンダ８によって操作される。これらの第
１アーム３％第２アーム５％第３アーム７の相対角度は
各枢屑点またはその近傍に設けた検出４９〜１１によっ
て検出される。検出器９〜１１の検出器は噴出回路１２
によって入力側に伝送される。多関節構造機械の運転席
（図示せず）には入力装置１３が設置さ几ている。入力
装置１３は移動速度の大きさを与える操作レバーと第３
アームの姿勢速度の大きさを与える操作レバー及び相対
接ａ距離の大きさを与えるダイヤルと第３アームの姿勢
角度の大きさを与えるダイヤルと、位置制御動作開始ス
イッチ及び関節構造機械を従来通りの操作方法で操作す
るための操作レバー類を備えてい谷。

位置−速度変換：１ｔ１４は入力装置１３からの信号よ
り、第３アーム先端の水平方向速度成分速度指令値−を
求め演算制御装置１５へ送る。

第３アーム先端ＣＸ、Ｙ）が所望の軌跡を移動するため
に必要な４１アームシリンダ４及び第２アームシリンダ
６及び第３アームシリンダ８の動作速度を演算し、更に
各シリンダの受圧面積から。

各シリンダへ供給すべき圧油の流量、すなわち制御弁の
指令値を演算する。これらの眉令値は電圧の形でサーボ
増雇器１６へ送られる。

サーボ増幅器１６はこの電圧入力を電流に変換し、入力
電流に対して出力流量が比例する心気油圧サーボ弁、い
わゆるサーボ弁１７〜１９を、駆動する。サーボ弁１７
〜１９からの圧油はそれぞれ直接各シリンダ４，６．８
に導かれる。これてよシ第３アームの先端は所望の軌跡
を移動する。

５０．５１は第１シリンダの圧力を検出する圧力検Ｌｔ
ｌ器であシ、この検出器で検出された値を用いることに
よって、アームの振動を抑制することが可能である。な
お、　１ｌＥ２．第３シリンダ系についても省略するが
同様の圧力検出器が装備されて除る。

第１因に示された入力装置１３と位置−速度変換装置１
４について更に詳しく説明する。

第２図は入力装置１３と位置−速度変換装置１４０機能
ブロック図である。入力装置１３は第３アーム先端の水
平方向速度大カンバー１３１゜垂直方向速度大カンバー
１３２．第３アームの姿勢速度入力レバー１３３．水平
方向相対位置入力ダイヤル１３４．垂直方向相対位置入
力ダイヤル１３５、第３アームの相対姿勢入力ダイヤル
１３６及び位置指令制御開始スイッチ１３７から構成さ
れており、それぞれｖ”　”　ｒ　＋　（”　＋　Ｘ　
＊　Ｙ　、φと０Ｎ−ＯＦＦ信号を位置−速度変換装置
１４へ出力する。なお、位置指令制御開始スイッチ１３
７は押している間だけＯＮ信号となる、いわゆるモーメ
ンタリスイッチである。

位置−速度変換装置１４は入力装置１３からの信号と演
算制御装置１５からのアーム相対角度θｌ〜θ３及びサ
ーボ偏差ｇ１〜Ｃ３に基づいて。

−ムの姿勢速度指令１直φｔ−演算して出力するもので
あり、アーム相対角度０１〜θ３から第３アーム先端の
位Ｉｔ（Ｘ、Ｙ）と姿勢角度φを演算する位置演算部１
４１とサーボ偏差ε１〜ε３からサーボ偏差が許容値以
内であるかどうかを判定する比較部１４２及び位置制御
動作開始スイッチ１３７の信号と前記位置演算部１４１
と比較部１４２の信号から速度指令レバー１３１，１３
２，１３３と位置、角度指令ダイヤル１３４，１３５，
１３６のうちどちらかを入力として速度指令値を演算す
る速度指令演算部１４３，１４４，１４５から構成され
ている。

第３図は位置−速度変換装置１４の処理内容を示すフロ
ーチャートである。まず１手順２０で位置指令ダイヤル
１直ｘ＊　　Ｙ＋　　φと速度指令レバー１直ｙｘ、ｙ
、、　　φをと９込む。次に２手順２１で位置指令制御
開始スイッチ１３７がＯＮかどうか調べる。もし、ＯＦ
Ｆ’ならば次に、手順２４に移り、動作フラグがＯＮか
どうか調べる。この動作フラグとは位置指令制御中にＯ
Ｎとなるフラグであり、当然最初はＯＦＦとなっている
ので手順は２３へ速度指令レバー直ｖ、、ｖア、ａを演
算°制御装置１５へ出力する。位置指令制御開始スイッ
チ１３７が押されない間は、この手順２０．　２１．　
２２゜２３を繰シ返し行っている。そして、位置指令制
御開始スイッチ１３７が押されると、手順２１から手順
２４へ移シ動作フラグがＯＮかどうか判定される。当然
最初はＯＦＦとなっているので処理は手順２５へ移る。

手順２５では、現在のアーム相対角度０１〜θ３から演
算した第３アーム先端の位［ＩＸＯ、Ｙｏと第３アーム
の姿勢角度φＧに位置指令ダイヤル値ｘ＊Ｙ＊　　φを
加えた値を位置指令ｌ直ｘｉｓ）’ｌ＋　　φ１とする
。そして、手順２６で動作フラグをＯＮＬ、手ｊ頁２０
に戻る。次に１手順２２　ｔたは手順２４で動作フラグ
がＯＮ・仁判断され処理は手順２７に移る。手順２７は
先福求めた位置指令値を速度指令値に変換する。

第４図は手順２７の処理内容の一例を示すフローチャー
トである。まず、ｖ８に関する演算について説明する。

手順３０で位１指令値ＸＩ　と前回サンプリング時のア
ーム位ｆｘｏ　との偏差ΔＸを演算する。次に、偏差Δ
Ｘの絶対値がある規定値ΔＸ１よシ大きいかどうか判定
し、もし大きければ処理は手順３２に移り、規定値ΔＸ
ｌをグログラムのサンプリング時間ΔＴで割った値を速
度指令値Ｙ８として演算制御袋＃ｔ１５へ出力する。そ
して、前回サンプリング時のアーム位置Ｘ、Ｈに規定値
ΔＸｉを加えた値ｘＯを今回サンプリング時のアーム位
置とする。次に、位置−速度変換完了フラグをＯＦＦに
する。ここで１位置−速度変換完了フラグがＯＦＦとい
うことは位置指令を速度指令に変換中であることを示す
。そして、処理はＶア、φに関するｉＸ算を行い１手順
２８へ移る。

また、■！に関する演算の手順３１で偏差ΔＸの絶対値
が規定値Δｘ１より小さければ処理は手順３７へ移り、
速度指令値ｖ８を０とすると共に位置−速度変換完了フ
ラグをＯＮにする。

第３図における手順２７で位置指令−速度指令変換を行
った後、手順２８で位置指令制御が完了したかどうか判
定する。この判定は、ｖｘ、ｖ、。

／ト φに関する演算における位置−速度変換完了フラグが全
てＯＮで、かつサーボ偏差Ｃ１〜ε３のすべてがある規
定値以下になったことによって行う。

最初は自然位置−速度変換完了フラグがＯＦＦなので、
処理は手順２０に戻る。そして、手順２０゜２１．２２
，２７，２８．２０・・・を繰り返すうちに２位置指令
制御が完了すると処理は手順２９に移シ動作フラグｔＯ
ＦＦにする。動作フラグがＯＦＦになると、処理の手順
は２０，２１，２２゜２３．２０・・・とな）、最初説
明した処理内容に戻る。

以上、説明したように位置−速度変換装置１４は、位置
指令制御開始スイッチ１３７が押されない間は速度指令
値として速度指令レバー値を、開始スイッチ１３７が押
さｎると速度指令値として位置指令値から演算した値を
演算制御袋ｆ１５へ出力するものである。

次Ｋ、第１図に示された演算制御装置１５について詳し
く説明する。

第５図は演算制御装置１５の機能ブロック図である。演
算制御装置１５は角度角速度演算部３９゜サーボ制御部
４０．ＩＪンク補正部４１、流量変換部４２から構成さ
れている。

角度角速度演算部３９では、位置−速度変換装置からの
第３アーム先端の速度指令値Ｖ、、　　’ｌ／。

と第３アームの角速度指令値ａ及び第１〜第３アーム３
，５．７の相対角度信号θｌ〜θ３に基づいて、第１〜
第３アーム３，５．７の相対角速度指令信号θ１．〜θ
３２ならびに相対角度指令信号θ１．〜θ３、を６ＩＬ
′ｘシて出力する。

この演算を実行するために多関節構造機械における各部
の角度及び長さを第６図のように定めると、第３アーム
先端ＰＫおける速度成分Ｖｘ、Ｖｙと角速度Ｊ及び第１
〜第３アーム３，５．７の相対角速度θｌ〜θ３との関
係は次式のように書き表わすことができる。

シｌ　＝（Ａｔ　”　（ｄＩ＋θｚ）　Ｖｘ＋　ｔｘ　
ｓｉｎ　（θｌ十０２）ｖ！＋　Ｌ２１３　ｓｉｎ　θ
３＃　）／　（ｔｌ　ｔ２　ｓｉｎ　　θｚ　）　　　
　　　・”（１）θ’２＝Ｃ（ｔｘ”Ｓθｔ＋ｔｚ”（
θｌ十θｚ））ｖｘ、’Ｊ’ｊ”　　　（ｚ、５ｉｎｌ
りｔ＋ｊ□５ｉｎ（ｊｘ＋、ｚ））Ｖ、−（ｌ　１７３
ｓｉＱ（θｌ＋θｚ　）＋　ｔ２　ｔ３ｓｉｎθ３）φ
〕／　（４ｔ２　ｓｉｎθ２　］　　　　　　　　　　
＝　（２）θ３＝φ−θ１−θ２　　　　　　　　　　
　　・・・（３）また、角速度θ１〜θ３と角度θ１〜
θ３との関係は時間をｔとし、角度θｌ〜θ３の初期値
（軌跡制御開始時の値）をθｎｏ〜θ３ｏとすると自明
のことながら となる。

第７図は式（１）〜（６）の関係の演算をブロック線図
で表現したものである。

へ　　　　Δ　　　　′ト 位置−速度変換装置１４の出力Ｙｘ、Ｖ、、　　φを入
力として、第１〜第３アーム３，５．７の自相角速度θ
ｌ、〜θ３、ならびに目標角度θｌ、〜θ３．を演算し
出力するものである。

第７図において、ＡＤは加算ブロックを、ＭＵは乗算ブ
ロックを、ＤＩは除算ブロックを、ＳＩは正弦関数ブロ
ックを、Ｃｏは余弦関数ブロックを、Ｋは係数ブロック
を表わす。

次にサーボ制御部４０について詳細に説明する。

第８図はサーボ制御部４０における処理を示すブロック
線図である。図における記号は第７図と同様に人りは加
Ｘ器を、には係数器を表わす。すなわち、第１アーム系
について説明すると、角度角速度演算部３９の出力であ
る第１アーム角度指令値θ１２と実際の角度θｌとを比
較してその差をと９、そのサーボ偏差ε！に係数に１を
かけた結果と、角度角速度演算部３９の出力である第１
アーム角速度指令値θｌ、を加算して、新たな第１アー
ム角速度指令値θ１．を出力する。第２アーム。

２、第３アームの角速度指令値θ２１．θ３．を出力す
る。

次にリンク補正部４１について第９図を用いて説明する
。第９図において４３〜４５はアーム角度指令値θ１．
〜θｓｒｔ入力として、各アームのシリンダ速度と角速
度との比を出力する関数発生器である。すなわち、関数
発生器４３に第１アーム角度指令値θｔｒ’？入力する
と、その０１．に対するシリンダ速度Ｚｉ　と第１アー
ム角速度ｉ！の比が、　　　　ζ＼ 出力される。このＺ１／θｌにθｌ、を乗すると、その
時の第１シリンダ速度Ｚ１を得ることができ、この演算
結果を出力する。ｉ２アーム、第３アーム系についても
同様の処理を施して、第２．第３ツリンダ速度”２＋　
　２３に出力する。

流量変換部４２はリンク補正部４１の出力である各シリ
ンダ速度指令値Ｚ１％Ｚ３に各シリンダ４．６，８の受
圧面積を乗じることによりす〜ボ弁１７〜１９が吐出す
べ′＠流ｆを演算し、電圧Ｖｌ−Ｖ３の形でサーボ増幅
器１６へ出力する。

サーボ増幅器１６ではこの電圧ｖｌ〜ｖ３に応じた流量
を各サーボ弁１６〜１８が吐出するように、それぞれの
各サーボ弁を制御する。

このように多関節構造機械の軌跡制御装置全構成すると
、速度指令１位置指令の切換えを簡単に行うことができ
、しかも位置指令制御が完了しないうちは速度指令や位
置指令制御開始スイッチが０Ｎ−ＯＦＦしても位置指令
制御が継続するので誤操作することがない。

なお、位置指令−速度指令変換の手順２７の一例として
第４図の処理内容を示したか、規定値Δｘ１＋　　Δ’
Ｊ１ｔ　Δφ１を位置指令ダイヤルｉｌｃ関する演算の
位置−速度変換完了フラグが同時にＯＦＦからＯＮに切
換わるようにし九り、他の位を指令−速度指令変換処理
を施しても本発明の主旨から外れるものではない。

また、実施列は多関節構造機械の軌跡制御について示し
たが、軌跡側＠を行わず、直交速度成分ＶＸ＋　ｖ７の
代シに角速度指令を、ｘ、ｙの代９に角度指令を与える
構成にしても本発明の主旨から外れるものではない。

〔発明の効果〕

以上述べたように１本発明によれば、速度指令と位置指
令のどちらでも入力することができるので、位置決めす
る場所の近傍へ大まかに移動する時は速度指令を用い、
最終的な位置へ位置決めするときは位置指令を用いるこ
とができ、非常に操作性が向上する。

【図面の簡単な説明】

第１図は本発明の制御装置の一例を備えた多関節構造機
械の構成を示す図、第２図は入力装置と位置−速度変換
装置の機能ブロック図、第３図は位置−速度変換装置の
処理内容を示すフローチャート、第４図は手順２７の処
理内容の一例を示すフローチャート、第５図は演算制御
装置の機能ブロック図、第６図は多関節構造機械の幾何
学的関係を示す図、第７図は角度角速度演算部の処理内
容を示すブロック線図、第８図はサーボ制御部の処理内
容を示すブロック組図、第９図はリンク補正部の処理内
容を示すブロック線図である。 ３・・・第１アーム、４・・・第１シリンダ、５・・・
第２アーム、６・・・第２シリンダ、７・・・第３アー
ム５８・・・第３シリンダ、９〜１１・・・検出器、１
２・・・検出回路、１３・・・入力装置、１４・・・位
置−速度変換装置、１５・・・演算制御装置、１６・・
・サーボ増幅器、１７第　４　図 手＝ｒ＠２２ｆｔ−１５手−ＩＱ２４Ｊ　ソψ 手”＋ｉ　ＺＢへ 第　５　図 篤　９　図 手続有１１正書（自発） １．事件の表示 昭和６１年特許願第　２５８２９５　　号２、発明の名
称　　多関節構造機械の制御装置３、補正をする者 事件との関係　　特許出願人 名称（５５２）日立建機株式会社 ４、代　理　人 居　所〒１００東京都千代田区丸の内−丁目５番１号株
式会社　　日　立　製　作　所　内 電話東京　　２１２−１１１１（大代表）氏名（６８５
０）弁理士　小　川　勝　刃圧する。 （２）図面に添付第１０図を加入する。 以上 全文訂正明細書 日　１、発明の名称 多関節構造機械の制御装置 ２、特許請求の範囲 １、少なくとも１個以上のアームと、そのアームを駆動
する１個以上のアクチュエータから構成され、そのアー
ムの先端を移動させるものにおいて、前記アクチュエー
タへ供給すべき圧油の流量を制御する流量制御手段と、
移動速度の条件を与える速度指令手段と、移動距離の条
件を与える」指定手段と５位置指令による位置指令制御
を開始させる位置指令制御開始手段と、位置指令を速度
指令に変換すると共に位置指令段からの条件によりアー
ムの相対角速度指令値および相対角度指令値を演算する
角度角速度演算部と、この角度角速度演算部からの指令
値に応じて圧油流量を演する演算制御手段とを備えたこ
とを特徴とする多関節構造機械の制御装置。 ３、発明の詳細な説明 〔産業上の利用分野〕 本発明は多関節アームを備える多関節構造機械の制御装
置、さらに詳しくは多関節構造機械のアームの移動軌跡
を制御するのに好適な制御装置に関する。 〔従来の技術〕 多関節構造機械は一般に２ないし３本以上の関節から構
成されており、それぞれの部材は円弧軌跡を描いて動作
する。したがって、建屋内への資材の差し込みのように
アームの先端が直線軌跡を描くように動作される場合に
は、複数のレバーを同時に操作しなければならず、相当
の熟練を必要とするばかりでなく作業能率が良好でない
。 多関節構造機械の軌跡制御装置の一例として、油圧ショ
ベルの直線掘削制御装置（特公昭５４−３７４０６号）
がある。この直線掘削制御装置ではアーム先端の移動速
度を与える操作レバーと掘削面の角度及びパケット姿勢
角度を設定するダイヤル類、及び自動手動切換えスイッ
チ等から構成される操作盤からの指令に基づいて、演算
制御装置にて所望の掘削軌跡を実現するためのブーム、
アーム及びパケットの動作角度を演算し、その演算結果
に対して油圧サーボ機構により、ブーム、アーム及びパ
ケットの動作角度を追従させるものである。 〔発明が解決しようとする問題点〕 上記従来技術はアーム先端を移動させる入力として、操
作レバーでアーム先端の移動速度を与えている。従来技
術を条間ｍ　４％造機械に適用し、ある物体を一定の場
所へ位置決めする場合、位置決めする場所の近傍へ大ま
かに移動する時は移動速度を入力する方法が操作性が良
いが、最終的な位置に位置決めする時は操作性が悪いと
いう欠点があった。その理由は以下の通りである。多関
節構造機械は一般に大リーチであり、その長さは数メー
トルから数十メートルにも及ぶ。また、各アームの駆動
部はほとんどアームの支点の近傍となっている。このよ
うな構造の物を用いて、位置決めするために数センチメ
ートルの微動を速度入力で行うのは非常にむずかしいか
らである。 本発明の目的は最終的な位置決めを容易に出来る多関節
構造機械の制御装置を提供することを目的とする。 〔問題点を解決するための手段〕 上記目的は、軌跡制御装置を速度指令値を与える速度指
令手段と、位置指令値を与える位置指令手段と１位置指
令による位置指令制御を開始させる位置指令制御開始手
段と、位置−速度変換手段と、これらの手段からの条件
と各アームの相対角度とによってアーム先端が所望の軌
跡を移動するための各シリンダへの圧油流量を演算する
演算制御手段と、各シリンダへの圧油流量を制御する流
量制御手段とを備えることにより達成される。 〔作用〕 位置指令手段はダイヤルあるいはディジタルスイッチ等
で現在のアーム位置からの相対移動距離を与える。位置
−速度変換手段は位置指令制御開始手段からの信号と信
号が入力された時のアーム位置から絶対的な目標位置を
演算すると共に、指令速度を演算する。また１位置指令
制御開始手段から一旦信号が入力されると、位置指令制
御が完了するまで速度指令手段からの信号は受は付けな
いようになっている。本発明は以上説明したような動作
をするので、速度指令と位置指令による制御動作を簡単
に切換えることができるので最終的な位置決めを容易に
行うことができる。 〔実施例〕 以下、本発明の一実施例を図面を参照して説明する。 第１図は本発明の制御装置の一例を備えた多関節構造機
械の構成を示すもので１図において１は旋回体、２は旋
回体ｌ上に設けたブラケット、３はブラケット２に設け
た第１アーム、５は第１アーム３の先端に取付けた第２
アーム、７は第２アーム５の先端に取付けた第３アーム
である。これらの第１アーム３．第２アーム５及び第３
アーム７はそれぞれ第１シリンダ４．第２シリンダ６及
び第３シリンダ８によって操作される。これらの第１ア
ーム３．第２７−４５．第３アーム７の相対角度は各枢
着点またはその近傍に設けた検出器９〜１１によって検
出される。検出器９〜１１の検出値は検出回路１２によ
って入力側に伝送される。多関節構造機械の運転席（図
示せず）には入力装置１３が設置されている。入力装置
１３は移動速度の大きさを与える操作レバーと第３アー
ムの姿勢速度の大きさを与える操作レバー及び相対移動
距離の大きさを与えるダイヤルと第３アームの姿勢角度
の大きさを与えるダイヤルと、位置制御動作開始スイッ
チ及び関節構造機械を従来通りの操作方法で操作するた
めの操作レバー類を備えている。 位置−速度変換装置１４は入力装置１３からの第３アー
ム先端（Ｘ、Ｙ）が所望の軌跡を移動するために必要な
第１アームシリンダ４及び第２アームシリンダ６及び第
３アームシリンダ８の動作速度を演算し、更に各シリン
ダの受圧面積から、各シリンダへ供給すべき圧油の流量
、すなわち制御弁の指令値を演算する。これらの指令値
は電圧の形でサーボ増幅器１６へ送られる。 サーボ増幅器１６はこの電圧入力を電流に変換し、入力
電流に対して出力流量が比例する電気油圧サーボ弁、い
わゆるサーボ弁１７〜１９を駆動する。サーボ弁１７〜
１９からの圧油はそれぞれ直接各シリンダ４，６．８に
導かれる。これにより第３アームの先端は所望の軌跡を
移動する。 ５０．５１は第１シリンダの圧力を検出する圧力検出器
であり、この検出器で検出された値を用いることによっ
て、アームの振動を抑制することが可能である。なお、
第２．第３シリンダ系についても省略するが同様の圧力
検出器が装備されている。 第１図に示された入力装置１３と位置−速度変換装置１
４について更に詳しく説明する。 第２図は入力装置１３と位置−速度変換装置１４の機能
ブロック図である。入力表［１３は第３アーム先端の水
平方向速度入力レバー１３１゜垂直方向速度入力レバー
１３２．第３アームの姿勢速度入力レバー１３３．水平
方向相対位置入力ダイヤル１３４．垂直方向相対位置入
力ダイヤル１３５、第３アームの相対姿勢入力ダイヤル
１３６及び位置指令制御開始スイッチ１３７から構成さ
れており、それぞれＶｘ　ｇ　Ｖｙ　Ｈφ、ｘ、ｙ、φ
と０Ｎ−ＯＦＦ信号を位置−速度変換装置１４へ出力す
る。なお、位置指令制御開始スイッチ１３７は押してい
る間だけＯＮ信号となる、いわゆるモーメンタリスイッ
チである。 位置−速度変換装置１４は入力装置１３からの信号と演
算制御装置１５からのアーム相対角度Δ −ムの姿勢速度指令値φを演算して出力するもので娶り
、アーム相対角度０１〜θ３から第３アーム先端の位置
（Ｘ、Ｙ）と姿勢角度φを演算する、位置演算部１４１
とサーボ偏差ε１〜ε８からサーボ偏差が許容値以内で
あるかどうかを判定する比較部１４２及び位置制御動作
開始スイッチ１３７の信号と前記位置演算部１４１と比
較部１４２の信号から速度指令レバー１３１，１３２，
１３３と位置、角度指令ダイヤル１３４，１３５，１３
６のうちどちらかを入力として速度指令値を演算する速
度指令演算部１４３，１４４，１４５から構成されてい
る。 第３図は位置−速度変換装置１４に処理内容を示すフロ
ーチャートである。まず、手順２ｏで位置指令ダイヤル
値ＸｖＶｐ　φと速度指令レバー値Ｖｘ　ＨＭｙ　ｐ　
φをとり込む。次に、手順２１で位置指令制御開始スイ
ッチ１３７がＯＮかどうか調べる。もし、ＯＦＦならば
次に、手順２４に移り、動作フラグがＯＮかどうか調べ
る。この動作フラグとは位置指令制御中にＯＮとなるフ
ラグであり、速度指令レバー値ＶＸ　、　ＶＦ　、φを
演算制御装置１５へ出力する。位置指令制御開始スイッ
チ１３７が押されない間は、この手順２０，２１，２２
゜２３を繰り返し行っている。そして、位置指令制御開
始スイッチ１３７が押されると、手順２１から手順２４
へ移り動作フラグがＯＮかどうか判定される。当然最初
はＯＦＦとなっているので処理は手順２５へ移る。手順
２５では、現在のアーム相対角度０１〜θ３から演算し
た第３アーム先端の位置ＸＯ、ＹＯと第３アームの姿勢
角度φ０に位置指令ダイヤル値ｘ、ｙ、φを加えた値を
位置指令値ＸＩ　ｖ　ｙｌ　ｐ　φ１とする。そして、
手順２６で動作フラグをＯＮし、手順２０に戻る１次に
、手順２２または手順２４で動作フラグがＯＮと判断さ
れ処理は手Ｍ２７に移る１手順２７は先程求めた位置指
令値を速度指令値に変換する。 第４図は手順２７の処理内容の一例を示すフロΔ −チャートである。まず、ＶＸに関する演算について説
明する。手順３０で位置指令値Ｘ＋　と前回サンプリン
グ時のアーム化ｆＦＪ　ｘ　ｏとの偏差ΔＸを演算する
。次に、偏差ΔＸの絶対値がある規定値Δｘ１より大き
いかどうか判定し、もし太きければ処理は手順３２に移
り、規定値Δｘ１をプログラムのサンプリング時間ΔＴ
で割った値を速度積△ 令値Ｖｘとして演算制御装置１５へ出力する。そして、
前回サンプリング時のアーム化Ｗｘｏに規定値ΔＸ１を
加えた値ｘｏを今回サンプリング時のアーム位置とする
。次に、位置−速度変換完了フラグをＯＦＦにする。こ
こで、位置−速度変換完了フラグがＯＦＦということは
位置指令を速度△ また、ＶＸに関する演算の手順３１で偏差ΔＸの絶対値
が規定値ΔＸｉより小さければ処理は手順△ ３７へ移り、速度指令値■８を０とすると共に位置−速
度変換完了フラグをＯＮにする。 第３図における手順２７で位置指令−速度指令ハ φに関する演算における位置−速度変換完了フラグが全
てＯＮで、かつサーボ偏差ｆｆ１ｉ〜ε８のすべてかあ
る規定値以下になったことによって行う。 最初は当然位置−速度変換完了フラグがＯＦＦなので、
処理手順２０に戻る。そして、手）ＩＮ　２０　。 ２１，２２，２７，２８，２０・・・を繰り返すうちに
、位ｆｆ２指令制御が完了すると処理は手順２９に移り
動作フラグをＯＦＦにする。動作フラグがＯＦＦになる
と、処理の手順は２０，２１，２２゜２３．２０・・・
となり、最初説明した処理内容に戻る。 以上、説明したように位置−速度変換装置１４は１位置
指令制御開始スイッチ１３７が押されない間は速度指令
値として速度指令レバー値を、開始スイッチ１３７が押
されると速度指令値として位置指令値から演算した値を
演算制御装置１５へ出力するものである。 次に、第１図に示された演算制御装置１．５について詳
しく説明する。 第５図は演算制御装置１５の機能ブロック図である。演
算制御装置１５は角度角速度演算部３９゜サーボ制御部
４０．リンク補正部４１．流量変換部４２から構成され
ている。 角度角速度演算部３９では、位置−速度変換装置 と第３アームの角速度指令値φ及び第１〜第３アーム３
，５．７の相対角度信号０１〜θ３に基づいて、第１〜
第３アーム３，５．７の相対角速度指令信号θＬｒ〜θ
３ｒならびに相対角度指令信号ｅ１ｒ〜０８．を演算し
て出力する。 この演算を実行するために多関節構造機械における各部
の角度及び長さを第６図のように定めると、第３アーム
先端Ｐにおける速度成分ＶＸ＃　ＶＹと角速度φ及び第
１〜第３アーム３，５，７の相対角速度θＬ〜θＳとの
関係は次式のように書き表わすことができる。 θ１＝（Ｌｚｃｏｓ（θ１十ｏｚ）　Ｖｘ＋　Ｑ　ｚ　
５ｉｎ（θ１＋θｚ）Ｖｙ＋ＱｚＱｓｓｉｎθ３φ）／
（ＱｘＱｚｓｉｎθ！）　　　　・（１）０２、＝　（
−（Ｑ　１　ｃｏｓθ、＋　ｎ　２　ｃｏｓ（θ工＋ｏ
ｚ））Ｖｘ（１２ｘｓｉｒｌθ１＋　Ｑ　２５ｉｎ（θ
１＋０２））ＶＹ−（ＱｘＱｓ　５ｉｎ（１７ｔ＋θｚ
）＋　Ｑ　ｚＱ　ａ　ｓｉｎθ８）φ°〕／　（Ｑ工Ｑ
ｓ　ｓｉｎθｚ）　　　　　　　　　　　　−（２）ま
た、角速度０１〜θ８と角度０１〜０３との関係は時間
をｔとし、角度０１〜θ８の初期値（軌跡制御開始時の
値）をθ１ａ−Ｏｓｏとすると自明のことながら θ１（ｔ）＝、／　　θｔｄ　τ＋　Ｏｓｏ　　　　　
　　”・（４）１７ｚ（ｔ）＝ｆ　Ｏｚｄτ＋θｚｏ　
　　　　　　−（５）θ５（ｔ）＝／　　θ３ｄτ＋θ
ｓｏ　　　　　　　−（６）となる。 第７図は式（１）〜（６）の関係の演算をブロック線位
置−速度変換装置１４の出力ＶＸ　、　ｖｙ　、φを入
力として、第１〜第３アーム３，５．７の同相角速度θ
１ｒ〜θ３．ならびに目標角度θＩｒ〜Ｏａｒを演算し
出力するものである。 第７図において、ＡＤは加算ブロックを、ＭＵは乗算ブ
ロックを、ＤＩは除算ブロックを、Ｓ■は正弦関数ブロ
ックを、ＣＯは余弦関数ブロックを、には係数ブロック
を表わす。 次にサーボ制御部４０について詳細に説明する。 第８図はサーボ制御部４０における処理を示すブロック
線図である。図における記号は第７図と同様にＡＤは加
算器を、Ｋを係数器を表わす。すなわち、第１アーム系
について説明すると、角度角速度演算部３９の出力であ
る第１アーム角度指令値θ１．と実際の角度θ１とを比
較してその差をとり、そのサーボ偏差ε五に係数に１を
かけた結果と、角度角速度演算部３９の出力である第１
７−ム角速度指令値θｌｒを加算して、新たな第１７ヘ ーム角速度指令値θｌ、を出力する。第２アーム。 する。 次にリンク補正部４１について第９図を用いて説明する
。第９図において４３〜４５はアーム角度指令値θ工、
〜ｅａｒを入力として、各アームのシリング速度と角速
度との比を出力する関数発生器である。すなわち、関数
発生器４３に第１アーム角度指令値Ｏｘｒを入力すると
、そのθｌｒに対するの時の第１シリンダ速度Ｚ１を得
ることができ。 この演算結果を出力する。第２アーム、第３アーム系に
ついても同様の処理を施して、第２．第３シリンダ速度
２２．Ｚδを出力する。 流量変換部４２はリンク補正部４１の出力である各シリ
ンダ速度指令値ｚ１〜２δに各シリンダ４．６．８の受
圧面積を乗じることによりサーボ弁１７〜１９が吐出す
べき流量を演算し、電圧■１〜■３の形でサーボ増幅器
１６へ出力する。 サーボ増幅器１６ではこの電圧ｖ１〜ｖ８に応じた流量
を各サーボ弁１７〜１９が吐出するように、それぞれの
各サーボ弁を制御する。 次に、サーボ増幅器１６について第１０図を用いて説明
する。第１０図には、圧力検出器５０゜５１を用いて、
アームの振動を抑制する回路を含む。 まず、第１シリンダ４の系統について説明すると、検出
器５１からの第１シリンダ４のヘッド側圧力信号ＰＩＨ
に係数器１６０において、ヘッド側面積Ａｌ）ｌを掛け
た値と、検出器５０からのロンド側圧力信号ＰＩＨに係
数器１６１においてロンド側面＠Ａｘ＊を掛けた値との
差を、比較器１６２により求め、その値、すなわちシリ
ンダ推力ｆ１をフィルタ回路１６３に出力する。このフ
ィルタ１６３は、重力加速度によるシリンダ推力の影響
を除去するためのバイパスフィルタ（２次〜３次モード
以上の振動成分を除去するためのバンドパスフィルタに
してもよい）である。 そして、演算制御袋Ｗ１１１５からの信号ｖ１とフィル
タ回路１６３を通過後のシリンダ推力信号ｆ１に係数器
１６４において係数Ｋｓｚを掛けた値との差ｔ１を、比
較器１６５により求める。さらに、その値ε１を電圧−
電流変換回路１６６により、電流変換し、サーボ弁１７
に供給する。 なお、第２．第３シリンダ系に７いては省略するが同様
の回路となっている。 このように多関節構造機械の軌跡制御装置を構成すると
、速度指令９位置指令の切換えを簡単に行うことができ
、しかも位置指令制御が完了しないうちは速度指令や位
置指令制御開始スイッチが０Ｎ−ＯＦＦｔ、でも位置指
令制御が継続するので誤操作することがない。 なお、位置指令−速度指令変換の手順２７の一例として
第４図の処理内容を示したが、規定値間する演算の位置
−速度変換完了フラグが同時にＯＦＦからＯＮに切換ね
るようにしたり、他の位置指令−速度指令変換処理を施
しても本発明の主旨から外れるものではない。 また、実施例は多関節構造機械の軌跡制御について示し
たが、軌跡制御を行わず、直交速度成分Ｖｘ　、ｖｙの
代りに角速度指令を、Ｘｔ　ｙの代りに角度指令を与え
る構成にしても本発明の主旨から外れるものではない。 （発明の効果） 以上述べたように、本発明によれば、速度指令と位置指
令のどちらでも入力することができるので、位置決めす
る場所の近傍へ大まかに移動する時は速度指令を用い、
最終的な位置へ位置決めするときは位置指令を用いるこ
とができ、非常に操作性が向上する。 ４、図面の簡単な説明 第１図は本発明の制御装置の一例を備えた多関節構造機
械の構成を示す図、第２図は入力装置と位置−速度変換
装置の機能ブロック図、第３図は位置−速度変換装置の
処理内容を示すフローチャート、第４図は手順２７の処
理内容の一例を示すフローチャート、第５図は演算制御
装置の機能ブロック図、第６図は多関節構造機械の幾何
学的関係を示す図、第７図は角度角速度演算部の処理内
容を示すブロック線図、第８図はサーボ制御部の処理内
容を示すブロック線図、第９図はリンク補正部の処理内
容を示すブロック線図、第１０図はサーボ増幅器１６の
構成を示すブロック線図である。 ３・・・第１アーム、４・・・第１シリンダ、５・・・
第２アーム、６・・・第２シリンダ、７・・・第３アー
ム、８・・・第３シリンダ、９〜１１・・・検出器、１
２・・・検出回路、１３・・・入力装置、１４・・・位
置−速度変換装置。 １５・・・演算制御装置、１６・・・サーボ増幅器、１
７〜１９・・・サーボ弁。

Claims (1)

【特許請求の範囲】
1. １、少なくとも１個以上のアームと、そのアームを駆動
   する１個以上のアクチュエータから構成され、そのアー
   ムの先端を移動させるものにおいて、前記アクチュエー
   タへ供給すべき圧油の流量を制御する流量制御手段と、
   移動速度の条件を与える速度指令手段と、移動距離の条
   件を与える速度指令手段と、位置指令による位置指令制
   御を開始させる位置指令制御開始手段と、位置指令を速
   度指令に変換すると共に位置指令制御と速度指令による
   速度指令制御を切換える位置−速度変換手段と、この位
   置−速度変換手段からの条件によりアームの相対角速度
   指令値および相対角度指令値を演算する角度角速度演算
   部と、この角度角速度演算部からの指令値に応じて圧油
   流量を演する演算制御手段とを備えたことを特徴とする
   多関節構造機械の制御装置。