Localization using Fuzzy-Extended Kalman Filter

퍼지-확장칼만필터를 이용한 위치추정

Abstract

This paper proposes robot localization using Fuzzy-Extended Kalman Filter algorithm of the mobile robots equipped with least sensors. In order to improve the accuracy of the localization, we usually add the sensors or equipment. However, it increases the simulation time and expenses. This paper solves this problem using only the odometer and ultrasonic sensors to get the localization with the Fuzzy-Extended Kalman Filter algorithm method. By inputting the robot's angular velocity, sensor data variation, and residual errors into the fuzzy algorithm, we get the sensor weight factor to decide the sensor's importance. The performance of the designed method shows by the simulation and Pioneer 3-DX mobile robot test in the indoor environment.

1. 서 론

국방, 서비스, 의료용 로봇 등 다양한 분야에서 지능형 로봇이 연구되고 있으며, 사회의 고도화에 따라 고정된 위치에서의 단순작업이 아닌 이동성을 가진 지능형 로봇이 필요해지고 있다.

지능형 로봇의 중요한 문제 중 하나는 위치측정이다. 로봇은 자신의 현재위치를 통해 주어진 환경조건에서 이동을 계획할 수 있다. 위치측정 방법은 크게 상대위치 측정과 절대위치 측정으로 나눌 수 있으며, 각각의 장단점이 존재한다.

상대위치 측정은 추측항법이라고도 불린다. 주로 주행기록계와 관성 센서를 이용하며[1], 속도 또는 가속도를 이용하여 초기위치에서부터 로봇의 변위를 계산하는 방법으로 항공기 또는 선박에서 주로 사용하던 방법이다[2]. 이는 로봇의 가속도를 적분함으로써 구현할 수 있기 때문에 빠른 응답성을 가진다. 그러나 시간에 따라 오차가 누적되는 문제를 가지고 있다[3][4].

반면에 절대위치 측정은 다수의 센서로부터 얻은 정보를 기하학적 해석을 통해 로봇의 위치를 측정한다[5]. 상대위치 측정과 달리 과거의 위치정보에 의존하지 않고 현재의 데이터만을 사용하므로 오차가 누적되지는 않지만, 상대적으로 연산량이 많아 느린 응답특성을 가진다.

위의 두 방법에서 언급된 장점들을 극대화하기 위해 다양한 센서를 사용하는 시도는 줄곧 있어왔으며, 모노 카메라와 주행기록계를 융합한 위치추정[6], 스테레오 카메라로부터 받은 3차원 영상정보를 이용한 위치추정[7], GPS와 주행기록계를 이용한 위치추정[8] 등이 연구되고 있다. 또한 퍼지 가중치를 사용하여 위치측정의 오차를 줄이는 방법[9]도 제시되고 있다.

그림 1퍼지-확장칼만필터 시스템 Fig. 1 Fuzzy-EKF system

본 연구에서는 저비용으로 위치추정을 하기 위해 초음파 위성과 주행기록계를 융합하는 방식을 택하였다. 주행기록계는 시스템 모델로서, 초음파 위성은 측정 모델로서 확장칼만필터를 구성한다. 다만 센서가 신호를 받는 시간을 줄이기 위해 기존의 초음파 위성처럼 세 개의 초음파 정보를 순차적으로 받는 형태가 아니라, 단일 정보만을 받아 위치를 추정하게 하였다. 그리고 시스템의 성능을 향상시키기 위해 퍼지 시스템을 도입하여 각각의 센서로부터 받은 정보와 로봇의 상태를 바탕으로 칼만이득을 보정한다. 이로부터 기존의 위치측정 시스템에 비해 저비용에 속하면서도 주기가 짧은 위치정보를 얻을 수 있다.

자세하게는, 사용되는 센서들의 위치측정 방법과 장단점을 2장에서 설명하였고, 3장에서 위치측정 시스템을 기반으로 퍼지-확장칼만필터를 설계하여 위치추정의 정확도를 향상시켰다. 그리고 4장에서 모의실험과 주행실험을 통해 설계된 시스템의 성능을 검증하였다.

 

2. 위치측정 방법

2.1 주행기록계 시스템

주행기록계를 이용한 위치인식 방법은 초기위치를 기반으로 현재위치를 계산하는 추측항법의 일종이다. 각각의 바퀴에는 엔코더가 있으며, 바퀴의 회전을 측정하여 로봇의 이동량 ΔS와 회전량 Δθ를 누적시켜 로봇의 위치를 측정한다[10].

그림 2주행 기록계를 이용한 이동로봇의 위치 측정 Fig. 2 Localization of mobile robot using odometer

그림 3주행 기록계 모델 Fig. 3 Odometer model

주행기록계는 좌우 양 바퀴의 엔코더로 구성되며 로봇의 이동에 따라 바퀴의 회전량을 이용하여 로봇의 이동거리와 방향을 추정하게 된다. 여기서 로봇의 속도와 각속도는 엔코더에서 측정한 양 바퀴의 회전량 ∅r과 ∅l을 이용하여 구하게 되며 식 (1), (2)와 같이 나타낼 수 있다.

uk : 로봇의 이동속도wk : 로봇의 각속도Δ∅r : 오른쪽 바퀴 엔코더 회전 값Δ∅l : 왼쪽 바퀴 엔코더 회전 값d : 좌우 바퀴의 폭Ts : 샘플링 주기

로봇의 이동거리는 양 바퀴 이동거리의 평균에 비례하고, 회전량은 양 바퀴의 이동거리에 비례하고 폭에 반비례한다. 주행기록계에서 로봇의 이동속도와 각속도를 이용하여 로봇의 위치를 추정하는 방식을 이산시간에서 표현하면 수식 (3)과 같다.

여기서 k는 샘플링 시간을, k-1은 이전 샘플링 시간을 나타낸다. 즉 식 (3)은 이전 샘플링 단계에서 얻어진 위치정보를 기준으로 이동거리와 회전량을 이용하여 이동거리를 누적시킴으로써 로봇의 위치를 추정할 수 있다.

2.2 초음파 위성 시스템

일반적으로 초음파를 이용한 위치 추정 방법은 복수의 초음파를 이용하여 삼각측량 및 거리측정의 중첩을 통하여 위치측정 데이터의 정확도를 높이는 방법을 사용한다[8]. 그러나 초음파의 특성에 의해 복수의 초음파를 동시에 수신할 수 없기 때문에 순차적으로 수신할 수밖에 없고, 따라서 시간지연이 발생하게 된다.

이 문제를 해결하기 위해 하나의 초음파 센서를 사용하게 되면 초음파 센서에 의한 위치측정은 주행기록계의 누적오차가 완전히 제거되지 않는 현상이 나타난다. J. Z. Sasiadek와 Q Wang은 퍼지가중치를 사용한 위치측정의 오차를 줄이는 방법[9]을 제시하였는데, 이 방법은 초음파 센서에 적용하기에는 반응이 느리고 오차의 감소가 두 개의 초음파 센서를 사용한 위치인식에 비해 적어 시스템에 적용하기에 문제가 남아있었다.

본 논문에서는 J. Z. Sasiadek and Q Wang이 제안한 퍼지가중치를 보안하여 환경의 변화와 외력에 빠르고 효과적으로 위치측정의 오차를 제거하고 초음파 센서의 특성에 따른 지연을 제거하기 위하여 하나의 초음파 센서만을 사용하여 높은 신뢰도의 위치 추정이 가능하도록 제안한다.

초음파 위성 중 초음파 발신기의 위치는 미리 주어지며, 초음파 수신기는 로봇의 양 바퀴축의 중앙에 위치한다. 초음파 발신기가 초음파를 발생시키면 로봇의 수신기에서 이를 수신하여 발신기와 수신기의 거리를 계산할 수 있다. 로봇이 그림 4와 같이 2차원 평면상에서 움직일 때, 초음파 위성을 통하여 얻은 거리정보는 식 (4)과 같다.

그림 4초음파 위성을 이용한 이동로봇의 위치 측정 Fig. 4 Localization with ultrasonic satellite

여기서 si는 i번째 초음파 발신기에 대한 위치벡터이다.

 

3. 위치추정 알고리즘

본 논문은 주행 기록계와 초음파 위성을 융합함으로써 측정 오차를 최소화 하는 것을 목표로 한다. 두 시스템을 통합하기 위해 시스템 모델은 앞에서 설계한 주행기록계 모델을, 측정 모델은 초음파 위성 모델을 사용하여 확장칼만필터로 측정결과를 융합하였으며[11], 성능을 향상시키기 위해 퍼지 시스템을 도입하였다.

3.1 시스템 모델

확장칼만필터에서 시스템의 상태 변화를 나타내는 시스템 모델은 식 (5)와 같으며, 시간 k−1에서의 시스템 상태 와 입력 uk−1, 시스템 노이즈 wk−1에 따라 현재 시스템 상태 가 정해진다는 것을 표현한다.

본 논문에서는 주행기록계를 시스템 모델 식으로 사용하므로 식 (3)을 식 (5)에 적용하면 식 (6)을 구할 수 있다.

시스템의 입력으로 사용되는 주행기록계의 출력 uk−1는 Δs와 Δθ로 이루어진다. 비선형 시스템을 확장칼만필터에 적용하기 위해서는 시스템 모델의 선형화 작업이 필요한데, 여기서는 선형화 작업을 위하여 자코비안 행렬을 사용하였다.

마지막으로 확장칼만필터의 시스템 모델을 완성하기 위해서는 노이즈에 대한 검토가 필요하다. 본 논문에서는 오차가 가우시안 정규분포 형태를 띤다고 가정하였으며, 시스템 모델 오차 Qk는 식 (8)과 같이 표현된다.

여기서 δ는 오차의 분산을 나타내는 계수이며, 각각의 오차는 서로 독립이므로 대각행렬을 통해 나타낸다.

3.2 측정 모델

측정 모델은 시스템 모델을 통해 예측한 시스템의 상태에 측정된 데이터를 적용하는 과정이다. 확장칼만필터에서 사용되는 측정 모델은 식 (9)와 같은 형태로 나타낸다.

여기서 zk는 센서를 통해 측정된 데이터를 말하며, 측정 모델 h(xk)에 측정 노이즈 uk가 포함되어 있다는 것을 표현한다. 본 논문에서는 위에서 소개한 초음파 위성을 이용하여 측정모델을 설계하며, 설계된 측정모델은 시스템 모델과 같이 선형화 과정이 필요하다. 여기서도 시스템 모델과 같이 자코비안 행렬을 이용하여 선형화 작업을 하면 식 (10)을 얻을 수 있다.

측정 모델에 대한 측정 노이즈 uk는 초음파 위성의 거리 측정에 대한 오차로써, 역시 정규분포 형태로 정의하면 공분산 행렬 Rk는 식 (11)과 같이 나타낼 수 있다.

여기서 λ는 측정 시 발생하는 오차 공분산을 의미한다.

3.3 예측 단계

확장칼만필터의 예측 단계는 사전 추정치 와 확률분포를 나타내는 오차 공분산 를 예측하는 단계로 식 (12), (13)과 같이 표현된다.

여기서 식 (12)는 시스템 모델을 통해 로봇의 실제 위치를 예측하며, 식 (13)은 로봇의 위치에 대한 확률분포를 나타낸다. 이 확률분포에는 시스템 모델에서의 오차 공분산 Qk가 반영된다.

3.4 보정 단계

확장칼만필터의 보정 단계는 사후 추정치 xk와 오차 공분산 Pk를 구하는 단계로, 측정된 데이터를 이용하여 예측 값에 포함된 오차를 보정하는 단계이다. 이 단계에서 중요한 부분은 칼만이득을 구하는 부분인데, 칼만이득은 측정값이 예측값에 반영되는 정도를 나타낸다.

Kk : 칼만이득zk : 초음파 위성 측정치

3.5 퍼지 시스템

확장칼만필터의 칼만이득은 시스템 오차 Q와 측정 오차 R의 관계에 의하여 그 값이 정해는 것을 알 수 있다. Q가 증가하면 사전확률분포 가 커지게 되어 R에 대한 영향이 줄어들기 때문에 칼만이득은 증가한다. 반대로 R이 증가하게 되면 칼만이득은 감소한다.

이처럼 칼만필터의 성능은 시스템 모델과 측정 모델에서의 에러 모델의 설계에 따라 달라지는 것을 알 수 있다. 하지만 모든 상황에 맞게 에러 모델을 설계하는 것은 어려움이 존재한다. 그래서 본 논문에서는 퍼지 로직을 이용하여 R과 Q값을 조절하여 상황에 따라 추정을 하는 방법으로 식(17)을 제안한다.

확장칼만필터에서는 시스템 모델의 잡음과 측정 모델의 잡음의 크기에 따라 각 모델의 신뢰도가 다르다. 이를 이용하여 에러 Q와 R에 가중치를 적용하여 상황에 따라 시스템 모델과 측정 모델의 신뢰도를 조절하였다. 즉 R이 상대적으로 커지게 되면 시스템 모델의 신뢰도가 높아지고 반대로 Q값이 상대적으로 커지게 되면 측정모델을 더 신뢰하도록 하여 칼만 필터의 정확도를 높일 수 있다.

유사한 방법으로, J. Z. Sasiadek and Q Wang의 weighted EKF에서는 Q와 R에 각각 α−2을 곱하여 가중치를 적용하였다[8]. 그러나 R과 Q 모두에 α값이 똑같이 적용되면 α에 의한 칼만이득의 변화가 적으며 오차의 변화에 대한 반응이 늦어지게 된다.

따라서 본 논문에서는 Q에 센서 가중치 α2을 곱하여 R과의 상관관계를 유지하여 오차에 빠르게 대응하도록 하였으며, 안정적인 동작을 위하여 α를 0<α<2 사이의 값으로 제한하였다. 본 논문에서 사용되는 센서의 오차특성으로부터 에러 Q와 R은 주행기록계의 각속도와 센서의 측정변화량, 그리고 칼만필터의 잔여오차에 큰 영향을 받는다는 사실을 알고 있으므로, 이를 입력으로 하는 퍼지 시스템을 구현할 수 있다. 퍼지 멤버 함수 관계는 그림 5와 같이 표현된다.

그림 5퍼지 모델 Fig. 5 Fuzzy model

퍼지 제어 규칙은 각속도, 잔여오차, 센서 측정변화와 센서 가중치 α 간의 관계를 표현한다. α가 1보다 커지면 초음파 위성을 더욱 신뢰하고, α가 1보다 작아지면 주행기록계를 더욱 신뢰한다는 뜻이므로 오차특성에 근거하여 아래와 같은 문장으로 규칙을 생성할 수 있다.

표 1퍼지 제어 규칙 Table 1 Fuzzy control rule

 

4. 실 험

4.1 모의실험

주행기록계, 확장칼만필터, 퍼지-확장칼만필터 간의 성능 비교를 위해, 이동로봇이 직사각형의 궤적을 (300, 100)에서 시작하여 반시계 방향으로 움직이며 위치를 추정하는 모의 실험을 진행하였다. 초음파 발신기들은 궤적이 이루는 직사각형의 각 모서리에 위치한다.

그림 6모의실험 결과 Fig. 6 The simulation results

그림 6은 모의실험의 결과로 네 가지의 궤적을 나타내고 있다. 설정된 궤적과 주행기록계에 의한 궤적을 비교해 보면 회전구간을 지날 때마다 주행기록계 궤적의 오차가 커지며, 그 오차는 사라지지 않고 누적되는 것을 확인할 수 있다. 확장칼만필터로 구한 궤적은 주행기록계에 비해 오차가 조금 줄어들었으나, 여전히 오차가 큰 것을 확인할 수 있다. 반면에 퍼지-확장칼만필터로부터 얻어진 궤적은 타 궤적에 비해 설정된 궤적에 오차가 적은 것을 확인하였다.

그림 7확장칼만필터와 퍼지-확장칼만필터의 오차비교 Fig. 7 Comparison of EKF and Fuzzy-EKF’s error

그림 7은 확장칼만필터와 퍼지-확장칼만필터의 궤적이 가지는 오차를 비교한 것이다. 확장칼만필터의 궤적은 초음파 위성 정보를 이용할 수 있는 구간에서 오차를 어느 정도 줄이는 것을 확인할 수 있었으나, 전반적으로 퍼지-확장칼만필터의 궤적이 우수한 것을 확인하였다.

4.2 주행실험

제안한 퍼지-확장칼만필터가 현실에서도 유효한지 확인하기 위해, 이동로봇 Pioneer 3-DX를 직사각형 형태의 궤적으로 주행하게 하였다.

그림 8주행 결과 Fig. 8 The actual results of running

그림 9확장칼만필터와 퍼지-확장칼만필터의 오차비교 Fig. 9 Comparison of EKF and Fuzzy-EKF’s error

그림 8은 Pioneer 3-DX를 이용한 주행기록이며, 그림 9는 확장칼만필터와 퍼지-확장칼만필터와의 오차를 비교한 그래프이다. 퍼지-확장칼만필터를 사용하였을 경우 확장칼만필터보다 평균 오차가 2.6cm 감소한 것으로 나타났다. 즉 모의실험과 마찬가지로 퍼지-확장칼만필터가 더 나은 성능을 보여주었다.

 

5. 결 론

본 논문에서는 최소한의 센서만을 사용하여 저비용으로 위치측정이 가능한 알고리즘을 구현하기 위하여 초음파 위성과 주행기록계를 퍼지-확장칼만필터를 통해 융합하였다. 측정에 걸리는 시간을 단축하기 위해 초음파 위성을 기존의 방법대로 세 개의 초음파 정보를 순차적으로 받는 형태가 아니라 단일 정보만을 받아 위치를 추정하게 하였다. 다만 이렇게 사용하게 되면 오차가 누적되는 문제가 생긴다. 그 해결책으로써 퍼지 시스템을 이용하여 칼만이득을 보정하는 방법을 사용하였으며, 퍼지 시스템에는 각 센서의 오차특성이 적용되었다. 따라서 제안된 시스템은 저비용의 센서를 사용하면서도 빠른 주기를 갖게 된다.

모의실험과 주행실험을 통하여 기존의 확장칼만필터와 제안한 퍼지-확장칼만필터 알고리즘을 비교하였으며, 두 실험의 결과로부터 제안된 방법이 오차제거에 더욱 효과적임을 보였다.