JPH03121505A - ロボットの制御方法とその装置 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 ［産業上の利用分野］ 本発明はロボット等の多自由度機構の軌道制御方法に係
り、特に任意の与えられた軌道に対して高精度な軌道制
御を行うに好適なロボットの制御方法に関する。

［従来の技術］ 従来のロボットの制御方法は、特公昭６１−２９６４号
公報に記載のように与えられた直線軌道を補間すること
により中間点を求め、この中間点に対応したアクチュエ
ータの変位を座標変換により計算し、この変位を７クチ
ユエータの目標値として位置制御を行うものであった。

また特開昭６１−１１８８０９号公報に記載のようにハ
ンドの最終目標位置と現“右位置との差に応じたハンド
の速度を決定し。

これをアクチュエータの速度に変換して、アクチュエー
タの速度制御を行うものがあった。

［発明が解決しようとする課題］ 上記従来技術はその前者の方法ではロボットの実際の駆
動情報がアクチュエータ毎のサーボ機構にしかフィード
バックされず、ロボットの機構の非線形性に起因する制
御誤差を補償することができないため、軌道精度を向上
させることが困難であった。また後者の方法では直線軌
道にしか適用できないため、一般の曲線軌道に対応する
ことができない問題があった。

本発明の目的は任意の与えられた軌道に沿って高精度に
ハンドを駆動することができるロボットの制御方法を提
供することにある。

［課題を解決するための手段］ 上記目的を達成するために１本発明によるロボットの制
御方法は与えられた軌道上に各サンプリング時毎の目標
位置としての中間点を定め、この中間点とハンドの現在
位置との偏差を求め、この偏差に応じてハンドの目標速
度を決定してロボットを制御するようにしたものである
。

また軌道精度をより向上させるために、上記偏差を軌道
の接線方向の誤差と法線方向の誤差とに分解し、法線方
向の誤差に対するゲインをより大きくすることを可能に
したものである。

［作用］ 上記ロボットの制御方法は任意の与えられた軌道に対し
速度パターンを設定して各サンプリング時毎の目標位置
としての中間点を定め、この中間点とハンドの位置との
偏差に応じてハンドの目標速度を決定して軌道制御して
いるので、直線軌道のみでなく一般の曲線軌道に対応し
て高精度な制御を行うことができる。

また上記偏差を軌道上の局所座標系の要素としての軌道
の接線方法と法線方向の誤差に分解し。

別個に最適なフィードバックゲインを設定できるので、
さらに高精度な制御を図ることができる。

［実施例］ 以下に本発明の一実施例を第１図ないし第４図により説
明する。

第３図は本発明によるロボットの制御方法の一実施例を
示すロボットの軌道の説明図である。第４図は第３図の
軌道に沿っての速度パターン図である。

第３図に示すような静止座標系０−ｘｙｚにおいて与え
ら九た軌道に対し、その軌道に沿ってハンドを駆動する
ときの速度パターンを設定する。

第３図の軌道において点Ａを始点１点Ｂを終点とする。

軌道の始点Ａおよび終点Ｂでの加減速を考慮し、たとえ
ば第４図に示すような台形速度パターンとする。第４図
の台形パターンで囲まれた部分の面積Ｓｎは、 ５ｎ＝ｆ　　　ｆ（ｔ）ｄｔ ・・・（１） で、軌道の点Ａ、Ｂ間の長さに等しい、ここにｆ（ｔ）
は速度パターンの時間関数、Ｔはサンプリング時間、ｎ
Ｔは軌道の点Ａ、Ｂ間をハンドを駆動するのに要する時
間である。

個々でハンドの駆動時の各サンプリング時刻Ｔ。

２Ｔ、３Ｔ、・・・、ｎＴ毎の目標位置として、軌道上
に中間点を定める。サンプリング時刻ｉＴ、ｉ＝１．２
．・・・、ｎのとき、第４図の速度パターンより。

５４＝ｆ　　　ｆ（ｔ）ｄｔ　　　　　　　　　　・・
・（２）とし、中間点ｐｉを軌道上に始点Ａからの距離
Ｓ。

の位置に定める。この中間点ｐｉを時刻ｉＴでの軌道制
御の目標位置とする。さらに中間点Ｐ１において軌道の
接線方向、法線方向を座標軸とする局所座標系である直
交座標系（フレネ標構）を定める。軌道が直線のときに
は座標軸の接線方向は直線方向であり、法線方向は任意
であるが、これは後で示すようにハンドの位置から軌道
への垂線方向にとる。

このサンプリング時刻ｉＴでのハンドの現在位置はロボ
ットのアクチュエータの変位を検出した結果から計算に
より求めることができる。いまハンドの時刻ｉＴにおけ
る現在位置を静止座標系０−ｘｙｚにおいてベクトルｒ
ｉ、中間点Ｐ、の目標位置をベクトルＰｉで表わす。さ
らに軌道の中間点Ｐｉ上において定めた直交座標系の軸
の方向を与える単位ベクトルを第３図のＯ印部分の拡大
図に示すように接線方向をＸｉ、法線方向をｙＩ、もう
１軸をｚ＋　（＝ｘ＋Ｘｙ４）とする。

ここでハンドの目標位置からの偏差ｅ１は、ｅｉ＝ｐｌ
　　ｒｉ　　　　　　　　　　−°−（：。

で与えられ、偏差ｅ、の局所座標系Ｐｉ　　ｘｔｙ＋Ｚ
ｉの各軸方向の成分ｅχｇ　ｆ３１／ｓ　ｅＺは、に沿
ったフィードバックゲインである。にχは軌道に沿って
の遅れ、ｋ、およびに２は軌道からの垂直誤差に関係す
るゲインとなる。垂直誤差を小さくするにはに、および
に２を大きくすればよい。

ハンドの速度目標値Ｖｉが決定されると、これに対応し
たアクチュエータの速度６は、〒 θ＝Ｊ−・７．　　　　　　　　　　　・・・（６）す で与えられる。ここにθはアクチュエータの速度δ８．
δ８．・・・から成るベクトル、Ｊはヤコビ行列である
。ヤコビ行列Ｊは機構の寸法定数とアクチエータの変位
から計算することができる。

軌道が直線のときには軌道の法線方向に決定されない、
そこで直線の方向の単位ベクトルをＸｉとし、 である。ここに右辺はベクトルの内積である。そこでハ
ンドの目標速度ｖＩを、 Ｖ＋＝ｋｚｅｚｘｔ＋ｋｙｅｙｙ＋＋ｋｚｅｚｚ＋−（
５）とする、ここににχｅ　ｋＦｐ　ｋ、は局所座標系
の軸と定める。ｙＩはハンドの位置から直線軌道に下し
た垂線の方向の単位ベクトルである。以上の制御を実現
する制御ブロック図を第１図に示す。

第１図は本発明によるロボットの制御方法の一実施例を
示すロボットの制御ブロック図である。

第２図は第１図の制御フロー図である。

第１図の図示しないマイクロプロセッサ等の制御装置に
ハンドを駆動する軌道およびその始点Ａと終点Ｂを与え
る（第２図のステップ２０１）、マイクロプロセッサ等
の制御装置は第４図の速度パターンｆ（ｔ）より（２）
式により軌道を補間して、始点Ａから各サンプリング時
刻ｉＴでの距離Ｓｉより中間点Ｐ１を決定し、この中間
点Ｐ１の目標位置Ｐｉを入力する（ステップ２０２）　
、第１図のアクチュエータ１のモータには変位検出器と
してのパルスエンコーダ２が取り付けられる。パルスエ
ンコーダ２からの出力パルスをカウンタ３で計数するこ
とによりアクチュエータ１の変位が決定できる（ステッ
プ２０３）、アクチュエ−９１の変位をもとにマイクロ
プロセッサの演算装置の座標変換部６によりハンドの現
在位置ｒｉを計算する（ステップ２０４）。

つぎに演算装置の減算部７によりハンドの目標位置ｐ＋
からの偏差θ量を（３）式により計算しくステップ２０
５）、これから目標速度演算部８によりハンドの目標速
度Ｖｌを（５）式により決定する（ステップ２０６）、
アクチュエータ１の目標速度演算部９によりヤコビ行列
Ｊをアクチュエータ１の変位から計算し、その逆行列Ｊ
−１からアクチュエータ１の目標速度θを（６）式によ
り決定する（ステップ２０７）。一方のアクチュエータ
１の速度はカウンタ３の計数値のアクチュエータ１の変
位を微分回路５により微分して検出し、目標速度すとア
クチュエータ１の速度との差を減算器１０により求め、
サーボアンプ４により増幅して、その出力をアクチュエ
ータ１に印加してハンドを駆動する（ステップ２０８）
　、この処理を各サンプリング時刻ｉＴ。

ｉ＝１．２．・・・ｎ毎に行い、ｉ　＝　ｎで処理を終
了する（ステップ２０９）。

なお６自由度ロボットにおいてはアクチュエータ１、エ
ンコーダ２、カウンタ３、アンプ４および微分回路５は
６組あり、アクチュエータ１の変位や速度は６次元ベク
トルとして扱われる。またハンドの現在位置の検出はア
クチュエータ１の変位から計算するのではなく、ハンド
に近接センサ等のセンサを取り付けることにより軌道か
らの偏差を検出するようにしてもよい、またアクチュエ
ータ１の速度はアクチュエータ１の変位を微分して求め
るのではなく、アクチュエータ１にタコジェネレータを
取り付けることによ・り検出することも可能である。

［発明の効果〕 本発明によれば、任意の与えら九た軌道に対してハンド
の高精度な軌道制御を行うことができる。

またフィードバックゲインを軌道の接線方向と法線方向
に別個に設定することができるため、最適なフィードバ
ックパターンを決定することができ、さらに高精度化を
図ることができる効果がある。

【図面の簡単な説明】

第１図は本発明によるロボットの制御方法の一実施例を
示す制御ブロック図、第２図は第１図の制御フロー図、
第３図は第１図のロボットの軌道の説明図、第４図は第
３図の軌道に沿っての速度パターン図である。 １・・・アクチュエータ、２・・・パルスエンコーダ、
３・・・カウンタ、４・・・アンプ、５・・・微分回路
、６・・・座標変換部、７・・・減算部、８・・・ハン
ドの目標速度演算部、９・・・アクチュエータの目標速
度演算部、Ｐ＋・・・ハンドの目標位置、ｒ４・・・ハ
ンドの現在位置。 ｅＩ・・・ハンドの位置の偏差、ｖＩ・・・ハンドの目
標速度、θ・・・アクチュエータの目標速度。

Claims (1)

1. 【特許請求の範囲】 １、与えられた軌道上にサンプリング時毎の目標位置と
   しての中間点を定め、この中間点とその時点でのハンド
   の位置との差に比例するようにハンドの速度指令値を決
   定し、これをロボットを駆動するアクチュエータの速度
   指令値に変換し、これをもとにアクチュエータを制御す
   るロボットの制御方法。 ２、ハンドの位置をその時点でのアクチュエータの変位
   検出結果から計算により求める請求項１記載のロボット
   の制御方法。 ３、与えられた軌道上にサンプリング時毎の目標位置と
   しての中間点とその中間点に対しての局所座標系とを定
   め、この中間点とその時点でのハンドの位置との差を求
   め、その差を局所座標系の要素に分解し、各要素に対し
   別個に設定されたゲインをかけ、それらの値のベクトル
   和としてハンドの速度指令値を決定し、これをロボット
   を駆動するアクチュエータの速度指令値に変換し、これ
   をもとにアクチュエータを制御するロボットの制御方法
   。 ４、局所座標系として中間点を原点とし、軌道方向とハ
   ンドの位置から軌道への垂線に平行な方向とに座標軸を
   定めた直交座標系を用いる請求項３記載のロボットの制
   御方法。 ５、局所座標系として中間点における与えられた軌道の
   フレネ標構を用いる請求項３記載のロボットの制御方法
   。