KR100936710B1 - 주행기록계와 방향센서를 이용한 이동체의 위치 결정 방법,시스템 및 그 방법을 기록한 기록매체 - Google Patents

주행기록계와 방향센서를 이용한 이동체의 위치 결정 방법,시스템 및 그 방법을 기록한 기록매체 Download PDF

Info

Publication number
KR100936710B1
KR100936710B1 KR1020070026154A KR20070026154A KR100936710B1 KR 100936710 B1 KR100936710 B1 KR 100936710B1 KR 1020070026154 A KR1020070026154 A KR 1020070026154A KR 20070026154 A KR20070026154 A KR 20070026154A KR 100936710 B1 KR100936710 B1 KR 100936710B1
Authority
KR
South Korea
Prior art keywords
axis
sensor
following equation
odometer
moving
Prior art date
Application number
KR1020070026154A
Other languages
English (en)
Other versions
KR20080084435A (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 KR1020070026154A priority Critical patent/KR100936710B1/ko
Publication of KR20080084435A publication Critical patent/KR20080084435A/ko
Application granted granted Critical
Publication of KR100936710B1 publication Critical patent/KR100936710B1/ko

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
    • G01C22/02—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers by conversion into electric waveforms and subsequent integration, e.g. using tachometer generator
    • G01C22/025—Differential odometers
    • 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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • 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
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
    • G01S5/0294—Trajectory determination or predictive filtering, e.g. target tracking or Kalman filtering
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0268—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means
    • G05D1/027—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means comprising intertial navigation means, e.g. azimuth detector

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Navigation (AREA)

Abstract

본 발명의 주행기록계와 방향센서를 이용한 이동체의 위치 결정 시스템은, 주행기록계에서 측정한 절대 위치 변이량에 cos(θ(t))를 곱하여 X축으로 이동한 거리를 산출하는 X축 변환부와, 주행기록계에서 측정한 절대 위치 변이량에 sin(θ(t))를 곱하여 Y축으로 이동한 거리를 산출하는 Y축 변환부와, 방향센서로부터 기준 방향 정보를 이용하고 방향 변이값을 관측하는 방향 변이 센서와, X축 변환부, Y축 변환부 및 방향 변이 센서로부터 각각 X축으로 이동한 거리, Y축으로 이동한 거리 및 방향 변이값을 수신하여 이동체의 현재 위치를 산출하는 칼만필터로 구성되므로, 가속도계와 외부 관측 센서의 도움없이 이동체의 위치를 결정할 수 있으므로 고주파 잡음을 거의 가지지 않게 되며 이러한 이유로 저속도 이동체의 경우에 그 위치를 파악하는 데에 매우 유용하게 이용될 수 있는 효과가 있다.
위치 결정(position estimation), 주행기록계(odometer), 자이로(gyroscope), 자기 센서(magnetic compass), 칼만필터(Kalman filter)

Description

주행기록계와 방향센서를 이용한 이동체의 위치 결정 방법, 시스템 및 그 방법을 기록한 기록매체{Position estimation Method, System and recorded media of mobile system using odometer and heading sensor}
도 1은 본 발명의 일실시예에 따른 이동체의 위치를 계산하기 위하여 이동체를 좌표상에 표시한 좌표도.
도 2는 본 발명의 일실시예에 따라 주행기록계, 방향 센서 및 방향 변이 센서를 결합하여 나타낸 블록도.
도 3은 본 발명의 일실시예에 따른 주행기록계, 방향센서, 방향 변이 센서의 측정값을 확장칼만필터에 적용하여 이동체의 위치를 추적하고 갱신하는 과정을 나타낸 순서도.
<도면의 주요부분에 대한 부호의 간단한 설명>
2 : 이동체 8 : 증폭회로
10 : 방향 변이 센서 12 : 입력값 생성부
20 : 방향 센서 30 : 주행기록계
40 : 칼만필터
본 발명은 주행기록계와 방향센서를 이용한 이동체의 위치 결정 방법 및 시스템에 관한 것으로, 더욱 상세하게는 추측 항법(Dead reckoning system)에 있어서, 속도에 의존하지 않은 새로운 종류의 저속도 이동체용 추측 항법 시스템의 개념을 제시하고자 하며, 이 개념을 채용하여 주행기록계, 방향 변이 센서, 방향 센서의 결합을 통하여 저속도용 이동체의 추측 항법을 계발한 주행기록계와 방향센서를 이용한 이동체의 위치 결정 장치 및 시스템에 관한 것이다.
본 발명의 기술분야는 추측 항법 시스템(Dead reckoning system)이라 할 수 있다. 추측 항법은 이동하는 물체의 속도(speed), 이동 시간, 방향 등의 알려진 정보를 이용하여 기준 좌표에 대한 물체의 위치를 결정하는 과정이다. 통상적으로 추측 항법은 해양 선박이나, 자동차, 항공기, 우주선의 항법용으로 이용되지만, 로봇 시스템의 위치 결정에도 보조적으로 이용되고 있다. 간단한 예로 물체가 직선으로 Δt시간 동안 속도 v로 이동하였다면, 이동 거리는 Δt*v 가 되고, 이를 통하여 위치를 추측할 수 있다. 이와 같은 간단한 예에서 보듯이 통상적으로 물체의 위치를 계산하기 위해서는 물체의 속도가 중요한 관측 정보가 된다. 물체의 속도 v는 가속도에 의하여 다음과 같이 v = v0 +aΔt(v0 : 초기 속도, a: 가속도)로 표현되기 때문에 속도를 알기 위해서는 가속도를 관측하는 게 필수적이다. 참고적으로 가속도 를 관측하기 위한 장비인 가속도계(accelerometer)를 이용한 위치 결정 시스템을 좁은 의미에서 추측 항법이라 칭하기도 한다. 따라서, 추측 항법의 성공은 가속도나 방향 가속도를 측정하는 정밀한 관성항법장치의 개발이라 할 수 있다. 하지만 가속도 센서는 일반적으로 많은 고주파 잡음과 흔들림 등의 외부 요소에 영향을 받기 때문에 저속도 이동체의 속도 계산에 이용하기 쉽지 않으며, 이용한다 하더라도 많은 오차 요소를 내포하는 문제점이 있다.
이와 같은 문제점을 해결하기 위하여 이동체의 위치 추적을 위한 일반적인 방법은 자이로(gyroscope)와 가속도계(accelerometer)를 동시에 이용하는 관성항법장치(inertial measurement unit; IMU)와 GPS같은 외부의 위치 정보 장치를 결합하여 2차원 평면에서 이동체의 위치를 칼만필터(kalman filter)로 추적하는 것이다. 이와 같은 방법은 가속도계에서 발생하는 오류를 보완하는 것이다. 그러나, 주지하는 바와 같이 잡음을 완전하게 없애는 것은 거의 불가능하다.
이와 같은 종래 기술의 예로는 The Penn State Reserach Foundation이 출원한 미국특허 제US5902351호로 그 제목이 "Apparatus and method for tracking a vehicle"에서 개시하는 바와 같이 가속도계를 포함하는 관성항법장치와 센서들을 결합하여 차량 추적시에 발생하는 오류를 감소시켜 그 가속도계에서 발생하는 오류를 최소화시킬 수 있다.
그러나, 전술한 종래기술에 있어서도 여전히 가속도계를 사용하므로, 잡음이 발생하며 또한, 이로 인하여 많은 오차 요소를 여전히 내포하게 되므로 여전히 오차가 발생하게 된다. 또한, 앞에 언급된 종래 기술은 고속으로 운행하는 차량 등에 적용한 것이어서 다수의 추적장치를 사용하므로 그 오차범위는 줄어들었지만, 상호 간섭 등에 의해 여전히 에러가 발생할 수 있는 가능성은 더 증가하게 된 문제점이 있다.
종래의 기술에 있어서는 외부의 관측 정보가 필요하기 때문에 외부의 관측이 불가능한 경우 신뢰성 있는 위치 인식을 위한 센서 결합 시스템을 구축할 수 없는 문제점이 있다.
종래의 기술에 있어서는 가속도계나 외부 관측 장비는 기반 시설이 필요하며 고가(high cost)이기 때문에 실용화 및 상업화하기 쉽지 않으며 실용화 및 상업화한다고 하여도 고가의 장비가 되어 소비자에게 부담을 주는 등의 문제점이 있다.
본 발명은 전술한 종래기술의 문제점을 해결하기 위하여 제안된 것으로서, 그 목적은 가속도계를 사용하는 기존의 방법이 가지는 단점을 극복하고 위치 추적 성능을 향상시키기 위한 것으로 가속도계를 사용하지 않고 이동체의 위치를 추적하는 주행기록계와 방향센서를 이용한 이동체의 위치 결정 방법 및 시스템을 제공하는 데에 있는 것이다.
또한, 본 발명의 다른 목적은 잡음이 많은 가속도계를 이용하지 않고 외부의 관측 정보 없이 이동체의 위치를 실시간 추적하는 새로운 방법을 제시함으로써 외부의 도움없이 이동체의 위치 추적 성능을 향상시킨 주행기록계와 방향센서를 이용한 이동체의 위치 결정 방법 및 시스템을 제공하는 데에 있는 것이다.
또한, 본 발명의 다른 목적은 가속도계를 사용하지 않고 주행기록계와 방향센서만을 이용하여 위치를 추적할 수 있으므로 저렴한 비용으로 위치 추적장치를 구현할 수 있는 주행기록계와 방향센서를 이용한 이동체의 위치 결정 방법 및 시스템을 제공하는 데에 있는 것이다.
본 발명의 주행기록계와 방향센서를 이용한 이동체의 위치 결정 시스템은, 주행기록계에서 측정한 절대 위치 변이량에 cos(θ(t))를 곱하여 X축으로 이동한 거리를 산출하는 X축 변환부와, 주행기록계에서 측정한 절대 위치 변이량에 sin(θ(t))를 곱하여 Y축으로 이동한 거리를 산출하는 Y축 변환부와, 방향센서로부터 기준 방향 정보를 이용하고 방향 변이값을 관측하는 방향 변이 센서와, X축 변환부, Y축 변환부 및 방향 변이 센서로부터 각각 X축으로 이동한 거리, Y축으로 이동한 거리 및 방향 변이값을 수신하여 이동체의 현재 위치를 산출하는 칼만필터로 구성되는 것을 특징으로 한다.
또한, 본 발명의 주행기록계와 방향센서를 이용한 이동체의 위치 결정 방법은, 이동체(2)는 주행기록계로부터 출력된 Δl(t)를 이용하여 X축 및 Y축으로 이동한 거리를 측정하는 이동거리 측정단계와, 방향 변이 센서는 방향 변이값을 측정하는 방향 변이값 측정단계와, 측정한 이동거리 및 방향 변이값을 이용하여 이동체(2)의 위치를 예측하는 예측단계와, 칼만필터를 이용하여 각각 상태 벡터와 공분산 행렬을 구하고 칼만게인(K)을 이용하여 상태 벡터와 공분산 행렬을 갱신하는 갱신단계로 이루어진 것을 특징한다.
또한, 본 발명의 주행기록계와 방향센서를 이용한 이동체의 위치 결정 방법을 기록한 기록매체는 이동체는 주행기록계로부터 출력된 Δl(t)를 이용하여 X축 및 Y축으로 이동한 거리를 측정하는 이동거리 측정단계와, 방향 변이 센서는 방향 변이값(Δθ(t))을 측정하는 방향 변이값 측정단계와, 측정한 이동거리 및 방향 변이값을 이용하여 이동체(2)의 위치를 예측하는 예측단계와, 칼만필터를 이용하여 각각 상태 벡터와 공분산 행렬을 구하고 칼만게인(K)을 이용하여 상태 벡터와 공분산 행렬을 갱신하는 갱신단계로 구성된 주행기록계와 방향센서를 이용한 이동체의 위치 결정 방법을 수행할 수 있는 것을 특징으로 한다.
이하, 첨부된 도면을 참조하여 본 발명의 구성에 대하여 보다 상세하게 설명하기로 한다.
도 1은 본 발명의 일실시예에 따른 이동체의 위치를 계산하기 위하여 이동체를 좌표상에 표시한 좌표도이며, 도 2는 본 발명의 일실시예에 따라 주행기록계, 방향 센서 및 방향 변이 센서를 결합하여 나타낸 블록도이다.
도 1 및 도 2를 참조하여 보면, 잡음이 많은 가속도계를 이용하지 않고 외부의 관측 정보 없이 이동체의 위치를 실시간 추적하는 새로운 방법을 제시함으로써 이동체(2)의 위치 추적 성능을 향상시켰다. 이를 위해서 본 발명은 방향 변이 센서(10)와 자기 센서와 같은 방향 센서(20) 및 주행기록계(30)의 관측 정보를 칼만필터(40)를 통하여 결합한다. 본 발명의 장점은 가속도계를 이용하지 않기 때문에 관측에 신뢰성이 높고, 저가이며, 외부의 관측 센서가 필요 없기 때문에 이동체(2) 의 자율주행 능력이 획기적으로 향상된다는 점이다.
먼저, 도 1에서 보면, 이동체(2)의 위치를 2차원 평면에서 x, y로 나타낸다. 여기서 x는 기준 좌표계에서 이동체가 X-축을 기준으로 한 위치이고 y는 Y-축을 기준으로 한 위치이다. 이동체(2)의 이동 방향을 θ로 나타낸다. 이동체(2)는 2차원 평면에 좌표 및 방향으로 나타내어 진다.
이동체(2)의 이동을 기술하기 위해서는 운동 방정식을 도출해야 하는데, 본 발명에서의 운동 방정식은 속력(velocity)를 고려하지 않고 설계한다. 즉, 기존의 방법은 속도와 위치를 동시에 상태변수로 이용하기 때문에 속도의 변화량을 파악하기 위해서 가속도계를 이용하지만, 본 발명은 속도를 무시하고 순수하게 위치와 방향각(θ)만을 상태변수로 고려하여 이동체(2)의 위치를 추적한다.
먼저, 시각 t+1에 위치(l)는 다음과 같이 계산되기 때문에 l(t+1) = l(t) + Δl(t), X-Y 2차원 좌표계에서는 다음과 같이 상태변수를 전달(propagation)하는 운동식은 도 1의 도식에 따라서, 다음의 수학식 1 내지 수학식 3을 유도할 수 있다. 수학식 1 내지 수학식 3은 다음과 같다.
x(t+1)=x(t)+cos(θ(t))Δl(t)
y(t+1)=y(t)+sin(θ(t))Δl(t)
θ(t+1)=θ(t)+Δθ(t)
위의 식에서 Δθ(t)는 자이로(gyroscope)와 같은 방향 변이 센서(10)로부터 관측된 방향 변이값이고, Δl(t)는 주행기록계(30)에서 관측된 절대 위치 변이량이다. 칼만필터(40)를 설계하기 위해서는 기준 관측값(reference measurement)이 필요하다. 즉, θ(t) 및 θ(t+1)의 함수를 만들기 위해서는 방향 센서(20)에서 관측된 기준 관측값과 방향 변이 센서(10)로부터 관측된 방향 변이값(Δθ(t))이 모두 필요하다. 본 발명에서는 자기 센서와 같은 방향 센서(20)를 이용한다. 즉, 기준 관측식은 다음의 수학식 4와 같이 간단하게 결정된다. 수학식 4는 다음과 같다.
Figure 112007021434117-pat00001
여기서 w는 관측시 섞여있는 관측 잡음을 나타내며
Figure 112007021434117-pat00002
(t)는 실제 관측값이다. 위의 수학식 1, 수학식 2 및 수학식 3에서 보이듯이 비록 식은 간단하다 하더라도 상태식이 비선형이기 때문에 확장칼만필터(extended Kalman filter)를 이용하여야 한다. 수학식 1, 수학식 2 및 수학식 3에서 Δθ(t)와 Δl(t)가 실제값이고 이들 실제값은 관측이 불가능하기 때문에 이들 식을 직접적으로 이용할 수 없다. 이들 변이량의 관측값을 각각
Figure 112007021434117-pat00003
,
Figure 112007021434117-pat00004
로 나타내면, 관측값은 실제값에 잡음이 포함된 것이기 때문에 각각 다음의 수학식 5와 같이 나타낸다. 수학식 5는 다음과 같다.
Figure 112007021434117-pat00005
여기서 각각의 nl, nθ는 각각 주행기록계(30)와 방향 변이 센서(10)에서 의 관측된 관측 잡음이다. 이제 수학식 5를 수학식 1, 수학식 2 및 수학식 3에 대입하면 잡음이 포함된 상태식(state equations)인 수학식 6, 수학식 7 및 수학식 8을 얻을 수 있다. 다음의 수학식 6, 수학식 7 및 수학식 8은 다음과 같다.
Figure 112007021434117-pat00006
Figure 112007021434117-pat00007
Figure 112007021434117-pat00008
칼만필터(40)에서는 잡음의 공분산 행렬(covariance matrix)이 필요한데, 본 발명에서는 |cos(θ)| < 1, |sin(θ)| < 1 이기 때문에 다음의 수학식 9와 같이 상태잡음 공분산 행렬(Q)을 구할 수 있다. 수학식 9는 다음과 같다.
Q = E{[nl, nl, nθ]T[nl, nl, nθ]}
여기서 E는 평균값을 나타낸다. 관측 잡음 공분산 행렬(R)은 다음의 수학식 10과 같다.
R = E(w2)
사실 위의 R은 관측이 하나이기 때문에 행렬이 아니라 스칼라이다. 다음으로는 상태 벡터(X(t))와 입력 벡터(U(t))를 다음의 수학식 11과 같이 정리한다. 수학식 11은 다음과 같다.
Figure 112007021434117-pat00009
수학식 6, 수학식 7 및 수학식 8을 X(t+1)=f(X(t), U(t), N)로 나타낸다. 그러면 f(X(t), U(t), N)가 비선형이기 때문에 칼만필터(40)를 위해서 다음과 같이 다음의 수학식 12와 같은 자코비안(Jacobian) 행렬을 만든다. 수학식 12는 다음과 같다.
Figure 112007021434117-pat00010
여기서, 관측값은 θ이기 때문에 관측 행렬(C)은 다음의 수학식 13과 같으며 수학식 13은 다음과 같다.
C =[0 0 1]
이제 추정된 상태 벡터를
Figure 112007021434117-pat00011
로 나타내고 상태식의 공분산 행렬을 P(t) = E{X(t)XT(t)}로 정의하면, 다음과 같은 수학식 14 내지 수학식 18에 의해 설계되는 확장칼만필터를 설계할 수 있다. 수학식 14 내지 수학식 18은 다음과 같다.
Figure 112007021434117-pat00012
Figure 112007021434117-pat00013
Figure 112007021434117-pat00014
Figure 112007021434117-pat00015
Figure 112007021434117-pat00016
수학식 14는 추정된 상태값과 주행기록계와 방향 변이 센서(10)에서 나온 관측값을 통하여 상태 벡터를 전파하는 것이며, 수학식 15는 상태 벡터의 공분산을 전파하는 것이다. 수학식 15에서 전송된 공분산 행렬은 칼만게인(K)을 획득하기 위해서 수학식 16에 이용되고, 수학식 17은 전송된 상태 벡터를 다시 보정(correction)하기 위해서 실제 관측된
Figure 112007021434117-pat00017
과 추정된 관측행렬과 상태 벡터의 곱인 CX(t+1)의 차이를 구하여 이를 칼만게인(K)과 곱하여 업데이트하는 것이다. 수학식 18은 공분산 행렬을 보정하는 과정을 보여준다. 이들 보정된 상태와 공분산은 다시 수학식 14와 수학식15에 상태 벡터와 공분산 행렬을 전파(전송)하기 위해서 이용된다.
주행기록계의 관측값은 칼만필터(40)에서 추정되어 나온 방향 θ(t)와 결합되며 방향 변이 센서(10)의 관측값은 직접적으로 입력으로 이용되는 것을 보여준다.
먼저, x축 변환부(6)는 절대 위치 변이량에 cos(θ(t))를 곱하면, x축으로 이동한 거리가 산출되며, 수학식 2와 대응된다. 그리고, y축 변환부(4)는 절대 위치 변이량에 sin(θ(t))를 곱하면, y축으로 이동한 거리가 산출되며 수학식 1과 대응된다.
그리고, 수학식 2의 값에서 이동체(2)가 Y축으로의 이동한 값을 나타내는 식에 따라 출력된 값은 앞서 언급된 수학식 12의 자코비안 행렬을 만들기 위해서 역상으로 된다. 즉, -1값만큼 증폭되는 증폭회로(8)를 거쳐 입력값 생성부(12)로 입력된다. 입력값 생성부(12)로는 증폭기(8)로부터 입력되는 -sin(θ(t))Δl과 주행기록계(30)와 X축 변환부(6)를 거쳐 생성된 cos(θ(t))Δl이 입력되고, 다시 방향 변이 센서에 의해 입력된 방향 변이값(Δθ(t))이 입력되다.
이를 입력받은 입력값 생성부(12)는 칼만필터(40)에 입력되는 입력값으로 U(t)이 생성되며 이 입력값이 칼만필터(40)로 입력되면, 칼만필터(40)에서는 수학식 14 내지 수학식 18의 과정을 거쳐 상태 벡터와 공분산 행렬을 구할 수 있다.
입력값 생성부(12)에서 생성한 이러한 입력 벡터는 수학식 14에서 U(t)에 해당한다. 또한, 방향 센서(20)의 관측값은 칼만필터(40)에서 관측잔차(measurement residual)로 이용된다.
지금까지 확장 칼만필터(40)의 기준 관측값으로 방향각을 이용하고 있으나, 위치를 알고 있는 사물이나 기준점에 대해서 이동체의 진행방향이 가지게 되는 상대적 각도를 이용하거나 기준점에서 이동체와의 거리를 기준 관측으로 이용하는 경우도 쉽게 칼만필터를 설계할 수 있다. 거리를 이용하는 경우 수학식 4는 다음의 수학식 19와 같이 바뀐다. 수학식 19는 다음과 같다.
Figure 112007021434117-pat00018
여기서 (xr, yr)은 기준점의 좌표이다. 또는 거리와 방향각 모두가 기준 관측값들로 이용되는 경우 관측식은 다음의 수학식 20으로 결정된다. 수학식 20은 다음과 같다.
Figure 112007021434117-pat00019
수학식 19와 수학식 20을 확장칼만필터에 적용하여 이용하는 것은 일반적인 과정이기 때문에 자세한 설명은 생략한다.
도 3은 본 발명의 일실시예에 따른 주행기록계, 방향센서, 방향 변이 센서의 측정값을 확장칼만필터에 적용하여 이동체의 위치를 추적하고 갱신하는 과정을 나타낸 순서도이다.
도 3을 참조하여 보면, 먼저 이동체(2)의 이동 위치 추적 기능을 사용할 것인지의 여부를 판단한다(S302).
S302에서 판단하여 이동체(2)의 이동 위치 추적 기능을 사용하는 것으로 판단되면 이동체(2)는 오도미터와 같은 주행기록계(30)로부터 출력된 Δl(t)를 이용하여 X축 및 Y축으로 이동한 거리를 측정한다(S304).
방향 변이 센서(10)는 방향 변이값(Δθ(t))을 측정한다(S306).
S304 및 S306에 의해 측정된 정보를 이용하여 앞서 언급된 수학식1 내지 수학식 8에 의해 이동체(2)의 위치를 예측한다(S308).
방향 센서(20)를 이용한 정확한 기준 관측값을 측정한 것인지의 여부를 판단한다(S310). 즉, 방향센서(20)로부터 θ값의 입력되었는지의 여부를 판단한다.
S310에서 판단하여 θ값의 입력이 있는 경우면 전술한 수학식 14 내지 수학 식 18을 적용하고 있는 칼만필터(40)를 이용하여 수학식 14 및 수학식 15에 의해 각각 상태 벡터와 공분산 행렬을 구하고 다시 수학식 16에 의해서는 칼만게인(K)을 구한 후 다시 각각 수학식 17 및 수학식 18에 의해 상태 벡터와 공분산 행렬을 갱신하는 과정을 통해 이동체의 정확한 위치를 인지한다(S312).
S310에서 판단하여 θ값의 입력이 없는 경우면 수학식 14에 따른 상태 벡터만을 업데이트하고(S314), S302로 리턴한다.
본 발명은 가속도계와 외부 관측 센서의 도움없이 이동체의 위치를 결정할 수 있으므로 고주파 잡음을 거의 가지지 않게 되며 이러한 이유로 저속도 이동체의 경우에 그 위치를 파악하는 데에 매우 유용하게 이용될 수 있는 효과가 있다.
또한, 본 발명은 속도 성분을 상태 변수로 취급하지 않고, 상태 변수로 위치와 방향각만을 이용하기 때문에 고주파 잡음이 칼만필터에 추가되지 않으며, 방향각을 기준 관측값으로 이용하기 때문에 외부 관측 센서가 필요 없는 효과가 있다.
또한, 본 발명은 가속도계를 이용하지 않기 때문에 위치 계산의 신뢰성을 높이고, 이동체의 위치 인식 시스템 제작 비용에 큰 비중을 차지하는 가속도계가 필요 없으며, 이동로봇이나 청소용 로봇의 위치 인식 시스템 및 움직이는 사물의 위치 인식, 자동차나 비행체의 항법 등에 폭넓게 이용될 수 있으므로 저비용으로 위치추적 시스템을 실용화할 수 있는 효과가 있다.

Claims (13)

  1. 삭제
  2. 삭제
  3. 이동체의 기준 관측값을 출력하는 방향센서;
    상기 이동체의 방향 변화에 대응하여 방향 변이값(Δθ(t))을 출력하는 방향 변이 센서;
    칼만필터로부터 출력되는 방향각(θ)을 적용시켜 코사인 함수로 변환하는 X축 변환부;
    칼만필터로부터 출력되는 방향각을 적용시켜 사인 함수로 변환하는 Y축 변환부;
    절대 위치 변이량을 측정하는 주행기록계;
    상기 Y축 변환부를 통과한 Y축으로 이동한 거리를 자코비안 행렬로 만들기 위해서 역상으로 -1값만큼 증폭시키는 증폭회로;
    상기 X축 변환부와 상기 증폭회로에서 전송받은 거리정보와 상기 방향 변이 센서에서 전송받은 방향 변이값을 수신하여 자코비안 행렬을 이용하여 입력값을 구하는 입력값 생성부; 및
    상기 입력값을 수신받아 X축 위치, Y축 위치, 방향각을 포함하는 이동체의 현재 위치를 산출하는 칼만필터를 포함하는 것을 특징으로 하는 주행기록계와 방향센서를 이용한 이동체의 위치 결정 시스템.
  4. 제3항에 있어서,
    상기 칼만필터는 상태 벡터(X(t+1))를 다음의 수학식을 통해 구하고,
    Figure 712009005851266-pat00020
    공분산(P(t+1)을 다음의 수학식을 통해 구하며,
    Figure 712009005851266-pat00021
    상기 식에서
    Figure 712009005851266-pat00022
    이며, P(t)는 이전의 공분산이고, Q는 공분산 행렬(Q)인 것을 특징으로 하는 주행기록계와 방향센서를 이용한 이동체의 위치 결정 시스템.
  5. 제4항에 있어서,
    상기 칼만필터가 상기 상태 벡터 및 상기 공분산을 다음의 수학식에 의해 칼만게인인 K를 구하고,
    Figure 112007021434117-pat00023
    상기 칼만게인을 이용하여 다음의 수학식에 의해 각각 상기 상태 벡터 및 상기 공분산을 구하여 갱신하며,
    Figure 112007021434117-pat00024
    Figure 112007021434117-pat00025
    상기 C는 [0 0 1]의 관측행렬인 것을 특징으로 하는 주행기록계와 방향센서를 이용한 이동체의 위치 결정 시스템.
  6. 제5항에 있어서,
    상기 방향 변이 센서는 자이로인 것을 특징으로 하는 주행기록계와 방향센서를 이용한 이동체의 위치 결정 시스템.
  7. 제5항에 있어서,
    상기 방향 센서는 자기 센서인 것을 특징으로 하는 주행기록계와 방향센서를 이용한 이동체의 위치 결정 시스템.
  8. 제5항에 있어서,
    상기 방향센서를 이용하지 않고, 이동 거리를 측정하고 상기 이동 거리를 상기 이동체의 위치 측정에 사용하는 경우에는 다음의 수학식을 채용하여 다음의 수학식에 의해 상기 칼만필터를 설계하며,
    Figure 112007021434117-pat00026
    여기서 (xr, yr)은 기준점의 좌표이며, w는 관측시 섞여있는 관측 잡음을 나타내는 것을 특징으로 하는 주행기록계와 방향센서를 이용한 이동체의 위치 결정 시스템.
  9. 제5항에 있어서,
    이동거리가 측정된 경우에는 상기 이동거리도 함께 위치 측정에 사용하며, 다음의 수학식을 채용하여 다음의 수학식에 의해 상기 칼만필터를 설계하며,
    Figure 112009028003118-pat00027
    여기서 (xr, yr)은 기준점의 좌표이며, w는 관측시 섞여있는 관측 잡음을 나타내는 것을 특징으로 하는 주행기록계와 방향센서를 이용한 이동체의 위치 결정 시스템.
  10. 삭제
  11. 이동체가 주행기록계로부터 출력된 Δl(t)를 이용하여 X축 및 Y축으로 이동한 거리를 측정하는 이동거리 측정단계와;
    방향 변이 센서가 방향 변이값(Δθ(t))을 측정하는 방향 변이값 측정단계와;
    상기 측정한 이동거리 및 상기 방향 변이값을 이용하여 X축 위치, Y축 위치, 방향각을 포함하는 이동체의 현재 위치를 예측하는 예측단계와;
    상기 예측된 위치에 대응하는 입력 벡터를 칼만필터에 입력하여 각각 상태 벡터와 공분산 행렬을 구하고 칼만게인(K)을 이용하여 상기 상태 벡터와 공분산 행렬을 갱신하는 갱신단계를 포함하며;
    상기 이동체가 X축으로 이동한 거리는 다음의 수학식에 의해 예측하며,
    Figure 712009005851266-pat00028
    상기 이동체가 Y축으로 이동한 거리는 다음의 수학식에 의해 예측하고,
    Figure 712009005851266-pat00029
    상기 이동체가 이동한 방향은 다음의 수학식에 의해 예측하며,
    Figure 712009005851266-pat00030
    상기
    Figure 712009005851266-pat00031
    는 방향의 변이를 측정하는 방향 변이 센서에서 측정한 θ(t) 방향의 변이량의 관측값이고,
    Figure 712009005851266-pat00032
    은 주행을 기록하는 주행기록계에서 측정한 거리의 변이량의 관측값이며, 각각의 nθ, nl은 각각 상기 방향 변이 센서와 상기 주행기록계에서 관측된 관측 잡음인 것을 특징으로 하는 주행기록계와 방향센서를 이용한 이동체의 위치 결정 방법.
  12. 제11항에 있어서,
    상기 갱신과정은,
    Figure 712009005851266-pat00033
    공분산(P(t+1)을 다음의 수학식을 통해 구하며,
    Figure 712009005851266-pat00034
    다음의 수학식에 의해 칼만게인인 K를 구하고,
    Figure 712009005851266-pat00035
    상기 칼만게인을 이용하여 다음의 수학식에 의해 각각 상기 상태 벡터 및 상기 공분산을 구하여 갱신하며,
    Figure 712009005851266-pat00036
    Figure 712009005851266-pat00037
    상기 식에서
    Figure 712009005851266-pat00038
    이며, P(t)는 이전의 공분산이고, Q는 공분산 행렬(Q)이고, 상기 C는 [0 0 1]의 관측행렬인 것을 특징으로 하는 주행기록계와 방향센서를 이용한 이동체의 위치 결정 방법.
  13. 삭제
KR1020070026154A 2007-03-16 2007-03-16 주행기록계와 방향센서를 이용한 이동체의 위치 결정 방법,시스템 및 그 방법을 기록한 기록매체 KR100936710B1 (ko)

Priority Applications (1)

Application Number Priority Date Filing Date Title
KR1020070026154A KR100936710B1 (ko) 2007-03-16 2007-03-16 주행기록계와 방향센서를 이용한 이동체의 위치 결정 방법,시스템 및 그 방법을 기록한 기록매체

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
KR1020070026154A KR100936710B1 (ko) 2007-03-16 2007-03-16 주행기록계와 방향센서를 이용한 이동체의 위치 결정 방법,시스템 및 그 방법을 기록한 기록매체

Publications (2)

Publication Number Publication Date
KR20080084435A KR20080084435A (ko) 2008-09-19
KR100936710B1 true KR100936710B1 (ko) 2010-01-13

Family

ID=40024788

Family Applications (1)

Application Number Title Priority Date Filing Date
KR1020070026154A KR100936710B1 (ko) 2007-03-16 2007-03-16 주행기록계와 방향센서를 이용한 이동체의 위치 결정 방법,시스템 및 그 방법을 기록한 기록매체

Country Status (1)

Country Link
KR (1) KR100936710B1 (ko)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR20220056903A (ko) * 2020-10-28 2022-05-09 한국조선해양 주식회사 구형 산업차량의 주행속도 관리장치

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110213715B (zh) * 2019-05-17 2020-10-02 华中科技大学 一种基于车辆间最优估计距离的v2v通信模式切换方法
CN110561424A (zh) * 2019-07-28 2019-12-13 华南理工大学 基于多传感器混合滤波器的在线机器人运动学校准方法
CN113639744B (zh) * 2021-07-07 2023-10-20 武汉工程大学 一种用于仿生机器鱼的导航定位方法和系统

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR19980073341A (ko) * 1997-03-13 1998-11-05 김영환 자이로스코프 데이타와 스피드센서 데이타를 이용한 상대위치 측정장치 및 그 제어방법
US7015831B2 (en) 2002-12-17 2006-03-21 Evolution Robotics, Inc. Systems and methods for incrementally updating a pose of a mobile device calculated by visual simultaneous localization and mapping techniques
US20060106533A1 (en) 2004-11-12 2006-05-18 Mitsubishi Denki Kabushiki Kaisha System for autonomous vehicle navigation with carrier phase DGPS and laser-scanner augmentation
KR20070021872A (ko) * 2005-08-19 2007-02-23 한국전자통신연구원 추측 항법과 지피에스를 이용한 복합 항법 장치 및 그 방법

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR19980073341A (ko) * 1997-03-13 1998-11-05 김영환 자이로스코프 데이타와 스피드센서 데이타를 이용한 상대위치 측정장치 및 그 제어방법
US7015831B2 (en) 2002-12-17 2006-03-21 Evolution Robotics, Inc. Systems and methods for incrementally updating a pose of a mobile device calculated by visual simultaneous localization and mapping techniques
US20060106533A1 (en) 2004-11-12 2006-05-18 Mitsubishi Denki Kabushiki Kaisha System for autonomous vehicle navigation with carrier phase DGPS and laser-scanner augmentation
KR20070021872A (ko) * 2005-08-19 2007-02-23 한국전자통신연구원 추측 항법과 지피에스를 이용한 복합 항법 장치 및 그 방법

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR20220056903A (ko) * 2020-10-28 2022-05-09 한국조선해양 주식회사 구형 산업차량의 주행속도 관리장치
KR102505757B1 (ko) 2020-10-28 2023-03-07 한국조선해양 주식회사 구형 산업차량의 주행속도 관리장치

Also Published As

Publication number Publication date
KR20080084435A (ko) 2008-09-19

Similar Documents

Publication Publication Date Title
US7970491B2 (en) Robot localization system
KR102018450B1 (ko) 단일 안테나에 기초한 ｇｎｓｓ―ｉｎｓ 차량 자세 결정 방법
Aghili et al. Driftless 3-D attitude determination and positioning of mobile robots by integration of IMU with two RTK GPSs
KR100520166B1 (ko) 네비게이션시스템에서 이동체의 위치검출장치 및 그 방법
Allotta et al. Cooperative localization of a team of AUVs by a tetrahedral configuration
Kinsey et al. In situ alignment calibration of attitude and Doppler sensors for precision underwater vehicle navigation: Theory and experiment
US7168177B2 (en) Method and apparatus for determining a geomagnetic field by using a compass and method and apparatus for determining an azimuth angle of a moving object using the same
US8209055B2 (en) System for sensing state and position of robot
US9482536B2 (en) Pose estimation
EP3865914A1 (en) Gnss/imu surveying and mapping system and method
CN103697910B (zh) 自主水下航行器多普勒计程仪安装误差的校正方法
US20050125108A1 (en) Motion estimation method and system for mobile body
Scheding et al. Experiments in autonomous underground guidance
KR101789188B1 (ko) 수중 이동체의 위치 추적을 위한 수중 통합 항법 시스템
Antonelli et al. Observability analysis of relative localization for AUVs based on ranging and depth measurements
JP4726134B2 (ja) 移動体制御装置及び移動体制御方法
CN106679662B (zh) 一种基于tma技术的水下机器人单信标组合导航方法
CN104197927A (zh) 水下结构检测机器人实时导航系统及方法
CN105259902A (zh) 水下机器人惯性导航方法及系统
JP2008076389A (ja) ナビゲーション・システムおよびナビゲーション方法
CN101393025A (zh) Auv组合导航系统无迹切换方法
CN105698822A (zh) 基于反向姿态跟踪的自主式惯性导航行进间初始对准方法
JP2012193965A (ja) 位置標定装置、位置標定装置の位置標定方法および位置標定プログラム
KR100936710B1 (ko) 주행기록계와 방향센서를 이용한 이동체의 위치 결정 방법,시스템 및 그 방법을 기록한 기록매체
Troni et al. Preliminary experimental evaluation of a Doppler-aided attitude estimator for improved Doppler navigation of underwater vehicles

Legal Events

Date Code Title Description
A201 Request for examination
E902 Notification of reason for refusal
AMND Amendment
E902 Notification of reason for refusal
AMND Amendment
E601 Decision to refuse application
J201 Request for trial against refusal decision
AMND Amendment
B701 Decision to grant
GRNT Written decision to grant
FPAY Annual fee payment

Payment date: 20121206

Year of fee payment: 4

FPAY Annual fee payment

Payment date: 20131209

Year of fee payment: 5

FPAY Annual fee payment

Payment date: 20141229

Year of fee payment: 6

FPAY Annual fee payment

Payment date: 20170206

Year of fee payment: 8

LAPS Lapse due to unpaid annual fee