KR101174423B1 - 초음파 거리 측정 방법 및 이를 이용한 이동 로봇의 자세를 구하는 방법 - Google Patents

초음파 거리 측정 방법 및 이를 이용한 이동 로봇의 자세를 구하는 방법 Download PDF

Info

Publication number
KR101174423B1
KR101174423B1 KR1020100004466A KR20100004466A KR101174423B1 KR 101174423 B1 KR101174423 B1 KR 101174423B1 KR 1020100004466 A KR1020100004466 A KR 1020100004466A KR 20100004466 A KR20100004466 A KR 20100004466A KR 101174423 B1 KR101174423 B1 KR 101174423B1
Authority
KR
South Korea
Prior art keywords
ultrasonic
mobile robot
time delay
calculating
integer
Prior art date
Application number
KR1020100004466A
Other languages
English (en)
Other versions
KR20110079405A (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 숭실대학교산학협력단
Publication of KR20110079405A publication Critical patent/KR20110079405A/ko
Application granted granted Critical
Publication of KR101174423B1 publication Critical patent/KR101174423B1/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

Abstract

초음파 거리 측정 방법 및 이를 이용한 이동 로봇의 자세를 구하는 방법이 제공된다. 본 발명의 실시예에 따른 초음파 거리 측정 방법은 초음파 발신기에서 발신한 초음파를 초음파 수신기가 수신할 때의 시간 지연과 초음파의 속도를 이용하여 초음파 발신기와 초음파 수신기 사이의 거리를 측정하는 방법에 있어서, 시간 지연을 계산하는 단계는 수신 초음파의 각 주기에서 포락선의 크기 gi를 계산하는 단계, 수신 초음파의 각 주기의 포락선의 크기 gi가 일정한 임계값(threshold) 이하로 계속되는 신호가 없는 구간을 제외하는 잡음구간 배제 단계; 시간 지연 d를 초음파 주기시간 c의 정수부분 i 및 소수부분 f로 구분하고, 발신 초음파와 수신 초음파 사이의 위상차로부터 소수부분 f를 계산하는 단계, 시간 지연 d를 초음파 주기시간 c의 정수부분 i 및 소수부분 f로 구분하여 잡음 구간 배제 단계에서 잡음을 제거한 수신 초음파의 정합 필터
Figure 112010003241566-pat00043
를, 여기서 ei는 발신 초음파의 각 주기에서의 포락선의 크기이고 hi=gi(1-f)+gi+1f인, 계산하는 단계 및 정합 필터 y(jc)가 최대값을 갖는 j의 값인 정수부분 i를 구하여, 시간 지연 d=ic+f를 구하는 단계를 포함하며, gi는
Figure 112010003241566-pat00044
를, 여기서 여기서 Ci와 Si는
Figure 112010003241566-pat00045
,
Figure 112010003241566-pat00046
이고 T는 임의의 샘플링 구간인, 이용하여 계산한다.

Description

초음파 거리 측정 방법 및 이를 이용한 이동 로봇의 자세를 구하는 방법 {Method measuring distance using ultrasonic and method measuring pose of mobile robot using the same method}
본 발명은 초음파 거리 측정 방법 및 이를 이용한 이동 로봇의 자세를 구하는 방법에 관한 것으로, 보다 상세하게는 초음파 발신기와 초음파 수신기 사이의 초음파의 시간 지연과 초음파의 속도를 이용하여 초음파 발신기와 초음파 수신기 사이의 거리를 구하고 이를 이용하여 이동 로봇의 자세를 구하는 초음파 거리 측정 방법 및 이를 이용한 이동 로봇의 자세를 구하는 방법에 관한 것이다.
초음파의 송수신을 이용하여 거리를 측정하는 방법은 초음파 거리계, 초음파 진단기, 로봇의 장애물 검출 등 다방면에 사용되고 있다. 측정 물체를 향해 초음파를 발신하여 반사되는 신호를 수신하고 반사에서 수신까지의 시간을 측정하여 측정된 시간을 왕복 거리로 환산하여 거리를 측정하는 것이 가장 보편적인 방법이다. 또한, 초음파 발신기와 초음파 수신기가 각각 이격된 위치에 분리되어 있는 경우에
는, 초음파 발신기와 초음파 수신기 사이에 초음파가 도달하는 데 걸리는 시간을 측정하여 초음파 발신기와 초음파 수신기 사이의 거리를 측정할 수가 있다.
초음파 발신기와 초음파 수신기 사이의 초음파 도달 시간인 시간 지연 d를 측정하는 방법으로 신호를 증폭하여 비교기(comparator)에 의하여 일정한 전압 이상이 되는 시간을 측정하는 방법이 사용된다. 이 방법은 하드웨어가 간단하지만, 신호의 크기가 측정 오차에 직접적인 영향을 주며, 특히 원거리 측정의 경우 측정 신호가 미약하면 측정 오차가 상당히 커지는 단점이 있다.
따라서, 시간 지연 d의 정확한 측정을 위하여 정합 필터(matched filter)를 이용한 방법이 사용되고 있다. 정합 필터는 수신된 신호의 처리에 잡음의 영향을 최소화하는, 즉 신호 대 잡음비(Signal to Noise Ratio: SNR)를 최대화하는 우수한 방식이나 연산시간이 많이 소요된다는 단점이 있다.
등록 특허 KR 10-0739506-0000에서는 시간 지연 d를 초음파 주기 시간의 정부 부분 및 소수 부분으로 구분하는 방법으로 정합필터의 계산량을 줄일 수 있는 방법을 개시하고 있다.
하지만, 초음파 발신기와 초음파 수신기의 개수가 증가하면, 정합 필터의 계산량도 증가하기 때문에 계산량을 더욱 줄일 수 있는 방법이 필요하다.
예를 들어, 다수의 초음파 발신기를 천정에 설치하고 두 개의 초음파 수신기를 이동 로봇에 설치하여, 각 초음파 발신기와 각 초음파 수신기 사이의 거리를 측정하고 삼각법을 이용하여 이동 로봇의 위치와 방향을 포함하는 이동 로봇의 자세를 측정하는 방법이 있다. 이 경우 초음파 송수신기의 개수가 많아 질수록 더욱 많은 거리 데이터를 이용하여 이동 로봇의 자세를 구하기 때문에 정확도를 향상시킬 수가 있다. 그러므로, 이동 로봇의 자세에 대한 정확도를 향상시키기 위해 초음파 송수신기의 개수를 늘릴 필요가 있다. 이때, 초음파 송수신기 사이의 거리를 측정 횟수가 증가하게 되면 정합필터의 계산량이 증가하게 된다. 따라서, 정합필터의 계산량을 더욱 줄이도록 하는 것이 필요한 것이다.
본 발명이 해결하려는 과제는 초음파를 이용한 거리 측정 방법에 있어서 시간 지연을 계산하기 위한 정합필터의 계산량을 줄이는 방법을 제공하는 것이다.
본 발명이 해결하려는 또 다른 과제는 복수의 초음파 송수신기를 이용하여 이동 로봇의 자세의 정확도를 향상시키는 방법을 제공하는 것이다.
본 발명이 해결하려는 과제들은 이상에서 언급한 과제들로 제한되지 않으며, 언급되지 않은 또 다른 과제들은 아래의 기재로부터 당업자에게 명확하게 이해될 수 있을 것이다.
상기 과제를 해결하기 위한 본 발명의 일 실시예에 따른 초음파 거리 측정 방법은 초음파 발신기에서 발신한 초음파를 초음파 수신기가 수신할 때의 시간 지연과 상기 초음파의 속도를 이용하여 상기 초음파 발신기와 상기 초음파 수신기 사이의 거리를 측정하는 방법에 있어서, 상기 시간 지연을 계산하는 단계는 수신 초음파의 각 주기에서 포락선의 크기를 계산하는 단계; 상기 수신 초음파의 각 주기의 포락선의 크기 gi가 일정한 임계값(threshold) 이하로 계속되는 신호가 없는 구간을 제외하는 잡음구간 배제 단계; 상기 시간 지연 d를 초음파 주기시간 c의 정수부분 i 및 소수부분 f로 구분하고, 발신 초음파와 상기 수신 초음파 사이의 위상차로부터 상기 소수부분 f를 계산하는 단계; 상기 시간 지연 d를 초음파 주기시간 c의 정수부분 i 및 소수부분 f로 구분하여 상기 잡음 구간 배제 단계에서 잡음을 제거한 상기 수신 초음파의 정합 필터
Figure 112010003241566-pat00001
를, 여기서 ei는 상기 발신 초음파의 각 주기에서의 포락선의 크기이고 hi=gi(1-f)+gi+1f인, 계산하는 단계; 및 상기 정합 필터 y(jc)가 최대값을 갖는 j의 값인 정수부분 i를 구하여, 시간 지연 d=ic+f를 구하는 단계를 포함하며, 상기 gi는
Figure 112010003241566-pat00002
를, 여기서 여기서 Ci와 Si는
Figure 112010003241566-pat00003
,
Figure 112010003241566-pat00004
이고 T는 임의의 샘플링 구간인, 이용하여 계산한다.
상기 과제를 해결하기 위한 본 발명의 일 실시예에 따른 이동 로봇의 자세를 구하는 방법은 천정에 형성된 복수의 초음파 발신기와 이동 로봇에 형성된 복수의 초음파 수신기 각각에 대하여, 상기 초음파 발신기에서 발신한 초음파를 상기 초음파 수신기가 수신할 때의 시간 지연과 상기 초음파의 속도를 이용하여 상기 초음파 발신기와 상기 초음파 수신기 사이의 거리를 각각 계산하는 단계; 각각의 계산된 상기 초음파 발신기와 상기 초음파 수신기 사이의 거리를 이용하여 상기 복수의 초음파 수신기의 위치를 계산하는 단계; 및 상기 복수의 초음파 수신기의 위치를 이용하여 상기 이동 로봇의 자세를 계산하는 단계를 포함하며, 상기 시간 지연은 상기 수신 초음파의 각 주기에서 포락선의 크기를 계산하는 단계; 상기 수신 초음파의 각 주기의 포락선의 크기 gi가 일정한 임계값(threshold) 이하로 계속되는 신호가 없는 구간을 제외하는 잡음구간 배제 단계; 상기 시간 지연 d를 초음파 주기시간 c의 정수부분 i 및 소수부분 f로 구분하고, 상기 발신 초음파와 상기 수신 초음파 사이의 위상차로부터 상기 소수부분 f를 계산하는 단계; 상기 시간 지연 d를 초음파 주기시간 c의 정수부분 i 및 소수부분 f로 구분하여 상기 잡음 구간 배제 단계에서 잡음을 제거한 상기 수신 초음파의 정합 필터
Figure 112010003241566-pat00005
를, 여기서 ei는 상기 발신 초음파의 각 주기에서의 포락선의 크기이고 hi=gi(1-f)+gi+1f인, 계산하는 단계; 및 상기 정합 필터 y(jc)가 최대값을 갖는 j의 값인 정수부분 i를 구하여, 시간 지연 d=ic+f를 구하는 단계를 포함하며 계산되는데, 상기 gi는
Figure 112010003241566-pat00006
를, 여기서 여기서 Ci와 Si는
Figure 112010003241566-pat00007
,
Figure 112010003241566-pat00008
이고 T는 임의의 샘플링 구간인, 이용하여 계산한다.
본 발명의 기타 구체적인 사항들은 상세한 설명 및 도면들에 포함되어 있다.
상기한 바와 같은 본 발명의 초음파 거리 측정 방법 및 이를 이용한 이동 로봇의 자세를 구하는 방법에 따르면 다음과 같은 효과가 하나 혹은 그 이상 있다.
첫째, 초음파를 이용한 거리 측정 방법에 있어서 시간 지연을 계산하기 위한 정합 필터의 계산량을 현저하게 줄일 수가 있어서 저성능의 계산기를 이용하여서도 초음파를 이용한 거리 측정이 가능하다는 장점이 있다.
둘째, 초음파를 이용한 거리 측정 방법에 있어서 시간 지연을 계산을 계사하기 위한 정합 필터의 계산량을 현저하게 줄일 수가 있어서 다수의 초음파 발신기와 수신기를 이용하여 이동 로봇의 자세를 측정할 수가 있어서 측정된 이동 로봇의 자세 정확도를 향상시킬 수가 있다는 장점도 있다.
도 1은 정합 필터를 이용한 거리 측정 방법의 순서도이다.
도 2는 도 1에서 시간 지연 계산 단계를 더욱 구체적으로 도시한 순서도이다.
도 3은 본 발명의 일 실시예에 따른 이동 로봇에 장착된 초음파 수신기와 천정에 장착된 초음파 발신기의 배치를 도시한 도면이다.
도 4는 본 발명의 일 실시예에 따른 이동 로봇의 자세를 구하는 방법의 순서도이다.
도 5는 3개의 초음파 수신기의 위치와 실제 위치 사이의 오차를 이용하여 로봇의 실제 위치를 구하는 방법을 설명하기 위한 도면이다.
도 6은 본 발명의 일 실시예에 따른 확장 칼만 필터를 개념을 도시한 블록도이다.
본 발명의 이점 및 특징, 그리고 그것들을 달성하는 방법은 첨부되는 도면과 함께 상세하게 후술되어 있는 실시예들을 참조하면 명확해질 것이다. 그러나 본 발명은 이하에서 개시되는 실시예들에 한정되는 것이 아니라 서로 다른 다양한 형태로 구현될 것이며, 단지 본 실시예들은 본 발명의 개시가 완전하도록 하며, 본 발명이 속하는 기술분야에서 통상의 지식을 가진 자에게 발명의 범주를 완전하게 알려주기 위해 제공되는 것이며, 본 발명은 청구항의 범주에 의해 정의될 뿐이다. 명세서 전체에 걸쳐 동일 참조 부호는 동일 구성 요소를 지칭한다.
다른 정의가 없다면, 본 명세서에서 사용되는 모든 용어(기술 및 과학적 용어를 포함)는 본 발명이 속하는 기술분야에서 통상의 지식을 가진 자에게 공통적으로 이해될 수 있는 의미로 사용될 수 있을 것이다. 또 일반적으로 사용되는 사전에 정의되어 있는 용어들은 명백하게 특별히 정의되어 있지 않는 한 이상적으로 또는 과도하게 해석되지 않는다.
초음파 발신기와 초음파 수신기 사이의 거리 x는 초음파의 진행 속도를 v라고 하고 초음파 발신기로부터 발신된 초음파가 초음파 수신기에 수신하는데 까지 걸리는 시간인 시간 지연을 d라고 할 때, 다음과 같이 구해진다.
<수학식 1>
x = v*d
거리 x의 정확도는 v와 d의 정확도에 비례하는데, 초음파의 속도 v의 정확도는 온도를 측정하여 온도에 상응하는 초음파의 속도를 적용시킴으로써 가능하다. 따라서, 거리 x의 정확도를 향상시키기 위해서는 시간 지연 d를 정확하게 측정하는 것이 중요하다.
먼저, 본 발명의 일 실시예에 따른 초음파 거리 측정 방법 및 이를 이용한 이동 로봇의 자세를 구하는 방법을 설명하기에 앞서, 등록 특허 KR 10-0739506-0000에 개시된 내용을 기초로 시간 지연 d를 주기시간(cycle time)의 정수부분 및 소수부분으로 구분하여 정합 필터를 계산하는 방법을 설명하기로 한다. 더욱 상세한 설명은 등록 특허 KR 10-0739506-0000에 개시된 내용을 참조할 수가 있다.
도 1은 정합 필터를 이용한 거리 측정 방법의 순서도이다.
정합 필터를 이용한 거리 측정 방법은 초음파 발신 단계(S110), 초음파 수신 단계(S120), 및 시간 지연 계산 단계(S130)를 포함할 수가 있다.
초음파 발신 단계(S110)는 초음파 발신기에서 거리 측정을 위한 초음파를 발신하는 단계이다. 여기서 거리 측정이라고 함은 전술한 바와 같이 초음파 발신기와 초음파 수신기 사이의 거리이다. 초음파 발사 단계에서 발신되는 발신 초음파 s(t)는 포락선(envelop) e(t)와 사인(sine) 진동함수의 곱으로 수학식 2와 같이 나타낼 수가 있다.
<수학식 2>
s(t)=e(t)sin(ωt)
여기서 ω는 발신 초음파의 각속도, 즉 발신 초음파 수파수 f의 2π배를 나타낸다. 포락선 e(t)가 발신 초음파의 한 주기시간 c 내에서는 일정하다고 근사화하면, 발신 초음파 s(t)는 수학식 3과 같이 나타낼 수가 있다.
<수학식 3>
Figure 112010003241566-pat00009
,
여기서 ic≤t,(i+1)c, i = 0, 1,..., L-1 인 정수
발신 초음파 주기 수와 송신기의 잔여 진동에 의하여 송신 신호의 의미 있는 구간은 L주기로 제한된다.
초음파 수신 단계(S120)는 초음파 수신기가 초음파 발신기에서 발신된 초음파를 수신하는 단계이다. 초음파 수신 단계에서 수신되는 수신 초음파 m(t)는 포락선 gi과 사인 진동함수의 곱으로 수학식 4와 같이 근사적으로 나타낼 수가 있다.
<수학식 4>
Figure 112010003241566-pat00010
,
여기서 ic≤t<(i+1)c, i = 0, 1,..., M-1 인 정수이고, n(t)는 잡음이다.
M은 원하는 최대측정거리 값이다.
시간 지연 계산 단계(S130)는 초음파 수신기가 수신한 수신 초음파로부터 시간 지연 d를 계산하는 단계이다.
시간 지연 d를 주기 시간 c로 나눈 몫을 i, 나머지를 f라고 하면 다음과 같은 관계가 성립한다.
<수학식 5>
d = ic + f
여기서 i는 정수이고, f는 c 미만의 실수이다. 특히, ωf는 m(t)의 s(t)에 대한 위상 지연(phase delay)를 나타낸다.
도 2는 도 1에서 시간 지연 계산 단계를 더욱 구체적으로 도시한 순서도이다.
시간 지연을 계산하는 단계(S130)는 수신 초음파의 각 주기의 포락선의 크기를 계산하는 단계(S131), 잡음 구간을 배제하는 단계(S132), 시간 지연 d의 소수 부분인 f를 계산하는 단계(S133), 수신 초음파의 정합 필터를 계산하는 단계(S134) 및 시간 지연 d의 정수 부분인 i를 계산하여 시간 지연 d를 계산하는 단계(S135)를 포함할 수가 있다.
각 주기의 포락선의 크기를 계산하는 단계(S131)는 수신 초음파의 각 주기에서의 포락선의 크기를 계산하는 단계이다.
수신 신호의 시점기반 i 번째 주기에서의 Ci, Si를 아래의 수학식 6과 같이 정의한다.
<수학식 6>
Figure 112010003241566-pat00011
잡음 n(t)의 주파수는 ω와 관련이 없으므로 수학식 6은 수학식 7과 같이 정의할 수가 있다.
<수학식 7>
Figure 112010003241566-pat00012
수학식 7에서 위상 지연 ωf는
<수학식 8>
Figure 112010003241566-pat00013
을 사용하여 구할 수가 있다.
샘플링 구간 T의 정수배가 초음파 주기 시간 c가 되도록, 즉 c=NT가 되도록 T를 결정하면, Ci, Si는 다음과 같이 이산시간영역에서 근사적으로 계산된다.
<수학식 9>
Figure 112010003241566-pat00014
특히, N=4일 경우 다음과 같이 간략하게 계산할 수가 있다.
<수학식 10>
Figure 112010003241566-pat00015
이때 수신 신호의 각 주기의 포락선의 크기 gi는 아래의 수학식 11로 계산된다.
<수학식 11>
Figure 112010003241566-pat00016
잡음 구간을 배제하는 단계(S132)는 수신 초음파의 각 주기의 포락선의 크기 gi가 일정 임계값(threshold) 이하로 계속되는 신호가 없는 구간은 제외하는 단계이다.
시간 지연 d의 소수 부분인 f를 계산하는 단계(S133)는 수학식 10과 수학식 8을 이용하여 계산할 수가 있다.
다음, 수신 초음파의 정합 필터를 계산한다(S134).
gi에 대하여 위상 지연 ωf를 고려한 주기별 포락선의 크기 hi는 구간 ic+f≤t<(i+1)c+f에서의 포락선의 크기를 나타내며 다음의 수학식 12로 계산된다.
<수학식 12>
hi=gi(1-f)+gi+1f
정합 필터 계산 단계(S134)는 시간 지연 d를 주기 시간 c의 정부 부분 i 및 소수 부분 f로 구분하여 잡음 구간 배제 단계(S132)에서 잡음을 제거한 수신 초음파의 발신 및 수신 신호의 위상차로부터 소수 부분 f를 계산함으로써 다음과 같이 주기 기반의 정합 필터를 계산하는 단계이다.
<수학식 13>
Figure 112010003241566-pat00017
시간 지연 d를 계산하는 단계(S135)는 y(jc)가 최대값을 갖는 j의 값 i를 구하여 <수학식 5>로부터 시간 지연 d를 계산하는 단계이다.
전술한 바와 같이 시간 지연 d를 구하면 수학식 1을 이용하여 초음파 발신기와 초음파 수신기 사이의 거리를 측정할 수가 있다.
전술한 시간 지연 d를 계산하는 과정에 있어서, Ci, Si의 값은 A/D 변환기의 값을 계산하여 그대로 사용하기 때문에 정수값을 가지도록 만들 수가 있다. 하지만 수학식 11과 같이 제곱근 계산을 하게 될 경우, 일반적으로 실수형 계산이 필요하고 정수 부분만 취하도록 정수형 제곱근 계산을 하더라도 많은 양의 루프 반복이 필요하다.
이를 위해 본 발명에서는 수학식 3과 수학식 4를 각각 다음과 같이 수정한다.
<수학식 14>
Figure 112010003241566-pat00018
<수학식 15>
Figure 112010003241566-pat00019
정합 필터는 s(t)*m(t)의 회전(convolution) 값이 최대값을 가지게 하는 t의 값을 찾는 것이다. 그런데, 수학식 3, 수학식 4에서 ei와 gi는 양수이므로 ei를 ei 2으로 gi를 gi 2으로 바꾸어 수학식 14 및 수학식 15와 같이 수정하고, s'(t)와 m'(t)를 이용하여 정합 필터를 계산하여 최대값을 가지게 하는 t의 값을 구하게 되면 s(t)*m(t)를 이용한 결과와 동일한 결과를 얻을 수가 있다. 따라서, 수학식 11에서 제곱근을 제거하여 수학식 16으로 수정하여 정합 필터를 수행하여도 결과는 동일하다.
<수학식 16>
Figure 112010003241566-pat00020
수학식 16을 이용하여 계산을 할 경우 제곱근 계산을 없앨 수가 있어서 정합 필터의 계산을 간략하게 할 수가 있다.
또한, 본 발명에서는 위상 지연 ωf를 계산할 때 룩업테이블(lookup table)을 이용할 수가 있다. 정합 필터를 계산하는 과정에서 ωf를 구하는 식은 수학식 8과 같다. atan의 계산은 역시 실수형 계산을 하여야 하므로 계산량이 많아지게 하는 요인이 된다. 따라서, 본 발명에서는 atan 계산 부분을 1차원의 룩업테이블로 만들어서 메모리에 저장시킨 후, Ci, Si의 값에 따라서 룩업테이블을 참조하여 계산을 할 수가 있다. 역탄젠트에 대하여 별도의 계산 없이 메모리에서 값을 읽어 들이면 되기 때문에 계산량을 줄일 수가 있다.
또한, 본 발명에서는 수학식 12를 이용하여 위상 지연 ωf를 고려한 포락선의 크기 hi를 계산할 때 실수형 계산을 정수형 계산으로 다음과 같이 변형시킬 수가 있다.
수학식 8로부터 계산한 소수 부분 f를 이용하여 위상 지연을 고려한 포락선의 크기 hi는 수학식 12와 같이 나타낼 수가 있다. 하지만 f는 0≤f<1의 값을 가지므로 hi의 계산을 실수 계산을 포함하게 된다. 이를 위해 f를 다음과 같이 변형시킬 수가 있다.
<수학식 17>
Figure 112010003241566-pat00021
여기서 정수부분인 fn만 취하고 fp는 버린다. 따라서, f=fn/210으로 근사화시킬 수가 있고, 이를 수학식 12에 적용하면 다음과 같다.
<수학식 18>
Figure 112010003241566-pat00022
여기서 gi+1, gi, fn은 모두 정수이므로 210으로 나누어 주는 계산은 10비트의 우측 쉬프트(right shift) 연산으로 대체시킬 수가 있다. 쉬프트 연산의 경우 실수형의 나눗셈 연산보다 계산량을 줄일 수 있기 때문이다.
전술한 바와 같이 본 발명에서는 실수형 계산을 현저하게 줄일 수가 있어서 정합 필터를 이용하여 초음파 발신기와 초음파 수신기 사이의 거리를 계산할 때 계산량을 현저하게 줄일 수가 있다.
이하, 전술한 초음파 거리 측정 방법을 적용하여 본 발명의 일 실시예에 따른 이동 로봇의 자세를 구하는 방법에 대해서 설명하기로 한다.
도 3은 본 발명의 일 실시예에 따른 이동 로봇에 장착된 초음파 수신기와 천정에 장착된 초음파 발신기의 배치를 도시한 도면이고, 도 4는 본 발명의 일 실시예에 따른 이동 로봇의 자세를 구하는 방법의 순서도이며, 도 5는 3개의 초음파 수신기의 위치와 실제 위치 사이의 오차를 이용하여 로봇의 실제 위치를 구하는 방법을 설명하기 위한 도면이며, 도 6은 본 발명의 일 실시예에 따른 확장 칼만 필터를 개념을 도시한 블록도이다.
먼저, 본 발명에 따른 이동 로봇(1000)은 모터 등에 의해 회전하는 바퀴(1100)를 구비하고 자율 주행을 할 수 있는 이동 로봇(1000)이다. 이동 로봇(1000)의 자세(pose)라고 하면 공간상의 이동 로봇(1000)의 위치(x, y)(position)와 회전 각도인 방향각(θ)(direction)를 의미한다. 본 발명에서는 이동 로봇(1000)의 자세를 구하기 위해 도 3에 도시되어 있는 것과 같이 천정에 복수의 초음파 발신기를 구비하고, 이동 로봇(1000)에는 복수의 초음파 수신기를 구비하도록 한다. 도 3에는 초음파 발신기가 천장에 4개 설치되고, 초음파 수신기가 이동 로봇(1000)의 상부에 3개 형성되어 있는 것을 도시하고 있다. 초음파 발신기와 초음파 수신기의 개수는 이에 한정되지 않고 더 많은 개수로 장착될 수 있음은 물론이다. 후술하겠지만, 본 발명에서는 복수의 초음파 발신기와 복수의 초음파 수신기에 대하여 각 초음파 발신기와 각 초음파 수신기 사이의 거리를 정합 필터를 이용하여 계산을 한다. 정합 필터를 계산할 때 전술한 바와 같이 계산량을 현저하게 줄일 수 있기 때문에 더 많은 개수의 초음파 발신기와 초음파 수신기를 장착하여 이동 로봇(1000)의 자세를 구할 수가 있다. 따라서, 더 많은 개수의 초음파 발신기와 초음파 수신기를 이용하여 이동 로봇(1000)의 자세를 계산하기 때문에 이동 로봇(1000)의 자세에 대한 정확도를 향상시킬 수가 있는 것이다. 이하, 설명에서는 초음파 수신기의 개수는 3개 초음파 발신기의 개수는 2개 이상인 경우에 대해서 설명하기로 한다.
초음파 발신기는 천정 위의 각 모서리에 도 3과 같이 배치할 수가 있다. 또한 초음파 수신기는 이동 로봇(1000)의 상부에 정삼각형을 형성하도록 배치할 수가 있다.
이동 로봇(1000)의 자세를 구하기 위해 먼저 복수의 초음파 발신기와 복수의 초음파 수신기에 대하여 각각 초음파 발신기와 초음파 수신기 사이의 거리를 계산할 수가 있다(S210). 초음파 발신기와 초음파 수신기 사이의 거리는 전술한 본 발명의 일 실시예에 따른 초음파 거리 측정 방법을 이용하여 계산하는 것이 바람직하다. 전술한 바와 같이 정합 필터를 이용하여 시간 지연 d를 계산할 때 실수형 계산을 줄여서 계산량을 현저하게 줄일 수가 있다.
다음, 각각의 계산된 초음파 발신기와 초음파 수신기 사이의 거리를 이용하여 복수의 초음파 수신기의 위치를 각각 계산할 수가 있다(S220). 이때, 특정 초음파 수신기의 위치는 특정 초음파 수신기와 복수의 초음파 발신기 사이의 각 거리에 대하여 최소 오차 제곱법을 이용하여 계산할 수가 있다.
예를 들어, 수신기 1의 위치는 송신기 1과 수신기 1 사이의 거리 d1,1, 송신기 2와 수신기 1사이의 거리 d2,1, 송신기 3과 수신기 1사이의 거리 d3,1, 송신기 4와 수신기 1사이의 거리 d4,1에 대하여 최소 오차 제곱법을 이용하여 계산할 수가 있다.
동일한 방법으로 수신기 2의 위치 및 수신기 3의 위치를 계산할 수가 있다.
다음, 계산된 복수의 초음파 수신기의 위치를 이용하여 이동 로봇(1000)의 자세를 계산할 수가 있다(S230). 이때에도 최소 오차 제곱법을 이용하여 계산할 수가 있다.
전술한 단계에서 계산된 초음파 수신기의 위치를 Rx1', Rx2', Rx3'라고 하고, 실제 초음파의 위치를 Rx1, Rx2, Rx3라고 하면 계산된 초음파 수신기의 위치와 실제 초음파 수신기의 위치 사이의 거리 오차 e1, e2, e3는 아래의 수학식 19와 같다.
<수학식 19>
Figure 112010003241566-pat00023
여기서, Rx1'=(x1', y1'), Rx2'=(x2', y2'), Rx3'=(x3', y3')이고, Rx1=(x1, y1), Rx2=(x2, y2), Rx3=(x3, y3)이다. 이때, z1= z2=z3=0라고 가정할 수가 있다.
도 5에 도시되어 있는 것과 같이 Rx1, Rx2, Rx3를 실제 이동 로봇(1000)의 위치(R=(x, y)), 방향각(θ), 이동 로봇(1000)의 중심과 초음파 수신기 사이의 거리(L)로 나타내면 수학식 <20>과 같다.
<수학식 20>
Figure 112010003241566-pat00024
수학식 20을 이용하여 수학식 19를 수정하면 각 거리 오차는 다음과 같이 나타낼 수가 있다.
<수학식 21>
Figure 112010003241566-pat00025
전체 오차 E는
<수학식 22>
Figure 112010003241566-pat00026
와 같이 표현되며, E를 최소화 시키는 (x, y, θ)를 찾는 문제로 나타낼 수가 있다. 이를 해결하기 위해 최소 오차 제곱법 문제를 미분(
Figure 112010003241566-pat00027
)를 이용하여 계산하면,
<수학식 23>
Figure 112010003241566-pat00028
와 같은 식을 얻을 수가 있다. 여기서 이동 로봇(1000)의 위치 (x, y)는 계산된 3개의 초음파 수신기의 무게 중심이 된다. 이 경우 초음파 수식기의 개수가 많아 질수록 이동 로봇(1000)의 자세 정확도를 향상시킬 수가 있다.
마지막으로 확장 칼만 필터(extended Kalman filter)를 적용하여 이동 로봇(1000)의 자세를 갱신하는 단계(S240)를 더 포함할 수가 있다.
이동 로봇(1000)의 경우 바퀴(1100)를 구동시키는 모터의 인코더 등으로부터 또는 별도의 센서로부터 이동 로봇(1000)의 이동 속도 및 각속도와 같은 오도메트리(odometer) 정보를 획득할 수가 있다. 이동하는 이동 로봇(1000)의 오도메트리 정보를 활용하여 초음파로 계산된 이동 로봇(1000)의 자세와 융합하면 이동 로봇(1000)의 자세의 정확도를 더욱 향상시킬 수가 있다. 본 발명에서는 오도메트리 정보와 초음파로 측정된 이동 로봇(1000)의 자세 값의 융합시키기 위해 도 6에서와 같이 확장 칼만 필터를 사용한다.
이동 로봇(1000)의 양 바퀴로부터 획득한 바퀴의 회전수(φl, φr)를 이용하여, PWS (power wheel steering) 기구학으로부터 이동 로봇(1000)의 속도 υ(k)와 각속도 ω(k)를 구할 수가 있다. PWS방식의 로봇이란 2개의 독립적인 구동 바퀴를 가지는 로봇이고, PWS 기구학은 양 구동 바퀴의 회전수로부터 로봇의 속도와 각속도를 구하는 것을 말한다. 이동 로봇(1000)의 속도, 각속도 및 이전에 갱신된 이동 로봇(1000)의 위치와 방향(x, y, θ)을 입력으로 차동 이동 로봇 기구학을 구하면 이동 로봇(1000)의 예측된 상태
Figure 112010003241566-pat00029
를 구할 수 있다.
확장 칼만 필터를 이용하여 이동 로봇(1000)의 자세를 갱신하는 단계에서는 전술한 바와 같이 초음파을 이용하여 계산된 이동 로봇(1000)의 자세
Figure 112010003241566-pat00030
와 예측된 상태
Figure 112010003241566-pat00031
를 융합하여 실제 이동 로봇(1000)의 자세를 추정하는 단계이다. 이로부터 갱신된 상태 값
Figure 112010003241566-pat00032
은 확장 칼만 필터의 출력이다.
이상 첨부된 도면을 참조하여 본 발명의 실시예들을 설명하였지만, 본 발명이 속하는 기술분야에서 통상의 지식을 가진 자는 본 발명이 그 기술적 사상이나 필수적인 특징을 변경하지 않고서 다른 구체적인 형태로 실시될 수 있다는 것을 이해할 수 있을 것이다. 그러므로 이상에서 기술한 실시예들은 모든 면에서 예시적인 것이며 한정적이 아닌 것으로 이해해야만 한다.
1000: 이동 로봇 1100: 바퀴

Claims (10)

  1. 초음파 발신기에서 발신한 초음파를 초음파 수신기가 수신할 때의 시간 지연과 상기 초음파의 속도를 이용하여 상기 초음파 발신기와 상기 초음파 수신기 사이의 거리를 측정하는 방법에 있어서,
    상기 시간 지연을 계산하는 단계는
    수신 초음파의 각 주기에서 포락선의 크기 gi를 계산하는 단계;
    상기 수신 초음파의 각 주기의 포락선의 크기 gi가 일정한 임계값(threshold) 이하로 계속되는 신호가 없는 구간을 제외하는 잡음구간 배제 단계;
    상기 시간 지연 d를 초음파 주기시간 c의 정수부분 i 및 소수부분 f로 구분하고, 발신 초음파와 상기 수신 초음파 사이의 위상차로부터 상기 소수부분 f를 계산하는 단계;
    상기 시간 지연 d를 초음파 주기시간 c의 정수부분 i 및 소수부분 f로 구분하여 상기 잡음 구간 배제 단계에서 잡음을 제거한 상기 수신 초음파의 정합 필터
    Figure 112011100816642-pat00033
    를, 여기서 ei는 상기 발신 초음파의 각 주기에서의 포락선의 크기이고 hi=gi(1-f)+gi+1f인, 계산하는 단계; 및
    상기 정합 필터 y(jc)가 최대값을 갖는 j의 값인 정수부분 i를 구하여, 시간 지연 d=ic+f를 구하는 단계를 포함하며,
    상기 소수부분 f는 위상지연
    Figure 112011100816642-pat00053
    식에 의해 계산되며,
    상기 atan 계산부분을 룩업 테이블로 만들어서, Ci와 Si의 값에 따라 상기 룩업 테이블로부터 값을 읽어 들여 계산하고,
    상기 gi는
    Figure 112011100816642-pat00034
    를, 여기서 여기서 Ci와 Si는
    Figure 112011100816642-pat00035
    ,
    Figure 112011100816642-pat00036
    , N=c/T인 정수,
    Figure 112011100816642-pat00054
    , j는 정수, w는 각속도이고 T는 임의의 샘플링 구간인, 이용하여 계산하는 초음파 거리 측정 방법.
  2. 삭제
  3. 삭제
  4. 제 1 항에 있어서,
    상기 hi의 계산에서,
    f*210=fn+fp로 정의하고, 여기서 fn 는 정수, fp는 0=fp<1인 실수, fp의 값을 버려서, f= fn/210으로 계산을 하면,
    상기 hi=gi(1-f)+gi+1f는 hi=gi+(gi+1-gi) fn/210으로 정의할 수가 있고, (gi+1-gi) fn/210 부분의 계산은 정수값인 gi+1, gi, fn를 이용하여 계산한 (gi+1-gi) fn에 10비트의 우측 시프트 연산으로 수행되는 초음파 거리 측정 방법.
  5. 천정에 형성된 복수의 초음파 발신기와 이동 로봇에 형성된 복수의 초음파 수신기 각각에 대하여, 상기 초음파 발신기에서 발신한 초음파를 상기 초음파 수신기가 수신할 때의 시간 지연과 상기 초음파의 속도를 이용하여 상기 초음파 발신기와 상기 초음파 수신기 사이의 거리를 각각 계산하는 단계;
    각각의 계산된 상기 초음파 발신기와 상기 초음파 수신기 사이의 거리를 이용하여 상기 복수의 초음파 수신기의 위치를 계산하는 단계; 및
    상기 복수의 초음파 수신기의 위치를 이용하여 상기 이동 로봇의 자세를 계산하는 단계를 포함하며,
    상기 시간 지연은
    상기 수신 초음파의 각 주기에서 포락선의 크기를 계산하는 단계;
    상기 수신 초음파의 각 주기의 포락선의 크기 gi가 일정한 임계값(threshold) 이하로 계속되는 신호가 없는 구간을 제외하는 잡음 구간 배제 단계;
    상기 시간 지연 d를 초음파 주기시간 c의 정수부분 i 및 소수부분 f로 구분하고, 상기 발신 초음파와 상기 수신 초음파 사이의 위상차로부터 상기 소수부분 f를 계산하는 단계;
    상기 시간 지연 d를 초음파 주기시간 c의 정수부분 i 및 소수부분 f로 구분하여 상기 잡음 구간 배제 단계에서 잡음을 제거한 상기 수신 초음파의 정합 필터
    Figure 112011100816642-pat00038
    를, 여기서 ei는 상기 발신 초음파의 각 주기에서의 포락선의 크기이고 hi=gi(1-f)+gi+1f인, 계산하는 단계; 및
    상기 정합 필터 y(jc)가 최대값을 갖는 j의 값인 정수부분 i를 구하여, 시간 지연 d=ic+f를 구하는 단계를 포함하며 계산되는데,
    상기 소수부분 f는 위상지연
    Figure 112011100816642-pat00055
    식에 의해 계산되며,
    상기 atan 계산부분을 룩업 테이블로 만들어서, Ci와 Si의 값에 따라 상기 룩업 테이블로부터 값을 읽어 들여 계산하고,
    상기 gi는
    Figure 112011100816642-pat00039
    를, 여기서 여기서 Ci와 Si는
    Figure 112011100816642-pat00040
    ,
    Figure 112011100816642-pat00041
    , N=c/T인 정수,
    Figure 112011100816642-pat00056
    , j는 정수, w는 각속도이고 T는 임의의 샘플링 구간인, 이용하여 계산하는 이동 로봇의 자세를 구하는 방법.
  6. 제 5 항에 있어서,
    상기 초음파 수신기 위치를 계산하는 단계는 상기 초음파 수신기와 상기 복수의 초음파 발신기 사이의 각 거리에 대하여 최소 오차 제곱법을 이용하여 계산하는 이동 로봇의 자세를 구하는 방법.
  7. 제 5 항에 있어서,
    상기 이동 로봇의 자세를 계산하는 단계는 상기 복수의 초음파 수신기 각각의 위치에 대하여 최소 오차 제곱법을 이용하여 계산하는 이동 로봇의 자세를 구하는 방법.
  8. 제 5 항에 있어서,
    상기 이동 로봇의 이동 속도 및 각속도와 상기 이동 로봇의 자세를 확장 칼만 필터에 입력하여 상기 이동 로봇의 자세를 갱신하는 이동 로봇의 자세를 구하는 방법.
  9. 제 5 항에 있어서,
    상기 초음파 발신기의 개수는 2개 이상이고, 상기 초음파 수신기의 개수는 3개 이상인 이동 로봇의 자세를 구하는 방법.
  10. 제 5 항에 있어서,
    상기 초음파 수신기의 개수가 3개일 때,
    상기 이동 로봇의 자세 (x, y, θ)는, 여기서 (x, y)는 상기 이동 로봇의 위치이고 θ는 상기 이동 로봇의 방향각,
    Figure 112010003241566-pat00042
    을 이용하여 구하는, 여기서 (x1', y1'), (x2', y2'), (x3', y3')은 각각의 상기 계산한 초음파 수신기의 위치인 이동 로봇의 자세를 구하는 방법.
KR1020100004466A 2009-12-30 2010-01-18 초음파 거리 측정 방법 및 이를 이용한 이동 로봇의 자세를 구하는 방법 KR101174423B1 (ko)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
KR20090134130 2009-12-30
KR1020090134130 2009-12-30

Publications (2)

Publication Number Publication Date
KR20110079405A KR20110079405A (ko) 2011-07-07
KR101174423B1 true KR101174423B1 (ko) 2012-08-17

Family

ID=44918786

Family Applications (1)

Application Number Title Priority Date Filing Date
KR1020100004466A KR101174423B1 (ko) 2009-12-30 2010-01-18 초음파 거리 측정 방법 및 이를 이용한 이동 로봇의 자세를 구하는 방법

Country Status (1)

Country Link
KR (1) KR101174423B1 (ko)

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR100739506B1 (ko) * 2006-09-25 2007-07-13 한국과학기술원 정합필터의 간략한 계산을 사용한 초음파 거리 정밀측정방법
KR100860966B1 (ko) * 2006-06-05 2008-09-30 삼성전자주식회사 이동 로봇의 위치 추정 방법 및 장치

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR100860966B1 (ko) * 2006-06-05 2008-09-30 삼성전자주식회사 이동 로봇의 위치 추정 방법 및 장치
KR100739506B1 (ko) * 2006-09-25 2007-07-13 한국과학기술원 정합필터의 간략한 계산을 사용한 초음파 거리 정밀측정방법

Also Published As

Publication number Publication date
KR20110079405A (ko) 2011-07-07

Similar Documents

Publication Publication Date Title
JP7320635B2 (ja) 補間された仮想開口レーダ追跡のためのシステムおよび方法
JP5009812B2 (ja) 合成開口を用いた無線ベースの位置決定システム
US8299959B2 (en) Apparatus and imaging method with synthetic aperture for determining an incident angle and/or a distance
JP5460148B2 (ja) 測位装置及びプログラム
JP5503961B2 (ja) 観測信号処理装置
US8908475B2 (en) Acoustic positioning system and method
US10732273B2 (en) Radar device for vehicle and method for estimating angle of target using same
JP6926775B2 (ja) 移動目標探知システム及び移動目標探知方法
US9075142B2 (en) Device and method for selecting signal, and radar apparatus
JP2009139321A (ja) レーダ信号処理装置および方法
JP2021524590A (ja) 波動に基づいた信号を用いて少なくとも１つの物体の位置特定を行うための位置特定方法及び位置特定システム
WO2018038128A1 (ja) 移動目標探知システム及び移動目標探知方法
CN109164440B (zh) 一种多频雷达测距的方法
CN110531358B (zh) 信息测量方法及信息测量装置
EP3517994B1 (en) Method and device for determining a distance from an object using radar
KR101174423B1 (ko) 초음파 거리 측정 방법 및 이를 이용한 이동 로봇의 자세를 구하는 방법
JP6843568B2 (ja) レーダ装置および到来方向推定方法
JP6289672B2 (ja) 合成開口レーダ信号処理装置及び合成開口レーダ信号処理プログラム
US10379212B2 (en) Multi frequency range estimation
Bouron et al. Set-membership non-linear observers with application to vehicle localisation
JP3022001B2 (ja) 飛しょう体の誘導制御装置
KR102059689B1 (ko) 차량 레이다용 각도 추정 방법 및 장치
Kim et al. Relative azimuth estimation algorithm using rotational displacement
WO2018146723A1 (ja) 測位装置及び測位方法
US20210223383A1 (en) Method for capture in a radar system

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: 20150609

Year of fee payment: 4

FPAY Annual fee payment

Payment date: 20160627

Year of fee payment: 5

FPAY Annual fee payment

Payment date: 20170703

Year of fee payment: 6

LAPS Lapse due to unpaid annual fee