JP4876204B2 - 小型姿勢センサ - Google Patents

Description

姿勢制御を要する移動物体に搭載する、汎用性のある小型姿勢センサに関する。

姿勢制御を要する移動物体には、その三次元姿勢角を計測するため姿勢センサを備えている。一般的に姿勢センサは移動物体の独立な３軸上の角速度を計測する角速度センサ、及び角速度センサにより得られる角速度の計測値を時間的に積分することにより、移動物体の三次元姿勢角を取得する処理を行う演算処理部を備えている。三次元姿勢角はロール・ピッチ・ヨー角等を用いてオイラー角により表現されるものがあり、姿勢センサにより計測される三次元姿勢角を基に移動物体の姿勢制御が行われる。

しかしながら、オイラー角により三次元姿勢角を表現する場合、オイラー角特有の特異点の問題がある。具体的に説明すると、ピッチ角が±９０度の特異姿勢に近づくとロール角及びヨー角の出力値が急激に変動してしまうという問題がある。さらに、角速度センサは、誤差が生じやすいという問題がある。このように、姿勢センサにより計測される三次元姿勢角が高精度に計測されなければ、移動物体の姿勢制御もまた高精度に行うことができないという問題がある。

こうした問題を解決するため、３つの虚数と１つの実数からなる４次のベクトル表現を用いたクォータニオンにより三次元姿勢角を表現し、これにより、上記特異姿勢の問題を解消する姿勢センサについて開示した特許文献がある。
特開平９−５１０４号公報

図１０に示すように、特許文献１に開示されている姿勢センサ１０００は、移動物体の独立な３軸上の角速度を計測する角速度センサ１００１、移動物体の独立な３軸上の加速度を計測する加速度センサ１００２、及び角速度センサ１００１及び加速度センサ１００２の計測値を基にクォータニオンを算出する演算処理部１００３を備えている。演算処理部１００３では、加速度の計測値を基にして得られる誤差と拡張カルマンフィルタから得られるカルマンゲインとを用いてクォータニオンにより表現する三次元姿勢角の補正値を算出し、補正値を用いて角速度の計測値を補正し、クォータニオンにより表現する三次元姿勢角を算出する処理を行う。これにより、特許文献１に開示されている姿勢センサは、特異姿勢の問題がなく、移動物体の姿勢制御も高精度に行うことができることについて開示している。

しかしながら、上記特許文献１に示されている「拡張カルマンフィルタ」は非線形システムにおいて用いられるので、現実的な実装には不向きであるという問題がある。すなわち、上記特許文献１に示される姿勢センサは、概念的に説明されてはいるものの、非線形モデル式が大変複雑となることから「カルマンゲイン」や「クォータニオン」の算出も大変複雑となり、現実的な姿勢センサの実装は非常に困難であるという問題がある。なお、仮に実装できたとしても、小型化は実現できず、また、演算処理部で行う補正処理が収束しないおそれもあり、さらに実時間処理に乏しいという問題がある。

こうした問題に着目し、本発明は、カルマンゲインやクォータニオンの算出を容易にして現実的な姿勢センサの実装を可能にし、さらに、小型化及び実時間処理を可能とする小型姿勢センサを提供することを目的とする。

本発明の請求項１に記載の小型姿勢センサは、移動物体の独立な３軸上の角速度を計測する角速度センサと、移動物体の独立な３軸上の加速度を計測する加速度センサと、移動物体の独立な３軸上の地磁気を計測する地磁気センサと、前記角速度センサ、前記加速度センサ、及び前記地磁気センサの計測値を基にクォータニオンの推定値を算出する演算処理部とを備え、前記演算処理部は、予め推定されたクォータニオン推定値を用いて座標変換行列を算出し、当該座標変換行列を用いて地上固定座標系成分の地磁気ベクトル及び重力加速度ベクトルを変換し、現ステップのセンサ座標系成分の地磁気ベクトル及び重力加速度ベクトルを推定する座標変換部と、前記地磁気センサ及び前記加速度センサから現ステップのセンサ座標系成分の地磁気ベクトル及び重力加速度ベクトルを取得する地磁気・加速度センサデータ取得部と、前記座標変換部で推定した現ステップのセンサ座標系成分の地磁気ベクトル及び重力加速度ベクトルと、前記地磁気・加速度センサデータ取得部で取得した現ステップのセンサ座標系成分の地磁気ベクトル及び重力加速度ベクトルとの誤差を算出する誤差算出部と、前記誤差算出部で算出した誤差について、ヤコビ行列を用いてクォータニオンレベルの誤差を算出するクォータニオン誤差算出部と、前記角速度センサから現ステップのセンサ座標系成分の角速度ベクトルを取得する角速度センサデータ取得部と、前記クォータニオン誤差算出部が算出した現ステップのクォータニオンレベルの誤差、前記角速度センサデータ取得部から取得した現ステップの角速度ベクトル、ならびに前記予め推定されたクォータニオン推定値を用いて定常カルマンフィルタを構成し、現ステップのクォータニオン補正を行うクォータニオン補正部と、前記クォータニオン補正部で補正を行ったクォータニオンを積分することにより次ステップのクォータニオン推定値を算出するクォータニオン推定部とを有し、前記クォータニオン推定値をクォータニオンにより表現する三次元姿勢角として出力することを特徴とする。

本発明の請求項２に記載の発明は、請求項１に記載の小型姿勢センサについて、前記算出された次ステップのクォータニオン推定値は、前記座標変換部あるいは前記クォータニオン補正部に入力されることを特徴とする。

本発明の請求項３に記載の発明は、請求項１に記載の小型姿勢センサについて、前記定常カルマンフィルタは、

上記の式で演算処理することを特徴とする。

本発明の請求項４に記載の発明は、請求項１に記載の小型姿勢センサについて、前記演算処理部は、前記角速度センサを用いて前記加速度センサの取り付け位置によって生じる加速度の誤測を補正し、移動物体の速度データを検知するＧＰＳセンサを用いて当該速度データを微分することにより取得する移動物体の並進加速度データを用いて、移動物体に生じる加速度に伴う誤測を補正することを特徴とする。

本発明の請求項５に記載の発明は、請求項１に記載の小型姿勢センサについて、前記演算処理部は、角速度センサから取得する角速度ベクトルを用いて前記加速度センサから取得するセンサ座標系成分の重力加速度ベクトルの補正を行うセンサ座標系加速度補正部と、
移動物体の速度データを検知するＧＰＳセンサを用いて当該速度データを微分することにより取得する移動物体の並進加速度データを用いて地上固定座標系成分の重力加速度ベクトルの補正を行う地上固定座標系加速度補正部とを有し、移動物体に生じる加速度に伴う誤測を補正することを特徴とする。

本発明の請求項６に記載の発明は、請求項４または５に記載の小型姿勢センサについて、前記角速度センサ及び前記ＧＰＳセンサを用いて行う補正は、

上記の式で演算処理することを特徴とする。

上記の通り、本発明の小型姿勢センサは、１入力１出力の線形システムにおいて適用し、定常カルマンフィルタを構成することにより、「カルマンゲイン」や「クォータニオン」の算出を容易にすることができ、現実的な実装を可能にし、さらに、小型化及び実時間処理を可能とする。本発明の姿勢センサにより計測される三次元姿勢角は高精度であり、移動物体の姿勢制御もまた高精度に行うことができる。

また、本発明の小型姿勢センサは、移動物体に加速度が生じたときでも加速度センサデータの誤差補正を行うことができる。この時の誤差補正は、加速度センサの取り付け位置によって生じる加速度の誤測について角速度センサを用いて補正し、また、移動物体に生じる加速度に伴う誤測について、ＧＰＳセンサから取得する速度データを微分することにより取得する並進加速度データを用いて補正することにより、移動物体の任意の運動に対応して正確なクォータニオンの推定値を算出することができる。これにより、移動物体に加速度が生じたときでも、高精度な三次元姿勢角を計測することができ、移動物体の姿勢制御もまた高精度に行うことができる。

以下、本発明を実施するための最良の形態について説明する。

以下、本発明の第１実施例について説明する。図１は本実施例の小型姿勢センサに備えられる各構成をブロックで示したものである。小型姿勢センサ１００は、移動物体の独立な３軸上の角速度を計測する角速度センサ１０１、移動物体の独立な３軸上の加速度を計測する加速度センサ１０２、移動物体の独立な３軸上の地磁気を計測する地磁気センサ１０３、及び角速度センサ１０１、加速度センサ１０２、及び地磁気センサ１０３の計測値を基にクォータニオンを算出する演算処理部１０４を備えている。本実施例の小型姿勢センサ１００は１入力１出力の線形システムに適用し、定常カルマンフィルタを構成することにより、「カルマンゲイン」や「クォータニオン」の算出を容易にすることができ、現実的な実装を可能にし、さらに、小型化及び実時間処理を可能とする。

図２は演算処理部１０４で行う処理をブロックで示したものである。演算処理部１０４は、座標変換部２０１、地磁気・加速度センサデータ取得部２０２、誤差算出部２０３、クォータにオン誤差算出部２０４、角速度センサデータ取得部２０５、クォータニオン補正部２０６、クォータニオン推定値算出部２０７を備えている。また、図３は演算処理部１０４で行う処理をフローで示したものである。図４は演算処理部１０４で処理を行う際に用いる数式やデータの流れを示したものである。以下、図２乃至図４を用いて、演算処理部１０４で行う処理について説明する。

クォータニオンの各要素は、Ｓｉｍｐｌｅ Ｒｏｔａｔｉｏｎと呼ばれている姿勢表現方法の（λｘ，λｙ，λｚ，φ）を用いて次の式（１）で定義されている。

大まかに説明すると、ｑ₀は基準座標系からの姿勢変動量の二分の一の余弦値と関係している。他の３つの値ｑ₁，ｑ₂，ｑ₃は、基準座標系からの姿勢変動量の二分の一の正弦値と、ＸＹＺ軸の各方向への姿勢変動量の割合を掛けたものである。Ｓｉｍｐｌｅ Ｒｏｔａｔｉｏｎは任意軸回りの回転などとも呼ばれ、ある単位軸（λｘ，λｙ，λｚ）まわりに回転角φというように座標を回転させることで姿勢を表現する。ステップ３０１では、３つの虚数と１つの実数からなる４次のベクトル表現を用いた予め設定されるクォータニオンの初期値（式（２））を用いて座標変換行列を算出する（式（３））。座標変換行列は地上固定座標系からセンサ座標系へ変換する行列である。

ステップ３０２では、座標変換部２０１は初期姿勢における地磁気、加速度について、ステップ３０１で算出した座標変換行列を用いて座標変換を行う。初期姿勢における地磁気は、地磁気ベクトルの地上固定座標系成分であり、一方、初期姿勢における加速度は、重力加速度ベクトルの地上固定座標系成分である。これらを縦に並べたベクトル式は式（４）のようになる。

そして、これらをステップ３０１で算出した座標変換行列によりセンサ座標系に座標変換すると式（５）のようになる。

これにより、ステップ３０３では、予め設定されるクォータニオンの初期値を基に現ステップのセンサ座標系の地磁気、加速度を推定することができ、これらを誤差算出部２０３に入力する。

ステップ３０４では、地磁気・加速度センサデータ取得部２０２は、地磁気センサ１０３及び加速度センサ１０２から現ステップの地磁気ベクトル、重力加速度ベクトルを取得する（式（６））。取得する現ステップの地磁気ベクトル、重力加速度ベクトルは、センサ座標系成分のデータであり、これらを誤差算出部２０３に入力する。

ステップ３０５では、誤差算出部２０３は、ステップ３０３で座標変換部２０１が算出した推定される現ステップの地磁気ベクトル及び重力加速度ベクトルと、ステップ３０４で地磁気・加速度センサデータ取得部２０２から取得した現ステップの地磁気ベクトル及び重力加速度ベクトルとの誤差を算出する（式（７））。

つづいてステップ３０６では、クォータニオン誤差算出部２０４は、ステップ３０５で算出した誤差について、ヤコビ行列（式（８））を用いてクォータニオンレベルの誤差を算出し（式（９））、これをクォータニオン補正部２０５に入力する。

ステップ３０７では、角速度センサデータ取得部２０５は、角速度センサ１０１から現ステップの角速度を取得する。取得する現ステップの角速度は、センサ座標系成分のデータであり、これをクォータニオン補正部２０６に入力する。

ステップ３０８では、クォータニオン補正部２０６は、ステップ３０６でクォータニオン誤差算出部２０４が算出した現ステップのクォータニオンレベルの誤差、ステップ３０７で角速度センサデータ取得部２０５から取得した現ステップの角速度ベクトル、ならびに予め設定されるクォータニオンの初期値を用いて、式（１０）のような定常カルマンフィルタを構成し、現ステップのクォータニオン補正を行う。

最後に、ステップ３０９では、上記のように補正を行ったクォータニオンを積分することにより、次ステップのクォータニオン推定値を算出し、これをクォータニオンにより表現する三次元姿勢角とし、姿勢センサ１００の出力とする（ＥＮＤ）。また、算出された次ステップのクォータニオン推定値は、ステップ３０１やステップ３０８に戻って、座標変換部２０１やクォータニオン補正部２０６に入力される。すなわち、座標変換部２０１では、算出された次ステップのクォータニオン推定値を用いて次ステップの座標変換行列を算出し、クォータニオン補正部２０６では、算出された次ステップのクォータニオン推定値を用いて次ステップのクォータニオン補正に用いる。上記ステップ３０１乃至ステップ３０９の処理が、小型姿勢センサの演算処理部１０４で行う一ステップの処理であり、これを繰り返し行う。

本実施例の小型姿勢センサを移動物体に搭載し、クォータニオンにより表現する三次元姿勢角を計測した実験データを図５に示す。このデータは本発明の小型姿勢センサにより計測される三次元姿勢角の有効性を検証するデータとなっている。図５に示すのは本発明の小型姿勢センサを小型無人ヘリコプタに搭載し、小型無人ヘリコプタがホバリング飛行状態にあるときの三次元姿勢角を示しており、ほぼ零を示している。このほか小型無人ヘリコプタがロール方向のアクロバット飛行状態にあるときの三次元姿勢角も計測できる。このように、本発明の小型姿勢センサを小型無人ヘリコプタに搭載し、クォータニオンにより表現する三次元姿勢角を計測することにより、小型無人ヘリコプタの自律飛行制御を実現している。演算処理部で行う補正処理を速やかに行い、高精度な実時間処理を行うことにより、移動物体の姿勢制御もまた高精度に行うことができる。

以上、本実施例の小型姿勢センサ１００は、１入力１出力の線形システムに適用し、定常カルマンフィルタを構成することにより、「カルマンゲイン」や「クォータニオン」の取得を容易にすることができ、現実的な実装を可能にし、さらに、小型化及び実時間処理を可能とする。

なお、本実施例の小型姿勢センサは、推定される次ステップのクォータニオンを出力するものであるが、計測周期を短く設定することにより、システムへの影響は極めて小さいものと考えられる。

以下、本発明の第２実施例について説明する。実施例１で用いられている加速度センサ１０２は、重力加速度ベクトルのセンサ座標系成分のデータを取得するものである。そのため、加速度センサ１０２は重力加速度ベクトルのみを取得することが望ましいが、図６に示すような移動物体に加速度ａが生じた場合には、加速度センサ１０２は実際の重力加速度ベクトルｇと加速度ａを合成した重力加速度ベクトルｇ’を検知してしまう。このような加速度センサ１０２の誤測によって生じる誤差は最終的に小型姿勢センサより出力される三次元姿勢角にも少なからず影響を及ぼすので、補正を行う必要がある。

図７は小型姿勢センサに備えられる各構成をブロックで示したものである。基本的な構成については実施例１と同様であるが、本実施例では、移動物体に加速度ａが生じることによって起こる加速度センサ１０２の誤測を、ＧＰＳセンサ７０１及び角速度センサ１０１を用いて補正を行う。

図８は演算処理部１０４で行う処理をブロックで示したものである。演算処理部１０４は、実施例１で示した構成の他、センサ座標系加速度補正部８０１及び地上固定座標系加速度補正部８０２を備えている。センサ座標系加速度補正部８０１は、加速度センサ１０２から取得するセンサ座標系成分の重力加速度ベクトルについて、角速度センサ１０１から取得する角速度ベクトルを用いて補正を行う。また、地上固定座標系加速度補正部８０２は、初期姿勢における重力加速度ベクトル、すなわち、地上固定座標系成分の重力加速度ベクトルについてＧＰＳセンサ７０１から取得する重力加速度ベクトルを用いて補正を行う。図９は演算処理部１０４で処理を行う際に用いる数式やデータの流れを示したものである。以下、図８及び図９を用いて、演算処理部１０４で行う、加速度センサ１０２の誤測をＧＰＳセンサ７０１及び角速度センサ１０１を用いて補正を行う処理について説明する。

はじめに、ＧＰＳセンサ７０１と小型姿勢センサ７００の取り付け位置の違いによる誤差を補正する。ＧＰＳセンサ７０１から取得するのは移動物体の並進加速度であるが、小型姿勢センサ７００は移動物体の回転中心からある程度ずれた位置に取り付けられているので小型姿勢センサ７００内の加速度センサ１０２は並進加速度にくわえて移動物体の回転運動による向心加速度も取得してしまう。

そこで、角速度センサ１０１から取得する角速度センサデータ（式（１２））を基にＧＰＳセンサ７０１の速度データを微分することにより取得する並進加速度データと加速度センサ１０２から取得する加速度の差を補正する。そのために、式（１１）に示すような位置、速度、加速度レベルそれぞれでのＧＰＳ加速度と加速度センサ１０２の加速度の関係式を考える。

つづいて、ＧＰＳセンサ７０１の速度データを微分することにより取得する並進加速度データを用いて、重力加速度ベクトルの地上固定座標系成分を式１３のように補正することができる。

この式に、式（１１）の３番目の式を代入し、さらにこの式をクォータニオン推定アルゴリズム中で加速度センサが検知する重力ベクトルと重力ベクトルの地上固定座標成分を座標変換したものの差をとる部分に代入すると以下の式のようになる。結局、式（１４）で表されるような演算処理を加えることによって加速度センサの誤差を補正することができる。その後、座標変換行列を用いて現ステップの加速度の誤差を取得する。以後のステップについては、上記実施例１と同様である。

以上、本実施例の小型姿勢センサ７００は移動物体に加速度が生じたときでも加速度センサデータの誤差補正を行うことができる。この時の誤差補正は、加速度センサの取り付け位置によって生じる加速度の誤測について角速度センサを用いて補正し、また、移動物体に生じる加速度に伴う誤測についてＧＰＳセンサを用いて補正することにより、移動物体の任意の運動に対応して正確なクォータニオンの推定値を取得することができる。これにより、移動物体に加速度が生じたときでも、高精度な三次元姿勢角を計測することができ、移動物体の姿勢制御もまた高精度に行うことができる。

本発明の小型姿勢センサは、小型化を実現したことにより、大小を問わない陸海空の様々な移動物体に搭載することができ、汎用性がある。例えば、自律飛行制御を可能とする小型無人ヘリコプタや人工衛星など産業上の利用可能性は高いものである。

本発明の一実施例で用いる小型姿勢センサに備えられる各構成をブロックで示した図（実施例１） 本発明の一実施例で用いる小型姿勢センサの演算処理部で行う処理をブロックで示した図（実施例１） 本発明の一実施例で用いる小型姿勢センサの演算処理部で行う処理をフローで示した図（実施例１） 本発明の一実施例で用いる小型姿勢センサの演算処理部で処理を行う際に用いる数式やデータの流れを示した図（実施例１） 小型姿勢センサを移動物体に搭載し、クォータニオンにより表現する三次元姿勢角を計測した実験データを示した図（実施例１） 移動物体に加速度が生じた場合、加速度センサが取得する重力加速度ベクトルに誤差が生じることについて説明する図（実施例２） 本発明の一実施例で用いる小型姿勢センサに備えられる各構成をブロックで示した図（実施例２） 本発明の一実施例で用いる小型姿勢センサの演算処理部で行う処理をブロックで示した図（実施例２） 本発明の一実施例で用いる小型姿勢センサの演算処理部で処理を行う際に用いる数式やデータの流れを示した図（実施例２） 特許文献１に開示されている姿勢センサに備えられる各構成をブロックで示した図（背景技術）

符号の説明

１００，７００ 小型姿勢センサ
１０１ 角速度センサ
１０２ 加速度センサ
１０３ 地磁気センサ
１０４ 演算処理部
２０１ 座標変換部
２０２ 地磁気・加速度センサデータ取得部
２０３ 誤差取得部
２０４ クォータニオン誤差取得部
２０５ 角速度センサデータ取得部
２０６ クォータニオン補正部
２０７ クォータニオン推定値取得部
７０１ ＧＰＳセンサ
８０１ センサ座標系加速度補正部
８０２ 地上固定座標系加速度補正部

Claims (6)

1. 移動物体の独立な３軸上の角速度を計測する角速度センサと、
   移動物体の独立な３軸上の加速度を計測する加速度センサと、
   移動物体の独立な３軸上の地磁気を計測する地磁気センサと、
   前記角速度センサ、前記加速度センサ、及び前記地磁気センサの計測値を基にクォータニオンの推定値を算出する演算処理部とを備え、
   前記演算処理部は、予め推定されたクォータニオン推定値を用いて座標変換行列を算出し、当該座標変換行列を用いて地上固定座標系成分の地磁気ベクトル及び重力加速度ベクトルを変換し、現ステップのセンサ座標系成分の地磁気ベクトル及び重力加速度ベクトルを推定する座標変換部と、
   前記地磁気センサ及び前記加速度センサから現ステップのセンサ座標系成分の地磁気ベクトル及び重力加速度ベクトルを取得する地磁気・加速度センサデータ取得部と、
   前記座標変換部で推定した現ステップのセンサ座標系成分の地磁気ベクトル及び重力加速度ベクトルと、前記地磁気・加速度センサデータ取得部で取得した現ステップのセンサ座標系成分の地磁気ベクトル及び重力加速度ベクトルとの誤差を算出する誤差算出部と、
   前記誤差算出部で算出した誤差について、ヤコビ行列を用いてクォータニオンレベルの誤差を算出するクォータニオン誤差算出部と、
   前記角速度センサから現ステップのセンサ座標系成分の角速度ベクトルを取得する角速度センサデータ取得部と、
   前記クォータニオン誤差算出部が算出した現ステップのクォータニオンレベルの誤差、前記角速度センサデータ取得部から取得した現ステップの角速度ベクトル、ならびに前記予め推定されたクォータニオン推定値を用いて定常カルマンフィルタを構成し、現ステップのクォータニオン補正を行うクォータニオン補正部と、
   前記クォータニオン補正部で補正を行ったクォータニオンを積分することにより次ステップのクォータニオン推定値を算出するクォータニオン推定部とを有し、
   前記クォータニオン推定値を、クォータニオンにより表現する三次元姿勢角として出力することを特徴とする小型姿勢センサ。
2. 前記算出された次ステップのクォータニオン推定値は、前記座標変換部あるいは前記クォータニオン補正部に入力されることを特徴とする請求項１に記載の小型姿勢センサ。
3. 前記定常カルマンフィルタは、
   

   

   上記の式で演算処理することを特徴とする請求項１に記載の小型姿勢センサ。
4. 前記演算処理部は、前記角速度センサを用いて前記加速度センサの取り付け位置によって生じる加速度の誤測を補正し、移動物体の速度データを検知するＧＰＳセンサを用いて当該速度データを微分することにより取得する移動物体の並進加速度データを用いて、移動物体に生じる加速度に伴う誤測を補正することを特徴とする請求項１に記載の小型姿勢センサ。
5. 前記演算処理部は、角速度センサから取得する角速度ベクトルを用いて前記加速度センサから取得するセンサ座標系成分の重力加速度ベクトルの補正を行うセンサ座標系加速度補正部と、移動物体の速度データを検知するＧＰＳセンサを用いて当該速度データを微分することにより取得する移動物体の並進加速度データを用いて地上固定座標系成分の重力加速度ベクトルの補正を行う地上固定座標系加速度補正部とを有し、移動物体に生じる加速度に伴う誤測を補正することを特徴とする請求項１に記載の小型姿勢センサ。
6. 前記角速度センサ及び前記ＧＰＳセンサを用いて行う補正は、
   

   

   上記の式で演算処理することを特徴とする請求項４または５に記載の小型姿勢センサ。

Images