JPH0199103A - ロボット制御装置 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 ［産業上の利用分野］ 本発明は、自動組立工場等で使用されるロボットの動作
を制御するロボット制御装置に関する。

［従来の技術］ 種々の工業分野で用いられるロボット（主にロボットア
ーム）を適切に制御するためには、間欠的に与えられる
目標位置の間を補間する連続的な軌道を生成することが
必要であり、直線軌道補間と円弧軌道補間が採用されて
いる。

それらのうち、特に円弧軌道の生成には、＃公開６１−
２１４００５号公報に記載されているように、複雑な計
算を行なうため、かなりの演算時間がかかっていた。す
なわち、従来の一般的な円弧軌道生成方法は、円の中心
Ｇから円弧上の一点（目標位り、ある時刻ｔにおけるベ
クトルＧＰｉを、それより１サンプリング時間前のベク
トルＧ　Ｐ　ｉ−１をΔθだけ回転させることによって
求めるようにしている。このとき、Δθを一定にすると
円弧の始点と終点で速度が不連続となるため、ロボット
に振動が発生する。そのため、Δθの値を目標位置毎に
変えて演算を行なうようにしているが、演算に多くの時
間を要し、サーボ制御のサンプリング時間を短縮できな
かった。

これを解決するため、上記公報に開示された発明では、
円弧軌道の加減速部分を直線軌道に置き換えることによ
り、計算を簡単にしている。

［発明が解決しようとする問題点］ しかしながら、上記の発明に係る円弧軌道生成法では、
加減速部分を直線とするので、正確な円弧軌道ではなく
なる。特に、加速時間が長い場合や円の半径が小さい場
合には、直線に沿って移動する距離の割合が大きくなり
、正確な円弧軌道を生成できないという問題点があった
。

本発明は、かかる問題点に鑑みてなされたものであり、
正確な円弧軌道上で加減速を行なうことができるロボッ
ト制御装置を提供することを目的とする。

［問題点を解決するための手段］ 本発明は、円弧軌道を生成する軌道生成部と、直交座標
系による位置表現を関節角表現に変換する座標変換部と
、ロボットを前記座標変換部で変換された関節角に位置
決め制御するサーボ制御部とを備えたロボット制御装置
において、前記軌道生成部は、生成すべき円弧軌道の中
心、その円弧を含む平面の法線ベクトル、及び円弧の始
点から終点までの角度により、円弧上の移動点を示すベ
クトルを求め、該ベクトルの角度の加減速によって円弧
軌道を生成するように構成したことを特徴とする。

［作用］ 本発明のロボット制御装置では、要求される円弧軌道の
中心、その円弧を含む平面の法線ベクトル、及び円弧の
始点から終点までの角度を求めれば１円弧軌道上の加減
速は、移動点を示すベクトルの円弧軌道の始点からの角
度の加減速により行なわれる。これは、直線軌道補間と
同程度の計算で実現することができ、円弧軌道と直線軌
道の連続動作軌道の生成も可能である。

［実施例］ 第１図に示すように、本発明のロボット制御装置は、円
弧軌道を生成する軌道生成部１と、直交座標系による位
置表現を関節角表現に変換する座標変換部２と、ロボッ
ト４を座標変換部２で変換された関節角に位置決め制御
する信号を生成するサーボ制御部３とを具備する。その
サーボ制御部３では、座標変換部２からの関節角信号と
ロボット４からフィードバックされた現在位置信号とか
ら制御信号を生成し、ロボット４の駆動部に送るように
なっている。

第２図は、本発明のロボット制御装置における円弧軌道
生成の原理を示す図である。

まず、点Ｃを中心とする半径ａの円弧ＡＢに沿ってロボ
ットアームを移動させる場合、その始点（移動開始点）
Ａ及び終点（移動終了点）Ｂの位置と姿勢を表わす同次
変換行列を、それぞれＰｌ。

Ｐ２とする。このとき、円弧軌道上の時刻ｔにおける移
動点Ｘの位置と姿勢を表わす同次変換行列Ｐ　（ｔ）は
、次のように表わされる。

Ｐ　（ｔ）　＝　Ｐ　１　φＤ　（ｒ（ｔ））　　　　
　　−−−−（１）ここで、ｒ（ｔ）は後述のように無
次元化された相対移動量であり、以下これをｒと表わす
、Ｄ（ｒ）はＰｌから見た変換行列であり、次式で表わ
すことができる。

ここで、Ｗｒは回転を表わす３Ｘ３行列、ｐｒは並進を
表わす３列ベクトルであり、ｒに比例して回転量、並進
量が変化する。また、Ｄ　（ｒ）は次の条件を満たすよ
うに決める。

これにより、Ｄ（「）はＰｉ、Ｐ２間の軌道に沿ってｒ
に比例した移動を行なわせる変換行列となる。

また、始点Ａの位置を示すベクトルをＰｓ、姿勢を示す
３×３行列をＷｓとすると、Ｐｌは次式％式％ 式（４）及び（２）を式（１）に代入すると、時刻ｔに
おける同次変換行列Ｐ（ｔ）は、次式で表わされる。

ここで、Ｄ　（ｒ）は増分量を表わしており、そのうち
のＰｒはＰｌからの位置の増分量を表わしている。

また、円弧の中心Ｃを示すベクトルをｐｃ、円弧を含む
平面の法線ベクトルをｅ２円弧の始点Ａから終点Ｂまで
の移動角度をθＣとすれば、時刻ｔにおける移動点Ｘの
位置を示すベクトルＰ　（ｔ）は、次のように表わされ
る。

ｐ　（七）　　＝ｊ）Ｃ＋　α−ｃａｓ（ｒ　θｃ）＋
ｑ−ｓｉｎ（ｒθ　Ｃ）ただし、αは円弧の中心Ｃから
始点Ａへの半径を示すベクトルで、α＝Ｐｓ−ＰＣと表
わされ、ｎはベクトルｅとαの外積（ｅＸα）である、
ここで、ｒ（０≦ｒ≦１）は相対移動量を表わす。

従って、上記（６）式における角度ｒθＣをＯ〜Ｏｃの
範囲で増加させることにより、点Ｘは指定の円弧軌道を
たどる。その円弧軌道上で加減速を行なうためには、ｒ
の増加量を変化させればよい。

ｒとしては、ロボットを制御するために用いられている
種々の加減速関数を無次元化し、Ｏ〜１の範囲として（
８）式に代入する。これにより、点Ｘは正確な円弧軌道
上を加減速しながら移動することになる。

ここで、上記の円弧軌道におけるＰｌからの位置の増分
量を表わすベクトルをΔｐとすれば、ΔＰ　＝　Ｐ　（
ｔ）　−ｐｓ　　　　　　　　−一−−（７）となる。

このΔｐは基準座標系における増分量を表わしているの
で、始点Ｐ１から見た値に変換し増分量Ｐｒを求める。

ｐ、＝ｗｓ−’Δｐ−一一−（８） これを式（５）に代入すると、 となり、式（８）の計算をする必要がなくなる。従って
、直線補間と同程度の計算量で円弧上の軌道を計算する
ことができる。

次に、ｒに適用する加減速方式を説明する。

加減速の計算に必要なパラメータは、最大加速度ａｓ＋
最大速度Ｖｒ＋移動距ｆｌｌＩ　ｄ　ｐ及び加速時間ｔ
８であるｅａｓ、Ｖｒ、ｄＰの関係より、第３図に示す
ような２通りの動作モードに分けることができる。

第３図（Ａ）は、加速と減速の間に最大速度Ｖｒによる
定速移動を含む動作モードを示す、この動作は、次式が
成り立つときに行なわれる。

３ｖｒ２／２ａｓ≦ｄ　ｐ　　　　　　　　−−−（１
０）このときの加速時間１．及び定速移動時間ｔ。

は次式で表わされる。

ｔ２　＝　３　Ｖｒ　／　２　ａｓ　　　　　　　　−
一へ（１１）ｔｓ　＝　（ｄｐ−２ｄｅ）／Ｖｒ　　　
　−−−（１２）ただし、ｄｅは移動開始から加速終了
までの移動距離である。

第３図（Ｂ）は、移動距離が短いため、指令速度に達す
る前に減速を始める動作モードを示す、この動作は、式
（７）が成り立たないときに行なわれる。このときの加
速時間１ｅ及び定速移動時間ｔ。

は次式で表わされる。

ｔｅ＝ｖ’丁了７フ１−τ １、＝０ これらの値から再動作モードに対して、指令位置θ（１
）は次のように求められる。

加速区間（０≦ｔ≦ｔｅ）では θ（ｔ）　＝Ａ　（ｔ４−２　ｔｅｔ３　）　＝０１（
ｔ）定速区間（ｔ　ｅ　＜　ｔ　＜　ｔ　ｅ＋　ｔ　Ｓ
　）では０（ｔ）　＝　ｖｒ　　（ｔ　−ｔｅ　）　＋
０Ｌ（ｔｅ　）減速区間（ｔｅ　＋ｔＳ≦ｔ≦ｔｅｎｄ
）ではθ（ｔ）＝ｄｐ−０１（ｔｅｎｄ　−ｔ）　　　
　　−−−（１５）ただし、Ａ＝４ａｓ　／ｌｅ　、ｔ
ｅ＝２ｔ＠　＋ｔｓである。

前述の円弧軌道の場合、θ（１）は移動点Ｘの角度を表
わす。このとき１式（１３）〜（１５）において、ｄｐ
＝θＣとすれば Ｏ≦θ（１）≦θＣ−−−（１Ｂ） であるので、相対移動量ｒは次式により無次元化される
。

ｒ　＝　０　（ｔ）　　／　Ｏｃ　　　　　　　　　　
　　−−−（１７）移動点Ｘの角速度６（ｔ）、角加速
度？ｊ（ｔ）についても、同様にｒの速度、加速度が用
いられる。

これにより、円弧軌道上で加減速を含めた運動を実現す
ることができる。

次に、軌道を滑らかにつないで連続動作を実現する方式
について説明する。

第４図に示すように、２つの直線軌道Ｐ１→Ｐ２及びＰ
２→Ｐ３の補間動作を行なう場合において、Ｐ２は経由
点として止まらないものとする。このとき、ＰＩ−Ｐ２
及びＰ２→Ｐ３の動作に関する変換行列をそれぞれＤｉ
　（ｒ）　、　Ｄ２　（ｒ）とする。

始点Ｐ１から補間開始点Ｍまでの軌道は、前掲の式（１
）より Ｐ（ｔ）　＝　ＰＩ　・Ｄｔ　（ｒ）　　　　　　−−
−（１８）と表わされる。Ｍ点に達した時にＰ２→Ｐ３
の動作を開始すれば、Ｍ−８間の移動はＰ１→Ｐ２とＰ
２→Ｐ３の動作の合成になる。従って、この軌道は次式
で表わされる。

Ｐ（ｔ）　＝　Ｐｉ　　−Ｄｉ　（ｒ）　＠　Ｄ２　（
ｒ’）ただし、ｒ′はＭ点においてｒ’＝０となる相対
移動量である。Ｎ点に達した時、Ｐ１→Ｐ２の動作が終
了するので、Ｎ点からＰ３までの動作は、次式で表わさ
れる。

Ｐ（ｔ）　＝　Ｐ２−　Ｄ２　（ｒ’）　　　　　　−
−−（２０）次に、Ｍ点の決定方法を説明する０Ｍ点は
、Ｐｉ→Ｐ２及びＰ２→Ｐ３の動作の加速時間を比較す
ることにより決定する。第５図（Ａ）に示すようにＰ１
→Ｐ２の減速時間が短い場合、減速と同時にＰ２→Ｐ３
の加速を開始する。逆に、第５図（Ｂ）のようにＰ２→
Ｐ３の加速時間が短い場合は、Ｐ１→Ｐ２の減速終了と
Ｐ２→Ｐ３の加速終了が一致するようにＢ点を決定する
。

直線補間を例として説明したが、円弧補間についても同
様の方法で連続動作を行なうことができる。また、直線
補間と円弧補間は共に式（１）の変換行列Ｄ　（ｒ）を
用いて計算を行なうことができるので、相互の連続動作
も可能である。

［発明の効果］ 以上のように、本発明のロボット制御装置は、要求され
る円弧軌道上の移動点を示すベクトルを求め、該ベクト
ルの円弧軌道の始点からの角度の加減速によって円弧軌
道を生成するようにしたので、ロボットを正確な円弧軌
道上で加減速制御することができる。また、円弧軌道を
直線軌道と同程度の計算量で計算できるので、演算時間
を短縮できると共に、両軌道を滑らかに連結することが
できるという効果が得られる。

【図面の簡単な説明】

第１図は本発明の構成図、 第２図は本発明における円弧軌道生成方式の説明図、 第３図は加減速の動作パターンを示す図、第４図は連続
動作軌道を示す図。 第５図は連続動作のタイムチャートである。 ｌ−一一一軌道生成部、 ２−一一一座標変換部、 ３−一一一サーボ制御部、 ４−一一一ロボット。 第３図 ｔ、　　（Ａ）　　　　　ｔｅｎｄ 第５図 （Ａ） （Ｂ）

Claims (1)

【特許請求の範囲】
1. 円弧軌道を生成する軌道生成部と、直交座標系による位
   置表現を関節角表現に変換する座標変換部と、ロボット
   を前記座標変換部で変換された関節角に位置決め制御す
   るサーボ制御部とを備えたロボット制御装置において、
   前記軌道生成部は、生成すべき円弧軌道の中心、その円
   弧を含む平面の法線ベクトル、及び円弧の始点から終点
   までの角度により、前記円弧上の点を示すベクトルを求
   め、該ベクトルの角度の加減速によって円弧軌道を生成
   するように構成したことを特徴とするロボット制御装置
   。