JP6552448B2 - Vehicle position detection device, vehicle position detection method, and computer program for vehicle position detection - Google Patents
Vehicle position detection device, vehicle position detection method, and computer program for vehicle position detection Download PDFInfo
- Publication number
- JP6552448B2 JP6552448B2 JP2016073531A JP2016073531A JP6552448B2 JP 6552448 B2 JP6552448 B2 JP 6552448B2 JP 2016073531 A JP2016073531 A JP 2016073531A JP 2016073531 A JP2016073531 A JP 2016073531A JP 6552448 B2 JP6552448 B2 JP 6552448B2
- Authority
- JP
- Japan
- Prior art keywords
- vehicle
- line
- particles
- particle
- current time
- 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
Landscapes
- Navigation (AREA)
- Traffic Control Systems (AREA)
- Image Analysis (AREA)
Description
本発明は、車両に搭載されたカメラにより撮影された画像と地図を利用してその車両の位置を検出する車両位置検出装置、車両位置検出方法及び車両位置検出用コンピュータプログラムに関する。 The present invention relates to a vehicle position detection apparatus, a vehicle position detection method, and a computer program for vehicle position detection that detect the position of the vehicle using an image and a map taken by a camera mounted on the vehicle.
従来より、ナビゲーションまたは運転支援のために、Global Positioning System(GPS)信号を利用して、走行中の自車両の位置を検出することが行われている。しかし、車両の近くに、高層建築物など、GPSの測位衛星からの測位信号を遮る構造物が存在する場合、車両において測位信号を受信することが困難となり、自車両の位置を正確に検出できないことがあった。そこで、車載カメラにより撮影された、車両周囲の画像と、事前に準備された地図などの情報とを対応付けることで、自車両の位置の検出精度を向上させる方法が提案されている(例えば、特許文献1〜3及び非特許文献1及び2を参照)。 2. Description of the Related Art Conventionally, for the purpose of navigation or driving assistance, it has been performed to detect the position of a host vehicle while traveling by using a Global Positioning System (GPS) signal. However, if there is a structure near the vehicle, such as a high-rise building, that blocks the positioning signal from the GPS positioning satellite, it will be difficult for the vehicle to receive the positioning signal and the vehicle's position can not be detected accurately There was a thing. Then, the method of improving the detection accuracy of the position of self-vehicles is proposed by matching the information of a vehicle circumference, etc. which were picturized with the in-vehicle camera, and the map prepared beforehand (for example, patent) References 1-3 and Non-Patent References 1 and 2).
例えば、特許文献1に記載の位置測位装置は、車両の前方を撮影する撮影手段により撮影した画像の中にある道路標示に基づいて現在位置を特定する。 For example, the positioning device described in Patent Document 1 specifies the current position based on the road markings in the image captured by the imaging unit for capturing the front of the vehicle.
また、特許文献2に記載された移動体位置測定装置は、地図上の参照地点付近で移動体に搭載された撮像部が撮影して得た画像から抽出した特徴情報と、地図の参照地点付近の画像から抽出した特徴情報とをマッチングして推定した参照地点付近の複数の特徴の位置に応じて移動体の現在位置を推定する。 In addition, the moving body position measuring device described in Patent Document 2 includes feature information extracted from an image captured by an imaging unit mounted on a moving body in the vicinity of a reference point on the map, and the vicinity of the reference point on the map. The present position of the mobile object is estimated according to the positions of a plurality of features in the vicinity of the reference point estimated by matching with the feature information extracted from the image of.
また、特許文献3に記載された方法は、車両の進行方向の画像データに対してテンプレートマッチングを適用することで、画像内で識別された光学指標に関して車両の位置を決定する。 Further, the method described in Patent Document 3 determines the position of the vehicle with respect to the optical index identified in the image by applying template matching to the image data in the traveling direction of the vehicle.
さらに、非特許文献1に記載された方法は、縁石及び道路標示を表す線分含む地図とステレオカメラにより得られた画像から抽出された特徴点とのマッチングにより、自車両の位置を検出する。 Furthermore, the method described in Non-Patent Document 1 detects the position of the vehicle by matching a map including line segments representing curbs and road markings with feature points extracted from an image obtained by a stereo camera.
さらにまた、非特許文献2に記載された方法は、GPS、IMU、オドメトリ、LIDARなど、車載された機器により取得される情報を用いて、高解像度の環境地図を生成する。そしてこの方法は、車両をこれらの地図上で位置決めする際に、LIDER測定値と地図との相関をとるパーティクルフィルタを利用する。 Furthermore, the method described in Non-Patent Document 2 generates a high-resolution environment map using information acquired by a vehicle-mounted device, such as GPS, IMU, odometry, and LIDAR. The method then utilizes a particle filter to correlate the LIDER measurements with the map when positioning the vehicle on these maps.
しかし、特許文献1に記載の装置は、撮影時の環境の影響などにより、道路標示の特徴点の検出が困難な場合、自車両の位置を正確に特定できないおそれがある。特に、特許文献1に記載の装置は、道路標示の端点などを特徴点として抽出し、その特徴点を用いて車両の現在位置を求めるので、道路標示の経年劣化などにより、道路標示の端点がわずかに掠れるだけでも抽出された特徴点の位置がずれてしまい、自車両の位置の検出精度の低下を招いてしまう。 However, the device described in Patent Document 1 may not be able to accurately identify the position of the host vehicle if detection of the feature points of the road marking is difficult due to the influence of the environment at the time of shooting. In particular, the device described in Patent Document 1 extracts the end point of a road marking or the like as a feature point and determines the current position of the vehicle using the feature point, so the end point of the road marking Even if the position is slightly deviated, the positions of the extracted feature points are shifted, which causes a decrease in the detection accuracy of the position of the host vehicle.
また、特許文献2に記載の装置は、地図の参照地点付近で撮影された画像を自車両の位置の特定に利用するが、参照地点の付近の建物の建て替え、あるいは、撮影時の環境(例えば、天候、撮影時間帯など)によって、同じ場所について撮影された画像であっても、事前登録時に撮影された画像に写っている景色と、自車両位置の検出時に撮影された画像に写っている景色は大きく異なることがある。このような場合、画像間のマッチングに失敗する可能性が高くなり、そのため、自車両の位置を正確に特定できないおそれがある。 Further, although the device described in Patent Document 2 uses an image taken near the reference point of the map for specifying the position of the vehicle, the building in the vicinity of the reference point is reconstructed or the environment at the time of shooting (for example, Depending on the weather, shooting time zone, etc.), even if the image was taken at the same place, it is shown in the image taken at the time of pre-registration and in the image taken at the time of detection of the vehicle position. The scenery can vary greatly. In such a case, there is a high possibility that the matching between the images fails, and therefore, there is a possibility that the position of the vehicle can not be accurately identified.
さらに、特許文献3に記載の方法は、テンプレートマッチングを利用している。テンプレートマッチングでは、比較対象となる画像とテンプレート間の位置を変えつつ、繰り返しマッチング演算する必要が有るため、自車両の位置の検出に要する演算量が多い。また、非特許文献1に記載の方法では、縁石の検出と道路標示の検出に異なるアルゴリズムが利用されており、それらの検出にステレオ画像を利用するので、演算量が多い。そのためこれらの方法では、自車両の位置の検出に必要なハードウェアリソースが増えてしまうか、あるいは、自車両の位置の検出に時間を要するので、自車両の位置の更新間隔が長くなってしまう。さらに、非特許文献2に記載の方法では、自車両の位置の検出にレーザ測距システムが必要となり、システム全体のコストが高くなる。 Furthermore, the method described in Patent Document 3 utilizes template matching. In template matching, it is necessary to repeatedly perform matching calculations while changing the position between the image to be compared and the template, so the amount of calculation required to detect the position of the host vehicle is large. Further, in the method described in Non-Patent Document 1, different algorithms are used for detecting a curb and detecting a road marking, and since stereo images are used for detecting them, the amount of calculation is large. Therefore, in these methods, hardware resources necessary for detecting the position of the own vehicle increase, or time is required for detecting the position of the own vehicle, so that the update interval of the position of the own vehicle becomes long. . Furthermore, in the method described in Non-Patent Document 2, a laser ranging system is required to detect the position of the host vehicle, and the cost of the entire system is increased.
そこで、本発明は、低い処理負荷で、自車両の位置を正確に推定できる車両位置検出装置、車両位置検出方法及び車両位置検出用コンピュータプログラムを提供することを目的とする。 Then, this invention aims at providing the vehicle position detection apparatus which can estimate the position of the own vehicle correctly with low processing load, the vehicle position detection method, and the computer program for vehicle position detection.
請求項1の記載によれば、本発明の一つの形態として、車両位置検出装置が提供される。この車両位置検出装置は、道路上に標示される線の位置及びその線の種類を表す地図情報を記憶する記憶部(41)と、車両(10)の直前の位置から推定される車両の現時刻における予測位置に応じて道路上に標示される線を地図情報から抽出し、抽出した線のそれぞれについて、その線の種類に応じて、その線を横切る方向の輝度変化を表すパーティクルを、その線に沿った方向に応じた位置を変えながら複数生成するパーティクル生成部(23)と、抽出した線のそれぞれについての複数のパーティクルのそれぞれについて、予測位置を想定される誤差範囲内で変化させながら車両(10)を基準とする位置を求め、その位置にしたがって車両(10)に搭載された撮像部(2)により得られた現時刻の画像に投影する投影部(24)と、抽出した線のそれぞれについての複数のパーティクルのうち、画像との一致度合が所定の一致条件を満たすパーティクルを選択する選択部(25)と、選択されたパーティクルのそれぞれを、現時刻における車両(10)の推定位置に基づいて対応する道路上に標示される線に射影したときの誤差の総和に応じて車両(10)の推定位置を求める位置更新部(26)とを有する。
本発明による車両位置検出装置は、上記の構成を有することにより、低い処理負荷で、自車両の位置を正確に推定できる。
According to the first aspect of the present invention, a vehicle position detecting device is provided as one aspect of the present invention. The vehicle position detection device includes a storage unit (41) that stores the position of a line marked on the road and map information representing the type of the line, and the current position of the vehicle estimated from the position immediately before the vehicle (10). A line marked on the road is extracted from the map information according to the predicted position at the time, and for each of the extracted lines, a particle representing a change in luminance in a direction crossing the line according to the type of the line With respect to each of the plurality of particles for each of the particle generation unit (23) that generates a plurality of positions while changing the position according to the direction along the line, and changing the predicted position within an expected error range A projection unit (24) which finds a position based on the vehicle (10) and projects the image at the current time obtained by the imaging unit (2) mounted on the vehicle (10) according to the position A selection unit (25) for selecting a particle having a predetermined degree of coincidence with the image among a plurality of particles for each of the extracted lines; And a position updating unit (26) for obtaining an estimated position of the vehicle (10) in accordance with the sum of errors when projected onto a corresponding road on the basis of the estimated position of 10).
The vehicle position detection device according to the present invention can accurately estimate the position of the host vehicle with low processing load by having the above configuration.
また請求項2の記載によれば、位置更新部(26)は、選択されたパーティクルのそれぞれを対応する道路上に標示される線に射影したときの誤差の総和が最小となるように車両(10)の推定位置を求めることが好ましい。
この構成を有することで、車両位置検出装置は、自車両の位置をより正確に推定できる。
Further, according to the second aspect of the present invention, the position updating unit (26) makes it possible to minimize the sum of errors when projecting each of the selected particles onto a line marked on the corresponding road. Preferably, the estimated position of 10) is determined.
By having this configuration, the vehicle position detection device can estimate the position of the host vehicle more accurately.
また請求項3の記載によれば、位置更新部(26)は、選択されたパーティクルのそれぞれを対応する道路上に標示される線に射影したときの誤差の総和と、過去の複数の時刻のそれぞれにおける車両(10)の推定位置からオドメトリ情報を用いて予測した次の時刻における車両の位置と次の時刻における車両の推定位置との誤差の総和との和が最小となるように、現時刻及び過去の複数の時刻のそれぞれにおける車両(10)の推定位置を求めることが好ましい。
この構成を有することで、車両位置検出装置は、オドメトリ情報も利用するので、自車両の位置をより正確に推定できる。
According to the third aspect of the present invention, the position updating unit (26) adds the sum of the errors when projecting each of the selected particles onto the line marked on the corresponding road, and the time of the past plural times. The current time is calculated so that the sum of the error between the position of the vehicle at the next time predicted from the estimated position of the vehicle (10) and the estimated position of the vehicle at the next time is minimized. It is preferable to obtain the estimated position of the vehicle (10) at each of a plurality of past times.
With this configuration, the vehicle position detection device also uses odometry information, so that the position of the host vehicle can be estimated more accurately.
さらに、請求項4の記載によれば、パーティクル生成部(23)は、抽出した線の種類が点線または破線を含む線を含む場合、点線または破線のブロックが有る場合に相当する道路上に標示される線を横切る方向の輝度変化を表す第1のパーティクルと、点線または破線のブロックが無い場合に相当する道路上に標示される線を横切る方向の輝度変化を表す第2のパーティクルとを生成することが好ましい。
この構成を有することで、車両位置検出装置は、実際に道路上に標示される線と一致する可能性が高いパーティクルを生成できる。
Furthermore, according to the fourth aspect of the present invention, the particle generation unit (23) displays a mark on the road corresponding to the case where there is a dotted line or a broken line block if the extracted line type includes a dotted line or a broken line. Create a first particle representing the change in brightness across the line being drawn and a second particle representing the change in brightness across the line marked on the road that corresponds in the absence of the dotted or dashed block It is preferable to do.
With this configuration, the vehicle position detection device can generate particles that are highly likely to coincide with a line that is actually marked on the road.
さらに、請求項5の記載によれば、撮像部(2)は、二つのカメラを含むステレオカメラであり、選択部(25)は、二つのカメラのうちの一方により現時刻に得られた第1の画像上で一致度合が一致条件を満たし、かつ、二つのカメラのうちの他方により現時刻に得られた第2の画像上で一致度合が一致条件を満たすパーティクルを選択することが好ましい。
この構成を有することで、車両位置検出装置は、他の車両などによるオクルージョンにより路面が隠れているところに投影されたパーティクルを誤って選択する可能性を低減することができる。
Furthermore, according to the fifth aspect of the present invention, the imaging unit (2) is a stereo camera including two cameras, and the selection unit (25) is a unit obtained at the current time by one of the two cameras. It is preferable to select particles in which the coincidence degree satisfies the coincidence condition on one image and the coincidence degree satisfies the coincidence condition on the second image obtained at the current time by the other of the two cameras.
With this configuration, the vehicle position detection device can reduce the possibility of erroneously selecting a particle projected on a road surface hidden by occlusion by another vehicle or the like.
請求項6の記載によれば、本発明の他の形態として、車両位置検出方法が提供される。この車両位置検出方法は、車両(10)の直前の位置から推定されるその車両の現時刻における予測位置に応じて道路上に標示される線を地図情報から抽出し、抽出した線のそれぞれについて、地図情報に表されたその線の種類に応じて、その線を横切る方向の輝度変化を表すパーティクルを、その線に沿った方向に応じた位置を変えながら複数生成するステップと、抽出した線のそれぞれについての複数のパーティクルのそれぞれについて、予測位置を想定される誤差範囲内で変化させながら車両(10)を基準とする位置を求め、その位置にしたがって車両(10)に搭載された撮像部(2)により得られた現時刻の画像に投影するステップと、抽出した線のそれぞれについての複数のパーティクルのうち、画像との一致度合が所定の一致条件を満たすパーティクルを選択するステップと、選択されたパーティクルのそれぞれを、現時刻における車両(10)の推定位置に基づいて対応する道路上に標示される線に射影したときの誤差の総和に応じて車両(10)の推定位置を求めるステップとを含む。
本発明による車両位置検出方法は、上記の構成を有することにより、低い処理負荷で、自車両の位置を正確に推定できる。
According to the description of claim 6, a vehicle position detection method is provided as another embodiment of the present invention. This vehicle position detection method extracts the line marked on the road from the map information according to the predicted position at the current time of the vehicle estimated from the position immediately before the vehicle (10), and for each of the extracted lines Generating a plurality of particles representing a change in luminance in a direction crossing the line according to the type of the line represented in the map information, changing the position according to the direction along the line, and extracting the line For each of a plurality of particles for each of the above, the position based on the vehicle (10) is determined while changing the predicted position within the assumed error range, and the imaging unit mounted on the vehicle (10) according to the position The step of projecting onto the image at the current time obtained by (2), and among the plurality of particles for each of the extracted lines, the degree of coincidence with the image is a predetermined one According to the sum of the errors when selecting the particles that satisfy the conditions and projecting each of the selected particles on a line marked on the corresponding road based on the estimated position of the vehicle (10) at the current time Determining the estimated position of the vehicle (10).
The vehicle position detection method according to the present invention can accurately estimate the position of the host vehicle with low processing load by having the above configuration.
請求項6の記載によれば、本発明の他の形態として、車両位置検出用コンピュータプログラムが提供される。この車両位置検出用コンピュータプログラムは、車両(10)の直前の位置から推定されるその車両の現時刻における予測位置に応じて道路上に標示される線を地図情報から抽出し、抽出した線のそれぞれについて、地図情報に表されたその線の種類に応じて、その線を横切る方向の輝度変化を表すパーティクルを、その線に沿った方向に応じた位置を変えながら複数生成するステップと、抽出した線のそれぞれについての複数のパーティクルのそれぞれについて、予測位置を想定される誤差範囲内で変化させながら車両(10)を基準とする位置を求め、その位置にしたがって車両(10)に搭載された撮像部(2)により得られた現時刻の画像に投影するステップと、抽出した線のそれぞれについての複数のパーティクルのうち、画像との一致度合が所定の一致条件を満たすパーティクルを選択するステップと、選択されたパーティクルのそれぞれを、現時刻における車両(10)の推定位置に基づいて対応する道路上に標示される線に射影したときの誤差の総和に応じて車両(10)の推定位置を求めるステップとを車両(10)に搭載されたプロセッサ(43)に実行させる命令を含む。
本発明による車両位置検出用コンピュータプログラムは、上記の構成を有することにより、低い処理負荷で、自車両の位置を正確に推定できる。
According to the sixth aspect of the present invention, a computer program for detecting a vehicle position is provided as another aspect of the present invention. The computer program for vehicle position detection extracts the line marked on the road from the map information according to the predicted position at the current time of the vehicle estimated from the position immediately before the vehicle (10) In each of the steps, according to the type of the line represented in the map information, a plurality of particles representing a change in luminance in the direction crossing the line are generated while changing positions corresponding to the direction along the line; For each of a plurality of particles for each of the above-mentioned lines, the position based on the vehicle (10) is determined while changing the predicted position within an assumed error range, and mounted on the vehicle (10) according to the position The step of projecting on the image at the current time obtained by the imaging unit (2), and the image among the plurality of particles for each of the extracted lines When the step of selecting a particle whose coincidence degree satisfies a predetermined coincidence condition and each of the selected particles is projected on a line marked on the corresponding road based on the estimated position of the vehicle (10) at the current time Determining an estimated position of the vehicle (10) in accordance with the sum of the errors of (1) and (2), and instructing the processor (43) mounted on the vehicle (10) to execute.
The computer program for detecting a vehicle position according to the present invention has the above configuration, so that the position of the host vehicle can be accurately estimated with a low processing load.
上記各部に付した括弧内の符号は、後述する実施形態に記載の具体的手段との対応関係を示す一例である。 The reference numerals in parentheses attached to the above-described portions are an example showing the correspondence with specific means described in the embodiments to be described later.
以下、図を参照しつつ、車両位置検出システムについて説明する。
この車両位置検出システムは、区画線または道路標示などの道路上に標示された線の種類に関する情報を含む地図情報を参照して、直前の自車両の位置などから推定される現時刻における予測位置の周辺の地図上の線に沿って、線の種類に応じた、線を横切る方向の輝度変化を表す輝度プロファイルを持つパーティクルを複数生成する。そしてこの車両位置検出システムは、地図上の座標系で表される各パーティクルを、自車両の現時刻における予測位置及び予測方位に応じて車両を基準とする座標系での位置に変換してから車両に搭載されたカメラにより得られる画像上へ投影する。そしてこの車両位置検出システムは、画像上に投影された各パーティクルのうち、画像との一致度合が所定の一致条件を満たすパーティクルを選択し、選択したパーティクルの地図上の線への投影誤差が小さくなるように、現時刻における自車両の位置及び方位を推定する。
Hereinafter, the vehicle position detection system will be described with reference to the drawings.
This vehicle position detection system refers to map information including information on the type of line marked on the road such as a dividing line or a road marking, and estimates the predicted position at the current time estimated from the position of the own vehicle immediately before A plurality of particles having a luminance profile representing a luminance change in a direction crossing the line according to the type of the line are generated along a line on the map around the area. Then, the vehicle position detection system converts each particle represented by the coordinate system on the map into a position in the coordinate system based on the vehicle according to the predicted position and the predicted direction at the current time of the vehicle. Project onto an image obtained by a camera mounted on a vehicle. Then, the vehicle position detection system selects particles among the particles projected on the image that satisfy the predetermined coincidence condition with the image, and the projection error of the selected particles onto the line on the map is small. Thus, the position and orientation of the vehicle at the current time are estimated.
図1は、一つの実施形態による車両位置検出システムの概略構成図である。図1に示すように、車両位置検出システム1は、車両10に搭載され、カメラ2と、コントローラ4とを有する。カメラ2と、コントローラ4とは、コントロールエリアネットワーク(以下、CANという)3によって互いに接続されている。なお、図1では、説明の都合のため、車両位置検出システム1の各構成要素及び車両10の形状、サイズ及び配置は、実際のものとは異なっている。 FIG. 1 is a schematic block diagram of a vehicle position detection system according to one embodiment. As shown in FIG. 1, a vehicle position detection system 1 is mounted on a vehicle 10 and has a camera 2 and a controller 4. The camera 2 and the controller 4 are connected to each other by a control area network (hereinafter referred to as CAN) 3. In addition, in FIG. 1, for convenience of description, each component of the vehicle position detection system 1 and the shape, size, and arrangement of the vehicle 10 are different from actual ones.
カメラ2は、撮像部の一例であり、車両の前方領域を撮影し、その前方領域の画像を生成する。そのために、カメラ2は、CCDあるいはC-MOSなど、可視光に感度を有する光電変換素子のアレイで構成された2次元検出器と、その2次元検出器上に車両10の前方に存在する地面または構造物などの像を結像する結像光学系を有する。そしてカメラ2は、例えば、結像光学系の光軸が地面に対して略平行となり、かつ車両10の前方を向くように、車両10の車室内に配置される。そしてカメラ2は、一定の時間間隔(例えば1/30秒)ごとに撮影し、車両10の前方領域を撮影したカラー画像を生成する。なお、カメラ2は、近赤外光に感度を有する2次元検出器を有し、その撮像範囲内の近赤外光の照度に応じたモノクロ画像を生成してもよい。 The camera 2 is an example of an imaging unit, captures an area in front of the vehicle, and generates an image of the area in front of the vehicle. For that purpose, the camera 2 is a two-dimensional detector composed of an array of photoelectric conversion elements sensitive to visible light, such as a CCD or C-MOS, and the ground existing ahead of the vehicle 10 on the two-dimensional detector. Alternatively, an imaging optical system that forms an image of a structure or the like is included. The camera 2 is disposed, for example, in the compartment of the vehicle 10 such that the optical axis of the imaging optical system is substantially parallel to the ground and faces the front of the vehicle 10. Then, the camera 2 captures an image at predetermined time intervals (for example, 1/30 seconds) and generates a color image obtained by capturing an area in front of the vehicle 10. The camera 2 may have a two-dimensional detector having sensitivity to near-infrared light, and may generate a monochrome image according to the illuminance of the near-infrared light within the imaging range.
図2は、地図座標系と、車両座標系と、カメラ座標系の関係を示す図である。本実施形態では、便宜上、地図上の任意の位置を原点とする地図座標系(Xm, Ym, Zm)と、車両10を原点とする車両座標系(Xv, Yv, Zv)と、カメラ2を原点とするカメラ座標系(Xc, Yc, Zc)を利用する。本実施形態では、車両座標系(Xv, Yv, Zv)は、車両10の左右の後輪間の中点かつ地面上の点を原点とする。そして車両の進行方向をZv軸とし、Zv軸と直交し、かつ、地面に平行な方向をXv軸とし、鉛直方向をYv軸とする。また地図座標系(Xm, Ym, Zm)においても、傾きの無い地面に平行な面内にXm軸及びZm軸が設定され、その地面に対する鉛直方向にYm軸が設定される。また、カメラ座標系(Xc, Yc, Zc)では、説明の簡単化のために、車両座標系の原点から鉛直方向に沿って上方かつカメラ2が設置される高さの位置に撮像面の中心があると仮定して、その撮像面の中心を原点とする。そして車両座標系と同様に、車両10の進行方向をZc軸とし、Zc軸と直交し、かつ、地面に平行な方向をXc軸とし、鉛直方向をYc軸とする。
なお、実際にカメラ2が取り付けられる位置は、車両10の左右の後輪間の中点の上方からずれていることもあるが、このずれは、単純な平行移動によって補正すればよい。
FIG. 2 is a diagram illustrating the relationship among the map coordinate system, the vehicle coordinate system, and the camera coordinate system. In this embodiment, a map coordinate system (Xm, Ym, Zm) whose origin is an arbitrary position on the map, a vehicle coordinate system (Xv, Yv, Zv) whose origin is the vehicle 10, and the camera 2 are provided for convenience. Use the camera coordinate system (Xc, Yc, Zc) as the origin. In the present embodiment, the vehicle coordinate system (Xv, Yv, Zv) uses the middle point between the left and right rear wheels of the vehicle 10 and the point on the ground as the origin. A traveling direction of the vehicle is taken as a Zv axis, a direction perpendicular to the Zv axis, a direction parallel to the ground is taken as an Xv axis, and a vertical direction is taken as an Yv axis. Also in the map coordinate system (Xm, Ym, Zm), the Xm axis and the Zm axis are set in a plane parallel to the ground without inclination, and the Ym axis is set in the vertical direction with respect to the ground. In the camera coordinate system (Xc, Yc, Zc), the center of the imaging plane at a position above the origin of the vehicle coordinate system along the vertical direction and at the height at which the camera 2 is installed, for simplicity of explanation. It is assumed that the center of the imaging plane is the origin. Then, as in the vehicle coordinate system, the traveling direction of the vehicle 10 is taken as the Zc axis, the direction orthogonal to the Zc axis, parallel to the ground is taken as the Xc axis, and the vertical direction is taken as the Yc axis.
Although the position where the camera 2 is actually attached may be offset from above the middle point between the left and right rear wheels of the vehicle 10, this offset may be corrected by a simple parallel movement.
なお、車両位置検出システム1は、撮像部として、車両10の前方領域を撮影するカメラの代わりに、あるいはそのカメラとともに、車両の後方領域を撮影するリアカメラを有していてもよい。 The vehicle position detection system 1 may have, as an imaging unit, a rear camera for imaging the rear area of the vehicle, instead of or together with the camera for imaging the front area of the vehicle 10.
カメラ2は、生成した画像を逐次コントローラ4へ送信する。なお、車両10の前方領域を撮影するカメラと車両10の後方領域を撮影するカメラが取り付けられている場合、コントローラ4は、車両10が進行している方向を撮影するカメラからの画像のみを選択的に取得してもよい。そのために、コントローラ4は、CAN3を介して車両10の電子制御ユニット(ECU)11から、シフトレバーのポジションを表すシフトポジション信号を取得する。そしてコントローラ4は、シフトポジション信号が、車両10が前進することを示すドライブポジジョンなどとなっている場合、車両10の前方領域を撮影するカメラから画像を取得する。一方、コントローラ4は、シフトポジション信号が、車両10が後進することを示すリバースポジションとなっている場合、車両10の後方領域を撮影するカメラから画像を取得する。 The camera 2 sequentially transmits the generated images to the controller 4. In addition, when the camera which image | photographs the front area | region of the vehicle 10, and the camera which image | photographs the back area | region of the vehicle 10 are attached, the controller 4 selects only the image from the camera which image | photographs the direction which the vehicle 10 is advancing. May be acquired automatically. To that end, the controller 4 obtains a shift position signal representing the position of the shift lever from the electronic control unit (ECU) 11 of the vehicle 10 via the CAN 3. Then, when the shift position signal indicates a drive position or the like indicating that the vehicle 10 moves forward, the controller 4 acquires an image from a camera that captures an area in front of the vehicle 10. On the other hand, when the shift position signal is in the reverse position indicating that the vehicle 10 moves backward, the controller 4 obtains an image from a camera that captures an area behind the vehicle 10.
コントローラ4は、車両位置検出装置の一例であり、記憶部41と、通信部42と、制御部43とを有する。記憶部41は、例えば、電気的に書き換え可能な不揮発性メモリ及び揮発性メモリなどの半導体メモリを有する。そして記憶部41は、車両位置検出システム1を制御するための各種プログラム、地図情報、カメラ2の地面からの高さ及び光軸方向といったカメラの位置情報、結像光学系の焦点距離及び画角といったカメラパラメータなどの各種パラメータ、及び制御部43による一時的な演算結果などを記憶する。また記憶部41は、カメラ2の結像光学系による歪曲収差を補正するためのパラメータ、例えば、画素ごとの歪曲収差の補正量(すなわち、歪曲収差を打ち消すための画像上での画素の移動量及び移動方向)を記憶してもよい。 The controller 4 is an example of a vehicle position detection device, and includes a storage unit 41, a communication unit 42, and a control unit 43. The storage unit 41 includes, for example, semiconductor memories such as electrically rewritable non-volatile memory and volatile memory. The storage unit 41 stores various programs for controlling the vehicle position detection system 1, map information, camera position information such as height from the ground of the camera 2 and optical axis direction, focal length and angle of view of the imaging optical system. And various parameters such as camera parameters, and temporary calculation results by the control unit 43. In addition, the storage unit 41 is a parameter for correcting distortion by the imaging optical system of the camera 2, for example, a correction amount of distortion for each pixel (that is, a movement amount of pixels on an image for canceling distortion). And the moving direction) may be stored.
以下、車両位置検出に利用される、地図情報について説明する。
図3(a)〜図3(d)は、地図情報の一例を示す図である。図3(a)に示される地図情報300は、その地図情報で表された領域の位置を表す緯度経度情報と関連付けられている。そして地図情報300は、白線、黄線などで表される区画線などの道路上に標示される線301に関する情報を含む。
Hereinafter, map information used for vehicle position detection will be described.
Fig.3 (a)-FIG.3 (d) are figures which show an example of map information. The map information 300 shown in FIG. 3A is associated with latitude / longitude information indicating the position of the area represented by the map information. The map information 300 includes information related to the line 301 marked on the road such as a lane marking represented by a white line, a yellow line, or the like.
実際の道路上に標示される線には、複数の種類がある。例えば、図3(b)において点線で示される線311は、実線の白線と一方の側に設けられる破線状の減速標示の組み合わせで表される。また図3(c)において点線で示される線312は、点線の白線とその両側に設けられる減速標示の組み合わせで表される。また、図3(d)において点線で示される線313は、相対的に太い実線の白線のみで表される。このように、道路上に標示される線には、様々な種類のものがある。しかし、地図情報300では、例えば、それぞれの道路上の線について、一つの実線で表され、線ごとに、その線の種類を表すラベルが付されていることがある。例えば、図3(b)に示される例では、線311は、実線321と、(白線(細)+右減速標示)というラベルで表される。また、図3(c)に示される例では、線312は、実線322と、(白点線(細)+両側減速標示)というラベルで表される。そして図3(d)に示される例では、線313は、実線323と、白線(太)というラベルで表される。さらに、地図情報300では、道路上に標示される各線について、その両端点の3次元座標が含まれる。
なお、以下では、地図情報に含まれる、道路上に標示される線を地図線分と呼ぶ。
There are multiple types of lines marked on the actual road. For example, a line 311 indicated by a dotted line in FIG. 3B is represented by a combination of a solid white line and a broken line decelerating mark provided on one side. A line 312 indicated by a dotted line in FIG. 3C is represented by a combination of a dotted white line and deceleration signs provided on both sides thereof. Further, the line 313 indicated by a dotted line in FIG. 3D is represented only by a relatively thick solid white line. Thus, there are various types of lines that are marked on the road. However, in the map information 300, for example, a line on each road may be represented by one solid line, and each line may be labeled to indicate the type of the line. For example, in the example illustrated in FIG. 3B, the line 311 is represented by a solid line 321 and a label of (white line (thin) + right deceleration mark). Further, in the example shown in FIG. 3C, the line 312 is represented by a solid line 322 and a label (white dotted line (thin) + both-side deceleration sign). In the example shown in FIG. 3D, the line 313 is represented by a solid line 323 and a label of a white line (thick). Furthermore, in the map information 300, for each line marked on the road, three-dimensional coordinates of both end points thereof are included.
In addition, below, the line marked on the road included in map information is called a map line segment.
通信部42は、カメラ2、ECU11及び車輪速センサ(図示せず)などの各種センサとCAN3を通じて通信する通信インターフェース及びその制御回路を有する。そして通信部42は、カメラ2から画像を受け取り、その画像を制御部43へ渡す。また通信部42は、位置推定処理を実行する周期ごとに、オドメトリ情報として、ECU11から、車両10の速度及び移動量などを取得したり、あるいは、車輪速センサから車輪速を取得して、制御部43へ渡す。 The communication unit 42 includes a communication interface that communicates with the various sensors such as the camera 2, the ECU 11 and a wheel speed sensor (not shown), the CAN 3, and a control circuit thereof. Then, the communication unit 42 receives an image from the camera 2 and passes the image to the control unit 43. In addition, the communication unit 42 acquires the speed and movement amount of the vehicle 10 from the ECU 11 as odometry information or acquires the wheel speed from the wheel speed sensor as odometry information for each cycle in which the position estimation process is performed. Pass to section 43.
制御部43は、1個もしくは複数個の図示していないプロセッサ及びその周辺回路を有する。そして制御部43は、車両位置検出システム1全体を制御する。
図4に、制御部43の機能ブロック図を示す。図4に示すように、制御部43は、初期値設定部21と、予測位置算出部22と、パーティクル生成部23と、投影部24と、選択部25と、位置更新部26とを有する。制御部43が有するこれらの各部は、例えば、制御部43が有するプロセッサ上で実行されるコンピュータプログラムによって実現される機能モジュールとして実装される。
The control unit 43 includes one or a plurality of processors (not shown) and their peripheral circuits. Then, the control unit 43 controls the entire vehicle position detection system 1.
FIG. 4 shows a functional block diagram of the control unit 43. As shown in FIG. As shown in FIG. 4, the control unit 43 includes an initial value setting unit 21, a predicted position calculation unit 22, a particle generation unit 23, a projection unit 24, a selection unit 25, and a position update unit 26. Each of these units included in the control unit 43 is implemented as a functional module realized by a computer program executed on a processor included in the control unit 43, for example.
初期値設定部21は、本実施形態による、車両10の位置の検出、すなわち、自車両の位置の検出を開始する際の自車両の位置の初期値を設定する。例えば、初期値設定部21は、GPSの測位信号が最後に受信できたときのその測位信号から求められる位置を、例えば、ナビゲーションシステムから取得し、その位置に基づいて、自車両の位置の初期値を設定する。そして得られた位置及び方位を、自車両の位置の初期値に設定する。
なお、初期値設定部21により設定される初期値は、本実施形態による自車両の位置の検出精度よりも低い検出精度で得られる値であってもよい。
The initial value setting unit 21 sets an initial value of the position of the host vehicle at the start of the detection of the position of the vehicle 10, that is, the detection of the position of the host vehicle according to the present embodiment. For example, the initial value setting unit 21 acquires, for example, from the navigation system, the position obtained from the positioning signal when the GPS positioning signal was finally received, and, based on the position, the initial position of the vehicle. Set the value Then, the obtained position and direction are set to the initial value of the position of the host vehicle.
Note that the initial value set by the initial value setting unit 21 may be a value obtained with detection accuracy lower than the detection accuracy of the position of the host vehicle according to the present embodiment.
本実施形態では、時刻tにおける、自車両の位置の初期値μt=(xt,zt,θt)tは、ガウス分布に従って、次式で表される。
初期値設定部21により設定された自車両の位置の初期値は、記憶部41に記憶される。 The initial value of the position of the host vehicle set by the initial value setting unit 21 is stored in the storage unit 41.
予測位置算出部22は、自車両の位置の初期値、あるいは、位置更新部26により求められた直前の自車両の位置と、車両10の車輪速等に基づいて、現時刻における自車両の予測位置を算出する。
本実施形態では、現時刻tにおける自車両の予測位置は、平均値μp t、共分散Σp tのガウス分布として表される。そこで予測位置算出部22は、次式に従って、現時刻tにおける自車両の水平方向における予測位置と予測方位の平均値μp t=(xp t,zp t,θp t)と共分散行列Σp tを算出する。
In the present embodiment, the predicted position of the host vehicle at the current time t is expressed as a Gaussian distribution with an average value μ p t and a covariance Σ p t . Therefore prediction position calculation unit 22, according to the following equation, the covariance and the average value mu p t = predicted azimuth and predicted position in the horizontal direction of the vehicle at the current time t (x p t, z p t, θ p t) Calculate the matrix p p t .
(3)式において、ut=(vt,ωt)tは、は、車輪速から定められる制御入力を表す。vtは、現時刻tにおける車両10の速度であり、左右の後輪に取り付けられた車輪速センサから通信部42を介して受信した、最新の右後輪の車輪速vR tと左後輪の車輪速vL tの平均値(vR t+vL t)/2として算出される。またωtは、現時刻tにおける車両10の角速度であり、ωt=(vR t-vL t)/2daである。daは、左右の後輪間の距離である。
またΔtは、現時刻tと直前の時刻(t-1)の時間間隔、すなわち、制御部43が自車両位置の検出処理を行う時間間隔である。行列Qtは、後輪車輪速による自車両の位置の予測に含まれる誤差の度合いを表している。行列Qtの各要素σx、σz、σθは、それぞれ、地図座標系におけるXm方向の位置の誤差の標準偏差、Zm方向の位置の誤差の標準偏差、及び方位の誤差の標準偏差に相当し、例えば、σx=σz=1(m)、σθ=10(°)に設定される。
なお、予測位置算出部22は、現時刻tにおける自車両の標高ytについては、例えば、(2)式に従って算出すればよい。
In equation (3), u t = (v t , ω t ) t represents a control input determined from the wheel speed. v t is the speed of the vehicle 10 at the present time t, and the latest right rear wheel wheel speed v R t and left rear received from the wheel speed sensors attached to the left and right rear wheels via the communication unit 42 The average value (v R t + v L t ) / 2 of the wheel speed v L t of the wheel is calculated. Further, ω t is an angular velocity of the vehicle 10 at the current time t, and is ωt = (v R t −v L t ) / 2da. da is the distance between the left and right rear wheels.
Δt is a time interval between the current time t and the immediately preceding time (t−1), that is, a time interval at which the control unit 43 performs the detection processing of the host vehicle position. Matrix Q t represents the degree of error contained in the predicted position of the vehicle by the rear wheel speed. The elements σ x , σ z , and σ θ of the matrix Q t are the standard deviation of the position error in the Xm direction, the standard deviation of the position error in the Zm direction, and the standard deviation of the orientation error in the map coordinate system, respectively. Correspondingly, for example, σ x = σ z = 1 (m) and σ θ = 10 (°) are set.
The predicted position calculation unit 22 may calculate the altitude y t of the host vehicle at the current time t, for example, according to the equation (2).
予測位置算出部22は、μp t及びΣp tを記憶部41に記憶する。 The predicted position calculation unit 22 stores μ p t and Σ p t in the storage unit 41.
パーティクル生成部23は、車両10の現時刻における予測位置に基づいて、その予測位置におけるカメラ2の撮影範囲に含まれる地図線分を横切る方向の輝度変化を表す輝度プロファイルを持つ輝度パターンであるパーティクルを、地図情報に含まれるその地図線分の種類に応じて生成する。 The particle generation unit 23 is a particle that is a luminance pattern having a luminance profile that represents a luminance change in a direction across the map line segment included in the shooting range of the camera 2 at the predicted position based on the predicted position of the vehicle 10 at the current time. Are generated according to the type of the map line segment included in the map information.
本実施形態では、パーティクル生成部23は、パーティクルの生成範囲及び密度を適切に設定するために、予測位置の周囲の所定範囲(例えば、30m以内)、かつ、予測位置を基準としたカメラ2の撮影範囲内にある各地図線分を地図情報から抽出し、抽出した各地図線分をカメラ2により得られる画像上に投影する。なお、一端が所定範囲及び撮影範囲内にあり、かつ他端が所定範囲または撮影範囲から外れる地図線分については、パーティクル生成部23は、その所定の範囲の境界及び撮影範囲の境界のうちの予測位置から近い方と交差した点を地図線分の他端として、その地図線分を画像上に投影する。以下、各地図線分に対して同一の処理が行われるので、r番目の地図線分を例として説明する。 In the present embodiment, the particle generation unit 23 sets the generation range and density of the particles appropriately in a predetermined range around the predicted position (for example, within 30 m) and the camera 2 with the predicted position as a reference. Each map line segment within the imaging range is extracted from the map information, and each extracted map line segment is projected onto an image obtained by the camera 2. In addition, for the map line segment where one end is within the predetermined range and the photographing range and the other end is out of the predetermined range or the photographing range, the particle generating unit 23 determines the boundary between the predetermined range and the photographing range. The map line segment is projected onto the image, with the point intersecting with the near side from the predicted position as the other end of the map line segment. Hereinafter, since the same process is performed on each map line segment, the r-th map line segment will be described as an example.
地図座標系で表される地図上の点pm=(xm,ym,zm)と画像上の点uc=(u,v)との関係は、次式で表される。
パーティクル生成部23は、μptに基づいて(xvm t,zvm t,θvm t)を設定し、(4)式に従って、r番目の地図線分の両端点(ms r,me r)をカメラ2により得られる画像上に投影する。そしてパーティクル生成部23は、画像上でのその地図線分の長さlimg rを求める。パーティクル生成部23は、その地図線分の長さlimg rを一本の地図線分あたりに発生させるパーティクルの総数(例えば、500〜1000)で等分して得られる位置のそれぞれをパーティクルを発生させる位置の候補とする。パーティクル生成部23は、各候補の中から所定数の候補を一様分布に従ってランダムに選択する。そしてパーティクル生成部23は、選択した候補に対応する地図座標系での3次元位置にパーティクルを発生させる。あるいは、パーティクル生成部23は、全ての候補についてパーティクルを発生させてもよい。 The particle generation unit 23 sets (x vm t , z vm t , θ vm t ) based on μp t, and according to the equation (4), both end points (m s r , m e of the r-th map line segment Project r ) onto the image obtained by the camera 2. Then, the particle generation unit 23 obtains the length l img r of the map line segment on the image. The particle generation unit 23 divides each of the positions obtained by equally dividing the length l img r of the map line segment by the total number of particles (for example, 500 to 1000) to be generated per one map line segment. It is a candidate for the position to be generated. The particle generator 23 randomly selects a predetermined number of candidates from each candidate according to a uniform distribution. Then, the particle generation unit 23 generates particles at three-dimensional positions in the map coordinate system corresponding to the selected candidate. Alternatively, the particle generator 23 may generate particles for all candidates.
パーティクル生成部23は、パーティクルを発生させる地図線分の種類に応じて、パーティクルのタイプを決定する。なお、地図線分の種類は、上述したように、地図情報に含まれる。その際、パーティクル生成部23は、地図線分の種類によっては、複数の種類のパーティクルを、そのパーティクルの種類に応じた確率に従って発生させる。 The particle generation unit 23 determines the type of particle according to the type of map line segment that generates the particle. The type of map line segment is included in the map information as described above. At this time, depending on the type of map line segment, the particle generation unit 23 generates a plurality of types of particles according to the probability according to the type of particles.
図5は、地図線分の種類が「白線(細)」である場合のパーティクルの一例を示す図である。図5において、横軸は、実線である白線の位置を原点とする、地図線分と直交する方向に沿った位置を表し、縦軸は輝度を表す。そしてプロファイル500は、発生させるパーティクルの輝度プロファイルを表す。地図線分の種類が「白線(細)」である場合には、一本の白線が道路上に標示されているので、地図線分に沿った方向の位置によらず、地図線分を横切る方向の輝度プロファイルは変化しない。そこでこの場合には、パーティクル生成部23は、発生確率p1=1.0にて、プロファイル500で表される、白線の位置に相当し、かつ、白線の幅と同じ幅を持つ中央部分だけが高い輝度を持ち、その両側が低い輝度となるパーティクルを発生させる。 FIG. 5 is a view showing an example of the particle in the case where the type of map line segment is "white line (thin)". In FIG. 5, the horizontal axis represents a position along the direction orthogonal to the map line segment, with the position of the white line being a solid line as the origin, and the vertical axis represents luminance. A profile 500 represents a luminance profile of particles to be generated. When the type of map line segment is "white line (thin)", one white line is marked on the road, so the map line segment is crossed regardless of the position in the direction along the map line segment. The luminance profile in the direction does not change. Therefore, in this case, the particle generation unit 23 has a high luminance only in the central portion corresponding to the position of the white line represented by the profile 500 and having the same width as the width of the white line with the occurrence probability p1 = 1.0. And generate particles with low brightness on both sides.
図6は、地図線分の種類が「白線(細)+右側減速標示」である場合のパーティクルの一例を示す図である。図6において、横軸は、実線である白線の位置を原点とする、地図線分と直交する方向に沿った位置を表し、縦軸は輝度を表す。そしてプロファイル600及び610は、それぞれ、発生させるパーティクルの輝度プロファイルを表す。地図線分の種類が「白線(細)+右側減速標示」である場合には、一本の白線だけでなく、減速標示である破線が道路上に標示されているので、地図線分に沿った方向の位置によって、減速標示のブロックの有無により、地図線分を横切る方向の輝度プロファイルは変化する。そこでこの場合には、パーティクル生成部23は、発生確率p1=0.4にて、プロファイル600で表される、白線だけが存在する場合に相当する、白線の位置に相当し、かつ、白線の幅と同じ幅を持つ中央部分だけが高い輝度を持ち、その両側が低い輝度となるパーティクルを発生させ、発生確率p2=0.6にて、プロファイル610で表される、白線とともに減速標示のブロックが存在する場合に相当する、白線に相当する位置と減速標示のブロックに相当する位置の2か所で相対的に高い輝度を持つパーティクルを発生させる。なお、発生確率p1及びp2は、減速標示のブロックの長さとブロック間の長さの比に応じて、予め設定される。 FIG. 6 is a view showing an example of the particle in the case where the type of map line segment is “white line (thin) + right deceleration mark”. In FIG. 6, the horizontal axis represents a position along the direction orthogonal to the map line segment, with the position of the white line which is a solid line as the origin, and the vertical axis represents luminance. Profiles 600 and 610 respectively represent luminance profiles of particles to be generated. When the type of map line segment is "white line (thin) + right deceleration mark", not only one white line but also a broken line that is a deceleration mark is displayed on the road, so Depending on the position in the different direction, the brightness profile in the direction crossing the map line segment changes depending on the presence or absence of the block of the deceleration marking. Therefore, in this case, the particle generation unit 23 corresponds to the position of the white line, which corresponds to the case where only the white line exists, represented by the profile 600, with the occurrence probability p1 = 0.4, and the width of the white line. In the case where a central portion having the same width has high luminance and both sides generate particles with low luminance, and there is a block of deceleration marking with a white line represented by profile 610 with occurrence probability p2 = 0.6 The particle having a relatively high brightness is generated at two positions corresponding to the white line and the position corresponding to the block of the deceleration marking. The occurrence probabilities p1 and p2 are preset according to the ratio of the block length of the deceleration marking and the length between the blocks.
図7は、地図線分の種類が「白点線(細)+両側減速標示」である場合のパーティクルの一例を示す図である。図7において、横軸は、白点線の位置を原点とする、地図線分と直交する方向に沿った位置を表し、縦軸は輝度を表す。そしてプロファイル700〜720は、それぞれ、発生させるパーティクルの輝度プロファイルを表す。地図線分の種類が「白点線(細)+両側減速標示」である場合には、一本の白点線だけでなく、減速標示である破線が白点線の両側に道路上に標示されているので、地図線分に沿った方向の位置によって、白点線のブロックの有無、及び、減速標示のブロックの有無により、地図線分を横切る方向の輝度プロファイルは変化する。そこでこの場合には、パーティクル生成部23は、発生確率p1=0.3にて、プロファイル700で表される、白点線のブロックだけが存在する場合に相当する、白点線に相当する中央部分だけが高い輝度を持ち、その両側が低い輝度となるパーティクルを発生させる。またパーティクル生成部23は、発生確率p2=0.2にて、プロファイル710で表される、白点線のブロックとともに減速標示のブロックが存在する場合に相当する、白点線のブロック及び減速標示のブロックに相当する3ヵ所で相対的に高い輝度を持つパーティクルを発生させる。そしてパーティクル生成部23は、発生確率p3=0.5にて、プロファイル720で表される、減速標示のブロックのみが存在する場合に相当する、減速標示のブロックに相当する2か所で高い輝度を持つパーティクルを発生させる。なお、発生確率p1〜p3は、白点線のブロックの長さとブロック間の長さの比、及び、減速標示のブロックの長さとブロック間の長さの比に応じて、予め設定される。 FIG. 7 is a view showing an example of the particle in the case where the type of the map line segment is "white dotted line (thin) + both-side deceleration sign". In FIG. 7, the horizontal axis represents a position along the direction orthogonal to the map line segment, with the position of the white dotted line as the origin, and the vertical axis represents luminance. Profiles 700 to 720 respectively represent luminance profiles of particles to be generated. When the type of map line segment is "white dotted line (thin) + both-side deceleration marking", not only one white dotted line but also broken lines that are deceleration markings are displayed on the road on both sides of the white dotted line Therefore, depending on the position in the direction along the map line segment, the brightness profile in the direction crossing the map line segment changes depending on the presence or absence of the white dotted block and the presence or absence of the deceleration marking block. Therefore, in this case, the particle generation unit 23 is high only in the central portion corresponding to the white dotted line, which corresponds to the case where only the white dotted line block represented by the profile 700 exists at the occurrence probability p1 = 0.3. Produces particles that have luminance and low luminance on both sides. Further, the particle generation unit 23 corresponds to the block of the white dotted line and the block of the deceleration sign corresponding to the case of the block of the deceleration sign with the block of the white dotted line represented by the profile 710 with the occurrence probability p2 = 0.2. Generate particles with relatively high brightness at three locations. The particle generation unit 23 has high luminance at two locations corresponding to the block of the deceleration marking, which corresponds to the case where only the block of the deceleration marking represented by the profile 720 exists with the occurrence probability p3 = 0.5. Generate particles. The occurrence probabilities p1 to p3 are preset according to the ratio of the block length of the white dotted line to the length between the blocks and the ratio of the block length of the deceleration marking to the length between the blocks.
パーティクル生成部23は、生成した各パーティクルを投影部24へ渡す。 The particle generation unit 23 passes the generated particles to the projection unit 24.
投影部24は、各パーティクルをカメラ2により現時刻に得られた画像上へ投影する。なお、車両位置検出処理の実行周期とカメラ2による撮影周期とが同期していない場合には、カメラ2により得られた最新の画像を、現時刻に得られた画像とすればよい。その際、投影部24は、車両10の予測位置の不確実性を考慮して、予測位置を算出した際の共分散行列により表される正規分布にしたがって、パーティクルごとに予測位置及び予測方位を修正する。例えば、修正された予測位置及び予測方位μq ti=(xq ti,zq ti,θq ti)は次式で表される。
投影部24は、各パーティクルについて、修正された予測位置及び予測方位に基づいてそのパーティクルの車両座標系での位置を次式に従って算出する。
投影部24は、車両座標系上の位置で表された各パーティクルを(5)式にしたがって、時刻tにおけるカメラ2により得られた画像上に投影する。 The projection unit 24 projects each particle represented by the position on the vehicle coordinate system onto the image obtained by the camera 2 at time t according to the equation (5).
図8は、各パーティクルが投影された画像の一例を示す図である。図8に示された画像800において、道路上の白線801〜803のそれぞれごとに、その白線と交差するように複数のパーティクル810が投影されている。なお、この例では、パーティクル生成部23は、1本の白線ごとに、最大で1000個のパーティクルを発生させている。パーティクル810によって、白線と直交する方向の位置及び白線となす角が少しずつ変化していることが分かる。なお、各パーティクル810において、相対的に白い部分は輝度が相対的に低く、相対的に黒い部分が輝度が相対的に高いことを表す。 FIG. 8 is a view showing an example of an image on which each particle is projected. In the image 800 shown in FIG. 8, for each of the white lines 801 to 803 on the road, a plurality of particles 810 are projected so as to intersect the white lines. In this example, the particle generation unit 23 generates up to 1000 particles for each white line. It can be seen from the particle 810 that the position in the direction orthogonal to the white line and the angle formed with the white line change little by little. In each particle 810, a relatively white portion indicates relatively low luminance, and a relatively black portion indicates relatively high luminance.
投影部24は、画像上に投影された各パーティクルを選択部25にわたす。 The projection unit 24 passes each particle projected on the image to the selection unit 25.
選択部25は、画像上に投影された各パーティクルについて、画像との一致度合を表す評価値を算出する。そして選択部25は、評価値が所定の一致条件を満たすパーティクルを選択する。 The selection unit 25 calculates, for each of the particles projected on the image, an evaluation value representing the degree of coincidence with the image. Then, the selection unit 25 selects particles whose evaluation value satisfies a predetermined match condition.
例えば、選択部25は、各パーティクルについて、そのパーティクルの輝度プロファイル上で所定数(例えば、50点)の位置の輝度値と画像の対応位置の輝度値との間での正規化相互相関値を評価値として算出する。そして選択部25は、評価値が所定の閾値(例えば、0.8)以上となるパーティクルを、一致条件を満たすとして選択する。なお、画像の画質が悪くても一定数以上のパーティクルが選択されるように、所定の閾値は、例えば、評価値の最大値に1未満の所定数(例えば、0.8)を乗じて得られた値としてもよい。 For example, for each particle, the selection unit 25 calculates a normalized cross-correlation value between the luminance value at a predetermined number (for example, 50 points) on the luminance profile of the particle and the luminance value at the corresponding position of the image. Calculated as an evaluation value. Then, the selection unit 25 selects a particle whose evaluation value is equal to or more than a predetermined threshold value (for example, 0.8) as the matching condition is satisfied. The predetermined threshold value is obtained by, for example, multiplying the maximum evaluation value by a predetermined number less than 1 (for example, 0.8) so that a certain number of particles or more are selected even if the image quality is poor. It may be a value.
あるいは、選択部25は、評価値が最も高い方から順に所定数(例えば、生成されたパーティクルの総数の1/5〜1/10)のパーティクルを、一致条件を満たすとして選択してもよい。 Alternatively, the selection unit 25 may select a predetermined number of particles (for example, 1/5 to 1/10 of the total number of generated particles) in order from the highest evaluation value as the matching condition.
なお、選択部25は、各パーティクルについて、正規化相互相関値を算出する代わりに、誤差二乗和(SSD)あるいは誤差絶対値和(SAD)を評価値として算出してもよい。この場合には、選択部25は、評価値が所定の閾値以下となるパーティクルを、一致条件を満たすとして選択する。 The selection unit 25 may calculate an error sum of squares (SSD) or an error absolute sum (SAD) as an evaluation value instead of calculating the normalized cross-correlation value for each particle. In this case, the selecting unit 25 selects a particle whose evaluation value is equal to or less than a predetermined threshold value as the matching condition is satisfied.
選択部25は、選択された各パーティクルについて、位置の検出誤差を推定する。i番目のパーティクルについての位置の検出誤差は、例えば、画像上でのカメラ2の結像光学系の歪曲収差による誤差成分、あるいは、画素単位で区切られることによる誤差成分を含み、共分散行列RI iで表されるものとする。共分散行列RI iは、画像の中心、すなわち、カメラ2の光軸から離れるほど2次で大きくなると仮定される場合、例えば、次式で表される。
選択部25は、選択された各パーティクルについて、画像上での検出誤差の共分散行列を、車両座標系での共分散行列に変換する。この場合、選択部25は、車両座標系でのパーティクルの位置qiにおける共分散行列Rq iは、画像上での対応する検出誤差の共分散行列RI iを鳥瞰変換したものとして算出できる。この鳥瞰変換は、例えば、次式で表される。
図9は、車両座標系における、選択された各パーティクルについての共分散行列Rq iに相当する誤差楕円の一例を表す図である。図9において、横軸は、車両10の進行方向と直交する方向の位置を表し、縦軸は車両10の進行方向に沿った、車両10からの距離を表す。なお、上側ほど、車両10から離れているとする。個々の楕円901が、それぞれ、一つのパーティクルに対応する共分散行列Rq iに相当する。また個々の楕円901の中心の点902は、対応するパーティクルの中心位置を表す。図9に示されるように、パーティクルの位置が車両10から離れるほど誤差楕円901は大きくなることが分かる。 FIG. 9 is a diagram illustrating an example of an error ellipse corresponding to the covariance matrix R q i for each selected particle in the vehicle coordinate system. In FIG. 9, the horizontal axis represents the position in the direction orthogonal to the traveling direction of the vehicle 10, and the vertical axis represents the distance from the vehicle 10 along the traveling direction of the vehicle 10. In addition, it is assumed that the upper side is away from the vehicle 10. Each ellipse 901 corresponds to a covariance matrix R q i corresponding to one particle. The center point 902 of each ellipse 901 represents the center position of the corresponding particle. As shown in FIG. 9, it can be seen that the error ellipse 901 becomes larger as the position of the particle moves away from the vehicle 10.
選択部25は、選択された各パーティクルについて、そのパーティクルの中心位置の車両座標系における位置qi及び共分散行列Rq iと、対応する地図線分の番号を位置更新部26へわたす。 The selection unit 25 transmits the position q i and the covariance matrix R q i in the vehicle coordinate system of the center position of the selected particle and the corresponding map line number to the position update unit 26 for each of the selected particles.
位置更新部26は、直近の過去の1以上の時刻における車両10の推定位置と、現時刻における、選択された各パーティクルに基づいて、現時刻における車両10の位置を推定する。 The position update unit 26 estimates the position of the vehicle 10 at the current time based on the estimated position of the vehicle 10 at the latest one or more times and the selected particles at the current time.
例えば、位置更新部26は、次式に従って評価値Eを算出し、その評価値Eが最小となるように、直近の過去の1以上の時刻及び現時刻における車両の推定位置を算出する。
また、右辺の第2項におけるδI(μcj,qj,χrj)は、時刻cj(すなわち、選択されたj番目のパーティクルに対応する時刻であり、時刻τs〜τeの何れか)の車両10の推定位置μcjにおける、j番目(j∈{1,...,I}、Iは、時刻τs〜τeにおいて選択されたパーティクルの総数)の地図線分rjについてのパーティクルqjを、推定位置μcjを用いて地図線分rjに投影したときの地図座標系における射影誤差を表す。すなわち、第2項は、選択されたパーティクルについての射影誤差の二乗和を表す。なお、cjは、番号jに対応する時刻を表す。またRδ jは、地図座標系での誤差の共分散行列を表す。射影誤差δI(μcj,qj,χrj)及び共分散行列Rδ jは、次式で表される。
位置更新部26は、評価値Eが最小となるように、各時刻の車両10の推定位置{μτs,...,μτe}を算出する。すなわち、(11)式において、第1項のμt及び第2項μcjが変数となる。その際、位置更新部26は、例えば、勾配法、レーベンバーグマーカート法、あるいはニュートン法を(11)式に適用することで各時刻の車両10の推定位置{μτs,...,μτe}を算出できる。あるいは、位置更新部26は、RANSAC法またはM推定といったロバストな推定法を(11)式に適用して、各時刻の車両10の推定位置{μτs,...,μτe}を算出してもよい。これにより、評価値Eが多峰性を持つパーティクルによる局所解とならないように、位置更新部26は、評価値Eの最小値をもとめることができる。 The position updating unit 26 calculates the estimated position {μ τs ,..., Μ τe } of the vehicle 10 at each time so that the evaluation value E is minimized. That is, in the equation (11), the first term μ t and the second term μ cj are variables. At that time, the position update unit 26 applies, for example, the gradient method, the Levenberg-Markert method, or the Newton method to the equation (11) to estimate the estimated position {μ τs,. We can calculate τ e }. Alternatively, the position update unit 26 calculates the estimated position {μ τs ,..., Μ τe } of the vehicle 10 at each time by applying the RANSAC method or a robust estimation method such as M estimation to equation (11). May be As a result, the position updating unit 26 can obtain the minimum value of the evaluation value E so that the evaluation value E does not become a local solution by particles having multimodality.
位置更新部26は、求めた各時刻の車両10の推定位置{μτs,...,μτe}を記憶部41に保存する。 The position update unit 26 stores the estimated position {μ τs ,..., Μ τe } of the vehicle 10 at each time obtained in the storage unit 41.
図10は、車両位置検出処理の動作フローチャートである。車両位置検出システム1は、所定の周期ごとに、例えば、カメラ2の撮影周期ごとに、以下に示す動作フローチャートに従って自車両の位置を推定する。 FIG. 10 is an operation flowchart of the vehicle position detection process. The vehicle position detection system 1 estimates the position of the host vehicle according to the operation flowchart shown below, for example, for each shooting cycle of the camera 2 at predetermined cycles.
予測位置算出部22は、前時刻(t-1)における自車両の位置とオドメトリ情報に基づいて、現時刻tにおける自車両の予測位置を算出する(ステップS101)。なお、前時刻(t-1)における自車両の位置は、現時刻tにおいて行われる車両位置検出処理が初回であれば、初期値設定部21により設定された自車両の位置の初期値である。一方、前時刻(t-1)において、既に車両位置検出処理が行われている場合には、前時刻(t-1)において位置更新部26により求められた自車両の位置である。 The predicted position calculation unit 22 calculates the predicted position of the host vehicle at the current time t based on the position of the host vehicle at the previous time (t-1) and the odometry information (step S101). The position of the host vehicle at the previous time (t-1) is the initial value of the position of the host vehicle set by the initial value setting unit 21 when the vehicle position detection process performed at the current time t is the first process. . On the other hand, when the vehicle position detection processing has already been performed at the previous time (t-1), the position of the host vehicle is obtained by the position update unit 26 at the previous time (t-1).
パーティクル生成部23は、その予測位置から所定の範囲内、かつ、カメラ2の撮影範囲内含まれる地図線分を、地図情報に基づいて抽出する(ステップS102)。そしてパーティクル生成部23は、抽出した地図線分ごとに、その地図線分の種類に応じた、地図線分を横切る方向の輝度プロファイルを持つパーティクルを地図線分に沿って複数発生させる(ステップS103)。 The particle generation unit 23 extracts, based on the map information, map line segments included in a predetermined range from the predicted position and within the imaging range of the camera 2 (step S102). Then, the particle generation unit 23 generates, for each of the extracted map line segments, a plurality of particles having a luminance profile in a direction crossing the map line segment according to the type of the map line segment along the map line segment (step S103). ).
投影部24は、各パーティクルを、車両10の予測位置の誤差による位置の揺らぎを加味して、カメラ2により得られた現時刻における画像上に投影する(ステップS104)。 The projection unit 24 projects each particle onto the image at the current time obtained by the camera 2 in consideration of the fluctuation of the position due to the error of the predicted position of the vehicle 10 (step S104).
選択部25は、画像上に投影された各パーティクルについて、そのパーティクルと画像との一致度合を表す評価値を算出し、その評価値が所定の一致条件を満たすパーティクルを選択する(ステップS105)。そして選択部25は、選択したパーティクルのそれぞれについて、車両座標系での検出誤差を表す共分散行列を算出する(ステップS106)。 The selection unit 25 calculates, for each particle projected on the image, an evaluation value representing the degree of coincidence between the particle and the image, and selects a particle whose evaluation value satisfies a predetermined coincidence (step S105). Then, the selection unit 25 calculates, for each of the selected particles, a covariance matrix representing a detection error in the vehicle coordinate system (step S106).
位置更新部26は、過去の複数の時刻における推定位置とオドメトリ情報に基づく車両の予測位置間の誤差の総和と、検出誤差に基づいて算出される、選択された各パーティクルの射影誤差の総和との和に基づく評価値Eが最小となるように、各時刻の車両の推定位置を更新する(ステップS107)。そして制御部43は、車両位置検出処理を終了する。 The position updating unit 26 sums the errors between the estimated positions at a plurality of times in the past and the predicted positions of the vehicle based on the odometry information, and the sum of projection errors of the selected particles calculated based on the detection errors. The estimated position of the vehicle at each time is updated so as to minimize the evaluation value E based on the sum of (step S107). And the control part 43 complete | finishes a vehicle position detection process.
以上説明してきたように、車両位置検出システムは、テンプレートマッチングのように、繰り返し試行する処理を伴わずに自車両の位置を検出できる。そのため、この車両位置検出システムは、処理負荷を抑制できる。またこの車両位置検出システムは、車両の周囲の道路上に標示された線を横切る方向の輝度プロファイルを持つ複数のパーティクルを生成して実際に撮影された画像上に投影し、その画像との一致度合が良好なパーティクルを利用して、自車両の位置を更新するので、自車両の位置を正確に検出できる。さらに、この車両位置検出システムは、地図情報を参照して、車両の周囲の道路上に標示された線の種類に応じてパーティクルを生成するので、道路に実際に標示されている線と一致する可能性が高いパーティクルを生成できる。そのため、この車両位置検出システムは、車両位置の推定に有用なパーティクルの数を増やすことができるので、車両位置の推定精度を向上できる。 As described above, the vehicle position detection system can detect the position of the host vehicle without the process of repeatedly trying like template matching. Therefore, this vehicle position detection system can suppress processing load. The vehicle position detection system also generates a plurality of particles having a luminance profile in the direction crossing the line marked on the road around the vehicle, projects it onto the actually captured image, and matches it with the image Since the position of the vehicle is updated using particles with a good degree, the position of the vehicle can be accurately detected. Furthermore, the vehicle position detection system refers to the map information and generates particles according to the type of line marked on the road around the vehicle, so that it matches the line actually marked on the road Probable particles can be generated. Therefore, since this vehicle position detection system can increase the number of particles useful for estimating the vehicle position, it is possible to improve the estimation accuracy of the vehicle position.
なお、変形例によれば、パーティクル生成部23は、パーティクル生成対象となる地図線分のそれぞれについて、線の太さ、あるいは、線間の間隔を互いに異ならせたパーティクルを生成してもよい。これは、道路上に標示される線(白線、減速線など)の太さが道路規格で定めされた範囲内で変動する可能性が有ること、及び、車線に対するカメラ2の向きにより、画像上でのその線の太さが変化する可能性があるためである。例えば、パーティクル生成部23は、着目する地図線分に含まれる、道路上に標示される線の幅の変動が正規分布に従うと仮定して、その正規分布に従って線の幅あるいは線間の間隔を変えたパーティクルを複数生成すればよい。これにより、パーティクル生成部23は、車両10が実際に走行している位置での道路上に標示される線と一致するパーティクルの数を増やすことができる。 Note that according to the modification, the particle generation unit 23 may generate particles in which the line thickness or the interval between the lines are different from each other for each of the map line segments to be particle generation targets. This is because the thickness of the line (white line, deceleration line, etc.) marked on the road may fluctuate within the range defined by the road standard, and the orientation of the camera 2 with respect to the lane This is because the thickness of the line may change. For example, assuming that the variation of the width of the line indicated on the road included in the map line segment of interest follows a normal distribution, the particle generation unit 23 determines the width of the lines or the interval between the lines according to the normal distribution. A plurality of changed particles may be generated. Thereby, the particle generation unit 23 can increase the number of particles that match the line marked on the road at the position where the vehicle 10 is actually traveling.
他の変形例によれば、パーティクル生成部23は、車両10の左側に位置する地図線分と右側に位置する地図線分とを一つの組として、その組ごとにパーティクルを生成してもよい。これにより、道路上に描画されている線の一部の形状が、道路上の補修痕、あるいは、摩耗などによって本来の形状から変化している場合でも、誤って選択される可能性が低いパーティクルを生成できる。またこの場合、車両10の左側に位置する地図線分と右側に位置する地図線分間の間隔は、車線に対するカメラ2の向き、あるいはキャリブレーションの誤差などにより変化することがある。そこで上記の変形例と同様に、パーティクル生成部23は、車両10の左側に位置する地図線分と右側に位置する地図線分間の間隔の変動が正規分布に従うと仮定して、その正規分布に従って間隔を変えたパーティクルを複数生成すればよい。 According to another modification, the particle generation unit 23 may generate a particle for each set of the map line segment located on the left side of the vehicle 10 and the map line segment located on the right side as one set. . As a result, even if the shape of a part of the line drawn on the road is changed from the original shape due to a repair mark on the road or abrasion, etc., there is a low possibility of being selected erroneously. Can be generated. Further, in this case, the distance between the map line segment located on the left side of the vehicle 10 and the map line segment located on the right side may change due to the orientation of the camera 2 with respect to the lane or an error in calibration. Therefore, as in the above modification, the particle generation unit 23 follows the normal distribution on the assumption that the variation of the interval between the map segment located on the left side of the vehicle 10 and the map line located on the right follows the normal distribution. A plurality of particles with different intervals may be generated.
また他の変形例によれば、パーティクル生成部23は、道路の路面に対して平行な面状のパーティクルを生成してもよい。 Further, according to another modification, the particle generation unit 23 may generate planar particles parallel to the road surface of the road.
図11は、この変形例により生成されるパーティクルと道路上に標示される線との関係の一例を示す図である。図11では、道路上に標示される線1100は、鋸波状の形状を有している。そして縦軸は、線1100の延伸方向を表し(縦軸の位置は、延伸方向と直交する方向における線1100の重心位置に相当)、横軸は、線1100の延伸方向と直交する方向を表す。この例では、線1100の延伸方向に沿って、その延伸方向と直交する方向に沿った輝度変化を持つプロファイルを持つ3個の線の組が一つのパーティクル1101、1102として生成される。なお、図11では、パーティクル1101、1102を構成する個々の線において、相対的に明るい部分は相対的に低い輝度を持ち、相対的に暗い部分は相対的に高い輝度を持つ。そして各線において、輝度が高い部分は、線1100の対応部分に応じて設定される。このようなパーティクルを生成することで、車両位置検出システムは、道路上に標示される線が複雑な形状を有している場合でも、その線と一致するパーティクルを適切に選択できるので、車両10の位置を推定できる。 FIG. 11 is a diagram showing an example of the relationship between particles generated by this modification and lines marked on the road. In FIG. 11, the line 1100 marked on the road has a sawtooth shape. The vertical axis represents the extending direction of the line 1100 (the position of the vertical axis corresponds to the center of gravity of the line 1100 in the direction orthogonal to the extending direction), and the horizontal axis represents the direction orthogonal to the extending direction of the line 1100. . In this example, a set of three lines having a profile having a luminance change along a direction perpendicular to the extending direction of the line 1100 is generated as one particle 1101, 1102. In FIG. 11, in the individual lines constituting the particles 1101 and 1102, a relatively bright part has a relatively low luminance, and a relatively dark part has a relatively high luminance. Then, in each line, a portion with high luminance is set according to the corresponding portion of line 1100. By generating such particles, the vehicle position detection system can appropriately select the particles coinciding with the line even if the line marked on the road has a complicated shape. Can be estimated.
さらに他の変形例によれば、カメラ2がカラー画像を生成する場合には、パーティクル生成部23は、地図線分の種類に応じて、パーティクルにおける、道路上に標示される線に相当する部分の色をその線の色に応じて設定してもよい。あるいは、パーティクル生成部23は、道路上に標示される線の色の確率分布に従って、パーティクルにおける、道路上に標示される線に相当する部分の色を設定してもよい。 According to still another modified example, when the camera 2 generates a color image, the particle generation unit 23 corresponds to the part of the particle corresponding to the line marked on the road according to the type of the map line segment. May be set according to the color of the line. Alternatively, the particle generation unit 23 may set the color of the portion of the particle corresponding to the line marked on the road according to the probability distribution of the color of the line marked on the road.
さらに他の変形例によれば、カメラ2はステレオカメラであってもよい。この場合には、投影部24は、各パーティクルを、例えば、ステレオカメラにより得られた左右の画像のそれぞれに投影する。また選択部25は、例えば、ステレオカメラにより得られた左右の画像のそれぞれから路面に相当する路面領域を判定する。そして選択部25は、左右の画像の何れかにおいて、パーティクルの一部が路面領域に含まれないパーティクルを選択しない。これにより、選択部25は、例えば、先行車両などに隠れた道路上の線に対応するパーティクルを、車両10の位置の推定から排除できるので、車両10の位置の推定精度をより向上できる。なお、選択部25は、路面領域の判定方法として、例えば、関他、「ステレオ動画像を用いた車両の前方環境認識」、情報処理学会研究報告コンピュータビジョンとイメージメディア(CVIM)2007、1-16、2007年などに開示された方法を利用できる。 According to yet another variant, the camera 2 may be a stereo camera. In this case, the projection unit 24 projects each particle, for example, on each of the left and right images obtained by the stereo camera. Further, the selection unit 25 determines, for example, a road surface area corresponding to the road surface from each of the left and right images obtained by the stereo camera. Then, the selection unit 25 does not select particles in which a part of the particles is not included in the road surface area in any of the left and right images. Thereby, since the selection part 25 can exclude the particle | grains corresponding to the line on the road hidden in the preceding vehicle etc. from estimation of the position of the vehicle 10, the estimation accuracy of the position of the vehicle 10 can be improved more. In addition, as a method of determining the road surface area, the selection unit 25 may, for example, perform a "forward environment recognition of a vehicle using a stereo moving image", Information Processing Society research report computer vision and image media (CVIM) 2007, 1- 16, the method disclosed in 2007, etc. can be used.
また、選択部25は、各パーティクルについて、左右それぞれの画像についての評価値のうちの小さい方(評価値として正規化相互相関値を利用する場合、評価値としてSSDまたはSADを利用する場合には、左右それぞれの画像についての評価値のうちの大きい方)、あるいは、評価値の平均値が所定の一致条件を満たすものを選択してもよい。この変形例によれば、着目するパーティクルが投影された位置が先行車両により隠されるオクルージョンが発生している場合、その投影された位置と重なる先行車両上の位置は、左右それぞれの画像において異なるため、その先行車両において2か所以上、着目するパーティクルと類似するパターンが存在しない限り、そのパーティクルは選択されないことになる。したがって、ロバスト性が向上する。 In addition, for each particle, the selection unit 25 is the smaller of the evaluation values for the left and right images (when using a normalized cross-correlation value as the evaluation value, when using SSD or SAD as the evaluation value, , Or the larger one of the evaluation values of the left and right images) or the average value of the evaluation values may be selected to satisfy a predetermined matching condition. According to this modification, when an occlusion occurs in which the position where the focused particle is projected is hidden by the preceding vehicle, the position on the leading vehicle overlapping the projected position is different in each of the left and right images. The particle is not selected unless there is a pattern similar to the particle of interest at two or more places in the preceding vehicle. Therefore, robustness is improved.
なお、カメラ2がステレオカメラである場合も、位置更新部26は、(11)式で表される評価値Eが最小となるように、各時刻における車両10の推定位置を算出すればよい。 Even when the camera 2 is a stereo camera, the position updating unit 26 may calculate the estimated position of the vehicle 10 at each time so that the evaluation value E represented by equation (11) is minimized.
さらに他の変形例によれば、パーティクル生成部23は、直前の時刻において選択されたパーティクルに基づいて、現時刻のパーティクルを発生させてもよい。この場合、パーティクル生成部23は、一般的なパーティクルフィルタに従って、現時刻のパーティクルを発生させればよい。これにより、パーティクル生成部23は、生成するパーティクルの数を減らすことができる。すなわち、選択部25による評価対象となるパーティクルの数を減らすことができる。 According to still another modification, the particle generation unit 23 may generate a particle at the current time based on the particle selected at the immediately preceding time. In this case, the particle generation unit 23 may generate particles at the current time according to a general particle filter. Thereby, the particle generation unit 23 can reduce the number of generated particles. That is, the number of particles to be evaluated by the selection unit 25 can be reduced.
さらに他の変形例によれば、位置更新部26は、要求される精度によっては、(11)式における第1項を省略し、第2項のみに基づいて、評価値Eが最小となるように、現時刻における車両10の位置を推定してもよい。この場合には、位置更新部26は、第2項においても、現時刻において選択されたパーティクルのみに基づいて第2項を算出してもよい。これにより、位置更新部26は、演算量を削減できる。 According to still another modification, the position update unit 26 omits the first term in the equation (11) depending on the required accuracy, and the evaluation value E is minimized based on only the second term. In addition, the position of the vehicle 10 at the current time may be estimated. In this case, also in the second term, the position updating unit 26 may calculate the second term based on only the particles selected at the current time. Thereby, the position update part 26 can reduce the amount of calculations.
上記の実施形態または変形例による車両位置検出システムから出力された自車両の位置は、例えば、CAN3を介して運転支援システムの制御回路(図示せず)へ送信される。運転支援システムの制御回路は、例えば、自車両の位置とその周囲の情報とを比較して、自車両から所定距離範囲内に特定の構造物(例えば、高速道路の料金所、ナビゲーション中の経路において左折または右折が必要な交差点など)が有れば、車内に設置されたディスプレイまたはスピーカを介して、その構造物が近いことをドライバに通知する。あるいは、運転支援システムの制御回路は、ECU11に、速度を落とす命令を出力してもよい。 The position of the host vehicle output from the vehicle position detection system according to the above embodiment or modification is transmitted to a control circuit (not shown) of the driving support system via, for example, the CAN 3. The control circuit of the driving support system compares, for example, the position of the host vehicle and information around the host vehicle, and detects a specific structure (for example, a toll booth on a highway, a route during navigation) within a predetermined distance from the host vehicle. If there is an intersection that requires a left turn or a right turn, etc., the driver is notified that the structure is near via a display or a speaker installed in the car. Alternatively, the control circuit of the driving support system may output an instruction to reduce the speed to the ECU 11.
以上のように、当業者は、本発明の範囲内で、実施される形態に合わせて様々な変更を行うことができる。 As described above, those skilled in the art can make various modifications to the embodiment to be implemented within the scope of the present invention.
1 車両位置検出システム
2 カメラ
3 コントロールエリアネットワーク(CAN)
4 コントローラ(車両位置検出装置)
10 車両
11 ECU
41 記憶部
42 通信部
43 制御部
21 初期値設定部
22 予測位置算出部
23 パーティクル生成部
24 投影部
25 選択部
26 位置更新部
1 Vehicle position detection system 2 Camera 3 Control area network (CAN)
4 Controller (vehicle position detection device)
10 vehicles 11 ECU
41 storage unit 42 communication unit 43 control unit 21 initial value setting unit 22 predicted position calculation unit 23 particle generation unit 24 projection unit 25 selection unit 26 position update unit
Claims (7)
車両(10)の直前の位置から推定される当該車両の現時刻における予測位置に応じて前記道路上に標示される線を前記地図情報から抽出し、抽出した線のそれぞれについて、当該線の種類に応じて、当該線を横切る方向の輝度変化を表すパーティクルを、当該線に沿った方向に応じた位置を変えながら複数生成するパーティクル生成部(23)と、
前記抽出した線のそれぞれについての前記複数のパーティクルのそれぞれについて、前記予測位置を想定される誤差範囲内で変化させながら前記車両(10)を基準とする位置を求め、当該位置にしたがって前記車両(10)に搭載された撮像部(2)により得られた現時刻の画像に投影する投影部(24)と、
前記抽出した線のそれぞれについての前記複数のパーティクルのうち、前記画像との一致度合が所定の一致条件を満たすパーティクルを選択する選択部(25)と、
前記選択されたパーティクルのそれぞれを、現時刻における前記車両(10)の推定位置に基づいて対応する前記線に射影したときの誤差の総和に応じて前記推定位置を求める位置更新部(26)と、
を有することを特徴とする車両位置検出装置。 A storage unit (41) for storing a position of a line marked on the road and map information representing the type of the line;
The line labeled on the road is extracted from the map information according to the predicted position of the vehicle at the current time estimated from the position immediately before the vehicle (10), and the type of the line is extracted for each of the extracted lines And a particle generation unit (23) that generates a plurality of particles representing a change in luminance in a direction crossing the line according to the change of the position according to the direction along the line.
For each of the plurality of particles for each of the extracted lines, a position based on the vehicle (10) is determined while changing the predicted position within an assumed error range, and the vehicle (according to the position ( 10) a projection unit (24) for projecting the image at the current time obtained by the imaging unit (2) mounted on
A selection unit (25) for selecting, among the plurality of particles for each of the extracted lines, a particle whose coincidence degree with the image satisfies a predetermined coincidence condition;
A position update unit (26) for obtaining the estimated position according to the sum of errors when each of the selected particles is projected onto the corresponding line based on the estimated position of the vehicle (10) at the current time ,
A vehicle position detection device characterized by having.
前記選択部(25)は、前記二つのカメラのうちの一方により現時刻に得られた第1の画像上で前記一致度合が前記一致条件を満たし、かつ、前記二つのカメラのうちの他方により現時刻に得られた第2の画像上で前記一致度合が前記一致条件を満たすパーティクルを選択する、請求項1〜4の何れか一項に記載の車両位置検出装置。 The imaging unit (2) is a stereo camera including two cameras,
The selection unit (25) is configured such that the degree of coincidence satisfies the coincidence condition on the first image obtained at the current time by one of the two cameras, and by the other of the two cameras. The vehicle position detection device according to any one of claims 1 to 4, wherein a particle whose coincidence degree satisfies the coincidence condition is selected on a second image obtained at the current time.
前記抽出した線のそれぞれについての前記複数のパーティクルのそれぞれについて、前記予測位置を想定される誤差範囲内で変化させながら前記車両(10)を基準とする位置を求め、当該位置にしたがって前記車両(10)に搭載された撮像部(2)により得られた現時刻の画像に投影するステップと、
前記抽出した線のそれぞれについての前記複数のパーティクルのうち、前記画像との一致度合が所定の一致条件を満たすパーティクルを選択するステップと、
前記選択されたパーティクルのそれぞれを、現時刻における前記車両(10)の推定位置に基づいて対応する前記線に射影したときの誤差の総和に応じて前記推定位置を求めるステップと、
を含むことを特徴とする車両位置検出方法。 According to the predicted position of the vehicle at the current time estimated from the position immediately before the vehicle (10), a line labeled on the road is extracted from the map information, and each of the extracted lines is represented in the map information Generating a plurality of particles representing a change in luminance in a direction crossing the line according to the type of the line, changing a plurality of positions according to the direction along the line;
For each of the plurality of particles for each of the extracted lines, a position based on the vehicle (10) is determined while changing the predicted position within an assumed error range, and the vehicle (according to the position ( Projecting the image at the current time obtained by the imaging unit (2) mounted on 10);
Selecting a particle satisfying a predetermined matching condition with a degree of matching with the image from the plurality of particles for each of the extracted lines;
Obtaining the estimated position according to the sum of errors when each of the selected particles is projected onto the corresponding line based on the estimated position of the vehicle (10) at the current time;
A vehicle position detection method comprising:
前記抽出した線のそれぞれについての前記複数のパーティクルのそれぞれについて、前記予測位置を想定される誤差範囲内で変化させながら前記車両(10)を基準とする位置を求め、当該位置にしたがって前記車両(10)に搭載された撮像部(2)により得られた現時刻の画像に投影するステップと、
前記抽出した線のそれぞれについての前記複数のパーティクルのうち、前記画像との一致度合が所定の一致条件を満たすパーティクルを選択するステップと、
前記選択されたパーティクルのそれぞれを、現時刻における前記車両(10)の推定位置に基づいて対応する前記線に射影したときの誤差の総和に応じて前記推定位置を求めるステップと、
を前記車両(10)に搭載されたプロセッサ(43)に実行させる命令を含むことを特徴とする車両位置検出用コンピュータプログラム。 According to the predicted position of the vehicle at the current time estimated from the position immediately before the vehicle (10), a line labeled on the road is extracted from the map information, and each of the extracted lines is represented in the map information Generating a plurality of particles representing a change in luminance in a direction crossing the line according to the type of the line, changing a plurality of positions according to the direction along the line;
For each of the plurality of particles for each of the extracted lines, a position based on the vehicle (10) is determined while changing the predicted position within an assumed error range, and the vehicle (according to the position ( Projecting the image at the current time obtained by the imaging unit (2) mounted on 10);
Selecting a particle satisfying a predetermined matching condition with a degree of matching with the image from the plurality of particles for each of the extracted lines;
Obtaining the estimated position according to the sum of errors when each of the selected particles is projected onto the corresponding line based on the estimated position of the vehicle (10) at the current time;
A computer program for vehicle position detection, comprising: an instruction for causing a processor (43) mounted on the vehicle (10) to execute the program.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2016073531A JP6552448B2 (en) | 2016-03-31 | 2016-03-31 | Vehicle position detection device, vehicle position detection method, and computer program for vehicle position detection |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2016073531A JP6552448B2 (en) | 2016-03-31 | 2016-03-31 | Vehicle position detection device, vehicle position detection method, and computer program for vehicle position detection |
Publications (2)
Publication Number | Publication Date |
---|---|
JP2017181476A JP2017181476A (en) | 2017-10-05 |
JP6552448B2 true JP6552448B2 (en) | 2019-07-31 |
Family
ID=60004269
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2016073531A Active JP6552448B2 (en) | 2016-03-31 | 2016-03-31 | Vehicle position detection device, vehicle position detection method, and computer program for vehicle position detection |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP6552448B2 (en) |
Families Citing this family (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP6735717B2 (en) | 2017-09-21 | 2020-08-19 | 栗田工業株式会社 | Steam heating efficiency improvement method and papermaking method |
US11450026B2 (en) | 2017-12-27 | 2022-09-20 | Sony Corporation | Information processing apparatus, information processing method, and mobile object |
JP6601516B2 (en) | 2018-02-15 | 2019-11-06 | 栗田工業株式会社 | Method for improving heating efficiency by steam and paper making method |
JP7052543B2 (en) * | 2018-05-09 | 2022-04-12 | トヨタ自動車株式会社 | Vehicle position estimation device |
CN114323035A (en) * | 2020-09-30 | 2022-04-12 | 华为技术有限公司 | Positioning method, device and system |
CN113819918B (en) * | 2021-09-17 | 2023-05-16 | 中国第一汽车股份有限公司 | Positioning method, positioning device, electronic equipment and storage medium |
Family Cites Families (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP4397573B2 (en) * | 2002-10-02 | 2010-01-13 | 本田技研工業株式会社 | Image processing device |
JP5041165B2 (en) * | 2008-06-23 | 2012-10-03 | アイシン・エィ・ダブリュ株式会社 | GUIDANCE CONTROL DEVICE, GUIDANCE CONTROL METHOD, AND GUIDANCE CONTROL PROGRAM |
JP5471919B2 (en) * | 2010-07-14 | 2014-04-16 | 富士通株式会社 | Image processing apparatus, image processing program, image processing method, and moving body |
EP2619742B1 (en) * | 2010-09-24 | 2018-02-28 | iRobot Corporation | Systems and methods for vslam optimization |
JP5891802B2 (en) * | 2012-01-17 | 2016-03-23 | 日産自動車株式会社 | Vehicle position calculation device |
JP5867176B2 (en) * | 2012-03-06 | 2016-02-24 | 日産自動車株式会社 | Moving object position and orientation estimation apparatus and method |
BR112016010089B1 (en) * | 2013-11-13 | 2021-06-08 | Nissan Motor Co. Ltd. | moving body position estimating device and moving body position estimating method |
JP6278791B2 (en) * | 2014-03-31 | 2018-02-14 | 株式会社デンソーアイティーラボラトリ | Vehicle position detection device, vehicle position detection method, vehicle position detection computer program, and vehicle position detection system |
US9727793B2 (en) * | 2015-12-15 | 2017-08-08 | Honda Motor Co., Ltd. | System and method for image based vehicle localization |
-
2016
- 2016-03-31 JP JP2016073531A patent/JP6552448B2/en active Active
Also Published As
Publication number | Publication date |
---|---|
JP2017181476A (en) | 2017-10-05 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
TWI695181B (en) | Methods and systems for color point cloud generation | |
JP6552448B2 (en) | Vehicle position detection device, vehicle position detection method, and computer program for vehicle position detection | |
US11915099B2 (en) | Information processing method, information processing apparatus, and recording medium for selecting sensing data serving as learning data | |
CN108692719B (en) | Object detection device | |
CN111856491B (en) | Method and apparatus for determining geographic position and orientation of a vehicle | |
JP6758160B2 (en) | Vehicle position detection device, vehicle position detection method and computer program for vehicle position detection | |
JP4702569B2 (en) | Image processing apparatus for vehicle | |
WO2019208101A1 (en) | Position estimating device | |
CN105335955A (en) | Object detection method and object detection apparatus | |
US10984263B2 (en) | Detection and validation of objects from sequential images of a camera by using homographies | |
EP3324359B1 (en) | Image processing device and image processing method | |
KR101573576B1 (en) | Image processing method of around view monitoring system | |
JP6278791B2 (en) | Vehicle position detection device, vehicle position detection method, vehicle position detection computer program, and vehicle position detection system | |
JP2018048949A (en) | Object recognition device | |
JP4894771B2 (en) | Calibration apparatus and calibration method | |
JP6278790B2 (en) | Vehicle position detection device, vehicle position detection method, vehicle position detection computer program, and vehicle position detection system | |
JP2018189463A (en) | Vehicle position estimating device and program | |
KR20100066952A (en) | Apparatus for tracking obstacle using stereo vision and method thereof | |
JP2016148956A (en) | Positioning device, positioning method and positioning computer program | |
KR102195040B1 (en) | Method for collecting road signs information using MMS and mono camera | |
JP2019128153A (en) | Distance calculation device and vehicle control device | |
KR20160125803A (en) | Apparatus for defining an area in interest, apparatus for detecting object in an area in interest and method for defining an area in interest | |
JP7234840B2 (en) | position estimator | |
JP6886136B2 (en) | Alignment device, alignment method and computer program for alignment | |
KR102003387B1 (en) | Method for detecting and locating traffic participants using bird's-eye view image, computer-readerble recording medium storing traffic participants detecting and locating program |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20180731 |
|
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: 20190604 |
|
A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20190531 |
|
A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20190702 |
|
R150 | Certificate of patent or registration of utility model |
Ref document number: 6552448 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |