JPS6037009A - 多関節ロボツトの制御装置 - Google Patents

Abstract

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

Description

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

技術の背景 多関節型ロボットに作業をさせる際、人間に理解しやす
いあるいは外部環境と適合しやすいＸＹＺ直角座標系の
データで指示を与えることが一般的に行なわれている。

これに対して、多関節型ロボットはその構造上から来る
座標系（多関節ロボットは回動可能あるいは更に伸縮可
能な複数個の腕からなるから、該腕を座標軸にとった座
標系で、関節座標系と呼ばれる）で動作するから、直角
座標系−関節座標系への座標変換演算をリアルタイムで
行ないながらロボット各関節駆動部を動作させることに
なる。

第１図に示すようにＡ点からＢ点までロボットハンドＨ
の向きを一定にしたまま直線的軌道を描いて移動させる
場合で説明すると、まず直角座標系ｘｙｚ上でＡ点から
Ｂ点に向う線速度’ｓ　ｏ＋の大きさを各時点の位置に
応じた加減速制御を含めて決める。これをＸ（１１（ｉ
＝ｏ、１．２．・・・・・・）とすると、６１秒後（Δ
Ｔはサンプリングタイムで、座標変換時間などを考慮し
て定める）に位置すべき座標Ｘ（ｉｌｌ）は ×（ｉ＋　Ｉ）　＝　Ｘ　（１１＋　Ｘ　（１）・ΔＴ
　式＋１１としてめられる。これを座標変換して関節座
標系の座標θ（１伺）を得ると、ロボットの各関節に与
えるべき指示速度θ（１１は として計算できる。ここで、θ（１）′　はＸ　（１１
に対応する関節座標系上の点であるが、これは指示速度
ｌ１７（＋１を出力する直前においてロボットの各軸に
実装しているモータの回転角検知器により実測した値と
する。この実測値を用いる理由は特願昭５５−１８７８
８７で詳述しているが、次のような効果がある。即ち第
２図は１つの関節について指示速度Ｖ＋と実際の移動速
度■２を対比したものであるが、モータの出力トルクが
有限であるという物理的な制約のため、実際の速度Ｖ２
はΔＴ秒ごとにステップ状に更新されて行く指示速度■
１に完全には追いつけず、斜線を施した３角形の面積の
分だけ予定位置θ（ｉｌｌ）からずれることになる。

しかし追従差分法と呼ぶ式（２）の方法では、θ（１）
′に実測値を用いているため次の時点の指示速度ではこ
のずれが自動的に補正されていく形になっている。なお
後述のように指示速度をＶ＋の如くステップ状に変化さ
せずｖ２のように傾斜させると斜線部分が僅少になり、
ロボット各関節駆動部が過大入力を受けて飽和するなど
の問題がなくなって、ロボットの運動は円滑になりかつ
各関節駆動部の利得を上げて高精度な制御を行なうこと
ができる。

従来技術と問題点 このようにして第１図のような直線的軌跡を描いてＢ点
に接近することができるようになったが、Ｂ点にビタリ
位置決めするにはまだ問題が残る。

というのは、第２図のように指示速度と実際の速度が完
全には一致しないことにより、たとえＢ点において駆動
位庫Ｘｉ＋１従ってθ、＋１が該目標位置Ｂに一致する
ように、式（２）により々ｉを計算してこれを指示速度
としても、へＴ秒後に必らずしも正確に該Ｂ点に到着は
しない。一般にはこの時点で各軸は該ｂｉなる速度を持
っているので、次の６１秒後にはＢ点を通り越してしま
い、実測した現在位置Ｘｉによる戻り制御が行なわれ、
ロボットハンドはＢ点の周りで振動もしくはループを画
きなから該Ｂ点に接近することになる。しかもやがＣは
完全にＢ点に到達できるかと言うとそうではなく、指示
速度の量子化誤差と指示速度を更新する時間々隔の粗さ
のために、永久にＢ点の周りに振動することもあるとい
う問題がある。前述のように直角座標系での線速度’ｓ
　ｔｏは加減速を考１、ｆ、　ｌ、て定めるので目標位
置近傍では小になっているが、関節座標系は直角座標系
に対して比例関係にある訳ではないから、関節座標系に
変換するとそ０月は示速度は目標位置近傍でも大きいこ
とが有りｉυ、これが上記の問題を惹き起し易い。

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

直角座標系と関節座標系の２つのレベルでＢ点に接近す
るにつれて減速させることは一見２度手間のように感じ
られるが、■直角座標系のレベルのみではΔＴが大きく
、粗い制御になるので迅速かつ高精度な位置決めが困難
、■直角座標系上で計画された軌道に従がいながら第１
図Ｂ点に向って減速して行く時、関節座標系レベルのみ
で減速動作させると、各関節固有の加速度で減速される
ため、ロボットハンドは前記計画された軌道上を正確に
通らなくなる。■直角座標系−関節座標系の変換は非線
形であるため前述のように、ある関節が伸びきる姿勢で
は、直角座標系では微小量の変位でも関節座標系上では
速度、加速度が無限大になる（これは特異点と呼ばれる
）ことがある。

各関節の駆動部は勿論無限大の速度、加速度には追従で
きず、パルスモータ等を使用した時は脱調するし、直流
モータで閉ループ制御する時も制御回路が飽和して暴走
の危険がある。したがって、直角座標系のレベルだけの
加、減速制御では保安上においても問題があり、関節座
標レベルでの加減速制御をしないと、特異点を遠く避け
て動かすなどの処理をしなければならない、等、多関節
型ロボット特有の事情があるので、上記２重減速はけっ
して無駄ではない。

２重減速を行なうには、第１図Ｂ点における各関節の座
標データ（各関節角度の目標値）を、各関節ごとに実装
した制御回路に予め設定してお（。

これらの各関節の制御回路は、上記各関節の目標角度（
目標位置）を原点とする減速曲線を発生し、各関節の回
転速度をこの減速曲線に従って減速しながらロボットハ
ンドを目標角度に導き、そこに位置決めする様構成する
。

しかしながらこ＼でまた新たな問題が出てくる。

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

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

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

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

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

ごごで、半径ｒ又は値ｄを十分小さく選べば第３図でＣ
点からＢ点まで円弧を描いて運動することがなくなる。

また、Ｃ点とＢ点が第４図のように接近する場合にはそ
の恐れはあるが、短がい円弧なのでほぼ直線に近似する
ことができるため、実用上問題はない。

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

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

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

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

ロボットば６自由度の多関節型ロボットとすると、直角
座標系でのロボットハンドの位置ベクトルＸは Ｘ−（ｘ、ｙ、ｚ、α、β、γ） で表わされる。即ちロボットを取り付けた台に原点と座
標軸を設定し、ハンドの位置をｘ、ｙ、ｚでまた向きを
α、β、γ（オイラー角）で表わす。

そして、ｘＡは移動前のハンドの座標、ＸＢは次の移動
により位置決めすべきハンドの目標位置、Ｘ（１）は移
動途中の離散時間関数としてのハンドの座標、Ｘ（ｉｌ
ｌ）はそれから６１秒後のハンドの座標である。なおベ
クトル記号・はこ−では適宜省略する。一方、関節座標
系でのロボットハンドの位置ベクトルθは θ＝（θ１．θ２．θ３．θ４．θ５．θ６）で表わさ
れ、θ■〜θ６はロボットの各関節の回転角である。こ
−ではロボットの各腕は伸縮しないとしている。そして
、θＡは移動開始前のハンドの回転角度、θＢは次の移
動により位置決めすべき目標角度、θ（１）は移動途中
の離散時間関数としての角度、θ（ｉｌｌ）はその６１
秒後に位置すべき角度、θ′（１）は実際の回転角を実
測した角度、θｆｔ＋は連続時間関数としてロボットに
指令する角度である。

各ブロックの説明をすると、１１は移動長演算部で移動
前後のハンドの座標ＸＡ、ＸＢより移動長（点Ａ、Ｂ間
の直線距離）ＳＢを算出する。ＳＲ”’　ｌ　ＸＢ　Ｘ
ｐ、　ｌは として計算する。ここで、ハンドの向きを表わすｔｒ、
β、Ｔは単位がラジアン（ｒａｄ　）であるので、’ｌ
’ｉＭ　１　ｍを掛けて長さとする。なお一般的には任
意の半径が設定できるが、ここでは簡単のため１ｍとし
た。１２は単位方向ベクトルｅの演算部で（ＸＢ　ＸＡ
）／　ｌ　ＸＢ　ＸＡ　ｌとしＴＡ点よりＢ点へ向うベ
クトルの単位ベクトルｅを算出する。

１３は座標変換部で、直角座標での目標位置ベクトルｘ
Ｂを関節座標での目標位置ベクトルθＢに変換する。１
４は線速制御部で、横軸に移動距離、縦軸に線速度をと
って最大値をｓＭに制限した例えば図示の如き台形の移
動距離−線速度特性を持つ線速度＆（１）を出力する。

＆（１）は移動途中のハンド特にその先端部の線速度の
大きさを指令する。

この値は、もしハンドの向きに変化がない時にはハンド
それ自体の移動速度を示し、ハンドの位ｆｉｆｆｉ（回
動軸）が固定され向きのみが変わる時にはそのハンド位
置からハンドの向きに１ｍ延ばした点の周速度の大きさ
を示す。ハンドの位置と方向（向き）が同時に変わる時
はこれらの合成速度である。１５は移動途中のハンドの
線速度ベクトルＸ　ｆｉｌをｅ　−Ｓ（すとして算出す
る演算部である。なお単位ベクトルｅは、直線運動では
、その１回の移動の間は一定値である。１６は６１秒後
のハンド座標ベクトルＸ（ｉｌｌ）をＸ　（１１十交（
１１・ΔＴとしてめる演算部、１７は直角座標χ（１＋
すを関節座標θ（ｉｌｌ）に変換する座標変換部、１８
は指示（角）速度演算部で前記式（２）より指示速度会
（１）を算出する。１９はサーボ制御回路で詳細は後述
するが、要はδ（１１を受けてロボットＲＢに指令角度
θ（１）を与える。

２０は速度制御モードから位置決め制御モードへの切換
えタイミングの判定回路、そして２１はこの判定のため
の一方の入力である現時点までの移動距離Ｓ　（１１を
計算する演算回路である。

動作を説明するに、目標位置ｘＢが入力されると演算部
１１で次の移動の総線長ＳＢをまた演算部１２で移動の
方向ベクトルｅを計算する。また変換部１３では座標変
換ベクトルＡにより〆ＢをθＢに変換し、θＢを各軸の
サーボ制御回路１９に目標角度として設定する。移動開
始は、目標位置ＸＢが入力して、総線長ＳＢおよび現時
点までの移動距ｍ　Ｓ　ｆｉｌにより制御部１４が線速
度５（Ｏ）を０でない小さな値として出力することによ
り開始され、この５（０）即ちｉ＝０のときのζ（１１
（−々説明しないが他も同様）は演算部１５により速度
ベクトルＸ（０）に分解され、演算部１６で６１秒後の
座標Ｘ（１）をＸ　（０）　＋Ｘ　（０）　・ΔＴとし
て計算する。ここで、Ｘ（０）は移動前の座標ＸＡとす
る。その次の変換部７では座標変換ベクトルＡによりや
はり６１秒後に位置すべき関節座標としてθ（１）を計
算する。さらに演算部１８では現時点での各関節の角度
センサによる実測値θ’（０）（これも移動前だからθ
Ａと等しい）を用いてΔＴ秒間の各関節角の平均角速度
θ　（０）を計算して制御回路１９に出力する。なお変
換部１７と演算部１８との間には特願昭５６−１０１８
５２で説明しているように軌跡補間部を設けることがあ
る。これは、直角座標から関節座標への変換には長い演
算時間を必要とし、このため直角座標系での指示値Ｘ（
ｎ＋）は飛び飛びの値でしか出せない（サンプリングタ
イムが大）が、これではロボットの運動が円滑を欠く。

そこで補間して飛び飛びの指示値Ｘ（ｉ＋１）の間に更
に多数の指示値を発生するとロボットの運動を円滑化す
ることができる。制御回路１９ではθ（０）まで第２図
の■２のように滑らかに加速しながらロボットの各関節
を駆動するための連続な関数θ（ｔｌを生成し、これを
ロボットＲＢに加える。ｉを６１秒ごとに１づつ増しな
がら、演算部１４からの指令’Ｓ　（１１を最大値ｓＭ
になるまで増大させて行くと、前記と同様な手順で各演
算部等は出力を生じロボットの各関節の回転が制御され
、ロボットハンドの動きがこれに従って加速されていく
。そして’ｓ　ｏ＋がｓＭに達すると以降はゐ（１１＝
　Ｓ　Ｍとなり、各演算部等はこれに準じた動作を行な
う。

演算部１４においてはＳ　（Ｉｆ　＝　Ｓ　Ｂのとき＆
（１）＃０となるような減速曲線部ｆ　（ＳＢ−３（１
１）　（第７図の台形の下降部分）を備えており、ハン
ドの位置がｘＢに近づいてｓｏ）とｔ　（ＳＢ−３（１
１）　が等しくなると、それ以降はｆ　（ＳＢ−３（１
１）を邑（１１とし、減速指示に移る。そして、さらに
ｘＢに接近して５Ｂ−３（１１が位置決め制御モードへ
の切り替え判定値ｒよりも小さくなると判定回路２０は
制御回路１９に対して速度制御モードから位置決め制御
モードへの切り替え指令Ｓｗを出力する。

すると制御回路１９では各軸それぞれについて第６図の
ような減速曲線りを設定（有効に）し、各関節の速度が
この減速曲線に出会った時、以降、この減速曲線に従が
って減速して行き、θＢに到ｉ！すると、これでＡ点か
らＢ点への位置決め動作が完ｒする。

サーボ制御回路１９は１軸分を示すと第９図の如くなっ
ている。これは特開昭５６’−２２１０６で詳述されて
いるが、概要は次の如（である。４５はロボットの１軸
を駆動するモータ、４４はその駆動用の増幅器である。

目標角度θ８が与えられまた前述の演算器１８から速度
指令θｉが与えられると、カウンタ３４の計数値θｊは
この時点では０であるので比較器３１はθｉ〉θｊに従
ってスイッチ３３を接点ａ側に切換え、カウンタ３４は
発振器３２の出力パルスをカウントアツプする。発振器
３２の発振周波数はサーボ系がとり得る最大加速度に対
応する値を持ち、゛従ってこれを計数するカウンタ３４
の出力θｊは該最大加速度で加速したときの速度時間関
数を示す。勿論、最大加速度以下なら支障はないから発
振周波数はそれ以下でもよ（、また制御部１４が出力す
る速度指令に対応できるよう該制御部１４が制限する加
減速度以上の値にする。カウンタ４１の出力θｒは現時
点では前記θ八と等しく、１θＢ−θＡ１がＲＯＭ３６
に加わる。ＲＯＭ３６は前述の減速曲線１〕を出力する
もので、１θＢ−θｒ１をアトトスとしてその平方根を
出力する。現時点ではこの出力θＳは大きいから、比較
器３７はθＢ〉θｊに従ってスイッチ３８をａ側に切換
える。従ってカウンタ３４の出力θｊがＤ／Ａ変換器３
９に入力し、アナログに変換され、これは電圧−周波数
変換器４０に入力して周波数にされ、該周波数はカウン
タ４１で計数されて位置指令θｒとなる。

一方、モータ４５の回転軸にはエンコーダ４６が連結さ
れており、カウンタ４７は該エンコーダ４６の出力パル
スを計数して現在位置を示す出力θ′ｉを生じる。加減
算器４８はθｒ、θ′１を受けてその差へ〇を出力し、
これはＤ／Ａ変換器４９によりアナログに変換される。

Ｄ／Ａ変換器４９の出力は位置誤差を示しており、これ
は閉ループ制御回路５１に入力すると共に速度偏差推定
回路５０にも入力し、こ〜で微分されて速度偏差となり
該制御回路５１に入力する。閉ループ制御回路５１は速
度制御モードでも位置制御モードでも位置偏差θｒに追
従させる出力を生じ、加算器４３を通してこれを増幅器
４４に加えてモータ４５を駆動させる。該加算器４３へ
はＤ／Ａ変換器３９の出力を微分回路４２で微分した加
速度成分も入力しており、モータ４５はこの加速度成分
によって指令された加速電流を供給される。

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

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

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

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

【図面の簡単な説明】

第１図〜第６図は関節型ロボットの動作説明図、第７図
及び第８図は本発明の実施例を示すブロック図、第９図
は第７図および第８図の一部の詳細を示すブロック図で
ある。 図面で１１．１２．１４〜１６，２１．２２は第１の装
置、１３．１７〜１９は第２の装置、２０．２３は切換
判定装置である。 出願人　富士通株式会社 代理人弁理士　青　柳　稔 第１図 Ｂ 第２図 一△Ｔ− 第４図 第５図 第６図

Claims (1)

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