KR101960164B1 - 확장 칼만 필터를 이용한 물체의 실시간 방위각 산출 방법 - Google Patents

확장 칼만 필터를 이용한 물체의 실시간 방위각 산출 방법 Download PDF

Info

Publication number
KR101960164B1
KR101960164B1 KR1020170063350A KR20170063350A KR101960164B1 KR 101960164 B1 KR101960164 B1 KR 101960164B1 KR 1020170063350 A KR1020170063350 A KR 1020170063350A KR 20170063350 A KR20170063350 A KR 20170063350A KR 101960164 B1 KR101960164 B1 KR 101960164B1
Authority
KR
South Korea
Prior art keywords
gps
azimuth
ekf
calculating
azimuth angle
Prior art date
Application number
KR1020170063350A
Other languages
English (en)
Other versions
KR20180128173A (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 KR1020170063350A priority Critical patent/KR101960164B1/ko
Publication of KR20180128173A publication Critical patent/KR20180128173A/ko
Application granted granted Critical
Publication of KR101960164B1 publication Critical patent/KR101960164B1/ko

Links

Images

Classifications

    • 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
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G01C21/203—Specially adapted for sailing ships
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G01C21/22—Plotting boards
    • 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/03—Cooperating elements; Interaction or communication between different cooperating elements or between cooperating elements and receivers
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Automation & Control Theory (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)
  • Navigation (AREA)

Abstract

본 발명은 확장 칼만 필터를 이용한 물체의 실시간 방위각 산출 방법에 관한 것으로, 보다 상세하게는 다수의 GPS 수신기, 단일의 EC 모듈, 및 단일의 IMU 모듈을 사용하여 산출된 GPS 및 EC 모듈에 대한 방위각과 가속도계값을 토대로 공분산 교차점(CI)이 적용된 확장 칼만 필터(EKF)를 사용하여 최종 융합 방위각(psi_CI)을 산출함으로써, 저가의 센서로도 물체의 방위각을 정확하게 산출할 수 있는 공분산 교차점이 적용된 확장 칼만 필터(EKF-Cl)를 이용한 물체의 실시간 방위각 산출 방법에 관한 것이다.
또한, 수평 위치 오차(HDOP) 및 수정 품질(FIX)값을 토대로 GPS 데이터의 유효성을 별도로 평가하도록 하여 다수의 GPS로부터 획득한 GPS 데이터가 유효하지 않거나, 누락된 경우에도 EC에 대한 방위각과 IMU의 가속도계값을 이용하여 물체의 방위각을 산출하는 방법에 관한 것이다.

Description

확장 칼만 필터를 이용한 물체의 실시간 방위각 산출 방법{A method for calculating a Real-Time Heading value of object using EKF-Cl}
본 발명은 확장 칼만 필터를 이용한 물체의 실시간 방위각 산출 방법에 관한 것으로, 보다 상세하게는 다수의 GPS, 단일의 EC, 및 단일의 IMU를 사용하여 산출된 GPS 및 EC에 대한 방위각과 가속도계값을 토대로 공분산 교차점이 적용된 확장 칼만 필터를 사용하여 최종 융합 방위각을 산출함으로써, 저가의 센서로도 물체의 방위각을 정확하게 산출할 수 있는 공분산 교차점이 적용된 확장 칼만 필터를 이용한 물체(정밀 농기계)의 실시간 방위각 산출 방법에 관한 것이다.
실내 및 도심과 같은 GPS(Global Positioning System)의 수신이 불량한 곳에서도 연속적인 위치정보를 제공할 수 있도록 GPS, 관성센서 등을 이용한 개인의 위치측정장치 및 방법이 발명되었으며 지속적인 개발 중에 있다. 또한 선박, 자동차, 항공기 등을 예정된 경로와 고도 또는 항로로 항해하기 위한 자동항법장치(Autopilot)에 이용할 수 있는 관성항법 및 위치측정기법이 발명되었다.
선박에의 자동항법장치는 GPS 또는 외부입력에 의해 표시되는 목표방위각과 컴파스(Compass)에 의한 선수방향을 비교하여 타각(즉, 선박이 일정한 목표지점으로 항해하려고 할 때 항로의 방향을 나타내 주는 각)을 조절하여 줌으로써 목표방향으로 자동 운항되도록 하는 장치로서, 대형 선박뿐만 아니라 소형선박에도 적용되어 사용되고 있다.
특히 항공분야에서는 무인기 기종을 위한 제어보드에 항법관련신호, GPS신호 및 관성센서의 각속도와 가속도 정보를 GPS/INS 통합 항법절차에 따라 계산하여 실시간으로 전송처리하는 항법모듈를 이용한 무인기용 통합 비행제어 컴퓨터 시스템이 적용되어 사용되고 있다.
그러나 저가의 센서로도 센서 융합 기술에 의해 농기계 등과 같은 물체의 이동 및 방위값을 정확하게 산출할 수 시스템 및 방법이 제시되고 있지 않다.
또한 인공위성을 이용한 위치 측정 데이터가 유효하지 않거나, 누락된 경우에도 경우 대비하여 GPS 데이터의 유효성을 별도로 평가하도록 하는 기술은 제시하고 있지 않다.
따라서, 다수의 저가용 센서 데이터들의 센서 융합을 통하여 물체의 방위각을 실시간으로 보다 정확하게 산출할 수 있는 기술이 절실한 실정이다. 또한 인공위성 정보의 신뢰성을 수평 위치 오차(HDOP, Horizontal Dilution Of Precision) 및 수정 품질(FIX, fix quality of GPS data)값을 토대로 평가하여 사용할 수 없는 부정확한 데이터일 경우 다른 센서 데이터로 대체할 수 있는 시스템이 필수적이다.
대한민국 등록특허 제 10-0414439 호(등록일: 2003년12월2일)
본 발명은 상기와 같은 문제점을 감안하여 안출한 것으로, 본 발명의 목적은 3개의 GPS, 단일의 EC, 및 단일의 IMU(Inertial Measurement Unit)를 사용하여 산출된 GPS 및 EC(Electronic Compass)에 대한 방위각과 가속도계값을 토대로 공분산 교차점(CI, Covariance Intersection)이 적용된 확장 칼만 필터(EKF, Extended Kalman Filter)를 사용하여 최종 융합 방위각(psi_CI)을 산출함으로써, 저가의 센서로도 물체(정밀 농기계 등)의 방위각을 정확하게 산출할 수 있는 확장 칼만 필터를 이용한 물체의 실시간 방위각 산출 방법을 제공하는 것이다.
본 발명의 다른 목적은, 수평 위치 오차(HDOP, Horizontal Dilution Of Precision) 및 수정 품질(FIX, fix quality of GPS data) 값을 토대로 GPS 데이터의 유효성을 별도로 평가하도록 하여 다수의 GPS로부터 획득한 GPS 데이터가 유효하지 않거나, 누락된 경우에도 경우에도 EC에 대한 방위각과 IMU의 가속도계값을 이용하여 물체의 방위각을 산출하는 방법을 제공하는 것이다.
상기 목적들을 달성하기 위한 본 발명에 따른 공분산 교차점이 적용된 확장 칼만 필터(EKF-Cl)를 이용한 물체의 실시간 방위각 산출 방법은 총 6가지 단계의 재귀적 구조를 가지고 있다. 실시간으로 검출되는 GPS, EC, IMU 모듈의 데이터에 대하여 방위각, 유효성 평가, 가중치, 공분산 행렬을 계산하고 실시간 센서 융합을 수행한다.
정밀한 방위각을 산출하기 위하여 본 발명에서는 다수의 GSP 수신기와 단일의 EC모듈 및 IMU모듈을 사용한다. 그 첫 번째 이유로는 GPS 수신 정보에 관성 및 지자기 데이터를 포함시켜 방위각 산출의 정밀도를 높이기 위해서이고, 두 번째 이유로는 GPS 수신기의 고장 또는 사용할 수 없는 데이터를 출력할 상황에 대응하기 위해서이다.
정밀한 위치 데이터 및 방위각을 출력하는 GPS 수신기 및 EC 모듈은 고가의 장비이다. 그러나 본 발명에서는 저가용 센서들을 EKF-CI 기법을 이용하여 융합하고 고가의 장비들과 비슷한 정밀도의 데이터를 출력할 수 있다.
이에 따라 GPS, EC, IMU 모듈의 데이터를 사용하여 보다 정밀한 방위각을 계산하기 위한 방법으로 공분산 교차점이 적용된 확장 칼만 필터(EKF-CI)를 사용하였다. 최종 융합 방위각의 유도에 사용되는 매개변수로는 GPS수신기 및 EC모듈 각각의 데이터에 대한 전방 방위각, 유효성 평가에 따른 가중치, 및 그들의 융합 데이터인 GPS 및 EC 모듈에 대한 방위각과 IMU 모듈의 수신값으로부터 X-축 가속도계(X-Axis accelerometer) 및 Y-축 가속도계(X-Axis accelerometer)의 값(IMU(Accx, Accy)), 그리고 공분산행렬이 있다.
또한 개별적으로 GPS 수신 데이터가 누락되었거나 사용할 수 없는 데이터임을 검증하는 유효성 지표를 따로 정의하였다. 유효성을 나타내는 지표로는 수평 위치 오차(HDOP)와 수정 품질(FIX)값이 주로 이용된다.
이때, 개별적인 HDOP는 그 값이 1보다 작을 경우 가장 높은 신뢰 수준으로 써 이상적(ideal), 1 내지 2일 경우는 신뢰할 수 있는 수준으로 훌륭함(Excellent), 2 내지 5일 경우는 신뢰할 수 있는 최소 수준으로 좋음(Good), 5 내지 10일 경우는 위치 측정을 위한 계산에 사용될 수 있지만 수정 품질이 높아야 하는 보통(Moderate), 10 내지 20일 경우는 낮은 신뢰 수준으로 페어(Fair), 20 이상은 부정확하여 사용할 수 없는 값인 나쁨(Poor)으로 간주한다.
또한, FIX는 그 값이 0일 때 사용할 수 없는 값이며, 1일 때 GPS fix, 2일 때 DGPS fix, 3 일 때 PPS fix, 4일 때 RTK(Real Time Kinematics), 5일 때 Float RTK로 인식한다.
본 발명에 있어 상기 수평 위치 오차(HDOP)와 수정 품질(FIX)값을 결합하는 경우에, 상기 HDOP가 1 내지 2이고, FIX가 2 내지 5인 경우는 이상적(Ideal)인 것으로 판단하고, 상기 HDOP가 1 내지 2이고, FIX가 1인 경우와, HDOP가 3 내지 5이고 FIX가 2 내지 5인 경우는 매우 우수(Excellent)한 것으로 판단하고, HDOP가 3 내지 5이고 FIX가 1인 경우와, HDOP가 6 내지 10이고 FIX가 2 내지 5인 경우는 우수(Good)한 것으로 판단하고, 그 외에는 모두 나쁜 것(bad)으로 판단하여 배제시키도록 함이 바람직하다.
본 발명에서는 두 지표를 함께 사용하여 GPS 데이터의 유효성을 판단하고, 유요한 GSP 데이터에 대해서는 가중치(weight)를 부여한다.
GPS 수신 데이터가 누락되었거나 사용할 수 없는 데이터일 경우에는 그 GPS 수신기를 대신하여 EC 모듈이 방위각 산출 및 가중치를 부여 받는다.
상술한 본 발명에 따른 확장 칼만 필터를 이용한 물체의 실시간 방위각 산출 방법에 의하면, 3개의 GPS, 단일의 EC, 및 단일의 IMU를 사용하여 산출된 GPS 및 EC에 대한 방위각과 가속도계값을 토대로 공분산 교차점이 적용된 확장 칼만 필터(EKF-CI)를 사용하여 최종 융합 방위각(psi_CI)을 산출함으로써, 저가의 센서로도 EKF-CI 센서 융합 기술에 의해 물체의 방위각을 정확하게 산출할 수 있는 효과가 있다.
또한, HDOP 및 FIX 값을 토대로 GPS 데이터의 유효성을 별도로 평가하도록 하여 다수의 GPS로부터 획득한 GPS 데이터가 유효하지 않거나, 누락된 경우에도 EC에 대한 방위각과 IMU의 가속도계값을 적용할 수 있는 장점도 있다.
도 1은 본 발명의 일 실시예에 따른 공분산 교차점이 적용된 확장 칼만 필터(EKF-CI)를 이용한 물체의 방위각 산출 시스템의 개략적인 구성을 나타내는 블록도이다.
도 2는 본 발명의 일 실시예에 따른 공분산 교차점이 적용된 확장 칼만 필터 (EKF-CI)를 이용한 물체의 실시간 방위각 산출 방법을 나타내는 순서도이다.
도 3은 본 발명의 일 실시예에 따른 공분산 교차점이 적용된 확장 칼만 필터 (EKF-CI)를 이용한 물체의 최종 융합 방위각을 유도하는 순서도이다.
본 발명은 그 기술적 사상 또는 주요한 특징으로부터 벗어남이 없이 다른 여러가지 형태로 실시될 수 있다. 따라서, 본 발명의 실시예들은 모든 점에서 단순한 예시에 지나지 않으며 한정적으로 해석되어서는 안된다.
제 1, 제 2 등의 용어는 다양한 구성요소들을 설명하는데 사용될 수 있지만, 상기 구성요소들은 상기 용어들에 의해 한정되어서는 안 된다.
상기 용어들은 하나의 구성요소를 다른 구성요소로부터 구별하는 목적으로만 사용된다. 예를 들어, 본 발명의 권리 범위를 벗어나지 않으면서 제 1 구성요소는 제 2 구성요소로 명명될 수 있고, 유사하게 제 2 구성요소도 제 1 구성요소로 명명될 수 있다.
어떤 구성요소가 다른 구성요소에 "연결되어" 있다거나 "접속되어" 있다고 언급된 때에는, 그 다른 구성요소에 직접적으로 연결되어 있거나 또는 접속되어 있을 수도 있지만, 중간에 다른 구성요소가 존재할 수도 있다고 이해되어야 할 것이다.
반면에, 어떤 구성요소가 다른 구성요소에 "직접 연결되어" 있다거나 "직접 접속되어" 있다고 언급된 때에는, 중간에 다른 구성요소가 존재하지 않는 것으로 이해되어야 할 것이다.
본 출원에서 사용한 용어는 단지 특정한 실시 예를 설명하기 위해 사용된 것으로, 본 발명을 한정하려는 의도가 아니다. 단수의 표현은 문맥상 명백하게 다르게 뜻하지 않는 한, 복수의 표현을 포함한다.
본 출원에서, "포함하다" 또는 "구비하다", "가지다" 등의 용어는 명세서상에 기재된 특징, 숫자, 단계, 동작, 구성요소, 부품 또는 이들을 조합한 것이 존재함을 지정하려는 것이지, 하나 또는 그 이상의 다른 특징들이나 숫자, 단계, 동작, 구성요소, 부품 또는 이들을 조합한 것들의 존재 또는 부가 가능성을 미리 배제하지 않는 것으로 이해되어야 한다.
다르게 정의되지 않는 한, 기술적이거나 과학적인 용어를 포함해서 여기서 사용되는 모든 용어들은 본 발명이 속하는 기술 분야에서 통상의 지식을 가진 자에 의해 일반적으로 이해되는 것과 동일한 의미를 가지고 있다.
일반적으로 사용되는 사전에 정의되어 있는 것과 같은 용어들은 관련 기술의 문맥상 가지는 의미와 일치하는 의미를 가지는 것으로 해석되어야 하며, 본 출원에서 명백하게 정의하지 않는 한, 이상적이거나 과도하게 형식적인 의미로 해석되지 않는다.
이하, 본 발명이 속하는 기술분야에서 통상의 지식을 가진 자가 본 발명을 용이하게 실시할 수 있을 정도로 상세히 설명하기 위하여, 본 발명의 가장 바람직한 실시 예를 첨부된 도면을 참조로 하여 상세히 설명하기로 한다.
도 1은 본 발명의 일 실시예에 따른 EKF-CI를 이용한 물체의 방위각 산출 장치의 개략적인 구성을 나타내는 블록도이다.
도 1에 도시된 바와 같이, 본 발명의 일 실시예에 따른 EKF-Cl를 이용한 물체의 방위각 산출 장치(110)는, 전원부(10), GPS 모듈(20), EC 모듈(30), IMU 모듈(40), 제어부(50), 제 1 통신부(60), 제 2 통신부(70), 제 3 통신부(80), 제 4 통신부(90) 및 데이터 저장부(100)를 포함하여 이루어진다.
상기 전원부(10)는 24V 또는 5V 등의 전원을 입력받아 본 실시예에 따른 확장 칼만 필터(EKF-Cl)를 이용한 물체의 방위각 산출 장치(100)에서 필요로 하는 전체적인 전원을 공급한다. 이때 상기 전원부(10)는 전원을 공급하는 구성요소에 따라 전압 레벨(예 : 12V, 5V, 3.3V 등)을 조정하여 공급할 수 있다.
상기 GPS 모듈(20)은 총 3개의 GPS(20-1, 20-2, 20-3)로 구성되며, GPS수신정보를 상기 제어부(50)에 전송한다.
여기서, 도시된 예에서는 총 3개의 GPS로 구성되는 GPS 모듈(20)을 나타내고 있지만, 4개 이상의 GPS도 가능하며, 본 발명에 있어 그 개수를 한정하는 것은 아니다.
상기 EC(Electronic Compass; 전자 나침반) 모듈(30)은 MEMS 기반의 지자기 센서로써 근접한 자석 등에 의한 큰 자기장 뿐 아니라 지구 자기장과 같은 작은 자장도 감지 가능하며 지구 자기장을 기반으로 북쪽을 찾을 수 있다. 이러한 EC 모듈(30)로부터 지자기 데이터를 이용하여 방위각을 알 수 있다. EC 모듈(30)의 지자기 데이터 값으로부터 물체의 방위각 계산은 알려진 기술임으로 부연 설명은 생략하기로 한다.
상기 IMU(Inertial Measurement Unit; 관성측정장치) 모듈(40)은 물체의 가속도를 측정하기 위한 장치로서 MEMS기반의 센서이다. 상기 IMU 모듈(40)은 3축 가속도센서가 내장되어 있어 진행방향, 횡방향, 높이방향의 가속도 측정이 가능하다.
본 발명에 있어서는 물체의 방위각을 계산을 위해 상기 IMU 모듈(40)의 X축 가속도계 성분 및 Y축 가속도계 성분만을 사용한다.
상기 제 1 내지 제 4 통신부(60, 70, 80, 90)는 GPS 모듈(20), EC (센서) 모듈(30), IMU (센서) 모듈(40)와 전기적으로 연결되어 물체의 방위(방향) 정보를 수신할 수 있게 된다.
상기 제 1 내지 제 4 통신부는 각각 RS232, I2C, CAN, USB 통신 방식을 이용하여 통신할 수 있다. 그러나 상기 통신 방식들은 예시적으로 기재한 것일 뿐이며 통신방식을 한정하고자 하는 것은 아니다.
상기 제어부(50)는 마이크로프로세서 또는 이를 포함하는 제어 보드(Control Board) 형태로 구성할 수 있다.
상기 제어부(50)는 상기 GPS 모듈(20), 상기 EC 모듈(30), 및 상기 IMU 모듈(40)로부터 물체의 실시간의 이동 가속도 및 방위(방향)에 대한 정보(또는 센싱 정보)를 수신한다.
상기 제어부(50)는 상기 GPS 모듈(20)로부터 수신된 물체의 실시간 방위각 산출을 위하여 3개의 GSP 수신기에 대한 방위각(FAz, Forward Azimuth)과 EC모듈에 대한 방위각 그리고 IMU 모듈에 대한 X축 및 Y축 가속도계값을 이용한다.
즉, GPS 방위각과 초기 방위각(Ψ; GPSi_yaw)의 수학식 1과 같다.
수학식 1)
Figure 112017049020175-pat00001
여기서, Lat는 latitude의 약자이고, Long은 longitude의 약자이다.
상기 제어부(50)는 또한, 상기 GPS 모듈(20)로부터 수신된 값으로부터 분류 및 가중치를 사용하여 유효한 GPS 데이터 값인지를 평가한다. 즉, 상기 GPS 모듈(20)로부터 수신된 값을 수평 위치 오차(HDOP) 및 수정 품질(fix quality of GPS data)값을 토대로 그 유효성을 판단하며, 그 판단기준은 다음과 같다.
즉, 상기 수평 위치 오차(HDOP) 및 수정 품질(fix quality of GPS data)값을 결합하는 경우, HDOP가 1 내지 2이고, FIX가 2 내지 5인 경우는 이상적(Ideal)이고, HDOP가 1 내지 2이고, FIX가 1인 경우와, HDOP가 3 내지 5이고 FIX가 2 내지 5인 경우는 매우 우수(Excellent)하고, HDOP가 3 내지 5이고 FIX가 1인 경우와, HDOP가 6 내지 10이고 FIX가 2 내지 5인 경우는 우수(Good)하며, 그 외에는 모두 나쁜 것(bad)으로 판단한다.
아울러, 상기 제어부(50)는 상기 GPS 모듈의 방위각과 가중치, EC 모듈의 방위각과 가중치를 이용하여 GPS 및 EC에 대한 방위각을 산출할 수 있으며, 수학식 2와 같다.
수학식 2)
Figure 112017049020175-pat00002
여기서, n은 GPS의 총수, idealValue 는 이상적인 상태에서의 가중치, h fused 는 융합된 GPS 및 EC 헤딩, h EC 는 EC의 방위각, w allGPS 는 조합되어 산출된 GPS 방위각의 전체 가중치, w EC 는 EC의 가중치, h allGPS 는 n개 GPS의 전체 헤딩, w i 는 i 번째 GPS에 할당된 가중치, h i 는 i 번째 GPS의 방위각이다.
한편, 상기 제어부(50)는 3개의 GPS로부터 획득한 GPS 데이터가 유효하지 않거나, 누락된 경우에는 EC에 대한 방위각(h EC )와 가중치(W EC )의 곱을 h fused 로 할당하여 적용할 수 있다.
또한, 상기 제어부(50)는 상기 IMU 모듈(40)로부터 수신된 값으로부터 X-축 가속도계(X-Axis accelerometer) 및 Y-축 가속도계(X-Axis accelerometer)의 값(IMU(Accx, Accy))을 산출한다.
아울러, 상기 제어부(50)는 3개의 저가 GPS 수신기, 단일의 EC 모듈, 및 단일의 IMU 모듈을 사용하여 산출된 GPS 및 EC에 대한 방위각과 IMU(Accx, Accy)로부터 향상된 방위를 산출하기 위해 공분산 교차점(CI)이 적용된 확장 칼만 필터(EKF)를 사용하여 최종 융합 방위각(psi_CI)을 계산하여 물체의 방위각을 정확하게 산출할 수 있게 된다.
즉, 3개의 GPS 수신기와 단일의 EC 모듈을 융합한 상태에서 다시 단일의 IMU 모듈의 가속도계값을 융합하고 확장 칼만 필터(EKF, Extended Kalman Filter)를 수행하되, EKF 입력은 lati, longi, GPSi_yaw와 국부적인 가속도계 측정값인 IMU의 국부 중력 벡터가 결합된 공분산 행렬(PCI)이고, EKF 출력은 lati_EKF, longi_EKF, psii_EKF 및 공분산 행렬 P_EKF가 되며, 상기 EKF 출력을 입력으로 하여 CI를 수행하여 출력값이 lat_CI, long_CI, psi_CI 및 공분산 행렬(PCI)로 되도록 반복 수행함으로써, 최종 융합 헤딩 값은 psi_CI 로 산출되어진다.
물론, 이동중인 물체(농기계)에 대해 새로 검출된 GPS, EC, 그리고 IMU 값에 대해서는 상술한 계산을 반복하여 수행한다.
상기 데이터 저장부(100)에는 GPS 모듈(20), EC 모듈(30), 및 IMU 모듈(40)로 부터 수신되는 모든 데이터와 상기 제어부(50)에서 산출되는 GPS 방위삭, EC 방위각, GPS-EC 방위각, 및 최종 융합 방위각 등이 실시간으로 저장된다.
도 2는 본 발명의 일 실시예에 따른 공분산 교차점이 적용된 확장 칼만 필터 (EKF-CI)를 이용한 물체의 실시간 방위각 산출 방법을 나타내는 순서도이고, 도 3은 본 발명의 일 실시예에 따른 공분산 교차점이 적용된 확장 칼만 필터 (EKF-CI)를 이용한 물체의 최종 융합 방위각을 유도하는 순서도로서, 도 1을 함께 참조하여 설명하기로 한다.
도시된 바와 같이, 본 발명의 일 실시예에 따른 EKF-Cl를 이용한 물체의 실시간 방위각 산출 방법은, 제어부(50)에서 GPS 모듈(20)로부터 수신된 물체의 GPS 데이터로부터 GPS의 전방 방위각을 산출하는 단계(S10); 상기 제어부(50)에서 수평 위치 오차(HDOP) 및 수정 품질(FIX)값을 토대로 GPS 데이터의 유효성을 평가하는 단계(S20); 상기 제어부(50)에서 EC(Electronic Compass) 모듈(30)로부터 수신된 지자기 데이터를 통해 물체의 EC 방위각을 산출하는 단계(S30); 상기 제어부(50)에서 상기 GPS 모듈의 방위각과 가중치, EC 모듈의 방위각과 가중치로부터 물체의 GPS 및 EC에 대한 방위각을 산출하는 단계(S40); 상기 제어부(50)에서 IMU 모듈(40)로부터 수신된 값으로부터 물체의 (X축 및 Y축)가속도계값을 산출하는 단계(S50); 상기 제어부(50)에서 상기 GPS 및 EC에 대한 방위각과 상기 IMU 모듈로부터 수신된 가속도계값을 토대로 공분산 교차점(CI)이 적용된 확장 칼만 필터(EKF)를 사용하여 실시간 최종 융합 방위각(psi_CI)을 산출하는 단계(S60); 및 상기 제어부(50)에서 검출된 GPS, EC, 그리고 IMU 모듈 수신값에 대해서 상술한 단계(S10∼S60)을 반복하는 단계(70)를 포함하여 이루어진다.
상기 확장 칼만 필터는 널려 알려진 알고리즘으로서 추가적인 부연 설명은 생략하기로 한다.
상기 제어부(50)에서 GPS 모듈(20)로부터 수신된 물체의 실시간의 GPS 데이터로부터 전방 방위각(FAz, Forward Azimuth)을 사용하여 GPS 방위각을 산출하는 단계(S10)에서는, 상기 제어부(50)가 다수의 GPS 수신기에 대한 초기 방위각을 상기 수학식 1에 따라 산출한다.
또한, 상기 제어부(50)에서 GPS 데이터의 유효성을 평가하는 단계(S20)에서는, 상기 제어부가 상기 GPS 모듈로부터 수신된 값에 대해 수평 위치 오차(HDOP, horizontal dilution of precision) 및 수정 품질(fix quality of GPS data) 값을 토대로 그 유효성을 판단하고, 유효한 GPS 데이터에 대해서는 가중치(weight)를 부여하도록 한다.
또한, 상기 HDOP가 1 내지 2이고, FIX가 2 내지 5인 경우는 이상적(Ideal)인 것으로 판단하고, 상기 HDOP가 1 내지 2이고, FIX가 1인 경우와, HDOP가 3 내지 5이고 FIX가 2 내지 5인 경우는 매우 우수(Excellent)한 것으로 판단하고, HDOP가 3 내지 5이고 FIX가 1인 경우와, HDOP가 6 내지 10이고 FIX가 2 내지 5인 경우는 우수(Good)한 것으로 판단하고, 그 외에는 모두 나쁜 것(bad)으로 판단하여 배제시키는 것이 바람직하다.
또한, 상기 GPS 및 EC에 대한 방위각을 산출하는 단계에서는, 상기 제어부가 산출된 GPS의 방위각 및 초기 방위각, 상기 가중치, 및 상기 EC의 방위각으로부터 GPS 및 EC에 대한 방위각이 상기 수학식 2에 의해 산출됨이 바람직하다.
또한, 상기 제어부는 상기 GPS 모듈로부터 수신한 GPS 데이터가 유효하지 않는 경우, EC에 대한 방위각만을 적용함이 바람직하다.
상기 제어부(50)에서 EC(Electronic Compass) 모듈(30)로부터 수신된 지자기 데이터를 통해 물체의 실시간 EC 방위각을 산출하는 단계(S30)에서는, 상기 제어부(50)가 상기 EC(Electronic Compass) 모듈(30)로부터 수신된 지자기 데이터 값으로부터 EC 방위각을 산출하게 된다.
또한, 상기 가속도계값을 산출하는 단계에서는, 상기 제어부가 상기 IMU 모듈의 수신값으로부터 X-축 가속도계(X-Axis accelerometer) 및 Y-축 가속도계(X-Axis accelerometer)의 값(IMU(Accx, Accy))을 산출함이 바람직하다.
상기 제어부(50)에서 상기 GPS의 방위각, 가중치, 및 EC 방위각으로부터 물체의 실시간의 GPS 및 EC에 대한 방위각을 산출하는 단계(S40)에서는, 상기 제어부(50)가 산출된 GPS의 방위각 및 초기 방위각, 상기 가중치, 및 상기 EC의 방위각으로부터 GPS 및 EC에 대한 방위각을 상기 수학식 2에 의해 산출하게 된다.
여기서, 상기 제어부(50)는 3개의 GPS로부터 획득한 GPS 데이터가 유효하지 않거나, 누락된 경우에는 EC에 대한 방위각만으로 h fused 를 "GPSEC _ yaw" 로 할당하여 적용될 수 있다.
상기 제어부(50)에서 IMU 모듈(40)로부터 수신된 값으로부터 물체의 실시간의 가속도계값을 산출하는 단계(S50)에서는, 상기 제어부(50)가 상기 IMU 모듈(40)로부터 수신된 값으로부터 X-축 가속도계(X-Axis accelerometer) 및 Y-축 가속도계(X-Axis accelerometer)의 값(Accx, Accy)을 산출하게 된다.
상기 제어부(50)에서 상기 GPS 모듈 및 상기 EC 모듈로부터 수신된 GPS 데이터 및 지자기 데이터를 통해 산출된 GPS 및 EC에 대한 방위각과 상기 IMU 모듈로부터 수신된 가속도계값을 토대로 공분산 교차점(CI)이 적용된 확장 칼만 필터(EKF-CI)를 사용하여 최종 융합 방위각(psi_CI)을 산출하는 단계(S60)에서는, 제어부(50)가 저비용의 3개의 GPS, 단일의 EC, 및 단일의 IMU를 사용하여 산출된 GPS 및 EC에 대한 방위각과 IMU(Accx, Accy)로부터 향상된 방위를 산출하기 위해 공분산 교차점(CI)이 적용된 확장 칼만 필터(EKF)를 사용하여 최종 융합 방위각(psi_CI)을 계산하여 이동중인 물체의 방위각을 정확하게 산출할 수 있게 된다.
즉, 3개의 GPS와 단일의 EC를 융합한 상태에서 다시 단일의 IMU의 가속도계값을 융합하고 확장 칼만 필터(EKF;Extended Kalman Filter)를 수행하되, EKF 입력은 lati, longi, GPSi_yaw(GPS가 유효하지 않는 경우 상술한 GPSEC _yaw 적용)와 국부적인 가속도계 측정값인 IMU의 국부 중력 벡터가 결합된 공분산 행렬(PCI)이고, EKF 출력은 lati_EKF, longi_EKF, psii_EKF 및 공분산 행렬 P_EKF가 되며, 상기 EKF 출력을 입력으로 하여 CI를 수행하여 출력값이 lat_CI, long_CI, psi_CI 및 공분산 행렬(PCI)로 되도록 반복 수행함으로써, 최종 융합 헤딩 값은 psi_CI 로 산출되어진다.
또한, 이동중인 물체(농기계)에 대해 지속적으로 GPS, EC, 그리고 IMU 모듈의 갱신된 값을 상술한 단계(S10∼S60)를 반복계산(S70)하도록 하여 물체의 방위각을 실시간으로 산출한다.
본 발명은 상기의 상세한 설명에서 언급되는 형태로만 한정되는 것은 아님을 잘 이해할 수 있을 것이다. 따라서 본 발명의 진정한 기술적 보호 범위는 첨부된 특허청구범위의 기술적 사상에 의해 정해져야 할 것이다. 또한, 본 발명은 첨부된 청구범위에 의해 정의되는 본 발명의 정신과 그 범위 내에 있는 모든 변형물과 균등물 및 대체물을 포함하는 것으로 이해되어야 한다.
110: 확장 칼만 필터(EKF-Cl)를 이용한 물체의 방위각 산출 장치
10: 전원부
20: GPS 모듈
30: EC 모듈
40: IMU 모듈
50: 제어부
60: 제 1 통신부
70: 제 2 통신부
80: 제 3 통신부
90: 제 4 통신부
100: 데이터 저장부

Claims (9)

  1. 제어부에서 GPS 모듈로부터 수신된 물체의 실시간의 GPS 데이터로부터 GPS의 전방 방위각을 사용하여 GPS 방위각을 산출하는 단계;
    상기 제어부에서 수평 위치 오차(HDOP, Horizontal Dilution Of Precision) 및 수정 품질(FIX, fix quality of GPS data)값을 토대로 GPS 데이터의 유효성을 평가하는 단계;
    상기 제어부에서 EC(Electronic Compass) 모듈로부터 수신된 지자기 데이터를 통해 물체의 실시간 EC 방위각을 산출하는 단계;
    상기 제어부에서 상기 GPS의 방위각 및 EC 방위각으로부터 물체의 실시간의 GPS 및 EC에 대한 방위각을 산출하는 단계;
    상기 제어부에서 IMU 모듈로부터 수신된 값으로부터 물체의 실시간의 가속도계값을 산출하는 단계;
    상기 제어부에서 상기 GPS 및 EC 모듈에 대한 방위각과 상기 IMU 모듈로부터 수신된 가속도계값을 토대로 공분산 교차점(CI)이 적용된 확장 칼만 필터(EKF)를 사용하여 최종 융합 방위각(psi_CI)을 산출하는 단계; 및
    상기 제어부에서 새로이 검출되는 상기 GPS 모듈, 상기 EC 모듈, 및 상기 IMU 모듈로부터의 수신값에 대해서 상술 단계들을 반복하는 단계를 포함하여 이루어지는 것을 특징으로 하며,
    상기 GPS 데이터의 유효성을 평가하는 단계에서는, 상기 제어부가 상기 GPS 모듈로부터 수신된 값에 대해 수평 위치 오차(HDOP) 및 수정 품질(fix quality of GPS data)값을 토대로 그 유효성을 판단하고, 유효한 GPS 데이터에 대해서는 가중치(weight)를 부여하는 것을 특징으로 하고,
    상기 GPS 및 EC에 대한 방위각을 산출하는 단계에서는, 상기 제어부가 산출된 GPS의 방위각 및 초기 방위각, 상기 가중치, 및 상기 EC의 방위각으로부터 GPS 및 EC에 대한 방위각을 하기 수학식 2에 의해 산출되는 것을 특징으로 하는 공분산 교차점이 적용된 확장 칼만 필터(EKF-Cl)를 이용한 물체의 실시간 방위각 산출 방법.
    수학식 2)
    Figure 112018132327337-pat00008

    여기서, n은 GPS의 총수, idealValue 는 이상적인 상태에서의 가중치, hfused 는 융합된 GPS 및 EC 헤딩, hEC 는 EC의 방위각, wallGPS 는 조합되어 산출된 GPS 방위각의 전체 가중치, wEC 는 EC의 가중치, hallGPS 는 n개 GPS의 전체 헤딩, wi - i 번째 GPS에 할당된 가중치, hi 는 i 번째 GPS의 계산된 방위각이다.
  2. 제 1 항에 있어서,
    상기 전방 방위각을 사용하여 GPS 방위각을 산출하는 단계에서는, 다수의 GPS에 대한 GPS 방위각과 초기 헤딩 값(heading value)을 Faz(Forward Azimuth)를 이용하여 하기 수학식 1에 따라 산출하는 것을 특징으로 하는 공분산 교차점이 적용된 확장 칼만 필터(EKF-Cl)를 이용한 물체의 실시간 방위각 산출 방법.
    수학식 1)
    Figure 112019501879677-pat00003

    여기서, Lat는 latitude의 약자이고, Long은 longitude의 약자이다.
  3. 삭제
  4. 제 1 항에 있어서,
    상기 HDOP가 1 내지 2이고, FIX가 2 내지 5인 경우는 이상적(Ideal)인 것으로 판단하고, 상기 HDOP가 1 내지 2이고, FIX가 1인 경우와, HDOP가 3 내지 5이고 FIX가 2 내지 5인 경우는 매우 우수(Excellent)한 것으로 판단하고, HDOP가 3 내지 5이고 FIX가 1인 경우와, HDOP가 6 내지 10이고 FIX가 2 내지 5인 경우는 우수(Good)한 것으로 판단하고, 그 외에는 모두 나쁜 것(bad)으로 판단하여 배제시키는 것을 특징으로 하는 공분산 교차점이 적용된 확장 칼만 필터(EKF-Cl)를 이용한 물체의 실시간 방위각 산출 방법.
  5. 삭제
  6. 제 1 항에 있어서,
    상기 제어부는 상기 GPS 모듈로부터 수신한 GPS 데이터가 유효하지 않는 경우, EC에 대한 방위각만을 적용하는 것을 특징으로 하는 공분산 교차점이 적용된 확장 칼만 필터(EKF-Cl)를 이용한 물체의 실시간 방위각 산출 방법.
  7. 제 1 항에 있어서,
    상기 가속도계값을 산출하는 단계에서는, 상기 제어부가 상기 IMU 모듈로부터 수신된 값으로부터 X-축 가속도계(X-Axis accelerometer) 및 Y-축 가속도계(X-Axis accelerometer)의 값(Accx, Accy)을 산출하는 것을 특징으로 하는 공분산 교차점이 적용된 확장 칼만 필터(EKF-Cl)를 이용한 물체의 실시간 방위각 산출 방법.
  8. 제 1 항에 있어서,
    상기 최종 융합 방위각(psi_CI)을 산출하는 단계에서는, 상기 제어부가 다수의 GPS, 단일의 EC, 및 단일의 IMU를 사용하여 산출된 상기 GPS 및 EC에 대한 방위각과 상기 가속도계값으로부터 향상된 방위를 산출하기 위해 공분산 교차점(CI)이 적용된 확장 칼만 필터(EKF)를 사용하여 최종 융합 방위각 (psi_CI)을 계산하는 것을 특징으로 하는 공분산 교차점이 적용된 확장 칼만 필터(EKF-Cl)를 이용한 물체의 실시간 방위각 산출 방법.
  9. 제 8 항에 있어서,
    다수의 GPS 및 단일의 EC에 대한 방위각으로 융합한 상태에서 다시 단일의 IMU의 가속도계값을 융합하고 확장 칼만 필터(EKF;Extended Kalman Filter)를 수행하되, EKF 입력은 lati, longi, GPSi_yaw와 국부적인 가속도계 측정값인 IMU의 국부 중력 벡터가 결합된 공분산 행렬(PCI)이고, EKF 출력은 lati_EKF, longi_EKF, psii_EKF 및 공분산 행렬 P_EKF가 되며, 상기 EKF 출력을 입력으로 하여 CI를 수행하여 출력값이 lat_CI, long_CI, psi_CI 및 공분산 행렬(PCI)로 되도록 반복 수행함으로써, 최종 융합 헤딩 값은 psi_CI 로 산출하는 것을 특징으로 하는 공분산 교차점이 적용된 확장 칼만 필터(EKF-Cl)를 이용한 물체의 실시간 방위각 산출 방법.
KR1020170063350A 2017-05-23 2017-05-23 확장 칼만 필터를 이용한 물체의 실시간 방위각 산출 방법 KR101960164B1 (ko)

Priority Applications (1)

Application Number Priority Date Filing Date Title
KR1020170063350A KR101960164B1 (ko) 2017-05-23 2017-05-23 확장 칼만 필터를 이용한 물체의 실시간 방위각 산출 방법

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
KR1020170063350A KR101960164B1 (ko) 2017-05-23 2017-05-23 확장 칼만 필터를 이용한 물체의 실시간 방위각 산출 방법

Publications (2)

Publication Number Publication Date
KR20180128173A KR20180128173A (ko) 2018-12-03
KR101960164B1 true KR101960164B1 (ko) 2019-07-15

Family

ID=64743330

Family Applications (1)

Application Number Title Priority Date Filing Date
KR1020170063350A KR101960164B1 (ko) 2017-05-23 2017-05-23 확장 칼만 필터를 이용한 물체의 실시간 방위각 산출 방법

Country Status (1)

Country Link
KR (1) KR101960164B1 (ko)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR102206630B1 (ko) * 2020-04-01 2021-01-22 한국남동발전 주식회사 실시간 자세 측정 시스템

Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2006126148A (ja) * 2004-11-01 2006-05-18 Tokimec Inc 方位姿勢検出装置

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR100414439B1 (ko) 2001-10-08 2004-01-13 장경회 선박용 자동운항 장치
KR20160087249A (ko) * 2015-01-13 2016-07-21 삼영이엔씨 (주) Ｇｐｓ 수신기가 내장된 전자 컴퍼스
KR20150122097A (ko) * 2015-10-08 2015-10-30 김인태 항법 알고리즘을 이용한 네비게이션 연동방법

Patent Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2006126148A (ja) * 2004-11-01 2006-05-18 Tokimec Inc 方位姿勢検出装置

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
임신택 외 3명. 선박의 자율항해에 필요한 GPS와 IMU의 데이터를 이용한 Compass 연구. CICS 2015 정보 및 제어 학술대회 논문집. 2015.10. pp.274-275.*

Also Published As

Publication number Publication date
KR20180128173A (ko) 2018-12-03

Similar Documents

Publication Publication Date Title
CN104503466B (zh) 一种微小型无人机导航装置
Kinsey et al. A survey of underwater vehicle navigation: Recent advances and new challenges
KR101454153B1 (ko) 가상차선과 센서 융합을 통한 무인 자율주행 자동차의 항법시스템
CN101382431B (zh) 定位***及其方法
EP3073223B1 (en) Navigation system with star tracking sensors
CN110779521A (zh) 一种多源融合的高精度定位方法与装置
CN103744098B (zh) 基于sins/dvl/gps的auv组合导航***
CN105468006A (zh) 用于自动的着陆***的位置数据的冗余测定
CN104729506A (zh) 一种视觉信息辅助的无人机自主导航定位方法
CN107907900A (zh) 一种gnss双天线辅助的多传感器组合导航***及方法
CN103900609B (zh) 一种船用惯性导航***的航向精度实时检测***及检测方法
GB2458357A (en) Underwater navigation system for measuring position and velocity whilst surveying underwater terrain
CN110779496B (zh) 三维地图构建***、方法、设备和存储介质
US20170363749A1 (en) Attitude angle calculating device, method of calculating attitude angle, and attitude angle calculating program
CN107643088A (zh) 无人机导航方法、装置、无人机及存储介质
CN113670334B (zh) 一种飞行汽车的初始对准方法和装置
CN105242682A (zh) 靶机目标特性测量***
GB2444814A (en) Estimating inertial acceleration bias errors
CN111679680A (zh) 一种无人机自主着舰方法及***
Bäumker et al. Enhancements in UAV flight control and sensor orientation
CN110209188A (zh) 一种控制无人机飞行的方法及***、无人机
KR20160038319A (ko) 차량의 위치 표시 방법
CN105910623B (zh) 利用磁强计辅助gnss/mins紧组合***进行航向校正的方法
WO2002014890A2 (en) Attitude determination using a global positioning system
KR102209422B1 (ko) Rtk gnss 기반 운전면허시험 차량의 포지션 결정 장치

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
GRNT Written decision to grant