JP7025215B2 - 測位システム及び測位方法 - Google Patents

Description

本発明は測位システム及び測位方法に関する。

昨今、車両の自動運転や先進運転支援システムＡＤＡＳ（Advanced Driving Assistant System）などの技術開発が盛んであり、運動体の方位角や傾斜角を算出する技術開発が求められている。こうした技術開発の要望は、車両、船舶、航空機だけではなく、携帯電話、スマートフォン、タブレット端末、さらには、カーナビゲーション、デジタルカメラなどの電子機器やＣＧ（Computer Graphic）にも同じことが言える。これらの電子機器等においては、運動体の向きや傾きなどの姿勢やその変化をリアルタイムに、かつ的確に把握することが求められる。

特許文献１は、測位システム及び測位方法を開示する。測位システムは、ジャイロセンサ、加速度センサ、ＧＰＳ（Global Positioning System）信号受信部、さらには演算制御部を備えている。演算制御部は、ジャイロセンサ、加速度センサ、及びＧＰＳ信号受信部から出力されるデータに基づいて、運動体の位置、速度、姿勢、ジャイロセンサバイアス、及び加速度センサバイアスの各誤差推定及び誤差補正を行う。

特許文献１の演算制御部は、バイアス補正部、姿勢演算部、座標変換部、位置／速度演算部、エラー補正部、及び拡張カルマンフィルタ演算部を主たる構成成分としている。

上記拡張カルマンフィルタ演算部は、位置／速度演算部が演算した位置・速度と演算部が演算した姿勢データとに基づいて、乗算型クオータニオン誤差モデルを用いてジャイロバイアス誤差、加速度バイアス誤差、及び位置誤差・速度誤差・方位誤差の推定を行う。

特許文献２は、運動体の姿勢角検出装置を開示し、角速度センサの積分誤差の蓄積を防止して運動体の姿勢角を高精度に検出することを目的とし、角速度センサ、加速度センサ、加速度姿勢角演算手段等を有する。さらに角速度ローパスフィルタからの角速度姿勢行列を正規化及び直交化する手段等を有する。

特許文献３は、携帯デバイスを開示し、ＧＰＳセンサ、加速度センサ、方位センサ、地図情報取得部、三次元データ記憶部、三次元画像データ生成部等を含む。携帯デバイスの姿勢を表すために、加速度センサから検出した方位値によってクオータニオン（四元数）を生成する。具体的には、まず加速度センサで得られた値に基づき、携帯デバイスから見た重力方向ベクトルを得る。次に、三次元空間内における下向きの（０，－１，０）が前記重力方向ベクトルと一致するようなクオータニオンｑ１を求める。さらに、三次元空間内における例えば北（０，０，－１）を基準に方位センサから得た値の方角を向くようなクオータニオンｑ２を求め、クオータニオンｑ１とｑ２を掛け合わせて、姿勢を表すクオータニオンｑを得る。このように生成されたクオータニオンｑは、例えば（ｘ，ｙ，ｚ，ｗ）という形式で表すことができる。

特許文献４は、方位角センサ、即ち地磁気センサを開示する。特許文献４は、方位角を精度良く検出するために、方位角センサを搭載したセットの内部で発生する磁場、若しくは方位角センサの周囲に置かれた強力マグネットなどが発する磁場に起因するオフセットや方位角センサ自体の性能に起因するオフセットをキャンセルして、真の地磁気を検出する必要があることを示唆する。

図１０は、特許文献１に開示される測位システムの概要を示すブロック構成図である。測位システム１は、演算制御部１０、ジャイロセンサ２０、加速度センサ３０、及びＧＰＳ信号受信部４０を有する。演算制御部１０は、ジャイロセンサ２０、加速度センサ３０、及びＧＰＳ信号受信部４０から出力される各データに基づき、運動体の位置、速度、姿勢、ジャイロセンサバイアス、及び加速度センサバイアスの各誤差推定と誤差補正を行い、運動体の表示部６１や駆動装置６２などの出力部に出力し、運動体を制御する。なお、図１０（特許文献１の図１）に示された測位システムは、慣性センサデータ及びＧＰＳ測位データに基づいて、ジャイロバイアス誤差及び加速度バイアス誤差の推定、さらには位置誤差、速度誤差、及び方位誤差の推定を拡張カルマンフィルタ演算部で行うものであり、測位システムにＧＰＳ信号を用いる。

特開２０１２－１７３１９０号公報 特表２００９－５０７２７２号公報 特開２０１１－７０３４１号公報 特開２０１６－１３３３４３号公報

上記の特許文献は、一部を除きＧＰＳとジャイロセンサ（角速度センサ）を組み合わせたものである。こうしたジャイロセンサ等を含む運動体は、ＧＰＳ信号の入らない屋内やトンネル内、さらに運動体の不動状態においては、運動体の方位角や傾斜角の絶対角をリアルタイムに、かつ的確に算出できないという不具合が発生する。また、運動体の不動状態においては、ＧＰＳ測定データの精度が低下するという不具合が発生する。

本発明は、ジャイロセンサ等を含む、従来の測位システムに存在する不具合を克服した測位システム及び測位方法を提供する。

本書において、運動体、回転体、及び移動体なる語句は、ほぼ同義として使用する。運動体、回転体、移動体等の中には、それ自体の位置、速度、姿勢等を算出する機能を有するスマートフォン等の電子機器、ゲーム機器、または航空機、船舶、車両等の物体が含まれる。また、本発明において、測位システムとは、少なくとも上記運動体の少なくとも角速度を検出する機能が含まれるシステムを指す。また、本発明において、測位方法とは、上記測位システムの少なくとも角速度及び姿勢を検出する手順を指す。また、ジャイロセンサと角速度センサは同義であり、地磁気センサと方位センサも同義である。また、例えば“ジャイロセンサの出力”なる語句は、ジャイロセンサの出力を角速度で表現したもの、クオータニオンで表現したもの、及び回転行列で表現したものの中の少なくともいずれか１つを指す。地磁気センサ及び加速度センサについても同様である。即ち、“出力”なる意味合いは、各種センサから直接出力された信号だけではなく、その出力をクオータニオン、回転行列、さらには平滑化や正規化が行われたものを含む。また、“姿勢”とは、運動体の地表に対する傾きを表す角度を指す。

本発明に係る測位システムの一態様は、運動体の角速度を検出するジャイロセンサと、運動体が置かれる方向を検出する地磁気センサと、運動体の加速度を検出する加速度センサと、ジャイロセンサの出力を補正するジャイロセンサ出力補正部とを備え、ジャイロセンサ出力補正部は地磁気センサ及び加速度センサの出力成分に基づきジャイロセンサの出力成分を回転行列及びクオータニオンの少なくとも一方に変換し、回転行列またはクオータニオンに基づき運動体の姿勢を算出する。さらに、一態様において、ジャイロセンサ出力補正部は、ジャイロセンサの出力成分の一部に地磁気センサ及び加速度センサの出力成分を加えて正規・直交化を行った後に回転行列またはクオータニオンに変換することができる。さらに、ジャイロセンサの出力成分をクオータニオンで表現するクオータニオン導出部及びジャイロセンサの出力成分を回転行列で表現する第１の回転行列導出部の少なくとも一方を備え、クオータニオン導出部及び第１の回転行列導出部の少なくとも一方の出力成分をジャイロセンサ出力補正部で正規・直交化を行うことができる。さらに、地磁気センサ及び加速度センサの出力成分を回転行列で表現する第２の回転行列導出部を備え、前記第２の回転行列導出部で変換した回転行列に基づきジャイロセンサ出力補正部で正規・直交化を行うことができる。

本発明に係る別に発明である測位方法の一態様は、ジャイロセンサ、地磁気センサ、及び加速度センサを有する運動体の測位方法であって、ジャイロセンサの出力成分をクオータニオンまたは回転行列に変換するステップと、地磁気センサ及び前記加速度センサの出力成分でジャイロセンサの出力成分を補正するステップとを有する。測位方法の一態様において、好ましくは、ジャイロセンサの出力を第１の回転行列で表現し、かつ地磁気センサ及び加速度センサの出力を第２の回転行列で表現する第１ステップと、加速度センサのｘ軸、ｙ軸、及びｚ軸のスカラー量を基にして、その値が０～１．０の範囲に選ばれる加速度補正係数αを算出する第２ステップと、地磁気センサのオフセットまたは伏角を用いて地磁気補正係数βを算出する第３ステップと、第２ステップで算出した加速度補正係数αを用いて、加速度センサのｘ軸、ｙ軸、及びｚ軸の成分の平滑化（平均化）を行う第４ステップと、第３ステップで算出した地磁気補正係数βを用いて、加速度センサの成分と地磁気センサの成分との外積演算を行うステップと、外積演算の結果を回転行列に表現する第５ステップと、第５ステップで算出した回転行列をクオータニオンに変換する第６ステップとを備え、第６ステップで算出したクオータニオンに基づき運動体の姿勢を測定する。

本発明に係る測位システム及び測位方法は、ＧＰＳ信号を受信できない環境下に置かれた運動体の方位角、傾斜角をジャイロセンサの出力を基にして、地磁気センサ及び加速度センサの出力に基づき的確に求めることができる。

本発明に係る測位システムを示すブロック構成図 本発明に係る地磁気センサと加速度センサを説明するに用いる回転ベクトルの座標系 本発明に係る地磁気センサと加速度センサの内積計算を説明するために用いるベクトル図 本発明に係る測位方法を示すフローチャート 本発明に係る測位システムが搭載されたスマートフォンの外観図 本発明に係る測位システムが搭載されたタブレット端末の外観図 本発明に係る測位システムが搭載されたスマートウオッチの外観図 本発明に係る測位システムが搭載されたＡＲ装置、ＶＲ装置の外観図 本発明に係る測位システムが搭載された車両の外観図 従来の測位システムの一例を示すブロック構成図

（実施の形態）
図１は、本発明に係る測位システムの概要を示すブロック構成図である。測位システム１００は、ジャイロセンサ（角速度センサ）１１０、地磁気（方位）センサ１２０、加速度センサ１３０、クオータニオン（四元数）導出部１４０、第１の回転行列導出部１５０、第２の回転行列導出部１６０、及びジャイロセンサ出力補正部１７０を備える。なお、測位システム１００は、演算制御部の一部として、図示しないＣＰＵ（Central Processing Unit）を有する。クオータニオン導出部１４０、第１の回転行列導出部１５０、第２の回転行列導出部１６０、及びジャイロセンサ出力補正部１７０は、演算制御部の一部を構成する。また、測位システム１００の上記演算制御部は、記憶部を有する。記憶部は、測位システム１００で実行されるプログラム（アルゴリズム）を格納する。また、測位システム１００は、図示しない表示部を有することが少なくない。また、測位システム１００は、１つの半導体基板に集積化することが可能である。測位システム１００の特徴の１つは、ＧＰＳセンサを必須の構成要件としないことである。

ジャイロセンサ１１０は、車両等の運動体、スマートフォン等の電子機器、拡張現実（ＡＲ：Augmented Reality）装置、仮想現実（ＶＲ：Virtual Reality）装置等に搭載される。ジャイロセンサ１１０は、例えばコリオリ力を利用した振動型の音叉型センサや、半導体基板を微細加工して作製したＭＥＭＳ（Micro Electro Mechanical System）センサ等を利用して、電子機器等の回転角速度や角度を検出する。

地磁気センサ１２０は、ジャイロセンサ１１０と同様に、例えば車両等の運動体に搭載され、例えばマグネト・インピーダンス素子（ＭＩ素子）、ホール素子、磁気抵抗素子等で構成することができる。地磁気センサ１２０は、特定の三次元座標系に添って地磁気の基準磁気を測定する際にＸ軸、Ｙ軸、及びＺ軸の三次元（ｘ，ｙ，ｚ）の磁気成分を検出できればよく、一般的に小型化、応答性、感度、精度等からみてＭＩ素子を採用することが好ましい。

加速度センサ１３０は、ジャイロセンサ１１０及び地磁気センサ１２０と同様に、運動体等に搭載され、例えばＭＥＭＳ、静電容量、ピエゾ抵抗型、熱検知型等を用いることができる。ジャイロセンサ１１０、地磁気センサ１２０、及び加速度センサ１３０は、１つの半導体基板に集積化することが可能である。

クオータニオン導出部１４０は、ジャイロセンサ１１０の出力をクオータニオンの形式に変換する。なお、クオータニオン導出部１４０は、後段の第１の回転行列導出部１５０に至る前の演算過程の１つとして用意される。ジャイロセンサ１１０の出力成分を直接クオータニオンで表現することで、運動体の姿勢を４つの数値で表現できるというメリットが得られる。なお、クオータニオン導出部１４０を介さずに、ジャイロセンサ１１０の出力成分を直接第１の回転行列導出部１５０に入力するようにしてもかまわない。

クオータニオンは、回転軸（ベクトル）と回転角（スカラー）からなる４成分で３次元空間の回転を表現するものである。前述した各特許文献に開示されるクオータニオンは、測位システムやジャイロセンサの他にも、ゲーム機器、ＣＧ（Computer Graphic）、さらにはＡＲ装置やＶＲ装置において、３次元空間の運動を表現するために多く採用されている。

本発明に係る測位システム１００は、ジャイロセンサ１１０を有する。ここで、ジャイロセンサ１１０のｘ軸、ｙ軸、及びｚ軸周りの角速度の出力を式（１）で、その角速度ベクトルの周りの回転の微小角を式（２）でそれぞれ表すと、クオータニオンは式（３）で表現できる。

なお、式（３）のクオータニオンの形式に用いられる４つの成分ｑ_０，ｑ_１，ｑ_２，ｑ_３において、ｑ_０はスカラー部を、ｑ_１，ｑ_２，及びｑ₃はベクトル部をそれぞれ表す。

上記の式（３）は、物体の３次元回転をほぼ完全に表現している。しかし、ジャイロセンサ１１０の出力は、角速度から角度を求めるための積分処理を行う際にドリフト誤差や時間積分誤差が生じる。そのため、ジャイロセンサ１１０の出力を補正し続けることが重要となる。ここで、ドリフト誤差とは、入力角速度がないときのジャイロセンサ１１０の出力を指す。また、時間積分誤差とは、積分処理（ローパスフィルタ処理）を行うときに発生する誤差を指す。

本発明は、式（３）に示すクオータニオンによる表現を回転行列の形式に変換し、その回転行列成分に所定の補正を施した後、再度クオータニオンの表現に変換する。ここで、微小回転行列Ｒ_Δは、クオータニオンｑを用い、式（４）で表現できる。

時刻ｔにおける回転行列Ｒ_Ｇ（ｔ）と微小時間Δｔ後、即ち時刻ｔ＋Δｔにおける回転行列Ｒ_Ｇ（ｔ＋Δｔ）との関係は、式（５）で表現できる。また、回転行列の初期値Ｒ_Ｇを式（６）で表現することにより、ジャイロセンサ１１０の回転の時間的遷移をほぼ的確に表現できる。

第１の回転行列導出部１５０は、クオータニオン導出部１４０の後段に配置される。第１の回転行列導出部１５０は、ジャイロセンサ１１０の出力成分を回転行列で表現する。本来、運動体の姿勢をクオータニオンで表現しても、回転行列で表現しても実質的には同じことである。しかし、回転行列は運動体の姿勢を簡単に表現できるというメリットを有する。そこで、本発明の一実施の形態では、ジャイロセンサ１１０の出力成分を回転行列で表現して姿勢の状態を簡単に把握できるようにする。なお、第１の回転行列導出部１５０は、クオータニオン導出部１４０が用意される場合には省略することができる。

以上の説明によって、ジャイロセンサ１１０の出力をクオータニオン及び回転行列で表現する手順を示した。しかし、ジャイロセンサ１１０の出力のみで運動体等の方位角、傾斜角を算出するには積分誤差が生じるため、十分とはいえない。そこで、本発明は、地磁気センサ１２０と加速度センサ１３０の出力によりジャイロセンサ１１０の出力を補正することを試みた。なお、ジャイロセンサの出力を加速度センサ等の出力で補正するという概念そのものは従来から知られている。

第２の回転行列導出部１６０は、地磁気センサ１２０及び加速度センサ１３０の出力を回転行列の形式に変換する。第２の回転行列導出部１６０から導出される回転行列は、符号Ｒ_ＭＡで示される。なお、第２の回転行列導出部１６０は、ジャイロセンサ１１０の後段に備えた第１の回転行列導出部１５０と表現形式を合わせるために用意したものである。なお、第２の回転行列導出部１６０は必須の構成要件ではなく、例えば地磁気センサ１２０及び加速度センサ１３０からの出力を直接、後段のジャイロセンサ出力補正部１７０に入力するようにしてもよい。いずれにしても、回転行列は運動体の姿勢角を簡単に表現できるというメリットを有する。

図２は、地磁気センサ１２０及び加速度センサ１３０の出力を回転行列で表現するために用意した回転ベクトルの座標系を示す。以下、図２について図１を参照して説明する。図２において、ｚ軸は加速度センサ１３０でセンシングした地表に対する重力方向を示し、ｙ軸は地磁気センサ１２０でセンシングした磁北方向を示し、ｘ軸はｚ軸とｙ軸との直交方向を示す。ここで、ｘ軸は説明の便宜上、東の方向（東方）を表す。運動体等が置かれた環境、または機種の関係では西方を表すこともできる。ここで“磁北”とは方位磁針のＮ極が指す方向を示し、“真北”、即ち地球の自転軸の北端（北緯９０度）とは区別される。なお、“磁北”の値と“真北”の値には所定の関係が成立するので、一方の値が分かれば他方を算出することは容易である。従って、本発明で“磁北”と“真北”は同義とみなすことができる。なお、“磁北”は“磁南”に、“東方”は西の方向、即ち“西方”にそれぞれ置き換えてもよい。

図２で、地磁気センサ１２０の出力のベクトルをＢ＝（Ｂ_ｘ，Ｂ_ｙ，Ｂ_ｚ）とし、加速度センサ１３０の出力のベクトルをＡ＝（Ａ_ｘ，Ａ_ｙ，Ａ_ｚ）とすると、ベクトルＡ及びベクトルＢを含む平面と直交する、即ち東方（ｘ軸）のベクトルＨは、Ｈ＝Ａ×ＢまたはＨ＝Ｂ×Ａと表現できる。また、ベクトルＡとベクトルＨを含む平面と直交する、即ち磁北方向のベクトルＭをＭ＝Ａ×ＨまたはＭ＝Ｈ×Ａとすると、第２の回転行列導出部１６０の出力成分である回転行列Ｒ_ＭＡは、式（７）のように表現できる。

当然のことではあるが、上記の式（７）で表現した回転行列Ｒ_ＭＡの各要素は、加速度成分と地磁気成分が関わっているので外力や外部の地磁気などの影響を受けてしまうという不具合が生じる。したがって、ジャイロセンサ１１０の出力を地磁気センサ１２０及び加速度センサ１３０の出力で補正するにあたり、次の２つの点を考慮しなければならない。その１つは、回転行列Ｒ_ＭＡの算出精度を高めることである。その２つは回転行列Ｒ_ＭＡを求めるにあたり、運動体の種類やそれが使用される環境等に応じて、地磁気センサ１２０と加速度センサ１３０のどちらに重み付けを持たせるべきかどうかを逐次変更できるようにすることである。

ジャイロセンサ１１０側に設けた第１の回転行列導出部１５０から出力される回転行列Ｒ_Ｇのベクトル成分の積分誤差を減らすには、第２の回転行列導出部１６０で求めた回転行列Ｒ_ＭＡの精度が高いとき、即ち運動体に与えられる外力が小さく、かつ外部磁場の影響が小さいときに補正することが好ましい。また、補正するにあたっては、地磁気センサ１２０及び加速度センサ１３０のそれぞれの出力に基づき補正係数を用意することが必要となる。さらに、補正係数を用いて各種演算を行うために、それぞれの出力を一定の規則に基づき利用し易くしておくこと、即ち正規化・直交化することが必要となる。以下、こうしたことについて説明する。

まず上記の式（７）の加速度成分Ａ（Ａ_ｘ，Ａ_ｙ，Ａ_ｚ）のベクトルに着目する。加速度成分Ａのスカラー量は、式（８）で示すことができる。ここで、加速度センサ１３０の出力が１．０に正規化されるとすると、式（８）が１．０の周辺であるときは重力加速度をほぼ検出しているので、ジャイロセンサ１１０の出力を補正するのに好適である。これに対し、数式（８）の値が１．０から離れるに従い、運動体には重力以外の力が作用していることになり、ジャイロセンサ１１０の出力を補正する精度が低下する。

加速度センサ１３０の出力によりジャイロセンサ１１０の出力を補正するときに用いる加速度補正係数αの算出について説明する。加速度補正係数αは、式(９)または式(１０)で表現できる。ここで、α_ｍａｘは、加速度補正係数αの最大値であり、０≦α_ｍａｘ≦１．０である。なお、ｃ_０は定数である。重力加速度が支配的である場合はα＝α_ｍａｘと、重力加速度以外の作用が大きい場合はα≒０となるように設定すればよく、運動体等の種類とこれが使用される環境等に応じて適宜変更できるようにすることが好ましい。

ここで、加速度補正係数αの算出にあたっては、上記式（９），（１０）とは別に最大加速度補正係数α_ｍａｘを最大値とする「上に凸となる関数」を用いて算出してもよい。即ち、加速度の絶対値が１．０のときに加速度補正係数αが最大となるような例えば二次関数や三次関数に基づき算出してもよい。なお、このときの最大値は１．０だけではなく例えば０．５に設定してもよい。

次に、地磁気センサ１２０の出力によりジャイロセンサ１１０の出力を補正するときに用いる地磁気補正係数βの算出について説明する。地磁気補正係数βを算出するために磁気のオフセット補正後の磁気値を用意することになる。磁気オフセット補正は、例えば本発明者らが既に提案した特許文献４に開示される。

本書では、磁気オフセット補正が施されたという前提で説明する。磁気オフセット補正後の周辺磁場のベクトルをＢ_０で、外部磁場のベクトルをＢで、ジャイロセンサ１１０の出力を補正するときに用いる地磁気補正係数β_０を式（１１）で表す。

式（１１）において、Ｂ_０＞Ｂとすると、地磁気補正係数βの最大値である地磁気最大補正係数β_ｍａｘは、β_ｍａｘ≒１．０ということになる。しかし、実運用上はジャイロセンサ１１０の出力を補正する補正係数を運動体の種類やそれが使用される環境等に応じて変更できるように地磁気最大補正係数β_ｍａｘに上限閾値を設定しておくことが好ましい。地磁気最大補正係数β_ｍａｘは、１．０だけではなく１．０以下の任意の値に設定することがある。

式（１１）において、磁気オフセット補正後の周辺磁場を求めて地磁気補正係数β_０を算出する方法を示した。しかし、別の方法として、伏角を利用するものもある。以下、伏角γを用いて地磁気補正係数を算出する方法を説明する。

図３は、伏角γを算出するために用意した、加速度センサ１３０から出力されるベクトルＡ（Ａ_ｘ，Ａ_ｙ，Ａ_ｚ）と、地磁気センサ１２０から出力されるベクトルＢ（Ｂ_ｘ，Ｂ_ｙ，Ｂ_ｚ）と、地表との関係を示す。伏角γは、ベクトルＡとベクトルＢの内積（Ａ・Ｂ）と、両ベクトルが成す角度θに基づいて算出できる。２つのベクトルの内積演算はオフセット補正済みの地磁気成分と加速度成分との内積演算により算出する。伏角γは、２つのベクトルが成す角度をθとすると、式（１２）、式（１３）で表されるなお、ベクトルＡとベクトルＢとが直交するとき、即ちθ＝π／２＝９０°のとき、ベクトルＡとベクトルＢのスカラー量はゼロとなる。

式（１３）で算出した伏角γを地磁気補正係数として利用するためには、何らかの基準となる伏角を用意することになる。そのための１つの方法は、特許文献４に開示された磁気オフセット補正値を採用することである。また、別の方法は、ＧＰＳによる緯度経度情報から得られた位置に基づく伏角ライブラリを活用することである。ここで、いずれかの方法で求めた伏角の呼称を磁気オフセット補正伏角とし、その値をγ_０で表とすると、補正後の地磁気補正係数β _１ は式（１４）で算出できる。

但し、加速度センサ１３０は重力のみをセンシングすべきであり、伏角γの計測は運動体が静止中、等速運動中、あるいは小さな移動中であるときに行うことが好ましい。地磁気補正係数β_１が１．０に近い場合は外部磁場の影響が大きく、ゼロに近い場合は外部磁場の影響が小さいと考えることができる。地磁気補正係数β_１は、実運用上は地磁気補正係数β_０と同様に上限閾値β_１ｍａｘを設けるとよい。上限閾値β_１ｍａｘは、１．０だけではなく１．０を下回ってもよい。

上記式（９）、（１０）で算出した加速度補正係数α及び式（１１）、（１４）で算出した地磁気補正係数β_０，β_１は、ジャイロセンサ１１０で測位する姿勢角を求めるときの補正計数として活用する。なお、地磁気補正係数β_０、β_１をそのまま用いるのではなく、β_１ｍａｘ－β（β_０＝β_１）という形やβ／β_１ｍａｘという形に変形して活用してもよい。

また、ジャイロセンサ１１０の出力を補正するにあたり、加速度補正係数α及び地磁気補正係数βをそれぞれ単独に用いるのではなく、両者の積（α・β）や商（β／α）または（α／β）に変換して活用することもできる。

ここで再度、図１に戻って説明を行う。ジャイロセンサ出力補正部１７０は、第２の回転行列導出部１６０で算出した回転行列Ｒ_ＭＡを用いて、第１の回転行列導出部１５０で算出した回転行列Ｒ_Ｇの各成分を補正する。ジャイロセンサ出力補正部１７０は、平滑化１７１、正規化１７２、外積演算１７３、正規化１７４、平滑化１７５、正規化１７６、直交化１７７、正規化１７８、外積演算１７９、及び回転行列１８０を有する。回転行列１８０の後段には回転行列１８０で算出した回転行列をクオータニオンに変換するクオータニオン変換部１８１を有する。

平滑化１７１は、加速度センサ１３０から出力される加速度成分のｘ軸、ｙ軸、及びｚ軸の最終的なスカラー量を平滑化または平均化する演算処理を行う。本来、加速度センサ１３０から出力されるベクトル成分の大きさは、式（９）、（１０）で求めた加速度補正係数αをｘ軸、ｙ軸、及びｚ軸の成分に乗じて算出できる。しかし、本発明においては、さらにそれらの成分をなだらかにするため、即ち平滑化するために、ジャイロセンサ１１０のｘ軸のスカラー量Ｒ_Ｇ２０を加速度センサ１３０のｘ軸に、ｙ軸のスカラー量Ｒ_Ｇ２1を加速度センサ１３０のｙ軸に、ｚ軸のスカラー量Ｒ_Ｇ２２を加速センサ１３０のｚ軸にそれぞれ加算する。ジャイロセンサ１１０は角速度を、加速度センサ１３０は加速度をそれぞれセンシングする。加速度は角速度を微分することで算出できる。即ち、ジャイロセンサ１１０の出力と加速度センサ１３０の出力には所定の関係が成立する。こうしたことに鑑み、本発明において、加速度センサ１３０の出力にジャイロセンサ１１０の出力を所定の割合で加算して加速度センサ１３０の出力を平滑化する。ここで、加速度センサ１３０のｘ軸，ｙ軸、及びｚ軸の平滑化後のそれぞれの成分Ｒ_２０、Ｒ_２１、及びＲ_２２は、式（１５）を用いて算出する。符号αは、式（９）または式（１０）で求めた加速度補正係数αである。

正規化１７２は、式（１５）で算出した平滑後の成分Ｒ_２０、Ｒ_２１、及びＲ_２２を正規化する手段である。正規化は、出力を一定の規則に基づいて変形し、利用し易くするための手段である。加速度成分の正規化は、地磁気成分のベクトル演算、即ち外積演算を行うために設けられる。平滑後の成分Ｒ_２０、Ｒ_２１、及びＲ_２２の正規化は次式に基づいて算出される。

外積演算１７３は、地磁気成分と加速度成分との外積演算により、図２に示したｘ軸方向、即ち東方のベクトル成分を算出する。東方のｘ軸、ｙ軸、及びｚ軸のベクトル成分をそれぞれＨ_ｘ，Ｈ_ｙ，及びＨ_ｚとすると、式（１５）で求めた値を利用して、それを式（１６）に当てはめると、式（１７）で算出できる。

正規化１７４は、式（１７）で算出した東方のｘ軸、ｙ軸、及びｚ軸の各成分を正規化する手段であり、式（１８）に基づいて算出する。

平滑化１７５は、地磁気センサ１２０から出力されるｘ軸、ｙ軸、及びｚ軸の各成分の最終的なスカラー量を平均化して演算処理を行う手段である。平滑化後のそれぞれの成分Ｒ^´ _００、Ｒ^´ _０１、及びＲ^´ _０２は、東方の正規化成分を表す式（１６）、（１７）及び式（１１）で求めた地磁気補正係数β_０、または式（１３）で求めた地磁気補正係数β_１のいずれかをβとして表すと次式で求めることができる。なお、ジャイロセンサ１１０のｘ軸、ｙ軸、及びｚ軸のそれぞれのスカラー量をそれぞれＲ_Ｇ，００、Ｒ_Ｇ，０1、及びＲ_Ｇ，０２、とする。

正規化１７６は、平滑化１７５で算出した地磁気センサ１２０の成分を正規化する手段である。正規化は、前述したように、出力を一定の規則に基づいて変形し、利用し易くするための手段であり、後段の直交化を行うために設けられる。平滑化１７５で算出した地磁気センサ１２０の成分の正規化は、次式に基づいて算出される。

直交化１７７は最終的にはジャイロセンサ１１０の姿勢角を求めるために東方のｘ軸、ｙ軸、及びｚ軸の各成分Ｒ_００，Ｒ_０１，及びＲ_０２を直交化するものであり、式（７）、（８）、（１９）、及び式（２０）を参照し、式（２１）に基づいて行う。

さて、これまでの各種演算により、各種補正係数を用いて回転行列を補正してきたが、正しい回転行列にならないことがある。なぜならば、正規・直交化が行われていないからである。こうした直交化と正規化の組み合わせ、即ち正規・直交化によりジャイロセンサ１１０の出力を適正に補正することができる。

正規化１７８は、式（２１）で求めた直交化の値を正規化する手段であり、後段の外積演算の精度を高めるために行われる。正規化１７８は、次式に基づいて算出される。

外積演算１７９は、地磁気センサ１２０のベクトル成分を、加速度センサ１３０の出力とジャイロセンサ１１０の出力の外積演算から求める手段であり、式（２２）、式（１６）を利用し、さらに次式を用いて算出する。

回転行列演算部１８０は、上記式（１６）、（２２）及び式（２３）で算出した各成分を利用して、次式に示した回転行列Ｒを算出する。式（２４）に示した回転行列Ｒは、加速度補正係数αと地磁気補正係数βが加味されたものである。

なお、式（２４）に示した回転行列Ｒは、ジャイロセンサ１１０の新たな回転行列Ｒ_Ｇとして置き換えられる。

式（２４）に示した回転行列Ｒは、これまでの加速度成分及び地磁気成分の正規化と直交化により算出された行列に変換されている。従って、式（２４）の行と列を入れ替えた式（２５）と同じになる。

したがって、式（２４）の例えば第１列Ｐ１と式（２５）の例えば第1列Ｐｔ１は共に１となる。式（２４）、（２５）の第２列、第３列についても同様である。

クオータニオン変換部１８１は、式（２４）または式（２５）に示した回転行列Ｒ、Ｒｔを再度クオータニオンに変換する手段である。クオータニオン変換部１８１での演算は実質的に式（２４）または式（２５）に示した回転行列Ｒ、Ｒｔと等価である。回転行列とクオータニオンとの間での相互変換するプログラム、アルゴリズムは既に公知である。回転行列をクオータニオンに変換することにより、演算数が少なくて済み、かつ処理時間の短縮化が図れる。これによって、本発明の目的とする測位システム１００の姿勢角を的確、かつ迅速に算出することができる。

なお、図１に示したジャイロセンサ出力補正部１７０は、加速度成分の平滑化を最初に行うが、地磁気成分の平滑化を最初に行い、その後加速度成分の平滑化を行うようにしてもよい。また、ジャイロセンサ１１０及び加速度センサ１３０の後段に相補フィルタやカルマンフィルタを有してもよい。また、正規化と直交化は別々に処理するように示したが、これらを同時に行うようにしてもかまわない。

以上、図１～図３を用いてに本発明に係る測位システム１００の動作を説明した。次に本発明に係る測位方法について説明する。

図４は、本発明に係る運動体等の測位方法の各ステップ（測位手順）を示す。以下、図１～図３を参照して測位方法を説明する。本発明に係る測位方法は、ステップＳ４１０から始まり、ステップＳ４８０で終了する。なお、各ステップに併記した式番号（１）～（２５）は、本発明に係る測位方法の理解を助長するためにあり、あくまでも参照程度であると解すべきである。

ステップＳ４１０は、ジャイロセンサ１１０の出力をクオータニオン導出部１４でクオータニオンに表現し、さらに、そのクオータニオンの表現を第１の回転行列導出部１５０で回転行列に変換する。回転行列の特徴は、運動体の姿勢を比較的簡単に表現できることにある。さらにステップＳ４１０では、地磁気センサ１２０から取り出したベクトルＢ＝（Ｂ_ｘ，Ｂ_ｙ，Ｂ_ｚ）と加速度センサ１３０から取り出したベクトルＡと（Ａ_ｘ，Ａ_ｙ，Ａ_ｚ）を第２の回転行列導出部１６０で回転行列_ＲＭＡとして導出する。ステップＳ４１０では例えば式（１）～（６）で示した演算処理を経て、最終的に回転行列は例えば式（７）で表現される。

ステップＳ４２０は、加速度センサ１３０の出力でジャイロセンサ１１０の出力を補正するための加速度補正係数αを算出する。加速度補正係数αの算出は、上記の式（９）または式（１０）に基づいて行う。加速度補正係数αはα＝０～１．０であり、その値は運動体の種類やそれが使用される環境等に応じて異なり、初期設定はもちろんのこと、使用中においてもその値の変更が可能であることが好ましい。加速度補正係数αの最大値、即ち加速度最大補正係数α_ｍａｘは通常は１．０であるが、実運用上は１．０よりも小さく設定することもありうる。ステップＳ４２０は、例えば式（９）、（１０）に示した演算処理を行う。

ステップＳ４３０は、地磁気センサ１２０の出力でジャイロセンサ１１０の出力を補正するための地磁気補正係数βを算出する。地磁気補正係数βの算出は、例えば、地磁気センサ１２０の出力と磁気オフセット補正後の周辺磁場の比から求める方法がある。また、伏角γと磁気オフセット補正後の伏角γ_０との比から求める別の方法もある。ステップＳ４３０は、例えば式（１１）～（１４）を用いて各種の演算処理を行う。

ステップＳ４４０は、ステップＳ４２０で算出した加速度補正係数αを用いて加速度センサ１３０の出力成分を平滑化し、さらに正規化する。正規化するにあたっては、たとえば式（１５）で示す平滑化を行った後に式（１６）を用いて行う。

ステップＳ４５０は、加速度ベクトルと地磁気ベクトルとの外積演算及び地磁気補正係数を用いた平滑化、直交化、及び正規化により、東方（図２参照）のベクトル成分の正規化を行う。東方のベクトル成分は例えば式（２１）に基づく演算処理結果を式（２２）に当てはめて算出する。ステップＳ４５０は、式（２２）に至るまでに例えば式（１７）～（２１）に演算処理を行う。

ステップＳ４６０は、加速度センサ１３０と地磁気センサ１２０の外積演算及び、地磁気補正係数を用いた平滑化、直交化、及び正規化により、磁北の回転行列成分を導出する。東方のベクトル成分は、式（２３）により導出する。

ステップＳ４７０は、ジャイロセンサ１１０側の回転行列Ｒを回転行列ＲＧに更新する。回転行列は、式（２４）または（２５）で表す。なお、ステップＳ４７０は、必須の構成ステップではなく、前のステップＳ４６０から直接ステップＳ４８０に移行するようにしてもよい。

ステップＳ４８０は、更新した回転行列ＲＧから再度クオータニオンに変換することで運動体の姿勢角等を求める。ステップＳ４８０の式（２４）または式（２５）で表した回転行列をクオータニオンに変換するのは、回転行列では姿勢角を簡単に表せないこと、さらに、クオータニオンは４つの数値で表現でき、演算数が少なくて済むため、演算処理時間の短縮化が図れることに基づく。なお、ステップＳ４８０は、ステップＳ４７０と同様に必須のステップではなく、ステップＳ４７０が用意される場合にはステップＳ４８０を省略してもよい。以上、ステップＳ４１０～Ｓ４８０によって本発明に係る測位方法は終了する。なお、ステップＳ４１０～Ｓ４８０で行う各種の演算処理の順番は、ステップＳ４７０、４８０を除き任意である。例えばステップＳ４２０とＳ４３０は入れ替えてもかまわない。また、ステップＳ４４０～Ｓ４６０の処理も入れ替えてもかまわない。

また、ステップＳ４１０～Ｓ４８０のすべてが必須でなく、運動体の種類や使用環境等によっては、例えばステップＳ４２０及びＳ４３０の少なくとも一方をスキップしてもかまわない。

本発明に係る測位システム、測位方法の特徴を端的に表すと、運動体の加速度成分及び地磁気成分に対して平滑化、正規化、及び直交化を施すことによってジャイロセンサの出力を補正し、これらの回転行列を求め、この回転行列をクオータニオンに変換することによって運動体等の姿勢を測位することである。

図５～図９は、本発明の測位システムが搭載される各種の電子機器、車両等の外観図である。図５はスマートフォンを、図６はタブレット端末を、図７はスマートウオッチを、図８はＡＲ／ＶＲ装置を、図９は車両をそれぞれ示す。なお、図７のスマートウオッチに搭載する目的は例えば腕の軌道解析を行うためである。また、本発明に係る測位システムまたは測位方法は、これらの他に航空機、船舶、デジタルカメラ、ゲーム機器等に搭載され、これら運動体の回転や向きの変化を検知する用途に用いられる。

以上説明したように、本発明は、ＧＰＳ測定データに依存するトンネル内や不動状態においても、運動体の位置、速度、姿勢等を的確に把握できる。そのため、産業上の利用可能性は極めて高い。

１００ 測位システム
１１０ ジャイロセンサ
１２０ 地磁気センサ
１３０ 加速度センサ
１４０ クオータニオン導出部
１５０ 第１の回転行列導出部
１６０ 第２の回転行列導出部
１７０ ジャイロセンサ出力補正部
１７１ 平滑化（加速度成分）
１７２ 第１正規化(加速度成分)
１７３ 外積演算（加速度成分）
１７４ 第２正規化（加速度成分）
１７５ 平滑化（地磁気成分）
１７６ 正規化(地磁気成分)
１７７ 直交化（地磁気成分）
１７８ 正規化（地磁気成分）
１７９ 外積演算（加速度成分）
１８０ 回転行列演算部
１８１ クオータニオン変換部
５００ スマートフォン
６００ タブレット端末
７００ スマートウオッチ
８００ ＡＲ／ＶＲ装置
９００ 車両
Ｓ４１０～Ｓ４８０ ステップ

Claims (22)

1. 運動体の角速度を検出するジャイロセンサと、
   前記運動体が置かれる方向を検出する地磁気センサと、
   前記運動体の加速度を検出する加速度センサと、
   前記ジャイロセンサの出力を補正するジャイロセンサ出力補正部とを備え、
   前記ジャイロセンサ出力補正部は、前記地磁気センサ及び前記加速度センサの出力成分に基づいて前記ジャイロセンサの出力成分を回転行列及びクオータニオンの少なくとも一方に変換し、前記回転行列または前記クオータニオンに基づき前記運動体の姿勢を算出するものであって、前記ジャイロセンサの出力成分の一部に前記地磁気センサ及び前記加速度センサの出力成分を加えて正規化・直交化を行った後に前記回転行列または前記クオータニオンに変換する、測位システム。
2. 前記ジャイロセンサの出力成分をクオータニオンで表現するクオータニオン導出部及び前記ジャイロセンサの出力成分を回転行列で表現する第１の回転行列導出部の少なくとも一方を備え、前記クオータニオン導出部及び前記第１の回転行列導出部の少なくとも一方の出力成分を前記ジャイロセンサ出力補正部で前記正規・直交化を行う請求項１に記載の測位システム。
3. 前記地磁気センサ及び前記加速度センサの出力成分を回転行列で表現する第２の回転行列導出部を備え、前記第２の回転行列導出部で表現した回転行列に基づき前記ジャイロセンサ出力補正部で前記正規・直交化を行う請求項２に記載の測位システム。
4. 前記ジャイロセンサ出力補正部は、前記加速度センサ及び前記地磁気センサの平滑化を行った後に前記ジャイロセンサの出力成分の前記正規化・直交化を行う請求項１に記載の測位システム。
5. 前記ジャイロセンサ出力補正部は、前記加速度センサの加速度補正係数αを算出し（０≦α≦１．０）、前記加速度補正係数αを前記回転行列の成分に反映し、かつ前記ジャイロセンサ出力補正部は、前記地磁気センサの地磁気補正係数βを算出し（０≦β≦１．０）、前記地磁気補正係数βを前記補正後の回転行列の成分に反映する請求項４に記載の測位システム。
6. 前記ジャイロセンサ出力補正部は、前記加速度補正係数αの算出において、加速度の絶対値が１．０のときに加速度補正係数αが最大となる「上に凸の関数」に基づき算出する請求項５に記載の測位システム。
7. 前記ジャイロセンサ出力補正部は、前記加速度補正係数α及び前記地磁気補正係数βの積または商の値を前記回転行列の成分に反映する請求項５に記載の測位システム。
8. 前記ジャイロセンサ出力補正部は、前記加速度センサのベクトル成分と前記地磁気センサのベクトル成分の外積演算によって、前記運動体の加速度センサ及び前記地磁気センサのベクトル成分で求めた磁北と直交する方向のベクトル成分を算出する請求項７に記載の測位システム。
9. 前記ジャイロセンサ出力補正部は、前記加速度センサのベクトル成分と前記磁北と直交する方向のベクトル成分との外積演算によって前記磁北のベクトル成分を算出する請求項８に記載の測位システム。
10. 前記ジャイロセンサ出力補正部は、前記加速度センサから出力されるｘ軸、ｙ軸、及びｚ軸のベクトル成分Ａを（Ａ_ｘ，Ａ_ｙ，Ａ_ｚ）としたとき、ベクトル成分Ａのスカラー量を次式で算出し、
    

    

    定数をｃ_０、加速度最大補正係数をα_ｍａｘとしたとき、加速度補正係数αを次式のいずれかで算出し、
    

    

    

    前記加速度補正係数αを用いて前記ジャイロセンサの出力成分を補正する請求項７に記載の測位システム。
11. 前記加速度センサのｘ軸、ｙ軸、及びｚ軸のそれぞれの成分をＲ_２０、Ｒ_２１、及びＲ_２２とし、前記ジャイロセンサのｘ軸、ｙ軸、及びｚ軸のそれぞれの成分をＲ_Ｇ２０、Ｒ_Ｇ２１、及びＲ_Ｇ２２としたとき、加速度補正係数αを次式のいずれかで算出し、
    

    

    

    算出した前記加速度補正係数αを用いた次式に基づいて前記加速度センサの出力成分を平滑化する請求項７に記載の測位システム。
    

    
12. 前記磁北と直交する方向のベクトル成分を（Ｈ_ｘ、Ｈ_ｙ、Ｈ_ｚ）とし、前記地磁気センサの地磁気成分を（Ｂ_ｘ、Ｂ_ｙ、Ｂ_ｚ）としたとき、次式で示す外積演算を行って、前記磁北と直交する方向のベクトル成分を求める請求項９に記載の測位システム。
    

    
13. 前記外積演算して求めた前記磁北と直交する方向のベクトル成分を次式に基づき正規化する請求項１２に記載の測位システム。
    

    
14. 前記運動体が置かれる前記地磁気センサの磁気オフセット補正後の磁場ベクトルをＢ_０とし、補正前の外部磁場のベクトルをＢとしたとき、地磁気補正係数β_０を次式で算出し、
    

    

    重力方向と前記磁北と直交する方向のベクトル成分（Ｈｘ、Ｈｙ、Ｈｚ）の正規化した値を次式で表し、
    

    

    かつ前記地磁気補正係数をβとし、前記ジャイロセンサのｘ軸、ｙ軸、及びｚ軸のベクトル成分をＲ_Ｇ００、Ｒ_Ｇ０１、及びＲ_Ｇ０２とし、前記地磁気センサのｘ軸、ｙ軸、及びｚ軸のベクトル成分を地磁気成分Ｒ´_００、Ｒ´_０１、及びＲ´_０２で表すとき、前記磁北と直交する方向のベクトル成分を次式に基づき平滑化する請求項１３に記載の測位システム。
    

    
15. 前記磁北と直交する方向の平滑化された値を次式に基づき正規化し、
    

    

    前記磁北と直交する方向のベクトル成分を直交化するにあたり、前記加速度センサのベクトル成分を（Ａｘ、Ａｙ、Ａｚ）としたとき、次式に基づき直交化する請求項１４に記載の測位システム。
    

    
16. 前記磁北と直交する方向の直交化されたベクトル成分を次式で正規化し、
    

    

    前記磁北のベクトル成分を算出するにあたり、前記加速度センサの出力成分の平滑後に算出した正規化した値と、前記地磁気成分の直交化を行った後に算出した正規化した値との外積演算である次式で算出する請求項１５に記載の測位システム。
    

    
17. 前記磁北と直交する方向の正規化された値、前記磁北のベクトル成分を求めた外積値、及び前記加速度センサの出力成分の平滑化に算出した正規化した値をもとに前記ジャイロセンサの出力を次式に等価な回転行列に置き換える請求項１６に記載の測位システム。
    

    
18. ジャイロセンサ、地磁気センサ、及び加速度センサを有する運動体の測位方法であって、
    前記地磁気センサ及び前記加速度センサの出力成分に基づいて前記ジャイロセンサの出力成分を回転行列及びクオータニオンの少なくとも一方に変換し、前記回転行列または前記クオータニオンに基づき前記運動体の姿勢を算出する第１のステップを備え、前記ジャイロセンサの出力成分を補正するものであり、
    前記ジャイロセンサの出力成分の一部に前記地磁気センサ及び前記加速度センサの出力成分を加えて正規・直交化を行った後に前記回転行列または前記クオータニオンに変換する、測位方法。
19. ジャイロセンサ、地磁気センサ、及び加速度センサを有する運動体の測位方法であって、
    前記ジャイロセンサの出力を第１の回転行列で表現し、かつ前記地磁気センサ及び加速度センサの出力を第２の回転行列で表現する第１ステップと、前記加速度センサのｘ軸、ｙ軸、及びｚ軸のスカラー量を基にして、その値が０～１．０の範囲に選ばれる加速度補正係数αを算出する第２ステップと、地磁気センサのオフセットまたは伏角を用いて地磁気補正係数βを算出する第３ステップと、前記第２ステップで算出した前記加速度補正係数αを用いて、前記加速度センサのｘ軸、ｙ軸、及びｚ軸の成分の平滑化または平均化を行う第４ステップと、前記第３ステップで算出した前記地磁気補正係数βを用いて、前記加速度センサの成分と前記地磁気センサの成分との外積演算を行うステップと、前記外積演算の結果を回転行列に表現する第５ステップと、前記第５ステップで算出した回転行列をクオータニオンに変換する第６ステップとを備え、前記第６ステップで算出したクオータニオンに基づき前記運動体の姿勢を測る測位方法。
20. 請求項１～１７のいずれか一項に記載の測位システムが搭載される運動体。
21. 請求項１８または１９に記載の測位方法で制御される運動体。
22. 電子機器、スマートフォン、タブレット端末、スマートウオッチ、ＡＲ／ＶＲ装置、カメラ、ゲーム機器、車両、船舶、及び航空機のいずれか１つに搭載される請求項２０または２１に記載の運動体。

Images