特許 7037317 自車位置検出装置

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

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

(11)【特許番号】

(24)【登録日】2022-03-08

(45)【発行日】2022-03-16

(54)【発明の名称】自車位置検出装置

(51)【国際特許分類】

   G01C 21/28 20060101AFI20220309BHJP

   G09B 29/10 20060101ALI20220309BHJP

   G09B 29/00 20060101ALI20220309BHJP

   B60W 40/072 20120101ALI20220309BHJP

   B60W 30/095 20120101ALI20220309BHJP

   G06T 7/60 20170101ALI20220309BHJP

   G08G 1/133 20060101ALI20220309BHJP

【ＦＩ】

G01C21/28

G09B29/10 A

G09B29/00 A

B60W40/072

B60W30/095

G06T7/60 200J

G08G1/133

【請求項の数】 3

(21)【出願番号】P 2017188484

(22)【出願日】2017-09-28

(65)【公開番号】P2019066193

(43)【公開日】2019-04-25

【審査請求日】2020-08-27

(73)【特許権者】

【識別番号】000005348

【氏名又は名称】株式会社ＳＵＢＡＲＵ

(74)【代理人】

【識別番号】110002907

【氏名又は名称】特許業務法人イトーシン国際特許事務所

(74)【代理人】

【識別番号】100076233

【弁理士】

【氏名又は名称】伊藤 進

(74)【代理人】

【識別番号】100101661

【氏名又は名称】長谷川 靖

(74)【代理人】

【識別番号】100135932

【弁理士】

【氏名又は名称】篠浦 治

(72)【発明者】

【氏名】財前 元希

【審査官】久保田 創

(56)【参考文献】

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

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

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

Ｇ０１Ｃ ２１／２８

Ｇ０９Ｂ ２９／１０

Ｇ０９Ｂ ２９／００

Ｂ６０Ｗ ４０／０７２

Ｂ６０Ｗ ３０／０９５

Ｇ０６Ｔ ７／６０

Ｇ０８Ｇ １／１３３

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

【請求項1】

自車両の走行した道路の道路曲率を求める曲率演算手段と、
道路地図情報を記憶する道路地図記憶手段と、
前記道路地図記憶手段に記憶されている道路地図情報に基づき測位衛星からの測位信号に基づいて前記自車両の自車位置を推定し、或いは前記測位衛星からの受信感度が低い場合は該測位信号と自律航法との併用により前記自車位置を推定して道路地図上にマップマッチングさせる自車位置推定手段と
を備える自車位置検出装置において、
前記自車位置推定手段は、
前記曲率演算手段で求めた前記道路曲率に基づき該道路曲率の変化からクロソイド区間を検出するクロソイド区間検出手段と、
前記クロソイド区間検出手段で検出した前記クロソイド区間における、前記曲率演算手段で求めた前記道路曲率及び移動距離から求めた第１走行軌跡と前記道路地図上にマップマッチングさせた前記自車位置に基づく該道路地図上の道路曲率及び移動距離から求めた第２走行軌跡とを比較して、前後方向のずれ幅である区間誤差を算出する区間誤差算出手段と、
前記区間誤差算出手段で算出した前記区間誤差に基づき、前記曲率演算手段で求めた前記道路曲率と前記道路地図上の前記道路曲率との類似度を求め、該類似度の高い区間で前記道路地図上にマップマッチングさせた前記自車位置の前後位置を補正する自車位置補正手段と
を備えていることを特徴とする自車位置検出装置。

【請求項2】

前記第１走行軌跡は第１ａ走行軌跡と第１ｂ走行軌跡とを含み、
前記曲率演算手段は、前記自車両の挙動に基づいて求めた前記第１ａ走行軌跡から前記クロソイド区間検出手段で検出した前記クロソイド区間の第１道路曲率を求める第１演算部と、カメラユニットで撮像した前方走行環境画像から該クロソイド区間検出手段で検出した該クロソイド区間の第２道路曲率を求める第２演算部とを有し、
前記自車位置推定手段は、
前記第２演算部で求めた前記第２道路曲率及び移動距離から求めた前記第１ｂ走行軌跡を前記カメラユニットの有する前記自車位置から前方の前記第２道路曲率を検出する位置までのオフセット距離分だけ修正するオフセット補正手段を更に有し、
前記区間誤差算出手段は、前記オフセット補正手段で補正した前記第１ｂ走行軌跡と、前記第２走行軌跡とを比較して前記区間誤差を算出する
ことを特徴とする請求項１記載の自車位置検出装置。

【請求項3】

前記自車位置補正手段は、前記区間誤差と予め設定されているしきい値とを比較し、該区間誤差が該しきい値以下の場合は、前記自車位置を補正しない
ことを特徴とする請求項１或いは２記載の自車位置検出装置。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、クロソイド区間の曲率変化を利用して道路地図上の自車位置の前後方向を補正するようにした自車位置検出装置に関する。

【背景技術】

【0002】

最近の車両においては、運転者の負担を軽減し、快適且つ安全に運転できるようにするための自動運転の技術が種々提案され、一部は既に実用化されている。自動運転の技術では、ＧＮＳＳ(Global Navigation Satellite System)測位衛星からの測位信号に基づき、自車位置を測位し、道路地図上に反映させることで、ローカライゼーション（自車位置推定）を行う。

【0003】

この場合、トンネル内走行等のように、測位信号の受信感度の低下によりＧＮＳＳ衛星からの有効な測位信号を受信することができない環境では、自律航法を併用する。自律航法では、車速センサで検出した車速とジャイロセンサで検出した角速度、及び前後加速度センサで検出した前後加速度等に基づき、積分処理によって求めた移動距離と方位からローカライゼーションを行う。

【0004】

しかし、自律航法では積分処理にて自車位置を推定しているので、積分区間が長くなると積分誤差が次第に大きくなり前後位置が劣化してしまう不都合がある。そのため、この積分誤差を所定にクリアさせる必要がある。

【0005】

例えば、特許文献１（特開２０１７－１５１１４８号公報）には、移動体（車両）に取り付けられたカメラによって撮像された地物（建物、道路標識）の現在の参照画像を取得し、この参照画像とその撮影位置情報とが関連付けられた地図データを取得する。そして、現在画像と参照画像とを照合することによって現在画像に対応する参照画像を特定し、特定された参照画像に地図データにて関連付けられた撮影位置に基づいて、移動体（車両）の現在位置を推定する技術が開示されている。

【先行技術文献】

【特許文献】

【0006】

【文献】特開２０１７－１５１１４８号公報

【発明の概要】

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

【0007】

しかし、上述した文献に開示されている技術では、そもそも地図データに地物の参照画像の情報が登録されていなければ、移動体（車両）の現在位置を特定することは困難である。しかも、道路標識等の地物は撤去され、或いは新設されたりするため、参照画像を常に更新しておかなければならず、維持費が嵩む不都合がある。

【0008】

更に、地物による現在位置の特定は大まかであり、自車位置を精密な精度で特定する必要のある自動運転に適用することは困難である。

【0009】

本発明は、上記事情に鑑み、自律航法との併用において道路地図データに設定した地物を目標点とせず、従って、地図データを常に更新する必要が無く、地物が撤去、或いは新設されても自車位置を高精度に推定することができ、しかも、維持費を安価に抑えることのできる自車位置検出装置を提供することを目的とする。

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

【0010】

本発明は、自車両の走行した道路の道路曲率を求める曲率演算手段と、道路地図情報を記憶する道路地図記憶手段と、前記道路地図記憶手段に記憶されている道路地図情報に基づき測位衛星からの測位信号に基づいて前記自車両の自車位置を推定し、或いは前記測位衛星からの受信感度が低い場合は該測位信号と自律航法との併用により前記自車位置を推定して道路地図上にマップマッチングさせる自車位置推定手段とを備える自車位置検出装置において、前記自車位置推定手段は、前記曲率演算手段で求めた前記道路曲率に基づき該道路曲率の変化からクロソイド区間を検出するクロソイド区間検出手段と、前記クロソイド区間検出手段で検出した前記クロソイド区間における、前記曲率演算手段で求めた前記道路曲率及び移動距離から求めた第１走行軌跡と前記道路地図上にマップマッチングさせた前記自車位置に基づく該道路地上の道路曲率及び移動距離から求めた第２走行軌跡とを比較して、前後方向のずれ幅である区間誤差を算出する区間誤差算出手段と、前記区間誤差算出手段で算出した前記区間誤差に基づき、前記曲率演算手段で求めた前記道路曲率と前記道路地図上の前記道路曲率との類似度を求め、該類似度の高い区間で前記道路地図上にマップマッチングさせた前記自車位置の前後位置を補正する自車位置補正手段とを備えている。

【発明の効果】

【0011】

本発明によれば、曲率演算手段で求めた道路曲率の変化から検出したクロソイド区間における道路曲率の走行軌跡と道路地図上にマップマッチングさせた自車位置に基づく道路曲率から求めた走行軌跡とを比較して、前後方向のずれ幅である区間誤差を算出し、この区間誤差に基づき、曲率演算手段で求めた道路曲率と道路地図上の道路曲率との類似度を求め、該類似度の高い区間で道路地図上にマップマッチングさせた自車位置の前後位置を補正するようにしたので、自律航法との併用において道路地図データに設定した地物を目標点とする必要が無く、従って、地図データを常に更新する必要が無く、地物が撤去、或いは新設されても自車位置を高精度に推定することができる。その結果、維持費を安価に抑えることが可能となる。

【図面の簡単な説明】

【0012】

【図1】自車位置検出装置の概略構成図

【図2】自車位置検出ルーチンを示すフローチャート（その１）

【図3】自車位置検出ルーチンを示すフローチャート（その２）

【図4】自車位置検出ルーチンを示すフローチャート（その３）

【図5】自車位置検出ルーチンを示すフローチャート（その４）

【図6】自車両がクロソイドカーブを走行している状態を示す説明図

【図7】自律航法による地図曲率に沿った走行と実際の道路曲率とのずれを示す説明図

【図8】（ａ）は車載カメラの視野角と自車位置との関係を示す側面図、（ｂ）は車載カメラで撮像した前方画像と自車位置との関係を示す説明図

【図9】クロソイドカーブ走行時における自車両の車両曲率の変化とその近似直線、及びクロソイド判定しきい値とを示す特性図

【図10】クロソイドカーブ走行時における車両曲率と地図曲率との変化を示す特性図

【図11】クロソイドカーブ走行時における車両曲率とカメラ曲率との変化を示す特性図

【図12】（ａ）はカーブ走行時の車両曲率の軌跡を示す走行図、（ｂ）はカーブ走行時のカメラ曲率の軌跡を示す走行図、（ｃ）は曲線路走行時の地図曲率の軌跡を示す走行図

【図13】（ａ）は車両曲率と地図曲率との軌跡を重ね合わせた状態の走行図、（ｂ）はカメラ曲率と地図曲率の軌跡を重ね合わせた状態の走行図

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

【0013】

以下、図面に基づいて本発明の一実施形態を説明する。図１に示す自車位置検出装置１は、自車両Ｍ（図８(a)参照）に搭載されている。この自車位置検出装置１は、道路地図上の自車位置を推定するロケータユニット１１を有し、このロケータユニット１１の入力側にカメラユニット１２が接続されている。

【0014】

カメラユニット１２は、車室内前部の上部中央に固定されており、車幅方向中央を挟んで左右対称な位置に配設されているメインカメラ１２ａ及びサブカメラ１２ｂからなる車載カメラ（ステレオカメラ）と、画像処理ユニット（ＩＰＵ）１２ｃとを有している。このカメラユニット１２は、両カメラ１２ａ，１２ｂで撮像した自車両Ｍ前方の前方走行環境画像情報をＩＰＵ１２ｃにて所定に画像処理して、ロケータユニット１１へ送信する。

【0015】

更に、このロケータユニット１１の入力側には、自車両Ｍの車速を検出する車速センサ１４、自車両Ｍに作用する前後加速度を検出する加速度センサ１５、自車両Ｍの角速度或いは角加速度を検出するジャイロセンサ１６、複数の測位衛星（ＧＮＳＳ衛星）から発信される測位信号を受信するＧＮＳＳ受信機１７等、自車両Ｍの走行時の位置（自車位置）を推定するに際し、必要とするセンサ類が接続されている。又、ジャイロセンサ１６はヨーレートセンサに代えても、同一の機能を得ることができる。

【0016】

ロケータユニット１１は、ＣＰＵ，ＲＡＭ，ＲＯＭ等を備える周知のマイクロコンピュータ、及びその周辺機器で構成されており、ＲＯＭにはＣＰＵで実行するプログラムやベースマップ等の固定データが予め記憶されている。

【0017】

このロケータユニット１１は自車位置を推定する機能として、自車位置推定手段としての自車位置推定部１１ａ、曲率演算手段としての曲率演算部１１ｂ、地図情報取得部１１ｃを備えている。地図情報取得部１１ｃは、道路地図記憶手段としての高精度道路地図データベース１８に記憶されている道路地図データに基づき、例えば、運転者が自動運転に際して、現在の自車位置からセットした目的地までのルートを設定する。

【0018】

この高精度道路地図データベース１８はＨＤＤ等の大容量記憶媒体であり、高精度な道路地図情報（ダイナミックマップ）が記憶されている。この高精度道路地図情報は、自動運転を行う際に必要とする車線データ（車線幅データ、車線中央の位置座標データ及び曲率データ、車線の進行方位角データ、制限速度等）を保有しており、この車線データは、道路地図上の各車線に数メートル間隔で格納されている。

【0019】

曲率演算部１１ｂは、第１、第２演算部を有している。すなわち、第１演算部は、後述する自律航法との併用において、加速度センサ１５で検出した自車両Ｍに作用する前後加速度、及びジャイロセンサ１６で検出した角速度（ヨーレート）ωｙ（図７参照）を積分処理して求めた、第１走行軌跡に含まれる第１ａ走行軌跡から自車両Ｍの走行した第１道路曲率（以下、「車両曲率」と称する）ＲCARを求める。又、第２演算部はカメラユニット１２の車載カメラ１２ａ，１２ｂで撮像した画像に基づき、走行車線の左右を区画する区画線を認識し、この区画線の中央（車線中央）の第２道路曲率（以下、「カメラ曲率」と称する）ＲCAM [1/m]を求める。

【0020】

このカメラ曲率ＲCAM [1/m]の求め方は種々知られているが、例えば、左右区画線を輝度差による二値化処理にて認識し、その曲率を最小二乗法による曲線近似式等にて求め、この左右区画線の曲率から車線中央のカメラ曲率ＲCAMを求める。

【0021】

地図情報取得部１１ｃは、例えば運転者が自動運転に際してセットした目的地に基づき、現在地から目的地までのルート地図情報を高精度道路地図データベース１８に格納されている地図情報から取得し、取得したルート地図情報（ルート地図上の車線データ）を自車位置推定部１１ａへ送信する。自車位置推定部１１ａは、ＧＮＳＳ受信機１７で受信した測位信号に基づき自車両Ｍの位置座標を取得し、この位置座標をルート地図情報上にマップマッチングして、道路地図上の自車位置を推定すると共に走行車線を特定し、道路地図データに記憶されている走行車線中央（図７に一点鎖線で示す）の位置座標に対する車幅方向の横位置偏差（ωｙ・ｓｉｎθｙ）を求める（図７参照）。尚、θｙはヨー角である。

【0022】

走行制御部２１は、自動運転時における走行制御に関する制御系（操舵制御、ブレーキ制御、車速制御）であり、自車位置推定部１１ａで求めた横位置偏差に基づき、この横位置偏差が０となるように操舵制御等をフィードバック制御して、自車両Ｍが車線中央に沿って走行するように制御する。その際、カメラユニット１２からの前方走行環境画像に基づき走行車線の左右を区画する区画線からその中央（車線中央）を求め、車線中央に沿って自車両Ｍを走行させる車線維持制御を同時に実行する。

【0023】

ところで、自車両Ｍがトンネル内の走行等、ＧＮＳＳ受信機１７の受信感度が低下して、ＧＮＳＳ衛星からの測位信号を有効に受信することのできない環境では自律航法による測位を併用する。自律航法では、ルート地図の現在位置から進行方向に設定されている目標進行路の車線データに基づき、自車両Ｍの進行すべき方位を設定して、自車両Ｍがルート地図上に設定した目標進行路に沿って走行させる走行制御を行う。

【0024】

すなわち、車速センサ１４で検出した車速を積分処理して移動距離を求め、ジャイロセンサ１６で検出した角速度（ヨーレート）、及び加速度センサ１５で検出した前後加速度を積分処理して移動方位角を検出し、これらに基づいて自車両Ｍを道路地図上に設定した目標進行路に沿って走行させる。従って、自車位置推定部１１ａは自車両Ｍの移動距離、及び移動方位角から現在の道路地図上の自車位置を推定する。この場合も、上述と同様、カメラユニット１２からの前方走行環境画像に基づき設定した車線中央に沿って自車両Ｍを走行させる車線維持制御を同時に実行する。

【0025】

ところで、自律航法との併用では、上述したように自車両Ｍの移動距離、及び移動方位角を積分処理により求めているため、トンネル走行等の受信感度が低下している区間（積分期間）が長いと積分誤差が増大し、前後方向の自車位置検出精度が劣化し、自動運転を継続することが困難となる。そのため、本実施形態では、カメラユニット１２からの前方走行環境画像に基づいて求めたカメラ曲率ＲCAM、及び自車両Ｍの第１ａ走行軌跡から求めた車両曲率ＲCARと道路地図上に記憶されている車線中央の道路曲率（以下、「地図曲率」と称する）ＲMPU [1/m]とを比較し、その類似度（一致度）に応じて道路地図上における自車位置の前後位置を補正する。

【0026】

しかし、直線路、及び一定曲率の曲線路では、カメラ曲率ＲCAM及び車両曲率ＲCARと地図曲率ＲMPUとを比較しても、前後位置のずれを検出することは困難である。そのため、本実施形態では、自車両Ｍが、直線区間と曲率がほぼ一定のカーブ区間とを連結するクロソイド区間を走行した際のカメラ曲率ＲCAM及び車両曲率ＲCARと地図曲率ＲMPUとの類似度（一致度）を比較して、自車位置の前後方向のずれを補正する。

【0027】

ここで、クロソイド区間とはクロソイド曲線で形成されたカーブ路である。クロソイド曲線とは、カーブの曲率を一定割合で変化させた曲線であり、曲率をＲ [1/m]、クロソイド曲線の区間長をＬ（図６ではＬ２で表している）とした場合、
（１／Ｒ）・Ｌ＝Ａ^２ …（１）
の関係がある。尚、Ａは長さの次元を持つ定数（クロソイドパラメータ）である。

【0028】

上述した自車位置推定部１１ａでの自律航法との併用時における自車位置検出は、具体的には、図２～図５に示す自車位置検出ルーチンに従って求める。

【0029】

このルーチンでは、先ず、ステップＳ１で、ＧＮＳＳ受信機１７で受信する測位信号の感度を調べ、受信感度か良好な場合は、そのままルーチンを抜ける。一方、受信感度が低下しており、自車位置を正確に検出することが困難な場合は、ステップＳ２へ進む。ステップＳ２へ進むと、自律航法併用による自車位置推定を実行させる。尚、自律航法については既述したので、ここでの説明を省略する。

【0030】

次いで、ステップＳ３へ進み、自律航法により走行した自車位置を道路地図上にマップマッチングして、ステップＳ４へ進む。ステップＳ４では、道路地図上にマップマッチングさせた自車位置の進行方向のルート地図に記憶されている車線データから所定区間長（例えば、２０[m]）の道路曲率データを読込み、その区間平均曲率ＲAVを算出する。

【0031】

そして、ステップＳ５へ進み、区間平均曲率ＲAVとカーブ曲率判定しきい値Ｒｏとを比較する。この急カーブ判定しきい値は、カメラ曲率ＲCAMと車両曲率ＲCARとの検出特性に基づいて設定されている。すなわち、カメラユニット１２はカーブ曲率が大きい場合、図８（ｂ）に示すように、遠位までの画像を取得することが困難となる。一方、自車両Ｍの第１ａ走行軌跡から検出する車両曲率ＲCARはカーブ曲率が小さい場合、車両挙動（ヨーレート）が小さいため検出精度の誤差が大きくなる。

【0032】

従って、カーブ曲率が小さい場合はカメラ曲率ＲCAMが車両曲率ＲCARよりも信頼性は高い、それに対し、カーブ曲率が大きい場合は車両曲率ＲCARがカメラ曲率ＲCAMよりも信頼性は高い。カーブ曲率判定しきい値Ｒｏは、カメラ曲率ＲCAMと車両曲率ＲCARとの特性に基づいて設定されており、本実施形態では、Ｒｏ＝１／４００[m]に設定しているが、これに限定されるものではない。

【0033】

そして、ＲAV≦Ｒｏの場合はステップＳ６へ進み、ＲAV＞Ｒｏの場合はステップＳ７へ進む。ステップＳ６では、上述した区間平均曲率ＲAVを検出した区間（例えば、区間長は２０[m]）と同一区間の走行軌跡から当該区間長の車両曲率ＲCARを読込み、ステップＳ８へ進む。一方、ステップＳ７へ分岐した場合は、区間平均曲率ＲAVを検出した時刻と同時刻に撮像した前方走行環境画像に基づいて求めたカメラ曲率ＲCAMを読込み、ステップＳ１４へジャンプする。

【0034】

［車両曲率に基づく前後位置補正］
先ず、ステップＳ８～Ｓ１３で実行される車両曲率ＲCARと地図曲率ＲMPUとの比較に基づく前後位置補正について説明する。

【0035】

ステップＳ８へ進むと、区間平均曲率ＲAVを検出した区間の走行軌跡に基づいて算出した車両曲率ＲCARに基づきクロソイド区間を検出する。具体的には、図９に示す横軸の移動距離を区間長とし、縦軸を曲率とした場合、演算周期毎に求めた車両曲率ＲCARを最小二乗法により、同図に実線で示す直線近似式を求め、その傾きである曲率増加量を求める。尚、本実施形態では、左旋回をプラスで表し、右旋回をマイナスで表している。従って、図９は左旋回を示している。

【0036】

更に、各車両曲率ＲCARの相関係数を求める。相関係数は、車両曲率ＲCARの時系列データの相関を求める係数であり、相関係数＝１で完全な相関を有していることを意味する。尚、このステップＳ８，Ｓ９、及び後述するステップＳ１４，Ｓ１５での処理が、本発明のクロソイド区間検出手段に対応している。

【0037】

そして、ステップＳ９へ進み、曲率増加量（直線近似式の傾き）とクロソイド判定しきい値とを比較し（図９参照）、且つ、相関係数と相関判定しきい値とを比較して、通過した走行車線の区間がクロソイド区間であったか否かを調べる。尚、クロソイド判定しきい値は直線路、及び一定曲率の曲線路を排除するものである。又、本実施形態では、相関判定しきい値を０．９５に設定しているが、これに限定されるものではない。この場合、クロソイド区間を検出する区間長を相関係数との関係で可変させても良い。すなわち、相関係数が相関判定しきい値以上の場合に、当該相関係数に応じて区間長を延ばすことで、クロソイド区間の検出精度を高めることができる。

【0038】

そして、曲率増加量がクロソイド判定しきい値以上でかつ、相関係数が相関判定しきい値以上の場合、通過した進行路はクロソイド区間Ｌ２（図６参照）であると判定し、ステップＳ１０へ進む。又、曲率増加量がクロソイド判定しきい値未満、或いは、相関係数が相関判定しきい値未満の場合、通過した進行路は直線路Ｌ１、或いは一定曲率の曲線路Ｌ３（図６参照）であると判定し、ルーチンを抜ける。

【0039】

ステップＳ１０へ進むと、車両曲率ＲCARを検出した区間の前後１００ｍの道路地図上の区間にある地図曲率を読込み、これを探索対象地図区間とする。そして、ステップＳ１１へ進み、この探索対象地図区間に未探索区間があるか否かを調べ、無い場合は、そのままルーチンを抜ける。一方、未探索区間がある場合は、ステップＳ１２へ進み、探索対象地図区間の未探索区間の手前から車両曲率ＲCARを検出した区間と同一距離となる地図曲率ＲMPUを選択する。そして、ステップＳ１３で地図曲率ＲMPUの最小二乗法による直線近似式を求め、この直線近似式に基づいて算出した近似直線のクロソイド区間の開始点と終了点を求める。次いで、車両曲率ＲCARを検出した区間の開始点と終了位置との距離差分から誤差ｄ１，ｄ２を算出し（図１０参照）、この両誤差ｄ１，ｄ２を加算して、前後方向のずれ幅である区間誤差Δｄ（＝ｄ１＋ｄ２）を求める。尚、このステップＳ１０～Ｓ１３，及び後述するステップＳ２１～Ｓ２４での処理が、本発明の区間誤差算出手段に対応している。

【0040】

そして、ステップＳ１４へ進み、区間誤差Δｄの絶対値と誤差判定しきい値とを比較する。この誤差判定しきい値は、地図上の何処の区間が実際に走行して検出した車両曲率ＲCARと一致（類似）しているかを判定するものであり、それを両近似直線の近接度（重なり度合い）から調べる。尚、本実施形態では誤差判定しきい値を±５[m}に設定しているが、これに限定されず、±５[m}未満であっても良い。誤差判定しきい値を短い値に設定することで類似度をより高い精度で判定することができる。

【0041】

そして、区間誤差Δｄの絶対値が誤差判定しきい値以下の場合、換言すれば、区間誤差Δｄが誤差判定しきい値以内に収まっている場合、類似度が高いと判定し、ステップＳ１５へ進む。一方、区間誤差Δｄの絶対値が誤差判定しきい値を超えている場合、類似度が低いと判定し、ステップＳ１１へ戻り、探索対象地図区間の未探索区間から地図曲率ＲMPUを再設定し、ステップＳ１２以降を繰り返し実行する。そして、探索対象地図区間の未探索区間が存在しなくなった場合、ステップＳ１１からルーチンを抜ける。

【0042】

又、ステップＳ１５へ進むと、車両曲率ＲMPUを検出した区間と類似度が高いと判定した道路地図区間の位置で自車位置を補正してルーチンを抜ける。尚、このステップＳ１４，Ｓ１５での処理が、本発明の自車位置補正手段に対応している。

【0043】

その結果、同一区間の，第１ａ走行軌跡を示す図１２（ａ）の車両曲率ＲCARと第２走行区間を示す同図（ｃ）の地図曲率ＲMPUとを、類似度が高い区間を基準に重ね合わせることで、図１３（ａ）に示すように、地図曲率ＲMPUの第２走行軌跡が車両曲率ＲCARと一致される。そのため、自律航法との併用であっても、道路地図上の自車位置を、クロソイド区間を走行中の自車両Ｍの車両曲率ＲCARに基づき高精度に検出することができる。

【0044】

［カメラ曲率に基づく前後位置補正］
次に、ステップＳ１６～Ｓ２８で実行されるカメラ曲率ＲCAMと車両曲率ＲCAR、及び地図曲率ＲMPUとの比較に基づく前後位置補正について説明する。

【0045】

先ず、ステップＳ７からステップＳ１６へ進むと、区間平均曲率ＲAVを検出した区間の走行軌跡に基づいて算出したカメラ曲率ＲCAMの区間と同一時刻に検出した車両曲率ＲCARに基づきクロソイド区間を検出する。具体的には、図１１に示す横軸の移動距離を区間長とし、縦軸を曲率とした場合、演算周期毎に求めた車両曲率ＲCARを最小二乗法により、同図に実線で示す直線近似式を求め、その傾きである曲率増加量を求める。更に、各車両曲率ＲCARの相関係数を求める。

【0046】

そして、ステップＳ１７へ進み、図９に示す曲率増加量（直線近似式の傾き）とクロソイド判定しきい値とを比較し、且つ、相関係数と相関判定しきい値とを比較して、通過した走行車線の区間がクロソイド区間であったか否かを調べる。尚、クロソイド判定しきい値は、上述と同様に直線路、及び一定曲率の曲線路を排除するものである。又、本実施形態では、緩やかなカーブ路を走行時に検出する車両曲率ＲCARは、上述した急なカープ路を走行した際に検出する値に比し乱れやすいため、相関判定しきい値を０．８に設定している。

【0047】

その後、ステップＳ１８へ進み、カメラ曲率ＲCAMと同時刻に検出した区間の車両曲率ＲCARを読込み、ステップＳ１９で、同一時刻の区間で検出したカメラ曲率ＲCAMに基づき最小二乗法により、図１１に破線で示す近似直線を求める。

【0048】

次いで、ステップＳ２０で、車両曲率ＲCARによる直線近似式の傾きがカメラ曲率ＲCAMの直線近似式の傾きに対して所定範囲±α[%}に収まっているか否かを調べる。車両曲率ＲCARによる直線近似時期の傾きがカメラ曲率ＲCAMによる直線近似式の傾きに対して大きくずれていた場合、カメラ曲率ＲCAMによる、第１走行軌跡に含まれる第１ｂ走行軌跡の前後方向を車両曲率ＲCARによる第１ａ走行軌跡に対して正しく整合させることが困難となり、誤差が大きくなる。尚、本実施形態では、この範囲±αを±３０{%}に設定しているが、これに限定されるものではない。

【0049】

そして、車両曲率ＲCARの近似直線の傾きが、カメラ曲率ＲCAMの近似直線の傾きに対して所定範囲±α[%}に収まっている場合は、ステップＳ２１へ進み、外れている場合は、ルーチンを抜ける。

【0050】

ステップＳ２１へ進むと、カメラ曲率ＲCAMを検出した区間の前後１００ｍの道路地図上の区間にある地図曲率を読込み、探索対象地図区間とする。そして、ステップＳ２２で、この探索対象地図区間に未探索区間があるか否かを調べ、無い場合は、そのままルーチンを抜ける。一方、未探索区間がある場合は、ステップＳ２３へ進み、探索対象地図区間の未探索区間の手前からカメラ曲率ＲCARを検出した区間と同一距離となる地図曲率ＲMPUを選択する。そして、ステップＳ２４で地図曲率ＲMPUの最小二乗法による直線近似式を求め、この直線近似式に基づいて算出した近似直線のクロソイド区間の開始点と終了点を求める。次いで、前述したステップＳ１３と同様に（図１０参照）カメラ曲率ＲCAMを検出した区間の開始点と終了位置との距離差分から誤差ｄ１，ｄ２を算出し、この両誤差ｄ１，ｄ２を加算して、前後方向のずれ幅である区間誤差Δｄ（＝ｄ１＋ｄ２）を求める。

【0051】

そして、ステップＳ２５へ進み、区間誤差Δｄの絶対値と誤差判定しきい値とを比較する。この誤差判定しきい値は、地図上の何処の区間が実際に走行して検出したカメラ曲率ＲCAMと一致（類似）しているかを判定するものであり、それを両近似直線の近接度（重なり度合い）から調べる。尚、本実施形態では誤差判定しきい値を±５[m}に設定しているが、これに限定されず、±５[m}未満であっても良い。誤差判定しきい値を短い値に設定することで類似度をより高い精度で判定することができる。

【0052】

そして、区間誤差Δｄの絶対値が誤差判定しきい値以下の場合、換言すれば、区間誤差Δｄが誤差判定しきい値以内に収まっている場合、類似度が高いと判定し、ステップＳ２６へ進む。一方、区間誤差Δｄの絶対値が誤差判定しきい値を超えている場合、類似度が低いと判定し、ステップＳ２２へ戻り、探索対象地図区間の未探索区間から地図曲率ＲMPUを再設定し、ステップＳ２３以降へ進む。探索対象地図区間の未探索区間が存在しなくなった場合、ステップＳ２２からルーチンを抜ける。

【0053】

又、ステップＳ２６へ進むと、曲率ＲMPUを検出した区間と類似度が高いと判定した道路地図区間の位置で自車位置補正する。

【0054】

そして、ステップＳ２７へ進み、車両曲率ＲCARによる第１ａ走行軌跡に対するカメラ曲率ＲCAMによる前後オフセット量を算出する。すなわち、図８（ａ）に示すように、自車両Ｍの前後方向の位置（自車位置）を前輪の回転中心に設定した場合、車両曲率ＲCARは前輪の操舵角によって決定されるため、自車位置と走行軌跡とはほぼ同時刻である。これに対し、車載カメラ１２ａ，１２ｂで撮像する前方走行環境は、自車位置よりも前方の画像を常に取得しているため、この画像に基づいて求めるカメラ曲率ＲCAMは、同図（ｂ）に示すように、自車位置よりも常にオフセット距離Ｌｚだけ前方の道路形状を含んだ区間曲率を検出していることになり、同時刻では、車両曲率ＲCARとの間にずれが生じている。このオフセット距離Ｌｚは、道路曲率によって変化するため、車両曲率ＲCARによる第１ａ走行軌跡とカメラ曲率ＲCAMによる第１ｂ走行軌跡とを比較してオフセット距離Ｌｚを求め、このオフセット距離Ｌｚ分だけカメラ曲率ＲCAMによる第１ａ走行軌跡を修正する。

【0055】

先ず、ステップＳ２７では、同時刻の車両曲率ＲCARとカメラ曲率ＲCAMによる近似直線における開始点と終了点の曲率差分ｄ５，ｄ６を求めて（図１１参照）、これを平均する（（ｄ＋ｄ６）／２）。そして、この平均値（ｄ５＋ｄ６）／２）をカメラ曲率ＲCAMによる近似直線の傾きで除算して、移動方向（前後方向）のオフセット距離を算出する。すなわち、傾きをθａ、前後オフセット量をβとした場合、
β＝（ｄ５＋ｄ６）／２）／ｔａｎθａ
となる。

【0056】

そして、ステップＳ２２８へ進み、オフセット距離Ｌｚ分だけ、前述したステップＳ２６で補正した後の自車位置を前後方向にオフセットさせ、ルーチンを抜ける。尚、ステップＳ２７，Ｓ２８での処理が、本発明のオフセット補正手段に対応している。

【0057】

その結果、同一区間の、第１ｂ走行軌跡を示す図１２（ｂ）のカメラ曲率ＲCAMと第２走行軌跡を示す同図（ｃ）の地図曲率ＲMPUとを、類似度が高い区間を基準に重ね合わせることで、図１３（ｂ）に示すように、地図曲率ＲMPUの第２走行軌跡がカメラ曲率ＲCAMと一致される。そして、カメラ曲率ＲCAMが持つ自車位置とのオフセット距離Ｌｚを考慮することで、自律航法との併用であっても、道路地図上の自車位置を、クロソイド区間を走行中の自車両Ｍのカメラ曲率ＲCAMに基づき高精度に検出することができる。

【0058】

このように、本実施形態では、道路曲率が連続的に変化するクロソイド区間を検出し、このクロソイド区間を通過した際の車両曲率ＲCAR、或いはカメラ曲率ＲCAMに基づいて、ロケータユニット１１の自車位置推定部１１ａは道路地図上の自車位置を修正するようにしたので、自律航法において道路地図データに設定した地物を目標点とすることなく、自車位置を高精度に修正することができる。その結果、地図データを常に更新する必要が無く、地物が撤去、或いは新設されても自車位置を高精度に推定することができるため、維持費を安価に抑えることができる。

【0059】

更に、道路曲率に応じ、道路曲率が急な場合は車両曲率ＲCARによる第１ａ走行軌跡に基づいて前後位置を修正し、又、道路曲率が緩やかな場合はカメラ曲率ＲCAMによる第1ｂ走行軌跡に基づいて前後位置を修正するようにしたので、道路地図上の自車位置をより正確に修正することができる。その際、カメラ曲率ＲCAMを取得する車載カメラ１２ａ，１２ｂの自車位置に対するオフセット距離Ｌｚを、車両曲率ＲCARの第１ａ走行軌跡とカメラ曲率ＲCAMの第1ｂ走行軌跡とに基づいて修正するようにしたので、正しいカメラ曲率ＲCAM(t-1)による第1ａ走行軌跡に基づいて道路地図上の自車位置を修正することができる。

【0060】

尚、本発明は、上述した実施形態に限るものではなく、例えば自車進行方向のルート地図に類似するクロソイド区間を有するカーブ路が複数存在する場合、道路地図上で通過中のカーブ路には前後位置補正は適用するが、通過済みのカーブには適用しないようにしても良い。又、前方に類似するクロソイド区間を有するカーブ路が複数存在している場合は、自車両Ｍに最も近いカーブ路のみで前後位置補正を行うようにしても良い。

【符号の説明】

【0061】

１…自車位置検出装置、
１１…ロケータユニット、
１１ａ…自車位置推定部、
１１ｂ…曲率演算部、
１１ｃ…地図情報取得部、
１２…カメラユニット、
１２ａ，１２ｂ…車載カメラ、
１４…車速センサ、
１５…加速度センサ、
１６…ジャイロセンサ、
１７…ＧＮＳＳ受信機、
１８…高精度道路地図データベース、
２１…走行制御部、
ｄ１，ｄ２…誤差、
ｄ５，ｄ６…曲率差分、
Ｌ１…直線路、
Ｌ２…クロソイド区間、
Ｌ３…曲線路、
Ｌｚ…オフセット距離、
Ｍ…自車両、
ＲCAM…カメラ曲率、
ＲAV…区間平均曲率、
ＲCAR…車両曲率、
ＲMPU…地図曲率、
Ｒｏ…カーブ曲率判定しきい値、
Δｄ…区間誤差

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】

【図13】