KR101562042B1 - 수중 로봇의 위치 추정 방법 및 장치 - Google Patents

수중 로봇의 위치 추정 방법 및 장치 Download PDF

Info

Publication number
KR101562042B1
KR101562042B1 KR1020140060237A KR20140060237A KR101562042B1 KR 101562042 B1 KR101562042 B1 KR 101562042B1 KR 1020140060237 A KR1020140060237 A KR 1020140060237A KR 20140060237 A KR20140060237 A KR 20140060237A KR 101562042 B1 KR101562042 B1 KR 101562042B1
Authority
KR
South Korea
Prior art keywords
vector
underwater robot
value
error covariance
extended state
Prior art date
Application number
KR1020140060237A
Other languages
English (en)
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 KR1020140060237A priority Critical patent/KR101562042B1/ko
Application granted granted Critical
Publication of KR101562042B1 publication Critical patent/KR101562042B1/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
    • G01S1/00—Beacons or beacon systems transmitting signals having a characteristic or characteristics capable of being detected by non-directional receivers and defining directions, positions, or position lines fixed relatively to the beacon transmitters; Receivers co-operating therewith
    • G01S1/02—Beacons or beacon systems transmitting signals having a characteristic or characteristics capable of being detected by non-directional receivers and defining directions, positions, or position lines fixed relatively to the beacon transmitters; Receivers co-operating therewith using radio waves
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01P—MEASURING LINEAR OR ANGULAR SPEED, ACCELERATION, DECELERATION, OR SHOCK; INDICATING PRESENCE, ABSENCE, OR DIRECTION, OF MOVEMENT
    • G01P3/00—Measuring linear or angular speed; Measuring differences of linear or angular speeds
    • G01P3/42—Devices characterised by the use of electric or magnetic means

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)
  • Manipulator (AREA)

Abstract

본 발명은 수중 로봇의 위치 추정 방법 및 장치에 관한 것으로, 본 발명에 따른 방법은 미리 정해진 시각(t)에 측정된 수중 로봇의 내수용성 센서 측정 정보를 이용하여 수중 로봇의 위치, 수중 로봇의 방향각 및 해수면의 높이를 포함하는 상태 변수의 예측 값 및 예측 오차 공분산을 계산하는 제1 단계, 그리고 수중 로봇의 외수용성 센서 측정 정보 및 미리 정해진 지형 지도에 대한 정보를 이용하여 상태 변수의 예측 값 및 예측 오차 공분산을 보정하여 상태 변수의 추정 값 및 추정 오차 공분산을 계산하는 제2 단계를 포함한다.

Description

수중 로봇의 위치 추정 방법 및 장치{Method and Apparatus for Estimating Position of Underwater Robot}
본 발명은 수중 로봇의 위치 추정 방법 및 장치에 관한 것이다.
일반적으로 수중 로봇은 운영 방식에 따라 자율 수중 이동체(Autonomous Underwater Vehicle: AUV)와 원격 조작 이동체(Remotely Operated Underwater Vehicle: ROV)로 구분되며, 심해 자원 조사, 수중 구조물 건설, 수중 조사 관측, 선박 선저 청소나 항만 청소 등과 같은 수중 작업, 수산업, 기뢰 탐색/제거, 무인 항만 감시나 수중 정찰 등과 같은 군사적 용도로 사용되고 있다. 이러한 작업을 수행하기 위해서는 수중 로봇의 위치를 알아내는 위치 추정 기술이 무엇보다 중요하다.
종래에는 수중 로봇의 위치를 추정하기 위해서 LBL(Long Base Line), SBL(Short Base Line) 또는 USBL(Ultra Short Base Line) 등과 같은 시스템을 사용하였다. LBL을 이용한 위치 추정 시스템은 통상적으로 수중 로봇에 하나의 송수신부를 설치하고, 해저면에 4대의 발신부를 설치하여 송수신부와 발신부 간에 초음파 신호를 주고받아서 수중 로봇의 위치를 추정할 수 있다. SBL 및 USBL을 이용한 위치 추정 시스템은 선체에서 줄에 매달린 발신부를 물속으로 내려 발신부와 수중 로봇에 설치된 송수신부 간에 초음파 신호를 주고받아서 위치를 추정할 수 있다.
하지만, 종래의 LBL, SBL 또는 USBL를 이용한 위치 추정 시스템은 조수 간만, 강우, 강물 유입 등과 같이 각종 요인에 의해 변화하는 해수면의 높이나 수중 로봇에서 해저면까지의 거리 등을 정확하게 반영하지 못하는 문제점이 있었다.
이와 같이, 해수면의 높이나 수중 로봇에서 해저면까지의 거리 등에 포함된 오차를 정확하게 반영하지 못하기 때문에 수중 로봇의 위치 추정 성능이 저하되는 문제점이 있었다.
한국등록특허 제10-0969878호 (등록일: 2010.07.06.)
따라서 본 발명이 해결하고자 하는 기술적 과제는 내수용성 센서의 측정 정보를 이용하여 수중 로봇의 위치, 수중 로봇의 방향각 및 해수면의 높이를 포함하는 상태 변수의 값과 오차를 예측하고, 외수용성 센서의 측정 정보 및 미리 정해진 지형 지도에 대한 정보를 이용하여 예측된 상태 변수의 값과 오차를 보정함으로써 수중 로봇의 위치 추정 성능을 향상시킬 수 있는 수중 로봇의 위치 추정 방법 및 장치를 제공하는 것이다.
또한, 본 발명은 명시적으로 언급된 목적 이외에도, 후술하는 본 발명의 구성으로부터 달성될 수 있는 다른 목적도 포함한다.
상기한 과제를 해결하기 위한 본 발명의 실시예에 따른 수중 로봇의 위치 추정 방법은 미리 정해진 시각(t)에 측정된 수중 로봇의 내수용성 센서의 측정 정보를 이용하여 상기 수중 로봇의 위치, 상기 수중 로봇의 방향각 및 해수면의 높이를 포함하는 상태 변수의 예측 값 및 예측 오차 공분산을 계산하는 제1 단계, 그리고 상기 수중 로봇의 외수용성 센서의 측정 정보 및 미리 정해진 지형 지도에 대한 정보를 이용하여 상기 상태 변수의 예측 값 및 상기 예측 오차 공분산을 보정하여 상기 상태 변수의 추정 값 및 추정 오차 공분산을 계산하는 제2 단계를 포함한다.
상기 제1 단계는 미리 정해진 이전 시각(t-1)에 계산된 상기 상태 변수의 추정 값 및 상기 추정 오차 공분산과, 상기 미리 정해진 시각(t)에 측정된 상기 수중 로봇의 내수용성 센서의 측정 정보를 이용하여 상기 상태 변수의 예측 값 및 상기 예측 오차 공분산을 계산하는 단계를 포함할 수 있다.
상기 제1 단계는 미리 정해진 이전 시각(t-1)에 계산된 상기 상태 변수의 추정 값을 이용하여 상기 미리 정해진 이전 시각(t-1)에서의 확장 상태 벡터 및 상기 확장 상태 벡터의 오차 공분산을 구하는 단계를 포함하고, 상기 확장 상태 벡터는 상기 상태 변수, 상기 내수용성 센서의 측정 오차 정보 및 상기 외수용성 센서의 측정 오차 정보를 포함하는 확장 상태 변수들로 이루어질 수 있다.
상기 제1 단계는 상기 확장 상태 벡터 및 상기 확장 상태 벡터의 오차 공분산을 이용하여 상기 확장 상태 변수들이 평균값들을 중심으로 어떻게 분포되어 있는지를 나타내는 확장 상태 분포 벡터를 생성하는 단계를 포함할 수 있다.
상기 제1 단계는 상기 확장 상태 분포 벡터를 이용하여 상기 수중 로봇이 상기 시각(t)에 어느 곳에 위치하고 상기 해수면의 높이가 어떤 값을 가질지 나타낼 수 있는 예측 벡터를 구하는 단계, 그리고 상기 예측 벡터를 기반으로 평균 예측 벡터 및 상기 예측 벡터의 오차 공분산을 구하는 단계를 포함할 수 있다.
상기 제2 단계는 상기 예측 벡터를 이용하여 상기 외수용성 센서의 측정 예측 값 및 상기 측정 예측 값의 오차 공분산을 구하는 단계, 그리고 상기 예측 벡터 및 상기 외수용성 센서의 측정 예측 값의 교차 공분산을 구하는 단계를 포함할 수 있다.
상기 제2 단계는 상기 측정 예측 값의 오차 공분산 및 상기 교차 공분산을 이용하여 이득 벡터를 구하는 단계를 포함할 수 있다.
상기 제2 단계는 상기 외수용성 센서의 측정 정보, 상기 측정 예측 값 및 상기 이득 벡터를 이용하여 상기 상태 변수의 예측 값 및 상기 예측 오차 공분산을 보정하여 상기 상태 변수의 추정 값 및 추정 오차 공분산을 계산하는 단계를 포함할 수 있다.
한편, 본 발명의 실시예에 따른 수중 로봇의 위치 추정 장치는 미리 정해진 시각(t)에 측정된 수중 로봇의 내수용성 센서의 측정 정보를 이용하여 상기 수중 로봇의 위치, 상기 수중 로봇의 방향각 및 해수면의 높이를 포함하는 상태 변수의 예측 값 및 예측 오차 공분산을 계산하는 예측부, 그리고 상기 수중 로봇의 외수용성 센서의 측정 정보 및 미리 정해진 지형 지도에 대한 정보를 이용하여 상기 상태 변수의 예측 값 및 상기 예측 오차 공분산을 보정하여 상기 상태 변수의 추정 값 및 추정 오차 공분산을 계산하는 보정부를 포함한다.
상기 예측부는 미리 정해진 이전 시각(t-1)에 계산된 상기 상태 변수의 추정 값 및 상기 추정 오차 공분산과, 상기 미리 정해진 시각(t)에 측정된 수중 로봇의 내수용성 센서의 측정 정보를 이용하여 상기 상태 변수의 예측 값 및 상기 예측 오차 공분산을 계산할 수 있다.
상기 예측부는 미리 정해진 이전 시각(t-1)에 계산된 상기 상태 변수의 추정 값을 이용하여 상기 미리 정해진 이전 시각(t-1)에서의 확장 상태 벡터 및 상기 확장 상태 벡터의 오차 공분산을 구하는 확장 상태 벡터 계산부를 포함하고, 상기 확장 상태 벡터는 상기 상태 변수, 상기 내수용성 센서의 측정 오차 정보 및 상기 외수용성 센서의 측정 오차 정보를 포함하는 확장 상태 변수들로 이루어질 수 있다.
상기 예측부는 상기 확장 상태 벡터 및 상기 확장 상태 벡터의 오차 공분산을 이용하여 상기 확장 상태 변수들이 평균값들을 중심으로 어떻게 분포되어 있는지를 나타내는 확장 상태 분포 벡터를 생성하는 확장 상태 분포 벡터 생성부를 포함할 수 있다.
상기 예측부는 상기 확장 상태 분포 벡터를 이용하여 상기 수중 로봇이 상기 시각(t)에 어느 곳에 위치하고 상기 해수면의 높이가 어떤 값을 가질지 나타낼 수 있는 예측 벡터를 구하는 제1 예측 벡터 계산부, 그리고 상기 예측 벡터를 기반으로 평균 예측 벡터 및 상기 예측 벡터의 오차 공분산을 구하는 제2 예측 벡터 계산부를 포함할 수 있다.
상기 보정부는 상기 예측 벡터를 이용하여 상기 외수용성 센서의 측정 예측 값 및 상기 측정 예측 값의 오차 공분산을 구하는 측정 예측 값 계산부, 그리고 상기 예측 벡터 및 상기 외수용성 센서의 측정 예측 값의 교차 공분산을 구하는 교차 공분산 계산부를 포함할 수 있다.
상기 보정부는 상기 측정 예측 값의 오차 공분산 및 상기 교차 공분산을 이용하여 이득 벡터를 구하는 이득 벡터 계산부를 포함할 수 있다.
상기 보정부는 상기 외수용성 센서의 측정 정보, 상기 측정 예측 값 및 상기 이득 벡터를 이용하여 상기 상태 변수의 예측 값 및 상기 예측 오차 공분산을 보정하여 상기 상태 변수의 추정 값 및 추정 오차 공분산을 계산하는 상태 변수 보정부를 포함할 수 있다.
이와 같이 본 발명의 실시예에 따른 수중 로봇의 위치 추정 방법 및 장치에 따르면, 내수용성 센서의 측정 정보를 이용하여 수중 로봇의 위치, 수중 로봇의 방향각 및 해수면의 높이를 포함하는 상태 변수의 값과 오차를 예측하고, 외수용성 센서의 측정 정보 및 미리 정해진 지형 지도에 대한 정보를 이용하여 예측된 상태 변수의 값과 오차를 보정함으로써 수중 로봇의 위치 추정 성능을 향상시킬 수 있다.
또한, 해수면이 변화할 때 수중 로봇의 위치와 해수면 높이를 동시에 추정하는 방식을 사용하여 수중 로봇의 위치만 추정하는 방식에 비해 추정 궤적의 거리 오차를 줄일 수 있다.
한편, 본 발명의 효과는 상술된 것에 국한되지 않고 후술하는 본 발명의 구성으로부터 도출될 수 있는 다른 효과도 본 발명의 효과에 포함된다.
도 1은 본 발명의 실시예에 따른 수중 로봇의 위치 추정 방법이 적용되는 장치의 일 예를 설명하기 위해 참조되는 도면이다.
도 2는 본 발명의 실시예에 따른 위치 추정 방법 및 장치가 적용되는 수중 로봇을 개략적으로 도시한 도면이다.
도 3은 본 발명의 일 실시예에 따른 수중 로봇의 위치를 추정하는 방법을 개략적으로 설명하기 위한 흐름도이다.
도 4a 및 도 4b는 본 발명의 일 실시예에 따른 수중 로봇의 위치를 추정하는 방법을 상세하게 설명하기 위한 흐름도이다.
도 5는 본 발명의 실시예에 따른 수중 로봇의 위치 및 해수면의 높이를 추정하기 위해 구축된 시뮬레이션 환경을 나타내는 예이다.
도 6은 도 5의 시뮬레이션 환경에서 사용되는 해저 지형도의 예이다.
도 7은 해수면의 높이가 변할 경우, 수중 로봇의 위치와 해수면의 높이를 동시에 추정하기 위한 예로, 도 7a는 수중 로봇의 이동 궤적을 나타내는 그래프, 도 7b는 해수면의 변화를 나타나는 그래프 및 도 7c는 수중 로봇의 오차를 나타내는 그래프이다.
도 8은 해수면의 높이가 변할 경우, 수중 로봇의 위치만 추정하기 위한 예로, 도 8a는 수중 로봇의 이동 궤적을 나타내는 그래프 및 도 8b는 수중 로봇의 오차를 나타내는 그래프이다.
본 명세서 및 청구범위에 사용된 용어나 단어는 통상적이거나 사전적인 의미로 한정해서 해석되어서는 아니 되며, 발명자는 그 자신의 발명을 가장 최선의 방법으로 설명하기 위해 용어의 개념을 적절하게 정의할 수 있다는 원칙에 입각하여 본 발명의 기술적 사상에 부합하는 의미와 개념으로 해석되어야만 한다.
따라서 본 명세서에 기재된 실시예와 도면에 도시된 구성은 본 발명의 가장 바람직한 실시예에 불과할 뿐이고 본 발명의 기술적 사상을 모두 대변하는 것은 아니므로, 본 출원시점에 있어서 이들을 대체할 수 있는 다양한 균등물과 변형 예들이 있을 수 있음을 이해하여야 한다.
그러면 첨부한 도면을 참고로 하여 본 발명의 실시예에 대하여 본 발명이 속하는 기술 분야에서 통상의 지식을 가진 자가 용이하게 실시할 수 있도록 상세히 설명한다.
도 1은 본 발명의 실시예에 따른 수중 로봇의 위치 추정 방법이 적용되는 장치의 일 예를 설명하기 위해 참조되는 도면이다.
도 1을 참조하면, 본 발명에 따른 장치는 예측부(100) 및 보정부(200)를 포함한다.
예측부(100)는 미리 정해진 시각(t)에 측정된 수중 로봇의 내수용성 센서의 측정 정보를 이용하여 수중 로봇의 위치, 수중 로봇의 방향각 및 해수면의 높이를 포함하는 상태 변수의 예측 값 및 예측 오차 공분산을 계산한다.
보다 자세하게는, 예측부(100)는 확장 상태 벡터 계산부(110), 확장 상태 분포 벡터 생성부(130), 제1 예측 벡터 계산부(150) 및 제2 예측 벡터 계산부(170)를 포함할 수 있다.
확장 상태 벡터 계산부(110)는 미리 정해진 이전 시각(t-1)에 계산된 상태 변수의 추정 값을 이용하여 이전 시각(t-1)에서의 확장 상태 벡터 및 확장 상태 벡터의 오차 공분산을 구한다. 여기서, 확장 상태 벡터는 상태 변수, 내수용성 센서의 측정 정보 및 외수용성 센서의 측정 정보를 포함하는 확장 상태 변수들로 이루어질 수 있다. 확장 상태 분포 벡터 생성부(130)는 확장 상태 벡터 및 확장 상태 벡터의 오차 공분산을 이용하여 확장 상태 변수들이 평균값들을 중심으로 어떻게 분포되어 있는지를 나타내는 확장 상태 분포 벡터를 생성하고, 제1 예측 벡터 계산부(150)는 확장 상태 분포 벡터를 이용하여 수중 로봇이 시각(t)에 어느 곳에 위치하고 해수면의 높이가 어떤 값을 가질지 나타낼 수 있는 예측 벡터를 구한다. 그리고, 제2 예측 벡터 계산부(170)는 예측 벡터를 기반으로 평균 예측 벡터 및 예측 벡터의 오차 공분산을 구한다.
보정부(200)는 수중 로봇의 외수용성 센서의 측정 정보 및 미리 정해진 지형 지도에 대한 정보를 이용하여 상태 변수의 예측 값 및 예측 오차 공분산을 보정하여 상태 변수의 추정 값 및 추정 오차 공분산을 계산한다.
보다 자세하게는, 보정부(200)는 측정 예측 값 계산부(210), 교차 공분산 계산부(230), 이득 벡터 계산부(250) 및 상태 변수 보정부(270)를 포함할 수 있다.
측정 예측 값 계산부(210)는 예측 벡터를 이용하여 외수용성 센서의 측정 예측 값 및 측정 예측 값의 오차 공분산을 구하고, 교차 공분산 계산부(230)는 예측 벡터 및 외수용성 센서의 측정 예측 값의 교차 공분산을 구한다. 이득 벡터 계산부(250)는 측정 예측 값의 오차 공분산 및 교차 공분산을 이용하여 이득 벡터를 구한다. 그리고, 상태 변수 보정부(270)는 외수용성 센서의 측정 정보, 측정 예측 값 및 이득 벡터를 이용하여 상태 변수의 예측 값 및 예측 오차 공분산을 보정하여 상태 변수의 추정 값 및 추정 오차 공분산을 계산하고, 계산된 상태 변수의 추정 값 및 추정 오차 공분산을 출력하거나 예측부(100)에 전달한다.
도 2는 본 발명의 실시예에 따른 위치 추정 방법 및 장치가 적용되는 수중 로봇을 개략적으로 도시한 도면이다.
도 1 및 도 2를 참조하면, 수중 로봇(10)은 내수용성 센서(11) 및 외수용성 센서(13)를 구비하고, 내수용성 센서(11) 및 외수용성 센서(13)를 이용하여 내수용성 센서의 측정 정보 및 외수용성 센서의 측정 정보를 각각 수신할 수 있다.
내수용성 센서(11)는 수중 로봇(10)의 내부 상태를 관측하여 수중 로봇(10)의 운동 상태를 알아내는 센서로, 보통 추진기의 회전 속도, 러더(rudder)나 엘리베이터(elevator) 등의 방향타 각도 정보와, 관성 측정 장치의 관성 측정 정보를 이용하여 수중 로봇(10)의 선속도 및 회전 각속도를 알아낼 수 있다.
외수용성 센서(13)는 수중 로봇(10)의 외부 환경과 관련된 신호를 이용하여 수중 로봇(10)의 운동 상태를 알아내는 센서로, 주로 외부 비컨(beacon)으로부터의 거리나 수중 로봇의 외부 환경으로부터의 거리를 측정하여 알아낼 수 있다.
내수용성 센서의 측정 벡터는
Figure 112014047162289-pat00001
로 표현되며, 내수용성 센서(11)에 의해 측정되는 3차원 수중 공간에서의 수중 로봇(10)의 선속도와 회전 각속도를 나타내는 변수들로 구성된다. 여기서,
Figure 112014047162289-pat00002
는 로봇 좌표계에서 수중 로봇의 X축 방향 선속도,
Figure 112014047162289-pat00003
는 로봇 좌표계에서 수중 로봇의 Y축 방향 선속도,
Figure 112014047162289-pat00004
는 로봇 좌표계에서 수중 로봇의 Z축 방향 선속도,
Figure 112014047162289-pat00005
는 로봇 좌표계에서 수중 로봇의 X축을 회전축으로 한 회전 각속도,
Figure 112014047162289-pat00006
는 로봇 좌표계에서 수중 로봇의 Y축을 회전축으로 한 회전 각속도,
Figure 112014047162289-pat00007
는 로봇 좌표계에서 수중 로봇의 Z축을 회전축으로 한 회전 각속도를 나타낸다.
외수용성 센서의 측정 벡터는
Figure 112014047162289-pat00008
로 표현되며, 외수용성 센서(13)에 의해 측정되는 3차원 수중 공간에서 수중 로봇(10)으로부터 해저면까지의 거리를 나타내는 4개의 변수와, 해수면으로부터 수중 로봇(10)까지의 깊이를 나타내는 1개의 변수들로 구성된다. 본 발명의 실시예에서는 4개의 거리를 측정하는 것으로 가정하며,
Figure 112014047162289-pat00009
는 1번째 거리 센서에 의해 측정된 수중 로봇에서 해저면까지의 거리,
Figure 112014047162289-pat00010
는 2번째 거리 센서에 의해 측정된 수중 로봇에서 해저면까지의 거리,
Figure 112014047162289-pat00011
는 3번째 거리 센서에 의해 측정된 수중 로봇에서 해저면까지의 거리,
Figure 112014047162289-pat00012
는 4번째 거리 센서에 의해 측정된 수중 로봇에서 해저면까지의 거리,
Figure 112014047162289-pat00013
는 깊이 센서에 의해 측정된 해수면에서 수중 로봇까지의 깊이를 나타낸다.
확장 상태 벡터는
Figure 112014047162289-pat00014
로 표현되며, 다음과 같은 18개의 상태 변수들로 이루어진다.
Figure 112014047162289-pat00015
여기서, 상태 벡터는
Figure 112014047162289-pat00016
로 표현되며, 수중 로봇(10)의 위치, 수중 로봇(10)의 방향각 및 해수면의 높이를 나타내는 상태 변수들로 이루어진다. 일반적으로 추정하고자 하는 상태 벡터는
Figure 112014047162289-pat00017
를 의미한다.
Figure 112014047162289-pat00018
는 3차원 지구 고정 좌표계에서 X축 방향의 수중 로봇 좌표,
Figure 112014047162289-pat00019
는 3차원 지구 고정 좌표계에서 Y축 방향의 수중 로봇 좌표,
Figure 112014047162289-pat00020
는 3차원 지구 고정 좌표계에서 Z축 방향의 수중 로봇 좌표,
Figure 112014047162289-pat00021
는 3차원 지구 고정 좌표계에서 X축 중심의 수중 로봇 회전각,
Figure 112014047162289-pat00022
는 3차원 지구 고정 좌표계에서 Y축 중심의 수중 로봇 회전각,
Figure 112014047162289-pat00023
는 3차원 지구 고정 좌표계에서 Z축 중심의 수중 로봇 회전각,
Figure 112014047162289-pat00024
는 해수면의 높이를 나타내는 상태 변수이다.
내수용성 센서의 측정 오차 벡터는
Figure 112014047162289-pat00025
로 표현되며, 내수용성 센서(11)에 의해 측정된 3차원 수중 공간에서 수중 로봇(10)의 선속도와 회전 각속도에 포함되어 있는 오차(잡음)를 나타내는 변수들로 이루어진다.
외수용성 센서의 측정 오차 벡터는
Figure 112014047162289-pat00026
로 표현되며, 외수용성 센서(13)에 의해 측정된 4개의 거리 정보와, 해수면에서 수중 로봇(10)까지의 깊이에 포함되어 있는 오차(잡음)를 나타내는 변수들로 이루어진다.
그러면, 이와 같은 구성을 기반으로 본 발명의 실시예에 따른 수중 로봇의 위치 추정 방법에 대하여 보다 상세하게 설명한다.
도 3은 본 발명의 일 실시예에 따른 수중 로봇의 위치를 추정하는 방법을 개략적으로 설명하기 위한 흐름도이고, 도 4a 및 도 4b는 본 발명의 일 실시예에 따른 수중 로봇의 위치를 추정하는 방법을 상세하게 설명하기 위한 흐름도이다.
도 3 및 도 4a를 참조하면, 미리 정해진 시각(t)에 측정된 수중 로봇의 내수용성 센서의 측정 정보를 이용하여 수중 로봇의 위치, 수중 로봇의 방향각 및 해수면의 높이를 포함하는 상태 변수의 예측 값 및 예측 오차 공분산을 계산한다(S300).
보다 자세하게는, 미리 정해진 시각(t)에서 수중 로봇의 내수용성 센서의 측정 정보(수중 로봇의 선속도 및 회전 각속도)에 대한 오차 공분산 행렬(Mt)은 다음 [수학식 1]과 같이 구한다(S301).
[수학식 1]
Figure 112014047162289-pat00027
여기서,
Figure 112014047162289-pat00028
로, 오차 공분산 행렬(Mt)은 수중 로봇의 선속도 및 회전 각속도의 제곱 값들에 대한 선형 결합으로 나타내어지고, 선형 결합의 가중치
Figure 112014047162289-pat00029
는 오차가 큰 내수용성 센서를 사용하는 경우에는 큰 값을 가지며, 오차가 작은 정밀한 내수용성 센서를 사용하는 경우에는 작은 값을 가질 수 있다.
그리고, 시각(t)에서 수중 로봇의 외수용성 센서의 측정 정보(수중 로봇으로부터 해저면까지의 거리와, 해수면으로부터 수중 로봇까지의 깊이)에 대한 오차 공분산 행렬(Qt)은 다음 [수학식 2]와 같이 구한다(S302).
[수학식 2]
Figure 112014047162289-pat00030
여기서, 오차 공분산 행렬(Qt)은 각각의 외수용성 센서에 의해 여러 번 실험하여 측정된 값들을 확률적으로 처리한 후, 구해진 분산 값을 사용하여 계산되는데, 오차가 큰 외수용성 센서에 의해 측정이 이루어지면 대각선 요소 값을 크게 설정하고, 오차가 작은 정밀한 외수용성 센서에 의해 측정이 이루어지면 대각선 요소 값을 작게 설정할 수 있다.
다음으로, 이전 시각(t-1)에 계산된 상태 변수의 추정 값을 이용하여 이전 시각(t-1)에서의 확장 상태 벡터를 다음 [수학식 3]과 같이 구한다(S303).
[수학식 3]
Figure 112014047162289-pat00031
여기서, 확장 상태 벡터는 상태 변수, 내수용성 센서의 측정 오차 정보 및 외수용성 센서의 측정 오차 정보를 포함하는 확장 상태 변수들로 이루어지고, 내수용성 센서와 외수용성 센서의 오차(잡음)의 추정 값은 “0”으로 설정한다. 수중 로봇의 위치를 나타내는 6개의 상태 변수와 해수면 높이를 나타내는 상태 변수의 값은 이전 시각(t-1)에서의 계산된 상태 변수의 추정 값을 사용할 수 있다.
그리고, 이전 시각(t-1)에서의 확장 상태 벡터의 오차 공분산 행렬을 다음 [수학식 4]와 같이 구한다(S304).
[수학식 4]
Figure 112014047162289-pat00032
여기서,
Figure 112014047162289-pat00033
은 이전 시각(t-1)에서의 상태 변수의 추정 값의 오차 공분산 행렬인 추정 오차 공분산이고, Mt는 내수용성 센서에 의해 측정되는 로봇의 선속도와 회전 각속도를 나타내는 6개의 변수들의 오차 공분산 행렬로서, [수학식 1]에서 구한 값을 사용한다. Qt는 외수용성 센서에 의해 측정되는 해저면까지의 거리와 로봇의 수면으로부터의 깊이를 나타내는 5개의 변수들의 오차 공분산 행렬로서, [수학식 2]에서 구한 값을 사용한다.
그 다음, 확장 상태 벡터 및 확장 상태 벡터의 오차 공분산을 이용하여 확장 상태 변수들이 평균값들을 중심으로 어떻게 분포되어 있는지를 나타내는 확장 상태 분포 벡터를 다음 [수학식 5]와 같이 생성한다(S305).
[수학식 5]
Figure 112014047162289-pat00034
여기서,
Figure 112014047162289-pat00035
은 [수학식 4]에서 구해진 확장 상태 벡터의 오차 공분산 행렬에 대한 제곱근 행렬이다.
Figure 112014047162289-pat00036
은
Figure 112014047162289-pat00037
의 촐레스키 분해(Cholesky decomposition)에 의해 구해지며 18행*18열의 행렬이다.
Figure 112014047162289-pat00038
는 확장 상태 벡터와 동일하게 18개의 변수들로 이루어진 벡터들이 37(1+18*2)개 모여서 구성된다.
이때, 37개의 확장 상태 분포 벡터는 다음과 같이 구해진다.
Figure 112014047162289-pat00039
는 [수학식 3]에서 구해진
Figure 112014047162289-pat00040
그대로의 벡터이고,
Figure 112014047162289-pat00041
는
Figure 112014047162289-pat00042
로부터 구해진 행렬의 18개의 열 각각에
Figure 112014047162289-pat00043
를 곱한 후,
Figure 112014047162289-pat00044
에 더하여 구해진 18개의 벡터이고,
Figure 112014047162289-pat00045
는
Figure 112014047162289-pat00046
로부터 구해진 행렬 18개의 열 각각에
Figure 112014047162289-pat00047
를 곱한 후,
Figure 112014047162289-pat00048
에서 뺀 18개의 벡터이다.
이때,
Figure 112014047162289-pat00049
는 확장 상태 변수들의 분포 정도를 조절하는 값으로서,
Figure 112014047162289-pat00050
가 크면 확장 상태 변수들이 넓은 범위에 분포하게 되고,
Figure 112014047162289-pat00051
가 작으면 확장 상태 변수들이 좁은 범위에 분포하게 된다. 이 값은 후술되는 가중치
Figure 112014047162289-pat00052
과
Figure 112014047162289-pat00053
값을 결정한다.
Figure 112014047162289-pat00054
는
Figure 112014047162289-pat00055
(
Figure 112014047162289-pat00056
: 확장 상태 변수의 개수 = 18)으로서 확장 상태 변수의 개수인 18의 제곱근에 상수
Figure 112014047162289-pat00057
를 곱한 값이다.
Figure 112014047162289-pat00058
는 10-4부터 1 사이의 값으로 설정한다.
이와 같이, 구해진 37개의 확장 상태 분포 벡터는 확장 상태 벡터의 오차 공분산을 이용하여 구해지며, 확장 상태 분포 벡터는 확장 상태 변수들이 평균값들을 중심으로 어떻게 분포되어 있는지를 나타낼 수 있다.
다음으로, 37개의 확장 상태 분포 벡터를 이용하여 수중 로봇이 시각(t)에 어느 곳에 위치하고, 해수면 높이가 어떤 값을 가질지 나타낼 수 있는 예측 벡터를 다음 [수학식 6]과 같이 구한다(S306).
[수학식 6]
Figure 112014047162289-pat00059
여기서, 함수
Figure 112014047162289-pat00060
는 이전 시각(t-1)에서의 위치에
Figure 112014047162289-pat00061
(이전 시각(t-1)과 시각(t) 사이의 시간 간격) 동안의 수중 로봇의 이동 변위와 수중 로봇의 방향각 증분 값을 더하여 시각(t)에서의 수중 로봇의 위치와 방향각을 구할 수 있다.
Figure 112014047162289-pat00062
동안의 수중 로봇의 이동 변위와 수중 로봇의 방향각 증분 값은 수중 로봇의 선속도와 각속도로부터 구해질 수 있다. 함수
Figure 112014047162289-pat00063
는 해수면의 높이 변화가 실질적으로 매우 느리게 이루어지는 현상을 반영하여, 시각(t)에서의 해수면의 높이는 이전 시각(t-1)에서의 해수면의 높이와 같은 것으로 예측할 수 있다. 전체적으로 함수
Figure 112014047162289-pat00064
는 수중 로봇의 내수용성 센서의 측정 정보인 선속도와 회전 각속도를 이용하여 이전 시각(t-1)에서의 수중 로봇의 위치, 수중 로봇의 방향각과, 해수면의 높이로부터 시각(t)에서의 수중 로봇의 위치, 수중 로봇의 방향각과, 해수면 높이의 예측 값을 계산할 수 있다.
이와 같이, 37개의 확장 상태 분포 벡터를 구하여 각각의 벡터 값을 사용하여 함수
Figure 112014047162289-pat00065
를 적용하므로
Figure 112014047162289-pat00066
는 수중 로봇의 위치, 수중 로봇의 방향각과, 해수면의 높이에 관한 37개의 예측 벡터로 이루어질 수 있다.
다음으로, 구해진 37개의 예측 벡터를 사용하여 하나의 평균 예측 벡터
Figure 112014047162289-pat00067
를 다음 [수학식 7]과 같이 구한다(S307).
[수학식 7]
Figure 112014047162289-pat00068
여기서,
Figure 112014047162289-pat00069
Figure 112014047162289-pat00070
Figure 112014047162289-pat00071
로 정한다.
이 경우,
Figure 112014047162289-pat00072
이 되어 평균 예측 벡터
Figure 112014047162289-pat00073
는 37개의 예측 벡터들의 가중 평균값이 된다.
이와 같이, 37개의 예측 벡터 각각에 가중치
Figure 112014047162289-pat00074
을 곱하여 더하면 평균 예측 벡터
Figure 112014047162289-pat00075
가 구해진다.
그리고, 구해진 37개의 예측 벡터를 사용하여 예측 벡터들의 오차 공분산 행렬
Figure 112014047162289-pat00076
을 다음 [수학식 8]과 같이 구한다(S308).
[수학식 8]
Figure 112014047162289-pat00077
여기서, 37개의 예측 벡터의 오차 제곱 각각에 가중치
Figure 112014047162289-pat00078
를 곱하여 더하면 가중 오차 공분산 행렬
Figure 112014047162289-pat00079
이 구해진다.
Figure 112014047162289-pat00080
은
Figure 112014047162289-pat00081
Figure 112014047162289-pat00082
로 정한다.
이와 같이, 평균 예측 벡터
Figure 112014047162289-pat00083
와 예측 벡터의 오차 공분산 행렬
Figure 112014047162289-pat00084
은 예측된 수중 로봇의 위치, 수중 로봇의 방향각 및 해수면의 높이에 대한 평균값과, 오차 공분산 행렬이 되며, 이는 후술되는 외수용성 센서의 측정 정보에 의해서 보정되어 시각(t)에서 추정된 최종 수중 로봇의 위치, 수중 로봇의 방향각과, 해수면의 높이 및 이들의 추정 오차 공분산 행렬이 될 수 있다.
도 3 및 도 4b를 참조하면, 수중 로봇의 외수용성 센서의 측정 정보 및 미리 정해진 지형 지도에 대한 정보를 이용하여 상태 변수의 예측 값 및 예측 오차 공분산을 보정하여 상태 변수의 추정 값 및 추정 오차 공분산을 계산한다(S310).
보다 자세하게는, 예측 벡터를 이용하여 외수용성 센서의 측정 예측 값을 다음 [수학식 9]과 같이 계산한다(S311).
[수학식 9]
Figure 112014047162289-pat00085
즉, 37개의 예측된 수중 로봇의 위치, 수중 로봇의 방향각과, 해수면 높이
Figure 112014047162289-pat00086
와 외수용성 센서 측정의 잡음 분포를 나타내는
Figure 112014047162289-pat00087
를 사용하여 예상되는 37개의 외수용성 센서의 측정 예측 값을 계산할 수 있다.
여기서,
Figure 112014047162289-pat00088
는 수중 로봇의 위치, 수중 로봇의 방향각과, 해수면의 높이가 주어졌을 때, 수중 로봇에서 해저면까지의 4개의 거리 값(
Figure 112014047162289-pat00089
,
Figure 112014047162289-pat00090
,
Figure 112014047162289-pat00091
,
Figure 112014047162289-pat00092
)과 해수면에서 수중 로봇의 깊이(
Figure 112014047162289-pat00093
)를 계산하는 함수이다.
Figure 112014047162289-pat00094
는 확장 상태 분포 벡터
Figure 112014047162289-pat00095
에서 외수용성 센서 측정 오차 벡터 즉,
Figure 112014047162289-pat00096
에서 구해진 37개의 벡터들 각각에서 14~18번째 원소들을 나타낸다.
다음으로, 구해진 37개의 예측된 외수용성 센서의 측정 예측 값의 평균 벡터를 다음 [수학식 10]과 같이 구한다(S312).
[수학식 10]
Figure 112014047162289-pat00097
여기서,
Figure 112014047162289-pat00098
은 가중치로서, 평균 예측 벡터를 구할 때 사용된 값과 동일한 값을 사용한다. 즉,
Figure 112014047162289-pat00099
는 외수용성 센서의 측정 예측 값의 가중 평균 벡터로, 외수용성 센서의 측정 예측 값의 공분산 행렬을 계산할 때 사용될 수 있다.
그리고, 37개의 외수용성 센서의 측정 예측 값과 [수학식 10]에서 구해진 평균 벡터를 사용하여 외수용성 센서 측정 예측 값의 오차 공분산 행렬을 다음 [수학식 11]과 같이 계산한다(S313).
[수학식 11]
Figure 112014047162289-pat00100
이때, 예측 벡터의 오차 공분산 행렬을 구할 때 사용한 값들과 동일한 값의 가중치
Figure 112014047162289-pat00101
를 사용한다. 즉, St는 외수용성 센서의 측정 예측 값의 가중 공분산 행렬이 되고, St는 후술되는 이득 벡터를 구하는데 사용된다.
다음으로, 예측 벡터 및 외수용성 센서의 측정 예측 값의 교차 공분산을 다음 [수학식 12]와 같이 계산한다(S314).
[수학식 12]
Figure 112014047162289-pat00102
즉, 수중 로봇의 위치, 수중 로봇의 방향각과, 해수면의 높이를 나타내는 예측 벡터와 외수용성 센서의 측정 예측 값(4개의 해저면까지의 거리와 수중 로봇의 해수면으로 부터의 깊이)을 나타내는 벡터의 교차 공분산(cross covariance)행렬을 구한다.
이때, 예측 단계에서 구해진
Figure 112014047162289-pat00103
와,
Figure 112014047162289-pat00104
, 외수용성 센서의 측정 예측 값
Figure 112014047162289-pat00105
, 그리고 외수용성 센서의 측정 예측 값의 평균 벡터
Figure 112014047162289-pat00106
를 이용할 수 있다.
그 다음으로, 외수용성 센서의 측정 예측 값의 오차 공분산 St과 교차 공분산
Figure 112014047162289-pat00107
을 사용하여 다음 [수학식 13]과 같이 보정을 위한 이득 벡터 Kt를 구한다(S315).
[수학식 13]
Figure 112014047162289-pat00108
이득 벡터 Kt는 수중 로봇의 위치, 수중 로봇의 방향각과, 해수면의 높이를 보정하는 것은 물론, 이들 상태 벡터의 공분산 행렬을 구하는데 사용된다.
그리고, 외수용성 센서의 측정 정보
Figure 112014047162289-pat00109
와 외수용성 센서의 측정 예측 값
Figure 112014047162289-pat00110
의 차이에 이득 벡터 Kt를 곱한 후, 이 벡터를 예상된 상태 벡터인 평균 예측 벡터
Figure 112014047162289-pat00111
에 더하여 상태 벡터의 예측 값을 보정함으로써 상태 변수의 추정 값을 다음 [수학식 14]와 같이 계산한다.
[수학식 14]
Figure 112014047162289-pat00112
보정된 상태 벡터
Figure 112014047162289-pat00113
는 외수용성 센서의 측정 예측 값을 사용하여 최종적으로 추정된 상태 변수들의 값이다. 즉, 예측 단계와 보정 단계에서 최종적으로 출력되는 추정된 상태 변수 값들로 이루어진 벡터이다.
그런 후, 예측된 상태 벡터의 오차 공분산 행렬
Figure 112014047162289-pat00114
과 이득 벡터 Kt를 사용하여 다음 [수학식 15]와 같이 최종적으로 보정된 상태 벡터의 오차 공분산 행렬
Figure 112014047162289-pat00115
인 추정 오차 공분산을 구한다(S316).
[수학식 15]
Figure 112014047162289-pat00116
이는 전체 예측 단계와 보정 단계를 통하여 최종적으로 구해진 결과로서 추정된 상태 벡터인 상태 변수의 추정 값과 상태 벡터의 오차 공분산 행렬인 추정 오차 공분산을 출력한다.
이러한 상태 변수의 추정 값과 추정 오차 공분산은 다음 [수학식 16]과 같이 센서 측정 값 처리를 위한 과정에 입력되어 새로운 예측 단계와 보정 단계가 시작될 수 있다.
[수학식 16]
Figure 112014047162289-pat00117
도 5는 본 발명의 실시예에 따른 수중 로봇의 위치 및 해수면의 높이를 추정하기 위해 구축된 시뮬레이션 환경을 나타내는 예이고, 도 6은 도 5의 시뮬레이션 환경에서 사용되는 해저 지형도의 예이다.
도 5 및 도 6을 참조하면, 해저 지형을 위한 수중 로봇의 위치 및 해수면의 높이를 추정하는 것은 도 5와 같은 시뮬레이션 환경을 구축하여 실험을 수행할 수 있으며, 도 6와 같이 지형 환경은 x: 60m, y: 60m, z: 15m로 나타낼 수 있다.
시뮬레이션의 속도 정보는 다음 [표 1]과 같으며, 해수면의 변화가 있을 때 수중 로봇의 위치와 해수면의 높이를 추정하는 경우와, 수중 로봇의 위치만 추정하는 경우를 비교하여 설명하도록 한다.
surge(u) pitch(q) yaw(r) △t
3.5(m/s) -0.1(rad/s) -0.55(rad/s) 0.1(sec)
도 7은 해수면의 높이가 변할 경우, 수중 로봇의 위치와 해수면의 높이를 동시에 추정하기 위한 예로, 도 7a는 수중 로봇의 이동 궤적을 나타내는 그래프, 도 7b는 해수면의 변화를 나타나는 그래프, 도 7c는 수중 로봇의 오차를 나타내는 그래프이다.
도 7a 내지 도 7c를 참조하면, 도 7은 해수면의 높이가 기준보다 0.5m 상승한 경우, 수중 로봇의 위치와 해수면의 높이를 동시에 추정하는 경우로, 도 7a는 수중 로봇의 추정 궤적이고, 도 7c는 기준 로봇과 추정 로봇과의 처리 샘플링 수에 따른 오차를 나타낸다. 도 5와 같은 시뮬레이션 환경을 구축하여 실험하면, 도 7a에서와 같이, 추정 궤도는 기준 궤도에 근접하게 만들어지고, 이에 따라 수중 로봇의 추정 궤적에 대한 거리 오차는 도 7c에서와 같이, 오차의 평균은 0.13(m), 표준 편차는 0.09(m), 최대 오차는 0.52(m)가 된다.
도 8은 해수면의 높이가 변할 경우, 수중 로봇의 위치만 추정하기 위한 예로, 도 8a는 수중 로봇의 이동 궤적을 나타내는 그래프 및 도 8b는 수중 로봇의 오차를 나타내는 그래프이다.
도 8a 및 도 8b를 참조하면, 도 8은 해수면의 높이가 기준보다 0.5m 상승한 경우, 수중 로봇의 위치만 추정하는 경우로, 도 8a는 수중 로봇의 추정 궤적이고, 도 8b는 기준 로봇과 추정 로봇과의 처리 샘플링 수에 따른 오차를 나타낸다. 도 5와 같은 시뮬레이션 환경을 구축하여 실험하면, 도 8a에서와 같이, 추정 궤도는 기준 궤도와 다소 차이가 있게 만들어지고, 이에 따라 수중 로봇의 추정 궤적에 대한 거리 오차는 도 8b에서와 같이, 오차의 평균은 2.07(m), 표준 편차는 0.97(m), 최대 오차는 3.61(m)가 된다.
이와 같이, 수중 로봇의 위치만 추정하는 경우에 비해 수중 로봇의 위치와 해수면의 높이를 동시에 추정하는 것이 수중 로봇의 추정 궤적에 대한 거리 오차가 작아지므로 해수면이 변화할 때 수중 로봇의 위치와 해수면 높이를 동시에 추정하는 방식을 사용하면, 추정 궤적의 거리 오차를 줄일 수 있다.
본 발명의 실시예는 다양한 컴퓨터로 구현되는 동작을 수행하기 위한 프로그램 명령을 포함하는 컴퓨터로 읽을 수 있는 매체를 포함한다. 이 매체는 앞서 설명한 수중 로봇의 위치 추정 방법을 실행시키기 위한 프로그램을 기록한다. 이 매체는 프로그램 명령, 데이터 파일, 데이터 구조 등을 단독으로 또는 조합하여 포함할 수 있다. 이러한 매체의 예에는 하드디스크, 플로피디스크 및 자기 테이프와 같은 자기 매체, CD 및 DVD와 같은 광기록 매체, 플롭티컬 디스크(floptical disk)와 자기-광 매체, 롬, 램, 플래시 메모리 등과 같은 프로그램 명령을 저장하고 수행하도록 구성된 하드웨어 장치 등이 있다. 또는 이러한 매체는 프로그램 명령, 데이터 구조 등을 지정하는 신호를 전송하는 반송파를 포함하는 광 또는 금속선, 도파관 등의 전송 매체일 수 있다. 프로그램 명령의 예에는 컴파일러에 의해 만들어지는 것과 같은 기계어 코드뿐만 아니라 인터프리터 등을 사용해서 컴퓨터에 의해서 실행될 수 있는 고급 언어 코드를 포함한다.
이상에서 본 발명의 바람직한 실시예에 대하여 상세하게 설명하였지만 본 발명의 권리범위는 이에 한정되는 것은 아니고 다음의 청구범위에서 정의하고 있는 본 발명의 기본 개념을 이용한 당업자의 여러 변형 및 개량 형태 또한 본 발명의 권리범위에 속하는 것이다.
10: 수중 로봇
11: 내수용성 센서 13: 외수용성 센서
100: 예측부
110: 확장 상태 벡터 계산부 130: 확장 상태 분포 벡터 생성부
150: 제1 예측 벡터 계산부 170: 제2 예측 벡터 계산부
200: 보정부
210: 측정 예측 값 계산부 230: 교차 공분산 계산부
250: 이득 벡터 계산부 270: 상태 변수 보정부

Claims (16)

  1. 미리 정해진 시각(t)에 측정된 수중 로봇의 내수용성 센서의 측정 정보를 이용하여 상기 수중 로봇의 위치, 상기 수중 로봇의 방향각 및 해수면의 높이를 포함하는 상태 변수의 예측 값 및 예측 오차 공분산을 계산하는 제1 단계, 그리고
    상기 수중 로봇의 외수용성 센서의 측정 정보 및 미리 정해진 지형 지도에 대한 정보를 이용하여 상기 상태 변수의 예측 값 및 상기 예측 오차 공분산을 보정하여 상기 상태 변수의 추정 값 및 추정 오차 공분산을 계산하는 제2 단계
    를 포함하고,
    상기 제1 단계는 미리 정해진 이전 시각(t-1)에 계산된 상기 상태 변수의 추정 값을 이용하여 상기 미리 정해진 이전 시각(t-1)에서의 확장 상태 벡터 및 상기 확장 상태 벡터의 오차 공분산을 구하는 단계를 포함하고,
    상기 확장 상태 벡터는 상기 상태 변수, 상기 내수용성 센서의 측정 오차 정보 및 상기 외수용성 센서의 측정 오차 정보를 포함하는 확장 상태 변수들로 이루어지는 수중 로봇의 위치 추정 방법.
  2. 삭제
  3. 삭제
  4. 제 1 항에서,
    상기 제1 단계는,
    상기 확장 상태 벡터 및 상기 확장 상태 벡터의 오차 공분산을 이용하여 상기 확장 상태 변수들이 평균값들을 중심으로 어떻게 분포되어 있는지를 나타내는 확장 상태 분포 벡터를 생성하는 단계를 포함하는 수중 로봇의 위치 추정 방법.
  5. 제 4 항에서,
    상기 제1 단계는,
    상기 확장 상태 분포 벡터를 이용하여 상기 수중 로봇이 상기 시각(t)에 어느 곳에 위치하고 상기 해수면의 높이가 어떤 값을 가질지 나타낼 수 있는 예측 벡터를 구하는 단계, 그리고
    상기 예측 벡터를 기반으로 평균 예측 벡터 및 상기 예측 벡터의 오차 공분산을 구하는 단계를 포함하는 수중 로봇의 위치 추정 방법.
  6. 제 5 항에서,
    상기 제2 단계는,
    상기 예측 벡터를 이용하여 상기 외수용성 센서의 측정 예측 값 및 상기 측정 예측 값의 오차 공분산을 구하는 단계, 그리고
    상기 예측 벡터 및 상기 외수용성 센서의 측정 예측 값의 교차 공분산을 구하는 단계를 포함하는 수중 로봇의 위치 추정 방법.
  7. 제 6 항에서,
    상기 제2 단계는,
    상기 측정 예측 값의 오차 공분산 및 상기 교차 공분산을 이용하여 이득 벡터를 구하는 단계를 포함하는 수중 로봇의 위치 추정 방법.
  8. 제 7 항에서,
    상기 제2 단계는,
    상기 외수용성 센서의 측정 정보, 상기 측정 예측 값 및 상기 이득 벡터를 이용하여 상기 상태 변수의 예측 값 및 상기 예측 오차 공분산을 보정하여 상기 상태 변수의 추정 값 및 추정 오차 공분산을 계산하는 단계를 포함하는 수중 로봇의 위치 추정 방법.
  9. 미리 정해진 시각(t)에 측정된 수중 로봇의 내수용성 센서의 측정 정보를 이용하여 상기 수중 로봇의 위치, 상기 수중 로봇의 방향각 및 해수면의 높이를 포함하는 상태 변수의 예측 값 및 예측 오차 공분산을 계산하는 예측부, 그리고
    상기 수중 로봇의 외수용성 센서의 측정 정보 및 미리 정해진 지형 지도에 대한 정보를 이용하여 상기 상태 변수의 예측 값 및 상기 예측 오차 공분산을 보정하여 상기 상태 변수의 추정 값 및 추정 오차 공분산을 계산하는 보정부
    를 포함하고,
    상기 예측부는 미리 정해진 이전 시각(t-1)에 계산된 상기 상태 변수의 추정 값을 이용하여 상기 미리 정해진 이전 시각(t-1)에서의 확장 상태 벡터 및 상기 확장 상태 벡터의 오차 공분산을 구하는 확장 상태 벡터 계산부를 포함하고,
    상기 확장 상태 벡터는 상기 상태 변수, 상기 내수용성 센서의 측정 오차 정보 및 상기 외수용성 센서의 측정 오차 정보를 포함하는 확장 상태 변수들로 이루어지는 수중 로봇의 위치 추정 장치.
  10. 삭제
  11. 삭제
  12. 제 9 항에서,
    상기 예측부는,
    상기 확장 상태 벡터 및 상기 확장 상태 벡터의 오차 공분산을 이용하여 상기 확장 상태 변수들이 평균값들을 중심으로 어떻게 분포되어 있는지를 나타내는 확장 상태 분포 벡터를 생성하는 확장 상태 분포 벡터 생성부를 포함하는 수중 로봇의 위치 추정 장치.
  13. 제 12 항에서,
    상기 예측부는,
    상기 확장 상태 분포 벡터를 이용하여 상기 수중 로봇이 상기 시각(t)에 어느 곳에 위치하고 상기 해수면의 높이가 어떤 값을 가질지 나타낼 수 있는 예측 벡터를 구하는 제1 예측 벡터 계산부, 그리고
    상기 예측 벡터를 기반으로 평균 예측 벡터 및 상기 예측 벡터의 오차 공분산을 구하는 제2 예측 벡터 계산부를 포함하는 수중 로봇의 위치 추정 장치.
  14. 제 13 항에서,
    상기 보정부는,
    상기 예측 벡터를 이용하여 상기 외수용성 센서의 측정 예측 값 및 상기 측정 예측 값의 오차 공분산을 구하는 측정 예측 값 계산부, 그리고
    상기 예측 벡터 및 상기 외수용성 센서의 측정 예측 값의 교차 공분산을 구하는 교차 공분산 계산부를 포함하는 수중 로봇의 위치 추정 장치.
  15. 제 14 항에서,
    상기 보정부는,
    상기 측정 예측 값의 오차 공분산 및 상기 교차 공분산을 이용하여 이득 벡터를 구하는 이득 벡터 계산부를 포함하는 수중 로봇의 위치 추정 장치.
  16. 제 15 항에서,
    상기 보정부는,
    상기 외수용성 센서의 측정 정보, 상기 측정 예측 값 및 상기 이득 벡터를 이용하여 상기 상태 변수의 예측 값 및 상기 예측 오차 공분산을 보정하여 상기 상태 변수의 추정 값 및 추정 오차 공분산을 계산하는 상태 변수 보정부를 포함하는 수중 로봇의 위치 추정 장치.
KR1020140060237A 2014-05-20 2014-05-20 수중 로봇의 위치 추정 방법 및 장치 KR101562042B1 (ko)

Priority Applications (1)

Application Number Priority Date Filing Date Title
KR1020140060237A KR101562042B1 (ko) 2014-05-20 2014-05-20 수중 로봇의 위치 추정 방법 및 장치

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
KR1020140060237A KR101562042B1 (ko) 2014-05-20 2014-05-20 수중 로봇의 위치 추정 방법 및 장치

Publications (1)

Publication Number Publication Date
KR101562042B1 true KR101562042B1 (ko) 2015-10-20

Family

ID=54400040

Family Applications (1)

Application Number Title Priority Date Filing Date
KR1020140060237A KR101562042B1 (ko) 2014-05-20 2014-05-20 수중 로봇의 위치 추정 방법 및 장치

Country Status (1)

Country Link
KR (1) KR101562042B1 (ko)

Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR101304579B1 (ko) 2011-07-15 2013-09-05 삼성중공업 주식회사 수중 이동체 위치측정장치 및 그 방법

Patent Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR101304579B1 (ko) 2011-07-15 2013-09-05 삼성중공업 주식회사 수중 이동체 위치측정장치 및 그 방법

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
노성우. 수중로봇 위치추정을 위한 베이시안 필터 방법의 실현과 거리 측정 특성 분석. 로봇공학회 논문지 제9권 제1호. 2014.2.14. p28-p38
이상윤. 한국형 수중로봇시스템의 기술개발연구. 한국전자통신학회 논문지 제8권 제1호. 2013.1.21. p27-p40.

Similar Documents

Publication Publication Date Title
US20180128909A1 (en) Method for obtaining horizontal longitudinal correlation of deep-sea great-depth sound field
Chen et al. Review of AUV underwater terrain matching navigation
CN107990891B (zh) 基于长基线和信标在线标定的水下机器人组合导航方法
Norgren et al. A multibeam-based SLAM algorithm for iceberg mapping using AUVs
CN111896962B (zh) 一种海底应答器定位方法、系统、存储介质及应用
CN110132281B (zh) 一种基于询问应答模式的水下高速目标高精度自主声学导航方法
Yona et al. Compensating for partial Doppler velocity log outages by using deep-learning approaches
JP2020063958A (ja) 位置推定装置及び方法
Kinsey et al. Towards in-situ calibration of gyro and doppler navigation sensors for precision underwater vehicle navigation
CN111220146B (zh) 一种基于高斯过程回归学习的水下地形匹配定位方法
Morice et al. Terrain referencing for autonomous navigation of underwater vehicles
Chen In-situ alignment calibration of attitude and ultra short baseline sensors for precision underwater positioning
Zhu et al. Kalman-based underwater tracking with unknown effective sound velocity
CN110703187B (zh) 一种基于水下机动平台的声信标测向及定位方法
Lager et al. Underwater terrain navigation using standard sea charts and magnetic field maps
KR101562042B1 (ko) 수중 로봇의 위치 추정 방법 및 장치
CN116358544A (zh) 基于声学特征匹配定位修正惯性导航误差的方法和系统
KR101480834B1 (ko) 다중 경로 음파 전달 모델 및 표적 식별을 이용한 표적 기동분석 방법
RU2196341C1 (ru) Способ определения параметров движения маневрирующего объекта
CN113495275A (zh) 一种单水听器垂直合成孔径被动定位方法、系统及应用
Tholen et al. Optimal Receiver Configuration Of Short-Baseline Localisation Systems Using Particle Swarm Optimisation.
Van Ballegooijen et al. Measurement of towed array position, shape, and attitude
Christensen et al. USBL pose estimation using multiple responders
Devassykutty et al. Evaluation of high precision localization approach for a fleet of unmanned deep ocean vehicles
Burdinsky et al. Estimation of observation error of AUV using sonar positioning system

Legal Events

Date Code Title Description
E701 Decision to grant or registration of patent right
GRNT Written decision to grant
FPAY Annual fee payment

Payment date: 20180917

Year of fee payment: 4

FPAY Annual fee payment

Payment date: 20190819

Year of fee payment: 5

R401 Registration of restoration