JP6460822B2 - 移動物体追跡方法および移動物体追跡装置 - Google Patents

Description

本発明は、移動する物体の位置を時系列的に計測する、移動物体追跡方法および移動物体追跡装置に関する。

特許文献１は、受光した全受光素子の形状の重心位置に基いて、照射したレーザー光の反射光を受光する受光素子を特定することによって、レーザー光の照射方向と受光素子との対応関係を把握する対応関係把握手段を備えた距離測定装置を提案している。
特許文献２は、先行車両のテールランプを検出し、検出したテールランプの重心に追跡エリアの中心を合致させることで先行車両のテールランプを追跡するテールランプ検出装置を提案している。
特許文献１および特許文献２でも開示されているように、特に車両の走行において、レーザーやカメラを用いて、移動物体を追跡することは従来から行われている。

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

単に自車両とその前方を走行する他車両との車間距離を制御する場合には、これら従来の方法で対応できる。
しかし、車線変更や追い越しなどにより自車両が他車両の側方又は前方に位置することになったとしても他車両との間隔を制御し円滑に走行する自動運転を実現するためには、どの方位から他車両を観測しても、他車両の位置、走行方向および速度を正確に把握することが必要となるが、従来の手法では、それまでの追跡基準（重心位置など）に代えて新たな追跡基準を設定することができないか、または新たな追跡基準を設定できたとしてもその前の追跡基準との対応関係が不明なため、他車両の位置を正確に把握することができない。例えば、図６は従来の他車両追跡方法を説明する図である。従来の追跡方法は、重心位置（計測手段１が取得する点群データの平均値）を基準として他車両（検知対象車両Ａ）を追跡するものである。計測手段１が、検知対象車両Ａの後方に位置する状態（図６（ａ）の状態）からやや右に移動した状態（図６（ｂ）の状態）になると、検知対象車両Ａの重心位置がＺ１からＺ２に変更されるため、実際には検知対象車両Ａは真直ぐ走行しているにも関わらず、右に方向転換したと誤認識してしまう。

そこで本発明は、例えば自車両が他車両を追い越すときのように自車両の他車両に対する位置が、後方から側方、さらに前方へと変わる場合であっても、他車両の位置、走行方向および速度を正確に把握し、安定して他車両を追跡することができる移動物体追跡方法および移動物体追跡装置を提供することを目的とする。

請求項１記載の本発明の移動物体追跡方法は、レーザーを用いた送受信センサーを自車両に搭載し、前記自車両の周辺を走行する検知対象車両の位置を時系列的に計測する移動物体追跡方法であって、前記送受信センサーによって三次元の点群データを取得するデータ取得ステップと、前記データ取得ステップで取得した前記点群データから、前記検知対象車両の一側面、前記検知対象車両の背面、および前記検知対象車両の前面の内、少なくとも一つの面を一辺とする矩形枠によって前記検知対象車両の一部又は全体の形状を近似する近似ステップと、前記近似ステップで近似した前記矩形枠の角を基準点の候補として定義する基準点定義ステップと、前記検知対象車両について、少なくとも一時刻前の検知対象車両位置と現在の前記検知対象車両位置とから検知時刻での前記検知対象車両位置を予測する予測ステップと、前記予測ステップで予測した前記検知時刻での前記検知対象車両位置と、前記検知時刻での自車両位置とから、前記基準点定義ステップで候補として定義した複数の前記基準点の中から、前記送受信センサーに最も近い位置となる前記基準点を選択する基準点確定ステップと、前記基準点確定ステップで選択した前記基準点を用いて前記検知時刻での前記検知対象車両位置を算出する位置算出ステップとを有することを特徴とする。
請求項２記載の本発明は、請求項１に記載の移動物体追跡方法において、一時刻前の前記検知対象車両位置の算出に用いた前記基準点と、現在の前記検知対象車両位置の算出に用いた前記基準点とを比較する基準点比較ステップを有し、前記基準点比較ステップで前記基準点が異なると判断した場合には、前記矩形枠の枠長さのデータを用いて現在の前記検知対象車両位置の補正を行うことを特徴とする。
請求項３記載の本発明は、請求項１または請求項２に記載の移動物体追跡方法において、 前記矩形枠の一辺の中心点を、更に前記基準点の候補としたことを特徴とする。
請求項４記載の本発明の移動物体追跡装置は、移動する物体の位置を時系列的に計測する移動物体追跡装置であって、送受信センサーによって取得する三次元の点群データから、矩形枠によって前記物体の一部又は全体の形状を近似する近似部と、前記近似部で近似した前記矩形枠の角を基準点の候補として定義する基準点定義部と、前記物体について、少なくとも一時刻前の物体位置と現在の前記物体位置とから検知時刻での前記物体位置を予測する物体位置予測部と、前記物体位置予測部で予測した前記検知時刻での前記物体位置と、前記検知時刻での送受信センサー位置とから、前記基準点定義部で候補として定義した複数の前記基準点の中から、前記送受信センサーに最も近い位置となる前記基準点を選択する基準点確定部と、前記基準点確定部で選択した前記基準点を用いて前記検知時刻での前記物体位置を算出する物体位置算出部とを備えたことを特徴とする。
請求項５記載の本発明は、請求項４に記載の移動物体追跡装置において、前記送受信センサーを自車両に搭載したことを特徴とする。
請求項６記載の本発明は、請求項５に記載の移動物体追跡装置において、前記物体を、前記自車両周辺を走行する車両としたことを特徴とする。
請求項７記載の本発明は、請求項４から請求項６のいずれかに記載の移動物体追跡装置において、一時刻前の前記物体位置の算出に用いた前記基準点と、現在の前記物体位置の算出に用いた前記基準点とを比較する基準点比較部を備え、前記基準点比較部で前記基準点が異なると判断した場合には、前記矩形枠の枠長さのデータを用いて現在の前記物体位置の補正を行うことを特徴とする。

本発明によれば、送受信センサーが物体（検知対象車両）の後方位置にあるときだけでなく、送受信センサーの物体に対する位置が、後方から側方へ、さらに側方から前方へなどと変わる場合であっても物体の位置、走行方向および速度を正確に把握し、安定して物体（検知対象車両）を追跡することができる。また、送受信センサーに最も近い基準点は、検知時刻での物体位置（検知対象車両位置）を予測してあらかじめ選択することができるので、その予測した基準点と検知時刻において取得した点群データから選択する基準点との対応付けを行うことで、より精度良く物体（検知対象車両）の位置、走行方向および速度を把握することができる。

本発明の一実施例による移動物体追跡装置を示す概略構成図 本実施例による移動物体追跡方法を示すフローチャート 本実施例による矩形枠による検知対象車両形状の近似方法と基準点候補の定義方法を説明する図 本実施例による移動物体追跡装置の説明図 本実施例による移動物体追跡装置の説明図 本実施例による移動物体認識装置との比較例を示す説明図

本発明の第１の実施の形態による移動物体追跡方法は、送受信センサーによって三次元の点群データを取得するデータ取得ステップと、データ取得ステップで取得した点群データから、検知対象車両の一側面、検知対象車両の背面、および検知対象車両の前面の内、少なくとも一つの面を一辺とする矩形枠によって検知対象車両の一部又は全体の形状を近似する近似ステップと、近似ステップで近似した矩形枠の角を基準点の候補として定義する基準点定義ステップと、検知対象車両について、少なくとも一時刻前の検知対象車両位置と現在の検知対象車両位置とから検知時刻での検知対象車両位置を予測する予測ステップと、予測ステップで予測した検知時刻での検知対象車両位置と、検知時刻での自車両位置とから、基準点定義ステップで候補として定義した複数の基準点の中から、送受信センサーに最も近い位置となる基準点を選択する基準点確定ステップと、基準点確定ステップで選択した基準点を用いて検知時刻での検知対象車両位置を算出する位置算出ステップとを有するものである。本実施の形態は、検知対象車両の一部又は全体の形状を矩形枠によって近似し、その矩形枠の四角（四隅）を基準点の候補として定義し、定義した四角の基準点の中から自車両に搭載した送受信センサーに最も近い基準点を基準として検知対象車両位置を時系列的に計測する。従って、本実施の形態によれば、送受信センサーが検知対象車両の後方位置にあるときだけでなく、送受信センサーの検知対象車両に対する位置が、後方から側方へ、さらに側方から前方へなどと変わる場合であっても検知対象車両の位置、走行方向および速度を正確に把握し、安定して検知対象車両を追跡することができる。また、送受信センサーに最も近い基準点は、検知時刻での検知対象車両位置を予測してあらかじめ選択することができるので、その予測した基準点と検知時刻において取得した点群データから選択する基準点との対応付けを行うことで、より精度良く物体の位置、走行方向および速度を把握することができる。

本発明の第２の実施の形態は、第１の実施の形態による移動物体追跡方法において、一時刻前の検知対象車両位置の算出に用いた基準点と、現在の検知対象車両位置の算出に用いた基準点とを比較する基準点比較ステップを有し、基準点比較ステップで基準点が異なると判断した場合には、矩形枠の枠長さのデータを用いて現在の検知対象車両位置の補正を行うものである。本実施の形態によれば、一時刻前と現在とで検知対象車両位置の算出に用いた基準点が異なっていても、検知時刻での検知対象車両位置を精度良く予測することができる。

本発明の第３の実施の形態は、第１または第２の実施の形態による移動物体追跡方法において、矩形枠の一辺の中心点を、更に基準点の候補としたものである。本実施の形態によれば、候補となる基準点が4箇所増えて合計8箇所となるので、基準点の移動を最小限に抑え、より安定して精度良く検知対象車両を追跡することができ、特に大型トラックなど全長の長い車両を追跡する際に有効である。

本発明の第４の実施の形態による移動物体追跡装置は、送受信センサーによって取得する三次元の点群データから、物体を矩形枠によって物体の一部又は全体の形状を近似する近似部と、近似部で近似した矩形枠の角を基準点の候補として定義する基準点定義部と、物体について、少なくとも一時刻前の物体位置と現在の物体位置とから検知時刻での物体位置を予測する物体位置予測部と、物体位置予測部で予測した検知時刻での物体位置と、検知時刻での送受信センサー位置とから、基準点定義部で候補として定義した複数の基準点の中から、送受信センサーに最も近い位置となる基準点を選択する基準点確定部と、基準点確定部で選択した基準点を用いて検知時刻での物体位置を算出する物体位置算出部とを備えたものである。本実施の形態によれば、送受信センサーが物体の後方位置にあるときだけでなく、送受信センサーの物体に対する位置が、後方から側方へ、さらに側方から前方へなどと変わる場合であっても、物体の位置、走行方向および速度を正確に把握し、安定して物体を追跡することができる。また、送受信センサーに最も近い基準点は、検知時刻での物体位置を予測してあらかじめ選択することができるので、その予測した基準点と検知時刻において取得した点群データから選択する基準点との対応付けを行うことで、より精度良く物体の位置、移動方向および速度を把握することができる。

本発明の第５の実施の形態は、第４の実施の形態による移動物体追跡装置において、送受信センサーを自車両に搭載したものである。本実施の形態によれば、自車両から物体の位置を検知することができるため、自車両の運転をサポートする情報を外部と通信することなく即時に提供できる。

本発明の第６の実施の形態は、第５の実施の形態による移動物体追跡装置において、物体を、自車両周辺を走行する車両としたものである。本実施の形態によれば、自車両周辺を走行する車両の位置を正確に検知できるため、自車両の運転をサポートする情報を提供でき、更には自車両の自動運転を実現できる。

本発明の第７の実施の形態は、第４から第６のいずれかの実施の形態による移動物体追跡装置において、一時刻前の物体位置の算出に用いた基準点と、現在の物体位置の算出に用いた基準点とを比較する基準点比較部を備え、基準点比較部で基準点が異なると判断した場合には、矩形枠の枠長さのデータを用いて現在の物体位置の補正を行うものである。本実施の形態によれば、一時刻前と現在とで物体位置の算出に用いた基準点が異なっていても、検知時刻での物体位置を精度良く予測することができる。

以下本発明の一実施例による移動物体追跡装置について説明する。
図１は本実施例による移動物体追跡装置を示す概略構成図である。
本実施例による移動物体追跡装置は、計測手段１、制御手段２、記憶手段３、物体情報出力手段４、および自車両走行手段５を備えている。
計測手段１は、三次元の点群データを取得するもので、観測点の距離と方向を計測でき観測点の位置を計測する。計測手段１には、例えば複数個のレーザーを内蔵して水平全方位と垂直視野が３０度程度の三次元イメージングが可能なレーザー送受信センサーを用いることができる。
制御手段２は、計測手段１からの位置データを用いて自車両周辺を移動する物体の位置を時系列的に計測する。物体は、例えば自車両周辺を走行する車両である。
記憶手段３は、制御手段２で取り込んだ計測データや制御手段２で特定した物体に関するデータを記憶する。
物体情報出力手段４は、例えば自車両周辺を走行する車両の速度や走行方向を表示するなど、制御手段２で特定した物体に関するデータを自車両の運転をサポートする情報として出力する。
自車両走行手段５は、例えば駆動機構、減速機構、および操舵機構などの車両走行に必要な機構である。

制御手段２は、計測データ読み込み部２１、近似部２２、基準点定義部２３、基準点比較部２４、位置補正部２５、物体位置予測部２６、基準点確定部２７、物体位置算出部２８および自車両走行制御部２９を有している。
近似部２２では、計測データ読み込み部２１で読み込まれた三次元の点群データから、物体の一側面、背面および前面の内、少なくとも一つの面を一辺とする矩形枠によって物体の一部又は全体の形状を近似する。
基準点定義部２３では、近似部２２で近似した矩形枠の四つの角を基準点の候補として定義する。

基準点比較部２４では、一時刻前の物体位置の算出に用いた基準点および現在の物体位置の算出に用いた基準点を記憶手段３から読み出して、一時刻前の物体位置の算出に用いた基準点と、現在の物体位置の算出に用いた基準点とを比較する。基準点比較部２４で両者の基準点が異なると判断した場合には、位置補正部２５は、矩形枠の枠長さのデータを用いて現在の物体位置の補正を行う。
物体位置予測部２６では、物体について、記憶手段３に記憶された少なくとも一時刻前の物体位置と現在の物体位置とから検知時刻での物体位置を予測する。なお、位置補正部２５によって現在の物体位置が補正された場合は、補正前の現在の物体位置に代えて補正後の現在の物体位置を用いる。
なお、基準点比較部２４および位置補正部２５を省略し、一時刻前と現在の物体位置の算出に用いた基準点を比較および補正せずに、物体位置予測部２６で検知時刻での物体位置予測をすることもできるが、本実施例のように基準点比較部２４および位置補正部２５を設け、一時刻前と現在とで物体の算出に用いた基準点が異なる場合には補正を行うことで、検知時刻での物体位置を精度良く予測することができる。
基準点確定部２７では、物体位置予測部２６で予測した検知時刻での物体位置と、検知時刻での計測手段１の位置とから、基準点定義部２３で候補として定義した複数の基準点の中から、計測手段１に最も近い位置となる基準点を選択する。このように、計測手段１に最も近い基準点は、検知時刻での物体位置を予測してあらかじめ選択することができるので、その予測した基準点と検知時刻において取得した点群データから選択する基準点との対応付けを行うことで、より精度良く物体の位置、移動方向および速度を把握することができる。
基準点確定部２７で選択された計測手段１に最も近い基準点は、記憶手段３に記憶された後、基準点比較部２４に送出される。

物体位置算出部２８では、基準点確定部２７で選択した基準点と、時系列的に物体位置を計測することにより求められる物体の走行方向と速度等とを用いて、検知時刻での物体位置を算出する。
物体位置算出部２８で算出された検知時刻での物体位置は、記憶手段３に記憶されるとともに、物体情報出力手段４に送出される。
なお、計測手段１からは定期的に計測データが送出され、制御手段２におけるデータ処理も計測データが送出されるタイミングと同じタイミングで連続して行われる。
自車両走行制御部２９では、物体位置算出部２８で算出された検知時刻での物体位置に応じて自車両の走行制御を行う。自車両走行制御部２９における走行制御データは自車両走行手段５に送出される。

図２は本実施例による移動物体追跡方法を示すフローチャートである。
本実施例による移動物体認識方法は、計測手段（送受信センサー）１によって定期的に三次元の点群データを取得し、計測データ読み込み部２１ではこれらの位置データを取り込む（ステップ１）。
ステップ１のデータ取得ステップで取り込んだ位置データから、検知対象車両（物体）の一側面、検知対象車両の背面、および検知対象車両の前面の内、少なくとも一つの面を一辺とする矩形枠によって検知対象車両の一部または全体の形状を近似する（ステップ２）。
ステップ２で近似した矩形枠の角を基準点の候補として定義する（ステップ３）。

図３を用いて、矩形枠による検知対象車両形状の近似方法と基準点候補の定義方法について説明する。
まず、矩形枠Ｘによる検知対象車両形状の近似方法を説明する。例えば図３（ａ）のように、計測手段１が後方から検知対象車両Ａを計測する場合は、検知対象車両Ａの背面の位置データα１、α２、α３、・・・αｎが得られる。この位置データα１、α２、α３、・・・αｎを用いて制御手段２の近似部２２では、検知対象車両Ａの背面部分の形状を矩形枠Ｘで近似する。矩形枠Ｘは、検知対象車両Ａを上面視した状態で検知対象車両Ａの背面を一辺とするものであり、検知対象車両Ａの背面における車幅を長手方向とする細長い矩形枠Ｘである。
また、図３（ｂ）のように、計測手段１が右（図３（ｂ）において下側）斜め後方から検知対象車両Ａを計測する場合は、検知対象車両Ａの背面の位置データα１、α２、α３、・・・αｎと、検知対象車両Ａの右側面の位置データβ１、β２、β３、・・・βｎが得られる。この位置データα１、α２、α３、・・・αｎ、β１、β２、β３、・・・βｎを用いて近似部２２では、検知対象車両Ａの全体の形状を矩形枠Ｘで近似する。矩形枠Ｘは、検知対象車両Ａを上面視した状態で検知対象車両Ａの背面、前面、左側面および右側面を辺とするものであり、検知対象車両Ａの外形全体に外接するように設定される。矩形枠Ｘの枠長さは、車幅方向は検知対象車両Ａの全幅とほぼ同じであり、車長方向は検知対象車両Ａの全長とほぼ同じである。
このように、計測手段１が検知対象車両Ａの一つの面の位置データα１、α２、α３、・・・αｎを計測できる場合は、その面を一辺とする細長い矩形枠Ｘによって当該部分を近似することができ、計測手段１が検知対象車両Ａの隣接する二つの面の位置データα１、α２、α３、・・・αｎ、β１、β２、β３、・・・βｎを計測できる場合は、それらの面の辺を含む矩形枠Ｘによって検知対象車両Ａの全体の形状を近似することができる。

次に、基準点候補の定義方法について説明する。図３（ａ）、（ｂ）にあっては、Ｙ１、Ｙ２、Ｙ３およびＹ４が角に設定した基準点の候補である。本実施例においては、更に矩形枠Ｘの一辺の中心点も基準点の候補として定義する。図３（ａ）、（ｂ）にあっては、Ｙ５、Ｙ６、Ｙ７およびＹ８が各辺の中心に設定した基準点の候補である。
このように、候補となる基準点を角（Ｙ１、Ｙ２、Ｙ３およびＹ４）だけでなく各辺の中心（Ｙ５、Ｙ６、Ｙ７およびＹ８）にも設けることで、候補となる基準点が4箇所増えて合計8箇所となるので、基準点の移動を最小限に抑え、より安定して精度良く検知対象車両Ａを追跡することができ、特に大型トラックなど全長の長い車両を追跡する際に有効である。

図２に示すフローチャートにおいて、記憶手段３から、一時刻前の検知対象車両位置データ（位置データと基準点データ）の読み出しを行う（ステップ４）。
ステップ４で読み込んだ一時刻前の検知対象車両位置データを基に、一時刻前の検知対象車両位置の算出に用いた基準点と、現在の検知対象車両位置の算出に用いた基準点とを比較する（ステップ５）。ステップ５で両基準点が同じ場合には、検知対象車両Ａについて、一時刻前の検知対象車両位置と現在の検知対象車両位置とから検知時刻での検知対象車両位置を予測する（ステップ６）。
ステップ５において両基準点が異なると判断した場合には、位置補正部２５において矩形枠Ｘの枠長さのデータを用いて現在の検知対象車両位置の補正を行い（ステップ７）、その後にステップ６に移行する。例えば、一時刻前の検知対象車両位置の算出に用いた基準点がY１であり、現在の検知対象車両位置の算出に用いた基準点がY５であった場合には、Y５とY１との距離差の分だけ検知対象車両位置に誤差が生じるが、位置補正部２５がその誤差を補正することで、正確に現在の検知対象車両位置を求めることができる。
ステップ６で予測した検知時刻での検知対象車両位置と、検知時刻での自車両位置とから、ステップ３で候補として定義した複数の基準点の中から、計測手段１に最も近い位置となる基準点を確定する（ステップ８）。
ステップ８で選択した基準点を用いて検知時刻での検知対象車両位置を算出する（ステップ９）。

このように、検知時刻での検知対象車両位置を予測して計測手段１に最も近い基準点を予測しておくことで、その予測した基準点と検知時刻において実測により選択する基準点とを対応付けて計測手段１に最も近い基準点を確定することができる。従って、より精度良く検知対象車両Ａの位置、移動方向および速度を把握することができる。
自車両走行制御部２９では、ステップ９で算出した検知時刻での検知対象車両位置に関するデータを取り込み、当該データに基いて自車両の走行制御を行う。
ステップ９で検知時刻での検知対象車両位置を算出した後は、ステップ１に戻る。

図４および図５は本実施例の移動物体追跡装置を示す説明図であり、自車両から検知対象車両を追跡する状態を示している。
図４および図５では、検知対象車両Ａおよび自車両Ｂは前進走行中である。
自車両Ｂは、上部に計測手段１を備えている。計測手段１は、複数の発信部からパルス状に発光するレーザー３０を照射し、複数の受信部で散乱光を検出することによって、検知対象車両Ａから三次元の点群データを取得する。

図４（ａ）は自車両Ｂが検知対象車両Ａと同一車線の後方やや右寄りを走行している状態を示している。自車両Ｂは検知対象車両Ａから十分離れた後方に位置するため、計測手段１は検知対象車両Ａの背面の位置データα１、α２、α３、・・・αｎを取得する。この位置データα１、α２、α３、・・・αｎを用いて制御手段２の近似部２２では、検知対象車両Ａの背面部分の形状を矩形枠Ｘで近似する。矩形枠Ｘの車幅方向の枠長さは、検知対象車両Ａの背面部分の車幅とほぼ同じである。計測手段１に最も近い基準点はＹ１であるから、基準点Ｙ１を基準として検知対象車両Ａを追跡する。
図４（ｂ）は図４（ａ）の状態から自車両Ｂが右側の車線に移動しようとしている状態を示している。自車両Ｂは検知対象車両Ａから十分離れた後方に位置するため、図４（ａ）の状態と同様に、検知対象車両Ａの背面の位置データα１、α２、α３、・・・αｎを用いて検知対象車両Ａの背面部分の形状を矩形枠Ｘで近似する。この状態においても計測手段１に最も近い基準点はＹ１であるから、基準点Ｙ１を基準として検知対象車両Ａを追跡する。
図４（ｃ）は図４（ｂ）の状態から自車両Ｂが右側の車線への移動を完了し、検知対象車両Ａ近くの右斜め後方を走行している状態を示している。自車両Ｂが検知対象車両Ａ近くの右斜め後方に位置するため、計測手段１は、検知対象車両Ａの背面の位置データα１、α２、α３、・・・αｎと、検知対象車両Ａの右側面の位置データβ１、β２、β３、・・・βｎを取得する。この位置データβ１、β２、β３、・・・βｎを用いて近似部２２では、検知対象車両Ａの全体の形状を矩形枠Ｘで近似する。この状態においても計測手段１に最も近い基準点はＹ１であるから、基準点Ｙ１を基準として検知対象車両Ａを追跡する。

このように、矩形枠Ｘに設定した８箇所の基準点の中で自車両Ｂに搭載した計測手段１に最も近い基準点を基準として検知対象車両Ａの追跡を行うことで、検知対象車両Ａの後方を走行する自車両Ｂが車線を変更して、自車両Ｂと検知対象車両Ａとの相対位置が変化したとしても、検知対象車両Ａの位置を計測する基準点はＹ１のままであるため、安定して検知対象車両Ａの位置を時系列的に計測することができる。

図５（ａ）は図４（ｃ）の状態から自車両Ｂが検知対象車両Ａと並びかけている状態を示している。自車両Ｂは検知対象車両Ａの横側に位置するため、計測手段１は検知対象車両Ａの右側面の位置データβ１、β２、β３、・・・βｎを取得する。この位置データβ１、β２、β３、・・・βｎを用いて近似部２２では、検知対象車両Ａの右側面部分の形状を細長の矩形枠Ｘで近似する。矩形枠Ｘの全長方向の枠長さは、検知対象車両Ａの全長とほぼ同じである。この状態においても計測手段１に最も近い基準点はＹ１であるから、基準点Ｙ１を基準として検知対象車両Ａを追跡する。
図５（ｂ）は図５（ａ）の状態から自車両Ｂが検知対象車両Ａを少し追い越した状態を示している。自車両Ｂは図５（ａ）の状態と同様に、検知対象車両Ａの右側面の位置データβ１、β２、β３、・・・βｎを用いて検知対象車両Ａの右側面部分の形状を細長の矩形枠Ｘで近似する。この状態においては、計測手段１に最も近い基準点はＹ５であるから、基準点Ｙ５を基準として検知対象車両Ａを追跡する。
図５（ｃ）は図５（ｂ）の状態から自車両Ｂが検知対象車両Ａよりも更に前に出た状態を示している。自車両Ｂは図５（ａ）および（ｂ）の状態と同様に、検知対象車両Ａの右側面の位置データβ１、β２、β３、・・・βｎを用いて検知対象車両Ａの右側面部分の形状を細長の矩形枠Ｘで近似する。この状態においては、計測手段１に最も近い基準点はＹ２であるから、基準点Ｙ２を基準として検知対象車両Ａを追跡する。

このように、矩形枠Ｘに設定した８箇所の基準点の中で自車両Ｂに搭載した計測手段１に最も近い基準点を基準として検知対象車両Ａの追跡を行うことで、自車両Ｂが検知対象車両Ａを追い越し、自車両Ｂと検知対象車両Ａとの相対位置が変化したとしても、基準点の移動を最小限に抑えることができ、検知対象車両Ａを精度よく追跡することができる。

これに対して図６で示す従来例においては、 図６（ａ）に示すように、計測手段１が検知対象車両Ａの右斜め後方に位置しており、検知対象車両Ａの背面の位置データは得られるが右側面の位置データがほとんど得られないときは、背面の重心位置Ｚ１を基準として検知対象車両Ａを追跡する。そして、図６（ｂ）に示すように、計測手段１がやや右（図６（ｂ）において下側）に移動して、検知対象車両Ａの背面の位置データはほとんど得られないが右側面の位置データが得られるようになったときは、重心位置が右側面のＺ２に切り替わり、重心位置Ｚ２が検知対象車両Ａの追跡の基準となるため、実際には検知対象車両Ａは真直ぐ走行しているにも関わらず、右に方向転換したと追跡装置が誤認識して自車両Ｂが衝突回避行動を行うなどといった不具合が生じてしまう。

以上のように本実施例によれば、計測手段１が検知対象車両Ａの後方位置にあるときだけでなく、計測手段１の検知対象車両Ａに対する位置が、後方から側方へ、さらに側方から前方へなどと変わる場合であっても、検知対象車両Ａの位置、走行方向および速度を正確に把握し、安定して検知対象車両Ａを追跡することができる。また、計測手段１に最も近い基準点は、検知時刻での検知対象車両位置を予測してあらかじめ選択することができるので、その予測した基準点と検知時刻において取得した点群データから選択する基準点との対応付けを行うことで、より精度良く検知対象車両Ａの位置、走行方向および速度を把握することができる。

本発明は、四輪車だけでなく、二輪車などの移動物体に対しても適用できる。

１ 計測手段（送受信センサー）
２ 制御手段
２２ 近似部
２３ 基準点定義部
２４ 基準点比較部
２５ 位置補正部
２６ 物体位置予測部
２７ 基準点確定部
２８ 物体位置算出部

Claims (7)

1. レーザーを用いた送受信センサーを自車両に搭載し、前記自車両の周辺を走行する検知対象車両の位置を時系列的に計測する移動物体追跡方法であって、
   前記送受信センサーによって三次元の点群データを取得するデータ取得ステップと、
   前記データ取得ステップで取得した前記点群データから、前記検知対象車両の一側面、前記検知対象車両の背面、および前記検知対象車両の前面の内、少なくとも一つの面を一辺とする矩形枠によって前記検知対象車両の一部又は全体の形状を近似する近似ステップと、
   前記近似ステップで近似した前記矩形枠の角を基準点の候補として定義する基準点定義ステップと、
   前記検知対象車両について、少なくとも一時刻前の検知対象車両位置と現在の前記検知対象車両位置とから検知時刻での前記検知対象車両位置を予測する予測ステップと、
   前記予測ステップで予測した前記検知時刻での前記検知対象車両位置と、前記検知時刻での自車両位置とから、前記基準点定義ステップで候補として定義した複数の前記基準点の中から、前記送受信センサーに最も近い位置となる前記基準点を選択する基準点確定ステップと、
   前記基準点確定ステップで選択した前記基準点を用いて前記検知時刻での前記検知対象車両位置を算出する位置算出ステップと
   を有することを特徴とする移動物体追跡方法。
2. 一時刻前の前記検知対象車両位置の算出に用いた前記基準点と、現在の前記検知対象車両位置の算出に用いた前記基準点とを比較する基準点比較ステップを有し、
   前記基準点比較ステップで前記基準点が異なると判断した場合には、前記矩形枠の枠長さのデータを用いて現在の前記検知対象車両位置の補正を行うことを特徴とする請求項１に記載の移動物体追跡方法。
3. 前記矩形枠の一辺の中心点を、更に前記基準点の候補としたことを特徴とする請求項１または請求項２に記載の移動物体追跡方法。
4. 移動する物体の位置を時系列的に計測する移動物体追跡装置であって、
   送受信センサーによって取得する三次元の点群データから、矩形枠によって前記物体の一部又は全体の形状を近似する近似部と、
   前記近似部で近似した前記矩形枠の角を基準点の候補として定義する基準点定義部と、
   前記物体について、少なくとも一時刻前の物体位置と現在の前記物体位置とから検知時刻での前記物体位置を予測する物体位置予測部と、
   前記物体位置予測部で予測した前記検知時刻での前記物体位置と、前記検知時刻での送受信センサー位置とから、前記基準点定義部で候補として定義した複数の前記基準点の中から、前記送受信センサーに最も近い位置となる前記基準点を選択する基準点確定部と、
   前記基準点確定部で選択した前記基準点を用いて前記検知時刻での前記物体位置を算出する物体位置算出部と
   を備えたことを特徴とする移動物体追跡装置。
5. 前記送受信センサーを自車両に搭載したことを特徴とする請求項４に記載の移動物体追跡装置。
6. 前記物体を、前記自車両周辺を走行する車両としたことを特徴とする請求項５に記載の移動物体追跡装置。
7. 一時刻前の前記物体位置の算出に用いた前記基準点と、現在の前記物体位置の算出に用いた前記基準点とを比較する基準点比較部を備え、
   前記基準点比較部で前記基準点が異なると判断した場合には、前記矩形枠の枠長さのデータを用いて現在の前記物体位置の補正を行うことを特徴とする請求項４から請求項６のいずれかに記載の移動物体追跡装置。