JP6459745B2 - 自己位置算出装置及び自己位置算出方法 - Google Patents

Description

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

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

また、車両姿勢の変化量を算出する技術として、車両から路面に投光したパターン光をカメラで撮像し、その画像上の形状から三角測量の原理を用いてカメラに対する路面の平面式を算出して路面に対する車両の距離や姿勢を算出する技術がある（特許文献２参照）。また、屋外、特に晴天下では、太陽光の影響が強く、パターン光をカメラ画像から抽出することが困難な場合がある。そこで、このような場合のために、従来では同期検波技術を用いてパターン光を抽出していた。

特開２００８−１７５７１７号公報 特開２００７−２７８９５１号公報

上述した同期検波の技術を用いる場合、カメラの絞りを開いて撮影すると、画像が明るくなってパターン光の抽出は容易になるが、被写体深度が浅くなるため、画像全体にピントを合わせることが難しくなり、狭い範囲でしか路面の特徴を検出できなくなる。

逆に、カメラの絞りを絞ると、被写体深度が深くなるため、広い範囲で路面の特徴を検出できるようになるが、画像が暗くなることから同期検波によるパターン光の抽出が困難になるという問題が生じる。

したがって、同期検波によるパターン光の抽出と特徴点の検出との間にはトレードオフの関係があり、両立させることは困難であるという問題点があった。

そこで、本発明は上述した実情に鑑みて提案されたものであり、トレードオフの関係にある同期検波によるパターン光の抽出と特徴点の検出とを両立させて高精度且つロバストに自己位置を算出できる自己位置算出装置及びその方法を提供することを目的とする。

上述した課題を解決するために、本発明の一態様に係る自己位置算出装置及びその方法は、投光器から車両周囲の路面にパターン光を投光し、パターン光の輝度が周期的に変化するように制御する。そして、撮像部によって、パターン光が投光された領域を含む車両周囲の路面を撮像して画像を取得する。このときパターン光が投光されているときには、パターン光が投光されていないときより撮像部の絞りを開いて撮像する。この後、撮像部で取得された画像からパターン光を抽出してパターン光の位置から路面に対する車両の姿勢角を算出し、撮像部により取得された画像から路面上の複数の特徴点を検出して各特徴点の時間変化に基づいて車両の姿勢変化量を算出する。これらの結果を用いて、本発明の一態様に係る自己位置算出装置及びその方法は、車両の初期位置及び姿勢角に、姿勢変化量を加算してゆくことで、車両の現在位置及び姿勢角を算出する。

本発明によれば、撮像部の設定を調整することにより、トレードオフの関係にある同期検波によるパターン光の抽出と特徴点の検出とを両立させて高精度且つロバストに自己位置を算出することができる。

図１は、本発明の一実施形態に係る自己位置算出装置の構成を示すブロック図である。 図２は、投光器及びカメラの車両への搭載例を示す外観図である。 図３は、パターン光を照射する領域と特徴点を検出する領域を説明するための図である。 図４は、カメラにより取得された画像に対して二値化処理を施したパターン光の画像を示す図である。 図５は、カメラの絞りを調整して撮像された画像の一例を示す図である。 図６は、カメラの画像取得領域を調整して撮像された画像の一例を示す図である。 図７は、距離及び姿勢角の変化量を算出する方法を説明するための模式図である。 図８は、時刻ｔと時刻ｔ＋Δｔに取得されたフレーム（画像）の一例を示す図である。 図９は、本発明の一実施形態に係る自己位置算出装置による自己位置算出方法の処理手順を示すフローチャートである。 図１０は、本発明の一実施形態に係る自己位置算出装置によるカメラを調整するタイミングを説明するためのタイミングチャートである。

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

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

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

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

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

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

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

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

複数の情報処理部には、絞り調整部１８と、フレームレート調整部１９と、画像取得領域設定部２０と、パターン光抽出部２１と、姿勢角算出部２２と、特徴点検出部２３と、姿勢変化量算出部２４と、自己位置算出部２６と、パターン光制御部２７が含まれる。姿勢変化量算出部２４には、特徴点検出部２３が含まれる。

絞り調整部１８は、カメラ１２の絞りを調整し、パターン光が投光されているときにはパターン光が投光されていないときより絞りを開くように調整する。例えば、パターン光が投光されているときには絞り値をＦ２に設定し、パターン光が投光されていないときには絞り値をＦ５．６に設定する。Ｆ値を大きくすることで、カメラの絞りを開くように調整する。

絞りは、レンズを通って撮像素子上に写る像の明るさのことで、絞りを絞るとレンズを通る光が少なくなって、撮像素子上に写る像が暗くなり、ピントが合って見える距離の範囲が広くなる（被写体深度が深い）。逆に、絞りを開くとレンズを通る光が多くなり、撮像素子上に写る像が明るくなるが、ピントが合って見える距離の範囲が狭くなる（被写体深度が浅い）。

本実施形態では、カメラ１２は、図３（ａ）に示すようにほぼ平坦な路面を至近から撮像している。そのため、例えば画像中心でピントを合わせている場合に、絞りを開いて被写体深度が浅くなると、図５（ａ）に示すように画像は明るくなるが、ピントが合う領域が狭くなって画像の外縁部ではピントが合わなくなる。一方、絞りを絞って被写体深度が深くなると、図５（ｂ）に示すように画像は暗くなるものの広い領域でピントが合うようになって画像全体でピントを合わせることができる。

ここで、特徴点を検出、トラッキングする場合には、特徴点がカメラ１２の画像上の何処に現れるか予測できないので、できるだけ広い範囲でピントを合わせておく必要がある。すなわち、絞りを絞る必要がある。しかし、絞りを絞ると、図５（ｂ）に示すように画像は暗くなり、パターン光の微弱な変化を抽出することは困難になる。

これに対して、同期検波によってパターン光を抽出する場合には、パターン光の微弱な変化を検出するために、絞りを開いて明るい画像にしたほうが良い。このとき、パターン光が投光される範囲は、投光器１１の車両への搭載位置、姿勢から決定できるので、パターン光が投光されている限定された狭い領域だけにピントを合わせればよい。例えば、図５（ａ）に示すように画像の中心近傍にパターン光を投光するように設計しておき、画像中心にカメラ１２のピントを合わせればよい。

したがって、絞り調整部１８は、パターン光が投光されているときには、パターン光が投光されていないときよりもカメラ１２の絞りを開くように調整する。あるいは、パターン光が投光されているときには、パターン光が投光されている領域だけにピントが合うように絞りを開く。これにより、パターン光を抽出するときに画像が明るくなって、パターン光の微弱な変化まで抽出することが可能となる。

フレームレート調整部１９は、カメラ１２のフレームレートを調整し、パターン光が投光されていないときには、パターン光が投光されて同期検波を行っているときよりもカメラ１２のフレームレートを低くする。例えば、車速が７２ｋｍ／ｈの場合では、パターン光が投光されていないときにフレームレートを２００ｆｐｓに設定し、パターン光が投光されているときにはフレームレートを２４００ｆｐｓに設定する。

フレームレート調整部１９は、パターン光が投光されて同期検波を行っているときには数百〜数千ｆｐｓという高いフレームレートに設定している。しかし、特徴点を検出するときにこのような高いフレームレートで撮像すると、画面上で特徴点の移動量が１画素以下、すなわちカメラ１２の画像上で特徴点が動かないことになる。これでは移動量が算出できないので、積分して推定した車両の現在位置や姿勢角の誤差要因となる。したがって、特徴点を検出するときには、同期検波を行うときと比べてフレームレートを低くする必要がある。

また、フレームレートを低くすると、露光時間を長くとって（シャッタースピードを遅くして）明るい画像を得ることができる。特に、同期検波でパターン光を抽出するときには、高いフレームレートで撮影を行っており、これと比較すると特徴点を検出するときのフレームレートは十分に低くてよい。

さらに、特徴点を検出するときはカメラ１２の絞りを絞るので、画像が暗くなってしまう。そこで、モーションブラーが発生しない範囲で可能な限り露光時間を長く、すなわちフレームレートを低くする。

本実施形態では、モーションブラーが問題とならないようなフレームレートと露光時間の値を、車速毎に予め実験で求めておき、メモリに記録しておく。また、車両が照度計を装備している場合には、周囲の明るさ毎に、モーションブラーが問題とならないフレームレートと露光時間の値を予め実験で求めておき、メモリに記録しておく。これにより、モーションブラーの発生を回避しながらフレームレートを十分に低くすることができ、画像は明るくなるため、カメラ１２の絞りを絞ることによって画像が暗くなることを抑えることができる。

画像取得領域設定部２０は、カメラ１２の画像取得領域を設定しており、通常はカメラ１２の最大撮像領域である全画素領域を画像取得領域に設定しているが、パターン光が投光されているときには、予め設定されている所定領域を画像取得領域に設定する。この所定領域は、パターン光が投光されている領域を含む領域であり、全画素領域よりも狭く、パターン光が投光されている領域よりも広い範囲である。特に、パターン光が投光されている領域と同一の領域に設定することが好ましい。

例えば、本実施形態では、図６（ａ）に示すように、全画素領域６１の中心に位置する縦１／３、横１／４の範囲を所定領域６３とし、この所定領域６３を画像取得領域に設定する。所定領域６３は全画素領域６１の１／１２の領域になるので、この部分の画像データのみをカメラ１２から出力すれば、出力される画像データ量は１／１２になる。したがって、カメラ１２の露光時間が十分短ければ（シャッタースピードが速ければ）、同じデータ転送量でフレームレートを１２倍にすることができる。

したがって、画像取得領域設定部２０は、特徴点Ｔｅを検出するときには、図６（ｂ）に示すように全画素領域６１を画像取得領域に設定し、パターン光が投光されているときには図６（ａ）の所定領域６３に画像取得領域を設定する。これにより、フレームレートを高速化できるので、同期検波によるパターン光の抽出を容易に行うことができる。尚、所定領域６３は、全画素領域の１／４や１／９等のその他の大きさに設定されていてもよい。

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

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

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

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

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

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

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

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

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

特徴点検出部２３は、カメラ１２により取得された画像をメモリから読み込み、メモリから読み込んだ画像から、路面３１上の特徴点を検出する。詳細には、パターン光が投光されていない時間帯に特徴点検出フラグを「１」に設定し、この時間帯において特徴点を検出する。

更に、特徴点検出部２３は、路面３１上の特徴点を検出するために、例えば、「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」に記載の手法を用いることができる。

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

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

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

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

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

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

なお、姿勢変化量算出部２４は、時刻ｔと時刻ｔ＋Δｔで検出される各画像中で相対位置が算出された特徴点の全てを用いるのではなく、特徴点同士の位置関係に基づいて最適な特徴点を選定してもよい。選定方法としては、例えば、エピポーラ幾何（エピ極線幾何，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)）を用いることができる。

このように、姿勢変化量算出部２４が、時刻ｔのフレーム画像３８で相対位置（ｘｉ，ｙｉ，ｚｉ）が算出された特徴点Ｔｅ１、Ｔｅ２、Ｔｅ３を、時刻ｔ＋Δｔにおけるフレーム画像３８’からも検出した場合に、該姿勢変化量算出部２４は、路面上の複数の特徴点の相対位置（ｘｉ，ｙｉ，ｚｉ）と特徴点の画像上の位置（Ｕｉ，Ｖｉ）の時間変化から、「車両の姿勢変化量」を算出することができる。更には、車両１０の移動量を算出することができる。

即ち、前回フレームと今回フレームの間で対応関係が取れる３点以上の特徴点を検出し続けることができれば、距離及び姿勢角の変化量を加算する処理（即ち、積分演算）を継続することにより、パターン光３２ａを用いることなく、距離及び姿勢角を更新し続けることができる。但し、最初の情報処理サイクルにおいては、パターン光３２ａを用いて算出された距離及び姿勢角、或いは所定の初期的な距離及び姿勢角を用いてもよい。つまり、積分演算の起点となる距離及び姿勢角は、パターン光３２ａを用いて算出しても、或いは、所定の初期値を用いても構わない。所定の初期的な距離及び姿勢角は、少なくとも車両１０への乗員及び搭載物を考慮した距離及び姿勢角であることが望ましい。例えば、車両１０のイグニションスイッチがオン状態であって、且つシフトポジションがパーキングから他のポジションへ移動した時に、パターン光３２ａを投光し、パターン光３２ａから算出された距離及び姿勢角を、所定の初期的な距離及び姿勢角として用いればよい。これにより、車両１０の旋回や加減速によるロール運動或いはピッチ運動が発生していない時の距離や姿勢角を求めることができる。

なお、前後フレーム間で特徴点を対応付けるには、例えば、検出した特徴点の周辺の小領域の画像をメモリに記録しておき、輝度や色情報の類似度から判断すればよい。具体的には、ＥＣＵ１３は、検出した特徴点を中心とする５×５（水平×垂直）画素分の画像をメモリに記録する。姿勢変化量算出部２４は、例えば、輝度情報が２０画素以上で誤差１％以下に収まっていれば、前後フレーム間で対応関係が取れる特徴点であると判断する。そして、上記の処理で取得した姿勢変化量は、後段の自己位置算出部２６にて車両１０の自己位置を算出する際に用いられる。

図１に示す自己位置算出部２６は、姿勢変化量算出部２４で算出された「距離及び姿勢角の変化量」から車両１０の距離及び姿勢角を算出する。更に、姿勢変化量算出部２４で算出された「車両の移動量」から車両１０の自己位置を算出する。

具体的な例として、姿勢角算出部２２にて算出された距離及び姿勢角（パターン光を用いて算出された距離及び姿勢角）が起点として設定された場合について説明する。この場合には、この起点（距離及び姿勢角）に対して、自己位置算出部２６は、姿勢変化量算出部２４で算出された各フレーム毎の距離及び姿勢角の変化量を逐次加算して（積分演算して）、距離及び姿勢角を最新な数値に更新する。また、姿勢角算出部２２にて距離及び姿勢角が算出された際の車両位置を起点（車両の初期位置）とし、この初期位置からの車両１０の移動量を逐次加算して（積分演算して）、車両１０の自己位置を算出する。例えば、地図上の位置と照合された起点（車両の初期位置）を設定することで、地図上の車両の現在位置を逐次算出することができる。

従って、姿勢変化量算出部２４は、時間Δｔの間でのカメラ１２の移動量（ΔＬ）を求めることにより、車両１０の自己位置を算出することができる。更には、距離及び姿勢角の変化量も同時に算出することができるので、姿勢変化量算出部２４は、車両１０の距離及び姿勢角の変化量を考慮して、６自由度（前後、左右、上下、ヨー、ピッチ、ロール）の移動量（ΔＬ）を精度よく算出することができる。即ち、車両１０の旋回や加減速によるロール運動或いはピッチ運動によって距離や姿勢角が変化しても、移動量（ΔＬ）の推定誤差を抑制することができる。

なお、実施形態では、距離及び姿勢角の変化量を算出し、距離及び姿勢角を更新することにより、カメラ１２の移動量（ΔＬ）を算出した。しかし、路面３１に対するカメラ１２の姿勢角だけを変化量算出及び更新の対象としても構わない。この場合、路面３１とカメラ１２との距離は一定と仮定すればよい。これにより、姿勢角の変化量を考慮して、移動量（ΔＬ）の推定誤差を抑制しつつ、ＥＣＵ１３の演算負荷を軽減し、且つ演算速度を向上させることもできる。

［情報処理サイクル］
次に、カメラ１２により取得された画像３８（図７参照）から車両１０の移動量を推定する自己位置算出方法の一例として、ＥＣＵ１３により繰り返し実行される情報処理サイクルを、図９に示すフローチャート、図１０に示すタイミングチャートを参照して説明する。

図９に示す情報処理サイクルは、車両１０のイグニションスイッチがオン状態となり、自己位置算出装置１００が起動すると同時に開始され、該自己位置算出装置１００が停止するまで、繰り返し実行される。

初めに、図９のステップＳ１１において、ＥＣＵ１３は、今回の情報処理サイクルが特徴点を検出、トラッキングするサイクルであるか否かを判定する。例えば、図１０のタイミングチャートにおいて、時刻ｔ１より前で特徴点検出フラグが「１」の場合には特徴点を検出するサイクルであると判定して（ステップＳ１１でＹＥＳ）ステップＳ１２に処理を進める。一方、時刻ｔ１を過ぎて特徴点検出フラグが「０」の場合には特徴点を検出するサイクルではないと判定して（ステップＳ１１でＮＯ）ステップＳ１５に処理を進める。

ステップＳ１２において、ＥＣＵ１３は、特徴点を検出するときの設定となるようにカメラ１２を調整する。まず、絞り調整部１８は、カメラ１２の絞りを絞って、例えば絞り値をＦ５．６に設定する。また、フレームレート調整部１９は、カメラ１２のフレームレートをパターン光が投光されているときよりも低い２００ｆｐｓに設定する。さらに、画像取得領域設定部２０は、カメラ１２の画像取得領域を図６（ｂ）に示すように最大の撮像領域となる全画素領域６１に設定する。

ステップＳ１３において、カメラ１２は、ステップＳ１２で調整された設定に応じて路面３１を撮像して画像を取得する。

ステップＳ１４において、特徴点検出部２３は、パターン光が投光されていない時間帯に路面３１に存在する特徴点を検出する。この際、特徴点Ｔｅを検出する領域は、図６（ｂ）に示すように全画素領域６１となる。更に、姿勢変化量算出部２４は、特徴点検出部２３で検出された特徴点の画像上の位置に基づき、カメラ１２に対する特徴点の変化量を算出する。

また、ステップＳ１４では、特徴点検出部２３が、画像３８から特徴点（例えば、アスファルト上に存在する凹凸部位）を検出すると、姿勢変化量算出部２４が、前回の情報処理サイクルと今回の情報処理サイクルの間で対応関係が取れる特徴点を抽出し、特徴点の画像上の位置（Ｕｉ、Ｖｉ）から距離及び姿勢角を更新する。

具体的には、特徴点検出部２３は、特徴点検出フラグが「１」のとき、カメラ１２により取得された画像３８をメモリから読み込み、画像３８から路面３１上の特徴点を検出し、各特徴点の画像上の位置（Ｕｉ、Ｖｉ）をメモリに記憶する。姿勢変化量算出部２４は、各特徴点の画像上の位置（Ｕｉ、Ｖｉ）をメモリから読み込み、距離及び姿勢角と、特徴点の画像上の位置（Ｕｉ、Ｖｉ）とから、カメラ１２に対する特徴点の相対位置（Ｘｉ，Ｙｉ，Ｚｉ）を算出する。姿勢変化量算出部２４は、カメラ１２に対する特徴点の相対位置（Ｘｉ，Ｙｉ，Ｚｉ）をメモリに記憶する。

そして、姿勢変化量算出部２４は、特徴点の画像上の位置（Ｕｉ、Ｖｉ）と、前回の情報処理サイクルのステップＳ１４において算出された特徴点の相対位置（Ｘｉ，Ｙｉ，Ｚｉ）をメモリから読み込む。姿勢変化量算出部２４は、前回の情報処理サイクルと今回の情報処理サイクルの間で対応関係が取れる特徴点の相対位置（Ｘｉ，Ｙｉ，Ｚｉ）及び画像上の位置（Ｕｉ、Ｖｉ）を用いて、距離及び姿勢角の変化量を算出する。姿勢変化量算出部２４は、前回の情報処理サイクルで求められた距離及び姿勢角に、上記した距離及び姿勢角の変化量を加算することにより、距離及び姿勢角を更新する。そして、更新後の距離及び姿勢角をメモリに記憶する。つまり、前回のサイクルのステップＳ１９（後述）の処理で設定された距離及び姿勢角に対して、今回の情報処理サイクル毎の距離及び姿勢角の変化量を積分演算することにより、距離及び姿勢角を更新する処理を実行する。その後、ステップＳ１９に処理を進める。

一方、ステップＳ１５において、ＥＣＵ１３は、パターン光を抽出するときの設定となるようにカメラ１２を調整する。まず、絞り調整部１８は、図１０において時刻ｔ１になると、カメラ１２の絞りを開いて、例えば絞り値をＦ２に設定する。また、フレームレート調整部１９は、カメラ１２のフレームレートをパターン光が投光されていないときの１２倍となる２４００ｆｐｓに設定する。さらに、画像取得領域設定部２０は、カメラ１２の画像取得領域を図６（ａ）に示すように全画素領域６１の１／１２となる所定領域６３に設定する。このように所定領域６３を全画素領域６１の１／１２の領域にしたので、出力される画像データ量が１／１２になり、これによって同じデータ転送量でフレームレートを１２倍にすることができる。

ステップＳ１６において、パターン光制御部２７は、投光器１１を制御して、車両周囲の路面３１にパターン光を投光する。この際、パターン光制御部２７は、パターン光の輝度が所定周期の正弦波状に変化するように投光電力を制御する。例えば、正弦波の周波数を２００［Ｈｚ］とする。その結果、時間経過に伴い輝度が正弦波状に変化するパターン光が路面３１に投光される。

ステップＳ１７において、カメラ１２は、ステップＳ１５で調整された設定に応じてパターン光が投光されている領域を含む路面３１を撮像して画像を取得する。

ステップＳ１８において、パターン光抽出部２１は、上述した同期検波処理によりパターン光３２ａを抽出する。そして、パターン光３２ａの位置を示すデータとして、スポット光Ｓｐの画像上の座標（Ｕｊ、Ｖｊ）を算出し、これをメモリに記憶する。更に、姿勢角算出部２２は、パターン光３２ａの位置を示すデータをメモリから読み込み、パターン光３２ａの位置から、距離及び姿勢角を算出し、メモリに記憶する。距離及び姿勢角の算出方法は、前述した通りである。なお、以下では、姿勢角算出部２２によりパターン光を用いて路面３１に対する車両１０の距離及び姿勢角を求め、これを積分演算の起点となる距離及び姿勢角として再設定する処理を「距離及び姿勢角をリセットする」と表現する。この後、ステップＳ１９に処理を進める。

ステップＳ１９において、ＥＣＵ１３は、積分演算の起点を選択する。この処理では、最初の情報処理サイクルにおいては、パターン光３２ａより算出される距離及び姿勢角を、起点に選択して設定する。更に、予め設定した条件が成立した場合、例えば、特徴点検出部２３における特徴点の検出状態が低下し、特徴点検出フラグが「１」となるタイミングで複数の特徴点を検出できなくなった場合には、移動量算出の起点をパターン光により算出する距離及び姿勢角、即ちステップＳ１８の処理で算出された距離及び姿勢角にリセットする。他方、特徴点検出部２３にて特徴点が正常に検出されている場合には、該特徴点の位置に基づいて距離及び姿勢角を更新する。

つまり、特徴点検出部２３により特徴点が正常に検出されない場合には、カメラ１２の距離及び姿勢角を高精度に設定することができず、この精度の低い距離及び姿勢角を採用して車両の移動量を算出すると、車両の移動量を高精度に検出できなくなる。従って、このような場合には、移動量算出の起点をパターン光の位置から求めた距離及び姿勢角にリセットする。こうすることで、距離及び姿勢角に大幅な誤差が生じることを防止する。

次いで、ステップＳ２０において、自己位置算出部２６は、ステップＳ１４或いはＳ１８の処理で求められた距離及び姿勢角と、積分演算の起点、及び、特徴点の画像上の位置（Ｕｉ、Ｖｉ）の変化量とから、路面３１に対するカメラ１２の移動量（ΔＬ）、即ち車両１０の移動量を算出する。

こうして、本実施形態に係る自己位置算出装置では、上記した一連の情報処理サイクルを繰り返し実行して車両１０の移動量を積算することにより、車両１０の位置を算出することができる。

［実施形態の効果］
以上、詳細に説明したように、本実施形態に係る自己位置算出装置１００では、パターン光が投光されているときには、パターン光が投光されていないときよりカメラ１２の絞りを開いて画像を撮像する。これにより、パターン光が投光されているときに明るい画像を撮像することができるので、パターン光の微弱な変化まで検出することができる。したがって、同期検波によるパターン光の抽出と特徴点の検出との間にあるトレードオフの関係を低減して高精度且つロバストに自己位置を算出することができる。

具体的に説明すると、同期検波によるパターン光の抽出では、カメラ１２の絞りを開いてフレームレートを高速にする必要があり、必要となる画像領域はカメラ画像の一部である。これに対して、特徴点の検出では、カメラ１２の絞りを絞ってフレームレートは低速でよく、必要な画像領域はカメラ画像の全体である。このように、同期検波によるパターン光の抽出と特徴点の検出との間にはトレードオフの関係がある。

しかしながら、上述したように本実施形態に係る自己位置算出装置１００では、パターン光が投光されているときにカメラ１２の絞りを開いて明るい画像を撮像するので、パターン光の微弱な変化まで検出することができる。これにより、上述したトレードオフの関係を低減することができる。

また、本実施形態に係る自己位置算出装置１００では、パターン光が投光されていないときには、パターン光が投光されているときよりカメラ１２のフレームレートを低くする。これにより、カメラ１２の絞りを絞っても露光時間を長くして明るい画像を取得することができ、特徴点の検出状態を向上させて上述したトレードオフの関係を低減することができる。

さらに、本実施形態に係る自己位置算出装置１００では、パターン光が投光されているときには、パターン光が投光されている領域を含む所定領域をカメラ１２の画像取得領域に設定する。これにより、出力される画像データ量を低減できるので、その分だけフレームレートを高めることができ、上述したトレードオフの関係を低減することができる。

また、本実施形態に係る自己位置算出装置１００では、パターン光が投光されていないときに特徴点を検出して車両の姿勢変化量を算出する。これにより、特徴点の検出に適した設定で撮像された画像から特徴点を検出できるので、特徴点の検出状態が向上して上述したトレードオフの関係を低減することができる。

さらに、本実施形態に係る自己位置算出装置１００では、上述したすべての構成を備えていることにより、トレードオフの関係にある同期検波によるパターン光の抽出と特徴点の検出とを両立させて高精度且つロバストに自己位置を算出することができる。

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

１０ 車両
１１ 投光器
１２ カメラ（撮像部）
１３ ＥＣＵ
１８ 絞り調整部
１９ フレームレート調整部
２０ 画像取得領域設定部
２１ パターン光抽出部
２２ 姿勢角算出部
２３ 特徴点検出部
２４ 姿勢変化量算出部
２６ 自己位置算出部
２７ パターン光制御部
３２ａ，３２ｂ パターン光
１００ 自己位置算出装置

Claims (5)

1. 車両周囲の路面にパターン光を投光する投光器と、
   前記パターン光の輝度が周期的に変化するように制御するパターン光制御部と、
   前記車両に搭載され、前記パターン光が投光された領域を含む車両周囲の路面を撮像して画像を取得する撮像部と、
   前記パターン光が投光されているときには、前記パターン光が投光されていないときより前記撮像部の絞りを開く絞り調整部と、
   前記撮像部で取得された画像からパターン光を抽出するパターン光抽出部と、
   前記パターン光抽出部にて抽出されたパターン光の位置から、前記路面に対する車両の姿勢角を算出する姿勢角算出部と、
   前記撮像部により取得された画像から路面上の複数の特徴点を検出し、各特徴点の時間変化に基づいて、前記車両の姿勢変化量を算出する姿勢変化量算出部と、
   前記車両の初期位置及び姿勢角に、前記姿勢変化量を加算してゆくことで、前記車両の現在位置及び姿勢角を算出する自己位置算出部と、
   を備えたことを特徴とする自己位置算出装置。
2. 前記パターン光が投光されていないときには、前記パターン光が投光されているときより前記撮像部のフレームレートを低くするフレームレート調整部をさらに備えたことを特徴とする請求項１に記載の自己位置算出装置。
3. 前記パターン光が投光されているときには、前記パターン光が投光されている領域を含む所定領域を、前記撮像部の画像取得領域に設定する画像取得領域設定部をさらに備えたことを特徴とする請求項２に記載の自己位置算出装置。
4. 前記姿勢変化量算出部は、前記パターン光が投光されていないときに、前記撮像部により取得された画像から路面上の複数の特徴点を検出し、各特徴点の時間変化に基づいて、前記車両の姿勢変化量を算出することを特徴とする請求項１〜３のいずれか１項に記載の自己位置算出装置。
5. 車両に搭載された投光器から車両周囲の路面にパターン光を投光する手順と、
   前記車両の制御部が、前記パターン光の輝度が周期的に変化するように制御する手順と、
   前記制御部が、前記パターン光が投光されているときには、前記パターン光が投光されていないときより前記車両に搭載された撮像部の絞りを開く手順と、
   前記撮像部によって、前記パターン光が投光された領域を含む車両周囲の路面を撮像して画像を取得する手順と、
   前記制御部が、前記撮像部で取得された画像からパターン光を抽出する手順と、
   前記制御部が、抽出されたパターン光の位置から、前記路面に対する車両の姿勢角を算出する手順と、
   前記制御部が、前記撮像部により取得された画像から路面上の複数の特徴点を検出し、各特徴点の時間変化に基づいて、前記車両の姿勢変化量を算出する手順と、
   前記制御部が、前記車両の初期位置及び姿勢角に、前記姿勢変化量を加算してゆくことで、前記車両の現在位置及び姿勢角を算出する手順と、
   を含むことを特徴とする自己位置算出方法。