JPS62204109A - ロボツトア−ム姿勢測定装置 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 イ、「発明の目的」 〔産業上の利用分野〕 本発明は、回転する３つのリンクを持つロボットアーム
に、回転角速度を検出する手段を設置ｉ、この検出手段
の出力を基にしてリンクの各３＠における回転角を求め
るロボットアーム姿勢測定装置に関するものである。

〔従来の技術〕

ロボットは、通常、ＩＩＩ　ｔｉｌｌ装貿の命令に従っ
て、アームを動かし、各種の作業を行なうが、このアー
ムがＩ制御装置の命令通りの空間的位置に移動している
か否かを監視することが必要である。そうでないと、期
待する作業を行なうことができないからである。本明ａ
Ｓで論じる装置は、このロボットアームの現在位置を把
握する装置である。更に詳しく述べると、基準となる座
標系（固定リンク）の３軸方向に対する各可動リンクの
回転角を求める装置である。なお、この装置で求めた回
転角を基にして、アーム先端の現在位置を割出す装置の
部分は、水明Ｉｍでは論じていない。

第３図と第４図を用いて従来のロボットアーム姿勢測定
装置を説明する。第３図は３つの自由度を持つロボット
アームを示したものである。同図において、リンク０は
、ロボットアームを台座に固定する部材であり、その座
標系を（χＯ＋！’０ｒＺｏ）とする。このリンクＯは
回転しない。

リンク１０部材は、第４図に示すモータ４の駆動力によ
りリンクＯの１軸（２０軸）回りを回転（回転角ψ）す
るもので、その座標系を（ｘｌ。

ｙＩ＋　Ｚ＋　）とする。

リンク２の部材は、第４図に示すモータ５の駆動力によ
りリンク１の２軸（Ｚ／＋軸）回りを回転（回転角θ）
するもので、その座標系を（ｘ２゜’／２＊　２２　）
とする。

リンク３の部材は、第４図に示すモータ６の駆動力によ
りリンク２の３軸（χ２軸）回りを回転（回転角φ）す
るもので、その座標系を（χコ。

Ｖｓ　＊　Ｚ３　）とする。

Ｗ４３図のロボットは、以−ヒの３つのモータ４゜５．
６によりリンク１，２．３を回転させ、その回転角甲、
θ、φを制御回路（図示せず）で指令することにより任
意の位置にアームの先端部を移動させることができる。

従って、アーム先端の現在位置を知るためには、この回
転角ψ、θ、φの検出を行なうことが有効である。従来
は、第４図で示す各モータ４．５゜６の軸にパルス発生
器（図示せず）を取付けている。このパルス発生器は、
各モータ４．５．６が一定角度、正回転又は負回転する
ごとにアップ（Ｌｌｌ））パルス又はダウン（ｄｏｗｎ
）パルスを出力する。そして、これをパルス／デジタル
変換器８，９．１０でデジタル信号に変換して里、θ。

φを求めている。

（発明が解決しようとする問題点） しかし、以上のような手段は、回転角の測定精度に改善
の余地がある。出願人の実験によると、パルス発生器の
精度は１００万パルス／回転　において、約＋６０パル
スである。これを角度に直ずと、６Ｑ １ｏｏｏＱＯＱ　　Ｘ　６０〜０，０２°である。

本発明の目的は、高精度で回転角ψ、θ、φの測定を行
なうことができるロボットアーム姿勢測定装置を提供す
ることである。

口、「発明の構成」 （＠照点を解決するための手段） 本発明は、上記問題点を解決するために回転する３つの
リンクを持つロボットアームと、各リンクが一定角度回
転するごとに信号を出力する基準回転角検出手段（３１
，３２，３３）と、各リンクの回転角速度を検出する手
段（１５）と、この手段〈１５）と前記基準回転角検出
手段とから信号を導入し、以下（Ａ＞に記す演算機能を
持つ制御装置と、を備えるようにしたものである。

（Ａ）　　制御装置の演算機能 ロボットアーム先端のリンク（３）の座標系をロボット
アームを台座に固定しているリンク（０）の座標系に座
標変換するための変換行列（Ｃ；）を演算する機能と、 この変換行列（Ｃ３）に含まれる制御装置・の切捨て誤
差と、前記手段（１５）の誤差と、初期設定誤差とを補
正する演算機能と、 前記基準回転角検出手段からの信号を導入し、回転角誤
差ベクトル（ε０）と回転角速度行列ベクトル（δＩＪ
Ｊ（３）の推定を行なう機能と、回転する各リンクの回
転角（ｖ、θ、φ）を算出する機能。

〔実施例〕 以下、図面を用いて本発明の詳細な説明する。

第１図は、本発明の一実施例を示したブロック図である
。また、第２図は第１図装置の動作フローを表わした図
である。第１図の構成は第３図、第４図に示した従来例
と似ている。即ち、ロボットアームは、リンク０〜３で
構成される。そして、リンクＯにてロボットアームは台
座に固定される。

リンク１はモータ４（第１図では図示を省略）により１
軸（Ｚｏ軸）まわりを回転し、リンク２はモータ５（第
１図では図示を省略）により２軸（！１１軸）まわりを
回転し、リンク３はモータ６（第１図では図示を省略）
により３軸（χ２軸）まわりを回転する。

一方、異なる点は次の通りである。

第１は、従来のパルス発生器とパルス／デジタル変換器
８，９．１０に代えて、リンク３の部分に回転角速度検
出手段１５を設けた点である。この回転角速度検出手段
１５として、本川ｍ書ではジャイロ・パッケージを用い
た例で説明する。このジャイロ・パッケージ１５は、例
えば、直交する３つの軸（χコ＊　ｙ３　＊　Ｚｓ　）
のそれぞれについてジャイロが配置された構成であり、
各可動リンク１゜２．３の回転角速度（ωχ、ω１．ω
２）に対応した信号を出力することができるものである
。

第２は、補正演算のための推定値を得るため、各リンク
１．２．３が一定角度回転するごとに信号を出力する基
準回転角検出手段３１．３２．３３を設けた点である。

これについては第５図を用いて詳述する。

第３は、上記ジャイロ・パッケージ１５からの信号（ω
Ｘ、ωｙ、ω２）と上記基準回転角検出手段３１．３２
．３３からの信号（Ｍ／３１．θ３２．φ３３）を導入
し、これに演算を加える制御装置２０＠ＥＱけた点であ
る。この゛制御装置２０は、例えばコンピュータ等で構
成される。第１図では、本発明を分り易く説明するため
、制御Ｉ装置２０の部分を、制御装置２０が行なう動作
のブロック２１〜２８で示している。

なお、本発明では、この制ｔｉｌｌ装置２０の演算機能
が究明の要部であるので、以下、（ａ）〜（ｄ）に項分
けしてその機能を詳述する。

（ａ）　　座標変換するための変換行列を得る機能制御
装［２０は、以下に述べるような演算を行い変換行列を
得る。

第１図のロボットは３自由度を持つものであり、第３図
で説明したようにリンクＯ〜リンク３まで複数の座標系
を持っている。そしてジャイロ・パッケージ１５から出
力される回転角速度のデータは、リンク３の座標系にお
けるものである。一方、このリンク３は、ロボットの作
業と共に時々刻々変化するものであるから、ロボットア
ーム先端の位置を正確に知るためには、リンク３からの
データをロボットアームを固定している静止した座標系
（リンクＯ）に変換することが必要である。もっとも、
このリンクＯの座標系も自転する地球の大地に置かれて
いるものであるから、完全に静止した座標系とは言えず
、この点については後述する。

まず、制御装置２０では、角速度検出部２２によりジャ
イロ・パッケージ１５か１３の角速ＰＸ（ωＸ、ω９、
ω２）を表わす信号を検出する。そして、座標変換部２
６にて、以下の演算を行い、リンク３からリンクＯへ座
標変換する変換行列ｃ３を得る。

まず、リンクＯ［座標系（χｏ、Ｗｏ、Ｚｏ）］を座標
軸ＺＯの回りに仮想的に角度軍だけ回転して得られる座
標系は、リンク１である。このリンク１の座標系（χ＋
　＋　ｙ＋　＋　Ｚ＋　）をリンクＯの座標系に変換す
る場合の座標変換式は（ａｌ）式で表わすことができ、
その変換行列Ｃ４は、一般に（ａ２）式で表わすことが
できる。

同様に、リンク１を座標軸ｙ＋の回りに角度θだけ回転
して得られる座標系は、リンク２であるから、リンク２
の座標系（χ２　＋　’ｌ　２　＊　２２　）をリンク
１の座標系（χＩ　＋　ｙ＋　’＋　Ｚ　＋　）に変換
する場合の変換行列Ｃ２は、一般に（ａ３）式で表わす
ことができる。

同様に、リンク２を座標軸χ２の回りに角度φだけ回転
して得られる座標系は、リンク３であるから、リンク３
の座標系（χ３１　’／３＋　Ｚ３　）をリンク２の座
標系（χ２．！／２１Ｚ２）に変換する場合の変換行列
Ｃ蚤は、一般に（ａ４）式で表わすことができる。

ここで、以下に示す４つの式が成立っている。

操行列Ｃ３は、次式で表わずことができる。

Ｃｚ＝Ｃ＋　　・Ｃ２・Ｃ１ ・・・　（ａ５） 上式でいうχコ、ｙコ＋Ｚ３は、具体的には角速度ωχ
、ωｙ、ω２を検出した各ジャイロの軸のことである。

以上の演算を座標変換部２Ｇで行い、リンク３からのデ
ータをリンク０における座標へ変換する変換行列Ｃ３を
得る。

（ｂ）　　各可動リンクの回転角（ｖ、θ、φ）を算出
する機能 制御装Ｍ２０は角速度検出部２２．座標変換行列演算部
２Ｇ１回転角演譚部２８を用い次に述べるような演算を
行なって回転角（１１／、θ、φ）を得る。

一般にリンク３の座標系（χ３　＊　ｙ３　＊　ｚ、　
）を慣性座標系（χｔ　、　ｙｉ　、Ｚｉ・・・地球の
自転にも影響されない静止座標系）に座標変換する式は
（ｂｌ）式で表わすことができる。

ここで、Ｃ３は前記（ａ）項で説明したように、リンク
３の座標系を慣性座標系に座標変換する場合の変換行列
を表わすものである。

（ｂｌ）式において右辺の行列の要素χ３　＊　’／　
３　ｒＺ３は、具体的には回転角遠吠ωχ、ω１．ω２
を検出した各ジャイロの軸を意味する。この回転角速度
ωχ、ωｙ、ω２を（χｓ　＋　Ｖｓ　＊　２３　）で
表現した行列（以下、回転角速度行列と記す）をＷｉ３
とすれば（ｂ２）式で表わすことができる。

また、同様な理論により、（ｂ２）式に相当する各回転
角速度行列Ｗｔ　ｅ　＋　Ｗｅ　ｏ　＋　Ｗｏコを次の
ように定義する。

（イ＞Ｗ（ｅは、上記した慣性座標系に対する地球固定
座標系〈χｅ　、ｙｅ　＋　Ｚ　ｅ・・・地球の自転に
より変動する座標系）の回転角速度行列であり、（ｂ３
）式で表わされる。

（ロ）Ｗｅｏは、地球固定座標系に対するリンクＯの座
標系（１：ｏ　＊　ｙｏ　＊　Ｚｏ　）の回転角速度行
列であり、（ｂ４）式で表わされる。

ンク３の座標系（χコ、ｙコ、Ｚ３）の回転角速度行列
であり、（ｂ５）式で表わされる。

クトル表現を、以下のように定義する。

リンク３の座標系（χ３　＋　ｙ３　＋　Ｚ３　）から
リンクＯの座標系（Ｚｏ　＋　’！Ｉｏ　、　Ｚｏ　）
への変換行列Ｃ；　（前記（Ｃ５）式ンは、上記した回
転角速度行列ＷＯ３を用いて、（ｂｅ）式を解くことに
より求めることができる。

＃Ｃ３−Ｃ３・Ｗｏ　　３　　　　　　　　　　　　　
　（ｂｅ）ここで、通常、ジャイロは地球の自転をも検
出する程の感度であるから、ジャイロの出力は、慣性座
標系に対するリンク３の座標系で表現される（ｂ２）式
で表わされた回転角速度行列ＷＬ３を示していることに
なる。そして、このジャイロ出力、即ち、慣性座標系に
対するＷｔｓは、今、求めたい回転角速度行列ＷＯ３（
リンク０の座標系に対するリンク３の座標系の回転角速
度）のほかに、（ｂ７）式で示す余分な回転角速度行列
の成分をも含むものである。

Ｗ＋　　３　　＝Ｗｉ　　ｅ　　＋Ｗｅ　　ｏ　　＋Ｗ
ｏ　　３　　　　　　　　　　　　（１）？＞ここで、
リンクＯの座標系（χＯ＋　’！１０　ｌ　ｚｏ）は、
地球に固定した座標であるから、 Ｗｅｏ＝０ とすることができる。

また、リンクＯの座標系に対するリンク３の座標系にお
いて、通常、ロボットアームの動ぎは速く、その回転角
速度は例えば １ω９゜、１〜３２４００’　／　ｈである。これに対
して、慣性座標系に対する地球固定座標系の回転角速度
は例えば、１ωｘｔｅｌ　　〜１５°／ｈであるからＷ
　ｔ　　ｅ　＜　Ｗ　Ｏ３ とすることができる。従って、 Ｗｊ　　３　　”；Ｗｏ　　３ 言換えれば、ジャイロの出力はＷＯ３と見做すことがで
きる。従って、（ｂｅ）式は＜１）８）式に置換えるこ
とができる。

ｆ　Ｃ３−０３・Ｗｔ　３　　　　　　　　　　　　（
ｂ８）（ｂ８）式を解くことによりＣ３が得られる。

この（ｂ８）式の解法は、ジャイロ・パッケージ１５か
らのデータＷｔｍをサンプルするごとに逐次Ｃ１を計算
し、これを更新する。なお、Ｃ３の初期値Ｃ３（ｏ）は
、何らかの方法で求めておく。

そして、時刻　ｔ−ｔｋにおけるＣ５（ｔｋ）と、Δを
時間後のｔ”ｔｋ　＋＋　””ｔｋ＋ΔｔにおけるＷｉ
　ｓ　　（ｔｌｃ　４１　＞から、Ｃ３（ｉｓｃ　＋＋
　）を次１は単位行列である。また、Ｎは２次か３次ぐ
らいにとる。

以上のようにして求めたＣ３から、回転角ψ、θ。

φは（Ｃ５）式を用いて次のように得ることができ式の
３行、２列の値を示している。

（Ｃ）　　変換行列Ｃ３の誤差を補正する機能制御装置
２０は操作量Ｕ０演算部２４．操作曲座標変換部２５．
座標変換行列正規化補正演算部２７を用い次に述べるよ
うな演算を行なって変換行列Ｃ３の誤差を補正する。

上記（ｂ）項の演算で求めたＣ３は、制御装置２０（コ
ンピュータ）における切捨誤差、ジャイロ誤差、初Ｉ９
］設定誤差等による誤差を含んでいるので、下記（Ｃ１
）式を満たすことができなくなる。

なお、（Ｃ３）　　はＣ３の転置行列を表わしている。

（Ｃ１）式を満足することができない言うことは、例え
ば、ロボットアームが成る座標軸を中心にして角度α回
転し、次に（−α）回転して戻った時に、元の位置と一
致せず、ずれた位置にきてしまうことを意味する。即ち
、（Ｃ１）式を満たすことができずに、上記（ｂ）項で
説明した演算を行い回転角ψ、θ、φを求めても正確な
ロボットアームの位置を割出すことができず、しかも、
ロボットアームの動きの軽過とともに誤差量は積算・拡
大される。そこで本発明では次のような動作で変換行列
Ｃ２の誤差を補正している。

（δ６）式に制御人力Ｃ３Ｕ３を付加する。

ｆ；−０３＝　０３　　（ＷＯｓ　＋　Ｕ３　）　　　
　　　　（Ｃ２）ただし、Ｕ３は（Ｃ３）式で表わされ
る。

ここで、’Ｘ３　＊　ｕＹ３　＋　’Ｚ３は、回転角誤
差ベクトルの推定値ε０の線形フィードバックより得ら
れる。回転角誤差ベクトルとは、例えば８１算値の回転
角をθｃ１実際のロボットアームの回転角をθｄ１誤差
をεとした場合、θＣ−θｄ＋εが成立ち、このεをベ
クトルとして表わしたものである。なお、推定値εにつ
いては次の（ｄ）項で詳述する。

回転角誤差ε０の座標表現は次式で表わされる。

６０丁　＝　　〔ε　χ　０　　、　　ε　ｙ　　Ｏ＋
　　　ε　２０）　　　　　　　　　　　　　（Ｃ４）
ここで、Ｃｏ７はＣ０の転置行列であり、εＸＯ＋　ε
ｙＯ１εＺＯはリンク０の座標系（χ０　＋　ｙｏ　＋
　ｚｏ　）で表現した回転角誤差である。

次にＣ０の式を求める。これは、（１１６）式を誤差δ
Ｃ３１δＷＯ３に関して摂動展開して、その−次近似よ
り（Ｃ５）式のように得られる。

ｆ　ｔｏ−δＷ＝８−　［Ｗ＝Ａ　＋ＷＡ　］ε０＋（
δＷ；名十δｌＪ）ｅｇ）　　　（Ｃ５）ただし、 Ｗｉコ、δＷｉ３は、慣性座標系に対するリンク３の座
標系（χ３　＊　ｙ３　＊　Ｚ３　）の回転角速度行列
、ベクトルの（Ｘｏ　、　Ｙｏ　、　Ｚｏ　）表現であ
る。

Ｗ　ｔ　ｅ　＊　δＩＪＪｉｅは、慣性座標系に対する
地球固定座標系（Ｚｅ　＊　’／ｅ　＋　Ｚｅ　）の回
転角速度行列、ベクトルの（Ｘｏ　、　Ｙｏ　、　Ｚｏ
　）表現である。

Ｗｅｂ、δ１１７ｅｏは、地球固定座標系に対するリン
クＯの座標系（Ｘｏ　＋　ｙｏ　、　Ｚｏ　）の回転角
速度行列、ベクトルの（Ｘｏ　、　Ｙｏ　、　Ｚｏ　）
表現である。

ここで、リンクＯの座標系（Ｘｏ　＋　Ｖｏ　＋　Ｚｏ
　）は地球に固定しているので、Ｗ６ｏ−０，δｗｅ０
＝０である。また、（δ８）式の導出の所で説明したの
と同じ理由により、Ｗ　（ｅ　＜Ｗ　６コであるから、
Ｗｉ　ｅは無視することができ、またδＷ（ｅ〜０であ
る。従って、（Ｃ５）式は、次のように書くことができ
る。

ｋｔｏ−δｔＪ）（ｓ　　　　　　　　　　　　　（Ｃ
６）ここで、εＸＯ＋　εｙｏ、εｚ０が推定できたと
すると（この推定については次の（ｄ）項で詳述する）
、この推定１ｉｆｔを用いて（ＣＧ）式に対し、次の制
御入力ベクトルを印加する。

ＩＬｌｏ−１番（ε　０　−　　（δ　ＷＬｉ　　　）
　　　Ｓ　　Ｓされ、また、ＩＫはフィードバックゲイ
ン行列である。添字ＳＳは定常状態を示す。これにより
、ｆ、ＥＯ−δ１ＪＪＬｓ　＋　ｔＬｉ　６−δＷｔｓ
　　ＩＫεｏ（δｔＪ）Ｌ３）　ｓ　ｓ　　　（（’７
）となり、閉ループが形成されて補正される。

以上の誤差補正式は、リンク０の座標系（χ０゜Ｚｌｏ
　＊　Ｚｏ　）に関するものなので、Ｃ３を求めるため
の式（Ｃ３）のυ、に変換する必要がある。

そこで、 ’ＬＪｏ”−（ｕｘｏ＋　　ｕｙＯ＋　　ｕｚｏ）　　
　　（Ｃ８）とすると、この行列表現は これからＮ　’ＬＪ　３は次のＧｏによる双似変換で得
られる。

１１１３−　Ｃｏ　ＬＪＯＣ３（ｃｌｏ　）これにより
、ジャイロ誤差や初期設定誤差を補正することができる
が、計算時の切捨誤差は補正することができない。

そごで、更に以下の正規化補正を行なう。

０ｆｆ（＋）　−Ｇｙ。−圭り。（＜　ｃ；、−、）ｒ
（Ｃ；、−、）　−ｘ　）・・・（ａｌｌ） ただし、Ｃ：、、は補正前、ｃ’ｒ＜＋＞　　は補正後
である。

また、■　は単位行列である。

以上のようにして、変換行列Ｃ３の誤差を補正すること
ができる。

（ｄ）　　回転角誤差ベクトルε。２回転角速度ベクト
ルδＵ）ｉ３の推定を行なう機能 制御装置２０は基準回転角検出部２１．誤差推定演算部
２３を用い次に述べるような演算を行なってε。、δＩ
Ｊ）Ｌ３の推定を行なう。

ここでは以下に説明する信号と、カルマンフィルタを用
いた例で推定動作の説明を行なう。本発明においては、
第１図に示す各リンクの回転軸に、例えば、第５図に示
すような基準回転角検出手段を設け、リンクが一定角度
の回転を行なう毎に、電気信号を発生するようにしてお
く。第５図は第１図のリンクＯとリンク１に設けられた
基準回転角検出手段３１部の断面図であり、４０は回転
するリンク１に取付ｔノられた発光素子、５０はリンク
Ｏに取付けられた受光素子である。

このような構成にしておくと、リンクが回転して基準点
く受光素子が設けられた点）を通過時点に電気信号が発
生するので、制御装置２０の基準回転角検出部２１は、
この信号を検知し、予めメモリに書込んでおいた、その
点での角度基準データ（ｖｒ、θｒ、φｒ）と、この時
にジャイロ出力から求めた（ｖ、θ、φ）との差（Ｄψ
、Ｄθ。

Ｄφ）　＝　（ｖ−’４’ｒ　、θ−θ１．φ−φｒ）
を求め、これを観測データとしてカルマンフィルタに入
力する。

しかしカルマンフィルタの回転角の誤差モデル（Ｃ７）
式はリンク０の座標系（ｚｏ　＊　２１０　＊　ｚ、　
）で記述されているので、上記した回転角誤差を総べて
リンク０の座標系に座標変換する必要がある。

この座標変換は、リンクＯの座標系（χＯ＋　’Ｊ　Ｏ
＋Ｚｏ）での誤差を（Ｄ、。、へ２．Ｄ２□。）とする
と、次に示すような演算で（Ｄψ、Ｄθ、Ｄψ）がら（
Ｄ、、塵ブ。、Ｄ２８゜）が導かれる。

Ｄψ、Ｄθ、Ｄφは、１軸、２軸、３軸における回転角
誤差なので各軸の回転単位ベクトルをｅψ、ｅｅ、Ｃφ
とし、また、リンクＯの座標系〈χＯｒ　？ＪＯｒ　ｚ
ｏ　）で回転単位ベクトルをｅｚｏ・ｅｙｏ・ｅｚｏと
すると ｏｌｔｏ　ｅｚｏ　＋　％。匂。＋　ｏ、ｏｅＢ　−Ｄ
ψｅ＊　＋Ｄｅ　ｅｅ　＋Ｄｅ　ｅｐ　　　　（ｄｌ＞
ここで、 ｅψ−ｅｚ　ｏ　＊　ｅｅ　−、ｅｙ　＋　＋　ｅｐ　
−ｅｘ２・・・（ｄ２） であるから、 へ変換し、これを（ｄ３）式に代入して次の（ｄ４）式
が１１られる。

ｅｙ、−−ｓｉｎｔｌ／−ｅｘ、）＋ｃｏｓＶ−ｇｙ。

・・・（ｄ４） また、 へ変換し、これと上記ｃｌとを（ｄ５）式に代入して次
の（ｄ６）式が得られる。

ｅｌ２１Ｉｌ＝ＣＯ８θｃｏｓψ”　ｅｘ。

＋ＣＯＳθＳｉｎ　Ｖ　−ｅｙ　Ｑ−３ｉｌｌθ’ｅｚ
。

・・・（ｄ６） （ｄｌ）、（ｄ２）、（ｄ４）、〈ｄ５）式より、次の
（ｄｌ）式が１ｑられる。

ＤＥＺＯｅｚｏ　＋　０１１゜ｅｌ。十幅。（ｈ。

−［Ｄｐｃｏｓθｃｏｓ　ｖ−［）θｓｉｎ　１４／　
］　ｅｚ。

＋［ＤＬＰｃｏｓθｓｉｎ　Ｖ＋Ｄｅｃｏｓ　Ｖ］　ｅ
ｇ。

［Ｄ　　φ　ｓｉｎ　　θ　−Ｄ　　φ　コ　ｅｚｏ　
　　　　・・・　（ｄｌ　）従って、 Ｄユ。−Ｄ、６ＣＯ８θｃｏｓ　１１／−１）θｓｉｎ
　甲　　　　（ｄ８）Ｄｉｌ。−１）φｃｏｓ　　θｓ
ｉｎ　ｖ＋Ｄｅ　ＣＯ３ｖ（ｄ９）Ｕ）ｕｏ　＝　　Ｄ
　ｐ　ｓｉｎθ＋Ｄψ（ｄｌｏ）この（ｄ８）〜（ｄ　
１０）式を用いて、カルマンフィルタモデルは次式のよ
うになる。

ｋεχ０＝δＷＬ３十に＋（Ｄ２．ｃｏ−εχ。）十に
４（−０−ε１゜） ＋　Ｋ６　（Ｄ、Ｑａｘ。）　＋　ｕχｏ　（ｄ　１１
）十に＋（鴨。−ε１ｏ　＞ ＋に＋　　　（１１４，。−ε３゜　）　　＋ｕ２ｏ　
　　　（ｄ　　１２）５２Ｂ。−６９，３’　＋　ＫＳ
’　　（［）ＥＺＯ’ｇ、、　）＋　Ｋ＋（Ｄ、ｉ。−
ε謬０） ＋　Ｋｒ　（Ｄ、２ｏ−εｚｏ　）　＋　ｕｉｏ　　（
ｄ　１３）Ｊト　δ　ｔｌ）＝ｓ　　−Ｋ＋　　　（０
ｉｘｏ　　−ελＱ　）＋に４（Ｄ中−εプ０） ＋　Ｋ、４−　　（曙。−６２０）　　　（ｄ１４）舟
δ省、％ｏ　−ＫＨｌ　くし。−εχＱ）＋　Ｋ＋（ｏ
、ｉ。−匂０） ＋　Ｋ’；３（ｏ、ｏ−εｚｏ）　　（ｄ１５）丼δ１
ＪＪＬ３−に＋（賑。−ε（０）＋−（Ｄ２．。−εづ
０） ＋に、（Ｄ！□。−εｚｏ）　　　（ｄ１６）以上で、
）く＋　〜１＜＋　　は、それぞれのカルマンフィルタ
ゲインであり、各推定誤差の分散を最小にするように求
まるものである。

このように本発明では、カルマンフィルタで各誤差を推
定しているので、例えば時間とともに推定誤差の推移を
グラフに示せば、第６図のようになる。第６図において
、一点鎖線（ａ）は実際のリンクの回転角度であり、実
線（ｂ）はカルマンフィルタで推定した誤差を示してい
る。時刻ｊ＋＋２ｔｌにて、第５図に示した検出手段か
ら基準点を通過した旨の信号が出力され、これによりコ
ンピュータは予めメモリに書込んでいる角度基準データ
（ｖｒ　＊　θｒ、φｒ）を読み出し、ジャイロ出力を
補正するので時刻ｔＩ、２ｔ＋　、・・・における推定
値は誤差がＯとなる。そして、第６図のように時刻ｔＩ
＋２ｉ＋＋・・・から時間的に離れるにしたがい、また
徐々に推定値は、実際の値とずれてくるが、カルマンフ
ィルタの持つ特性により、第６図の如く、測定回数を増
すごとに（データを取るごとに）その推定値は、確かな
ものに漸近し、次第に実際の値に近いものを推定できる
ようになる。

以上、説明したように本発明によれば、ロボットアーム
の回転角度を正確に知ることができ、その結果、この角
度のデータを用いてロボットアーム先端の位置を正確に
把握することができる。

なＪ３、以上では、ジャイロを用いてアームの回転角速
度を求めるように説明したが、ジャイロに限定するわけ
でなく、回転角ψ、θ、φの決定はジャイロの代りに加
速度計を用いても上の説明と同様に、本発明の動作を行
なわせることができる。

また、フィードバックゲインには、回転角誤差ε０を最
小にするように求められた最適ゲインを用いても良い。

また、以上では、作業用ロボットアームを測定対象とし
たが、その他の姿勢変化型ロボットの姿９Ａ決定に本発
明を用いるようにしても良い。

また、カルマンフィルタを用いないで、直接（ｖ−ｖｒ
　）、（０−ｏｒ）、（φ−φｒ）等の座標変換型をフ
ィードバックしても良い。

また、１軸レ一ト積分ジャイロの代りに２＠レ一ト積分
ジャイロを用いても良い。

また、ジャイロとして従来の機械式ジャイロの代りに光
ジヤイロを用いても良い。

また、１軸レ一ト積分ジャイロを３つともロボットアー
ムの先端に取付けないで、各アームに取付けても良い。

この場合、〈ｂ８）式と（ｂｉｏ）〜（ｂ１２）式によ
る回転角の計算は必要でなく、直接ジャイロ出力を積分
して　Ｖ、θ、φを求め、Ｃ３を計算し、正規化補正を
行なっても良い。

ハ、「本発明の効果」 以上述べたように、本発明によれば、次の効果が得られ
る。

■　従来のエンコーダと同程度の価格のジャイロを用い
て、より高精度な回転角度の測定を行なうことができる
。

■　ジャイロを用いて回転角度決定系を閉ループとして
構成できるので外乱に対して安定かつ高精度な回転角度
の測定ができる。

■　正規化補正演算部により、座標変換の計算で生じる
直交誤差を繰返し補正できるので、高精度な回転角の測
定ができる。

■　０．１’／ｈのジャイロと１０秒ごとの回転角度キ
ャリブレーションにより、約２．７°／　１００００の
回転角測定精度が得られ、従来より２桁も精度が向上す
る。

■　更にジャイロ誤差０．１°／ｈをキャリブレーショ
ンすることにより、約２．７°／１０００００の回転角
度の測定精度が１９られる。

【図面の簡単な説明】

第１図は本発明に係るロボットアーム姿勢測定装置のブ
ロック図、第２図は本発明に係る装置のフローチャート
、第３図と第４図は従来装置の構成図、第５図は基準回
転角検出手段の構成例を示した図、第６図は推定動作を
説明するための図である。 Ｏ〜３・・・リンク、４〜６・・・モータ、１５・・・
回転角速度検出手段（ジャイロ・パッケージ）、２０・
・・制御装置、３１．３２．３３・・・基準回転角検出
手段。 第３図 第　４　図 第５図 時間

Claims (1)

1. 【特許請求の範囲】 回転する３つのリンクを持つロボットアームと、各リン
   クが一定角度回転するごとに信号を出力する基準回転角
   検出手段（３１、３２、３３）と、各リンクの回転角速
   度を検出する手段（１５）と、この手段（１５）と前記
   基準回転角検出手段とから信号を導入し、以下（Ａ）に
   記す演算機能を持つ制御装置と、を備えたロボットアー
   ム姿勢測定装置。 （Ａ）制御装置の演算機能 ロボットアーム先端のリンク（３）の座標系をロボット
   アームを台座に固定しているリンク（０）の座標系に座
   標変換するための変換行列（Ｃ＾０＿３）を演算する機
   能と、 この変換行列（Ｃ＾０＿３）に含まれる制御装置の切捨
   て誤差と、前記手段（１５）の誤差と、初期設定誤差と
   を補正する演算機能と、 前記基準回転角検出手段からの信号を導入し、回転角誤
   差ベクトル（ε＿０）と回転角速度ベクトル（δＷ＿ｉ
   ＿３）の推定を行なう機能と、回転する各リンクの回転
   角（Ψ、θ、φ）を算出する機能。