diff --git a/code/week-2/README.md b/code/week-2/README.md index 59901e6c..06234de0 100644 --- a/code/week-2/README.md +++ b/code/week-2/README.md @@ -7,6 +7,45 @@ ## Assignment +[HW_Week2] + + +[1] Motion model (assuming 1-D Gaussian dist) +def motion_model(position, mov, priors, map_size, stdev): + + position_prob = 0.0 + + for i in range(map_size): + position_prob += norm_pdf(position-i,stdev, mov)*priors[i] + + return position_prob + +- Motion Model 은 차량이 이동할 수 있는 모든 직전 위치의 확률과 이동에 따른 현재 위치를 예측하여 확률로 나타낸다. +- Position Prob 는 차량 위치를 예측한 확률로, 차량이 존재할 수 있는 모든 이전 위치의 확률을 Priors 로 나타낸다 +또한 이전 위치에서 현재 차량이 존재하는 위치로 이동할 수 있는 확률은 Position 으로 , 직전 위치 확률 * 현재 위치로 올 확률로 계산한다. + +[2] Observation model (assuming independent Gaussian) +def observation_model(landmarks, observations, pseudo_ranges, stdev): + + distance_prob = 1.0 + + if len(observations) == len(pseudo_ranges): + for i in range(len(pseudo_ranges)): + distance_prob *= norm_pdf(pseudo_ranges[i],observations[i],1) + else : distance_prob = 0 + + return distance_prob + +- Observations Model 은 차량 주변의 나무(LandMark)를 관측할 수 있지만, 불확실성을 가지고 있다. +관측한 나무 위치 값을 기준으로 차량이 존재할 수 있는 위치에 대한 신뢰도를 높인다. + +- 나무가 존재하여도 관측 값이 0이거나, 나무 수보다 많게 관측되면 사용할 수 없는 값으로 처리한다. +신뢰할만한 관측 결과는 (관측된 나무 위치 * 현재 차량 위치에서 나무가 관측될 확률)로 나타낼 수 있다. + +- 정확도가 점차 커지는 과정은 관측된 수만큼 반복하여 차량에서 관측한 나무 사이의 거리와 나무가 그 거리에서 보일 확률을 곱하여 누적한다. +위와 같은 반복 작업으로 계산된 위치에 대한 확률과 센서 측정에 대한 확률을 이용하여 현재 차량이 위치할 수 있는 확률을 나타낸다. + + You will complete the implementation of a simple Markov localizer by writing the following two functions in `markov_localizer.py`: * `motion_model()`: For each possible prior positions, calculate the probability that the vehicle will move to the position specified by `position` given as input. @@ -21,3 +60,4 @@ If you correctly implement the above functions, you expect to see a plot similar ![Expected Result of Markov Localization][plot] If you run the program (`main.py`) without any modification to the code, it will generate only the frame of the above plot because all probabilities returned by `motion_model()` are zero by default. + diff --git a/code/week-2/mov_localizer.py b/code/week-2/mov_localizer.py new file mode 100644 index 00000000..49a1f842 --- /dev/null +++ b/code/week-2/mov_localizer.py @@ -0,0 +1,85 @@ +from helper import norm_pdf + +# Initialize prior probabilities +# taking into account that the vehicle is initially parked +# around one of the landmarks and we do not know which. +def initialize_priors(map_size, landmarks, stdev): + # Set all probabilities to zero initially. + priors = [0.0] * map_size + # Initialize prior distribution assuming the vehicle is at + # landmark +/- 1.0 meters * stdev. + positions = [] + for p in landmarks: + start = int(p - stdev) - 1 + if start < p: + start += 1 + c = 0 + while start + c <= p + stdev: + # Gather positions to set initial probability. + positions.append(start + c) + c += 1 + # Calculate actual probability to be uniformly distributed. + prob = 1.0 / len(positions) + # Set the probability to each position. + for p in positions: + priors[p] += prob + return priors + +# Estimate pseudo range determined according to the +# given pseudo position. +def estimate_pseudo_range(landmarks, p): + pseudo_ranges = [] + # Loop over each landmark and estimate pseudo ranges + for landmark in landmarks: + dist = landmark - p + # Consider only those landmarks ahead of the vehicle. + if dist > 0: + pseudo_ranges.append(dist) + return pseudo_ranges + +# Motion model (assuming 1-D Gaussian dist) +def motion_model(position, mov, priors, map_size, stdev): + # Initialize the position's probability to zero. + position_prob = 0.0 + + for i in range(map_size): + position_prob += norm_pdf(position-i,stdev, mov)*priors[i] + + # TODO: Loop over state space for all possible prior positions, + # calculate the probability (using norm_pdf) of the vehicle + # moving to the current position from that prior. + # Multiply this probability to the prior probability of + # the vehicle "was" at that prior position. + return position_prob + +# Observation model (assuming independent Gaussian) +def observation_model(landmarks, observations, pseudo_ranges, stdev): + # Initialize the measurement's probability to one. + distance_prob = 1.0 + + if len(observations) == len(pseudo_ranges): + for i in range(len(pseudo_ranges)): + distance_prob *= norm_pdf(pseudo_ranges[i],observations[i],1) + else : distance_prob = 0 + + # TODO: Calculate the observation model probability as follows: + # (1) If we have no observations, we do not have any probability. + # (2) Having more observations than the pseudo range indicates that + # this observation is not possible at all. + # (3) Otherwise, the probability of this "observation" is the product of + # probability of observing each landmark at that distance, where + # that probability follows N(d, mu, sig) with + # d: observation distance + # mu: expected mean distance, given by pseudo_ranges + # sig: squared standard deviation of measurement + return distance_prob + +# Normalize a probability distribution so that the sum equals 1.0. +def normalize_distribution(prob_dist): + normalized = [0.0] * len(prob_dist) + total = sum(prob_dist) + for i in range(len(prob_dist)): + if (total != 0.0): + normalized[i] = prob_dist[i] / total + return normalized + diff --git a/code/week-3/EKF/kalman_filter.py b/code/week-3/EKF/kalman_filter.py index 4f2ddf29..27cb0da6 100644 --- a/code/week-3/EKF/kalman_filter.py +++ b/code/week-3/EKF/kalman_filter.py @@ -2,6 +2,7 @@ from math import sqrt from math import atan2 from tools import Jacobian +from math import pi class KalmanFilter: def __init__(self, x_in, P_in, F_in, H_in, R_in, Q_in): @@ -19,6 +20,7 @@ def predict(self): def update(self, z): S = np.dot(np.dot(self.H, self.P), self.H.T) + self.R K = np.dot(np.dot(self.P, self.H.T), np.linalg.inv(S)) + # Calculate new estimates self.x = self.x + np.dot(K, z - np.dot(self.H, self.x)) self.P = self.P - np.dot(np.dot(K, self.H), self.P) @@ -26,10 +28,25 @@ def update(self, z): def update_ekf(self, z): # TODO: Implement EKF update for radar measurements # 1. Compute Jacobian Matrix H_j + px, py, vx, vy = self.x + H_j = Jacobian(self.x) + # 2. Calculate S = H_j * P' * H_j^T + R - # 3. Calculate Kalman gain K = H_j * P' * Hj^T + R + S = np.dot(np.dot(H_j, self.P), H_j.T) + self.R + + # 3. Calculate Kalman gain K = H_j * P' * H_j^T + R + K = np.dot(np.dot(self.P, H_j.T), np.linalg.inv(S)) + # 4. Estimate y = z - h(x') + y = z - [sqrt(px*px+py*py), atan2(py,px), (px*vx+py*vy)/sqrt(px*px+py*py)] + # 5. Normalize phi so that it is between -PI and +PI + while (y[1] > pi): y[1] -= 2*pi + while (y[1] < -pi): y[1] += 2*pi + # 6. Calculate new estimates # x = x' + K * y # P = (I - K * H_j) * P + self.x = self.x + np.dot(K, y) + self.P = self.P - np.dot(np.dot(K, H_j), self.P) + diff --git a/code/week-3/README.md b/code/week-3/README.md index e78b110e..d47fafe1 100644 --- a/code/week-3/README.md +++ b/code/week-3/README.md @@ -1,5 +1,57 @@ # Week 3 - Kalman Filters, EKF and Sensor Fusion +### Assignment + +Complete the implementation of EKF with sensor fusion by writing the function `update_ekf()` in the module `kalman_filter`. Details are given in class and instructions are included in comments. + +[HW_week3] +Update Extended Kalman filter + +- EKF는 기존의 Kalman Filter와 유사하나 선형화하는 기준점을 계속 갱신한다는 특징을 가지고 있다. +EKF 업데이트는 레이더 관측을 활용한다. + + def update_ekf(self, z): + # TODO: Implement EKF update for radar measurements + # 1. Compute Jacobian Matrix H_j + px, py, vx, vy = self.x + H_j = Jacobian(self.x) + +- 자코비안(Jacobian) 행렬은 원소들이 모두 1차 미분 계수로 구성되어 있으며, 미소 영역에서 ‘비선형 변환’을 ‘선형 변환으로 근사화 시키는 것이 특징이다. +Jacobian 함수를 이용하여 상태변수 Jacobian 행렬을 계산한다. + + # 2. Calculate S = H_j * P' * H_j^T + R + S = np.dot(np.dot(H_j, self.P), H_j.T) + self.R + +- Kalman Gain "k" 를 구하기 위해 (자코비안 행렬 * 예측 공분산 P * 측정 에러 공분산 R)을 계산한다. + + # 3. Calculate Kalman gain K = H_j * P' * H_j^T + R + K = np.dot(np.dot(self.P, H_j.T), np.linalg.inv(S)) + +- Kalman Gain "K" 계산 + + # 4. Estimate y = z - h(x') + y = z - [sqrt(px*px+py*py), atan2(py,px), (px*vx+py*vy)/sqrt(px*px+py*py)] + +- 레이더 관측 변수 "z" 와 관측 상태변수 "y" 차이로 에러를 구한다. + + # 5. Normalize phi so that it is between -PI and +PI + while (y[1] > pi): y[1] -= 2*pi + while (y[1] < -pi): y[1] += 2*pi + +- 관측 변수의 단위는 라디안(rad)이고, 모든 각도를 pi : -pi 사이의 값을 가질 수 있다. +위 각도 범위를 벗어나지 않도록 조건을 생성한다. + + # 6. Calculate new estimates + # x = x' + K * y + # P = (I - K * H_j) * P + self.x = self.x + np.dot(K, y) + self.P = self.P - np.dot(np.dot(K, H_j), self.P) + +- 예측 상태변수 "x" 와 관측 에러 "y", Kalman Gain "K" 를 활용하여 새로운 예측 상태변수 "x" 를 구할 수 있다. +새로운 공분산 "P" 도 자코비안 "H_j" 와 Kalman Gain "K" 를 활용하여 계산할 수 있다. + +- 측정 에러와 새로운 예측을 반복하여 매 Sample 마다 위치 예측을 갱신한다. + --- [//]: # (Image References) @@ -48,7 +100,3 @@ The program consists of five modules: * `plot.py` creates a 2D plot comparing the ground truth against our estimation. The following figure illustrates an example: ![Testing of EKF with Sensor Fusion][EKF-results] - -### Assignment - -Complete the implementation of EKF with sensor fusion by writing the function `update_ekf()` in the module `kalman_filter`. Details are given in class and instructions are included in comments. diff --git a/code/week-4/README.md b/code/week-4/README.md index 30d32d97..cee4445d 100644 --- a/code/week-4/README.md +++ b/code/week-4/README.md @@ -8,6 +8,126 @@ ## Assignment +[HW_week4] +[1] Update Weights + + # Update the weights of each particle using a multi-variate + # Gaussian distribution. + def update_weights(self, sensor_range, std_landmark_x, std_landmark_y, + observations, map_landmarks): + import math + # TODO: For each particle, do the following: + # 1. Select the set of landmarks that are visible + # (within the sensor range) + + for p in self.particles: + visible_landmarks = [] + for id, landmark in map_landmarks.items(): + + if distance(p, landmark) < sensor_range: + visible = {'x': landmark['x'], + 'y': landmark['y'], 'id': id} + visible_landmarks.append(visible) + +- distance(p, landmark) 함수를 이용하여 map에 표시된 랜드마크와 파티클 사이의 거리를 계산한다. +- 계측된 거리가 Sensor range 보다 작으면 visible 함수에 랜드마크 id와 x, y 값을 저장한다. + + # 2. Transform each observed landmark's coordinates from the + # particle's coordinate system to the map's coordinates. + + transformed_obs = [] + for obs in observations: + t_observ = {} + t_observ['x'] = p['x'] + (obs['x'] * np.cos(p['t'])) - \ (obs['y'] * np.sin(p['t'])) + t_observ['y'] = p['y'] + (obs['x'] * np.sin(p['t'])) + \ (obs['y'] * np.cos(p['t'])) + transformed_obs.append(t_observ) + + if len(visible_landmarks) == 0: + continue + +- 계측된 랜드마크의 x, y 좌표를 Map 기준의 global 좌표로 변환시킨다. + + # 3. Associate each transformed observation to one of the + # predicted (selected in Step 1) landmark positions. + # Use self.associate() for this purpose - it receives + # the predicted landmarks and observations; and returns + # the list of landmarks by implementing the nearest-neighbour + # association algorithm. + + assoc_landmarks = self.associate(visible_landmarks, transformed_obs) + p['assoc'] = [landmark['id'] for landmark in assoc_landmarks] + +- Sensor range 내의 랜드마크와 global 좌표로 변환된 랜드마크를 매칭 시킨다. +- 가장 가까운 랜드마크를 반환하기 위해 랜드마크의 id 를 "assoc" 의 값으로 입력한다. + + # 4. Calculate probability of this set of observations based on + # a multi-variate Gaussian distribution (two variables being + # the x and y positions with means from associated positions + # and variances from std_landmark_x and std_landmark_y). + # The resulting probability is the product of probabilities + # for all the observations. + + for t, a in zip(transformed_obs, assoc_landmarks): + + Gaussian_x = norm_pdf(t['x'], a['x'], std_landmark_x) + Gaussian_y = norm_pdf(t['y'], a['y'], std_landmark_y) + + Gaussian = Gaussian_x * Gaussian_y + +- transformed 랜드마크가 해당 위치에 존재할 확률은 multi-variate Gaussian distribution 으로 계산 가능하며, +assoc의 랜드마크 좌표 x, y가 표준 편차이여야 한다. + + + # 5. Update the particle's weight by the calculated probability. + + p['w'] *= Gaussian + +- transformed 의 랜드마크에 대한 확률 값을 Gaussian 으로 구하여 최종 값이 Particle 의 Weight로 곱해진다. +- 랜드마크 매칭 및 파티클 확률을 가우시안 분포로 계산하고, Particle weight 를 업데이트한다. + +[2] Resample + + # Resample particles with replacement with probability proportional to + # their weights. + + def resample(self): + + # TODO: Select (possibly with duplicates) the set of particles + # that captures the posteior belief distribution, by + # 1. Drawing particle samples according to their weights. + + weights = [p['w'] for p in self.particles.copy()] + resampled_particles = [] + positions = (np.arange(len(weights)) + np.random.random()) / len(weights) + + idx = np.zeros(len(weights), 'i') + cum_sum = np.cumsum(weights) + + i, j = 0, 0 + while i < len(weights) and j< len(weights) : + if positions[i] < cum_sum[j]: + idx[i] = j + i += 1 + else: + j += 1 + +- 파티클 weight 에 따라 resampling 되며 가중치가 크면 여러 번, 가중치가 작으면 클 때보다는 작게 sampling 된다. +- weight idx 함수를 이용하여 다음 주기의 Particle 에 weihted sampling 하게 된다. + + # 2. Make a copy of the particle; otherwise the duplicate particles + # will not behave independently from each other - they are + # references to mutable objects in Python. + # Finally, self.particles shall contain the newly drawn set of + # particles. + + resampled_particles.append(self.particles[idx[i]]) + self.particles = resampled_particles + + # return + +- resampling 된 파티클을 복사하여 재입력한다. + + You will complete the implementation of a simple particle filter by writing the following two methods of `class ParticleFilter` defined in `particle_filter.py`: * `update_weights()`: For each particle in the sample set, calculate the probability of the set of observations based on a multi-variate Gaussian distribution. diff --git a/code/week-4/helpers.py b/code/week-4/helpers.py index 2c12c93b..325c4cd8 100644 --- a/code/week-4/helpers.py +++ b/code/week-4/helpers.py @@ -2,7 +2,14 @@ # Calculate Euclidean distance between two 2-D points. def distance(p1, p2): + dx = p1['x'] - p2['x'] dy = p1['y'] - p2['y'] d = np.sqrt(dx ** 2 + dy ** 2) return d + + +def norm_pdf(x, m, s): + + one_over_sqrt_2pi = 1 / np.sqrt(2 * np.pi) + return (one_over_sqrt_2pi / s) * np.exp(-0.5 * ((x - m) / s) ** 2) diff --git a/code/week-4/particle_filter.py b/code/week-4/particle_filter.py index b9d54f5a..663244ef 100644 --- a/code/week-4/particle_filter.py +++ b/code/week-4/particle_filter.py @@ -1,5 +1,5 @@ import numpy as np -from helpers import distance +from helpers import * class ParticleFilter: def __init__(self, num_particles): @@ -71,44 +71,86 @@ def associate(self, predicted, observations): return associations # Update the weights of each particle using a multi-variate - # Gaussian distribution. + # Gaussian distribution. def update_weights(self, sensor_range, std_landmark_x, std_landmark_y, observations, map_landmarks): # TODO: For each particle, do the following: # 1. Select the set of landmarks that are visible # (within the sensor range). + for p in self.particles: + visible_landmarks = [] + for id, landmark in map_landmarks.items(): + if distance(p, landmark) < sensor_range: + visible = {'x': landmark['x'], 'y': landmark['y'], 'id': id} + visible_landmarks.append(visible) + # 2. Transform each observed landmark's coordinates from the # particle's coordinate system to the map's coordinates. + transformed_obs = [] + for obs in observations: + t_observ = {} + t_observ['x'] = p['x'] + (obs['x'] * np.cos(p['t'])) - \ + (obs['y'] * np.sin(p['t'])) + t_observ['y'] = p['y'] + (obs['x'] * np.sin(p['t'])) + \ + (obs['y'] * np.cos(p['t'])) + transformed_obs.append(t_observ) + + if len(visible_landmarks) == 0: + continue + # 3. Associate each transformed observation to one of the # predicted (selected in Step 1) landmark positions. # Use self.associate() for this purpose - it receives # the predicted landmarks and observations; and returns # the list of landmarks by implementing the nearest-neighbour # association algorithm. + assoc_landmarks = self.associate(visible_landmarks, transformed_obs) + p['assoc'] = [landmark['id'] for landmark in assoc_landmarks] + # 4. Calculate probability of this set of observations based on # a multi-variate Gaussian distribution (two variables being # the x and y positions with means from associated positions # and variances from std_landmark_x and std_landmark_y). # The resulting probability is the product of probabilities # for all the observations. - # 5. Update the particle's weight by the calculated probability. - - pass + for t, a in zip(transformed_obs, assoc_landmarks): + Gaussian_x = norm_pdf(t['x'], a['x'], std_landmark_x) + Gaussian_y = norm_pdf(t['y'], a['y'], std_landmark_y) + Gaussian = Gaussian_x * Gaussian_y + # 5. Update the particle's weight by the calculated probability. + p['w'] *= Gaussian + # Resample particles with replacement with probability proportional to # their weights. def resample(self): - return # TODO: Select (possibly with duplicates) the set of particles # that captures the posteior belief distribution, by # 1. Drawing particle samples according to their weights. + weights = [p['w'] for p in self.particles.copy()] + resampled_particles = [] + positions = (np.arange(len(weights)) + np.random.random()) / len(weights) + + idx = np.zeros(len(weights), 'i') + cum_sum = np.cumsum(weights) + i, j = 0, 0 + while i < len(weights) and j< len(weights) : + if positions[i] < cum_sum[j]: + idx[i] = j + i += 1 + else: + j += 1 + # 2. Make a copy of the particle; otherwise the duplicate particles # will not behave independently from each other - they are # references to mutable objects in Python. - # Finally, self.particles shall contain the newly drawn set of - # particles. - - pass + # Finally, self.particles shall contain the newly drawn set of + # particles. + + resampled_particles.append(self.particles[idx[i]]) + self.particles = resampled_particles + + # return # Choose the particle with the highest weight (probability) def get_best_particle(self): diff --git a/code/week-5/README.md b/code/week-5/README.md index 048d3a5a..6a065985 100644 --- a/code/week-5/README.md +++ b/code/week-5/README.md @@ -1,3 +1,120 @@ +## Assignment + +You will complete the implementation of a simple path planning algorithm based on the dynamic programming technique demonstrated in policy.py. A template code is given by assignment.py. + +[HW_week5] + +def optimum_policy_2D(grid, init, goal, cost): + # Initialize the value function with (infeasibly) high costs. + value = np.full((4, ) + grid.shape, 999, dtype=np.int32) + # Initialize the policy function with negative (unused) values. + policy = np.full((4,) + grid.shape, -1, dtype=np.int32) + # Final path policy will be in 2D, instead of 3D. + policy2D = np.full(grid.shape, ' ') + + # Apply dynamic programming with the flag change. + change = True + while change: + change = False + # This will provide a useful iterator for the state space. + p = itertools.product( + range(grid.shape[0]), + range(grid.shape[1]), + range(len(forward)) + ) + # Compute the value function for each state and + # update policy function accordingly. + for y, x, t in p: + # Mark the final state with a special value that we will + # use in generating the final path policy. + if (y, x) == goal and value[(t, y, x)] > 0: + # TODO: implement code. + value[(t,y,x)] = 0; + policy[(t,y,x)] = -999 + change = True + + # Try to use simple arithmetic to capture state transitions. + elif grid[(y, x)] == 0: + # TODO: implement code. + for i, act in enumerate(action): + direction = (t+act) % 4 + y2, x2 = y + forward[direction][0], x + forward[direction][1] + + if 0 <= y2 < grid.shape[0] \ + and 0 <= x2 < grid.shape[1] \ + and grid[(y2, x2)] == 0: + val = value[(direction, y2, x2)] + cost[i] + + if val < value[(t, y, x)]: + value[(t, y, x)] = val + policy[(t, y, x)] = act + change = True + + # Now navigate through the policy table to generate a + # sequence of actions to take to follow the optimal path. + # TODO: implement code. + y,x,o = init + + policy_start = policy[(o, y, x)] + for i in range(len(action)): + if policy_start == action[i]: + policy_ID = action_name[i] + + policy2D[(y, x)] = policy_ID + while policy[(o, y, x)] != -999: + if policy[(o, y, x)] == action[0]: + o2 = (o - 1) % 4 # turn left + elif policy[(o, y, x)] == action[1]: + o2 = o # go straight + elif policy[(o, y, x)] == action[2]: + o2 = (o + 1) % 4 # turn right + + y, x = y + forward[o2][0], x + forward[o2][1] + o = o2 + + policy_temp = policy[(o,y,x)] + if policy_temp == -999: + policy_name = "*" + else: + for i in range(len(action)): + if policy_temp == action[i]: + policy_name = action_name[i] + + policy2D[(y,x)] = policy_name + + +- "경로 탐색 알고리즘" + +- 차량 모델에서 Heading Angle 을 고려하여 거동에 따른 Cost를 계산하여 최적화된 경로를 탐색한다. + +- 차량의 방향 "t" 이용하여 Optimal_Policy_2D 초기 위치인 start 위치에 차량의 이동 방향 정보를 갱신한다. + +- 반복하여 이동할 지역을 탐색하다 목적지에 도착하면 탐색을 종료하기 위해 value = 0, policy = -999 로 설정. + +- 이동이 가능한 지역인지 Map 범위 안에 있는지 확인한다. + +- 차량의 action은 직진,좌회전,후진,우회전(up / left / down / right) 을 의미하며 +4가지 거동에 대한 방향은 2차원 x,y 위에 존재한다. -> "t" + +- 차량 action에 따른 이동에 대한 정보는 현재 상태와 방향에서 이동된 상태와 방향으로 +갱신된 action 정보를 나타낸다 -> "act" + +- 갱신된 상태와 방향이 Map grid 내의 지점이면 action에 대한 cost를 부여해 value 값 에 더해주고, +기존의 value 와 비교하여 최적의 action으로 갱신한다. + +- 시작점에서 출발하여 차량 거동에따라 갱신된 action 정보를 policy 에 저장 하는 것을 반복하여 최적 경로를 탐색한다. +-> "Policy_ID" + +- 목표지점에 도달하지 못하거나, map grid 를 벗어나게 된다면 경로 탐색은 실패한다. + +- 좌회전 cost에 따른 결과 비교 (가중치를 크게 주면 교차로에서 직진 후 우회전으로 돌아가는 것을 확인할 수 있다.) +- cost (2, 1, 10) <-> (2, 1, 100) + + +![image](https://user-images.githubusercontent.com/80089347/117440973-b7835400-af6f-11eb-8647-cb1b3f221ae5.png) + + + # Week 5 - Path Planning & the A* Algorithm --- diff --git a/code/week-5/assignment.py b/code/week-5/assignment.py index 0811731c..a63d117f 100644 --- a/code/week-5/assignment.py +++ b/code/week-5/assignment.py @@ -71,15 +71,58 @@ def optimum_policy_2D(grid, init, goal, cost): # use in generating the final path policy. if (y, x) == goal and value[(t, y, x)] > 0: # TODO: implement code. - pass + value[(t,y,x)] = 0; + policy[(t,y,x)] = -999 + change = True + # Try to use simple arithmetic to capture state transitions. elif grid[(y, x)] == 0: # TODO: implement code. - pass + for i, act in enumerate(action): + direction = (t+act) % 4 + y2, x2 = y + forward[direction][0], x + forward[direction][1] + + if 0 <= y2 < grid.shape[0] \ + and 0 <= x2 < grid.shape[1] \ + and grid[(y2, x2)] == 0: + val = value[(direction, y2, x2)] + cost[i] + + if val < value[(t, y, x)]: + value[(t, y, x)] = val + policy[(t, y, x)] = act + change = True # Now navigate through the policy table to generate a # sequence of actions to take to follow the optimal path. # TODO: implement code. + y,x,o = init + + policy_start = policy[(o, y, x)] + for i in range(len(action)): + if policy_start == action[i]: + policy_ID = action_name[i] + policy2D[(y, x)] = policy_ID + while policy[(o, y, x)] != -999: + if policy[(o, y, x)] == action[0]: + o2 = (o - 1) % 4 # turn left + elif policy[(o, y, x)] == action[1]: + o2 = o # go straight + elif policy[(o, y, x)] == action[2]: + o2 = (o + 1) % 4 # turn right + + y, x = y + forward[o2][0], x + forward[o2][1] + o = o2 + + policy_temp = policy[(o,y,x)] + if policy_temp == -999: + policy_name = "*" + else: + for i in range(len(action)): + if policy_temp == action[i]: + policy_name = action_name[i] + + policy2D[(y,x)] = policy_name + # Return the optimum policy generated above. return policy2D diff --git a/code/week-6/BP/cost_functions.py b/code/week-6/BP/cost_functions.py index e1617388..f9de8e74 100644 --- a/code/week-6/BP/cost_functions.py +++ b/code/week-6/BP/cost_functions.py @@ -32,14 +32,34 @@ def goal_distance_cost(vehicle, trajectory, predictions, data): Cost of being out of goal lane also becomes larger as vehicle approaches the goal distance. ''' - return 0 + intended_lane_distance = vehicle.goal_lane - data[0] + final_lane_distance = vehicle.goal_lane - data[1] + goal_distance = data[2] + + if goal_distance / vehicle.goal_s > 0.4: + cost = 0.0 + elif goal_distance > 0: + cost = 1 - exp((intended_lane_distance + final_lane_distance) / (goal_distance)) + else: + cost = 1 + print("goal_distance_cost:", cost) + return cost def inefficiency_cost(vehicle, trajectory, predictions, data): ''' Cost becomes higher for trajectories with intended lane and final lane that have slower traffic. ''' - return 0 + intented_lane = data[0] + final_lane = data[1] + goal_distance = data[2] + + if goal_distance / vehicle.goal_s > 0.4: + cost = exp(-(intented_lane + final_lane)) + else: + cost = 1 - exp(-(intented_lane + final_lane)) + + return cost def calculate_cost(vehicle, trajectory, predictions): ''' diff --git a/code/week-6/BP/vehicle.py b/code/week-6/BP/vehicle.py index 0d5d157b..2b40414a 100644 --- a/code/week-6/BP/vehicle.py +++ b/code/week-6/BP/vehicle.py @@ -38,7 +38,6 @@ def choose_next_state(self, predictions): 1. successor_states(): Returns a vector of possible successor states for the finite state machine. - 2. generate_trajectory(self, state, predictions): Returns a vector of Vehicle objects representing a vehicle trajectory, given a state and predictions. @@ -47,7 +46,6 @@ def choose_next_state(self, predictions): vehicle is occupying the space to the ego vehicle's right, then there is no possible trajectory without first transitioning to another state. - 3. calculate_cost(vehicle, trajectory, predictions): Imported from cost_functions.py, computes the cost for a trajectory. @@ -58,10 +56,25 @@ def choose_next_state(self, predictions): # Note that the return value is a trajectory, where a trajectory # is a list of Vehicle objects with two elements. - return [ - Vehicle(self.lane, self.s, self.v, self.a, self.state), - Vehicle(self.lane, self.position_at(1), self.v, 0, self.state) - ] + possible_successor_states = self.successor_states() + costs = [] + for state in possible_successor_states: + print("possibles:", state) + trajectory = self.generate_trajectory(state, predictions) + print("trajectory state:", trajectory[1].state) + cost_for_state = calculate_cost(self, trajectory, predictions) + costs.append({'state': state, 'cost': cost_for_state}) + + good_state = None + min_cost = 9999999 + for c in costs: + if c['cost'] < min_cost: + min_cost = c['cost'] + good_state = c['state'] + + next_trajectory = self.generate_trajectory(good_state, predictions) + + return next_trajectory def successor_states(self): ''' diff --git a/code/week-6/GNB/classifier.py b/code/week-6/GNB/classifier.py index e453a71e..dfc3da58 100644 --- a/code/week-6/GNB/classifier.py +++ b/code/week-6/GNB/classifier.py @@ -28,23 +28,30 @@ def process_vars(self, vars): # Train the GNB using a combination of X and Y, where # X denotes the observations (here we have four variables for each) and # Y denotes the corresponding labels ("left", "keep", "right"). + def train(self, X, Y): - ''' - Collect the data and calculate mean and standard variation - for each class. Record them for later use in prediction. - ''' - # TODO: implement code. - - # Given an observation (s, s_dot, d, d_dot), predict which behaviour - # the vehicle is going to take using GNB. + self.X_arr, self.Y_arr = np.array(X), np.array(Y) + unique, cnt = np.unique(Y, return_counts=True) + self.priors = cnt / len(Y) + + self.Means = np.array([self.X_arr[np.where(self.Y_arr==i)].mean(axis=0) for i in self.classes]) + self.Standevi = np.array([self.X_arr[np.where(self.Y_arr==i)].std(axis=0) for i in self.classes]) + + return self + def predict(self, observation): - ''' - Calculate Gaussian probability for each variable based on the - mean and standard deviation calculated in the training process. - Multiply all the probabilities for variables, and then - normalize them to get conditional probabilities. - Return the label for the highest conditional probability. - ''' - # TODO: implement code. - return "keep" + probas = np.zeros(len(self.classes)) + # Calculate Gaussian probability for each variable based on the + # mean and standard deviation calculated in the training process. + for i in range(len(self.classes)): + probability = 1 + for k in range(self.X_arr.shape[1]): + # Multiply all the probabilities for variables, and then + # normalize them to get conditional probabilities. + probability *= gaussian_prob(observation[k], self.Means[i][k], self.Standevi[i][k]) + probas[i] = probability + nmz = probas / probas.sum() + + # Return the label for the highest conditional probability. + return self.classes[nmz.argmax(axis=0)] diff --git a/code/week-6/README.md b/code/week-6/README.md index 9cfbdd76..89df7b9d 100644 --- a/code/week-6/README.md +++ b/code/week-6/README.md @@ -1,5 +1,167 @@ -# Week 6 - Prediction & Behaviour Planning +## HW_week 6 GNB & BP + +# Gaussian Naive Bayes Classifier +--- + + def train(self, X, Y): + self.X_arr, self.Y_arr = np.array(X), np.array(Y) + unique, cnt = np.unique(Y, return_counts=True) + self.priors = cnt / len(Y) + + self.Means = np.array([self.X_arr[np.where(self.Y_arr==i)].mean(axis=0) for i in self.classes]) + self.Standevi = np.array([self.X_arr[np.where(self.Y_arr==i)].std(axis=0) for i in self.classes]) + + return self + + def predict(self, observation): + probas = np.zeros(len(self.classes)) + # Calculate Gaussian probability for each variable based on the + # mean and standard deviation calculated in the training process. + for i in range(len(self.classes)): + probability = 1 + for k in range(self.X_arr.shape[1]): + # Multiply all the probabilities for variables, and then + # normalize them to get conditional probabilities. + probability *= gaussian_prob(observation[k], self.Means[i][k], self.Standevi[i][k]) + + probas[i] = probability + nmz = probas / probas.sum() + + # Return the label for the highest conditional probability. + return self.classes[nmz.argmax(axis=0)] + +- Gaussian Naive Bayes Classifier 는 차량의 궤적 (좌회전 / 직진 / 우회전) 에 대한 데이터를 수집하고 평균과 표준편차를 계산하여 관측을 통해 확률이 높은 주행 경로를 예측한다. + +- 수집된 데이터를 분류하여 사전확률을 구하고 주행 state (s / d / sd_ot / d_dot)에 대한 평균과 표준편차를 계산한다. + +- 관측 데이터의 확률을 가우시안으로 계산하고 사전확률을 곱한다. + +- 주행 state에 대한 평균, 표준편차와 관측 데이터를 이용하여 가장 높은 확률의 주행경로를 예측한다. +- 100%의 정확도 기준으로 84%의 확률을 결과로 나타낸다. + + +![image](https://user-images.githubusercontent.com/80089347/117452276-86f6e680-af7e-11eb-9b7e-702cf88a0386.png) + + +# Behaviour Planning +--- + + def choose_next_state(self, predictions): + ''' + Implement the transition function code for the vehicle's + behaviour planning finite state machine, which operates based on + the cost function (defined in a separate module cost_functions.py). + INPUTS: A predictions dictionary with vehicle id keys and predicted + vehicle trajectories as values. Trajectories are a list of + Vehicle objects representing the vehicle at the current timestep + and one timestep in the future. + OUTPUT: The the best (lowest cost) trajectory corresponding to + the next ego vehicle state. + Functions that will be useful: + 1. successor_states(): + Returns a vector of possible successor states + for the finite state machine. + 2. generate_trajectory(self, state, predictions): + Returns a vector of Vehicle objects representing a + vehicle trajectory, given a state and predictions. + Note that trajectories might be empty if no possible trajectory + exists for the state; for example, if the state is LCR, but a + vehicle is occupying the space to the ego vehicle's right, + then there is no possible trajectory without first + transitioning to another state. + 3. calculate_cost(vehicle, trajectory, predictions): + Imported from cost_functions.py, computes the cost for + a trajectory. + ''' + + # TODO: implement state transition function based on the cost + # associated with each transition. + + + possible_successor_states = self.successor_states() + costs = [] + for state in possible_successor_states: + print("possibles:", state) + trajectory = self.generate_trajectory(state, predictions) + print("trajectory state:", trajectory[1].state) + cost_for_state = calculate_cost(self, trajectory, predictions) + costs.append({'state': state, 'cost': cost_for_state}) + + good_state = None + min_cost = 9999999 + for c in costs: + if c['cost'] < min_cost: + min_cost = c['cost'] + good_state = c['state'] + + next_trajectory = self.generate_trajectory(good_state, predictions) + # Note that the return value is a trajectory, where a trajectory + # is a list of Vehicle objects with two elements. + return next_trajectory + + + def goal_distance_cost(vehicle, trajectory, predictions, data): + ''' + Cost increases based on distance of intended lane (for planning a + lane change) and final lane of a trajectory. + Cost of being out of goal lane also becomes larger as vehicle approaches + the goal distance. + ''' + intended_lane_distance = vehicle.goal_lane - data[0] + final_lane_distance = vehicle.goal_lane - data[1] + goal_distance = data[2] + + if goal_distance / vehicle.goal_s > 0.4: + cost = 0.0 + elif goal_distance > 0: + cost = 1 - exp((intended_lane_distance + final_lane_distance) / (goal_distance)) + else: + cost = 1 + print("goal_distance_cost:", cost) + return cost + + def inefficiency_cost(vehicle, trajectory, predictions, data): + ''' + Cost becomes higher for trajectories with intended lane and final lane + that have slower traffic. + ''' + intented_lane = data[0] + final_lane = data[1] + goal_distance = data[2] + + if goal_distance / vehicle.goal_s > 0.4: + cost = exp(-(intented_lane + final_lane)) + else: + cost = 1 - exp(-(intented_lane + final_lane)) + # print(cost) + return cost + + +- 목표 차로와 차량의 속도에 따라 cost를 부여하여 현재 궤적을 구한다. + +- 목표 차로에 도달하기 위해 차로 변경을 하며 목표지점에 가까워 질수록 cost가 증가한다. + +- 현재 자차의 위치에 대한 상태 정보 (KL/ PLCL/ PLCR/ LCL/ LCR) 을 어떻게 정의할 것인지 결정하고, +현재 상태를 보고 이동가능한 차로가 있는지 이동 궤적을 계산한다. 계산된 궤적 중 최소 cost를 선택한다 -> "choose next state" + +- 목표 차로와 자차가 주해중인 차로 사이의 거리를 계산하여 3개의 weight를 산출하고 cost를 부여한다. +(intended / final lane) -> "goal distance cost" + +- 속도가 낮으면 목표 도달까지 시간이 지연되기 때문에 cost 를 더 부여하며, +최적의 경로 (최소 cost)는 속도가 가장 높은 차로로 주행하다가 목적지 인근에서 차로 변경을 하는 것이다. + +- 자차는 차로변경을 이용한 주행으로 목표 차로까지 도달하는데 33초가 소요되었다. + + + +![image](https://user-images.githubusercontent.com/80089347/117452946-519ec880-af7f-11eb-854c-d5af6a303f59.png) + + + + + +# Week 6 - Prediction & Behaviour Planning --- ## Assignment #1 diff --git a/code/week-7/PTG/constants.py b/code/week-7/PTG/constants.py new file mode 100644 index 00000000..573d4643 --- /dev/null +++ b/code/week-7/PTG/constants.py @@ -0,0 +1,13 @@ +N_SAMPLES = 10 +SIGMA_S = [10.0, 4.0, 2.0] # s, s_dot, s_double_dot +SIGMA_D = [1.0, 1.0, 1.0] +SIGMA_T = 2.0 + +MAX_JERK = 10 # m/s/s/s +MAX_ACCEL= 10 # m/s/s + +EXPECTED_JERK_IN_ONE_SEC = 2 # m/s/s +EXPECTED_ACC_IN_ONE_SEC = 1 # m/s + +SPEED_LIMIT = 30 +VEHICLE_RADIUS = 1.5 # model vehicle as circle to simplify collision detection diff --git a/code/week-7/PTG/cost_functions.py b/code/week-7/PTG/cost_functions.py new file mode 100644 index 00000000..47486cba --- /dev/null +++ b/code/week-7/PTG/cost_functions.py @@ -0,0 +1,137 @@ +import numpy as np +from helpers import logistic, to_equation, differentiate, \ + nearest_approach_to_any_vehicle, \ + get_f_and_N_derivatives +from constants import * + +# COST FUNCTIONS +def time_diff_cost(traj, target_vehicle, delta, T, predictions): + """ + Penalizes trajectories that span a duration which is longer or + shorter than the duration requested. + """ + _, _, t = traj + return logistic(float(abs(t - T)) / T) + +def s_diff_cost(traj, target_vehicle, delta, T, predictions): + """ + Penalizes trajectories whose s coordinate (and derivatives) + differ from the goal. + """ + s, _, T = traj + target = predictions[target_vehicle].state_in(T) + target = list(np.array(target) + np.array(delta)) + s_targ = target[:3] + S = [f(T) for f in get_f_and_N_derivatives(s, 2)] + cost = 0 + for actual, expected, sigma in zip(S, s_targ, SIGMA_S): + diff = float(abs(actual - expected)) + cost += logistic(diff / sigma) + return cost + +def d_diff_cost(traj, target_vehicle, delta, T, predictions): + """ + Penalizes trajectories whose d coordinate (and derivatives) + differ from the goal. + """ + _, d_coeffs, T = traj + + d_dot_coeffs = differentiate(d_coeffs) + d_ddot_coeffs = differentiate(d_dot_coeffs) + + d = to_equation(d_coeffs) + d_dot = to_equation(d_dot_coeffs) + d_ddot = to_equation(d_ddot_coeffs) + + D = [d(T), d_dot(T), d_ddot(T)] + + target = predictions[target_vehicle].state_in(T) + target = list(np.array(target) + np.array(delta)) + d_targ = target[3:] + cost = 0 + for actual, expected, sigma in zip(D, d_targ, SIGMA_D): + diff = np.fabs(actual - expected) + cost += logistic(diff / sigma) + return cost + +def collision_cost(traj, target_vehicle, delta, T, predictions): + """ + Binary cost function which penalizes collisions. + """ + nearest = nearest_approach_to_any_vehicle(traj, predictions) + if nearest < 2*VEHICLE_RADIUS: + return 1.0 + else: + return 0.0 + +def buffer_cost(traj, target_vehicle, delta, T, predictions): + """ + Penalizes getting close to other vehicles. + """ + nearest = nearest_approach_to_any_vehicle(traj, predictions) + return logistic(2 * VEHICLE_RADIUS / nearest) + +def efficiency_cost(traj, target_vehicle, delta, T, predictions): + """ + Rewards high average speeds. + """ + s, _, t = traj + s = to_equation(s) + avg_v = float(s(t)) / t + targ_s, _, _, _, _, _ = predictions[target_vehicle].state_in(t) + targ_v = float(targ_s) / t + return logistic(2 * float(targ_v - avg_v) / avg_v) + +def total_accel_cost(traj, target_vehicle, delta, T, predictions): + s, d, t = traj + s_dot = differentiate(s) + s_d_dot = differentiate(s_dot) + a = to_equation(s_d_dot) + total_acc = 0 + dt = float(T) / 100.0 + for i in range(100): + t = dt * i + acc = a(t) + total_acc += abs(acc*dt) + acc_per_second = total_acc / T + + return logistic(acc_per_second / EXPECTED_ACC_IN_ONE_SEC) + +def max_accel_cost(traj, target_vehicle, delta, T, predictions): + s, d, t = traj + s_dot = differentiate(s) + s_d_dot = differentiate(s_dot) + a = to_equation(s_d_dot) + all_accs = [a(float(T)/100 * i) for i in range(100)] + max_acc = max(all_accs, key=abs) + if abs(max_acc) > MAX_ACCEL: + return 1.0 + else: + return 0.0 + +def total_jerk_cost(traj, target_vehicle, delta, T, predictions): + s, d, t = traj + s_dot = differentiate(s) + s_d_dot = differentiate(s_dot) + jerk = to_equation(differentiate(s_d_dot)) + total_jerk = 0 + dt = float(T) / 100.0 + for i in range(100): + t = dt * i + j = jerk(t) + total_jerk += abs(j * dt) + jerk_per_second = total_jerk / T + return logistic(jerk_per_second / EXPECTED_JERK_IN_ONE_SEC) + +def max_jerk_cost(traj, target_vehicle, delta, T, predictions): + s, d, t = traj + s_dot = differentiate(s) + s_d_dot = differentiate(s_dot) + jerk = differentiate(s_d_dot) + jerk = to_equation(jerk) + all_jerks = [jerk(float(T) / 100 * i) for i in range(100)] + max_jerk = max(all_jerks, key=abs) + if abs(max_jerk) > MAX_JERK: + return 1 + else: + return 0 diff --git a/code/week-7/PTG/evaluate_ptg.py b/code/week-7/PTG/evaluate_ptg.py new file mode 100644 index 00000000..8f9b3549 --- /dev/null +++ b/code/week-7/PTG/evaluate_ptg.py @@ -0,0 +1,18 @@ +#!/usr/bin/env python + +from ptg import PTG +from helpers import Vehicle, show_trajectory + +def main(): + vehicle = Vehicle([0, 10, 0, 0, 0, 0]) + predictions = {0: vehicle} + target = 0 + delta = [0, 0, 0, 0, 0 ,0] + start_s = [10, 10, 0] + start_d = [4, 0, 0] + T = 5.0 + best = PTG(start_s, start_d, target, delta, T, predictions) + show_trajectory(best[0], best[1], best[2], vehicle) + +if __name__ == "__main__": + main() diff --git a/code/week-7/PTG/helpers.py b/code/week-7/PTG/helpers.py new file mode 100644 index 00000000..8822e425 --- /dev/null +++ b/code/week-7/PTG/helpers.py @@ -0,0 +1,108 @@ +from math import sqrt, exp +from matplotlib import pyplot as plt + +class Vehicle(object): + """ + Helper class. Non-ego vehicles move w/ constant acceleration + """ + def __init__(self, start): + self.start_state = start + + def state_in(self, t): + s = self.start_state[:3] + d = self.start_state[3:] + state = [ + s[0] + (s[1] * t) + s[2] * t ** 2 / 2.0, + s[1] + s[2] * t, + s[2], + d[0] + (d[1] * t) + d[2] * t ** 2 / 2.0, + d[1] + d[2] * t, + d[2], + ] + return state + +def logistic(x): + """ + A function that returns a value between 0 and 1 for x in the + range [0, infinity] and -1 to 1 for x in the range [-infinity, infinity]. + + Useful for cost functions. + """ + return 2.0 / (1 + exp(-x)) - 1.0 + +def to_equation(coefficients): + """ + Takes the coefficients of a polynomial and creates a function of + time from them. + """ + def f(t): + total = 0.0 + for i, c in enumerate(coefficients): + total += c * t ** i + return total + return f + +def differentiate(coefficients): + """ + Calculates the derivative of a polynomial and returns + the corresponding coefficients. + """ + new_cos = [] + for deg, prev_co in enumerate(coefficients[1:]): + new_cos.append((deg + 1) * prev_co) + return new_cos + +def nearest_approach_to_any_vehicle(traj, vehicles): + """ + Calculates the closest distance to any vehicle during a trajectory. + """ + closest = 999999 + for v in vehicles.values(): + d = nearest_approach(traj, v) + if d < closest: + closest = d + return closest + +def nearest_approach(traj, vehicle): + closest = 999999 + s_, d_, T = traj + s = to_equation(s_) + d = to_equation(d_) + for i in range(100): + t = float(i) / 100 * T + cur_s = s(t) + cur_d = d(t) + targ_s, _, _, targ_d, _, _ = vehicle.state_in(t) + dist = sqrt((cur_s - targ_s) ** 2 + (cur_d - targ_d) ** 2) + if dist < closest: + closest = dist + return closest + +def show_trajectory(s_coeffs, d_coeffs, T, vehicle=None): + s = to_equation(s_coeffs) + d = to_equation(d_coeffs) + X = [] + Y = [] + if vehicle: + X2 = [] + Y2 = [] + t = 0 + while t <= T + 0.01: + X.append(s(t)) + Y.append(d(t)) + if vehicle: + s_, _, _, d_, _, _ = vehicle.state_in(t) + X2.append(s_) + Y2.append(d_) + t += 0.25 + plt.scatter(X, Y, color="blue") + if vehicle: + plt.scatter(X2, Y2, color="red") + plt.show() + +def get_f_and_N_derivatives(coeffs, N=3): + functions = [to_equation(coeffs)] + for i in range(N): + coeffs = differentiate(coeffs) + functions.append(to_equation(coeffs)) + return functions diff --git a/code/week-7/PTG/ptg.py b/code/week-7/PTG/ptg.py new file mode 100644 index 00000000..c9bbcba8 --- /dev/null +++ b/code/week-7/PTG/ptg.py @@ -0,0 +1,136 @@ +import numpy as np +import random +from cost_functions import * +from constants import * + +# TODO - tweak weights to existing cost functions +WEIGHTED_COST_FUNCTIONS = [ + (time_diff_cost, 1), + (s_diff_cost, 1), + (d_diff_cost, 1), + (collision_cost, 1), + (buffer_cost, 1), + (efficiency_cost, 1), + (total_accel_cost, 1), + (max_accel_cost, 1), + (total_jerk_cost, 1), + (max_jerk_cost, 1), +] + +def PTG(start_s, start_d, target_vehicle, delta, T, predictions): + """ + Finds the best trajectory according to WEIGHTED_COST_FUNCTIONS (global). + + arguments: + start_s - [s, s_dot, s_ddot] + + start_d - [d, d_dot, d_ddot] + + target_vehicle - id of leading vehicle (int) which can be used to retrieve + that vehicle from the "predictions" dictionary. This is the vehicle that + we are setting our trajectory relative to. + + delta - a length 6 array indicating the offset we are aiming for + between us and the target_vehicle. So if at time 5 the target vehicle + will be at [100, 10, 0, 0, 0, 0] and delta is [-10, 0, 0, 4, 0, 0], + then our goal state for t = 5 will be [90, 10, 0, 4, 0, 0]. + This would correspond to a goal of "follow 10 meters behind and + 4 meters to the right of target vehicle" + + T - the desired time at which we will be at the goal + (relative to now as t=0) + + predictions - dictionary of {v_id : vehicle }. Each vehicle has a method + vehicle.state_in(time) which returns a length 6 array giving + that vehicle's expected [s, s_dot, s_ddot, d, d_dot, d_ddot] state + at that time. + + return: + (best_s, best_d, best_t) where best_s are the 6 coefficients + representing s(t) best_d gives coefficients for d(t) and + best_t gives duration associated with this trajectory. + """ + target = predictions[target_vehicle] + # generate alternative goals + all_goals = [] + timestep = 0.5 + t = T - 4 * timestep + while t <= T + 4 * timestep: + target_state = np.array(target.state_in(t)) + np.array(delta) + goal_s = target_state[:3] + goal_d = target_state[3:] + goals = [(goal_s, goal_d, t)] + for _ in range(N_SAMPLES): + perturbed = perturb_goal(goal_s, goal_d) + goals.append((perturbed[0], perturbed[1], t)) + all_goals += goals + t += timestep + + # find best trajectory + trajectories = [] + for goal in all_goals: + s_goal, d_goal, t = goal + s_coefficients = JMT(start_s, s_goal, t) + d_coefficients = JMT(start_d, d_goal, t) + trajectories.append(tuple([s_coefficients, d_coefficients, t])) + + best = min( + trajectories, + key=lambda tr: calculate_cost( + tr, target_vehicle, delta, T, predictions, WEIGHTED_COST_FUNCTIONS + ) + ) + calculate_cost( + best, target_vehicle, delta, T, predictions, WEIGHTED_COST_FUNCTIONS, + verbose=True + ) + return best + + +def calculate_cost(trajectory, target_vehicle, delta, goal_t, + predictions, cost_functions_with_weights, verbose=False): + cost = 0 + for cf, weight in cost_functions_with_weights: + new_cost = weight * cf(trajectory, target_vehicle, + delta, goal_t, predictions) + cost += new_cost + if verbose: + print("cost for {} is \t {}".format(cf.__name__, new_cost)) + return cost + +def perturb_goal(goal_s, goal_d): + """ + Returns a "perturbed" version of the goal. + """ + new_s_goal = [] + for mu, sig in zip(goal_s, SIGMA_S): + new_s_goal.append(random.gauss(mu, sig)) + + new_d_goal = [] + for mu, sig in zip(goal_d, SIGMA_D): + new_d_goal.append(random.gauss(mu, sig)) + + return tuple([new_s_goal, new_d_goal]) + +def JMT(start, end, T): + """ + Calculates Jerk Minimizing Trajectory for start, end and T. + """ + a_0, a_1, a_2 = start[0], start[1], start[2] / 2.0 + c_0 = a_0 + a_1 * T + a_2 * T ** 2 + c_1 = a_1 + 2 * a_2 * T + c_2 = 2 * a_2 + + A = np.array([ + [ T ** 3, T ** 4, T ** 5], + [3 * T ** 2, 4 * T ** 3, 5 * T ** 4], + [6 * T, 12 * T ** 2, 20 * T ** 3], + ]) + B = np.array([ + end[0] - c_0, + end[1] - c_1, + end[2] - c_2 + ]) + a_3_4_5 = np.linalg.solve(A, B) + alphas = np.concatenate([np.array([a_0, a_1, a_2]), a_3_4_5]) + return alphas diff --git a/code/week-7/PTG/ptg_example.png b/code/week-7/PTG/ptg_example.png new file mode 100644 index 00000000..4da1bf6e Binary files /dev/null and b/code/week-7/PTG/ptg_example.png differ diff --git a/code/week-7/README.md b/code/week-7/README.md new file mode 100644 index 00000000..1eca31d4 --- /dev/null +++ b/code/week-7/README.md @@ -0,0 +1,203 @@ +## HW_week7 hybrid a* + +# Hybrid A* Algorithm +--- + + def expand(self, current, goal): + g = current['g'] + x, y, theta = current['x'], current['y'], current['t'] + + # The g value of a newly expanded cell increases by 1 from the + # previously expanded cell. + g_2 = g + 1 + next_states = [] + + # Consider a discrete selection of steering angles. + for delta_t in range(self.omega_min, self.omega_max + self.omega_step, self.omega_step): + # TODO: implement the trajectory generation based on + # a simple bicycle model. + # Let theta2 be the vehicle's heading (in radian) + # between 0 and 2 * PI. + # Check validity and then add to the next_states list. + delta_t *= (np.pi / 180) + omega = self.speed / self.length * np.tan(delta_t) + theta2 = self.normalize(theta + omega) + x_2 = x + self.speed * np.cos(theta2) + y_2 = y + self.speed * np.sin(theta2) + + if 0 <= self.idx(x_2) < self.dim[1] and 0 <= self.idx(y_2) < self.dim[2]: + f_2 = g_2 + self.heuristic(x_2, y_2, goal) + expanded_state = self.State(x_2, y_2, theta2, g_2, f_2) + next_states.append(expanded_state) + return next_states + + def search(self, grid, start, goal): + # Initial heading of the vehicle is given in the + # last component of the tuple start. + theta = start[-1] + # Determine the cell to contain the initial state, as well as + # the state itself. + stack = self.theta_to_stack_num(theta) + g = 0 + s = { + 'f': self.heuristic(start[0], start[1], goal), + 'g': g, + 'x': start[0], + 'y': start[1], + 't': theta, + } + self.final = s + # Close the initial cell and record the starting state for + # the sake of path reconstruction. + self.closed[stack][self.idx(s['x'])][self.idx(s['y'])] = 1 + self.came_from[stack][self.idx(s['x'])][self.idx(s['y'])] = s + total_closed = 1 + opened = [s] + + # Examine the open list, according to the order dictated by + # the heuristic function. + cycle = 0 + while len(opened) > 0: + # TODO: implement prioritized breadth-first search + # for the hybrid A* algorithm. + cycle += 1 + print(cycle) + + opened.sort(key=lambda s : s['f'], reverse=True) + curr = opened.pop() + x, y = curr['x'], curr['y'] + if (self.idx(x), self.idx(y)) == goal: + self.final = curr + found = True + break + + # Compute reachable new states and process each of them. + next_states = self.expand(curr, goal) + for n in next_states: + idx_x, idx_y = self.idx(n['x']), self.idx(n['y']) + stack = self.theta_to_stack_num(n['t']) + if grid[idx_x, idx_y] == 0: + dist_x, dist_y = abs(self.idx(x) - idx_x), abs(self.idx(y) - idx_y) + min_x, min_y = np.minimum(self.idx(x), idx_x), np.minimum(self.idx(y), idx_y) + possible = True + for d_x in range(dist_x + 1): + for d_y in range(dist_y + 1): + if grid[min_x + d_x, min_y + d_y] != 0: + possible = False + if possible and self.closed[stack][idx_x][idx_y] == 0: + self.closed[stack][idx_x][idx_y] = 1 + total_closed += 1 + self.came_from[stack][idx_x][idx_y] = curr + opened.append(n) + + else: + # We weren't able to find a valid path; this does not necessarily + # mean there is no feasible trajectory to reach the goal. + # In other words, the hybrid A* algorithm is not complete. + found = False + print(f"speed: {self.speed} NUM_THETA_CELLS: {self.NUM_THETA_CELLS}") + return found, total_closed + + def theta_to_stack_num(self, theta): + # TODO: implement a function that calculate the stack number + # given theta represented in radian. Note that the calculation + # should partition 360 degrees (2 * PI rad) into different + # cells whose number is given by NUM_THETA_CELLS. + interval = 2 * np.pi / self.dim[0] + stack_num = 0 + while theta > interval: + theta -= interval + stack_num += 1 + if stack_num == self.NUM_THETA_CELLS: + stack_num = 0 + return int(stack_num) + + def heuristic(self, x, y, goal): + # TODO: implement a heuristic function. + cost_manhattan_dist = abs(x - goal[0]) + abs(y - goal[1]) + return cost_manhattan_dist + +- 최적의 경로 탐색이 특징인 A* 알고리즘과 다르게, Hybrid A* 알고리즘은 completeness, optimality를 포기하고 drivable path 를 찾을 수 있다. + +- 자차의 조향각 (delta), 속도(speed)를 이용하여 자차가 이동할 위치를 구한다 -> "motion model" + +- 차량이 이동할 다음 state (x y 좌표, delta 조향각)를 구하고, 각도를 정규화하여 map (grid) 내에 존재하는, 이동 가능한 state 만 입력하도록 한다. -> "expand" + +- 시작점에서 목표점으로 도달할 때까지 반복하여 경로를 찾으며, map grid 내에 존재하는지와 장애물 & 중복 지점인지 확인하고 기록한다. -> "search" + +- 시작점에서 목표점까지의 cost 계산은 heuristic 알고리즘을 사용하였으며, heuristic cost 계산을 위해 Manhatten Distance 방법을 적용하였다. + +- 자차의 방향을 기준으로 입력 받은 조향각을 0~360 사이로 반환 -> "theta to stack num" + +- 자차의 motion state (theta, speed)에 따라 결과가 달라진다. + +- 1. speed = 1, num theta cells = 180 + + +![image](https://user-images.githubusercontent.com/80089347/117464664-da236600-af8b-11eb-8a1e-5d16ec38130a.png) + + +- 2. speed = 1.5, num theta cells = 360 + + +![image](https://user-images.githubusercontent.com/80089347/117464762-f1faea00-af8b-11eb-8f38-13046f4eb48f.png) + + + +--- + +# Week 7 - Hybrid A* Algorithm & Trajectory Generation + +--- + +[//]: # (Image References) +[has-example]: ./hybrid_a_star/has_example.png +[ptg-example]: ./PTG/ptg_example.png + +## Assignment: Hybrid A* Algorithm + +In directory [`./hybrid_a_star`](./hybrid_a_star), a simple test program for the hybrid A* algorithm is provided. Run the following command to test: + +``` +$ python main.py +``` + +The program consists of three modules: + +* `main.py` defines the map, start configuration and end configuration. It instantiates a `HybridAStar` object and calls the search method to generate a motion plan. +* `hybrid_astar.py` implements the algorithm. +* `plot.py` provides an OpenCV-based visualization for the purpose of result monitoring. + +You have to implement the following sections of code for the assignment: + +* Trajectory generation: in the method `HybridAStar.expand()`, a simple one-point trajectory shall be generated based on a basic bicycle model. This is going to be used in expanding 3-D grid cells in the algorithm's search operation. +* Hybrid A* search algorithm: in the method `HybridAStar.search()`, after expanding the states reachable from the current configuration, the algorithm must process each state (i.e., determine the grid cell, check its validity, close the visited cell, and record the path. You will have to write code in the `for n in next_states:` loop. +* Discretization of heading: in the method `HybridAStar.theta_to_stack_num()`, you will write code to map the vehicle's orientation (theta) to a finite set of stack indices. +* Heuristic function: in the method `HybridAStar.heuristic()`, you define a heuristic function that will be used in determining the priority of grid cells to be expanded. For instance, the distance to the goal is a reasonable estimate of each cell's cost. + +You are invited to tweak various parameters including the number of stacks (heading discretization granularity) and the vehicle's velocity. It will also be interesting to adjust the grid granularity of the map. The following figure illustrates an example output of the program with the default map given in `main.py` and `NUM_THETA_CELLS = 360` while the vehicle speed is set to 0.5. + +![Example Output of the Hybrid A* Test Program][has-example] + +--- + +## Experiment: Polynomial Trajectory Generation + +In directory [`./PTG`](./PTG), a sample program is provided that tests polynomial trajectory generation. If you input the following command: + +``` +$ python evaluate_ptg.py +``` + +you will see an output such as the following figure. + +![Example Output of the Polynomial Trajectory Generator][ptg-example] + +Note that the above figure is an example, while the result you get will be different from run to run because of the program's random nature. The program generates a number of perturbed goal configurations, computes a jerk minimizing trajectory for each goal position, and then selects the one with the minimum cost as defined by the cost functions and their combination. + +Your job in this experiment is: + +1. to understand the polynomial trajectory generation by reading the code already implemented and in place; given a start configuration and a goal configuration, the algorithm computes coefficient values for a quintic polynomial that defines the jerk minimizing trajectory; and +2. to derive an appropriate set of weights applied to the cost functions; the mechanism to calculate the cost for a trajectory and selecting one with the minimum cost is the same as described in the previous (Week 6) lecture. + +Experiment by tweaking the relative weight for each cost function. It will also be very interesting to define your own cost metric and implement it using the information associated with trajectories. diff --git a/code/week-7/hybrid_a_star/has_example.png b/code/week-7/hybrid_a_star/has_example.png new file mode 100644 index 00000000..ef2ce2c4 Binary files /dev/null and b/code/week-7/hybrid_a_star/has_example.png differ diff --git a/code/week-7/hybrid_a_star/hybrid_astar.py b/code/week-7/hybrid_a_star/hybrid_astar.py new file mode 100644 index 00000000..0883aa85 --- /dev/null +++ b/code/week-7/hybrid_a_star/hybrid_astar.py @@ -0,0 +1,180 @@ +import numpy as np + +class HybridAStar: + # Determine how many grid cells to have for theta-axis. + NUM_THETA_CELLS = 180 + + # Define min, max, and resolution of steering angles + omega_min = -35 + omega_max = 35 + omega_step = 5 + + # A very simple bicycle model + speed = 1.0 + length = 0.5 + + # Initialize the search structure. + def __init__(self, dim): + self.dim = dim + self.closed = np.zeros(self.dim, dtype=np.int) + self.came_from = np.full(self.dim, None) + + # Expand from a given state by enumerating reachable states. + def expand(self, current, goal): + g = current['g'] + x, y, theta = current['x'], current['y'], current['t'] + + # The g value of a newly expanded cell increases by 1 from the + # previously expanded cell. + g_2 = g + 1 + next_states = [] + + # Consider a discrete selection of steering angles. + for delta_t in range(self.omega_min, self.omega_max + self.omega_step, self.omega_step): + # TODO: implement the trajectory generation based on + # a simple bicycle model. + # Let theta2 be the vehicle's heading (in radian) + # between 0 and 2 * PI. + # Check validity and then add to the next_states list. + delta_t *= (np.pi / 180) + omega = self.speed / self.length * np.tan(delta_t) + theta2 = self.normalize(theta + omega) + x_2 = x + self.speed * np.cos(theta2) + y_2 = y + self.speed * np.sin(theta2) + + if 0 <= self.idx(x_2) < self.dim[1] and 0 <= self.idx(y_2) < self.dim[2]: + f_2 = g_2 + self.heuristic(x_2, y_2, goal) + expanded_state = self.State(x_2, y_2, theta2, g_2, f_2) + next_states.append(expanded_state) + return next_states + + def normalize(self, theta): + while theta >= 2 * np.pi: + theta -= 2 * np.pi + while theta < 0: + theta += 2 * np.pi + if theta == 2 * np.pi: + theta -= 2 * np.pi + return theta + + def State(self, x, y, theta, g, f): + state = { + 'f': f, + 'g': g, + 'x': x, + 'y': y, + 't': theta, + } + return state + + # Perform a breadth-first search based on the Hybrid A* algorithm. + def search(self, grid, start, goal): + # Initial heading of the vehicle is given in the + # last component of the tuple start. + theta = start[-1] + # Determine the cell to contain the initial state, as well as + # the state itself. + stack = self.theta_to_stack_num(theta) + g = 0 + s = { + 'f': self.heuristic(start[0], start[1], goal), + 'g': g, + 'x': start[0], + 'y': start[1], + 't': theta, + } + self.final = s + # Close the initial cell and record the starting state for + # the sake of path reconstruction. + self.closed[stack][self.idx(s['x'])][self.idx(s['y'])] = 1 + self.came_from[stack][self.idx(s['x'])][self.idx(s['y'])] = s + total_closed = 1 + opened = [s] + + # Examine the open list, according to the order dictated by + # the heuristic function. + cycle = 0 + while len(opened) > 0: + # TODO: implement prioritized breadth-first search + # for the hybrid A* algorithm. + cycle += 1 + print(cycle) + + opened.sort(key=lambda s : s['f'], reverse=True) + curr = opened.pop() + x, y = curr['x'], curr['y'] + if (self.idx(x), self.idx(y)) == goal: + self.final = curr + found = True + break + + # Compute reachable new states and process each of them. + next_states = self.expand(curr, goal) + for n in next_states: + idx_x, idx_y = self.idx(n['x']), self.idx(n['y']) + stack = self.theta_to_stack_num(n['t']) + if grid[idx_x, idx_y] == 0: + dist_x, dist_y = abs(self.idx(x) - idx_x), abs(self.idx(y) - idx_y) + min_x, min_y = np.minimum(self.idx(x), idx_x), np.minimum(self.idx(y), idx_y) + possible = True + for d_x in range(dist_x + 1): + for d_y in range(dist_y + 1): + if grid[min_x + d_x, min_y + d_y] != 0: + possible = False + if possible and self.closed[stack][idx_x][idx_y] == 0: + self.closed[stack][idx_x][idx_y] = 1 + total_closed += 1 + self.came_from[stack][idx_x][idx_y] = curr + opened.append(n) + + else: + # We weren't able to find a valid path; this does not necessarily + # mean there is no feasible trajectory to reach the goal. + # In other words, the hybrid A* algorithm is not complete. + found = False + print(f"speed: {self.speed} NUM_THETA_CELLS: {self.NUM_THETA_CELLS}") + return found, total_closed + + # Calculate the stack index of a state based on the vehicle's heading. + def theta_to_stack_num(self, theta): + # TODO: implement a function that calculate the stack number + # given theta represented in radian. Note that the calculation + # should partition 360 degrees (2 * PI rad) into different + # cells whose number is given by NUM_THETA_CELLS. + interval = 2 * np.pi / self.dim[0] + stack_num = 0 + while theta > interval: + theta -= interval + stack_num += 1 + if stack_num == self.NUM_THETA_CELLS: + stack_num = 0 + return int(stack_num) + + # Calculate the index of the grid cell based on the vehicle's position. + def idx(self, pos): + # We simply assume that each of the grid cell is the size 1 X 1. + return int(np.floor(pos)) + + # Implement a heuristic function to be used in the hybrid A* algorithm. + def heuristic(self, x, y, goal): + # TODO: implement a heuristic function. + cost_manhattan_dist = abs(x - goal[0]) + abs(y - goal[1]) + return cost_manhattan_dist + + # Reconstruct the path taken by the hybrid A* algorithm. + def reconstruct_path(self, start, goal): + # Start from the final state, and follow the link to the + # previous state using the came_from matrix. + curr = self.final + x, y = curr['x'], curr['y'] + path = [] + while x != start[0] and y != start[1]: + path.append(curr) + stack = self.theta_to_stack_num(curr['t']) + x, y = curr['x'], curr['y'] + curr = self.came_from[stack][self.idx(x)][self.idx(y)] + # Reverse the path so that it begins at the starting state + # and ends at the final state. + path.reverse() + + return path diff --git a/code/week-7/hybrid_a_star/main.py b/code/week-7/hybrid_a_star/main.py new file mode 100644 index 00000000..a30b93ac --- /dev/null +++ b/code/week-7/hybrid_a_star/main.py @@ -0,0 +1,54 @@ +import numpy as np +from hybrid_astar import HybridAStar +from plot import GridPlot + +# The grid is of shape 16 x 16. +# Note that we use (x, y) instead of (y, x) coordinates, +# since that makes application of trigonometric functions easier. +grid = np.array([ + [0, 1, 1, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0], + [0, 1, 1, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 0], + [0, 1, 1, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0], + [0, 1, 1, 0, 0, 0, 0, 1, 1, 0, 0, 0, 1, 1, 1, 0], + [0, 1, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 1, 1, 0, 0], + [0, 1, 1, 0, 0, 1, 1, 0, 0, 0, 1, 1, 1, 0, 0, 0], + [0, 1, 1, 0, 1, 1, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0], + [0, 1, 1, 1, 1, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0], + [0, 1, 1, 1, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0], + [0, 1, 1, 0, 0, 0, 1, 1, 1, 0, 0, 1, 1, 1, 1, 1], + [0, 1, 0, 0, 0, 1, 1, 1, 0, 0, 1, 1, 1, 1, 1, 1], + [0, 0, 0, 0, 1, 1, 1, 0, 0, 1, 1, 1, 1, 1, 1, 1], + [0, 0, 0, 1, 1, 1, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1], + [0, 0, 1, 1, 1, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1], + [0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], + [1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], +], dtype=np.int) + +# The coordinates are represented by (x, y, theta). +# Note that this order does not agree with numpy array indices. +start = (0.0, 0.0, 0.0) +# Note that the goal is specified by (x, y) +goal = (grid.shape[0] - 1, grid.shape[1] - 1) + +if __name__ == '__main__': + has = HybridAStar((HybridAStar.NUM_THETA_CELLS,) + grid.shape) + success, expanded = has.search(grid, start, goal) + if success: + print("Found path to goal in %d expansions" % expanded) + else: + print("No valid path found after %d expansions" % expanded) + path = has.reconstruct_path(start, goal) + + ''' + for i, p in enumerate(path, start=1): + print("##### step %d #####" % i) + print("x %f" % p['x']) + print("y %f" % p['y']) + print("theta %f" % p['t']) + ''' + + plot = GridPlot(grid.shape) + plot.plot_grid(grid) + if success: + plot.plot_path(path, start, goal) + plot.show() diff --git a/code/week-7/hybrid_a_star/plot.py b/code/week-7/hybrid_a_star/plot.py new file mode 100644 index 00000000..7bf0fdb3 --- /dev/null +++ b/code/week-7/hybrid_a_star/plot.py @@ -0,0 +1,58 @@ +import numpy as np +import cv2 + +class GridPlot: + + # Horizontal and vertical size (in pixels) for one grid cell. + dx = 50 + dy = 50 + # Horizontal (left & right) and vertical (top & bottom) margins. + mx = 30 + my = 30 + + def __init__(self, dim): + self.dim = dim + img_dim = ( + dim[0] * self.dx + 2 * self.mx, + dim[1] * self.dy + 2 * self.my, + 3 # 3-channel color image + ) + self.img = np.full(img_dim, 255, dtype=np.uint8) + + def plot_grid(self, grid): + # Draw grid lines. + for i in range(self.dim[1] + 1): + begin, end = self.conv(i, 0), self.conv(i, self.dim[0]) + cv2.line(self.img, begin, end, (255, 0, 0), 1) + for i in range(self.dim[0] + 1): + begin, end = self.conv(0, i), self.conv(self.dim[1], i) + cv2.line(self.img, begin, end, (255, 0, 0), 1) + # Fill in obstacles. + for x in range(self.dim[0]): + for y in range(self.dim[1]): + if grid[x][y] == 1: + ul = self.conv(x, y) + ul = (ul[0] + 1, ul[1] + 1) + lr = self.conv(x + 1, y + 1) + lr = (lr[0] - 1, lr[1] - 1) + cv2.rectangle(self.img, ul, lr, (120, 150, 150), -1) + + def plot_path(self, path, start, goal): + prev = self.conv(path[0]['x'], path[0]['y']) + cv2.circle(self.img, prev, 5, (0, 0, 255), -1) + for p in path[1:]: + x, y = p['x'], p['y'] + curr = self.conv(x, y) + cv2.circle(self.img, curr, 5, (0, 0, 255), -1) + cv2.line(self.img, prev, curr, (0, 0, 255), 2) + prev = curr + + def show(self): + cv2.imshow('grid plot', self.img) + cv2.waitKey(0) + cv2.destroyAllWindows() + + def conv(self, x, y): + px = int(y * self.dx + self.mx) + py = int(x * self.dy + self.my) + return (px, py)