JP2016091468A - 目標軌跡算出装置及びプログラム - Google Patents

Abstract

【課題】レーンマークが存在しない場合であっても、車両の目標軌跡を算出することができるようにする。【解決手段】画像情報取得部１６によって、車両の前方画像を取得する。エッジ検出部１８によって、取得された車両の前方画像から、複数のエッジ線分を検出する。そして、座標変換部２０によって、検出された複数のエッジ線分の各々について、当該エッジ線分を、車両の位置を基準とする車両座標系におけるエッジ線分の位置を計算する。そして、目標軌跡算出部２２によって、座標変換部２０によって複数のエッジ線分の各々について計算された車両座標系におけるエッジ線分の位置に基づいて、複数のエッジ線分の各々についての、車両の左右方向への平行移動量を自由変数としたエッジ線分と、車両の目標軌跡との差分を最小化するように、複数のエッジ線分の各々の平行移動量、及び車両の目標軌跡を表す多項式のパラメータである目標軌跡パラメータを算出する。【選択図】図１

Description

本発明は、目標軌跡算出装置及びプログラムに関する。

従来より、レーンマークがない道路でも、道路鋲やポストコーンなどの複数種類の検出手段を用い、レーン形状の認識に適した情報を選択することで、レーンキープのためのパラメータを算出する走行レーン認識装置が知られている（特許文献１）。

また、白線もしくは道路鋲といったレーンマークの種類を区別せずに、エッジ点として利用し、消失点に向かって伸びるエッジ点を統合することでレーン形状を認識するレーンマーク認識装置が知られている（特許文献２）。

また、車線の右端及び左端位置のエッジ線分（レーンマークや段差の変化する点による）を検出し、路面モデルを当てはめることで路面形状を認識する路面形状認識装置が知られている（特許文献３）。

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

しかし、上記の特許文献１に記載の技術では、レーン検出に用いる対象物ごとに検出手段を備える必要があり、演算量が多くなるという問題がある。

また、上記の特許文献２に記載の技術では、任意のエッジをレーン検出に利用できるが、消失点に向かってエッジが延びていることを想定しているため、曲線形状の道路に対応できない。

また、上記の特許文献３に記載の技術では、任意のエッジを路面形状検出に利用できるが、エッジは車線の右端又は左端のそれぞれについて、遠方まで連続して分布していることが必要になる。したがって、断片的な線分は路面形状検出に適さない。

このように、従来技術の多くは、車両付近から消失点付近まで連続して分布するエッジを利用して、車線の境界を求めることを想定している。しかし整備された道路以外では、上記の条件を満たすことは難しい。

本発明は、上記の問題点を解決するためになされたもので、レーンマークが存在しない場合であっても、車両の目標軌跡を算出することができる目標軌跡算出装置及びプログラムを提供することを目的とする。

上記の目的を達成するために本発明に係る目標軌跡算出装置は、車両の前方画像を取得する画像情報取得手段と、前記画像情報取得手段によって取得された前記車両の前方画像から、複数のエッジ線分を検出するエッジ検出手段と、前記エッジ検出手段によって検出された前記複数のエッジ線分の各々について、前記エッジ線分を、前記車両の位置を基準とする車両座標系における前記エッジ線分の位置を計算する座標変換手段と、前記座標変換手段によって前記複数のエッジ線分の各々について計算された前記車両座標系における前記エッジ線分の位置に基づいて、前記複数のエッジ線分の各々についての、前記車両の左右方向への平行移動量を自由変数とした前記エッジ線分と、前記車両の目標軌跡との差分を最小化するように、前記複数のエッジ線分の各々の前記平行移動量、及び前記車両の目標軌跡を表す多項式のパラメータである目標軌跡パラメータを算出する目標軌跡算出手段と、を含んで構成されている。

本発明に係るプログラムは、コンピュータを、車両の前方画像を取得する画像情報取得手段、前記画像情報取得手段によって取得された前記車両の前方画像から、複数のエッジ線分を検出するエッジ検出手段、前記エッジ検出手段によって検出された前記複数のエッジ線分の各々について、前記エッジ線分を、前記車両の位置を基準とする車両座標系における前記エッジ線分の位置を計算する座標変換手段、及び前記座標変換手段によって前記複数のエッジ線分の各々について計算された前記車両座標系における前記エッジ線分の位置に基づいて、前記複数のエッジ線分の各々についての、前記車両の左右方向への平行移動量を自由変数とした前記エッジ線分と、前記車両の目標軌跡との差分を最小化するように、前記複数のエッジ線分の各々の前記平行移動量、及び前記車両の目標軌跡を表す多項式のパラメータである目標軌跡パラメータを算出する目標軌跡算出手段として機能させるためのプログラムである。

本発明によれば、画像情報取得手段によって、車両の前方画像を取得する。エッジ検出手段によって、画像情報取得手段によって取得された車両の前方画像から、複数のエッジ線分を検出する。

そして、座標変換手段によって、エッジ検出手段によって検出された複数のエッジ線分の各々について、当該エッジ線分を、車両の位置を基準とする車両座標系におけるエッジ線分の位置を計算する。

そして、目標軌跡算出手段によって、座標変換手段によって複数のエッジ線分の各々について計算された車両座標系におけるエッジ線分の位置に基づいて、複数のエッジ線分の各々についての、車両の左右方向への平行移動量を自由変数としたエッジ線分と、車両の目標軌跡との差分を最小化するように、複数のエッジ線分の各々の平行移動量、及び車両の目標軌跡を表す多項式のパラメータである目標軌跡パラメータを算出する。

このように、車両の前方画像から検出された複数のエッジ線分の各々について、車両の位置を基準とする車両座標系における当該エッジ線分の位置を計算し、車両座標系におけるエッジ線分の位置に基づいて、複数のエッジ線分の各々についての、車両の左右方向への平行移動量を自由変数としたエッジ線分と、車両の目標軌跡との差分を最小化するように、複数のエッジ線分の各々の平行移動量、及び車両の目標軌跡を表す多項式のパラメータである目標軌跡パラメータを算出することにより、レーンマークが存在しない場合であっても、車両の目標軌跡を算出することができる。

本発明に係る前記画像情報取得手段は、車両の前方画像を各時刻ｔについて取得し、前記エッジ検出手段は、各時刻ｔについて、前記画像情報取得手段によって取得された時刻ｔの前記車両の前方画像から、時刻ｔの複数のエッジ線分を検出し、前記座標変換手段は、各時刻ｔについて、前記エッジ検出手段によって検出された時刻ｔの前記複数のエッジ線分の各々について、前記エッジ線分を、時刻ｔの前記車両の位置を基準とする車両座標系における前記エッジ線分の位置を計算し、前記目標軌跡算出手段は、各時刻ｔについて、前記座標変換手段によって前記複数のエッジ線分の各々について計算された前記車両座標系における前記エッジ線分の位置に基づいて、時刻ｔの前記複数のエッジ線分の各々についての、前記車両の左右方向への平行移動量を自由変数とした前記エッジ線分と、時刻ｔの前記車両の目標軌跡との差分、及び時刻ｔ−１の前記車両の目標軌跡と時刻ｔの前記車両の目標軌跡との差分を最小化するように、前記複数のエッジ線分の各々の前記平行移動量、及び前記目標軌跡パラメータを算出するようにすることができる。

本発明は、前記目標軌跡算出手段によって算出された時刻ｔの前記目標軌跡パラメータと、時刻ｔ−１までの前記目標軌跡パラメータとに基づいて、時刻ｔの前記目標軌跡パラメータを算出する平滑化手段を更に含むようにすることができる。

本発明に係る前記平滑化手段は、時刻ｔ−１の前記目標軌跡パラメータに基づいて、前記目標軌跡パラメータを表すパーティクルを複数生成し、算出された時刻ｔの前記目標軌跡パラメータに基づいて、前記複数のパーティクルの各々に対する尤度を算出し、算出された前記複数のパーティクルの各々に対する前記尤度を用いて、前記複数のパーティクルの各々が表す前記目標軌跡パラメータの重み付け和を、時刻ｔの前記目標軌跡パラメータとして算出するようにすることができる。

本発明に係る前記平滑化手段は、算出された時刻ｔの前記目標軌跡パラメータの候補と、時刻ｔ−１の前記目標軌跡パラメータと、予め定められた重みとに基づいて、時刻ｔの前記目標軌跡パラメータの候補と時刻ｔ−１の前記目標軌跡パラメータとの重み付け和を、時刻ｔの前記目標軌跡パラメータとして算出するようにすることができる。

本発明は、算出された前記目標軌跡パラメータに基づいて、前記目標軌跡パラメータを用いた多項式で表わされる前記車両の目標軌跡上から、前記車両の目標位置を検出する目標点検出手段を更に含むようにすることができる。

以上説明したように、本発明の目標軌跡算出装置及びプログラムによれば、車両の前方画像から検出された複数のエッジ線分の各々について、車両の位置を基準とする車両座標系における当該エッジ線分の位置を計算し、車両座標系におけるエッジ線分の位置に基づいて、複数のエッジ線分の各々についての、車両の左右方向への平行移動量を自由変数としたエッジ線分と、車両の目標軌跡との差分を最小化するように、複数のエッジ線分の各々の平行移動量、及び車両の目標軌跡を表す多項式のパラメータである目標軌跡パラメータを算出することにより、レーンマークが存在しない場合であっても、車両の目標軌跡を算出することができる、という効果が得られる。

本発明の実施の形態に係る目標軌跡算出装置を示すブロック図である。 車両座標系を示すイメージ図である。 パーティクルフィルタの動作例を示すイメージ図である。 本発明の実施の形態に係る目標軌跡算出装置における制御パラメータ算出処理ルーチンの内容を示すフローチャートである。 本発明の実施の形態に係る目標軌跡算出装置における平滑化処理ルーチンの内容を示すフローチャートである。

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

［第１の実施の形態］
本発明の実施の形態では、自車両の目標軌跡を算出する目標軌跡算出装置に本発明を適用させた場合を例に説明する。

自車両前方の道路上の線分を抽出すると、それらの多くは進行方向に沿った線分となっている。したがって、それらの線分を左右に平行移動すれば、自車両が進むべく目標軌跡の上に乗ると考えられる。したがって、線分の平行移動を許すように自由なパラメータを設定したうえで、最小二乗法により軌跡推定をすれば、目標軌跡が求められる。

図１に示すように、本発明の第１の実施の形態に係る目標軌跡算出装置１０は、道路領域を含む自車両前方を撮像して画像を出力する撮像装置１２と、撮像装置１２から得られる画像に基づいて、自車両を制御するための制御パラメータを算出する処理を実行するコンピュータ１４と、コンピュータ１４によって算出された制御パラメータに基づいて、自車両を制御する車両制御装置３０とを含んで構成されている。

撮像装置１２は、車両に搭載され、道路領域を含む自車両前方を逐次撮像して、自車両の前方画像を出力する。撮像装置１２は、自車両前方を撮像し、画像の画像信号を生成する２つの撮像部（図示省略）と、２つの撮像部で生成された画像信号をＡ／Ｄ変換するＡ／Ｄ変換部（図示省略）と、Ａ／Ｄ変換された画像信号を一時的に格納するための画像メモリ（図示省略）とを備えている。

コンピュータ１４は、ＣＰＵと、ＲＡＭと、後述する各処理ルーチンを実行するためのプログラムを記憶したＲＯＭと、これらを接続するバスとを含んで構成されている。このコンピュータ１４をハードウエアとソフトウエアとに基づいて定まる機能実現手段毎に分割した機能ブロックで説明すると、画像情報取得部１６と、エッジ検出部１８と、座標変換部２０と、目標軌跡算出部２２と、平滑化部２４と、目標点検出部２６と、制御パラメータ算出部２８と、を含んだ構成で表すことができる。

画像情報取得部１６は、撮像装置１２によって各時刻ｔについて撮像された自車両の前方画像を取得する。

エッジ検出部１８は、各時刻ｔについて、画像情報取得部１６によって取得された時刻ｔの自車両の前方画像から、複数のエッジ線分を検出する。具体的には、エッジ検出部１８は、時刻ｔの自車両の前方画像から、Ｈｏｕｇｈ変換等の手段を用いて画像中のエッジを検出する。そして、エッジ検出部１８は、Ｈｏｕｇｈ変換により求められた直線の中で、エッジ上にある区間のみを時刻ｔのエッジ線分として抽出する。

座標変換部２０は、各時刻ｔについて、エッジ検出部１８によって検出された時刻ｔの複数のエッジ線分の各々について、時刻ｔの自車両の位置を基準とする車両座標系における当該エッジ線分の位置を計算する。

図２に、自車両の位置を基準とする車両座標系の一例を示す。座標変換部２０は、エッジ検出部１８により求められたエッジ線分を、図２に示されるように、自車両の位置を中心とする車両座標系に変換する。なお、車両座標系におけるｘ軸は、車両の左右方向の位置を表す。また、車両座標系におけるｚ軸は、車両の前後方向の位置を表す。なお、車両の左右方向とは、車両を正面から見た場合の車両幅方向を表す。

ここで、自車両の前方画像の画像サイズを（ｗ，ｈ）［ｐｉｘｅｌ］、撮像装置１２の地面からの高さをｙ［ｍ］、撮像装置１２の焦点距離をｆ［ｐｉｘｅｌ］、ピッチ角（上向きが正）をα［ｒａｄ］とする（簡単化のため、ピッチ角は固定とみなす）。また、自車両の前方画像の歪み補正が前もって行われているものとすると、以下の式（１）により画像上のエッジ線分のエッジ端点（ｕ，ｖ）［ｐｉｘｅｌ］を、車両座標系の座標（ｘ，ｚ）［ｍ］へと変換することができる。

目標軌跡算出部２２は、各時刻ｔについて、座標変換部２０によって複数のエッジ線分の各々について計算された車両座標系における当該エッジ線分の位置に基づいて、時刻ｔの複数のエッジ線分の各々についての、自車両の左右方向への平行移動量を自由変数としたエッジ線分と、時刻ｔの自車両の目標軌跡との差分を最小化するように、目標軌跡パラメータの候補を算出する。本実施の形態では、後述する目標軌跡を表す多項式ｆ（ｚ）の係数ｆ_０，…，ｆ_ｄを目標軌跡パラメータとする。

本実施の形態では、目標軌跡を多項式モデル（次元数ｄ＝１，２，３）により表現し、最小二乗法を用いてエッジ線分群に目標軌跡を当てはめる。
エッジは道路全域に分布するため、分布しているエッジを統合して目標軌跡を求める必要がある。そのため、上記図２に示されるように、エッジ線分については車両の左右方向（ｘ軸方向）の平行移動を許しながら、エッジ線分に最もあてはまる目標軌跡を求める。

ここで、エッジ線分を一般化して多項式で表現した式をｓ_ｎ（ｚ）（ｎ＝１，...，Ｎ；Ｎは線分数）、目標軌跡（エッジ線分のあてはめ結果）の多項式をｆ（ｚ）と表す。次元数をｄとすると、エッジ線分の多項式と、目標軌跡を表す多項式とは以下の式（２）により表される。

上記式（２）のｓ_ｎ，０が、車両の左右方向のオフセット量となる。ｓ_ｎ（ｚ）は線分であるため、２次以上の係数は０となる（ｓ_ｎ，２＝ｓ_ｎ，３＝０）。また、ｚ_ｎ，１は自車両に近い方のエッジ端点、ｚ_ｎ，２は自車から遠い方のエッジ端点であり、線分が定義されるｚはこの範囲内となる。範囲［ｚ_ｎ，１，ｚ_ｎ，２］内でこれら多項式間の距離を最小化することで、エッジ線分に沿うような目標軌跡が求められる。
この場合、未知変数は目標軌跡を表す多項式ｆ（ｚ）の目標軌跡パラメータｆ_０，…，ｆ_ｄ（ｄ＋１個）と、エッジ線分の左右方向の平行移動量（０次の係数）ｓ_１，０，…，ｓ_Ｎ，０を求める必要がある。したがって、未知変数の数はｄ＋Ｎ＋１となり、以下の連立方程式（３）を解くことで最小二乗法の解が求められる。

ここで、Ｊ（ｆ，ｓ_ｎ）及びＪは、目標軌跡とエッジ線分との距離（単独および総和）であり、以下の式（４）となる。

上記式（４）については、実際は、エッジ線分の統合後の多項式についての左右方向の位置が不定（∂Ｊ／∂ｆ_０＝０と∂Ｊ（ｆ，ｓ_ｎ）／∂ｓ_ｎ，０＝０とが従属）であるため、上記式（３）及び（４）を解くことはできない。そのため、追加の条件として、左右方向の位置を固定した多項式ｓ_０（ｚ）を定義し、多項式ｓ_０（ｚ）と目標軌跡を表す多項式ｆ（ｚ）との距離を最小化する式∂Ｊ（ｆ，ｓ_０）／∂ｆ_０＝０を与える。

この多項式ｓ_０（ｚ）としては、例えば、後述する平滑化部２４によって前回算出された時刻ｔ−１の目標軌跡パラメータを用いた目標軌跡を表す多項式ｆ（ｚ）（前時刻の当てはめ結果）を用いることができる。ここで、Δ_ｎ ^（ｉ）＝ｚ_ｎ，２ ^ｉ−ｚ_ｎ，１ ^ｉとおくと、これらを統合した最小二乗法の解は以下の方程式（５）を解くことにより求められる。なお、Δ_ｎ ^（ｉ）＝ｚ_ｎ，２ ^ｉ−ｚ_ｎ，１ ^ｉにおける右辺のｉは乗数を表し、左辺の（ｉ）は添え字を表す。
従って、目標軌跡算出部２２は、自車両の左右方向への平行移動量を自由変数としたエッジ線分と、時刻ｔの自車両の目標軌跡との差分、及び時刻ｔ−１の自車両の目標軌跡と時刻ｔの自車両の目標軌跡との差分を最小化するように、目標軌跡パラメータの候補を算出する。

平滑化部２４は、目標軌跡算出部２２によって算出された時刻ｔの目標軌跡パラメータの候補と、時刻ｔ−１の目標軌跡パラメータとに基づいて、時刻ｔの目標軌跡パラメータを算出する。

平滑化部２４は、各時刻ｔの自車両の前方画像について、画像フレーム間の連続性を考慮し、パーティクルフィルタ（例えば、参考文献（M. Isard and A. Blake，“Condensation-conditional density propagation for visual tracking.”International Journal of Computer Vision，Vol.29，No.1，pp.5-28，1998）を参照。）を用いて、時刻ｔの目標軌跡パラメータを算出する。
パーティクルフィルタは、前状態から現状態のパラメータを推定する手法であり、物体追跡等に利用されている。本実施の形態では、時刻ｔ−１を前状態とし、時刻ｔを現状態とする。

具体的には、平滑化部２４は、目標軌跡算出部２２によって前回計算された時刻ｔ−１の目標軌跡パラメータに基づいて、時刻ｔ−１の目標軌跡パラメータを分布中心としてパーティクル（粒子）を拡散させて、目標軌跡パラメータを表すパーティクルを複数生成し、時刻ｔの目標軌跡パラメータの候補に基づいて、複数のパーティクルの各々に対する尤度を算出する。そして、平滑化部２４は、算出された複数のパーティクルの各々に対する尤度を用いて、複数のパーティクルが表す目標軌跡パラメータの重み付け和を、時刻ｔの目標軌跡パラメータとして算出する。

本実施の形態では、目標軌跡を表す多項式の目標軌跡パラメータ、すなわち上記式（２）の［ｆ_０ ｆ_１ … ｆ_ｄ］^ｔをパラメータとする。

算出対象のパラメータの次元数が１の場合の動作例を図３に示す。図３に示されるσは拡散時のパーティクルの広がりを表すパラメータである。σの値が大きいほど状態変動に早く順応できる一方、外れ値によるばらつき、振動の影響を受けやすい。σの値は係数の次元に応じて異なる値を用いることができる。一般に、ばらつきを抑えるため高次の係数ほど小さなσの値を用いる（本実施の形態では０〜３次の係数をそれぞれ［０．１ ０．０１ ０．００１ ０．０００１］^ｔとする）。

ここで、尤度は、前状態（過去のフレーム）のパラメータから粒子拡散させたパラメータと、新たに算出した目標軌跡（次元数ｄ＝１，２，３）の距離から定義する。拡散させた目標軌跡パラメータの多項式をｆ￣（ｚ）、次元数ｄの目標軌跡を表す多項式をｆ^（ｄ）（ｚ）（ｄ次以上の係数は０）とする。これらの多項式間の距離を、自車両から所定距離ｚ_２までの範囲内で累積し、以下の式（６）で尤度を求める。

上記式（６）では、ｄ＝１，２，３いずれかの次元の多項式との距離が小さい場合に、尤度は大きな値をとる。なお、ｚ_２として、目標点のｚ位置（自車両からの距離）を用いると有効である。
また、次元数ｄをあらかじめ指定し、以下の式（６’）によって尤度を推定することもできる。

このようにして求めた尤度Ｌを重みとして粒子拡散させたパラメータを平均することで、各次元を統合した目標軌跡を表す多項式が求められる。

目標点検出部２６は、平滑化部２４によって算出された目標軌跡パラメータに基づいて、当該目標軌跡パラメータを用いた多項式で表わされる自車両の目標軌跡上から、自車両の目標位置を検出する。具体的には、目標点検出部２６は、平滑化部２４によって算出された目標軌跡パラメータを用いた上記式（２）に、目標点ｚ座標（主に数秒後の自車両の前後方向位置）を代入することで、目標点位置を検出する。

制御パラメータ算出部２８は、目標点検出部２６によって算出された自車両の目標位置に基づいて、自車両の軌跡を制御するための制御パラメータを算出する。例えば、目標点に向かう軌道を、以下の参考文献２の方法を用いて生成することで、ふらつきの少ない車両制御が可能となる。

参考文献２（浅井彰司、天野真輝、服部義和、小野英一、「コウモリの飛行モデルに基づく車線追従制御の改良と実車検証」、自動車技術会論文集、vol. 43、no. 1、p.33-38、2012．）

車両制御装置３０は、例えば、電動パワーステアリングによって構成される。車両制御装置３０は、制御パラメータ算出部２８によって算出された制御パラメータに従って、車輪の操舵角を制御する。

＜目標軌跡算出装置１０の作用＞
次に、本発明の実施の形態に係る目標軌跡算出装置１０の作用について説明する。まず、撮像装置１２によって、自車両前方の撮像が開始されると、コンピュータ１４において、図４に示す制御パラメータ算出処理ルーチンが繰り返し実行される。

ステップＳ１００において、画像情報取得部１６によって、撮像装置１２によって撮像された時刻ｔの自車両の前方画像を取得する。

ステップＳ１０２において、エッジ検出部１８によって、上記ステップＳ１００で取得された時刻ｔの自車両の前方画像から、複数のエッジ線分を検出する。

ステップＳ１０４において、座標変換部２０によって、上記ステップＳ１０２で検出された時刻ｔの複数のエッジ線分の各々について、時刻ｔの自車両の位置を基準とする車両座標系における当該エッジ線分の位置を計算する。

ステップＳ１０６において、目標軌跡算出部２２によって、上記ステップＳ１０４で複数のエッジ線分の各々について計算された車両座標系における当該エッジ線分の位置に基づいて、時刻ｔの複数のエッジ線分の各々についての、自車両の左右方向への平行移動量を自由変数としたエッジ線分と、時刻ｔの自車両の目標軌跡との差分、及び時刻ｔ−１の自車両の目標軌跡と時刻ｔの自車両の目標軌跡との差分を最小化するように、目標軌跡パラメータの候補を算出する。
そして、ステップＳ１０８において、時刻ｔ−１の目標軌跡パラメータと、上記ステップＳ１０６で算出された目標軌跡パラメータの候補に基づいて、平滑化処理を行う。ステップＳ１０８は、図５に示す平滑化処理ルーチンによって実現される。

＜平滑化処理ルーチン＞
ステップＳ２００において、平滑化部２４によって、前回のステップＳ２０６で算出された時刻ｔ−１の目標軌跡パラメータに基づいて、時刻ｔ−１の目標軌跡パラメータを分布中心としてパーティクル（粒子）を拡散させて、目標軌跡パラメータを表すパーティクルを複数生成する。

ステップＳ２０２において、平滑化部２４によって、上記ステップＳ１０６で算出された目標軌跡パラメータの候補に基づいて、上記ステップＳ２００で拡散された複数のパーティクルの各々に対する尤度を算出する。

ステップＳ２０４において、平滑化部２４によって、上記ステップＳ２０２で算出された複数のパーティクルの各々に対する尤度を用いて、複数のパーティクルが表す目標軌跡パラメータの重み付け和を、時刻ｔの目標軌跡パラメータとして算出する。

ステップＳ２０６において、平滑化部２４によって、上記ステップＳ２０４で算出された時刻ｔの目標軌跡パラメータを結果として出力して、平滑化処理ルーチンを終了する。

次に、制御パラメータ算出処理ルーチンに戻り、ステップＳ１１０において、上記ステップＳ１０８で算出された目標軌跡パラメータに基づいて、当該目標軌跡パラメータを用いた多項式で表わされる自車両の目標軌跡上から、自車両の目標位置を検出する。

ステップＳ１１２において、制御パラメータ算出部２８によって、上記ステップＳ１０８で算出された自車両の目標位置に基づいて、自車両の軌跡を制御するための制御パラメータを算出する。

ステップＳ１１４において、上記ステップＳ１１２で算出された制御パラメータを結果として出力し、制御パラメータ算出処理ルーチンを終了する。

制御パラメータ算出処理ルーチンによって算出された制御パラメータは、車両制御装置３０によって自車両の制御に用いられる。

以上説明したように、本発明の実施の形態の目標軌跡算出装置によれば、車両の前方画像から検出された複数のエッジ線分の各々について、車両の位置を基準とする車両座標系における当該エッジ線分の位置を計算し、車両座標系におけるエッジ線分の位置に基づいて、複数のエッジ線分の各々についての、車両の左右方向への平行移動量を自由変数としたエッジ線分と、車両の目標軌跡との差分を最小化するように、複数のエッジ線分の各々の平行移動量、及び車両の目標軌跡を表す多項式のパラメータである目標軌跡パラメータを算出することにより、レーンマークが存在しない場合であっても、車両の前方画像から検出された複数のエッジ線分から、車両の目標軌跡を算出することができる。

また、レーンマークの存在しない道路や、レーンマークの検出できない道路であっても、周囲の線分情報を利用して目標点を検出することで、レーンマーク検出に基づく車線維持支援装置と同様の機能が実現可能となる。

また、所定時刻後の自車の目標点が定まれば、制御パラメータ推定可能であることを利用し、必ずしも連続していない散在するエッジ線から目標点検出が可能である。

また、道路上の断片的なエッジ線分を用いて目標軌跡を設定することができる。

また、目標点位置を安定化させるための平滑化処理を備えているため、フレームごとに結果がばらつくことを抑えることができる。

［第２の実施の形態］
次に、本発明の第２の実施の形態について説明する。なお、第１の実施の形態と同様の構成となる部分については、同一符号を付して説明を省略する。

第２の実施の形態では、パーティクルフィルタによる平滑化によって目標軌跡パラメータを算出する処理に替えて、算出された時刻ｔの目標軌跡パラメータの候補と、時刻ｔ−１までの目標軌跡パラメータとの平均を計算して、時刻ｔの目標軌跡パラメータとする点が、第１の実施の形態と異なっている。

第２の実施の形態の平滑化部２４は、目標軌跡算出部２２によって算出された時刻ｔの目標軌跡パラメータの候補と、時刻ｔ−１までの目標軌跡パラメータとの平均を計算し、時刻ｔの目標軌跡パラメータとする。

具体的には、平滑化部２４は、最近Ｍフレームの目標軌跡パラメータを、以下の式（７）に示すように平均化する。なお、以下の式（７）におけるｆ［ｔ］を現在（時刻ｔ）フレームの目標軌跡パラメータの候補とし、ｆ［ｔ−ｍ］を前（時刻ｔ−ｍ）フレームの目標軌跡パラメータとし、ｆ’［ｔ］を時刻ｔの目標軌跡パラメータとする。

なお、第２の実施の形態に係る目標軌跡算出装置の構成及び作用については、第１の実施の形態と同様であるため、説明を省略する。

なお、上記の第２の形態では、算出された時刻ｔの目標軌跡パラメータの候補と、時刻ｔ−１までの目標軌跡パラメータとの平均を計算して、時刻ｔの目標軌跡パラメータとする場合を例に説明したが、これに限定されるものではない。例えば、以下の式（８）に示すように、算出された時刻ｔの目標軌跡パラメータの候補と、時刻ｔ−１の目標軌跡パラメータとの重み付き平均を計算し、時刻ｔの目標軌跡パラメータとしてもよい。なお、以下の式（８）におけるｆ［ｔ］を現在（時刻ｔ）フレームの目標軌跡パラメータの候補とし、ｆ［ｔ−１］を前（時刻ｔ−１）フレームの目標軌跡パラメータとし、ｆ’［ｔ］を時刻ｔの目標軌跡パラメータとする。また、減衰率αは予め定められた１以下のパラメータとする。

また、上記実施の形態の目標軌跡算出部２２において、多項式ｓ_０（ｚ）としては、例えば、後述する平滑化部２４によって前回算出された時刻ｔ−１の目標軌跡パラメータを用いた目標軌跡を表す多項式ｆ（ｚ）を用いる場合を例に説明したが、これに限定されるものではない。例えば、平滑化部２４によって前回算出された時刻ｔ−１の目標軌跡パラメータを用いた目標軌跡を表す多項式ｆ（ｚ）にｚ＝０を代入した値ｆ（０）を、多項式ｓ_０（ｚ）として用いても良い。
または、平滑化部２４によって前回算出された時刻ｔ−１の目標軌跡パラメータを用いた目標軌跡を表す多項式ｆ（ｚ）にｚ＝０を代入した値ｆ（０）と、前回算出された時刻ｔ−１の目標軌跡パラメータを用いた目標軌跡を表す多項式ｆ（ｚ）との重み付け和を、多項式ｓ_０（ｚ）として用いても良い。

また、上記の実施の形態では、目標軌跡算出部２２が目標軌跡パラメータの候補を算出し、平滑化部２４が目標軌跡パラメータの候補に基づいて、目標軌跡パラメータを算出する場合を例に説明したが、これに限定されるものではない。例えば、目標軌跡算出部２２によって算出された目標軌跡パラメータの候補を、目標軌跡パラメータとしてもよい。

本発明のプログラムは、記憶媒体に格納して提供するようにしてもよい。

１０ 目標軌跡算出装置
１２ 撮像装置
１４ コンピュータ
１６ 画像情報取得部
１８ エッジ検出部
２０ 座標変換部
２２ 目標軌跡算出部
２４ 平滑化部
２６ 目標点検出部
２８ 制御パラメータ算出部
３０ 車両制御装置

Claims (7)

1. 車両の前方画像を取得する画像情報取得手段と、
   前記画像情報取得手段によって取得された前記車両の前方画像から、複数のエッジ線分を検出するエッジ検出手段と、
   前記エッジ検出手段によって検出された前記複数のエッジ線分の各々について、前記エッジ線分を、前記車両の位置を基準とする車両座標系における前記エッジ線分の位置を計算する座標変換手段と、
   前記座標変換手段によって前記複数のエッジ線分の各々について計算された前記車両座標系における前記エッジ線分の位置に基づいて、前記複数のエッジ線分の各々についての、前記車両の左右方向への平行移動量を自由変数とした前記エッジ線分と、前記車両の目標軌跡との差分を最小化するように、前記複数のエッジ線分の各々の前記平行移動量、及び前記車両の目標軌跡を表す多項式のパラメータである目標軌跡パラメータを算出する目標軌跡算出手段と、
   を含む目標軌跡算出装置。
2. 前記画像情報取得手段は、車両の前方画像を各時刻ｔについて取得し、
   前記エッジ検出手段は、各時刻ｔについて、前記画像情報取得手段によって取得された時刻ｔの前記車両の前方画像から、時刻ｔの複数のエッジ線分を検出し、
   前記座標変換手段は、各時刻ｔについて、前記エッジ検出手段によって検出された時刻ｔの前記複数のエッジ線分の各々について、前記エッジ線分を、時刻ｔの前記車両の位置を基準とする車両座標系における前記エッジ線分の位置を計算し、
   前記目標軌跡算出手段は、各時刻ｔについて、前記座標変換手段によって前記複数のエッジ線分の各々について計算された前記車両座標系における前記エッジ線分の位置に基づいて、時刻ｔの前記複数のエッジ線分の各々についての、前記車両の左右方向への平行移動量を自由変数とした前記エッジ線分と、時刻ｔの前記車両の目標軌跡との差分、及び時刻ｔ−１の前記車両の目標軌跡と時刻ｔの前記車両の目標軌跡との差分を最小化するように、前記複数のエッジ線分の各々の前記平行移動量、及び前記目標軌跡パラメータを算出する
   請求項１に記載の目標軌跡算出装置。
3. 前記目標軌跡算出手段によって算出された時刻ｔの前記目標軌跡パラメータと、時刻ｔ−１までの前記目標軌跡パラメータとに基づいて、時刻ｔの前記目標軌跡パラメータを算出する平滑化手段を更に含む、
   請求項２に記載の目標軌跡算出装置。
4. 前記平滑化手段は、時刻ｔ−１の前記目標軌跡パラメータに基づいて、前記目標軌跡パラメータを表すパーティクルを複数生成し、算出された時刻ｔの前記目標軌跡パラメータに基づいて、前記複数のパーティクルの各々に対する尤度を算出し、算出された前記複数のパーティクルの各々に対する前記尤度を用いて、前記複数のパーティクルの各々が表す前記目標軌跡パラメータの重み付け和を、時刻ｔの前記目標軌跡パラメータとして算出する
   請求項３に記載の目標軌跡算出装置。
5. 前記平滑化手段は、算出された時刻ｔの前記目標軌跡パラメータの候補と、時刻ｔ−１の前記目標軌跡パラメータと、予め定められた重みとに基づいて、時刻ｔの前記目標軌跡パラメータの候補と時刻ｔ−１の前記目標軌跡パラメータとの重み付け和を、時刻ｔの前記目標軌跡パラメータとして算出する
   請求項３に記載の目標軌跡算出装置。
6. 算出された前記目標軌跡パラメータに基づいて、前記目標軌跡パラメータを用いた多項式で表わされる前記車両の目標軌跡上から、前記車両の目標位置を検出する目標点検出手段を更に含む、
   請求項１〜請求項５の何れか１項記載の目標軌跡算出装置。
7. コンピュータを、
   車両の前方画像を取得する画像情報取得手段、
   前記画像情報取得手段によって取得された前記車両の前方画像から、複数のエッジ線分を検出するエッジ検出手段、
   前記エッジ検出手段によって検出された前記複数のエッジ線分の各々について、前記エッジ線分を、前記車両の位置を基準とする車両座標系における前記エッジ線分の位置を計算する座標変換手段、及び
   前記座標変換手段によって前記複数のエッジ線分の各々について計算された前記車両座標系における前記エッジ線分の位置に基づいて、前記複数のエッジ線分の各々についての、前記車両の左右方向への平行移動量を自由変数とした前記エッジ線分と、前記車両の目標軌跡との差分を最小化するように、前記複数のエッジ線分の各々の前記平行移動量、及び前記車両の目標軌跡を表す多項式のパラメータである目標軌跡パラメータを算出する目標軌跡算出手段
   として機能させるためのプログラム。