JPH0256681B2 - - Google Patents

Description

【発明の詳細な説明】 発明の技術分野 本発明は、ロボツトのハンドなどの制御部位を
目標位置に対して、計画した軌道に沿つて接近さ
せ、かつ迅速にその目標位置に位置決めする多関
節型ロボツトの制御装置に関する。

技術の背景 多関節型ロボツトに作業をさせる際、人間に理
解しやすいあるいは外部環境と適合しやすい
XYZ直角座標系のデータで指示を与えることが
一般的に行なわれている。これに対して、多関節
型ロボツトはその構造上から来る座標系（多関節
ロボツトは回動可能あるいは更に伸縮可能な複数
個の腕からなるから、該腕を座標軸にとつた座標
系で、関節座標系と呼ばれる）で動作するから、
直角座標系→関節座標系への座標変換演算をリア
ルタイムで行ないながらロボツト各関節駆動部を
動作させることになる。

第１図に示すようにＡ点からＢ点までロボツト
ハンドＨの向きを一定にしたまま直線的軌道を描
いて移動させる場合で説明すると、まず直角座標
系XYZ上でＡ点からＢ点に向う線速度S〓（ｉ）の
大きさを各時点の位置に応じた加減速制御を含め
て決める。これをX〓（ｉ）（ｉ＝０，１，２，…
…）とすると、ΔT秒後（ΔTはサンプリングタ
イムで、座標変換時間などを考慮して定める）に
位置すべき座標X_(i+1)は X_(i+1)＝Ｘ（ｉ）＋X〓（ｉ）・ΔT 式(1) として求められる。これを座標変換して関節座標
系の座標θ_(i+1)を得ると、ロボツトの各関節に与え
るべき指示速度θ〓（ｉ）は θ〓（ｉ）＝θ_i+1−θ′（ｉ）／ΔT 式(2) として計算できる。ここで、θ（ｉ）′はＸ（ｉ）
に対応する関節座標系上の点であるが、これは指
示速度θ〓（ｉ）を出力する直前においてロボツトの
各軸に実装しているモータの回転角検知器により
実測した値とする。この実測値を用いる理由は特
願昭55−187887で詳述しているが、次のような効
果がある。即ち第２図は１つの関節について指示
速度V₁と実際の移動速度V₂を対比したものであ
るが、モータの出力トルクが有効であるという物
理的な制約のため、実際の速度V₂はΔT秒ごとに
ステツプ状に更新されて行く指示速度V₁に完全
には追いつけず、斜線を施した３角形の面積の分
だけ予定位置θ_(i+1)からずれることになる。しかし
追従差分法と呼ぶ式(2)の方法では、θ（ｉ）′に実
測値を用いているため次の時点の指示速度ではこ
のずれが自動的に補正されていく形になつてい
る。なお後述のように指示速度をV₁の如くステ
ツプ状に変化させずV₂のように傾斜させると斜
線部分が僅少になり、ロボツト各関節駆動部が過
大入力を受けて飽和するなどの問題がなくなつ
て、ロボツトの運動は円滑になりかつ各関節駆動
部の利得を上げて高精度な制御を行なうことがで
きる。

従来技術と問題点 このようにして第１図のような直線的軌跡を描
いてＢ点に接近することができるようになつた
が、Ｂ点にピタリ位置決めするにはまだ問題が残
る。というのは、第２図のように指示速度と実際
の速度が完全には一致しないことにより、たとえ
Ｂ点において駆動位置X_i+1従つてθ_i+1が該目標位
置Ｂに一致するように、式(2)によりθ〓iを計算して
これを指示速度としても、ΔT秒後に必らずしも
正確に該Ｂ点に到着はしない。一般にはこの時点
で各軸は該θ〓iなる速度を持つているので、次の
ΔT秒後にはＢ点を通り越してしまい、実測した
現在位置Xiによる戻り制御が行なわれ、ロボツ
トハンドはＢ点の周りで振動もしくはループを画
きながら該Ｂ点に接近することになる。しかもや
がては完全にＢ点に到達できるかと言うとそうで
はなく、指示速度の量子化誤差と指示速度を更新
する時間々隔の粗さのために、氷久にＢ点の周り
に振動することもあるという問題がある。前述の
ように直角座標系での線速度S〓（ｉ）は加減速を
考慮して定めるので目標位置近傍では小になつて
いるが、関節座標系は直角座標系に対して比例関
係にある訳ではないから、関節座標系に変換する
とその指示速度は目標位置近傍でも大きいことが
有り得、これが上記の問題を惹き起し易い。

かゝる問題は、第１図Ｂ点に接近する時直角座
標系のレベルで線速度S〓（ｉ）を徐々に落とす
（これはΔT毎のため粗い制御である）だけでな
く、関節座標系のレベルにおいてもそれぞれの軸
についてＢ点への接近につれて減速することによ
り改善できる。

直角座標系と関節座標系の２つのレベルでＢ点
に接近するにつれて減速させることは一見２度手
間のように感じられるが、直角座標系のレベル
のみではΔTが大きく、粗い制御になるので迅速
かつ高精度は位置決めが困難、直角座標系上で
計画された軌道に従がいながら第１図Ｂ点に向つ
て減速して行く時、関節座標系レベルのみで減速
動作させると、各関節固有の加速度で減速される
ため、ロボツトハンドは前記計画された軌道上を
正確に通らなくなる。直角座標系→関節座標系
の変換は非線形であるため前述のように、ある関
節が伸びきる姿勢では、直角座標系では微小量の
変位でも関節座標系上では速度、加速度が無限大
になる（これは特異点と呼ばれる）ことがある。
各関節の駆動部は勿論無限大の速度、加速度には
追従できず、パルスモータ等を使用した時は脱調
するし、直流モータで閉ループ制御する時も制御
回路が飽和して暴走の危険がある。したがつて、
直角座標系のレベルだけの加、減速制御では保安
上においても問題があり、関節座標レベルでの加
減速制御をしないと、特異点を遠く避けて動かす
などの処理をしなければならない。等、多関節型
ロボツト特有の事情があるので、上記２重減速は
けつして無駄ではない。

２重減速を行なうには、第１図Ｂ点における各
関節の座標データ（各関節角の目標値）を、各関
節ごとに実装した制御回路に予め設定しておく。。
これらの各関節の制御回路は、上記各関節の目標
角度（目標位置）を原点とする減速曲線を発生
し、各関節の回転速度をこの減速曲線に従つて減
速しながらロボツトハンドを目標角度に導き、そ
こに位置決めする様構成する。

しかしながらこゝでまた新たな問題が出てく
る。これを、最も簡単な２自由度極座標型ロボツ
トで、Ｒ軸とθ軸を操作してＡ点からＢ点へ直線
状に動かす例により説明するに、第３図は該ロボ
ツトを示し、ARは伸縮及び回動が可能な該ロボ
ツトの腕、Ｌは該腕の先端軌跡である。ここでは
腕ARの先端をＢ点に位置決めしたいのであらか
じめＢ点におけるＲ軸とθ軸の目標位置をそれぞ
れのサーボ制御回路にセツトしておき、Ａ点から
出発する。図面から明らかなように出発点Ａにお
けるＲ，θは目標点Ｂにおけるそれとは異なる
（Ｒは大、θも水平軸からの角で言つて大）なの
で、差を０にする運動が開始し、この結果腕AR
の先端は直線Ｌに沿つてＢへ向かうが、途中のＣ
点でＲ軸の長さがＢ点でのそれと同じになる。こ
の結果θ軸についてはＡ点からＢ点まで動き続け
るのに、Ｒ軸はＣ点で位置決め完了となつてしま
う（各関節独立制御であるから）。そこでＡ点か
らＣ点までは各軸の指示速度をうまく調整するの
で腕先端は直線状に動いていくが、Ｃ点からＢ点
まではＲ軸の動きが止まつてしまつたので第３図
破線の様に円弧状に動くことになり、軌道は正し
くない。

発明の目的 本発明はかゝる点を改善し、ロボツトハンドは
予定軌跡を通つて目標位置へ迅速に、振動など生
じることなく正確に、到達するように制御するロ
ボツト制御法を提供しようとするものである。

発明の構成 本発明の多関節ロボツトの制御装置は多関節ロ
ボツトの構造から定まる関節座標系とは異なる座
標系で表わされたロボツト目標位置データ及び移
動前位置データを取込んで、所定の加減速度で時
間変化する指示速度データを各サンプリングタイ
ミング毎に出力する第１の装置と、該指示速度デ
ータを入力されてそれを関節座標系の指示速度デ
ータに変換し、更に各関節の許容加減速で時間変
化する指示速度に変更してロボツト各関節駆動部
へ出力し、かつ前記目標位置データを関節座標系
に変換して得たロボツト目標角度では角速度が零
になる減速曲線を各関節毎に内蔵する第２の装置
と、ロボツトが目標位置に接近したとき、各関節
駆動部を一斉に速度制御から前記減速曲線に従う
位置制御に切換える信号を出力する切換判定装置
とを備えることを特徴とするが、次に実施例を参
照しながらこれを詳細に説明する。

発明の実施例 第３図で述べたように直角座標では単純な直線
軌跡でも関節座標では、Ｒ軸が出発長から目標長
に減少し、更にそれ以下に減少し、次いで増大し
て目標長になつて始めて目標点に達するというこ
とが生じ、このようなことは関節ロボツトでは頻
繁に発生する。かゝる問題に対してはＲ軸に対し
て第３図Ｃ点では位置決め完了とさせないのがよ
く、このためにはＡ点を出発してＣ点を通過する
まではＲ，θ各軸における減速−位置決め機能を
殺しておくとよい。そして第３図Ｂ点に接近して
真に位置決め動作が必要になつてきた時に初めて
各軸の減速−位置決め機能を生かし、Ｂ点に正確
に位置決めさせるようにする。後者Ｂ点附近での
制御を位置決め制御モードと呼び、前者Ａ〜Ｃ間
等での制御を速度制御モードと呼ぶ。

第３図Ｂ点への接近を判定する方法として次の
２通りが考えられる。そして、この判定が成立し
た時、全軸に速度制御モードから位置決め制御モ
ードへの切り替えを指令する。直角座標系上
で、第３図のＢ点を中心とする半径ｒの円（より
多関節のロボツトでは球）を設定して、この円
（球）の内部に入つたことを判定する。関節座
標系上で、各軸に予め設定されている目標位置に
対して、各軸の全てがそれぞれの目標位置に一定
の値ｄまで接近したことを判定する。

ここで、半径ｒ又は値ｄを十分小さく選べば第
３図でＣ点からＢ点まで円弧を描いて運動するこ
とがなくなる。また、Ｃ点とＢ点が第４図のよう
に接近する場合にはその恐れはあるが、短かい円
弧なのでほぼ直線に近似することができるため、
実用上問題はない。

しかし、ｒ又はｄをいくらでも小さくできるか
と言うとそうではない。上記の場合にｄを０と
した時には第５図のように、減速されてはいるが
まだ或る値V₂₀なる速度を持つている状態でロボ
ツトハンドはすでに目標位置Ｂ上に来ている時、
これからその目標位置に位置決めしようと言うこ
とになるから、移動速度V₂が０でない以上目標
位置Ｂを行きすぎ、後戻りしなければならなくな
る。そこで、ｒ又はｄの与え方としては第６図の
ように移動速度が減速曲線Ｄを越えない様なｄと
すれば、（目標位置−ｄ）の位置までロボツトハ
ンドが到達したとき位置決め制御モードに切換
え、減速曲線Ｄに従つて目標位置まで減速して高
精度かつ迅速な位置決めができる。

以上では第３図の２自由度の極座標型ロボツト
の簡単な例を想定しているが、より自由度の高い
一般の多関節型ロボツトについても同じことが言
える。

ここでまだ、問題が残つている。第５図のよう
に１度目標位置Ｂをオーバシユートして目標位置
に位置決めする場合と、第６図のようにオーバシ
ユートなしにスムーズに目標位置に位置決めする
場合の２つがあるが、後者の方が望ましいことは
明らかである。そこで、第６図の様な状態にする
にはｒ又はｄを十分大きくとる、直角座標系
上で計画される線速度について、Ｂ点への接近に
つれて精密に減速する、をしなければならない
が、についてはｒ又はｄを大きく取りすぎると
第３図のようにＣ点とＢ点の間で円弧状に動くこ
とが多くなるので限度がある。については、線
速度の減速制御を精密に行なえばｒ又はｄが小さ
くても第６図の如き円滑な位置決めを期待できる
が、多関節ロボツトの場合には前述のように特異
点が存在するので、Ｂ点が特異点の真上かその近
傍である場合にはの方法でいくら線速度を小さ
くしていつてもある軸に対する指示速度は無限大
に近い値となることがあり、このような場合には
振動状態になることは避けられない。本発明はこ
のような場合でも１回のオーバシユートを許すの
みで確実な位置決めができるようにするものであ
り、速度制御から位置制御への切換えは各関節駆
動部で個々に行なうのではなく、前記ｒまたはｄ
以下になつたとき全関節駆動部で同時に行なうよ
うにする。実施例を第７図および第８図に示す。

第７図は速度制御モードから位置決め制御モー
ドへの切り替え判定を直角座標系上で、目標位置
X_Bを中心とした半径ｒの内側に入つたことによ
り行なう例を示し、第８図は関節座標系上で、各
軸に予め設定しておいた目標角度θ_Bに対して全軸
がすべてθ_B−ｄまでの距離まで接近したことによ
り行なう例を示す。

ロボツトは６自由度の多関節型ロボツトとする
と、直角座標系でのロボツトハンドの位置ベクト
ルＸは Ｘ＝（ｘ，ｙ，ｚ，α，β，γ） で表わされる。即ちロボツトを取り付けた台に原
点と座標軸を設定し、ハンドの位置をｘ，ｙ，ｚ
でまた向きをα，β，γ（オイラー角）で表わす。
そして、X_Aは移動前のハンドの座標、X_Bは次の
移動により位置決めすべきハンドの目標位置、Ｘ
（ｉ）は移動途中の離散時間関数としてのハンド
の座標、X_(i+1)はそれからΔT秒後のハンドの座標
である。なおベクトル記号・はこゝでは適宜省略
する。一方、関節座標系でのロボツトハンドの位
置ベクトルθは θ＝（θ₁，θ₂，θ₃，θ₄，θ₅，θ₆） で表わされ、θ₁〜θ₆はロボツトの各関節の回転角
である。こゝではロボツトの各腕は伸縮しないと
している。そして、θ_Aは移動開始前のハンドの回
転角度、θ_Bは次の移動により位置決めすべき目標
角度、θ（ｉ）は移動途中の離散時間関数として
の角度、θ_(i+1)はそのΔT秒後に位置すべき角度、
θ′（ｉ）は実際の回転角を実測した角度、θ（ｔ）
は連続時間関数としてロボツトに指令する角度で
ある。

各ブロツクの説明をすると、１１は移動長演算
部で移動前後のハンドの座標X_A，X_Bより移動長
（点Ａ，Ｂ間の直線距離）S_Bを算出する。S_B＝｜
X_B−X_A｜は S_B＝√（_B−_A）²＋（_B−_A）²＋（_B−
_A）²＋｛（_B−_A）²＋（_B−_A）²＋（_B−_A
）²｝×1² として計算する。ここで、ハンドの向きを表わす
α，β，γは単位がラジアン（rad）であるの
で、半径1mを掛けて長さとする。なお一般的は
任意の半径が設定できるが、ここでは簡単のため
１ｍとした。12は単位方向ベクトルｅの演算部で
（X_B−X_A）／｜X_B−X_A｜としてＡ点よりＢ点へ
向くベクトルの単位ベクトルｅを算出する。１３
は座標変換部で、直角座標での目標位置ベクトル
X_Bを関節座標での目標位置ベクトルθ_Bに変換す
る。１４は線速制御部で、横軸に移動距離、縦軸
に線速度をとつて最大値をS_Mに制限した例えば
図示の如き台形の移動距離−線速度特性を持つ線
速度S〓（ｉ）を出力する。S〓（ｉ）は移動途中のハ
ンドにその先端部の線速度の大きさを指令する。
この値は、もしハンドの向きに変化がない時には
ハンドそれ自体の移動速度を示し、ハンドの位置
（回動軸）が固定され向きのみが変わる時にはそ
のハンド位置からハンドの向きに１ｍ延ばした点
の周速度の大きさを示す。ハンドの位置と方向
（向き）が同時に変わる時はこれらの合成速度で
ある。１５は移動途中のハンドの線速度ベクトル
X〓（ｉ）をｅ・S〓（ｉ）として算出する演算部であ
る。なお単位ベクトルｅは、直線運動では、その
１回の移動の間は一定値である。１６はΔT秒後
のハンド座標ベクトルX_(i+1)をＸ（ｉ）＋X〓（ｉ）・
ΔTとして求める演算部、１７は直角座標系X_(i+1)
を関節座標θ_(i+1)に変換する座標変換部、１８は指
示（角）速度演算部で前記式(2)より指示速度θ〓
（ｉ）を算出する。１９はサーボ制御回路で詳細
は後述するが、要はθ〓（ｉ）を受けてロボツトRB
に指令角度θ（ｔ）を与える。２０は速度制御モ
ードから位置決め制御モードへの切換えタイミン
グの判定回路、そして２１はこの判定のための一
方の入力である現時点までの移動距離Ｓ（ｉ）を
計算する演算回路である。

動作を説明するに、目標位置X_Bが入力される
と演算部１１で次の移動の総線長S_Bをまた演算部
１２で移動の方向ベクトルｅを計算する。また変
換部１３では座標変換ベクトルＡによりX_Bをθ_B
に変換し、θ_Bを各軸のサーボ制御回路１９に目標
角度として設定する。移動開始は、目標位置X_B
が入力して、総線長S_Bおよび現時点までの移動距
離Ｓ（ｉ）により制御部１４が線速度S〓（０）を０
でない小さな値として出力することにより開始さ
れ、このS〓（０）即ちｉ＝０のときのS〓（ｉ）（一々
説明しないが他も同様）は演算部１５により速度
ベクトルX〓（０）に分解され、演算部１６でΔT秒
後の座標Ｘ（１）をＸ（０）＋X〓（０）・ΔTとして計
算する。ここで、Ｘ（０）は移動前の座標X_Aとす
る。その次の変換部７では座標変換ベクトルＡに
よりやはりΔT秒後に位置すべき関節座標として
θ（１）を計算する。さらに演算部１８では現時
点での各関節の角度センサによる実測値θ′（０）
（これも移動前だからθ_Aと等しい）を用いてΔT
秒間の各関節角の平均角速度θ〓（０）を計算して制
御回路１９に出力する。なお変換部１７と演算部
１８との間には特願昭56−101852で説明している
ように軌跡補間部を設けることがある。これは、
直角座標から関節座標への変換には長い演算時間
を必要とし、このため直角座標系での指示値
X_(i+1)は飛び飛びの値でしか出せない（サンプリ
ングタイムが大）が、これではロボツトの運動が
円滑を欠く。そこで補間して飛び飛びの指示値
X_(i+1)の間に更に多数の指示値を発生するとロボ
ツトの運動を円滑化することができる。制御回路
１９ではθ〓（０）まで第２図のV₂のように滑らか
に加速しながらロボツトの各関節を駆動するため
の連続な関数θ（ｔ）を生成し、これをロボツト
RBに加える。ｉをΔT秒ごとに１づつ増しなが
ら、演算部１４からの指令S〓（ｉ）を最大値S_Mに
なるまで増大させて行くと、前記と同様な手順で
各演算部は出力を生じロボツトの各関節の回転が
制御され、ロボツトハンドの動きがこれに従つて
加速されていく。そしてS〓（ｉ）がS_Mに達すると
以降はS〓（ｉ）＝S_Mとなり、各演算部等はこれに準
じた動作を行なう。

演算部１４においてはＳ（ｉ）＝S_BのときS〓（ｉ）
≒０となるような減速曲線部ｆ（S_B−Ｓ（ｉ））（第
７図の台形の下降部分）を備えており、ハンドの
位置がX_Bに近づいてS〓（ｉ）とｆ（S_B−Ｓ（ｉ）が
等しくなると、それ以際はｆ（S_B−Ｓ（ｉ）をS〓
（ｉ）とし、減速指示に移る。そして、さらにX_B
に接近してS_B−Ｓ（ｉ）が位置決め制御モードへ
の切り替え判定値ｒよりも小さくなると判定回路
２０は制御回路１９に対して速度制御モードから
位置決め制御モードへの切り替え指令S_Wを出力
する。すると制御回路１９では各軸それぞれにつ
いて第６図のような減速曲線Ｄを設定（有効に）
し、各関節の速度がこの減速曲線に出合つた時、
以降、この減速曲線に従がつて減速して行き、θ_B
に到達すると、これでＡ点からＢ点への位置決め
動作が完了する。

サーボ制御回路１９は１軸分を示すと第９図の
如くなつている。これは特開昭56−22106で詳述
されているが、概要は次の如くである。４５はロ
ボツトの１軸を駆動するモータ、４４はその駆動
用の増幅器である。目標角度θ_Bが与えられまた前
述の演算器１８から速度指令θiが与えられると、
カウンタ３４の計数値θjはこの時点では０である
ので比較器３１はθi〉θjに従つてスイツチ３３を
接点ａ側に切換え、カウンタ３４は発振器３２の
出力パルスをカウントアツプする。発振器３２の
発振周波数はサーボ系がとり得る最大加速度に対
応する値を持ち、従つてこれを計数するカウンタ
３４の出力θjは該最大加速度で加速したときの速
度時間関数を示す。勿論、最大加速度以下なら支
障はないから発振周波数はそれ以下でもよく、ま
た制御部１４が出力する速度指令に対応できるよ
う該制御部１４が制限する加減速度以上の値にす
る。カウンタ４１の出力θrは現時点では前記θ_Aと
等しく、｜θ_B−θ_A｜がROM３６に加わる。ROM
３６は前述の減速曲線Ｄを出力するもので、｜θ_B
−θr｜をアドレスとしてその平方根を出力する。
現時点ではこの出力θsは大きいから、比較器３７
はθs＞θjに従つてスイツチ３８をａ側に切換え
る。従つてカウンタ３４の出力θjがＤ／Ａ変換器
３９に入力し、アナログ変換され、これは電圧−
周波数変換器４０に入力して周波数にされ、該周
波数はカウンタ４１で計数されて位置指令θrとな
る。一方、モータ４５の回転軸にはエンコーダ４
６が連結されており、カウンタ４７は該エンコー
ダ４６の出力パルスを計数して現在位置を示す出
力θ′iを生じる。加減算器４８はθr，θ′_iを受けて
そ
の差Δθを出力し、これはＤ／Ａ変換器４９によ
りアナログに変換される。Ｄ／Ａ変換器４９の出
力は位置誤差を示しており、これは閉ループ制御
回路５１に入力すると共に速度偏差推定回路５０
にも入力し、こゝで微分されて速度偏差となり該
制御回路５１に入力する。閉ループ制御回路５１
は速度制御モードでも位置制御モードでも位置偏
差θrに追従させる出力を生じ、加算器４３を通し
てこれを増幅器４４に加えてモータ４５を駆動さ
せる。該加算器４３へはＤ／Ａ変換器３９の出力
を微分回路４２で微分した加速度成分も入力して
おり、モータ４５はこの加速度成分によつて指令
された加速電流を供給される。

カウンタ３１が計数を続けてやがてθi＝θjにな
ると比較器３１はスイツチ３３を接点ｂに切換
え、カウンタ３４は計数を停止する。従つてθjは
一定となり、モータ４５は定速制御される。θiは
第２図で言えばV₁に相当し、ステツプ的に変化
する。この変化に応じて比較器３１はスイツチ３
３をθi＞θjならａへ、θi＝θlならｂへ、そしてθi
＜θjならｃへ切換え、ｃ位置ではカウンタ３４は
カウントダウンする。第２図ではステツプ的に変
化するV₁が指令速度としたが、第９図の回路で
はカウンタ３４等の働きによりこれを最大加減速
度で補正した（角をとつた）ものが指令速度とな
り、モータ４５は滑らかな加減速を行なうことが
できる。

やがてカウンタ４１の出力θrが目標位置θ_Bに近
ずいてくると差｜θ_B−θr｜が小になり、ROM３
６の出力θsは減少を始める。比較器３７は第７図
の場合は判定回路２０よりまた第８図の場合は判
定回路２３より切換指令Swが入るときスイツチ
３８を接点ｂ側に切換え、ROM３６の出力θsを
Ｄ／Ａ変換器３９へ導く。従つてモータ４５は
ROM３６が出力する減速曲線に従う位置制御を
受け、目標位置へ確実に停止する。

第８図はθ_B−θ（ｔ）が前記ｄ以下になつたと
きこれを判定回路２３で判定して切換信号Swを
出力する点を除いては第７図と同様である。

発明の効果 以上説明したことから明らかなように本発明に
よれば、関節座標型ロボツトを座標変換により見
かけと直角座標型ロボツトのように動かす場合に
おいても、各関節に対する目標角度（θ_B）にそれ
ぞれ、減速しながら位置決めさせることにより、
迅速でかつ高精度な位置決めが可能になる。ま
た、関節座標系と直角座標系それぞれに加減速制
御機構を設けたことにより、各関節の駆動部の出
力トルクが有限であることによる閉ループサーボ
系の飽和等による暴走の心配がなくなり、かつ、
目標位置近傍で全関節駆動部一斉に速度制御から
位置制御に切換えるようにしたので軌道精度の高
い運動制御が可能になる。

【図面の簡単な説明】

第１図〜第６図は関節型ロボツトの動作説明
図、第７図及び第８図は本発明の実施例を示すブ
ロツク図、第９図は第７図および第８図の一部の
詳細を示すブロツク図である。 図面で１１，１２，１４〜１６，２１，２２は
第１の装置、１３，１７〜１９は第２の装置、２
０，２３は切換判定装置である。

Claims (1)

1. 【特許請求の範囲】 １ 多関節ロボツトの構造から定まる関節座標系
   とは異なる座標系で表わされたロボツト目標位置
   データ及び移動前位置データを取込んで、所定の
   加減速度で時間変化する指示速度データを各サン
   プリングタイミング毎に出力する第１の装置と、 該指示速度データを入力されてそれを関節座標
   系の指示速度データに変換し、更に各関節の許容
   加減速度で時間変化する指示速度に変更してロボ
   ツト各関節駆動部へ出力し、かつ前記目標位置デ
   ータを関節座標系に変換して得たロボツト目標角
   度では角速度が零になる減速曲線を各関節毎に内
   蔵する第２の装置と、 ロボツトが目標位置に接近したとき、各関節駆
   動部を一斉に速度制御から前記減速曲線に従う位
   置制御に切換える信号を出力する切換判定装置と
   を備えることを特徴とする多関節ロボツトの制御
   装置。 ２ 切換判定装置が、関節座標系とは異なる座標
   系でのロボツト現在位置を取り込み、それが目標
   位置を中心とする所定半径の球内に入つたとき切
   換信号を出力するようにされてなることを特徴と
   する特許請求の範囲第１項記載の多関節ロボツト
   の制御装置。 ３ 切換判定装置が、関節座標系でのロボツト現
   在位置を取り込み、それが目標位置に対し所定値
   内に入つたとき切換信号を出力するようにされて
   なることを特徴とする特許請求の範囲第１項記載
   の多関節ロボツトの制御装置。