KR20220059754A - 센서 퓨전에 의한 차량의 위치 추정 방법 - Google Patents

센서 퓨전에 의한 차량의 위치 추정 방법 Download PDF

Info

Publication number
KR20220059754A
KR20220059754A KR1020200145317A KR20200145317A KR20220059754A KR 20220059754 A KR20220059754 A KR 20220059754A KR 1020200145317 A KR1020200145317 A KR 1020200145317A KR 20200145317 A KR20200145317 A KR 20200145317A KR 20220059754 A KR20220059754 A KR 20220059754A
Authority
KR
South Korea
Prior art keywords
vehicle
state information
information
calculating
location estimation
Prior art date
Application number
KR1020200145317A
Other languages
English (en)
Other versions
KR102460842B1 (ko
Inventor
김병우
임명
윤재우
Original Assignee
울산대학교 산학협력단
한국수력원자력 주식회사
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by 울산대학교 산학협력단, 한국수력원자력 주식회사 filed Critical 울산대학교 산학협력단
Priority to KR1020200145317A priority Critical patent/KR102460842B1/ko
Publication of KR20220059754A publication Critical patent/KR20220059754A/ko
Application granted granted Critical
Publication of KR102460842B1 publication Critical patent/KR102460842B1/ko

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W40/00—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models
    • B60W40/10—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models related to vehicle motion
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W10/00—Conjoint control of vehicle sub-units of different type or different function
    • B60W10/20—Conjoint control of vehicle sub-units of different type or different function including control of steering systems
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W40/00—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models
    • B60W40/02—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models related to ambient conditions
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W50/00—Details of control systems for road vehicle drive control not related to the control of a particular sub-unit, e.g. process diagnostic or vehicle driver interfaces
    • B60W2050/0001—Details of the control system
    • B60W2050/0002—Automatic control, details of type of controller or control system architecture
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W50/00—Details of control systems for road vehicle drive control not related to the control of a particular sub-unit, e.g. process diagnostic or vehicle driver interfaces
    • B60W2050/0001—Details of the control system
    • B60W2050/0019—Control system elements or transfer functions
    • B60W2050/0022—Gains, weighting coefficients or weighting functions
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W50/00—Details of control systems for road vehicle drive control not related to the control of a particular sub-unit, e.g. process diagnostic or vehicle driver interfaces
    • B60W2050/0001—Details of the control system
    • B60W2050/0019—Control system elements or transfer functions
    • B60W2050/0028—Mathematical models, e.g. for simulation
    • B60W2050/0031—Mathematical model of the vehicle
    • B60W2050/0033—Single-track, 2D vehicle model, i.e. two-wheel bicycle model
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W50/00—Details of control systems for road vehicle drive control not related to the control of a particular sub-unit, e.g. process diagnostic or vehicle driver interfaces
    • B60W2050/0001—Details of the control system
    • B60W2050/0043—Signal treatments, identification of variables or parameters, parameter estimation or state estimation
    • B60W2050/0052—Filtering, filters
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W2420/00—Indexing codes relating to the type of sensors based on the principle of their operation
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W2520/00—Input parameters relating to overall vehicle dynamics
    • B60W2520/14—Yaw
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W2720/00—Output or target parameters relating to overall vehicle dynamics
    • B60W2720/10—Longitudinal speed
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60Y—INDEXING SCHEME RELATING TO ASPECTS CROSS-CUTTING VEHICLE TECHNOLOGY
    • B60Y2400/00—Special features of vehicle units
    • B60Y2400/30—Sensors

Landscapes

  • Engineering & Computer Science (AREA)
  • Transportation (AREA)
  • Mechanical Engineering (AREA)
  • Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Mathematical Physics (AREA)
  • Chemical & Material Sciences (AREA)
  • Combustion & Propulsion (AREA)
  • Navigation (AREA)
  • Traffic Control Systems (AREA)

Abstract

일 실시예에 따른 센서 퓨전(fusion)에 의한 차량의 위치 추정 방법은, 파티클 필터(Particle Filter, PF)를 이용하여 센서(sensor)로부터 수신되는 측정 데이터 및 이전 시점에서 추정된 이전 상태 정보에 기초하여 차량의 제1 상태 정보를 산출하는 단계, 및 무향 칼만 필터(Unscented Kalman Filter, UKF)를 이용하여 파티클 필터로부터 수신되는 차량의 제1 상태 정보에 기초하여 차량의 최종 상태 정보를 산출하는 단계를 포함할 수 있다.

Description

센서 퓨전에 의한 차량의 위치 추정 방법{METHOD FOR VEHICLE LOCATION ESTIMATION BY SENSOR FUSION}
이하의 설명은 파티클 필터(Particle Filter, PF) 및 무향 칼만 필터(Unscented Kalman Filter, UKF)를 이용하여 차량의 위치를 추정하는 방법에 관한 것이다.
차량의 위치 추정은 자율 주행 차량 운영의 핵심 요소이다. 칼만 필터(Kalman Filter, KF)는 가우시안 노이즈(Gaussian noise)가 포함되어 있는 측정치를 바탕으로 선형 역학계의 상태를 추정하는 재귀 필터이다. 칼만 필터에서는 기본적으로 모델의 선형성을 가정하고 있지만, 실제적으로는 많은 모델들이 비선형 구조를 가지고 있다. 이에, 칼만 필터를 수정해 비선형 모델에도 사용할 수 있도록 한 무향 칼만 필터(Unscented Kalman Filter, UKF)가 제안되었다. 무향 칼만 필터는 평균 주변에 시그마 포인트(sigma point)를 사용하여 무향 변환(unscented transform)을 기초로 비가우시안 노이즈(non-Gaussian noise)를 가우시안 노이즈로 근사할 수 있다. 무향 칼만 필터는 비선형성을 적절히 캡처(capture)할 수 있다. 무향 칼만 필터는 비가우시안 노이즈를 시그마 포인트로 근사하기 때문에, 시그마 포인트를 선택하는 경우 다른 정보를 결합하기 쉽고 자코비안 행렬(Jacobian matrix)를 계산할 필요가 없다.
칼만 필터 및 무향 칼만 필터는 측정치의 노이즈가 가우시안이라는 기본적인 가정이 있다. 그러나, 실제로 대부분의 노이즈에는 가우시안 속성이 존재하지 않는다. 이에, 비가우시안 노이즈를 처리하기 위하여 파티클 필터(Particle Filter, PF)가 제안된다. 파티클 필터는 파티클(particle)을 사용하여 비가우시안 속성을 근사할 수 있다. 파티클은 무작위로 생성되기 때문에 충분한 수로 생성되면, 측정치의 비가우시안 노이즈의 속성을 정확하게 표현할 수 있다. 그러나 차량에는 계산 리소스(computational resource)가 제한되어 있다. 다시 말해, 효과적인 파티클 필터 기반 시스템 모델을 생성하는 경우, 정밀도와 계산 리소스 사이에 트레이드 오프 관계가 있다. 무향 칼만 필터는 비가우시안 노이즈를 필터링 하는데 한계점이 존재하여, 파티클 필터는 계산 리소스가 제한되기 때문에 파티클을 일정량 이상 생성하지 못하여 비가우시안 노이즈의 속성을 정확하게 표현하지 못하는 문제점뿐만 아니라 추정결과가 비연속인 문제점도 존재한다. 이하에서는, 무향 칼만 필터 및 파티클 필터를 융합함으로써 비가우시안 노이즈를 효과적으로 필터링하고, 차량의 위치 추정의 정확도를 높이는 센서 퓨전 방법이 설명된다.
일 실시예에 따른 차량(vehicle)에 의해 수행되는 위치 추정 방법은 파티클 필터(Particle Filter, PF)를 이용하여 센서(sensor)로부터 수신되는 측정 데이터 및 이전 시점에서 추정된 이전 상태 정보에 기초하여 상기 차량의 제1 상태 정보를 산출하는 단계, 및 무향 칼만 필터(Unscented Kalman Filter, UKF)를 이용하여 상기 제1 상태 정보에 기초하여 상기 차량의 제2 상태 정보를 산출하는 단계를 포함할 수 있다.
일 실시예에 따른 차량에 의해 수행되는 위치 방법의 상기 차량의 제1 상태 정보를 산출하는 단계는, 미리 정한 개수의 파티클(particle)을 생성하는 단계, 상기 파티클 필터의 제1 예측 모델을 사용하여 상기 차량의 상태에 관한 제1 임시 정보를 예측하는 단계, 및 상기 생성된 파티클 마다 가중치(weight)를 계산하는 단계를 포함할 수 있다.
일 실시예에 따른 차량에 의해 수행되는 위치 방법에서, 상기 제1 예측 모델은 바이시클 모델(bicycle model)일 수 있다.
일 실시예에 따른 차량에 의해 수행되는 위치 방법의 상기 차량의 제2 상태 정보를 산출하는 단계는, 시그마 포인트들(sigma points)을 생성하는 단계, 상기 무향 칼만 필터의 제2 예측 모델을 사용하여 상기 차량의 상태에 관한 제2 임시 정보를 예측하고, 상기 제2 임시 정보로부터 상기 차량의 측정 값을 예측하는 단계, 및 상기 파티클 필터로부터 추출된 상기 제1 상태 정보, 상기 제2 임시 정보, 및 상기 측정 값에 기초하여 상기 차량의 상기 제2 상태 정보를 산출하는 단계를 포함할 수 있다.
일 실시예에 따른 차량에 의해 수행되는 위치 방법의 상기 제2 임시 정보로부터 상기 차량의 측정 값을 예측하는 단계는, 상기 제2 임시 정보로부터 상기 차량의 위치 좌표 및 요우잉 각도에 관한 정보를 추출하는 단계를 포함할 수 있다.
일 실시예에 따른 차량에 의해 수행되는 위치 방법의 상기 제2 예측 모델은 등선회율 및 등속도(constant turn rate and velocity, CTRV) 모델일 수 있다.
일 실시예에 따른 차량에 의해 수행되는 위치 방법의 상기 제1 상태 정보는 상기 차량의 위치 좌표 및 방향(orientation)에 관한 정보를 포함할 수 있다.
일 실시예에 따른 차량에 의해 수행되는 위치 방법의 상기 제2 상태 정보는 상기 차량의 위치 좌표, 요우잉 각도, 및 상기 차량의 상태에 대한 공변량 행렬(covariance matrix)을 포함할 수 있다.
일 실시예에 따른 차량에 의해 수행되는 위치 방법의 상기 차량의 제2 상태 정보를 산출하는 단계는, 측정 공변량 행렬, 칼만 이득(Kalman gain), 및 이전 공변량 행렬에 기초하여 상기 공변량 행렬을 산출하는 단계를 포함할 수 있다.
일 실시예에 따른 차량에 의해 수행되는 위치 방법은 상기 제2 상태 정보에 기초하여 상기 차량의 조향, 속도, 및 가속도 중 적어도 하나를 조정하는 단계를 더 포함할 수 있다.
일 실시예에 따른 위치 추정을 수행하는 차량은 파티클 필터의 제1 예측 모델 및 무향 칼만 필터의 제2 예측 모델을 저장하는 메모리, 및 상기 파티클 필터를 이용하여 외부 센서로부터 수신되는 측정 데이터 및 이전 시점에서 추정된 이전 상태 정보에 기초하여 상기 차량의 제1 상태 정보를 산출하고, 상기 무향 칼만 필터를 이용하여 상기 제1 상태 정보에 기초하여 상기 차량의 제2 상태 정보를 산출하는 프로세서를 포함할 수 있다.
도 1은 바이시클 모델(bicycle model)을 설명한다.
도 2에서는 파티클 필터를 이용하여 차량의 제1 상태 정보를 산출하는 방법에 대하여 설명한다.
도 3은 글로벌 좌표계에서 차량과 인프라의 배치를 도시한다.
도 4는 글로벌 좌표계에서 실제 차량 주변에 임의로 생성된 파티클들을 도시한다.
도 5는 일 실시예에 따른 필터 융합(fusion)에 의한 차량 위치 추정 방법에 관하여 설명한다.
도 6은 무향 칼만 필터에서 평균 값 주위의 대칭적인 영역으로 생성된 시그마 포인트들(sigma points)을 도시한다.
도 7은 시뮬레이션 상에서 차량이 이동하는 도로를 나타낸다.
도 8a는 S자형 도로에서 추정된 필터들의 궤적 결과를 도시한다.
도 8b는 직선형 도로에서 추정된 필터들의 궤적 결과를 도시한다.
도 9는 일 실시예에 따른 차량의 구성을 설명하는 블록도이다.
실시예들에 대한 특정한 구조적 또는 기능적 설명들은 단지 예시를 위한 목적으로 개시된 것으로서, 다양한 형태로 변경되어 실시될 수 있다. 따라서, 실시예들은 특정한 개시형태로 한정되는 것이 아니며, 본 명세서의 범위는 기술적 사상에 포함되는 변경, 균등물, 또는 대체물을 포함한다.
제1 또는 제2 등의 용어를 다양한 구성요소들을 설명하는데 사용될 수 있지만, 이런 용어들은 하나의 구성요소를 다른 구성요소로부터 구별하는 목적으로만 해석되어야 한다. 예를 들어, 제1 구성요소는 제2 구성요소로 명명될 수 있고, 유사하게 제2 구성요소는 제1 구성요소로도 명명될 수 있다.
어떤 구성요소가 다른 구성요소에 "연결되어" 있다고 언급된 때에는, 그 다른 구성요소에 직접적으로 연결되어 있거나 또는 접속되어 있을 수도 있지만, 중간에 다른 구성요소가 존재할 수도 있다고 이해되어야 할 것이다.
단수의 표현은 문맥상 명백하게 다르게 뜻하지 않는 한, 복수의 표현을 포함한다. 본 명세서에서, "포함하다" 또는 "가지다" 등의 용어는 설명된 특징, 숫자, 단계, 동작, 구성요소, 부분품 또는 이들을 조합한 것이 존재함으로 지정하려는 것이지, 하나 또는 그 이상의 다른 특징들이나 숫자, 단계, 동작, 구성요소, 부분품 또는 이들을 조합한 것들의 존재 또는 부가 가능성을 미리 배제하지 않는 것으로 이해되어야 한다.
다르게 정의되지 않는 한, 기술적이거나 과학적인 용어를 포함해서 여기서 사용되는 모든 용어들은 해당 기술 분야에서 통상의 지식을 가진 자에 의해 일반적으로 이해되는 것과 동일한 의미를 가진다. 일반적으로 사용되는 사전에 정의되어 있는 것과 같은 용어들은 관련 기술의 문맥상 가지는 의미와 일치하는 의미를 갖는 것으로 해석되어야 하며, 본 명세서에서 명백하게 정의하지 않는 한, 이상적이거나 과도하게 형식적인 의미로 해석되지 않는다. 이하, 실시예들을 첨부된 도면을 참조하여 상세하게 설명한다. 각 도면에 제시된 동일한 참조 부호는 동일한 부재를 나타낸다.
도 1은 바이시클 모델(bicycle model)을 설명한다.
파티클 필터를 이용하여 차량의 상태 정보를 추정하는 경우, 차량의 모션(motion)을 표현하기 위하여 제1 예측 모델(prediction model)이 사용될 수 있다. 제1 예측 모델은 바이시클 모델(bicycle model)일 수 있다. 도 1을 참조하면, 바이시클 모델을 이용하여 차량의 모션을 표현할 수 있다. 차량(vehicle)은 1개의 앞 바퀴(front wheel, 101) 및 1개의 뒷 바퀴(rear wheel, 102)로 간략화 할 수 있으며, 차량의 상태는 2차원 공간에서의 x 좌표, y 좌표, 및 요우잉 각도(yaw angle,
Figure pat00001
)로 표현할 수 있다. 이때, 요우잉 각도는 앞 바퀴(101)와 뒷 바퀴(102)를 잇는 가상의 직선과 x축이 이루는 각도를 의미할 수 있다. 이하에서는, 파티클 필터를 이용하여 외부 센서(sensor)로부터 수신되는 측정 데이터 및 이전 시점에서 추정된 노이즈가 포함된 이전 상태 정보에 기초하여 차량의 제1 상태 정보를 산출하는 방법에 대하여 설명한다.
도 2에서는 파티클 필터를 이용하여 차량의 제1 상태 정보를 산출하는 방법에 대하여 설명한다.
단계(201)에서는 초기화(initialization) 과정이 이루어질 수 있다. 차량의 위치를 글로벌 좌표계(global coordinates)에서 결정하기 위하여, 차량의 초기 위치는 필수적으로 제공되어야 한다. 따라서 일 실시예에서는, GPS(Global Positioning System) 센서를 이용하여 차량의 초기 위치를 제공받을 수 있다. 차량에서 다중 경로 및 차단 문제(blocking issue)로 인하여 GPS 신호가 원활히 수신되지 않더라도, GPS 신호는 차량의 초기 위치를 결정하기 위한 제한적 영역(limited area)을 제공할 수 있다. 파티클 필터가 GPS 신호를 수신하는 경우, 파티클 필터는 N개의 파티클들(particles)을 생성할 수 있다. 예를 들어, N은 1 이상의 자연수를 나타낼 수 있다.
단계(202)에서는 차량의 상태에 관한 제1 임시 정보를 제1 예측 모델을 이용하여 예측하는 과정이 이루어질 수 있다. 차량의 상태는, 2차원 공간에서 차량의 위치 좌표(x축 좌표, y축 좌표) 및 요우잉 각도에 관한 정보를 포함할 수 있다. 구체적으로, k시간에서 추정된 노이즈가 포함된 이전 상태 정보 및 제1 예측 모델을 이용하여 k+1 시간에서 차량의 상태에 관한 제1 임시 정보를 예측할 수 있다. 제1 예측 모델은 바이시클 모델일 수 있다.
수학식 1에서는, k 시간에서 추정된 차량의 이전 상태 정보 및 바이시클 모델을 이용하여 k+1 시간에서의 차량의 상태에 관한 제1 임시 정보를 예측하는 과정을 나타낸다. 수학식 1에서는, 도시 내에서 차량의 이동은 일반적으로 빠르지 않기 때문에 미끄러짐 각도(slip angle)는 무시된다.
Figure pat00002
여기서,
Figure pat00003
은 k+1 시간에서 예측된 제1 임시 정보,
Figure pat00004
는 k 시간에서의 차량의 속도,
Figure pat00005
는 k 간에서의 차량의 요우잉 속도(yaw rate)를 나타낼 수 있다.
단계(203)에서는 생성된 N개의 파티클들 마다 가중치(weight)를 계산하는 과정이 설명된다.
차량 외부에는 하나 이상의 인프라들(infrastructures)이 배치될 수 있다. 이하에서는 L개의 인프라가 차량 외부에 배치되는 것으로 가정한다. L은 1 이상의 자연수를 나타낼 수 있다.
차량은 외부 인프라와 차량 사이의 거리 데이터 및 방향 데이터를 센서로부터 수신할 수 있다. 예를 들어, 센서는 고정밀지도(HD map) 또는 거리 센서(range sensor)를 의미할 수 있다. 센서로부터 수신한 데이터는 노이즈를 포함할 수 있으며, 해당 노이즈는 비선형 노이즈(nonlinear noise)일 수 있다. 이하에서는, 비선형 노이즈를 포함하는 데이터를 필터링하여 차량의 제1 상태 정보를 산출하는 과정을 설명한다.
센서로부터 수신한 거리 데이터 및 방향 데이터는 노이즈를 포함할 수 있으며 수학식 2와 같이 모델링 될 수 있다.
Figure pat00006
여기서,
Figure pat00007
은 k+1 시간에서 차량과 인프라들 사이의 상대적 거리 및 상대적 각도에 대한 정보를 포함하는 측정 벡터(measurement vector)를 나타낼 수 있다.
수학식 2는 하기 수학식 3과 같이 구체화될 수 있다.
Figure pat00008
도 3은 글로벌 좌표계에서 차량과 인프라의 배치를 도시한다. 도 3에서, 차량(301)은 바이시클 모델에 의하여 간략화 될 수 있고, 차량(301)의 중심점(
Figure pat00009
)은 바이시클 모델에서 차량의 중심점과 대응된다.
도 3을 참조하면,
Figure pat00010
는 차량(301)과 i번째 인프라(311) 사이의 상대적 거리,
Figure pat00011
는 차량(301)과 i번째 인프라(311) 사이의 상대적 각도를 나타낼 수 있다.
Figure pat00012
는 k 시간에서 차량의 예측된 x축 위치 좌표,
Figure pat00013
는 k 시간에서 차량의 예측된 y축 위치 좌표를 나타낼 수 있다.
Figure pat00014
는 인프라(311)의 x축 위치 좌표,
Figure pat00015
는 인프라(311)의 y축 위치 좌표를 나타낼 수 있다.
Figure pat00016
는 차량(301)의 진행 방향(391)과 x축이 이루는 각도를 나타낼 수 있다. 또한,
Figure pat00017
는 센서로부터 수신된 차량과 인프라 사이의 거리 데이터에 관한 노이즈,
Figure pat00018
는 센서로부터 수신된 차량과 인프라 사이의 방향 데이터에 관한 노이즈를 나타낼 수 있다. 해당 노이즈들은 비선형(non-linear)의 노이즈일 수 있다.
도 4는 글로벌 좌표계에서 실제 차량 주변에 임의로 생성된 파티클들을 도시한다.
도 4를 참조하면, 초기화 단계에서 생성된 파티클들은 글로벌 좌표계에서 차량의 실제 위치 주변에 임의로 생성될 수 있다. 제1 파티클(421), 제2 파티클(422), 및 제3 파티클(423)이 차량 주변으로 생성될 수 있다. 파티클은 차량에서 미리 설정된 개수만큼 생성될 수 있다.
각 파티클 마다 i번째 인프라의 위치가 다르게 측정될 수 있다. 파티클들 마다 배치된 위치가 다르기 때문에, i번째 인프라의 위치는 파티클 마다 다르게 측정될 수 있다. 예를 들어, i번째 인프라의 글로벌 좌표계에서 실제 위치(311)는 제n 파티클(441)에서 측정된 i번째 인프라 가상의 위치(451)와 상이할 수 있다. 하기 수학식 4에서는 파티클에서 측정된 인프라 가상의 위치와 실제 인프라의 위치 차이에 따른 에러(error)를 이용하여 파티클들의 가중치를 계산하는 과정이 설명된다. 하기 수학식 4에서는, 파티클은 N개가 생성되며, 차량 주변으로 인프라가 L개 배치되는 것으로 가정한다. 여기서, N 및 L은 1 이상의 자연수일 수 있다.
Figure pat00019
n은 1부터 N사이 자연수의 값을 가질 수 있으며, i는 1부터 L사이 자연수의 값을 가질 수 있다.
Figure pat00020
는 제n 파티클(441)과 i번째 인프라(311) 사이의 가중치를 나타낼 수 있다.
Figure pat00021
는 센서로부터 수신한 거리 데이터의 x축 방향 노이즈의 공분산(covariance),
Figure pat00022
는 센서로부터 수신한 거리 데이터의 y축 방향 노이즈의 공분산을 나타낼 수 있다.
Figure pat00023
는 제n 파티클(441)로부터 측정된 i번째 인프라의 가상의 위치(451)에 대한 x축 좌표,
Figure pat00024
는 제n 파티클(441)로부터 측정된 i번째 인프라의 가상의 위치(451)에 대한 y축 좌표를 나타낼 수 있다.
Figure pat00025
는 글로벌 좌표계에서 실측 정보(예를 들어, 그라운드 트루스)에 따른 i번째 인프라의 실제 x위치 좌표,
Figure pat00026
는 글로벌 좌표계에서 실측 정보에 따른 i번째 인프라의 y 위치 좌표를 나타낼 수 있다. 파티클과 개별 인프라 사이의 가중치를 계산하기 위하여, 다변량 정규 분포 함수(multivariable normal distribution function)가 사용될 수 있다.
제n 파티클(441)과 i번째 인프라(311) 사이의 가중치인
Figure pat00027
는, 실측 정보에 따른 i번째 인프라의 실제 위치(311)와 제n 파티클에서 측정된 i번째 인프라의 가상의 위치(451) 차이에 상응하는 오류(error)가 커질수록 감소할 수 있다.
하기 수학식 5에서는, 수학식 4에 의하여 계산된 파티클과 인프라 사이 개별적으로 계산된 가중치를 이용하여 파티클의 가중치를 계산하는 과정이 설명된다.
Figure pat00028
n은 1부터 N사이의 자연수 값을 가질 수 있다. 위 수학식 5를 참조하면, 제n 파티클의 가중치는, 제n 파티클의 L개의 모든 인프라에 대한 개별 가중치들을 곱하여 계산될 수 있다.
단계(204)에서는 리샘플링(resampling)이 이루어지는 과정이 설명된다. 수학식 5에 의하여 각 파티클 마다 가중치가 계산된 후, 가중치에 기초하여 파티클을 선택함으로써 차량의 제1 상태 정보를 산출할 수 있다. 예를 들어, 그러나 한정되지 않게, 생성된 파티클들 중 가중치가 가장 높은 파티클을 선택할 수 있다.
하기 표 1은 파티클 필터의 의사 코드(pseudo-code)를 나타낸다.
Figure pat00029
여기서,
Figure pat00030
은 제1 파티클(particle), 제2 파티클, ?? 제N 파티클의 가중치를 나타내며,
Figure pat00031
는 센서에서 측정된 거리의 불확실성,
Figure pat00032
는 센서에서 측정된 방향의 불확실성,
Figure pat00033
는 파티클 필터에서 추정된 차량의 제1 상태 정보를 나타낼 수 있다.
아래에서 설명하겠으나, 파티클 필터로부터 추정된 차량의 최종 상태 정보
Figure pat00034
를 무향 칼만 필터에 융합하여 차량의 위치 추정의 정확도를 향상시킬 수 있다.
도 5는 일 실시예에 따른 필터 융합(fusion)에 의한 차량 위치 추정 방법에 관하여 설명한다.
무향 칼만 필터에서, 파티클 필터로부터 추정된 차량의 제1 상태 정보에 기초하여 차량의 제2 상태 상태 정보를 산출할 수 있다. 파티클 필터로부터 산출된 차량의 제1 상태 정보를 이용하여 무향 칼만 필터에서 차량의 위치를 보다 정확하게 추정할 수 있다. 이하에서는, 무향 칼만 필터와 파티클 필터를 융합한 파티클 보조 무향 칼만 필터(Particle Aided Unscented Kalman Filter, PAUKF)에 관하여 설명한다.
도 5를 참조하면, 단계(511)에서는 파티클 필터에서 바이시클 모델 및 노이즈가 포함된 이전 상태 정보에 기초하여 차량의 상태에 관한 제1 임시 정보를 예측할 수 있다. 단계(512)에서는, 차량은 센서로부터 비가우시안 노이즈가 포함된 측정 데이터를 수신할 수 있다. 단계(513)에서는 제1 임시 정보 및 측정 데이터에 기초하여 차량의 제1 상태 정보를 산출할 수 있다. 단계(521)에서는 무향 칼만 필터에서 노이즈가 포함된 이전 상태 정보에 기초하여 차량의 상태에 관한 제2 임시 정보를 예측할 수 있다. 단계(522)에서는 파티클 필터로부터 산출된 차량의 제1 상태 정보를 수신할 수 있다. 단계(523)에서는 제2 임시 정보를 제1 상태 정보를 이용하여 업데이트함으로써 차량의 최종 상태 정보인 제2 상태 정보를 산출할 수 있다. 이하에서는, 파티클 보조 무향 칼만 필터의 차량 상태 정보의 추정 과정을 수학식과 함께 구체적으로 설명한다.
도 6은 무향 칼만 필터에서 평균 값 주위의 대칭적인 영역으로 생성된 시그마 포인트들(sigma points)을 도시한다.
도 6을 참조하면, 시그마 포인트들은 랜덤 베리어블인 상태 벡터의 기대치, 즉 평균 값(600) 주위의 대칭적인 영역으로 생성될 수 있다. 예를 들어, 제1 시그마 포인트(601)은 제2 시그마 포인트(602)와 평균 값을 기준으로 대칭일 수 있고, 제3 시그마 포인트(603)은 제4 시그마 포인트(604)와 평균 값을 기준으로 대칭일 수 있다. 시그마 포인트들은 하기 수학식 6을 사용하여 계산된 파라미터
Figure pat00035
에 의하여 정의될 수 있다. 수학식 7은 무향 칼만 필터에서 차량의 상태 정보에 대응하는 상태 벡터를 나타낸다.
Figure pat00036
Figure pat00037
수학식 6에서,
Figure pat00038
는 상태 벡터의 크기(size)로서 5를 나타낼 수 있다. 수학식 7에서,
Figure pat00039
는 k 시간에서 제2 예측 모델을 이용하여 예측된 차량의 상태 정보에 대응하는 상태 벡터를 나타낼 수 있다. 수학식 7에서,
Figure pat00040
는 차량의 x 위치 좌표,
Figure pat00041
는 차량의 y 위치 좌표,
Figure pat00042
는 차량의 속도(velocity),
Figure pat00043
는 차량의 요우잉 각도,
Figure pat00044
는 차량의 요우잉 속도(예를 들어, 회전 속도)를 나타낼 수 있다. 상태 벡터
Figure pat00045
는 시그마 포인트들이 개별적으로 갖는 상태 벡터의 평균을 나타낼 수 있다. 하기 수학식 8을 사용하여 시그마 포인트들이 생성될 수 있다.
Figure pat00046
Figure pat00047
는 k 시간에서 시그마 포인트들의 상태 정보에 대응하는 상태 벡터,
Figure pat00048
는 k 시간에서 차량 상태의 평균 값,
Figure pat00049
는 k 시간에서 차량 상태의 공분산 행렬(covariance matrix)을 나타낼 수 있다.
Figure pat00050
는
Figure pat00051
의 행렬을 나타낼 수 있다. 수학식 8에 의하여, 평균 값에 상응하는 시그마 포인트를 포함하여 총
Figure pat00052
개의 시그마 포인트들이 생성될 수 있다.
시그마 포인트들을 생성한 후, 무향 칼만 필터의 제2 예측 모델을 사용하여 차량의 상태에 관한 제2 임시 정보를 예측할 수 있다. 예를 들어, 제2 예측 모델은 등선회율 및 등속도(constant turn rate and velocity magnitude, CTRV) 차량 모션 모델일 수 있다. 이하에서는, 등선회율 및 등속도 차량 모션 모델을 CTRV 모델이라고 설명한다. 제2 예측 모델은 CTRV 모델로 한정하지 않고, 다른 예측 모델이 사용될 수도 있다.
차량의 정확한 위치를 획득하기 위하여, 무향 칼만 필터는 차량의 더 많은 상태들 및 상태들에 대한 비선형 노이즈의 영향을 고려할 수 있다. 예를 들어, 차량은 가속도(acceleration) 및 요우잉 가속도(yaw acceleration)를 노이즈로 고려할 수 있다. 특히, 가속도 및 요우잉 가속도 노이즈는 비선형일 수 있다. 해당 비선형 잡음을 처리하기 위하여, 하기 수학식 9에 표현된 바와 같은 보강된(augmented) 상태 벡터가 고려될 수 있다.
Figure pat00053
수학식 9에서,
Figure pat00054
는 k 시간에서 가속도 및 요우잉 가속도를 상태 벡터로 포함하는 보강된 상태 벡터를 나타낼 수 있다.
Figure pat00055
는 차량 가속도의 노이즈,
Figure pat00056
는 차량 요우잉 가속도의 노이즈를 나타낼 수 있다.
하기 수학식 10 및 수학식 11을 참조하면,
Figure pat00057
는
Figure pat00058
의 분산을 갖는 정규 가우스 분포(normal Gaussian distribution)로 설정될 수 있고,
Figure pat00059
는
Figure pat00060
의 분산을 갖는 정규 가우스 분포로 설정될 수 있다.
Figure pat00061
Figure pat00062
차량은 차량의 상태 벡터로 가속도 및 요우잉 가속도를 고려함으로써, 공분산 행렬
Figure pat00063
는 하기 수학식 12에 표현된 바와 같이, 가속도 노이즈의 분산 및 요우잉 가속도 노이즈의 분산을 포함하는
Figure pat00064
로 보강될 수 있다.
Figure pat00065
일 실시예에 따른 PAUKF 필터에서, k 시간에서의 확장된 상태 정보
Figure pat00066
및 제2 예측 모델을 사용하여 차량의 상태에 관한 제2 임시 정보를 예측할 수 있다. 제2 예측 모델은 CTRV 모델일 수 있다. 수학식 13 및 수학식 14에서는 k 시간에서 추정된 차량의 이전 상태 정보 및 CTRV 모델을 이용하여 k+1 시간에서의 차량의 상태에 관한 제2 임시 정보를 예측하는 과정을 표현한다. 제2 임시 정보란, 이전 시간에서 추정된 차량의 이전 상태 정보로부터 제2 예측 모델을 이용하여 예측된 시그마 포인트들의 상태 벡터에 대응하는 차량의 예측된 상태 정보를 의미한다.
Figure pat00067
수학식 13에서,
Figure pat00068
은 k+1 시간에서의 차량의 상태 벡터,
Figure pat00069
는 k 시간에서 차량의 상태 벡터, 및
Figure pat00070
는 k 시간에서 차량의 상태 벡터를 미분한 값을 나타낼 수 있다.
수학식 13을 기초로, 수학식 14에서는 k 시간에서의 차량의 확장된 상태 벡터 및 CTRV 모델을 이용하여 k+1 시간에서의 차량의 확장된 상태 벡터를 예측하는 과정을 설명한다.
Figure pat00071
수학식 14에서,
Figure pat00072
는 k+1 시간에서의 차량의 확장된 상태 벡터,
Figure pat00073
는 k 시간에서의 차량의 확장된 상태 벡터를 나타낼 수 있다.
더 나아가, 가속도 및 요우잉 가속도를 고려하는 확장된 상태 벡터 예측 단계에서
Figure pat00074
는 확장된 상태 벡터의 크기로서 7을 나타내는
Figure pat00075
로 변환되고, 파라미터
Figure pat00076
는 하기 수학식 15에 의하여 계산될 수 있다.
Figure pat00077
Figure pat00078
각 시그마 포인트들의 가중치는 하기 수학식 17 및 수학식 18에 기초하여 계산될 수 있다.
수학식 17은 평균 값에 상응하는 시그마 포인트(600)의 가중치를 계산하는 과정을 설명하고, 수학식 18은 시그마 포인트(600) 이외의 시그마 포인트들의 가중치를 계산하는 과정을 설명한다.
Figure pat00079
Figure pat00080
수학식 19는 시그마 포인트들의 상태 정보 및 가중치에 기초하여 평균 상태 벡터를 계산하는 과정을 설명하고, 수학식 20은 시그마 포인트들의 상태 정보 및 평균 상태 벡터에 기초하여 공분산을 계산하는 과정을 설명한다.
Figure pat00081
Figure pat00082
수학식 19에서,
Figure pat00083
는 k+1 시간에서 시그마 포인트들의 상태 벡터에 가중치가 곱해진 예측된 평균 상태 벡터를 나타낼 수 있다.
수학식 20에서,
Figure pat00084
는 k+1시간에서 예측된 공분산 행렬을 나타낼 수 있다.
수학식 14를 참조하면, k+1 시간에서의 차량의 확장된 상태 벡터를 CTRV 모델을 이용하여 예측하는 경우, 가속도 노이즈 및 요우잉 가속도 노이즈에 관한 정보를 x좌표, y좌표, 차량의 속도, 요우잉 각도, 요우잉 속도가 포함할 수 있다. 따라서, 보강된 상태 벡터는 가속도 상태 벡터 및 요우잉 가속도 상태 벡터를 제외하고, 5의 크기(size)를 갖는 기존 상태 벡터로 다시 변경될 수 있다.
이하에서는, 무향 칼만 필터에서 예측된 제2 임시 정보를 파티클 필터로부터 수신한 제1 상태 정보를 이용하여 업데이트함으로써 차량의 제2 상태 정보를 산출하는 과정을 설명한다.
무향 칼만 필터에서는 차량이 센서로부터 노이즈를 갖는 측정 데이터를 수신하는 대신, 파티클 필터로부터 산출된 차량의 제1 상태 정보(
Figure pat00085
)를 수신하여 제2 상태 정보를 산출할 수 있다. 다시 말해, 차량의 제1 상태 정보(
Figure pat00086
)는 가상의 센서(virtual sensor)가 되어, 기존 센서보다 더 정확한 차량의 상태를 산출할 수 있다.
하기 수학식 21은 파티클 필터로부터 수신하는 제1 상태 정보에 관한 정보를 나타낸다.
Figure pat00087
수학식 21을 참조하면, 파티클 필터로부터 수신하는 제1 상태 정보는 차량의 x 위치 좌표, y 위치 좌표, 및 요우잉 각도를 포함할 수 있다.
수학식 22에서는, 측정 값에 상응하는 측정 벡터를 예측하는 과정을 설명한다.
Figure pat00088
Figure pat00089
수학식 22에서,
Figure pat00090
는 k+1 시간에서의 측정 벡터를 나타낼 수 있다.
Figure pat00091
는 k+1 시간에서의 시그마 포인트들의 상태 벡터를 나타낼 수 있고,
Figure pat00092
은 k+1 시간에서의 비선형 노이즈(non-linear)를 나타낼 수 있다.
수학식 23의 A행렬을 참조하면, 수학식 22에서 측정 벡터를 계산하기 위하여 제2 임시 정보의 k+1 시간에서 예측된 시그마 포인트들의 상태 벡터로부터 차량의 위치 좌표(x 위치 좌표, y 위치 좌표) 및 요우잉 각도에 관한 정보를 추출할 수 있다. k+1 시간에서의 시그마 포인트들의 상태 벡터는 이전 k 시간에서의 시그마 포인트들을 이용하여 상태 벡터를 계산하므로, 노이즈가 포함될 수 있다. 따라서, 수학식 23에서는 비선형 노이즈인
Figure pat00093
을 추가하여 모델링할 수 있다.
수학식 24는 예측된 평균 측정 벡터를 계산하는 과정을 설명한다.
Figure pat00094
수학식 24에서,
Figure pat00095
는 k+1시간에서의 예측된 평균 측정 벡터를 나타낼 수 있다.
이어서, 수학식 25는 측정 공분산을 예측하는 과정을 설명한다. 수학식 26은 노이즈 공분산을 나타낼 수 있다.
Figure pat00096
Figure pat00097
수학식 25에서,
Figure pat00098
는 k+1 시간에서 예측된 측정 공분산 행렬을 나타낼 수 있다. 수학식 26에서,
Figure pat00099
는 파티클 필터에서 센서로부터 수신한 거리 데이터의 x축 방향 노이즈의 공분산,
Figure pat00100
는 파티클 필터에서 센서로부터 수신한 거리 데이터의 y축 방향 노이즈의 공분산,
Figure pat00101
는 파티클 필터에서 센서로부터 수신한 방향 데이터의 요 각도 노이즈의 공분산을 나타낼 수 있다. 측정 공분산 행렬
Figure pat00102
는 수학식 25 및 26을 참조하면, 파티클 필터에서 센서로부터 수신한 데이터의 노이즈에 따라 변할 수 있다.
하기 수학식 27에서는 교차 상관 관계 행렬(cross-correlation matrix)을 계산하는 과정이 설명된다.
Figure pat00103
Figure pat00104
는 k+1시간에서 시그마 포인트들의 상태 정보 및 k+1시간에서 시그마 포인트들의 측정 정보 사이의 교차 상관 관계 행렬을 나타낼 수 있다. 교차 상관 관계 행렬과 측정 공분산에 기초하여, 칼만 이득(Kalman gain)은 수학식 28과 같이 계산된다.
Figure pat00105
Figure pat00106
는 k+1 시간에서의 칼만 이득을 나타낼 수 있다.
차량의 제2 임시 정보는 파티클 필터로부터 수신한 제1 상태 정보를 이용하여 제2 상태 정보로 업데이트될 수 있다. 하기 수학식 29는 제2 상태 정보는 차량의 위치 좌표, 요우잉 각도, 및 차량의 상태에 대한 공변량 행렬(covariance matrix)을 포함할 수 있다.
Figure pat00107
수학식 29에서,
Figure pat00108
는 PAUKF에서 계산된 차량의 최종 상태 벡터를 나타낸다.
Figure pat00109
는 차량의 위치 좌표(x 좌표, y 좌표) 및 요우잉 각도에 관한 정보를 포함할 수 있다. 수학식 29를 참조하면, 차량의 최종 상태 벡터는 파티클 필터로부터 수신한 제1 상태 정보 및 칼만 이득에 기초하여 산출될 수 있다.
Figure pat00110
수학식 30에서,
Figure pat00111
는 PAUKF에서 계산된 차량의 최종 상태에 대한 공변량 행렬을 나타낼 수 있다. 측정 공변량 행렬, 칼만 이득, 및 k 시간에서의 공변량 행렬에 기초하여 k+1시간에서의 공변량 행렬을 산출할 수 있다.
차량의 최종 상태 벡터
Figure pat00112
및 차량의 최종 상태에 대한 공변량 행렬
Figure pat00113
를 포함하는 제2 상태 정보에 기초하여 차량의 조향, 속도, 및 가속도 중 적어도 하나가 조정될 수 있다.
PAUKF 알고리즘의 전체 의사 코드(pseudo code) 하기 표 2에서 설명된다.
Figure pat00114
도 7 내지 도 9는 파티클 보조 무향 칼만 필터을 통한 차량의 위치 추정 결과에 관하여 설명한다.
이하에서는 시뮬레이션에 따른 차량의 위치 추정 결과에 관하여 설명한다. 상술한 시뮬레이션에서, 거리 센서의 주파수는 100Hz로 설정되고, 차량은 가우시안 노이즈 및 비가우시안 노이즈를 포함하는 데이터를 센서로부터 수신할 수 있다. 예를 들어, 가우시안 노이즈는 normrnd 함수를 이용하여 생성되고, 비가우시안 노이즈는 정현파 함수를 이용하여 생성된다. 하기 표 3은 생성된 노이즈를 설명한다.
Figure pat00115
도 7은 시뮬레이션 상에서 차량이 이동하는 도로를 나타낸다.
도로(701)은 S자 형태의 도로를 나타내며, 도로(702)는 일직선의 형태를 갖는 도로를 나타낸다. 도로(701)는 곡선 도로에서 각 필터의 성능을 검증하는데 사용되고, 도로(702)는 직선 도로에서 각 필터의 성능을 검증하기 위하여 사용된다. 도로 주변에는 12개의 인프라들이 배치될 수 있고, 지도가 변경되더라도 인프라들의 위치는 고정되는 것으로 시뮬레이션 된다. 인프라들의 위치가 필터들의 성능에 영향을 미치지 않도록 인프라들은 대칭으로 배치될 수 있다. 차량의 속도는 일정한 값으로 설정되고, 속도는 60km/h, 80km/h, 100km/h, 및 120km/h로 각각 설정되어 시뮬레이션 된다.
수학식 31을 참조하면, 필터들의 성능은 평균 제곱근 오차(Root Mean Square Error, RMSE)를 기반으로 평가된다.
Figure pat00116
수학식 31에서, N은 데이터 포인트들(data points)의 수를 나타낼 수 있다. 필터로부터 추정된 차량의 궤적과 차량의 실제 궤적을 비교하여 필터의 성능을 검증할 수 있다. 필터로부터 추정된 차량의 궤적은 도 8에서 설명한다.
도 8a는 S자형 도로에서 추정된 필터들의 궤적 결과를 도시한다.
궤적(801)은 노이즈를 포함하는 차량의 궤적을 나타낸다. 궤적(802)은 파티클 필터(PF)로부터 추정된 차량의 궤적을 나타낸다. 궤적(803)은 무향 칼만 필터(UKF)로부터 추정된 차량의 궤적을 나타낸다. 궤적(804)는 파티클 보조 무향 칼만 필터(PAUKF)의 궤적을 나타낸다. 궤적(805)은 차량의 실제 궤적을 나타낸다.
파티클 필터로부터 추정된 차량의 궤적(802)은 차량의 실제 궤적(805)과 가깝지만, 궤적 오류가 굉장히 크다. 파티클 필터는 노이즈를 갖는 인프라와 차량의 상대적 거리와 노이즈를 갖는 차량 데이터만으로 차량 위치를 결정하기 때문이다. 파티클 필터와 비교할 때 무향 칼만 필터로부터 추정된 차량의 궤적(803)은 비교적 부드럽게 나타난다. 그러나, 무향 칼만 필터는 GPS 데이터의 노이즈를 필터링할 수 없다. 무향 칼만 필터의 GPS 측정은 분산이 높고 거리 센서를 사용하지 않기 때문에, 무향 칼만 필터는 측정보다 차량 모델에 더 의존하게 된다. 측정 노이즈는 무향 칼만 필터가 위치 및 요우잉의 변화에 덜 민감하게 만든다. 반면, 일 실시예에 따른 파티클 보조 무향 칼만 필터는 위치 및 요우잉의 변화에 더 빠르고 정확하게 반응할 수 있다.
하기 표 4는 필터의 성능에 대한 결과를 나타낸다. 무향 칼만 필터는 거리 센서 데이터를 사용하지 않기 때문에 파티클 필터 및 파티클 보조 무향 칼만 필터와 비교하는 것은 적절하지 않다. 따라서, 표 4에는 무향 칼만 필터에 대한 RMSE를 나타내지 않는다. PAUKF에 대한 추정 오차의 평균은 1.08m로 나타나며, 분산은 0.7147m로 나타난다. 차량의 속도가 증가하는 경우, PF 및 PAUKF 추정 오차가 약간 증가한다. 그러나 21.336m에서 21.712m까지 노이즈 변화의 RMSE 크기를 고려하면 차량의 속도가 60km/h에서 120km/h로 증가하더라도 추정 오차의 RMSE는 변하지 않는다. 가우시안 노이즈와 비교하여 비가우시안 노이즈는 더 큰 평균값을 생성한다. PF의 정밀도는 노이즈가 증가하더라도 변하지 않으며 정밀도는 5.489m 내지5.959m의 RMSE 범위와 거의 동일하다. PAUKF의 RMSE 범위는 노이즈가 증가하고 차량의 속도가 증가하더라도 1.440m 내지 1.772m로 나타난다. PAUKF는 PF에 비해 정확도를 4.028m 내지 4.049m 향상시킬 수 있다.
Figure pat00117
도 8b는 직선형 도로에서 추정된 필터들의 궤적 결과를 도시한다.
시뮬레이션 상에서 차량의 속도는 60km/h이고, 노이즈는 가우시안을 따르르 수 있다. 직선 도로에 의한 추정 결과는 차량이 X 방향으로만 이동하는 경우의 성능을 결정하는데 사용된다. 도 8b를 참조하면, PAUKF가 PF 및 UKF 보다 실제 차량의 궤적(805)에 가깝게 수렴한다. 차량이 X 방향으로만 이동하기 때문에, Y방향으로의 이동에 대한 정보가 없다. 따라서, PAUKF의 추정이 UKF보다 우수하더라도, 응답시간을 개선하기 위하여 PAUKF는 Y방향의 노이즈에 대해 더 많이 믿는 경향이 있다. 결과적으로 PAUKF는 도 8b에 도시된 바와 같이 Y방향으로는 충분히 정밀하지 않을 수 있다.
차량이 X 방향으로 이동할 때 필터 성능에 대한 결과는 표 5에 도시된다. PF 및 PAUKF 추정 특성들은 S자형 도로 주행시 차량과 동일함을 알 수 있다. 추정 결과는 도로가 변경되어도 필터의 성능이 변하지 않음을 보여준다. PF의 RMSE는 5.384m 내지 5.692m로 나타나며, PAUKF는 소음이 증가하고 속도가 증가하더라도 RMSE는 1.312m 내지 1.800m로 나타난다. PAUKF는 PF에 비해 정확도가 3.892m 내지 4.072m로 향상된다.
Figure pat00118
도 9는 일 실시예에 따른 차량의 구성을 설명하는 블록도이다.
일 실시예에 따른 위치 추정을 수행하는 차량에 있어서, 차량은 프로세서, 메모리, 및 입출력 인터페이스로 구성될 수 있다. 프로세서(910)는 파티클 필터를 이용하여 외부 센서로부터 수신되는 측정 데이터 및 이전 시점에서 추정된 이전 상태 정보에 기초하여 차량의 제1 상태 정보를 산출할 수 있다. 또한, 프로세서(910)는 무향 칼만 필터를 이용하여 제1 상태 정보에 기초하여 차량의 제2 상태 정보를 산출할 수 있다. 메모리(920)는 파티클 필터의 제1 예측 모델 및 무향 칼만 필터의 제2 예측 모델을 저장할 수 있다.
이상에서 설명된 실시예들은 하드웨어 구성요소, 소프트웨어 구성요소, 및/또는 하드웨어 구성요소 및 소프트웨어 구성요소의 조합으로 구현될 수 있다. 예를 들어, 실시예들에서 설명된 장치, 방법 및 구성요소는, 예를 들어, 프로세서, 콘트롤러, ALU(arithmetic logic unit), 디지털 신호 프로세서(digital signal processor), 마이크로컴퓨터, FPGA(field programmable gate array), PLU(programmable logic unit), 마이크로프로세서, 또는 명령(instruction)을 실행하고 응답할 수 있는 다른 어떠한 장치와 같이, 하나 이상의 범용 컴퓨터 또는 특수 목적 컴퓨터를 이용하여 구현될 수 있다. 처리 장치는 운영 체제(OS) 및 상기 운영 체제 상에서 수행되는 하나 이상의 소프트웨어 애플리케이션을 수행할 수 있다. 또한, 처리 장치는 소프트웨어의 실행에 응답하여, 데이터를 접근, 저장, 조작, 처리 및 생성할 수도 있다. 이해의 편의를 위하여, 처리 장치는 하나가 사용되는 것으로 설명된 경우도 있지만, 해당 기술분야에서 통상의 지식을 가진 자는, 처리 장치가 복수 개의 처리 요소(processing element) 및/또는 복수 유형의 처리 요소를 포함할 수 있음을 알 수 있다. 예를 들어, 처리 장치는 복수 개의 프로세서 또는 하나의 프로세서 및 하나의 콘트롤러를 포함할 수 있다. 또한, 병렬 프로세서(parallel processor)와 같은, 다른 처리 구성(processing configuration)도 가능하다.
소프트웨어는 컴퓨터 프로그램(computer program), 코드(code), 명령(instruction), 또는 이들 중 하나 이상의 조합을 포함할 수 있으며, 원하는 대로 동작하도록 처리 장치를 구성하거나 독립적으로 또는 결합적으로(collectively) 처리 장치를 명령할 수 있다. 소프트웨어 및/또는 데이터는, 처리 장치에 의하여 해석되거나 처리 장치에 명령 또는 데이터를 제공하기 위하여, 어떤 유형의 기계, 구성요소(component), 물리적 장치, 가상 장치(virtual equipment), 컴퓨터 저장 매체 또는 장치, 또는 전송되는 신호 파(signal wave)에 영구적으로, 또는 일시적으로 구체화(embody)될 수 있다. 소프트웨어는 네트워크로 연결된 컴퓨터 시스템 상에 분산되어서, 분산된 방법으로 저장되거나 실행될 수도 있다. 소프트웨어 및 데이터는 하나 이상의 컴퓨터 판독 가능 기록 매체에 저장될 수 있다.
실시예에 따른 방법은 다양한 컴퓨터 수단을 통하여 수행될 수 있는 프로그램 명령 형태로 구현되어 컴퓨터 판독 가능 매체에 기록될 수 있다. 컴퓨터 판독 가능 매체는 프로그램 명령, 데이터 파일, 데이터 구조 등을 단독으로 또는 조합하여 포함할 수 있다. 컴퓨터 판독 가능 매체에 기록되는 프로그램 명령은 실시예를 위하여 특별히 설계되고 구성된 것들이거나 컴퓨터 소프트웨어 당업자에게 공지되어 사용 가능한 것일 수도 있다. 컴퓨터 판독 가능 기록 매체의 예에는 하드 디스크, 플로피 디스크 및 자기 테이프와 같은 자기 매체(magnetic media), CD-ROM, DVD와 같은 광기록 매체(optical media), 플롭티컬 디스크(floptical disk)와 같은 자기-광 매체(magneto-optical media), 및 롬(ROM), 램(RAM), 플래시 메모리 등과 같은 프로그램 명령을 저장하고 수행하도록 특별히 구성된 하드웨어 장치가 포함된다. 프로그램 명령의 예에는 컴파일러에 의해 만들어지는 것과 같은 기계어 코드뿐만 아니라 인터프리터 등을 사용해서 컴퓨터에 의해서 실행될 수 있는 고급 언어 코드를 포함한다. 상기된 하드웨어 장치는 실시예의 동작을 수행하기 위해 하나 이상의 소프트웨어 모듈로서 작동하도록 구성될 수 있으며, 그 역도 마찬가지이다.
이상과 같이 실시예들이 비록 한정된 도면에 의해 설명되었으나, 해당 기술분야에서 통상의 지식을 가진 자라면 상기를 기초로 다양한 기술적 수정 및 변형을 적용할 수 있다. 예를 들어, 설명된 기술들이 설명된 방법과 다른 순서로 수행되거나, 및/또는 설명된 시스템, 구조, 장치, 회로 등의 구성요소들이 설명된 방법과 다른 형태로 결합 또는 조합되거나, 다른 구성요소 또는 균등물에 의하여 대치되거나 치환되더라도 적절한 결과가 달성될 수 있다.

Claims (12)

  1. 차량(vehicle)에 의해 수행되는 위치 추정 방법에 있어서,
    파티클 필터(Particle Filter, PF)를 이용하여 센서(sensor)로부터 수신되는 측정 데이터 및 이전 시점에서 추정된 이전 상태 정보에 기초하여 상기 차량의 제1 상태 정보를 산출하는 단계; 및
    무향 칼만 필터(Unscented Kalman Filter, UKF)를 이용하여 상기 제1 상태 정보에 기초하여 상기 차량의 제2 상태 정보를 산출하는 단계
    를 포함하고,
    상기 센서로부터 수신되는 측정 데이터는 비가우시안 노이즈(non-Gaussian noise)를 포함하는,
    위치 추정 방법.
  2. 제1항에 있어서,
    상기 차량의 제1 상태 정보를 산출하는 단계는,
    미리 정한 개수의 파티클(particle)을 생성하는 단계;
    상기 파티클 필터의 제1 예측 모델을 사용하여 상기 차량의 상태에 관한 제1 임시 정보를 예측하는 단계; 및
    상기 생성된 파티클 마다 가중치(weight)를 계산하는 단계
    를 포함하는 위치 추정 방법.
  3. 제2항에 있어서,
    상기 제1 예측 모델은 바이시클 모델(bicycle model)인,
    위치 추정 방법.
  4. 제1항에 있어서,
    상기 차량의 제2 상태 정보를 산출하는 단계는,
    시그마 포인트들(sigma points)을 생성하는 단계;
    상기 무향 칼만 필터의 제2 예측 모델을 사용하여 상기 차량의 상태에 관한 제2 임시 정보를 예측하고, 상기 제2 임시 정보로부터 상기 차량의 측정 값을 예측하는 단계; 및
    상기 파티클 필터로부터 추출된 상기 제1 상태 정보, 상기 제2 임시 정보, 및 상기 측정 값에 기초하여 상기 차량의 상기 제2 상태 정보를 산출하는 단계
    를 포함하는 위치 추정 방법.
  5. 제4항에 있어서,
    상기 제2 임시 정보로부터 상기 차량의 측정 값을 예측하는 단계는,
    상기 제2 임시 정보로부터 상기 차량의 위치 좌표 및 요우잉 각도에 관한 정보를 추출하는 단계
    를 포함하는 위치 추정 방법.
  6. 제4항에 있어서,
    상기 제2 예측 모델은 등선회율 및 등속도(constant turn rate and velocity, CTRV) 모델인,
    위치 추정 방법.
  7. 제1항에 있어서,
    상기 제1 상태 정보는,
    상기 차량의 위치 좌표 및 요우잉 각도에 관한 정보를 포함하는,
    위치 추정 방법.
  8. 제1항에 있어서,
    상기 제2 상태 정보는,
    상기 차량의 위치 좌표, 요우잉 각도, 및 상기 차량의 상태에 대한 공변량 행렬(covariance matrix)을 포함하는,
    위치 추정 방법.
  9. 제8항에 있어서,
    상기 차량의 제2 상태 정보를 산출하는 단계는,
    측정 공변량 행렬, 칼만 이득(Kalman gain), 및 이전 공변량 행렬에 기초하여 상기 공변량 행렬을 산출하는 단계
    를 포함하는 위치 추정 방법.
  10. 제1항에 있어서,
    상기 제2 상태 정보에 기초하여 상기 차량의 조향, 속도, 및 가속도 중 적어도 하나를 조정하는 단계
    를 더 포함하는 위치 추정 방법.
  11. 제1항 내지 제10항 중 어느 한 항의 방법을 수행하기 위한 명령어를 포함하는 하나 이상의 컴퓨터 프로그램을 저장한 컴퓨터 판독 가능 기록 매체.
  12. 위치 추정을 수행하는 차량에 있어서,
    파티클 필터의 제1 예측 모델 및 무향 칼만 필터의 제2 예측 모델을 저장하는 메모리; 및
    상기 파티클 필터를 이용하여 센서로부터 수신되는 측정 데이터 및 이전 시점에서 추정된 이전 상태 정보에 기초하여 상기 차량의 제1 상태 정보를 산출하고, 상기 무향 칼만 필터를 이용하여 상기 제1 상태 정보에 기초하여 상기 차량의 제2 상태 정보를 산출하는 프로세서
    를 포함하고,
    상기 센서로부터 수신되는 측정 데이터는 비가우시안 노이즈를 포함하는,
    차량.
KR1020200145317A 2020-11-03 2020-11-03 센서 퓨전에 의한 차량의 위치 추정 방법 KR102460842B1 (ko)

Priority Applications (1)

Application Number Priority Date Filing Date Title
KR1020200145317A KR102460842B1 (ko) 2020-11-03 2020-11-03 센서 퓨전에 의한 차량의 위치 추정 방법

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
KR1020200145317A KR102460842B1 (ko) 2020-11-03 2020-11-03 센서 퓨전에 의한 차량의 위치 추정 방법

Publications (2)

Publication Number Publication Date
KR20220059754A true KR20220059754A (ko) 2022-05-10
KR102460842B1 KR102460842B1 (ko) 2022-10-31

Family

ID=81592158

Family Applications (1)

Application Number Title Priority Date Filing Date
KR1020200145317A KR102460842B1 (ko) 2020-11-03 2020-11-03 센서 퓨전에 의한 차량의 위치 추정 방법

Country Status (1)

Country Link
KR (1) KR102460842B1 (ko)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116908832A (zh) * 2023-09-11 2023-10-20 北京城建智控科技股份有限公司 列车速度确定方法及装置

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR100816269B1 (ko) * 2006-09-22 2008-03-25 학교법인 포항공과대학교 언센티드 필터를 적용한 강인한 동시 위치 추정 및 지도작성 방법
US10365364B1 (en) * 2018-05-18 2019-07-30 Zendar Inc. Systems and methods for detecting objects
WO2020085062A1 (ja) * 2018-10-24 2020-04-30 パイオニア株式会社 計測精度算出装置、自己位置推定装置、制御方法、プログラム及び記憶媒体

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR100816269B1 (ko) * 2006-09-22 2008-03-25 학교법인 포항공과대학교 언센티드 필터를 적용한 강인한 동시 위치 추정 및 지도작성 방법
US10365364B1 (en) * 2018-05-18 2019-07-30 Zendar Inc. Systems and methods for detecting objects
WO2020085062A1 (ja) * 2018-10-24 2020-04-30 パイオニア株式会社 計測精度算出装置、自己位置推定装置、制御方法、プログラム及び記憶媒体

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
Estimation de l'état du système non linéaire à l'aide d'un filtre à particules non parfumé* *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116908832A (zh) * 2023-09-11 2023-10-20 北京城建智控科技股份有限公司 列车速度确定方法及装置
CN116908832B (zh) * 2023-09-11 2024-01-23 北京城建智控科技股份有限公司 列车速度确定方法及装置

Also Published As

Publication number Publication date
KR102460842B1 (ko) 2022-10-31

Similar Documents

Publication Publication Date Title
EP3714290B1 (en) Lidar localization using 3d cnn network for solution inference in autonomous driving vehicles
JP6757820B2 (ja) 重み付け幾何学的コストを有する区分的螺旋曲線を使用した基準線平滑化方法
EP3714285B1 (en) Lidar localization using rnn and lstm for temporal smoothness in autonomous driving vehicles
WO2020154970A1 (en) Deep learning–based feature extraction for lidar localization of autonomous driving vehicles
CN108995657A (zh) 操作自动驾驶车辆的方法和数据处理系统
JP6578331B2 (ja) 自律走行車のコマンド遅延を決定するための方法
US11704554B2 (en) Automated training data extraction method for dynamic models for autonomous driving vehicles
Naets et al. Online state and input force estimation for multibody models employing extended Kalman filtering
CN109030867B (zh) 使用加速度传感器和地磁传感器计算角速度的方法和设备
US20190332743A1 (en) Wind Simulation Device
US10379224B2 (en) Invariant particle filtering
CN110389582A (zh) 利用多个线索跟踪对象
US10754030B2 (en) Methods and systems for radar simulation and object classification
KR102460842B1 (ko) 센서 퓨전에 의한 차량의 위치 추정 방법
US7813907B2 (en) Hybrid method for enforcing curvature related boundary conditions in solving one-phase fluid flow over a deformable domain
KR102489865B1 (ko) 다중 필터 및 센서 퓨전 기반의 차량의 위치 추정 방법
JP2022081396A (ja) 車両側位方法及び装置
KR20220109537A (ko) 자율주행 자동차의 센서 시스템 교정 장치 및 그 방법
JP2023171384A (ja) アビオニクスシステム、航空機、及びストレージ媒体
JP6849803B2 (ja) 静止状態判定方法および装置
KR102431724B1 (ko) 센서 데이터를 통한 자율주행 차량의 실시간 다이나믹 추정 방법 및 장치
KR102316274B1 (ko) 시뮬레이션을 위한 주변 차량의 주행 궤적 생성 방법
WO2022186777A1 (en) Method and system for localizing a mobile robot
KR102589970B1 (ko) 객체 측위 방법 및 장치
KR102075831B1 (ko) V2v 통신과 레이다 센서 객체 번호 일치 방법 및 장치

Legal Events

Date Code Title Description
E701 Decision to grant or registration of patent right
GRNT Written decision to grant