JPS6314365B2

JPS6314365B2 -

Info

Publication number: JPS6314365B2
Application number: JP9544881A
Authority: JP
Inventors: Susumu Kawakami; Takashi Uchama
Original assignee: Fujitsu Ltd
Current assignee: Fujitsu Ltd
Priority date: 1981-06-20
Filing date: 1981-06-20
Publication date: 1988-03-30
Also published as: JPS57211490A

Description

【発明の詳細な説明】本発明はロボツトの軌道制御方式に係り、とく
に教示された３次元空間の各点近傍でなめらかな
円周移動を行なわせるとともに、隣接する内接円
と直線の接点が相互に重なり合わないようにした
ロボツトの軌道制御方式に関するものである。

従来、ロボツトのアームが障害物を回避しつつ
目的地点に到達する場合、第１図に示すように教
示ないしロボツト言語により指定された３次元空
間の各位置Ｒ→₁〜Ｒ→_Nを順次通過しながら目的地点
に到達していた。ここで、各位置Ｒ→₁〜Ｒ→_Nを通過
する際、連続的に通過することは困難であり、各
指定点間で、加速―減速―停止を実施しなくては
ならなかつた。このため、多数の指定点を経由す
る移動では時間がかかり、また運動形態も一様で
ないため、加速減速が大きく、アームが振動し易
く、好ましい運動性能を得るに支障を来すという
欠点があつた。

これに対し、本出願人は特願昭55−171722号
「ロボツトの軌道制御方式」を提案した。詳細は
実施例で後述するように、ロボツトの被制御体を
あらかじめ指定された複数の指定経由位置間を一
定の線速度で移動させ、該指定経由位置近傍では
該位置とあらかじめ設定された該位置の近傍通過
誤差量とで決まる内接円上を前記線速度と同一の
円周速度で移動させるように軌道制御したもので
ある。これにより、ロボツトのアーム移動の指定
された各経由点通過の厳密性を緩和し、かつその
緩和量すなわち、近傍通過誤差量をオペレータが
任意に指定できるようにすることにより、各経由
点ごとに加速―減速―停止することなく、なめら
かな速度で経由点を連続的に移動できるものであ
る。

しかし、この方式の問題点の１つは隣接する内
接円から直線に、直線から内接円に切換える接点
が重なり合うことがあり、なめらかな円移動に不
都合を生ずることである。

本発明の目的は３次元空間の各点近傍でなめら
かな円周移動を行なわせるとともに、隣接する内
接円と直線の接点が相互に重なり合わないように
したロボツトの軌道制御方式を提供することであ
る。

前記目的を達成するため、本発明のロボツトの
軌道制御方式はロボツトの被制御体をあらかじめ
指定された複数の指定経由位置間にて一定の線速
度で移動させ、該指定経由位置近傍では該指定経
由位置とあらかじめ設定された該指定経由位置の
近傍通過誤差量とで決まる内接円上を移動させる
ように軌道を制御する方式において、隣接する内
接円から直線に、および直線から内接円に前記軌
道を切換える接点が重なり合わないように前記近
傍通過誤差量を修正した内接円上を移動させるよ
うにしたことを特徴とするものである。

以下本発明を実施例につき詳述する。

最初に本発明の基本構成として、前述の特願昭
55−171722号「ロボツトの軌道制御方式」を用い
るものとし、第２図〜第１１図は提案例の動作と
構成を示し、第１２図以下に本発明の要部が提案
例と異なる点につき説明する。

第２図は上記提案例の軌道説明図である。ま
ず、同図により、ロボツトのアームが動作する上
での準備すべきパラメータについて説明する。こ
れらのパラメータの相互関係を明確にする上での
ポイントとしては、次の３点がある。

(I) 位置Ｒ→_i-1、Ｒ→ｉ、Ｒ→_i+1のそれぞれの間を
ア
ームが速度Ｖで直線移動するとすれば、各位置
近傍内接円の円周上においても同じ値の円周速
度でなめらかに移動するものであること。

() オペレータが作業内容および精度に応じて
近傍通過誤差量△ｉを任意に与えることがで
き、移動品位を把握しながら作業を計画するこ
とができるものであること。

() 位置Ｒ→_i-1、Ｒ→_i、Ｒ→_i+1によつて平面が構
成さ
れ、かつ上記操作はその平面内で記述できるこ
とに着目し、３次元空間内ででの複雑な手続を
簡明にできるものであること。

上記３つのポイントを基本にして、各パラメー
ターの内容を詳細に説明する。

ロボツトのアームの移動のスタート位置から目
標位置までの間にアームが通過する３次元空間の
指定位置Ｒ→₁〜Ｒ→_Nのうちの任意の位置をＲ→_iとし
、
その前後の指定位置をＲ→_i-1、Ｒ→_i+1とする。Ｒ→_i
と
Ｒ→_i-1との間を結ぶ直線の方向単位ベクトルをｅ→_i-
_１、Ｒ→_iとＲ→_i+1との間を結ぶ直線の方向単位ベクト
ルをｅ→_iとすれば、ｅ→_i-1、ｅ→_iはそれぞれ下記式
に
よつて与えられる。

ｅ→_i-1＝（Ｒ→_i−Ｒ→_i-1）／｜Ｒ→_i−Ｒ→_i-1｜
…(1) ｅ→_i＝（Ｒ→_i+1−Ｒ→_i）／｜Ｒ→_i+1−Ｒ→_i｜…(
2) 上記２つの単位ベクトルｅ→_i、ｅ→_i-1によつて決
まる内積角をπ−2θ_iとすれば、θ_iは次式で得られ
る。

θ_i＝｛π−cos^-1（ｅ→_i-1・ｅ→_i）｝／２ …(3) このθ_iは、位置Ｒ→_iでの直線移動の方向変化角を
二等分したものである。この直線移動の方向角の
二等分線が位置Ｒ→_iの内接円と交わる交点と位置
Ｒ→_iとの間の距離を△_iとすれば、この△_iはオペレ
ータが任意に決められる近傍通過誤差量である。

アームの移動が直線運動から円運動に切換わる
位置をＯ→_iとし、位置Ｒ→_iと位置Ｏ→_iとの間の距離
を
L_iとすれば、この距離L_iは前記二等分角θ_iと近傍
通過誤差量△_iとにより次式より一義的に決まる。

L_i＝△_i・cosθ_i／（１−sinθ_i） …(4) さらに、前記位置Ｏ→_iと、アームの移動が円運
動から直線運動に切換る位置Ｐ→_i-1とは、位置Ｒ→_i、
距離L_i、単位ベクトルｅ→_i-1によつて次式から得ら
れる。

Ｏ→_i＝Ｒ→_i−L_i・ｅ→_i-1 …(5) Ｐ→_i-1＝Ｒ→_i-1＋L_i-1・ｅ→_i-1 …(6) ここで、添字ｉ−１は、前位置Ｒ→_i-1によつて
決まるパラメータに付けられたものである。

また、内接円の半径D_iは次式より得られる。

D_i＝L_i・tanθ_i …(7) 直線運動時の速度をＶとすれば、これが円運動
になつて円周速度の変化がないように、円運動の
角速度を前記半径D_iと速度Ｖとにより次式から決
定する。

ω_i＝φ_i＝Ｖ／D_i …(8) φ_iは円周移動の変位角である。

さらに、円周移動する面を定めるベクトルβ→_i
は、直線方向単位ベクトルｅ→_i、ｅ→_i-1と、二等分
角θ_iとによつて次のように決められる。

β→_i＝｛ｅ→_i-1−ｅ→_i・cos2θ_i｝／｜sin2θ_i｜
…(9) 上記単位ベクトルβ→_iは、単位ベクトルｅ→_i、ｅ→
_i
_−１および位置Ｒ→_iを含む面上にあり、単位ベクト
ルｅ→_i-1に直交するベクトルである。

Ｐ→_i-1からＯ→_iへの直線移動距離l_iとすれば、距離
l_iは、直線上の単位距離あたりの移動時間△t_sと、
単位移動のステツプ数Ｓ、速度Ｖによつて決めら
れ、また移動変位角φ_iは円周上の単位距離あたり
の移動時間△t_cと、単位移動のステツプ数Ｃ、角
速度ω_iによつて決められる。すなわち l_i-1＝Ｖ・△t_s・Ｓ …(10) φ_i＝ω_i・△t_c・Ｃ …(11) である。

さらに、直線／円アームの移動位置をｒ→_i-1お
よびｒ→_iとすれば、直線運動上の該移動位置は、
位置Ｐ→_i-1、距離l_i-1、単位ベクトルｅ→_i-1によつて
決まり、円周運動上の該移動位置は位置Ｏ→_i、半
径D_i、移動φ_i、単位ベクトルｅ→_i-1，β→_iによつて
決まる。すなわち、ｒ→_i-1＝Ｐ→_i-1＋l_i-1・ｅ→_i-1 …(12) ｒ→_i＝Ｏ→_i＋D_i｛sinφ_i・ｅ→_i-1 ＋（１−cosφ_i）β→_i｝ …(13) である。

以上により、第２図に示されたアームが動作す
る上での準備すべきパラメータのすべてについて
の相互の関係が明らかになつた。

次に上記パラメータをもとにロボツトのアーム
が移動する動作内容を第３図ａ〜ｄのフローチヤ
ートにより説明する。

まず、オペレータが決めるべきパラメータとし
ては、ステツプ(A)に示されるように３次元空間の
位置Ｒ→_p…Ｒ→_i…Ｒ→_N、近傍通過誤差量△₁…△ｉ…
△_N-1、直線運動Ｖ、直線および円の単位移動時
間の△t_sおよび△t_cがある。近傍通過誤差量△_i
は、各位置共通の値にしてもよい。これらのパラ
メータが与えられることにより、ロボツトのアー
ムの移動が始まる。ステツプ(B)でスタートし、ま
ず第１ステツプｉ＝１(C)で前述した様々なパラメ
ータを準備する。

直線移動をするための単位方向ベクトルｅ→_i-1、
ｅ→_iがステツプ(D)により決まる。直線移動の方向
変位角θ_iがステツプ(E)により決まる。直線から円
あるいはその逆の切り換えに必要な切り換え点Ｏ→
_ｉ，Ｐ→_iおよび位置Ｒ→_iと切換点Ｏ→_i，Ｐ→_iとの間
の距離
L_iがステツプ(F)により決まる。内接円の半径D_iは
ステツプ（Ｇ）により決まる。角速度ω_iはステツ
プ（Ｈ）により決まる。円周移動する面を定める
ベクトルはステツプ（Ｉ）により決まる。このよ
うにして必要なパラメータが決定される。

次に、直線移動のステツプがスタートする。

まず、スタート地点、目標地点の設定を行な
う。ｉ＝１の時｛ステツプ（Ｊ）｝Ｐ→_i-1＝Ｒ→_pに置
換える｛ステツプ（Ｋ）｝。ｉ≠１であれば次のス
テツプへ行く。ｉ＝Ｎ−１の時｛ステツプ
（Ｌ）｝、Ｏ→_i＝Ｒ→_Nに置換える｛ステツプ（Ｍ）｝
。
次にステツプＳ＝１｛ステツプ（Ｎ）｝とする。位
置Ｐ→_iとＯ→_iとの間の直線移動距離l_imaxはステツプ
（Ｏ）で決まる。この移動距離中各ステツプ数Ｓ
でのスタート地点からの長さl_i-1は各ステツプ
（Ｐ）で決まる。移動距離の各ステツプの位置ｒ→_i
_−１はステツプ（Ｑ）で決まる。このように決めら
れた移動位置ｒ→_iへロボツトのアームはサーボ機
能により移動していく｛ステツプ（Ｒ）｝。移動距
離l_i-1が直線最大移動距離l_imaxよりも小さい間
｛ステツプ（Ｓ）｝はステツプ数Ｓを一段ずつ進め
て｛ステツプ（Ｔ）｝、ステツプ（Ｐ）からステツ
プ（Ｓ）までのステツプが繰返えされる。l_i-1がl_i
maxに等しくなると直線運動は終り円運動へ移
る。ただしここでｉ＝Ｎ−１の時｛ステツプ
（Ｕ）｝には目標地点に到達しているので移動は完
了し“OWARI”の信号が出る。

まず、円運動のステツプ数Ｃ＝１が設定される
｛ステツプ（Ｖ）｝。移動変位角φ_iはステツプ（Ｗ）
で決まる。内接円上の移動位置ｒ→_iはステツプ
（Ｘ）で決まる。このようにして決められた移動
位置ｒ→_iへロボツトのアームはサーボ機能により
移動していく。

｛ステツプ（Ｙ）｝移動変位角φ_iが単位ベクト
ルの内積角π−2θ_iよりも小さい間｛ステツプ
（Ｚ）｝はステツプ数Ｃを一段ずつ進めて｛ステツ
プ（ZA）｝、ステツプ（Ｗ）からステツプ（Ｚ）
までのステツプが繰返えされる。移動変位角φ_iが
π−2θ_iに等しくなると円運動は完了し、次のス
テツプの直線運動に移る。次のステツプではｉ＝
ｉ＋１に変えられて｛ステツプ（ZB）｝、ステツ
プ(D)へ戻る。

以上により、アームの動作内容が明らかになつ
た。

次に、これらの動作を実際に制御するための制
御ブロツク系統図について説明する。

第４図は制御システムの全体図である。計算機
などの外部制御装置よりのスタート信号が読み出
し装置（アドレスカウンタ）１へ入ると、アドレ
スカウンタ１は初期状態にリセツトされアドレス
カウンタ１が１に設定される。メモリ２には、各
アドレスに、第５図に示されたオペレーターによ
つて設定される第１のパラメーターが記憶されて
いる。これらの第１のパラメーターは、座標変換
回路３へ転送される。この座標変換回路３内で
は、第１のパラメーターをもとに直線および円移
動を実行するに必要な第２の準備パラメーターを
得るための演算を行なう。これらのパラメーター
は、直線移動軌跡出力回路４および円移動軌跡出
力回路５へ入力される。直線移動軌跡出力回路４
では、上記パラメーターをもとに直線移動距離と
３次元空間移動位置を決定してロボツトサーボ駆
動回路６へこれらデータを送り出す。円移動軌跡
出力回路５では、上記パラメーターをもとに移動
変位角と３次元空間移動位置を決定してロボツト
サーボ駆動回路６へこれらデータを送り出す。ロ
ボツトサーボ駆動回路６は、直線移動軌跡出力回
路４と円移動軌跡出力回路５からのデータを受け
てロボツトの被制御体を駆動する。ここで、直線
移動と円移動との切換えは、状態判定回路７によ
つて決定される。以上が制御システムの概要であ
る。なお、第４図で実線の矢印は制御信号の流れ
を示し、斜線の入つた太い矢印はデータの流れを
示す。

次に、各ブロツクの詳細について説明する。前
述のようにメモリ２には、第５図に示された内容
が記憶されている。ＯからＮ番地のアドレスに
は、３次元空間の指定位置Ｒ→_O〜Ｒ→_N、２からＮ番
地のアドレスには近傍通過誤差量△₁〜△_N-1、０
番地に速度Ｖ、直線単位移動時間△t_s、円周単位
移動時間△t_cが記憶されている。

次のブロツクの座標変換回路３の内容は、第６
図に描かれている。まず、メモリ２から入力用バ
ツフアメモリ８へアドレスカウンタ１からのアド
レスの指定を受けて必要なデータが入力される。
たとえば第７図のＩに示されるように、アドレス
カウンタ１のカウントがｉの時、メモリ２のｉ−
２番地の内容である位置Ｒ→_i-2の情報が入力用バ
ツフアメモリ８のアドレスＩ３からＩ５へ、ｉ−
１番地にある位置Ｒ→_i-1の情報がＩ６からＩ８へ、
ｉ番地にある位置Ｒ→_iの情報がＩ９からＩ１１へ、
ｉ＋１番地にある位置Ｒ→_i+1の情報がＩ１２から
Ｉ１４へ、近傍通過誤差量△_iがＩ１５へそれぞ
れ入力される。ここでｉ番地の計算において、位
置ベクトルＲ→_i-2までが必要である理由は、とく
に切換点Ｐ→_i-1を求める前述した(6)式において、
距離L_i-1を求めるためには(4)式よりθ_i-1が必要で
あり、さらにθ_i-1を得るためには(3)式よりｅ→_i-2が
必要であり、ｅ→_i-2を得るために(1)式よりＲ→_i-2が
必要なためである。ただしｉ＝１の時、ｉ−１す
なわち０番地の内容である速度ＶがＩ０へ、直線
単位移動時間△t_sがＩ１へ、円周単位移動時間△
t_cがＩ２へ入力される。このようにして、次の変
換手順メモリ９の演算に必要なパラメーターが準
備される。

変換手順メモリ９では入力用バツフアメモリ８
から必要なデータを呼出し、さらに演算子の種類
に応じて演算器選択回路１０を経由して、演算器
収納部１１から所望の演算器を選定し、計算を行
ない、結果を中間結果用バツフアメモリ１２ある
いは出力用バツフアメモリ１３へ格納する。これ
ら変換手順メモリ９、中間結果用バツフアメモリ
１２、出力用バツフアメモリ１３の相互の機能的
な関係について、第７図、第８図により具体例を
基に説明する。

たとえば、(1)式の方向単位ベクトルｅ→_i＝（Ｒ→_i
−
Ｒ→_i-1）／｜Ｒ→_i−Ｒ→_i-1｜を求めることを考える
。
第８図に示されるように、変換手順メモリ９のア
ドレスＣ０には減算の演算子と、必要な入力バツ
フアメモリ８内のデータ（Ｒ→_i）ｘ成分、（Ｒ→_i-1）
ｘ成分の格納されているアドレスＩ９とＩ６が入
力されている。Ｉ９とＩ６には、位置ベクトルＲ→
_ｉ、Ｒ→_i-1のｘ成分が入つている。これらの必要情
報がアドレスＣ０内のデータにより呼び出されて
も必要な演算が実施されて、その出力は中間結果
用バツフアメモリ１２のアドレスＭ０へ入力され
る。これが、第７図のに示されるT_1Xである。
同様にして、アドレスＣ１からｙ成分の演算結果
が、アドレスＣ２からｚ成分の演算結果がそれぞ
れ出力されて、中間結果用バツフアメモリ１２の
アドレスＭ１，Ｍ２へそれぞれT_1y，T_1zの形で
入力される。次に、中間結果用バツフアメモリ１
２のアドレスＭ０からのデータが変換手順メモリ
Ｃ３の指示により呼出されてさらに乗算の演算子
の指示により乗算器が選択されて、演算が実施さ
れてx²のデータがアドレスＭ３へT_2Xとして入力
される。同様にして、アドレスＣ４，Ｃ５より
y²，z²のデータが出力されて、アドレスＭ４，Ｍ
５へT_2y，T_2zとして入力される。さらにアドレ
スＭ３，Ｍ４のデータは、アドレスＣ６に加算さ
れてアドレスＭ６へ入れられ、アドレスＭ５，Ｍ
６のデータはＣ７にて加算されてx²＋y²＋z²とし
てアドレスＭ７へ入れられ、アドレスＭ７のデー
タはアドレスＣ８にて平方根を求めアドレスＭ８
へＷ３として入力される。次に、アドレスＣ９に
おいてアドレスＭ０，Ｍ８のデータが呼出されて
除算されて、その結果は出力用バツフアメモリ１
３のアドレス０18へ入力される。アドレスＣ１
０，Ｃ１１からの出力も同様にして、それぞれア
ドレス０19、０20へ入力される。このようにし
て、方向単位ベクトルｅ→_iの情報が出力用バツフ
アメモリ１３のアドレス０18〜０20へ入力される
のである。その他の準備パラメータも同様の手順
によつて得られ、第７図に示されるように出力
用バツフアメモリ１３の各アドレスへ入力され
る。以上が座標変換回路３の内容である。

次に、状態判定回路７について第９図により説
明する。アドレスカウンタ１の状態がｉであると
すると、ｉ＝１判定回路１４において、ｉ＝１か
否かを判定し、ｉ＝１であれば、第３図のフロー
チヤートのＫのように書換えをする指令を出す。
すなわち、書換え指令回路１５により、たとえば
アドレスＩ６，Ｉ７，Ｉ８にあるｘ、ｙ、ｚ成分
がアドレス０７、０８、０９へ書き換えられる。
また、同時にｉ＝１であるという信号は、直線移
動開始信号出力回路１６へ入れられ直線移動開始
信号出力回路１６はこの信号を受けて、直線移動
軌跡出力回路４へ駆動指令を出す。ｉ≠１の場合
は次のｉ＝Ｎ−１判定回路１７へアドレスカウン
タ１の信号が入る。ここでｉ＝Ｎ−１であれば、
第３図のＭのように書換えをする指令を出す。す
なわち、書換え指令回路１８によりたとえばアド
レスＩ１２，Ｉ１３，Ｉ１４にあるｘ、ｙ、ｚ成
分がアドレス０４、０５、０６へ書換えらる。ま
た、同時にｉ＝Ｎ−１という信号は、移動完了信
号として出力される。アドレスカウンタ１の信号
がｉ≠１かつｉ≠Ｎ−１の場合は、該信号は円移
動開始信号出力回路１９へ入れられ、円移動開始
信号出力回路１９はこの信号を受けて、円移動軌
跡出力回路５へ駆動指令を出す。このようにし
て、状態判定回路７からの指令により直線移動軌
跡出力回路４および円移動軌跡出力回路５が駆動
する。なお、直線移動が完了すると、直線移動軌
跡出力回路４より直線移動完了信号が出力されｉ
＝Ｎ−１判定回路１７へ入力される。該信号がｉ
＝Ｎ−１に等価である場合には、ｉ＝Ｎ−１判定
回路１７より移動完了信号が出力され、該信号が
ｉ≠Ｎ−１の場合には円移動開始信号出力回路１
９へ信号が入れられるのは、前述した通りであ
る。また、円移動が完了すると、円移動軌跡出力
回路５により、円移動完了信号が出力され直線移
動開始信号出力回路１６入力される。

次に、第１０図に基づいて直線移動軌跡出力回
路４の内容について説明する。状態判定回路７よ
り出力された直線移動開始信号は、ラツチ回路２
０によりラツチされ、アツプカウンタ２１をリセ
ツトし、上限移動長決定回路２２を駆動する。

直線移動用クロツク発振器２３からのクロツク
信号は、ラツチされた直線移動開始信号によりア
ツプカウンタ２１へ入れられる。一方、上限移動
長決定回路２２により、直線移動最大距離l_imax
が決定された後、移動距離決定回路２４は、アツ
プカウンタ２１の状態に応じて、直線移動距離
l_i-1のデータを、クロツク周期△t_sごとに離散的
に３次元位置決定回路２５およびl_i-1＜l_imax判定
回路２６へ出力する。３次元位置決定回路２５は
直線移動距離l_i-1のデータに基づき３次元空間の
移動位置ｒ→_i-1を演算して決め、ロボツトサーボ
駆動回路６へその結果を送出し、ロボツトサーボ
駆動回路６はこのデータによりロボツトを駆動す
る。l_i-1＜l_imax判定回路２６では直線移動距離
l_i-1が直線移動最大距離l_imaxを超えたかどうか判
定し、超えた場合には状態判定回路３へ直線移動
終了信号を出し、ラツチ回路２０をリセツトす
る。このようにして直線移動軌跡出力回路４によ
り３次元移動位置ｒ→_i-1が決められロボツトサー
ボ駆動回路６を駆動する。

次に、円移動軌跡出力回路５について第１１図
により説明する。状態判定回路７より出力された
円移動開始信号はラツチ回路２７によりラツチさ
れ、アツプカウンタ２８をリセツトし、上限移動
角度決定回路２９を駆動する。円移動用クロツク
発振器３０からのクロツク信号は、ラツチされた
円移動開始によりアツプカウンタ２８へ入れられ
る。一方、上限移動角度決定回路２９により円周
移動最大角度φ_imaxが決定された後、移動角度決
定回路３１は、アツプカウンタ２８の状態に応じ
て移動変位角φ_iのデータを、クロツク周期△t_cご
とに離散的に３次元位置決定回路３２およびφ_i＜
φ_imax判定回路３３へ出力する。３次元位置決定
回路３２は、移動変位角φ_iのデータに基づき３次
元空間の移動位置ｒ→_iを演算して決め、ロボツト
サーボ駆動回路６へその結果を送出し、ロボツト
サーボ駆動回路６は、このデータによりロボツト
を駆動する。φ_i＜φ_imax判定回路３３では移動変
位角φ_iが円周移動最大角度φ_imaxを超えたかどう
か判定し、超えた場合には状態判定回路３へ円移
動終了信号を出力し、ラツチ回路２７をリセツト
する。このようにして、円移動軌跡出力回路５に
より３次元移動位置ｒ→_iが決められ、ロボツトサ
ーボ駆動回路６を駆動する。さらに、円移動軌跡
出力回路５のφ_i＜φ_imax判定回路３３からの円移
動終了信号は、アドレスカウンタ１へも送られア
ドレスカウンタ１を１個進める。

アドレスカウンタ１がｉからｉ＋１へ進むと、
メモリ２のデータが入力用バツフアメモリ８へ送
られる。すなわち、入力用バツフアメモリ８で
は、アドレスＩ６〜Ｉ８の内容がアドレスＩ３〜
Ｉ５へ移され、アドレスＩ９〜Ｉ１１の内容がア
ドレスＩ６〜Ｉ８へ移され、アドレスＩ１２〜Ｉ
１４の内容がアドレスＩ９〜Ｉ１１へ移されて、
アドレスＩ１２〜Ｉ１４へはメモリ２から位置Ｒ→
_ｉ−２の情報が書きこまれる。また、アドレスＩ１
５の近傍通過誤差量△_iは△_i+1に書き換えられる。
以上のようにして次のステツプの必要な情報が入
力用バツフアメモリ８へ入れられることにより、
これらの情報をもとに前述した各回路が同様に動
作し、直線移動および円移動の制御が実行され
る。

ロボツトのアームが目標地点に到達すると、前
述したように、アドレスカウンタｉはＮ−１とな
り、出力バツフアメモリ書き換え回路１８によつ
て切換位置O_iは最終目標位置R_Nに置き換えられ
る。また、ｉ＝Ｎ−１判定回路１７からの移動完
了信号は計算機等の外部制御に送出し、ロボツト
サーボ駆動回路６の駆動は完全停止する。

以上は前記提案例による動作と構成を示し、ロ
ボツトのアームをを３次元空間の各点間を一定速
度で移動させ、該点近傍で該位置とあらかじめ設
定された近傍通過誤差量とで決まる内接円上で線
速度と同一の円周速度でなめらかに移動させるよ
うに軌道制御したことにより、各点間をなめらか
な速度で連続的に移動することができる。

第１２図は本発明の原理説明図である。

同図は隣接した内接円と直線を含む軌道の１部
を示したもので、上記提案例によれば、｜Ｐ→_i−Ｒ→_i｜＋｜Ｒ→_i+1−Ｏ→_i+1｜｜Ｒ→_i+1
−Ｒ→_i｜
(14) の条件を満しているとしたが、θ_iが90゜に近い場
合、または｜Ｒ→_i+1−Ｒ→_i｜が小さい場合には（14）
式の条件を満せない経由点が現われ、連続した軌
跡上を移動できなくなる。

そこで本発明ではこの問題点に対処するもので
あり、その要旨はＰ→_iＯ→_i+1点がＲ→_iとＲ→_i+1との
中心
Ａ→_iよりそれぞれＲ→_i、Ｒ→_i+1に対し存在するよう
に
近傍通過誤差量を修正することである。なお、上
記補正は近傍通過誤差量を減少するように変更す
るから精度劣化の問題は起らない。またＡ→_iをＲ→_i、
Ｒ→_i+1の中点としたがＲ→_i、Ｒ→_i+1の間にあれば任
意
の点に設定してもよい。

近傍通過誤差量の修正は前式における△_i、L_i、
D_iの何れかを選択可能であるが、以下に△_iを選
定した例につき説明する。

第１３図ａ〜ｄは第１２図で説明した補正を含
む本発明の動作を示すフローチヤートである。

同図において、第３図と異なるステツプにはダ
ツシユを付して示す。以下このダツシユを付した
ステツプにつき説明し、他のステツプは第３図の
とおりであるから説明を省略する。

まず、本発明では直線と内接円の接点Ｏ→_i，Ｐ→_i
の位置が問題となるから、スタート後ステツプ(D)
の代りにこの判定を含むステツプ（D′₁）〜
（D′₅）が適用される。すなわち、ｉ≠Ｎが判定さ
れ｛ステツプ（D′₁）｝、ｉ≠Ｎならば次のステツ
プに行き、ｉ＝ＮならばＯ→_i＝Ｒ→_Nに置換えられ
｛ステツプ（D′₂）｝、＊２を経由してステツプ
（Ｎ）に進められる。次にｉ≠１が判定され｛ス
テツプ（D′₃）｝、ｉ≠１であれば次のステツプ
（D′₅）で単位方向ベクトルｅ→_iが決められ、ｉ＝
１であればステツプ（D′₄）とステツプ（D′₅）で
それぞれ単位方向ベクトルｅ→_i-1とｅ→_iが決められ
る。

次に異なるステツプは、本発明の要部となる直
線円の切り換え点の決定を行なうステツプ
（F′）である。前述のように、隣接する内接円と
直線の接点が相互に重なり合わないようにするた
め、ステツプ（F′₁）において当初あらかじめ設
定された近傍通過誤差量△_iにより(4)式を用いＯ→_i、
Ｒ→_iの距離L_iを求める。そしてｉ≠１が判定され
｛ステツプ（F′₂）｝、ｉ≠１ならば次のステツプ
（F′₄）でＲ→_iとＲ→_i+1の中心距離A_iが求められ、ｉ
＝１ならばステツプ（F′₃）でその前のＲ→_pとＲ→_iの
中心距離A_i-1が求められた後ステツプ（F′₄）に
進められる。次に、ステツプ（F′₅）においてL_i
とA_i-1とが比較され、L_iがA_i-1より大きくないと
判定されると次のステツプ（F′₇）に進み、L_iが
A_i-1より大きいときはL_i＝A_i-1に置き換えられる
｛ステツプ（F′₆）｝。ステツプ（F′₇）においてL_i
とA_iとが比較され、L_iがA_iより大きくないと判定
されると次のステツプ（F′₉）に進み、L_iがA_iよ
り大きいときはL_i＝A_iに置き換えられる｛ステツ
プ（F′₈）｝。このようにして、L_iを中心距離A_i-1、
A_iより大きくないように設定するためには△_iを
減少することにより行なわれる。このようにし
て、ステツプ（F′₉）においてＯ→_iＰ→_iの位置が下記
のように決められる。

Ｏ→_i＝Ｒ→_i−L_iｅ→_i-1 …(15) Ｐ→_i＝Ｒ→_i＋L_iｅ→_i …(16) 以下、直線移動のステツプにおいて、ｉ≠１の
判定に関連しステツプ（J′）、（L′）、（K′）、ｉ
≠
Ｎの判定に関連しステツプ（U′）が異なるが、
略同様の手順を経由し直線移動部と円移動部とが
重なり合うことなく円滑に連結される。そしてス
テツプ（Ｚ）でφ_iがπ−2θ_iより小さくない場合
には、ｅ→_i、A_i、Ｐ→_iを格納するステツプ（ZB′）
を設けて、このデータによりステツプ（ZB）の
ｉ＝ｉ＋１による手順を繰り返えす。

その構成は第４図〜第１１図が用いられる外、
第１０図の直線移動軌跡出力回路４に対し、第１
３図のフローチヤートの要部のステツプ（F′₁）
〜（F′₉）等に対応する、所要の実現回路を別系
として付加すればよい。その構成の態様は提案例
と同様であるから詳細は省略する。

以上説明したように、本発明によれば、隣接す
る内接円から直線に、および直線から内接円に切
り換える接点が重なり合わないため、各点と接点
間の距離が各点間の中心距離を超えないように、
近傍通過誤差量を減少させる修正がその都度行な
われる。このようにして、隣接する内接円と直線
の接点の重複が避けられるから、ある経由点から
次の経由点になめらかな円移動を行なわせること
ができるものである。

本発明のロボツトの軌道制御では３次元の各点
近傍で該位置と近傍通過誤差量とで決まる内接円
上を移動することを前提としているが、本発明の
要部の適用はこれに限定することなく、内接放物
線、内接多項式曲線、外接多項式曲線にも適用で
きるものである。ここで内接、外接多項式曲線と
は、第２図で直交軸がR_i-1R_iとR_iR_i+1との２等分
ベクトルとこれに直交するベクトルより成り、本
発明の内接円に相当する関係条件を有する各種の
曲線を意味するものである。

【図面の簡単な説明】

第１図はロボツトアームの移動を示す状態図、
第２図は本発明のロボツトの軌道制御方式の既提
案例の軌道図、第３図ａ〜ｄは第２図の軌道を実
行するためのフローチヤート、第４図は提案例の
ロボツトの軌道制御方式の１実施例の全体的ブロ
ツク系統図、第５図は第４図のメモリの内容を示
す図、第６図は第４図の座標変換回路の内容を示
すブロツク系統図、第７図は第６図の各メモリの
内容を示す図、第８図は第６図の変換手順メモリ
の内容の１例を示す図、第９図は第４図の状態判
定回路図の内容を示すブロツク系統図、第１０図
は第４図の直線移動軌跡出力回路の内容を示すブ
ロツク系統図、第１１図は第４図の円移動軌跡出
力回路の内容を示すブロツク系統図、第１２図は
本発明の原理説明図、第１３図ａ〜ｄは本発明の
フローチヤートを示す。１…アドレスカウンタ、２…メモリ、３…座標
変換回路、４…直線移動軌跡出力回路、５…円移
動軌跡出力回路、６…ロボツトサーボ駆動回路、
７…状態判定回路、８…入力用バツフアメモリ、
９…変換手順メモリ、１０…演算器選択回路、１
１…演算器収納部、１２…中間結果用バツフアメ
モリ、１３…出力用バツフアメモリ、１４…ｉ＝
１判定回路、１５…書き換え指令回路、１６…直
線移動開始信号出力回路、１７…ｉ＝Ｎ−１判定
回路、１８…書き換え指令回路、１９…円移動開
始信号出力回路、２０…ラツチ回路、２１…アツ
プカウンタ、２２…上限移動長決定回路、２３…
直線移動用クロツク発振器、２４…移動距離決定
回路、２５…３次元位置決定回路、２６…l_i-1＜l_i
max判定回路、２７…ラツチ回路、２８…アツ
プカウンタ、２９…上限移動角度決定回路、３０
…円移動用クロツク発振器、３１…移動角度決定
回路、３２…３次元位置決定回路、３３…φ_i＜φ_i
max判定回路。

Claims

【特許請求の範囲】

１ロボツトの被制御体をあらかじめ指定された
複数の指定経由位置間にて一定の線速度で移動さ
せ、該指定経由位置近傍では該指定経由位置とあ
らかじめ設定された該指定経由位置の近傍通過誤
差量とで決まる内接円上を移動させるように軌道
を制御する方式において、隣接する内接円から直
線に、および直線から内接円に前記軌道を切り換
える接点が重なり合わないように前記近傍通過誤
差量を修正した内接円上を移動させるようにした
ことを特徴とするロボツトの軌道制御方式。