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

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

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

(11)【特許番号】6575686

(24)【登録日】2019年8月30日

(45)【発行日】2019年9月18日

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

(51)【国際特許分類】

   G01C 21/30 20060101AFI20190909BHJP

   G05D 1/02 20060101ALI20190909BHJP

【ＦＩ】

   G01C21/30

   G05D1/02 H

【請求項の数】7

【全頁数】16

(21)【出願番号】特願2018-530245(P2018-530245)

(86)(22)【出願日】2016年7月26日

(86)【国際出願番号】JP2016071922

(87)【国際公開番号】WO2018020589

(87)【国際公開日】20180201

【審査請求日】2018年11月5日

(73)【特許権者】

【識別番号】000003997

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

(74)【代理人】

【識別番号】100103850

【弁理士】

【氏名又は名称】田中 秀▲てつ▼

(74)【代理人】

【識別番号】100114177

【弁理士】

【氏名又は名称】小林 龍

(74)【代理人】

【識別番号】100066980

【弁理士】

【氏名又は名称】森 哲也

(72)【発明者】

【氏名】▲高▼野 博幸

(72)【発明者】

【氏名】佐野 泰仁

(72)【発明者】

【氏名】土谷 千加夫

(72)【発明者】

【氏名】南里 卓也

【審査官】 白石 剛史

(56)【参考文献】

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

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

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

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

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

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

Ｇ０１Ｃ ２１／３０

Ｇ０５Ｄ １／０２

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

【請求項1】

移動体の周囲に存在する物標の前記移動体に対する相対位置を検出し、
前記移動体の移動量を推定し、
前記移動体の移動量に基づき前記相対位置を補正して物標位置データとして蓄積し、
前記移動体の走行路の勾配量を検出し、
蓄積した前記物標位置データのうち、勾配量が閾値未満である区間の物標の物標位置データを選択し、
選択した前記物標位置データと２次元地図上の前記物標の位置を示す地図情報とを照合することにより前記移動体の現在位置を推定する、
ことを特徴とする自己位置推定方法。

【請求項2】

前記閾値以上の勾配量を有する勾配区間を通過してから現在位置までの区間の物標の物標位置データを選択して、前記地図情報と照合することを特徴とする請求項１に記載の自己位置推定方法。

【請求項3】

前記勾配区間へ進入する前に前記閾値未満の勾配量を有する第１区間の物標の物標位置データと前記地図情報とを照合することにより前記勾配区間へ進入する前の前記移動体の第１位置を推定し、
前記勾配区間を通過した後に前記閾値未満の勾配量を有する第２区間の物標の物標位置データと前記地図情報とを照合することにより前記勾配区間を通過した後の前記移動体の第２位置を推定し、
前記第１位置と前記第２位置との相対位置に基づいて、前記第１区間の物標の前記物標位置データを補正する、
ことを特徴とする請求項２に記載の自己位置推定方法。

【請求項4】

前記閾値以上の勾配量を有する区間が所定長以上継続した場合に、前記閾値未満の勾配量を有する区間の物標の物標位置データを選択して前記地図情報と照合し、
前記閾値以上の勾配量を有する区間が前記所定長以上継続しない場合に、前記閾値以上の勾配量を有する区間の物標の物標位置データを、前記地図情報と照合する物標位置データに含める、
ことを特徴とする請求項１〜３の何れか一項に記載の自己位置推定方法。

【請求項5】

前記移動体の走行路の勾配量が大きいほど前記所定長を短く設定することを特徴とする請求項４に記載の自己位置推定方法。

【請求項6】

前記移動体の現在位置の周囲の物標の物標位置データを選択して前記地図情報と照合することを特徴とする請求項１〜５のいずれか一項に記載の自己位置推定方法。

【請求項7】

移動体の周囲に存在する物標の前記移動体に対する相対位置を検出する物標検出センサと、
前記移動体の車輪速を検出する車輪速センサと、
前記移動体の走行路の勾配量を検出する勾配検出センサと、
少なくとも前記車輪速センサの検出結果に応じて前記移動体の移動量を推定し、前記物標検出センサが検出した前記相対位置を前記移動量に基づき補正して物標位置データとして記憶装置に蓄積し、蓄積した前記物標位置データのうち、勾配量が閾値未満である区間の物標の物標位置データを選択し、選択した前記物標位置データと２次元地図上の前記物標の位置を示す地図情報とを照合することにより前記移動体の現在位置を推定する自己位置推定回路と、
を備えることを特徴とする自己位置推定装置。

【発明の詳細な説明】

【技術分野】

【0001】

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

【背景技術】

【0002】

既知の物標と移動体との間の相対位置を検出して移動体の位置を推定する技術として、特許文献１に記載の技術が知られている。
特許文献１に記載のロボットは、移動可能な領域を点集合データで示す点環境地図と、ロボットに搭載したレーザーレンジセンサの検出結果を点集合データで示す周囲環境情報との位置ずれ量に基づき、ロボットの自己位置の推定結果を補正する。

【先行技術文献】

【特許文献】

【0003】

【特許文献1】特開２００８‐２５０９０６号公報

【発明の概要】

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

【0004】

勾配区間では斜距離と水平距離との間に差があるため、２次元地図上の移動体の位置の推定精度が低下するおそれがある。
本発明は、勾配区間における斜距離と水平距離との間の差に起因する２次元地図上の位置の推定精度の低下を抑制することを目的とする。

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

【0005】

本発明の一態様に係る自己位置推定方法では、移動体の周囲に存在する物標の相対位置を検出し、移動体の移動量に基づき相対位置を補正して物標位置データとして蓄積する。蓄積した物標位置データのうち、勾配量が閾値未満である区間の物標の物標位置データと２次元地図上の物標の位置を示す地図情報とを照合することにより前記移動体の現在位置を推定する。

【発明の効果】

【0006】

本発明の一態様によれば、勾配区間における斜距離と水平距離との間の差に起因する２次元地図上の位置の推定精度の低下を抑制できる。
本発明の目的及び利点は、特許請求の範囲に示した要素及びその組合せを用いて具現化され達成される。前述の一般的な記述及び以下の詳細な記述の両方は、単なる例示及び説明であり、特許請求の範囲のように本発明を限定するものでないと解するべきである。

【図面の簡単な説明】

【0007】

【図1】実施形態の自己位置推定装置を搭載した車両の概略構成の一例のブロック図である。

【図2】自己位置推定回路の概略構成の一例のブロック図である。

【図3】物標位置データと地図情報との照合による自己位置の推定方法の一例の説明図である。

【図4】勾配に起因する物標位置データの誤差の説明図である。

【図5】勾配に起因する車両の位置推定誤差の説明図である。

【図6】選択済物標位置データの説明図である。

【図7】第１実施形態の自己位置推定方法の一例を示すフローチャートである。

【図8】勾配区間通過判定処理の一例を示すフローチャートである。

【図9】第２実施形態の自己位置推定回路の概略構成の一例のブロック図である。

【図10】第２実施形態の自己位置推定方法の一例を示すフローチャートである。

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

【0008】

以下、本発明の実施形態について、図面を参照しつつ説明する。
（第１実施形態）
（構成）
図１を参照する。以下、移動体の一例として車両の現在位置を推定する場合について説明するが、本発明は車両に限らず様々な移動体の現在位置の推定に広く適用することができる。
車両１には、自己位置推定装置２と運転支援システム３が搭載される。自己位置推定装置２は、撮像装置１０と、距離測定装置１１と、車輪速センサ１２と、操舵角センサ１３と、ジャイロセンサ１４と、加速度センサ１５と、自己位置推定回路１６を備える。

【0009】

撮像装置１０は、車両１の車室内などに取り付けられ、例えば車両１の前方領域を撮像する。撮像装置１０は、例えば広角カメラであってよい。撮像装置１０は、車両１の前方領域の撮像画像を自己位置推定回路１６へ出力する。
距離測定装置１１は、車両１の車室外などに取り付けられ、車両１の前方領域に電磁波を照射しその反射波を検出する。距離測定装置１１は、例えばレーザレンジファインダであってよい。また、距離測定装置１１の取り付け位置は、例えば車両１のボンネット、バンパー、ナンバープレート、ヘッドライト、又はサイドミラーの周辺であってよい。距離測定装置１１は、測定結果を自己位置推定回路１６へ出力する。

【0010】

車輪速センサ１２は、車両１の車輪が１回転する毎に予め設定した数の車輪速パルスを発生させる。車輪速センサ１２は、車輪速パルスを自己位置推定回路１６へ出力する。
操舵角センサ１３は、例えば、車両１のステアリングホイールを回転可能に支持するステアリングコラムに設けられる。操舵角センサ１３は、操舵操作子であるステアリングホイールの現在の回転角度（操舵操作量）である現在操舵角を検出する。操舵角センサ１３は、検出した現在操舵角を自己位置推定回路１６へ出力する。

【0011】

ジャイロセンサ１４は、車両１に発生するヨーレート、ピッチ方向の変位量、及びロール方向の変位量を検出する。ジャイロセンサ１４は、検出したヨーレート、ピッチ方向の変位量、及びロール方向の変位量を自己位置推定回路１６へ出力する。
加速度センサ１５は、車両１に発生する車幅方向の加減速度である横Ｇ、及び前後方向の加減速度を検出する。加速度センサ１５は、検出した横Ｇ及び前後方向の加減速度を自己位置推定回路１６へ出力する。

【0012】

自己位置推定回路１６は、ＣＰＵ（Central Processing Unit）等のプロセッサ、記憶装置及び周辺部品を含む電子回路装置である。
自己位置推定回路１６は、撮像装置１０、距離測定装置１１、車輪速センサ１２、操舵角センサ１３、及びジャイロセンサ１４から受信した信号と、既知の物標の２次元地図上の位置を示す２次元地図情報に基づいて、車両１の地図上の現在位置を推定する。以下、車両１の地図上の現在位置を「自己位置」と表記することがある。自己位置推定回路１６は、自己位置を示す自己位置信号を運転支援システム３へ出力する。

【0013】

運転支援システム３は、自己位置推定回路１６から受信した自己位置信号が示す自己位置を用いて、運転者による車両１の運転に対する運転支援を行う。
運転支援の一例は、例えば運転者に対する警報等の情報提供であってよい。運転支援システム３は、車両１の自己位置に応じて運転者に提示する警報の種類及び強度の少なくとも一方を制御してよい。
運転支援の一例は、車両１の制動制御、加速制御及び操舵制御の少なくとも１つを含む車両１の走行状態の制御であってもよい。例えば運転支援システム３は、車両１の自己位置に応じて車両１に制動力及び駆動力のいずれを発生させるのかを決定してよい。

【0014】

次に、自己位置推定回路１６の構成を説明する。図２を参照する。自己位置推定回路１６は、物標位置検出部２０と、移動量推定部２１と、勾配検出部２２と、物標位置蓄積部２３と、記憶装置２４と、選択部２５と、位置推定部２６と、地図情報取得部２７を備える。
自己位置推定回路１６が備えるプロセッサは、記憶装置２４に格納されたコンピュータプログラムを実行することにより、物標位置検出部２０、移動量推定部２１、勾配検出部２２、物標位置蓄積部２３、選択部２５、位置推定部２６、及び地図情報取得部２７の機能を実現する。

【0015】

物標位置検出部２０は、撮像装置１０が生成した車両１の前方領域の撮像画像を受信する。また、物標位置検出部２０は、距離測定装置１１の測定結果を受信する。
物標位置検出部２０は、車両１の前方領域の撮像画像と距離測定装置１１の測定結果に基づいて、車両１の周囲に存在する物標を検出する。例えば物標位置検出部２０は、車両１の前方に存在する物標を検出する。
さらに、物標位置検出部２０は、車両１に対する物標の相対位置を検出する。物標位置検出部２０は、検出した相対位置を示す相対位置信号を物標位置蓄積部２３へ出力する。
ここで物標とは、例えば車両１が走行する走行路面上の線（車線区分線等）や、路肩の縁石、ガードレール等であってよい。

【0016】

移動量推定部２１は、車輪速センサ１２、操舵角センサ１３及びジャイロセンサ１４から、それぞれ車輪速パルス、現在操舵角及びヨーレートを受信する。移動量推定部２１は、車輪速センサ１２、操舵角センサ１３及びジャイロセンサ１４から受信したこれらの信号に基づいて、前回の処理周期で車両１の自己位置を推定してから現在までの車両１の移動量ΔＰをオドメトリにより推定する。移動量推定部２１は、推定した移動量ΔＰを示す移動量信号を物標位置蓄積部２３へ出力する。

【0017】

勾配検出部２２は、ジャイロセンサ１４からピッチ方向の変位量を受信する。勾配検出部２２は、ジャイロセンサ１４から受信したピッチ方向の変位量に基づいて車両１の走行路の勾配量、すなわち車両１の走行方向の傾斜を検出する。
また、勾配検出部２２は、撮像装置１０が生成した車両１の前方領域の撮像画像を受信してもよい。勾配検出部２２は、撮像画像を解析することにより３Ｄ点群のフローに基づいて車両１の走行路の勾配量を検出してもよい。
勾配検出部２２は、車両１の走行路が勾配区間であるか否かを判定する。例えば勾配検出部２２は、車両１の走行路の勾配量が所定の閾値以上である場合に走行路が勾配区間であると判定してよい。勾配検出部２２は、判定結果を示す判定結果信号を選択部２５に出力する。

【0018】

物標位置蓄積部２３は、物標位置検出部２０から相対位置信号を受信し、移動量推定部２１から移動量信号を受信する。
物標位置蓄積部２３は、相対位置信号が示す車両１の周囲の物標の相対位置を記憶装置２４に蓄積する。
また、物標位置蓄積部２３は、過去に蓄積した物標の相対位置を、現在までの経過時間と移動量信号が示す移動量ΔＰを用いて、車両１の現在位置に対する相対位置へ補正する。すなわち、物標位置蓄積部２３は、現在までの経過時間に車両が移動した移動量ΔＰだけ車両１の移動方向と逆方向に相対位置を移動させる。

【0019】

物標位置蓄積部２３は、補正した相対位置である物標位置のデータ（以下「物標位置データ」と記載する場合がある）を記憶装置２４に蓄積する。
既に物標位置データが記憶装置２４に蓄積されている場合には、物標位置蓄積部２３は、移動量信号が示す移動量ΔＰを用いて蓄積された物標位置データを更新する。すなわち物標位置蓄積部２３は、蓄積された物標位置データの相対位置を、移動量ΔＰ分だけ車両１の移動方向と逆方向に移動させる。その後、物標位置蓄積部２３は、移動量ΔＰ分だけ相対移動させた相対位置を、蓄積していた物標位置データに上書きする。

【0020】

選択部２５は、記憶装置２４に蓄積されている物標位置データから、車両１の自己位置の推定に用いる物標位置データを選択する。自己位置の推定に用いるために選択される物標位置データを、以下「選択済物標位置データ」と記載する場合がある。
選択部２５が選択済物標位置データを選択する処理については後述する。

【0021】

位置推定部２６は、選択済物標位置データを、地図情報取得部２７が取得した２次元地図情報と照合することにより、車両１の自己位置を推定する。
地図情報取得部２７は、地図データと、地図データ上に存在する物標の２次元地図上の位置を示す２次元地図情報を取得する。例えば、地図情報取得部２７は、カーナビゲーションシステムや地図データベース等である。なお、地図情報取得部２７は、無線通信（路車間通信、または、車車間通信でも可）等の通信システムを介して外部から２次元地図情報を取得してもよい。この場合、地図情報取得部２７は、定期的に最新の２次元地図情報を入手して、保有する２次元地図情報を更新してもよい。また、地図情報取得部２７は、車両１が実際に走行した走路で検出した物標の位置情報を、２次元地図情報として蓄積してもよい。

【0022】

位置推定部２６は、例えば以下のようなデータ照合処理によって、選択済物標位置データと２次元地図情報とを照合して車両１の自己位置を推定してよい。
図３を参照する。参照符号Ｓ_ｉは選択済物標位置データを示す。インデックスｉは１〜Ｎの整数であり、Ｎは選択済物標位置データの個数である。
位置推定部２６は、前回の処理周期で推定した自己位置を移動量ΔＰで補正して車両１の仮位置を決定する。

【0023】

位置推定部２６は、車両１の２次元地図上の位置が仮位置であると仮定して、選択済物標位置データＳ_ｉが示す物標の相対位置を２次元地図上の絶対位置に変換する。位置推定部２６は、選択済物標位置データＳ_ｉの絶対位置に最も近い２次元地図情報中の物標の位置情報Ｍ_ｊを選択する。図３の例では、位置情報Ｍ_ｘが選択済物標位置データＳ_１に最も近く、位置情報Ｍ_ｙが選択済物標位置データＳ_２に最も近く、位置情報Ｍ_ｚが選択済物標位置データＳ_３に最も近い。
位置推定部２６は、選択済物標位置データＳ_ｉとこのデータに最も近い位置情報Ｍ_ｊとの距離Ｄ_ｉｊを算出し、以下の式（１）を用いて距離Ｄ_ｉｊの平均Ｓを算出する。

【0024】

【数1】

【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】

【図8】

【図9】

【図10】