JP2009159787A - 回転駆動される球体の姿勢を決定する装置及び方法 - Google Patents

Abstract

【課題】広い駆動範囲で圧電素子により駆動される球体の姿勢を高精度に決定する方法を提供するにある。
【解決手段】駆動される球体の姿勢を決定する方法においては、球体１００の表面上の観測点に対向されてセンサが設置され、球体１００の表面の２次元平行移動量に相当するセンサ信号からセンサ観測面座標系におけるベクトル量が求められ、このベクトル量が基準座標系に変換され、外積演算により球体中心Ｏを通るベクトルが求められて回転軸ベクトル１８１が演算で特定される。球体１００の半径ｒ及び球体中心Ｏと回転面との距離ｈで構成される直角三角形の長さ比から回転軸ベクトル１８１に直交し、センサ観測点１８２を含む回転面１８２の半径ｒ２が演算される。また、回転面半径ｒ２と回転面円弧と等価であるセンサ観測面座標系におけるベクトル長から回転角Φが演算され、球体１００の姿勢が決定される。
【選択図】 図１

Description

この発明は、回転駆動される球体の姿勢を決定する装置及び方法に関する。

従来、球体が圧電素子で回転駆動され、回転された球体の姿勢を決定、即ち、球体の回転する位置を検知する方法が球面アクチュエータの分野で知られている。例えば、特許文献１には、球面アクチュエータの回転位置を検出する検出装置が開示されている。この検出装置では、被駆動球体１００に棒状のポールが立設され、そのポールが外部の位置検出板に通され、位置検出板の移動量が検出されて球体の姿勢が決定される。

特許文献２には、球体の表面に経度緯度線と同様のグリッドを２箇所の９０度離れた場所に設置してあるイメージセンサによって観測し、観測された情報から球体の姿勢を検出する方法が開示されている。

また、特許文献３には、球体の表面上の電極とそれに対向して設けた４個の電極間の静電容量を検出することにより、球体の姿勢を検出する方法が開示されている。

特許文献４には、球体に埋設した光源からの光ビームを位置検出素子により検出することにより、球体の姿勢を検出する方法が開示されている。

非特許文献１に開示された球面アクチュエータにおいては、被駆動球体が磁化され、その磁力線が非接触に離れた位置に設置されたホールセンサで検出され、この検出信号に基づいて球体の姿勢が決定される。非特許文献１には、この方法が実験により検証されている旨が記載されている。更に、非特許文献２に開示された球面アクチュエータにおいては、特許文献４と同様に、被駆動球体の下部に発光体が設置され、底面側に設置したＰＳＤセンサに発光体からの光線が照射され、光線の照射位置から球体の姿勢が非接触で決定される。
特開平７−８７７６６ ＵＳＰ ５３１９５７７ Ｊｕｎ．７， １９９４ 特開平１０−１２２８０９ 特開平１０−１４２５１７ 日本機械学会関東支部第７期総会講演会講演論文集２００１／０３／１６−０３／１７ １５３−１５４頁 精密工学会誌Ｖｏｌ．６１，Ｎｏ．３，１９９５ ３８６−３９０頁

特許文献１に開示された方法では、棒状のポールの検出範囲が移動検出板の移動範囲のみに限られ、球体の観測範囲が限定される。しかも、ポールを移動検出板に挿通させる穴は、滑らかな球体駆動を実現するためにポールとの間に隙間が必要とされ、高い観測精度での球体姿勢の位置検出が困難である問題がある。

特許文献２に開示された方法では、球体の表面に経度緯度線と同様のグリッドを有する必要がある場合に限られ、また、そのグリッドを観測するイメージセンサは球体の中心を通って直行する平面上での移動量をもとに演算するために９０度離れて設置する必要があるといった設置上の問題がある。

特許文献３に開示された方法では、球体表面電極の移動範囲が４個の電極の設置部分に限定されるため、移動可能量が制限される問題がある。

特許文献４に開示された方法では、球体に埋設した光源からの光ビームを位置検出素子にあてる必要があるため、位置検出素子の面積内に移動可能量が制限される問題がある。

また、非特許文献１に開示された方法では、単純に一方向に磁化された球体の磁力線が読み取られることから、観測精度が非常に低いという問題がある。

更に、非特許文献２に開示された方法では、ＰＳＤセンサの検出範囲が非常に小さいため、観測範囲が狭い問題があり、また、球体に発光体を埋め込む加工が必要とされ、また、発行体に通電の為の配線が必要とされる問題がある。

以上のように、従来の圧電素子で回転駆動される球体の姿勢を決定する方法或いは装置においては、観測する範囲が狭く、また、観測範囲が広い場合にあっても、観測精度が低い問題がある。

この発明は、上記問題点を解決するためになされたものであり、その目的は、広い駆動範囲に亘って球体の姿勢を高精度に決定することができる装置及び方法を提供することにある。

この発明によれば、
回転駆動される球体と、
前記球体を支える基台と、
球体表面上の第１及び第２の観測点を含む第１及び第２の観測面に非接触に対向して設置され、前記第１及び第２の観測面の２次元平行移動量を座標量として検知して少なくとも第１及び第２のセンサ信号を夫々発生する少なくとも第１及び第２のセンサと、
を具備する球体の姿勢を決定する装置において、
前記球体が姿勢を変更した際に、前記第１及び第２のセンサ信号から前記第１及び第２の観測面の平行移動量をセンサ観測面座標系における第１及び第２の移動ベクトル量として演算し、
この第１、第２の移動ベクトル量を基準となる慣性座標系に変換して第１及び第２の変換ベクトルを求め、
前記第１及び第２の変換ベクトルの外積演算により求めるベクトルから、前記球体の中心を通るベクトルを特定して前記球体の姿勢変更の基準となる慣性座標系における回転軸ベクトルを演算し、
この回転軸ベクトル及び前記球体表面上の観測点の座標から回転面を特定し、この回転面から前記球体の中心までの回転面距離を求め、
この回転面距離及び前記球の半径から前記回転面の回転面半径を求め、
前記第１及び第２のベクトル量のいずれかのベクトル量及び前記回転面半径から回転角を演算することを特徴とする球体の姿勢を決定する方法が提供される。

また、この発明によれば、
回転駆動される球体と、
前記球体を支える基台と、
球体表面上の第１及び第２の観測点を含む第１及び第２の観測面に非接触に対向して設置され、前記第１及び第２の観測面の２次元平行移動量を座標量として検知して少なくとも第１及び第２のセンサ信号を夫々発生する少なくとも第１及び第２のセンサと、
前記球体が姿勢を変更した際に、前記第１及び第２のセンサ信号から前記第１及び第２の観測面の平行移動量をセンサ観測面座標系における第１及び第２の移動ベクトル量として演算し、この第１、第２の移動ベクトル量を基準座標系に変換して第１及び第２の変換ベクトルを求め、前記第１及び第２の変換ベクトルの外積演算により求めるベクトルから、前記球体の中心を通るベクトルを特定して前記球体の姿勢変更の基準となる回転軸ベクトルを演算し、この回転軸ベクトル及び前記球体表面上の観測点の座標から回転面を特定し、この回転面から前記球体の中心までの回転面距離を求め、この回転面距離及び前記球の半径から前記回転面の回転面半径を求め、前記第１及び第２の移動ベクトル量のいずれかのベクトル量及び前記回転面半径から回転角を演算する演算部と、
を具備する球体の姿勢を決定する装置が提供される。

以上説明したように、この発明によれば、広い駆動範囲に亘って回転駆動される球体の姿勢を高精度で決定することができる装置及び方法を提供することができる。

以下、必要に応じて図面を参照しながら、この発明の一実施の形態に係る回転駆動される球体の姿勢を決定する装置及び方法を説明する。

（第１の実施の形態）
図１から図１０は、この発明の第１の実施の形態に係る回転駆動される球体の姿勢を決定する装置及び方法を示している。

図１に示されるように回転駆動される球体１００の姿勢を決定する装置においては、球体１００が球体１００の中心を原点Ｏとした３次元慣性座標系で回転可能に基台１０５上に載置されている。球体１００は、基台１０５上に載置されている限り、その中心は、基台１０５に対して相対的に不動に維持されて球体１００が回転可能とされる。基台１０５には、少なくとも３組の圧電素子(図示せず）が互いに略交差する関係に配置されたアクチュエータ機構５０が設けられている。アクチュエータ機構５０に載置された球体１００は、球体１００がアクチュエータ機構５０の圧電素子に摩擦接触され、この圧電素子からの駆動力によってその中心の回りに回転されてその姿勢が変更される。より詳細には、このアクチュエータ機構５０では、２３組の圧電素子での駆動力を調整することで球体１００の姿勢を３次元内で任意の姿勢に変更することができる。

ここで、球体１００を回転駆動するアクチュエータ機構は、球体１００をその中心の回りに回転駆動する機構であれば、圧電素子に限らず、他の駆動素子或いは駆動機構で構成されても良い。

図１に示すように、中心の回りに回転駆動される球体１００の姿勢を非接触に観測(モニタ）する為に少なくとも1対のセンサ１０１，１０２が球体１００外に配置され、1対のセンサ１０１，１０２が球体１００の表面に向けられ、センサ１０１，１０２によって球体１００の表面が観察され、球体１００の移動に応じてセンサ１０１，１０２からはセンサ信号Ｓ１、Ｓ２が発生される。

ここで、1対のセンサ１０１，１０２は、球体１００の表面のいずれの表面位置を観察しても良いが、センサ１０１，１０２は、互いに平行に、且つ、互いに相対する関係（球体１００を介して互いに対向される関係）に配置されることは好ましくない。センサ１０１，１０２が互いに平行に、且つ、相対して同一平面上に配置される場合には、センサ１０１，１０２は、同一平面上における互いに平行な２次元的な被検出対象の移動量のみを検出することとなり、被検出対象の３次元内の移動量を検出することができない。また、センサ１０１，１０２が球体１００の姿勢を検出する場合には、センサ１０１，１０２は、夫々球体１００の表面上の観測点を通り、球面に接する平面に対して平行となるようにこの平面に非接触で対向されて設置される。センサ１０１，１０２がこのように配置される場合には、球体１００の表面と相対して球体１００を外部から見ると、球体１００を観察する際にセンサ１０１，１０２のいずれかが視野中に配置され、視野の一部を遮ることになる。同様に、球体１００から球体外が観察される場合にも、配置されたセンサの一方がその視野の一部を遮ることになる。球体１００に撮影レンズ並びに撮像装置が組み込まれ、球体１００が眼球と同様にみなされて姿勢が制御される機構或いは装置に組み込まれる場合には、その視野が制限され、視野を広げることができないこととなる。

図１に示すように回転駆動される球体１００の姿勢を決定する装置においては、圧電素子が設けられる基台１０５に平行であって、球体１００の中心を通る基準面(略水平な基準面）より基台１０５側に、即ち、装置の基準面の下方の下部空間にセンサ１０１，１０２が配置されることが好ましい。この配置により、球体１００を眼球と同様に姿勢が制御される機構では、実質的に眼球とみなされる球体１００の視野を妨げず、球体１００の稼動範囲を±９０度以上に設定することができ、広範囲な球体１００の姿勢変更を実現することができる。

図１に示される球体１００姿勢を決定する装置では、センサ１０１，１０２からセンサ信号Ｓ１，Ｓ２が出力され、このセンサ信号Ｓ１，Ｓ２が姿勢座標検出部１０に入力される。姿勢座標検出部１０においては、後に詳細に説明するようにセンサ信号Ｓ１，Ｓ２から球体１００の移動量及び移動方向に応じて移動量ベクトルが算出され、この移動量ベクトルが慣性座標系に変換されて球体１００の姿勢が求められる。この求められた球体の姿勢が姿勢制御部２０のメモリ(図示せず）に格納される。装置外から目標姿勢を指定する指定信号が姿勢制御部２０に入力されると、姿勢制御部２０では、球体１００の現姿勢と指定された目標姿勢とを比較して圧電アクチュエータ駆動部３０が駆動信号を発生し、この駆動信号に従って圧電アクチュエータが動作される。球体１００は、圧電アクチュエータ５０で駆動され、球体１００が指定された姿勢に変更される。

上述した装置において、センサ１０１，１０２は、２組以上設けることが観測精度を上げるためには好ましいが、以下の第１の実施の形態では、説明を単純化する目的で1対のセンサ１０１，１０２のみが設けられる場合について説明する。

図１に示すように観測センサ１０１、１０２の夫々は、観測センサ１０１、１０２の夫々に平行に非接触で相対する平面(実質的に平面とみなされる観測面）の２次元的な平行移動量を座標量として検知している。この観測センサ１０１、１０２は、夫々、例えば、非接触に移動量を検知する光学マウスに用いられるセンサ等に相当し、図２（ａ）に示されるようにセンサ台１２０上に配置され、図２（ｂ）に示されるように当該観測面に向けて光線を照射する光源１２１及び観測面１０３，１０４からの反射光線を検出するセンサデバイス１２２から構成される。また、観測センサ１０１、１０２は、夫々、図２（ｃ）に示されるように外光線が向けられた観測面からの反射光線を検出するセンサデバイス１２２のみから構成することもできる。

球体１００は、球体表面を有し、この球体１００の表面は、実質的に平面とみなされる観察面が連続している面とみなすことができ、観測センサ１０１、１０２が画像の変化を検出することができないような光学的な鏡面には形成されず、観測センサ１０１、１０２のセンサ分解能に相当する大きさに等価な表面パターン(模様）或いは凹凸を有している。即ち、観測センサ１０１、１０２が観測面１０３，１０４の表面パターン(模様）或いは凹凸の変化を画像として認識することができる分解能を備えることによって画像の変化によって観測面の２次元的移動を検出でき、画像の時間的変化に従って２次元的平行移動量を検出することができる。

図２（ａ）及び（ｂ）に示される観測センサ１０１、１０２では、光源１２１としてコヒーレントな光ビーム或いは光強度変調された或いは波長変調された変調光ビームを観測観察面（観測領域）に照射することができる半導体レーザ或いはダイオードで構成することができる。図２（ａ）及び（ｂ）に示される観測センサ１０１、１０２の光学系では、光源１２１、観測点１２３及びセンサデバイス１２２は、光源１２１とセンサデバイス１２２との間のセンサ台１２０上の中点を通る垂直線が観測面上の観測点を通るように配置されている。また、光源１２１から観測点に向けられる照射光線に含まれる主光線が観測点を通る垂直線と成す入射角が観測点からセンサデバイス１２２に向けられる反射光線が観測点を通る垂直線と成す反射角に等しく、主光線及び光源１２１とセンサデバイス１２２と間の基線が２等辺３角形を成すように光源１２１、センサデバイス１２２及び観測点が配置される。

図２（ｃ）に示すように、装置の設置下の環境で、外部からの光線光量のみでセンサデバイス１２２が観測面を観測(モニタ）可能な場合には、センサデバイス１２２がセンサ台１２０にのみ搭載されるが、この図２（ｃ）に示す光学系では、センサデバイス１２２の面の垂直線が観測面上の観測点を通り、観測点からセンサデバイス１２２に向けられる反射光線がこの垂直線に沿って導かれる。

図３に示すように、観測センサ１０１、１０２は、既に説明したように、球体１００に対して、センサ１０１，１０２が平面移動量として検知できる狭い範囲の観測対象領域(観測面）１０３，１０４について、平面上の２次元平行移動量を観測できる。球体１００がある基準姿勢（ホームポジション）からある他の姿勢へ姿勢を変更するとき、基準姿勢における観測センサ１０１，１０２の座標値をゼロとするならば、姿勢変更後の２組の観測センサ１０１，１０２は、夫々姿勢変更に伴う平行移動量及び方向に関する情報を含むセンサ信号Ｓ１、Ｓ２を発生する。ここで、センサ信号Ｓ１、Ｓ２は、観測面の表面パターン或いは凹凸等の画像信号として出力される。この画像信号が姿勢座標検出部１０で検出されて画像の周期的に変化が解析されて移動量及び移動方向が求められる。即ち、姿勢座標検出部１０において、センサ信号Ｓ１、Ｓ２からセンサ観測平面座標系１３５，１３６における移動量及び移動方向を含む観測ベクトル１３７，１３８が算出される。ここで、平行移動量は、所謂、球体１００の経度緯度を表してはいない。

図４から図６を参照して姿勢座標検出部１０における観測ベクトル１３７，１３８を算出する解析処理の概念を説明する。

図４には、球体１００を横切る概念上の大円１４３及び小円１４４が示されている。ここで、球１４０が定まり、その球面上に任意の点１４１が与えられたとき、点１４１を通って球の中心１４２を通る平面で切られた断面の円を大円１４３と称している。また、点１４１を通る任意の平面で球を切ることができ、任意の平面で球１００の断面が描く一つの円１４４を小円と称する。大円１４３は、唯一存在し、小円１４４は、無限に存在する。

上述した姿勢決定装置における検出原理の理解をより高める為に、比較例として一般的に知られている接触型トラックボールでの移動検出について図５を参照して説明する。ここで、図５は、一般的に知られている接触型トラックボールの構造を上部から平面的に図示している。

接触型トラックボールでは、球体１００の縦回転量及び横回転量によって相対的な２次元的平行移動距離を検出している。接触型トラックボールには、縦回転量及び横回転量を夫々検出する２つのコロ１５１，１５２が配置され、球体１００の動きがコロ１５１，１５２を通じてコロ１５１，１５２の軸の回転移動量を検出するエンコーダ１５３，１５４に伝達され、エンコーダ１５３，１５４におけるカウントが平行移動距離に換算される。２つのコロ１５１，１５２は、直径上に配置されるため、必ず１つの大円に接触されている。

このようにトラックボールでは、大円での２つのコロ１５１，１５２の移動量を
観測して経度緯度情報を求めることができることを前提としているが、図１に示す観測装置では、図６を参照して説明するように、測定原理を異にし、平行移動量から経度緯度情報を求めることができない。

図６は、図１に示した装置を横方向から図示した側面図を示し、軸１６３は、球体１００の中心(大円１４３の中心）を通り、大円１４３に垂直に立てた回転軸を示し、軸１６３，１６５は、任意の小円１４４，１４５の中心を通り、小円１４４，１４５に垂直に立てた回転軸を示している。図６に示される軸１６３の回りで球体１００の姿勢が変更された場合、センサ１０１（１０２）は、観測面１０４（１０３）或いは観測点の座標が真横方向に移動する平行移動量を検知することができる。しかし、同一の平行移動量で真横方向に観測点座標の移動が生じても、この移動量だけで軸１６３の回りで移動が生じたと断定することはできない。即ち、軸１６４或いは軸１６５等のいずれの軸周りの回転でも同一の平行移動量での真横方向に観測点座標が移動が生ずる可能性があるからである。換言すれば、この平行移動は、軸１６３の回りでの移動に限らず、軸１６４或いは軸１６５等のいずれかの軸周りの回転であっても同様に観察面１０４（１０３）或いは観測点座標の真横の平行移動として検知される。従って、センサ１０１（１０２）で検出される平行移動量からそのまま経度緯度情報を求めることができず、センサ１０１（１０２）は、このような小円１４３（１４５）の回転をも観測することを前提に処理する必要がある。

ここで、図３に示すような夫々のセンサに正対して、球に接する微小な平面である観測面１０３，１０４について考察する。この平面は、既に説明したように、夫々のセンサ１０１，１０２に対向される観測面１０３，１０４に相当している。２次元平行移動量を検知するセンサ１０１（１０２）は、平面上の２次元の動きを観測するセンサであるから、図３に示すように観測面１０３，１０４、即ち、観測点座標の動きが動き座標系１３５，１３６での動きベクトル１３７，１３８として捉えられる。よって、観測結果として得られる座標値は、移動後の観測面１０３，１０４の座標を表し、ある姿勢変化の前後において座標変化量で表される観測ベクトル１３７，１３８をセンサ１０１，１０２に対応する座標系１３５，１３６上に描くことができる。

被駆動球体１００が様々な姿勢を描く場合には、この観測ベクトル１３７，１３８が座標系１３５，１３６上の原点Ｏを中心に座標系１３５，１３６上を動き回ることを意味している。姿勢変化量が大きい場合には、観測ベクトル１３７、１３８の振幅が大きくなり、両観測ベクトル１３７、１３８の振幅がゼロのときは、球体１００が全く動いていないことを意味し、また、観測ベクトル１３７、１３８の一方の振幅がゼロのときは、ゼロの振幅を有する一方の観測ベクトル１３７、１３８に対応する一方の観測面１０３，１０４の観測点を軸として球体１００が回っていることを意味している。

一般に、球体１００の姿勢変化は、ある回転軸とその周りに回る回転角度によって表現できる。回転軸が定まると、センサ１０１，１０２の観測点を通る回転軸に垂直な回転面を定めることができ、センサ１０１，１０２が観測する軌跡は、この回転面の円弧の軌跡になる。座標系１３５，１３６で得られるベクトルは、観測面１０３，１０４、即ち、観測点上に必ず存在するベクトルであると同時に、この回転面の円弧の接線でもある。そして、その接線は円弧の接線であることから、必ず回転軸に直交することとなる。

図７には、上述した観測点、回転軸、回転面及び観測ベクトルの関係が示されている。観測面１０３，１０４内の２つの観測点１７０，１７１には、夫々観測方向１７２，１７３に観測点１７０，１７１を通り、回転軸１７８に垂直な回転面１７４，１７５を定義することができる。また、センサ観測点１７０，１７１での回転面の円弧の接線が観測ベクトル１７６，１７７の方向に相当する。センサ１０１，１０２が２組以上あれば、観測ベクトル１７６，１７７が２組以上獲得される。この観測ベクトル１７６，１７７は、回転面の１７４，１７５の接線に相当することから、観測ベクトル１７６，１７７に垂直なベクトルは、回転軸１７８に平行なベクトルであり、このベクトル中で球体１００の中心を通るベクトルは、回転軸１７８に一致される。ここで、空間に存在する２組以上のベクトル１７６，１７７に垂直なベクトルは、いずれかの２組のベクトル１７６，１７７の外積演算によって容易に求めることができ、外積演算されたベクトルであって、球の中心を通るベクトルも容易に特定することができ、回転軸１７８を特定することができる。従って、センサ観測面において、両センサ１０１，１０２に固有の側面座標系１３５，１３６で得られる観測ベクトルを夫々１７６，１７７とすると、そのベクトル１７６，１７７を基準となる慣性座標系（３次元慣性座標系）に座標変換し、その外積を計算することで回転軸１７８を定めることができる。この演算過程で、２組以上の観測ベクトル１７６，１７７の大きさがゼロである場合、球体１００は、姿勢変更されずに静止しているとみなすことができる。また、１組の観測ベクトル１７６，１７７の一方の大きさがゼロである場合には、大きさゼロを有する観測ベクトル１７６，１７７で特定される一方の観測点１７０，１７１と球１００の中心を結ぶ線を回転軸１７８として定めることができる。この演算によって回転軸１７８が定まると、次に、その軸１７８の回転角度が求められて球体１００の姿勢変更位置が決定される。

センサ１０１、１０２が検出する観測ベクトル１７６，１７７の振幅は、姿勢変化量の大きさを示すと同時に、回転面円弧の軌跡の長さそのものでもある。よって、回転面の半径を決めることができれば、その円弧の長さを回転面半径で除算ことによって回転角度を知ることが出来る。

図８は、回転面の半径ｒ２から回転面円弧の軌跡の長さを求めることができる関係を示している。図８に示されるように、空間に存在する半径ｒの球１００と回転軸１８１が与えられたとき、センサ観測点１８２を通る平面で球１００を切断した観測面１８３が描く回転面の半径ｒ２は、初等幾何学に基づいて下記式（１）から式（３）を用いて求めることができる。

観測点の座標（ｘ１，ｙ１，ｚ１）を通り、法線ベクトル（ｘａ，ｙａ，ｚａ）を有する平面は、下記式（１）で書き表され、原点Ｏから平面までの距離ｈは、下記式（２）で表される。

従って、求めたい回転半径ｒ２は、下記式（３）で表される。得られた回転半径ｒ２より、図９に示すように観測半径ｒ２の円弧１９１の長さ１９２は、観測ベクトルの長さから求めることができる。回転角度Φは、観測ベクトル１７６，１７７のいずれかの平行移動量の長さｄが２次元平行移動座標量ｄｘ，ｄｙで表されるとき、下記式（４）に基づいて求めることができる。

ここで、回転角度Φの符号を決める必要がある。センサ観測点１０３，１０４における観測ベクトル１７６，１７７（１３７，１３８）を慣性空間で表し、回転軸ベクトル１８１との外積を算出する。その外積演算の結果がセンサ側を向くか、あるいは球の内側を向くかによって符号を判定することができる。具体的には、観測ベクトル１７６，１７７と回転軸１８１の外積で算出されるベクトルを法線とし、観測点１０３，１０４を通る平面を定義し、観測点を始点としてその法線方向に存在する座標値がその平面の表側にあるか裏側にあるかを判定すれば良い。

図１０は、上述した手順を基にした図１に示した姿勢座標検出部１０における処理をフローチャートとして示している。

図１０に示す処理においては、ステップＳ０１に示されるように姿勢決定処理が開始されると、ステップＳ０２に示されるように始めに２組以上のセンサ１０１，１０４によって観測面１０３，１０４が観察され、センサ信号Ｓ１、Ｓ２が姿勢座標検出部１０で検出され、センサ信号Ｓ１、Ｓ２から観測面１０３，１０４上における球体１００の移動量及び方向が得られ、移動量ベクトル１３７，１３８としてセンサ座標系１３５、１３６に展開される。

ステップＳ０３において、センサ座標系１３５、１３６における測定された移動量ベクトル１３７，１３８の全てがセンサ１０１、１０４の分解能よりも小さい場合には、球体１００は、回転されていないとして、再びステップＳ０１に戻される。センサ座標系１３５、１３６における測定された移動量ベクトル１３７，１３８の全てがセンサ１０１、１０４の分解能よりも小さくない場合には、ステップＳ０３に移行される。ステップＳ０３において、測定された移動量ベクトル１３７，１３８の１つがセンサ１０１、１０４の分解能よりも小さい場合には、ステップＳ０４において、その１つのセンサ１０１、１０４に対応する観測面１０３，１０４内の観測点と球体１００の中心軸とを結ぶ軸が回転軸に定められる。また、ステップＳ０３において、測定された移動量ベクトル１３７，１３８のいずれもがセンサ１０１、１０４の分解能よりも大きい場合には、ステップＳ０５において、測定された移動量ベクトル１３７，１３８が慣性座標系(３次元座標系）に変換される。ステップＳ０６において、慣性座標系に変換されたベクトルの外積演算によって回転軸１７８（１８１）が特定される。ステップＳ０４，Ｓ０６において、回転軸１７８，（１８１）が定まると、ステップＳ０７において、この回転軸１７８（１８１）とセンサ１０１、１０４で観測される観測点の座標によって回転面１８３が決定される。ステップＳ０８において、この回転面１８３の式（１）で示される平面座標と球体１００との関係から式（２）及び式（３）を利用して回転面１８３の半径ｒ２が求められる。この求められた回転面の半径ｒ２から回転角度Φが式（４）を利用して求められる
この一連の処理により回転軸１７８（１８１）とそのまわりの回転角度Φが得られるので、この回転軸１７８（１８１）及び回転角度Φから３次元空間内における物体の姿勢を表す方向余弦行列が得られることになる。もちろん、物体の姿勢表現手法は方向余弦行列に限定する必要はなく、目的に応じてオイラー角、クォータニオンへの変換を行ってもよい。これは例えば以下のような演算となる。基準となる慣性座標系の基底ベクトルをＣｘ，Ｃｙ，Ｃｚとしたとき、球体に固定された球体座標系は姿勢変更前に慣性座標系と一致しており、姿勢変更後には球体座標系を表す基底ベクトルｂｘ、ｂｙ、ｂｚはＣｘ，Ｃｙ，Ｃｚと式（５）のような関係で書き表される。

これをベクトルと行列形式で書き表すと式（６）のように表すことができ、

これは例えば３−２−１オイラー角で姿勢変更を表すとすると様々な公知文献で開示されているように、式（７）のように表すことができる。ここでＸ軸まわりφ、Ｙ軸まわりθ、Ｚ軸周りψとした。

この３ｘ３行列を方向余弦行列と呼び、この方向余弦行列はクォータニオンを用いても、ある回転軸とその周りの回転角を用いても書き表すことが可能である。本発明では、回転軸１７８，（１８１）とそのまわりの回転角度Φが分かるので、回転軸を単位ベクトルλ１、λ２、λ３、回転角度φとしたとき、方向余弦行列は式（８）

で書き表される。

（第２の実施の形態）
次に、この発明の第２実施の形態に係る回転駆動される球体１００の姿勢を決定する装置及び方法について図１１を参照して説明する。

観測精度を上げる為に２組以上のセンサ１０１，１０２が設けられた装置に関して以下に説明する。

第１の実施の形態で説明したように二次元平行移動量を観測するセンサ１０１，１０２を用いることで、そのセンサ１０１，１０２の分解能に観測精度が依存することになる。センサ１０１，１０２そのものの移動量検知分解能は明確に定義できるが、球表面の移動距離を計測するための演算では、センサ分解能は姿勢計測分解能と等価ではなく、幾つかの精度悪化要因がある。例えば、非常に小さな観測ベクトルが観測された場合には、それらに直行する回転軸１８１を求める場合に大きな誤差をもたらす場合がある。即ち、姿勢変更で生ずる回転軸ベクトルが求められる際の外積演算に大きな誤差をもたらす場合がある。空間に存在する２組以上のベクトル１７６，１７７に直交するベクトル１７８を求めるとき、そのベクトル１７８は、このベクトル１７８が存在する平面の法線ベクトルに相当し、外積演算によって求めることが出来る。しかし、２組以上の観測ベクトル１７６，１７７の大きさは、センサ１０１，１０２の分解能に依存し、例えば、センサ１０１，１０２の画素分解能をΔｃとしたとき、２組以上の観測ベクトル１７６，１７７のいずれか２組をｉ，ｊの添え字で表したとき、これらに直行するベクトルは式（９）で表される。

式９は、下記式（１０）のように書き直すことができる。

この式（１０）の第二項の係数である式（１１）のベクトルＵのノルムが小さいほど画素分解能による精度悪化を防ぐことができる。即ち、２個以上のセンサが存在する場合は式（１１）で示すベクトルＵのノルムが最小になるような２つの観測ベクトルを選択することによって精度改善を図ることができる。

また、回転軸１７８を求める計算手続きの際に回転軸１７８は、式（１２）における演算で導出することから、２組以上の観測ベクトルが得られた場合に、夫々の観測ベクトルに対して式（３）で計算される半径ｒ２が最も大きい観測点を選択することがセンサ分解能による誤差を受けにくいことは明らかである。

図１１は、上述した手順に沿った姿勢座標検出部１０における演算処理を示すフローチャートである。

図１１に示す処理においては、ステップＳ１０に示されるように姿勢決定処理が開始されると、ステップＳ１１に示されるように始めに２組以上のセンサ１０１，１０４によって観測面１０３，１０４が観察され、センサ信号Ｓ１、Ｓ２が姿勢座標検出部１０で検出され、２組以上のセンサ信号Ｓ１、Ｓ２から観測面１０３，１０４上における球体１００の移動量及び方向が得られ、移動量ベクトル１３７，１３８としてセンサ座標系１３５、１３６に展開される。ステップＳ１２に示すように、式（７）で示すベクトルＵのノルムが最小になるような２つの観測ベクトル１３７，１３８が選択される。この選択された２つの観測ベクトル１３７，１３８が利用されてステップＳ０３〜Ｓ０８の処理が実施される。ここで、図１０に示したと同一の処理は、同一の符号を付してその処理の説明を省略する。従って、ステップＳ０３〜Ｓ０８の処理に関しては、図１０に関するステップの説明を参照されたい。

ステップＳ０８において、この回転面１８３の式（１）で示される平面座標と球体１００との関係から式（２）及び式（３）を利用して回転面１８３の半径ｒ２が求められると、ステップＳ１３に示すように半径ｒ２が最も大きいベクトルが選択される。ステップＳ０９に示すようにこの最も大きいベクトル及び回転面１８３の半径ｒ２から回転角度Φが式（４）を利用して求められる。

以上のように、２組以上のセンサを用いる場合には、より観測精度を上げることが可能となる。

（第３の実施の形態）
以下、本発明の第３の実施の形態における球体１００の姿勢決定装置及び方法について図１２を参照して説明する。

第１の実施の形態で説明したように、二次元平行移動量を観測する複数センサで球体１００が姿勢変化を決定する場合、球体１００の姿勢変化の前後で図１２（ａ）にセンサ１０１，１０２による第１及び第２観測として示されるに回転軸１８１及び回転軸回りの回転角度Φが得られる。圧電モータで球体１００を駆動する場合には、特定の観測周期に従って球体１００の姿勢変化量が計測され、姿勢制御部２０において、その姿勢変化量の値と目標姿勢信号で指定される座標とに基づいて圧電アクチュエータ３０によって圧電モータが駆動制御される。

一般に制御では、サンプリング時間と称される特定の時間周期が定められ、その周期ごとに観測量が獲得される。第１の実施例では、姿勢変化の前後において、球体の姿勢が観測される観測方法であったが、特に、姿勢変動のイベントが終了した後に、球体姿勢が観測される必然性はなく、球体姿勢が変動されている間においても球体姿勢が観測(モニタ）されても良い。

図１２（ｂ）にタイミングチャートで示すように、サンプリング期間Ｔs毎に姿勢変化量が検知され、そのサンプリング期間Ｔs毎の回転軸１８１とその回転軸回りの回転角度Φが得られても良い。そのときは、回転軸１８１とその回転軸回りの回転角度Φから算出されるサンプリング期間Ｔs毎の方向余弦行列を算出し、サンプリング期間Ｔs毎にその方向余弦行列を自乗して球体姿勢が観測される。

（第４の実施の形態）
以下、本発明の第４の実施の形態における球体１００の姿勢決定装置及び方法について図１３を参照して説明する。

第１の実施の形態で説明したように、二次元平行移動量を観測する複数センサ１０１，１０２からのセンサ信号Ｓ１，Ｓ２で、球体１００が姿勢変化した場合の回転軸１８１とその回転軸回りの回転角度Φが得られる。第１の実施の形態から第３の実施の形態では、これら二次元平行移動量を観測するセンサ１０１，１０２は、正しい移動量情報を出すことを前提としている。しかし、これらのセンサ１０１，１０２は、移動量を観測する手段として画像情報を基にすることが多いため、フレームレート、或いは、画像焦点距離合わせの不合致や画像特徴量の影響によって必ずしも正しい移動量情報を生成しない場合がある。図１３に示すような装置においては、例えば、画像情報を画像センサによって取得するタイプのセンサ１０１，１０２が使用される場合にセンサ１０１，１０２の仕様と分解能とから移動量を検出するために、必要な画像特徴量の大きさが規定できるので、図１３に示される観測面には、図１３（ｃ）に断面として示すようにその特徴量に相当する大きさの模様或いは凹凸を有する必要がある。例えば、圧電モータとして圧電素子で球体１００を直接的に摩擦駆動する場合には、球体１００を駆動する圧電素子の伸縮動作における最大振幅値と最小振幅値の差よりも凹凸の大きさを小さくする必要がある。もし、凹凸の大きさがセンサ移動量検出条件に満たない場合には、センサ１０１，１０２は、正しい移動量情報を生成できなくなる。

第２の実施の形態において説明したように球体１００の姿勢を決定するためには最低２組の二次元移動量検出センサ１０１，１０２が必要であり、数が増えた場合には、観測誤差量が小さくなるようなセンサの取捨選択を行うことができる。これに加えて、センサが３組以上存在する場合にはセンサ１０１，１０２が正しい移動量情報を生成しているかどうかをチェックすることで、信頼性の高い移動量を検出することができる。

センサ１０１，１０２が正しい移動量情報を生成しているかどうかについて以下で説明する。一般に、冗長量のセンサ出力がある場合のセンサ出力の取捨選択手法については確率的手法などが提案されているが、本実施例における姿勢情報算出においては以下に示す方法が採用される。

第１の実施の形態で説明したように２次元移動量センサ１０１，１０２が出力するセンサ信号Ｓ１、Ｓ２の値はセンサ観測座標空間における観測平面上の観測ベクトルとして利用される。２組以上のセンサ１０１，１０２がある場合には、この観測ベクトルの数が増大することとなる。観測ベクトルのどの２つの組を用いてもそれらに垂直な軸１８１を得ることが可能であるが、３組以上の場合、あるセンサ出力が正しい移動量を検出していない場合には、全てに対して垂直な軸１８１を決定することができなくなる。しかし、第３の実施の形態に示したようにサンプリング期間Ｔs毎に姿勢情報を更新する手法を用いた場合には、サンプリング期間Ｔs毎に観測ベクトルが取得できる。球体１００の姿勢変更スピードに比較してサンプリング時間が十分小さい場合には、前サンプリング期間の観測ベクトルとの大きさの違いは小さくなる。よって移動量を正しく検出できていないセンサ１０１，１０２の観測ベクトルは、前サンプリング期間において取得する観測ベクトル値と大きな違いが出ることなる。よって、観測ベクトルのサンプリング期間当たりの変化量が球体１００の姿勢変更スピードから算出される量よりも異なる場合には、その観測ベクトルは正しい情報ではないと判断してそのセンサからの情報を破棄し、この破棄処理によって信頼性の高い移動量の検出を行うことができる。

以上のように、この発明の回転駆動される球体の姿勢を決定する装置及び方法によれば、広い駆動範囲に亘って球体の姿勢を高精度に決定することができる。

この発明の１実施の形態に係る球体の姿勢を検知するためのセンサを備える姿勢を決定する装置を概略的に示すブロック図である。 （ａ）〜（ｃ）は、図１に示されるセンサ構成例を示す概略図である。 図１に示す装置における球体の姿勢変化に伴いセンサ観測面で生じる平行移動量ベクトルを説明する為の概略図である。 図１及び図３に示される球体の姿勢変化に伴う回転軌跡としての１つの大円及び複数の小円がある旨を説明する為の概略図である。 比較例としてトラックボールの観測原理を説明する為のトラックボール機構を概略的に示す平面図である。 図１及び図３に示される装置における球体の姿勢変化に伴い観測される平行移動量と観測軸との関係を示す概略図である。 図１及び図３に示される装置における球体の回転軸を決定する過程を説明する為の概略図である。 図７に示される関係から決定された回転軸に基づいて回転面を決定し、及びその回転半径を求める過程を説明する為の概略図である。 図８に示された回転角度の決定 図１に示される姿勢座標検出部における第１の実施の形態に係る演算処理を示すフローチャートである。 図１に示される姿勢座標検出部における第２の実施の形態に係る演算処理を示すフローチャートである。 （ａ）及び（ｂ）は、夫々第１及び第３の実施の形態に係る観測方法を実施する為のタイミングチャートである。 （ａ）、（ｂ）及び（ｃ）は、図１に示された装置における装置を示す概略図、観測面を拡大して示す平面図及び観測面の表面の凹凸を示す断面図である。

符号の説明

１０．．．姿勢座標検出部、２０．．．姿勢制御部、３０．．．圧電アクチュエータ駆動部、５０．．．アクチュエータ機構、１００．．．球体、１０１，１０２．．．センサ、球体、１０５．．．基台、１２０．．．センサ台、１２１．．．光源、１２２．．．センサデバイス、１２３．．．観測点

Claims (14)

1. 回転駆動される球体と、
   前記球体を支える基台と、
   球体表面上の第１及び第２の観測点を含む第１及び第２の観測面に非接触に対向して設置され、前記第１及び第２の観測面の２次元平行移動量を座標量として検知して少なくとも第１及び第２のセンサ信号を夫々発生する少なくとも第１及び第２のセンサと、
   を具備する球体の姿勢を決定する装置において、
   前記球体が姿勢を変更した際に、前記第１及び第２のセンサ信号から前記第１及び第２の観測面の平行移動量をセンサ観測面座標系における第１及び第２の移動ベクトル量として演算し、
   この第１、第２の移動ベクトル量を基準となる慣性座標系に変換して第１及び第２の変換ベクトルを求め、
   前記第１及び第２の変換ベクトルの外積演算により求めるベクトルから、前記球体の中心を通るベクトルを特定して前記球体の姿勢変更の基準となる慣性座標系における回転軸ベクトルを演算し、
   この回転軸ベクトル及び前記球体表面上の観測点の座標から回転面を特定し、この回転面から前記球体の中心までの回転面距離を求め、
   この回転面距離及び前記球の半径から前記回転面の回転面半径を求め、
   前記第１及び第２の移動ベクトル量のいずれかのベクトル量及び前記回転面半径から回転角を演算することを特徴とする球体の姿勢を決定する方法。
2. 前記第１及び第２のセンサは、夫々観測分解能を有し、前記第１及び第２の移動ベクトル量の一方が前記観測分解能よりも小さい場合に当該一方の移動ベクトル量に係るベクトルであって、前記一方の移動ベクトル量に対応する前記第１及び第２の観測点の一方と前記球体中心を結ぶベクトルを球体が姿勢変更に用いる回転軸として選択することを特徴とする請求項１に記載の方法。
3. 前記第１及び第２の移動ベクトル量のいずれかのベクトル量を前記回転面半径で除算して回転角を演算することを特徴とする球体の姿勢を決定する方法。
4. 前記球体表面上に第３の観測点を含む第３の観測面に非接触に対向して設置され、前記第３の観測面の２次元平行移動量を座標量として検知して少なくとも第３のセンサ信号を夫々発生する少なくとも第３のセンサを更に具備し、
   前記第１、第２及び第３のセンサは、夫々観測分解能を有し、
   前記球体が姿勢を変更した際に、前記第３のセンサ信号から前記第３の観測面の平行移動量をセンサ観測面座標系における第３の移動ベクトル量として演算し、
   この第３の移動ベクトル量を基準座標系に変換して第３の変換ベクトルを求め、
   前記観測分解能に依存した観測誤差量を最小にするために、前記第１、第２及び第３の変換ベクトルの組み合わせに係る式（１１）で示すベクトルＵのノルムを最小にする２組の変換ベクトルの組を選択し、
   

   

   ここで、ｘi、ｙｉ，ｚｉ，ｘｊ、ｙｊ，ｚｊは、前記ベクトルＵのノルムを最小にする２組の変換ベクトルに相当する観測点の座標を示し、
   前記選択された変換ベクトルの外積演算により求めるベクトルから、前記球体の中心を通るベクトルを特定して前記球体の姿勢変更の基準となる回転軸ベクトルを演算することを特徴とする請求項１に記載の方法。
5. 前記球体表面上に第３の観測点を含む第３の観測面に非接触に対向して設置され、前記第３の観測面の２次元平行移動量を座標量として検知して少なくとも第３のセンサ信号を夫々発生する少なくとも第３のセンサを更に具備し、
   前記第１、第２及び第３のセンサは、夫々観測分解能を有し、
   前記球体が姿勢を変更した際に、前記第３のセンサ信号から前記第３の観測面の平行移動量をセンサ観測面座標系における第３の移動ベクトル量として演算し、
   この第３の移動ベクトル量を基準となる慣性座標系に変換して第３の変換ベクトルを求め、
   前記回転中心と、前記観測点を通る回転軸に垂直な平面である観測面との距離から決まる前記回転面半径が最も大きい２組の変換ベクトルの組を選択し、
   前記選択された変換ベクトルの外積演算により求めるベクトルから、前記球体の中心を通るベクトルを特定して前記球体の姿勢変更の基準となる慣性座標系における回転軸ベクトルを演算することを特徴とする請求項１に記載の方法。
6. 前記第１及び第２のセンサは、前記球体の赤道面よりも下部を観測点とし、前記第１及び第２のセンサは、前記赤道面の下方の空間に配置されていることを特徴とする請求項１に記載の方法。
7. 前記球体表面は、前記１及び第２のセンサが２次元平行移動量を検出するために、センサ分解能に相当する大きさと等価な模様あるいは凹凸を有し、この凹凸の大きさは、前記球体の駆動を妨げない大きさであることを特徴とする請求項１に記載の方法。
8. 回転駆動される球体と、
   前記球体を支える基台と、
   球体表面上の第１及び第２の観測点を含む第１及び第２の観測面に非接触に対向して設置され、前記第１及び第２の観測面の２次元平行移動量を座標量として検知して少なくとも第１及び第２のセンサ信号を夫々発生する少なくとも第１及び第２のセンサと、
   前記球体が姿勢を変更した際に、前記第１及び第２のセンサ信号から前記第１及び第２の観測面の平行移動量をセンサ観測面座標系における第１及び第２の移動ベクトル量として演算し、この第１、第２の移動ベクトル量を基準座標系に変換して第１及び第２の変換ベクトルを求め、前記第１及び第２の変換ベクトルの外積演算により求めるベクトルから、前記球体の中心を通るベクトルを特定して前記球体の姿勢変更の基準となる回転軸ベクトルを演算し、この回転軸ベクトル及び前記球体表面上の観測点の座標から回転面を特定し、この回転面から前記球体の中心までの回転面距離を求め、この回転面距離及び前記球の半径から前記回転面の回転面半径を求め、前記第１及び第２のベクトル量のいずれかのベクトル量及び前記回転面半径から回転角を演算する演算部と、
   を具備する球体の姿勢を決定する装置。
9. 前記第１及び第２のセンサは、夫々観測分解能を有し、
   前記演算部は、前記第１及び第２の移動ベクトル量の一方が前記観測分解能よりも小さい場合に当該一方の移動ベクトル量に係るベクトルであって、前記一方の移動ベクトル量に対応する前記第１及び第２の観測点の一方と前記球体中心を結ぶベクトルを球体が姿勢変更に用いる回転軸として選択することを特徴とする請求項８に記載の装置。
10. 前記演算部は、前記第１及び第２の移動ベクトル量のいずれかのベクトル量を前記回転面半径で除算して回転角を演算することを特徴とする請求項８の装置。
11. 前記球体表面上に第３の観測点を含む第３の観測面に非接触に対向して設置され、前記第３の観測面の２次元平行移動量を座標量として検知して少なくとも第３のセンサ信号を夫々発生する少なくとも第３のセンサを更に具備し、
    前記第１、第２及び第３のセンサは、夫々観測分解能を有し、
    前記演算部は、前記球体が姿勢を変更した際に、前記第３のセンサ信号から前記第３の観測面の平行移動量をセンサ観測面座標系における第３の移動ベクトル量として演算し、この第３の移動ベクトル量を基準座標系に変換して第３の変換ベクトルを求め、前記観測分解能に依存した観測誤差量を最小にするために、前記第１、第２及び第３の変換ベクトルの組み合わせに係る式（１１）で示すベクトルＵのノルムを最小にする２組の変換ベクトルの組を選択し、
    

    

    ここで、ｘi、ｙｉ，ｚｉ，ｘｊ、ｙｊ，ｚｊは、前記ベクトルＵのノルムを最小にする２組の変換ベクトルに相当する観測点の座標を示し、
    前記選択された変換ベクトルの外積演算により求めるベクトルから、前記球体の中心を通るベクトルを特定して前記球体の姿勢変更の基準となる回転軸ベクトルを演算することを特徴とする請求項８に記載の装置。
12. 前記球体表面上に第３の観測点を含む第３の観測面に非接触に対向して設置され、前記第３の観測面の２次元平行移動量を座標量として検知して少なくとも第３のセンサ信号を夫々発生する少なくとも第３のセンサを更に具備し、
    前記第１、第２及び第３のセンサは、夫々観測分解能を有し、
    前記演算部は、前記球体が姿勢を変更した際に、前記第３のセンサ信号から前記第３の観測面の平行移動量をセンサ観測面座標系における第３の移動ベクトル量として演算し、この第３の移動ベクトル量を基準座標系に変換して第３の変換ベクトルを求め、前記回転中心と、前記観測点を通る回転軸に垂直な平面である観測面との距離から決まる前記回転面半径が最も大きい２組の変換ベクトルの組を選択し、前記選択された変換ベクトルの外積演算により求めるベクトルから、前記球体の中心を通るベクトルを特定して前記球体の姿勢変更の基準となる回転軸ベクトルを演算することを特徴とする請求項８に記載の装置。
13. 前記第１及び第２のセンサは、前記球体の赤道面よりも下部を観測点とし、前記第１及び第２のセンサは、前記赤道面の下方の空間に配置されていることを特徴とする請求項８に記載の装置。
14. 前記球体表面は、前記１及び第２のセンサが２次元平行移動量を検出するために、センサ分解能に相当する大きさと等価な模様あるいは凹凸を有し、この凹凸の大きさは、前記球体の駆動を妨げない大きさであることを特徴とする請求項８に記載の装置。

Images