JP2015024705A - 小型電動ヘリコプタの自動離着陸制御方法 - Google Patents

Abstract

【課題】離着陸時の小型電動ヘリコプタの高度、水平方向運動を安定化できる小型電動ヘリコプタの自動離着陸制御方法を提供する。
【解決手段】小型自動ヘリコプタの自動離着陸制御方法では、小型電動ヘリコプタに搭載した加速度センサの出力から定常カルマンフィルタ１１を用いて水平方向加速度を推定し、推定値を用いて、離着陸時の水平方向運動を安定化させる加速度フィードバック制御を行う。また、小型電動ヘリコプタに搭載した超音波センサの出力からカルマンフィルタを用いて対地高度と高度方向速度を推定し、これらの推定値を用いて離着陸時の高度制御を行う。
【選択図】図８

Description

本発明は、小型無人航空機である小型電動ヘリコプタの自動離着陸制御方法に関する。

先の１０年間において、無人航空機（Ｕｎｍａｎｎｅｄ Ａｅｒｉａｌ Ｖｅｈｉｃｌｅ：ＵＡＶ）技術が飛躍的な進歩を遂げており、研究開発の現場のみでなく、実際的なミッションにＵＡＶが適用されることが多くなっている。ＵＡＶは有人航空機に比べてコストや安全性、利便性に優れ、今後その応用範囲はさらに拡大するであろうと予測されている。

ＵＡＶを実際のミッションに適用する際、オペレータの負担軽減の観点から自律制御技術が必須であり、これを背景としてＵＡＶの自律制御に関する研究が世界中で行われてきている（非特許文献１、２）。また、近年では重量１０ｋｇ以下程度の小型ＵＡＶが運搬や取り扱いの容易さから大きな注目を集めており、自律制御の研究に関してもその研究対象は年々小型化の一途を辿っている（非特許文献３）。

本発明者等の研究グループでは、小型ＵＡＶの１種である小型無人ヘリコプタの自律制御に関する研究を行っており、片手で持ち運び可能な２ｋｇ程度の小型電動ヘリコプタの自律化を目指している。（なお、本明細書においては、重量が１０ｋｇ程度以下の無人電動ヘリコプタを「小型電動ヘリコプタ」と呼ぶものとする。）小型機の自律制御は中型以上の機体の制御よりはるかに困難である。何故なら、小型ゆえに外乱の影響を受けやすいということに加え、搭載可能重量の制限が厳しいため、制御に用いるセンサが著しく制限されてしまうためである。実際、２ｋｇの電動ヘリコプタに搭載可能なセンサは自律制御に用いる上で十分な精度を有していないという問題がある。

本発明者等による先行研究によって開発された小型電動ヘリコプタの自律制御システムでは、複数の低精度センサデータを統合することで高精度な機体姿勢、位置、および速度の推定を行い（複合航法システム）、得られた推定データを用いて、当該ヘリコプタの姿勢制御、ホバリング制御、誘導制御等を精度良く行うことに成功を収めている（非特許文献４、５）。

以下に、本発明に関連する先行技術文献（非特許文献１〜１２）を列記する。

Oh, C.K. and Barlow, G.J., " Autonomous controller design for unmanned aerial vehicles using multi-objective genetic programming, "in Proceedings of Congress on Evolytional Computation 2004, 2004, Vol. 2, pp. 1538-1545. Eric, N. Johnson, Suresh, K. Kannan,"Adaptive Trajectory Control for Autonomous Helicopters," in AIAA Journal of Guidance Control and Dynamics, Vol. 28, No. 3, pp. 524-538, 2005. Shaojie Shen, Nathan Michael, Vijay Kumar, "Autonomous Multi-Floor Indoor Navigation with a Computationally Constrained MAV, "in Proceedings of IEEE International Conference on Robotics and Automation 2011, TuA101.4, 2011. Satoshi Suzuki and Kenzo Nonami,"Nonlinear Adaptive Control for Small-Scale Helicopter, "in Journal of System Design and Dynamics, Vol. 5, No. 5, pp.866-880, 2011. Satoshi Suzuki, "Low Accuracy Sensor-based Navigation and Fully Autonomous Guidance Control of Small Electric Helicopter,"in Proceedings of IEEE International Conference onRobotics and Automation 2011, ThA101.4, 2011. R.M. Rogers, Applied Mathematics in Integrated Navigation System Second Edition, AIAA Education Series, 2003. 坂井丈泰, 惟村和宣, 新美賢治,"気圧高度計による高度測定誤差とその補正,"電子航法研究所研究報告, 第114 号, 2005. 野波健蔵, 西村秀和, MATLAB による制御理論の基礎, 東京電機大学出版局, 1998. Kenzo Nonami, Farid Kendoul, Satoshi Suzuki, Wei Wang and Daisuke Nakazawa, Autonomous Flying Robots Unmanned Aerial Vehicles and Micro Aerial Vehicles, Springer 2010. Jack B. Kuipers, Quaternions and Rotation Sequences, Princeton University Press, 2002. 田島洋, マルチボディダイナミクスの基礎-3 次元運動方程式の立て方, 東京電機大学出版局, 2006. 加藤寛一朗, 今永勇生, ヘリコプタ入門, 東京大学出版会, 1985.

上記のように、本発明者等の研究グループによって開発された自律制御システムにより、小型電動ヘリコプタの自律飛行を実現できる。しかしながら、ヘリコプタの離着陸の自動化はいまだに実現できていないのが現状である。離着陸を自動化できない最大の理由として、ヘリコプタに搭載しているＧＰＳと気圧計が離着陸時に誤差を持ってしまうことが挙げられる。

ＧＰＳに関しては、地上付近の樹木や建造物に電波が反射するマルチパスという現象によって測位データの誤差が増大してしまう。一方、気圧計に関しては、メインロータの吹き降ろしによって、ロータと地面間で空気が圧縮されるために気圧が上昇し、大気圧を基に推定している高度に大きな誤差が生じてしまう。このため、従来の自律制御システムを小型電動ヘリコプタの自動離着陸にそのまま適用することは困難であると考えられる。

本発明の課題は、離着陸時の小型電動ヘリコプタの高度および水平方向運動を安定化させることのできる、ＧＰＳと気圧計を用いない新たな制御方法を提案することにある。

上記の課題を解決するために、本発明の小型電動ヘリコプタの自動離着陸制御方法は、
小型電動ヘリコプタに加速度センサを搭載し、
前記加速度センサの出力から、定常カルマンフィルタを用いて、前記小型電動ヘリコプタの水平方向加速度を推定し、
推定した前記水平方向加速度を用いて、前記小型電動ヘリコプタの離着陸時の水平方向運動を安定化させるための加速度フィードバック制御を行い、
前記定常カルマンフィルタは以下の状態方程式を用いて設計されたものであり、

前記状態方程式において、
ａ_ｘ及びａ_ｙは機体前方向及び右方向加速度、
ｐ、ｑはそれぞれロール、ピッチ角速度、
ａ〜ｄはメインロータ及びスタビライザのフラップ角度、
Ｌ_ｂ及びＭ_ａは機体重心とロータヘッドの幾何学的配置やロータブレードの曲げ剛性等によって決まる適当な係数、
Ｉ_ｘｘ、Ｉ_ｙｙは各軸の慣性モーメント、
τ_ｂ及びτ_ｓはメインロータ及びスタビライザのフラッピング運動に関する時定数、
Ｋ_１〜Ｋ_３はロータまわりのリンク機構によって決まる係数、および、
δ_ａｉｌ、δ_ｅｌｅはエルロン及びエレベータ操舵入力を表す、
ことを特徴としている。

また、本発明の小型電動ヘリコプタの自動離着陸制御方法は、
前記小型電動ヘリコプタに、計測可能距離が少なくともメインロータの直径以上である超音波センサを搭載し、
前記超音波センサの出力から、カルマンフィルタを用いて、前記小型電動ヘリコプタの対地高度および高度方向速度を推定し、
推定した前記対地高度および高度方向速度を用いて離着陸時の高度制御を行い、
前記カルマンフィルタは以下の状態方程式を用いて設計したものであり、

前記状態方程式において、ｈ_ｇは対地高度、ｖ_ｄは鉛直下向きの速度、ｂ_ａは加速度センサ出力のバイアス誤差、ｒ_３は機体固定座標系から地上固定座標系への座標変換行列の行に対応したベクトル、ａ^ｂは加速度センサの出力ベクトル、ｇは重力加速度を表すことを特徴としている。

本発明の自動離着陸制御方法では、加速度フィードバック制御と超音波センサを用いた
高度制御により自動離着陸制御を行うようにしている。本発明によれば、自律制御に用いていたセンサが自動離着陸を行う際に大きな誤差を生じてしまうという問題点を解消できることが実験により確認された。よって、本発明の方法を用いることで、小型電動ヘリコプタの自動離着陸制御を含む完全自律制御系を構築することが可能になる。

本発明の制御対象である小型電動ヘリコプタの一例を示す画像である。 水平位置の測位データを示すグラフである。 高度の測位データを示すグラフである。 誘導制御系を示すブロック図である。 座標系を示す説明図である。 ホバリング制御実験の結果を示すグラフである。 複合航法システムを用いて推定した高度データを示すグラフである。 本発明による加速度フィードバック制御系を示すブロック図である。 ホバリング飛行実験の結果を示すグラフである。 本発明による対地高度の推定結果を示すグラフである。 本発明による高度方向速度の推定結果を示すグラフである。

図面を参照して、本発明を適用した小型電動ヘリコプタ（以下、単に「ヘリコプタ」と呼ぶ場合もある。）の自動離着陸制御方法の実施の形態を説明する。以下においては、先行研究で開発された小型電動ヘリコプタの自律制御システムの概要を述べた後、自動離着陸実現に向けた現状のシステムの問題点を明確にしたうえで、その問題点を解決するための制御系を説明する。具体的には、最初に、小型電動ヘリコプタをはじめとした本発明の対象の実験システムの一例を述べ、次に、先行研究で構築された複合航法システムと誘導制御系からなる自律制御システムの概要とその有効性について述べる。その次に、自動離着陸を実現する上での現状のシステムの問題点を述べ、その解決法となる本発明の制御系を設計し、実験によってその有効性を検証する。最後に結言を述べる。

［実験システムの例］
本発明の方法による制御対象である小型電動ヘリコプタは、例えば、Ｌｅｐｔｏｎ(登録商標）−Ｅｘ（商品名）であり、その全体像を図１に、主な諸元を表１（Ｔａｂｌｅ １）にそれぞれ示す。Ｌｅｐｔｏｎ−Ｅｘはホビー用に開発された高機動ヘリコプタであり、自重と比較して大きな推力を出力可能であることから、ホバリング等の静的飛行だけではなく、アクロバット飛行を行うこともできる。

小型電動ヘリコプタ（Ｌｅｐｔｏｎ−Ｅｘ）に搭載する制御装置の構成を表２（Ｔａｂｌｅ ２）に示す。制御装置はＦＰＧＡボード、無線モジュール、小型姿勢センサ、ＧＰＳモジュール、及び気圧計から構成されている。本制御装置の特徴として、２ｋｇの小型
電動ヘリコプタに搭載できるように数十グラム程度の軽量なセンサのみを用いていることが挙げられる。一般に、センサの精度と重量は比例関係にあり、軽量なセンサほどその精度は低い。特に、ヘリコプタの位置・速度計測に用いる小型ＧＰＳモジュールは小型ヘリコプタの自律制御に用いるのに十分な精度を有しておらず、複数のセンサデータを統合することで精度向上を図る必要がある。

［小型電動ヘリコプタの自律制御システム］
次に、本発明者等の研究グループで開発した小型電動ヘリコプタの自律制御システムの概要として、複合航法システムと誘導制御系の設計について述べる。

（複合航法システム）
空撮やリモートセンシング等に自律無人ヘリコプタを適用することを考えた場合、無風時におけるホバリング精度が少なくとも水平方向で０．５ｍ程度、鉛直方向で１ｍ程度であることが望ましい。よって、ヘリコプタの位置計測精度は少なくともそれ以上である必要がある。しかし、先に説明したとおり、位置・速度計測に用いる小型ＧＰＳモジュールは、この要求仕様に対して十分な精度を有していない。表３（Ｔａｂｌｅ ３）に真値との比較試験より得られた小型ＧＰＳモジュールの誤差を示す。表３より、水平・鉛直方向のデータともに大きな誤差を持っており、小型電動ヘリコプタの自律制御にそのまま用いることは困難であることがわかる。

このような小型ＧＰＳモジュールの弱点を補うため、慣性航法及び気圧高度計測の基礎方程式をプロセスモデルとした拡張カルマンフィルタを用いることによって誤差を補正する複合航法システムを構築する。

まず、システムの状態方程式と観測方程式を導出する。いま、ｖ_ｎ、ｖ_ｅ、ｖ_ｄをそれぞれ北向き、東向き、及び鉛直下向きの小型電動ヘリコプタの速度とすると、慣性航法の基礎方程式（非特許文献６）より、それらの時間微分は以下の式（１）〜（３）で表される。

ここで、ａ^ｂは機体に搭載された加速度センサの出力ベクトル、ｒ_１〜ｒ_３は機体固定座標系から地上固定座標系への座標変換行列の各行に対応したベクトル、Ωは地球の自転角速度、Ｌはヘリコプタの現在位置の緯度、ｇは重力加速度である。

一般に加速度センサ出力にはバイアス誤差が含まれており、これが慣性航法演算を行う際に大きな問題となる。そこで、加速度センサのバイアス誤差のダイナミクスを次式（４）で仮定して拡張カルマンフィルタに組み込むことで誤差推定を行い、これを補正する。

ここで、 ｗ_ｘ〜ｗ_ｚはホワイトノイズである。

いま、ｂ_ａ＝［ｂ_ａｘ；ｂ_ａｙ；ｂ_ａｚ］^Ｔというベクトル導入し、式（１）〜式（３）をバイアス誤差補正を含む方程式に書き換えると以下の式（５）〜（７）となる。

続いて、電動小型ヘリコプタの現在位置の緯度Ｌ、経度λ、高度ｈの時間微分は次式（８）〜（１０）で表すことができる。

ここで、Ｒ_ｎ、Ｒ_ｅはそれぞれ地球の南北方向と東西方向の極率半径を表している。

さらに、気圧高度計測（非特許文献７）に用いられる基礎方程式から大気圧Ｐの時間微分と鉛直方向速度との関係を以下の式（１１）で表すことができる。

ただし、Ｒは気体定数、Ｔは気温、Ｍは大気のモル質量をそれぞれ表している。

式（４）〜式（１１）をまとめると、以下に示す非線形状態方程式を得る。

つづいて、観測方程式を導出する。システムの観測量はＧＰＳより得ることができる緯度、経度、高度及び水平方向２軸の速度及び気圧計を用いて計測される大気圧であるため、出力ベクトルをｙ＝［Ｌ ｌ ｈ ｖ_ｎ ｖ_ｅ Ｐ］^Ｔとすれば、観測方程式は次式となる。

式中のｖは観測ノイズを表すベクトルである。

以上で得られたプロセスモデルに対して拡張カルマンフィルタアルゴリズムを適用することによって複合航法システムを構築し、検証実験を行った。検証実験の結果を図２及び図３に示す。この実験では、全ての機器を台車に搭載し、移動しながら位置・速度の推定を行った。また、同台車上にＲＴＫ−ＧＰＳを搭載し、ＲＴＫ−ＧＰＳの測位データを真値と扱っている。図２が水平位置の測位データ、図３が高度の測位データをそれぞれ示している。各図において、破線が小型ＧＰＳモジュールの測位データ、実線が航法システムで推定した推定値、鎖線がＲＴＫ−ＧＰＳを用いて取得した真値である。

各図より、小型ＧＰＳモジュールのデータは真値に対して水平方向で最大３ｍ程度、高度方向で最大１０ｍ程度の大きな誤差を持ち、前述した要求仕様を全く満たしていないのに対し、航法システムによって推定したデータは、水平方向、高度方向共に最大でも０．３ｍ程度の誤差にとどまる正確な位置推定が行えており、構築した複合航法システムの有効性を示す結果となっている。

（誘導制御系）
次に、複合航法システムで得られた推定データを用いて小型電動ヘリコプタを任意の目標点へと誘導する誘導制御系を構築する。図４に示すように、誘導制御系１は、小型電動ヘリコプタ２の誘導に必要な加速度ベクトルを生成する誘導制御器３、ロータ推力を用いて所望の加速度ベクトルを実現するためのロータコレクティブピッチ角及び目標機体姿勢を計算する推力生成器４、および計算された目標姿勢に機体姿勢を追従させる姿勢制御系５から構成される。誘導制御器３に、上記の複合航法システム６から推定データが入力さ
れる。姿勢制御系５は先行研究において既に構築及び実証がなされているため（非特許文献４）、以下では誘導制御器及び推力生成器の設計を行う。

はじめに、小型電動ヘリコプタの並進運動モデルを導出する。いま、３次元空間中の各軸方向の目標加速度をシステムの入力としたとき、入力から機体速度までのダイナミクスを、空気抵抗によるダンピングやインナーループの姿勢制御系の遅れ等を考慮して以下の式（１７）で表される３次の伝達関数で近似する。

ここで、伝達関数中のパラメータは各軸で全て同一としている。

つづいて、得られたモデルを用いて誘導制御器を設計する。まず、目標点と現在位置の偏差を用いて比例制御により計算された各軸の目標速度をｖ_ｎｄ、ｖ_ｅｄ、ｖ_ｄｄとしたとき、この目標速度に機体速度を追従させるための最適制御器を設計する。式（１７）の伝達関数の実現を求め、サーボ拡大系（非特許文献８）を構成し、拡大系に対して最適制御理論を適用して状態フィードバック制御器を設計した。いま、サーボ拡大系の状態量を
としたとき、状態フィードバック制御は次式（１８）で表される。

ただし、ａ_ｉｄは目標加速度、Ｆ_ｉは状態フィードバックゲイン、式中の添え字は北方向、東方向及び鉛直下方向の成分であることを示している。

また、ｅｉは次式（１９）で表される目標速度と機体速度の偏差の積分を表している。

ここで、実験時に直接観測できない状態量に関しては、同一次元オブザーバによって推定している。最後に、生成された各軸の目標加速度を用いて構成されるベクトル
ａ_ｄ＝［ａ_ｎｄ；ａ_ｅｄ；ａ_ｄｄ＋ｇ］^Ｔ
を考え、このベクトルで表される各軸の目標加速度を、ロータ推力の各軸成分を用いて実現する推力生成器を設計する。小型電動ヘリコプタの構造上、ロータ推力の大きさとその傾きを適当に設定することで、任意のａ_ｄを実現可能である。そのため、推力の大きさを決定するロータコレクティブピッチ角と、傾きを決定する機体姿勢を求めることで、ａ_ｄを実現できる。

いま、ロータコレクティブピッチ角をθ_ｐｉｔとしたとき、ロータ回転数が一定であるという仮定の下でθ_ｐｉｔとロータ推力の関係が線形である（非特許文献９）ことを利用すれば、ａ_ｄのノルムとそれを実現するためのロータコレクティブピッチ角の関係は、Ｋ_ｐｉｔを適当なゲインとして以下の式（２０）で表される。

続いて、目標機体姿勢を求める。先行研究（非特許文献４）において、小型電動ヘリコプタの姿勢表現にクォータニオン（非特許文献１０）を用いているため、ここでは目標機体姿勢を表すクォータニオンを計算する。計算を容易にするために、図５に示す座標系を定義する。まず、Ｎ− ｆｒａｍｅは小型電動ヘリコプタの重心に原点をもち、Ｚｎ軸を重力方向、Ｘｎ軸を真北方向にとった座標系である。続いて、Ｒ−ｆｒａｍｅはＮ−ｆｒａｍｅをＺｎ軸周りに目標方位角度ｙｄ［ｒａｄ］だけ回転させた座標系である。

Ｎ− ｆｒａｍｅに対するＲ−ｆｒａｍｅの姿勢を表すクォータニオンをｑ^ｒ _ｎとすると、これは次式（２１）で計算することができる。

ａ_ｄはＮ−ｆｒａｍｅ上のベクトルとして表現されており、これをＲ−ｆｒａｍｅ上に座標変換したものをａ^ｒ _ｄとすると、ａ_ｄとａ^ｒ _ｄの関係はｑ^ｒ _ｎを用いて次式（２２）で表される。

ここで、鉛直上方向きの参照ベクトルを次のように定義すれば、
Ｔ_ｄ＝［０ ０ −Ｋ_ｐｉｔ｜｜ａ_ｄ｜｜］^Ｔ
Ｔ_ｄによってａ_ｄを達成するために必要な回転姿勢を表すＳｉｍｐｌｅ Ｒｏｔａｔｉｏｎ（非特許文献１１）の回転軸λ_ｄ及び回転角度φ_ｄはＴ_ｄとａ^ｒ _ｄを用いて次式（２３）で計算できる。

また、このＳｉｍｐｌｅ Ｒｏｔａｔｉｏｎと同様の回転姿勢を表すクォータニオンｑ_ｒ ^ｄは次式（２４）となる。

ただしｓ（ｘ）=ｓｉｎｘ，ｃ（ｘ）=ｃｏｓｘであり、λｄｘ−λｄｚはλｄのＲ−ｆｒａｍｅにおける各軸成分を表している。

以上のｑ^ｄ _ｒとｑ^ｒ _ｎを用いることによって、ａ_ｄを実現するための目標姿勢を表すクォータニオンｑ_ｄは次式（２５）で計算することができる。

以上で得られた式（１８）、式（２０）、式（２５）及び先行研究の姿勢制御系を用いて行ったホバリング制御実験の結果を図６に示す。実験時はほぼ無風状態であり、地上高約５ｍの位置に設定した原点を目標点としてホバリング飛行を行った。図中の実線は約２分間の３次元飛行軌跡を示している。飛行軌跡の９割が原点を中心とした半径０．５ｍに収まっており、自律無人ヘリコプタ運用時の要求仕様を十分に満たす結果を得ることができた。

［自動離着陸制御］
次に、小型電動ヘリコプタの自動離着陸の実現へ向けた制御系設計を行う。上記のように、複合航法システムと誘導制御系を用いて小型電動ヘリコプタの自律飛行を実現した。しかし、離着陸の自動化はいまだに実現できていないのが現状である。離着陸を自動化できない最大の理由としては、先に述べたように、ヘリコプタに搭載しているＧＰＳと気圧計が離着陸時に誤差を持ってしまうことが挙げられる。ＧＰＳに関しては、地上付近の樹木や建造物に電波が反射するマルチパスという現象によって測位データの誤差が増大してしまうからである。気圧計に関しては、メインロータの吹き降ろしによってロータと地面間で空気が圧縮されるために気圧が上昇し、大気圧を基に推定している高度に大きな誤差が生じてしまうからである。

実際にヘリコプタが離陸する際に複合航法システムを用いて推定した高度データを図７に示す。図７の５〜１０秒付近において小型電動ヘリコプタが離陸をしているにも関わらず、上述した理由により高度データが減少していることがわかる。以上より、前述の自律制御システムを自動離着陸にそのまま適用することは困難であると考えられる。そのため、本発明では、ＧＰＳと気圧計を用いない新たな制御系を提案し、離着陸時のヘリコプタの高度及び水平方向運動を安定化することを目指す。

（加速度フィードバック制御）
ＧＰＳが使用できないため、小型電動ヘリコプタの水平方向運動の安定化に用いることができるのは加速度センサのみである。ここでは、加速度フィードバック制御を用いて小型電動ヘリコプタの水平方向運動を安定化する。

一般に、加速度センサの出力には大きなノイズが含まれており、直接フィードバック制御に用いることは難しい。そこで、小型電動ヘリコプタの運動モデルを基礎としたカルマンフィルタを用いて、水平方向加速度を推定する。まず、フィルタ設計に用いるプロセスモデルを導出する。モデルを単純化するために、以下に示す幾つかの仮定をおく。
（１）ヘリコプタのロール及びピッチ角度は十分小さく、ヨー角速度は安定化されており零とみなすことができる。
（２）メインロータ及びスタビライザのフラップ角度は十分小さい。
（３）メインロータの推力の大きさは、ホバリングなどの定常状態において自重に等しい。

まず第１の仮定に関しては、離着陸及びホバリング時の姿勢の傾きは微小であり、ヨー方向に関してはレートジャイロと呼ばれる安定化装置によって安定化されているため妥当な仮定であると考えられる。第２の仮定に関しては、そもそもロータとスタビライザのフラップ角度は最大でも数度程度であるのでやはり妥当であるといえる。第３の仮定に関し
ては自明である。

これらの仮定の下で、小型電動ヘリコプタに印加される水平方向の加速度は次式（２６）、（２７）で表される。
ここで、ａ_ｘ及びａ_ｙは機体前方向及び右方向加速度、Ｔ_ｍｒはメインロータ推力、ｍはヘリコプタの質量、ｆ及びｑはロール及びピッチ角度をそれぞれ表している。

つづいて、ロール及びピッチ角度と各方向の角速度の関係は次式（２８）、（２９）となる。
ここで、ｐ、ｑ、ｒはそれぞれロール、ピッチ、ヨー角速度である。

式（２６）〜式（２９）と先行研究（非特許文献９）において導出された小型無人ヘリコプタの回転運動モデルを組み合わせることによって、以下の状態方程式（３０）、（３１）を得ることができる。

ここで、ａ〜ｄはメインロータ及びスタビライザのフラップ角度、Ｌ_ｂ及びＭ_ａは機体重心とロータヘッドの幾何学的配置やロータブレードの曲げ剛性等によって決まる適当な係数、Ｉ_ｘｘ、Ｉ_ｙｙは各軸の慣性モーメント、τ_ｂ及びτ_ｓはメインロータ及びスタビライザのフラッピング運動に関する時定数、Ｋ_１〜Ｋ_３はロータまわりのリンク機構によって決まる係数、δ_ａｌｉ、δ_ｅｌｅはエルロン及びエレベータ操舵入力を表している。

ここで得られた状態方程式を用いて定常カルマンフィルタを設計した。そして、図８に示すように、定常カルマンフィルタ１１に、加速度の座標変換１２およびＰＩ制御器１３を組み合わせて、加速度フィードバック制御系１０を構成した。

加速度フィードバック制御系１０による加速度フィードバック制御のみを用いて行ったホバリング飛行実験の結果を図９に示す。図中の実線及び破線は実験時の水平速度を表しており、先に述べた自律航法システムを用いて取得している。図９より、水平速度を安定化することができており、設計した加速度フィードバックが小型電動ヘリコプタの水平運動の安定化にある程度有効であることが示された。

（超音波センサを用いた高度制御）
ＧＰＳと気圧計を自動離着陸時の高度計測に用いることは困難であるため、対地高度を計測するセンサが別途必要となる。必要なセンサの仕様として、十分に小型・軽量であることが挙げられる。また、気圧計の問題は一般にヘリコプタのロータ面と地面との距離がメインロータの直径以下のときに発生することが知られているため（非特許文献１２）、少なくともそれ以上の距離計測が可能でなければならない。今回の小型電動ヘリコプタの場合には１ｍ以上の計測可能距離が必要である。以上の仕様を考慮して、今回は超音波センサを選択した。選択した超音波センサの緒元を表４（Ｔａｂｌｅ ４）に示す。

選択した超音波センサは小型・軽量であり、計測可能距離も十分であることがわかる。しかしながら、超音波センサは音響ノイズの影響を受けやすいという特徴を持つため、小型電動ヘリコプタのロータ風切り音の影響で大きな計測誤差が生じてしまう。また、小型電動ヘリコプタの高度制御のためには高度データのみではなく、高度方向速度が必要となる。そのため、カルマンフィルタを用いて音響ノイズの影響を低減し、かつ高度方向速度の推定も行う。

カルマンフィルタの設計に用いる状態方程式を次式（３２）に示す。
ここで、ｈ_ｇは対地高度、ｂ_ａは加速度センサ出力のバイアス誤差を表している。

この状態方程式を用いてカルマンフィルタを設計し、対地高度及び高度方向速度の推定を行った。推定にはＧＰＳと気圧計の誤差が小さくなるのに十分な高度で小型電動ヘリコプタを飛行させた際のデータを用いている。推定結果を図１０及び図１１に示す。

まず、図１０に関して、実線がカルマンフィルタにより推定した対地高度を、破線が超音波センサの生データを表している。４０秒及び８０秒付近において、音響ノイズの影響で超音波センサのデータに大きな誤差が生じているが、推定値ではこのノイズの影響を低減することができている。図１１に関して、実線が高度方向速度の推定値を、破線が航法システムを用いて推定した高度速度をそれぞれ表している。２つの推定値がほぼ一致しており、設計したカルマンフィルタで正確な速度推定が行えていることが示された。

このように推定された対地高度及び高度方向速度と、先に述べた誘導制御系を用いることによって、小型電動ヘリコプタの自動離着陸時の高度制御を行うことが可能である。

［結言］
以上説明したように、小型電動ヘリコプタの自律化を目的として、自動離着陸制御を含む完全自律制御系の構築を行った。これまで自律制御に用いていたセンサが自動離着陸を行う際には大きな誤差を生じてしまうという問題点を解消するために加速度フィードバック制御と超音波センサを用いた高度制御を含む新たな自動離着陸制御方法を提案した。そして、実験によって、本発明による自動離着陸制御方法の有効性が確認された。

１ 誘導制御系
２ 小型電動ヘリコプタ
３ 誘導制御器
４ 推力生成器
５ 姿勢制御系
１０ 加速度フィードバック制御系
１１ 定常カルマンフィルタ
１２ 加速度の座標変換
１３ ＰＩ制御器

Claims (2)

1. 小型電動ヘリコプタに加速度センサを搭載し、
   前記加速度センサの出力から、定常カルマンフィルタを用いて、前記小型電動ヘリコプタの水平方向加速度を推定し、
   推定した前記水平方向加速度を用いて、前記小型電動ヘリコプタの離着陸時の水平方向運動を安定化させるための加速度フィードバック制御を行い、
   前記定常カルマンフィルタは以下の状態方程式を用いて設計されたものであり、
   前記状態方程式において、
   ａ_ｘ及びａ_ｙは機体前方向及び右方向加速度、
   ｐ、ｑはそれぞれロール、ピッチ角速度、
   ａ〜ｄはメインロータ及びスタビライザのフラップ角度、
   Ｌ_ｂ及びＭ_ａは機体重心とロータヘッドの幾何学的配置やロータブレードの曲げ剛性等によって決まる適当な係数、
   Ｉ_ｘｘ、Ｉ_ｙｙは各軸の慣性モーメント、
   τ_ｂ及びτ_ｓはメインロータ及びスタビライザのフラッピング運動に関する時定数、
   Ｋ_１〜Ｋ_３はロータまわりのリンク機構によって決まる係数、および、
   δ_ａｉｌ、δ_ｅｌｅはエルロン及びエレベータ操舵入力を表す、
   ことを特徴とする小型電動ヘリコプタの自動離着陸制御方法。
2. 前記小型電動ヘリコプタに、計測可能距離が少なくともメインロータの直径以上である超音波センサを搭載し、
   前記超音波センサの出力から、カルマンフィルタを用いて、前記小型電動ヘリコプタの対地高度および高度方向速度を推定し、
   推定した前記対地高度および高度方向速度を用いて離着陸時の高度制御を行い、
   前記カルマンフィルタは以下の状態方程式を用いて設計したものであり、
   前記状態方程式において、ｈ_ｇは対地高度、ｖ_ｄは鉛直下向きの速度、ｂ_ａは加速度センサ出力のバイアス誤差、ｒ_３は機体固定座標系から地上固定座標系への座標変換行列の行に対応したベクトル、ａ^ｂは加速度センサの出力ベクトル、ｇは重力加速度を表す、
   請求項１に記載の小型電動ヘリコプタの自動離着陸制御方法。