JP7078066B2

JP7078066B2 - Position calculation system, program and position calculation method

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

本発明は、位置算出システム、プログラムおよび位置算出方法に関する。 The present invention relates to a position calculation system, a program and a position calculation method.

近年、自律的に走行する自動運転車両を実現するための技術が盛んに研究されている。自動運転車両による自動運転は、自動運転車両の周囲を走行する他の車両の認識に基づいて実現され得る。他の車両の認識に関し、以下の特許文献１および特許文献２などに開示されているように、ＬｉＤＡＲ（ＬａｓｅｒＩｍａｇｉｎｇＤｅｔｅｃｔｉｏｎａｎｄＲａｎｇｉｎｇ）と呼ばれるセンサ装置が知られている。 In recent years, technologies for realizing autonomous vehicles that travel autonomously have been actively researched. Autonomous driving by an autonomous vehicle can be realized based on the recognition of other vehicles traveling around the autonomous vehicle. Regarding the recognition of other vehicles, as disclosed in Patent Document 1 and Patent Document 2 below, a sensor device called LiDAR (Laser Imaging Detection and Ranking) is known.

特開２０２０－２７４１５号公報Japanese Unexamined Patent Publication No. 2020-27415 特開２０１６－１７７８０４号公報Japanese Unexamined Patent Publication No. 2016-177804

しかし、高速道路の合流地点、および見通しの悪い交差点などにおいては、自動運転車両から他の車両までの距離、または死角の発生により、自動運転車両に搭載されたセンサ装置だけでは自動運転車両の周囲の交通状況を正確に把握することが困難となる場合がある。 However, at the confluence of highways and intersections with poor visibility, the distance from the autonomous vehicle to other vehicles or the occurrence of blind spots causes the sensor device mounted on the autonomous vehicle to be around the autonomous vehicle. It may be difficult to accurately grasp the traffic conditions of the vehicle.

このため、路側にもセンサ装置を設置し、自動運転車両に搭載されたセンサ装置の死角となる範囲を路側に設置されたセンサ装置がカバーし、路側に設置されたセンサ装置が交通状況を自動運転車両に伝える方法が考えられる。例えば路側に設置されたセンサ装置側で高速道路の本線を走行する車両の車間距離を計測できると、当該車間距離と各車両の合流地点までの距離とに基づき、自動運転車両が本線車両に合流するための走行計画を立てることが可能である。また、交差点においては、対向車、人または自転車等の移動物体が何秒後に交差点に到着および侵入するかが分かると、自動運転車両の右折判断または事故の回避に繋げることができる。 For this reason, a sensor device is also installed on the roadside, the sensor device installed on the roadside covers the area that becomes the blind spot of the sensor device mounted on the autonomous vehicle, and the sensor device installed on the roadside automatically detects the traffic situation. A method of telling the driving vehicle is conceivable. For example, if the distance between vehicles traveling on the main line of a highway can be measured by the sensor device installed on the road side, the autonomous driving vehicle will join the main line vehicle based on the distance between the vehicles and the distance to the confluence of each vehicle. It is possible to make a driving plan to do so. Further, at an intersection, knowing how many seconds later a moving object such as an oncoming vehicle, a person, or a bicycle arrives at and enters the intersection can lead to a right turn determination of an autonomous vehicle or avoidance of an accident.

ここで、車間距離は、前方車両の後尾部位と、後方車両の先頭部位との間の距離で算出されることが望ましい。また、交差点への到達時刻は、車両の先頭部位から交差点までの距離と車両の速度から算出されることが望ましい。 Here, it is desirable that the inter-vehicle distance is calculated by the distance between the tail portion of the vehicle in front and the front portion of the vehicle behind. Further, it is desirable that the arrival time at the intersection is calculated from the distance from the head portion of the vehicle to the intersection and the speed of the vehicle.

そこで、本発明は、上記問題に鑑みてなされたものであり、本発明の目的とするところは、物体における進行方向を基準にして特定される部位の位置を算出することが可能な、新規かつ改良された位置算出システム、プログラムおよび位置算出方法を提供することにある。 Therefore, the present invention has been made in view of the above problems, and an object of the present invention is to be novel and capable of calculating the position of a specified portion based on the traveling direction in an object. It is to provide an improved position calculation system, program and position calculation method.

上記課題を解決するために、本発明のある観点によれば、レーザーセンサから複数の時点における点群を取得する点群取得部と、前記点群取得部により取得された前記複数の時点における検出データの各々から物体を検出する物体検出部と、前記物体検出部による前記物体の検出結果に基づいて前記物体の進行方向を算出する進行方向算出部と、前記物体の点群を、第１座標軸および第２座標軸により規定される座標空間上で、前記物体の進行方向が前記第１座標軸の方向に一致するように第１の角度だけ回転させ、回転後の前記物体の点群のうち、前記第１座標軸上の値が最大となる点、または最小となる点を特定し、特定された点の座標値および前記座標空間における前記第１の角度の逆回転に基づき、前記物体の先頭部位または前記後尾部位の位置を算出する位置算出部と、を備える、位置算出システムが提供される。
In order to solve the above problems, according to a certain viewpoint of the present invention, a point group acquisition unit that acquires a point group at a plurality of time points from a laser sensor, and a detection at the plurality of time points acquired by the point group acquisition unit. The first coordinate axis of the object detection unit that detects an object from each of the data, the traveling direction calculation unit that calculates the traveling direction of the object based on the detection result of the object by the object detecting unit, and the point group of the object. And, on the coordinate space defined by the second coordinate axis, the object is rotated by a first angle so that the traveling direction of the object coincides with the direction of the first coordinate axis, and the point group of the object after rotation is described. The point where the value on the first coordinate axis becomes the maximum or the minimum is specified, and the head portion of the object or the head portion of the object or based on the coordinate value of the specified point and the reverse rotation of the first angle in the coordinate space. A position calculation system including a position calculation unit for calculating the position of the tail portion is provided.

前記位置算出システムは、前記位置算出部により算出された位置を示す位置表示画面を生成する表示制御部をさらに備えてもよい。 The position calculation system may further include a display control unit that generates a position display screen indicating a position calculated by the position calculation unit.

前記物体検出部は、前記複数の時点における前記物体の代表位置を検出し、前記進行方向算出部は、前記物体の代表位置の移動方向を前記進行方向として算出してもよい。 The object detection unit may detect the representative position of the object at the plurality of time points, and the traveling direction calculation unit may calculate the moving direction of the representative position of the object as the traveling direction.

前記進行方向を基準にして特定される部位は、前記進行方向に対する先頭部位または後尾部位を含んでもよい。 The site specified with respect to the direction of travel may include a head site or a tail site with respect to the direction of travel.

前記位置算出システムは、前記進行方向と所定の関係にある方向上における前記物体のサイズを算出する物体サイズ算出部をさらに備えてもよい。 The position calculation system may further include an object size calculation unit that calculates the size of the object in a direction having a predetermined relationship with the traveling direction.

前記進行方向と所定の関係にある方向は前記進行方向に直交する幅方向を含み、前記物体サイズ算出部は、前記回転後の前記物体における前記第２座標軸上の最大値および最小値との差分から前記物体の幅サイズを算出してもよい。 The direction having a predetermined relationship with the traveling direction includes a width direction orthogonal to the traveling direction, and the object size calculation unit is a difference between the maximum value and the minimum value on the second coordinate axis of the object after rotation. The width size of the object may be calculated from.

前記進行方向と所定の関係にある方向は前記進行方向に一致する前後方向を含み、前記物体サイズ算出部は、前記物体の前記先頭部位および前記後尾部位の間の距離を前記物体の前後サイズとして算出してもよい。 The direction having a predetermined relationship with the traveling direction includes the anteroposterior direction corresponding to the traveling direction, and the object size calculation unit uses the distance between the head portion and the tail portion of the object as the anteroposterior size of the object. It may be calculated.

前記位置算出システムは、第１の物体に搭載された装置を含み、前記第１の物体に搭載された装置は、第２の物体について算出された位置を示す位置表示画面を表示する表示部を有してもよい。 The position calculation system includes a device mounted on the first object, and the device mounted on the first object has a display unit for displaying a position display screen indicating a position calculated for the second object. You may have.

前記位置算出システムは、前記位置算出部により算出された位置と他の位置との距離に基づき、前記他の位置に前記物体の前記部位が到達する時刻を算出する到達時刻予測部をさらに備えてもよい。 The position calculation system further includes an arrival time prediction unit that calculates the time when the part of the object arrives at the other position based on the distance between the position calculated by the position calculation unit and the other position. May be good.

前記位置算出システムは、前記進行方向算出部により算出された前記物体の進行方向に基づき、前記物体の以降の動きを予測する動き予測部をさらに備えてもよい。
The position calculation system may further include a motion prediction unit that predicts the subsequent movement of the object based on the travel direction of the object calculated by the travel direction calculation unit.

また、上記課題を解決するために、本発明の別の観点によれば、コンピュータを、レーザーセンサから複数の時点における点群を取得する点群取得部と、前記点群取得部により取得された前記複数の時点における点群の各々から物体を検出する物体検出部と、前記物体の点群を、第１座標軸および第２座標軸により規定される座標空間上で、前記物体の進行方向が前記第１座標軸の方向に一致するように第１の角度だけ回転させ、回転後の前記物体の点群のうち、前記第１座標軸上の値が最大となる点、または最小となる点を特定し、特定された点の座標値および前記座標空間における前記第１の角度の逆回転に基づき、前記物体の先頭部位または後尾部位の位置を算出する位置算出部と、として機能させるための、プログラムが提供される。
Further, in order to solve the above problem, according to another viewpoint of the present invention, the computer is acquired by the point group acquisition unit that acquires the point group at a plurality of time points from the laser sensor and the point group acquisition unit. An object detection unit that detects an object from each of the point groups at a plurality of time points , and the point group of the object are placed in a coordinate space defined by a first coordinate axis and a second coordinate axis, and the traveling direction of the object is the first. Rotate the object by the first angle so as to match the direction of one coordinate axis, and specify the point where the value on the first coordinate axis becomes the maximum or the minimum among the points of the object after rotation. A program is provided to function as a position calculation unit that calculates the position of the head portion or the tail portion of the object based on the coordinate values of the specified points and the reverse rotation of the first angle in the coordinate space. Will be done.

また、上記課題を解決するために、本発明の別の観点によれば、レーザーセンサから複数の時点における点群を取得することと、前記複数の時点における点群の各々から物体を検出することと、前記物体の検出結果に基づいて前記物体の進行方向を算出することと、前記物体の点群を、第１座標軸および第２座標軸により規定される座標空間上で、前記物体の進行方向が前記第１座標軸の方向に一致するように第１の角度だけ回転させ、回転後の前記物体の点群のうち、前記第１座標軸上の値が最大となる点、または最小となる点を特定し、特定された点の座標値および前記座標空間における前記第１の角度の逆回転に基づき、前記物体の先頭部位または後尾部位の位置を算出することと、を含む、コンピュータにより実行される位置算出方法が提供される。 Further, in order to solve the above-mentioned problems, according to another viewpoint of the present invention, acquiring a point group at a plurality of time points from a laser sensor and detecting an object from each of the point groups at the plurality of time points. And, the traveling direction of the object is calculated based on the detection result of the object, and the point group of the object is set in the coordinate space defined by the first coordinate axis and the second coordinate axis. Rotate by the first angle so as to match the direction of the first coordinate axis, and specify the point where the value on the first coordinate axis becomes the maximum or the minimum among the point group of the rotated object. A position performed by a computer, including calculating the position of the head or tail of the object based on the coordinate values of the identified point and the reverse rotation of the first angle in the coordinate space. A calculation method is provided.

以上説明した本発明によれば、物体における進行方向を基準にして特定される部位の位置を算出することが可能である。 According to the present invention described above, it is possible to calculate the position of the specified portion based on the traveling direction of the object.

本発明の一実施形態による位置算出システムの構成例を示す説明図である。It is explanatory drawing which shows the structural example of the position calculation system by one Embodiment of this invention. 本発明の一実施形態による運転支援装置２００の構成を示す説明図である。It is explanatory drawing which shows the structure of the operation support apparatus 200 by one Embodiment of this invention. 位置表示画面の具体例を示す説明図である。It is explanatory drawing which shows the specific example of the position display screen. 本発明の一実施形態による位置算出装置５０の構成を示す説明図である。It is explanatory drawing which shows the structure of the position calculation apparatus 50 by one Embodiment of this invention. 物体の代表位置の移動例を示す説明図である。It is explanatory drawing which shows the movement example of the representative position of an object. 車両３０の先頭部位および後尾部位の位置を算出する方法を示す説明図である。It is explanatory drawing which shows the method of calculating the position of the head part and the tail part of a vehicle 30. 車両３０の先頭部位および後尾部位の位置を算出する方法を示す説明図である。It is explanatory drawing which shows the method of calculating the position of the head part and the tail part of a vehicle 30. 車両３０の先頭部位および後尾部位の位置を算出する方法を示す説明図である。It is explanatory drawing which shows the method of calculating the position of the head part and the tail part of a vehicle 30. 車両３０の先頭部位および後尾部位の位置を算出する方法を示す説明図である。It is explanatory drawing which shows the method of calculating the position of the head part and the tail part of a vehicle 30. 車両３０の先頭部位および後尾部位の位置を算出する方法を示す説明図である。It is explanatory drawing which shows the method of calculating the position of the head part and the tail part of a vehicle 30. 本発明の一実施形態による位置算出装置５０の動作を示すフローチャートである。It is a flowchart which shows the operation of the position calculation apparatus 50 by one Embodiment of this invention. 位置算出およびサイズ算出の詳細を示すフローチャートである。It is a flowchart which shows the details of position calculation and size calculation. 位置算出装置５０のハードウェア構成を示したブロック図である。It is a block diagram which showed the hardware composition of the position calculation apparatus 50.

以下に添付図面を参照しながら、本発明の実施の形態について詳細に説明する。なお、本明細書及び図面において、実質的に同一の機能構成を有する構成要素については、同一の符号を付することにより重複説明を省略する。 Hereinafter, embodiments of the present invention will be described in detail with reference to the accompanying drawings. In the present specification and the drawings, components having substantially the same functional configuration are designated by the same reference numerals, so that duplicate description will be omitted.

また、本明細書及び図面において、実質的に同一の機能構成を有する複数の構成要素を、同一の符号の後に異なるアルファベットを付して区別する場合もある。例えば、実質的に同一の機能構成または論理的意義を有する複数の構成を、必要に応じて車両３０Ａ、３０Ｂおよび３０Ｃのように区別する。ただし、実質的に同一の機能構成を有する複数の構成要素の各々を特に区別する必要がない場合、複数の構成要素の各々に同一符号のみを付する。例えば、車両３０Ａ、３０Ｂおよび３０Ｃを特に区別する必要が無い場合には、各車両を単に車両３０と称する。 Further, in the present specification and the drawings, a plurality of components having substantially the same functional configuration may be distinguished by adding different alphabets after the same reference numerals. For example, a plurality of configurations having substantially the same functional configuration or logical significance are distinguished as necessary, such as vehicles 30A, 30B, and 30C. However, when it is not necessary to particularly distinguish each of the plurality of components having substantially the same functional configuration, only the same reference numerals are given to each of the plurality of components. For example, when it is not necessary to distinguish between the vehicles 30A, 30B and 30C, each vehicle is simply referred to as a vehicle 30.

＜１．位置算出システムの概要＞
本発明の一実施形態は、物体における一部位の位置を算出するための位置算出システムに関する。物体としては、車両、船舶および飛行体などの移動物体、人間および動物などの生物などが挙げられる。車両、船舶および飛行体などの物体は、物体内に操舵者が存在する有人式であってもよいし、物体内に操舵者が存在しない無人式であってもよい。以下では、物体が車両である例を主に想定して本発明の一実施形態による位置算出システムを説明する。 <1. Overview of position calculation system>
One embodiment of the present invention relates to a position calculation system for calculating the position of a part of an object. Objects include moving objects such as vehicles, ships and flying objects, and living things such as humans and animals. Objects such as vehicles, ships, and flying objects may be manned, with a steer inside the object, or unmanned, with no steer inside the object. Hereinafter, a position calculation system according to an embodiment of the present invention will be described, mainly assuming an example in which the object is a vehicle.

図１は、本発明の一実施形態による位置算出システムの構成例を示す説明図である。図１に示した例では、支線道路１２が本線道路１４に合流している。また、支線道路１２を車両２０（第１の物体）が走行しており、本線道路１４を車両３０Ｂ～３０Ｅ（第２の物体）が走行している。車両２０は、図１に示した状態の後も支線道路１２で走行を続け、合流地点において本線道路１４に入ることにより、本線道路１４を走行する車両３０に合流する。車両２０は、運転支援装置２００を搭載しており、運転支援装置２００による支援に基づいて本線道路１４を走行する車両３０に合流することができる。なお、本明細書では、支線道路１２を走行する車両を合流車両と称し、本線道路１４を走行する車両を本線車両と称する場合もある。 FIG. 1 is an explanatory diagram showing a configuration example of a position calculation system according to an embodiment of the present invention. In the example shown in FIG. 1, the branch road 12 joins the main road 14. Further, the vehicle 20 (first object) is traveling on the branch road 12, and the vehicles 30B to 30E (second object) are traveling on the main road 14. The vehicle 20 continues to travel on the branch road 12 even after the state shown in FIG. 1, and enters the main road 14 at the confluence point to join the vehicle 30 traveling on the main road 14. The vehicle 20 is equipped with a driving support device 200, and can join the vehicle 30 traveling on the main road 14 based on the support provided by the driving support device 200. In this specification, a vehicle traveling on a branch road 12 may be referred to as a merging vehicle, and a vehicle traveling on a main road 14 may be referred to as a main line vehicle.

図１に示したように、本発明の一実施形態による位置算出システムは、上述した車両２０および車両３０に加え、路側に位置するレーザーセンサ４０および位置算出装置５０を有する。レーザーセンサ４０は、センサの一例であり、レーザーセンサ４０の周辺における物体の物理的配置を示す検出データを取得する。例えば、レーザーセンサ４０はＬｉＤＡＲであり、本線道路１４を走行する車両３０の形状を示す点群を検出データとして取得する。 As shown in FIG. 1, the position calculation system according to the embodiment of the present invention has a laser sensor 40 and a position calculation device 50 located on the roadside in addition to the vehicle 20 and the vehicle 30 described above. The laser sensor 40 is an example of a sensor, and acquires detection data indicating the physical arrangement of an object around the laser sensor 40. For example, the laser sensor 40 is a LiDAR, and a point cloud indicating the shape of the vehicle 30 traveling on the main road 14 is acquired as detection data.

位置算出装置５０は、レーザーセンサ４０により取得された点群に基づき、車両３０の位置を算出する。ここで、車両２０は、２つの車両３０の間隔部分に合流する。２つの車両３０の間隔部分は、厳密には、前側の車両３０の後尾部位と、後側の車両３０の先頭部位との間の部分である。このため、位置算出装置５０は、各車両３０の進行方向を算出し、各車両３０の進行方向に対する先頭部位の位置および後尾部位の位置を算出する。そして、位置算出装置５０は、算出された先頭部位の位置および後尾部位の位置を車両２０に搭載されている運転支援装置２００に送信する。 The position calculation device 50 calculates the position of the vehicle 30 based on the point cloud acquired by the laser sensor 40. Here, the vehicle 20 joins the spaced portion between the two vehicles 30. Strictly speaking, the distance between the two vehicles 30 is a portion between the tail portion of the front vehicle 30 and the front portion of the rear vehicle 30. Therefore, the position calculation device 50 calculates the traveling direction of each vehicle 30, and calculates the position of the head portion and the position of the tail portion with respect to the traveling direction of each vehicle 30. Then, the position calculation device 50 transmits the calculated positions of the head portion and the tail portion to the driving support device 200 mounted on the vehicle 20.

車両２０の運転支援装置２００は、位置算出装置５０から受信された情報に基づいて車両２０の自動運転を制御することで、本線道路１４を走行する車両３０の車間部分に適切に合流することができる。または、運転支援装置２００は、合流地点までの速度調整情報などを示す表示または音声を出力することにより、合流地点までの速度調整情報を運転手に伝えてもよい。 By controlling the automatic driving of the vehicle 20 based on the information received from the position calculation device 50, the driving support device 200 of the vehicle 20 can appropriately join the inter-vehicle portion of the vehicle 30 traveling on the main road 14. can. Alternatively, the driving support device 200 may convey the speed adjustment information to the confluence to the driver by outputting a display or a voice indicating speed adjustment information to the confluence.

上記では、本発明の一実施形態による位置算出システムが車両２０の合流支援に適用される例を説明したが、本発明の一実施形態による位置算出システムは他の用途にも適用可能である。例えば、本発明の一実施形態による位置算出システムは、交差点における右折支援にも適用可能である。具体的には、位置算出装置５０が交差点の周囲の各車両の進行方向、進行方向に対する先頭部位の位置および後尾部位の位置を算出することで、交差点を右折しようとする車両が対向車の車間部分で適切に右折することを支援することが可能である。 In the above, an example in which the position calculation system according to the embodiment of the present invention is applied to the merging support of the vehicle 20 has been described, but the position calculation system according to the embodiment of the present invention can also be applied to other uses. For example, the position calculation system according to one embodiment of the present invention can also be applied to support for turning right at an intersection. Specifically, the position calculation device 50 calculates the traveling direction of each vehicle around the intersection, the position of the head portion and the position of the tail portion with respect to the traveling direction, so that the vehicle trying to turn right at the intersection is between the oncoming vehicles. It is possible to help make a proper right turn at the part.

以上、本発明の一実施形態による位置算出システムの概要を説明した。続いて、本発明の一実施形態による位置算出システムの構成および動作を順次詳細に説明する。 The outline of the position calculation system according to the embodiment of the present invention has been described above. Subsequently, the configuration and operation of the position calculation system according to the embodiment of the present invention will be sequentially described in detail.

＜２．運転支援装置の構成＞
図２は、本発明の一実施形態による運転支援装置２００の構成を示す説明図である。図２に示したように、本発明の一実施形態による運転支援装置２００は、通信部２１０、自車両情報検出部２２０、計画算出部２３０、車両制御部２４０、表示制御部２５０および表示部２６０を備える。 <2. Configuration of driving support device>
FIG. 2 is an explanatory diagram showing a configuration of a driving support device 200 according to an embodiment of the present invention. As shown in FIG. 2, the driving support device 200 according to the embodiment of the present invention includes a communication unit 210, an own vehicle information detection unit 220, a plan calculation unit 230, a vehicle control unit 240, a display control unit 250, and a display unit 260. To prepare for.

（通信部２１０）
通信部２１０は、位置算出装置５０と無線通信を行う構成である。通信部２１０は、位置算出装置５０から、位置算出装置５０により算出された各車両３０の進行方向に対する先頭部位の位置および後尾部位の位置を受信する。その他、通信部２１０は、各車両３０のサイズ、代表位置などを示す情報も受信し得る。 (Communication unit 210)
The communication unit 210 is configured to perform wireless communication with the position calculation device 50. The communication unit 210 receives from the position calculation device 50 the position of the head portion and the position of the tail portion with respect to the traveling direction of each vehicle 30 calculated by the position calculation device 50. In addition, the communication unit 210 may also receive information indicating the size, representative position, etc. of each vehicle 30.

（自車両情報検出部２２０）
自車両情報検出部２２０は、運転支援装置２００を搭載する車両２０（すなわち、自車両）の情報を検出する。例えば、自車両情報検出部２２０は、自車両の位置、速度および合流地点までの距離を取得する。このような自車両情報検出部２２０は、汎用的に車両に搭載されている車速パルス発生器や慣性センサ（加速度センサ、ジャイロスコープ）、カーナビゲーションで利用される地図情報、またはＧＮＳＳ（ＧｌｏｂａｌＮａｖｉｇａｔｉｏｎＳａｔｅｌｌｉｔｅＳｙｓｔｅｍ）などを有してもよい。 (Own vehicle information detection unit 220)
The own vehicle information detection unit 220 detects information on the vehicle 20 (that is, the own vehicle) on which the driving support device 200 is mounted. For example, the own vehicle information detection unit 220 acquires the position, speed, and distance to the confluence of the own vehicle. Such own vehicle information detection unit 220 is a vehicle speed pulse generator or inertial sensor (accelerometer, gyroscope) generally mounted on the vehicle, map information used in car navigation, or GNSS (Global Navigation Satellite). It may have a System) or the like.

（計画算出部２３０）
計画算出部２３０は、自車両情報検出部２２０により検出された情報、および位置算出装置５０から受信された情報に基づき、自車両が本線車両に合流するための走行計画を算出する。例えば、計画算出部２３０は、本線道路１４における本線車両の走行位置の分布に基づき、自車両が合流することが困難な範囲を特定する。そして、計画算出部２４８は、他の範囲である合流候補範囲内で本線車両に合流するための自車両の速度を算出する。 (Plan calculation unit 230)
The plan calculation unit 230 calculates a travel plan for the own vehicle to join the main line vehicle based on the information detected by the own vehicle information detection unit 220 and the information received from the position calculation device 50. For example, the plan calculation unit 230 identifies a range in which it is difficult for the own vehicle to merge based on the distribution of the traveling positions of the main line vehicles on the main road 14. Then, the plan calculation unit 248 calculates the speed of the own vehicle for merging with the main line vehicle within the merging candidate range which is another range.

（車両制御部２４０）
車両制御部２４０は、自車両の走行を自動制御するための構成である。車両制御部２４０は、計画算出部２３０により算出された走行計画に従って自車両の速度調整を行う。 (Vehicle control unit 240)
The vehicle control unit 240 is configured to automatically control the traveling of the own vehicle. The vehicle control unit 240 adjusts the speed of the own vehicle according to the travel plan calculated by the plan calculation unit 230.

（表示制御部２５０、表示部２６０）
表示制御部２５０は、多様な表示画面を生成する。表示部２６０は、表示制御部２５０により生成された表示画面を表示する。例えば、表示部２６０は、通信部２１０により受信された各車両３０の進行方向に対する先頭部位の位置および後尾部位の位置などを示す位置表示画面を生成し、当該位置表示画面を表示部２６０に表示させる。 (Display control unit 250, display unit 260)
The display control unit 250 generates various display screens. The display unit 260 displays the display screen generated by the display control unit 250. For example, the display unit 260 generates a position display screen showing the position of the head portion and the position of the tail portion with respect to the traveling direction of each vehicle 30 received by the communication unit 210, and displays the position display screen on the display unit 260. Let me.

図３は、位置表示画面の具体例を示す説明図である。図３に示したように、位置表示画面は、車両フレーム３２、先頭部位表示Ｈ、および後尾部位表示Ｔを含む。車両フレーム３２は、車両３０に外接する直方体である。車両フレーム３２Ｃに着目すると、車両フレーム３２Ｃは車両３０Ｃに外接する直方体である。また、車両フレーム３２Ｃの先頭部位表示Ｈは、車両３０Ｃの進行方向に対する先頭部位として特定された車両３０Ｃの部位の座標位置を示し、車両フレーム３２Ｃの後尾部位表示Ｔは、車両３０Ｃの進行方向に対する後尾部位として特定された車両３０Ｃの部位の座標位置を示す。車両２０の運転手は、
このような位置表示画面を見ることにより、自動運転に対する安心感を得ることができる。 FIG. 3 is an explanatory diagram showing a specific example of the position display screen. As shown in FIG. 3, the position display screen includes the vehicle frame 32, the head portion display H, and the tail portion display T. The vehicle frame 32 is a rectangular parallelepiped that circumscribes the vehicle 30. Focusing on the vehicle frame 32C, the vehicle frame 32C is a rectangular parallelepiped circumscribing the vehicle 30C. Further, the head portion display H of the vehicle frame 32C indicates the coordinate position of the portion of the vehicle 30C specified as the head portion with respect to the traveling direction of the vehicle 30C, and the tail portion display T of the vehicle frame 32C indicates the tail portion display T with respect to the traveling direction of the vehicle 30C. The coordinate position of the part of the vehicle 30C specified as the tail part is shown. The driver of vehicle 20
By looking at such a position display screen, it is possible to obtain a sense of security for automatic driving.

＜３．位置算出装置の構成＞
以上、本発明の一実施形態による運転支援装置２００の構成を説明した。続いて、図４を参照し、本発明の一実施形態による位置算出装置５０の構成を説明する。 <3. Configuration of position calculation device>
The configuration of the driving support device 200 according to the embodiment of the present invention has been described above. Subsequently, with reference to FIG. 4, the configuration of the position calculation device 50 according to the embodiment of the present invention will be described.

図４は、本発明の一実施形態による位置算出装置５０の構成を示す説明図である。図４に示したように、本発明の一実施形態による位置算出装置５０は、点群取得部５１０、物体検出部５１２、物体情報記憶部５１４、物体追跡部５１６、進行方向算出部５１８、位置算出部５２０、物体サイズ算出部５２２、表示制御部５２４、表示部５２６、予測部５２８および通信部５３０を備える。 FIG. 4 is an explanatory diagram showing the configuration of the position calculation device 50 according to the embodiment of the present invention. As shown in FIG. 4, the position calculation device 50 according to the embodiment of the present invention includes a point group acquisition unit 510, an object detection unit 512, an object information storage unit 514, an object tracking unit 516, a traveling direction calculation unit 518, and a position. It includes a calculation unit 520, an object size calculation unit 522, a display control unit 524, a display unit 526, a prediction unit 528, and a communication unit 530.

（点群取得部５１０）
点群取得部５１０は、データ取得部の一例であり、異なる時点に対応する複数のフレームの各々について、レーザーセンサ４０から点群（レーザーセンサデータ）を取得する。点群は、多数の点の位置を表現する座標値の集合である。 (Point cloud acquisition unit 510)
The point cloud acquisition unit 510 is an example of a data acquisition unit, and acquires a point cloud (laser sensor data) from the laser sensor 40 for each of a plurality of frames corresponding to different time points. A point cloud is a set of coordinate values that represent the positions of many points.

（物体検出部５１２）
物体検出部５１２は、点群取得部５１０により取得された各フレームの点群から物体を検出する。例えば、物体検出部５１２は、背景差分技術またはクラスタリング技術などを用いて物体を検出する。これにより、物体を構成する点群である三次元点群情報が得られる。特に、本発明の一実施形態で得られる物体の三次元点群情報には、移動物体である車両３０の三次元点群情報が含まれる。 (Object detection unit 512)
The object detection unit 512 detects an object from the point cloud of each frame acquired by the point cloud acquisition unit 510. For example, the object detection unit 512 detects an object using background subtraction technique, clustering technique, or the like. As a result, three-dimensional point cloud information, which is a point cloud constituting an object, can be obtained. In particular, the three-dimensional point cloud information of the object obtained in one embodiment of the present invention includes the three-dimensional point cloud information of the vehicle 30 which is a moving object.

（物体情報記憶部５１４）
物体情報記憶部５１４は、物体検出部５１２により得られた物体の三次元点群情報をフレームごとに記憶する。 (Object information storage unit 514)
The object information storage unit 514 stores the three-dimensional point cloud information of the object obtained by the object detection unit 512 for each frame.

（物体追跡部５１６）
物体追跡部５１６は、あるフレームで検出された物体と、他のフレームで検出された物体との対応付けを行う。例えば、現フレームＦ＿ｋで検出された物体Ｏの代表位置を（Ｘ＿Ｏ，Ｙ＿Ｏ，Ｚ＿Ｏ）とする。代表位置は、検出された物体Ｏの点群の重心位置、または物体Ｏの速度から求めた推定値等であってもよい。物体追跡部５１６は、物体情報記憶部５１４をから過去フレームＦ＿（ｋ－１）で検出された物体群の点群を取得し、物体群のうち、（Ｘ＿Ｏ，Ｙ＿Ｏ，Ｚ＿Ｏ）に最も距離が近い物体を物体Ｏとして認識して物体Ｏと対応付ける。上記距離は三次元でのユークリッド距離であってもよい。 (Object Tracking Unit 516)
The object tracking unit 516 associates an object detected in a certain frame with an object detected in another frame. For example, the representative position of the object O detected in the current frame F_k is (X_O, Y_O, Z_O). The representative position may be the position of the center of gravity of the detected point cloud of the object O, an estimated value obtained from the velocity of the object O, or the like. The object tracking unit 516 acquires a point group of the object group detected in the past frame F_ (k-1) from the object information storage unit 514, and the distance is the longest to (X_O, Y_O, Z_O) among the object groups. A close object is recognized as an object O and associated with the object O. The above distance may be a three-dimensional Euclidean distance.

（進行方向算出部５１８）
進行方向算出部５１８は、物体検出部５１２により検出され、物体追跡部５１６によりフレーム間で対応付けられた物体の進行方向を算出する。進行方向算出部５１８は、物体の代表位置のフレーム間での移動量（ベクトル）に基づいて進行方向を算出し得る。以下、図５を参照して、物体の進行方向の算出方法を具体的に説明する。 (Traveling direction calculation unit 518)
The traveling direction calculation unit 518 is detected by the object detection unit 512, and the object tracking unit 516 calculates the traveling direction of the object associated between the frames. The traveling direction calculation unit 518 can calculate the traveling direction based on the amount of movement (vector) between frames of the representative position of the object. Hereinafter, a method of calculating the traveling direction of the object will be specifically described with reference to FIG.

図５は、物体の代表位置の移動例を示す説明図である。図５には、時刻ｔ１～ｔ４の各々における物体の代表位置Ｃ（ｔ１）～Ｃ（ｔ４）を示している。例えば、時刻ｔ２における物体の進行方向ベクトルＤ＿ｔ２（ｄｘ，ｄｙ，ｄｚ）は、以下の数式１のように表現される。なお、数式１においては成分毎の計算が行われる。 FIG. 5 is an explanatory diagram showing an example of movement of a representative position of an object. FIG. 5 shows representative positions C (t1) to C (t4) of the object at each of the times t1 to t4. For example, the traveling direction vector D_t2 (dx, dy, dz) of the object at time t2 is expressed by the following mathematical formula 1. In the formula 1, the calculation for each component is performed.

また、進行方向算出部５１８は、算出結果の安定化のために、加重移動平均やカルマンフィルタやパーティクルフィルター等で平滑化された進行方向を算出してもよい。例えば、加重移動平均が用いられる場合、進行方向ベクトルは、以下の数式２のように表現される。なお、数式２においては成分毎の計算が行われる。また、αは重みを表す。 Further, the traveling direction calculation unit 518 may calculate the traveling direction smoothed by a weighted moving average, a Kalman filter, a particle filter, or the like in order to stabilize the calculation result. For example, when a weighted moving average is used, the traveling direction vector is expressed by the following equation 2. In the formula 2, the calculation for each component is performed. Also, α represents a weight.

（位置算出部５２０）
位置算出部５２０は、物体における、進行方向を基準にして特定される部位の位置を算出する。特に、本発明の一実施形態による位置算出部５２０は、物体である車両３０の、進行方向に対する先頭部位および後尾部位の位置を算出する。以下、図６～図１０を参照して、先頭部位および後尾部位の位置を算出する方法を説明する。 (Position calculation unit 520)
The position calculation unit 520 calculates the position of the specified portion of the object with reference to the traveling direction. In particular, the position calculation unit 520 according to the embodiment of the present invention calculates the positions of the head portion and the tail portion of the vehicle 30, which is an object, with respect to the traveling direction. Hereinafter, a method of calculating the positions of the head portion and the tail portion will be described with reference to FIGS. 6 to 10.

図６～図１０は、車両３０の先頭部位および後尾部位の位置を算出する方法を示す説明図である。位置算出部５２０は、車両３０の点群（車両点群）の向きを、Ｘ軸（第１座標軸）、Ｙ軸（第２座標軸）およびＺ軸により規定される座標空間上で原点を中心として回転させることにより正規化する。より詳細には、位置算出部５２０は、車両３０の進行方向がＸ軸の方向に一致するように車両点群を角θ（第１の角度）だけ回転させることにより正規化する。角θは、進行方向算出部５１８により算出された進行方向ベクトルと、方向の基準ベクトルＺがなす角度である。図６に示した例では、基準ベクトルＺが（１，０）であるので、車両点群はＺ軸を中心に回転させられることとなる。 6 to 10 are explanatory views showing a method of calculating the positions of the head portion and the tail portion of the vehicle 30. The position calculation unit 520 sets the direction of the point cloud (vehicle point cloud) of the vehicle 30 centered on the origin in the coordinate space defined by the X axis (first coordinate axis), the Y axis (second coordinate axis), and the Z axis. Normalize by rotating. More specifically, the position calculation unit 520 normalizes the vehicle point cloud by rotating the vehicle point cloud by an angle θ (first angle) so that the traveling direction of the vehicle 30 coincides with the direction of the X axis. The angle θ is an angle formed by the traveling direction vector calculated by the traveling direction calculation unit 518 and the reference vector Z of the direction. In the example shown in FIG. 6, since the reference vector Z is (1,0), the vehicle point cloud is rotated about the Z axis.

図７は、回転後の車両点群を示す説明図である。図７に示したように、車両３０の進行方向がＸ軸の方向に一致するように車両点群の向きを正規化することにより、車両３０の進行方向によらずに車両３０の先頭部位および後尾部位の位置を容易に算出することが可能となる。以下、車両点群を回転させる方法をより具体的に説明する。 FIG. 7 is an explanatory diagram showing a vehicle point cloud after rotation. As shown in FIG. 7, by normalizing the direction of the vehicle point cloud so that the traveling direction of the vehicle 30 coincides with the direction of the X axis, the head portion of the vehicle 30 and the head portion of the vehicle 30 regardless of the traveling direction of the vehicle 30 The position of the tail part can be easily calculated. Hereinafter, the method of rotating the vehicle point cloud will be described more specifically.

車両３０の進行方向ベクトルＤ（ｄｘ，ｄｙ）と基準ベクトルＺ（ｚｘ，ｚｙ）のなす角θ（０≦θ≦１８０°）について、以下の数式３が成り立つ。 The following mathematical formula 3 holds for the angle θ (0 ≦ θ ≦ 180 °) formed by the traveling direction vector D (dx, dy) of the vehicle 30 and the reference vector Z (zx, zy).

位置算出部５２０は、上記数式３で算出されるｃｏｓθ、ｓｉｎθに基づき回転行列Ｒ（θ）を求める。図８に示したように、進行方向ベクトルＤのｙ成分Ｄ．ｙ＞０のときは回転方向が右方向、Ｄ．ｙ≦０のときは回転方向が左方向となる。回転方向が右方向である場合、回転行列Ｒ（θ）は、以下の数式４のように表現される。 The position calculation unit 520 obtains the rotation matrix R (θ) based on the cos θ and sin θ calculated by the above equation 3. As shown in FIG. 8, the y component D of the traveling direction vector D. When y> 0, the rotation direction is to the right, D. When y ≦ 0, the rotation direction is to the left. When the rotation direction is the right direction, the rotation matrix R (θ) is expressed by the following equation 4.

一方、回転方向が左方向である場合、回転行列Ｒ（θ）は、以下の数式５のように表現される。 On the other hand, when the rotation direction is the left direction, the rotation matrix R (θ) is expressed by the following equation 5.

そして、位置算出部５２０は、車両点群を構成する各点Ｐ（ｘ，ｙ，ｚ）を、回転行列Ｒ（θ）を用いて回転させる。例えば、ある点Ｐ（ｘ，ｙ，ｚ）の回転後の座標Ｐ’（ｘ’，ｙ’，ｚ’）は、以下の数式６のように表現される。 Then, the position calculation unit 520 rotates each point P (x, y, z) constituting the vehicle point cloud using the rotation matrix R (θ). For example, the coordinate P'(x', y', z') after rotation of a certain point P (x, y, z) is expressed by the following mathematical formula 6.

その後、位置算出部５２０は、回転後の車両点群から、先頭部位の位置Ｈ’（ｈｘ’，ｈｙ’，ｈｚ’）、後尾部位の位置Ｔ’（ｔｘ’，ｔｙ’，ｔｚ’）を求める。Ｘ軸の正方向に進行方向が正規化されているため、図７に示したように、車両点群のうちＸ座標軸上の値が最大となる点が先頭部位、Ｘ座標軸上の値が最小となる点が後尾部位となる。回転後の車両点群Ｏ’_ｐｃの任意の点Ｐ’（ｘ’，ｙ’，ｚ’）とそのｘ成分Ｐ’．ｘ’に関して、ｈｘ’，ｔｘ’は以下の数式７により求められる。 After that, the position calculation unit 520 determines the position H'(hx', hy', hz') of the head portion and the position T'(tx', ty', tz') of the tail portion from the vehicle point cloud after rotation. Ask. Since the traveling direction is normalized in the positive direction of the X axis, as shown in FIG. 7, the point where the value on the X axis is the maximum is the head part, and the value on the X axis is the minimum in the vehicle point cloud. The point that becomes is the tail part. Arbitrary point P'(x', y', z') of the vehicle point cloud _O'pc after rotation and its x component P'. Regarding x', hx'and tx' can be obtained by the following mathematical formula 7.

位置算出部５２０は、ｍａｘ（Ｐ’．ｘ’）となる点Ｐ’＿ｍａｘのｙ成分，ｚ成分をｈｙ’，ｈｚ’として算出してもよい。位置算出部５２０は、同様に、ｍｉｎ（Ｐ’．ｘ’）となる点Ｐ’＿ｍｉｎのｙ成分，ｚ成分をｔｙ’，ｔｚ’として算出してもよい。 The position calculation unit 520 may calculate the y component and z component of the point P ′ _max at max (P ′. X ′) as hy ′ and hz ′. Similarly, the position calculation unit 520 may calculate the y component and z component of the point P'_min at min (P'.x') as ty'and tz'.

または、位置算出部５２０は、図１０に示したようにｈｙ’，ｈｚ’が安定的に算出されるように、以下の数式８によりｈｙ’，ｈｚ’を算出してもよい。 Alternatively, the position calculation unit 520 may calculate hy', hz'by the following mathematical formula 8 so that hy', hz' can be stably calculated as shown in FIG.

同様に、位置算出部５２０は、以下の数式９によりｈｙ’，ｈｚ’を算出してもよい。 Similarly, the position calculation unit 520 may calculate hy', hz' by the following mathematical formula 9.

位置算出部５２０は、上述した演算で算出した先頭部位の位置Ｈ’、後尾部位の位置Ｔ’を、図９に示すように逆回転行列Ｒ（－θ）で逆回転し、先頭部位の位置Ｈ（ｈｘ，ｈｙ，ｈｚ）、および後尾部位の位置Ｔ（ｔｘ，ｔｙ，ｔｚ）を算出する。例えば、回転方向が右方向の場合、Ｒ（－θ）は以下の数式１０のように表現される。 The position calculation unit 520 reversely rotates the position H'of the head portion and the position T'of the tail portion calculated by the above calculation in the reverse rotation matrix R (−θ) as shown in FIG. 9, and the position of the head portion. H (hx, hy, hz) and the position T (tx, ty, tz) of the tail portion are calculated. For example, when the rotation direction is to the right, R (−θ) is expressed by the following mathematical formula 10.

このため、先頭部位の位置Ｈの算出は、以下の数式１１のように行われる。後尾部位の位置Ｔも同様に、後尾部位の位置Ｔ’を回転行列Ｒ（－θ）で逆回転させることで算出される。 Therefore, the calculation of the position H of the head portion is performed as in the following mathematical formula 11. Similarly, the position T of the tail portion is calculated by rotating the position T'of the tail portion in the reverse direction by the rotation matrix R (−θ).

（物体サイズ算出部５２２）
物体サイズ算出部５２２は、進行方向算出部５１８により算出された進行方向と所定の関係にある方向上における物体のサイズを算出する。特に、本発明の一実施形態による物体サイズ算出部５２２は、車両３０の進行方向に一致する前後方向上での前後サイズ、および車両３０の進行方向に直交する幅方向上での幅サイズなどの車両サイズを算出する。 (Object size calculation unit 522)
The object size calculation unit 522 calculates the size of the object in the direction having a predetermined relationship with the traveling direction calculated by the traveling direction calculation unit 518. In particular, the object size calculation unit 522 according to the embodiment of the present invention has a front-rear size in the front-rear direction corresponding to the traveling direction of the vehicle 30, a width size in the width direction orthogonal to the traveling direction of the vehicle 30, and the like. Calculate the vehicle size.

物体サイズ算出部５２２は、車両３０の幅サイズを、回転行列Ｒ（θ）により回転された後の車両点群におけるＹ軸上の最大値およびＹ軸上の最小値の差分から算出することができる。例えば、物体サイズ算出部５２２は、以下の数式１２により幅サイズＷを算出してもよい。 The object size calculation unit 522 may calculate the width size of the vehicle 30 from the difference between the maximum value on the Y axis and the minimum value on the Y axis in the vehicle point cloud after being rotated by the rotation matrix R (θ). can. For example, the object size calculation unit 522 may calculate the width size W by the following mathematical formula 12.

また、物体サイズ算出部５２２は、車両３０の先頭部位および後尾部位の間の距離を車両３０の前後サイズとして算出することができる。例えば、物体サイズ算出部５２２は、以下の数式１３に示すように、点Ｈ’と点Ｔ’、または点Ｈと点Ｔの二点間ユークリッド距離を車両３０の前後サイズＬとして算出してもよい。 Further, the object size calculation unit 522 can calculate the distance between the head portion and the tail portion of the vehicle 30 as the front-rear size of the vehicle 30. For example, as shown in the following mathematical formula 13, the object size calculation unit 522 may calculate the Euclidean distance between the points H'and T'or the two points between the points H and T as the front-rear size L of the vehicle 30. good.

（表示制御部５２４、表示部５２６）
表示制御部５２４は、多様な表示画面を生成する。表示部５２６は、表示制御部５２４により生成された表示画面を表示する。例えば、表示部５２６は、位置算出部５２０により算出された車両３０の先頭部位の位置および後尾部位の位置などを示す位置表示画面を生成し、当該位置表示画面を表示部５２６に表示させてもよい。位置表示画面の具体例は図３を参照して説明した通りである。 (Display control unit 524, display unit 526)
The display control unit 524 generates various display screens. The display unit 526 displays the display screen generated by the display control unit 524. For example, the display unit 526 may generate a position display screen showing the position of the head portion and the position of the tail portion of the vehicle 30 calculated by the position calculation unit 520, and display the position display screen on the display unit 526. good. A specific example of the position display screen is as described with reference to FIG.

（予測部５２８）
予測部５２８は、進行方向算出部５１８により算出された車両３０の進行方向、位置算出部５２０により算出された車両３０の先頭部位の位置または後尾部位の位置に基づき、車両３０に関する予測を行う。例えば、予測部５２８は、位置算出部５２０により算出された車両３０の先頭部位の位置と合流地点との距離に基づき、合流地点に車両３０の先頭部位が到達する時刻を算出する到達時刻予測部としての機能を有する。ここで、合流地点は他の位置の一例であり、本発明の一実施形態が交差点における運転支援に適用される場合には、他の位置は交差点であってもよい。 (Prediction unit 528)
The prediction unit 528 makes a prediction about the vehicle 30 based on the traveling direction of the vehicle 30 calculated by the traveling direction calculation unit 518, the position of the head portion or the tail portion of the vehicle 30 calculated by the position calculation unit 520. For example, the prediction unit 528 calculates the arrival time prediction unit that calculates the time when the head portion of the vehicle 30 arrives at the confluence point based on the distance between the position of the head portion of the vehicle 30 and the confluence point calculated by the position calculation unit 520. Has the function as. Here, the merging point is an example of another position, and when one embodiment of the present invention is applied to driving assistance at an intersection, the other position may be an intersection.

また、予測部５２８は、進行方向算出部５１８により算出された車両３０の進行方向に基づき、車両３０の以降の動きを予測する動き予測部としての機能を有してもよい。例えば、予測部５２８は、車両３０の進行方向が大幅に変化する頻度は低いという想定の下、進行方向算出部５１８により算出された車両３０の現在の進行方向に基づき、数秒後、または数分後などの所定時間後に車両３０が存在する可能性がある位置を予測する。ここで、車両３０の進入禁止エリアとして設定されているエリア内の位置に所定時間後に車両３０が存在する可能性があると予測された場合、アラートの制御が行われてもよい。 Further, the prediction unit 528 may have a function as a motion prediction unit for predicting the subsequent movement of the vehicle 30 based on the travel direction of the vehicle 30 calculated by the traveling direction calculation unit 518. For example, the prediction unit 528 is based on the current traveling direction of the vehicle 30 calculated by the traveling direction calculation unit 518 under the assumption that the traveling direction of the vehicle 30 changes infrequently, after a few seconds or a few minutes. Predict the position where the vehicle 30 may exist after a predetermined time such as after. Here, if it is predicted that the vehicle 30 may exist at a position in the area set as the entry prohibited area of the vehicle 30 after a predetermined time, the alert may be controlled.

（通信部５３０）
通信部５３０は、他の装置と多様な情報を送信および受信する。例えば、通信部５３０は、位置算出部５２０により算出された各車両３０の先頭部位の位置および後尾部位の位置、物体サイズ算出部５２２により算出された各車両３０の車両サイズ、または予測部５２８により得られた予測結果、のうちの少なくともいずれかの情報を車両２０に搭載された運転支援装置２００に送信してもよい。運転支援装置２００は、この通信部５３０から受信される情報に基づき、車両２０の走行計画を適切にたてることが可能となる。 (Communication unit 530)
The communication unit 530 transmits and receives various information with other devices. For example, the communication unit 530 is based on the position of the head portion and the tail portion of each vehicle 30 calculated by the position calculation unit 520, the vehicle size of each vehicle 30 calculated by the object size calculation unit 522, or the prediction unit 528. At least one of the obtained prediction results may be transmitted to the driving support device 200 mounted on the vehicle 20. The driving support device 200 can appropriately make a traveling plan of the vehicle 20 based on the information received from the communication unit 530.

＜４．位置算出装置の動作＞
以上、本発明の一実施形態による位置算出システムの構成を説明した。続いて、図１１および図１２を参照し、本発明の一実施形態による位置算出装置５０の動作を整理する。 <4. Operation of position calculation device>
The configuration of the position calculation system according to the embodiment of the present invention has been described above. Subsequently, with reference to FIGS. 11 and 12, the operation of the position calculation device 50 according to the embodiment of the present invention will be arranged.

図１１は、本発明の一実施形態による位置算出装置５０の動作を示すフローチャートである。図１１に示したように、まず、位置算出装置５０の点群取得部５１０がレーザーセンサ４０から点群を取得する（Ｓ３１０）。そして、物体検出部５１２が、点群取得部５１０により取得された各フレームの点群から、背景差分技術またはクラスタリング技術などを用いて物体を検出する（Ｓ３２０）。これにより、各フレームから物体を構成する点群である三次元点群情報が得られ、各フレームの三次元点群情報が表示制御部５２４に記憶される。 FIG. 11 is a flowchart showing the operation of the position calculation device 50 according to the embodiment of the present invention. As shown in FIG. 11, first, the point cloud acquisition unit 510 of the position calculation device 50 acquires the point cloud from the laser sensor 40 (S310). Then, the object detection unit 512 detects an object from the point cloud of each frame acquired by the point cloud acquisition unit 510 using background subtraction technique, clustering technique, or the like (S320). As a result, three-dimensional point cloud information, which is a point cloud constituting an object, is obtained from each frame, and the three-dimensional point cloud information of each frame is stored in the display control unit 524.

続いて、物体追跡部５１６が、あるフレームで検出された物体と、他のフレームで検出された物体との対応付けを行う（Ｓ３３０）。さらに、進行方向算出部５１８が、物体検出部５１２により検出され、物体追跡部５１６によりフレーム間で対応付けられた物体の進行方向を算出する（Ｓ３４０）。 Subsequently, the object tracking unit 516 associates the object detected in a certain frame with the object detected in another frame (S330). Further, the traveling direction calculation unit 518 is detected by the object detection unit 512, and the object tracking unit 516 calculates the traveling direction of the object associated between the frames (S340).

そして、位置算出部５２０は、物体である車両３０の、進行方向に対する先頭部位および後尾部位の位置を算出する（Ｓ３５０）。さらに、物体サイズ算出部５２２が、車両３０の進行方向に一致する前後方向上での前後サイズ、および車両３０の進行方向に直交する幅方向じょうでの幅サイズなどの車両サイズを算出する（Ｓ３６０）。そして、表示制御部５２４による位置表示画面の表示、および、通信部５３０による各車両３０の先頭部位の位置および後尾部位の位置の運転支援装置２００への送信などが行われる（Ｓ３７０）。上述したＳ３５０およびＳ３６０の処理については図１２を参照してより詳細に整理する。 Then, the position calculation unit 520 calculates the positions of the head portion and the tail portion of the vehicle 30, which is an object, with respect to the traveling direction (S350). Further, the object size calculation unit 522 calculates the vehicle size such as the front-rear size in the front-rear direction corresponding to the traveling direction of the vehicle 30 and the width size in the width direction orthogonal to the traveling direction of the vehicle 30 (S360). ). Then, the display control unit 524 displays the position display screen, and the communication unit 530 transmits the position of the head portion and the position of the tail portion of each vehicle 30 to the driving support device 200 (S370). The above-mentioned processes of S350 and S360 will be organized in more detail with reference to FIG.

図１２は、位置算出およびサイズ算出の詳細を示すフローチャートである。図１２に示したように、位置算出部５２０は、車両３０の進行方向がＸ軸の方向に一致するように車両点群の向きを正規化する回転行列Ｒ（θ）を算出し（Ｓ４１０）、当該回転行列Ｒ（θ）を用いて車両点群を回転させる（Ｓ４２０）。 FIG. 12 is a flowchart showing the details of position calculation and size calculation. As shown in FIG. 12, the position calculation unit 520 calculates a rotation matrix R (θ) that normalizes the direction of the vehicle point group so that the traveling direction of the vehicle 30 coincides with the direction of the X axis (S410). , The vehicle point group is rotated using the rotation matrix R (θ) (S420).

そして、位置算出部５２０は、回転後の車両点群から、先頭部位の位置Ｈ’（ｈｘ’，ｈｙ’，ｈｚ’）、後尾部位の位置Ｔ’（ｔｘ’，ｔｙ’，ｔｚ’）を取得する（Ｓ４３０）。Ｘ軸の正方向に進行方向が正規化されているため、図７に示したように、車両点群のうちＸ座標軸上の値が最大となる点が先頭部位、Ｘ座標軸上の値が最小となる点が後尾部位となる。 Then, the position calculation unit 520 determines the position H'(hx', hy', hz') of the head portion and the position T'(tx', ty', tz') of the tail portion from the vehicle point cloud after rotation. Acquire (S430). Since the traveling direction is normalized in the positive direction of the X axis, as shown in FIG. 7, the point where the value on the X axis is the maximum is the head part, and the value on the X axis is the minimum in the vehicle point cloud. The point that becomes is the tail part.

さらに、位置算出部５２０は、先頭部位の位置Ｈ’、後尾部位の位置Ｔ’を、逆回転行列Ｒ（－θ）で逆回転し、先頭部位の位置Ｈ（ｈｘ，ｈｙ，ｈｚ）、および後尾部位の位置Ｔ（ｔｘ，ｔｙ，ｔｚ）を算出する（Ｓ４４０）。その後、物体サイズ算出部５２２が、車両３０の進行方向に一致する前後方向上での前後サイズ、および車両３０の進行方向に直交する幅方向上での幅サイズなどの車両サイズを算出する。 Further, the position calculation unit 520 reversely rotates the position H'of the head portion and the position T'of the tail portion by the reverse rotation matrix R (−θ), and the position H (hx, hy, hz) of the head portion and the position Hz. The position T (tx, ty, tz) of the tail portion is calculated (S440). After that, the object size calculation unit 522 calculates the vehicle size such as the front-rear size in the front-rear direction corresponding to the traveling direction of the vehicle 30 and the width size in the width direction orthogonal to the traveling direction of the vehicle 30.

＜５．作用効果＞
上述した本発明の一実施形態によれば、多様な作用効果が得られる。例えば、本発明の一実施形態によれば、道路の方向が事前設定されていない場合や、交差点など車両３０の進行方向が不定である場所でも、車両３０の進行方向を適切に算出することが可能である。 <5. Action effect>
According to the above-described embodiment of the present invention, various effects can be obtained. For example, according to one embodiment of the present invention, it is possible to appropriately calculate the traveling direction of the vehicle 30 even when the direction of the road is not preset or the traveling direction of the vehicle 30 is undefined such as an intersection. It is possible.

さらに、本発明の一実施形態によれば、車両３０の進行方向に基づき、進行方向に対する車両３０の先頭部位の位置および後尾部位の位置を算出できる。このような車両３０の先頭部位の位置および後尾部位の位置は、車車間距離の算出や、車両３０の先頭部位が合流地点および交差点などの所定の位置に到達する到着時刻の算出に用いられ得る。結果、自動運転の運転支援および安全支援を実現することが可能である。 Further, according to one embodiment of the present invention, the position of the head portion and the position of the tail portion of the vehicle 30 with respect to the traveling direction can be calculated based on the traveling direction of the vehicle 30. Such positions of the head portion and the tail portion of the vehicle 30 can be used for calculating the inter-vehicle distance and for calculating the arrival time when the head portion of the vehicle 30 reaches a predetermined position such as a confluence point and an intersection. .. As a result, it is possible to realize driving support and safety support for autonomous driving.

また、本発明の一実施形態によれば、車両３０の進行方向がある座標軸に一致するように車両点群を回転させることにより、車両３０の先頭部位の位置および後尾部位の位置を算出する。かかる構成によれば、簡易、かつより正確に車両３０の先頭部位の位置および後尾部位の位置を算出可能である。 Further, according to one embodiment of the present invention, the position of the head portion and the position of the tail portion of the vehicle 30 are calculated by rotating the vehicle point cloud so that the traveling direction of the vehicle 30 coincides with a certain coordinate axis. According to such a configuration, the position of the head portion and the position of the tail portion of the vehicle 30 can be calculated easily and more accurately.

＜６．ハードウェア構成＞
以上、本発明の一実施形態を説明した。上述した進行方向の算出および先頭部位の位置の算出などの情報処理は、ソフトウェアと、以下に説明する位置算出装置５０のハードウェアとの協働により実現される。 <6. Hardware configuration>
The embodiment of the present invention has been described above. Information processing such as the calculation of the traveling direction and the calculation of the position of the head portion described above is realized by the cooperation between the software and the hardware of the position calculation device 50 described below.

図１３は、位置算出装置５０のハードウェア構成を示したブロック図である。位置算出装置５０は、ＣＰＵ（ＣｅｎｔｒａｌＰｒｏｃｅｓｓｉｎｇＵｎｉｔ）５０１と、ＲＯＭ（ＲｅａｄＯｎｌｙＭｅｍｏｒｙ）５０２と、ＲＡＭ（ＲａｎｄｏｍＡｃｃｅｓｓＭｅｍｏｒｙ）５０３と、ホストバス５０４と、を備える。また、位置算出装置５０は、ブリッジ５０５と、外部バス５０６と、インターフェース５０７と、入力装置５０８と、表示装置５０９と、音声出力装置５１１と、ストレージ装置（ＨＤＤ）５１３と、ネットワークインターフェース５１５とを備える。 FIG. 13 is a block diagram showing the hardware configuration of the position calculation device 50. The position calculation device 50 includes a CPU (Central Processing Unit) 501, a ROM (Read Only Memory) 502, a RAM (Random Access Memory) 503, and a host bus 504. Further, the position calculation device 50 includes a bridge 505, an external bus 506, an interface 507, an input device 508, a display device 509, an audio output device 511, a storage device (HDD) 513, and a network interface 515. Be prepared.

ＣＰＵ５０１は、演算処理装置および制御装置として機能し、各種プログラムに従って位置算出装置５０内の動作全般を制御する。また、ＣＰＵ５０１は、マイクロプロセッサであってもよい。ＲＯＭ５０２は、ＣＰＵ５０１が使用するプログラムや演算パラメータ等を記憶する。ＲＡＭ５０３は、ＣＰＵ５０１の実行において使用するプログラムや、その実行において適宜変化するパラメータ等を一時記憶する。これらはＣＰＵバスなどから構成されるホストバス５０４により相互に接続されている。これらＣＰＵ５０１、ＲＯＭ５０２およびＲＡＭ５０３とソフトウェアとの協働により、点群取得部５１０、物体検出部５１２、物体追跡部５１６、進行方向算出部５１８、位置算出部５２０、物体サイズ算出部５２２、表示制御部５２４および予測部５２８などの機能が実現され得る。 The CPU 501 functions as an arithmetic processing device and a control device, and controls the overall operation in the position calculation device 50 according to various programs. Further, the CPU 501 may be a microprocessor. The ROM 502 stores programs, calculation parameters, and the like used by the CPU 501. The RAM 503 temporarily stores a program used in the execution of the CPU 501, parameters that appropriately change in the execution, and the like. These are connected to each other by a host bus 504 composed of a CPU bus or the like. By collaborating with the CPU 501, ROM 502 and RAM 503, the point cloud acquisition unit 510, the object detection unit 512, the object tracking unit 516, the traveling direction calculation unit 518, the position calculation unit 520, the object size calculation unit 522, and the display control unit. Functions such as 524 and predictor 528 can be realized.

ホストバス５０４は、ブリッジ５０５を介して、ＰＣＩ（ＰｅｒｉｐｈｅｒａｌＣｏｍｐｏｎｅｎｔＩｎｔｅｒｃｏｎｎｅｃｔ／Ｉｎｔｅｒｆａｃｅ）バスなどの外部バス５０６に接続されている。なお、必ずしもホストバス５０４、ブリッジ５０５および外部バス５０６を分離構成する必要はなく、１つのバスにこれらの機能を実装してもよい。 The host bus 504 is connected to an external bus 506 such as a PCI (Peripheral Component Interconnect / Interface) bus via a bridge 505. It is not always necessary to separately configure the host bus 504, the bridge 505, and the external bus 506, and these functions may be implemented in one bus.

入力装置５０８は、マウス、キーボード、タッチパネル、ボタン、マイクロフォン、センサ、スイッチおよびレバーなどユーザが情報を入力するための入力手段と、ユーザによる入力に基づいて入力信号を生成し、ＣＰＵ５０１に出力する入力制御回路などから構成されている。位置算出装置５０のユーザは、該入力装置５０８を操作することにより、位置算出装置５０に対して各種のデータを入力したり処理動作を指示したりすることができる。 The input device 508 is an input means for the user to input information such as a mouse, a keyboard, a touch panel, a button, a microphone, a sensor, a switch, and a lever, and an input signal generated based on the input by the user and output to the CPU 501. It consists of a control circuit and so on. By operating the input device 508, the user of the position calculation device 50 can input various data to the position calculation device 50 and instruct the processing operation.

表示装置５０９は、例えば、液晶ディスプレイ（ＬＣＤ）装置、プロジェクター装置、ＯＬＥＤ（ＯｒｇａｎｉｃＬｉｇｈｔＥｍｉｔｔｉｎｇＤｉｏｄｅ）装置およびランプなどの表示装置を含む。また、音声出力装置５１１は、スピーカおよびヘッドホンなどの音声出力装置を含む。 The display device 509 includes, for example, a liquid crystal display (LCD) device, a projector device, an OLED (Organic Light Emitting Diode) device, and a display device such as a lamp. Further, the audio output device 511 includes an audio output device such as a speaker and headphones.

ストレージ装置５１３は、本実施形態にかかる位置算出装置５０の記憶部の一例として構成されたデータ格納用の装置である。ストレージ装置５１３は、記憶媒体、記憶媒体にデータを記録する記録装置、記憶媒体からデータを読み出す読出し装置および記憶媒体に記録されたデータを削除する削除装置などを含んでもよい。ストレージ装置５１３は、例えば、ＨＤＤ（ＨａｒｄＤｉｓｋＤｒｉｖｅ）またはＳＳＤ（ＳｏｌｉｄＳｔｒａｇｅＤｒｉｖｅ）、あるいは同等の機能を有するメモリ等で構成される。このストレージ装置５１３は、ストレージを駆動し、ＣＰＵ５０１が実行するプログラムや各種データを格納する。 The storage device 513 is a data storage device configured as an example of the storage unit of the position calculation device 50 according to the present embodiment. The storage device 513 may include a storage medium, a recording device for recording data on the storage medium, a reading device for reading data from the storage medium, a deleting device for deleting data recorded on the storage medium, and the like. The storage device 513 is composed of, for example, an HDD (Hard Disk Drive) or SSD (Solid Storage Drive), or a memory having an equivalent function. The storage device 513 drives the storage and stores programs and various data executed by the CPU 501.

ネットワークインターフェース５１５は、例えば、ネットワークに接続するための通信デバイス等で構成された通信インターフェースである。また、ネットワークインターフェース５１５は、無線ＬＡＮ（ＬｏｃａｌＡｒｅａＮｅｔｗｏｒｋ）対応通信装置であっても、有線による通信を行うワイヤー通信装置であってもよい。 The network interface 515 is, for example, a communication interface composed of a communication device or the like for connecting to a network. Further, the network interface 515 may be a wireless LAN (Local Area Network) compatible communication device or a wire communication device that performs wired communication.

なお、上述した位置算出装置５０のハードウェア構成は運転支援装置２００にも適用可能である。 The hardware configuration of the position calculation device 50 described above can also be applied to the operation support device 200.

＜７．補足＞
以上、添付図面を参照しながら本発明の好適な実施形態について詳細に説明したが、本発明はかかる例に限定されない。本発明の属する技術の分野における通常の知識を有する者であれば、特許請求の範囲に記載された技術的思想の範疇内において、各種の変更例または修正例に想到し得ることは明らかであり、これらについても、当然に本発明の技術的範囲に属するものと了解される。 <7. Supplement>
Although the preferred embodiments of the present invention have been described in detail with reference to the accompanying drawings, the present invention is not limited to these examples. It is clear that a person having ordinary knowledge in the field of the art to which the present invention belongs can come up with various modifications or modifications within the scope of the technical idea described in the claims. , These are also naturally understood to belong to the technical scope of the present invention.

例えば、上記ではセンサとしてＬｉＤＡＲのようなレーザーセンサ４０を主に想定して説明したが、レーダーセンサやまたはカメラセンサ等、物体を形状として捉えるセンサを利用可能である。さらに、物体を形状として捉えることのできない単体の検出データを組み合わせ、物体を形状として捉えられるように合成することも可能である。 For example, in the above description, a laser sensor 40 such as LiDAR is mainly assumed as a sensor, but a sensor such as a radar sensor or a camera sensor that captures an object as a shape can be used. Furthermore, it is also possible to combine single detection data that cannot capture an object as a shape and synthesize it so that the object can be captured as a shape.

また、図３には、車両３０の先頭部位および後尾部位を表示する位置表示画面を示した。位置表示画面は、さらに、進行方向ベクトル、または基準ベクトルに対する進行方向ベクトルの角度θを表示してもよい。 Further, FIG. 3 shows a position display screen for displaying the head portion and the tail portion of the vehicle 30. The position display screen may further display the angle θ of the traveling direction vector or the traveling direction vector with respect to the reference vector.

また、上記ではレーザーセンサ４０が路側に固定的に設置される例を想定して説明したが、レーザーセンサ４０は移動物体、例えば車両３０に搭載されてもよい。この場合、車両３０の移動量や移動方向が判明していれば、検出された周囲の車両とレーザーセンサ４０との相対的な位置関係から、周囲の車両の進行方向や先頭部位の位置などに関して、ある地点を基準とした進行方向や先頭部位の位置などに変換することも可能である。 Further, in the above description, the example in which the laser sensor 40 is fixedly installed on the roadside has been described, but the laser sensor 40 may be mounted on a moving object, for example, a vehicle 30. In this case, if the amount of movement and the direction of movement of the vehicle 30 are known, the relative positional relationship between the detected surrounding vehicle and the laser sensor 40 indicates the traveling direction of the surrounding vehicle, the position of the leading portion, and the like. , It is also possible to convert to the direction of travel based on a certain point or the position of the leading part.

また、上述した実施形態では、車両３０がバックで移動している場合、車両３０の実際の後尾部位が先頭部位として特定され、車両３０の実際の先頭部位が後尾部位として特定される。一方、本発明の一実施形態の用途によっては、車両３０がバックで移動している場合にも、車両３０の実際の先頭部位が先頭部位として特定され、車両３０の実際の後尾部位が後尾部位として特定されることが望まれ得る。そこで、位置算出装置５０は、車両３０が車両前方に移動しているかバックで移動しているかを判断し、バックで移動していると判断した場合には、進行方向に対する先頭部位を後尾部位として扱い、進行方向に対する後尾部位を先頭部位として扱ってもよい。なお、車両３０がバックで移動しているか否かの判断は、車両３０の進行方向が反対方向に変化することに基づき行うことが可能である。ただし、進行方向が反対方向に変化しただけでは、走行モードが通常モードからバックに切り替えられたのか、バックから通常モードに切り替えられたかの判断が困難な場合がある。この点に関し、車両３０はバックよりも通常モードで走行する時間が一般的に長いことに基づき、位置算出装置５０は、適用されている時間が短い方の走行モードがバックであると判断してもよい。 Further, in the above-described embodiment, when the vehicle 30 is moving in the back, the actual tail portion of the vehicle 30 is specified as the head portion, and the actual head portion of the vehicle 30 is specified as the tail portion. On the other hand, depending on the use of one embodiment of the present invention, even when the vehicle 30 is moving in the back, the actual head portion of the vehicle 30 is specified as the head portion, and the actual tail portion of the vehicle 30 is the tail portion. It may be desired to be specified as. Therefore, the position calculation device 50 determines whether the vehicle 30 is moving in front of the vehicle or in the back, and if it is determined that the vehicle is moving in the back, the head portion in the traveling direction is set as the tail portion. It may be treated and the tail part in the traveling direction may be treated as the head part. It is possible to determine whether or not the vehicle 30 is moving in the back based on the fact that the traveling direction of the vehicle 30 changes in the opposite direction. However, it may be difficult to determine whether the traveling mode has been switched from the normal mode to the back mode or the back mode has been switched to the normal mode simply by changing the traveling direction in the opposite direction. In this regard, based on the fact that the vehicle 30 generally travels in the normal mode longer than the back, the position calculation device 50 determines that the traveling mode with the shorter applied time is the back. May be good.

また、車両３０がレーザーセンサ４０を通り過ぎると、レーザーセンサ４０が取得する点群に車両３０の実際の先頭部位が含まれなくなり得る。このような場合、位置算出部５２０は、車両３０の後尾部位の位置に、過去に算出された車両３０の前後サイズを進行方向に沿って加算することで、車両３０の先頭部位の位置を算出してもよい。 Further, when the vehicle 30 passes through the laser sensor 40, the point cloud acquired by the laser sensor 40 may not include the actual head portion of the vehicle 30. In such a case, the position calculation unit 520 calculates the position of the head portion of the vehicle 30 by adding the previously calculated front-rear size of the vehicle 30 to the position of the tail portion of the vehicle 30 along the traveling direction. You may.

また、本明細書の位置算出装置５０、運転支援装置２００の処理における各ステップは、必ずしもフローチャートとして記載された順序に沿って時系列に処理する必要はない。例えば、位置算出装置５０の処理における各ステップは、フローチャートとして記載した順序と異なる順序で処理されても、並列的に処理されてもよい。 Further, each step in the processing of the position calculation device 50 and the operation support device 200 of the present specification does not necessarily have to be processed in chronological order according to the order described as the flowchart. For example, each step in the processing of the position calculation device 50 may be processed in an order different from the order described in the flowchart, or may be processed in parallel.

また、位置算出装置５０、運転支援装置２００に内蔵されるＣＰＵ、ＲＯＭおよびＲＡＭなどのハードウェアに、上述した位置算出装置５０、運転支援装置２００の各構成と同等の機能を発揮させるためのコンピュータプログラムも作成可能である。また、該コンピュータプログラムを記憶させた記憶媒体も提供される。 Further, a computer for causing the hardware such as the CPU, ROM, and RAM built in the position calculation device 50 and the operation support device 200 to exhibit the same functions as the configurations of the position calculation device 50 and the operation support device 200 described above. Programs can also be created. A storage medium for storing the computer program is also provided.

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

20 Vehicle 200 Driving support device 210 Communication unit 220 Vehicle information detection unit 230 Plan calculation unit 240 Vehicle control unit 248 Plan calculation unit 250 Display control unit 260 Display unit 30 Vehicle 40 Laser sensor 50 Position calculation device 510 Point group acquisition unit 512 Object detection Unit 514 Object information storage unit 516 Object tracking unit 518 Travel direction calculation unit 520 Position calculation unit 522 Object size calculation unit 524 Display control unit 526 Display unit 528 Prediction unit 530 Communication unit

Claims

A point cloud acquisition unit that acquires point clouds at multiple time points from a laser sensor ,
An object detection unit that detects an object from each of the point clouds at the plurality of time points acquired by the point cloud acquisition unit, and
A traveling direction calculation unit that calculates the traveling direction of the object based on the detection result of the object by the object detection unit, and a traveling direction calculation unit.
The point cloud of the object is rotated by a first angle on the coordinate space defined by the first coordinate axis and the second coordinate axis so that the traveling direction of the object coincides with the direction of the first coordinate axis.
Among the point cloud of the object after rotation, the point where the value on the first coordinate axis becomes the maximum or the point becomes the minimum is specified.
A position calculation unit that calculates the position of the head portion or the tail portion of the object based on the coordinate values of the specified points and the reverse rotation of the first angle in the coordinate space.
A position calculation system.

The position calculation system according to claim 1, further comprising a display control unit that generates a position display screen indicating a position calculated by the position calculation unit.

The object detection unit detects representative positions of the object at the plurality of time points, and determines the representative positions of the object.
The position calculation system according to claim 1 or 2, wherein the traveling direction calculation unit calculates a moving direction of a representative position of the object as the traveling direction.

The position calculation system according to any one of claims 1 to 3, wherein the portion specified with reference to the traveling direction includes a leading portion or a trailing portion with respect to the traveling direction.

The position calculation system according to any one of claims 1 to 4 , further comprising an object size calculation unit for calculating the size of the object in a direction having a predetermined relationship with the traveling direction. ..

The direction having a predetermined relationship with the traveling direction includes the width direction orthogonal to the traveling direction.
The position calculation system according to claim 5 , wherein the object size calculation unit calculates the width size of the object from the difference between the maximum value and the minimum value on the second coordinate axis of the object after rotation.

The direction having a predetermined relationship with the traveling direction includes the front-rear direction corresponding to the traveling direction.
The position calculation system according to claim 5 or 6 , wherein the object size calculation unit calculates the distance between the head portion and the tail portion of the object as the front-back size of the object.

The position calculation system includes a device mounted on a first object.
The position calculation system according to claim 2, wherein the device mounted on the first object has a display unit for displaying a position display screen indicating a position calculated for the second object.

The position calculation system further includes an arrival time prediction unit that calculates the time when the portion of the object arrives at the other position based on the distance between the position calculated by the position calculation unit and the other position. The position calculation system according to any one of claims 1 to 8 .

Any one of claims 1 to 9 , wherein the position calculation system further includes a motion prediction unit that predicts the subsequent movement of the object based on the travel direction of the object calculated by the travel direction calculation unit. The position calculation system described in.

Computer,
A point cloud acquisition unit that acquires point clouds at multiple time points from a laser sensor ,
An object detection unit that detects an object from each of the point clouds at the plurality of time points acquired by the point cloud acquisition unit, and
A traveling direction calculation unit that calculates the traveling direction of the object based on the detection result of the object by the object detection unit, and a traveling direction calculation unit.
The point cloud of the object is rotated by a first angle on the coordinate space defined by the first coordinate axis and the second coordinate axis so that the traveling direction of the object coincides with the direction of the first coordinate axis.
Among the point cloud of the object after rotation, the point where the value on the first coordinate axis becomes the maximum or the point becomes the minimum is specified.
A position calculation unit that calculates the position of the head portion or the tail portion of the object based on the coordinate values of the specified points and the reverse rotation of the first angle in the coordinate space.
A program to function as.

Obtaining a point cloud at multiple time points from a laser sensor ,
Detecting an object from each of the point clouds at multiple time points,
To calculate the traveling direction of the object based on the detection result of the object,
The point cloud of the object is rotated by a first angle on the coordinate space defined by the first coordinate axis and the second coordinate axis so that the traveling direction of the object coincides with the direction of the first coordinate axis.
Among the point cloud of the object after rotation, the point where the value on the first coordinate axis becomes the maximum or the point becomes the minimum is specified.
To calculate the position of the head part or the tail part of the object based on the coordinate value of the specified point and the reverse rotation of the first angle in the coordinate space.
A position calculation method performed by a computer, including.