JP7078066B2

JP7078066B2 - 位置算出システム、プログラムおよび位置算出方法

Info

Publication number: JP7078066B2
Application number: JP2020096040A
Authority: JP
Inventors: 駿志岡本; 美智代松井
Original assignee: Oki Electric Industry Co Ltd
Current assignee: Oki Electric Industry Co Ltd
Priority date: 2020-06-02
Filing date: 2020-06-02
Publication date: 2022-05-31
Anticipated expiration: 2040-06-02
Also published as: JP2021189882A

Description

本発明は、位置算出システム、プログラムおよび位置算出方法に関する。

近年、自律的に走行する自動運転車両を実現するための技術が盛んに研究されている。自動運転車両による自動運転は、自動運転車両の周囲を走行する他の車両の認識に基づいて実現され得る。他の車両の認識に関し、以下の特許文献１および特許文献２などに開示されているように、ＬｉＤＡＲ（ＬａｓｅｒＩｍａｇｉｎｇＤｅｔｅｃｔｉｏｎａｎｄＲａｎｇｉｎｇ）と呼ばれるセンサ装置が知られている。

特開２０２０－２７４１５号公報特開２０１６－１７７８０４号公報

しかし、高速道路の合流地点、および見通しの悪い交差点などにおいては、自動運転車両から他の車両までの距離、または死角の発生により、自動運転車両に搭載されたセンサ装置だけでは自動運転車両の周囲の交通状況を正確に把握することが困難となる場合がある。

このため、路側にもセンサ装置を設置し、自動運転車両に搭載されたセンサ装置の死角となる範囲を路側に設置されたセンサ装置がカバーし、路側に設置されたセンサ装置が交通状況を自動運転車両に伝える方法が考えられる。例えば路側に設置されたセンサ装置側で高速道路の本線を走行する車両の車間距離を計測できると、当該車間距離と各車両の合流地点までの距離とに基づき、自動運転車両が本線車両に合流するための走行計画を立てることが可能である。また、交差点においては、対向車、人または自転車等の移動物体が何秒後に交差点に到着および侵入するかが分かると、自動運転車両の右折判断または事故の回避に繋げることができる。

ここで、車間距離は、前方車両の後尾部位と、後方車両の先頭部位との間の距離で算出されることが望ましい。また、交差点への到達時刻は、車両の先頭部位から交差点までの距離と車両の速度から算出されることが望ましい。

そこで、本発明は、上記問題に鑑みてなされたものであり、本発明の目的とするところは、物体における進行方向を基準にして特定される部位の位置を算出することが可能な、新規かつ改良された位置算出システム、プログラムおよび位置算出方法を提供することにある。

上記課題を解決するために、本発明のある観点によれば、レーザーセンサから複数の時点における点群を取得する点群取得部と、前記点群取得部により取得された前記複数の時点における検出データの各々から物体を検出する物体検出部と、前記物体検出部による前記物体の検出結果に基づいて前記物体の進行方向を算出する進行方向算出部と、前記物体の点群を、第１座標軸および第２座標軸により規定される座標空間上で、前記物体の進行方向が前記第１座標軸の方向に一致するように第１の角度だけ回転させ、回転後の前記物体の点群のうち、前記第１座標軸上の値が最大となる点、または最小となる点を特定し、特定された点の座標値および前記座標空間における前記第１の角度の逆回転に基づき、前記物体の先頭部位または前記後尾部位の位置を算出する位置算出部と、を備える、位置算出システムが提供される。

前記位置算出システムは、前記位置算出部により算出された位置を示す位置表示画面を生成する表示制御部をさらに備えてもよい。

前記物体検出部は、前記複数の時点における前記物体の代表位置を検出し、前記進行方向算出部は、前記物体の代表位置の移動方向を前記進行方向として算出してもよい。

前記進行方向を基準にして特定される部位は、前記進行方向に対する先頭部位または後尾部位を含んでもよい。

前記位置算出システムは、前記進行方向と所定の関係にある方向上における前記物体のサイズを算出する物体サイズ算出部をさらに備えてもよい。

前記進行方向と所定の関係にある方向は前記進行方向に直交する幅方向を含み、前記物体サイズ算出部は、前記回転後の前記物体における前記第２座標軸上の最大値および最小値との差分から前記物体の幅サイズを算出してもよい。

前記進行方向と所定の関係にある方向は前記進行方向に一致する前後方向を含み、前記物体サイズ算出部は、前記物体の前記先頭部位および前記後尾部位の間の距離を前記物体の前後サイズとして算出してもよい。

前記位置算出システムは、第１の物体に搭載された装置を含み、前記第１の物体に搭載された装置は、第２の物体について算出された位置を示す位置表示画面を表示する表示部を有してもよい。

前記位置算出システムは、前記位置算出部により算出された位置と他の位置との距離に基づき、前記他の位置に前記物体の前記部位が到達する時刻を算出する到達時刻予測部をさらに備えてもよい。

前記位置算出システムは、前記進行方向算出部により算出された前記物体の進行方向に基づき、前記物体の以降の動きを予測する動き予測部をさらに備えてもよい。

また、上記課題を解決するために、本発明の別の観点によれば、コンピュータを、レーザーセンサから複数の時点における点群を取得する点群取得部と、前記点群取得部により取得された前記複数の時点における点群の各々から物体を検出する物体検出部と、前記物体の点群を、第１座標軸および第２座標軸により規定される座標空間上で、前記物体の進行方向が前記第１座標軸の方向に一致するように第１の角度だけ回転させ、回転後の前記物体の点群のうち、前記第１座標軸上の値が最大となる点、または最小となる点を特定し、特定された点の座標値および前記座標空間における前記第１の角度の逆回転に基づき、前記物体の先頭部位または後尾部位の位置を算出する位置算出部と、として機能させるための、プログラムが提供される。

また、上記課題を解決するために、本発明の別の観点によれば、レーザーセンサから複数の時点における点群を取得することと、前記複数の時点における点群の各々から物体を検出することと、前記物体の検出結果に基づいて前記物体の進行方向を算出することと、前記物体の点群を、第１座標軸および第２座標軸により規定される座標空間上で、前記物体の進行方向が前記第１座標軸の方向に一致するように第１の角度だけ回転させ、回転後の前記物体の点群のうち、前記第１座標軸上の値が最大となる点、または最小となる点を特定し、特定された点の座標値および前記座標空間における前記第１の角度の逆回転に基づき、前記物体の先頭部位または後尾部位の位置を算出することと、を含む、コンピュータにより実行される位置算出方法が提供される。

以上説明した本発明によれば、物体における進行方向を基準にして特定される部位の位置を算出することが可能である。

本発明の一実施形態による位置算出システムの構成例を示す説明図である。本発明の一実施形態による運転支援装置２００の構成を示す説明図である。位置表示画面の具体例を示す説明図である。本発明の一実施形態による位置算出装置５０の構成を示す説明図である。物体の代表位置の移動例を示す説明図である。車両３０の先頭部位および後尾部位の位置を算出する方法を示す説明図である。車両３０の先頭部位および後尾部位の位置を算出する方法を示す説明図である。車両３０の先頭部位および後尾部位の位置を算出する方法を示す説明図である。車両３０の先頭部位および後尾部位の位置を算出する方法を示す説明図である。車両３０の先頭部位および後尾部位の位置を算出する方法を示す説明図である。本発明の一実施形態による位置算出装置５０の動作を示すフローチャートである。位置算出およびサイズ算出の詳細を示すフローチャートである。位置算出装置５０のハードウェア構成を示したブロック図である。

以下に添付図面を参照しながら、本発明の実施の形態について詳細に説明する。なお、本明細書及び図面において、実質的に同一の機能構成を有する構成要素については、同一の符号を付することにより重複説明を省略する。

また、本明細書及び図面において、実質的に同一の機能構成を有する複数の構成要素を、同一の符号の後に異なるアルファベットを付して区別する場合もある。例えば、実質的に同一の機能構成または論理的意義を有する複数の構成を、必要に応じて車両３０Ａ、３０Ｂおよび３０Ｃのように区別する。ただし、実質的に同一の機能構成を有する複数の構成要素の各々を特に区別する必要がない場合、複数の構成要素の各々に同一符号のみを付する。例えば、車両３０Ａ、３０Ｂおよび３０Ｃを特に区別する必要が無い場合には、各車両を単に車両３０と称する。

＜１．位置算出システムの概要＞
本発明の一実施形態は、物体における一部位の位置を算出するための位置算出システムに関する。物体としては、車両、船舶および飛行体などの移動物体、人間および動物などの生物などが挙げられる。車両、船舶および飛行体などの物体は、物体内に操舵者が存在する有人式であってもよいし、物体内に操舵者が存在しない無人式であってもよい。以下では、物体が車両である例を主に想定して本発明の一実施形態による位置算出システムを説明する。

図１は、本発明の一実施形態による位置算出システムの構成例を示す説明図である。図１に示した例では、支線道路１２が本線道路１４に合流している。また、支線道路１２を車両２０（第１の物体）が走行しており、本線道路１４を車両３０Ｂ～３０Ｅ（第２の物体）が走行している。車両２０は、図１に示した状態の後も支線道路１２で走行を続け、合流地点において本線道路１４に入ることにより、本線道路１４を走行する車両３０に合流する。車両２０は、運転支援装置２００を搭載しており、運転支援装置２００による支援に基づいて本線道路１４を走行する車両３０に合流することができる。なお、本明細書では、支線道路１２を走行する車両を合流車両と称し、本線道路１４を走行する車両を本線車両と称する場合もある。

図１に示したように、本発明の一実施形態による位置算出システムは、上述した車両２０および車両３０に加え、路側に位置するレーザーセンサ４０および位置算出装置５０を有する。レーザーセンサ４０は、センサの一例であり、レーザーセンサ４０の周辺における物体の物理的配置を示す検出データを取得する。例えば、レーザーセンサ４０はＬｉＤＡＲであり、本線道路１４を走行する車両３０の形状を示す点群を検出データとして取得する。

位置算出装置５０は、レーザーセンサ４０により取得された点群に基づき、車両３０の位置を算出する。ここで、車両２０は、２つの車両３０の間隔部分に合流する。２つの車両３０の間隔部分は、厳密には、前側の車両３０の後尾部位と、後側の車両３０の先頭部位との間の部分である。このため、位置算出装置５０は、各車両３０の進行方向を算出し、各車両３０の進行方向に対する先頭部位の位置および後尾部位の位置を算出する。そして、位置算出装置５０は、算出された先頭部位の位置および後尾部位の位置を車両２０に搭載されている運転支援装置２００に送信する。

車両２０の運転支援装置２００は、位置算出装置５０から受信された情報に基づいて車両２０の自動運転を制御することで、本線道路１４を走行する車両３０の車間部分に適切に合流することができる。または、運転支援装置２００は、合流地点までの速度調整情報などを示す表示または音声を出力することにより、合流地点までの速度調整情報を運転手に伝えてもよい。

上記では、本発明の一実施形態による位置算出システムが車両２０の合流支援に適用される例を説明したが、本発明の一実施形態による位置算出システムは他の用途にも適用可能である。例えば、本発明の一実施形態による位置算出システムは、交差点における右折支援にも適用可能である。具体的には、位置算出装置５０が交差点の周囲の各車両の進行方向、進行方向に対する先頭部位の位置および後尾部位の位置を算出することで、交差点を右折しようとする車両が対向車の車間部分で適切に右折することを支援することが可能である。

以上、本発明の一実施形態による位置算出システムの概要を説明した。続いて、本発明の一実施形態による位置算出システムの構成および動作を順次詳細に説明する。

＜２．運転支援装置の構成＞
図２は、本発明の一実施形態による運転支援装置２００の構成を示す説明図である。図２に示したように、本発明の一実施形態による運転支援装置２００は、通信部２１０、自車両情報検出部２２０、計画算出部２３０、車両制御部２４０、表示制御部２５０および表示部２６０を備える。

（通信部２１０）
通信部２１０は、位置算出装置５０と無線通信を行う構成である。通信部２１０は、位置算出装置５０から、位置算出装置５０により算出された各車両３０の進行方向に対する先頭部位の位置および後尾部位の位置を受信する。その他、通信部２１０は、各車両３０のサイズ、代表位置などを示す情報も受信し得る。

（自車両情報検出部２２０）
自車両情報検出部２２０は、運転支援装置２００を搭載する車両２０（すなわち、自車両）の情報を検出する。例えば、自車両情報検出部２２０は、自車両の位置、速度および合流地点までの距離を取得する。このような自車両情報検出部２２０は、汎用的に車両に搭載されている車速パルス発生器や慣性センサ（加速度センサ、ジャイロスコープ）、カーナビゲーションで利用される地図情報、またはＧＮＳＳ（ＧｌｏｂａｌＮａｖｉｇａｔｉｏｎＳａｔｅｌｌｉｔｅＳｙｓｔｅｍ）などを有してもよい。

（計画算出部２３０）
計画算出部２３０は、自車両情報検出部２２０により検出された情報、および位置算出装置５０から受信された情報に基づき、自車両が本線車両に合流するための走行計画を算出する。例えば、計画算出部２３０は、本線道路１４における本線車両の走行位置の分布に基づき、自車両が合流することが困難な範囲を特定する。そして、計画算出部２４８は、他の範囲である合流候補範囲内で本線車両に合流するための自車両の速度を算出する。

（車両制御部２４０）
車両制御部２４０は、自車両の走行を自動制御するための構成である。車両制御部２４０は、計画算出部２３０により算出された走行計画に従って自車両の速度調整を行う。

（表示制御部２５０、表示部２６０）
表示制御部２５０は、多様な表示画面を生成する。表示部２６０は、表示制御部２５０により生成された表示画面を表示する。例えば、表示部２６０は、通信部２１０により受信された各車両３０の進行方向に対する先頭部位の位置および後尾部位の位置などを示す位置表示画面を生成し、当該位置表示画面を表示部２６０に表示させる。

図３は、位置表示画面の具体例を示す説明図である。図３に示したように、位置表示画面は、車両フレーム３２、先頭部位表示Ｈ、および後尾部位表示Ｔを含む。車両フレーム３２は、車両３０に外接する直方体である。車両フレーム３２Ｃに着目すると、車両フレーム３２Ｃは車両３０Ｃに外接する直方体である。また、車両フレーム３２Ｃの先頭部位表示Ｈは、車両３０Ｃの進行方向に対する先頭部位として特定された車両３０Ｃの部位の座標位置を示し、車両フレーム３２Ｃの後尾部位表示Ｔは、車両３０Ｃの進行方向に対する後尾部位として特定された車両３０Ｃの部位の座標位置を示す。車両２０の運転手は、
このような位置表示画面を見ることにより、自動運転に対する安心感を得ることができる。

＜３．位置算出装置の構成＞
以上、本発明の一実施形態による運転支援装置２００の構成を説明した。続いて、図４を参照し、本発明の一実施形態による位置算出装置５０の構成を説明する。

図４は、本発明の一実施形態による位置算出装置５０の構成を示す説明図である。図４に示したように、本発明の一実施形態による位置算出装置５０は、点群取得部５１０、物体検出部５１２、物体情報記憶部５１４、物体追跡部５１６、進行方向算出部５１８、位置算出部５２０、物体サイズ算出部５２２、表示制御部５２４、表示部５２６、予測部５２８および通信部５３０を備える。

（点群取得部５１０）
点群取得部５１０は、データ取得部の一例であり、異なる時点に対応する複数のフレームの各々について、レーザーセンサ４０から点群（レーザーセンサデータ）を取得する。点群は、多数の点の位置を表現する座標値の集合である。

（物体検出部５１２）
物体検出部５１２は、点群取得部５１０により取得された各フレームの点群から物体を検出する。例えば、物体検出部５１２は、背景差分技術またはクラスタリング技術などを用いて物体を検出する。これにより、物体を構成する点群である三次元点群情報が得られる。特に、本発明の一実施形態で得られる物体の三次元点群情報には、移動物体である車両３０の三次元点群情報が含まれる。

（物体情報記憶部５１４）
物体情報記憶部５１４は、物体検出部５１２により得られた物体の三次元点群情報をフレームごとに記憶する。

（物体追跡部５１６）
物体追跡部５１６は、あるフレームで検出された物体と、他のフレームで検出された物体との対応付けを行う。例えば、現フレームＦ＿ｋで検出された物体Ｏの代表位置を（Ｘ＿Ｏ，Ｙ＿Ｏ，Ｚ＿Ｏ）とする。代表位置は、検出された物体Ｏの点群の重心位置、または物体Ｏの速度から求めた推定値等であってもよい。物体追跡部５１６は、物体情報記憶部５１４をから過去フレームＦ＿（ｋ－１）で検出された物体群の点群を取得し、物体群のうち、（Ｘ＿Ｏ，Ｙ＿Ｏ，Ｚ＿Ｏ）に最も距離が近い物体を物体Ｏとして認識して物体Ｏと対応付ける。上記距離は三次元でのユークリッド距離であってもよい。

（進行方向算出部５１８）
進行方向算出部５１８は、物体検出部５１２により検出され、物体追跡部５１６によりフレーム間で対応付けられた物体の進行方向を算出する。進行方向算出部５１８は、物体の代表位置のフレーム間での移動量（ベクトル）に基づいて進行方向を算出し得る。以下、図５を参照して、物体の進行方向の算出方法を具体的に説明する。

図５は、物体の代表位置の移動例を示す説明図である。図５には、時刻ｔ１～ｔ４の各々における物体の代表位置Ｃ（ｔ１）～Ｃ（ｔ４）を示している。例えば、時刻ｔ２における物体の進行方向ベクトルＤ＿ｔ２（ｄｘ，ｄｙ，ｄｚ）は、以下の数式１のように表現される。なお、数式１においては成分毎の計算が行われる。

また、進行方向算出部５１８は、算出結果の安定化のために、加重移動平均やカルマンフィルタやパーティクルフィルター等で平滑化された進行方向を算出してもよい。例えば、加重移動平均が用いられる場合、進行方向ベクトルは、以下の数式２のように表現される。なお、数式２においては成分毎の計算が行われる。また、αは重みを表す。

（位置算出部５２０）
位置算出部５２０は、物体における、進行方向を基準にして特定される部位の位置を算出する。特に、本発明の一実施形態による位置算出部５２０は、物体である車両３０の、進行方向に対する先頭部位および後尾部位の位置を算出する。以下、図６～図１０を参照して、先頭部位および後尾部位の位置を算出する方法を説明する。

図６～図１０は、車両３０の先頭部位および後尾部位の位置を算出する方法を示す説明図である。位置算出部５２０は、車両３０の点群（車両点群）の向きを、Ｘ軸（第１座標軸）、Ｙ軸（第２座標軸）およびＺ軸により規定される座標空間上で原点を中心として回転させることにより正規化する。より詳細には、位置算出部５２０は、車両３０の進行方向がＸ軸の方向に一致するように車両点群を角θ（第１の角度）だけ回転させることにより正規化する。角θは、進行方向算出部５１８により算出された進行方向ベクトルと、方向の基準ベクトルＺがなす角度である。図６に示した例では、基準ベクトルＺが（１，０）であるので、車両点群はＺ軸を中心に回転させられることとなる。

図７は、回転後の車両点群を示す説明図である。図７に示したように、車両３０の進行方向がＸ軸の方向に一致するように車両点群の向きを正規化することにより、車両３０の進行方向によらずに車両３０の先頭部位および後尾部位の位置を容易に算出することが可能となる。以下、車両点群を回転させる方法をより具体的に説明する。

車両３０の進行方向ベクトルＤ（ｄｘ，ｄｙ）と基準ベクトルＺ（ｚｘ，ｚｙ）のなす角θ（０≦θ≦１８０°）について、以下の数式３が成り立つ。

位置算出部５２０は、上記数式３で算出されるｃｏｓθ、ｓｉｎθに基づき回転行列Ｒ（θ）を求める。図８に示したように、進行方向ベクトルＤのｙ成分Ｄ．ｙ＞０のときは回転方向が右方向、Ｄ．ｙ≦０のときは回転方向が左方向となる。回転方向が右方向である場合、回転行列Ｒ（θ）は、以下の数式４のように表現される。

一方、回転方向が左方向である場合、回転行列Ｒ（θ）は、以下の数式５のように表現される。

そして、位置算出部５２０は、車両点群を構成する各点Ｐ（ｘ，ｙ，ｚ）を、回転行列Ｒ（θ）を用いて回転させる。例えば、ある点Ｐ（ｘ，ｙ，ｚ）の回転後の座標Ｐ’（ｘ’，ｙ’，ｚ’）は、以下の数式６のように表現される。

その後、位置算出部５２０は、回転後の車両点群から、先頭部位の位置Ｈ’（ｈｘ’，ｈｙ’，ｈｚ’）、後尾部位の位置Ｔ’（ｔｘ’，ｔｙ’，ｔｚ’）を求める。Ｘ軸の正方向に進行方向が正規化されているため、図７に示したように、車両点群のうちＸ座標軸上の値が最大となる点が先頭部位、Ｘ座標軸上の値が最小となる点が後尾部位となる。回転後の車両点群Ｏ’_ｐｃの任意の点Ｐ’（ｘ’，ｙ’，ｚ’）とそのｘ成分Ｐ’．ｘ’に関して、ｈｘ’，ｔｘ’は以下の数式７により求められる。

位置算出部５２０は、ｍａｘ（Ｐ’．ｘ’）となる点Ｐ’＿ｍａｘのｙ成分，ｚ成分をｈｙ’，ｈｚ’として算出してもよい。位置算出部５２０は、同様に、ｍｉｎ（Ｐ’．ｘ’）となる点Ｐ’＿ｍｉｎのｙ成分，ｚ成分をｔｙ’，ｔｚ’として算出してもよい。

または、位置算出部５２０は、図１０に示したようにｈｙ’，ｈｚ’が安定的に算出されるように、以下の数式８によりｈｙ’，ｈｚ’を算出してもよい。

同様に、位置算出部５２０は、以下の数式９によりｈｙ’，ｈｚ’を算出してもよい。

位置算出部５２０は、上述した演算で算出した先頭部位の位置Ｈ’、後尾部位の位置Ｔ’を、図９に示すように逆回転行列Ｒ（－θ）で逆回転し、先頭部位の位置Ｈ（ｈｘ，ｈｙ，ｈｚ）、および後尾部位の位置Ｔ（ｔｘ，ｔｙ，ｔｚ）を算出する。例えば、回転方向が右方向の場合、Ｒ（－θ）は以下の数式１０のように表現される。

このため、先頭部位の位置Ｈの算出は、以下の数式１１のように行われる。後尾部位の位置Ｔも同様に、後尾部位の位置Ｔ’を回転行列Ｒ（－θ）で逆回転させることで算出される。

（物体サイズ算出部５２２）
物体サイズ算出部５２２は、進行方向算出部５１８により算出された進行方向と所定の関係にある方向上における物体のサイズを算出する。特に、本発明の一実施形態による物体サイズ算出部５２２は、車両３０の進行方向に一致する前後方向上での前後サイズ、および車両３０の進行方向に直交する幅方向上での幅サイズなどの車両サイズを算出する。

物体サイズ算出部５２２は、車両３０の幅サイズを、回転行列Ｒ（θ）により回転された後の車両点群におけるＹ軸上の最大値およびＹ軸上の最小値の差分から算出することができる。例えば、物体サイズ算出部５２２は、以下の数式１２により幅サイズＷを算出してもよい。

また、物体サイズ算出部５２２は、車両３０の先頭部位および後尾部位の間の距離を車両３０の前後サイズとして算出することができる。例えば、物体サイズ算出部５２２は、以下の数式１３に示すように、点Ｈ’と点Ｔ’、または点Ｈと点Ｔの二点間ユークリッド距離を車両３０の前後サイズＬとして算出してもよい。

（表示制御部５２４、表示部５２６）
表示制御部５２４は、多様な表示画面を生成する。表示部５２６は、表示制御部５２４により生成された表示画面を表示する。例えば、表示部５２６は、位置算出部５２０により算出された車両３０の先頭部位の位置および後尾部位の位置などを示す位置表示画面を生成し、当該位置表示画面を表示部５２６に表示させてもよい。位置表示画面の具体例は図３を参照して説明した通りである。

（予測部５２８）
予測部５２８は、進行方向算出部５１８により算出された車両３０の進行方向、位置算出部５２０により算出された車両３０の先頭部位の位置または後尾部位の位置に基づき、車両３０に関する予測を行う。例えば、予測部５２８は、位置算出部５２０により算出された車両３０の先頭部位の位置と合流地点との距離に基づき、合流地点に車両３０の先頭部位が到達する時刻を算出する到達時刻予測部としての機能を有する。ここで、合流地点は他の位置の一例であり、本発明の一実施形態が交差点における運転支援に適用される場合には、他の位置は交差点であってもよい。

また、予測部５２８は、進行方向算出部５１８により算出された車両３０の進行方向に基づき、車両３０の以降の動きを予測する動き予測部としての機能を有してもよい。例えば、予測部５２８は、車両３０の進行方向が大幅に変化する頻度は低いという想定の下、進行方向算出部５１８により算出された車両３０の現在の進行方向に基づき、数秒後、または数分後などの所定時間後に車両３０が存在する可能性がある位置を予測する。ここで、車両３０の進入禁止エリアとして設定されているエリア内の位置に所定時間後に車両３０が存在する可能性があると予測された場合、アラートの制御が行われてもよい。

（通信部５３０）
通信部５３０は、他の装置と多様な情報を送信および受信する。例えば、通信部５３０は、位置算出部５２０により算出された各車両３０の先頭部位の位置および後尾部位の位置、物体サイズ算出部５２２により算出された各車両３０の車両サイズ、または予測部５２８により得られた予測結果、のうちの少なくともいずれかの情報を車両２０に搭載された運転支援装置２００に送信してもよい。運転支援装置２００は、この通信部５３０から受信される情報に基づき、車両２０の走行計画を適切にたてることが可能となる。

＜４．位置算出装置の動作＞
以上、本発明の一実施形態による位置算出システムの構成を説明した。続いて、図１１および図１２を参照し、本発明の一実施形態による位置算出装置５０の動作を整理する。

図１１は、本発明の一実施形態による位置算出装置５０の動作を示すフローチャートである。図１１に示したように、まず、位置算出装置５０の点群取得部５１０がレーザーセンサ４０から点群を取得する（Ｓ３１０）。そして、物体検出部５１２が、点群取得部５１０により取得された各フレームの点群から、背景差分技術またはクラスタリング技術などを用いて物体を検出する（Ｓ３２０）。これにより、各フレームから物体を構成する点群である三次元点群情報が得られ、各フレームの三次元点群情報が表示制御部５２４に記憶される。

続いて、物体追跡部５１６が、あるフレームで検出された物体と、他のフレームで検出された物体との対応付けを行う（Ｓ３３０）。さらに、進行方向算出部５１８が、物体検出部５１２により検出され、物体追跡部５１６によりフレーム間で対応付けられた物体の進行方向を算出する（Ｓ３４０）。

そして、位置算出部５２０は、物体である車両３０の、進行方向に対する先頭部位および後尾部位の位置を算出する（Ｓ３５０）。さらに、物体サイズ算出部５２２が、車両３０の進行方向に一致する前後方向上での前後サイズ、および車両３０の進行方向に直交する幅方向じょうでの幅サイズなどの車両サイズを算出する（Ｓ３６０）。そして、表示制御部５２４による位置表示画面の表示、および、通信部５３０による各車両３０の先頭部位の位置および後尾部位の位置の運転支援装置２００への送信などが行われる（Ｓ３７０）。上述したＳ３５０およびＳ３６０の処理については図１２を参照してより詳細に整理する。

図１２は、位置算出およびサイズ算出の詳細を示すフローチャートである。図１２に示したように、位置算出部５２０は、車両３０の進行方向がＸ軸の方向に一致するように車両点群の向きを正規化する回転行列Ｒ（θ）を算出し（Ｓ４１０）、当該回転行列Ｒ（θ）を用いて車両点群を回転させる（Ｓ４２０）。

そして、位置算出部５２０は、回転後の車両点群から、先頭部位の位置Ｈ’（ｈｘ’，ｈｙ’，ｈｚ’）、後尾部位の位置Ｔ’（ｔｘ’，ｔｙ’，ｔｚ’）を取得する（Ｓ４３０）。Ｘ軸の正方向に進行方向が正規化されているため、図７に示したように、車両点群のうちＸ座標軸上の値が最大となる点が先頭部位、Ｘ座標軸上の値が最小となる点が後尾部位となる。

さらに、位置算出部５２０は、先頭部位の位置Ｈ’、後尾部位の位置Ｔ’を、逆回転行列Ｒ（－θ）で逆回転し、先頭部位の位置Ｈ（ｈｘ，ｈｙ，ｈｚ）、および後尾部位の位置Ｔ（ｔｘ，ｔｙ，ｔｚ）を算出する（Ｓ４４０）。その後、物体サイズ算出部５２２が、車両３０の進行方向に一致する前後方向上での前後サイズ、および車両３０の進行方向に直交する幅方向上での幅サイズなどの車両サイズを算出する。

＜５．作用効果＞
上述した本発明の一実施形態によれば、多様な作用効果が得られる。例えば、本発明の一実施形態によれば、道路の方向が事前設定されていない場合や、交差点など車両３０の進行方向が不定である場所でも、車両３０の進行方向を適切に算出することが可能である。

さらに、本発明の一実施形態によれば、車両３０の進行方向に基づき、進行方向に対する車両３０の先頭部位の位置および後尾部位の位置を算出できる。このような車両３０の先頭部位の位置および後尾部位の位置は、車車間距離の算出や、車両３０の先頭部位が合流地点および交差点などの所定の位置に到達する到着時刻の算出に用いられ得る。結果、自動運転の運転支援および安全支援を実現することが可能である。

また、本発明の一実施形態によれば、車両３０の進行方向がある座標軸に一致するように車両点群を回転させることにより、車両３０の先頭部位の位置および後尾部位の位置を算出する。かかる構成によれば、簡易、かつより正確に車両３０の先頭部位の位置および後尾部位の位置を算出可能である。

＜６．ハードウェア構成＞
以上、本発明の一実施形態を説明した。上述した進行方向の算出および先頭部位の位置の算出などの情報処理は、ソフトウェアと、以下に説明する位置算出装置５０のハードウェアとの協働により実現される。

図１３は、位置算出装置５０のハードウェア構成を示したブロック図である。位置算出装置５０は、ＣＰＵ（ＣｅｎｔｒａｌＰｒｏｃｅｓｓｉｎｇＵｎｉｔ）５０１と、ＲＯＭ（ＲｅａｄＯｎｌｙＭｅｍｏｒｙ）５０２と、ＲＡＭ（ＲａｎｄｏｍＡｃｃｅｓｓＭｅｍｏｒｙ）５０３と、ホストバス５０４と、を備える。また、位置算出装置５０は、ブリッジ５０５と、外部バス５０６と、インターフェース５０７と、入力装置５０８と、表示装置５０９と、音声出力装置５１１と、ストレージ装置（ＨＤＤ）５１３と、ネットワークインターフェース５１５とを備える。

ＣＰＵ５０１は、演算処理装置および制御装置として機能し、各種プログラムに従って位置算出装置５０内の動作全般を制御する。また、ＣＰＵ５０１は、マイクロプロセッサであってもよい。ＲＯＭ５０２は、ＣＰＵ５０１が使用するプログラムや演算パラメータ等を記憶する。ＲＡＭ５０３は、ＣＰＵ５０１の実行において使用するプログラムや、その実行において適宜変化するパラメータ等を一時記憶する。これらはＣＰＵバスなどから構成されるホストバス５０４により相互に接続されている。これらＣＰＵ５０１、ＲＯＭ５０２およびＲＡＭ５０３とソフトウェアとの協働により、点群取得部５１０、物体検出部５１２、物体追跡部５１６、進行方向算出部５１８、位置算出部５２０、物体サイズ算出部５２２、表示制御部５２４および予測部５２８などの機能が実現され得る。

ホストバス５０４は、ブリッジ５０５を介して、ＰＣＩ（ＰｅｒｉｐｈｅｒａｌＣｏｍｐｏｎｅｎｔＩｎｔｅｒｃｏｎｎｅｃｔ／Ｉｎｔｅｒｆａｃｅ）バスなどの外部バス５０６に接続されている。なお、必ずしもホストバス５０４、ブリッジ５０５および外部バス５０６を分離構成する必要はなく、１つのバスにこれらの機能を実装してもよい。

入力装置５０８は、マウス、キーボード、タッチパネル、ボタン、マイクロフォン、センサ、スイッチおよびレバーなどユーザが情報を入力するための入力手段と、ユーザによる入力に基づいて入力信号を生成し、ＣＰＵ５０１に出力する入力制御回路などから構成されている。位置算出装置５０のユーザは、該入力装置５０８を操作することにより、位置算出装置５０に対して各種のデータを入力したり処理動作を指示したりすることができる。

表示装置５０９は、例えば、液晶ディスプレイ（ＬＣＤ）装置、プロジェクター装置、ＯＬＥＤ（ＯｒｇａｎｉｃＬｉｇｈｔＥｍｉｔｔｉｎｇＤｉｏｄｅ）装置およびランプなどの表示装置を含む。また、音声出力装置５１１は、スピーカおよびヘッドホンなどの音声出力装置を含む。

ストレージ装置５１３は、本実施形態にかかる位置算出装置５０の記憶部の一例として構成されたデータ格納用の装置である。ストレージ装置５１３は、記憶媒体、記憶媒体にデータを記録する記録装置、記憶媒体からデータを読み出す読出し装置および記憶媒体に記録されたデータを削除する削除装置などを含んでもよい。ストレージ装置５１３は、例えば、ＨＤＤ（ＨａｒｄＤｉｓｋＤｒｉｖｅ）またはＳＳＤ（ＳｏｌｉｄＳｔｒａｇｅＤｒｉｖｅ）、あるいは同等の機能を有するメモリ等で構成される。このストレージ装置５１３は、ストレージを駆動し、ＣＰＵ５０１が実行するプログラムや各種データを格納する。

ネットワークインターフェース５１５は、例えば、ネットワークに接続するための通信デバイス等で構成された通信インターフェースである。また、ネットワークインターフェース５１５は、無線ＬＡＮ（ＬｏｃａｌＡｒｅａＮｅｔｗｏｒｋ）対応通信装置であっても、有線による通信を行うワイヤー通信装置であってもよい。

なお、上述した位置算出装置５０のハードウェア構成は運転支援装置２００にも適用可能である。

＜７．補足＞
以上、添付図面を参照しながら本発明の好適な実施形態について詳細に説明したが、本発明はかかる例に限定されない。本発明の属する技術の分野における通常の知識を有する者であれば、特許請求の範囲に記載された技術的思想の範疇内において、各種の変更例または修正例に想到し得ることは明らかであり、これらについても、当然に本発明の技術的範囲に属するものと了解される。

例えば、上記ではセンサとしてＬｉＤＡＲのようなレーザーセンサ４０を主に想定して説明したが、レーダーセンサやまたはカメラセンサ等、物体を形状として捉えるセンサを利用可能である。さらに、物体を形状として捉えることのできない単体の検出データを組み合わせ、物体を形状として捉えられるように合成することも可能である。

また、図３には、車両３０の先頭部位および後尾部位を表示する位置表示画面を示した。位置表示画面は、さらに、進行方向ベクトル、または基準ベクトルに対する進行方向ベクトルの角度θを表示してもよい。

また、上記ではレーザーセンサ４０が路側に固定的に設置される例を想定して説明したが、レーザーセンサ４０は移動物体、例えば車両３０に搭載されてもよい。この場合、車両３０の移動量や移動方向が判明していれば、検出された周囲の車両とレーザーセンサ４０との相対的な位置関係から、周囲の車両の進行方向や先頭部位の位置などに関して、ある地点を基準とした進行方向や先頭部位の位置などに変換することも可能である。

また、上述した実施形態では、車両３０がバックで移動している場合、車両３０の実際の後尾部位が先頭部位として特定され、車両３０の実際の先頭部位が後尾部位として特定される。一方、本発明の一実施形態の用途によっては、車両３０がバックで移動している場合にも、車両３０の実際の先頭部位が先頭部位として特定され、車両３０の実際の後尾部位が後尾部位として特定されることが望まれ得る。そこで、位置算出装置５０は、車両３０が車両前方に移動しているかバックで移動しているかを判断し、バックで移動していると判断した場合には、進行方向に対する先頭部位を後尾部位として扱い、進行方向に対する後尾部位を先頭部位として扱ってもよい。なお、車両３０がバックで移動しているか否かの判断は、車両３０の進行方向が反対方向に変化することに基づき行うことが可能である。ただし、進行方向が反対方向に変化しただけでは、走行モードが通常モードからバックに切り替えられたのか、バックから通常モードに切り替えられたかの判断が困難な場合がある。この点に関し、車両３０はバックよりも通常モードで走行する時間が一般的に長いことに基づき、位置算出装置５０は、適用されている時間が短い方の走行モードがバックであると判断してもよい。

また、車両３０がレーザーセンサ４０を通り過ぎると、レーザーセンサ４０が取得する点群に車両３０の実際の先頭部位が含まれなくなり得る。このような場合、位置算出部５２０は、車両３０の後尾部位の位置に、過去に算出された車両３０の前後サイズを進行方向に沿って加算することで、車両３０の先頭部位の位置を算出してもよい。

また、本明細書の位置算出装置５０、運転支援装置２００の処理における各ステップは、必ずしもフローチャートとして記載された順序に沿って時系列に処理する必要はない。例えば、位置算出装置５０の処理における各ステップは、フローチャートとして記載した順序と異なる順序で処理されても、並列的に処理されてもよい。

また、位置算出装置５０、運転支援装置２００に内蔵されるＣＰＵ、ＲＯＭおよびＲＡＭなどのハードウェアに、上述した位置算出装置５０、運転支援装置２００の各構成と同等の機能を発揮させるためのコンピュータプログラムも作成可能である。また、該コンピュータプログラムを記憶させた記憶媒体も提供される。

２０車両
２００運転支援装置
２１０通信部
２２０車両情報検出部
２３０計画算出部
２４０車両制御部
２４８計画算出部
２５０表示制御部
２６０表示部
３０車両
４０レーザーセンサ
５０位置算出装置
５１０点群取得部
５１２物体検出部
５１４物体情報記憶部
５１６物体追跡部
５１８進行方向算出部
５２０位置算出部
５２２物体サイズ算出部
５２４表示制御部
５２６表示部
５２８予測部
５３０通信部

Claims

レーザーセンサから複数の時点における点群を取得する点群取得部と、
前記点群取得部により取得された前記複数の時点における点群の各々から物体を検出する物体検出部と、
前記物体検出部による前記物体の検出結果に基づいて前記物体の進行方向を算出する進行方向算出部と、
前記物体の点群を、第１座標軸および第２座標軸により規定される座標空間上で、前記物体の進行方向が前記第１座標軸の方向に一致するように第１の角度だけ回転させ、
回転後の前記物体の点群のうち、前記第１座標軸上の値が最大となる点、または最小となる点を特定し、
特定された点の座標値および前記座標空間における前記第１の角度の逆回転に基づき、前記物体の先頭部位または後尾部位の位置を算出する位置算出部と、
を備える、位置算出システム。
前記位置算出システムは、前記位置算出部により算出された位置を示す位置表示画面を生成する表示制御部をさらに備える、請求項１に記載の位置算出システム。
前記物体検出部は、前記複数の時点における前記物体の代表位置を検出し、
前記進行方向算出部は、前記物体の代表位置の移動方向を前記進行方向として算出する、請求項１または２に記載の位置算出システム。
前記進行方向を基準にして特定される部位は、前記進行方向に対する先頭部位または後尾部位を含む、請求項１～３までのいずれか一項に記載の位置算出システム。
前記位置算出システムは、前記進行方向と所定の関係にある方向上における前記物体のサイズを算出する物体サイズ算出部をさらに備える、請求項１～４までのいずれか一項に記載の位置算出システム。
前記進行方向と所定の関係にある方向は前記進行方向に直交する幅方向を含み、
前記物体サイズ算出部は、前記回転後の前記物体における前記第２座標軸上の最大値および最小値との差分から前記物体の幅サイズを算出する、請求項５に記載の位置算出システム。
前記進行方向と所定の関係にある方向は前記進行方向に一致する前後方向を含み、
前記物体サイズ算出部は、前記物体の前記先頭部位および前記後尾部位の間の距離を前記物体の前後サイズとして算出する、請求項５または６に記載の位置算出システム。
前記位置算出システムは、第１の物体に搭載された装置を含み、
前記第１の物体に搭載された装置は、第２の物体について算出された位置を示す位置表示画面を表示する表示部を有する、請求項２に記載の位置算出システム。
前記位置算出システムは、前記位置算出部により算出された位置と他の位置との距離に基づき、前記他の位置に前記物体の前記部位が到達する時刻を算出する到達時刻予測部をさらに備える、請求項１～８までのいずれか一項に記載の位置算出システム。
前記位置算出システムは、前記進行方向算出部により算出された前記物体の進行方向に基づき、前記物体の以降の動きを予測する動き予測部をさらに備える、請求項１～９までのいずれか一項に記載の位置算出システム。
コンピュータを、
レーザーセンサから複数の時点における点群を取得する点群取得部と、
前記点群取得部により取得された前記複数の時点における点群の各々から物体を検出する物体検出部と、
前記物体検出部による前記物体の検出結果に基づいて前記物体の進行方向を算出する進行方向算出部と、
前記物体の点群を、第１座標軸および第２座標軸により規定される座標空間上で、前記物体の進行方向が前記第１座標軸の方向に一致するように第１の角度だけ回転させ、
回転後の前記物体の点群のうち、前記第１座標軸上の値が最大となる点、または最小となる点を特定し、
特定された点の座標値および前記座標空間における前記第１の角度の逆回転に基づき、前記物体の先頭部位または後尾部位の位置を算出する位置算出部と、
として機能させるための、プログラム。
レーザーセンサから複数の時点における点群を取得することと、
前記複数の時点における点群の各々から物体を検出することと、
前記物体の検出結果に基づいて前記物体の進行方向を算出することと、
前記物体の点群を、第１座標軸および第２座標軸により規定される座標空間上で、前記物体の進行方向が前記第１座標軸の方向に一致するように第１の角度だけ回転させ、
回転後の前記物体の点群のうち、前記第１座標軸上の値が最大となる点、または最小となる点を特定し、
特定された点の座標値および前記座標空間における前記第１の角度の逆回転に基づき、前記物体の先頭部位または後尾部位の位置を算出することと、
を含む、コンピュータにより実行される位置算出方法。