測位システム、測位方法
【課題】慣性センサーデータとＧＰＳ測位データとを融合した高精度な測位方法を実現する。
【解決手段】測位方法は、慣性センサーデータとＧＰＳ測位データとを融合して移動体を測位する測位方法であって、慣性センサーデータに基づき、乗算型クォータニオン誤差モデルを用いる拡張カルマンフィルターの時間更新を行う工程と、ＧＰＳ測位データに基づき、拡張カルマンフィルターの観測更新を行い、位置誤差・速度誤差・方位角誤差、及びジャイロバイアス誤差・加速度バイアス誤差を推定する工程と、推定された位置誤差・速度誤差・方位角誤差・ジャイロバイアス誤差・加速度バイアス誤差に基づき、位置・速度・姿勢の各誤差を補正する工程と、を有する。
【解決手段】測位方法は、慣性センサーデータとＧＰＳ測位データとを融合して移動体を測位する測位方法であって、慣性センサーデータに基づき、乗算型クォータニオン誤差モデルを用いる拡張カルマンフィルターの時間更新を行う工程と、ＧＰＳ測位データに基づき、拡張カルマンフィルターの観測更新を行い、位置誤差・速度誤差・方位角誤差、及びジャイロバイアス誤差・加速度バイアス誤差を推定する工程と、推定された位置誤差・速度誤差・方位角誤差・ジャイロバイアス誤差・加速度バイアス誤差に基づき、位置・速度・姿勢の各誤差を補正する工程と、を有する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、測位システム、及び測位方法に関する。
【背景技術】
【０００２】
屋内やトンネル等、測位データが得にくい場所では、慣性航法データと測位データとを融合させることによって、屋内外を問わず、移動体の位置、速度、姿勢等の高精度な測位を行う。
慣性航法データと測位データとを融合させた測位システムや測位方法としては、慣性航法データと測位データを用いて、状態推定カルマンフィルターにより、移動体の位置、速度、姿勢の誤差補正を行うものがある。状態推定カルマンフィルターでは、要素数の少ない状態変数で特定される微小変化単位四元数を用いて、移動体の位置、速度、姿勢の誤差をモデリングし、誤差推定を行い、さらに、推定された誤差データを乗じることで、位置、速度、姿勢のデータを補正する測位システム及び測位方法が提案されている（例えば、特許文献１参照）。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２００７−２７６５０７号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
特許文献１では、慣性センサーデータとＧＰＳ測位データとを融合して、移動体の位置、速度、姿勢の補正を行っている。しかし、小型・軽量・ローコストの慣性センサー（例えば、ＭＥＭＳセンサー等）は、バイアス誤差およびランダムドリフトが大きく、移動体の位置、速度、姿勢の高精度な測位ができないという課題を有している。
【課題を解決するための手段】
【０００５】
本発明は、上述の課題の少なくとも一部を解決するためになされたものであり、以下の形態または適用例として実現することが可能である。
【０００６】
［適用例１］本適用例に係る測位システムは、慣性センサーデータとＧＰＳ測位データとに基づきジャイロバイアス誤差の推定及び加速度バイアス誤差の推定と、位置誤差・速度誤差・方位誤差の推定と、を行う拡張カルマンフィルター演算部と、前記ジャイロバイアス誤差に基づき角速度データのバイアス補正を行い、かつ、前記加速度バイアス誤差に基づき加速度データのバイアス補正を行うバイアス補正部と、前記バイアス補正された角速度データに基づき姿勢データを演算する姿勢演算部と、前記バイアス補正された加速度データおよび前記姿勢データに基づきナビゲーションフレームの加速度データを出力する座標変換部と、前記ナビゲーションフレームの加速度データに基づき位置・速度を演算する位置／速度演算部と、前記位置／速度演算部が演算した位置・速度および前記姿勢データを、前記拡張カルマンフィルター演算部から出力された位置・速度・方位の推定誤差に基づいて補正するエラー補正部と、を有し、前記拡張カルマンフィルター演算部は、前記位置／速度演算部が演算した前記位置・速度と、前記姿勢演算部が演算した前記姿勢データとに基いて、乗算型クォータニオン誤差モデルを用いて前記ジャイロバイアス誤差、前記加速度バイアス誤差および前記位置誤差・速度誤差・方位誤差の推定を行うことを特徴とする。
なお、慣性センサーデータとは、ジャイロセンサー及び加速度センサーにて検出する角速度データ、加速度データを意味する。
【０００７】
本適用例の測位システムによれば、乗算型クォータニオン（以降、乗算型Quaternionと表すことがある）誤差モデルを用いて姿勢誤差のモデリングを行い、拡張カルマンフィルター演算部によって移動体の位置、速度、姿勢の誤差を推定と、ジャイロセンサー及び加速度センサーの各バイアス誤差の推定と、誤差補正を行う。姿勢の乗算型クォータニオン誤差モデルは、加算型クォータニオン誤差モデルに比べ推定誤差の精度がよい。このことから、小型化、低コスト化が可能ではあるが、バイアス誤差及びランダムドリフトの変動が大きなＭＥＭＳ（Micro Electro Mechanical System）センサーを用いる場合でも、トンネルや屋内などＧＰＳ測位データが途切れる場所においても、高精度な測位が可能である。
【０００８】
［適用例２］本適用例に係る測位方法は、慣性センサーデータとＧＰＳ測位データとを融合して移動体の測位を行う測位方法であって、前記慣性センサーデータに基づき、乗算型クォータニオン誤差モデルを用いる拡張カルマンフィルターの時間更新を行う工程と、前記ＧＰＳ測位データに基づき、前記拡張カルマンフィルターの観測更新を行い、位置誤差・速度誤差・方位角誤差、及びジャイロバイアス誤差・加速度バイアス誤差を推定する工程と、前記推定された位置誤差・速度誤差・方位角誤差に基づき、位置・速度・姿勢の各誤差を補正する工程と、推定されたジャイロバイアス誤差・加速度バイアス誤差に基づき、慣性センサーデータを補正する工程とを有し、前記移動体の位置・速度・姿勢のうち少なくともいずれか一つを求めることを特徴とする。
なお、拡張カルマンフィルターの時間更新とは、状態ベクトルと誤差共分散を時間毎に更新することであり、観測更新とは、外部からの補正情報を得て状態ベクトルと誤差共分散とを更新することである。
【０００９】
本適用例の測位方法によれば、乗算型クォータニオン誤差モデルを用いて姿勢誤差をモデリングし、拡張カルマンフィルターの時間更新と観測更新を行い、ジャイロバイアス誤差・加速度バイアス誤差の推定及び誤差補正と、ＧＰＳ測位データ（位置、速度、方位）の誤差推定及び誤差補正とを融合させることにより、トンネルや屋内などにおいて、移動体の高精度な測位が可能である。
【００１０】
［適用例３］上記適用例に係る測位方法は、前記方位角誤差及び前記位置誤差の少なくとも一つを、乗算型クォータニオンを用いて誤差モデリングすることが好ましい。
【００１１】
このように、姿勢の誤差モデリングに加え、位置の誤差モデリングを行うことで、より高精度な移動体の測位を行うことができる。
【００１２】
［適用例４］上記適用例に係る測位方法は、外部から観測された距離情報及び相対速度情報を観測情報として用い、乗算型クォータニオン誤差モデルによって前記移動体の前記位置・速度・方位角の誤差、及び前記ジャイロバイアス誤差・加速度バイアス誤差の推定を行い、前記移動体の前記位置・速度・姿勢を補正することが好ましい。
ここで、外部から観測された距離情報及び相対速度情報（Ｍｅａｓｕｒｅｍｅｎｔと表すことがある）には、擬似距離（ｐｓｅｕｄｏｒａｎｇｅ）やドップラー効果（ｄｏｐｐｌｅｒ）を用いた相対速度情報が含まれる。
【００１３】
このように、Ｍｅａｓｕｒｅｍｅｎｔデータを、乗算型クォータニオン誤差モデルを用いて誤差補正することによっても、移動体の高精度な測位を行うことができる。
【００１４】
［適用例５］本適用例に係る測位方法は、前記移動体が移動している場合には、請求項２ないし請求項４のいずれか一項に記載の測位方法を用いて測位し、前記移動体が停止している場合には、速度を０、ヨー角を不変として、さらに加速度センサーの検出値からロール角とピッチ角とを演算して観測値とし、前記観測値に基づき、姿勢の補正及び前記ジャイロバイアス誤差及び前記加速度バイアス誤差の補正を行うことが好ましい。
【００１５】
移動体が停止している場合には、ＧＰＳ測位データの精度が低下する。そこで、速度０、ヨー角不変（δψ = 0）、さらに加速度センサーの検出値から算出されたロール角とピッチ角を観測値として乗算型クォータニオン誤差モデルを用いて、姿勢の補正及びジャイロバイアス誤差及び加速度バイアス誤差の補正を行うことで、停止時における移動体の位置、姿勢を検出することができる。
【図面の簡単な説明】
【００１６】
【図１】実施形態１に係る測位システムの概要を示す構成ブロック図。
【図２】実施形態１に係る演算制御部の概要を示す構成ブロック図。
【図３】実施形態１に係る測位方法の１例を示すフローチャート。
【図４】シミュレーションによる移動体の移動軌跡の１例を表すグラフ。
【図５】シミュレーションによる移動体の速度の１例を表すグラフ。
【図６】Ｚ軸方向のシミュレーションによる性能評価の結果。
【図７】姿勢誤差に係るシミュレーションによる性能評価の結果。
【図８】姿勢誤差に係るシミュレーションによる性能評価の結果。
【図９】ジャイロバイアス誤差に係るシミュレーションによる性能評価の結果。
【図１０】加速度バイアス誤差に係るシミュレーションによる性能評価の結果。
【発明を実施するための形態】
【００１７】
以下、本発明の実施の形態を図面を参照して説明する。
（実施形態１）
【００１８】
図１は、実施形態１に係る測位システムの概要を示す構成ブロック図である。測位システム１は、ジャイロセンサー２０と、加速度センサー３０と、ＧＰＳ（Global Positioning System）信号受信部４０と、ジャイロセンサー２０、加速度センサー３０、及びＧＰＳ信号受信部４０から入力されたデータを演算処理する演算制御部１０と、記憶部５０を含んで構成される。演算制御部１０は、移動体の位置、速度、姿勢を制御する駆動装置６２、或いは、必要なデータの形態で表示するための表示部６１に接続されている。
【００１９】
ジャイロセンサー２０は、移動体に配設され、例えば、水晶の振動型センサーやＭＥＭＳセンサーを利用し、微小振動板が回転により受けるコリオリ力に基づき、回転角速度を検出するものである。本実施形態では、ジャイロセンサー２０がボディーフレーム（ｂ‐ｆｒａｍｅ）の各軸まわりの回転角速度を検出するように複数個設けられ、軸まわりの回転角速度を表すデータを出力する。
【００２０】
加速度センサー３０は、移動体に配設され、少なくとも一つの方向に対する移動体の加速度を測定し出力する。本実施形態では、この加速度センサー３０は、水晶の振動型センサーやＭＥＭＳセンサーであり、ボディーフレームの各軸方向への移動体の移動加速度を測定し出力する。
【００２１】
ＧＰＳ信号受信部４０は、ＧＰＳ衛星からの信号を受信して、移動体の位置（緯度、経度、高度）を表すデータを出力する。本実施形態で用いられるＧＰＳ信号受信部４０は広く用いられているものであるので、ここでの詳細な説明を省略する。
【００２２】
演算制御部１０は、ＣＰＵ（Central Processing Unit）等のプログラム制御デバイスを用いて構成され、記憶部５０に格納されているプログラムに従って動作する。本実施形態では、この演算制御部１０が、ジャイロセンサー２０、加速度センサー３０やＧＰＳ信号受信部４０から出力される各データに基づいて、移動体の位置、速度、姿勢、およびジャイロセンサーバイアス、加速度センサーバイアスの各誤差推定と誤差補正を行い、移動体の表示部６１や駆動装置６２などの出力部に出力し、移動体を制御する。
この演算制御部１０の具体的な構成と作用については後に詳しく述べる。
【００２３】
記憶部５０は、ＲＡＭ（Random Access Memory）等の記憶素子であり、演算制御部１０によって実行されるプログラムを保持する。また、この記憶部５０は、演算制御部１０のワークメモリーとしても動作する。なお、記憶部５０は、演算制御部１０に内蔵する構成としてもよい。
【００２４】
以下の説明では、位置、速度、姿勢の各データは、どの座標系におけるデータであるかを識別するために、データにインデックスを付して、いずれかの座標系であるかを明確にしている。
（１）地球中心を原点として、地球の自転軸と平行になるようＺ軸を定めた右手系直交座標系を「ｉフレーム」とする。
（２）地球中心を原点として、緯度０度、経度０度方向をＸ軸とし、かつ地球の自転軸と平行になるようＺ軸を定めた右手系直交座標系を「ｅフレーム」とする。
（３）移動体を原点として、北方向をＸ軸、地球による重力の方向をＺ軸とした右手系直交座標系を「ｇフレーム」とする。
（４）移動体を原点として、地球による重力方向をＺ軸とした右手系直交座標系であって、ｇフレームにおけるＺ軸を中心にｇフレームをα（ｒａｄ）だけ回転させたときの座標系を「ｎフレーム（ｎ‐ｆｒａｍｅ）」とする。
（５）移動体を原点として、移動体の進行方向をＸ軸、揚力方向をＺ軸とした右手系直交座標系を「ｂフレーム（ｂ‐ｆｒａｍｅ）」とする。
【００２５】
なお、ｂフレームにおけるＸ軸まわりの回転を「ロール（ロール角φ）」、Ｙ軸まわりの回転を「ピッチ（ピッチ角θ）」、Ｚ軸まわりの回転を「ヨー（ヨー角ψ）」と称する。
【００２６】
続いて、演算制御部１０の構成及び作用について図面、及び数式を参照して説明する。
図２は、実施形態１に係る演算制御部の概要を示す構成ブロック図である。演算制御部１０は、バイアス補正部１１と、姿勢演算部１２と、座標変換部１３と、位置／速度演算部１４と、エラー補正部１５（ｅｒｒｏｒ補正部１５と表記）と、拡張カルマンフィルター演算部１６と、を主たる構成要素として構成している。
【００２７】
バイアス補正部１１は、拡張カルマンフィルター演算部１６によって算出されたジャイロバイアス誤差、加速度バイアス誤差の推定値を利用して、角速度データ及び加速度データの補正値を数式１を用いて算出し、加速度データ（ｂ‐ｆｒａｍｅ）は座標変換部１３に、角速度データは姿勢演算部１２に出力する。
【００２８】
【数１】
【００２９】
姿勢演算部１２は、移動体の姿勢に関わる運動方程式（数式２）を用いて、姿勢を求め、座標変換部１３、エラー補正部１５、及び拡張カルマンフィルター演算部１６に入力する。
【００３０】
【数２】
【００３１】
座標変換部１３は、加速度センサー３０が出力するｂ‐ｆｒａｍｅの加速度データと、姿勢演算部１２が出力する姿勢データとに基づき、数式３を用いて、ｎ‐ｆｒａｍｅの加速度データに変換して位置／速度演算部１４に出力する。
【００３２】
【数３】
【００３３】
位置／速度演算部１４は、位置および速度に関わる運動方程式（数式４）を用いて、位置と速度を演算してエラー補正部１５、及び拡張カルマンフィルター演算部１６に出力する。
【００３４】
【数４】
【００３５】
拡張カルマンフィルター演算部１６は、姿勢演算部１２から出力される姿勢データと、位置／速度演算部１４から出力された位置データ及び速度データと、ＧＰＳ測位データ（位置、速度、方位）を用いて、カルマンフィルターによって、位置、速度、方位の誤差を推定すると共に、ジャイロバイアス誤差及び加速度バイアス誤差を算出し、バイアス補正部１１に入力する。本実施形態では、速度、姿勢の運動方程式が非線形のため、カルマンフィルターを拡張した誤差モデルを適用する。誤差モデルは数式５によって表すことができる。
【００３６】
【数５】
【００３７】
ここでは、姿勢の誤差モデルは、Ｑｕａｔｅｒｎｉｏｎの加算ではなく、より要素数の少ない微小変化単位四元数を導入し、乗算型Ｑｕａｔｅｒｎｉｏｎを用いて誤差をモデリングする。なお、微小誤差を用いた加算型Ｑｕａｔｅｒｎｉｏｎの誤差モデルは、次式で表される。
【００３８】
【数６】
【００３９】
加算型Ｑｕａｔｅｒｎｉｏｎのノルムは、次式で表される。
【００４０】
【数７】
【００４１】
数式７に表されるように、微小変化単位四元数を用いた加算型Ｑｕａｔｅｒｎｉｏｎのノルムは、制約条件のノルム＝１を満足できない。
【００４２】
一方、微小誤差ベクトル要素を用いた乗算型Ｑｕａｔｅｒｎｉｏｎでは姿勢誤差を次式で表すことができる。
【００４３】
【数８】
【００４４】
乗算型Ｑｕａｔｅｒｎｉｏｎのノルムは次式で表すことができる。
【００４５】
【数９】
【００４６】
乗算型Ｑｕａｔｅｒｎｉｏｎのノルムは、数式９から分かるように、乗算型Ｑｕａｔｅｒｎｉｏｎ誤差モデルを利用することで、誤差を含んだ状態でも、ノルムが１であることが保証され、姿勢誤差ベクトルの要素数は４から３になり、推定する状態ベクトルの数を減少させることができる。
【００４７】
ここで、推定する誤差ベクトルは、数式１０で表すことができる。
【００４８】
【数１０】
【００４９】
また、ＧＰＳ測位データ（位置、速度、方位）を観測データとして、数式１１のような観測方程式、及び数式１２〜数式１４のような誤差共分散を算出することができる。
【００５０】
【数１１】
【００５１】
推定誤差共分散Ｐは次式で表される。
【００５２】
【数１２】
【００５３】
システム誤差共分散Ｑは次式で表される。
【００５４】
【数１３】
【００５５】
観測誤差共分散Ｒは次式で表される。
【００５６】
【数１４】
【００５７】
数式１２、数式１３から、カルマンフィルターの時間更新は、次式で表すことができる。
【００５８】
【数１５】
【００５９】
また、カルマンフィルターの観測更新は、数式１６、数式１７で表すことができる。
【００６０】
【数１６】
【００６１】
【数１７】
【００６２】
拡張カルマンフィルター演算部１６によって推定された位置、速度、方位角の誤差及びジャイロバイアス誤差及び加速度バイアス誤差の推定値を用いて、移動体の位置、速度、姿勢の補正と共に、ジャイロバイアス誤差及び加速度バイアス誤差の補正をエラー補正部（Ｅｒｒｏｒ補正部）１５で行い、出力部６０（表示部６１、駆動装置６２）に出力し、移動体を制御する。
【００６３】
次に、測位方法についてフローチャートを参照して説明する。
図３は、本実施形態に係る測位方法の１例を示すフローチャートである。図１、図２も参照する。まず、ジャイロセンサー２０によって角速度データ、加速度センサー３０によって加速度データを検出する（ＳＴ１）。これら検出した角速度データ及び加速度データを後述する工程（ＳＴ７）において推定されたジャイロバイアス誤差、加速度バイアス誤差に基づき、バイアス補正部１１にてバイアス補正を行う（ＳＴ２）。
【００６４】
続いて、バイアス補正された角速度データに基づき姿勢演算部１２で移動体の姿勢を演算する（ＳＴ３）。バイアス補正された加速度データ（ｂ‐ｆｒａｍｅ）は座標変換部１３でｎ‐ｆｒａｍｅデータに変換され、位置／速度演算部１４に入力される。位置／速度演算部１４では、移動体の現時点の位置及び速度を演算する（ＳＴ４）。
【００６５】
次に、乗算型Ｑｕａｔｅｒｎｉｏｎ誤差モデルを用いる拡張カルマンフィルターの時間更新を行う（ＳＴ５）。拡張カルマンフィルターは、時間更新と観測更新の２ステップ交代で構成される。ここで、時間更新とは、状態方程式を用いて、状態ベクトルと誤差共分散を時間毎に更新することであり、外部からの補正情報がなければ、誤差がどんどん蓄積されていく。また、観測更新とは、外部からの補正情報を得て、蓄積された誤差を補正して、状態ベクトルと誤差共分散とを更新することである。
【００６６】
次に、ＧＰＳ観測信号（ＧＰＳ測位データ）の検出の有無を判定する（ＳＴ６）。ＧＰＳ観測信号が検出されない（Ｎｏ）場合、ＳＴ１〜ＳＴ５のステップを繰り返す。ＧＰＳ観測信号を検出（Ｙｅｓ）した場合、拡張カルマンフィルター演算部１６において、乗算型Ｑｕａｔｅｒｎｉｏｎ誤差モデルを用いる拡張カルマンフィルターの観測更新を行い、位置・速度・姿勢、及びジャイロバイアス・加速度バイアスの各誤差を推定する（ＳＴ７）。
【００６７】
次に、ジャイロバイアス値及び加速度バイアス値の更新、位置・速度・姿勢の誤差補正を行い（ＳＴ８）、誤差が補正された位置・速度・姿勢データを移動体の出力部６０に出力する（ＳＴ９）。なお、ＳＴ８において、更新されたジャイロバイアス値及び加速度バイアス値は、バイアス補正部１１に入力され、更新された姿勢は姿勢演算部１２に入力され、更新された位置・速度情報は位置／速度演算部１４に入力され、図示するようなループで繰り返される。
（実施例）
【００６８】
次に、本実施形態の具体的な実施例について、乗算型Ｑｕａｔｅｒｎｉｏｎと加算型Ｑｕａｔｅｒｎｉｏｎの各誤差モデルの誤差推定の比較結果を図面を参照して説明する。まず、比較基礎となる移動体の移動軌跡、速度について説明する。
図４、図５は、移動体のシミュレーションによる移動軌跡と速度の１例を表すグラフである。図４（ａ）は移動体の移動軌跡（横軸は経度、縦軸は緯度）を表しており、図４（ｂ）は高度の移動（横軸は時間［ｔｉｍｅ：ｓｅｃ］、縦軸は高度［ｍ］）を表している。図５（ａ）は南北方向の移動、図５（ｂ）は東西方向の移動を表している。共に、横軸は時間（ｔｉｍｅ［ｓｅｃ］）、縦軸は速度［ｍ／ｓ］を表している。図５（ｃ）は水平面に対する垂直方向（Ｚ軸方向）の速度［ｍ／ｓ］を表している。つまり、本シミュレーションは、移動体は水平方向の移動をしている場合である。
【００６９】
続いて、図４、図５に示した移動体の移動軌跡に基づき、乗算型Ｑｕａｔｅｒｎｉｏｎ誤差モデルと加算型Ｑｕａｔｅｒｎｉｏｎ誤差モデルの性能比較結果について説明する。
【００７０】
図６は、Ｚ軸方向のシミュレーションによる性能評価の結果を表し、図６（ａ）は高度の変化、図６（ｂ）はＺ軸方向の速度変化［ｍ／ｓ］を表している。なお、図４（ｂ）で示したように、Ｚ軸方向では高度６８４ｍで水平移動をした場合であって、加算型Ｑｕａｔｅｒｎｉｏｎ誤差モデルに比べ、乗算型Ｑｕａｔｅｒｎｉｏｎ誤差モデルの方が精度がよいことが分かる。
【００７１】
次に、姿勢誤差のシミュレーション結果について説明する。
図７、図８は、姿勢誤差に係るシミュレーションによる性能評価の結果を表し、図７（ａ）はロール角誤差（φ［ｄｅｇ］）、図７（ｂ）はピッチ角誤差（θ［ｄｅｇ］）、図８はヨー角誤差（ψ［ｄｅｇ］）を表している。横軸は時間（ｔｉｍｅ[ｓｅｃ]）、縦軸は誤差［ｄｅｇ］である。
また、表１に、上記シミュレーション結果から演算して求めた各姿勢の分散値を表している。
【００７２】
【表１】
【００７３】
図７、図８及び表１からも分かるように、移動体の姿勢誤差、つまり、ロール角誤差、ピッチ角誤差、及びヨー角誤差においても、加算型Ｑｕａｔｅｒｎｉｏｎに比べ、乗算型Ｑｕａｔｅｒｎｉｏｎの方が誤差が小さくなる。
【００７４】
次に、バイアス誤差のシミュレーション結果について説明する。
図９は、バイアス誤差に係るシミュレーションによる性能評価の結果を表し、図９はＺ軸のジャイロバイアス誤差（Ｇｙｒｏ Ｂｉａｓ［ｒａｄ／Ｓ］）、図１０（a）はＸ軸の加速度バイアス誤差（Ａｃｃ Ｂｉａｓ［ｍ／ｓ／ｓ］）、図１０（b）は、Ｙ軸の加速度バイアス誤差（Ａｃｃ Ｂｉａｓ［ｍ／ｓ／ｓ］）を表している。共に横軸は時間（ｔｉｍｅ［ｓｅｃ］）である。
【００７５】
図９、図１０から分かるように、ジャイロバイアス誤差及び加速度バイアス誤差の推定値は、加算型Ｑｕａｔｅｒｎｉｏｎに比べ、乗算型Ｑｕａｔｅｒｎｉｏｎの方が誤差が小さくなる。
【００７６】
以上説明した本実施形態の測位システム及び測位方法によれば、慣性センサーデータ（加速度データ及び角速度データ）とＧＰＳ測位データを融合して、拡張カルマンフィルター（拡張カルマンフィルター演算部１６）によって移動体の位置、速度、姿勢の誤差を推定すると共に、ジャイロセンサー２０及び加速度センサー３０の各バイアス誤差の推定を行う。また、拡張カルマンフィルターでは、乗算型Ｑｕａｔｅｒｎｉｏｎ誤差モデルを用いて姿勢誤差を正確にモデリングできることから、位置、速度、姿勢およびセンサーバイアスの誤差を精度よく補正することができる。
【００７７】
従って、位置、速度、姿勢及びジャイロバイアス、加速度バイアスの各誤差を精度よく補正することができ、慣性センサーとして、小型、低コスト化が可能ではあるが、バイアス誤差及びランダムドリフトの変動が大きなＭＥＭＳセンサーを用いても、高精度、高安定なセンサー出力を得ることができることから、トンネルや屋内などＧＰＳ測位データが途切れる場所においても、高精度な測位が可能である。
（実施形態２）
【００７８】
続いて、実施形態２について説明する。前述した実施形態１が、要素数の少ない微小変化単位四元数を導入し、乗算型Ｑｕａｔｅｒｎｉｏｎを用いて姿勢誤差をモデリングしていることに対して、実施形態２は、姿勢誤差のモデルだけでなく、位置誤差モデルでも、乗算型Ｑｕａｔｅｒｎｉｏｎを用いて誤差をモデリングすることに特徴を有する。具体的には、数式１８に示すように誤差のモデリングを行う。
【００７９】
【数１８】
【００８０】
このように、姿勢の誤差モデリングに加え、位置の誤差モデリングを行うことで、乗算型クォータニオンを用いて正確に誤差のモデリングを行い、位置、速度、及びセンサーバイアスの誤差をより高精度に補正することができる。
（実施形態３）
【００８１】
続いて、実施形態３について説明する。前述した実施形態１は、ＧＰＳ信号受信部４０からの測位データ（位置、速度、方位角）を観測データとして利用し、拡張カルマンフィルターにより移動体の位置、速度、姿勢の誤差及びジャイロセンサー２０、加速度センサー３０のバイアス誤差を推定している。この実施形態１に対して、実施形態３では、外部から観測された少なくとも距離情報及び相対速度情報（ｍｅａｓｕｒｅｍｅｎｔ）を観測情報として用い、乗算型クォータニオン誤差モデルによって移動体の位置・速度・姿勢の誤差、及びジャイロバイアス誤差・加速度バイアス誤差の推定を行い、移動体の位置・速度・姿勢を補正することを特徴とする。
【００８２】
ｍｅａｓｕａｍｅｎｔとしては、擬似距離（ｐｓｕｄｏｒａｎｇｅ）やドップラー効果（ｄｏｐｐｌｅｒ）を用いた相対速度情報を用いることができ、このような手法を用いても、移動体の高精度な測位を行うことができる。
（実施形態４）
【００８３】
続いて、実施形態４について説明する。実施形態４は、移動体が移動している間には、前述した実施形態１、実施形態２、実施形態３に記載の測位方法を用いてＧＰＳ信号受信部４０からの測位データ（位置、速度、方位角）、または距離情報、相対速度情報を観測データとして利用して、移動体の位置、速度、姿勢の誤差及びジャイロセンサー２０、加速度センサー３０の各バイアス誤差を推定し、補正する。
ここで、移動体が静止している間はＧＰＳ信号受信部４０からの測位データの精度がよくないことから、実施形態４では、以下の観測データを利用して、位置、速度、姿勢の誤差、及び慣性センサーのバイアス誤差を推定する。
（１）速度をゼロ（Ｖ＝０）とする。
（２）ヨー角は不変（Δψ＝０）とする。
（３）移動体が静止しているため、加速度センサー３０の出力から次に示す数式も用いてロール（Ｒｏｌｌ）角とピッチ（Ｐｉｔｃｈ）角を求める。
【００８４】
【数１９】
【００８５】
このようにすれば、数式１９によって算出されたロール角とピッチ角と、ゼロ速度、ヨー角不変を観測値として、拡張カルマンフィルターに入れて、移動体が停止している場合においても、姿勢の補正、及びセンサーバイアスの補正ができる。
【符号の説明】
【００８６】
１…測位システム、１０…演算制御部、１１…バイアス補正部、１２…姿勢演算部、１３…座標変換部、１４…位置／速度演算部、１５…エラー（Ｅｒｒｏｒ）補正部、１６…拡張カルマンフィルター演算部、２０…ジャイロセンサー、３０…加速度センサー、４０…ＧＰＳ信号受信部。
【技術分野】
【０００１】
本発明は、測位システム、及び測位方法に関する。
【背景技術】
【０００２】
屋内やトンネル等、測位データが得にくい場所では、慣性航法データと測位データとを融合させることによって、屋内外を問わず、移動体の位置、速度、姿勢等の高精度な測位を行う。
慣性航法データと測位データとを融合させた測位システムや測位方法としては、慣性航法データと測位データを用いて、状態推定カルマンフィルターにより、移動体の位置、速度、姿勢の誤差補正を行うものがある。状態推定カルマンフィルターでは、要素数の少ない状態変数で特定される微小変化単位四元数を用いて、移動体の位置、速度、姿勢の誤差をモデリングし、誤差推定を行い、さらに、推定された誤差データを乗じることで、位置、速度、姿勢のデータを補正する測位システム及び測位方法が提案されている（例えば、特許文献１参照）。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２００７−２７６５０７号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
特許文献１では、慣性センサーデータとＧＰＳ測位データとを融合して、移動体の位置、速度、姿勢の補正を行っている。しかし、小型・軽量・ローコストの慣性センサー（例えば、ＭＥＭＳセンサー等）は、バイアス誤差およびランダムドリフトが大きく、移動体の位置、速度、姿勢の高精度な測位ができないという課題を有している。
【課題を解決するための手段】
【０００５】
本発明は、上述の課題の少なくとも一部を解決するためになされたものであり、以下の形態または適用例として実現することが可能である。
【０００６】
［適用例１］本適用例に係る測位システムは、慣性センサーデータとＧＰＳ測位データとに基づきジャイロバイアス誤差の推定及び加速度バイアス誤差の推定と、位置誤差・速度誤差・方位誤差の推定と、を行う拡張カルマンフィルター演算部と、前記ジャイロバイアス誤差に基づき角速度データのバイアス補正を行い、かつ、前記加速度バイアス誤差に基づき加速度データのバイアス補正を行うバイアス補正部と、前記バイアス補正された角速度データに基づき姿勢データを演算する姿勢演算部と、前記バイアス補正された加速度データおよび前記姿勢データに基づきナビゲーションフレームの加速度データを出力する座標変換部と、前記ナビゲーションフレームの加速度データに基づき位置・速度を演算する位置／速度演算部と、前記位置／速度演算部が演算した位置・速度および前記姿勢データを、前記拡張カルマンフィルター演算部から出力された位置・速度・方位の推定誤差に基づいて補正するエラー補正部と、を有し、前記拡張カルマンフィルター演算部は、前記位置／速度演算部が演算した前記位置・速度と、前記姿勢演算部が演算した前記姿勢データとに基いて、乗算型クォータニオン誤差モデルを用いて前記ジャイロバイアス誤差、前記加速度バイアス誤差および前記位置誤差・速度誤差・方位誤差の推定を行うことを特徴とする。
なお、慣性センサーデータとは、ジャイロセンサー及び加速度センサーにて検出する角速度データ、加速度データを意味する。
【０００７】
本適用例の測位システムによれば、乗算型クォータニオン（以降、乗算型Quaternionと表すことがある）誤差モデルを用いて姿勢誤差のモデリングを行い、拡張カルマンフィルター演算部によって移動体の位置、速度、姿勢の誤差を推定と、ジャイロセンサー及び加速度センサーの各バイアス誤差の推定と、誤差補正を行う。姿勢の乗算型クォータニオン誤差モデルは、加算型クォータニオン誤差モデルに比べ推定誤差の精度がよい。このことから、小型化、低コスト化が可能ではあるが、バイアス誤差及びランダムドリフトの変動が大きなＭＥＭＳ（Micro Electro Mechanical System）センサーを用いる場合でも、トンネルや屋内などＧＰＳ測位データが途切れる場所においても、高精度な測位が可能である。
【０００８】
［適用例２］本適用例に係る測位方法は、慣性センサーデータとＧＰＳ測位データとを融合して移動体の測位を行う測位方法であって、前記慣性センサーデータに基づき、乗算型クォータニオン誤差モデルを用いる拡張カルマンフィルターの時間更新を行う工程と、前記ＧＰＳ測位データに基づき、前記拡張カルマンフィルターの観測更新を行い、位置誤差・速度誤差・方位角誤差、及びジャイロバイアス誤差・加速度バイアス誤差を推定する工程と、前記推定された位置誤差・速度誤差・方位角誤差に基づき、位置・速度・姿勢の各誤差を補正する工程と、推定されたジャイロバイアス誤差・加速度バイアス誤差に基づき、慣性センサーデータを補正する工程とを有し、前記移動体の位置・速度・姿勢のうち少なくともいずれか一つを求めることを特徴とする。
なお、拡張カルマンフィルターの時間更新とは、状態ベクトルと誤差共分散を時間毎に更新することであり、観測更新とは、外部からの補正情報を得て状態ベクトルと誤差共分散とを更新することである。
【０００９】
本適用例の測位方法によれば、乗算型クォータニオン誤差モデルを用いて姿勢誤差をモデリングし、拡張カルマンフィルターの時間更新と観測更新を行い、ジャイロバイアス誤差・加速度バイアス誤差の推定及び誤差補正と、ＧＰＳ測位データ（位置、速度、方位）の誤差推定及び誤差補正とを融合させることにより、トンネルや屋内などにおいて、移動体の高精度な測位が可能である。
【００１０】
［適用例３］上記適用例に係る測位方法は、前記方位角誤差及び前記位置誤差の少なくとも一つを、乗算型クォータニオンを用いて誤差モデリングすることが好ましい。
【００１１】
このように、姿勢の誤差モデリングに加え、位置の誤差モデリングを行うことで、より高精度な移動体の測位を行うことができる。
【００１２】
［適用例４］上記適用例に係る測位方法は、外部から観測された距離情報及び相対速度情報を観測情報として用い、乗算型クォータニオン誤差モデルによって前記移動体の前記位置・速度・方位角の誤差、及び前記ジャイロバイアス誤差・加速度バイアス誤差の推定を行い、前記移動体の前記位置・速度・姿勢を補正することが好ましい。
ここで、外部から観測された距離情報及び相対速度情報（Ｍｅａｓｕｒｅｍｅｎｔと表すことがある）には、擬似距離（ｐｓｅｕｄｏｒａｎｇｅ）やドップラー効果（ｄｏｐｐｌｅｒ）を用いた相対速度情報が含まれる。
【００１３】
このように、Ｍｅａｓｕｒｅｍｅｎｔデータを、乗算型クォータニオン誤差モデルを用いて誤差補正することによっても、移動体の高精度な測位を行うことができる。
【００１４】
［適用例５］本適用例に係る測位方法は、前記移動体が移動している場合には、請求項２ないし請求項４のいずれか一項に記載の測位方法を用いて測位し、前記移動体が停止している場合には、速度を０、ヨー角を不変として、さらに加速度センサーの検出値からロール角とピッチ角とを演算して観測値とし、前記観測値に基づき、姿勢の補正及び前記ジャイロバイアス誤差及び前記加速度バイアス誤差の補正を行うことが好ましい。
【００１５】
移動体が停止している場合には、ＧＰＳ測位データの精度が低下する。そこで、速度０、ヨー角不変（δψ = 0）、さらに加速度センサーの検出値から算出されたロール角とピッチ角を観測値として乗算型クォータニオン誤差モデルを用いて、姿勢の補正及びジャイロバイアス誤差及び加速度バイアス誤差の補正を行うことで、停止時における移動体の位置、姿勢を検出することができる。
【図面の簡単な説明】
【００１６】
【図１】実施形態１に係る測位システムの概要を示す構成ブロック図。
【図２】実施形態１に係る演算制御部の概要を示す構成ブロック図。
【図３】実施形態１に係る測位方法の１例を示すフローチャート。
【図４】シミュレーションによる移動体の移動軌跡の１例を表すグラフ。
【図５】シミュレーションによる移動体の速度の１例を表すグラフ。
【図６】Ｚ軸方向のシミュレーションによる性能評価の結果。
【図７】姿勢誤差に係るシミュレーションによる性能評価の結果。
【図８】姿勢誤差に係るシミュレーションによる性能評価の結果。
【図９】ジャイロバイアス誤差に係るシミュレーションによる性能評価の結果。
【図１０】加速度バイアス誤差に係るシミュレーションによる性能評価の結果。
【発明を実施するための形態】
【００１７】
以下、本発明の実施の形態を図面を参照して説明する。
（実施形態１）
【００１８】
図１は、実施形態１に係る測位システムの概要を示す構成ブロック図である。測位システム１は、ジャイロセンサー２０と、加速度センサー３０と、ＧＰＳ（Global Positioning System）信号受信部４０と、ジャイロセンサー２０、加速度センサー３０、及びＧＰＳ信号受信部４０から入力されたデータを演算処理する演算制御部１０と、記憶部５０を含んで構成される。演算制御部１０は、移動体の位置、速度、姿勢を制御する駆動装置６２、或いは、必要なデータの形態で表示するための表示部６１に接続されている。
【００１９】
ジャイロセンサー２０は、移動体に配設され、例えば、水晶の振動型センサーやＭＥＭＳセンサーを利用し、微小振動板が回転により受けるコリオリ力に基づき、回転角速度を検出するものである。本実施形態では、ジャイロセンサー２０がボディーフレーム（ｂ‐ｆｒａｍｅ）の各軸まわりの回転角速度を検出するように複数個設けられ、軸まわりの回転角速度を表すデータを出力する。
【００２０】
加速度センサー３０は、移動体に配設され、少なくとも一つの方向に対する移動体の加速度を測定し出力する。本実施形態では、この加速度センサー３０は、水晶の振動型センサーやＭＥＭＳセンサーであり、ボディーフレームの各軸方向への移動体の移動加速度を測定し出力する。
【００２１】
ＧＰＳ信号受信部４０は、ＧＰＳ衛星からの信号を受信して、移動体の位置（緯度、経度、高度）を表すデータを出力する。本実施形態で用いられるＧＰＳ信号受信部４０は広く用いられているものであるので、ここでの詳細な説明を省略する。
【００２２】
演算制御部１０は、ＣＰＵ（Central Processing Unit）等のプログラム制御デバイスを用いて構成され、記憶部５０に格納されているプログラムに従って動作する。本実施形態では、この演算制御部１０が、ジャイロセンサー２０、加速度センサー３０やＧＰＳ信号受信部４０から出力される各データに基づいて、移動体の位置、速度、姿勢、およびジャイロセンサーバイアス、加速度センサーバイアスの各誤差推定と誤差補正を行い、移動体の表示部６１や駆動装置６２などの出力部に出力し、移動体を制御する。
この演算制御部１０の具体的な構成と作用については後に詳しく述べる。
【００２３】
記憶部５０は、ＲＡＭ（Random Access Memory）等の記憶素子であり、演算制御部１０によって実行されるプログラムを保持する。また、この記憶部５０は、演算制御部１０のワークメモリーとしても動作する。なお、記憶部５０は、演算制御部１０に内蔵する構成としてもよい。
【００２４】
以下の説明では、位置、速度、姿勢の各データは、どの座標系におけるデータであるかを識別するために、データにインデックスを付して、いずれかの座標系であるかを明確にしている。
（１）地球中心を原点として、地球の自転軸と平行になるようＺ軸を定めた右手系直交座標系を「ｉフレーム」とする。
（２）地球中心を原点として、緯度０度、経度０度方向をＸ軸とし、かつ地球の自転軸と平行になるようＺ軸を定めた右手系直交座標系を「ｅフレーム」とする。
（３）移動体を原点として、北方向をＸ軸、地球による重力の方向をＺ軸とした右手系直交座標系を「ｇフレーム」とする。
（４）移動体を原点として、地球による重力方向をＺ軸とした右手系直交座標系であって、ｇフレームにおけるＺ軸を中心にｇフレームをα（ｒａｄ）だけ回転させたときの座標系を「ｎフレーム（ｎ‐ｆｒａｍｅ）」とする。
（５）移動体を原点として、移動体の進行方向をＸ軸、揚力方向をＺ軸とした右手系直交座標系を「ｂフレーム（ｂ‐ｆｒａｍｅ）」とする。
【００２５】
なお、ｂフレームにおけるＸ軸まわりの回転を「ロール（ロール角φ）」、Ｙ軸まわりの回転を「ピッチ（ピッチ角θ）」、Ｚ軸まわりの回転を「ヨー（ヨー角ψ）」と称する。
【００２６】
続いて、演算制御部１０の構成及び作用について図面、及び数式を参照して説明する。
図２は、実施形態１に係る演算制御部の概要を示す構成ブロック図である。演算制御部１０は、バイアス補正部１１と、姿勢演算部１２と、座標変換部１３と、位置／速度演算部１４と、エラー補正部１５（ｅｒｒｏｒ補正部１５と表記）と、拡張カルマンフィルター演算部１６と、を主たる構成要素として構成している。
【００２７】
バイアス補正部１１は、拡張カルマンフィルター演算部１６によって算出されたジャイロバイアス誤差、加速度バイアス誤差の推定値を利用して、角速度データ及び加速度データの補正値を数式１を用いて算出し、加速度データ（ｂ‐ｆｒａｍｅ）は座標変換部１３に、角速度データは姿勢演算部１２に出力する。
【００２８】
【数１】
【００２９】
姿勢演算部１２は、移動体の姿勢に関わる運動方程式（数式２）を用いて、姿勢を求め、座標変換部１３、エラー補正部１５、及び拡張カルマンフィルター演算部１６に入力する。
【００３０】
【数２】
【００３１】
座標変換部１３は、加速度センサー３０が出力するｂ‐ｆｒａｍｅの加速度データと、姿勢演算部１２が出力する姿勢データとに基づき、数式３を用いて、ｎ‐ｆｒａｍｅの加速度データに変換して位置／速度演算部１４に出力する。
【００３２】
【数３】
【００３３】
位置／速度演算部１４は、位置および速度に関わる運動方程式（数式４）を用いて、位置と速度を演算してエラー補正部１５、及び拡張カルマンフィルター演算部１６に出力する。
【００３４】
【数４】
【００３５】
拡張カルマンフィルター演算部１６は、姿勢演算部１２から出力される姿勢データと、位置／速度演算部１４から出力された位置データ及び速度データと、ＧＰＳ測位データ（位置、速度、方位）を用いて、カルマンフィルターによって、位置、速度、方位の誤差を推定すると共に、ジャイロバイアス誤差及び加速度バイアス誤差を算出し、バイアス補正部１１に入力する。本実施形態では、速度、姿勢の運動方程式が非線形のため、カルマンフィルターを拡張した誤差モデルを適用する。誤差モデルは数式５によって表すことができる。
【００３６】
【数５】
【００３７】
ここでは、姿勢の誤差モデルは、Ｑｕａｔｅｒｎｉｏｎの加算ではなく、より要素数の少ない微小変化単位四元数を導入し、乗算型Ｑｕａｔｅｒｎｉｏｎを用いて誤差をモデリングする。なお、微小誤差を用いた加算型Ｑｕａｔｅｒｎｉｏｎの誤差モデルは、次式で表される。
【００３８】
【数６】
【００３９】
加算型Ｑｕａｔｅｒｎｉｏｎのノルムは、次式で表される。
【００４０】
【数７】
【００４１】
数式７に表されるように、微小変化単位四元数を用いた加算型Ｑｕａｔｅｒｎｉｏｎのノルムは、制約条件のノルム＝１を満足できない。
【００４２】
一方、微小誤差ベクトル要素を用いた乗算型Ｑｕａｔｅｒｎｉｏｎでは姿勢誤差を次式で表すことができる。
【００４３】
【数８】
【００４４】
乗算型Ｑｕａｔｅｒｎｉｏｎのノルムは次式で表すことができる。
【００４５】
【数９】
【００４６】
乗算型Ｑｕａｔｅｒｎｉｏｎのノルムは、数式９から分かるように、乗算型Ｑｕａｔｅｒｎｉｏｎ誤差モデルを利用することで、誤差を含んだ状態でも、ノルムが１であることが保証され、姿勢誤差ベクトルの要素数は４から３になり、推定する状態ベクトルの数を減少させることができる。
【００４７】
ここで、推定する誤差ベクトルは、数式１０で表すことができる。
【００４８】
【数１０】
【００４９】
また、ＧＰＳ測位データ（位置、速度、方位）を観測データとして、数式１１のような観測方程式、及び数式１２〜数式１４のような誤差共分散を算出することができる。
【００５０】
【数１１】
【００５１】
推定誤差共分散Ｐは次式で表される。
【００５２】
【数１２】
【００５３】
システム誤差共分散Ｑは次式で表される。
【００５４】
【数１３】
【００５５】
観測誤差共分散Ｒは次式で表される。
【００５６】
【数１４】
【００５７】
数式１２、数式１３から、カルマンフィルターの時間更新は、次式で表すことができる。
【００５８】
【数１５】
【００５９】
また、カルマンフィルターの観測更新は、数式１６、数式１７で表すことができる。
【００６０】
【数１６】
【００６１】
【数１７】
【００６２】
拡張カルマンフィルター演算部１６によって推定された位置、速度、方位角の誤差及びジャイロバイアス誤差及び加速度バイアス誤差の推定値を用いて、移動体の位置、速度、姿勢の補正と共に、ジャイロバイアス誤差及び加速度バイアス誤差の補正をエラー補正部（Ｅｒｒｏｒ補正部）１５で行い、出力部６０（表示部６１、駆動装置６２）に出力し、移動体を制御する。
【００６３】
次に、測位方法についてフローチャートを参照して説明する。
図３は、本実施形態に係る測位方法の１例を示すフローチャートである。図１、図２も参照する。まず、ジャイロセンサー２０によって角速度データ、加速度センサー３０によって加速度データを検出する（ＳＴ１）。これら検出した角速度データ及び加速度データを後述する工程（ＳＴ７）において推定されたジャイロバイアス誤差、加速度バイアス誤差に基づき、バイアス補正部１１にてバイアス補正を行う（ＳＴ２）。
【００６４】
続いて、バイアス補正された角速度データに基づき姿勢演算部１２で移動体の姿勢を演算する（ＳＴ３）。バイアス補正された加速度データ（ｂ‐ｆｒａｍｅ）は座標変換部１３でｎ‐ｆｒａｍｅデータに変換され、位置／速度演算部１４に入力される。位置／速度演算部１４では、移動体の現時点の位置及び速度を演算する（ＳＴ４）。
【００６５】
次に、乗算型Ｑｕａｔｅｒｎｉｏｎ誤差モデルを用いる拡張カルマンフィルターの時間更新を行う（ＳＴ５）。拡張カルマンフィルターは、時間更新と観測更新の２ステップ交代で構成される。ここで、時間更新とは、状態方程式を用いて、状態ベクトルと誤差共分散を時間毎に更新することであり、外部からの補正情報がなければ、誤差がどんどん蓄積されていく。また、観測更新とは、外部からの補正情報を得て、蓄積された誤差を補正して、状態ベクトルと誤差共分散とを更新することである。
【００６６】
次に、ＧＰＳ観測信号（ＧＰＳ測位データ）の検出の有無を判定する（ＳＴ６）。ＧＰＳ観測信号が検出されない（Ｎｏ）場合、ＳＴ１〜ＳＴ５のステップを繰り返す。ＧＰＳ観測信号を検出（Ｙｅｓ）した場合、拡張カルマンフィルター演算部１６において、乗算型Ｑｕａｔｅｒｎｉｏｎ誤差モデルを用いる拡張カルマンフィルターの観測更新を行い、位置・速度・姿勢、及びジャイロバイアス・加速度バイアスの各誤差を推定する（ＳＴ７）。
【００６７】
次に、ジャイロバイアス値及び加速度バイアス値の更新、位置・速度・姿勢の誤差補正を行い（ＳＴ８）、誤差が補正された位置・速度・姿勢データを移動体の出力部６０に出力する（ＳＴ９）。なお、ＳＴ８において、更新されたジャイロバイアス値及び加速度バイアス値は、バイアス補正部１１に入力され、更新された姿勢は姿勢演算部１２に入力され、更新された位置・速度情報は位置／速度演算部１４に入力され、図示するようなループで繰り返される。
（実施例）
【００６８】
次に、本実施形態の具体的な実施例について、乗算型Ｑｕａｔｅｒｎｉｏｎと加算型Ｑｕａｔｅｒｎｉｏｎの各誤差モデルの誤差推定の比較結果を図面を参照して説明する。まず、比較基礎となる移動体の移動軌跡、速度について説明する。
図４、図５は、移動体のシミュレーションによる移動軌跡と速度の１例を表すグラフである。図４（ａ）は移動体の移動軌跡（横軸は経度、縦軸は緯度）を表しており、図４（ｂ）は高度の移動（横軸は時間［ｔｉｍｅ：ｓｅｃ］、縦軸は高度［ｍ］）を表している。図５（ａ）は南北方向の移動、図５（ｂ）は東西方向の移動を表している。共に、横軸は時間（ｔｉｍｅ［ｓｅｃ］）、縦軸は速度［ｍ／ｓ］を表している。図５（ｃ）は水平面に対する垂直方向（Ｚ軸方向）の速度［ｍ／ｓ］を表している。つまり、本シミュレーションは、移動体は水平方向の移動をしている場合である。
【００６９】
続いて、図４、図５に示した移動体の移動軌跡に基づき、乗算型Ｑｕａｔｅｒｎｉｏｎ誤差モデルと加算型Ｑｕａｔｅｒｎｉｏｎ誤差モデルの性能比較結果について説明する。
【００７０】
図６は、Ｚ軸方向のシミュレーションによる性能評価の結果を表し、図６（ａ）は高度の変化、図６（ｂ）はＺ軸方向の速度変化［ｍ／ｓ］を表している。なお、図４（ｂ）で示したように、Ｚ軸方向では高度６８４ｍで水平移動をした場合であって、加算型Ｑｕａｔｅｒｎｉｏｎ誤差モデルに比べ、乗算型Ｑｕａｔｅｒｎｉｏｎ誤差モデルの方が精度がよいことが分かる。
【００７１】
次に、姿勢誤差のシミュレーション結果について説明する。
図７、図８は、姿勢誤差に係るシミュレーションによる性能評価の結果を表し、図７（ａ）はロール角誤差（φ［ｄｅｇ］）、図７（ｂ）はピッチ角誤差（θ［ｄｅｇ］）、図８はヨー角誤差（ψ［ｄｅｇ］）を表している。横軸は時間（ｔｉｍｅ[ｓｅｃ]）、縦軸は誤差［ｄｅｇ］である。
また、表１に、上記シミュレーション結果から演算して求めた各姿勢の分散値を表している。
【００７２】
【表１】
【００７３】
図７、図８及び表１からも分かるように、移動体の姿勢誤差、つまり、ロール角誤差、ピッチ角誤差、及びヨー角誤差においても、加算型Ｑｕａｔｅｒｎｉｏｎに比べ、乗算型Ｑｕａｔｅｒｎｉｏｎの方が誤差が小さくなる。
【００７４】
次に、バイアス誤差のシミュレーション結果について説明する。
図９は、バイアス誤差に係るシミュレーションによる性能評価の結果を表し、図９はＺ軸のジャイロバイアス誤差（Ｇｙｒｏ Ｂｉａｓ［ｒａｄ／Ｓ］）、図１０（a）はＸ軸の加速度バイアス誤差（Ａｃｃ Ｂｉａｓ［ｍ／ｓ／ｓ］）、図１０（b）は、Ｙ軸の加速度バイアス誤差（Ａｃｃ Ｂｉａｓ［ｍ／ｓ／ｓ］）を表している。共に横軸は時間（ｔｉｍｅ［ｓｅｃ］）である。
【００７５】
図９、図１０から分かるように、ジャイロバイアス誤差及び加速度バイアス誤差の推定値は、加算型Ｑｕａｔｅｒｎｉｏｎに比べ、乗算型Ｑｕａｔｅｒｎｉｏｎの方が誤差が小さくなる。
【００７６】
以上説明した本実施形態の測位システム及び測位方法によれば、慣性センサーデータ（加速度データ及び角速度データ）とＧＰＳ測位データを融合して、拡張カルマンフィルター（拡張カルマンフィルター演算部１６）によって移動体の位置、速度、姿勢の誤差を推定すると共に、ジャイロセンサー２０及び加速度センサー３０の各バイアス誤差の推定を行う。また、拡張カルマンフィルターでは、乗算型Ｑｕａｔｅｒｎｉｏｎ誤差モデルを用いて姿勢誤差を正確にモデリングできることから、位置、速度、姿勢およびセンサーバイアスの誤差を精度よく補正することができる。
【００７７】
従って、位置、速度、姿勢及びジャイロバイアス、加速度バイアスの各誤差を精度よく補正することができ、慣性センサーとして、小型、低コスト化が可能ではあるが、バイアス誤差及びランダムドリフトの変動が大きなＭＥＭＳセンサーを用いても、高精度、高安定なセンサー出力を得ることができることから、トンネルや屋内などＧＰＳ測位データが途切れる場所においても、高精度な測位が可能である。
（実施形態２）
【００７８】
続いて、実施形態２について説明する。前述した実施形態１が、要素数の少ない微小変化単位四元数を導入し、乗算型Ｑｕａｔｅｒｎｉｏｎを用いて姿勢誤差をモデリングしていることに対して、実施形態２は、姿勢誤差のモデルだけでなく、位置誤差モデルでも、乗算型Ｑｕａｔｅｒｎｉｏｎを用いて誤差をモデリングすることに特徴を有する。具体的には、数式１８に示すように誤差のモデリングを行う。
【００７９】
【数１８】
【００８０】
このように、姿勢の誤差モデリングに加え、位置の誤差モデリングを行うことで、乗算型クォータニオンを用いて正確に誤差のモデリングを行い、位置、速度、及びセンサーバイアスの誤差をより高精度に補正することができる。
（実施形態３）
【００８１】
続いて、実施形態３について説明する。前述した実施形態１は、ＧＰＳ信号受信部４０からの測位データ（位置、速度、方位角）を観測データとして利用し、拡張カルマンフィルターにより移動体の位置、速度、姿勢の誤差及びジャイロセンサー２０、加速度センサー３０のバイアス誤差を推定している。この実施形態１に対して、実施形態３では、外部から観測された少なくとも距離情報及び相対速度情報（ｍｅａｓｕｒｅｍｅｎｔ）を観測情報として用い、乗算型クォータニオン誤差モデルによって移動体の位置・速度・姿勢の誤差、及びジャイロバイアス誤差・加速度バイアス誤差の推定を行い、移動体の位置・速度・姿勢を補正することを特徴とする。
【００８２】
ｍｅａｓｕａｍｅｎｔとしては、擬似距離（ｐｓｕｄｏｒａｎｇｅ）やドップラー効果（ｄｏｐｐｌｅｒ）を用いた相対速度情報を用いることができ、このような手法を用いても、移動体の高精度な測位を行うことができる。
（実施形態４）
【００８３】
続いて、実施形態４について説明する。実施形態４は、移動体が移動している間には、前述した実施形態１、実施形態２、実施形態３に記載の測位方法を用いてＧＰＳ信号受信部４０からの測位データ（位置、速度、方位角）、または距離情報、相対速度情報を観測データとして利用して、移動体の位置、速度、姿勢の誤差及びジャイロセンサー２０、加速度センサー３０の各バイアス誤差を推定し、補正する。
ここで、移動体が静止している間はＧＰＳ信号受信部４０からの測位データの精度がよくないことから、実施形態４では、以下の観測データを利用して、位置、速度、姿勢の誤差、及び慣性センサーのバイアス誤差を推定する。
（１）速度をゼロ（Ｖ＝０）とする。
（２）ヨー角は不変（Δψ＝０）とする。
（３）移動体が静止しているため、加速度センサー３０の出力から次に示す数式も用いてロール（Ｒｏｌｌ）角とピッチ（Ｐｉｔｃｈ）角を求める。
【００８４】
【数１９】
【００８５】
このようにすれば、数式１９によって算出されたロール角とピッチ角と、ゼロ速度、ヨー角不変を観測値として、拡張カルマンフィルターに入れて、移動体が停止している場合においても、姿勢の補正、及びセンサーバイアスの補正ができる。
【符号の説明】
【００８６】
１…測位システム、１０…演算制御部、１１…バイアス補正部、１２…姿勢演算部、１３…座標変換部、１４…位置／速度演算部、１５…エラー（Ｅｒｒｏｒ）補正部、１６…拡張カルマンフィルター演算部、２０…ジャイロセンサー、３０…加速度センサー、４０…ＧＰＳ信号受信部。
【特許請求の範囲】
【請求項１】
慣性センサーデータとＧＰＳ測位データとに基づきジャイロバイアス誤差の推定及び加速度バイアス誤差の推定と、位置誤差・速度誤差・方位誤差の推定と、を行う拡張カルマンフィルター演算部と、
前記ジャイロバイアス誤差に基づき角速度データのバイアス補正を行い、かつ、前記加速度バイアス誤差に基づき加速度データのバイアス補正を行うバイアス補正部と、
前記バイアス補正された角速度データに基づき姿勢データを演算する姿勢演算部と、
前記バイアス補正された加速度データおよび前記姿勢データに基づきナビゲーションフレームの加速度データを出力する座標変換部と、
前記ナビゲーションフレームの加速度データに基づき位置・速度を演算する位置／速度演算部と、
前記位置／速度演算部が演算した位置・速度および前記姿勢データを、前記拡張カルマンフィルター演算部から出力された位置・速度・方位の推定誤差に基づいて補正するエラー補正部と、
を有し、
前記拡張カルマンフィルター演算部は、前記位置／速度演算部が演算した前記位置・速度と、前記姿勢演算部が演算した前記姿勢データとに基いて、乗算型クォータニオン誤差モデルを用いて前記ジャイロバイアス誤差、前記加速度バイアス誤差および前記位置誤差・速度誤差・方位誤差の推定を行うことを特徴とする測位システム。
【請求項２】
慣性センサーデータとＧＰＳ測位データとを融合して移動体の測位を行う測位方法であって、
前記慣性センサーデータに基づき、乗算型クォータニオン誤差モデルを用いる拡張カルマンフィルターの時間更新を行う工程と、
前記ＧＰＳ測位データに基づき、前記拡張カルマンフィルターの観測更新を行い、位置誤差・速度誤差・方位角誤差、及びジャイロバイアス誤差・加速度バイアス誤差を推定する工程と、
前記推定された位置誤差・速度誤差・方位角誤差に基づき、位置・速度・姿勢の各誤差を補正する工程と、
推定されたジャイロバイアス誤差・加速度バイアス誤差に基づき、慣性センサーデータを補正する工程と
を有し、
前記移動体の位置・速度・姿勢のうち少なくともいずれか一つを求めることを特徴とする測位方法。
【請求項３】
請求項２に記載の測位方法において、
前記方位角誤差及び前記位置誤差の少なくとも一つを、乗算型クォータニオンを用いて誤差モデリングすることを特徴とする測位方法。
【請求項４】
請求項２に記載の測位方法において、
外部から観測された距離情報及び相対速度情報を観測情報として用い、乗算型クォータニオン誤差モデルによって前記移動体の前記位置・速度・方位角の誤差、及び前記ジャイロバイアス誤差・加速度バイアス誤差の推定を行い、前記移動体の前記位置・速度・姿勢を補正することを特徴とする測位方法。
【請求項５】
前記移動体が移動している場合には、請求項２ないし請求項４のいずれか一項に記載の測位方法を用いて測位し、
前記移動体が停止している場合には、速度を０、ヨー角を不変として、さらに加速度センサーの検出値からロール角とピッチ角とを演算して観測値とし、前記観測値に基づき、姿勢の補正及び前記ジャイロバイアス誤差及び前記加速度バイアス誤差の補正を行うことを特徴とする測位方法。
【請求項１】
慣性センサーデータとＧＰＳ測位データとに基づきジャイロバイアス誤差の推定及び加速度バイアス誤差の推定と、位置誤差・速度誤差・方位誤差の推定と、を行う拡張カルマンフィルター演算部と、
前記ジャイロバイアス誤差に基づき角速度データのバイアス補正を行い、かつ、前記加速度バイアス誤差に基づき加速度データのバイアス補正を行うバイアス補正部と、
前記バイアス補正された角速度データに基づき姿勢データを演算する姿勢演算部と、
前記バイアス補正された加速度データおよび前記姿勢データに基づきナビゲーションフレームの加速度データを出力する座標変換部と、
前記ナビゲーションフレームの加速度データに基づき位置・速度を演算する位置／速度演算部と、
前記位置／速度演算部が演算した位置・速度および前記姿勢データを、前記拡張カルマンフィルター演算部から出力された位置・速度・方位の推定誤差に基づいて補正するエラー補正部と、
を有し、
前記拡張カルマンフィルター演算部は、前記位置／速度演算部が演算した前記位置・速度と、前記姿勢演算部が演算した前記姿勢データとに基いて、乗算型クォータニオン誤差モデルを用いて前記ジャイロバイアス誤差、前記加速度バイアス誤差および前記位置誤差・速度誤差・方位誤差の推定を行うことを特徴とする測位システム。
【請求項２】
慣性センサーデータとＧＰＳ測位データとを融合して移動体の測位を行う測位方法であって、
前記慣性センサーデータに基づき、乗算型クォータニオン誤差モデルを用いる拡張カルマンフィルターの時間更新を行う工程と、
前記ＧＰＳ測位データに基づき、前記拡張カルマンフィルターの観測更新を行い、位置誤差・速度誤差・方位角誤差、及びジャイロバイアス誤差・加速度バイアス誤差を推定する工程と、
前記推定された位置誤差・速度誤差・方位角誤差に基づき、位置・速度・姿勢の各誤差を補正する工程と、
推定されたジャイロバイアス誤差・加速度バイアス誤差に基づき、慣性センサーデータを補正する工程と
を有し、
前記移動体の位置・速度・姿勢のうち少なくともいずれか一つを求めることを特徴とする測位方法。
【請求項３】
請求項２に記載の測位方法において、
前記方位角誤差及び前記位置誤差の少なくとも一つを、乗算型クォータニオンを用いて誤差モデリングすることを特徴とする測位方法。
【請求項４】
請求項２に記載の測位方法において、
外部から観測された距離情報及び相対速度情報を観測情報として用い、乗算型クォータニオン誤差モデルによって前記移動体の前記位置・速度・方位角の誤差、及び前記ジャイロバイアス誤差・加速度バイアス誤差の推定を行い、前記移動体の前記位置・速度・姿勢を補正することを特徴とする測位方法。
【請求項５】
前記移動体が移動している場合には、請求項２ないし請求項４のいずれか一項に記載の測位方法を用いて測位し、
前記移動体が停止している場合には、速度を０、ヨー角を不変として、さらに加速度センサーの検出値からロール角とピッチ角とを演算して観測値とし、前記観測値に基づき、姿勢の補正及び前記ジャイロバイアス誤差及び前記加速度バイアス誤差の補正を行うことを特徴とする測位方法。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【公開番号】特開２０１２−１７３１９０（Ｐ２０１２−１７３１９０Ａ）
【公開日】平成２４年９月１０日（２０１２．９．１０）
【国際特許分類】
【出願番号】特願２０１１−３６８０６（Ｐ２０１１−３６８０６）
【出願日】平成２３年２月２３日（２０１１．２．２３）
【出願人】（０００００２３６９）セイコーエプソン株式会社 (51,324)
【Ｆターム（参考）】
【公開日】平成２４年９月１０日（２０１２．９．１０）
【国際特許分類】
【出願日】平成２３年２月２３日（２０１１．２．２３）
【出願人】（０００００２３６９）セイコーエプソン株式会社 (51,324)
【Ｆターム（参考）】
[ Back to top ]