JP2021163211A - 情報処理装置 - Google Patents

Abstract

【課題】より正確に移動体の速度を算出する、【解決手段】移動体方向が定められた移動体路ＲＷの基準点Ｏより上流にある第１の領域Ａ１の三次元情報に基づく第１の点群情報を時系列にし、第１の点群情報Ａ１から、２点間の距離に基づいて同一の移動体に対応する点群を推定し、同一の移動体に対応すると推定された点群から、基準点Ｏに移動体ＡＭが接近するにつれて小さく設定される先端部分領域ＡＴの点群を抽出し、当該抽出された点群に基づいて代表点を設定し、設定された代表点に基づいて、移動体ＡＭの速度ＶＡＭを算出する。【選択図】図１５

Description

本発明は、情報処理装置に関する。

物体の形状情報を得るために、ＬｉＤＡＲ（Ｌｉｇｈｔ Ｄｅｔｅｃｔｉｏｎ ａｎｄ Ｒａｎｇｉｎｇ、Ｌａｓｅｒ Ｉｍａｇｉｎｇ Ｄｅｔｅｃｔｉｏｎ ａｎｄ Ｒａｎｇｉｎｇ）が使用されている。この装置は、レーザレーダや光学式測距センサと呼ばれることもある。例えば、特許文献１には、ＬｉＤＡＲにより得られた点群情報により車両の幅や長さを推定する方法が開示されている。

ＬｉＤＡＲは、被検出空間にレーザ光を照射し、対象物で反射されて返ってくる反射光を受光することで、物体までの距離を測定する。照射するレーザ光の拡がり角が大きい場合は、レーザ光の反射成分が減少するために測定距離が短くなる特性がある。一方、照射するレーザ光の拡がり角が小さい場合は、レーザ光の反射成分が増加するために測定距離が長くなる特性がある（特許文献２、３）。

また、ＬｉＤＡＲは、照射するレーザ光をＭＥＭＳミラーなどで走査させることで、被検出空間の物体の形状情報を得る。ＭＥＭＳミラーによる走査角度を拡大するために、ＭＥＭＳミラーで偏向されたレーザ光にレンズを組み合わせることが開示されている（特許文献４）。広角レンズを組み合わせたタイプのＬｉＤＡＲは、走査範囲が広くなるが、レーザ光の拡がり角が大きくなるため測定距離が短くなる特性がある。一方、望遠レンズを組み合わせたタイプのＬｉＤＡＲは、走査範囲が狭くなるが、レーザ光の拡がり角が小さくなるため測定距離が長くなる特性がある。

計測環境によっては、上記のような複数タイプのＬｉＤＡＲを組み合わせることが行われている。

特開２０１７−１４６１５３号公報 特開２００６−３１７３０４号公報 国際公開第２０１２／１１７５４２号 特許第６０１９８６６号

ＬｉＤＡＲは、被検出空間にレーザ光を照射し、当該レーザ光が対象物で反射されて返ってくる反射光を受光することで、複数の検出方向毎の物体までの距離を測定することにより、対象物の形状を取得する。このため、対象物が黒色の車両などのレーザ光を反射しにくい物体の場合、ＬｉＤＡＲによる測定距離の精度が低下し、対象物の全体の形状を得ることができないことがある。

本発明が解決しようとする課題としては、レーザ光を反射しにくい材質が使われていても、移動中の移動体の長さの計測を高精度で行うことが一例として挙げられる。

上記課題を解決するために、請求項１に記載の発明は、移動体方向が定められた移動体路の基準点より上流にある第１の領域の三次元情報に基づく第１の点群情報を時系列に取得する点群取得部と、前記第１の点群情報から、２点間の距離に基づいて同一の移動体に対応する点群を推定するセグメンテーション部と、前記同一の移動体に対応すると推定された点群から先端部分領域の点群を抽出し、当該抽出された点群に基づいて代表点を設定する代表点設定部と、前記設定された代表点に基づいて、前記移動体の速度を算出する速度算出部と、を有し、前記代表点設定部は、前記基準点に前記移動体が接近するにつれ、前記先端部分領域を小さく設定する。

請求項５に記載の発明は、移動体方向が定められた移動体路の基準点より上流にある第１の領域の三次元情報に基づく第１の点群情報を時系列に取得する点群取得工程と、前記第１の点群情報から、２点間の距離に基づいて同一の移動体に対応する点群を推定するセグメンテーション工程と、前記同一の移動体に対応すると推定された点群から先端部分領域の点群を抽出し、当該抽出された点群に基づいて代表点を設定する代表点設定工程と、前記設定された代表点に基づいて、前記移動体の速度を算出する速度算出工程と、を有し、前記代表点設定工程において、前記基準点に前記移動体が接近するにつれ、前記先端部分領域は小さく設定される。

請求項６に記載の発明は、請求項５に記載の情報処理方法を、コンピュータにより実行させる。

請求項７に記載の発明は、請求項６に記載の情報処理プログラムを記憶している。

本発明の一実施例に係るＬｉＤＡＲシステム１０００を示す図である。 ＬｉＤＡＲ１００と第１の領域Ａ１、第２の領域Ａ２、第３の領域Ａ３の関係を示す図である。 ＬｉＤＡＲ１００の視野と第１の領域Ａ１、第２の領域Ａ２、第３の領域の関係を示す図である。 本発明の一実施例に係る制御部２００を示す図である。 第１の領域Ａ１を走行する車両ＡＭと、このときに測定された車両ＡＭに対応する点群の一例を図である。 第２の領域Ａ２を走行する車両ＡＭと、このときに測定された車両ＡＭに対応する点群の一例を図である。 第３の領域Ａ３を走行する車両ＡＭと、このときに測定された車両ＡＭに対応する点群の一例を示す図である。 車両の長さＬＡＭの算出の一例を説明する図である。 本発明の一実施例に係る制御部１２０における処理動作の一例を示す図である。 本発明の一実施例に係る制御部２００の車両速度算出部２２０を示す図である。 第１の領域Ａ１に２台の車両がいる場合の一例を示す図である。 先端部分領域ＡＴの一例を示す図である。 推定先端位置の移動の一例を示す図である。 本発明の一実施例に係る車両速度算出部２２０における処理動作の一例を示す図である。 セグメンテーション部２２１により車両ＡＭに対応すると推定される点群と先端部分領域ＡＴの幅ＷＡＴとの関係の一例を示す図である。 １つの車両に対応する点群が２つのセグメントに分離する例を示す図である。 点群と連続性判定領域ＡＣとの関係を示す図である。 点群の移動と連続性判定領域ＡＣとの関係の一例を示す図である。 本発明の一実施例に係る連続性判定部２２５における処理動作の一例を示す図である。 本発明の一実施例に係る車両速度算出部２２０における処理動作の一例を示す図である。 ＬｉＤＡＲによる走査を説明する図である。 第１の領域Ａ１における、車道ＲＷ上の物体の位置と、時刻情報が示す走査時刻（タイムスタンプ）と物体が走査される時刻との時間差と、を対応付けた規則の一例を示す図である。 点群と予測先端位置ｙＦＰと先端位置検出領域ＶＦＤとの関係を示す図である。 点群と先端位置検出領域ＶＦＤとの関係を示す図である。 本発明の一実施例に係る先端位置検出部２３０における処理動作の一例を示す図である。 点群と予測後端位置ｙＲＰと後端位置検出領域ＶＲＤとの関係を示す図である。 点群と後端位置検出領域ＶＲＤとの関係を示す図である。 本発明の一実施例に係る後端位置検出部２４０における処理動作の一例を示す図である。

本発明の一実施形態に係る情報処理装置は、移動体方向が定められた移動体路の基準点より上流にある第１の領域の三次元情報に基づく第１の点群情報を時系列に取得する点群取得部と、前記第１の点群情報から、２点間の距離に基づいて同一の移動体に対応する点群を推定するセグメンテーション部と、前記同一の移動体に対応すると推定された点群から先端部分領域の点群を抽出し、当該抽出された点群に基づいて代表点を設定する代表点設定部と、前記設定された代表点に基づいて、前記移動体の速度を算出する速度算出部と、を有し、前記代表点設定部は、前記基準点に前記移動体が接近するにつれ、前記先端部分領域を小さく設定する。このため、本実施形態では、代表点を設定する際に用いる先端部分領域の点群に、移動体の側面の点等の、移動体の前面以外の点が含まれることを防ぐことが可能になる。結果、本実施形態では、算出された移動体の速度が、実際の移動体の速度よりも小さくなることを防ぐことが可能になり、より正確に移動体の速度を算出することが可能になる。

前記点群情報取得部は、前記移動体路の第２の領域の三次元点群情報に基づく第２の点群情報と、前記第２の領域よりも下流にある第３の領域の三次元点群情報に基づく第３の点群情報と、を時系列に取得し、前記情報処理装置は、前記第２の点群情報から、第１の時刻における前記移動体の先端位置を算出する先端位置算出部と、前記第３の点群情報から、第２の時刻における前記移動体の後端位置を算出する後端位置算出部と、前記算出された前記移動体の速度、前記算出された前記第１の時刻における前記移動体の先端位置、および前記算出された第２の時刻における前記移動体の後端位置に基づき、前記移動体の長さを算出する移動体長算出部と、をさらに有するようにしても良い。このようにすることで、より正確に算出された移動体の速度に基づいて、移動体の長さを算出することが可能になる。結果、本実施形態では、より正確に移動体の長さを算出することが可能にある。

本発明の一実施形態に係るＬｉＤＡＲシステムは、前記情報処理装置と、前記第１の領域の三次元点群情報を生成するＬｉＤＡＲと、を有する。このため、本実施形態では、移動体の速度を算出するための点群情報を生成することが可能になる。

本発明の一実施形態に係るＬｉＤＡＲシステムは、前記情報処理装置と、前記第１の領域、前記第２の領域、および前記第３の領域の三次元点群情報を生成するＬｉＤＡＲと、を有する。このため、本実施形態では、移動体の速度、前端位置、後端位置、長さを算出するための点群情報を生成することが可能になる。

また、本発明の一実施形態にかかる情報処理方法は、移動体方向が定められた移動体路の基準点より上流にある第１の領域の三次元情報に基づく第１の点群情報を時系列に取得する点群取得工程と、前記第１の点群情報から、２点間の距離に基づいて同一の移動体に対応する点群を推定するセグメンテーション工程と、前記同一の移動体に対応すると推定された点群から先端部分領域の点群を抽出し、当該抽出された点群に基づいて代表点を設定する代表点設定工程と、前記設定された代表点に基づいて、前記移動体の速度を算出する速度算出工程と、を有し、前記代表点設定工程において、前記基準点に前記移動体が接近するにつれ、前記先端部分領域は小さく設定される。このため、本実施形態では、代表点を設定する際に用いる先端部分領域の点群に、移動体の側面の点等の、移動体の前面以外の点が含まれることを防ぐことが可能になる。結果、算出された移動体の速度が、実際の移動体の速度よりも小さくなることを防ぐことが可能になり、より正確に移動体の速度を算出することが可能になる。

また、本発明の一実施形態に係る情報処理プログラムは、上記の情報処理方法を、コンピュータに実行させる。このようにすることで、コンピュータを用いて、より正確に移動体の速度を算出することが可能になる。

また、本発明の一実施形態に係るコンピュータ読み取り可能な記憶媒体は、上記の情報処理プログラムを記憶している。このようにすることで、上記の情報処理プログラムを、機器に組み込む以外にも単体で流通することが可能になり、バージョンアップ等を容易に行うことが可能になる。

＜ＬｉＤＡＲシステム１０００＞
図１は、本発明の一実施例に係るＬｉＤＡＲシステム１０００を示す図である。ＬｉＤＡＲシステム１０００は、ＬｉＤＡＲ１００と、制御部２００と、を有する。本実施例では、ＬｉＤＡＲ１００により生成された三次元点群情報に基づいて、制御部２００により車両の長さを算出する。

ＬｉＤＡＲ１００は、車道（移動体路）ＲＷの手前側の車線を走行する車両（移動体）ＡＭの長さを算出するために用いる点群情報を生成する。このために、ＬｉＤＡＲ１００は、車道ＲＷの第１の領域Ａ１、第２の領域Ａ２、第３の領域Ａ３の３つの領域を測定できるように、例えば、図２に示すように、車道ＲＷの脇に配置される。ＬｉＤＡＲ１００は、車道ＲＷの上方に設置されても良く、３つの領域を上方から測定するようにしても良い。

ＬｉＤＡＲ１００は、車道ＲＷの第１の領域Ａ１、領域Ａ２、領域Ａ３の３つの領域に測定し、第１の領域Ａ１と第２の領域Ａ２と第３の領域Ａ３の三次元点群情報を時系列で生成する。つまり、ＬｉＤＡＲ１００は、所定のフレームレートで、車道ＲＷの第１の領域Ａ１、第２の領域Ａ２、第３の領域Ａ３の３つの領域を測定し、第１の領域Ａ１と第２の領域Ａ２と第３の領域Ａ３の三次元点群情報を、所定のフレームレートで生成する。よって、第１の領域Ａ１と第２の領域Ａ２と第３の領域Ａ３の三次元点群情報の各々は、所定のフレームレートで生成された複数のフレームで構成される。

各フレームにおいて、各点は、直交座標（ｘ、ｙ、ｚ）で表現される。このとき、図２に示すように、ＬｉＤＡＲ１００の位置を原点Ｏとし、車道ＲＷの幅方向をｘ軸方向とし、車道ＲＷの手前側の車線の走行方向をｙ軸方向とし、車道ＲＷの幅方向、走行方向に垂直な方向をｚ軸方向とする。

図２に示すように、第１の領域Ａ１は、車道ＲＷの手前側の車線の走行方向に関して、３つの領域のうちで一番上流にあり、第２の領域Ａ２は、第１の領域Ａ１の下流にあり、第３の領域Ａ３は、第２の領域Ａ２の下流にある。図２に示す例では、第１の領域Ａ１と第２の領域Ａ２は重なっていないが、第１の領域Ａ１と第２の領域Ａ２は、重なっていても良い。また、図２に示す例では、第２の領域Ａ２と第３の領域Ａ３は重なっていないが、第２の領域Ａ２と第３の領域Ａ３は、重なっていても良い。

図３は、ＬｉＤＡＲ１００の視野と第１の領域Ａ１、第２の領域Ａ２、第３の領域の関係を示す図である。ＬｉＤＡＲ１００は、第１の領域Ａ１、第２の領域Ａ２、第３の領域のすべての領域が視野に入る一台のＬｉＤＡＲにより構成されても良い。また、ＬｉＤＡＲ１００は、例えば、図３（Ａ）に示すように、第１の領域Ａ１が入る視野ＦＶ１を有する第１のＬｉＤＡＲ１１０と、第２の領域Ａ２が入る視野ＦＶ２を有する第２のＬｉＤＡＲ１２０と、第３の領域Ａ３が入る視野ＦＶ３を有する第３のＬｉＤＡＲの三台のＬｉＤＡＲ１３０により構成されても良い。また、ＬｉＤＡＲ１００は、例えば、図３（Ｂ）に示すように、第１の領域Ａ１と第２の領域Ａ２の両方が入る視野ＦＶ４を有する第４のＬｉＤＡＲ１４０と、第３の領域Ａ３が入る視野ＦＶ５を有する第５のＬｉＤＡＲ１５０の二台のＬｉＤＡＲにより構成されても良い。ＬｉＤＡＲ１００が複数のＬｉＤＡＲにより構成されているときは、複数のＬｉＤＡＲのうちのいずれかの１つが配置される位置を原点Ｏとすると良い。

図２に示すように、第１の領域Ａ１、第２の領域Ａ２は、ＬｉＤＡＲ１００の上流にあり、第３の領域Ａ３は、ＬｉＤＡＲ１００の下流にある。このため、ＬｉＤＡＲ１００は、第１の領域Ａ１、第２の領域Ａ２を走行する車両ＡＭの前方面（フロント面やボンネット、フロントガラスなど）と側方面（ＬｉＤＡＲ１００が配置されている側）を測定することが可能であり、また、第３の領域Ａ３を走行する車両ＡＭの後方面と側方面（ＬｉＤＡＲ１００が配置されている側）を測定することが可能である。

図４は、本発明の一実施例に係る制御部２００を示す図である。制御部２００は、点群情報取得部２１０と、車両速度算出部（移動体速度算出部）２２０と、先端位置検出部２３０と、後端位置検出部２４０と、車長算出部２５０と、を有する。制御部２００は、例えば、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）などを有するコンピュータにより構成される。

点群情報取得部２１０は、ＬｉＤＡＲ１００により生成された第１の領域Ａ１の三次元点群情報に基づく第１の点群情報、ＬｉＤＡＲ１００により生成された第２の領域Ａ２の三次元情報に基づく第２の点群情報、ＬｉＤＡＲ１００により生成された第３の領域Ａ３の三次元情報に基づく第３の点群情報を取得する。

本実施例では、制御部２００は、車両の長さを算出する。よって、制御部２００が、少なくとも車両の長さ方向、つまり、車両の走行方向（ｙ軸方向）の情報を取得できれば良い。そこで、第１の点群情報は、ＬｉＤＡＲにより生成された第１の領域Ａ１の三次元点群情報そのものでも良いし、この三次元点群情報のうちの一次元点群情報（各点のｙ座標値）であっても良いし、二次元点群情報（各点のｘ座標値、ｙ座標値）であっても良い。同様に、第２の点群情報（第３の点群情報）は、ＬｉＤＡＲにより生成された第２の領域Ａ２（第３の領域Ａ３）の三次元点群情報そのものでも良いし、この三次元点群情報のうちの一次元点群情報（各点のｙ座標値）であっても良いし、二次元点群情報（各点のｘ座標値、ｙ座標値）であっても良い。

本実施例では、制御部２００は、この点群情報取得部２１０により取得されたこの３つの点群情報に基づいて、車道ＲＷを走行する車両ＡＭの長さＬＡＭを算出する。具体的には、車両ＡＭが第１の領域Ａ１を走行しているときの情報（第１の点群情報）により、車両ＡＭの速度ＶＡＭを算出し、この車両ＡＭが第２の領域Ａ２を走行しているときの情報（第２の点群情報）により、第１の時刻Ｔ１における車両ＡＭの先端位置ＦＥを算出し、この車両ＡＭが第３の領域Ａ３を走行しているときの情報（第３の点群情報）により、第２の時刻Ｔ２における車両ＡＭの後端位置ＲＥを算出し、この算出された車両ＡＭの速度ＶＡＭ、第１の時刻Ｔ１における車両ＡＭの先端位置ＦＥ、第２の時刻Ｔ２における車両ＡＭの後端位置ＲＥから、車両ＡＭの長さＬＡＭを算出する。

車両速度算出部２２０は、第１の点群情報の時間変化により、車両ＡＭの速度ＶＡＭを算出する。図５（Ａ）は、第１の領域Ａ１を走行する車両ＡＭを示す図であり、図５（Ｂ）は、このときに測定された車両ＡＭに対応する点群を示す図である。第１の領域Ａ１は、図２、５に示すように、ＬｉＤＡＲ１００の上流にある。このため、図５（Ａ）に示すように、ＬｉＤＡＲ１００からは第１の領域Ａ１を走行する車両ＡＭの前方面（フロント面やボンネット、フロントガラスなど）と側方面（ＬｉＤＡＲ１００が配置されている側）が見え、第１の点群情報には、例えば、図５（Ｂ）示すように、車両ＡＭの前方面と側方面に対応する点群データが取得される。そこで、車両速度算出部２２０は、例えば、この点群を用いて、車両ＡＭの先端位置ＦＥの移動を検出することで、車両ＡＭの速度ＶＡＭを算出する。また、第１の領域Ａ１は、車両が移動する方向に広いため、比較的長い期間の点群情報を取得される。このことから、車両速度算出部２２０では、複数フレームの時系列データを、カルマンフィルタ等を用いることにより速度ＶＡＭの算出精度を向上させてもよい。

先端位置検出部２３０は、第２の点群情報に基づいて、第１の時刻Ｔ１における車両ＡＭの先端位置ＦＥを算出する。図６（Ａ）は、第２の領域Ａ２を走行する車両ＡＭを示す図であり、図６（Ｂ）は、このときに測定された車両ＡＭに対応する点群を示す図である。第２の領域Ａ２は、図２に示すように、ＬｉＤＡＲ１００の上流にある。このため、図６（Ａ）に示すように、ＬｉＤＡＲ１００からは第２の領域Ａ２を走行する車両ＡＭの前方面（フロント面やボンネット、フロントガラスなど）と側方面（ＬｉＤＡＲ１００が配置されている側）が見え、図６（Ｂ）に示すように、例えば、車両ＡＭの前方面と側方面に対応する点群データが取得される。そこで、先端位置検出部２３０は、例えば、第２の点群情報のうちの第１の時刻Ｔ１のフレームにおいて車両ＡＭの先端位置ＦＥに対応する点を検出することで、第１の時刻Ｔ１での車両ＡＭの先端位置ＦＥを得る。また、第２の領域Ａ２は車両が移動する方向に狭いため、比較的短い期間の点群情報が取得される。このことから、先端位置検出部２３０では、複数フレーム（例えば３個）の時系列データの時刻情報（Ｔ１１、Ｔ１２、Ｔ１３）と先端位置（ＦＥ１、ＦＥ２、ＦＥ３）の平均値を算出することにより、先端位置ＦＥの精度を向上させてもよい。

後端位置検出部２４０は、第３の点群情報に基づいて、第２の時刻における車両の後端位置ＲＥを算出する。図７（Ａ）は、第３の領域Ａ３を走行する車両ＡＭを示す図であり、図７（Ｂ）は、このときに測定された車両ＡＭに対応する点群を示す図である。第３の領域Ａ３は、図２に示すように、ＬｉＤＡＲ１００の下流にある。このため、図７（Ａ）に示すように、ＬｉＤＡＲ１００からは第３の領域Ａ３を走行する車両ＡＭの後方面と側方面が見え、第３の点群情報には、例えば、図７（Ｂ）に示すように、車両ＡＭの後方面と側方面に対応する点群データが取得される。そこで、後端位置検出部２４０は、例えば、第３の点群情報のうちの第２の時刻Ｔ２のフレームにおいて車両ＡＭの後端位置ＲＥに対応する点を検出することで、第２の時刻Ｔ２での車両ＡＭの先端位置ＲＥを得る。また、第３の領域Ａ３は車両が移動する方向に狭いため、比較的短い期間の点群情報が取得される。このことから、後端位置検出部２４０では、複数フレーム（例えば３個）の時系列データの時刻情報（Ｔ２１、Ｔ２２、Ｔ２３）と後端位置（ＲＥ１、ＲＥ２、ＲＥ３）の平均値を算出することにより、後端位置ＲＥの精度を向上させてもよい。

車長算出部２５０は、車両ＡＭの速度ＶＡＭと、第１の時刻Ｔ１における車両ＡＭの先端位置ＦＥと、第２の時刻Ｔ２における車両ＡＭの後端位置ＲＥと、に基づいて、車両ＡＭの長さＬＡＭを算出する。例えば、先端位置検出部２３０により算出された第１の時刻Ｔ１での先端位置ＦＥのｙ座標がｙＦＥＴ１であるとき（図８（Ａ））、車両ＡＭが、第１の時刻Ｔ１と第２の時刻Ｔ２の間、車両速度算出部２２０により算出された車両ＡＭの速度ＶＡＭで走行しているならば、第２の時刻Ｔ２での車両ＡＭの先端位置ＦＥのｙ座標はｙＦＥＴ２＝ｙＦＥＴ１＋ＶＡＭ（Ｔ２−Ｔ１）であると推測される（図８（Ｂ））。よって、後端位置検出部２４０により算出された第２の時刻Ｔ２での後端位置ＲＥのｙ座標がｙＲＥＴ２であるとき、車両ＡＭの長さＬＡＭは、例えば、ＬＡＭ＝ｙＦＥＴ２−ｙＲＥＴ２＝ｙＦＥＴ１−ｙＲＥＴ２＋ＶＡＭ（Ｔ２−Ｔ１）により算出することが可能である。

このように、本実施例では、車両の長さを得るために、走行中の車両の前面を含んだ第２の点群情報と走行中の車両の後面を含んだ第３の点群情報の２つの点群情報を用いている。結果、本実施例では、レーザ光を反射しにくい黒色の車両の場合であっても、車両の前面や後面に配置されたナンバープレートやバンパーなどのレーザ光を反射しやすい部分に対応する点群を用いることが可能になり、確実に車両の先端位置、後端位置を取得することが可能になる。このため、本実施例では、黒色の車両などレーザ光を反射しにくい、移動中の車両の長さの計測を高精度で行うことが可能である。

図９は、本発明の一実施例に係る制御部１２０における処理動作の一例を示す図である。点群情報取得部２１０が、車道ＲＷの第１の領域Ａ１の三次元情報に基づいた第１の点群情報、第２の領域Ａ２の三次元情報に基づいた第２の点群情報、第３の領域Ａ３の三次元情報に基づいた第３の点群情報を取得する（ステップＳ９０１）。速度算出部２１０は、第１の点群情報の時間変化に基づき、車両ＡＭの速度ＶＡＭを算出する（ステップＳ９０２）。先端位置検出部２３０は、第２の点群情報に基づいて、第１の時刻Ｔ１における車両ＡＭの先端位置ＦＥを算出する（ステップＳ９０３）。後端位置検出部２４０は、第３の点群情報に基づいて、第２の時刻Ｔ２における車両ＡＭの後端位置ＲＥを算出する（ステップＳ９０４）。車長算出部２５０は、車両ＡＭの速度ＶＡＭと、第１の時刻Ｔ１における車両ＡＭの先端位置ＦＥと、第２の時刻Ｔ２における車両ＡＭの後端位置ＲＥと、に基づいて、車両ＡＭの長さＬＡＭを算出する（ステップＳ９０５）。

＜車両の速度ＶＡＭの算出＞
図１０は、本発明の一実施例に係る制御部２００の車両速度算出部２２０を示す図である。車両速度算出部２２０は、セグメンテーション部２２１と、代表点設定部２２２と、先端位置移動推定部２２３と、速度算出部２２４と、を有する。

セグメンテーション部２２１は、第１の点群情報の各フレームにおいて、２点間の距離に基づいて同一の車両に対応する点群を推定する。

第１の点群情報の各フレームには、複数の車両が写っていることがある。図１１は、第１の領域Ａ１に２台の車両がいる場合の例を示す図である。図１１（Ａ）のように、第１の領域Ａ１に２台の車両（車両ＡＭ１、車両ＡＭ２）が走行している場合、第１の点群情報には、図１１（Ｂ）のように、２つの点群の塊が写る。

そこで、セグメンテーション部２２１は、第１の点群情報の各フレームにおいて、そのフレームの点を、各々が同一の車両に対応すると推定される点群（セグメント）に分割する。図１１（Ｂ）に示した例では、セグメンテーション部２２１は、フレームの点を、車両ＡＭ１に対応するセグメントＳ１と車両ＡＭ２に対応するセグメントＳ２の２つのセグメントに分割する。

このとき、セグメンテーション部２２１は、２点間の距離に基づいて、フレームの点を、複数のセグメントに分割する。具体的には、セグメンテーション部２２１は、各フレームにおいて、第１の距離（例えば、１ｍや１．５ｍ）以下の２点は同じセグメントに含まれるように、フレームの点を分割する。２点（Ｐ１（ｘ１、ｙ１、ｚ１）、Ｐ２（ｘ２、ｙ２、ｚ２））間の距離は、三次元距離（（（ｘ１−ｘ２）^２＋（ｙ１−ｙ２）^２＋（ｚ１―ｚ２）^２）^１／２）でも良いし、二次元距離（（（ｘ１−ｘ２）^２＋（ｙ１−ｙ２）^２）^１／２）でも良いし、一次元距離（｜ｙ１−ｙ２｜）でも良い。

代表点設定部２２２は、第１の点群情報の各フレームにおいて、同一の車両に対応すると推定された点群（セグメント）の各々から先端部分領域の点群を抽出し、当該抽出された点群に基づいて代表点を設定する。

セグメンテーション部２２１により点を分割することで得られたセグメントの各々の最も先端の点（最もｙ座標値が大きい点）は、そのセグメントに対応する車両の先端位置を示す可能性が高い。よって、例えば、各フレームでセグメントのこの最も先端の点を検出し、この検出された最も先端の点の時間変化、つまり、最も先端の点の移動に基づいて、このセグメントに対応する車両の速度を得ることが可能である。

しかしながら、この最も先端の点は、測距精度が悪い可能性もある。そこで、本実施例では、代表点設定部２２２により、各セグメントに対して、そのセグメントを代表する代表点を設定し、この最も先端の点の移動の代わりに、この代表点の移動に基づいて、各セグメントに対応する車両の速度を算出する。

このとき、本実施例では、代表点設定部２２２は、図１２に示すように、各セグメントに対して、最も先端の点である先端点ＰＴから後方に拡がる領域（先端部分領域ＡＴ）を設定する。つまり、代表点設定部２２２は、先端点のｙ座標をｙＰＴであるとき、ｙＰＴ−ＷＡＴ≦ｙ≦ｙＰＴにより定まる領域を先端部分領域ＡＴとして設定する。このとき、先端部分領域ＡＴは、ｙ軸方向の幅ＷＡＴを持つ領域となる。幅ＷＡＴは、例えば、１０ｃｍ〜１ｍにすると良い。

そして、代表点設定部２２２は、この先端部分領域ＡＴ内の点のｙ座標を平均し、この平均値を代表点として設定する。つまり、本実施例では、代表点は、先端部分領域内ＡＴの点のｙ座標値のみにより設定されており、代表点は、ｙ座標値のみを持っている。例えば、先端部分領域ＡＴに、ｎ個の点：Ｐ１（ｘ１、ｙ１、ｚ１）、Ｐ２（ｘ２、ｙ２、ｚ２）、Ｐ３（ｘ３、ｙ３、ｚ３）、・・・、Ｐｎ（ｘｎ、ｙｎ、ｚｎ）が含まれているとき、代表点のｙ座標は、（ｙ１＋ｙ２＋ｙ３＋・・・＋ｙｎ）／ｎである。

先端位置移動推定部２２３は、代表点設定部２２２により設定された代表点に基づいて、車両の先端位置の推定値である推定先端位置を算出する。このとき、単純に、代表点のｙ座標値を、車両の先端位置の推定値である推定先端位置としても良い。また、先端位置移動推定部２２３は、代表点のｙ座標値にカルマンフィルタを適用することで、車両の先端位置の推定値である推定先端位置の移動を算出するようにしても良い。つまり、先端位置移動推定部２２３は、各フレームにおいて、代表点のｙ座標値を車両の先端位置の観測値とし、カルマンフィルタを用いて、車両の先端位置を推定する（つまり、推定先端位置を算出する）ようにしても良い。このとき、本実施例において、代表点は、ｙ座標値のみを有するので、推定先端位置もｙ座標値のみで決まる位置である。

以上のように、本実施例では、各フレームにおいて、セグメンテーション部２２１よりフレームの点を同一の車両に対応する点群（セグメント）に分割し、そして、このセグメントごとに、代表点設定部２２２により代表点を設定し、そして、セグメントごとに、先端位置移動推定部２２３により設定された代表点に基づいて推定先端位置を算出する。このように算出された推定先端位置（つまり、ｙ座標値）は、図１３のように、フレームが進むごとに、つまり、時間が経過するにつれ、移動する。図１３は、車両の先端がフレームＦ１で第１の領域Ａ１に入り、フレームＦ６の後に第１の領域Ａ１を出る場合の、この車両に対応する推定先端位置の変化の一例を示している。本実施例では、この推定先端位置がどのように移動するのかを見ることで、この推定先端位置に対応する車両の移動を見る。

速度算出部２２４は、第１の領域Ａ１内に設定された速度算出領域ＡＶに推定先端位置が達したときに、推定先端位置の移動に基づいて、この推定先端位置に対応する車両の速度を算出する。速度算出領域ＡＶは、例えば、ｙＡＶＬ≦ｙ≦ｙＡＶＨにより定まる領域として設定される。つまり、速度算出部２２４は、推定先端位置の値が、ｙＡＶＬを超えたときに、推定先端位置の移動に基づいて、この推定先端位置に対応する車両の速度を算出する。

車両の速度の算出としては、２つのフレームの間での推定先端位置の移動を使っても良いし、複数フレームの間での移動を使っても良い。図１３に示した例であれば、推定先端位置は、フレームＦ７で速度算出領域ＡＶに達するため、フレームＦ７に対する推定先端位置を算出した段階で、車両の速度を算出する。そこで、図１３に示した例であれば、例えば、速度算出領域ＡＶに達したときのフレーム（フレームＦ７）とその直前のフレーム（フレームＦ６）との間での推定先端位置の移動に基づいて車両の速度を算出しても良いし、車両の先端が車両Ａ１に入ってから出るまでのフレーム（フレームＦ１−Ｆ７）の間での推定先端位置の移動に基づいて車両の速度を算出しても良い。あるいは、前述のカルマンフィルタ処理では、先端位置の推定と同時に移動速度も算出することから、カルマンフィルタ算出の速度値を使用しても良い。

図１４は、本実施例に係る車両速度算出部２２０における処理動作の一例を示す図である。図１４では、第１の点群情報がＭ１枚のフレームＦｎ（ｎ＝１、２、３、・・・、Ｍ１）で構成されているとしている。車両速度算出部２２０は、ｎ＝１と設定する（ステップＳ１４０１）。セグメンテーション部２２１は、フレームＦｎの点を、同一の車両に対応すると推定される点群（セグメント）に分割する（ステップＳ１４０２）。代表点設定部２２２は、同一の車両に対応すると推定された点群（セグメント）の各々に対して、代表点を設定する（ステップＳ１４０３）。先端位置移動推定部２２３は、同一の車両に対応すると推定された点群（セグメント）の各々に対して、設定された代表点に基づいて、当該セグメントに対応する車両の先端位置の推定値である推定先端位置を算出する（ステップＳ１４０４）。

フレームＦｎにおいて、推定先端位置が速度算出領域ＡＶに達しているセグメントがあるならば（ステップＳ１４０５、ＹＥＳ）、速度算出部２２４は、その推定先端位置が速度算出領域ＡＶに達しているセグメントに対応する車両の速度を算出し（ステップＳ１４０６）、ｎ＝Ｍ１であるのか否かを確認する（ステップＳ１４０７）。ｎ＝Ｍ１であるならば（ステップＳ５０７、ＹＥＳ）、車両速度算出部２２０は、処理を終了する。ｎ＝Ｍ１でないならば（ステップＳ１４０７、ＮＯ）、車両速度算出部２２０は、ｎ＝ｎ＋１と設定し（ステップＳ１４０８）、ステップＳ１４０２に戻る。

フレームＦｎにおいて、推定先端位置が速度算出領域ＡＶに達しているセグメントがないならば（ステップＳ１４０５、ＮＯ）、車両速度算出部２２０は、ｎ＝Ｍ１であるのか否かを確認する（ステップＳ１４０７）。

＜先端部分領域のサイズ＞
上記実施例において、代表点設定部２２２は、先端部分領域ＡＴの点群を基づいて代表点を設定する。図１５に示すように、車両ＡＭがＬｉＤＡＲ１００から遠方にあるときにセグメンテーション部２２１により車両ＡＭに対応すると推定される点群（図１５（Ａ））は、車両ＡＭがＬｉＤＡＲ１００の近傍にあるときにセグメンテーション部２２１により車両ＡＭに対応すると推定される点群（図１５（Ｂ））に比べ、点の数が少ない。このため、車両ＡＭがＬｉＤＡＲ１００から遠方にあるときに、先端部分領域ＡＴのｙ方向の幅ＷＡＴとして小さい幅ＷＴＡ１を用いると、先端領域ＡＴに入る点の数が少なくなり、また、近傍点よりも遠方点の方が測距精度が悪くなる傾向があることから、ノイズの影響が大きくなる可能性がある。そこで、車両ＡＭがＬｉＤＡＲ１００から遠方にあるときの先端部分領域ＡＴのｙ方向の幅ＷＡＴは、図１５（Ａ）の幅ＷＴＡ２のように、ある程度大きくする必要がある。しかしながら、車両ＡＭがＬｉＤＡＲ１００の近傍にあるときにも、先端部分領域ＡＴのｙ方向の幅ＷＡＴとして、車両ＡＭがＬｉＤＡＲ１００から遠方にあるときに用いる幅ＷＡＴ２を用いると、この先端部分領域ＡＴには、車両ＡＭの前面の点だけでなく、車両ＡＭの側面の点も含まれるようになる。このようなとき、先端部分領域ＡＴに入る点のｙ座標値の平均である代表点は、実際の先端位置のｙ座標値よりも小さくなる。このため、この代表値に基づいて推定される推定先端位置は、実際の先端位置よりも後方に位置することになってしまう。結果、この代表点を用いて、先端位置の推定をし、車両ＡＭの速度ＶＡＭを算出した場合、算出された速度ＶＡＭは、車両ＡＭの実際の速度よりも小さくなってしまう。

そこで、本発明の一実施例に係る代表点設定部２２２は、ＬｉＤＡＲ１００に車両ＡＭが接近するにつれ、先端部分領域ＡＴのｙ方向の幅ＷＡＴを小さく設定する。このとき、代表点設定部２２２は、例えば、ＬｉＤＡＲ１００と車両ＡＭとの距離に基づいて、先端部分領域ＡＴのｙ方向の幅ＷＡＴを設定する。つまり、代表点設定部２２２は、例えば、ＬｉＤＡＲ１００と車両ＡＭとの距離が小さくなるにつれ、先端部分領域ＡＴのｙ方向の幅ＷＡＴを小さく設定する。

代表点設定部２２２は、例えば、各フレームＦｎにおいて、ＬｉＤＡＲ１００と車両ＡＭとの距離を、１つの前のフレームＦｎ−１での当該車両ＡＭに対応するセグメントに対する推定先端位置を用いることで算出するようにすると良い。つまり、代表点設定部２２２は、例えば、１つの前のフレームＦｎ−１の推定先端位置と原点Ｏとの距離ＤＡＭが小さくなるにつれ、各フレームＦｎにおける先端部分領域ＡＴのｙ方向の幅ＷＡＴを小さく設定するようにすると良い。

このとき、幅ＷＡＴは、距離ＤＡＭに対して、連続に変化するようにしても良いし、不連続に変化するようにしても良い。例えば、距離ＤＡＭに対して、複数の閾値を用意し、距離ＤＡＭがその閾値を下回るごとに幅ＷＡＴが変化するようにしても良い。特に、２つの閾値（第１の閾値（例えば、２５ｍ）、第２の閾値（例えば、１７ｍ））を用意し、距離ＤＡＭが第１の閾値以上のときは、幅ＷＡＴを第１の幅（例えば、６０ｃｍ）とし、距離ＤＡＭが第１の閾値より小さく、第２の閾値以上のときは、幅ＷＡＴを第２の幅（例えば、３０ｃｍ）とし、距離ＤＡＭが第２の閾値よりも小さいときは、幅ＷＡＴを第３の幅（例えば、１５ｃｍ）とすると良い。

＜点群の連続性＞
セグメンテーション部２２１は、各フレームにおいて、第１の距離以下の２点が同じセグメントに含まれるように、フレームの点群を分割する。このとき、異なる車両に対応する点が同じセグメントに入らないようにするために、この第１の距離は、小さい値に設定する必要がある。しかしながら、第１の距離が小さ過ぎると、実際は同じ車両に対応する点が、異なる２つのセグメントに分かれてしまうことがある。例えば、図１６（Ａ）のように、トラクタＴＲ１がトレーラーＴＲ２を牽引するセミトレーラーＴＲの場合は、図１６（Ｂ）のように、トラクタＴＲ１とトレーラーＴＲ２のそれぞれの前面、側面の点群が写るため、１台のセミトラクターが２つのセグメント（トラクタＴＲ１に対応するセグメントとトレーラーＴＲ２に対応するセグメント）に分離する場合がある。

そこで、本発明の一実施例に係る車両速度算出部２２０は、連続性判定部２２５をさらに有する。連続性判定部２２５は、第１の領域Ａ１内に設定された連続性判定領域ＡＣ内に含まれる点群の時間的変化に基づいて、第１の点群情報の点の連続性を判定する。連続性判定領域ＡＣは、例えば、ｙＡＣＬ≦ｙ≦ｙＡＣＨにより定まる領域として設定される。

例えば、連続性判定部２２５は、連続性判定領域ＡＣにおける第１の点群情報の点の連続性を示す連続性状態を記憶するようにすると良い。例えば、連続性判定部２２５は、図１７（Ａ）のように、連続性判定領域ＡＣに点が入っていないときは、そのフレームの連続性状態を「ＯＦＦ」であると記憶し、図１７（Ｂ）に示すように、連続性判定領域ＡＣに点が入っているときは、そのフレームの連続性状態を「ＯＮ」であると記憶する。つまり、図１８のように、３つの点（ＰＣ１、ＰＣ２、ＰＣ３）が移動しているとき、図１８（Ａ）、１８（Ｇ）に示したフレームの連続性状態は「ＯＦＦ」であると記憶され、図１８（Ｂ）〜１８（Ｆ）に示したフレームの連続性状態は「ＯＮ」であると記憶される。

よって、３つの点のうちの先端の点ＰＣ１が連続性判定領域ＡＣに達したフレーム（図１８（Ｂ））の１つの前のフレーム（図１８（Ａ））の連続性状態は「ＯＦＦ」である。一方、３つの点のうちの先端でない点ＰＣ２が連続性判定領域ＡＣに達したフレーム（図１８（Ｃ））の１つの前のフレーム（図１８（Ｂ））の連続性状態は「ＯＮ」である。よって、各点が連続性判定領域ＡＣに達したフレームの１つの前の連続性状態を確認することで、その点が連続する点の先端の点であるのか否かを確認することが可能になる。よって、連続性判定部２２５は、第１の点群情報の各点が他の点と連続であるのか否かの判定することが可能である。

本実施例において、先端位置推定部２２３は、セグメンテーション部２２１により異なる移動体に対応すると推定された２つの点群（セグメント）の点が連続であると連続性判定部２２５により判定された場合、当該２つの点群（セグメント）は、同一の車両に対応する点群であるとする。先端位置推定部２２３は、例えば、推定先端位置が連続性判定領域ＡＣに達したときに、達した際のフレームの１つ前のフレームの連続性状態を確認し、連続性状態が「ＯＮ」であるときは、その推定先端位置に対応するセグメントは、当該セグメントの前方にあるセグメントが対応する同一の車両に対応すると判定する。

そして、速度算出部２２４は、先端位置推定部２２３により同一の車両に対応すると判定された２つのセグメントに対しては、１つの速度のみが算出されるようにする。つまり、速度算出部２２４は、速度算出領域ＡＶに推定先端位置が達したときに、この推定先端位置に対応するセグメントが先端位置推定部２２３により同一の車両に対応すると判定された複数のセグメントの最も前方のセグメントであるならば、車両の速度の算出をする。一方で、速度算出部２２４は、速度算出領域ＡＶに推定先端位置が達したときに、この推定先端位置に対応するセグメントが先端位置推定部２２３により同一の車両に対応すると判定された複数のセグメントの最も前方のセグメントでないならば、そのセグメントに対しては、車両の速度の算出をしない。このようにすることで、セグメンテーション部２２１により、１つの車両が２つのセグメントに分離されたとしても、それぞれのセグメントに対して、車両の速度が算出されることを防ぐことが可能になる。あるいは、前述のカルマンフィルタ処理では、先端位置の推定と同時に移動速度も算出することから、速度算出部２２４では、カルマンフィルタで算出されている移動速度を選択する処理でも良い。

このとき、速度算出領域ＡＶの下限ｙＡＶＬと、連続性判定領域ＡＣの下限ｙＡＣＬと、が同じになるように速度算出領域ＡＶ、連続性領域ＡＣを設定すると良い。このようにすることで、推定先端位置がｙＡＶＬ以上になったときに、同時に、推定先端位置はｙＡＣＬ以上になり、推定先端位置の連続性を判定するタイミングと、この推定先端位置に対応する車両の速度を算出するタイミングを同じにすることが可能になる。

連続性判定領域ＡＣのｙ軸方向の幅ｙＡＣＨ−ｙＡＣＬは、車道ＲＷを走行中の車両が取る車間距離より短くするようにする。このようにすることで、連続性判定領域ＡＣに２台の車両に入ることがなくなる。このため、本実施例では、連続判定領域ＡＣに点が入っている点は、すべてが１台の車両に対応する点である。結果、本実施例では、連続性判定部２２５は、連続性判定領域ＡＣに含まれる点群の時間的変化を確認し、連続性判定領域ＡＣに点が含まれた状態が続いている限りは、１台の車両が連続性判定領域ＡＣを通過しているとし、この間に連続性判定領域ＡＣに入った点は１台の点であると判定することが可能である。走行中の車両は、一般に、１０ｍ以上の車間距離を取る。そこで、連続判定領域ＡＣのｙ軸方向の幅ｙＡＣＨ−ｙＡＣＬは、１０ｍより小さい値、例えば、３ｍにすると良い。

車両ＡＭがＬｉＤＡＲ１００の近傍にあるときに測定された車両ＡＭに対応する点群の数は、車両ＡＭがＬｉＤＡＲ１００から遠方にあるときに測定された車両ＡＭに対応する点群の数より多い。そこで、連続性判定領域ＡＣは、ＬｉＤＡＲ１００に近傍に位置するようにすると良い。このようにすることで、同一の車両ＡＭに対応する点間の距離が小さくなり、より正確に点群の連続性を判定することが可能になる。

図１９は、本実施例に係る連続性判定部２２５における処理動作の一例を示す図である。図１９では、第１の点群情報がＭ１枚のフレームＦｎ（ｎ＝１、２、３、・・・、Ｍ１）で構成されているとしている。連続性判定部２２５は、ｎ＝１と設定する（ステップＳ１９０１）。連続性判定部２２５は、連続性判定領域ＡＣに点が存在するのか否かを確認する（ステップＳ１９０２）。連続性判定領域ＡＣに点が存在するときは（ステップＳ１９０２、ＹＥＳ）、フレームＦｎの連続性状態を「ＯＮ」と記憶し（ステップＳ１９０３）、ｎ＝Ｍ１であるのか否かを確認する（ステップＳ１９０４）。ｎ＝Ｍ１であるならば（ステップＳ１９０４、ＹＥＳ）、連続性判定部２２５は、処理を終了する。ｎ＝Ｍ１でないならば（ステップＳ１９０４、ＮＯ）、車両速度算出部２２０は、ｎ＝ｎ＋１と設定し（ステップＳ１９０５）、ステップＳ１９０２に戻る。

連続性判定領域ＡＣに点が存在しないときは（ステップＳ１９０２、ＮＯ）、フレームＦｎの連続性状態を「ＯＦＦ」と記憶し（ステップＳ１９０６）、ｎ＝Ｍ１であるのか否かを確認する（ステップＳ１９０４）。

図２０は、本実施例に係る車両速度算出部２２０における処理動作の一例を示す図である。図２０では、第１の点群情報がＭ１枚のフレームＦｎ（ｎ＝１、２、３、・・・、Ｍ１）で構成されているとしている。車両速度算出部２２０は、ｎ＝１と設定する（ステップＳ２００１）。セグメンテーション部２２１は、フレームＦｎの点を、同一の車両に対応すると推定される点群のセグメントに分割する（ステップＳ２００２）。代表点設定部２２２は、同一の車両に対応すると推定された点群のセグメントの各々に対して、代表点を設定する（ステップＳ２００３）。先端位置移動推定部２２３は、同一の車両に対応すると推定された点群のセグメントの各々に対して、設定された代表点に基づいて、当該セグメントに対応する車両の先端位置の推定値である推定先端位置を算出する（ステップＳ２００４）。

フレームＦｎにおいて、推定先端位置が速度算出領域ＡＶに達しているセグメントがあるならば（ステップＳ２００５、ＹＥＳ）、先端位置移動推定部２２３は、当該推定先端位置に対応するセグメントの点が、このセグメントの前方のセグメントの点と連続であると連続性判定部２２５により判定されたのか否かを確認する（ステップＳ２００６）。連続でない判定された場合（ステップＳ２００６、ＮＯ）、速度算出部２２４は、このセグメントに対応する車両の速度を算出する（ステップＳ２００９）。連続であると判定された場合（ステップＳ２００６、ＹＥＳ）、先端位置移動推定部２２３は、このセグメントは前方のセグメントに対応する車両と同一の車両に対応するとして、速度算出部２２４による車両の速度計算を行わない。

フレームＦｎにおいて、推定先端位置が速度算出領域ＡＶに達しているセグメントがないならば（ステップＳ２００５、ＮＯ）、車両速度算出部２２０は、ｎ＝Ｍ１であるのか否かを確認する（ステップＳ２００７）。

車両速度算出部２２０は、ｎ＝Ｍ１であるのか否かを確認する（ステップＳ２００７）。ｎ＝Ｍ１であるならば（ステップＳ２００７、ＹＥＳ）、車両速度算出部２２０は、処理を終了する。ｎ＝Ｍ１でないならば（ステップＳ２００７、ＮＯ）、車両速度算出部２２０は、ｎ＝ｎ＋１と設定し（ステップＳ２００８）、ステップＳ２００２に戻る。

＜代表点に対する時間補正＞
図２１に示すように、上方から下方に（もしくは下方から上方に）走査することで１つのフレームのデータを生成するＬｉＤＡＲの場合、走査レイヤの各々が実際に走査される時刻とフレームのタイムスタンプの時刻との間には差が生じる。図２１に示した例では、範囲ＡＳ全体を走査するために、左から右への走査が１０回行われている。例えば、各フレームのタイムスタンプとして走査開始された時刻が用いられている場合は、範囲ＡＳ全体を走査するのにかかる時間をＴＳとすると、各フレームにおいて、フレームのタイムスタンプとｎ番目のレイヤが走査される時刻との差は、（ｎ−１）ＴＳ／１０である。

よって、このようなＬｉＤＡＲにより生成されたデータに基づいて、車両の速度を算出するときは、この時間差を考慮することで、より正確な速度を算出することが可能になる。例えば、各フレームのタイムスタンプとして走査開始された時刻が用いられている場合は、ｎ番目のレイヤが走査される時刻は、フレームのタイムスタンプより遅い時刻である。結果、上記のような時間差を考慮しない場合には、算出された車両の速度は、実際の車両の速度よりも大きいものになってしまう。しかしながら、本実施例では、車両の速度を算出する際に用いる代表点は、先端部分領域ＡＴ内の点のｙ座標の平均値である。このため、代表点がどの走査レイヤの点であるかの情報は無く、上記のような時間差を考慮した時間補正をすることができない。

そこで、本発明の一実施例に係る車両速度算出部２２０は、代表点走査時刻推定部２２６をさらに有する。代表点走査時刻推定部２２６は、車道ＲＷ上の物体の位置と、時刻情報が示す走査時刻（タイムスタンプ）と物体が走査される時刻との時間差と、を対応付けた規則、および代表点の位置を用いて、時刻情報が示す走査時刻（タイムスタンプ）を補正し、代表点が走査された時刻を推定する。そして、本実施例において、先端位置移動推定部２２３は、代表点走査時刻推定部２２６により代表点が走査されたと推定された時刻を用いて、車両の先端位置の推定値である推定先端位置を算出する。また、速度算出部２２４は、代表点走査時刻推定部２２６により代表点が走査されたと推定された時刻を用いて、車両の速度を算出する。

図２２は、第１の領域Ａ１における、車道ＲＷ上の物体の位置と、時刻情報が示す走査時刻（タイムスタンプ）と物体が走査される時刻との時間差と、を対応付けた規則の一例を示す図である。この図では、横軸は、ｙ座標であり、縦軸は、フレームのタイムスタンプと、その物体が走査される時間と、の時間差である。つまり、図２２では、ｙ＝ｙｓ１にある物体が実際に走査される時間とフレームとの時間差は、ｔｓ１である。この規則は、車道ＲＷ上で車両を走行させ、その車両をＬｉＤＡＲ１００により測定することや、車道ＲＷの手前側の車線の中心線上に複数の物体を配置し、この複数の物体をＬｉＤＡＲ１００により測定することなどをし、事前に得ておく。

代表点走査時刻推定部２２６は、各フレームにおいて、代表点設定部２２２により設定された代表点の値と上記規則から、代表点が走査された時刻を推定する。例えば、各フレームのタイムスタンプとして走査開始された時刻が用いられている場合は、図２２に示した例では、タイムスタンプがＴＦｎであるフレームＦｎで代表点設定部２２２により設定された代表点がｙｓ１であれば、代表点が走査された時刻は、ＴＦｎ＋ｔｓ１であると推定される。

＜先端位置の算出＞
先端位置検出部２３０は、車両速度算出部２２０により車速ＶＡＭが算出された車両ＡＭに対して、第２の点群情報に基づいて、第１の時刻Ｔ１でのその車両ＡＭの先端位置ＦＥを算出する。第１の領域Ａ１で車速ＶＡＭであった車両ＡＭは、この車速ＶＡＭとほぼ同じ車速で、第1の領域Ａ１の下流にある第２の領域Ａ２に達すると考えられる。そこで、先端位置検出部２３０は、第２の領域Ａ２のうちの、この車速ＶＡＭにより車両ＡＭの先端位置ＦＥが存在すると予測される領域（先端位置検出領域ＶＦＤ）において、この車両の先端位置ＦＥを検出する。

まず、先端位置検出部２３０は、車両速度算出部２２０により車速ＶＡＭが算出された車両ＡＭに対して、その車両の先端位置ＦＥが存在すると予測される位置のｙ座標値（予測先端位置ｙＦＰ）を算出する。上述したように、車両速度算出部２２０の速度算出部２２４は、この推定先端位置が速度算出領域ＡＶに達したときに、車両ＡＭの速度ＶＡＭを算出する。そこで、先端位置検出部２３０は、例えば、この推定先端位置が速度算出領域ＡＶに達した時刻ｔＶＡＭと推定先端位置の値ｙＶＡＭと、車速ＶＡＭを取得する。このようにすることで、先端位置検出部２３０は、例えば、時刻ｔでの予測先端位置ｙＦＰを、ｙＦＰ＝ｙＶＡＭ＋ＶＡＭ（ｔ−ｔＶＡＭ）により求めることが可能である（図２３）。

そして、先端位置検出部２３０は、この予測先端位置ｙＦＰを含む領域を先端位置検出領域ＶＦＤとして設定する。先端位置検出領域ＶＦＤは、例えば、ｙＶＦＤＬ≦ｙ≦ｙＶＦＤＨにより定まる領域として設定される。車両ＡＭの先端位置ＦＥは、この予測先端位置ｙＦＰより前方に位置することも、後方に位置することもあるため、先端位置検出領域ＶＦＤのｙ座標値の下限ｙＶＦＤＬ、上限ｙＶＦＤＨは、図２３（Ｂ）に示すように、予測先端位置ｙＦＰを挟むように設定される（ｙＶＦＤＬ＜ｙＦＰ＜ｙＶＦＤＨ）。

そして、先端位置検出部２３０は、先端位置検出領域ＶＦＤの上限ｙＶＦＤＨが第２の領域Ａ２にあるときに（図２４）、この先端位置検出領域ＶＦＤ内の点群から最も先端の点ＰＦ（最もｙ座標が大きい点）を検出する。このとき、先端位置検出部２３０は、先端位置検出領域ＶＦＤの上限ｙＶＦＤＨが第２の領域Ａ２にあるときのフレームのすべてにおいて、先端の点ＰＦを検出しても良いし、先端位置検出領域ＶＦＤの上限ｙＶＦＤＨが第２の領域Ａ２にあるときのフレームの一部で、先端の点ＰＦを検出しても良い。

先端の点ＰＦを検出するのに用いたフレームの数がＭ２枚であるとき、先端位置検出部２３０は、Ｍ２個の先端の点ＰＦｎ（１、・・、Ｍ２）を検出する。先端位置検出部２３０は、先端位置ＦＥとし、このＭ２個の先端の点ＰＦｎのｙ座標値ｙＰＦｎの平均（ｙＰＦｎ＋・・・＋ｙＰＦＭ２）／Ｍ２を算出する。また、先端位置検出部２３０は、第１の時刻Ｔ１として、先端の点ＰＦｎが検出されたＭ２個のフレームＦｎのタイムスタンプＴＦｎの平均（ＴＦ１＋・・・＋ＴＦＭ２）／Ｍ２を算出する。

図２５は、本実施例に係る先端位置検出部２３０における処理動作の一例を示す図である。先端位置検出部２３０は、車両速度算出部２２０により車速ＶＡＭが算出された車両ＡＭに対して、予測先端位置ｙＦＰを算出する（ステップＳ２５０１）。先端位置検出部２３０は、この予測先端位置ｙＦＰを含む領域を先端位置検出領域ＶＦＤとして設定する（ステップＳ２５０２）。先端位置検出部２３０は、先端位置検出領域ＶＦＤの上限ｙＶＦＤＨが第２の領域Ａ２にある間のフレームＦｎにおいて、この先端位置検出領域ＶＦＤ内の点群から最も先端の点ＰＦｎを検出する（ステップＳ２５０３）。先端位置検出部２３０は、検出された先端の点ＰＦｎと、先端の点ＰＦｎが検出されたフレームＦｎのタイムスタンプＴＦｎに基づき、第１の時刻Ｔ１における車両ＡＭの先端位置ＦＥを算出する（ステップＳ２５０４）。

＜後端位置の算出＞
後端位置検出部２４０は、車両速度算出部２２０により車速ＶＡＭが算出された車両ＡＭに対して、第３の点群情報に基づいて、第２の時刻Ｔ２での後端位置ＲＥを算出する。第１の領域Ａ１で車速ＶＡＭであった車両ＡＭは、この車速ＶＡＭとほぼ同じ車速で、第1の領域Ａ１の下流にある第３の領域Ａ３に達すると考えられる。そこで、後端位置検出部２４０は、この車速ＶＡＭにより車両ＡＭの後端位置ＲＥが存在すると予測される領域（後端位置検出領域ＶＲＤ）において、この車両の後端位置ＲＥを検出する。

後端位置検出部２４０も、先端位置検出部２３０同様に、車両速度算出部２２０により車速ＶＡＭが算出された車両ＡＭの先端位置ＦＥが存在すると予測される位置のｙ座標値（予測先端位置ｙＰＦ）を算出する。そして、後端位置検出部２４０は、この車両ＡＭの先端位置ＦＥが存在すると予測される位置の推定車長（推定移動体長）ＬＥだけ後方の位置のｙ座標値を後端位置ＲＥが存在すると予測される位置（予測後端位置ｙＰＲ）とする。つまり、予測後端位置ｙＰＲは、ｙＰＲ＝ｙＰＦ−ＬＥである。ここで、推定車両ＬＥは、車両ＡＭの長さの推定値であり、典型的な車両の長さを用いるようにしても良いし、下記で詳述するように、第１の点群情報に基づいて算出するようにしても良い。

そして、後端位置検出部２４０は、この予測後端位置ｙＲＰを含む領域を後端位置検出領域ＶＲＤとして設定する。後端位置検出領域ＶＲＤは、例えば、ｙＶＲＤＬ≦ｙ≦ｙＶＲＤＨにより定まる領域として設定される。車両ＡＭの後端位置ＲＥは、この予測後端位置ｙＲＰより前方に位置することも、後方に位置することもあるため、後端位置検出領域ＶＲＤの下限ｙＶＲＤＬ、上限ｙＶＲＤＨは、図２６に示すように、予測後端位置ｙＲＰを挟むように配置される（ｙＶＲＤＬ＜ｙＲＰ＜ｙＶＲＤＨ）。図２６に示した例では、後端位置検出領域ＶＲＤの上限ｙＶＲＤＨを予測先端位置ｙＦＰと同じ位置に設定しているが、予測先端位置ｙＦＰより後方に設定するようにしても良い。

そして、後端位置検出部２４０は、後端位置検出領域ＶＲＤの下限ｙＶＲＤＬが第３の領域Ａ３にあるときに（図２７）、この後端位置検出領域ＶＲＤ内の点群から最も後端の点ＰＲ（最もｙ座標が小さい点）を検出する。このとき、後端位置検出部２４０は、後端位置検出領域ＶＲＤの下限ｙＶＲＤＬが第３の領域Ａ３にあるときのフレームのすべてにおいて、後端の点ＰＲを検出しても良いし、後端位置検出領域ＶＲＤの下限ｙＶＲＤＬが第３の領域Ａ３にあるときのフレームの一部で、後端の点ＰＲを検出しても良い。

後端の点ＰＲを検出するのに用いたフレームの数がＭ３枚であるとき、後端位置検出部２４０は、Ｍ３個の後端の点ＰＲｎ（１、・・、Ｍ３）を検出する。そして、後端位置検出部２４０は、後端位置ＲＥとし、このＭ３個の後端の点ＰＲｎのｙ座標値ｙＰＲｎの平均（ｙＰＲｎ＋・・・＋ｙＰＲＭ３）／Ｍ３を算出する。また、後端位置検出部２４０は、第２の時刻Ｔ２として、後端の点ＰＲｎが検出されたＭ３個のフレームＦｎのタイムスタンプＴＦｎの平均（ＴＦ１＋・・・＋ＴＦＭ３）／Ｍ３を算出する。

図２８は、本実施例に係る後端位置検出部２４０における処理動作の一例を示す図である。後端位置検出部２４０は、車両速度算出部２２０により車速ＶＡＭが算出された車両ＡＭに対して、予測後端位置ｙＲＰを算出する（ステップＳ２８０１）。後端位置検出部２４０は、この予測後端位置ｙＲＰを含む領域を後端位置検出領域ＶＲＤとして設定する（ステップＳ２８０２）。後端位置検出部２４０は、後端位置検出領域ＶＲＤの下限ｙＶＲＤＬが第３の領域Ａ３にある間のフレームＦｎにおいて、この後端位置検出領域ＶＲＤ内の点群から最も後端の点ＰＲｎを検出する（ステップＳ２８０３）。後端位置検出部２４０は、検出された後端の点ＰＲｎと、後端の点ＰＲｎが検出されたフレームＦｎのタイムスタンプＴＦｎに基づき、第２の時刻Ｔ２における車両ＡＭの後端位置ＲＥを算出する（ステップＳ２８０４）。

＜推定車長＞
推定車長ＬＥは、例えば、典型的な車両の長さを用いるようにしても良い。しかしながら、二輪車とセミトレーラーとでは、車長が大きく異なる。そこで、本発明の一実施例に係る制御部２００の車両速度算出部２２０は、第１の点群情報に基づいて、車両ＡＭの車長を推定する値である推定車長ＬＥを算出する推定車長算出部２２７をさらに有する。そして、本実施例に係る後端位置検出部２４０は、この推定車長算出部２２７により算出された推定車長ＬＥを用いて、予測後端位置ｙＰＲを算出する。

推定車長算出部２２７は、連続性判定領域ＡＣに含まれる点群の時間的変化に基づいて、車両ＡＭの車長の推定値である推定車長ＬＥを算出する。推定車長算出部２２７は、連続性判定部２２５同様に、連続するフレームに、連続性判定領域ＡＣに点が存在するうちは、同一の車両の点群が連続していると判定する。つまり、推定車長算出部２２７は、図１８の３つの点ＰＣ１、ＰＣ２、ＰＣ３は、連続であると判定する。

そして、推定車長算出部２２７は、連続している点の先端の点と後端の点の距離を求めることで、推定車長ＬＥを算出する。図１８の例では、点ＰＣ１と点ＰＣ３の距離を求める。例えば、連続している点の先端の点が連続性判定領域ＡＣに入ったときのフレームＦＥでの先端の点のｙ座標値ｙＥであり、そのフレームＦＥのタイムスタンプＴＦＥであり、連続している点の後端の点が連続性判定領域ＡＣを出たときのフレームＦＬでの後端の点のｙ座標値ｙＬであり、そのフレームＦＬのタイムスタンプＴＦＬであるとすると、推定車長ＬＥは、この連続する点に対応する車両の速度がＶＡＭであれば、推定車長ＬＥは、ＬＥ＝ｙＥ−ｙＬ＋ＶＡＭ（ＴＦＬ−ＴＦＥ）で求まる。また、推定車長ＬＥは、ＡＣの幅とタイムスタンプから、ＬＥ＝ＡＣ（ＴＦＬ−ＴＦＥ）として算出しても良い。

以上、本発明の好適な実施の形態により本発明を説明した。ここでは特定の具体例を示して本発明を説明したが、特許請求の範囲に記載した本発明の趣旨および範囲から逸脱することなく、これら具体例に様々な修正および変更が可能である。

１０００ ＬｉＤＡＲシステム
１００ ＬｉＤＡＲ
２００ 制御部
２１０ 点群情報取得部
２２０ 車両速度算出部
２２１ セグメンテーション部
２２２ 代表点設定部
２２３ 先端位置移動推定部
２２４ 速度算出部
２２５ 連続性判定部
２２６ 代表点走査時間推定部
２２７ 推定車長算出部
２３０ 先端位置検出部
２４０ 後端位置検出部
２５０ 車長算出部

Claims (7)

1. 移動体方向が定められた移動体路の基準点より上流にある第１の領域の三次元情報に基づく第１の点群情報を時系列に取得する点群取得部と、
   前記第１の点群情報から、２点間の距離に基づいて同一の移動体に対応する点群を推定するセグメンテーション部と、
   前記同一の移動体に対応すると推定された点群から先端部分領域の点群を抽出し、当該抽出された点群に基づいて代表点を設定する代表点設定部と、
   前記設定された代表点に基づいて、前記移動体の速度を算出する速度算出部と、を有し、
   前記代表点設定部は、前記基準点に前記移動体が接近するにつれ、前記先端部分領域を小さく設定する、情報処理装置。
2. 前記点群情報取得部は、前記移動体路の第２の領域の三次元点群情報に基づく第２の点群情報と、前記第２の領域よりも下流にある第３の領域の三次元点群情報に基づく第３の点群情報と、を時系列に取得し、
   前記情報処理装置は、
   前記第２の点群情報から、第１の時刻における前記移動体の先端位置を算出する先端位置算出部と、
   前記第３の点群情報から、第２の時刻における前記移動体の後端位置を算出する後端位置算出部と、
   前記算出された前記移動体の速度、前記算出された前記第１の時刻における前記移動体の先端位置、および前記算出された第２の時刻における前記移動体の後端位置に基づき、前記移動体の長さを算出する移動体長算出部と、をさらに有する、請求項１に記載された情報処理装置。
3. 請求項１に記載の情報処理装置と、
   前記第１の領域の三次元点群情報を生成するＬｉＤＡＲと、を有するＬｉＤＡＲシステム。
4. 請求項２に記載の情報処理装置と、
   前記第１の領域、前記第２の領域、および前記第３の領域の三次元点群情報を生成するＬｉＤＡＲと、を有するＬｉＤＡＲシステム。
5. 移動体方向が定められた移動体路の基準点より上流にある第１の領域の三次元情報に基づく第１の点群情報を時系列に取得する点群取得工程と、
   前記第１の点群情報から、２点間の距離に基づいて同一の移動体に対応する点群を推定するセグメンテーション工程と、
   前記同一の移動体に対応すると推定された点群から先端部分領域の点群を抽出し、当該抽出された点群に基づいて代表点を設定する代表点設定工程と、
   前記設定された代表点に基づいて、前記移動体の速度を算出する速度算出工程と、を有し、
   前記代表点設定工程において、前記基準点に前記移動体が接近するにつれ、前記先端部分領域は小さく設定される、情報処理方法。
6. 請求項５に記載の情報処理方法を、コンピュータに実行させる情報処理プログラム。
7. 請求項６に記載の情報処理プログラムを記憶しているコンピュータ読み取り可能な記憶媒体。

Images