JP6251873B2 - 慣性装置 - Google Patents

Description

本発明は、ヨー角ψ、ピッチ角θ及びロール角φを検出する慣性装置に関する。

従来用いられていたこの種の慣性装置としては、例えば下記の特許文献１等に示されている構成を挙げることができる。すなわち、従来装置では、例えばジャイロスコープ等によりヨー角ψ、ピッチ角θ及びロール角φを検出している。ヨー角ψ（方位角）、ピッチ角θ（仰俯角）及びロール角φは、互いに直交する３軸回りの回転角度である。

特開２００７−１５５５８４号公報

例えば飛翔体や潜水体等の移動体では、ピッチ角θが±９０°付近となる場合がある。このような移動体に上記のような従来の慣性装置を適用した場合、ピッチ角θが±９０°付近となった際にヨー角ψ及びロール角φの変化が急峻になり、ヨー角ψ及びロール角φを用いた移動体の姿勢制御を行うことが困難となる。

本発明は、上記のような課題を解決するためになされたものであり、その目的は、ピッチ角θが±９０°付近となるような移動体であっても移動体の姿勢制御をより適切に行うことができる慣性装置を提供することである。

本発明に係る慣性装置は、
移動体に取付けられ、移動体のヨー角ψ、ピッチ角θ及びロール角φを検出する慣性センサと、
慣性センサによって検出されるヨー角ψ、ピッチ角θ及びロール角φに基づいて下記の式で表わされるクオータニオンＱ（ｑｘ，ｑｙ，ｑｚ，ｑｗ）を算出して出力するクオータニオン算出部と

を備え
クオータニオン算出部は、慣性センサによって検出されるピッチ角θが＋８５°よりも大きいか又は−８５°よりも小さい場合に、下記の式で表わされるｑｘ _{（ψ＝０）} を算出するとともに該ｑｘ _{（ψ＝０）} をロール角算出手段に入力し、

ロール角算出手段は、ｑｘ _{（ψ＝０）} を下記の式に代入してロール角φを算出する

。

本発明の慣性装置によれば、クオータニオン算出部がクオータニオンＱ（ｑｘ，ｑｙ，ｑｚ，ｑｗ）を算出して出力するので、ピッチ角θが±９０°付近となるような移動体であっても移動体の姿勢制御をより適切に行うことができる。

本発明の実施の形態１による慣性装置を示すブロック図である。

以下、本発明を実施するための形態について、図面を参照して説明する。
実施の形態１．
図１は、本発明の実施の形態１による慣性装置を示すブロック図である。図において、慣性装置には、慣性センサ１０及びクオータニオン算出部１１が含まれている。

慣性センサ１０は、飛翔体及び潜水体を含む移動体（図示せず）に取付けられており、移動体のヨー角ψ、ピッチ角θ及びロール角φを検出する。周知のように、ヨー角ψ（方位角）、ピッチ角θ（仰俯角）及びロール角φは、互いに直交する３軸回りの回転角度である。飛翔体とは空中を飛翔する物体であり、潜水体とは水中を潜行する物体である。これら飛翔体及び潜水体は、ピッチ角θが±９０°付近となるような運動を行うことができるものである。なお、慣性センサ１０は、例えばガスレートジャイロやＭＥＭＳジャイロ等により構成されている。

クオータニオン算出部１１は、所定のプログラムに沿って演算を行うコンピュータ等により構成されるものである。クオータニオン算出部１１には、慣性センサ１０によって検出されるヨー角ψ、ピッチ角θ及びロール角φが入力される。クオータニオン算出部１１は、下記の式で表わされるクオータニオンＱ（ｑｘ，ｑｙ，ｑｚ，ｑｗ）を慣性センサ１０の出力に基づいて算出して、このクオータニオンＱを姿勢角情報として出力する。

また、クオータニオン算出部１１は、慣性センサ１０によって検出されるピッチ角θが＋８５°よりも大きいか又は−８５°よりも小さい場合に、下記の式で表わされるｑｘ_{（ψ＝０）}を算出して出力する。ｑｘ_{（ψ＝０）}は、上述のクオータニオンＱに含まれるｑｘにおいて、ヨー角ψを０°としたものである。θ及びφには、慣性センサ１０によって検出されるピッチ角θ及びロール角φが代入される。

クオータニオン算出部１１の出力は、ロール角算出手段１２に入力される。ロール角算出手段１２は、所定のプログラムに沿って演算を行うコンピュータ等により構成されるものであり、慣性センサ１０に含まれていてよい。ロール角算出手段１２は、クオータニオン算出部１１によってｑｘ_{（ψ＝０）}が算出された場合に、ｑｘ_{（ψ＝０）}を下記の式に代入して、ロール角φを算出する。

クオータニオン算出部１１によってｑｘ_{（ψ＝０）}が算出された場合、すなわちピッチ角θが＋８５°よりも大きいか又は−８５°よりも小さい場合にロール角算出手段１２がロール角φを算出するのは、ピッチ角θが＋８５°よりも大きいか又は−８５°よりも小さい場合に、慣性センサ１０によって検出されるヨー角ψ及びロール角φの変化が急峻になり、これらを用いた制御が困難になるためである。なお、ロール角算出手段１２によって算出されるロール角φをシミュレーションにより検証したところ、その誤差は±０．５°程度であった。その際のｑｘ_{（ψ＝０）}は、小数点以下３桁までを用いた。

慣性センサ１０及びロール角算出手段１２の出力は、姿勢制御手段１３に入力される。姿勢制御手段１３は、通常は慣性センサ１０の出力に基づいて移動体の姿勢を制御する。また、姿勢制御手段１３は、ロール角算出手段１２によってロール角φが算出された場合に、慣性センサ１０によって検出されるロール角φを無効にして、ロール角算出手段１２によって算出されたロール角φを用いて移動体の姿勢制御を行う。このとき、姿勢制御手段１３は、慣性センサ１０からのヨー角ψも無効にして、ψ＝０°として移動体の姿勢制御を行う。

このような慣性装置では、クオータニオン算出部がクオータニオンＱ（ｑｘ，ｑｙ，ｑｚ，ｑｗ）を算出して出力するので、ピッチ角θが±９０°付近となるような移動体であっても移動体の姿勢制御をより適切に行うことができる。本実施の形態の構成は、飛翔体又は潜水体に特に有効である。

慣性センサ１０によって検出されるピッチ角θが＋８５°よりも大きいか又は−８５°よりも小さい場合に、クオータニオン算出部１１がｑｘ_{（ψ＝０）}を算出し、このｑｘ_{（ψ＝０）}を用いてロール角算出手段１２がロール角φを算出するので、より確実に移動体の姿勢制御を適切に行うことができる。

また、姿勢制御手段１３は、ロール角算出手段１２によってロール角φが算出される場合に、慣性センサ１０によって検出されるロール角φを無効にして、ロール角算出手段１２によって算出されたロール角φを用いて移動体の姿勢制御を行うので、より確実に移動体の姿勢制御を行うことができる。

１０ 慣性センサ
１１ クオータニオン算出部
１２ ロール角算出手段
１３ 姿勢制御手段

Claims (3)

1. 移動体に取付けられ、前記移動体のヨー角ψ、ピッチ角θ及びロール角φを検出する慣性センサと、
   前記慣性センサによって検出されるヨー角ψ、ピッチ角θ及びロール角φに基づいて下記の式で表わされるクオータニオンＱ（ｑｘ，ｑｙ，ｑｚ，ｑｗ）を算出して出力するクオータニオン算出部と
   

   

   を備え、
   前記クオータニオン算出部は、前記慣性センサによって検出されるピッチ角θが＋８５°よりも大きいか又は−８５°よりも小さい場合に、下記の式で表わされるｑｘ _{（ψ＝０）} を算出するとともに該ｑｘ _{（ψ＝０）} をロール角算出手段に入力し、
   

   

   前記ロール角算出手段は、前記ｑｘ _{（ψ＝０）} を下記の式に代入してロール角φを算出する
   

   

   ことを特徴とする慣性装置。
2. 前記慣性センサ及び前記ロール角算出手段の出力は姿勢制御手段に入力され、
   前記姿勢制御手段は、前記ロール角算出手段によってロール角φが算出される場合に、前記慣性センサによって検出されるロール角φを無効にして、前記ロール角算出手段によって算出されるロール角φを用いて前記移動体の姿勢制御を行う
   ことを特徴とする請求項１記載の慣性装置。
3. 前記移動体は、飛翔体又は潜水体であることを特徴とする請求項１又は請求項２に記載の慣性装置。

Images