JPWO2017017705A1 - 自己位置推定装置及び自己位置推定方法 - Google Patents

自己位置推定装置及び自己位置推定方法 Download PDF

Info

Publication number
JPWO2017017705A1
JPWO2017017705A1 JP2017530455A JP2017530455A JPWO2017017705A1 JP WO2017017705 A1 JPWO2017017705 A1 JP WO2017017705A1 JP 2017530455 A JP2017530455 A JP 2017530455A JP 2017530455 A JP2017530455 A JP 2017530455A JP WO2017017705 A1 JPWO2017017705 A1 JP WO2017017705A1
Authority
JP
Japan
Prior art keywords
self
host vehicle
target
target position
position data
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
JP2017530455A
Other languages
English (en)
Other versions
JP6365780B2 (ja
Inventor
宏寿 植田
宏寿 植田
山口　一郎
一郎 山口
千加夫 土谷
千加夫 土谷
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Nissan Motor Co Ltd
Original Assignee
Nissan Motor Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Nissan Motor Co Ltd filed Critical Nissan Motor Co Ltd
Publication of JPWO2017017705A1 publication Critical patent/JPWO2017017705A1/ja
Application granted granted Critical
Publication of JP6365780B2 publication Critical patent/JP6365780B2/ja
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0268—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means
    • G05D1/0272—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means comprising means for registering the travel distance, e.g. revolutions of wheels
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0268—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means
    • G05D1/0274—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means using mapping information stored in a memory device
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G01C21/30—Map- or contour-matching
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G01C21/1656—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with passive imaging devices, e.g. cameras
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
    • G05D1/0238—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors
    • G05D1/024—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors in combination with a laser
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
    • G05D1/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means

Abstract

自車両に搭載されて自車両の周囲に存在する物標と自車両との相対位置を検出する物標位置検出部と、自車両の移動量を推定する移動量推定部と、物標位置検出部が検出した相対位置を移動量推定部が推定した移動量だけ移動させて物標位置データとして蓄積する物標位置蓄積部と、地図上に存在する物標の物標位置情報を含む地図情報を取得する地図情報取得部と、自車両が走行する走行路のカーブ開始位置を推定するカーブ開始位置推定部と、物標位置蓄積部に蓄積した物標位置データのうち少なくとも自車両の周囲に存在する物標位置データ、及び自車両の現在位置からカーブ開始位置推定部が推定したカーブ開始位置までの間に存在する物標位置データと、地図情報取得部が取得した地図情報に含まれる物標位置情報とを照合して、自車両の現在位置である自己位置を推定する自己位置推定部と、を備える。

Description

本発明は、自己位置推定装置及び自己位置推定方法に関する。
移動体の現在位置である自己位置を推定する技術としては、例えば、特許文献１に記載されている構成のものがある。特許文献１に記載されている技術は、移動体を基準として予め定めた領域内に存在する周囲の環境情報を限定し、この限定した環境情報を、予め保持している環境地図と照合して、自己位置を推定する。
特開２００８‐２５０９０６号公報
しかしながら、上述した特許文献１の構成では、例えば、移動体が車両である場合、直線路の走行において、前後方向の位置推定を、左右輪の回転角と回転角速度に応じて移動体の移動距離と方向を求める方法（オドメトリ）によって行われる。
直線路からカーブ路への進入直後は、カーブ路よりも直線部における環境情報が多いため、オドメトリで自己位置を推定する技術では、カーブ路における環境情報が蓄積されるまでは、前後方向の位置に対する推定精度が低下する。したがって、オドメトリで自己位置を推定する技術では、直線路から進入したカーブ路で自己位置を推定する際に、自己位置の推定精度が低下するという問題が発生するおそれがある。
本発明は、上記のような問題点に着目してなされたもので、カーブ路における自己位置の推定精度低下を抑制することが可能な、自己位置推定装置及び自己位置推定方法を提供することを目的とする。
上記課題を解決するために、本発明の一態様は、自車両の周囲に存在する物標と自車両との相対位置を検出し、自車両の移動量と、自車両が走行する走行路のカーブ開始位置を推定する。さらに、検出した相対位置を推定した移動量だけ移動させて、物標位置データとして蓄積する。また、地図上に存在する物標の物標位置情報を含む地図情報を取得する。
これに加え、蓄積した物標位置データのうち少なくとも自車両の周囲に存在する物標位置データ、及び自車両の現在位置から推定したカーブ開始位置までの間に存在する物標位置データと、地図情報に含まれる物標位置情報とを照合して、自己位置を推定する。
本発明の一態様によれば、自車両が直線路からカーブ路へ進入しても、カーブの形状に沿った物標位置データを用いて、自己位置を推定する。なお、カーブの形状に沿った物標位置データは、少なくとも自車両の周囲に存在する物標位置データ及び自車両の現在位置からカーブ路開始位置までの間に存在するデータである。
これにより、カーブ路における自己位置の推定を、少なくとも自車両の周囲の走路に沿った物標位置データ及びカーブの形状に沿った物標位置データで行うことが可能となる。このため、カーブ路における自己位置の推定精度低下を抑制することが可能な自己位置推定装置及び自己位置推定方法を提供することが可能となる。
本発明の第一実施形態の自己位置推定装置の構成を表すブロック図である。 本発明の第一実施形態の自己位置推定装置を搭載した車両の構成を表す図である。 カーブ開始位置と、データ選択処理の説明図である。 データ選択処理の説明図である。 データ選択処理の説明図である。 データ照合処理の説明図である。 本発明の第一実施形態の自己位置推定装置が行う処理を表すフローチャートである。 本発明の第一実施形態の自己位置推定装置が行う処理を表すフローチャートである。 本発明の第二実施形態の自己位置推定装置が行う処理を表すフローチャートである。 本発明の第二実施形態の自己位置推定装置が行う処理を表すフローチャートである。
以下の詳細な説明では、本発明の実施形態について、完全な理解を提供するように、特定の細部について記載する。しかしながら、かかる特定の細部が無くとも、一つ以上の実施形態が実施可能であることは明確である。また、図面を簡潔なものとするために、周知の構造及び装置を、略図で表す場合がある。
（第一実施形態）
以下、本発明の第一実施形態について、図面を参照しつつ説明する。
（自己位置推定装置の構成）
図１から図６を用いて、第一実施形態の自己位置推定装置１の構成を説明する。
図１中に表すように、自己位置推定装置１は、物標位置検出部１０と、移動量推定部２０と、物標位置蓄積部３０と、地図情報取得部４０と、カーブ開始位置推定部５０と、自己位置推定部６０を備える。
物標位置検出部１０は、例えば、図２中に表すように、広角カメラ等の撮像装置２や、レーザーレンジファインダ（ＬＲＦ）等の距離測定装置４を用いて形成する。
撮像装置２は、図２中に表すように、自己位置推定装置１を備える車両（以降の説明では、「自車両」と記載する場合がある）に搭載する。また、撮像装置２は、自車両ＭＣの車室内に取り付けて、自車両ＭＣの前方領域を撮像する。
距離測定装置４は、図２中に表すように、自車両ＭＣの車室外に取り付けて、自車両ＭＣに搭載する。また、距離測定装置４は、自車両ＭＣの前方領域に電磁波を照射し、その反射波を検出する。なお、距離測定装置４を取り付ける位置は、例えば、ボンネット、バンパー、ナンバープレート、ヘッドライト、サイドミラー周辺である。
また、物標位置検出部１０は、撮像装置２や距離測定装置４により、自車両ＭＣの周囲、特に、自車両ＭＣの前方に存在する物標を検出する。さらに、物標位置検出部１０は、物標と自車両ＭＣとの相対位置を検出する。物標と自車両ＭＣとの相対位置を検出した物標位置検出部１０は、検出した相対位置を含む情報信号（以降の説明では、「相対位置信号」と記載する場合がある）を、物標位置蓄積部３０へ出力する。
ここで、物標とは、例えば、自車両ＭＣが走行する走行路面ＲＯ上の線（車線区分線等）や、路肩の縁石、ガードレール等である。
以上により、物標位置検出部１０は、自車両ＭＣに搭載し、自車両ＭＣの周囲に存在する物標と自車両ＭＣとの相対位置を検出する。
移動量推定部２０は、例えば、車輪速センサ６、操舵角センサ８、ジャイロセンサ（図示せず）等のセンサを用いて形成する。
車輪速センサ６は、図２中に表すように、自車両ＭＣに搭載する。また、車輪速センサ６は、自車両ＭＣが備える車輪Ｗの一回転について、予め設定した数の車輪速パルスを発生させる。
操舵角センサ８は、例えば、ステアリングホイール（図示せず）を回転可能に支持するステアリングコラム（図示せず）に設けて、図２中に表すように、自車両ＭＣに搭載する。また、操舵角センサ８は、操舵操作子であるステアリングホイールの現在の回転角度（操舵操作量）である現在操舵角を検出する。
ジャイロセンサは、自車両ＭＣに搭載し、自車両ＭＣに発生するヨーレートを検出する。
また、移動量推定部２０は、各センサが検出したパラメータを用いて、前回処理周期からの自車両ＭＣの移動量ΔＰを推定する。自車両ＭＣの移動量ΔＰを推定した移動量推定部２０は、推定した移動量ΔＰを含む情報信号（以降の説明では、「移動量信号」と記載する場合がある）を、物標位置蓄積部３０へ出力する。
以上により、移動量推定部２０は、自車両ＭＣに搭載し、自車両ＭＣの移動量を推定する。
物標位置蓄積部３０は、物標位置検出部１０から自車両ＭＣの周囲に存在する物標の相対位置信号の入力を受け、移動量推定部２０から移動量信号の入力を受ける。
そして、物標位置蓄積部３０は、自車両ＭＣの周囲に存在する物標の相対位置信号が含む相対位置を蓄積するとともに、過去に蓄積した物標の相対位置を、現在までの経過時間と、移動量信号が含む移動量を用いて、現在の自車両ＭＣに対する相対位置へ補正する。さらに、物標位置蓄積部３０は、補正した相対位置である物標位置のデータ（以降の説明では、「物標位置データ」と記載する場合がある）として蓄積する。
また、物標位置蓄積部３０は、既に物標位置データを蓄積している場合には、蓄積している物標位置データを、移動量信号が含む移動量を用いて更新する。
物標位置データの更新は、既に蓄積している物標位置データが含む相対位置を、移動量信号が含む移動量ΔＰ分だけ相対移動させる。これに加え、移動量ΔＰ分だけ相対移動させた相対位置を、蓄積している物標位置データに上書きして、物標位置データの更新を行う。
以上により、物標位置蓄積部３０は、自車両ＭＣに搭載する。また、物標位置蓄積部３０は、物標位置検出部１０が検出した相対位置のデータを蓄積する。これに加え、物標位置蓄積部３０は、過去に蓄積した物標の相対位置のデータを、移動量推定部２０が推定した現在までの経過時間の移動量ΔＰだけ自車両ＭＣの移動方向と逆方向に移動させて、物標位置データとして蓄積する。
地図情報取得部４０は、自車両ＭＣに搭載し、地図データと、地図データ上に存在する物標の位置情報を取得する。例えば、地図情報取得部４０は、カーナビゲーションシステムや地図データベース等である。
なお、地図情報取得部４０は、無線通信（路車間通信、または、車車間通信でも可）等の通信システムを介して外部から地図情報を取得しても良い。この場合、地図情報取得部４０は、定期的に最新の地図情報を入手して、保有する地図情報を更新しても良い。また、地図情報取得部４０は、自車が実際に走行した走路を、地図情報として蓄積しても良い。
以上により、地図情報取得部４０は、地図上に存在する物標の物標位置情報を含む地図情報を取得する。
カーブ開始位置推定部５０は、自車両ＭＣが走行している走行路が、カーブ区間であるか否かを判定する。自車両ＭＣが走行している走行路がカーブ区間であるか否かを判定したカーブ開始位置推定部５０は、判定結果を含む情報信号（以降の説明では、「判定結果信号」と記載する場合がある）を、自己位置推定部６０へ出力する。
これに加え、カーブ開始位置推定部５０は、自車両ＭＣが走行している走行路がカーブ区間であると判定すると、カーブ区間の開始位置（以降の説明では、「カーブ開始位置」と記載する場合がある）を推定する。
ここで、カーブ開始位置は、図３中に表すように、自車両ＭＣが走行している走行路において、走行路が直線区間からカーブ区間へ変化する位置である。また、図３中では、カーブ開始位置を符号「Ｃ０」で表す。また、図３中では、自車両ＭＣの重心を、符号「ＣＧ」で表す。
カーブ開始位置Ｃ０を推定したカーブ開始位置推定部５０は、推定したカーブ開始位置Ｃ０を含む情報信号（以降の説明では、「カーブ開始位置信号」と記載する場合がある）を、自己位置推定部６０へ出力する。
カーブ開始位置推定部５０が、自車両ＭＣが走行している走行路がカーブ区間であるか否かを判定する処理（カーブ区間判定処理）としては、例えば、以下のカーブ区間判定処理ＡＩ〜ＡＩＩＩのうち、少なくとも一つの処理を用いる。
（カーブ区間判定処理ＡＩ）
操舵角センサ８が検出した現在操舵角の絶対値が、予め設定した操舵角閾値未満から操舵角閾値以上となると、自車両ＭＣが走行している走行路が、カーブ区間であると判定する。
（カーブ区間判定処理ＡＩＩ）
ジャイロセンサが検出したヨーレートの絶対値が、予め設定したヨーレート閾値未満からヨーレート閾値以上となると、自車両ＭＣが走行している走行路が、カーブ区間であると判定する。
（カーブ区間判定処理ＡＩＩＩ）
地図情報取得部４０が取得した地図データに、前回の処理で自己位置推定部６０が推定した自己位置を入力する。そして、前回の処理で自己位置推定部６０が推定した自己位置が、地図データのカーブ区間上である場合に、自車両ＭＣが走行している走行路が、カーブ区間であると判定する。
以上により、カーブ開始位置推定部５０は、自車両ＭＣに搭載し、自車両ＭＣが走行する走行路のカーブ開始位置Ｃ０を推定する。
自己位置推定部６０は、物標位置蓄積部３０が蓄積している物標位置データから、自車両ＭＣの現在位置（自己位置）の推定に用いる物標位置データを選択する。なお、以降の説明では、自己位置の推定に用いるために選択した物標位置データを、「選択済物標位置データ」と記載する場合がある。また、自己位置の推定に用いる物標位置データを選択する処理の説明は、後述する。
さらに、自己位置推定部６０は、選択済物標位置データを、地図情報取得部４０が取得した物標の位置情報と照合することにより、自己位置を推定する。なお、選択済物標位置データを、地図情報取得部４０が取得した物標位置情報と照合する処理の説明は、後述する。
第一実施形態では、自己位置推定部６０の構成を、自車両ＭＣの周囲に存在する物標位置データ、及び自車両ＭＣの現在位置からカーブ路開始位置までの間の物標位置データを用いて自己位置を推定する構成とした場合について説明する。
ここで、自己位置の推定は、自車両ＭＣが直進走行した距離である直進走行距離ＬＳが、予め設定した推定用閾値距離ＬＳ０（例えば、ＬＳ０＝１００［ｍ］）以上の場合に行う。なお、直進走行距離ＬＳを算出する処理の説明は、後述する。
自己位置を推定した自己位置推定部６０は、推定した自己位置を含む情報信号（以降の説明では、「自己位置信号」と記載する場合がある）を、図２中に表すように、運転支援システム１００へ出力する。
運転支援システム１００は、公知のシステムであり、自車両ＭＣに搭載する。
また、運転支援システム１００は、自己位置推定装置１から入力を受けた自己位置信号が含む自己位置を用いて、自車両ＭＣの運転者に対する警報等の情報提供や、自車両ＭＣの制動等の運転支援を行う。また、運転支援システム１００の構成は公知技術であるため、説明は省略する。
（直進走行距離ＬＳを算出する処理）
以下、図１から図３を参照して、自己位置推定部６０が、直進走行距離ＬＳを算出する処理（直進走行距離算出処理）について説明する。
直進走行距離算出処理では、まず、自車両ＭＣが直進走行中であるか否かを判定する。そして、自車両ＭＣが直進走行中であると判定した時点から、走行距離のカウントを開始し、自車両ＭＣが直進走行中ではないと判定した時点で、走行距離のカウントを停止する。さらに、カウントの開始から停止までに自車両ＭＣが移動した距離を、直進走行距離ＬＳとして算出する。
自己位置推定部６０が、自車両ＭＣが直進走行中であるか否かを判定する処理（直進走行判定処理）としては、例えば、以下の直進走行判定処理Ｉ〜ＩＩＩのうち、少なくとも一つの処理を用いる。
（直進走行判定処理Ｉ）
操舵角センサ８が検出した現在操舵角の絶対値が、操舵角閾値未満である場合に、自車両ＭＣが直進走行中であると判定する。
（直進走行判定処理ＩＩ）
ジャイロセンサが検出したヨーレートの絶対値が、予め設定したヨーレート閾値未満であると、自車両ＭＣが直進走行中であると判定する。
（直進走行判定処理ＩＩＩ）
地図情報取得部４０が取得した地図データに、前回の処理で自己位置推定部６０が推定した自己位置を入力する。そして、前回の処理で自己位置推定部６０が推定した自己位置が、地図データの直線区間上である場合に、自車両ＭＣが直進走行中であると判定する。例えば、直線区間は、地図データに含まれる道路区間の曲率が１０００Ｒ以上など、所定の閾値を設けて、判定することが可能である。また、地図データのノード点を曲線近似することで、道路区間の曲率を推定しても良い。
なお、操舵角センサ８が検出した操舵角の絶対値や、ジャイロセンサが検出したヨーレートの絶対値は、例えば、操舵角センサ８やジャイロセンサから取得する。また、操舵角センサ８が検出した操舵角の絶対値や、ジャイロセンサが検出したヨーレートの絶対値は、例えば、移動量推定部２０から取得してもよい。
（自己位置の推定に用いる物標位置データを選択する処理）
以下、図１から図３を参照しつつ、図４及び図５を用いて、自己位置推定部６０が、物標位置蓄積部３０が蓄積している物標位置データから、自己位置の推定に用いる物標位置データを選択する処理（データ選択処理）について説明する。
データ選択処理では、まず、以下の条件ＢＩ〜ＢＩＩＩが成立しているか否かを判定する。
（条件ＢＩ）
カーブ開始位置推定部５０が推定したカーブ開始位置Ｃ０の直前の直進走行距離ＬＳが推定用閾値距離ＬＳ０以上である。または、カーブ開始位置推定部５０が推定したカーブ開始位置Ｃ０の直前の直進走行中に移動速度が移動速度閾値以下である。
なお、直進走行距離ＬＳは、上述した（直進走行距離ＬＳを算出する処理）にて算出される。
（条件ＢＩＩ）
自車両ＭＣと、カーブ開始位置推定部５０が推定したカーブ開始位置Ｃ０との距離が、予め設定した第一閾値距離Ｌ１（例えば、Ｌ１＝１００［ｍ］）以下である。
ここで、第一閾値距離Ｌ１は、例えば、移動量推定部２０が推定する移動量の蓄積される誤差が許容できる距離範囲から設定する。
（条件ＢＩＩＩ）
自車両ＭＣと、カーブ開始位置推定部５０が推定したカーブ開始位置Ｃ０との距離が、予め設定した第二閾値距離Ｌ２（例えば、Ｌ２＝２０［ｍ］）以上である。
そして、上記の各条件のうち、条件ＢＩ、条件ＢＩＩ及び条件ＢＩＩＩの全てが成立している場合、物標位置蓄積部３０が蓄積している物標位置データから、自車両ＭＣの周囲に存在する物標位置データを選択する。これに加え、物標位置蓄積部３０が蓄積している物標位置データから、自車両ＭＣの現在位置からカーブ開始位置Ｃ０までの間の物標位置データを選択する。
すなわち、物標位置蓄積部３０が蓄積している物標位置データから、図３中に表すように、自車両ＭＣの周囲及び自車両ＭＣの現在位置からカーブ開始位置Ｃ０までの間の領域Ｒ１内に存在する物標位置データのみを、選択済物標位置データとして選択する。
なお、図３中では、自車両ＭＣの進行方向でカーブ開始位置Ｃ０よりも前に、相対位置を検出した選択済物標位置データを、符号「Ｅｉ」で表す。同様に、図３中では、自車両ＭＣの進行方向で自車両ＭＣの現在位置からカーブ開始位置Ｃ０までの間に、相対位置を検出した選択済物標位置データを、符号「Ｓｉ」で表す。
また、上記の各条件のうち、条件ＢＩ及び条件ＢＩＩのみが成立している場合、物標位置蓄積部３０が蓄積している物標位置データから、自車両ＭＣとの相対距離が第二閾値距離Ｌ２以下の領域に存在する物標位置データを選択する。
なお、第二閾値距離Ｌ２は、物標位置蓄積部３０が蓄積した物標位置データのうち、自車両ＭＣの現在位置からカーブ開始位置推定部５０が推定したカーブ開始位置Ｃ０までの間に存在する物標位置データを含めた距離である。
すなわち、物標位置蓄積部３０が蓄積している物標位置データから、図４中に表すように、自車両ＭＣが走行済みの領域内で、自車両ＭＣから第二閾値距離Ｌ２以下の領域Ｒ２内に存在する物標位置データを、選択済物標位置データとして選択する。第二閾値距離Ｌ２は、地図情報取得部４０が取得した物標の位置情報と照合して、自己位置を推定するために必要な物標位置データを最低限に確保できる距離範囲として設定する。
なお、図４中では、自車両ＭＣが走行済みの領域内で、自車両ＭＣから第二閾値距離Ｌ２を超える領域Ｒ２内に存在する選択済物標位置データを、符号「Ｅｉ」で表す。同様に、図４中では、自車両ＭＣが走行済みの領域内で、自車両ＭＣから第二閾値距離Ｌ２以下の領域Ｒ２内に存在する選択済物標位置データを、符号「Ｓｉ」で表す。また、図４中では、領域Ｒ２を、自車両ＭＣの重心ＣＧから第二閾値距離Ｌ２以下の領域とする。
また、上記の各条件のうち、条件ＢＩのみが成立している場合、物標位置蓄積部３０が蓄積している物標位置データから、自車両ＭＣとの相対距離が、予め設定した第三閾値距離Ｌ３以下の領域に存在する物標位置データを選択する。また、上記の各条件が成立していない場合も同様に、物標位置蓄積部３０が蓄積している物標位置データから、自車両ＭＣとの相対距離が第三閾値距離Ｌ３以下の領域に存在する物標位置データを選択する。
ここで、第三閾値距離Ｌ３は、地図情報取得部４０が取得した物標の位置情報と照合して、自己位置を推定するために必要な物標位置データを十分に確保できる距離範囲として設定する。すなわち、第三閾値距離Ｌ３は、例えば、第一閾値距離Ｌ１未満の距離であるとともに、第二閾値距離Ｌ２よりも大きい距離に設定（Ｌ１＞Ｌ３＞Ｌ２）することが可能である。
なお、第三閾値距離Ｌ３は、物標位置蓄積部３０が蓄積した物標位置データのうち、自車両ＭＣの現在位置からカーブ開始位置推定部５０が推定したカーブ開始位置Ｃ０までの間に存在する物標位置データを含めた距離である。
すなわち、物標位置蓄積部３０が蓄積している物標位置データから、図５中に表すように、自車両ＭＣが走行済みの領域内で、自車両ＭＣから第三閾値距離Ｌ３以下の領域Ｒ３内に存在する物標位置データを、選択済物標位置データとして選択する。
なお、図５中では、自車両ＭＣが走行済みの領域内で、自車両ＭＣから第三閾値距離Ｌ３を超える領域Ｒ３内に存在する選択済物標位置データを、符号「Ｅｉ」で表す。同様に、図５中では、自車両ＭＣが走行済みの領域内で、自車両ＭＣから第三閾値距離Ｌ３以下の領域Ｒ３内に存在する選択済物標位置データを、符号「Ｓｉ」で表す。また、図５中では、領域Ｒ３を、自車両ＭＣの重心ＣＧから第三閾値距離Ｌ３以下の領域とする。
（選択済物標位置データを、地図情報取得部４０が取得した物標の位置情報と照合する処理）
以下、図１から図５を参照しつつ、図６を用いて、自己位置推定部６０が、選択済物標位置データを、地図情報取得部４０が取得した物標の位置情報と照合する処理（データ照合処理）について説明する。
データ照合処理では、データ選択処理で選択した選択済物標位置データを、地図情報取得部４０が取得した物標の位置情報と照合する。
具体的には、図６中に示すように、選択済物標位置データのうち、選択した一つの選択済物標位置データＳｉと、地図情報取得部４０が取得した物標の位置情報のうち、一つの選択済物標位置データＳｉに最も近い位置情報Ｍｊとの距離Ｄｉｊを算出する。したがって、選択済物標位置データＳｉと位置情報Ｍｊは、同じ領域（Ｒ１、Ｒ２、Ｒ３）内に存在する。
また、距離Ｄｉｊの算出は、選択済物標位置データの全てに対して行う。
さらに、距離Ｄｉｊの平均Ｓを、以下の式（１）を用いて算出する。
Figure 2017017705
なお、式（１）中の「Ｎ」は、データ選択処理で選択した選択済物標位置データの数である。
そして、数値解析により、平均Ｓが最小となる自車両ＭＣの位置及び姿勢を算出して、算出した自車両ＭＣの位置及び姿勢を、自己位置の推定値とする。
以上説明したように、自己位置推定部６０は、自車両ＭＣの周囲に存在する物標位置データ及び自車両ＭＣの現在位置からカーブ開始位置Ｃ０までの間に存在する選択済物標位置データと、地図データ情報に含まれる物標位置情報を照合して、自己位置を推定する。また、自己位置推定部６０は、自車両ＭＣに搭載する。
これに加え、自己位置推定部６０は、物標位置蓄積部３０に蓄積した物標位置データのうち、自車両ＭＣの走行経路を閾値距離（第二閾値距離Ｌ２、第三閾値距離Ｌ３）で遡った範囲（領域Ｒ２、Ｒ３）内に存在する物標位置データを選択する。さらに、自己位置推定部６０は、地図情報取得部４０が取得した物標位置情報のうち、自車両ＭＣの走行経路を閾値距離で遡った範囲内に存在する物標の位置と、選択済物標位置データと、を照合して、自己位置を推定する。
（動作）
図１から図６を参照しつつ、図７−１及び図７−２を用いて、第一実施形態の自己位置推定装置１を用いて行なう動作の一例を説明する。
図７−１及び図７−２中に表すように、自己位置推定装置１を用いて行なう動作を開始（ＳＴＡＲＴ）すると、まず、ステップＳ１００の処理を行う。
ステップＳ１００では、物標位置検出部１０により、自車両ＭＣの前方を含む自車両ＭＣの周囲に存在する物標を検出し、検出した物標と自車両ＭＣとの相対位置を検出（図中に表す「物標位置検出」）する。ステップＳ１００において、物標と自車両ＭＣとの相対位置を検出すると、自己位置推定装置１を用いて行なう動作は、ステップＳ１０２へ移行する。
ステップＳ１０２では、移動量推定部２０により、前回処理周期からの自車両ＭＣの移動量ΔＰを推定（図中に表す「移動量推定」）する。ステップＳ１０２において、自車両ＭＣの移動量ΔＰを推定すると、自己位置推定装置１を用いて行なう動作は、ステップＳ１０４へ移行する。
ステップＳ１０４では、物標位置蓄積部３０が、ステップＳ１００で検出した相対位置を、ステップＳ１０２で推定した自車両ＭＣの移動量を用いて補正する。さらに、ステップＳ１０４では、物標位置蓄積部３０が、物標位置データを蓄積（図中に表す「物標位置情報蓄積」）する。ステップＳ１０４において、物標位置データを蓄積すると、自己位置推定装置１を用いて行なう動作は、ステップＳ１０６へ移行する。
ステップＳ１０６では、カーブ開始位置推定部５０により、自車両ＭＣが走行している走行路が、カーブ区間であるか否かを判定する。これにより、ステップＳ１０６では、自車両ＭＣが、カーブ区間の走行を開始したか否かを判定する処理（図中に表す「カーブ開始」）を行う。
ステップＳ１０６において、自車両ＭＣがカーブ区間の走行を開始した（図中に表す「Ｙｅｓ」）と判定した場合、自己位置推定装置１を用いて行なう動作は、ステップＳ１０８へ移行する。
一方、ステップＳ１０６において、自車両ＭＣがカーブ区間の走行を開始していない（図中に表す「Ｎｏ」）と判定した場合、自己位置推定装置１を用いて行なう動作は、ステップＳ１２２へ移行する。
ステップＳ１０８では、カーブ開始位置推定部５０により、カーブ開始位置Ｃ０を推定（図中に表す「カーブ開始位置Ｃ０推定」）する。ステップＳ１０８において、カーブ開始位置Ｃ０を推定すると、自己位置推定装置１を用いて行なう動作は、ステップＳ１１０へ移行する。
ステップＳ１１０では、自己位置推定部６０により、カーブ開始位置Ｃ０の直前の直進走行距離ＬＳが推定用閾値距離ＬＳ０以上であるか否かを判定する処理（図中に表す「カーブ開始位置の直前の直進走行距離が推定用閾値距離以上」）を行う。
ステップＳ１１０において、カーブ開始位置Ｃ０の直前の直進走行距離ＬＳが推定用閾値距離ＬＳ０以上である（図中に表す「Ｙｅｓ」）と判定した場合、自己位置推定装置１を用いて行なう動作は、ステップＳ１１４へ移行する。
一方、ステップＳ１１０において、カーブ開始位置Ｃ０の直前の直進走行距離ＬＳが推定用閾値距離ＬＳ０未満である（図中に表す「Ｎｏ」）と判定した場合、自己位置推定装置１を用いて行なう動作は、ステップＳ１１２へ移行する。
ステップＳ１１２では、自己位置推定部６０により、カーブ開始位置Ｃ０の直前の直進走行区間での移動速度ＶＰが、予め設定した閾値速度ＶＰ０（例えば、ＶＰ０＝１．４［ｍ／ｓｅｃ］）以下であるか否かを判定する処理を行う。すなわち、ステップＳ１１２では、図中に表す「カーブ開始位置の直前の直進走行区間での移動速度が閾値速度以下」であるか否かを判定する処理を行う。なお、移動速度は、車速であってもよい。この場合、閾値速度ＶＰ０は、例えば、５［ｋｍ／ｈ］と設定することが可能である。
ステップＳ１１２において、カーブ開始位置Ｃ０の直前の直進走行区間での移動速度ＶＰが閾値速度ＶＰ０以下である（図中に表す「Ｙｅｓ」）と判定した場合、自己位置推定装置１を用いて行なう動作は、ステップＳ１１４へ移行する。
一方、ステップＳ１１２において、カーブ開始位置Ｃ０の直前の直進走行区間での移動速度ＶＰが閾値速度ＶＰ０を超えている（図中に表す「Ｎｏ」）と判定した場合、自己位置推定装置１を用いて行なう動作は、ステップＳ１２２へ移行する。
ステップＳ１１４では、自己位置推定部６０により、上記の条件ＢＩＩが成立しているか否かを判定する処理を行う。すなわち、ステップＳ１１４では、自車両ＭＣとカーブ開始位置Ｃ０との距離Ｌ（図３参照）が、第一閾値距離Ｌ１以下であるか否かを判定する処理（図中に表す「カーブ開始位置から第一閾値距離以下」）を行う。なお、自車両ＭＣとカーブ開始位置Ｃ０との距離Ｌは、具体的には、自車両ＭＣの重心ＣＧとカーブ開始位置Ｃ０との距離Ｌである。
ステップＳ１１４において、自車両ＭＣとカーブ開始位置Ｃ０との距離Ｌが第一閾値距離Ｌ１以下である（図中に表す「Ｙｅｓ」）と判定した場合、自己位置推定装置１を用いて行なう動作は、ステップＳ１１６へ移行する。
一方、ステップＳ１１４において、自車両ＭＣとカーブ開始位置Ｃ０との距離Ｌが第一閾値距離Ｌ１を超えている（図中に表す「Ｎｏ」）と判定した場合、自己位置推定装置１を用いて行なう動作は、ステップＳ１２２へ移行する。
ステップＳ１１６では、自己位置推定部６０により、上記の条件ＢＩＩＩが成立しているか否かを判定する処理を行う。すなわち、ステップＳ１１６では、自車両ＭＣとカーブ開始位置Ｃ０との距離Ｌ（図３参照）が、第二閾値距離Ｌ２以上であるか否かを判定する処理（図中に表す「カーブ開始位置から第二閾値距離以上」）を行う。
ステップＳ１１６において、自車両ＭＣとカーブ開始位置Ｃ０との距離Ｌが第二閾値距離Ｌ２以上である（図中に表す「Ｙｅｓ」）と判定した場合、自己位置推定装置１を用いて行なう動作は、ステップＳ１１８へ移行する。
一方、ステップＳ１１６において、自車両ＭＣとカーブ開始位置Ｃ０との距離Ｌが第二閾値距離Ｌ２未満である（図中に表す「Ｎｏ」）と判定した場合、自己位置推定装置１を用いて行なう動作は、ステップＳ１２０へ移行する。
ステップＳ１１８では、自己位置推定部６０により、カーブ開始位置Ｃ０と自車両ＭＣとの間の領域Ｒ１内に存在する物標位置データを、選択済物標位置データＳｉとして選択する（図３参照）。すなわち、ステップＳ１１８では、選択済物標位置データＳｉとして、自車両ＭＣの周囲に存在する物標位置データ及び自車両ＭＣの現在位置からカーブ開始位置Ｃ０までの間の領域Ｒ１内に存在する物標位置データを選択する。したがって、ステップＳ１１８では、図中に表す「自車両からカーブ開始位置までの間の物標情報を選択」する処理を行う。ステップＳ１１８において、領域Ｒ１内に存在する物標位置データを選択済物標位置データＳｉとして選択すると、自己位置推定装置１を用いて行なう動作は、ステップＳ１２４へ移行する。
ステップＳ１２０では、自己位置推定部６０により、自車両ＭＣとの相対距離が第二閾値距離Ｌ２以下の領域Ｒ２内に存在する物標位置データを、選択済物標位置データＳｉとして選択する（図４参照）。すなわち、ステップＳ１２０では、選択済物標位置データＳｉとして、自車両ＭＣとの相対距離が第二閾値距離Ｌ２以下の領域Ｒ２内に存在する物標位置データを選択（図中に表す「自車両との距離がＬ２以下の物標情報を選択」）する。ステップＳ１２０において、領域Ｒ２内に存在する物標位置データを選択済物標位置データＳｉとして選択すると、自己位置推定装置１を用いて行なう動作は、ステップＳ１２４へ移行する。
ステップＳ１２２では、自己位置推定部６０により、自車両ＭＣとの相対距離が第三閾値距離Ｌ３以下の領域Ｒ３内に存在する物標位置データを、選択済物標位置データＳｉとして選択する（図５参照）。すなわち、ステップＳ１２２では、選択済物標位置データＳｉとして、自車両ＭＣとの相対距離が第三閾値距離Ｌ３以下の領域Ｒ３内に存在する物標位置データを選択（図中に表す「自車両との距離がＬ３以下の物標情報を選択」）する。ステップＳ１２２において、領域Ｒ３内に存在する物標位置データを選択済物標位置データＳｉとして選択すると、自己位置推定装置１を用いて行なう動作は、ステップＳ１２４へ移行する。
ステップＳ１２４では、自己位置推定部６０により、ステップＳ１１８、Ｓ１２０、Ｓ１２２のうちいずれか一つのステップで選択した選択済物標位置データＳｉを、地図情報取得部４０が取得した物標の位置情報と照合する。これにより、ステップＳ１２４では、ステップＳ１１８、Ｓ１２０、Ｓ１２２のうちいずれか一つのステップで選択した選択済物標位置データＳｉを用いて、自己位置を推定（図中に表す「自己位置推定」）する。ステップＳ１２４において、自己位置を推定すると、自己位置推定装置１を用いて行なう動作は、ステップＳ１２６へ移行する。
ステップＳ１２６では、自車両ＭＣのイグニッションスイッチ（図示せず）が停止状態であるか否かを判定する処理（図中に表す「ＩＧＮ ＯＦＦ」）を行う。
ステップＳ１２６において、自車両ＭＣのイグニッションスイッチが停止状態である（図中に表す「Ｙｅｓ」）と判定した場合、自己位置推定装置１を用いて行なう動作を終了（ＥＮＤ）する。
一方、ステップＳ１２６において、自車両ＭＣのイグニッションスイッチが停止状態ではない（図中に表す「Ｎｏ」）と判定した場合、自己位置推定装置１を用いて行なう動作は、ステップＳ１００へ復帰する。
上述したように、第一実施形態の自己位置推定装置１の動作で実施する自己位置推定方法では、自車両ＭＣの周囲に存在する物標と自車両ＭＣとの相対位置を検出し、自車両ＭＣの移動量と、自車両ＭＣが走行する走行路のカーブ開始位置Ｃ０を推定する。そして、物標位置データのうち自車両ＭＣの周囲に存在する物標位置データ及び自車両ＭＣの現在位置から推定したカーブ開始位置Ｃ０までの間に存在する物標位置データと、地図データに含まれる物標位置情報とを照合して、自己位置を推定する。
なお、上述した第一実施形態は、本発明の一例であり、本発明は、上述した第一実施形態に限定されることはなく、この実施形態以外の形態であっても、本発明に係る技術的思想を逸脱しない範囲であれば、設計等に応じて種々の変更が可能である。
（第一実施形態の効果）
第一実施形態の自己位置推定装置１であれば、以下に記載する効果を奏することが可能となる。
（１）カーブ開始位置推定部５０が、自車両ＭＣが走行する走行路のカーブ開始位置Ｃ０を推定する。これに加え、自己位置推定部６０が、自車両ＭＣの周囲に存在する物標位置データ及び自車両ＭＣの現在位置からカーブ開始位置Ｃ０までの間に存在する物標位置データと、地図情報に含まれる物標位置情報とを照合して、自己位置を推定する。
このため、自車両ＭＣが直線路からカーブ路へ進入しても、カーブの形状に沿った物標位置データを用いて、自己位置を推定することとなる。ここで、カーブの形状に沿った物標位置データは、自車両ＭＣの周囲に存在する物標位置データ及び自車両ＭＣの現在位置からカーブ路開始位置Ｃ０までの間に存在するデータである。
その結果、カーブ路における自己位置の推定を、カーブの形状に沿った物標位置データで行うため、カーブ路における自己位置の推定精度低下を抑制することが可能となる。
（２）自己位置推定部６０が、自車両ＭＣが直進走行した距離である直進走行距離ＬＳが、予め設定した推定用閾値距離ＬＳ０以上の場合に、自己位置を推定する。
このため、直線走行距離ＬＳが推定用閾値距離ＬＳ０以上の場合に、自車両ＭＣの周囲に存在する物標位置データ及び自車両ＭＣの現在位置からカーブ開始位置Ｃ０までの間に存在する物標の位置を用いて、自己位置を推定する。
その結果、カーブ路の直前の直線路の走行距離が長かった場合、すなわち、オドメトリの誤差が蓄積しやすい走行があった場合でも、カーブの形状に沿った物標位置データで自己位置を推定することが可能となる。これにより、カーブ路における自己位置の推定精度低下を抑制することが可能となる。
（３）自己位置推定部６０が、自車両ＭＣが閾値速度以下の移動速度（車速）で走行した場合に、自己位置を推定する。
このため、自車両ＭＣが閾値速度以下の移動速度（車速）で走行した場合に、自車両ＭＣの周囲に存在する物標位置データ及び自車両ＭＣの現在位置からカーブ開始位置Ｃ０までの間に存在する物標の位置を用いて自己位置を推定することが可能となる。
その結果、停止や発進等、極低速の走行後、すなわち、オドメトリの誤差が蓄積しやすい走行後であっても、カーブ路における自己位置の推定精度低下を抑制することが可能となる。
（４）自己位置推定部６０が、自車両ＭＣの走行経路を閾値距離で遡った範囲内に存在する物標位置データと、地図情報に含まれている物標位置情報を照合して、自己位置を推定する。
このため、自車両ＭＣの走行経路を閾値距離で遡った範囲内に存在する物標位置データと、地図情報に含まれている物標位置情報を用いて、自己位置を推定する。ここで、自車両ＭＣの走行経路を閾値距離で遡った範囲内に存在する物標位置データには、自車両ＭＣの周囲に存在する物標位置データ及び自車両ＭＣの現在位置からカーブ開始位置Ｃ０までに間に存在する物標を含む。
その結果、直線路からカーブ路への進入時や、直線路からカーブ路への進入直後であっても、必要最低限の閾値距離の範囲内に存在する物標位置データを用いて自己位置を推定することが可能となる。ここで、必要最低限の閾値距離の範囲内に存在する物標位置データは、自車両ＭＣの周囲に存在する物標位置データ及び自車両ＭＣの現在位置からカーブ開始位置Ｃ０までの間に存在する物標を含む。これにより、カーブの形状に沿った物標位置データを含めて自己位置を推定することが可能となるため、カーブ路における自己位置の推定精度低下を抑制することが可能となる。
（５）自己位置推定部６０が、自車両ＭＣの周囲に存在する物標位置データ及び自車両ＭＣの現在位置からカーブ開始位置推定部５０が推定したカーブ開始位置Ｃ０まので間のみに存在する物標位置データと、地図情報に含まれている物標位置情報とを照合する。そして、自己位置を推定する。この推定は、上記の各条件のうち、条件ＢＩ、条件ＢＩＩ及び条件ＢＩＩＩが全て成立している場合に行う。
このため、自車両ＭＣが直線路からカーブ路へ進入しても、直線路で検出したデータを用いずに、カーブの形状に沿った物標位置データを用いて、自己位置を推定することとなる。ここで、カーブの形状に沿った物標位置データは、自車両ＭＣの周囲に存在する物標位置データおよび自車両ＭＣの現在位置からカーブ路開始位置Ｃ０までの間に存在する物標位置データである。
その結果、カーブ路における自己位置の推定を、カーブの形状に沿った物標位置データのみで行うため、カーブ路における自己位置の推定精度低下を抑制することが可能となる。
（６）第一実施形態の自己位置推定装置１の動作で実施する自己位置推定方法では、自車両ＭＣの周囲に存在する物標と自車両ＭＣとの相対位置を検出し、自車両ＭＣの移動量と、自車両ＭＣが走行する走行路のカーブ開始位置Ｃ０を推定する。そして、物標位置データのうち、自車両ＭＣの周囲に存在する物標位置データ及び自車両ＭＣの現在位置から推定したカーブ開始位置Ｃ０までの間に存在する物標位置データと、地図データに含まれている物標位置情報とを照合して、自己位置を推定する。
このため、自車両ＭＣが直線路からカーブ路へ進入しても、カーブの形状に沿った物標位置データを用いて、自己位置を推定することとなる。ここで、カーブの形状に沿った物標位置データは、自車両ＭＣの周囲に存在する物標位置データおよび自車両ＭＣの現在位置からカーブ路開始位置Ｃ０までの間に存在する物標位置データである。
その結果、カーブ路における自己位置の推定を、カーブの形状に沿った物標位置データで行うため、カーブ路における自己位置の推定精度低下を抑制することが可能となる。
（第一実施形態の変形例）
（１）第一実施形態では、移動量推定部２０、物標位置蓄積部３０、地図情報取得部４０、カーブ開始位置推定部５０及び自己位置推定部６０を自車両ＭＣに搭載したが、この構成に限定するものではない。
すなわち、移動量推定部２０、物標位置蓄積部３０、地図情報取得部４０、カーブ開始位置推定部５０及び自己位置推定部６０のうち少なくとも一つを、例えば、データセンター（基地局）が備える構成としてもよい。
（第二実施形態）
以下、本発明の第二実施形態について、図面を参照しつつ説明する。
（自己位置推定装置の構成）
第二実施形態の自己位置推定装置１の構成は、自己位置推定部６０が行う処理を除き、第一実施形態と同様の構成である。このため、第二実施形態の自己位置推定装置１の構成については、自己位置推定部６０の構成のみを説明する。
自己位置推定部６０は、物標位置蓄積部３０が蓄積している物標位置データから、選択済物標位置データを選択する。なお、自己位置の推定に用いる物標位置データを選択する処理は、上述した第一実施形態と同様である。
さらに、自己位置推定部６０は、選択済物標位置データを、地図情報取得部４０が取得した物標の位置情報と照合することにより、自己位置を推定する。なお、選択済物標位置データを、地図情報取得部４０が取得した物標の位置情報と照合する処理は、上述した第一実施形態と同様である。
第二実施形態では、自己位置推定部６０の構成を、選択済物標位置データと物標の位置情報との照合を複数回行う構成とする。さらに、自己位置推定部６０の構成を、複数回の照合の結果から、各照合における複数の最小誤差を算出し、複数の最小誤差の和が予め設定した誤差閾値（例えば、1[ｍ]）以上の場合に、自己位置を推定する構成とした場合について説明する。
自己位置を推定した自己位置推定部６０は、自己位置信号を、運転支援システム１００へ出力する（図２参照）。
その他の構成は、自車両ＭＣの構成も含め、上述した第一実施形態と同様である。
（動作）
図１から図６を参照しつつ、図８−１及び図８−２を用いて、第二実施形態の自己位置推定装置１を用いて行なう動作の一例を説明する。
図８−１及び図８−２中に表すように、自己位置推定装置１を用いて行なう動作を開始（ＳＴＡＲＴ）すると、まず、ステップＳ１００の処理を行う。
ステップＳ１００からステップＳ１０４の処理は、上述した第一実施形態と同様であるため、その説明を省略する。
ステップＳ１０６では、カーブ開始位置推定部５０により、自車両ＭＣが走行している走行路が、カーブ区間であるか否かを判定する。これにより、ステップＳ１０６では、自車両ＭＣが、カーブ区間の走行を開始したか否かを判定する処理（図中に表す「カーブ開始」）を行う。
ステップＳ１０６において、自車両ＭＣがカーブ区間の走行を開始した（図中に表す「Ｙｅｓ」）と判定した場合、自己位置推定装置１を用いて行なう動作は、ステップＳ１０８へ移行する。
一方、ステップＳ１０６において、自車両ＭＣがカーブ区間の走行を開始していない（図中に表す「Ｎｏ」）と判定した場合、自己位置推定装置１を用いて行なう動作は、ステップＳ１２２へ移行する。
ステップＳ１０８では、カーブ開始位置推定部５０により、カーブ開始位置Ｃ０を推定（図中に表す「カーブ開始位置Ｃ０推定」）する。ステップＳ１０８において、カーブ開始位置Ｃ０を推定すると、自己位置推定装置１を用いて行なう動作は、ステップＳ２００へ移行する。
ステップＳ２００では、自己位置推定部６０により、過去に行った処理の結果を用いて、過去に行った複数回の、選択済物標位置データと物標の位置情報との照合の結果を取得する。これに加え、ステップＳ２００では、複数回の照合の結果から、各照合における複数の最小誤差の平均値を算出（図中に表す「誤差平均算出」）する。ステップＳ２００において、複数回の照合における複数の最小誤差の平均値を算出すると、自己位置推定装置１を用いて行なう動作は、ステップＳ２０２へ移行する。
ここで、ステップＳ２００では、上述した第一実施形態と同様の方法により、式（１）を用いて、距離Ｄｉｊの平均Ｓを算出する（図６参照）。但し、第二実施形態では、平均Ｓの算出に用いる選択済物標位置データＳｉを、領域Ｒ３内に存在する選択済物標位置データＳｉに限定する（図５参照）。
ステップＳ２０２では、自己位置推定部６０により、ステップＳ２００で算出した最小誤差の平均値が誤差閾値以下であるか否かを判定する処理（図中に表す「誤差平均値が誤差閾値以下」）を行う。
ステップＳ２０２において、ステップＳ２００で算出した最小誤差の平均値が誤差閾値以下である（図中に表す「Ｙｅｓ」）と判定した場合、自己位置推定装置１を用いて行なう動作は、ステップＳ１１４へ移行する。
一方、ステップＳ２０２において、ステップＳ２００で算出した最小誤差の平均値が誤差閾値を超えている（図中に表す「Ｎｏ」）と判定した場合、自己位置推定装置１を用いて行なう動作は、ステップＳ１２２へ移行する。
ステップＳ１１４からステップＳ１２６の処理は、上述した第一実施形態と同様であるため、その説明を省略する。
なお、上述した第二実施形態は、本発明の一例であり、本発明は、上述した第二実施形態に限定されることはなく、この実施形態以外の形態であっても、本発明に係る技術的思想を逸脱しない範囲であれば、設計等に応じて種々の変更が可能である。
（第二実施形態の効果）
第二実施形態の自己位置推定装置１であれば、以下に記載する効果を奏することが可能となる。
（１）自己位置推定部６０が、選択済物標位置データと物標の位置情報との複数回の照合の結果から、各照合における複数の最小誤差を算出し、複数の最小誤差の和が予め設定した誤差閾値以上の場合に、自己位置を推定する。
このため、複数の最小誤差の和が予め設定した誤差閾値以上の場合に、自車両ＭＣの周囲に存在する物標位置データ及び自車両ＭＣの現在位置からカーブ開始位置Ｃ０までの間に存在する物標の位置を用いて、自己位置を推定する。
その結果、自車両ＭＣに対し、前後位置の推定精度が低下しやすい状況であっても、カーブの形状に沿った物標位置データで自己位置を推定するため、カーブ路における自己位置の推定精度低下を抑制することが可能となる。
１…自己位置推定装置、２…撮像装置、４…距離測定装置、６…車輪速センサ、８…操舵角センサ、１０…物標位置検出部、２０…移動量推定部、３０…物標位置蓄積部、４０…地図情報取得部、５０…カーブ開始位置推定部、６０…自己位置推定部、１００…、ＭＣ…自車両、ＲＯ…走行路面、Ｗ…車輪、Ｃ０…カーブ開始位置、ＣＧ…自車両ＭＣの重心、Ｒ１…カーブ開始位置Ｃ０と自車両ＭＣとの間の領域、Ｒ２…自車両ＭＣから第二閾値距離Ｌ２以下の領域、Ｒ３…自車両ＭＣから第三閾値距離Ｌ３以下の領域、Ｅｉ…自車両ＭＣの進行方向でカーブ開始位置Ｃ０よりも前に、相対位置を検出した選択済物標位置データ、Ｓｉ…自車両ＭＣの進行方向で自車両ＭＣの現在位置からカーブ開始位置Ｃ０までの間に、相対位置を検出した選択済物標位置データ、Ｌ…自車両ＭＣとカーブ開始位置Ｃ０との距離、Ｌ２…第二閾値距離、Ｌ３…第三閾値距離、Ｍｊ…地図情報取得部４０が格納している物標の位置情報のうち選択した一つの選択済物標位置データＳｉに最も近い位置情報、Ｄｉｊ…選択した一つの選択済物標位置データＳｉと位置情報Ｍｊとの距離

Claims (7)

  1. 自車両に搭載され、前記自車両の周囲に存在する物標と自車両との相対位置を検出する物標位置検出部と、
    前記自車両の移動量を推定する移動量推定部と、
    前記検出した相対位置を前記推定した移動量だけ移動させて物標位置データとして蓄積する物標位置蓄積部と、
    地図上に存在する物標の物標位置情報を含む地図情報を取得する地図情報取得部と、
    前記自車両が走行する走行路のカーブ開始位置を推定するカーブ開始位置推定部と、
    前記蓄積した物標位置データのうち少なくとも前記自車両の周囲に存在する物標位置データ、及び前記自車両の現在位置から前記推定したカーブ開始位置までの間に存在する物標位置データと、前記地図情報に含まれる物標位置情報と、を照合して前記自車両の現在位置である自己位置を推定する自己位置推定部と、を備えることを特徴とする自己位置推定装置。
  2. 前記自己位置推定部は、前記自車両の直進走行距離が予め設定した推定用閾値距離以上の場合に、前記自己位置を推定することを特徴とする請求項１に記載した自己位置推定装置。
  3. 前記自己位置推定部は、前記自車両が予め設定した閾値速度以下の車速で走行した場合に、前記自己位置を推定することを特徴とする請求項１または請求項２に記載した自己位置推定装置。
  4. 前記自己位置推定部は、前記蓄積した物標位置データのうち前記自車両の走行経路を予め設定した閾値距離で遡った範囲内に存在する物標位置データと、前記地図情報に含まれる物標位置情報と、を照合して、前記自己位置を推定し、
    前記閾値距離は、前記蓄積した物標位置データのうち前記自車両の現在位置から前記推定したカーブ開始位置までの間に存在する物標位置データを含めた距離であることを特徴とする請求項１から請求項３のうちいずれか１項に記載した自己位置推定装置。
  5. 前記自己位置推定部は、前記照合を複数回行い、さらに、前記複数回の照合の結果から各照合における複数の最小誤差を算出し、前記複数の最小誤差の和が予め設定した誤差閾値以上の場合に、前記自己位置を推定することを特徴とする請求項１から請求項４のうちいずれか１項に記載した自己位置推定装置。
  6. 前記自己位置推定部は、前記自車両の周囲に存在する物標位置データ、及び前記自車両の現在位置から前記推定したカーブ開始位置までの間のみに存在する物標位置データと、前記地図情報に含まれる物標位置情報と、を照合して前記自己位置を推定することを特徴とする請求項１から請求項５のうちいずれか１項に記載した自己位置推定装置。
  7. 自車両の周囲に存在する物標と前記自車両との相対位置を検出し、
    前記自車両の移動量と、前記自車両が走行する走行路のカーブ開始位置と、を推定し、
    前記検出した相対位置を前記推定した移動量だけ移動させて物標位置データとして蓄積し、
    地図上に存在する物標の物標位置情報を含む地図情報を取得し、
    前記蓄積した物標位置データのうち少なくとも前記自車両の周囲に存在する物標位置データ、及び前記自車両の現在位置から前記推定したカーブ開始位置までの間に存在する物標位置データと、前記地図情報に含まれる物標位置情報と、を照合して前記自車両の現在位置である自己位置を推定することを特徴とする自己位置推定方法。
JP2017530455A 2015-07-24 2015-07-24 自己位置推定装置及び自己位置推定方法 Active JP6365780B2 (ja)

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
PCT/JP2015/003728 WO2017017705A1 (ja) 2015-07-24 2015-07-24 自己位置推定装置及び自己位置推定方法

Publications (2)

Publication Number Publication Date
JPWO2017017705A1 true JPWO2017017705A1 (ja) 2018-02-22
JP6365780B2 JP6365780B2 (ja) 2018-08-01

Family

ID=57885477

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2017530455A Active JP6365780B2 (ja) 2015-07-24 2015-07-24 自己位置推定装置及び自己位置推定方法

Country Status (10)

Country Link
US (1) US10289120B2 (ja)
EP (1) EP3327532B1 (ja)
JP (1) JP6365780B2 (ja)
KR (1) KR101966603B1 (ja)
CN (1) CN107924192B (ja)
BR (1) BR112018001499B1 (ja)
CA (1) CA2993420C (ja)
MX (1) MX365228B (ja)
RU (1) RU2679277C1 (ja)
WO (1) WO2017017705A1 (ja)

Families Citing this family (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US10508923B2 (en) 2015-08-28 2019-12-17 Nissan Motor Co., Ltd. Vehicle position estimation device, vehicle position estimation method
US10528056B2 (en) 2016-05-06 2020-01-07 GM Global Technology Operations LLC Vehicle guidance system
US10528826B2 (en) * 2016-05-06 2020-01-07 GM Global Technology Operations LLC Vehicle guidance system
CA3031723C (en) * 2016-07-26 2020-06-23 Nissan Motor Co., Ltd. Self-position estimation method and self-position estimation device
JP6771435B2 (ja) * 2017-07-20 2020-10-21 株式会社ソニー・インタラクティブエンタテインメント 情報処理装置および位置情報取得方法
WO2019061311A1 (zh) * 2017-09-29 2019-04-04 深圳市大疆创新科技有限公司 自动驾驶车辆的控制方法、控制终端及机器可读存储介质
US11698642B2 (en) * 2017-12-12 2023-07-11 Sony Corporation Information processing apparatus, mobile object, control system, and information processing method
US11520335B2 (en) * 2018-04-12 2022-12-06 Baidu Usa Llc Determining driving paths for autonomous driving vehicles based on map data
GB2591515B (en) * 2020-01-31 2023-07-12 Mclaren Automotive Ltd Track assistant
JP2021148572A (ja) * 2020-03-18 2021-09-27 本田技研工業株式会社 車載装置、車両、および制御方法
CN112987717A (zh) * 2020-12-30 2021-06-18 东风汽车集团有限公司 一种车辆坡道和弯道识别的方法及系统
WO2023096961A2 (en) * 2021-11-24 2023-06-01 ClearMotion, Inc. Methods and systems for terrain-based localization of a vehicle

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH0322108A (ja) * 1989-06-20 1991-01-30 Shinko Electric Co Ltd 移動ロボット
JPH11160078A (ja) * 1997-12-02 1999-06-18 Toyota Motor Corp 走行路形状推定装置
JPH11230765A (ja) * 1998-02-10 1999-08-27 Nissan Motor Co Ltd カーブ検出装置
JP2000230836A (ja) * 1999-02-10 2000-08-22 Sumitomo Electric Ind Ltd 移動体の位置検出装置および位置検出プログラムを記録した記録媒体
JP2014211862A (ja) * 2013-04-02 2014-11-13 パナソニック株式会社 自律移動装置及び自律移動装置の自己位置推定方法

Family Cites Families (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH08247775A (ja) * 1995-03-15 1996-09-27 Toshiba Corp 移動体の自己位置同定装置および自己位置同定方法
JP5023407B2 (ja) * 2000-11-22 2012-09-12 マツダ株式会社 車両の制御装置
US7287884B2 (en) * 2002-02-07 2007-10-30 Toyota Jidosha Kabushiki Kaisha Vehicle operation supporting device and vehicle operation supporting system
US9250081B2 (en) * 2005-03-25 2016-02-02 Irobot Corporation Management of resources for SLAM in large environments
JP2008250906A (ja) * 2007-03-30 2008-10-16 Sogo Keibi Hosho Co Ltd 移動ロボット、自己位置補正方法および自己位置補正プログラム
JP4788722B2 (ja) * 2008-02-26 2011-10-05 トヨタ自動車株式会社 自律移動ロボット、自己位置推定方法、環境地図の生成方法、環境地図の生成装置、及び環境地図のデータ構造
US20110190972A1 (en) * 2010-02-02 2011-08-04 Gm Global Technology Operations, Inc. Grid unlock
JP5057184B2 (ja) * 2010-03-31 2012-10-24 アイシン・エィ・ダブリュ株式会社 画像処理システム及び車両制御システム
JP5251927B2 (ja) * 2010-06-21 2013-07-31 日産自動車株式会社 移動距離検出装置及び移動距離検出方法
US8452535B2 (en) * 2010-12-13 2013-05-28 GM Global Technology Operations LLC Systems and methods for precise sub-lane vehicle positioning
TWI456165B (zh) * 2011-02-14 2014-10-11 Mitac Int Corp 車輛導航方法與車輛導航系統
JP5761162B2 (ja) * 2012-11-30 2015-08-12 トヨタ自動車株式会社 車両位置推定装置
US9435653B2 (en) * 2013-09-17 2016-09-06 GM Global Technology Operations LLC Sensor-aided vehicle positioning system
CA2976344A1 (en) * 2015-02-10 2016-08-18 Mobileye Vision Technologies Ltd. Sparse map for autonomous vehicle navigation

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH0322108A (ja) * 1989-06-20 1991-01-30 Shinko Electric Co Ltd 移動ロボット
JPH11160078A (ja) * 1997-12-02 1999-06-18 Toyota Motor Corp 走行路形状推定装置
JPH11230765A (ja) * 1998-02-10 1999-08-27 Nissan Motor Co Ltd カーブ検出装置
JP2000230836A (ja) * 1999-02-10 2000-08-22 Sumitomo Electric Ind Ltd 移動体の位置検出装置および位置検出プログラムを記録した記録媒体
JP2014211862A (ja) * 2013-04-02 2014-11-13 パナソニック株式会社 自律移動装置及び自律移動装置の自己位置推定方法

Also Published As

Publication number Publication date
KR20180031030A (ko) 2018-03-27
EP3327532B1 (en) 2021-01-27
BR112018001499B1 (pt) 2022-09-13
CN107924192A (zh) 2018-04-17
JP6365780B2 (ja) 2018-08-01
BR112018001499A2 (pt) 2018-09-11
EP3327532A4 (en) 2018-08-15
MX365228B (es) 2019-05-27
US20180373265A1 (en) 2018-12-27
CA2993420A1 (en) 2017-02-02
WO2017017705A1 (ja) 2017-02-02
CA2993420C (en) 2018-11-20
MX2018000969A (es) 2018-05-17
US10289120B2 (en) 2019-05-14
CN107924192B (zh) 2019-03-19
EP3327532A1 (en) 2018-05-30
RU2679277C1 (ru) 2019-02-06
KR101966603B1 (ko) 2019-04-05

Similar Documents

Publication Publication Date Title
JP6365780B2 (ja) 自己位置推定装置及び自己位置推定方法
US10310508B2 (en) Vehicle control apparatus
KR101901024B1 (ko) 지도 갱신 판정 시스템
US11243080B2 (en) Self-position estimation method and self-position estimation device
JP6575685B2 (ja) 自己位置推定方法及び自己位置推定装置
JP2019532292A (ja) 車両位置特定の自律走行車両
JP7232100B2 (ja) 車両の走行制御装置
US11608059B2 (en) Method and apparatus for method for real time lateral control and steering actuation assessment
KR20190051786A (ko) 자차 위치 추정 장치
JP2021026387A (ja) 車両の走行制御装置
JP2019095210A (ja) 車両制御装置、車両制御方法、およびプログラム
US20180208197A1 (en) Lane keeping assistance system
JP2007505377A (ja) 路上における自動車の位置決定方法及び装置
KR20170078096A (ko) 자율주행 차량의 제어방법
JP6747157B2 (ja) 自己位置推定方法及び自己位置推定装置
KR101980509B1 (ko) 차간 거리 추정 방법 및 차간 거리 추정 장치
JP2008287480A (ja) 車両の走行支援装置
KR101544797B1 (ko) 차량 간 상대위치 추정 장치 및 방법
CN112352268A (zh) 用于车辆的驾驶辅助系统、带驾驶辅助系统的车辆和用于车辆的驾驶辅助方法
JP6455292B2 (ja) 物体検出装置及び物体検出方法

Legal Events

Date Code Title Description
A621 Written request for application examination

Free format text: JAPANESE INTERMEDIATE CODE: A621

Effective date: 20171016

A521 Request for written amendment filed

Free format text: JAPANESE INTERMEDIATE CODE: A523

Effective date: 20180205

TRDD Decision of grant or rejection written
A01 Written decision to grant a patent or to grant a registration (utility model)

Free format text: JAPANESE INTERMEDIATE CODE: A01

Effective date: 20180605

A61 First payment of annual fees (during grant procedure)

Free format text: JAPANESE INTERMEDIATE CODE: A61

Effective date: 20180618

R151 Written notification of patent or utility model registration

Ref document number: 6365780

Country of ref document: JP

Free format text: JAPANESE INTERMEDIATE CODE: R151