JP4961166B2 - 自律飛行制御装置及び自律飛行制御装置を搭載した小型無人ヘリコプタ - Google Patents

Description

本発明は、設定される目標値に向けて小型無人ヘリコプタを自律的に飛行制御する装置及び方法に関し、特に離陸から着陸までの完全な自律飛行を実現する自律飛行制御装置及び自律飛行方法に関する。

ヘリコプタ機体は、前後、左右、上下方向の運動や空中静止（ホバリング）等、固定翼機では実施するのが難しい行動範囲を有する機体であり、様々な場面で柔軟に活用できる可能性を有している。例えば、送電線点検のような高所で行う点検作業、災害現場での撮影作業、あるいは地雷を探知する作業等、有人機が行うのには困難または危険な場所での活用が期待されている。近年、こうしたヘリコプタ機体の有効性に着目し、ヘリコプタ機体を予め与えられた目標値に向けて、自律的に飛行させるための研究がなされている（特許文献１参照）。
特開２０００−１１８４９８号公報

しかしながら、上記特許文献１に提案されているヘリコプタ機体は、離陸制御時から着陸制御時までの完全な自律飛行を実現するものではなく、離着陸制御時に地上操作員が無線遠隔操作を行っている。これは、離着陸制御時において、以下のような技術上・実用上の課題が存在していたためである。すなわち、離陸制御時において、ヘリコプタ機体は接地状態にあり、ヘリコプタ機体の脚が地面に拘束された状態にある。この状態では姿勢角指令に追随できなくなり、過大な制御指令が出力されて機体が転倒するおそれがある。また、姿勢角の均衡が取れていないため地面から速やかに離隔させることができず、仮にできたとしても姿勢角の均衡が取れるまでは地面近傍で不安定な状態が続くため、安定した離陸制御ができないという問題がある。一方、着陸制御時において、ヘリコプタ機体を安定した状態で着陸させるために、接地時の降下速度を十分小さくしなければならない。このため、通常の自律飛行とは異なる「着陸制御時の制御系」を別途備えるという考え方があるが、必然的にソフトウェア構成の複雑化をもたらすことによりコストが増大するという問題がある。なお、仮に「着陸制御時の制御系」を別途備えることができたとしても、接地時に機体が地面との摩擦により拘束されて姿勢角指令に追随できなくなり、過大な制御指令が出力されて機体が転倒するおそれがあり、安定した着陸制御ができないという問題がある。

こうした課題を解決する技術として、離着陸制御時に地上操作員が無線遠隔操作を行わず、離陸制御時から着陸制御時まで完全なヘリコプタ機体の自律飛行を実現するとともに、コストが増大するのを抑制することを目的とした、自律飛行制御装置及び方法が提案されている（特許文献２参照）。
特開２００４−１３０８５２号公報

以下、特許文献２に開示されている技術内容について説明する。図２２はヘリコプタ機体に搭載する自律飛行制御装置を示したものである。自律飛行制御装置２２００は、その構成を大きく分けると、鉛直方向について制御を行う鉛直方向制御部２２１０と、水平方向について制御を行う水平方向制御部２２２０と、鉛直方向制御部２２１０及び水平方向制御部２２２０を実行する各種制御プログラムを管理するプログラム格納部２２３０とからなる。

鉛直方向制御部２２１０は、現在の高度、所定の鉛直速度目標値、鉛直速度目標値を積分して算出する高度目標値、及び現在の鉛直速度を基に鉛直速度制御値を算出する高度制御部２２１１を備え、鉛直速度制御値を鉛直方向制御値として出力する。舵角駆動用アクチュエータは鉛直方向制御値に従ってコレクティブピッチ舵角を制御することにより、機体の鉛直方向を制御する。

水平方向制御部２２２０は、現在の水平位置、及び所定の水平位置目標値を基に水平位置制御値（水平速度目標値）を算出し、現在の水平速度、及び算出する水平位置制御値（水平速度目標値）を基に水平速度制御値（姿勢角目標値）を算出する水平位置制御部２２２１と、現在の姿勢角、現在の姿勢角速度、及び算出する水平速度制御値（姿勢角目標値）を基に姿勢角制御値を算出する姿勢角制御部２２２２とを備え、姿勢角制御値を水平方向制御値として出力する。舵角駆動用アクチュエータは水平方向制御値に従ってサイクリックピッチ舵角を制御することにより、機体の水平方向を制御する。

また、プログラム格納部８３０は、離陸制御時の制御に用いる離陸制御プログラム、着陸制御時の制御に用いる着陸制御プログラム、及び自律飛行制御時の制御に用いる所定の飛行パターンデータに基づく自律飛行制御プログラムを管理する。

すなわち、自律飛行制御装置２２００は、地上操作員からの指令、あるいはＧＰＳセンサ、慣性センサ、及び角速度センサ等の各種センサがヘリコプタ機体の現状態等を検出することにより、それに応じて各種制御プログラムを実行し、高度制御部２２１１が鉛直方向制御値を算出し、水平位置制御部８２１及び姿勢角制御部８２２が水平方向制御値を算出している。これにより、鉛直方向制御値に従って舵角駆動用アクチュエータがコレクティブピッチ舵角を制御して機体の鉛直方向を制御し、また、水平方向制御値に従って舵角駆動用アクチュエータがサイクリックピッチ舵角を制御して機体の水平方向を制御する。

また、自律飛行制御装置２２００は、離陸制御時において、ヘリコプタ機体の脚が地面に拘束されない程度、すなわち、地面から完全に離隔する安全高度（以下、安全高度と記載する）を第１の目標高度として設定し、また、ヘリコプタ機体が自律飛行を開始する点（以下、自律飛行開始点と記載する）を第２の目標高度として設定している。そして、自律飛行制御装置２２００で実行する離陸制御プログラムは、離陸開始点から第１の目標高度（安全高度）までの制御と、第１の目標高度（安全高度）から第２の目標高度（自律飛行開始点）までの制御とに分け、それぞれの制御で用いる制御部の構成や値について変更や抑制等しており、第１の目標高度（安全高度）の検出に従って切り換わる。

具体的には、鉛直方向制御部２２１０において、離陸開始点から第１の目標高度（安全高度）までは、高度制御部２２１１は用いず、一定の割合で増加する鉛直速度制御値を鉛直方向制御値として与えることによって、舵角駆動用アクチュエータがコレクティブピッチ舵角を制御し、機体を上昇させる。そして、第１の目標高度（安全高度）の検出に従って切り換わり、第１の目標高度（安全高度）から第２の目標高度（自律飛行開始点）までは、高度制御部２２１１を用いて、現在の高度、所定の鉛直速度目標値、鉛直速度目標値を積分して算出する高度目標値、及び現在の鉛直速度を基に算出する鉛直速度制御値を鉛直方向制御値として出力し、これに従って舵角駆動用アクチュエータがコレクティブピッチ舵角を制御し、機体を上昇させる。

一方、水平方向制御部２２２０において、離陸開始点から第１の目標高度（安全高度）までは、水平位置偏差をゼロに設定して水平位置制御部２２２１を抑制し、また、姿勢角偏差をゼロに設定して姿勢角制御部２２２２を抑制する。そして、第１の目標高度（安全高度）の検出に従って切り換わり、第１の目標高度（安全高度）から第２の目標高度（自律飛行開始点）までは、水平位置制御部２２２１及び姿勢角制御部２２２２の抑制を解除し、水平位置制御部２２２１において、現在の水平位置と所定の水平位置目標値との偏差を入力して水平位置偏差積分値を算出し、また、姿勢角制御部８２２において、現在の姿勢角と水平位置制御部２２２１から入力される姿勢角目標値との偏差を入力して姿勢角偏差積分値を算出する。これにより、接地状態で機体の脚と地面との摩擦により機体が拘束されて姿勢角指令に追随できない場合に、過大な制御指令が出力されて機体が転倒するのを防止している。

さらに、自律飛行制御装置２２００は、着陸制御時において、ヘリコプタ機体が安全に着陸できる地上付近（以下、地上付近と記載する）を第１の目標高度として設定し、また、ヘリコプタ機体が着陸する点（以下、着陸点）を第２の目標高度として設定している。そして、自律飛行制御装置２２００で実行する着陸制御プログラムは、着陸開始点から第１の目標高度（地上付近）までの制御と、第１の目標高度（地上付近）から第２の目標高度（着陸点）までの制御とに分け、それぞれの制御で用いる制御部の構成や値について変更や抑制等しており、第１の目標高度（地上付近）の検出に従って切り換わる。

具体的には、鉛直方向制御部２２１０において、着陸開始点から第１の目標高度（地上付近）までは、高度制御部２２１１を用いて、現在の高度、高度変化に従って減少する所定の鉛直速度目標値、鉛直速度目標値を積分して算出する高度目標値、及び現在の鉛直速度を基に算出する鉛直速度制御値を鉛直方向制御値として出力し、これに従って舵角駆動用アクチュエータがコレクティブピッチ舵角を制御し、機体を下降させる。そして、第１の目標高度（地上付近）の検出に従って切り換わり、第１の目標高度（地上付近）から第２の目標高度（着陸点）までは、高度制御部２２１１を用いて、高度変化に従って減少する所定の鉛直速度目標値をさらに小さくすることによって、算出する鉛直速度制御値をさらに小さくし、これを鉛直方向制御値として出力し、これに従って舵角駆動用アクチュエータがコレクティブピッチ舵角を制御し、機体を下降させる。

一方、水平方向制御部２２２０において、着陸開始点から第１の目標高度（地上付近）までは、水平位置制御部２２２１及び姿勢角制御部２２２２を抑制しないで水平方向制御を行う。つまり、現在の水平位置と所定の水平位置目標値との偏差を入力して水平位置偏差積分値を算出し、また、姿勢角制御部２２２２を用いて、現在の姿勢角と水平位置制御部２２２１から入力される姿勢角目標値との偏差を入力して姿勢角偏差積分値を算出する。そして、第１の目標高度（地上付近）の検出に従って切り換わり、第１の目標高度（地上付近）から第２の目標高度（着陸点）までは、水平位置偏差をゼロに設定して水平位置制御部２２２１を抑制し、また、姿勢角偏差をゼロに設定して姿勢角制御部２２２２を抑制する。なお、地上付近は、安全高度とほぼ同じ高度を示しているが、説明の便宜上、用語を統一していない。また、自律飛行制御装置２２００は、自律飛行制御時において、自律飛行制御プログラムを実行し、自律飛行制御装置２２００内の各種制御部の構成を用いて所定の飛行パターンデータに基づいた自律飛行を行う。

以下、自律飛行制御装置２２００の各種制御部の構成、ならびに離陸制御時、自律飛行制御時、及び着陸制御時の動作について説明する。なお、ここでいう安全高度とは、ヘリコプタ機体の脚が地面に拘束されないほどの高度を示し、この点から高度制御部２２１１を適用する。また、地上付近は、安全高度とほぼ同じ高度を示しているが、説明の便宜上、用語を統一していない。また、自律飛行制御装置２２００は、自律飛行制御時において、自律飛行制御プログラムを実行し、自律飛行制御装置２２００内の各種制御部の構成を用いて所定の飛行パターンデータに基づいた自律飛行を行う。

つづいて、各種制御部について説明する。図２３は高度制御部２２１１の内部構成をブロック図で示したものである。高度制御部２２１１は、ヘリコプタ機体に搭載しているＧＰＳセンサが検出する現在の高度と、所定の鉛直速度目標値を積分して算出する高度目標値との偏差（高度偏差）を算出する高度偏差算出部２３０１と、この高度偏差の積分値を算出する高度偏差積分値算出部２３０２と、ＧＰＳセンサが検出する現在の鉛直速度と所定の鉛直速度目標値との偏差（鉛直速度偏差）を算出する鉛直速度偏差算出部２３０３と、高度偏差算出部２３０１が算出する高度偏差、高度偏差積分値算出部２３０２が算出する高度偏差積分値、及び鉛直速度偏差算出部２３０３が算出する鉛直速度偏差に対して所定のゲインをかけるゲイン処理部２３０４と、所定のゲインをかけた、高度偏差、高度偏差積分値、及び鉛直速度偏差を加算することにより、鉛直速度制御値を算出する加算部２３０５とを備えている。これにより、高度制御部２２１１では、現在の高度、所定の鉛直速度目標値、鉛直速度目標値を積分して算出する高度目標値、及び現在の鉛直速度を基に鉛直速度制御値を算出し、鉛直速度制御値を鉛直方向制御値として出力し、鉛直方向制御値に従って舵角駆動用アクチュエータがコレクティブピッチ舵角を制御して機体の鉛直方向を制御する。

図２４は水平位置制御部２２２１の内部構成をブロック図で示したものである。水平位置制御部２２２１は、ヘリコプタ機体に搭載しているＧＰＳセンサが検出する現在の水平位置と所定の水平位置目標値との偏差（水平位置偏差）を算出する水平位置偏差算出部２４０１と、この水平位置偏差の積分値を算出する水平位置偏差積分値算出部２４０２と、水平位置偏差算出部２４０１が算出する水平位置偏差、水平位置偏差積分値算出部２４０２が算出する水平位置偏差積分値に対して所定のゲインをかけるゲイン処理部２４０３と、所定のゲインをかけた、水平位置偏差、及び水平位置偏差積分値を加算することにより、地球座標系の水平位置制御値（水平速度目標値）を算出する加算部２４０４と、算出する地球座標系の水平位置制御値（水平速度目標値）を機体座標系の水平位置制御値（水平速度目標値）に座標変換する座標変換部２４０５と、ヘリコプタ機体に搭載しているＧＰＳセンサが検出する現在の水平速度と座標変換部２４０５から出力する機体座標系の水平位置制御値（水平速度目標値）との偏差（水平速度偏差）を算出する水平速度偏差算出部２４０６と、水平速度偏差算出部２４０６が算出する水平速度偏差に対して所定のゲインをかけて水平速度制御値（姿勢角目標値）を算出するゲイン処理部２４０７とを備えている。これにより、水平位置制御部２２２１では、現在の水平位置、所定の水平位置目標値を基に水平位置制御値（水平速度目標値）を算出し、算出する水平位置制御値（水平速度目標値）、現在の水平速度を基に水平速度制御値（姿勢角目標値）を算出し、これを姿勢角制御部２２２２へ出力する。

図２５は姿勢角制御部２２２２の内部構成をブロック図で示したものである。姿勢角制御部２２２２は、ヘリコプタ機体に搭載している慣性センサが検出する現在の姿勢角と水平位置制御部２２２１が算出する水平速度制御値（姿勢角目標値）との偏差（姿勢角偏差）を算出する姿勢角偏差算出部２５０１と、この姿勢角偏差の積分値を算出する姿勢角偏差積分値算出部２５０２と、姿勢角偏差算出部２５０１が算出する姿勢角偏差、姿勢角偏差積分値算出部２５０２が算出する姿勢角偏差積分値に対して所定のゲインをかけるゲイン処理部２５０３と、所定のゲインをかけた、姿勢角偏差、及び姿勢角偏差積分値を加算する加算部２５０４と、加算部２５０４で加算した値からヘリコプタ機体に搭載している角速度センサが検出する現在の姿勢角速度を減算することにより、水平速度制御値（姿勢角制御値）を算出する減算部１１０５とを備えている。これにより、姿勢角制御部２２２２では、水平速度制御値（姿勢角目標値）、現在の姿勢角、及び現在の姿勢角速度を基に姿勢角制御値を算出し、姿勢角制御値を水平方向制御値として出力し、水平方向制御値に従って舵角駆動用アクチュエータがサイクリックピッチ舵角を制御して機体の水平方向を制御する。

以上のように構成された自律飛行制御装置の離陸制御プログラムを実行したときの離陸制御時の制御について図２６を用いて説明する。はじめに、離陸制御時の鉛直方向制御を説明する。自律飛行制御装置２２００は、地上操作員からの離陸指令を受信すると、エンジンを始動し、離陸制御プログラムを実行する。離陸制御プログラムは、機体が離陸開始点から第１の目標高度（安全高度）まで上昇する間、高度制御部２２１１を用いずに鉛直方向制御を行う（ステップ２６１ａ）。具体的には、離陸制御プログラムは、高度制御部２２１１を用いず、一定の割合で増加する鉛直速度制御値を鉛直方向制御値として与えることによって、舵角駆動用アクチュエータがコレクティブピッチ舵角を制御し、機体を上昇させる（ステップ２６２ａ）。そして、ヘリコプタ機体に搭載しているＧＰＳセンサが、第１の目標高度（安全高度）を検出すると（ステップ２６３ａ）、実行中の離陸制御プログラムは、機体が第１の目標高度（安全高度）から第２の目標高度（自律飛行開始点）まで上昇する間、高度制御部２２１１を用いて鉛直方向制御を行う（ステップステップ２６４ａ）。具体的には、離陸制御プログラムは、現在の高度、所定の鉛直速度目標値、鉛直速度目標値を積分して算出する高度目標値、及び現在の鉛直速度を基に算出する鉛直速度制御値を鉛直方向制御値として出力し、これに従って舵角駆動用アクチュエータがコレクティブピッチ舵角を制御し、機体を上昇させる（ステップ２６５ａ）。その後、ヘリコプタ機体に搭載しているＧＰＳセンサが、第２の目標高度（自律飛行開始点）を検出すると（ステップ２６６ａ）、自律飛行制御装置２２００は、離陸制御プログラムを終了して一定時間ホバー飛行を行った後、プログラム格納部２２３０で管理している自律飛行制御プログラムを実行し、自律飛行制御装置２２００内の各種制御部の構成を用いて所定の飛行パターンデータに基づいた自律飛行を行う。

一方、離陸制御時の水平方向制御を説明する。離陸制御プログラムは、機体が離陸開始点から第１の目標高度（安全高度）まで上昇する間、水平位置制御部２２２１及び姿勢角制御部２２２２を抑制して水平方向制御を行う（ステップ２６１ｂ）。具体的には、離陸制御プログラムは、水平位置偏差積分値算出部２４０２への水平位置偏差をゼロに設定して水平位置制御部２２２１を抑制し、また、姿勢角偏差積分値算出部２５０２への姿勢角偏差をゼロに設定して姿勢角制御部２２２２を抑制する（ステップ２６２ｂ）。これにより、水平位置制御部２２２１で算出される水平速度制御値（姿勢角目標値）や、姿勢角制御部２２２２で算出される姿勢角制御値を抑制することができ、姿勢角制御部２２２２で算出する姿勢角制御値を水平方向制御値として出力し、これに従って舵角駆動用アクチュエータがサイクリックピッチ舵角を制御し、機体の水平方向を制御する（ステップ２６３ｂ）。これにより、接地状態で機体の脚と地面との摩擦により機体が拘束されて姿勢角指令に追随できない場合に、過大な制御指令が出力されて機体が転倒するのを防止している。そして、ヘリコプタ機体に搭載しているＧＰＳセンサが、第１の目標高度（安全高度）に到達したことを検出すると、実行中の離陸制御プログラムは、機体が第１の目標高度（安全高度）から第２の目標高度（自律飛行開始点）まで上昇する間、水平位置制御部２２２１及び姿勢角制御部２２２２の抑制を解除して水平方向制御を行う（ステップ２６４ｂ）。具体的には、離陸制御プログラムは、水平位置制御部２２２１において、現在の水平位置と所定の水平位置目標値との偏差を水平位置偏差積分値算出部２４０２に入力して水平位置偏差積分値を算出し、また、姿勢角制御部２２２２において、現在の姿勢角と水平位置制御部２２２１が算出する水平速度制御値（姿勢角目標値）との偏差を姿勢角偏差積分値算出部２５０２に入力して姿勢角偏差積分値を算出する（ステップ２６５ｂ）。そして、姿勢角制御部２２２２が算出する姿勢角制御値を水平方向制御値として出力し、これに従って舵角駆動用アクチュエータがサイクリックピッチ舵角を制御し、機体の水平方向を制御する（ステップ２６６ｂ）。

つづいて自律飛行制御装置の着陸制御プログラムを実行したときの着陸制御時の制御について図２７を用いて説明する。自律飛行制御装置８００は、自律飛行制御プログラムを実行し、自律飛行制御装置２２００内の各種制御部の構成を用いて所定の飛行パターンデータに基づいた自律飛行を行った後、機体を所定の着陸点の上空の着陸開始点で一定時間ホバー飛行させ、着陸制御プログラムを実行する。はじめに、着陸制御時の鉛直方向制御を説明する。着陸制御プログラムは、機体が着陸開始点から第１の目標高度（地上付近）まで下降する間、高度制御部２２１１を用いて鉛直方向制御を行う（ステップ２７１ａ）。具体的には、着陸制御プログラムは、高度制御部２２１１を用いて、現在の高度、高度変化に従って減少する所定の鉛直速度目標値、鉛直速度目標値を積分して算出する高度目標値、及び現在の鉛直速度を基に算出する鉛直速度制御値を鉛直方向制御値として出力し、これに従って舵角駆動用アクチュエータがコレクティブピッチ舵角を制御し、機体を下降させる（ステップ２７２ａ、２７３ａ）．そして、ヘリコプタ機体に搭載しているＧＰＳセンサが、第１の目標高度（地上付近）を検出すると（ステップ２７４ａ）、実行中の着陸制御プログラムは、機体が第１の目標高度（地上付近）から第２の目標高度（着陸点）まで下降する間、さらに小さな鉛直速度目標値を与え、高度制御部２２１１を用いて鉛直方向制御を行う（ステップ２７５ａ、２７６ａ）。具体的には、高度変化に従って減少する所定の鉛直速度目標値をさらに小さくすることによって、高度制御部２２１１が算出する鉛直速度制御値をさらに小さくし、これを鉛直方向制御値として出力し、これに従って舵角駆動用アクチュエータがコレクティブピッチ舵角を制御し、機体を下降させる。その後、ヘリコプタ機体に搭載しているＧＰＳセンサが、第２の目標高度（着陸点）を検出すると（ステップ２７７ａ）、自律飛行制御装置２２００は、着陸制御プログラムを終了する。

一方、着陸制御時の水平方向の動作を説明する。着陸制御プログラムは、機体が着陸開始点から第１の目標高度（地上付近）まで下降する間、水平位置制御部２２２１及び姿勢角制御部２２２２を用いて水平方向制御を行う（ステップ２７１ｂ）。具体的には、着陸制御プログラムは、水平位置制御部２２２１において、現在の水平位置と所定の水平位置目標値との偏差を水平位置偏差積分値算出部２４０２に入力して水平位置偏差積分値を算出し、また、姿勢角制御部２２２２において、現在の姿勢角と水平位置制御部２２２１が算出する水平速度制御値（姿勢角目標値）との偏差を姿勢角偏差積分値算出部２５０２に入力して姿勢角偏差積分値を算出する。そして、姿勢角制御部２２２２が算出する姿勢角制御値を水平方向制御値として出力し、これに従って舵角駆動用アクチュエータがサイクリックピッチ舵角を制御し、機体の水平方向を制御する（ステップ２７２ｂ）。そして、ヘリコプタ機体に搭載しているＧＰＳセンサが、第１の目標高度（地上付近）に到達したことを検出すると、実行中の着陸制御プログラムは、機体が第１の目標高度（地上付近）から第２の目標高度（着陸点）まで下降する間、水平位置制御部２２２１及び姿勢角制御部２２２２を抑制して水平方向制御を行う（ステップ２７３ｂ）。具体的には、着陸制御プログラムは、水平位置偏差積分値算出部２４０２への水平位置偏差をゼロに設定して水平位置制御部２２２１を抑制し、また、姿勢角偏差積分値算出部２５０２への姿勢角偏差をゼロに設定して姿勢角制御部２２２２を抑制する（ステップ２７４ｂ）。そして、水平位置制御部２２２１で算出される水平速度制御値（姿勢角目標値）や、姿勢角制御部２２２２で算出される姿勢角制御値を抑制することができ、姿勢角制御部２２２２で算出する姿勢角制御値を水平方向制御値として出力し、これに従って舵角駆動用アクチュエータがサイクリックピッチ舵角を制御し、機体の水平方向を制御する（ステップ２７５ｂ）。これにより、接地状態で機体の脚と地面との摩擦により機体が拘束されて姿勢角指令に追随できない場合に、過大な制御指令が出力されて機体が転倒するのを防止している。

しかしながら、特許文献２は、離陸制御時における離陸開始点から安全高度までの制御、または着陸制御時における地上付近から着陸点までの制御において、水平位置を用いているため、安定した離着陸制御ができないという問題がある。すなわち、ＧＰＳセンサにより検出する機体の水平位置は数メートルから十数メートルの範囲で値が不連続的に変化する場合があり、このような場合、機体の水平位置は不連続的に検出される。これは特に、地上付近において顕著であり、機体の水平位置が不連続的に検出されることに伴い、最終的に出力される水平方向制御値が地上付近において不安定な状態になる。この対応策として、特許文献２は、機体が地上付近にあるときは、水平位置偏差積分値算出部２４０２及び姿勢角偏差積分値算出部２５０２への入力をゼロに設定することにより、算出する水平方向制御値を抑制しているが、安定した離着陸制御を保証するものではない。このことは、機体の高度の検出についても同様であり、機体の高度も不連続的に検出されることがあり、最終的に出力される鉛直方向制御値が地上付近において不安定な状態になる。

また、特許文献２は、離着陸制御時において、第１の目標高度、第２の目標高度を設定し、離陸または着陸開始点から第１の目標高度までの制御と、第１の目標高度から第２の目標高度までの制御とに分け、第１の目標高度の検出に従って、それぞれの制御で用いる制御部の構成や値について変更や抑制等しているため、処理が複雑になるという問題がある。しかも、それは、第１の目標高度の検出に従って切り換わるので、正常に行われなかった場合には安定した離着陸制御を保証するものではない。

さらに、特許文献２は、第１の目標高度の検出に従って、ゲインを変更しているおそれもある。ゲインを変更することにより、余分な処理が加わることはもちろん、最悪の場合、ゲインを変更する前後で指令値信号の連続性が保たれず、安定した離着陸制御を保証するものではない。

一方、本出願人は、先の出願において、実験によりヘリコプタ機体の動特性を示す線形モデル式を導出し、これを基に製造した自律飛行制御装置を提案している（特許文献３参照）。特許文献３に示されている自律飛行制御装置は、線形モデル式を用いることにより装置内部で行う処理を単純化できるので、自律飛行制御装置の小型化を実現でき、また、これにより小型無人ヘリコプタへ搭載することも可能にし、汎用性を高めることができる。
特開２００４−２５６０２０号公報

本発明は、上記特許文献１及び２のような問題点に着目し、また、ヘリコプタ機体の動特性を示す線形モデル式を基に製造した小型化を実現できる自律飛行制御装置について、安定した離着陸制御を行うことができる自律飛行制御装置及び自律飛行制御装置を搭載した小型無人ヘリコプタを提供することを目的とする。

本発明の自律飛行制御装置は、離陸制御時または着陸制御時に安定した制御を行うため、鉛直方向においては高度を用いた演算を行わずに鉛直方向制御値を出力し、一方、水平方向においては水平位置を用いた演算を行わずに水平方向制御値を出力する。すなわち、本発明の自律飛行制御装置は、現在の鉛直速度、及び離陸制御時または着陸制御時に用いる所定の鉛直速度目標値を基に鉛直速度制御値を算出する鉛直速度制御部を有する鉛直方向制御部と、現在の水平速度、及び所定の水平速度目標値を基に水平速度制御値を算出する水平速度制御部と、現在の姿勢角、及び前記水平速度制御値を基に姿勢角制御値を算出する姿勢角制御部とを有する水平方向制御部とを備え、前記鉛直速度制御部が算出する鉛直速度制御値を鉛直方向制御値として出力し、前記姿勢角制御部が算出する姿勢角制御値を水平方向制御値として出力することを特徴とする。上記した本発明の自律飛行制御装置を小型無人ヘリコプタに搭載することにより、ヘリコプタ機体は、離陸開始時から着陸終了時まで完全な自律飛行を実現し、離着陸制御時には安定した制御を行う。

また、本発明の自律飛行制御装置は、離陸制御時または着陸制御時においては、安全高度または地上付近の検出による切り換えを行わず、所定の目標高度に到達するまで、現在の値と所定の目標値との偏差を積分し、ヘリコプタ機体の動特性を基にした各種モデル式を用いてゲインを決定し、各種偏差及び各種偏差積分値に決定したゲインをかけた後に加算することにより各種制御値を算出することを特徴とする。

また、本発明の自律飛行制御装置は、離陸制御時または着陸制御時においては、各種制御部の構成や値について変更や抑制等せず、各種制御部で算出する制御値が、設定した各種制限値を超えているか否かについての判定処理を行い、超えている場合には、各種制御値と制限値との偏差を算出し、アンチワインドアップ処理に用いるフィードバックゲインをかけてフィードバックし、一方、超えていない場合には、算出する各種制御値を出力するアンチワインドアップ処理部を備えることを特徴とする。

本発明の自律飛行制御装置によれば、鉛直方向においては高度を用いた演算を行わずに鉛直方向制御値を出力し、一方、水平方向においては水平位置を用いた演算を行わずに水平方向制御値を出力するので、不連続的に検出される高度や水平位置の影響を受けず、離陸制御時または着陸制御時に安定した制御を行うことができる。

また、本発明の自律飛行制御装置を搭載した小型無人ヘリコプタによれば、ヘリコプタ機体は、離陸開始時から着陸終了時まで完全な自律飛行を実現し、離着陸制御時には安定した制御を行うことができる。

以下、本発明を実施するための最良の形態について説明する。図１はヘリコプタ機体に搭載する自律飛行制御装置を示したものである。自律飛行制御装置１００は、その構成を大きく分けると、鉛直方向について制御を行う鉛直方向制御部１１０と、水平方向について制御を行う水平方向制御部１２０と、鉛直方向制御部１１０及び水平方向制御部１２０を実行する各種制御プログラムを管理するプログラム格納部１３０とからなる。

鉛直方向制御部１１０は、現在の高度、及び所定の高度目標値を基に高度制御値（鉛直速度目標値）を算出する高度制御部１１１と、現在の鉛直速度、及び算出する高度制御値（鉛直速度目標値）または所定の鉛直速度目標値を基に鉛直速度制御値を算出する鉛直速度制御部１１２とを備え、鉛直速度制御値を鉛直方向制御値として出力する。舵角駆動用アクチュエータは鉛直方向制御値に従ってコレクティブピッチ舵角を制御することにより、機体の鉛直方向を制御する。

水平方向制御部１２０は、現在の水平位置、及び所定の水平位置目標値を基に水平位置制御値（水平速度目標値）を算出する水平位置制御部１２１と、現在の水平速度、及び算出する水平速度制御値（水平速度目標値）または所定の水平速度目標値を基に水平速度制御値（姿勢角目標値）を算出する水平速度制御部１２２と、現在の姿勢角、及び算出する姿勢角目標値を基に姿勢角制御値を算出する姿勢角制御部１２３とを備え、姿勢角制御値を水平方向制御値として出力する。舵角駆動用アクチュエータは水平方向制御値に従ってサイクリックピッチ舵角を制御することにより、機体の水平方向を制御する。

また、プログラム格納部１３０は、離陸制御時の制御に用いる離陸制御プログラム、着陸制御時の制御に用いる着陸制御プログラム、及び自律飛行制御時の制御に用いる所定の飛行パターンデータに基づく自律飛行制御プログラムを管理する。

すなわち、自律飛行制御装置１００は、地上操作員からの指令、あるいはヘリコプタ機体に搭載しているＧＰＳセンサ及び慣性センサ等の各種センサがヘリコプタ機体の現状態等を検出することにより、それに応じて各種制御プログラムを実行し、各種制御プログラムに従って鉛直方向制御値、及び水平方向制御値を算出している。

また、自律飛行制御装置１００は、離陸制御時または着陸制御時、自律飛行制御時において、各種制御部の構成や値について設定しており、各種プログラムは、離陸制御や着陸制御の開始、あるいは自律飛行制御の開始等の検出に従って切り換わる。

具体的には、鉛直方向制御部１１０において、離陸制御時または着陸制御時では、高度制御部１１１は用いず、鉛直速度制御部１１２のみを用いる。この時、鉛直速度制御部１１２は、現在の鉛直速度、及び離陸制御時または着陸制御時に用いる所定の鉛直速度目標値を入力して鉛直速度制御値を算出する。

また、鉛直方向制御部１１０において、自律飛行制御時では、高度制御部１１１及び鉛直速度制御部１１２を用いる。この時、高度制御部１１１は、現在の高度及び自律飛行制御時に用いる所定の高度目標値を入力して高度制御値（鉛直速度目標値）を算出し、鉛直速度制御部１１２は、現在の鉛直速度及び高度制御部１１１が算出する高度制御値（鉛直速度目標値）を入力して鉛直速度制御値を算出する。

一方、水平方向制御部１２０において、離陸制御時または着陸制御時では、水平位置制御部１２１は用いず、水平速度制御部１２２及び姿勢角制御部１２３を用いる。この時、水平速度制御部１２２は、現在の水平速度、及び離陸制御時または着陸制御時に用いる所定の水平速度目標値を入力して水平速度制御値（姿勢角目標値）を算出する。姿勢角制御部１２３は、現在の姿勢角及び水平速度制御部１２２が算出する水平速度制御値（姿勢角目標値）を入力して姿勢角制御値を算出する。

また、水平方向制御部１２０において、自律飛行制御時では、水平位置制御部１２１、水平速度制御部１２２及び姿勢角制御部１２３を用いる。この時、水平位置制御部１２１は、現在の水平位置及び自律飛行制御時に用いる所定の水平位置目標値を入力して水平位置制御値（水平速度目標値）を算出し、水平速度制御部１２２は、現在の水平速度及び水平位置制御部１２１が算出する水平位置制御値（水平速度目標値）を入力して水平速度制御値を算出し、姿勢角制御部１２３は、現在の姿勢角及び水平速度制御部１２２が算出する水平速度制御値（姿勢角目標値）を入力して姿勢角制御値を算出する。

また、自律飛行制御装置１００は、鉛直速度制御部１１２において算出する鉛直速度制御値、水平速度制御部１２２において算出する水平速度制御値（姿勢角目標値）、及び姿勢角制御部１２３において算出する姿勢角制御値について、出力閾値である制限値を超えているか否かについての判定処理を行い、各種制限値以下の制御値を出力するアンチワインドアップ処理を行う。

具体的には、鉛直速度制御部１１２は、「鉛直速度制御値」の出力閾値である「鉛直速度に関する制限値」について、離陸制御時または着陸制御時と、自律飛行制御時とで異なる制限値を設定している（表１参照）。「鉛直速度に関する制限値」は過大な鉛直速度制御値の出力を防止するために設定された値であり、自律飛行制御時よりも離着陸制御時の方を大きく設定している。これはブレードのコレクティブピッチを下げることを考慮したものである。鉛直速度制御部１１２では、算出する「鉛直速度制御値」が「鉛直速度に関する制限値（表１参照）」を超えているか否かについての判定処理を行い、超えている場合には、「鉛直速度制御値」と「鉛直速度に関する制限値」との偏差を算出し、アンチワインドアップ処理に用いるフィードバックゲインをかけてフィードバックし、一方、超えていない場合には、算出する「鉛直速度制御値」を鉛直方向制御値として出力するアンチワインドアップ処理を行う。

また、水平速度制御部１２２は、「水平速度制御値（姿勢角目標値）」の出力閾値である「水平速度に関する制限値」について、離陸制御時または着陸制御時と、自律飛行制御時とで異なる制限値を設定している（表２参照）。「水平速度に関する制限値」は過大な水平速度制御値（姿勢角目標値）の出力を防止するために設定された値であり、自律飛行制御時よりも離着陸制御時の方を小さく設定している。これは離陸制御時または着陸制御時の機体の姿勢を安定した状態に維持することを考慮したものである。水平速度制御部１２２では、算出する「水平速度制御値（姿勢角目標値）」が「水平速度に関する制限値（表２参照）」を超えているか否かについての判定処理を行い、超えている場合には、「水平速度制御値（姿勢角目標値）」と「水平速度に関する制限値」との偏差を算出し、アンチワインドアップ処理に用いるフィードバックゲインをかけてフィードバックし、一方、超えていない場合には、算出する「水平速度制御値」を姿勢角制御部１２３へ出力するアンチワインドアップ処理を行う。

さらに、姿勢角制御部１２３は、「姿勢角制御値」の出力閾値である「姿勢角に関する制限値」について、離陸制御時または着陸制御時と、自律飛行制御時とで異なる制限値を設定している（表３参照）。「姿勢角に関する制限値」は過大な姿勢角制御値の出力を防止するために設定された値であり、自律飛行制御時よりも離着陸制御時の方を小さく設定している。これは、離陸制御時または着陸制御時の機体の姿勢を安定した状態に維持することを考慮したものである。姿勢角制御部１２３では、算出する「姿勢角制御値」が「姿勢角に関する制限値（表３参照）」を超えているか否かについての判定処理を行い、超えている場合には、「姿勢角制御値」と「姿勢角に関する制限値」との偏差を算出し、アンチワインドアップ処理に用いるフィードバックゲインをかけてフィードバックし、一方、超えていない場合には、算出する「姿勢角制御値」を水平方向制御値として出力するアンチワインドアップ処理を行う。

つづいて、各種制御部について説明する。図２は高度制御部１１１の内部構成をブロック図で示したものである。高度制御部１１１は、ヘリコプタ機体に搭載しているＧＰＳセンサが検出する現在の高度と、自律飛行制御時に用いる所定の高度目標値との偏差（高度偏差）を算出する高度偏差算出部２０１と、与えられた高度目標値に到達するよう、ヘリコプタ機体の動特性を基にした高度を制御する高度制御モデル式を用いてゲインを決定し、「高度偏差」に決定したゲインをかけることにより「高度制御値（鉛直速度目標値）」を算出するゲイン処理部２０２を備え、「高度制御値（鉛直速度目標値）」を鉛直速度制御部１１２へ出力する。ここで、高度制御モデル式は、以下に示すモデル式を用いる。

図３は鉛直速度制御部１１２の内部構成をブロック図で示したものである。鉛直速度制御部１１２は、ヘリコプタ機体に搭載しているＧＰＳセンサが検出する現在の鉛直速度と、自律飛行制御時に高度制御部１１１で算出する「高度制御値（鉛直速度目標値）」あるいは離陸制御時または着陸制御時に用いる「所定の鉛直速度目標値」との偏差（鉛直速度偏差）を算出する鉛直速度偏差算出部３０１と、この鉛直速度偏差の積分値を算出する鉛直速度偏差積分値算出部３０２と、与えられた鉛直速度目標値に到達するよう、ヘリコプタ機体の動特性を基にした鉛直速度を制御する鉛直速度制御モデル式を用いてゲインを決定し、「鉛直速度偏差」及び「鉛直速度偏差積分値」に決定したゲインをかけた後に加算することにより「鉛直速度制御値」を算出するゲイン処理部３０３と、ゲイン処理部３０３で算出する「鉛直速度制御値」が「鉛直速度に関する制限値（表１参照）」を超えているか否かについての判定処理を行い、超えている場合には、「鉛直速度制御値」と「鉛直速度に関する制限値」との偏差を算出し、アンチワインドアップ処理に用いるフィードバックゲインをかけて鉛直速度偏差算出部３０１へフィードバックし、一方、超えていない場合には、算出する「鉛直速度制御値」を鉛直方向制御値として出力するアンチワインドアップ処理部３０４とを備えている。ここで、鉛直速度制御モデル式は、以下に示すモデル式を用いる。

図４は水平位置制御部１２１の内部構成をブロック図で示したものである。水平位置制御部１２１は、ヘリコプタ機体に搭載しているＧＰＳセンサが検出する現在の水平位置と、自律飛行制御時に用いる所定の水平位置目標値との偏差（水平位置偏差）を算出する水平位置偏差算出部４０１と、与えられた水平位置目標値に到達するよう、ヘリコプタ機体の動特性を基にした水平位置を制御する水平位置制御モデル式を用いてゲインを決定し、「水平位置偏差」に決定したゲインをかけることにより「水平位置制御値（水平速度目標値）」を算出するゲイン処理部４０２を備え、「水平位置制御値（水平速度目標値）」を水平速度制御部１２２へ出力する。ここで、水平位置制御モデル式は、以下に示すモデル式を用いる。

図５は水平速度制御部１２２の内部構成をブロック図で示したものである。水平速度制御部１２２は、ヘリコプタ機体に搭載しているＧＰＳセンサが検出する現在の水平速度と、自律飛行制御時に水平位置制御部１２１で算出する「水平位置制御値（水平速度目標値）」あるいは離陸制御時または着陸制御時に用いる「所定の水平速度目標値」との偏差（水平速度偏差）を算出する水平速度偏差算出部５０１と、この水平速度偏差の積分値を算出する水平速度偏差積分値算出部５０２と、与えられた水平速度目標値に到達するよう、ヘリコプタ機体の動特性を基にした水平速度を制御する水平速度制御モデル式を用いてゲインを決定し、「水平速度偏差」及び「水平速度偏差積分値」に決定したゲインをかけた後に加算することにより「水平速度制御値（姿勢角目標値）」を算出するゲイン処理部５０３と、ゲイン処理部５０３で算出する「水平速度制御値（姿勢角目標値）」が「水平速度に関する制限値（表２参照）」を超えているか否かについての判定処理を行い、超えている場合には、「水平速度制御値（姿勢角目標値）」と「水平速度に関する制限値」との偏差を算出し、アンチワインドアップ処理に用いるフィードバックゲインをかけて水平速度偏差算出部５０１へフィードバックし、一方、超えていない場合には、算出する「水平速度制御値（姿勢角目標値）」を出力するアンチワインドアップ処理部５０４とを備えている。ここで、水平速度制御モデル式は、以下に示すモデル式を用いる。

図６は姿勢角制御部１２３の内部構成をブロック図で示したものである。姿勢角制御部１２３は、ヘリコプタ機体に搭載している慣性センサが検出する現在の姿勢角と、水平速度制御部１２２で算出する「水平速度制御値（姿勢角目標値）」との偏差（姿勢角偏差）を算出する姿勢角偏差算出部６０１と、この姿勢角偏差の積分値を算出する姿勢角偏差積分値算出部６０２と、与えられた「水平速度制御値（姿勢角目標値）」に到達するよう、ヘリコプタ機体の動特性を基にした姿勢角を制御する姿勢角制御モデル式を用いてゲインを決定し、「姿勢角偏差」及び「姿勢角偏差積分値」に決定したゲインをかけた後に加算することにより「姿勢角制御値」を算出するゲイン処理部６０３と、ゲイン処理部６０４で算出する「姿勢角制御値」が「姿勢角に関する制限値（表３参照）」を超えているか否かについての判定処理を行い、超えている場合には、「姿勢角制御値」と「姿勢角に関する制限値」との偏差を算出し、アンチワインドアップ処理に用いるフィードバックゲインをかけて姿勢角偏差算出部６０１へフィードバックし、一方、超えていない場合には、算出する「姿勢角制御値」を水平方向制御値として出力するアンチワインドアップ処理部６０４とを備えている。ここで、姿勢角制御モデル式は、以下に示すモデル式を用いる。

以上のように構成された自律飛行制御装置をヘリコプタ機体に搭載したときの離陸開始時から着陸終了時までの全体の流れについて図７を用いて説明する。自律飛行制御装置１００は、地上操作員からの離陸指令を受信すると（ＳＴＡＲＴ→ステップ７１）、エンジンを始動し、回転数制御を行う（ステップ７２）。回転数制御は、ヘリコプタ機体に搭載している回転数計測センサによって計測される現在のエンジン回転数と予め設定されている回転数目標値との偏差にゲインをかけることで回転数制御値(スロットル信号)を算出し、前記回転数制御値をフィードバックすることによって現在のエンジン回転数を回転数目標値に常に追従させる制御である。ヘリコプタ機体に搭載している回転数計測センサが回転数目標値に到達したことを検出すると、自律飛行制御装置１００は、離陸制御プログラムを実行し、離陸制御時の鉛直方向制御及び水平方向制御を行う（ステップ７３）。これに従い、ヘリコプタ機体は、離陸開始点から離陸制御時の目標高度である自律飛行開始点まで上昇する。つづいて、ヘリコプタ機体に搭載しているＧＰＳセンサが自律飛行開始点を検出すると、自律飛行制御装置１００は、離陸制御プログラムの実行から自律飛行制御プログラムの実行に切り換え、自律飛行制御時の鉛直方向制御及び水平方向制御を行う（ステップ７４）。これに従い、ヘリコプタ機体は、一定時間のホバー飛行を行った後、所定の飛行パターンデータに従って自律飛行する。その後、ヘリコプタ機体は所定の着陸点の上空の着陸開始点まで移動する。つづいて、ヘリコプタ機体に搭載しているＧＰＳセンサが着陸開始点を検出すると、自律飛行制御装置１００は、自律飛行制御プログラムの実行から着陸制御プログラムの実行に切り換え、着陸制御時の鉛直方向制御及び水平方向制御を行う（ステップ７５）。これに従い、ヘリコプタ機体は、一定時間のホバー飛行を行った後、着陸開始点から着陸制御時の目標高度である着陸点まで下降する。その後、ヘリコプタ機体に搭載しているＧＰＳセンサが着陸点を検出すると、自律飛行制御装置１００は、着陸制御プログラムの実行を終了するとともに、ヘリコプタ機体のエンジンを停止する（ステップ７６，７７→ＥＮＤ）。上記ステップ７１乃至７０７に示すような流れにより、自律飛行制御装置１００を搭載したヘリコプタ機体は、離陸開始時から着陸終了時まで完全な自律飛行を実現し、離着陸制御時には安定した離着陸を行う。なお、機体が着陸したか否かについての判定、エンジンの停止等は、地上操作員が行っても良い。

次に、ステップ７３で実行する離陸制御プログラムの動作について図８を用いて説明する。はじめに、離陸制御時の鉛直方向制御について説明する。自律飛行制御装置１００は、離陸制御プログラムを実行すると、機体が離陸開始点から離陸制御時の目標高度である自律飛行開始点まで上昇する間、高度制御部１１１を用いずに鉛直速度制御部１１２のみを用いて鉛直方向制御を行う（ステップ８１ａ）。

具体的には、鉛直速度制御部１１２は、「ヘリコプタ機体に搭載しているＧＰＳセンサが検出する現在の鉛直速度」と「離陸制御時に用いる所定の鉛直速度目標値」との偏差を積分し（ステップ８２ａ）、また、「離陸制御時に用いる所定の鉛直速度目標値」に到達するよう、ヘリコプタ機体の動特性を基にした鉛直速度を制御する鉛直速度制御モデル式を用いてゲインを決定し、「鉛直速度偏差」及び「鉛直速度偏差積分値」に決定したゲインをかけた後に加算することにより「鉛直速度制御値」を算出する（ステップ８３ａ）。そして、算出する「鉛直速度制御値」が「離陸制御時の鉛直速度に関する制限値（表１参照）」を超えているか否かについての判定処理を行い、超えている場合には、算出する「鉛直速度制御値」と「鉛直速度に関する制限値」との偏差を算出し、アンチワインドアップ処理に用いるフィードバックゲインをかけてフィードバックし、一方、超えていない場合には、算出する「鉛直速度制御値」を鉛直方向制御値として出力するアンチワインドアップ処理を行う（ステップ８４ａ）。すなわち、鉛直速度制御部１１２で算出する「鉛直速度制御値」が過大となっても、「離陸制御時の鉛直速度に関する制限値」以上の「鉛直速度制御値」が鉛直方向制御値として出力されることはないので、安定した状態で機体を上昇させることができる。上記ステップ８１ａからステップ８４ａの処理を繰り返し行い、鉛直方向制御値に従って舵角駆動用アクチュエータがコレクティブ舵角を制御して機体の鉛直方向を制御することにより、機体は離陸開始点から自律飛行開始点まで安定した状態で上昇する。そして、ヘリコプタ機体に搭載しているＧＰＳセンサが自律飛行開始点を検出すると、自律飛行制御装置１００は、離陸制御プログラムの実行から自律飛行制御プログラムの実行に切り換える（ステップ８５ａ）。

一方、離陸制御時の水平方向制御について説明する。自律飛行制御装置１００は、離陸制御プログラムを実行すると、機体が離陸開始点から離陸制御時の目標高度である自律飛行開始点まで上昇する間、水平位置制御部１２１を用いず水平速度制御部１２２及び姿勢角制御部１２３を用いて水平方向制御を行う（ステップ８１ｂ）。

具体的には、水平速度制御部１２２は、「ヘリコプタ機体に搭載しているＧＰＳセンサが検出する現在の水平速度」と「離陸制御時に用いる所定の水平速度目標値」との偏差を積分し（ステップ８２ｂ）、また、「離陸制御時に用いる所定の水平速度目標値」に到達するよう、ヘリコプタ機体の動特性を基にした水平速度を制御する水平速度制御モデル式を用いてゲインを決定し、「水平速度偏差」及び「水平速度偏差積分値」に決定したゲインをかけた後に加算することにより「水平速度制御値（姿勢角目標値）」を算出する（ステップ８３ｂ）。そして、算出する「水平速度制御値（姿勢角目標値）」が「離陸制御時の水平速度に関する制限値（表２参照）」を超えているか否かについての判定処理を行い、超えている場合には、「水平速度制御値」と「水平速度に関する制限値」との偏差を算出し、アンチワインドアップ処理に用いるフィードバックゲインをかけてフィードバックし、一方、超えていない場合には、算出する「水平速度制御値（姿勢角目標値）」を出力するアンチワインドアップ処理を行う（ステップ８４ｂ）。すなわち、水平速度制御部１２２で算出する「水平速度制御値（姿勢角目標値）」が過大となっても、「離陸制御時の水平速度に関する制限値」以上の「水平速度制御値（姿勢角目標値）」が出力されることはない。

つづいて、姿勢角制御部１２３は、「ヘリコプタ機体に搭載している慣性センサが検出する現在の姿勢角」と「水平速度制御値（姿勢角目標値）」との偏差を積分し（ステップ８５ｂ）、また、「水平速度制御値（姿勢角目標値）」に到達するよう、ヘリコプタ機体の動特性を基にした姿勢角を制御する姿勢角制御モデル式を用いてゲインを決定し、「姿勢角偏差」及び「姿勢角偏差積分値」に決定したゲインをかけた後に加算することにより「姿勢角制御値」を算出する（ステップ８６ｂ）。そして、算出する「姿勢角制御値」が「離陸制御時の姿勢角に関する制限値（表３参照）」を超えているか否かについての判定処理を行い、超えている場合には、「姿勢角制御値」と「姿勢角に関する制限値」との偏差を算出し、アンチワインドアップ処理に用いるフィードバックゲインをかけてフィードバックし、一方、超えていない場合には、算出する「姿勢角制御値」を水平方向制御値として出力するアンチワインドアップ処理を行う（ステップ８７ｂ）。すなわち、姿勢角制御部１２３で算出する「姿勢角制御値」が過大となっても、「離陸制御時の姿勢角に関する制限値」以上の「姿勢角制御値」が水平方向制御値として出力されることはないので、機体が安定した状態を維持することができる。例えば、接地状態で機体の脚と地面との摩擦により機体が拘束されて姿勢角指令に追随できない場合には過大な制御値が算出されるが、これに制限値を設定してアンチワインドアップ処理を行うことにより、過大な水平方向制御値の出力を防止し、機体が転倒するのを防止している。上記ステップ８１ｂからステップ８７ｂの処理を繰り返し行い、水平方向制御値に従って舵角駆動用アクチュエータがサイクリックピッチ舵角を制御して機体の水平方向を制御することにより、機体は離陸開始点から自律飛行開始点まで安定した状態を維持する。

次に、ステップ７４で実行する自律飛行制御プログラムの動作について図９を用いて説明する。はじめに、自律飛行制御時の鉛直方向制御について説明する。自律飛行制御装置１００は、自律飛行制御プログラムを実行すると、機体が自律飛行開始点から自律飛行制御時の目標高度である着陸開始点まで自律飛行する間、高度制御部１１１及び鉛直速度制御部１１２を用いて鉛直方向制御を行う（ステップ９１ａ）。

具体的には、高度制御部１１１は、「ヘリコプタ機体に搭載しているＧＰＳセンサが検出する現在の高度」と「自律飛行制御時に用いる所定の高度目標値」との偏差を算出し、また、「自律飛行制御時に用いる所定の高度目標値」に到達するよう、ヘリコプタ機体の動特性を基にした高度を制御する高度制御モデル式を用いてゲインを決定し、「高度偏差」に決定したゲインをかけることにより「高度制御値（鉛直速度目標値）」を算出する（ステップ９２ａ）。

つづいて、鉛直速度制御部１１２は、「ヘリコプタ機体に搭載しているＧＰＳセンサが検出する現在の鉛直速度」と「高度制御値（鉛直速度目標値）」との偏差を積分し（ステップ９３ａ）、また、「高度制御値（鉛直速度目標値）」に到達するよう、ヘリコプタ機体の動特性を基にした鉛直速度を制御する鉛直速度制御モデル式を用いてゲインを決定し、「鉛直速度偏差」及び「鉛直速度偏差積分値」に決定したゲインをかけた後に加算することにより「鉛直速度制御値」を算出する（ステップ９４ａ）。そして、算出する「鉛直速度制御値」が「自律飛行制御時の鉛直速度に関する制限値（表１参照）」を超えているか否かについての判定処理を行い、超えている場合には、算出する「鉛直速度制御値」と「鉛直速度に関する制限値」との偏差を算出し、アンチワインドアップ処理に用いるフィードバックゲインをかけてフィードバックし、一方、超えていない場合には、算出する「鉛直速度制御値」を鉛直方向制御値として出力するアンチワインドアップ処理を行う（ステップ９５ａ）。すなわち、鉛直速度制御部１１２で算出する「鉛直速度制御値」が過大となっても、「自律飛行制御時の鉛直速度に関する制限値」以上の「鉛直速度制御値」が鉛直方向制御値として出力されることはないので、安定した状態で機体を自律飛行させることができる。上記ステップ９１ａからステップ９５ａの処理を繰り返し行い、鉛直方向制御値に従って舵角駆動用アクチュエータがコレクティブ舵角を制御して機体の鉛直方向を制御することにより、機体は自律飛行開始点から着陸開始点まで所定の飛行パターンデータに従って安定した状態で自律飛行する。そして、ヘリコプタ機体に搭載しているＧＰＳセンサが着陸開始点を検出すると、自律飛行制御装置１００は自律飛行制御プログラムの実行から着陸制御プログラムの実行に切り替える（ステップ９６ａ）

一方、自律飛行制御時の水平方向制御について説明する。自律飛行制御装置１００は、自律飛行制御プログラムを実行すると、機体が自律飛行開始点から着陸開始点まで自律飛行する間、水平位置制御部１２１、水平速度制御部１２２及び姿勢角制御部１２３を用いて水平方向制御を行う（ステップ９１ｂ）。

具体的には、水平位置制御部１２１は、「ヘリコプタ機体に搭載しているＧＰＳセンサが検出する現在の水平位置」と「自律飛行制御時に用いる所定の水平位置目標値」との偏差を算出し、また、「自律飛行制御時に用いる所定の水平位置目標値」に到達するよう、ヘリコプタ機体の動特性を基にした水平位置を制御する水平位置制御モデル式を用いてゲインを決定し、「水平位置偏差」に決定したゲインをかけることにより「水平位置制御値（水平速度目標値）」を算出する（ステップ９２ｂ）。

つづいて、水平速度制御部１２２は、「ヘリコプタ機体に搭載しているＧＰＳセンサが検出する現在の水平速度」と「水平位置制御値（水平速度目標値）」との編纂を積分し（ステップ９３ｂ）、また、「水平位置制御値（水平速度目標値）」に到達するよう、ヘリコプタ機体の動特性を基にした水平速度を制御する水平速度制御モデル式を用いてゲインを決定し、「水平速度偏差」及び「水平速度偏差積分値」に決定したゲインをかけた後に加算することにより「水平速度制御値（姿勢角目標値）」を算出する（ステップ９４ｂ）。そして、算出する「水平速度制御値（姿勢角目標値）」が「自律飛行制御時の水平速度に関する制限値（表２参照）」を超えているか否かについての判定処理を行い、超えている場合には、算出する「水平速度制御値（姿勢角目標値）」と「水平速度に関する制限値」との偏差を算出し、アンチワインドアップ処理に用いるフィードバックゲインをかけてフィードバックし、一方、超えていない場合には、算出する「水平速度制御値（姿勢角目標値）」を出力するアンチワインドアップ処理を行う（ステップ９５ｂ）。すなわち、水平速度制御部１１２で算出する「水平速度制御値（姿勢角目標値）」が過大となっても、「自律飛行制御時の水平速度に関する制限値」以上の「水平速度制御値（姿勢角目標値）」が出力されることはない。

つづいて、姿勢角制御部１２３は、「ヘリコプタ機体に搭載している慣性センサが検出する現在の姿勢角」と「水平速度制御値（姿勢角目標値）」との偏差を積分し（ステップ９６ｂ）、また、「水平速度制御値（姿勢角目標値）」に到達するよう、ヘリコプタ機体の動特性を基にした姿勢角を制御する姿勢角制御モデル式を用いてゲインを決定し、「姿勢角偏差」及び「姿勢角偏差積分値」に決定したゲインをかけた後に加算することにより「姿勢角制御値」を算出する（ステップ９７ｂ）。そして、算出する「姿勢角制御値」が「自律飛行制御時の姿勢角に関する制限値（表３参照）」を超えているか否かについての判定処理を行い、超えている場合には、「姿勢角制御値」と「姿勢角に関する制限値」との偏差を算出し、アンチワインドアップ処理に用いるフィードバックゲインをかけてフィードバックし、一方、超えていない場合には、算出する「姿勢角制御値」を水平方向制御値として出力するアンチワインドアップ処理を行う（ステップ９８ｂ）。すなわち、姿勢角制御部１２３で算出する「姿勢角制御値」が過大となっても、「自律飛行制御時の姿勢角に関する制限値」以上の「姿勢角制御値」が水平方向制御値として出力されることはないので、機体が安定した状態を維持することができる。

次に、ステップ７５で実行する着陸制御プログラムの動作について図１０を用いて説明する。はじめに、着陸制御時の鉛直方向制御について説明する。自律飛行制御装置１００は、着陸制御プログラムを実行すると、機体が着陸開始点から着陸制御時の目標高度である着陸点まで下降する間、高度制御部１１１を用いずに鉛直速度制御部１１２のみを用いて鉛直方向制御を行う（ステップ１０１ａ）。

具体的には、鉛直速度制御部１１２は、「ヘリコプタ機体に搭載しているＧＰＳセンサが検出する現在の鉛直速度」と「着陸制御時に用いる所定の鉛直速度目標値」との偏差を積分し（ステップ１０２ａ）、また、「着陸制御時に用いる所定の鉛直速度目標値」に到達するよう、ヘリコプタ機体の動特性を基にした鉛直速度を制御する鉛直速度制御モデル式を用いてゲインを決定し、「鉛直速度偏差」及び「鉛直速度偏差積分値」に決定したゲインをかけた後に加算することにより「鉛直速度制御値」を算出する（ステップ１０３ａ）。そして、算出する「鉛直速度制御値」が「着陸制御時の鉛直速度に関する制限値（表１参照）」を超えているか否かについての判定処理を行い、超えている場合には、算出する「鉛直速度制御値」と「鉛直速度に関する制限値」との偏差を算出し、アンチワインドアップ処理に用いるフィードバックゲインをかけてフィードバックし、一方、超えていない場合には、算出する「鉛直速度制御値」を鉛直方向制御値として出力するアンチワインドアップ処理を行う（ステップ１０４ａ）。すなわち、鉛直速度制御部１１２で算出する「鉛直速度制御値」が過大となっても、「着陸制御時の鉛直速度に関する制限値」以上の「鉛直速度制御値」が鉛直方向制御値として出力されることはないので、安定した状態で機体を下降させることができる。上記ステップ１０１ａからステップ１０４ａの処理を繰り返し行い、鉛直方向制御値に従って舵角駆動用アクチュエータがコレクティブピッチ舵角を制御して機体の鉛直方向を制御することにより、機体は着陸開始点から着陸点まで安定した状態で下降する。そして、ヘリコプタ機体に搭載しているＧＰＳセンサが着陸点を検出すると、自律飛行制御装置１００は、着陸制御プログラムの実行を終了する（ステップ１０５ａ）。

一方、着陸制御時の水平方向制御について説明する。自律飛行制御装置１００は、着陸制御プログラムを実行すると、機体が着陸開始点から着陸制御時の目標高度である着陸点まで下降する間、水平位置制御部１２１を用いずに水平速度制御部１２２及び姿勢角制御部１２３を用いて水平方向制御を行う（ステップ１０１ｂ）。

具体的には、水平速度制御部１２２は、「ヘリコプタ機体に搭載しているＧＰＳセンサが検出する現在の水平速度」と「着陸制御時に用いる所定の水平速度目標値」との偏差を積分し（ステップ１０２ｂ）、また、「着陸制御時に用いる所定の水平速度目標値」に到達するよう、ヘリコプタ機体の動特性を基にした水平速度を制御する水平速度制御モデル式を用いてゲインを決定し、「水平速度偏差」及び「水平速度偏差積分値」に決定したゲインをかけた後に加算することにより「水平速度制御値（姿勢角目標値）」を算出する（ステップ１０３ｂ）。そして、算出する「水平速度制御値（姿勢角目標値）」が「着陸制御時の水平速度に関する制限値（表２参照）」を超えているか否かについての判定処理を行い、超えている場合には、「水平速度制御値」と「水平速度に関する制限値」との偏差を算出し、アンチワインドアップ処理に用いるフィードバックゲインをかけてフィードバックし、一方、超えていない場合には、算出する「水平速度制御値（姿勢角目標値）」を出力するアンチワインドアップ処理を行う（ステップ１０４ｂ）。すなわち、水平速度制御部１２２で算出する「水平速度制御値（姿勢角目標値）」が過大となっても、「着陸制御時の水平速度に関する制限値」以上の「水平速度制御値（姿勢角目標値）」が出力されることはない。

つづいて、姿勢角制御部１２３は、「ヘリコプタ機体に搭載している慣性センサが検出する現在の姿勢角」と「水平速度制御値（姿勢角目標値）」との偏差を積分し（ステップ１０５ｂ）、また、「水平速度制御値（姿勢角目標値）」に到達するよう、ヘリコプタ機体の動特性を基にした姿勢角を制御する姿勢角制御モデル式を用いてゲインを決定し、「姿勢角偏差」及び「姿勢角偏差積分値」に決定したゲインをかけた後に加算することにより「姿勢角制御値」を算出する（ステップ１０６ｂ）。そして、算出する「姿勢角制御値」が「着陸制御時の姿勢角に関する制限値（表３参照）」を超えているか否かについての判定処理を行い、超えている場合には、「姿勢角制御値」と「姿勢角に関する制限値」との偏差を算出し、アンチワインドアップ処理に用いるフィードバックゲインをかけてフィードバックし、一方、超えていない場合には、算出する「姿勢角制御値」を水平方向制御値として出力するアンチワインドアップ処理を行う（ステップ１０７ｂ）。すなわち、姿勢角制御部１２３で算出する「姿勢角制御値」が過大となっても、「着陸制御時の姿勢角に関する制限値」以上の「姿勢角制御値」が水平方向制御値として出力されることはないので、機体が安定した状態を維持することができる。例えば、接地状態で機体の脚と地面との摩擦により機体が拘束されて姿勢角指令に追随できない場合には過大な制御値が算出されるが、これに制限値を設定してアンチワインドアップ処理を行うことにより、過大な水平方向制御値の出力を防止し、機体が転倒するのを防止している。上記ステップ１０１ｂからステップ１０７ｂの処理を繰り返し行い、水平方向制御値に従って舵角駆動用アクチュエータがサイクリックピッチ舵角を制御して機体の水平方向を制御することにより、機体は着陸開始点から着陸点まで安定した状態を維持する。

以上の説明から明らかなように、本実施の形態の自律飛行制御装置１００は、であり、
これをヘリコプタ機体に搭載する。自律飛行制御装置１００は、ヘリコプタ機体に搭載する形態が基本であるが、他の装置にも必要に応じて適用することも可能である。ヘリコプタ機体に搭載するその他の構成には、自律飛行制御装置１００から出力される制御値を制御信号に変換する制御信号処理部、制御信号に従って駆動するサーボモータ、及びヘリコプタ機体の飛行状態を検出するＧＰＳセンサや慣性センサ等の各種センサがある。なお、自律飛行制御装置１００の一部あるいは全部を地上局に備える形態もある。地上局に自律飛行制御装置１００の一部あるいは全部を備える場合はヘリコプタ機体との間で無線通信を行うのでヘリコプタ機体には無線部を備える必要がある。

なお、本実施の形態の自律飛行制御装置１００は、離着陸制御時や自律飛行制御時に地上操作員が無線遠隔操作を行わず、離陸制御時から着陸制御時まで完全なヘリコプタ機体の自律飛行を実現することができるものであるが、オペレータアシストモード機能としても良い。すなわち、自律飛行制御装置１００の鉛直方向制御部１１０あるいは水平方向制御部１２０に与えられる各種所定の目標値に変えて、図２１に示すようなコントローラをオペレータが操作することにより、各種目標値を与えても良い。上記の説明からも明らかなように、自律飛行制御装置１００の各種制御部において算出する制御値が各種制御部の出力閾値である制限値を超えているか否かについての判定処理をし、制限値以下の制御値を出力するアンチワインドアップ処理を行うので、過大な制御値が出力されることはなく、素人でも簡単に小型無人ヘリコプタを操作できる。

図１１は自律飛行制御時の鉛直方向制御部１１０の閉ループを示している。また、図１２は、離着陸制御時の鉛直方向制御部１１０の閉ループを示している。なお、各ブロックは図１１に示されているものと同様である。図１３は、離着陸制御時の水平方向制御部１１０の閉ループを示している。また、図１４は、アンチワインドアップ処理部６０４を有する姿勢角制御部１２３の閉ループを示している。なお、本実施の形態では、鉛直速度制御部１１２、水平速度制御部１２２、姿勢角制御部１２３にアンチワインドアップ処理部を備えているが、必要に応じて高度制御部１１１や水平位置制御部１２１にアンチワインドアップ処理部を備えてもよい。図１５は、高度制御実験の結果を示している。点線で示されているのは高度目標値であり、実線で示されているのは、ＧＰＳセンサにより検出された高度である。図に示すように、機体は高度目標値に追従していることがわかる。

また、図１６は、１０秒付近で離陸し、３５秒付近で着陸する実験を行ったときの鉛直速度制御の結果を示している。点線で示されているのは鉛直速度目標値であり、実線で示されているのは、ＧＰＳセンサにより検出された鉛直速度である。図に示すように、機体は鉛直速度目標値に追従していることがわかる。図１７は、鉛直方向に関する実験結果を示しており、それぞれ高度データ(最上段)、鉛直速度データ(中段)、鉛直方向制御値(下段)を示している。図１８は、離着陸時における水平方向に関する実験結果を示しており、それぞれヘリコプタ機体前後方向の速度と左右方向の速度の値を示している。また、図１９は、ピッチ運動に関する実験データを示している。図２０は、ロール運動に関する実験データを示している。

本発明の一実施の形態で用いる自律飛行制御装置の内部構成を示したブロック図 高度制御部の内部構成を示したブロック図 鉛直速度制御部の内部構成を示したブロック図 水平位置制御部の内部構成を示したブロック図 水平速度制御部の内部構成を示したブロック図 姿勢角制御部の内部構成を示したブロック図 離陸開始時から着陸終了時までの全体の流れを示したフロー図 離陸制御プログラムを実行したときの自律飛行制御装置の動作を示したフロー図 自律飛行制御プログラムを実行したときの自律飛行制御装置の動作を示したフロー図 着陸制御プログラムを実行したときの自律飛行制御装置の動作を示したフロー図 自律飛行制御時の鉛直方向制御部の閉ループを示した図 離着陸制御時の鉛直方向制御部の閉ループを示した図 離着陸制御時の水平方向制御部の閉ループを示した図 アンチワインドアップ処理部を有する姿勢角制御部の閉ループを示した図 高度制御実験の結果を示した図 １０秒付近で離陸し、３５秒付近で着陸する実験を行ったときの鉛直速度制御の結果を示した図 鉛直方向に関する実験結果を示した図 水平方向に関する実験結果を示した図 ピッチ運動に関する実験データを示した図 ロール運動に関する実験データを示した図 オペレータアシストモードに用いるコントローラを示した図 特許文献２の自律飛行制御装置の内部構成を示したブロック図 特許文献２の高度制御部の内部構成を示したブロック図 特許文献２の水平位置制御部の内部構成を示したブロック図 特許文献２の姿勢角制御部の内部構成を示したブロック図 特許文献２の離陸制御プログラムを実行したときの自律飛行制御装置の動作を示したフロー図 特許文献２の着陸制御プログラムを実行したときの自律飛行制御装置の動作を示したフロー図

符号の説明

１００ 自律飛行制御装置
１１０ 鉛直方向制御部
１１１ 高度制御部
１１２ 鉛直速度制御部
１２０ 水平方向制御部
１２１ 水平位置制御部
１２２ 水平速度制御部
１２３ 姿勢角制御部
２０１ 高度偏差算出部
２０２ ゲイン処理部
３０１ 鉛直速度偏差算出部
３０２ 鉛直速度偏差積分値算出部
３０３ ゲイン処理部
３０４ アンチワインドアップ処理部
４０１ 水平位置偏差算出部
４０２ ゲイン処理部
５０１ 水平速度偏差算出部
５０２ 水平速度偏差積分値算出部
５０３ ゲイン処理部
５０４ アンチワインドアップ処理部
６０１ 姿勢角偏差算出部
６０２ 姿勢角偏差積分値算出部
６０３ ゲイン処理部
６０４ アンチワインドアップ処理部

Claims (17)

1. 現在の鉛直速度、及び離陸制御時または着陸制御時に用いる所定の鉛直速度目標値を基に鉛直速度制御値を算出する鉛直速度制御部を有する鉛直方向制御部と、
   現在の水平速度、及び所定の水平速度目標値を基に水平速度制御値を算出する水平速度制御部と、現在の姿勢角、及び前記水平速度制御値を基に姿勢角制御値を算出する姿勢角制御部とを有する水平方向制御部とを備え、
   前記鉛直速度制御部が算出する鉛直速度制御値を鉛直方向制御値として出力し、
   前記姿勢角制御部が算出する姿勢角制御値を水平方向制御値として出力するとともに、
   前記鉛直速度制御部は、
   現在の鉛直速度と、離陸制御時または着陸制御時に用いる所定の鉛直速度目標値との鉛直速度偏差を算出する鉛直速度偏差算出部と、
   前記鉛直速度偏差の積分値を算出する鉛直速度偏差積分値算出部と、
   ヘリコプタ機体の動特性を基にした鉛直速度を制御する鉛直速度制御モデル式を用いてゲインを決定し、前記鉛直速度偏差及び前記鉛直速度偏差積分値に決定したゲインをかけた後に加算することにより鉛直速度制御値を算出するゲイン処理部とを備え、
   前記鉛直速度制御モデル式は、
   

   

   を用いることを特徴とする自律飛行制御装置。
2. 前記ゲイン処理部で算出する鉛直速度制御値が鉛直速度に関する制限値を超えているか否かについての判定処理を行い、超えている場合には、鉛直速度制御値と鉛直速度に関する制限値との偏差を算出し、アンチワインドアップ処理に用いるフィードバックゲインをかけて前記鉛直速度偏差算出部へフィードバックし、一方、超えていない場合には、算出する鉛直速度制御値を鉛直方向制御値として出力するアンチワインドアップ処理部を備えることを特徴とする請求項１に記載の自律飛行制御装置。
3. 前記水平速度制御部は、現在の水平速度と、離陸制御時または着陸制御時に用いる所定の水平速度目標値との水平速度偏差を算出する水平速度偏差算出部と、前記水平速度偏差の積分値を算出する水平速度偏差積分値算出部と、ヘリコプタ機体の動特性を基にした水平速度を制御する水平速度制御モデル式を用いてゲインを決定し、前記水平速度偏差及び前記水平速度偏差積分値に決定したゲインをかけた後に加算することにより水平速度制御値を算出するゲイン処理部とを備えることを特徴とする請求項１に記載の自律飛行制御装置。
4. 前記水平速度制御モデル式は、
   

   

   を用いることを特徴とする請求項３に記載の自律飛行制御装置。
5. 前記ゲイン処理部で算出する水平速度制御値が水平速度に関する制限値を超えているか否かについての判定処理を行い、超えている場合には、水平速度制御値と水平速度に関する制限値との偏差を算出し、アンチワインドアップ処理に用いるフィードバックゲインをかけて前記水平速度偏差算出部へフィードバックし、一方、超えていない場合には、算出する水平速度制御値を前記姿勢角制御部へ出力するアンチワインドアップ処理部を備えることを特徴とする請求項３又は４に記載の自律飛行制御装置。
6. 前記姿勢角制御部は、現在の姿勢角と、前記水平速度制御部で算出する水平速度制御値を用いて姿勢角偏差を算出する姿勢角偏差算出部と、前記姿勢角偏差の積分値を算出する姿勢角偏差積分値算出部と、ヘリコプタ機体の動特性を基にした姿勢角を制御する姿勢角制御モデル式を用いてゲインを決定し、前記姿勢角偏差及び前記姿勢角偏差積分値に決定したゲインをかけた後に加算することにより姿勢角制御値を算出するゲイン処理部とを備えることを特徴とする請求項１に記載の自律飛行制御装置。
7. 前記姿勢角制御モデル式は、
   

   

   を用いることを特徴とする請求項６に記載の自律飛行制御装置。
8. 前記ゲイン処理部で算出する姿勢角制御値が姿勢角に関する制限値を超えているか否かについての判定処理を行い、超えている場合には、姿勢角制御値と姿勢角に関する制限値との偏差を算出し、アンチワインドアップ処理に用いるフィードバックゲインをかけて前記姿勢角偏差算出部へフィードバックし、一方、超えていない場合には、算出する姿勢角制御値を水平方向制御値として出力するアンチワインドアップ処理部を備えることを特徴とする請求項６又は７に記載の自律飛行制御装置。
9. 前記鉛直方向制御部は、現在の高度、及び自律飛行制御時に用いる所定の高度目標値を基に高度制御値を算出する高度制御部を備え、
   前記水平方向制御部は、現在の水平位置、及び所定の水平位置目標値を基に水平位置制御値を算出する水平位置制御部を備えることを特徴とする請求項１に記載の自律飛行制御装置。
10. 前記高度制御部は、現在の高度と、自律飛行制御時に用いる所定の高度目標値との高度偏差を算出する高度偏差算出部と、ヘリコプタ機体の動特性を基にした高度を制御する高度制御モデル式を用いてゲインを決定し、前記高度偏差に決定したゲインをかけることにより高度制御値を算出するゲイン処理部とを備え、前記高度制御値を前記鉛直速度制御部へ出力することを特徴とする請求項９に記載の自律飛行制御装置。
11. 前記高度制御モデル式は、
    

    

    を用いることを特徴とする請求項１０に記載の自律飛行制御装置。
12. 前記水平位置制御部は、現在の水平位置と、自律飛行制御時に用いる所定の水平位置目標値との水平位置偏差を算出する水平位置偏差算出部と、ヘリコプタ機体の動特性を基にした水平位置を制御する水平位置制御モデル式を用いてゲインを決定し、前記水平位置偏差に決定したゲインをかけることにより水平位置制御値を算出するゲイン処理部とを備え、前記水平位置制御値を前記水平速度制御部へ出力することを特徴とする請求項９に記載の自律飛行制御装置。
13. 前記水平位置制御モデル式は、
    

    

    を用いることを特徴とする請求項１２に記載の自律飛行制御装置。
14. 鉛直速度に関する制限値は、離陸あるいは着陸制御時と自律飛行制御時とで異なる値に設定することを特徴とする請求項２に記載の自律飛行制御装置。
15. 水平速度に関する制限値は、離陸あるいは着陸制御時と自律飛行制御時とで異なる値に設定することを特徴とする請求項５に記載の自律飛行制御装置。
16. 姿勢角に関する制限値は、離陸あるいは着陸制御時と自律飛行制御時とで異なる値に設定することを特徴とする請求項８に記載の自律飛行制御装置。
17. 現在の鉛直速度、及び離陸制御時または着陸制御時に用いる所定の鉛直速度目標値を基に鉛直速度制御値を算出する鉛直速度制御部を有する鉛直方向制御部と、
    現在の水平速度、及び所定の水平速度目標値を基に水平速度制御値を算出する水平速度制御部と、現在の姿勢角、及び前記水平速度制御値を基に姿勢角制御値を算出する姿勢角制御部とを有する水平方向制御部とを備え、
    前記鉛直速度制御部が算出する鉛直速度制御値を鉛直方向制御値として出力し、
    前記姿勢角制御部が算出する姿勢角制御値を水平方向制御値として出力するとともに、
    前記鉛直速度制御部は、
    現在の鉛直速度と、離陸制御時または着陸制御時に用いる所定の鉛直速度目標値との鉛直速度偏差を算出する鉛直速度偏差算出部と、
    前記鉛直速度偏差の積分値を算出する鉛直速度偏差積分値算出部と、
    ヘリコプタ機体の動特性を基にした鉛直速度を制御する鉛直速度制御モデル式を用いてゲインを決定し、前記鉛直速度偏差及び前記鉛直速度偏差積分値に決定したゲインをかけた後に加算することにより鉛直速度制御値を算出するゲイン処理部とを備え、
    前記鉛直速度制御モデル式は、
    

    

    を用いる自律飛行制御装置を搭載したことを特徴とする小型無人ヘリコプタ。

Images