(57)【要約】【課題】  本発明は機上に搭載するセンサを最小限としながらも、小型軽量無人機の自動飛行制御に十分な検出精度の確保が可能な姿勢検出装置を提供することを目的とする。【解決手段】  機体の姿勢検出装置において、無人機の機体３次元位置情報としての緯度経度および高度を計測するためのＧＰＳ受信機と、機体３次元位置情報から機体の対地速度推定信号を発生するための対地速度推定器と、対地速度推定信号から機体の方位姿勢角を発生する方位角推定器と、対地速度推定信号から機体の対地加速度信号である対地速度変動推定信号を発生するための対地速度変動推定器と、機体に印可する加速度（重力加速度を含む）である機体３軸加速度信号を検出するため機体に搭載した３軸加速度計と、対地速度変動推定信号と機体３軸加速度信号とから機体の姿勢角であるロール角とピッチ角とを発生するＲｏｌｌ・Ｐｉｔｃｈ角推定器と、を有する。【選択図】  図１
2008230579.tif 000002
【特許請求の範囲】
【請求項１】
機体の姿勢検出装置において、
機体３次元位置情報としての緯度経度および高度を計測するためのＧＰＳ受信機と、
前記機体３次元位置情報から前記機体の対地速度推定信号を発生するための対地速度推定器と、
前記対地速度推定信号から前記機体の方位姿勢角を発生する方位角推定器と、
前記対地速度推定信号から前記機体の対地加速度信号である対地速度変動推定信号を発生するための対地速度変動推定器と、
前記機体に印可する加速度である機体３軸加速度信号を検出するため前記機体に搭載した３軸加速度計と、
前記対地速度変動推定信号と前記機体３軸加速度信号とから前記機体の姿勢角であるロール角とピッチ角とを発生するＲｏｌｌ・Ｐｉｔｃｈ角推定器と、
を有することを特徴とする姿勢検出装置。
【請求項２】
前記方位角推定器が、時系列的に連続する２つの前記対地速度（３軸）推定信号ベクトルＶnとＶn+1を用いた以下の式により前記機体の方位角を生成することを特徴とする請求項１記載の姿勢検出装置。
【数１】
2008230579.tif 000013
【請求項３】
前記Ｒｏｌｌ・Ｐｉｔｃｈ角推定器が、前記対地速度（３軸）変動推定信号と前記機体３軸加速度信号との差分信号ベクトルである
【数２】
2008230579.tif 000014
と重量加速度ｇ とからなる
【数３】
2008230579.tif 000015
式により前記機体のロール角とピッチ角を生成することを特徴とする請求項１または請求項２に記載の姿勢検出装置。
【請求項４】
機体の姿勢検出方法において、
ＧＰＳ受信機により機体３次元位置情報としての緯度経度および高度を計測し、
対地速度推定器が前記機体３次元位置情報から前記機体の対地速度推定信号を発生し、
方位角推定器が前記対地速度推定信号から前記機体の方位姿勢角を発生し、
対地速度変動推定器が前記対地速度推定信号から前記機体の対地加速度信号である対地速度変動推定信号を発生し、
３軸加速度計が前記機体に印可する加速度である機体３軸加速度信号を検出し、
Ｒｏｌｌ・Ｐｉｔｃｈ角推定器が前記対地速度変動推定信号と前記機体３軸加速度信号とから前記機体の姿勢角であるロール角とピッチ角とを発生することを特徴とする姿勢検出方法。
【請求項５】
前記方位角推定器が、時系列的に連続する２つの前記対地速度（３軸）推定信号ベクトルＶnとＶn+1を用いた以下の式により前記機体の方位角を生成することを特徴とする請求項４記載の姿勢検出方法。
【数４】
2008230579.tif 000016
【請求項６】
前記Ｒｏｌｌ・Ｐｉｔｃｈ角推定器が、前記対地速度（３軸）変動推定信号と前記機体３軸加速度信号との差分信号ベクトルである
【数５】
2008230579.tif 000017
と重量加速度ｇ とからなる
【数６】
2008230579.tif 000018
式により前記機体のロール角とピッチ角を生成することを特徴とする請求項４または請求項５に記載の姿勢検出方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、空中を飛行する飛行機や海上を航海する船舶に対する姿勢検出装置および方法に関するものである。
【背景技術】
【０００２】
図２は、特許文献１（特開２００４−１７７４３号公報）などに示された自動飛行制御装置の構成を示すブロック図である。
【０００３】
図２に示されるように、特許文献１に開示された自動飛行制御装置は、ジャイロ１０１、加速度計１０２、対気速度センサ１０３、気圧高度センサ１０４、磁方位センサ１０５、ＧＰＳ受信機１０６、飛行制御コントローラ１０７、モータ１０８、エレベータ１０９、ラダー１１０、エルロン１１１より構成されている。
【０００４】
飛行制御コントローラ１０７には、ジャイロ１０１の出力信号と加速度計１０２の出力信号とから算出した機体姿勢信号と、対気速度センサ１０３の出力信号である機体対気速度信号と、気圧高度センサ１０４の出力信号である機体高度信号と、磁方位センサ１０５の出力信号である機体方位角信号と、ＧＰＳ受信機１０６の出力信号である機体位置信号とが入力される。
【０００５】
飛行制御コントローラ１０７は、これら各種検出信号を用いて、機体位置姿勢を推定しながらＰＩＤ制御など汎用的な線形制御ロジックにより、モータ１０８、エレベータ１０９、ラダー１１０、エルロン１１１などの飛行制御用アクチュエータを駆動し、機体の飛行制御を実施する。
【０００６】
具体的にはモータ１０８で機体速度や機体高度を調整し、エレベータで機体高度や機体速度を調整し、ラダーで機体方位や機体位置を調整し、エルロンで機体方位や機体位置を調整する。
【０００７】
一方、特許文献２（特開２００６−３０６２５４号公報）に開示される飛行制御装置は、自動飛行制御装置内に対気速度センサが搭載されていない場合でも、機体加速度信号とモータ制御信号とに基づき、対気速度を推定しながら対地速度目標信号の発生を可能としている。
【特許文献１】特開２００４−１７７４３号公報
【特許文献２】特開２００６−３０６２５４号公報
【発明の開示】
【発明が解決しようとする課題】
【０００８】
図２に示すような自動飛行制御装置では、１０１〜１０６で示す６個のセンサを必ず機体上に搭載しなければならないため、機体の小型軽量化に対する制約条件になるという問題点がある。特許文献２に記載の発明では対気速度センサが不要とはなるものの、やはり多くのセンサが必要となっている。
【０００９】
また、飛行制御コントローラ１０７内でＰＩＤ制御器などの汎用的線形制御系を適用する限り、図中１０１〜１０６で示す６個の搭載センサによる信号計測精度がある程度確保できないと、機体の自動飛行制御が達成できないという問題点がある。
【００１０】
本発明は上述したような従来の技術が有する問題点に鑑みてなされたものであって、機上に搭載するセンサを最小限としながらも、小型軽量無人機の自動飛行制御に十分な検出精度の確保が可能な姿勢検出装置および方法を提供することを目的とする。
【課題を解決するための手段】
【００１１】
本発明は上記の目的を達成するため、本発明は、姿勢検出装置において、機体の緯度経度および高度（機体３次元位置情報）を計測するためのＧＰＳ受信機と、機体３次元位置情報から機体の対地速度（３軸）推定信号を発生するための対地速度（３軸）推定器と、対地速度（３軸）推定信号から機体の方位姿勢角（方位角）を発生する方位角推定器と、対地速度（３軸）推定信号から機体の対地加速度信号である対地速度（３軸）変動推定信号を発生するための対地速度（３軸）変動推定器と、機体に印可する加速度（重力加速度を含む）である機体３軸加速度信号を検出するため機体に搭載した３軸加速度計と、対地速度（３軸）変動推定信号と機体３軸加速度信号とから機体の姿勢角であるロール角とピッチ角とを発生するＲｏｌｌ・Ｐｉｔｃｈ角推定器と、を有する構成としたものである。本発明は、ＧＰＳ受信機の取得信号から推定した機体３軸の対地加速度ベクトルと３軸加速度計から取得できる重力効果を含む機体３軸対地加速度ベクトルとの比較により機体姿勢を計測可能としたもので、従来の小型無人飛行機と同等の構成に付加するだけでも容易に実現できる。
【００１２】
本発明では、機体に搭載するセンサ構成を最小化することで機体全体の小型軽量化を実現可能とすると同時に、センサノイズに対し、ロバスト性に優れた姿勢検出手法を実現することで、小型軽量無人機の姿勢検出が安定的に実現可能となる。
【発明の効果】
【００１３】
以上説明したように、本発明によれば、機体姿勢がＧＰＳ受信機と３軸加速度計を基本とする構成により導出可能となるため、機体の小型軽量化の実現が容易となるという特長を有するものである。
【発明を実施するための最良の形態】
【００１４】
次に、本発明の実施例について図面を参照して説明する。
【００１５】
図１は本発明による小型軽量無人飛行機の姿勢検出装置の一実施例の構成を示すブロック図である。この姿勢検出装置は、ＧＰＳ受信機１、３軸加速度計２、対地速度（３軸）推定器３、対地速度（３軸）変動推定器４、方位角推定器５、Ｒｏｌｌ・Ｐｉｔｃｈ角推定器６により構成することで、小型軽量無人機向けの安定的な姿勢検出が実施できる。
【００１６】
本発明の実施の形態に係る小型軽量無人機向け姿勢検出装置は図１に示すように、ＧＰＳ受信機１で取得する機体３次元位置情報７に基づき、対地速度（３軸）推定器３でその時間変化から対地速度（３軸）推定信号８を生成、さらに対地速度（３軸）変動推定器４により対地速度（３軸）推定信号８の時間変化値となる対地速度（３軸）変動推定信号９を生成すると共に、３軸加速度計２により検出する機体３軸加速度信号１０を取得することによって、機体の３軸姿勢状態を示すロール角、ピッチ角、および方位角をそれぞれＲｏｌｌ・Ｐｉｔｃｈ角推定器６と方位角推定器５により推定する。
【００１７】
次に、図１に示す本実施例の動作について、方位角推定器５とＲｏｌｌ・Ｐｉｔｃｈ角推定器６を中心に説明する。
【００１８】
小型軽量無人飛行機においてその姿勢状態は、一般の航空機同様、ロール角、ピッチ角、および方位角によって記述され、これらはそれぞれエルロン、エレベータ、およびラダーと呼ばれる舵面により制御される。しかし飛行中の機体では、その姿勢に対し基準となる状態量が容易に検出できないことから、通常はＧＰＳとジャイロ及び加速度計などから検出された値と推定アルゴリズムを適切に組み合わせ、姿勢情報を導出する必要がある。そのため特にジャイロによる姿勢角速度の検出精度が重要となるが、小型軽量なジャイロは検出精度に制約があることから、小型軽量ジャイロを小型無人機へ適用するには限界があり、これが機体の小型軽量化に対する制約条件となっている。
【００１９】
そこで図１に示すように本実施例では、ジャイロをあえて適用せず、ＧＰＳ受信機１と３軸加速度計２とから検出可能となる対地速度と対地加速度との関係を利用することで、機体姿勢状態を示すロール角、ピッチ角、および方位角を導出する。
【００２０】
まず方位角推定器５では、対地速度（３軸）推定器３により生成する対地速度（３軸）推定信号８のうち、時系列的に連続する２つの対地速度ベクトルＶnとＶn+1との内積から方位角推定値である
【００２１】
【数１】
2008230579.tif 000003
【００２２】
を（１）式を用いて導出する。
【００２３】
【数２】
2008230579.tif 000004
【００２４】
・・・(1)
一方、Ｒｏｌｌ・Ｐｉｔｃｈ角推定器６では、対地速度（３軸）変動推定器４により生成する対地速度（３軸）変動推定信号９をベクトル値ΔＶnとして表し、その変動時間間隔Δｔnから、まず加速度推定ベクトル
【００２５】
【数３】
2008230579.tif 000005
【００２６】
を（２）式により生成する。
【００２７】
【数４】
2008230579.tif 000006
【００２８】
・・・(2)
次に、３軸加速度計２によって検出した機体３軸加速度信号１０をａnとし，加速度推定ベクトルである
【００２９】
【数５】
2008230579.tif 000007
【００３０】
との差分を（３）式により生成する。
【００３１】
【数６】
2008230579.tif 000008
【００３２】
・・・(3)
すると（３）式で導出された加速度差分ａdは、機体に印可する重力加速度ｇの機体座標系におけるベクトル表現値と見なせる。
そこでこの差分ベクトルａdの成分を
【００３３】
【数７】
2008230579.tif 000009
【００３４】
・・・(4)
とおくと、機体のロール角推定値
【００３５】
【数８】
2008230579.tif 000010
【００３６】
およびピッチ角推定値
【００３７】
【数９】
2008230579.tif 000011
【００３８】
は（５）式によって決定できる。
【００３９】
【数１０】
2008230579.tif 000012
【００４０】
・・・(5)
これにより、本実施例によれば、ＧＰＳ受信機１と３軸加速度計２とを機体に搭載するだけで、機体姿勢の推定値が（１）式と（５）式から容易に導出することが可能となり、ジャイロや対気速度センサなどに依存する必要がなくなることから機体の小型軽量化が実現できる。特にここで（３）式で示した機体加速度の差分を機体姿勢検出に適用する点は独自の技術であり、特開２００６−３０６２５４号広報など従来の発明から容易に類推できるものではない。
【産業上の利用可能性】
【００４１】
なお、本発明は上記の実施例に限定されるものではなく、例えば空中を飛行する無人飛行船や海上を航海する船舶に対する姿勢検出装置などにも適用することができるものである。
【図面の簡単な説明】
【００４２】
【図１】本発明の一実施例のブロック図である。
【図２】従来方式の一例のブロック図である。
【符号の説明】
【００４３】
１    ＧＰＳ受信機
２    ３軸加速度計
３    対地速度（３軸）推定器
４    対地速度（３軸）変動推定器
５    方位角推定器
６    Ｒｏｌｌ・Ｐｉｔｃｈ角推定器
７    機体３次元位置情報
８    対地速度（３軸）推定信号
９    対地速度（３軸）変動推定信号
１０    機体３軸加速度信号
１０１    ジャイロ
１０２    加速度計
１０３    対気速度センサ
１０４    気圧高度センサ
１０５    磁方位センサ
１０６    ＧＰＳ受信機
１０７    飛行制御コントローラ
１０８    モータ
１０９    エレベータ
１１０    ラダー
１１１    エルロン
【図１】
2008230579.tif 000019
【図２】
2008230579.tif 000020