JPH10267603A

JPH10267603A - 位置姿勢検出装置

Info

Publication number: JPH10267603A
Application number: JP31188196A
Authority: JP
Inventors: Eizou Nishimura; 営三西村; Sadami Tsutsumi; 定美堤; Toshiyuki Nanbu; 敏之南部
Original assignee: LIFE TEC KENKYUSHO KK
Current assignee: LIFE TEC KENKYUSHO KK
Priority date: 1996-11-22
Filing date: 1996-11-22
Publication date: 1998-10-09

Abstract

(57)【要約】（修正有）【課題】６自由度パラレルメカニズムを適用して、移動
体１の位置および姿勢を精度良く、なおかつ短い時間で
検出できる位置姿勢検出装置を提供する。【解決手段】演算手段２を特定の３つの検出点で形成さ
れる第１三角形の実形状およびこれら特定の３つの検出
点への実距離をそれぞれ測定する特定の３つの距離測定
手段から得られる３つの実距離に基づいてこれら特定の
３つの検出点の原点に対する位置を第１仮定位置に設定
する位置仮定手段３と、第１仮定位置に基づいて計算さ
れる仮想データと既知の実データとの比較をしつつ、こ
の誤差を縮小し、第１仮定位置を実際の位置に近づける
べく更新していく更新手段４と、この誤差が定めた値以
下になった時点で、移動体１の位置および姿勢を演算し
確定する位置姿勢確定手段５とから構成して、６自由度
パラレルメカニズム構造を用いて移動体１の位置および
姿勢を検出を可能とした。

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】本発明は、空間における移動
体の位置および姿勢を検出する位置姿勢検出装置に関す
る。

【０００２】

【従来の技術】移動体が、空間においてその位置および
姿勢を自在に変えて移動できるように、移動体にその一
端を取着され、その他端を基準点に支持された伸縮可能
なリンクを並列に６本備えた機構、すなわち６自由度パ
ラレルメカニズムについて、近年研究や応用分野の模索
が活発に行われている。なお、前述した位置とは、空間
の原点に設けられたｘ、ｙ、ｚ軸それぞれの座標で表わ
すことができるもので、姿勢とは前記ｘ、ｙ、ｚ軸回り
のそれぞれの回転角度で表わすことができるものであ
る。また、６自由度とは、位置に関して前記ｘ、ｙ、ｚ
軸方向への直線移動の３自由度と、姿勢に関してのｘ、
ｙ、ｚ軸回りの回転移動の３自由度とを総称したもので
ある。

【０００３】このような６自由度パラレルメカニズム
は、従来、移動体を目標に動かすアクチュエータとして
用いられている。すなわち、移動体の移動目標位置およ
び姿勢を設定し、この設定値から一意的に決まる各リン
クの長さを算出し、その長さとなるように各リンクを伸
縮させることにより、移動体を設定位置および姿勢に移
動させる。この６自由度パラレルメカニズムの長所とし
ては、リンクを直列に連結したシリアルメカニズムに比
べ、各リンクの長さの誤差が蓄積されないため、移動物
体を精度良く目標に移動させることができる点や、各リ
ンクに作用する負荷が分散でき、重量物を駆動できる等
が挙げられる。さらには、このように移動体の目標位置
および姿勢から各リンクの長さを求めるといういわゆる
逆運動学は、解析的にも数値計算法等を用いても容易に
解を得ることができるため、算出に時間がかからず、移
動体を速やかに目標に移動させられる点も挙げられる。

【０００４】

【発明が解決しようとする課題】ところが、上述したよ
うに、６自由度パラレルメカニズムを、外力で操作され
る移動物体の位置および姿勢の検出装置として使用した
場合には、これをアクチュエータとして使用した場合に
比較して、次のような問題点が生じ、実用化は未だなさ
れていない。

【０００５】すなわち、６自由度パラレルメカニズムを
位置姿勢検出装置として使用した場合、順運動学的に各
リンクの長さから移動物体の位置および姿勢を計算する
のは、解析的に不可能で、数値計算法によっても非常に
難しく、コンピュータ等を用いても多大な時間を必要と
したため、実用には適さないという問題点があった。

【０００６】

【課題を解決するための手段】上記の問題点を解決する
ために、本発明は、移動体の位置を仮定し、この仮定位
置から逆運動学の手法を用いて計算した移動体の仮想デ
ータを、実データと比較し、これに近づけるべく前記仮
定位置を更新していくという負帰還ループを用いた手法
によって、６自由度パラレルメカニズム構造を用いて移
動体の位置および姿勢を検出できるようにした全く新し
いものである。

【０００７】

【発明の実施の形態】すなわち、本発明による位置姿勢
検出装置は、原点に対して移動可能な移動体の空間にお
ける位置および姿勢を検出する位置姿勢検出装置であっ
て、原点に対して位置を固定して３箇所以上に設けた基
準点と、移動体の３箇所以上に固設した検出点と、前記
各基準点と各検出点との実距離をそれぞれ測定する６つ
の距離測定手段８と、前記実距離から前記移動体の位置
および姿勢を演算する演算手段２とから構成され、図２
に示すように、各距離測定手段８から得られる実距離に
基づいて演算手段２により移動体の位置および姿勢を検
出するものである。そして、この演算手段２が、図３に
示すように特定の３つの検出点で形成される第１三角形
の実形状およびこれら特定の３つの検出点への実距離を
それぞれ測定する特定の３つの距離測定手段８から得ら
れる３つの実距離に基づいて、これら特定の３つの検出
点の原点に対する位置を第１仮定位置に設定する位置仮
定手段３と、前記第１仮定位置に基づいて計算される仮
想データと、既知または測定された実データとの比較を
しつつ、この誤差を縮小し、前記第１仮定位置を実際の
位置に近づけるべく更新していく更新手段４と、この誤
差が定めた値以下になった時点で、前記移動体の位置お
よび姿勢を演算し確定する位置姿勢確定手段５とを備え
てなるようにしたものである。

【０００８】このような構成によれば、６自由度パラレ
ルメカニズムの長所を生かして、移動体の位置および姿
勢を精度良く検出できる。具体的に移動体の位置姿勢検
出を数学的に簡単な問題に帰着させ、短い時間で行うに
は、更新手段４が、図４に示すように前記第１仮定位置
から計算される各検出点の仮想位置に基づいて、前記仮
想データたる各検出点への仮想距離を計算する距離計算
手段６と、前記仮想距離とこれらに対応する実データた
る実距離とを比較しこの比較の結果導き出された誤差を
縮小する方向に前記第１仮定位置を補正する補正手段７
とを備えてなるものが好適である。

【０００９】また、他の実施の態様としては、更新手段
４が、図５に示すように他の３つの距離測定手段８に対
応する３つの検出点で形成される第２三角形の実形状お
よびこれら他の３つ距離測定手段８から測定される実距
離とに基づいてこれら３つの検出点のそれぞれの位置を
第２仮定位置に設定する第２位置仮定手段３０と、前記
第１仮定位置および第２仮定位置に基づいて計算される
仮想データたる各検出点の仮想相対位置関係と、実デー
タたる実際の各検出点の実相対位置関係とを比較し、こ
の比較の結果導き出された誤差を縮小する方向に前記第
１仮定位置および第２仮定位置を補正する第２補正手段
７０とを備えてなるものも考えられる。

【００１０】

【実施例】以下本発明の一実施例を図１、および図６〜
図１１を参照して説明する。本発明による位置姿勢検出
装置は、図１に示すように、空間内を移動可能な移動体
１の原点ｏに対する位置および姿勢を検出するもので、
この移動体１をプラットホーム１１と呼ばれるプレート
に固設し、このプラットホーム１１に６つの検出点Ａ，
Ｂ，Ｃ，Ｄ，Ｅ，Ｆを配置するとともに、空間内に固定
されたベース１２と呼ばれるプレートに６つの基準点
ａ，ｂ，ｃ，ｄ，ｅ，ｆを固設したものである。そし
て、検出点Ａと基準点ａとをリンクＬ１，検出点Ｂと基
準点ｂとをリンクＬ２、検出点Ｃと基準点ｃとをリンク
Ｌ３、検出点Ｄと基準点ｄとをリンクＬ４、検出点Ｅと
基準点ｅとをリンクＬ５、検出点Ｆと基準点ｆとをリン
クＬ６によってそれぞれ連結するようにしている。

【００１１】これら６本のリンクＬ１〜Ｌ６は、シリン
ダ機構（図示しない）を採用して伸縮可能とし、両端の
連結部分にボールジョイント等の自在継手を使用して各
リンクＬ１〜Ｌ６が連結部分を中心に自在に回動できる
ようにして、移動体１の位置および姿勢の自由度に制限
を与えないようにしたものである。また、各リンクＬ１
〜Ｌ６の長さｒ１〜ｒ６を内部にそれぞれ設けられたリ
ニアポテンショ８（図示しない）により検出できるよう
にして、これらリニアポテンショ８に、それらが連結す
る各基準点Ａ〜Ｆと各検出点ａ〜ｆとの間のそれぞれの
実距離ｒ１〜ｒ６を測定する距離測定手段としての機能
を担わせるようにしたものである。

【００１２】そして、図６に示すように、実距離たる各
リンクＬ１〜Ｌ６の長さｒ１〜ｒ６を電気信号として演
算手段たるマイクロコンピュータ装置２（以下マイコン
２という）に伝達し、マイコン２によって移動体の位置
および姿勢を演算し、出力するようにしている。マイコ
ン２は、ＣＰＵ、メモリ、入出力インタフェース、Ａ／
Ｄコンバータおよびクロック発生器を含むそれ自体すで
に一般的に知られているものであり、本実施例において
は、マイコン２は図７にブロック図を示すように次の機
能を有する手段を備えている。

【００１３】特定の３つの検出点Ａ，Ｂ，Ｃで形成さ
れる第１三角形たる△ＡＢＣの実形状およびこれら３つ
の検出点Ａ、Ｂ、Ｃにそれぞれ連結された各リンクＬ
１、Ｌ２、Ｌ３の長さｒ１、ｒ２、ｒ３に基づいて、こ
れら３つの検出点Ａ、Ｂ、Ｃの原点に対する位置を第１
仮定位置に設定する位置仮定手段３。第１仮定位置から計算される各検出点Ａ〜Ｆの仮想位
置に基づいて、前記仮想データたる各検出点Ａ〜Ｆへの
仮想距離ｓ１〜ｓ６を計算する距離計算手段６。

【００１４】仮想距離ｓ１〜ｓ６とこれらに対応する
実データたる実距離ｒ１〜ｒ６とを比較し、この比較の
結果導き出された誤差を縮小する方向に前記第１仮定位
置を補正する補正手段７。この誤差が定めた値以下になった時点で、前記移動体
１の位置および姿勢を演算し確定する位置姿勢確定手段
５。

【００１５】なお、、、が更新手段４に相当す
る。上述した構成での、本実施例による位置姿勢検出装
置の作用を順に説明する。（０）初期設定まず、電源が投入されると、移動体１の位置姿勢確定精
度を決める限界誤差、および、第１仮定位置の１回あた
りの移動更新量を決めるＧＡＩＮの初期設定を行い、そ
の後各リンクＬ１〜Ｌ６の長さｒ１〜ｒ６をＡ／Ｄコン
バータを介してメモリ内に入力し保存する。

【００１６】（１）位置仮定手段３による第１仮定位置
の設定次に、検出点Ａ、Ｂ、Ｃによって形成される△ＡＢＣ、
および検出点Ａ、Ｂ、Ｃに対応するリンクＬ１、Ｌ２、
Ｌ３の長さが実値ｒ１〜ｒ３と等しくなるように検出点
Ａ、Ｂ、Ｃの位置を第１仮定位置に設定する。具体的に
は、検出点Ａ、Ｂ、Ｃのうち例えば点Ａを選び、この
ｘ、ｙ座標を適当に与え、リンクＬ１の長さｒ１からｚ
座標を算出することによって、点Ａの位置を仮定する。
次に、例えば点Ｂを選び、このｘ座標を適当に与え、リ
ンクＬ２の長さｒ２および点Ａと点Ｂとの実相対距離Ａ
Ｂに基づいて点Ｂのｙ、ｚ座標を算出することによっ
て、点Ｂの位置を仮定する。最後に残った点Ｃのｘ、
ｙ、ｚ座標を、リンクＬ３の長さｒ３および点Ａ、Ｂそ
れぞれと点Ｃとの実相対距離ＡＣ、ＢＣに基づいて算出
し、点Ｃの位置を仮定する。なお、この点Ｃの位置算出
方法をサブル−チンＭａｉｎｔｒｉということにする。
このようにして、検出点Ａ、Ｂ、Ｃを頂点とする△ＡＢ
Ｃが、位置は正しくないにせよ、その形状と各リンク長
を実際のものと等しくして、第１仮定位置に仮定された
ことになる。

【００１７】（２）距離計算手段６による仮想距離ｓ
４、ｓ５、ｓ６の計算さらに、前記第１仮定位置に基づいて残りの検出点Ｄ、
Ｅ、Ｆの位置を計算し、これら計算した検出点Ｄ、Ｅ、
Ｆの位置から対応するリンクＬ４、Ｌ５、Ｌ６の仮想長
さ（仮想距離）ｓ４、ｓ５、ｓ６を計算する。なお、第
１仮定位置から導き出される仮想距離ｓ１、ｓ２、ｓ３
は、（１）で述べたように実距離ｒ１、ｒ２、ｒ３と等
しく設定しているので計算は行わない。

【００１８】（３）補正手段による第１仮定位置の補正そして、実距離ｒ４〜ｒ６と仮想距離ｓ４〜ｓ６とをそ
れぞれ比較し、その誤差を評価する式、すなわち誤差評
価式の値を小さくする向きに第１仮定位置を補正すべく
移動させる。この誤差評価式は（（（ｒ４−ｓ４）²＋（ｒ５−ｓ５）²＋（ｒ６−ｓ６）²）^1/2）／３・・・（イ）と設定する。なお、第１仮定位置から導き出される仮想
距離ｓ１、ｓ２、ｓ３は、（１）で述べたように実距離
ｒ１、ｒ２、ｒ３と等しく設定しているので比較は行わ
ない。また、この時の第１仮定位置の移動は、△ＡＢ
Ｃ、および検出点Ａ、Ｂ、Ｃに対応するリンクＬ１、Ｌ
２、Ｌ３の長さが常に実値ｒ１、ｒ２、ｒ３と等しくな
るようにする。

【００１９】具体的な第１仮定位置の移動手法を述べる
と、まず、検出点Ａ、Ｂ、Ｃを次のいずれかに決める。（点Ａ、点Ｂ、点Ｃ）＝（移動点、固定点、追帆点）（点Ａ、点Ｂ、点Ｃ）＝（追帆点、移動点、固定点）（点Ａ、点Ｂ、点Ｃ）＝（固定点、追帆点、移動点）最初にいちばん上の組合わせを例にして説明する。

【００２０】（点Ａ、点Ｂ、点Ｃ）＝（移動点、固定点、追帆点）各点に名前がつけられているように、検出点Ｂを固定し
て検出点Ａを移動させる。この移動方向は、△ＡＢＣ、
および検出点Ａ、Ｂ、Ｃに対応するリンクＬ１、Ｌ２、
Ｌ３の長さが常に実値ｒ１、ｒ２、ｒ３と等しくなるよ
うにしなければならないため、図１０に示すように、△
ＢＡａを、点ａと点Ｂとを結ぶ直線ａＢを中心に回転移
動させることになる。また、このときの回転角ｔの大き
さは、｜ｒ４−ｓ４｜、｜ｒ５−ｓ５｜、｜ｒ６−ｓ６
｜の中で最も大きい値とＧＡＩＮとの積としている。回
転には正逆２通りあり、それぞれについて新しい検出点
Ａの位置を算出した後、残った検出点Ｃをサブルーチン
Ｍａｉｎｔｒｉによって計算し、検出点Ａ、Ｂ、Ｃの新
しい仮定位置をそれぞれ得る。なお、このときの新しい
検出点Ａの位置を算出方法をサブルーチンＲｏｔａｔｉ
ｏｎと呼ぶ。この後同様に、残った２つの組合わせ、す
なわち検出点Ｂ、Ｃをそれぞれ移動点にして前述の作業
を行えば、△ＡＢＣの新しい仮定位置が合計６通り得ら
れることになる。最後に、このように得られた６通りの
仮定位置についてそれぞれ誤差評価式（イ）の値を求
め、最もこの値が小さくなるものを△ＡＢＣの正式な移
動先、すなわち、新たな第１仮定位置とする。

【００２１】この新たな第１仮定位置をもとに同様の作
業を繰り返し行い、第１仮定位置を次々更新していく。
そして、最終的に誤差評価式（イ）の値が予め定めた限
界誤差よりも小さくなった時点での検出点Ａ〜Ｆの位置
を確定値とし、繰り返し作業を終了する。（４）位置姿勢確定手段による移動体１の位置および姿
勢の演算および確定このように確定した検出点Ａ〜Ｆの
位置より、移動体１の位置および姿勢を計算し、確定し
て出力する。

【００２２】以上（０）〜（４）に本実施例による姿勢
位置検出装置の作用を述べたが、以下には演算手段たる
マイコン２の動作を、図８、図９に示すフローチャート
を参照してさらに具体的に詳述する。なお、図８、図９
に示すフローチャートの説明中Ｎ１、Ｎ２、・・・は処
理手順（ステップ）の番号を示す。まずＮ１、Ｎ２にお
いては限界誤差とＧＡＩＮの初期設定を行ない、各リン
クＬ１〜Ｌ６の長さｒ１〜ｒ６を、各リニアポテンショ
からの電気信号によってＡ／Ｄコンバータを介してメモ
リ内に保存する。

【００２３】次に、Ｎ３では、適当に与えた検出点Ａの
ｘ、ｙ座標およびリンクＬ１の長さｒ１に基づいて検出
点Ａのｚ座標を算出し、検出点Ａの位置を仮定する。Ｎ
４では、適当に与えた検出点Ｂのｘ座標、リンクＬ２の
長さｒ２および検出点Ａと検出点Ｂとの実相対距離ＡＢ
に基づいて検出点Ｂのｙ、ｚ座標を算出し、検出点Ｂの
位置を仮定する。

【００２４】Ｎ５では、リンクＬ３の長さｒ３および検
出点Ａ、Ｂそれぞれと検出点Ｃとの実相対距離ＡＣ、Ｂ
Ｃに基づいて検出点Ｃのｘ、ｙ、ｚ座標を算出し、検出
点Ｃの位置を仮定する。なお、このステップをサブルー
チンＭａｉｎｔｒｉとする。次に、Ｎ６においては、残
りの検出点Ｄ、Ｅ、Ｆの位置を前記第１仮定位置に基づ
いて算出する。

【００２５】続くＮ７においては、算出した検出点Ｄ、
Ｅ、Ｆのデータに基づいて、仮想距離ｓ４〜ｓ６を算出
し、誤差評価式（イ）に代入してこれを計算し、ＧＯＳ
Ａという変数に代入する。すなわち、ＧＯＳＡ＝（（（ｒ４−ｓ４）²＋（ｒ５−ｓ５）²＋
（ｒ６−ｓ６）²）^1/2）／３Ｎ８では、検出点Ｄ、Ｅ、Ｆへの実距離と仮想距離との
差、すなわち｜ｒ４−ｓ４｜、｜ｒ５−ｓ５｜、｜ｒ６
−ｓ６｜の中から最も大きい値を選出しメモリ内に記憶
する。

【００２６】Ｎ９では、点Ａを移動点、点Ｂを固定点、
点Ｃを追帆点とする。Ｎ１０〜Ｎ１６では、サブルーチ
ンＲｏｔａｔｉｏｎによって、検出点Ａを直線ａＢ中心
に角度ｔだけ正逆両方向に回転移動させ、新たに検出点
Ａの位置を２通りに仮定するとともに、この新たな検出
点Ａの位置と固定された検出点Ｂの位置とを用いて、サ
ブルーチンＭａｉｎｔｒｉにより、検出点Ｃの位置を２
通りに仮定する。このようにして得られた２通りの△Ａ
ＢＣの新たな第１仮定位置に対応して、それぞれ検出点
Ｄ、Ｅ、Ｆの位置をＮ６と同様に算出し、Ｎ７と同様に
誤差評価式（イ）の値を計算して、それぞれＧＯＳＡ
（０）、ＧＯＳＡ（１）とする。

【００２７】Ｎ１７〜Ｎ１９では、検出点Ａを追帆点、
検出点Ｂを移動点、検出点Ｃを固定点とし、Ｎ１０〜Ｎ
１６と同様の作業によって、ＧＯＳＡ（２）、ＧＯＳＡ
（３）を算出する。Ｎ２０〜Ｎ２２では、検出点Ａを固
定点、検出点Ｂを追帆点、検出点Ｃを移動点とし、Ｎ１
０〜Ｎ１６と同様の作業によって、ＧＯＳＡ（４）、Ｇ
ＯＳＡ（５）を算出する。

【００２８】Ｎ２３、Ｎ２４では、ＧＯＳＡ（０）〜Ｇ
ＯＳＡ（５）の内、最も小さいものを選出し、その場合
での検出点Ａ、Ｂ、Ｃの位置を、新たな第１仮定位置と
する。Ｎ２５では、この新たな第１仮定位置での誤差評
価式（イ）の値を、初期設定において定めた限界誤差と
比較し、限界誤差より小さければＮ２６へ進み、大きけ
れば、Ｎ８へ戻って、再び新たに第１仮定位置を更新す
る。

【００２９】Ｎ２６では、第１仮定位置での検出点Ａ、
Ｂ、Ｃを確定位置とし、これらから移動体の位置姿勢デ
ータを出力する。その後、再びＮ２へ戻る。このよう
な、マイコン２による移動体１の位置および姿勢確定ま
で演算時間、すなわちＮ１からＮ２６までに要する時間
を、１２ｂｉｔＡ／Ｄコンバータを有するノート型パー
ソナルコンピュータ（ＣＰＵは４８６ＳＸ（３３ＭＨ
ｚ））を使用した場合の実験データとして図１１に示
す。ＧＡＩＮを増大させれば、第１仮定位置の１回あた
りの移動量が増えるので、この演算時間を短くでき、ま
た、限界誤差値を大きくして、移動体１の位置および姿
勢データの精度を落とせば、同様に演算時間を短くでき
ることを示している。

【００３０】なお、ステップ中のサブルーチンについて
は、その機能内容を以下に簡単に説明する。サブルーチ
ンＭａｉｎｔｒｉは、例えば、前述したようにリンクＬ
３の長さｒ３および検出点Ａ、Ｂそれぞれと検出点Ｃと
の実相対距離ＡＣ、ＢＣに基づいて検出点Ｃのｘ、ｙ、
ｚ座標を算出するものである。これはすなわち、３つの
位置の確定した点（この場合ではＡ，Ｂ、ｃ）からの距
離のみがそれぞれ与えられている点の位置（この場合は
検出点Ｃ）を確定することである。したがって、この確
定方法は、中心および半径が既知である３つの球の交点
を求める問題となり次の（ロ）（ハ）（ニ）の３式の
Ｘ，Ｙ、Ｚ（点Ｃのｘ、ｙ、ｚ座標に相当）を解くとい
う簡単な問題に帰着し、これは、フェラーリの公式とカ
ルダーノの公式を用いて解けることが証明されている。

【００３１】Ｒ１²＝（Ｘ１−Ｘ）²＋（Ｙ１−Ｙ）²＋（Ｚ１−Ｚ）²・・・（ロ）Ｒ２²＝（Ｘ２−Ｘ）²＋（Ｙ２−Ｙ）²＋（Ｚ２−Ｚ）²・・・（ハ）Ｒ²＝Ｘ²＋Ｙ²＋Ｚ²・・・（ニ）なお、この場合の、上式中、Ｘ１、Ｙ１、Ｚ１は、点Ａ
のｘ、ｙ、ｚ座標に相当し、Ｘ２、Ｙ２、Ｚ２は、点Ｂ
のｘ、ｙ、ｚ座標に相当する。また、Ｒ１、Ｒ２はそれ
ぞれ実相対距離ＡＣ、ＢＣに相当する。

【００３２】サブルーチンＲｏｔａｔｉｏｎは図１０に
概略を示すように、例えば基準点ａ、ｂ、ｃからそれぞ
れ距離ｒ１、ｒ２、ｒ３離間した検出点Ａ，Ｂ、Ｃの位
置を△ＡＢＣの形状および距離ｒ１、ｒ２、ｒ３を保持
しつつ移動させるものであり、その際に検出点Ａ，Ｂ、
Ｃのいずれか１点を固定点とし、他のいずれか１点を移
動点としてその移動点の移動後のｘ、ｙ、ｚ座標を算出
するものである。具体的に点Ｂを固定点、点Ａを移動点
に選んだ場合を考える。この場合、△ＢＡａの形状は確
定しており、点ａおよび点Ｂは固定であるため、点Ａの
移動は点ａと点Ｂとを結ぶ直線ａＢを中心の回転移動に
限定されることになり、その回転角度をｔとして、点Ａ
の新たな移動位置を求める問題に帰着する。

【００３３】上述した構成の本実施例によれば次のよう
な利点がある。すなわち、移動体１の位置および姿勢の
検出を、６自由度パラレルメカニズムの長所を生かして
精度良く測定できるのはもちろんのこと、距離測定手段
を、６本の伸縮可能なリンクＬ１〜Ｌ６と、それらに付
随させたリニアポテンショ８によって構成したため、構
造が簡単で安価なものとなる。

【００３４】また、特に第１仮定位置の更新を（移動
点、固定点、追帆点）なる組合わせで行うことにより、
サブルーチンＭａｉｎｔｒｉ、あるいはサブルーチンＲ
ｏｔａｔｉｏｎといった簡単な関数に帰着させて行うこ
とが可能となったため、マイコン２の動作中、どのステ
ップにおいても複雑で時間のかかる計算が含まれないよ
うになり、位置姿勢の確定までの時間を図１１に示した
ように非常に短くできる。したがって、移動体１が頻繁
に動くような用途、例えば３次元デジタイザ等にも十分
適用できるし、本実施例を６自由度パラレルロボットな
どのアクチュエータに付属させ、逆運動のアルゴリズム
に、この順運動のアルゴリズムを取り入れ、アクチュエ
ータとしての作動特性をより安全で確実なものにするこ
ともできるようになる。

【００３５】なお、本発明は以上示した実施例のみに限
定されるものではない。特に、演算手段の機能に関して
は種々の変形例が考えられる。例えば、図１２にブロッ
ク図を示すように次の機能を有する手段を備えるように
してもよい。特定の３つの検出点Ａ，Ｂ，Ｃで形成される△ＡＢＣ
の実形状およびこれら３つの検出点Ａ、Ｂ、Ｃにそれぞ
れ連結された各リンクＬ１〜Ｌ３の長さｒ１〜ｒ３に基
づいて、これら３つの検出点Ａ、Ｂ、Ｃの原点に対する
位置を第１仮定位置に設定する位置仮定手段３。

【００３６】他の３つの検出点Ｄ，Ｅ，Ｆで形成され
る△ＤＥＦの実形状およびこれら３つの検出点Ｄ、Ｅ、
Ｆにそれぞれ連結された各リンクＬ４〜Ｌ６の長さｒ４
〜ｒ６に基づいて、これら３つの検出点Ｄ、Ｅ、Ｆの原
点に対する位置を第２仮定位置に設定する第２位置仮定
手段３０。前記第１仮定位置および第２仮定位置に基づいて計算
される仮想データたる各検出点Ａ〜Ｆの仮想相対位置関
係と、実データたる実際の各検出点Ａ〜Ｆの実相対位置
関係とを比較し、この比較の結果導き出された誤差を縮
小する方向に前記第１仮定位置および第２仮定位置を補
正する第２補正手段７０。

【００３７】この誤差が定めた値以下になった時点
で、移動体１の位置および姿勢を演算し確定する位置姿
勢確定手段５。演算手段は、このような他にも、第１仮定位置を負帰還
ループによって更新していく手法を用いたものであれば
よいのであって、現代制御やファジィ制御など種々の方
法の適用が考えられる。また、更新量やその方向などの
決定方法は、実施例に限定されるものではない。

【００３８】さらに、基準点、および検出点の個数はそ
れぞれ最低３点が直線にならないように設けられていれ
ばよい。距離測定手段も、本実施例においては、伸縮可
能なシリンダ機構を有するリンクにリニアポテンショを
設けてなるものにしていたが、レーザー距離測定器や、
超音波を利用した距離測定器などによってもよいのはも
ちろんである。

【００３９】その他、各部の構成は図示例に限定される
ものではなく、本発明の趣旨を逸脱しない範囲で種々変
形が可能である。

【００４０】

【発明の効果】本発明は、以上に説明したような形態で
実施され以下に記載されるような効果を奏する。すなわ
ち、本発明によれば、検出点の位置を仮定し、この仮定
位置から逆運動学の手法を用いて計算した移動体の仮想
データを、実データと比較し、これに近づけるべく更新
手段によって前記仮定位置を更新していくという負帰還
ループを用いた手法によって、６自由度パラレルメカニ
ズム構造の利点を生かし、移動体の位置および姿勢の検
出を精度良く測定できる。

【００４１】特に、更新手段が、前記第１仮定位置から
計算される移動体の仮想位置姿勢に基づいて、前記仮想
データたる各基準点と各検出点の仮想距離を計算する距
離計算手段と、前記仮想距離とこれらに対応する実デー
タたる実距離とを比較し、この比較の結果導き出された
誤差を縮小する方向に前記第１仮定位置を補正する補正
手段とを備えてなるものであれば、移動体の位置姿勢検
出を簡単な数学の問題に帰着させ複雑な計算等無しに短
い時間で行うことができ、移動体が頻繁に動くような用
途、例えば３次元デジタイザ等にも十分適用できるし、
本実施例を６自由度パラレルロボットなどのアクチュエ
ータに付属させ、逆運動のアルゴリズムに、この順運動
のアルゴリズムを取り入れ、アクチュエータとしての作
動特性をより安全で確実なものにすることもできるよう
になる。

【図面の簡単な説明】

【図１】本発明の一実施例を示す位置姿勢検出装置の全
体図。

【図２】請求項１に対応する図面。

【図３】請求項１に対応する図面。

【図４】請求項２に対応する図面。

【図５】請求項３に対応する図面。

【図６】同実施例の機能全体図。

【図７】同実施例のマイコンの機能ブロック図。

【図８】同実施例のマイコンの動作を示すフローチャー
ト。

【図９】同実施例のマイコンの動作を示すフローチャー
ト。

【図１０】同実施例のサブルーチンＲｏｔａｔｉｏｎの
概略機能の説明図。

【図１１】マイコンの動作時間の実験データ。

【図１２】他の実施例を示す演算手段の機能ブロック
図。

【符号の説明】

１・・・移動体２・・・演算手段（マイコン）３・・位置仮定手段４・・・更新手段５・・・位置姿勢確定手段６・・・距離計算手段７・・・補正手段８・・・距離測定手段（リニアポテンショ）３０・・・第２位置仮定手段７０・・・第２補正手段ｏ・・・原点Ａ〜Ｆ・・・基準点ａ〜ｆ・・・検出点ｒ１〜ｒ６・・・実距離ｓ１〜ｓ６・・・仮想距離

───────────────────────────────────────────────────── フロントページの続き (72)発明者南部敏之豊中市新千里東町１丁目４−２千里ライフサイエンスセンタービル12階1220号室株式会社ライフテック研究所内

Claims

【特許請求の範囲】

【請求項１】原点に対して移動可能な移動体の空間にお
ける位置および姿勢を検出する位置姿勢検出装置であっ
て、原点に対して位置を固定して３箇所以上に設けた基
準点と、移動体の３箇所以上に固設した検出点と、前記
各基準点と各検出点との実距離をそれぞれ測定する６つ
の距離測定手段と、前記実距離から前記移動体の位置お
よび姿勢を演算する演算手段とを備えてなり、前記演算手段が、特定の３つの検出点で形成される第１三角形の実形状お
よびこれら特定の３つの検出点への実距離をそれぞれ測
定する特定の３つの距離測定手段から得られる３つの実
距離に基づいて、これら特定の３つの検出点の原点に対
する位置を第１仮定位置に設定する位置仮定手段と、前記第１仮定位置に基づいて計算される仮想データと、
既知または測定された実データとの比較をしつつ、この
誤差を縮小し、前記第１仮定位置を実際の位置に近づけ
るべく更新していく更新手段と、この誤差が定めた値以下になった時点で、前記移動体の
位置および姿勢を演算し確定する位置姿勢確定手段とを
備えてなるものであることを特徴とする位置姿勢検出装
置。
【請求項２】前記更新手段が、前記第１仮定位置から計算される各検出点の仮想位置に
基づいて、前記仮想データたる各検出点への仮想距離を
それぞれ計算する距離計算手段と、前記仮想距離とこれらに対応する実データたる実距離と
を比較し、この比較の結果導き出された誤差を縮小する
方向に前記第１仮定位置を補正する補正手段とを備えて
なるものである請求項１記載の位置姿勢検出装置。
【請求項３】前記更新手段が、他の３つの距離測定手段に対応する３つの検出点で形成
される第２三角形の実形状およびこれら他の３つ距離測
定手段から測定される実距離とに基づいて、これら３つ
の検出点のそれぞれの位置を第２仮定位置に設定する第
２位置仮定手段と、前記第１仮定位置および第２仮定位置に基づいて計算さ
れる仮想データたる各検出点の仮想相対位置関係と、実
データたる実際の各検出点の実相対位置関係とを比較す
る相対位置比較手段と、この比較の結果導き出された誤差を縮小する方向に前記
第１仮定位置および第２仮定位置を補正する第２補正手
段とを備えてなるものである請求項１記載の位置姿勢検
出装置。