位置標定装置、位置標定装置の位置標定方法および位置標定プログラム
【課題】航法の最初から慣性計測装置のデータを高い精度で補正して高い精度で位置を標定できるようにすることを目的とする。
【解決手段】自己位置標定装置１００は航法処理を２回行う。１、２回目の航法処理において、ＩＭＵ処理部１４０はカルマンフィルタ１５０により算出されるＩＭＵ誤差推定値に基づいて慣性データを補正し、補正した慣性データに基づいて慣性航法により位置、姿勢および速度を算出する。カルマンフィルタ１５０はＧＰＳ処理部１２０またはＯＤＯ処理部１３０により算出される残差に基づいてＩＭＵ誤差推定値を算出する。２回目の航法処理において、ＩＭＵ処理部１４０は１回目の航法処理においてカルマンフィルタ１５０により算出されたＩＭＵ誤差推定値をＩＭＵ誤差推定値の初期値として用いる。ＩＭＵ処理部１４０は２回目の航法処理で算出した位置、姿勢および速度を航法結果として出力する。
【解決手段】自己位置標定装置１００は航法処理を２回行う。１、２回目の航法処理において、ＩＭＵ処理部１４０はカルマンフィルタ１５０により算出されるＩＭＵ誤差推定値に基づいて慣性データを補正し、補正した慣性データに基づいて慣性航法により位置、姿勢および速度を算出する。カルマンフィルタ１５０はＧＰＳ処理部１２０またはＯＤＯ処理部１３０により算出される残差に基づいてＩＭＵ誤差推定値を算出する。２回目の航法処理において、ＩＭＵ処理部１４０は１回目の航法処理においてカルマンフィルタ１５０により算出されたＩＭＵ誤差推定値をＩＭＵ誤差推定値の初期値として用いる。ＩＭＵ処理部１４０は２回目の航法処理で算出した位置、姿勢および速度を航法結果として出力する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、例えば、ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）や慣性計測装置のデータを用いて位置を標定する位置標定装置、位置標定方法および位置標定プログラムに関するものである。
【背景技術】
【０００２】
ＧＰＳを利用する航法装置は、その特性上、慣性計測装置（ＩＭＵ：Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ）やオドメトリ（ＯＤＯｍｅｔｒｙ）と組み合わせて、それぞれのセンサの長所を利用した構成とするのが一般的である。しかし、慣性計測装置にはバイアスとスケールファクタ誤差が、オドメトリにはスケールファクタ誤差とオフセット誤差が存在する。これらの誤差は、ＧＰＳを使用できる時間が十分にあれば推定・補正することができる。しかし、計測の開始時は誤差量が不明であるため通常、補正を行わない状態（補正量０）となっている。よって、誤差の推定・補正を十分に行えるようになる前にＧＰＳを使用できない状況が発生した場合、慣性航法装置とオドメトリとを用いた自律航法（ＩＭＵ／ＯＤＯ）の誤差が顕著となる。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特許３８７５７１４号公報
【特許文献２】特開２００６−１３８８３４号公報
【特許文献３】特開２００８−２４９６３９号公報
【非特許文献】
【０００４】
【非特許文献１】山口裕之，浅野勝宏，天野也寸志，“車体横すべり角推定法の開発”，日本機械学會論文集Ｃ編６７（６５９），ｐｐ．２２９１−２２９８，２００１
【発明の概要】
【発明が解決しようとする課題】
【０００５】
本発明は、例えば、航法の最初から慣性計測装置やオドメトリのデータを補正して高い精度で位置を標定できるようにすることを目的とする。
【課題を解決するための手段】
【０００６】
本発明の位置標定装置は、
移動体が備えるＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）受信機により異なる時刻に取得された複数のＧＰＳデータと、前記移動体が備える慣性計測装置により異なる時刻に取得された複数の慣性データとを記憶する航法データ記憶部と、
前記航法データ記憶部に記憶された複数のＧＰＳデータと複数の慣性データとに基づいて前記慣性計測装置の計測誤差の推定値として誤差推定初期値を算出する誤差推定部と、
前記複数の慣性データのうち最初に取得された先頭慣性データを前記誤差推定部により算出された誤差推定初期値に基づいて補正する慣性データ補正部と、
前記慣性データ補正部により補正された先頭慣性データに基づいて前記先頭慣性データが取得された先頭時刻における前記移動体の位置として先頭時刻の標定位置を算出する慣性航法部とを備える。
【発明の効果】
【０００７】
本発明によれば、例えば、慣性計測装置の先頭慣性データを補正して先頭時刻の位置を高い精度で標定することができる。
【図面の簡単な説明】
【０００８】
【図１】実施の形態１における自己位置標定装置１００の機能構成図。
【図２】実施の形態１における車両２００の構成図。
【図３】実施の形態１における自己位置標定装置１００の位置標定方法を示すフローチャート。
【図４】実施の形態１の位置標定方法において１回目の航法処理（Ｓ１００）で算出される誤差推定値（および分散）の時間変化を示すグラフ。
【図５】実施の形態１の位置標定方法において２回目の航法処理（Ｓ２００）で算出される誤差推定値（および分散）の時間変化を示すグラフ。
【図６】実施の形態１における位置標定方法の１回目の航法処理（Ｓ１００）と２回目の航法処理（Ｓ２００）とを示すフローチャート。
【図７】実施の形態１におけるＧＰＳ／ＩＮＳ複合航法処理（Ｓ３００）を示すフローチャート。
【図８】実施の形態１におけるＯＤＯ／ＩＮＳ複合航法処理（Ｓ４００）を示すフローチャート。
【図９】実施の形態１における自己位置標定装置１００のＧＰＳ処理部１２０の機能構成図。
【図１０】実施の形態１における二重位相差残差算出処理（Ｓ３１０）を示すフローチャート。
【図１１】実施の形態１における自己位置標定装置１００のＯＤＯ処理部１３０とＩＭＵ処理部１４０との機能構成図。
【図１２】実施の形態１における車両座標系ｘｙｚとオドメトリ２３０のオフセット誤差ｏｓとの関係図。
【図１３】実施の形態１における横滑り角βを示す図。
【図１４】実施の形態１におけるコーナリングパワーＳｓｌｉｐとオフセット誤差ｏｓ［１］とを表すグラフ。
【図１５】実施の形態１におけるオドメトリ２３０のスケールファクタ誤差を表すグラフ。
【図１６】実施の形態１における横滑り特性学習処理（Ｓ５００）を示すフローチャート。
【図１７】実施の形態１における速度残差算出処理（Ｓ４１０）を示すフローチャート。
【図１８】実施の形態１における自己位置標定装置１００のハードウェア資源の一例を示す図。
【図１９】実施の形態２における自己位置標定装置１００の機能構成図。
【発明を実施するための形態】
【０００９】
実施の形態１．
車両（移動体の一例）の位置をＧＰＳ、慣性計測装置およびオドメトリを用いて標定する形態について説明する。
【００１０】
図１は、実施の形態１における自己位置標定装置１００の機能構成図である。
実施の形態１における自己位置標定装置１００の機能構成について、図１に基づいて以下に説明する。
【００１１】
自己位置標定装置１００（位置標定装置の一例）は、横滑り特性学習部１１０、ＧＰＳ処理部１２０、ＯＤＯ処理部１３０、ＩＭＵ処理部１４０、カルマンフィルタ１５０および航法データ記憶部１９０を備える。
【００１２】
航法データ記憶部１９０は、ＧＰＳデータ群１９１、慣性データ群１９３およびオドメトリデータ群１９２を記憶する。
ＧＰＳデータ群１９１は、車両が備えるＧＰＳ受信機により異なる時刻に取得された複数のＧＰＳデータである。
慣性データ群１９３は、車両が備える慣性計測装置により異なる時刻に取得された複数の慣性データである。
オドメトリデータ群１９２は、車両が備えるオドメトリにより異なる時刻に取得された複数のオドメトリデータである。
【００１３】
図２は、実施の形態１における車両２００の構成図である。
ＧＰＳデータ群１９１、慣性データ群１９３およびオドメトリデータ群１９２を取得する車両２００の構成について、図２に基づいて以下に説明する。
【００１４】
車両２００には天板２０１が設置され、天板２０１には３台のＧＰＳ受信機と慣性計測装置２２０とが取り付けられている。
車両２００は自己位置標定装置１００およびオドメトリ２３０を備える。
【００１５】
天板２０１の３台のＧＰＳ受信機のうち１台を主局のＧＰＳ受信機（主局ＧＰＳ２１０）とし、残りの２台を従局のＧＰＳ受信機（従局ＧＰＳ２１１、従局ＧＰＳ２１２）とする。
各ＧＰＳ受信機は、ＧＰＳ衛星から発信されるＧＰＳの搬送波（測位信号）を受信し、受信結果から観測値を取得し、取得した観測値をＧＰＳデータとして出力する。
ＧＰＳ受信機が出力するＧＰＳデータには、受信した搬送波の位相（搬送波位相）、ＧＰＳ受信機とＧＰＳ衛星との距離（疑似距離）、単独測位で求められたＧＰＳ受信機の座標値、搬送波から得られる航法メッセージなどが含まれる。
【００１６】
慣性計測装置２２０（ＩＭＵ）は、ジャイロセンサと加速度センサとを備える。
ジャイロセンサは、周期的に、車両２００の３軸方向ｘｙｚそれぞれの角速度を計測して出力する。以下、３軸方向ｘｙｚの角速度を「角速度ベクトル」という。
加速度センサは、周期的に、車両２００の３軸方向ｘｙｚの加速度を計測して出力する。以下、３軸方向ｘｙｚの加速度を「加速度ベクトル」という。
慣性計測装置２２０は、角速度ベクトルと加速度ベクトルとを慣性データとして出力する。
【００１７】
オドメトリ２３０（ＯＤＯ）は、周期的に、車両２００の速度を表す車速パルス（速度データの一例）を計測し、計測した車速パルスをオドメトリデータとして出力する。
車速パルスは単位時間当たりのタイヤの回転数を表す。車速パルスにタイヤの円周の長さを掛けることにより車両２００の速度のスカラー量（以下、速度スカラーという）が求まる。
【００１８】
自己位置標定装置１００は、ＧＰＳデータと慣性データとオドメトリデータとに基づいて各時刻における車両２００の位置、姿勢および速度を算出する。
例えば、自己位置標定装置１００は車両２００の位置として三次元の座標値（緯度、経度、高度）を算出する。
また、自己位置標定装置１００は車両２００の姿勢として三次元の姿勢角（ロール角、ピッチ角、ヨー角）を算出する。
また、自己位置標定装置１００は車両２００の速度として速度ベクトル（速度のベクトル量）を算出する。
【００１９】
以下、車両２００の前後方向（縦方向、長さ方向）を「ｘ軸」、車両２００の左右方向（横方向、幅方向）を「ｙ軸」、車両２００の上下方向（高さ方向）を「ｚ軸」とする。
また、ｘ軸回りの角度を「ロール角φ（回転角）」、ｙ軸回りの角度を「ピッチ角θ（仰角）」、ｚ軸回りの角度を「ヨー角ψ（方位角）」とする。
また、ｘｙｚ軸で表される座標系（原点Ｏ）を「車両座標系」とする。
【００２０】
自己位置標定装置１００の航法データ記憶部１９０（図１参照）には、車両２００に設置された３つのＧＰＳ受信機それぞれにより取得された複数のＧＰＳデータがＧＰＳデータ群１９１として予め記憶される。また、航法データ記憶部１９０には、位置が固定であり座標値が既知であるＧＰＳ受信機（電子基準点）（以下、基準局ＧＰＳという）により取得された複数のＧＰＳデータもＧＰＳデータ群１９１として予め記憶される。
航法データ記憶部１９０には、車両２００に設置された慣性計測装置２２０により計測された複数の慣性データが慣性データ群１９３として予め記憶される。
航法データ記憶部１９０には、車両２００に設置されたオドメトリ２３０により計測された複数のオドメトリデータがオドメトリデータ群１９２として予め記憶される。
【００２１】
ＧＰＳデータ群１９１と慣性データ群１９３とオドメトリデータ群１９２とに含まれる各データは、取得時刻（計測時刻、検知時刻）と対応付けられているものとする。
【００２２】
図１に戻り、自己位置標定装置１００の構成の説明を続ける。
【００２３】
カルマンフィルタ１５０（誤差推定部の一例）は、ＧＰＳデータ群１９１と慣性データ群１９３とに基づいて慣性計測装置２２０の計測誤差の推定値として誤差推定初期値（後述するＩＭＵ誤差推定初期値）を算出する。
【００２４】
ＩＭＵ処理部１４０（慣性データ補正部、慣性航法部の一例）は、慣性データ群１９３のうち最初に取得された先頭慣性データをカルマンフィルタ１５０により算出された誤差推定初期値に基づいて補正する。
ＩＭＵ処理部１４０は、補正した先頭慣性データに基づいて先頭慣性データが取得された先頭時刻における車両２００の位置として先頭時刻の標定位置を算出する。
【００２５】
ＧＰＳ処理部１２０（搬送波位相残差算出部の一例）は、ＧＰＳデータに含まれる搬送波位相とＩＭＵ処理部１４０により算出される参考位置とに基づいて、搬送波位相の残差（後述する並進系・姿勢系二重位相差残差）を算出する。
【００２６】
例えば、誤差推定初期値は以下のように出力される。
【００２７】
ＩＭＵ処理部１４０は、カルマンフィルタ１５０により誤差推定初期値が算出される前に、慣性データ群１９３のうち最初に取得された先頭慣性データに基づいて先頭慣性データが取得された先頭時刻における車両２００の位置として先頭時刻の参考位置を算出する。
ＩＭＵ処理部１４０は、慣性データ群１９３のうち先頭慣性データを除く残りの慣性データ毎に、カルマンフィルタ１５０により算出される当該時刻の誤差推定値に基づいて当該慣性データを補正する。ＩＭＵ処理部１４０は、補正した当該慣性データに基づいて当該慣性データが取得された当該時刻における車両２００の位置として当該時刻の参考位置を算出する。
ＧＰＳ処理部１２０は、ＩＭＵ処理部１４０により算出された当該時刻の参考位置と当該時刻に取得されたＧＰＳデータに含まれる搬送波位相とに基づいて、当該時刻における搬送波位相の残差として当該時刻の搬送波位相残差を算出する。
カルマンフィルタ１５０は、ＧＰＳ処理部１２０により算出された当該時刻の搬送波位相残差を用いて当該時刻に対する慣性計測装置２２０の計測誤差の推定値として当該時刻の誤差推定値を算出する。
カルマンフィルタ１５０は、ＧＰＳデータ群１９１のうち最後に取得された最終ＧＰＳデータの取得時刻の誤差推定値に基づいて誤差推定初期値を出力する。
【００２８】
また、先頭時刻の標定位置が算出された後、以下のように動作する。
【００２９】
ＧＰＳ処理部１２０は、ＩＭＵ処理部１４０により算出された先頭時刻の標定位置と先頭時刻に取得された先頭ＧＰＳデータに含まれる搬送波位相とに基づいて、先頭時刻の搬送波位相残差を算出する。
ＧＰＳ処理部１２０は、ＧＰＳデータ群１９１のうち先頭ＧＰＳデータを除く残りのＧＰＳデータ毎に、ＩＭＵ処理部１４０により算出される当該時刻の標定位置と当該時刻に取得された当該ＧＰＳデータに含まれる搬送波位相とに基づいて当該時刻の搬送波位相残差を算出する。
カルマンフィルタ１５０は、ＧＰＳ処理部１２０により算出された当該時刻の搬送波位相残差に基づいて当該時刻の誤差推定値を算出する。
ＩＭＵ処理部１４０は、当該時刻に取得された当該慣性データの次に取得された次慣性データをカルマンフィルタ１５０により算出された当該時刻の誤差推定値に基づいて補正する。ＩＭＵ処理部１４０は、補正した次慣性データに基づいて次慣性データが取得された次時刻の標定位置を算出する。
【００３０】
ＩＭＵ処理部１４０は、補正した慣性データに基づいて当該時刻における車両２００の速度を算出する。
横滑り特性学習部１１０は、オドメトリデータ群１９２のうち当該時刻に取得されたオドメトリデータとＩＭＵ処理部１４０により算出された当該時刻の速度とに基づいて、オドメトリ２３０の計測誤差の推定値として誤差推定初期値（後述するＯＤＯ誤差推定初期値）を算出する。
ＯＤＯ処理部１３０は、当該時刻に取得されたオドメトリデータとＩＭＵ処理部１４０により算出された当該時刻の速度と横滑り特性学習部１１０により算出された誤差推定初期値とに基づいて、当該時刻の速度残差を算出する。
カルマンフィルタ１５０は、ＯＤＯ処理部１３０により算出された当該時刻の速度残差に基づいて、当該時刻の誤差推定値（後述するＩＭＵ誤差推定値）を算出する。
【００３１】
図３は、実施の形態１における自己位置標定装置１００の位置標定方法を示すフローチャートである。実施の形態１における自己位置標定装置１００の位置標定方法について、図３に基づいて以下に説明する。
【００３２】
１回目の航法処理（Ｓ１００）において、自己位置標定装置１００は、ＧＰＳデータ群１９１、慣性データ群１９３およびオドメトリデータ群１９２といった航法データに基づいて、車両２００の位置、姿勢および速度を参考航法結果として算出する。
そして、自己位置標定装置１００は、参考航法結果に基づいてＩＭＵ誤差推定初期値とＯＤＯ誤差推定初期値とを算出する。
ＩＭＵ誤差推定初期値とは、慣性計測装置２２０の計測誤差の推定値（以下、ＩＭＵ誤差推定値という）である。
ＯＤＯ誤差推定初期値とは、オドメトリ２３０の計測誤差の推定値（以下、ＯＤＯ誤差推定値という）である。
【００３３】
２回目の航法処理（Ｓ２００）において、自己位置標定装置１００は、ＧＰＳデータ群１９１、慣性データ群１９３、オドメトリデータ群１９２、ＩＭＵ誤差推定初期値およびＯＤＯ誤差推定初期値に基づいて、車両２００の位置、姿勢および速度を標定航法結果として算出する。
自己位置標定装置１００は、標定航法結果を車両２００の位置、姿勢および速度として出力する。
【００３４】
図４は、実施の形態１の位置標定方法において１回目の航法処理（Ｓ１００）で算出される誤差推定値（および分散）の時間変化を示すグラフである。
上のグラフは誤差推定値の時間変化を示し、下のグラフは誤差推定値の分散の時間変化を示している。
【００３５】
慣性計測装置２２０（ジャイロセンサ、加速度センサ）により取得される慣性データには、バイアスやスケールファクタ誤差といった計測誤差が含まれる。慣性計測装置２２０のスケールファクタ誤差は電源を投入するたびに変化することが知られている。
また、オドメトリ２３０により取得されるオドメトリデータには、スケールファクタ誤差、オフセット誤差、オフセット誤差に関するパラメータ（例えば、後述するコーナリングパワー）といった計測誤差が含まれる。オドメトリ２３０のスケールファクタ誤差、オフセット誤差、オフセット誤差に関するパラメータは車両２００の重心位置の変化、タイヤの空気圧の変化、路面の状況などに応じて変化することが知られている。
これらの計測誤差は不明であるため、慣性データに基づいて算出する航法結果には計測誤差分の誤差が含まれる。
【００３６】
但し、これらの計測誤差は、慣性データから得られる特定値とＧＰＳデータから得られる特定値とを比較することにより推定することができる。
計測誤差の推定値（誤差推定値）は、推定を繰り返すことにより一定値に近づき（図４上）、十分に収斂する（図４下）。十分に収斂した誤差推定値の精度は十分に高い。
【００３７】
自己位置標定装置１００は、１回目の航法処理（Ｓ１００）によって十分に収斂した誤差推定値を誤差推定初期値として２回目の航法処理（Ｓ２００）で用いる。
【００３８】
図５は、実施の形態１の位置標定方法において２回目の航法処理（Ｓ２００）で算出される誤差推定値（および分散）の時間変化を示すグラフである。
【００３９】
２回目の航法処理（Ｓ２００）において誤差推定初期値を用いることにより、誤差推定値は始めからほぼ一定値を示し（図５上）、すぐに収斂する（図５下）。
【００４０】
自己位置標定装置１００は、２回目の航法処理（Ｓ２００）において誤差推定初期値を用いることにより誤差推定値をすぐに収斂させ、高い精度で航法結果を得ることができる。
【００４１】
図６は、実施の形態１における位置標定方法の１回目の航法処理（Ｓ１００）と２回目の航法処理（Ｓ２００）とを示すフローチャートである。
１回目の航法処理（Ｓ１００）と２回目の航法処理（Ｓ２００）とについて、図６に基づいて以下に説明する。
【００４２】
１回目の航法処理（Ｓ１００）をＳ１１０〜Ｓ１２１およびＳ３００〜Ｓ５００で示し、２回目の航法処理（Ｓ２００）をＳ２０１〜Ｓ２２１およびＳ３００〜Ｓ５００で示す。
【００４３】
自己位置標定装置１００は、１回目の航法処理（Ｓ１００）と２回目の航法処理（Ｓ２００）とで同様な航法処理を行う。
但し、ＩＭＵ・ＯＤＯ誤差推定初期値は１回目の航法処理（Ｓ１００）により特定されるため、ＩＭＵ・ＯＤＯ誤差推定初期値を用いる処理（Ｓ２０１、Ｓ２１１）は２回目の航法処理（Ｓ２００）でのみ行われる。
【００４４】
まず、１回目の航法処理（Ｓ１００）について説明する。
【００４５】
Ｓ１１０において、ＩＭＵ処理部１４０は慣性データ群１９３のうち最初に取得された慣性データ（以下、先頭慣性データという）を取得する。
ＩＭＵ処理部１４０は、先頭慣性データを用いて慣性航法（ＩＮＳ：Ｉｎｅｒｔｉａｌ Ｎａｖｉｇａｔｉｏｎ Ｓｙｓｔｅｍ）を行い、先頭慣性データが取得された時刻（以下、先頭時刻という）における車両２００の位置、姿勢および速度を算出する。
Ｓ１１０の後、Ｓ１２０に進む。
【００４６】
Ｓ１２０において、ＩＭＵ処理部１４０は、次の慣性データが有るか否か（処理していない慣性データが残っているか否か）を判定する。
次の慣性データが有る場合（ＹＥＳ）、Ｓ１２１に進む。
次の慣性データが無い場合（ＮＯ）、１回目の航法処理（Ｓ１００）は終了する。
【００４７】
Ｓ１２１において、ＧＰＳ処理部１２０は、ＩＭＵ処理部１４０により算出された車両２００の位置、姿勢および速度に対応する当該時刻がＧＰＳ可視時であるか否かを判定する。
【００４８】
ＧＰＳ可視時とは、車両２００に設置された３つのＧＰＳ受信機と所定地に設けられたＧＰＳ受信機（基準局ＧＰＳ）とが所定数以上のＧＰＳ衛星から搬送波を受信できた時刻を意味する。
【００４９】
ＧＰＳ処理部１２０は、ＧＰＳデータ群１９１から各ＧＰＳ受信機の当該時刻のＧＰＳデータを取得し、取得した各ＧＰＳデータに所定数以上の搬送波位相が含まれている場合にＧＰＳ可視時であると判定する。
ＧＰＳ処理部１２０は、取得した各ＧＰＳデータに所定数以上の搬送波位相が含まれていない場合、ＧＰＳ不可視時であると判定する。
【００５０】
ＧＰＳ可視時である場合（ＹＥＳ）、ＧＰＳ／ＩＮＳ複合航法処理（Ｓ３００）に進む。
ＧＰＳ不可視時である場合（ＮＯ）、ＯＤＯ／ＩＮＳ複合航法処理（Ｓ４００）に進む。
【００５１】
ＧＰＳ／ＩＮＳ複合航法処理（Ｓ３００）において、ＩＭＵ処理部１４０は、ＧＰＳデータと慣性データとに基づいて車両２００の位置、姿勢および速度を算出する。
カルマンフィルタ１５０は、車両２００の位置、姿勢および速度に基づいてＩＭＵ誤差推定値を算出する。
ＧＰＳ／ＩＮＳ複合航法処理（Ｓ３００）の詳細については後述する。
ＧＰＳ／ＩＮＳ複合航法処理（Ｓ３００）の後、横滑り特性学習処理（Ｓ５００）に進む。
【００５２】
横滑り特性学習処理（Ｓ５００）において、横滑り特性学習部１１０は、ＧＰＳ／ＩＮＳ複合航法処理（Ｓ３００）で算出された車両２００の速度と当該時刻のオドメトリデータに含まれる車速パルスとに基づいて、ＯＤＯ誤差推定値を算出する。
横滑り特性学習処理（Ｓ５００）の後、Ｓ１２０に戻る。
【００５３】
ＯＤＯ／ＩＮＳ複合航法処理（Ｓ４００）において、ＩＭＵ処理部１４０は、オドメトリデータと慣性データとに基づいて車両２００の位置、姿勢および速度を算出する。
カルマンフィルタ１５０は、車両２００の位置、姿勢および速度に基づいてＩＭＵ誤差推定値を算出する。
ＯＤＯ／ＩＮＳ複合航法処理（Ｓ４００）の詳細については後述する。
ＯＤＯ／ＩＮＳ複合航法処理（Ｓ４００）の後、Ｓ１２０に戻る。
【００５４】
自己位置標定装置１００は、１回目の航法処理（Ｓ１００）のＧＰＳ／ＩＮＳ複合航法処理（Ｓ３００）で最後に算出されたＩＭＵ誤差推定値をＩＭＵ誤差推定初期値として用いる。
自己位置標定装置１００は、１回目の航法処理（Ｓ１００）の横滑り特性学習処理（Ｓ５００）で最後に算出されたＯＤＯ誤差推定値をＯＤＯ誤差推定初期値として用いる。
【００５５】
自己位置標定装置１００は、Ｓ１２０においてＩＭＵ誤差推定値およびＯＤＯ誤差推定値が収斂していれば、次の慣性データが有る場合でも１回目の航法処理（Ｓ１００）を終了して構わない。
例えば、ＩＭＵ処理部１４０は、それまでに算出されたＩＭＵ誤差推定値（またはＯＤＯ誤差推定値）を新しい順に所定数取得し、取得したうちの最大値と最小値との差が所定値未満であればＩＭＵ誤差推定値が収斂したと判定し、１回目の航法処理（Ｓ１００）を終了する。
【００５６】
自己位置標定装置１００は、１回目の航法処理で最後に算出されたＩＭＵ・ＯＤＯ誤差推定値をＩＭＵ・ＯＤＯ誤差推定初期値にしなくても構わない。
例えば、自己位置標定装置１００は、それまでに算出されたＩＭＵ誤差推定値（またはＯＤＯ誤差推定値）を最後のＩＭＵ誤差推定値を含めて新しい順に所定数取得し、取得した各ＩＭＵ誤差推定値の平均値をＩＭＵ誤差推定初期値としても構わない。
【００５７】
次に、２回目の航法処理（Ｓ２００）について説明する。
【００５８】
Ｓ２０１において、ＩＭＵ処理部１４０は、１回目の航法処理（Ｓ１００）で算出されたＩＭＵ誤差推定初期値を誤差補正量として先頭慣性データの値に加算する。先頭慣性データの値に誤差補正量を加算することにより、先頭慣性データを補正することができる。
Ｓ２０１の後、Ｓ２１０に進む。
【００５９】
Ｓ２１０において、ＩＭＵ処理部１４０は、補正した先頭慣性データに基づいて先頭時刻における車両２００の位置、姿勢および速度を算出する（Ｓ１１０と同様）。
Ｓ２１０の後、Ｓ２１１に進む。
【００６０】
Ｓ２１１において、横滑り特性学習部１１０は、１回目の航法処理（Ｓ２００）で算出されたＯＤＯ誤差推定初期値を初期設定する。
Ｓ２１１の後、Ｓ２２０に進む。
【００６１】
Ｓ２２０において、次の慣性データが有れば（ＹＥＳ）、Ｓ２２１に進む。
また、次の慣性データが無ければ（ＮＯ）、２回目の航法処理（Ｓ２００）は終了する。
【００６２】
Ｓ２２１において、ＧＰＳ可視時であれば（ＹＥＳ）、ＧＰＳ／ＩＮＳ複合航法処理（Ｓ３００）に進む。
また、ＧＰＳ不可視時であれば（ＮＯ）、ＯＤＯ／ＩＮＳ複合航法処理（Ｓ４００）に進む。
【００６３】
ＧＰＳ／ＩＮＳ複合航法処理（Ｓ３００）において、ＩＭＵ処理部１４０は、ＧＰＳデータと慣性データとに基づいて車両２００の位置、姿勢および速度を算出する。
カルマンフィルタ１５０は、車両２００の位置、姿勢および速度に基づいてＩＭＵ誤差推定値を算出する。
ＧＰＳ／ＩＮＳ複合航法処理（Ｓ３００）の後、横滑り特性学習処理（Ｓ５００）に進む。
【００６４】
横滑り特性学習処理（Ｓ５００）において、横滑り特性学習部１１０は、ＧＰＳ／ＩＮＳ複合航法処理（Ｓ３００）で算出された車両２００の速度と当該時刻のオドメトリデータに含まれる車速パルスとに基づいてＯＤＯ誤差推定値を算出する。
横滑り特性学習処理（Ｓ５００）の後、Ｓ２２０に戻る。
【００６５】
ＯＤＯ／ＩＮＳ複合航法処理（Ｓ４００）において、ＩＭＵ処理部１４０は、オドメトリデータと慣性データとに基づいて車両２００の位置、姿勢および速度を算出する。
カルマンフィルタ１５０は、車両２００の位置、姿勢および速度に基づいてＩＭＵ誤差推定値を算出する。
ＯＤＯ／ＩＮＳ複合航法処理（Ｓ４００）の後、Ｓ２２０に戻る。
【００６６】
自己位置標定装置１００は、２回目の航法処理（Ｓ２００）のＧＰＳ／ＩＮＳ複合航法処理（Ｓ３００）とＯＤＯ／ＩＮＳ複合航法処理（Ｓ４００）とで算出された各時刻における車両２００の位置、姿勢および速度を航法結果（標定値）として出力する。
【００６７】
図７は、実施の形態１におけるＧＰＳ／ＩＮＳ複合航法処理（Ｓ３００）を示すフローチャートである。
実施の形態１におけるＧＰＳ／ＩＮＳ複合航法処理（Ｓ３００）について、図７に基づいて以下に説明する。
【００６８】
二重位相差残差算出処理（Ｓ３１０）において、ＧＰＳ処理部１２０は、一つ前に算出された車両２００の位置と姿勢と当該時刻のＧＰＳデータに含まれる搬送波位相とに基づいて、当該時刻の二重位相差残差を算出する。
二重位相差残差算出処理（Ｓ３１０）の詳細については後述する。
二重位相差残差算出処理（Ｓ３１０）の後、Ｓ３２０に進む。
【００６９】
Ｓ３２０において、カルマンフィルタ１５０は、当該時刻の二重位相差残差を入力として当該時刻のＩＭＵ誤差推定値を算出する。
【００７０】
カルマンフィルタ１５０の概要について説明する。
カルマンフィルタ１５０は、モデル化された状態方程式と観測方程式とを用いて誤差補正量（ＩＭＵ誤差推定値や後述する航法誤差推定値）を算出する。
カルマンフィルタ１５０に入力される残差（二重位相差残差や速度残差）は観測方程式の変数として用いられる。
【００７１】
カルマンフィルタ１５０は、観測ノイズ行列Ｒ、誤差共分散行列Ｐおよび観測行列Ｈを用いてカルマンゲインベクトルＫを算出する。
観測ノイズ行列Ｒと観測行列Ｈは残差の種類ごとに用意される。
誤差共分散行列Ｐは、状態量（位置、姿勢、速度、ＩＭＵ誤差推定値、航法誤差推定値など）の共分散値を表す。
観測行列Ｈは、状態量と観測量（二重位相差残差、速度残差など）との関係を表す。
【００７２】
カルマンゲインベクトルＫの算出式（１）を以下に示す。式（１）において「ＨＴ」は観測行列Ｈの転置行列を表す。
【００７３】
【数１】
【００７４】
残差ｄｚにカルマンゲインベクトルＫを掛けて求まる複数の値Ｋｄｚが誤差補正量である。
【００７５】
Ｓ３２０の後、Ｓ３３０に進む。
【００７６】
Ｓ３３０において、ＩＭＵ処理部１４０は、当該時刻のＩＭＵ誤差推定値を誤差補正量として次の慣性データの値に加算する。慣性データの値に誤差補正量を加算することにより、慣性データを補正することができる。
Ｓ３３０の後、Ｓ３４０に進む。
【００７７】
Ｓ３４０において、ＩＭＵ処理部１４０は、補正した慣性データを用いて慣性航法を行い、慣性データが取得された時刻（次の時刻）における車両２００の位置、姿勢および速度を算出する。
Ｓ３４０の後、ＧＰＳ／ＩＮＳ複合航法処理（Ｓ３００）は終了する。
【００７８】
図８は、実施の形態１におけるＯＤＯ／ＩＮＳ複合航法処理（Ｓ４００）を示すフローチャートである。
実施の形態１におけるＯＤＯ／ＩＮＳ複合航法処理（Ｓ４００）について、図８に基づいて以下に説明する。
【００７９】
速度残差算出処理（Ｓ４１０）において、ＯＤＯ処理部１３０は、一つ前に算出された車両２００の速度と当該時刻のＯＤＯ誤差推定値とに基づいて、当該時刻の速度残差を算出する。
速度残差算出処理（Ｓ４１０）の詳細については後述する。
速度残差算出処理（Ｓ４１０）の後、Ｓ４２０に進む。
【００８０】
Ｓ４２０において、カルマンフィルタ１５０は、当該時刻の速度残差を入力として当該時刻のＩＭＵ誤差推定値を算出する。
Ｓ４２０の後、Ｓ４３０に進む。
【００８１】
Ｓ４３０において、ＩＭＵ処理部１４０は、当該時刻のＩＭＵ誤差推定値を誤差補正量として次の慣性データを補正する。
Ｓ４３０の後、Ｓ４４０に進む。
【００８２】
Ｓ４４０において、ＩＭＵ処理部１４０は、補正した慣性データを用いて慣性航法を行い、慣性データが取得された時刻（次の時刻）における車両２００の位置、姿勢および速度を算出する。
Ｓ４４０の後、ＯＤＯ／ＩＮＳ複合航法処理（Ｓ４００）は終了する。
【００８３】
次に、ＧＰＳ／ＩＮＳ複合航法処理（Ｓ３００）で行われる二重位相差残差算出処理（Ｓ３１０）とＯＤＯ／ＩＮＳ複合航法処理（Ｓ４００）で行われる速度残差算出処理（Ｓ４１０）との具体例について説明する。
また、横滑り特性学習処理（Ｓ５００）の具体例について説明する。
【００８４】
図９は、実施の形態１における自己位置標定装置１００のＧＰＳ処理部１２０の機能構成図である。
実施の形態１におけるＧＰＳ処理部１２０の機能構成の具体例について、図９に基づいて以下に説明する。
【００８５】
ＧＰＳ処理部１２０は、並進系二重位相差残差と姿勢系二重位相差残差とを算出する。
ＧＰＳ処理部１２０は、並進系二重位相差残差を算出する構成として、並進系二重位相差計算部１２１、並進系二重位相差予測部１２２および並進系二重位相差残差計算部１２３を備える。
ＧＰＳ処理部１２０は、姿勢系二重位相差残差を算出する構成として、姿勢系二重位相差計算部１２４、姿勢系二重位相差予測部１２５および姿勢系二重位相差残差計算部１２６を備える。
さらに、ＧＰＳ処理部１２０はデザイン行列計算部１２７を備える。
ＧＰＳ処理部１２０の各構成の詳細については後述する。
【００８６】
カルマンフィルタ１５０は、ＧＰＳ処理部１２０により算出される並進系二重位相差残差と姿勢系二重位相差残差とに基づいて、ＩＭＵ誤差推定値（および後述する航法誤差推定値）を算出する。
【００８７】
図１０は、実施の形態１における二重位相差残差算出処理（Ｓ３１０）を示すフローチャートである。
実施の形態１における二重位相差残差算出処理（Ｓ３１０）の具体例について、図１０に基づいて以下に説明する。
【００８８】
Ｓ３１１において、並進系二重位相差計算部１２１は、ＧＰＳデータ群１９１から基準局ＧＰＳと主局ＧＰＳ２１０との対象時刻のＧＰＳデータを取得し、取得したＧＰＳデータからＧＰＳ衛星Ａの搬送波位相ＡとＧＰＳ衛星Ｂの搬送波位相Ｂとを取得する。
並進系二重位相差計算部１２１は、基準局ＧＰＳの搬送波位相Ａと主局ＧＰＳ２１０の搬送波位相Ａとの差を搬送波位相差Ａとして算出し、基準局ＧＰＳの搬送波位相Ｂと主局ＧＰＳ２１０の搬送波位相Ｂとの差を搬送波位相差Ｂとして算出する。
並進系二重位相差計算部１２１は、搬送波位相差Ａと搬送波位相差Ｂとの差を並進系二重位相差として算出する。
【００８９】
並進系とは、基準局ＧＰＳに対する主局ＧＰＳ２１０の相対位置により車両２００の位置が求まることを意味する。
【００９０】
Ｓ３１１の後、Ｓ３１２に進む。
【００９１】
Ｓ３１２において、並進系二重位相差予測部１２２は、基準局ＧＰＳ２９９の位置とＩＭＵ処理部１４０により算出された対象時刻における車両２００の位置とデザイン行列計算部１２７により算出される並進系デザイン行列とに基づいて、並進系二重位相差を算出する。
デザイン行列計算部１２７は、各ＧＰＳデータに含まれるエフェメリス（航法メッセージに含まれる軌道情報）に基づいて、２つのＧＰＳ受信機の位置と並進系二重位相差との関係を表す行列を並進系デザイン行列として算出する。
Ｓ３１２の後、Ｓ３１３に進む。
【００９２】
Ｓ３１３において、並進系二重位相差残差計算部１２３は、Ｓ３１１で算出された並進系二重位相差とＳ３１２で算出された並進系二重位相差との差を対象時刻の並進系二重位相差残差として算出する。
Ｓ３１３の後、Ｓ３１４に進む。
【００９３】
Ｓ３１４において、姿勢系二重位相差計算部１２４は、ＧＰＳデータ群１９１から主局ＧＰＳ２１０、従局ＧＰＳ２１１および従局ＧＰＳ２１２の対象時刻のＧＰＳデータを取得する。姿勢系二重位相差計算部１２４は、取得したＧＰＳデータからＧＰＳ衛星Ａの搬送波位相ＡとＧＰＳ衛星Ｂの搬送波位相Ｂとを入力する。
姿勢系二重位相差計算部１２４は、主局ＧＰＳ２１０の搬送波位相Ａと従局ＧＰＳ２１１の搬送波位相Ａとの差を第１の搬送波位相差Ａとして算出し、主局ＧＰＳ２１０の搬送波位相Ｂと従局ＧＰＳ２１１の搬送波位相Ｂとの差を第１の搬送波位相差Ｂとして算出する。同様に、姿勢系二重位相差計算部１２４は、主局ＧＰＳ２１０の搬送波位相Ａ・Ｂと従局ＧＰＳ２１２の搬送波位相Ａ・Ｂとに基づいて第２の搬送波位相差Ａ・Ｂを算出する。
姿勢系二重位相差計算部１２４は、第１の搬送波位相差Ａと第１の搬送波位相差Ｂとの差を第１の姿勢系二重位相差として算出し、第２の搬送波位相差Ａと第２の搬送波位相差Ｂとの差を第２の姿勢系二重位相差として算出する。
【００９４】
姿勢系とは、主局ＧＰＳ２１０に対する従局ＧＰＳ２１１および従局ＧＰＳ２１２の相対位置（基線ベクトル）により車両２００の姿勢が求まることを意味する。
【００９５】
Ｓ３１４の後、Ｓ３１５に進む。
【００９６】
Ｓ３１５において、姿勢系二重位相差予測部１２５は、主局ＧＰＳ２１０、従局ＧＰＳ２１１および従局ＧＰＳ２１２の相対位置（所定の計測値）とＩＭＵ処理部１４０により算出された対象時刻における車両２００の姿勢とデザイン行列計算部１２７により算出される姿勢系デザイン行列とに基づいて、第１・第２の姿勢系二重位相差を算出する。
デザイン行列計算部１２７は、各ＧＰＳデータに含まれるエフェメリスに基づいて、２つのＧＰＳ受信機の位置と姿勢系二重位相差との関係を表す行列を姿勢系デザイン行列として算出する。
Ｓ３１５の後、Ｓ３１６に進む。
【００９７】
Ｓ３１６において、姿勢系二重位相差残差計算部１２６は、Ｓ３１４で算出された第１の姿勢系二重位相差とＳ３１５で算出された第１の姿勢系二重位相差との差を対象時刻の第１の姿勢系二重位相差残差として算出する。
姿勢系二重位相差残差計算部１２６は、Ｓ３１４で算出された第２の姿勢系二重位相差とＳ３１５で算出された第２の姿勢系二重位相差との差を対象時刻の第２の姿勢系二重位相差残差として算出する。
Ｓ３１６の後、二重位相差残差算出処理（Ｓ３１０）は終了する。
【００９８】
カルマンフィルタ１５０は、二重位相差残差算出処理（Ｓ３１０）で算出された並進系二重位相差残差と第１・第２の姿勢系二重位相差残差とを入力として、対象時刻のＩＭＵ誤差推定値（および後述する航法誤差推定値）を算出する。
【００９９】
図１１は、実施の形態１における自己位置標定装置１００のＯＤＯ処理部１３０とＩＭＵ処理部１４０との機能構成図である。
ＯＤＯ処理部１３０とＩＭＵ処理部１４０との機能構成の具体例について、図１１に基づいて以下に説明する。
【０１００】
ＯＤＯ処理部１３０は、速度／加速度計算部１３１と速度／加速度予測部１３２と速度／加速度残差計算部１３３とを備える。
ＯＤＯ処理部１３０の各構成の詳細については後述する。
【０１０１】
カルマンフィルタ１５０は、慣性計測装置２２０を構成するジャイロセンサと加速度センサとのそれぞれの計測誤差の推定値をＩＭＵ誤差推定値として出力する。
さらに、カルマンフィルタ１５０は、ＩＭＵ処理部１４０により算出された車両２００の位置、姿勢および速度のそれぞれの誤差推定値を航法誤差推定値として出力する。
【０１０２】
ＩＭＵ処理部１４０は、補正計算部１４１とストラップダウン演算部１４２とを備える。
【０１０３】
補正計算部１４１は、対象時刻の慣性データに含まれる角速度ベクトルと加速度ベクトルとにカルマンフィルタ１５０により算出されたＩＭＵ誤差推定値を誤差補正量として加算する。角速度ベクトルと加速度ベクトルとにそれぞれの誤差補正量を加算することにより、角速度ベクトルと加速度ベクトルとを補正することができる。
【０１０４】
ストラップダウン演算部１４２は、補正計算部１４１により補正された角速度ベクトルおよび加速度ベクトルを用いてストラップダウン演算（慣性航法の一例）を行う。
ストラップダウン演算では、角速度ベクトルを積分することにより姿勢（姿勢角ベクトル）が求まり、加速度ベクトルを積分することにより速度（速度ベクトル）が求まる。
さらに、速度（速度ベクトル）を積分することにより距離（距離ベクトル）が求まり、一つ前に算出した位置に距離ベクトルを加算することにより対象時刻の位置（三次元座標値）が求まる。
【０１０５】
ストラップダウン演算部１４２は、ストラップダウン演算により得られた対象時刻における車両２００の位置、姿勢および速度にカルマンフィルタ１５０により算出された航法誤差推定値を誤差補正量として加算する。位置、姿勢および速度にそれぞれの誤差補正量を加算することにより、位置、姿勢および速度を補正することができる。
【０１０６】
横滑り特性学習部１１０は、ストラップダウン演算部１４２により算出された速度ベクトルに基づいて、車両２００のコーナリングパワー、オドメトリ２３０のオフセット誤差およびスケールファクタ誤差を算出する。
コーナリングパワー、オフセット誤差、スケールファクタ誤差はＯＤＯ誤差推定値の一例である。
【０１０７】
図１２は、実施の形態１における車両座標系ｘｙｚとオドメトリ２３０のオフセット誤差ｏｓとの関係図である。
図１３は、実施の形態１における横滑り角βを示す図である。
オドメトリ２３０のオフセット誤差ｏｓについて、図１２と図１３とに基づいて以下に説明する。
【０１０８】
図１２において、車両２００は、搭載物（例えば、乗車している人）のバランスや道路の起伏などに応じて傾き、また、図１３に示すような横滑りをする。
このため、車両２００のタイヤの向きｘと車両２００の進行方向Ｖとは異なり、オドメトリ２３０の車速パルスから算出される正面方向ｘの速度（速度スカラー）は進行方向Ｖの速度に対して誤差を有する。この誤差量をオドメトリ２３０のオフセット誤差という。
ｏｓ［０］はｚ軸方向のオフセット誤差を示し、ｏｓ［１］はｙ軸方向のオフセット誤差を示している。
【０１０９】
図１３において、横向きの力を受ける車両２００はタイヤの傾きよりも浅い角度の方向へ進む。つまり、車両２００はコーナリング時などに横滑りし、横滑り角βを生じる。横滑り角βは、タイヤの向きと車両２００の進行方向とが成すヨー角を示している。
【０１１０】
図１４は、実施の形態１におけるコーナリングパワーＳｓｌｉｐとオフセット誤差ｏｓ［１］とを表すグラフである。
コーナリングパワーＳｓｌｉｐとオフセット誤差ｏｓ［１］とについて、図１４に基づいて以下に説明する。
【０１１１】
横滑り特性直線Ｌは、単位コーナリングフォースａｙと横滑り角βとの関係を表している。
単位コーナリングフォースａｙとは加速度ベクトルの左右方向成分値である。
コーナリングパワーＳｓｌｉｐとは横滑り特性直線Ｌの傾きである。
オスセット誤差ｏｓ［１］は単位コーナリングフォースａｙが「０」のときの横滑り角βに相当する。もう一つのオフセット誤差ｏｓ［０］は加速度ベクトルの上下方向成分値に対する値である。
【０１１２】
図１５は、実施の形態１におけるオドメトリ２３０のスケールファクタ誤差を表すグラフである。
オドメトリ２３０のスケールファクタ誤差について、図１５に基づいて以下に説明する。
【０１１３】
実線は、ＧＰＳデータに基づいて算出されるＧＰＳ距離とオドメトリデータに基づいて算出されるＯＤＯ距離との関係を表している。
オドメトリ２３０のスケールファクタ誤差とはＧＰＳ距離に対するＯＤＯ距離の割合（パーセント）である。
例えば、ＧＰＳ距離が「１００センチメートル」であり、ＯＤＯ距離が「９５センチメートル」であれば、オドメトリ２３０のスケールファクタ誤差は「−５パーセント」である。
点線は、オドメトリ２３０のスケールファクタ誤差が「０パーセント」の場合を示している。つまり、ＧＰＳ距離とＯＤＯ距離とは等しい。
【０１１４】
図１６は、実施の形態１における横滑り特性学習処理（Ｓ５００）を示すフローチャートである。
実施の形態１における横滑り特性学習処理（Ｓ５００）の具体例について、図１６に基づいて以下に説明する。
【０１１５】
Ｓ５１０において、速度／加速度計算部１３１は、対象時刻のオドメトリデータに含まれる車速パルスに車両２００のタイヤの円周長を掛けて対象時刻における車両２００の速度のスカラー量（以下、速度スカラーという）を算出する。
速度／加速度計算部１３１は、速度スカラーを積分して距離（以下、ＯＤＯ距離という）を算出する。
Ｓ５１０の後、Ｓ５２０に進む。
【０１１６】
Ｓ５２０において、横滑り特性学習部１１０は、Ｓ５１０で算出された速度スカラーを速度ベクトルの前後方向成分として用い、ストラップダウン演算部１４２により算出された対象時刻の速度ベクトルのうち左右方向成分値を用いる。
横滑り特性学習部１１０は、速度ベクトルの左右方向成分値を速度スカラーで割った値を横滑り角として算出する。
すなわち、「横滑り角β＝速度ベクトルの左右方向成分値Ｖｙ／速度スカラーＶｏｄｏ」である。タンジェント（ｔａｎ）の値が「Ｖｙ／Ｖｏｄｏ」になる角度を横滑り角βとしてもよい。
Ｓ５２０の後、処理はＳ５３０に進む。
【０１１７】
Ｓ５３０において、横滑り特性学習部１１０は、Ｓ５２０で算出した横滑り角と補正計算部１４１により補正された対象時刻の加速度ベクトルの左右方向成分値とを対応づけて航法データ記憶部１９０に追加して記憶する。
航法データ記憶部１９０には、複数の横滑り角と加速度ベクトルの左右方向成分値（単位コーナリングフォース）とが蓄積される。
Ｓ５３０の後、Ｓ５４０に進む。
【０１１８】
Ｓ５４０において、横滑り特性学習部１１０は、航法データ記憶部１９０に蓄積された複数の横滑り角と単位コーナリングフォースとに基づいて横滑り特性直線を算出する（図１４参照）。
Ｓ５４０の後、Ｓ５５０に進む。
【０１１９】
Ｓ５５０において、横滑り特性学習部１１０は、横滑り特性直線に基づいて車両２００のコーナリングパワーとオドメトリ２３０のオフセット誤差とを算出する（図１４参照）。
Ｓ５５０の後、Ｓ５６０に進む。
【０１２０】
Ｓ５６０において、横滑り特性学習部１１０は、ストラップダウン演算部１４２により速度ベクトルに基づいて算出された距離（ＧＰＳ距離）とＳ５１０で算出されたＯＤＯ距離とに基づいて、オドメトリ２３０のスケールファクタ誤差を算出する（図１５参照）。
Ｓ５６０の後、横滑り特性学習処理（Ｓ５００）は終了する。
【０１２１】
図１７は、実施の形態１における速度残差算出処理（Ｓ４１０）を示すフローチャートである。
実施の形態１における速度残差算出処理（Ｓ４１０）の具体例について、図１７に基づいて以下に説明する。
【０１２２】
Ｓ４１１において、速度／加速度計算部１３１は、対象時刻のオドメトリデータに含まれる車速パルスに基づいて速度スカラーを算出する。
速度／加速度計算部１３１は、速度スカラーを微分して加速度スカラーを算出する。
Ｓ４１１の後、Ｓ４１２に進む。
【０１２３】
Ｓ４１２において、速度／加速度予測部１３２は、Ｓ４１１で算出された速度スカラーと横滑り特性学習部１１０により算出されたＯＤＯ誤差推定値と補正計算部１４１により補正された加速度ベクトルとに基づいて、対象時刻における車両２００の速度ベクトルを算出する。
速度／加速度予測部１３２は、Ｓ４１１で算出された加速度スカラーに基づいて対象時刻における車両２００の加速度ベクトルを算出する。
【０１２４】
車両２００の速度ベクトルＶｂの算出式（２）を以下に示す。
式（２）において、「Ｖｏｄｏ」は速度スカラー、「ｏｓ［０］」はピッチ角のオフセット誤差、「ｏｓ［１］」はヨー角のオフセット誤差、「Ｓｓｌｉｐ」はコーナリングパワー、「ａｙ」は加速度ベクトルの左右方向成分値（単位コーナリングフォース）を示す。
【０１２５】
【数２】
【０１２６】
速度／加速度予測部１３２は、式（２）の速度スカラーＶｏｄｏを加速度スカラーに置き換えて加速度ベクトルを算出する。
【０１２７】
Ｓ４１２の後、Ｓ４１３に進む。
【０１２８】
Ｓ４１３において、速度／加速度残差計算部１３３は、Ｓ４１２で算出された速度ベクトルとストラップダウン演算部１４２により算出された対象時刻の速度ベクトルとの差を対象時刻の速度ベクトル残差として算出する。
速度／加速度残差計算部１３３は、Ｓ４１２で算出された加速度ベクトルと補正計算部１４１により補正された対象時刻の加速度ベクトルとの差を対象時刻の加速度ベクトル残差として算出する。Ｓ４１３の後、速度残差算出処理（Ｓ４１０）は終了する。
【０１２９】
カルマンフィルタ１５０は、速度残差算出処理（Ｓ４１０）で算出された速度ベクトル残差と加速度ベクトル残差とを入力として、対象時刻のＩＭＵ誤差推定値と航法誤差推定値とを算出する。
【０１３０】
図１８は、実施の形態１における自己位置標定装置１００のハードウェア資源の一例を示す図である。図１８において、自己位置標定装置１００は、ＣＰＵ９１１（Ｃｅｎｔｒａｌ・Ｐｒｏｃｅｓｓｉｎｇ・Ｕｎｉｔ）（マイクロプロセッサ、マイクロコンピュータともいう）を備えている。ＣＰＵ９１１は、バス９１２を介してＲＯＭ９１３、ＲＡＭ９１４、通信ボード９１５、磁気ディスク装置９２０と接続され、これらのハードウェアデバイスを制御する。
通信ボード９１５は、有線または無線で、ＬＡＮ（Ｌｏｃａｌ Ａｒｅａ Ｎｅｔｗｏｒｋ）、インターネット、電話回線などの通信網に接続している。
磁気ディスク装置９２０には、ＯＳ９２１（オペレーティングシステム）、プログラム群９２３、ファイル群９２４が記憶されている。
プログラム群９２３には、実施の形態において「〜部」として説明する機能を実行するプログラムが含まれる。プログラムは、ＣＰＵ９１１により読み出され実行される。すなわち、プログラムは、「〜部」としてコンピュータを機能させるものであり、また「〜部」の手順や方法をコンピュータに実行させるものである。
ファイル群９２４には、実施の形態において説明する「〜部」で使用される各種データ（入力、出力、判定結果、計算結果、処理結果など）が含まれる。
実施の形態において構成図およびフローチャートに含まれている矢印は主としてデータや信号の入出力を示す。
実施の形態において「〜部」として説明するものは「〜回路」、「〜装置」、「〜機器」であってもよく、また「〜ステップ」、「〜手順」、「〜処理」であってもよい。すなわち、「〜部」として説明するものは、ファームウェア、ソフトウェア、ハードウェアまたはこれらの組み合わせのいずれで実装されても構わない。
【０１３１】
実施の形態１において、例えば、以下のような自己位置標定装置１００および自己位置標定アルゴリズムについて説明した。
ＧＰＳや慣性計測装置（ＩＭＵ）やオドメトリ（ＯＤＯ）のデータを解析して、これらのセンサが搭載されたプラットフォーム（例えば、車両２００）の位置を標定する。
このとき、同一データによる処理を２回行い、２回目に１回目の処理結果（ＩＭＵ・ＯＤＯ誤差推定初期値）を使用する。
これにより、１回のみの処理を行う場合に比べて自己位置標定精度を向上することができる。
【０１３２】
実施の形態２．
実施の形態１は、航法データの収集が全て終了したのちに自己位置を標定する後処理に適した形態である。
実施の形態２では、航法データの収集を行いながら自己位置を標定するリアルタイム処理に適した形態について説明する。
【０１３３】
図１９は、実施の形態２における自己位置標定装置１００の機能構成図である。
実施の形態２における自己位置標定装置１００について、図１９に基づいて以下に説明する。
【０１３４】
航法データ記憶部１９０には、事前に行われた予備計測により収集されたＧＰＳ予備データ群１９４（第１のＧＰＳデータ群）と慣性予備データ群１９６（第１の慣性データ群）とオドメトリ予備データ群１９５とが予め記憶されているものとする。
【０１３５】
また、航法データ記憶部１９０には、現在行われている本計測により収集されたＧＰＳデータと慣性データとオドメトリデータとが随時記憶されるものとする。
【０１３６】
自己位置標定装置１００は、本計測の前に、予備計測により得られたＧＰＳ予備データ群１９４、慣性予備データ群１９６、オドメトリ予備データ群１９５を用いて１回目の航法処理（Ｓ１００）を行う。
自己位置標定装置１００は、１回目の航法処理（Ｓ１００）で算出されたＩＭＵ・ＯＤＯ誤差推定初期値を２回目の航法処理（Ｓ２００）で用いる。
２回目の航法処理（Ｓ２００）において、自己位置標定装置１００は、本計測により得られたＧＰＳデータ群１９１、慣性データ群１９３（第２の慣性データ群）およびオドメトリデータ群１９２を処理して位置、姿勢および速度を標定する。
【０１３７】
予備計測は本計測と同じ地域において本計測の直前に行うのが望ましい。本計測との条件が近いほどＩＭＵ・ＯＤＯ誤差推定初期値の信頼性が高く、標定精度を向上させることができるからである。
【符号の説明】
【０１３８】
１００ 自己位置標定装置、１１０ 横滑り特性学習部、１２０ ＧＰＳ処理部、１２１ 並進系二重位相差計算部、１２２ 並進系二重位相差予測部、１２３ 並進系二重位相差残差計算部、１２４ 姿勢系二重位相差計算部、１２５ 姿勢系二重位相差予測部、１２６ 姿勢系二重位相差残差計算部、１２７ デザイン行列計算部、１３０ ＯＤＯ処理部、１３１ 速度／加速度計算部、１３２ 速度／加速度予測部、１３３ 速度／加速度残差計算部、１４０ ＩＭＵ処理部、１４１ 補正計算部、１４２ ストラップダウン演算部、１５０ カルマンフィルタ、１９０ 航法データ記憶部、１９１ ＧＰＳデータ群、１９２ オドメトリデータ群、１９３ 慣性データ群、１９４ ＧＰＳ予備データ群、１９５ オドメトリ予備データ群、１９６ 慣性予備データ群、２００ 車両、２０１ 天板、２１０ 主局ＧＰＳ、２１１，２１２ 従局ＧＰＳ、２２０ 慣性計測装置、２３０ オドメトリ、２９９ 基準局ＧＰＳ、９１１ ＣＰＵ、９１２ バス、９１３ ＲＯＭ、９１４ ＲＡＭ、９１５ 通信ボード、９２０ 磁気ディスク装置、９２１ ＯＳ、９２３ プログラム群、９２４ ファイル群。
【技術分野】
【０００１】
本発明は、例えば、ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）や慣性計測装置のデータを用いて位置を標定する位置標定装置、位置標定方法および位置標定プログラムに関するものである。
【背景技術】
【０００２】
ＧＰＳを利用する航法装置は、その特性上、慣性計測装置（ＩＭＵ：Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ）やオドメトリ（ＯＤＯｍｅｔｒｙ）と組み合わせて、それぞれのセンサの長所を利用した構成とするのが一般的である。しかし、慣性計測装置にはバイアスとスケールファクタ誤差が、オドメトリにはスケールファクタ誤差とオフセット誤差が存在する。これらの誤差は、ＧＰＳを使用できる時間が十分にあれば推定・補正することができる。しかし、計測の開始時は誤差量が不明であるため通常、補正を行わない状態（補正量０）となっている。よって、誤差の推定・補正を十分に行えるようになる前にＧＰＳを使用できない状況が発生した場合、慣性航法装置とオドメトリとを用いた自律航法（ＩＭＵ／ＯＤＯ）の誤差が顕著となる。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特許３８７５７１４号公報
【特許文献２】特開２００６−１３８８３４号公報
【特許文献３】特開２００８−２４９６３９号公報
【非特許文献】
【０００４】
【非特許文献１】山口裕之，浅野勝宏，天野也寸志，“車体横すべり角推定法の開発”，日本機械学會論文集Ｃ編６７（６５９），ｐｐ．２２９１−２２９８，２００１
【発明の概要】
【発明が解決しようとする課題】
【０００５】
本発明は、例えば、航法の最初から慣性計測装置やオドメトリのデータを補正して高い精度で位置を標定できるようにすることを目的とする。
【課題を解決するための手段】
【０００６】
本発明の位置標定装置は、
移動体が備えるＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）受信機により異なる時刻に取得された複数のＧＰＳデータと、前記移動体が備える慣性計測装置により異なる時刻に取得された複数の慣性データとを記憶する航法データ記憶部と、
前記航法データ記憶部に記憶された複数のＧＰＳデータと複数の慣性データとに基づいて前記慣性計測装置の計測誤差の推定値として誤差推定初期値を算出する誤差推定部と、
前記複数の慣性データのうち最初に取得された先頭慣性データを前記誤差推定部により算出された誤差推定初期値に基づいて補正する慣性データ補正部と、
前記慣性データ補正部により補正された先頭慣性データに基づいて前記先頭慣性データが取得された先頭時刻における前記移動体の位置として先頭時刻の標定位置を算出する慣性航法部とを備える。
【発明の効果】
【０００７】
本発明によれば、例えば、慣性計測装置の先頭慣性データを補正して先頭時刻の位置を高い精度で標定することができる。
【図面の簡単な説明】
【０００８】
【図１】実施の形態１における自己位置標定装置１００の機能構成図。
【図２】実施の形態１における車両２００の構成図。
【図３】実施の形態１における自己位置標定装置１００の位置標定方法を示すフローチャート。
【図４】実施の形態１の位置標定方法において１回目の航法処理（Ｓ１００）で算出される誤差推定値（および分散）の時間変化を示すグラフ。
【図５】実施の形態１の位置標定方法において２回目の航法処理（Ｓ２００）で算出される誤差推定値（および分散）の時間変化を示すグラフ。
【図６】実施の形態１における位置標定方法の１回目の航法処理（Ｓ１００）と２回目の航法処理（Ｓ２００）とを示すフローチャート。
【図７】実施の形態１におけるＧＰＳ／ＩＮＳ複合航法処理（Ｓ３００）を示すフローチャート。
【図８】実施の形態１におけるＯＤＯ／ＩＮＳ複合航法処理（Ｓ４００）を示すフローチャート。
【図９】実施の形態１における自己位置標定装置１００のＧＰＳ処理部１２０の機能構成図。
【図１０】実施の形態１における二重位相差残差算出処理（Ｓ３１０）を示すフローチャート。
【図１１】実施の形態１における自己位置標定装置１００のＯＤＯ処理部１３０とＩＭＵ処理部１４０との機能構成図。
【図１２】実施の形態１における車両座標系ｘｙｚとオドメトリ２３０のオフセット誤差ｏｓとの関係図。
【図１３】実施の形態１における横滑り角βを示す図。
【図１４】実施の形態１におけるコーナリングパワーＳｓｌｉｐとオフセット誤差ｏｓ［１］とを表すグラフ。
【図１５】実施の形態１におけるオドメトリ２３０のスケールファクタ誤差を表すグラフ。
【図１６】実施の形態１における横滑り特性学習処理（Ｓ５００）を示すフローチャート。
【図１７】実施の形態１における速度残差算出処理（Ｓ４１０）を示すフローチャート。
【図１８】実施の形態１における自己位置標定装置１００のハードウェア資源の一例を示す図。
【図１９】実施の形態２における自己位置標定装置１００の機能構成図。
【発明を実施するための形態】
【０００９】
実施の形態１．
車両（移動体の一例）の位置をＧＰＳ、慣性計測装置およびオドメトリを用いて標定する形態について説明する。
【００１０】
図１は、実施の形態１における自己位置標定装置１００の機能構成図である。
実施の形態１における自己位置標定装置１００の機能構成について、図１に基づいて以下に説明する。
【００１１】
自己位置標定装置１００（位置標定装置の一例）は、横滑り特性学習部１１０、ＧＰＳ処理部１２０、ＯＤＯ処理部１３０、ＩＭＵ処理部１４０、カルマンフィルタ１５０および航法データ記憶部１９０を備える。
【００１２】
航法データ記憶部１９０は、ＧＰＳデータ群１９１、慣性データ群１９３およびオドメトリデータ群１９２を記憶する。
ＧＰＳデータ群１９１は、車両が備えるＧＰＳ受信機により異なる時刻に取得された複数のＧＰＳデータである。
慣性データ群１９３は、車両が備える慣性計測装置により異なる時刻に取得された複数の慣性データである。
オドメトリデータ群１９２は、車両が備えるオドメトリにより異なる時刻に取得された複数のオドメトリデータである。
【００１３】
図２は、実施の形態１における車両２００の構成図である。
ＧＰＳデータ群１９１、慣性データ群１９３およびオドメトリデータ群１９２を取得する車両２００の構成について、図２に基づいて以下に説明する。
【００１４】
車両２００には天板２０１が設置され、天板２０１には３台のＧＰＳ受信機と慣性計測装置２２０とが取り付けられている。
車両２００は自己位置標定装置１００およびオドメトリ２３０を備える。
【００１５】
天板２０１の３台のＧＰＳ受信機のうち１台を主局のＧＰＳ受信機（主局ＧＰＳ２１０）とし、残りの２台を従局のＧＰＳ受信機（従局ＧＰＳ２１１、従局ＧＰＳ２１２）とする。
各ＧＰＳ受信機は、ＧＰＳ衛星から発信されるＧＰＳの搬送波（測位信号）を受信し、受信結果から観測値を取得し、取得した観測値をＧＰＳデータとして出力する。
ＧＰＳ受信機が出力するＧＰＳデータには、受信した搬送波の位相（搬送波位相）、ＧＰＳ受信機とＧＰＳ衛星との距離（疑似距離）、単独測位で求められたＧＰＳ受信機の座標値、搬送波から得られる航法メッセージなどが含まれる。
【００１６】
慣性計測装置２２０（ＩＭＵ）は、ジャイロセンサと加速度センサとを備える。
ジャイロセンサは、周期的に、車両２００の３軸方向ｘｙｚそれぞれの角速度を計測して出力する。以下、３軸方向ｘｙｚの角速度を「角速度ベクトル」という。
加速度センサは、周期的に、車両２００の３軸方向ｘｙｚの加速度を計測して出力する。以下、３軸方向ｘｙｚの加速度を「加速度ベクトル」という。
慣性計測装置２２０は、角速度ベクトルと加速度ベクトルとを慣性データとして出力する。
【００１７】
オドメトリ２３０（ＯＤＯ）は、周期的に、車両２００の速度を表す車速パルス（速度データの一例）を計測し、計測した車速パルスをオドメトリデータとして出力する。
車速パルスは単位時間当たりのタイヤの回転数を表す。車速パルスにタイヤの円周の長さを掛けることにより車両２００の速度のスカラー量（以下、速度スカラーという）が求まる。
【００１８】
自己位置標定装置１００は、ＧＰＳデータと慣性データとオドメトリデータとに基づいて各時刻における車両２００の位置、姿勢および速度を算出する。
例えば、自己位置標定装置１００は車両２００の位置として三次元の座標値（緯度、経度、高度）を算出する。
また、自己位置標定装置１００は車両２００の姿勢として三次元の姿勢角（ロール角、ピッチ角、ヨー角）を算出する。
また、自己位置標定装置１００は車両２００の速度として速度ベクトル（速度のベクトル量）を算出する。
【００１９】
以下、車両２００の前後方向（縦方向、長さ方向）を「ｘ軸」、車両２００の左右方向（横方向、幅方向）を「ｙ軸」、車両２００の上下方向（高さ方向）を「ｚ軸」とする。
また、ｘ軸回りの角度を「ロール角φ（回転角）」、ｙ軸回りの角度を「ピッチ角θ（仰角）」、ｚ軸回りの角度を「ヨー角ψ（方位角）」とする。
また、ｘｙｚ軸で表される座標系（原点Ｏ）を「車両座標系」とする。
【００２０】
自己位置標定装置１００の航法データ記憶部１９０（図１参照）には、車両２００に設置された３つのＧＰＳ受信機それぞれにより取得された複数のＧＰＳデータがＧＰＳデータ群１９１として予め記憶される。また、航法データ記憶部１９０には、位置が固定であり座標値が既知であるＧＰＳ受信機（電子基準点）（以下、基準局ＧＰＳという）により取得された複数のＧＰＳデータもＧＰＳデータ群１９１として予め記憶される。
航法データ記憶部１９０には、車両２００に設置された慣性計測装置２２０により計測された複数の慣性データが慣性データ群１９３として予め記憶される。
航法データ記憶部１９０には、車両２００に設置されたオドメトリ２３０により計測された複数のオドメトリデータがオドメトリデータ群１９２として予め記憶される。
【００２１】
ＧＰＳデータ群１９１と慣性データ群１９３とオドメトリデータ群１９２とに含まれる各データは、取得時刻（計測時刻、検知時刻）と対応付けられているものとする。
【００２２】
図１に戻り、自己位置標定装置１００の構成の説明を続ける。
【００２３】
カルマンフィルタ１５０（誤差推定部の一例）は、ＧＰＳデータ群１９１と慣性データ群１９３とに基づいて慣性計測装置２２０の計測誤差の推定値として誤差推定初期値（後述するＩＭＵ誤差推定初期値）を算出する。
【００２４】
ＩＭＵ処理部１４０（慣性データ補正部、慣性航法部の一例）は、慣性データ群１９３のうち最初に取得された先頭慣性データをカルマンフィルタ１５０により算出された誤差推定初期値に基づいて補正する。
ＩＭＵ処理部１４０は、補正した先頭慣性データに基づいて先頭慣性データが取得された先頭時刻における車両２００の位置として先頭時刻の標定位置を算出する。
【００２５】
ＧＰＳ処理部１２０（搬送波位相残差算出部の一例）は、ＧＰＳデータに含まれる搬送波位相とＩＭＵ処理部１４０により算出される参考位置とに基づいて、搬送波位相の残差（後述する並進系・姿勢系二重位相差残差）を算出する。
【００２６】
例えば、誤差推定初期値は以下のように出力される。
【００２７】
ＩＭＵ処理部１４０は、カルマンフィルタ１５０により誤差推定初期値が算出される前に、慣性データ群１９３のうち最初に取得された先頭慣性データに基づいて先頭慣性データが取得された先頭時刻における車両２００の位置として先頭時刻の参考位置を算出する。
ＩＭＵ処理部１４０は、慣性データ群１９３のうち先頭慣性データを除く残りの慣性データ毎に、カルマンフィルタ１５０により算出される当該時刻の誤差推定値に基づいて当該慣性データを補正する。ＩＭＵ処理部１４０は、補正した当該慣性データに基づいて当該慣性データが取得された当該時刻における車両２００の位置として当該時刻の参考位置を算出する。
ＧＰＳ処理部１２０は、ＩＭＵ処理部１４０により算出された当該時刻の参考位置と当該時刻に取得されたＧＰＳデータに含まれる搬送波位相とに基づいて、当該時刻における搬送波位相の残差として当該時刻の搬送波位相残差を算出する。
カルマンフィルタ１５０は、ＧＰＳ処理部１２０により算出された当該時刻の搬送波位相残差を用いて当該時刻に対する慣性計測装置２２０の計測誤差の推定値として当該時刻の誤差推定値を算出する。
カルマンフィルタ１５０は、ＧＰＳデータ群１９１のうち最後に取得された最終ＧＰＳデータの取得時刻の誤差推定値に基づいて誤差推定初期値を出力する。
【００２８】
また、先頭時刻の標定位置が算出された後、以下のように動作する。
【００２９】
ＧＰＳ処理部１２０は、ＩＭＵ処理部１４０により算出された先頭時刻の標定位置と先頭時刻に取得された先頭ＧＰＳデータに含まれる搬送波位相とに基づいて、先頭時刻の搬送波位相残差を算出する。
ＧＰＳ処理部１２０は、ＧＰＳデータ群１９１のうち先頭ＧＰＳデータを除く残りのＧＰＳデータ毎に、ＩＭＵ処理部１４０により算出される当該時刻の標定位置と当該時刻に取得された当該ＧＰＳデータに含まれる搬送波位相とに基づいて当該時刻の搬送波位相残差を算出する。
カルマンフィルタ１５０は、ＧＰＳ処理部１２０により算出された当該時刻の搬送波位相残差に基づいて当該時刻の誤差推定値を算出する。
ＩＭＵ処理部１４０は、当該時刻に取得された当該慣性データの次に取得された次慣性データをカルマンフィルタ１５０により算出された当該時刻の誤差推定値に基づいて補正する。ＩＭＵ処理部１４０は、補正した次慣性データに基づいて次慣性データが取得された次時刻の標定位置を算出する。
【００３０】
ＩＭＵ処理部１４０は、補正した慣性データに基づいて当該時刻における車両２００の速度を算出する。
横滑り特性学習部１１０は、オドメトリデータ群１９２のうち当該時刻に取得されたオドメトリデータとＩＭＵ処理部１４０により算出された当該時刻の速度とに基づいて、オドメトリ２３０の計測誤差の推定値として誤差推定初期値（後述するＯＤＯ誤差推定初期値）を算出する。
ＯＤＯ処理部１３０は、当該時刻に取得されたオドメトリデータとＩＭＵ処理部１４０により算出された当該時刻の速度と横滑り特性学習部１１０により算出された誤差推定初期値とに基づいて、当該時刻の速度残差を算出する。
カルマンフィルタ１５０は、ＯＤＯ処理部１３０により算出された当該時刻の速度残差に基づいて、当該時刻の誤差推定値（後述するＩＭＵ誤差推定値）を算出する。
【００３１】
図３は、実施の形態１における自己位置標定装置１００の位置標定方法を示すフローチャートである。実施の形態１における自己位置標定装置１００の位置標定方法について、図３に基づいて以下に説明する。
【００３２】
１回目の航法処理（Ｓ１００）において、自己位置標定装置１００は、ＧＰＳデータ群１９１、慣性データ群１９３およびオドメトリデータ群１９２といった航法データに基づいて、車両２００の位置、姿勢および速度を参考航法結果として算出する。
そして、自己位置標定装置１００は、参考航法結果に基づいてＩＭＵ誤差推定初期値とＯＤＯ誤差推定初期値とを算出する。
ＩＭＵ誤差推定初期値とは、慣性計測装置２２０の計測誤差の推定値（以下、ＩＭＵ誤差推定値という）である。
ＯＤＯ誤差推定初期値とは、オドメトリ２３０の計測誤差の推定値（以下、ＯＤＯ誤差推定値という）である。
【００３３】
２回目の航法処理（Ｓ２００）において、自己位置標定装置１００は、ＧＰＳデータ群１９１、慣性データ群１９３、オドメトリデータ群１９２、ＩＭＵ誤差推定初期値およびＯＤＯ誤差推定初期値に基づいて、車両２００の位置、姿勢および速度を標定航法結果として算出する。
自己位置標定装置１００は、標定航法結果を車両２００の位置、姿勢および速度として出力する。
【００３４】
図４は、実施の形態１の位置標定方法において１回目の航法処理（Ｓ１００）で算出される誤差推定値（および分散）の時間変化を示すグラフである。
上のグラフは誤差推定値の時間変化を示し、下のグラフは誤差推定値の分散の時間変化を示している。
【００３５】
慣性計測装置２２０（ジャイロセンサ、加速度センサ）により取得される慣性データには、バイアスやスケールファクタ誤差といった計測誤差が含まれる。慣性計測装置２２０のスケールファクタ誤差は電源を投入するたびに変化することが知られている。
また、オドメトリ２３０により取得されるオドメトリデータには、スケールファクタ誤差、オフセット誤差、オフセット誤差に関するパラメータ（例えば、後述するコーナリングパワー）といった計測誤差が含まれる。オドメトリ２３０のスケールファクタ誤差、オフセット誤差、オフセット誤差に関するパラメータは車両２００の重心位置の変化、タイヤの空気圧の変化、路面の状況などに応じて変化することが知られている。
これらの計測誤差は不明であるため、慣性データに基づいて算出する航法結果には計測誤差分の誤差が含まれる。
【００３６】
但し、これらの計測誤差は、慣性データから得られる特定値とＧＰＳデータから得られる特定値とを比較することにより推定することができる。
計測誤差の推定値（誤差推定値）は、推定を繰り返すことにより一定値に近づき（図４上）、十分に収斂する（図４下）。十分に収斂した誤差推定値の精度は十分に高い。
【００３７】
自己位置標定装置１００は、１回目の航法処理（Ｓ１００）によって十分に収斂した誤差推定値を誤差推定初期値として２回目の航法処理（Ｓ２００）で用いる。
【００３８】
図５は、実施の形態１の位置標定方法において２回目の航法処理（Ｓ２００）で算出される誤差推定値（および分散）の時間変化を示すグラフである。
【００３９】
２回目の航法処理（Ｓ２００）において誤差推定初期値を用いることにより、誤差推定値は始めからほぼ一定値を示し（図５上）、すぐに収斂する（図５下）。
【００４０】
自己位置標定装置１００は、２回目の航法処理（Ｓ２００）において誤差推定初期値を用いることにより誤差推定値をすぐに収斂させ、高い精度で航法結果を得ることができる。
【００４１】
図６は、実施の形態１における位置標定方法の１回目の航法処理（Ｓ１００）と２回目の航法処理（Ｓ２００）とを示すフローチャートである。
１回目の航法処理（Ｓ１００）と２回目の航法処理（Ｓ２００）とについて、図６に基づいて以下に説明する。
【００４２】
１回目の航法処理（Ｓ１００）をＳ１１０〜Ｓ１２１およびＳ３００〜Ｓ５００で示し、２回目の航法処理（Ｓ２００）をＳ２０１〜Ｓ２２１およびＳ３００〜Ｓ５００で示す。
【００４３】
自己位置標定装置１００は、１回目の航法処理（Ｓ１００）と２回目の航法処理（Ｓ２００）とで同様な航法処理を行う。
但し、ＩＭＵ・ＯＤＯ誤差推定初期値は１回目の航法処理（Ｓ１００）により特定されるため、ＩＭＵ・ＯＤＯ誤差推定初期値を用いる処理（Ｓ２０１、Ｓ２１１）は２回目の航法処理（Ｓ２００）でのみ行われる。
【００４４】
まず、１回目の航法処理（Ｓ１００）について説明する。
【００４５】
Ｓ１１０において、ＩＭＵ処理部１４０は慣性データ群１９３のうち最初に取得された慣性データ（以下、先頭慣性データという）を取得する。
ＩＭＵ処理部１４０は、先頭慣性データを用いて慣性航法（ＩＮＳ：Ｉｎｅｒｔｉａｌ Ｎａｖｉｇａｔｉｏｎ Ｓｙｓｔｅｍ）を行い、先頭慣性データが取得された時刻（以下、先頭時刻という）における車両２００の位置、姿勢および速度を算出する。
Ｓ１１０の後、Ｓ１２０に進む。
【００４６】
Ｓ１２０において、ＩＭＵ処理部１４０は、次の慣性データが有るか否か（処理していない慣性データが残っているか否か）を判定する。
次の慣性データが有る場合（ＹＥＳ）、Ｓ１２１に進む。
次の慣性データが無い場合（ＮＯ）、１回目の航法処理（Ｓ１００）は終了する。
【００４７】
Ｓ１２１において、ＧＰＳ処理部１２０は、ＩＭＵ処理部１４０により算出された車両２００の位置、姿勢および速度に対応する当該時刻がＧＰＳ可視時であるか否かを判定する。
【００４８】
ＧＰＳ可視時とは、車両２００に設置された３つのＧＰＳ受信機と所定地に設けられたＧＰＳ受信機（基準局ＧＰＳ）とが所定数以上のＧＰＳ衛星から搬送波を受信できた時刻を意味する。
【００４９】
ＧＰＳ処理部１２０は、ＧＰＳデータ群１９１から各ＧＰＳ受信機の当該時刻のＧＰＳデータを取得し、取得した各ＧＰＳデータに所定数以上の搬送波位相が含まれている場合にＧＰＳ可視時であると判定する。
ＧＰＳ処理部１２０は、取得した各ＧＰＳデータに所定数以上の搬送波位相が含まれていない場合、ＧＰＳ不可視時であると判定する。
【００５０】
ＧＰＳ可視時である場合（ＹＥＳ）、ＧＰＳ／ＩＮＳ複合航法処理（Ｓ３００）に進む。
ＧＰＳ不可視時である場合（ＮＯ）、ＯＤＯ／ＩＮＳ複合航法処理（Ｓ４００）に進む。
【００５１】
ＧＰＳ／ＩＮＳ複合航法処理（Ｓ３００）において、ＩＭＵ処理部１４０は、ＧＰＳデータと慣性データとに基づいて車両２００の位置、姿勢および速度を算出する。
カルマンフィルタ１５０は、車両２００の位置、姿勢および速度に基づいてＩＭＵ誤差推定値を算出する。
ＧＰＳ／ＩＮＳ複合航法処理（Ｓ３００）の詳細については後述する。
ＧＰＳ／ＩＮＳ複合航法処理（Ｓ３００）の後、横滑り特性学習処理（Ｓ５００）に進む。
【００５２】
横滑り特性学習処理（Ｓ５００）において、横滑り特性学習部１１０は、ＧＰＳ／ＩＮＳ複合航法処理（Ｓ３００）で算出された車両２００の速度と当該時刻のオドメトリデータに含まれる車速パルスとに基づいて、ＯＤＯ誤差推定値を算出する。
横滑り特性学習処理（Ｓ５００）の後、Ｓ１２０に戻る。
【００５３】
ＯＤＯ／ＩＮＳ複合航法処理（Ｓ４００）において、ＩＭＵ処理部１４０は、オドメトリデータと慣性データとに基づいて車両２００の位置、姿勢および速度を算出する。
カルマンフィルタ１５０は、車両２００の位置、姿勢および速度に基づいてＩＭＵ誤差推定値を算出する。
ＯＤＯ／ＩＮＳ複合航法処理（Ｓ４００）の詳細については後述する。
ＯＤＯ／ＩＮＳ複合航法処理（Ｓ４００）の後、Ｓ１２０に戻る。
【００５４】
自己位置標定装置１００は、１回目の航法処理（Ｓ１００）のＧＰＳ／ＩＮＳ複合航法処理（Ｓ３００）で最後に算出されたＩＭＵ誤差推定値をＩＭＵ誤差推定初期値として用いる。
自己位置標定装置１００は、１回目の航法処理（Ｓ１００）の横滑り特性学習処理（Ｓ５００）で最後に算出されたＯＤＯ誤差推定値をＯＤＯ誤差推定初期値として用いる。
【００５５】
自己位置標定装置１００は、Ｓ１２０においてＩＭＵ誤差推定値およびＯＤＯ誤差推定値が収斂していれば、次の慣性データが有る場合でも１回目の航法処理（Ｓ１００）を終了して構わない。
例えば、ＩＭＵ処理部１４０は、それまでに算出されたＩＭＵ誤差推定値（またはＯＤＯ誤差推定値）を新しい順に所定数取得し、取得したうちの最大値と最小値との差が所定値未満であればＩＭＵ誤差推定値が収斂したと判定し、１回目の航法処理（Ｓ１００）を終了する。
【００５６】
自己位置標定装置１００は、１回目の航法処理で最後に算出されたＩＭＵ・ＯＤＯ誤差推定値をＩＭＵ・ＯＤＯ誤差推定初期値にしなくても構わない。
例えば、自己位置標定装置１００は、それまでに算出されたＩＭＵ誤差推定値（またはＯＤＯ誤差推定値）を最後のＩＭＵ誤差推定値を含めて新しい順に所定数取得し、取得した各ＩＭＵ誤差推定値の平均値をＩＭＵ誤差推定初期値としても構わない。
【００５７】
次に、２回目の航法処理（Ｓ２００）について説明する。
【００５８】
Ｓ２０１において、ＩＭＵ処理部１４０は、１回目の航法処理（Ｓ１００）で算出されたＩＭＵ誤差推定初期値を誤差補正量として先頭慣性データの値に加算する。先頭慣性データの値に誤差補正量を加算することにより、先頭慣性データを補正することができる。
Ｓ２０１の後、Ｓ２１０に進む。
【００５９】
Ｓ２１０において、ＩＭＵ処理部１４０は、補正した先頭慣性データに基づいて先頭時刻における車両２００の位置、姿勢および速度を算出する（Ｓ１１０と同様）。
Ｓ２１０の後、Ｓ２１１に進む。
【００６０】
Ｓ２１１において、横滑り特性学習部１１０は、１回目の航法処理（Ｓ２００）で算出されたＯＤＯ誤差推定初期値を初期設定する。
Ｓ２１１の後、Ｓ２２０に進む。
【００６１】
Ｓ２２０において、次の慣性データが有れば（ＹＥＳ）、Ｓ２２１に進む。
また、次の慣性データが無ければ（ＮＯ）、２回目の航法処理（Ｓ２００）は終了する。
【００６２】
Ｓ２２１において、ＧＰＳ可視時であれば（ＹＥＳ）、ＧＰＳ／ＩＮＳ複合航法処理（Ｓ３００）に進む。
また、ＧＰＳ不可視時であれば（ＮＯ）、ＯＤＯ／ＩＮＳ複合航法処理（Ｓ４００）に進む。
【００６３】
ＧＰＳ／ＩＮＳ複合航法処理（Ｓ３００）において、ＩＭＵ処理部１４０は、ＧＰＳデータと慣性データとに基づいて車両２００の位置、姿勢および速度を算出する。
カルマンフィルタ１５０は、車両２００の位置、姿勢および速度に基づいてＩＭＵ誤差推定値を算出する。
ＧＰＳ／ＩＮＳ複合航法処理（Ｓ３００）の後、横滑り特性学習処理（Ｓ５００）に進む。
【００６４】
横滑り特性学習処理（Ｓ５００）において、横滑り特性学習部１１０は、ＧＰＳ／ＩＮＳ複合航法処理（Ｓ３００）で算出された車両２００の速度と当該時刻のオドメトリデータに含まれる車速パルスとに基づいてＯＤＯ誤差推定値を算出する。
横滑り特性学習処理（Ｓ５００）の後、Ｓ２２０に戻る。
【００６５】
ＯＤＯ／ＩＮＳ複合航法処理（Ｓ４００）において、ＩＭＵ処理部１４０は、オドメトリデータと慣性データとに基づいて車両２００の位置、姿勢および速度を算出する。
カルマンフィルタ１５０は、車両２００の位置、姿勢および速度に基づいてＩＭＵ誤差推定値を算出する。
ＯＤＯ／ＩＮＳ複合航法処理（Ｓ４００）の後、Ｓ２２０に戻る。
【００６６】
自己位置標定装置１００は、２回目の航法処理（Ｓ２００）のＧＰＳ／ＩＮＳ複合航法処理（Ｓ３００）とＯＤＯ／ＩＮＳ複合航法処理（Ｓ４００）とで算出された各時刻における車両２００の位置、姿勢および速度を航法結果（標定値）として出力する。
【００６７】
図７は、実施の形態１におけるＧＰＳ／ＩＮＳ複合航法処理（Ｓ３００）を示すフローチャートである。
実施の形態１におけるＧＰＳ／ＩＮＳ複合航法処理（Ｓ３００）について、図７に基づいて以下に説明する。
【００６８】
二重位相差残差算出処理（Ｓ３１０）において、ＧＰＳ処理部１２０は、一つ前に算出された車両２００の位置と姿勢と当該時刻のＧＰＳデータに含まれる搬送波位相とに基づいて、当該時刻の二重位相差残差を算出する。
二重位相差残差算出処理（Ｓ３１０）の詳細については後述する。
二重位相差残差算出処理（Ｓ３１０）の後、Ｓ３２０に進む。
【００６９】
Ｓ３２０において、カルマンフィルタ１５０は、当該時刻の二重位相差残差を入力として当該時刻のＩＭＵ誤差推定値を算出する。
【００７０】
カルマンフィルタ１５０の概要について説明する。
カルマンフィルタ１５０は、モデル化された状態方程式と観測方程式とを用いて誤差補正量（ＩＭＵ誤差推定値や後述する航法誤差推定値）を算出する。
カルマンフィルタ１５０に入力される残差（二重位相差残差や速度残差）は観測方程式の変数として用いられる。
【００７１】
カルマンフィルタ１５０は、観測ノイズ行列Ｒ、誤差共分散行列Ｐおよび観測行列Ｈを用いてカルマンゲインベクトルＫを算出する。
観測ノイズ行列Ｒと観測行列Ｈは残差の種類ごとに用意される。
誤差共分散行列Ｐは、状態量（位置、姿勢、速度、ＩＭＵ誤差推定値、航法誤差推定値など）の共分散値を表す。
観測行列Ｈは、状態量と観測量（二重位相差残差、速度残差など）との関係を表す。
【００７２】
カルマンゲインベクトルＫの算出式（１）を以下に示す。式（１）において「ＨＴ」は観測行列Ｈの転置行列を表す。
【００７３】
【数１】
【００７４】
残差ｄｚにカルマンゲインベクトルＫを掛けて求まる複数の値Ｋｄｚが誤差補正量である。
【００７５】
Ｓ３２０の後、Ｓ３３０に進む。
【００７６】
Ｓ３３０において、ＩＭＵ処理部１４０は、当該時刻のＩＭＵ誤差推定値を誤差補正量として次の慣性データの値に加算する。慣性データの値に誤差補正量を加算することにより、慣性データを補正することができる。
Ｓ３３０の後、Ｓ３４０に進む。
【００７７】
Ｓ３４０において、ＩＭＵ処理部１４０は、補正した慣性データを用いて慣性航法を行い、慣性データが取得された時刻（次の時刻）における車両２００の位置、姿勢および速度を算出する。
Ｓ３４０の後、ＧＰＳ／ＩＮＳ複合航法処理（Ｓ３００）は終了する。
【００７８】
図８は、実施の形態１におけるＯＤＯ／ＩＮＳ複合航法処理（Ｓ４００）を示すフローチャートである。
実施の形態１におけるＯＤＯ／ＩＮＳ複合航法処理（Ｓ４００）について、図８に基づいて以下に説明する。
【００７９】
速度残差算出処理（Ｓ４１０）において、ＯＤＯ処理部１３０は、一つ前に算出された車両２００の速度と当該時刻のＯＤＯ誤差推定値とに基づいて、当該時刻の速度残差を算出する。
速度残差算出処理（Ｓ４１０）の詳細については後述する。
速度残差算出処理（Ｓ４１０）の後、Ｓ４２０に進む。
【００８０】
Ｓ４２０において、カルマンフィルタ１５０は、当該時刻の速度残差を入力として当該時刻のＩＭＵ誤差推定値を算出する。
Ｓ４２０の後、Ｓ４３０に進む。
【００８１】
Ｓ４３０において、ＩＭＵ処理部１４０は、当該時刻のＩＭＵ誤差推定値を誤差補正量として次の慣性データを補正する。
Ｓ４３０の後、Ｓ４４０に進む。
【００８２】
Ｓ４４０において、ＩＭＵ処理部１４０は、補正した慣性データを用いて慣性航法を行い、慣性データが取得された時刻（次の時刻）における車両２００の位置、姿勢および速度を算出する。
Ｓ４４０の後、ＯＤＯ／ＩＮＳ複合航法処理（Ｓ４００）は終了する。
【００８３】
次に、ＧＰＳ／ＩＮＳ複合航法処理（Ｓ３００）で行われる二重位相差残差算出処理（Ｓ３１０）とＯＤＯ／ＩＮＳ複合航法処理（Ｓ４００）で行われる速度残差算出処理（Ｓ４１０）との具体例について説明する。
また、横滑り特性学習処理（Ｓ５００）の具体例について説明する。
【００８４】
図９は、実施の形態１における自己位置標定装置１００のＧＰＳ処理部１２０の機能構成図である。
実施の形態１におけるＧＰＳ処理部１２０の機能構成の具体例について、図９に基づいて以下に説明する。
【００８５】
ＧＰＳ処理部１２０は、並進系二重位相差残差と姿勢系二重位相差残差とを算出する。
ＧＰＳ処理部１２０は、並進系二重位相差残差を算出する構成として、並進系二重位相差計算部１２１、並進系二重位相差予測部１２２および並進系二重位相差残差計算部１２３を備える。
ＧＰＳ処理部１２０は、姿勢系二重位相差残差を算出する構成として、姿勢系二重位相差計算部１２４、姿勢系二重位相差予測部１２５および姿勢系二重位相差残差計算部１２６を備える。
さらに、ＧＰＳ処理部１２０はデザイン行列計算部１２７を備える。
ＧＰＳ処理部１２０の各構成の詳細については後述する。
【００８６】
カルマンフィルタ１５０は、ＧＰＳ処理部１２０により算出される並進系二重位相差残差と姿勢系二重位相差残差とに基づいて、ＩＭＵ誤差推定値（および後述する航法誤差推定値）を算出する。
【００８７】
図１０は、実施の形態１における二重位相差残差算出処理（Ｓ３１０）を示すフローチャートである。
実施の形態１における二重位相差残差算出処理（Ｓ３１０）の具体例について、図１０に基づいて以下に説明する。
【００８８】
Ｓ３１１において、並進系二重位相差計算部１２１は、ＧＰＳデータ群１９１から基準局ＧＰＳと主局ＧＰＳ２１０との対象時刻のＧＰＳデータを取得し、取得したＧＰＳデータからＧＰＳ衛星Ａの搬送波位相ＡとＧＰＳ衛星Ｂの搬送波位相Ｂとを取得する。
並進系二重位相差計算部１２１は、基準局ＧＰＳの搬送波位相Ａと主局ＧＰＳ２１０の搬送波位相Ａとの差を搬送波位相差Ａとして算出し、基準局ＧＰＳの搬送波位相Ｂと主局ＧＰＳ２１０の搬送波位相Ｂとの差を搬送波位相差Ｂとして算出する。
並進系二重位相差計算部１２１は、搬送波位相差Ａと搬送波位相差Ｂとの差を並進系二重位相差として算出する。
【００８９】
並進系とは、基準局ＧＰＳに対する主局ＧＰＳ２１０の相対位置により車両２００の位置が求まることを意味する。
【００９０】
Ｓ３１１の後、Ｓ３１２に進む。
【００９１】
Ｓ３１２において、並進系二重位相差予測部１２２は、基準局ＧＰＳ２９９の位置とＩＭＵ処理部１４０により算出された対象時刻における車両２００の位置とデザイン行列計算部１２７により算出される並進系デザイン行列とに基づいて、並進系二重位相差を算出する。
デザイン行列計算部１２７は、各ＧＰＳデータに含まれるエフェメリス（航法メッセージに含まれる軌道情報）に基づいて、２つのＧＰＳ受信機の位置と並進系二重位相差との関係を表す行列を並進系デザイン行列として算出する。
Ｓ３１２の後、Ｓ３１３に進む。
【００９２】
Ｓ３１３において、並進系二重位相差残差計算部１２３は、Ｓ３１１で算出された並進系二重位相差とＳ３１２で算出された並進系二重位相差との差を対象時刻の並進系二重位相差残差として算出する。
Ｓ３１３の後、Ｓ３１４に進む。
【００９３】
Ｓ３１４において、姿勢系二重位相差計算部１２４は、ＧＰＳデータ群１９１から主局ＧＰＳ２１０、従局ＧＰＳ２１１および従局ＧＰＳ２１２の対象時刻のＧＰＳデータを取得する。姿勢系二重位相差計算部１２４は、取得したＧＰＳデータからＧＰＳ衛星Ａの搬送波位相ＡとＧＰＳ衛星Ｂの搬送波位相Ｂとを入力する。
姿勢系二重位相差計算部１２４は、主局ＧＰＳ２１０の搬送波位相Ａと従局ＧＰＳ２１１の搬送波位相Ａとの差を第１の搬送波位相差Ａとして算出し、主局ＧＰＳ２１０の搬送波位相Ｂと従局ＧＰＳ２１１の搬送波位相Ｂとの差を第１の搬送波位相差Ｂとして算出する。同様に、姿勢系二重位相差計算部１２４は、主局ＧＰＳ２１０の搬送波位相Ａ・Ｂと従局ＧＰＳ２１２の搬送波位相Ａ・Ｂとに基づいて第２の搬送波位相差Ａ・Ｂを算出する。
姿勢系二重位相差計算部１２４は、第１の搬送波位相差Ａと第１の搬送波位相差Ｂとの差を第１の姿勢系二重位相差として算出し、第２の搬送波位相差Ａと第２の搬送波位相差Ｂとの差を第２の姿勢系二重位相差として算出する。
【００９４】
姿勢系とは、主局ＧＰＳ２１０に対する従局ＧＰＳ２１１および従局ＧＰＳ２１２の相対位置（基線ベクトル）により車両２００の姿勢が求まることを意味する。
【００９５】
Ｓ３１４の後、Ｓ３１５に進む。
【００９６】
Ｓ３１５において、姿勢系二重位相差予測部１２５は、主局ＧＰＳ２１０、従局ＧＰＳ２１１および従局ＧＰＳ２１２の相対位置（所定の計測値）とＩＭＵ処理部１４０により算出された対象時刻における車両２００の姿勢とデザイン行列計算部１２７により算出される姿勢系デザイン行列とに基づいて、第１・第２の姿勢系二重位相差を算出する。
デザイン行列計算部１２７は、各ＧＰＳデータに含まれるエフェメリスに基づいて、２つのＧＰＳ受信機の位置と姿勢系二重位相差との関係を表す行列を姿勢系デザイン行列として算出する。
Ｓ３１５の後、Ｓ３１６に進む。
【００９７】
Ｓ３１６において、姿勢系二重位相差残差計算部１２６は、Ｓ３１４で算出された第１の姿勢系二重位相差とＳ３１５で算出された第１の姿勢系二重位相差との差を対象時刻の第１の姿勢系二重位相差残差として算出する。
姿勢系二重位相差残差計算部１２６は、Ｓ３１４で算出された第２の姿勢系二重位相差とＳ３１５で算出された第２の姿勢系二重位相差との差を対象時刻の第２の姿勢系二重位相差残差として算出する。
Ｓ３１６の後、二重位相差残差算出処理（Ｓ３１０）は終了する。
【００９８】
カルマンフィルタ１５０は、二重位相差残差算出処理（Ｓ３１０）で算出された並進系二重位相差残差と第１・第２の姿勢系二重位相差残差とを入力として、対象時刻のＩＭＵ誤差推定値（および後述する航法誤差推定値）を算出する。
【００９９】
図１１は、実施の形態１における自己位置標定装置１００のＯＤＯ処理部１３０とＩＭＵ処理部１４０との機能構成図である。
ＯＤＯ処理部１３０とＩＭＵ処理部１４０との機能構成の具体例について、図１１に基づいて以下に説明する。
【０１００】
ＯＤＯ処理部１３０は、速度／加速度計算部１３１と速度／加速度予測部１３２と速度／加速度残差計算部１３３とを備える。
ＯＤＯ処理部１３０の各構成の詳細については後述する。
【０１０１】
カルマンフィルタ１５０は、慣性計測装置２２０を構成するジャイロセンサと加速度センサとのそれぞれの計測誤差の推定値をＩＭＵ誤差推定値として出力する。
さらに、カルマンフィルタ１５０は、ＩＭＵ処理部１４０により算出された車両２００の位置、姿勢および速度のそれぞれの誤差推定値を航法誤差推定値として出力する。
【０１０２】
ＩＭＵ処理部１４０は、補正計算部１４１とストラップダウン演算部１４２とを備える。
【０１０３】
補正計算部１４１は、対象時刻の慣性データに含まれる角速度ベクトルと加速度ベクトルとにカルマンフィルタ１５０により算出されたＩＭＵ誤差推定値を誤差補正量として加算する。角速度ベクトルと加速度ベクトルとにそれぞれの誤差補正量を加算することにより、角速度ベクトルと加速度ベクトルとを補正することができる。
【０１０４】
ストラップダウン演算部１４２は、補正計算部１４１により補正された角速度ベクトルおよび加速度ベクトルを用いてストラップダウン演算（慣性航法の一例）を行う。
ストラップダウン演算では、角速度ベクトルを積分することにより姿勢（姿勢角ベクトル）が求まり、加速度ベクトルを積分することにより速度（速度ベクトル）が求まる。
さらに、速度（速度ベクトル）を積分することにより距離（距離ベクトル）が求まり、一つ前に算出した位置に距離ベクトルを加算することにより対象時刻の位置（三次元座標値）が求まる。
【０１０５】
ストラップダウン演算部１４２は、ストラップダウン演算により得られた対象時刻における車両２００の位置、姿勢および速度にカルマンフィルタ１５０により算出された航法誤差推定値を誤差補正量として加算する。位置、姿勢および速度にそれぞれの誤差補正量を加算することにより、位置、姿勢および速度を補正することができる。
【０１０６】
横滑り特性学習部１１０は、ストラップダウン演算部１４２により算出された速度ベクトルに基づいて、車両２００のコーナリングパワー、オドメトリ２３０のオフセット誤差およびスケールファクタ誤差を算出する。
コーナリングパワー、オフセット誤差、スケールファクタ誤差はＯＤＯ誤差推定値の一例である。
【０１０７】
図１２は、実施の形態１における車両座標系ｘｙｚとオドメトリ２３０のオフセット誤差ｏｓとの関係図である。
図１３は、実施の形態１における横滑り角βを示す図である。
オドメトリ２３０のオフセット誤差ｏｓについて、図１２と図１３とに基づいて以下に説明する。
【０１０８】
図１２において、車両２００は、搭載物（例えば、乗車している人）のバランスや道路の起伏などに応じて傾き、また、図１３に示すような横滑りをする。
このため、車両２００のタイヤの向きｘと車両２００の進行方向Ｖとは異なり、オドメトリ２３０の車速パルスから算出される正面方向ｘの速度（速度スカラー）は進行方向Ｖの速度に対して誤差を有する。この誤差量をオドメトリ２３０のオフセット誤差という。
ｏｓ［０］はｚ軸方向のオフセット誤差を示し、ｏｓ［１］はｙ軸方向のオフセット誤差を示している。
【０１０９】
図１３において、横向きの力を受ける車両２００はタイヤの傾きよりも浅い角度の方向へ進む。つまり、車両２００はコーナリング時などに横滑りし、横滑り角βを生じる。横滑り角βは、タイヤの向きと車両２００の進行方向とが成すヨー角を示している。
【０１１０】
図１４は、実施の形態１におけるコーナリングパワーＳｓｌｉｐとオフセット誤差ｏｓ［１］とを表すグラフである。
コーナリングパワーＳｓｌｉｐとオフセット誤差ｏｓ［１］とについて、図１４に基づいて以下に説明する。
【０１１１】
横滑り特性直線Ｌは、単位コーナリングフォースａｙと横滑り角βとの関係を表している。
単位コーナリングフォースａｙとは加速度ベクトルの左右方向成分値である。
コーナリングパワーＳｓｌｉｐとは横滑り特性直線Ｌの傾きである。
オスセット誤差ｏｓ［１］は単位コーナリングフォースａｙが「０」のときの横滑り角βに相当する。もう一つのオフセット誤差ｏｓ［０］は加速度ベクトルの上下方向成分値に対する値である。
【０１１２】
図１５は、実施の形態１におけるオドメトリ２３０のスケールファクタ誤差を表すグラフである。
オドメトリ２３０のスケールファクタ誤差について、図１５に基づいて以下に説明する。
【０１１３】
実線は、ＧＰＳデータに基づいて算出されるＧＰＳ距離とオドメトリデータに基づいて算出されるＯＤＯ距離との関係を表している。
オドメトリ２３０のスケールファクタ誤差とはＧＰＳ距離に対するＯＤＯ距離の割合（パーセント）である。
例えば、ＧＰＳ距離が「１００センチメートル」であり、ＯＤＯ距離が「９５センチメートル」であれば、オドメトリ２３０のスケールファクタ誤差は「−５パーセント」である。
点線は、オドメトリ２３０のスケールファクタ誤差が「０パーセント」の場合を示している。つまり、ＧＰＳ距離とＯＤＯ距離とは等しい。
【０１１４】
図１６は、実施の形態１における横滑り特性学習処理（Ｓ５００）を示すフローチャートである。
実施の形態１における横滑り特性学習処理（Ｓ５００）の具体例について、図１６に基づいて以下に説明する。
【０１１５】
Ｓ５１０において、速度／加速度計算部１３１は、対象時刻のオドメトリデータに含まれる車速パルスに車両２００のタイヤの円周長を掛けて対象時刻における車両２００の速度のスカラー量（以下、速度スカラーという）を算出する。
速度／加速度計算部１３１は、速度スカラーを積分して距離（以下、ＯＤＯ距離という）を算出する。
Ｓ５１０の後、Ｓ５２０に進む。
【０１１６】
Ｓ５２０において、横滑り特性学習部１１０は、Ｓ５１０で算出された速度スカラーを速度ベクトルの前後方向成分として用い、ストラップダウン演算部１４２により算出された対象時刻の速度ベクトルのうち左右方向成分値を用いる。
横滑り特性学習部１１０は、速度ベクトルの左右方向成分値を速度スカラーで割った値を横滑り角として算出する。
すなわち、「横滑り角β＝速度ベクトルの左右方向成分値Ｖｙ／速度スカラーＶｏｄｏ」である。タンジェント（ｔａｎ）の値が「Ｖｙ／Ｖｏｄｏ」になる角度を横滑り角βとしてもよい。
Ｓ５２０の後、処理はＳ５３０に進む。
【０１１７】
Ｓ５３０において、横滑り特性学習部１１０は、Ｓ５２０で算出した横滑り角と補正計算部１４１により補正された対象時刻の加速度ベクトルの左右方向成分値とを対応づけて航法データ記憶部１９０に追加して記憶する。
航法データ記憶部１９０には、複数の横滑り角と加速度ベクトルの左右方向成分値（単位コーナリングフォース）とが蓄積される。
Ｓ５３０の後、Ｓ５４０に進む。
【０１１８】
Ｓ５４０において、横滑り特性学習部１１０は、航法データ記憶部１９０に蓄積された複数の横滑り角と単位コーナリングフォースとに基づいて横滑り特性直線を算出する（図１４参照）。
Ｓ５４０の後、Ｓ５５０に進む。
【０１１９】
Ｓ５５０において、横滑り特性学習部１１０は、横滑り特性直線に基づいて車両２００のコーナリングパワーとオドメトリ２３０のオフセット誤差とを算出する（図１４参照）。
Ｓ５５０の後、Ｓ５６０に進む。
【０１２０】
Ｓ５６０において、横滑り特性学習部１１０は、ストラップダウン演算部１４２により速度ベクトルに基づいて算出された距離（ＧＰＳ距離）とＳ５１０で算出されたＯＤＯ距離とに基づいて、オドメトリ２３０のスケールファクタ誤差を算出する（図１５参照）。
Ｓ５６０の後、横滑り特性学習処理（Ｓ５００）は終了する。
【０１２１】
図１７は、実施の形態１における速度残差算出処理（Ｓ４１０）を示すフローチャートである。
実施の形態１における速度残差算出処理（Ｓ４１０）の具体例について、図１７に基づいて以下に説明する。
【０１２２】
Ｓ４１１において、速度／加速度計算部１３１は、対象時刻のオドメトリデータに含まれる車速パルスに基づいて速度スカラーを算出する。
速度／加速度計算部１３１は、速度スカラーを微分して加速度スカラーを算出する。
Ｓ４１１の後、Ｓ４１２に進む。
【０１２３】
Ｓ４１２において、速度／加速度予測部１３２は、Ｓ４１１で算出された速度スカラーと横滑り特性学習部１１０により算出されたＯＤＯ誤差推定値と補正計算部１４１により補正された加速度ベクトルとに基づいて、対象時刻における車両２００の速度ベクトルを算出する。
速度／加速度予測部１３２は、Ｓ４１１で算出された加速度スカラーに基づいて対象時刻における車両２００の加速度ベクトルを算出する。
【０１２４】
車両２００の速度ベクトルＶｂの算出式（２）を以下に示す。
式（２）において、「Ｖｏｄｏ」は速度スカラー、「ｏｓ［０］」はピッチ角のオフセット誤差、「ｏｓ［１］」はヨー角のオフセット誤差、「Ｓｓｌｉｐ」はコーナリングパワー、「ａｙ」は加速度ベクトルの左右方向成分値（単位コーナリングフォース）を示す。
【０１２５】
【数２】
【０１２６】
速度／加速度予測部１３２は、式（２）の速度スカラーＶｏｄｏを加速度スカラーに置き換えて加速度ベクトルを算出する。
【０１２７】
Ｓ４１２の後、Ｓ４１３に進む。
【０１２８】
Ｓ４１３において、速度／加速度残差計算部１３３は、Ｓ４１２で算出された速度ベクトルとストラップダウン演算部１４２により算出された対象時刻の速度ベクトルとの差を対象時刻の速度ベクトル残差として算出する。
速度／加速度残差計算部１３３は、Ｓ４１２で算出された加速度ベクトルと補正計算部１４１により補正された対象時刻の加速度ベクトルとの差を対象時刻の加速度ベクトル残差として算出する。Ｓ４１３の後、速度残差算出処理（Ｓ４１０）は終了する。
【０１２９】
カルマンフィルタ１５０は、速度残差算出処理（Ｓ４１０）で算出された速度ベクトル残差と加速度ベクトル残差とを入力として、対象時刻のＩＭＵ誤差推定値と航法誤差推定値とを算出する。
【０１３０】
図１８は、実施の形態１における自己位置標定装置１００のハードウェア資源の一例を示す図である。図１８において、自己位置標定装置１００は、ＣＰＵ９１１（Ｃｅｎｔｒａｌ・Ｐｒｏｃｅｓｓｉｎｇ・Ｕｎｉｔ）（マイクロプロセッサ、マイクロコンピュータともいう）を備えている。ＣＰＵ９１１は、バス９１２を介してＲＯＭ９１３、ＲＡＭ９１４、通信ボード９１５、磁気ディスク装置９２０と接続され、これらのハードウェアデバイスを制御する。
通信ボード９１５は、有線または無線で、ＬＡＮ（Ｌｏｃａｌ Ａｒｅａ Ｎｅｔｗｏｒｋ）、インターネット、電話回線などの通信網に接続している。
磁気ディスク装置９２０には、ＯＳ９２１（オペレーティングシステム）、プログラム群９２３、ファイル群９２４が記憶されている。
プログラム群９２３には、実施の形態において「〜部」として説明する機能を実行するプログラムが含まれる。プログラムは、ＣＰＵ９１１により読み出され実行される。すなわち、プログラムは、「〜部」としてコンピュータを機能させるものであり、また「〜部」の手順や方法をコンピュータに実行させるものである。
ファイル群９２４には、実施の形態において説明する「〜部」で使用される各種データ（入力、出力、判定結果、計算結果、処理結果など）が含まれる。
実施の形態において構成図およびフローチャートに含まれている矢印は主としてデータや信号の入出力を示す。
実施の形態において「〜部」として説明するものは「〜回路」、「〜装置」、「〜機器」であってもよく、また「〜ステップ」、「〜手順」、「〜処理」であってもよい。すなわち、「〜部」として説明するものは、ファームウェア、ソフトウェア、ハードウェアまたはこれらの組み合わせのいずれで実装されても構わない。
【０１３１】
実施の形態１において、例えば、以下のような自己位置標定装置１００および自己位置標定アルゴリズムについて説明した。
ＧＰＳや慣性計測装置（ＩＭＵ）やオドメトリ（ＯＤＯ）のデータを解析して、これらのセンサが搭載されたプラットフォーム（例えば、車両２００）の位置を標定する。
このとき、同一データによる処理を２回行い、２回目に１回目の処理結果（ＩＭＵ・ＯＤＯ誤差推定初期値）を使用する。
これにより、１回のみの処理を行う場合に比べて自己位置標定精度を向上することができる。
【０１３２】
実施の形態２．
実施の形態１は、航法データの収集が全て終了したのちに自己位置を標定する後処理に適した形態である。
実施の形態２では、航法データの収集を行いながら自己位置を標定するリアルタイム処理に適した形態について説明する。
【０１３３】
図１９は、実施の形態２における自己位置標定装置１００の機能構成図である。
実施の形態２における自己位置標定装置１００について、図１９に基づいて以下に説明する。
【０１３４】
航法データ記憶部１９０には、事前に行われた予備計測により収集されたＧＰＳ予備データ群１９４（第１のＧＰＳデータ群）と慣性予備データ群１９６（第１の慣性データ群）とオドメトリ予備データ群１９５とが予め記憶されているものとする。
【０１３５】
また、航法データ記憶部１９０には、現在行われている本計測により収集されたＧＰＳデータと慣性データとオドメトリデータとが随時記憶されるものとする。
【０１３６】
自己位置標定装置１００は、本計測の前に、予備計測により得られたＧＰＳ予備データ群１９４、慣性予備データ群１９６、オドメトリ予備データ群１９５を用いて１回目の航法処理（Ｓ１００）を行う。
自己位置標定装置１００は、１回目の航法処理（Ｓ１００）で算出されたＩＭＵ・ＯＤＯ誤差推定初期値を２回目の航法処理（Ｓ２００）で用いる。
２回目の航法処理（Ｓ２００）において、自己位置標定装置１００は、本計測により得られたＧＰＳデータ群１９１、慣性データ群１９３（第２の慣性データ群）およびオドメトリデータ群１９２を処理して位置、姿勢および速度を標定する。
【０１３７】
予備計測は本計測と同じ地域において本計測の直前に行うのが望ましい。本計測との条件が近いほどＩＭＵ・ＯＤＯ誤差推定初期値の信頼性が高く、標定精度を向上させることができるからである。
【符号の説明】
【０１３８】
１００ 自己位置標定装置、１１０ 横滑り特性学習部、１２０ ＧＰＳ処理部、１２１ 並進系二重位相差計算部、１２２ 並進系二重位相差予測部、１２３ 並進系二重位相差残差計算部、１２４ 姿勢系二重位相差計算部、１２５ 姿勢系二重位相差予測部、１２６ 姿勢系二重位相差残差計算部、１２７ デザイン行列計算部、１３０ ＯＤＯ処理部、１３１ 速度／加速度計算部、１３２ 速度／加速度予測部、１３３ 速度／加速度残差計算部、１４０ ＩＭＵ処理部、１４１ 補正計算部、１４２ ストラップダウン演算部、１５０ カルマンフィルタ、１９０ 航法データ記憶部、１９１ ＧＰＳデータ群、１９２ オドメトリデータ群、１９３ 慣性データ群、１９４ ＧＰＳ予備データ群、１９５ オドメトリ予備データ群、１９６ 慣性予備データ群、２００ 車両、２０１ 天板、２１０ 主局ＧＰＳ、２１１，２１２ 従局ＧＰＳ、２２０ 慣性計測装置、２３０ オドメトリ、２９９ 基準局ＧＰＳ、９１１ ＣＰＵ、９１２ バス、９１３ ＲＯＭ、９１４ ＲＡＭ、９１５ 通信ボード、９２０ 磁気ディスク装置、９２１ ＯＳ、９２３ プログラム群、９２４ ファイル群。
【特許請求の範囲】
【請求項１】
移動体が備えるＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）受信機により異なる時刻に取得された複数のＧＰＳデータと、前記移動体が備える慣性計測装置により異なる時刻に取得された複数の慣性データとを記憶する航法データ記憶部と、
前記航法データ記憶部に記憶された複数のＧＰＳデータと複数の慣性データとに基づいて前記慣性計測装置の計測誤差の推定値として誤差推定初期値を算出する誤差推定部と、
前記複数の慣性データのうち最初に取得された先頭慣性データを前記誤差推定部により算出された誤差推定初期値に基づいて補正する慣性データ補正部と、
前記慣性データ補正部により補正された先頭慣性データに基づいて前記先頭慣性データが取得された先頭時刻における前記移動体の位置として先頭時刻の標定位置を算出する慣性航法部と
を備えたことを特徴とする位置標定装置。
【請求項２】
前記航法データ記憶部は、搬送波位相を含む複数のＧＰＳデータを記憶し、
前記慣性航法部は、前記誤差推定部により誤差推定初期値が算出される前に複数の慣性データのうち最初に取得された先頭慣性データに基づいて前記先頭慣性データが取得された先頭時刻における前記移動体の位置として先頭時刻の参考位置を算出し、前記複数の慣性データのうち前記先頭慣性データを除く残りの慣性データ毎に、前記慣性データ補正部により補正される当該慣性データに基づいて当該慣性データが取得された当該時刻における前記移動体の位置として当該時刻の参考位置を算出し、
前記位置標定装置は、さらに、
前記慣性航法部により算出された当該時刻の参考位置と当該時刻に取得されたＧＰＳデータに含まれる搬送波位相とに基づいて当該時刻における搬送波位相の残差として当該時刻の搬送波位相残差を算出する搬送波位相残差算出部を備え、
前記誤差推定部は、前記搬送波位相残差算出部により算出された当該時刻の搬送波位相残差を用いて当該時刻に対する前記慣性計測装置の計測誤差の推定値として当該時刻の誤差推定値を算出し、前記複数のＧＰＳデータのうち最後に取得された最終ＧＰＳデータの取得時刻の誤差推定値に基づいて前記誤差推定初期値を出力する
ことを特徴とする請求項１記載の位置標定装置。
【請求項３】
前記搬送波位相残差算出部は、前記慣性航法部により算出された先頭時刻の標定位置と先頭時刻に取得された先頭ＧＰＳデータに含まれる搬送波位相とに基づいて先頭時刻の搬送波位相残差を算出し、前記複数のＧＰＳデータのうち前記先頭ＧＰＳデータを除く残りのＧＰＳデータ毎に、前記慣性航法部により算出される当該時刻の標定位置と当該時刻に取得された当該ＧＰＳデータに含まれる搬送波位相とに基づいて当該時刻の搬送波位相残差を算出し、
前記誤差推定部は、前記搬送波位相残差算出部により算出された当該時刻の搬送波位相残差に基づいて当該時刻の誤差推定値を算出し、
前記慣性データ補正部は、当該時刻に取得された当該慣性データの次に取得された次慣性データを前記誤差推定部により算出された当該時刻の誤差推定値に基づいて補正し、
前記慣性航法部は、前記慣性データ補正部により補正された次慣性データに基づいて次慣性データが取得された次時刻の標定位置を算出する
ことを特徴とする請求項２記載の位置標定装置。
【請求項４】
移動体が備えるＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）受信機により第１の時間内の異なる時刻に取得された複数のＧＰＳデータを第１のＧＰＳデータ群として記憶し、前記移動体が備える慣性計測装置により前記第１の時間内の異なる時刻に取得された複数の慣性データを第１の慣性データ群として記憶し、前記慣性計測装置により第２の時間内の異なる時刻に取得された複数の慣性データを第２の慣性データ群として記憶する航法データ記憶部と、
前記航法データ記憶部に記憶された第１のＧＰＳデータ群と第１の慣性データ群とに基づいて前記慣性計測装置の計測誤差の推定値として誤差推定初期値を算出する誤差推定部と、
第２の慣性データ群のうち最初に取得された先頭慣性データを前記誤差推定部により算出された誤差推定初期値に基づいて補正する慣性データ補正部と、
前記慣性データ補正部により補正された先頭慣性データに基づいて前記先頭慣性データが取得された先頭時刻における前記移動体の位置として先頭時刻の標定位置を算出する慣性航法部と
を備えたことを特徴とする位置標定装置。
【請求項５】
移動体が備えるＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）受信機により異なる時刻に取得された複数のＧＰＳデータと、前記移動体が備える慣性計測装置により異なる時刻に取得された複数の慣性データとを記憶する航法データ記憶部を備える位置標定装置の位置標定方法において、
誤差推定部が、前記航法データ記憶部に記憶された複数のＧＰＳデータと複数の慣性データとに基づいて前記慣性計測装置の計測誤差の推定値として誤差推定初期値を算出し、
慣性データ補正部が、前記複数の慣性データのうち最初に取得された先頭慣性データを前記誤差推定部により算出された誤差推定初期値に基づいて補正し、
慣性航法部が、前記慣性データ補正部により補正された先頭慣性データに基づいて前記先頭慣性データが取得された先頭時刻における前記移動体の位置として先頭時刻の標定位置を算出する
ことを特徴とする位置標定装置の位置標定方法。
【請求項６】
移動体が備えるＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）受信機により第１の時間内の異なる時刻に取得された複数のＧＰＳデータを第１のＧＰＳデータ群として記憶し、前記移動体が備える慣性計測装置により前記第１の時間内の異なる時刻に取得された複数の慣性データを第１の慣性データ群として記憶し、前記慣性計測装置により第２の時間内の異なる時刻に取得された複数の慣性データを第２の慣性データ群として記憶する航法データ記憶部を備える位置標定装置の位置標定方法において、
誤差推定部が、前記航法データ記憶部に記憶された第１のＧＰＳデータ群と第１の慣性データ群とに基づいて前記慣性計測装置の計測誤差の推定値として誤差推定初期値を算出し、
慣性データ補正部が、第２の慣性データ群のうち最初に取得された先頭慣性データを前記誤差推定部により算出された誤差推定初期値に基づいて補正し、
慣性航法部が、前記慣性データ補正部により補正された先頭慣性データに基づいて前記先頭慣性データが取得された先頭時刻における前記移動体の位置として先頭時刻の標定位置を算出する
ことを特徴とする位置標定装置の位置標定方法。
【請求項７】
請求項５または請求項６記載の位置標定方法をコンピュータに実行させる位置標定プログラム。
【請求項１】
移動体が備えるＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）受信機により異なる時刻に取得された複数のＧＰＳデータと、前記移動体が備える慣性計測装置により異なる時刻に取得された複数の慣性データとを記憶する航法データ記憶部と、
前記航法データ記憶部に記憶された複数のＧＰＳデータと複数の慣性データとに基づいて前記慣性計測装置の計測誤差の推定値として誤差推定初期値を算出する誤差推定部と、
前記複数の慣性データのうち最初に取得された先頭慣性データを前記誤差推定部により算出された誤差推定初期値に基づいて補正する慣性データ補正部と、
前記慣性データ補正部により補正された先頭慣性データに基づいて前記先頭慣性データが取得された先頭時刻における前記移動体の位置として先頭時刻の標定位置を算出する慣性航法部と
を備えたことを特徴とする位置標定装置。
【請求項２】
前記航法データ記憶部は、搬送波位相を含む複数のＧＰＳデータを記憶し、
前記慣性航法部は、前記誤差推定部により誤差推定初期値が算出される前に複数の慣性データのうち最初に取得された先頭慣性データに基づいて前記先頭慣性データが取得された先頭時刻における前記移動体の位置として先頭時刻の参考位置を算出し、前記複数の慣性データのうち前記先頭慣性データを除く残りの慣性データ毎に、前記慣性データ補正部により補正される当該慣性データに基づいて当該慣性データが取得された当該時刻における前記移動体の位置として当該時刻の参考位置を算出し、
前記位置標定装置は、さらに、
前記慣性航法部により算出された当該時刻の参考位置と当該時刻に取得されたＧＰＳデータに含まれる搬送波位相とに基づいて当該時刻における搬送波位相の残差として当該時刻の搬送波位相残差を算出する搬送波位相残差算出部を備え、
前記誤差推定部は、前記搬送波位相残差算出部により算出された当該時刻の搬送波位相残差を用いて当該時刻に対する前記慣性計測装置の計測誤差の推定値として当該時刻の誤差推定値を算出し、前記複数のＧＰＳデータのうち最後に取得された最終ＧＰＳデータの取得時刻の誤差推定値に基づいて前記誤差推定初期値を出力する
ことを特徴とする請求項１記載の位置標定装置。
【請求項３】
前記搬送波位相残差算出部は、前記慣性航法部により算出された先頭時刻の標定位置と先頭時刻に取得された先頭ＧＰＳデータに含まれる搬送波位相とに基づいて先頭時刻の搬送波位相残差を算出し、前記複数のＧＰＳデータのうち前記先頭ＧＰＳデータを除く残りのＧＰＳデータ毎に、前記慣性航法部により算出される当該時刻の標定位置と当該時刻に取得された当該ＧＰＳデータに含まれる搬送波位相とに基づいて当該時刻の搬送波位相残差を算出し、
前記誤差推定部は、前記搬送波位相残差算出部により算出された当該時刻の搬送波位相残差に基づいて当該時刻の誤差推定値を算出し、
前記慣性データ補正部は、当該時刻に取得された当該慣性データの次に取得された次慣性データを前記誤差推定部により算出された当該時刻の誤差推定値に基づいて補正し、
前記慣性航法部は、前記慣性データ補正部により補正された次慣性データに基づいて次慣性データが取得された次時刻の標定位置を算出する
ことを特徴とする請求項２記載の位置標定装置。
【請求項４】
移動体が備えるＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）受信機により第１の時間内の異なる時刻に取得された複数のＧＰＳデータを第１のＧＰＳデータ群として記憶し、前記移動体が備える慣性計測装置により前記第１の時間内の異なる時刻に取得された複数の慣性データを第１の慣性データ群として記憶し、前記慣性計測装置により第２の時間内の異なる時刻に取得された複数の慣性データを第２の慣性データ群として記憶する航法データ記憶部と、
前記航法データ記憶部に記憶された第１のＧＰＳデータ群と第１の慣性データ群とに基づいて前記慣性計測装置の計測誤差の推定値として誤差推定初期値を算出する誤差推定部と、
第２の慣性データ群のうち最初に取得された先頭慣性データを前記誤差推定部により算出された誤差推定初期値に基づいて補正する慣性データ補正部と、
前記慣性データ補正部により補正された先頭慣性データに基づいて前記先頭慣性データが取得された先頭時刻における前記移動体の位置として先頭時刻の標定位置を算出する慣性航法部と
を備えたことを特徴とする位置標定装置。
【請求項５】
移動体が備えるＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）受信機により異なる時刻に取得された複数のＧＰＳデータと、前記移動体が備える慣性計測装置により異なる時刻に取得された複数の慣性データとを記憶する航法データ記憶部を備える位置標定装置の位置標定方法において、
誤差推定部が、前記航法データ記憶部に記憶された複数のＧＰＳデータと複数の慣性データとに基づいて前記慣性計測装置の計測誤差の推定値として誤差推定初期値を算出し、
慣性データ補正部が、前記複数の慣性データのうち最初に取得された先頭慣性データを前記誤差推定部により算出された誤差推定初期値に基づいて補正し、
慣性航法部が、前記慣性データ補正部により補正された先頭慣性データに基づいて前記先頭慣性データが取得された先頭時刻における前記移動体の位置として先頭時刻の標定位置を算出する
ことを特徴とする位置標定装置の位置標定方法。
【請求項６】
移動体が備えるＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）受信機により第１の時間内の異なる時刻に取得された複数のＧＰＳデータを第１のＧＰＳデータ群として記憶し、前記移動体が備える慣性計測装置により前記第１の時間内の異なる時刻に取得された複数の慣性データを第１の慣性データ群として記憶し、前記慣性計測装置により第２の時間内の異なる時刻に取得された複数の慣性データを第２の慣性データ群として記憶する航法データ記憶部を備える位置標定装置の位置標定方法において、
誤差推定部が、前記航法データ記憶部に記憶された第１のＧＰＳデータ群と第１の慣性データ群とに基づいて前記慣性計測装置の計測誤差の推定値として誤差推定初期値を算出し、
慣性データ補正部が、第２の慣性データ群のうち最初に取得された先頭慣性データを前記誤差推定部により算出された誤差推定初期値に基づいて補正し、
慣性航法部が、前記慣性データ補正部により補正された先頭慣性データに基づいて前記先頭慣性データが取得された先頭時刻における前記移動体の位置として先頭時刻の標定位置を算出する
ことを特徴とする位置標定装置の位置標定方法。
【請求項７】
請求項５または請求項６記載の位置標定方法をコンピュータに実行させる位置標定プログラム。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図１９】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図１９】
【公開番号】特開２０１１−１８５８９９（Ｐ２０１１−１８５８９９Ａ）
【公開日】平成２３年９月２２日（２０１１．９．２２）
【国際特許分類】
【出願番号】特願２０１０−５４３６１（Ｐ２０１０−５４３６１）
【出願日】平成２２年３月１１日（２０１０．３．１１）
【出願人】（０００００６０１３）三菱電機株式会社 (33,312)
【Ｆターム（参考）】
【公開日】平成２３年９月２２日（２０１１．９．２２）
【国際特許分類】
【出願日】平成２２年３月１１日（２０１０．３．１１）
【出願人】（０００００６０１３）三菱電機株式会社 (33,312)
【Ｆターム（参考）】
[ Back to top ]