(57)【要約】【課題】  少ないデータ量及び計算量で精度良く自己位置を推定し得る自己位置推定装置及び方法を提案する。【解決手段】  ＮＳＳセンサの出力に基づき得られる第１の測位結果と、内界センサの出力を積分することにより得られる第２の測位結果とに基づいて第１の測位結果に異常が生じているか否かを判定し、第１の測位結果に異常が生じていないと判定した場合には、第２の測位結果を第１の測位結果に基づいて修正したものを自己の位置と推定し、第１の測位結果に異常が生じていると判定した場合には、第２の測位結果を、第１の測位結果に異常が生じていないと判定した場合よりも少ない修正量で修正したものを自己の位置と推定するようにした。【選択図】        図１
2014228495.tif 000002
【特許請求の範囲】
【請求項１】
測位衛星からの電波に基づいて自己の位置を測位するＮＳＳ（Navigation Satellite System）センサと、
自己の状態を検出する内界センサと、
前記ＮＳＳセンサの出力に基づき得られる第１の測位結果と、前記内界センサの出力を積分することにより得られる第２の測位結果とに基づいて自己の位置を推定する位置推定部と
を備え、
前記位置推定部は、
前記第１及び第２の測位結果に基づいて前記ＮＳＳセンサに測定異常が発生しているか否かを判定し、
前記ＮＳＳセンサに測定異常が発生していないと判定した場合には、前記第２の測位結果を前記第１の測位結果に基づいて修正したものを自己の位置と推定し、前記ＮＳＳセンサに測定異常が発生していると判定した場合には、前記第２の測位結果を、前記ＮＳＳセンサに測定異常が発生していないと判定したときよりも少ない修正量で修正したものを自己の位置と推定する
ことを特徴とする自己位置推定装置。
【請求項２】
前記位置推定部は、
自己の状態を内部状態として管理しており、
管理している最新の内部状態に基づいて、前記内部状態の観測値を取得した時刻における自己の内部状態を予測し、
予測した前記内部状態に基づいて、少なくとも前記第１の測位結果を予測し、
予測した前記第１の測位結果と、現実の前記第１の測位結果との差分に基づいて前記ＮＳＳセンサに測定異常が発生しているか否かを判定する
ことを特徴とする請求項１に記載の自己位置推定装置。
【請求項３】
前記位置推定部は、
予測した前記第１の測位結果及び現実の前記第１の測位結果の差分の絶対値と、当該差分のマハラノビス距離と、前記内部状態の修正量の大きさとの何れかに基づいて、前記ＮＳＳセンサに測定異常が発生しているか否かを判定する
ことを特徴とする請求項２に記載の自己位置推定装置。
【請求項４】
前記位置推定部は、
前記第１の測位結果に異常が生じているか否かを判定した場合に、観測ノイズを大きな値に切り替える第１の修正方法と、修正量を、予測した前記第１の測位結果及び現実の前記第１の測位結果の差分のマハラノビス距離で除算した値に切り替える第２の修正方法と、修正量を、前記修正量の対応する成分の大きさで除算する第３の修正方法とのいずれかの方法により、前記ＮＳＳセンサに測定異常が発生していないと判定したときよりも少ない修正量で前記第２の測位結果を修正する
ことを特徴とする請求項２に記載の自己位置推定装置。
【請求項５】
前記位置推定部は、
前記ＮＳＳセンサに測定異常が発生していると判定した場合には、所定の修正処理を実行した上で、再度、前記ＮＳＳセンサに測定異常が発生しているか否かを判断し、
当該判断により、前記ＮＳＳセンサに測定異常が発生していないと判定した場合には、前記第２の測位結果を前記第１の測位結果に基づいて修正したものを自己の位置と推定し、前記ＮＳＳセンサに測定異常が発生していると判定した場合には、前記第２の測位結果を、前記ＮＳＳセンサに測定異常が発生していないと判定した場合よりも少ない修正量で修正したものを自己の位置と推定する
ことを特徴とする請求項２に記載の自己位置推定装置。
【請求項６】
測位衛星からの電波に基づいて自己の位置を測位するＮＳＳ（Navigation Satellite System）センサの出力と、自己の状態を検出する内界センサの出力とを取得する第１のステップと、
前記ＮＳＳセンサの出力に基づき得られる第１の測位結果と、前記内界センサの出力を積分することにより得られる第２の測位結果とに基づいて自己の位置を推定する第２のステップと
を備え、
前記第２のステップでは、
前記第１及び第２の測位結果に基づいて前記第１の測位結果に異常が生じているか否かを判定し、
前記ＮＳＳセンサに測定異常が発生していないと判定した場合には、前記第２の測位結果を前記第１の測位結果に基づいて修正したものを自己の位置と推定し、前記ＮＳＳセンサに測定異常が発生していると判定した場合には、前記第２の測位結果を、前記ＮＳＳセンサに測定異常が発生していないと判定したときよりも少ない修正量で修正したものを自己の位置と推定する
ことを特徴とする自己位置推定方法。
【請求項７】
自己の状態を内部状態として管理しており、
前記第２のステップでは、
管理している最新の内部状態に基づいて、前記内部状態の観測値を取得した時刻における自己の内部状態を予測し、
予測した前記内部状態に基づいて、少なくとも前記第１の測位結果を予測し、
予測した前記第１の測位結果と、現実の前記第１の測位結果との差分に基づいて前記ＮＳＳセンサに測定異常が発生しているか否かを判定する
ことを特徴とする請求項６に記載の自己位置推定方法。
【請求項８】
前記第２のステップでは、
予測した前記第１の測位結果及び現実の前記第１の測位結果の差分の絶対値と、当該差分のマハラノビス距離と、前記内部状態の修正量の大きさとの何れかに基づいて、前記ＮＳＳセンサに測定異常が発生しているか否かを判定する
ことを特徴とする請求項７に記載の自己位置推定方法。
【請求項９】
前記第２のステップでは、
前記ＮＳＳセンサに測定異常が発生しているか否かを判定した場合に、観測ノイズを大きな値に切り替える第１の修正方法と、修正量を、予測した前記第１の測位結果及び現実の前記第１の測位結果の差分のマハラノビス距離で除算した値に切り替える第２の修正方法と、修正量を、前記修正量の対応する成分の大きさで除算する第３の修正方法とのいずれかの方法により、前記第２の測位結果を、前記ＮＳＳセンサに測定異常が発生していないと判定した場合よりも少ない修正量に修正する
ことを特徴とする請求項７に記載の自己位置推定方法。
【請求項１０】
前記第２のステップでは、
前記ＮＳＳセンサに測定異常が発生していると判定した場合には、所定の修正処理を実行した上で、再度、前記第１の測位結果に異常が生じているか否かを判断し、
当該判断により、前記ＮＳＳセンサに測定異常が発生していないと判定した場合には、前記第２の測位結果を前記第１の測位結果に基づいて修正したものを自己の位置と推定し、前記ＮＳＳセンサに測定異常が発生していると判定した場合には、前記ＮＳＳセンサに測定異常が発生していないと判定した場合よりも少ない修正量で前記第２の測位結果を修正したものを自己の位置と推定する
ことを特徴とする請求項７に記載の自己位置推定方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、自己位置推定装置及び方法に関し、例えばＮＳＳ（Navigation Satellite Systems：航法衛星システム）及びＩＮＳ（Inertial Navigation System：慣性航法システム）を利用した車両の移動軌道を制御する車両制御システムに適用して好適なものである。
【背景技術】
【０００２】
従来、ロボット、車両又は携帯端末などの位置を推定する技術としてＮＳＳがある。ＮＳＳは、複数の測位衛星からの電波を受信することで各測位衛星との距離を求め、求めた各測位衛星の位置に基づいて受信機の位置を推定する方法である。一般的に普及しているＮＳＳとして、アメリカが運用するＧＰＳ（Global Positioning System）がある。
【０００３】
ＮＳＳを利用して受信機の位置を測位する場合、受信機付近の建物、樹木又は地表からの反射波などの影響によりマルチパスが発生し、このマルチパスの影響により衛星との距離を正確に求められないことがある。従来、このようなマルチパスの影響を低減する方法として、以下の２つの技術が提案されている。
【０００４】
第１の方法は、自己位置と、衛星の位置と、全周囲の遮蔽物に関する情報とに基づいて各衛星からの信号が直接波及び反射波のいずれであるかを判別する方法である。この場合における「全周囲の遮蔽物に関する情報」の取得方法としては、地理情報データベースから取得する方法（特許文献１）や、赤外・可視カメラ等により周囲を計測する方法（特許文献２）などが提案されている。
【０００５】
また第２の方法は、ＮＳＳによる測位結果と、ＩＮＳによる位置推定結果とを比較して、差が大きいときにはＮＳＳによる測位結果にマルチパスによる誤差が含まれていると判定し、ＩＮＳによる位置推定結果を採用する方法である。この場合におけるＮＳＳによる測位結果と、ＩＮＳによる位置推定結果とを比較する比較方法として、測位対象（例えば車両）の位置及び姿勢を比較する方法（特許文献３）や、加速度を比較する方法（特許文献４）などが提案されている。
【０００６】
なお、ＩＮＳは、一般的には加速度計の情報を積分して距離を求め、ジャイロ情報を積分して方角を求め、移動距離と方角のベクトルを細分点ごとに合成してゆくことにより、起点からの移動距離及び方角を算出するシステムである。またＩＮＳとして、車輪エンコーダやステア角エンコーダなどの情報を利用して、移動距離及び方角を補正するものもある。ＩＮＳは、測位対象の位置を直接計測しないため、速度、加速度及び角速度の計測誤差が積分され、位置誤差が徐々に大きくなる特徴がある。従って、ＩＮＳを利用する場合には、一般的にＩＮＳによる位置推定結果を定期的にＮＳＳによる測位結果に基づいて補正することが行われる。
【先行技術文献】
【特許文献】
【０００７】
【特許文献１】特許第４４１８３５７号明細書
【特許文献２】特許第４３９１４５８号明細書
【特許文献３】特許第４８０３８６２号明細書
【特許文献４】特許第３３３８３０４号明細書
【発明の概要】
【発明が解決しようとする課題】
【０００８】
ところで、上述の第１の方法は、測位対象が存在する場所の周囲の地理データを必要とし、例えば地理情報データベースを用いる場合に測位対象が進入するエリア全体のデータが必要となり、その分、計算量も多くなる問題がある。また第１の方法において、特許文献２のようにカメラ等で周囲を計測することにより周囲の遮蔽物の情報を取得しようとする場合には、遮蔽物の認識など複雑な処理が必要となったり、逆光などの光学的なノイズの影響を受けやすいという問題もある。
【０００９】
一方、上述の第２の方法によると、大量のデータを処理することなく、マルチパスを検出できるという利点がある。しかしながら、ＩＮＳによる位置の推定では、タイヤのスリップや振動などのノイズの影響により位置誤差が発生する。このためＮＳＳによる測位結果と、ＩＮＳによる位置推定結果とが一致しない場合、ＮＳＳによる測位結果よりもＩＮＳによる位置推定結果の方が、実際の位置に対する誤差が大きい可能性もある。
【００１０】
このような状況において、このためＮＳＳによる測位結果と、ＩＮＳによる位置推定結果とが一致しないときに、自己位置推定処理の処理結果としてＩＮＳによる位置推定結果を採用した場合、ＮＳＳにより正しい測位が行われているものとすると、ＮＳＳによる測位結果と、ＩＮＳによる位置推定結果との間の差が次第に大きくなるため、結果的にこれ以降、常にＩＮＳによる位置推定結果が採用されることとなり、この結果、推定される位置と実際の位置との差が拡大してしまうという問題があった。
【００１１】
本発明は以上の点を考慮してなされたもので、少ないデータ量及び計算量で精度良く自己位置を推定し得る自己位置推定装置及び方法を提案しようとするものである。
【課題を解決するための手段】
【００１２】
かかる課題を解決するため本発明においては、自己位置推定装置において、測位衛星からの電波に基づいて自己の位置を測位するＮＳＳ（Navigation Satellite System）センサと、自己の状態を検出する内界センサと、前記ＮＳＳセンサの出力に基づき得られる第１の測位結果と、前記内界センサの出力を積分することにより得られる第２の測位結果とに基づいて自己の位置を推定する位置推定部とを設け、前記位置推定部が、前記第１及び第２の測位結果に基づいて前記ＮＳＳセンサに測定異常が発生しているか否かを判定し、前記ＮＳＳセンサに測定異常が発生していないと判定した場合には、前記第２の測位結果を前記第１の測位結果に基づいて修正したものを自己の位置と推定し、前記ＮＳＳセンサに測定異常が発生していると判定した場合には、前記第２の測位結果を、前記ＮＳＳセンサに測定異常が発生していないと判定したときよりも少ない修正量で修正したものを自己の位置と推定するようにした。
【００１３】
また本発明においては、自己位置推定方法において、測位衛星からの電波に基づいて自己の位置を測位するＮＳＳ（Navigation Satellite System）センサの出力と、自己の状態を検出する内界センサの出力とを取得する第１のステップと、前記ＮＳＳセンサの出力に基づき得られる第１の測位結果と、前記内界センサの出力を積分することにより得られる第２の測位結果とに基づいて自己の位置を推定する第２のステップとを設け、前記第２のステップでは、前記第１及び第２の測位結果に基づいて前記第１の測位結果に異常が生じているか否かを判定し、前記ＮＳＳセンサに測定異常が発生していないと判定した場合には、前記第２の測位結果を前記第１の測位結果に基づいて修正したものを自己の位置と推定し、前記ＮＳＳセンサに測定異常が発生していると判定した場合には、前記第２の測位結果を、前記ＮＳＳセンサに測定異常が発生していないと判定したときよりも少ない修正量で修正したものを自己の位置と推定するようにした。
【００１４】
かかる本発明の自己位置推定装置及び自己位置推定方法によれば、第２の測位結果が常に第１の測位結果に基づいて修正されるため、最終的に第２の測位結果が第１の測位結果に近づくよう修正される。従って、自己位置の推定結果として第２の測位結果を採用した場合に、その後、第１の測位結果が利用されなくなり、自己位置の推定結果と、現実の位置との間の誤差が拡大するという事態が発生するのを未然かつ有効に防止することができる。
【００１５】
また本発明の自己位置推定装置及び自己位置推定方法によれば、自己が存在する場所の周囲の地理データを必要とせず、少ないデータ量及び計算量で自己位置を推定することができる。
【発明の効果】
【００１６】
本発明によれば、少ないデータ量及び計算量で精度良く自己位置を推定し得る自己位置推定装置及び方法を実現できる。
【図面の簡単な説明】
【００１７】
【図１】第１及び第２の実施の形態による車両制御システムの概略構成を示すブロック図である。
【図２】ＩＮＳ処理の処理手順を示すフローチャートである。
【図３】ＩＮＳ処理における更新処理の処理手順を示すフローチャートである。
【図４】ＮＳＳ処理の処理手順を示すフローチャートである。
【図５】ＮＳＳ処理における更新処理の処理手順を示すフローチャートである。
【図６】第１の異常判定処理の処理手順を示すフローチャートである。
【図７】第２の異常判定処理の処理手順を示すフローチャートである。
【図８】第３の異常判定処理の処理手順を示すフローチャートである。
【図９】通常修正処理の処理手順を示すフローチャートである。
【図１０】第１の異常時修正処理の処理手順を示すフローチャートである。
【図１１】第２の異常時修正処理の処理手順を示すフローチャートである。
【図１２】第３の異常時修正処理の処理手順を示すフローチャートである。
【図１３】第２の実施の形態による更新処理の処理手順を示すフローチャートである。
【図１４】異常判定及び修正処理の処理手順を示すフローチャートである。
【発明を実施するための形態】
【００１８】
以下図面について、本発明の一実施の形態を詳述する。
【００１９】
（１）第１の実施の形態
（１−１）本実施の形態によるシステム構成
図１において、１は全体として本実施の形態による車両制御システムを示す。この車両制御システム１は、当該車両制御システム１が搭載された車両（図示せず）の位置を測位する測位装置２と、測位装置２の測位結果に基づいて車両の移動を制御する制御装置３とから構成される。
【００２０】
測位装置２は、ＮＳＳ及びＩＮＳの双方を利用して車両の位置及び姿勢を推定する自己位置推定機能を有し、ＮＳＳ用のセンサ（以下、これをＮＳＳセンサと呼ぶ）１０としてＧＰＳ／ＧＰＳコンパス１３を備え、ＩＮＳ用のセンサ（以下、これをＩＮＳセンサと呼ぶ）１１として車輪用エンコーダ１４、ジャイロセンサ１５及びステア角エンコーダ１６を備える。また測位装置２は、ＮＳＳセンサ１０及びＩＮＳセンサ１１に加えて、これらＮＳＳセンサ１０及びＩＮＳセンサ１１のセンサ出力に基づいて自己位置を推定する位置推定部１２を備える。
【００２１】
ＧＰＳ／ＧＰＳコンパス１３は、ＧＰＳアンテナを２つ以上用い、位置だけでなく方位も計測するセンサであり、かかる計測により得られた車両の位置及び方位を位置・姿勢情報として位置推定部１２に送出する。
【００２２】
車輪用エンコーダ１４は、ロータリーエンコーダから構成され、車両の車輪の回転角度に応じた出力をエンコーダ情報として位置推定部１２に送出する。またジャイロセンサ１５は、車両の角速度を検出し、検出結果を角速度情報として位置推定部１２に送出する。さらにステア角エンコーダ１６は、車両のステアリングの回転角度（以下、これをステア角と呼ぶ）を検出するエンコーダであり、検出結果をステア角情報として位置推定部１２に送出する。
【００２３】
位置推定部１２は、ＣＰＵ（Central Processing Unit）及びメモリ等の情報処理資源を備えて構成され、車輪用エンコーダ１４から送信されるエンコーダ情報に基づき得られる車両の速度と、ジャイロセンサ１５から送信される角速度情報に基づき得られる車両の角速度と、ステア角エンコーダ１６から送信されるステア角情報に基づき得られる車両の移動軌道の曲率とを積算処理することにより、車両の位置及び姿勢を推定する。また位置推定部１２は、このようにして得られた車両の位置及び姿勢を、ＧＰＳ／ＧＰＳコンパス１３から送信される位置・姿勢情報に基づいて補正し、かくして得られた車両の位置及び姿勢を表す位置・姿勢情報を制御装置３に送出する。
【００２４】
制御装置３は、行動計画部１７、車両制御部１８及びロボット駆動部１９を備えて構成される。本実施の形態においては、これら行動計画部１７、車両制御部１８及びロボット駆動部１９は、制御装置３を構成する図示しないＣＰＵが対応するプログラムを実行することにより具現化されるものとする。ただし、これら行動計画部１７、車両制御部１８及びロボット駆動部１９をそれぞれ別個のハードウェア構成としても良い。
【００２５】
行動計画部１７は、測位装置２の位置推定部１２から与えられる位置・姿勢情報に基づいて予め設定された目標地点に向けた車両の軌道を生成する軌道生成部１７Ａを備えて構成され、軌道生成部１７Ａが生成した軌道に関する情報を軌道情報として車両制御部１８に送出する。
【００２６】
車両制御部１８は、行動計画部１７から与えられる軌道情報に基づいて、車両が軌道生成部１７Ａにより生成された軌道上を移動するようにロボット駆動部を駆動するための指令値（目標値）を算出する指令値算出部１８Ａを備える。そして車両制御部１８は、指令値算出部１８Ａにより算出された指令値をロボット駆動部１９に送出する。
【００２７】
ロボット駆動部１９は、例えばエンジン又はモータ等の車両の駆動源と、車両のステアリングを回転駆動するモータなどを備えて構成される。そしてロボット駆動部１９は、車両制御部１８から与えられる指令値に従った回転量又は回転角でエンジンやモータなどを駆動することにより、当該指令値に従った状態に車両を駆動する。
【００２８】
（１−２）本実施の形態による自己位置推定方式
（１−２−１）概要
次に、測位装置２の位置推定部１２において実行される自己位置推定処理の方式（自己位置推定方式）について説明する。
【００２９】
本実施の形態による自己位置推定方式は、ＮＳＳにより測定した自己の位置及び姿勢と、ＩＮＳにより求めた自己の位置及び姿勢とを比較して、ＮＳＳにマルチパスなどの影響による測定異常が発生しているか否かを判定する点を特徴の１つとしている。また本自己位置推定方式は、かかる判定によりＮＳＳに測定異常が発生していないとの判定結果を得た場合には、ＩＮＳで求めた車両の位置及び姿勢をＮＳＳの測定結果に基づいて修正する一方、測定異常が発生しているとの判定結果を得た場合には、ＩＮＳで求めた位置を、ＮＳＳに測定異常が発生していないとの判定結果を得た場合よりも少ない修正量で修正する点をもう１つの特徴としている。
【００３０】
このような自己位置推定方式を実現するための手段として、本実施の形態においては、自己位置推定処理にカルマンフィルタ理論を導入する。すなわち測位装置２の位置推定部１２は、ＮＳＳセンサ１０及びＩＮＳセンサ１１のセンサ出力に基づき得られる観測値を用いてカルマンフィルタ理論により車両の位置及び姿勢を推定する。
【００３１】
カルマンフィルタ理論では、位置、姿勢、速度及び角速度といった推定したいパラメータを内部状態とし、ＮＳＳセンサ１０及びＩＮＳセンサ１１のセンサ出力に基づくこれら位置、姿勢、速度及び角速度の観測値を用いて内部状態を順次更新することで内部状態を正しい値に収束させていく。
【００３２】
この場合において、カルマンフィルタ理論では、推定値の誤差分散をも併せて算出するため、ＮＳＳによる位置及び姿勢の推定値が誤差範囲に入っているか否かを判定することによって、ＮＳＳに測定異常が発生しているか否かを判定することが可能となる。
【００３３】
またＮＳＳに測定異常が発生している場合には、ＩＮＳで求めた位置を、ＮＳＳに測定異常が発生していないとの判定結果を得た場合よりも少ない修正量で修正することにより、この後、ＮＳＳにより計測した自己の位置及び姿勢と、ＩＮＳにより求めた自己の位置及び姿勢との間の誤差が拡大して、ＮＳＳの計測結果が採用されなくなるという事態が発生することを未然かつ有効に防止することができる。
【００３４】
（１−２−２）カルマンフィルタ理論を利用した自己位置推定処理の内容
次に、カルマンフィルタ理論を利用した本実施の形態による自己位置推定処理の具体的な処理内容について説明する。
【００３５】
図２は、所定の第１の更新周期（例えば0.02秒周期）で位置推定部１２により実行されるＩＮＳ処理の処理内容を示す。位置推定部は、この図２に示すＩＮＳ処理を実行することにより、各ＩＮＳセンサ１１のセンサ出力に基づいて、内部状態を第１の更新周期で更新する。
【００３６】
実際上、位置推定部１２は、図２に示すＩＮＳ処理を開始すると、まず、現在時刻を取得し（ＳＰ１）、この後、車輪用エンコーダ１４、ジャイロセンサ１５及びステア角エンコーダ１６からそれぞれ送信されるエンコーダ情報、角速度情報及びステア角情報をそれぞれ順次取得する（ＳＰ２〜ＳＰ４）。続いて、位置推定部１２は、ステップＳＰ２〜ステップＳＰ４において取得したエンコーダ情報、角速度情報及びステア角情報に基づいて内部状態を更新し（ＳＰ５）、この後、ステップＳＰ１に戻る。位置推定部１２は、以上のＩＮＳ処理を第１の更新周期で繰り返し実行する。
【００３７】
図３は、かかるＩＮＳ処理（図２）のステップＳＰ５において、位置推定部１２により実行される更新処理の具体的な処理内容を示す。
【００３８】
位置推定部１２は、ＩＮＳ処理のステップＳＰ５に進むと、この更新処理を開始し、まず、保持している最新の内部状態に基づいて、各ＩＮＳセンサ１１（車輪用エンコーダ１４、ジャイロセンサ１５及びステア角エンコーダ１６）が対応するセンサ値を取得した時刻（以下、これを観測時刻と呼ぶ）における内部状態を予測する内部状態予測処理を実行する（ＳＰ１０）。
【００３９】
続いて、位置推定部１２は、車輪用エンコーダ１４からのエンコーダ情報に基づき得られる車両の速度と、ジャイロセンサ１５からの角速度情報に基づき得られる車両の角速度と、ステア角エンコーダ１６からのステア角情報に基づき得られる車両の移動軌道の曲率との観測値の予測値をそれぞれ求める観測値予測処理を実行し（ＳＰ１１）、この後、ステップＳＰ１１において求めた観測値の予測値と、実際の観測値とに基づいて内部状態を修正する修正処理を実行する（ＳＰ１２）。そして位置推定部１２は、この後、ＩＮＳ処理に戻る。
【００４０】
一方、図４は、上述のＩＮＳ処理（図２）と並行して所定の第２の更新周期（例えば１秒周期）で位置推定部１２により実行されるＮＳＳ処理の処理内容を示す。位置推定部１２は、この図４に示すＮＳＳ処理を実行することにより、ＮＳＳセンサ１０のセンサ出力に基づいて内部状態を更新する。
【００４１】
実際上、位置推定部１２は、図４に示すＮＳＳ処理を開始すると、まず、現在時刻を取得し（ＳＰ２０）、この後、ＧＰＳ／ＧＰＳコンパス１３から与えられる位置・姿勢情報を取得する（ＳＰ２１）。続いて、位置推定部１２は、ステップＳＰ２１において取得した位置・姿勢情報に含まれる位置情報に基づいて内部状態を更新する更新処理を実行し（ＳＰ２２）、さらに、かかる位置・姿勢情報に含まれる姿勢情報（方位情報）に基づいて内部状態を更新する更新処理を実行した後（ＳＰ２３）、ステップＳＰ２０に戻る。位置推定部１２は、以上のＮＳＳ処理を第２の更新周期で繰り返し実行する。
【００４２】
図５は、かかるＮＳＳ処理（図４）のステップＳＰ２２及びステップＳＰ２３において、位置推定部１２により実行される更新処理の具体的な処理内容を示す。
【００４３】
位置推定部１２は、ＮＳＳ処理のステップＳＰ２２又はステップＳＰ２３に進むと、この更新処理を開始し、まず、保持している最新の内部状態に基づいて、ＧＰＳ／ＧＰＳコンパス１３が当該位置・姿勢を測位した時刻（観測時刻）における内部状態を予測する内部状態予測処理を実行する（ＳＰ３０）。
【００４４】
続いて、位置推定部１２は、ＧＰＳ／ＧＰＳコンパス１３の観測値の予測値及び予測値の誤差範囲を求める観測値予測処理を実行する（ＳＰ３１）。この後、位置推定部１２は、ステップＳＰ３０で求めた車両の位置又は姿勢の観測値の予測値と、実際のＧＰＳ／ＧＰＳコンパス１３により観測された車両の位置又は姿勢の観測値とを比較（ステップＳＰ２２の更新処理では位置、ステップＳＰ２３の更新処理では姿勢を比較）し、これらの差がステップＳＰ３１において求めた予測値の誤差範囲を超えるか否かによってＮＳＳに測定異常が発生しているか否かを判定する異常判定処理を実行する（ＳＰ３２）。
【００４５】
そして位置推定部１２は、かかる異常判定処理の処理結果に基づいて、内部状態を補正する（ＳＰ３３）。具体的に、位置推定部１２は、ステップＳＰ３２の異常判定処理の結果、観測値に異常がないと判定した場合には、ＮＳＳの測定結果を利用して内部状態を修正する一方、観測値に異常があると判定した場合には、観測値に異常がないと判定した場合の修正量よりも少ない修正量で内部状態を修正する。そして位置推定部１２は、この後、ＮＳＳ処理に戻る。
【００４６】
（１−２−３）更新処理の詳細
（１−２−３−１）内部状態及びその分散・共分散
次に、上述した内部状態予測処理、観測値予測処理、異常判定処理及び修正処理の具体的な内容について説明する。なお、以下の説明において、「対象とするセンサ」とは、ＩＮＳ処理時には車輪用エンコーダ１４、ジャイロセンサ１５及びステア角エンコーダ１６を指し、ＮＳＳ処理（図４）のステップＳＰ２２の更新処理ではＧＰＳ／ＧＰＳコンパス１３のうちのＧＰＳセンサを指し、ＮＳＳ処理のステップＳＰ２３の更新処理ではＧＰＳ／ＧＰＳコンパス１３のうちのＧＰＳコンパスを指すものとする。
【００４７】
まず、以下の説明において使用する車両の内部状態及びその分散・共分散について説明する。位置推定部１２は、車両の位置を（ｘ、ｙ）、姿勢をθ、速度をｖｘ、角速度をｗｚとして、これらのパラメータを次式
【数１】
2014228495.tif 000003
で表される内部状態ベクトルＸｔ（以下、これを単に内部状態Ｘｔと呼ぶ）として管理している。
【００４８】
また位置推定部１２は、内部状態Ｘｔの推定値の誤差範囲の分散・共分散を表す行列式として、次式
【数２】
2014228495.tif 000004
で表される分散・共分散行列ＣｏｖＸｔ（以下、これを単に分散・共分散ＣｏｖＸｔと呼ぶ）をメモリ内に保持している。（２）式における行列の対角項は、各パラメータの分散を表し、非対角項は、パラメータ間の共分散を表す。
【００４９】
なお、車両の内部状態Ｘｔ及び分散・共分散ＣｏｖＸｔは、いずれも時間変化する値である。以下、内部状態Ｘｔ及び分散・共分散ＣｏｖＸｔを表す時刻をモデル時刻ｔとし、各パラメータは、モデル時刻がｔの時点の値を示すものとする。
【００５０】
そして位置推定部１２は、ＧＰＳ／ＧＰＳコンパス１３から最初に与えられた位置・姿勢情報に基づいて、内部状態Ｘｔを次式のように初期化する。
【数３】
2014228495.tif 000005
【００５１】
また位置推定部１２は、かかる内部状態Ｘｔの初期化と併せて、内部状態Ｘｔの分散・共分散ＣｏｖＸｔを次式のように初期化する。
【数４】
2014228495.tif 000006
【００５２】
（３）式及び（４）式において、ｔは、ＧＰＳ／ＧＰＳコンパス１３が車両の位置及び姿勢を測定した時刻に設定される。また（４）式において、σinitx〜σinitwzは、ＧＰＳ／ＧＰＳコンパス１３から与えられる位置・姿勢情報の精度などを考慮して適宜決定される定数である。
【００５３】
（１−２−３−２）内部状態予測処理
内部状態予測処理では、対象とするセンサのセンサ出力を取得した時刻（以下、これを観測時刻と呼ぶ）における内部状態Ｘｔ及びその分散・共分散ＣｏｖＸｔを予測する。この予測値は、観測時刻とモデル時刻との差分をΔｔ（観測時刻−モデル時刻）として、
次式のように求めることができる。
【数５】
2014228495.tif 000007
【数６】
2014228495.tif 000008
【００５４】
なお、（６）式において、「Ａ」は、次式
【数７】
2014228495.tif 000009
で表される行列であり、「Ｑ」は、次式
【数８】
2014228495.tif 000010
で表される行列である。
【００５５】
（１−２−３−３）観測値予測処理
観測値予測処理では、次式
【数９】
2014228495.tif 000011
で表される観測方程式を、対象とするセンサごとに定義し、上述の内部状態予測処理により求めた内部状態Ｘｔの予測値Ｘｔ＋Δｔと、分散・共分散ＣｏｖＸｔの分散・共分散ＣｏｖＸｔ＋Δｔとを代入することにより、対象とするセンサの観測値を予測する。
【００５６】
ここで（９）式において、Ｙｔは観測値の予測値を表し、Ｘｔ＊は、修正処理前の内部状態（予測処理によって、ｔが観測時刻に一致しているものとする）を表す。同様に、以下、「＊」は修正処理前の値であることを表すものとする。
【００５７】
また観測値予測処理では、対象とするセンサごとに、当該センサの観測値の予測値Ｙｔの分散・共分散ＣｏｖＹｔを次式
【数１０】
2014228495.tif 000012
により求める。なお、（１０）式において、Ｂは、次式
【数１１】
2014228495.tif 000013
で与えられる観測方程式の偏微分行列（以下、これを観測行列と呼ぶ）を表す。
【００５８】
さらに観測値予測処理では、対象とするセンサごとに、次式
【数１２】
2014228495.tif 000014
によりセンサノイズを含んだ分散期待値Ｖｔを求める。なお（１２）式において、Ｒは、センサ値にどの程度のノイズが含まれているかを表す観測ノイズである。
【００５９】
本実施の形態の場合、ＧＰＳ／ＧＰＳコンパス１３に含まれるＧＰＳセンサについて、関数ｇ（Ｘｔ＊）、観測行列Ｂ及び観測ノイズＲは、それぞれ次式
【数１３】
2014228495.tif 000015
【数１４】
2014228495.tif 000016
【数１５】
2014228495.tif 000017
のように定義される。
【００６０】
またＧＰＳ／ＧＰＳコンパス１３に含まれるＧＰＳコンパスについて、関数ｇ（Ｘｔ＊）、観測行列Ｂ及び観測ノイズＲは、それぞれ次式
【数１６】
2014228495.tif 000018
【数１７】
2014228495.tif 000019
【数１８】
2014228495.tif 000020
のように定義される。
【００６１】
さらに車輪用エンコーダ１４及びジャイロセンサ１５について、関数ｇ（Ｘｔ＊）、観測行列Ｂ及び観測ノイズＲは、それぞれ次式
【数１９】
2014228495.tif 000021
【数２０】
2014228495.tif 000022
【数２１】
2014228495.tif 000023
のように定義され、ステア角エンコーダ１６について、関数ｇ（Ｘｔ＊）、観測行列Ｂ及び観測ノイズＲは、それぞれ次式
【数２２】
2014228495.tif 000024
【数２３】
2014228495.tif 000025
【数２４】
2014228495.tif 000026
のように定義される。
【００６２】
なお、車輪用エンコーダ１４からのエンコーダ情報に基づき得られた修正前の車両の速度の予測値ｖｘ＊が「０」のときは、ステア角の予測値を算出できないため、ステア角エンコーダの観測値による修正処理を実行しない。またσgps_x〜σ_κは、各センサの精度（ばらつき）を考慮して決定する。
【００６３】
従って、位置推定部１２は、ＩＮＳ処理（図２）の観測予測処理では、（１９）式〜（２４）式により、車輪用エンコーダ１４からのエンコーダ情報に基づき得られる速度ｖｘの観測値の予測値Ｙｔと、ジャイロセンサ１５からの角速度情報に基づき得られる角速度ｗｚの観測値の予測値Ｙｔと、ステア角エンコーダ１６からのステア角情報に基づき得られる曲率κの観測値の予測値Ｙｔとを求める。
【００６４】
また位置推定部１２は、ＮＳＳ処理（図４）のステップＳＰ２２の更新処理内の観測予測処理では、（１３）式〜（１５）式により、ＧＰＳ／ＧＰＳコンパス１３からの位置・姿勢情報に基づき得られる位置（ｘ、ｙ）の観測値の予測値Ｙｔを求め、ＮＳＳ処理のステップＳＰ２３の更新処理内の観測予測処理では、（１６）式〜（１８）式により、ＧＰＳ／ＧＰＳコンパス１３からの位置・姿勢情報に基づき得られる姿勢θの観測値の予測値Ｙｔを求めることになる。
【００６５】
（１−２−３−４）異常判定処理
異常判定処理は、上述のようにＮＳＳ処理においてのみ実行される処理である。この異常判定処理では、マルチパスの影響等による測定異常がＧＰＳ／ＧＰＳコンパス１３に発生しているか否かを判定する。
【００６６】
ここで、かかる異常判定の判定方法としては、予測値及び観測値の差分の絶対値を評価する第１の異常判定方法と、マハラノビス距離の大きさを評価する第２の異常判定方法と、内部状態Ｘｔの修正量ＸＣｏｒｒの大きさを評価する第３の異常判定方法とが考えられる。
【００６７】
このうち第１の異常判定方法は、（１３）式により算出される車両の位置の予測値Ｙｔと、実際の車両の位置の観測値Ｙｓとの差分の絶対値（ＮＳＳ処理のステップＳＰ２２の場合）、又は（１６）式により算出される車両の姿勢の予測値Ｙｔと、実際の車両の姿勢の観測値Ｙｓとの差分の絶対値（ＮＳＳ処理のステップＳＰ２３の場合）が、予め位置又は姿勢について設定された閾値よりも大きいときに、ＧＰＳ／ＧＰＳコンパス１３に測定異常が発生していると判定する方法である。
【００６８】
図６は、ＧＰＳ／ＧＰＳコンパス１３に測定異常が発生しているか否かを判定する判定方法として、第１の異常判定方法を採用した場合の位置推定部１２の具体的な処理手順を示す。
【００６９】
この場合、位置推定部１２は、この図６に示す処理手順に従って、まず、次式
【数２５】
2014228495.tif 000027
により、車両の位置又は姿勢について、（１３）式又は（１６）式により算出される予測値Ｙｔと、実際の観測値Ｙｓとの差分（以下、これを観測残差と呼ぶ）Ｅｙを算出する（ＳＰ４０）。
【００７０】
続いて、位置推定部１２は、（２５）式により算出した観測残差Ｅｙについて、次式
【数２６】
2014228495.tif 000028
のように、観測残差Ｅｙの絶対値が予め定められた閾値（Th_Ey）よりも大きいか否かを判断する（ＳＰ４１）。
【００７１】
そして位置推定部１２は、かかる判断において否定結果を得た場合には、通常の修正処理を実行すべきと判定し（ＳＰ４２）、かかる判断において肯定結果を得た場合には、異常時の修正処理を実行すべきと判断する（ＳＰ４３）。
【００７２】
一方、第２の異常判定方法は、マハラノビス距離が大きいときに、ＧＰＳ／ＧＰＳコンパス１３に測定異常が発生していると判定する方法である。
【００７３】
車両の運動誤差やセンサの測定誤差がすべて正規分布であれば、観測残差Ｅｙも平均「０」、分散期待値Ｖｔの正規分布となる。従って、誤差（観測残差Ｅｙ）の二乗を誤差分散で割った次式
【数２７】
2014228495.tif 000029
により算出されるマハラノビス距離Ｍは、99.7％の確率で３以下となるため（正規分布誤差が３σ範囲に入る確率が99.7％であるため）、これを超える場合は異常値であると判定することができる。
【００７４】
そこで、本実施の形態においては、ＮＳＳ処理のステップＳＰ２２では、次式
【数２８】
2014228495.tif 000030
を満たすか否かに基づいて、またＮＳＳ処理のステップＳＰ２３では、次式
【数２９】
2014228495.tif 000031
を満たすか否かに基づいて、ＧＰＳ／ＧＰＳコンパス１３に測定異常が発生しているか否かを判定する。
【００７５】
なお（２８）式において、Th_gxyは、マハラノビス距離Ｍについて予め定められた位置の閾値を表し、（２９）式においてTh_gthは、マハラノビス距離Ｍについて予め定められた姿勢の閾値を表す。これらの閾値Th_gxy及びTh_gthは、実際の実験データを参考に決定される閾値であるが、上述のようにマハラノビス距離Ｍは、99.7％の確率で３以下となるため、３〜４の範囲で決定されることになる。
【００７６】
図７は、ＧＰＳ／ＧＰＳコンパス１３に測定異常が発生しているか否かを判定する判定方法として、この第２の異常判定方法を採用した場合の位置推定部１２の具体的な処理手順を示す。
【００７７】
この場合、位置推定部１２は、この図７に示す処理手順に従って、まず、車両の位置について、（１３）式により算出される予測値Ｙｔと、実際の観測値Ｙｓとの観測残差Ｅｙを（２５）式により算出する（ＳＰ５０）。
【００７８】
続いて、位置推定部１２は、マハラノビス距離Ｍを（２７）式により算出し（ＳＰ５１）、この後、ＮＳＳ処理のステップＳＰ２２では、位置について算出したマハラノビス距離Ｍが予め設定された位置についての閾値Th_gxyよりも小さいか否かを判断し、ＮＳＳ処理のステップＳＰ２３では、姿勢について算出したマハラノビス距離Ｍが予め設定された姿勢についての閾値Th_gthよりも小さいか否かを判断する（ＳＰ５２）。
【００７９】
そして位置推定部１２は、かかる判断において否定結果を得た場合には、通常の修正処理を実行すべきと判定し（ＳＰ５３）、かかる判断において肯定結果を得た場合には、異常時の修正処理を実行すべきと判断する（ＳＰ５４）。
【００８０】
他方、第３の異常判定方法は、異常判定の際にマハラノビス距離Ｍでなく、次式
【数３０】
2014228495.tif 000032
で表される内部状態Ｘｔの修正量ＸＣｏｒｒを利用して、ＧＰＳ／ＧＰＳコンパス１３に測定異常が発生しているか否かを判定する方法である。
【００８１】
なお（３０）式において、Ｋは、次式
【数３１】
2014228495.tif 000033
により算出されるカルマンゲインを表す。
【００８２】
この第３の異常判定方法を採用する場合、ＮＳＳ処理のステップＳＰ２２では、修正量ＸＣｏｒｒの各成分（XCorr_x、XCorr_y、XCorr_θ、XCorr_vx、XCorr_wz及びXCorr_κ）のうち、位置に関する成分XCorr_x及びXCorr_yを利用して、次式
【数３２】
2014228495.tif 000034
を満たすか否かに基づいて、またＮＳＳ処理のステップＳＰ２３では、修正量ＸＣｏｒｒの各成分のうち、姿勢に関する成分XCorr_θを利用して、次式
【数３３】
2014228495.tif 000035
を満たすか否かに基づいて、ＧＰＳ／ＧＰＳコンパス１３に測定異常が発生しているか否かを判定する。
【００８３】
なお（３２）式において、Th_gxyは、評価値ＶＣｏｒｒについて予め定められた位置の閾値を表し、（３３）式においてTh_gthは、評価値ＶＣｏｒｒについて予め定められた姿勢の閾値を表す。
【００８４】
図８は、ＧＰＳ／ＧＰＳコンパス１３に測定異常が発生しているか否かを判定する判定方法として、この第３の異常判定方法が採用された場合の位置推定部１２の具体的な処理手順を示す。
【００８５】
この場合、位置推定部１２は、この図８に示す処理手順に従って、まず、車両の位置について、（１３）式により算出される予測値Ｙｔと、実際の観測値Ｙｓとの観測残差Ｅｙを（２５）式により算出する（ＳＰ）。
【００８６】
続いて、位置推定部１２は、（３１）式によりカルマンゲインＫを算出し（ＳＰ６１）、この後、（３０）式で表される修正量ＸＣｏｒｒを算出す（ＳＰ６２）。そして位置推定部１２は、この後、ＮＳＳ処理のステップＳＰ２２では、ステップＳＰ６２において算出した修正量ＸＣｏｒｒのうちの位置に関する成分XCorr_x及びXCorr_yを利用して、（３２）式を満たすか否かを判断し、ＮＳＳ処理のステップＳＰ２３では、ステップＳＰ６２において算出した修正量ＸＣｏｒｒのうちの姿勢に関する成分XCorr_θを利用して、（３３）式を満たすか否かを判断する（ＳＰ６３）。
【００８７】
そして位置推定部１２は、かかる判断において否定結果を得た場合には、通常の修正処理を実行すべきと判定し（ＳＰ６４）、かかる判断において肯定結果を得た場合には、異常時の修正処理を実行すべきと判断する（ＳＰ６５）。
【００８８】
（１−２−３−５）修正処理
（１−２−３−５−１）通常の修正処理
位置推定部１２は、上述の異常判定処理において、通常の修正処理を実行すべきと判定した場合、次式
【数３４】
2014228495.tif 000036
により内部状態Ｘｔを修正すると共に、次式
【数３５】
2014228495.tif 000037
により内部状態Ｘｔの分散・共分散ＣｏｖＸｔを修正する。なお、（３４）式及び（３５）式において、Ｋは、上述の（３１）式により算出されるカルマンゲインを表す。
【００８９】
図９は、上述の異常判定処理において修正処理として通常処理が選択された場合に位置推定部１２により実行される修正処理（以下、これを通常修正処理と呼ぶ）の具体的な処理手順を示す。
【００９０】
この場合、位置推定部１２は、まず、（３１）の演算を実行することによりカルマンゲインＫを算出する（ＳＰ７０）。続いて、位置推定部１２は、ステップＳＰ７１において算出したカルマンゲインＫを利用して（３４）式の演算を実行することにより、新たな内部状態Ｘｔを算出し、そのとき保持している内部状態Ｘｔを、このとき算出した新たな内部状態Ｘｔに置き換えるようにして修正する（ＳＰ７１）。
【００９１】
続いて、位置推定部１２は、ステップＳＰ７１において算出したカルマンゲインＫを利用して（３６）式の演算を実行することにより、新たな内部状態Ｘｔの分散・共分散ＣｏｖＸｔを算出し（ＳＰ７２）、そのとき保持している分散・共分散ＣｏｖＸｔを、このとき算出した新たな分散・共分散ＣｏｖＸｔに置き換えるようにして修正する。そして位置推定部１２は、この後、この通常修正処理を終了する。
【００９２】
（１−２−３−５−２）異常時修正処理
一方、上述の異常判定処理において、異常時の修正処理を実行すべきと判定した場合には、通常修正処理時よりも少ない修正量で内部状態Ｘｔ及びその分散・共分散ＣｏｖＸｔを修正する。
【００９３】
ここで、通常修正処理時よりも少ない修正量で内部状態Ｘｔ及びその分散・共分散ＣｏｖＸｔを修正する方法としては、観測ノイズを切り替える第１の異常時修正方法と、重み付けにより修正を行う第２の異常時修正方法と、修正量の上限処理を行う第３の異常時修正方法が考えられる。
【００９４】
このうち第１の異常時修正方法は、ＧＰＳ／ＧＰＳコンパス１３から出力される位置・姿勢情報の観測ノイズＲを通常修正処理時よりも大きな値に切り替える方法である。例えば、ＮＳＳ処理（図４）のステップＳＰ２２において実行した更新処理のステップＳＰ３２（図５）の異常判定処理で異常判定を得た場合において、通常修正処理時における（１５）式の「σgsp_x」及び「σgsp_y」の値がいずれも0.3〔m〕に設定されている場合には、これら「σgsp_x」及び「σgsp_y」の値をいずれもその10倍の３〔m〕に切り替える。また、ＮＳＳ処理のステップＳＰ２３において実行した更新処理のステップＳＰ３２の異常判定処理で異常判定を得た場合において、通常修正処理時における（１８）式の「σgsp_θ」の値が15〔度〕に設定されている場合には、当該「σgsp_θ」の値をその10倍の150〔度〕に切り替える。
【００９５】
このようにＧＰＳ／ＧＰＳコンパス１３から出力される位置・姿勢情報の観測ノイズＲを大きな値に切り替えることによって、（３１）式で表されるカルマンゲインＫを低減させることができ、結果として通常修正処理時よりも少ない修正量で内部状態Ｘｔ及びその分散・共分散ＣｏｖＸｔを修正することができる。
【００９６】
図１０は、異常判定処理時にＧＰＳ／ＧＰＳコンパス１３に測定異常が発生していると判定されたときの修正方法として、この第１の異常時修正方法が採用された場合の位置推定部１２の具体的な処理手順を示す。
【００９７】
この場合、位置推定部１２は、この図１０に示す処理手順に従って、まず、観測ノイズＲを予め設定された異常時修正処理時の値（通常修正処理時よりも大きな値）に変更した上で、（１２）式により観測残差Ｅｙの分散期待値Ｖｔを再計算する（ＳＰ８０）。
【００９８】
続いて、位置推定部１２は、ステップＳＰ８０において算出した観測残差Ｅｙの分散期待値Ｖｔを利用して、（３１）式によりカルマンゲインＫを算出する（ＳＰ８１）。
【００９９】
次いで、位置推定部１２は、ステップＳＰ８１において算出したカルマンゲインＫを用いて（３４）式により内部状態Ｘｔを算出し、保持している内部状態Ｘｔを、このとき算出した新たな内部状態Ｘｔに置き換えるようにして修正する（ＳＰ８２）。
【０１００】
また位置推定部１２は、ステップＳＰ８１において算出したカルマンゲインＫを用いて（３５）式により内部状態Ｘｔの分散・共分散ＣｏｖＸｔを算出し、保持している分散・共分散ＣｏｖＸｔを、このとき算出した新たな分散・共分散ＣｏｖＸｔに置き換えるようにして修正する（ＳＰ８３）。
【０１０１】
一方、第２の異常時修正方法は、内部状態Ｘｔについては、次式
【数３６】
2014228495.tif 000038
のように、また分散・共分散ＣｏｖＸｔについては次式
【数３７】
2014228495.tif 000039
のように、修正量（Ｋ×Ｅｙ又は−Ｋ×Ｂ×ＣｏｖＸｔ＊）をマハラノビス距離Ｍで除算する方法である。
【０１０２】
ＮＳＳの測定異常の程度が大きければ大きいほどマハラノビス距離Ｍも大きくなるため、（３５）式及び（３６）式のように修正量をマハラノビス距離Ｍで除算することにより、通常修正処理時よりも少ない修正量で内部状態Ｘｔ及び分散・共分散ＣｏｖＸｔを修正することができる。
【０１０３】
図１１は、異常判定処理時にＧＰＳ／ＧＰＳコンパス１３に測定異常が発生していると判定されたときの修正方法として、この第２の異常時修正方法が採用された場合の位置推定部１２の具体的な処理手順を示す。
【０１０４】
この場合、位置推定部１２は、この図１１に示す処理手順に従って、まず、通常時のカルマンゲインＫを（３１）式により算出すると共に（ＳＰ９０）、算出したカルマンゲインＫを（２７）式で与えられるマハラノビス距離Ｍで除算した係数を算出する（ＳＰ９１）。
【０１０５】
続いて、位置推定部１２は、ステップＳＰ９１において算出した係数を用いて（３４）式により内部状態Ｘｔを算出し、保持している内部状態Ｘｔを、このとき算出した新たな内部状態Ｘｔに置き換えるようにして修正する（ＳＰ９２）。
【０１０６】
また位置推定部１２は、ステップＳＰ９１において算出した係数を用いて（３５）式により内部状態Ｘｔの分散・共分散ＣｏｖＸｔを算出し、保持している分散・共分散ＣｏｖＸｔを、このとき算出した新たな分散・共分散ＣｏｖＸｔに置き換えるようにして修正する（ＳＰ９３）。
【０１０７】
他方、第３の異常時修正方法は、内部状態Ｘｔについては、次式
【数３８】
2014228495.tif 000040
のように、また分散・共分散ＣｏｖＸｔについては次式
【数３９】
2014228495.tif 000041
のように修正する方法である。なお、（３８）式及び（３９）式において、Ｔｈは、システムが許容できる修正量の上限を表し、ＸＣｏｒｒは、通常の修正処理における修正量を表す。
【０１０８】
この第３の異常時修正方法によれば、内部状態Ｘｔの修正量を、予め設定された閾値以下に上限処理することができるという効果を得ることができる。
【０１０９】
図１２は、異常判定処理時にＧＰＳ／ＧＰＳコンパス１３に測定異常が発生していると判定されたときの修正方法として、この第３の異常時修正方法が採用された場合の位置推定部１２の具体的な処理手順を示す。
【０１１０】
この場合、位置推定部１２は、この図１２に示す処理手順に従って、まず、カルマンゲインＫを（３１）式により算出し（ＳＰ１００）、算出したカルマンゲインＫを用いて（３０）式により内部状態Ｘｔの修正量ＸＣｏｒｒを算出する（ＳＰ１０１）。
【０１１１】
なお、例えば異常判定処理における異常判定方法として上述した第３の異常判定方法を採用している場合には、異常判定処理の段階で既にカルマンゲインＫ及び修正量ＸＣｏｒｒを算出しているため（図８のステップＳＰ６１及びステップＳＰ６２）、ステップＳＰ１００及びステップＳＰ１０１の処理を省略することができる。
【０１１２】
続いて、位置推定部１２は、次式
【数４０】
2014228495.tif 000042
により、修正量ＸＣｏｒｒを減らすための係数αを算出する（ＳＰ１０２）。
【０１１３】
次いで、位置推定部１２は、ステップＳＰ１００において算出したカルマンゲインＫと、ステップＳＰ１０２において算出した係数αとを用いて（３８）式により内部状態Ｘｔを算出し、保持している内部状態Ｘｔを、このとき算出した新たな内部状態Ｘｔに置き換えるようにして修正する（ＳＰ１０３）。
【０１１４】
また位置推定部１２は、ステップＳＰ１００において算出したカルマンゲインＫと、ステップＳＰ１０２において算出した係数αとを用いて（３９）式により内部状態Ｘｔの分散・共分散ＣｏｖＸｔを算出し、保持している分散・共分散ＣｏｖＸｔを、このとき算出した新たな分散・共分散ＣｏｖＸｔに置き換えるようにして修正する（ＳＰ１０４）。
【０１１５】
（１−３）本実施の形態の効果
以上の車両制御システム１によれば、ＩＮＳによる測位結果が常にＭＳＳの測定結果に基づいて修正されるため、最終的にＩＮＳによる測位結果がＮＳＳの測位結果に近づくよう修正される。従って、自己位置の推定結果としてＩＮＳによる測位結果を採用した場合に、その後、ＮＳＳによる測位結果が利用されなくなって、自己位置の推定結果と、現実の位置との間の誤差が拡大するという事態が発生するのを未然かつ有効に防止することができる。
【０１１６】
また本車両制御システム１によれば、自己が存在する場所の周囲の地理データを必要とせず、少ないデータ量及び計算量で自己位置を推定することができる。
【０１１７】
かくするにつき、本車両制御システム１によれば、少ないデータ量及び計算量で精度良く自己位置を推定することができる。
【０１１８】
（２）第２の実施の形態
図１において、２０は全体として第２の実施の形態による車両制御システムを示す。この車両制御システム２０は、測位装置２１の位置推定部２２により実行される更新処理の処理内容が異なる点を除いて第１の実施の形態の車両制御システム１と同様に構成されている。
【０１１９】
図１３は、ＮＳＳ処理（図４）のステップＳＰ２２及びステップＳＰ２３において、図５について上述した更新処理に代えて実行される本実施の形態による更新処理の処理内容を示す。
【０１２０】
位置推定部２２は、ＮＳＳ処理のステップＳＰ２２及びステップＳＰ２３に進むと、この更新処理を開始し、まず、ステップＳＰ１１０の内部状態予測処理及びステップＳＰ１１１の観測値予測処理を、図５について上述した第１の実施の形態の更新処理の内部状態予測処理及び観測値予測処理（ＳＰ３０、ＳＰ３１）と同様に処理する。
【０１２１】
続いて、位置推定部２２は、ＮＳＳに測定異常が発生しているか否かの判定を行うと共に、ＮＳＳに測定異常が発生しているときには内部状態及びその分散・共分散を修正する異常判定及び修正処理を実行する（ＳＰ１１２）。
【０１２２】
図１４は、かかる更新処理のステップＳＰ１１２において位置推定部２２により実行される異常判定及び修正処理の具体的な処理内容を示す。
【０１２３】
位置推定部２２は、更新処理（図１３）のステップＳＰ１１２に進むと、この図１４に示す異常判定処理及び修正処理を開始し、まず、図７について上述した第２の異常判定処理のステップＳＰ５０〜ステップＳＰ５２と同様にステップＳＰ１２０〜ステップＳＰ１２２を処理することにより、ＮＳＳ処理のステップＳＰ２２では、位置について算出したマハラノビス距離Ｍが予め設定された位置についての閾値Th_gxyよりも小さいか否かを判断し、ＮＳＳ処理のステップＳＰ２３では、姿勢について算出したマハラノビス距離Ｍが予め設定された姿勢についての閾値Th_gthよりも小さいか否かを判断する（ＳＰ１２０〜ＳＰ１２２）。
【０１２４】
そして位置推定部２２は、この判定で否定結果を得ると、図１０について上述した第１の異常時修正処理のステップＳＰ８０及びステップＳＰ８１と同様にして、観測ノイズＲを予め設定された異常時修正処理時の値（通常修正処理時よりも大きな値）に変更した上で、（１２）式により観測残差Ｅｙの分散期待値Ｖｔを再計算し（ＳＰ１２６）、さらにこの分散期待値Ｖｔを利用して、（３１）用いてカルマンゲインＫを算出する（ＳＰ１２７）。
【０１２５】
続いて位置推定部２２は、図１１について上述した第２の異常時修正処理のステップＳＰ９１と同様に、ステップＳＰ１２７において算出したカルマンゲインＫを（２７）式で与えられるマハラノビス距離Ｍで除算した新たなカルマンゲインＫを算出する（ＳＰ１２８）。
【０１２６】
次いで、位置推定部２２は、図８について上述した第３の異常時修正処理のステップＳＰ６２と同様にして、ステップＳＰ１２８において算出した新たなカルマンゲインＫを用いて修正量ＸＣｏｒｒを算出する（ＳＰ１２９）。
【０１２７】
この後、位置推定部２２は、ＮＳＳ処理のステップＳＰ２２では、ステップＳＰ６２において算出した修正量ＸＣｏｒｒのうちの位置に関する成分XCorr_x及びXCorr_yを利用して、（３２）式を満たすか否かを判断し、ＮＳＳ処理のステップＳＰ２３では、ステップＳＰ６２において算出した修正量ＸＣｏｒｒのうちの姿勢に関する成分XCorr_θを利用して、（３３）式を満たすか否かを判断する（ＳＰ１３０）。
【０１２８】
そして位置推定部２２は、この判断で肯定結果を得ると、図１２について上述した第３の異常時修正方法のステップＳＰ１０２〜ステップＳＰ１０４と同様にして内部状態Ｘｔ及びその分散・共分散ＣｏｖＸｔを修正し（ＳＰ１３１〜ＳＰ１３３）、この後、この更新処理を終了する。
【０１２９】
これに対して位置推定部２２は、ステップＳＰ１３０の判断で否定結果を得ると、ステップＳＰ１２８において算出した新たなカルマンゲインＫを用いて上述の（３４）式により新たな内部状態Ｘｔを算出すると共に、保持している内部状態Ｘｔをこのとき算出した内部状態Ｘｔに置き換えることにより修正する（ＳＰ１３４）。
【０１３０】
また位置推定部２２は、ステップＳＰ１２８において算出した新たなカルマンゲインＫを用いて上述の（３５）式により新たな分散・共分散ＣｏｖＸｔを算出すると共に、保持している分散・共分散ＣｏｖＸｔをこのとき算出した分散・共分散ＣｏｖＸｔに置き換えることにより修正する（ＳＰ１３４）。そして位置推定部２２は、この後、この更新処理を終了する。
【０１３１】
以上の本実施の形態の車両制御システム２０によれば、異常判定処理においてＮＳＳの測位結果に異常が発生していると判定された場合に、観測ノイズＲを大きな値に変更する修正処理を実行した上で、再度、異常判定処理を行い、この異常判定処理において異常を検出した場合に、さらに修正処理を実行した上で内部状態Ｘｔ及びその分散・共分散ＣｏｖＸｔを修正するようにしているため、より精度良く自己位置を推定することができる。
【０１３２】
（３）他の実施の形態
なお上述の第１及び第２の実施の形態においては、本発明を車両の位置を推定する測位装置に適用するようにした場合について述べたが、本発明はこれに限らず、車両以外の移動体の位置を推定するこの他種々の自己位置推定装置に広く適用することができる。
【０１３３】
また上述の第１及び第２の実施の形態においては、ＮＳＳ処理（図４）において、位置及び姿勢のそれぞれについて計測結果に異常が発生しているか否かの異常判定を行うようにした場合について述べたが、本発明はこれに限らず、位置及び姿勢のいずれか一方のみで異常判定を行うようにしても良い。
【０１３４】
さらに上述の第１及び第２の実施の形態においては、ＩＮＳセンサ１１として車輪用エンコーダ１４、ジャイロセンサ１５及びステア角エンコーダ１６の３つの内界センサを適用するようにした場合について述べたが、本発明はこれに限らず、これら３つの内界センサすべてを利用する必要はない。例えば車輪用エンコーダ１４のみを用いて、左右両側の車輪の回転量から速度及び角速度を測定することによりジャイロセンサ１５を省略するようにしても良く、またジャイロセンサ１５のみを用いて、加速度及び角速度を検出し、これらに基づいて内部状態の修正を行うようにしても良い。ただし、内界センサの数（種類）を増やしたり、精度を上げることによって、マハラノビス距離Ｍの算出時の分散期待値Ｖｔが小さな値となるため、異常判定を行い易くなるという利点がある。
【０１３５】
さらに上述の第１及び第２の実施の形態においては、ＩＮＳセンサ１１として車輪用エンコーダ１４、ジャイロセンサ１５及びステア角エンコーダ１６を適用するようにした場合について述べたが、本発明はこれに限らず、例えば図１に示すように、ＩＮＳセンサ１１として加速度センサ３０を追加するようにしても良い。
【産業上の利用可能性】
【０１３６】
本発明は、車両の位置を推定する測位装置に適用することができる。
【符号の説明】
【０１３７】
１，２０……車両制御システム、２，２１……測位装置、３……制御装置、１０……ＮＳＳセンサ、１１……ＩＮＳセンサ、１２，２２……位置推定部、１３……ＧＰＳ／ＧＰＳコンパス、１４……車輪用エンコーダ、１５……ジャイロセンサ、１６……ステア角エンコーダ。
【図１】
2014228495.tif 000043
【図２】
2014228495.tif 000044
【図３】
2014228495.tif 000045
【図４】
2014228495.tif 000046
【図５】
2014228495.tif 000047
【図６】
2014228495.tif 000048
【図７】
2014228495.tif 000049
【図８】
2014228495.tif 000050
【図９】
2014228495.tif 000051
【図１０】
2014228495.tif 000052
【図１１】
2014228495.tif 000053
【図１２】
2014228495.tif 000054
【図１３】
2014228495.tif 000055
【図１４】
2014228495.tif 000056