지난 시간 계속해서 Path Planning에 대해서 배웠었는데, 실제로 그렇게 설정된 경로를 로봇이 완벽하게 갈 수는 없다. 아래 그림을 보아도, 파란 경로를 우리가 설정해 주었다고 해서 그대로 갈 수 있는 것이 아니라 실제로는 빨간 경로와 같은 움직임을 보일것이다. (로봇이 갑자기 바퀴의 각도를 90도를 틀 수는 없다.)
그럼 어떻게 이러한 smooth한 Path를 우리가 미리 만들어 줄 수 있을까?
위 사진과 같이, 우리가 만들어줄 smooth path의 요소들을 y_i라고 하였을 때, 원래 경로와의 오차를 최소화함과 동시에, smooth path의 요소들끼리의 오차도 최소화 하는 방식으로 이를 해결할 수 있다고 한다.
그럼 이 두 식을 이용해서 실제로 smoothing을 하는 실습을 해 보자. 그림에서는 3줄의 코드가 나와 있는데, 실제로 우리가 사용할 식을 첫번째와 세번째 줄이다. 둘째 줄을 변형한 것이 셋째 줄인데, 단지 바로 앞의 점과 최적화를 하지 않고 앞고 뒤를 모두 보는 방식으로 변형한 것이다. 매개변수인 a와 b는 사실 우리가 적절히 정해 주어야 하는 값들이다.
그리고 주의해야 할 점으로는 제일 처음과 마지막 점은 수정하면 안된다는 것이다. 그럼 디폴트 소스는 아래와 같다.
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 | # ----------- # User Instructions # # Define a function smooth that takes a path as its input # (with optional parameters for weight_data, weight_smooth, # and tolerance) and returns a smooth path. The first and # last points should remain unchanged. # # Smoothing should be implemented by iteratively updating # each entry in newpath until some desired level of accuracy # is reached. The update should be done according to the # gradient descent equations given in the instructor's note # below (the equations given in the video are not quite # correct). # ----------- from copy import deepcopy # thank you to EnTerr for posting this on our discussion forum def printpaths(path,newpath): for old,new in zip(path,newpath): print '['+ ', '.join('%.3f'%x for x in old) + \ '] -> ['+ ', '.join('%.3f'%x for x in new) +']' # Don't modify path inside your function. path = [[0, 0], [0, 1], [0, 2], [1, 2], [2, 2], [3, 2], [4, 2], [4, 3], [4, 4]] def smooth(path, weight_data = 0.5, weight_smooth = 0.1, tolerance = 0.000001): # Make a deep copy of path into newpath newpath = deepcopy(path) ####################### ### ENTER CODE HERE ### ####################### return newpath # Leave this line for the grader! printpaths(path,smooth(path)) | cs |
정답은 아래와 같다. 사실 식을 그냥 코드로 복사한 것에 지나지 않는다.
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 | # ----------- # User Instructions # # Define a function smooth that takes a path as its input # (with optional parameters for weight_data, weight_smooth, # and tolerance) and returns a smooth path. The first and # last points should remain unchanged. # # Smoothing should be implemented by iteratively updating # each entry in newpath until some desired level of accuracy # is reached. The update should be done according to the # gradient descent equations given in the instructor's note # below (the equations given in the video are not quite # correct). # ----------- from copy import deepcopy # thank you to EnTerr for posting this on our discussion forum def printpaths(path,newpath): for old,new in zip(path,newpath): print '['+ ', '.join('%.3f'%x for x in old) + \ '] -> ['+ ', '.join('%.3f'%x for x in new) +']' # Don't modify path inside your function. path = [[0, 0], [0, 1], [0, 2], [1, 2], [2, 2], [3, 2], [4, 2], [4, 3], [4, 4]] def smooth(path, weight_data = 0.5, weight_smooth = 0.1, tolerance = 0.000001): # Make a deep copy of path into newpath newpath = deepcopy(path) while True: for i in range(len(path) - 2): i += 1 for j in range(len(path[0])): old = newpath[i][j] newpath[i][j] += weight_data * (path[i][j] - newpath[i][j]) + \ weight_smooth * (newpath[i-1][j] + newpath[i+1][j] - 2.0 * newpath[i][j]) change = abs(newpath[i][j] - old) if change < tolerance: break return newpath # Leave this line for the grader! printpaths(path,smooth(path)) | cs |
이러면 이제, path를 smoothing 하는 것을 할 수 있게 된 것이다!!
우리 실제로 자동차 혹은 자동차 같은 로봇을 움직인다고 해 보자. 위 사진의 검정색 화살표를 따라서 움직이도록 하고 싶고 그러기 위해는 앞바퀴의 각도를 조정해 주어야 할 것이다. 그런데, 무슨 근거로 얼마나 각도를 틀게 할 것인가?
이에 대한 문제이다. 자동차의 중심과 원하는 경로 사이의 수직 거리를 Error라고 잡을 것이다. 그리고 이 Error를 최대한 작게 만드는 것이 우리의 목표이다. Error가 클수록 바퀴를 더 기울이게 할 것이다.
수식으로 나타내자면 이렇다. 지금 Error에 비례해서 앞바퀴를 틀 것이다. 그래서 이를 P(proportional) term이라 할 것이다.
갑자기 다른 예를 들어서 미안하지만, 샤워를 할 때 온도를 맞춰본 적이 있는가?? 차가워서 왼쪽으로 돌리면 잠시 지나 뜨거워서 다시 오른쪽으로 돌리게 된다. 운이 좋으면 최적의 온도를 찾지만 그렇지 않으면 계속 좌우로 반복을 해야 한다. 이러한 현상을 overshoot이라고 하고, 이를 막는 것이 절실하다. (따뜻하게 샤워를 하기 위해서라도...)
그래서 고안한 방법이 새로운 항을 추가하자는 것이다. 샤워를 할 때, 천천히 각도를 움직이면서 적절히 따뜻해 진다 싶으면, 돌리는 각도를 적게 해야 할 것이다. 이렇게 Error의 변화량도 고려해서 각도를 제어해야 한다. 이것이 D term이다.
그리고 각 term들에 대해서 각기 다른 매개변수를 사용하게 된다. 이 값들이 얼마인지에 따라서 얼마나 빨리 목표를 달성하느냐가 결정이 된다.
이정도 시점에서 실습을 해 보자. PD Controller을 만드는 실습이다.
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 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 | # ----------- # User Instructions # # Implement a PD controller by running 100 iterations # of robot motion. The steering angle should be set # by the parameter tau_p and tau_d so that: # # steering = -tau_p * CTE - tau_d * diff_CTE # where differential crosstrack error (diff_CTE) # is given by CTE(t) - CTE(t-1) # # # Only modify code at the bottom! Look for the TODO # ------------ import random import numpy as np import matplotlib.pyplot as plt # ------------------------------------------------ # # this is the Robot class # class Robot(object): def __init__(self, length=20.0): """ Creates robot and initializes location/orientation to 0, 0, 0. """ self.x = 0.0 self.y = 0.0 self.orientation = 0.0 self.length = length self.steering_noise = 0.0 self.distance_noise = 0.0 self.steering_drift = 0.0 def set(self, x, y, orientation): """ Sets a robot coordinate. """ self.x = x self.y = y self.orientation = orientation % (2.0 * np.pi) def set_noise(self, steering_noise, distance_noise): """ Sets the noise parameters. """ # makes it possible to change the noise parameters # this is often useful in particle filters self.steering_noise = steering_noise self.distance_noise = distance_noise def set_steering_drift(self, drift): """ Sets the systematical steering drift parameter """ self.steering_drift = drift def move(self, steering, distance, tolerance=0.001, max_steering_angle=np.pi / 4.0): """ steering = front wheel steering angle, limited by max_steering_angle distance = total distance driven, most be non-negative """ if steering > max_steering_angle: steering = max_steering_angle if steering < -max_steering_angle: steering = -max_steering_angle if distance < 0.0: distance = 0.0 # apply noise steering2 = random.gauss(steering, self.steering_noise) distance2 = random.gauss(distance, self.distance_noise) # apply steering drift steering2 += self.steering_drift # Execute motion turn = np.tan(steering2) * distance2 / self.length if abs(turn) < tolerance: # approximate by straight line motion self.x += distance2 * np.cos(self.orientation) self.y += distance2 * np.sin(self.orientation) self.orientation = (self.orientation + turn) % (2.0 * np.pi) else: # approximate bicycle model for motion radius = distance2 / turn cx = self.x - (np.sin(self.orientation) * radius) cy = self.y + (np.cos(self.orientation) * radius) self.orientation = (self.orientation + turn) % (2.0 * np.pi) self.x = cx + (np.sin(self.orientation) * radius) self.y = cy - (np.cos(self.orientation) * radius) def __repr__(self): return '[x=%.5f y=%.5f orient=%.5f]' % (self.x, self.y, self.orientation) ############## ADD / MODIFY CODE BELOW #################### # ------------------------------------------------------------------------ # # run - does a single control run # previous P controller def run_p(robot, tau, n=100, speed=1.0): x_trajectory = [] y_trajectory = [] for i in range(n): cte = robot.y steer = -tau * cte robot.move(steer, speed) x_trajectory.append(robot.x) y_trajectory.append(robot.y) return x_trajectory, y_trajectory robot = Robot() robot.set(0, 1, 0) def run(robot, tau_p, tau_d, n=100, speed=1.0): x_trajectory = [] y_trajectory = [] # TODO: your code here return x_trajectory, y_trajectory x_trajectory, y_trajectory = run(robot, 0.2, 3.0) n = len(x_trajectory) fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(8, 8)) ax1.plot(x_trajectory, y_trajectory, 'g', label='PD controller') ax1.plot(x_trajectory, np.zeros(n), 'r', label='reference') | cs |
로봇에 대한 클래스는 앞선 문제들을 보았다면 쉽게 이해할 수 있을 것이라고 생각한다. 답은 아래와 같다.
1 2 3 4 5 6 7 8 9 10 11 12 13 | def run(robot, tau_p, tau_d, n=100, speed=1.0): x_trajectory = [] y_trajectory = [] cte_1 = robot.y for i in range(n): cte = robot.y steer = -tau_p * cte - tau_d * (cte - cte_1) robot.move(steer, speed) x_trajectory.append(robot.x) y_trajectory.append(robot.y) cte_1 = cte # TODO: your code here return x_trajectory, y_trajectory | cs |
오, 그럼 이제 자동차를 Control 할 수 있는 것인가! 할 수는 있다. 그런데, 우리가 간과한 것이 하나 있었는데, 만약 우리의 자동차가 처음부터 일정 각도 틀어져 있었다면... 에 대한 문제이다. 사람이라면 처음 직진하면서 바로 고치겠지만, 로봇은 처음 상태로만 가면 직진인줄 알고 계속 아래와 같이 잘못 움직이게 된다. 그래서 이러한 문제를 해결하기 위한 방법으로 I(integral) term이 고안되었다.
시간이 지날수록 계속 Error가 줄어들 기미를 보이지 않는다면 초기 조건에 문제가 생겼거나 무엇인가 이를 방해하고 있다는 뜻이다. 그래서 계속해서 누적되는 오차를 제어하기 위한 방법이 I term을 추가하는 것이다.
결론적으로 이렇게 3가지 Term들이 만들어졌다. 제어의 꽃이자 기본인 PID 제어를 방금 배웠다. 이는 두고두고 쓰게 될 것이다. 그런데 여기에서 문제가 생겼다. 각 term들의 영향력을 결정하는 매개변수 t들을 어떻게 결정할 것인가!!
그에 대한 방법으로 Twiddle에 대해서 배워 보겠다.
이.... 이게 알고리즘인데... 혹시 몰라 영상도 첨부를 한다.
기본적인 개념은 이렇다. 처음에는 모든 매개변수를 0으로 맞춰서 시작을 하고, dp라는 것은 매개변수를 조정할 때 얼마나 조정해 줄 것인지에 대한 값들이다. 예를 들어서 p-term에 대한 매개변수가 3에서 가장 작았다가 4에서 다시 커진다고 하면 3-4 언저리일 것을 알 수 가 있다. 그럼 3-4 사이에서 3.1 3.2 3.3 이렇게 세분화를 해서 할 것이다. 이런 식으로 점차 자세히 Error를 최소화하는 매개변수들을 찾는 것이다.
계속해서 반복을 하고 변화시켜주는 값이 일정 양 아래로 떨어지면 그것을 쓰는 것이다. 구현을 하자면 다음과 같다.
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 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 | # ---------------- # User Instructions # # Implement twiddle as shown in the previous two videos. # Your accumulated error should be very small! # # Your twiddle function should RETURN the accumulated # error. Try adjusting the parameters p and dp to make # this error as small as possible. # # Try to get your error below 1.0e-10 with as few iterations # as possible (too many iterations will cause a timeout). # No cheating! # ------------ import random import numpy as np # ------------------------------------------------ # # this is the Robot class # class Robot(object): def __init__(self, length=20.0): """ Creates robot and initializes location/orientation to 0, 0, 0. """ self.x = 0.0 self.y = 0.0 self.orientation = 0.0 self.length = length self.steering_noise = 0.0 self.distance_noise = 0.0 self.steering_drift = 0.0 def set(self, x, y, orientation): """ Sets a robot coordinate. """ self.x = x self.y = y self.orientation = orientation % (2.0 * np.pi) def set_noise(self, steering_noise, distance_noise): """ Sets the noise parameters. """ # makes it possible to change the noise parameters # this is often useful in particle filters self.steering_noise = steering_noise self.distance_noise = distance_noise def set_steering_drift(self, drift): """ Sets the systematical steering drift parameter """ self.steering_drift = drift def move(self, steering, distance, tolerance=0.001, max_steering_angle=np.pi / 4.0): """ steering = front wheel steering angle, limited by max_steering_angle distance = total distance driven, most be non-negative """ if steering > max_steering_angle: steering = max_steering_angle if steering < -max_steering_angle: steering = -max_steering_angle if distance < 0.0: distance = 0.0 # make a new copy # res = Robot() # res.length = self.length # res.steering_noise = self.steering_noise # res.distance_noise = self.distance_noise # res.steering_drift = self.steering_drift # apply noise steering2 = random.gauss(steering, self.steering_noise) distance2 = random.gauss(distance, self.distance_noise) # apply steering drift steering2 += self.steering_drift # Execute motion turn = np.tan(steering2) * distance2 / self.length if abs(turn) < tolerance: # approximate by straight line motion self.x += distance2 * np.cos(self.orientation) self.y += distance2 * np.sin(self.orientation) self.orientation = (self.orientation + turn) % (2.0 * np.pi) else: # approximate bicycle model for motion radius = distance2 / turn cx = self.x - (np.sin(self.orientation) * radius) cy = self.y + (np.cos(self.orientation) * radius) self.orientation = (self.orientation + turn) % (2.0 * np.pi) self.x = cx + (np.sin(self.orientation) * radius) self.y = cy - (np.cos(self.orientation) * radius) def __repr__(self): return '[x=%.5f y=%.5f orient=%.5f]' % (self.x, self.y, self.orientation) ############## ADD / MODIFY CODE BELOW #################### # ------------------------------------------------------------------------ # # run - does a single control run def make_robot(): """ Resets the robot back to the initial position and drift. You'll want to call this after you call `run`. """ robot = Robot() robot.set(0.0, 1.0, 0.0) robot.set_steering_drift(10.0 / 180.0 * np.pi) return robot # NOTE: We use params instead of tau_p, tau_d, tau_i def run(robot, params, n=100, speed=1.0): x_trajectory = [] y_trajectory = [] err = 0 # TODO: your code here prev_cte = robot.y int_cte = 0 for i in range(2 * n): cte = robot.y diff_cte = cte - prev_cte int_cte += cte prev_cte = cte steer = -params[0] * cte - params[1] * diff_cte - params[2] * int_cte robot.move(steer, speed) x_trajectory.append(robot.x) y_trajectory.append(robot.y) if i >= n: err += cte ** 2 return x_trajectory, y_trajectory, err / n # Make this tolerance bigger if you are timing out! def twiddle(tol=0.2): # TODO: Add code here # Don't forget to call `make_robot` before you call `run`! p = [0.0, 0.0, 0.0] dp = [1.0, 1.0, 1.0] robot = make_robot() x_trajectory, y_trajectory, best_err = run(robot, p) it = 0 while sum(dp) > tol: # print("Iteration {}, best error = {}".format(it, best_err)) for i in range(len(p)): p[i] += dp[i] robot = make_robot() x_trajectory, y_trajectory, err = run(robot, p) if err < best_err: best_err = err dp[i] *= 1.1 else: p[i] -= 2 * dp[i] robot = make_robot() x_trajectory, y_trajectory, err = run(robot, p) if err < best_err: best_err = err dp[i] *= 1.1 else: p[i] += dp[i] dp[i] *= 0.9 it += 1 print(p, dp) return p, best_err best_p, best_p_err = twiddle() print(best_p) | cs |
감이 안온다면 그냥 위 코드를 돌려보고 훑어보기를 바란다.
이렇게 해서 PID Controller && 매개변수를 조정하는 방법인 Twiddle까지 배워 보았다. 그런데 사실 매개변수의 값들이 정확히 어떠한 역할을 하는지 아직 감이 오지 않을 것 같아서 이에 대한 예제를 보려고 한다.
각 매개변수가 0일때, 어떠한 오류가 발생할 것인가에 대한 이야기이다.
각 상황들에 대해서 살펴보자. 제일 위 경우는 Error가 점차 커지고 있고 목표를 지나쳐 버리고 있다. 온수를 맞추는 상황이라고 하면, 차가워서 왼쪽으로 돌렸는데 뜨거워서 다시 오른쪽으로 돌리고, 또 차가우니까 왼쪽으로 돌리는데, 화가 나서 점차 더 큰 각도로 돌리는 것과 비슷하다고 볼 수 있을 것이다. 온도의 변화량을 무시하고 화가 나니까 그냥 돌려 버렸다. 변화량에 대한 term, D가 빠져 있다.
2번 경우는 문제가 없으므로 패스
3번 경우는 점차 Error가 줄기는 하는데, 어느 정도 이상 줄지 않고 있다. 이는 아마도 초기 조건을 잘못 알고 있거나, 로봇이 어떠한 요인에 의해서 원하는 대로 움직이지 않고 있어서일 것이다. 오차가 누적되는 것을 막기 위해서는?? I-term이 필요했다.
4번은 엄청난 오차에, 심지어 무자비하게 벗어나고 있다. 제일 중요한 P-term이 없다. 이렇게, 각 term이 없으면 어떻게 되는지 알아 보았다. 그럼, 앞서 배웠던 Smoothing과 PID-Controller를 합해 보는 실습을 해 보자.
우선, 우리는 이런 직사각형 경로를 돌아다니게 하고 싶다. 로봇이 이렇게 해병대처럼 직!각!으로 움직일 수는 없으므로 이 경로를 Smoothing하는 것이 첫 번째 과제이다.
이런 식으로 달걀형의 경로가 나올 것임을 알 수가 있다. 그럼 문제 나간다.
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 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 | # ------------- # User Instructions # # Here you will be implementing a cyclic smoothing # algorithm. This algorithm should not fix the end # points (as you did in the unit quizzes). You # should use the gradient descent equations that # you used previously. # # Your function should return the newpath that it # calculates. # # Feel free to use the provided solution_check function # to test your code. You can find it at the bottom. # # -------------- # Testing Instructions # # To test your code, call the solution_check function with # two arguments. The first argument should be the result of your # smooth function. The second should be the corresponding answer. # For example, calling # # solution_check(smooth(testpath1), answer1) # # should return True if your answer is correct and False if # it is not. from math import * # Do not modify path inside your function. path=[[0, 0], [1, 0], [2, 0], [3, 0], [4, 0], [5, 0], [6, 0], [6, 1], [6, 2], [6, 3], [5, 3], [4, 3], [3, 3], [2, 3], [1, 3], [0, 3], [0, 2], [0, 1]] ############# ONLY ENTER CODE BELOW THIS LINE ########## # ------------------------------------------------ # smooth coordinates # If your code is timing out, make the tolerance parameter # larger to decrease run time. # def smooth(path, weight_data = 0.1, weight_smooth = 0.1, tolerance = 0.00001): # # Enter code here # return newpath # thank you - EnTerr - for posting this on our discussion forum #newpath = smooth(path) #for i in range(len(path)): # print '['+ ', '.join('%.3f'%x for x in path[i]) +'] -> ['+ ', '.join('%.3f'%x for x in newpath[i]) +']' ##### TESTING ###### # -------------------------------------------------- # check if two numbers are 'close enough,'used in # solution_check function. # def close_enough(user_answer, true_answer, epsilon = 0.001): if abs(user_answer - true_answer) > epsilon: return False return True # -------------------------------------------------- # check your solution against our reference solution for # a variety of test cases (given below) # def solution_check(newpath, answer): if type(newpath) != type(answer): print "Error. You do not return a list." return False if len(newpath) != len(answer): print 'Error. Your newpath is not the correct length.' return False if len(newpath[0]) != len(answer[0]): print 'Error. Your entries do not contain an (x, y) coordinate pair.' return False for i in range(len(newpath)): for j in range(len(newpath[0])): if not close_enough(newpath[i][j], answer[i][j]): print 'Error, at least one of your entries is not correct.' return False print "Test case correct!" return True # -------------- # Testing Instructions # # To test your code, call the solution_check function with # two arguments. The first argument should be the result of your # smooth function. The second should be the corresponding answer. # For example, calling # # solution_check(smooth(testpath1), answer1) # # should return True if your answer is correct and False if # it is not. testpath1 = [[0, 0], [1, 0], [2, 0], [3, 0], [4, 0], [5, 0], [6, 0], [6, 1], [6, 2], [6, 3], [5, 3], [4, 3], [3, 3], [2, 3], [1, 3], [0, 3], [0, 2], [0, 1]] answer1 = [[0.4705860385182691, 0.4235279620576893], [1.1764695730296597, 0.16470408411716733], [2.058823799247812, 0.07058633859438503], [3.000001503542886, 0.04705708651959327], [3.9411790099468273, 0.07058689299792453], [4.8235326678889345, 0.16470511854183797], [5.529415336860586, 0.4235293374365447], [5.76470933698621, 1.1058829941330384], [5.764708805535902, 1.8941189433780983], [5.5294138118186265, 2.5764724018811056], [4.823530348360371, 2.835296251305122], [3.941176199414957, 2.929413985845729], [2.9999985709076413, 2.952943245204772], [2.0588211310939526, 2.9294134622132018], [1.1764675231284938, 2.8352952720424938], [0.4705848811030855, 2.5764710948028178], [0.23529088056307781, 1.8941174802285707], [0.23529138316655338, 1.1058815684272394]] testpath2 = [[1, 0], # Move in the shape of a plus sign [2, 0], [2, 1], [3, 1], [3, 2], [2, 2], [2, 3], [1, 3], [1, 2], [0, 2], [0, 1], [1, 1]] answer2 = [[1.2222234770374059, 0.4444422843711052], [1.7777807251383388, 0.4444432993123497], [2.111114925633848, 0.8888894279539462], [2.5555592020540376, 1.2222246475393077], [2.5555580686154244, 1.7777817817879298], [2.111111849558437, 2.1111159707965514], [1.7777765871460525, 2.55556033483712], [1.2222194640861452, 2.5555593592828543], [0.8888853322565222, 2.111113321684573], [0.44444105139827167, 1.777778212019149], [0.44444210978390364, 1.2222211690821811], [0.8888882042812255, 0.8888870211766268]] # solution_check(smooth(testpath1), answer1) # solution_check(smooth(testpath2), answer2) | cs |
기존 문제와 다른 점이 있다면 경로가 closed(원형이니까 시작 지점이 곧 끝 지점이다.)되어 있으므로, 처음과 마지막 점을 고정시킬 필요가 없다는 점이다. 정답은 아래와 같다.
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 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 | # ------------- # User Instructions # # Here you will be implementing a cyclic smoothing # algorithm. This algorithm should not fix the end # points (as you did in the unit quizzes). You # should use the gradient descent equations that # you used previously. # # Your function should return the newpath that it # calculates. # # Feel free to use the provided solution_check function # to test your code. You can find it at the bottom. # # -------------- # Testing Instructions # # To test your code, call the solution_check function with # two arguments. The first argument should be the result of your # smooth function. The second should be the corresponding answer. # For example, calling # # solution_check(smooth(testpath1), answer1) # # should return True if your answer is correct and False if # it is not. from math import * from copy import deepcopy # Do not modify path inside your function. path=[[0, 0], [1, 0], [2, 0], [3, 0], [4, 0], [5, 0], [6, 0], [6, 1], [6, 2], [6, 3], [5, 3], [4, 3], [3, 3], [2, 3], [1, 3], [0, 3], [0, 2], [0, 1]] ############# ONLY ENTER CODE BELOW THIS LINE ########## # ------------------------------------------------ # smooth coordinates # If your code is timing out, make the tolerance parameter # larger to decrease run time. # def smooth(path, weight_data = 0.1, weight_smooth = 0.1, tolerance = 0.00001): newpath = deepcopy(path) while True: for i in range(len(path)): for j in range(len(path[0])): old = newpath[i][j] newpath[i][j] += weight_data * (path[i][j] - newpath[i][j]) + \ weight_smooth * (newpath[i-1][j] + newpath[(i+1) % len(path)][j] - 2.0 * newpath[i][j]) change = abs(newpath[i][j] - old) if change < tolerance: break return newpath # thank you - EnTerr - for posting this on our discussion forum #newpath = smooth(path) #for i in range(len(path)): # print '['+ ', '.join('%.3f'%x for x in path[i]) +'] -> ['+ ', '.join('%.3f'%x for x in newpath[i]) +']' ##### TESTING ###### # -------------------------------------------------- # check if two numbers are 'close enough,'used in # solution_check function. # def close_enough(user_answer, true_answer, epsilon = 0.001): if abs(user_answer - true_answer) > epsilon: return False return True # -------------------------------------------------- # check your solution against our reference solution for # a variety of test cases (given below) # def solution_check(newpath, answer): if type(newpath) != type(answer): print("Error. You do not return a list.") return False if len(newpath) != len(answer): print ('Error. Your newpath is not the correct length.') return False if len(newpath[0]) != len(answer[0]): print ('Error. Your entries do not contain an (x, y) coordinate pair.') return False for i in range(len(newpath)): for j in range(len(newpath[0])): if not close_enough(newpath[i][j], answer[i][j]): print ('Error, at least one of your entries is not correct.') return False print ("Test case correct!") return True # -------------- # Testing Instructions # # To test your code, call the solution_check function with # two arguments. The first argument should be the result of your # smooth function. The second should be the corresponding answer. # For example, calling # # solution_check(smooth(testpath1), answer1) # # should return True if your answer is correct and False if # it is not. testpath1 = [[0, 0], [1, 0], [2, 0], [3, 0], [4, 0], [5, 0], [6, 0], [6, 1], [6, 2], [6, 3], [5, 3], [4, 3], [3, 3], [2, 3], [1, 3], [0, 3], [0, 2], [0, 1]] answer1 = [[0.4705860385182691, 0.4235279620576893], [1.1764695730296597, 0.16470408411716733], [2.058823799247812, 0.07058633859438503], [3.000001503542886, 0.04705708651959327], [3.9411790099468273, 0.07058689299792453], [4.8235326678889345, 0.16470511854183797], [5.529415336860586, 0.4235293374365447], [5.76470933698621, 1.1058829941330384], [5.764708805535902, 1.8941189433780983], [5.5294138118186265, 2.5764724018811056], [4.823530348360371, 2.835296251305122], [3.941176199414957, 2.929413985845729], [2.9999985709076413, 2.952943245204772], [2.0588211310939526, 2.9294134622132018], [1.1764675231284938, 2.8352952720424938], [0.4705848811030855, 2.5764710948028178], [0.23529088056307781, 1.8941174802285707], [0.23529138316655338, 1.1058815684272394]] testpath2 = [[1, 0], # Move in the shape of a plus sign [2, 0], [2, 1], [3, 1], [3, 2], [2, 2], [2, 3], [1, 3], [1, 2], [0, 2], [0, 1], [1, 1]] answer2 = [[1.2222234770374059, 0.4444422843711052], [1.7777807251383388, 0.4444432993123497], [2.111114925633848, 0.8888894279539462], [2.5555592020540376, 1.2222246475393077], [2.5555580686154244, 1.7777817817879298], [2.111111849558437, 2.1111159707965514], [1.7777765871460525, 2.55556033483712], [1.2222194640861452, 2.5555593592828543], [0.8888853322565222, 2.111113321684573], [0.44444105139827167, 1.777778212019149], [0.44444210978390364, 1.2222211690821811], [0.8888882042812255, 0.8888870211766268]] solution_check(smooth(testpath1), answer1) solution_check(smooth(testpath2), answer2) | cs |
너무 길어서 죄송 ㅠ 나중에 접는 방법을 배워서 다시 수정해야겠다. 뒤의 문제에서는 600줄 되는 것도 있어서 ㅠㅠ
이번 문제는 위에 대한 내용이다. 가운데 이렇게 벽이 있을 경우 최적의 경로(검은색)를 Smoothing하면 빨간색 경로가 만들어진다. 그런데 로봇이 벽을 뚫고 갈 수는 없는 상황, 이런 경우를 고려해서 Smoothing을 하기 위해서는 어떠한 방법을 써야 할까? 에 대한 실습과 방법이다.
앞선 Cyclic예제의 경우 위와 같은 경로가 만들어질 것이다. 가장자리에 장애물이 있다고 가정하고, 이 안쪽으로는 들어가지 않으면서 바깥쪽으로 돌아가는 Smoothing을 하자는 것이다.
그러려면 특정 지점을 고정시켜야 한다. 고정시키고 싶을 지점들을 fix라는 리스트로 만들어 놓은 다음, 우선 코드를 아래와 같이 수정해 놓는다.
그리고 이 고정시켜놓은 점을 기준으로 해서 바깥으로 smoothing을 하기 위해서는 smoothing을 하는 방식 자체를 아래와 같이 바꾸어야 한다.
갑자기 이러한 식이 어디서 나왔는지 의문이 생길 수 있는데, 이는 Gradient Descent라는 방법에서 파생된 공식이다. 이에 대해서 설명을 하고 넘어가자.
Gradient Descent라는 방법은 인공지능 분야에서 많이 쓰이는 방법이다. 예를 들어 우리가 언덕을 오른다고 해 보자. 언덕의 꼭대기 까지 오르고 싶다면 경사가 높아지는 방향쪽으로 계속해서 움직이면 된다. 이 정상을 가리키는 벡터가 Gradient이고, 이는 1계 미분을 통해서 알 수가 있다.
고등수학을 배웠다면, 2차 함수의 최소, 최저 지점을 찾기 위해서 1번 미분을 하고 그 값이 0이 되게 하는 x를 찾아야 한다는 것을 기억할 것이다. 우리가 smoothing을 하기 위해서 만들었던 기존의 방정식을 y에 대해서 미분을 하게 되면 아래와 같은 gradient가 나온다. 이를 최소화하는 지점에 도달하는 것이 우리의 목표이고 그래서 이 공식을 코드로 구현했던 것이다. 그럼 전체 코드를 작성하는 실습을 해 보자.
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 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 | # ------------- # User Instructions # # Now you will be incorporating fixed points into # your smoother. # # You will need to use the equations from gradient # descent AND the new equations presented in the # previous lecture to implement smoothing with # fixed points. # # Your function should return the newpath that it # calculates. # # Feel free to use the provided solution_check function # to test your code. You can find it at the bottom. # ######################## ENTER CODE BELOW HERE ######################### def smooth(path, fix, weight_data = 0.0, weight_smooth = 0.1, tolerance = 0.00001): # # Enter code here. # The weight for each of the two new equations should be 0.5 * weight_smooth # return newpath # -------------- # Testing Instructions # # To test your code, call the solution_check function with the argument smooth: # solution_check(smooth) # def solution_check(smooth, eps = 0.0001): def test_case_str(path, fix): assert( len(path) == len(fix) ) if len(path) == 0: return '[]' if len(path) == 1: s = '[' + str(path[0]) + ']' if fix[0]: s += ' #fix' return s s = '[' + str(path[0]) + ',' if fix[0]: s += ' #fix' for pt,f in zip(path[1:-1],fix[1:-1]): s += '\n ' + str(pt) + ',' if f: s += ' #fix' s += '\n ' + str(path[-1]) + ']' if fix[-1]: s += ' #fix' return s testpaths = [[[0, 0],[1, 0],[2, 0],[3, 0],[4, 0],[5, 0],[6, 0],[6, 1],[6, 2],[6, 3],[5, 3],[4, 3],[3, 3],[2, 3],[1, 3],[0, 3],[0, 2],[0, 1]], [[0, 0],[2, 0],[4, 0],[4, 2],[4, 4],[2, 4],[0, 4],[0, 2]]] testfixpts = [[1, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0], [1, 0, 1, 0, 1, 0, 1, 0]] pseudo_answers = [[[0, 0],[0.7938620981547201, -0.8311168821106101],[1.8579052986461084, -1.3834788165869276],[3.053905318597796, -1.5745863173084],[4.23141390533387, -1.3784271816058231],[5.250184859723701, -0.8264215958231558],[6, 0],[6.415150091996651, 0.9836951698796843],[6.41942442687092, 2.019512290770163],[6, 3],[5.206131365604606, 3.831104483245191],[4.142082497497067, 4.383455704596517],[2.9460804122779813, 4.5745592975708105],[1.768574219397359, 4.378404668718541],[0.7498089205417316, 3.826409771585794],[0, 3],[-0.4151464728194156, 2.016311854977891],[-0.4194207879552198, 0.9804948340550833]], [[0, 0],[2.0116767115496095, -0.7015439080661671],[4, 0],[4.701543905420104, 2.0116768147460418],[4, 4],[1.9883231877640861, 4.701543807525115],[0, 4],[-0.7015438099112995, 1.9883232808252207]]] true_answers = [[[0, 0],[0.7826068175979299, -0.6922616156406778],[1.826083356960912, -1.107599209206985],[2.999995745732953, -1.2460426422963626],[4.173909508264126, -1.1076018591282746],[5.217389489606966, -0.6922642758483151],[6, 0],[6.391305105067843, 0.969228211275216],[6.391305001845138, 2.0307762911524616],[6, 3],[5.217390488523538, 3.6922567975830876],[4.17391158149052, 4.107590195596796],[2.9999982969959467, 4.246032043344827],[1.8260854997325473, 4.107592961155283],[0.7826078838205919, 3.692259569132191],[0, 3],[-0.3913036785959153, 2.030774470796648], [-0.3913035729270973, 0.9692264531461132]], [[0, 0],[1.9999953708444873, -0.6666702980585777],[4, 0],[4.666670298058577, 2.000005101453379],[4, 4],[1.9999948985466212, 4.6666612524128],[0, 4],[-0.6666612524127998, 2.000003692691148]]] newpaths = map(lambda p: smooth(*p), zip(testpaths,testfixpts)) correct = True for path,fix,p_answer,t_answer,newpath in zip(testpaths,testfixpts, pseudo_answers,true_answers, newpaths): if type(newpath) != list: print "Error: smooth did not return a list for the path:" print test_case_str(path,fix) + '\n' correct = False continue if len(newpath) != len(path): print "Error: smooth did not return a list of the correct length for the path:" print test_case_str(path,fix) + '\n' correct = False continue good_pairs = True for newpt,pt in zip(newpath,path): if len(newpt) != len(pt): good_pairs = False break if not good_pairs: print "Error: smooth did not return a list of coordinate pairs for the path:" print test_case_str(path,fix) + '\n' correct = False continue assert( good_pairs ) # check whether to check against true or pseudo answers answer = None if abs(newpath[1][0] - t_answer[1][0]) <= eps: answer = t_answer elif abs(newpath[1][0] - p_answer[1][0]) <= eps: answer = p_answer else: print 'smooth returned an incorrect answer for the path:' print test_case_str(path,fix) + '\n' continue assert( answer is not None ) entries_match = True for p,q in zip(newpath,answer): for pi,qi in zip(p,q): if abs(pi - qi) > eps: entries_match = False break if not entries_match: break if not entries_match: print 'smooth returned an incorrect answer for the path:' print test_case_str(path,fix) + '\n' continue assert( entries_match ) if answer == t_answer: print 'smooth returned the correct answer for the path:' print test_case_str(path,fix) + '\n' elif answer == p_answer: print 'smooth returned a correct* answer for the path:' print test_case_str(path,fix) print '''*However, your answer uses the "nonsimultaneous" update method, which is not technically correct. You should modify your code so that newpath[i][j] is only updated once per iteration, or else the intermediate updates made to newpath[i][j] will affect the final answer.\n''' solution_check(smooth) | cs |
자동으로 체점을 해 주는 부분이 아래에 있는데 무시를 해도 좋다.
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 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 | # ------------- # User Instructions # # Now you will be incorporating fixed points into # your smoother. # # You will need to use the equations from gradient # descent AND the new equations presented in the # previous lecture to implement smoothing with # fixed points. # # Your function should return the newpath that it # calculates. # # Feel free to use the provided solution_check function # to test your code. You can find it at the bottom. # ######################## ENTER CODE BELOW HERE ######################### from copy import deepcopy def smooth(path, fix, weight_data = 0.0, weight_smooth = 0.1, tolerance = 0.00001): newpath = deepcopy(path) change = tolerance while change >= tolerance: change = 0 for i in range(len(path)): if not fix[i]: for j in range(len(path[0])): old = newpath[i][j] newpath[i][j] += weight_smooth * (newpath[(i-1)%len(path)][j] + newpath[(i+1)%len(path)][j] - \ 2.0 * newpath[i][j]) + \ (weight_smooth / 2.0) * (2.0 * newpath[(i-1)%len(path)][j] - \ newpath[(i-2)%len(path)][j] - newpath[i][j]) + \ (weight_smooth / 2.0) * (2.0 * newpath[(i+1)%len(path)][j] - \ newpath[(i+2)%len(path)][j] - newpath[i][j]) change = abs(newpath[i][j] - old) for i in range(len(newpath)): print(newpath[i]) return newpath # -------------- # Testing Instructions # # To test your code, call the solution_check function with the argument smooth: # solution_check(smooth) # def solution_check(smooth, eps = 0.0001): def test_case_str(path, fix): assert( len(path) == len(fix) ) if len(path) == 0: return '[]' if len(path) == 1: s = '[' + str(path[0]) + ']' if fix[0]: s += ' #fix' return s s = '[' + str(path[0]) + ',' if fix[0]: s += ' #fix' for pt,f in zip(path[1:-1],fix[1:-1]): s += '\n ' + str(pt) + ',' if f: s += ' #fix' s += '\n ' + str(path[-1]) + ']' if fix[-1]: s += ' #fix' return s testpaths = [[[0, 0],[1, 0],[2, 0],[3, 0],[4, 0],[5, 0],[6, 0],[6, 1],[6, 2],[6, 3],[5, 3],[4, 3],[3, 3],[2, 3],[1, 3],[0, 3],[0, 2],[0, 1]], [[0, 0],[2, 0],[4, 0],[4, 2],[4, 4],[2, 4],[0, 4],[0, 2]]] testfixpts = [[1, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0], [1, 0, 1, 0, 1, 0, 1, 0]] pseudo_answers = [[[0, 0],[0.7938620981547201, -0.8311168821106101],[1.8579052986461084, -1.3834788165869276],[3.053905318597796, -1.5745863173084],[4.23141390533387, -1.3784271816058231],[5.250184859723701, -0.8264215958231558],[6, 0],[6.415150091996651, 0.9836951698796843],[6.41942442687092, 2.019512290770163],[6, 3],[5.206131365604606, 3.831104483245191],[4.142082497497067, 4.383455704596517],[2.9460804122779813, 4.5745592975708105],[1.768574219397359, 4.378404668718541],[0.7498089205417316, 3.826409771585794],[0, 3],[-0.4151464728194156, 2.016311854977891],[-0.4194207879552198, 0.9804948340550833]], [[0, 0],[2.0116767115496095, -0.7015439080661671],[4, 0],[4.701543905420104, 2.0116768147460418],[4, 4],[1.9883231877640861, 4.701543807525115],[0, 4],[-0.7015438099112995, 1.9883232808252207]]] true_answers = [[[0, 0],[0.7826068175979299, -0.6922616156406778],[1.826083356960912, -1.107599209206985],[2.999995745732953, -1.2460426422963626],[4.173909508264126, -1.1076018591282746],[5.217389489606966, -0.6922642758483151],[6, 0],[6.391305105067843, 0.969228211275216],[6.391305001845138, 2.0307762911524616],[6, 3],[5.217390488523538, 3.6922567975830876],[4.17391158149052, 4.107590195596796],[2.9999982969959467, 4.246032043344827],[1.8260854997325473, 4.107592961155283],[0.7826078838205919, 3.692259569132191],[0, 3],[-0.3913036785959153, 2.030774470796648], [-0.3913035729270973, 0.9692264531461132]], [[0, 0],[1.9999953708444873, -0.6666702980585777],[4, 0],[4.666670298058577, 2.000005101453379],[4, 4],[1.9999948985466212, 4.6666612524128],[0, 4],[-0.6666612524127998, 2.000003692691148]]] newpaths = map(lambda p: smooth(*p), zip(testpaths,testfixpts)) correct = True for path,fix,p_answer,t_answer,newpath in zip(testpaths,testfixpts, pseudo_answers,true_answers, newpaths): if type(newpath) != list: print "Error: smooth did not return a list for the path:" print test_case_str(path,fix) + '\n' correct = False continue if len(newpath) != len(path): print "Error: smooth did not return a list of the correct length for the path:" print test_case_str(path,fix) + '\n' correct = False continue good_pairs = True for newpt,pt in zip(newpath,path): if len(newpt) != len(pt): good_pairs = False break if not good_pairs: print "Error: smooth did not return a list of coordinate pairs for the path:" print test_case_str(path,fix) + '\n' correct = False continue assert( good_pairs ) # check whether to check against true or pseudo answers answer = None if abs(newpath[1][0] - t_answer[1][0]) <= eps: answer = t_answer elif abs(newpath[1][0] - p_answer[1][0]) <= eps: answer = p_answer else: print 'smooth returned an incorrect answer for the path:' print test_case_str(path,fix) + '\n' continue assert( answer is not None ) entries_match = True for p,q in zip(newpath,answer): for pi,qi in zip(p,q): if abs(pi - qi) > eps: entries_match = False break if not entries_match: break if not entries_match: print 'smooth returned an incorrect answer for the path:' print test_case_str(path,fix) + '\n' continue assert( entries_match ) if answer == t_answer: print 'smooth returned the correct answer for the path:' print test_case_str(path,fix) + '\n' elif answer == p_answer: print 'smooth returned a correct* answer for the path:' print test_case_str(path,fix) print '''*However, your answer uses the "nonsimultaneous" update method, which is not technically correct. You should modify your code so that newpath[i][j] is only updated once per iteration, or else the intermediate updates made to newpath[i][j] will affect the final answer.\n''' solution_check(smooth) | cs |
공식을 그대로 코드로화한 것에 지나지 않고 그리 어렵지 않다. 그럼 이번에는 PID Controller를 보다 일반적인 경우에 사용해 보자. 아래와 같은 코스를 만들고, 이를 따라가도록 시키는 것이다!!
참고로 주의해야 할 점이 있는데, Error를 지정할 때, 자동차의 방향도 고려를 해야 한다는 것이다. 이게 무슨 말이냐 하면
좌표 상 경로보다 위에 있다고 해서 무조건 Error가 양수라는 것이 아니라 움직이고 있는 방향도 생각을 해 주어야 한다는 것이다. 그래서 결론적으로, 경로 밖이면 Error가 양수이고, 경로 안이면 Error가 음수이다. 이에 유의하면서 로봇을 움직여보라!!
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 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 | # -------------- # User Instructions # # Define a function cte in the robot class that will # compute the crosstrack error for a robot on a # racetrack with a shape as described in the video. # # You will need to base your error calculation on # the robot's location on the track. Remember that # the robot will be traveling to the right on the # upper straight segment and to the left on the lower # straight segment. # # -------------- # Grading Notes # # We will be testing your cte function directly by # calling it with different robot locations and making # sure that it returns the correct crosstrack error. from math import * import random # ------------------------------------------------ # # this is the robot class # class robot: # -------- # init: # creates robot and initializes location/orientation to 0, 0, 0 # def __init__(self, length = 20.0): self.x = 0.0 self.y = 0.0 self.orientation = 0.0 self.length = length self.steering_noise = 0.0 self.distance_noise = 0.0 self.steering_drift = 0.0 # -------- # set: # sets a robot coordinate # def set(self, new_x, new_y, new_orientation): self.x = float(new_x) self.y = float(new_y) self.orientation = float(new_orientation) % (2.0 * pi) # -------- # set_noise: # sets the noise parameters # def set_noise(self, new_s_noise, new_d_noise): # makes it possible to change the noise parameters # this is often useful in particle filters self.steering_noise = float(new_s_noise) self.distance_noise = float(new_d_noise) # -------- # set_steering_drift: # sets the systematical steering drift parameter # def set_steering_drift(self, drift): self.steering_drift = drift # -------- # move: # steering = front wheel steering angle, limited by max_steering_angle # distance = total distance driven, most be non-negative def move(self, steering, distance, tolerance = 0.001, max_steering_angle = pi / 4.0): if steering > max_steering_angle: steering = max_steering_angle if steering < -max_steering_angle: steering = -max_steering_angle if distance < 0.0: distance = 0.0 # make a new copy res = robot() res.length = self.length res.steering_noise = self.steering_noise res.distance_noise = self.distance_noise res.steering_drift = self.steering_drift # apply noise steering2 = random.gauss(steering, self.steering_noise) distance2 = random.gauss(distance, self.distance_noise) # apply steering drift steering2 += self.steering_drift # Execute motion turn = tan(steering2) * distance2 / res.length if abs(turn) < tolerance: # approximate by straight line motion res.x = self.x + (distance2 * cos(self.orientation)) res.y = self.y + (distance2 * sin(self.orientation)) res.orientation = (self.orientation + turn) % (2.0 * pi) else: # approximate bicycle model for motion radius = distance2 / turn cx = self.x - (sin(self.orientation) * radius) cy = self.y + (cos(self.orientation) * radius) res.orientation = (self.orientation + turn) % (2.0 * pi) res.x = cx + (sin(res.orientation) * radius) res.y = cy - (cos(res.orientation) * radius) return res def __repr__(self): return '[x=%.5f y=%.5f orient=%.5f]' % (self.x, self.y, self.orientation) ############## ONLY ADD / MODIFY CODE BELOW THIS LINE #################### def cte(self, radius): # # # Add code here # # return cte ############## ONLY ADD / MODIFY CODE ABOVE THIS LINE #################### # ------------------------------------------------------------------------ # # run - does a single control run. def run(params, radius, printflag = False): myrobot = robot() myrobot.set(0.0, radius, pi / 2.0) speed = 1.0 # motion distance is equal to speed (we assume time = 1) err = 0.0 int_crosstrack_error = 0.0 N = 200 crosstrack_error = myrobot.cte(radius) # You need to define the cte function! for i in range(N*2): diff_crosstrack_error = - crosstrack_error crosstrack_error = myrobot.cte(radius) diff_crosstrack_error += crosstrack_error int_crosstrack_error += crosstrack_error steer = - params[0] * crosstrack_error \ - params[1] * diff_crosstrack_error \ - params[2] * int_crosstrack_error myrobot = myrobot.move(steer, speed) if i >= N: err += crosstrack_error ** 2 if printflag: print myrobot return err / float(N) radius = 25.0 params = [10.0, 15.0, 0] err = run(params, radius, True) print '\nFinal parameters: ', params, '\n ->', err | cs |
경로를 이용해서 Error만 계산해 주면 된다. 이에 따라 steering angle만 바꾸게 되고 바꾸는 정도는 PID가 해준다. 지금까지 했던 과정을 전부 다시 할 필요는 없다는 것이다.
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 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 | # -------------- # User Instructions # # Define a function cte in the robot class that will # compute the crosstrack error for a robot on a # racetrack with a shape as described in the video. # # You will need to base your error calculation on # the robot's location on the track. Remember that # the robot will be traveling to the right on the # upper straight segment and to the left on the lower # straight segment. # # -------------- # Grading Notes # # We will be testing your cte function directly by # calling it with different robot locations and making # sure that it returns the correct crosstrack error. from math import * import random # ------------------------------------------------ # # this is the robot class # class robot: # -------- # init: # creates robot and initializes location/orientation to 0, 0, 0 # def __init__(self, length = 20.0): self.x = 0.0 self.y = 0.0 self.orientation = 0.0 self.length = length self.steering_noise = 0.0 self.distance_noise = 0.0 self.steering_drift = 0.0 # -------- # set: # sets a robot coordinate # def set(self, new_x, new_y, new_orientation): self.x = float(new_x) self.y = float(new_y) self.orientation = float(new_orientation) % (2.0 * pi) # -------- # set_noise: # sets the noise parameters # def set_noise(self, new_s_noise, new_d_noise): # makes it possible to change the noise parameters # this is often useful in particle filters self.steering_noise = float(new_s_noise) self.distance_noise = float(new_d_noise) # -------- # set_steering_drift: # sets the systematical steering drift parameter # def set_steering_drift(self, drift): self.steering_drift = drift # -------- # move: # steering = front wheel steering angle, limited by max_steering_angle # distance = total distance driven, most be non-negative def move(self, steering, distance, tolerance = 0.001, max_steering_angle = pi / 4.0): if steering > max_steering_angle: steering = max_steering_angle if steering < -max_steering_angle: steering = -max_steering_angle if distance < 0.0: distance = 0.0 # make a new copy res = robot() res.length = self.length res.steering_noise = self.steering_noise res.distance_noise = self.distance_noise res.steering_drift = self.steering_drift # apply noise steering2 = random.gauss(steering, self.steering_noise) distance2 = random.gauss(distance, self.distance_noise) # apply steering drift steering2 += self.steering_drift # Execute motion turn = tan(steering2) * distance2 / res.length if abs(turn) < tolerance: # approximate by straight line motion res.x = self.x + (distance2 * cos(self.orientation)) res.y = self.y + (distance2 * sin(self.orientation)) res.orientation = (self.orientation + turn) % (2.0 * pi) else: # approximate bicycle model for motion radius = distance2 / turn cx = self.x - (sin(self.orientation) * radius) cy = self.y + (cos(self.orientation) * radius) res.orientation = (self.orientation + turn) % (2.0 * pi) res.x = cx + (sin(res.orientation) * radius) res.y = cy - (cos(res.orientation) * radius) return res def __repr__(self): return '[x=%.5f y=%.5f orient=%.5f]' % (self.x, self.y, self.orientation) ############## ONLY ADD / MODIFY CODE BELOW THIS LINE #################### def cte(self, radius): if self.x < radius: cte = ((self.x - radius)**2 + (self.y - radius)**2)**0.5 - radius elif self.x > 3.0 * radius: cte = ((self.x - 3.0 * radius)**2 + (self.y - 1.0 * radius)**2)**0.5 - radius elif self.y > radius: cte = self.y - 2.0 * radius else: cte = -1.0 * self.y return cte ############## ONLY ADD / MODIFY CODE ABOVE THIS LINE #################### # ------------------------------------------------------------------------ # # run - does a single control run. def run(params, radius, printflag = False): myrobot = robot() myrobot.set(0.0, radius, pi / 2.0) speed = 1.0 # motion distance is equal to speed (we assume time = 1) err = 0.0 int_crosstrack_error = 0.0 N = 200 crosstrack_error = myrobot.cte(radius) # You need to define the cte function! for i in range(N*2): diff_crosstrack_error = - crosstrack_error crosstrack_error = myrobot.cte(radius) diff_crosstrack_error += crosstrack_error int_crosstrack_error += crosstrack_error steer = - params[0] * crosstrack_error \ - params[1] * diff_crosstrack_error \ - params[2] * int_crosstrack_error myrobot = myrobot.move(steer, speed) if i >= N: err += crosstrack_error ** 2 if printflag: print myrobot return err / float(N) radius = 25.0 params = [10.0, 15.0, 0] err = run(params, radius, True) print '\nFinal parameters: ', params, '\n ->', err | cs |
이 긴 코드들 사이에서 핵심은 아래 조금이다.
1 2 3 4 5 6 7 8 9 10 | def cte(self, radius): if self.x < radius: cte = ((self.x - radius)**2 + (self.y - radius)**2)**0.5 - radius elif self.x > 3.0 * radius: cte = ((self.x - 3.0 * radius)**2 + (self.y - 1.0 * radius)**2)**0.5 - radius elif self.y > radius: cte = self.y - 2.0 * radius else: cte = -1.0 * self.y return cte | cs |
이다. 반원 경로를 지나고 있다면, 그 원 중심과의 거리 - 반지름이 Error가 될 것이고, 직선 구간을 지나고 있다면, 직선과 얼마나 떨어져 있는냐가 Error일 것이다. 더불어 앞서 주의한 바와 같이 방향에 따라 Error를 잘 고려해야 하는 점도 한 번 보길 바란다.
마지막으로 Q & A 이슈들에 대해서 알아보는 시간을 갖겠다.
Q: Twiddle의 문제는??
A: 일단 Twiddle은 1-차원에서의 문제 해결 방법이다. 그러니까 쉬운 경우에 사용할 수 있다는 것이다. 문제가 좀 더 복잡해진다면 Twiddle이 해결하지 못할 수도 있다. 다만, 확실한 것은 Twiddle이 제일 쉽고 코드를 짜기에도 제일 간단하다는 것이다. 최적화 확률을 더욱 높이기 위해서 초기값을 대강 예측해서 잡아준다던가 하면 더 좋을 것이다.
Q: PID Error의 기준을 우리는 계속해서 y = 0이렇게 잡았었는데, 이 이렇게 움직이게 하는 도중에 y = 2로 바꿔 보았다고 한다. 그랬더니 중심을 잃은 것 마냥 이리저리 방황했다고 하는데, 왜 이런 방식이 불가능한 걸까?
A: Controller에서 Basin of Attraction이라는 것이 있다. 예를 들어서, 포켓볼을 친다고 할 때, 공이 구멍에 들어가기 위해서는 특정한 영역 이내로 들어가야 한다. 그 범위 안에 들기만 하면 결과는 같아진다. 이런 개념?? 이다. 예를 들기 위해서 아무렇게나 지어냈는데 ㅠ 요약하면, 우리의 Control이 성공하기 위해서는 특정한 범위 내의 초기 상태값(초기 틀어진 각도, 경로에서 떨어진 거리 등 - stable해야 한다고 불린다.)이 보장되어야 한다는 것이다. 어떤 제어 시스템을 만들 때, 실질적으로, 그리고 이론적으로 이 Basin of Attraction 내에서 작동할 수 있도록 설계해야 한다.
Q: 차의 속도는 어떻게 정하는가? 예를 들어 코너를 돌면 속도를 줄이고 해야 하는데...
A: 차가 코너를 돌면서 전복되지 않기 위한 제한 속도를 계산하고, 경로를 알고 있으므로 그 곡률 등을 고려해서 미리 감속을 해야 할 것이다. 그리고 도로의 상황(비가 오거나 눈이 오거나 했을 때)에도 대비해야 할 것이다. 윾...
Q: Twiddle은 한 번에 한 Parameter만 수정하고 있는데, 우리가 특정한 방향(감소나 증가)으로 이들을 바꾸고 싶으면, 다수의 Parameter들을 바꿔야 한다. 이런 상황이라면 어떻게 해야 할 것인가??
A: 각각의 Parameter들이 어떠한 방향으로 움직일지 알고 싶을 때 그 부분만 사용하는 Adaptive Sampling이라는 방법도 있고, Surface shape를 보고 경사를 알아내는 방법 등 많고 많은 알고리즘들이 있는데~ Twiddle은 쉽다!! 그리고 효율적이다! 그래서 알려준 거다!! 아래에 Adaptive Sampling에 대한 영상을 첨부한다.
Q: Time delay에 대해서 이를 어떻게 처리했는가??
A: time sync가 맞지 않으면 모든 것이 의미가 없어진다ㅠㅠ 그래서 predictive controller을 사용했다고 하는데, 차의 모델도 알고 각 프로세스를 거치기 위해 얼마나 시간이 걸리는지도 예측할 수 있고, 어떤 measurement가 관측이 될지 예상할 수 있다면, 전부 미리 plan을 짜고 그에 맞춰서 Control을 할 수 있을 것이다.
그리고, 우리가 바퀴의 각도를 바꾸면 바로 휙 바뀌는 것이 아니다. 외부와의 마찰도 있고 차의 무게와 중심에 따라서 관성도 다르다. 이러한 세세한 것까지 모두 고려해서 더 정교한 모델을 짜기 위해 노력했다고 한다.
Q: 차량의 타이어가 공기가 빠지고, 갑자기 외부에서 힘이 작용하고, 예기치 않은 수많은 상황들이 벌어지면 어떻게 대응하는가?
A: 사람은 수없이 많은 상황들에 대해서 임기응변을 할 수 있다. 그런데 로봇이 이들을 다루게 하기 위해서는 각 상황들에 대한 메뉴얼을 무수히 많이 주는 수밖엔 없다. PID Controller는 상당히 이런 것들에 대해서 강인한 편이지만 그래도 오차가 계속 누적되는 상황이 발생하면 어쩔 수 없이 실패할 수 밖엔 없다. 이 문제들을 풀기 위해 지금도 계속해서 노력하는 중이다.
'공부' 카테고리의 다른 글
| 백준 - 1009번 (0) | 2019.01.16 |
|---|---|
| 파이썬 - 리스트 중간 수정 (0) | 2019.01.16 |
| Search 공부했다. (2) (0) | 2019.01.05 |
| Search - 공부했다. (0) | 2019.01.05 |
| Particle Filter 공부했다 (2) (0) | 2019.01.02 |
