(19)【発行国】日本国特許庁(JP)
(12)【公報種別】公開特許公報(A)
(11)【公開番号】P2024014488
(43)【公開日】2024-02-01
(54)【発明の名称】自己位置推定装置および該方法
(51)【国際特許分類】
G01C 21/28 20060101AFI20240125BHJP
【ＦＩ】
G01C21/28
【審査請求】未請求
【請求項の数】4
【出願形態】ＯＬ
(21)【出願番号】P 2022117346
(22)【出願日】2022-07-22
(71)【出願人】
【識別番号】000003137
【氏名又は名称】マツダ株式会社
(74)【代理人】
【識別番号】100115381
【弁理士】
【氏名又は名称】小谷 昌崇
(74)【代理人】
【識別番号】100111453
【弁理士】
【氏名又は名称】櫻井 智
(72)【発明者】
【氏名】依田 大輝
(72)【発明者】
【氏名】勝谷 泰荘
(72)【発明者】
【氏名】浅胡 武志
【テーマコード（参考）】
2F129
【Ｆターム（参考）】
2F129AA03
2F129AA05
2F129AA06
2F129BB19
2F129BB23
2F129CC14
2F129CC15
2F129EE78
2F129GG17
2F129GG18
2F129HH12
2F129HH21
(57)【要約】
【課題】本発明は、推定精度を改善できる自己位置推定装置および該方法を提供する。
【解決手段】本発明の自己位置推定装置Ｄは、過去推定タイミングで検出した過去検出物標の位置を物標存在確率分布に変換し、現在推定タイミングでオドメトリで求めた仮自己位置を移動体存在確率分布に変換し、これらに基づいて、前記仮自己位置から見た前記過去検出物標の物標存在確率分布を移動体考慮物標存在確率分布として求め、所定の高精度地図における、前記現在検出物標および前記過去検出物標それぞれの各位置、前記現在推定タイミングで検出した現在検出物標の物標存在確率分布、ならびに、前記過去検出物標の移動体考慮物標存在確率分布に基づいて、現在の自己位置を推定する。
【選択図】図１
【解決手段】本発明の自己位置推定装置Ｄは、過去推定タイミングで検出した過去検出物標の位置を物標存在確率分布に変換し、現在推定タイミングでオドメトリで求めた仮自己位置を移動体存在確率分布に変換し、これらに基づいて、前記仮自己位置から見た前記過去検出物標の物標存在確率分布を移動体考慮物標存在確率分布として求め、所定の高精度地図における、前記現在検出物標および前記過去検出物標それぞれの各位置、前記現在推定タイミングで検出した現在検出物標の物標存在確率分布、ならびに、前記過去検出物標の移動体考慮物標存在確率分布に基づいて、現在の自己位置を推定する。
【選択図】図１
【特許請求の範囲】
【請求項1】
移動体に搭載され、自己位置を推定する自己位置推定装置であって、
高精度地図を記憶する地図記憶部と、
前記移動体の周囲に存在する物標を検出し、前記検出した物標の、前記移動体に対する相対位置を検出する物標検出部と、
過去に自己位置を推定した過去推定タイミングから、現在の自己位置を推定する現在推定タイミングまでにおける前記移動体の移動状況に関する所定の移動情報を検出し、前記検出した移動情報に基づいて現在の自己位置を仮自己位置として検出する仮自己位置検出部と、
前記物標検出部で検出した物標の相対位置を、物標の存在確率の分布である所定の物標存在確率分布に変換する第１確率分布変換部と、
前記仮自己位置検出部で検出した仮自己位置を、移動体の存在確率の分布である所定の移動体存在確率分布に変換する第２確率分布変換部と、
前記過去推定タイミングにおいて前記物標検出部で検出した物標を過去検出物標とした場合に、前記過去検出物標の、前記第１確率分布変換部で求められる物標存在確率分布、および、前記現在推定タイミングにおいて前記仮自己位置検出部で検出した仮自己位置の、前記第２確率分布変換部で求められる移動体存在確率分布に基づいて、前記仮自己位置から見た前記過去検出物標の物標存在確率分布を移動体考慮物標存在確率分布として求める物標存在確率分布処理部と、
前記現在推定タイミングにおいて前記物標検出部で検出した物標を現在検出物標とした場合に、前記地図記憶部に記憶された高精度地図における、前記現在検出物標および前記過去検出物標それぞれの各位置、前記現在検出物標の、前記第１確率分布変換部で求められる物標存在確率分布、ならびに、前記物標存在確率分布処理部で求めた前記過去検出物標の移動体考慮物標存在確率分布に基づいて、現在の自己位置を推定する推定部とを備える、
自己位置推定装置。
高精度地図を記憶する地図記憶部と、
前記移動体の周囲に存在する物標を検出し、前記検出した物標の、前記移動体に対する相対位置を検出する物標検出部と、
過去に自己位置を推定した過去推定タイミングから、現在の自己位置を推定する現在推定タイミングまでにおける前記移動体の移動状況に関する所定の移動情報を検出し、前記検出した移動情報に基づいて現在の自己位置を仮自己位置として検出する仮自己位置検出部と、
前記物標検出部で検出した物標の相対位置を、物標の存在確率の分布である所定の物標存在確率分布に変換する第１確率分布変換部と、
前記仮自己位置検出部で検出した仮自己位置を、移動体の存在確率の分布である所定の移動体存在確率分布に変換する第２確率分布変換部と、
前記過去推定タイミングにおいて前記物標検出部で検出した物標を過去検出物標とした場合に、前記過去検出物標の、前記第１確率分布変換部で求められる物標存在確率分布、および、前記現在推定タイミングにおいて前記仮自己位置検出部で検出した仮自己位置の、前記第２確率分布変換部で求められる移動体存在確率分布に基づいて、前記仮自己位置から見た前記過去検出物標の物標存在確率分布を移動体考慮物標存在確率分布として求める物標存在確率分布処理部と、
前記現在推定タイミングにおいて前記物標検出部で検出した物標を現在検出物標とした場合に、前記地図記憶部に記憶された高精度地図における、前記現在検出物標および前記過去検出物標それぞれの各位置、前記現在検出物標の、前記第１確率分布変換部で求められる物標存在確率分布、ならびに、前記物標存在確率分布処理部で求めた前記過去検出物標の移動体考慮物標存在確率分布に基づいて、現在の自己位置を推定する推定部とを備える、
自己位置推定装置。
【請求項2】
前記物標検出部は、前記移動体と前記物標との間の距離が短いほど小さい誤差で前記物標を検出し、
前記推定部は、誤差が所定の閾値以下である前記現在検出物標および前記過去検出物標、または、前記誤差が前記所定の閾値より小さい前記現在検出物標および前記過去検出物標、を用いて、前記現在の自己位置を推定する、
請求項１に記載の自己位置推定装置。
前記推定部は、誤差が所定の閾値以下である前記現在検出物標および前記過去検出物標、または、前記誤差が前記所定の閾値より小さい前記現在検出物標および前記過去検出物標、を用いて、前記現在の自己位置を推定する、
請求項１に記載の自己位置推定装置。
【請求項3】
前記物標検出部は、前記移動体と前記物標との間の距離が短いほど小さい誤差で前記物標を検出し、
前記推定部は、前記現在検出物標および前記過去検出物標の個数の総和が所定の第２閾値以上である場合、または、前記現在検出物標および前記過去検出物標の個数の総和が前記所定の第２閾値より多い場合、前記現在検出物標および前記過去検出物標の中から、誤差が小さい順に所定の個数だけ抽出し、前記抽出した前記現在検出物標および前記過去検出物標を用いて、前記現在の自己位置を推定する、
請求項１に記載の自己位置推定装置。
前記推定部は、前記現在検出物標および前記過去検出物標の個数の総和が所定の第２閾値以上である場合、または、前記現在検出物標および前記過去検出物標の個数の総和が前記所定の第２閾値より多い場合、前記現在検出物標および前記過去検出物標の中から、誤差が小さい順に所定の個数だけ抽出し、前記抽出した前記現在検出物標および前記過去検出物標を用いて、前記現在の自己位置を推定する、
請求項１に記載の自己位置推定装置。
【請求項4】
移動体で実行される、自己位置を推定する自己位置推定方法であって、
前記移動体の周囲に存在する物標を検出し、前記検出した物標の、前記移動体に対する相対位置を検出する物標検出工程と、
過去に自己位置を推定した過去推定タイミングから、現在の自己位置を推定する現在推定タイミングまでにおける前記移動体の移動状況に関する所定の移動情報を検出し、前記検出した移動情報に基づいて現在の自己位置を仮自己位置として検出する仮自己位置検出工程と、
前記物標検出工程で検出した物標の相対位置を、物標の存在確率の分布である所定の物標存在確率分布に変換する第１確率分布変換工程と、
前記仮自己位置検出工程で検出した仮自己位置を、移動体の存在確率の分布である所定の移動体存在確率分布に変換する第２確率分布変換工程と、
前記過去推定タイミングにおいて前記物標検出工程で検出した物標を過去検出物標とした場合に、前記過去検出物標の、前記第１確率分布変換工程で求められる物標存在確率分布、および、前記現在推定タイミングにおいて前記仮自己位置検出工程で検出した仮自己位置の、前記第２確率分布変換工程で求められる移動体存在確率分布に基づいて、前記仮自己位置から見た前記過去検出物標の物標存在確率分布を移動体考慮物標存在確率分布として求める物標存在確率分布処理工程と、
前記現在推定タイミングにおいて前記物標検出工程で検出した物標を現在検出物標とした場合に、所定の高精度地図における、前記現在検出物標および前記過去検出物標それぞれの各位置、前記現在検出物標の、前記第１確率分布変換工程で求められる物標存在確率分布、ならびに、前記物標存在確率分布処理工程で求めた前記過去検出物標の移動体考慮物標存在確率分布に基づいて、現在の自己位置を推定する推定工程とを備える、
自己位置推定方法。
前記移動体の周囲に存在する物標を検出し、前記検出した物標の、前記移動体に対する相対位置を検出する物標検出工程と、
過去に自己位置を推定した過去推定タイミングから、現在の自己位置を推定する現在推定タイミングまでにおける前記移動体の移動状況に関する所定の移動情報を検出し、前記検出した移動情報に基づいて現在の自己位置を仮自己位置として検出する仮自己位置検出工程と、
前記物標検出工程で検出した物標の相対位置を、物標の存在確率の分布である所定の物標存在確率分布に変換する第１確率分布変換工程と、
前記仮自己位置検出工程で検出した仮自己位置を、移動体の存在確率の分布である所定の移動体存在確率分布に変換する第２確率分布変換工程と、
前記過去推定タイミングにおいて前記物標検出工程で検出した物標を過去検出物標とした場合に、前記過去検出物標の、前記第１確率分布変換工程で求められる物標存在確率分布、および、前記現在推定タイミングにおいて前記仮自己位置検出工程で検出した仮自己位置の、前記第２確率分布変換工程で求められる移動体存在確率分布に基づいて、前記仮自己位置から見た前記過去検出物標の物標存在確率分布を移動体考慮物標存在確率分布として求める物標存在確率分布処理工程と、
前記現在推定タイミングにおいて前記物標検出工程で検出した物標を現在検出物標とした場合に、所定の高精度地図における、前記現在検出物標および前記過去検出物標それぞれの各位置、前記現在検出物標の、前記第１確率分布変換工程で求められる物標存在確率分布、ならびに、前記物標存在確率分布処理工程で求めた前記過去検出物標の移動体考慮物標存在確率分布に基づいて、現在の自己位置を推定する推定工程とを備える、
自己位置推定方法。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、自己（自機）の位置を推定する自己位置推定装置および自己位置推定方法に関する。
【背景技術】
【0002】
例えば、運転者の運転操作をアシスト（手助け）する場合や自律走行する場合、移動体は、自己（自機）の位置（自己位置）を推定することが必要である。このような自己位置を推定する技術は、例えば、特許文献１に開示されている。
【0003】
この特許文献１に開示された自己位置推定装置は、自車両に搭載され、自車両の周囲に存在する物標と自車両との間の相対位置を検出する物標検出センサを備え、前記物標検出センサによって検出された相対位置を自車両の移動量だけ移動させて物標位置データとして蓄積し、蓄積された前記物標位置データと、地図上に存在する物標の物標位置情報を含む地図情報とを照合して、自車両の現在位置である自己位置を推定する。
【先行技術文献】
【特許文献】
【0004】
【特許文献1】特開２０１８－８４９６０号公報
【発明の概要】
【発明が解決しようとする課題】
【0005】
ところで、過去に得られた物標の位置に基づいて現在の自己位置を推定する場合、前記過去の時点から前記現在の時点までに前記自己が移動した移動量およびその移動方向がさらに必要となる。このため、前記過去に得られた物標の位置に基づいて推定した現在の自己位置における推定精度は、物標の位置を検出する第１装置の検出精度と、前記自己が移動した移動量およびその移動方向を検出する第２装置の検出精度とに依存し、推定精度には改善の余地がある。
【0006】
本発明は、上述の事情に鑑みて為された発明であり、その目的は、推定精度を改善できる自己位置推定装置および自己位置推定方法を提供することである。
【課題を解決するための手段】
【0007】
本発明者は、種々検討した結果、上記目的は、以下の本発明により達成されることを見出した。すなわち、本発明の一態様にかかる自己位置推定装置は、移動体に搭載され、自己位置を推定する自己位置推定装置であって、高精度地図を記憶する地図記憶部と、前記移動体の周囲に存在する物標を検出し、前記検出した物標の、前記移動体に対する相対位置を検出する物標検出部と、過去に自己位置を推定した過去推定タイミングから、現在の自己位置を推定する現在推定タイミングまでにおける前記移動体の移動状況に関する所定の移動情報を検出し、前記検出した移動情報に基づいて現在の自己位置を仮自己位置として検出する仮自己位置検出部と、前記物標検出部で検出した物標の相対位置を、物標の存在確率の分布である所定の物標存在確率分布に変換する第１確率分布変換部と、前記仮自己位置検出部で検出した仮自己位置を、移動体の存在確率の分布である所定の移動体存在確率分布に変換する第２確率分布変換部と、前記過去推定タイミングにおいて前記物標検出部で検出した物標を過去検出物標とした場合に、前記過去検出物標の、前記第１確率分布変換部で求められる物標存在確率分布、および、前記現在推定タイミングにおいて前記仮自己位置検出部で検出した仮自己位置の、前記第２確率分布変換部で求められる移動体存在確率分布に基づいて、前記仮自己位置から見た前記過去検出物標の物標存在確率分布を移動体考慮物標存在確率分布として求める物標存在確率分布処理部と、前記現在推定タイミングにおいて前記物標検出部で検出した物標を現在検出物標とした場合に、前記地図記憶部に記憶された高精度地図における、前記現在検出物標および前記過去検出物標それぞれの各位置、前記現在検出物標の、前記第１確率分布変換部で求められる物標存在確率分布、ならびに、前記物標存在確率分布処理部で求めた前記過去検出物標の移動体考慮物標存在確率分布に基づいて、現在の自己位置を推定する推定部とを備える。好ましくは、上述の自己位置推定装置において、前記移動情報は、過去に自己位置を推定した過去推定タイミングから、現在の自己位置を推定する現在推定タイミングまでにおける前記移動体の移動方向および移動量それぞれを表す情報である。好ましくは、前記移動体の移動方向および移動量は、オドメトリ（デッドレコニング）によって得られる。好ましくは、上述の自己位置推定装置において、前記高精度地図は、物標の種類およびその位置を少なくとも含み、前記物標検出部は、前記物標をその種類で検出し、前記推定部は、前記現在検出物標の種類および前記過去検出物標の種類に基づいて、前記地図記憶部に記憶された高精度地図における、前記現在検出物標および前記過去検出物標それぞれの各位置を求める。好ましくは、上述の自己位置推定装置において、前記物標検出部は、前記物標を所定の検出範囲で検出し、前記過去推定タイミングは、前記現在推定タイミングにおける前記物標検出部の検出範囲外に位置する過去検出物標を検出した過去推定タイミングである。好ましくは、上述の自己位置推定装置において、前記物標存在確率分布は、前記物標検出部の検出誤差（検出精度）に応じて予め適宜に設定される。好ましくは、上述の自己位置推定装置において、前記移動体存在確率分布は、前記仮自己位置検出部の検出誤差（検出精度）に応じて予め適宜に設定される。好ましくは、上述の自己位置推定装置において、前記物標検出部の個々におけるエラーのクセを勘案するために、前記第１確率分布変換部は、前記物標の相対位置を、予め設定された第１オフセット量だけずらしてから、前記物標存在確率分布を求める。好ましくは、上述の自己位置推定装置において、前記仮自己位置検出部の個々におけるエラーのクセを勘案するために、前記第２確率分布変換部は、前記仮自己位置を、予め設定された第２オフセット量だけずらしてから、前記移動体存在確率分布を求める。好ましくは、上述の自己位置推定装置において、前記標存在確率分布処理部は、前記物標存在確率分布および前記移動体存在確率分布それぞれを分散共分散行列で表した場合に、前記過去検出物標の前記物標存在確率分布の分散共分散行列と、前記仮自己位置の前記移動体存在確率分布の分散共分散行列との和を求めることによって、前記移動体考慮物標存在確率分布を求める。好ましくは、上述の自己位置推定装置において、前記推定部は、推定結果を評価するための所定の評価関数（コスト関数）を最適化することによって、前記現在の自己位置を推定し、前記評価関数は、前記現在検出物標および前記過去検出物標それぞれの各位置、前記現在検出物標の物標存在確率分布、ならびに、前記過去検出物標の移動体考慮物標存在確率分布に基づく関数である。好ましくは、前記評価関数は、前記現在検出物標の物標存在確率分布に対する前記現在検出物標の位置におけるマハラノビス距離、および、前記過去検出物標の移動体考慮物標存在確率分布に対する前記過去検出物標の位置におけるマハラノビス距離の二乗和である。
【0008】
このような自己位置推定装置は、物標検出部で検出した物標の相対位置を、所定の物標存在確率分布に変換するので、前記物標検出部の検出誤差（検出精度）を考慮でき、仮自己位置検出部で検出した仮自己位置を、所定の移動体存在確率分布に変換するので、前記仮自己位置検出部の検出誤差（検出精度）を考慮できるから、推定精度を改善できる。
【0009】
他の一態様では、上述の自己位置推定装置において、前記物標検出部は、前記移動体と前記物標との間の距離が短いほど小さい誤差で前記物標を検出し、前記推定部は、誤差が所定の閾値以下である前記現在検出物標および前記過去検出物標、または、前記誤差が前記所定の閾値より小さい前記現在検出物標および前記過去検出物標、を用いて、前記現在の自己位置を推定する。
【0010】
このような自己位置推定装置は、誤差が所定の閾値以下またはより小さい前記現在検出物標および前記過去検出物標を用いて、前記現在の自己位置を推定するので、より高い精度で自己位置を推定できる。
【0011】
他の一態様では、上述の自己位置推定装置において、前記物標検出部は、前記移動体と前記物標との間の距離が短いほど小さい誤差で前記物標を検出し、前記推定部は、前記現在検出物標および前記過去検出物標の個数の総和が所定の第２閾値以上である場合、または、前記現在検出物標および前記過去検出物標の個数の総和が前記所定の第２閾値より多い場合、前記現在検出物標および前記過去検出物標の中から、誤差が小さい順に所定の個数だけ抽出し、前記抽出した前記現在検出物標および前記過去検出物標を用いて、前記現在の自己位置を推定する。
【0012】
このような自己位置推定装置は、所定の個数で現在の自己位置を推定するので、推定処理を簡略化できる。
【0013】
本発明の他の一態様にかかる自己位置推定方法は、移動体で実行される、自己位置を推定する自己位置推定方法であって、前記移動体の周囲に存在する物標を検出し、前記検出した物標の、前記移動体に対する相対位置を検出する物標検出工程と、過去に自己位置を推定した過去推定タイミングから、現在の自己位置を推定する現在推定タイミングまでにおける前記移動体の移動状況に関する所定の移動情報を検出し、前記検出した移動情報に基づいて現在の自己位置を仮自己位置として検出する仮自己位置検出工程と、前記物標検出工程で検出した物標の相対位置を、物標の存在確率の分布である所定の物標存在確率分布に変換する第１確率分布変換工程と、前記仮自己位置検出工程で検出した仮自己位置を、移動体の存在確率の分布である所定の移動体存在確率分布に変換する第２確率分布変換工程と、前記過去推定タイミングにおいて前記物標検出工程で検出した物標を過去検出物標とした場合に、前記過去検出物標の、前記第１確率分布変換工程で求められる物標存在確率分布、および、前記現在推定タイミングにおいて前記仮自己位置検出工程で検出した仮自己位置の、前記第２確率分布変換工程で求められる移動体存在確率分布に基づいて、前記仮自己位置から見た前記過去検出物標の物標存在確率分布を移動体考慮物標存在確率分布として求める物標存在確率分布処理工程と、前記現在推定タイミングにおいて前記物標検出工程で検出した物標を現在検出物標とした場合に、所定の高精度地図における、前記現在検出物標および前記過去検出物標それぞれの各位置、前記現在検出物標の、前記第１確率分布変換工程で求められる物標存在確率分布、ならびに、前記物標存在確率分布処理工程で求めた前記過去検出物標の移動体考慮物標存在確率分布に基づいて、現在の自己位置を推定する推定工程とを備える。
【0014】
このような自己位置推定方法は、物標検出工程で検出した物標の相対位置を、所定の物標存在確率分布に変換するので、前記物標検出工程での検出誤差（検出精度）を考慮でき、仮自己位置検出工程で検出した仮自己位置を、所定の移動体存在確率分布に変換するので、前記仮自己位置検出工程での検出誤差（検出精度）を考慮できるから、推定精度を改善できる。
【発明の効果】
【0015】
本発明にかかる自己位置推定装置および自己位置推定方法は、推定精度を改善できる。
【図面の簡単な説明】
【0016】
【図1】実施形態における自己位置推定装置の構成を示すブロック図である。
【図2】前記自己位置推定装置の物標検出部による物標の検出の様子を説明するための図である。
【図3】位置を存在確率分布に変換する処理を説明するための図である。
【図4】オドメトリで求めた現在の仮自己位置から見た過去検出物標の物標存在確率分布を求める処理を説明するための図である。
【図5】方位に関する分散共分散行列の算出方法の考え方を説明するための図である。
【図6】前記自己位置推定装置の動作を示すフローチャートである。
【発明を実施するための形態】
【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】
【数1】
【0046】
物標存在確率分布が例えばガウス関数で表され、その分散共分散行列Σｃａｍを次式２とした場合、物標位置μ１（ｘ’ａ、ｙ’ｂ）、分散共分散行列Σｃａｍの測定データにおける真値位置ν１（ｘ１、ｙ１）の確率密度関数ｆ（ν１）は、次式３によって表される。分散共分散行列Σｃａｍにおいて、ａは、ｘの分散であり、ｃは、ｙの分散であり、ｂは、ｘとｙとの共分散である。
【0047】
【数2】
【0048】
【数3】
【0049】
同様に、移動量に関し、移動体存在確率分布が例えばガウス関数で表され、その分散共分散行列Σｏｄｏを次式４とした場合、仮自己位置μ２（ｘｃ、ｙｄ）、分散共分散行列Σｏｄｏの測定データにおける真値位置ν２（ｘ２、ｙ２）の確率密度関数は、次式５によって表される。分散共分散行列Σｃａｍにおいて、ｄは、ｘの分散であり、ｆは、ｙの分散であり、ｅは、ｘとｙとの共分散である。
【0050】
【数4】
【0051】
【数5】
【0052】
方位に関し、その分散をσとした場合、図５に示すように、自車両ＶＣから見た、方位θ、距離ｌにある物標を方位の精度（分散）σで観測しているとみなすことができる。σ＜＜１として近似すると、方位θに直交する方向に精度がｌｓｉｎσだけ劣化すると考えられ、分散共分散行列Σａｎｇｌｅは、次式６によって表される。
【0053】
【数6】
【0054】
推定部３６は、前記現在推定タイミングにおいて前記物標検出部１で検出した物標を現在検出物標とした場合に、前記高精度地図記憶部５１に記憶された高精度地図における、前記現在検出物標および前記過去検出物標それぞれの各位置、前記現在検出物標の、前記第１確率分布変換部３３で求められる物標存在確率分布、ならびに、前記物標存在確率分布処理部３５で求めた前記過去検出物標の移動体考慮物標存在確率分布に基づいて、現在の自己位置を推定するものである。これによって統計的に最も起こり易い位置で現在の自己位置が推定される。例えば、上述の図４Ｂに示すように、現在推定タイミングｔｋで物標検出部１で第３物標Ｏｂ－３を検出した場合において、推定部３６は、前記高精度地図記憶部５１に記憶された高精度地図における、前記現在検出物標の第３物標Ｏｂ－３および前記過去検出物標の第１および第２物標Ｏｂ－１、Ｏｂ－２それぞれの各位置、前記現在検出物標である第３物標Ｏｂ－３の、前記第１確率分布変換部３３で求められる物標存在確率分布ＰＤ１、ならびに、前記物標存在確率分布処理部３５で求めた前記過去検出物標である第１物標Ｏｂ－１の移動体考慮物標存在確率分布ＰＤＣ、および、前記物標存在確率分布処理部３５で求めた前記過去検出物標である第２物標Ｏｂ－２の移動体考慮物標存在確率分布ＰＤＣ、に基づいて、現在の自己位置ＰＶ（ｔｋ）を推定する。ここで、前記過去推定タイミングは、前記現在推定タイミングにおける前記物標検出部の検出範囲外に位置する過去検出物標を検出した過去推定タイミングであってよい。例えば、図４に示す場合では、現在推定タイミングｔｋでは、物標検出部１の検出範囲ＲＧ外に位置する過去検出物標の第１および第２物標Ｏｂ－１、Ｏｂ－２を検出した過去推定タイミングｔｋ－１が用いられる。
【0055】
より具体的には、推定部３６は、前記現在検出物標の種類および前記過去検出物標の種類に基づいて、前記高精度地図記憶部５１に記憶された高精度地図における、前記現在検出物標および前記過去検出物標それぞれの各位置を求める。なお、推定部３６は、前記高精度地図記憶部５１に記憶された高精度地図における検索範囲を絞り込んでもよい。すなわち、推定部３６は、現在推定タイミングでの仮自己位置を中心に所定範囲内における高精度地図から、物標検出部１で検出した前記現在検出物標の種類と一致する種類の物標を選定し、前記選定した物標の位置を前記現在検出物標の位置として求め、過去推定タイミングでの自己位置（または仮自己位置）を中心に所定範囲内における高精度地図から、物標検出部１で検出した前記過去検出物標の種類と一致する種類の物標を選定し、前記選定した物標の位置を前記過去検出物標の位置として求める。後述のように、本実施形態では、各推定タイミングで繰り返し自己位置が推定されているので、前記過去検出物標の位置には、前記過去推定タイミングが現在推定タイミングであった際に現在検出物標の位置として求められた位置が用いられてもよい。
【0056】
そして、推定部３６は、推定結果を評価するための所定の評価関数（コスト関数）を最適化することによって、前記現在の自己位置を推定し、前記評価関数は、前記現在検出物標および前記過去検出物標それぞれの各位置、前記現在検出物標の物標存在確率分布、ならびに、前記過去検出物標の移動体考慮物標存在確率分布に基づく関数である。前記評価関数の最適化は、当該評価関数の評価が最も高くなる前記現在の自己位置を探索することであり、前記評価関数が最適化された場合における自己位置が、推定部３６で推定した前記現在の自己位置となる。前記評価関数は、例えば、前記現在検出物標の物標存在確率分布に対する前記現在検出物標の位置におけるマハラノビス距離、および、前記過去検出物標の移動体考慮物標存在確率分布に対する前記過去検出物標の位置におけるマハラノビス距離の二乗和である。このため、前記評価関数（コスト関数）の最適化は、当該評価関数の値が最も小さくなる前記現在の自己位置を探索することである。定性的には、前記現在検出物標および前記過去検出物標について、マハラビノス距離で最もフィットする自己位置が探索される。前記評価関数の最適化には、公知の常套手段、例えば、全量探索のグリッドサーチ、勾配法の最急降下方、確率的最急降下法およびＡＤＡＭ（Ａｄａｐｔｉｖｅ Ｍｏｍｅｎｔ Ｅｓｔｉｍａｔｉｏｎ）、ならびに、解析的手法の最小二乗法等が用いられる。
【0057】
この評価関数ｆ（Ｐ、Ａ）は、ワールド座標系での自己位置における位置および方位（自車両の向き）それぞれをＰ（Ｘ、Ｙ）およびＡとした場合、次式７によって表され、前記過去検出物標および前記現在検出物標における物標ｉのマハラノビス距離の二乗ｄｉは、次式８によって表され、物標ｉのワールド座標系での分散共分散行列Σ’ｏｂｊｉは、ローカル座標系からワールド座標系への変換から、次式９によって表され、方位Ａへの回転行列Ｒは、次式１０によって表される。
【0058】
【数7】
【0059】
【数8】
【0060】
【数9】
【0061】
【数10】
【0062】
ここで、ｍｉは、物標ｉの高精度地図での位置（Ｘｍｉ、Ｙｍｉ）である。物標検出部１で検出された物標ｉのワールド座標系での位置ｏｉ（Ｘｉ、Ｙｉ）は、物標検出部１で検出されたローカル座標系での位置μｉ（ｘ’ａｉ、ｙ’ｂｉ）に対し、次式１１によって表される。
【0063】
【数11】
【0064】
なお、上述では、ローカル座標系およびワールド座標系が２次元である場合について説明したが、ローカル座標系およびワールド座標系は、３次元であってよく、この場合も上述と同様に説明できる。
【0065】
このような制御処理部３および記憶部５は、いわゆるＥＣＵ（Ｅｌｅｃｔｒｏｎｉｃ Ｃｏｎｔｒｏｌ Ｕｎｉｔ）と呼称される、車両に搭載されたコンピュータによって構成可能である。
【0066】
次に、本実施形態の動作について説明する。図６は、前記自己位置推定装置の動作を示すフローチャートである。
【0067】
このような自己位置推定装置ＤＳは、車両が稼働を始めると、必要な各部の初期化を実行し、その稼働を始める。その制御処理プログラムの実行によって、制御処理部３には、制御部３１、自律航法処理部３２、第１確率分布変換部３３、第２確率分布変換部３４、物標存在確率分布処理部３５および推定部３６が機能的に構成される。
【0068】
自己位置推定装置Ｄは、車両の稼働終了等の終了まで、所定の時間間隔ごとの各推定タイミングで図６に示す処理Ｓ１ないし処理Ｓ８の各処理を繰り返し、実行する。ここで、前記現在推定タイミングは、今回の推定タイミングであり、前記過去推定タイミングは、例えば、前回の推定タイミングとする。なお、前記過去推定タイミングは、前々回の推定タイミング等としてもよい。
【0069】
図６において、現在推定タイミング（今回の推定タイミング）になると、自己位置推定装置Ｄは、制御処理部３の制御部３１によって、物標検出部１から、その検出した物標における種類および相対位置を取得し、これらを前記今回の推定タイミングと対応付けて記憶部５に記憶し、仮自己位置検出部２におけるロータリエンコーダ２１およびヨーレートセンサ２２それぞれから取得した各測定結果に基づき、自律航法処理部３２（２３）によって仮自己位置を求め、これを前記今回の推定タイミングと対応付けて記憶部５に記憶する（Ｓ１）。前記推定タイミングは、例えば番号で表され、最初の推定タイミングから順番に各推定タイミングに連番で番号が割り振られる。この処理Ｓ１で物標検出部１で検出した物標は、前記現在推定タイミングにおいて前記物標検出部１で検出した物標であるので、現在検出物標である。
【0070】
続いて、自己位置推定装置ＤＳは、制御処理部３の制御部３１によって、前記処理Ｓ１で求めた仮自己位置で自己位置が特定できるか否かを判定する（Ｓ２）。この判定の結果、特定できる場合には、自己位置推定装置ＤＳは、次に、処理Ｓ７を実行する。例えば、制御部３１は、前記仮自己位置に基づき前記処理Ｓ１で検出した物標の相対位置をワールド座標系の位置に変換し、前記処理Ｓ１で検出した物標の種類を検索キーとして、高精度地図記憶部５１に記憶されている高精度地図から、前記処理Ｓ１で検出した物標に対応する物標を選定し、前記高精度地図から、前記選定した物標の位置を取り出し、前記変換した位置と前記取り出した位置との差が所定の範囲（第１範囲）内である場合には、前記処理Ｓ１で求めた仮自己位置で自己位置が特定できると、判定する。あるいは例えば、物標検出部１が、移動体（上述では車両）と物標との間の距離が短いほど小さい誤差で前記物標を検出できる場合、前記処理Ｓ１で検出した物標までの距離（相対距離）が所定の範囲（第２範囲）内である場合には、制御部３１は、前記処理Ｓ１で求めた仮自己位置で自己位置が特定できると、判定する。なお、この場合、前記誤差で前記仮自己位置が補正され、補正後の仮自己位置が前記自己位置とされてもよい。一方、前記判定の結果、特定できない場合には、自己位置推定装置Ｄは、次に、処理Ｓ３を実行する。上述の例では、前記変換した位置と前記取り出した位置との差が前記第１範囲外である場合や前記処理Ｓ１で検出した物標までの距離（相対距離）が前記第２範囲外である場合には、制御部３１は、前記処理Ｓ１で求めた仮自己位置で自己位置が特定できないと、判定する。
【0071】
この処理Ｓ３では、自己位置推定装置Ｄは、制御処理部３の第１確率分布変換部３３によって、過去推定タイイングにおいて前記物標検出部１で検出した物標（過去検出物標）の相対位置を、物標存在確率分布に変換し、これを記憶部５に記憶する。なお、前記過去推定タイミングにおいて複数の物標（過去検出物標）が検出された場合には、第１確率分布変換部３３は、前記複数の過去検出物標それぞれについて、当該過去検出物標の相対位置を物標存在確率分布に変換する。
【0072】
続いて、自己位置推定装置Ｄは、制御処理部３の第２確率分布変換部３４によって、前記処理Ｓ１において前記仮自己位置検出部２で検出した仮自己位置を移動体存在確率分布に変換し、これを記憶部５に記憶する（Ｓ４）。
【0073】
続いて、自己位置推定装置Ｄは、制御処理部３の物標存在確率分布処理部３５によって、前記処理Ｓ３で求めた過去検出物標の物標存在確率分布、および、前記処理Ｓ４で求めた仮自己位置の移動体存在確率分布に基づいて、前記過去検出物標の移動体考慮物標存在確率分布を求め、これを記憶部５に記憶する（Ｓ５）。なお、前記過去推定タイミングにおいて複数の物標（過去検出物標）が検出された場合には、物標存在確率分布処理部３５は、前記複数の過去検出物標それぞれについて、当該過去検出物標の移動体考慮物標存在確率分布を求める。
【0074】
続いて、自己位置推定装置Ｄは、第１確率分布変換部３３によって前記現在検出物標の物標存在確率分布を求め、制御処理部３の推定部３６によって、前記高精度地図記憶部５１に記憶された高精度地図における、前記現在検出物標および前記過去検出物標それぞれの各位置、前記現在検出物標の物標存在確率分布、ならびに、前記処理Ｓ５で求めた前記過去検出物標の移動体考慮物標存在確率分布に基づいて、現在の自己位置を推定し、これを前記今回の推定タイミングと対応付けて記憶部５に記憶する（Ｓ６）。
【0075】
続いて、自己位置推定装置ＤＳは、制御部３１によって、前記処理Ｓ６で求めた現在の自己位置を表示部４に表示する。例えば、高精度地図記憶部５１に記憶された高精度地図が表示部４に表示され、この表示された高精度地図上に、現在の自己位置が所定のマークで表示される。
【0076】
続いて、自己位置推定装置Ｄは、制御部３１によって、終了か否かを判定する。この判定の結果、終了である場合（Ｙｅｓ）には、自己位置推定装置Ｄは、本処理を終了する。例えば、前記処理Ｓ１ないし処理Ｓ７の各処理の間に、車両の稼働終了等があった場合には、制御部３１は、終了と判定する。一方、前記判定の結果、終了ではない場合（Ｎｏ）には、次回の推定タイミングで前記処理Ｓ１ないし処理Ｓ８の各処理を実行するために、制御部３１は、処理を処理Ｓ１に戻す。
【0077】
以上説明したように、実施形態における自己位置推定装置Ｄおよびこれに実装された自己位置推定方法は、物標検出部１で検出した物標の相対位置を、所定の物標存在確率分布に変換するので、前記物標検出部１の検出誤差（検出精度）を考慮でき、仮自己位置検出部２で検出した仮自己位置を、所定の移動体存在確率分布に変換するので、前記仮自己位置検出部の検出誤差（検出精度）を考慮できるから、推定精度を改善できる。
【0078】
なお、上述の実施形態において、物標検出部１が物標を検出する際の誤差に応じて、推定部３６で現在の自己位置を推定する際に用いる現在検出物標および過去検出物標が限定されてもよい。
【0079】
例えば、第１態様として、物標検出部１は、移動体（上述では車両）と物標との間の距離が短いほど小さい誤差で前記物標を検出し、推定部３６は、誤差が所定の第１閾値Ｔｈ１以下である前記現在検出物標および前記過去検出物標、または、前記誤差が前記所定の第１閾値Ｔｈ１より小さい前記現在検出物標および前記過去検出物標、を用いて、前記現在の自己位置を推定する。前記所定の第１閾値Ｔｈ１は、予め適宜に設定される。このような自己位置推定装置Ｄは、誤差が所定の第１閾値Ｔｈ１以下またはより小さい前記現在検出物標および前記過去検出物標を用いて、前記現在の自己位置を推定するので、より高い精度で自己位置を推定できる。
【0080】
また例えば、第２態様として、物標検出部１は、移動体（上述では車両）と物標との間の距離が短いほど小さい誤差で前記物標を検出し、推定部３６は、前記現在検出物標および前記過去検出物標の個数の総和が所定の第２閾値Ｔｈ２以上である場合、または、前記現在検出物標および前記過去検出物標の個数の総和が前記所定の第２閾値Ｔｈ２より多い場合、前記現在検出物標および前記過去検出物標の中から、誤差が小さい順に所定の個数Ｎだけ抽出し、前記抽出した前記現在検出物標および前記過去検出物標を用いて、前記現在の自己位置を推定する。前記所定の第２閾値Ｔｈ２は、予め適宜に設定される。このような自己位置推定装置Ｄは、所定の個数で現在の自己位置を推定するので、推定処理を簡略化できる。
【0081】
本発明を表現するために、上述において図面を参照しながら実施形態を通して本発明を適切且つ十分に説明したが、当業者であれば上述の実施形態を変更および／または改良することは容易に為し得ることであると認識すべきである。したがって、当業者が実施する変更形態または改良形態が、請求の範囲に記載された請求項の権利範囲を離脱するレベルのものでない限り、当該変更形態または当該改良形態は、当該請求項の権利範囲に包括されると解釈される。
【符号の説明】
【0082】
Ｄ 自己位置推定装置
１ 物標検出部
２ 仮自己位置検出部
３ 制御処理部
４ 表示部
５ 記憶部
２１ ロータリエンコーダ
２２ ヨーレートセンサ
３１ 制御部
３２（２３） 自律航法処理部
３３ 第１確率分布変換部
３４ 第２確率分布変換部
３５ 物標存在確率分布処理部
３６ 推定部
５１ 高精度地図記憶部
１ 物標検出部
２ 仮自己位置検出部
３ 制御処理部
４ 表示部
５ 記憶部
２１ ロータリエンコーダ
２２ ヨーレートセンサ
３１ 制御部
３２（２３） 自律航法処理部
３３ 第１確率分布変換部
３４ 第２確率分布変換部
３５ 物標存在確率分布処理部
３６ 推定部
５１ 高精度地図記憶部
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
