KR100879701B1 - 무향 칼만 필터를 이용한 동시 위치 추정 및 지도 작성시스템 및 방법 - Google Patents

무향 칼만 필터를 이용한 동시 위치 추정 및 지도 작성시스템 및 방법 Download PDF

Info

Publication number
KR100879701B1
KR100879701B1 KR1020070046015A KR20070046015A KR100879701B1 KR 100879701 B1 KR100879701 B1 KR 100879701B1 KR 1020070046015 A KR1020070046015 A KR 1020070046015A KR 20070046015 A KR20070046015 A KR 20070046015A KR 100879701 B1 KR100879701 B1 KR 100879701B1
Authority
KR
South Korea
Prior art keywords
probability distribution
moving object
measured
objects
current period
Prior art date
Application number
KR1020070046015A
Other languages
English (en)
Other versions
KR20080100004A (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 KR1020070046015A priority Critical patent/KR100879701B1/ko
Publication of KR20080100004A publication Critical patent/KR20080100004A/ko
Application granted granted Critical
Publication of KR100879701B1 publication Critical patent/KR100879701B1/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
    • 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
    • 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
    • G01S15/00—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems
    • G01S15/02—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems using reflection of acoustic waves
    • G—PHYSICS
    • G09—EDUCATION; CRYPTOGRAPHY; DISPLAY; ADVERTISING; SEALS
    • G09B—EDUCATIONAL OR DEMONSTRATION APPLIANCES; APPLIANCES FOR TEACHING, OR COMMUNICATING WITH, THE BLIND, DEAF OR MUTE; MODELS; PLANETARIA; GLOBES; MAPS; DIAGRAMS
    • G09B29/00—Maps; Plans; Charts; Diagrams, e.g. route diagram
    • G09B29/003—Maps
    • H—ELECTRICITY
    • H03—ELECTRONIC CIRCUITRY
    • H03M—CODING; DECODING; CODE CONVERSION IN GENERAL
    • H03M1/00—Analogue/digital conversion; Digital/analogue conversion
    • H03M1/12—Analogue/digital converters

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Remote Sensing (AREA)
  • Theoretical Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • General Physics & Mathematics (AREA)
  • Educational Technology (AREA)
  • Educational Administration (AREA)
  • Acoustics & Sound (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Business, Economics & Management (AREA)
  • Mathematical Physics (AREA)
  • Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)
  • Navigation (AREA)

Abstract

미리 설정된 주기마다 이동체의 운항 환경에 존재하는 물체들의 위치를 측정하는 측정 수단; 및 상기 측정 수단과 송수신하며, 측정된 상기 물체들의 위치를 이용하여 상기 이동체의 위치를 측정하고, 이전 주기에 측정된 상기 이동체의 위치 및 상기 물체들의 위치를 포함하는 확률 분포로부터 가우시안(Gaussian) 분포를 샘플링하기 위해 설정된 시그마 포인트(sigma point)를 사용하여 현 주기의 상기 확률 분포를 계산하고, 계산된 상기 확률 분포 및 상기 측정 수단에서 현 주기에 측정된 상기 물체들의 위치를 이용하여 상기 확률 분포를 갱신하는 분석 수단을 포함하는 동시 위치 추정 및 지도 작성 시스템이 개시된다. 본 발명에 따른 동시 위치 추정 및 지도 작성 시스템 및 방법을 사용하여, 운항 환경의 비선형성으로 인한 오차 발생을 최소화할 수 있으며, AUV가 운항하는 해역을 몇 개의 서브맵으로 분할하여 계산함으로써 연산 부하를 줄일 수 있는 이점이 있다.
SLAM, 슬램, AUV, 무인 자율 잠수정, UKF, 무향 칼만 필터

Description

무향 칼만 필터를 이용한 동시 위치 추정 및 지도 작성 시스템 및 방법{System and Method for Simultaneous Localization and Mapping Using Unscented Kalman Filter}
도 1은 본 발명의 일 실시예에 따른 동시 위치 추정 및 지도 작성 시스템에 포함되는 구성 요소 사이의 신호 흐름을 도시한 구성도이다.
도 2는 본 발명의 일 실시예에 따른 동시 위치 추정 및 지도 작성 방법의 각 단계를 도시한 순서도이다.
도 3a는 종래 기술에 따른 확장 칼만 필터를 사용한 동시 위치 추정 및 지도 작성 방법에 의하여 AUV에서 물체의 위치를 측정한 결과를 도시한 그래프이다.
도 3b는 종래 기술에 따른 확장 칼만 필터를 사용한 동시 위치 추정 및 지도 작성 방법에 의하여 AUV에서 물체의 위치를 측정한 또 다른 결과를 도시한 그래프이다.
도 4a는 본 발명의 일 실시예에 따른 동시 위치 추정 및 지도 작성 방법에 의하여 AUV에서 물체의 위치를 측정한 결과를 도시한 그래프이다.
도 4b는 본 발명의 일 실시예에 따른 동시 위치 추정 및 지도 작성 방법에 의하여 AUV에서 물체의 위치를 측정한 또 다른 결과를 도시한 그래프이다.
본 발명은 무향 칼만 필터(Unscented Kalman Filter)를 이용한 동시 위치 추정 및 지도 작성(SLAM; Simultaneous Localization and Mapping) 시스템 및 방법에 관한 것이다. 더욱 상세하게는, 수중에서 자율 운항되는 무인 자율 잠수정(Autonomous Underwater Vehicle, 이하 AUV라고 함)에 있어서 기존의 확장 칼만 필터(Extended Kalman Filter)가 아닌 무향 칼만 필터를 사용하여 잠수정의 위치를 측정하고 주변 해역의 지도를 작성할 수 있는 시스템 및 방법에 관한 것이다.
넓은 해역에서 지속적으로 자율 운항되는 무인 자율 잠수정(AUV)의 활용도가 높아지면서, 긴 시간 동안 운용되는 무인 자율 잠수정의 항해 오차를 줄이고 정확한 위치를 파악하는 것은 무인 자율 잠수정의 안전하고 효율적인 운용에 있어서 중요한 문제가 되고 있다. 이에 대한 해결책으로 제시되는 방법 중 AUV 운용 해역에 대한 사전 정보 없이 고정되어 있는 지형 지물 정보를 이용하여 지형지물에 대한 위치를 기록하고 이로부터 AUV의 위치를 파악하는 기법으로 SLAM(Simultaneous Localization and Mapping, 이하 SLAM이라 함) 기법이 있다.
SLAM 기법은 다수 개의 센서를 사용하여 여러 개의 공간 정보를 획득하고, 이러한 다수의 공간 정보들을 결합하여 만들어낸 지도가 낮은 오차를 가지도록 하는 것을 목적으로 한다. 이를 위해서, 센서에 의하여 얻을 수 있는 거리 및 방위각 등과 같은 공간 정보에 대한 측정치와 센서의 제원을 이용하여 당해 공간 정보에 대한 확률 분포를 구성한다. 또한, AUV와 탐지된 물체 정보를 결합하기 위하여 AUV 및 탐지되는 물체들에 대한 공간 정보를 하나의 시스템 상태 벡터(system state vector)로 구성한다. 구성된 시스템 상태 벡터에 대하여, 해당 확률 분포를 이용하여 기대값과 공분산 행렬(covariance matrix)을 추정한다. 이때 기대값은 정보에 대한 추정치를 나타내고, 공분산 행렬은 각 정보에 대한 오차 및 정보들 사이의 의존도를 나타낸다.
이때 시스템 상태 벡터와 공분산 행렬에 대한 추정을 위하여 칼만 필터(Kalman Filter)가 주로 사용되어 왔다. 특히, 칼만 필터 중에서도, 비선형적으로 표시되는 상태 방정식을 가진 시스템에 대해 자코비안(Jacobian)을 이용하여 상태 방정식을 선형화하여 사용되는 확장 칼만 필터가 주로 사용되어 왔다. 그러나 확장 칼만 필터를 사용하기 위해서는 비선형성 시스템일지라도 부분 선형성을 가지는 가정을 만족하여야 하는데, AUV의 운동 방정식은 속도와 선수각의 곱으로 표현되는 비선형성 방정식이므로 이러한 가정을 충족시키지 못할 가능성이 존재한다.
또한 확장 칼만 필터는 기본적으로 시스템이 비선형일지라도, 부분적으로 선형성을 가진다는 가정이 성립되어야만 사용될 수 있다. 부분 선형성에 대한 가정이 위배될 경우 추정 결과가 발산하게 되어 올바른 추정 결과를 기대할 수 없다. 또한 비선형 시스템 관계식을 확장 칼만 필터에 적용하기 위하여 계산하여야 하는 자코비안은 실제 적용시 유일해를 갖는 형태로 계산할 때 많은 어려움이 있어, 이를 통하여 상태 방정식을 선형화하는 데는 많은 문제점이 있다. 따라서, 비선형 관계식을 갖는 시스템에 적용되어 비선형성으로 인한 오차 발생을 최소화할 수 있는 방법이 필요하다.
전술한 종래 기술의 문제점을 해결하기 위한 본 발명은, 확장 칼만 필터가 비선형성 시스템에 대하여 갖는 약점을 해결하고, 비선형적인 시스템에서도 오차를 최소화하면서 AUV의 위치 추정 및 지도 작성을 동시에 수행할 수 있는 시스템 및 방법을 제공하는 것을 목적으로 한다.
전술한 목적을 달성하기 위한 본 발명의 일 실시예에 따른 동시 위치 추정 및 지도 작성 시스템은, 미리 설정된 주기마다 이동체의 운항 환경에 존재하는 물체들의 위치를 측정하는 측정 수단; 및 상기 측정 수단과 송수신하며, 측정된 상기 물체들의 위치를 이용하여 상기 이동체의 위치를 측정하고, 이전 주기에 측정된 상기 이동체의 위치 및 상기 물체들의 위치를 포함하는 확률 분포로부터 가우시안(Gaussian) 분포를 샘플링하기 위해 설정된 시그마 포인트(sigma point)를 사용하여 현 주기의 상기 확률 분포를 계산하고, 계산된 상기 확률 분포 및 상기 측정 수단에서 현 주기에 측정된 상기 물체들의 위치를 이용하여 상기 확률 분포를 갱신하는 분석 수단을 포함하여 구성될 수 있다.
본 발명의 다른 측면에 따른 동시 위치 추정 및 지도 작성 방법은, 이전 주기에 측정된, 이동체의 위치 및 상기 이동체의 운항 환경에 존재하는 물체들의 위치를 포함하는 확률 분포로부터, 가우시안 분포를 샘플링하기 위한 시그마 포인트 를 설정하는 단계 (a); 상기 시그마 포인트를 사용하여 현 주기의 상기 확률 분포 및 상기 이동체에서 상기 물체들의 위치를 측정한 측정값을 계산하는 단계 (b); 현 주기에 상기 이동체에서 상기 물체들의 위치를 측정한 실제 측정값을 구하는 단계 (c); 및 상기 단계 (b)에서 계산된 상기 확률 분포 및 상기 측정값과, 상기 단계 (c)에서 구한 상기 실제 측정값을 사용하여 상기 확률 분포를 갱신하는 단계 (d)를 포함하되, 미리 설정된 주기마다 상기 단계 (a) 내지 상기 단계 (d)를 반복하도록 구성될 수 있다.
이하, 첨부된 도면을 참조하여 본 발명의 바람직한 실시예에 대하여 상세히 설명한다.
도 1은 본 발명의 일 실시예에 따른 동시 위치 추정 및 지도 작성 시스템에 포함되는 장치 사이의 신호 흐름을 도시하는 구성도이다. 상기 실시예에 따른 시스템은 음파를 송수신하는 소나(Sonar; 13, 14), 송신될 신호를 발진 신호로 변환하여 송신하기 위한 발진회로(12), 수신된 신호를 증폭하는 증폭회로(15), 증폭된 회로의 변환을 수행하는 A/D(analog/digital) 변환기(16) 및 변환된 신호를 분석하고 시작 신호를 송출하는 분석 수단(11)을 포함한다.
분석 수단(11)에서 시작 신호가 주어지면 시작 신호는 발진 회로(12)에서 발진 신호로 변환된다. 송신용 소나(13)는 발진 회로(12)에서 발진 신호를 수신하여 이를 음파로 출력한다. 출력된 음파가 AUV의 운항 환경에 존재하는 물체에 반사되어 되돌아올 경우, 수신용 소나(14)에서 이를 수신한다. 수신한 신호는 증폭회 로(15)에서 증폭되고, A/D 변환 회로(16)에서 디지털 변환되어 분석 수단(11)으로 전달된다. 분석 수단(11)은 변환된 신호에 무향 칼만 필터를 적용하여, 정확한 AUV의 위치 및 운항 환경에 존재하는 물체의 위치를 동시에 알아내고 각 위치가 기록된 지도를 구성한다.
본 발명의 일 실시예에 따른 동시 위치 추정 및 지도 작성 시스템은, 분석 수단(11)에서 소나(13, 14)에 의한 신호를 무향 칼만 필터를 사용하여 분석하는 것을 특징으로 한다. 무향 칼만 필터는 비선형 시스템 관계식에 대하여 선형화 작업을 수행하지 않고, 비선형 관계식을 그대로 사용함으로써 비선형성으로 인한 오차 발생을 최소화하는 방법이다. 무향 칼만 필터를 사용함에 따라, 분석 수단(11)은 소나(13, 14)에 의한 신호를 사용하여 구성된 시스템 상태 벡터에서 가우시안(Gaussian) 분포의 평균과 분산을 샘플링(sampling) 할 수 있는 시그마 포인트(sigma point)들을 계산한다. 그리고 설정된 시그마 포인트의 시간에 따른 변경을 예측함으로써, 시스템 상태 벡터 및 공분산 행렬을 예측하게 된다. 다음으로, 분석 수단(11)은 소나(13, 14)에서 실제로 측정된 값과 예측된 값을 비교하여 시스템 상태 벡터 및 공분산 행렬을 갱신하는 과정을 수행하며, AUV의 움직임에 따라 전술한 과정들을 반복하여 수행함으로써 AUV 및 주변 환경에 존재하는 물체들의 위치를 정확하게 파악할 수 있다.
도 2는 전술한 본 발명의 일 실시예에 따른 위치 추정 및 지도 작성을 동시에 수행하는 방법의 각 단계를 도시한 순서도이다. 이하, 도 2를 참조하여 본 발명의 일 실시예에 따른 동시 위치 추정 및 지도 작성 방법의 각 단계에 대하여 상세 히 설명한다.
시스템 상태 벡터의 정의
SLAM 기법은 AUV와 물체를 하나의 시스템 상태 벡터로 구성하여 AUV와 물체들에 대한 추정치를 계산하고 지도를 작성하는 방법이다. 본 명세서에 기재되는 SLAM 기법에 있어서 '지도'란 AUV와 운항 환경에 존재하는 물체의 위치 정보를 가지고 있는 시스템 상태 벡터와, 기록된 위치에 대한 오차 범위를 표시하는 공분산 행렬로 표현된다. 우선, AUV의 상태를 표시하는 벡터 x v 는 다음 수학식에 의하여 정의될 수 있다.
Figure 112007035052613-pat00001
상기 수학식 1에서 x와 y는 AUV의 2차원적 위치를 표시하는 좌표들이며, V는 AUV의 속도를 나타내고
Figure 112007035052613-pat00002
는 선수각을 나타낸다. 반면 AUV에 장비된 소나(13, 14)에 의하여 탐지되는 물체의 상태를 나타내는 벡터 x on는 다음 수학식에 의하여 정의될 수 있다.
Figure 112007035052613-pat00003
상기 수학식 2에서 xon과 yon은 탐지된 물체의 2차원적 위치를 표시하는 좌표들이다. n은 지도상에 존재하는 물체의 개수를 나타낸다. 이때, 현재 시스템의 상태를 나타내는 시스템 상태 벡터는 다음 수학식에 의하여 표현될 수 있다.
Figure 112007035052613-pat00004
전술한 실시예에서, AUV 및 물체들의 위치를 표시하는 좌표는 x 좌표 및 y 좌표를 포함하는 2차원 좌표로 기술되었다. 그러나 이는 예시적으로 기술된 것으로써, 다른 실시예에서 AUV 및 물체들의 좌표는 1차원 내지 3차원 좌표 중 어느 것으로 설정되어도 무방하다.
시그마 포인트의 계산
상기 시스템 상태 벡터는 AUV의 운항 환경이 비선형적임으로 인하여 비선형성을 띈다. 본 발명에서는 무향 칼만 필터를 이용하고 있는데, 이는 가우시안(Gaussian) 분포의 평균과 분산을 샘플링(sampling)하여 적절하게 비선형 관계식에 적용하는 변환 방법이다. 이를 위해서는 시스템 상태 벡터에서 가우시안 분포를 샘플링할 수 있는 적절한 시그마 포인트를 계산하여야 하는데, 상기 시그마 포인트를 계산하는 과정을 무향 변환(Unscented Transform)이라고 한다.
따라서, 수신용 소나(14)에 의한 수신값이 증폭 및 변환되어 입력되면, 분석 수단(11)은 먼저 시그마 포인트를 구하는 단계를 수행한다(S201). 시스템 상태 벡 터의 차원을 Nx 라 하고, 공분산 행렬을 P xx 라 할 경우 시그마 포인트 x (0) , x (i) , x (i+ Nx ) 는 다음 수학식들에 의하여 구할 수 있다.
Figure 112007035052613-pat00005
Figure 112007035052613-pat00006
Figure 112007035052613-pat00007
여기서
Figure 112007035052613-pat00008
는
Figure 112007035052613-pat00009
의 제곱근 행렬(matrix square root)의 i번째 열을 나타낸다.
Figure 112007035052613-pat00010
는 가중합 평균 계산 시 각 x (i) 에 대한 가중치이다.
위치에 대한 사전 예측
다음으로 무향 칼만 필터는 전술한 무향 변환을 통하여 구한 시그마 포인트들을 이용하여, 추정치의 평균과 분산에 대한 예측을 수행하게 된다(S202). 본 명 세서에 설명되는 실시예에서 AUV의 운동은 이산적 2차 운동학적 모델로 표현되는데, 이는 가속도가 백색 잡음으로 존재하는 등속도 운동 모델이다. 이산화된 시간을 k라 하고, AUV가 운항하는 환경의 특성으로 인한 비선형 전달 함수를 f라 하며, 백색 가우시안 잡음을 표현할 수 있는 오차 항을 q라 할 경우 AUV의 운동에 따른 시스템 상태 벡터는 일반적으로 다음 수학식에 의하여 표현된다.
Figure 112007035052613-pat00011
상기 수학식을 시스템 상태 벡터 x의 각 성분에 대하여 적용한 결과는 다음 수학식들과 같다.
Figure 112007035052613-pat00012
Figure 112007035052613-pat00013
Figure 112007035052613-pat00014
Figure 112007035052613-pat00015
Figure 112007035052613-pat00016
본 발명의 실시예에 따른 방법에서, 시스템 벡터에 대한 예측은 전술한 시그마 포인트를 사용하여 수행된다. 본 명세서에서 임의의 벡터 A에 대하여 A(k+1|k) 라고 쓸 경우, 이는 이산시간 k에서의 값에 기초하여 시간 k+1의 값에 대하여 추정한 값을 의미한다. 무향 칼만 필터를 사용한 방법에서는, 전술한 과정에서 구한 각 시그마 포인트가 전달 함수에 의하여 전파되는 값을 예측하는 방법으로 시스템 상태 벡터를 예측하게 된다. 각 시그마 포인트
Figure 112007035052613-pat00017
의 예측값은 다음 수학식과 같다.
Figure 112007035052613-pat00018
상기 수학식 13에서 u(k)는 AUV의 조종을 위한 입력값이며, q(k)는 AUV가 운동할 때 발생할 수 있는 예상치 못한 거동에 대한 수학적 표현이다. 시간 k+1에서의 시그마 포인트의 값이 예측되면, 다음으로 이를 결합하여 시스템 상태 벡터 및 공분산 행렬을 예측한다.
Figure 112007035052613-pat00019
Figure 112007035052613-pat00020
상기 수학식들에 의하여 시스템 상태 벡터 및 공분산 행렬을 구함으로써, 시간 k+1에서의 AUV 위치 및 운항 환경에 존재하는 물체들의 위치를 예측할 수 있다. 다음으로, 분석 수단(11)은 예측된 AUV 위치에서 다시 소나(13, 14)를 사용하여 물체들을 측정할 경우 그 측정값에 대한 예측을 수행한다.
측정값에 대한 사전 예측
측정값은 물체에 대한 거리 및 방위각으로 얻어진다. 측정 오차의 기대값은 0 이라고 가정하며, 분산은 소나(13, 14)의 제원에 의하여 구할 수 있다. 이산시간 k에서의 측정값 Z(k)는, 물체까지의 거리를 r이라 하고, 방위각을
Figure 112007035052613-pat00021
라 하며, 각각에 대한 측정 오차를 w라 할 경우 다음 수학식과 같이 표현된다.
Figure 112007035052613-pat00022
이때, 상기 수학식 16은 z(k) = h(x(k)) + w(k) 와 같이 시스템 상태의 함수로서 표현될 수 있으며, 이때 함수 h는 다음 수학식과 같이 표현된다.
Figure 112007035052613-pat00023
상기 수학식들에 의하여 측정값 z(k)가 표현되며, 분석 수단(11)은 측정값에 대한 예측 과정을 수행한다(S202). 시스템 상태 벡터를 예측하는 경우와 마찬가지로, z(k+1|k) 라고 쓸 경우, 이는 이산시간 k에서의 값에 기초하여 시간 k+1의 값에 대하여 예측한 측정값을 의미한다. 우선, 각 시그마 포인트에 대하여 측정값을 예측할 경우, 예측값은 다음 수학식에 의하여 표현된다.
Figure 112007035052613-pat00024
각 시그마 포인트에 대한 측정값이 예측되면, 다음으로 이를 결합하여 전체 시스템에 대한 측정값을 예측하여 계산하게 되며, 이는 다음 수학식과 같다.
Figure 112007035052613-pat00025
전술한 과정에 의하여, 시그마 포인트를 사용하여 예측된 AUV의 위치에서 측정한 물체들의 위치의 측정값을 예측할 수 있다. 다음으로, AUV가 움직이면서 소 나(13, 14)에 의하여 실제 측정값을 얻고, 실제 측정값에 의하여 예측값을 갱신하거나 새로운 물체를 추가하는 과정이 필요하다.
실제 측정값 및 기존 물체 재탐지 여부 결정
예측된 측정값이 계산되면, 다음으로 분석수단(11)은 소나(13, 14)를 사용하여 실제 측정값을 구한다(S203). 복수 개의 물체에 대한 측정값은 다음 수학식과 같은 형태의 행렬로 표현될 수 있다. z n 은 n번째 물체에 대하여 측정한 측정값을 표시하는 벡터이다.
Figure 112007035052613-pat00026
이때 만약 소나(13, 14)에 의하여 특정 물체가 감지된 경우, 감지된 물체를 기존의 물체로 볼 것인지 또는 새로운 물체로 판단할 것인지 여부의 결정은, 전술한 예측된 측정값 중에서 지도에 있는 n 번째 물체에 대한 측정값 z n과, 실제 측정값 중 m 번째 물체에 대한 측정값 z m을 비교하여 수행된다. 아래 수학식들에서, z m(k+1)은 시간 k+1에서의 m번째 물체의 실제 측정값이며, z n(k+1|k)는 시간 k의 값 을 토대로 계산된 n번째 물체의 예측된 측정값이다.
Figure 112007035052613-pat00027
는 전술한 z m(k+1)과 z n(k+1|k)의 차이를 나타내는 이노베이션(innovation)으로서, 다음 수학식에 의하여 정의된다.
Figure 112007035052613-pat00028
상기 수학식 21에 의하여 계산된 이노베이션
Figure 112007035052613-pat00029
을 사용하여, 비교를 위한 행렬
Figure 112007035052613-pat00030
의 값을 다음 수학식에 의하여 계산한다.
Figure 112007035052613-pat00031
상기 수학식 22에서
Figure 112007035052613-pat00032
은
Figure 112007035052613-pat00033
의 공분산 행렬이며,
Figure 112007035052613-pat00034
및
Figure 112007035052613-pat00035
에 대한 평균값으로 계산된다. 분석 수단(11)은 만약
Figure 112007035052613-pat00036
이 미리 정해진 값보다 작으면 n 번째의 물체가 다시 탐지된 것으로 판단하며, 미리 정해진 값 이상인 경우에는 새로운 물체가 탐지된 것으로 판단한다. 새로운 물체가 탐지된 경우에는 시스템 상태 벡터에 새로운 물체의 정보를 추가하여야 하며, 이 과정은 후술한다.
새로운 물체로 판단된 경우 물체 정보 추가
전술한 과정에 의하여 새로운 물체가 탐지된 것으로 판단된 경우, 새로운 물체의 시스템 상태를 구성하여 기존의 시스템 상태 벡터에 추가하게 되며, 새로운 시스템 상태 벡터에 대한 공분산 행렬이 구성되어야 한다(S205). 새로운 물체의 위치를 나타내는 벡터를 x Onew 라 하고, 새로운 물체에 대한 측정값을 z new 라 할 경우, 새로운 물체의 위치 벡터 및 측정값은 다음 수학식들에 의하여 표현된다.
Figure 112007035052613-pat00037
Figure 112007035052613-pat00038
이때, 새로운 물체의 벡터를 포함하여 시스템 상태 벡터 및 공분산 행렬을 계산하면 이는 다음 수학식들에 의하여 표현된다.
Figure 112007035052613-pat00039
Figure 112007035052613-pat00040
Figure 112007035052613-pat00041
Figure 112007035052613-pat00042
운항 환경에서 새로운 물체가 검출된 경우, 전술한 과정에 의하여 새롭게 탐지된 물체의 위치 및 측정값을 표시하는 벡터를 시스템 상태 벡터 및 공분산 행렬에 추가함으로써 AUV의 운항 환경을 정확하게 탐지할 수 있다.
갱신 작업
전술한 과정에 의하여 기존 물체에 관련된 실제 측정치 탐색 및 새로운 물체에 대한 추가 작업이 완료되면, 다음으로 갱신 작업 및 지도 작성 작업이 수행된다(S206). 갱신 작업을 위하여 측정값에 대한 이노베이션의 공분산 행렬, 및 상기 이노베이션과 시스템 상태 벡터의 추정오차 간의 공분산 행렬을 계산한다. 본 발명 의 실시예에서, 이노베이션에 대한 공분산 행렬 S k 및 상기 이노베이션과 추정오차 사이의 공분산 행렬 P xv는 각 시그마 포인트를 사용하여 다음 수학식들에 의해 계산된다.
Figure 112007035052613-pat00043
Figure 112007035052613-pat00044
상기 수학식들에 의하여 계산된 S k 와 P xv 를 이용하여 m 번째 물체에 대한 칼만 이득(gain) K를 다음 수학식에 의하여 계산한다..
Figure 112007035052613-pat00045
칼만 이득이 계산되면, k+1 시간에 대하여 갱신된 시스템 상태 벡터 및 공분산 행렬은 칼만 이득 및 시스템 내의 실제 측정값 전체와 예측된 측정값 전체의 차이를 표시하는 이노베이션
Figure 112007035052613-pat00046
를 사용하여 다음 수학식들에 의하여 계산된다.
Figure 112007035052613-pat00047
Figure 112007035052613-pat00048
전술한 과정에 의하여 시그마 포인트를 계산하고 시스템의 상태를 표시하는 시스템 상태 벡터 및 공분산 행렬을 측정, 예측 및 갱신하는 단계를 반복함으로써, 적은 오차로 주변 환경에 대한 정보를 얻는 동시에 AUV의 위치를 측정할 수 있다.
지도 크기 관리
분석 수단(11)의 연산 부하를 줄이기 위해서는, 시스템 상태 벡터 및 시스템 상태 벡터의 공분산 행렬로 이루어진 지도가 일정한 크기로 관리되는 것이 바람직하다. 본 발명의 일 실시예에서는, 지도가 소정의 크기에 도달하는 경우 AUV에 대한 정보만을 이용하여 새로운 지도를 작성함으로써 지도의 크기를 일정하게 유지한다(S207). 전체 환경에 대한 지도와 달리, 미리 설정된 크기 범위 내로 관리되는 지도를 서브맵(submap)이라고 한다. 서브맵은 서브맵이 생성될 당시 AUV 위치를 중심으로 하여 미리 정해진 반지름을 가지는 원(2차원) 또는 구(3차원)가 된다. 즉, 서브맵 내에 최초로 기록된 AUV의 위치가 서브맵의 중심이 된다.
AUV의 이동에 따라, 기존에 속해 있던 서브맵의 중심으로부터 AUV까지의 거리가 미리 정해진 반지름보다 커지면, 분석 수단(11)은 AUV가 기존 서브맵에서 떠 난 것으로 판단한다. 이 경우 분석 수단(11)은 기존 서브맵에 대한 정보를 분석 수단(11) 내의 저장 장치에 별도로 저장한다. 다음으로, 현재의 AUV의 위치가 저장 장치 내에 저장되어 있는 기존 서브맵에 포함되는지 여부를 확인한다. 포함되는 경우에는 AUV가 기존 서브맵으로 진입하는 것으로 보아 기존 서브맵을 저장 장치로부터 읽어들여 다시 사용하고, 그렇지 않을 경우에는 전술한 방법으로 새로운 서브맵을 생성한다. 생성되는 서브맵은 AUV 및 AUV로부터 소정의 거리 이내에 존재하는 물체들의 위치를 포함하는 시스템 벡터와 상기 시스템 벡터의 공분산으로 표현되는 확률 분포이다.
서브맵을 사용하는 SLAM 방법의 일 실시예가, 2004년 출간되어 "서브맵 네트워크를 이용한 효율적인 동시 위치 추정 및 지도 작성 알고리즘(Efficient Simultaneous Localization and Mapping Algorithm Using Submap Networks)"의 제목을 가지는 S.J.Kim의 논문에 기술되어 있으므로, 본 명세서에서는 자세한 설명을 생략한다.
실제 실험 결과
도 3a 및 도 3b는 종래 기술에 따른 확장 칼만 필터를 사용한 동시 위치 추정 및 지도 작성 방법을 사용하여 AUV에서 물체의 위치를 측정한 결과를 도시한 그래프이다. AUV는 평평한 양 측벽의 가운데에서 운행하면서 양 측벽의 위치를 측정하도록 실험되었다. 도 3a는 xy평면에서 측정된 AUV 및 물체 측정치를 도시하고 있다. 그래프(31)에 도시되는 바와 같이 AUV는 x축 방향으로 진행하던 중 y축 방향으 로 변위를 가지는 것을 알 수 있다. 실험에서 양 측벽은 평평하게 구성되어 x축 방향에 평행하게 측정되어야 하나, AUV의 운동에 따라서 측정값에 오차가 생겨 AUV의 y축 변위에 비례하는 곡선(32, 33)의 형태로 나타난 것을 알 수 있다. 도 3b는 xz 평면에서 측정된 값을 도시하고 있는데, 전술한 xy평면과 마찬가지로 AUV의 운동 변위를 나타낸 곡선(34)에 따라 벽면의 위치를 측정한 곡선(35)에도 오차가 나타나는 것을 알 수 있다.
도 4a 및 도 4b는 본 발명의 실시예에 따른 동시 위치 추정 및 지도 작성 방법을 사용하여 AUV에서 양 측벽의 위치를 측정한 결과를 도시한 그래프이다. 도 4a에 도시되는 바와 같이, AUV의 운동은 xy평면에서 x축 방향으로 진행하다가 y축 방향으로 변위를 갖는 곡선(41)을 나타낸다. 그러나 종래 기술과 달리, AUV가 운동하더라도 AUV에서 측정된 양 측벽의 위치를 도시하는 곡선(42, 43)은 x축과 거의 평행 함을 알 수 있다. 도 4b는 xz 평면에서 측정된 결과를 도시한다. 마찬가지로, AUV의 운동 변위를 나타낸 곡선(44)은 z축 방향으로 변위를 가지지만, AUV에서 측정된 측벽의 위치를 도시한 곡선(45)은 x축과 거의 평행한 것을 알 수 있다.
따라서, 도 3a 및 도 3b에 도시된 확장 칼만 필터를 사용한 종래 기술에 비교할 때, 본 발명에 따른 동시 위치 추정 및 지도 작성 방법은 AUV의 비선형적인 운동에 불구하고 물체의 위치를 적은 오차를 가지고 측정할 수 있는 이점이 있다는 것을 알 수 있다.
이상 본 발명의 특정 실시예를 도시하고 설명하였으나, 본 발명의 기술사상은 첨부된 도면과 상기한 설명내용에 한정하지 않으며 본 발명의 사상을 벗어나지 않는 범위 내에서 다양한 형태의 변형이 가능함은 이 분야의 통상의 지식을 가진 자에게는 자명한 사실이며, 이러한 형태의 변형은, 본 발명의 정신에 위배되지 않는 범위 내에서 본 발명의 특허청구범위에 속한다고 볼 것이다.　
본 발명에 따른 동시 위치 추정 및 지도 작성 시스템 및 방법을 사용하여, AUV가 운항하는 환경의 비선형성으로 인한 오차 발생을 최소화할 수 있으며, AUV가 운항하는 해역을 몇 개의 서브맵으로 분할하여 계산함으로써 연산 부하를 줄일 수 있는 이점이 있다.

Claims (9)

  1. 미리 설정된 주기마다 이동체의 운항 환경에 존재하는 물체들의 위치를 측정하는 측정 수단; 및
    상기 측정 수단과 송수신하며, 측정된 상기 물체들의 위치를 이용하여 상기 이동체의 위치를 측정하고, 이전 주기에 측정된 상기 이동체의 위치 및 상기 물체들의 위치를 포함하는 확률 분포로부터 가우시안(Gaussian) 분포를 샘플링하기 위해 설정된 시그마 포인트(sigma point)를 사용하여 현 주기의 상기 확률 분포를 계산하고, 계산된 상기 확률 분포 및 상기 측정 수단에서 현 주기에 측정된 상기 물체들의 위치를 이용하여 상기 확률 분포를 갱신하는 분석 수단을 포함하는 것을 특징으로 하는 지도 작성 시스템.
  2. 제 1항에 있어서,
    상기 측정 수단은,
    상기 이동체로부터 음파를 송신하고 상기 물체들에 반사된 상기 음파를 수신하여, 상기 이동체로부터 상기 물체들까지의 거리 및 방위각을 측정하는 소나(Sonar)를 포함하는 것을 특징으로 하는 지도 작성 시스템.
  3. 제 1항에 있어서,
    상기 분석 수단에서 수신한 신호를 발진 신호로 변환하여 상기 측정 수단에 전달하는 발진 회로;
    상기 측정 수단에서 수신한 신호를 증폭하는 증폭 회로; 및
    상기 증폭 회로에서 수신한 신호를 변환하여 상기 분석 수단에 전달하는 A/D 변환기를 더 포함하는 것을 특징으로 하는 지도 작성 시스템.
  4. 제 1항에 있어서,
    상기 분석 수단은,
    상기 확률 분포에 최초로 기록된 상기 이동체의 위치로부터 현 주기의 상기 이동체의 위치까지의 거리가 미리 설정된 거리 이상일 경우 상기 확률 분포를 저장하고, 현 주기의 상기 이동체의 위치를 포함하는 상기 확률 분포가 존재할 경우 상기 확률 분포를 읽어오기 위한 저장 장치를 포함하는 것을 특징으로 하는 지도 작성 시스템.
  5. 이전 주기에 측정된, 이동체의 위치 및 상기 이동체의 운항 환경에 존재하는 물체들의 위치를 포함하는 확률 분포로부터, 가우시안 분포를 샘플링하기 위한 시그마 포인트를 설정하는 단계 (a);
    상기 시그마 포인트를 사용하여 현 주기의 상기 확률 분포 및 상기 이동체에서 상기 물체들의 위치를 측정한 측정값을 계산하는 단계 (b);
    현 주기에 상기 이동체에서 상기 물체들의 위치를 측정한 실제 측정값을 구하는 단계 (c); 및
    상기 단계 (b)에서 계산된 상기 확률 분포 및 상기 측정값과, 상기 단계 (c)에서 구한 상기 실제 측정값을 사용하여 상기 확률 분포를 갱신하는 단계 (d)를 포함하되, 미리 설정된 주기마다 상기 단계 (a) 내지 상기 단계 (d)를 반복하는 것을 특징으로 하는 지도 작성 방법.
  6. 제 5항에 있어서,
    상기 단계 (b)는,
    상기 이동체의 이동에 따른 상기 시그마 포인트의 변화를 계산하는 단계; 및
    계산된 상기 시그마 포인트를 결합하여 현 주기의 상기 확률 분포 및 상기 측정값을 계산하는 단계를 포함하는 것을 특징으로 하는 지도 작성 방법.
  7. 제 5항에 있어서,
    상기 단계 (c)는,
    상기 이동체로부터 음파를 송신하는 단계;
    상기 물체들에 반사되어 돌아오는 상기 음파를 수신하는 단계; 및
    수신된 상기 음파를 통하여 상기 이동체로부터 상기 물체들까지의 거리 및 방위각을 계산하는 단계를 포함하는 것을 특징으로 하는 지도 작성 방법.
  8. 제 5항에 있어서,
    상기 확률 분포에 최초로 기록된 상기 이동체의 위치로부터 현 주기의 상기 이동체의 위치까지의 거리가 미리 설정된 거리 이상일 경우, 상기 확률 분포를 저장 장치에 저장하는 단계;
    상기 저장 장치 내에 현 주기의 상기 이동체의 위치를 포함하는 상기 확률 분포가 존재할 경우 상기 확률 분포를 상기 저장 장치로부터 읽어오는 단계; 및
    현 주기의 상기 이동체의 위치를 포함하는 상기 확률 분포가 존재하지 않을 경우, 상기 이동체의 위치 및 상기 이동체로부터 상기 미리 설정된 거리 이내에 존재하는 상기 물체들의 위치를 포함하는 상기 확률 분포를 생성하는 단계를 더 포함하는 것을 특징으로 하는 지도 작성 방법.
  9. 제 5항에 있어서,
    상기 단계 (b)에서 계산된 상기 측정값과 상기 단계 (c)에서 구한 상기 실제 측정값을 사용하여, 측정된 물체가 처음 측정된 물체인지 판단하는 단계; 및
    상기 측정된 물체가 처음 측정된 물체로 판단될 경우, 상기 처음 측정된 물체의 위치를 상기 확률 분포에 추가하는 단계를 더 포함하는 것을 특징으로 하는 지도 작성 방법.
KR1020070046015A 2007-05-11 2007-05-11 무향 칼만 필터를 이용한 동시 위치 추정 및 지도 작성시스템 및 방법 KR100879701B1 (ko)

Priority Applications (1)

Application Number Priority Date Filing Date Title
KR1020070046015A KR100879701B1 (ko) 2007-05-11 2007-05-11 무향 칼만 필터를 이용한 동시 위치 추정 및 지도 작성시스템 및 방법

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
KR1020070046015A KR100879701B1 (ko) 2007-05-11 2007-05-11 무향 칼만 필터를 이용한 동시 위치 추정 및 지도 작성시스템 및 방법

Publications (2)

Publication Number Publication Date
KR20080100004A KR20080100004A (ko) 2008-11-14
KR100879701B1 true KR100879701B1 (ko) 2009-01-21

Family

ID=40286817

Family Applications (1)

Application Number Title Priority Date Filing Date
KR1020070046015A KR100879701B1 (ko) 2007-05-11 2007-05-11 무향 칼만 필터를 이용한 동시 위치 추정 및 지도 작성시스템 및 방법

Country Status (1)

Country Link
KR (1) KR100879701B1 (ko)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107544256A (zh) * 2017-10-17 2018-01-05 西北工业大学 基于自适应反步法的水下机器人滑模控制方法
KR20210055982A (ko) 2019-11-08 2021-05-18 대양전기공업 주식회사 수중 운동체와 수중 음향 탐지 장비를 이용한 수중 환경 지도 작성 시스템 및 그 방법

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR100931004B1 (ko) * 2008-11-27 2009-12-10 (주)미도지리정보 지리정보 및 지형정보에 따른 데이터의 오차 여부를 확인 및 갱신해서 수치지도의 정밀성을 향상시킨 수치지도 제작시스템
CN109905846B (zh) * 2019-02-18 2020-09-15 天津城建大学 一种基于自主水下航行器的水下无线传感器网络定位方法
CN110320520B (zh) * 2019-06-26 2022-08-02 哈尔滨工程大学 一种测深信息同步定位与建图的鲁棒后端图优化方法
CN113900062A (zh) * 2021-12-09 2022-01-07 浙江大学 一种auv水下回收电磁导引装置
CN117856904B (zh) * 2023-12-12 2024-09-24 山东科技大学 一种基于深度强化学习的多auv协同移动光通信方法

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR20070026912A (ko) * 2005-08-27 2007-03-09 한국과학기술원 입자 필터 프레임 워크에서 전방향 시각센서 기반 위치추정 및 매핑을 위한 방법
US20070106423A1 (en) 2005-11-07 2007-05-10 Samsung Electronics Co. Ltd. Robot and method of localizing the same
KR100816269B1 (ko) 2006-09-22 2008-03-25 학교법인 포항공과대학교 언센티드 필터를 적용한 강인한 동시 위치 추정 및 지도작성 방법

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR20070026912A (ko) * 2005-08-27 2007-03-09 한국과학기술원 입자 필터 프레임 워크에서 전방향 시각센서 기반 위치추정 및 매핑을 위한 방법
US20070106423A1 (en) 2005-11-07 2007-05-10 Samsung Electronics Co. Ltd. Robot and method of localizing the same
KR100816269B1 (ko) 2006-09-22 2008-03-25 학교법인 포항공과대학교 언센티드 필터를 적용한 강인한 동시 위치 추정 및 지도작성 방법

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107544256A (zh) * 2017-10-17 2018-01-05 西北工业大学 基于自适应反步法的水下机器人滑模控制方法
CN107544256B (zh) * 2017-10-17 2020-02-14 西北工业大学 基于自适应反步法的水下机器人滑模控制方法
KR20210055982A (ko) 2019-11-08 2021-05-18 대양전기공업 주식회사 수중 운동체와 수중 음향 탐지 장비를 이용한 수중 환경 지도 작성 시스템 및 그 방법

Also Published As

Publication number Publication date
KR20080100004A (ko) 2008-11-14

Similar Documents

Publication Publication Date Title
KR100879701B1 (ko) 무향 칼만 필터를 이용한 동시 위치 추정 및 지도 작성시스템 및 방법
Jaulin A nonlinear set membership approach for the localization and map building of underwater robots
EP2098882A2 (en) Location measurement method based on predictive filter
JP5460148B2 (ja) 測位装置及びプログラム
Karimi et al. A comparison of DVL/INS fusion by UKF and EKF to localize an autonomous underwater vehicle
US20160124081A1 (en) Metrology method and device for calibrating the geometry of a network of underwater acoustic beacons
CN103697910A (zh) 自主水下航行器多普勒计程仪安装误差的校正方法
CN110132308B (zh) 一种基于姿态确定的usbl安装误差角标定方法
KR20080072628A (ko) 차량 운전 보조 방법 및 이를 이용한 장치
KR20180091372A (ko) 레이더의 표적 위치 추적 방법
US6819984B1 (en) LOST 2—a positioning system for under water vessels
KR101833007B1 (ko) 도플러 비컨을 이용한 수중 이동체의 위치 및 속도 추정 방법 및 시스템
CN114265047B (zh) 一种大潜深auv的定位基阵联合标校方法
Kinsey et al. Towards in-situ calibration of gyro and doppler navigation sensors for precision underwater vehicle navigation
KR101303417B1 (ko) 정보 처리 장치, 정보 처리 방법 및 기록 매체
CN110703205B (zh) 基于自适应无迹卡尔曼滤波的超短基线定位方法
Quintas et al. A comparison of nonlinear filters for underwater geomagnetic navigation
Nygren Robust and efficient terrain navigation of underwater vehicles
KR20230115027A (ko) Uwb 및 amcl 기반 모바일 로봇의 실내 위치 측위 정확도를 높이기 위한 장치 및 방법
Pan et al. AUV Tightly Coupled Terrain Aided Navigation Strategy Based on Isogonal MBES Modeling Method
CN116929343A (zh) 位姿估计方法、相关设备及存储介质
KR100936710B1 (ko) 주행기록계와 방향센서를 이용한 이동체의 위치 결정 방법,시스템 및 그 방법을 기록한 기록매체
Proletarsky et al. Method for improving accuracy of INS using scalar parametric identification
KR20220089392A (ko) 무인 수상선의 지구물리적 내비게이션 장치 및 방법
KR101217138B1 (ko) 이동체의 위치 추정 장치 및 방법

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
FPAY Annual fee payment

Payment date: 20130124

Year of fee payment: 5

FPAY Annual fee payment

Payment date: 20131231

Year of fee payment: 6

FPAY Annual fee payment

Payment date: 20150106

Year of fee payment: 7

FPAY Annual fee payment

Payment date: 20151224

Year of fee payment: 8

FPAY Annual fee payment

Payment date: 20161227

Year of fee payment: 9

FPAY Annual fee payment

Payment date: 20181101

Year of fee payment: 11

FPAY Annual fee payment

Payment date: 20191202

Year of fee payment: 12