(57)【要約】【課題】本発明は、ピッチ角θが±９０°付近となるような移動体であっても移動体の姿勢制御をより適切に行うことができる慣性装置を提供することを目的とするものである。【解決手段】慣性センサ１０は、移動体のヨー角ψ、ピッチ角θ及びロール角φを検出する。慣性センサ１０によって検出されたヨー角ψ、ピッチ角θ及びロール角φは、クオータ二オン算出部１１に入力される。クオータ二オン算出部１１は、慣性センサ１０の出力からクオータ二オンＱ（ｑｘ、ｑｙ、ｑｚ、ｑｗ）を算出して出力する。【選択図】図１
2015004650.tif 000002
【特許請求の範囲】
【請求項１】
移動体に取付けられ、前記移動体のヨー角ψ、ピッチ角θ及びロール角φを検出する慣性センサと、
前記慣性センサによって検出されるヨー角ψ、ピッチ角θ及びロール角φに基づいて下記の式で表わされるクオータ二オンＱ（ｑｘ，ｑｙ，ｑｚ，ｑｗ）を算出して出力するクオータ二オン算出部と
【数１】
2015004650.tif 000007
を備えていることを特徴とする慣性装置。
【請求項２】
前記クオータ二オン算出部は、前記慣性センサによって検出されるピッチ角θが＋８５°よりも大きいか又は−８５°よりも小さい場合に、下記の式で表わされるｑｘ（ψ＝０）を算出するとともに該ｑｘ（ψ＝０）をロール角算手段に入力し、
【数２】
2015004650.tif 000008
前記ロール角算出手段は、前記ｑｘ（ψ＝０）を下記の式に代入してロール角φを算出する
【数３】
2015004650.tif 000009
ことを特徴とする請求項１記載の慣性装置。
【請求項３】
前記慣性センサ及び前記ロール角算出手段の出力は姿勢制御手段に入力され、
前記姿勢制御手段は、前記ロール角算出手段によってロール角φが算出される場合に、前記慣性センサによって検出されるロール角φを無効にして、前記ロール角算出手段によって算出されるロール角φを用いて前記移動体の姿勢制御を行う
ことを特徴とする請求項２記載の慣性装置。
【請求項４】
前記移動体は、飛翔体又は潜水体であることを特徴とする請求項１から請求項３までのいずれか１項に記載の慣性装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ヨー角ψ、ピッチ角θ及びロール角φを検出する慣性装置に関する。
【背景技術】
【０００２】
従来用いられていたこの種の慣性装置としては、例えば下記の特許文献１等に示されている構成を挙げることができる。すなわち、従来装置では、例えばジャイロスコープ等によりヨー角ψ、ピッチ角θ及びロール角φを検出している。ヨー角ψ（方位角）、ピッチ角θ（仰俯角）及びロール角φは、互いに直交する３軸回りの回転角度である。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２００７−１５５５８４号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
例えば飛翔体や潜水体等の移動体では、ピッチ角θが±９０°付近となる場合がある。このような移動体に上記のような従来の慣性装置を適用した場合、ピッチ角θが±９０°付近となった際にヨー角ψ及びロール角φの変化が急峻になり、ヨー角ψ及びロール角φを用いた移動体の姿勢制御を行うことが困難となる。
【０００５】
本発明は、上記のような課題を解決するためになされたものであり、その目的は、ピッチ角θが±９０°付近となるような移動体であっても移動体の姿勢制御をより適切に行うことができる慣性装置を提供することである。
【課題を解決するための手段】
【０００６】
本発明に係る慣性装置は、
移動体に取付けられ、移動体のヨー角ψ、ピッチ角θ及びロール角φを検出する慣性センサと、
慣性センサによって検出されるヨー角ψ、ピッチ角θ及びロール角φに基づいて下記の式で表わされるクオータ二オンＱ（ｑｘ，ｑｙ，ｑｚ，ｑｗ）を算出して出力するクオータ二オン算出部と
【数１】
2015004650.tif 000003
を備える。
【発明の効果】
【０００７】
本発明の慣性装置によれば、クオータ二オン算出部がクオータ二オンＱ（ｑｘ，ｑｙ，ｑｚ，ｑｗ）を算出して出力するので、ピッチ角θが±９０°付近となるような移動体であっても移動体の姿勢制御をより適切に行うことができる。
【図面の簡単な説明】
【０００８】
【図１】本発明の実施の形態１による慣性装置を示すブロック図である。
【発明を実施するための形態】
【０００９】
以下、本発明を実施するための形態について、図面を参照して説明する。
実施の形態１．
図１は、本発明の実施の形態１による慣性装置を示すブロック図である。図において、慣性装置には、慣性センサ１０及びクオータ二オン算出部１１が含まれている。
【００１０】
慣性センサ１０は、飛翔体及び潜水体を含む移動体（図示せず）に取付けられており、移動体のヨー角ψ、ピッチ角θ及びロール角φを検出する。周知のように、ヨー角ψ（方位角）、ピッチ角θ（仰俯角）及びロール角φは、互いに直交する３軸回りの回転角度である。飛翔体とは空中を飛翔する物体であり、潜水体とは水中を潜行する物体である。これら飛翔体及び潜水体は、ピッチ角θが±９０°付近となるような運動を行うことができるものである。なお、慣性センサ１０は、例えばガスレートジャイロやＭＥＭＳジャイロ等により構成されている。
【００１１】
クオータ二オン算出部１１は、所定のプログラムに沿って演算を行うコンピュータ等により構成されるものである。クオータ二オン算出部１１には、慣性センサ１０によって検出されるヨー角ψ、ピッチ角θ及びロール角φが入力される。クオータ二オン算出部１１は、下記の式で表わされるクオータ二オンＱ（ｑｘ，ｑｙ，ｑｚ，ｑｗ）を慣性センサ１０の出力に基づいて算出して、このクオータ二オンＱを姿勢角情報として出力する。
【００１２】
【数２】
2015004650.tif 000004
【００１３】
また、クオータ二オン算出部１１は、慣性センサ１０によって検出されるピッチ角θが＋８５°よりも大きいか又は−８５°よりも小さい場合に、下記の式で表わされるｑｘ（ψ＝０）を算出して出力する。ｑｘ（ψ＝０）は、上述のクオータ二オンＱに含まれるｑｘにおいて、ヨー角ψを０°としたものである。θ及びφには、慣性センサ１０によって検出されるピッチ角θ及びロール角φが代入される。
【００１４】
【数３】
2015004650.tif 000005
【００１５】
クオータ二オン算出部１１の出力は、ロール角算出手段１２に入力される。ロール角算出手段１２は、所定のプログラムに沿って演算を行うコンピュータ等により構成されるものであり、慣性センサ１０に含まれていてよい。ロール角算出手段１２は、クオータ二オン算出部１１によってｑｘ（ψ＝０）が算出された場合に、ｑｘ（ψ＝０）を下記の式に代入して、ロール角φを算出する。
【００１６】
【数４】
2015004650.tif 000006
【００１７】
クオータ二オン算出部１１によってｑｘ（ψ＝０）が算出された場合、すなわちピッチ角θが＋８５°よりも大きいか又は−８５°よりも小さい場合にロール角算出手段１２がロール角φを算出するのは、ピッチ角θが＋８５°よりも大きいか又は−８５°よりも小さい場合に、慣性センサ１０によって検出されるヨー角ψ及びロール角φの変化が急峻になり、これらを用いた制御が困難になるためである。なお、ロール角算出手段１２によって算出されるロール角φをシミュレーションにより検証したところ、その誤差は±０．５°程度であった。その際のｑｘ（ψ＝０）は、小数点以下３桁までを用いた。
【００１８】
慣性センサ１０及びロール角算出手段１２の出力は、姿勢制御手段１３に入力される。姿勢制御手段１３は、通常は慣性センサ１０の出力に基づいて移動体の姿勢を制御する。また、姿勢制御手段１３は、ロール角算出手段１２によってロール角φが算出された場合に、慣性センサ１０によって検出されるロール角φを無効にして、ロール角算出手段１２によって算出されたロール角φを用いて移動体の姿勢制御を行う。このとき、姿勢制御手段１３は、慣性センサ１０からのヨー角ψも無効にして、ψ＝０°として移動体の姿勢制御を行う。
【００１９】
このような慣性装置では、クオータ二オン算出部がクオータ二オンＱ（ｑｘ，ｑｙ，ｑｚ，ｑｗ）を算出して出力するので、ピッチ角θが±９０°付近となるような移動体であっても移動体の姿勢制御をより適切に行うことができる。本実施の形態の構成は、飛翔体又は潜水体に特に有効である。
【００２０】
慣性センサ１０によって検出されるピッチ角θが＋８５°よりも大きいか又は−８５°よりも小さい場合に、クオータ二オン算出部１１がｑｘ（ψ＝０）を算出し、このｑｘ（ψ＝０）を用いてロール角算出手段１２がロール角φを算出するので、より確実に移動体の姿勢制御を適切に行うことができる。
【００２１】
また、姿勢制御手段１３は、ロール角算出手段１２によってロール角φが算出される場合に、慣性センサ１０によって検出されるロール角φを無効にして、ロール角算出手段１２によって算出されたロール角φを用いて移動体の姿勢制御を行うので、より確実に移動体の姿勢制御を行うことができる。
【符号の説明】
【００２２】
１０  慣性センサ
１１  クオータ二オン算出部
１２  ロール角算出手段
１３  姿勢制御手段
【図１】
2015004650.tif 000010