(57)【特許請求の範囲】
【請求項１】
光ファイバジャイロの異常検出装置であって、
光ファイバジャイロの時計回り信号及び反時計回り信号にそれぞれ含まれる時計回り方向の角速度及び反時計回り方向の角速度に応じた周期のパルスを所定時間内においてサンプリングしてパルス数を計数するサンプリング器と、
前記時計回り信号及び前記反時計回り信号に所定周期の疑似パルス信号を同一頻度で付加する擬似信号付加器であり、付加された擬似パルス信号は前記時計回り信号及び前記反時計回り信号の少なくともいずれかから検出可能である擬似信号付加器と、
前記時計回り信号のパルス数及び前記反時計回り信号のパルス数のいずれも所定のしきい値より小さく、前記擬似パルス信号が存在しないときに前記光ファイバジャイロの異常を検出する異常判定器と、
を有する光ファイバジャイロの異常検出装置。
【請求項２】
光ファイバジャイロの異常検出装置であって、
光ファイバジャイロの時計回り信号及び反時計回り信号にそれぞれ含まれる、時計回り方向の角速度及び反時計回り方向の角速度に応じた周期のパルスの量子化ノイズを検出するサンプリング器と、
前記時計回り信号及び前記反時計回り信号に所定周期の疑似パルス信号を同一頻度で付加する擬似信号付加器であり、付加された擬似パルス信号は前記時計回り信号及び前記反時計回り信号の少なくともいずれかから検出可能である擬似信号付加器と、
前記量子化ノイズと前記擬似パルス信号のうちの一つが存在しないときに前記光ファイバジャイロの異常を検出する異常判定器と、
を有する光ファイバジャイロの異常検出装置。
【請求項３】
光ファイバジャイロの異常検出装置であって、
光ファイバジャイロの時計回り信号及び反時計回り信号にそれぞれ含まれる、時計回り方向の角速度及び反時計回り方向の角速度に応じた周期のパルスを所定時間内においてサンプリングしてパルス数を計数するサンプリング器と、
光ファイバジャイロの時計回り信号及び反時計回り信号にそれぞれ含まれる、時計回り方向の角速度及び反時計回り方向の角速度に応じた周期のパルスの量子化ノイズを検出する検出器と、
前記時計回り信号及び前記反時計回り信号に所定周期の疑似パルス信号を同一頻度で付加する擬似信号付加器であり、付加された擬似パルス信号は前記時計回り信号及び前記反時計回り信号の少なくともいずれかから検出可能である擬似信号付加器と、
前記時計回り信号のパルス数及び前記反時計回り信号のパルス数のいずれも所定のしきい値より小さく、かつ、前記量子化ノイズが存在せず、前記擬似パルス信号が存在しないときに前記光ファイバジャイロの異常を検出する異常判定器と、
を有する光ファイバジャイロの異常検出装置。
【請求項４】
請求項１、３のいずれかに記載の装置において、さらに、
前記所定時間を可変設定する設定器
を有する光ファイバジャイロの異常検出装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、光ファイバジャイロの異常を検出する装置に関する。
【背景技術】
【０００２】
ロボット等の移動体の姿勢制御に加速度センサやヨーレートセンサが用いられている。直交する３軸をｘ軸、ｙ軸、ｚ軸とすると、各軸方向の加速度を３個の加速度センサで検出し、各軸回りのヨーレートを３個のヨーレートセンサで検出する。軸回りの角度、あるいは姿勢角は、ヨーレートセンサの出力を時間積分して得られ、ピッチ角、ロール角、ヨー角が算出される。
【０００３】
特開２００４−２６８７３０号公報には、ジャイロセンサから出力される加速度データ及び姿勢データを用いて姿勢制御する技術が開示されている。
【０００４】
しかしながら、角速度センサのオフセット及びドリフトが大きいと、角速度の積分により姿勢角を求めるため、オフセット等が徐々に蓄積して極めて大きな値となり、時間とともに増加、発散してしまう。光ファイバジャイロを用いることで、ドリフト分の少ない高精度な角速度検出が可能であるが、光ファイバジャイロは光回路を用いているため、内部の信号を取り出すことが困難で異常検出が難しい問題がある。
【発明の開示】
【発明が解決しようとする課題】
【０００５】
本発明の目的は、光ファイバジャイロの異常を容易に検出できる装置を提供することにある。
【課題を解決するための手段】
【０００６】
本発明は、光ファイバジャイロの異常検出装置であって、光ファイバジャイロの時計回り信号及び反時計回り信号にそれぞれ含まれる時計回り方向の角速度及び反時計回り方向の角速度に応じた周期のパルスを所定時間内においてサンプリングしてパルス数を計数するサンプリング器と、前記時計回り信号及び前記反時計回り信号に所定周期の疑似パルス信号を同一頻度で付加する擬似信号付加器であり、付加された擬似パルス信号は前記時計回り信号及び前記反時計回り信号の少なくともいずれかから検出可能である擬似信号付加器と、前記時計回り信号のパルス数及び前記反時計回り信号のパルス数のいずれも所定のしきい値より小さく、前記擬似パルス信号が存在しないときに前記光ファイバジャイロの異常を検出する異常判定器とを有する。
【０００７】
また、本発明は、光ファイバジャイロの異常検出装置であって、光ファイバジャイロの時計回り信号及び反時計回り信号にそれぞれ含まれる、時計回り方向の角速度及び反時計回り方向の角速度に応じた周期のパルスの量子化ノイズを検出するサンプリング器と、前記時計回り信号及び前記反時計回り信号に所定周期の疑似パルス信号を同一頻度で付加する擬似信号付加器であり、付加された擬似パルス信号は前記時計回り信号及び前記反時計回り信号の少なくともいずれかから検出可能である擬似信号付加器と、前記量子化ノイズと前記擬似パルス信号のうちの一つが存在しないときに前記光ファイバジャイロの異常を検出する異常判定器とを有する。
【０００８】
また、本発明は、光ファイバジャイロの異常検出装置であって、光ファイバジャイロの時計回り信号及び反時計回り信号にそれぞれ含まれる、時計回り方向の角速度及び反時計回り方向の角速度に応じた周期のパルスを所定時間内においてサンプリングしてパルス数を計数するサンプリング器と、光ファイバジャイロの時計回り信号及び反時計回り信号にそれぞれ含まれる、時計回り方向の角速度及び反時計回り方向の角速度に応じた周期のパルスの量子化ノイズを検出する検出器と、前記時計回り信号及び前記反時計回り信号に所定周期の疑似パルス信号を同一頻度で付加する擬似信号付加器であり、付加された擬似パルス信号は前記時計回り信号及び前記反時計回り信号の少なくともいずれかから検出可能である擬似信号付加器と、前記時計回り信号のパルス数及び前記反時計回り信号のパルス数のいずれも所定のしきい値より小さく、かつ、前記量子化ノイズが存在せず、前記擬似パルス信号が存在しないときに前記光ファイバジャイロの異常を検出する異常判定器とを有する。
【０００９】
本発明の１つの態様では、さらに、前記所定時間を可変設定する設定器を有する。
【００１０】
本発明の態様によれば、光ファイバジャイロの異常を簡易に検出できる。
【発明を実施するための最良の形態】
【００１１】
以下、図面に基づき本発明の実施形態について説明する。
【００１２】
＜第１実施形態＞
図１に、本実施形態の構成ブロック図を示す。ロボット等の運動体の所定位置に光ファイバジャイロ（ＦＯＧ）１０が設けられる。
【００１３】
光ファイバジャイロ１０は、時計回りの信号であるＣＷ信号及び反時計回りの信号であるＣＣＷ信号を出力する。光ファイバジャイロ１０は周知であるが、以下に簡単に説明する。光ファイバジャイロ１０では、光ファイバをコイルの回りに周回させ、光源からのレーザ光を光ファイバに入射させてそれぞれ時計回り及び反時計回りに進行させる。光の速度は光ファイバの動きによらず一定であるから、光ファイバの出口が動くことによりこの出口にレーザ光が到達するまでに要する時間が回転の速さに比例して変化する。この時間の変化を検出することで光ファイバの回転速度、すなわち運動体の角速度を検出する。光ファイバジャイロ１０は、運動体が時計回りに回転したときに、１パルスあたり例えば約４．５秒の角度に相当するＣＷ信号（パルス信号）を出力し、運動体が反時計回りに回転したときに、同様に１パルスあたり約４．５秒の角度に相当するＣＣＷ信号（パルス信号）を出力する。運動体の角速度が増大するとパルスの周期が短くなる。したがって、ＣＷ信号に含まれるパルス数を計測することでその時間における回転角度、すなわち時計回りの角速度が得られ、ＣＣＷ信号に含まれるパルス数を計測することで反時計回りの角速度が得られる。運動体の正味の角速度は、その時間におけるＣＷ方向の角速度とＣＣＷ方向の角速度の差により得られる。光ファイバジャイロ１０は、ＣＷ信号及びＣＣＷ信号をサンプリング器１２、１８に出力する。
【００１４】
サンプリング器１２、１８は、それぞれＣＷ信号、ＣＣＷ信号を所定の期間だけサンプリングしてパルス数を計数する。所定の期間はサンプリング時間発生器２２で設定される。サンプリング器１２、１８は、パルス数を角速度変換器１４、２０に出力する。また、サンプリング器１２、１８は、パルス数を異常判定器２８に出力する。
【００１５】
角速度変換器１４、２０は、サンプリング器１２、１８からのパルス数、すなわち所定のサンプリング期間におけるパルス数に予め定めた係数を乗じてパルス数をＣＷ方向の角速度及びＣＣＷ方向の角速度に変換する。例えば、１００ｍｓ間のサンプリング期間に１０００パルスがサンプリングされれば、４．５（秒角／パルス）＊１０００（パルス）／０．１（ｓ）＝４５０００（秒角／ｓ）＝１２．５（ｄｅｇ／ｓ）となる。角速度変換器１４、２０は、演算して得られた角速度を角速度合成器１６に出力する。
【００１６】
角速度合成器１６は、ＣＷ方向の角速度及びＣＣＷ方向の角速度成分を合成し（両者の差分を演算し）、角速度を検出する。角速度合成器１６は、演算して得られた角速度をフィルタ２４に出力する。
【００１７】
フィルタ（ローパスフィルタ）２４は、角速度合成器１６からの角速度に含まれるノイズを除去し、出力器２６に出力する。
【００１８】
出力器２６は、ロボットの姿勢を制御するメインプロセッサ（ホストプロセッサ）からのコマンドに応じて、検出した角速度、あるいはこれを積分して得られた姿勢角をメインプロセッサに送信する。
【００１９】
一方、所定のサンプリング期間におけるパルス数は、上記のように異常判定器２８にも出力される。異常判定器２８は、ＣＷ信号のパルス数及びＣＣＷ信号のパルス数をそれぞれしきい値と比較する。光ファイバジャイロ１０に断線、光路切断、接続不良等の異常があれば、ＣＷ信号あるいはＣＣＷ信号のパルス数が零、あるいは著しく低下する。そこで、異常判定器２８は、ＣＷ信号のパルス数及びＣＣＷ信号のパルス数の少なくともいずれかがしきい値以上であれば光ファイバジャイロ１０は正常と判定し、ＣＷ信号のパルス数及びＣＣＷ信号のパルス数のいずれもがしきい値より小さければ異常と判定して判定結果を出力器２６に出力する。異常判定器２８は、このように所定のサンプリング期間におけるパルス数を用いて異常の有無を判定するが、ＣＷ信号、ＣＣＷ信号にはパルス化に伴ってランダムな量子化ノイズが混入している。この量子化ノイズは通常信号処理に不要であるため除去するが、断線等の異常があると量子化ノイズも存在しないこととなる。そこで、このことを利用し、ＣＷ信号、ＣＣＷ信号に含まれる量子化ノイズを検出し、その有無により異常を判定してもよい。量子化ノイズの有無による異常判定は、上記のパルス数としきい値との大小比較による異常判定と併せて、あるいは補完的に用いることができる。例えば運動体の回転中あるいは移動中はパルス数としきい値との大小比較による異常判定を行い、運動体の静止中は量子化ノイズの有無による異常判定に切り替える等である。運動体の静止中はＣＷ信号、ＣＣＷ信号からパルスが出力されないが、このように量子化ノイズの有無を検出することで運動中、静止中を問わず異常を検出できる。量子化ノイズはＣＷ信号、ＣＣＷ信号のどちらにも含まれているが、十分長いサンプリング期間において両信号に含まれる量子化ノイズのパルス数がほぼ同じとなるため、ＣＷ信号とＣＣＷ信号の差分を演算することで、相殺されるという特徴を有している。従って、サンプリング器１２、１８後のＣＷ信号パルス数とＣＣＷ信号パルス数を個々にモニタすることにより光ファイバジャイロの異常を検知することができる。
【００２０】
異常検出のアルゴリズムを具体的に例示すると以下のとおりである。まず、サンプリング期間を所定期間に設定するとともにしきい値を十分小さい値に設定し、ＣＷ信号及びＣＣＷ信号のパルス数をしきい値と比較する。パルス数がしきい値以上であれば正常と判定するが、両信号のパルス数がいずれもしきい値より小さければ、次に量子化ノイズの有無を判定する。パルス数がしきい値より小さいが量子化ノイズが存在する場合には断線等がなく正常と判定し、パルス数がしきい値よりも小さくかつ量子化ノイズも存在しない場合に異常と判定する。
【００２１】
本実施形態では、光ファイバジャイロ１０の光回路をそのまま維持しつつ光ファイバジャイロ１０の異常を簡易に検出でき、光ファイバジャイロ１０を用いた角速度検出システムあるいは姿勢角検出システム、さらには姿勢制御システムの信頼性を向上できる。
【００２２】
＜第２実施形態＞
図２に、本実施形態の構成ブロック図を示す。図１と異なる点は、ＣＷ信号及びＣＣＷ信号に疑似信号を付加する疑似信号付加器３０、３２を設けた点、及び角速度変換器１４、２０にそれぞれ角速度を演算するための係数を供給する係数器３４、３６を設けた点である。
【００２３】
疑似信号付加器３０、３２は、疑似パルス信号として低周波のパルス信号を発生してそれぞれＣＷ信号、ＣＣＷ信号に付加する。サンプリング器１２、２２は、所定のサンプリング期間におけるパルス数をカウントするから、本来のＣＷ信号、ＣＣＷ信号のパルスの他に、この疑似パルス数も検出して異常判定器２８に出力する。異常判定器２８は、予め周期が既知の疑似パルスが検出されたか否かを判定し、疑似パルス信号が存在しない場合には断線あるいは接続不良等の異常が生じていると判定する。疑似パルスはＣＷ信号、ＣＣＷ信号に同じ頻度で重畳させるので、異常判定器２８で検知できるが、角速度合成器１６により差を演算することで、出力へ影響を与えない。疑似パルス信号はサンプリング期間においてＣＷ、ＣＣＷで同数とみなせる範囲で、その周期と個数を独立に調整できる。
【００２４】
係数器３４、３６は、ＣＷ信号及びＣＣＷ信号のパルス数から角速度を算出するための係数（変換係数）を角速度変換器１４、２０に供給する。係数器３４、３６の係数を独立に設定することで、ＣＷ、ＣＣＷ信号の感度差を調整することができる。
【００２５】
＜第３実施形態＞
図３に、本実施形態の構成ブロック図を示す。図１と異なる点は、サンプリング時間発生器２２における所定のサンプリング期間を設定するレジスタ３８、異常判定器２８における判定しきい値を設定するレジスタ４２、角速度変換器１４の変換係数を設定する係数器３４、３６を設け、これらの値をユーザが所望の値に設定するための入力器４０を設けた点である。入力器４０及びレジスタ３８によりサンプリング期間を可変設定することで、運動体の運動特性に合わせてサンプリング期間を適宜設定することができ、運動体に応じた応答性を実現できる。すなわち、低速で運動する運動体に対してはパルスの周期が増大するのでサンプリング期間を長くし、高速で運動する運動体に対してパルスの周期が短くなるのでサンプリング期間を短くする。入力器４０及びレジスタ４２により異常判定器２８でのしきい値を調整することによっても、運動体の運動特性に合わせて異常判定を実行できる。すなわち、低速で運動する運動体に対してしきい値を小さくし、高速で運動する運動体に対してしきい値を大きくする等である。
【００２６】
また、図３では、さらに角速度合成器１６からの角速度に含まれるノイズを除去するフィルタ（ローパスフィルタ）２４のカットオフ周波数ｆｃを可変設定するためのｆｃ（カットオフ周波数）設定器４８及びレジスタ５０と、減衰率を可変設定するための段数（タップ数）設定器４４及びレジスタ４６を設けている。ｆｃ及び段数はそれぞれレジスタ５０、４６により設定され、レジスタ５０、４６の値はユーザが入力器４０を用いて所望の値に設定できる。これにより、応答性、帯域の動的な対応が可能となる。
【図面の簡単な説明】
【００２７】
【図１】第１の実施形態の構成ブロック図である。
【図２】第２の実施形態の構成ブロック図である。
【図３】第３の実施形態の構成ブロック図である。
【図１】
0004751931.tif 000002
【図２】
0004751931.tif 000003
【図３】
0004751931.tif 000004