異常値判定装置、測位装置、及びプログラム
【課題】ＧＰＳ情報とＩＮＳ情報とを統合して測位を行う場合に、ＧＰＳ情報の異常値を精度良く判定する。
【解決手段】区間幅決定部２２で、速度情報の累積誤差と擬似距離誤差との差が所定範囲の値となるように、ＩＮＳ情報とＧＰＳ情報とを統合する区間幅を決定する。区間幅内の各時刻で観測された速度情報及び擬似距離を、Ｎ点観測値記憶部２４に記憶する。初期値設定部２６で、Ｎ点観測値記憶部２４に記憶されたＩＮＳ情報に基づいて方位角及び移動ベクトルの初期値を設定し、最適解推定部２８で、観測値（ρ，Δｘ）から推定初期値からの変位で表される状態（ｄｘ，Ｃｂ，ｄθ）を推定する方程式を立て、最小二乗法により推定初期値を更新しながら収束するまで繰り返して状態を推定し、収束したときの状態及び推定初期値から測位解を推定する。異常値判定部３０は、測位解と擬似距離から得られる位置との残差γが閾値を超える擬似距離を異常値と判定する。
【解決手段】区間幅決定部２２で、速度情報の累積誤差と擬似距離誤差との差が所定範囲の値となるように、ＩＮＳ情報とＧＰＳ情報とを統合する区間幅を決定する。区間幅内の各時刻で観測された速度情報及び擬似距離を、Ｎ点観測値記憶部２４に記憶する。初期値設定部２６で、Ｎ点観測値記憶部２４に記憶されたＩＮＳ情報に基づいて方位角及び移動ベクトルの初期値を設定し、最適解推定部２８で、観測値（ρ，Δｘ）から推定初期値からの変位で表される状態（ｄｘ，Ｃｂ，ｄθ）を推定する方程式を立て、最小二乗法により推定初期値を更新しながら収束するまで繰り返して状態を推定し、収束したときの状態及び推定初期値から測位解を推定する。異常値判定部３０は、測位解と擬似距離から得られる位置との残差γが閾値を超える擬似距離を異常値と判定する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、異常値判定装置、測位装置、及びプログラムに関する。
【背景技術】
【０００２】
従来、ＧＰＳ装置からのＧＰＳ速度データ及びＧＰＳ位置データを用いたハイブリッド計算により、姿勢角、速度、方位角、及び位置を算出し、誤差の少ない自立制御を行う自立制御装置が提案されている（例えば、特許文献１参照）。
【０００３】
また、ＧＰＳ受信機から入力された速度及び加速度から算出された姿勢角及び方位角と、ＩＮＳで測定された姿勢角及び方位角とに基づいて、チルトと方位角に関する観測値を算出して、他の位置や速度の観測値と共にカルマンフィルタに入力し、位置や速度と共に、姿勢角や方位角の推定誤差を算出するハイブリッド航法装置が提案されている（例えば、特許文献２参照）。特許文献２のハイブリッド航法装置では、得られた推定誤差を加算器に入力し、姿勢角及び方位角の推定誤差を用いて、ＩＮＳから直接入力された姿勢角及び方位角を補正して出力している。
【０００４】
また、ＧＰＳ装置単体でマルチパスを検出し排除する技術（例えば、非特許文献１参照）や、軌跡及びその絶対方位が既知で正確であると仮定して、疑似距離の外れ値を検出する技術（例えば、非特許文献２参照）が提案されている。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２００４−３５９００２号公報
【特許文献２】特開２００４−２３９６４３号公報
【非特許文献】
【０００６】
【非特許文献１】Position and velocity reliability testing in degraded GPS signal environments Pattern Recognition and Machine Learning
【非特許文献２】Precise Localization using Tightly Coupled Integration based on Trajectory estimated from GPS Doppler
【発明の概要】
【発明が解決しようとする課題】
【０００７】
しかしながら、特許文献１及び２のカルマンフィルタによる統合手法は、マルチパスによる外れ値の影響を受け、推定精度が悪化する、という問題がある。
【０００８】
また、非特許文献１の技術では、都市部などで外れ値の割合が大きくなると、推定に失敗する、という問題がある。
【０００９】
また、非特許文献２の技術では、正確であると仮定した軌跡が間違っている場合には、推定に失敗し、また、軌跡の算出精度以上の推定精度を出せない、という問題がある。
【００１０】
本発明は、上記の問題点を解決するためになされたもので、ＧＰＳ情報とＩＮＳ情報とを統合して測位を行う場合に、ＧＰＳ情報の異常値を精度良く判定することができる異常値判定装置、精度良く判定された異常値を除外して最適な測位解を推定する測位装置、及びこれらのプログラムを提供することを目的とする。
【課題を解決するための手段】
【００１１】
上記目的を達成するために、本発明の異常値判定装置は、移動体に搭載されたＧＰＳ受信機により受信した情報に基づいて得られる擬似距離、及び前記移動体に搭載された慣性航法装置で検出された速度情報に基づいて得られる前記移動体の移動ベクトルを取得する取得手段と、前記速度情報の各時刻における誤差を所定時間にわたって累積した前記移動ベクトルの累積誤差と、前記擬似距離の誤差との差が所定範囲内の値になるように、前記所定時間を決定する決定手段と、前記決定手段により決定された所定時間内の各時刻で取得された前記擬似距離により得られる位置、及び前記移動ベクトルにより得られる位置に基づいて、前記移動体の各時刻における位置を推定する推定手段と、前記推定手段により推定された各時刻における位置と前記擬似距離との残差に基づいて、各時刻における前記擬似距離が異常か否かを判定する判定手段と、を含んで構成されている。
【００１２】
本発明の異常値判定装置によれば、取得手段が、移動体に搭載されたＧＰＳ受信機により受信した情報に基づいて得られる擬似距離、及び移動体に搭載された慣性航法装置で検出された速度情報に基づいて得られる移動体の移動ベクトルを取得する。そして、決定手段が、速度情報の各時刻における誤差を所定時間にわたって累積した移動ベクトルの累積誤差と、擬似距離の誤差との差が所定範囲内の値になるように、所定時間を決定し、推定手段が、決定手段により決定された所定時間内の各時刻で取得された擬似距離により得られる位置、及び移動ベクトルにより得られる位置に基づいて、移動体の各時刻における位置を推定する。そして、判定手段が、推定手段により推定された各時刻における位置と擬似距離との残差に基づいて、各時刻における擬似距離が異常か否かを判定する。
【００１３】
このように、速度情報の累積誤差と擬似距離誤差との差が所定範囲内の値となるように所定時間を決定し、この所定時間内の各時刻における速度情報及び擬似距離に基づいて移動体の位置を推定し、推定した位置と擬似距離との残差に基づいて、擬似距離の異常を判定するため、ＩＮＳ情報の累積誤差がＧＰＳ情報の誤差と釣り合う範囲に収まり、ＧＰＳ情報とＩＮＳ情報とを統合して測位を行う場合に、ＧＰＳ情報の異常値を精度良く判定することができる。
【００１４】
また、前記決定手段は、地域毎に予め定めた擬似距離の誤差を記録したマップと、前記移動体の各時刻における位置とに基づいて、前記擬似距離の誤差を取得することができる。擬似距離誤差は、周辺建物の高さ等に影響されるため、予め周辺建物の高さに基づいた擬似距離を求めて、地域毎にマップとして記録しておくことができる。
【００１５】
また、前記推定手段は、前記移動体の位置を推定する際に、前記決定手段により決定した所定時間内において、最も離れた２つの時刻各々で取得された擬似距離から得られる位置に基づいて、前記移動ベクトルの方位角を推定することができる。上記のように所定時間を決定するため、２つの時刻間の距離を十分確保でき、方位角の推定も精度良く行うことができる。
【００１６】
また、前記決定手段は、前記所定時間内に３時刻以上の観測点が含まれるように、前記所定時間を決定することができる。所定時間が長くなるほど、所定時間内の観測点は多くなり、推定された位置と擬似距離との残差を用いた異常値の判定が行い易くなる。
【００１７】
また、本発明の測位装置は、前記推定手段において、上記異常値判定装置により異常であると判定された擬似距離を排除して、前記移動体の各時刻における位置を推定する。精度良く判定された擬似距離の異常値を排除することで、位置の推定も精度良く行うことができる。
【００１８】
また、本発明の異常値判定プログラムは、コンピュータを、上記異常値判定装置を構成する各手段として機能させるためのプログラムである。
【００１９】
また、本発明の測位プログラムは、コンピュータを、上記測位装置を構成する各手段として機能させるためのプログラムである。
【００２０】
なお、本発明のプログラムを記憶する記憶媒体は、特に限定されず、ハードディスクであってもよいし、ＲＯＭであってもよい。また、ＣＤ−ＲＯＭやＤＶＤディスク、光磁気ディスクやＩＣカードであってもよい。更にまた、該プログラムを、ネットワークに接続されたサーバ等からダウンロードするようにしてもよい。
【発明の効果】
【００２１】
以上説明したように、本発明の異常値判定装置及びプログラムによれば、速度情報の累積誤差と擬似距離誤差との差が所定範囲内の値となるように所定時間を決定し、この所定時間内の各時刻における速度情報及び擬似距離に基づいて移動体の位置を推定し、推定した位置とＧＰＳ情報との残差に基づいて、擬似距離の異常を判定するため、ＩＮＳ情報の累積誤差がＧＰＳ情報の誤差と釣り合う範囲に収まり、ＧＰＳ情報とＩＮＳ情報とを統合して測位を行う場合に、ＧＰＳ情報の異常値を精度良く判定することができる、という効果が得られる。
【００２２】
また、本発明の測位装置及びプログラムによれば、精度良く判定された擬似距離の異常値を排除することで、位置の推定も精度良く行うことができる、という効果が得られる。
【図面の簡単な説明】
【００２３】
【図１】本実施の形態に係る車載測位装置の機能的構成を示すブロック図である。
【図２】区間幅決定の原理を説明するための図である。
【図３】区間幅の決定を説明するための図である。
【図４】区間幅の決定の具体例を説明するための図である。
【図５】ＩＮＳ情報に基づく移動ベクトルを示す図である。
【図６】観測値の観測状況を示すイメージ図である。
【図７】本実施の形態に係る車載測位装置のコンピュータにおける測位処理ルーチンの内容を示すフローチャートである。
【図８】観測値の観測状況の他の例を示すイメージ図である。
【図９】観測値の観測状況の他の例を示すイメージ図である。
【発明を実施するための形態】
【００２４】
以下、図面を参照して本発明の実施の形態を詳細に説明する。なお、車両に搭載されて受信位置（自車両の位置）を測定する車載測位装置に、本発明を適用した場合を例に説明する。
【００２５】
図１に示すように、本実施の形態に係る車載測位装置１０は、自車両の速度等の運動を検出してＩＮＳ情報を出力するＩＮＳ（慣性航法）装置１２と、ＧＰＳ衛星からの電波を受信してＧＰＳ情報を出力するＧＰＳ装置１４と、ＩＮＳ装置１２及びＧＰＳ装置１４からの出力に基づいて、測位推定を実行するコンピュータ１６とを備えている。
【００２６】
ＩＮＳ装置１２は、自車両の速度を検出するセンサを含んで構成されており、ＩＮＳ情報として、自車両の速度情報を出力する。
【００２７】
ＧＰＳ装置１４は、ＧＰＳ情報として、複数のＧＰＳ衛星から受信した受信信号に基づいて算出された疑似距離を出力する。
【００２８】
コンピュータ１６は、ＣＰＵ、後述する測位処理ルーチンを実現するためのプログラムを記憶したＲＯＭ、データを一時的に記憶するＲＡＭ、及びＨＤＤ等の記憶装置を含んで構成されている。
【００２９】
このコンピュータ１６を以下で説明する測位処理ルーチンに従って機能ブロックで表すと、図１に示すように、地域別にＧＰＳ装置１４が出力する疑似距離の誤差を記録したＧＰＳ誤差マップ２０と、ＩＮＳ情報とＧＰＳ情報とを統合して測位解を推定する際の区間幅を決定する区間幅決定部２２と、決定された区間幅内のＮ点のＩＮＳ情報及びＧＰＳ情報（以下、これらを観測値ともいう）を記憶するＮ点観測値記憶部２４と、ＩＮＳ情報に基づいて、区間内の移動ベクトル及び方位角の初期値を設定する初期値設定部２６と、ＩＮＳ情報とＧＰＳ情報とを統合して、最適な測位解を推定する最適解推定部２８と、推定された測位解に基づいて、ＧＰＳ情報が異常か否かを判定する異常値判定部３０と、を含んだ構成で表すことができる。
【００３０】
ＧＰＳ誤差マップ２０は、周辺の建物の高さ等に基づいて、予め求められたＧＰＳの疑似距離誤差を、地域毎に埋め込んだマップである。疑似距離誤差は、ＧＰＳ装置１４による測位の際の測位誤差と同じオーダーの値となる。
【００３１】
区間幅決定部２２は、ＩＮＳ情報とＧＰＳ情報とを統合して測位解を推定する際の区間幅として、ＩＮＳ情報の誤差とＧＰＳ情報の誤差とが対応するように区間幅を決定する。
【００３２】
ここで、区間幅決定の原理について説明する。図２（ａ）に示すように、ＩＮＳ情報により得られる移動ベクトル（軌跡）は、初期値に対する変位の累積で算出されるため、区間幅が長くなるほど、ＩＮＳ装置１２の誤差が累積されて、正しい軌跡からの累積誤差が大きくなる。このように累積誤差が大きくなったＩＮＳ情報による軌跡を用いて、ＧＰＳの観測値が異常か否かを判定すると、適切な判定が行えない場合がある。例えば、同図（ａ）中Ａで示すＧＰＳ情報は、正しい走行軌跡からの残差は小さいものの、ＩＮＳ情報に基づく軌跡との残差は大きいため、異常値であると判定される。一方、図中Ｂで示すＧＰＳ情報は、正しい走行軌跡からの残差は大きいものの、ＩＮＳ情報に基づく軌跡との残差は小さいため、異常値とは判定されない。このように、区間幅が長すぎると、マルチパスの影響によるＧＰＳの異常観測値を排除しきれない。一方、同図（ｂ）に示すように、区間幅内の両端のＧＰＳの観測値に基づいて方位角を推定する場合に、区間幅が狭すぎると、方位角の推定精度が低くなる。そのため、ＩＮＳ情報の累積誤差がＧＰＳ情報の誤差と釣り合う範囲であって、方位角の推定に十分な距離が確保できるように、区間幅を決定することが必要となる。
【００３３】
また、区間幅が狭く、区間幅内のＧＰＳの観測値が２点しかない場合には、θを求める最小限の情報しかないため、ＩＮＳ情報による残差は発生しにくい。一方、区間幅が広く、区間幅内に３点以上のＧＰＳの観測値が存在する場合には、これらの全点にフィットする最適解は存在しないため、最適解とＧＰＳ観測値との間に残差が発生する。すなわち、区間幅内のＧＰＳの観測値が３点以上の場合には、ＧＰＳの観測値の異常を判定し易い。
【００３４】
そこで、区間幅決定部２２は、ＩＮＳ装置１２の計測精度、及びＧＰＳ装置１４の計測精度に基づいて、ＩＮＳ情報の累積誤差とＧＰＳ情報の誤差とが釣り合う区間幅を算出する。具体的には、図３に示すように、装置構成により予め既知のＩＮＳ装置１２の計測精度、及びＩＮＳ装置１２により計測可能な速度履歴から求まる所定距離移動した際のＩＮＳ情報の累積誤差と、ＧＰＳ誤差マップ２０に記憶されたその地域での疑似距離誤差とが一致するように定めた時の所定距離（または、この所定距離移動するのに要する所定時間）を区間幅とすることができる。なお、ＩＮＳ情報の累積誤差と擬似距離誤差とが一致するようにとは、完全一致する場合に限定されず、ＩＮＳ情報の累積誤差と擬似距離誤差との差が所定範囲内の値となればよい。
【００３５】
例えば、図４に示すように、Ｏ点からＡ点までの距離をＬ、Ａ点での疑似距離誤差をΔρｍ、ＩＮＳ装置１２による速度計測誤差をΔｖｍ／ｓとし、ＯＡ間を自車両がＶｍ／ｓで等速運動していた場合、ＯＡ間を移動するのに要する時間ｔは下記（１）式で求まる。
【００３６】
ｔ＝Ｌ／Ｖ ・・・（１）
この時間ｔにおけるＩＮＳ装置１２により検出される速度の累積誤差は、Δｖ×ｔとなり、この速度の累積誤差と、Ａ点での疑似距離誤差との一致は、下記（２）式で表わされる。
【００３７】
Δｖ×ｔ＝Δρ ・・・（２）
そして、（１）式及び（２）式から、下記（３）式のように、Ｌが求まり、これを区間幅とすることができる。
【００３８】
Ｌ＝ｔ×Ｖ＝Δρ／Δｖ×Ｖ ・・・（３）
Ｎ点観測値記憶部２４には、区間幅決定部２２で決定された区間幅内の各時刻におけるＮ点の観測値が記憶される。
【００３９】
初期値設定部２６は、方位角の初期値θを設定し、Ｎ点観測値記憶部２４に記憶されたＩＮＳ情報に基づいて、区間幅内における各時刻間の移動ベクトルの初期値を算出する。例えば、図５に示すように、区間幅Ｎにおける観測値が時刻ｔ１〜ｔ４の４点の場合には、方位角の初期値θと、時刻ｔ１で観測された速度から、時刻ｔ１〜ｔ２間の移動ベクトルの初期値Δｘ１２を算出する。同様にΔｘ２３及びΔｘ３４を算出する。
【００４０】
最適解推定部２８は、ＧＰＳ単独測位等で、区間幅内の各時刻の位置ｘの推定初期値を設定し、衛星の方角を求め、デザイン行列Ｈを生成し、観測値（ρｔ，Δｘｔ）から、状態（ｄｘｔ，Ｃｂｔ，ｄθ）を推定する方程式を立てる。なお、ρは擬似距離、Ｃｂはクロックバイアス誤差、ｄｘは初期値ｘからの変位、ｄθは初期値θからの変位である。また、Δｘｔは、時刻ｔ〜ｔ＋１間の移動ベクトルである。
【００４１】
具体的には、図６に示すような観測状況において、まず、観測値、状態、推定初期値を下記のようにおく。
【００４２】
【数１】
【００４３】
ここで、Ｅは東西方向、Ｎは南北方向、Ｕは高さを示し、ある固定基準座標ｘｂａｓｅからの相対座標系を表す。従って、（ｘＥ，ｘＮ，ｘＵ）は、ｘｂａｓｅから東にｘＥ、北にｘＮ、上にｘＵ移動した点を表す。
【００４４】
また、初期値と真値とのずれは、下記（４）式のように表わされる。
【００４５】
【数２】
【００４６】
また、ＧＰＳ情報に基づく観測方程式は、下記（５）式となる。
【００４７】
【数３】
【００４８】
また、ＩＮＳ情報に基づく観測方程式は、下記（６）式となる。
【００４９】
【数４】
【００５０】
そして、（５）式及び（６）式を、下記（７）式に示すように、行列式に書き下す。
【００５１】
【数５】
【００５２】
次に、（７）式を下記（８）式のように変形し、最小二乗法により状態を求める。
【００５３】
【数６】
【００５４】
ここで、Ｑは疑似距離誤差及びＩＮＳ情報の誤差から求めた観測値の共分散行列である。推定初期値ｘｔａｓｍ及びθａｓｍを更新し、収束するまで上記（７）式の方程式を立てる処理から繰り返す。収束した時点での状態及び推定初期値に基づいて、測位解を推定する。区間幅が十分な長さの距離を確保できていれば、区間幅に比べてＧＰＳの擬似距離誤差は無視できる大きさになるため、精度良くθを推定することができる。
【００５５】
異常値判定部３０は、区間幅内の時刻ｔにおける観測値と、最適解推定部２８により求めた時刻ｔにおける状態とに基づいて、下記（９）式により、時刻ｔにおけるＧＰＳ情報の残差γｔを算出する。算出した残差γｔと予め定めた閾値γｔｈとを比較することにより、時刻ｔにおけるＧＰＳ情報が異常値か否かを判定する。
【００５６】
【数７】
【００５７】
また異常値判定部３０は、異常と判定された観測値の情報を最適解推定部２８に出力し、最適解推定部２８では、異常と判定された観測値を排除して、再度測位解を推定して、最終的な最適測位解として出力する。
【００５８】
次に、図７を参照して、本実施の形態の車載測位装置１０において実行される測位処理ルーチンについて説明する。
【００５９】
ステップ１００で、時刻ｔ０における自車両の位置に対応する擬似距離誤差を、ＧＰＳ誤差マップ２０から取得する。時刻ｔ０における自車両の位置は、前ルーチンにより推定された位置を用いることができる。例えば、図４の例で、Ｏ点からＡ点までの距離を区間幅Ｌ、ＧＰＳ誤差マップ２０から取得した疑似距離誤差ａが５ｍ、ＩＮＳ装置１２による速度計測誤差Δｖが０．０８ｍ／ｓで、ＯＡ間を自車両が８ｍ／ｓ（Ｖ）で等速運動していた場合、上記（３）式から、区間幅Ｌが５００ｍと求まる。なお、ＧＰＳ誤差マップ２０から取得した擬似距離誤差は、Ｏ点における擬似距離誤差であるが、ここでは、Ａ点における擬似距離誤差も同様であるとみなして処理している。
【００６０】
次に、ステップ１０２で、上記ステップ１００で決定された区間幅内の各時刻におけるＮ点の観測値を、Ｎ点観測値記憶部２４に記憶する。例えば、区間幅Ｌ内で、時刻ｔ１〜ｔ４の４点で観測が行われていた場合には、各時刻で検出された４つの速度情報、及び各時刻で取得された４つの擬似距離が記憶される。なお、区間幅決定の起点となった時刻ｔ０の観測値は含まれない。
【００６１】
次に、ステップ１０４で、方位角の初期値θを設定し、Ｎ点観測値記憶部２４に記憶されたＩＮＳ情報に基づいて、区間幅内における各時刻間の移動ベクトルの初期値を算出する。
【００６２】
次に、ステップ１０６で、ＧＰＳ単独測位等で、区間幅内の各時刻の位置ｘの推定初期値を設定し、衛星の方角を求め、デザイン行列Ｈを生成し、観測値ｙｔ＝（ρｔ，Δｘｔ）から、状態（ｄｘｔ，Ｃｂｔ，ｄθ）を推定する方程式（（７）式）を立てる。次に、ステップ１０８で、（７）式を（８）式のように変形し、最小二乗法により状態を求める。
【００６３】
次に、ステップ１１０で、上記ステップ１０８で求めた状態が収束したか否かを判定する。状態が収束していない場合には、ステップ１１２へ移行し、推定初期値ｘｔａｓｍ及びθａｓｍ、並びにＣｂを更新して、ステップ１０６へ戻る。状態が収束した場合には、ステップ１１４へ移行し、ＧＰＳ情報が異常値か否かを判定済みかを判定する。異常値を判定済みの場合には、ステップ１２２へ移行し、まだ、異常値の判定を行っていない場合には、ステップ１１６へ移行する。
【００６４】
ステップ１１６では、上記ステップ１０８で求めた状態を用いて、（９）式に従って、時刻ｔにおけるＧＰＳ情報の残差γｔを算出する。そして、区間幅内の各時刻におけるＧＰＳ情報の残差γｔを、予め定めた閾値γｔｈと比較し、γｔ＞γｔｈとなるＧＰＳ情報を異常であると判定する。
【００６５】
次に、ステップ１１８で、上記ステップ１１６の判定で、区間幅内のＧＰＳ情報のうち、異常であると判定されたものが存在するか否かを判定する。異常であると判定されたＧＰＳ情報が存在する場合には、ステップ１２０へ移行して、異常であると判定されたＧＰＳ情報を排除して、ステップ１０６へ戻る。異常であると判定されたＧＰＳ情報が存在しない場合には、ステップ１２２へ移行する。
【００６６】
ステップ１２２では、上記ステップ１０８で求めた状態、及び上記ステップ１１２で設定した推定初期値に基づいて、最適測位解を推定して出力し、処理を終了する。
【００６７】
以上説明したように、本実施の形態の車載測位装置によれば、ＩＮＳ情報の累積誤差とＧＰＳ情報の誤差との差が所定範囲内の値となるように区間幅を決定し、この区間幅内のＩＮＳ情報とＧＰＳ情報とを統合して測位解を推定し、この測位解とＧＰＳ情報との残差で異常値を判定するため、ＩＮＳ情報の累積誤差がＧＰＳ情報の誤差と釣り合う範囲に収まり、マルチパスによるＧＰＳ情報の異常値を精度良く判定することができる。また、区間幅として十分な距離が確保できるため、方位角の推定も精度良く行うことができる。
【００６８】
また、このように精度良く判定された異常値を排除して、再度最適測位解を推定するため、精度良く測位解を推定することができる。
【００６９】
なお、上記実施の形態では、図６に示すように、単独測位により得られるＧＰＳ情報を用いる場合について説明したが、この場合に限定されない。
【００７０】
例えば、図８に示すような基準局及び基準衛星を用いた観測状況において、各時刻ｔの擬似距離２重差をＧＰＳ情報として用いてもよい。この場合、観測値及び状態は下記の通りとなる。なお、推定初期値は、上記実施の形態の場合と同様である。
【００７１】
【数８】
【００７２】
なお、ρｒは基準衛星による擬似距離であり、基準衛星は、電波を受信したＧＰＳ衛星のうち、仰角が最も高いＧＰＳ衛星とすることができる。
【００７３】
また、ＧＰＳ情報に基づく観測方程式は、下記（１０）式となる。なお、初期値と真値とのずれは、上記（４）式と同様であり、ＩＮＳ情報に基づく観測方程式は、上記（６）式と同様である。
【００７４】
【数９】
【００７５】
そして、（１０）式及び（６）式を、下記（１１）式に示すように、行列式に書き下すことで、観測値から状態を求める方程式が得られる。
【００７６】
【数１０】
【００７７】
次に、（１１）式を下記（１２）式のように変形し、最小二乗法により状態を求める。
【００７８】
【数１１】
【００７９】
このように、ＧＰＳ情報として、擬似距離の２重差を用いた場合には、最適解を推定する際の変数Ｃｂを削除することができる。また、測位解に対するＧＰＳ情報の残差γｔについても、下記（１３）式に示すように、Ｃｂを削除することができる。
【００８０】
【数１２】
【００８１】
また、例えば、図９に示すような基準局及び基準衛星を用いた観測状況において、各時刻ｔの擬似距離及び位相の２重差をＧＰＳ情報として用いてもよい。この場合、観測値及び状態は下記の通りとなる。なお、推定初期値は、上記実施の形態の場合と同様である。
【００８２】
【数１３】
【００８３】
なお、Ｎは衛星までの距離／波長で現される値である。
【００８４】
また、ＧＰＳ情報に基づく観測方程式は、下記（１４）式となる。なお、初期値と真値とのずれは、上記（４）式と同様であり、ＩＮＳ情報に基づく観測方程式は、上記（６）式と同様である。
【００８５】
【数１４】
【００８６】
そして、（１４）式及び（６）式を、下記（１５）式に示すように、行列式に書き下すことで、観測値から状態を求める方程式が得られる。
【００８７】
【数１５】
【００８８】
次に、（１５）式を下記（１６）式のように変形し、最小二乗法により状態を求める。
【００８９】
【数１６】
【００９０】
また、上記実施の形態では、ＧＰＳ誤差マップを用いて擬似距離誤差を取得する場合について説明したが、レーザレーダ等により周辺建物の高さを検出し、この検出値に基づいて時刻毎に擬似距離誤差を算出するようにしてもよい。
【符号の説明】
【００９１】
１０ 車載測位装置
１２ ＩＮＳ装置
１４ ＧＰＳ装置
１６ コンピュータ
２０ ＧＰＳ誤差マップ
２２ 区間幅決定部
２４ Ｎ点観測値記憶部
２６ 初期値設定部
２８ 最適解推定部
３０ 異常値判定部
【技術分野】
【０００１】
本発明は、異常値判定装置、測位装置、及びプログラムに関する。
【背景技術】
【０００２】
従来、ＧＰＳ装置からのＧＰＳ速度データ及びＧＰＳ位置データを用いたハイブリッド計算により、姿勢角、速度、方位角、及び位置を算出し、誤差の少ない自立制御を行う自立制御装置が提案されている（例えば、特許文献１参照）。
【０００３】
また、ＧＰＳ受信機から入力された速度及び加速度から算出された姿勢角及び方位角と、ＩＮＳで測定された姿勢角及び方位角とに基づいて、チルトと方位角に関する観測値を算出して、他の位置や速度の観測値と共にカルマンフィルタに入力し、位置や速度と共に、姿勢角や方位角の推定誤差を算出するハイブリッド航法装置が提案されている（例えば、特許文献２参照）。特許文献２のハイブリッド航法装置では、得られた推定誤差を加算器に入力し、姿勢角及び方位角の推定誤差を用いて、ＩＮＳから直接入力された姿勢角及び方位角を補正して出力している。
【０００４】
また、ＧＰＳ装置単体でマルチパスを検出し排除する技術（例えば、非特許文献１参照）や、軌跡及びその絶対方位が既知で正確であると仮定して、疑似距離の外れ値を検出する技術（例えば、非特許文献２参照）が提案されている。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２００４−３５９００２号公報
【特許文献２】特開２００４−２３９６４３号公報
【非特許文献】
【０００６】
【非特許文献１】Position and velocity reliability testing in degraded GPS signal environments Pattern Recognition and Machine Learning
【非特許文献２】Precise Localization using Tightly Coupled Integration based on Trajectory estimated from GPS Doppler
【発明の概要】
【発明が解決しようとする課題】
【０００７】
しかしながら、特許文献１及び２のカルマンフィルタによる統合手法は、マルチパスによる外れ値の影響を受け、推定精度が悪化する、という問題がある。
【０００８】
また、非特許文献１の技術では、都市部などで外れ値の割合が大きくなると、推定に失敗する、という問題がある。
【０００９】
また、非特許文献２の技術では、正確であると仮定した軌跡が間違っている場合には、推定に失敗し、また、軌跡の算出精度以上の推定精度を出せない、という問題がある。
【００１０】
本発明は、上記の問題点を解決するためになされたもので、ＧＰＳ情報とＩＮＳ情報とを統合して測位を行う場合に、ＧＰＳ情報の異常値を精度良く判定することができる異常値判定装置、精度良く判定された異常値を除外して最適な測位解を推定する測位装置、及びこれらのプログラムを提供することを目的とする。
【課題を解決するための手段】
【００１１】
上記目的を達成するために、本発明の異常値判定装置は、移動体に搭載されたＧＰＳ受信機により受信した情報に基づいて得られる擬似距離、及び前記移動体に搭載された慣性航法装置で検出された速度情報に基づいて得られる前記移動体の移動ベクトルを取得する取得手段と、前記速度情報の各時刻における誤差を所定時間にわたって累積した前記移動ベクトルの累積誤差と、前記擬似距離の誤差との差が所定範囲内の値になるように、前記所定時間を決定する決定手段と、前記決定手段により決定された所定時間内の各時刻で取得された前記擬似距離により得られる位置、及び前記移動ベクトルにより得られる位置に基づいて、前記移動体の各時刻における位置を推定する推定手段と、前記推定手段により推定された各時刻における位置と前記擬似距離との残差に基づいて、各時刻における前記擬似距離が異常か否かを判定する判定手段と、を含んで構成されている。
【００１２】
本発明の異常値判定装置によれば、取得手段が、移動体に搭載されたＧＰＳ受信機により受信した情報に基づいて得られる擬似距離、及び移動体に搭載された慣性航法装置で検出された速度情報に基づいて得られる移動体の移動ベクトルを取得する。そして、決定手段が、速度情報の各時刻における誤差を所定時間にわたって累積した移動ベクトルの累積誤差と、擬似距離の誤差との差が所定範囲内の値になるように、所定時間を決定し、推定手段が、決定手段により決定された所定時間内の各時刻で取得された擬似距離により得られる位置、及び移動ベクトルにより得られる位置に基づいて、移動体の各時刻における位置を推定する。そして、判定手段が、推定手段により推定された各時刻における位置と擬似距離との残差に基づいて、各時刻における擬似距離が異常か否かを判定する。
【００１３】
このように、速度情報の累積誤差と擬似距離誤差との差が所定範囲内の値となるように所定時間を決定し、この所定時間内の各時刻における速度情報及び擬似距離に基づいて移動体の位置を推定し、推定した位置と擬似距離との残差に基づいて、擬似距離の異常を判定するため、ＩＮＳ情報の累積誤差がＧＰＳ情報の誤差と釣り合う範囲に収まり、ＧＰＳ情報とＩＮＳ情報とを統合して測位を行う場合に、ＧＰＳ情報の異常値を精度良く判定することができる。
【００１４】
また、前記決定手段は、地域毎に予め定めた擬似距離の誤差を記録したマップと、前記移動体の各時刻における位置とに基づいて、前記擬似距離の誤差を取得することができる。擬似距離誤差は、周辺建物の高さ等に影響されるため、予め周辺建物の高さに基づいた擬似距離を求めて、地域毎にマップとして記録しておくことができる。
【００１５】
また、前記推定手段は、前記移動体の位置を推定する際に、前記決定手段により決定した所定時間内において、最も離れた２つの時刻各々で取得された擬似距離から得られる位置に基づいて、前記移動ベクトルの方位角を推定することができる。上記のように所定時間を決定するため、２つの時刻間の距離を十分確保でき、方位角の推定も精度良く行うことができる。
【００１６】
また、前記決定手段は、前記所定時間内に３時刻以上の観測点が含まれるように、前記所定時間を決定することができる。所定時間が長くなるほど、所定時間内の観測点は多くなり、推定された位置と擬似距離との残差を用いた異常値の判定が行い易くなる。
【００１７】
また、本発明の測位装置は、前記推定手段において、上記異常値判定装置により異常であると判定された擬似距離を排除して、前記移動体の各時刻における位置を推定する。精度良く判定された擬似距離の異常値を排除することで、位置の推定も精度良く行うことができる。
【００１８】
また、本発明の異常値判定プログラムは、コンピュータを、上記異常値判定装置を構成する各手段として機能させるためのプログラムである。
【００１９】
また、本発明の測位プログラムは、コンピュータを、上記測位装置を構成する各手段として機能させるためのプログラムである。
【００２０】
なお、本発明のプログラムを記憶する記憶媒体は、特に限定されず、ハードディスクであってもよいし、ＲＯＭであってもよい。また、ＣＤ−ＲＯＭやＤＶＤディスク、光磁気ディスクやＩＣカードであってもよい。更にまた、該プログラムを、ネットワークに接続されたサーバ等からダウンロードするようにしてもよい。
【発明の効果】
【００２１】
以上説明したように、本発明の異常値判定装置及びプログラムによれば、速度情報の累積誤差と擬似距離誤差との差が所定範囲内の値となるように所定時間を決定し、この所定時間内の各時刻における速度情報及び擬似距離に基づいて移動体の位置を推定し、推定した位置とＧＰＳ情報との残差に基づいて、擬似距離の異常を判定するため、ＩＮＳ情報の累積誤差がＧＰＳ情報の誤差と釣り合う範囲に収まり、ＧＰＳ情報とＩＮＳ情報とを統合して測位を行う場合に、ＧＰＳ情報の異常値を精度良く判定することができる、という効果が得られる。
【００２２】
また、本発明の測位装置及びプログラムによれば、精度良く判定された擬似距離の異常値を排除することで、位置の推定も精度良く行うことができる、という効果が得られる。
【図面の簡単な説明】
【００２３】
【図１】本実施の形態に係る車載測位装置の機能的構成を示すブロック図である。
【図２】区間幅決定の原理を説明するための図である。
【図３】区間幅の決定を説明するための図である。
【図４】区間幅の決定の具体例を説明するための図である。
【図５】ＩＮＳ情報に基づく移動ベクトルを示す図である。
【図６】観測値の観測状況を示すイメージ図である。
【図７】本実施の形態に係る車載測位装置のコンピュータにおける測位処理ルーチンの内容を示すフローチャートである。
【図８】観測値の観測状況の他の例を示すイメージ図である。
【図９】観測値の観測状況の他の例を示すイメージ図である。
【発明を実施するための形態】
【００２４】
以下、図面を参照して本発明の実施の形態を詳細に説明する。なお、車両に搭載されて受信位置（自車両の位置）を測定する車載測位装置に、本発明を適用した場合を例に説明する。
【００２５】
図１に示すように、本実施の形態に係る車載測位装置１０は、自車両の速度等の運動を検出してＩＮＳ情報を出力するＩＮＳ（慣性航法）装置１２と、ＧＰＳ衛星からの電波を受信してＧＰＳ情報を出力するＧＰＳ装置１４と、ＩＮＳ装置１２及びＧＰＳ装置１４からの出力に基づいて、測位推定を実行するコンピュータ１６とを備えている。
【００２６】
ＩＮＳ装置１２は、自車両の速度を検出するセンサを含んで構成されており、ＩＮＳ情報として、自車両の速度情報を出力する。
【００２７】
ＧＰＳ装置１４は、ＧＰＳ情報として、複数のＧＰＳ衛星から受信した受信信号に基づいて算出された疑似距離を出力する。
【００２８】
コンピュータ１６は、ＣＰＵ、後述する測位処理ルーチンを実現するためのプログラムを記憶したＲＯＭ、データを一時的に記憶するＲＡＭ、及びＨＤＤ等の記憶装置を含んで構成されている。
【００２９】
このコンピュータ１６を以下で説明する測位処理ルーチンに従って機能ブロックで表すと、図１に示すように、地域別にＧＰＳ装置１４が出力する疑似距離の誤差を記録したＧＰＳ誤差マップ２０と、ＩＮＳ情報とＧＰＳ情報とを統合して測位解を推定する際の区間幅を決定する区間幅決定部２２と、決定された区間幅内のＮ点のＩＮＳ情報及びＧＰＳ情報（以下、これらを観測値ともいう）を記憶するＮ点観測値記憶部２４と、ＩＮＳ情報に基づいて、区間内の移動ベクトル及び方位角の初期値を設定する初期値設定部２６と、ＩＮＳ情報とＧＰＳ情報とを統合して、最適な測位解を推定する最適解推定部２８と、推定された測位解に基づいて、ＧＰＳ情報が異常か否かを判定する異常値判定部３０と、を含んだ構成で表すことができる。
【００３０】
ＧＰＳ誤差マップ２０は、周辺の建物の高さ等に基づいて、予め求められたＧＰＳの疑似距離誤差を、地域毎に埋め込んだマップである。疑似距離誤差は、ＧＰＳ装置１４による測位の際の測位誤差と同じオーダーの値となる。
【００３１】
区間幅決定部２２は、ＩＮＳ情報とＧＰＳ情報とを統合して測位解を推定する際の区間幅として、ＩＮＳ情報の誤差とＧＰＳ情報の誤差とが対応するように区間幅を決定する。
【００３２】
ここで、区間幅決定の原理について説明する。図２（ａ）に示すように、ＩＮＳ情報により得られる移動ベクトル（軌跡）は、初期値に対する変位の累積で算出されるため、区間幅が長くなるほど、ＩＮＳ装置１２の誤差が累積されて、正しい軌跡からの累積誤差が大きくなる。このように累積誤差が大きくなったＩＮＳ情報による軌跡を用いて、ＧＰＳの観測値が異常か否かを判定すると、適切な判定が行えない場合がある。例えば、同図（ａ）中Ａで示すＧＰＳ情報は、正しい走行軌跡からの残差は小さいものの、ＩＮＳ情報に基づく軌跡との残差は大きいため、異常値であると判定される。一方、図中Ｂで示すＧＰＳ情報は、正しい走行軌跡からの残差は大きいものの、ＩＮＳ情報に基づく軌跡との残差は小さいため、異常値とは判定されない。このように、区間幅が長すぎると、マルチパスの影響によるＧＰＳの異常観測値を排除しきれない。一方、同図（ｂ）に示すように、区間幅内の両端のＧＰＳの観測値に基づいて方位角を推定する場合に、区間幅が狭すぎると、方位角の推定精度が低くなる。そのため、ＩＮＳ情報の累積誤差がＧＰＳ情報の誤差と釣り合う範囲であって、方位角の推定に十分な距離が確保できるように、区間幅を決定することが必要となる。
【００３３】
また、区間幅が狭く、区間幅内のＧＰＳの観測値が２点しかない場合には、θを求める最小限の情報しかないため、ＩＮＳ情報による残差は発生しにくい。一方、区間幅が広く、区間幅内に３点以上のＧＰＳの観測値が存在する場合には、これらの全点にフィットする最適解は存在しないため、最適解とＧＰＳ観測値との間に残差が発生する。すなわち、区間幅内のＧＰＳの観測値が３点以上の場合には、ＧＰＳの観測値の異常を判定し易い。
【００３４】
そこで、区間幅決定部２２は、ＩＮＳ装置１２の計測精度、及びＧＰＳ装置１４の計測精度に基づいて、ＩＮＳ情報の累積誤差とＧＰＳ情報の誤差とが釣り合う区間幅を算出する。具体的には、図３に示すように、装置構成により予め既知のＩＮＳ装置１２の計測精度、及びＩＮＳ装置１２により計測可能な速度履歴から求まる所定距離移動した際のＩＮＳ情報の累積誤差と、ＧＰＳ誤差マップ２０に記憶されたその地域での疑似距離誤差とが一致するように定めた時の所定距離（または、この所定距離移動するのに要する所定時間）を区間幅とすることができる。なお、ＩＮＳ情報の累積誤差と擬似距離誤差とが一致するようにとは、完全一致する場合に限定されず、ＩＮＳ情報の累積誤差と擬似距離誤差との差が所定範囲内の値となればよい。
【００３５】
例えば、図４に示すように、Ｏ点からＡ点までの距離をＬ、Ａ点での疑似距離誤差をΔρｍ、ＩＮＳ装置１２による速度計測誤差をΔｖｍ／ｓとし、ＯＡ間を自車両がＶｍ／ｓで等速運動していた場合、ＯＡ間を移動するのに要する時間ｔは下記（１）式で求まる。
【００３６】
ｔ＝Ｌ／Ｖ ・・・（１）
この時間ｔにおけるＩＮＳ装置１２により検出される速度の累積誤差は、Δｖ×ｔとなり、この速度の累積誤差と、Ａ点での疑似距離誤差との一致は、下記（２）式で表わされる。
【００３７】
Δｖ×ｔ＝Δρ ・・・（２）
そして、（１）式及び（２）式から、下記（３）式のように、Ｌが求まり、これを区間幅とすることができる。
【００３８】
Ｌ＝ｔ×Ｖ＝Δρ／Δｖ×Ｖ ・・・（３）
Ｎ点観測値記憶部２４には、区間幅決定部２２で決定された区間幅内の各時刻におけるＮ点の観測値が記憶される。
【００３９】
初期値設定部２６は、方位角の初期値θを設定し、Ｎ点観測値記憶部２４に記憶されたＩＮＳ情報に基づいて、区間幅内における各時刻間の移動ベクトルの初期値を算出する。例えば、図５に示すように、区間幅Ｎにおける観測値が時刻ｔ１〜ｔ４の４点の場合には、方位角の初期値θと、時刻ｔ１で観測された速度から、時刻ｔ１〜ｔ２間の移動ベクトルの初期値Δｘ１２を算出する。同様にΔｘ２３及びΔｘ３４を算出する。
【００４０】
最適解推定部２８は、ＧＰＳ単独測位等で、区間幅内の各時刻の位置ｘの推定初期値を設定し、衛星の方角を求め、デザイン行列Ｈを生成し、観測値（ρｔ，Δｘｔ）から、状態（ｄｘｔ，Ｃｂｔ，ｄθ）を推定する方程式を立てる。なお、ρは擬似距離、Ｃｂはクロックバイアス誤差、ｄｘは初期値ｘからの変位、ｄθは初期値θからの変位である。また、Δｘｔは、時刻ｔ〜ｔ＋１間の移動ベクトルである。
【００４１】
具体的には、図６に示すような観測状況において、まず、観測値、状態、推定初期値を下記のようにおく。
【００４２】
【数１】
【００４３】
ここで、Ｅは東西方向、Ｎは南北方向、Ｕは高さを示し、ある固定基準座標ｘｂａｓｅからの相対座標系を表す。従って、（ｘＥ，ｘＮ，ｘＵ）は、ｘｂａｓｅから東にｘＥ、北にｘＮ、上にｘＵ移動した点を表す。
【００４４】
また、初期値と真値とのずれは、下記（４）式のように表わされる。
【００４５】
【数２】
【００４６】
また、ＧＰＳ情報に基づく観測方程式は、下記（５）式となる。
【００４７】
【数３】
【００４８】
また、ＩＮＳ情報に基づく観測方程式は、下記（６）式となる。
【００４９】
【数４】
【００５０】
そして、（５）式及び（６）式を、下記（７）式に示すように、行列式に書き下す。
【００５１】
【数５】
【００５２】
次に、（７）式を下記（８）式のように変形し、最小二乗法により状態を求める。
【００５３】
【数６】
【００５４】
ここで、Ｑは疑似距離誤差及びＩＮＳ情報の誤差から求めた観測値の共分散行列である。推定初期値ｘｔａｓｍ及びθａｓｍを更新し、収束するまで上記（７）式の方程式を立てる処理から繰り返す。収束した時点での状態及び推定初期値に基づいて、測位解を推定する。区間幅が十分な長さの距離を確保できていれば、区間幅に比べてＧＰＳの擬似距離誤差は無視できる大きさになるため、精度良くθを推定することができる。
【００５５】
異常値判定部３０は、区間幅内の時刻ｔにおける観測値と、最適解推定部２８により求めた時刻ｔにおける状態とに基づいて、下記（９）式により、時刻ｔにおけるＧＰＳ情報の残差γｔを算出する。算出した残差γｔと予め定めた閾値γｔｈとを比較することにより、時刻ｔにおけるＧＰＳ情報が異常値か否かを判定する。
【００５６】
【数７】
【００５７】
また異常値判定部３０は、異常と判定された観測値の情報を最適解推定部２８に出力し、最適解推定部２８では、異常と判定された観測値を排除して、再度測位解を推定して、最終的な最適測位解として出力する。
【００５８】
次に、図７を参照して、本実施の形態の車載測位装置１０において実行される測位処理ルーチンについて説明する。
【００５９】
ステップ１００で、時刻ｔ０における自車両の位置に対応する擬似距離誤差を、ＧＰＳ誤差マップ２０から取得する。時刻ｔ０における自車両の位置は、前ルーチンにより推定された位置を用いることができる。例えば、図４の例で、Ｏ点からＡ点までの距離を区間幅Ｌ、ＧＰＳ誤差マップ２０から取得した疑似距離誤差ａが５ｍ、ＩＮＳ装置１２による速度計測誤差Δｖが０．０８ｍ／ｓで、ＯＡ間を自車両が８ｍ／ｓ（Ｖ）で等速運動していた場合、上記（３）式から、区間幅Ｌが５００ｍと求まる。なお、ＧＰＳ誤差マップ２０から取得した擬似距離誤差は、Ｏ点における擬似距離誤差であるが、ここでは、Ａ点における擬似距離誤差も同様であるとみなして処理している。
【００６０】
次に、ステップ１０２で、上記ステップ１００で決定された区間幅内の各時刻におけるＮ点の観測値を、Ｎ点観測値記憶部２４に記憶する。例えば、区間幅Ｌ内で、時刻ｔ１〜ｔ４の４点で観測が行われていた場合には、各時刻で検出された４つの速度情報、及び各時刻で取得された４つの擬似距離が記憶される。なお、区間幅決定の起点となった時刻ｔ０の観測値は含まれない。
【００６１】
次に、ステップ１０４で、方位角の初期値θを設定し、Ｎ点観測値記憶部２４に記憶されたＩＮＳ情報に基づいて、区間幅内における各時刻間の移動ベクトルの初期値を算出する。
【００６２】
次に、ステップ１０６で、ＧＰＳ単独測位等で、区間幅内の各時刻の位置ｘの推定初期値を設定し、衛星の方角を求め、デザイン行列Ｈを生成し、観測値ｙｔ＝（ρｔ，Δｘｔ）から、状態（ｄｘｔ，Ｃｂｔ，ｄθ）を推定する方程式（（７）式）を立てる。次に、ステップ１０８で、（７）式を（８）式のように変形し、最小二乗法により状態を求める。
【００６３】
次に、ステップ１１０で、上記ステップ１０８で求めた状態が収束したか否かを判定する。状態が収束していない場合には、ステップ１１２へ移行し、推定初期値ｘｔａｓｍ及びθａｓｍ、並びにＣｂを更新して、ステップ１０６へ戻る。状態が収束した場合には、ステップ１１４へ移行し、ＧＰＳ情報が異常値か否かを判定済みかを判定する。異常値を判定済みの場合には、ステップ１２２へ移行し、まだ、異常値の判定を行っていない場合には、ステップ１１６へ移行する。
【００６４】
ステップ１１６では、上記ステップ１０８で求めた状態を用いて、（９）式に従って、時刻ｔにおけるＧＰＳ情報の残差γｔを算出する。そして、区間幅内の各時刻におけるＧＰＳ情報の残差γｔを、予め定めた閾値γｔｈと比較し、γｔ＞γｔｈとなるＧＰＳ情報を異常であると判定する。
【００６５】
次に、ステップ１１８で、上記ステップ１１６の判定で、区間幅内のＧＰＳ情報のうち、異常であると判定されたものが存在するか否かを判定する。異常であると判定されたＧＰＳ情報が存在する場合には、ステップ１２０へ移行して、異常であると判定されたＧＰＳ情報を排除して、ステップ１０６へ戻る。異常であると判定されたＧＰＳ情報が存在しない場合には、ステップ１２２へ移行する。
【００６６】
ステップ１２２では、上記ステップ１０８で求めた状態、及び上記ステップ１１２で設定した推定初期値に基づいて、最適測位解を推定して出力し、処理を終了する。
【００６７】
以上説明したように、本実施の形態の車載測位装置によれば、ＩＮＳ情報の累積誤差とＧＰＳ情報の誤差との差が所定範囲内の値となるように区間幅を決定し、この区間幅内のＩＮＳ情報とＧＰＳ情報とを統合して測位解を推定し、この測位解とＧＰＳ情報との残差で異常値を判定するため、ＩＮＳ情報の累積誤差がＧＰＳ情報の誤差と釣り合う範囲に収まり、マルチパスによるＧＰＳ情報の異常値を精度良く判定することができる。また、区間幅として十分な距離が確保できるため、方位角の推定も精度良く行うことができる。
【００６８】
また、このように精度良く判定された異常値を排除して、再度最適測位解を推定するため、精度良く測位解を推定することができる。
【００６９】
なお、上記実施の形態では、図６に示すように、単独測位により得られるＧＰＳ情報を用いる場合について説明したが、この場合に限定されない。
【００７０】
例えば、図８に示すような基準局及び基準衛星を用いた観測状況において、各時刻ｔの擬似距離２重差をＧＰＳ情報として用いてもよい。この場合、観測値及び状態は下記の通りとなる。なお、推定初期値は、上記実施の形態の場合と同様である。
【００７１】
【数８】
【００７２】
なお、ρｒは基準衛星による擬似距離であり、基準衛星は、電波を受信したＧＰＳ衛星のうち、仰角が最も高いＧＰＳ衛星とすることができる。
【００７３】
また、ＧＰＳ情報に基づく観測方程式は、下記（１０）式となる。なお、初期値と真値とのずれは、上記（４）式と同様であり、ＩＮＳ情報に基づく観測方程式は、上記（６）式と同様である。
【００７４】
【数９】
【００７５】
そして、（１０）式及び（６）式を、下記（１１）式に示すように、行列式に書き下すことで、観測値から状態を求める方程式が得られる。
【００７６】
【数１０】
【００７７】
次に、（１１）式を下記（１２）式のように変形し、最小二乗法により状態を求める。
【００７８】
【数１１】
【００７９】
このように、ＧＰＳ情報として、擬似距離の２重差を用いた場合には、最適解を推定する際の変数Ｃｂを削除することができる。また、測位解に対するＧＰＳ情報の残差γｔについても、下記（１３）式に示すように、Ｃｂを削除することができる。
【００８０】
【数１２】
【００８１】
また、例えば、図９に示すような基準局及び基準衛星を用いた観測状況において、各時刻ｔの擬似距離及び位相の２重差をＧＰＳ情報として用いてもよい。この場合、観測値及び状態は下記の通りとなる。なお、推定初期値は、上記実施の形態の場合と同様である。
【００８２】
【数１３】
【００８３】
なお、Ｎは衛星までの距離／波長で現される値である。
【００８４】
また、ＧＰＳ情報に基づく観測方程式は、下記（１４）式となる。なお、初期値と真値とのずれは、上記（４）式と同様であり、ＩＮＳ情報に基づく観測方程式は、上記（６）式と同様である。
【００８５】
【数１４】
【００８６】
そして、（１４）式及び（６）式を、下記（１５）式に示すように、行列式に書き下すことで、観測値から状態を求める方程式が得られる。
【００８７】
【数１５】
【００８８】
次に、（１５）式を下記（１６）式のように変形し、最小二乗法により状態を求める。
【００８９】
【数１６】
【００９０】
また、上記実施の形態では、ＧＰＳ誤差マップを用いて擬似距離誤差を取得する場合について説明したが、レーザレーダ等により周辺建物の高さを検出し、この検出値に基づいて時刻毎に擬似距離誤差を算出するようにしてもよい。
【符号の説明】
【００９１】
１０ 車載測位装置
１２ ＩＮＳ装置
１４ ＧＰＳ装置
１６ コンピュータ
２０ ＧＰＳ誤差マップ
２２ 区間幅決定部
２４ Ｎ点観測値記憶部
２６ 初期値設定部
２８ 最適解推定部
３０ 異常値判定部
【特許請求の範囲】
【請求項１】
移動体に搭載されたＧＰＳ受信機により受信した情報に基づいて得られる擬似距離、及び前記移動体に搭載された慣性航法装置で検出された速度情報に基づいて得られる前記移動体の移動ベクトルを取得する取得手段と、
前記速度情報の各時刻における誤差を所定時間にわたって累積した前記移動ベクトルの累積誤差と、前記擬似距離の誤差との差が所定範囲内の値になるように、前記所定時間を決定する決定手段と、
前記決定手段により決定された所定時間内の各時刻で取得された前記擬似距離により得られる位置、及び前記移動ベクトルにより得られる位置に基づいて、前記移動体の各時刻における位置を推定する推定手段と、
前記推定手段により推定された各時刻における位置と前記擬似距離との残差に基づいて、各時刻における前記擬似距離が異常か否かを判定する判定手段と、
を含む異常値判定装置。
【請求項２】
前記決定手段は、地域毎に予め定めた擬似距離の誤差を記録したマップと、前記移動体の各時刻における位置とに基づいて、前記擬似距離の誤差を取得する請求項１記載の異常値判定装置。
【請求項３】
前記推定手段は、前記移動体の位置を推定する際に、前記決定手段により決定した所定時間内において、最も離れた２つの時刻各々で取得された擬似距離から得られる位置に基づいて、前記移動ベクトルの方位角を推定する請求項１または請求項２記載の異常値判定装置。
【請求項４】
前記決定手段は、前記所定時間内に３時刻以上の観測点が含まれるように、前記所定時間を決定する請求項１〜請求項３のいずれか１項記載の異常値判定装置。
【請求項５】
前記推定手段は、請求項１〜請求項４のいずれか１項記載の異常値判定装置により異常であると判定された擬似距離を排除して、前記移動体の各時刻における位置を推定する測位装置。
【請求項６】
コンピュータを、請求項１〜請求項４のいずれか１項記載の異常値判定装置を構成する各手段として機能させるための異常値判定プログラム。
【請求項７】
コンピュータを、請求項５記載の測位装置を構成する各手段として機能させるための測位プログラム。
【請求項１】
移動体に搭載されたＧＰＳ受信機により受信した情報に基づいて得られる擬似距離、及び前記移動体に搭載された慣性航法装置で検出された速度情報に基づいて得られる前記移動体の移動ベクトルを取得する取得手段と、
前記速度情報の各時刻における誤差を所定時間にわたって累積した前記移動ベクトルの累積誤差と、前記擬似距離の誤差との差が所定範囲内の値になるように、前記所定時間を決定する決定手段と、
前記決定手段により決定された所定時間内の各時刻で取得された前記擬似距離により得られる位置、及び前記移動ベクトルにより得られる位置に基づいて、前記移動体の各時刻における位置を推定する推定手段と、
前記推定手段により推定された各時刻における位置と前記擬似距離との残差に基づいて、各時刻における前記擬似距離が異常か否かを判定する判定手段と、
を含む異常値判定装置。
【請求項２】
前記決定手段は、地域毎に予め定めた擬似距離の誤差を記録したマップと、前記移動体の各時刻における位置とに基づいて、前記擬似距離の誤差を取得する請求項１記載の異常値判定装置。
【請求項３】
前記推定手段は、前記移動体の位置を推定する際に、前記決定手段により決定した所定時間内において、最も離れた２つの時刻各々で取得された擬似距離から得られる位置に基づいて、前記移動ベクトルの方位角を推定する請求項１または請求項２記載の異常値判定装置。
【請求項４】
前記決定手段は、前記所定時間内に３時刻以上の観測点が含まれるように、前記所定時間を決定する請求項１〜請求項３のいずれか１項記載の異常値判定装置。
【請求項５】
前記推定手段は、請求項１〜請求項４のいずれか１項記載の異常値判定装置により異常であると判定された擬似距離を排除して、前記移動体の各時刻における位置を推定する測位装置。
【請求項６】
コンピュータを、請求項１〜請求項４のいずれか１項記載の異常値判定装置を構成する各手段として機能させるための異常値判定プログラム。
【請求項７】
コンピュータを、請求項５記載の測位装置を構成する各手段として機能させるための測位プログラム。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【公開番号】特開２０１２−２０７９１９（Ｐ２０１２−２０７９１９Ａ）
【公開日】平成２４年１０月２５日（２０１２．１０．２５）
【国際特許分類】
【出願番号】特願２０１１−７１３２３（Ｐ２０１１−７１３２３）
【出願日】平成２３年３月２９日（２０１１．３．２９）
【出願人】（０００００３６０９）株式会社豊田中央研究所 (4,200)
【Ｆターム（参考）】
【公開日】平成２４年１０月２５日（２０１２．１０．２５）
【国際特許分類】
【出願日】平成２３年３月２９日（２０１１．３．２９）
【出願人】（０００００３６０９）株式会社豊田中央研究所 (4,200)
【Ｆターム（参考）】
[ Back to top ]