(57)【特許請求の範囲】
【請求項１】
慣性航法データと測位データとに基づいて状態推定フィルタ演算を行い、移動体の位置、姿勢、速度の情報を出力する状態推定フィルタ演算手段と、
前記慣性航法データについての前記状態推定フィルタ演算における、移動体の位置、姿勢、速度の各誤差を演算する誤差演算手段と、
を含み、
前記誤差演算手段が、前記状態推定フィルタ演算手段が出力する、位置、姿勢、速度の各々を表す状態変数に係り、それよりも要素数の少ない状態変数で特定される微小変化単位四元数として表示された誤差情報を用いて、前記位置、姿勢、速度の各誤差を演算し、
前記状態推定フィルタ演算手段が出力する、移動体の位置、姿勢、速度の各情報が、前記誤差演算手段によって演算された誤差情報を乗じることで補正され、移動体の制御に供されることを特徴とする移動体制御装置。
【請求項２】
慣性航法データと測位データとに基づいて状態推定フィルタ演算を行い、移動体の位置、姿勢、速度の情報を出力する工程と、
前記慣性航法データについての前記状態推定フィルタ演算における、移動体の位置、姿勢、速度の各誤差を演算する工程と、
を含み、
前記誤差演算工程において、前記状態推定フィルタ演算手段が出力する、位置、姿勢、速度の各々を表す状態変数に係り、それよりも要素数の少ない状態変数で特定される微小変化単位四元数として表示された誤差情報を用いて、前記位置、姿勢、速度の各誤差を演算し、
前記状態推定フィルタ演算手段が出力する、移動体の位置、姿勢、速度の各情報が、前記誤差演算手段によって演算された誤差情報を乗じることで補正され、移動体の制御に供することを特徴とする移動体制御方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、慣性航法センサ及び測位センサを用いた移動体制御装置、及び移動体制御方法に関する。
【背景技術】
【０００２】
近年では、災害支援など種々の観点から、パイロットを必要とせず、比較的軽量・かつ小型で、簡便な構造を有する無人航空機が求められている。さらにこうした無人航空機では、その用途によっては、人為的に地上からの制御を行う、いわばラジコン方式ではなく、無人航空機自体が自律的に航行して目的を達成することが求められることもある。
【０００３】
こうした目的のために、利用可能な技術として慣性航法システムを元にGlobal Positioning System等その他の航法システムを組み合わせた複合航法システムがある。複合航法システムは、旅客機などの大型の航空機に採用されており、機体の位置、速度、姿勢などを推定し、その推定結果を元にして自律的な航行を可能としている。
【０００４】
なお、人工衛星などにおける姿勢制御において状態推定フィルタを応用する例が、非特許文献１や非特許文献２に開示されている。
【非特許文献１】D.Choukroun, "A Novel QianternionKalman Filter", Paper 2002-4460 at 42th AIAA Guidance, Navigation, andControl Conference 2004.
【非特許文献２】F.Landis Markley,"Multiplicative vs. Additive Filtering for Spacecraft AttitudeDetermination", 6th Dynamics and Control of Systems and Structures inSpace, 2004
【発明の開示】
【発明が解決しようとする課題】
【０００５】
しかしながら、現在の複合航法システムは精密な位置等の推定を可能とするため、リングレーザージャイロなど特殊なジャイロを利用するなど、その機器重量や機器の価格の側面から、軽量・小型化を求められる無人航空機に簡易に採用できるものではなくなっている。
【０００６】
また、現在の複合航法システムは、車両や船舶など、地上を移動する移動体への採用も期待されているが、これもまた、機器重量や価格の観点から転用が容易でないという問題点がある。
【０００７】
本発明は上記実情に鑑みて為されたもので、ＭＥＭＳ（Micro Electro Mechanical Systems）センサなどの軽量、小型で、かつ安価なセンサ等を利用して複合航法システムを実現できる移動体制御装置を提供することを、その目的の一つとする。
【課題を解決するための手段】
【０００８】
上記従来例の問題点を解決するための本発明は、移動体制御装置であって、慣性航法データと測位データとに基づいて状態推定フィルタ演算を行い、移動体の位置、姿勢、速度の情報を出力する状態推定フィルタ演算手段と、前記慣性航法データについての前記状態推定フィルタ演算における、移動体の位置、姿勢、速度の各誤差を演算する誤差演算手段と、を含み、前記誤差演算手段が、前記状態推定フィルタ演算手段が出力する、位置、姿勢、速度の各々を表す状態変数に係り、それよりも要素数の少ない状態変数で特定される微小変化単位四元数として表示された誤差情報を用いて、前記位置、姿勢、速度の各誤差を演算し、前記状態推定フィルタ演算手段が出力する、移動体の位置、姿勢、速度の各情報が、前記誤差演算手段によって演算された誤差情報を乗じることで補正され、移動体の制御に供されることを特徴としている。
【０００９】
このようにした誤差演算手段が実現する誤差モデルによって、ＭＥＭＳ（Micro Electro Mechanical Systems）センサなどの軽量、小型で、かつ安価なセンサ等を利用しても、その値を効果的に真値に近い値に補正可能な複合航法システムを実現できる。
【００１０】
また、本発明の一態様に係る移動体制御方法は、慣性航法データと測位データとに基づいて状態推定フィルタ演算を行い、移動体の位置、姿勢、速度の情報を出力する工程と、前記慣性航法データについての前記状態推定フィルタ演算における、移動体の位置、姿勢、速度の各誤差を演算する工程と、を含み、前記誤差演算工程において、前記状態推定フィルタ演算手段が出力する、位置、姿勢、速度の各々を表す状態変数に係り、それよりも要素数の少ない状態変数で特定される微小変化単位四元数として表示された誤差情報を用いて、前記位置、姿勢、速度の各誤差を演算し、前記状態推定フィルタ演算手段が出力する、移動体の位置、姿勢、速度の各情報が、前記誤差演算手段によって演算された誤差情報を乗じることで補正され、移動体の制御に供することを特徴としている。
【発明を実施するための最良の形態】
【００１１】
本発明の実施の形態について図面を参照しながら説明する。本発明の実施の形態に係る移動体制御装置１０は、図１にその概要を示すように、加速度センサ１１と、ジャイロセンサ１２と、ＧＰＳ（Global Positioning System）信号受信部１３と、演算制御部１４と、記憶部１５と、出力部１６とを含んで構成されている。また、この移動体制御装置１０は、移動体の位置や姿勢を制御する駆動装置２０に接続されている。
【００１２】
なお、以下の説明では、位置、速度、姿勢の各情報は、
（１）地球中心を原点として、地球の自転軸と平行になるようＺ軸を定めた右手系直交座標系（以下、ｉフレームと呼ぶ）と、
（２）地球中心を原点として、緯度０度、経度０度方向をＸ軸とし、かつ地球の自転軸と平行になるようＺ軸を定めた右手系直交座標系（以下、ｅフレームと呼ぶ）と、
（３）移動体を原点として、北方向をＸ軸、地球による重力の方向をＺ軸とした右手系直交座標系（以下、ｇフレームと呼ぶ）と、
（４）移動体を原点として、地球による重力方向をＺ軸とした右手系直交座標系であって、ｇフレームにおけるＺ軸を中心にｇフレームをα（rad）だけ回転させたときの座標系（以下、ｎフレームと呼ぶ）と、
（５）移動体を原点として、移動体の進行方向をＸ軸、揚力方向をＺ軸とした右手系直交座標系（以下、ｂフレームと呼ぶ）と、
のいずれかの座標系にて表現される。
【００１３】
以下では、どの座標系における情報であるかを識別するため、情報にインデックスを付して表す。例えば、ｂフレームにおけるＺ軸は、
【数１】
0004726134.tif 000002
と表記する。
【００１４】
加速度センサ１１は、移動体に設置される。この加速度センサ１１は、少なくとも一つの方向に対する移動体の加速度を測定し、当該測定した加速度を表す情報を出力する。本実施の例では、この加速度センサ１１は、ＭＥＭＳセンサであり、ｂフレームの各軸の各方向への移動体の移動加速度を測定し、当該測定した加速度を表す情報
【数２】
0004726134.tif 000003
を出力する。ここでｂは座標系のインデックスであり、ｂフレームで出力された信号であることを表す。
【００１５】
ジャイロセンサ１２は、例えばＭＥＭＳセンサを利用し、微小振動板が回転により受けるコリオリ力に基づき、回転角速度を検出するものである。本実施の形態では、ジャイロセンサ１２がｂフレームの各軸の各方向まわりの回転各速度を検出するように複数設けられ、各軸まわりの回転角速度を表す情報
【数３】
0004726134.tif 000004
を出力する。ここで、
【数４】
0004726134.tif 000005
は、ｉフレームに対するｂフレームの値を、ｂフレームの情報として出力したものである。
【００１６】
なお、ｂフレームにおけるＸ軸まわりの回転を「ロール」、Ｙ軸まわりの回転を「ピッチ」、Ｚ軸まわりの回転を「ヨー」と称する。
【００１７】
ＧＰＳ信号受信部１３は、ＧＰＳ衛星からの信号を受信して、移動体の位置（緯度、経度、高度）を表す情報ｑGPSを出力する。このようなＧＰＳ信号受信部１３は、広く用いられているものであるので、ここでの詳細な説明を省略する。
【００１８】
演算制御部１４は、ＣＰＵ等のプログラム制御デバイスを用いて構成され、記憶部１５に格納されているプログラムに従って動作する。本実施の形態では、この演算制御部１４が、加速度センサ１１やジャイロセンサ１２、ＧＰＳ信号受信部１３から出力される各情報に基づいて、移動体の位置、姿勢、及び速度の情報を生成して出力する。この演算制御部１４の具体的な動作については後に詳しく述べる。
【００１９】
記憶部１５は、ＲＡＭ（Random Access Memory）等の記憶素子であり、演算制御部１４によって実行されるプログラムを保持する。また、この記憶部１５は演算制御部１４のワークメモリとしても動作する。
【００２０】
出力部１６は、演算制御部１４が出力する、移動体の位置、姿勢、速度の情報を外部（例えば駆動装置２０）に出力する。
【００２１】
次に演算制御部１４の具体的な動作について説明する。本実施の形態では、この演算制御部１４が記憶部１５に格納されているプログラムを実行することで、ソフトウエアとして図２に示すような、移動体の位置、姿勢、速度の情報を生成する装置を実現する。
【００２２】
すなわち、この演算制御部１４が実現する装置は、機能的には、図２に示すように、座標変換部２１、第１運動方程式演算部２２、フィードバック部２３、位置・速度情報出力部２４、第２運動方程式演算部２５、姿勢情報出力部２６、及び状態推定フィルタ演算部２７を含んで構成されている。
【００２３】
座標変換部２１は、加速度センサ１１が出力するｂフレームでの加速度の情報を、姿勢情報出力部２６が出力する情報に基づいてｎフレームの加速度の情報に変換して出力する。この変換の結果は、姿勢を表す情報としての四元数
【数５】
0004726134.tif 000006
を用いて、
【数６】
0004726134.tif 000007
と書くことができる。なお、
【数７】
0004726134.tif 000008
である。この表記は、共役四元数をあらわし、[数６]にあるように四元数と共役四元数を用いることによって回転を表記する方法として広く知られているので、ここでの詳しい説明を省略する。
【００２４】
第１運動方程式演算部２２は、速度及び位置に係る運動方程式を用い、速度と位置とを演算して出力する。ここで運動方程式は、速度に関しては、
【数８】
0004726134.tif 000009
であり、位置に関しては、
【数９】
0004726134.tif 000010
で表される。第１運動方程式演算部２２は、これらを数値的に解いて、速度及び位置を表す情報を生成する。
【００２５】
フィードバック部２３は、移動体が重力の影響を受ける場合（例えば飛行機等の場合）に、位置・速度情報出力部２４が出力する位置の情報に基づいて、ｎフレームでの重力の影響を表す四元数
【数１０】
0004726134.tif 000011
を生成して、第１運動方程式演算部２２に出力する。
【００２６】
すなわち第１運動方程式演算部２２は、座標変換部２１の出力とこのフィードバック部２３の出力とを用いて、速度に係る運動方程式
【数８】
0004726134.tif 000012
を生成することになる。
【００２７】
位置・速度情報出力部２４は、第１運動方程式演算部２２が出力する速度及び位置を表す情報を記憶する。
【００２８】
第２運動方程式演算部２５は、移動体の姿勢に関わる運動方程式
【数１１】
0004726134.tif 000013
を数値的に解いて、姿勢の情報
【数１２】
0004726134.tif 000014
を生成する。姿勢情報出力部２６は、第２運動方程式演算部２５が出力する姿勢の情報を記憶する。
【００２９】
状態推定フィルタ演算部２７は、カルマンフィルタを用い、慣性航法データとして位置・速度情報出力部２４及び姿勢情報出力部２６が出力する情報を、測位データとしてＧＰＳ信号受信部１３が出力する情報で補正して出力する（いわゆるルーズ・カップリング方式）。本実施の形態では、位置、速度、姿勢の各運動方程式が非線形であるため、カルマンフィルタを拡張して、誤差モデルを適用する。
ここで誤差モデルは、
【数１３】
0004726134.tif 000015
として得られる。
【００３０】
この誤差モデルでは、誤差との線形和を利用する代わりに、誤差情報を、位置、姿勢、速度の各々を表す状態変数に係り、それよりも要素数の少ない状態変数で特定される微小変化単位四元数として表示し、この誤差情報を各状態変数に乗じて、位置、姿勢、速度の各情報を補正する。
【００３１】
（５）から（１１）式の例では、補正の対象となる
【数１４】
0004726134.tif 000016
の全１２状態量に対し、そのうち四元数のスカラー要素を「１」とし、残りのベクトル要素を含んでなる
【数１５】
0004726134.tif 000017
の全１０状態量を以て補正を行う。このように四元数のスカラー要素を「１」として要素数を減じる方法（微小変化単位四元数を用いる方法）としたことで、四元数
【数１６】
0004726134.tif 000018
の制約条件
【数１７】
0004726134.tif 000019
を簡便に満足できるようにしている。
【００３２】
さてここで、
【数１８】
0004726134.tif 000020
とおくと、誤差モデルの運動方程式は
【数１９】
0004726134.tif 000021
と記述でき、ＧＰＳ信号受信部１３が出力する測位データとしての情報と、位置・速度情報出力部２４及び姿勢情報出力部２６が出力する情報との差を、
【数２０】
0004726134.tif 000022
と書けば、ＧＰＳ信号受信部１３が出力する情報の観測誤差をνとして、
【数２１】
0004726134.tif 000023
とすることができる。
【００３３】
従って状態推定フィルタ演算部２７では、ＧＰＳ信号受信部１３からの測位データとしての情報が入力されていない間は、次の誤差共分散行列Ｐ，Ｑ，Ｅ
【数２２】
0004726134.tif 000024
を用い、カルマンフィルタの予測（Predict）の式によって
【数２３】
0004726134.tif 000025
と更新する。
【００３４】
またＧＰＳ信号受信部１３からの測位データとしての情報が入力されている場合は、カルマンフィルタの修正（Correct）の式によって、
【数２４】
0004726134.tif 000026
と更新する。
【００３５】
なお、ここで添字「INS」は、慣性航法データとしての、位置・速度情報出力部２４及び姿勢情報出力部２６が出力する情報を意味し、添字「GPS」は、測位データとしての、ＧＰＳ信号受信部１３が出力する情報を意味する。
【００３６】
駆動装置２０は、こうして状態推定フィルタ演算部２７にて断続的に補正される位置・速度情報出力部２４及び姿勢情報出力部２６の出力する値を、出力部１６を介して受け入れ、これらの値を用いて、移動体の位置、速度、姿勢を検出し、これらの情報と、指示された情報とを一致させるよう移動体の駆動を行うことになる。
【００３７】
このように本実施の形態によると、ＭＥＭＳセンサなど簡易なセンサから得られる加速度やジャイロの情報である慣性航法データ（ＩＮＳ）と、ＧＰＳによる測位データとに基づき、状態推定フィルタであるカルマンフィルタを用いて、ＩＮＳから得られる位置、速度、姿勢の情報を、ＧＰＳから得られる位置や速度の情報で補正している（いわゆるルーズカップリング方式）。
【００３８】
ここで、ＩＮＳに係る運動方程式が非線形であることによるカルマンフィルタの拡張をなすにあたり、ＩＮＳの真の値に対する誤差モデルを、補正の対象となる状態数量より少ない、微小変化単位四元数を用いて構成し、位置及び姿勢についてはこれら微小変化単位四元数を乗じる方法で、ＩＮＳの出力値を補正する。そして移動体をこの補正値によって駆動する。
【００３９】
このようにした誤差モデルを適用して、移動体制御装置を構成したことにより、ＭＥＭＳ（Micro Electro Mechanical Systems）センサなどの軽量、小型で、かつ安価なセンサ等を利用しても、その値を効果的に真値に近い値に補正可能な複合航法システムを実現できる。
【００４０】
なお、本実施の形態はここまでに説明のものに限られるものではなく、位置や速度、姿勢の検出においてカルマンフィルタなどの状態推定フィルタを用いるものであれば、本実施の形態において説明した誤差モデルを適用できる。
【００４１】
また、ここまでの説明では、測位データとしてＧＰＳに基づく情報を利用する例について述べたが、本実施形態はこれに限られず、磁気センサに基づいて検出される方位の情報や、レーダ等による位置等の情報であってもよい。
【実施例】
【００４２】
次に、略立方体をなす筐体外周の互いに隣接する３つの面上にそれぞれ各軸方向からの傾きを測定するジャイロを配置し、また、これらの軸方向に対する加速度を検出する加速度センサをいずれかの面に配置し、さらにＧＰＳ受信部などを配した装置を航空機に搭載した場合の実験例について示す。
【００４３】
ここで加速度センサは、STMicro LIS3L02AS4を用い、ジャイロは、アナログデバイセズ社のADXRS150を用いた。また、ＧＰＳは、民生用のＬ１電波を受信するu-blox TIM-LA を用いた。これらのセンサの信号のうちアナログ出力されるものは、Ａ／Ｄ変換器AD7739を介してＣＰＵ等を含む制御基板へ入力している。
【００４４】
また比較のため、市販されているＩＮＳ／ＧＰＳ慣性航法装置として、クロスボー社のNAV420を同じ航空機に搭載して実験を行った。その結果を図３、図４に示す。
【００４５】
図３は、航空機の位置の検出結果を表す位置履歴であり、実線が本発明の実施の形態に係る移動体制御装置における位置情報の出力内容を表し、点線が比較用のNAV420の位置情報の出力を表す。
【００４６】
また図４は、航空機の速度の検出結果の一例であり、実線が本発明の実施の形態に係る移動体制御装置における速度情報の出力内容を表し、点線が比較用のNAV420の速度情報の出力を表す。
【００４７】
これらの実験結果から、本発明の実施の形態に係る移動体制御装置は、市販品であるNAV420よりもはるかに安価なセンサやジャイロを利用しつつも、NAV420とほぼ同等の出力結果を得ていることがわかる。
【図面の簡単な説明】
【００４８】
【図１】本発明の実施の形態に係る移動体制御装置の例を表す構成ブロック図である。
【図２】本発明の実施の形態に係る移動体制御装置の例を表す機能ブロック図である。
【図３】本発明の実施例に係る移動体制御装置が生成する移動体位置の情報の実験結果を表す説明図である。
【図４】本発明の実施例に係る移動体制御装置が生成する移動体速度の情報の実験結果を表す説明図である。
【符号の説明】
【００４９】
１０  移動体制御装置、１１  加速度センサ、１２  ジャイロセンサ、１３  ＧＰＳ信号受信部、１４  演算制御部、１５  記憶部、１６  出力部、２０  駆動装置、２１  座標変換部、２２  第１運動方程式演算部、２３  フィードバック部、２４  位置・速度情報出力部、２５  第２運動方程式演算部、２６  姿勢情報出力部、２７  状態推定フィルタ演算部。
【図１】
0004726134.tif 000027
【図２】
0004726134.tif 000028
【図３】
0004726134.tif 000029
【図４】
0004726134.tif 000030