実施の形態1.
以下、図面を参照して本発明の実施の形態について説明する。図1は、本発明の実施の形態1に係る追尾装置10の構成例を示すブロック図である。図1に示す追尾装置10は、追尾部15と、情報記憶部16と、表示部17とを備える。なお、図1には、追尾装置10と接続される計測装置18も併せて示している。以下、追尾装置10と計測装置18とを含むシステムを追尾システム100と呼ぶ場合がある。なお、追尾システム100において、追尾装置10の機能と計測装置18の機能とが1つの装置に具備されていてもよく、その場合は該装置を追尾装置10と呼ぶ。
本実施の形態では、追尾対象の物体として、大きさを無視できない移動体を想定している。したがって、検出された物体の属性には寸法(size)の項目が含まれる。なお、このことは、実際に点と見なせるほど小さい物体の寸法を点程度の値に設定すること、および寸法推定に必要な情報が得られない物体の寸法に不明な旨を付すことを否定するものではない。
計測装置18は、所定の観測者の周囲に存在する物点の位置および速度を計測する装置である。計測装置18は、例えば、レーダー技術を利用して自装置から所定の範囲内に存在する物点の位置およびその物点の速度を計測してその結果を出力する計測装置であってもよい。その場合、所定の観測者は、当該計測装置18または当該計測装置18を搭載した上位装置(車両等)とされる。本実施の形態において、計測装置18における物点の位置および速度の計測方法は特に限定されない。
追尾部15は、計測装置18による物点の位置および速度の計測結果を示す物点情報を取得して、取得された物点情報に基づき、所定の観測者の周囲の物体を検出して、その後追尾する追尾処理を行う処理部である。本例において、追尾部15は、物点情報取得部11と、関連付け部12と、運動状態推定部13と、寸法推定部14とを含む。
物点情報取得部11は、物点情報を取得する。物点情報取得部11は、例えば、計測装置18が計測を行うごとにその結果を示す物点情報を取得してもよいし、追尾に即時性が要求されない場合は複数回の計測結果をまとめた物点情報を取得してもよい。物点情報取得部11が取得した物点情報は、情報記憶部16に記憶される。
物点情報取得部11は、取得した物点情報を追尾装置10内で処理しやすいデータ構造およびデータ形式に変換した上で、情報記憶部16に記憶してもよい。ここで、データ構造の変換には、要素の削除および追加が含まれる。一例として、物点情報取得部11は、取得した物点情報に、各物点の識別子や、物点情報の取得時刻を付した上で、情報記憶部16に記憶してもよい。その際、各物点の識別子に取得時刻を示す情報を含ませることも可能である。取得時刻は、物点情報に各物点の検出時刻を示す情報が含まれていない場合に、検出時刻の代わりとして使用できる。ただし、複数回の検出結果をまとめた物点情報を取得する場合は、該物点情報に各物点の検出時刻を示す情報が含まれているものとする。
物点情報は、検出された各物点について、位置および速度の情報を含む。ここで、物点の位置は、絶対位置に限らず、観測者に対する相対位置でもよい。また、物点の速度は、絶対速度に限らず、観測者に対する相対速度または視線速度でもよい。
物点情報取得部11は、例えば、物点の位置を示す情報として、観測者の位置を原点とする座標系における物点までの距離および方位を示す情報を含む物点情報を取得する。この場合に、物点情報取得部11は、取得した距離および方位を示す情報から物点の相対位置を算出してもよい。そして、物点情報取得部11は、算出した相対位置を示す情報を含む物点情報を情報記憶部16に記憶してもよい。
図2は、情報記憶部16に記憶される物点情報の例を示す説明図である。図2に示す例では、情報記憶部16は、検出時刻に対応づけて、その検出時刻に検出された物点の識別子、位置および速度の情報を含む物点情報を記憶する。以下、検出時刻をt、物点の識別子をpid、物点の位置(検出位置)をdp、物体の速度(検出速度)をdvと表す場合がある。
計測装置18によってある物点の位置および速度が計測されると、その検出時刻において、その物点が属する物体(より具体的には、そのうちのある部位)が少なくともその位置に存在し、かつその位置においてその速度を有していたことがわかる。したがって、計測装置18の計測処理は物体の検出処理でもある。ただし、物点はあくまで物体上の任意の点でしかないため、物点を個々に検出するだけでは、その物点が属する物体全体としての位置およびその変化並びに寸法を把握できない。以下、計測装置18による計測処理を「物体全体の検出処理」と区別するために、「物点検出処理」と呼ぶ場合がある。
本実施の形態では、検出された各物点と関連付けられる物体を検出するだけでなく、異なる時点に検出された物点をその所属先物体の同一性に基づき同じ物体と関連付けることにより、検出済みの物体について検出時刻ごとの所属物点を定める。そして、このような関連付けの結果示される物点と物体との対応関係(より具体的には、検出済みの物体の検出時刻ごとの所属物点の情報)を利用して、一度検出された物体について、その位置を含む運動状態および寸法の高精度の推定を可能にする。以下、ある物体に対してその物体に属する物点を「所属物点」と呼ぶ場合がある。また、ある物点に対してその物点が属する物体を「所属先物体」と呼ぶ場合がある。
なお、複数の時点にまたがる物点同士の所属先物体の同一性判定において、判定対象とする一方の物体を、所属先が未知の物点のみが属すると仮定した仮の物体とすることも可能である。換言すると、同一性判定における所属先物体は、判定時点において検出済みの物体に限定されない。例えば、新たに検出された物点に対して検出済みの物体との間で同一性判定を行った結果、いずれの物体とも異なると判定された場合、該物点の所属先物体として新たな物体が定義される。このように、物点と物体の関連付け処理は、物点の所属先物体の決定処理ということもできる。
なお、物点と物体の関連付け結果(所属関係)は固定的なものではなく、変更される場合もある。また、同じ時刻に検出された複数の物点が同じ1つの物体に関連付けられる場合もある。なお、その場合において、その物体の所属物点としてそれら複数の物点を統合した新たな物点(代表物点)を定義することも可能である。
本実施の形態において、物体は、一次元の領域(線)、二次元の領域(面)または三次元の領域(立体)として表現されうる。したがって、物体の位置(推定位置)を示す情報は、線、面または立体としての物体の位置を特定可能な情報であると好ましい。また、物体の寸法を示す情報は、線、面または立体としての物体の寸法を特定可能な情報であると好ましい。
物体の位置を示す情報は、例えば、所定の2次元座標系または3次元座標系の位置を示す情報であってもよい。ここで、位置は、代表位置(代表点の位置)でもよい。その場合、線、面または立体としての物体の位置(物体の占有場所)は、物体の代表位置と寸法とを用いて表現される。
物体の寸法を示す情報は、例えば、所定の2次元座標系または3次元座標系の領域、もしくはそれら座標系の各軸上での長さを示す情報であってもよい。ここで、長さを示す情報は、全長値でもよいし、代表位置に対するオフセットでもよい。また、領域を示す情報は、外縁の位置を示す情報の集合または外縁の辺の長さを示す情報の集合でもよい。
なお、物体の寸法を示す情報は、予め定められた物体形状に対応した値でもよい。例えば、追尾装置10は、表示上の物体形状(直線状、矩形状または立方体形状等)を予め定めておき、該物体形状を所定の座標系で表現したものを物体の寸法として用いることも可能である。物体形状は1つに限定されず、複数パターンを定めることも可能である。なお、追尾装置10は、寸法の推定結果に応じて物体形状を定めることも可能である。例えば、直交する3方向に広がりを有する寸法が推定された場合は立方体形状としてもよい。また、例えば、直交する2方向にのみ広がりを有する寸法が推定された場合は、面形状としてもよい。また、例えば、一方向にのみ広がりを有する寸法が推定された場合は線形状としてもよい。そして、上記以外の場合は点形状としてもよい。
関連付け部12は、物点と物体を関連付ける関連付け処理を行う。関連付け部12は、取得された物点情報に基づいて、物点と物体を関連付ける。その際、関連付け部12は、検出済みの物体の情報を利用して、異なる時点に検出された物点を同じ物体に関連付ける。関連付け部12は、例えば、2つの時点で物点が検出された場合、一方の時点に検出された物点の所属先物体の情報(寸法を含む)を利用して、他方の時点に検出された物点をその所属先物体に関連付けてもよい。これにより、一方の時点において検出された物点(検出済みの物体の所属物点)と、他方の時点に検出された物点とがそれぞれ同じ1つの物体に関連付けられる。ここで、所属先物体の情報は、関連付けの対象とされる他方の時点に対して直近の過去の時点における該物体の運動状態と、該物体の最新の寸法とであってもよい。なお、最新の寸法は、上記の2つの時点に関わらず、関連付け処理を行う時点において最新の推定値とされる寸法であってもよい。
関連付け部12は、例えば、第1の時点において検出された物点を対象物点とし、第1の時点とは異なる第2の時点において検出済みの物体を判定物体として、対象物点の所属先物体が判定物体と同一か否かを判定することにより、対象物点を判定物体と関連付けてもよい。その場合において、関連付け部12は、例えば、対象物点の所属先物体と判定物体とが同一と判定された場合、対象物点と判定物体とを対応付けて情報記憶部16に記憶する。例えば、関連付け部12は、対象物点の情報を、判定物体の判定時点における所属物点の情報として情報記憶部16に記憶してもよい。これにより、対象物点と、判定物体の第1の時点以外の時点における所属物点とが、同一の判定物体の所属物点として関連付けられる。
また、関連付け部12は、ある対象物点に対して判定物体とされるすべての物体との間で同一性判定を行った結果、いずれの物体とも同一でないと判定された場合、該対象物点の所属先物体として新たな物体を定義して、対象物点とその新たな物体とを関連付けてもよい。その場合、この時点において、対象物点はいずれの物点とも関連付けられないが、時を経て新たに物点が検出されれば、その時点の物点と関連付けられうる。
関連付け処理のタイミングとして、関連付け部12は、新たな物点が検出されると、検出された物点を対象物点として関連付け処理を行ってもよい。その場合において、関連付け部12は、対象物点の所属先物体が、対象物点の検出時刻である第1の時点よりも過去の時点である第2の時点において検出済みの物体(判定物体)と同一か否かを判定してもよい。なお、本例は、第1の時点が最新の時点であり、第2の時点が過去の時点であるが、第1の時点および第2の時点はこの限りではない。以下、関連付け処理において、同一性の判定対象とされる物点(対象物点)の検出時刻に相当する第1の時点を「判定時点」といい、対象物点との間で同一性を判定される判定物体の運動状態の参照先に相当する第2の時点を「参照時点」という場合がある。
関連付け部12は、所属先物体の同一性を判定する際に、それら物体について現時点(実際に処理を行う時点)までに得られた情報を利用できる。例えば、判定対象の一方の物体の情報として、該物体の最新の検出時刻までの所属物点の情報(位置および速度等)を利用できる。また、例えば、判定対象の一方の物体の情報として、該物体の最新の検出時刻までの運動状態(位置、速度および加速度等)および寸法を利用できる。例えば、所属物点の情報および物体の運動状態として、対象とした時点(上記の判定時点または参照時点)の情報だけでなく、他の時点の情報を利用できる。また、物体の寸法として、後述する寸法推定部14によって推定された寸法を利用できる。なお、寸法についても、対象とした時点において推定された寸法に限らず、他の時点において推定された寸法を利用できる。
関連付け部12は、このような情報に基づき、判定物体の任意の部位が、判定時点において対象物点の位置に存在する蓋然性が高いと判断される場合に、対象物点の所属先物体と判定物体とは同一物体であると判定してもよい。
図3は、関連付け部12による関連付け処理のより詳細な処理フローの一例を示すフローチャートである。図3に示す例では、まず、関連付け部12は、ある対象物点に対して、所属先物体の同一性判定の対象とする物体(判定物体)を1つ選択する(ステップS101)。判定物体は、例えば、対象物点の検出時刻に相当する第1の時点(判定時点)よりも過去の所定期間内において検出済みの物体のいずれかであってもよい。その場合において、参照時点は、判定物体の検出時刻において判定時点より過去の直近の検出時刻とされる。
次に、関連付け部12は、判定物体の運動状態(少なくとも位置と速度とを含む)と判定物体の寸法とに基づいて、判定物体が判定時点において存在すると予測される位置の範囲を関連付け範囲として決定する(ステップS102)。例えば、関連付け部12は、少なくとも判定物体の寸法に基づいて、関連付け範囲の大きさを決定するとともに、判定物体の少なくとも参照時点における運動状態に基づいて、関連付け範囲の中心位置を決定してもよい。
関連付け範囲を決定すると、関連付け部12は、判定時点において対象物点が関連付け範囲内に存在しているか否かを判定する(ステップS103)。
対象物点が関連付け範囲内に存在している場合(ステップS103のYes)、関連付け部12は、対象物点の所属先物体と判定物体とは同一の物体であるとして、対象物点を判定物体の判定時点における所属物点の1つとする(ステップS104、S105)。関連付け部12は、例えば、対象物点の情報を、判定物体の判定時点における所属物点の1つとして情報記憶部16に記憶する。
一方、対象物点が関連付け範囲内に存在していない場合(ステップS103のNo)、関連付け部12は、対象物点の所属先物体と判定物体は非同一の物体であるとして、対象物点を判定物体の判定時点における所属物点から除外する(ステップS106,S107)。関連付け部12は、例えば、判定物体の判定時点における所属物点の1つとして対象物点の情報が情報記憶部16に記憶されている場合、その情報を削除してもよい。
このような関連付け処理を、1つの対象物点につき、判定物体として選択されうるすべての物体に対して行う。例えば、関連付け部12は、対象物点に対して、すべての判定物体との間で同一性判定を行ったか否かを判定する(ステップS108)。すべての判定物体との間で同一性判定が完了すると、当該対象物点に対する関連付け処理を終了する(ステップS108のYes)。すべての判定物体との間で同一性判定が完了していなければ(ステップS108のNo)、ステップS101に戻って新たな判定物体を選択すればよい。なお、上記の例は、1つの対象物点に着目した場合の関連付け処理の例であるが、関連付け処理では、例えば、所定期間内において検出されたすべての物体を判定物体として、各判定物体が判定時点において存在すると予測される位置の範囲を関連付け範囲として決定し、それらの範囲内に、判定時点に検出された物点が含まれるか否かを判定することで、判定時点に検出された各物点の所属先物体を決定してもよい。
なお、このような関連付け処理の結果、1つの物点が複数の物体の所属物点であると判定されることもありうる。そのような場合、関連付け部12は、それらすべての物体(所属先候補の物体)を対象物点の所属先物体としてもよいし、所定の条件に基づいてそれら物体のうちの一部の物体またはいずれか1つの物体を所属先物体としてもよい。複数の所属先候補の物体の中から所属先物体を決定する方法としては、例えば、対象物点の速度の情報をさらに利用して、所属先候補の物体と対象物点との間で判定時点における運動状態の類似度を算出し、その類似に基づき、決定する方法が挙げられる。
なお、関連付け部12は、関連付け処理で、ある対象物点の所属先物体がいずれの判定物体とも同一でないと判定された場合、新たな物体を定義して、対象物点をその物体の判定時点における所属物点の1つとしてもよい。このとき、関連付け部12は、当該物体の寸法として、予め定められた寸法(初期値)を設定してもよい。
関連付け部12による関連付け処理の結果は、例えば、情報記憶部16に記憶される物体情報に反映される。物体情報は、例えば、検出された物体ごとに、識別子と、寸法と、所属物点の履歴と、運動状態の履歴とを含む情報であってもよい。以下、ある項目(item/items)について履歴といった場合、少なくとも1つの時点におけるその項目の情報を含むものとする。なお、履歴は、2以上の時点におけるその項目の情報を含んでいてもよい。
所属物点の履歴は、例えば、検出時刻ごとの所属物点の情報であってもよい。また、運動状態の履歴は、検出時刻ごとの運動状態の情報であってもよい。また、所属物点の情報は、例えば、該当する物点の識別子など、物点情報への参照であってもよい。ここで、物体の検出時刻は、その物体の所属物点の検出時刻としてもよい。なお、その物体の所属物点が検出されない場合であっても、所定の追尾条件を満たしている間はその物体が観測者の周囲に存在しているとみなすことも可能である。その場合、その物体の所属物点が検出されていない場合であっても、物点検出処理により他の物点が検出されている間、その物点検出処理が行われた時刻(他の物点の検出時刻)を、各物体の検出時刻としてもよい。なお、その場合においても、その前後の時点における所属物点の情報等から当該検出時刻における運動状態が推定できた場合には、運動状態の履歴として当該検出時刻における運動状態の情報を登録してもよい。
所属物点および運動状態の履歴は、例えば、その物体の追尾中の間すなわちその物体が追尾条件を満たさなくなるまでの間、保持される。なお、メモリ容量の関係で、追尾中の期間のうち最新の時点から所定数分の時点の情報のみが保持されてもよい。追尾条件は、例えば、一度検出された後、所属物点が未検出の状態が所定の時間継続されないこと、等であってもよい。関連付け部12は、例えば、情報記憶部16に記憶されている物点情報と、情報記憶部16に記憶されている物体情報とを参照して、関連付け処理を行ってもよい。
物体情報における運動状態は、少なくとも位置を含む。例えば、位置の履歴から必要に応じて速度および加速度を算出する場合、物体情報は、位置のみを含む運動状態を保持する態様であってもよい。物体情報における運動状態は、位置と速度とを含んでいてもよく、さらに加速度を含んでいてもよい。また、物体情報は、1つの物体について、所属物点が新たに検出されたタイミングでその検出時刻における運動状態を逐次推定したときの運動状態(平滑化前の運動状態)と、そのような逐次推定した運動状態を基に平滑化した後の運動状態とを含んでいてもよい。なお、平滑化については後述する。また、物体情報は、上記以外にも、物体の運動状態および寸法の推定に必要な情報(例えば、各検出時刻における運動状態の標準偏差、誤差共分散等)を含んでいてもよい。
図4は、情報記憶部16に記憶される物体情報の例を示す説明図である。図4に示す例では、情報記憶部16は、物体の識別子と対応づけて、その物体の検出時刻ごとの所属物点の情報と、逐次推定された運動状態と、平滑化後の運動状態と、最新の寸法とを含む物体情報を記憶する。以下、物体の識別子をoid、ある時点の所属物点の情報をbp(所属物点の識別子pidを要素に持つ集合)、逐次推定された運動状態をem、平滑化後の運動状態をem’、寸法をsと表す場合がある。なお、記号に“’”を付すことによって平滑化後であることを表している(以下同様)。
なお、関連付け部12は、新たに物点が検出されたタイミングだけでなく、任意のタイミングで、関連付け処理を行うことができる。その場合、関連付け処理で、既に行われた関連付け結果の更新(検出済みの物体の分割および統合を含む)を行ってもよい。タイミングの例としては、過去の関連付け処理で用いた物体の情報が更新(要素の追加および削除、並びに値の変更を含む)されたとき、ユーザからの指示があったとき、一定時間が経過するごと、物点検出処理が所定の回数行われるごと、などが挙げられる。この場合における対象物点は、情報の更新を条件にする場合は更新前の情報を用いて関連付けを行ったすべての物点であってもよい。また、ユーザからの指示を条件にする場合はユーザから指示された物点であってもよい。また、周期を条件にする場合は現時点で情報を保持しているすべての物点であってもよいし、直近の一周期において追加された物点であってもよい。
また、関連付け部12は、例えば、物点を検出する以外の方法による物体の検出結果を取得可能な場合、該検出結果を利用して、関連付け結果を更新してもよい。該検出結果の例としては、観測者の周囲を撮影した画像等を用いた物体の検出結果が挙げられる。なお、該検出結果には、物体の検出結果に基づいて判別された属性の情報が含まれていてもよい。
関連付け部12は、関連付け処理において、参照時点を、判定時点よりも未来の時点に設定することも可能である。すなわち、関連付け部12は、対象物点の所属先物体の判定先として、対象物点の検出時刻である第1の時点(判定時点)よりも未来の時点に相当する第2の時点(参照時点)において検出済みの物体を選定できる。その場合、物体の運動状態を時系列上の逆向き(時間を遡る向き)に予測して、関連付け範囲を決定すればよい。なお、その場合においても、関連付け範囲の大きさは、判定物体の寸法(例えば、直近に推定された最新の寸法)に応じて変更されうる。
運動状態推定部13は、関連付け部12による関連付けの結果示される物点と物体との対応関係に基づいて、検出済みの物体の検出時刻ごとの運動状態であって位置を含む運動状態を推定する運動状態推定処理を行う。運動状態推定処理で推定する運動状態は、少なくとも位置を含む。なお、該運動状態は、位置と速度とを含んでいてもよく、さらに加速度を含んでいてもよい。以下、運動状態推定処理において、推定対象とされる物体を「対象物体」という。
運動状態推定部13は、例えば、新たに物点が検出されると、その物点と関連付けられた物体の少なくとも最新の検出時刻における運動状態を推定してもよい。これにより、逐次、運動状態が推定される。運動状態推定部13は、例えば、物点情報取得部11により取得された物点情報で示されるすべての物点の所属先物体の決定を受けて、決定された各所属先物体を対象物体として、各対象物体の運動状態を推定してもよい。推定対象とする運動状態は、最新の検出時刻における運動状態のみであってもよいし、最新の検出時刻を含むこれまでの各検出時刻における運動状態であってもよい。推定された運動状態は、対象物体に関する物体情報において運動状態の履歴として保持される。なお、運動状態推定部13は、最新の検出時刻における運動状態を第1の推定値(平滑化前の運動状態)として推定した上で、最新の検出時刻を含むこれまでの各検出時刻における第1の推定値を基に、最新の検出時刻を含むこれまでの各検出時刻における第2の推定値(平滑化された運動状態)を推定してもよい。その場合、運動状態推定部13は、対象時点を指定した運動状態推定処理を複数回実行してもよいし、最新の検出時刻の運動状態を推定する運動状態推定処理で、最新の検出時刻の運動状態と平滑化後の各検出時刻の運動状態を推定してもよい。
なお、運動状態推定部13は、新たに検出された物点の所属先物体に限らず、新たに物点が検出された時点において追尾中のすべての物体を対象物体としてもよい。これにより、追尾中の物体に新たな所属物点が検出されなかったとしても、その物体の運動状態を更新できる。その場合、推定結果とされるその物体の最新の検出時刻における運動状態は、仮の運動状態としてもよい。以下、運動状態推定処理において運動状態の推定対象とされる時点(上記の例では、最新の検出時刻)を対象時点という場合がある。
例えば、運動状態推定部13が、新たに検出された物体(対象物体)の最新の検出時刻(対象時点)の運動状態を推定することを考える。このとき、運動状態推定部13は、対象物体の対象時点における所属物点の情報のみに基づいて、対象時点の運動状態を推定してもよいが、対象物体について現時点までに得られている情報に基づいて、対象時点の運動状態を推定してもよい。対象物体について現時点までに得られている情報の例としては、対象物体の寸法、対象物体の対象時点以外の時点における所属物点の情報および運動状態、並びにそれらから得られる情報(運動状態の標準偏差、誤差分散)などが挙げられる。
また、運動状態推定部13は、新たに物点が検出されたタイミングに限らず、任意のタイミングで物体の運動状態を推定(再推定を含む)することも可能である。タイミングの例としては、過去の運動状態推定処理で用いた情報が更新されたとき、ユーザからの指示があったとき、一定時間が経過するごと、物点検出処理が所定の回数行われるごと、などが挙げられる。この場合における対象物体および対象時点は、情報の更新を条件にする場合は更新前の情報を用いて推定を行ったすべての物体およびすべての時点であってもよい。また、ユーザからの指示を条件にする場合はユーザから指示された物体および時点であってもよい。また、周期を条件にする場合は現時点で情報が保持されているすべての物体およびその周期中における各検出時刻であってもよいし、直近の一周期に所属物点が検出されたすべての物体およびその周期中における各検出時刻であってもよい。
また、運動状態推定部13は、最新の検出時刻以外の運動状態を推定する場合に、対象時点からみて現在および過去の時点における対象物体の情報だけでなく、未来の時点における対象物体の情報を利用して対象時点の運動状態を推定してもよい。これには、過去の検出時刻における運動状態を推定する際に、その前後の検出時刻における運動状態を利用することや、最新の寸法を利用することが含まれる。
既に説明したように、運動状態推定部13は、対象物体の運動状態を推定する際、平滑化された運動状態を推定してもよい。例えば、運動状態推定部13は、対象物体について現時点までに得られた情報に基づき、対象時点の運動状態として、その前後の時点の運動状態からの変化が滑らかになるような運動状態を推定してもよい。このとき、運動状態推定部13は、対象物体の寸法を利用して運動状態を推定してもよい。一例として、運動状態推定部13は、物体の寸法以下の位置ずれを許容するように、各検出時刻の運動状態を推定することができる。運動状態推定部13は、対象物体の寸法以下の位置の変化を物体内での反射点の移り変わりとみなして、推定誤差として扱わないようにして、運動状態を推定してもよい。
寸法推定部14は、検出済みの物体の寸法を推定する寸法推定処理を行う。以下、寸法推定処理において、推定対象とされる物体を「対象物体」という。寸法推定部14は、例えば、対象物体に関連付けられた物点(所属物点)の情報に基づき、該物体の寸法を推定する。
寸法推定部14は、例えば、対象物体の所属物点の履歴を利用して、対象物体の寸法を推定することができる。寸法推定部14は、対象物体についてこれまでに得られた所属物点の情報を統計処理して、対象物体の寸法を推定してもよい。寸法推定部14は、例えば、対象物体の所属物点の履歴により示される、検出時刻ごとの所属物点の検出位置のばらつきを算出し、算出されたばらつきに基づいて対象物体の寸法を推定してもよい。ここで、所属物点の検出位置のばらつきは、所属先物体である対象物体の推定位置(この場合、代表位置)に対する所属物点の検出位置のばらつきであってもよい。一例として、寸法推定部14は、検出済みの物体について、各検出時刻における物体の位置(代表位置)から各所属物点の位置(検出位置)までの距離を求め、求めた距離に基づいて物体の寸法を推定してもよい。
また、寸法推定部14は、対象物体について十分な(例えば、2以上の時点にわたる)履歴がない場合には、予め決められた寸法(初期値)を設定してもよい。例えば、寸法推定部14は、新たに物体Aが検出された場合、物体Aの寸法を予め決められた値に設定する。その場合、当該値は仮の寸法として情報記憶部16に保持されてもよい。そして、寸法推定部14は、物体Aに新たな物点が紐づけられた結果十分な履歴が保持された場合に、上記のばらつきを利用して物体Aの寸法を推定してもよい。なお、寸法推定部14以外の処理部(例えば、関連付け部12)が初期値を設定してもよい。
寸法推定部14は、新たに物点が検出されたタイミングに限らず、任意のタイミングで寸法を推定することも可能である。タイミングの例としては、過去の寸法推定処理で用いた情報が更新されたとき、ユーザからの指示があったとき、一定時間が経過するごと、物点検出処理が所定の回数行われるごと、などが挙げられる。この場合における対象物体は、情報の更新を条件にする場合は更新前の情報を用いて推定を行ったすべての物体であってもよい。また、ユーザからの指示を条件にする場合はユーザから指示された物体であってもよい。また、周期を条件にする場合は現時点で情報を保持しているすべての物体であってもよいし、直近の一周期において所属物点が検出された物体であってもよい。
表示部17は、追尾部15による処理の結果得られた情報に基づき、追尾結果を表示する。追尾結果には、検出された物体の検出時刻ごとの位置または移動の軌跡が含まれる。なお、検出結果は、検出時刻が1つしかない場合または検出された物体の位置が変化していない場合は、最新の位置のみでもよい。また、表示部17は、物体の寸法を含む追尾結果を表示してもよい。
表示部17は、例えば、追尾情報として、追尾部15により検出された周囲の物体の移動の軌跡を、検出時刻ごとの位置および該物体の寸法がわかる態様で表示する。例えば、表示部17は、追尾中の物体について、所定の座標系に検出時刻ごとの位置をプロットするとともに、該位置を時系列順に接続して移動の軌跡を表示してもよい。このとき、表示部17は、最新の位置において、その物体の寸法に応じて大きさが調整された表示上の形状(外縁形状)を重畳して表示してもよい。また、表示部17は、所定の座標系に物体の位置に加えてその物体の所属物点の検出位置をプロットした上で、移動の軌跡を表示することも可能である。表示部17は、このような表示を、例えば、物点検出処理が行われるごとに内容を更新しながら行ってもよい。
次に、本実施の形態に係る追尾装置10の全体動作を説明する。図5は、本実施の形態に係る追尾装置10の動作の一例を示すフローチャートである。図5に示す例では、まず、物点情報取得部11が、物点情報を取得する(ステップS11)。物点情報取得部11は、例えば、計測装置18から出力される物点情報を取得する。本例では、観測者は、計測装置18または計測装置18を搭載した上位装置(車両等)とする。計測装置18は、例えば、自装置周辺に存在する物点の位置および速度を計測し、その結果を出力する。計測装置18における自装置周辺の物点の位置および速度の計測方法は特に問わない。物点情報取得部11により取得された物点情報は、必要に応じて物点の識別子や物点の検出時刻とされる取得時刻が付与されて情報記憶部16に記憶される。
次いで、関連付け部12が、ステップS11で取得された物点情報に基づいて、関連付け処理を行う(ステップS12)。関連付け部12は、物点情報が示す物点の各々を対象物点として、関連付け処理を行ってもよい。ここでの関連付け結果は、情報記憶部16に記憶されている物体情報に反映される。
次いで、運動状態推定部13が、ステップS12の関連付け処理の結果に基づいて、運動状態推定処理を行う(ステップS13)。運動状態推定部13は、関連付けの結果示される物点と物体との対応関係に基づいて、検出済みの物体の少なくとも最新の検出時刻を含む時刻の運動状態であって物体の位置を含む運動状態を推定してもよい。ここでの運動状態の推定結果は、情報記憶部16に記憶されている物体情報に反映される。
次いで、寸法推定部14が、ステップS12の関連付け処理の結果およびステップS13の運動状態の推定結果に基づいて、寸法推定処理を行う(ステップS14)。寸法推定部14は、検出済みの物体について、各検出時刻における当該物体の位置から各所属物点の位置までの距離に基づいて、検出済みの物体の寸法を推定してもよい。ここでの寸法の推定結果は、情報記憶部16に記憶されている物体情報に反映される。
最後に、表示部17が、ステップS11〜ステップS14までの一連の処理の結果を含むこれまでに得られた情報に基づき、追尾結果を表示する(ステップS15)。
ステップS15の後は、新たな物点情報の取得待ちをするため再びステップS11に戻る。以降、物点情報が取得されるごとに、ステップS11〜ステップS15の処理を繰り返してもよい。
なお、ステップS15の前に、運動状態推定部13が、これまでに得られた情報を利用して、運動状態の再推定(平滑化等)を行ってもよい。運動状態推定部13は、例えば、物体情報に登録されている物体の各々を対象物体、該対象物体について履歴で保持されている検出時刻のいずれかまたはすべてを対象時点として、運動状態の再推定を行う。なお、当該処理は、上記一連の処理とは独立して行うことも可能である。
また、ステップS14の処理を、新たな物点情報の取得に伴う一連の処理から除外することも可能である。その場合において、追尾装置10は、ステップS14の処理を上記の一連の処理とは独立して行ってもよい。
以上のように、本実施の形態によれば、点と見なせない任意の物体の追尾を精度よく追尾できる。例えば、本実施の形態では、異なる時点に検出された2以上の物点を関連付ける際に、検出済みの物体の情報、特に寸法を利用する。これにより、1つの物体から複数の物点が検出されたり、検出される物点の物体内での位置が変化したりするような状況であっても、検出された物点を、検出済みの物体と精度よく関連付けることができる。
また、本実施の形態によれば、そのようにして得られる関連付けの結果に基づいて、検出済みの物体の運動状態(少なくとも位置を含む)および寸法を推定することにより、点と見なせない任意の物体が検出されても、その物体の位置および寸法を精度よく得ることができる。
また、本実施の形態では、例えば、そのようにして推定された物体の情報を、次の関連付け処理で利用する、またその関連付けの結果を物体の情報の推定または再推定に利用するなど、都度更新される情報を利用して関連付け精度および推定精度を向上させるよう構成できる。これにより、継続的な物点の検出処理を行うことで、より精度の高い追尾が可能になる。
また、本実施の形態では、例えば、そのような関連付け結果を利用して運動状態等を再推定する際に、推定対象とされた時点よりも未来の時点の情報を利用して平滑化するよう構成できるので、1つの物体から複数の物点が検出されたり、検出される物点の物体内での位置が変化したりするような状況であっても、推定誤差を低減できる。
また、本実施の形態では、例えば、画像認識など、物点の検出結果以外の方法によって得られる物体の情報を利用して物体を分離または統合するなど関連付け結果を見直すようにも構成できる。したがって、複数の物体が近接したのち離れるような状況であっても各物体を適切に分離/統合できるので、物体をさらに精度よく追尾することができる。
実施の形態2.
次に、本発明の実施の形態2に係る追尾装置について説明する。実施の形態2に係る追尾装置は、実施の形態1に係る追尾装置10を、車両の周囲の物体の追尾に適用した例である。本例において、本実施の形態に係る追尾装置20は、観測者である車両に搭載されているものとする。以下、追尾装置20が搭載された車両を自車両と呼ぶ。なお、本例は、車両が、上記の計測装置18に相当する計測手段21を備えることにより観測者とされる例である。追尾装置20の、計測手段21以外の構成要素の一部または全部は、車両以外に搭載されていてもよい。すなわち、追尾装置20の各構成要素の一部または全部は、複数の情報処理装置や回路等により実現されてもよく、その場合において、複数の情報処理装置や回路等は、集中配置されていてもよいし、分散配置されていてもよい。分散配置される場合、少なくとも計測手段21が車両に搭載されていればよい。その場合において、計測手段21が搭載された車両を自車両という。なお、運転中に追尾結果を表示したい場合などは、計測手段21と、上記の表示部17に対応する情報出力手段27とが車両に搭載されていることが好ましい。
図6および図7は、追尾方法の概略を示す説明図である。まず、図6および図7を参照しながら本実施の形態における物体の追尾方法について説明する。本例において、追尾対象とされる物体は2次元のxy平面上に存在するものとする。
図中において、白塗りの丸(符号DP1,DP2)は、検出された物点の位置を表す。また、黒塗りの菱形(符号P1,P2)は、検出済みの物体の推定位置を表す。なお、黒塗りの菱形のうち破線で示されるもの(符号α)は、検出済みの物体の1つ前の時刻における運動状態から予測されるその物体の1つ後の時刻における位置(予測位置)を表す。また、物点の検出位置から延びる矢印(例えば、符号DV2)はその物点の検出速度(本例では、視線速度)を表し、物体の推定位置から延びる矢印(例えば、符号V1)はその物体の推定速度を表す。また、物体の推定位置を囲む破線の四角形(例えば、符号S1,S2)は、その物体の寸法を表す。また、符号DR2は、符号DP2で示される物点の観測者を基準にした距離(検出距離)、符号Dθ2は、符号DP2で示される物点の観測者を基準にした方位角を表している。また、物体の位置をつなぐ破線(符号TR1)は、検出済みの物体の推定位置の軌跡を表している。
また、黒塗りの四角形(図7における符号P’1,P’2)は、検出済みの物体の平滑化後の位置(推定位置)を表す。なお、図7では、符号に”’”を付して平滑化後の推定値であることを表している。例えば、平滑化後の物体の位置をつなぐ破線(符号TR’1)は、検出済みの物体の平滑化後の推定位置の軌跡を表している。
以下では、符号DP1およびDV1が、検出済みの物体Jの時刻tk−1における所属物点の1つである物点jの位置dp(k−1,j)および視線速度dv(k−1,j)を表すものとする。また、符号DP2およびDV2が、時刻tkにおいて検出された物点mの位置dp(k,m)および視線速度dv(k,m)を表すものとする。以下、時刻tにおいて検出された物点xx(pid_xxの物点)に関する情報を、その情報の右肩に付(t,xx)によって識別する。ここで、tは任意の時刻、xxは検出された任意の物点を指す。
また、符号P1およびV1が、検出済みの物体Jの時刻tk−1における推定位置p(k−1,J)および推定速度v(k−1,J)を表すものとする。また、符号P2およびV2が、物体Jの時刻tk−1における推定位置p(k,J)および推定速度v(k,J)を表すものとする。また、符号S1およびS2が時刻tk−1において推定された物体Jの寸法および時刻tk−1において推定された物体Jの寸法を表すものとする。以下、時刻tにおいて検出済みの物体YY(oid_YYの物体)に関する情報を、その情報の右肩に付す(YY),(t,YY)または(t|t’,YY)によって識別する。ここで、YYは検出済みの任意の物体を指す。また、t’は、その情報の推定に用いた情報のうち最新の時点の情報を表す。
一例として、時刻tk−1における物体Jの情報を用いて時刻tkにおける物体Jの位置を推定した場合、その推定位置はp(k|k−1,J)と表される。なお、t’は省略可能である。t’が省略された場合、どの情報を用いて推定したかを問わず、時刻tにおいて算出された推定値を表す。また、寸法に関しては、tも省略可能である。その場合、どの情報を用いていつ推定されたかを問わず、その時点の最新の値を表す。
今、時刻tkにおいてNk個の物点が検出されたとする。本例では、物点情報として、検出された各物点について、観測者(自車両)を基準にした当該物点までの距離dr、方位角dθおよび視線速度dvrが取得される。
時刻tkにおける物点mの相対位置dp(k,m)は、dr(k,m)およびdθ(k,m)から式(1)のように表される。ここで、dpxは観測者に対する物点の相対位置のうち横方向(左右方向)の成分に相当するx成分、dpyは観測者に対する物点の相対位置のうち進行方向(前後方向)の成分に相当するy成分である。
また、本例では、時刻tk−1において、検出済みの物体Jの時刻tk−1における運動状態の第1の推定結果、すなわち位置p(k−1|k−1,J)、速度v(k−1|k−1,J)および加速度a(k−1|k−1,J)、並びに寸法s(k−1,J)が得られているものとする。本例において、位置pおよび平滑化後の位置p’はいずれも代表位置とする。なお、第1の推定結果が、平滑化前の逐次推定される運動状態に相当する。また、後述する第2の推定結果が、平滑化後の運動状態に相当する。
物体Jの時刻tk−1における運動状態の第1の推定結果とされる位置p(k−1|k−1,J)、速度v(k−1|k−1,J)および加速度a(k−1|k−1,J)はそれぞれ、式(2a)、式(2b)および式(2c)のように表される。ここで、pxは物体のx軸方向の位置成分、pyは物体のy軸方向の位置成分である。vxは物体のx軸方向の速度成分、vyは物体のy軸方向の速度成分である。axは物体のx軸方向の加速度成分、ayは物体のy軸方向の加速度成分である。
また、時刻tk−1において推定される物体Jの寸法s(k−1,J)は式(3)のように表される。ここで、sxは物体の幅(x軸方向の長さ)、syは物体の奥行(y軸方向の長さ)である。なお、式(3)では、右肩に推定時刻と識別子の情報とを付しているが、推定時刻を問わない場合は寸法をs(YY)と表わしてもよいことは既に説明したとおりである。
このような状況において、時刻tkに検出した物点mを、時刻tk−1において検出済みの物体J(またはその所属物点j∈bp(k−1、J))と関連付ける方法としては、例えば次のような方法を使用できる。まず、物体Jの時刻tkにおける位置p(k|k−1,J)は、物体Jの時刻tk−1における位置p(k−1,J)および速度v(k−1,J)に基づき、式(4)のように予測できる。ここで、Δtはtk−tk−1である。なお、Δtは、物点検出処理が周期的に行われる場合、物点検出処理の時間間隔に相当する。
次いで、物体Jの寸法s(k−1,J)を考慮し、式(5)に示す評価条件を満たしている場合に、時刻tkに検出された物点mを、物体Jと関連付ける。ここで、|p|はpの要素毎の絶対値を表し、ηは事前に設定する関連付け範囲を調整するベクトルである。
このように、本例では、物体Jの時刻tk−1における推定位置(符号P1)から予測される時刻tkの予測位置(符号α)と寸法(符号S2)とを基準にした関連付け範囲(符号β)を介して、時刻tkにおいて検出された物点m(符号DP2)と物体Jとが関連付けられる。図中の斜線の網掛けは、そのようにして物点m(符号DP2)が、検出済みの物体J(符号P1)の情報、特に寸法(符号S1)を利用して、その所属物点j(符号DP1)と関連付けられる様子を表している。なお、図示省略しているが、例えば、物体Jの時刻tk−1における所属物点jも、さらに過去の時刻において検出された物体およびその所属物点と関連付けられている。
なお、物体Jに対して評価条件を満たす物点が複数存在する場合は、式(5)の左辺のベクトルのノルムが最も小さい物点を物体Jと関連付けても良いし、評価条件を満たすすべての物点を物体Jと関連付けてもよい。このとき、条件を満たすすべての物点を新たな1つの物点に書き換えた上で物体Jと関連付けることも可能である。その場合、書き換え後の物点が物体Jのその時刻における代表点となる。複数物点から1つの物点への書き換えは、例えば、検出結果の平均化により行ってもよい。より具体的には、評価条件を満たすすべての物点の検出結果を平均化した検出結果を有する1つの物点を定義して、物体Jの時刻tkにおける唯一の所属物点(代表点)としてもよい。
また、物体Jの時刻tk−1における位置p(k−1|k−1,J)の標準偏差σx,σyを用いて、ηを式(6)のように算出して、関連付け範囲を可変としてもよい。
標準偏差は、物体Jの時刻tk−1における位置p(k−1|k−1,J)を推定する際に併せて算出すればよい。なお、追尾装置20は、物体Jの対象時点の所属物点の情報を基に推定対象の運動状態とその位置の標準偏差σx,σyまたは誤差分散σx 2,σy 2を求めておき、物体情報に保持しておいてもよい。そのようにすれば、次の時点において1つ前の時点の運動状態とその位置の標準偏差または誤差分散を利用できる。γは所定の係数である。
式(5)は、位置に関する条件式であるが、関連付けの評価条件に、さらに速度に関する条件式を加えることもできる。例えば、時刻tkにおける物点mの視線速度dvr (k,m)と物体Jの時刻tkにおける推定速度v(k|k−1,J)の視線方向への射影との差を限定する条件式を用いることができる。
なお、上記の方法は、検出済みの物体Jについて時刻tk−1における位置pおよび速度vと、寸法sと、標準偏差σが得られていることを前提としている。しかし、物体Jが時刻tk−1において初めて検出された物体である場合、すなわち物体Jの時刻tk−1における所属物点が時刻tk−2において追尾中のいずれの物体とも関連づけられていない場合が考えられる。その場合、物体Jの時刻tk−1における速度vを推定するところから始めてもよい。なお、物体Jの時刻tk−1における位置pは、物体Jの時刻tk−1の所属物点から求まる代表位置とすればよい。
以下、時刻tk−1において初めて検出された物体Jと、時刻tkに検出された物点mとを関連付ける方法を説明する。本例では、物点mが属する仮の物体と物体Jとが同一物体であると仮定して、まず、物体Jの時刻tk−1における速度v(k−1,J)を推定する。本仮定によると、物体Jの時刻tk−1における速度v(k−1,J)は、例えば時刻tk−1における物体Jの所属物点jの視線速度dvr (k−1,j)からの逆射影により、式(7)を用いて推定できる。なお、<a,b>はaとbの内積を、||a||はaの大きさを表す。ここで、所属物点jは、物点が複数ある場合はそれらの代表点であるとする。
ここで、物点と物体Jの位置の差であるΔp(k,m,k−1,J)は、式(8)で示される。なお、物体Jの所属物点が1つの物点(平均化された物点を含む)のみであれば、物体Jの位置として1つの所属物点jの位置dp(k−1,j)をそのまま用いてもよい。
物体Jの時刻tk−1における位置p(k−1,J)および速度v(k−1,J)を求めた後は、上述した方法と同様に、評価条件に基づき物点mと関連付けるか否かを判定すればよい。このとき、物体Jの寸法s(k−1,J)は所定の値またはゼロを設定してもよい。また、より正確に関連付けるために、時刻tk−2以前に検出されている物点の検出結果も用いて関連付けるか否かを判定してもよい。例えば、時刻tk−2とtk−1における位置から速度を推定し、その速度と時刻tk−1における位置から時刻tkにおける位置を推定し、その位置と時刻tkにおける検出位置を比較して、物点を関連付けるか否かを判定してもよい。
次に、このようにして行われる関連付けの結果を利用した物体の運動状態の推定方法の一例を示す。運動状態の推定には、例えば拡張カルマンフィルタを用いることができる。時刻tkにおいて検出された物点mと、時刻tk−1において検出済みの物体Jとが関連付けられた場合、追尾装置20は、関連付け結果に基づいて次のようにして物体Jの時刻tkにおける運動状態em(k|k,J)を推定してもよい。以下では、運動状態em(k|k,J)として、位置p(k|k,J)、速度v(k|k,J)および加速度a(k|k,J)を推定する例を示す。追尾装置20は、例えば、物体Jの時刻tk−1における位置p(k−1|k−1,J)、速度v(k−1|k−1,J)および加速度a(k−1|k−1,J)とそれらの誤差共分散行列と、時刻tkにおける所属物点mの観測者を基準にした距離dr(k,m)、方位角dθ(k,m)および視線速度dvr (k,m)とを用いて、物体Jの時刻tkにおける運動状態em(k|k,J)を推定してもよい。ここで、所属物点mは、物点が複数ある場合はそれらの代表点とする。
拡張カルマンフィルタの適用は、状態ベクトルを式(9a)、観測ベクトルを式(9b)とし、状態方程式を式(9c)、観測方程式を式(9d)とすればよい。なお、以下の式(9a)〜式(9f)では、右肩の括弧において物体や物点の識別子を省略し、時刻の情報のみを示している。なお、x(k)が上記のem(k,J)に相当する。
ここで、wはシステム雑音、zは観測雑音である。また、行列Aと関数h()はそれぞれ式(9e)、式(9f)で表される。
追尾装置20は、例えば、拡張カルマンフィルタを用いて、一つ前の時刻の推定状態ベクトルから現在の推定状態ベクトルを算出する予測ステップと、現在の観測ベクトルを用いて現在の推定状態ベクトルを補正する更新ステップとを繰り返して、状態ベクトルx(k)を推定していく。
ところで、運動状態を推定する対象物体についても、時刻tk−1において初めて検出された物体であることが考えられる。そのような場合、次のようにして、物体Jの時刻tkにおける位置p(k|k,J)、速度v(k|k,J)および加速度a(k|k,J)を推定してもよい。
一例として、時刻tkにおいて検出された物点mの位置dp(k,m)を物体Jの時刻tkにおける位置p(k|k,J)とする。また、以下の式(10)で示されるv(k|k,J)または上記の式(7)で示されるv(k−1,J)を物体Jの時刻tkにおける速度v(k|k,J)とする。また、加速度a(k|k,J)はゼロとする。
点と見なせる物体の追尾では、時刻tkを現在とすると、検出済みの物体について時刻tkまでの所属物点の位置(検出位置)および視線速度(検出速度)の履歴から、時刻tkの運動状態を推定する、更に状況によっては未来すなわち時刻tk+1以降の運動状態を予測するだけで十分な場合が多い。しかし、点と見なせない任意の物体の追尾では、物点と物体の関連付けが課題になる他、関連付け後においても、物点の物体内の移動による位置の推定誤差をいかに低減させるかが課題となる。
そのため、本実施の形態では、時刻tkにおいて、その物体の時刻tkまでの所属物点の位置および視線速度の履歴を利用して、時刻tk−n(n>0)の運動状態を再推定する。このとき、時刻tk−nにとって未来に相当する時刻(例えば、時刻tkー1に対して時刻tkなど)の所属物点の情報も利用して運動状態を再推定(平滑化)する。
このような再推定の方法として、例えば、拡張カルマンスムーサーを用いることができる。今、物体Jの時刻tkまでの所属物点の情報を利用して、物体Jの時刻tkーn(n>0)における運動状態em’(k−n,J)すなわち位置p’(k−n,J)、速度v’(k−n,J)および加速度a’(k−n,J)を推定することを考える。以下では、時刻tkまでの所属物点の情報を利用して推定される物体Jの時刻tkーnにおける位置p’(k−n|k,J)、速度v’(k−n|k,J)および加速度a’(k−n|k,J)をそれぞれ、式(11a)、式(11b)および式(11c)で表す。
上述した拡張カルマンフィルタでは、時刻tkの運動状態(第1の推定値)を推定する際に、時刻tk−1までの所属物点の履歴を反映した時刻tk−1の運動状態(第1の推定値)およびその誤差共分散行列を使うことにより効率的に推定できる。
一方、拡張カルマンスムーサーでは、対象物体について、平滑化された時刻tkにおける運動状態の推定値とその誤差共分散行列および時刻tk−1における運動状態の推定値とその誤差共分散行列とから、平滑化された時刻tk−1における運動状態の推定値とその誤差共分散行列とを推定できる。したがって、再推定処理で、最新の検出時刻における平滑化前の推定値(第1の推定値)およびその誤差共分散行列を第1の時点における平滑化された推定値(第2の推定値)とし、かつ第1の時点に対して直近の過去の検出時刻を第2の時点とする。その上で、第1の時点における第2の推定値とその誤差共分散行列と、第2の時点における第1の推定値とその誤差共分散行列とから、第2の時点における第2の推定値を推定する処理を、第1の時点とする検出時刻を時刻tk,tk−1,tk−2,・・・のように過去に遡りながら順次実行する。これにより、平滑化された運動状態も効率的に推定できる。
また、以下に示す例は、物体Jの最新の寸法s(k、J)をさらに用いて、物体Jの各検出時刻における運動状態を再推定する例である。本例では、物体Jの最新の寸法以下の位置のずれを許容するようにして、例えば、以下の式(12)によりx(k−n),n=0,1,...,Nを順次推定する。なお、推定されるx(k−n)が上記のem’(kーn,J)に相当する。
ここで、Wはw(k)の共分散行列、Zはz(k)の共分散行列である。なお、上記の例では、物点の検出位置の変化が寸法以下である場合に該変化を推定誤差として扱わないように、物点の検出位置の変化の許容範囲を示す変数ξを導入している。なお、物体の運動状態の再推定は、計算量を低減したい場合等には実行しなくてもよい。
次に、物体の寸法方法の一例を示す。物体Jの寸法は、例えば、物体Jの時刻tkまでの推定位置の履歴と、所属物点の検出位置の履歴とから、式(13)により推定する。
ここで、Ψx (k,J)は、物体Jの時刻tkまでの各検出時刻における推定位置と所属物点mの検出位置のx軸方向の差の集合、Ψy (k,J)は、y軸方向の差の集合である(式(14a)、式(14b)参照)。なお、所属物点が複数ある場合は、各所属物点mについて推定位置との差を求めるものとする。maxは集合内の最大値を取り出す関数、minは集合内の最小値を取り出す関数である。なお、最小値が+の値であった場合すなわち推定位置に対して+方向に位置する物点しか属していなかった場合、式(13)において各軸方向の寸法を求める式の第二項(推定位置に対して−方向の最大差として取得される最小値を−方向の寸法値として加算する項)は省略してもよい。
なお、実際の物体の寸法は変化するものではないため、寸法の推定は必ずしも各時刻で推定する必要はなく、所定の時間間隔で、または運動状態の変化が大きかった(所定の閾値以上となった等)時点で実施するようにしてもよい。
このように、本例では、関連付け後に、その結果を利用して求めた最新の運動状態をさらに利用して過去の運動状態を再推定するため、物体内での物点の移り変わりによる推定誤差が低減された運動状態を推定できる。例えば、図7には、物体Jの時刻tk−1における推定位置(符号P1参照)が更新されて符号P’1の示す位置となり、時刻tkにおける推定位置(符号P2参照)が更新されて符号P’2の示す位置となったことが示されている。このような更新の結果、物体Jの位置の変化が平滑化される。なお、図7では、さらに物体の寸法(符号S’1およびS’2)も更新されていることがわかる。なお、符号S’1および符号S’2はいずれも最新の寸法s’(k,J)を表している。
次に、図8を参照して本発明の実施の形態2に係る追尾装置20の構成を説明する。図8は、本実施の形態に係る追尾装置20の構成例を示すブロック図である。また、図9は、本実施の形態に係る追尾装置20のハードウェア構成の例を示す構成図である。本構成では伝搬波として電磁波を用いた例となっている。
図8に示すように、追尾装置20は、計測手段21と、関連付け手段22と、運動状態推定手段23と、寸法推定手段24と、運動状態再推定手段25と、情報記憶手段26と、情報出力手段27とを備える。なお、計測手段21が実施の形態1における計測装置18に対応する。なお、追尾装置20が計測装置18に相当する計測手段21を備える場合には、計測手段21が物点情報取得部11の機能を兼用しているものとする。また、関連付け手段22が実施の形態1における関連付け部12に対応する。また、運動状態推定手段23および運動状態再推定手段25が、実施の形態1における運動状態推定部13に対応する。また、寸法推定手段24が実施の形態1における寸法推定部14に対応する。また、情報記憶手段26が実施の形態1における情報記憶部16に対応する。また、情報出力手段27が実施の形態1における表示部17に対応する。
また、図9に示すように、追尾装置20は、信号生成部1、増幅器2、送信アンテナ3、受信アンテナ4、ミキサ5、フィルタ6、増幅器7、A/D変換器8および信号処理部9を備える。なお、図示省略しているが、追尾装置20は、出力装置および情報記憶装置をさらに備えている。
計測手段21は、伝搬波を送信し物体での反射波を受信して、周囲の物体上の点(物点)の位置および視線速度を計測する手段である。本例では、計測手段21は、信号生成部1、増幅器2、送信アンテナ3、受信アンテナ4、ミキサ5、フィルタ6、増幅器7、A/D変換器8および信号処理部9により実現される。
計測手段21は、伝搬波を送信し物体からの反射波を受信して1つ以上の物体が伝搬波を反射した時点の該物体上の点(物点)のそれぞれの位置および視線速度を計測することにより、1つ以上の物点を検出する。
本例では、信号生成部1が送信信号としてFMCW信号を生成する。送信信号は、増幅器2で増幅され、送信アンテナ3から送信される。送信アンテナ3から送信された送信信号が物体で反射されると、その反射した信号が、複数の受信アンテナ4で受信される。受信信号(反射信号)は、ミキサ5で送信信号と混合された後、フィルタ6で不要成分が除去されてビート信号として増幅器7に入力される。ビート信号は増幅器7で増幅され、A/D変換器8でデジタル信号に変換される。デジタル信号は信号処理部9で処理され、物点の位置と速度が検出される。なお、ここでは送信信号としてFMCW信号を例示したが、送信信号はこれに限定されない。
関連付け手段22は、上述した関連付け方法を用いて関連付け処理を行う。例えば、関連付け手段22は、最新の検出時刻において検出された物点(対象物点)の位置および速度と、検出済みの物体の最新(例えば、最新の物点の検出時刻に対して1つ前の検出時刻)の運動状態および寸法とを用い、例えば式(5)に基づき最新の検出時刻において検出された物点と、検出済みの物体の所属物点とを同一の物体の情報として関連付ける。
関連付け手段22は、例えば、既に実行済みの運動状態推定処理で算出された物体の運動状態の推定値のうちの最新の推定値と、既に実行済みの寸法推定処理で算出された物体の寸法の推定値のうちの最新の推定値とに基づいて、最新の検出時刻より前の検出時刻において検出された物体が最新の検出時刻において存在し得る関連付け範囲を決定し、決定した関連付け範囲と、最新の検出時刻において検出された物点の位置の検出値(物点の位置)とに基づいて、検出済みの物体に最新の検出時刻において検出された物点を関連付ける。
運動状態推定手段23は、上述した運動状態の推定方法を利用して運動状態推定処理を行う。より具体的に、運動状態推定手段23は、関連付け処理で同一物体として関連付けられた物点の情報(検出済みの物体の各検出時刻における所属物点の位置および速度の検出値に基づいて、最新の検出時刻までの各検出時刻における該物体の位置の推定値を含む運動状態を推定する。
例えば、運動状態推定手段23は、最新の検出時刻において検出された物点と関連付けられた検出済み物体(最新の物点検出処理で検出された物点が属する物体)を対象に、該物体が最初に検出された時刻から最新の検出時刻までの各検出時刻における所属物点の情報(位置および速度)を利用して、各検出時刻における当該物体の運動状態(位置および速度の推定値)を、拡張カルマンフィルタおよび拡張カルマンスムーサーを利用して推定する。
このとき、運動状態推定手段23は、演算処理の効率化の一例として、次のように各検出時刻における運動状態を求めてもよい。
まず、運動状態推定手段23は、拡張カルマンフィルタ等を利用して、最新の検出時刻における所属物点の情報と、既に実行済みの処理で算出された直近の過去の検出時刻における運動状態の第1の推定値およびその誤差共分散行列とに基づいて、最新の検出時刻における運動状態の第1の推定値およびその誤差共分散行列を算出する。
その後、運動状態推定手段23は、最新の検出時刻における第1の推定値およびその誤差共分散行列を、最新の検出時刻における第2の推定値およびその誤差共分散行列とし、最新の検出時刻を第1の時点とし、かつ第1の時点に対して直近の過去の検出時刻を第2の時点とする。そして、運動状態推定手段23は、拡張カルマンスムーサー等を利用して、第1の時点の第2の推定値およびその誤差共分散行列に基づいて第2の時点の第2の推定値およびその誤差共分散行列を算出する処理を、第1の時点とする検出時刻を過去に遡りながら順に実行する。そして、そのようにして算出された各検出時刻における第2の推定値を、各検出時刻における運動状態の推定値とする。なお、本例は、効率的な推定のために、運動状態推定手段23が、最新の検出時刻における運動状態を推定するタイミングで、最新の検出時刻における第1の推定値と合わせて最新の検出時刻までの各時刻における第2の推定値を求める例であるが、最新の検出時刻までの各時刻における第2の推定値を求める処理(第1の再推定処理)は、後述する運動状態再推定手段25が行ってもよい。
運動状態の推定に用いる手法は、拡張カルマンフィルタや拡張カルマンスムーサーに限定される必要はなく、時刻tkまでの所属物点の位置と視線速度の検出値の履歴に基づいて、時刻tkまでの各時刻の位置の推定値を含む運動状態の履歴を推定するものであればよい。
寸法推定手段24は、上述した寸法推定方法を利用して寸法推定処理を行う。例えば、寸法推定手段24は、運動状態推定処理で推定された物体の位置の推定値の履歴と、検出処理で検出された該物体の所属物点の位置の検出値の履歴とに基づいて、物体の寸法を推定する。
寸法推定手段24は、運動状態推定手段23により推定された各検出時刻における物体の推定位置に対する所属物点の検出位置の差から、式(12)等を利用して物体の寸法を推定してもよい。
運動状態再推定手段25は、上述した運動状態の再推定方法を利用して運動状態再推定処理を行う。例えば、運動状態再推定手段25は、任意のタイミングで、最新の時点の第1の推定値およびその誤差共分散行列を、第1の時点の第2の推定値およびその誤差共分散行列とみなして、拡張カルマンスムーサー等を利用して、上記の第1の再推定処理を行ってもよい。また、例えば、運動状態再推定手段25は、上記第1の再推定処理に加えてまたは上記第1の再推定処理に代えて、任意のタイミングで、寸法推定手段24により推定された物体の寸法を利用して、物体の寸法以下の位置のずれを許容するように、式(12)等を利用して物体の運動状態を再推定してもよい(第2の再推定処理)。
本例において、関連付け手段22、運動状態推定手段23、寸法推定手段24および運動状態再推定手段25は、信号処理部9により実現される。より具体的には、関連付け手段22、運動状態推定手段23、寸法推定手段24および運動状態再推定手段25は、信号処理部9(より具体的には、処理回路、またはプログラムに従って動作するCPU等)によって実現される。また、情報記憶手段26は、メモリにより実現される。また、情報出力手段27は、ディスプレイ等の出力装置により実現される。
図10は、本実施形の形態の信号処理部9およびその周辺のハードウェア構成の例を示す構成図である。図10(a)に示すように、信号処理部9の各機能は、処理回路9aにより実現されてもよい。また、図10(b)に示すように、信号処理部9の各機能は、プロセッサ9bとメモリ9cとにより実現されてもよい。また、信号処理部9は、バス等を介して、さらに各種情報の外部入出力用のインタフェースである外部入出力I/F110、マウスやキーボードなどの入力装置120、およびディスプレイなどの出力装置130と接続されていてもよい。
すなわち、追尾装置20は、上述の信号処理部9の機能を実現するための専用のハードウェアである処理回路9aを少なくとも備えるか、上述の信号処理部9の機能を実現するためのプロセッサ9bおよびメモリ9cを少なくとも備える。
処理回路9aは、例えば、単一回路、複合回路、プログラム化したプロセッサ、並列プログラム化したプロセッサ、ASIC、FPGA、またはこれらを組み合わせたものであってもよい。処理回路9aは、信号処理部9の各機能(関連付け手段22、運動状態推定手段23、寸法推定手段24および運動状態再推定手段25の機能)それぞれを実現してもよいし、まとめて実現してもよい。
プロセッサ9bは、例えば、メモリに格納されるプログラムを実行するCPU(Central Processing Unit、中央処理装置、処理装置、演算装置、マイクロプロセッサ、マイクロコンピュータ、DPSともいう)であってもよい。その場合、信号処理部9の各機能は、ソフトウェア、ファームウェア、またはソフトウェアとファームウェアとの組み合わせにより実現される。ソフトウェアやファームウェアはプログラムとして記述され、メモリ9cに格納される。プロセッサ9bは、例えば、メモリ9cに記憶されたプログラムを読み出して実行することにより、各機能を実現する。すなわち、追尾装置20は、信号処理部9が実現する各手段の機能を実現するための各種ステップが結果的に実行されることになるプログラムを格納するためのメモリ9cを備える。また、これらのプログラムは、上述した追尾方法をコンピュータに実現させるものであるともいえる。ここで、メモリ9cは、例えば、RAM、ROM、フラシュメモリー、EPROM、EEPROM等の、不揮発性または揮発性の半導体メモリや、磁気ディスク、フレキシブルディスク、光ディスク、コンパクトディスク、ミニディスク、DVD等が該当する。
なお、信号処理部9の各機能について、一部を専用のハードウェア(処理回路9a等)で実現し、一部をソフトウェアまたはファームウェア(より具体的には、それに従って動作するプロセッサ9b等)で実現するようにしてもよい。また、その際、それらハードウェア、ソフトウェアまたはファームウェアは、外部入出力I/F110および入力装置120を介して入力される情報を利用する、外部入出力I/F110および出力装置130を介して外部に情報を出力する等ができる。
このように、信号処理部9ひいては追尾装置20は、ハードウェア、ソフトウェア、ファームウェア、またはこれらの組み合わせによって、上述の各機能を実現することができる。
図11は、本実施の形態の追尾装置20の動作の一例を示すフローチャートである。図11に示す例では、まず計測手段21が、周囲の物点の位置および速度を計測して物点を検出する物点検出処理を行う(ステップS21)。次いで、関連付け手段22が関連付け処理を行う(ステップS22)。
次いで、運動状態推定手段23が、関連付け処理の結果に基づき運動状態推定処理を行う(ステップS23)。
次いで、寸法推定手段24が、寸法推定処理を行う(ステップS24)。次いで、運動状態再推定手段25が、寸法の推定結果を利用して運動状態再推定処理を行う(ステップS25)。なお、ステップS24およびステップS25の処理は、任意のタイミングで行うことができる。
ステップS21〜S25の処理の結果は、情報記憶手段26に記憶される。
次いで、情報出力手段27が、情報記憶手段26に記憶されている情報に基づき、追尾結果を出力する(ステップS26)。以降、一連の処理を繰り返し行う。
以上のように、本実施の形態の追尾装置によれば、自車両において第1の実施形態と同様の効果を得ることができる。すなわち、本実施の形態の追尾装置は、計測手段21と、関連付け手段22と、運動状態推定手段23と、寸法推定手段24とを備え、それら手段を利用して物点検出処理、関連付け処理、運動状態推定処理および寸法推定処理を繰り返し行う。このとき、関連付け手段22が、一方の物体の寸法を利用して関連付けを行うことにより、複数の反射点が存在する物体や反射点が移り変わる物体から物点が検出されたとしても、異なる時点の物点同士を同一物体に精度よく関連付けることができる。これにより、高い追尾精度と分離性能を実現することができる。
また、例えば、関連付け手段22が、既に実行済みの寸法推定処理で算出された物体の寸法の推定値のうちの最新の推定値に応じて、関連付け範囲を適宜変更することにより、検出済みの物体と新たに検出された物点の所属先物体とをより精度よく関連付けることができる。
また、例えば、運動状態推定手段23が、最新の検出処理までの各検出処理の時点における物体の位置の推定値を含む運動状態の履歴を推定する際に、推定対象時点よりも新しい検出処理の時点において検出された物点の情報を利用することにより、物体の位置の推定値の履歴の推定誤差を低減できる。これにより、反射点の移り変わりによる物体の位置の変動等を抑制して運動状態を精度よく推定することができる。加えて、そのような高精度の運動状態の推定結果を利用して、寸法推定や、次の時点における関連付け範囲が設定されるので、関連付け精度をさらに向上できる。このような相乗効果が繰り返されることにより、より高い追尾精度と分離性能を実現できる。
また、本実施の形態においても、推定した物体寸法を利用して、物体寸法以下の位置ずれを許容するように物体の運動状態を再推定するよう構成できるので、運動状態の推定精度をさらに向上させられる。これにより、さらに高い追尾精度と分離性能を実現することができる。
実施の形態3.
次に、本発明の実施の形態3に係る追尾装置について説明する。図12は、本発明の実施の形態3に係る追尾装置30の構成例を示すブロック図である。図12に示す追尾装置30は、図8に示した追尾装置20の構成に比べて、画像取得手段31、属性判定手段32および関連付け評価手段33をさらに備える点が異なる。また、図13は、本実施の形態に係る追尾装置30のハードウェア構成の例を示す構成図である。図13に示す例では、追尾装置30にさらにカメラ31Aが追加されている。なお、他の点は実施の形態2と同様である。
画像取得手段31は、例えば、カメラ31A、またはカメラ31Aとのインタフェース(外部入出力I/F110等)により実現される。なお、カメラ31Aは、追尾装置30が備えていてもよいし、追尾装置30の上位装置(観測者としての自車両)が備えていてもよい。また、属性判定手段32および関連付け評価手段33は、信号処理部9により実現される。
画像取得手段31は、観測者(例えば、自車両)の周囲の物体を含む周囲環境を撮影した画像を取得する。
属性判定手段32は、画像取得手段31が取得した画像から物体を認識して、認識された物体の属性を判定する。属性判定手段32は、取得された画像から物体を認識して物体の種類や寸法等の属性を判定する。画像から物体を認識する方法および認識された物体の属性を判定する判定は公知の画像処理技術を利用することができる。
関連付け評価手段33は、属性判定手段32で認識された物体よびその属性に基づき、現在の関連付けを評価し、必要に応じて、物体情報において定義されている物体(関連付け手段22によって物点と関連付けられる物体であって追尾中の物体)を分割もしくは統合する。関連付け評価手段33は、画像から認識された物体およびその属性が、物点検出処理により検出される物点の所属先とされた物体(追尾中の物体)およびその属性と矛盾する場合、追尾中の物体を分割または統合する。例えば、関連付け評価手段33は、画像から認識された物体の属性が示す寸法が、追尾中の物体の寸法と矛盾する場合、画像から認識された物体の属性に基づいて、追尾中の物体を分割または統合してもよい。
例えば、時刻tkにおいて画像から2台の普通乗用車が認識された位置に、追尾中の物体として普通乗用車では在りえない寸法の物体が物点情報を基に検出されている場合、該物体を新たな2つの物体に置き換える。また逆に、例えば画像から1台の大型車が認識された位置に、追尾中の物体として複数の小型物体が物点情報を基に検出されている場合、それら複数の物体を新たな1つの物体に置き換える。このとき、分割前または統合前の物体の情報(運動状態、所属物点、寸法等)は、分割または統合されて新たな物体に引き継がれる。例えば、1つの物体がそれぞれ異なる2つの領域に存在する2つの物体に分割する場合、分割後の領域に存在する物点をそれぞれ分割後の物体の所属物点とし、分割後の各物体の運動状態および寸法を推定しなおすことにより、情報を引き継いでもよい。また、例えば、2つの物体を1つの物体に統合する場合、統合前の各物体の所属物点の和集合を統合後の物体の所属物点とし、該物体の運動状態および寸法を推定しなおすことにより、情報を引き継いでもよい。時刻tk+1では、更新後の物体情報に基づき、処理を行う。
図14は、本実施の形態の追尾装置30の動作の一例を示すフローチャートである。なお、図14において実施の形態2と同様の動作については説明を省略する。
画像取得手段31により周囲の物体を含む周囲環境を撮影した画像が取得されると、属性判定手段32が、画像から物体を認識するとともに認識された物体の属性を判定する(ステップS31,S32)。次いで、関連付け評価手段33は、属性判定手段32で認識された物体よびその属性に基づき、現在の関連付けを評価し、必要に応じて追尾中の物体の分割または統合を行う(ステップS33)。
以上のように、実施の形態3の追尾装置によれば、カメラで撮影した画像から認識される物体とその属性を利用して、現在の関連付け(およびそれによって定義される追尾中の物体の情報)を評価し、必要に応じて追尾中の物体の分割または統合することができるため、物体をさらに精度良く分離追尾することができる。
本発明による追尾装置は、所定の観測者の周囲の物体上の点である物点に関する情報であって、物点の位置を示す情報と、物点の速度を示す情報とを含む物点情報を取得する物点情報取得部と、物点情報に基づき、物点と関連付けられる物体を検出するとともに、検出済みの物体の寸法を利用して異なる時点に検出された物点を同じ物体と関連付けることにより物体の検出時刻ごとの所属物点を定める関連付け部と、関連付けの結果示される物点と記物体との対応関係に基づいて、検出済みの物体の運動状態であって物体の位置を含む運動状態を推定する運動状態推定部とを備え、関連付け部は検出済みの物体の運動状態と物点情報取得部が取得したそれぞれの物点情報から算出する運動状態の類似度を用いて関連付けることを特徴とする。
また、本発明による追尾方法は、所定の観測者の周囲の物体上の点である物点に関する情報であって、物点の位置を示す情報と、物点の速度を示す情報とを含む物点情報を取得する第1のステップと、物点情報に基づき、物点と関連付けられる物体を検出するとともに、検出済みの物体の寸法を利用して異なる時点に検出された物点を同じ物体に関連付けることにより物体の検出時刻ごとの所属物点を定める第2のステップと、関連付けの結果示される物点と記物体との対応関係に基づいて、検出済みの物体の運動状態であって物体の位置を含む運動状態を推定する第3のステップとを含み、第2のステップは検出済みの物体の運動状態と第1のステップで取得したそれぞれの物点情報から算出する運動状態の類似度を用いて関連付けることを特徴とする。