KR102460842B1 - Method for vehicle location estimation by sensor fusion - Google Patents

Abstract

The method for estimating the position of a vehicle by sensor fusion according to an embodiment is based on measurement data received from a sensor using a particle filter (PF) and previous state information estimated at a previous time point calculating the first state information of the vehicle, and calculating the final state information of the vehicle based on the first state information of the vehicle received from the particle filter using an unscented Kalman filter (UKF). may include

Description

Method for estimating the location of a vehicle by sensor fusion

The following description relates to a method of estimating a vehicle position using a particle filter (PF) and an unscented Kalman filter (UKF).

Estimating the location of a vehicle is a key component of autonomous vehicle operation. A Kalman filter (KF) is a recursive filter that estimates the state of a linear dynamic system based on a measurement value that includes Gaussian noise. Although the Kalman filter basically assumes the linearity of the model, in reality, many models have a nonlinear structure. Accordingly, the Unscented Kalman Filter (UKF), which modified the Kalman filter so that it can be used even for nonlinear models, has been proposed. The undirected Kalman filter may approximate non-Gaussian noise to Gaussian noise based on an unscented transform using a sigma point around the mean. An anechoic Kalman filter can properly capture non-linearity. Since the undirected Kalman filter approximates the non-Gaussian noise as a sigma point, it is easy to combine other information when a sigma point is selected and there is no need to calculate a Jacobian matrix.

The Kalman filter and the unscented Kalman filter have the basic assumption that the noise in the measurement is Gaussian. However, in practice, most noises do not have Gaussian properties. Accordingly, a particle filter (PF) is proposed to process non-Gaussian noise. Particle filters can approximate non-Gaussian properties using particles. Since particles are randomly generated, if a sufficient number is generated, the properties of the non-Gaussian noise of the measurement can be accurately expressed. However, the vehicle has limited computational resources. In other words, when creating an effective particle filter-based system model, there is a trade-off between precision and computational resources. The undirected Kalman filter has a limitation in filtering non-Gaussian noise, and since the particle filter has limited computational resources, it cannot generate more than a certain amount of particles, so it cannot accurately express the properties of non-Gaussian noise. There are also problems. Hereinafter, a sensor fusion method for effectively filtering non-Gaussian noise and increasing the accuracy of vehicle position estimation by fusing an anechoic Kalman filter and a particle filter will be described.

A method for estimating a position performed by a vehicle according to an embodiment is based on measurement data received from a sensor using a particle filter (PF) and previous state information estimated at a previous point in time. The method may include calculating first state information of the vehicle, and calculating second state information of the vehicle based on the first state information using an unscented Kalman filter (UKF).

Calculating the first state information of the vehicle of the location method performed by the vehicle according to an embodiment may include generating a predetermined number of particles, using a first prediction model of the particle filter The method may include predicting first temporary information about the state of the vehicle, and calculating a weight for each of the generated particles.

In the location method performed by a vehicle according to an embodiment, the first prediction model may be a bicycle model.

Calculating the second state information of the vehicle of the location method performed by the vehicle according to an embodiment may include generating sigma points, using a second prediction model of the undirected Kalman filter. predicting second temporary information about the state of the vehicle, predicting a measurement value of the vehicle from the second temporary information, and the first state information extracted from the particle filter, the second temporary information, and The method may include calculating the second state information of the vehicle based on the measured value.

Predicting the measured value of the vehicle from the second temporary information of the location method performed by the vehicle according to an embodiment may include extracting information about the location coordinates and yaw angle of the vehicle from the second temporary information may include the step of

The second prediction model of the location method performed by the vehicle according to an embodiment may be a constant turn rate and velocity (CTRV) model.

The first state information of the location method performed by the vehicle according to an embodiment may include information about location coordinates and orientation of the vehicle.

The second state information of the position method performed by the vehicle according to an exemplary embodiment may include a position coordinate of the vehicle, a yaw angle, and a covariance matrix for the state of the vehicle.

Calculating the second state information of the vehicle of the location method performed by the vehicle according to an embodiment comprises: Calculating the covariate matrix based on a measurement covariate matrix, a Kalman gain, and a previous covariate matrix may include steps.

The location method performed by the vehicle according to an embodiment may further include adjusting at least one of steering, speed, and acceleration of the vehicle based on the second state information.

A vehicle performing position estimation according to an embodiment includes a memory for storing a first prediction model of a particle filter and a second prediction model of an unscented Kalman filter, and measurement data received from an external sensor using the particle filter and a previous time point a processor for calculating first state information of the vehicle based on the previous state information estimated in have.

1 illustrates a bicycle model.
In FIG. 2 , a method of calculating first state information of a vehicle using a particle filter will be described.
3 shows the arrangement of vehicles and infrastructure in a global coordinate system.
4 shows particles randomly generated around a real vehicle in a global coordinate system.
5 is a description of a vehicle position estimation method by filter fusion according to an embodiment.
6 shows sigma points generated as a symmetrical area around the mean value in an anechoic Kalman filter.
7 shows a road on which a vehicle moves in a simulation.
8A shows the results of the trajectories of filters estimated on the S-shaped road.
8B shows the trajectory results of filters estimated on a straight road.
9 is a block diagram illustrating a configuration of a vehicle according to an exemplary embodiment.

Specific structural or functional descriptions of the embodiments are disclosed for purposes of illustration only, and may be changed and implemented in various forms. Accordingly, the embodiments are not limited to a specific disclosure form, and the scope of the present specification includes changes, equivalents, or substitutes included in the technical spirit.

Although terms such as first or second may be used to describe various elements, these terms should be interpreted only for the purpose of distinguishing one element from another. For example, a first component may be termed a second component, and similarly, a second component may also be termed a first component.

When a component is referred to as being “connected” to another component, it may be directly connected or connected to the other component, but it should be understood that another component may exist in between.

The singular expression includes the plural expression unless the context clearly dictates otherwise. In this specification, terms such as "comprise" or "have" are intended to designate that the described feature, number, step, operation, component, part, or combination thereof exists, and includes one or more other features or numbers, It should be understood that the possibility of the presence or addition of steps, operations, components, parts or combinations thereof is not precluded in advance.

Unless defined otherwise, all terms used herein, including technical or scientific terms, have the same meaning as commonly understood by one of ordinary skill in the art. Terms such as those defined in a commonly used dictionary should be interpreted as having a meaning consistent with the meaning in the context of the related art, and should not be interpreted in an ideal or excessively formal meaning unless explicitly defined in the present specification. does not Hereinafter, embodiments will be described in detail with reference to the accompanying drawings. Like reference numerals in each figure indicate like elements.

1 illustrates a bicycle model.

)로 표현할 수 있다. 이때, 요우잉 각도는 앞 바퀴(101)와 뒷 바퀴(102)를 잇는 가상의 직선과 x축이 이루는 각도를 의미할 수 있다. 이하에서는, 파티클 필터를 이용하여 외부 센서(sensor)로부터 수신되는 측정 데이터 및 이전 시점에서 추정된 노이즈가 포함된 이전 상태 정보에 기초하여 차량의 제1 상태 정보를 산출하는 방법에 대하여 설명한다.When estimating vehicle state information using a particle filter, a first prediction model may be used to represent the motion of the vehicle. The first prediction model may be a bicycle model. Referring to FIG. 1 , a motion of a vehicle may be expressed using a bicycle model. A vehicle can be simplified to one front wheel 101 and one rear wheel 102, and the state of the vehicle is an x-coordinate, a y-coordinate, and a yaw in a two-dimensional space. angle (yaw angle,

) can be expressed as In this case, the yaw angle may mean an angle formed by an imaginary straight line connecting the front wheel 101 and the rear wheel 102 and the x-axis. Hereinafter, a method of calculating first state information of a vehicle based on measured data received from an external sensor using a particle filter and previous state information including noise estimated at a previous point in time will be described.

In FIG. 2 , a method of calculating first state information of a vehicle using a particle filter will be described.

In step 201, an initialization process may be performed. In order to determine the position of the vehicle in global coordinates, the initial position of the vehicle must be provided. Accordingly, in one embodiment, the initial location of the vehicle may be provided using a Global Positioning System (GPS) sensor. Even if the GPS signal is not smoothly received due to multi-path and blocking issues in the vehicle, the GPS signal may provide a limited area for determining the initial location of the vehicle. When the particle filter receives a GPS signal, the particle filter may generate N particles. For example, N may represent 1 or more natural numbers.

In step 202, a process of predicting the first temporary information about the vehicle state using the first prediction model may be performed. The state of the vehicle may include information about position coordinates (x-axis coordinates, y-axis coordinates) and yaw angle of the vehicle in a two-dimensional space. Specifically, it is possible to predict the first temporary information about the state of the vehicle at time k+1 by using the previous state information including noise estimated at time k and the first prediction model. The first predictive model may be a bicycle model.

Equation 1 shows a process of predicting the first temporary information about the state of the vehicle at time k+1 by using the vehicle model and previous state information of the vehicle estimated at time k. In Equation 1, the slip angle is neglected because the movement of a vehicle within a city is generally not fast.

은 k+1 시간에서 예측된 제1 임시 정보,

는 k 시간에서의 차량의 속도,

는 k 간에서의 차량의 요우잉 속도(yaw rate)를 나타낼 수 있다.here,

is the first temporary information predicted at k+1 time,

is the speed of the vehicle at time k,

may represent a yaw rate of the vehicle between k.

In step 203, a process of calculating a weight for each of the generated N particles is described.

One or more infrastructures may be deployed outside the vehicle. Hereinafter, it is assumed that L infrastructures are disposed outside the vehicle. L may represent a natural number of 1 or more.

The vehicle may receive distance data and direction data between the vehicle and the external infrastructure from the sensor. For example, the sensor may mean a high-definition map (HD map) or a range sensor. Data received from the sensor may include noise, and the corresponding noise may be nonlinear noise. Hereinafter, a process of calculating the first state information of the vehicle by filtering data including nonlinear noise will be described.

Distance data and direction data received from the sensor may include noise and may be modeled as in Equation (2).

은 k+1 시간에서 차량과 인프라들 사이의 상대적 거리 및 상대적 각도에 대한 정보를 포함하는 측정 벡터(measurement vector)를 나타낼 수 있다.here,

may represent a measurement vector including information on a relative distance and a relative angle between the vehicle and the infrastructure at k+1 time.

Equation 2 may be embodied as Equation 3 below.

)은 바이시클 모델에서 차량의 중심점과 대응된다.3 shows the arrangement of vehicles and infrastructure in a global coordinate system. In FIG. 3 , the vehicle 301 can be simplified by a bicycle model, and the center point of the vehicle 301 (

) corresponds to the center point of the vehicle in the bicycle model.

는 차량(301)과 i번째 인프라(311) 사이의 상대적 거리,

는 차량(301)과 i번째 인프라(311) 사이의 상대적 각도를 나타낼 수 있다.

는 k 시간에서 차량의 예측된 x축 위치 좌표,

는 k 시간에서 차량의 예측된 y축 위치 좌표를 나타낼 수 있다.

는 인프라(311)의 x축 위치 좌표,

는 인프라(311)의 y축 위치 좌표를 나타낼 수 있다.

는 차량(301)의 진행 방향(391)과 x축이 이루는 각도를 나타낼 수 있다. 또한,

는 센서로부터 수신된 차량과 인프라 사이의 거리 데이터에 관한 노이즈,

는 센서로부터 수신된 차량과 인프라 사이의 방향 데이터에 관한 노이즈를 나타낼 수 있다. 해당 노이즈들은 비선형(non-linear)의 노이즈일 수 있다.Referring to Figure 3,

is the relative distance between the vehicle 301 and the i-th infrastructure 311,

may represent a relative angle between the vehicle 301 and the i-th infrastructure 311 .

is the predicted x-axis position coordinates of the vehicle at time k,

may represent the predicted y-axis position coordinates of the vehicle at k time.

is the x-axis position coordinate of the infrastructure 311,

may indicate the y-axis location coordinates of the infrastructure 311 .

may represent an angle between the traveling direction 391 of the vehicle 301 and the x-axis. In addition,

is the noise about the distance data between the vehicle and the infrastructure received from the sensor,

may represent noise regarding the direction data between the vehicle and the infrastructure received from the sensor. The corresponding noises may be non-linear noise.

4 shows particles randomly generated around a real vehicle in a global coordinate system.

Referring to FIG. 4 , particles generated in the initialization step may be randomly generated around the actual location of the vehicle in the global coordinate system. The first particle 421 , the second particle 422 , and the third particle 423 may be generated around the vehicle. Particles may be generated as many as a preset number in the vehicle.

The location of the i-th infrastructure may be measured differently for each particle. Since the location of each particle is different, the location of the i-th infrastructure may be measured differently for each particle. For example, the real location 311 in the global coordinate system of the i-th infrastructure may be different from the virtual location 451 of the i-th infrastructure measured by the n-th particle 441 . In Equation 4 below, a process of calculating the weights of particles using an error according to a difference between the virtual location of the infrastructure and the actual location of the infrastructure measured from the particle is described. In Equation 4 below, it is assumed that N particles are generated and L infrastructures are disposed around the vehicle. Here, N and L may be 1 or more natural numbers.

는 제n 파티클(441)과 i번째 인프라(311) 사이의 가중치를 나타낼 수 있다.

는 센서로부터 수신한 거리 데이터의 x축 방향 노이즈의 공분산(covariance),

는 센서로부터 수신한 거리 데이터의 y축 방향 노이즈의 공분산을 나타낼 수 있다.n may have a value of a natural number between 1 and N, and i may have a value of a natural number between 1 and L.

may represent a weight between the n-th particle 441 and the i-th infrastructure 311 .

is the covariance of the noise in the x-axis direction of the distance data received from the sensor,

may represent the covariance of noise in the y-axis direction of distance data received from the sensor.

는 제n 파티클(441)로부터 측정된 i번째 인프라의 가상의 위치(451)에 대한 x축 좌표,

는 제n 파티클(441)로부터 측정된 i번째 인프라의 가상의 위치(451)에 대한 y축 좌표를 나타낼 수 있다.

는 글로벌 좌표계에서 실측 정보(예를 들어, 그라운드 트루스)에 따른 i번째 인프라의 실제 x위치 좌표,

는 글로벌 좌표계에서 실측 정보에 따른 i번째 인프라의 y 위치 좌표를 나타낼 수 있다. 파티클과 개별 인프라 사이의 가중치를 계산하기 위하여, 다변량 정규 분포 함수(multivariable normal distribution function)가 사용될 수 있다.

is the x-axis coordinate for the virtual location 451 of the i-th infrastructure measured from the n-th particle 441,

may represent the y-axis coordinate of the virtual location 451 of the i-th infrastructure measured from the n-th particle 441 .

is the actual x-location coordinate of the i-th infrastructure according to the actual measurement information (eg, ground truth) in the global coordinate system,

may indicate the y location coordinate of the i-th infrastructure according to the actual measurement information in the global coordinate system. To calculate the weights between particles and individual infrastructure, a multivariable normal distribution function can be used.

는, 실측 정보에 따른 i번째 인프라의 실제 위치(311)와 제n 파티클에서 측정된 i번째 인프라의 가상의 위치(451) 차이에 상응하는 오류(error)가 커질수록 감소할 수 있다.The weight between the n-th particle 441 and the i-th infrastructure 311 is

may decrease as an error corresponding to the difference between the actual location 311 of the i-th infrastructure according to the actual measurement information and the virtual location 451 of the i-th infrastructure measured from the n-th particle increases.

In Equation 5 below, a process of calculating the weight of a particle by using the weight individually calculated between the particle calculated by Equation 4 and the infrastructure is described.

n may have a value of a natural number between 1 and N. Referring to Equation 5 above, the weight of the n-th particle may be calculated by multiplying individual weights for all L infrastructures of the n-th particle.

In step 204, the resampling process is described. After the weight is calculated for each particle according to Equation 5, the first state information of the vehicle may be calculated by selecting the particle based on the weight. For example, but not limited to, a particle having the highest weight among generated particles may be selected.

Table 1 below shows the pseudo-code of the particle filter.

은 제1 파티클(particle), 제2 파티클, ?? 제N 파티클의 가중치를 나타내며,

는 센서에서 측정된 거리의 불확실성,

는 센서에서 측정된 방향의 불확실성,

는 파티클 필터에서 추정된 차량의 제1 상태 정보를 나타낼 수 있다.here,

is the first particle (particle), the second particle, ?? Represents the weight of the Nth particle,

is the uncertainty of the distance measured from the sensor,

is the uncertainty of the direction measured by the sensor,

may represent the first state information of the vehicle estimated by the particle filter.

를 무향 칼만 필터에 융합하여 차량의 위치 추정의 정확도를 향상시킬 수 있다.As described below, the final state information of the vehicle estimated from the particle filter

can be fused to an undirected Kalman filter to improve the accuracy of vehicle location estimation.

5 is a description of a vehicle position estimation method by filter fusion according to an embodiment.

In the non-directional Kalman filter, second state state information of the vehicle may be calculated based on the first state information of the vehicle estimated from the particle filter. The location of the vehicle may be more accurately estimated in the non-directional Kalman filter by using the first state information of the vehicle calculated from the particle filter. Hereinafter, a Particle Aided Unscented Kalman Filter (PAUKF) in which an unscented Kalman filter and a particle filter are fused will be described.

Referring to FIG. 5 , in step 511 , the particle filter may predict first temporary information about the state of the vehicle based on the bicycle model and previous state information including noise. In operation 512 , the vehicle may receive measurement data including non-Gaussian noise from the sensor. In operation 513, first state information of the vehicle may be calculated based on the first temporary information and the measurement data. In operation 521 , the second temporary information regarding the state of the vehicle may be predicted based on the previous state information including noise in the non-directional Kalman filter. In operation 522, first state information of the vehicle calculated from the particle filter may be received. In operation 523, second state information, which is final state information of the vehicle, may be calculated by updating the second temporary information using the first state information. Hereinafter, a process of estimating vehicle state information of the particle-assisted anechoic Kalman filter will be described in detail with equations.

6 shows sigma points generated as a symmetrical area around the mean value in an anechoic Kalman filter.

에 의하여 정의될 수 있다. 수학식 7은 무향 칼만 필터에서 차량의 상태 정보에 대응하는 상태 벡터를 나타낸다.Referring to FIG. 6 , sigma points may be generated as a symmetric region around an expected value of a state vector that is a random variable, that is, an average value 600 . For example, the first sigma point 601 may be symmetrical with respect to the second sigma point 602 and the average value, and the third sigma point 603 may be symmetric with the fourth sigma point 604 and the average value. can be symmetrical. The sigma points are parameters calculated using Equation 6 below.

can be defined by Equation 7 represents a state vector corresponding to vehicle state information in the undirected Kalman filter.

는 상태 벡터의 크기(size)로서 5를 나타낼 수 있다. 수학식 7에서,

는 k 시간에서 제2 예측 모델을 이용하여 예측된 차량의 상태 정보에 대응하는 상태 벡터를 나타낼 수 있다. 수학식 7에서,

는 차량의 x 위치 좌표,

는 차량의 y 위치 좌표,

는 차량의 속도(velocity),

는 차량의 요우잉 각도,

는 차량의 요우잉 속도(예를 들어, 회전 속도)를 나타낼 수 있다. 상태 벡터

는 시그마 포인트들이 개별적으로 갖는 상태 벡터의 평균을 나타낼 수 있다. 하기 수학식 8을 사용하여 시그마 포인트들이 생성될 수 있다.In Equation 6,

may represent 5 as the size (size) of the state vector. In Equation 7,

may represent a state vector corresponding to the state information of the vehicle predicted by using the second prediction model at k time. In Equation 7,

is the x position coordinate of the vehicle,

is the y position coordinate of the vehicle,

is the vehicle's velocity,

is the yaw angle of the vehicle,

may represent a yaw speed (eg, rotation speed) of the vehicle. state vector

may represent the average of the state vectors each sigma points have. Sigma points may be generated using Equation 8 below.

는 k 시간에서 시그마 포인트들의 상태 정보에 대응하는 상태 벡터,

는 k 시간에서 차량 상태의 평균 값,

는 k 시간에서 차량 상태의 공분산 행렬(covariance matrix)을 나타낼 수 있다.

는

의 행렬을 나타낼 수 있다. 수학식 8에 의하여, 평균 값에 상응하는 시그마 포인트를 포함하여 총

개의 시그마 포인트들이 생성될 수 있다.

is a state vector corresponding to the state information of the sigma points at time k,

is the average value of the vehicle state at time k,

may represent a covariance matrix of the vehicle state at k time.

Is

can represent a matrix of According to Equation 8, including the sigma point corresponding to the average value, the total

n sigma points may be generated.

After generating the sigma points, the second temporary information about the state of the vehicle may be predicted using the second prediction model of the undirected Kalman filter. For example, the second prediction model may be a constant turn rate and velocity magnitude (CTRV) vehicle motion model. Hereinafter, a constant turn rate and constant velocity vehicle motion model will be described as a CTRV model. The second predictive model is not limited to the CTRV model, and other predictive models may be used.

In order to obtain an accurate location of the vehicle, the anechoic Kalman filter can take into account more states and the influence of non-linear noise on the states of the vehicle. For example, a vehicle may consider acceleration and yaw acceleration as noise. In particular, the acceleration and yaw acceleration noise may be non-linear. In order to deal with the nonlinear noise, an augmented state vector as expressed in Equation 9 below may be considered.

는 k 시간에서 가속도 및 요우잉 가속도를 상태 벡터로 포함하는 보강된 상태 벡터를 나타낼 수 있다.

는 차량 가속도의 노이즈,

는 차량 요우잉 가속도의 노이즈를 나타낼 수 있다.In Equation 9,

may represent the augmented state vector including the acceleration and the yaw acceleration as state vectors at k time.

is the noise of vehicle acceleration,

may represent noise of vehicle yaw acceleration.

는

의 분산을 갖는 정규 가우스 분포(normal Gaussian distribution)로 설정될 수 있고,

는

의 분산을 갖는 정규 가우스 분포로 설정될 수 있다.Referring to Equations 10 and 11 below,

Is

It can be set to a normal Gaussian distribution with a variance of

Is

It can be set to a normal Gaussian distribution with a variance of .

는 하기 수학식 12에 표현된 바와 같이, 가속도 노이즈의 분산 및 요우잉 가속도 노이즈의 분산을 포함하는

로 보강될 수 있다.The vehicle considers the acceleration and yaw acceleration as the vehicle's state vector, so that the covariance matrix

As expressed in Equation 12 below, including the dispersion of acceleration noise and the dispersion of yaw acceleration noise

can be reinforced with

및 제2 예측 모델을 사용하여 차량의 상태에 관한 제2 임시 정보를 예측할 수 있다. 제2 예측 모델은 CTRV 모델일 수 있다. 수학식 13 및 수학식 14에서는 k 시간에서 추정된 차량의 이전 상태 정보 및 CTRV 모델을 이용하여 k+1 시간에서의 차량의 상태에 관한 제2 임시 정보를 예측하는 과정을 표현한다. 제2 임시 정보란, 이전 시간에서 추정된 차량의 이전 상태 정보로부터 제2 예측 모델을 이용하여 예측된 시그마 포인트들의 상태 벡터에 대응하는 차량의 예측된 상태 정보를 의미한다.In the PAUKF filter according to an embodiment, extended state information at k time

and the second prediction model may be used to predict second temporary information regarding the state of the vehicle. The second predictive model may be a CTRV model. Equations 13 and 14 express a process of predicting the second temporary information about the state of the vehicle at time k+1 by using the previous state information of the vehicle estimated at time k and the CTRV model. The second temporary information means predicted state information of a vehicle corresponding to a state vector of sigma points predicted using a second prediction model from previous state information of the vehicle estimated at a previous time.

은 k+1 시간에서의 차량의 상태 벡터,

는 k 시간에서 차량의 상태 벡터, 및

는 k 시간에서 차량의 상태 벡터를 미분한 값을 나타낼 수 있다.In Equation 13,

is the state vector of the vehicle at time k+1,

is the state vector of the vehicle at time k, and

may represent a value obtained by differentiating the state vector of the vehicle at time k.

Based on Equation 13, Equation 14 describes a process of predicting the extended state vector of the vehicle at time k+1 using the CTRV model and the extended state vector of the vehicle at time k.

는 k+1 시간에서의 차량의 확장된 상태 벡터,

는 k 시간에서의 차량의 확장된 상태 벡터를 나타낼 수 있다.In Equation 14,

is the extended state vector of the vehicle at time k+1,

may represent the extended state vector of the vehicle at time k.

는 확장된 상태 벡터의 크기로서 7을 나타내는

로 변환되고, 파라미터

는 하기 수학식 15에 의하여 계산될 수 있다.Further, in the extended state vector prediction step taking into account the acceleration and the yaw acceleration,

is the size of the expanded state vector, representing 7

is converted to a parameter

can be calculated by Equation 15 below.

The weight of each sigma point may be calculated based on Equations 17 and 18 below.

Equation 17 describes the process of calculating the weight of the sigma point 600 corresponding to the average value, and Equation 18 describes the process of calculating the weight of the sigma points other than the sigma point 600 .

Equation 19 describes a process of calculating an average state vector based on state information and weights of sigma points, and Equation 20 describes a process of calculating covariance based on state information and average state vectors of sigma points.

는 k+1 시간에서 시그마 포인트들의 상태 벡터에 가중치가 곱해진 예측된 평균 상태 벡터를 나타낼 수 있다.In Equation 19,

may represent the predicted average state vector in which the weight is multiplied by the state vector of sigma points at k+1 time.

는 k+1시간에서 예측된 공분산 행렬을 나타낼 수 있다.In Equation 20,

may represent the predicted covariance matrix at k+1 time.

Referring to Equation 14, when the extended state vector of the vehicle at time k+1 is predicted by using the CTRV model, information on acceleration noise and yaw acceleration noise can be obtained from x-coordinate, y-coordinate, vehicle speed, It may include yaw angle and yaw speed. Accordingly, the augmented state vector can be changed back to an existing state vector having a size of 5, except for the acceleration state vector and the yawing acceleration state vector.

Hereinafter, a process of calculating the second state information of the vehicle by updating the second temporary information predicted by the non-directional Kalman filter using the first state information received from the particle filter will be described.

)를 수신하여 제2 상태 정보를 산출할 수 있다. 다시 말해, 차량의 제1 상태 정보(

)는 가상의 센서(virtual sensor)가 되어, 기존 센서보다 더 정확한 차량의 상태를 산출할 수 있다.In the non-directional Kalman filter, instead of receiving the measurement data having noise from the sensor, the vehicle first state information (

) may be received to calculate the second state information. In other words, the vehicle's first state information (

) becomes a virtual sensor, which can calculate the vehicle state more accurately than existing sensors.

Equation 21 below represents information about the first state information received from the particle filter.

Referring to Equation 21, the first state information received from the particle filter may include an x position coordinate, a y position coordinate, and a yaw angle of the vehicle.

Equation 22 describes a process of predicting a measurement vector corresponding to a measurement value.

는 k+1 시간에서의 측정 벡터를 나타낼 수 있다.

는 k+1 시간에서의 시그마 포인트들의 상태 벡터를 나타낼 수 있고,

은 k+1 시간에서의 비선형 노이즈(non-linear)를 나타낼 수 있다.In Equation 22,

may represent the measurement vector at k+1 time.

may represent the state vector of sigma points at k+1 time,

may represent non-linear noise at k+1 time.

을 추가하여 모델링할 수 있다.Referring to matrix A in Equation 23, in order to calculate the measurement vector in Equation 22, the vehicle's position coordinates (x position coordinates, y position coordinates) from the state vector of sigma points predicted at k+1 time of the second temporary information ) and information about the yaw angle can be extracted. Since the state vector of the sigma points at k+1 time is calculated using the sigma points at the previous k time, noise may be included. Therefore, in Equation 23, the nonlinear noise

can be modeled by adding

Equation 24 describes a process of calculating the predicted average measurement vector.

는 k+1시간에서의 예측된 평균 측정 벡터를 나타낼 수 있다.In Equation 24,

may represent the predicted average measurement vector at k+1 time.

Next, Equation 25 describes a process of predicting the measurement covariance. Equation 26 may represent noise covariance.

는 k+1 시간에서 예측된 측정 공분산 행렬을 나타낼 수 있다. 수학식 26에서,

는 파티클 필터에서 센서로부터 수신한 거리 데이터의 x축 방향 노이즈의 공분산,

는 파티클 필터에서 센서로부터 수신한 거리 데이터의 y축 방향 노이즈의 공분산,

는 파티클 필터에서 센서로부터 수신한 방향 데이터의 요 각도 노이즈의 공분산을 나타낼 수 있다. 측정 공분산 행렬

는 수학식 25 및 26을 참조하면, 파티클 필터에서 센서로부터 수신한 데이터의 노이즈에 따라 변할 수 있다.In Equation 25,

may represent the predicted measurement covariance matrix at k+1 time. In Equation 26,

is the covariance of the noise in the x-axis direction of the distance data received from the sensor in the particle filter,

is the covariance of the noise in the y-axis direction of the distance data received from the sensor in the particle filter,

may represent the covariance of the yaw angle noise of the direction data received from the sensor in the particle filter. Measurement covariance matrix

Referring to Equations 25 and 26, the particle filter may change according to the noise of data received from the sensor.

In Equation 27 below, a process of calculating a cross-correlation matrix is described.

는 k+1시간에서 시그마 포인트들의 상태 정보 및 k+1시간에서 시그마 포인트들의 측정 정보 사이의 교차 상관 관계 행렬을 나타낼 수 있다. 교차 상관 관계 행렬과 측정 공분산에 기초하여, 칼만 이득(Kalman gain)은 수학식 28과 같이 계산된다.

may represent a cross-correlation matrix between state information of sigma points at time k+1 and measurement information of sigma points at time k+1. Based on the cross-correlation matrix and the measured covariance, the Kalman gain is calculated as in Equation (28).

는 k+1 시간에서의 칼만 이득을 나타낼 수 있다.

may represent the Kalman gain at k+1 time.

The second temporary information of the vehicle may be updated to the second state information by using the first state information received from the particle filter. In Equation 29 below, the second state information may include a position coordinate of a vehicle, a yaw angle, and a covariance matrix for a state of the vehicle.

는 PAUKF에서 계산된 차량의 최종 상태 벡터를 나타낸다.

는 차량의 위치 좌표(x 좌표, y 좌표) 및 요우잉 각도에 관한 정보를 포함할 수 있다. 수학식 29를 참조하면, 차량의 최종 상태 벡터는 파티클 필터로부터 수신한 제1 상태 정보 및 칼만 이득에 기초하여 산출될 수 있다.In Equation 29,

denotes the final state vector of the vehicle calculated in PAUKF.

may include information about the vehicle's position coordinates (x coordinate, y coordinate) and a yaw angle. Referring to Equation 29, the final state vector of the vehicle may be calculated based on the first state information received from the particle filter and the Kalman gain.

는 PAUKF에서 계산된 차량의 최종 상태에 대한 공변량 행렬을 나타낼 수 있다. 측정 공변량 행렬, 칼만 이득, 및 k 시간에서의 공변량 행렬에 기초하여 k+1시간에서의 공변량 행렬을 산출할 수 있다.In Equation 30,

may represent the covariate matrix for the final state of the vehicle calculated in PAUKF. A covariate matrix at k+1 time can be calculated based on the measurement covariate matrix, the Kalman gain, and the covariate matrix at k time.

및 차량의 최종 상태에 대한 공변량 행렬

를 포함하는 제2 상태 정보에 기초하여 차량의 조향, 속도, 및 가속도 중 적어도 하나가 조정될 수 있다.Vehicle's final state vector

and the covariate matrix for the final state of the vehicle.

At least one of steering, speed, and acceleration of the vehicle may be adjusted based on the second state information including

The full pseudo code of the PAUKF algorithm is described in Table 2 below.

7 to 9 will explain the result of estimating the position of the vehicle through the particle-assisted anechoic Kalman filter.

Hereinafter, a result of estimating the position of the vehicle according to the simulation will be described. In the above-described simulation, the frequency of the distance sensor is set to 100 Hz, and the vehicle may receive data including Gaussian noise and non-Gaussian noise from the sensor. For example, Gaussian noise is generated using a normrnd function, and non-Gaussian noise is generated using a sinusoidal function. Table 3 below describes the generated noise.

7 shows a road on which a vehicle moves in a simulation.

The road 701 indicates an S-shaped road, and the road 702 indicates a road having a straight line shape. The road 701 is used to verify the performance of each filter on a curved road, and the road 702 is used to verify the performance of each filter on a straight road. Twelve infrastructures can be arranged around the road, and the location of the infrastructure is simulated to be fixed even if the map is changed. The infrastructures may be placed symmetrically so that their location does not affect the performance of the filters. The vehicle speed is set to a constant value, and the speed is set to 60 km/h, 80 km/h, 100 km/h, and 120 km/h, respectively, and simulated.

Referring to Equation 31, the performance of the filters is evaluated based on a root mean square error (RMSE).

In Equation 31, N may represent the number of data points. The performance of the filter may be verified by comparing the trajectory of the vehicle estimated from the filter with the actual trajectory of the vehicle. The trajectory of the vehicle estimated from the filter will be described with reference to FIG. 8 .

8A shows the results of the trajectories of filters estimated on the S-shaped road.

The trajectory 801 indicates the trajectory of the vehicle including noise. The trajectory 802 represents the trajectory of the vehicle estimated from the particle filter PF. The trajectory 803 represents the trajectory of the vehicle estimated from the undirected Kalman filter (UKF). The trajectory 804 represents the trajectory of the particle-assisted anechoic Kalman filter (PAUKF). The trajectory 805 represents the actual trajectory of the vehicle.

The trajectory 802 of the vehicle estimated from the particle filter is close to the actual trajectory 805 of the vehicle, but the trajectory error is very large. This is because the particle filter determines the vehicle location only by the relative distance between the vehicle and the infrastructure having noise and vehicle data having noise. Compared with the particle filter, the vehicle trajectory 803 estimated from the undirected Kalman filter appears relatively smooth. However, the undirected Kalman filter cannot filter the noise of GPS data. Because the GPS measurement of an anechoic Kalman filter has high variance and does not use a distance sensor, the anechoic Kalman filter depends more on the vehicle model than the measurement. Measurement noise makes the anechoic Kalman filter less sensitive to changes in position and yaw. On the other hand, the particle-assisted anechoic Kalman filter according to an embodiment may respond more quickly and accurately to changes in position and yaw.

Table 4 below shows the results for the performance of the filter. Since the anechoic Kalman filter does not use distance sensor data, it is not appropriate to compare it with the particle filter and the particle-assisted anechoic Kalman filter. Therefore, Table 4 does not show the RMSE for the anechoic Kalman filter. The average of the estimation error for PAUKF is 1.08 m, and the variance is 0.7147 m. When the speed of the vehicle increases, the PF and PAUKF estimation errors slightly increase. However, considering the RMSE magnitude of the noise change from 21.336m to 21.712m, the RMSE of the estimation error does not change even if the vehicle speed increases from 60km/h to 120km/h. Compared to Gaussian noise, non-Gaussian noise produces a larger average value. The precision of the PF does not change even when the noise increases, and the precision is almost the same as the RMSE range of 5.489m to 5.959m. The RMSE range of PAUKF appears to be 1.440m to 1.772m even when the noise increases and the vehicle speed increases. PAUKF can improve accuracy by 4.028m to 4.049m compared to PF.

8B shows the trajectory results of filters estimated on a straight road.

In the simulation, the speed of the vehicle is 60 km/h, and the noise may follow Gaussian. The estimation result by the straight road is used to determine the performance when the vehicle moves only in the X direction. Referring to FIG. 8B , PAUKF converges closer to the trajectory 805 of the actual vehicle than PF and UKF. Since the vehicle only moves in the X direction, there is no information about the movement in the Y direction. Therefore, even if the estimation of PAUKF is better than that of UKF, PAUKF tends to believe more about noise in the Y direction in order to improve the response time. As a result, the PAUKF may not be sufficiently precise in the Y direction as shown in Fig. 8b.

The results for the filter performance when the vehicle moves in the X direction are shown in Table 5. It can be seen that the PF and PAUKF estimation characteristics are the same as those of the vehicle when driving on an S-shaped road. The estimation results show that the performance of the filter does not change even when the road changes. The RMSE of the PF is 5.384m to 5.692m, and the RMSE of the PAUKF is 1.312m to 1.800m even when the noise increases and the speed increases. Compared to PF, PAUKF has improved accuracy from 3.892m to 4.072m.

9 is a block diagram illustrating a configuration of a vehicle according to an exemplary embodiment.

In a vehicle performing location estimation according to an embodiment, the vehicle may be configured with a processor, a memory, and an input/output interface. The processor 910 may calculate the first state information of the vehicle based on the measured data received from the external sensor using the particle filter and the previous state information estimated at a previous time point. In addition, the processor 910 may calculate the second state information of the vehicle based on the first state information by using the non-directional Kalman filter. The memory 920 may store a first prediction model of the particle filter and a second prediction model of the undirected Kalman filter.

The embodiments described above may be implemented by a hardware component, a software component, and/or a combination of a hardware component and a software component. For example, the apparatus, methods and components described in the embodiments may include, for example, a processor, a controller, an arithmetic logic unit (ALU), a digital signal processor, a microcomputer, a field programmable gate (FPGA). array), a programmable logic unit (PLU), a microprocessor, or any other device capable of executing and responding to instructions, may be implemented using one or more general purpose or special purpose computers. The processing device may execute an operating system (OS) and one or more software applications running on the operating system. The processing device may also access, store, manipulate, process, and generate data in response to execution of the software. For convenience of understanding, although one processing device is sometimes described as being used, one of ordinary skill in the art will recognize that the processing device includes a plurality of processing elements and/or a plurality of types of processing elements. It can be seen that may include For example, the processing device may include a plurality of processors or one processor and one controller. Other processing configurations are also possible, such as parallel processors.

The software may comprise a computer program, code, instructions, or a combination of one or more thereof, which configures a processing device to operate as desired or is independently or collectively processed You can command the device. The software and/or data may be any kind of machine, component, physical device, virtual equipment, computer storage medium or device, to be interpreted by or to provide instructions or data to the processing device. , or may be permanently or temporarily embody in a transmitted signal wave. The software may be distributed over networked computer systems and stored or executed in a distributed manner. Software and data may be stored in one or more computer-readable recording media.

The method according to the embodiment may be implemented in the form of program instructions that can be executed through various computer means and recorded in a computer-readable medium. The computer-readable medium may include program instructions, data files, data structures, and the like, alone or in combination. The program instructions recorded on the computer-readable medium may be specially designed and configured for the embodiment, or may be known and available to those skilled in the art of computer software. Examples of the computer-readable recording medium include magnetic media such as hard disks, floppy disks and magnetic tapes, optical media such as CD-ROMs and DVDs, and magnetic such as floppy disks. - includes magneto-optical media, and hardware devices specially configured to store and execute program instructions, such as ROM, RAM, flash memory, and the like. Examples of program instructions include not only machine language codes such as those generated by a compiler, but also high-level language codes that can be executed by a computer using an interpreter or the like. The hardware devices described above may be configured to operate as one or more software modules to perform the operations of the embodiments, and vice versa.

As described above, although the embodiments have been described with reference to the limited drawings, those skilled in the art may apply various technical modifications and variations based on the above. For example, the described techniques are performed in a different order than the described method, and/or the described components of the system, structure, apparatus, circuit, etc. are combined or combined in a different form than the described method, or other components Or substituted or substituted by equivalents may achieve an appropriate result.

Claims (12)

A location estimation method performed by a vehicle, comprising:
calculating first state information of the vehicle based on measured data received from a sensor using a particle filter (PF) and previous state information estimated at a previous time point; and
calculating second state information of the vehicle based on the calculated first state information using an unscented Kalman filter (UKF);
including,
Measurement data received from the sensor includes non-Gaussian noise,
Calculating the second state information of the vehicle comprises:
generating sigma points;
predicting second temporary information about the state information of the generated sigma points using a second prediction model of the undirected Kalman filter, and predicting a measurement value of the vehicle from the predicted second temporary information; and
calculating second state information of the vehicle based on the calculated first state information, the predicted second temporary information, and the predicted measured value;
A location estimation method comprising a.
According to claim 1,
Calculating the first state information of the vehicle comprises:
generating a predetermined number of particles;
predicting first temporary information about the state of the vehicle by using the first prediction model of the particle filter; and
calculating a weight for each generated particle
A location estimation method comprising a.
3. The method of claim 2,
The first prediction model is a bicycle model (bicycle model),
Location estimation method.
delete According to claim 1,
Predicting the measured value of the vehicle from the predicted second temporary information includes:
extracting information about the location coordinates and yaw angle of the vehicle from the predicted second temporary information
A location estimation method comprising a.
According to claim 1,
The second prediction model is a constant turn rate and velocity (CTRV) model,
Location estimation method.
According to claim 1,
The first state information of the vehicle,
Containing information about the position coordinates and yaw angle of the vehicle,
Location estimation method.
According to claim 1,
The second state information of the vehicle,
comprising a covariance matrix for the position coordinates of the vehicle, the yaw angle, and the state of the vehicle,
Location estimation method.
9. The method of claim 8,
Calculating the second state information of the vehicle comprises:
calculating the covariate matrix based on a measurement covariate matrix, a Kalman gain, and a previous covariate matrix
A location estimation method comprising a.
According to claim 1,
adjusting at least one of steering, speed, and acceleration of the vehicle based on the second state information
A location estimation method further comprising a.
A computer-readable recording medium storing one or more computer programs including instructions for performing the method of any one of claims 1 to 3 and 5 to 10.
In a vehicle for performing location estimation,
a memory for storing a first prediction model of the particle filter and a second prediction model of the undirected Kalman filter; and
The first state information of the vehicle is calculated based on the measured data received from the sensor using the particle filter and the previous state information estimated at a previous time point, and the calculated first state information is applied to the calculated first state information using the non-directional Kalman filter. A processor for calculating second state information of the vehicle based on
including,
Measurement data received from the sensor includes non-Gaussian noise,
The processor is
sigma points are generated, second temporary information about the state information of the generated sigma points is predicted using a second prediction model of the undirected Kalman filter, and the measured value of the vehicle is obtained from the predicted second temporary information predicting, and calculating second state information of the vehicle based on the calculated first state information, the predicted second temporary information, and the predicted measured value,
vehicle.

Images