2025-176719 自己位置推定方法及び自己位置推定装置

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

(12)【公報種別】公開特許公報(A)

(11)【公開番号】P2025176719

(43)【公開日】2025-12-05

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

(51)【国際特許分類】

   G01S 19/48 20100101AFI20251128BHJP

   G01S 19/53 20100101ALI20251128BHJP

   G01C 21/28 20060101ALI20251128BHJP

【ＦＩ】

G01S19/48

G01S19/53

G01C21/28

【審査請求】未請求

【請求項の数】9

【出願形態】ＯＬ

(21)【出願番号】P 2022160259

(22)【出願日】2022-10-04

(71)【出願人】

【識別番号】000003997

【氏名又は名称】日産自動車株式会社

(74)【代理人】

【識別番号】100083806

【弁理士】

【氏名又は名称】三好 秀和

(74)【代理人】

【識別番号】100101247

【弁理士】

【氏名又は名称】高橋 俊一

(74)【代理人】

【識別番号】100095500

【弁理士】

【氏名又は名称】伊藤 正和

(74)【代理人】

【識別番号】100098327

【弁理士】

【氏名又は名称】高松 俊雄

(72)【発明者】

【氏名】酒井 佳奈子

(72)【発明者】

【氏名】土谷 千加夫

【テーマコード（参考）】

2F129

5J062

【Ｆターム（参考）】

2F129AA01

2F129AA11

2F129AA14

2F129BB03

2F129BB22

2F129BB26

5J062AA09

5J062AA11

5J062BB01

5J062CC07

5J062DD23

5J062DD24

5J062FF01

5J062FF04

(57)【要約】

【課題】人工衛星から受信する信号から測位された観測値の信頼度を正確に検出する。
【解決手段】自己位置推定方法は、人工衛星から受信する信号から移動体の位置及び角度を検出対象の観測値として計測し（Ｓ０１）、所定時刻における検出対象の観測値から所定距離遡る走行軌跡、又は所定時刻における検出対象の観測値を所定時刻から所定時間遡る移動体の走行軌跡を、移動体の相対的な移動量に基づいて算出し（Ｓ０２）、算出された走行軌跡と走行軌跡に対応する区間における過去の観測値との乖離度、及び過去の観測値に関連付けて記憶されている信頼度に基づいて、所定時刻の検出対象の観測値の信頼度を検出し（Ｓ０３）、所定時刻の検出対象の観測値と検出された信頼度とを関連付けて記憶し（Ｓ０４）、信頼度が第１所定値以上の観測値に基づいて移動体の自己位置を推定する（Ｓ０５）。
【選択図】図２

【特許請求の範囲】

【請求項1】

人工衛星から受信する信号から移動体の位置及び角度を検出対象の観測値として計測する測位処理と、
所定時刻における前記検出対象の観測値から所定距離遡る走行軌跡、又は前記所定時刻における前記検出対象の観測値を前記所定時刻から所定時間遡る前記移動体の走行軌跡を、前記移動体の相対的な移動量に基づいて算出する走行軌跡算出処理と、
前記走行軌跡算出処理で算出された前記走行軌跡と前記走行軌跡に対応する区間における過去の観測値との乖離度、及び前記過去の観測値に関連付けて記憶されている信頼度に基づいて、前記所定時刻の検出対象の観測値の信頼度を検出する信頼度検出処理と、
前記所定時刻の検出対象の観測値と検出された信頼度とを関連付けて記憶する信頼度記憶処理と、
前記信頼度が第１所定値以上の観測値に基づいて前記移動体の自己位置を推定する自己位置推定処理と、
を有する自己位置推定方法。

【請求項2】

前記信頼度検出処理において、
第１基準値よりも高い信頼度が関連付けられた前記過去の観測値を抽出し、
抽出された前記過去の観測値と前記走行軌跡との前記乖離度が閾値以下か否かを判定し、
前記走行軌跡に対応する区間における前記過去の観測値の全体数に対して、前記乖離度が前記閾値以下と判定された前記過去の観測値の数が占める割合が大きい程、前記検出対象の観測値の信頼度を高く検出する
請求項１記載の自己位置推定方法。

【請求項3】

前記信頼度検出処理において、
前記乖離度の各々に対して、前記乖離度に対応する前記過去の観測値の信頼度に応じた重み付けをし、
重み付けして計算した前記乖離度の平均値が小さい程、前記検出対象の観測値の信頼度を高く検出する
請求項１記載の自己位置推定方法。

【請求項4】

前記信頼度検出処理において、前記走行軌跡に対応する区間における前記過去の観測値の、前記検出対象の観測値から遡る距離が長い程、又は前記検出対象の観測値を計測した時刻から遡る時間が長い程、前記閾値を大きく設定する
請求項２記載の自己位置推定方法。

【請求項5】

前記信頼度検出処理において、前記走行軌跡に対応する区間における前記過去の観測値の、前記検出対象の観測値から遡る距離が長い程、又は前記検出対象の観測値を計測した時刻から遡る時間が長い程、前記乖離度を短く算出する
請求項３記載の自己位置推定方法。

【請求項6】

前記測位処理において、前記移動体の位置及び角度を検出対象の観測位置及び検出対象の観測角度として計測し、
前記走行軌跡算出処理において、前記所定時刻の検出対象の観測角度から、前記移動体の角度の走行軌跡を前記移動体の相対的な移動量に基づいて算出し、
前記信頼度検出処理において、前記角度の走行軌跡と前記角度の走行軌跡に対応する区間における過去の観測角度との乖離度、及び前記過去の観測角度に関連付けている角度信頼度に基づいて、前記所定時刻の検出対象の観測角度の角度信頼度を検出し、
前記信頼度記憶処理において、前記所定時刻の検出対象の観測角度と検出された角度信頼度とを関連付けて記憶し、
前記自己位置推定処理において、前記角度信頼度が第３所定値以上の観測角度に基づいて前記移動体の角度を推定し、
前記走行軌跡算出処理において、前記所定時刻の検出対象の観測位置及び前記自己位置推定処理において推定された前記移動体の角度から、前記移動体の位置の走行軌跡を前記移動体の相対的な移動量に基づいて算出し、
前記信頼度検出処理において、前記位置の走行軌跡と前記位置の走行軌跡に対応する区間における過去の観測位置との乖離度、及び前記過去の観測位置に関連付けている位置信頼度に基づいて、前記所定時刻の検出対象の観測位置の位置信頼度を検出し、
前記信頼度記憶処理において、前記所定時刻の検出対象の観測位置と検出された位置信頼度とを関連付けて記憶し、
前記自己位置推定処理において、前記位置信頼度が第４所定値以上の観測位置及び推定された前記移動体の角度に基づいて前記移動体の位置を推定し、推定された前記移動体の角度及び前記移動体の位置を、前記移動体の自己位置として出力する
請求項１記載の自己位置推定方法。

【請求項7】

前記測位処理での前記観測値の測定精度が低いほど、前記走行軌跡算出処理において前記所定距離又は前記所定時間を長く設定する請求項１記載の自己位置推定方法。

【請求項8】

前記走行軌跡算出処理において、前記移動体の相対的な移動量の誤差が大きい程、前記所定距離又は前記所定時間を短く設定する請求項１記載の自己位置推定方法。

【請求項9】

人工衛星から受信する信号から移動体の位置及び角度を検出対象の観測値として計測する測位部と、
所定時刻における前記検出対象の観測値から所定距離遡る走行軌跡、又は前記所定時刻における前記検出対象の観測値を前記所定時刻から所定時間遡る前記移動体の走行軌跡を、前記移動体の相対的な移動量に基づいて算出する走行軌跡算出部と、
前記走行軌跡算出部で算出された前記走行軌跡と前記走行軌跡に対応する区間における過去の観測値との乖離度、及び前記過去の観測値に関連付けて記憶されている信頼度に基づいて、前記所定時刻の検出対象の観測値の信頼度を検出する信頼度検出部と、
前記所定時刻の検出対象の観測値と検出された信頼度とを関連付けて記憶する信頼度記憶部と、
前記信頼度が第１所定値以上の観測値に基づいて前記移動体の自己位置を推定する自己位置推定部と、
を有する自己位置推定装置。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、自己位置推定方法及び自己位置推定装置に関する。

【背景技術】

【0002】

特許文献１に開示された異常値判定装置は、慣性航法装置で検出された速度情報の累積誤差とＧＰＳ受信情報に基づく擬似距離誤差との差が所定範囲内の値となるように所定時間を決定する。そして、この所定時間内の各時刻における速度情報及び擬似距離に基づいて移動体の位置を推定し、推定した位置と擬似距離との残差に基づいて、擬似距離の異常を判定する。

【先行技術文献】

【特許文献】

【0003】

【特許文献1】特開２０１２－２０７９１９号公報

【発明の概要】

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

【0004】

しかし、異常判定の基準として、ＧＰＳ受信情報と慣性航法装置で検出された情報とから推定された移動体の位置を用いている。このため、異常判定の基準自体がＧＰＳ受信情報の外れ値の影響を受けて、擬似距離の異常を誤判定してしまう場合がある。

【0005】

本発明は、上記課題に鑑みてなされたものであり、その目的は、人工衛星から受信する信号から測位された観測値の信頼度を正確に検出することができる自己位置推定方法及び自己位置推定装置を提供することにある。

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

【0006】

上述した問題を解決するために、本発明の一態様に係る自己位置推定方法は、人工衛星から受信する信号から移動体の位置及び角度を検出対象の観測値として計測し、所定時刻における検出対象の観測値から所定距離遡る走行軌跡、又は所定時刻における検出対象の観測値を所定時刻から所定時間遡る移動体の走行軌跡を、移動体の相対的な移動量に基づいて算出し、算出された走行軌跡、走行軌跡に対応する区間における過去の観測値との乖離度、及び過去の観測値に関連付けて記憶されている信頼度に基づいて、所定時刻の検出対象の観測値の信頼度を検出し、所定時刻の検出対象の観測値と検出された信頼度とを関連付けて記憶し、信頼度が第１所定値以上の観測値に基づいて移動体の自己位置を推定する。

【発明の効果】

【0007】

本発明によれば、人工衛星から受信する信号から測位された観測値の信頼度を正確に検出することができる。

【図面の簡単な説明】

【0008】

【図1】図１は、第１～第３実施形態及びその変形例に係る自己位置推定装置１の構成を示すブロック図である。

【図2】図２は、図１の自己位置推定装置１を用いた自己位置推定方法であって、第１～第３実施形態及びその変形例に係る自己位置推定方法を示すフローチャートである。

【図3】図３は、図２のフローチャートにおけるステップＳ０３の具体的な処理例（第２実施形態）を示すフローチャートである。

【図4】図４は、図２のフローチャートにおけるステップＳ０３の具体的な処理例（第３実施形態）を示すフローチャートである。

【図5】図５は、第４実施形態及びその変形例に係る自己位置推定装置２の構成を示すブロック図である。

【図6】図６は、図５の自己位置推定装置２を用いた自己位置推定方法であって、第４実施形態及びその変形例に係る自己位置推定方法を示すフローチャートである。

【図7】図７は、検出対象の観測値としての車両の位置（Ｐｇｎ（ｔ））と、走行軌跡３１に対応する区間（ウィンドウサイズ）３２における過去の観測値の一例としての車両の位置（Ｐｇｎ（ｔ）、Ｐｇｎ（ｔ－１）、Ｐｇｎ（ｔ－２）、・・・）と、乖離度の一例としての距離（Ｄ（ｔ－１）、Ｄ（ｔ－２）、・・・）を示す概念図である。

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

【0009】

次に、図面を参照して、本発明の実施の形態を詳細に説明する。説明において、同一のものには同一符号を付して重複する説明を省略する。

【0010】

（第１実施形態）
図１を参照して、第１実施形態を含む複数の実施形態に係る自己位置推定装置１の構成を説明する。自己位置推定装置１は、測位部１１と、走行軌跡算出部１４と、信頼度検出部１５と、判定履歴記憶部１６（信頼度記憶部の一例）と、自己位置推定部１７と、を有する。実施形態では、移動体として車両を例示して説明するが、車両以外の船、飛行機、ロケットなど、車輪を有していない移動体にも適用可能である。

【0011】

測位部１１は、人工衛星から受信する信号から車両（移動体の一例）の位置及び角度を検出対象の観測値として計測する。測位部１１は、ＧＮＳＳ（Global Navigation Satellite System／全球測位衛星システム）を用いて車両の位置及角度を求める。このため、測位部１１は、ＧＮＳＳを構成する人工衛星から信号を受信する、例えばＧＰＳ（Global Positioning System／全地球測位システム）受信機を備え、更に、受信機が受信した受信信号からグローバルな座標系における車両の絶対位置（例えば、緯度、経度、高度）及び絶対角度（例えば、方位角、仰角）を演算する演算部を備える。「検出対象の観測値」とは、信頼度を検出する対象の観測値を意味し、例えば、測位部１１が繰り返し計測した複数の観測値の時系列の内の最新の観測値が、検出対象の観測値となる。

【0012】

走行軌跡算出部１４は、所定時刻における検出対象の観測値から所定距離遡る走行軌跡、又は検出対象の観測値を所定時刻から所定時間遡る車両の走行軌跡を、車両の相対的な移動量に基づいて算出する。例えば、走行軌跡算出部１４は、最新の観測値を起点として、起点から車両の進行方向とは逆方向に所定距離だけ遡る車両の走行軌跡、又は最新の観測値が計測された時刻（所定時刻）から過去に向かって経過時間遡る間の車両の走行軌跡を算出する。

【0013】

走行軌跡は、車両の相対的な移動量に基づいて算出することができる。この場合、自己位置推定装置１はＩＭＵ装置（Inertial Measurement Unit／慣性計測装置）１２を更に備えていても構わない。ＩＭＵ装置１２は、３次元の慣性運動（直行３軸方向の並進運動および回転運動）を検出する装置であり、並進運動を検出する加速度センサと、回転運動を検出する角速度(ジャイロ)センサと、並進運動の加速度を時間で積分して並進運動の速度を演算する演算部とを備える。観測値の信頼性の検出基準となる走行軌跡は、測位部１１が計測する観測値の影響を受けることが無くなり、観測値の外れ値による信頼性の誤判定を抑制することができる。

【0014】

具体的には、先ず、検出対象の観測値として位置と角度の情報が得られている。検出対象の観測値を起点として、起点から、検出対象の観測値を計測した時刻に最も近い時刻における速度と角速度を差し引くことにより、一つ前の時刻における位置と角度を推定する。次に、推定した位置と角度から、この推定値に対応する時刻に最も近い時刻における速度と角速度を差し引くことにより、二つ前の時刻における位置と角度情報を推定する。この差し引きを、起点から所定距離内又は経過時間内、繰り返すことで、位置及び角度の走行軌跡を算出する。以上説明したように、走行軌跡の算出において観測値は起点としてのみ用いられるため、観測値の信頼性の検出基準となる走行軌跡は、測位部１１が計測する観測値の影響を受けることが無くなり、観測値の外れ値による信頼性の誤判定を抑制することができる。換言すれば、車両の走行軌跡を車両の速度及び角速度のような車両の相対的な移動量に基づいて算出することにより、測位部１１が計測する信頼性の低い観測値の影響を受けることが無くなる。

【0015】

信頼度検出部１５は、走行軌跡算出部１４で算出された走行軌跡と走行軌跡に対応する区間における過去の観測値との乖離度、及び過去の観測値に関連付けて判定履歴記憶部１６に記憶されている信頼度に基づいて、所定時刻の検出対象の観測値の信頼度を検出する。

【0016】

具体的には、信頼度検出部１５は、走行軌跡に対応する区間内の各時刻における過去の観測値を判定履歴記憶部１６から抽出し、抽出された過去の観測値と走行軌跡との乖離度を算出する。例えば、観測値が車両の位置である場合、各時刻における車両の位置の走行軌跡と車両の位置の観測値（観測位置）とのユークリッド距離を、乖離度として算出する。一方、観測値が車両の角度である場合、各時刻における車両の角度の走行軌跡と車両の角度の観測値（観測角度）との角度差を、乖離度として算出する。「過去の観測値」とは、検出対象の観測値よりも過去に計測された観測値を意味する。「過去の観測値」は、信頼度検出部１５によって信頼度が検出された観測値に限っても構わない。信頼度が検出された過去の観測値は、後述するように、判定履歴記憶部１６に信頼度に紐づけて記憶されている。信頼度検出部１５が検出する「信頼度」は、「高い」又は「低い」のような二値分類であってもよいし、３つ以上の多値分類であっても構わない。

【0017】

判定履歴記憶部１６は、所定時刻の検出対象の観測値と信頼度検出部１５により検出された信頼度とを関連付けて記憶する。

【0018】

自己位置推定部１７は、信頼度が第１所定値以上の観測値に基づいて車両の自己位置を推定する。自己位置推定部１７は、既知の技術を用いて自己位置を推定することができる。例えば、観測値（車両の絶対位置と絶対角度）と車両の相対的な移動量（車速と角速度）に基づいて、カルマンフィルタなどの推定手法を用いて、尤もらしい車両の位置及び角度を車両の自己位置として推定する。ここで、観測値のうち低い信頼度が検出された観測値は、事前に排除するか、又は自己位置の推定処理への寄与度が小さくなるように設定することができる。

【0019】

自己位置推定装置１は、ＩＭＵ装置１２により検出された車両の速度及び角速度を示すデータと、測位部１１により計測された観測値を示すデータとを記憶する記憶部１３を更に有していてもよい。これらのデータは、走行軌跡算出部１４及び信頼度検出部１５によって利用される。

【0020】

図１に示す自己位置推定装置１のうち、記憶部１３と、走行軌跡算出部１４と、信頼度検出部１５と、判定履歴記憶部１６と、自己位置推定部１７とは、ＣＰＵ（中央処理装置）、メモリ、及び入出力部を備える汎用のコンピュータで実現可能である。マイクロコンピュータには、自己位置推定装置１として機能させるためのコンピュータプログラムがインストールされている。コンピュータプログラムを実行することにより、コンピュータは自己位置推定装置１が備える複数の情報処理回路（１４、１５、１７）として機能する。記憶部１３及び判定履歴記憶部１６は、メモリ又はマイクロコンピュータに接続されたハードディスクドライブなどの外部記憶装置により実現可能である。なおここでは、ソフトウェアによって自己位置推定装置１が備える複数の情報処理回路を実現する例を示すが、専用のハードウェアを用意して情報処理回路を構成することも可能である。また複数の情報処理回路を個別のハードウェアにより構成してもよい。

【0021】

図２及び図７を参照して、図１の自己位置推定装置１を用いた第１実施形態に係る自己位置推定方法を説明する。図２に示す動作フローは、予め定められた周期で、繰り返し実施される。図７は、検出対象の観測値としての車両の位置（Ｐｇｎ（ｔ））と、走行軌跡３１に対応する区間（ウィンドウサイズ）３２における過去の観測値の一例としての車両の位置（Ｐｇｎ（ｔ）、Ｐｇｎ（ｔ－１）、Ｐｇｎ（ｔ－２）、・・・）と、乖離度の一例としての距離（Ｄ（ｔ－１）、Ｄ（ｔ－２）、・・・）を示す概念図である。ステップＳ０１（測位処理）において、測位部１１は、人工衛星から受信する信号から車両の位置Ｐｇｎ（ｔ）及び角度を検出対象の観測値として計測する。ステップＳ０２（走行軌跡算出処理）に進み、走行軌跡算出部１４は、検出対象の観測値としての車両の位置Ｐｇｎ（ｔ）から所定距離遡る走行軌跡３１、又は検出対象の観測値を所定時刻から所定時間遡る車両の走行軌跡３１を、車両の相対的な移動量に基づいて算出する。ステップＳ０３（信頼度検出処理）に進み、信頼度検出部１５は、走行軌跡算出処理で算出された走行軌跡３１と走行軌跡３１に対応する区間（ウィンドウサイズ）３２内における過去の観測値との乖離度、及び過去の観測値に関連付けて記憶されている信頼度に基づいて、検出対象の観測値の信頼度を検出する。「乖離度」とは、例えば、走行軌跡３１と測位部１１により計測された車両の各位置（Ｐｇｎ（ｔ－１）、Ｐｇｎ（ｔ－２）、・・・）との距離である。距離とは、例えば、車両の各位置（Ｐｇｎ（ｔ－１）、Ｐｇｎ（ｔ－２）、・・・）と車両の各位置（Ｐｇｎ（ｔ－１）、Ｐｇｎ（ｔ－２）、・・・）に最も近い走行軌跡３１上の各地点（Ｐｏｄ（ｔ－１）、Ｐｏｄ（ｔ－２）、・・・）との距離（Ｄ（ｔ－１）、Ｄ（ｔ－２）、・・・）である。ステップＳ０４（信頼度記憶処理）に進み、判定履歴記憶部１６は、検出対象の観測値と検出された信頼度とを関連付けて記憶する。ステップＳ０５（自己位置推定処理）に進み、自己位置推定部１７は、信頼度が第１所定値以上の観測値に基づいて車両の自己位置を推定する。上記した動作フローを繰り返し実行する事により、信頼度が関連付けられた複数の観測値の時系列が求められ、判定履歴記憶部１６に過去の観測値として記憶される。この過去の観測値に関連付けた信頼度を用いて、例えば最新の観測値の信頼度を検出することができる。

【0022】

以上説明したように、図１の自己位置推定装置１及び図２の自己位置推定方法は、信頼度の判定基準として、ＧＮＳＳによる観測値に依存しない車両の走行軌跡を用いている。このため、ＧＮＳＳ観測値の外れ値による判定基準の精度悪化に起因する信頼度の誤判定を抑制できる。また、最新の観測値の信頼度を判定する際に、過去の観測値の信頼度を考慮している。このため、ＧＮＳＳ観測値に一定誤差が生じている場合であっても、正確な信頼度判定が可能となる。

【0023】

（第２実施形態）
第２実施形態では、図３を参照して、図１の信頼度検出部１５の具体的な動作例、及び図２のステップ０３（信頼度検出処理）の具体的な処理例を説明する。第２実施形態に係る自己位置推定装置１及び自己位置推定方法の全体は、図１及び図２と同じであり説明を省略する。以下に述べる全てのステップＳ３０１～Ｓ３１０の動作主体は、図１の信頼度検出部１５である。

【0024】

先ず、ステップＳ３０１において、信頼度検出部１５が備えるカウンタをゼロにリセットする。ステップＳ３０２に進み、ウィンドウサイズ内の過去の観測値を１つ選択する。「ウィンドウサイズ」とは、走行軌跡算出処理（ステップ０２）で算出された走行軌跡に対応する区間を意味する。ステップＳ３０３に進み、選択した過去の観測値の信頼度が第１基準値よりも高いか否かを判断する。信頼度が第１基準値よりも高い場合（ステップＳ３０３でＹＥＳ）、ステップＳ３０４へ進み、信頼度が第１基準値以下である場合（ステップＳ３０３でＮＯ）、ステップＳ３０６へ進む。ステップＳ３０４において、過去の観測値と走行軌跡との乖離度を算出し、乖離度が閾値以下か否かを判定する。乖離度が閾値以下である場合（ステップＳ３０４でＹＥＳ）、ステップＳ３０５に進み、乖離度が閾値よりも大きい場合（ステップＳ３０４でＮＯ）、ステップＳ３０６に進む。ステップＳ３０５において、カウンタを１足し合わせる。ステップＳ３０６に進み、ウィンドウサイズ内の過去の観測値を全て選択したか否かを判断する。全ての観測値を選択していない場合（ステップＳ３０６でＮＯ）、ステップＳ３０２に戻り、未だ選択されていないウィンドウサイズ内の過去の観測値を１つ選択し、ステップＳ３０３～３０５の処理を行う。

【0025】

全ての観測値を選択した場合（ステップＳ３０６でＹＥＳ）、ステップＳ３０７に進み、走行軌跡に対応する区間（ウィンドウサイズ）における過去の観測値の全体数に対して、ステップＳ３０４で乖離度が閾値以下と判定された過去の観測値の数が占める割合を算出する。ステップＳ３０８に進み、この割合が所定割合以上であるか否かを判断する。前記した割合が所定割合以上である場合（ステップＳ３０８でＹＥＳ）、ステップＳ３０９に進み、ステップＳ０１で計測された検出対象の観測値の信頼性は高いと判断する。前記した割合が所定割合未満である場合（ステップＳ３０８でＮＯ）、ステップＳ３１０に進み、ステップＳ０１で計測された検出対象の観測値の信頼性は低いと判断する。ステップＳ３０８～Ｓ３１０に依れば、ウィンドウサイズ内の過去の観測値の全体数に対して乖離度が閾値以下と判定された過去の観測値の数が占める割合に基づいて、検出対象の観測値の信頼度を検出することができる。ここでは、信頼度を二値分類する例を示すが、多値分類してもよい。すなわち、前記した割合が大きい程、検出対象の観測値の信頼度を高く検出することができる。

【0026】

信頼度検出部１５は、走行軌跡と過去の観測値のうち走行軌跡に対応した区間における観測値との乖離度を算出し、乖離度と閾値とを比較し、閾値以下の観測値の割合が多い場合に少ない場合よりも高い信頼度を算出する。信頼度検出部１５は、ウィンドウサイズ内の観測値全体の平均値に対する閾値判断ではなく、観測値の各々に対して閾値判断を行う。このため、ウィンドウサイズ内に大きな誤差を含む観測値があった場合でも、その観測値の影響を受けることなく、正確な信頼度の判定が可能となる。

【0027】

（第３実施形態）
第３実施形態では、図４を参照して、図１の信頼度検出部１５の他の具体的な動作例、及び図２のステップ０３（信頼度検出処理）の他の具体的な処理例を説明する。第３実施形態に係る自己位置推定装置１及び自己位置推定方法の全体は、図１及び図２と同じであり説明を省略する。以下に述べる全てのステップＳ３２１～Ｓ３２６の動作主体は、図１の信頼度検出部１５である。

【0028】

ステップＳ３２１において、ウィンドウサイズ内の過去の観測値と走行軌跡との乖離度を算出する。ウィンドウサイズ内に複数の過去の観測値がある場合には、全ての過去の観測値について、走行軌跡との乖離度を算出する。ステップＳ３２２に進み、乖離度の各々に対して、乖離度に対応する過去の観測値の信頼度に応じた重み付けをする。具体的には、信頼度が高い程、乖離度を大きくする（重くする）。ステップＳ３２３に進み、重み付けして計算した乖離度の平均値（以後、「加重平均値」という）を算出する。ステップＳ３２４に進み、加重平均値が閾値以下か否かを判断する。加重平均値が閾値以下である場合（ステップＳ３２４でＹＥＳ）、ステップＳ３２５に進み、ステップＳ０１で計測された検出対象の観測値の信頼性は高いと判断する。加重平均値が閾値よりも大きい場合（ステップＳ３２４でＮＯ）、ステップＳ３２６に進み、ステップＳ０１で計測された検出対象の観測値の信頼性は低いと判断する。ステップＳ３２４～Ｓ３２６に依れば、ウィンドウサイズ内の過去の観測値と走行軌跡との乖離度の加重平均値に基づいて、検出対象の観測値の信頼度を検出することができる。ここでは、信頼度を二値分類する例を示すが、多値分類してもよい。すなわち、加重平均値が小さい程、検出対象の観測値の信頼度を高く検出することができる。

【0029】

信頼度検出部１５は、走行軌跡と過去の観測値のうち走行軌跡に対応した区間における過去の観測値との乖離度に対して、過去の観測値に対応して記憶されている信頼度に応じた重み付けをする。そして、加重平均値が小さい程、検出対象の観測値の信頼度を高く検出する。過去の観測値の信頼度を考慮した加重平均値を用いて検出対象の観測値の信頼度を検出する。これにより、ある特定の過去の観測値と走行軌跡との乖離度が大きくても、その過去の観測値の信頼度が低い場合には乖離度に付される重みが軽くなるので、加重平均値は大きくならない。すなわち、ウィンドウサイズ内に大きな誤差を含む観測値があっても、その観測値の影響を受けることなく、正確な信頼度の判定が可能となる。

【0030】

（第４実施形態）
第１乃至第３実施形態では、検出対象の観測値としての車両の位置（以後、「観測位置」という）及び角度（以後、「観測角度」という）の双方の信頼度（以後、「位置信頼度」及び「角度信頼度」という）を同時に検出し、車両の位置及び角度を同時に推定する例を示した。しかし、位置の走行軌跡を算出する際に起点として用いる観測角度に誤差があると、位置の走行軌跡が誤って算出され、ひいては位置信頼度の誤判定につながる可能性がある。そこで、第４実施形態では、位置信頼性と角度信頼性を個別に検出する例を説明する。具体的には、先ず、角度信頼度を検出して角度を推定し、その後に、推定した角度を、位置の走行軌跡を算出する際に起点として用いる例を説明する。

【0031】

図５を参照して、第４実施形態に係る自己位置推定装置２の構成を説明する。自己位置推定装置２は、測位部１１と、ＩＭＵ装置１２と、角度部２ａと、位置部２ｂとを有する。自己位置推定装置２のうち、データ記憶及び演算処理に関する機能ブロックが、角度部１ａと位置部１ｂとに分割された点が、図１と相違している。一方、図５の測位部１１及びＩＭＵ装置１２は、図１のそれらと同じであり、ここでは説明を割愛する。

【0032】

角度部１ａには、角度記憶部１３ａと、角度走行軌跡算出部１４ａと、角度信頼度検出部１５ａと、角度判定履歴記憶部１６ａと、角度推定部１７ａと、が含まれる。

【0033】

角度記憶部１３ａは、ＩＭＵ装置１２により検出された車両の角速度を示すデータと、測位部１１により計測された観測角度を示すデータとを記憶する。これらのデータは、角度走行軌跡算出部１４ａ、角度信頼度検出部１５ａ及び角度判定履歴記憶部１６ａによって利用される。

【0034】

角度走行軌跡算出部１４ａは、検出対象の観測角度から所定距離遡る角度の走行軌跡、又は検出対象の観測角度を計測した時刻から所定時間遡る車両の角度の走行軌跡を、算出する。角度走行軌跡算出部１４ａは、ＩＭＵ装置１２から得られる車両の相対的な移動量に基づいて角度の走行軌跡を算出する。観測角度の信頼性の検出基準となる角度の走行軌跡は、測位部１１が計測する観測角度の影響を受けることが無くなり、観測角度の外れ値による信頼性の誤判定を抑制することができる。

【0035】

具体的には、先ず、検出対象の観測角度を起点として、起点から、検出対象の観測角度を計測した時刻に最も近い時刻における角速度を差し引くことにより、一つ前の時刻における車両の角度を推定する。次に、推定した角度から、この推定角度に対応する時刻の直前の時刻における角速度を差し引くことにより、二つ前の時刻における角度を推定できる。この差し引きを、起点から所定距離内、もしくは検出された時刻からの経過時間が所定時間内である限り繰り返すことで、角度の走行軌跡を算出する。角度の走行軌跡の算出において観測角度は起点としてのみ用いられるため、観測角度の信頼性の検出基準となる角度の走行軌跡は、測位部１１が計測する観測角度の影響を受けることが無くなり、観測角度の外れ値による角度信頼性の誤判定を抑制することができる。すなわち、車両の角度の走行軌跡を車両の相対的な移動量に基づいて算出することにより、測位部１１が計測する信頼性の低い観測角度の影響を受けることが無くなる。

【0036】

角度信頼度検出部１５ａは、角度走行軌跡算出部１４ａで算出された角度の走行軌跡と角度の走行軌跡に対応する区間における過去の観測角度との乖離度と、過去の観測角度に関連付けて角度判定履歴記憶部１６ａに記憶されている角度信頼度とに基づいて、検出対象の観測角度の角度信頼度を検出する。具体的には、角度信頼度検出部１５ａは、角度の走行軌跡に対応する区間内の各時刻における過去の観測角度を角度判定履歴記憶部１６ａから抽出し、抽出された過去の観測角度と角度の走行軌跡との角度差を、角度の乖離度として算出する。

【0037】

角度信頼度検出部１５ａは、第２実施形態（図３）又は第３実施形態（図４）で説明した図２のステップＳ０３（信頼度検出処理）の詳細な具体例を車両の観測角度に適用することにより、検出対象の観測角度の角度信頼度を検出することができる。

【0038】

角度判定履歴記憶部１６ａは、検出対象の観測角度と検出された角度信頼度とを関連付けて記憶する。

【0039】

角度推定部１７ａは、信頼度が第３所定値以上の観測角度に基づいて車両の角度を推定する。角度推定部１７ａは、既知の技術を用いて角度を推定することができる。例えば、観測角度（車両の絶対角度）と車両の相対的な移動量（車速と角速度）に基づいて、カルマンフィルタなどの推定手法を用いて、尤もらしい車両の角度を推定する。ここで、観測角度のうち低い信頼度が検出された観測角度は、事前に排除するか、又は角度の推定処理への寄与度が小さくなるように設定することができる。

【0040】

位置部１ｂには、位置記憶部１３ｂと、位置走行軌跡算出部１４ｂと、位置信頼度検出部１５ｂと、位置判定履歴記憶部１６ｂと、位置推定部１７ｂと、が含まれる。

【0041】

位置記憶部１３ｂは、ＩＭＵ装置１２により検出された車両の速度及び角速度を示すデータと、測位部１１により計測された観測位置を示すデータとを記憶する。これらのデータは、位置走行軌跡算出部１４ｂ、位置信頼度検出部１５ｂ及び位置判定履歴記憶部１６ｂによって利用される。

【0042】

位置走行軌跡算出部１４ｂは、測位部１１により計測された検出対象の観測位置及び角度推定部１７ａにより推定された角度から、車両の位置の走行軌跡を算出する。位置走行軌跡算出部１４ｂは、ＩＭＵ装置１２から得られる車両の相対的な移動量に基づいて位置の走行軌跡を算出する。観測位置の信頼性の検出基準となる位置の走行軌跡は、測位部１１が計測する観測位置の影響を受けることが無くなり、観測位置の外れ値による信頼性の誤判定を抑制することができる。

【0043】

具体的には、先ず、検出対象の観測位置及び角度推定部１７ａにより推定された車両の角度を起点として、起点から、検出対象の観測位置を計測した時刻に最も近い時刻における速度と角速度を差し引くことにより、一つ前の時刻における車両の位置と角度を推定する。次に、推定した位置と角度から、この推定位置及び推定角度に対応する時刻の直前の時刻における速度と角速度を差し引くことにより、二つ前の時刻における位置と角度を推定できる。この差し引きを、起点から所定距離内、もしくは検出された時刻からの経過時間が所定時間内である限り繰り返すことで、位置の走行軌跡を算出する。位置の走行軌跡の算出において観測位置は起点としてのみ用いられるため、観測位置の信頼性の検出基準となる位置の走行軌跡は、測位部１１が計測する観測位置の影響を受けることが無くなり、観測位置の外れ値による位置信頼性の誤判定を抑制することができる。すなわち、車両の位置の走行軌跡を車両の相対的な移動量に基づいて算出することにより、測位部１１が計測する信頼性の低い観測位置の影響を受けることが無くなる。

【0044】

位置信頼度検出部１５ｂは、位置走行軌跡算出部１４ｂで算出された位置の走行軌跡と位置の走行軌跡に対応する区間における過去の観測位置との乖離度と、過去の観測位置に関連付けて位置判定履歴記憶部１６ｂに記憶されている位置信頼度とに基づいて、検出対象の観測位置の位置信頼度を検出する。具体的には、位置信頼度検出部１５ｂは、位置の走行軌跡に対応する区間内の各時刻における過去の観測位置を位置判定履歴記憶部１６ｂから抽出し、抽出された過去の観測位置と位置の走行軌跡とのユークリッド距離を、位置の乖離度として算出する。

【0045】

位置信頼度検出部１５ｂは、第２実施形態（図３）又は第３実施形態（図４）で説明した図２のステップＳ０３（信頼度検出処理）の詳細な具体例を車両の観測位置に適用することにより、検出対象の観測位置の位置信頼度を検出することができる。

【0046】

位置判定履歴記憶部１６ｂは、検出対象の観測位置と検出された位置信頼度とを関連付けて記憶する。

【0047】

位置推定部１７ｂは、信頼度が第４所定値以上の観測位置及び角度推定部１７ａにより推定された車両の角度に基づいて車両の位置を推定する。位置推定部１７ｂは、既知の技術を用いて位置を推定することができる。例えば、観測位置（車両の絶対位置）、角度推定部１７ａにより推定された車両の角度、及び車両の相対的な移動量（車速と角速度）に基づいて、カルマンフィルタなどの推定手法を用いて、尤もらしい車両の位置を推定する。ここで、観測位置のうち低い信頼度が検出された観測位置は、事前に排除するか、又は位置の推定処理への寄与度が小さくなるように設定することができる。

【0048】

最後に、自己位置推定装置２は、角度推定部１７ａにより推定された車両の角度及び位置推定部１７ｂにより推定された車両の位置を、車両の自己位置として出力する。

【0049】

図６を参照して、図５の自己位置推定装置２を用いた第４実施形態に係る自己位置推定方法を説明する。図６に示す動作フローは、予め定められた周期で、繰り返し実施される。ステップＳ０１（測位処理）において、測位部１１は、人工衛星から受信する信号から前記車両の位置及び角度を検出対象の観測値として計測する。ステップＳ０２－１（走行軌跡算出処理）に進み、角度走行軌跡算出部１４ａは、検出対象の観測角度から、車両の角度の走行軌跡を算出する。ステップＳ０３－１（信頼度検出処理）に進み、角度信頼度検出部１５ａは、角度の走行軌跡、角度の走行軌跡に対応する区間における過去の観測角度との乖離度、及び過去の観測角度に関連付けている角度信頼度に基づいて、検出対象の観測角度の角度信頼度を検出する。ステップＳ０４－１（信頼度記憶処理）に進み、角度判定履歴記憶部１６ａは、検出対象の観測角度と検出された角度信頼度とを関連付けて記憶する。ステップＳ０５－１（自己位置推定処理）に進み、角度推定部１７ａは、角度信頼度が第３所定値以上の観測角度に基づいて車両の角度を推定する。

【0050】

ステップＳ０２－２（走行軌跡算出処理）に進み、位置走行軌跡算出部１４ｂは、検出対象の観測位置及び自己位置推定処理（ステップＳ０５－１）において推定された車両の角度から、車両の位置の走行軌跡を算出する。ステップＳ０３－２（信頼度検出処理）に進み、位置信頼度検出部１５ｂは、位置の走行軌跡、位置の走行軌跡に対応する区間における過去の観測位置との乖離度、及び過去の観測位置に関連付けている位置信頼度に基づいて、検出対象の観測位置の位置信頼度を検出する。ステップＳ０４－２（信頼度記憶処理）に進み、位置判定履歴記憶部１６ｂは、検出対象の観測位置と検出された位置信頼度とを関連付けて記憶する。ステップＳ０５－２（自己位置推定処理）に進み、位置推定部１７ｂは、位置信頼度が第４所定値以上の観測位置及びステップＳ０５－１で推定された移動体の角度に基づいて車両の位置を推定する。ステップＳ０６に進み、自己位置推定装置２は、推定された車両の角度及び車両の位置を、車両の自己位置として出力する。

【0051】

上記した動作フローを繰り返し実行する事により、信頼度が関連付けられた複数の観測位置の時系列及び複数の観測角度の時系列がそれぞれ求められ、位置判定履歴記憶部１６ｂ及び角度判定履歴記憶部１６ａに過去の観測位置及び過去の観測角度としてそれぞれ記憶される。この過去の観測位置及び過去の観測角度に関連付けた位置信頼度及び角度信頼度を用いて、例えば最新の観測位置及び最新の観測角度の信頼度をそれぞれ検出することができる。

【0052】

位置信頼度の検出基準となる位置の走行軌跡を算出する際に用いる起点として車両の位置と角度の情報が必要となる。起点となる角度の情報が正確で無いと、誤った位置の走行軌跡が算出され、ひいては位置信頼度の誤判定につながる。そこで、先ず、測位部１１が計測する観測角度の角度信頼度を算出し、車両の角度を推定する。その後、推定された角度を用いて、位置の走行軌跡を算出し、位置信頼度を検出し、車両の位置を推定する。よって、誤った位置走行軌跡に起因する位置信頼度の誤判定を抑制できる。

【0053】

（第１変形例）
以下、上記した各実施形態の第１乃至第５変形例を説明する。第１乃至第５変形例の２以上の変形例を任意に組み合わせて各実施形態に適用することもできる。

【0054】

第１変形例では、過去の観測値の信頼度に関する情報量が十分でない場合の対応について説明する。例えば、測位部１１が測位を開始した直後は、測位部１１は観測対象の観測値を測定できるが、過去の観測値の信頼度が未だ十分に記憶されていないため、過去の観測値の信頼度に関する情報量が十分でない。

【0055】

そこで、信頼度検出部１５は、走行軌跡に対応する区間（ウィンドウサイズ内）における過去の観測値のうち、信頼性が関連付けられていない過去の観測値の数が第２所定値以上ある場合、信頼性が関連付けられていない過去の観測値に対して、ステップＳ３０３に示す第１基準値よりも高い信頼度が検出されていると見做して、信頼度検出処理（ステップＳ０３）を行う。これにより、測位処理（Ｓ０１）の開始直後のような過去の観測値の信頼度に関する情報が十分でない場合であっても、検出対象の観測値の信頼度を検出することができる。第１変形例は、第１乃至第４実施形態に適用することができる。

【0056】

（第２変形例）
第２変形例では、起点から遡る距離又は時間に応じて閾値を制御する変形例を説明する。検出対象の観測値から遡る距離が長い程、又は検出対象の観測値を計測した時刻から遡る時間が長い程、ステップＳ０２で算出される走行軌跡に含まれる車両の位置又は角度の誤差は大きくなる。そこで、信頼度検出部１５は、信頼度検出処理（ステップＳ０３）において、走行軌跡に対応する区間（ウィンドウサイズ内）における過去の観測値の、検出対象の観測値から遡る距離が長い程、又は検出対象の観測値を計測した時刻から遡る時間が長い程、図３のステップＳ３０４で用いる閾値を大きく設定する。これにより、走行軌跡に含まれる位置又は角度の誤差に起因する信頼度の誤判定を抑制できる。第１変形例は、第２実施形態及び第４実施形態に適用することができる。

【0057】

（第３変形例）
第３変形例では、起点から遡る距離又は時間に応じて乖離度を制御する変形例を説明する。検出対象の観測値から遡る距離が長い程、又は検出対象の観測値を計測した時刻から遡る時間が長い程、ステップＳ０２で算出される走行軌跡に含まれる車両の位置又は角度の誤差は大きくなる。そこで、信頼度検出部１５は、信頼度検出処理（ステップＳ０３）において、走行軌跡に対応する区間（ウィンドウサイズ内）における過去の観測値の、検出対象の観測値から遡る距離が長い程、又は検出対象の観測値を計測した時刻から遡る時間が長い程、乖離度を短く算出する。これにより、走行軌跡に含まれる位置又は角度の誤差に起因する信頼度の誤判定を抑制できる。第３変形例は、第１乃至第４実施形態に適用することができる。

【0058】

（第４変形例）
第４変形例では、測位処理（ステップＳ０１）での観測値の測定精度が悪い場合の対応について説明する。一般的に，高層ビルが立ち並ぶ都市部では，衛星の電波がビル等に反射してしまうことで測位精度が大きく悪化することが知られている。都市部の高層ビルの他にも、山間、トンネルなどでは電波障害が発生し、測位精度が低下、又は測位が不可能となる。

【0059】

そこで、測位処理（ステップＳ０１）での観測値の測定精度が第２基準値よりも低い車両の走行距離が長い程、走行軌跡算出部１４は、走行軌跡算出処理（ステップＳ０２）において前記した所定距離又は前記した所定時間を長く設定する。これにより、走行軌跡算出部１４が算出する走行軌跡に対応する区間が長くなる。よって、当該区間における過去の観測値の信頼度が全て低いという状況を抑制して、信頼度の検出精度を高く維持することができる。第４変形例は、第１乃至第４実施形態に適用することができる。

【0060】

（第５変形例）
第５変形例では、ＩＭＵ装置１２の測定精度が悪い場合の対応について説明する。例えば、温度及び湿度などの環境の変化、又は角速度センサ及び加速度センサの経時劣化によってＩＭＵ装置１２の測定精度が悪くなる。車両の速度及び加速度の誤差は、車両の相対的な移動量の精度、及び相対的な移動量に基づき算出される走行軌跡の算出精度を悪化させる。また、非舗装路等の上下方向（Ｚ方向）の加速度または峠道などの左右方向（Ｚ軸周り）の角速度等が大きいシーンでも、走行軌跡の算出精度が悪化する場合がある。

【0061】

そこで、走行軌跡算出部１４は、車両の相対的な移動量の誤差が大きい程、走行軌跡算出処理（ステップＳ０２）において、起点から遡る所定距離又は所定時間を短く設定する。これにより、走行軌跡形状の誤差に起因する信頼度の誤判定を抑制できる。第５変形例は、第１乃至第４実施形態に適用することができる。

【0062】

以上、実施形態に沿って本発明の内容を説明したが、本発明はこれらの記載に限定されるものではなく、種々の変形及び改良が可能であることは、当業者には自明である。この開示の一部をなす論述及び図面は本発明を限定するものであると理解すべきではない。この開示から当業者には様々な代替実施形態、実施例及び運用技術が明らかとなろう。

【0063】

本発明はここでは記載していない様々な実施形態等を含むことは勿論である。したがって、本発明の技術的範囲は上記の説明から妥当な特許請求の範囲に係る発明特定事項によってのみ定められるものである。

【符号の説明】

【0064】

１、２ 自己位置推定装置
１１ 測位部
１４ 走行軌跡算出部
１５ 信頼度検出部
１６ 判定履歴記憶部（信頼度記憶部）
１７ 自己位置推定部
Ｓ０１ 測位処理
Ｓ０２、Ｓ０２－１、Ｓ０２－２ 走行軌跡算出処理
Ｓ０３、Ｓ０３－１、Ｓ０３－２ 信頼度検出処理
Ｓ０４、Ｓ０４－１、Ｓ０４－２ 信頼度記憶処理
Ｓ０５、Ｓ０５－１、Ｓ０５－２ 自己位置推定処理

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】