特許 6260983 自己位置推定装置及び方法

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】6260983

(24)【登録日】2017年12月22日

(45)【発行日】2018年1月17日

(54)【発明の名称】自己位置推定装置及び方法

(51)【国際特許分類】

   G01S 19/49 20100101AFI20180104BHJP

   G01C 21/28 20060101ALI20180104BHJP

【ＦＩ】

   G01S19/49

   G01C21/28

【請求項の数】4

【全頁数】26

(21)【出願番号】特願2013-110370(P2013-110370)

(22)【出願日】2013年5月24日

(65)【公開番号】特開2014-228495(P2014-228495A)

(43)【公開日】2014年12月8日

【審査請求日】2016年4月25日

(73)【特許権者】

【識別番号】000000099

【氏名又は名称】株式会社ＩＨＩ

(73)【特許権者】

【識別番号】500302552

【氏名又は名称】株式会社ＩＨＩエアロスペース

(74)【代理人】

【識別番号】100093861

【弁理士】

【氏名又は名称】大賀 眞司

(74)【代理人】

【識別番号】100129218

【弁理士】

【氏名又は名称】百本 宏之

(72)【発明者】

【氏名】江本 周平

(72)【発明者】

【氏名】坂野 肇

【審査官】 中村 説志

(56)【参考文献】

【文献】 特開２０１２−００７９３８（ＪＰ，Ａ）

【文献】 特開２０１２−２０７９１９（ＪＰ，Ａ）

【文献】 特開２０１２−２１５４９１（ＪＰ，Ａ）

【文献】 特開２０１２−２３７６０６（ＪＰ，Ａ）

【文献】 特開２０１２−０４２３１８（ＪＰ，Ａ）

【文献】 特開２００８−１０７１６０（ＪＰ，Ａ）

【文献】 特開２０１１−１３７７０８（ＪＰ，Ａ）

【文献】 米国特許第０８５６０２１８（ＵＳ，Ｂ１）

(58)【調査した分野】（Int.Cl.，ＤＢ名）

Ｇ０１Ｓ ５／００− ５／１４

Ｇ０１Ｓ１９／００−１９／５５

Ｇ０１Ｃ２１／００−２１／３６

Ｇ０１Ｃ２３／００−２５／００

(57)【特許請求の範囲】

【請求項1】

測位衛星からの電波に基づいて自己の位置を測位するＮＳＳ（Navigation Satellite System）センサと、
自己の状態を検出する内界センサと、
前記ＮＳＳセンサの出力に基づき得られる第１の測位結果と、前記内界センサの出力を積分することにより得られる第２の測位結果とに基づいて自己の位置を推定する位置推定部と
を備え、
前記位置推定部は、
自己の状態を内部状態として管理しており、
管理している最新の内部状態に基づいて、前記内部状態の観測値を取得した時刻における自己の内部状態を予測し、
予測した前記内部状態に基づいて、少なくとも前記第１の測位結果を予測し、
予測した前記第１の測位結果と、現実の前記第１の測位結果との差分に基づいて前記ＮＳＳセンサに測定異常が発生しているか否かを判定する１回目の異常判定処理を実行し、
当該１回目の異常判定処理により前記ＮＳＳセンサに測定異常が発生していると判定した場合には、観測ノイズを大きな値に変更する修正処理を実行した上で、再度、前記ＮＳＳセンサに測定異常が発生しているか否かを判断する２回目の異常判定処理を実行し、
当該２回目の異常判定処理により前記ＮＳＳセンサに測定異常が発生していないと判定した場合には、前記第２の測位結果を前記第１の測位結果に基づいて修正したものを自己の位置と推定し、前記ＮＳＳセンサに測定異常が発生していると判定した場合には、前記第２の測位結果を、前記ＮＳＳセンサに測定異常が発生していないと判定した場合よりも少ない修正量で修正したものを自己の位置と推定する
ことを特徴とする自己位置推定装置。

【請求項2】

前記位置推定部は、
前記１回目の異常判定処理において、予測した前記第１の測位結果及び現実の前記第１の測位結果の差分の絶対値と、当該差分のマハラノビス距離と、前記内部状態の修正量の大きさとの何れかに基づいて、前記ＮＳＳセンサに測定異常が発生しているか否かを判定する
ことを特徴とする請求項１に記載の自己位置推定装置。

【請求項3】

自己位置推定装置により実行される自己位置推定方法において、
測位衛星からの電波に基づいて自己の位置を測位するＮＳＳ（Navigation Satellite System）センサの出力と、自己の状態を検出する内界センサの出力とを取得する第１のステップと、
前記ＮＳＳセンサの出力に基づき得られる第１の測位結果と、前記内界センサの出力を積分することにより得られる第２の測位結果とに基づいて自己の位置を推定する第２のステップと
を備え、
前記自己位置推定装置は、
自己の状態を内部状態として管理しており、
前記第２のステップにおいて、前記自己位置推定装置は、
管理している最新の内部状態に基づいて、前記内部状態の観測値を取得した時刻における自己の内部状態を予測し、
予測した前記内部状態に基づいて、少なくとも前記第１の測位結果を予測し、
予測した前記第１の測位結果と、現実の前記第１の測位結果との差分に基づいて前記ＮＳＳセンサに測定異常が発生しているか否かを判定する１回目の異常判定処理を実行し、
当該１回目の異常判定処理により前記ＮＳＳセンサに測定異常が発生していると判定した場合には、観測ノイズを大きな値に変更する修正処理を実行した上で、再度、前記ＮＳＳセンサに測定異常が発生しているか否かを判断する２回目の異常判定処理を実行し、
当該２回目の異常判定処理により前記ＮＳＳセンサに測定異常が発生していないと判定した場合には、前記第２の測位結果を前記第１の測位結果に基づいて修正したものを自己の位置と推定し、前記ＮＳＳセンサに測定異常が発生していると判定した場合には、前記第２の測位結果を、前記ＮＳＳセンサに測定異常が発生していないと判定した場合よりも少ない修正量で修正したものを自己の位置と推定する
ことを特徴とする自己位置推定方法。

【請求項4】

前記自己位置推定装置は、
前記１回目の異常判定処理において、予測した前記第１の測位結果及び現実の前記第１の測位結果の差分の絶対値と、当該差分のマハラノビス距離と、前記内部状態の修正量の大きさとの何れかに基づいて、前記ＮＳＳセンサに測定異常が発生しているか否かを判定する
ことを特徴とする請求項３に記載の自己位置推定方法。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、自己位置推定装置及び方法に関し、例えばＮＳＳ（Navigation Satellite Systems：航法衛星システム）及びＩＮＳ（Inertial Navigation System：慣性航法システム）を利用した車両の移動軌道を制御する車両制御システムに適用して好適なものである。

【背景技術】

【0002】

従来、ロボット、車両又は携帯端末などの位置を推定する技術としてＮＳＳがある。ＮＳＳは、複数の測位衛星からの電波を受信することで各測位衛星との距離を求め、求めた各測位衛星の位置に基づいて受信機の位置を推定する方法である。一般的に普及しているＮＳＳとして、アメリカが運用するＧＰＳ（Global Positioning System）がある。

【0003】

ＮＳＳを利用して受信機の位置を測位する場合、受信機付近の建物、樹木又は地表からの反射波などの影響によりマルチパスが発生し、このマルチパスの影響により衛星との距離を正確に求められないことがある。従来、このようなマルチパスの影響を低減する方法として、以下の２つの技術が提案されている。

【0004】

第１の方法は、自己位置と、衛星の位置と、全周囲の遮蔽物に関する情報とに基づいて各衛星からの信号が直接波及び反射波のいずれであるかを判別する方法である。この場合における「全周囲の遮蔽物に関する情報」の取得方法としては、地理情報データベースから取得する方法（特許文献１）や、赤外・可視カメラ等により周囲を計測する方法（特許文献２）などが提案されている。

【0005】

また第２の方法は、ＮＳＳによる測位結果と、ＩＮＳによる位置推定結果とを比較して、差が大きいときにはＮＳＳによる測位結果にマルチパスによる誤差が含まれていると判定し、ＩＮＳによる位置推定結果を採用する方法である。この場合におけるＮＳＳによる測位結果と、ＩＮＳによる位置推定結果とを比較する比較方法として、測位対象（例えば車両）の位置及び姿勢を比較する方法（特許文献３）や、加速度を比較する方法（特許文献４）などが提案されている。

【0006】

なお、ＩＮＳは、一般的には加速度計の情報を積分して距離を求め、ジャイロ情報を積分して方角を求め、移動距離と方角のベクトルを細分点ごとに合成してゆくことにより、起点からの移動距離及び方角を算出するシステムである。またＩＮＳとして、車輪エンコーダやステア角エンコーダなどの情報を利用して、移動距離及び方角を補正するものもある。ＩＮＳは、測位対象の位置を直接計測しないため、速度、加速度及び角速度の計測誤差が積分され、位置誤差が徐々に大きくなる特徴がある。従って、ＩＮＳを利用する場合には、一般的にＩＮＳによる位置推定結果を定期的にＮＳＳによる測位結果に基づいて補正することが行われる。

【先行技術文献】

【特許文献】

【0007】

【特許文献1】特許第４４１８３５７号明細書

【特許文献2】特許第４３９１４５８号明細書

【特許文献3】特許第４８０３８６２号明細書

【特許文献4】特許第３３３８３０４号明細書

【発明の概要】

【発明が解決しようとする課題】

【0008】

ところで、上述の第１の方法は、測位対象が存在する場所の周囲の地理データを必要とし、例えば地理情報データベースを用いる場合に測位対象が進入するエリア全体のデータが必要となり、その分、計算量も多くなる問題がある。また第１の方法において、特許文献２のようにカメラ等で周囲を計測することにより周囲の遮蔽物の情報を取得しようとする場合には、遮蔽物の認識など複雑な処理が必要となったり、逆光などの光学的なノイズの影響を受けやすいという問題もある。

【0009】

一方、上述の第２の方法によると、大量のデータを処理することなく、マルチパスを検出できるという利点がある。しかしながら、ＩＮＳによる位置の推定では、タイヤのスリップや振動などのノイズの影響により位置誤差が発生する。このためＮＳＳによる測位結果と、ＩＮＳによる位置推定結果とが一致しない場合、ＮＳＳによる測位結果よりもＩＮＳによる位置推定結果の方が、実際の位置に対する誤差が大きい可能性もある。

【0010】

このような状況において、このためＮＳＳによる測位結果と、ＩＮＳによる位置推定結果とが一致しないときに、自己位置推定処理の処理結果としてＩＮＳによる位置推定結果を採用した場合、ＮＳＳにより正しい測位が行われているものとすると、ＮＳＳによる測位結果と、ＩＮＳによる位置推定結果との間の差が次第に大きくなるため、結果的にこれ以降、常にＩＮＳによる位置推定結果が採用されることとなり、この結果、推定される位置と実際の位置との差が拡大してしまうという問題があった。

【0011】

本発明は以上の点を考慮してなされたもので、少ないデータ量及び計算量で精度良く自己位置を推定し得る自己位置推定装置及び方法を提案しようとするものである。

【課題を解決するための手段】

【0012】

かかる課題を解決するため本発明においては、測位衛星からの電波に基づいて自己の位置を測位するＮＳＳ（Navigation Satellite System）センサと、自己の状態を検出する内界センサと、前記ＮＳＳセンサの出力に基づき得られる第１の測位結果と、前記内界センサの出力を積分することにより得られる第２の測位結果とに基づいて自己の位置を推定する位置推定部とを設け、前記位置推定部が、自己の状態を内部状態として管理しており、
管理している最新の内部状態に基づいて、前記内部状態の観測値を取得した時刻における自己の内部状態を予測し、予測した前記内部状態に基づいて、少なくとも前記第１の測位結果を予測し、予測した前記第１の測位結果と、現実の前記第１の測位結果との差分に基づいて前記ＮＳＳセンサに測定異常が発生しているか否かを判定する１回目の異常判定処理を実行し、当該１回目の異常判定処理により前記ＮＳＳセンサに測定異常が発生していると判定した場合には、観測ノイズを大きな値に変更する修正処理を実行した上で、再度、前記ＮＳＳセンサに測定異常が発生しているか否かを判断する２回目の異常判定処理を実行し、当該２回目の異常判定処理により前記ＮＳＳセンサに測定異常が発生していないと判定した場合には、前記第２の測位結果を前記第１の測位結果に基づいて修正したものを自己の位置と推定し、前記ＮＳＳセンサに測定異常が発生していると判定した場合には、前記第２の測位結果を、前記ＮＳＳセンサに測定異常が発生していないと判定した場合よりも少ない修正量で修正したものを自己の位置と推定するようにした。

【0013】

また本発明においては、自己位置推定装置により実行される自己位置推定方法において、測位衛星からの電波に基づいて自己の位置を測位するＮＳＳ（Navigation Satellite System）センサの出力と、自己の状態を検出する内界センサの出力とを取得する第１のステップと、前記ＮＳＳセンサの出力に基づき得られる第１の測位結果と、前記内界センサの出力を積分することにより得られる第２の測位結果とに基づいて自己の位置を推定する第２のステップとを設け、前記自己位置推定装置は、自己の状態を内部状態として管理しており、前記第２のステップにおいて、前記自己位置推定装置が、管理している最新の内部状態に基づいて、前記内部状態の観測値を取得した時刻における自己の内部状態を予測し、予測した前記内部状態に基づいて、少なくとも前記第１の測位結果を予測し、予測した前記第１の測位結果と、現実の前記第１の測位結果との差分に基づいて前記ＮＳＳセンサに測定異常が発生しているか否かを判定する１回目の異常判定処理を実行し、当該１回目の異常判定処理により前記ＮＳＳセンサに測定異常が発生していると判定した場合には、観測ノイズを大きな値に変更する修正処理を実行した上で、再度、前記ＮＳＳセンサに測定異常が発生しているか否かを判断する２回目の異常判定処理を実行し、当該２回目の異常判定処理により前記ＮＳＳセンサに測定異常が発生していないと判定した場合には、前記第２の測位結果を前記第１の測位結果に基づいて修正したものを自己の位置と推定し、前記ＮＳＳセンサに測定異常が発生していると判定した場合には、前記第２の測位結果を、前記ＮＳＳセンサに測定異常が発生していないと判定した場合よりも少ない修正量で修正したものを自己の位置と推定するようにした。

【0014】

かかる本発明の自己位置推定装置及び自己位置推定方法によれば、第２の測位結果が常に第１の測位結果に基づいて修正されるため、最終的に第２の測位結果が第１の測位結果に近づくよう修正される。従って、自己位置の推定結果として第２の測位結果を採用した場合に、その後、第１の測位結果が利用されなくなり、自己位置の推定結果と、現実の位置との間の誤差が拡大するという事態が発生するのを未然かつ有効に防止することができる。

【0015】

また本発明の自己位置推定装置及び自己位置推定方法によれば、自己が存在する場所の周囲の地理データを必要とせず、少ないデータ量及び計算量で自己位置を推定することができる。

【発明の効果】

【0016】

本発明によれば、少ないデータ量及び計算量で精度良く自己位置を推定し得る自己位置推定装置及び方法を実現できる。

【図面の簡単な説明】

【0017】

【図1】第１及び第２の実施の形態による車両制御システムの概略構成を示すブロック図である。

【図2】ＩＮＳ処理の処理手順を示すフローチャートである。

【図3】ＩＮＳ処理における更新処理の処理手順を示すフローチャートである。

【図4】ＮＳＳ処理の処理手順を示すフローチャートである。

【図5】ＮＳＳ処理における更新処理の処理手順を示すフローチャートである。

【図6】第１の異常判定処理の処理手順を示すフローチャートである。

【図7】第２の異常判定処理の処理手順を示すフローチャートである。

【図8】第３の異常判定処理の処理手順を示すフローチャートである。

【図9】通常修正処理の処理手順を示すフローチャートである。

【図10】第１の異常時修正処理の処理手順を示すフローチャートである。

【図11】第２の異常時修正処理の処理手順を示すフローチャートである。

【図12】第３の異常時修正処理の処理手順を示すフローチャートである。

【図13】第２の実施の形態による更新処理の処理手順を示すフローチャートである。

【図14】異常判定及び修正処理の処理手順を示すフローチャートである。

【発明を実施するための形態】

【0018】

以下図面について、本発明の一実施の形態を詳述する。

【0019】

（１）第１の実施の形態
（１−１）本実施の形態によるシステム構成
図１において、１は全体として本実施の形態による車両制御システムを示す。この車両制御システム１は、当該車両制御システム１が搭載された車両（図示せず）の位置を測位する測位装置２と、測位装置２の測位結果に基づいて車両の移動を制御する制御装置３とから構成される。

【0020】

測位装置２は、ＮＳＳ及びＩＮＳの双方を利用して車両の位置及び姿勢を推定する自己位置推定機能を有し、ＮＳＳ用のセンサ（以下、これをＮＳＳセンサと呼ぶ）１０としてＧＰＳ／ＧＰＳコンパス１３を備え、ＩＮＳ用のセンサ（以下、これをＩＮＳセンサと呼ぶ）１１として車輪用エンコーダ１４、ジャイロセンサ１５及びステア角エンコーダ１６を備える。また測位装置２は、ＮＳＳセンサ１０及びＩＮＳセンサ１１に加えて、これらＮＳＳセンサ１０及びＩＮＳセンサ１１のセンサ出力に基づいて自己位置を推定する位置推定部１２を備える。

【0021】

ＧＰＳ／ＧＰＳコンパス１３は、ＧＰＳアンテナを２つ以上用い、位置だけでなく方位も計測するセンサであり、かかる計測により得られた車両の位置及び方位を位置・姿勢情報として位置推定部１２に送出する。

【0022】

車輪用エンコーダ１４は、ロータリーエンコーダから構成され、車両の車輪の回転角度に応じた出力をエンコーダ情報として位置推定部１２に送出する。またジャイロセンサ１５は、車両の角速度を検出し、検出結果を角速度情報として位置推定部１２に送出する。さらにステア角エンコーダ１６は、車両のステアリングの回転角度（以下、これをステア角と呼ぶ）を検出するエンコーダであり、検出結果をステア角情報として位置推定部１２に送出する。

【0023】

位置推定部１２は、ＣＰＵ（Central Processing Unit）及びメモリ等の情報処理資源を備えて構成され、車輪用エンコーダ１４から送信されるエンコーダ情報に基づき得られる車両の速度と、ジャイロセンサ１５から送信される角速度情報に基づき得られる車両の角速度と、ステア角エンコーダ１６から送信されるステア角情報に基づき得られる車両の移動軌道の曲率とを積算処理することにより、車両の位置及び姿勢を推定する。また位置推定部１２は、このようにして得られた車両の位置及び姿勢を、ＧＰＳ／ＧＰＳコンパス１３から送信される位置・姿勢情報に基づいて補正し、かくして得られた車両の位置及び姿勢を表す位置・姿勢情報を制御装置３に送出する。

【0024】

制御装置３は、行動計画部１７、車両制御部１８及びロボット駆動部１９を備えて構成される。本実施の形態においては、これら行動計画部１７、車両制御部１８及びロボット駆動部１９は、制御装置３を構成する図示しないＣＰＵが対応するプログラムを実行することにより具現化されるものとする。ただし、これら行動計画部１７、車両制御部１８及びロボット駆動部１９をそれぞれ別個のハードウェア構成としても良い。

【0025】

行動計画部１７は、測位装置２の位置推定部１２から与えられる位置・姿勢情報に基づいて予め設定された目標地点に向けた車両の軌道を生成する軌道生成部１７Ａを備えて構成され、軌道生成部１７Ａが生成した軌道に関する情報を軌道情報として車両制御部１８に送出する。

【0026】

車両制御部１８は、行動計画部１７から与えられる軌道情報に基づいて、車両が軌道生成部１７Ａにより生成された軌道上を移動するようにロボット駆動部を駆動するための指令値（目標値）を算出する指令値算出部１８Ａを備える。そして車両制御部１８は、指令値算出部１８Ａにより算出された指令値をロボット駆動部１９に送出する。

【0027】

ロボット駆動部１９は、例えばエンジン又はモータ等の車両の駆動源と、車両のステアリングを回転駆動するモータなどを備えて構成される。そしてロボット駆動部１９は、車両制御部１８から与えられる指令値に従った回転量又は回転角でエンジンやモータなどを駆動することにより、当該指令値に従った状態に車両を駆動する。

【0028】

（１−２）本実施の形態による自己位置推定方式
（１−２−１）概要
次に、測位装置２の位置推定部１２において実行される自己位置推定処理の方式（自己位置推定方式）について説明する。

【0029】

本実施の形態による自己位置推定方式は、ＮＳＳにより測定した自己の位置及び姿勢と、ＩＮＳにより求めた自己の位置及び姿勢とを比較して、ＮＳＳにマルチパスなどの影響による測定異常が発生しているか否かを判定する点を特徴の１つとしている。また本自己位置推定方式は、かかる判定によりＮＳＳに測定異常が発生していないとの判定結果を得た場合には、ＩＮＳで求めた車両の位置及び姿勢をＮＳＳの測定結果に基づいて修正する一方、測定異常が発生しているとの判定結果を得た場合には、ＩＮＳで求めた位置を、ＮＳＳに測定異常が発生していないとの判定結果を得た場合よりも少ない修正量で修正する点をもう１つの特徴としている。

【0030】

このような自己位置推定方式を実現するための手段として、本実施の形態においては、自己位置推定処理にカルマンフィルタ理論を導入する。すなわち測位装置２の位置推定部１２は、ＮＳＳセンサ１０及びＩＮＳセンサ１１のセンサ出力に基づき得られる観測値を用いてカルマンフィルタ理論により車両の位置及び姿勢を推定する。

【0031】

カルマンフィルタ理論では、位置、姿勢、速度及び角速度といった推定したいパラメータを内部状態とし、ＮＳＳセンサ１０及びＩＮＳセンサ１１のセンサ出力に基づくこれら位置、姿勢、速度及び角速度の観測値を用いて内部状態を順次更新することで内部状態を正しい値に収束させていく。

【0032】

この場合において、カルマンフィルタ理論では、推定値の誤差分散をも併せて算出するため、ＮＳＳによる位置及び姿勢の推定値が誤差範囲に入っているか否かを判定することによって、ＮＳＳに測定異常が発生しているか否かを判定することが可能となる。

【0033】

またＮＳＳに測定異常が発生している場合には、ＩＮＳで求めた位置を、ＮＳＳに測定異常が発生していないとの判定結果を得た場合よりも少ない修正量で修正することにより、この後、ＮＳＳにより計測した自己の位置及び姿勢と、ＩＮＳにより求めた自己の位置及び姿勢との間の誤差が拡大して、ＮＳＳの計測結果が採用されなくなるという事態が発生することを未然かつ有効に防止することができる。

【0034】

（１−２−２）カルマンフィルタ理論を利用した自己位置推定処理の内容
次に、カルマンフィルタ理論を利用した本実施の形態による自己位置推定処理の具体的な処理内容について説明する。

【0035】

図２は、所定の第１の更新周期（例えば0.02秒周期）で位置推定部１２により実行されるＩＮＳ処理の処理内容を示す。位置推定部は、この図２に示すＩＮＳ処理を実行することにより、各ＩＮＳセンサ１１のセンサ出力に基づいて、内部状態を第１の更新周期で更新する。

【0036】

実際上、位置推定部１２は、図２に示すＩＮＳ処理を開始すると、まず、現在時刻を取得し（ＳＰ１）、この後、車輪用エンコーダ１４、ジャイロセンサ１５及びステア角エンコーダ１６からそれぞれ送信されるエンコーダ情報、角速度情報及びステア角情報をそれぞれ順次取得する（ＳＰ２〜ＳＰ４）。続いて、位置推定部１２は、ステップＳＰ２〜ステップＳＰ４において取得したエンコーダ情報、角速度情報及びステア角情報に基づいて内部状態を更新し（ＳＰ５）、この後、ステップＳＰ１に戻る。位置推定部１２は、以上のＩＮＳ処理を第１の更新周期で繰り返し実行する。

【0037】

図３は、かかるＩＮＳ処理（図２）のステップＳＰ５において、位置推定部１２により実行される更新処理の具体的な処理内容を示す。

【0038】

位置推定部１２は、ＩＮＳ処理のステップＳＰ５に進むと、この更新処理を開始し、まず、保持している最新の内部状態に基づいて、各ＩＮＳセンサ１１（車輪用エンコーダ１４、ジャイロセンサ１５及びステア角エンコーダ１６）が対応するセンサ値を取得した時刻（以下、これを観測時刻と呼ぶ）における内部状態を予測する内部状態予測処理を実行する（ＳＰ１０）。

【0039】

続いて、位置推定部１２は、車輪用エンコーダ１４からのエンコーダ情報に基づき得られる車両の速度と、ジャイロセンサ１５からの角速度情報に基づき得られる車両の角速度と、ステア角エンコーダ１６からのステア角情報に基づき得られる車両の移動軌道の曲率との観測値の予測値をそれぞれ求める観測値予測処理を実行し（ＳＰ１１）、この後、ステップＳＰ１１において求めた観測値の予測値と、実際の観測値とに基づいて内部状態を修正する修正処理を実行する（ＳＰ１２）。そして位置推定部１２は、この後、ＩＮＳ処理に戻る。

【0040】

一方、図４は、上述のＩＮＳ処理（図２）と並行して所定の第２の更新周期（例えば１秒周期）で位置推定部１２により実行されるＮＳＳ処理の処理内容を示す。位置推定部１２は、この図４に示すＮＳＳ処理を実行することにより、ＮＳＳセンサ１０のセンサ出力に基づいて内部状態を更新する。

【0041】

実際上、位置推定部１２は、図４に示すＮＳＳ処理を開始すると、まず、現在時刻を取得し（ＳＰ２０）、この後、ＧＰＳ／ＧＰＳコンパス１３から与えられる位置・姿勢情報を取得する（ＳＰ２１）。続いて、位置推定部１２は、ステップＳＰ２１において取得した位置・姿勢情報に含まれる位置情報に基づいて内部状態を更新する更新処理を実行し（ＳＰ２２）、さらに、かかる位置・姿勢情報に含まれる姿勢情報（方位情報）に基づいて内部状態を更新する更新処理を実行した後（ＳＰ２３）、ステップＳＰ２０に戻る。位置推定部１２は、以上のＮＳＳ処理を第２の更新周期で繰り返し実行する。

【0042】

図５は、かかるＮＳＳ処理（図４）のステップＳＰ２２及びステップＳＰ２３において、位置推定部１２により実行される更新処理の具体的な処理内容を示す。

【0043】

位置推定部１２は、ＮＳＳ処理のステップＳＰ２２又はステップＳＰ２３に進むと、この更新処理を開始し、まず、保持している最新の内部状態に基づいて、ＧＰＳ／ＧＰＳコンパス１３が当該位置・姿勢を測位した時刻（観測時刻）における内部状態を予測する内部状態予測処理を実行する（ＳＰ３０）。

【0044】

続いて、位置推定部１２は、ＧＰＳ／ＧＰＳコンパス１３の観測値の予測値及び予測値の誤差範囲を求める観測値予測処理を実行する（ＳＰ３１）。この後、位置推定部１２は、ステップＳＰ３０で求めた車両の位置又は姿勢の観測値の予測値と、実際のＧＰＳ／ＧＰＳコンパス１３により観測された車両の位置又は姿勢の観測値とを比較（ステップＳＰ２２の更新処理では位置、ステップＳＰ２３の更新処理では姿勢を比較）し、これらの差がステップＳＰ３１において求めた予測値の誤差範囲を超えるか否かによってＮＳＳに測定異常が発生しているか否かを判定する異常判定処理を実行する（ＳＰ３２）。

【0045】

そして位置推定部１２は、かかる異常判定処理の処理結果に基づいて、内部状態を補正する（ＳＰ３３）。具体的に、位置推定部１２は、ステップＳＰ３２の異常判定処理の結果、観測値に異常がないと判定した場合には、ＮＳＳの測定結果を利用して内部状態を修正する一方、観測値に異常があると判定した場合には、観測値に異常がないと判定した場合の修正量よりも少ない修正量で内部状態を修正する。そして位置推定部１２は、この後、ＮＳＳ処理に戻る。

【0046】

（１−２−３）更新処理の詳細
（１−２−３−１）内部状態及びその分散・共分散
次に、上述した内部状態予測処理、観測値予測処理、異常判定処理及び修正処理の具体的な内容について説明する。なお、以下の説明において、「対象とするセンサ」とは、ＩＮＳ処理時には車輪用エンコーダ１４、ジャイロセンサ１５及びステア角エンコーダ１６を指し、ＮＳＳ処理（図４）のステップＳＰ２２の更新処理ではＧＰＳ／ＧＰＳコンパス１３のうちのＧＰＳセンサを指し、ＮＳＳ処理のステップＳＰ２３の更新処理ではＧＰＳ／ＧＰＳコンパス１３のうちのＧＰＳコンパスを指すものとする。

【0047】

まず、以下の説明において使用する車両の内部状態及びその分散・共分散について説明する。位置推定部１２は、車両の位置を（ｘ、ｙ）、姿勢をθ、速度をｖｘ、角速度をｗｚとして、これらのパラメータを次式

【数1】

で表される内部状態ベクトルＸ_ｔ（以下、これを単に内部状態Ｘ_ｔと呼ぶ）として管理している。

【0048】

また位置推定部１２は、内部状態Ｘ_ｔの推定値の誤差範囲の分散・共分散を表す行列式として、次式

【数2】

で表される分散・共分散行列ＣｏｖＸ_ｔ（以下、これを単に分散・共分散ＣｏｖＸ_ｔと呼ぶ）をメモリ内に保持している。（２）式における行列の対角項は、各パラメータの分散を表し、非対角項は、パラメータ間の共分散を表す。

【0049】

なお、車両の内部状態Ｘ_ｔ及び分散・共分散ＣｏｖＸ_ｔは、いずれも時間変化する値である。以下、内部状態Ｘ_ｔ及び分散・共分散ＣｏｖＸ_ｔを表す時刻をモデル時刻ｔとし、各パラメータは、モデル時刻がｔの時点の値を示すものとする。

【0050】

そして位置推定部１２は、ＧＰＳ／ＧＰＳコンパス１３から最初に与えられた位置・姿勢情報に基づいて、内部状態Ｘ_ｔを次式のように初期化する。

【数3】

【0051】

また位置推定部１２は、かかる内部状態Ｘ_ｔの初期化と併せて、内部状態Ｘ_ｔの分散・共分散ＣｏｖＸ_ｔを次式のように初期化する。

【数4】

【0052】

（３）式及び（４）式において、ｔは、ＧＰＳ／ＧＰＳコンパス１３が車両の位置及び姿勢を測定した時刻に設定される。また（４）式において、σinit_x〜σinit_wzは、ＧＰＳ／ＧＰＳコンパス１３から与えられる位置・姿勢情報の精度などを考慮して適宜決定される定数である。

【0053】

（１−２−３−２）内部状態予測処理
内部状態予測処理では、対象とするセンサのセンサ出力を取得した時刻（以下、これを観測時刻と呼ぶ）における内部状態Ｘ_ｔ及びその分散・共分散ＣｏｖＸ_ｔを予測する。この予測値は、観測時刻とモデル時刻との差分をΔｔ（観測時刻−モデル時刻）として、
次式のように求めることができる。

【数5】

【数6】

【0054】

なお、（６）式において、「Ａ」は、次式

【数7】

で表される行列であり、「Ｑ」は、次式

【数8】

で表される行列である。

【0055】

（１−２−３−３）観測値予測処理
観測値予測処理では、次式

【数9】

で表される観測方程式を、対象とするセンサごとに定義し、上述の内部状態予測処理により求めた内部状態Ｘ_ｔの予測値Ｘ_ｔ＋Δｔと、分散・共分散ＣｏｖＸ_ｔの分散・共分散ＣｏｖＸ_ｔ＋Δｔとを代入することにより、対象とするセンサの観測値を予測する。

【0056】

ここで（９）式において、Ｙ_ｔは観測値の予測値を表し、Ｘ_ｔ^＊は、修正処理前の内部状態（予測処理によって、ｔが観測時刻に一致しているものとする）を表す。同様に、以下、「＊」は修正処理前の値であることを表すものとする。

【0057】

また観測値予測処理では、対象とするセンサごとに、当該センサの観測値の予測値Ｙ_ｔの分散・共分散ＣｏｖＹ_ｔを次式

【数10】

により求める。なお、（１０）式において、Ｂは、次式

【数11】

で与えられる観測方程式の偏微分行列（以下、これを観測行列と呼ぶ）を表す。

【0058】

さらに観測値予測処理では、対象とするセンサごとに、次式

【数12】

によりセンサノイズを含んだ分散期待値Ｖ_ｔを求める。なお（１２）式において、Ｒは、センサ値にどの程度のノイズが含まれているかを表す観測ノイズである。

【0059】

本実施の形態の場合、ＧＰＳ／ＧＰＳコンパス１３に含まれるＧＰＳセンサについて、関数ｇ（Ｘ_ｔ^＊）、観測行列Ｂ及び観測ノイズＲは、それぞれ次式

【数13】

【数14】

【数15】

のように定義される。

【0060】

またＧＰＳ／ＧＰＳコンパス１３に含まれるＧＰＳコンパスについて、関数ｇ（Ｘ_ｔ^＊）、観測行列Ｂ及び観測ノイズＲは、それぞれ次式

【数16】

【数17】

【数18】

のように定義される。

【0061】

さらに車輪用エンコーダ１４及びジャイロセンサ１５について、関数ｇ（Ｘ_ｔ^＊）、観測行列Ｂ及び観測ノイズＲは、それぞれ次式

【数19】

【数20】

【数21】

のように定義され、ステア角エンコーダ１６について、関数ｇ（Ｘ_ｔ^＊）、観測行列Ｂ及び観測ノイズＲは、それぞれ次式

【数22】

【数23】

【数24】

のように定義される。

【0062】

なお、車輪用エンコーダ１４からのエンコーダ情報に基づき得られた修正前の車両の速度の予測値ｖｘ^＊が「０」のときは、ステア角の予測値を算出できないため、ステア角エンコーダの観測値による修正処理を実行しない。またσgps_x〜σ_κは、各センサの精度（ばらつき）を考慮して決定する。

【0063】

従って、位置推定部１２は、ＩＮＳ処理（図２）の観測予測処理では、（１９）式〜（２４）式により、車輪用エンコーダ１４からのエンコーダ情報に基づき得られる速度ｖｘの観測値の予測値Ｙ_ｔと、ジャイロセンサ１５からの角速度情報に基づき得られる角速度ｗｚの観測値の予測値Ｙ_ｔと、ステア角エンコーダ１６からのステア角情報に基づき得られる曲率κの観測値の予測値Ｙ_ｔとを求める。

【0064】

また位置推定部１２は、ＮＳＳ処理（図４）のステップＳＰ２２の更新処理内の観測予測処理では、（１３）式〜（１５）式により、ＧＰＳ／ＧＰＳコンパス１３からの位置・姿勢情報に基づき得られる位置（ｘ、ｙ）の観測値の予測値Ｙ_ｔを求め、ＮＳＳ処理のステップＳＰ２３の更新処理内の観測予測処理では、（１６）式〜（１８）式により、ＧＰＳ／ＧＰＳコンパス１３からの位置・姿勢情報に基づき得られる姿勢θの観測値の予測値Ｙ_ｔを求めることになる。

【0065】

（１−２−３−４）異常判定処理
異常判定処理は、上述のようにＮＳＳ処理においてのみ実行される処理である。この異常判定処理では、マルチパスの影響等による測定異常がＧＰＳ／ＧＰＳコンパス１３に発生しているか否かを判定する。

【0066】

ここで、かかる異常判定の判定方法としては、予測値及び観測値の差分の絶対値を評価する第１の異常判定方法と、マハラノビス距離の大きさを評価する第２の異常判定方法と、内部状態Ｘ_ｔの修正量Ｘ_Ｃｏｒｒの大きさを評価する第３の異常判定方法とが考えられる。

【0067】

このうち第１の異常判定方法は、（１３）式により算出される車両の位置の予測値Ｙ_ｔと、実際の車両の位置の観測値Ｙ_ｓとの差分の絶対値（ＮＳＳ処理のステップＳＰ２２の場合）、又は（１６）式により算出される車両の姿勢の予測値Ｙ_ｔと、実際の車両の姿勢の観測値Ｙ_ｓとの差分の絶対値（ＮＳＳ処理のステップＳＰ２３の場合）が、予め位置又は姿勢について設定された閾値よりも大きいときに、ＧＰＳ／ＧＰＳコンパス１３に測定異常が発生していると判定する方法である。

【0068】

図６は、ＧＰＳ／ＧＰＳコンパス１３に測定異常が発生しているか否かを判定する判定方法として、第１の異常判定方法を採用した場合の位置推定部１２の具体的な処理手順を示す。

【0069】

この場合、位置推定部１２は、この図６に示す処理手順に従って、まず、次式

【数25】

により、車両の位置又は姿勢について、（１３）式又は（１６）式により算出される予測値Ｙ_ｔと、実際の観測値Ｙ_ｓとの差分（以下、これを観測残差と呼ぶ）Ｅｙを算出する（ＳＰ４０）。

【0070】

続いて、位置推定部１２は、（２５）式により算出した観測残差Ｅｙについて、次式

【数26】

のように、観測残差Ｅｙの絶対値が予め定められた閾値（Th_Ey）よりも大きいか否かを判断する（ＳＰ４１）。

【0071】

そして位置推定部１２は、かかる判断において否定結果を得た場合には、通常の修正処理を実行すべきと判定し（ＳＰ４２）、かかる判断において肯定結果を得た場合には、異常時の修正処理を実行すべきと判断する（ＳＰ４３）。

【0072】

一方、第２の異常判定方法は、マハラノビス距離が大きいときに、ＧＰＳ／ＧＰＳコンパス１３に測定異常が発生していると判定する方法である。

【0073】

車両の運動誤差やセンサの測定誤差がすべて正規分布であれば、観測残差Ｅｙも平均「０」、分散期待値Ｖ_ｔの正規分布となる。従って、誤差（観測残差Ｅｙ）の二乗を誤差分散で割った次式

【数27】

により算出されるマハラノビス距離Ｍは、99.7％の確率で３以下となるため（正規分布誤差が３σ範囲に入る確率が99.7％であるため）、これを超える場合は異常値であると判定することができる。

【0074】

そこで、本実施の形態においては、ＮＳＳ処理のステップＳＰ２２では、次式

【数28】

を満たすか否かに基づいて、またＮＳＳ処理のステップＳＰ２３では、次式

【数29】

を満たすか否かに基づいて、ＧＰＳ／ＧＰＳコンパス１３に測定異常が発生しているか否かを判定する。

【0075】

なお（２８）式において、Th_gxyは、マハラノビス距離Ｍについて予め定められた位置の閾値を表し、（２９）式においてTh_gthは、マハラノビス距離Ｍについて予め定められた姿勢の閾値を表す。これらの閾値Th_gxy及びTh_gthは、実際の実験データを参考に決定される閾値であるが、上述のようにマハラノビス距離Ｍは、99.7％の確率で３以下となるため、３〜４の範囲で決定されることになる。

【0076】

図７は、ＧＰＳ／ＧＰＳコンパス１３に測定異常が発生しているか否かを判定する判定方法として、この第２の異常判定方法を採用した場合の位置推定部１２の具体的な処理手順を示す。

【0077】

この場合、位置推定部１２は、この図７に示す処理手順に従って、まず、車両の位置について、（１３）式により算出される予測値Ｙ_ｔと、実際の観測値Ｙ_ｓとの観測残差Ｅｙを（２５）式により算出する（ＳＰ５０）。

【0078】

続いて、位置推定部１２は、マハラノビス距離Ｍを（２７）式により算出し（ＳＰ５１）、この後、ＮＳＳ処理のステップＳＰ２２では、位置について算出したマハラノビス距離Ｍが予め設定された位置についての閾値Th_gxyよりも小さいか否かを判断し、ＮＳＳ処理のステップＳＰ２３では、姿勢について算出したマハラノビス距離Ｍが予め設定された姿勢についての閾値Th_gthよりも小さいか否かを判断する（ＳＰ５２）。

【0079】

そして位置推定部１２は、かかる判断において否定結果を得た場合には、通常の修正処理を実行すべきと判定し（ＳＰ５３）、かかる判断において肯定結果を得た場合には、異常時の修正処理を実行すべきと判断する（ＳＰ５４）。

【0080】

他方、第３の異常判定方法は、異常判定の際にマハラノビス距離Ｍでなく、次式

【数30】

で表される内部状態Ｘ_ｔの修正量Ｘ_Ｃｏｒｒを利用して、ＧＰＳ／ＧＰＳコンパス１３に測定異常が発生しているか否かを判定する方法である。

【0081】

なお（３０）式において、Ｋは、次式

【数31】

により算出されるカルマンゲインを表す。

【0082】

この第３の異常判定方法を採用する場合、ＮＳＳ処理のステップＳＰ２２では、修正量Ｘ_Ｃｏｒｒの各成分（X_Corr_x、X_Corr_y、X_Corr_θ、X_Corr_vx、X_Corr_wz及びX_Corr_κ）のうち、位置に関する成分X_Corr_x及びX_Corr_yを利用して、次式

【数32】

を満たすか否かに基づいて、またＮＳＳ処理のステップＳＰ２３では、修正量Ｘ_Ｃｏｒｒの各成分のうち、姿勢に関する成分X_Corr_θを利用して、次式

【数33】

を満たすか否かに基づいて、ＧＰＳ／ＧＰＳコンパス１３に測定異常が発生しているか否かを判定する。

【0083】

なお（３２）式において、Th_gxyは、評価値Ｖ_Ｃｏｒｒについて予め定められた位置の閾値を表し、（３３）式においてTh_gthは、評価値Ｖ_Ｃｏｒｒについて予め定められた姿勢の閾値を表す。

【0084】

図８は、ＧＰＳ／ＧＰＳコンパス１３に測定異常が発生しているか否かを判定する判定方法として、この第３の異常判定方法が採用された場合の位置推定部１２の具体的な処理手順を示す。

【0085】

この場合、位置推定部１２は、この図８に示す処理手順に従って、まず、車両の位置について、（１３）式により算出される予測値Ｙ_ｔと、実際の観測値Ｙ_ｓとの観測残差Ｅｙを（２５）式により算出する（ＳＰ６０）。

【0086】

続いて、位置推定部１２は、（３１）式によりカルマンゲインＫを算出し（ＳＰ６１）、この後、（３０）式で表される修正量Ｘ_Ｃｏｒｒを算出す（ＳＰ６２）。そして位置推定部１２は、この後、ＮＳＳ処理のステップＳＰ２２では、ステップＳＰ６２において算出した修正量Ｘ_Ｃｏｒｒのうちの位置に関する成分X_Corr_x及びX_Corr_yを利用して、（３２）式を満たすか否かを判断し、ＮＳＳ処理のステップＳＰ２３では、ステップＳＰ６２において算出した修正量Ｘ_Ｃｏｒｒのうちの姿勢に関する成分X_Corr_θを利用して、（３３）式を満たすか否かを判断する（ＳＰ６３）。

【0087】

そして位置推定部１２は、かかる判断において否定結果を得た場合には、通常の修正処理を実行すべきと判定し（ＳＰ６４）、かかる判断において肯定結果を得た場合には、異常時の修正処理を実行すべきと判断する（ＳＰ６５）。

【0088】

（１−２−３−５）修正処理
（１−２−３−５−１）通常の修正処理
位置推定部１２は、上述の異常判定処理において、通常の修正処理を実行すべきと判定した場合、次式

【数34】

により内部状態Ｘ_ｔを修正すると共に、次式

【数35】

により内部状態Ｘ_ｔの分散・共分散ＣｏｖＸ_ｔを修正する。なお、（３４）式及び（３５）式において、Ｋは、上述の（３１）式により算出されるカルマンゲインを表す。

【0089】

図９は、上述の異常判定処理において修正処理として通常処理が選択された場合に位置推定部１２により実行される修正処理（以下、これを通常修正処理と呼ぶ）の具体的な処理手順を示す。

【0090】

この場合、位置推定部１２は、まず、（３１）の演算を実行することによりカルマンゲインＫを算出する（ＳＰ７０）。続いて、位置推定部１２は、ステップＳＰ７１において算出したカルマンゲインＫを利用して（３４）式の演算を実行することにより、新たな内部状態Ｘ_ｔを算出し、そのとき保持している内部状態Ｘ_ｔを、このとき算出した新たな内部状態Ｘ_ｔに置き換えるようにして修正する（ＳＰ７１）。

【0091】

続いて、位置推定部１２は、ステップＳＰ７１において算出したカルマンゲインＫを利用して（３６）式の演算を実行することにより、新たな内部状態Ｘ_ｔの分散・共分散ＣｏｖＸ_ｔを算出し（ＳＰ７２）、そのとき保持している分散・共分散ＣｏｖＸ_ｔを、このとき算出した新たな分散・共分散ＣｏｖＸ_ｔに置き換えるようにして修正する。そして位置推定部１２は、この後、この通常修正処理を終了する。

【0092】

（１−２−３−５−２）異常時修正処理
一方、上述の異常判定処理において、異常時の修正処理を実行すべきと判定した場合には、通常修正処理時よりも少ない修正量で内部状態Ｘ_ｔ及びその分散・共分散ＣｏｖＸ_ｔを修正する。

【0093】

ここで、通常修正処理時よりも少ない修正量で内部状態Ｘ_ｔ及びその分散・共分散ＣｏｖＸ_ｔを修正する方法としては、観測ノイズを切り替える第１の異常時修正方法と、重み付けにより修正を行う第２の異常時修正方法と、修正量の上限処理を行う第３の異常時修正方法が考えられる。

【0094】

このうち第１の異常時修正方法は、ＧＰＳ／ＧＰＳコンパス１３から出力される位置・姿勢情報の観測ノイズＲを通常修正処理時よりも大きな値に切り替える方法である。例えば、ＮＳＳ処理（図４）のステップＳＰ２２において実行した更新処理のステップＳＰ３２（図５）の異常判定処理で異常判定を得た場合において、通常修正処理時における（１５）式の「σgsp_x」及び「σgsp_y」の値がいずれも0.3〔m〕に設定されている場合には、これら「σgsp_x」及び「σgsp_y」の値をいずれもその10倍の３〔m〕に切り替える。また、ＮＳＳ処理のステップＳＰ２３において実行した更新処理のステップＳＰ３２の異常判定処理で異常判定を得た場合において、通常修正処理時における（１８）式の「σgsp_θ」の値が15〔度〕に設定されている場合には、当該「σgsp_θ」の値をその10倍の150〔度〕に切り替える。

【0095】

このようにＧＰＳ／ＧＰＳコンパス１３から出力される位置・姿勢情報の観測ノイズＲを大きな値に切り替えることによって、（３１）式で表されるカルマンゲインＫを低減させることができ、結果として通常修正処理時よりも少ない修正量で内部状態Ｘ_ｔ及びその分散・共分散ＣｏｖＸ_ｔを修正することができる。

【0096】

図１０は、異常判定処理時にＧＰＳ／ＧＰＳコンパス１３に測定異常が発生していると判定されたときの修正方法として、この第１の異常時修正方法が採用された場合の位置推定部１２の具体的な処理手順を示す。

【0097】

この場合、位置推定部１２は、この図１０に示す処理手順に従って、まず、観測ノイズＲを予め設定された異常時修正処理時の値（通常修正処理時よりも大きな値）に変更した上で、（１２）式により観測残差Ｅｙの分散期待値Ｖ_ｔを再計算する（ＳＰ８０）。

【0098】

続いて、位置推定部１２は、ステップＳＰ８０において算出した観測残差Ｅｙの分散期待値Ｖ_ｔを利用して、（３１）式によりカルマンゲインＫを算出する（ＳＰ８１）。

【0099】

次いで、位置推定部１２は、ステップＳＰ８１において算出したカルマンゲインＫを用いて（３４）式により内部状態Ｘ_ｔを算出し、保持している内部状態Ｘ_ｔを、このとき算出した新たな内部状態Ｘ_ｔに置き換えるようにして修正する（ＳＰ８２）。

【0100】

また位置推定部１２は、ステップＳＰ８１において算出したカルマンゲインＫを用いて（３５）式により内部状態Ｘ_ｔの分散・共分散ＣｏｖＸ_ｔを算出し、保持している分散・共分散ＣｏｖＸ_ｔを、このとき算出した新たな分散・共分散ＣｏｖＸ_ｔに置き換えるようにして修正する（ＳＰ８３）。

【0101】

一方、第２の異常時修正方法は、内部状態Ｘ_ｔについては、次式

【数36】

のように、また分散・共分散ＣｏｖＸ_ｔについては次式

【数37】

のように、修正量（Ｋ×Ｅｙ又は−Ｋ×Ｂ×ＣｏｖＸ_ｔ^＊）をマハラノビス距離Ｍで除算する方法である。

【0102】

ＮＳＳの測定異常の程度が大きければ大きいほどマハラノビス距離Ｍも大きくなるため、（３５）式及び（３６）式のように修正量をマハラノビス距離Ｍで除算することにより、通常修正処理時よりも少ない修正量で内部状態Ｘ_ｔ及び分散・共分散ＣｏｖＸ_ｔを修正することができる。

【0103】

図１１は、異常判定処理時にＧＰＳ／ＧＰＳコンパス１３に測定異常が発生していると判定されたときの修正方法として、この第２の異常時修正方法が採用された場合の位置推定部１２の具体的な処理手順を示す。

【0104】

この場合、位置推定部１２は、この図１１に示す処理手順に従って、まず、通常時のカルマンゲインＫを（３１）式により算出すると共に（ＳＰ９０）、算出したカルマンゲインＫを（２７）式で与えられるマハラノビス距離Ｍで除算した係数を算出する（ＳＰ９１）。

【0105】

続いて、位置推定部１２は、ステップＳＰ９１において算出した係数を用いて（３４）式により内部状態Ｘ_ｔを算出し、保持している内部状態Ｘ_ｔを、このとき算出した新たな内部状態Ｘ_ｔに置き換えるようにして修正する（ＳＰ９２）。

【0106】

また位置推定部１２は、ステップＳＰ９１において算出した係数を用いて（３５）式により内部状態Ｘ_ｔの分散・共分散ＣｏｖＸ_ｔを算出し、保持している分散・共分散ＣｏｖＸ_ｔを、このとき算出した新たな分散・共分散ＣｏｖＸ_ｔに置き換えるようにして修正する（ＳＰ９３）。

【0107】

他方、第３の異常時修正方法は、内部状態Ｘ_ｔについては、次式

【数38】

のように、また分散・共分散ＣｏｖＸ_ｔについては次式

【数39】

のように修正する方法である。なお、（３８）式及び（３９）式において、Ｔｈは、システムが許容できる修正量の上限を表し、Ｘ_Ｃｏｒｒは、通常の修正処理における修正量を表す。

【0108】

この第３の異常時修正方法によれば、内部状態Ｘ_ｔの修正量を、予め設定された閾値以下に上限処理することができるという効果を得ることができる。

【0109】

図１２は、異常判定処理時にＧＰＳ／ＧＰＳコンパス１３に測定異常が発生していると判定されたときの修正方法として、この第３の異常時修正方法が採用された場合の位置推定部１２の具体的な処理手順を示す。

【0110】

この場合、位置推定部１２は、この図１２に示す処理手順に従って、まず、カルマンゲインＫを（３１）式により算出し（ＳＰ１００）、算出したカルマンゲインＫを用いて（３０）式により内部状態Ｘ_ｔの修正量Ｘ_Ｃｏｒｒを算出する（ＳＰ１０１）。

【0111】

なお、例えば異常判定処理における異常判定方法として上述した第３の異常判定方法を採用している場合には、異常判定処理の段階で既にカルマンゲインＫ及び修正量Ｘ_Ｃｏｒｒを算出しているため（図８のステップＳＰ６１及びステップＳＰ６２）、ステップＳＰ１００及びステップＳＰ１０１の処理を省略することができる。

【0112】

続いて、位置推定部１２は、次式

【数40】

により、修正量Ｘ_Ｃｏｒｒを減らすための係数αを算出する（ＳＰ１０２）。

【0113】

次いで、位置推定部１２は、ステップＳＰ１００において算出したカルマンゲインＫと、ステップＳＰ１０２において算出した係数αとを用いて（３８）式により内部状態Ｘ_ｔを算出し、保持している内部状態Ｘ_ｔを、このとき算出した新たな内部状態Ｘ_ｔに置き換えるようにして修正する（ＳＰ１０３）。

【0114】

また位置推定部１２は、ステップＳＰ１００において算出したカルマンゲインＫと、ステップＳＰ１０２において算出した係数αとを用いて（３９）式により内部状態Ｘ_ｔの分散・共分散ＣｏｖＸ_ｔを算出し、保持している分散・共分散ＣｏｖＸ_ｔを、このとき算出した新たな分散・共分散ＣｏｖＸ_ｔに置き換えるようにして修正する（ＳＰ１０４）。

【0115】

（１−３）本実施の形態の効果
以上の車両制御システム１によれば、ＩＮＳによる測位結果が常にＮＳＳの測定結果に基づいて修正されるため、最終的にＩＮＳによる測位結果がＮＳＳの測位結果に近づくよう修正される。従って、自己位置の推定結果としてＩＮＳによる測位結果を採用した場合に、その後、ＮＳＳによる測位結果が利用されなくなって、自己位置の推定結果と、現実の位置との間の誤差が拡大するという事態が発生するのを未然かつ有効に防止することができる。

【0116】

また本車両制御システム１によれば、自己が存在する場所の周囲の地理データを必要とせず、少ないデータ量及び計算量で自己位置を推定することができる。

【0117】

かくするにつき、本車両制御システム１によれば、少ないデータ量及び計算量で精度良く自己位置を推定することができる。

【0118】

（２）第２の実施の形態
図１において、２０は全体として第２の実施の形態による車両制御システムを示す。この車両制御システム２０は、測位装置２１の位置推定部２２により実行される更新処理の処理内容が異なる点を除いて第１の実施の形態の車両制御システム１と同様に構成されている。

【0119】

図１３は、ＮＳＳ処理（図４）のステップＳＰ２２及びステップＳＰ２３において、図５について上述した更新処理に代えて実行される本実施の形態による更新処理の処理内容を示す。

【0120】

位置推定部２２は、ＮＳＳ処理のステップＳＰ２２及びステップＳＰ２３に進むと、この更新処理を開始し、まず、ステップＳＰ１１０の内部状態予測処理及びステップＳＰ１１１の観測値予測処理を、図５について上述した第１の実施の形態の更新処理の内部状態予測処理及び観測値予測処理（ＳＰ３０、ＳＰ３１）と同様に処理する。

【0121】

続いて、位置推定部２２は、ＮＳＳに測定異常が発生しているか否かの判定を行うと共に、ＮＳＳに測定異常が発生しているときには内部状態及びその分散・共分散を修正する異常判定及び修正処理を実行する（ＳＰ１１２）。

【0122】

図１４は、かかる更新処理のステップＳＰ１１２において位置推定部２２により実行される異常判定及び修正処理の具体的な処理内容を示す。

【0123】

位置推定部２２は、更新処理（図１３）のステップＳＰ１１２に進むと、この図１４に示す異常判定処理及び修正処理を開始し、まず、図７について上述した第２の異常判定処理のステップＳＰ５０〜ステップＳＰ５２と同様にステップＳＰ１２０〜ステップＳＰ１２２を処理することにより、ＮＳＳ処理のステップＳＰ２２では、位置について算出したマハラノビス距離Ｍが予め設定された位置についての閾値Th_gxyよりも小さいか否かを判断し、ＮＳＳ処理のステップＳＰ２３では、姿勢について算出したマハラノビス距離Ｍが予め設定された姿勢についての閾値Th_gthよりも小さいか否かを判断する（ＳＰ１２０〜ＳＰ１２２）。

【0124】

そして位置推定部２２は、この判定で肯定結果を得ると、図１０について上述した第１の異常時修正処理のステップＳＰ８０及びステップＳＰ８１と同様にして、観測ノイズＲを予め設定された異常時修正処理時の値（通常修正処理時よりも大きな値）に変更した上で、（１２）式により観測残差Ｅｙの分散期待値Ｖ_ｔを再計算し（ＳＰ１２６）、さらにこの分散期待値Ｖ_ｔを利用して、（３１）式を用いてカルマンゲインＫを算出する（ＳＰ１２７）。

【0125】

続いて位置推定部２２は、図１１について上述した第２の異常時修正処理のステップＳＰ９１と同様に、ステップＳＰ１２７において算出したカルマンゲインＫを（２７）式で与えられるマハラノビス距離Ｍで除算した新たなカルマンゲインＫを算出する（ＳＰ１２８）。

【0126】

次いで、位置推定部２２は、図８について上述した第３の異常時修正処理のステップＳＰ６２と同様にして、ステップＳＰ１２８において算出した新たなカルマンゲインＫを用いて修正量Ｘ_Ｃｏｒｒを算出する（ＳＰ１２９）。

【0127】

この後、位置推定部２２は、ＮＳＳ処理のステップＳＰ２２では、ステップＳＰ６２において算出した修正量Ｘ_Ｃｏｒｒのうちの位置に関する成分X_Corr_x及びX_Corr_yを利用して、（３２）式を満たすか否かを判断し、ＮＳＳ処理のステップＳＰ２３では、ステップＳＰ６２において算出した修正量Ｘ_Ｃｏｒｒのうちの姿勢に関する成分X_Corr_θを利用して、（３３）式を満たすか否かを判断する（ＳＰ１３０）。

【0128】

そして位置推定部２２は、この判断で肯定結果を得ると、図１２について上述した第３の異常時修正方法のステップＳＰ１０２〜ステップＳＰ１０４と同様にして内部状態Ｘ_ｔ及びその分散・共分散ＣｏｖＸ_ｔを修正し（ＳＰ１３１〜ＳＰ１３３）、この後、この更新処理を終了する。

【0129】

これに対して位置推定部２２は、ステップＳＰ１３０の判断で否定結果を得ると、ステップＳＰ１２８において算出した新たなカルマンゲインＫを用いて上述の（３４）式により新たな内部状態Ｘ_ｔを算出すると共に、保持している内部状態Ｘ_ｔをこのとき算出した内部状態Ｘ_ｔに置き換えることにより修正する（ＳＰ１３４）。

【0130】

また位置推定部２２は、ステップＳＰ１２８において算出した新たなカルマンゲインＫを用いて上述の（３５）式により新たな分散・共分散ＣｏｖＸ_ｔを算出すると共に、保持している分散・共分散ＣｏｖＸ_ｔをこのとき算出した分散・共分散ＣｏｖＸ_ｔに置き換えることにより修正する（ＳＰ１３５）。そして位置推定部２２は、この後、この更新処理を終了する。

【0131】

以上の本実施の形態の車両制御システム２０によれば、異常判定処理においてＮＳＳの測位結果に異常が発生していると判定された場合に、観測ノイズＲを大きな値に変更する修正処理を実行した上で、再度、異常判定処理を行い、この異常判定処理において異常を検出した場合に、さらに修正処理を実行した上で内部状態Ｘ_ｔ及びその分散・共分散ＣｏｖＸ_ｔを修正するようにしているため、より精度良く自己位置を推定することができる。

【0132】

（３）他の実施の形態
なお上述の第１及び第２の実施の形態においては、本発明を車両の位置を推定する測位装置に適用するようにした場合について述べたが、本発明はこれに限らず、車両以外の移動体の位置を推定するこの他種々の自己位置推定装置に広く適用することができる。

【0133】

また上述の第１及び第２の実施の形態においては、ＮＳＳ処理（図４）において、位置及び姿勢のそれぞれについて計測結果に異常が発生しているか否かの異常判定を行うようにした場合について述べたが、本発明はこれに限らず、位置及び姿勢のいずれか一方のみで異常判定を行うようにしても良い。

【0134】

さらに上述の第１及び第２の実施の形態においては、ＩＮＳセンサ１１として車輪用エンコーダ１４、ジャイロセンサ１５及びステア角エンコーダ１６の３つの内界センサを適用するようにした場合について述べたが、本発明はこれに限らず、これら３つの内界センサすべてを利用する必要はない。例えば車輪用エンコーダ１４のみを用いて、左右両側の車輪の回転量から速度及び角速度を測定することによりジャイロセンサ１５を省略するようにしても良く、またジャイロセンサ１５のみを用いて、加速度及び角速度を検出し、これらに基づいて内部状態の修正を行うようにしても良い。ただし、内界センサの数（種類）を増やしたり、精度を上げることによって、マハラノビス距離Ｍの算出時の分散期待値Ｖ_ｔが小さな値となるため、異常判定を行い易くなるという利点がある。

【0135】

さらに上述の第１及び第２の実施の形態においては、ＩＮＳセンサ１１として車輪用エンコーダ１４、ジャイロセンサ１５及びステア角エンコーダ１６を適用するようにした場合について述べたが、本発明はこれに限らず、例えば図１に示すように、ＩＮＳセンサ１１として加速度センサ３０を追加するようにしても良い。

【産業上の利用可能性】

【0136】

本発明は、車両の位置を推定する測位装置に適用することができる。

【符号の説明】

【0137】

１，２０……車両制御システム、２，２１……測位装置、３……制御装置、１０……ＮＳＳセンサ、１１……ＩＮＳセンサ、１２，２２……位置推定部、１３……ＧＰＳ／ＧＰＳコンパス、１４……車輪用エンコーダ、１５……ジャイロセンサ、１６……ステア角エンコーダ。

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】

【図13】

【図14】