JPH11190618A - 移動物体の位置及び姿勢検出方法 - Google Patents

Abstract

(57)【要約】 【課題】 角度格子と角度センサによる両者の相
対位置と姿勢を高精度にかつ迅速、簡便に検出する。 【解決手段】 角度センサ２０を角度格子１０の表面
に沿って定められた方向に所定の速度で走査し、角度格
子１０が静止していると見なせる状態でも生じる２次元
角度センサの出力信号の時間的変化から、その最初の立
ち上がりゼロクロス点、立ち上がり後の立ち下がりゼロ
クロス点、次の立ち上がりのゼロクロス点を検出し、そ
れらの間の時間間隔を計測し、この計測結果から２次元
目盛の目盛間の内挿値と２次元角度格子１０の角度セン
サ２０走査方向に対する面内回転角度姿勢であるヨーイ
ング角及び面外角度誤差であるピッチング角とローリン
グ角を検出する。

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】本発明は、角度格子と角度セ
ンサを用いて移動する物体の位置及び姿勢を検出するの
に好適な移動物体の位置及び姿勢検出方法に関するもの
である。

【０００２】

【従来の技術】この種の移動物体の位置及び姿勢検出方
法としては、先願として本出願人により開発され出願さ
れた特願平８−１９９１１５号がある。上記出願に関す
る移動物体の位置及び姿勢検出方法において、角度格子
のピッチ間の内挿（補間）値を得る手法としては、角度
格子の形状を正確に校正し、形状の確定している関数の
アナログ出力を読み取る方法が採られている。また、角
度格子と角度センサの相対的な角度姿勢、すなわちヨー
イング角、ピッチング角及びローリング角を得る手法と
しては、２個あるいは３個の２次元角度センサを既知の
間隔をおいて並べ、それぞれの角度センサが検出する２
〜３個の角度成分に関する連立方程式をたて、これを解
く方法が採られている。

【０００３】

【発明が解決しようとする課題】しかしながら、上記の
ような先願の検出方法では、角度格子を構成する角度目
盛の目盛間の内挿値を高精度に読み取るために角度格子
の角度形状の詳細を正確に知っている必要があり、その
ためには角度格子の校正が面倒であった。また、先願の
光学式の角度センサでは、一般に平行光束をを用いてい
るので、光ビームの細さに限界があり、格子ピッチが小
さい場合には、１つの光ビームのスポット内に複数の格
子ピッチが含まれるため、信号の変化量が小さくなる傾
向にある。また、角度格子の内面回転姿勢を検出するの
に２個または３個の角度センサを用いる方法では、検出
機構が複雑になり、データ処理にも時間を要するという
問題があった。

【０００４】本発明は、前記事情に鑑みてなされたもの
で、角度格子と角度センサによる両者の相対位置と姿勢
を高精度にかつ迅速、簡便に検出することができる移動
物体の位置及び姿勢検出方法を提供することを目的とす
る。

【０００５】

【課題を解決するための手段】前記目的を達成するため
に本発明は、移動する物体の位置及び姿勢を検出する検
出方法であって、基板の表面上または面内に形成され、
角度に関する性質が２次元方向に既知の関数の形で変化
する目盛を構成する角度格子と、前記角度格子面と相対
向して配置された角度センサを備え、前記角度センサも
しくは該角度センサから発する光スポットを前記角度格
子面に沿って定められた方向に所定の速度で走査し、該
走査時における前記角度センサの２次元角度出力のそれ
ぞれの成分がゼロとなるゼロクロス点を検出し、この各
ゼロクロス点間の通過時間を計測し、この時間計測値か
ら前記角度格子の目盛ピッチ間の内挿値を角度格子の面
内の回転角度姿勢であるヨーイング角及び角度格子の面
外の回転角度誤差であるピッチング角とローリング角を
算出することを特徴とする。

【０００６】本発明によれば、角度センサもしくは該角
度センサから発する光スポットを前記角度格子面に沿っ
て定められた方向に所定の速度で走査することにより、
両者の基準点が相対的に静止している時でも角度センサ
の角度出力に時間変化を生じさせ、その信号のゼロクロ
ス点の時間間隔を計測することで、角度格子の目盛間内
挿値、面内の角度姿勢及び面外の角度姿勢誤差であるピ
ッチング角とローリング角を実時間で検出することが可
能になる。また、基準点の相対移動があってもその速度
が走査速度に比べて小さいときには、同様の検出が可能
となる。

【０００７】また、本発明は、前記角度センサから前記
角度格子に向けて照射される光ビームの光路上に配置さ
れ、前記光ビームの透過量を大きくしたドットを前記角
度格子の目盛ピッチと同一のピッチで形成することによ
り前記角度格子に形成される光スポット中に多数の強い
光のピンスポットを生じさせるピンスポット生成部材を
有するものである。

【０００８】本発明によれば、角度格子に照射される光
スポット中に多数の強い光のピンスポットを形成し、こ
の各ピンスポットによる出力信号が加え合わされるか
ら、角度センサの角度信号の出力レベルを大きくできる
とともに、検出感度が向上し、かつ検出信号の対雑音比
を向上できる。

【０００９】

【発明の実施の形態】以下、本発明の実施の形態を図面
に基づいて説明する。図１は、本発明の実施の形態にお
ける角度格子と角度センサの関係を示す基本構成図であ
る。図１において、角度格子１０は、基板１０１の表面
上において直交する２方向（ｘ，ｙ方向）の角度に関す
る性質が既知の関数の形で変化する一定振幅（正弦波
状）の山谷の集合から構成されている。そして、この
ｘ，ｙの２次元角度格子１０の上方には、その表面に沿
って角度センサ２０が一定の方向に一定の速度で走査で
きるように配置されている。この図１における角度セン
サ２０の走査方向は、走査ライン３０で示すように、２
次元角度格子１０のｘ軸方向でｘ軸に対して角度αだけ
傾いた方向である。角度センサ２０には、図２に示す角
度格子１０の目盛線であるゼロクロスラインと交わる時
のｘ，ｙ方向の信号に基づいて制御され、ゼロクロス点
間隔を計測するカウンタ２２が接続され、さらに、カウ
ンタ２２には、カウンタ２２の計測値から角度αの値を
算出する演算部２３が接続されている。

【００１０】なお、角度センサ２０を角度格子１０に対
して一定の方向に一定の速度で走査する方法としては、
アクチュエータにより一定範囲を移動させる。また、角
度センサ２０が光学式のものである場合は、角度検出の
ための光スポットを音響光学偏向素子や電気光学結晶を
利用して高速に振ることにより、角度センサを走査する
のと同様な効果を与えることができる。

【００１１】図２は、目盛間内挿と面内回転姿勢の検出
方法を説明するための原理図を示すもので、角度センサ
２０の角度検出基準軸２０１が角度格子１０の表面に対
し鉛直な状態で走査した時の角度出力信号がゼロとなる
線を角度格子１０の目盛線として図示すると図２に示す
ようになる。

【００１２】図２において、実線で示す線４１はｘ軸方
向の正の方向に角度センサ２０を走査した時、２次元角
度格子１０の角度がゼロとなる信号の立ち上がり側ｘ方
向ゼロクロスラインであり、破線で示す線４２はｘ軸方
向の正の方向に角度センサ２０を走査した時、２次元角
度格子１０の角度がゼロとなる信号の立ち下がり側ｘ方
向ゼロクロスラインである。また、実線で示す線４３は
ｙ軸方向の正の方向に角度センサ２０を走査した時、２
次元角度格子１０の角度がゼロとなる信号の立ち上がり
側ｙ方向ゼロクロスラインであり、破線で示す線４４は
ｙ軸方向の正の方向に角度センサ２０を走査した時、２
次元角度格子１０の角度がゼロとなる信号の立ち下がり
側ｘ方向ゼロクロスラインである。

【００１３】なお、実線で示す立ち上がり側ｘ方向ゼロ
クロスライン４１と立ち上がり側ｙ方向ゼロクロスライ
ン４３とがクロスする部分が図１に示した角度格子の山
の部分に対応し、破線で示す立ち下がり側ｘ方向ゼロク
ロスライン４２と立ち下がり側ｙ方向ゼロクロスライン
４４とがクロスする部分が図１に示した角度格子の谷の
部分に対応する。また、ｘ軸とαの角度なしている太い
実線３０は、角度センサ２０の走査線を示している。こ
の走査線３０の出発点が走査の基準点５０であり、この
走査基準点５０は角度センサ２０の位置を決める基準点
となる。

【００１４】角度格子１０と角度センサ基準点５０が相
対的に静止していると見なせる状態では、図２における
走査線３０が各ゼロクロスラインと交わる各ゼロクロス
点間の間隔は、対応する角度信号のゼロクロス点の時間
間隔に比例する。以下、立ち上がり側のゼロクロス点だ
けを使用して２次元角度格子の目盛間の内挿値と２次元
角度格子の角度センサ走査方向に対する面内回転角度姿
勢を検出する場合について説明する。

【００１５】この場合、角度センサ２０がｘ軸方向また
はｙ軸方向に走査された時にそれぞれの立ち上がり側ｘ
方向ゼロクロスライン４１または立ち上がり側ｙ方向ゼ
ロクロスライン４３で生じるゼロクロス点の時間間隔Ｔ
ｐｘ，Ｔｐｙは既知と考えてよいから、走査線３０がｘ
軸に対してαの角度傾いている時、ｘ，ｙ方向それぞれ
の立ち上がり側ｘ方向ゼロクロスライン４１から次の立
ち上がり側ｘ方向ゼロクロスライン４１までのゼロクロ
ス点間隔Ｔｘ（ｘ方向信号周期６０）、または立ち上が
り側ｙ方向ゼロクロスライン４３から次の立ち上がり側
ｙ方向ゼロクロスライン４３までのゼロクロス点間隔Ｔ
ｙ（ｙ方向信号周期６１）をカウンタ２２により計測
し、この計測結果に基づいて演算部２３により、次の
（１）式または（２）式を用いて角度αを計算すること
ができる。 ｃｏｓ（α）＝Ｔｘ／Ｔｐｘ・・・・・・・・・・・・（１） ｓｉｎ（α）＝Ｔｙ／Ｔｐｙ・・・・・・・・・・・・（２）

【００１６】また、角度センサが走査を開始してから走
査の基準点５０からｘ，ｙそれぞれの方向の最初の立ち
上がり側ｘ方向ゼロクロスライン４１または立ち上がり
側ｙ方向ゼロクロスライン４３と交わるゼロクロス点ま
での時間ｔｘ（ｘ方向信号端数時間６２），ｔｙ（ｙ方
向信号端数時間６３）をカウンタ２２により計測し、こ
の計測結果と上記ゼロクロス点間隔Ｔｘ，Ｔｙを基に、
ｔｘ／Ｔｘ，ｔｙ／Ｔｙから、角度センサ２０の基準位
置に関して、ｘ，ｙ方向の目盛間の内挿が可能になる。
なお、角度αが０度や９０度に近づくと、角度センサ２
０の走査長さによっては必要なゼロクロス点の幾つかが
現れない状態となる。このようにα＝４５度を中心にし
て、±４０度以上の回転角度を検出する必要がある時
は、図２の走査線３０の直交する方向にも角度センサ２
０を走査することで対処できる。

【００１７】図３は、角度格子１０がｘ，ｙ方向に正弦
波状に変化する山谷の集合から構成された場合におい
て、この角度格子１０に対して角度センサ２０がｘ軸に
対しαの角度方向に走査された時の角度信号のｘ方向成
分の時間変化の様子と、ピッチング角によるゼロクロス
点の変化の様子を示す説明図である。図３において、立
ち上がり側のゼロクロス点から立ち下がり側のゼロクロ
ス点までの時間をＴｘｄとする。ここで、角度格子１０
の表面に対して角度センサ２０の角度検出基準軸２０１
が正しく鉛直な関係にあれば、ｘ方向角度信号７０の出
力波形は図３に示すように一定振幅の正弦波となり、そ
の角度信号の出力がゼロとなるライン７１は信号振幅の
中心にある。この時、立ち下がりのゼロクロス点は立ち
上がりゼロクロス点の真ん中になり、Ｔｘｄ＝Ｔｘ／２
となる。

【００１８】もし、角度センサ２０の角度検出基準軸２
０１が角度格子１０の表面に対してｘ方向にβだけ傾く
と角度センサ２０の角度信号の出力がゼロとなるライン
７２は、図３に示すように−β分ゼロライン７１よりシ
フトする。これに伴い、立ち上がり側のゼロクロス点か
ら立ち下がり側のゼロクロス点までの時間はＴｘｄ’＝
Ｔｘｄ＋２ｔ₀ となり、鉛直となる時のＴｘｄよりも大
きくなる。一方、１周期の値はβの影響を受けないの
で、図３でＴｘ＝Ｔｘ’である。ここで、角度格子１０
の形状が既知であれば、βとＴｘｄ／Ｔｘの関係を予め
計算しておくことができるので、これを基に、ｘ方向の
傾斜角（ピッチング角）も計測することができる。勿
論、このピッチングによる目盛間内挿値ｔｘ／Ｔｘの誤
差も補正することができる。また、同様にして、ｙ方向
の角度信号の出力を用いれば、ｙ方向の傾斜角（ローリ
ング角）に関しても同様に計測することができる。

【００１９】このような本実施の形態によれば、角度格
子と角度センサによる位置検出時の目盛間内挿を高精度
に行うことができる。また、１個の２次元角度センサだ
けで、面内の２自由度の位置と姿勢を一括して検出する
ことができるほか、ピッチング角とローリング角という
２つの面外の角度姿勢の影響を目的の３自由度成分から
取り除くことができ、しかも、その面外の角度姿勢変化
を同時に検出することができる。また、１つの角度セン
サだけで、２次元的な移動物体の角度格子からの高さ方
向並進成分を除いた位置と姿勢の５自由度を分離検出す
ることができる。さらに、時間計測結果の簡単な演算処
理だけで位置と姿勢の成分が分離検出できるので、５自
由度の位置情報をリアルタイムに得ることができる。

【００２０】なお、角度格子を正弦波で構成されるもの
として説明したが、例えば、直径の決まった球を２次元
的に並べるだけでも角度格子は構成される。このときは
谷に相当する範囲の角度信号は検出できないので、ゼロ
クロスラインは、立上がり側だけしか得られない。この
ような簡便な角度格子を用いるときは、上記説明で述べ
た立下がり側のゼロクロス点を用いる方法は成立せず、
ピッチングなどの影響は無視するか、別途に求めた値を
利用するか、もう一つ角度センサを追加する方法を用い
ることになる。

【００２１】なお、上記実施の形態では、角度センサ２
０を角度格子１０に対して走査する場合について説明し
たが、本発明はこれに限定されず、角度センサ２０と角
度格子１０を相対的に走査する方式にも同様に適用する
ことができる。

【００２２】なお、以上は角度センサあるいは光スポッ
トを相対的に走査する所定の速度を一定として説明した
が、この速度は、変化の様子が既知であれば一定でなく
ても良い。例えば、一定周波数で角度センサを振動させ
るとセンサのｘ方向出力成分には１／Ｔｘを周波数とす
る波が既知の振動周波数に重なった形で含まれるので、
これを同期検波などの既存の技術で分離して必要な１／
Ｔｘを周波数とする波だけをとりだせば、上記の時間間
隔の計測手法でも、また周波数変化、すなわち１／Ｔｘ
の変化の計測手法でも、同様の効果を得ることが出来
る。

【００２３】図４は、本発明の検出方法における他の実
施の形態を示す投射光学系の構成図である。図４におい
て、投射光学系は、角度センサ２０から角度格子１０に
向けて照射される光ビーム２０２の光路上に光軸を一致
して配置され、光ビーム２０２を所定径（例えば0.２m
m）の平行光線に変換して光スポット８１を形成する投
射レンズ８０と、この投射レンズ８０の光スポット出射
側に配置され、光の透過量を大きくしたドット８２を角
度格子１０の目盛ピッチと同一のピッチで形成すること
により光スポット８１中に多数の強い光のピンスポット
８１１を生じさせるピンスポット生成部材８３とから構
成されている。

【００２４】上記のように構成された実施の形態におい
て、角度センサ２０から出射される光ビーム２０２が投
射光学系を通して角度格子１０に目盛表面に照射される
と、その光スポット８１中に多数の強い光のピンスポッ
ト８１１が形成され、この各ピンスポット８１１が角度
センサ一個分のビームとして作用するため、各ピンスポ
ット８１１の同一位相での反射光の方向変化だけが同期
して加え合わされ、その結果、角度センサ２０の角度信
号出力は、各ピンスポット８１１による出力信号が加え
合わされたものとなる。

【００２５】このような本実施の形態によれば、角度セ
ンサの角度信号の出力レベルを大きくできるとともに、
検出感度が向上し、かつ検出信号の対雑音比を向上する
ことができる。

【００２６】なお、上記実施形態は、請求項１の光学式
センサを走査する場合だけでなく、走査をしないで用い
る本出願人が既に出願している特願平８−１９９１１５
号の移動物体の位置及び姿勢検出方法でも有効である。

【００２７】

【発明の効果】上記のように本発明によれば、角度格子
と角度センサによる位置検出時の目盛間内挿を高精度に
行うことができる。また、本発明によれば、１個の２次
元角度センサだけで、面内の２自由度の位置と姿勢を一
括して検出することができるほか、ピッチング角とロー
リング角という１つの面外の角度姿勢の影響を目的の３
自由度成分から取り除くことができ、しかも、その面外
の角度姿勢変化を同時に検出することができる。また、
本発明によれば、１つの角度センサだけで、２次元的な
移動物体の角度格子からの高さ方向並進成分を除いた位
置と姿勢の５自由度を分離検出することができる。さら
に、本発明によれば、時間計測結果の簡単な演算処理だ
けで位置と姿勢の成分が分離検出できるので、５自由度
の位置情報をリアルタイムに得ることができる。

【００２８】また、本発明によれば、角度格子に照射さ
れる光スポット中に多数の強い光のピンスポットを形成
し、この各ピンスポットによる出力信号が加え合わされ
る構成にしたので、角度センサの角度信号の出力レベル
を大きくできるとともに、検出感度が向上し、かつ検出
信号の対雑音比を向上することができる。

【図面の簡単な説明】

【図１】本発明方法を適用した検出装置の実施の形態に
おける角度格子と角度センサの関係を示す基本構成図で
ある。

【図２】本発明の実施の形態における目盛間内挿と面内
回転姿勢の検出方法を説明するための原理図である。

【図３】本発明の実施の形態における角度信号のｘ方向
成分の時間変化の様子と、ピッチング角によるゼロクロ
ス点の変化の様子を示す説明図である。

【図４】本発明の検出方法における他の実施の形態を示
す投射光学系の構成図である。

【符号の説明】

１０ 角度格子 １０１ 基板 ２０ 角度センサ ２２ カウンタ ２３ 演算部 ２０１ 角度検出基準軸 ３０ 走査線 ４１ 立ち上がり側ｘ方向ゼロクロスライン ４２ 立ち下がり側ｘ方向ゼロクロスライン ４３ 立ち上がり側ｙ方向ゼロクロスライン ４４ 立ち下がり側ｙ方向ゼロクロスライン ８０ 投射レンズ ８１ 光スポット ８２ ドット ８３ ピンスポット生成部材 ８１１ ピンスポット

Claims (2)

【特許請求の範囲】
1. 【請求項１】 移動する物体の位置及び姿勢を検出する
   検出方法であって、基板の表面上または面内に形成さ
   れ、角度に関する性質が２次元方向に既知の関数の形で
   変化する目盛を構成する角度格子と、前記角度格子面と
   相対向して配置された角度センサを備え、前記角度セン
   サもしくは該角度センサから発する光スポットを前記角
   度格子面に沿って定められた方向に所定の速度で走査
   し、該走査時における前記角度センサの２次元角度出力
   のそれぞれの成分がゼロとなるゼロクロス点を検出し、
   この各ゼロクロス点間の通過時間を計測し、この時間計
   測値から前記角度格子の目盛ピッチ間の内挿値と角度格
   子の面内の回転角度姿勢であるヨーイング角及び角度格
   子の面外の回転角度誤差であるピッチング角とローリン
   グ角を算出することを特徴とする移動物体の位置及び姿
   勢検出方法。
2. 【請求項２】 前記角度センサから前記角度格子に向け
   て照射される光ビームの光路上に配置され、前記光ビー
   ムの透過量を大きくしたドットを前記角度格子の目盛ピ
   ッチと同一のピッチで形成することにより前記角度格子
   に形成される光スポット中に多数の強い光のピンスポッ
   トを生じさせるピンスポット生成部材を有する請求項１
   記載の移動物体の位置及び姿勢検出方法。