import math
import numpy as np
import matplotlib.pyplot as plt
from scipy.stats import norm
EXTEND_AREA = 10.0 # [m] grid map extention length
show_animation = True
일단 임포트하는 라이브러리랑
기본 파라미터로 추가 길이를 선언해주는데
뒤에서 천천히 살펴봐야겠습니다.
메인으로 넘어와서 보면
def main():
print(__file__ + " start!!")
xyreso = 0.5 # xy grid resolution
STD = 5.0 # standard diviation for gaussian distribution
for i in range(5):
ox = (np.random.rand(4) - 0.5) * 10.0
oy = (np.random.rand(4) - 0.5) * 10.0
gmap, minx, maxx, miny, maxy = generate_gaussian_grid_map(
ox, oy, xyreso, STD)
if show_animation: # pragma: no cover
plt.cla()
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect('key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
draw_heatmap(gmap, minx, maxx, miny, maxy, xyreso)
plt.plot(ox, oy, "xr")
plt.plot(0.0, 0.0, "ob")
plt.pause(1.0)
if __name__ == '__main__':
main()
루프를 돌리기 전에
해상도와 가우시안 분포 표준 편차를 설정해 줍니다.
이전에 본 위치 추정 애니메이션과는 달리
5번만 반복수행하나 봅니다.
이제 루프 내부 위주로 보겠습니다.
for i in range(5):
ox = (np.random.rand(4) - 0.5) * 10.0
oy = (np.random.rand(4) - 0.5) * 10.0
gmap, minx, maxx, miny, maxy = generate_gaussian_grid_map(
ox, oy, xyreso, STD)
내부 루프에 애니메이션 조건문을 빼고 보면
균일 분포 4개 데이터를 만들고 - 0.5를 해주면
4개의 리스트가 -0.5 ~ 0.5사이 값이 될것이고
*10.0을 하였으니
나오는 값은 x와 y에 대해 (4, )의 -5.0 ~ 5.0 사이 값이 되겠습니다.
이제 가우시안 그리드 맵 함수를 보겠습니다.
def generate_gaussian_grid_map(ox, oy, xyreso, std):
minx, miny, maxx, maxy, xw, yw = calc_grid_map_config(ox, oy, xyreso)
gmap = [[0.0 for i in range(yw)] for i in range(xw)]
for ix in range(xw):
for iy in range(yw):
x = ix * xyreso + minx
y = iy * xyreso + miny
# Search minimum distance
mindis = float("inf")
for (iox, ioy) in zip(ox, oy):
d = math.hypot(iox - x, ioy - y)
if mindis >= d:
mindis = d
pdf = (1.0 - norm.cdf(mindis, 0.0, std))
gmap[ix][iy] = pdf
return gmap, minx, maxx, miny, maxy
def generate_gaussian_grid_map(ox, oy, xyreso, std):
minx, miny, maxx, maxy, xw, yw = calc_grid_map_config(ox, oy, xyreso)
gmap = [[0.0 for i in range(yw)] for i in range(xw)]
for ix in range(xw):
for iy in range(yw):
x = ix * xyreso + minx
y = iy * xyreso + miny
# Search minimum distance
mindis = float("inf")
for (iox, ioy) in zip(ox, oy):
d = math.hypot(iox - x, ioy - y)
if mindis >= d:
mindis = d
pdf = (1.0 - norm.cdf(mindis, 0.0, std))
gmap[ix][iy] = pdf
return gmap, minx, maxx, miny, maxy
그리드 지도 설정 후에는
중첩 반복문으로 xw, yw로 모든 셀의 값이 0.0인 격자 지도가 만들어집니다.
이 함수의 반복문 내부를 보면
for ix in range(xw):
for iy in range(yw):
x = ix * xyreso + minx
y = iy * xyreso + miny
# Search minimum distance
mindis = float("inf")
for (iox, ioy) in zip(ox, oy):
d = math.hypot(iox - x, ioy - y)
if mindis >= d:
mindis = d
pdf = (1.0 - norm.cdf(mindis, 0.0, std))
gmap[ix][iy] = pdf
minx = -10, xyreso = 0.5, ix = 1 이면
x = -10 + 0.5 = -9.5
ox, oy는 랜드마크의 좌표값으로
zip으로 묶어주면 각 랜드마크들의 리스트가 만들어 집니다.
https://wikidocs.net/32
for ix in range(xw):
for iy in range(yw):
x = ix * xyreso + minx
y = iy * xyreso + miny
# Search minimum distance
mindis = float("inf")
for (iox, ioy) in zip(ox, oy):
d = math.hypot(iox - x, ioy - y)
if mindis >= d:
mindis = d
pdf = (1.0 - norm.cdf(mindis, 0.0, std))
gmap[ix][iy] = pdf
첫번째 격자와 랜드마크 사이 거리 d를 계산해주고,
minds는 처음에 inf 이므로 이후에 mindis = d가 되겠습니다.
4개의 랜드마크에 대해 반복 하면
첫번째 셀에 가장 가까운 랜드마크의 거리가 구해질겁니다.
하지만 첫번째 셀이랑 가장 가까운 거리가 매우 멀다고 가정하고,
예를들어 10이라면 cdf는 1에 가까운 값이 될것이며
확률밀도함수 pdf는 0에 가까운 값이 될것입니다.
즉 이 함수는 랜드마크와 가장 가까운 셀일수록
높은 확률값을 가지게 됩니다.
"""
2D gaussian grid map sample
author: Atsushi Sakai (@Atsushi_twi)
"""
import math
import numpy as np
import matplotlib.pyplot as plt
from scipy.stats import norm
EXTEND_AREA = 10.0 # [m] grid map extention length
show_animation = True
def generate_gaussian_grid_map(ox, oy, xyreso, std):
minx, miny, maxx, maxy, xw, yw = calc_grid_map_config(ox, oy, xyreso)
gmap = [[0.0 for i in range(yw)] for i in range(xw)]
for ix in range(xw):
for iy in range(yw):
x = ix * xyreso + minx
y = iy * xyreso + miny
# Search minimum distance
mindis = float("inf")
for (iox, ioy) in zip(ox, oy):
d = math.hypot(iox - x, ioy - y)
if mindis >= d:
mindis = d
pdf = (1.0 - norm.cdf(mindis, 0.0, std))
gmap[ix][iy] = pdf
return gmap, minx, maxx, miny, maxy
def calc_grid_map_config(ox, oy, xyreso):
minx = round(min(ox) - EXTEND_AREA / 2.0)
miny = round(min(oy) - EXTEND_AREA / 2.0)
maxx = round(max(ox) + EXTEND_AREA / 2.0)
maxy = round(max(oy) + EXTEND_AREA / 2.0)
xw = int(round((maxx - minx) / xyreso))
yw = int(round((maxy - miny) / xyreso))
return minx, miny, maxx, maxy, xw, yw
def draw_heatmap(data, minx, maxx, miny, maxy, xyreso):
x, y = np.mgrid[slice(minx - xyreso / 2.0, maxx + xyreso / 2.0, xyreso),
slice(miny - xyreso / 2.0, maxy + xyreso / 2.0, xyreso)]
plt.pcolor(x, y, data, vmax=1.0, cmap=plt.cm.Blues)
plt.axis("equal")
def main():
print(__file__ + " start!!")
xyreso = 0.5 # xy grid resolution
STD = 5.0 # standard diviation for gaussian distribution
for i in range(5):
ox = (np.random.rand(4) - 0.5) * 10.0
oy = (np.random.rand(4) - 0.5) * 10.0
gmap, minx, maxx, miny, maxy = generate_gaussian_grid_map(
ox, oy, xyreso, STD)
if show_animation: # pragma: no cover
plt.cla()
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect('key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
draw_heatmap(gmap, minx, maxx, miny, maxy, xyreso)
plt.plot(ox, oy, "xr")
plt.plot(0.0, 0.0, "ob")
plt.pause(1.0)
if __name__ == '__main__':
main()
이전에 살펴본 위치 추정 알고리즘들은 로봇의 자세를 구하는 기술들로 지도가 존재하는 상황에서만 사용가능 합니다. 하지만 대부분의 경우에는 그런 지도 정보가 없을 것이고, 건물 설계도 조차 실제 건물과는 조금씩 다를것입니다. 그래서 로봇이 실내 공간을 주행하기 위해서 지도 작성은 중요한 기술이라 할수 있겠습니다.
지도 작성은 다음과 같은 이유들로 매우 중요한 문제라고 할수 있겠습니다.
지도 작성의 중요성
- 모든 가능한 가정 공간들이 매우 거대하며, 지도는 연속 공간상에서 정의되므로 무한대의 차원이 될수 있겠습니다. 그러므로 그리드 근사화를 통해 이산 추정을 통해 구하면 계산량을 크게 줄일수가 있겠습니다.
- 지도 작성은 "닭이 먼저일까 계란이 먼저일까"의 문제와 동일한 것이며 동시적 위치 추정 및 지도 작성 slam이라고 부르기도 합니다. 로봇은 주행하면서 오도메트리 상에 오차가 누적이되고, 오도메트리 정보로만 어디에 존재하는지 신뢰하기는 어렵습니다. 하지만 지도가 존재할때 로봇의 자세를 추정하는 방법들을 살펴봤었고, 로봇의 위치를 알때 지도를 만든느것은 상대적으로 쉬운 문제가 됩니다. 하지만 일반적으로는 지도 정보도 없고 자세 정보도 없으니 로봇은 지도 근사화와 위치 추정 둘다 수행해야만 합니다.
지도 작성 문제가 어려운 이유
- 크기 : 로봇 센서의 측정 가능거리보다 큰 공간에서는 완전한 지도를 만들기가 더 어렵스비다.
- 노이즈 : 로봇 센서와 엑추에이터들은 노이즈를 가지고 있으므로, 노이즈가 클수록 지도 작성이 더 여렵습니다.
- 관측시 모호함 : 비슷하게 생깃 곳이 많을 수록, 이전에 찾은 장소와 다른 장소들 간에 관계를 구분하기가 더 힘들게 됩니다.
- 사이클 : 이전 장소에 돌아와서 만들어지는 사이클이 지도 작성 문제를 더 어렵게 합니다. 왜냐하면 오도메틜 정보가 다시 되돌아왔을때는 누적이 되있고, 다른 경로로 돌아아와 사이클이 닫히면서 누적된 오도메트리 오차가 커질수 있습니다
지도 작성 예시
아래의 그림은 오도메트리 정보와 레이저 관측 정보로 만든 실내 지도 예시가 되겠습니다.
a 그림은 오도메트리 위치에따라 인덱싱 된 처리되지 않은 관측 데이터들로 단순히 겹쳐지면서 지도의 형태가 되어있지 않습니다.
b 그림은 지도 작성 알고리즘에 이 데이터를 사용하여 만든 지도를 보여주고 있습니다.
일단 이번에는 로봇의 위치 정보가 주어진 상황이라고 생각해보고 지도 작성 문제를 다뤄볼것이고
이후 slam의 경우도 살펴볼 것이며, 아래의 그림처럼 가장 일반적인 지도 작성 기술을 점유격자 지도작성 이라고 합니다.
점유 격자 지도작성 알고리즘 개요
점유 격자 지도작성 알고리즘은 로봇의 자세 정보를 알고 있을때 불확실한 관측 데이터만가지고
일관성 있는 지도를 만들어내는 문제를 다룹니다.
점유 격자의 기본 개념은 지도를 임의의 확률 변수값을 가진 그리드 셀로 표현하고,
이 변수는 이진 값이되에 이 장소가 점유 된것인지 아닌지 판단하는 것입니다.
즉, 점유 격자 지도작성 알고리즘은 이러한 점유 여부 값 변수들을 이용하여 사후확률 추정치를 근사화하는것이라고 할수 있겠습니다.
점유 격자 지도작성 알고리즘
기본 적인 점유 격자 지도작성 알고리즘은 로봇의 경로와 관측 데이터들을 이용하여
지도에 대한 사후확률을 계산하는것이라 할수 있겠습니다.
여기서 지도는 연속 공간을 그리드로 분할하여 나타내므로,
지도를 인덱스 i의 각 그리드 셀을 합한것이라고 할수 있겠습니다.
그러면 각 그리드 셀의 값은 어떻게 될까요?
기본 점유 격자 지도작성 알고리즘에는 그리드 셀은 이진값을 가지게 됩니다.
즉, 점유되어있으면/ 장애물이 있으면 1이고, 없다면 0이 됩니다.
위 기본 점유격자 지도작성알고리즘 9.1을 각 셀의 사후확률 근사에 대한 식으로 정리해보면 다음과 같으며,
전체 지도에 대한 사후확률은 각 그리드의 주변 확률들의 곱으로 근사하여 계산할수 있겠습니다.
지도를 각 그리드셀에 대해 분해한 덕분에, 각 그리드 셀에 대한 점유 확률 추정은 정적 상태에서 이진 추정 문제가 되었습니다.
이 정적 상태에서 이진 추정 문제는 이전에 4.1.4장서 살펴본적이 있었는데
이 이진 베이즈 필터를 점유 격자 지도 작성에 적용한 알고리즘이 아래와 같겠습니다.
점유 격자 지도 알고리즘과 로그 오즈
위 알고리즘에서는 점유 여부를 로그 오즈를 사용해서 구하였다.
확률을 로그 오즈로 계산시 장점은 확률 값이 0, 1사이에만 한정되는것을 방지할수 있고, 다음의 로그 오즈비로 손쉽게 확률로 되돌릴수 있다.
역 센서 모델
지금 살펴보고 있는 기본 점유 격자 지도 알고리즘은 역 센서 모델을 사용하고 있다.
역 센서 모델은 역 관측 모델의 로그 오즈 형태로 나타낸 것이다.
역 관측 모델
기존의 관측 모델은 로봇의 상태가 주어질때 관측값에 대한 확률이니
역 관측 모델은 관측값이 주어질때 로봇의 상태에 대한 확률일 것이다.
이 모델에서는 각 셀에다가 이진 값 l_occ와 l_cc을 저장하는데,
여기서 값이 할당되는 셀의 범위는 장애물의 두께인 alpha와 센서 빔 폭, 각도 beta가 된다.
def normalize_probability(grid_map):
sump = sum([sum(i_data) for i_data in grid_map.data])
for ix in range(grid_map.x_w):
for iy in range(grid_map.y_w):
grid_map.data[ix][iy] /= sump
return grid_map
while SIM_TIME >= time:
time += DT
print("Time:", time)
u = calc_input()
yaw = xTrue[2, 0] # Orientation is known
xTrue, z, ud = observation(xTrue, u, RF_ID)
grid_map = histogram_filter_localization(grid_map, u, z, yaw)
if show_animation:
plt.cla()
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
draw_heat_map(grid_map.data, mx, my)
plt.plot(xTrue[0, :], xTrue[1, :], "xr")
plt.plot(RF_ID[:, 0], RF_ID[:, 1], ".k")
for i in range(z.shape[0]):
plt.plot([xTrue[0, :], z[i, 1]], [
xTrue[1, :], z[i, 2]], "-k")
plt.title("Time[s]:" + str(time)[0: 4])
plt.pause(0.1)
여기서 시각화 하는 부분을 빼면
1. 입력 계산 calc_input
2. 관측 observation
3. 히스토그램 필터 위치추정 histogram_filter_localization
이렇게 3 파트로 나눌수 있겠습니다.
입력 계산을 살펴보겠습니다.
def calc_input():
v = 1.0 # [m/s]
yaw_rate = 0.1 # [rad/s]
u = np.array([v, yaw_rate]).reshape(2, 1)
return u
여기는 속도 기반 모델인지
2,1의 행렬로
첫재 값은 선속도, 둘때 값은 각속도를 넣어서 반환해 줍니다
다음으로 관측을 봅시다
def observation(xTrue, u, RFID):
xTrue = motion_model(xTrue, u)
z = np.zeros((0, 3))
for i in range(len(RFID[:, 0])):
dx = xTrue[0, 0] - RFID[i, 0]
dy = xTrue[1, 0] - RFID[i, 1]
d = math.hypot(dx, dy)
if d <= MAX_RANGE:
# add noise to range observation
dn = d + np.random.randn() * NOISE_RANGE
zi = np.array([dn, RFID[i, 0], RFID[i, 1]])
z = np.vstack((z, zi))
# add noise to speed
ud = u[:, :]
ud[0] += np.random.randn() * NOISE_SPEED
return xTrue, z, ud
일단 관측하기전에
실제 상태에 노이즈 없는 입력을 주어
실제 이동 위치를 구하고
실제 이동 위치와 RFID 사이의 거리를 계산하겠습니다.
여기서 구한 거리 d가 관측 가능 거리에 속한다면
관측 노이즈 dn과 가우시안 분포를 곱하여 노이즈를 반영한 실제 측정치가 됩니다.
이 실제 측정치들을 모든 RFID에 구하여 z에 vstack으로 쌓아주면
아마 zi가 거리, rfid x, rfid y이므로 (1, 3)
zi가 4개이니
z는 (4, 3)d의 데이터가 될겁니다.
이제 실제 이동후 위치와 노이즈가 반영된 관측치, 노이즈가 반영된 입력치를 필터에 사용할수있도록 반환해줍시다.
while SIM_TIME >= time:
time += DT
print("Time:", time)
u = calc_input()
yaw = xTrue[2, 0] # Orientation is known
xTrue, z, ud = observation(xTrue, u, RF_ID)
grid_map = histogram_filter_localization(grid_map, u, z, yaw)
if show_animation:
plt.cla()
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
draw_heat_map(grid_map.data, mx, my)
plt.plot(xTrue[0, :], xTrue[1, :], "xr")
plt.plot(RF_ID[:, 0], RF_ID[:, 1], ".k")
for i in range(z.shape[0]):
plt.plot([xTrue[0, :], z[i, 1]], [
xTrue[1, :], z[i, 2]], "-k")
plt.title("Time[s]:" + str(time)[0: 4])
plt.pause(0.1)
이제 이 시뮬레이션의 핵심인
히스토그램 위치 추정 파트를 살펴보겠습니다.
def histogram_filter_localization(grid_map, u, z, yaw):
grid_map = motion_update(grid_map, u, yaw)
grid_map = observation_update(grid_map, z, RANGE_STD)
return grid_map
def observation_update(grid_map, z, std):
for iz in range(z.shape[0]):
for ix in range(grid_map.x_w):
for iy in range(grid_map.y_w):
grid_map.data[ix][iy] *= calc_gaussian_observation_pdf(
grid_map, z, iz, ix, iy, std)
grid_map = normalize_probability(grid_map)
return grid_map
관측 갱신에서는 기존의 지도, 관측 데이터,
관측 노이즈 표준편차를 사용하여 계산됩니다.
우선 z에 대해서 도는것을 보니 각각의 관측치 데이터를 가지고
전체 격자들의 값들을 변경시키는것 같습니다.
루프문 맨 안에 보면 가우시안 관측 밀도 함수 계산값을 구하여 각 격자 값을 갱신시키고 있습니다.
가우시안 관측 확률 밀도 계산 함수를 살펴보겠습니다.
def calc_gaussian_observation_pdf(grid_map, z, iz, ix, iy, std):
# predicted range
x = ix * grid_map.xy_resolution + grid_map.min_x
y = iy * grid_map.xy_resolution + grid_map.min_y
d = math.hypot(x - z[iz, 1], y - z[iz, 2])
# likelihood
pdf = (1.0 - norm.cdf(abs(d - z[iz, 0]), 0.0, std))
return pdf
일단 z는 첫번째 rfid (10, 0)로
여기서 z는 관측 모델에서 구한 (10, 10, 0)으로 가정하겠습니다. (거리, x, y)
def calc_gaussian_observation_pdf(grid_map, z, iz, ix, iy, std):
# predicted range
x = ix * grid_map.xy_resolution + grid_map.min_x
y = iy * grid_map.xy_resolution + grid_map.min_y
d = math.hypot(x - z[iz, 1], y - z[iz, 2])
# likelihood
pdf = (1.0 - norm.cdf(abs(d - z[iz, 0]), 0.0, std))
return pdf
격자 (1,1)에서 예측 d는 첫번재 랜드마크와 매우 멀으므로
abs(예측 거리 - 실제 거리)는 매우 큰 값이 될것이고,
누적 정규 분포에 따라 1에 가까운 값이 됩니다.
하지만 우도를 계산하는만큼 1 - (1에 가까운 값)이라면 0에 가까운 확률 밀도가 반환되고
결국 (1,1) 격자는 가중치가 매우 작아질겁니다.
하지만 실제 첫번째 랜드마크와 가까운 격자라면
예측 거리 - 실제 거리는 매우 작은 값이 될것이며
높은 pdf 값을 반환한다고 볼수 있겠습니다.
def observation_update(grid_map, z, std):
for iz in range(z.shape[0]):
for ix in range(grid_map.x_w):
for iy in range(grid_map.y_w):
grid_map.data[ix][iy] *= calc_gaussian_observation_pdf(
grid_map, z, iz, ix, iy, std)
grid_map = normalize_probability(grid_map)
return grid_map
다시 관측 갱신으로 돌아와
확률 정규화 과정을 시키면
첫번째 루프의 히스토그램 필터 수행이 완료됩니다.
# Parameters
SIM_TIME = 20.0 # simulation time [s]
DT = 0.5 # time tick [s]
MAX_RANGE = 10.0 # maximum observation range
MOTION_STD = 1.0 # standard deviation for motion gaussian distribution
RANGE_STD = 3.0 # standard deviation for observation gaussian distribution
파라미터를 조금 수정해서
동작시켜보겠습니다.
전체 코드 histogram_filter.py
"""
Histogram Filter 2D localization example
In this simulation, x,y are unknown, yaw is known.
Initial position is not needed.
author: Atsushi Sakai (@Atsushi_twi)
"""
import copy
import math
import matplotlib.pyplot as plt
import numpy as np
from scipy.ndimage import gaussian_filter
from scipy.stats import norm
# Parameters
SIM_TIME = 20.0 # simulation time [s]
DT = 0.5 # time tick [s]
MAX_RANGE = 10.0 # maximum observation range
MOTION_STD = 1.0 # standard deviation for motion gaussian distribution
RANGE_STD = 3.0 # standard deviation for observation gaussian distribution
# grid map param
XY_RESOLUTION = 0.5 # xy grid resolution
MIN_X = -15.0
MIN_Y = -5.0
MAX_X = 15.0
MAX_Y = 25.0
# simulation parameters
NOISE_RANGE = 2.0 # [m] 1σ range noise parameter
NOISE_SPEED = 0.5 # [m/s] 1σ speed noise parameter
show_animation = True
class GridMap:
def __init__(self):
self.data = None
self.xy_resolution = None
self.min_x = None
self.min_y = None
self.max_x = None
self.max_y = None
self.x_w = None
self.y_w = None
self.dx = 0.0 # movement distance
self.dy = 0.0 # movement distance
def histogram_filter_localization(grid_map, u, z, yaw):
grid_map = motion_update(grid_map, u, yaw)
grid_map = observation_update(grid_map, z, RANGE_STD)
return grid_map
def calc_gaussian_observation_pdf(grid_map, z, iz, ix, iy, std):
# predicted range
x = ix * grid_map.xy_resolution + grid_map.min_x
y = iy * grid_map.xy_resolution + grid_map.min_y
d = math.hypot(x - z[iz, 1], y - z[iz, 2])
# likelihood
pdf = (1.0 - norm.cdf(abs(d - z[iz, 0]), 0.0, std))
return pdf
def observation_update(grid_map, z, std):
for iz in range(z.shape[0]):
for ix in range(grid_map.x_w):
for iy in range(grid_map.y_w):
grid_map.data[ix][iy] *= calc_gaussian_observation_pdf(
grid_map, z, iz, ix, iy, std)
grid_map = normalize_probability(grid_map)
return grid_map
def calc_input():
v = 1.0 # [m/s]
yaw_rate = 0.1 # [rad/s]
u = np.array([v, yaw_rate]).reshape(2, 1)
return u
def motion_model(x, u):
F = np.array([[1.0, 0, 0, 0],
[0, 1.0, 0, 0],
[0, 0, 1.0, 0],
[0, 0, 0, 0]])
B = np.array([[DT * math.cos(x[2, 0]), 0],
[DT * math.sin(x[2, 0]), 0],
[0.0, DT],
[1.0, 0.0]])
x = F @ x + B @ u
return x
def draw_heat_map(data, mx, my):
max_value = max([max(i_data) for i_data in data])
plt.pcolor(mx, my, data, vmax=max_value, cmap=plt.cm.get_cmap("Blues"))
plt.axis("equal")
def observation(xTrue, u, RFID):
xTrue = motion_model(xTrue, u)
z = np.zeros((0, 3))
for i in range(len(RFID[:, 0])):
dx = xTrue[0, 0] - RFID[i, 0]
dy = xTrue[1, 0] - RFID[i, 1]
d = math.hypot(dx, dy)
if d <= MAX_RANGE:
# add noise to range observation
dn = d + np.random.randn() * NOISE_RANGE
zi = np.array([dn, RFID[i, 0], RFID[i, 1]])
z = np.vstack((z, zi))
# add noise to speed
ud = u[:, :]
ud[0] += np.random.randn() * NOISE_SPEED
return xTrue, z, ud
def normalize_probability(grid_map):
sump = sum([sum(i_data) for i_data in grid_map.data])
for ix in range(grid_map.x_w):
for iy in range(grid_map.y_w):
grid_map.data[ix][iy] /= sump
return grid_map
def init_grid_map(xy_resolution, min_x, min_y, max_x, max_y):
grid_map = GridMap()
grid_map.xy_resolution = xy_resolution
grid_map.min_x = min_x
grid_map.min_y = min_y
grid_map.max_x = max_x
grid_map.max_y = max_y
grid_map.x_w = int(round((grid_map.max_x - grid_map.min_x)
/ grid_map.xy_resolution))
grid_map.y_w = int(round((grid_map.max_y - grid_map.min_y)
/ grid_map.xy_resolution))
grid_map.data = [[1.0 for _ in range(grid_map.y_w)]
for _ in range(grid_map.x_w)]
grid_map = normalize_probability(grid_map)
return grid_map
def map_shift(grid_map, x_shift, y_shift):
tmp_grid_map = copy.deepcopy(grid_map.data)
for ix in range(grid_map.x_w):
for iy in range(grid_map.y_w):
nix = ix + x_shift
niy = iy + y_shift
if 0 <= nix < grid_map.x_w and 0 <= niy < grid_map.y_w:
grid_map.data[ix + x_shift][iy + y_shift] =\
tmp_grid_map[ix][iy]
return grid_map
def motion_update(grid_map, u, yaw):
grid_map.dx += DT * math.cos(yaw) * u[0]
grid_map.dy += DT * math.sin(yaw) * u[0]
x_shift = grid_map.dx // grid_map.xy_resolution
y_shift = grid_map.dy // grid_map.xy_resolution
if abs(x_shift) >= 1.0 or abs(y_shift) >= 1.0: # map should be shifted
grid_map = map_shift(grid_map, int(x_shift), int(y_shift))
grid_map.dx -= x_shift * grid_map.xy_resolution
grid_map.dy -= y_shift * grid_map.xy_resolution
grid_map.data = gaussian_filter(grid_map.data, sigma=MOTION_STD)
return grid_map
def calc_grid_index(grid_map):
mx, my = np.mgrid[slice(grid_map.min_x - grid_map.xy_resolution / 2.0,
grid_map.max_x + grid_map.xy_resolution / 2.0,
grid_map.xy_resolution),
slice(grid_map.min_y - grid_map.xy_resolution / 2.0,
grid_map.max_y + grid_map.xy_resolution / 2.0,
grid_map.xy_resolution)]
return mx, my
def main():
print(__file__ + " start!!")
# RF_ID positions [x, y]
RF_ID = np.array([[10.0, 0.0],
[10.0, 10.0],
[0.0, 15.0],
[-5.0, 20.0]])
time = 0.0
xTrue = np.zeros((4, 1))
grid_map = init_grid_map(XY_RESOLUTION, MIN_X, MIN_Y, MAX_X, MAX_Y)
mx, my = calc_grid_index(grid_map) # for grid map visualization
while SIM_TIME >= time:
time += DT
print("Time:", time)
u = calc_input()
yaw = xTrue[2, 0] # Orientation is known
xTrue, z, ud = observation(xTrue, u, RF_ID)
grid_map = histogram_filter_localization(grid_map, u, z, yaw)
if show_animation:
plt.cla()
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
draw_heat_map(grid_map.data, mx, my)
plt.plot(xTrue[0, :], xTrue[1, :], "xr")
plt.plot(RF_ID[:, 0], RF_ID[:, 1], ".k")
for i in range(z.shape[0]):
plt.plot([xTrue[0, :], z[i, 1]], [
xTrue[1, :], z[i, 2]], "-k")
plt.title("Time[s]:" + str(time)[0: 4])
plt.pause(0.1)
print("Done")
if __name__ == '__main__':
main()
마지막으로 3차원 지도 작성에 대해서 살펴보겠습니다. 이 지도는 그동안 보았던 2차원 지도보다 3가지 핵심 장점을 가지고 있는데
- 첫째, 3차원 지도는 관점에 따라서 2차원 보다 얻기 쉽습니다. 건물 내부의 3차원 구조들을 보듯이 대응관계 추정 문제는 더 단순해 집니다. 이는 2차원에서 보던 장소가 3차원이 되면서 이들 서로서로를 햇갈릴 위험이 줄어들기 때문입니다.
- 두번째, 3차원 지도는 주행시에 유용합니다. 많은 로봇 환경은 수직 차원의 점유로 다양한 변형들을 사용할수 있겠는데, 이러한 환경을 설계함으로서 장애물과의 충돌 위험을 크게 줄일수 있습니다.
- 셋째, 많은 로봇 동작에서 3차원 정보를 필요로 하며, 이러한 작업들은 위치 추정뿐만이 아니라 사람이나 물체의 탐색도 포함합니다.
- 마지막으로, 3차원 지도는 지도의 잠재 사용자에게 더 많은 정보들을 전달합니다. 로봇 주행에 한하여 지도를 만들경우 여러가지 상황들에서 사용할수 없겠지만, 사람에 의해 사용하기 위한 지도를 얻는 경우에(지진후 구조작업자가 건물에 들어가는등) 3차원 지도는 매우 중요한 역활을 할것입니다.
2차원 지도보다 3차원이 다양한 이점들이 존재하나 로봇 공학 커뮤니티에서 3차원 지도작성에 덜 관심을 갖는것은 놀랍습니다.
그림 14.8 (a) 3차원 지도작성을 위해 두개의 레이저 거리계가 장착된 로봇
(b) 이 로봇으로 얻은 파노라마 이미지. 이미지는 거리계로 측정한 것 지점에 대응하게 됩니다.
3차원 추정은 계산량에 있어서 도전적인 문제라 할수 있는데 그동안 살펴본 많은 기술들을 수정해야 합니다. 하지만 유용한 2차원 지도와 함께 유용한 3차원 지도를 얻을수 있겠습니다. 이번 장에서는 3차원 지도 얻는 작업의 결과를 살벼볼것이고, 여기에는 2차원 레이저 거리계와 파노라마 거리계가 장착된 탐사 로봇을 사용하겠습니다. 이 로봇은 그림 14.8a에서 볼수 있는데, 레이저 스캐너 하나는 전방을 감지하여 수평 방향으로 레이저를 방출할겁니다. 이 레이저 거리계는 이번 장에서 소개할 알고리즘을 사용하여 동시적 지도작성과 위치 추정을 수행하는데 사용할겁니다. 두번째는 위를 향하여 로봇의 운동 방향과 직교합니다. 이 레이저는 벽과 천장을 스캔하여 3차원 지도 작성에 중요한 거리 정보들을 모읍니다. 마지막으로 파노라마 카메라는 위를 향한 래이저 스케너와 인접하게 장착되는데, 관측 치에 알맞은 색상 정보를 얻을수 있도록 해줍니다.
그림 14.9. 3차원 구조 지도. 좌측은 고해상도 모델, 우측은 저해상도 모델ㅇ비니다.
그림 14.9는 실내 복도환경에서 얻은 3차원 지도의 영상을 보여주고 있습니다. 위치 추정은 사이클이 있는 증분 최대 우도 지도작성 알고리즘을 사용하였습니다. 수직 거리 측정치는 추가적인 자세 추정없이 폴리곤으로 변환되어 결과 지도를 가상 환경에서 볼수 있습니다. 그리고 그림 14.9a와 d같이 현실적으로 불가능한 관점에서 볼수있도록 해줍니다.
이 지도의 한계는 매우 많은 수의 폴리곤이 존재하는것인데, 이 폴리곤 지도를 단순화 하는것은 이런 복잡한 폴리곤 지도를 빠르게 만들기 위한 방법을 찾기 위해 컴퓨터 그래픽스 분야에서 오랜 시간 연구되어 왔습니다. 그림 14.9의 왼쪽 열은 고해상도 지도를 보여주고 있으며 우측 열은 왼쪽 지도에서 5%의 폴리곤만 가지는 지도를 보여주고 있습니다. 이러한 폴리곤 지도의 축소는 컴퓨터 그래픽 서적에 나온 기술을 적용하여 얻었습니다. 이 축소된 지도는 정확도 손실없이 실시간으로 만들어질수 있겠습니다.
그림 14.10 3차원 택스처 지도의 스냅샷
마지막으로 그림 14.10은 3차원 텍스처 지도로 얻은 이미지들을 보여주고 있습니다. 이 지도는 파노라마 색상정보를 구조 모델에 사영하여 얻은것인데, 이 지도는 가상환경에서 만들수 있습니다. 이 색상 정보는 위치 추정을 위해 사용된 것이 아니라 구조적 요소의 위치 추정치와 택스처는 2차원 지도로부터 얻었고, 로봇의 지역 좌표계에 상대적인 위방향 레이저와 파노라마 카메라의 위치 정보를 사용하였습니다.
14.7 정리
이번 장에서는 자세에 대한 최대 우도 추정과 사후확률 추정 기술들을 이용한 다양한 알고리즘들을 살펴보았습니다.
- 우선 지도가 커지는 경우를 위한 고속 최대 우도 알고리즘을 소개하였습니다.
- 이 알고리즘을 구현하기 위해 경사하강법을 사용하였는데, 인지 모델과 동작 모델에 대한 로그 우도 공간의 미분들을 계산하는 예제 프로그램을 사용하였습니다.
- 이 방법의 강점과 단점들을 살펴보았고, 증분 방법은 사이클이 존재하는 환경에서 좋은 지도를 만드는데 실패하였습니다.
- 로봇 자세에 대한 완전 사후확률 추정기를 반영한 확장 알고리즘을 살펴보았는데, 이를 통해 어떻게 사이클이 존재하는 환경에서 불일관성 문제를 처리하는지 사후 확률 추정기를 사용하여 보았습니다.
- 예제들을 통해 이 알고리즘의 강인함을 살펴보았으나, 중첩된 사이클까지 다루기는 어렵습니다.
- 다중 로봇 지도 작성의 경우를 살펴보았습니다.
- 마지막으로 위 방향 레이저 거리계와 파노라마 카메라가 장착된 로봇을 사용하여 3차원 지도를 생성하는 예제들을 살펴보았습니다.
14.4 증분 지도 작성과 사후확률 추정 incremental mapping with posterior estimation
이러한 사이클 문제를 극복하려면 과거 자세 추정치 개선이 필요합니다. 이는 데이터가 기억되어야 하는데, 에를들자면 우리가 시간에 대해 각점에서 단일 점유 격자 값만 유지한다면 사이클이 닫힐때 발견되는 불일관성을 따라 그리드를 바꿀수가 없습니다. 그래서 자세가 회상하여 고칠수 있도록 과거의 센서 관측값과 추정 자세를 기억하는게 필수입니다.
14.4.1 사이클 감지
다음으로 살펴볼 매카니즘은 사이클 감지하는것인데, 로봇의 자세에 대한 완전 사후확률을 수행하는 두번째 추정기를 소개하면서 살펴보겠습니다. 사후확률 자세 추정은 위치 추정과 동일합니다. 로봇 위치 추정기는 이미 2~ 7장사이의 내용들로 넓게 살펴봤는데, 이 기본 개념 방정식을 이용해서 다시 간단히 정리하면 다음과 같습니다.
여기서 $x^{(t)}$는 시간 t에서 로봇의 자세가 됩니다. 자세가 주어질때 사후확률 추정하는 알고리즘은 표 7.1에서 볼수 있습니다.
위치 추정을 구현할때 고려해야할것은 이미 로봇의 위치를 추정하는 증분 지도 추정기와 함께 동시적으로 수행되어야 합니다. 최소한 최대 우도 추정기는 비사이클 상황에서 일관성을 가져야만 합니다. 그래서 P($x^{t}$ | $a^{(t-1)}$, $x^{(t-1)}$, m)은 로봇 동작 모델이 아닙니다. 대신 이것은 최대 우도 추정기로 구한 잔여 에러 모델이며, 일관되게 하기 위해서 P($x^{t}$ | $a^{(t-1)}$, $x^{(t-1)}$, m)의 모드는 식 14.13의 최대 우도 자세 추정기의 결과는 같아야 합니다.
조건부 확률의 불확실성은 이 추정의 사후확률 불확실성을 반영하며, 이 불확실성은 0평균 정규 분포를 따른다고 가정할때, 역 공분산 행렬은 식 14.13의 로그 우도 함수의 이차미분에서 복워할수 있습니다. 비슷하게 그동안 살펴본 동작 모델 중 하나를 사용하여 시도하여 동작 노이즈 파라미터를 추측할수도 있습니다. 다른 경우로 P($x^{t}$ | $a^{(t-1)}$, $x^{(t-1)}$, m)는 이 모델은 센서 관측에 의해 자세를 갱신한 후의 불호가실성을 설계하기 때문에 기존의 로봇 동작 모델보다 더 중요한 확률분포가 됩니다.
비슷하게 P($o^{t}$ | $x^{(t)}$, $m^{(t)}$)는 인지 모델이라고 할수 없는데, 이것을 인지 확률로 사용한다면 최근 센서 스캔으로 얻은 정보는 최대 우도 추정기에서 한번 사후확률 추정에서 한번 두번 사용더ㅣ어야 합니다. 그리고 로봇은 자신의 자세를 잘못 과신하게 될수 있습니다. 대신 다이클이 닫힐때, 충돌하는 센서 관측에 대한 P($o^{t}$ | $x^{(t)}$, $m^{(t)}$)를 평가하겠습니다. 이러한 과거 관측은 추적 시간과 최대 우도 위치를 유지하여 쉽게 식별할수 있습니다. 그 결과 사후확률 Bel($x^{(t)}$)는 로봇이 탐사되지 않은 공간에 이동하면서 증가하게 됩니다. 이전에 만난 지역과 다시 연결할때 인지 확률 P($o^{t}$ | $x^{(t)}$, $m^{(t)}$)에서의 결과 요소들이 감소하게 될것입니다.
14.4.2 시간 역방향 자세 교정 correcting poses backwards in time
지도 작성의 목표를 고려하면 로봇의 자세 추정치는 사후확률의 Bel($x^{(t)}$)의 모드로 바뀔 것입니다. 다른말로 하자면 14.13을 자세 추정기로 사용하는 대신 사후확률 P($x^{t}$ | $a^{(t-1)}$, $x^{(t-1)}$, m)를 사용하겠습니다. 로봇이 탐사되지 않은 지역으로 이동할 때 $argmax_{x^{(t)}}$ Bel($x^{(t)}$)는 최대 우도 추정기의 근사치와 동일할 것입니다. 하지만 사이클이 닫힐때 두 추정치는 달라질 것입니다.
지도의 일관성을 유지하기위해서 이러한 차이들은 사이클을 따라 시간 역방향으로 전파되어야 합니다. 이것은 다시 중첩 최대 우도 문제가 됩니다. 하지만 계산 효율성도 고려하여야합니다. 역방향 교정을 효율적으로 구현하기 위한 방법으로 다음의 두 페이즈 알고리즘이 있습니다.
1. 단계별 최대 우도 추정치와 사후 확률의 모드 사이의 차이는 사이클을 따라 모든 자세에 대해 선형적으로 분포합니다. 이 선형 수정 항을 계산하는것은 매우 빠르고 교정된 추정치로 닫혀야하는 곳에 위치하게 됩니다. 하지만 최적화 문제는 매우 비선형 적입니다.
2. 이후 증분 최대 우도 추정기는 시간에 대해 역방향으로 이웃이 주어질때 자세의 확률을 최대화 하도록 적용되어 집니다. 이 알고리즘은 위에서 보았던것과 비슷하나 여기서 자세하게 설명하지는 않겠습니다.
표 14.5 사이클이 존재하는 환경에서 지도 작성을 위한 증분 최대 우도 지도작성 알고리즘의 확장판
- 이 알고리즘은 단계별 최대 우도 지도를 따라 자세의 사후확률 추정치를 전달합니다.
- 사이클이 닫힐때 최대우도 추정치 s'와 사후확률의 모드 s''는 달라지고, 로봇 자세의 역방향 조정이 시작됩니다.
표 14.5는 자세에 대한 최대 우도 지도와 완전 사후확률 추정치를 동시에 유지하는 결과 알고리즘을 보여주고 있습니다. 사이클이 닫힐때 최대 우도 자세 추정과 사후확률의 모드는 달라지게 되고, 이 알고리즘의 5번째에서 감지됩니다. 6, 7번째 단계에서 시간 역방향으로 자세 교정 매카니즘이 수행 됩니다.
14.4.3 설명
그림 14.3 사후 확률 자세 추정을 위해 파티클 필터를 사용하여 사이클 환경에서 지도 작성
- 로봇을 중심으로하는 주위 점들은 시간에 따라 증가하는 사후확률 신뢰도를 의미합니다.
- 그림 (c)에서 사이클이 닫힐때, 이 지도는 고쳐져서 사후확률은 다시 작아지게 됩니다.
그림 14.3은 그림 14.2에서 본 지도를 생성하기 위해 같은 데이터들을 이 알고리즘에 적용한 예시를 보여주고 있습니다. 여기서 사후확률 추정기는 몬테카를로 위치 추정기 MCL로 구현하였습니다. 시작시 로봇은 모르는 지역에서 시작하여 샘플 집합은 퍼지게 됩니다. 이것은 로봇의 동작 모델처럼 아저 멀리 퍼지지는 않습니다. 그림 14.3a에서는 증분 최대 우도 추정기로 얻은 지도를 따라 사후확률 샘플들을 보여주고 있습니다.
그림 14.3b는 사이클이 닫히기 전 상황을 보여주고 있는데, 이 전에 로봇은 최대 우도 추정기에서 자세추정 오차가 크게 누적되고 있습니다. 하지만 이 에러는 자세의 사후 확률 추정을 파티클 셋으로 잘 잡혀져 왔습니다. 그림 14.3c는 사이클이 닫힌 후 상황을 보여주는데, 지도의 시작 부분과 관측치들이 합해지기 때문에 사후확률 추정치들이 매우 주목받고 있습니다.
최대 우도 추정기를 사용함에도 불구하고 사이클을 위한 증분 최대 우도 지도 작성 incremental ML mapping for cycle 알고리즘은 매우 강인합니다.
그림 14.4 오도메트리 없이 지도작성
- 좌측은 처리하지 않은 데이터
- 실시간으로 생성된 지도
그림 14.4는 오도메트리 데이터가 주어지지 않는 경우 지도 작성시에 이 알고리즘을 적용한 결과를 보여주고 있습니다. 결과적으로 로 데이터는 해석하기 매우 힘듭니다. 그림 14.4a는 오도메트리 정보의 부재로 인한 데이터 집합을 보여주는데 스캔들의 겹침으로 인해 사람의 눈으로 이해할수 없는 이미지가 나오게 됩니다.
우리가 구한 알고리즘을 이 데이터셋에 적용하면 그림 14.4b처럼 그래도 정확한 지도가 나오게 됩니다. 여기서 기본 알고리즘과 차이는 기울기 $\Delta_{s'}$ log P(s' | a , s)를 생략한 것으로 이것은 동작 a가 존재하지 않으므로 0으로 만들기 위해 생략하였습니다.
지도작성을 하는동안 누적되는 에러는 오도매트리 정보 만큼 크게 근사화됩니다. 하지만 사이클이 닫히면서 루프를 따라 존재하던 에러는 크게 감소하고 결과 지도는 오도메트리 정보가 주어지는 경우와 동일한 퀄리티를 가지게 됩니다. 하지만 이런 결과는ㄴ 소금 한알과 같은데, 이 방법은 레이저 스캐너 데이터로부터 자세를 추정할만큼 충분한 구조적 특성이 없는 특징없는 복도의 경우 실패하게 됩니다. 그렇지만 이 결과들은 일반적인 경우에 강인함을 보여주고 있습니다.
위에서 구한 모든 지도들은 300Mhz 팬티엄 2와 초당 60cm 이동하는 로봇으로 실시간으로 작성하였습니다. 하지만 우리가 알아야 할 것은 이러한 방법들은 중첩된 루프에서 사용할수 없고, 루프가 매우 크다면 실시간으로 사이클을 딸 ㅏ자세 추정치를 고치기는 힘들수도 있습니다.
7.6.1 대응관계를 모를때 EKF 위치 추정 EKF Localization with unknown correspondences
이전에 살펴본 EKF 위치 추정은 랜드마크의 대응관계를 알고있을때에만 사용가능하나 현실적으로 이 경우는 매우 드뭅니다. 그래서 대부분의 구현에 있어서 위치 추정을 수행하는 동안에는 랜드마크의 아이디를 구분해야 합니다. 앞으로 자주 이 대응 관계 문제를 대처하는 많은 방법들을 알아볼건데 가장 단순한 것은 최대 우도 대응관계 mamximum likelihood correspondence라 부르는 방법으로 가장 가능성이 큰 대응관계를 고르는 방법입니다.
최대 우도 기술은 대응 관계 변수에 대해 많은 가정들이 존재할때 불안정 할수 있으나 이 경우에는 그렇지는 않습니다. 잘못된 데이터 연관의 위험을 막기 위해서 두가지 기술이 필수적으로 사용되는데, 첫번째로 유일하고, 서로 햇갈리지 않도록 충분한 거리를 가지는 랜드마크를 선택하여야 합니다. 둘째는 로봇의 자세 불호가실성을 작게 유지하여야 합니다. 불행이도 이 두가지 방법들은 서로서로 반대되는데, 올바른 랜드마크를 찾음으로서 해결할 수 있습니다.
표 7.3 대응관계를 모를 때 확장 칼만 필터 위치 추정 알고리즘. 여기서 대응 관계 j(i)는 최대 우도 추정기를 통해 추정됩니다.
그렇지만 최대 우도 기술은 현실적으로 매우 중요한데, 표 7.3에서 최대 우도 추정기를 사용하는 EKF 위치 추정알고리즘을 보여주고 있습니다. 동작은 2~4번째 줄에서 갱신되는데 이는 표 7.2과 동일하며, 주요 차이점은 관측 갱신 과정이 됩니다. 우선 6 ~ 12번째 줄에서 지도에 존재하는 모든 랜드마크 k에 대해 대응 관계를 구하는데 필요한 값들을 계산합니다. 대응 관계 변수는 14번째 줄에서 이차 마할라 노비스 함수를 최소화 시켜 구하는데, 이차 마할라 노비스 거리 quadratice mahalanobis distance 함수는 지도상에 존재하는 랜드마크 $m_k$에 대해 관측 특징 벡터 $z_t^i$와 기대 관측 $\hat{z_t^k}$로 정의됩니다. 공분산은 5번째줄에 계산되어 관측 불확실성을 구성하고, 11번째 줄에 로봇의 불확실성이 관측 공간으로 사영되어 집니다. 마지막으로 EKF는 17, 18번째 줄에 가장 가능성있는 대응관계가 반영됩니다.
표 7.3은 비효율적인 알고리즘인데 6~12번째 줄사이에 랜드마크 결정을 선정시 더 개선될 수 있습니다. 대부분의 경우 로봇은 한 시간에 적은 수의 랜드마크만 볼수 있습니다.
더나아가, 이 알고리즘은 아웃라이어를 수용하라수 있도록 바꿀수 있는데, 일반적인 방법은 14번째 줄의 마할라 노비스 거리에 대한 랜드마크들을 허용하거나, 관련 확률이 임계치를 넘기는지 시험해보면 됩니다. 이는 좋은 방법인데 가우시안은 지수적으로 떨어지고 단일 아웃라이어가 자세 추정에 큰 영향을 줄수 있습니다. 현실적으로 임계치를 추가하지 않는 알고리즘은 더 불안정하게 됩니다.
우리가 움직이는 물체를 추적할때 그 물체가 전투기, 미사일이거나 크리켓을 하는 사람들일 수도 있다. 우리가 배운 필터중에 어떤 필터를 사용해야 할까? 일단 이 문제애 대한 특성을 생각해보자
- 멀티모달 multimodal : 0, 1, 또는 그이상의 물체를 동시에 추적해야한다
- 합침 occlusions : 한 물체가 다른 물체에 숨겨져서 여러개의 물체가 하나로 관측될 수 있다.
- 비선형 운동 : 비행체는 바람에 영향을 받고, 사람들을 서로서로 부딪칠수도 있습니다.
- 비선형 관측 : 라이다는 물체까지의 거리를 주지만 x, y, z 좌표계로 변환하는 가정에 제곱근이 필요하며 비선형이 됩니다.
- 비가우시안 노이즈 non-Gaussian noise : 컴퓨터 비전에서 배경 주위를 이동하는 물체가 배경의 일부분으로 인식 될수도 있습니다.
- 연속 contiunuous : 물체의 위치와 속도는 시간에 따라 연속적으로 됩니다.
- 다변수 : 위치, 속도, 회전속도 등 다양한 속성들을 추적해야합니다.
- 알수없는 동작 모델 unknown process model : 이 시스템의 동작 모델을 모르는 경우도 있습니다.
우리가 그동안 배운 필터들은 이러한 제약들을 가지고 있습니다.
- 이산 베이즈 필터 discrete bayes filter : 멀티모달로 비선형적인 측정값과 운동을 다룰수 있지만 이산적이고 단변수입니다.
- 칼만 필터 : 칼만 필터는 가우시안 노이즈를 가진 유니모달 선형 시스템의 추정에 사용되나 이 문제에 적합하지 않습니다
- 무향 칼만 핕터 : 무향 칼만 필터는 비선형, 연속, 다변수 문제를 다루지만 멀티 모달이 아니고, 합침 문제를 다룰수가 없습니다. UKF는 가우시안이아닌 노이즈도 다룰수는 있지만 매우 비선형적이거나 심한 비가우시안에 적합하지 않습니다.
- 확장 칼만 필터 : 확장 칼만필터는 UKF와 동일한 이점과 제한을 가지고 있으나 비선형과 비가우시안 노이즈에 강하게 대응할수 있습니다.
몬테카를로 샘플링 Monte Carlo Sampling
UKF 챕터에서 아래의 비선시스템의 영향을 가우시안으로 그려보았습니다.
좌측 그림은 3000개의 점이 가우시안을 따르며
오른쪽 그림은 다음의 식에따라 이 점들
결과를 구하기 위해 유한한 임의의 수의 샘플 포인트를 사용하는 방법을 몬테카를로 방법이라 합니다. 이 방법은 생각보다 간단한데, 이 문제를 나타내는 샘플들을 충분히 생성시키고, 이 점들이 시스템에 따라 이동시킵니다. 그리고 점들의 변형을 결과로 계산하면 됩니다.
간단히 말하면 이게 파티클 필터의 동작이 됩니다. 베이지안 필터 알고리즘을 각각이 시스템의 가능한 상태를 나타내는 파티클 수 천개에 적용되고, 파티클의 통계적인 가중치를 이용하여 수천개의 파티클로부터 추정 상태를 구하게 됩니다.
일반적인 파티클 필터 알고리즘 Generic Particle Filter Algorithm
1. 임의의 파티클 집합을 생성
파티클은 위치와 방위 그리고 필요한 추가적인 상태들을 가지고 있습니다. 각각의 파티클들은 가중치(확률)도 가지고 있는 이는 실제 시스템의 상태와 얼마나 일치하는지를 의미합니다. 처음에는 모든 파티클들을 같은 가중치로 초기화 시킵니다.
2. 파티클의 다음 상태를 예측
실제 시스템의 운동처럼 파티클을 이동시킵니다.
3. 갱신
관측 정보를 기반으로 파티클들의 가중치를 갱신시킵니다. 실제 관측치와 가까운 파티클들은 실제 관측지와 가깝지 않은 파티클들에 비해 더 높은 가중치를 가지게 됩니다.
4. 리샘플링
신뢰할수 없는 파티클들을 버리고 가중치가 큰 파티클들로 대체시킵니다.
5. 추정치 계산
상태 추정치를 구하기 위해 파티클 집합의 평균과 공분산을 계산 합니다.
이 알고리즘은 실제 사용하는데 극복해야할 문제들을 가지고 있지만 이게 기본 개념이 됩니다. 예를 보면 이전에 UKF, EKF로 로봇의 위치 추정 문제를 다뤘던것 처럼 파티클 필터를 사용하면, 로봇은 선회와 속도 제어 입력을 가지고 각각의 랜드마크로 부터 거리를 측정하는 센서도 가지고 있습니다.. 이 센서와 입력 또한 노이즈를 가지고 있지만 우리는 이들을 이용해서 로봇의 위치를 추정해야 합니다.
몬테카를로를 이용한 확률 분포
우리가 구간이 [0, $\pi$]에서 곡선 y = $e^{sin(x)}$의 넓이를 구해야 한다고 가정해 봅시다. 넓이는 $\int_{pi}^{0}$ $e^{sin(x)} dx$를 계산하면 되므로 살펴봅시다.
만약 당신이 현명하다면 이 문제를 도전하지는 않을겁니다. y = $e^{sin(x)}$는 해석적으로 적분될수 없기 때문인데, 우리가 살고 있는 세상은 지금처럼 적분할수 없는 방정식들이 가득 합니다. 예를들면 한 물체의 밝기를 계산을 하는 경우 그 물체가 일정량의 빛을 반사할것이지만 그 반사되는 빛의 일부는 다른 물체에 부딪혀 원래 물체에 다시 부딪히게 될것입니다. 이 결과 기존의 물체의 밝기는 더 밝아지게 되는데 이는 재귀 적분 recursive integral이 됩니다.
하지만 몬테카를로 기술을 이용하면 이 적분은 다루기 쉬워집니다. 곡선의 넓이를 구하기 위해 곡선의 구간을 감싸는 박스를 만들고, 이 박스 안에 임의의 위치를 가진 점들을 생성시킨 뒤 전체 점들의 갯수와 곡선 안에 존재하는 점들의 비율을 계산하면 됩니다.
예를 들어 박스의 넓이가 1인데 점의 40%가 곡선 안에 존재한다고 하면 그 곡선의 넓이는 약 0.4가 될것입니다. 여기서 점들의 갯수가 많아질수록 더 정확해지나 수천개의 점들만 사용해도 충분히 정확한 결과를 구할 수 있습니다.
수치적으로 적분하기 힘든 함수를 다룰때 이 기술을 사용할수 있으며 적분 불가능하고 비연속적인 함수인 경우에도 사용 가능합니다.
원의 넓이를 구하여 원주율 $\pi$를 계산하여 봅시다. 우선 반지름이 1인 원을 준비하고, 그 원을 사각형 안에다가 놓읍시다. 이 사각형 변의 길이는 2로 넓이는 4가 됩니다. 이제 이 상자 안에 균일 분포를 따르는 임의의 점들을 생성하고, 원안에 점들이 얼마나 많이 존재하는지 세어봅시다. 원의 넓이는 원안의 점들의 개수/전체 점들의 개수*상자의 넓이로 구할 수 있습니다. 우리는 A = $\pi$ $r^2$임을 알고 있으니, $\pi$ = A/$r^2$인지 계산해봅시다.
우선 점들을 만들어 봅시다.
N = 20000
pts = uniform(-1, 1, (N, 2))
만약 원의 중심으로부터 거리가 반지름보다 같거나 작으면 점들은 원안에 있다고 볼수 있는데, 이 거리를 벡터의 크기를 계산하는 numpy.linalg.norm 함수로 거리를 구하겠습니다. 벡터들은 (0,0)에서 시작하므로 원점을 기준으로 점들의 거리를 계산하겠습니다.
dist = np.linalg.norm(pts, axis=1)
다음으로 이 기준에 맞는 점들을 구하기 위해 다음 조건을 주어 구해봅시다.
in_circle = dist <= 1
이를 이용해서 다음과 같이 pi를 계산할 수 있습니다.
import matplotlib.pyplot as plt
import numpy as np
from numpy.random import uniform
N = 20000 # number of points
radius = 1.
area = (2*radius)**2
pts = uniform(-1, 1, (N, 2))
# distance from (0,0)
dist = np.linalg.norm(pts, axis=1)
in_circle = dist <= 1
pts_in_circle = np.count_nonzero(in_circle)
pi = 4 * (pts_in_circle / N)
# plot results
plt.scatter(pts[in_circle,0], pts[in_circle,1],
marker=',', edgecolor='k', s=1)
plt.scatter(pts[~in_circle,0], pts[~in_circle,1],
marker=',', edgecolor='r', s=1)
plt.axis('equal')
print('mean pi(N={})= {:.4f}'.format(N, pi))
print('err pi(N={})= {:.4f}'.format(N, np.pi-pi))
