KR102396855B1 - 측위 방향각 추정 시스템 및 방법 - Google Patents

측위 방향각 추정 시스템 및 방법 Download PDF

Info

Publication number
KR102396855B1
KR102396855B1 KR1020170165757A KR20170165757A KR102396855B1 KR 102396855 B1 KR102396855 B1 KR 102396855B1 KR 1020170165757 A KR1020170165757 A KR 1020170165757A KR 20170165757 A KR20170165757 A KR 20170165757A KR 102396855 B1 KR102396855 B1 KR 102396855B1
Authority
KR
South Korea
Prior art keywords
direction angle
curvature
weight
gnss
dead reckoning
Prior art date
Application number
KR1020170165757A
Other languages
English (en)
Other versions
KR102396855B9 (ko
KR20190066229A (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 KR1020170165757A priority Critical patent/KR102396855B1/ko
Publication of KR20190066229A publication Critical patent/KR20190066229A/ko
Application granted granted Critical
Publication of KR102396855B1 publication Critical patent/KR102396855B1/ko
Publication of KR102396855B9 publication Critical patent/KR102396855B9/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
    • 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
    • B60W50/0097—Predicting future 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
    • 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
    • B60W40/06—Road conditions
    • B60W40/072—Curvature of the road
    • 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/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/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • 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/10—Longitudinal 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
    • 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
    • B60W2520/00—Input parameters relating to overall vehicle dynamics
    • B60W2520/28—Wheel 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
    • B60W2540/00—Input parameters relating to occupants
    • B60W2540/18—Steering angle

Abstract

본 발명에 따른 측위 방향각 추정 방법은 센서부로부터 요 레이트 정보, 휠 스피드 정보 및 GNSS 방향각 정보를 수신하는 단계; 상기 요 레이트 정보를 시간 적분하여 데드 레코닝 방향각을 산출하는 단계; 상기 휠 스피드 정보로부터 현재 속도에 대응하는 차속 구간 별로 미리 준비된 곡률 임계값을 검출하는 단계; 상기 검출된 곡률 임계값과 현재 주행 곡률을 비교하는 단계; 상기 비교 결과에 기초하여 데드 레코닝 방향각 및 GNSS 방향각의 반영 가중치를 조절하는 단계 및 상기 조절된 가중치에 따른 데드 레코닝 방향각 및 GNSS 방향각을 반영하여 측위 방향각을 추정하는 단계를 포함한다.

Description

측위 방향각 추정 시스템 및 방법{SYSTEM AND METHOD FOR ESTIMATING POSITIONING DIRECTION ANGLE}
본 발명은 측위 방향각 추정 시스템 및 방법에 관한 것이다.
도 1은 종래 기술에 따른 측위 방향각 측정 시스템을 설명하기 위한 도면이다. 도 2는 측위 방향각 오차가 증가하는 문제점을 설명하기 위한 도면이다.
종래 기술에 따른 측위 방향각 측정 시스템(10)은 도 1과 같이 방향각 계산부(11), 이동거리 계산부(12), GNSS 방향각 업데이트부(13)를 포함한다.
방향각 계산부(11)는 요 레이트 센서(Yaw Rate Sensor; YRS, 14)에서 출력되는 요 레이트를 시간 적분하여 데드 레코닝(Dead Reckoning) 방향각을 계산한다.
이동거리 계산부(12)는 휠 스피드 센서(Wheel Speed Sensor; WSS, 15)로부터의 휠 스피드를 수신하여 이동 거리를 계산하여 데드 레코딩 위치를 계산한다.
GNSS 방향각 업데이트부(13)는 GNSS 모듈(16)에서 출력되는 방향각과 상기 데드 레코닝 방향각 각각에 대해 가중치(Gain)를 적용하여 합산하여 측위 방향각을 산출할 수 있다.
이 경우, 도 2와 같이 GNSS 데이터 샘플링 주기(1~10Hz)가 느릴 경우, 높은 각속도(예를 들어, 10deg/s) 또는 높은 곡률(1/r)로 선회 주행시 GNSS 방향각 오차가 증가하여 결국 측위 방향각 오차가 증가하게 된다는 문제가 있다(1/r1<1/r2→Δφr1<Δφr2).
본 발명은 칼만 필터의 Gain에 영향을 미치는 측정 잡음 공분산 R값을 곡률에 따라 선택적으로 설정하여 높은 각속도 또는 높은 곡률로 선회주행시 GNSS 방향각 오차로 인해 측위 방향각 오차가 증가하는 문제를 해소할 수 있는 측위 방향각 추정 시스템 및 방법을 제공하고자 한다.
다만, 본 실시예가 이루고자 하는 기술적 과제는 상기된 바와 같은 기술적 과제로 한정되지 않으며, 또 다른 기술적 과제들이 존재할 수 있다.
상술한 기술적 과제를 달성하기 위한 기술적 수단으로서, 본 발명의 제 1 측면에 따른 측위 방향각 추정 방법은 센서부로부터 요 레이트 정보, 휠 스피드 정보 및 GNSS 방향각 정보를 수신하는 단계; 상기 요 레이트 정보를 시간 적분하여 데드 레코닝 방향각을 산출하는 단계; 상기 휠 스피드 정보로부터 현재 속도에 대응하는 차속 구간 별로 미리 준비된 곡률 임계값을 검출하는 단계; 상기 검출된 곡률 임계값과 현재 주행 곡률을 비교하는 단계; 상기 비교 결과에 기초하여 데드 레코닝 방향각 및 GNSS 방향각의 반영 가중치를 조절하는 단계 및 상기 조절된 가중치에 따른 데드 레코닝 방향각 및 GNSS 방향각을 반영하여 측위 방향각을 추정하는 단계를 포함한다.
상기 비교 결과에 기초하여 데드 레코닝 방향각 및 GNSS 방향각의 반영 가중치를 조절하는 단계는, 칼만 필터가 적용된 하기 수학식에 기초하여 상기 데드 레코닝 방향각 및 GNSS 방향각의 반영 가중치가 조절될 수 있다.
[수학식]
Figure 112017121136556-pat00001
이때, 상기
Figure 112017121136556-pat00002
는 칼만필터의 Gain, 상기
Figure 112017121136556-pat00003
는 시간 K에서의 오차 공분산(Error Covariance), 상기 H는 관측모델(Observation Model between the state and the observation)을 의미한다.
상기 비교 결과에 기초하여 데드 레코닝 방향각 및 GNSS 방향각의 반영 가중치를 조절하는 단계는, 상기 현재 주행곡률이 상기 임계값 이상인 경우 상기 데드 레코닝 방향각의 반영 가중치를 증가시키고, 상기 GNSS 방향각의 반영 가중치를 감소시킬 수 있다.
상기 비교 결과에 기초하여 데드 레코닝 방향각 및 GNSS 방향각의 반영 가중치를 조절하는 단계는, 상기 현재 주행곡률이 상기 임계값 미만인 경우 상기 데드 레코닝 방향각의 반영 가중치를 감소시키고, 상기 GNSS 방향각의 반영 가중치를 증가시킬 수 있다.
상기 조절된 가중치에 따른 데드 레코닝 방향각 및 GNSS 방향각을 반영하여 측위 방향각을 추정하는 단계는, 하기 수학식에 기초하여 상기 측위 방향각을 추정할 수 있다.
[수학식]
측위방향각=Gain×GNSS 방향각+(1-Gain) ×데드 레코닝 방향각
본 발명에 따른 측위 방향각 추정 방법은 상기 센서부에 포함된 조향각 센서로부터 조향각 정보를 수신하여 제 1 곡률을 산출하는 단계; 상기 센서부에 포함된 요 레이트 센서로부터 상기 요 레이트 정보를 수신하여 제 2 곡률을 산출하는 단계 및 상기 산출된 제 1 및 제 2 곡률에 기초하여 상기 현재 주행 곡률을 추정하는 단계를 더 포함할 수 있다.
본 발명에 따른 측위 방향각 추정 방법은 상기 현재 속도를 미리 설정된 임계 속도와 비교하는 단계 및 상기 비교 결과에 기초하여 상기 제 1 및 제 2 곡률의 가중치를 조절하는 단계를 더 포함할 수 있다. 이때, 상기 검출된 곡률 임계값과 현재 주행 곡률을 비교하는 단계는, 상기 제 1 및 제 2 곡률의 가중치가 조절된 현재 주행 곡률과 상기 검출된 곡률 임계값을 비교할 수 있다.
상기 제 1 및 제 2 곡률의 가중치를 조절하는 단계는, 상기 비교 결과 상기 현재 속도가 상기 임계 속도 미만인 경우 상기 제 1 곡률의 가중치를 증가시키고 상기 제 2 곡률의 가중치를 감소시킬 수 있다.
상기 제 1 및 제 2 곡률의 가중치를 조절하는 단계는, 상기 비교 결과 상기 현재 속도가 상기 임계 속도 이상인 경우 상기 제 1 곡률의 가중치를 감소시키고 상기 제 2 곡률의 가중치를 증가시킬 수 있다.
또한, 본 발명의 제 2 측면에 따른 측위 방향각 추정 시스템은 센서부로부터 요 레이트 정보, 휠 스피드 정보 및 GNSS 방향각 정보를 수신하는 센서 정보 수신부 및 상기 요 레이트 정보를 시간 적분하여 데드 레코닝 방향각을 산출하고, 상기 휠 스피드 정보로부터 현재 속도에 대응하는 차속 구간별로 미리 준비된 곡률 임계값을 검출하며, 상기 검출된 곡률 임계값과 현재 주행 곡률을 비교한 결과에 기초하여 상기 데드 레코닝 방향각 및 상기 GNSS 방향각의 반영 가중치를 조절하고, 상기 조절된 가중치에 따른 데드 레코닝 방향각 및 GNSS 방향각을 반영하여 측위 방향각을 추정하는 측위 방향각 추정부를 포함한다.
측위 방향각 추정부는 칼만 필터가 적용된 하기 수학식에 기초하여 상기 데드 레코닝 방향각 및 GNSS 방향각의 반영 가중치를 조절할 수 있다.
[수학식]
Figure 112017121136556-pat00004
상기
Figure 112017121136556-pat00005
는 칼만필터의 Gain, 상기
Figure 112017121136556-pat00006
는 시간 K에서의 오차 공분산(Error Covariance), 상기 H는 관측모델(Observation Model between the state and the observation)을 의미한다.
측위 방향각 추정부는 상기 현재 주행곡률이 상기 임계값 이상인 경우 상기 데드 레코닝 방향각의 반영 가중치를 증가시키고 상기 GNSS 방향각의 반영 가중치를 감소시키며, 상기 현재 주행곡률이 상기 임계값 미만인 경우 상기 데드 레코닝 방향각의 반영 가중치를 감소시키고, 상기 GNSS 방향각의 반영 가중치를 증가시킬 수 있다.
본 발명에 따른 측위 방향각 추정 시스템은 상기 센서부에 포함된 조향각 센서로부터 조향각 정보를 수신하여 제 1 곡률을 산출하고, 상기 요 레이트 정보를 수신하여 제 2 곡률을 산출하며, 상기 산출된 제 1 및 제 2 곡률에 기초하여 상기 현재 주행 곡률을 추정하는 곡률 추정부를 더 포함할 수 있다.
상기 곡률 추정부는 상기 현재 속도를 미리 설정된 임계 속도와 비교하고, 상기 비교 결과에 기초하여 상기 제 1 및 제 2 곡률의 가중치를 조절하되, 상기 측위 방향각 추정부는 상기 제 1 및 제 2 곡률의 가중치가 조절된 현재 주행 곡률과 상기 검출된 곡률 임계값을 비교할 수 있다.
상기 곡률 추정부는 상기 비교 결과 상기 현재 속도가 상기 임계 속도 미만인 경우 상기 제 1 곡률의 가중치를 증가시키고 상기 제 2 곡률의 가중치를 감소시키며, 상기 비교 결과 상기 현재 속도가 상기 임계 속도 이상인 경우 상기 제 1 곡률의 가중치를 감소시키고 상기 제 2 곡률의 가중치를 증가시킬 수 있다.
전술한 본 발명의 과제 해결 수단 중 어느 하나에 의하면, 높은 각속도 또는 높은 곡률로 선회 주행시 GNSS 방향각과 데드 레코닝 방향각의 반영 가중치를 조절하여 측위 방향각을 추정하는 성능을 향상시킬 수 있다.
또한, 요 레이트 센서 및 조향각 센서에 의해 산출된 각각의 곡률의 가중치를 조절하여 곡률을 추정 및 반영함으로써 측위 방향각 추정 성능을 보다 향상시킬 수 있다.
이와 같이 추정된 측위 방향각을 이용함으로써 이동거리 및 위치 정확도를 향상시킬 수 있다.
도 1은 종래 기술에 따른 측위 방향각 측정 시스템을 설명하기 위한 도면이다.
도 2는 측위 방향각 오차가 증가하는 문제점을 설명하기 위한 도면이다.
도 3은 본 발명의 일 실시예에 따른 측위 방향각 추정 시스템의 블록도이다.
도 4는 본 발명의 일 실시예에 따른 측위 방향각 추정 방법의 순서도이다.
아래에서는 첨부한 도면을 참조하여 본원이 속하는 기술 분야에서 통상의 지식을 가진 자가 용이하게 실시할 수 있도록 본원의 실시예를 상세히 설명한다. 그러나 본원은 여러 가지 상이한 형태로 구현될 수 있으며 여기에서 설명하는 실시예에 한정되지 않는다. 그리고 도면에서 본원을 명확하게 설명하기 위해서 설명과 관계없는 부분은 생략하였으며, 명세서 전체를 통하여 유사한 부분에 대해서는 유사한 도면 부호를 붙였다.
명세서 전체에서 어떤 부분이 어떤 구성요소를 “포함”한다고 할 때 이는 특별히 반대되는 기재가 없는 한 다른 구성요소를 제외하는 것이 아니라 다른 구성요소를 더 포함할 수 있는 것을 의미한다.
도 3은 본 발명의 일 실시예에 따른 측위 방향각 추정 시스템(100)의 블록도이다.
본 발명의 일 실시예에 따른 측위 방향각 추정 시스템(100)은 센서 정보 수신부(110) 및 측위 방향각 추정부(120)를 포함한다.
센서 정보 수신부(110)는 요 레이트 센서(142), 휠 스피드 센서(143) 및 GNSS 모듈(144)로부터 각각 요 레이트 정보, 휠 스피드 정보 및 GNSS 방향각 정보를 수신한다.
측위 방향각 추정부(120)는 상기 요 레이트 정보를 시간 적분하여 데드 레코닝 방향각을 산출한다. 그리고 휠 스피드 정보로부터 현재 속도에 대응하는 차속 구간별로 미리 준비된 곡률 임계값을 검출한다.
예를 들어, 차속 구간별로 미리 준비된 곡률 임계값은 메모리 상에 매칭 테이블로 다음과 같이 저장되어 있을 수 있다. 곡률은 차속에 따라 변화하므로 차속 구간별로 곡률 임계값을 시험을 통해 미리 찾아내어 표 1과 같은 매칭 테이블로 저장할 수 있다.
차속(kph) 곡률 임계값(rad/m)
~15 A
15~30 B
30~60 C
60~90 D
90~200 E
그리고 측위 방향각 추정부(120)는 검출된 곡률 임계값과 현재 주행 곡률을 비교한 결과에 기초하여 데드 레코닝 방향각 및 GNSS 방향각의 반영 가중치를 조절하고, 조절된 가중치에 따른 데드 레코닝 방향각 및 GNSS 방향각을 반영하여 측위 방향각을 추정할 수 있다.
한편, 본 발명의 일 실시예에 따른 측위 방향각 추정 시스템(100)은 곡률 추정부(130)를 더 포함할 수 있다.
곡률 추정부(130)는 센서부(140)에 포함된 조향각 센서(141)로부터 조향각 정보를 수신하여 제 1 곡률을 산출하고, 요 레이트 정보를 수신하여 제 2 곡률을 산출할 수 있다. 그리고 산출된 제 1 및 제 2 곡률에 기초하여 현재 주행 곡률을 추정할 수 있다.
이때, 곡률 추정부(130)는 현재 속도를 미리 설정된 임계 속도와 비교하고, 비교 결과에 기초하여 제 1 및 제 2 곡률의 가중치를 조절할 수 있다. 이에 따라, 측위 방향각 추정부(120)는 제 1 및 제 2 곡률의 가중치가 조절된 현재 주행 곡률과 상기 차속 구간별로 미리 준비된 곡률 임계값 중 검출된 곡률 임계값을 비교하여 상기 상기 데드 레코닝 방향각 및 상기 GNSS 방향각의 반영 가중치를 조절할 수 있다.
한편, 본 발명의 일 실시예에 따른 측위 방향각 추정 시스템(100)의 각 구성요소는 메모리(미도시)와, 메모리에 저장된 프로그램을 실행시키는 프로세서(미도시)로 구성될 수 있다.
여기에서, 메모리는 전원이 공급되지 않아도 저장된 정보를 계속 유지하는 비휘발성 저장장치 및 휘발성 저장장치를 통칭하는 것이다.
예를 들어, 메모리는 콤팩트 플래시(compact flash; CF) 카드, SD(secure digital) 카드, 메모리 스틱(memory stick), 솔리드 스테이트 드라이브(solid-state drive; SSD) 및 마이크로(micro) SD 카드 등과 같은 낸드 플래시 메모리(NAND flash memory), 하드 디스크 드라이브(hard disk drive; HDD) 등과 같은 마그네틱 컴퓨터 기억 장치 및 CD-ROM, DVD-ROM 등과 같은 광학 디스크 드라이브(optical disc drive) 등을 포함할 수 있다.
참고로, 본 발명의 실시예에 따른 도 3에 도시된 구성 요소들은 소프트웨어 또는 FPGA(Field Programmable Gate Array) 또는 ASIC(Application Specific Integrated Circuit)와 같은 하드웨어 형태로 구현될 수 있으며, 소정의 역할들을 수행할 수 있다.
그렇지만 '구성 요소들'은 소프트웨어 또는 하드웨어에 한정되는 의미는 아니며, 각 구성 요소는 어드레싱할 수 있는 저장 매체에 있도록 구성될 수도 있고 하나 또는 그 이상의 프로세서들을 재생시키도록 구성될 수도 있다.
따라서, 일 예로서 구성 요소는 소프트웨어 구성 요소들, 객체지향 소프트웨어 구성 요소들, 클래스 구성 요소들 및 태스크 구성 요소들과 같은 구성 요소들과, 프로세스들, 함수들, 속성들, 프로시저들, 서브루틴들, 프로그램 코드의 세그먼트들, 드라이버들, 펌웨어, 마이크로 코드, 회로, 데이터, 데이터베이스, 데이터 구조들, 테이블들, 어레이들 및 변수들을 포함한다.
구성 요소들과 해당 구성 요소들 안에서 제공되는 기능은 더 작은 수의 구성 요소들로 결합되거나 추가적인 구성 요소들로 더 분리될 수 있다.
본 발명의 방법 및 시스템은 특정 실시예와 관련하여 설명되었지만, 그것들의 구성 요소 또는 동작의 일부 또는 전부는 범용 하드웨어 아키텍쳐를 갖는 컴퓨터 시스템을 사용하여 구현될 수 있다.
이하에서는 도 4를 참조하여, 본 발명의 일 실시예에 따른 측위 방향각 추정 시스템(100)에서의 측위 방향각 추정 방법에 대해 보다 구체적으로 설명하도록 한다.
도 4는 본 발명의 일 실시예에 따른 측위 방향각 추정 방법의 순서도이다.
본 발명의 일 실시예에 따른 측위 방향각 추정 방법은 먼저, 센서부(140)에 포함된 요 레이트 센서(142)로부터 요 레이트 정보를, 휠 스피드 센서(143)로부터 휠 스피드 정보를, 그리고 GNSS 모듈(144)로부터 GNSS 방향각 정보를 각각 수신한다(S105).
다음으로, 요 레이트 정보를 시간 적분하여 데드 레코닝 방향각을 산출한다(S110).
다음으로, 휠 스피드 정보로부터 현재 속도에 대응하는 차속 구간별로 미리 준비된 곡률 임계값을 검출하고(S115), 검출된 곡률 임계값과 현재 주행 곡률을 비교한다(S120).
그리고 비교 결과에 기초하여 데드 레코닝 방향각 및 GNSS 방향각의 반영 가중치를 조절한다. 이때, 본 발명의 일 실시예는 칼만 필터가 적용된 하기 수학식 1에 기초하여 데드 레코닝 방향각 및 GNSS 방향각의 반영 가중치를 산출할 수 있다.
[수학식 1]
Figure 112017121136556-pat00007
이때,
Figure 112017121136556-pat00008
는 칼만필터의 Gain을 의미하고,
Figure 112017121136556-pat00009
는 시간 K에서의 오차 공분산(Error Covariance)을 의미하며, H는 관측모델(Observation Model between the state and the observation)을 의미한다.
즉, 본 발명의 일 실시예는 상기 수학식 1에 따른 칼만 필터에서 가중치 Gain을 산출할 수 있다.
이때, 측정 잡음 공분산 R값에 의해 GNSS 방향각 가중치가 조절되게 된다.
한편, 측정 잡음 공분산 R은 단일 상수값으로 튜닝하여 사용하는 방법, 도심지와 평야 지역별로 측정 잡음 공분산을 다르게 하여 평야 GNSS 방향각 반영 가중치를 올리는 방법, GSNN HDOP(Horizontal Dilution of Precision)가 임계값 이하인 경우 GNSS 가중치를 올리는 방법이 적용될 수 있다.
이 경우, GNSS 데이터 샘플링 주기(1~10Hz)가 느릴 경우, 높은 각속도(예를 들어, 10deg/s) 또는 높은 곡률(1/r)로 선회 주행시 GNSS 방향각 오차가 증가하여 결국 측위 방향각 오차가 증가하게 된다는 문제가 있는바, 본 발명의 일 실시예는 이러한 문제점을 해소하기 위하여 상기 현재 주행곡률이 상기 임계값 이상인 경우 상기 데드 레코닝 방향각의 반영 가중치를 증가시키고(S125), 상기 GNSS 방향각의 반영 가중치를 감소시킬 수 있다(S130).
또는, 상기 현재 주행곡률이 상기 임계값 미만인 경우 상기 데드 레코닝 방향각의 반영 가중치를 감소시키고(S135), 상기 GNSS 방향각의 반영 가중치를 증가시킬 수 있다(S140).
이와 같이 데드 레코닝 방향각과 GNSS 방향각의 반영 가중치가 조절되고 나면, 조절된 가중치에 따른 데드 레코닝 방향각 및 GNSS 방향각을 반영하여 측위 방향각을 추정한다(S145).
이때, 측위 방향각은 하기 수학식 2에 의해 산출되어 추정될 수 있다.
[수학식 2]
측위방향각=Gain×GNSS 방향각+(1-Gain) ×데드 레코닝 방향각
한편, 본 발명의 일 실시예에 따른 측위 방향각 추정 방법은 측위 방향각 추정의 정확도를 향상시키기 위하여 이용되는 현재 주행 곡률의 정확도를 향상시키기 위한 과정을 추가적으로 수행할 수 잇다.
이를 위해, 본 발명의 일 실시예는 센서부(140)에 포함된 조향각 센서(141)로부터 조향각 정보를 수신하여 제 1 곡률을 산출한다. 그리고 센서부에 포함된 요 레이트 센서(142)로부터 요 레이트 정보를 수신하여 제 2 곡률을 산출한다(S150, S155).
이때, 제 1 및 제 2 곡률은 하기 수학식 3 및 4에 기초하여 산출될 수 있다. 여기에서 θSAS는 조향각을 의미하고, WheelLength는 휠베이스 즉, 차량의 앞바퀴 차축과 뒷바퀴 차축간의 거리를 의미한다.
[수학식 3]
Figure 112017121136556-pat00010
[수학식 4]
Figure 112017121136556-pat00011
, γ:요 레이트,υ: 속도
이와 같이 제 1 및 제 2 곡률이 산출되고 나면, 제 1 및 제 2 곡률에 기초하여 현재 주행 곡률을 추정한다.
구체적으로, 본 발명의 일 실시예는 현재 속도를 미리 설정된 임계 속도와 비교하고(S160), 비교 결과에 기초하여 제 1 및 제 2 곡률의 가중치를 조절한다. 그리고 제 1 및 제 2 곡률의 가중치가 조절된 현재 주행 곡률과 검출된 곡률 임계값을 비교하게 된다.
이때, 상기 비교 결과 상기 현재 속도가 상기 임계 속도 이상인 경우, 즉 고속 주행시의 경우에는 슬립(slip)이 발생할 수 있으므로, 상기 제 1 곡률의 가중치를 감소시키고(S165) 상기 제 2 곡률의 가중치를 증가시킬 수 있다(S170).
이와 반대로 상기 비교 결과 상기 현재 속도가 상기 임계 속도 미만인 경우, 즉 저속 주행시에는 슬립이 거의 없으므로 상기 제 1 곡률의 가중치를 증가시키고(S175) 상기 제 2 곡률의 가중치를 감소시킬 수 있다(S180).
한편, 제 1 및 제 2 곡률의 가중치를 조절하는 방법에는 상술한 수학식 1에 따른 칼만 필터를 이용할 수 있다.
이와 같이 조정된 제 1 및 제 2 곡률에 기초하여 현재 주행 곡률을 산출하고, 산출된 현재 주행 곡률은 상술한 단계 S120에 적용될 수 있다(S185).
상술한 설명에서, 단계 S105 내지 S185은 본 발명의 구현예에 따라서, 추가적인 단계들로 더 분할되거나, 더 적은 단계들로 조합될 수 있다. 또한, 일부 단계는 필요에 따라 생략될 수도 있고, 단계 간의 순서가 변경될 수도 있다. 아울러, 기타 생략된 내용이라 하더라도 도 3에서의 측위 방향각 추정 시스템(100)에 관하여 이미 기술된 내용은 도 4의 측위 방향각 추정 방법에도 적용된다.
전술한 본 발명의 설명은 예시를 위한 것이며, 본 발명이 속하는 기술분야의 통상의 지식을 가진 자는 본 발명의 기술적 사상이나 필수적인 특징을 변경하지 않고서 다른 구체적인 형태로 쉽게 변형이 가능하다는 것을 이해할 수 있을 것이다. 그러므로 이상에서 기술한 실시예들은 모든 면에서 예시적인 것이며 한정적이 아닌 것으로 이해해야만 한다. 예를 들어, 단일형으로 설명되어 있는 각 구성 요소는 분산되어 실시될 수도 있으며, 마찬가지로 분산된 것으로 설명되어 있는 구성 요소들도 결합된 형태로 실시될 수 있다.
본 발명의 범위는 상기 상세한 설명보다는 후술하는 특허청구범위에 의하여 나타내어지며, 특허청구범위의 의미 및 범위 그리고 그 균등 개념으로부터 도출되는 모든 변경 또는 변형된 형태가 본 발명의 범위에 포함되는 것으로 해석되어야 한다.
100: 측위 방향각 추정 시스템
110: 센서 정보 수신부
120: 측위 방향각 추정부
130: 곡률 추정부
140: 센서부
141: 조향각 센서
142: 요 레이트 센서
143: 휠 스피드 센서
144: GNSS 모듈

Claims (15)

  1. 센서부로부터 요 레이트 정보, 휠 스피드 정보 및 GNSS 방향각 정보를 수신하는 단계;
    상기 요 레이트 정보를 시간 적분하여 데드 레코닝 방향각을 산출하는 단계;
    상기 휠 스피드 정보로부터 현재 속도에 대응하는 차속 구간 별로 미리 준비된 곡률 임계값을 검출하는 단계;
    상기 검출된 곡률 임계값과 현재 주행 곡률을 비교하는 단계;
    상기 비교 결과에 기초하여 데드 레코닝 방향각 및 GNSS 방향각의 반영 가중치를 조절하는 단계 및
    상기 조절된 가중치에 따른 데드 레코닝 방향각 및 GNSS 방향각을 반영하여 측위 방향각을 추정하는 단계를 포함하고,
    상기 비교 결과에 기초하여 데드 레코닝 방향각 및 GNSS 방향각의 반영 가중치를 조절하는 단계는,
    상기 비교 결과 현재 주행곡률이 상기 곡률 임계값 이상 또는 미만인지 여부에 기초하여, 상기 데드 레코닝 방향각의 반영 가중치를 증감시키고, 이에 상응하여 상기 GNSS 방향각의 반영 가중치를 증감시키는 것인 측위 방향각 추정 방법.
  2. 제 1 항에 있어서,
    상기 비교 결과에 기초하여 데드 레코닝 방향각 및 GNSS 방향각의 반영 가중치를 조절하는 단계는,
    칼만 필터가 적용된 하기 수학식에 기초하여 상기 데드 레코닝 방향각 및 GNSS 방향각의 반영 가중치가 조절되는 것인 측위 방향각 추정 방법.
    [수학식]
    Figure 112017121136556-pat00012

    상기
    Figure 112017121136556-pat00013
    는 칼만필터의 Gain, 상기
    Figure 112017121136556-pat00014
    는 시간 K에서의 오차 공분산(Error Covariance), 상기 H는 관측모델(Observation Model between the state and the observation)을 의미함
  3. 제 1 항에 있어서,
    상기 비교 결과에 기초하여 데드 레코닝 방향각 및 GNSS 방향각의 반영 가중치를 조절하는 단계는,
    상기 현재 주행곡률이 상기 임계값 이상인 경우 상기 데드 레코닝 방향각의 반영 가중치를 증가시키고, 상기 GNSS 방향각의 반영 가중치를 감소시키는 것인 측위 방향각 추정 방법.
  4. 제 1 항에 있어서,
    상기 비교 결과에 기초하여 데드 레코닝 방향각 및 GNSS 방향각의 반영 가중치를 조절하는 단계는,
    상기 현재 주행곡률이 상기 임계값 미만인 경우 상기 데드 레코닝 방향각의 반영 가중치를 감소시키고, 상기 GNSS 방향각의 반영 가중치를 증가시키는 것인 측위 방향각 추정 방법.
  5. 제 1 항에 있어서,
    상기 조절된 가중치에 따른 데드 레코닝 방향각 및 GNSS 방향각을 반영하여 측위 방향각을 추정하는 단계는,
    하기 수학식에 기초하여 상기 측위 방향각을 추정하는 것인 측위 방향각 추정 방법.
    [수학식]
    측위방향각=Gain×GNSS 방향각+(1-Gain) ×데드 레코닝 방향각
  6. 제 1 항에 있어서,
    상기 센서부에 포함된 조향각 센서로부터 조향각 정보를 수신하여 제 1 곡률을 산출하는 단계;
    상기 센서부에 포함된 요 레이트 센서로부터 상기 요 레이트 정보를 수신하여 제 2 곡률을 산출하는 단계 및
    상기 산출된 제 1 및 제 2 곡률에 기초하여 상기 현재 주행 곡률을 추정하는 단계를 더 포함하는 측위 방향각 추정 방법.
  7. 제 6 항에 있어서,
    상기 현재 속도를 미리 설정된 임계 속도와 비교하는 단계 및
    상기 비교 결과에 기초하여 상기 제 1 및 제 2 곡률의 가중치를 조절하는 단계를 더 포함하되,
    상기 검출된 곡률 임계값과 현재 주행 곡률을 비교하는 단계는, 상기 제 1 및 제 2 곡률의 가중치가 조절된 현재 주행 곡률과 상기 검출된 곡률 임계값을 비교하는 것인 측위 방향각 추정 방법.
  8. 제 7 항에 있어서,
    상기 제 1 및 제 2 곡률의 가중치를 조절하는 단계는,
    상기 비교 결과 상기 현재 속도가 상기 임계 속도 미만인 경우 상기 제 1 곡률의 가중치를 증가시키고 상기 제 2 곡률의 가중치를 감소시키는 것인 측위 방향각 추정 방법.
  9. 제 7 항에 있어서,
    상기 제 1 및 제 2 곡률의 가중치를 조절하는 단계는,
    상기 비교 결과 상기 현재 속도가 상기 임계 속도 이상인 경우 상기 제 1 곡률의 가중치를 감소시키고 상기 제 2 곡률의 가중치를 증가시키는 것인 측위 방향각 추정 방법.
  10. 측위 방향각 추정 시스템에 있어서,
    센서부로부터 요 레이트 정보, 휠 스피드 정보 및 GNSS 방향각 정보를 수신하는 센서 정보 수신부 및
    상기 요 레이트 정보를 시간 적분하여 데드 레코닝 방향각을 산출하고, 상기 휠 스피드 정보로부터 현재 속도에 대응하는 차속 구간별로 미리 준비된 곡률 임계값을 검출하며, 상기 검출된 곡률 임계값과 현재 주행 곡률을 비교한 결과 에 기초하여 상기 데드 레코닝 방향각 및 상기 GNSS 방향각의 반영 가중치를 조절하고, 상기 조절된 가중치에 따른 데드 레코닝 방향각 및 GNSS 방향각을 반영하여 측위 방향각을 추정하는 측위 방향각 추정부를 포함하고,
    상기 측위 방향각 추정부는 상기 비교 결과 현재 주행곡률이 상기 곡률 임계값 이상 또는 미만인지 여부에 기초하여, 상기 데드 레코닝 방향각의 반영 가중치를 증감시키고, 이에 상응하여 상기 GNSS 방향각의 반영 가중치를 증감시키는 것인 측위 방향각 추정 시스템.
  11. 제 10 항에 있어서,
    측위 방향각 추정부는 칼만 필터가 적용된 하기 수학식에 기초하여 상기 데드 레코닝 방향각 및 GNSS 방향각의 반영 가중치를 조절하는 것인 측위 방향각 추정 시스템.
    [수학식]
    Figure 112017121136556-pat00015

    상기
    Figure 112017121136556-pat00016
    는 칼만필터의 Gain, 상기
    Figure 112017121136556-pat00017
    는 시간 K에서의 오차 공분산(Error Covariance), 상기 H는 관측모델(Observation Model between the state and the observation)을 의미함
  12. 제 10 항에 있어서,
    측위 방향각 추정부는 상기 현재 주행곡률이 상기 임계값 이상인 경우 상기 데드 레코닝 방향각의 반영 가중치를 증가시키고 상기 GNSS 방향각의 반영 가중치를 감소시키며, 상기 현재 주행곡률이 상기 임계값 미만인 경우 상기 데드 레코닝 방향각의 반영 가중치를 감소시키고, 상기 GNSS 방향각의 반영 가중치를 증가시키는 것인 측위 방향각 추정 시스템.
  13. 제 10 항에 있어서,
    상기 센서부에 포함된 조향각 센서로부터 조향각 정보를 수신하여 제 1 곡률을 산출하고, 상기 요 레이트 정보를 수신하여 제 2 곡률을 산출하며, 상기 산출된 제 1 및 제 2 곡률에 기초하여 상기 현재 주행 곡률을 추정하는 곡률 추정부를 더 포함하는 측위 방향각 추정 시스템.
  14. 제 13 항에 있어서,
    상기 곡률 추정부는 상기 현재 속도를 미리 설정된 임계 속도와 비교하고, 상기 비교 결과에 기초하여 상기 제 1 및 제 2 곡률의 가중치를 조절하되,
    상기 측위 방향각 추정부는 상기 제 1 및 제 2 곡률의 가중치가 조절된 현재 주행 곡률과 상기 검출된 곡률 임계값을 비교하는 것인 측위 방향각 추정 시스템.
  15. 제 14 항에 있어서,
    상기 곡률 추정부는 상기 비교 결과 상기 현재 속도가 상기 임계 속도 미만인 경우 상기 제 1 곡률의 가중치를 증가시키고 상기 제 2 곡률의 가중치를 감소시키며, 상기 비교 결과 상기 현재 속도가 상기 임계 속도 이상인 경우 상기 제 1 곡률의 가중치를 감소시키고 상기 제 2 곡률의 가중치를 증가시키는 것인 측위 방향각 추정 시스템.
KR1020170165757A 2017-12-05 2017-12-05 측위 방향각 추정 시스템 및 방법 KR102396855B1 (ko)

Priority Applications (1)

Application Number Priority Date Filing Date Title
KR1020170165757A KR102396855B1 (ko) 2017-12-05 2017-12-05 측위 방향각 추정 시스템 및 방법

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
KR1020170165757A KR102396855B1 (ko) 2017-12-05 2017-12-05 측위 방향각 추정 시스템 및 방법

Publications (3)

Publication Number Publication Date
KR20190066229A KR20190066229A (ko) 2019-06-13
KR102396855B1 true KR102396855B1 (ko) 2022-05-11
KR102396855B9 KR102396855B9 (ko) 2023-05-11

Family

ID=66847610

Family Applications (1)

Application Number Title Priority Date Filing Date
KR1020170165757A KR102396855B1 (ko) 2017-12-05 2017-12-05 측위 방향각 추정 시스템 및 방법

Country Status (1)

Country Link
KR (1) KR102396855B1 (ko)

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2013533962A (ja) 2010-06-10 2013-08-29 クアルコム，インコーポレイテッド 慣性センサデータを使用した移動局測位の改良
KR101390776B1 (ko) 2013-03-14 2014-04-30 인하대학교 산학협력단 퍼지 확장 칼만 필터를 이용한 위치인식 장치, 방법 및 이동로봇
KR101697645B1 (ko) * 2014-10-06 2017-01-18 현대모비스 주식회사 추측 항법과 gps를 이용한 복합 항법 시스템 및 그 방법
KR102159360B1 (ko) 2014-09-26 2020-09-23 현대모비스 주식회사 곡률 추정 장치 및 방법

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR20110055854A (ko) * 2009-11-20 2011-05-26 현대모비스 주식회사 커브 구간의 곡률 및 차량 속도를 이용하여 커브 구간 경로 안내를 수행하는 내비게이션 시스템 및 그 경로 안내 방법

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2013533962A (ja) 2010-06-10 2013-08-29 クアルコム，インコーポレイテッド 慣性センサデータを使用した移動局測位の改良
KR101390776B1 (ko) 2013-03-14 2014-04-30 인하대학교 산학협력단 퍼지 확장 칼만 필터를 이용한 위치인식 장치, 방법 및 이동로봇
KR102159360B1 (ko) 2014-09-26 2020-09-23 현대모비스 주식회사 곡률 추정 장치 및 방법
KR101697645B1 (ko) * 2014-10-06 2017-01-18 현대모비스 주식회사 추측 항법과 gps를 이용한 복합 항법 시스템 및 그 방법

Also Published As

Publication number Publication date
KR102396855B9 (ko) 2023-05-11
KR20190066229A (ko) 2019-06-13

Similar Documents

Publication Publication Date Title
CN103270543B (zh) 驾驶辅助装置
US8185309B2 (en) Enhanced inertial system performance
WO2020253260A1 (zh) 时间同步处理方法、电子设备及存储介质
US8255081B2 (en) Apparatus and method for calculating position of robot
US20210179140A1 (en) Drifting correction between planning stage and controlling stage of operating autonomous driving vehicles
CN109297486B (zh) 惯性与多里程计信息辅助的车体运动状态判定方法及系统
CN107933564B (zh) 道路坡度估算方法、道路坡度估算装置、终端设备以及计算机可读存储介质
KR101106048B1 (ko) 센서오차의 작동 중 자동교정 방법과 이를 이용한 관성항법장치
US20200029176A1 (en) Motion detection method, device, and medium
JP2020134528A (ja) 定常横方向偏差の除去方法、装置、記憶媒体、及びプログラム
CN110146076A (zh) 一种无逆矩阵自适应滤波的sins/dvl组合定位方法
CN103644910A (zh) 基于分段rts平滑算法的个人自主导航系统定位方法
US20180203094A1 (en) Emitter Geolocation Using Sorted Observations
US11650077B2 (en) Strict reverse navigation method for optimal estimation of fine alignment
EP4345421A2 (en) Method for calibrating sensor parameters based on autonomous driving, apparatus, storage medium, and vehicle
CN104613965A (zh) 一种基于双向滤波平滑技术的步进式行人导航方法
US20230392938A1 (en) Navigation information processing method, electronic device, and storage medium
JPWO2020184013A1 (ja) 車両制御装置
US10466054B2 (en) Method and system for estimating relative angle between headings
CN104819717A (zh) 一种基于mems惯性传感器组的多旋翼飞行器姿态检测方法
KR102396855B1 (ko) 측위 방향각 추정 시스템 및 방법
CN111912414B (zh) 车辆位姿的验证方法、装置、设备及存储介质
CN109710594B (zh) 地图数据的有效性判断方法、装置及可读存储介质
CN116772858A (zh) 车辆定位方法、装置、定位设备及存储介质
CN116381760A (zh) Gnss rtk/ins紧耦合定位方法、装置及介质

Legal Events

Date Code Title Description
A201 Request for examination
E902 Notification of reason for refusal
E701 Decision to grant or registration of patent right
G170 Re-publication after modification of scope of protection [patent]