JPS62204307A - ロボツトの軌道制御方法 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 〔産業上の利用分野〕 この発明は回転対偶を有する関節形ロボットの軌道を制
御する方法に関するものである。

〔従来の技術〕

第８図は６自由度関節形ロボットを示す図、第９図は第
８図に示したロボットの機構を示す図である。図におい
て、１はロボットの本体、　　２１ｄ制御装置、３は本
体１と制御装置２とを接続するケーブルである。また、
１１〜１６は回転対偶（関節）、１７はハンドである。

このような６自由度関節形ロボットは空間内の物体の位
置と姿勢の６自由度を制御できるため、すなわち回転対
偶１１〜１６を回転させることによシ、ハンド１７の位
置、姿勢を自由に操ることができるため、従来人手によ
ってなされた作業を代行させうる利点がある。

第１０図は第９図に示した機構の回転対偶の駆動系を示
す図である。図において、２１はモータ、２２はモータ
２１に連結されたタコジェネレータで、タコジェネレー
タ２２でモータ２１の角速度を検出する。２６はタコジ
ェネレータ２２に接続されたＡ／Ｄ変換器％　２３はタ
コジェネレータ２２に連結されたパルスエンコーダ、２
７はパルスエンコーダ２３に接続されたカラ／りで、パ
ルスエンコーダ２３によシモータ２１の回転をパルス列
に置き換え、このパルスをカウンタ２７で計数すること
によシ、モータ２１の回転角度を求める。

２４はパルスエンコーダ２５に連結された減速機で、減
速機２４は回転対偶１１に連結されている。

２８はＤ／Ａ変換器、２９はアンプ、３０はシステムバ
スである。なお、回転対偶１２〜１６の駆動系も同様の
構成である。

そして、従来このような関節形ロボットの軌道を制御す
るには、モータ２１等の回転対偶１１〜１６の駆動装置
の回転角度に基づいて駆動装置の角速度を指令している
。

〔発明が解決しようとする問題点〕

しかしながら、モータ２１と回転対偶１１との間の動力
伝達系に減速機２４があシ、減速機２４は通常低剛性で
あるため、ワーク、ツール等の重力による外部負荷、ハ
ンド１７等のロボットの自重、回転対偶１１〜１６の回
転による遠心力、回転対偶１１〜１６相互の回転による
コリオリカ等によって減速機２４ＧＣバネたわみが生ず
るので、モータ２１の回転角度と回転対偶１１の回転角
度との間には減速機２４のバネたわみＫよる角度偏差が
生ずる。たとえば、減速機２４のバネ特性が第１１図に
示される特性であるときには、減速機２４から１ｍのと
ころに６．８Ｋ１１＋の力が作用すると、減速機２４は
Ｚ４分ねじれるので、減速機２４から１ｍ離れたところ
の変位は２．１５ｗ＋となる。このことは回転対偶１２
〜１６についても同様である。

したがって、角度偏差が生ずると、ロボットの軌道を高
精度で制御することができない。

この発明は上述の問題点を解決するためになされたもの
で、軌道を高精度で制御することができるロボットの軌
道制御方法を提供することを目的とする。

〔問題点を解決するための手段〕

この目的を達成するため、この発明においては、回転対
偶を有する関節形ロボットの軌道を上記回転対偶の駆動
装置の回転角度に基づいて制御する方法において、上記
回転対偶の動力伝達系に生じたバネたわみによる角度偏
差を求め、上記回転角度に上記角度偏差を加算する。

〔作用〕

このロボットの軌道制御方法においては、駆動装置の回
転角度に角度偏差を加算するから、回転対偶の実際の回
転角度に近い値に基づいてロボットの軌道を制御するこ
とができる。

〔実施例〕

まず、減速機のバネたわみによる各回転対偶自体の角度
偏差Δθ□の演算について説明する。第２図に示すよう
に、回転対偶１と回転対偶ｉ＋１との間をリンク１、回
転対偶１の対偶軸に沿った単位ベクトルをｚｉ−１％回
転対偶ｉ、ｉ＋１の対偶軸の共通垂線に沿った単位ベク
トルをＸ−、、リンク１の原点をＯｉ、単位ベクトルｚ
１−４に沿ったリンク１の長さすなわちオフセット長を
ｄ□、単位ベクトルマ。

Ｋ沿ったリンク１の長さすなわちリンク長さを”ｔ、ｔ
＋ｔ　％単位ベクトルマ、−１と単位ベクトルｉ、との
なす角すなわち回転角度をθい単位ベクトル；１−４と
単位ベクトルｚ１とのなす角すなわちリンクねじれ角を
αｉｉや、とする。この場合、座標系０ｉ−Ｘｉｙｌｚ
ｌを座標系０１−１−ｘｌ−１”ｉ−１２ｉ−１に変換
する行列Ａ　は次式で表わされる。

ｃｏｓθ　１蝕θ”ｃｏｇα　　　ｓｉｎθ・ｓｉｎα
ｉ　　　　　　　ｉ　　　　　　工、ｉ−Ｈｉ　　　　
　　ｉ、ｉ＋１Ａ、−１＝＝　ｓｉｎθ、　　ｃｏｓθ
、”ＯＯＢα、、＋、−ｃＱ９θ、６ｓｉｎα１．１＋
、（１）Ｏｓｉｎαｉ、１＋１　　　　００８α甲＋１
そして、静止座標系に対するリンク１の姿勢を示す行列
Ａ。は次式で表わされる。

Ａｏ＝Ａ　ｏ＠　Ａ、、　　　　　　　　　　　　　　
（２）原点δ、の原点ｄ、−１に対する位置偏差Ｐ１−
１は次式で表わされる。

ｐ、−、＝ｄ、°ｚｉ−１　”　ａｉ’ｉ＋ｌ°ｘ　、
　　　　　　　（３）静止座標原点に対するリンク１の
原点呂は次式で表わされる。

凡＝ｖ′否゛十可−１（４） また、第３図に示すようＩｃ、ＩＪンク１の重心位置を
座標系０．−ｘエフ１　ｚｉで表わしたものを（Ｘ〒、
へ。

ｚｇ、）とすると、静止座標原点からのリンク１の重心
位置ｐ　ｉは、転置を（）ｔで表わすと、次式でｇ。

与えられる。

Ｐ、：　＝ｕ　＋”６　（Ｘ？　＋　３’ｔ　＋　Ｚ−
）’　　　　　　（５）また、回転対偶１の角速度を１
１とすると、リンク１の原点への静止座標原点に対する
角速度ω０、並進速度虱は次式で与えられる。

１１ｄ＝＝（ｄＪ　　・；（６） ｉ　　　　　ｉ　−１ｉ　　　　ｉ　−１ワ、＝；、、
−八１・】、・”５−４（７）す／り１の重心位置の並
進速度引は次式で与えられる。

ｖ＝ｖ１−Ｐｇｏ・ω、（８） さらに、回転対偶１の角加速度を−ｉ□とすると、リン
ク１の原点可の静止座標原点に対する角加速度ゐ１、並
進加速度÷１は次式で表わされる。

ゐ、＝ゐ、−１＋λ・Ｍｉ−１”ｉ−１・】、・丁、−
、、（９）Ｖｉ　＝ｖ、−ｊ　＋ＧＪｉ・Ｐ、−４＋ω
１°ω１°Ｐｔ−１（１０リンク１の重心位置の並進加
速度÷ｇｉは次式で表わされる。

ｖｇｉ＝ｖ１＋ω１（ｐｇｏ−Ｐｏ） ＋ω１・ω、　（Ｐｇｏ−Ｐｏ）　　　　　　　　（ロ
）リンク１の重心ｉＲにおけるＯｌ　　”ｉ　）’ｉ　
ｚｉ　座標系に対する慣性モーメントテンソルを工。１
とすると、リンク１０重心位置における静止座標系に対
する慣性モーメントテンソルエ、は次式で与えられる。

１１＝輻・Ｉｏ、（Ａ′ｏ）’　　　　　　　　　　　
（１回転対偶ｉに対するリンクｊの重心位置における慣
性モーメントテンソルＩ、は次式で与えられる。

Ｉｉ＞　＝（Ｍｔ−１）を弓、・；１−１　　　　　　
　α３したがって、リンクｊの質量を町とすると、第４
図に示す関係から、回転対偶１の全慣性モーメントＪｉ
は次式で求められる。

、ｒ、＝　３；　（工、＋ｍ、ｌ（ｒ、。−Ｐニー’）
虱−１１２）　　α→ｊｌｌＩ！１＋１ さらに、第５図に示すように、リンク１が回転対偶１か
ら受ける力、モーメントをそれぞた’１−１１Ｍ、−４
とし、リンク１が回転対偶１＋１に及ぼす力、モーメン
トをそれぞれＶ、　、　Ｗ、とし、また重力加速度を１
とすると、次式が成立する。

ｍ１°ｙ、＝ｐ、、　−Ｆ、＋ｍ、　・ｇ０１９工、・
ゐ、鋼、・工、・；フｉ＝”１−１−ｉ＋（ｉ’ニー’
−ｉテニー’）’ｉ−１−（Ｐｏｌ−１−（Ｐｏ−Ｐ 叫、（至）式からＦｉ　−１１ｉ’１ｉ−１は次式で与
えられる。

Ｖｓ−１＝ｍｓ　（ｖｌ　ｇ　）＋　Ｆｌ　　　　　　
　　　（ｔｉｉｌｉｉｌ−１”ＩｉｏＺｉ”’ｉ・工１
”ｉ　（ｊ’ニー’　ｊ’：０）Ｆｌ−１＋（ｐｏ−ｐ
。）　Ｆ、　＋　Ｍ、　　　　　　　　　　（至）回転
対偶１にかかるトルクτ１は次式で表わされる。

τ１°ｚ１−１°Ｍ、−１ｃ′９ このトルクτｉは回転対偶１の慣性モーメントＪ、を駆
動するトルクをも含んでいるので、回転対偶１にかかる
外乱トルクτ、１は次式で求められる。

τ　＝τ１−Ｊ１・λ１　　　　　　　　　　　　（１
）”　ｉ　＝”ａｓ／ｋｔ　　　　　　　　　　　　　
（２１）第６図はこの発明に係るロボットの軌道制御方
法を実施するための制御装置のブロック図である。

図において、２２ａは各回転対偶の駆動装置の角速度を
検出する角速度検出器、２３ａは各回転対偶の駆動装置
の回転角度を検出する回転角度検出器、３８は各回転対
偶の角速度出力器、３３は制御プログラム格納装置、３
１は制御プログラム格納装置３３からシステムバス３０
を介してロボット制御プログラムを読み出し処理する中
央処理装置、３２は中央処理装置３１を補助するために
浮動小数点演算を行なう数値演算処理装置で、数値演算
処理装置３２は浮動小数点の加減乗除、三角関数、逆三
角関数、平方根演算を行なう。３４は操作者がロボット
に動作を指令するだめのロボット教示装置で、ロボット
教示装置３４に上多動作軌道を教示する。３５はロボッ
ト教示装置３４によって教示された軌道データとしての
各回転対偶の回転角度、直交位置等を格納しておく制御
データ格納装置、３６は制御プログラムを制御プログラ
ム格網装置３３１Ｃ格納する前に一時格納しておき、ま
た軌道データを一時格納しておく外部記憶装置である。

つぎｌＣ１第１図に示す制御プログラムに基づいてこの
発明忙係るロボットの軌道制御方法を説明する。まず、
回転角度検出器２３ａから各回転対偶の駆動装置の回転
角度を読み込み、また角速度検出器２２ａから各回転対
偶の駆動装置の角速度を読み込む。つぎに、上記回転角
度から各回転対偶自体の回転角度θ、を計算し、また上
記角速度から各回転対偶自体の角速度１１を計算する。

つぎＫ、回転角度θ１から各リンクの原点Ｐ。と姿勢Ａ
。を計算し、また各リンクの重心位置Ｐｇｏを計算する
。

つぎＫ、重心位置ｒ。、角速度】工から各リンクの重心
位置の並進速度弘、並進加速度Ｑｇ□を計算する。つぎ
に、姿勢Ａ１から各リンクの慣性モーメントテンソルＩ
、を計算し、また各回転対偶の全慣性モーメントＪ１を
計算する。つぎに、各回転対偶にかかるトルクτ１を計
算し、各回転対偶自体の角度偏差Δθ□を計算する。つ
ぎに、各回転対偶の駆動装置の回転角度に角度偏差Δθ
□から求めた上記駆動装置の角度偏差を加算した値に基
づいて、ハンドの位置、姿勢から指令速度を計算し、指
令速度を各回転対偶の駆動装置の角速度に変換し、その
駆動装置の角速度を角速度出力器３８から出力する。

このようにすれば、ワーク、ツール等の重力による外部
負荷、ハンド等のロボットの自重、ロボットの作動中に
生じる回転対偶の回転による遠心力、回転対偶相互の回
転によりコリオリカによって減速機にバネたわみが生じ
たとしても、回転対偶の実際の回転角度に近い値に基づ
いてロボットの軌道を制御することができる。

また、ロボットが停止しているときには、外部負荷、ロ
ボットの自重によって減速機にバネたわみが生ずるが、
このとき角度偏差Δθ１を求め、この角度偏差Δθ、を
回転角度θ工に加算した値を回転角度θ、としてさらに
角度偏差Δθ１を求める演算を５角度偏差Δθ、が十分
に小さくなるまで繰返せば、回転角度θ□を回転対偶の
実際の回転角度に極めて近くすることができる。

さらに、ロボットが稼動しているときに、所定時間ごと
に角度偏差Δθ１を求め、角度偏差Δθ１を求めた時点
から次に角度偏差Δθ、を求める時点まで、直前に求め
た角度例年Δθ、に基づいて軌道を制御しても、角度偏
差Δθ１を求める時点間における各回転対偶にかかるト
ルクτ、の変動は小さいので、回転対偶の実際の回転角
度に近い値に基づいてロボットの軌道を制御することが
可能である。

第７図はハンドを停止位置から速度１００１ＩＩＩＩ／
秒で水平に移動したときの、時間と７・ンドの指令軌道
からの鉛直方向ｕｆｔ偏差との関係を示すグラフで、実
線はこの発明の方法を実施した場合を示し、破線はこの
発明の方法を実施しない場合を示す。

このグラフから明らかなように、この発明の方法を実施
しない場合には、最大偏差が−２，５ｆｉでちるのに対
して、この発明の方法を実施した場合には。

最大偏差が−［１，３５ｆｉであるから、精度を約７倍
向上することができ、とくに停止状態では偏差が−ａ、
０：２＋ｍであυ、停止時の偏差が極めて小さい。

なお、上述実施例においては、角度偏差Δθ工を外部負
荷、ロボットの自重、遠心力、コリオリカから求めたが
、角度偏差Δθ、ｔ−外部負荷、ロボットの自重から求
めてもよい。

〔発明の効果〕 以上説明したように、この発明に係るロボットの軌道制
御方法においては、回転対偶の実際の回転角度に近い値
に基づいてロボットの軌道を制御することができるから
、軌道を高精度で制御することが可能である。このよう
に、この発明の効果は顕著である。

【図面の簡単な説明】

第１図はこの発明に係るロボットの軌道制御方法を説明
するための制御プログラムを示す図、第２図ないし第５
図は各回転対偶自体の角度偏差の演算の説明図、第６図
はこの発明に係るロボットの軌道制御方法を実施するた
めの制御装置のブロック図、第７図はハンドを水平に移
動したときの時間とハンドの鉛直方向位置偏差との関係
を示すグラフ、第８図は６自由度関節形ロボットを示す
図、第９図は第８図に示したロボットの機構を示す図、
第１０図は第９図に示した機構の回転対偶の駆動系を示
す図、第１１図は減速機のバネ特性を示すグラフである
。 １１〜１６・・・回転対偶 ２３ａ・・・回転角度検出器 ２４・・・減速機 垢　１　図 生ロｌｒアロ７°ラム 箪３図 児４図 第５図 ￥１６　　図 第　ｑ　図 篤１０口 一−−−−−」込止工ムーー＼

Claims (1)

1. 【特許請求の範囲】 １、回転対偶を有する関節ロボットの軌道を上記回転対
   偶の駆動装置の回転角度に基づいて制御する方法におい
   て、上記回転対偶の動力伝達系に生じたバネたわみによ
   る角度偏差を求め、上記回転角度に上記角度偏差を加算
   することを特徴とするロボットの軌道制御方法。 ２、上記角度偏差を外部負荷および上記ロボットの自重
   から求めることを特徴とする特許請求の範囲第１項記載
   のロボットの軌道制御方法。 ３、上記角度偏差を外部負荷および上記ロボットの自重
   ならびに上記回転対偶の回転による遠心力およびコリオ
   リカから求めることを特徴とする特許請求の範囲第１項
   記載のロボットの軌道制御方法。 ４、停止時に、上記角度偏差を求め、上記回転角度にそ
   の角度偏差を加算した値を上記回転角度として角度偏差
   を求める演算を繰返すことを特徴とする特許請求の範囲
   第１項記載のロボットの軌道制御方法。 ５、稼動時に、所定時間ごとに上記角度偏差を求め、上
   記回転角度に上記角度偏差を加算することを特徴とする
   特許請求の範囲第１項のロボットの軌道制御方法。