KR101234797B1 - 로봇 및 산출된 공분산을 이용한 로봇의 위치 추정 방법 - Google Patents

Abstract

본 발명은 로봇 및 산출된 공분산을 이용한 로봇의 위치 추정 방법에 관한 것으로서, 칼만 필터(Kalman Filter)를 이용하여 로봇의 위치를 추정하는 경우 시스템 노이즈의 공분산 및 측정 노이즈의 공분산을 산출함에 있어서, 파라미터의 발산 여부를 고려하고 진화 연산 시 자손의 실험 순서를 수정하여 연산 속도를 향상시키는 로봇 및 산출된 공분산을 이용한 로봇의 위치 추정 방법에 관한 것이다.

본 발명의 실시예에 따른 로봇은 회전 각도에 대한 정보를 제공하는 자이로스코프 모듈과, 바퀴의 동작을 감지하여 이동 속도 및 상기 바퀴의 회전 각도에 대한 정보를 제공하는 엔코더 모듈 및 상기 엔코더 모듈과 상기 자이로스코프 모듈로부터 제공되는 정보들을 기초로, 진화 연산을 통하여 산출된 시스템 노이즈(system noise)의 공분산 및 측정 노이즈(measurement noise)의 공분산이 적용된 칼만 필터 방법으로 현재의 위치를 추정하는 제어 모듈을 포함하는데, 상기 제어 모듈은 상기 진화 연산 과정에서 상기 칼만 필터의 파라미터가 발산하지 않는 것을 만족시키는 상기 시스템 노이즈의 공분산 및 상기 측정 노이즈의 공분산을 상기 칼만 필터 방법에 적용한다.

칼만 필터, 발산, 시스템 노이즈, 측정 노이즈, 공분산

Description

로봇 및 산출된 공분산을 이용한 로봇의 위치 추정 방법{Robot and method for localization of the robot using calculated covariance}

도 1은 본 발명의 실시예에 따른 로봇의 구조를 나타낸 블록도이다.

도 2는 본 발명의 실시예에 따른 시스템 노이즈의 공분산 및 측정 노이즈의 공분산이 산출되는 과정을 나타낸 흐름도이다.

도 3은 본 발명의 실시예에 따른 칼만 필터링 과정을 나타낸 흐름도이다.

도 4는 본 발명의 실시예에 따른 칼만 필터의 상태 방정식이다.

도 5는 본 발명의 실시예에 따른 칼만 필터의 측정 방정식이다.

도 6은 종래의 칼만 필터의 측정 방정식이다.

도 7은 본 발명의 실시예에 따른 진화 연산 과정에서 자손의 실험 순서가 재배치되는 것을 나타낸 개념도이다.

도 8은 본 발명의 실시예에 따른 진화 연산 과정을 통하여 산출된 시스템 노이즈의 공분산 및 측정 노이즈의 공분산을 나타낸 도면이다.

도 9는 본 발명의 실시예에 따른 시스템 노이즈의 공분산 및 측정 노이즈의 공분산을 적용한 경우의 오차를 나타낸 실험 결과이다.

<도면의 주요 부분에 관한 부호의 설명>

110 : 자이로스코프 모듈 120 : 엔코더 모듈

130 : 구동 모듈 140 : 제어 모듈

본 발명은 로봇 및 산출된 공분산을 이용한 로봇의 위치 추정 방법에 관한 것으로서, 더욱 상세하게는 칼만 필터(Kalman Filter)를 이용하여 로봇의 위치를 추정하는 경우 시스템 노이즈의 공분산 및 측정 노이즈의 공분산을 산출함에 있어서, 파라미터의 발산 여부를 고려하고 진화 연산 시 자손의 실험 순서를 수정하여 연산 속도를 향상시키는 로봇 및 산출된 공분산을 이용한 로봇의 위치 추정 방법에 관한 것이다.

최근, 기술의 발달에 따라 다양한 형태의 로봇이 등장하게 되었고, 특히 가정 내에서 스스로 이동하면서 인간의 일을 대신 수행하는 로봇도 등장하게 되었다.

그런데, 이와 같이 스스로 이동하는 로봇에게 있어서 자신의 위치를 파악하는 것은 매우 중요한 문제이며, 이를 해결하기 위해 많은 방법들이 제안되었다.

종래에는 로봇의 위치를 추정하기 위하여 로봇의 구동부(예: 바퀴)에 엔코더(encoder, 또는 주행 거리계(odometry)라고도 함)를 부착하거나, 더 나아가 로봇의 회전 각도를 정확하게 측정하기 위하여 로봇에 자이로스코프(gyroscope)를 탑재하기도 하였다.

그러나, 로봇의 위치를 추정할 때에는 바퀴의 미끄러짐 혹은 기계적 드리프트(mechanical drift)로 인하여 오차가 발생하게 되는데, 이러한 오차가 미미하더 라도 계속해서 누적되면 결국 큰 오차가 생겨 문제가 발생할 수 있다.

한편, 종래에는 로봇의 위치를 추정하기 위하여 칼만 필터(Kalman Filter) 알고리즘을 적용하였는데, 칼만 필터의 시스템 노이즈(system error)의 공분산 및 측정 노이즈(measurement error)의 공분산을 어떻게 설정하느냐에 따라 그 성능이 달라진다.

시스템 노이즈의 공분산 및 측정 노이즈의 공분산을 산출하는 방법으로는 시행착오(trial-and-error) 방법, 온라인 업데이트 방법 및 진화 연산(evolutionary computation) 방법이 있는데, 시행착오 방법은 시스템 노이즈의 공분산 및 측정 노이즈의 공분산에 대한 다양한 값을 입력한 후 그 중 가장 높은 효율을 나타내는 값을 선택하는 방법으로서, 실험자에 의하여 입력된 값에 따라 그 효율이 달라질 수 있는 단점이 있다.

한편, 진화 연산 방법은 미리 실험 데이터를 통하여 산출된 참(True) 값과 칼만 필터 값의 오차를 측정한 후에 그 오차를 최소화하는 시스템 노이즈의 공분산 및 측정 노이즈의 공분산을 산출하는 방법이다. 그런데, 진화 연산 방법은 각 개체의 목적 함수(Objective Function)에 대한 연산을 매회 수행하여야 하므로, 많은 연산 시간이 소요될 수 있다.

예를 들어, 개체(individual)의 수가 30이고, 세대(generation)의 수가 80이고, 실험 종류의 수가 40이며, 하나의 실험에 대한 연산 시간이 1초라고 가정하면 이에 소요되는 연산 시간(T)는 다음과 같다.

한국 공개 특허 2004-060829호는 절대 방위각 측정부의 측정 잡음을 필터링하는 칼만 필터를 포함하는 것을 특징으로 하는 로봇용 로컬라이제이션 시스템을 개시하고 있다. 그러나, 개시된 발명은 측정 잡음의 공분산을 산출하기 위한 방법이 제시되어 있지 않으므로, 해당 시스템에만 적용될 수 있는 특수한 값이 사용되었다고 간주될 수 있다. 즉, 시스템이 변경될 때마다 측정 잡음의 공분산이 새롭게 산출되어야 하는 것인데 이에 따라, 시스템의 효율이 저하되거나 측정 잡음의 공분산을 산출하기 위한 많은 연산 시간이 소요될 수도 있다.

따라서, 시스템 노이즈의 공분산 및 측정 노이즈의 공분산을 산출하기 위한 연산 시간을 단축시키는 발명의 등장이 요구된다.

본 발명은 진화 연산에 의한 시스템 노이즈의 공분산 및 측정 노이즈의 공분산 산출 시 칼만 필터 파라미터를 발산시키지 않는 자손에 대해서만 연산을 수행함으로써 불필요한 연산 과정을 감소시키는데 그 목적이 있다.

또한, 본 발명은 참값과 칼만 필터에 의한 값의 오차를 산출하는 매 실험에 있어서, 이전 실험에서 일단 발산된 실험에 대한 오차 산출 연산을 이후 실험의 처음으로 전환함으로써 불필요한 연산 과정을 감소시키는데 그 목적이 있다.

본 발명의 목적들은 이상에서 언급한 목적들로 제한되지 않으며, 언급되지 않은 또 다른 목적들은 아래의 기재로부터 당업자에게 명확하게 이해되어질 수 있을 것이다.

상기 목적을 달성하기 위하여, 본 발명의 실시예에 따른 로봇은 회전 각도에 대한 정보를 제공하는 자이로스코프 모듈과, 바퀴의 동작을 감지하여 이동 속도 및 상기 바퀴의 회전 각도에 대한 정보를 제공하는 엔코더 모듈 및 상기 엔코더 모듈과 상기 자이로스코프 모듈로부터 제공되는 정보들을 기초로, 진화 연산을 통하여 산출된 시스템 노이즈(system noise)의 공분산 및 측정 노이즈(measurement noise)의 공분산이 적용된 칼만 필터 방법으로 현재의 위치를 추정하는 제어 모듈을 포함하는데, 상기 제어 모듈은 상기 진화 연산 과정에서 상기 칼만 필터의 파라미터가 발산하지 않는 것을 만족시키는 상기 시스템 노이즈의 공분산 및 상기 측정 노이즈의 공분산을 상기 칼만 필터 방법에 적용한다.

본 발명의 실시예에 따른 산출된 공분산을 이용한 로봇의 위치 추정 방법은 (a) 회전 각도에 대한 정보를 제공하는 단계와, (b) 바퀴의 동작을 감지하여 이동 속도 및 상기 바퀴의 회전 각도에 대한 정보를 제공하는 단계 및 (c) 상기 (a) 단계와 상기 (b) 단계로부터 제공되는 정보들을 기초로, 진화 연산을 통하여 산출된 시스템 노이즈(system noise)의 공분산 및 측정 노이즈(measurement noise)의 공분산이 적용된 칼만 필터 방법으로 현재의 위치를 추정하는 단계를 포함하는데, 상기 (c) 단계는 상기 진화 연산 과정에서 상기 칼만 필터의 파라미터가 발산하지 않는 것을 만족시키는 상기 시스템 노이즈의 공분산 및 상기 측정 노이즈의 공분산을 상 기 칼만 필터 방법에 적용하는 단계를 포함한다.

기타 실시예들의 구체적인 사항들은 상세한 설명 및 도면들에 포함되어 있다.

본 발명의 이점 및 특징, 그리고 그것들을 달성하는 방법은 첨부되는 도면과 함께 상세하게 후술되어 있는 실시예들을 참조하면 명확해질 것이다. 그러나 본 발명은 이하에서 개시되는 실시예들에 한정되는 것이 아니라 서로 다른 다양한 형태로 구현될 수 있으며, 단지 본 실시예들은 본 발명의 개시가 완전하도록 하고, 본 발명이 속하는 기술분야에서 통상의 지식을 가진 자에게 발명의 범주를 완전하게 알려주기 위해 제공되는 것이며, 본 발명은 청구항의 범주에 의해 정의될 뿐이다. 명세서 전체에 걸쳐 동일 참조 부호는 동일 구성 요소를 지칭한다.

이하, 첨부된 도면을 참조하여 본 발명의 바람직한 실시예를 상세히 설명하기로 한다.

도 1은 본 발명의 실시예에 따른 로봇의 구조를 나타낸 블록도로서, 로봇은 자이로스코프(gyroscope) 모듈(110), 엔코더(encoder) 모듈(120), 구동 모듈(130) 및 제어 모듈(140)을 포함하여 구성된다.

이 때, 본 실시예에서 사용되는 모듈이라는 용어는 소프트웨어 또는 FPGA 또는 ASIC과 같은 하드웨어 구성요소를 의미하며, 모듈은 어떤 역할들을 수행한다. 그렇지만, 모듈은 소프트웨어 또는 하드웨어에 한정되는 의미는 아니다. 모듈은 어드레싱할 수 있는 저장 매체에 있도록 구성될 수도 있고, 하나 또는 그 이상의 프로세서들을 재생시키도록 구성될 수도 있다. 따라서, 일 예로서 모듈은 소프트웨어 구성요소들, 객체지향 소프트웨어 구성요소들, 클래스 구성요소들 및 태스크 구성요소들과 같은 구성요소들과, 프로세스들, 함수들, 속성들, 프로시저들, 서브루틴들, 프로그램 코드의 세그먼트들, 드라이버들, 펌웨어, 마이크로 코드, 회로, 데이터, 데이터베이스, 데이터 구조들, 테이블들, 어레이들 및 변수들을 포함한다. 구성요소들과 모듈들 안에서 제공되는 기능은 더 작은 수의 구성요소들 및 모듈들로 결합되거나 추가적인 구성요소들과 모듈들로 더 분리될 수 있다. 뿐만 아니라, 구성요소들 및 모듈들은 디바이스 또는 보안 멀티미디어카드 내의 하나 또는 그 이상의 CPU들을 재생시키도록 구현될 수도 있다.

또한, 본 발명에서의 로봇이란, 자이로스코프와 엔코더를 이용하여 자신의 위치를 추정하는 물체를 의미한다.

자이로스코프 모듈(110)은 이동 로봇에 부착되어 자이로스코프가 회전하는 각도에 대한 정보를 제공한다.

그리고, 엔코더 모듈(120)은 바퀴 등을 포함하는 구동 모듈(130)의 동작을 감지하여 이동 로봇의 이동 속도 및 바퀴의 회전 각도에 대한 정보를 제공한다.

제어 모듈(140)은 엔코더 모듈(120)과 자이로스코프 모듈(110)로부터 제공되는 정보들을 기초로, 진화 연산을 통하여 산출된 시스템 노이즈(system noise)의 공분산 및 측정 노이즈(measurement noise)의 공분산이 적용된 칼만 필터(Kalman filter) 방법으로 현재의 위치를 추정한다. 여기서, 제어 모듈(140)은 진화 연산 과정에서 칼만 필터의 파라미터가 발산하지 않는 것을 만족시키는 시스템 노이즈의 공분산 및 측정 노이즈의 공분산을 칼만 필터 방법에 적용하여 현재의 위치를 추정 한다.

제어 모듈(140)이 이동 로봇의 위치를 추정하게 되면 그에 대응하여 구동 모듈(130)을 동작시킴으로써 이동 로봇은 이동하게 된다.

한편, 칼만 필터의 파라미터에는 바퀴의 스케일 팩터(scale factor) 및 양 바퀴간의 거리를 포함하는데, 발산은 파라미터가 소정 임계 범위를 초과하는 상태를 포함한다. 즉, 진화 연산 과정에서 산출된 파라미터의 값이 해당 파라미터에 대하여 기 설정된 임계 범위를 초과하면 이는 발산으로 간주되는 것이다.

진화 연산 과정은 각 단계에서 자손에 할당된 값(특정 시스템 노이즈의 공분산 및 특정 측정 노이즈의 공분산)을 수정하면서 실험에 의하여 산출된 참(True)값과 칼만 필터에 의하여 산출된 값의 차이인 오차를 산출하는 것인데, 제어 모듈(140)은 진화 연산 과정에서 산출된 오차의 평균 중 가장 작은 값이 산출되도록 한 시스템 노이즈의 공분산 및 측정 노이즈의 공분산을 칼만 필터 방법에 적용한다.

예를 들어, 10개의 실험이 있는 경우 진화 연산 과정 중 각 개체당 실험에서의 참값과 칼만 필터에 의한 값의 오차는 10개가 산출되고 이에 대한 평균이 산출될 수 있는데, 이렇게 산출된 각 세대에서의 오차의 평균 중 작은 값을 가진 개체가 선택될 확률을 높게 하여 진화시켜 나간다. 여기서, 오차는 참값과 칼만 필터에 의하여 산출된 값간의 제곱 평균(Root-Mean-Square)으로 표현될 수 있으며, 로봇의 거리 오차 및 각도 오차를 포함한다.

이 때, 진화 연산 과정에서 칼만 필터의 파라미터가 발산하는 경우 각 자손 에 대한 오차는 기 설정된 임의의 값으로 대체될 수 있다. 여기서, 임의의 값은 임의의 큰 값을 의미하는데 이에 따라, 파라미터를 발산시키는 세대에서의 오차의 평균은 파라미터를 발산시키지 않는 세대에서의 오차의 평균보다 상대적으로 큰 값을 갖게 되고, 파라미터를 발산시키는 세대에 포함된 시스템 노이즈의 공분산 및 측정 노이즈의 공분산이 칼만 필터 방법에 적용되는 가능성이 줄어들게 된다.

또한, 칼만 필터 방법에 적용되는 시스템 노이즈의 공분산 및 측정 노이즈의 공분산은 복수 개의 자손에 대한 복수 개의 실험을 통한 진화 연산 과정에서 칼만 필터의 파라미터를 발산시키는 특정 실험의 연산 순서를 이후의 진화 연산 과정 즉, 다음 세대에서의 연산 과정에서 최우선으로 수행함으로써 산출된 것일 수 있다.

본 발명의 실시예에 따른 진화 연산 과정은 각 세대별 연산을 수행하면서 특정 실험에서 칼만 필터의 파라미터가 발산하는 경우 이후의 연산을 생략하는데, 해당 개체에서 발산 가능성이 높은 특정 자손에 대한 실험의 연산을 우선적으로 수행함으로써, 불필요한 연산이 수행되는 것을 방지할 수 있다. 여기서, 실험은 특정 경로를 통하여 로봇을 주행시키는 것을 의미한다. 예를 들어, 특정 공간을 회전하는 실험 및 특정 공간을 왕복하는 실험 등이 포함될 수 있는 것으로서, 동일한 경로를 주행하더라도 서로 다른 실험으로 간주될 수 있다. 실험의 순서를 재배치시키면서 수행하는 진화 연산에 대한 자세한 설명은 도 7을 통하여 후술하기로 한다.

도 2는 본 발명의 실시예에 따른 시스템 노이즈의 공분산 및 측정 노이즈의 공분산이 산출되는 과정을 나타낸 흐름도이다.

로봇의 노이즈를 산출하기 위하여 우선 변수의 범위가 설정된다(S210). 즉, 칼만 필터의 공분산은 그 최대값이 입력되고, 파라미터는 그 범위가 입력되는 것인데, 여기서 입력된 파라미터의 범위에 따라 발산 여부를 판단하는 단계에서 해당 파라미터의 발산 여부가 결정된다.

변수의 범위 설정이 완료되면 진화 연산을 위한 시스템 노이즈의 공분산 및 측정 노이즈의 공분산에 대한 염색체(chromosome)가 구성된다(S220). 이를 수학식으로 표현하면 수학식 2와 같다.

여기서, x는 염색체의 개체(individual)를 나타내고, q는 시스템 노이즈의 공분산에 대한 염색체를 나타내고 r은 측정 노이즈의 공분산에 대한 염색체를 나타내며, S는 실험을 나타낸다. 또한, i는 개체의 수를 나타내고, t는 세대(generation)의 수를 나타낸다.

그리고, 초기 세대(first generation)가 생성된다(S230). 즉, 시스템 노이즈의 공분산 및 측정 노이즈의 공분산이 S210 단계에서 설정된 범위 내에서 노멀 랜덤(normal random)하게 그 값이 설정되고, 실험의 수행 순서가 유니폼 랜덤(uniform random)하게 설정되는 것으로서, 초기 세대의 자손에 대한 초기값이 설정되는 것이다.

초기 세대 생성 후, 초기 세대에 대한 자손이 생성된다(S240). 즉, 시스템 노이즈의 공분산 및 측정 노이즈의 공분산에 대한 값이 결정되는 것이다. 자손 생성은 수학식 3에 의하여 결정될 수 있다.

여기서, σ는 자손의 값이 결정될 수 있는 확률 분포를 나타낸다. 또한, 자손 생성 과정에서 실험의 순서가 재배치 될 수 있다. 즉, 특정 순서의 실험을 수행하는 도중에 칼만 필터의 파라미터가 발산하는 경우 해당 실험의 순서가 최상위로 배치되는 것이다.

자손 생성 후, 각 자손에 대한 칼만 필터링이 수행되는데(S250), 칼만 필터링 과정에서 칼만 필터의 파라미터가 발산하는지 여부가 확인된다. 그리하여, 파라미터가 발산하지 않으면 참값과 칼만 필터에 의하여 산출된 값간의 차이가 해당 자손에서의 오차로 입력되고, 파라미터가 발산하면 기 설정된 임의의 큰 값이 해당 자손에서의 오차로 입력된다. 이러한 과정은 모든 실험에 대한 칼만 필터링이 완료될 때까지 수행되는데, 칼만 필터링에 대한 자세한 설명은 도 3을 통하여 후술하기로 한다.

칼만 필터링이 완료되면, 오차가 작은 개체가 선택된다(S260). 즉, 자손 생성 과정에서 생성된 자손에 대한 개체와 그 부모 세대의 개체 중 작은 오차를 산출하도록 한 개체들이 선택되는 것이다. 예를 들어, 개체의 수가 30인 경우, 부모 세 대에 대한 개체의 수 및 그 자손에 대한 개체의 수를 합하면 60이 되는데, 60개의 개체 중 최적의 30개의 개체가 선택되는 것이다. 선택 방법으로는 랜덤 룰렛휠(Random Roulette Wheel) 등과 같은 확률적인 방법이 사용될 수 있다.

최적의 개체가 선택되면 진화 연산 과정이 최종 세대까지 수행되었는지 확인된다(S270). 그리하여, 최종 세대까지 수행되지 않았으면 새로운 세대에 대한 진화 연산 과정이 수행되고, 최종 세대까지 수행되었으면 최적의 시스템 노이즈의 공분산 및 측정 노이즈의 공분산이 산출된다(S280).

이렇게 산출된 시스템 노이즈의 공분산 및 측정 노이즈의 공분산은 이동 로봇에 적용되어 로봇의 위치 추정에 사용된다.

도 3은 본 발명의 실시예에 따른 칼만 필터링 과정을 나타낸 흐름도이다. 칼만 필터링 과정은 종래의 칼만 필터에 의하여 수행될 수도 있으나 본 발명에서는 제한 조건(constraints)이 있는 칼만 필터에 의하여 수행되는 것을 가정한다.

칼만 필터링 과정에서 우선 상태 변수(state variable)를 예측(prediction)하는데(S310), 본 발명에서는 종래의 기술에 따른 상태 방정식에 새로운 상태 변수를 추가하여 관측가능 행렬(observability matrix)의 계수(rank)의 크기를 향상시키도록 한다.

종래의 상태 방정식에 의한 간접 칼만 필터는 관측 가능성(observability)이 좋지 않다. 즉, 종래의 상태 방정식에서 상태 벡터(state vector)의 디멘션(dimension)은 9이지만, 관측가능 행렬의 계수는 상태 전이 행렬(state transition matrix)이 시간 불변 시스템(time-invariant system)의 정상 상 태(steady state)에 있다고 가정할 때 단지 2에 불과하다. 즉, 상태에 대한 수렴 여부를 보장할 수 없게 되는 것인데, 이는 단지 회전각 상태만이 관측 행렬에 포함되어 있으므로 로봇의 위치의 오차와 같은 상태가 참값으로 수렴하는지 여부는 보장될 수 없는 것이다. 시스템이 시간 변이(time-varying)인 경우라 하더라도 관측 가능 행렬의 계수는 6정도라서 여전히 불안정하다고 볼 수 있다.

따라서, 본 발명에서는 이를 해결하기 위하여 관측 가능한 시스템 상태(system state)를 추가하여 예측하도록 할 수 있다.

일반적으로, 이산 구간(discrete-time interval)에 있어서 로봇과 같은 동적 시스템 모델(dynamic system model)의 관측 가능 행렬은 수학식 4와 같이 나타낼 수 있다.

여기서, H _k 는 k시간에서의 관측 행렬을 나타내고, F _k 는 1≤k≤k _f 에 대하여 k부터 (k+1)까지의 상태 전이 행렬을 나타낸다.

이 때, 로봇의 상태 전이 행렬이 시간 불변인 경우에는 관측 가능성은 수학식 5와 같은 행렬의 계수에 의해 결정될 수 있다.

만일, 수학식 5에서 계수가 n인 경우, 해당 시스템은 관측 가능하다고 할 수 있다. 이 때, n은 시스템 상태 벡터의 디멘션을 나타낸다.

전술한 바와 같이, 종래의 방법을 따르게 되면, 계수가 2이므로 로봇 시스템을 완전히 관측할 수 없다. 본 발명에서는 자이로스코프와 2개의 바퀴 대한 엔코더를 이용하므로 자이로스코프의 회전 각도가 반영된 로봇의 위치는 관측 가능하다.

이 때, 로봇의 위치와 로봇의 헤딩 각도(heading angle)(ø(k))는 수학식 6과 같이 표현될 수 있다.

여기서,

와

는 계산된 자이로스코프의 회전 각도를 이용하여 추정된 로봇의 위치를 나타내고,

와

는 자이로스코프에 의해 추정된 로봇의 위치 오차를 나타낸다. 그리고, ø는 자이로스코프의 회전 각도를 나타낸다.

따라서,

와

를 새로운 상태 변수로 하여 종래의 상태 방정식 에 추가하면 도 4와 같은 상태 방정식이 된다. 여기서, 410으로 표시되는 상태 변수가 새로 추가된 상태 변수이고, 420으로 표시되는 부분이 새로 추가된 시스템 행렬 부분이다.

도 4를 기초로 하는 상태 예측을 수학식으로 표현하면, 수학식 7과 같다.

여기서, x는 도 4에서의 상태 변수를 나타내고, u는 로봇의 속도 성분을 나타낸다. 또한, A는 도 4에서의 행렬 A를 나타내고, P는 공분산 행렬, Q는 시스템 노이즈의 공분산을 나타낸다. 또한,

를 나타낸다.

도 4와 같이 상태 변수가 예측된 상태 방정식을 기반으로 하여 상태가 관측된다(S320).

이를 위하여, 측정 방정식(measurement equation)이 사용될 수 있는데, 본 발명에 따른 측정 방정식을 개략적으로 표현하면 수학식 8과 같으며, 이를 구체화시킨 측정 방정식이 도 5에 도시되어 있다.

측정 방정식 = 엔코더로 추정한 로봇의 위치 - 자이로스코프로 추정한 로봇의 위치 + 노이즈

도 5에서, v는 측정 노이즈를 나타낸다.

한편, 종래의 기술에 따른 측정 방정식은 각각 수학식 9 및 도 6과 같이 표현될 수 있는데, 도 6에서는 단지 회전각 상태만이 관측 행렬 H에 포함되어 있음을 알 수 있다.

측정 방정식 = 엔코더로 추정한 로봇의 방향 - 자이로스코프로 추정한 로봇의 방향 + 노이즈

도 6에서, v는 측정 노이즈를 나타낸다.

이에 반하여, 도 5에서 도시된 본 발명에 따른 측정 방정식에서의 관측 행렬 H는 회전각 상태뿐만 아니라 로봇의 위치에 대한 상태도 포함되어 있으므로, 수학식 4에서 개시하고 있는 관측 가능 행렬의 계수가 향상되는 것이다.

즉, 본 발명에 따른 방법을 사용하면, 시간 불변 시스템의 경우 rank(O) = 4가 되어, x, y 거리의 오차가 참값으로 수렴하기에 충분한 조건을 만족하며 또한, 시간 변이 시스템의 경우 rank(O) = 10이 되어 시스템이 안정된다. 이 때, 도 4에서

상태 변수는 자이로스코프의 스케일 팩터 오차로서 사전 보정에 관련된 것이므로 계수 연산 시 제외한다.

S320 단계에서 로봇의 상태를 관측하여 관측 행렬 H를 포함하는 Z _k 를 얻으면(여기서, H와 Z _k 는 도 6에 도시된 H와 Z를 의미한다), H를 기초로 칼만 이득(Kalman gain)을 얻고 칼만 이득 및 Z _k 를 이용하여 수학식 7에서의 상태 변수 x와 공분산 P를 갱신한다(S330). 이를 수학식으로 표현하면 수학식 10과 같다.

여기서, K _k 는 칼만 이득을, R _k 는 측정 노이즈의 공분산을 나타낸다. 그리고,

를 나타낸다.

한편, 이동 로봇에 제한 조건(constraints)이 존재하는지 확인하여(S340) 제한 조건이 존재하지 않는 경우에는 S330 단계에서 갱신된 상태 변수

를 이용하여 이동 로봇의 위치를 추정하게 되고, S310 단계를 반복하게 된다.

그러나, 이동 로봇에 제한 조건이 존재하는 경우에는 제한 조건에 적합하도록 S330 단계에서 갱신된 상태 변수

를 보정함으로써 보정된 상태 변수를 기초로 이동 로봇의 위치를 추정하게 된다(S350).

이 때, 자이로스코프를 탑재한 이동 로봇의 제한 조건으로서, 양 바퀴 사이의 거리, 바퀴의 지름, 온도 변화에 따른 자이로스코프의 스케일 팩터와 바이어스(bias) 등이 있으며, 제한 조건은 선형(linear) 제한 조건, 비선형(non-linear) 제한 조건, 부등(inequality) 제한 조건, 등(equality) 제한 조건 등을 포함할 수 있다.

S350 단계는 제한 조건을 사용하여 상태 변수

를 갱신하는 단계와 패널 티 파라미터(penalty parameter) 및 라그랑제 멀티플라이어(Lagrange multiplier)를 갱신하는 단계를 포함한다.

한편, 본 발명은 제한 조건이 있는 칼만 필터 문제라고 가정하였으므로, 이를 해결하기 위하여 제한 조건이 없는 상태 변수를 제한 조건 평면으로 투사함으로써 그 해(solution)을 구할 수 있다.

즉, 제한 조건을 g, h라고 하면, g_i(x)≤0, h_j(x)=0과 같은 조건에서 수학식 11을 만족하는 해를 구하는 것이다.

여기서, g_i(x)와 h_j(x)는 각각 부등(inequality) 제한 조건과 등(equality) 제한 조건을 나타내고,

는 칼만 필터된 상태(Kalman filtered state), W는 단위 행렬(identity matrix) 또는 시스템 노이즈의 공분산 행렬(system noise covariance matrix)과 같은 임의의 양의 정부호 행렬(positive definite matrix)을 나타낸다.

수학식 11과 같은 제한 조건 최적화 문제(constrained optimization problem)에서 국부적으로 최소화시키는 상태의 존재를 위한 필요 조건은 라그랑제 멀티플라이어를 필요로 하는데, 종래의 쿤-터커 최적화 조건(Kuhn-Tucker optimality conditions)이 이러한 조건을 제공할 수 있다.

한편, 위 수학식 11은 컨벡스 문제(convex problem)에 해당하므로, 수학식 11에 종래의 패널티 함수 정리(penalty function theorem)를 적용할 수 있다.

패널티 함수 정리는 컨벡스 문제에서 패널티 파라미터 s_k가 점점 증가하면, 라그랑지언 함수(Lagrangian function) 함수의 해는 최적 해(optimal solution)로 수려한다는 정리로서, 수학식 12과 같은 패널티 함수(penalty function)에 의해 표현될 수 있다.

여기서,

를 만족한다. 또한 여기서,

는 S330 단계에서 갱신된 상태 변수를 나타내고,

는 제한 조건을 사용하여 갱신된 상태 변수를 나타낸다. 또한, g는 부등(inequality) 제한 조건을, h는 등(equality) 제한 조건을 나타내고, s는 패널티 파라미터, λ와 μ는 라그랑제 멀티플라이어를 나타낸다.

여기서, 패널티 파라미터 s는 앞서 설명한 패널티 함수 정리(penalty function theorem)를 기반으로 하여 선택될 수 있다.

한편, 패널티 파라미터 s와 라그랑제 멀티플라이어 λ와 μ에 대한 갱신은 다음과 같이 수행될 수 있다.

초기에는 ε=0, α=양의 상수(positive constant)로 하다가, 일정 시간 후에 ε 도 작은 양의 상수로 변경함으로써, s, λ, μ를 갱신할 수 있다. 물론, α 값은 수렴 속도를 향상시키기 위하여 일정 시간 후에 필요에 따라 다른 야의 상수로 변경할 수도 있다.

결국, 수학식 12과 같이 제한 조건이 있는 칼만 필터의 갱신은 쿤-터커 최적화 조건과 패널티 함수 정리를 만족하므로 최적화된 해라고 볼 수 있다.

도 7은 본 발명의 실시예에 따른 진화 연산 과정에서 실험의 순서가 재배치되는 것을 나타낸 개념도이다.

진화 연산의 실험 수행 순서는 칼만 필터의 파라미터가 발산하는지 여부에 따라 재배치될 수 있음은 전술한 바와 같다.

예를 들어, 도 7에서와 같이 100개의 실험이 제 1세대(710)에서 소정 순서로 배치되어 있는 경우 배치된 순서대로 실험이 수행되어 칼만 필터의 파라미터에 대한 발산 여부가 확인된다. 여기서, 발산 여부는 칼만 필터의 파라미터가 기 설정된 임계 범위에 포함되는지에 따라 결정된다. 즉, 파라미터가 임계 범위에 포함되지 않거나 수배 또는 수백배를 초과하는 경우 발산하는 것으로 간주되는 것이다. 여기서, 배수는 사전에 사용자에 의하여 결정될 수 있다.

그리하여, 제 1세대(710)에서 89번 실험(711)에 의하여 파라미터가 발산하는 것으로 확인되면, 89번 실험(711)은 다음 세대인 제 2세대(720)에서 최상위로 배치된다. 그리고, 제 2세대(720)의 최상위에 배치된 89번 실험(721)은 제 2 세대(720)에서의 진화 연산 과정에서 최우선으로 발산 여부가 확인된다.

본 발명에서 진화 연산 과정은 특정 실험 중 발산하는 것이 있으면 작업을 종료하고 다음 세대로 넘어가는데, 89번 실험(711)과 같이 발산의 가능성이 높은 것을 다음 세대에서 최우선으로 발산 여부를 확인함으로써 이후의 작업을 생략할 수 있게 되고 이에 따라, 연산 시간이 절감될 수 있다.

한편, 제 2세대(720)에서 89번 실험(721)에 의한 연산 결과 파라미터가 발산하지 않는 경우 계속해서 다음 실험에 대한 발산 여부가 확인되는데, 그러다가 22번 실험(722)에 의하여 파라미터가 발산하는 것으로 확인되면, 22번 실험(722)은 다음 세대인 제 3세대(730)에서 최상위로 배치된다. 그리고, 제 3세대(730)의 최상위에 배치된 22번 실험(731)은 제 3세대(730)에서의 진화 연산 과정에서 최우선으로 발산 여부가 확인되고, 그 다음 89번 실험(732)에 의한 파라미터의 발산 여부가 확인된다.

이상 첨부된 도면을 참조하여 본 발명의 실시예를 설명하였지만, 본 발명이 속하는 기술분야에서 통상의 지식을 가진 자는 본 발명이 그 기술적 사상이나 필수적인 특징을 변경하지 않고서 다른 구체적인 형태로 실시될 수 있다는 것을 이해할 수 있을 것이다. 그러므로 이상에서 기술한 실시예들은 모든 면에서 예시적인 것이며 한정적이 아닌 것으로 이해해야만 한다.

상기한 바와 같은 본 발명의 로봇 및 산출된 공분산을 이용한 로봇의 위치 추정 방법에 따르면 다음과 같은 효과가 하나 혹은 그 이상 있다.

첫째, 진화 연산에 의한 시스템 노이즈의 공분산 및 측정 노이즈의 공분산 산출 시 칼만 필터 파라미터를 발산시키지 않는 자손에 대해서만 연산을 수행하고 또한, 참값과 칼만 필터에 의한 값의 오차를 산출하는 매 실험에 있어서, 이전 실험에서 일단 발산된 자손에 대한 오차 산출 연산을 이후 실험의 처음으로 전환함으로써 연산 시간을 단축할 수 있는 장점이 있다.

둘째, 일반적인 비선형 제한 조건에 대해서 제한 조건을 만족하도록 칼만 필터를 적용함으로써 시스템의 안정성을 향상시키는 장점도 있다.

Claims (20)

1. 회전 각도에 대한 정보를 제공하는 자이로스코프 모듈;

   바퀴의 동작을 감지하여 이동 속도 및 상기 바퀴의 회전 각도에 대한 정보를 제공하는 엔코더 모듈; 및

   상기 엔코더 모듈과 상기 자이로스코프 모듈로부터 제공되는 정보들을 기초로, 진화 연산 과정에서 시스템 노이즈의 공분산 및 측정 노이즈의 공분산을 산출하고, 산출된 시스템 노이즈(system noise)의 공분산 및 측정 노이즈(measurement noise)의 공분산이 적용된 칼만 필터 방법으로 현재의 위치를 추정하는 제어 모듈을 포함하는데,

   상기 제어 모듈은 상기 진화 연산 과정에서 상기 칼만 필터의 파라미터가 발산하지 않는 것을 만족시키는 상기 시스템 노이즈의 공분산 및 상기 측정 노이즈의 공분산을 상기 칼만 필터 방법에 적용하는 로봇.
2. 제 1항에 있어서,

   상기 발산은 상기 칼만 필터의 파라미터가 소정 임계 범위를 초과하는 상태를 포함하는 로봇.
3. 제 1항에 있어서,

   상기 제어 모듈은 상기 진화 연산 과정에서 산출된 오차의 평균 중 가장 작은 값이 산출되도록 한 상기 시스템 노이즈의 공분산 및 상기 측정 노이즈의 공분 산을 상기 칼만 필터 방법에 적용하는 로봇.
4. 제 3항에 있어서,

   상기 진화 연산 과정에서 상기 칼만 필터의 파라미터가 발산하는 경우 상기 오차는 기 설정된 임의의 값으로 대체되는 로봇.
5. 제 3항에 있어서,

   상기 오차는 실험에 의하여 산출된 참값과 상기 칼만 필터에 의하여 산출된 값간의 제곱 평균(Root-Mean-Square)을 포함하는 로봇.
6. 제 3항에 있어서,

   상기 오차는 거리 오차 및 각도 오차를 포함하는 로봇.
7. 제 1항에 있어서,

   상기 시스템 노이즈의 공분산 및 상기 측정 노이즈의 공분산은 복수 개의 실험에 대한 상기 진화 연산 과정에서 상기 칼만 필터의 파라미터를 발산시키는 특정 실험의 연산 순서를 이후의 상기 진화 연산 과정에서 최우선으로 수행함으로써 산출된 것을 포함하는 로봇.
8. 제 1항에 있어서,

   상기 칼만 필터 방법은 상기 파라미터의 범위가 정의된 제한 조건이 있는 칼만 필터 방법을 포함하는 로봇.
9. 제 8항에 있어서,

   상기 칼만 필터 방법은 쿤-터커 최적화 조건(Kuhn-Tucker optimality conditions)과 패널티 함수 정리(penalty function theorem)을 만족하는 로봇.
10. 제 8항에 있어서,

    상기 제한 조건은 패널티 함수(penalty function)로 표현되는 로봇.
11. (a) 회전 각도에 대한 정보를 제공하는 단계;

    (b) 바퀴의 동작을 감지하여 이동 속도 및 상기 바퀴의 회전 각도에 대한 정보를 제공하는 단계; 및

    (c) 상기 (a) 단계와 상기 (b) 단계로부터 제공되는 정보들을 기초로, 진화 연산 과정에서 시스템 노이즈의 공분산 및 측정 노이즈의 공분산을 산출하고, 산출된 시스템 노이즈(system noise)의 공분산 및 측정 노이즈(measurement noise)의 공분산이 적용된 칼만 필터 방법으로 현재의 위치를 추정하는 단계를 포함하는데,

    상기 (c) 단계는 상기 진화 연산 과정에서 상기 칼만 필터의 파라미터가 발산하지 않는 것을 만족시키는 상기 시스템 노이즈의 공분산 및 상기 측정 노이즈의 공분산을 상기 칼만 필터 방법에 적용하는 단계를 포함하는 산출된 공분산을 이용한 로봇의 위치 추정 방법.
12. 제 11항에 있어서,

    상기 발산은 상기 칼만 필터의 파라미터가 소정 임계 범위를 초과하는 상태를 포함하는 산출된 공분산을 이용한 로봇의 위치 추정 방법.
13. 제 11항에 있어서,

    상기 (c) 단계는 상기 진화 연산 과정에서 산출된 오차의 평균 중 가장 작은 값이 산출되도록 한 상기 시스템 노이즈의 공분산 및 상기 측정 노이즈의 공분산을 상기 칼만 필터 방법에 적용하는 단계를 포함하는 산출된 공분산을 이용한 로봇의 위치 추정 방법.
14. 제 13항에 있어서,

    상기 진화 연산 과정에서 상기 칼만 필터의 파라미터가 발산하는 경우 상기 오차는 기 설정된 임의의 값으로 대체되는 단계를 포함하는 산출된 공분산을 이용한 로봇의 위치 추정 방법.
15. 제 13항에 있어서,

    상기 오차는 실험에 의하여 산출된 참값과 상기 칼만 필터에 의하여 산출된 값간의 제곱 평균(Root-Mean-Square)을 포함하는 산출된 공분산을 이용한 로봇의 위치 추정 방법.
16. 제 13항에 있어서,

    상기 오차는 거리 오차 및 각도 오차를 포함하는 산출된 공분산을 이용한 로봇의 위치 추정 방법.
17. 제 11항에 있어서,

    상기 시스템 노이즈의 공분산 및 상기 측정 노이즈의 공분산은 복수 개의 실험에 대한 상기 진화 연산 과정에서 상기 칼만 필터의 파라미터를 발산시키는 특정 실험의 연산 순서를 이후의 상기 진화 연산 과정에서 최우선으로 수행함으로써 산출된 것을 포함하는 산출된 공분산을 이용한 로봇의 위치 추정 방법.
18. 제 11항에 있어서,

    상기 칼만 필터 방법은 상기 파라미터의 범위가 정의된 제한 조건이 있는 칼만 필터 방법을 포함하는 산출된 공분산을 이용한 로봇의 위치 추정 방법.
19. 제 18항에 있어서,

    상기 칼만 필터 방법은 쿤-터커 최적화 조건(Kuhn-Tucker optimality conditions)과 패널티 함수 정리(penalty function theorem)을 만족하는 산출된 공분산을 이용한 로봇의 위치 추정 방법.
20. 제 18항에 있어서,

    상기 제한 조건은 패널티 함수(penalty function)로 표현되는 산출된 공분산을 이용한 로봇의 위치 추정 방법.

Images