JPH07113848B2 - 多関節作業機の軌跡制御装置 - Google Patents
多関節作業機の軌跡制御装置Info
- Publication number
- JPH07113848B2 JPH07113848B2 JP61083358A JP8335886A JPH07113848B2 JP H07113848 B2 JPH07113848 B2 JP H07113848B2 JP 61083358 A JP61083358 A JP 61083358A JP 8335886 A JP8335886 A JP 8335886A JP H07113848 B2 JPH07113848 B2 JP H07113848B2
- Authority
- JP
- Japan
- Prior art keywords
- speed
- arms
- arm
- mode
- angular velocity
- 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
- Numerical Control (AREA)
- Manipulator (AREA)
Description
【発明の詳細な説明】 A.産業上の利用分野 本発明は、３本以上のアームが各関節を介して互いに回
動可能とされた多関節作業機の軌跡制御装置に関する。
動可能とされた多関節作業機の軌跡制御装置に関する。
B.従来の技術 この種の多関節作業機をモデル化して示すと、第４図に
示すようになる。第１のアーム１は関節７を介して本体
に連結され、第２のアーム２は関節８を介して第１のア
ーム１と連結され、第３のアーム３は関節９を介して第
２のアーム２と連結されている。そして、各アーム１〜
３は例えば図示の如く設けられた第１〜第３の油圧シリ
ンダ４〜６によりそれぞれ回動される。
示すようになる。第１のアーム１は関節７を介して本体
に連結され、第２のアーム２は関節８を介して第１のア
ーム１と連結され、第３のアーム３は関節９を介して第
２のアーム２と連結されている。そして、各アーム１〜
３は例えば図示の如く設けられた第１〜第３の油圧シリ
ンダ４〜６によりそれぞれ回動される。
ところで、この主の作業機で軌跡制御を行う際、軌跡制
御対象部位である第３のアーム３の先端の水平方向（ｘ
方向）の速度Vxと垂直方向（ｙ方向）の速度Vyとを指令
値として入力するが、可動アームが３本以上あるので、
全てのアームを駆動させて軌跡制御することができな
い。そこで、従来は、例えば拘束条件として第３のアー
ム３を固定し、残余の第１および第２のアーム1,2を駆
動して軌跡制御が行われている。
御対象部位である第３のアーム３の先端の水平方向（ｘ
方向）の速度Vxと垂直方向（ｙ方向）の速度Vyとを指令
値として入力するが、可動アームが３本以上あるので、
全てのアームを駆動させて軌跡制御することができな
い。そこで、従来は、例えば拘束条件として第３のアー
ム３を固定し、残余の第１および第２のアーム1,2を駆
動して軌跡制御が行われている。
C.発明が解決しようとする問題点 上述した従来の軌跡制御では次のような問題がある。
第４図において、地面（本体）と第１のアーム１の
なす角度をθ1、第１のアーム１の延長線と第２のアー
ム２とがなす角度をθ2、第２のアーム２の延長線と第
３のアーム３のなす角度をθ3、第１〜第３のアーム１
〜３の長さをそれぞれl1〜l3、第１〜第３の油圧シリン
ダ４〜６の全長をZ1〜Z3、第１の油圧シリンダ４の両固
定端と関節７までの距離をl1′，l1″、同様に第２、第
３の油圧シリンダ5,6についてもl2′，l2″、l3′，
l3″、関節７と第１の油圧シリンダ４の地面側固定端と
を結ぶ線と地面との角度をλとする。
なす角度をθ1、第１のアーム１の延長線と第２のアー
ム２とがなす角度をθ2、第２のアーム２の延長線と第
３のアーム３のなす角度をθ3、第１〜第３のアーム１
〜３の長さをそれぞれl1〜l3、第１〜第３の油圧シリン
ダ４〜６の全長をZ1〜Z3、第１の油圧シリンダ４の両固
定端と関節７までの距離をl1′，l1″、同様に第２、第
３の油圧シリンダ5,6についてもl2′，l2″、l3′，
l3″、関節７と第１の油圧シリンダ４の地面側固定端と
を結ぶ線と地面との角度をλとする。
第３のアーム３の先端のｘ座標,y座標は、 ｘ＝l1cosθ1−l2cos（θ1＋θ2）−l3cos（θ1＋θ2＋
θ3） …（１） ｙ＝l1sinθ1−l2sin（θ1＋θ2）−l3sin（θ1＋θ2＋
θ3） …（２） と表わされる。また、アーム角度θi（ｉ＝１〜３）に
対する、各シリンダ速度ｉと各アーム角速度ｉの比
i/ｉは、 i/ｉ＝l1′・l1″sin（θ1＋λ）× ｛l1′2＋l1″2−２l1′・l1″cos（θ1＋λ）｝-1/2…
（３）2 ／2＝l2′＋l2″sinθ2× （l2′2＋l2″2−２l2′・l2″cosθ2）-1/2 …（４）3 ／3＝l3′l3sinθ3× （l3′2＋l3″2−２l3′・l3″cosθ3）-1/2 …（５） となり、各アーム１〜３についてθｉとi/ｉとの関
係を示せば第５図（ａ）〜（ｃ）のようになる。この図
から、油圧シリンダ４〜６への流量を一定（ｉ一定）
としても、各アームの各速度ｉはアーム角度θｉによ
って変換しアーム１〜３の移動角度が大きくなったり小
さくなったりしてしまうことがわかる。
θ3） …（１） ｙ＝l1sinθ1−l2sin（θ1＋θ2）−l3sin（θ1＋θ2＋
θ3） …（２） と表わされる。また、アーム角度θi（ｉ＝１〜３）に
対する、各シリンダ速度ｉと各アーム角速度ｉの比
i/ｉは、 i/ｉ＝l1′・l1″sin（θ1＋λ）× ｛l1′2＋l1″2−２l1′・l1″cos（θ1＋λ）｝-1/2…
（３）2 ／2＝l2′＋l2″sinθ2× （l2′2＋l2″2−２l2′・l2″cosθ2）-1/2 …（４）3 ／3＝l3′l3sinθ3× （l3′2＋l3″2−２l3′・l3″cosθ3）-1/2 …（５） となり、各アーム１〜３についてθｉとi/ｉとの関
係を示せば第５図（ａ）〜（ｃ）のようになる。この図
から、油圧シリンダ４〜６への流量を一定（ｉ一定）
としても、各アームの各速度ｉはアーム角度θｉによ
って変換しアーム１〜３の移動角度が大きくなったり小
さくなったりしてしまうことがわかる。
ここで、多関節作業機を例えば塗装作業に用いる場合に
ついて考えると、下塗り時には、軌跡精度ではなく、よ
り速い速度が要求され、仕上げ塗装時には、速度ではな
くより高い軌跡精度が要求される。
ついて考えると、下塗り時には、軌跡精度ではなく、よ
り速い速度が要求され、仕上げ塗装時には、速度ではな
くより高い軌跡精度が要求される。
しかしながら、上述した従来の軌跡制御では、３本アー
ムの場合いずれかひとつのアームが固定され、例えば第
３のアーム３が固定される場合には、第５図（ａ），
（ｂ）に示す特性が重ね合わされて得られる軌跡精度と
速度とにより画一的に第１および第２のアーム1,2が駆
動制御されるので、所望の精度，速度が得られないこと
があった。
ムの場合いずれかひとつのアームが固定され、例えば第
３のアーム３が固定される場合には、第５図（ａ），
（ｂ）に示す特性が重ね合わされて得られる軌跡精度と
速度とにより画一的に第１および第２のアーム1,2が駆
動制御されるので、所望の精度，速度が得られないこと
があった。
また、上述の従来の軌跡制御では、駆動される一方
の油圧シリンダがストロークエンドに達すると、それ以
上アーム先端を移動させることができないので、制御範
囲が非常に狭かった。この欠点は、先端アーム３の対地
角φ（第４図）が一定となるような拘束条件を用いれ
ば、上述した３本のアームを有する多関節作業機の場合
には解決できる。しかし、それぞれ回動可能な４本以上
のアームをもつ多関節作業機においては、特定の２本の
アームまたは先端のアームがストロークエンドに達する
とアーム先端をそれ以上移動できず、同様に制御範囲が
狭いという問題があった。
の油圧シリンダがストロークエンドに達すると、それ以
上アーム先端を移動させることができないので、制御範
囲が非常に狭かった。この欠点は、先端アーム３の対地
角φ（第４図）が一定となるような拘束条件を用いれ
ば、上述した３本のアームを有する多関節作業機の場合
には解決できる。しかし、それぞれ回動可能な４本以上
のアームをもつ多関節作業機においては、特定の２本の
アームまたは先端のアームがストロークエンドに達する
とアーム先端をそれ以上移動できず、同様に制御範囲が
狭いという問題があった。
本発明の目的は、同時に駆動する２つのアクチュエータ
の組合せをアームの角度に応じて変えることにより上述
した問題点を解決した多関節作業機の軌跡制御装置を提
供することにある。
の組合せをアームの角度に応じて変えることにより上述
した問題点を解決した多関節作業機の軌跡制御装置を提
供することにある。
D.問題点を解決するための手段 本発明は、関節を介して回動可能に連結された３本以上
のアームと、それらのアームを駆動するアクチュエータ
と、そのアクチュエータによるアームの移動量を制御す
る制御部とを有する多関節作業機の軌跡制御装置に適用
される。
のアームと、それらのアームを駆動するアクチュエータ
と、そのアクチュエータによるアームの移動量を制御す
る制御部とを有する多関節作業機の軌跡制御装置に適用
される。
そして、軌跡制御対象部位の所定方向における速度を指
令し、速度指令信号を出力する指令手段と、各アームの
相対角度を検出して角度信号を出力する角度検出手段
と、検出された角度から各アームの回動角速度と各アク
チュエータの速度との速度比を演算し、これらの速度比
に基づき駆動すべき２本のアームを選択し、速度指令信
号および角度信号に基づいて選択された２本の駆動アー
ムの角速度指令信号を演算、出力する角速度演算手段
と、その角速度指令信号により制御部への入力信号を演
算する入力信号演算手段とを具備することにより、上述
の目的が達成される。
令し、速度指令信号を出力する指令手段と、各アームの
相対角度を検出して角度信号を出力する角度検出手段
と、検出された角度から各アームの回動角速度と各アク
チュエータの速度との速度比を演算し、これらの速度比
に基づき駆動すべき２本のアームを選択し、速度指令信
号および角度信号に基づいて選択された２本の駆動アー
ムの角速度指令信号を演算、出力する角速度演算手段
と、その角速度指令信号により制御部への入力信号を演
算する入力信号演算手段とを具備することにより、上述
の目的が達成される。
特許請求の範囲第２項に記載の角速度演算手段は速度比
のうち値の小さい２本のアームを常時駆動するものであ
る。
のうち値の小さい２本のアームを常時駆動するものであ
る。
特許請求の範囲第３項に記載の角速度演算手段は速度比
のうち値の大きい２本のアームを常時駆動するものであ
る。
のうち値の大きい２本のアームを常時駆動するものであ
る。
特許請求の範囲第４項に記載の装置は、軌跡制御対象部
位の動き量に関する少なくとも２つのモードを選択する
モード選択手段をさらに備え、角速度演算手段はモード
に基づき速度比のうちのいずれか２つを選択し駆動すべ
き２本のアームを決定するものである。
位の動き量に関する少なくとも２つのモードを選択する
モード選択手段をさらに備え、角速度演算手段はモード
に基づき速度比のうちのいずれか２つを選択し駆動すべ
き２本のアームを決定するものである。
特許請求の範囲第５の装置は、アクチュエータを油圧シ
リンダとし、制御部を電磁比例弁としたものである。
リンダとし、制御部を電磁比例弁としたものである。
特許請求の範囲第６の装置は、精度モードと速度モード
とを設け、精度モードが選択されたときは速度比のうち
値の小さい２本のアームを常時駆動し、速度モードが選
択されたときは速度比のうち値の大きい２本のアームを
常時駆動するようにしたものである。
とを設け、精度モードが選択されたときは速度比のうち
値の小さい２本のアームを常時駆動し、速度モードが選
択されたときは速度比のうち値の大きい２本のアームを
常時駆動するようにしたものである。
E.作用 角速度演算手段には検出された角度信号と指令手段から
の速度指令信号とが入力され、これらの入力信号に基づ
いて各アームの回動角速度とアーム駆動用アクチュエー
タの速度との速度比を演算する。さらに、この速度比に
基づいて駆動すべき２本のアームを選択する。駆動アー
ムが選択されると、速度指令信号と角度信号とにより駆
動アームの角速度指令信号が演算される。この角速度指
令信号は入力信号演算手段に入力されて、アクチュエー
タの駆動方向や駆動速度等を制御する制御部への入力信
号が演算される。その入力信号に基づいて制御部が駆動
され、これによりアクチュエータが駆動される。
の速度指令信号とが入力され、これらの入力信号に基づ
いて各アームの回動角速度とアーム駆動用アクチュエー
タの速度との速度比を演算する。さらに、この速度比に
基づいて駆動すべき２本のアームを選択する。駆動アー
ムが選択されると、速度指令信号と角度信号とにより駆
動アームの角速度指令信号が演算される。この角速度指
令信号は入力信号演算手段に入力されて、アクチュエー
タの駆動方向や駆動速度等を制御する制御部への入力信
号が演算される。その入力信号に基づいて制御部が駆動
され、これによりアクチュエータが駆動される。
F.実施例 第１図〜第３図はこの発明の実施例を説明する図であ
り、第４図に示したモデル図の如く３本のアームを有す
る多関節作業機に適用するように構成されたものであ
る。
り、第４図に示したモデル図の如く３本のアームを有す
る多関節作業機に適用するように構成されたものであ
る。
第１図において、第３のアーム３先端の水平方向（ｘ方
向）および垂直方向（ｙ方向）の速度を指令し速度指令
信号Vx,Vyを出力する速度指令装置10は制御装置20と接
続されている。また、軌跡制御の際にアーム３先端の軌
跡精度を重視した精度モードまたは速度を重視した速度
モードを選択し精度モード信号Maおよび速度モード信号
Mvを出力するモード選択スイッチ30も制御装置20と接続
されている。更に、第１〜第３のアーム１〜３の角度
（θ1〜θ3）をそれぞれ検出して角度信号θ1〜θ3を出
力するポテンショメータの如き角度センサ41−１〜41−
３も制御装置20に接続されている。制御装置20には各油
圧シリンダ４〜６への流量および吐出油の方向を制御す
る電磁比例弁（サーボ弁）51−１〜51−３が後続し、制
御装置20に入力される各種信号θ1〜θ3,Vx,Vy,Ma,Mvに
基づいて後述の如き演算が行われて電磁比例弁51−１〜
51−３に入力信号i1〜i3が出力されるように構成されて
いる。なお、電磁比例弁51−１〜51−３によって制御さ
れる各アーム１〜３の角度θ1〜θ3が制御装置20にフィ
ードバックされている。
向）および垂直方向（ｙ方向）の速度を指令し速度指令
信号Vx,Vyを出力する速度指令装置10は制御装置20と接
続されている。また、軌跡制御の際にアーム３先端の軌
跡精度を重視した精度モードまたは速度を重視した速度
モードを選択し精度モード信号Maおよび速度モード信号
Mvを出力するモード選択スイッチ30も制御装置20と接続
されている。更に、第１〜第３のアーム１〜３の角度
（θ1〜θ3）をそれぞれ検出して角度信号θ1〜θ3を出
力するポテンショメータの如き角度センサ41−１〜41−
３も制御装置20に接続されている。制御装置20には各油
圧シリンダ４〜６への流量および吐出油の方向を制御す
る電磁比例弁（サーボ弁）51−１〜51−３が後続し、制
御装置20に入力される各種信号θ1〜θ3,Vx,Vy,Ma,Mvに
基づいて後述の如き演算が行われて電磁比例弁51−１〜
51−３に入力信号i1〜i3が出力されるように構成されて
いる。なお、電磁比例弁51−１〜51−３によって制御さ
れる各アーム１〜３の角度θ1〜θ3が制御装置20にフィ
ードバックされている。
制御装置20は、角速度演算部21と、サーボ制御部22と、
リング補正部23と、電流演算部24とが順次に接続されて
構成されている。
リング補正部23と、電流演算部24とが順次に接続されて
構成されている。
角速度演算部21は例えばマイクロコンピュータで構成さ
れ、選択された精度モードまたは速度モードに応じて、
入力される角度信号θ1〜θ3および速度指令信号Vx,Vy
から以下に示す演算を行い角速度演算信号ia,ja
（ｉ＝１〜3,j＝１〜３）を演算して出力するように構
成されている。周知の通り３本のアームにより軌跡制御
する場合、ある拘束条件を定めて２本のアームだけを駆
動するが、本発明ではアーム回動角速度ｉとアクチュ
エータ速度、すなわち本実施例では油圧シリンダ速度
ｉとの速度比θi/Ziに基づき駆動すべき２本のアームを
選択する。本実施例の如き、精度モードの場合には、上
記速度比i/ｉが小さい２つのアームを常に選択し、
速度モードの場合には速度比i/ｉが大きいアームを
常に２本選択する。例えば、第５図（ａ）〜（ｃ）にお
いて、各アーム角度がθ1＝A,θ2＝B,θ3＝Ｃであれば
各速度比は、1 ／1＝Ｄ＜3／3＝Ｆ＜2／2＝Ｅ であり、この場合、精度モードでは第１のアーム１と第
３のアーム３とが選択駆動され、速度モードでは第２の
アーム２と第３のアーム３とが選択される。
れ、選択された精度モードまたは速度モードに応じて、
入力される角度信号θ1〜θ3および速度指令信号Vx,Vy
から以下に示す演算を行い角速度演算信号ia,ja
（ｉ＝１〜3,j＝１〜３）を演算して出力するように構
成されている。周知の通り３本のアームにより軌跡制御
する場合、ある拘束条件を定めて２本のアームだけを駆
動するが、本発明ではアーム回動角速度ｉとアクチュ
エータ速度、すなわち本実施例では油圧シリンダ速度
ｉとの速度比θi/Ziに基づき駆動すべき２本のアームを
選択する。本実施例の如き、精度モードの場合には、上
記速度比i/ｉが小さい２つのアームを常に選択し、
速度モードの場合には速度比i/ｉが大きいアームを
常に２本選択する。例えば、第５図（ａ）〜（ｃ）にお
いて、各アーム角度がθ1＝A,θ2＝B,θ3＝Ｃであれば
各速度比は、1 ／1＝Ｄ＜3／3＝Ｆ＜2／2＝Ｅ であり、この場合、精度モードでは第１のアーム１と第
３のアーム３とが選択駆動され、速度モードでは第２の
アーム２と第３のアーム３とが選択される。
なお、アーム角度θｉに対する速度比i/ｉは各アー
ム毎に第５図（ａ）〜（ｃ）のようになるが、逐次計算
して求めたり、あるいは予めアーム角度θｉ−速度比
i/ｉを二次元マップとして記憶しておき、そのマップ
を入力されたアーム角度θｉでルックアップして速度比
i/ｉを求めてもよい。
ム毎に第５図（ａ）〜（ｃ）のようになるが、逐次計算
して求めたり、あるいは予めアーム角度θｉ−速度比
i/ｉを二次元マップとして記憶しておき、そのマップ
を入力されたアーム角度θｉでルックアップして速度比
i/ｉを求めてもよい。
このように２本の駆動アームが決定されたら下記の
（６）式〜（８）式に従って角速度演算信号ia,ja
が演算される。
（６）式〜（８）式に従って角速度演算信号ia,ja
が演算される。
第１のアーム１と第２のアーム２が選択されると次
式（６）によりia,jaとして1，2が演算され
る。
式（６）によりia,jaとして1，2が演算され
る。
第２のアーム２と第３のアーム３が選択されると次
式（７）により、ia,jaとして2，3が演算され
る。
式（７）により、ia,jaとして2，3が演算され
る。
第１のアーム１と第３のアーム３が選択されると次
式（８）によりia,jaとして2，3が演算され
る。
式（８）によりia,jaとして2，3が演算され
る。
サーボ制御部22は、制御開始時の各アーム角度θｉ0を
初期値として角速度演算信号iaを積分して位置指令信
号θibを出力する積分器221−１〜221−３と、積分器22
1−１〜221−３の出力θibと実際のアーム角度θｉとの
偏差Δθibをとる偏差発生器222−１〜222〜３と、偏差
Δθibに所定のゲインをかけるゲイン設定器223−１〜2
23−３と、ゲイン設定器223−１〜223−３の出力Δic
に角速度演算信号iaを加え合わせて角速度指令信号
idを得る加算器224−１〜224−３とを有する。
初期値として角速度演算信号iaを積分して位置指令信
号θibを出力する積分器221−１〜221−３と、積分器22
1−１〜221−３の出力θibと実際のアーム角度θｉとの
偏差Δθibをとる偏差発生器222−１〜222〜３と、偏差
Δθibに所定のゲインをかけるゲイン設定器223−１〜2
23−３と、ゲイン設定器223−１〜223−３の出力Δic
に角速度演算信号iaを加え合わせて角速度指令信号
idを得る加算器224−１〜224−３とを有する。
そして、加算器224−１〜224−３に各リンク補正部23−
１〜23−３が後続している。このリンク補正部23−１〜
23−２にはアームの位置信号θibが入力され、入力され
る角速度指令信号idにアーム角度に応じたゲインを与
えてシリンダ速度信号ｉを出力する。このリンク補正
部23−１〜23−３には電流演算部24−１〜24−３が後続
し、それらには電磁比例弁51−１〜51−３が後続してい
る。電磁比例弁51−１〜51−３はその入力信号iiにより
図示していない油圧ポンプからの吐出油の方向および流
量を制御する。
１〜23−３が後続している。このリンク補正部23−１〜
23−２にはアームの位置信号θibが入力され、入力され
る角速度指令信号idにアーム角度に応じたゲインを与
えてシリンダ速度信号ｉを出力する。このリンク補正
部23−１〜23−３には電流演算部24−１〜24−３が後続
し、それらには電磁比例弁51−１〜51−３が後続してい
る。電磁比例弁51−１〜51−３はその入力信号iiにより
図示していない油圧ポンプからの吐出油の方向および流
量を制御する。
本実施例の各構成要素のうち、油圧シリンダ４〜６がア
クチュエータ80を、電磁比例弁51−１〜51−３が制御部
50を、速度指令装置10が指令手段を、モード選択スイッ
チ30が選択手段を、角度センサ41−１〜41−３が角度検
出手段40を、各速度演算部21,サーボ制御部22が角速度
演算手段60を、リンク補正部23と電流演算部24とが入力
信号演算手段70をそれぞれ構成している。そして、軌跡
制御対象部位が第３のアーム３の最先端である。
クチュエータ80を、電磁比例弁51−１〜51−３が制御部
50を、速度指令装置10が指令手段を、モード選択スイッ
チ30が選択手段を、角度センサ41−１〜41−３が角度検
出手段40を、各速度演算部21,サーボ制御部22が角速度
演算手段60を、リンク補正部23と電流演算部24とが入力
信号演算手段70をそれぞれ構成している。そして、軌跡
制御対象部位が第３のアーム３の最先端である。
このように構成された本実施例の作用について説明す
る。
る。
モード選択スイッチ30により精度モードまたは速度モー
ドのいずれかが選択されるとモード信号MaまたはMvが角
速度演算部21に入力され、速度指令装置10から、アーム
３先端の水平方向（ｘ方向）および垂直方向（ｙ方向）
の速度指令信号Vx,Vyが角速度演算部21に入力される。
更に、角度検出センサ41−１〜41−３から各アームの相
対角度信号θ1〜θ3が角速度演算部21に入力される。
ドのいずれかが選択されるとモード信号MaまたはMvが角
速度演算部21に入力され、速度指令装置10から、アーム
３先端の水平方向（ｘ方向）および垂直方向（ｙ方向）
の速度指令信号Vx,Vyが角速度演算部21に入力される。
更に、角度検出センサ41−１〜41−３から各アームの相
対角度信号θ1〜θ3が角速度演算部21に入力される。
このような各種信号が入力された角速度演算部21は、第
２図に示す手順によって角速度演算信号ia,jaを出
力する。
２図に示す手順によって角速度演算信号ia,jaを出
力する。
手順P1では、各信号Vx,Vy,θ1，θ2，θ3およびMaまた
はMvを読み込む。手順P2では角度信号θ1〜θ3により各
アーム毎に二次元マップから速度比i/ｉをルックア
ップして演算する。手順P3では選択されたモードをモー
ド信号により判別する。精度モードが選択されている
と、手順P4において、演算した速度比i/ｉのうち小
さい値を示している２本のアームを選択し、手順P5にお
いて、（６）式〜（８）式に基づいて一対の角速度演算
信号ia,jaが出力される。速度モードが選択されて
いると、手順P6において速度比ｉｉうちの大きい値
を示している２本のアームを選択し、手順P5において同
様に一対の角速度演算信号ia,jaが出力される。
はMvを読み込む。手順P2では角度信号θ1〜θ3により各
アーム毎に二次元マップから速度比i/ｉをルックア
ップして演算する。手順P3では選択されたモードをモー
ド信号により判別する。精度モードが選択されている
と、手順P4において、演算した速度比i/ｉのうち小
さい値を示している２本のアームを選択し、手順P5にお
いて、（６）式〜（８）式に基づいて一対の角速度演算
信号ia,jaが出力される。速度モードが選択されて
いると、手順P6において速度比ｉｉうちの大きい値
を示している２本のアームを選択し、手順P5において同
様に一対の角速度演算信号ia,jaが出力される。
一対の角速度演算信号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,221jの位置指令信号θib,θjb、すな
わちアーム角度に応じた補正演算が実行されてシリンダ
速度信号ｊが得られる。この補正演算は、角速度指令
信号id,jdに所定のゲインを与えることにより行わ
れる。シリンダ速度信号i,ｊは電流演算部24−i,24
−ｊに入力されて電磁比例弁51−i,51−ｊの入力信号i
i,ijに変換される。そして、電磁比例弁51−i,51−ｊが
その入力信号ii,ijによって制御されアームi,jが駆動さ
れる。駆動される２本のアームi,jは、選択されたモー
ドおよび逐次入力される角度θ1〜θ3により決定される
が、その拠所となる速度比i/ｉは各アーム毎に第５
図（ａ）〜（ｃ）のようになっているので、同一モード
においてもアーム角度により速度比i/ｉが変わると
駆動アームが適宜変わることになる。
ｊで積分されて位置指令信号θ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,221jの位置指令信号θib,θjb、すな
わちアーム角度に応じた補正演算が実行されてシリンダ
速度信号ｊが得られる。この補正演算は、角速度指令
信号id,jdに所定のゲインを与えることにより行わ
れる。シリンダ速度信号i,ｊは電流演算部24−i,24
−ｊに入力されて電磁比例弁51−i,51−ｊの入力信号i
i,ijに変換される。そして、電磁比例弁51−i,51−ｊが
その入力信号ii,ijによって制御されアームi,jが駆動さ
れる。駆動される２本のアームi,jは、選択されたモー
ドおよび逐次入力される角度θ1〜θ3により決定される
が、その拠所となる速度比i/ｉは各アーム毎に第５
図（ａ）〜（ｃ）のようになっているので、同一モード
においてもアーム角度により速度比i/ｉが変わると
駆動アームが適宜変わることになる。
第３図（ａ），（ｂ）に、垂直方向をyo一定とし、水平
方向でアーム３先端をxsa→xt→xsと移動させるように
指令したときのアーム３先端の軌跡を示し、第３図
（ａ）が速度モード、第３図（ｂ）が精度モードの場合
を示す。この図からわかるように、精度モードで軌跡制
御すると略ｙ＝yoでアーム３先端を移動できる。また速
度モードの場合には、垂直方向の位置精度は悪くなる
が、指令する速度Vx,Vyに対して精度モードに比べて水
平方向のxs→xt→xsの移動時間が早くなる。
方向でアーム３先端をxsa→xt→xsと移動させるように
指令したときのアーム３先端の軌跡を示し、第３図
（ａ）が速度モード、第３図（ｂ）が精度モードの場合
を示す。この図からわかるように、精度モードで軌跡制
御すると略ｙ＝yoでアーム３先端を移動できる。また速
度モードの場合には、垂直方向の位置精度は悪くなる
が、指令する速度Vx,Vyに対して精度モードに比べて水
平方向のxs→xt→xsの移動時間が早くなる。
従って、例えば多関節作業機を用いて下塗り、仕上げ塗
りを行うような場合、下塗り時に速度モードを選択し仕
上げ塗り時に精度モードを選択すれば効率よく作業がで
きる。また、精度だけが必要な作業に精度モードで軌跡
制御すれば、従来のように特定の２本のアームだけを駆
動する場合に比べて位置精度が向上する。
りを行うような場合、下塗り時に速度モードを選択し仕
上げ塗り時に精度モードを選択すれば効率よく作業がで
きる。また、精度だけが必要な作業に精度モードで軌跡
制御すれば、従来のように特定の２本のアームだけを駆
動する場合に比べて位置精度が向上する。
以上では３本アームの作業機について説明したが４本以
上のアームを有する作業機にも同様に適用でき、この場
合、軌跡モードとして精度と速度との中間的なモードを
設定し、速度比i/ｉの２つの中間値を有する２本の
アームを駆動するようにしてもよい。更に以上ではアク
チュエータを油圧シリンダとしたが空圧シリンダでもよ
く、油圧、空圧モータまたは電動機の回転力を直線運動
に変換してアームを駆動するようなものでもよい。
上のアームを有する作業機にも同様に適用でき、この場
合、軌跡モードとして精度と速度との中間的なモードを
設定し、速度比i/ｉの２つの中間値を有する２本の
アームを駆動するようにしてもよい。更に以上ではアク
チュエータを油圧シリンダとしたが空圧シリンダでもよ
く、油圧、空圧モータまたは電動機の回転力を直線運動
に変換してアームを駆動するようなものでもよい。
G.発明の効果 本発明によれば、アーム回転角速度とアクチュエータの
駆動速度との比に基づいて駆動すべき２本の駆動アーム
を選択するようにしたので、たとえば予め所望の基準速
度比を設定しておき、その基準速度比に近い速度比の２
本のアームを選択するように構成しておき、その基準速
度比に近い速度比の２本のアームを選択するように構成
すれば、基準速度比を大きくすることで速度を重視した
運転ができ、基準速度比を小さくすうことで精度を重視
した運転が可能となるなど、軌跡対称部位の操作の自由
度が広がる。また、精度モードや速度モードを設定し、
精度モードが選択されたときに速度比のうち値の小さい
２本のアームを常時駆動し、速度モードが選択されたと
きは速度比のうち値の大きい２本のアームを常時駆動す
るようにすれば、より操作性が向上する。
駆動速度との比に基づいて駆動すべき２本の駆動アーム
を選択するようにしたので、たとえば予め所望の基準速
度比を設定しておき、その基準速度比に近い速度比の２
本のアームを選択するように構成しておき、その基準速
度比に近い速度比の２本のアームを選択するように構成
すれば、基準速度比を大きくすることで速度を重視した
運転ができ、基準速度比を小さくすうことで精度を重視
した運転が可能となるなど、軌跡対称部位の操作の自由
度が広がる。また、精度モードや速度モードを設定し、
精度モードが選択されたときに速度比のうち値の小さい
２本のアームを常時駆動し、速度モードが選択されたと
きは速度比のうち値の大きい２本のアームを常時駆動す
るようにすれば、より操作性が向上する。
第１図は本発明の一実施例を示すブロック図、第２図は
その処理手順の一例を示すフローチャート、第３図
（ａ），（ｂ）は実際の軌跡の２例を示す図、第４図は
３本アームのモデル図、第５図（ａ）〜（ｃ）はアーム
角度に対する速度比i/ｉを示すグラフである。 １〜3:アーム、４〜6:油圧シリンダ 10:指令手段、30:選択手段 40:角度検出手段、50:制御部 60:角速度演算手段 70:入力信号演算手段 80:アクチュエータ
その処理手順の一例を示すフローチャート、第３図
（ａ），（ｂ）は実際の軌跡の２例を示す図、第４図は
３本アームのモデル図、第５図（ａ）〜（ｃ）はアーム
角度に対する速度比i/ｉを示すグラフである。 １〜3:アーム、４〜6:油圧シリンダ 10:指令手段、30:選択手段 40:角度検出手段、50:制御部 60:角速度演算手段 70:入力信号演算手段 80:アクチュエータ
───────────────────────────────────────────────────── フロントページの続き (72)発明者 山口 武 茨城県土浦市神立町650番地 日立建機株 式会社土浦工場内 (72)発明者 白井 隆 茨城県土浦市神立町650番地 日立建機株 式会社土浦工場内 (72)発明者 橋本 昭 茨城県土浦市神立町650番地 日立建機株 式会社技術研究所内 (56)参考文献 特開 昭59−81081（ＪＰ，Ａ) 特開 昭60−44278（ＪＰ，Ａ) 特開 昭59−149510（ＪＰ，Ａ) 特開 昭55−119705（ＪＰ，Ａ) 特開 昭60−29288（ＪＰ，Ａ) 特開 昭61−23212（ＪＰ，Ａ)
Claims (6)
- 【請求項１】関節を介して回動可能に連結された３本以
上のアームと、それらのアームを駆動するアクチュエー
タと、そのアクチュエータによるアームの移動量を制御
する制御部とを有する多関節作業機の軌跡制御装置にお
いて、軌跡制御対象部位の所定方向における速度を指令
し、速度指令信号を出力する指令手段と、前記各アーム
の相対角度を検出して角度信号を出力する角度検出手段
と、検出された角度から各アームの回動角速度と各アク
チュエータの速度との速度比を演算し、これらの速度比
に基づき駆動すべき２本のアームを選択し、前記速度指
令信号および角度信号に基づいて選択された２本の駆動
アームの角速度指令信号を演算、出力する角速度演算手
段と、その角速度指令信号により前記制御部への入力信
号を演算する入力信号演算手段とを具備したことを特徴
とする多関節作業機の軌跡制御装置。 - 【請求項２】特許請求の範囲第１項に記載の装置におい
て、前記角速度演算手段は前記速度比のうち値の小さい
２本のアームを常時駆動することを特徴とする多関節作
業機の軌跡制御装置。 - 【請求項３】特許請求の範囲第１項に記載の装置におい
て、前記角速度演算手段は前記速度比のうち値の大きい
２本のアームを常時駆動することを特徴とする多関節作
業機の軌跡制御装置。 - 【請求項４】特許請求の範囲第１項に記載の装置におい
て、前記軌跡制御対象部位の動き量に関する少なくとも
２つのモードを選択するモード選択手段を設け、前記角
速度演算手段は前記モードに基づき前記速度比のうちの
いずれか２つを選択し駆動すべき２本のアームを決定す
ることを特徴とする多関節作業機の軌跡制御装置。 - 【請求項５】特許請求の範囲第１〜第４項のいずれかに
記載の装置において、前記アクチュエータを油圧シリン
ダとし、前記制御部を電磁比例弁としたことを特徴とす
る多関節作業機の軌跡制御装置。 - 【請求項６】特許請求の範囲第４項または第５項に記載
の装置において、前記モードは精度モードと速度モード
とし、精度モードが選択されたときは前記速度比のうち
値の小さい２本のアームを常時駆動し、速度モードが選
択されたときは前記速度比のうち値の大きい２本のアー
ムを常時駆動することを特徴とする多関節作業機の軌跡
制御装置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP61083358A JPH07113848B2 (ja) | 1986-04-11 | 1986-04-11 | 多関節作業機の軌跡制御装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP61083358A JPH07113848B2 (ja) | 1986-04-11 | 1986-04-11 | 多関節作業機の軌跡制御装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JPS62241010A JPS62241010A (ja) | 1987-10-21 |
| JPH07113848B2 true JPH07113848B2 (ja) | 1995-12-06 |
Family
ID=13800206
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP61083358A Expired - Lifetime JPH07113848B2 (ja) | 1986-04-11 | 1986-04-11 | 多関節作業機の軌跡制御装置 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JPH07113848B2 (ja) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| DE4030954C2 (de) * | 1990-09-29 | 1994-08-04 | Danfoss As | Verfahren zur Steuerung der Bewegung eines hydraulisch bewegbaren Arbeitsgeräts und Bahnsteuereinrichtung zur Durchführung des Verfahrens |
| JPH06131029A (ja) * | 1992-10-15 | 1994-05-13 | Fanuc Ltd | 加速度定数切り換え方式 |
Family Cites Families (8)
| 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 |
| JPS55119705A (en) * | 1979-03-09 | 1980-09-13 | Kawasaki Heavy Ind Ltd | Set method of instruction value for industrial robot |
| 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 JP61083358A patent/JPH07113848B2/ja not_active Expired - Lifetime
Also Published As
| Publication number | Publication date |
|---|---|
| JPS62241010A (ja) | 1987-10-21 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US5799419A (en) | Method for controlling the operation of power excavator | |
| US5062264A (en) | Hydraulic control system | |
| JP4215944B2 (ja) | 油圧ショベルのフロント制御装置 | |
| Farkhatdinov et al. | Hybrid position-position and position-speed command strategy for the bilateral teleoperation of a mobile robot | |
| Hong et al. | Tethered mobile robot for automating highway maintenance operations | |
| JPH07113848B2 (ja) | 多関節作業機の軌跡制御装置 | |
| Ishimoto et al. | A practical trajectory following of an articulated steering type vehicle | |
| JP2791030B2 (ja) | 多自由度作業機械の曲面倣い制御装置 | |
| KR100335363B1 (ko) | 굴삭기의제어장치 | |
| JP2000303492A (ja) | 建設機械のフロント制御装置 | |
| JPH06312392A (ja) | 多関節ロボットの制御装置 | |
| JP3437348B2 (ja) | 建設機械の軌跡制御装置 | |
| JP3749319B2 (ja) | 建設機械の軌跡制御装置 | |
| JP3705661B2 (ja) | 建設機械の作業機の姿勢制御装置 | |
| JPH0823155B2 (ja) | 作業機の制御装置 | |
| JP3444313B2 (ja) | 工業用ロボットの制御装置 | |
| JPH1058368A (ja) | マニピュレータ制御装置 | |
| JPH02101228A (ja) | 作業機の制御装置 | |
| KR0155281B1 (ko) | 다관절 로보트의 직선보간방법 | |
| JP2672724B2 (ja) | 多関節アームを備えた軌跡制御装置 | |
| JPH0791844B2 (ja) | 作業機の制御装置 | |
| Lietmann et al. | Visual servoing for a non-holonomic mobile robot using a range-image-camera | |
| JP2509368B2 (ja) | 作業アタッチメントの姿勢角制御装置 | |
| JPS62241009A (ja) | 多関節作業機の軌跡制御装置 | |
| JPS6365507A (ja) | 多関節作業機の軌跡制御装置 |