JP2020077297A - 位置姿勢推定装置 - Google Patents

Abstract

【課題】車両の位置及び姿勢の推定精度を向上できる位置姿勢推定装置を提供する。【解決手段】位置姿勢推定装置は、車両１０に搭載された撮像部２により得られた画像から、車両１０の周囲の道路上の第１の構造物を検出する検出部３１と、車両１０の複数の仮定位置及び仮定姿勢のそれぞれについて、その仮定位置及び仮定姿勢に従って第１の構造物を地図上に投影するか、または、その仮定位置及び仮定姿勢に従って地図上の車両の周囲の領域に表された道路上の第２の構造物を画像上に投影したときの、第１の構造物と第２の構造物との一致度合を、第１の構造物についての検出の確からしさを表す信頼度、第１の構造物の種類及び第１の構造物の劣化の程度の少なくとも一つに従って設定される第１の構造物の重みに応じて算出し、一致度合が最大となるときの車両１０の仮定位置及び仮定姿勢を車両１０の位置及び姿勢として推定する位置姿勢推定部３２とを有する。【選択図】図３

Description

本発明は、車両の位置及び姿勢を推定する位置姿勢推定装置に関する。

車両の運転支援あるいは自動運転制御に利用するため、車両周囲に存在する物体を検知するためのカメラが車両に搭載されることがある。カメラが撮影することで得られた画像に表された物体と車両との位置関係を正確に推定するためには、車両の位置及び姿勢が正確に求められることが好ましい。そこで、自車両に搭載された撮像部により得られた画像から道路の第１形状候補を抽出し、抽出した第１形状候補と、自車両の周辺の地図データから抽出された道路の第２形状候補とを３次元または２次元で構成された一つの論理的空間に投影し、投影された双方の道路形状候補の重なり状態に基づいて自車両の姿勢及び位置を推定する技術が提案されている（例えば、特許文献１を参照）。

特開２００１−３３１７８７号公報

しかしながら、車両周囲の環境によっては、車両に搭載されたカメラにより得られた画像に路面の一部が明瞭に表されないことがある。このような場合、上記の技術では、画像から抽出された道路の形状候補と地図データから抽出された道路の形状候補との重なり度合いを正確に評価できなくなり、その結果として、車両の位置及び姿勢の推定精度も低下する。

そこで、本発明は、車両の位置及び姿勢の推定精度を向上できる位置姿勢推定装置を提供することを目的とする。

一つの実施形態によれば、位置姿勢推定装置が提供される。この位置姿勢推定装置は、車両に搭載された撮像部により得られた画像から、車両の周囲の道路上の第１の構造物を検出する検出部と、車両の複数の仮定位置及び仮定姿勢のそれぞれについて、その仮定位置及び仮定姿勢に従って画像から検出された第１の構造物を地図上に投影するか、または、その仮定位置及び仮定姿勢に従って地図上の車両の周囲の領域に表された道路上の第２の構造物を画像上に投影したときの、第１の構造物と第２の構造物との一致度合を、第１の構造物についての検出の確からしさを表す信頼度、第１の構造物の種類及び第１の構造物の劣化の程度の少なくとも一つに従って設定される第１の構造物の重みに応じて算出し、複数の仮定位置及び仮定姿勢のうち、一致度合が最大となるときの車両の仮定位置及び仮定姿勢を車両の位置及び姿勢として推定する位置姿勢推定部とを有する。

本発明に係る位置姿勢推定装置は、車両の位置及び姿勢の推定精度を向上できるという効果を奏する。

位置姿勢推定装置が実装される車両制御システムの概略構成図である。 位置姿勢推定装置の一つの実施形態である電子制御装置のハードウェア構成図である。 位置姿勢推定処理を含む車両制御処理に関する、電子制御装置のプロセッサの機能ブロック図である。 画像から検出された道路上の構造物ごとの重み係数の一例を示す図である。 位置姿勢推定処理の概要説明図である。 位置姿勢推定処理を含む、車両制御処理の動作フローチャートである。 画像上の照明領域の一例を示す図である。

以下、図を参照しつつ、位置姿勢推定装置について説明する。この位置姿勢推定装置は、車両に搭載されたカメラにより得られた、車両の周囲が表された画像から道路上の構造物を検出するとともに、検出された構造物ごとにその検出の確からしさを表す信頼度を求め、かつ、その構造物の劣化の程度を推定する。またこの位置姿勢推定装置は、画像から検出された構造物ごとに、その構造物についての信頼度、その構造物の種類、または、その構造物の劣化の程度に応じて、その構造物に重み付けして、車両の位置及び姿勢を仮定して画像上に投影された地図上の車両の周囲の領域に表された道路の構造物と検出された構造物との一致度合をもとめる。そしてこの位置姿勢推定装置は、一致度合が最大となるときの仮定された車両の位置及び姿勢を、車両の実際の位置及び姿勢として推定する。これにより、例えば、夜間あるいは荒天下で車両が走行している場合など、画像上において車両の周囲に存在する道路上の構造物の一部が見え難い場合でも、この位置姿勢推定部は、車両の位置及び姿勢の推定精度を向上できる。
なお、道路上の構造物には、例えば、実線あるいは点線にて路面上に描画された車線区画線といった道路標示、道路端の縁石などが含まれる。

以下では、位置姿勢推定装置を、車両制御システムに適用した例について説明する。この例では、位置姿勢推定装置は、車両に搭載されたカメラにより得られた画像に対して位置姿勢推定処理を実行することで、車両の位置及び姿勢を推定し、その推定結果に基づいて、車両の周囲に存在する各種の物体、例えば、他の車両または人との相対的な位置関係を求めて、車両の運転制御に利用する。

図１は、位置姿勢推定装置が実装される車両制御システムの概略構成図である。また図２は、位置姿勢推定装置の一つの実施形態である電子制御装置のハードウェア構成図である。本実施形態では、車両１０に搭載され、かつ、車両１０を制御する車両制御システム１は、車両１０の周囲を撮影するためのカメラ２と、測位情報受信機３と、位置姿勢推定装置の一例である電子制御装置（ＥＣＵ）４とを有する。カメラ２と、測位情報受信機３と、ＥＣＵ４とは、コントローラエリアネットワークといった規格に準拠した車内ネットワーク５を介して通信可能に接続される。

カメラ２は、撮像部の一例であり、CCDあるいはC-MOSなど、可視光に感度を有する光電変換素子のアレイで構成された２次元検出器と、その２次元検出器上に撮影対象となる領域の像を結像する結像光学系を有する。そしてカメラ２は、車両１０の前方を向くように、例えば、車両１０の車室内に取り付けられる。そしてカメラ２は、所定の撮影周期（例えば1/30秒〜1/10秒）ごとに車両１０の前方領域を撮影し、その前方領域が写った画像を生成する。カメラ２により得られた画像は、カラー画像であってもよく、あるいは、グレー画像であってもよい。

カメラ２は、画像を生成する度に、その生成した画像を、車内ネットワーク５を介してＥＣＵ４へ出力する。

測位情報受信機３は、測位部の一例であり、車両１０の現在位置を表す測位情報を取得する。例えば、測位情報受信機３は、ＧＰＳ受信機とすることができる。そして測位情報受信機３は、測位情報を取得する度に、取得した測位情報を、車内ネットワーク５を介してＥＣＵ４へ出力する。

ＥＣＵ４は、車両１０を制御する。本実施形態では、ＥＣＵ４は、カメラ２により得られた画像から検出された他の物体と車両１０との相対的な位置関係に基づいて車両１０を自動運転するよう、車両１０を制御する。そのために、ＥＣＵ４は、通信インターフェース２１と、メモリ２２と、プロセッサ２３とを有する。

通信インターフェース２１は、通信部の一例であり、ＥＣＵ４を車内ネットワーク５に接続するためのインターフェース回路を有する。すなわち、通信インターフェース２１は、車内ネットワーク５を介して、カメラ２及び測位情報受信機３と接続される。そして通信インターフェース２１は、カメラ２から画像を受信する度に、受信した画像をプロセッサ２３へわたす。また通信インターフェース２１は、測位情報受信機３から測位情報を受信する度に、受信した測位情報をプロセッサ２３へわたす。

メモリ２２は、記憶部の一例であり、例えば、揮発性の半導体メモリ及び不揮発性の半導体メモリを有する。そしてメモリ２２は、ＥＣＵ４のプロセッサ２３により実行される位置姿勢推定処理において使用される各種のデータ、例えば、カメラ２から受信した画像、測位情報受信機３から受信した測位情報、及び、位置姿勢推定処理で利用される識別器を特定するための各種パラメータなどを記憶する。さらに、メモリ２２は、地図情報を記憶する。この地図情報は、道路上の構造物の種類及び位置を表す情報を含む。なお、道路上の構造物の位置は、実空間における所定の基準位置を原点とする世界座標系で表される。

プロセッサ２３は、制御部の一例であり、１個または複数個のＣＰＵ(Central Processing Unit)及びその周辺回路を有する。プロセッサ２３は、論理演算ユニット、数値演算ユニットあるいはグラフィック処理ユニットといった他の演算回路をさらに有していてもよい。そしてプロセッサ２３は、車両１０が走行している間、カメラ２から画像を受信する度に、受信した画像に対して位置姿勢推定処理を実行する。さらに、プロセッサ２３は、受信した画像から検出された車両１０の周囲の他の物体と車両１０との相対的な位置関係に基づいて、車両１０を自動運転するよう、車両１０を制御する。

図３は、位置姿勢推定処理を含む車両制御処理に関する、ＥＣＵ４のプロセッサ２３の機能ブロック図である。プロセッサ２３は、検出部３１と、位置姿勢推定部３２と、投影部３３と、運転計画部３４と、車両制御部３５とを有する。プロセッサ２３が有するこれらの各部は、例えば、プロセッサ２３上で動作するコンピュータプログラムにより実現される機能モジュールである。あるいは、プロセッサ２３が有するこれらの各部は、プロセッサ２３に設けられる、専用の演算回路であってもよい。また、プロセッサ２３が有するこれらの各部のうち、検出部３１、位置姿勢推定部３２及び投影部３３が、位置姿勢推定処理を実行する。また、プロセッサ２３は、カメラ２から画像が得られる度に、その画像に対して位置姿勢推定処理を実行する。そのため、以下では、検出部３１、位置姿勢推定部３２及び投影部３３について、一つの画像に対する処理を説明する。

検出部３１は、カメラ２により生成された画像から、車両１０の周囲の道路上の構造物を検出する。そして検出部３１は、検出した構造物ごとに、その構造物の種類及び劣化の程度を推定するとともに、その構造物についての検出の信頼度をもとめる。

例えば、検出部３１は、画像を識別器に入力することで、画像に表された道路上の構造物を検出する。検出部３１は、識別器として、例えば、入力された画像から、その画像の各画素について、その画素に表される可能性の有る物体の種類ごとに、その物体がその画素に表されている確からしさを出力し、確からしさが最大となる物体が表されていると識別するように予め学習されたセグメンテーション用の識別器を用いることができる。検出部３１は、そのような識別器として、例えば、Fully Convolutional Network（ＦＣＮ）といった、セグメンテーション用のコンボリューショナルニューラルネットワーク型（ＣＮＮ）のアーキテクチャを有するディープニューラルネットワーク（ＤＮＮ）を用いることができる。あるいは、検出部３１は、ランダムフォレストあるいはサポートベクトルマシンといった他の機械学習手法に従ったセグメンテーション用の識別器を利用してもよい。

この場合、検出部３１は、画像をセグメンテーション用の識別器に入力することで、その画像において道路上の構造物が写っている画素を特定する。そして検出部３１は、同じ種類の構造物が写っている画像の集合をその構造物が表された領域とする。その際、検出部３１は、同じ種類の物体が表された画素の集合に対してラベリング処理を実行することで、互いに連結された同じ種類の物体が表された画素同士をグルーピングしてもよい。そして検出部３１は、構造物ごとに、その構造物が表された領域に含まれる各画素についての、識別器が出力する確からしさの平均値を、その構造物についての検出の信頼度とする。

あるいは、検出部３１は、画像に対してSobelフィルタといったエッジ検出フィルタを適用して、画像からエッジが表されたエッジ画素を検出してもよい。検出部３１は、エッジ画素の集合に対してハフ変換を実行することで、画像から直線を検出してもよい。そして検出部３１は、画像から検出された直線が車両１０の進行方向に沿って延伸されている場合に、その直線を、道路上の構造物の一例である車線区画線として検出してもよい。この場合、検出部３１は、検出した直線上に位置するエッジ画素の数が多いほど、信頼度を高く設定すればよい。

検出部３１は、画像から検出された道路上の構造物のそれぞれについて、その構造物の劣化の程度を推定する。例えば、道路上の構造物の一例である道路標示は、劣化するほど掠れて見えなくなる。そこで、検出部３１は、道路上の構造物のうちの同じ種類のものが表されている画素の集合に対してモルフォロジーの膨張収縮演算を行うことで、その種類の構造物の外周で囲まれた、その構造物の外形形状を表す領域（以下、外形領域と呼ぶ）をもとめることができる。そして検出部３１は、外形領域に含まれる画素の総数に対する、外形領域に含まれる、その種類以外のものが表された画素の数の比率を劣化の程度を表す値として算出する。

あるいは、識別器そのものが、構造物の劣化の程度が異なるものを別の種類の物体として検出するように予め学習されてもよい。例えば、識別器は、実線で表された車線区画線について、劣化の程度が「高」、「中」、「低」であるものが異なる物体の種類として識別されるよう、予め学習されてもよい。同様に、識別器は、点線で表された車線区画線について、劣化の程度が「高」、「中」、「低」であるものが異なる物体の種類として識別されるよう、予め学習されてもよい。この場合には、検出部３１は、識別器により検出された道路上の構造物の種類として表された劣化の程度を、その構造物の劣化の程度として推定すればよい。

さらに、検出部３１は、画像から、道路上の構造物以外の、車両１０の走行に影響を与え得る他の物体（例えば、車両あるいは人など）も、道路上の構造物の検出と同様に、画像を識別器に入力することで検出する。その際、検出部３１は、道路上の構造物の検出に用いる識別器とは別個に、入力された画像から、その画像に表された物体を検出するように予め学習された識別器を用いてもよい。検出部３１は、そのような識別器として、例えば、Single Shot MultiBox Detector（ＳＳＤ）、または、Faster R-CNNといった、コンボリューショナルニューラルネットワーク型（ＣＮＮ）のアーキテクチャを持つＤＮＮを用いることができる。

検出部３１は、検出された道路上の構造物の種類、その構造物が表された領域を表す情報（例えば、その構造物が表された領域を示すビットマップ）、その構造物についての検出の信頼度及び劣化の程度を位置姿勢推定部３２へ出力する。さらに、検出部３１は、画像から検出された他の物体の種類及びその物体が表された領域を表す情報を、投影部３３及び運転計画部３４へ出力する。

位置姿勢推定部３２は、車両１０の位置及び姿勢を仮定して、画像から検出された道路上の構造物を地図上に投影するか、あるいは、地図情報に表された地図上の車両１０の周囲の道路上の構造物を画像上に投影する。そして位置姿勢推定部３２は、画像から検出された道路上の構造物と地図上に表された道路上の構造物との一致度合を算出する。その際、位置姿勢推定部３２は、画像から検出された道路上の構造物ごとに、その構造物についての検出の信頼度、その構造物の種類、及びその構造物の劣化の程度のうちの少なくとも一つに応じて重みを設定し、設定した重みを用いて一致度合を算出する。すなわち、設定された重みが大きい構造物ほど、算出された一致度合にその構造物に関する一致度合が反映される。そして位置姿勢推定部３２は、一致度合が最大となるときの仮定された位置及び姿勢を、車両１０の実際の位置及び姿勢として推定する。

例えば、位置姿勢推定部３２は、画像から検出された構造物についての検出の信頼度が高いほど、その構造物についての重み係数w_ｒを大きくする。検出の信頼度が高いほど、実際にその構造物が存在する可能性が高いので、検出された構造物と地図上の構造物との一致度合を正確に評価できる可能性が高いためである。位置姿勢推定部３２は、例えば、画像から検出された構造物についての検出の信頼度そのもの、あるいは、その信頼度を所定の値の範囲（例えば、0〜1）で正規化した値をその構造物に対する重み係数w_ｒとすればよい。

また、位置姿勢推定部３２は、画像から検出された構造物の種類に応じて、その構造物に重み係数w_tを設定する。なお、重み係数w_tは、道路上に明瞭に表される可能性が高い種類の構造物ほど大きな値となるように設定されることが好ましい。例えば、実線で表された車線区画線に対する重み係数w_t(例えば、1.0)は、点線で表された車線区画線に対する重み係数w_t(例えば、0.7)よりも大きいことが好ましい。道路上に明瞭に表される可能性が高い種類の構造物ほど、検出された構造物と地図上の構造物との一致度合を正確に評価できる可能性が高いためである。この場合、構造物の種類ごとの重み係数がメモリ２２に予め記憶される。そして位置姿勢推定部３２は、画像から検出された構造物の種類に対応する重み係数をメモリ２２から読み込めばよい。

さらに、位置姿勢推定部３２は、画像から検出された構造物が劣化しているほど、すなわち、劣化の程度が大きいほど、その構造物に対する重み係数w_dを小さくする。劣化の程度が大きい構造物ほど、画像上に表された構造物と、地図上に表された構造物との差異が大きく、一致度合の評価の精度が低下する可能性が高いためである。例えば、劣化の程度が識別器により複数の段階の何れかに推定されている場合（例えば、「高」「中」「低」の何れかに推定されている場合）、位置姿勢推定部３２は、劣化の程度が大きい段階ほど、重み係数w_dを小さくする。例えば、劣化の程度が「高」であると推定された構造物に対して、位置姿勢推定部３２は、重み係数w_dを0.6に設定する。また、劣化の程度が「中」であると推定された構造物に対して、位置姿勢推定部３２は、重み係数w_dを0.8に設定する。そして劣化の程度が「低」であると推定された構造物に対して、位置姿勢推定部３２は、重み係数w_dを1.0に設定する。

また、劣化の程度が、外形領域に含まれる画素の総数に対する、外形領域に含まれる、その種類以外のものが表された画素の数の比率で表される場合には、位置姿勢推定部３２は、その比率rに1を加えた数の逆数1/(r+1)を重み係数w_dとしてもよい。

位置姿勢推定部３２は、画像から検出された構造物ごとに、検出の信頼度に関する重み係数w_r、構造物の種類に関する重み係数w_t、及び、劣化の程度に関する重み係数w_dを互いに乗じて得られる重み係数w=w_rw_tw_dを算出し、算出した重み係数wを最終的に一致度合の算出に利用する。

図４は、画像から検出された道路上の構造物ごとの重み係数の一例を示す図である。画像４００において、実線で表された車線区画線４０１（構造物の種類に関する重み係数w_t=1.0）と、点線で表された車線区画線４０２及び４０３（構造物の種類に関する重み係数w_t=0.7）が検出されている。車線区画線４０１についての検出の信頼度が0.7であり、車線区画線４０２についての検出の信頼度が0.8であり、車線区画線４０３についての検出の信頼度が0.7であったとする。また、車線区画線４０１及び４０２について推定された劣化の程度が低であり、車線区画線４０３について推定された劣化の程度が中であったとする。この場合、車線区画線４０１について、重み係数wは、0.7(=w_rw_tw_d=0.7x1x1)に設定される。また、車線区画線４０２について、重み係数wは、0.56(=w_rw_tw_d=0.8x0.7x1)に設定される。さらに、車線区画線４０３について、重み係数wは、0.392(=w_rw_tw_d=0.7x0.7x0.8)に設定される。

変形例として、位置姿勢推定部３２は、画像から検出された構造物ごとに、検出の信頼度に関する重み係数w_r、構造物の種類に関する重み係数w_t、及び、劣化の程度に関する重み係数w_dの総和w=w_r+w_t+w_dを算出し、算出した重み係数wを最終的に一致度合の算出に利用してもよい。あるいはまた、位置姿勢推定部３２は、検出の信頼度に関する重み係数w_r、構造物の種類に関する重み係数w_t、及び、劣化の程度に関する重み係数w_dのうちの何れか一つを、最終的に一致度合の算出に利用する重み係数wとしてもよい。あるいはまた、位置姿勢推定部３２は、検出の信頼度に関する重み係数w_r、構造物の種類に関する重み係数w_t、及び、劣化の程度に関する重み係数w_dのうちの何れか二つの積または和を、最終的に一致度合の算出に利用する重み係数wとしてもよい。

位置姿勢推定部３２は、画像から検出された構造物ごとの重み係数を設定すると、車両１０の位置及び姿勢を仮定して、メモリ２２に記憶された地図情報に表された車両１０の周囲の道路上の構造物を画像上に投影する。例えば、位置姿勢推定部３２は、測位情報受信機３から受信した最新の測位情報で表される車両１０の位置、及び、直近の複数の測位情報から求められる車両１０の進行方向に相当する車両１０の姿勢を、車両１０の仮定位置及び仮定姿勢とする。位置姿勢推定部３２は、その仮定位置及び仮定姿勢に従って、世界座標系から、カメラ２の位置を原点とし、カメラ２の光軸方向を一つの軸方向とするカメラ座標系への変換式を求める。そのような変換式は、座標系間の回転を表す回転行列と座標系間の平行移動を表す並進ベクトルの組み合わせで表される。そして位置姿勢推定部３２は、その変換式に従って、地図情報に含まれる、世界座標系で表された車両１０の周囲の道路上の構造物の座標を、カメラ座標系の座標に変換する。そして位置姿勢推定部３２は、カメラ２の焦点距離といったカメラ２の内部パラメータに基づいて、カメラ座標系で表された車両１０の周囲の道路上の構造物を画像上に投影する。

位置姿勢推定部３２は、画像上に投影された、地図情報に表された車両１０の周囲の道路上の構造物と画像から検出された道路上の構造物との一致度合を、画像から検出された構造物ごとの重み係数を用いて算出する。例えば、位置姿勢推定部３２は、画像から検出された道路上の構造物の何れかが表された画素ごとに一致係数を算出する。その際、位置姿勢推定部３２は、画像から検出された道路上の構造物と投影された地図情報に表された車両１０の周囲の道路上の構造物とが一致するか否か判定し、一致する画素についてはその画素に表された構造物の重み係数の値を一致係数の値とし、一致しない画素の一致係数を0とする。そして位置姿勢推定部３２は、一致係数の総和を一致度合とする。これにより、重み係数が大きい構造物ほど、その構造物に関する一致度合が算出された一致度合に反映される。

位置姿勢推定部３２は、仮定位置及び仮定姿勢を所定量ずつ変化させながら、上記と同様の座標系変換、投影及び一致度合算出の各処理を実行することで、複数の仮定位置及び仮定姿勢のそれぞれについて、地図情報に表された車両１０の周囲の道路上の構造物と画像から検出された道路上の構造物との一致度合を算出する。そして位置姿勢推定部３２は、一致度合が最大となるときの仮定位置及び仮定姿勢を特定し、その仮定位置及び仮定姿勢を、車両１０の実際の位置及び姿勢として推定する。

変形例によれば、位置姿勢推定部３２は、画像に対して、カメラ２の内部パラメータ及び取り付け位置などの情報を用いて視点変換処理を実行することで、その画像をカメラ座標系で表される鳥瞰画像に変換する。そして位置姿勢推定部３２は、上記と同様に、車両１０の位置及び姿勢を仮定して、カメラ座標系から世界座標系への変換式を求め、その変換式に従って、鳥瞰画像上の各画素の位置をカメラ座標系から世界座標系の位置に変換することで、画像から検出された道路上の構造物を地図上に投影してもよい。この場合も、位置姿勢推定部３２は、上記と同様の処理を行って、地図情報に表された車両１０の周囲の道路上の構造物と画像から検出された道路上の構造物との一致度合を算出すればよい。そしてこの場合も、位置姿勢推定部３２は、仮定位置及び仮定姿勢を所定量ずつ変化させながら、上記と同様の鳥瞰画像から世界座標系への位置変換及び一致度合の算出の各処理を実行することで、複数の仮定位置及び仮定姿勢のそれぞれについて、地図情報に表された車両１０の周囲の道路上の構造物と画像から検出された道路上の構造物との一致度合を算出する。そしてこの場合も、位置姿勢推定部３２は、一致度合が最大となるときの仮定位置及び仮定姿勢を特定し、その仮定位置及び仮定姿勢を、車両１０の実際の位置及び姿勢として推定すればよい。

図５は、位置姿勢推定処理の概要説明図である。画像５００において、検出部３１により、道路上の構造物として車線区画線５０１〜５０３が検出されている。これに対して、車両１０の位置及び姿勢を仮定して地図５１０に表された車線区画線５１１〜５１３が画像５００に投影される。そして検出された車線区画線５０１〜５０３と、投影された車線区画線５１１〜５１３とが一致するように、車両１０の位置及び姿勢が探索される。その際、車線区画線５０１及び５０２について設定される重み係数が、車線区画線５０３について設定される重み係数よりも大きい場合、算出された一致度合において車線区画線５０１及び５０２と車線区画線５１１及び５１２間の一致度合の方が、車線区画線５０３と車線区画線５１３間の一致度合よりも反映される。そして、検出された車線区画線５０１〜５０３と、投影された車線区画線５１１〜５１３とが最も一致するときの車両１０の仮定位置及び仮定姿勢が車両１０の位置及び姿勢として推定される。

位置姿勢推定部３２は、推定された車両１０の位置及び姿勢を表す情報を投影部３３へ通知する。

投影部３３は、推定された車両１０の位置及び姿勢に従って、地図上に表された道路上の構造物を画像上に投影する。その際、投影部３３は、位置姿勢推定部３２による一致度合の算出の際に、画像上に対応する構造物が無い（すなわち、一致係数が0となる）、地図上に表された構造物（例えば、画像に表された車線区画線と同じ車線区画線のうち、画像に表されていない部分など）も画像上に投影してもよい。そして投影部３３は、地図上に表された道路上の構造物が投影された画像を運転計画部３４へわたす。

あるいは、投影部３３は、推定された車両１０の位置及び姿勢に従って、画像から検出された、車両１０の周囲に存在する他の物体を地図上に投影してもよい。そして投影部３３は、地図上に表された道路上の構造物及び地図上に投影された画像から検出された他の物体を表す情報を運転計画部３４へわたす。なお、投影部３３により実行される投影処理は、位置姿勢推定部３２に関して説明した処理と同様の処理であるため、この投影処理の詳細な説明は省略する。

運転計画部３４は、画像から検出された、車両１０の周囲に存在する他の物体と車両１０とが衝突しないように車両１０の走行予定経路を１以上生成する。走行予定経路は、例えば、現時刻から所定時間先までの各時刻における、車両１０の目標位置の集合として表される。運転計画部３４は、画像上に投影された、地図に表された道路上の構造物と、その画像から検出された他の物体との位置関係に応じて、車両１０と他の物体との相対的な位置関係を推定する。例えば、運転計画部３４は、画像上に投影された、地図に表された車線区画線と、他の物体との位置関係に応じて、他の物体が走行している車線を特定することで、他の物体と車両１０とが同じ車線を走行しているか否かを判定する。例えば、運転計画部３４は、他の物体の水平方向の中心位置を挟むように位置する互いに隣接する二つの車線区画線で特定される車線を他の物体が走行していると判定する。同様に、運転計画部３４は、車両１０を挟むように位置する互いに隣接する二つの車線区画線で特定される車線を車両１０が走行していると判定する。そして運転計画部３４は、車両１０が走行中の車線と他の物体が走行中の車線とが同一か否かを判定すればよい。また、地図に表された隣接する二つの車線区画線間の間隔は既知であり、かつ、カメラ２の焦点距離といった内部パラメータが既知であるため、画像上での隣接する二つの車線区画線間の間隔により、車両１０からの距離が推定できる。そこで、運転計画部３４は、画像上での他の物体の位置における、画像上に投影された、地図に表された隣接する二つの車線区画線間の間隔に基づいて、車両１０から他の物体までの距離を推定してもよい。このように、運転計画部３４は、画像上に正確に投影された、地図に表された道路上の構造物との位置関係で他の物体と車両１０との相対的な位置関係を推定する。そのため、運転計画部３４は、画像に写った車線区画線などの道路上の構造物が不明瞭でも、他の物体と車両１０との相対的な位置関係を正確に推定できる。

運転計画部３４は、検出された他の物体が走行中の車線及び相対距離に基づいて、他の物体と車両１０とが異なる車線を走行するか、あるいは、車両１０から他の物体までの相対距離が所定距離以上となるように、車両１０の走行予定経路を生成する。
なお、運転計画部３４は、複数の走行予定経路を生成してもよい。この場合、運転計画部３４は、複数の走行予定経路のうち、車両１０の加速度の絶対値の総和が最小となる経路を選択してもよい。

運転計画部３４は、生成した走行予定経路を車両制御部３５へ通知する。

車両制御部３５は、車両１０が通知された走行予定経路に沿って走行するように車両１０の各部を制御する。例えば、車両制御部３５は、通知された走行予定経路、及び、車速センサ（図示せず）により測定された車両１０の現在の車速に従って、車両１０の加速度を求め、その加速度となるようにアクセル開度またはブレーキ量を設定する。そして車両制御部３５は、設定されたアクセル開度に従って燃料噴射量を求め、その燃料噴射量に応じた制御信号を車両１０のエンジンの燃料噴射装置へ出力する。あるいは、車両制御部３５は、設定されたブレーキ量に応じた制御信号を車両１０のブレーキへ出力する。

さらに、車両制御部３５は、車両１０が走行予定経路に沿って走行するために車両１０の進路を変更する場合には、その走行予定経路に従って車両１０の操舵角を求め、その操舵角に応じた制御信号を、車両１０の操舵輪を制御するアクチュエータ（図示せず）へ出力する。

図６は、プロセッサ２３により実行される、位置姿勢推定処理を含む、車両制御処理の動作フローチャートである。プロセッサ２３は、カメラ２から画像を受信する度に、図６に示される動作フローチャートに従って車両制御処理を実行する。なお、以下に示される動作フローチャートにおいて、ステップＳ１０１〜Ｓ１０６の処理が位置姿勢推定処理に対応する。

プロセッサ２３の検出部３１は、カメラ２から得られた画像から道路上の構造物及び他の物体を検出する（ステップＳ１０１）。さらに、検出部３１は、検出された道路上の構造物の種類、劣化の程度及び検出の信頼度をもとめる（ステップＳ１０２）。

プロセッサ２３の位置姿勢推定部３２は、画像から検出された道路上の構造物について、その構造物の種類、劣化の程度、及び信頼度の少なくとも一つに基づいて重み係数を設定する（ステップＳ１０３）。そして位置姿勢推定部３２は、車両１０について仮定された複数の位置及び姿勢のそれぞれについて、その位置及び姿勢に従って地図上に表された車両１０の周囲の道路上の構造物を画像上に投影し、画像から検出された道路上の構造物との一致度合を各構造物について設定された重み係数を用いて算出する（ステップＳ１０４）。そして位置姿勢推定部３２は、仮定された複数の位置及び姿勢のうち、一致度合が最大となるときの位置及び姿勢を、車両１０の実際の位置及び姿勢として推定する（ステップＳ１０５）。

プロセッサ２３の投影部３３は、推定された車両１０の位置及び姿勢に従って、地図上に表された車両１０の周囲の道路上の構造物を画像上に投影する（ステップＳ１０６）。

プロセッサ２３の運転計画部３４は、画像から検出された他の物体と画像上に投影された地図上に表された道路上の構造物との位置関係に応じて、車両１０と他の物体との相対的な位置関係を推定し、その推定結果に応じて車両１０の走行予定経路を生成する（ステップＳ１０７）。そしてプロセッサ２３の車両制御部３５は、走行予定経路に沿って車両１０が走行するように車両１０を制御する（ステップＳ１０８）。そしてプロセッサ２３は、車両制御処理を終了する。

なお、上記のように、位置姿勢推定部３２は、ステップＳ１０４にて、車両１０について仮定された複数の位置及び姿勢のそれぞれについて、その姿勢に従って画像から検出された道路上の構造物を地図上に投影し、地図上に表された車両１０の周囲の道路上の構造物との一致度合を各構造物について設定された重み係数を用いて算出してもよい。また、投影部３３は、推定された車両１０の位置及び姿勢に従って、画像から検出された他の物体を地図上に投影してもよい。

以上に説明してきたように、この位置姿勢推定装置は、車両に搭載されたカメラにより生成された画像から検出された道路上の構造物と、地図上に表された道路上の構造物との一致度合に応じて、車両の位置及び姿勢を推定する。その際、この位置姿勢推定装置は、画像から検出された道路上の構造物の種類、劣化の程度及び検出の信頼度の少なくとも一つに基づいて一致度合を算出する際に利用される重み係数を設定する。そのため、この位置姿勢推定装置は、画像に路面の一部が明瞭に表されない場合でも、車両の位置及び姿勢の推定精度を向上できる。

変形例によれば、位置姿勢推定部３２は、車両１０に搭載された照明装置（例えば、車両１０のヘッドライト）が点灯している場合、その照明装置により照明される範囲に相当する画像上の照明領域内の道路上の構造物が表された各画素についての重み係数を、照明領域外の道路上の構造物が表された各画素についての重み係数よりも大きくしてもよい。この場合、位置姿勢推定部３２は、例えば、検出された道路上の構造物の種類、劣化の程度及び検出の信頼度の少なくとも一つに基づいて設定された重み係数に対して、さらに、照明領域内か否かで異なる係数を乗じればよい。その際、照明領域内の道路上の構造物に対して適用される係数（例えば、1.0）よりも、照明領域外の道路上の構造物に対して適用される係数（例えば、0.7）は小さな値に設定される。なお、プロセッサ２３は、車両１０のドライバが照明装置を点灯させる操作を行ったことを表す操作信号を通信インターフェース２１を介して受信することで照明装置が点灯したか否かを判定できる。あるいは、プロセッサ２３は、車両１０に設けられた照度センサ（図示せず）により測定された照度に基づいて照明装置を点灯させる制御信号を生成し、その制御信号を通信インターフェース２１を介して照明装置へ出力したときに、照明装置が点灯したと判定できる。さらに、照明装置による照明範囲は既知であるので、画像上の照明領域も既知となる。そこで、画像上の照明領域を表す情報は、メモリ２２に予め記憶されればよい。なお、ヘッドライトにおけるハイビームとロービームのように、照明装置が照明する方向が変化する場合には、メモリ２２には、照明方向ごとの照明領域を表す情報が記憶されていればよい。そして位置姿勢推定部３２は、プロセッサ２３が通信インターフェース２１を介して受信した、照明装置の照明方向を切り替える操作信号に応じて、現在の照明方向に対応する照明領域を表す情報をメモリ２２から読み込んで適用すればよい。

図７は、画像上の照明領域の一例を示す図である。画像７００において、照明装置からの照明光が届く範囲が照明領域７１０として設定される。したがって、照明領域７１０内の車線区画線７０１は、照明領域７１０外の車線区画線７０２よりも明瞭に見える。そのため、車線区画線７０１に対する重み係数は、車線区画線７０２に対する重み係数よりも大きく設定される。

また、上記の実施形態または変形例による、位置姿勢推定装置のプロセッサ２３の各部の機能を実現するコンピュータプログラムは、半導体メモリ、磁気記録媒体または光記録媒体といった、コンピュータ読取可能な可搬性の記録媒体に記録された形で提供されてもよい。

以上のように、当業者は、本発明の範囲内で、実施される形態に合わせて様々な変更を行うことができる。

１ 車両制御システム
２ カメラ
３ 測位情報受信機
４ 電子制御装置（位置姿勢推定装置）
５ 車内ネットワーク
２１ 通信インターフェース
２２ メモリ
２３ プロセッサ
３１ 検出部
３２ 位置姿勢推定部
３３ 投影部
３４ 運転計画部
３５ 車両制御部

Claims (1)

1. 車両に搭載された撮像部により得られた画像から、前記車両の周囲の道路上の第１の構造物を検出する検出部と、
   前記車両の複数の仮定位置及び仮定姿勢のそれぞれについて、当該仮定位置及び当該仮定姿勢に従って前記画像から検出された前記第１の構造物を地図上に投影するか、または、当該仮定位置及び当該仮定姿勢に従って前記地図上の前記車両の周囲の領域に表された道路上の第２の構造物を前記画像上に投影したときの、前記第１の構造物と前記第２の構造物との一致度合を、前記第１の構造物についての検出の確からしさを表す信頼度、当該第１の構造物の種類及び当該第１の構造物の劣化の程度の少なくとも一つに従って設定される前記第１の構造物の重みに応じて算出し、前記複数の仮定位置及び仮定姿勢のうち、当該一致度合が最大となるときの前記車両の仮定位置及び仮定姿勢を前記車両の位置及び姿勢として推定する位置姿勢推定部と、
   を有する姿勢推定装置。