JP7418196B2

JP7418196B2 - Travel trajectory estimation method and travel trajectory estimation device

Info

Publication number: JP7418196B2
Application number: JP2019224691A
Authority: JP
Inventors: 祐一武田; 千加夫土谷
Original assignee: Renault SAS
Current assignee: Renault SAS
Priority date: 2019-12-12
Filing date: 2019-12-12
Publication date: 2024-01-19
Anticipated expiration: 2039-12-12
Also published as: JP2021092508A

Description

本発明は、車両が走行した走行軌跡を推定する走行軌跡推定方法及び走行軌跡推定装置に関する。 The present invention relates to a travel trajectory estimation method and a travel trajectory estimation device for estimating a travel trajectory traveled by a vehicle.

従来から地図データを更新する方法が知られており、地図データ更新方法として特許文献１が開示されている。この特許文献１では、ＧＰＳデータとオドメトリを用いて車両の自車位置を検出し、走行時における走行軌跡を演算していた。 BACKGROUND ART Methods for updating map data have been known for a long time, and Patent Document 1 discloses a method for updating map data. In Patent Document 1, the position of the vehicle is detected using GPS data and odometry, and the travel trajectory during travel is calculated.

特開２００５－９８８５３号公報Japanese Patent Application Publication No. 2005-98853

しかしながら、ＧＰＳ信号は必ずしも安定して取得できるとは限らない。そのため、上述した従来の地図データ更新方法では、ＧＰＳ信号を安定して取得できない状況において、車両の走行軌跡を精度よく推定することができないという問題点があった。 However, GPS signals cannot always be obtained stably. Therefore, the conventional map data updating method described above has a problem in that it is not possible to accurately estimate the travel trajectory of the vehicle in a situation where GPS signals cannot be stably acquired.

そこで、本発明は、上記課題に鑑みて成されたものであり、ＧＰＳ信号を安定して取得できない状況であっても、車両の走行軌跡を精度よく推定することのできる走行軌跡推定方法及び走行軌跡推定装置を提供することを目的とする。 SUMMARY OF THE INVENTION The present invention has been made in view of the above-mentioned problems, and provides a driving trajectory estimation method and driving trajectory that can accurately estimate the driving trajectory of a vehicle even in situations where GPS signals cannot be stably acquired. The purpose of this invention is to provide a trajectory estimation device.

本発明の一態様に係る走行軌跡推定方法及びその装置は、走行データに基づいて車両が走行した走行軌跡を算出し、車両周辺の環境情報と地図情報に記録された環境情報とを照合して車両の基準位置を検出する。そして、基準位置と走行軌跡との差である車両位置誤差を算出し、走行軌跡上にある車両位置のうち、基準位置が検出された車両位置では車両位置誤差に基づいて走行軌跡を補正する。一方、基準位置が検出されなかった車両位置では車両位置の前後に隣接する車両位置の間の移動量を示す相対変位と車両位置におけるオドメトリとの差である変位誤差を算出し、変位誤差に基づいて走行軌跡を補正する。 A travel trajectory estimation method and device according to one aspect of the present invention calculates a travel trajectory traveled by a vehicle based on travel data, and compares environmental information around the vehicle with environmental information recorded in map information. Detect the reference position of the vehicle. Then, a vehicle position error, which is the difference between the reference position and the travel trajectory, is calculated, and among the vehicle positions on the travel trajectory, the travel trajectory is corrected based on the vehicle position error at vehicle positions where the reference position is detected. On the other hand, for vehicle positions where no reference position has been detected , the displacement error, which is the difference between the relative displacement indicating the amount of movement between the vehicle positions before and after the vehicle position and the odometry at the vehicle position, is calculated, and based on the displacement error. Correct the traveling trajectory.

本発明によれば、ＧＰＳ信号を安定して取得できない状況であっても、車両の走行軌跡を精度よく推定することができる。 According to the present invention, even in a situation where GPS signals cannot be stably acquired, it is possible to accurately estimate the travel trajectory of a vehicle.

図１は、本発明の一実施形態に係る走行軌跡推定装置を備えた走行軌跡推定システムの構成を示すブロック図である。FIG. 1 is a block diagram showing the configuration of a travel trajectory estimation system including a travel trajectory estimation device according to an embodiment of the present invention. 図２は、本発明の一実施形態に係る走行軌跡推定装置による走行軌跡の補正方法を説明するための図である。FIG. 2 is a diagram for explaining a method of correcting a traveling trajectory by the traveling trajectory estimating device according to an embodiment of the present invention. 図３は、本発明の一実施形態に係る走行軌跡推定装置による走行軌跡の補正方法を説明するための図である。FIG. 3 is a diagram for explaining a method of correcting a traveling trajectory by the traveling trajectory estimating device according to an embodiment of the present invention. 図４は、本発明の一実施形態に係る走行軌跡推定装置による走行軌跡の補正方法を説明するための図である。FIG. 4 is a diagram for explaining a method of correcting a traveling trajectory by the traveling trajectory estimating device according to an embodiment of the present invention. 図５は、本発明の一実施形態に係る走行軌跡推定装置による走行軌跡の推定処理の処理手順を示すフローチャートである。FIG. 5 is a flowchart showing a processing procedure for estimating a travel trajectory by the travel trajectory estimation device according to an embodiment of the present invention.

以下、本発明の一実施形態について、図面を参照して説明する。図面の記載において同一部分には同一符号を付して詳細な説明は省略する。 Hereinafter, one embodiment of the present invention will be described with reference to the drawings. In the description of the drawings, the same parts are denoted by the same reference numerals and detailed explanations will be omitted.

［走行軌跡推定システムの構成］
図１を参照して、本実施形態に係る走行軌跡推定装置を備えた走行軌跡推定システムの構成を説明する。図１に示すように、走行軌跡推定システム１００は、走行軌跡推定装置１と、ＧＰＳ受信機３と、ＩＭＵ（慣性計測装置）５と、カメラ７と、レーザレンジファインダー９と、データベース１１とを備える。この他にミリ波レーダーや通信機等を備えていてもよい。また、本実施形態では、走行軌跡推定システム１００が車両に搭載されている場合について説明するが、走行軌跡推定装置１については車両に搭載されていなくてもよい。 [Configuration of travel trajectory estimation system]
With reference to FIG. 1, the configuration of a travel trajectory estimation system including a travel trajectory estimation device according to this embodiment will be described. As shown in FIG. 1, the traveling trajectory estimation system 100 includes a traveling trajectory estimation device 1, a GPS receiver 3, an IMU (inertial measurement unit) 5, a camera 7, a laser range finder 9, and a database 11. Be prepared. In addition to this, it may also be equipped with a millimeter wave radar, a communication device, etc. Further, in this embodiment, a case will be described in which the travel trajectory estimation system 100 is installed in a vehicle, but the travel trajectory estimation device 1 may not be installed in a vehicle.

ＧＰＳ受信機３は、人工衛星からの電波を受信することにより、地上における自車両の現在地を検出し、検出したデータをデータベース１１に出力する。 The GPS receiver 3 detects the current location of the vehicle on the ground by receiving radio waves from an artificial satellite, and outputs the detected data to the database 11.

ＩＭＵ５は、３軸のジャイロセンサと３方向の加速度計から構成され、３次元の角度（または角速度）と加速度を検出する。また、ＩＭＵ５は、車輪速センサを備えていてもよい。そして、ＩＭＵ５は、検出したセンサ値を用いてオドメトリを算出する。オドメトリは、車両の単位時間当たりの移動量であり、ＩＭＵ５で検出されたデータや車両に搭載されたセンサ群で検出されたセンサ値を用いて算出することができる。ＩＭＵ５は、検出したデータと算出したオドメトリをデータベース１１に出力する。 The IMU 5 is composed of a 3-axis gyro sensor and a 3-direction accelerometer, and detects a 3-dimensional angle (or angular velocity) and acceleration. Moreover, IMU5 may be equipped with a wheel speed sensor. The IMU 5 then calculates odometry using the detected sensor values. Odometry is the amount of movement of the vehicle per unit time, and can be calculated using data detected by the IMU 5 and sensor values detected by a group of sensors mounted on the vehicle. The IMU 5 outputs the detected data and the calculated odometry to the database 11.

カメラ７は、ＣＣＤ（Charge-Coupled Device）やＣＭＯＳ（Complementary Metal Oxide Semiconductor）などの撮像素子を有する。カメラ７は、自車両に搭載され、自車両の周囲を撮影する。カメラ７は画像処理機能を有しており、撮影した画像から車両周囲の環境情報を検出する。環境情報には、物標と道路構造が含まれている。物標とは、道路や歩道に設けられた物体であり、例えば信号機や電柱、交通標識等である。また、道路構造とは、道路の構成物であり、例えば白線や黄色線等の道路の区画線や停止線の他に道路端部や縁石、中央分離帯等である。カメラ７は、検出したデータをデータベース１１に出力する。 The camera 7 has an image sensor such as a CCD (Charge-Coupled Device) or a CMOS (Complementary Metal Oxide Semiconductor). The camera 7 is mounted on the own vehicle and photographs the surroundings of the own vehicle. The camera 7 has an image processing function and detects environmental information around the vehicle from the captured images. The environmental information includes landmarks and road structures. A target is an object provided on a road or sidewalk, such as a traffic light, a utility pole, or a traffic sign. Furthermore, the term "road structure" refers to components of a road, such as road markings such as white lines and yellow lines, stop lines, road edges, curbs, median strips, and the like. The camera 7 outputs detected data to the database 11.

レーザレンジファインダー９は、光センサ技術であるＬｉＤＡＲ（Light Detection and Ranging）を用いて車両周囲の環境情報を検出するセンサである。具体的に、レーザレンジファインダー９は、レーザ光をある角度範囲内で走査し、その時の反射光を受光して、レーザ発射時点と反射光の受光時点との間の時間差を計測して自車両に対する物標や道路構造の相対距離や方向（すなわち、自車両に対する相対位置）などを検出する。レーザレンジファインダー９は、検出したデータをデータベース１１に出力する。尚、レーザレンジファインダー９は、車両周囲の物標の車両に対する相対位置を検出するセンサであれば、ＬｉＤＡＲ以外であっても良く、例えばレーザレーダあるいは音波レーダなどのレーザレンジファインダー以外のセンサを用いてもよい。 The laser range finder 9 is a sensor that detects environmental information around the vehicle using LiDAR (Light Detection and Ranging), which is an optical sensor technology. Specifically, the laser range finder 9 scans the laser beam within a certain angular range, receives the reflected light at that time, measures the time difference between the time when the laser is emitted and the time when the reflected light is received, and detects the distance between the host vehicle and the vehicle. Detects the relative distance and direction of targets and road structures (i.e., relative position to the own vehicle). The laser range finder 9 outputs detected data to the database 11. Note that the laser range finder 9 may be a sensor other than LiDAR as long as it detects the relative position of a target around the vehicle with respect to the vehicle; for example, a sensor other than the laser range finder such as a laser radar or a sonic radar may be used. You can.

また、走行軌跡推定装置１は、車両に搭載されたセンサ群（図示せず）に接続されている。例えば、アクセルセンサ、ステアリングセンサ、ブレーキセンサ、車速センサ等に接続されて、これらのセンサ群から出力されるセンサ値を取得することができる。 Further, the travel trajectory estimating device 1 is connected to a sensor group (not shown) mounted on the vehicle. For example, it can be connected to an accelerator sensor, a steering sensor, a brake sensor, a vehicle speed sensor, etc., and obtain sensor values output from these sensor groups.

データベース１１は、走行データや環境情報、地図情報等を格納している。走行データは、ＧＰＳ受信機３で検出された車両の位置情報と、ＩＭＵ５で検出された車両の角度や加速度等と、ＩＭＵ５で算出されたオドメトリを含んでいる。環境情報は、カメラ７とレーザレンジファインダー９で検出された物標の位置や道路構造等の情報を含んでいる。地図情報は、カーナビゲーション装置等に記憶されている情報であり、物標情報や施設情報などの経路案内に必要となる各種データを含んでいる。さらに、地図情報は、高精度な地図情報であり、道路や歩道に設けられた信号機や電柱などの立体物、あるいは車線境界線等の物標の地図上の位置情報や道路の車線数や道路境界線、停止線等の道路構造情報を含んでいる。データベース１１は、走行軌跡推定装置１の要求に応じて、格納している情報を走行軌跡推定装置１へ出力する。また、データベース１１は、走行軌跡の推定に必要となるその他の情報も記憶している。 The database 11 stores driving data, environmental information, map information, and the like. The driving data includes vehicle position information detected by the GPS receiver 3, vehicle angle and acceleration detected by the IMU 5, and odometry calculated by the IMU 5. The environmental information includes information such as the positions of targets detected by the camera 7 and the laser range finder 9, and the road structure. Map information is information stored in a car navigation device or the like, and includes various data necessary for route guidance such as target information and facility information. Furthermore, the map information is highly accurate map information, including the position information on the map of three-dimensional objects such as traffic lights and telephone poles installed on roads and sidewalks, and landmarks such as lane boundaries, the number of lanes on roads, and road information. Contains road structure information such as boundary lines and stop lines. The database 11 outputs the stored information to the travel trajectory estimation device 1 in response to a request from the travel trajectory estimation device 1 . The database 11 also stores other information necessary for estimating the travel trajectory.

尚、データベース１１は、サーバから定期的に最新の地図情報を入手して、保有する地図情報を更新する。本実施形態では、地図情報として第一地図（高精度地図、車両のルート案内に用いる地図）を備えており、第一地図を補足するものとして、第二地図を備えていてもよい。この第二地図は、第一地図だけでは不完全な場合や、より詳細な地図情報が必要な場合に、車両や外部のサーバで作成される。 Note that the database 11 periodically obtains the latest map information from the server and updates the map information it holds. In this embodiment, a first map (a high-precision map, a map used for vehicle route guidance) is provided as map information, and a second map may be provided to supplement the first map. This second map is created by the vehicle or an external server when the first map alone is insufficient or when more detailed map information is required.

走行軌跡推定装置１は、車両が走行した走行軌跡を推定する。走行軌跡推定装置１は、車両が事前に走行して検出しておいた走行データをデータベース１１から取得し、取得した走行データを用いて、過去に車両が走行した走行軌跡を推定する。推定された走行軌跡は、地図データを更新するために使用される。例えば、推定された走行軌跡を用いて物標の位置を計算し、この計算結果を用いて地図データに記録された物標の位置を更新する。 The travel trajectory estimating device 1 estimates the travel trajectory traveled by the vehicle. The travel trajectory estimating device 1 acquires from the database 11 travel data detected by the vehicle traveling in advance, and uses the acquired travel data to estimate the travel trajectory the vehicle has traveled in the past. The estimated travel trajectory is used to update map data. For example, the position of the target is calculated using the estimated travel trajectory, and the position of the target recorded in the map data is updated using the calculation result.

また、走行軌跡推定装置１は、ＧＰＳ受信機３、ＩＭＵ５、カメラ７、レーザレンジファインダー９、データベース１１から取得したデータを処理する制御部を備えており、例えばＩＣ、ＬＳＩ等によって構成される。走行軌跡推定装置１は、これを機能的に捉えた場合、走行軌跡算出部２１と、地図照合部２３と、走行軌跡補正部２５と、走行軌跡調整部２７に分類することができる。 The travel trajectory estimating device 1 also includes a GPS receiver 3, an IMU 5, a camera 7, a laser range finder 9, and a control unit that processes data acquired from a database 11, and is configured by, for example, an IC, an LSI, or the like. The driving trajectory estimating device 1 can be classified into a driving trajectory calculating section 21, a map matching section 23, a driving trajectory correcting section 25, and a driving trajectory adjusting section 27 when viewed functionally.

尚、走行軌跡推定装置１は、マイクロコンピュータ、マイクロプロセッサ、ＣＰＵを含む汎用の電子回路とメモリ等の周辺機器から構成されている。このような走行軌跡推定装置１の各機能は、１または複数の処理回路によって実装することができる。処理回路は、例えば電気回路を含む処理装置等のプログラムされた処理装置を含み、また実施形態に記載された機能を実行するようにアレンジされた特定用途向け集積回路（ＡＳＩＣ）や従来型の回路部品のような装置も含んでいる。 The travel trajectory estimating device 1 is composed of a general-purpose electronic circuit including a microcomputer, a microprocessor, and a CPU, and peripheral devices such as a memory. Each function of the travel trajectory estimating device 1 as described above can be implemented by one or more processing circuits. Processing circuitry may include a programmed processing device, such as a processing device that includes electrical circuitry, and may include an application specific integrated circuit (ASIC) or conventional circuitry arranged to perform the functions described in the embodiments. It also includes equipment such as parts.

走行軌跡算出部２１は、車両が走行したときに検出した走行データをデータベース１１から取得し、取得した走行データに基づいて、車両が走行した走行軌跡を算出する。具体的に、走行軌跡算出部２１は、ＧＰＳ受信機３で検出した車両位置を、単位時間当たりの移動量であるオドメトリの分だけ移動させていくことによって、車両の走行軌跡を算出する。ここで算出される走行軌跡は、時刻毎の車両位置と車両の姿勢との集合によって表される。車両の姿勢とは、車両の角度、すなわち車両の向きである。例えば、走行軌跡は、時刻ｔｉ（ｉ＝１～ｎ）における座標（ｘ、ｙ）と車両の向きθの集合によって表される。 The travel trajectory calculation unit 21 acquires travel data detected when the vehicle travels from the database 11, and calculates the travel trajectory traveled by the vehicle based on the acquired travel data. Specifically, the traveling trajectory calculation unit 21 calculates the traveling trajectory of the vehicle by moving the vehicle position detected by the GPS receiver 3 by the amount of odometry, which is the amount of movement per unit time. The travel trajectory calculated here is represented by a set of vehicle positions and vehicle postures at each time. The attitude of the vehicle is the angle of the vehicle, that is, the orientation of the vehicle. For example, a travel trajectory is represented by a set of coordinates (x, y) and vehicle direction θ at time ti (i=1 to n).

地図照合部２３は、車両が走行したときに検出した車両周辺の環境情報と地図情報をデータベース１１から取得し、取得した車両周辺の環境情報と地図情報に記録された環境情報とを照合して車両の基準位置を検出する。 The map comparison unit 23 acquires from the database 11 environmental information and map information around the vehicle detected when the vehicle is traveling, and collates the acquired environmental information around the vehicle with the environmental information recorded in the map information. Detect the reference position of the vehicle.

具体的に、地図照合部２３は、レーザレンジファインダー９によって検出された車両周辺の物標の車両に対する相対位置であるＬｉＤＡＲデータと、地図情報に点群データとして記録された物標の地図上の位置とをマップマッチングによって照合して、地図上における車両の基準位置を検出する。また、地図情報に記録された白線や縁石等の道路構造を抽出し、車両周辺のＬｉＤＡＲデータからも白線や縁石等の道路構造を抽出して、マップマッチングによって照合し、地図上における車両の基準位置を検出してもよい。地図照合部２３は、ＮＤＴ（Normal Distributions Transform）のような点群データのマッチング手法を用いて基準位置を検出しているので、検出された基準位置は、オドメトリを用いて求められた走行軌跡よりも正確な位置である可能性が高い。そこで、本実施形態では、この基準位置を所定の間隔で検出し、検出された基準位置に近づけるように走行軌跡を補正している。尚、地図照合部２３は、マップマッチングの他にも、カメラ７で検出された物標や道路構造を地図情報と照合して車両の基準位置を検出してもよい。 Specifically, the map comparison unit 23 uses LiDAR data, which is the relative position of targets around the vehicle detected by the laser range finder 9, and LiDAR data, which is the relative position of targets around the vehicle, detected by the laser range finder 9, and LiDAR data, which is the relative position of targets around the vehicle, detected by the laser range finder 9, and LiDAR data, which is the relative position of targets around the vehicle, detected by the laser range finder 9, and LiDAR data, which is the relative position of targets around the vehicle, detected by the laser range finder 9, and LiDAR data, which is the relative position of targets around the vehicle, detected by the laser range finder 9. The reference position of the vehicle on the map is detected by comparing the position with the vehicle using map matching. In addition, road structures such as white lines and curbs recorded in map information are extracted, and road structures such as white lines and curbs are also extracted from LiDAR data around the vehicle, and compared by map matching, the vehicle is referenced on the map. The position may also be detected. The map matching unit 23 detects the reference position using a point cloud data matching method such as NDT (Normal Distributions Transform), so the detected reference position is based on the travel trajectory determined using odometry. is also likely to be the correct location. Therefore, in this embodiment, this reference position is detected at predetermined intervals, and the traveling trajectory is corrected so as to approach the detected reference position. In addition to map matching, the map matching unit 23 may also match targets and road structures detected by the camera 7 with map information to detect the reference position of the vehicle.

こうして基準位置が検出されると、地図照合部２３は、基準位置と走行軌跡との差である車両位置誤差を算出する。例えば、図２に示すように、走行軌跡上において、車両が時刻ｔ１に車両位置Ｘ１に位置し、時刻ｔ２に車両位置Ｘ２に位置し、時刻ｔ３に車両位置Ｘ３に位置している。そして、時刻ｔ１における基準位置Ｙ１と、時刻ｔ３における基準位置Ｙ３がそれぞれ検出されている。このような場合に、地図照合部２３は、基準位置Ｙ１と車両位置Ｘ１の差である車両位置誤差ｄ１を算出する。同様に、基準位置Ｙ３と車両位置Ｘ３の差である車両位置誤差ｄ３を算出する。尚、車両位置誤差ｄ１、ｄ３は、車両位置の差と車両の向き（角度）の差で表される。 When the reference position is detected in this manner, the map comparison unit 23 calculates a vehicle position error, which is the difference between the reference position and the travel trajectory. For example, as shown in FIG. 2, on the travel trajectory, the vehicle is located at vehicle position X1 at time t1, located at vehicle position X2 at time t2, and located at vehicle position X3 at time t3. Then, a reference position Y1 at time t1 and a reference position Y3 at time t3 are respectively detected. In such a case, the map comparison unit 23 calculates a vehicle position error d1, which is the difference between the reference position Y1 and the vehicle position X1. Similarly, a vehicle position error d3, which is the difference between the reference position Y3 and the vehicle position X3, is calculated. Note that the vehicle position errors d1 and d3 are expressed by the difference in vehicle position and the difference in direction (angle) of the vehicle.

地図照合部２３は、所定時間毎または所定距離毎に基準位置を検出している。基準位置を検出する間隔が広すぎると、走行軌跡の推定精度が低下し、間隔が狭すぎると、推定の制約（条件）が多くなって走行軌跡の推定が難しくなる。そこで、所定時間と所定距離の設定方法については、走行軌跡の推定精度を高くすることのできる基準位置の間隔を予め実験やシミュレーションによって求めておき、その間隔となるように所定時間と所定距離を設定すればよい。 The map comparison unit 23 detects the reference position at every predetermined time or every predetermined distance. If the interval at which the reference positions are detected is too wide, the accuracy of estimating the traveling trajectory will decrease, and if the interval is too narrow, the estimation constraints (conditions) will increase, making it difficult to estimate the traveling trajectory. Therefore, regarding the method of setting the predetermined time and predetermined distance, the interval between the reference positions that can increase the accuracy of estimating the traveling trajectory is determined in advance through experiments and simulations, and the predetermined time and predetermined distance are set so that the interval becomes that interval. Just set it.

また、基準位置は、車両が所定の走行状態のときに検出されるようにしてもよい。例えば、所定の走行状態は、車両の停止状態または定速走行状態または直進状態のいずれかの走行状態である。このように車両が停止や定速走行、直進している場合には、マップマッチングによる照合を正確に行うことができるので、基準位置も正確に検出することができる。そこで、地図照合部２３は、このような所定の走行状態のときに基準位置を検出する。 Further, the reference position may be detected when the vehicle is in a predetermined running state. For example, the predetermined running state is a running state in which the vehicle is stopped, running at a constant speed, or going straight. In this way, when the vehicle is stopped, running at a constant speed, or going straight, map matching can be performed accurately, so the reference position can also be accurately detected. Therefore, the map comparison unit 23 detects the reference position during such a predetermined driving state.

さらに、地図照合部２３は、車両の姿勢が変化した後に、車両が定速直進走行に移行した時点で、基準位置を検出してもよい。例えば、車両が交差点を左折した場合には、交差点の前で基準位置を検出し、さらに交差点を左折した後にも基準位置を検出できれば、交差点の前後で基準位置を検出できるので、走行軌跡をより正確に推定することができる。そこで、車両が交差点やカーブを通過したことを検出するために、地図照合部２３は、車両の姿勢が変化したことを検出する。さらに、交差点の途中やカーブの途中で基準位置を検出することがないように、地図照合部２３は、車両が定速直進走行に移行した時点で基準位置を検出する。これにより、交差点やカーブを完全に通過した後に、基準位置を検出することができる。 Furthermore, the map comparison unit 23 may detect the reference position at the time when the vehicle shifts to constant-speed straight-ahead running after the attitude of the vehicle changes. For example, when a vehicle turns left at an intersection, if the reference position can be detected in front of the intersection and also after turning left at the intersection, the reference position can be detected before and after the intersection, making the driving trajectory more accurate. can be estimated accurately. Therefore, in order to detect that the vehicle has passed through an intersection or a curve, the map matching unit 23 detects that the attitude of the vehicle has changed. Furthermore, in order to avoid detecting the reference position in the middle of an intersection or a curve, the map comparison unit 23 detects the reference position at the time when the vehicle shifts to constant-speed straight-ahead running. This allows the reference position to be detected after the vehicle has completely passed through an intersection or curve.

また、地図照合部２３は、車両周辺の環境情報と地図情報に記録された環境情報との一致度が低い車両位置では、基準位置を検出しないようにする。例えば、レーザレンジファインダー９で検出した複数の物標間の相対位置関係と、地図情報に記録された複数の物標の相対位置関係とを照合したときに、その一致度が所定の一致度に満たない場合には、誤検出である可能性が高いので、そのような車両位置では基準位置を検出しないようにする。これにより、正確な環境情報を検出できた車両位置のみで、基準位置を検出するので、走行軌跡の推定精度を向上させることができる。 Furthermore, the map comparison unit 23 does not detect the reference position at a vehicle position where the degree of coincidence between the environmental information around the vehicle and the environmental information recorded in the map information is low. For example, when comparing the relative positional relationship between multiple targets detected by the laser range finder 9 and the relative positional relationship between multiple targets recorded in map information, the degree of agreement will reach a predetermined degree of agreement. If this is not the case, there is a high possibility that the detection is erroneous, so the reference position is not detected at such a vehicle position. Thereby, the reference position is detected only from the vehicle position for which accurate environmental information has been detected, so that the accuracy of estimating the traveling trajectory can be improved.

走行軌跡補正部２５は、走行軌跡算出部２１で算出された走行軌跡を補正する。特に、走行軌跡補正部２５は、走行軌跡上にある車両位置のうち、基準位置が検出された車両位置では、地図照合部２３で算出された車両位置誤差に基づいて走行軌跡を補正する。 The travel trajectory correction section 25 corrects the travel trajectory calculated by the travel trajectory calculation section 21. In particular, the traveling trajectory correction section 25 corrects the traveling trajectory based on the vehicle position error calculated by the map comparison section 23 at the vehicle position where the reference position is detected among the vehicle positions on the traveling trajectory.

例えば、図２に示すように、走行軌跡上にある車両位置のうち、車両位置Ｘ１、Ｘ３では基準位置Ｙ１、Ｙ３が検出されており、車両位置誤差ｄ１、ｄ３が算出されている。そこで、走行軌跡補正部２５は、時刻ｔ１では、車両位置Ｘ１を車両位置誤差ｄ１だけ移動させる。その結果、図３に示すように、車両位置Ｘ１は基準位置Ｙ１の位置に移動する。同様に、車両位置Ｘ３も車両位置誤差ｄ３だけ移動させて基準位置Ｙ３の位置に移動する。 For example, as shown in FIG. 2, among vehicle positions on the travel trajectory, reference positions Y1 and Y3 are detected at vehicle positions X1 and X3, and vehicle position errors d1 and d3 are calculated. Therefore, at time t1, the travel trajectory correction unit 25 moves the vehicle position X1 by the vehicle position error d1. As a result, as shown in FIG. 3, the vehicle position X1 moves to the reference position Y1. Similarly, the vehicle position X3 is also moved by the vehicle position error d3 to the reference position Y3.

一方、走行軌跡補正部２５は、基準位置が検出されなかった車両位置では、その車両位置の前後に隣接する車両位置に基づいて走行軌跡を補正する。例えば、図２に示すように、時刻ｔ２の車両位置Ｘ２では基準位置が検出されていない。基準位置は、上述したように、所定時間毎または所定距離毎に検出されており、車両が所定の走行状態のときに検出される場合もある。そのため、走行軌跡上にある車両位置のすべてで基準位置が検出されるわけではない。そこで、車両位置Ｘ２のように基準位置が検出されなかった車両位置では、走行軌跡補正部２５は、車両位置Ｘ２の前後に隣接する車両位置Ｘ１、Ｘ３に基づいて走行軌跡を補正する。 On the other hand, at vehicle positions where no reference position has been detected, the traveling trajectory correction unit 25 corrects the traveling trajectory based on vehicle positions adjacent before and after the vehicle position. For example, as shown in FIG. 2, the reference position is not detected at vehicle position X2 at time t2. As described above, the reference position is detected at predetermined time intervals or predetermined distance intervals, and may also be detected when the vehicle is in a predetermined running state. Therefore, the reference position is not detected at all vehicle positions on the travel trajectory. Therefore, at a vehicle position where no reference position has been detected, such as vehicle position X2, the traveling trajectory correction unit 25 corrects the traveling trajectory based on vehicle positions X1 and X3 adjacent before and after vehicle position X2.

具体的に、走行軌跡補正部２５は、前後に隣接する車両位置の間の移動量を示す相対変位と車両位置におけるオドメトリとの差である変位誤差を算出し、この変位誤差に基づいて走行軌跡を補正する。 Specifically, the traveling trajectory correction unit 25 calculates a displacement error that is the difference between the relative displacement indicating the amount of movement between the front and rear adjacent vehicle positions and the odometry at the vehicle position, and adjusts the traveling trajectory based on this displacement error. Correct.

例えば、図３に示すように、車両位置Ｘ３は車両位置誤差ｄ３だけ移動しているので、車両位置Ｘ２におけるオドメトリＢ２の先端の位置からずれている。この場合に、走行軌跡補正部２５は、前後に隣接する車両位置Ｘ２とＸ３の間の移動量を示す相対変位Ａ２を算出し、この相対変位Ａ２とオドメトリＢ２との差である変位誤差Ｃ２を算出する。相対変位とオドメトリは車両位置と車両の向き（角度）で表されるので、変位誤差は車両位置の差と車両の向き（角度）の差で表される。 For example, as shown in FIG. 3, the vehicle position X3 has moved by the vehicle position error d3, and is therefore deviated from the position of the tip of the odometry B2 at the vehicle position X2. In this case, the travel trajectory correction unit 25 calculates a relative displacement A2 indicating the amount of movement between the front and rear adjacent vehicle positions X2 and X3, and calculates a displacement error C2 that is the difference between this relative displacement A2 and the odometry B2. calculate. Since the relative displacement and odometry are expressed by the vehicle position and the vehicle orientation (angle), the displacement error is expressed by the difference between the vehicle position and the vehicle orientation (angle).

そして、走行軌跡補正部２５は、車両位置Ｘ３の位置に近づけるように、変位誤差Ｃ２の分だけ車両位置Ｘ２を移動させる。これにより、車両位置Ｘ２は、図４に示すように車両位置Ｘ３と並ぶような位置に移動する。車両位置Ｘ３はすでに基準位置Ｙ３の位置に移動されているので、正確な位置である可能性が高い。したがって、車両位置Ｘ３の位置に近づけるように車両位置Ｘ２を移動させれば、車両位置Ｘ２も正確な位置に補正することが可能となる。 Then, the travel trajectory correction unit 25 moves the vehicle position X2 by the displacement error C2 so as to approach the vehicle position X3. As a result, the vehicle position X2 moves to a position aligned with the vehicle position X3, as shown in FIG. Since the vehicle position X3 has already been moved to the reference position Y3, it is likely to be an accurate position. Therefore, if the vehicle position X2 is moved closer to the vehicle position X3, the vehicle position X2 can also be corrected to an accurate position.

同様に、走行軌跡補正部２５は、図３に示すように、前後に隣接する車両位置Ｘ１とＸ２の間の移動量を示す相対変位Ａ１を算出し、この相対変位Ａ１と車両位置Ｘ１におけるオドメトリＢ１との差である変位誤差Ｃ１を算出する。 Similarly, as shown in FIG. 3, the traveling trajectory correction unit 25 calculates a relative displacement A1 indicating the amount of movement between the front and rear adjacent vehicle positions X1 and X2, and calculates the relative displacement A1 and the odometry at the vehicle position X1. A displacement error C1, which is the difference from B1, is calculated.

そして、走行軌跡補正部２５は、車両位置Ｘ１の位置に近づけるように、変位誤差Ｃ１の分だけ車両位置Ｘ２を移動させる。これにより、車両位置Ｘ２は、図４に示すように車両位置Ｘ１と並ぶような位置に移動する。車両位置Ｘ１はすでに基準位置Ｙ１の位置に移動されているので、正確な位置である可能性が高い。したがって、車両位置Ｘ１の位置に近づけるように車両位置Ｘ２を移動させれば、車両位置Ｘ２も正確な位置に補正することが可能となる。 Then, the travel trajectory correction unit 25 moves the vehicle position X2 by the displacement error C1 so as to approach the vehicle position X1. As a result, the vehicle position X2 moves to a position aligned with the vehicle position X1, as shown in FIG. Since the vehicle position X1 has already been moved to the reference position Y1, it is likely to be an accurate position. Therefore, if the vehicle position X2 is moved closer to the vehicle position X1, the vehicle position X2 can also be corrected to an accurate position.

ただし、車両位置Ｘ１、Ｘ３のように基準位置が検出されている場合には、車両位置Ｘ１、Ｘ３は車両位置誤差ｄ１、ｄ３だけ移動しているので、変異誤差Ｃ１、Ｃ２と車両位置誤差ｄ１、ｄ３は等しくなる。図３では、車両位置Ｘ２の前後に隣接する車両位置Ｘ１、Ｘ３は、いずれも基準位置が検出されているが、実際には、基準位置が検出された車両位置の間には、基準位置の検出されていない車両位置が多く存在している。 However, when the reference position is detected like vehicle positions X1 and X3, vehicle positions X1 and X3 have moved by vehicle position errors d1 and d3, so the variation errors C1 and C2 and the vehicle position error d1 , d3 are equal. In FIG. 3, reference positions are detected for vehicle positions X1 and X3 adjacent to the front and back of vehicle position X2, but in reality, there are There are many vehicle positions that are not detected.

そこで、実際の走行軌跡の補正では、基準位置が検出された車両位置の近くにある車両位置から順に、変異誤差を算出して車両位置を移動させていき、最終的に基準位置が検出された車両位置の間にあるすべての車両位置を移動させて走行軌跡の補正を完了する。このように、走行軌跡補正部２５は、基準位置が検出された車両位置をまず移動させ、その後に基準位置が検出された車両位置の間にある車両位置を順番に移動させていくことによって、走行軌跡を補正する。 Therefore, in correcting the actual driving trajectory, the mutation error is calculated and the vehicle position is moved in order from the vehicle position closest to the vehicle position where the reference position was detected, and finally the reference position is detected. All vehicle positions between the two vehicle positions are moved to complete the correction of the travel trajectory. In this way, the traveling trajectory correction unit 25 first moves the vehicle position where the reference position has been detected, and then sequentially moves the vehicle positions between the vehicle positions where the reference position has been detected. Correct the driving trajectory.

走行軌跡調整部２７は、走行軌跡補正部２５で補正された走行軌跡の誤差が小さくなるように調整し、調整された走行軌跡を、走行軌跡の推定結果として出力する。具体的に、走行軌跡調整部２７は、地図照合部２３によって算出された車両位置誤差と、走行軌跡補正部２５によって算出された変位誤差をすべて加算して誤差総和を算出し、この誤差総和が小さくなるように走行軌跡を調整する。 The travel trajectory adjustment unit 27 adjusts the travel trajectory corrected by the travel trajectory correction unit 25 so that the error in the travel trajectory is reduced, and outputs the adjusted travel trajectory as an estimation result of the travel trajectory. Specifically, the traveling trajectory adjusting section 27 calculates the total error by adding all the vehicle position errors calculated by the map matching section 23 and the displacement errors calculated by the traveling trajectory correcting section 25, and calculates the total error. Adjust the travel trajectory so that it becomes smaller.

例えば、走行軌跡調整部２７は、ガウスニュートン法やＬＭ法等の最適化手法を使用して、車両位置Ｘｉ（ｉ＝１～ｎ）を移動させながら繰り返し計算の中で徐々に誤差総和が小さくなるように走行軌跡を調整する。そして、誤差総和が予め設定された所定値以下になると、走行軌跡調整部２７は、走行軌跡の調整が収束したと判定して、調整された走行軌跡を、走行軌跡の推定結果として出力する。 For example, the travel trajectory adjustment unit 27 uses an optimization method such as the Gauss-Newton method or the LM method to gradually reduce the total error through repeated calculations while moving the vehicle position Xi (i=1 to n). Adjust the travel trajectory so that Then, when the total error becomes equal to or less than a predetermined value set in advance, the traveling trajectory adjustment unit 27 determines that the adjustment of the traveling trajectory has converged, and outputs the adjusted traveling trajectory as the estimation result of the traveling trajectory.

［走行軌跡の推定処理］
次に、図５を参照して、本実施形態に係る走行軌跡推定装置１による走行軌跡の推定処理を説明する。図５は、走行軌跡の推定処理を示すフローチャートである。 [Travel trajectory estimation processing]
Next, with reference to FIG. 5, a process for estimating a travel trajectory by the travel trajectory estimation device 1 according to the present embodiment will be described. FIG. 5 is a flowchart showing the process of estimating the travel trajectory.

図５に示すように、ステップＳ１において、走行軌跡算出部２１は、車両が走行したときに検出した走行データをデータベース１１から取得する。また、地図照合部２３は、車両が走行したときに検出した環境情報と地図情報をデータベース１１から取得する。 As shown in FIG. 5, in step S1, the travel trajectory calculation unit 21 acquires travel data detected when the vehicle travels from the database 11. Furthermore, the map comparison unit 23 acquires from the database 11 environmental information and map information detected when the vehicle is traveling.

ステップＳ３において、走行軌跡算出部２１は、ステップＳ１で取得した走行データに基づいて、車両が走行した走行軌跡を算出する。ここで算出される走行軌跡は、車両が過去に走行した走行軌跡であり、車両位置をオドメトリの分だけ移動させていくことによって走行軌跡を算出する。走行軌跡は、時刻毎の車両位置と車両の姿勢との集合によって表され、具体的に時刻ｔｉ（ｉ＝１～ｎ）における座標（ｘ、ｙ）と車両の向きθの集合によって表される。例えば、図２に示すように、走行軌跡は、時刻ｔ１の車両位置Ｘ１、時刻ｔ２の車両位置Ｘ２、時刻ｔ３の車両位置Ｘ３の集合によって表されている。ただし、車両位置Ｘｎには、車両の向きθｎが含まれている。 In step S3, the travel trajectory calculation unit 21 calculates the travel trajectory traveled by the vehicle based on the travel data acquired in step S1. The travel trajectory calculated here is the travel trajectory that the vehicle has traveled in the past, and the travel trajectory is calculated by moving the vehicle position by the amount of odometry. The travel trajectory is represented by a set of vehicle positions and vehicle postures at each time, and specifically represented by a set of coordinates (x, y) and vehicle orientation θ at time ti (i = 1 to n). . For example, as shown in FIG. 2, the travel trajectory is represented by a set of vehicle position X1 at time t1, vehicle position X2 at time t2, and vehicle position X3 at time t3. However, the vehicle position Xn includes the vehicle direction θn.

ステップＳ５において、地図照合部２３は、車両周辺の環境情報と地図情報に記録された環境情報とを照合して車両の基準位置を検出する。具体的に、地図照合部２３は、レーザレンジファインダー９によって検出された車両周辺のＬｉＤＡＲデータである車両周辺の物標の自車両に対する相対位置と、地図情報に記録された点群データである地図上の物標の位置とをマップマッチングによって照合して、地図上における車両の基準位置を検出している。 In step S5, the map comparison unit 23 detects the reference position of the vehicle by comparing the environmental information around the vehicle with the environmental information recorded in the map information. Specifically, the map comparison unit 23 calculates the relative positions of targets around the vehicle relative to the host vehicle, which is LiDAR data around the vehicle detected by the laser range finder 9, and the map, which is point cloud data recorded in the map information. The reference position of the vehicle on the map is detected by matching the position of the target on the map using map matching.

ステップＳ７において、地図照合部２３は、走行軌跡上にある車両位置の近傍に基準位置が検出されているか否かを判定し、基準位置が検出されている場合にはステップＳ９に進み、基準位置が検出されていない場合にはステップＳ１３に進む。例えば、図２に示すように、車両位置Ｘ１、Ｘ３では基準位置Ｙ１、Ｙ３が検出されているのでステップＳ９に進み、車両位置Ｘ２では基準位置が検出されていないのでステップＳ１３に進む。 In step S7, the map matching unit 23 determines whether or not a reference position has been detected near the vehicle position on the travel trajectory. If the reference position has been detected, the process proceeds to step S9, and If not detected, the process advances to step S13. For example, as shown in FIG. 2, since reference positions Y1 and Y3 have been detected at vehicle positions X1 and X3, the process proceeds to step S9, and since no reference position has been detected at vehicle position X2, the process proceeds to step S13.

地図照合部２３は、まず時刻ｔ１の車両位置Ｘ１の近傍に基準位置が検出されているか否かを判断してステップＳ９～１３の処理を行い、その後は順番に車両位置Ｘ２から車両位置ＸｎまでステップＳ９～Ｓ１３の処理を行う。 The map matching unit 23 first determines whether a reference position is detected in the vicinity of the vehicle position Processing of steps S9 to S13 is performed.

ステップＳ９において、地図照合部２３は、基準位置と走行軌跡上の車両位置との差である車両位置誤差を算出する。例えば、図２の車両位置Ｘ１では、基準位置Ｙ１と車両位置Ｘ１の差ｄ１を車両位置誤差として出力する。同様に、時刻ｔ３では、基準位置Ｙ３と車両位置Ｘ３の差ｄ３を車両位置誤差として出力する。 In step S9, the map comparison unit 23 calculates a vehicle position error, which is the difference between the reference position and the vehicle position on the travel trajectory. For example, at the vehicle position X1 in FIG. 2, the difference d1 between the reference position Y1 and the vehicle position X1 is output as the vehicle position error. Similarly, at time t3, the difference d3 between the reference position Y3 and the vehicle position X3 is output as a vehicle position error.

ステップＳ１１において、走行軌跡補正部２５は、基準位置が検出された車両位置について、ステップＳ９で算出された車両位置誤差に基づいて走行軌跡を補正する。例えば、図２に示すように、走行軌跡補正部２５は、基準位置Ｙ１、Ｙ３が検出された車両位置Ｘ１、Ｘ３について、車両位置誤差ｄ１、ｄ３だけ車両位置Ｘ１、Ｘ３を移動させて走行軌跡を補正する。 In step S11, the travel trajectory correction unit 25 corrects the travel trajectory for the vehicle position where the reference position was detected based on the vehicle position error calculated in step S9. For example, as shown in FIG. 2, the traveling trajectory correction unit 25 moves the vehicle positions X1, X3 by vehicle position errors d1, d3 with respect to the vehicle positions X1, X3 where the reference positions Y1, Y3 are detected, and adjusts the traveling trajectory. Correct.

ステップＳ１３において、地図照合部２３は、走行軌跡上のすべての車両位置についてステップＳ７～１３の処理が行われたか否かを判定する。そして、ステップＳ７～１３の処理が行われていない車両位置がある場合にはステップＳ７に戻り、すべての車両位置についてステップＳ７～１３の処理が行われている場合にはステップＳ１５に進む。ステップＳ１５に進む場合には、基準位置が検出されたＸ１、Ｘ３のような車両位置では、図３に示すように車両位置がすべて移動されており、基準位置が検出されなかったＸ２のような車両位置では図３に示すように車両位置が移動されていないままとなっている。 In step S13, the map comparison unit 23 determines whether the processes of steps S7 to S13 have been performed for all vehicle positions on the travel trajectory. If there is a vehicle position that has not been processed in steps S7-13, the process returns to step S7, and if all vehicle positions have been processed in steps S7-13, the process proceeds to step S15. When proceeding to step S15, the vehicle positions such as X1 and X3 where the reference position has been detected have all been moved as shown in FIG. As shown in FIG. 3, the vehicle position remains unchanged.

ステップＳ１５において、走行軌跡補正部２５は、基準位置が検出されなかった車両位置について、前後に隣接する車両位置の間の移動量を示す相対変位と車両位置におけるオドメトリとの差である変位誤差を算出する。例えば、図３に示すように、基準位置が検出されなかった車両位置Ｘ２について、前後に隣接する車両位置Ｘ２とＸ３の間の移動量を示す相対変位Ａ２を算出し、この相対変位Ａ２と車両位置Ｘ２におけるオドメトリＢ２との差である変位誤差Ｃ２を算出する。同様に、走行軌跡補正部２５は、変位誤差Ｃ１を算出する。 In step S15, the travel trajectory correction unit 25 calculates a displacement error, which is the difference between the relative displacement indicating the amount of movement between the front and rear adjacent vehicle positions, and the odometry at the vehicle position for vehicle positions where no reference position has been detected. calculate. For example, as shown in FIG. 3, for a vehicle position X2 where no reference position has been detected, a relative displacement A2 indicating the amount of movement between the front and rear adjacent vehicle positions X2 and A displacement error C2, which is the difference from the odometry B2 at the position X2, is calculated. Similarly, the travel trajectory correction unit 25 calculates the displacement error C1.

ステップＳ１７において、走行軌跡補正部２５は、基準位置が検出されなかった車両位置について、ステップＳ１５で算出された変位誤差に基づいて走行軌跡を補正する。走行軌跡補正部２５は、図３に示すように、基準位置が検出されなかった車両位置Ｘ２について、変位誤差Ｃ１、Ｃ２だけ車両位置Ｘ２を移動させることで、図４に示すように走行軌跡を補正する。尚、前後に隣接する車両位置で基準位置が検出されている場合には、変位誤差は車両位置誤差と等しくなる。 In step S17, the travel trajectory correction unit 25 corrects the travel trajectory for vehicle positions where no reference position has been detected, based on the displacement error calculated in step S15. As shown in FIG. 3, the traveling trajectory correction unit 25 corrects the traveling trajectory as shown in FIG. 4 by moving the vehicle position X2 by the displacement errors C1 and C2 with respect to the vehicle position to correct. Note that if the reference position is detected at a vehicle position adjacent to the front and back, the displacement error will be equal to the vehicle position error.

ステップＳ１９において、走行軌跡補正部２５は、基準位置が検出されなかった車両位置について、車両位置の補正が終了したか否かを判定する。図３の車両位置Ｘ２のように基準位置が検出されなかったすべての車両位置について、走行軌跡補正部２５は、車両位置の補正が終了したか否かを判定する。そして、車両位置の補正が終了していない場合にはステップＳ１５に戻り、終了している場合にはステップＳ２１に進む。 In step S19, the traveling trajectory correction unit 25 determines whether the correction of the vehicle position has been completed for the vehicle position where the reference position has not been detected. For all vehicle positions for which no reference position has been detected, such as vehicle position X2 in FIG. 3, the traveling trajectory correction unit 25 determines whether or not the correction of the vehicle position has been completed. If the vehicle position correction has not been completed, the process returns to step S15, and if it has been completed, the process proceeds to step S21.

ステップＳ２１において、走行軌跡調整部２７は、ステップＳ９で算出された車両位置誤差と、ステップＳ１５で算出された変位誤差をすべて加算して誤差総和を算出する。この誤差総和は、車両位置Ｘ１から車両位置Ｘｎまでのすべての車両位置の車両位置誤差と変位誤差を加算したものである。 In step S21, the traveling trajectory adjustment unit 27 calculates the total error by adding all the vehicle position errors calculated in step S9 and the displacement errors calculated in step S15. This total error is the sum of vehicle position errors and displacement errors for all vehicle positions from vehicle position X1 to vehicle position Xn.

ステップＳ２３において、走行軌跡調整部２７は、ステップＳ２１で算出された誤差総和が小さくなるように走行軌跡を調整し、収束したか否かを判定する。走行軌跡調整部２７は、ガウスニュートン法やＬＭ法等の最適化手法を使用して、車両位置Ｘｉ（ｉ＝１～ｎ）を移動させながら繰り返し計算の中で徐々に誤差総和が小さくなるように走行軌跡を調整する。そして、誤差総和が予め設定された所定値以下とならない場合には走行軌跡の調整が収束しないと判定してステップＳ３に戻る。一方、誤差総和が予め設定された所定値以下になると、走行軌跡の調整が収束したと判定し、調整された走行軌跡を、走行軌跡の推定結果として出力して本実施形態に係る走行軌跡の推定処理を終了する。 In step S23, the travel trajectory adjustment unit 27 adjusts the travel trajectory so that the total error calculated in step S21 becomes small, and determines whether or not it has converged. The travel trajectory adjustment unit 27 uses an optimization method such as the Gauss-Newton method or the LM method to gradually reduce the total error through repeated calculations while moving the vehicle position Xi (i=1 to n). Adjust the travel trajectory accordingly. If the total error does not become equal to or less than a predetermined value, it is determined that the travel trajectory adjustment has not converged, and the process returns to step S3. On the other hand, when the total error becomes equal to or less than a predetermined value, it is determined that the adjustment of the traveling trajectory has converged, and the adjusted traveling trajectory is output as the estimation result of the traveling trajectory, and the traveling trajectory according to the present embodiment is determined to be converged. End the estimation process.

［実施形態の効果］
以上詳細に説明したように、本実施形態に係る走行軌跡推定装置１では、走行データに基づいて車両が走行した走行軌跡を算出し、車両周辺の環境情報と地図情報に記録された環境情報とを照合して車両の基準位置を検出する。そして、基準位置と走行軌跡との差である車両位置誤差を算出し、走行軌跡上にある車両位置のうち、基準位置が検出された車両位置では車両位置誤差に基づいて走行軌跡を補正する。一方、基準位置が検出されなかった車両位置では車両位置の前後に隣接する車両位置に基づいて走行軌跡を補正する。これにより、ＧＰＳ信号を安定して取得できない状況であっても、車両周辺の環境情報から基準位置を検出して走行軌跡を補正できるので、車両の走行軌跡を精度よく推定することができる。 [Effects of embodiment]
As described above in detail, the driving trajectory estimating device 1 according to the present embodiment calculates the driving trajectory traveled by the vehicle based on the driving data, and combines the environmental information around the vehicle and the environmental information recorded in the map information. The reference position of the vehicle is detected. Then, a vehicle position error, which is the difference between the reference position and the travel trajectory, is calculated, and among the vehicle positions on the travel trajectory, the travel trajectory is corrected based on the vehicle position error at vehicle positions where the reference position is detected. On the other hand, at vehicle positions where no reference position has been detected, the traveling trajectory is corrected based on vehicle positions adjacent to the front and back of the vehicle position. As a result, even in a situation where a GPS signal cannot be stably acquired, the reference position can be detected from the environmental information around the vehicle and the driving trajectory can be corrected, so that the driving trajectory of the vehicle can be estimated with high accuracy.

特に、本実施形態に係る走行軌跡推定装置１では、レーザレンジファインダー９で取得したＬｉＤＡＲデータを用いてＮＤＴ（Normal Distributions Transform）のような点群データのマッチング手法で基準位置を検出している。このようにして検出された基準位置は、オドメトリを用いて求められた走行軌跡よりも正確な位置である可能性が高い。したがって、本実施形態に係る走行軌跡推定装置１は、正確な位置に検出された基準位置に基づいて走行軌跡を補正するので、車両の走行軌跡を精度よく推定することができる。 In particular, in the travel trajectory estimation device 1 according to the present embodiment, the reference position is detected using a point group data matching method such as NDT (Normal Distributions Transform) using LiDAR data acquired by the laser range finder 9. The reference position detected in this way is likely to be a more accurate position than the travel trajectory determined using odometry. Therefore, since the driving trajectory estimating device 1 according to the present embodiment corrects the driving trajectory based on the reference position detected at an accurate position, it is possible to accurately estimate the driving trajectory of the vehicle.

また、本実施形態に係る走行軌跡推定装置１では、所定時間毎または所定距離毎に基準位置を検出する。これにより、予め推定精度が高くなるように設定された所定の間隔で基準位置を検出して走行軌跡を補正するので、車両の走行軌跡を精度よく推定することができる。 Furthermore, the traveling trajectory estimating device 1 according to the present embodiment detects a reference position every predetermined time or every predetermined distance. Thereby, the reference position is detected at predetermined intervals that are set in advance so as to increase the estimation accuracy, and the driving trajectory is corrected, so that the driving trajectory of the vehicle can be estimated with high accuracy.

さらに、本実施形態に係る走行軌跡推定装置１では、車両が所定の走行状態のときに基準位置を検出する。これにより、マップマッチングによる照合を正確に行うことができる走行状態のときに基準位置を検出することができる。したがって、正確な位置に検出された基準位置を用いて走行軌跡を補正できるので、車両の走行軌跡を精度よく推定することができる。 Furthermore, the driving trajectory estimating device 1 according to the present embodiment detects the reference position when the vehicle is in a predetermined driving state. Thereby, the reference position can be detected in a driving state in which verification by map matching can be performed accurately. Therefore, since the traveling trajectory can be corrected using the reference position detected at an accurate position, the traveling trajectory of the vehicle can be estimated with high accuracy.

また、本実施形態に係る走行軌跡推定装置１では、所定の走行状態として、車両の停止状態または定速走行状態または直進状態のいずれかのときに基準位置を検出する。これにより、マップマッチングによる照合を正確に行うことができる停止状態、定速走行状態、直進状態のときに基準位置を検出することができる。したがって、正確な位置に検出された基準位置を用いて走行軌跡を補正できるので、車両の走行軌跡を精度よく推定することができる。 Furthermore, in the travel trajectory estimation device 1 according to the present embodiment, the reference position is detected when the vehicle is in a stopped state, a constant speed traveling state, or a straight-ahead state, as the predetermined traveling state. As a result, the reference position can be detected in a stopped state, a constant speed running state, and a straight-ahead state in which verification by map matching can be performed accurately. Therefore, since the traveling trajectory can be corrected using the reference position detected at an accurate position, the traveling trajectory of the vehicle can be estimated with high accuracy.

さらに、本実施形態に係る走行軌跡推定装置１では、車両の姿勢が変化した後に車両が定速直進走行に移行した時点で基準位置を検出する。これにより、カーブや交差点のように車両の姿勢が変化した地点の後で基準位置を検出できるので、走行軌跡をより正確に推定することができる。 Furthermore, in the driving trajectory estimation device 1 according to the present embodiment, the reference position is detected at the time when the vehicle shifts to constant speed straight running after the attitude of the vehicle changes. Thereby, the reference position can be detected after a point where the posture of the vehicle changes, such as at a curve or an intersection, so the traveling trajectory can be estimated more accurately.

また、本実施形態に係る走行軌跡推定装置１では、車両周辺の環境情報と地図情報に記録された環境情報との一致度が低い車両位置では、基準位置を検出していない。これにより、正確な環境情報を検出できた車両位置のみで、基準位置を検出するので、走行軌跡の推定精度を向上させることができる。 Furthermore, in the driving trajectory estimation device 1 according to the present embodiment, the reference position is not detected at a vehicle position where the degree of coincidence between the environmental information around the vehicle and the environmental information recorded in the map information is low. Thereby, the reference position is detected only from the vehicle position for which accurate environmental information has been detected, so that the accuracy of estimating the traveling trajectory can be improved.

さらに、本実施形態に係る走行軌跡推定装置１では、時刻毎の車両位置と車両の姿勢との集合によって、走行軌跡を表している。これにより、走行軌跡を時間毎に正確に表すことができるので、車両の走行軌跡を精度よく推定することができる。 Furthermore, in the driving trajectory estimating device 1 according to the present embodiment, a driving trajectory is represented by a set of vehicle positions and vehicle postures at each time. As a result, the traveling trajectory can be accurately represented for each time, so that the traveling trajectory of the vehicle can be estimated with high accuracy.

また、本実施形態に係る走行軌跡推定装置１では、基準位置が検出されなかった車両位置において、前後に隣接する車両位置の間の移動量を示す相対変位と車両位置におけるオドメトリとの差である変位誤差を算出する。そして、この変位誤差に基づいて走行軌跡を補正する。これにより、基準位置が検出されなかった車両位置でも、前後に隣接する車両位置に基づいて走行軌跡を補正できるので、車両の走行軌跡を精度よく推定することができる。 Furthermore, in the traveling trajectory estimation device 1 according to the present embodiment, at a vehicle position where the reference position is not detected, the difference between the relative displacement indicating the amount of movement between the front and rear adjacent vehicle positions and the odometry at the vehicle position is calculated. Calculate the displacement error. The travel trajectory is then corrected based on this displacement error. Thereby, even at a vehicle position where no reference position has been detected, the traveling trajectory can be corrected based on the vehicle positions adjacent to the front and rear, so that the traveling trajectory of the vehicle can be estimated with high accuracy.

さらに、本実施形態に係る走行軌跡推定装置１では、車両位置誤差と変位誤差をすべて加算した誤差総和を算出し、この誤差総和が小さくなるように、補正された走行軌跡を調整する。これにより、走行軌跡全体の整合性を取ることができるので、推定された走行軌跡の精度を向上させることができる。 Furthermore, the travel trajectory estimating device 1 according to the present embodiment calculates the total error sum of all vehicle position errors and displacement errors, and adjusts the corrected travel trajectory so that this total error sum becomes small. This makes it possible to ensure the consistency of the entire travel trajectory, thereby improving the accuracy of the estimated travel trajectory.

尚、上述の実施形態は本発明の一例である。このため、本発明は、上述の実施形態に限定されることはなく、この実施形態以外の形態であっても、本発明に係る技術的思想を逸脱しない範囲であれば、設計などに応じて種々の変更が可能であることは勿論である。 Note that the above-described embodiment is an example of the present invention. Therefore, the present invention is not limited to the above-described embodiments, and even forms other than the above embodiments may be used according to the design, etc., as long as they do not depart from the technical idea of the present invention. Of course, various modifications are possible.

１走行軌跡推定装置
３ＧＰＳ受信機
５ＩＭＵ（慣性計測装置）
７カメラ
９レーザレンジファインダー
１１データベース
２１走行軌跡算出部
２３地図照合部
２５走行軌跡補正部
２７走行軌跡調整部
１００走行軌跡推定システム 1 Travel trajectory estimation device 3 GPS receiver 5 IMU (inertial measurement unit)
7 Camera 9 Laser range finder 11 Database 21 Travel trajectory calculation section 23 Map matching section 25 Travel trajectory correction section 27 Travel trajectory adjustment section 100 Travel trajectory estimation system

Claims

A travel trajectory estimation method for estimating a travel trajectory traveled by a vehicle, the method comprising:
Obtaining travel data detected when the vehicle travels and environmental information around the vehicle detected when the vehicle travels;
calculating a travel trajectory traveled by the vehicle based on the travel data;
detecting a reference position of the vehicle by comparing environmental information around the vehicle with environmental information recorded in map information;
Calculating a vehicle position error that is the difference between the reference position and the travel trajectory;
Among vehicle positions on the travel trajectory, at a vehicle position where the reference position is detected, the travel trajectory is corrected based on the vehicle position error;
At a vehicle position where the reference position has not been detected, a displacement error is calculated, which is the difference between the relative displacement indicating the amount of movement between the vehicle positions adjacent to the front and back of the vehicle position, and the odometry at the vehicle position, and the displacement error is calculated. A method for estimating a traveling trajectory, comprising correcting the traveling trajectory based on an error .

The traveling trajectory estimation method according to claim 1, wherein the reference position is detected at every predetermined time or every predetermined distance.

3. The traveling trajectory estimation method according to claim 1, wherein the reference position is detected when the vehicle is in a predetermined traveling state.

4. The traveling trajectory estimation method according to claim 3, wherein the predetermined running state is one of a stopped state, a constant speed running state, or a straight-going state of the vehicle.

3. The traveling trajectory estimation method according to claim 1, wherein the reference position is detected at a time when the vehicle shifts to constant-speed straight-ahead traveling after the attitude of the vehicle changes.

6. The reference position according to claim 1, wherein the reference position is not detected at a vehicle position where the degree of coincidence between environmental information around the vehicle and environmental information recorded in the map information is low. Driving trajectory estimation method.

The traveling trajectory estimation method according to any one of claims 1 to 6, wherein the traveling trajectory is represented by a set of vehicle positions and vehicle postures at each time.

8. A total error sum of the vehicle position error and the displacement error is calculated, and the corrected travel trajectory is adjusted so that the total error sum becomes smaller. The traveling trajectory estimation method described in section .

A travel trajectory estimating device comprising a control unit for estimating a travel trajectory traveled by a vehicle,
The control unit includes:
Obtaining travel data detected when the vehicle travels and environmental information around the vehicle detected when the vehicle travels;
calculating a travel trajectory traveled by the vehicle based on the travel data;
detecting a reference position of the vehicle by comparing environmental information around the vehicle with environmental information recorded in map information;
Calculating a vehicle position error that is the difference between the reference position and the travel trajectory;
Among vehicle positions on the travel trajectory, at a vehicle position where the reference position is detected, the travel trajectory is corrected based on the vehicle position error;
At a vehicle position where the reference position has not been detected, a displacement error is calculated, which is the difference between the relative displacement indicating the amount of movement between the vehicle positions adjacent to the front and back of the vehicle position, and the odometry at the vehicle position, and the displacement error is calculated. A travel trajectory estimation device characterized in that the travel trajectory is corrected based on an error .