KR101263482B1 - 이동 로봇의 동적 위치 및 방향 인식 방법 및 장치 - Google Patents

이동 로봇의 동적 위치 및 방향 인식 방법 및 장치 Download PDF

Info

Publication number
KR101263482B1
KR101263482B1 KR1020100108275A KR20100108275A KR101263482B1 KR 101263482 B1 KR101263482 B1 KR 101263482B1 KR 1020100108275 A KR1020100108275 A KR 1020100108275A KR 20100108275 A KR20100108275 A KR 20100108275A KR 101263482 B1 KR101263482 B1 KR 101263482B1
Authority
KR
South Korea
Prior art keywords
mobile robot
ultrasonic
time
calculated
transmitter
Prior art date
Application number
KR1020100108275A
Other languages
English (en)
Other versions
KR20120046558A (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 KR1020100108275A priority Critical patent/KR101263482B1/ko
Publication of KR20120046558A publication Critical patent/KR20120046558A/ko
Application granted granted Critical
Publication of KR101263482B1 publication Critical patent/KR101263482B1/ko

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J13/00—Controls for manipulators
    • B25J13/08—Controls for manipulators by means of sensing devices, e.g. viewing or touching devices
    • B25J13/088—Controls for manipulators by means of sensing devices, e.g. viewing or touching devices with position, velocity or acceleration sensors
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
    • B25J19/02—Sensing devices
    • B25J19/026—Acoustical sensing devices
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1694—Programme controls characterised by use of sensors other than normal servo-feedback from position, speed or acceleration sensors, perception control, multi-sensor controlled systems, sensor fusion
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y10—TECHNICAL SUBJECTS COVERED BY FORMER USPC
    • Y10S—TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y10S901/00—Robots
    • Y10S901/01—Mobile robot

Abstract

본 발명은 이동 로봇의 동적 위치 및 방향 인식 방법 및 장치에 관한 것으로서, 더욱 상세하게는 초음파 거리의 시간 보상 방법 및, 더 나아가서는 확장 칼만 필터(extended Kalman filter)를 이용한 이동 로봇의 동적 위치 및 방향 인식 방법 및 장치에 관한 것이다.
본 발명에 의하면, 이동 로봇에 사용되는 기존의 초음파 송신기 및 초음파 수신기를 이용한 위치 및 방향 인식 시스템에서 서로 다른 시각에서 측정된 다수의 초음파 거리들을 로봇의 위치와 방향이 계산되는 시각의 거리들로 보상하여 위치 및 방향 인식을 수행함으로써 그 정확도를 향상시킨다. 또한, 엔코더(encoder) 정보를 사용한 확장 칼만 필터를 유효적절히 활용하여 이동 로봇의 위치 및 방향 인식의 정확도를 더욱 향상시킨다.

Description

이동 로봇의 동적 위치 및 방향 인식 방법 및 장치{Dynamic localization method and apparatus for mobile robots}
본 발명은 이동 로봇의 동적 위치 및 방향 인식 방법 및 장치에 관한 것으로서, 더욱 상세하게는 초음파 거리의 시간 보상 방법 및, 더 나아가서는 확장 칼만 필터(extended Kalman filter)를 이용한 이동 로봇의 동적 위치 및 방향 인식 방법 및 장치에 관한 것이다.
이동 로봇의 위치와 방향을 인식하기 위하여 다수의 초음파 송신기를 천정에 설치하고 다수의 초음파 수신기를 이동 로봇 상부에 설치하여, 각 초음파 송신기와 초음파 수신기 사이의 거리를 측정하고 삼각법을 이용하여 이동 로봇의 위치와 방향을 계산하는 시스템이 사용되고 있다. 초음파의 송수신을 이용하여 거리를 측정하는 방법은 측정 물체를 향해 초음파를 발신하여 반사되는 신호를 수신하고 반사에서 수신까지의 시간을 측정하여 측정된 시간을 왕복 거리로 환산하여 거리를 측정하는 것이 가장 보편적인 방법이다. 또한, 초음파 발신기와 초음파 수신기가 떨어져 있는 경우에는, 초음파 발신기와 초음파 수신기 사이에 초음파가 도달하는 데 걸리는 시간을 측정하여 초음파 발신기와 초음파 수신기 사이의 거리를 측정할 수 있다. 이때 초음파 송신 시각을 초음파 수신기가 알기 위해 통상적으로 고주파신호를 추가로 사용한다.
이 경우 로봇 상부에 설치된 초음파 수신기 하나의 위치를 인식하기 위해서는 초음파 송신기와 초음파 수신기 간에 최소 3개의 거리를 측정해야 하는데, 거리 측정은 송신기 하나씩 순차적으로 이루어져야 할 뿐 아니라 하나의 거리를 측정하는 데에도 초음파의 도달시간이 소요되기 때문에 로봇이 이동시에는 측정된 초음파 신호에 의한 거리들이 동일한 위치에서 측정된 거리가 아니라는 문제가 있다. 따라서 초음파 거리를 이용해서 이동 로봇의 위치를 인식 하는 시스템에서, 로봇이 이동 시에 측정된 초음파 거리를 로봇의 위치와 방향이 계산되는 시각에서의 거리로 보상하는 방법이 필요하다. 또한 이와 아울러 엔코더 정보를 활용하여 정확도를 향상시키는 방안이 필요하다.
본 발명은 이와 같은 문제점을 해결하기 위해 창안된 것으로서, 이동 로봇에 사용되는 기존의 초음파 송신기와 초음파 수신기를 이용한 위치 및 방향 인식 시스템에서 서로 다른 시각에서 측정된 다수의 초음파 거리들을 로봇의 위치와 방향이 계산되는 시각의 거리들로 보상하여 위치 및 방향 인식을 수행함으로써 그 정확도를 향상시킴에 그 목적이 있다. 또한, 엔코더(encoder) 정보를 사용한 확장 칼만 필터를 유효적절히 활용하여 이동 로봇의 위치 및 방향 인식의 정확도를 더욱 향상시키는데 그 목적이 있다.
이와 같은 목적을 달성하기 위하여 본 발명에 따른, 이동 로봇에 장착된 동적 위치 및 방향 인식 장치가, 이동 로봇의 이동시 그 위치 및 방향을 동적으로 인식하는 방법은, (a) 특정 송신기(이하 '현 주기 송신기'라 한다)에서 송신된 초음파 신호를, 이동 로봇 동적 위치 및 방향 인식 장치의 초음파 수신기가 수신하는 단계; (b) 상기 수신된 초음파 신호를 이용하여, 이동 로봇의 현재 위치에서 상기 현 주기 송신기로부터 상기 초음파 수신기까지의 초음파 거리를 측정하는 단계; (c) 상기 이동 로봇의 이전 주기 위치에서의 상기 초음파 수신기로부터 타 송신기(이하 '이전 주기 송신기'라 한다)까지의 초음파 거리를, 상기 이동 로봇의 현재 위치에서 상기 초음파 수신기로부터 상기 이전 주기 송신기까지의 초음파 거리로 환산하도록 시간 보상을 수행하는 단계; 및 (d) 상기 단계(b)에서 측정된 초음파 거리 및 단계(c)에서 시간 보상된 초음파 거리로부터 상기 이동 로봇의 현재 위치와 방향을 산출하는 단계를 포함하고, 상기 단계(c)는, (c11) 이동 로봇의 양 바퀴의 회전량을 나타내는 엔코더 신호로부터, 다수의 선속도와 각속도를 산출하는 단계; (c12) 상기 다수의 선속도와 각속도 정보로부터 상기 이동 로봇의 현재 위치와 방향을 산출하는 단계; (c13) 상기 단계(c12)에서 산출된 이동 로봇의 위치와 방향으로부터 초음파 수신기의 현재 위치를 산출하는 단계; 및 (c14) 상기 이동 로봇의 이전 주기 위치에서 상기 초음파 수신기와 상기 이전 주기 송신기 간의 초음파 거리 측정시 발생된 측정 오차를, 상기 단계(c13)에서 산출된 초음파 수신기의 위치와 상기 이전 주기 송신기 간의 거리에 적용하여, 시간 보상된 초음파 거리를 구하는 단계를 포함하며, 상기 단계(c11)의 선속도와 각속도 산출주기는, 송신기의 초음파 송신주기보다 작은 것이 바람직하다.
상기 초음파 수신기는 두 개 이상 설치되며, 상기 단계(a) 내지 단계(c)는 설치된 상기 각각의 초음파 수신기에 대하여 수행되고, 상기 단계(d)에서 산출되는 이동 로봇의 동적 위치 및 방향은, 상기 각 초음파 수신기에서 수신된 초음파로부터 측정되거나 시간 보상된 모든 초음파 거리를 이용하여 산출될 수 있다.
상기 단계(b)의 초음파 거리는, 상기 단계(a)에서 수신한 초음파 신호에 대하여 정합 필터 계산을 수행하여 산출될 수 있다.
m 개의 송신기가 존재하고 현재 시각을 tk, 이전 시각을 순차적으로 tk-1, tk-2, ... ,tk-(m-1)로 표현할 경우, 상기 단계(c)에서 현재 위치 초음파 거리로의 시간 보상은, tk-1 시각에 측정된 초음파 거리, tk-2 시각에 측정된 초음파 거리, ..., 및 tk-(m-1) 시각에 측정된 초음파 거리에 대하여 각각 서로 다른 시간량(tk-tk-1, tk-tk-2, ... , tk-tk-(m-1))의 시간 보상을 수행하는 것일 수 있다.
삭제
m 개의 송신기가 존재하고 현재 시각을 tk, 이전 시각을 순차적으로 tk-1, tk-2, ... ,tk-(m-1)로 표현할 경우, 상기 단계(c)에서 현재 위치 초음파 거리로의 시간 보상은, tk-1 시각에 측정된 초음파 거리 및 tk-2, ... ,tk-(m-1) 시각에 측정된 초음파 거리로부터 tk-1 시각으로 시간 보상된 m-2 개의 초음파 거리에 대하여 동일한 시간량(tk-tk-1)의 반복적인 시간 보상을 수행하는 것일 수 있다.
상기 단계(d) 이후, (e) 확장 칼만 필터(extended Kalman filter)가, 상기 단계(d)에서 산출된 상기 이동 로봇의 현재 위치와 방향 정보 및, 엔코더 신호로부터 산출한 선속도 및 각속도 정보를 포함하는 오도메트리(odometry) 정보로부터, 이동 로봇의 위치와 방향을 산출하는 단계를 더 포함할 수 있다.
상기 단계(e)는, (e11) 상기 오도메트리 정보 및 이전 주기에 상기 확장 칼만 필터에서 산출된 이동 로봇의 위치와 방향 값으로부터, 현재 주기의 이동 로봇의 위치와 방향 예측값 및 그 예측값의 표준편차의 제곱을 나타내는 공분산 행렬(covariance matrix) 예측값을 산출하는 단계; 및 (e12) 상기 단계(d)에서 산출된 상기 이동 로봇의 현재 위치와 방향 정보 및, 상기 단계(e11)에서 산출된 현재 주기의 이동 로봇의 위치와 방향 예측값 및 공분산 행렬 예측값을 이용하여 최종적으로 이동 로봇의 위치와 방향값 및 그 공분산행렬 갱신값을 산출하는 단계를 포함할 수 있다.
본 발명의 다른 측면에 따르면, 이동 로봇의 이동시 그 위치 및 방향을 동적으로 인식하는 장치는, 송신기에서 송신된 초음파 신호를 수신하는 초음파 수신기; 상기 수신된 초음파 신호를 이용하여, 송신기로부터 상기 초음파 수신기까지의 초음파 거리를 측정하는 초음파 거리 측정 모듈; 상기 이동 로봇의 이전 주기 위치에서의 상기 초음파 수신기로부터 타 송신기(이하 '이전 주기 송신기'라 한다)까지의 초음파 거리를, 상기 이동 로봇의 현재 위치에서 상기 초음파 수신기로부터 상기 이전 주기 송신기까지의 초음파 거리로 환산하도록 시간 보상을 수행하는 시간 보상부; 상기 초음파 거리 측정 모듈에서 측정된 초음파 거리 및 상기 시간 보상부에서 시간 보상된 초음파 거리로부터 상기 이동 로봇의 현재 위치와 방향을 산출하는 위치와 방향 산출부; 이동 로봇의 선속도 및 각속도 정보를 포함하는 오도메트리(odometry) 정보를 산출하는 오도메트리 정보 산출부; 및 상기 위치와 방향 산출 모듈에 의해 산출된 상기 이동 로봇의 현재 위치와 방향 정보 및, 상기 오도메트리 정보 산출부에 의해 산출된 오도메트리 정보로부터 이동 로봇의 위치와 방향을 산출하는 확장 칼만 필터(extended Kalman filter)부를 포함하고, 상기 확장 칼만 필터부는, 상기 오도메트리 정보 및 이전 주기에 상기 확장 칼만 필터부에서 최종 산출된 이동 로봇의 위치와 방향 값으로부터, 현재 주기의 이동 로봇의 위치와 방향 예측값 및 그 예측값의 표준편차의 제곱을 나타내는 공분산 행렬(covariance matrix) 예측값을 산출하는 위치와 방향값 예측부; 및 상기 위치와 방향 산출 모듈에 의해 산출된 상기 이동 로봇의 현재 위치와 방향 정보 및, 상기 위치와 방향 예측부에서 산출된 현재 주기의 이동 로봇의 위치와 방향 예측값 및 공분산 행렬 예측값을 이용하여 최종적으로 이동 로봇의 위치와 방향값 및 그 공분산행렬 갱신값을 산출하는 위치와 방향값 갱신부를 포함한다.
상기 초음파 거리 측정 모듈은, 상기 수신된 초음파 신호로부터, 정합 필터에 의한 계산을 수행하여 초음파 거리를 산출할 수 있다.
삭제
상기 오도메트리 정보 산출부는, 이동 로봇의 양 바퀴의 회전량을 엔코더 신호로서 출력하는 엔코더; 및 상기 엔코더 신호로부터 이동 로봇의 선속도 및 각속도 정보를 포함하는 오도메트리(odometry) 정보를 산출하는 오도메트리 정보 산출 모듈을 포함할 수 있다.
상기 시간 보상부는, 상기 오도메트리 정보 산출부에 의해 산출된 오도메트리 정보를 이용하여 상기 이동 로봇의 현재 위치와 방향을 산출하는 위치와 방향 산출 모듈; 상기 위치와 방향 산출 모듈에서 산출된 이동 로봇의 위치와 방향으로부터 초음파 수신기의 현재 위치를 산출하는 수신기 위치 산출 모듈; 및 상기 이동 로봇의 이전 주기 위치에서 상기 초음파 수신기와 상기 이전 주기 송신기 간의 초음파 거리 측정시 발생된 측정 오차를, 상기 수신기 위치 산출 모듈에서 산출된 초음파 수신기의 위치와 상기 이전 주기 송신기 간의 거리에 적용하여, 시간 보상된 초음파 거리를 구하는 시간 보상 모듈을 포함할 수 있다.
본 발명에 의하면, 이동 로봇에 사용되는 기존의 초음파 송초음파 수신기를 이용한 위치 및 방향 인식 시스템에서 서로 다른 시각에서 측정된 다수의 초음파 거리들을 로봇의 위치와 방향이 계산되는 시각의 거리들로 보상하여 위치 및 방향 인식을 수행함으로써 그 정확도를 향상시키는 효과가 있다. 또한, 엔코더(encoder) 정보를 사용한 확장 칼만 필터를 유효적절히 활용하여 이동 로봇의 위치 및 방향 인식의 정확도를 더욱 향상시키는 효과가 있다.
도 1은 이동 로봇의 위치 및 방향 측정이 이루어지는 전체 시스템을 개략적으로 나타낸 도면.
도 2는 이동 로봇의 이동을 나타내는 평면도.
도 3은 이동 로봇의 동적 위치 및 방향 인식 장치의 구성을 나타내는 도면.
도 4는 이동 로봇의 동적 위치 및 방향 인식이 이루어지는 일 실시예로서의 순서도.
도 5는 초음파 거리의 시간 보상 방법의 일 실시예를 나타내는 도면.
도 6은 초음파 거리의 시간 보상 방법의 다른 실시예를 나타내는 도면.
도 7은 이동 로봇의 동적 위치 및 방향 인식이 이루어지는 다른 실시예로서의 순서도.
이하 첨부된 도면을 참조로 본 발명의 바람직한 실시예를 상세히 설명하기로 한다. 이에 앞서, 본 명세서 및 청구범위에 사용된 용어나 단어는 통상적이거나 사전적인 의미로 한정해서 해석되어서는 아니 되며, 발명자는 그 자신의 발명을 가장 최선의 방법으로 설명하기 위해 용어의 개념을 적절하게 정의할 수 있다는 원칙에 입각하여 본 발명의 기술적 사상에 부합하는 의미와 개념으로 해석되어야만 한다. 따라서, 본 명세서에 기재된 실시예와 도면에 도시된 구성은 본 발명의 가장 바람직한 일 실시예에 불과할 뿐이고 본 발명의 기술적 사상을 모두 대변하는 것은 아니므로, 본 출원시점에 있어서 이들을 대체할 수 있는 다양한 균등물과 변형 예들이 있을 수 있음을 이해하여야 한다.
도 1은 이동 로봇의 위치 및 방향 측정이 이루어지는 전체 시스템을 개략적으로 나타낸 도면이며, 도 2는 이동 로봇의 이동을 나타내는 평면도로써 나타낸 도면이다.
도 1에서, 이동 로봇(10)은 모터 등에 의해 회전하는 바퀴(21,22)를 구비하고 자율 주행을 할 수 있는 로봇이다. 이동 로봇(10)이 평면상을 이동한다고 가정하면, 이동 로봇(10)의 위치 및 방향은 평면상의 이동 로봇(10)의 위치(position) (x,y)와 이동 로봇(10)의 방향(orientation) θ 를 포함한 3개의 변수 r=(x,y,θ)로 나타낼 수 있다. 또한 본 발명이 적용되는 초음파 위치 및 방향 인식 시스템에서 초음파 송수신기는 임의의 복수 개를 사용할 수 있으나 본 실시예에서는 송수신이 가능한 범위내의 초음파 송신기가 4개, 초음파 수신기가 3개로 설정하고 진행하기로 한다. 즉, 천정에 복수의 초음파 송신기 Tx1(41), Tx2(42), Tx3(43), Tx4(44)가 특정위치 p Txi =(x Txi ,y Txi ,z Txi ), i=1,2,3,4 에 설치되어 있고, 이동 로봇(10)에는 3개의 초음파 수신기 Rx1(31), Rx2(32), Rx3(33)가 설치되어 있다.
한편 도 1에는, t1 시각의 로봇의 위치와 방향(101)이 t2 시각에 새로운 위치와 방향(102)으로 이동한 경우가 도시되어 있다. 이와 같은 로봇의 이동 시에, 하나의 송신 주기에서 선택된 송신기 1(41)이 송신한 초음파 신호를 3개의 초음파 수신기(31, 32, 33)가 시각 t1에 수신하여, 이를 정합 필터로 계산한 각각의 거리(111, 121, 131)와, 다음 주기에서 선택된 송신기 2(42)가 송신한 초음파 신호를 3개의 초음파 수신기(31, 32, 33)가 시각 t2에 수신하여, 이를 정합 필터로 계산한 거리(212, 222, 232)가 도시되어 있다. 또한 이와 함께, t1 시각의 로봇의 위치 및 방향(101)으로부터의 거리들(111, 121, 131)이 t2 시각에서 로봇의 위치 및 방향(102)으로부터의 거리로 시간 보상된 거리들(112, 122, 132)도 도시되어 있다. 이와 같이 t2 시각에 측정된 거리들(212, 222, 232)과 함께, 보상된 거리들(112, 122, 132)이 동일한 시각에서의 6개의 초음파 거리를 이루고, 이러한 초음파 거리들을 로봇의 위치 및 방향 인식에 사용함으로써 로봇의 위치 및 방향의 정확도를 현저히 향상시킬 수 있게 된다.
한편 이와 같은 이동 모습을 평면도로써 나타낸 도 2에서는, 이동 로봇의 t1에서의 위치와 방향(101)에서 t2에서의 위치와 방향(102)으로 이동하였을 때, 초음파 수신기는 처음 위치(311, 321, 331)에서 다음 위치(312, 322, 332)로 이동하였음이 도시되어 있다.
이와 같은 도 1 및 도 2의 실시예를 이용하여, 이동 로봇의 동적 위치 및 방향 인식의 구체적인 방법을 도 4의 순서도를 참조하여 상세히 후술한다.
도 3은 이동 로봇의 동적 위치 및 방향 인식 장치(500)의 구성을 나타내는 도면이다.
초음파 거리 측정부(510)는, 초음파 송신기에서 송신된 초음파 신호를 수신하여 초음파 거리를 측정하는 역할을 담당한다.
초음파 수신기(511)는 초음파 송신기에서 송신된 초음파 신호를 수신한다. 도 1의 실시예에서는 3개의 초음파 수신기(31,32,33)를 갖는 경우가 도시되었으며, 도 3에서는 이를 총칭하여 초음파 수신기(511)로 표시하였다.
A/D 변환기(512)는, 상기 수신된 초음파 신호를 A/D(analog to digital) 변환하며, 초음파 신호 저장부(513)는 A/D 변환기(512)에 의해 변환된 신호를 저장한다.
초음파 거리 측정 모듈(514)은 상기 수신하여 초음파 신호 저장부(513)에 저장된 초음파 신호를 이용하여, 송신기로부터 상기 초음파 수신기까지의 초음파 거리를 측정한다. 상기 초음파 거리 측정 모듈(514)은, 상기 수신된 초음파 신호로부터, 정합 필터에 의한 계산을 수행하여 초음파 거리를 산출할 수 있다.
오도메트리 정보 산출부(520)는, 이동 로봇의 선속도 및 각속도 정보를 포함하는 오도메트리(odometry) 정보를 산출하는 역할을 담당한다. 이러한 오도메트리 정보 산출부(520)는, 이동 로봇의 양 바퀴의 회전량을 엔코더 신호로서 출력하는 엔코더(521) 및 상기 엔코더 신호로부터 이동 로봇의 선속도 및 각속도 정보를 포함하는 오도메트리(odometry) 정보를 산출하는 오도메트리 정보 산출 모듈(522)을 포함할 수 있다.
시간 보상부(530)는, 상기 이동 로봇의 이전 주기 위치에서의 상기 초음파 수신기(511)로부터 타 송신기(이하 '이전 주기 송신기'라 한다)까지의 초음파 거리를, 상기 이동 로봇의 현재 위치에서 상기 초음파 수신기(511)로부터 상기 이전 주기 송신기까지의 초음파 거리로 환산하도록 시간 보상을 수행하는 역할을 담당한다. 이러한 시간 보상부(530)는, 상기 오도메트리 정보 산출부(520)에 의해 산출된 오도메트리 정보를 이용하여 상기 이동 로봇의 현재 위치와 방향을 산출하는 위치와 방향 산출 모듈(531), 상기 위치와 방향 산출 모듈(531)에서 산출된 이동 로봇의 위치와 방향으로부터 초음파 수신기(511)의 현재 위치를 산출하는 수신기 위치 산출 모듈(532) 및 상기 이동 로봇의 이전 주기 위치에서 상기 초음파 수신기와 상기 이전 주기 송신기 간의 초음파 거리 측정시 발생된 측정 오차를, 상기 수신기 위치 산출 모듈(532)에서 산출된 초음파 수신기의 위치와 상기 이전 주기 송신기 간의 거리에 적용하여, 시간 보상된 초음파 거리를 구하는 시간 보상 모듈(533)을 포함할 수 있다.
위치와 방향 산출부(540)는, 상기 초음파 거리 측정 모듈(514)에서 측정된 초음파 거리 및 상기 시간 보상부(530)에서 시간 보상된 초음파 거리로부터 상기 이동 로봇의 현재 위치와 방향을 산출하는 역할을 담당한다.
한편, 이동 로봇의 동적 위치 및 방향 인식 장치(500)는, 더욱 정확한 위치와 방향 산출을 위해 확장 칼만 필터(extended Kalman filter)부(550)를 더 포함할 수 있다.
확장 칼만 필터부(550)는, 상기 위치와 방향 산출부(540)에 의해 산출된 상기 이동 로봇의 현재 위치와 방향 정보 및, 상기 오도메트리 정보 산출부(520)에 의해 산출된 오도메트리 정보로부터 더욱 정확한 이동 로봇의 위치와 방향을 산출하는 역할을 담당한다.
이러한 확장 칼만 필터부(550)는, 상기 오도메트리 정보 및 이전 주기에 상기 확장 칼만 필터부(550)에서 최종 산출된 이동 로봇의 위치와 방향 값으로부터, 현재 주기의 이동 로봇의 위치와 방향 예측값 및 그 예측값의 표준편차의 제곱을 나타내는 공분산 행렬(covariance matrix) 예측값을 산출하는 위치와 방향값 예측부(551)를 포함하고, 또한 상기 위치와 방향 산출부(540)에 의해 산출된 상기 이동 로봇의 현재 위치와 방향 정보 및, 상기 위치와 방향값 예측부(551)에서 산출된 현재 주기의 이동 로봇의 위치와 방향 예측값 및 공분산 행렬 예측값을 이용하여 최종적으로 이동 로봇의 위치와 방향값 및 그 공분산행렬 갱신값을 산출하는 위치와 방향값 갱신부(552)를 포함할 수 있다.
도 4는 이동 로봇의 동적 위치 및 방향 인식이 이루어지는 일 실시예로서의 순서도이다. 이하 전술한 바와 같이 도 1 및 도 2의 실시예를 이용하여 설명한다.
우선, 현재 초음파를 송신할 송신기를 결정한다(S401). 현재 송수신이 가능한 위치에 있는 송신기의 수가 도 1의 실시예와 같이 4 개라 하면, 4 개의 송신기를 각각의 송신 주기마다 교대로 송신한다. 즉, 송신주기 1, 5, 9, ...에서는 송신기 1이 송신되며, 송신주기 2, 6, 10, ...에서는 송신기 2가 송신되며, 송신주기 3, 7, 11, ...에서는 송신기 3 이 송신되며, 끝으로 송신기 4는 4의 배수의 송신주기에 선택되어 송신을 수행한다. 송신 주기의 시작 시각에서의 이동 로봇과 초음파 송신기 간의 정보교환은 다음과 같다. 송신 선택 신호는 이동 로봇으로부터 별도의 고주파 신호를 이용하여 초음파 송신기로 전달된다. 송신 시각은 선택된 초음파 송신기로부터 동일한 고주파 신호를 사용하여 이동 로봇으로 전달된다.
그 송신기에서 송신된 초음파 신호를, 이동 로봇 동적 위치 및 방향 인식 장치의 초음파 수신기가 수신한다(S402). 도 1에서는, 전술한 바와 같이 하나의 송신주기에 송신된 하나의 초음파 신호를 3개의 수신기(31,32,33)가 각각 수신한다. 이렇게 수신한 초음파 신호로부터 초음파 거리 측정을 위한 정합 필터의 계산에 필요한 초음파의 신호를 A/D(analog to digital) 변환기를 사용하여 일정한 주기로 샘플링(sampling)하여 저장한다. 하나의 송신주기에 하나의 초음파 송신기가 송신하면, 초음파 수신기의 수만큼 초음파 신호들이 수신된다.
이렇게 수신된 초음파 신호를 이용하여, 이동 로봇의 현재 위치에서 송신기로부터 초음파 수신기까지의 초음파 거리를 측정한다(S403). 이러한 초음파 거리는, 수신한 초음파 신호에 대하여 정합 필터 계산을 수행하여 산출할 수 있다. 즉, 각각의 초음파 신호에 대하여 정합 필터 계산을 수행하면 초음파 수신기의 수신시각이 매우 정확히 구해진다. 등록 특허 "KR 10-0739506"호에 개시된 알고리즘을 이용하면 정합 필터의 정확도를 유지하면서도 계산량을 현저히 감소시킬 수 있다. 각 수신기의 수신 시각은 초음파 송신기와 각 수신기간의 거리에 비례하므로, 초음파의 속도로부터 각 초음파 수신기의 초음파 거리를 구한다. 각 초음파 수신 시각은 극소한 시간차가 있으므로, 수신 시각들의 평균으로 초음파 수신 시각이 정해진다.
이후, 상기 이동 로봇의 이전 주기 위치에서의 상기 초음파 수신기로부터 타 송신기, 즉 이전 주기에서 초음파를 송신한 송신기까지의 초음파 거리를, 상기 이동 로봇의 현재 위치에서 상기 초음파 수신기로부터 상기 이전 주기 송신기까지의 초음파 거리로 환산하도록 시간 보상을 수행하게 된다. 이렇게 시간 보상을 수행하는 단계는 다음과 같다.
먼저 이동 로봇의 양 바퀴의 회전량을 나타내는 엔코더 신호를 여러 번 짧은 주기로 읽어들여, 이들로부터 이전 주기 위치로부터 현재 위치로 이동하기까지의 시간 중에 다수의 선속도와 각속도를 산출한다(S404). 이러한 선속도, 각속도를 포함하는 정보를 오도메트리(odometry) 정보라 한다. 이렇게 선속도와 각속도를 산출하는 주기는, 송신기의 초음파 송신주기보다 현저히 작게 설정한다. 이러한 다수의 선속도와 각속도 정보로부터 이동 로봇의 현재 위치와 방향을 산출하게 된다(S405). 이렇게 이동 로봇의 위치와 방향이 결정되면 이로부터 그 로봇 상의 초음파 수신기의 현재 위치를 산출한다(S406). 상기 이동 로봇의 이전 주기 위치에서 상기 초음파 수신기와 상기 이전 주기 송신기 간의 초음파 거리 측정시 발생된 측정 오차를, 현재 위치에서 산출된 초음파 수신기의 위치와 상기 이전 주기 송신기 간의 거리에 적용하여, 시간 보상된 초음파 거리를 구하게 된다(S407).
이하에서는 이러한 시간 보상 단계를, 전술한 도 1 및 도 2의 실시예를 사용하여 더욱 상세히 설명한다.
로봇이 이동 시에 하나의 송신 주기에서 송신기 1(41)이 선택되어 송신한 초음파 신호를 3개의 초음파 수신기(31, 32, 33)가 시각 t1에 수신하여, 정합 필터로 계산된 각각의 거리를 d 11 (t 1 )(111), d 12 (t 1 )(121), d 13 (t 1 )(131)이라 한다. 다음 주기에서 송신기 2(42)가 선택되어 초음파 신호를 송신하고, 그 결과 3개의 초음파 수신기(31, 32, 33)가 시각 t2에 수신하여, 정합 필터로 계산된 각각의 거리를 d 21 (t 2 )(212), d 22 (t 2 )(222), d 23 (t 2 )(232)이라 한다. t1 시각에는 로봇의 위치가 로봇의 중심(101)으로 나타내지고, 로봇의 방향이 초음파 수신기 1(31) 의 방향으로 정해진다고 하고, 이를 로봇의 위치와 방향(101)로 표시하기로 한다. 또한 로봇의 위치 및 방향이 인식되어야 할 시각을 t2라 할 때, 이 t2 시각에는 로봇이 새로운 위치(102)로 이동하였다고 하자. t1 시각에 로봇의 위치 및 방향(101)으로부터의 거리 d 11 (t 1 )(111), d 12 (t 1 )(121), d 13 (t 1 )(131)들이 t2 시각에서 로봇의 위치 및 방향(102)으로부터의 거리 f 11 (t 2 )(112), f 12 (t 2 )(122), f 13 (t 2 )(132)로 시간 보상이 이루어지면, t2 시각에 측정된 거리들 d 21 (t 2 )(212), d 22 (t 2 )(222), d 23 (t 2 )(232)과 함께 보상된 거리들 f 11 (t 2 )(112), f 12 (t 2 )(122), f 13 (t 2 )(132)이 동일한 시각에서의 6개의 초음파 거리를 이룸으로써, 이 시간 보상된 초음파 거리들을 로봇의 위치 및 방향 인식에 사용하여, 로봇의 위치 및 방향의 정확도를 현저히 향상시킬 수 있게 된다.
이를 더욱 상세하게 설명하면, 각 초음파 송수신기로 거리를 측정한 시각(t1)에서의 이동 로봇(10)의 위치와 방향(101)이, 시각(t2)에서의 위치 및 방향(102)으로 이동하였으므로, t1시각에서 측정된 초음파 거리에 대하여 t2 시각의 초음파 거리로 시간 보상을 수행해야 한다. 이 과정에서 가능한한 정확하게 구한 t2 시각에서의 이동 로봇(10)의 위치와 방향(102)이 필요하다. 이동 로봇(10)의 경우 도 2에 도시된 바와 같이 두 바퀴(21, 22)를 구동시키는 모터의 엔코더 등으로부터 또는 별도의 센서로부터 이동 로봇(10)의 이동 속도 및 각속도와 같은 오도메트리(odometry) 정보를 획득할 수가 있다. 따라서 전 시각 t1에서 이동 로봇의 동적 위치 및 방향 인식 장치에서 구한 이동 로봇(10)의 위치 및 방향(101)을 기준으로 t2 시각까지 이동했을 때의 오도메트리 정보를 이용하여 t2 시각에서의 이동 로봇(10)의 엔코더에 의한 위치 및 방향(102)을 구하게 된다.
이동 로봇의 위치 및 방향(101)이 주어지면, 초음파 수신기의 위치가 결정될 수 있다. 구체적으로, 도 3에 도시된 바와 같이, 시각 t1에서의 이동 로봇(10)의 위치 및 방향(101)이 r(t 1 )=(x(t 1 ),y(t 1 ),θ(t 1 ))로 주어졌다고 하자. 이동 로봇의 상부에 정삼각형으로 배치된 3개의 초음파 수신기의 t1 시각의 위치 p Rxj (t 1 )=(x Rxj (t 1 ),y Rxj (t 1 ),0), j=1,2,3 (31, 32, 33)는 다음과 같이 구해진다. 이 때, 수신기의 위치는 평면상으로 투영된 2차원의 위치를 나타낸다.
Figure 112010071528985-pat00001
여기서 L은 도 3에 표시된 바와 같이 이동 로봇(10)의 중심(101)으로부터 각 초음파 수신기까지의 거리이다.
한편 이와 같은 이동 모습을 평면도로써 나타낸 도 2에서는, 이동 로봇의 t1에서의 위치와 방향(101)에서 t2에서의 위치와 방향(102)으로 이동하였을 때, 초음파 수신기는 처음 위치(311, 321, 331)에서 다음 위치(312, 322, 332)로 이동하였음이 도시되어 있다.
보다 일반적인 수식 표현을 위하여 수신주기를 k로, 송신기 번호를 i로, 수신기 번호를 j로 나타내기로 하자.
t k-1 시각(도 2에서 t1 시각)에서의 이동 로봇의 위치(101)에 근거하여 수학식 1을 사용하여 초음파 수신기 j(1)의 위치 p Rxj (t k-1 )=(xRxj(t k-1 ),yRxj(t k-1 ),0)(311)를 구하고, 이 수신기의 위치 p Rxj (t k-1 )=(x Rxj (t k-1 ),y Rxj (t k-1 ),0) (311)와 초음파 송신기 i(1) (41)의 고정된 위치 p Txi =(x Txi ,y Txi ,z Txi )를 직선으로 연결하여, 이 직선상에 초음파 송신기 i(41)로부터 d ij(t k-1 )만큼 떨어진 점 p ij (t k-1)(P111)을 다음과 같이 구할 수 있다.
Figure 112010071528985-pat00002
여기서 α는 다음 식으로 구해지는 상수이다.
Figure 112010071528985-pat00003
이 때 상기 직선 상에 초음파 거리의 측정오차에 의하여 오차 e ij (t k-1 )(E111)가 필연적으로 생기게 된다. 이제 시각 t k (도 2의 t2)에서 수신기 j의 위치 p Rxj (t k )=(x Rxj (t k ),y Rxj (t k ),0) (312)는 수학식 1을 활용하여 구할 수 있다. 이 수신기의 위치 p Rxj (t k )=(x Rxj (t k ),y Rxj (t k ),0) (312)와 초음파 송신기 i (41)의 고정된 위치 p Txi =(x Txi ,y Txi ,z Txi )를 직선(112)으로 연결하고, 수신기 j (312)로부터 e ij (t k-1 )(E111)과 3차원에서 동일한 오차 e ij (t k )(E112)를 갖는, 즉
Figure 112010071528985-pat00004
인 점 P112를 다음과 같이 구한다.
Figure 112010071528985-pat00005
여기서 β는 다음 식으로 구해지는 상수이다.
Figure 112010071528985-pat00006
이 점 P112와 송신기 1(41)과의 3차원 거리가 바로 시각 t2에서의 시간 보상된 초음파 거리 f ij (t k ) (112)를 나타낸다. 즉 시간 보상된 초음파 거리 f ij (t k ) (112)는 다음 식으로 구해진다.
Figure 112010071528985-pat00007
위와 같은 시간 보상을 초음파 수신기 2에 대해서 수신기의 위치가 321에서 322로 이동하였음을 고려하여 시간보상을 수행한다. 초음파 수신기 3에 대해서도 수신기의 위치가 331에서 332로 이동하였음을 고려하여 동일한 방법으로 시간보상을 수행한다.
이와 같이 각 수신기에서 측정된 초음파 거리 및 시간 보상된 초음파 거리들로부터 이동 로봇의 현재 위치와 방향을 산출한다(S408). 즉, 현 송신주기에서 3개의 초음파 수신기가 획득한 초음파 거리 3개와, 과거의 세 주기에서 3개의 초음파 수신기가 획득하고 시간 보상이 수행된 초음파 거리 9개, 총 12개의 거리 데이터를 입력으로 하여 이동 로봇의 초음파에 의한 위치 및 방향 (x, y, θ)세 개의 변수를 산출해 내는 것이다.
위치 및 방향 인식의 일례로 2단계 해법을 들 수 있다. 첫째 단계로써, 각 초음파 수신기 j당 4개의 데이터가 존재하므로, 이들로부터 초음파 수신기 j의 위치 p Rxj =(x Rxj ,y Rxj ,0)를 가장 작은 오차를 갖도록 계산해 낸다. 이 과정을 j=1,2,3 에 대하여 반복한다. 둘째 단계로써, 첫째 단계에서 구해 낸 3개의 초음파 수신기의 위치들 p Rxj =(x Rxj ,y Rxj ,0), j=1,2,3 로부터 최소의 오차를 갖는 이동 로봇의 위치 및 방향 (x, y, θ)을 구한다.
본 발명의 이동 로봇의 위치 및 방향 인식을 위하여, 최소한 두 개의 초음파 송신기가 초음파 송수신 범위 이내에 존재해야 한다. 왜냐하면, 위에서 초음파 수신기 j의 위치를 구하기 위해서는 최소한 2개의 초음파 송신기로부터의 거리 데이터가 필요하기 때문이다. 송수신 범위 이내의 초음파 송신기가 2개일 경우, 2개의 거리 데이터로부터 초음파 수신기의 위치 p Rxj =(x Rxj ,y Rxj ,0)인 두 변수를 계산해 내야 하므로 단 하나의 해를 가진다. 송수신 범위 이내의 초음파 송신기가 3개 이상인 m개의 경우, m개의 거리 데이터로부터 초음파 수신기의 위치 변수 2개를 계산해 내야 하므로 주어진 거리에 대한 식들에 대하여 최소한의 오차를 갖는 로봇의 수신기 위치의 해를 구할 수 있다.
한편 본 발명은 앞서 설명한 시간 보상을 구하는 방식에 대하여 두 가지 방식을 제안하는데, 각각 도 5 및 도 6을 참조하여 이하에서 후술한다.
도 5는 초음파 거리의 시간 보상 방법의 일 실시예를 나타내는 도면이다.
도 5의 실시예의 경우에는, m 개의 송신기가 존재하고 현재 시각을 tk, 이전 시각을 순차적으로 tk-1, tk-2, ... ,tk-(m-1)로 표현할 경우, 현재 위치 초음파 거리로의 시간 보상은, tk-1 시각에 측정된 초음파 거리 및 tk-2, ... ,tk-(m-1) 시각에 측정된 초음파 거리로부터 tk-1 시각으로 시간 보상된 m-2 개의 초음파 거리에 대하여 동일한 시간량(tk-tk-1)의 반복적인 시간 보상을 수행하게 된다.
m=4 인 경우에 대하여 도 4 및 도 5를 참조하여 이와 같은 과정을 좀 더 상세히 설명하면 다음과 같다.
첫 번째 송신 주기에서는 단계 1(S401)에서 초음파 송신기 1(Tx1)이 선택되어 송신을 수행하며, 단계 2(S402)에서 초음파 수신기들이 초음파 신호 Rx1j, j = 1, 2, 3를 수신한다. 다음으로 단계 3(S403)의 정합 필터의 계산에 의하여 수신 시각 t1, 초음파 거리 d 1j (t 1 ), j=1,2,3 이 계산된다.
두 번째 송신 주기에서는 단계 1(S401)에서 초음파 송신기 2(Tx2)가 선택되고, 단계 2(S402)에서 초음파 수신기들이 초음파 신호 Rx2j, j=1, 2, 3를 수신한다. 다음으로 단계 3(S403)의 정합 필터의 계산에 의하여 수신 시각 t2, 초음파 거리 d 2j (t 2 ), j=1,2,3 이 계산되며, 초음파 거리 d 1j (t 1 ), j=1,2,3 의 시간 보상이 수행되어 시간 보상된 초음파 거리 f 1j (t 2 ), j=1,2,3 이 구해진다.
세 번째 송신 주기에서는 단계 1(S401)에서 송신기 3(Tx3)이 선택되고, 단계 2(S402)에서 초음파 수신기들이 초음파 신호 Rx3j, j=1, 2, 3를 수신한다. 다음으로 단계 3(S3)의 정합 필터의 계산에 의하여 수신 시각 t3, 초음파 거리 d 3j (t 3 ), j=1,2,3 이 계산되며, 초음파 거리 d 2j (t 2 ), j=1,2,3 의 시간 보상이 수행되어 시간 보상된 초음파 거리 f 2j (t 3 ), j=1,2,3 이 구해진다. 아울러 기 시간 보상된 f 1j (t 2 ), j=1,2,3 대하여 동일한 시간 보상을 수행하여 f 1j (t 3 ), j=1,2,3 이 구해진다.
이와 같은 과정을 네 송신 주기까지 반복 수행하면 모두 12개의 초음파 거리 d 1j (t 1 ),d 2j (t 2 ),d 3j (t 3 ),d 4j (t 4 ), j=1,2,3 이 얻어지며, d 3j (t 3 ), j=1,2,3 와 기 보상된 f 2j (t 3 ), f 1j (t 3 ), j=1,2,3 에 대하여 초음파 거리의 시간 보상을 수행하여(S407) f 3j (t 4 ), f 2j (t 4 ), f 1j (t 4 ), j=1,2,3 를 위치 및 방향 인식의 입력으로 전달한다.
초음파 송신기 4개가 순차적으로 초음파 신호를 송신하는 경우, 각 주기에서 시간 보상된 초음파 거리를 구하는 송신기 번호 조합은 다음 표와 같다.
k i i' i'' i'''
4의 배수 4 3 2 1
4의 배수+1 1 4 3 2
4의 배수+2 2 1 4 3
4의 배수+3 3 2 1 4
표 1의 규칙을 설명하면 다음과 같다. 즉 k 번째 송신 주기에서는 다음과 같이 선택될 송신기 i를 구한다.
Figure 112010071528985-pat00008
여기서 %는 modulo 연산을 나타내며, 위 수학식의 첫 번째 행은 k를 4로 나눈 나머지를 i로 한다는 의미이다. 초음파 송신기 i의 초음파 송신이 수행되면, 다수의 초음파 수신에 의해 R xij 가 얻어지며, 정합 필터에 의하여 수신 시각 tk와 초음파 거리 d ij (t k )가 얻어진다. 거리의 시간 보상은 전 송신 주기에서 얻어진 초음파 거리 d i'j (t k-1 )과 기 시간 보상된 초음파 거리 f i''j (t k-1 ),f i'''j (t k-1 ) 들로부터 f i'j (t k ),f i''j (t k ),f i'''j (t k )를 계산하는 것이다. 이는 도 5의 각 시간 보상된 거리의 조합들(701 내지 705)에 도시되어 있다.
일반적으로 m개의 초음파 송신기가 초음파 송수신 범위 이내에 존재하고, n개의 초음파 수신기가 존재하는 경우, 상기 알고리즘을 다음과 같이 수정할 수 있다. 단계 S401의 k 번째 송신 주기에서는 다음과 같이 선택될 송신기 i를 구한다.
Figure 112010071528985-pat00009
단계 S402의 초음파 수신기 신호 수신에서는 n 개의 초음파 수신기가 각각 초음파 신호를 수신한다. 단계 S403의 초음파 거리 계산을 위한 정합 필터 계산은 n개의 수신된 초음파 신호 각각에 대하여 정합 필터의 계산을 수행하여 수신 시각 tk와 n개의 초음파 거리 d ij (t k ),j=1,2,...,n을 출력한다. 이후 단계 S407에서 초음파 거리의 시간 보상에 있어서, 시각 tk-1의 시간 보상된 (m-1)n 개의 초음파 거리들 f i'j (t k-1 ),f i''j (t k-1 ),...,f i(m-1)j (t k-1 ), j=1,2,...,n (여기서 i(m-1)은 (m-1)회 이전 송신 주기의 송신기를 나타냄)에 동일한 양의 시간 보상 (t k -t k-1 )을 수행하여 시간 보상된 초음파 거리들을 구한다.
도 6은 초음파 거리의 시간 보상 방법의 다른 실시예를 나타내는 도면이다.
도 6의 실시예의 경우에는, m 개의 송신기가 존재하고 현재 시각을 tk, 이전 시각을 순차적으로 tk-1, tk-2, ... ,tk-(m-1)로 표현할 경우, 현재 위치 초음파 거리로의 시간 보상은, tk-1 시각에 측정된 초음파 거리, tk-2 시각에 측정된 초음파 거리, ..., 및 tk-(m-1) 시각에 측정된 초음파 거리에 대하여 각각 서로 다른 시간량(tk-tk-1, tk-tk-2, ... , tk-tk-(m-1))의 시간 보상을 수행하게 된다.
즉, 초음파 송신기 4개가 순차적으로 초음파 신호를 송신하는 경우, 과거 세 송신 주기에서 구해진 d i'j (t k-1 ),d i''j (t k-2 ),d i'''j (t k-3 ), j=1,2,3 으로부터 초음파 거리의 시간 보상을 적용하여 f i'j (t k ),f i''j (t k ),f i'''j (t k ), j=1,2,3 를 구하는 것이다. 이 경우 과거의 측정된 초음파 거리들로부터 각각 다른 시간 구간 (t k -t k-1 ,t k -t k-2 ,t k -t k-3 )을 적용하여야 한다. 이는 도 6의 각 시간 보상된 거리의 조합들(801 내지 805)에 도시되어 있다.
즉, 단계 S407에서 초음파 거리의 시간 보상의 경우, 시각 tk에 있어서, (m-1)개 각각의 초음파 송신기로부터 n 개의 초음파 수신기로 측정된 (m-1)n 개의 거리들 d i(m-1)j (t k-(m-1) ),...,d i''j (t k-2 ),d i'j (t k-1 ), j=1,2,3,...,n 에 대하여 측정 시각에 따라 각각에 다른 양의 시간 보상 (t k -t k-(m-1) ,...,t k -t k-2 ,t k -t k-1 )을 수행하여 시간 보상된 초음파 거리들을 구한다.
도 7은 이동 로봇의 동적 위치 및 방향 인식이 이루어지는 다른 실시예로서의 순서도이다.
도 4에서 초음파 거리를 이용하여 이동 로봇의 현재 위치와 방향을 산출한 후(S408), 더욱 정확한 위치와 방향을 산출하기 위해 확장 칼만 필터(extended Kalman filter)를 이용할 수 있다. 확장 칼만 필터는, 상기 단계(S408)에서 산출된 상기 이동 로봇의 현재 위치와 방향 정보 및, 엔코더 신호로부터 산출한 선속도 및 각속도 정보를 포함하는 오도메트리(odometry) 정보로부터, 이동 로봇의 위치와 방향을 산출하게 된다.
본 도면의 단계 S701 내지 S708은 도 4의 단계 S401 내지 S408과 같다. 이후, 단계 S704에서 획득한 상기 오도메트리 정보 및 이전 주기에 확장 칼만 필터에서 산출된 이동 로봇의 위치와 방향 값으로부터, 확장 칼만 필터부(550)의 위치와 방향값 예측부(551)(도 3 참조)는, 현재 주기의 이동 로봇의 위치와 방향 예측값
Figure 112010071528985-pat00010
및 그 예측값의 표준편차의 제곱을 나타내는 공분산 행렬(covariance matrix) 예측값
Figure 112010071528985-pat00011
을 산출하게 된다(S709).
이후 확장 칼만 필터부(550)의 위치와 방향값 갱신부(552)(도 3 참조)는, 단계 S708에서 초음파 거리들로부터 산출된 현재 위치와 방향값
Figure 112010071528985-pat00012
과, 단계 S709에서 확장 칼만 필터부(550)의 위치와 방향값 예측부(551)에 의해 산출된 현재 주기의 이동 로봇의 위치와 방향 예측값
Figure 112010071528985-pat00013
및 공분산 행렬 예측값
Figure 112010071528985-pat00014
를 이용하여, 최종적으로 이동 로봇의 위치와 방향값
Figure 112010071528985-pat00015
및 그 공분산행렬의 갱신값
Figure 112010071528985-pat00016
을 산출하게 된다(S710). 이러한 확장 칼만 필터부(550)의 위치와 방향값 갱신부(552)에 의한 위치 및 방향값 은 다음 주기에서 확장 칼만 필터부(550)의 위치와 방향값 예측부(551)에 입력값으로 사용되며, 동시에 초음파 거리 시간 보상(S707) 산출시에 이동 로봇의 위치 및 방향값으로 입력된다.
10: 이동 로봇 21,22: 바퀴
31,32,33: 초음파 수신기 41,42,43,44: 초음파 송신기
101,102: 이동 로봇의 위치
111,121,131,212,222,232: 측정된 초음파 거리
112,122,132: 보상된 초음파 거리
P111,P112: 초음파 거리 측정된 수신기 위치
E111,E112: 초음파 거리 측정 오차
500: 이동 로봇의 동적 위치 및 방향 인식 장치
510: 초음파 거리 측정부
511: 초음파 수신기 512: A/D 변환기
513: 초음파 신호 저장부 514: 초음파 거리 측정 모듈
520: 오도메트리 정보 산출부
521: 엔코더 522: 오도메트리 정보 산출 모듈
530: 시간 보상부
531: 위치와 방향 산출 모듈 532: 수신기 위치 산출 모듈
533: 시간 보상 모듈
540: 위치와 방향 산출부
550: 확장 칼만 필터부
551: 위치와 방향값 예측부 552: 위치와 방향값 갱신부

Claims (16)

  1. 이동 로봇에 장착된 동적 위치 및 방향 인식 장치가, 이동 로봇의 이동시 그 위치 및 방향을 동적으로 인식하는 방법으로서,
    (a) 특정 송신기(이하 '현 주기 송신기'라 한다)에서 송신된 초음파 신호를, 이동 로봇 동적 위치 및 방향 인식 장치의 초음파 수신기가 수신하는 단계;
    (b) 상기 수신된 초음파 신호를 이용하여, 이동 로봇의 현재 위치에서 상기 현 주기 송신기로부터 상기 초음파 수신기까지의 초음파 거리를 측정하는 단계;
    (c) 상기 이동 로봇의 이전 주기 위치에서의 상기 초음파 수신기로부터 타 송신기(이하 '이전 주기 송신기'라 한다)까지의 초음파 거리를, 상기 이동 로봇의 현재 위치에서 상기 초음파 수신기로부터 상기 이전 주기 송신기까지의 초음파 거리로 환산하도록 시간 보상을 수행하는 단계; 및
    (d) 상기 단계(b)에서 측정된 초음파 거리 및 단계(c)에서 시간 보상된 초음파 거리로부터 상기 이동 로봇의 현재 위치와 방향을 산출하는 단계
    를 포함하고,
    상기 단계(c)는,
    (c11) 이동 로봇의 양 바퀴의 회전량을 나타내는 엔코더 신호로부터, 다수의 선속도와 각속도를 산출하는 단계;
    (c12) 상기 다수의 선속도와 각속도 정보로부터 상기 이동 로봇의 현재 위치와 방향을 산출하는 단계;
    (c13) 상기 단계(c12)에서 산출된 이동 로봇의 위치와 방향으로부터 초음파 수신기의 현재 위치를 산출하는 단계; 및
    (c14) 상기 이동 로봇의 이전 주기 위치에서 상기 초음파 수신기와 상기 이전 주기 송신기 간의 초음파 거리 측정시 발생된 측정 오차를, 상기 단계(c13)에서 산출된 초음파 수신기의 위치와 상기 이전 주기 송신기 간의 거리에 적용하여, 시간 보상된 초음파 거리를 구하는 단계
    를 포함하며,
    상기 단계(c11)의 선속도와 각속도 산출주기는,
    송신기의 초음파 송신주기보다 작은 것
    을 특징으로 하는 이동 로봇의 동적 위치 및 방향 인식 방법.
  2. 청구항 1에 있어서,
    상기 초음파 수신기는 두 개 이상 설치되며,
    상기 단계(a) 내지 단계(c)는 설치된 상기 각각의 초음파 수신기에 대하여 수행되고,
    상기 단계(d)에서 산출되는 이동 로봇의 동적 위치 및 방향은, 상기 각 초음파 수신기에서 수신된 초음파로부터 측정되거나 시간 보상된 모든 초음파 거리를 이용하여 산출되는 것
    을 특징으로 하는 이동 로봇의 동적 위치 및 방향 인식 방법.
  3. 청구항 1에 있어서,
    상기 단계(b)의 초음파 거리는,
    상기 단계(a)에서 수신한 초음파 신호에 대하여 정합 필터 계산을 수행하여 산출되는 것
    을 특징으로 하는 이동 로봇의 동적 위치 및 방향 인식 방법.
  4. 삭제
  5. 삭제
  6. 청구항 1에 있어서,
    m 개의 송신기가 존재하고 현재 시각을 tk, 이전 시각을 순차적으로 tk-1, tk-2, ... ,tk-(m-1)로 표현할 경우,
    상기 단계(c)에서 현재 위치 초음파 거리로의 시간 보상은,
    tk-1 시각에 측정된 초음파 거리,
    tk-2 시각에 측정된 초음파 거리, ..., 및
    tk-(m-1) 시각에 측정된 초음파 거리
    에 대하여 각각 서로 다른 시간량(tk-tk-1, tk-tk-2, ... , tk-tk-(m-1))의 시간 보상을 수행하는 것
    을 특징으로 하는 이동 로봇의 동적 위치 및 방향 인식 방법.
  7. 청구항 1에 있어서,
    m 개의 송신기가 존재하고 현재 시각을 tk, 이전 시각을 순차적으로 tk-1, tk-2, ... ,tk-(m-1)로 표현할 경우,
    상기 단계(c)에서 현재 위치 초음파 거리로의 시간 보상은,
    tk-1 시각에 측정된 초음파 거리 및
    tk-2, ... ,tk-(m-1) 시각에 측정된 초음파 거리로부터 tk-1 시각으로 시간 보상된 m-2 개의 초음파 거리
    에 대하여 동일한 시간량(tk-tk-1)의 반복적인 시간 보상을 수행하는 것
    을 특징으로 하는 이동 로봇의 동적 위치 및 방향 인식 방법.
  8. 청구항 1에 있어서,
    상기 단계(d) 이후,
    (e) 확장 칼만 필터(extended Kalman filter)가, 상기 단계(d)에서 산출된 상기 이동 로봇의 현재 위치와 방향 정보 및, 엔코더 신호로부터 산출한 선속도 및 각속도 정보를 포함하는 오도메트리(odometry) 정보로부터, 이동 로봇의 위치와 방향을 산출하는 단계
    를 더 포함하는 것을 특징으로 하는 이동 로봇의 동적 위치 및 방향 인식 방법.
  9. 청구항 8에 있어서,
    상기 단계(e)는,
    (e11) 상기 오도메트리 정보 및 이전 주기에 상기 확장 칼만 필터에서 산출된 이동 로봇의 위치와 방향 값으로부터, 현재 주기의 이동 로봇의 위치와 방향 예측값 및 그 예측값의 표준편차의 제곱을 나타내는 공분산 행렬(covariance matrix) 예측값을 산출하는 단계; 및
    (e12) 상기 단계(d)에서 산출된 상기 이동 로봇의 현재 위치와 방향 정보 및, 상기 단계(e11)에서 산출된 현재 주기의 이동 로봇의 위치와 방향 예측값 및 공분산 행렬 예측값을 이용하여 최종적으로 이동 로봇의 위치와 방향값 및 그 공분산행렬 갱신값을 산출하는 단계
    를 포함하는 것을 특징으로 하는 이동 로봇의 동적 위치 및 방향 인식 방법.
  10. 이동 로봇의 이동시 그 위치 및 방향을 동적으로 인식하는 장치로서,
    송신기에서 송신된 초음파 신호를 수신하는 초음파 수신기;
    상기 수신된 초음파 신호를 이용하여, 송신기로부터 상기 초음파 수신기까지의 초음파 거리를 측정하는 초음파 거리 측정 모듈;
    상기 이동 로봇의 이전 주기 위치에서의 상기 초음파 수신기로부터 타 송신기(이하 '이전 주기 송신기'라 한다)까지의 초음파 거리를, 상기 이동 로봇의 현재 위치에서 상기 초음파 수신기로부터 상기 이전 주기 송신기까지의 초음파 거리로 환산하도록 시간 보상을 수행하는 시간 보상부;
    상기 초음파 거리 측정 모듈에서 측정된 초음파 거리 및 상기 시간 보상부에서 시간 보상된 초음파 거리로부터 상기 이동 로봇의 현재 위치와 방향을 산출하는 위치와 방향 산출부;
    이동 로봇의 선속도 및 각속도 정보를 포함하는 오도메트리(odometry) 정보를 산출하는 오도메트리 정보 산출부; 및
    상기 위치와 방향 산출 모듈에 의해 산출된 상기 이동 로봇의 현재 위치와 방향 정보 및, 상기 오도메트리 정보 산출부에 의해 산출된 오도메트리 정보로부터 이동 로봇의 위치와 방향을 산출하는 확장 칼만 필터(extended Kalman filter)부
    를 포함하고,
    상기 확장 칼만 필터부는,
    상기 오도메트리 정보 및 이전 주기에 상기 확장 칼만 필터부에서 최종 산출된 이동 로봇의 위치와 방향 값으로부터, 현재 주기의 이동 로봇의 위치와 방향 예측값 및 그 예측값의 표준편차의 제곱을 나타내는 공분산 행렬(covariance matrix) 예측값을 산출하는 위치와 방향값 예측부; 및
    상기 위치와 방향 산출 모듈에 의해 산출된 상기 이동 로봇의 현재 위치와 방향 정보 및, 상기 위치와 방향값 예측부에서 산출된 현재 주기의 이동 로봇의 위치와 방향 예측값 및 공분산 행렬 예측값을 이용하여 최종적으로 이동 로봇의 위치와 방향값 및 그 공분산행렬 갱신값을 산출하는 위치와 방향값 갱신부
    를 포함하는 것을 특징으로 하는 이동 로봇의 동적 위치 및 방향 인식 장치.
  11. 청구항 10에 있어서,
    상기 초음파 거리 측정 모듈은,
    상기 수신된 초음파 신호로부터, 정합 필터에 의한 계산을 수행하여 초음파 거리를 산출하는 것
    을 특징으로 하는 이동 로봇의 동적 위치 및 방향 인식 장치.
  12. 삭제
  13. 청구항 10에 있어서,
    상기 오도메트리 정보 산출부는,
    이동 로봇의 양 바퀴의 회전량을 엔코더 신호로서 출력하는 엔코더; 및
    상기 엔코더 신호로부터 이동 로봇의 선속도 및 각속도 정보를 포함하는 오도메트리(odometry) 정보를 산출하는 오도메트리 정보 산출 모듈
    을 포함하는 것을 특징으로 하는 이동 로봇의 동적 위치 및 방향 인식 장치.
  14. 청구항 10에 있어서,
    상기 시간 보상부는,
    상기 오도메트리 정보 산출부에 의해 산출된 오도메트리 정보를 이용하여 상기 이동 로봇의 현재 위치와 방향을 산출하는 위치와 방향 산출 모듈;
    상기 위치와 방향 산출 모듈에서 산출된 이동 로봇의 위치와 방향으로부터 초음파 수신기의 현재 위치를 산출하는 수신기 위치 산출 모듈; 및
    상기 이동 로봇의 이전 주기 위치에서 상기 초음파 수신기와 상기 이전 주기 송신기 간의 초음파 거리 측정시 발생된 측정 오차를, 상기 수신기 위치 산출 모듈에서 산출된 초음파 수신기의 위치와 상기 이전 주기 송신기 간의 거리에 적용하여, 시간 보상된 초음파 거리를 구하는 시간 보상 모듈
    을 포함하는 것을 특징으로 하는 이동 로봇의 동적 위치 및 방향 인식 장치.
  15. 삭제
  16. 삭제
KR1020100108275A 2010-11-02 2010-11-02 이동 로봇의 동적 위치 및 방향 인식 방법 및 장치 KR101263482B1 (ko)

Priority Applications (1)

Application Number Priority Date Filing Date Title
KR1020100108275A KR101263482B1 (ko) 2010-11-02 2010-11-02 이동 로봇의 동적 위치 및 방향 인식 방법 및 장치

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
KR1020100108275A KR101263482B1 (ko) 2010-11-02 2010-11-02 이동 로봇의 동적 위치 및 방향 인식 방법 및 장치

Publications (2)

Publication Number Publication Date
KR20120046558A KR20120046558A (ko) 2012-05-10
KR101263482B1 true KR101263482B1 (ko) 2013-05-13

Family

ID=46265625

Family Applications (1)

Application Number Title Priority Date Filing Date
KR1020100108275A KR101263482B1 (ko) 2010-11-02 2010-11-02 이동 로봇의 동적 위치 및 방향 인식 방법 및 장치

Country Status (1)

Country Link
KR (1) KR101263482B1 (ko)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR102126888B1 (ko) * 2020-03-05 2020-07-08 주식회사 메이버스 초음파를 이용한 작업장 안전 관리 시스템

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR100561855B1 (ko) * 2002-12-30 2006-03-16 삼성전자주식회사 로봇용 로컬라이제이션 시스템
KR100962674B1 (ko) * 2009-07-28 2010-06-11 엘아이지넥스원 주식회사 이동 로봇의 위치 추정 방법 및 이를 위한 이동 로봇

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR100561855B1 (ko) * 2002-12-30 2006-03-16 삼성전자주식회사 로봇용 로컬라이제이션 시스템
KR100962674B1 (ko) * 2009-07-28 2010-06-11 엘아이지넥스원 주식회사 이동 로봇의 위치 추정 방법 및 이를 위한 이동 로봇

Also Published As

Publication number Publication date
KR20120046558A (ko) 2012-05-10

Similar Documents

Publication Publication Date Title
CN109548141B (zh) 基于卡尔曼滤波算法的室内环境基站坐标位置标定方法
CN111860589B (zh) 一种多传感器多目标协同探测信息融合方法、系统
CN107064861B (zh) 用于估计到达角的设备以及用于波束成形的设备
CN103926474B (zh) 相控阵天线单元特性近场测量方法
KR100561855B1 (ko) 로봇용 로컬라이제이션 시스템
RU2624461C1 (ru) Способ определения координат объекта
CN111055289B (zh) 机器人的手眼标定方法、装置、机器人及存储介质
KR101767924B1 (ko) 다중 목표물 위치 추정 시스템 및 방법
KR100882590B1 (ko) 위치 판단 장치 및 방법
CN110471029B (zh) 一种基于扩展卡尔曼滤波的单站无源定位方法及装置
JP2010190629A (ja) 位置推定装置、無線端末、位置推定システム
KR101263482B1 (ko) 이동 로봇의 동적 위치 및 방향 인식 방법 및 장치
EP3041736B1 (en) One way time of flight distance measurement
JP2006201084A (ja) 目標追尾装置
CN103092219A (zh) 一种fsm远程实时控制时间补偿系统及方法
CN113960523A (zh) 基于fpga的通用超宽带校正测向方法及系统
US8660815B2 (en) Methods and apparatus for filtering noise in a three-dimensional position measurement system
RU2196341C1 (ru) Способ определения параметров движения маневрирующего объекта
KR20110059334A (ko) 고속 이동체의 주행 궤도 예측에 의한 위치인식 방법
US20220321164A1 (en) Angle information estimation of ultra-wideband wireless signals
JP5441611B2 (ja) レーダ装置
KR101094990B1 (ko) 수중 환경에서 직교한 안테나를 이용한 동시 지도 작성 기반 위치 인식 방법
CN112637897A (zh) 一种无线信号传输系统和方法
CN112504187B (zh) 一种应用于移动测量的自主导航系统和方法
Hsu et al. An improved ultrasonic-based localization using reflection method

Legal Events

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

Payment date: 20160427

Year of fee payment: 4

LAPS Lapse due to unpaid annual fee