KR102103651B1 - Method for reduction of particle filtering degeneracy exploiting lane number from digital map and system using the method - Google Patents

Abstract

Disclosed is a method for reducing degeneracy of particle filtering. The method comprises the steps of: predicting a current state of a particle using a state transition model and a previous state of the particle; updating a weight of the particle by matching driving environment information and precision map information detected through an environment recognition sensor; and clustering the particle according to whether a feature point capable of determining a driving lane of a vehicle exists in a detection area of the sensor and performing resampling.

Description

METHOD FOR REDUCTION OF PARTICLE FILTERING DEGENERACY EXPLOITING LANE NUMBER FROM DIGITAL MAP AND SYSTEM USING THE METHOD}

An embodiment according to the concept of the present invention relates to a method for reducing particle filtering deterioration that reduces a problem of deterioration when using particle filtering for implementing sensor fusion-based precision positioning.

Accurate position estimation of vehicles is an important element required in advanced driver assistance systems (ADAS) or autonomous driving. Recently, various global positioning systems (GPS) and inertial navigation systems (INS), which have been used to estimate the position of vehicles, as well as environmental sensors such as cameras and LIDARs, etc. Methods for fusion and use of sensor information have been developed.

Among the filtering techniques that fuse various sensor information, particle filtering has been used in various fields because of its advantages that can be applied to nonlinear non-Gaussian systems. In particular, it is widely used to estimate the position of a moving object such as a vehicle or a robot.

The particle filter is an analytical method based on the Kalman Filter, that is, it is assumed to be a linear motion and does not look for parameters, but as a predictive technique through simulation based on trial and error. Also called the Sequential Monte Carlo (SMC) method. The operating principle of the particle filter is to guess the information of the system by synthesizing several randomly generated inputs with the proposed probability distribution appropriate for the system. The purpose of the particle filter is to predict information that comes in continuously from observations with errors.

Particle filtering does not need to estimate the state variable directly from the sensor data, so various sensors as well as sensors that directly output the state variable information can be utilized.

Registered Patent Publication No. 10-0809352 relates to a method and apparatus for estimating vehicle movements of a mobile robot that estimates the attitude of a mobile robot relatively accurately by adjusting weights by reflecting errors of a sensor in a grid-based slam algorithm using particle filters.

In the prior art document, a posture change amount of a mobile robot is detected to apply a posture change amount detected to a previous particle to obtain a posture of a current particle, and a posture probability prediction of a current particle from range data and map information obtained from a sensor And a configuration for obtaining a weight and resampling the current particle based on the weight to adjust the weight in consideration of the error of the sensor.

However, since particle filtering uses a finite number of particles, there is a limitation in accurately representing a high-dimensional probability distribution. Therefore, the prior art document has a problem that arises from particle degeneracy, that is, description of an incorrect probability distribution of particles.

Registered Patent Publication No. 10-0809352

The present invention has been devised to solve the above problems, and the particle filtering deterioration reduction method of the present invention aims to maintain a driving lane by utilizing the number of lanes obtained from a map.

The second object of the present invention is to use lane end points, road markings, and road signs as well as lanes for driving environment information used for location estimation.

Particle filtering deterioration reduction method of the present invention for achieving the above object is a step of predicting the current state of the particle using the state transition model and the previous state of the particle, and the driving environment information and precision detected through the environmental sensor And matching the map information to update the weight of the particles, and clustering the particles according to whether a feature point capable of determining the driving lane of the vehicle exists in the detection area of the sensor, and performing resampling.

The step of performing the resampling may include clustering particles when a feature point capable of determining the driving lane of the vehicle does not exist in the detection area of the sensor, and driving the driving road where the number of particle clusters can be obtained from a map. When the number of lanes is equal to the number of lanes, resampling is performed for each cluster to include particles in all of the lanes.

The step of performing the resampling includes resampling all particles when the number of particle clusters is not equal to the number of driving lanes of the driving road that can be obtained from the map.

The step of performing the resampling includes determining a driving lane by resampling all particles when a feature point capable of determining the driving lane of the vehicle exists in the detection area of the sensor.

In the step of updating the weight of the particles, lanes, lane ends, road markings, and road signs are used as matching information.

When the road sign is used as matching information, the direction of the road sign with respect to the driving direction of the vehicle is used as the matching information.

The vehicle position estimation device of the present invention for achieving the above object is matched with a sensor for recognizing the driving environment information of the vehicle, a map information receiving unit for receiving and outputting map information, and the driving environment information and the map information And a particle filter unit for estimating the position of the vehicle, wherein the particle filter unit travels on a driving road obtained from the map information when a feature point capable of determining the driving lane of the vehicle does not exist in the detection area of the sensor. The number of available lanes is used to estimate the location of the vehicle.

The particle filter unit performs particle clustering until a feature point capable of determining the driving lane of the vehicle appears, and maintains the number of particle clusters equal to the number of driving lanes.

When the number of particle clusters is equal to the number of driving lanes on the driving road that can be obtained from a map, resampling is performed for each cluster to keep particles in all of the driving lanes.

The particle filtering deterioration reduction method of the present invention as described above has an effect of reducing particle degeneration problems by maintaining particles in a driving lane by using the number of lanes obtained from a map when estimating the location of the vehicle.

In addition, the particle filtering deterioration reduction method of the present invention as described above uses the lane end point, road markings, and road signs as well as lanes for driving environment information used for position estimation to estimate the position of the vehicle using only lane offsets. There is an effect that can compensate for the uncertainty of the longitudinal position occurring in the case.

1 is a view for explaining particle degeneration.
2 is a view showing a direction (Φ) of a road sign with respect to a driving direction of a vehicle according to an embodiment of the present invention.
3 is a flowchart for explaining a method for alleviating particle degeneration problems according to an embodiment of the present invention.
4A to 4C are conceptual views illustrating a particle clustering process according to time according to an embodiment of the present invention.

Hereinafter, the present invention will be further described with reference to examples and drawings according to the present invention.

1 is a view showing particle degeneration. The environment-aware sensor-based location estimation method detects a static feature point (eg, a lane, a lane end point, a road sign, etc.) from the environment-aware sensor and estimates the location by matching the detected feature point with precision map information.

On roads with few feature points, there is a section in which road surface markers (RSMs) that can confirm driving lanes do not appear. If the driving lane cannot be determined, the possibility for all lanes that can be considered to be driven is maintained. Particle filtering expresses this possibility through particles, which means that particles must remain in every possible lane.

1 shows a situation in which particles are not maintained in all possible lanes due to particle degeneration. Assuming that blue represents particles, particles in the initial position (lanes 2, 3, and 4) were present in all possible lanes, but particles in lanes 2 and 3 gradually disappeared while driving. When the actual driving lane is two or three lanes, this result causes a misrecognition problem of incorrectly recognizing the driving lane.

Particle filtering estimates the current probability distribution of state variables from the measured information. Here, the probability distribution is represented by a set of M particles having a weight. That is, particle filtering is a method of extracting a plurality of samples having a predicted position and attitude of a vehicle, and estimating an optimal position and attitude of the vehicle by using a probability that each sample is an actual location and attitude of the vehicle.

를 예측하는 시간 업데이트(time update) 단계와, 측정된 정보로부터 파티클의 가중치

를 갱신하는 측정 업데이트(measurement update) 단계와, 가중치를 기반으로 현재 파티클

을 분배하는 리샘플링(resampling) 단계를 반복함으로써 최적의 상태 변수를 추정한다. Particle filtering uses the state transition model and the particle's previous state to determine the particle's current state.

Time update step to predict the, and the weight of the particles from the measured information

The measurement update step to update the and the current particle based on the weight

The optimal state variable is estimated by repeating the resampling step of distributing.

은 하기의 [수학식 1]과 같이 설정할 수 있다.Nth particle over time t to estimate vehicle position and posture

Can be set as [Equation 1] below.

[Equation 1]

Here, x and y are two-dimensional positions of the vehicle, and θ means heading indicating the attitude of the vehicle.

In the time update step , the current vehicle position and posture estimation using the vehicle's velocity v and yaw rate w obtained through the CTRV (Constant Turn Rate and Velocity) state transition model and the behavior sensor 2].

The CTRV model is a model for estimating the motion (position and posture) of a moving object such as a vehicle or a robot. The CTRV model is one of the motion models considering rotation around the Z axis, and is a model that estimates motion by assuming that the velocity and yaw rate are constant.

[Equation 2]

Here, the sample () function is a function for generating uncertainty, and σ _v and σ _w each represent standard deviations of the signal noise of the velocity and yaw rate sensors.

In the measurement update step, the weight of the particle is updated by matching the driving environment information and the precision map information detected through the environment recognition sensor. The weight represents a probability corresponding to the estimated position and posture of the moving object. In the case of a lane, which is a typical driving environment information used for position estimation, when a lane is detected by a front camera, a lateral offset of a lane to a host vehicle can be detected, which is observed when a particle position is assumed on the map. Compare with the lateral offset obtained from the information.

In this specification, the lateral offset of the lane to the host vehicle means a lateral distance between the host vehicle and the lane.

However, in general, since the lanes are drawn parallel to the driving direction, when only the lane offset is used, there is a longitudinal position uncertainty with respect to the host vehicle. The present invention uses lane endpoints, road markings, and road signs as matching information in particle filtering to compensate for longitudinal position uncertainty. Since the front camera image can be used to estimate 3D information of the lane end point and the road marking, it is compared with the lane end point and road marking observed when the particle position on the map is assumed to be owned.

However, in the case of a road sign, instead of using the relative position as matching information because the error generated in the 3D reconstruction is large, matching information of the road sign direction Φ with respect to the driving direction of the vehicle as shown in FIG. 2 Utilize as.

의 가중치

는 하기의 [수학식 3]과 같다.particle

Weight of

Is as shown in [Equation 3] below.

[Equation 3]

각각은 차선 오프셋으로부터 계산된 가중치, 차선 끝점의 상대 위치로부터 계산된 가중치, 노면표시의 상대 위치로부터 계산된 가중치, 도로 표지판의 방향으로부터 계산된 가중치를 의미하며, 가우시안 확률 분포를 활용하여 하기의 [수학식 4]와 같이 표현된다.here,

Each means the weight calculated from the lane offset, the weight calculated from the relative position of the lane end point, the weight calculated from the relative position of the road marking, and the weight calculated from the direction of the road sign. Using the Gaussian probability distribution, Equation 4].

[Equation 4]

,

,

,

각각은 파티클과 정밀 지도 정보를 통해 얻은 차선 오프셋, 차선 끝점의 상대 위치, 노면표시의 상대 위치, 도로 표지판의 방향을 의미하며,

,

,

,

각각은 환경 인식 센서로부터 측정된 차선 오프셋, 차선 끝점의 상대 위치, 노면표시의 상대 위치, 도로 표지판의 방향을 의미한다. 또한,

와

각각은 환경 인식 센서로부터 측정된 차선 오프셋과 도로 표지판의 방향 검출 잡음에 대한 표준편차이며,

와

각각은 차선 끝점의 상대 위치와 노면표시의 상대 위치 검출 공분산 행렬을 의미한다. here,

,

,

,

Each means the lane offset obtained through the particle and precision map information, the relative position of the lane end point, the relative position of the road marking, and the direction of the road sign,

,

,

,

Each means the lane offset measured from the environmental sensor, the relative position of the lane end point, the relative position of the road marking, and the direction of the road sign. In addition,

Wow

Each is the standard deviation of the lane offset measured from the environmental sensor and the direction detection noise of the road sign,

Wow

Each means a covariance matrix for detecting the relative position of the lane end point and the relative position of the road marking.

When the weights of all particles are obtained, the weights of the particles are normalized, so that the weight sum is 1.

In the resampling step, new particles are generated in proportion to the weight of the particles. This is to redistribute meaningful particles mainly to estimate the state more accurately using a limited number of particles. That is, particles with a small weight disappear and particles with a larger weight generate more new particles. For example, the low variance sampling technique is a sampling technique that arranges particles in proportion to the weight and selects particles at fixed intervals, and requires a small amount of computation.

3 is a flowchart illustrating a method for alleviating a particle degeneration problem according to an embodiment of the present invention, and FIGS. 4A to 4C are conceptual views illustrating a particle clustering process over time according to an embodiment of the present invention to be. The first graph of each of FIGS. 4A to 4C shows the particle distribution after the time update step, in particular, the first graph of FIG. 4A shows the particle distribution at the initial position, and the second graph of each of FIGS. 4A to 4C shows the result of particle clustering. Shown in different colors for each cluster, the third graph of each of FIGS. 4A to 4C shows the distribution of particles after the resampling step.

Referring to Figure 3, the state (position and posture) of the particles at the initial time (t = 1) is set (S110).

를 예측한다(S120). 환경 인식 센서를 통해 검출된 주행 환경 정보와 정밀 지도 정보 매칭을 통해 파티클의 가중치

를 갱신한다(S130).The current state of the particle using the state transition model and the previous state of the particle

Predict (S120). Weight of particles through matching of driving environment information and precision map information detected by the environmental sensor

It is updated (S130).

It is determined whether a feature point capable of determining the driving lane exists in the detection area of the environmental recognition sensor (S140). Here, the feature point capable of determining the driving lane means a feature point having a difference in detection results depending on the driving lane, such as a road sign or a road sign. In the case of the lane end point, the same is detected for each driving lane, and thus does not correspond to a feature point capable of determining the driving lane. For example, in the case of a front camera, a feature point capable of determining the driving lane is detected through an image captured from the camera.

The detection area of the environmental recognition sensor means a range (area) in which a feature point is detected from the position of the host vehicle. For example, in the case of a front camera, only feature points within 6 to 19 m in the longitudinal direction of the host vehicle can be detected.

The section in which the feature point is detected means a section (region) in which the feature point is detected from the environmental recognition sensor. For example, the section in which the feature point is detected is information obtained through the location of the feature point on the map.

For example, assuming that the detection area of the environmental recognition sensor is 20 m and there is a road sign 100 m ahead, the section where the feature point is detected from the environmental recognition sensor is from 80 m.

4C shows particle distribution in a section in which a feature point capable of determining a driving lane is detected. When a feature point capable of determining the driving lane exists in the detection area of the sensor (Yes in S140), resampling is performed for all particles without performing particle clustering. Through this, it is possible to determine the driving lane in the resampling stage.

However, if the feature point capable of determining the driving lane does not exist in the detection area of the sensor (No in S140), the particles are clustered (S150). Here, the particle clustering may use a mean-shift algorithm that detects the center and cluster of the data distribution based on the density distribution of the data.

When the number (c) of the detected particle clusters is equal to the number (m) of driving lanes on the driving road obtainable from the map (Yes in S160), resampling is performed for each cluster (S170).

4B shows a case where the number of detected particle clusters is three and the number of lanes that can be driven on the driving road is the same. Since independent resampling is performed for each cluster, particles are maintained in all driving lanes as before the resampling, even after the resampling step.

However, when the number (c) of the detected particle clusters is not the same as the number (m) of driving lanes on the driving road obtainable from the map (No in S160), resampling is performed for all particles (S180).

The first graph in FIG. 4A shows the particle distribution at the initial position. As a result of the particle clustering, since the number of detected particle clusters is 14 and not equal to the number (3) of driving lanes on the driving road, resampling is performed for all particles.

This method maintains the number of particle clusters equal to the number of driving lanes by performing particle clustering until a feature point capable of determining the driving lane appears, and particle clustering is performed in a section in which a feature point capable of determining the driving lane is detected. Re-sampling is performed on all particles without performing to determine the driving lane.

5 shows an apparatus for estimating a position of a vehicle according to an embodiment of the present invention. Referring to FIG. 5, the vehicle location estimation apparatus 10 includes an environment recognition sensor 100, a precision map information receiving unit 200, a particle filter unit 300, a feature point detection unit 500, and a vehicle location calculation unit 400 It may include. The environmental sensor 100 may be a camera or a LIDAR. The precision map information receiving unit 200 may receive and store the precision map information from the outside. The feature point detection unit 500 may receive an image or the like from the environment recognition sensor 100 and detect a static feature point such as a lane, a lane end point, or a road sign from the received image.

The particle filter unit 300 receives static feature points such as lanes, lane end points, and road signs from the feature point detector 500. The particle filter unit 300 may read the precision map information from the precision map information receiver 200 and match the feature points with the precision map information to estimate the driving lane.

When the particle filter unit 300 estimates the location of the vehicle by matching the feature points and the precise map information, the particle filter unit 300 performs particle clustering until the feature points capable of determining the driving lane appear, thereby counting the number of particle clusters and the number of driving lanes. By maintaining the same, there is an effect that can reduce the problem of particle degradation.

The vehicle location calculator 400 may estimate the location of the vehicle using the estimated driving lane.

The present invention has been described with reference to one embodiment shown in the drawings, but this is merely exemplary, and those skilled in the art will understand that various modifications and other equivalent embodiments are possible therefrom. Therefore, the true technical protection scope of the present invention should be determined by the technical spirit of the appended claims.

10; Vehicle position estimation device
100; Environmental awareness sensor
200; Precision map information receiver
300; Particle filter

Claims (9)

Predicting the current state of the particle using the state transition model and the previous state of the particle;
Updating the weight of the particle by matching the driving environment information and the precision map information detected through the environmental recognition sensor according to the following equation;
And clustering particles according to whether a feature point capable of determining a driving lane of a vehicle exists in a detection area of a sensor and performing resampling.
[Mathematics]

here,

Each means weight calculated from the lane offset, weight calculated from the relative position of the lane end point, weight calculated from the relative position of the road marking, and weight calculated from the direction of the road sign,

,

,

,

Each means the lane offset obtained through the particle and precision map information, the relative position of the lane end point, the relative position of the road marking, and the direction of the road sign,

,

,

,

Each means the lane offset measured from the environmental sensor, the relative position of the lane end point, the relative position of the road marking, and the direction of the road sign,

Wow

Each is the standard deviation of the lane offset measured from the environmental sensor and the direction detection noise of the road sign,

Wow

Each means a covariance matrix for detecting the relative position of the lane end point and the relative position of the road marking,
remind

Is normalized so that the sum is 1. According to claim 1, The step of performing the resampling,
Clustering particles when a feature point capable of determining the driving lane of the vehicle does not exist in the detection area of the sensor; And
If the number of particle clusters is the same as the number of driving lanes of the driving road that can be obtained from the map, performing resampling for each cluster to keep particles in all of the driving lanes. According to claim 2, The step of performing the resampling,
And if the number of particle clusters is not equal to the number of driving lanes of the driving road that can be obtained from a map, resampling all particles. According to claim 1, The step of performing the resampling,
And re-sampling all particles to determine a driving lane when a feature point capable of determining the driving lane of the vehicle is present in a detection area of the sensor. According to claim 1, Updating the weight of the particles,
A method for reducing particle filtering degeneration, characterized by using lanes, lane ends, road markings, and road signs as matching information. The method of claim 5,
When using the road sign as matching information, the method for reducing particle filtering deterioration, characterized in that the direction of the road sign relative to the driving direction of the vehicle is used as the matching information. A sensor for recognizing driving environment information of the vehicle;
A feature point detector for detecting a feature point from the driving environment information;
A map information receiver configured to receive and output map information; And
It includes; Particle filter unit for estimating the location of the vehicle by matching the driving environment information and the map information by the following equation:
The particle filter unit estimates the position of the vehicle by using the number of driving lanes on the driving road obtained from the map information when a feature point capable of determining the driving lane of the vehicle does not exist in the detection area of the sensor. A vehicle location estimation device.
[Mathematics]

here,

Each means weight calculated from the lane offset, weight calculated from the relative position of the lane end point, weight calculated from the relative position of the road marking, and weight calculated from the direction of the road sign,

,

,

,

Each means the lane offset obtained through the particle and precision map information, the relative position of the lane end point, the relative position of the road marking, and the direction of the road sign,

,

,

,

Each means the lane offset measured from the environmental sensor, the relative position of the lane end point, the relative position of the road marking, and the direction of the road sign,

Wow

Each is the standard deviation of the lane offset measured from the environmental sensor and the direction detection noise of the road sign,

Wow

Each means a covariance matrix for detecting the relative position of the lane end point and the relative position of the road marking,
remind

Is normalized so that the sum is 1. The method of claim 7,
The particle filter unit performs particle clustering until a feature point capable of determining a driving lane of a vehicle appears, and maintains the number of particle clusters equal to the number of driving lanes. The method of claim 8,
When the number of the particle clusters is the same as the number of driving lanes on the driving road that can be obtained from a map, the apparatus for estimating the position of a vehicle is characterized in that particles are maintained in all of the driving lanes by resampling each cluster.

Images