特許 7204390 位置制御装置および飛しょう体

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】

(24)【登録日】2023-01-05

(45)【発行日】2023-01-16

(54)【発明の名称】位置制御装置および飛しょう体

(51)【国際特許分類】

   B64C 13/18 20060101AFI20230106BHJP

【ＦＩ】

B64C13/18 Z

【請求項の数】 4

(21)【出願番号】P 2018173721

(22)【出願日】2018-09-18

(65)【公開番号】P2020044920

(43)【公開日】2020-03-26

【審査請求日】2021-09-15

(73)【特許権者】

【識別番号】000003078

【氏名又は名称】株式会社東芝

(73)【特許権者】

【識別番号】598076591

【氏名又は名称】東芝インフラシステムズ株式会社

(74)【代理人】

【識別番号】110001634

【氏名又は名称】弁理士法人志賀国際特許事務所

(72)【発明者】

【氏名】近藤 宗輝

【審査官】伊藤 秀行

(56)【参考文献】

【文献】特開２００５－１１５６２３（ＪＰ，Ａ）

【文献】特開２００４－１３０８５２（ＪＰ，Ａ）

【文献】特開昭５８－０９６３０９（ＪＰ，Ａ）

【文献】特開２００１－３０６１４４（ＪＰ，Ａ）

【文献】特開２００２－３１８１００（ＪＰ，Ａ）

【文献】特表２０１３－５４０９８７（ＪＰ，Ａ）

【文献】特開２０１２－１０６５１５（ＪＰ，Ａ）

【文献】米国特許出願公開第２０１８／０１５５０２６（ＵＳ，Ａ１）

【文献】韓国登録特許第１０－１１４６７４８（ＫＲ，Ｂ１）

【文献】特開昭６３－２０１８０６（ＪＰ，Ａ）

【文献】特開平１１－２０１７００（ＪＰ，Ａ）

(58)【調査した分野】(Int.Cl.，ＤＢ名)

Ｂ６４Ｃ １３／１８

(57)【特許請求の範囲】

【請求項1】

飛しょう体の機体固定座標系における加速度を、慣性座標系における慣性座標系加速度に変換する慣性座標系変換部と、
前記慣性座標系変換部により変換された前記慣性座標系加速度を積分して、前記飛しょう体の推定速度を算出する速度算出部と、
前記速度算出部により算出された推定速度を積分して、前記飛しょう体の推定位置を導出する位置算出部と、
前記位置算出部により算出された推定位置が位置指令値となるよう制御するための速度指令値を求める位置制御コントローラと、
前記速度指令値と前記推定速度との差分である速度差分を算出する速度差分算出部と、
前記推定速度が前記速度指令値となるように制御する慣性座標系加速度指令値を前記速度差分から求める速度コントローラと、
前記慣性座標系加速度指令値を、前記機体固定座標系における加速度指令値に座標変換する機体固定座標系変換部と、
を備える位置制御装置。

【請求項2】

前記加速度指令値と前記加速度との加速度差分により求められた角速度指令値と、前記飛しょう体の機体固定座標系における角速度との差分を求め、前記角速度指令値となるよう飛しょう体の運動を制御するために操舵装置へ入力する舵角指令値を生成する角速度コントローラとを、更に備える、
請求項１に記載の位置制御装置。

【請求項3】

前記位置指令値を外部装置から受け付ける位置指令値受付部を、更に備える、
請求項１または２に記載の位置制御装置。

【請求項4】

飛しょう体の機体固定座標系における機体の加速度及び角速度を計測する慣性計測装置と、
前記加速度を機体固定座標系から慣性座標系に座標変換し、慣性座標系加速度とする慣性座標系変換部と、
前記慣性座標系変換部により変換された前記慣性座標系加速度を積分して、前記飛しょう体の推定速度を算出する速度算出部と、
前記速度算出部により算出された推定速度を積分して、前記飛しょう体の推定位置を導出する位置算出部と、
前記位置算出部により算出された推定位置と位置指令値の差分を用いて速度指令値を求める位置制御コントローラと、
前記速度指令値と前記推定速度との差分である速度差分を算出する速度差分算出部と、
前記推定速度を前記速度指令値となるように制御する慣性座標系加速度指令値を前記速度差分から求める速度コントローラと、
前記慣性座標系加速度指令値を、前記機体固定座標系における加速度指令値に座標変換する機体固定座標系変換部と、
前記加速度指令値と前記加速度との加速度差分を求め、前記加速度差分から角速度指令値を求める加速度コントローラと、
前記角速度指令値と前記角速度との差分を求め、舵角指令値を生成する角速度コントローラと、
前記機体の運動を前記舵角指令値により制御する舵角装置と、
を備える飛しょう体。

【発明の詳細な説明】

【技術分野】

【0001】

本発明の実施形態は、位置制御装置および飛しょう体に関する。

【背景技術】

【0002】

飛しょう体には、機体の状態を観測するために複数のセンサ装置が搭載されており、搭載された複数のセンサ装置を用いて飛行する際の機体の運動を制御している。上記複数のセンサ装置の中で一般的なセンサ装置として、慣性計測装置がある。慣性計測装置は、一般的に加速度センサ及び角速度センサから構成され、飛しょう体の機体に生じる角速度や加速度を計測する。

【0003】

また、飛しょう体の位置や高度を制御する場合、上記慣性計測装置以外に、電波高度計や速度計（ピトー管など）を機体に搭載する必要がある。例えば、飛しょう体をレベルフライトの状態で飛行させる場合、飛しょう体の高度を常に監視する必要がある。そのため、電波高度計を備えない場合、慣性制御装置から出力される加速度を積分処理することにより、飛しょう体の速度及び飛行している位置を算出することが行なわれている。飛しょう体にレベルフライトを行なわせる場合、慣性制御装置の出力する加速度から速度及び位置の推定を行なうことにより、電波高度計や速度計などを搭載する必要がなく、機体の重量の低下や製造原価の低減を行なうことができる。

【0004】

しかしながら、加速度データに積分処理を行って飛しょう体の自己位置を算出し、レベルフライトを含めた飛行の制御を行なう場合には、従来は慣性座標系における飛しょう軌道の経路情報を、予め詳細に設定しておく必要があった。そのため、煩雑な飛しょう軌道を導出する計算が必要であった。

【先行技術文献】

【特許文献】

【0005】

【文献】特開昭６３－２０１８０６号公報

【文献】特開昭５８－０９６３０９号公報

【発明の概要】

【発明が解決しようとする課題】

【0006】

本発明が解決しようとする課題は、電波高度計や速度センサを用いることなく、飛しょう体の高精度な位置制御を行うことができる位置制御装置および飛しょう体を提供することである。

【課題を解決するための手段】

【0007】

実施形態の位置制御装置は、慣性座標系変換部と、速度算出部と、位置算出部と、位置制御コントローラと、速度差分算出部と、速度コントローラと、機体固定座標系変換部とを持つ。慣性座標系変換部は、飛しょう体の機体固定座標系における加速度を機体固定座標系から慣性座標系に座標変換する。速度算出部は、前記慣性座標系変換部により変換された前記慣性座標系加速度を積分して、前記飛しょう体の推定速度を算出する。位置算出部は、前記速度算出部により算出された推定速度を積分して、前記飛しょう体の推定位置を導出する。位置制御コントローラは、前記位置算出部により算出された推定位置と位置指令値から速度指令値を求める。速度差分算出部は、前記速度指令値と前記推定速度との差分である速度差分を算出する。速度コントローラは、前記推定速度と前記速度指令値の差分から慣性座標系における加速度指令値を求める。機体固定座標系変換部は、前記慣性座標系における加速度指令値を、前記機体固定座標系における加速度指令値に座標変換する。

【図面の簡単な説明】

【0008】

【図1】実施形態の飛しょう体１００の構成例を示すブロック図。

【図2】慣性座標系と機体固定座標系との対応を説明する図。

【図3】実施形態の飛しょう体１００における慣性制御処理の動作例を説明するフローチャート。

【図4】実施形態の飛しょう体１００の飛行における位置制御処理の動作例を示す図。

【図5】実施形態の飛しょう体１００の飛行における位置制御処理の他の動作例を示す図。

【発明を実施するための形態】

【0009】

以下、実施形態の位置制御装置および飛しょう体を、図面を参照して説明する。
図１は、実施形態の飛しょう体１００の構成例を示すブロック図である。
飛しょう体１００は、例えば、位置制御装置１０と、慣性計測装置２０と、操舵装置３０とを備える。位置制御装置１０は、例えば、慣性座標系変換部１０２と、速度算出部１０３と、位置算出部１０４と、第１演算部１０６と、位置制御コントローラ１０７と、第２演算部１０８と、速度コントローラ１０９と、機体固定座標系変換部１１０と、第３演算部１１１と、加速度コントローラ１１２と、第４演算部１１３と、角速度コントローラ１１４とを備える。

【0010】

慣性計測装置２０は、飛しょう体１００の飛行における加速度Ａ及び角速度ＶＡの各々を、所定の周期Ｔ毎に計測し、慣性座標系変換部１０２、第３演算部１１１及び第４演算部１１３に出力する。ここで、加速度Ａ（Ａx，Ａy，Ａz）及び角速度ＶＡ（ＶＡx，ＶＡy，ＶＡz）の各々はベクトルで表すことができ、飛しょう体１００の機体に固定された座標系である機体固定座標系において定義される。

【0011】

慣性座標系変換部１０２は、機体固定座標系における加速度Ａ（Ａx，Ａy，Ａz）を慣性座標系に対応させるように、以下の（１）式を用いた座標変換を行う。

【0012】

【数1】

【0013】

（１）式の座標変換行列Ｔにおける角度Ψ、Φ及びΘの各々は、慣性座標系（または、地面固定座標系）と機体固定座標系との対応関係、すなわち機体固定座標系の軸の各々が、対応する慣性座標系の軸それぞれに対して有する角度を示している。図２は、慣性座標系と機体固定座標系との対応を説明する図である。慣性座標系は、地上の所定の位置に固定された座標系であり、Ｘ軸、Ｙ軸及びＺ軸から形成されている。飛しょう体１００は、慣性座標系の示す３次元空間を飛行する。機体固定座標系は、飛しょう体１００の機体に固定された座標系であり、ｘ軸、ｙ軸及びｚ軸から構成されている。また、ｘ軸はＸ軸に対応し、ｙ軸はＹ軸に対応し、ｚ軸はＺ軸に対応している。この対応関係より、角度Ψは、Ｘ軸とｘ軸との成す角度である。角度Φは、Ｙ軸とｙ軸との成す角度である。角度Θは、Ｚ軸とｚ軸との成す角度である。

【0014】

図１に戻り、慣性座標系変換部１０２は、上記（１）式により、機体固定座標系における加速度Ａ（Ａｘ，Ａｙ，Ａｚ）を、慣性座標系における加速度（ベクトル）Ａｅ（ＡｅＸ，ＡｅＹ，ＡｅＺ）に変換する。そして、慣性座標系変換部１０２は、（１）式により求めた加速度Ａｅ（ＡｅＸ，ＡｅＹ，ＡｅＺ）を速度算出部１０３に出力する。

【0015】

速度算出部１０３は、直前の周期に算出した推定速度（ベクトル）Ｖ（ＶＸ，ＶＹ，ＶＺ）に対して、加速度Ａｅ（ＡｅＸ，ＡｅＹ，ＡｅＺ）を周期Ｔにより積分して求められる変化速度（ベクトル）ΔＶ（ΔＶＸ，ΔＶＹ，ΔＶＺ）を加算して、現在の周期における推定速度Ｖ（ＶＸ，ＶＹ，ＶＺ）とする。そして、速度算出部１０３は、求めた推定速度Ｖ（ＶＸ，ＶＹ，ＶＺ）を位置算出部１０４に出力する。

【0016】

位置算出部１０４は、直前の周期に算出した推定位置Ｐ（ＰＸ，ＰＹ，ＰＺ）に対して、推定速度Ｖ（ＶＸ，ＶＹ，ＶＺ）を周期Ｔにより積分して求められる位置変化（ベクトル）ΔＰ（ΔＰＸ，ΔＰＹ，ΔＰＺ）を加算して、現在の周期における推定位置Ｐ（ＰＸ，ＰＹ，ＰＺ）を求める（導出する）。そして、位置算出部１０４は、求めた推定位置Ｐ（ＰＸ，ＰＹ，ＰＺ）を第１演算部１０６に出力する。

【0017】

第１演算部１０６は、位置指令値受付部として、あらかじめ設定されるもしくは外部装置（不図示）から供給される位置指令値Ｐｃｍｄ（ＰｃｍｄＸ，ＰｃｍｄＹ，ＰｃｍｄＺ）を受け付ける。また、第１演算部１０６は、受け付けた位置指令値Ｐｃｍｄ（ＰｃｍｄＸ，ＰｃｍｄＹ，ＰｃｍｄＺ）と、位置算出部１０４から供給される推定位置Ｐ（ＰＸ，ＰＹ，ＰＺ）との差分を求め、位置差分ΔＰ（ΔＰＸ，ΔＰＹ，ΔＰＺ）とする。そして、第１演算部１０６は、求めた位置差分ΔＰ（ΔＰＸ，ΔＰＹ，ΔＰＺ）を位置制御コントローラ１０７に出力する。外部装置とは、例えば、飛しょう体１００と無線通信を行うことで遠隔操作する飛しょう体制御装置である。飛しょう体制御装置は、例えば、地上の施設や車両、他の飛しょう体等に設けられる。

【0018】

位置制御コントローラ１０７は、第１演算部１０６から供給される位置差分ΔＰ（ΔＰＸ，ΔＰＹ，ΔＰＺ）から速度指令値（ベクトル）Ｖｃｎ（ＶｃｎＸ，ＶｃｎＹ，ＶｃｎＺ）を求める。そして、位置制御コントローラ１０７は、求めた速度指令値（ベクトル）Ｖｃｎ（ＶｃｎＸ，ＶｃｎＹ，ＶｃｎＺ）を第２演算部１０８に出力する。

【0019】

第２演算部１０８は、位置制御コントローラ１０７から供給される速度指令値Ｖｃｎ（ＶｃｎＸ，ＶｃｎＹ，ＶｃｎＺ）と、速度算出部１０３から供給される推定速度Ｖ（ＶＸ，ＶＹ，ＶＺ）との差分を求め、速度差分ΔＶ（ΔＶＸ，ΔＶＹ，ΔＶＺ）とする。そして、第２演算部１０８は、求めた速度差分（ベクトル）ΔＶ（ΔＶＸ，ΔＶＹ，ΔＶＺ）を速度コントローラ１０９に出力する。

【0020】

速度コントローラ１０９は、第２演算部１０８から供給される速度差分ΔＶ（ΔＶＸ，ΔＶＹ，ΔＶＺ）から加速度指令値（ベクトル）Ａｃｎ（ＡｃｎＸ，ＡｃｎＹ，ＡｃｎＺ）を求める。そして、速度コントローラ１０９は、求めた加速度指令値Ａｅｃｎ（ＡｅｃｎＸ，ＡｅｃｎＹ，ＡｅｃｎＺ）を機体固定座標系変換部１１０に出力する。

【0021】

機体固定座標系変換部１１０は、速度コントローラ１０９から供給される加速度指令値Ａｅｃｎ（ＡｅｃｎＸ，ＡｅｃｎＹ，ＡｅｃｎＺ）を、以下の（２）式により、慣性座標系から機体固定座標系に変換し、加速度指令値Ａｃｎ（Ａｃｎｘ，Ａｃｎｘ，Ａｃｎｘ）とする。

【0022】

【数2】

ここで、（２）式において、座標変換行列Ｔ^－１は、（１）式における座標変換行列Ｔの逆行列を示している。そして、機体固定座標系変換部１１０は、（２）式により求めた加速度指令値Ａｃｎ（Ａｃｎｘ，Ａｃｎｘ，Ａｃｎｘ）を第３演算部１１１に出力する。

【0023】

第３演算部１１１は、機体固定座標系変換部１１０から供給される加速度指令値Ａｃｎ（Ａｃｎｘ，Ａｃｎｘ，Ａｃｎｘ）と、慣性計測装置２０から供給される加速度Ａ（Ａｘ，Ａｙ，Ａｚ）との差分を求め、加速度差分（ベクトル）ΔＡ（ΔＡｘ，ΔＡｙ，ΔＡｚ）とする。そして、第３演算部１１１は、求めた加速度差分ΔＡ（ΔＡｘ，ΔＡｙ，ΔＡｚ）を加速度コントローラ１１２に出力する。

【0024】

加速度コントローラ１１２は、第３演算部１１１から供給される加速度差分ΔＡ（ΔＡｘ，ΔＡｙ，ΔＡｚ）から、角速度指令値（ベクトル）ＶＡｃｎ（ＶＡｃｎｘ，ＶＡｃｎｙ，ＶＡｃｎｚ）を求める。そして、加速度コントローラ１１２は、求めた角速度指令値ＶＡｃｎ（ＶＡｃｎｘ，ＶＡｃｎｙ，ＶＡｃｎｚ）を第４演算部１１３に出力する。

【0025】

第４演算部１１３は、加速度コントローラ１１２から供給される角速度指令値ＶＡｃｎ（ＶＡｃｎｘ，ＶＡｃｎｙ，ＶＡｃｎｚ）と、慣性計測装置２０から供給される角速度ＶＡ（ＶＡｘ，ＶＡｙ，ＶＡｚ）との差分を求め、角速度差分（ベクトル）ΔＶＡ（ΔＶＡｘ，ΔＶＡｙ，ΔＶＡｚ）とする。そして、第４演算部１１３は、求めた角速度差分ΔＶＡ（ΔＶＡｘ，ΔＶＡｙ，ΔＶＡｚ）を角速度コントローラ１１４に出力する。

【0026】

角速度コントローラ１１４は、第４演算部１１３から供給される角速度差分ΔＶＡ（ΔＶＡｘ，ΔＶＡｙ，ΔＶＡｚ）から、操舵における舵角の制御量を示す舵角指令値を求める。そして、角速度コントローラ１１４は、求めた舵角指令値を操舵装置３０へ出力する。

【0027】

操舵装置３０は、角速度コントローラ１１４から供給される舵角指令値に対応して、操舵翼の調整により舵角の制御を行い、飛しょう体１００に生じる力の調整を行う。

【0028】

図３は、実施形態の飛しょう体１００の飛行における位置制御処理の流れの一例を説明するフローチャートである。以下のフローチャートは、例えば、所定の制御周期Ｔで繰り返し実行されてよい。

【0029】

まず、慣性座標系変換部１０２は、慣性計測装置２０から供給される加速度Ａ（Ａｘ，Ａｙ，Ａｚ）を読み込む（ステップＳ１０１）。一方、所定の制御周期Ｔが経過していない場合、慣性座標系変換部１０２は、ステップＳ１０１の処理を繰り返す。

【0030】

次に、慣性座標系変換部１０２は、慣性座標系のＸ軸、Ｙ軸及びＸ軸の各々と、機体固定座標系のｘ軸、ｙ軸、ｚ軸それぞれとが成す角度Ψ、Φ、Θによる変換行列Ｔを生成する。そして、慣性座標系変換部１０２は、生成した変換行列Ｔを用いた（１）式により、機体固定座標系における加速度Ａ（Ａｘ，Ａｙ，Ａｚ）を、慣性座標系における加速度Ａｅ（Ａｅｘ，Ａｅｙ，Ａｅｚ）に座標変換し、速度算出部１０３に出力する（ステップＳ１０２）。

【0031】

次に、速度算出部１０３は、慣性座標系変換部１０２から供給される加速度Ａｅ（Ａｅｘ，Ａｅｙ，Ａｅｚ）を、所定の周期Ｔの期間で積分し、直前の周期からの速度の変化速度ΔＶ（ΔＶＸ，ΔＶＹ，ΔＶＺ）を求める。そして、速度算出部１０３は、直前の周期における、飛しょう体１００の慣性座標系における速度Ｖｅ（ＶｅＸ，ＶｅＹ，ＶｅＺ）に対して、算出した変化速度ΔＶ（ΔＶＸ，ΔＶＹ，ΔＶＺ）を加算し、加算結果を現在の速度Ｖｅ（ＶｅＸ，ＶｅＹ，ＶｅＺ）とする（ステップＳ１０３）。速度算出部１０３は、算出した速度Ｖｅ（ＶｅＸ，ＶｅＹ，ＶｅＺ）を位置算出部１０４及び第２演算部１０８の各々に出力する。

【0032】

次に、位置算出部１０４は、速度算出部１０３から供給される速度Ｖｅ（ＶｅＸ，ＶｅＹ，ＶｅＺ）を、所定の周期Ｔの期間で積分し、直前の周期からの位置の位置変化ΔＰ（ΔＰＸ，ΔＰＹ，ΔＰＺ）を求める。そして、位置算出部１０４は、直前の周期における、飛しょう体１００の慣性座標系における推定位置Ｐ（ＰＸ，ＰＹ，ＰＺ）に対して、算出した位置変化ΔＰ（ΔＰＸ，ΔＰＹ，ΔＰＺ）を加算し、加算結果を現在の推定位置Ｐ（ＰＸ，ＰＹ，ＰＺ）とする（ステップＳ１０４）。位置算出部１０４は、算出した推定位置Ｐ（ＰＸ，ＰＹ，ＰＺ）を第１演算部１０６に出力する。

【0033】

次に、第１演算部１０６は、予め設定されているもしくは外部装置から供給される位置指令値Ｐｃｍｄ（ＰｃｍｄＸ，ＰｃｍｄＹ，ＰｃｍｄＺ）と、位置算出部１０４から供給される推定位置Ｐ（ＰＸ，ＰＹ，ＰＺ）との位置差分ΔＰ（ＰＸ，ＰＹ，ＰＺ）を求める（ステップＳ１０５）。そして、第１演算部１０６は、位置差分ΔＰ（ＰＸ，ＰＹ，ＰＺ）を位置制御コントローラ１０７に出力する。

【0034】

次に、位置制御コントローラ１０７は、供給される位置差分ΔＰ（ＰＸ，ＰＹ，ＰＺ）から速度指令値Ｖｃｎ（ＶｃｎＸ，ＶｃｎＹ，ＶｃｎＺ）を算出する（ステップＳ１０６）。そして、位置制御コントローラ１０７は、算出した速度指令値Ｖｃｎ（ＶｃｎＸ，ＶｃｎＹ，ＶｃｎＺ）を、第２演算部１０８に出力する。

【0035】

次に、第２演算部１０８は、位置制御コントローラ１０７から供給される速度指令値Ｖｃｎ（ＶｃｎＸ，ＶｃｎＹ，ＶｃｎＺ）と、速度算出部１０３から供給される推定速度Ｖ（ＶＸ，ＶＹ，ＶＺ）との速度差分ΔＶ（ΔＶＸ，ΔＶＹ，ΔＶＺ）を算出する（ステップＳ１０７）。そして、第２演算部１０８は、算出した速度差分ΔＶ（ΔＶＸ，ΔＶＹ，ΔＶＺ）を速度コントローラ１０９に出力する。

【0036】

次に、速度コントローラ１０９は、供給される速度差分ΔＶ（ΔＶＸ，ΔＶＹ，ΔＶＺ）から加速度指令値Ａｅｃｎ（ＡｅｃｎＸ，ＡｅｃｎＹ，ＡｅｃｎＺ）を算出する（ステップＳ１０８）。そして、速度コントローラ１０９は、算出した加速度指令値Ａｅｃｎ（ＡｅｃｎＸ，ＡｅｃｎＹ，ＡｅｃｎＺ）を、機体固定座標系変換部１１０に出力する。

【0037】

次に、機体固定座標系変換部１１０は、（１）式における座標変換行列Ｔの逆行列Ｔ^－１を用いた（２）式により、慣性座標系における加速度指令値Ａｅｃｎ（ＡｅｃｎＸ，ＡｅｃｎＹ，ＡｅｃｎＺ）を、機体固定座標系における加速度指令値Ａｃｎ（ＡｃｎＸ，ＡｃｎＹ，ＡｃｎＺ）に座標変換し、第３演算部１１１に出力する（ステップＳ１０９）。

【0038】

次に、第３演算部１１１は、機体固定座標系変換部１１０から供給される加速度指令値Ａｃｎ（ＡｃｎＸ，ＡｃｎＹ，ＡｃｎＺ）と、慣性計測装置２０から供給される加速度Ａ（ＡＸ，ＡＹ，ＡＺ）との加速度差分ΔＡ（ΔＡＸ，ΔＡＹ，ΔＡＺ）を算出する（ステップＳ１１０）。そして、第３演算部１１１は、算出した加速度差分ΔＡ（ΔＡＸ，ΔＡＹ，ΔＡＺ）を、加速度コントローラ１１２に出力する。

【0039】

次に、加速度コントローラ１１２は、第３演算部１１１から供給される加速度差分ΔＡ（ΔＡＸ，ΔＡＹ，ΔＡＺ）から角速度指令値ＶＡｃｎ（ＶＡｃｎｘ，ＶＡｃｎｙ，ＶＡｃｎｚ）を算出する（ステップＳ１１１）。そして、加速度コントローラ１１２は、算出した角速度指令値ＶＡｃｎ（ＶＡｃｎｘ，ＶＡｃｎｙ，ＶＡｃｎｚ）を第４演算部１１３に出力する。

【0040】

次に、第４演算部１１３は、加速度コントローラ１１２から供給される角速度指令値ＶＡｃｎ（ＶＡｃｎｘ，ＶＡｃｎｙ，ＶＡｃｎｚ）と、慣性計測装置２０から供給される角速度ＶＡ（ＶＡｘ，ＶＡｙ，ＶＡｚ）との角速度差分ΔＶＡ（ΔＶＡｘ，ΔＶＡｙ，ΔＶＡｚ）を算出する（ステップＳ１１２）。そして、第４演算部１１３は、算出した角速度差分ΔＶＡ（ΔＶＡｘ，ΔＶＡｙ，ΔＶＡｚ）を、角速度コントローラ１１４に出力する。角速度コントローラ１１４は、所定の飛しょう体に生じる力を得るための舵角指令値を算出し、舵角装置１１６に出力する。

【0041】

図４は、実施形態の飛しょう体１００の飛行における位置制御処理の動作例示す図である。図４においては、Ｘ軸及びＹ軸の２次元平面ＸＹに対して垂直なＺ軸（高さ）における飛しょう体１００の飛行状態を示している。飛しょう体１００は位置制御が行なわれていない状態で、任意の方向に飛行している場合について考える。例えば、位置Ｐ１において位置制御を行なう場合、位置指令値が供給されると、すでに説明した図３のフローチャートの動作に基づき、位置指令値に対応した位置制御が行なわれる。

【0042】

上述した位置制御装置を用いた飛しょう体によれば、機体固定座標系として得られる加速度を慣性座標系に変換し、慣性座標系に変換された加速度から、順次、慣性座標系における推定速度、推定位置を求め、位置指令値に対応した位置に移動するための加速度指令値を求めて、この慣性座標系における加速度指令値を機体固定座標系に座標変換し、機体固定座標系における加速度及び角速度の制御指令値を得て位置制御を行なう。このため、上述した位置制御装置を用いることにより、飛行経路をあらかじめ決定する必要がなく、図４に示すように、飛しょう体１００の飛行が開始された後、任意の飛行経路により位置制御させることが可能となり、飛行経路を設定せずに任意の目標座標を与えることで位置制御を行なわせることができる。

【0043】

また、上述した位置制御装置を用いることにより、図４に示すように、任意の高さＨ１に位置制御を行い、この高さＨ１を維持するように巡航飛行を行わせること、すなわちレベルフライトが可能となる。ゆえにあらかじめ詳細な飛行経路を定める必要がないだけでなく、一定の高さＨ１を飛行させる際に一般的に用いられる電波高度計などの高度を測定するセンサを必要としないため、飛しょう体１００の構成品を削減することで重量を低減し、かつ製造原価を低減させることができる。

【0044】

図５は、実施形態の飛しょう体１００の飛行における位置制御処理の他の動作例を示す図である。図４の場合は、一般的に用いられている緯度経度及び高度からなる地面固定座標系を示している。しかしながら、図５の場合は、慣性計測装置で計測される加速度を変換する際、地面固定座標系ではない固定座標系を用いている。すなわち、実施形態によれば、座標変換行列Ｔにより、機体固定座標系を任意の座標系に座標変換できるため、変換先が地面固定座標系である必要はなく、意図した飛しょう軌跡となるよう位置指令値と座標系を自由に選択することができる。

【0045】

例えば、図５の場合は、位置指令値を供給する外部装置が、例えば山の斜面を２次元平面（Ｘ’軸及びＹ’軸からなる平面Ｘ’Ｙ’）として設定し、斜面からの距離を高さ（Ｚ’軸）として設定している。平面Ｘ’Ｙ’は、図４における平面ＸＹに対して角度θずれている。これにより、位置制御装置は、この他の動作例の場合、山の斜面からの距離（Ｈ２）を一定とする位置指令値を供給することにより、山の斜面を基準として、電波高度計を用いる必要なく安定したレベルフライトを行なわせることが可能であり、安定した高度で飛しょう体１００を巡航飛行させることができる。

【0046】

以上述べた実施形態の位置制御装置によれば、機体固定座標系として得られる加速度を慣性座標系に変換し、慣性座標系に変換された加速度から、慣性座標系における飛しょう体の推定速度、推定位置を求めることができる。このため、実施形態の位置制御装置によれば、高度計が搭載されていなくとも、レベルフライトを行なう際に、飛しょう体の慣性座標系における推定位置により高度を容易に推定することができる。

【0047】

また、実施形態の位置制御装置によれば、慣性座標系における飛しょう体の推定速度、推定位置を求めた後、位置指令値に対応した位置に飛行させるために必要な加速度指令値を求め、この加速度指令値を機体固定座標系に変換して用いる。このため、位置制御装置を用いることにより、飛行経路を決定させる必要がなく、位置目標の座標を与えることで位置制御させることが可能となり、位置目標の座標を途中で任意に変更しても位置制御を行なわせることができる。

【0048】

本発明のいくつかの実施形態を説明したが、これらの実施形態は、例として提示したものであり、発明の範囲を限定することは意図していない。これら実施形態は、その他の様々な形態で実施されることが可能であり、発明の要旨を逸脱しない範囲で、種々の省略、置き換え、変更を行うことができる。これら実施形態やその変形は、発明の範囲や要旨に含まれると同様に、特許請求の範囲に記載された発明とその均等の範囲に含まれるものである。

【符号の説明】

【0049】

１０…位置制御装置、２０…慣性計測装置、３０…操舵装置、１００…飛しょう体、１０２…慣性座標系変換部、１０３…速度算出部、１０４…位置算出部、１０６…第１演算部、１０７…位置制御コントローラ、１０８…第２演算部、１０９…速度コントローラ、１１０…機体固定座標系変換部、１１１…第３演算部、１１２…加速度コントローラ、１１３…第４演算部、１１４…角速度コントローラ

【図1】

【図2】

【図3】

【図4】

【図5】