SLAM은 Simultaneous Localization And Mapping의 약자이다. 로봇이나 자율 이동체가 미지의 주변 환경을 탐색하면서 움직인다고 가정하자.
이 때 로봇은 스스로 이동하기 위한 자신의 제어 입력과 주변 환경을 계측하기 위한 센서를 가지고 있다. 제어 입력과 센서 계측 정보를 안다면 이를 이용하여 공간(환경) 내에서 자기의 위치를 인식하고 주변 환경의 지도를 동시에 작성하는 것이 가능하다. 이것이 SLAM이다.
자기 위치를 알면 환경 지도를 만들 수 있고, 환경 지도가 있으면 그 속에서 자신의 위치를 알 수 있다. 그러나, 2개의 문제가 닭과 달걀 문제처럼 서로 연관되어 있으며, 동시에 풀어야 하는 문제이다. 이것이 SLAM을 어렵게 만드는 원인이다.
SLAM에 관련된 이론은 Kalman filter, Monte Carlo localization, Particle filter등이 있으며 대표적 SLAM 알고리즘 중에서 EKF-SLAM과 FAST-SLAM에 대해 차례로 살펴 보기로 한다. 먼저 본 문서에서 EKF(Extended Kalman Filter)-slam에 대해 살펴본다.
(from wikipedia)
SLAM process
SLAM을 수행하는 절차를 몇 개의 그림을 통해서 개념적으로 설명한다. 그림에서 삼각형은 로봇이고 별은 공간에 고정된 랜드 마크(표식)를 나타낸다.
로봇이 미지 환경에 놓여져 있다 하자. 이 때 장착된 센서를 이용하여 처음으로 3개의 랜드 마크의 위치를 측정하였다.
로봇이 제어 입력을 사용하여 이동한다. 제어 입력을 가한 후, 주행 거리계(odometry) 등을 통해 어느 방향으로 얼마나 로봇이 움직였는지 측정이 가능하다. 따라서 로봇은 이동 후 자신의 위치를 짐작할 수 있다.
로봇이 다시 센서를 이용하여 표식의 위치를 측정한다. 그런데, 로봇이 생각하기에 표식이 있어야 하는 위치에 있지 않다는 것을 발견한다. 이것은 다시 말하면 로봇이 자신이 생각한 위치에 있지 않다는 것이다.
로봇이 odometry 보다 센서 데이터를 더 신뢰한다고 하자(실제 로봇 바퀴는 슬립 등으로 인해 제어 입력만큼 이동이 안될 수도 있다).
따라서 자신의 위치를 더 정확하게 파악하기 위해 표식의 위치 정보를 사용하여 로봇 위치를 보정할 수 있다. 그림에서 점선 표시는 로봇이 원래 자신이 있어야 한다고 생각한 위치이다.
진짜 로봇의 위치는 실선으로 표시된 곳이다. 보정된 위치와 약간의 오차가 있다.
센서들은 완전하지 않기 때문에 로봇이 정밀하게 자신의 위치를 알 수는 없다. 그러나 상기한 방법의 위치 추정은 odometry 정보에만 의존하는 것보다 휠씬 정확하게 로봇 위치 추정을 가능하게 한다.
EKF SLAM
EKF process 요약
랜드 마크(LM: Landmark)가 추출되고, 관찰된 LM가 기존 것인지 새로운 것인지를 분석하는 데이터 연관성 해석(DA: data association)이 되었다면, SLAM과정은 3개 단계의 반복을 통해 수행된다:
Step 1: 로봇을 움직이게 하는 제어입력을 사용해서 상태 추정치를 갱신
.이전의 로봇 상태에 제어 입력을 주면 로봇의 이동(상태 변화)이 발생한다.
.만일 이전의 robot 상태가 (x, y, theta)라면, 가장 간단한 제어 입력의 한 예는 (dx, dy, dt)이다. 제어 입력 추가는 로봇이 새로운 상태 값 (x+dx, y+dy, t+dt)을 갖게 한다.
.본 문서에서는 로봇 양 바퀴의 이동 (delta sr, delta sl)이 제어 입력이다.
Step 2: 재 관찰된 LM으로부터 상태 추정치를 갱신
.재 관찰(즉, 이전에 한번 보였던 LM을 다시 관찰) LM이 상태와 상태의 공분산의 갱신에 사용된다.
.Step 1의 갱신 상태 추정치를 사용한다면 재 관찰된 LM이 어디에 있는지에 대한 계산이 가능하다. 실제 센서에서 측정된 값과 계산된 값으로부터 두 값 사이에 차이가 있다면 이 값은 innovation이 된다. 따라서 innovation이란 추정된 로봇 위치와 실제 로봇 위치 사이의 차이가 된다.
.또 Step 2에서는 상태 변수의 공분산도 최근 변화를 반영하여 갱신된다.
Step 3: 현 상태변수에 새로운 LM을 추가
.관찰된 LM이 기존에 관찰된 것이 아니라면 이것은 새로운 LM이고 상태변수에 추가된다.
시스템 상태 변수:
2차원 평면상에서 로봇이 움직인다 가정하면, 어떤 global 기준 축에 대해, 로봇의 위치와 자세는 (xr, yr, thetar)을 가진다.
또한 상태는 각 LM의 위치 (xi,yi)를 포함한다. 상태변수의 크기는 (3+2n)크기의 벡터이다. LM의 수는 n이다.
분산행렬: P
두 변수의 공분산(covariance)이란 두 변수가 상호 얼마나 강하게 상관성이 있느냐에 대한 척도를 제공한다. 이것은 변수들 사이의 선형의존성(linear dependency)을 계산하는 correlation으로 측정한다.
행렬 P는 여러 부분 행렬로 구성된다. 먼저 로봇 위치 변수 사이의 공분산, LM 사이의 공분산, LM과 로봇위치 사이의 공분산 등이 결합되어 구성된다.
위 그림은 공분산 행렬 P를 보여준다. P의 크기는 (3+2n)x(3+2n)이다.
A: 3x3, 로봇 변수(x,y,theta)에 대한 공분산
B: 2x2, 첫번째 LM에 대한 공분산. LM은 각도 정보 없이 위치만 가진다. 각 LM에 대해 대각 방향으로 마지막 LM의 공분산인 C에 도달할 때까지 반복된다.
D: 2x3, 로봇 상태와 첫번째 LM사이의 공분산을 포함한다.
E: 3x2, 첫번째 LM과 로봇 상태에 대한 공분산으로 D와 전치(transpose) 관계이다.
F: 2x2, 마지막 LM과 첫번째 LM 사이의 공분산이다. G는 F와 전치이다.
초기에는 로봇이 어떤 LM도 관찰하지 않았으므로 P는 행렬 A만 포함한다. 초기 행렬은 대각방향으로 어떤 기본 값을 가지도록 초기화 될 수 있다. 이 값은 초기 위치에 대한 불확실성을 반영한다 (불확실성이 없더라도 초기화가 필요하다. 따라오는 여러 행렬 연산의 singularity를 방지한다)
칼만게인: K
칼만게인은 관찰된 LM을 얼마나 신뢰할 수 있는지, 즉 관찰된 데이터를 얼마나 반영해야 하는지의 신뢰도를 나타낸다.
센서는 정확하지 않고 불확실성을 가지며, 로봇의 odometry도 정확하지 않다. 따라서, 상태 갱신을 위해서 두 데이터를 절충할 수 있다.
만일 센서(레인지 측정장치)가 로봇의 odometry에 비해 부정확하다면 센서 데이터는 신뢰하기 어렵고 칼만게인은 낮아진다. 측정 장치가 우수하고 정확하다면 칼만게인은 높아질 것이다. 칼만게인 행렬은 (3+2n)x2의 크기로 다음과 같다:
행렬의 첫번째 행은 상태변수의 첫번째 행에 기여한다. 즉, 로봇 위치변수 x에 innovation을 얼마나 반영해야 하는지를 나타낸다. 레이저 스캐너 센서의 경우 innovation은 2x1 크기인 (LM 거리, LM 각도)'이므로 게인 행렬의 첫 행의 첫번째 열(r: range)은 LM거리 값을 얼마나 반영해야 할지, 두번째 열(b: bearing)은 LM각도를 얼마나 반영해야 할지를 나타낸다. 둘 다는 상태변수의 첫번째 행에 대한 것이므로 로봇 위치 x에 대한 것이다.
센서모델 및 자코비안: H
먼저 센서 모델은 다음과 같다:
여기서 lambda x는 LM의 x방향의 좌표 위치, x는 로봇 위치의 추정치이다. 이 식은 로봇 위치에서 측정했을 때, LM에 대한 거리(range)와 각도(bearing)에 대한 예측치를 준다. 거리는 로봇과 LM의 직선거리이고, 각도는 기준축(수평축)에 대한 LM의 각도에서 로봇의 자세각을 뺀 값이다. v는 노이즈이다.
이 센서 모델의 x, y, theta에 대한 자코비안은 다음과 같다:
H행렬은 (x,y,theta)가 변할 때, 얼마나 많이 range와 bearing이 변할지를 보여준다.
1행의 첫번째 요소는 x축의 변화에 대한 range의 변화이다. 두번째 요소는 y축에서 변화에 대한 range의 변화에 대한 것이다.
1행의 마지막 요소는 theta의 변화에 대한 것이다. 물론 로봇이 회전할 때 range변화는 없으므로 이것은 0이다.
실제 SLAM에서는 상태 변수에 LM가 포함되어 있으므로, H행렬은 LM에 대한 항들도 포함해야 한다.
한 예로 위 행렬은 우리가 관찰한 센서 정보가 LM 2번일 때의 h및 자코비안 H행렬이다.
HLM2행렬의 첫 3열은 바로 앞에서 나온 H행렬이다.
그리고 LM 하나당 두개의 열이 추가된다. LM 2에 대한 H행렬은 해당 LM 위치인 6과 7열에서만 값을 가지고, 나머지는 모두 0이다. 즉, hLM2가 LM 2에 대한 정보만을 포함하므로 LM 2를 제외한 나머지 LM 상태변수에 대한 h의 도함수는 모두 0이다.
로봇의 이동 모델
제어 입력에 대한 로봇의 이동 모델은 다음과 같다.
여기서 delta sl, sr은 로봇의 왼쪽 바퀴와 오른쪽 바퀴가 회전하여 이동한 양이다. b는 좌우 바퀴 사이의 거리이다. 로봇의 이동 후 위치는 xr(k)=xr(k-1)+f(xr,u)이다.
로봇 상태에 대한 이동 모델의 자코비안은 다음과 같다:
프로세스 노이즈: Q
제어 입력의 노이즈는 좌우 바퀴의 입력에 대한 불확실성이다.
제어 입력에 대한 이동 모델의 자코비안: W
이동 모델의 제어 입력에 대한 1차 도함수 행렬로 구성된다:
센서 측정 오차에 대한 공분산 행렬: R
range와 bearing을 측정하는 레이저스캐너는 오차를 가진다. 상수 c, d는 센서의 정확도를 나타내는 값으로 range값이 1cm의 분산을 가진다면 c=0.01이다. 만일 각도가 1도의 분산을 가진다면 d=1이다.
EKF SLAM의 세 단계
Step 1: odometry 데이터를 사용한 상태의 갱신
실제 SLAM에서 상태 변수는 LM을 포함하므로 상태 추측 모델(이동 모델)은 로봇 변수와 LM을 모두 포함하며 다음과 같다:
공분산 행렬 P의 갱신에 필요한 상태 추측 모델의 상태변수에 대한 자코비안을 구하면 다음과 같다:
행렬 Ak의 크기는 (3+2n)x(3+2n)이다.
제어 입력 (delta sr, delta sl)'에 대한 이동 모델의 자코비안은 다음과 같다:
공분산 행렬 P의 갱신 식은 다음과 같다:
Step 2: 재 관찰된 LM로부터 상태 갱신
로봇이 제어 입력에 의해 이동 후 LM가 하나 관찰되었다면 이것을 이용하여 상태 변수와 상태의 공분산 행렬을 갱신할 수 있다. 즉, 로봇에 설치된 센서에서 LM와의 거리(r: range)와 각도(b: bearing) 값의 벡터 z를 계측할 수 있다.
그런데 이 LM이 이미 한번 관측한 것이라면 로봇의 이동 후 (r, b)의 예측도 가능하다. 예측 값을 벡터 h라 하자. 그러면, z와 h의 차이를 이용해서 상태값과 공분산 행렬을 갱신하는 것이 가능하다. 단, 이 때 칼만 게인을 통해 로봇의 이동(예측) 모델과 센서의 측정 모델의 불확실성을 반영해 준다.
먼저 칼만 게인 식은 다음과 같다:
여기서, 게인 행렬 계산에 사용된 P, H, R 등의 행렬 크기가 표시되었고, S 및 K행렬도 크기를 나타내었다. 게인 계산에 사용되는 행렬들은 모두 앞 부분에 유도 되어 있다. R이 커지면 게인은 작아진다.
일단 칼만 게인이 계산되면 상태와 공분산 행렬의 갱신이 가능하다:
Step 3: 현 상태에 새로운 LM의 추가
만일 관찰한 LM가 기존에 관찰된 LM가 아니라면 이 LM는 새로운 것으로 상태변수에 추가된다. 또한 상태의 크기가 커지면 공분산 행렬도 커진다.
새로운 LM 위치를 y라 하자. y는 LM가 로봇에 의해 관찰된 시점의 값이다. 새로운 상태와 공분산 행렬은 다음과 같다:
Jxr는 y의 로봇 상태에 대한 변화율(자코비안)이고, Jz는 y의 센서 측정값 z에 대한 변화율이다.
센서와 로봇은 각각 불확실성을 나타내는 분산 값 R과 Pxx를 가지므로 이 값들이 LM 위치의 불확실성으로 전파된 값이 공분산 행렬에 추가된다.
.Jz: 센서 측정치 (r, b)의 변화에 대한 LM y의 자코비안
.Jxr: 로봇 상태 (xr,yr,thetar)의 변화에 대한 LM y의 자코비안
.(dy/dxr)Pxx: 불확실성 Pxx는 xr의 것이고 이것이 y에 영향을 줌
.(dy/dz)R: 불확실성 R은 z의 것이고 이것이 y에 영향을 줌
(c.f.)
(df/du)Q: 불확실성 Q는 제어입력 u에 대한 것이고 로봇 이동모델 f에 영향을 줌
JxrPxxJxr'는 로봇 공분산이 LM 공분산으로 전파된 것이고, JzRJz'는 센서의 불확실성이 LM 공분산으로 확산된 것을 보여준다. LM 값에 로봇 위치와 센서 계측이 둘 다 영항을 주므로 두 전파된 공분산의 합이 대각방향에 추가되는 공분산 값이다.
JxrPx(x~yn)은 추가 설명...
참고 자료
[1] SLAM for dummies, MIT Open Course ware.
[2] Open Robotics blog, SLAM part.
[3] Cyrill Stachniss, EKF SLAM 자료
댓글 없음:
댓글 쓰기