JP2007276507A - 移動体制御装置及び移動体制御方法 - Google Patents

Abstract

【課題】軽量、小型で、かつ安価なセンサ等を利用して複合航法システムを実現できる移動体制御装置を提供する。
【解決手段】慣性航法データと測位データとに基づいて状態推定フィルタ演算を行い、移動体の位置、姿勢、速度の情報を出力する。このとき、慣性航法データについての状態推定フィルタ演算における、移動体の位置、姿勢、速度の各誤差を演算するにあたり、位置、姿勢、速度の各々を表す状態変数に係り、それよりも要素数の少ない状態変数で特定される微小変化単位四元数として表示された誤差情報を用いて、位置、姿勢、速度の各誤差を演算し、当該誤差情報を乗じることで位置、姿勢、速度の情報が補正され、移動体の制御に供される移動体制御装置である。
【選択図】図１

Description

本発明は、慣性航法センサ及び測位センサを用いた移動体制御装置、及び移動体制御方法に関する。

近年では、災害支援など種々の観点から、パイロットを必要とせず、比較的軽量・かつ小型で、簡便な構造を有する無人航空機が求められている。さらにこうした無人航空機では、その用途によっては、人為的に地上からの制御を行う、いわばラジコン方式ではなく、無人航空機自体が自律的に航行して目的を達成することが求められることもある。

こうした目的のために、利用可能な技術として慣性航法システムを元に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）だけ回転させたときの座標系（以下、ｎフレームと呼ぶ）と、
（５）移動体を原点として、移動体の進行方向をＸ軸、揚力方向をＺ軸とした右手系直交座標系（以下、ｂフレームと呼ぶ）と、
のいずれかの座標系にて表現される。

以下では、どの座標系における情報であるかを識別するため、情報にインデックスを付して表す。例えば、ｂフレームにおけるＺ軸は、

と表記する。

加速度センサ１１は、移動体に設置される。この加速度センサ１１は、少なくとも一つの方向に対する移動体の加速度を測定し、当該測定した加速度を表す情報を出力する。本実施の例では、この加速度センサ１１は、ＭＥＭＳセンサであり、ｂフレームの各軸の各方向への移動体の移動加速度を測定し、当該測定した加速度を表す情報

を出力する。ここでｂは座標系のインデックスであり、ｂフレームで出力された信号であることを表す。

ジャイロセンサ１２は、例えばＭＥＭＳセンサを利用し、微小振動板が回転により受けるコリオリ力に基づき、回転角速度を検出するものである。本実施の形態では、ジャイロセンサ１２がｂフレームの各軸の各方向まわりの回転各速度を検出するように複数設けられ、各軸まわりの回転角速度を表す情報

を出力する。ここで、

は、ｉフレームに対するｂフレームの値を、ｂフレームの情報として出力したものである。

なお、ｂフレームにおけるＸ軸まわりの回転を「ロール」、Ｙ軸まわりの回転を「ピッチ」、Ｚ軸まわりの回転を「ヨー」と称する。

ＧＰＳ信号受信部１３は、ＧＰＳ衛星からの信号を受信して、移動体の位置（緯度、経度、高度）を表す情報ｑGPSを出力する。このようなＧＰＳ信号受信部１３は、広く用いられているものであるので、ここでの詳細な説明を省略する。

演算制御部１４は、ＣＰＵ等のプログラム制御デバイスを用いて構成され、記憶部１５に格納されているプログラムに従って動作する。本実施の形態では、この演算制御部１４が、加速度センサ１１やジャイロセンサ１２、ＧＰＳ信号受信部１３から出力される各情報に基づいて、移動体の位置、姿勢、及び速度の情報を生成して出力する。この演算制御部１４の具体的な動作については後に詳しく述べる。

記憶部１５は、ＲＡＭ（Random Access Memory）等の記憶素子であり、演算制御部１４によって実行されるプログラムを保持する。また、この記憶部１５は演算制御部１４のワークメモリとしても動作する。

出力部１６は、演算制御部１４が出力する、移動体の位置、姿勢、速度の情報を外部（例えば駆動装置２０）に出力する。

次に演算制御部１４の具体的な動作について説明する。本実施の形態では、この演算制御部１４が記憶部１５に格納されているプログラムを実行することで、ソフトウエアとして図２に示すような、移動体の位置、姿勢、速度の情報を生成する装置を実現する。

すなわち、この演算制御部１４が実現する装置は、機能的には、図２に示すように、座標変換部２１、第１運動方程式演算部２２、フィードバック部２３、位置・速度情報出力部２４、第２運動方程式演算部２５、姿勢情報出力部２６、及び状態推定フィルタ演算部２７を含んで構成されている。

座標変換部２１は、加速度センサ１１が出力するｂフレームでの加速度の情報を、姿勢情報出力部２６が出力する情報に基づいてｎフレームの加速度の情報に変換して出力する。この変換の結果は、姿勢を表す情報としての四元数

を用いて、

と書くことができる。なお、

である。この表記は、共役四元数をあらわし、[数６]にあるように四元数と共役四元数を用いることによって回転を表記する方法として広く知られているので、ここでの詳しい説明を省略する。

第１運動方程式演算部２２は、速度及び位置に係る運動方程式を用い、速度と位置とを演算して出力する。ここで運動方程式は、速度に関しては、

であり、位置に関しては、

で表される。第１運動方程式演算部２２は、これらを数値的に解いて、速度及び位置を表す情報を生成する。

フィードバック部２３は、移動体が重力の影響を受ける場合（例えば飛行機等の場合）に、位置・速度情報出力部２４が出力する位置の情報に基づいて、ｎフレームでの重力の影響を表す四元数

を生成して、第１運動方程式演算部２２に出力する。

すなわち第１運動方程式演算部２２は、座標変換部２１の出力とこのフィードバック部２３の出力とを用いて、速度に係る運動方程式

を生成することになる。

位置・速度情報出力部２４は、第１運動方程式演算部２２が出力する速度及び位置を表す情報を記憶する。

第２運動方程式演算部２５は、移動体の姿勢に関わる運動方程式

を数値的に解いて、姿勢の情報

を生成する。姿勢情報出力部２６は、第２運動方程式演算部２５が出力する姿勢の情報を記憶する。

状態推定フィルタ演算部２７は、カルマンフィルタを用い、慣性航法データとして位置・速度情報出力部２４及び姿勢情報出力部２６が出力する情報を、測位データとしてＧＰＳ信号受信部１３が出力する情報で補正して出力する（いわゆるルーズ・カップリング方式）。本実施の形態では、位置、速度、姿勢の各運動方程式が非線形であるため、カルマンフィルタを拡張して、誤差モデルを適用する。
ここで誤差モデルは、

として得られる。

この誤差モデルでは、誤差との線形和を利用する代わりに、誤差情報を、位置、姿勢、速度の各々を表す状態変数に係り、それよりも要素数の少ない状態変数で特定される微小変化単位四元数として表示し、この誤差情報を各状態変数に乗じて、位置、姿勢、速度の各情報を補正する。

（５）から（１１）式の例では、補正の対象となる

の全１２状態量に対し、そのうち四元数のスカラー要素を「１」とし、残りのベクトル要素を含んでなる

の全１０状態量を以て補正を行う。このように四元数のスカラー要素を「１」として要素数を減じる方法（微小変化単位四元数を用いる方法）としたことで、四元数

の制約条件

を簡便に満足できるようにしている。

さてここで、

とおくと、誤差モデルの運動方程式は

と記述でき、ＧＰＳ信号受信部１３が出力する測位データとしての情報と、位置・速度情報出力部２４及び姿勢情報出力部２６が出力する情報との差を、

と書けば、ＧＰＳ信号受信部１３が出力する情報の観測誤差をνとして、

とすることができる。

従って状態推定フィルタ演算部２７では、ＧＰＳ信号受信部１３からの測位データとしての情報が入力されていない間は、次の誤差共分散行列Ｐ，Ｑ，Ｅ

を用い、カルマンフィルタの予測（Predict）の式によって

と更新する。

またＧＰＳ信号受信部１３からの測位データとしての情報が入力されている場合は、カルマンフィルタの修正（Correct）の式によって、

と更新する。

なお、ここで添字「INS」は、慣性航法データとしての、位置・速度情報出力部２４及び姿勢情報出力部２６が出力する情報を意味し、添字「GPS」は、測位データとしての、ＧＰＳ信号受信部１３が出力する情報を意味する。

駆動装置２０は、こうして状態推定フィルタ演算部２７にて断続的に補正される位置・速度情報出力部２４及び姿勢情報出力部２６の出力する値を、出力部１６を介して受け入れ、これらの値を用いて、移動体の位置、速度、姿勢を検出し、これらの情報と、指示された情報とを一致させるよう移動体の駆動を行うことになる。

このように本実施の形態によると、ＭＥＭＳセンサなど簡易なセンサから得られる加速度やジャイロの情報である慣性航法データ（ＩＮＳ）と、ＧＰＳによる測位データとに基づき、状態推定フィルタであるカルマンフィルタを用いて、ＩＮＳから得られる位置、速度、姿勢の情報を、ＧＰＳから得られる位置や速度の情報で補正している（いわゆるルーズカップリング方式）。

ここで、ＩＮＳに係る運動方程式が非線形であることによるカルマンフィルタの拡張をなすにあたり、ＩＮＳの真の値に対する誤差モデルを、補正の対象となる状態数量より少ない、微小変化単位四元数を用いて構成し、位置及び姿勢についてはこれら微小変化単位四元数を乗じる方法で、ＩＮＳの出力値を補正する。そして移動体をこの補正値によって駆動する。

このようにした誤差モデルを適用して、移動体制御装置を構成したことにより、ＭＥＭＳ（Micro Electro Mechanical Systems）センサなどの軽量、小型で、かつ安価なセンサ等を利用しても、その値を効果的に真値に近い値に補正可能な複合航法システムを実現できる。

なお、本実施の形態はここまでに説明のものに限られるものではなく、位置や速度、姿勢の検出においてカルマンフィルタなどの状態推定フィルタを用いるものであれば、本実施の形態において説明した誤差モデルを適用できる。

また、ここまでの説明では、測位データとしてＧＰＳに基づく情報を利用する例について述べたが、本実施形態はこれに限られず、磁気センサに基づいて検出される方位の情報や、レーダ等による位置等の情報であってもよい。

次に、略立方体をなす筐体外周の互いに隣接する３つの面上にそれぞれ各軸方向からの傾きを測定するジャイロを配置し、また、これらの軸方向に対する加速度を検出する加速度センサをいずれかの面に配置し、さらにＧＰＳ受信部などを配した装置を航空機に搭載した場合の実験例について示す。

ここで加速度センサは、STMicro LIS3L02AS4を用い、ジャイロは、アナログデバイセズ社のADXRS150を用いた。また、ＧＰＳは、民生用のＬ１電波を受信するu-blox TIM-LA を用いた。これらのセンサの信号のうちアナログ出力されるものは、Ａ／Ｄ変換器AD7739を介してＣＰＵ等を含む制御基板へ入力している。

また比較のため、市販されているＩＮＳ／ＧＰＳ慣性航法装置として、クロスボー社のNAV420を同じ航空機に搭載して実験を行った。その結果を図３、図４に示す。

図３は、航空機の位置の検出結果を表す位置履歴であり、実線が本発明の実施の形態に係る移動体制御装置における位置情報の出力内容を表し、点線が比較用のNAV420の位置情報の出力を表す。

また図４は、航空機の速度の検出結果の一例であり、実線が本発明の実施の形態に係る移動体制御装置における速度情報の出力内容を表し、点線が比較用のNAV420の速度情報の出力を表す。

これらの実験結果から、本発明の実施の形態に係る移動体制御装置は、市販品であるNAV420よりもはるかに安価なセンサやジャイロを利用しつつも、NAV420とほぼ同等の出力結果を得ていることがわかる。

本発明の実施の形態に係る移動体制御装置の例を表す構成ブロック図である。 本発明の実施の形態に係る移動体制御装置の例を表す機能ブロック図である。 本発明の実施例に係る移動体制御装置が生成する移動体位置の情報の実験結果を表す説明図である。 本発明の実施例に係る移動体制御装置が生成する移動体速度の情報の実験結果を表す説明図である。

符号の説明

１０ 移動体制御装置、１１ 加速度センサ、１２ ジャイロセンサ、１３ ＧＰＳ信号受信部、１４ 演算制御部、１５ 記憶部、１６ 出力部、２０ 駆動装置、２１ 座標変換部、２２ 第１運動方程式演算部、２３ フィードバック部、２４ 位置・速度情報出力部、２５ 第２運動方程式演算部、２６ 姿勢情報出力部、２７ 状態推定フィルタ演算部。

Claims (2)

1. 慣性航法データと測位データとに基づいて状態推定フィルタ演算を行い、移動体の位置、姿勢、速度の情報を出力する状態推定フィルタ演算手段と、
   前記慣性航法データについての前記状態推定フィルタ演算における、移動体の位置、姿勢、速度の各誤差を演算する誤差演算手段と、
   を含み、
   前記誤差演算手段が、前記状態推定フィルタ演算手段が出力する、位置、姿勢、速度の各々を表す状態変数に係り、それよりも要素数の少ない状態変数で特定される微小変化単位四元数として表示された誤差情報を用いて、前記位置、姿勢、速度の各誤差を演算し、
   前記状態推定フィルタ演算手段が出力する、移動体の位置、姿勢、速度の各情報が、前記誤差演算手段によって演算された誤差情報を乗じることで補正され、移動体の制御に供されることを特徴とする移動体制御装置。
2. 慣性航法データと測位データとに基づいて状態推定フィルタ演算を行い、移動体の位置、姿勢、速度の情報を出力する工程と、
   前記慣性航法データについての前記状態推定フィルタ演算における、移動体の位置、姿勢、速度の各誤差を演算する工程と、
   を含み、
   前記誤差演算工程において、前記状態推定フィルタ演算手段が出力する、位置、姿勢、速度の各々を表す状態変数に係り、それよりも要素数の少ない状態変数で特定される微小変化単位四元数として表示された誤差情報を用いて、前記位置、姿勢、速度の各誤差を演算し、
   前記状態推定フィルタ演算手段が出力する、移動体の位置、姿勢、速度の各情報が、前記誤差演算手段によって演算された誤差情報を乗じることで補正され、移動体の制御に供することを特徴とする移動体制御方法。
   

Images