JPS58114207A - ロボツトの軌道制御方式 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 （１）発明の技術分野 本発明はロボットの軌道制御方式に係り、特に教示され
た３次元空間の各点を、停止することなく、かつ速度の
不連続を生ぜずに自動的に移動させる制御方式に関する
。

（２）技術の背景 最近の大規模集積回路化（ＬＳＩ化）技術の発達に伴っ
て、マイクロコンピュータを搭載した産業用ロボットの
進歩もめざましい。このようなロボットはますます知能
化し、障害物等を自動的に回避できるようになっている
。

（３）従来技術と問題点 従来、ロボットのアームが障害物を回避しつつ目的地点
に到達する場合第１図に示すように、教示あるいはロボ
ット言語により指定された３次元空間の各位置Ｗ　ｏ　
−Ｒｉ〜１Ｎを順次通過しながら目的地点に到達してい
た。

しかし、各位置Ｒｏ　””　ＲＮを通−する際、一定の
速度であると折れ線状移動の性質上、各位置Ｒｏ”ＲＨ
で加速度に大きな不連続を生じ、そのため減速−停止−
加速を実施せざるを得なかった。

このため多数の指定点を経ｊする移動では時間がかかり
、また運動形態も一様でないため、加／減速度が頻繁に
起り、アームが振動し易く、好ましい運動性能を得るに
支障をきたすという欠点があった。

４）発明の目的 本発明の目的は、アーム移動の指定された経由された点
と実際の軌道曲線との差の最大値である最大近傍通過誤
差量をオペレータが作業の内容により任！に指定できる
ようにすることにより、各経由点上を通過しながら停止
を伴うことなく、振Ｊ−生じない滑らかな加／減速を行
いつつ経由点を移動できるロボットの軌道制御方式を提
供することにある。

（５）発明の構成 上記目的を達成するために、本発明はロボソ）の被制御
体をあらかじめ指定された複数の指定経由位置間にて、
それらを結ぶ直線上を移動しつつ、該指定経由位置近傍
では、あらかじめ設定された前記直線からの最大近傍通
過誤差量、該指定経由位置を通過するとの条件、および
外接点における線速度が前記直線移動のそれに等ルいと
の条件を基本として決定される外接多項式曲線上を、前
記外接多項式曲線の変曲点と外接点との間では、線速度
を一定に保ちながら、また前記変曲点と該指定経由位置
との間では線速度を変化させながら速度を制御すること
により、発生する加速度を小さく抑えて移動させるよう
に軌道制御をするものである。

（６）発明の実施例 本発明の一実施例について（図面と共に説明する。まず
第２図でロボットのアームが動作する上での準備すべき
パラメータについて説明する。これらパラメータの１互
関係を明確にする上でのポイントとして外接多項式を決
定するには次の４点がある。

■Ｐ　Ｉ−１から０１へ一定の速度ｖ１で直線移動を行
い、０１点でその速度の大きさ■、および方向を受けつ
ぎ、かつＲ１点を通過でき、さらにｐｔ点でその速度の
大きさ■と方向を次の直線移動に引き渡してＰＩからＯ
ｍへ移動できるものであること。

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

■前記移動に際して、振動発生の原因である速度の変化
率、すなわち加速度については、オペレータが気にする
ことなく自動的に抑制されるように移動中の速度制御が
なされる。

れ、かつ上記操作は、その平面上で２次元の問題として
記述できることに着目し、３次元空間での複雑で時間の
かかる演算を簡明にできること。

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

ロボットのアームの移動のスタート位置から目標位置ま
での間に、アームが通過する３次元空間とし、その前後
の指定位置をＲｉ−１＋　　Ｒｉｌｌとする。

Ｒ１とＲ１，、との間を結ぶ直線の方向単位ベクトルを
ｅＩ−＋＊　Ｒ１とＲ、ｌとの間を結ぶ直線の方向ベク
トルをｅＩとすれば、ｃｉ、Ｉ＋ｅ＋　はそれぞれ次式
で与えられる。

ｅＩ−＋＝＜Ｒｍ　　Ｒｍ−＋）／ＩＲ＋　　Ｒｍ−＋
１　・−１１１乙＝（’Ｅ＋−＋　　Ｌ　）　／　ｌ　
Ｒｈ＋　　Ｙｌｌ　・・（２１上記２つの単位ベクトル
’＋、＋　＋　”　ｉによって決る内積角をπ−２θ１
とすればｅＩは次式で与えられる。

θ曇−（π−ｃｏｓ’　　（ｅＩ−＋　・；：　＋　）
　）　／　２・・・・　・　・（３） このθ１は位置Ｒ３での直線移動°の方向変化角すなわ
ち、Ｒ１まで直−移動し、１１点方向を変えて再び直線
移動をする場合の変化角を三等分したものである。また
この三等分の方向ベクトルＦ１は次式で得られ、 ハ＝（ｒ＋　　ｅ、−、）／ｌ♂ｉ　　ｅ、−，１・・
（４）前記外接多項式曲線を２次元問題として扱うとき
ｙ軸となるものである。Ｒｍ−＋、Ｒ＋　、Ｒｍの定め
る面の法線ベクトルν１は νｌ　”　（ｅｉ、Ｉ′ｘｅ　ｌ　：ｌ　／　ｌ　”＋
−＋ｘｅｔ　　ｌｏ”（５）と得られる。ここで（”’
ｔ−＋　Ｘ　ｒ　ｔ　）はベクトル積である。すなわち
、ＴＩと１６．、との両方に直交するベクトルであって
、’ｉ−１の方向からＴ１の方向に回転させたときに右
ネジの進む方向の単位ベクトルである。

前記外接・多項式曲線（以下外接曲線と略称する）を２
次元問題として扱うためのＸ軸を示す方向単位ベクトル
をλ、とするとμ、−シ１→λ１を右手直交系を構成す
ると定義して、次式でλ１が一意的に決定される。

λ１＝〔μｍ×ν１〕　　・・・・・・・・（６）＝＋
　（ｅ　＋　＋　ｅ　＋−＋）　／　ｌ　ｅ　ｔ　＋　
８　＋−＋　ｌ・・・・・（７） なお、上記ｘ、ｙ座標軸の原点はＲ３とする０以上によ
り、３次元空間での外接曲線を２次元の問題に変換して
扱うための、ｘ＋　　ｙ軸および原点を定めることがで
きた。

アームの移動が直線運動から外接曲線に切り換える位置
を０１．外接曲線から直線運動に切り換える位置をＰＬ
とし、０．とＲ６との距離およびＲＩからＰｉとの距離
をり、とする。Ｌｌは後述の（３５）式により、オペレ
ータが直線運動からの最大通過誤差量δ、を指定して決
定される。前記位置０１と、アーム移動が外接曲線から
直線移動に切り換える位置Ｐ　＋−１とは、位置Ｒｉ、
距離り、。

単位ベクトル７１．１によって次式で得られる。ベクト
ルサ、は、ベクトルＷ＋からベクトルＬ、・丁５．１を
引いたものであるし、ベクトルＰ　Ｌ−１はベクトル成
立する。

ここで添字ｔ−１は前位置Ｒｉ−１によって決るパラメ
ータに付けられたものである。

以上の準備により、外接曲線を２次元問題に簡単化する
ことができた。次に第３図、第４図を用いて外接多項式
曲線を決定する。外接すべき直線は第３図より ｙ＝　（ｙ　ｏ／ｘ　ｏ）　　ｌ　ｘ　ｌ　　　　　　
　（１１）ｘｏ＝Ｌ１ｓｉｎθｉ　　　　　　　　　　
（１２）ｙｎ＝Ｌ１ｃｏｓθ−（１３） である。本発明の目的および前記ポイントより、外接多
項式は以下の条件を満たす必要がある。

（ｉ）切り換え点ａ、ｂを通る。

（ｉｉ）原点（０）を通る。

（ｉｉｉ）Ｒ＋点で速度をベクトル的にも不連続としな
いために、原点での接線の傾きが零である。

（ｉｖ）切り換え点Ｏｉ、ｐｔでの速度がベクトル的に
も不連続地ならないために、ａ、ｂ点での接線の傾きは
、直線（１１）の傾きに等しくならなければならない。

（ｉｖ−■）偶関数である。

以上が外接多項式決定の基本条件である。

また、次の条件を付加し工、振動発生の原因である加速
度をさらに抑制することができる。

（ｖ）切り換え点Ｏｉ、ＰＩでの加速度が直線部のそれ
と連続であるように、ａ、ｂでの２次微係数を零とする
。

（ｖｉ）Ｒ“１点での加速度を零とするために、７５１
点での２次微係数を零とする。

以上の条件をもとに外接多項式曲線を決定する。

次の多項式を対象とする。

ｙ＝ｆ′ａＫｘ１・・・・・・・・・・（１４）ｋ＝。

条件（ｉｉ）　、　　（ｉｉｉ）　、　　（ｉｖ−■）
よりｙ＝Σａ　、、　ｘ　Ｚ＃　・・・・　・・　・　
・・・　（１５）１：１ でなければならない。

取り扱いを簡明にするため、次の規格化を行う（第４図
）。すなわち、多項式曲線の定義域を（−ｘｏ、ｘｏ）
から（−１，１）にするように規格化を行う。

Ｘ　＝　ｘ　／　ｘ　ｏ　　・・・・・・・・・・　（
１６）Ｙ＝ｙ／ｙｏ　　・・・・・・・・・・　（１７
）（１５）　、　　（１６）　、　　（１７）よりｙ＝
ｆＡ　ｘ”・・・・・・・・・・　（１８）、８．ル ａｍ　＝　（ｙｏ／Ａ、Ｋ）　／　ｘ？　＋　ＨＨ＋　
　（１９）となり、残る条件よりＡ〉５を決定すれば良
い。その条件は規格化座標系（第４図）では以下で表わ
される。

（ｉ　′）ｌＸＩ＝１で　Ｙ＝１・・・・・（２０）（
ｉｖ”）ｌＸｌ＝１で　ｌｄＹ／ｄＸＩ＝１・　・　・
　・　・　＜２１） （ｖ’）ｌＸｌ＝１で　ｄ　’Ｙ／ｄＸ’　＝０・・・
・・　（２２） （ｖｉ’）Ｘ＝Ｏで　ｄ　′Ｙ／ｄＸ’　＝０・・・・
・　（２３） 前述の如＜　　（ｉ　′）　、　　（ｉｖ’）が基本条
件であり、（２０）　、　　（２１）式に（２２）　、
　　（２１）を組み合わせて、次の４種類の外接多項式
曲線を決定できる。

ケース■：基本条件（（２０）　、　　（２１）　）の
みケース■：基本条件（（２０）　、　　（２１）　）
と（２２）式（この場合は０１　、Ｐｉ点での加 速度が常に零となる） ケース■：基本条件（（２０）　、　　（２１）　）と
（２３）式（この場合はＲ２点での加速度が 常に零となる） ケースｉｖ：基本条％　（（２０）　、　　（２１）　
）と（２２）式と（２３）式（この場合はＯｒ　、　Ｐ
　＋およびＲ１点での加速度が常に零と なる） 以下では、ケース■について実施例を説明する。

（２０）〜（２２）式より外接多項式曲線の係数Ａ２社
を決定できる。（１８）を（２０）〜（２２）に適用し
て、 １−党Ａｚｋ １　＝ｆ、２　ｋＡエ　　　　　　　・・・　（２４）
ｏ−ｆ２ｋ（２に−ｔ）Ａ、。

ト％１ となり、（２４）を満たす外接多項式は、（１８）式の
３項のみが選定可能である。

以上より本発明の目的を満たす規格化外接多項式曲線は
、（１８）の任意の３つの項を選定して、（２４）の連
立方程式を解いて求めたＡよ、より決定される。実座標
での外接多項式曲線へは、（１９）式で変換される。

以上の外接多項式決定のプロセスを具体的に示すと以下
となる。

ｊ、に、文を選定（但し２以上の偶数でｊ＃に≠交）　
　↓ 規格化係数の決定 Ａｊ、＝Ｄ　＋　／Ｄ、ＡＫ　＝Ｄ　２／Ｄ。

Ａ　ｓ　＝　Ｄ　３　／　Ｄ Ｄ＝ｊ（ｊ−１）（又−ｋ） ＋ｋ　　（ｋ−１）　　（ｊ−文） 十文　（交−１）（ｋ−ｊ） Ｄ＋＝に文　（又−ｋ）＋ｋ　（ｋ−１）−文　（又−
１） Ｄ２−又ｊ　（ｊ−文）十文　（又−１）−ｊ（ｊ−１
） Ｄ３＝ｊｋ　　（ｋ−ｊ）＋ｊ　　（ｊ−１）−ｋ　　
（ｋ−１） ↓　　　　　　　　　　　　・　・　・　・　・　（２
５）実座標係数の決定 ａ、　＝　（ｙ　ｏ／Ａ・）　／ｘ’。

− ａＫ＝（ｙｏ／ＡＫ）／ｘｌｏ　・・・　（２６）↓ 実座標外接多項式の決定 ｙ−ａ　Ｊ’　Ｘ’　＋　ａｙ　Ｘ’　＋　ａ　ｔ　Ｘ
’・（・・・・　（２７） 以上で外接多項式曲線が決定された。

第４図に示す直線移動より離れた軌跡を通る誤差（外接
曲線より直線への垂線の距離）の最大値Δは、定義域（
−１，１）内の規格化多項式曲線の接線の傾きが±１を
満たすＸすなわち、□規格化座標では １　＝　ｌ　ｄＹ／ｄＸ　ｌ ＝　ｌ　ｊＡ、　ＸＪ−’＋ｋＡＫ　Ｘ″−１十交Ａ　
、　Ｘ’−’　”・・・・・　（３０） を満たす位置Ｘ＝ξで生じ Δ＝１ξ−Ａ、ξＪ　　ＡＫξ　−Ａ、ξ　１／ＪＴ　
　　　　　　　　　・　・　・　・　・　（３１）と求
められ、多項式選定時のｊ、に、１のみに依存し、Ｒｅ
　・・”Ｒ１＋　　＋＋　・＃、ｌ　　θ１．Ｌ１等に
はよらない値である。実座標での最大誤差δ１は、それ
に対応して ｘ＝ｘ　ｏξ　・・・・・・・・・・・　（３２）ｙ＝
）Ｆｏ　（ａ、Ｈｘ’　　＋ａｙ　ｘ’　＋ａＬｘ　　
）・・・・・・　（３３） で生じ、その値は δ＋　＝　（１／　ｖ’Ｔ）　　（Ｌ　＋　　５ｉｎ２
θＩ）Δ・・・・・・　（３４） となる、その性質は ＊θ−＝０および９０°でｉ小となり、その値は零、す
なわち直線上を移動する。

＊θ１＝４５°で最大となり、その直線は（１／Ｊ’ｉ
　＞　Ｌ　＋　・Δとなる。

であり、θ−に応じて滑らかな移動ができるように自動
的に誤差を調整する好ましい特性を有している。

前述のＬｌは、作業内容および必要精度に応じてオペレ
ータが任意に設定した最大誤差δ１より次式で決定され
る。

Ｌ　ｔ　＝　Ｊ’にδ１／（Δｇｉｎ２θ、）・・・・
・　（３５） なお、θ１が０°、９０°に近い場合はＬｌが過大とな
り、次の外接曲線と重なる場合があるが、それに対して
は Ｌ１″≦βδ１／（Δ５ｉｎ２θ、） ・・・・・　（３６） を満たす適当なＬｌ’を選定すればよく、その場合の最
大誤差は指定したそれよりも小さい値になるため精度劣
化の懸念はない。

次に、以上で決定された外接曲線上の移動の制御法を決
定する。その前に、外接曲線上の速度Ｖと加速度αの性
質を調べて、振動の原因となる加、　速度を最小とでき
る移動の抑制手続きを決定する。

外接曲線上の線速度をＶ、そのＸ軸への投射成分を女と
すると、それらの間には ｖ−（ｄ　ｘ／ｄ　ｔ）′Ｌ＋　（ｄ　ｙ／ｄ　ｔ）”
＝　；ｉ　Ｇ（Ｘ）／　ｓｉｎθ＋　　　　　　’　　
　　（３７）Ｇ（Ｘ＞＝　　　５ｉｎ１θ　＋　ｃｏ♂
θ　　（ｄＹ／ｄＸ）’・・・・・　（３８） 前述の実施例（２７）の場合には、 ｄ　Ｙ／　ｄ　Ｘ　＝　ｊ　ＡｊＸ’−’＋ｋ　ＡＫ　
Ｘ’−’十又Ａ、　ｘＱ−１・・・・・　（３８′） の関係が存在する。Ｇα）の性質は、（２１）および前
述の条件（ｉｉｉ　）よりＧ　（０）　＝　ｓｉｎθ−
１Ｇ　（１）＝１であり、またＧ（×）はｄゝＹ／ｄＸ
’＝０・・・自・・（３９）を満たすＸ＝ηで極大を示
す、　Ｇ（”＜３の概略を第５図に様子を示す。

一方、ＯＬ、’＋　　Ｐ　ｉ点で直線移動の速度Ｖと外
接曲線上の速度Ｙが等しくなければならない。その条件
をひき続いて外接曲線を移動さ゛せる方法に、２種類、
すなわち士を一定として制御する方法と、■を一定とす
る方法があり、前記接続条件および（３７）から ■−一定法では Ｖ”Ｖ　　　　　　　　　　　　　　　（４１）Ａ＝Ｖ
ｓｉｎθｉ　／　Ｇ（Ｘ）　　　　　　　　（４２）で
あり、第５図より士を増加させながら移動していく。

Ｘ−一定法では ｘ−ｖｓｉｎθ＋　／ｃｏｃｌ、。

＝Ｖｓｉｎθ＋／Ｇ（１） ＝Ｖｓｉｎθ１　　　　　　　・・　・　・　・　（４
３）ｖ　＝　Ｖ　Ｇ（Ｘ）　　・・・・・・・・・・・
　（４４）であり、■を第５図から判るようにＸ＝−η
まで増速したのち減達し、Ｒ１点では最低速度Ｖ　　ｓ
ｉｎθを経て、Ｘ＝ηまで単調に増速し、その後２１点
まで減速したのちａｍ移動を行うことになる。

これら制御法の長所／短所を滑らかな移動に動用の加速
度抑制の観点から調べる。

ｖ−一定法の加速度は、Ｖをベクトル速度としてα＝ｌ
ｄマ／ｄｔｌ ＝　（Ｖ′／Ｌｌ）　　ｓｉｎθＩＣＯ５θ１・　（ｄ
′Ｙ／ｄＸＬ）／　（Ｇ　（Ｘ））・・・・・　（４５
） と求められる。その欠点は、Ｘ＝Ｏで（４０）よりＧ　
（０）　＝　ｓｉｎθのため （ｒ＝　（Ｖ　’／Ｌ　＋　）　　（ｃｏｓθ１　／　
ｓｉｎ’θ１）ｘ；Ｏ ・　（ｄ　　Ｙ／ｄＸ　　） となり、θ１が小さくなるにつれて加速度が増大し、θ
、−０“で無限大となる点にある。

一方に＝一定法の加速度は、 １＝ｌｒｖ／ｄｔｌ ＝　（ｘ’　ｙ　ｏ／ｘＲ）（ｄ’Ｙ／ｄＸ’　）・・
・・・　（４６） （Ｖ　　／Ｌ＋）　　ｃｏｓθ舎 ・　（ｄゝＹ／ｄＸ”　）　　　・・・・　（４７）と
求められ、（４５）と比較して ＝（Ｖ＝一定法の加速度）・　（Ｇ　（Ｘｌ　）’／ｓ
ｉｎθ＋　　　　　　　・・・・・（４８）となる。そ
の長所は（４７）式により判るように、すべてのθ＋、
Ｘ＋で加速度が発散せず、有限の値を有することにあり
、実用に供せる制御法であるが、しかしく４４）で示さ
れた如く、η〈１×１〈１の範囲で増速を神うことに起
因し、第５図でその傾向はθ１が小さくなるにつれて顕
著となる。

そこで本発明ではＸの領域を分割し、前記の２方法の長
所を取り入れ、短所を避けた方法（領域分割法と名付け
る）を発案した。前述の２方法の長／短所をまとめると
、以下である。

−ｍ一定法 長所：すべてのθ１．Ｘでαが有限である。

短所：η＜ＩＸＩ＜ｌの範囲で、θ１が小さいほど、ｖ
＝ｌ定法に比してαが 大きい、その原因はη＜ＩＸＩ＜１ の範囲で増速を伴うことにある。

■−一定法 短所：Ｘ＝Ｏで、θ１が小さいほどαが大きくなり、θ
、−〇で無限大となる。

長所：η＜ＩＸＩ＜１では量＝一定法より加速度が小さ
い。

以上の分析から、次の如くに制御領域を分割することに
より、両者の長所を取り入れ、短所を相殺できることに
着目した。第５図に示した領域Ｉと領域■について、 領域１　　（７７＜ＩＸＩ＜１）：Ｖ＝一定法・・・・
・　（４９） 領域ＩＩ（Ｉｘｌ＜η）二に＝一定法 ・・・・・　（５０） とする。なお、領域間の接続点Ｘ＝？で１よ、速度を等
しいとして連結する。加速度の不連続は、（３９）　、
　　（４５）　、　　（４７）より加速度が零である点
であるため生じない。

この領域分割法により以下の効果が得られた。

ａ、すべてのθ、、Ｘ＊加速度が有限 す、η〈ＩＸＩ＜１での加速度が；＝一定法に比して　
ｓｉｎθ＋／（Ｇ（ｘ））倍に小さくなる。

・・・・・　（５４） ｃ、ｌｘｌ＜ηでの加速度もに＝一定法に比して１／（
Ｇ（η））倍に小さくなる。

・・・・・　（５７） ｄ、特にｂの効果は、加速度の増加しゃすいθ１の小さ
い場合に顯著な効果を発揮する。

・・・・・　（５１） 以下に、領域分割法により外接曲線状の移動制御を具体
的に示す。各領域のパラメータの各々にす（４５）より Ｖニー：’Ｊ　　・・・・・・・・・・・・・　（５２
）Ｘｊ　　＝ｖ　　ｓｉｎθ′り／Ｇ（Ｘ）　　・・・
・・・（５３）αｘ　＝　（Ｖ２／Ｌ　ｔ　）　　ｓｉ
ｎθ１　　ｃｏｓθ１・（ｄ”　Ｙ／ａ　ｘ２）　／　
（Ｇ（Ｘ））３・　・　・　・　・　（５４） 領域■では、１Ｘｌ＝ηで速度■を接続して、（３７）
　。

（４６）’より Ｍｕ　＝Ｖ　ｓｉｎθＩ／Ｇ（η）・・・・（５５）ｖ
ＴＬ＝　ｘＩＧ　（Ｘ）　／　ｓｉｎθ１＝　（Ｖ／Ｇ
　（η）　）　Ｇ　（Ｘ）　　・・・　（５６）α、　
＝（ｘ　、：　ｙ　ｏ／　ｘ″’ｏ）（ｄ’　Ｙ／ｄＸ
”　）＝　（Ｖ７Ｌ　ｔ　）　　ｃｏｓθ＋　　（１／
Ｇ　（η）　）’・　（ｄ　’　Ｙ／ｄ　Ｘ’　）　　
　　　　　（５７）となり、領域分割法での移動制御の
準備が終った。

次に、領域分割法での外接曲線上の移動を具体的に述べ
る。Ｐ８．１から０１への直線移動距離を文４．１とす
れば、距離又１．１は直線上の単位距離当りの移動時間
Δｔｓを、単位移動のステップＳ、速度Ｖによって決め
られる。すなわち 又＋、Ｉ　＝　ＶΔｔｓ−８・・・・・・・・　（５８
）また、Ｐｌ、１から０１への直線運動上の移動位置を
’　ｉ−１とすると’Ｐｉ−Ｉｎ　又、−７方向単位ベ
クトル；１．．にょって次式で３次元空間の位置として
次式で決定される。

久Ｇこ、Ｕ−かりｔ’Ｉへの外沃圀騨よ運動の世直ｒ１
を決定する。第４図−ＯＸ軸上の移動位置をＸｌと、単
位移動時間をΔｔｃとするとＸ＋は次のように決定され
る。規格化座標でのＸ方向移動速度を☆とすると、（１
２）より Ｘ＝ｄＸ／ｄ　ｔ　＝ｄ　　（ｘ／ｘ　ｏ）　／ｄ　ｔ
＝　（１／ｘ　ｏ）　ｄ　ｘ／ｄ　ｔ −（１／Ｌ＋　　ｓｉｎθ１）哀・・・・・（６０）で
あり、領域１．ＩＩに対する規格化速度を交１．父πと
すると（５３）　、　　（５５）より　０ＸＸ＝（■／
Ｌ１）／Ｇ（ｘ）　・・・・　（６１）Ｘ　　＝　（Ｖ
／Ｌ＋＞　／Ｇ　（７７）　　−・・−（６２）となる
。

規格化外接曲線上の移動位置Ｘ１は（６１）　、　　（
６２）より ｘ　Ｉ＝Ｘ　ＩＩ（Ｖ／Ｌ＝）／Ｇ　（ζ）・・・・・
　（６３） 決定された。ここで領域Ｉでは、ζ＝Ｘｉ、領域■で領
域−ηである。Ｙ：は、前述の滞りを（２り）式δ場合
には（１８）式より Ｙ　ｉ　＝Ａ−Ｘ３＋ＡＫ、Ｘ’　＋ＡＬＸ’・・・・
・・　（６４） と求められ、従って２次元実座標上の位置Ｘ１．）’１
は（１２）　、　　（１３）より Ｘ１＝ＸＯＸｌ　・・・・・・・・・・・　（６５））
’＋＝ｙｏＹ１　・・・・・・・・・・・　（６６）と
得られる。以上の準備によりｄ、からＦ＋への外接曲線
運動状の移動位置ｒｌは、前述のｘ、ｙ軸に対応する単
位ベクトルλ１１μｍ、および位置Ｒ１より次式で決定
される。

ｒｉ＝Ｒ１＋ＸｌＡｌ＋ｙＩμ＋・−・（６７）以上に
より、第２図に示されたアームが動作する上での準備す
べきパラメータのすべてについて相互の関係が明らかに
なり、本発明の着眼点が具体的に示され先。

外接多項式の決定法および性質をｊ＝８．に＝６゜又＝
２の８次の３項多項式についてさらに具体的に説明する
。

規格化多項式は Ｙ＝ＡｅＸ　　＋Ａ６Ｘ　　十Ａ２Ｘ ・・・・・　（−’６８） 係数は（２Ｓ）八゛より Ａ　ｅ　＝　１０／２４．　　Ａ　ａ　−２１／　２４
゜Ａ″’２−３５／　２４　　・　・　・　・　・　・
　・　・　・　・　（６９）と決定され、従って外接規
格化多項式はＹ＝　（ＩＯＸ’　−２１Ｘ’　　＋３５
Ｘ”　）　／２４・・・・・　（７０） と求められ、実座標では ）’＝）’　ｏ　　（１０（ｘ／ｘ　ｏ）　−２１（ｘ
／ｘ　ｏ）’＋３５　（ｘ／　ｘ　ｏ））　／２４　　
・・・　（７］）となる。直線よりの最大誤差量は（３
０）〜（３４）より Ｘ　＝　０．３５１８　　・・・・・・・・・・・　（
７２）で生じ、その値は ６ｉ＝　　　０．０８６Ｘ　　Ｌ　　＋　　　５ｉｎ２
　　θ　、ｊ　　　ＨＨＨ＋　　　（７３）であり、θ
番＝４５°の最悪の場合でも、Ｌｌの約９％と小さい誤
差である。

また、外接曲線上の移動速度Ｖは領域分割法では（３９
）より ／（＝０．６４９３　　・・・・・・・・・・・（７４
）であり、（５，２）　、　　（５６）より・０．６４
９３≦ＩＸＩ≦１でＶ工＝Ｖ・・・（７５）０．６４９
３≧ＩＸＩで Ｖ工＝　　（Ｖ／Ｇ　（η）　１　　Ｇ　（Ｘ）・・・
・・　（７６） となり、滑らかな加／減速を行いながら移動する。

θ−＝９０°すなわち直線では加／減速を行わず、θ１
が小さくなるにつれて、加／減速が大きくなり、直線の
方向変化に対応して自動的な速度制御がなされるとの好
ましい性質を有する。

また、移動中の加速度は（５４）　、　　（５７）より
０．６４９３≦ＩＸＩ≦１で αｚ　＝　（Ｖ２／Ｌ　＋　）　　ｓｉｎθ１　　ｃｏ
ｓθ。

・（ｄ　”　Ｙ／ｄ　Ｘ”　）　／　（Ｇ　（Ｘ）　）
”・・・・・　（７７） ０．６４９３≧ＩＸＩで αａ＝（ＶヤＬｌ）ｃｏｓθ＋（１／Ｇ（η））１・　
（ｄ”　Ｙ／ｄＸ”　）　　　・・・・　（７８）とな
り、θ、　−ｏｌｌのときにＸ＝Ｏで最大加速度となる
が、θ、＝Ｏａは近傍通過誤差が零であり（（３３）〜
（３４）　）　、直線上を折り返すとの最も激しい場合
である。誤差許容域１ｃｍの場合の速度を（６８）式を
例に具体的に示す。最大の加速度はθ、＝０°で発する
が、θ１＝０°の場合は（３４）式から判るように６１
が零であり、直線上を完全に折り返すことになる。Ｒ１
の１ｃ１１手前から減速をはじめ、突入進度１０３／３
と高速で直線で折り返し移動する場合でも０．１４（Ｇ
）の加速度を生ずるに過ぎず、移動の少ない安定な運動
が可能となる。また（７７）よ運動が可能となる。また
（７７）より、切り換え点０＋、Ｐ＋では常に加速度は
零である。

次に、上記パラメータをもとに、ロボットのアームが移
動する動作内容を第６図のフローチアートと共に説明す
る。

まず、オペレータが準備すべきパラメータとしては、ス
テップ（Ａ）に示されるような３次元空間の位置Ｒｏ・
・・下１・・・Ｒ８，近傍通過の最大誤差量６１・・・
６１・・・６Ｎ−＋、直線達度■、直線および外接曲線
の単位移動時間ΔｉｓおよびΔｔｃ、さらに多項式の次
数ｊ、におよび文がある。与えられたに、−ｊ、　　文
より規格化された外接多項式の係数Ａｋ、Ａ、、ＡＪＬ
をステップ（Ｂ）で決定する。ひき続いて、規格化多項
式の最大誤差Δをステップ（Ｃ１）で決定する。また（
Ｃ２）で領域分割点ηを決定する。これらのパラメータ
を与えられることによりアームの移動が始まる〔ステラ
　プ（Ｄ））、まず、第１ステンプとして、ステップＥ
からＪで前述したような種々のパラメータを準備する。

特翼点ｉ＝１およびＮで次の例外処理をする。

ｉ＝Ｎすなわち終点の場合は、直線移動から外接曲線へ
の切り換えが起きないから、その切り換え点０．をＲＮ
とし、ステップ（Ｋ）ののちに飛ぶ〔ステップ（Ｅ）〕
。組合せｉ＝ｌすなわちスタート時では、直線移動をす
る単位方向ベクトルで１．１をステップ（Ｆ）により決
定する。

Ｒ１からＲ１１，への直線移動の単位方向ベクトルｅｌ
をステップ（Ｇ）で決定する。直線移動のは方向変化角
θ１がステップ（Ｈ）により決る。

外接曲線移動を２次元問題として扱うための直交座標ベ
クトルλ１１μｍをステップ（１）で決定する。直線か
ら外接曲線あるいはその逆の切り換えに必要な点Ｏｉ、
Ｐｉおよび位置をＲ１と切り換え点０．．Ｐ、との距離
がステップ（１）により決る。

次に直線移動のステップがスタートとする。まずスター
ト地点の説明を行う。ｉ＝１のときすなわちスタート時
はＰ　、、＋　−Ｒｏに置き換える〔ステ距離又トド　
はステップ（Ｍ）で決る。この移動距離中容ステップ数
Ｓでの１’、、からの長さ又１．．はステップ（Ｎ）で
決る。移動距離の各ステップの位置γ４．．はステップ
（０）で決る。このように決められた移動位置ｒ　Ｉ−
１へロボットのアームはサーボにより移動していく　〔
ステップ（Ｐ）〕。移動距離文１．．が直線移動の最大
距離文１　ｗａｘよりも小さい間、すなわち０１点に到
達しない間は、ステップ数Ｓを１段づつ進め〔ステップ
（Ｑ）〕、ステップ（Ｎ）はステップ（Ｑ）までが繰り
返される。

又ト１が又１　ｍａＸに等しいか大きくなると直線移動
は終り、外接曲線移動に移る。ただしｉ＝Ｎのときには
目標地点に到達しているので移動は完了し、”０ＷＡＲ
Ｉ”の信号が出る。

次に外接曲線の移動がスタートする。ステップ（Ｓ）で
切り換え点のｘ、ｙ座標、すなわちｘｏ。

ｙｏを決定したのち、外接曲線の初期位置にＸ１＝−１
が設定される。次にステップ（Ｕｌ）で、領域１．　　
Ｉ［に対応するパラメータζがそれぞれに設定される。

そのζによりステップ（Ｕ２）で規格化速度の零点であ
るＧ（ζ）が決定される。ステップ（Ｕ３）で規格化直
線上の移動位置ＸＩ。

Ｙ、が決定される。Ｘｌ、Ｙ−を用いて、ｘ、　　ｙ座
標系での外接曲線上の移動位置Ｘ＋＋３’ｌが決定され
る〔ステップ（■）〕。外接曲線上の移動位置「、はス
テップ（Ｗ）で３次元空間のベクトルとして決る。この
ように決められた位置ｒ１ヘロボノトのアームはサーボ
機能により移動していく　〔ステップ（Ｘ）〕。Ｘ１が
１より小さい間は、ステップ（Ｙ）でステップ（Ｕｌ）
からステップ（Ｙ）までが繰り返えされる。Ｘｌが１°
に等しいか大きくなると、外接曲線移動は完了し、次の
ステップのために、ｔｌ、￥１をそれぞれ丁、１゜Ｐｌ
−１に格納し、次のステップの直線移動に移る〔ステッ
プ（Ｚ））、次にステップにはｉ　’＝ｉ＋１に置き換
えられて、ステップ（Ｆ）にもどる。

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

（７）発明の効果 （１１各経由点を厳密に通過しながら、速度がベクトル
的にも不連続を生ずることなく滑らかに移動できる （２）直線移動からの最大通過娯差量をオペレータが任
意に指定をすることにより、精度を把握しながら作業を
計画することができる。

（３）３次元空間での外接曲線を２次元的に取り扱うこ
とができるため取り扱いが簡単になり、従って高速での
滑らかな移動ができる。

（４）外接曲線上の速度が、テ、がらＴ＋までは単調に
減少し、Ｒ１点で最低速度になり、膚１がらＰ：までは
単調に増加し、１点で直線移動の速度に等しくなる滑ら
かな加／減速が外接曲線の鋭さに影響するδ１．θ−に
応じて自動的に行われる。

（５）また領域分割法により以下の効果がある。

■すべての０１で加速度が有限となり、θ１＝０．すな
わち直線折り返し移動のときにも自動的な速度制御がな
され肴限の加速度で移動できる。

■η′を規格化外接曲線の変曲点として＊η＜ＩＸＩ＜
ｌでの加速度力へ＝一定法に比してｓｉｎθ＋　／　（
Ｇ　（Ｘ）　戸　　借手さい。

＊ＩＸ１≧ηでの加速度もＸ＝一定法に比して１／（Ｇ
（η））　倍に小さい。

■前述の例により具体的に示すと、最大加速度はθ、−
０°で生ずるが、その場合でも、１ｃ１ｍから減速をは
じめて、突入進度が１０ｃ＋ａ／Ｓと高速の直線折り返
し移動でも、Ｒｉ点で０．１４（Ｇ）と小さな加速度を
生ずるに過ぎない。

（６）上記の如く、加速度発生を抑えて加／減速を行う
にもかかわらず、ＯＩからＰＩまでのその通過時間は直
線移動とほとんど変らない。その通過時間をＴｃ、その
直線での通過時間をＴｓとすると、規格化曲線の変曲点
をηとして Ｔｃ＝Ｔｓ　　［１＋　（（７７＜ＩＸＩ＜１の外接曲
線の長さ）−η）］ であるが、η＜ＩＸＩ＜１の外接曲線の長さはηの１〜
２割増であり、 Ｔ　Ｃｙ　１．２Ｔ　ｓ と短い時間で外接曲線上を移動できる。

以上では、最大通過誤差量δ１をオペレータが指定する
としたが、誤差許容域Ｌｉを指定してもよい。

また、直線移動部の速度を一様にＶとして一定としたが
、各経由点位置毎にオペレータが指定することも可能で
ある。

【図面の簡単な説明】

第１は従来のロボットの軌道を示す図、第２図は本発明
にかかるロボットの軌道制御方式における３次元空間で
のロボットの軌跡とパラメータを示す図、第３図は第２
図に示した本発明の実施例におけるｘ、ｙ実座標上の多
項式曲線とパラメータの関係図、第４図は第２図に示し
た実施例における規格化座標上の規格化多項式曲線とパ
ラメータの関係図、第５図は本発明にかかるロボットの
軌道制御方式において、速度制御の変更方法の説明図、
第６図は本発明にかかるロボットの軌道制御方式の１実
施例のフローチャートである。 手続補正書（方式） 昭和Ｉ年　を月　４日 ｌ嚇件の表示 昭和　ｓ年特許願第　ｚ／／／ｙ１号 ３　補正をする者 参件との関係　　　　　特許出願人 住所　神奈川県用崎市中原区上小田中１０１５番地（５
２２）名称富士通株式会社 ４　代　　理　　人　　　　　住所　神奈川県用崎市中
原区上小田中１０１５番地８補正の内容　１ＩＩ１１の
選り （１）　　１１４細書１１ｇ　３２　ｇ第６行Ｏｒ第１
ｔｉＪｔｒＪ１図は」と訂正する。 （２）　　明１ａ４１Ｋ　３２　Ｋｇ　１４（ｆＯｒ　
ＩＩＣｋｖｈで、　Ｊ　ｔ「における」と訂正する。

Claims (1)

【特許請求の範囲】
1. （１）ロボットの被制御体をあらかじめ指定された複数
   の指定経由位置間にて、それらを結ぶ直線上を移動しつ
   つ、該指定経由位置近傍ではあらかじめ設定された前記
   直線からの最大近傍通過誤差量、該指定経由位置を通過
   すると条件、および外接点における線速度が前記直線移
   動のそれに等しいとの条件を基本として決・定される外
   接多項式曲線上を前記外接多項式曲線の変曲点と外接点
   との間では線速度を一定に保ちながら、また前記変曲点
   と該指定経由位置間では線速度を変化させながら速度を
   制御することにより、加速度を小さく抑えて移動させる
   ように軌道制御することを特徴としたロボットの軌道制
   御方式。