JPH0766287B2 - 多関節作業機の軌跡制御装置 - Google Patents
多関節作業機の軌跡制御装置Info
- Publication number
- JPH0766287B2 JPH0766287B2 JP61083357A JP8335786A JPH0766287B2 JP H0766287 B2 JPH0766287 B2 JP H0766287B2 JP 61083357 A JP61083357 A JP 61083357A JP 8335786 A JP8335786 A JP 8335786A JP H0766287 B2 JPH0766287 B2 JP H0766287B2
- Authority
- JP
- Japan
- Prior art keywords
- arm
- arms
- angular velocity
- signal
- angle
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Lifetime
Links
Landscapes
- Manipulator (AREA)
- Numerical Control (AREA)
Description
【発明の詳細な説明】 A.産業上の利用分野 本発明は、３本以上のアームが各関節を介して互いに回
動可能とされた多関節作業機の軌跡制御装置に関する。
動可能とされた多関節作業機の軌跡制御装置に関する。
B.従来の技術 この種の多関節作業機をモデル化して示すと、第６図に
示すようになる。第１のアーム１は関節５を介して本体
に連結され、第２のアーム２は関節６を介して第１のア
ーム１と連結され、第３のアーム３は関節７を介して第
２のアーム２と連結されている。そして、各アームは図
示していない３本の油圧シリンダによりそれぞれ駆動さ
れる。
示すようになる。第１のアーム１は関節５を介して本体
に連結され、第２のアーム２は関節６を介して第１のア
ーム１と連結され、第３のアーム３は関節７を介して第
２のアーム２と連結されている。そして、各アームは図
示していない３本の油圧シリンダによりそれぞれ駆動さ
れる。
ところで、この種の作業機で軌跡制御を行う際、軌跡制
御対象部位である第３のアーム３の先端の水平方向（ｘ
方向）の速度Vxと垂直方向（ｙ方向）の速度Vyとを指令
値として入力するが、可動アームが３本以上あるので、
全てのアームを駆動させて軌跡制御することができな
い。そこで、従来は、例えば拘束条件として第３のアー
ム３を常時固定し、残余の第１および第２のアーム1,2
を駆動して軌跡制御が行われている。
御対象部位である第３のアーム３の先端の水平方向（ｘ
方向）の速度Vxと垂直方向（ｙ方向）の速度Vyとを指令
値として入力するが、可動アームが３本以上あるので、
全てのアームを駆動させて軌跡制御することができな
い。そこで、従来は、例えば拘束条件として第３のアー
ム３を常時固定し、残余の第１および第２のアーム1,2
を駆動して軌跡制御が行われている。
C.発明が解決しようとする問題点 しかしながら、このような従来装置では、第６図に示す
ように、駆動される２本のアーム1,2のうち一方がスロ
ークエンドに達すると、それ以上アーム先端を移動させ
ることができないので制御範囲が非常に狭かった。４本
以上のアームを有する多関節作業機でも同様である。
ように、駆動される２本のアーム1,2のうち一方がスロ
ークエンドに達すると、それ以上アーム先端を移動させ
ることができないので制御範囲が非常に狭かった。４本
以上のアームを有する多関節作業機でも同様である。
本発明の目的は、ある駆動アームがストロークエンドに
達すると残余のアームの２本を駆動して軌跡制御するこ
とにより上述した問題点を解消した多関節作業機の軌跡
制御装置を提供することにある。
達すると残余のアームの２本を駆動して軌跡制御するこ
とにより上述した問題点を解消した多関節作業機の軌跡
制御装置を提供することにある。
D.問題点を解決するための手段 本発明は、軌跡制御対象部位の所定方向における速度を
指令し、速度指令信号を出力する指令手段と、各アーム
の相対角度を検出して角度信号を出力する角度検出手段
と、少なくとも入力された角度信号に基づき、予め定め
られた２本のアームの組の中で２本のアームとも駆動可
能な組を判別する判別手段と、２本のアームとも駆動可
能と判別された組の中から予め定めた優先順位に従って
いずれかの組を選択し、前記速度指令信号および角度信
号に基づいて、選択された２本のアームの角速度指令信
号を演算する角速度演算手段と、その角速度指令信号に
より各アーム駆動用アクチュエータの駆動を制御する制
御部への入力信号を演算する入力信号演算手段とを具備
する。
指令し、速度指令信号を出力する指令手段と、各アーム
の相対角度を検出して角度信号を出力する角度検出手段
と、少なくとも入力された角度信号に基づき、予め定め
られた２本のアームの組の中で２本のアームとも駆動可
能な組を判別する判別手段と、２本のアームとも駆動可
能と判別された組の中から予め定めた優先順位に従って
いずれかの組を選択し、前記速度指令信号および角度信
号に基づいて、選択された２本のアームの角速度指令信
号を演算する角速度演算手段と、その角速度指令信号に
より各アーム駆動用アクチュエータの駆動を制御する制
御部への入力信号を演算する入力信号演算手段とを具備
する。
E.作用 角速度演算手段には、軌跡制御対象部位の所定方向の速
度指令信号が指令手段から入力されるとともに、角度検
出手段から各アームの相対角度を示す角度信号が入力さ
れる。そして、判別手段は、少なくとも入力された角度
信号に基づき、予め定められた２本のアームの組の中で
２本のアームとも駆動可能な組を判別し、前記角速度演
算手段は、２本のアームとも駆動可能と判別された組の
中から予め定めた優先順位に従っていずれかの組を選択
し、速度指令信号および角度信号に基づいて、選択され
た２本のアームの角速度指令信号を演算して出力する。
その角速度指令信号は入力信号演算手段で入力信号に変
換されて制御部に出力される。制御部はその入力信号に
相応してアクチュエータを駆動し、これにより２本のア
ームが駆動されて軌跡制御が行なわれる。
度指令信号が指令手段から入力されるとともに、角度検
出手段から各アームの相対角度を示す角度信号が入力さ
れる。そして、判別手段は、少なくとも入力された角度
信号に基づき、予め定められた２本のアームの組の中で
２本のアームとも駆動可能な組を判別し、前記角速度演
算手段は、２本のアームとも駆動可能と判別された組の
中から予め定めた優先順位に従っていずれかの組を選択
し、速度指令信号および角度信号に基づいて、選択され
た２本のアームの角速度指令信号を演算して出力する。
その角速度指令信号は入力信号演算手段で入力信号に変
換されて制御部に出力される。制御部はその入力信号に
相応してアクチュエータを駆動し、これにより２本のア
ームが駆動されて軌跡制御が行なわれる。
この際、まず第１優先順位の組の２本のアームが駆動さ
れ、その一方が例えばストロークエンドに達して駆動で
きなくなると、第２優先順位の組の２本のアームが駆動
され、以下、同様にして順次の組の２本のアームにより
軌跡制御が実行される。
れ、その一方が例えばストロークエンドに達して駆動で
きなくなると、第２優先順位の組の２本のアームが駆動
され、以下、同様にして順次の組の２本のアームにより
軌跡制御が実行される。
F.実施例 第１図〜第５図は本発明を３本のアームを有する多関節
作業機に適用した場合の一実施例を示す。
作業機に適用した場合の一実施例を示す。
第１図において、本体に固定された架台４にはピン５を
介して第１のアーム１が回動可能に連結されている。第
１のアーム１の他端にはピン６を介して第２のアーム２
が回動可能に連結され、第２のアーム２の他端にはピン
７を介して第３のアーム３が回動可能に連結されてい
る。第１のアーム１と本体との間には油圧シリンダ８が
介装されている。また第１のアーム１には油圧シリンダ
９の基端が連結され、そのピストンロッド91が、第１の
アーム１と第２のアーム２との間に取付けられたリンク
機構11に連結されている。また、油圧シリンダ10の基端
が第２のアーム２に連結され、そのピストンロッド101
が、第２のアーム２と第３のアーム３との間に取付けら
れたリンク機構12に連結されている。そして、各アーム
１〜３の相対角度を検出する角度センサ41−１〜41−３
がアーム１〜３の関節となるピン５〜７に関連して設け
られている。
介して第１のアーム１が回動可能に連結されている。第
１のアーム１の他端にはピン６を介して第２のアーム２
が回動可能に連結され、第２のアーム２の他端にはピン
７を介して第３のアーム３が回動可能に連結されてい
る。第１のアーム１と本体との間には油圧シリンダ８が
介装されている。また第１のアーム１には油圧シリンダ
９の基端が連結され、そのピストンロッド91が、第１の
アーム１と第２のアーム２との間に取付けられたリンク
機構11に連結されている。また、油圧シリンダ10の基端
が第２のアーム２に連結され、そのピストンロッド101
が、第２のアーム２と第３のアーム３との間に取付けら
れたリンク機構12に連結されている。そして、各アーム
１〜３の相対角度を検出する角度センサ41−１〜41−３
がアーム１〜３の関節となるピン５〜７に関連して設け
られている。
ここで第２図に基づいて各アーム１〜３の回動範囲につ
いて説明する。なお、以下では反時計方向回りを正の角
度としている。
いて説明する。なお、以下では反時計方向回りを正の角
度としている。
第１のアーム１は、基線l1を基準として時計方向に−θ
11、反時計方向にθ10回動でき、第２のアーム２は第１
のアーム１の軸線l2を基準として時計方向に−θ21、反
時計方向にθ20回動でき、第３のアーム３は第２のアー
ム２の軸線l3を基準として時計方向に−θ31、反時計方
向にθ30回動できる。すなわち、第１のアーム１が基線
l1となす角度をθ１、第２のアーム２が軸線l2となす角
度をθ２、第３のアーム３が軸線l3となす角度をθ３と
すれば、第１のアーム１はθ１＝θ10および−θ11でス
トロークエンドとなり、第２のアーム２はθ２＝θ20お
よびθ２＝−θ21でストロークエンドとなり、第３のア
ーム３はθ３＝θ30およびθ３＝−θ31でストロークエ
ンドとなる。
11、反時計方向にθ10回動でき、第２のアーム２は第１
のアーム１の軸線l2を基準として時計方向に−θ21、反
時計方向にθ20回動でき、第３のアーム３は第２のアー
ム２の軸線l3を基準として時計方向に−θ31、反時計方
向にθ30回動できる。すなわち、第１のアーム１が基線
l1となす角度をθ１、第２のアーム２が軸線l2となす角
度をθ２、第３のアーム３が軸線l3となす角度をθ３と
すれば、第１のアーム１はθ１＝θ10および−θ11でス
トロークエンドとなり、第２のアーム２はθ２＝θ20お
よびθ２＝−θ21でストロークエンドとなり、第３のア
ーム３はθ３＝θ30およびθ３＝−θ31でストロークエ
ンドとなる。
再び第１図において、各油圧シリンダ８〜10はそれぞれ
電磁比例弁51−１〜51〜３によりその速度および駆動方
向が制御され、電磁比例弁51−１〜51−３には油圧ポン
プ13から吐出油が供給されている。そして、電磁比例弁
51−１〜51−３は制御装置20からの入力信号i1〜i3によ
り制御される。制御装置20には速度指令装置30が接続さ
れ、その速度指令装置30から、軌跡制御対象部位である
第３のアーム３先端の水平方向（ｘ方向）および垂直方
向（ｙ方向）の速度を指令する速度指令信号Vx,Vyが供
給され、また、角度センサ41−１〜41−３も接続されア
ーム角度信号θ１〜θ３が入力されるようになってい
る。
電磁比例弁51−１〜51〜３によりその速度および駆動方
向が制御され、電磁比例弁51−１〜51−３には油圧ポン
プ13から吐出油が供給されている。そして、電磁比例弁
51−１〜51−３は制御装置20からの入力信号i1〜i3によ
り制御される。制御装置20には速度指令装置30が接続さ
れ、その速度指令装置30から、軌跡制御対象部位である
第３のアーム３先端の水平方向（ｘ方向）および垂直方
向（ｙ方向）の速度を指令する速度指令信号Vx,Vyが供
給され、また、角度センサ41−１〜41−３も接続されア
ーム角度信号θ１〜θ３が入力されるようになってい
る。
制御装置20について第３図に基づいて説明する。制御装
置20は、角速度演算部21と、サーボ制御部22と、リンク
補正部23と、電流演算部24とが順次に接続されて構成さ
れている。
置20は、角速度演算部21と、サーボ制御部22と、リンク
補正部23と、電流演算部24とが順次に接続されて構成さ
れている。
角速度演算部21は例えばマイクロコンピュータで構成さ
れ、入力される角度信号θ１〜θ３および速度指令信号
Vx,Vyから後述する角速度演算信号1a,2a,3aを演
算して出力するように構成されている。周知の通り３本
のアームにより軌跡制御する場合、ある拘束条件を定め
て２本のアームだけを駆動するが、本発明では、２本一
組の駆動アームi,j（ｉ＝１〜3,j＝１〜３）の各組に優
先順位を予め定め、その優先順位に従って逐次２本の駆
動アームi,jが決定され、それに基づいて上述の角速度
演算信号1a〜3aが演算されて出力される。
れ、入力される角度信号θ１〜θ３および速度指令信号
Vx,Vyから後述する角速度演算信号1a,2a,3aを演
算して出力するように構成されている。周知の通り３本
のアームにより軌跡制御する場合、ある拘束条件を定め
て２本のアームだけを駆動するが、本発明では、２本一
組の駆動アームi,j（ｉ＝１〜3,j＝１〜３）の各組に優
先順位を予め定め、その優先順位に従って逐次２本の駆
動アームi,jが決定され、それに基づいて上述の角速度
演算信号1a〜3aが演算されて出力される。
すなわち、３本アームの場合、第１のアーム１と第２の
アーム２の第１組、第１のアーム１と第３のアーム３の
第２組、第２のアーム２と第３のアーム３の第３組の３
つの組があるが、この実施例では、第２のアーム２と第
３のアーム３の第３組をまず優先させて軌跡制御し、そ
のうちのいずれかのアームがストロークエンドに達した
ら、第１のアーム１と第２のアーム２の第１組により軌
跡制御し、そのいずれかがストロークエンドに達したら
第１のアーム１と第３のアーム３の第２組により軌跡制
御するようにしている。
アーム２の第１組、第１のアーム１と第３のアーム３の
第２組、第２のアーム２と第３のアーム３の第３組の３
つの組があるが、この実施例では、第２のアーム２と第
３のアーム３の第３組をまず優先させて軌跡制御し、そ
のうちのいずれかのアームがストロークエンドに達した
ら、第１のアーム１と第２のアーム２の第１組により軌
跡制御し、そのいずれかがストロークエンドに達したら
第１のアーム１と第３のアーム３の第２組により軌跡制
御するようにしている。
次に、アーム１〜３の任意の姿勢においてどの組を用い
て軌跡制御するかについて第４図のフローチャートに従
って以下に説明する。
て軌跡制御するかについて第４図のフローチャートに従
って以下に説明する。
まず、手順P1において、角度信号θ１〜θ３および速度
指令信号Vx,Vyを読込み、手順P2において、各組におけ
る角速度演算信号（21,31），（12,22），（
13,33）を以下の（１）式〜（３）式により求める。
但し、次式では各アームの長さをそれぞれl1,l2,l3と
し、12,13は第１のアーム１、21,22は第２のア
ーム２、31,33は第３のアーム３の各組における角
速度演算信号を示す。
指令信号Vx,Vyを読込み、手順P2において、各組におけ
る角速度演算信号（21,31），（12,22），（
13,33）を以下の（１）式〜（３）式により求める。
但し、次式では各アームの長さをそれぞれl1,l2,l3と
し、12,13は第１のアーム１、21,22は第２のア
ーム２、31,33は第３のアーム３の各組における角
速度演算信号を示す。
第３組（21,31） そして、次の条件イ〜ニにおいては、イ手順P3〜P6、ロ
手順P3〜P5,P8、ハ手順P3,P7,P5,R6、ニ手順P3,P7,P5,P
8をそれぞれ経て手順P24に進み、角速度演算信号1a〜
3aとして1a＝0,2a＝21,3a＝31が出力され
る。
手順P3〜P5,P8、ハ手順P3,P7,P5,R6、ニ手順P3,P7,P5,P
8をそれぞれ経て手順P24に進み、角速度演算信号1a〜
3aとして1a＝0,2a＝21,3a＝31が出力され
る。
イ31＞０、θ３＜０、21＞０、θ２＜0: 第３のアーム３および第２のアーム２が反時計方向に動
けるとき。
けるとき。
ロ31＞０、θ３＜０、21＜０、θ２≠−θ21: 第３のアーム３が反時計方向に動ける場合で、かつ、第
２のアーム２が時計方向に動けるとき。
２のアーム２が時計方向に動けるとき。
ハ31＜０、θ３≠−θ31、21＞０、θ２＜0: 第３のアーム３が時計方向に動ける場合で、かつ、第２
のアーム２が反時計方向に動けるとき。
のアーム２が反時計方向に動けるとき。
ニ31＜０、θ３≠−θ31、21＜０、θ２≠−θ21: 第３のアーム３および第２のアーム２が時計方向に動け
るとき。
るとき。
また、これら各条件がいずれも満足しない場合には、次
の条件ホ〜チにおいて、ホ手順P9〜P12、ヘ手順P9〜P1
1,P14、ト手順P9,P13,P11,P12、チ手順P9,P13,P11,P14
をそれぞれ経て手順P23に進み、角速度演算信号1a〜
3aとして、1a＝12,2a＝22,3a＝０を出力す
る。
の条件ホ〜チにおいて、ホ手順P9〜P12、ヘ手順P9〜P1
1,P14、ト手順P9,P13,P11,P12、チ手順P9,P13,P11,P14
をそれぞれ経て手順P23に進み、角速度演算信号1a〜
3aとして、1a＝12,2a＝22,3a＝０を出力す
る。
ホ22＞０、θ２＜０、12＞０、θ１≠θ10: 第２のアーム２および第１のアーム１が反時計方向に動
けるとき。
けるとき。
ヘ22＞０、θ２＜０、12＜０、θ１≠−θ11: 第２のアーム２が反時計方向に動ける場合で、かつ、第
１のアーム１が時計方向に動けるとき。
１のアーム１が時計方向に動けるとき。
ト22＜０、θ２≠−θ20、12＞０、θ１≠θ10: 第２のアーム２が時計方向に動ける場合で、かつ、第１
のアーム１が反時計方向に動けるとき。
のアーム１が反時計方向に動けるとき。
チ22＜０、θ２≠−θ21、12＜０、θ１≠−θ11: 第２のアーム２および第１のアーム１が時計方向に動け
る場合。
る場合。
更に、これらの各条件が満足しない場合には、次の条件
リ〜ヲにおいて、リ手順P15〜P18、ヌ手順P15〜P17,P2
0、ル手順P15,P19,P17,P18、ヲ手順P15,P19,P17,P20を
それぞれ経て手順P22に進み、角速度演算信号θ1a〜θ
3aとして1a＝13,2a＝0,3a＝31が出力され
る。
リ〜ヲにおいて、リ手順P15〜P18、ヌ手順P15〜P17,P2
0、ル手順P15,P19,P17,P18、ヲ手順P15,P19,P17,P20を
それぞれ経て手順P22に進み、角速度演算信号θ1a〜θ
3aとして1a＝13,2a＝0,3a＝31が出力され
る。
リ33＞０、θ３＜０、13＞０、θ１≠θ10: 第３のアーム３および第１のアーム１が反時計方向に動
けるとき。
けるとき。
ヌ33＞０、θ３＜０、13＜０、θ１≠−θ11: 第３のアーム３が反時計方向に動ける場合で、かつ、第
１のアーム１が時計方向に動けるとき。
１のアーム１が時計方向に動けるとき。
ル33＜０、θ３≠−θ31、13＞０、θ１≠θ10: 第３のアーム３が時計方向に動ける場合で、かつ、第１
のアーム１が反時計方向に動けるとき。
のアーム１が反時計方向に動けるとき。
ヲ33＜０、θ３≠−θ31、13＜０、θ１≠−θ11: 第３のアーム３および第１のアーム１が時計方向に動け
るとき。
るとき。
これらいずれもの条件が満足されない場合には、手順P2
1において各角速度演算信号1a,2a,3aは零とされ
る。なお、θ３＜0,θ２＜０の範囲でのみ動作するよう
にしたが、θ３＞0,θ２＞０の範囲でも条件を適切に定
めれば同様に制御できる。
1において各角速度演算信号1a,2a,3aは零とされ
る。なお、θ３＜0,θ２＜０の範囲でのみ動作するよう
にしたが、θ３＞0,θ２＞０の範囲でも条件を適切に定
めれば同様に制御できる。
再び第３図において、サーボ制御部22は、制御開始時の
各アーム角度θi0を初期値として角速度演算信号iaを
積分して位置指令信号θibを出力する積分器221−１〜2
21−３と、積分器221−１〜221−３の出力θibと実際の
アーム角度θｉとの偏差Δθibをとる偏差発生器222−
１〜222−３と、偏差Δθibに所定のゲインをかけるゲ
イン設定器223−１〜223−３と、ゲイン設定器223−１
〜223−３の出力Δθicに角速度演算信号iaを加え合
わせて角速度指令信号idを得る加算器224−１〜224−
３とを有する。
各アーム角度θi0を初期値として角速度演算信号iaを
積分して位置指令信号θibを出力する積分器221−１〜2
21−３と、積分器221−１〜221−３の出力θibと実際の
アーム角度θｉとの偏差Δθibをとる偏差発生器222−
１〜222−３と、偏差Δθibに所定のゲインをかけるゲ
イン設定器223−１〜223−３と、ゲイン設定器223−１
〜223−３の出力Δθicに角速度演算信号iaを加え合
わせて角速度指令信号idを得る加算器224−１〜224−
３とを有する。
そして、加算器224−１〜224−３に各リンク補正部23−
１〜23−３が後続している。このリンク補正部23−１〜
23−３にはアームの位置信号θibが入力され、入力され
る角速度指令信号idにアーム角度に応じたゲインを与
えてシリンダ速度信号ｉを出力する。このリンク補正
部23−１〜23−３には電流演算部24−１〜24−３が後続
し、それらには電磁比例弁51−１〜51〜３が後続してい
る。電磁比例弁51−１〜51−３はその入力信号iiにより
油圧ポンプ13からの吐出油の方向および流量を制御す
る。
１〜23−３が後続している。このリンク補正部23−１〜
23−３にはアームの位置信号θibが入力され、入力され
る角速度指令信号idにアーム角度に応じたゲインを与
えてシリンダ速度信号ｉを出力する。このリンク補正
部23−１〜23−３には電流演算部24−１〜24−３が後続
し、それらには電磁比例弁51−１〜51〜３が後続してい
る。電磁比例弁51−１〜51−３はその入力信号iiにより
油圧ポンプ13からの吐出油の方向および流量を制御す
る。
本実施例の各構成要素のうち、油圧シリンダ８〜10がア
クチュエータ80を、電磁比例弁51−１〜51−３が制御部
50を、速度指令装置30が指令手段を、角度センサ41−１
〜41−３が角度検出手段40を、角速度演算部21,サーボ
制御部22が角速度演算手段60を、リンク補正部23と電流
演算部24とが入力信号演算手段70をそれぞれ構成してい
る。そして、軌跡制御対象部位が第３のアーム３の最先
端である。
クチュエータ80を、電磁比例弁51−１〜51−３が制御部
50を、速度指令装置30が指令手段を、角度センサ41−１
〜41−３が角度検出手段40を、角速度演算部21,サーボ
制御部22が角速度演算手段60を、リンク補正部23と電流
演算部24とが入力信号演算手段70をそれぞれ構成してい
る。そして、軌跡制御対象部位が第３のアーム３の最先
端である。
このように構成された本実施例の作用について説明す
る。
る。
速度指令装置30から、アーム３先端の水平方向（ｘ方
向）および垂直方向（ｙ方向）の速度指令信号Vx,Vyが
角速度演算部21に入力される。更に、角度センサ41−１
〜41−３から各アームの相対角度信号θ１〜θ３が角速
度演算部21に入力される。
向）および垂直方向（ｙ方向）の速度指令信号Vx,Vyが
角速度演算部21に入力される。更に、角度センサ41−１
〜41−３から各アームの相対角度信号θ１〜θ３が角速
度演算部21に入力される。
このような各種信号が入力された角速度演算部21は、上
述した第４図に示す手順によって角速度演算信号1a,
3aを出力する。なお、ここで出力値が零でない一対の
角速度演算信号をia,ja（ｉ＝１〜3,j＝１〜３）と
する。
述した第４図に示す手順によって角速度演算信号1a,
3aを出力する。なお、ここで出力値が零でない一対の
角速度演算信号をia,ja（ｉ＝１〜3,j＝１〜３）と
する。
一対の角速度演算信号ia,jaは積分器221−i,221−
ｊで積分されて位置指令信号θib,θjbとなる。その位
置指令信号θib,θjbと実際の角度θi,θｊとの偏差が
偏差器222−i,222−ｊでとられて偏差Δθib,Δθjbが
得られる。その偏差Δθib,Δθjbに対してゲイン設定
器223−i,223−ｊで所定のゲインが与えられてΔic,
Δjcとされ、加算器224−i,224−ｊで角速度演算信号
ia,jaとΔic,Δjcとが加算補正され、これによ
り角速度指令信号id,jdが得られる。この角速度指
令信号id,θjdはリンク補正部23−i,23−ｊに入力さ
れて積分器221−i,221−ｊの位置指令信号θib,θjb、
すなわちアーム角度に応じた補正演算が実行されてシリ
ンダ速度信号i,ｊが得られる。この補正演算は、角
速度指令信号id,jdに所定のゲインを与えることに
より行われる。シリンダ速度信号i,ｊは電流演算部
24−i,24−ｊに入力されて電磁比例弁51−i,51−ｊの入
力信号ii,ijに変換される。そして、電磁比例弁51−ｉ
〜51−ｊがその入力信号ii,ijによって制御され、これ
により油圧シリンダ８〜10が制御されてアームi,jが駆
動される。
ｊで積分されて位置指令信号θib,θjbとなる。その位
置指令信号θib,θjbと実際の角度θi,θｊとの偏差が
偏差器222−i,222−ｊでとられて偏差Δθib,Δθjbが
得られる。その偏差Δθib,Δθjbに対してゲイン設定
器223−i,223−ｊで所定のゲインが与えられてΔic,
Δjcとされ、加算器224−i,224−ｊで角速度演算信号
ia,jaとΔic,Δjcとが加算補正され、これによ
り角速度指令信号id,jdが得られる。この角速度指
令信号id,θjdはリンク補正部23−i,23−ｊに入力さ
れて積分器221−i,221−ｊの位置指令信号θib,θjb、
すなわちアーム角度に応じた補正演算が実行されてシリ
ンダ速度信号i,ｊが得られる。この補正演算は、角
速度指令信号id,jdに所定のゲインを与えることに
より行われる。シリンダ速度信号i,ｊは電流演算部
24−i,24−ｊに入力されて電磁比例弁51−i,51−ｊの入
力信号ii,ijに変換される。そして、電磁比例弁51−ｉ
〜51−ｊがその入力信号ii,ijによって制御され、これ
により油圧シリンダ８〜10が制御されてアームi,jが駆
動される。
第５図（ａ），（ｂ）に第３のアーム３先端をP1−P2に
沿って水平押出し（Vx＞０、Vy＝０）とした場合の各ア
ームの動きを示す。なお、第１のアーム１の回動支点を
O1、第２のアーム２の回動支点をO2、第３のアーム３の
回動支点をO3、第３のアーム３先端をO4で示し、各部の
初期位置をそれぞれO11,O21,O31,O41で示す。
沿って水平押出し（Vx＞０、Vy＝０）とした場合の各ア
ームの動きを示す。なお、第１のアーム１の回動支点を
O1、第２のアーム２の回動支点をO2、第３のアーム３の
回動支点をO3、第３のアーム３先端をO4で示し、各部の
初期位置をそれぞれO11,O21,O31,O41で示す。
第５図（ａ）は第２のアーム２および第３のアーム３を
駆動して軌跡制御した場合を示し、アーム３先端O4がO
41→O42→O43→O44と移動するとき、各アーム2,3の連結
点はO31→O32→O33→O34と移動し、第３のアーム３がス
トロークエンドに達した状態、すなわち第２および第３
のアーム2,3の第３組ではもはや軌跡制御ができなくな
った状態を示している。この実施例では、第３組に次い
で第１のアーム１および第２のアーム２の第１組に次の
優先順位が与えられており、第３のアーム３がストロー
クエンドに達しても引き続き第１および第２のアーム1,
2により軌跡制御が行なわれる。第５図（ｂ）はその場
合を示し、アーム３先端O4がO44→O45→O46と移動する
とき、各アーム1,2の連結点はO21→O22→O23、アーム2,
3の連結点はO34→O35→O36と移動し、第２のアーム２が
ストロークエンドに達した状態、すなわち各アーム１〜
３が一直線になった状態を示している。
駆動して軌跡制御した場合を示し、アーム３先端O4がO
41→O42→O43→O44と移動するとき、各アーム2,3の連結
点はO31→O32→O33→O34と移動し、第３のアーム３がス
トロークエンドに達した状態、すなわち第２および第３
のアーム2,3の第３組ではもはや軌跡制御ができなくな
った状態を示している。この実施例では、第３組に次い
で第１のアーム１および第２のアーム２の第１組に次の
優先順位が与えられており、第３のアーム３がストロー
クエンドに達しても引き続き第１および第２のアーム1,
2により軌跡制御が行なわれる。第５図（ｂ）はその場
合を示し、アーム３先端O4がO44→O45→O46と移動する
とき、各アーム1,2の連結点はO21→O22→O23、アーム2,
3の連結点はO34→O35→O36と移動し、第２のアーム２が
ストロークエンドに達した状態、すなわち各アーム１〜
３が一直線になった状態を示している。
このように本実施例では、第３のアーム３がストローク
エンドに達しても引き続き第１および第２のアーム1,2
により軌跡制御が行なわれるので、従来のように特定の
２本のアームのみにより軌跡制御する場合に比べて、第
５図（ａ），（ｂ）におけるアーム３先端軌跡O44→O45
→O46の分だけ作業範囲を広くできる。
エンドに達しても引き続き第１および第２のアーム1,2
により軌跡制御が行なわれるので、従来のように特定の
２本のアームのみにより軌跡制御する場合に比べて、第
５図（ａ），（ｂ）におけるアーム３先端軌跡O44→O45
→O46の分だけ作業範囲を広くできる。
以上では３本アームの作業機について説明したが４本以
上のｎ本のアームを有する作業機にも同様にこの発明を
適用できる。この場合、一対のアームは できるので、それらの組間に同様に優先順位を与えて上
述のように角速度演算信号ia,jaを求めればよい。
また、アクチュエータを油圧シリンダとしたが空圧シリ
ンダでもよく、油圧，空圧または電動機の回転力を直線
運動に変換してアームを駆動するようなものでもよい。
上のｎ本のアームを有する作業機にも同様にこの発明を
適用できる。この場合、一対のアームは できるので、それらの組間に同様に優先順位を与えて上
述のように角速度演算信号ia,jaを求めればよい。
また、アクチュエータを油圧シリンダとしたが空圧シリ
ンダでもよく、油圧，空圧または電動機の回転力を直線
運動に変換してアームを駆動するようなものでもよい。
G.発明の効果 本発明によれば、軌跡制御のために一対で駆動される２
本のアームの組の中から、予め定めた優先順位に従っ
て、少なくとも入力された角度信号に基づき２本のアー
ムとも駆動可能な組を選択するようにしたので、従来の
ように特定の２本のアームのみにより軌跡制御する場合
に比べて作業範囲を広くすることができる。
本のアームの組の中から、予め定めた優先順位に従っ
て、少なくとも入力された角度信号に基づき２本のアー
ムとも駆動可能な組を選択するようにしたので、従来の
ように特定の２本のアームのみにより軌跡制御する場合
に比べて作業範囲を広くすることができる。
第１図は本発明が適用される多関節作業機の一例と本発
明の一実施例とを示す図、第２図はその多関節作業機の
各アームの回動範囲（ストロークエンド）を示す図、第
３図は第１図の制御装置の詳細ブロック図、第４図はそ
の角速度演算部での処理手順例を示すフローチャート、
第５図（ａ），（ｂ）は軌跡制御時に選択されたアーム
とその動きを説明する図、第６図は３本アームによる従
来の作業機のモデル図である。 １〜3:アーム、８〜10:油圧シリンダ 30:指令手段、40:角度検出手段 50:制御部、60:角速度演算手段 70:入力信号演算手段 80:アクチュエータ
明の一実施例とを示す図、第２図はその多関節作業機の
各アームの回動範囲（ストロークエンド）を示す図、第
３図は第１図の制御装置の詳細ブロック図、第４図はそ
の角速度演算部での処理手順例を示すフローチャート、
第５図（ａ），（ｂ）は軌跡制御時に選択されたアーム
とその動きを説明する図、第６図は３本アームによる従
来の作業機のモデル図である。 １〜3:アーム、８〜10:油圧シリンダ 30:指令手段、40:角度検出手段 50:制御部、60:角速度演算手段 70:入力信号演算手段 80:アクチュエータ
フロントページの続き (72)発明者 本間 和男 茨城県土浦市神立町502番地 株式会社日 立製作所機械研究所内 (56)参考文献 特開 昭60−44278（ＪＰ，Ａ) 特開 昭59−81081（ＪＰ，Ａ) 特開 昭59−149510（ＪＰ，Ａ) 特開 昭61−25210（ＪＰ，Ａ)
Claims (2)
- 【請求項１】関節を介して回動可能に連結された３本以
上のアームと、それらのアームを駆動するアクチュエー
タと、入力信号に応じてそのアクチュエータによるアー
ムの移動量および移動方向を制御する制御部とを有する
多関節作業機の軌跡制御装置において、 軌跡制御対象部位の所定方向における速度を指令し、速
度指令信号を出力する指令手段と、 各アームの相対角度を検出して角度信号を出力する角度
検出手段と、 少なくとも入力された角度信号に基づき、予め定められ
た２本のアームの組の中で２本のアームとも駆動可能な
組を判別する判別手段と、 ２本のアームとも駆動可能と判別された組の中から予め
定めた優先順位に従っていずれかの組を選択し、前記速
度指令信号および角度信号に基づいて、選択された組の
２本のアームの角度速度指令信号を演算する角速度演算
手段と、 その角速度指令信号により前記制御部への入力信号を演
算する入力信号演算手段と、を具備したことを特徴とす
る多関節作業機の軌跡制御装置。 - 【請求項２】特許請求の範囲第１項に記載の装置におい
て、前記アクチュエータを油圧シリンダとし、前記制御
部を電磁比例弁としたことを特徴とする多関節作業機の
軌跡制御装置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP61083357A JPH0766287B2 (ja) | 1986-04-11 | 1986-04-11 | 多関節作業機の軌跡制御装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP61083357A JPH0766287B2 (ja) | 1986-04-11 | 1986-04-11 | 多関節作業機の軌跡制御装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JPS62241009A JPS62241009A (ja) | 1987-10-21 |
| JPH0766287B2 true JPH0766287B2 (ja) | 1995-07-19 |
Family
ID=13800181
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP61083357A Expired - Lifetime JPH0766287B2 (ja) | 1986-04-11 | 1986-04-11 | 多関節作業機の軌跡制御装置 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JPH0766287B2 (ja) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107310649A (zh) * | 2016-04-26 | 2017-11-03 | 广西大学 | 一种爬杆攀壁两用侦查机器人攀壁机构 |
Family Cites Families (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPS5488605A (en) * | 1977-12-23 | 1979-07-13 | Komatsu Mfg Co Ltd | Device for controlling position of working machine |
| JPS5981081A (ja) * | 1982-10-29 | 1984-05-10 | 株式会社東芝 | 多関節アームの制御方法 |
| JPS59149510A (ja) * | 1983-02-14 | 1984-08-27 | Toshiba Corp | 工業用ロボツトの制御装置 |
| JPS6029288A (ja) * | 1983-07-27 | 1985-02-14 | 株式会社日立製作所 | ロボツト装置 |
| JPS6044278A (ja) * | 1983-08-16 | 1985-03-09 | 小川 康男 | 位置決め方法 |
| JPS6123212A (ja) * | 1984-07-11 | 1986-01-31 | Hitachi Ltd | 多関節構造機械の制御装置 |
| JPS6125210A (ja) * | 1984-07-16 | 1986-02-04 | Hitachi Ltd | 工業用関節ロボツト |
-
1986
- 1986-04-11 JP JP61083357A patent/JPH0766287B2/ja not_active Expired - Lifetime
Also Published As
| Publication number | Publication date |
|---|---|
| JPS62241009A (ja) | 1987-10-21 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US9002519B2 (en) | Robot control method, robot control device, and robot control system | |
| EP0128355B1 (en) | Dynamic control for manipulator | |
| JP5743495B2 (ja) | ロボット制御装置 | |
| US4904152A (en) | Industrial robot | |
| US5357424A (en) | Dynamic model parameter identification system | |
| CN115700423A (zh) | 机架驱动系统和马达控制系统 | |
| JP2020044590A (ja) | ロボット装置 | |
| JPH0766287B2 (ja) | 多関節作業機の軌跡制御装置 | |
| JPH05345291A (ja) | ロボットの動作範囲制限方式 | |
| CN112512758B (zh) | 挠曲量推断装置、机器人控制装置以及挠曲量推断方法 | |
| EP0251514A2 (en) | Apparatus for controlling industrial multijoint arm robot | |
| JP2000303492A (ja) | 建設機械のフロント制御装置 | |
| RU2725930C1 (ru) | Комплекс копирующего управления манипуляторами антропоморфного робота | |
| JPS6324402A (ja) | ロボツトの軌跡制御装置 | |
| JPH04119203A (ja) | アクチュエータの駆動制御装置 | |
| WO2023238871A1 (ja) | 撓み量推定装置、ロボット制御装置、及び撓み量推定方法 | |
| JPH07113848B2 (ja) | 多関節作業機の軌跡制御装置 | |
| JP2798518B2 (ja) | マニピュレータの制御装置 | |
| JPH0343171A (ja) | 多関節ロボット制御装置 | |
| JP2000084878A (ja) | ロボットの制御装置 | |
| JPH0985658A (ja) | ロボットの制御システム | |
| JPH09285984A (ja) | 遠隔操作型ロボットの制御方法および遠隔操作型軸挿入装置 | |
| JPS63313207A (ja) | ２腕協調ロボットシステムの力制御方式 | |
| JP2504433B2 (ja) | 柔軟マニピユレ−タの制御方法 | |
| JPH0791844B2 (ja) | 作業機の制御装置 |