공부

Monte Carlo Robot Localization 파이썬 실습

Swimming_Kim 2018. 12. 18. 16:24

 

Copyright@2014 Udacity, Inc All Rights Reserved




지난 시간에 이러한 cyclic환경에서 움직이는 로봇에 대해서 배워 보았다. 그럼 이번에는 이를 이용한 실습을 해 보려고 한다. 우선, 가장 간소화한 시스템으로 구성해 볼 것이고, 파이썬을 사용해서 콘솔 프로그래밍을 할 것임을 밝힌다. 


Copyright@2014 Udacity, Inc All Rights Reserved


 이 그림이 기억나는가? 로봇이 빨간색임을 감지한 상황에서 로봇이 어떤 위치에 있을 확률을 구하는 부분이었다. 여기에서 Bayes' Rule을 사용해야 함도 배워 보았다. 그럼 실질적인 코딩을 위해서 이 과정을 알고리즘으로 다시 정립해 본다면,


1. 이전 위치에서의 확률에

2. 새로 감지한 색상에 따라서 확률을 각각 곱해준 뒤

3. 확률의 합이 1이 되도록 normalizing한다.


 이렇게 정리해 볼 수 있을 것이다. 그럼 이를 기반으로 한 다음과 같은 코드가 있을 때 


1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
p=[0.2, 0.2, 0.2, 0.2, 0.2]
world=['green', 'red', 'red', 'green', 'green']
Z = 'red'
pHit = 0.6
pMiss = 0.2
 
def sense(p, Z):
    q=[]
    for i in range(len(p)):
        hit = (Z == world[i])
        q.append(p[i] * (hit * pHit + (1-hit) * pMiss))
    s = sum(q)
    for i in range(len(p)):
        q[i]=q[i]/s 
    return q
    
print sense(p, Z)
cs



 hit = (Z == world[i])
q.append(p[i] * (hit * pHit + (1-hit) * pMiss))


 위 부분을 잘 살펴보자! 여기에서는 빨간색이 맞다면 -> 0.6을 곱하고 틀리다면 -> 0.2를 곱해야 한다. 이를 표현한 코딩이다.


Copyright@2014 Udacity, Inc All Rights Reserved


 그럼 이번에는 위와 같은 모션에 대한 부분을 코딩할 차례이다. 그런데 여기에서 문제가 있다. 로봇이 이동을 하게 되면 전체 확률도 그에 따라 이동을 해야 한다. 무슨 이야기인가 하면, 


[0, 1, 0, 0, 0] 의 확률을 가지고 있던 상태에서 1만큼 이동을 하면 움직임이 맞을 확률이 1이라고 가정했을 때 [0, 0, 1, 0, 0] 이렇게 확률들도 이동을 해야 한다는 것이다! 더불어서 Cyclic한 환경이기 때문에, 4번 이상 움직일 경우에 대한 예외처리도 해주어야 한다. 이를 나타내 보자면~


1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
p=[0, 1, 0, 0, 0]
world=['green', 'red', 'red', 'green', 'green']
measurements = ['red', 'green']
pHit = 0.6
pMiss = 0.2
pExact = 0.8
pOvershoot = 0.1
pUndershoot = 0.1
 
def sense(p, Z):
    q=[]
    for i in range(len(p)):
        hit = (Z == world[i])
        q.append(p[i] * (hit * pHit + (1-hit) * pMiss))
    s = sum(q)
    for i in range(len(q)):
        q[i] = q[i] / s
    return q
 
def move(p, U):
    q = []
    for i in range(len(p)):
        q.append(p[(i-U)%len(p)] * pExact)
    return q
 
print move(p, 1)
 
cs



 q.append(p[(i-U)%len(p)] * pExact) 이렇게 표현할 수 있겠다! %라는 것은 뒤에 것으로 나눈

나머지를 의미한다. 그래서 복도의 총 위치들이 5개의 다른 값을 가질 수 있어서 이렇게 표현이

가능한 것이다!


 그럼 이번에는 motion이 확실하지 않았던 경우(아래 그림)를 우리가 해주었던 작업을 코딩해보자.


Copyright@2014 Udacity, Inc All Rights Reserved


똑같은 그림이 위에 있지만 편의를 위해서 다시 올려 보았다.


1
2
3
4
5
6
7
8
def move(p, U):
    q = []
    for i in range(len(p)):
        s = pExact * p[(i-U) % len(p)]
        s = s + pOvershoot * p[(i-U-1) % len(p)]
        s = s + pUndershoot * p[(i-U+1) % len(p)]
        q.append(s)
    return q
cs

다음과 같이 1칸 더 가거나 덜 갔을 경우를 모두 생각해서 합성곱을 해 주어야 한다.

그럼 이번에는 완성된 1-D 시스템을 적용해 보자!!



1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
#Given the list motions=[1,1] which means the robot 
#moves right and then right again, compute the posterior 
#distribution if the robot first senses red, then moves 
#right one, then senses green, then moves right again, 
#starting with a uniform prior distribution.
 
p=[0.2, 0.2, 0.2, 0.2, 0.2]
world=['green', 'red', 'red', 'green', 'green']
measurements = ['red', 'green']
motions = [1,1]
pHit = 0.6
pMiss = 0.2
 
pExact = 0.8
pOvershoot = 0.1
pUndershoot = 0.1
 
def sense(p, Z):
    q=[]
    for i in range(len(p)):
        hit = (Z == world[i])
        q.append(p[i] * (hit * pHit + (1-hit) * pMiss))
    s = sum(q)
    for i in range(len(q)):
        q[i] = q[i] / s
    return q
 
def move(p, U):
    q = []
    for i in range(len(p)):
        s = pExact * p[(i-U) % len(p)]
        s = s + pOvershoot * p[(i-U-1) % len(p)]
        s = s + pUndershoot * p[(i-U+1) % len(p)]
        q.append(s)
    return q
 
for i in range(len(measurements)):
    p = sense(p, measurements[i])
    p = move(p, motions[i])
 
 
print(p)     
 
cs


참고로 measurement를 먼저 하고 나서 motion을 취한다고 한 상황이다. - 과제에서 그렇게 하란다.

다음은 assignment인데 이제는 2-D에서 이 과정을 코딩해 보라는 요청이다.

 


Copyright@2014 Udacity, Inc All Rights Reserved


예를 들면 다음과 같은 상황인 것이다. motion에 대해서는 두 가지 - up & down / left & right

로 표현을 하라고 되어 있다. 초기 시작은 모두 같은 확률을 가지고 있다고 가정한다.


Copyright@2014 Udacity, Inc All Rights Reserved


다음과 같은 상황이라면 처음에 가만히 있을 때에는 red 오른쪽으로 움직였더니 또 red라고 하는

상황에서 로봇이 지금 어디에 있을까?? 혹은 어디에 있을 확률이 가장 높을까 하는 것을 물어보는

것이다. 아마 제일 오른쪽에서 위에서 두번째 위치에 있을 것이란 것을 알 수 있다. 다만, 이것을

프로그래밍하고 이를 통해 확률적으로 이를 입증하라는 것이다.


Copyright@2014 Udacity, Inc All Rights Reserved

 그렇다. 우리가 해야 할 미션은 다음과 같다. 계속해서 녹색을 인식하였고 정지, 오른쪽, 아래, 아래,

오른쪽 이렇게 움직였을 때 로봇이 현재 어디에 있을까?? 짜잔~~~~






1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
# The function localize takes the following arguments:
#
# colors:
#        2D list, each entry either 'R' (for red cell) or 'G' (for green cell)
#
# measurements:
#        list of measurements taken by the robot, each entry either 'R' or 'G'
#
# motions:
#        list of actions taken by the robot, each entry of the form [dy,dx],
#        where dx refers to the change in the x-direction (positive meaning
#        movement to the right) and dy refers to the change in the y-direction
#        (positive meaning movement downward)
#        NOTE: the *first* coordinate is change in y; the *second* coordinate is
#              change in x
#
# sensor_right:
#        float between 0 and 1, giving the probability that any given
#        measurement is correct; the probability that the measurement is
#        incorrect is 1-sensor_right
#
# p_move:
#        float between 0 and 1, giving the probability that any given movement
#        command takes place; the probability that the movement command fails
#        (and the robot remains still) is 1-p_move; the robot will NOT overshoot
#        its destination in this exercise
#
# The function should RETURN (not just show or print) a 2D list (of the same
# dimensions as colors) that gives the probabilities that the robot occupies
# each cell in the world.
#
# Compute the probabilities by assuming the robot initially has a uniform
# probability of being in any cell.
#
# Also assume that at each step, the robot:
# 1) first makes a movement,
# 2) then takes a measurement.
#
# Motion:
#  [0,0] - stay
#  [0,1] - right
#  [0,-1] - left
#  [1,0] - down
#  [-1,0] - up
 
def normalize(p):
    s = 0.0
    for i in range(len(p)):
        s = s + sum(p[i])
 
    for i in range(len(p)):
        for j in range(len(p[0])):
            p[i][j] = p[i][j] / s
    return p
 
def sense(p, colors, measurements, sensor_right):
    q=[]
    for i in range(len(p)):
        line=[]
        for j in range(len(p[0])):
            hit = (measurements == colors[i][j]) 
            line.append(p[i][j] * (hit * sensor_right + (1-hit) * (1 - sensor_right)))
        q.append(line)
 
    q = normalize(q)
    return q
 
def move(p, motion, p_move):
    q = []
    s = 0
    for i in range(len(p)):
        line=[]
        for j in range(len(p[0])):
            s =  p_move * (p[(i - motion[0]) % len(p)][(j - motion[1]) % len(p[0])]) + (1 - p_move) * p[i][j]
            line.append(s)
        q.append(line)
 
    q = normalize(q)
    return q
 
 
def localize(colors,measurements,motions,sensor_right,p_move):
    pinit = 1.0 / float(len(colors)) / float(len(colors[0]))
    p = [[pinit for row in range(len(colors[0]))] for col in range(len(colors))]
    for i in range(len(motions)):
        p = move(p, motions[i], p_move)   
        p = sense(p, colors, measurements[i], sensor_right)  
    return p
 
def show(p):
    rows = ['[' + ','.join(map(lambda x: '{0:.5f}'.format(x),r)) + ']' for r in p]
    print('[' + ',\n '.join(rows) + ']')
    
#############################################################
# For the following test case, your output should be 
# [[0.01105, 0.02464, 0.06799, 0.04472, 0.02465],
#  [0.00715, 0.01017, 0.08696, 0.07988, 0.00935],
#  [0.00739, 0.00894, 0.11272, 0.35350, 0.04065],
#  [0.00910, 0.00715, 0.01434, 0.04313, 0.03642]]
# (within a tolerance of +/- 0.001 for each entry)
 
colors = [['R','G','G','R','R'],
          ['R','R','G','R','R'],
          ['R','R','G','G','R'],
          ['R','R','R','R','R']]
measurements = ['G','G','G','G','G']
motions = [[0,0],[0,1],[1,0],[1,0],[0,1]]
p = localize(colors,measurements,motions,sensor_right = 0.7, p_move = 0.8)
show(p) # displays your answer
 
cs


이 과제를 하면서 생긴 몇가지 이슈들이 있는데, 사실 파이썬 코딩에 대한 부분이다.

간단하게 아래에 정리를 해 본다.


    q = []
    s = 0
    for i in range(len(p)):
        line=[]
        for j in range(len(p[0])):
            s =  p_move * (p[(i - motion[0]) % len(p)][(j - motion[1]) % len(p[0])]) + (1 - p_move) * p[i][j]
            line.append(s)
        q.append(line)




이는 파이썬에서 2차원 배열을 만들어 주는 부분이다. p라는 행렬이 있을 때 행렬의 Column과 Row

은 각각 len(p[0]), len(p)이 된다. 위의 이중 for에서 볼 수 있듯이 대부분 행을 먼저 쓰고 열을 

나중에 쓰기에 for i in range(len(p)): 안에 for j in range(len(p[0])): 로 반복문을 구성하고 p[i][j]

이렇게 각 원소에 접급을 해 주었다.


더불어 2차원 배열을 만들면 계속해서 원소를 집어넣어 주는 것이 아니라 행이 끝나면 다음 행으로


넘어가 주어야 한다. 이 문제는 각 행마다 line에 임시로 저장을 해주고 line.append로 행 단위씩

append를 해주는 형식으로 해결을 하였다. 사실 간단한 것인데 왜 처음에는 이렇게 오래 걸렸는지

모르겠지만 원래 코딩이 이런거라고 생각한다. 적어도 나에게는 ㅠㅠㅠ