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