JP4946562B2

JP4946562B2 - 姿勢検出装置および方法

Info

Publication number: JP4946562B2
Application number: JP2007077361A
Authority: JP
Inventors: 英二仲森
Original assignee: NEC Corp
Current assignee: NEC Corp
Priority date: 2007-03-23
Filing date: 2007-03-23
Publication date: 2012-06-06
Anticipated expiration: 2027-03-23
Also published as: JP2008230579A

Description

本発明は、空中を飛行する飛行機や海上を航海する船舶に対する姿勢検出装置および方法に関するものである。

図２は、特許文献１（特開２００４−１７７４３号公報）などに示された自動飛行制御装置の構成を示すブロック図である。

図２に示されるように、特許文献１に開示された自動飛行制御装置は、ジャイロ１０１、加速度計１０２、対気速度センサ１０３、気圧高度センサ１０４、磁方位センサ１０５、ＧＰＳ受信機１０６、飛行制御コントローラ１０７、モータ１０８、エレベータ１０９、ラダー１１０、エルロン１１１より構成されている。

飛行制御コントローラ１０７には、ジャイロ１０１の出力信号と加速度計１０２の出力信号とから算出した機体姿勢信号と、対気速度センサ１０３の出力信号である機体対気速度信号と、気圧高度センサ１０４の出力信号である機体高度信号と、磁方位センサ１０５の出力信号である機体方位角信号と、ＧＰＳ受信機１０６の出力信号である機体位置信号とが入力される。

飛行制御コントローラ１０７は、これら各種検出信号を用いて、機体位置姿勢を推定しながらＰＩＤ制御など汎用的な線形制御ロジックにより、モータ１０８、エレベータ１０９、ラダー１１０、エルロン１１１などの飛行制御用アクチュエータを駆動し、機体の飛行制御を実施する。

具体的にはモータ１０８で機体速度や機体高度を調整し、エレベータで機体高度や機体速度を調整し、ラダーで機体方位や機体位置を調整し、エルロンで機体方位や機体位置を調整する。

一方、特許文献２（特開２００６−３０６２５４号公報）に開示される飛行制御装置は、自動飛行制御装置内に対気速度センサが搭載されていない場合でも、機体加速度信号とモータ制御信号とに基づき、対気速度を推定しながら対地速度目標信号の発生を可能としている。
特開２００４−１７７４３号公報特開２００６−３０６２５４号公報

図２に示すような自動飛行制御装置では、１０１〜１０６で示す６個のセンサを必ず機体上に搭載しなければならないため、機体の小型軽量化に対する制約条件になるという問題点がある。特許文献２に記載の発明では対気速度センサが不要とはなるものの、やはり多くのセンサが必要となっている。

また、飛行制御コントローラ１０７内でＰＩＤ制御器などの汎用的線形制御系を適用する限り、図中１０１〜１０６で示す６個の搭載センサによる信号計測精度がある程度確保できないと、機体の自動飛行制御が達成できないという問題点がある。

本発明は上述したような従来の技術が有する問題点に鑑みてなされたものであって、機上に搭載するセンサを最小限としながらも、小型軽量無人機の自動飛行制御に十分な検出精度の確保が可能な姿勢検出装置および方法を提供することを目的とする。

本発明は上記の目的を達成するため、本発明は、姿勢検出装置において、機体の緯度経度および高度（機体３次元位置情報）を計測するためのＧＰＳ受信機と、機体３次元位置情報から機体の対地速度（３軸）推定信号を発生するための対地速度（３軸）推定器と、対地速度（３軸）推定信号から機体の方位姿勢角（方位角）を発生する方位角推定器と、対地速度（３軸）推定信号から機体の対地加速度信号である対地速度（３軸）変動推定信号を発生するための対地速度（３軸）変動推定器と、機体に印可する加速度（重力加速度を含む）である機体３軸加速度信号を検出するため機体に搭載した３軸加速度計と、対地速度（３軸）変動推定信号と機体３軸加速度信号とから機体の姿勢角であるロール角とピッチ角とを発生するＲｏｌｌ・Ｐｉｔｃｈ角推定器と、を有する構成としたものである。本発明は、ＧＰＳ受信機の取得信号から推定した機体３軸の対地加速度ベクトルと３軸加速度計から取得できる重力効果を含む機体３軸対地加速度ベクトルとの比較により機体姿勢を計測可能としたもので、従来の小型無人飛行機と同等の構成に付加するだけでも容易に実現できる。

本発明では、機体に搭載するセンサ構成を最小化することで機体全体の小型軽量化を実現可能とすると同時に、センサノイズに対し、ロバスト性に優れた姿勢検出手法を実現することで、小型軽量無人機の姿勢検出が安定的に実現可能となる。

以上説明したように、本発明によれば、機体姿勢がＧＰＳ受信機と３軸加速度計を基本とする構成により導出可能となるため、機体の小型軽量化の実現が容易となるという特長を有するものである。

次に、本発明の実施例について図面を参照して説明する。

図１は本発明による小型軽量無人飛行機の姿勢検出装置の一実施例の構成を示すブロック図である。この姿勢検出装置は、ＧＰＳ受信機１、３軸加速度計２、対地速度（３軸）推定器３、対地速度（３軸）変動推定器４、方位角推定器５、Ｒｏｌｌ・Ｐｉｔｃｈ角推定器６により構成することで、小型軽量無人機向けの安定的な姿勢検出が実施できる。

本発明の実施の形態に係る小型軽量無人機向け姿勢検出装置は図１に示すように、ＧＰＳ受信機１で取得する機体３次元位置情報７に基づき、対地速度（３軸）推定器３でその時間変化から対地速度（３軸）推定信号８を生成、さらに対地速度（３軸）変動推定器４により対地速度（３軸）推定信号８の時間変化値となる対地速度（３軸）変動推定信号９を生成すると共に、３軸加速度計２により検出する機体３軸加速度信号１０を取得することによって、機体の３軸姿勢状態を示すロール角、ピッチ角、および方位角をそれぞれＲｏｌｌ・Ｐｉｔｃｈ角推定器６と方位角推定器５により推定する。

次に、図１に示す本実施例の動作について、方位角推定器５とＲｏｌｌ・Ｐｉｔｃｈ角推定器６を中心に説明する。

小型軽量無人飛行機においてその姿勢状態は、一般の航空機同様、ロール角、ピッチ角、および方位角によって記述され、これらはそれぞれエルロン、エレベータ、およびラダーと呼ばれる舵面により制御される。しかし飛行中の機体では、その姿勢に対し基準となる状態量が容易に検出できないことから、通常はＧＰＳとジャイロ及び加速度計などから検出された値と推定アルゴリズムを適切に組み合わせ、姿勢情報を導出する必要がある。そのため特にジャイロによる姿勢角速度の検出精度が重要となるが、小型軽量なジャイロは検出精度に制約があることから、小型軽量ジャイロを小型無人機へ適用するには限界があり、これが機体の小型軽量化に対する制約条件となっている。

そこで図１に示すように本実施例では、ジャイロをあえて適用せず、ＧＰＳ受信機１と３軸加速度計２とから検出可能となる対地速度と対地加速度との関係を利用することで、機体姿勢状態を示すロール角、ピッチ角、および方位角を導出する。

まず方位角推定器５では、対地速度（３軸）推定器３により生成する対地速度（３軸）推定信号８のうち、時系列的に連続する２つの対地速度ベクトルＶ_nとＶ_n+1との内積から方位角推定値である

を（１）式を用いて導出する。

・・・(1)
一方、Ｒｏｌｌ・Ｐｉｔｃｈ角推定器６では、対地速度（３軸）変動推定器４により生成する対地速度（３軸）変動推定信号９をベクトル値ΔＶ_nとして表し、その変動時間間隔Δｔ_nから、まず加速度推定ベクトル

を（２）式により生成する。

・・・(2)
次に、３軸加速度計２によって検出した機体３軸加速度信号１０をａ_nとし，加速度推定ベクトルである

との差分を（３）式により生成する。

・・・(3)
すると（３）式で導出された加速度差分ａ_dは、機体に印可する重力加速度ｇの機体座標系におけるベクトル表現値と見なせる。
そこでこの差分ベクトルａ_dの成分を

・・・(4)
とおくと、機体のロール角推定値

およびピッチ角推定値

は（５）式によって決定できる。

・・・(5)
これにより、本実施例によれば、ＧＰＳ受信機１と３軸加速度計２とを機体に搭載するだけで、機体姿勢の推定値が（１）式と（５）式から容易に導出することが可能となり、ジャイロや対気速度センサなどに依存する必要がなくなることから機体の小型軽量化が実現できる。特にここで（３）式で示した機体加速度の差分を機体姿勢検出に適用する点は独自の技術であり、特開２００６−３０６２５４号広報など従来の発明から容易に類推できるものではない。

なお、本発明は上記の実施例に限定されるものではなく、例えば空中を飛行する無人飛行船や海上を航海する船舶に対する姿勢検出装置などにも適用することができるものである。

本発明の一実施例のブロック図である。従来方式の一例のブロック図である。

符号の説明

１ＧＰＳ受信機
２３軸加速度計
３対地速度（３軸）推定器
４対地速度（３軸）変動推定器
５方位角推定器
６Ｒｏｌｌ・Ｐｉｔｃｈ角推定器
７機体３次元位置情報
８対地速度（３軸）推定信号
９対地速度（３軸）変動推定信号
１０機体３軸加速度信号
１０１ジャイロ
１０２加速度計
１０３対気速度センサ
１０４気圧高度センサ
１０５磁方位センサ
１０６ＧＰＳ受信機
１０７飛行制御コントローラ
１０８モータ
１０９エレベータ
１１０ラダー
１１１エルロン

Claims

機体の姿勢検出装置において、
機体３次元位置情報としての緯度経度および高度を計測するためのＧＰＳ受信機と、
前記機体３次元位置情報から前記機体の対地速度推定信号を発生するための対地速度推定器と、
前記対地速度推定信号から前記機体の方位姿勢角を発生する方位角推定器と、
前記対地速度推定信号から前記機体の対地加速度信号である対地速度変動推定信号を発生するための対地速度変動推定器と、
前記機体に印可する加速度である機体３軸加速度信号を検出するため前記機体に搭載した３軸加速度計と、
前記対地速度変動推定信号と前記機体３軸加速度信号とから前記機体の姿勢角であるロール角とピッチ角とを発生するＲｏｌｌ・Ｐｉｔｃｈ角推定器と、
を有し、
前記方位角推定器が、時系列的に連続する２つの前記対地速度（３軸）推定信号ベクトルＶ_nとＶ_n+1を用いた以下の式により前記機体の方位角を生成することを特徴とする姿勢検出装置。
前記Ｒｏｌｌ・Ｐｉｔｃｈ角推定器が、前記対地速度（３軸）変動推定信号と前記機体３軸加速度信号との差分信号ベクトルである

と重量加速度ｇとからなる

式により前記機体のロール角とピッチ角を生成することを特徴とする請求項１に記載の姿勢検出装置。
機体の姿勢検出方法において、
ＧＰＳ受信機により機体３次元位置情報としての緯度経度および高度を計測し、
対地速度推定器が前記機体３次元位置情報から前記機体の対地速度推定信号を発生し、
方位角推定器が前記対地速度推定信号から前記機体の方位姿勢角を発生し、
対地速度変動推定器が前記対地速度推定信号から前記機体の対地加速度信号である対地速度変動推定信号を発生し、
３軸加速度計が前記機体に印可する加速度である機体３軸加速度信号を検出し、
Ｒｏｌｌ・Ｐｉｔｃｈ角推定器が前記対地速度変動推定信号と前記機体３軸加速度信号とから前記機体の姿勢角であるロール角とピッチ角とを発生し、
前記方位角推定器が、時系列的に連続する２つの前記対地速度（３軸）推定信号ベクトルＶ_nとＶ_n+1を用いた以下の式により前記機体の方位角を生成することを特徴とする姿勢検出方法。
前記Ｒｏｌｌ・Ｐｉｔｃｈ角推定器が、前記対地速度（３軸）変動推定信号と前記機体３軸加速度信号との差分信号ベクトルである

と重量加速度ｇとからなる

式により前記機体のロール角とピッチ角を生成することを特徴とする請求項３に記載の姿勢検出方法。