KR102489865B1 - 다중 필터 및 센서 퓨전 기반의 차량의 위치 추정 방법 - Google Patents

다중 필터 및 센서 퓨전 기반의 차량의 위치 추정 방법 Download PDF

Info

Publication number
KR102489865B1
KR102489865B1 KR1020210069075A KR20210069075A KR102489865B1 KR 102489865 B1 KR102489865 B1 KR 102489865B1 KR 1020210069075 A KR1020210069075 A KR 1020210069075A KR 20210069075 A KR20210069075 A KR 20210069075A KR 102489865 B1 KR102489865 B1 KR 102489865B1
Authority
KR
South Korea
Prior art keywords
vehicle
particle
information
infrastructure
calculating
Prior art date
Application number
KR1020210069075A
Other languages
English (en)
Other versions
KR20220160881A (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 KR1020210069075A priority Critical patent/KR102489865B1/ko
Publication of KR20220160881A publication Critical patent/KR20220160881A/ko
Application granted granted Critical
Publication of KR102489865B1 publication Critical patent/KR102489865B1/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/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
    • 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/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
    • B60W40/105—Speed
    • 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
    • B60W40/107—Longitudinal acceleration
    • 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
    • B60W40/114—Yaw movement
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/01—Satellite radio beacon positioning systems transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/13—Receivers
    • G01S19/24—Acquisition or tracking or demodulation of signals transmitted by the system
    • G01S19/25—Acquisition or tracking or demodulation of signals transmitted by the system involving aiding data received from a cooperating element, e.g. assisted GPS
    • G01S19/252—Employing an initial estimate of location in generating assistance data
    • 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/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
    • B60W2520/00—Input parameters relating to overall vehicle dynamics
    • B60W2520/14—Yaw

Landscapes

  • Engineering & Computer Science (AREA)
  • Mechanical Engineering (AREA)
  • Physics & Mathematics (AREA)
  • Transportation (AREA)
  • Automation & Control Theory (AREA)
  • Mathematical Physics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Combustion & Propulsion (AREA)
  • Chemical & Material Sciences (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)
  • Traffic Control Systems (AREA)

Abstract

일 실시예에 따른 프로세서에 의해 수행되는 차량의 위치 추정 방법은, 센서로부터 적어도 하나의 기반시설의 3차원 위치 데이터를 수신하는 단계, 파티클 필터를 이용하여 수신된 3차원 위치 데이터 및 차량의 제1 임시 정보에 기초하여 차량의 제1 상태 정보를 산출하는 단계, 무향 칼만 필터(Unscented Kalman Filter, UKF)를 이용하여 제1 상태 정보에 기초하여 제2 상태 정보를 산출하는 단계를 포함하고, 3차원 위치 데이터는 차량의 종축(longitudinal axis), 횡축(lateral axis), 및 수직축(vertical axis)에 대한 비가우시안 노이즈(non-Gaussian noise) 성분을 포함할 수 있다.

Description

다중 필터 및 센서 퓨전 기반의 차량의 위치 추정 방법{METHOD FOR VEHICLE LOCATION ESTIMATION BASED ON SENSOR FUSION AND MULTI FILTER}
이하의 설명은 파티클 필터(Particle Filter, PF) 및 무향 칼만 필터(Unscented Kalman Filter, UKF)를 이용하여 차량의 위치를 추정하는 방법에 관한 것이다.
차량의 위치 추정은 자율 주행 차량 운영의 핵심 요소이다. 차량의 위치를 파악하기 위하여 가장 일반적으로 사용되는 위치 파악 방법은 GPS이다. 그러나 GPS 신호는 위성의 수와 도시 환경에서 다중 경로 효과의 영향을 받는다. GPS가 충분한 양의 위성을 확보 할 수 없는 경우에 위치 파악에 대한 대안적인 접근 방식이 필요하다. 고화질 지도(HD map)가 개발됨에 따라 지도 매칭을 기반으로 차량 위치를 파악하는 것이 가능해졌다. 그러나 HD 맵은 노이즈를 발생시키며, 차량에 부착된 센서 또한 노이즈를 감지한다. 따라서 노이즈를 필터링하는 방법이 필요하다.
칼만 필터(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)가 제한되어 있다. 다시 말해, 효과적인 파티클 필터 기반 시스템 모델을 생성하는 경우, 정밀도와 계산 리소스 사이에 트레이드 오프 관계가 있다. 무향 칼만 필터는 비가우시안 노이즈를 필터링 하는데 한계점이 존재하여, 파티클 필터는 계산 리소스가 제한되기 때문에 파티클을 일정량 이상 생성하지 못하여 비가우시안 노이즈의 속성을 정확하게 표현하지 못하는 문제점 뿐만 아니라 추정결과가 비연속인 문제점도 존재한다. 이하에서는, 무향 칼만 필터 및 파티클 필터를 융합함으로써 비가우시안 노이즈를 효과적으로 필터링하고, 차량의 위치 추정의 정확도를 높이는 센서 퓨전 방법이 설명된다.
일 실시예에 따른 프로세서에 의해 수행되는 차량의 위치 추정 방법은, 센서(sensor)로부터 적어도 하나의 기반시설(infrastructure)의 3차원 위치 데이터를 수신하는 단계, 파티클 필터(Particle Filter, PF)를 이용하여 상기 수신된 3차원 위치 데이터 및 상기 차량의 제1 임시 정보에 기초하여 상기 차량의 제1 상태 정보를 산출하는 단계, 및 무향 칼만 필터(Unscented Kalman Filter, UKF)를 이용하여 상기 제1 상태 정보에 기초하여 제2 상태 정보를 산출하는 단계를 포함하고, 상기 3차원 위치 데이터는, 상기 차량의 종축(longitudinal axis), 횡축(lateral axis), 및 수직축(vertical axis)에 대한 비가우시안 노이즈(non-Gaussian noise) 성분을 포함할 수 있다.
상기 3차원 위치 데이터는, 상기 센서와 상기 적어도 하나의 기반시설 사이의 베어링 각도(bearing angle) 정보 및 고도 각도(elevation angle) 정보를 포함할 수 있다.
일 실시예에 따른 프로세서에 의해 수행되는 차량의 위치 추정 방법의 상기 차량의 제1 상태 정보를 산출하는 단계는, 미리 정한 개수의 파티클(particle)을 생성하는 단계, 상기 파티클 필터의 제1 예측 모델을 사용하여 상기 차량의 상태에 관한 상기 제1 임시 정보를 예측하는 단계, 및 상기 생성된 파티클 마다 가중치(weight)를 산출하는 단계를 포함할 수 있다.
일 실시예에 따른 프로세서에 의해 수행되는 차량의 위치 추정 방법의 상기 생성된 파티클 마다 가중치를 산출하는 단계는, 상기 적어도 하나의 기반시설 마다 상기 예측된 제1 임시 정보 및 해당 기반시설에 대한 실제 위치 데이터에 기초하여 임시 가중치를 산출하는 단계 및 상기 적어도 하나의 기반시설 마다 산출된 상기 임시 가중치를 모두 곱하여 해당 파티클의 가중치를 계산하는 단계를 포함할 수 있다.
일 실시예에 따른 프로세서에 의해 수행되는 차량의 위치 추정 방법의 상기 3차원 측정 데이터를 수신하는 단계는, 복수개의 센서로부터 상기 적어도 하나 이상의 기반시설에 대한 3차원 측정 데이터를 개별적으로 수신하는 단계를 포함하고, 상기 생성된 파티클 마다 가중치를 산출하는 단계는, 상기 수신된 3차원 측정 데이터에 기초하여 상기 해당 파티클의 가중치를 상기 복수개의 센서 별로 계산하는 단계 및 상기 복수개의 센서 별로 계산된 상기 해당 파티클의 가중치를 모두 곱하여 상기 해당 파티클의 가중치를 산출하는 단계를 포함할 수 있다.
상기 제1 예측 모델은 3차원 바이시클 모델(bicycle model)일 수 있다.
일 실시예에 따른 프로세서에 의해 수행되는 차량의 위치 추정 방법의 상기 차량의 제2 상태 정보를 산출하는 단계는, 시그마 포인트들(sigma points)을 생성하는 단계, 상기 무향 칼만 필터의 제2 예측 모델을 사용하여 상기 차량의 상태에 관한 제2 임시 정보를 예측하고, 상기 제2 임시 정보로부터 상기 차량의 측정값을 예측하는 단계, 및 상기 제1 상태 정보, 상기 제2 임시 정보, 및 상기 측정값에 기초하여 상기 차량의 상기 제2 상태 정보를 산출하는 단계를 포함할 수 있다.
일 실시예에 따른 프로세서에 의해 수행되는 차량의 위치 추정 방법의 상기 제2 임시 정보로부터 상기 차량의 측정값을 예측하는 단계는, 상기 제2 임시 정보로부터 상기 차량의 위치 좌표 및 요우잉 각도에 관한 정보를 추출하는 단계를 포함할 수 있다.
상기 제2 예측 모델은 등선회율 및 등속도(constant turn rate and velocity, CTRV) 모델일 수 있다.
상기 제2 상태 정보는, 상기 차량의 위치 좌표, 요우잉 각도, 및 상기 차량의 상태에 대한 공변량 행렬(covariance matrix)을 포함할 수 있다.
일 실시예에 따른 프로세서에 의해 수행되는 차량의 위치 추정 방법은 상기 제2 상태 정보에 기초하여 상기 차량의 조향, 속도, 및 가속도 중 적어도 하나를 조정하는 단계를 더 포함할 수 있다.
일 실시예에 따른 위치 추정 장치는 적어도 하나의 기반시설(infrastructure)의 3차원 위치 데이터를 측정하는 센서부, 파티클 필터의 제1 예측 모델 및 무향 칼만 필터의 제2 예측 모델을 저장하는 메모리, 및 상기 파티클 필터를 이용하여 상기 센서부로부터 수신된 3차원 측정 데이터 및 제1 임시 정보에 기초하여 상기 차량의 제1 상태 정보를 산출하고, 상기 무향 칼만 필터를 이용하여 상기 제1 상태 정보에 기초하여 제2 상태 정보를 산출하는 프로세서를 포함하고, 상기 3차원 위치 데이터는, 상기 차량의 종축(longitudinal axis), 횡축(lateral axis), 및 수직축(vertical axis)에 대한 비가우시안 노이즈(non-Gaussian noise) 성분을 포함할 수 있다.
도 1은 일 실시예에 따른 프로세서에 의해 수행되는 차량의 위치 추정 방법을 설명한다.
도 2는 일 실시예에 따른 필터의 융합에 의한 차량의 위치 추정 방법에 관하여 개략적으로 설명한다.
도 3은 3차원 바이시클 모델(bicycle model)을 설명한다.
도 4는 파티클 필터를 이용하여 차량의 제1 상태 정보를 산출하는 방법에 대하여 설명한다.
도 5는 생성된 파티클 마다 예측된 차량의 상태에 관한 제1 임시 정보를 도시한다.
도 6은 센서가 적어도 하나의 기반시설에 대한 3차원 위치 데이터를 측정하는 과정을 설명한다.
도 7은 파티클 마다 예측된 차량의 제1 임시 정보를 이용하여 기반시설의 위치를 글로벌 좌표계로 변환하는 과정을 설명한다.
도 8은 파티클을 기준으로 계산된 기반시설의 가상의 위치와 기반시설의 실제 위치 차이에 따라 가중치를 산출하는 방법을 설명한다.
도 9는 무향 칼만 필터에서 평균 값 주위의 대칭적인 영역으로 생성된 시그마 포인트들(sigma points)을 도시한다.
도 10(a)는 파티클 보조 무향 칼만 필터의 궤적 결과를 다른 필터의 궤적 결과와 비교하여 도시한다.
도 10(b)는 일 실시예에 따른 확장된 파티클 보조 무향 칼만 필터의 궤적 결과를 다른 필터의 궤적 결과와 비교하여 도시한다.
도 11은 시뮬레이션에 의한 차량의 위치에 대한 확률을 시각화하여 도시한다.
도 12는 일 실시예에 따른 차량의 구성을 설명하는 블록도이다.
실시예들에 대한 특정한 구조적 또는 기능적 설명들은 단지 예시를 위한 목적으로 개시된 것으로서, 다양한 형태로 변경되어 구현될 수 있다. 따라서, 실제 구현되는 형태는 개시된 특정 실시예로만 한정되는 것이 아니며, 본 명세서의 범위는 실시예들로 설명한 기술적 사상에 포함되는 변경, 균등물, 또는 대체물을 포함한다.
제1 또는 제2 등의 용어를 다양한 구성요소들을 설명하는데 사용될 수 있지만, 이런 용어들은 하나의 구성요소를 다른 구성요소로부터 구별하는 목적으로만 해석되어야 한다. 예를 들어, 제1 구성요소는 제2 구성요소로 명명될 수 있고, 유사하게 제2 구성요소는 제1 구성요소로도 명명될 수 있다.
어떤 구성요소가 다른 구성요소에 "연결되어" 있다고 언급된 때에는, 그 다른 구성요소에 직접적으로 연결되어 있거나 또는 접속되어 있을 수도 있지만, 중간에 다른 구성요소가 존재할 수도 있다고 이해되어야 할 것이다.
단수의 표현은 문맥상 명백하게 다르게 뜻하지 않는 한, 복수의 표현을 포함한다. 본 명세서에서, "포함하다" 또는 "가지다" 등의 용어는 설명된 특징, 숫자, 단계, 동작, 구성요소, 부분품 또는 이들을 조합한 것이 존재함으로 지정하려는 것이지, 하나 또는 그 이상의 다른 특징들이나 숫자, 단계, 동작, 구성요소, 부분품 또는 이들을 조합한 것들의 존재 또는 부가 가능성을 미리 배제하지 않는 것으로 이해되어야 한다.
다르게 정의되지 않는 한, 기술적이거나 과학적인 용어를 포함해서 여기서 사용되는 모든 용어들은 해당 기술 분야에서 통상의 지식을 가진 자에 의해 일반적으로 이해되는 것과 동일한 의미를 가진다. 일반적으로 사용되는 사전에 정의되어 있는 것과 같은 용어들은 관련 기술의 문맥상 가지는 의미와 일치하는 의미를 갖는 것으로 해석되어야 하며, 본 명세서에서 명백하게 정의하지 않는 한, 이상적이거나 과도하게 형식적인 의미로 해석되지 않는다.
이하, 실시예들을 첨부된 도면들을 참조하여 상세하게 설명한다. 첨부 도면을 참조하여 설명함에 있어, 도면 부호에 관계없이 동일한 구성 요소는 동일한 참조 부호를 부여하고, 이에 대한 중복되는 설명은 생략하기로 한다.
도 1은 일 실시예에 따른 프로세서에 의해 수행되는 차량의 위치 추정 방법을 설명한다.
단계(101)에서는 프로세서가 센서(sensor)로부터 적어도 하나의 기반시설(infrastructure)에 대한 3차원 위치 데이터를 현재 시점에 대하여 수신할 수 있다. 센서는 차량의 외부 또는 내부에 배치된 센서일 수 있다. 센서는, 전파를 사용하여 목표물의 거리, 방향, 각도 및 속도를 측정하는 레이더(radar), 레이저로 목표물을 조사하여 반사되는 빛을 분석하는 라이더(lidar), 또는 빛을 이용하여 목표물을 촬영하는 카메라(camera)일 수 있고, 이외에도 목표물의 3차원 위치를 감지할 수 있는 인식 모듈(perception module)이면 충분하다.
단계(102)에서는 프로세서가 파티클 필터(Particle Filter, PF)를 이용하여 센서로부터 수신된 3차원 위치 데이터 및 이전 시점에서 추정된 이전 상태 정보에 기초하여 차량의 제1 상태 정보를 산출할 수 있다. 센서로부터 수신된 3차원 위치 데이터는 비가우시안 노이즈(non-Gaussian noise) 성분을 포함할 수 있다. 구체적으로, 지면과 평행하고 차량의 진행 방향을 나타내는 종축(longitudinal axis), 지면과 평행하고 차량의 진행 방향인 종축과 수직한 방향을 나타내는 횡축(lateral axis), 및 지면에 수직한 방향을 나타내는 수직축(vertical axis)에 대하여 수신된 데이터는 각각 비가우시안 노이즈 성분을 포함할 수 있다. 다시 말해, 3차원 위치 데이터는 차량의 종축, 횡축, 및 수직축에 대한 비가우시안 노이즈 성분을 포함할 수 있다. 이하에서는 편의를 위하여 차량의 종축을 x축, 차량의 횡축을 y축, 차량의 수직축을 z축으로 설명한다.
단계(103)에서는 프로세서가 무향 칼만 필터(Unscented Kalman Filter, UKF)를 이용하여 제1 상태 정보에 기초하여 차량의 제2 상태 정보를 산출할 수 있다. 산출된 차량의 제2 상태 정보는 차량의 실제 위치 정보와 매우 근접하게 나타난다.
도 2에서는 일 실시예에 따른 필터의 융합(fusion)에 의한 차량의 위치 추정 방법에 관하여 개략적으로 설명한다.
일 실시예에 따르면, 프로세서는 파티클 필터 및 무향 칼만 필터를 융합하여 차량의 위치를 보다 정확하게 추정할 수 있다. 일 실시예에 따른 차량의 위치를 추정하는 프로세서는 센서로부터 적어도 하나의 기반시설의 3차원 위치 데이터를 수신할 수 있다. 3차원 위치 데이터는 차량의 x축, y축, z축에 대한 비가우시안 노이즈 성분이 포함된 데이터를 나타낼 수 있다. 프로세서는 파티클 필터를 이용하여 수신된 3차원 위치 데이터 및 제1 임시 정보에 기초하여 차량의 제1 상태 정보를 산출할 수 있다. 다시 말해, 파티클 필터는 기반시설의 3차원 위치 데이터를 수신할 수 있으며, x축, y축, z축의 비가우시안 노이즈 성분을 모두 고려한다. 프로세서는 무향 칼만 필터를 이용하여 제1 상태 정보에 기초하여 차량의 최종 상태 정보인 제2 상태 정보를 산출할 수 있다. 이하에서는, 확장된 파티클 필터(Extended particle filter)와 무향 칼만 필터를 융합한 확장된 파티클 보조 무향 칼만 필터(Extended Particle Aided Unscented Kalman Filter)에 관하여 설명한다. 확장된 파티클 필터는 센서(예를 들어, 라이더 또는 레이더)로부터 수신한 기반시설의 3차원 데이터를 고려하여 노이즈를 필터링 할 수 있다. 확장된 파티클 필터는 센서로부터 수신한 기반시설의 3차원 정보를 사용하고, 지면에 수직한 방향(z축)의 노이즈를 고려할 수 있다. 본 명세서에서는 확장된 파티클 보조 무향 칼만 필터를 확장된 PAUKF 필터라고도 나타낸다.
후술하겠으나, 확장된 파티클 보조 무향 칼만 필터는 기본 파티클 보조 무향 칼만 필터(기본 PAUKF)와 비교하여 차량의 위치 추정의 정확도가 향상된다. 기본 파티클 보조 무향 칼만 필터란, 파티클 필터와 무향 칼만 필터를 융합한 필터로서 파티클 필터로부터 추출된 차량의 상태 정보를 무향 칼만 필터에 입력하는 개념은 동일하나, 파티클 필터에서 단순히 기반시설의 2차원 정보(x-y 평면에서의 위치 정보)만을 사용하기 때문에, x축 및 y축의 노이즈만을 고려하고, z축(지면에 수직한 방향 축)의 노이즈를 고려하지 않는다. 다시 말해, 기본 PAUKF 필터는 센서로부터 수신한 기반시설의 3차원 정보를 적절히 처리하지 못한다. 그러나, 확장된 파티클 보조 무향 칼만 필터는 확장된 파티클 필터를 이용함으로써 센서로부터 수신한 기반시설의 3차원 정보에 대하여, 차량의 z축 움직임에 의한 z축 노이즈까지 고려하여 처리할 수 있다.
구체적으로 도 2를 참조하면, 단계(211)는 파티클 필터에서 바이시클 모델 및 노이즈가 포함된 이전 상태 정보에 기초하여 차량의 상태에 관한 제1 임시 정보를 예측하는 과정을 나타낸다. 단계(212)에서는, 차량은 센서로부터 비가우시안 노이즈가 포함된 기반시설의 3차원 위치 데이터를 수신할 수 있다. 단계(213)에서는 제1 임시 정보 및 수신된 기반시설의 3차원 위치 데이터에 기초하여 차량의 제1 상태 정보를 산출하는 과정을 나타낸다. 단계(221)에서는 무향 칼만 필터에서 노이즈가 포함된 이전 상태 정보에 기초하여 차량의 상태에 관한 제2 임시 정보를 예측하는 과정을 나타낸다. 단계(222)에서는 파티클 필터로부터 산출된 차량의 제1 상태 정보를 수신하는 과정을 나타낸다. 단계(223)에서는 제2 임시 정보를 제1 상태 정보를 이용하여 업데이트함으로써 차량의 최종 상태 정보인 제2 상태 정보를 산출하는 과정을 나타낸다. 이하에서는, 확장된 파티클 보조 무향 칼만 필터의 차량 상태 정보의 추정 과정을 수학식과 함께 구체적으로 설명한다.
도 3은 3차원 바이시클 모델(bicycle model)을 설명한다.
파티클 필터(particle filter, PF)를 이용하여 차량의 상태 정보를 추정하는 경우, 차량의 움직임(motion)을 표현하기 위하여 제1 예측 모델(prediction model)이 사용될 수 있다. 제1 예측 모델은 바이시클 모델일 수 있다. 차량(vehicle)은 1개의 앞 바퀴(front wheel, 301) 및 1개의 뒷 바퀴(rear wheel, 302)로 모델링 할 수 있으며, 차량의 상태는 3차원 공간에서의 x좌표, y좌표, z좌표, 및 요우잉 각도(yaw angle,
Figure 112021061840196-pat00001
)로 표현될 수 있다. 요우잉 각도(
Figure 112021061840196-pat00002
)는 앞 바퀴(301)와 뒷 바퀴(302)를 잇는 가상의 직선과 x축이 이루는 각도를 의미할 수 있다. 차량의 스티어링(steering) 및 슬립(slip) 각도 정보의 변화는 요우잉 각도(
Figure 112021061840196-pat00003
)에 포함된다고 가정한다. 이하에서는, 파티클 필터에 기초하여 외부 센서(sensor)로부터 측정된 기반시설들에 대한 위치 데이터 및 이전 시점에서 추정된 이전 상태 정보에 기초하여 차량의 제1 상태 정보를 산출하는 방법에 대하여 설명한다.
도 4에서는 파티클 필터를 이용하여 차량의 제1 상태 정보를 산출하는 방법에 대하여 설명한다.
단계(401)에서는 초기화(initialization) 과정이 이루어질 수 있다. 차량의 위치를 글로벌 좌표계(global coordinates)에서 결정하기 위하여, 차량의 초기 위치는 필수적으로 제공되어야 한다. 일 실시예에 따르면, GPS(Global Positioning System) 센서를 이용하여 차량의 초기 위치를 제공받을 수 있다. 차량에서 다중 경로 및 차단 문제(blocking issue)로 인하여 GPS 신호가 원활히 수신되지 않더라도, GPS 신호는 차량의 초기 위치를 결정하기 위한 제한적 영역(limited area)을 제공할 수 있다. 파티클 필터가 GPS 신호로부터 차량의 초기 위치를 수신하는 경우, 파티클 필터에서는 N개의 파티클(particle)이 무작위로 생성된다. 생성된 파티클들은 글로벌 좌표계 내에서 차량의 실제 위치 주변에 임의로 생성될 수 있다. 파티클은 프로세서에서 미리 정한 개수만큼 생성될 수 있다. 예를 들어, N은 1 이상의 자연수를 나타낼 수 있다.
일 실시예에 따른 차량의 위치 추정하는 프로세서는 확장된 파티클 보조 무향 칼만 필터는 z축 상의 노이즈를 고려할 수 있다. 구체적으로, 차량이 평평한(flat) x-y 평면의 지면(예를 들어, 도로)에서 주행하고, 차량의 z축 움직임이 z축의 노이즈를 발생시키는 경우를 가정한다. 차량은 다양한 외부 요인에 의하여 지면과 수직한 방향의 z축(수직축)에 대하여 위치가 변화할 수 있으며, 차량의 z축 방향으로의 움직임(motion)에 의하여 센서로부터 수신된 데이터는 z축 방향의 노이즈를 포함하게 된다. 일 실시예에 따른 차량의 위치를 추정하는 프로세서는 z축 방향의 노이즈가 포함된 데이터를 처리할 수 있다.
차량의 z축 방향 움직임이 발생시키는 노이즈는 하기 수학식 1과 같이 랜덤 노이즈(random noise)로 가정한다. 수학식 1은 일 실시예에 따른 차량에 대한 z축 방향으로의 움직임을 나타낸다. 하기 수학식 1을 참조하면, 차량의 z축 위치는
Figure 112021061840196-pat00004
의 분산을 갖는 정규 가우스 분포(normal gaussian distribution)로 가정한다.
Figure 112021061840196-pat00005
수학식 1에서,
Figure 112021061840196-pat00006
는 차량의 z축 위치를 나타낼 수 있고,
Figure 112021061840196-pat00007
는 차량의 z축 방향 노이즈에 대한 표준 편차(standard deviation)를 나타낼 수 있다.
단계(402)에서는 차량의 상태에 관한 제1 임시 정보를 제1 예측 모델을 이용하여 예측하는 과정이 이루어질 수 있다. 차량의 상태는, 3차원 공간에서 차량의 위치 좌표(x축 좌표, y축 좌표, z축 좌표) 및 요우잉 각도에 관한 정보를 포함할 수 있다. 구체적으로, 생성된 각 파티클 마다 k 시간에서 추정된 노이즈가 포함된 이전 상태 정보 및 제1 예측 모델을 이용하여 k+1 시간에서 차량의 상태에 관한 제1 임시 정보를 예측할 수 있다. 제1 예측 모델은 3차원 바이시클 모델일 수 있다.
도 5를 통하여 구체적으로 설명한다.
도 5는 생성된 파티클 마다 예측된 차량의 상태에 관한 제1 임시 정보를 도시한다.
구체적으로, 초기화 단계에서 생성된 파티클들은 글로벌 좌표계에서 차량의 실제 위치(501) 주변에 임의로 생성될 수 있다. 제1 파티클(521), 제2 파티클(522), 제3 파티클(523), 제4 파티클(524)이 차량의 실제 위치(501) 주변에 임의로 생성될 수 있다. 파티클 필터에서는 생성된 N개의 파티클 마다 k+1 시간에서 차량의 상태에 관한 제1 임시 정보를 예측할 수 있다. 각 파티클은 하기 수학식 2 및 수학식 3을 이용하여 k+1 시간에 차량의 위치 좌표 및 요우잉 각도에 관한 정보를 포함하는 제1 임시 정보를 예측할 수 있다.
수학식 2에서는, 생성된 파티클 마다 k 시간에서 추정된 차량의 이전 상태 정보 및 3차원 바이시클 모델을 이용하여 k+1 시간에서의 차량의 상태에 관한 제1 임시 정보를 예측하는 과정을 나타낸다.
Figure 112021061840196-pat00008
여기서,
Figure 112021061840196-pat00009
은 k+1 시간에서 예측된 제1 임시 정보,
Figure 112021061840196-pat00010
는 k 시간에서 차량의 속도,
Figure 112021061840196-pat00011
는 k 시간에서 차량의 요우잉 속도,
Figure 112021061840196-pat00012
은 k+1 시간에서 차량의 z축 방향의 노이즈를 나타낸다.
차량의 요우잉 속도(
Figure 112021061840196-pat00013
)가 0인 경우에는 하기 수학식 3으로 파티클 마다 차량의 상태에 관한 제1 임시 정보를 예측한다.
Figure 112021061840196-pat00014
다시 도 4로 돌아오면, 단계(403)에서는 파티클 필터에서 생성된 N개의 파티클 마다 가중치(weight)를 산출하는 방법을 설명한다.
차량 외부에는 적어도 하나의 기반시설(infrastructure)가 배치될 수 있다. 기반시설은 차량에 대하여 일정 거리 내에 존재하는 특징점(feature point)을 나타낼 수 있다. 일 실시예에 따른 차량의 프로세서는 적어도 하나의 기반시설을 통하여 차량의 위치를 파악할 수 있다. 파티클 필터에서 생성된 N개의 파티클 마다 가중치를 산출하기 위하여, 차량의 프로세서는 센서로부터 기반시설에 대한 3차원 위치 데이터를 수신하여야 한다. 도 6을 통하여 구체적으로 설명한다.
도 6은 센서가 적어도 하나의 기반시설(611, 612, 613)에 대한 3차원 위치 데이터를 측정하는 과정을 설명한다.
일 실시예에 따른 차량의 프로세서는 센서로부터 적어도 하나의 기반시설에 대한 3차원 위치 데이터를 현재 시점(예를 들어, k+1 시간)에 대하여 수신할 수 있다. 센서(601)는 거리 센서(range sensor)를 나타낼 수 있고, 센서(601)는 적어도 하나의 기반시설(611, 612, 613)에 대한 3차원 위치 데이터를 측정할 수 있다. 예를 들어, 센서(601)는 현재 시점에서 센서(601)와 기반시설(611) 사이의 상대적 거리(d1), 센서(601)와 기반시설(612) 사이의 상대적 거리(d2), 센서(603)와 기반시설(613) 사이의 상대적 거리(d3)를 측정할 수 있다.
또한, 센서(601)는 센서(601)와 기반시설(611, 612, 613) 사이의 상대적 베어링 각도(relative bearing angle)를 측정할 수 있다. 센서(601)는 센서(601)와 기반시설(611, 612, 613) 사이의 상대적 고도 각도(relative elevation angle)을 측정할 수 있다. 상대적 베어링 각도와 상대적 고도 각도는 센서 좌표계(sensor coordinate)를 기준으로 설명한다. 센서 좌표계는, 센서(601)의 중심을 지나는 차량의 종축을 x축으로, 센서(601)의 중심을 지나는 차량의 횡축을 y축으로, 센서(601)의 중심을 지나며 지면에 수직한 수직축을 z축으로 하는 좌표계를 나타낸다.
상대적 베어링 각도는, 센서 좌표계에서 센서의 중심과 기반시설을 잇는 직선을 x-y 평면에 투영(projection)한 경우, x축과 투영된 직선 사이의 각도를 나타낸다. 예를 들어, 센서(601)와 기반시설(611) 사이의 상대적 베어링 각도는, 센서(601)의 중심과 기반시설(611)을 잇는 직선을 x-y 평면에 투영한 경우에, x축과 투영된 직선 사이의 각도(
Figure 112021061840196-pat00015
1)를 나타낼 수 있다. 결국, 센서(601)는 센서(601)와 기반시설(611) 사이의 상대적 베어링 각도(
Figure 112021061840196-pat00016
1), 센서(601)와 기반시설(612) 사이의 상대적 베어링 각도(
Figure 112021061840196-pat00017
2), 센서(601)와 기반시설(613) 사이의 상대적 각도(
Figure 112021061840196-pat00018
3)를 측정할 수 있다.
상대적 고도 각도는, 센서 좌표계에서 센서의 중심과 기반시설을 잇는 직선을 x-y 평면에 투영한 경우, 센서의 중심과 기반시설을 잇는 직선과 투영된 직선 사이의 각도를 나타낸다. 예를 들어, 센서(601)와 기반시설(611) 사이의 상대적 고도 각도는, 센서(601)의 중심과 기반시설(611)을 잇는 직선을 x-y 평면에 투영한 경우에, 센서(601)의 중심과 기반시설(611)을 잇는 직선과 투영된 직선 사이의 각도(
Figure 112021061840196-pat00019
1)를 나타낼 수 있다. 센서(601)는 센서(601)와 기반시설(611) 사이의 상대적 고도 각도(
Figure 112021061840196-pat00020
1), 센서(601)와 기반시설(612) 사이의 상대적 고도 각도(
Figure 112021061840196-pat00021
2), 센서(601)는 센서와 기반시설(613) 사이의 상대적 고도 각도(
Figure 112021061840196-pat00022
3)를 측정할 수 있다.
센서(601)는 일 실시예에 따른 차량의 위치를 추정하는 프로세서에 적어도 하나의 기반시설(611, 612, 613)의 현재 시점에 대한 3차원 위치 데이터를 송신할 수 있다. 해당 기반시설에 대한 3차원 위치 데이터는 노이즈를 포함할 수 있으며, 해당 노이즈는 비선형 노이즈(nonlinear noise)일 수 있다.
센서(601)로부터 수신된 기반시설의 현재 시점에 대한 3차원 위치 데이터는 노이즈를 포함할 수 있으며, 하기 수학식 4와 같이 모델링 될 수 있다.
Figure 112021061840196-pat00023
여기서,
Figure 112021061840196-pat00024
은 k+1 시간에서 차량과 기반시설 사이의 측정 벡터(measurement vector)를 나타낼 수 있다. 측정 벡터(Zk+1)는 k+1 시간에서의 차량과 기반시설 사이의 상대적 거리, 상대적 베어링 각도, 상대적 고도 각도에 대한 정보를 포함할 수 있다.
일 실시예에 따른 차량의 위치를 추정하는 프로세서는, 센서로부터 수신한 현재 시점에 대한 3차원 위치 데이터를 나타내는 측정 벡터(Zk+1)를 이용하여, 센서 좌표계 내에서 k+1 시간에서 차량과 기반시설 사이의 x축 방향의 상대적 거리, y축 방향의 상대적 거리, z축 방향의 상대적 거리를 산출할 수 있다.
도 7은 파티클 마다 예측된 차량의 제1 임시 정보를 이용하여 기반시설의 위치를 글로벌 좌표계로 변환하는 과정을 설명한다.
측정 벡터를 이용하여 생성된 파티클 마다 가중치를 산출할 수 있다. 이전 초기화 단계 및 예측 단계에서 N개의 파티클이 생성되고, 파티클 마다 k+1 시간에서의 차량의 제1 임시 정보가 예측된다. 파티클은 무작위로 생성되기 때문에, 파티클 마다 k+1 시간에서의 차량의 제1 임시 정보가 서로 다르게 예측된다. 도 7을 참조하면, 생성된 파티클(521, 522, 523, 524) 마다 글로벌 좌표계에서 i번째 기반시설의 위치가 다르게 산출될 수 있다. 다시 말해, 생성된 파티클들 마다 글로벌 좌표계에서 k+1시간에서 예측된 차량의 제1 임시 정보가 서로 다르기 때문에, i번째 기반시설의 위치는 글로벌 좌표계에서 파티클 마다 다르게 산출될 수 있다. 예를 들어, 제3 파티클(523)의 k+1 시간에서 예측된 제1 임시 정보(733)에 기초하여 글로벌 좌표계에서 계산되는 i번째 기반시설에 대한 가상의 위치(743) 및 제4 파티클(524)의 k+1 시간에서 예측된 제1 임시 정보(734)에 기초하여 글로벌 좌표계에서 계산되는 i번째 기반시설에 대한 가상의 위치(744)는 서로 상이할 수 있다.
도 7에서는 파티클 마다 예측된 차량의 제1 임시 정보를 이용하여, 각 기반시설에 대한 위치를 글로벌 좌표계로 변환하는 과정을 설명한다. 일 실시예에 따른 차량의 위치를 추정하는 프로세서는, 측정 벡터(Zk+1)를 이용하여 센서 좌표계 내에서 차량과 i 번째 기반시설 사이의 거리(di), 베어링 각도(
Figure 112021061840196-pat00025
), 및 고도 각도(
Figure 112021061840196-pat00026
)를 산출할 수 있다. 하기 수학식 5를 이용하여 생성된 파티클 마다 계산되는 기반시설의 위치를 글로벌 좌표계로 변환할 수 있다.
Figure 112021061840196-pat00027
수학식 5에서,
Figure 112021061840196-pat00028
는 k+1 시간에서 i번째 기반시설의 글로벌 좌표계에서의 x좌표를 나타내며,
Figure 112021061840196-pat00029
는 k+1 시간에서 i번째 기반시설의 글로벌 좌표계에서의 y좌표를 나타낸다.
Figure 112021061840196-pat00030
는 k+1 시간에서 i 번째 기반시설의 글로벌 좌표계에서의 z 좌표를 나타낸다.
Figure 112021061840196-pat00031
는 랜덤 파라미터(random parameter)로, 상술한 수학식 4에 따라 생성된다. 다시 말해,
Figure 112021061840196-pat00032
는 k+1 시간에서 예측된 제1 임시 정보의 z축 좌표(
Figure 112021061840196-pat00033
) 및 k+1 시간에서의 노이즈에 의하여 결정될 수 있다.
일 실시예에 따른 차량의 위치를 추정하는 프로세서는, 임의의 파티클을 기준으로 계산된 기반시설의 가상의 위치와 기반시설의 실제 위치 차이에 따른 에러(error)를 이용하여 해당 파티클의 가중치를 계산할 수 있다. 도 8에서 구체적으로 설명한다.
도 8은 파티클을 기준으로 계산된 기반시설의 가상의 위치와 기반시설의 실제 위치 차이에 따라 가중치를 산출하는 방법을 설명한다.
일 실시예에 따른 프로세서는, 적어도 하나의 기반시설들 마다 예측된 제1 임시 정보 및 기반시설에 대한 글로벌 좌표계에서의 실제 위치 데이터에 기초하여 임시 가중치를 산출할 수 있다. 프로세서는, 적어도 하나의 기반시설들 마다 산출된 임시 가중치를 모두 곱하여 해당 파티클의 가중치를 계산할 수 있다.
구체적으로, 일 실시예에 따른 프로세서는 HD 맵 등을 이용하여 기반시설의 실제 위치(840)에 대한 정보를 수신할 수 있다. 도 8을 참조하면, 프로세서는 제3 파티클(523)에서 산출된 i번째 기반시설의 가상의 위치(743)와 i번째 기반시설의 실제 위치(840) 사이의 차이를 계산할 수 있고, 제4 파티클(524)에서 산출된 i번째 기반시설의 가상의 위치(744)와 i번째 기반시설의 실제 위치(840) 사이의 차이를 계산할 수 있다. 하기 수학식 6에서는 파티클에서 계산된 기반시설의 가상의 위치와 기반시설의 실제 위치에 따른 에러(error)를 이용하여 파티클 마다 가중치를 산출하는 과정이 설명된다. 수학식 6을 참조하면, 파티클과 개별 기반시설 사이의 임시 가중치를 계산하기 위하여, 다변량 정규 분포 함수(multivariable normal distribution function)가 사용될 수 있다.
하기 수학식 6에서는, 파티클이 N개가 생성되고, 차량 주변으로 기반시설이 L개가 배치되는 것을 가정한다. 예를 들어, N 및 L은 1 이상의 자연수를 나타낼 수 있다.
Figure 112021061840196-pat00034
n은 1부터 N사이 자연수의 값을 가질 수 있으며, i는 1부터 L사이 자연수의 값을 가질 수 있다.
Figure 112021061840196-pat00035
는 제n 파티클과 i번째 기반시설 사이의 임시 가중치를 나타낼 수 있다.
Figure 112021061840196-pat00036
는 센서로부터 수신한 거리 데이터의 x축 방향 노이즈의 공분산(covariance),
Figure 112021061840196-pat00037
는 센서로부터 수신한 거리 데이터의 y축 방향 노이즈의 공분산,
Figure 112021061840196-pat00038
는 센서로부터 수신한 거리 데이터의 z축 방향 노이즈의 공분산 을 나타낼 수 있다.
Figure 112021061840196-pat00039
는 글로벌 좌표계에서 실측 정보(예를 들어, 그라운드 트루스)에 따른 i번째 기반시설의 실제 x위치 좌표,
Figure 112021061840196-pat00040
는 글로벌 좌표계에서 실측 정보에 따른 i번째 기반시설의 실제 y 위치 좌표,
Figure 112021061840196-pat00041
는 글로벌 좌표계에서 실측 정보에 따른 i번째 기반시설의 실제 z 위치 좌표 를 나타낼 수 있다. 제n 파티클과 i번째 기반시설 사이의 가중치인
Figure 112021061840196-pat00042
는, 실측 정보에 따른 i번째 기반시설의 실제 위치(840)와 제n 파티클에서 측정된 i번째 기반시설의 가상의 위치 차이에 상응하는 오류(error)가 커질수록 감소할 수 있다.
하기 수학식 7에서는, 수학식 6에 의하여 계산된 파티클과 기반시설 사이에 개별적으로 계산된 임시 가중치를 이용하여 파티클의 가중치를 산출하는 과정이 설명된다.
Figure 112021061840196-pat00043
n은 1부터 N사이의 자연수 값을 가질 수 있다. 위 수학식 7을 참조하면, 제n 파티클의 가중치는, 제n 파티클과 L개의 기반시설들에 대한 개별 임시 가중치들을 모두 곱하여 계산될 수 있다.
더 나아가, 일 실시예에 따른 차량의 위치를 추정하는 프로세서는 차량과 연결되는 하나의 센서를 통하여 파티클 마다 가중치를 계산할 수도 있으나, 복수개의 센서를 통하여 파티클 마다 가중치를 계산할 수도 있다. 구체적으로, 일 실시예에 따른 차량의 위치를 추정하는 프로세서는, 복수개의 센서(예를 들어, 차량과 연결되는 제1 라이더, 제2 라이더, 제1 레이더 , 제2 레이더, 카메라 등)로부터 적어도 하나 이상의 기반시설점들에 대한 3차원 측정 데이터를 개별적으로 수신할 수 있다. 프로세서는, 수신된 적어도 하나 이상의 기반시설점들에 대한 3차원 측정 데이터에 기초하여 임의의 파티클의 가중치를 복수개의 센서 별로 계산할 수 있다. 프로세서는 복수개의 센서 별로 계산된 해당 파티클의 가중치를 모두 곱하여 해당 파티클의 가중치를 산출할 수 있다. 아래 수학식 8은 제n 파티클에 대하여 라이더, 레이더, 카메라 및 이외의 인식 모듈 마다 계산된 가중치를 모두 곱함으로써 제n 파티클의 가중치를 산출하는 과정을 설명한다.
Figure 112021061840196-pat00044
수학식 8에서,
Figure 112021061840196-pat00045
는 제1 라이더를 통하여 제n 파티클에 대하여 산출된 가중치,
Figure 112021061840196-pat00046
는 제2 라이더를 통하여 제n 파티클에 대하여 산출된 가중치,
Figure 112021061840196-pat00047
는 레이더를 통하여 제n 파티클에 대하여 산출된 가중치,
Figure 112021061840196-pat00048
는 카메라를 통하여 제n 파티클에 대하여 산출된 가중치,
Figure 112021061840196-pat00049
는 기반시설의 3차원 위치를 인식할 수 있는 인식 모듈을 통하여 제n 파티클에 대하여 산출된 가중치를 나타낸다.
다시 도 4로 돌아오면, 단계(404)에서는 파티클 필터에서 가중치를 계산한 후에, 리샘플링(resampling)이 이루어지는 과정이 설명된다. 파티클 필터에서 생성된 파티클 마다 가중치가 계산된 후, 가중치에 기초하여 파티클을 선택함으로써 k+1 시간에 대한 차량의 제1 상태 정보를 산출할 수 있다. 예를 들어, 그러나 한정되지 않게, 생성된 파티클들 중 가중치가 가장 높은 파티클을 선택할 수 있다. 제1 상태 정보는 글로벌 좌표계 내에서 차량의 x축 좌표, y축 좌표, 및 요우잉 각도에 관한 정보를 포함할 수 있다.
아래에서 설명하겠으나, 파티클 필터로부터 추정된 차량의 제1 상태 정보 를 무향 칼만 필터에 입력하여 차량의 위치 추정의 정확도를 향상시킬 수 있다. 이하에서는 무향 칼만 필터(Unscented Kalman Filter, UKF)에서 파티클 필터로부터 추정된 차량의 제1 상태 정보에 기초하여 차량의 제2 상태 정보를 산출하는 과정을 설명한다.
도 9는 무향 칼만 필터에서 평균 값 주위의 대칭적인 영역으로 생성된 시그마 포인트들(sigma points)을 도시한다.
도 9를 참조하면, 시그마 포인트들은 랜덤 베리어블인 상태 벡터의 기대치, 즉 평균 값(900) 주위의 대칭적인 영역으로 생성될 수 있다. 예를 들어, 제1 시그마 포인트(901)은 제2 시그마 포인트(902)와 평균 값을 기준으로 대칭일 수 있고, 제3 시그마 포인트(903)은 제4 시그마 포인트(904)와 평균 값을 기준으로 대칭일 수 있다. 시그마 포인트들은 하기 수학식 9을 사용하여 계산된 파라미터
Figure 112021061840196-pat00050
에 의하여 정의될 수 있다. 수학식 10은 무향 칼만 필터에서 차량의 상태 정보에 대응하는 상태 벡터를 나타낸다.
Figure 112021061840196-pat00051
Figure 112021061840196-pat00052
수학식 9에서,
Figure 112021061840196-pat00053
는 상태 벡터의 크기(size)로서 5를 나타낼 수 있다. 수학식 10에서,
Figure 112021061840196-pat00054
는 k 시간에서 제2 예측 모델을 이용하여 예측된 차량의 상태 정보에 대응하는 상태 벡터를 나타낼 수 있다. 수학식 10에서,
Figure 112021061840196-pat00055
는 차량의
Figure 112021061840196-pat00056
위치 좌표,
Figure 112021061840196-pat00057
는 차량의
Figure 112021061840196-pat00058
위치 좌표,
Figure 112021061840196-pat00059
는 차량의 속도(velocity),
Figure 112021061840196-pat00060
는 차량의 요우잉 각도,
Figure 112021061840196-pat00061
는 차량의 요우잉 속도(예를 들어, 회전 속도)를 나타낼 수 있다. 상태 벡터
Figure 112021061840196-pat00062
는 시그마 포인트들이 개별적으로 갖는 상태 벡터의 평균을 나타낼 수 있다. 하기 수학식 11을 사용하여 시그마 포인트들이 생성될 수 있다.
Figure 112021061840196-pat00063
Figure 112021061840196-pat00064
는 k 시간에서 시그마 포인트들의 상태 정보에 대응하는 상태 벡터,
Figure 112021061840196-pat00065
는 k 시간에서 차량 상태의 평균 값,
Figure 112021061840196-pat00066
는 k 시간에서 차량 상태의 공분산 행렬(covariance matrix)을 나타낼 수 있다.
Figure 112021061840196-pat00067
는
Figure 112021061840196-pat00068
의 행렬을 나타낼 수 있다. 수학식 8에 의하여, 평균 값에 상응하는 시그마 포인트를 포함하여 총
Figure 112021061840196-pat00069
개의 시그마 포인트들이 생성될 수 있다.
시그마 포인트들을 생성한 후, 무향 칼만 필터의 제2 예측 모델을 사용하여 차량의 상태에 관한 제2 임시 정보를 예측할 수 있다. 예를 들어, 제2 예측 모델은 등선회율 및 등속도(constant turn rate and velocity magnitude, CTRV) 차량 모션 모델일 수 있다. 이하에서는, 등선회율 및 등속도 차량 모션 모델을 CTRV 모델이라고 설명한다. 제2 예측 모델은 CTRV 모델로 한정하지 않고, 다른 예측 모델이 사용될 수도 있다.
차량의 정확한 위치를 획득하기 위하여, 무향 칼만 필터는 차량의 더 많은 상태들 및 상태들에 대한 비선형 노이즈의 영향을 고려할 수 있다. 예를 들어, 차량은 가속도(acceleration) 및 요우잉 가속도(yaw acceleration)를 노이즈로 고려할 수 있다. 특히, 가속도 및 요우잉 가속도 노이즈는 비선형일 수 있다. 해당 비선형 잡음을 처리하기 위하여, 하기 수학식 12에 표현된 바와 같은 보강된(augmented) 상태 벡터가 고려될 수 있다.
Figure 112021061840196-pat00070
수학식 12에서,
Figure 112021061840196-pat00071
는 k 시간에서 가속도 및 요우잉 가속도를 상태 벡터로 포함하는 보강된 상태 벡터를 나타낼 수 있다.
Figure 112021061840196-pat00072
는 차량 가속도의 노이즈,
Figure 112021061840196-pat00073
는 차량 요우잉 가속도의 노이즈를 나타낼 수 있다.
하기 수학식 13 및 수학식 14을 참조하면,
Figure 112021061840196-pat00074
는 σvelacc 2의 분산을 갖는 정규 가우스 분포(normal Gaussian distribution)로 설정될 수 있고,
Figure 112021061840196-pat00075
는 σyawacc 2의 분산을 갖는 정규 가우스 분포로 설정될 수 있다.
Figure 112021061840196-pat00076
Figure 112021061840196-pat00077
차량은 차량의 상태 벡터로 가속도 및 요우잉 가속도를 고려함으로써, 공분산 행렬
Figure 112021061840196-pat00078
는 하기 수학식 15에 표현된 바와 같이, 가속도 노이즈의 분산 및 요우잉 가속도 노이즈의 분산을 포함하는
Figure 112021061840196-pat00079
로 보강될 수 있다.
Figure 112021061840196-pat00080
일 실시예에 따른 확장된 파티클 보조 무향 칼만 필터에서, k 시간에서의 확장된 상태 정보
Figure 112021061840196-pat00081
및 제2 예측 모델을 사용하여 차량의 상태에 관한 제2 임시 정보를 예측할 수 있다. 제2 예측 모델은 CTRV 모델일 수 있다. 수학식 16 및 수학식 17에서는 k 시간에서 추정된 차량의 이전 상태 정보 및 CTRV 모델을 이용하여 k+1 시간에서의 차량의 상태에 관한 제2 임시 정보를 예측하는 과정을 표현한다. 제2 임시 정보란, 이전 시간(예를 들어, k시간)에서 추정된 차량의 이전 상태 정보로부터 제2 예측 모델을 이용하여 예측된 시그마 포인트들의 상태 벡터에 대응하는 차량의 예측된 상태 정보를 의미한다.
Figure 112021061840196-pat00082
수학식 16에서,
Figure 112021061840196-pat00083
은 k+1 시간에서의 차량의 상태 벡터,
Figure 112021061840196-pat00084
는 k 시간에서 차량의 상태 벡터, 및
Figure 112021061840196-pat00085
는 k 시간에서 차량의 상태 벡터를 미분한 값을 나타낼 수 있다.
수학식 16을 기초로, 수학식 17에서는 k 시간에서의 차량의 확장된 상태 벡터 및 CTRV 모델을 이용하여 k+1 시간에서의 차량의 확장된 상태 벡터를 예측하는 과정을 설명한다.
Figure 112021061840196-pat00086
수학식 17에서,
Figure 112021061840196-pat00087
는 k+1 시간에서의 차량의 확장된 상태 벡터,
Figure 112021061840196-pat00088
는 k 시간에서의 차량의 확장된 상태 벡터를 나타낼 수 있다.
더 나아가, 가속도 및 요우잉 가속도를 고려하는 확장된 상태 벡터 예측 단계에서
Figure 112021061840196-pat00089
는 확장된 상태 벡터의 크기로서 7을 나타내는
Figure 112021061840196-pat00090
로 변환되고, 파라미터
Figure 112021061840196-pat00091
는 하기 수학식 18에 의하여 계산될 수 있다.
Figure 112021061840196-pat00092
Figure 112021061840196-pat00093
각 시그마 포인트들의 가중치는 하기 수학식 20 및 수학식 21에 기초하여 계산될 수 있다.
수학식 20은 평균 값에 상응하는 시그마 포인트(900)의 가중치를 계산하는 과정을 설명하고, 수학식 21은 시그마 포인트(900) 이외의 시그마 포인트들의 가중치를 계산하는 과정을 설명한다.
Figure 112021061840196-pat00094
Figure 112021061840196-pat00095
수학식 22는 시그마 포인트들의 상태 정보 및 가중치에 기초하여 평균 상태 벡터를 계산하는 과정을 설명하고, 수학식 23은 시그마 포인트들의 상태 정보 및 평균 상태 벡터에 기초하여 공분산을 계산하는 과정을 설명한다.
Figure 112021061840196-pat00096
Figure 112021061840196-pat00097
수학식 22에서,
Figure 112021061840196-pat00098
는 k+1 시간에서 시그마 포인트들의 상태 벡터에 가중치가 곱해진 예측된 평균 상태 벡터를 나타낼 수 있다.
수학식 23에서,
Figure 112021061840196-pat00099
는 k+1시간에서 예측된 공분산 행렬을 나타낼 수 있다.
수학식 17을 참조하면, k+1 시간에서의 차량의 확장된 상태 벡터를 CTRV 모델을 이용하여 예측하는 경우, 가속도 노이즈 및 요우잉 가속도 노이즈에 관한 정보를 x좌표, y좌표, 차량의 속도, 요우잉 각도, 요우잉 속도가 포함할 수 있다. 따라서, 보강된 상태 벡터는 가속도 상태 벡터 및 요우잉 가속도 상태 벡터를 제외하고, 5의 크기(size)를 갖는 기존 상태 벡터로 다시 변경될 수 있다.
이하에서는, 무향 칼만 필터에서 예측된 제2 임시 정보를 파티클 필터로부터 수신한 제1 상태 정보를 이용하여 업데이트함으로써 차량의 제2 상태 정보를 산출하는 과정을 설명한다.
무향 칼만 필터에서는 차량이 센서로부터 노이즈를 갖는 측정 데이터를 수신하는 대신, 파티클 필터로부터 산출된 차량의 제1 상태 정보(
Figure 112021061840196-pat00100
)를 수신하여 제2 상태 정보를 산출할 수 있다. 다시 말해, 차량의 제1 상태 정보(
Figure 112021061840196-pat00101
)는 가상의 센서(virtual sensor)가 되어, 기존 센서보다 더 정확한 차량의 상태를 산출할 수 있다.
하기 수학식 24는 파티클 필터로부터 수신하는 제1 상태 정보에 관한 정보를 나타낸다.
Figure 112021061840196-pat00102
수학식 24를 참조하면, 파티클 필터로부터 수신하는 제1 상태 정보는 차량의 x 위치 좌표, y 위치 좌표, 및 요우잉 각도를 포함할 수 있다.
수학식 25에서는, 측정 값에 상응하는 측정 벡터를 예측하는 과정을 설명한다.
Figure 112021061840196-pat00103
Figure 112021061840196-pat00104
수학식 25에서,
Figure 112021061840196-pat00105
는 k+1 시간에서의 측정 벡터를 나타낼 수 있다.
Figure 112021061840196-pat00106
는 k+1 시간에서의 시그마 포인트들의 상태 벡터를 나타낼 수 있고,
Figure 112021061840196-pat00107
은 k+1 시간에서의 비선형 노이즈(non-linear)를 나타낼 수 있다.
수학식 26의 A행렬을 참조하면, 수학식 22에서 측정 벡터를 계산하기 위하여 제2 임시 정보의 k+1 시간에서 예측된 시그마 포인트들의 상태 벡터로부터 차량의 위치 좌표(x 위치 좌표, y 위치 좌표) 및 요우잉 각도에 관한 정보를 추출할 수 있다. k+1 시간에서의 시그마 포인트들의 상태 벡터는 이전 k 시간에서의 시그마 포인트들을 이용하여 상태 벡터를 계산하므로, 노이즈가 포함될 수 있다. 따라서, 수학식 26에서는 비선형 노이즈인
Figure 112021061840196-pat00108
을 추가하여 모델링할 수 있다. k+1 시간에서의 측정 벡터(
Figure 112021061840196-pat00109
)에 관한 정보를 무향 칼만 필터에서 차량의 측정값이라고 한다.
수학식 27은 예측된 평균 측정 벡터를 계산하는 과정을 설명한다.
Figure 112021061840196-pat00110
수학식 27에서,
Figure 112021061840196-pat00111
는 k+1시간에서의 예측된 평균 측정 벡터를 나타낼 수 있다.
이어서, 수학식 28은 측정 공분산을 예측하는 과정을 설명한다. 수학식 29는 노이즈 공분산을 나타낼 수 있다.
Figure 112021061840196-pat00112
Figure 112021061840196-pat00113
수학식 28에서,
Figure 112021061840196-pat00114
는 k+1 시간에서 예측된 측정 공분산 행렬을 나타낼 수 있다. 수학식 29에서,
Figure 112021061840196-pat00115
는 파티클 필터에서 센서로부터 수신한 거리 데이터의 x축 방향 노이즈의 공분산,
Figure 112021061840196-pat00116
는 파티클 필터에서 센서로부터 수신한 거리 데이터의 y축 방향 노이즈의 공분산,
Figure 112021061840196-pat00117
는 파티클 필터에서 센서로부터 수신한 방향 데이터의 요 각도 노이즈의 공분산을 나타낼 수 있다. 측정 공분산 행렬
Figure 112021061840196-pat00118
는 수학식 28 및 29를 참조하면, 파티클 필터에서 센서로부터 수신한 데이터의 노이즈에 따라 변할 수 있다.
하기 수학식 30에서는 교차 상관 관계 행렬(cross-correlation matrix)을 계산하는 과정이 설명된다.
Figure 112021061840196-pat00119
Figure 112021061840196-pat00120
는 k+1시간에서 시그마 포인트들의 상태 정보 및 k+1시간에서 시그마 포인트들의 측정 정보 사이의 교차 상관 관계 행렬을 나타낼 수 있다. 교차 상관 관계 행렬과 측정 공분산에 기초하여, 칼만 이득(Kalman gain)은 수학식 30과 같이 계산된다.
Figure 112021061840196-pat00121
Figure 112021061840196-pat00122
는 k+1 시간에서의 칼만 이득을 나타낼 수 있다.
차량의 제2 임시 정보는 파티클 필터로부터 수신한 제1 상태 정보를 이용하여 제2 상태 정보로 업데이트될 수 있다. 하기 수학식 29는 제2 상태 정보는 차량의 위치 좌표, 요우잉 각도, 및 차량의 상태에 대한 공변량 행렬(covariance matrix)을 포함할 수 있다.
Figure 112021061840196-pat00123
수학식 32에서,
Figure 112021061840196-pat00124
는 확장된 파티클 보조 무향 칼만 필터에서 계산된 차량의 최종 상태 벡터를 나타낸다.
Figure 112021061840196-pat00125
는 차량의 위치 좌표(x 좌표, y 좌표) 및 요우잉 각도에 관한 정보를 포함할 수 있다. 수학식 32를 참조하면, 차량의 최종 상태 벡터는 파티클 필터로부터 수신한 제1 상태 정보 및 칼만 이득에 기초하여 산출될 수 있다.
Figure 112021061840196-pat00126
수학식 33에서,
Figure 112021061840196-pat00127
는 확장된 파티클 보조 무향 칼만 필터에서 계산된 차량의 최종 상태에 대한 공변량 행렬을 나타낼 수 있다. 측정 공변량 행렬, 칼만 이득, 및 k 시간에서의 공변량 행렬에 기초하여 k+1시간에서의 공변량 행렬을 산출할 수 있다.
차량의 최종 상태 벡터
Figure 112021061840196-pat00128
및 차량의 최종 상태에 대한 공변량 행렬
Figure 112021061840196-pat00129
를 포함하는 제2 상태 정보에 기초하여 차량의 조향, 속도, 및 가속도 중 적어도 하나가 조정될 수 있다.
확장된 파티클 보조 무향 칼만 필터의 전체 의사 코드(pseudo code)는 하기 표 1에서 설명된다.
Figure 112021061840196-pat00130
도 10 내지 도 12에서는 확장된 파티클 보조 무향 칼만 필터를 이용한 시뮬레이션을 통한 차량의 위치 추정 결과에 관하여 설명한다.
시뮬레이션에서 센서는 라이더(lidar)를 사용하며, 센서의 인식 거리는 50m로 설정된다. 또한, 상술한 시뮬레이션에서, 차량은 x축, y축, z축의 비가우시안 노이즈를 포함하는 데이터를 센서로부터 수신할 수 있다. 비가우시안 노이즈는 정현파 함수를 이용하여 생성된다. 차량은 일정한 속도로 주행하며, 속도는 60km/h, 70km/h, 80km/h, 90km/h, 100km/h, 110km/h, 및 120km/h로 각각 설정되어 시뮬레이션 된다. 하기 표 2는 시뮬레이션 내에서 생성된 노이즈를 나타낸다.
Figure 112021061840196-pat00131
수학식 34를 참조하면, 필터들의 성능은 평균 제곱근 오차(Root Mean Square Error, RMSE)를 기반으로 평가된다.
Figure 112021061840196-pat00132
수학식 34에서, N은 데이터 포인트들(data points)의 수를 나타낼 수 있다. 필터로부터 추정된 차량의 궤적과 차량의 실제 궤적을 비교하여 필터의 성능을 검증할 수 있다. 필터로부터 추정된 차량의 궤적은 도 10(a) 및 도 10(b)에서 설명한다. 도 10(a) 및 도 10(b)에서는 차량이 60km/h의 속도로 이동하는 것을 가정한다.
도 10(a)는 파티클 보조 무향 칼만 필터의 궤적 결과를 다른 필터의 궤적 결과와 비교하여 도시한다.
궤적(1011)는 차량의 실제 이동 궤적(그라운드 트루쓰)을 나타낸다. 궤적(1012)은 노이즈를 포함하는 차량의 궤적을 나타낸다. 궤적(1013)은 파티클 필터(PF)로부터 추정된 차량의 궤적을 나타낸다. 궤적(1020)은 기본 파티클 보조 무향 칼만 필터의 궤적을 나타낸다. 기본 파티클 보조 무향 칼만 필터는 일 실시예에 따른 확장된 파티클 보조 무향 칼만 필터와 비교하여, 지면에 수직한 방향(z축)의 노이즈를 고려하고 있지 못하며, 기반시설의 2차원(x-y 평면) 위치 데이터만을 이용하기 때문에 차량의 실제 이동 궤적과 차이가 나타난다. 도 10(a)를 참조하면, 기본 파티클 보조 무향 칼만 필터로부터 추정된 차량의 궤적(1020)은 지면에 수직한 방향의 노이즈 효과를 고려하고 있지 않아, 적절한 가중치가 있는 파티클을 선택할 수 없어 차량의 실제 이동 궤적(1011)과 비교하여 상당한 오류를 나타낸다.
도 10(b)는 일 실시예에 따른 확장된 파티클 보조 무향 칼만 필터의 궤적 결과를 다른 필터의 궤적 결과와 비교하여 도시한다.
궤적(1011)는 차량의 실제 이동 궤적을 나타낸다. 궤적(1012)은 노이즈를 포함하는 차량의 궤적을 나타낸다. 궤적(1013)은 파티클 필터(PF)로부터 추정된 차량의 궤적을 나타낸다. 궤적(1030)는 일 실시예에 따른 확장된 파티클 보조 무향 칼만 필터의 궤적을 나타낸다. 일 실시예에 따른 확장된 파티클 보조 무향 칼만 필터는 지면에 수직한 방향(z축)의 노이즈를 고려하고, 센서로부터 기반시설의 3차원 위치 데이터를 처리하여 보다 정확한 파티클의 가중치를 산출할 수 있다. 따라서, 확장된 파티클 보조 무향 칼만 필터의 궤적(1030)은 차량의 실제 이동 궤적(1011)과 매우 유사하게 나타난다.
도 11은 시뮬레이션에 의한 차량의 위치에 대한 확률을 시각화하여 도시한다.
그래프(1110, 1120)는 기반시설들, GPS에 의한 차량의 위치, 차량의 실제 위치, 확장된 PAUKF를 이용한 차량의 위치에 대한 확률 분포를 시각화하여 도시한다. 구체적으로, 그래프(1110)는 시뮬레이션에 따른 기반시설 및 차량의 위치에 대한 확률 분포를 x-z 평면에 대하여 시각화하여 나타내며, 그래프(1120)는 시뮬레이션에 따른 기반시설 및 차량의 위치에 대한 확률 분포를 y-z 평면에 대하여 시각화하여 도시한다. 위치(1101)는 GPS에 의한 차량의 위치를 나타낸다. 위치(1102)는 일 실시예에 따른 확장된 파티클 보조 무향 칼만 필터를 이용하여 추정된 차량의 위치를 나타낸다. 위치(1103)는 차량의 실제 위치를 나타낸다. GPS에 의하여 수신된 차량의 위치(1101)는 차량의 실제 위치(1103)와 비교하여 큰 오류가 나타난다. 반면, 확장된 파티클 보조 무향 칼만 필터를 이용하여 추정된 차량의 위치(1102)는 실제 차량의 위치(1103)와 매우 가깝게 나타난다.
하기 표 3에서는 기본 PAUKF 및 z축의 노이즈를 고려한 확장된 PAUKF 사이의 평균 제곱근 오차를 비교한다.
Figure 112021061840196-pat00133
표 3은 차량의 서로 다른 속도 조건에서 각 필터의 성능을 나타낸다. 차량의 속도는 60km/h에서 120km/h 까지 10개의 간격으로 설정된다. 기본 파티클 필터의 RMSE에서 기본 파티클 필터의 RMSE는 11.002m이고 확장 파티클 필터 추정의 평균 RMSE는 6.201m로 나타난다. 확장된 파티클 필터의 RMSE는 기본 파티클 필터에 비해 약 43.64 % 감소한다. 기본 PAUKF의 RMSE 평균은 10.295m, 확장된 PAUKF의 평균은 2.696m로 나타난다. 기본 PAUKF에 비해 확장된 PAUKF는 RMSE를 약 73.81 % 감소 시킨다. 확장된 PAUKF는 차량의 궤적을 보다 정확하게 추정 할 수 있다.
도 12는 일 실시예에 따른 차량의 구성을 설명하는 블록도이다.
일 실시예에 따른 위치 추정을 수행하는 차량은 센서부(1210), 메모리(1220), 및 프로세서(1230)를 포함할 수 있다. 센서부(1210)는 적어도 하나의 기반시설의 3차원 위치 데이터를 측정할 수 있다. 메모리(1220)는 파티클 필터의 제1 예측 모델 및 무향 칼만 필터의 제2 예측 모델을 저장할 수 있다. 프로세서(1230)는 센서부(1210)로부터 적어도 하나의 기반시설의 3차원 위치 데이터를 수신할 수 있고, 파티클 필터를 이용하여 센서부로부터 수신된 3차원 위치 데이터 및 제1 임시 정보에 기초하여 차량의 제1 상태 정보를 산출하고, 무향 칼만 필터를 이용하여 제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 (13)

  1. 프로세서에 의해 수행되는 차량(vehicle)의 위치 추정 방법에 있어서,
    센서(sensor)로부터 적어도 하나의 기반시설(infrastructure)의 3차원 위치 데이터를 수신하는 단계;
    파티클 필터(Particle Filter, PF)를 이용하여 상기 수신된 3차원 위치 데이터 및 상기 차량의 제1 임시 정보에 기초하여 상기 차량의 제1 상태 정보를 산출하는 단계; 및
    무향 칼만 필터(Unscented Kalman Filter, UKF)를 이용하여 상기 제1 상태 정보에 기초하여 제2 상태 정보를 산출하는 단계
    를 포함하고,
    상기 3차원 위치 데이터는, 상기 차량의 종축(longitudinal axis), 횡축(lateral axis), 및 수직축(vertical axis)에 대한 비가우시안 노이즈(non-Gaussian noise) 성분을 포함하며,
    상기 차량의 제1 상태 정보를 산출하는 단계는,
    미리 정한 개수의 파티클들을 생성하는 단계; 및
    상기 생성된 미리 정한 개수의 파티클들 각각에 대하여, 제1 예측 모델을 사용하여 해당 파티클에 대응하는 상기 차량의 수직축 좌표가 포함된 상기 차량의 상태에 관한 상기 제1 임시 정보를 예측하고, 상기 예측된 제1 임시 정보와 상기 적어도 하나의 기반시설의 글로벌 좌표계에서의 수직축 좌표가 포함된 실제 위치 데이터에 기초하여 산출되는 적어도 하나의 임시 가중치를 모두 곱하여 상기 해당 파티클의 가중치(weight)를 산출하는 단계
    를 포함하는 차량의 위치 추정 방법.
  2. 제1항에 있어서,
    상기 3차원 위치 데이터는,
    상기 센서와 상기 적어도 하나의 기반시설 사이의 베어링 각도(bearing angle) 정보 및 고도 각도(elevation angle) 정보를 포함하는,
    차량의 위치 추정 방법.
  3. 삭제
  4. 제1항에 있어서,
    상기 생성된 파티클 마다 가중치를 산출하는 단계는,
    상기 적어도 하나의 기반시설 마다 상기 예측된 제1 임시 정보 및 해당 기반시설에 대한 실제 위치 데이터에 기초하여 임시 가중치를 산출하는 단계;
    상기 적어도 하나의 기반시설 마다 산출된 상기 임시 가중치를 모두 곱하여 해당 파티클의 가중치를 계산하는 단계
    를 포함하는 차량의 위치 추정 방법.
  5. 제1항에 있어서,
    상기 3차원 위치 데이터를 수신하는 단계는,
    복수개의 센서들로부터 상기 적어도 하나 이상의 기반시설에 대한 3차원 위치 데이터를 개별적으로 수신하는 단계를 포함하고,
    상기 생성된 파티클 마다 가중치를 산출하는 단계는,
    상기 생성된 파티클 각각에 대하여, 상기 수신된 3차원 위치 데이터에 기초하여 해당 파티클의 가중치를 상기 복수개의 센서 별로 계산하는 단계;
    상기 복수개의 센서 별로 계산된 상기 해당 파티클의 가중치를 모두 곱하여 상기 해당 파티클의 가중치를 산출하는 단계
    를 포함하는 차량의 위치 추정 방법.
  6. 제1항에 있어서,
    상기 제1 예측 모델은 3차원 바이시클 모델(bicycle model)인,
    차량의 위치 추정 방법.
  7. 제1항에 있어서,
    상기 차량의 제2 상태 정보를 산출하는 단계는,
    시그마 포인트들(sigma points)을 생성하는 단계;
    상기 무향 칼만 필터의 제2 예측 모델을 사용하여 상기 차량의 상태에 관한 제2 임시 정보를 예측하고, 상기 제2 임시 정보로부터 상기 차량의 측정값을 예측하는 단계; 및
    상기 제1 상태 정보, 상기 제2 임시 정보, 및 상기 측정값에 기초하여 상기 차량의 상기 제2 상태 정보를 산출하는 단계
    를 포함하는 차량의 위치 추정 방법.
  8. 제7항에 있어서,
    상기 제2 임시 정보로부터 상기 차량의 측정값을 예측하는 단계는,
    상기 제2 임시 정보로부터 상기 차량의 위치 좌표 및 요우잉 각도에 관한 정보를 추출하는 단계
    를 포함하는 차량의 위치 추정 방법.
  9. 제7항에 있어서,
    상기 제2 예측 모델은 등선회율 및 등속도(constant turn rate and velocity, CTRV) 모델인,
    차량의 위치 추정 방법.
  10. 제1항에 있어서,
    상기 제2 상태 정보는,
    상기 차량의 위치 좌표, 요우잉 각도, 및 상기 차량의 상태에 대한 공변량 행렬(covariance matrix)을 포함하는,
    차량의 위치 추정 방법.
  11. 제1항에 있어서,
    상기 제2 상태 정보에 기초하여 상기 차량의 조향, 속도, 및 가속도 중 적어도 하나를 조정하는 단계
    를 더 포함하는 차량의 위치 추정 방법.
  12. 제1항, 제2항, 제4항 내지 제11항 중 어느 한 항의 방법을 수행하기 위한 명령어를 포함하는 하나 이상의 컴퓨터 프로그램을 저장한 컴퓨터 판독 가능 기록 매체.
  13. 위치 추정 장치에 있어서,
    적어도 하나의 기반시설(infrastructure)의 3차원 위치 데이터를 측정하는 센서부;
    파티클 필터의 제1 예측 모델 및 무향 칼만 필터의 제2 예측 모델을 저장하는 메모리; 및
    상기 파티클 필터를 이용하여 상기 센서부로부터 수신된 3차원 측정 데이터 및 제1 임시 정보에 기초하여 차량의 제1 상태 정보를 산출하고, 상기 무향 칼만 필터를 이용하여 상기 제1 상태 정보에 기초하여 제2 상태 정보를 산출하는 프로세서
    를 포함하고,
    상기 3차원 위치 데이터는, 차량의 종축(longitudinal axis), 횡축(lateral axis), 및 수직축(vertical axis)에 대한 비가우시안 노이즈(non-Gaussian noise) 성분을 포함하며,
    상기 프로세서는,
    미리 정한 개수의 파티클들을 생성하고,
    상기 생성된 미리 정한 개수의 파티클들 각각에 대하여, 제1 예측 모델을 사용하여 해당 파티클에 대응하는 상기 차량의 수직축 좌표가 포함된 상기 차량의 상태에 관한 상기 제1 임시 정보를 예측하고, 상기 예측된 제1 임시 정보와 상기 적어도 하나의 기반시설의 글로벌 좌표계에서의 수직축 좌표가 포함된 실제 위치 데이터에 기초하여 산출되는 적어도 하나의 임시 가중치를 모두 곱하여 상기 해당 파티클의 가중치(weight)를 산출하는,
    위치 추정 장치.
KR1020210069075A 2021-05-28 2021-05-28 다중 필터 및 센서 퓨전 기반의 차량의 위치 추정 방법 KR102489865B1 (ko)

Priority Applications (1)

Application Number Priority Date Filing Date Title
KR1020210069075A KR102489865B1 (ko) 2021-05-28 2021-05-28 다중 필터 및 센서 퓨전 기반의 차량의 위치 추정 방법

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
KR1020210069075A KR102489865B1 (ko) 2021-05-28 2021-05-28 다중 필터 및 센서 퓨전 기반의 차량의 위치 추정 방법

Publications (2)

Publication Number Publication Date
KR20220160881A KR20220160881A (ko) 2022-12-06
KR102489865B1 true KR102489865B1 (ko) 2023-01-18

Family

ID=84407145

Family Applications (1)

Application Number Title Priority Date Filing Date
KR1020210069075A KR102489865B1 (ko) 2021-05-28 2021-05-28 다중 필터 및 센서 퓨전 기반의 차량의 위치 추정 방법

Country Status (1)

Country Link
KR (1) KR102489865B1 (ko)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116518983B (zh) * 2023-07-05 2023-10-17 西安羚控电子科技有限公司 用于移动机器人定位的自适应融合方法及装置
CN118501007B (zh) * 2024-07-17 2024-10-18 中国船舶集团有限公司第七〇七研究所 基于多种冰晶粒子观测仪器的数据融合方法、系统及产品

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 (2)

* Cited by examiner, † Cited by third party
Title
SENSORS*
언센티드 파티클 필터를 이용한 비선형 시스템 상태 추정*

Also Published As

Publication number Publication date
KR20220160881A (ko) 2022-12-06

Similar Documents

Publication Publication Date Title
CN109521756B (zh) 用于无人驾驶车辆的障碍物运动信息生成方法和装置
CN109901567B (zh) 用于输出障碍物信息的方法和装置
WO2018221453A1 (ja) 出力装置、制御方法、プログラム及び記憶媒体
Jo et al. Interacting multiple model filter-based sensor fusion of GPS with in-vehicle sensors for real-time vehicle positioning
KR20210111180A (ko) 위치 추적 방법, 장치, 컴퓨팅 기기 및 컴퓨터 판독 가능한 저장 매체
KR102489865B1 (ko) 다중 필터 및 센서 퓨전 기반의 차량의 위치 추정 방법
CN112204346B (zh) 用于确定车辆位置的方法
WO2018221455A1 (ja) 更新装置、制御方法、プログラム及び記憶媒体
WO2018221454A1 (ja) 地図作成装置、制御方法、プログラム及び記憶媒体
CN110989619B (zh) 用于定位对象的方法、装置、设备和存储介质
CN109903367A (zh) 构建地图的方法、装置和计算机可读存储介质
KR102093743B1 (ko) 센서 융합을 이용한 차선 레벨 자동차 위치 정보 측위 시스템
US20230003521A1 (en) Information processing device, control method, program and storage medium
KR102460842B1 (ko) 센서 퓨전에 의한 차량의 위치 추정 방법
US20230408721A1 (en) Method and device for analyzing 3d target maneuver using line array sensor
KR102679941B1 (ko) 딥러닝을 기반으로 운전자의 운전습관을 판단하기 위해 gnss-imu 센서 퓨전을 이용하여 차량 정보를 획득하는 방법 및 시스템
KR101600772B1 (ko) 비행 동역학 모델을 활용한 항공기 정밀 추적 방법
JP7308141B2 (ja) 自己位置推定方法及び自己位置推定装置
US20210140783A1 (en) Navigation apparatus and operation method of navigation apparatus
KR102026114B1 (ko) 무인 항공기의 도심 항법 위치 추정장치 및 방법
Li et al. A robust terrain-based road vehicle localization algorithm
CN113302452A (zh) 用于提供参数估计的完整性范围的方法
KR20190079470A (ko) 객체 측위 방법 및 장치
JP7540559B1 (ja) 自己位置推定装置及び自己位置推定方法
KR101665375B1 (ko) 이동체 항법 시스템 및 방법

Legal Events

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