JP7313325B2 - Self-localization device - Google Patents
Self-localization device Download PDFInfo
- Publication number
- JP7313325B2 JP7313325B2 JP2020180860A JP2020180860A JP7313325B2 JP 7313325 B2 JP7313325 B2 JP 7313325B2 JP 2020180860 A JP2020180860 A JP 2020180860A JP 2020180860 A JP2020180860 A JP 2020180860A JP 7313325 B2 JP7313325 B2 JP 7313325B2
- Authority
- JP
- Japan
- Prior art keywords
- vehicle
- deviation
- map space
- lateral position
- correction
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Description
本発明は、自己位置推定装置に係り、特に、センサから出力される情報に基づいて自車両の位置を推定する自己位置推定装置に関する。 The present invention relates to a self-position estimation device, and more particularly to a self-position estimation device that estimates the position of a vehicle based on information output from sensors.
地図空間上で車両位置を確認しながら走行する車両において、操舵及び加減速制御に利用する位置情報は最も重要な情報の一つである。かかる位置情報とは、車線内における車両の横位置と方位、さらに走路における車両の前後位置が代表的な情報である。 Positional information used for steering and acceleration/deceleration control is one of the most important pieces of information for a vehicle that travels while confirming its position on a map space. Such positional information is typically the lateral position and direction of the vehicle in the lane, and the longitudinal position of the vehicle on the road.
走路に対する車両の横位置情報及び方位情報については、例えば走路の区画線を認識することで容易に計測が可能である一方、走路における車両の前後位置は、建物、道路標識又は看板等のいわゆるランドマーク的な他の情報を利用しないと計測ができない。ランドマーク的な他の情報に依存して計測される車両の前後位置は計測可能な機会が限られるため、計測ができない時間が長引くと、地図空間上で推定された車両の前後位置に誤差が累積していくという問題があった。 The lateral position information and azimuth information of the vehicle with respect to the track can be easily measured by, for example, recognizing the division lines of the track, while the longitudinal position of the vehicle on the track cannot be measured without using other so-called landmark information such as buildings, road signs, and signboards. There are limited opportunities to measure the longitudinal position of a vehicle that depends on other information such as landmarks, so there is a problem that errors accumulate in the longitudinal position of the vehicle estimated on the map space if the measurement is not possible for a long period of time.
特許文献１には、本地図情報の区画線および看板情報を、自己位置を推定するための情報として活用して、自動運転支援のための地図データを作成する自動運転支援システムの発明が開示されている。 Patent Document 1 discloses an invention of an automatic driving support system that creates map data for automatic driving support by utilizing lane markings and signboard information of this map information as information for estimating the self position.
特許文献２には、測位装置および撮像装置から得られた情報を、地図データベースと照合することによって車両の位置を推定する車両位置推定装置の発明が開示されている。 Patent Literature 2 discloses an invention of a vehicle position estimation device that estimates the position of a vehicle by comparing information obtained from a positioning device and an imaging device with a map database.
しかしながら、特許文献１、２に記載の発明は、車両の前後位置を補正する際に道路標識等のランドマークの参照を要するという問題があった。 However, the inventions described in Patent Literatures 1 and 2 have a problem that it is necessary to refer to landmarks such as road signs when correcting the longitudinal position of the vehicle.
本発明は、上記問題に鑑みてなされたものであり、ランドマークとなる外部情報に依存せずに車両の前後位置を補正可能な自己位置推定装置を実現することを目的とする。 SUMMARY OF THE INVENTION The present invention has been made in view of the above problem, and an object of the present invention is to realize a self-position estimation device capable of correcting the longitudinal position of a vehicle without depending on external information that serves as a landmark.
上記目的を達成するために、請求項１に記載の自己位置推定装置は、車両の周辺の情報を解析して走路の中心に対する前記車両の横位置計測偏差を算出する偏差計測部と、前記車両の運動を計測するセンサと、地図情報とを用いて前記車両の地図空間上の位置を推定する偏差予測部と、前記車両の地図空間上の推定位置に含まれる横位置予測偏差を、前記横位置計測偏差を用いて補正して横位置補正偏差を算出する偏差補正部と、旋回の入口及び出口における前記横位置補正偏差に基づいて前記地図空間上に推定した前記車両の前後位置を補正する前後位置補正部と、を含み、前記偏差計測部は、前記車両の周辺の情報を解析して前記走路の中心に対する前記車両の方位角計測偏差をさらに算出し、前記偏差補正部は、前記車両の地図空間上の推定位置に含まれる方位角予測偏差を、前記方位角計測偏差を用いて補正して方位角補正偏差をさらに算出し、前記前後位置補正部は、前記横位置補正偏差の変化率と前記方位角補正偏差との積に基づいて前記車両の地図空間上の前後位置の補正の要否を判定し、前記車両の地図空間上の前後位置の補正を要すると判定した場合に、前記前後位置を補正する。 In order to achieve the above object, the self-position estimation device according to claim 1 comprises: a deviation measuring unit that analyzes information around the vehicle and calculates the lateral position measurement deviation of the vehicle with respect to the center of the track; a sensor that measures the motion of the vehicle; a deviation prediction unit that estimates the position of the vehicle on the map space using map information;at the entrance and exit of the turna front-back position correction unit that corrects the front-back position of the vehicle estimated on the map space based on the lateral position correction deviation.The deviation measuring unit analyzes the information around the vehicle and further calculates the measured azimuth deviation of the vehicle with respect to the center of the road; the deviation correcting unit corrects the predicted azimuth angle deviation included in the estimated position of the vehicle on the map space using the measured azimuth angle deviation to further calculate the azimuth correction deviation; and correcting the longitudinal position of the vehicle on the map space when it is determined that the longitudinal position of the vehicle needs to be corrected..
また、請求項２に記載の自己位置推定装置は、請求項１に記載の自己位置推定装置において、前記前後位置補正部は、前記車両の運動を計測するセンサ、前記地図情報及び撮像装置を用いて取得した前記車両の周囲の画像情報のいずれかを用いて検出した前記走路の曲率が増加する場合に、前記横位置計測偏差を用いて前記横位置予測偏差を前記車両の進行方向に対して右方向に補正する際に前記車両の地図空間上の前後位置を前記進行方向に対して前方に補正し、前記横位置計測偏差を用いて前記横位置予測偏差を前記車両の進行方向に対して左方向に補正する際に前記車両の地図空間上の前後位置を前記進行方向に対して後方に補正する。 In addition, the claim2The self-localization device described in claim12. In the self-position estimation device according to claim 1, the longitudinal position correction unit corrects the longitudinal position of the vehicle on the map space forward with respect to the traveling direction when the curvature of the road detected using any one of the sensor for measuring the motion of the vehicle, the map information, and the image information of the surroundings of the vehicle obtained using an imaging device increases, when correcting the lateral position prediction deviation to the right with respect to the traveling direction of the vehicle using the lateral position measurement deviation, and corrects the lateral position prediction deviation using the lateral position measurement deviation. When correcting leftward with respect to the traveling direction of the vehicle, the front and rear position of the vehicle on the map space is corrected backward with respect to the traveling direction.
また、請求項３に記載の自己位置推定装置は、請求項１に記載の自己位置推定装置において、前記前後位置補正部は、前記車両の運動を計測するセンサ、前記地図情報及び撮像装置を用いて取得した前記車両の周囲の画像情報のいずれかを用いて検出した前記走路の曲率が減少する場合に、前記横位置計測偏差を用いて前記横位置予測偏差を前記車両の進行方向に対して右方向に補正する際に前記車両の地図空間上の前後位置を前記進行方向に対して後方に補正し、前記横位置計測偏差を用いて前記横位置予測偏差を前記車両の進行方向に対して左方向に補正する際に前記車両の地図空間上の前後位置を前記進行方向に対して前方に補正する。 In addition, the claim3The self-localization device described in claim12. In the self-position estimation device according to claim 1, the longitudinal position correction unit corrects the longitudinal position of the vehicle on the map space backward with respect to the traveling direction when the curvature of the road detected using any one of the sensor for measuring the motion of the vehicle, the map information, and the image information of the surroundings of the vehicle acquired using an imaging device decreases, using the lateral position measurement deviation to correct the lateral position prediction deviation to the right with respect to the traveling direction of the vehicle, and corrects the lateral position prediction deviation using the lateral position measurement deviation. When correcting leftward with respect to the traveling direction of the vehicle, the longitudinal position of the vehicle on the map space is corrected forward with respect to the traveling direction.
また、請求項４に記載の自己位置推定装置は、請求項２又は３に記載の自己位置推定装置において、前記前後位置補正部は、補正前の前記車両の地図空間上の前後位置に前記前後位置補正部の１制御周期当たりの前記車両の移動量を加算して前記車両の地図空間上の前後位置を前方に補正し、補正前の前記車両の地図空間上の前後位置から前記移動量を減算して前記車両の地図空間上の前後位置を後方に補正する。 Further, the self-position estimation device according to claim 4 is the self-position estimation device according to claim 2 or 3 , wherein the longitudinal position correction unit corrects the longitudinal position of the vehicle on the map space forward by adding the amount of movement of the vehicle per one control cycle of the longitudinal position correction unit to the longitudinal position of the vehicle on the map space before correction, and corrects the longitudinal position of the vehicle on the map space backward by subtracting the amount of movement from the longitudinal position of the vehicle on the map space before correction.
また、請求項５に記載の自己位置推定装置は、請求項１～４のいずれか１項に記載の自己位置推定装置において、前記横位置補正偏差と、前記方位角補正偏差と、前記前後位置補正部で補正された前後位置とに基づいて自己位置を推定する自己位置推定部をさらに含む。 Further, the self-position estimation device according to claim 5 is the self-position estimation device according to any one of claims 1 to 4 , further comprising a self-position estimation unit for estimating the self-position based on the lateral position correction deviation, the azimuth correction deviation, and the front-back position corrected by the front-back position correction unit.
また、請求項６に記載の自己位置推定装置は、請求項１～５のいずれか１項に記載の自己位置推定装置において、前記偏差計測部は、撮像装置を用いて取得した前記車両の周囲の画像情報、前記車両の周囲に電磁波を照射して取得した前記車両の周囲の情報、及び前記車両の周囲に音波を照射して取得した前記車両の周囲の情報のいずれかを解析して走路の中心に対する前記車両の横位置計測偏差及び方位角計測偏差を算出する。 Further, the self-position estimation device according to claim 6 is the self-position estimation device according to any one of claims 1 to 5 , wherein the deviation measurement unit analyzes any one of image information around the vehicle acquired using an imaging device, information around the vehicle acquired by irradiating electromagnetic waves around the vehicle, and information around the vehicle acquired by irradiating sound waves around the vehicle to calculate the lateral position measurement deviation and the azimuth measurement deviation of the vehicle with respect to the center of the road.
また、請求項７に記載の自己位置推定装置は、請求項１～６のいずれか１項に記載の自己位置推定装置において、前記車両の運動を計測するセンサは、前記車両の速度を検出する車速センサ、前記車両の走行時の車両の挙動を示す角速度及び加速度を検出する慣性計測装置、前記車両の操舵角を検出する操舵角センサ及び衛星からの情報に基づいて前記車両の位置を検出する衛星測位システムのいずれかである。 Further, the self-position estimation device according to claim 7 is the self-position estimation device according to any one of claims 1 to 6 , wherein the sensor that measures the motion of the vehicle is any one of a vehicle speed sensor that detects the speed of the vehicle, an inertial measurement device that detects angular velocity and acceleration that indicate the behavior of the vehicle when the vehicle is traveling, a steering angle sensor that detects the steering angle of the vehicle, and a satellite positioning system that detects the position of the vehicle based on information from satellites.
本発明によれば、旋回時に顕在化する車両２００の前後位置の推定誤差に起因した車両運動の特徴を捉えることにより、ランドマークとなる外部情報に依存せずに車両の前後位置を補正できるという効果を奏する。 According to the present invention, it is possible to correct the longitudinal position of the vehicle without relying on external information serving as a landmark by capturing the characteristics of the vehicle motion caused by the estimation error of the longitudinal position of the vehicle 200 that becomes apparent when turning.
以下、図面を参照して本発明の実施形態を詳細に説明する。図１に示すように、本実施形態に係る自己位置推定装置１０は、後述する演算装置１４の演算に必要なデータ及び演算装置１４による演算結果を記憶する記憶装置１８と、撮像装置２２が取得した画像情報から走路と車両との相対横位置、及び車両の方位角を算出する画像情報処理部２０と、画像情報処理部２０が算出した横位置偏差、方位角偏差及び曲率、車速センサ２４が検出した車両前後速度、ＩＭＵ（Inertial Measurement Unit）２６が検出した車両の方位角の偏差及び加速度、操舵角センサ２８が検出した車両の操舵角、衛星からの情報に基づいて車両の位置を検出する衛星測位システムであるＧＰＳ（Global Positioning System）３０で検出した車両の現在位置及び現在の方位角、ＬＩＤＡＲ（Laser Imaging Detection and Ranging）３２で検出した車両の現在位置及び現在の方位角、並びに地図情報データベース３４に格納された地図情報が各々入力される入力装置１２と、入力装置１２から入力された入力データ及び記憶装置１８に記憶されたデータに基づいて車両の自己位置の推定の演算を行なうコンピュータ等で構成された演算装置１４と、演算装置１４で演算された車両の位置等を表示するＣＲＴ又はＬＣＤ等で構成された表示装置１６と、で構成されている。 Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings. As shown in FIG. 1, the self-position estimation device 10 according to the present embodiment includes a storage device 18 that stores data necessary for calculation by the calculation device 14 and calculation results by the calculation device 14, which will be described later, an image information processing unit 20 that calculates the relative lateral position between the road and the vehicle and the azimuth angle of the vehicle from the image information acquired by the imaging device 22, the lateral position deviation, azimuth angle deviation and curvature calculated by the image information processing unit 20, the vehicle longitudinal speed detected by the vehicle speed sensor 24, and the IMU (Inert Vehicle azimuth angle deviation and acceleration detected by a steering angle sensor 26, vehicle steering angle detected by a steering angle sensor 28, vehicle current position and current azimuth angle detected by a GPS (Global Positioning System) 30, which is a satellite positioning system for detecting the vehicle position based on information from satellites, vehicle current position and current azimuth angle detected by a LIDAR (Laser Imaging Detection and Ranging) 32, and map information stored in a map information database 34. , a computing device 14 configured by a computer or the like that performs computations for estimating the vehicle's own position based on the input data input from the input device 12 and the data stored in the storage device 18, and the display device 16 configured by a CRT, LCD, or the like for displaying the vehicle position computed by the computing device 14.
本実施形態に係る外界センサである撮像装置２２は車載カメラ等であり、一例として、撮影により取得した車両周辺の画像情報を解析して走路の白線等を検出する。又は、画像と地図情報データベース３４に格納された高精度地図とのマッチングから現在位置の座標及び車両の方位角を算出してもよい。ＩＭＵ２６は、走行時の車両の挙動を示す３軸の角速度（ピッチレート、ロールレート、ヨーレート）と３軸の加速度（前後加速度、横加速度、上下加速度）とが検出可能な慣性計測装置である。ＬＩＤＡＲ３２は、一例として、車両周辺に照射したパルス状のレーザ（電磁波）の散乱光から道路の白線等を検出する。 The imaging device 22, which is an external sensor according to the present embodiment, is an in-vehicle camera or the like, and as an example, analyzes image information around the vehicle obtained by photography to detect white lines on the roadway. Alternatively, the coordinates of the current position and the azimuth angle of the vehicle may be calculated by matching the image with the high-precision map stored in the map information database 34 . The IMU 26 is an inertial measurement device capable of detecting 3-axis angular velocity (pitch rate, roll rate, yaw rate) and 3-axis acceleration (longitudinal acceleration, lateral acceleration, vertical acceleration) that indicate the behavior of the vehicle during running. As an example, the LIDAR 32 detects a white line on a road or the like from scattered light of a pulsed laser (electromagnetic waves) irradiated around the vehicle.
図２は、演算装置１４の具体的な構成の一例を示すブロック図である。演算装置１４は、一種のコンピュータであり、ＣＰＵ（Central Processing Unit）１４Ｂ、ＲＯＭ（Read Only Memory）１４Ａ、ＲＡＭ（Random Access Memory）１４Ｃ、及び入出力ポート１４Ｄを備える。 FIG. 2 is a block diagram showing an example of a specific configuration of the computing device 14. As shown in FIG. The computing device 14 is a kind of computer, and includes a CPU (Central Processing Unit) 14B, a ROM (Read Only Memory) 14A, a RAM (Random Access Memory) 14C, and an input/output port 14D.
演算装置１４では、ＣＰＵ１４Ｂ、ＲＯＭ１４Ａ、ＲＡＭ１４Ｃ、及び入出力ポート１４Ｄがアドレスバス、データバス、及び制御バス等の各種バスを介して互いに接続されている。入出力ポート１４Ｄには、各種の入出力機器として、入力装置１２、ハードディスク（ＨＤＤ）等である記憶装置１８、及び表示装置１６等が各々接続されている。 In the arithmetic device 14, the CPU 14B, ROM 14A, RAM 14C, and input/output port 14D are connected to each other via various buses such as an address bus, a data bus, and a control bus. Various input/output devices such as the input device 12, a storage device 18 such as a hard disk (HDD), and a display device 16 are connected to the input/output port 14D.
記憶装置１８には、自己位置推定のための自己位置推定プログラムがインストールされている。本実施形態では、ＣＰＵ１４Ｂが自己位置推定プログラムを実行することにより、自己位置推定を実行する。また、ＣＰＵ１４Ｂは、自己位置推定プログラムによる処理結果を表示装置１６に表示させる。なお、本実施形態の自己位置推定プログラムを演算装置１４にインストールするには、幾つかの方法があるが、例えば、自己位置推定プログラムをセットアッププログラムと共にＣＤ－ＲＯＭやＤＶＤ等に記憶しておき、入出力装置１２であるディスクドライブ等にディスクをセットし、ＣＰＵ１４Ｂに対してセットアッププログラムを実行することにより記憶装置１８に自己位置推定プログラムをインストールする。または、公衆電話回線又はネットワーク５２を介して演算装置１４と接続される他の情報処理機器と通信することで、記憶装置１８に自己位置推定プログラムをインストールするようにしてもよい。 A self-position estimation program for self-position estimation is installed in the storage device 18 . In this embodiment, the CPU 14B executes self-position estimation by executing a self-position estimation program. Further, the CPU 14B causes the display device 16 to display the processing result of the self-position estimation program. There are several methods for installing the self-position estimation program of the present embodiment in the arithmetic device 14. For example, the self-position estimation program is stored in a CD-ROM, DVD, or the like together with the setup program, and the self-position estimation program is installed in the storage device 18 by setting the disk in a disk drive or the like that is the input/output device 12 and executing the setup program in the CPU 14B. Alternatively, the self-position estimation program may be installed in the storage device 18 by communicating with other information processing equipment connected to the computing device 14 via the public telephone line or network 52 .
次に、演算装置１４のＣＰＵ１４Ｂが自己位置推定プログラムを実行することで実現される各種機能について説明する。自己位置推定プログラムは、撮像装置２２が取得した画像情報に基づいて走路の中心に対する車両の横位置偏差ｙsensor及び方位角偏差θsensorを計測する横位置偏差計測機能、車両の運動を計測するセンサを用いて車両の運動を推定し、地図情報データベース３４に格納された地図情報上に推定した車両位置を更新する地図情報を利用した横位置偏差予測機能、更新した車両位置に含まれる横位置偏差ｙpredict及び方位角偏差θpredictを、横位置偏差ｙsensor及び方位角偏差θsensorを用いて各々補正する横位置偏差・方位角偏差補正機能、地図空間上の車両の推定前後位置を補正する前後位置補正機能、及び各々補正された横位置偏差と方位角偏差と前後位置とに基づいて自車位置を算出する自己位置算出機能を備えている。ＣＰＵ１４Ｂがかかる各機能を有する自己位置推定プログラムを実行することで、ＣＰＵ１４Ｂは、図２に示すように、横位置偏差計測部１００、地図情報を利用した横位置偏差予測部１０２、横位置偏差・方位角偏差補正部１０４、前後位置補正部１０６、及び自己位置算出部１１０として機能する。 Next, various functions realized by executing the self-position estimation program by the CPU 14B of the arithmetic device 14 will be described.自己位置推定プログラムは、撮像装置２２が取得した画像情報に基づいて走路の中心に対する車両の横位置偏差ｙ sensor及び方位角偏差θ sensorを計測する横位置偏差計測機能、車両の運動を計測するセンサを用いて車両の運動を推定し、地図情報データベース３４に格納された地図情報上に推定した車両位置を更新する地図情報を利用した横位置偏差予測機能、更新した車両位置に含まれる横位置偏差ｙ predict及び方位角偏差θ predictを、横位置偏差ｙ sensor及び方位角偏差θ sensorを用いて各々補正する横位置偏差・方位角偏差補正機能、地図空間上の車両の推定前後位置を補正する前後位置補正機能、及び各々補正された横位置偏差と方位角偏差と前後位置とに基づいて自車位置を算出する自己位置算出機能を備えている。 When the CPU 14B executes a self-position estimation program having such functions, the CPU 14B functions as a lateral position deviation measurement unit 100, a lateral position deviation prediction unit 102 using map information, a lateral position deviation/azimuth angle deviation correction unit 104, a front-back position correction unit 106, and a self-position calculation unit 110, as shown in FIG.
図３は、本実施形態に係る自己位置推定装置１０の演算装置１４を中心とした機能ブロック図の一例である。本実施形態に係る自己位置推定装置１０は、横位置偏差計測部１００、地図情報を利用した横位置偏差予測部１０２、横位置偏差・方位角偏差補正部１０４、前後位置補正部１０６の主に４つのブロックで構成されている。前後位置補正部１０６で出力される車両２００の補正済み位置（ｘt+Δt、ｙt+Δt）と、補正済み方位角偏差θt+Δtとに基づいて自車位置算出部１１０において自車位置を推定し、推定した自車位置を記憶装置１８に記憶すると共に、地図情報を利用した横位置偏差予測部１０２にフィードバックして、再度の自車位置推定が行われる。 FIG. 3 is an example of a functional block diagram centering on the arithmetic device 14 of the self-position estimation device 10 according to this embodiment. The self-position estimation device 10 according to the present embodiment is mainly composed of four blocks: a lateral position deviation measurement unit 100, a lateral position deviation prediction unit 102 using map information, a lateral position deviation/azimuth angle deviation correction unit 104, and a front-back position correction unit 106. Based on the corrected position (x t + Δt , y t + Δt ) of the vehicle 200 output by the longitudinal position correction unit 106 and the corrected azimuth angle deviation θ t + Δt , the vehicle position is estimated by the vehicle position calculation unit 110, the estimated vehicle position is stored in the storage device 18, and fed back to the lateral position deviation prediction unit 102 using the map information, and the vehicle position is estimated again.
横位置偏差計測部１００は、車載カメラ等の撮像装置２２が取得した車両２００の周辺の画像情報を解析して検出した走路２０２の区画線２０６等に基づいて、図４に示したように、走路２０２の中心２０４に対する車両２００の横位置偏差ｙsensor及び方位角偏差θsensorを計測する。横位置偏差計測部１００は、撮像装置２２ではなくＬＩＤＡＲ３２若しくはレーダ等の車両２００の周囲に電磁波を照射して車両２００の周囲の情報を取得する計測器又は音波を利用したソナー等の計測器でもよいが、いずれにせよ走路２０２の区画線２０６等を認識するための何らかのセンサが必要である。 The lateral position deviation measuring unit 100 measures the lateral position deviation y sensor and the azimuth angle deviation θ sensor of the vehicle 200 with respect to the center 204 of the track 202 as shown in FIG. The lateral position deviation measuring unit 100 may be a measuring instrument such as a LIDAR 32 or a radar that emits electromagnetic waves around the vehicle 200 instead of the imaging device 22 to acquire information around the vehicle 200 or a measuring instrument such as a sonar that uses sound waves.
地図情報を利用した横位置偏差予測部１０２は、車速センサ２４、ＩＭＵ２６、操舵角センサ２８、及びＧＰＳ３０等の車両２００の運動を計測するセンサを用いて車両２００の運動を推定し、地図情報データベース３４に格納された地図情報上に、推定した車両位置を更新する。 A lateral position deviation prediction unit 102 using map information estimates the motion of the vehicle 200 using sensors for measuring the motion of the vehicle 200, such as the vehicle speed sensor 24, the IMU 26, the steering angle sensor 28, and the GPS 30, and updates the estimated vehicle position on the map information stored in the map information database 34.
地図情報を利用した横位置偏差予測部１０２では、図５に示したように、地図上の推定位置（ｘt、ｙt）に対し、車両２００の運動を計測するセンサを用いて車両運動推定を行い、更新された推定車両位置を地図情報と照らし合わせて、走路２０２に対する横位置偏差ｙpredict、方位角偏差θpredict及び横位置偏差ｙpredictと前後位置偏差ｘpredictとで記述された地図上の予測位置（ｘpredict、ｙpredict）を算出する。 As shown in FIG. 5, the lateral position deviation prediction unit 102 using map information performs vehicle motion estimation using a sensor that measures the motion of the vehicle 200 with respect to the estimated position (x t , y t ) on the map, compares the updated estimated vehicle position with the map information, and predicts the predicted position (x predict , y predict ) on the map described by the lateral position deviation y predict , azimuth angle deviation θ predict , lateral position deviation y predict and longitudinal position deviation x predict with respect to the track 202. ).
一般的に車両運動を計測するセンサには誤差があるため、運動の軌跡には誤差が生じる。したがって予測した横位置偏差ｙpredict及び方位角偏差θpredictには誤差が発生していると考えられる。さらに、推定位置としての車両２００の前後方向に係る前後位置偏差ｘpredictにも誤差が生じている。 Since sensors that measure vehicle motion generally have errors, errors occur in the trajectory of motion. Therefore, it is considered that an error occurs in the predicted lateral position deviation y predict and azimuth angle deviation θ predict . Furthermore, an error also occurs in the longitudinal position deviation x predict of the vehicle 200 in the longitudinal direction, which is the estimated position.
横位置偏差・方位角偏差補正部１０４は、地図情報を利用した横位置偏差予測部１０２で予測した横位置偏差ｙpredict及び方位角偏差θpredictを、横位置偏差計測部１００で計測した横位置偏差ｙsensor及び方位角偏差θsensorを用いて各々補正し、横位置補正偏差ｙt+Δt及び方位角補正偏差θt+Δtを算出する。横位置偏差計測部１００で計測した横位置偏差ｙsensor及び方位角偏差θsensorにも撮像装置２２の情報処理の精度に依存する誤差があるので、例えばカルマンフィルタ又はローパスフィルタ等のノイズ除去フィルタを活用する。本実施形態では、ローパスフィルタを用いて、上記の誤差を修正する。横位置偏差ｙpredict、ｙsensorについてのローパスフィルタは、下記の式（１）で表される。
The lateral position deviation/azimuth angle deviation correction unit 104 corrects the lateral position deviation y predict and the azimuth angle deviation θ predict predicted by the lateral position deviation prediction unit 102 using the map information using the lateral position deviation y sensor and the azimuth angle deviation θ sensor measured by the lateral position deviation measurement unit 100, respectively, and calculates the lateral position correction deviation y t + Δt and the azimuth correction deviation θ t + Δt . Since the lateral position deviation y sensor and the azimuth angle deviation θ sensor measured by the lateral position deviation measuring unit 100 also have errors depending on the accuracy of information processing of the imaging device 22, a noise removal filter such as a Kalman filter or a low-pass filter is used. In this embodiment, a low-pass filter is used to correct the above error. A low-pass filter for the lateral position deviations ypredict and ysensor is expressed by Equation (1) below.
式（１）中のτaは時定数であり、ローパスフィルタのカットオフ周波数ｆaを用いて下記の式（２）として定義される。
τa in the equation (1) is a time constant and is defined as the following equation (2) using the cutoff frequency f a of the low-pass filter.
また、横位置偏差の更新量ｙupdateは、下記の式（３）で定義する。
Also, the update amount y update of the lateral position deviation is defined by the following equation (3).
同様に、方位角偏差θpredict、θsensorについても下記の式（４）に示したローパスフィルタを適用する。
Similarly, the low-pass filter shown in Equation (4) below is also applied to the azimuth angle deviations θpredict and θsensor .
式（４）中のτbは時定数であり、ローパスフィルタのカットオフ周波数ｆbを用いて下記の式（５）として定義される。
τ b in the equation (4) is a time constant, and is defined as the following equation (5) using the cutoff frequency f b of the low-pass filter.
前後位置補正部１０６は、旋回の入り口及び出口における横位置補正量を用いて、地図空間上の車両２００の推定前後位置（以下、「前後位置」と略記）を補正する。車両が直進している場合、横位置の変位に前後位置の誤差は影響しないが、車両２００カーブに侵入する等により、車両２００が旋回する際には、横位置の変位に前後位置の誤差の影響が現れる。 The longitudinal position correcting unit 106 corrects the estimated longitudinal position of the vehicle 200 on the map space (hereinafter abbreviated as “longitudinal position”) using the lateral position correction amounts at the entrance and exit of the turn. When the vehicle is traveling straight, the longitudinal position error does not affect the lateral position displacement.
前述のように、横位置偏差・方位角偏差補正部１０４では地図空間上の車両２００の推定位置に係る横位置偏差ｙpredictを、横位置偏差計測部１００で撮像装置２２等を用いて計測した横位置偏差ｙsensorで補正して、横位置偏差の更新量ｙupdateを算出する。 As described above, the lateral position deviation/azimuth angle deviation correction unit 104 corrects the lateral position deviation y predict related to the estimated position of the vehicle 200 on the map space with the lateral position deviation y sensor measured by the lateral position deviation measurement unit 100 using the imaging device 22 or the like, and calculates the update amount y update of the lateral position deviation.
ここで、地図空間上の車両２００の推定位置は、初期状態で走路２０２の中心２０４に位置するとし、車両２００の真の位置は走路２０２の中心から車両２００がコースアウトしない程度に外れているとする。かかる場合では、地図空間上の車両２００の推定位置は、横位置偏差の更新量ｙupdateで補正されるが、車両２００の前後位置に誤差がなければ、横位置偏差の更新量ｙupdateで補正された地図空間上の車両２００の推定位置は適切であると考えられる。しかしながら、車両２００がカーブに侵入する際に、車両２００の前後位置に誤差があると、車両２００の旋回による横位置の変化の影響により、横位置偏差の更新量ｙupdateで補正された地図空間上の車両２００の推定位置は、車両２００の真の位置から外れることが考えられる。車両２００の真の位置に合わせるように車両２００の推定位置を補正しても、車両２００の前後位置の誤差は解消されないからである。本実施形態では、かかる横位置の補正の方向に着目して、車両２００の前後位置を補正するというものである。 Here, it is assumed that the estimated position of the vehicle 200 on the map space is located at the center 204 of the track 202 in the initial state, and the true position of the vehicle 200 is deviated from the center of the track 202 to such an extent that the vehicle 200 does not go off course. In such a case, the estimated position of the vehicle 200 on the map space is corrected by the update amount y update of the lateral position deviation, but if there is no error in the longitudinal position of the vehicle 200, the estimated position of the vehicle 200 on the map space corrected by the update amount y update of the lateral position deviation is considered appropriate. However, if there is an error in the longitudinal position of the vehicle 200 when the vehicle 200 enters the curve, the estimated position of the vehicle 200 on the map space corrected by the update amount y update of the lateral position deviation may deviate from the true position of the vehicle 200 due to the influence of the change in the lateral position due to the turning of the vehicle 200. This is because even if the estimated position of the vehicle 200 is corrected so as to match the true position of the vehicle 200, the error in the longitudinal position of the vehicle 200 cannot be eliminated. In the present embodiment, the longitudinal position of the vehicle 200 is corrected by focusing on the direction of lateral position correction.
以下に地図空間上に推定した車両２００の前後位置の誤差に起因する横位置誤差の発生の態様を説明する。図６（Ａ）は車両２００が左カーブの入り口に侵入する場合の横位置誤差の発生の態様を示した説明図である。図６（Ａ）左は、地図空間上の車両２００の推定位置が車両２００の真の位置の前方にある前方誤差の状態を示している。図６（Ａ）左では車両２００の真の位置が左に外れているので（方位角偏差θ＜０）、地図空間上の車両２００の推定位置は横位置偏差の更新量ｙupdateで左側に横位置補正される。補正された地図空間上の車両２００の推定位置で車両２００を自動運転させると、車両２００の真の位置は位置誤差２１０のように走路２０２の中心２０４から左側に外れてしまう。かかる位置誤差２１０を抑制するには、地図空間上の車両２００の推定位置を真の位置に合わせて後方に補正することを要する。 A mode of occurrence of a lateral position error due to an error in the longitudinal position of the vehicle 200 estimated on the map space will be described below. FIG. 6A is an explanatory diagram showing how a lateral position error occurs when the vehicle 200 enters the entrance of a left curve. The left side of FIG. 6A shows a forward error state in which the estimated position of the vehicle 200 on the map space is ahead of the true position of the vehicle 200 . On the left side of FIG. 6A, the true position of the vehicle 200 deviates to the left (azimuth angle deviation θ<0), so the estimated position of the vehicle 200 on the map space is laterally corrected to the left by the update amount y update of the lateral position deviation. When the vehicle 200 is automatically driven at the corrected estimated position of the vehicle 200 on the map space, the true position of the vehicle 200 deviates to the left from the center 204 of the track 202 as shown by the position error 210 . In order to suppress such a position error 210, it is necessary to correct the estimated position of the vehicle 200 on the map space backward so as to match the true position.
図６（Ａ）右は、地図空間上の車両２００の推定位置が車両２００の真の位置の後方にある後方誤差の状態を示している。図６（Ａ）右では車両２００の真の位置が右に外れているので（方位角偏差θ＞０）、地図空間上の車両２００の推定位置は横位置偏差の更新量ｙupdateで右側に横位置補正される。補正された地図空間上の車両２００の推定位置で車両２００を自動運転させると、車両２００の真の位置は位置誤差２１２のように走路２０２の中心２０４から右側に外れてしまう。かかる位置誤差２１２を抑制するには、地図空間上の車両２００の推定位置を真の位置に合わせて前方に補正することを要する。 The right side of FIG. 6(A) shows a rear error state in which the estimated position of the vehicle 200 on the map space is behind the true position of the vehicle 200 . On the right side of FIG. 6A, the true position of the vehicle 200 deviates to the right (azimuth angle deviation θ>0), so the estimated position of the vehicle 200 on the map space is laterally corrected to the right by the update amount y update of the lateral position deviation. When the vehicle 200 is automatically driven at the corrected estimated position of the vehicle 200 on the map space, the true position of the vehicle 200 deviates to the right from the center 204 of the track 202 as shown by the position error 212 . In order to suppress the position error 212, it is necessary to correct the estimated position of the vehicle 200 on the map space forward so as to match the true position.
図６（Ｂ）は車両２００が左カーブの出口から脱出する場合の横位置誤差の発生の態様を示した説明図である。図６（Ｂ）左は、地図空間上の車両２００の推定位置が車両２００の真の位置の前方にある前方誤差の状態を示している。図６（Ｂ）左では車両２００の真の位置が右に外れているので（方位角偏差θ＞０）、地図空間上の車両２００の推定位置は横位置偏差の更新量ｙupdateで右側に横位置補正される。補正された地図空間上の車両２００の推定位置で車両２００を自動運転させると、車両２００の真の位置は位置誤差２１４のように走路２０２の中心２０４から右側に外れてしまう。かかる位置誤差２１４を抑制するには、地図空間上の車両２００の推定位置を真の位置に合わせて後方に補正することを要する。 FIG. 6B is an explanatory diagram showing how the lateral position error occurs when the vehicle 200 exits from the exit of a left curve. The left side of FIG. 6B shows a forward error state in which the estimated position of the vehicle 200 on the map space is ahead of the true position of the vehicle 200 . On the left side of FIG. 6B, the true position of the vehicle 200 deviates to the right (azimuth angle deviation θ>0), so the estimated position of the vehicle 200 on the map space is laterally corrected to the right by the update amount y update of the lateral position deviation. When the vehicle 200 is automatically driven with the corrected estimated position of the vehicle 200 on the map space, the true position of the vehicle 200 deviates to the right from the center 204 of the track 202 as shown by the position error 214 . In order to suppress the position error 214, it is necessary to correct the estimated position of the vehicle 200 on the map space backward so as to match the true position.
図６（Ｂ）右は、地図空間上の車両２００の推定位置が車両２００の真の位置の後方にある後方誤差の状態を示している。図６（Ｂ）右では車両２００の真の位置が左に外れているので（方位角偏差θ＜０）、地図空間上の車両２００の推定位置は横位置偏差の更新量ｙupdateで左側に横位置補正される。補正された地図空間上の車両２００の推定位置で車両２００を自動運転させると、車両２００の真の位置は位置誤差２１６のように走路２０２の中心２０４から左側に外れてしまう。かかる位置誤差２１６を抑制するには、地図空間上の車両２００の推定位置を真の位置に合わせて前方に補正することを要する。 The right side of FIG. 6(B) shows a rearward error state in which the estimated position of the vehicle 200 on the map space is behind the true position of the vehicle 200 . On the right side of FIG. 6B, the true position of the vehicle 200 deviates to the left (azimuth angle deviation θ<0), so the estimated position of the vehicle 200 on the map space is laterally corrected to the left by the update amount y update of the lateral position deviation. When the vehicle 200 is automatically driven with the corrected estimated position of the vehicle 200 on the map space, the true position of the vehicle 200 deviates to the left from the center 204 of the track 202 as shown by the position error 216 . In order to suppress the position error 216, it is necessary to correct the estimated position of the vehicle 200 on the map space forward so as to match the true position.
図７（Ａ）は車両２００が右カーブの入り口に侵入する場合の横位置誤差の発生の態様を示した説明図である。図７（Ａ）左は、地図空間上の車両２００の推定位置が車両２００の真の位置の前方にある前方誤差の状態を示している。図７（Ａ）左では車両２００の真の位置が右に外れているので（方位角偏差θ＞０）、地図空間上の車両２００の推定位置は横位置偏差の更新量ｙupdateで右側に横位置補正される。補正された地図空間上の車両２００の推定位置で車両２００を自動運転させると、車両２００の真の位置は位置誤差２１８のように走路２０２の中心２０４から右側に外れてしまう。かかる位置誤差２１８を抑制するには、地図空間上の車両２００の推定位置を真の位置に合わせて後方に補正することを要する。 FIG. 7A is an explanatory diagram showing how a lateral position error occurs when the vehicle 200 enters the entrance of a right curve. The left side of FIG. 7A shows a forward error state in which the estimated position of the vehicle 200 on the map space is ahead of the true position of the vehicle 200 . On the left side of FIG. 7A, the true position of the vehicle 200 deviates to the right (azimuth angle deviation θ>0), so the estimated position of the vehicle 200 on the map space is laterally corrected to the right side by the update amount y update of the lateral position deviation. When the vehicle 200 is automatically driven at the corrected estimated position of the vehicle 200 on the map space, the true position of the vehicle 200 deviates to the right from the center 204 of the track 202 as shown by the position error 218 . In order to suppress such a position error 218, it is necessary to correct the estimated position of the vehicle 200 on the map space backward so as to match the true position.
図７（Ａ）右は、地図空間上の車両２００の推定位置が車両２００の真の位置の後方にある後方誤差の状態を示している。図７（Ａ）右では車両２００の真の位置が左に外れているので（方位角偏差θ＜０）、地図空間上の車両２００の推定位置は横位置偏差の更新量ｙupdateで左側に横位置補正される。補正された地図空間上の車両２００の推定位置で車両２００を自動運転させると、車両２００の真の位置は位置誤差２２０のように走路２０２の中心２０４から左側に外れてしまう。かかる位置誤差２２０を抑制するには、地図空間上の車両２００の推定位置を真の位置に合わせて前方に補正することを要する。 The right side of FIG. 7(A) shows a rear error state in which the estimated position of the vehicle 200 on the map space is behind the true position of the vehicle 200 . On the right side of FIG. 7A, the true position of the vehicle 200 deviates to the left (azimuth angle deviation θ<0), so the estimated position of the vehicle 200 on the map space is laterally corrected to the left by the update amount y update of the lateral position deviation. When the vehicle 200 is automatically driven at the corrected estimated position of the vehicle 200 on the map space, the true position of the vehicle 200 deviates to the left from the center 204 of the track 202 as shown by the position error 220 . In order to suppress the position error 220, it is necessary to correct the estimated position of the vehicle 200 on the map space forward so as to match the true position.
図７（Ｂ）は車両２００が右カーブの出口から脱出する場合の横位置誤差の発生の態様を示した説明図である。図７（Ｂ）左は、地図空間上の車両２００の推定位置が車両２００の真の位置の前方にある前方誤差の状態を示している。図７（Ｂ）左では車両２００の真の位置が左に外れているので（方位角偏差θ＜０）、地図空間上の車両２００の推定位置は横位置偏差の更新量ｙupdateで左側に横位置補正される。補正された地図空間上の車両２００の推定位置で車両２００を自動運転させると、車両２００の真の位置は位置誤差２２２のように走路２０２の中心２０４から左側に外れてしまう。かかる位置誤差２２２を抑制するには、地図空間上の車両２００の推定位置を真の位置に合わせて後方に補正することを要する。 FIG. 7B is an explanatory diagram showing how the lateral position error occurs when the vehicle 200 exits from the exit of a right curve. The left side of FIG. 7B shows a forward error state in which the estimated position of the vehicle 200 on the map space is ahead of the true position of the vehicle 200 . Since the true position of the vehicle 200 deviates to the left in FIG. 7B (azimuth angle deviation θ<0), the estimated position of the vehicle 200 on the map space is laterally corrected to the left by the update amount y update of the lateral position deviation. When the vehicle 200 is automatically driven at the corrected estimated position of the vehicle 200 on the map space, the true position of the vehicle 200 deviates to the left from the center 204 of the track 202 as shown by the position error 222 . In order to suppress the position error 222, it is necessary to correct the estimated position of the vehicle 200 on the map space backward so as to match the true position.
図７（Ｂ）右は、地図空間上の車両２００の推定位置が車両２００の真の位置の後方にある後方誤差の状態を示している。図７（Ｂ）右では車両２００の真の位置が右に外れているので（方位角偏差θ＞０）、地図空間上の車両２００の推定位置は横位置偏差の更新量ｙupdateで右側に横位置補正される。補正された地図空間上の車両２００の推定位置で車両２００を自動運転させると、車両２００の真の位置は位置誤差２２４のように走路２０２の中心２０４から右側に外れてしまう。かかる位置誤差２２４を抑制するには、地図空間上の車両２００の推定位置を真の位置に合わせて前方に補正することを要する。 The right side of FIG. 7(B) shows a rearward error state in which the estimated position of the vehicle 200 on the map space is behind the true position of the vehicle 200 . On the right side of FIG. 7B, the true position of the vehicle 200 deviates to the right (azimuth angle deviation θ>0), so the estimated position of the vehicle 200 on the map space is laterally corrected to the right side by the update amount y update of the lateral position deviation. When the vehicle 200 is automatically driven at the corrected estimated position of the vehicle 200 on the map space, the true position of the vehicle 200 deviates to the right from the center 204 of the track 202 as shown by the position error 224 . In order to suppress the position error 224, it is necessary to correct the estimated position of the vehicle 200 on the map space forward so as to match the true position.
図８は、カーブの左右、カーブでの車両２００の位置、及び地図空間上の車両２００の推定位置の横位置補正に対応した前後位置補正の態様を示した概略図である。図８に示した前後位置補正の態様は、図６及び図７を用いて説明した内容を一覧表にまとめたものである。具体的には、（１）左カーブの入り口で左に横位置補正をした場合は、地図空間上の車両２００の推定位置を後方に補正する。（２）左カーブの入り口で右に横位置補正をした場合は、地図空間上の車両２００の推定位置を前方に補正する。（３）左カーブの出口で左に横位置補正をした場合は、地図空間上の車両２００の推定位置を前方に補正する。（４）左カーブの出口で右に横位置補正をした場合は、地図空間上の車両２００の推定位置を後方に補正する。 8A and 8B are schematic diagrams showing aspects of longitudinal position correction corresponding to lateral position correction of the left and right of the curve, the position of the vehicle 200 on the curve, and the estimated position of the vehicle 200 on the map space. The aspect of the front-back position correction shown in FIG. 8 is a list of the contents described with reference to FIGS. 6 and 7 . Specifically, (1) when the lateral position is corrected to the left at the entrance of a left curve, the estimated position of the vehicle 200 on the map space is corrected backward. (2) If the lateral position is corrected to the right at the entrance of the left curve, the estimated position of the vehicle 200 on the map space is corrected forward. (3) When the lateral position is corrected to the left at the exit of the left curve, the estimated position of the vehicle 200 on the map space is corrected forward. (4) When correcting the lateral position to the right at the exit of a left curve, correct the estimated position of the vehicle 200 on the map space backward.
また、（５）右カーブの入り口で左に横位置補正をした場合は、地図空間上の車両２００の推定位置を前方に補正する。（６）右カーブの入り口で右に横位置補正をした場合は、地図空間上の車両２００の推定位置を後方に補正する。（７）右カーブの出口で左に横位置補正をした場合は、地図空間上の車両２００の推定位置を後方に補正する。（８）右カーブの出口で右に横位置補正をした場合は、地図空間上の車両２００の推定位置を前方に補正する。 (5) When the lateral position is corrected to the left at the entrance of the right curve, the estimated position of the vehicle 200 on the map space is corrected forward. (6) When correcting the lateral position to the right at the entrance of a right curve, correct the estimated position of the vehicle 200 on the map space backward. (7) When the lateral position is corrected to the left at the exit of the right curve, the estimated position of the vehicle 200 on the map space is corrected backward. (8) When correcting the lateral position to the right at the exit of a right curve, correct the estimated position of the vehicle 200 forward on the map space.
図８に示したように、右カーブ左カーブ、入り口出口、横位置補正の左右の各々の別を考慮すると、前後位置補正の態様は８通りとなる。しかしながら、車両走行軌跡の曲率（左方向に＋、右方向に－）は、左カーブ入口及び右カーブ出口で増加し、左カーブ出口及び右カーブ入口で減少するので、本実施形態における前後位置補正の態様は図９に示したような４通りとなる。具体的には、（１）横位置偏差ｙsensor又横位置偏差の更新量ｙupdateに基づいて横位置を左に補正かつ曲率増加の場合、前後位置を後方に補正、（２）横位置偏差ｙsensor又横位置偏差の更新量ｙupdateに基づいて横位置を左に補正かつ曲率減少の場合、前後位置を前方に補正、（３）横位置偏差ｙsensor又横位置偏差の更新量ｙupdateに基づいて横位置を右に補正かつ曲率増加の場合、前後位置を前方に補正、（４）横位置偏差ｙsensor又横位置偏差の更新量ｙupdateに基づいて横位置を右に補正かつ曲率減少の場合、前後位置を後方に補正、である。 As shown in FIG. 8, considering the right curve, the left curve, the entrance/exit, and the left and right sides of the lateral position correction, there are eight modes of front-back position correction. However, since the curvature (+ in the left direction and - in the right direction) of the vehicle travel trajectory increases at the left curve entrance and right curve exit and decreases at the left curve exit and right curve entrance, there are four ways of correcting the longitudinal position in this embodiment as shown in FIG.具体的には、（１）横位置偏差ｙ sensor又横位置偏差の更新量ｙ updateに基づいて横位置を左に補正かつ曲率増加の場合、前後位置を後方に補正、（２）横位置偏差ｙ sensor又横位置偏差の更新量ｙ updateに基づいて横位置を左に補正かつ曲率減少の場合、前後位置を前方に補正、（３）横位置偏差ｙ sensor又横位置偏差の更新量ｙ updateに基づいて横位置を右に補正かつ曲率増加の場合、前後位置を前方に補正、（４）横位置偏差ｙ sensor又横位置偏差の更新量ｙ updateに基づいて横位置を右に補正かつ曲率減少の場合、前後位置を後方に補正、である。
本実施形態では、図３に曲率変化率ヨーレート入力部１０８として示したように、前後位置補正部１０６での前後位置の補正に際して、ＩＭＵ２６及び操舵角センサ２８の検出値に基づいて算出されるカーブの曲率により、車両２００の前後位置補正の態様を決定する。 In this embodiment, as shown by the curvature change rate yaw rate input unit 108 in FIG.
次に、前後位置の補正を実施するか否かの判定方法について説明する。上述のように、本実施形態では、カーブの曲率の増減に応じて前後位置をどのように補正するかを整理した。しかしながら、本実施形態では、常に前後位置の補正を実施するのではなく、車両運動の妥当性を評価し、車両２００が車両運動の視点で整合の取れない運動をしたときのみ前後位置の補正を実施することとする。 Next, a method of determining whether or not to correct the front-back position will be described. As described above, in the present embodiment, how the front-back position is corrected according to the increase or decrease in the curvature of the curve is arranged. However, in this embodiment, the validity of the vehicle motion is evaluated, and the longitudinal position is corrected only when the vehicle 200 moves inconsistently from the viewpoint of the vehicle motion, instead of always performing the correction of the longitudinal position.
具体的には、方位角偏差θの正負と、車両２００の進行方向の正負とが各々逆になっている場合に前後位置の補正を行う。図１０は、前後位置の補正を行う場合の態様を示した説明図である。図１０の左では方位角偏差θは負で、車両２００が走路２０２の中心２０４に対して左を向いている。また、車両２００の進行方向を示す横位置偏差ｙの変化量は正で、車両２００の進行方向は走路２０２の中心２０４に対して右を向いている。図１０の右では方位角偏差θは正で、車両２００が走路２０２の中心２０４に対して右を向いている。また、車両２００の進行方向を示す横位置偏差ｙの変化量は負で、車両２００の進行方向は走路２０２の中心２０４に対して左を向いている。図１０に示したような、方位角偏差θと車両２００の進行方向とに不整合が生じた場合に前後位置を補正する。 Specifically, when the sign of the azimuth angle deviation θ and the sign of the traveling direction of the vehicle 200 are opposite to each other, the longitudinal position is corrected. 10A and 10B are explanatory diagrams showing a mode in which the front-rear position is corrected. At the left of FIG. 10, the azimuth deviation θ is negative and the vehicle 200 is pointing left with respect to the center 204 of the track 202 . Also, the amount of change in the lateral position deviation y indicating the direction of travel of the vehicle 200 is positive, and the direction of travel of the vehicle 200 is to the right with respect to the center 204 of the track 202 . On the right side of FIG. 10, the azimuth deviation θ is positive and the vehicle 200 is pointing right with respect to the center 204 of the track 202 . Also, the amount of change in the lateral position deviation y indicating the direction of travel of the vehicle 200 is negative, and the direction of travel of the vehicle 200 is left with respect to the center 204 of the track 202 . When the azimuth angle deviation θ and the traveling direction of the vehicle 200 do not match as shown in FIG. 10, the longitudinal position is corrected.
前後位置補正の要否の判定に際して、本実施形態では、下記の式（６）で示した判定値ｋevalを定義する。
In determining whether or not the front-back position correction is necessary, the present embodiment defines a determination value k eval expressed by the following equation (6).
判定値ｋevalは方位角偏差θと横位置偏差ｙの変化量の各々の符号の不一致を判定するものであり、判定値ｋevalが負になった場合に方位角偏差θと横位置偏差ｙの変化量の各々の符号が不一致であると判定できる。本実施形態では、判定値ｋevalが事前に設定された閾値ｋth以下になった場合に、前後位置の補正を行う。閾値ｋthは、実車等を用いた実験を通じて具体的に決定する。 The judgment value k eval judges whether the signs of the amounts of change in the azimuth angle deviation θ and the lateral position deviation y do not match. When the judgment value k eval becomes negative, it can be determined that the signs of the amounts of change in the azimuth angle deviation θ and the lateral position deviation y do not match. In this embodiment, when the judgment value k eval becomes equal to or less than a preset threshold value k th , the front-back position is corrected. The threshold kth is specifically determined through experiments using an actual vehicle or the like.
図１１は、本実施形態における前後位置補正の方向決定に係る処理の一例を示したフローチャートである。ステップ１００では、図３に示した横位置偏差・方位角偏差補正部１０４及び曲率変化率ヨーレート入力部１０８から方位角偏差、横位置補正量及び曲率変化率の各々を取得する。 FIG. 11 is a flowchart showing an example of processing related to direction determination for front-back position correction in this embodiment. In step 100, the azimuth angle deviation, the lateral position correction amount, and the curvature change rate are obtained from the lateral position deviation/azimuth angle deviation correction unit 104 and the curvature change rate yaw rate input unit 108 shown in FIG.
ステップ１０２では、上述の式（６）を用いて判定値ｋevalを算出する。そして、ステップ１０４では、算出した判定値ｋevalが閾値ｋth以下であるか否かを判定する。ステップ１０４で、判定値ｋevalが閾値ｋth以下の場合は手順をステップ１０６に移行し、判定値ｋevalが閾値ｋth以下でない場合は手順をステップ１００に戻す。 At step 102, the determination value k eval is calculated using the above equation (6). Then, in step 104, it is determined whether or not the calculated determination value k eval is equal to or less than the threshold value k th . At step 104, if the judgment value k eval is less than or equal to the threshold k th , the procedure proceeds to step 106 , and if the judgment value k eval is not less than the threshold k th , the procedure returns to step 100 .
ステップ１０６では、旋回半径が閾値Ｒth以上かつ曲率変化率が正か否かを判定する。曲率、曲率変化率、及び旋回半径は、例えば、ＩＭＵ２６及び操舵角センサ２８の検出値に基づいて算出する。または、地図情報データベース３４に格納された地図情報及び撮像装置２２を用いて取得した車両２００の周囲の画像情報のいずれかを用いて曲率、曲率変化率、及び旋回半径を算出してもよい。閾値Ｒthは、実車等を用いた実験を通じて具体的に決定する。ステップ１０６で、旋回半径が閾値Ｒth以上かつ曲率変化率が正の場合（曲率増加の場合）は手順をステップ１０８に移行し、旋回半径が閾値Ｒth以上でない場合又は曲率変化率が正でない場合（曲率減少の場合）は手順をステップ１１４に移行する。 In step 106, it is determined whether or not the turning radius is equal to or greater than the threshold value Rth and the curvature change rate is positive. The curvature, curvature change rate, and turning radius are calculated based on the values detected by the IMU 26 and the steering angle sensor 28, for example. Alternatively, the curvature, curvature change rate, and turning radius may be calculated using either map information stored in the map information database 34 or image information around the vehicle 200 acquired using the imaging device 22 . The threshold value Rth is specifically determined through experiments using an actual vehicle or the like. In step 106, if the turning radius is equal to or greater than the threshold value Rth and the curvature change rate is positive (curvature increase), the procedure proceeds to step 108. If the turning radius is not equal to or greater than the threshold value Rth or the curvature change rate is not positive (curvature decrease), the procedure proceeds to step 114.
ステップ１０８では、方位角偏差が正か否かを判定する。図９では、横位置の補正方向に応じて、前後位置の補正の態様を説明した。しかしながら、図６及び図７に示したように、推定位置を左に補正する場合は、車両２００の方位角偏差は負であり、推定位置を右に補正する場合は、車両２００の方位角偏差は正となる。本実施形態では、ステップ１０８と後述するステップ１１６において、横位置の補正方向に代えてＩＭＵ２６等によって検出が容易な方位角偏差の正負で前後位置の補正の態様を判定する。 At step 108, it is determined whether the azimuth deviation is positive. FIG. 9 illustrates how the front-rear position is corrected in accordance with the correction direction of the lateral position. However, as shown in FIGS. 6 and 7, the azimuth angle deviation of vehicle 200 is negative when the estimated position is corrected to the left, and the azimuth angle deviation of vehicle 200 is positive when the estimated position is corrected to the right. In this embodiment, in step 108 and step 116, which will be described later, the mode of front-back position correction is determined by the sign of the azimuth angle deviation, which can be easily detected by the IMU 26 or the like, instead of the correction direction of the lateral position.
ステップ１０８で、方位角偏差が正の場合は手順をステップ１１０に移行し、方位角偏差が正でない場合は手順をステップ１１２に移行する。 At step 108, if the azimuth deviation is positive, the procedure proceeds to step 110; otherwise, the procedure proceeds to step 112;
ステップ１１０では、車両２００の前後位置を後方に補正する後進補正を行って処理を終了する。ステップ１１２では、車両２００の前後位置を前方に補正する前進補正を行って処理を終了する。 In step 110, backward correction is performed to correct the longitudinal position of the vehicle 200 backward, and the process ends. At step 112, forward correction is performed to correct the longitudinal position of the vehicle 200 forward, and the process ends.
ステップ１１４では、旋回半径が閾値Ｒth以上かつ曲率変化率が負か否かを判定する。ステップ１１４で、旋回半径が閾値Ｒth以上かつ曲率変化率が負の場合は手順をステップ１１６に移行し、旋回半径が閾値Ｒth以上でない場合又は曲率変化率が負でない場合は手順をステップ１００に戻す。 At step 114, it is determined whether or not the turning radius is equal to or greater than the threshold value Rth and the curvature change rate is negative. At step 114, if the turning radius is equal to or greater than the threshold value Rth and the curvature change rate is negative, the procedure proceeds to step 116; otherwise, the procedure returns to step 100 if the turning radius is not equal to or greater than the threshold value Rth or the curvature change rate is not negative.
ステップ１１６では、方位角偏差が負か否かを判定する。ステップ１１６で、方位角偏差が負の場合は手順をステップ１１８に移行し、方位角偏差が負でない場合は手順をステップ１２０に移行する。 At step 116, it is determined whether the azimuth deviation is negative. At step 116, if the azimuth deviation is negative, the procedure proceeds to step 118, and if the azimuth deviation is not negative, the procedure proceeds to step 120.
ステップ１１８では、車両２００の前後位置を前方に補正する前進補正を行って処理を終了する。ステップ１２０では、車両２００の前後位置を後方に補正する後進補正を行って処理を終了する。 At step 118, forward correction is performed to correct the longitudinal position of the vehicle 200 forward, and the process ends. In step 120, backward correction is performed to correct the longitudinal position of the vehicle 200 backward, and the process ends.
続いて、前後位置の補正量について説明する。本実施形態で、前後位置の補正量は、前後位置補正部１０６すなわち演算装置１４の１制御周期当たりの車両２００の移動量である。具体的には、車両２００の１秒間の移動量をｘdiffとし、演算装置１４の制御クロックをｆsystem（Ｈｚ）とすると、演算装置１４の１制御周期当たりの車両２００の移動量は、ｘdiff /ｆsystemとなる。本実施形態では、かかる移動量を、誤差を含む前後位置を示すｘpredictに加算又は減算することによって車両の前後位置を補正する。 Next, the correction amount of the front-rear position will be described. In this embodiment, the correction amount of the longitudinal position is the amount of movement of the vehicle 200 per one control cycle of the longitudinal position correction unit 106 , that is, the arithmetic device 14 . Specifically, if the amount of movement of vehicle 200 per second is x diff and the control clock of arithmetic unit 14 is f system (Hz), the amount of movement of vehicle 200 per one control cycle of arithmetic unit 14 is x diff /f system . In this embodiment, the longitudinal position of the vehicle is corrected by adding or subtracting the amount of movement to or from x predict indicating the longitudinal position including the error.
例えば、図１１のステップ１１２、１１８の前進補正では、下記の式（７）を用いて前後位置を補正する。
For example, in the forward correction in steps 112 and 118 in FIG. 11, the forward/backward position is corrected using the following equation (7).
また、図１１のステップ１１０、１２０の後進補正では、下記の式（８）を用いて前後位置を補正する。
Further, in the backward correction of steps 110 and 120 in FIG. 11, the forward and backward positions are corrected using the following equation (8).
以上説明したように、本実施形態に係る自己位置推定装置１０によれば、旋回時に顕在化する車両２００の前後位置の推定誤差に起因した車両運動の特徴を捉えることにより、ランドマークとなる外部情報に依存せずに車両の前後位置を補正できる。 As described above, according to the self-position estimation device 10 according to the present embodiment, by capturing the characteristics of the vehicle motion caused by the estimation error of the longitudinal position of the vehicle 200 that becomes apparent when turning, it is possible to correct the longitudinal position of the vehicle without depending on external information that serves as a landmark.
地図空間上における車両２００の前後位置に誤差が生じると、地図空間上の車両の誤った前後位置を参考に操舵制御を行うため、車両２００の旋回開始及び終了のタイミングを誤り、車両２００が目標経路から逸脱してしまう。例えば、地図空間上に推定した自車位置が真の位置に比べて前方位置する誤差が生じていると、真の位置では車両２００はまだ旋回を開始していないが、地図空間上の推定位置では車両２００は旋回を開始するタイミングなので、車両２００の旋回を開始するような自動操舵が行われる。地図空間上の車両は走路を追従しているが、実際の車両２００は走路２０２から逸脱する方向に制御が行われてしまう。本技術は、かかる前後位置の誤差に起因する経路逸脱を車両の旋回時に検知し、地図空間上の車両の前後位置補正を行うことによって、車両２００が走路２０２を正しく追従することが可能となる。 If an error occurs in the longitudinal position of the vehicle 200 on the map space, steering control is performed with reference to the erroneous longitudinal position of the vehicle on the map space, so that the timing of starting and ending turning of the vehicle 200 is erroneous and the vehicle 200 deviates from the target route. For example, if there is an error in which the vehicle position estimated on the map space is ahead of the true position, the vehicle 200 has not yet started turning at the true position, but at the estimated position on the map space it is time for the vehicle 200 to start turning, so the vehicle 200 is automatically steered to start turning. Although the vehicle in the map space follows the track, the actual vehicle 200 is controlled in the direction of deviating from the track 202 . The present technology detects a route deviation caused by such an error in the longitudinal position when the vehicle turns, and corrects the longitudinal position of the vehicle in the map space, thereby enabling the vehicle 200 to correctly follow the road 202.
なお、特許請求の範囲における「横位置計測偏差」は、明細書の発明の詳細な説明における「横位置偏差ｙsensor」に、特許請求の範囲における「方位角計測偏差」は同「方位角偏差θsensor」に、特許請求の範囲に記載の「偏差計測部」は、同「横位置偏差計測部１００」に、特許請求の範囲における「横位置予測偏差」は、同「横位置偏差ｙpredict」に、特許請求の範囲における「方位角予測偏差」は、同「方位角偏差θpredict」に、特許請求の範囲における「偏差予測部」は、同「地図情報を利用した横位置偏差予測部１０２」に、特許請求の範囲における「横位置補正偏差」は、同「横位置補正偏差ｙt+Δt」に、特許請求の範囲における「方位角補正偏差」は、同「方位角補正偏差θt+Δt」に、特許請求の範囲における「偏差補正部」は、同「横位置偏差・方位角偏差補正部１０４」に、特許請求の範囲における「前後位置補正部」は、同「前後位置補正部１０６」に、各々相当する。 It should be noted that the "lateral position measurement deviation" in the claims is the same as the "lateral position deviation ysensor”, “Azimuth measurement deviation” in the scope of claims is the same “azimuth angle deviation θsensor, "deviation measuring section" in the claims means "lateral position deviation measuring section 100", and "lateral position prediction deviation" in the claims means "lateral position deviation ypredict, the "predicted azimuth angle deviation" in the claims is the same "azimuth angle deviation θpredict, the "deviation prediction unit" in the claims refers to the "lateral position deviation prediction unit 102 using map information", and the "lateral position correction deviation" in the claims refers to the "lateral position correction deviation yt+Δt, "the azimuth angle correction deviation" in the claims is the same as the "azimuth angle correction deviation θt+Δt”, the “deviation correction unit” in the scope of claims corresponds to the “lateral position deviation/azimuth angle deviation correction unit 104”, and the “front-back position correction unit” in the claims corresponds to the “front-back position correction unit 106”.
１０ 自己位置推定装置
１２ 入力装置
１４ 演算装置
１４Ａ ＲＯＭ
１４Ｂ ＣＰＵ
１４Ｃ ＲＡＭ
１８ 記憶装置
２０ 画像情報処理部
２２ 撮像装置
２４ 車速センサ
２６ ＩＭＵ
２８ 操舵角センサ
３０ ＧＰＳ
３２ ＬＩＤＡＲ
３４ 地図情報データベース
１００ 横位置偏差計測部
１０２ 地図情報を利用した横位置偏差予測部
１０４ 横位置偏差・方位角偏差補正部
１０６ 前後位置補正部
１０８ 曲率変化率ヨーレート入力部
１１０ 自車位置算出部
２００ 車両
２０２ 走路
２０４ 中心
２０６ 区画線
10 Self-position estimation device 12 Input device 14 Arithmetic device 14A ROM
14B CPU
14C RAM
18 storage device 20 image information processing unit 22 imaging device 24 vehicle speed sensor 26 IMU
28 steering angle sensor 30 GPS
32 LIDAR
34 map information database 100 lateral position deviation measurement unit 102 lateral position deviation prediction unit 104 using map information lateral position deviation/azimuth angle deviation correction unit 106 longitudinal position correction unit 108 curvature change rate yaw rate input unit 110 own vehicle position calculation unit 200 vehicle 202 track 204 center 206 lane marking
Claims (7)
前記車両の運動を計測するセンサと、地図情報とを用いて前記車両の地図空間上の位置を推定する偏差予測部と、
前記車両の地図空間上の推定位置に含まれる横位置予測偏差を、前記横位置計測偏差を用いて補正して横位置補正偏差を算出する偏差補正部と、
旋回の入口及び出口における前記横位置補正偏差に基づいて前記地図空間上に推定した前記車両の前後位置を補正する前後位置補正部と、
含み、
前記偏差計測部は、前記車両の周辺の情報を解析して前記走路の中心に対する前記車両の方位角計測偏差をさらに算出し、
前記偏差補正部は、前記車両の地図空間上の推定位置に含まれる方位角予測偏差を、前記方位角計測偏差を用いて補正して方位角補正偏差をさらに算出し、
前記前後位置補正部は、前記横位置補正偏差の変化率と前記方位角補正偏差との積に基づいて前記車両の地図空間上の前後位置の補正の要否を判定し、前記車両の地図空間上の前後位置の補正を要すると判定した場合に、前記前後位置を補正する
自己位置推定装置。 a deviation measuring unit that analyzes information around the vehicle and calculates the lateral position measurement deviation of the vehicle with respect to the center of the track;
a sensor for measuring motion of the vehicle and a deviation prediction unit for estimating the position of the vehicle on the map space using map information;
a deviation correction unit that calculates a lateral position correction deviation by correcting the lateral position prediction deviation included in the estimated position of the vehicle on the map space using the lateral position measurement deviation;
a longitudinal position correction unit that corrects the longitudinal position of the vehicle estimated on the map space based on the lateral position correction deviation at the entrance and exit of a turn ;
including
The deviation measurement unit analyzes information around the vehicle and further calculates the azimuth measurement deviation of the vehicle with respect to the center of the running path,
The deviation correction unit corrects an azimuth angle prediction deviation included in the estimated position of the vehicle on the map space using the azimuth measurement deviation to further calculate an azimuth angle correction deviation,
The fore-and-aft position correction unit determines whether correction of the fore-and-aft position of the vehicle on the map space is necessary based on the product of the rate of change of the lateral position correction deviation and the azimuth correction deviation, and corrects the fore-and-aft position when it is determined that the fore-and-aft position of the vehicle on the map space needs to be corrected.
Self-localization device.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2020180860A JP7313325B2 (en) | 2020-10-28 | 2020-10-28 | Self-localization device |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2020180860A JP7313325B2 (en) | 2020-10-28 | 2020-10-28 | Self-localization device |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JP2022071741A JP2022071741A (en) | 2022-05-16 |
| JP7313325B2 true JP7313325B2 (en) | 2023-07-24 |
Family
ID=81594011
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2020180860A Active JP7313325B2 (en) | 2020-10-28 | 2020-10-28 | Self-localization device |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP7313325B2 (en) |
Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2017211193A (en) | 2016-05-23 | 2017-11-30 | 本田技研工業株式会社 | Vehicle position identifying device, vehicle control system, vehicle position identifying method, and vehicle position identifying program |
Family Cites Families (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP7037317B2 (en) * | 2017-09-28 | 2022-03-16 | 株式会社Subaru | Vehicle position detector |
| JP6859927B2 (en) * | 2017-11-06 | 2021-04-14 | トヨタ自動車株式会社 | Vehicle position estimation device |
| JP2020003463A (en) * | 2018-07-02 | 2020-01-09 | トヨタ自動車株式会社 | Vehicle's self-position estimating device |
-
2020
- 2020-10-28 JP JP2020180860A patent/JP7313325B2/en active Active
Patent Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2017211193A (en) | 2016-05-23 | 2017-11-30 | 本田技研工業株式会社 | Vehicle position identifying device, vehicle control system, vehicle position identifying method, and vehicle position identifying program |
Also Published As
| Publication number | Publication date |
|---|---|
| JP2022071741A (en) | 2022-05-16 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111721285B (en) | Apparatus and method for estimating location in automatic valet parking system | |
| US9714034B2 (en) | Vehicle control device | |
| JP4724043B2 (en) | Object recognition device | |
| JP6036371B2 (en) | Vehicle driving support system and driving support method | |
| JP6451857B2 (en) | Method for controlling travel control device and travel control device | |
| KR102441073B1 (en) | Apparatus for compensating sensing value of gyroscope sensor, system having the same and method thereof | |
| KR20080037708A (en) | Vehicle positioning information update device | |
| JP2018009868A (en) | Position estimation device | |
| JP7189691B2 (en) | Vehicle cruise control system | |
| JP6943127B2 (en) | Position correction method, vehicle control method and position correction device | |
| JP6598122B2 (en) | Vehicle position determination device | |
| JP6645910B2 (en) | Position estimation device | |
| JP2018169319A (en) | Vehicle travel lane estimation device | |
| US20210197811A1 (en) | Course prediction device, computer readable medium, and course prediction method | |
| JP6790951B2 (en) | Map information learning method and map information learning device | |
| JP2019060814A (en) | Self-driving own vehicle location detection device | |
| JP7313325B2 (en) | Self-localization device | |
| JP7198005B2 (en) | Vehicle position detector | |
| JP7136050B2 (en) | Vehicle position estimation device | |
| KR102507172B1 (en) | Apparatus for sharing positioning information of autonomous vehicle and control method thereof | |
| JP7378591B2 (en) | Travel route generation device | |
| US11922836B2 (en) | Rendering system, display system, display control system, and rendering method | |
| WO2022168353A1 (en) | Vehicle position estimation device, autonomous driving device, and vehicle position estimation method | |
| US20240077320A1 (en) | Vehicle controller, method, and computer program for vehicle control | |
| US20240059288A1 (en) | Vehicle control device, storage medium storing computer program for vehicle control, and method for controlling vehicle |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20220411 |
|
| A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20230209 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20230221 |
|
| A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20230418 |
|
| TRDD | Decision of grant or rejection written | ||
| A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20230613 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20230711 |
|
| R150 | Certificate of patent or registration of utility model |
Ref document number: 7313325 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 |