特許 6369897 自己位置算出装置及び自己位置算出方法

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

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

(11)【特許番号】6369897

(24)【登録日】2018年7月20日

(45)【発行日】2018年8月8日

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

(51)【国際特許分類】

   G01B 11/00 20060101AFI20180730BHJP

   G01B 11/26 20060101ALI20180730BHJP

【ＦＩ】

   G01B11/00

   G01B11/26 H

【請求項の数】2

【全頁数】15

(21)【出願番号】特願2014-161646(P2014-161646)

(22)【出願日】2014年8月7日

(65)【公開番号】特開2016-38287(P2016-38287A)

(43)【公開日】2016年3月22日

【審査請求日】2017年8月1日

(73)【特許権者】

【識別番号】000003997

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

(73)【特許権者】

【識別番号】504157024

【氏名又は名称】国立大学法人東北大学

(74)【代理人】

【識別番号】100083806

【弁理士】

【氏名又は名称】三好 秀和

(74)【代理人】

【識別番号】100100712

【弁理士】

【氏名又は名称】岩▲崎▼ 幸邦

(74)【代理人】

【識別番号】100101247

【弁理士】

【氏名又は名称】高橋 俊一

(74)【代理人】

【識別番号】100095500

【弁理士】

【氏名又は名称】伊藤 正和

(74)【代理人】

【識別番号】100098327

【弁理士】

【氏名又は名称】高松 俊雄

(72)【発明者】

【氏名】山口 一郎

(72)【発明者】

【氏名】西内 秀和

(72)【発明者】

【氏名】伊藤 康一

(72)【発明者】

【氏名】酒井 修二

(72)【発明者】

【氏名】青木 孝文

【審査官】 三好 貴大

(56)【参考文献】

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

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

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

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

【文献】 再公表特許第２０１２／０８６０７０（ＪＰ，Ａ１）

【文献】 米国特許出願公開第２００３／００２１４４５（ＵＳ，Ａ１）

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

Ｇ０１Ｂ １１／００−１１／３０

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

【請求項1】

車両周囲の路面にパターン光を投光する投光器と、
前記パターン光の輝度が周期的に変化するように制御するパターン光制御部と、
前記車両に搭載され、前記パターン光が投光された領域を含む車両周囲の路面を撮像して画像を取得する撮像部と、
前記撮像部で取得された画像から周期的に変化するパターン光を抽出するパターン光抽出部と、
前記パターン光抽出部にて抽出されたパターン光の位置から、前記路面に対する車両の姿勢角を算出する姿勢角算出部と、
前記撮像部により取得された画像から路面上の複数の特徴点を検出し、各特徴点の時間変化に基づいて、前記車両の姿勢変化量を算出する姿勢変化量算出部と、
前記車両の初期位置及び姿勢角に、前記姿勢変化量を加算してゆくことで、前記車両の現在位置及び姿勢角を算出する自己位置算出部と、
前記車両の車速を検出する車速検出部と、を備え、
前記自己位置算出部は、前記車速が所定の閾値以下の場合、その時の前記車両の現在位置及び前記姿勢角算出部による車両の姿勢角を、前記車両の初期位置及び姿勢角に設定し、前記姿勢変化量の加算を開始することを特徴とする自己位置算出装置。

【請求項2】

車両に搭載された投光器から車両周囲の路面にパターン光を投光する投光手順と、
前記車両の制御部が、前記パターン光の輝度が周期的に変化するように制御するパターン光制御手順と、
前記車両に搭載された撮像部によって、前記パターン光が投光された領域を含む車両周囲の路面を撮像して画像を取得する撮像手順と、
前記制御部が、前記撮像手順で取得された画像から周期的に変化するパターン光を抽出するパターン光抽出手順と、
前記制御部が、前記パターン光抽出手順にて抽出されたパターン光の位置から、前記路面に対する車両の姿勢角を算出する姿勢角算出手順と、
前記制御部が、前記撮像手順により取得された画像から路面上の複数の特徴点を検出し、各特徴点の時間変化に基づいて、前記車両の姿勢変化量を算出する姿勢変化量算出手順と、
前記制御部が、前記車両の初期位置及び姿勢角に、前記姿勢変化量を加算してゆくことで、前記車両の現在位置及び姿勢角を算出する自己位置算出手順と、
前記制御部が、前記車両の車速を検出する車速検出手順とを含み、
前記自己位置算出手順では、前記車速が所定の閾値以下の場合、その時の前記車両の現在位置及び前記姿勢角算出手順による車両の姿勢角を、前記車両の初期位置及び姿勢角に設定し、前記姿勢変化量の加算を開始することを特徴とする自己位置算出方法。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、自己位置算出装置及び自己位置算出方法に係り、特に、移動量を推定する精度を向上させる技術に関する。

【背景技術】

【0002】

従来、車両に搭載されたカメラにより撮像された車両近傍の画像を取得し、画像の変化に基づいて車両の移動量を求める技術が知られている（特許文献１参照）。特許文献１では、車両の低速かつ微妙な移動においても精度よく移動量を求めるために、画像の中から特徴点を検出し、画像上の特徴点の位置を求め、特徴点の移動方向及び移動距離（移動量）から車両の移動量を求めている。

【0003】

また、格子パターン（パターン光）を投光するレーザ投光器を用いて三次元計測を行う技術が知られている（特許文献２参照）。特許文献２では、パターン光の投光領域をカメラで撮像し、撮像した画像からパターン光を抽出して、パターン光の位置から路面に対する車両の距離や姿勢角を求めている。

【0004】

この２つの技術を用いれば、路面に対する距離と姿勢角をパターン光から算出し、その路面の移動量を計測することで車両の移動量を高精度に算出することができる。

【先行技術文献】

【特許文献】

【0005】

【特許文献1】特開２００８−１７５７１７号公報

【特許文献2】特開２００７−２７８９５１号公報

【発明の概要】

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

【0006】

しかしながら、特許文献２に記載されたパターン光は、路面の照度が高くなる晴天下の日中では、太陽光の影響によって、撮像した画像から安定して抽出することが難しい。そのため、車両の路面に対する距離と姿勢角を求めにくいという問題点があった。

【0007】

本発明は、上記課題に鑑みて成されたものであり、その目的は、路面の照度が高い場合でも、車両の移動量を安定的、且つ高精度に推定することのできる自己位置算出装置及び自己位置算出方法を提供することにある。

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

【0008】

上記目的を達成するために、本発明の一態様に係る自己位置算出装置及びその方法は、車両周囲の路面にパターン光を投光し、パターン光の輝度が周期的に変化するように制御して、パターン光が投光された領域を含む車両周囲の路面を撮像して画像を取得する。そして、取得した画像から周期的に変化するパターン光を抽出し、抽出したパターン光の位置から路面に対する車両の姿勢角を算出する。また、取得した画像から路面上の複数の特徴点を検出し、各特徴点の時間変化に基づいて車両の姿勢変化量を算出する。その結果、自己位置算出装置及びその方法は、車両の初期位置及び姿勢角に姿勢変化量を加算してゆくことで、車両の現在位置及び姿勢角を算出する。ここで、自己位置算出装置及びその方法は、車両の車速を検出し、車速が所定の閾値以下の場合、その時の車両の現在位置及び姿勢角を、車両の初期位置及び姿勢角に設定して姿勢変化量の加算を開始する。

【発明の効果】

【0009】

本発明によれば、路面の照度が高い場合でも、車両の移動量を安定的、且つ高精度に算出することができる。

【図面の簡単な説明】

【0010】

【図1】本発明の一実施形態に係る自己位置算出装置の構成を示すブロック図である。

【図2】車両に搭載された投光器によって照射される投光パターン、及びカメラによる撮像範囲を示す説明図である。

【図3】（ａ）は、投光器とカメラの間の基線長Ｌｂと、各スポット光の画像上の座標（Ｕｊ，Ｖｊ）とから、各スポット光が照射された路面上の位置を算出する様子を示す図であり、（ｂ）は、パターン光が照射された領域とは異なる他の領域から検出された特徴点の時間変化から、カメラの移動方向を求める様子を示す説明図である。

【図4】（ａ）は、カメラにより取得した画像に対して二値化処理を施したパターン光の画像を示す図、（ｂ）はその拡大図、（ｃ）はパターン光抽出部により抽出した各スポット光の重心の位置を示す図である。

【図5】距離及び姿勢角の変化量を算出する方法を説明するための模式図である。

【図6】（ａ）は、時刻ｔに取得した画像の一例を示し、（ｂ）は、時刻ｔから時間Δｔだけ経過した時刻（ｔ＋Δｔ）に取得した画像の一例を示す。

【図7】本発明の一実施形態に係る自己位置算出装置による自己位置算出処理の処理手順を示すフローチャートである。

【図8】本発明の一実施形態に係る自己位置算出装置による輝度変化を示すタイミングチャートである。

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

【0011】

以下、本発明の一実施形態について図面を参照して説明する。

【0012】

［自己位置算出装置の構成］
図１は、本発明の一実施形態に係る自己位置算出装置１００の構成を示すブロック図である。図１に示すように、自己位置算出装置１００は、投光器１１と、カメラ１２（撮像部）と、エンジンコントロールユニット（ＥＣＵ）１３と、車速センサ１４とを備える。投光器１１は、車両に搭載され、車両周囲の路面に周期的に輝度が変化するパターン光を投光する。パターン光の詳細については後述する。カメラ１２は、車両に搭載され、パターン光が投光された領域を含む車両周囲の路面を撮像して画像を取得する。ＥＣＵ１３は、投光器１１の作動を制御し、且つカメラ１２により取得された画像から車両の移動量を推定する一連の情報処理サイクルを実行する制御部の一例である。

【0013】

投光器１１は、図２に示すように、カメラ１２の撮像範囲内の路面３１に向けて、正方形や長方形の格子像を含む所定の形状を有するパターン光３２ｂを投光する。この際、パターン光制御部２７により、パターン光３２ｂの輝度が、周期的に変化するように制御される。

【0014】

カメラ１２は、固体撮像素子、例えばＣＣＤ及びＣＭＯＳを用いたデジタルカメラであって、画像処理が可能なデジタル画像を取得する。カメラ１２の撮像対象は車両周囲の路面であって、車両周囲の路面には、車両の前部、後部、側部、車両底部の路面が含まれる。例えば、図２に示すように、カメラ１２は、車両１０の前部、具体的にはフロントバンパ上に搭載することができる。

【0015】

車両１０の前方の路面３１上の特徴点（「テクスチャ」ともいう）及び投光器１１により投光されたパターン光３２ｂを撮像できるように、カメラ１２が設置される高さ及び向きが調整され、且つ、カメラ１２が備えるレンズの焦点及び絞りが自動調整される。ここで、「特徴点」とは、アスファルト上に存在する凹凸部分である。

【0016】

カメラ１２は、所定の時間間隔をおいて繰り返し撮像を行い、一連の画像（フレーム）群を取得する。例えば、パターン光３２ｂの輝度を変化させる周期を５０［Ｈｚ］とした場合、１秒間に６００回の撮像を行う。これにより、パターン光３２ｂの輝度が最も高い時点、及び最も低い時点の画像を周期的に撮像することができる。カメラ１２により取得された画像データは、撮像周期毎にＥＣＵ１３へ転送され、ＥＣＵ１３が備えるメモリ（図示省略）に記憶される。

【0017】

カメラ１２は、路面３１に照射されたパターン光を撮像する。投光器１１は、例えば、レーザポインター及び回折格子を備える。レーザポインターから射出されたレーザ光を回折格子で回折することにより、投光器１１は、図２〜図４に示すように、格子像、或いは行列状に配列された複数のスポット光からなるパターン光（３２ｂ、３２ａ）を生成する。図３及び図４に示す例では、５×７のスポット光からなるパターン光３２ａを生成している。

【0018】

図１に戻り、ＥＣＵ１３は、ＣＰＵ、メモリ、及び入出力部を備えるマイクロコントローラからなり、予めインストールされたコンピュータプログラムを実行することにより、自己位置算出装置１００が備える複数の情報処理部を構成する。ＥＣＵ１３は、カメラ１２により取得された画像から車両の移動量を推定する一連の情報処理サイクルを、画像（フレーム）毎に繰り返し実行する。ＥＣＵ１３は、車両１０に関連する他の制御に用いるＥＣＵと兼用しても良い。

【0019】

車速センサ１４は、車輪、或いはディファレンシャルギア等に装着されたロータリーエンコーダ等の出力から車両１０の現在の車速を算出する。

【0020】

複数の情報処理部には、パターン光抽出部２１と、姿勢角算出部２２と、特徴点検出部２３と、姿勢変化量算出部２４と、自己位置算出部２６と、パターン光制御部２７と、車速検出部２８とが含まれる。姿勢変化量算出部２４には、特徴点検出部２３が含まれる。

【0021】

パターン光制御部２７は、投光器１１によるパターン光３２ａの投光を制御する。例えば、車両１０のイグニションスイッチがオン状態となり、自己位置算出装置１００が起動すると同時に、パターン光制御部２７は、周期的に輝度が変化するパターン光３２ａの投光を開始する。その後、パターン光制御部２７は、自己位置算出装置が停止するまで、パターン光３２ａを連続して投光する。或いは、必要に応じてパターン光３２ａを投光してもよい。本実施形態では、輝度変化の一例として、投光パターンの輝度が所定周波数の正弦波状に変化するように、投光器１１に供給する電力を制御する。例えば、後述の図８に示すように正弦波状に輝度が変化するように制御する。

【0022】

パターン光抽出部２１は、カメラ１２により取得された画像をメモリから読み込み、更に、前述した所定周波数にて同期検波処理を実施することにより、画像に含まれるパターン光３２ａを抽出する。同期検波処理については後述する。そして、取得したパターン光３２ａから、該パターン光３２ａの位置を抽出する。図３（ａ）に示すように、例えば、投光器１１が、行列状に配列された複数のスポット光からなるパターン光３２ａを路面３１に向けて投光し、路面３１で反射したパターン光３２ａを同期検波処理、或いは差分演算処理により検出する。

【0023】

更に、抽出したパターン光の画像に対して二値化処理を施すことにより、図４（ａ）及びその拡大図である図４（ｂ）に示すように、スポット光Ｓｐの画像のみを抽出する。パターン光抽出部２１は、図４（ｃ）に示すように、各スポット光Ｓｐの重心の位置Ｈｅ、即ち、スポット光Ｓｐの画像上の座標（Ｕｊ、Ｖｊ）を算出することにより、パターン光３２ａの位置を抽出する。座標は、カメラ１２の撮像素子の画素を単位とし、５×７のスポット光Ｓｐの場合、“ｊ”は１以上３５以下の整数である。スポット光Ｓｐの画像上の座標（Ｕｊ、Ｖｊ）は、パターン光３２ａの位置を示すデータとしてメモリに記憶される。

【0024】

次に、同期検波処理について説明する。カメラ１２で撮像した画像に含まれる測定信号をsin（ω０＋α）とすると、この測定信号には投光パターン以外に様々な周波数成分の太陽光や人工光が含まれている。そこで、周波数が変調周波数ωｒとなる参照信号sin（ωｒ＋β）を測定信号sin（ω０＋α）に乗じると、その結果は、cos（ω０−ωｒ＋α−β）／２−cos（ω０＋ωｒ＋α＋β）／２となる。

【0025】

この測定信号をローパスフィルタに通すと、ω０≠ωｒの信号、即ち、周波数がωｒではない投光パターン以外の太陽光や人工光は除去される。その一方で、ω０＝ωｒの信号、即ち、周波数がωｒの投光パターンは、cos（α−β）／２となって抽出することができる。このように、同期検波処理を用いることにより、様々な周波数成分を含む測定信号の中から、投光パターンのみを抽出した画像を得ることができる。

【0026】

即ち、本実施形態では、パターン光制御部２７は、予め設定した所定の変調周波数ωｒにてパターン光３２ａを輝度変調する。従って、路面には周波数ωｒで輝度変調された投光パターンが投光される。そして、パターン光抽出部２１は、カメラ１２で撮像される画像（測定信号）に変調周波数ωｒを乗じることにより、投光パターンのみを抽出することができる。

【0027】

図１に示す姿勢角算出部２２は、パターン光３２ａの位置を示すデータをメモリから読み込み、カメラ１２により取得された画像におけるパターン光３２ａの位置から、路面３１に対する車両１０の距離及び姿勢角を算出する。例えば、図３（ａ）に示すように、投光器１１とカメラ１２の間の基線長Ｌｂと、各スポット光の画像上の座標（Ｕｊ、Ｖｊ）とから、三角測量の原理を用いて、各スポット光が照射された路面３１上の位置を、カメラ１２に対する相対位置として算出する。そして、姿勢角算出部２２は、カメラ１２に対する各スポット光の相対位置から、パターン光３２ａが投光された路面３１の平面式、即ち、路面３１に対するカメラ１２の距離及び姿勢角（法線ベクトル）を算出する。なお、車両１０に対するカメラ１２の搭載位置及び撮像方向は既知であるので、実施形態においては、路面３１に対する車両１０の距離及び姿勢角の一例として、路面３１に対するカメラ１２の距離及び姿勢角を算出する。以後、路面３１に対するカメラ１２の距離及び姿勢角を、「距離及び姿勢角」と略す。姿勢角算出部２２により算出された距離及び姿勢角は、メモリに記憶される。

【0028】

具体的には、カメラ１２及び投光器１１は車両１０にそれぞれ固定されているので、パターン光３２ａの照射方向と、カメラ１２と投光器１１との距離（図３の基線長Ｌｂ）は既知である。そこで、姿勢角算出部２２は、三角測量の原理を用いて、各スポット光の画像上の座標（Ｕｊ、Ｖｊ）から各スポット光が照射された路面３１上の位置を、カメラ１２に対する相対位置（Ｘｊ，Ｙｊ，Ｚｊ）として求めることができる。

【0029】

なお、カメラ１２に対する各スポット光の相対位置（Ｘｊ，Ｙｊ，Ｚｊ）は同一平面上に存在しない場合が多い。なぜなら、路面３１に表出するアスファルトの凹凸に応じて各スポット光の相対位置が変化するからである。そこで、最小二乗法を用いて、各スポット光との距離誤差の二乗和が最小となるような平面式を求めても良い。

【0030】

特徴点検出部２３は、カメラ１２により取得された画像をメモリから読み込み、メモリから読み込んだ画像から、路面３１上の特徴点を検出する。詳細には、所定周波数で正弦波状に輝度が変化するパターン光の輝度が、予め設定した閾値輝度以下となった時間帯に特徴点検出フラグを「１」に設定し、この時間帯において特徴点を検出する。

【0031】

更に、特徴点検出部２３は、路面３１上の特徴点を検出するために、例えば、「D.G. Lowe，“Distinctive Image Features from Scale-Invariant Keypoints,” Int. J. Comput. Vis., vol.60, no.2, pp. 91-110, Nov. 200 」、或いは、「金澤 靖, 金谷健一, “コンピュータビジョンのための画像の特徴点抽出,” 信学誌, vol.87, no.12, pp.1043-1048, Dec. 2004」に記載の手法を用いることができる。

【0032】

具体的には、特徴点検出部２３は、例えば、ハリス（Harris）作用素又はSUSANオペレータを用いて、物体の頂点のように周囲に比べて輝度値が大きく変化する点を特徴点として検出する。或いは、特徴点検出部２３は、SIFT（Scale-Invariant Feature Transform）特徴量を用いて、その周囲で輝度値がある規則性のもとで変化している点を特徴点として検出しても良い。そして、特徴点検出部２３は、１つの画像から検出した特徴点の総数Ｎを計数し、各特徴点に識別番号（ｉ（１≦ｉ≦Ｎ））を付す。各特徴点の画像上の位置（Ｕｉ、Ｖｉ）は、ＥＣＵ１３内のメモリに記憶される。図６（ａ）及び図６（ｂ）は、カメラ１２により取得された画像から検出された特徴点Ｔｅの例を示す。更に、各特徴点Ｔｅの変化方向及び変化量をベクトルＤｔｅとして示している。

【0033】

なお、実施形態において、路面３１上の特徴点は、主に大きさが１ｃｍ以上２ｃｍ以下のアスファルト混合物の粒を想定している。この特徴点を検出するために、カメラ１２の解像度はＶＧＡ（約３０万画素）である。また、路面３１に対するカメラ１２の距離は、おおよそ７０ｃｍである。更に、カメラ１２の撮像方向は、水平面から約４５degだけ路面３１に向けて傾斜させる。また、カメラ１２により取得される画像をＥＣＵ１３に転送するときの輝度数値は、０〜２５５（０：最も暗い、２５５：最も明るい）の範囲内である。

【0034】

姿勢変化量算出部２４は、一定の情報処理サイクル毎に撮像される各フレームの画像のうち、前回（時刻ｔ）フレームの画像に含まれる複数の特徴点の画像上の位置座標（Ｕｉ，Ｖｉ）をメモリから読み込む。更に、今回（時刻ｔ＋Δｔ）フレームの画像に含まれる複数の特徴点の画像上の位置座標（Ｕｉ，Ｖｉ）をメモリから読み込む。そして、複数の特徴点の画像上での位置変化に基づいて、車両１０の姿勢変化量を求める。ここで、「車両の姿勢変化量」とは、路面に対する車両１０の「距離及び姿勢角の変化量」、及び路面上での「車両の移動量」の双方を含んでいる。以下、「距離及び姿勢角の変化量」及び「車両の移動量」の算出方法について説明する。

【0035】

距離及び姿勢角の変化量は、例えば、以下のようにして求めることができる。図６（ａ）は、時刻ｔに取得された第１フレームの画像３８の一例を示す。図５及び図６（ａ）に示すように、画像３８において、例えば３つの特徴点Ｔｅ１、Ｔｅ２、Ｔｅ３の相対位置（ｘｉ，ｙｉ，ｚｉ）がそれぞれ算出されている場合を考える。この場合、特徴点Ｔｅ１、Ｔｅ２、Ｔｅ３によって特定される平面Ｇ（図６（ａ）参照）を路面と見なすことができる。よって、姿勢変化量算出部２４は、各特徴点の相対位置（ｘｉ，ｙｉ，ｚｉ）から、路面（平面Ｇ）に対するカメラ１２の距離及び姿勢角（法線ベクトル）を求めることができる。更に、姿勢変化量算出部２４は、既知のカメラモデルによって、各特徴点Ｔｅ１、Ｔｅ２、Ｔｅ３の間の距離（ｌ１，ｌ２，ｌ３）及び夫々の特徴点Ｔｅ１、Ｔｅ２、Ｔｅ３を結ぶ直線が成す角度を求めることができる。図５のカメラ１２は、第１フレーム（時刻ｔ）におけるカメラの位置を示す。

【0036】

なお、カメラ１２に対する特徴点の相対位置を示す３次元座標（ｘｉ，ｙｉ，ｚｉ）として、カメラ１２の撮像方向をＺ軸に設定し、撮像方向を法線とし且つカメラ１２を含む平面内に、互いに直交するＸ軸及びＹ軸を設定する。一方、画像３８上の座標として、水平方向及び垂直方向をそれぞれＶ軸及びＵ軸に設定する。

【0037】

図６（ｂ）は、時刻ｔから時間Δｔだけ経過した時刻（ｔ＋Δｔ）に取得された第２フレームの画像３８’を示す。図５のカメラ１２’は、第２フレームの画像３８’を撮像したときのカメラ１２の位置を示す。図５及び図６（ｂ）に示すように、画像３８’において、カメラ１２’が特徴点Ｔｅ１、Ｔｅ２、Ｔｅ３を撮像し、特徴点検出部２３が特徴点Ｔｅ１、Ｔｅ２、Ｔｅ３を検出する。この場合、姿勢変化量算出部２４は、時刻ｔにおける各特徴点Ｔｅ１、Ｔｅ２、Ｔｅ３の相対位置（ｘｉ，ｙｉ，ｚｉ）と、各特徴点の画像３８’上の位置Ｐ１（Ｕｉ，Ｖｉ）と、カメラ１２のカメラモデルとから、時間Δｔにおけるカメラ１２の移動量ΔＬ（図５参照）を算出できる。ひいては車両１０の移動量を算出できる。更には、距離及び姿勢角の変化量も算出することができる。例えば、以下の式（１）からなる連立方程式をＲ、Ｔについて解くことにより、姿勢変化量算出部２４は、カメラ１２（車両）の移動量（ΔＬ）、及び、距離及び姿勢角の変化量を算出することができる。ここで、Ｒはカメラの姿勢角変化量を表す３次元回転行列であり、Ｔはカメラの移動方向を表す３次元並進ベクトルである。なお、下記の（１）式はカメラ１２が歪みや光軸ずれのない理想的なピンホールカメラとしてモデル化したものであり、λｉは定数、ｆは焦点距離である。カメラモデルのパラメータは、予めキャリブレーションをしておけばよい。

【0038】

【数1】

【0039】

なお、姿勢変化量算出部２４は、時刻ｔと時刻ｔ＋Δｔで検出された特徴点の全てを用いるのではなく、特徴点同士の位置関係に基づいて最適な特徴点を選定してもよい。選定方法としては、例えば、エピポーラ幾何（エピ極線幾何，R.I. Hartley: “A linear method for reconstruction from lines and points,” Proc. 5th International Conference on Computer Vision, Cambridge, Massachusetts, pp.882-887(1995)）を用いることができる。

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

そこで、本実施形態に係る自己位置算出装置１００では、最も日照量が多い夏至（６月）に近い晴天下においてもパターン光３２ａを検出できるように、パターン光３２ａの最大輝度（図８に示す輝度波形が最大となるピーク）を設定している。また、パターン光の輝度変調の周波数を５０［Ｈｚ］とし、カメラ１２のフレームレート（１秒間の撮影数）を６００［ｆｐｓ］としている。

【0065】

さらに、本実施形態に係る自己位置算出装置１００では、車速が所定の閾値以下の時にパターン光の輝度を周期的に変化させ、同期検波技術を用いてパターン光を抽出し、基点となる車両の初期位置及び姿勢角を設定する。そして、この基点に対して車速が閾値より大きい場合には、各特徴点の時間変化に基づいて算出した車両の姿勢変化量を加算する。これにより、路面の照度が高くなる晴天下の日中のような場合でも、積分誤差の影響を低減して車両の移動量を安定的、且つ高精度に算出することができる。

【0066】

また、本実施形態では、所定の変調周波数で輝度変調したパターン光を照射し、更に、撮像した画像を同期検波処理することにより、パターン光３２ａを抽出する例について説明した。しかし、同期画像を所定回数積算した画像を生成し、この積算した画像からパターン光３２ａを抽出するようにしてもよい。つまり、１周期の同期検波処理により得られる同期画像では、パターン光３２ａが投光された部分が十分強調されずにノイズが多く含まれてしまい、２値化処理を高精度に実施することができなくなる場合がある。そこで、２周期以上で取得した同期画像を積分し、パターン光３２ａが投光された部分を、より一層強調した同期検波画像を作成することにより、ノイズの影響をより排除して画像上の投光パターンを検出することができる。

【0067】

ただし、その場合、車速の閾値Ｖｔｈはより小さい値に設定する必要がある。具体的に２周期とする場合には、５．０÷２＝２．５［ｍ／ｓ］のように、同期検波でパターン光を抽出する間の移動量が、０．１［ｍ］を超えないように設定する必要がある。

【0068】

以上、本発明の自己位置算出装置、及び自己位置算出方法を図示の実施形態に基づいて説明したが、本発明はこれに限定されるものではなく、各部の構成は、同様の機能を有する任意の構成のものに置き換えることができる。

【符号の説明】

【0069】

１０ 車両
１１ 投光器
１２ カメラ（撮像部）
１３ ＥＣＵ
１４ 車速センサ
２１ パターン光抽出部
２２ 姿勢角算出部
２３ 特徴点検出部
２４ 姿勢変化量算出部
２６ 自己位置算出部
２７ パターン光制御部
２８ 車速検出部
３２ａ，３２ｂ パターン光
１００ 自己位置算出装置

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】