JPS6375904A - 多関節構造機械の軌跡制御装置 - Google Patents

Abstract

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

Description

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

〔従来の技術〕

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

多関節構造機械の軌跡制御装置の一例として。

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

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

上記従来技術は、移動速度指令が大きく、各動作軸用制
御弁の能力以上の移動速度指令が与えられた場合につい
ては配慮されていなかったので、例えば、３軸のうち２
軸は制御弁の許容値以内の移動速度指令と、１軸は許容
値以上の移動速度指令となるようなアーム先端の移動速
度指令が与えられた場合には、制御弁の許容値以内の２
軸については忠実に追従するが、許容値以上の１軸につ
いては追従することができない、このため、アーム先端
の動作軌跡は所望の軌跡から外れるという問題点があっ
た。

本発明の目的は、移動速度入力が大きく、各動作軸用制
御弁の許容値以上の移動速度指令が与えられた場合でも
、所望の軌跡から外れることのない多関節構造機械の軌
跡制御装置を提供することを目的とする。

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

本発明の上記目的は、軌跡制御装置を移動速度の条件を
与える入力手段と、角速度演算部、流景飽和防止制御部
、角度演算部、サーボ制御部、リンク補正部、流量変換
部から成る演算制御手段と、流量制御手段とから構成す
ることにより達成される。

〔作用〕

入力手段である操作レバーによりアーム先端の移動速度
の直交成分が演算制御手段に入力される。

演算制御手段は、まず角速度演算部により、アーム先端
の直交速度指令を各軸の角速度指令に変換する。流量飽
和防止制御部では角速度指令値。

角度指令値等から各動作軸制御弁の必要流量を演算し、
もしその必要流量が制御弁の許容値を越えたならば、ア
ーム先端の直交速度比すなわちベクトル方向を確保しつ
つ、すべての制御弁流量が許容値内に収まるように再び
角速度を演算し直し、その結果を角度演算部へ送る。こ
れによって、移動速度入力が大きく、各動作軸用制御弁
の許容値以上の移動速度指令が与えられた場合でも、所
望の軌跡から外れることがない。

〔実施例〕

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

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

演算制御装置！１４は、第３アーム先端の水平方向成分
Ｖｘと垂直方向成分ＶＹ及び第３アームの角速度指令値
φから、第３アーム先端（Ｘ、Ｙ）が所望の軌跡を移動
するために必要な第１アームシリンダ４及び第２アーム
シリンダ６及び第３アームシリンダ８の動作速度を演算
し、更に各シリンダの受圧面積から、各シリンダへ供給
すべき圧油の流量、すなわち制御弁の指令値を演算する
。

これらの指令値は電圧の形でサーボ増幅器１５に送られ
る。

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

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

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

角速度演算部１９では入力装置１３における第３アーム
先端の速度入力レバー２５．２６からの水平方向速度成
分Ｖｘ　、垂直方向速度成分Ｖｙと第３アームの角速度
入力レバー２７からの角速度指令値−及び第１〜第３ア
ーム３，５．７の相対角度信号θ工〜θ８及び第１〜第
３アーム３，５゜７の相対角度指令値θｌｒ〜θ８ｒに
基づいて、第１〜第３アーム３，５．７の相対角度指令
信号θ’ｌｒ〜θ′計を演算して出力する。

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

θｘ＝（Ｑｚｅｏｓ（θｌ＋θｚ）Ｖｘ＋　Ｑ　ｓ　５
ｉｎ（θｌ＋θｚ）Ｖｙ＋ｆｉｚｆｌａｓｉｎθδφ）
／　（Ｑ　ｚ　Ｑ　ｚ　ｓｉｎθｚ）　　　・・”ｌｌ
）θｚ＝（−（ｆｉ　１　ｃｏｓθ１＋　１１　ｚ　ｃ
ｏｓ（θｌ＋ｏｚ））Ｖｘ−（ｆｉｔｓｉｎθｌ＋　Ｑ
　ｚ　５ｉｎ（θ１＋ｏｚ））Ｖｙ−（１ｚＮｓｓｉｎ
（θ２＋θｇ）十Ｑ　ｚＱ　ａ　ｇｉｎθ８）φ〕／［
Ｑ１ｆｉｚｓｉｎθｘ〕−・＝　（２）第４図は式（１
）〜（３）の関係の演算をブロック線図で表現したもの
である。

入力袋Ｍ１３及び角度演算ブロック２１の出力Ｖｘ　、
ＶＹ　、φ、θ工１．θ討、θδ、を入力として。

第１〜第３アーム３，５．７の目標角速度θ′ｌｒ。

θ′計、θ’８ｒを演算し出力するものである。第４図
において、ＡＤは加算ブロックを、ＭＵは乗算ブロック
を、ＤＩは除算ブロックを、ＳＩは正弦関数ブロックを
、ｃｏを余弦関数ブロックを、Ｋは係数ブロックを表わ
す。

第５図は流量飽和防止制御部２０の処理内容を示すフロ
ーチャートである。まず１手順２８で角速度演算部１９
の出力である第１アームの角速度指令信号θ″１ｒと角
度演算部２１の出力である第１アーム角度指令値θ１ｒ
から第１シリンダ速度ｉ工′　　を演算する。この詳細
についてはリンク補正部２３で説明する０次に、手順２
９で第１シリンダ速度２″１に第１シリンダ４の受圧面
積Ａ１を乗じることにより第１サーボ弁１６の吐出すべ
き流量Ｑ’１を演算する。そして、手順３０で第１サー
ボ弁１６の流量許容値すなわち出力流量の最大値ＱＩＭ
＾ＸとＱ’ｌの比ＭＳ　を演算する。したがって、例え
ばＱ’１がＱＩＭ＾Ｘより大きければＭｌは１以下の値
となる０手順３１．３２は第１アーム系における手順２
８〜３０と同様の処理を第２．第３アーム系について行
い、Ｍｌ　、Ｍｓを演算する１次に、手順３３でＭ１〜
Ｍ３の内の最小値Ｍを選択する。そして、手順３４でそ
のＭがある規定値Ｍ。

より大きいか、小さいかを判定する。ここで１Ｍ。

＝１の場合はＭをサーボ弁の許容値と比較することにな
る。

以下の説明はＭｏ＝１の場合について行う、そして、Ｍ
ｏの方が小さいと判断された場合すなわち必要流量Ｑ’
ｌ〜Ｑ’ｓが各サーボ弁の許容値を越えていない場合は
手順３５に移り、角速度演算部１９の出力である角速度
指令値θ’ｌｒ〜θ’８ｒをそのまま流量飽和防止制御
部２０の出力である角速度指令値θ１．〜θδ１とする
０手順３４でＭの方が小さいと判断された場合、すなわ
ち必要流量Ｑ’１〜Ｑ’ｓのうちいずれかがサーボ弁の
許容値を越えている場合は手順３６に移り、角速度指令
値θ′五、〜θ′δ１にＭを乗じ、すべての角速度指令
値（Ｊｔｒ〜θ打がサーボ弁の許容値を越えないように
演算し直し出力する。

次に、角度演算部２１について説明する。角速度θｌｒ
〜θ８ｒと角度θ１ｒ〜θ３ｒとの関係は時間をｔ、角
度θ１ｒ〜θ３ｒの初期値（軌跡制御開始時の値）をθ
１０〜θ８ｏとすると自明のことながらθ５ｒ（ｔ）＝
ｆ　　θｚｒｄτ十θ１０　　　−　・・・（４）Ｏ θ１ｒ（ｔ）＝／　　θｚｒｄ　τ＋　Ｏｚｏ　　　　
　　−−（５）θ　５ｒ（ｔ）＝　　ｆ　　θ　ａｒｄ
　　τ　−ト　θ　ｓｏ　　　　　　　　　　　−・”
　（６）となる、角度演算部２１では、流量飽和防止制
御部２０の出力である角速度指令値θｉｒ〜θ８ｒと角
度θ五〜θδから式（４）〜（６）の演算を行い、角度
指令値Ｑｌｒ〜θＩと共に角速度指令値１１１１ｒ〜〜
θＪｌｒを出力するものである。

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

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

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

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

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

このように多関節構造機械の軌跡制御装置を構成すると
、移動速度入力が大きく、各アームのサーボ弁の流量許
容値以上の移動速度指令が与えられた場合でも、第３ア
ーム先端の直交速度比すなわちベクトル方向を確保しつ
つ、すべてのサーボ弁流量が許容値内に収まるよう実時
間で角速度指令値を演算し直すので、所望の軌跡を外れ
ることがない。

〔発明の効果〕

以上述べたように、本発明によれば、移動速度入力が大
きく、各動作軸制御弁の許容値以上の移動速度指令が与
えられた場合でも、所望の軌跡から外れることがないの
で、操作者の意図通りの動きが可能となり、操作性が著
しく向上する。

【図面の簡単な説明】

第１図は本発明の制御装置の一例を備えた多関節構造機
械の構成を示す図、第２図は本発明に用いられる演算制
御装置の機能ブロック図、第３図は多関節構造機械の幾
何学的関係を示す説明図、７第４図は本発明を構成する
角速度演算部の処理内容を示すブロック線図、第５図は
本発明を構成する流量飽和防止制御部の処理内容を示す
フローチャート、第６図は本発明を構成するサーボ制御
部の処理内容を示すブロック線図、第７図は本発明を構
成するリンク補正部の処理内容を示すブロック線図。 ３・・・第１アーム、４・・・第１シリンダ、５・・・
第２アーム、６・・・第２シリンダ、７・・・第３アー
ム、８・・・第３シリンダ、９〜１１・・・検出器、１
２・・・検出回路、１３・・・入力装置、１４・・・演
算制御装置、１５・・・サーボ増幅器、１６〜１８・・
・サーボ弁。

Claims (1)

【特許請求の範囲】
1. １、少なくとも１個以上のアームと、そのアームを駆動
   する１個以上のシリンダから構成され、そのアームの先
   端を所望の軌跡上を移動させるものにおいて、前記各シ
   リンダへ供給すべき圧油の流量を制御する流量制御手段
   と、移動速度の条件を与える入力手段と、この入力手段
   からの条件によりアームの相対角速度指令値を演算する
   角速度演算部、前記角速度指令値と角度指令値から常に
   流量制御手段の最大容量を越えないように角速度指令値
   を演算し直す流量飽和防止制御部、前記角速度指令値を
   積分し角度指令値を演算する角度演算部、この演算部か
   らの角速度指令値及び角度指令値に応じて圧油流量を演
   算する演算部を備えたことを特徴とする多関節構造機械
   の軌跡制御装置。