JP7482757B2 - 航空機の位置制御システム、航空機、及び航空機の位置制御方法 - Google Patents

Description

本開示は、航空機の位置制御システム、航空機、及び航空機の位置制御方法に関するものであり、特に、航空機の空間安定ホバリングのための位置制御に関するものとなっている。

従来、機体姿勢変化時に生じるビーム長変化速度に起因するドプラ周波数偏移による速度誤差の影響を低減し、良好な精度で位置情報を出力する機能を付加したドプラ航法装置が知られている（例えば、特許文献１参照）。このドプラ航法装置では、ビーム長変化により生じる速度誤差を補正する補正量を求めている。

特開平１１－１８３６０３号公報

特許文献１では、航空機から地表面へ向けて電波を送信するドプラ航法装置を用いている。ドプラ航法装置では、速度誤差を補正して機体の速度を計測している。ドプラ航法装置において計測した機体の速度は、機体の位置を飛行制御するために用いられる。飛行制御において、補正後の機体の速度を用いることができれば、目標着地点から位置がずれにくいものにすることができる。

一方で、機体の位置を飛行制御するために、機体の加速度を計測し、計測した加速度に基づく飛行制御を行うことが知られている。

航空機を目標座標に安定的にホバリングさせる場合、入力情報として、加速度計により計測した機体の加速度を用いて、航空機の飛行制御を行う。この場合、加速度計により取得した加速度の出力値には、ドリフト現象によるドリフト成分が含まれることがある。ドリフト成分を含む加速度の出力値を用いて、航空機をホバリングさせると、精度の良い加速度を得ることが困難であることから、本来の目標着地点から位置がずれてしまい、目標着地点上の高精度なホバリングが困難となる。

そこで、本開示は、空間上における航空機の位置を安定的に保持させることができる航空機の位置制御システム、航空機、及び航空機の位置制御方法を提供することを課題とする。

本開示の航空機の位置制御システムは、動揺する目標着地点に対して、空間上の目標座標に航空機を保持させる航空機の位置制御システムであって、前記航空機の加速度と前記航空機の姿勢とに基づいて、前記航空機の加速度を補正した第１姿勢補正加速度を出力する加速度補正処理部と、前記第１姿勢補正加速度と前記航空機の慣性速度とに基づいて、前記第１姿勢補正加速度に含まれるドリフト成分を除去した第２姿勢補正加速度を出力する相補フィルタと、前記第２姿勢補正加速度と、前記航空機と前記目標着地点との間の相対座標とに基づいて、前記相対座標を平滑化した平滑化相対座標を出力する平滑化処理部と、を備える。

本開示の航空機は、航空機の加速度を取得する加速度取得部と、前記航空機の姿勢を取得する姿勢取得部と、前記航空機の慣性速度を取得する慣性速度取得部と、前記航空機の相対座標を取得する相対座標取得部と、上記の航空機の位置制御システムと、を備える。

本開示の航空機の位置制御方法は、動揺する目標着地点に対して、空間上の所定座標に航空機を保持させる航空機の位置制御方法であって、前記航空機の加速度と前記航空機の姿勢とに基づいて、前記航空機の加速度を補正した第１姿勢補正加速度を出力するステップと、前記第１姿勢補正加速度と前記航空機の慣性速度とに基づいて、前記第１姿勢補正加速度に含まれるドリフト成分を除去した第２姿勢補正加速度を出力するステップと、前記第２姿勢補正加速度と、前記航空機と前記目標着地点との間の相対座標とに基づいて、前記相対座標を平滑化した平滑化相対座標を出力するステップと、を備える。

本開示によれば、目標着地点が動揺する場合であっても、その影響を受けず、目標着地点の動揺の平均的な位置を、航空機側で得られる情報のみで、安定的に保持させることができる。

図１は、本実施形態にかかる航空機の位置制御システムの一例を示す概略構成図である。 図２は、本実施形態にかかる航空機が目標着地点に向かう様子を示す説明図である。 図３は、目標着地点に設けられるマーカーの一例を示す説明図である。 図４は、平滑化された航空機の相対高度を算出する一例のブロック図である。 図５は、平滑化された航空機の相対位置を算出する一例のブロック図である。 図６は、相補フィルタでの処理の一例を示すブロック図である。 図７は、本実施形態にかかる航空機の位置制御方法に関する一例を示すフローチャートである。 図８は、本実施形態にかかる航空機の位置制御に関する一例の説明図である。 図９は、平滑化処理部の相対高度から平滑化相対高度までのボード線図である。 図１０は、平滑化処理部の相対位置から平滑化相対位置までのボード線図である。

以下に、本開示に係る実施形態を図面に基づいて詳細に説明する。なお、この実施形態によりこの発明が限定されるものではない。また、下記実施形態における構成要素には、当業者が置換可能かつ容易なもの、あるいは実質的に同一のものが含まれる。さらに、以下に記載した構成要素は適宜組み合わせることが可能であり、また、実施形態が複数ある場合には、各実施形態を組み合わせることも可能である。

［実施形態］
図１は、本実施形態にかかる航空機の位置制御システムの一例を示す概略構成図であり、図２は、本実施形態にかかる航空機が目標着地点に向かう様子を示す説明図である。

図１に示すように、航空機１は、回転翼機としての飛行体（例えばヘリコプタ、ドローン等）である。本実施形態において、航空機１は、無人機である。なお、航空機１は、前進、後進、横進、旋回、ホバリングが可能な飛行体であればよく、有人機であってもよい。航空機１は、位置制御システム１００を搭載しており、位置制御システム１００により飛行が制御され、図２に示す目標着地点２に着地する。

（目標着地点）
本実施形態において、目標着地点２は、図２に示すように、船舶５上に設けられている。このため、航空機１は、水上を移動する移動体としての船舶５に着地（着艦）する。なお、船舶５には、図示省略するが、目標着地点２に航空機１を着地させた際に、航空機１を拘束するための拘束装置が設けられている。ただし、目標着地点２は、船舶５に限らず、地上を移動する移動体としての車両等に設けられてもよいし、移動しない設備、地面に設けられてもよい。

目標着地点２には、航空機１が目標着地点２の位置を捕捉するためのマーカー７が設けられている。図３は、目標着地点に設けられるマーカーの一例を示す説明図である。図示するように、マーカー７は、例えば白黒の２色で色分けされたＡＲマーカーであり、正方形状のマーカーである。なお、マーカー７は、ＡＲマーカーに限らず、画像処理により目標着地点２の位置を捕捉することができるマーカーであればよく、例えばヘリポートの着地点を示すＨマーク、Ｒマーク等であってもよい。また、マーカー７は、船舶５に異なる形状のマーカーが複数設けられてもよく、航空機１は、異なるマーカー７のいずれかに対応した目標着地点２に誘導されるものであってもよい。また、本実施形態では、目標着地点２の位置を捕捉するために、船舶５にマーカー７を設けたが、目標着地点２の位置を取得可能な構成であれば、特に限定されない。

（位置制御システム）
本実施形態にかかる航空機の位置制御システム１００は、飛行中の航空機１を目標着地点２に着地させるために、航空機１の位置を制御するシステムである。位置制御システム１００は、航空機１に搭載される。位置制御システム１００は、図１に示すように、カメラ１０と、航法装置２０と、制御部３０とを備える。

カメラ１０は、航空機１に図示しないジンバルを介して搭載された撮影装置である。カメラ１０は、マーカー７を撮影することができれば、単眼カメラ、複眼カメラ、赤外線カメラ等であってもよい。カメラ１０は、航空機１から目標着地点２に設けられたマーカー７を撮影するために設けられる。カメラ１０は、図示しないジンバルを介して撮影方向を調整可能とされている。本実施形態において、カメラ１０は、その撮影範囲Ｂ（図２参照）が、一例として、鉛直方向の真下を向くように制御部３０によって制御される。なお、カメラ１０は、撮影範囲Ｂが、鉛直方向に対して斜め前方側を向くように制御部３０によって制御されてもよい。また、カメラ１０は、ジンバルを省いてもよく、撮影方向が鉛直方向の下方側を向くように、航空機１の機体直下に固定してもよい。

航法装置２０は、例えば、慣性航法装置（ＩＮＳ：Ｉｎｅｒｔｉａｌ Ｎａｖｉｇａｔｉｏｎ Ｓｙｓｔｅｍ）である。なお、本実施形態において、航法装置２０は、慣性航法装置に適用して説明するが、特に限定されず、いずれの航法装置２０を用いてもよい。また、航法装置２０は、位置の計測精度を向上させるために、ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）を含んだ慣性航法装置としてもよい。本実施形態では、ＧＰＳを含んだ慣性航法装置に適用して説明するが、ＧＰＳに特に限定されず、精度よく位置を計測可能な位置計測部であればよく、例えば、準天頂衛星システムを用いたものであってもよいし、航法装置２０のみで精度よく位置を計測可能であれば、ＧＰＳ等の位置計測部を省いた構成であってもよい。

ＧＰＳを含んだ航法装置２０は、航空機１のロール方向、ヨー方向およびピッチ方向の姿勢角、航空機１の機体速度、慣性速度、機体加速度、機首方位および地球座標系における位置座標等を取得する。なお、航法装置２０は、航空機１の姿勢角を検出する姿勢角センサ、航空機１の機体速度を検出する速度センサ、航空機１の機体加速度を検出する加速度センサ、航空機１の機首方位を検出するセンサを有するものであってもよい。航法装置２０は、取得した航空機１の姿勢角、機体速度、慣性速度、機体加速度、機首方位および位置座標を、制御部３０に出力する。このように、航法装置２０は、航空機１の加速度を取得する加速度取得部、航空機１の姿勢を取得する姿勢取得部、航空機１の慣性速度を取得する慣性速度取得部として機能している。

また、位置制御システム１００は、図１に示すように、航空機１の地表面または水面からの高度を検出する高度センサ２５を備えている。高度センサ２５は、例えば、レーザ高度計であり、航空機１から目標着地点２までの相対高度Δｈ（図２参照）を計測している。なお、高度センサ２５としては、電波高度計を用いてもよいし、気圧高度計を用いてもよく、いずれの高度計を用いてもよい。また、これらの高度計を、使用環境に応じて、すなわち地表面からの高度、海面からの高度を計測するために、適宜組み合わせて適用してもよい。高度センサ２５は、検出した航空機１の相対高度Δｈを制御部３０に出力する。なお、高度センサ２５は、航空機１の高度を計測して制御部３０に出力し、制御部３０は、後述する誘導演算部３４において、航空機１の高度に基づいて、目標着地点２までの相対高度Δｈを算出するものであってもよい。また、位置制御システム１００は、高度センサ２５に限らず、後述する画像処理部３２において、カメラ１０で撮影したマーカー７を含む画像に画像処理を施すことで、航空機１と船舶５との相対高度Δｈを算出するものであってもよい。

（制御部）
制御部３０は、画像処理部３２と、誘導演算部３４と、飛行制御部３６とを有する。なお、制御部３０は、航空機１に設けられた図示しないジンバルを介して、カメラ１０の撮影方向を制御する図示しない撮影制御部を備えている。本実施形態では、上述したように、カメラ１０の撮影範囲Ｂが鉛直方向の真下を向くように調整される。

（画像処理部）
画像処理部３２は、カメラ１０で撮影された画像に画像処理を施して、マーカー７すなわち目標着地点２の中心（Ｃｘ，Ｃｙ）（図３参照）を算出する。ここでの中心（Ｃｘ，Ｃｙ）は、カメラ１０で撮影された画像の中心を原点とするカメラ固定座標系における座標点であり、画像中心からの画素数により算出することができる。具体的には、画像処理部３２は、図３に示すように、画像処理によってマーカー７の角部同士の間を延びる対角線Ｌｄを２つ特定し、特定した２つの対角線Ｌｄの交点をマーカー７の中心（Ｃｘ，Ｃｙ）とする。なお、目標着地点２は、マーカー７の中心（Ｃｘ，Ｃｙ）に限定されず、マーカー７の四隅のいずれかであってもよいし、マーカー７の中心からオフセットした位置であってもよい。画像処理部３２は、算出したマーカー７の中心（Ｃｘ，Ｃｙ）を誘導演算部３４に出力する。

また、画像処理部３２は、カメラ１０で撮影したマーカー７を含む画像に画像処理を施すことで、マーカー７の向きを特定し、航法装置２０で取得される航空機１の機首方位と対応づけることで、船舶５の船首方位を算出してもよい。なお、画像処理部３２は、上述したように、カメラ１０で撮影したマーカー７を含む画像に画像処理を施すことで、航空機１と船舶５との相対高度Δｈを算出するものであってもよい。

（誘導演算部）
誘導演算部３４は、航空機１を目標着地点２に誘導するための航空機１の制御量を算出する。制御量は、航空機１の機体速度、姿勢角、姿勢角の変化レート等を調整するための制御量である。誘導演算部３４は、制御量を算出するために、航空機１と目標着地点２との相対座標を算出する。具体的に、誘導演算部３４は、相対座標として、航空機１と目標着地点２との相対位置（Ｘ，Ｙ）、航空機１と目標着地点２との相対高度Δｈおよび航空機１と目標着地点２との相対速度等を算出する。相対位置（Ｘ，Ｙ）は、水平方向における航空機１と目標着地点２との距離となる。相対高度Δｈは、鉛直方向における航空機１と目標着地点２との距離となる。

誘導演算部３４は、画像処理部３２で算出されたマーカー７の中心（Ｃｘ，Ｃｙ）と、カメラ１０の方位すなわち航空機１の機首方位と、航空機１の高度（目標着地点２に対する相対高度Δｈ）とに基づいて、航空機１と目標着地点２との相対位置（Ｘ，Ｙ）を算出する。なお、本実施形態では、カメラ１０の方位と航空機１の機首方位とを一致させているが、特に限定されず、カメラ１０の方位と航空機１の機首方位とを一致させなくてもよい。このように、画像処理部３２および誘導演算部３４は、航空機１と目標着地点２との相対位置を取得する相対位置取得部（相対座標取得部）として機能する。

また、誘導演算部３４は、高度センサ２５で検出された航空機１の高度に基づいて、目標着地点２までの相対高度Δｈを算出する。したがって、高度センサ２５および誘導演算部３４は、航空機１と目標着地点２との相対高度Δｈを取得する相対高度取得部（相対座標取得部）として機能する。なお、画像処理部３２において、カメラ１０で撮影したマーカー７を含む画像に画像処理を施すことで、航空機１と船舶５との相対高度Δｈを算出する場合は、画像処理部３２が相対高度取得部となる。

また、誘導演算部３４は、航空機１と目標着地点２との相対速度を算出する。したがって、誘導演算部３４は、航空機１と目標着地点２との相対速度を取得する相対速度取得部として機能する。より詳細には、誘導演算部３４は、相対位置（Ｘ，Ｙ）と機体速度（Ｖｘ、Ｖｙ）とに基づいて、航空機１と目標着地点２との相対速度（ΔＶｘ，ΔＶｙ）を算出する相対速度演算処理を実行する。したがって、誘導演算部３４は、航空機１と目標着地点２との相対速度（ΔＶｘ，ΔＶｙ）を取得する相対速度取得部として機能する。

そして、誘導演算部３４は、相対位置（Ｘ，Ｙ）、相対高度Δｈ、相対速度（ΔＶｘ，ΔＶｙ）および機体加速度に基づいて、フィードバック制御（例えばＰＩＤ制御）により制御量を算出する。なお、フィードバック制御は、ＰＩＤ制御に限らず、Ｐ制御、ＰＩ制御、ＰＤ制御等であってもよい。誘導演算部３４は、算出した制御量Ｃ´（図４、図５参照）を飛行制御部３６に出力する。

（飛行制御部）
飛行制御部３６は、誘導演算部３４で算出された制御量にしたがって、航空機１の各構成要素を制御して航空機１を飛行させる。飛行制御部３６は、制御量にしたがって各回転翼のブレードピッチ角、回転数等を制御し、航空機１の機体速度、姿勢角、姿勢角の変化レート等を調整する。それにより、航空機１は、目標着地点２へと誘導される。なお、本実施形態では、画像処理部３２および誘導演算部３４を飛行制御部３６とは別の機能部として説明するが、飛行制御部３６、画像処理部３２および誘導演算部３４は、一体の機能部であってもよい。すなわち、飛行制御部３６において画像処理部３２および誘導演算部３４の処理を行ってもよい。

（航空機の位置制御）
次に、図４及び図５を参照して、本実施形態にかかる航空機１の位置制御について説明する。航空機１の位置制御では、空間上において安定的にホバリングを実行する空間安定ホバリングを実行している。ここで、図８を参照して、空間安定ホバリングについて説明する。図８は、本実施形態にかかる航空機の位置制御に関する一例の説明図である。図８のラインＬ１は、波等の動揺により経時的に変化する目標着地点２の位置である。図８に示すように、空間安定ホバリングは、航空機１と目標着地点２との相対高度Δｈ及び相対位置（Ｘ，Ｙ）が、波等の動揺により経時的に変化する場合であっても、航空機１と、動揺による変位量を平滑化(平均化)した目標着地点２との相対高度Δｈ及び相対位置（Ｘ，Ｙ）が、目標相対高度及び目標相対位置と一致するように、ホバリングを実行する飛行制御である。相対位置（Ｘ，Ｙ）及び相対高度Δｈを、高度センサ２５等を用いて計測する場合、お互いの運動を含んだ瞬時値となるため、動揺などによる変化も含まれてしまう。そのため、動揺などによる変化を取り除く平滑化処理が必要となり、その平滑化された位置／高度に対して制御を行う必要がある。つまり、空間安定ホバリングでは、平滑化された（平均的な）相対高度Δｈと目標相対高度との差分がゼロとなるように、航空機１が飛行制御され、また、平滑化された（平均的な）相対位置（Ｘ，Ｙ）と目標相対位置との差分がゼロとなるように、航空機１が飛行制御される。

図４は、平滑化された航空機の相対高度を算出する一例のブロック図である。図５は、平滑化された航空機の相対位置を算出する一例のブロック図である。航空機１の位置制御では、誘導演算部３４が、図４及び図５に示すブロック図に基づいて、目標着地点２に対して、空間上の目標座標に航空機を保持させる空間安定ホバリングに関する位置制御を行っている。このため、誘導演算部３４は、空間安定ホバリングを実行するための航空機１の制御量を算出している。

（相対高度に関する空間安定ホバリング）
図４では、平滑化相対高度と目標相対高度との差分がゼロとなるように、空間安定ホバリングに関する位置制御を実行している。誘導演算部３４は、加速度補正処理部４１と、相補フィルタ４２と、平滑化処理部４３と、フィードバック制御部４４とを備えている。なお、加速度補正処理部４１、相補フィルタ４２及び平滑化処理部４３は、誘導演算部３４によって実現されていてもよいし、誘導演算部３４とは別体の処理部によって実現されていてもよいし、それらの組み合わせによって実現されていてもよく、特に限定されない。

加速度補正処理部４１は、航空機１の加速度と航空機１の姿勢とに基づいて、航空機１の加速度を補正した第１姿勢補正加速度を出力している。姿勢補正加速度とは、機体軸座標系の加速度を、航空機１の姿勢角に基づいて座標変換を行い、慣性空間座標系の加速度に変換したものである。具体的に、加速度補正処理部４１には、航法装置２０において取得された航空機１の加速度が入力され、また、航法装置２０において取得された航空機１の姿勢角が入力される。入力される加速度としては、縦方向（機体座標系における前後方向）、横方向（機体座標系における左右方向）、及び垂直方向（機体座標系における上下方向）の加速度である。また、入力される姿勢角としては、ピッチ軸、ロール軸及びヨー軸における姿勢角である。加速度補正処理部４１は、航空機１の加速度と航空機１の姿勢角とが入力されると、垂直方向における航空機１の加速度を補正した第１姿勢補正加速度を算出する。加速度補正処理部４１は、算出した第１姿勢補正加速度を相補フィルタ４２へ向けて出力する。

相補フィルタ４２は、第１姿勢補正加速度と航空機１の慣性速度とに基づいて、第１姿勢補正加速度に含まれるドリフト成分を除去した第２姿勢補正加速度を出力している。具体的に、相補フィルタ４２には、加速度補正処理部４１から出力された第１姿勢補正加速度が入力され、また、航法装置２０において取得された垂直方向における航空機１の慣性速度が入力される。入力される慣性速度は、航空機１を原点とした航空機慣性系における速度である。相補フィルタ４２は、第１姿勢補正加速度と航空機１の慣性速度とが入力されると、第１姿勢補正加速度の出力値に含まれる低周波成分を除去した高周波成分と、慣性速度の微分値に含まれる低周波成分とを合成して、垂直方向における第２姿勢補正加速度を出力している。

図６は、相補フィルタでの処理の一例を示すブロック図である。図６に示すように、相補フィルタ４２は、擬似微分フィルタ５１と、ローパスフィルタ５２と、ハイパスフィルタ５３とを含んでいる。擬似微分フィルタ５１は、慣性速度に擬似微分を施して、加速度成分とするフィルタである。擬似微分フィルタ５１の伝達関数Ｇ１（Ｓ）は、下記の式（１）で表される。式（１）中の“ｓ”は、演算子であり、“τ１”は、時定数である。

Ｇ１（Ｓ）＝ｓ／（τ１・ｓ＋１） …（１）

ローパスフィルタ５２は、擬似微分フィルタ５１から入力される慣性速度の微分値に対し、所定のカットオフ周波数以上の周波数を減衰させるフィルタである。ローパスフィルタ５２の伝達関数Ｇ２（ｓ）は、式（２）により表される。式（２）中の“ｓ”は、演算子であり、“τ２”は、時定数である。したがって、所定のカットオフ周波数は、“１／τ２”となる。これにより、慣性速度の微分値をローパスフィルタ５２に通すことで、慣性速度の微分値に含まれる低周波成分を得ることができる。

Ｇ２（Ｓ）＝１／（τ２・ｓ＋１） …（２）

ハイパスフィルタ５３は、第１姿勢補正加速度に対し、所定のカットオフ周波数未満の周波数を減衰させるフィルタである。ハイパスフィルタ５３の伝達関数Ｇ３（ｓ）は、式（３）により表される。式（３）中の“ｓ”は、演算子であり、“τ２”は、ローパスフィルタ５２と共通の時定数である。したがって、ハイパスフィルタ５３においても、所定のカットオフ周波数は、“１／τ２”となる。これにより、垂直方向における第１姿勢補正加速度をハイパスフィルタ５３に通すことで、第１姿勢補正加速度に含まれる高周波成分を得ることができる。

Ｇ３（Ｓ）＝τ２・ｓ／（τ２・ｓ＋１） …（３）

そして、相補フィルタ４２は、ローパスフィルタ５２から出力された慣性速度の微分値に含まれる低周波成分と、ハイパスフィルタ５３から出力された第１姿勢補正加速度に含まれる高周波成分とを合成して、第２姿勢補正加速度として算出する。これにより、相補フィルタ４２は、第１姿勢補正加速度に含まれるドリフト成分としての低周波成分を除去する。この後、相補フィルタ４２は、算出した第２姿勢補正加速度を平滑化処理部４３へ向けて出力する。

平滑化処理部４３は、目標着地点２が動揺により変化する場合であっても、航空機１と、動揺による変位量を平滑化(平均化)した目標着地点２との相対高度Δｈが目標相対高度と一致するように、飛行制御するための処理を行っている。平滑化処理部４３は、第２姿勢補正加速度と、相対高度Δｈとに基づいて、相対高度Δｈを平滑化した平滑化相対高度Δｈを出力している。具体的に、平滑化処理部４３には、相補フィルタ４２から出力された第２姿勢補正加速度が入力され、また、誘導演算部３４により算出された相対高度Δｈが入力される。平滑化処理部４３は、第２姿勢補正加速度と相対高度Δｈとが入力されると、航空機１と、動揺による変位量を平滑化(平均化)した目標着地点２との相対高度Δｈを目標相対高度と一致させるための、平滑化相対高度Δｈを算出する。そして、平滑化処理部４３は、算出した平滑化相対高度Δｈを減算回路部４５へ向けて出力する。減算回路部は、目標相対高度を取得し、取得した目標相対高度と、平滑化処理部４３から入力された平滑化相対高度Δｈとの差分を算出する。そして、減算回路部４５は、算出した差分をフィードバック制御部４４へ向けて出力する。

フィードバック制御部４４は、航空機１の慣性速度を取得し、取得した航空機１の慣性速度と、減算回路部４５から入力された目標相対高度と平滑化相対高度Δｈとの差分とに基づいて、制御量Ｃ´を算出する。そして、フィードバック制御部４４は、算出した制御量Ｃ´を飛行制御部３６へ向けて出力する。飛行制御部３６は、制御量Ｃ´に基づいて飛行制御を実行する。なお、飛行制御部３６による飛行制御の一例として、航空機１がヘリコプタである場合、ヘリコプタに設けられる回転ブレードのコレクティブピッチ角の角度を変更する飛行制御を行う。

ここで、図９を参照して、平滑化処理部４３の相対高度に関する信号処理について説明する。図９は、平滑化処理部の相対高度から平滑化相対高度までのボード線図である。図９は、その横軸が周波数となっており、その縦軸が、ゲイン（ｄＢ）及び位相（ｄｅｇ）となっている。図９において、動揺が発生する周波数の帯域が、動揺周波数帯域ｆとなっており、動揺周波数帯域ｆを含む動揺周波数帯域ｆ以上の周波数の帯域を、周波数帯ｆ１（第１の周波数帯）とし、動揺周波数帯域ｆ１よりも低い周波数の帯域を、周波数帯ｆ２（第２の周波数帯）とする。

平滑化処理部４３は、入力信号にゲインをかけて出力信号を増幅している。入力信号は、相対高度Δｈであり、出力信号は、平滑化相対高度Δｈである。図９に示すように、周波数帯ｆ１は、周波数帯ｆ２に比して、十分にゲインが低いものとなっていることから、周波数帯ｆ１における動揺の影響を低減している。また、周波数帯ｆ２は、減衰係数が大きく（ζ＞０．７）なっており、低周波の外乱応答が振動的にならないようにしている。減衰係数ζの値を大きくすると、オーバーシュートは抑えられ、ζ＞０．７であれば、望ましい結果が得られる。

（相対位置に関する空間安定ホバリング）
図５では、平滑化相対位置と目標相対位置との差分がゼロとなるように、空間安定ホバリングに関する位置制御を実行している。誘導演算部３４は、加速度補正処理部４１と、相補フィルタ４２と、平滑化処理部４３と、フィードバック制御部４４と、カルマンフィルタ４６と、相対速度推定処理部４７とを備えている。なお、加速度補正処理部４１、相補フィルタ４２、平滑化処理部４３、カルマンフィルタ４６及び相対速度推定処理部４７は、誘導演算部３４により実現されていてもよいし、誘導演算部３４とは別体の処理部によって実現されていてもよいし、それらの組み合わせによって実現されていてもよく、特に限定されない。なお、図５では、ピッチ軸の方向となるＸ方向の成分と、ロール軸の方向となるＹ方向の成分とを併記して図示しており、誘導演算部３４により各成分の制御量をそれぞれ算出している。

加速度補正処理部４１は、上記と同様に、航空機１の加速度と航空機１の姿勢とに基づいて、航空機１の加速度を補正した第１姿勢補正加速度を出力している。また、加速度補正処理部４１は、航空機１の加速度と航空機１の姿勢角とが入力されると、縦方向及び横方向における航空機１の加速度を補正した第１姿勢補正加速度を算出する。加速度補正処理部４１は、算出した第１姿勢補正加速度を相補フィルタ４２へ向けて出力する。

相補フィルタ４２は、上記と同様に、第１姿勢補正加速度と航空機１の慣性速度とに基づいて、第１姿勢補正加速度に含まれるドリフト成分を除去した第２姿勢補正加速度を出力している。具体的に、相補フィルタ４２には、加速度補正処理部４１から出力された第１姿勢補正加速度が入力され、また、航法装置２０において取得された縦方向及び横方向における航空機１の慣性速度が入力される。相補フィルタ４２は、第１姿勢補正加速度と航空機１の慣性速度とが入力されると、第１姿勢補正加速度の出力値に含まれる低周波成分を除去した高周波成分と、慣性速度の微分値に含まれる低周波成分とを合成して、縦方向及び横方向における第２姿勢補正加速度を出力している。相補フィルタ４２は、第２姿勢補正加速度を平滑化処理部４３及びフィードバック制御部４４へ向けて出力する。なお、相補フィルタ４２については、図６と同様である。

カルマンフィルタ４６は、相対位置（Ｘ，Ｙ）に基づく推定を行い、推定後の推定相対位置（Ｘ，Ｙ）を出力している。具体的に、カルマンフィルタ４６には、誘導演算部３４により算出された相対位置（Ｘ，Ｙ）が入力される。カルマンフィルタ４６は、相対位置（Ｘ，Ｙ）が入力されると、相対位置（Ｘ，Ｙ）の時間変化を推定することで、推定相対位置（Ｘ，Ｙ）を算出する。カルマンフィルタ４６は、算出した推定相対位置（Ｘ，Ｙ）を平滑化処理部４３へ向けて出力する。

平滑化処理部４３は、第２姿勢補正加速度と、推定相対位置（Ｘ，Ｙ）とに基づいて、推定相対位置（Ｘ，Ｙ）を平滑化した平滑化相対位置（Ｘ，Ｙ）を出力している。具体的に、平滑化処理部４３には、相補フィルタ４２から出力された第２姿勢補正加速度が入力され、また、カルマンフィルタ４６により算出された推定相対位置（Ｘ，Ｙ）が入力される。平滑化処理部４３は、第２姿勢補正加速度と推定相対位置（Ｘ，Ｙ）とが入力されると、航空機１と、動揺による変位量を平滑化(平均化)した目標着地点２との相対位置を目標相対位置と一致させるための、平滑化相対位置（Ｘ，Ｙ）を算出する。そして、平滑化処理部４３は、算出した平滑化相対位置（Ｘ，Ｙ）をフィードバック制御部４４へ向けて出力する。

相対速度推定処理部４７は、相対位置（Ｘ，Ｙ）と航空機１の機体速度とに基づいて、推定相対速度を出力している。具体的に、相対速度推定処理部４７には、誘導演算部３４により算出された相対位置（Ｘ，Ｙ）が入力され、また、航法装置２０において取得された縦方向及び横方向における航空機１の機体速度が入力される。相対速度推定処理部４７は、入力された相対位置（Ｘ，Ｙ）と機体速度とから、相対速度を推定し、推定相対速度としてフィードバック制御部４４へ向けて出力する。

フィードバック制御部４４は、相補フィルタ４２から入力された第２姿勢補正加速度と、平滑化処理部４３から入力された平滑化相対位置（Ｘ，Ｙ）と、相対速度推定処理部４７から入力された推定相対速度とに基づいて、制御量Ｃ´を算出する。そして、フィードバック制御部４４は、算出した制御量Ｃ´を飛行制御部３６へ向けて出力する。飛行制御部３６は、制御量Ｃ´に基づいて飛行制御を実行する。なお、飛行制御部３６による飛行制御の一例として、航空機１がヘリコプタである場合、ヘリコプタのメインロータを縦方向及び横方向に傾ける飛行制御を行う。

ここで、図１０を参照して、平滑化処理部４３の相対位置に関する信号処理について説明する。図１０は、平滑化処理部の相対位置から平滑化相対位置までのボード線図である。図１０は、その横軸が周波数となっており、その縦軸が、ゲイン（ｄＢ）及び位相（ｄｅｇ）となっている。図１０において、図９と同様に、動揺が発生する周波数の帯域が、動揺周波数帯域ｆとなっており、動揺周波数帯域ｆを含む動揺周波数帯域ｆ以上の周波数の帯域を、周波数帯ｆ１（第１の周波数帯）とし、動揺周波数帯域ｆ１よりも低い周波数の帯域を、周波数帯ｆ２（第２の周波数帯）とする。

平滑化処理部４３は、入力信号にゲインをかけて出力信号を増幅している。入力信号は、推定相対位置（Ｘ，Ｙ）であり、出力信号は、平滑化相対位置（Ｘ，Ｙ）である。図１０に示すように、周波数帯ｆ１は、図９と同様に、周波数帯ｆ２に比して、十分にゲインが低いものとなっていることから、周波数帯ｆ１における動揺の影響を低減している。また、周波数帯ｆ２は、相対高度と同様に、減衰係数が大きく（ζ＞０．７）なっており、低周波の外乱応答が振動的にならないようにしている。減衰係数ζの値を大きくすると、オーバーシュートは抑えられ、ζ＞０．７であれば、望ましい結果が得られる。

（航空機の位置制御方法）
次に、図７を参照して、本実施形態にかかる航空機１の位置制御システム１００を用いた位置制御方法、具体的には、平滑化処理部４３における平滑化相対高度Δｈ及び平滑化相対位置（Ｘ，Ｙ）を算出するまでの平滑化処理について説明する。図７は、本実施形態にかかる航空機の位置制御方法に関する一例を示すフローチャートである。なお、図７では、相対高度及び相対位置に関する平滑化処理となっており、また、平滑化処理部４３から平滑化相対高度Δｈ及び平滑化相対位置（Ｘ，Ｙ）を出力するまでの平滑化処理となっている。

平滑化処理では、先ず、加速度補正処理部４１が、入力される航空機１の加速度と航空機１の姿勢とに基づいて、第１姿勢補正加速度を出力する（ステップＳ１）。ステップＳ１では、相対高度Δｈに関する位置制御の場合、加速度補正処理部４１が、垂直方向における第１姿勢補正加速度を出力する。また、ステップＳ１では、相対位置（Ｘ，Ｙ）に関する位置制御の場合、加速度補正処理部４１が、縦方向及び横方向における第１姿勢補正加速度を出力する。

続いて、航空機１の位置制御方法では、相補フィルタ４２が、第１姿勢補正加速度と航空機１の慣性速度とに基づいて、第２姿勢補正加速度を出力する（ステップＳ２）。ステップＳ２では、第１姿勢補正加速度の低周波成分を除去することで、第１姿勢補正加速度に含まれるドリフト成分を除去する。ステップＳ２では、相対高度Δｈに関する位置制御の場合、相補フィルタ４２が、垂直方向における第２姿勢補正加速度を出力する。また、ステップＳ２では、相対位置（Ｘ，Ｙ）に関する位置制御の場合、相補フィルタ４２が、縦方向及び横方向における第２姿勢補正加速度を出力する。

この後、航空機１の位置制御方法では、平滑化処理部４３が、垂直方向における第２姿勢補正加速度及び相対高度Δｈと、縦方向及び横方向における第２姿勢補正加速度及び推定相対位置（Ｘ，Ｙ）とに基づいて、平滑化相対高度Δｈ及び平滑化相対位置（Ｘ，Ｙ）を出力する。

以上のように、実施形態に記載の航空機１の位置制御システム１００、航空機１及び航空機１の位置制御方法は、例えば、以下のように把握される。

第１の態様に係る航空機１の位置制御システム１００は、動揺する目標着地点２に対して、空間上の目標座標に航空機１を保持させる航空機１の位置制御システム１００であって、前記航空機１の加速度と前記航空機１の姿勢とに基づいて、前記航空機１の加速度を補正するための第１姿勢補正加速度を出力する加速度補正処理部４１と、前記第１姿勢補正加速度と前記航空機１の慣性速度とに基づいて、前記第１姿勢補正加速度に含まれるドリフト成分を除去した第２姿勢補正加速度を出力する相補フィルタ４２と、前記第２姿勢補正加速度と、前記航空機１と前記目標着地点２との間の相対座標とに基づいて、前記相対座標を平滑化した平滑化相対座標を出力する平滑化処理部４３と、を備える。

この構成によれば、取得される航空機１の加速度に基づいて出力される第１姿勢補正加速度に含まれるドリフト成分を、相補フィルタ４２において除去することができる。このため、ドリフト成分が除去された第２姿勢補正加速度を用いて、平滑化相対座標を取得することができるため、目標着地点２が動揺する場合であっても、空間上における航空機１の位置を安定的に保持させることができる。

第２の態様として、前記相補フィルタ４２は、前記第１姿勢補正加速度の出力値に含まれる低周波成分を除去した高周波成分と、前記慣性速度の微分値に含まれる低周波成分と、を合成して、前記第２姿勢補正加速度を出力する。

この構成によれば、第１姿勢補正加速度の出力値に含まれる低周波成分を除去することで、ドリフト成分を好適に除去することができる。また、除去した低周波成分を、慣性速度により好適に補間することができる。

第３の態様として、前記相対座標は、前記航空機と目標着地点との鉛直方向における相対高度Δｈ、及び前記航空機と目標着地点との水平面内における相対位置（Ｘ，Ｙ）の少なくとも一方を含む。

この構成によれば、相対高度Δｈ及び相対位置（Ｘ，Ｙ）において、航空機１の位置を安定的に保持させることができる。

第４の態様として、前記平滑化処理部４３は、前記相対座標が前記相対位置（Ｘ，Ｙ）を含む場合、前記平滑化相対座標として、平滑化相対位置を出力し、前記航空機１の機体速度と前記相対位置（Ｘ，Ｙ）とに基づいて、前記航空機１と前記目標着地点２との間の相対速度を推定した推定相対速度を出力する相対速度推定処理部４７を、さらに備え、前記推定相対速度と前記平滑化相対位置とに基づいて、前記航空機１を飛行制御する。

この構成によれば、相対位置（Ｘ，Ｙ）において、目標着地点２に対する位置制御を精度よく行うことができる。

第５の態様として、前記相対座標が前記相対位置（Ｘ，Ｙ）を含む場合、前記相対位置（Ｘ，Ｙ）に基づいて推定した推定相対位置を出力するカルマンフィルタ４６を、さらに備え、前記平滑化処理部４３は、前記第２姿勢補正加速度と前記推定相対位置とに基づいて、前記平滑化相対座標として、平滑化相対位置を出力する。

この構成によれば、相対位置（Ｘ，Ｙ）において、精度のよい平滑化相対位置を出力することができる。このため、相対位置（Ｘ，Ｙ）において、航空機１の位置を安定的に精度よく保持させることができる。

第６の態様として、前記平滑化処理部は、前記相対座標にゲインをかけて前記平滑化相対座標を出力しており、前記動揺が発生する動揺周波数帯域以上の周波数の帯域を、第１の周波数帯とし、前記動揺周波数帯域よりも低い周波数の帯域を、第２の周波数帯とすると、前記第１の周波数帯における前記ゲインは、前記第２の周波数帯における前記ゲインに対して低く、前記第２の周波数帯における減衰係数ζが、ζ＞０．７となっている。

この構成によれば、第１の周波数帯ｆ１は、第２の周波数帯に比して、十分にゲインが低いものとなっていることから、第１の周波数帯ｆ１における動揺の影響を低減することができる。また、第２の周波数帯ｆ２は、減衰係数が大きく（ζ＞０．７）なることから、低周波の外乱応答が振動的にならないようにすることができる。

第７の態様に係る航空機１は、航空機の加速度を取得する加速度取得部と、前記航空機の姿勢を取得する姿勢取得部と、前記航空機の慣性速度を取得する慣性速度取得部と、前記航空機の相対座標を取得する相対座標取得部と、上記の航空機１の位置制御システム１００と、を備える。

この構成によれば、目標着地点２が動揺する場合であっても、目標着地点２に対する位置が安定的となるように、飛行制御される航空機１を提供することができる。

第８の態様に係る航空機１の位置制御方法は、動揺する目標着地点に対して、空間上の所定座標に航空機を保持させる航空機の位置制御方法であって、前記航空機の加速度と前記航空機の姿勢とに基づいて、前記航空機の加速度を補正するための第１姿勢補正加速度を出力するステップと、前記第１姿勢補正加速度と前記航空機の慣性速度とに基づいて、前記第１姿勢補正加速度に含まれるドリフト成分を除去した第２姿勢補正加速度を出力するステップと、前記第２姿勢補正加速度と、前記航空機と前記目標着地点との間の相対座標とに基づいて、前記相対座標を平滑化した平滑化相対座標を出力するステップと、を備える。

この構成によれば、取得される航空機１の加速度に基づいて出力される第１姿勢補正加速度に含まれるドリフト成分を、相補フィルタ４２において除去することができる。このため、ドリフト成分が除去された第２姿勢補正加速度を用いて、平滑化相対座標を取得することができるため、目標着地点２が動揺する場合であっても、空間上における航空機１の位置を安定的に保持させることができる。

１ 航空機
２ 目標着地点
５ 船舶
７ マーカー
１０ カメラ
２０ 航法装置
３０ 制御部
３２ 画像処理部
３４ 誘導演算部
３６ 飛行制御部
４１ 加速度補正処理部
４２ 相補フィルタ
４３ 平滑化処理部
４６ カルマンフィルタ
４７ 相対速度推定処理部
５１ 擬似微分フィルタ
５２ ローパスフィルタ
５３ ハイパスフィルタ
１００ 位置制御システム

Claims (8)

1. 動揺する目標着地点に対して、空間上の目標座標に航空機を保持させる航空機の位置制御システムであって、
   前記航空機の加速度と前記航空機の姿勢とに基づいて、前記航空機の加速度を補正した第１姿勢補正加速度を出力する加速度補正処理部と、
   前記第１姿勢補正加速度と前記航空機の慣性速度とに基づいて、前記第１姿勢補正加速度に含まれるドリフト成分を除去した第２姿勢補正加速度を出力する相補フィルタと、
   前記第２姿勢補正加速度と、前記航空機と前記目標着地点との間の相対座標とに基づいて、前記相対座標を平滑化した平滑化相対座標を出力する平滑化処理部と、を備える航空機の位置制御システム。
2. 前記相補フィルタは、
   前記第１姿勢補正加速度の出力値に含まれる低周波成分を除去した高周波成分と、前記慣性速度の微分値に含まれる低周波成分と、を合成して、前記第２姿勢補正加速度を出力する請求項１に記載の航空機の位置制御システム。
3. 前記相対座標は、前記航空機と目標着地点との鉛直方向における相対高度、及び前記航空機と目標着地点との水平面内における相対位置の少なくとも一方を含む請求項１または２に記載の航空機の位置制御システム。
4. 前記平滑化処理部は、前記相対座標が前記相対位置を含む場合、前記平滑化相対座標として、平滑化相対位置を出力し、
   前記航空機の機体速度と前記相対位置とに基づいて、前記航空機と前記目標着地点との間の相対速度を推定した推定相対速度を出力する相対速度推定処理部を、さらに備え、
   前記推定相対速度と前記平滑化相対位置とに基づいて、前記航空機を飛行制御する請求項３に記載の航空機の位置制御システム。
5. 前記相対座標が前記相対位置を含む場合、
   前記相対位置に基づいて推定した推定相対位置を出力するカルマンフィルタを、さらに備え、
   前記平滑化処理部は、前記第２姿勢補正加速度と前記推定相対位置とに基づいて、前記平滑化相対座標として、平滑化相対位置を出力する請求項３または４に記載の航空機の位置制御システム。
6. 前記平滑化処理部は、前記相対座標にゲインをかけて前記平滑化相対座標を出力しており、
   前記動揺が発生する動揺周波数帯域以上の周波数の帯域を、第１の周波数帯とし、
   前記動揺周波数帯域よりも低い周波数の帯域を、第２の周波数帯とすると、
   前記第１の周波数帯における前記ゲインは、前記第２の周波数帯における前記ゲインに対して低く、
   前記第２の周波数帯における減衰係数ζが、ζ＞０．７となっている請求項１から５のいずれか１項に記載の航空機の位置制御システム。
7. 航空機の加速度を取得する加速度取得部と、
   前記航空機の姿勢を取得する姿勢取得部と、
   前記航空機の慣性速度を取得する慣性速度取得部と、
   前記航空機の相対座標を取得する相対座標取得部と、
   請求項１から６のいずれか１項に記載の航空機の位置制御システムと、を備える航空機。
8. 動揺する目標着地点に対して、空間上の所定座標に航空機を保持させる航空機の位置制御方法であって、
   前記航空機の加速度と前記航空機の姿勢とに基づいて、前記航空機の加速度を補正した第１姿勢補正加速度を出力するステップと、
   前記第１姿勢補正加速度と前記航空機の慣性速度とに基づいて、前記第１姿勢補正加速度に含まれるドリフト成分を除去した第２姿勢補正加速度を出力するステップと、
   前記第２姿勢補正加速度と、前記航空機と前記目標着地点との間の相対座標とに基づいて、前記相対座標を平滑化した平滑化相対座標を出力するステップと、を備える航空機の位置制御方法。

Images