├── Actual_trajectory.npy ├── BuggySimulator.py ├── Calculation and result ├── Model linearization.pdf ├── fast_controller_result_1.PNG ├── fast_controller_result_2.PNG ├── slow_controller_result_1.PNG └── slow_controller_result_2.PNG ├── Evaluation.py ├── README.md ├── buggyTrace.csv ├── controller.py ├── controller_fast.py ├── controller_slow.py ├── main.py └── util.py /Actual_trajectory.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yymmaa0000/Vehicle_Controller_Design_Project/675cda83015fb1a600815650b9031c3d6a0e6ef3/Actual_trajectory.npy -------------------------------------------------------------------------------- /BuggySimulator.py: -------------------------------------------------------------------------------- 1 | # please do not change this file 2 | import numpy as np 3 | import matplotlib.pyplot as plt 4 | 5 | 6 | def clamp(n, minn, maxn): 7 | return max(min(maxn, n), minn) 8 | 9 | 10 | def wrap2pi(a): 11 | return (a + np.pi) % (2 * np.pi) - np.pi 12 | 13 | 14 | def addGaussianNoise(a,u,sigma): 15 | # add Gaussian noise to a scalar a, with mean u and covariance sigma 16 | a += np.random.normal(u, sigma, 1) 17 | return a[0] 18 | 19 | 20 | class vehicle: 21 | # parameters 22 | lr = 1.7 23 | lf = 1.1 24 | Ca = 15000.0 25 | Iz = 3344.0 26 | f = 0.01 27 | m = 2000.0 28 | g = 10 29 | 30 | def __init__(self, state): 31 | # initialize the vehicle 32 | self.state = state 33 | self.observation = vehicle.observation( 34 | state.xd, state.yd, state.phid, state.delta, state.X, state.Y, state.phi) 35 | 36 | class state: 37 | deltaMax = np.pi/6 38 | deltaMin = -np.pi/6 39 | xdMax = 100.0 40 | xdMin = 0.0 41 | ydMax = 10.0 42 | ydMin = -10.0 43 | 44 | def __init__(self, xd = 0.0, yd = 0.0, phid = 0.0, delta = .0, X = .0, Y = .0, phi = .0): 45 | self.xd = xd 46 | self.yd = yd 47 | self.phid = phid 48 | self.delta = clamp(delta, self.deltaMin, self.deltaMax) 49 | self.X = X 50 | self.Y = Y 51 | self.phi = wrap2pi(phi) 52 | def showState(self): 53 | print('xd\t', self.xd, 'yd\t', self.yd, 'phid\t', self.phid, 'delta\t', self.delta, 54 | 'X\t', self.X, 'Y\t', self.Y, 'phi\t', self.phi) 55 | 56 | class observation(state): 57 | def __init__(self, xd = 0.0, yd = 0.0, phid = 0.0, delta = .0, X = .0, Y = .0, phi = .0): 58 | vehicle.state.__init__(self, xd, yd, phid, delta, X, Y, phi) 59 | self.addNoise() 60 | 61 | def copyState(self, state): 62 | self.xd = state.xd 63 | self.yd = state.yd 64 | self.phid = state.phid 65 | self.delta = state.delta 66 | self.X = state.X 67 | self.Y = state.Y 68 | self.phi = state.phi 69 | 70 | def addNoise(self): 71 | self.xd = addGaussianNoise(self.xd, 0, 0.5) 72 | self.yd = addGaussianNoise(self.yd, 0, 0.5) 73 | self.phid = addGaussianNoise(self.phid, 0, 0.05) 74 | self.delta = clamp(addGaussianNoise(self.delta, 0, 0.05), self.deltaMin, self.deltaMax) 75 | self.X = addGaussianNoise(self.X, 0, 1) 76 | self.Y = addGaussianNoise(self.Y, 0, 1) 77 | self.phi = wrap2pi(addGaussianNoise(self.phi, 0, 0.5)) 78 | 79 | class command: 80 | # F: N 81 | # deltad: rad/s 82 | deltadMax = np.pi/6.0 83 | deltadMin = -np.pi/6.0 84 | FMax = 10000.0 85 | Fmin = -10000.0 86 | 87 | def __init__(self, F_ = 0.0, deltad_ = 0.0): 88 | self.F = clamp(F_, self.Fmin, self.FMax) 89 | self.deltad = clamp(deltad_, self.deltadMin, self.deltadMax) 90 | 91 | def showCommand(self): 92 | print('F:\t', self.F, 'deltad:\t', self.deltad) 93 | 94 | def update(self, command): 95 | # time step 96 | dt = 0.05 97 | # update state 98 | Ff = np.sign(self.state.xd)*self.f*self.m*self.g 99 | Ftotal = command.F - Ff if np.abs(command.F)>=np.abs(Ff) else 0 100 | # print(Ftotal) 101 | ax = 1/self.m*Ftotal 102 | if np.abs(self.state.xd) <= 0.5: 103 | Fyf = 0.0 104 | Fyr = 0.0 105 | else: 106 | Fyf = 2.0*self.Ca*(self.state.delta-(self.state.yd+self.lf*self.state.phid)/self.state.xd) 107 | Fyr = 2.0*self.Ca*(-(self.state.yd-self.lr*self.state.phid)/self.state.xd) 108 | 109 | xdd = self.state.phid* self.state.yd + ax 110 | ydd = - self.state.phid* self.state.xd + 1.0/self.m*(Fyf*np.cos(self.state.delta) - Fyr) 111 | phidd = 1.0/self.Iz*(self.lf*Fyf - self.lr*Fyr) 112 | Xd = self.state.xd*np.cos(self.state.phi) - self.state.yd*np.sin(self.state.phi) 113 | Yd = self.state.xd*np.sin(self.state.phi) + self.state.yd*np.cos(self.state.phi) 114 | self.state.xd += xdd*dt 115 | self.state.yd += ydd*dt 116 | self.state.phid += phidd*dt 117 | 118 | self.state.delta += command.deltad*dt 119 | # self.state.delta = command.deltad 120 | 121 | 122 | self.state.X += Xd*dt 123 | self.state.Y += Yd*dt 124 | self.state.phi += self.state.phid*dt 125 | self.applyConstrain() 126 | # update observation 127 | self.observation.copyState(self.state) 128 | self.observation.addNoise() 129 | # self.state.showState() 130 | 131 | 132 | def applyConstrain(self): 133 | # phi should be between +- pi 134 | self.state.phi = wrap2pi(self.state.phi) 135 | # state constraint 136 | self.state.delta = clamp(self.state.delta,self.state.deltaMin,self.state.deltaMax) 137 | self.state.xd = clamp(self.state.xd, self.state.xdMin, self.state.xdMax) 138 | self.state.yd = clamp(self.state.yd, self.state.ydMin, self.state.ydMax) 139 | 140 | def showState(self): 141 | self.state.showState() 142 | 143 | 144 | if __name__ == "__main__": 145 | a = vehicle(vehicle.state(Y = 0.0,xd = 1)) 146 | n = 1000 147 | 148 | X = [] 149 | Y = [] 150 | delta = [] 151 | xd = [] 152 | yd = [] 153 | phi = [] 154 | phid = [] 155 | for i in range(n): 156 | # if i% 1 ==0: 157 | X.append(a.state.X) 158 | Y.append(a.state.Y) 159 | delta.append(a.state.delta) 160 | xd.append(a.state.xd) 161 | yd.append(a.state.yd) 162 | phid.append(a.state.phid) 163 | phi.append(a.state.phi) 164 | if a.state.xd > 3: 165 | c = a.command(deltad_=np.sin(i/10), F_=-10000) 166 | else: 167 | c = a.command(deltad_=np.sin(i/10), F_=10000.0) 168 | a.update(command = c) 169 | 170 | plt.subplot(321) 171 | plt.title('position') 172 | plt.plot(X,Y,'r') 173 | 174 | plt.subplot(322) 175 | plt.title('delta') 176 | plt.plot(delta, 'r') 177 | 178 | plt.subplot(323) 179 | plt.title('xd') 180 | plt.plot(xd, 'r') 181 | 182 | plt.subplot(324) 183 | plt.title('yd') 184 | plt.plot(yd, 'r') 185 | 186 | plt.subplot(325) 187 | plt.title('phi') 188 | plt.plot(phi, 'r') 189 | 190 | plt.subplot(326) 191 | plt.title('phid') 192 | plt.plot(phid, 'r') 193 | 194 | plt.show() -------------------------------------------------------------------------------- /Calculation and result/Model linearization.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yymmaa0000/Vehicle_Controller_Design_Project/675cda83015fb1a600815650b9031c3d6a0e6ef3/Calculation and result/Model linearization.pdf -------------------------------------------------------------------------------- /Calculation and result/fast_controller_result_1.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yymmaa0000/Vehicle_Controller_Design_Project/675cda83015fb1a600815650b9031c3d6a0e6ef3/Calculation and result/fast_controller_result_1.PNG -------------------------------------------------------------------------------- /Calculation and result/fast_controller_result_2.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yymmaa0000/Vehicle_Controller_Design_Project/675cda83015fb1a600815650b9031c3d6a0e6ef3/Calculation and result/fast_controller_result_2.PNG -------------------------------------------------------------------------------- /Calculation and result/slow_controller_result_1.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yymmaa0000/Vehicle_Controller_Design_Project/675cda83015fb1a600815650b9031c3d6a0e6ef3/Calculation and result/slow_controller_result_1.PNG -------------------------------------------------------------------------------- /Calculation and result/slow_controller_result_2.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yymmaa0000/Vehicle_Controller_Design_Project/675cda83015fb1a600815650b9031c3d6a0e6ef3/Calculation and result/slow_controller_result_2.PNG -------------------------------------------------------------------------------- /Evaluation.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from numpy import linalg as LA 3 | import math 4 | from util import * 5 | 6 | 7 | def dist(x1, y1, x2, y2): 8 | return math.sqrt((x2 - x1)**2 + (y2 - y1)**2) 9 | 10 | 11 | def clGrader(traj, X, Y, fs, Cmax_cl): 12 | ng = 0.0 13 | ntrack= traj.shape[0] 14 | XY = np.array([X,Y]) 15 | XY = XY.T 16 | for i in range(ntrack): 17 | minDist,_ = closest_node(traj[i,0], traj[i, 1], XY) 18 | if minDist <= Cmax_cl: 19 | ng += 1 20 | else: 21 | print(i) 22 | return fs * (ng / float(ntrack)) 23 | 24 | 25 | def adGrader(minDistList, fs, Cavg): 26 | avg = np.average(minDistList) 27 | if avg <= Cavg: 28 | return fs 29 | if avg <= Cavg*2: 30 | return (-20/Cavg)*avg + 40 31 | return 0 32 | 33 | 34 | def mdGrader(minDistList, fs, Cmax_md): 35 | ng = 0 36 | for i in range(len(minDistList)): 37 | if minDistList[i] <= Cmax_md: 38 | ng += 1 39 | return fs*ng/len(minDistList) 40 | 41 | 42 | def beatBaselineGrader(timeCurrent, timeBaseline): 43 | if timeCurrent <= timeBaseline: 44 | return 10 45 | elif timeCurrent <= 2.0*timeBaseline: 46 | return 20 - 10*timeCurrent/timeBaseline 47 | else: 48 | return 0 49 | 50 | 51 | def evaluation(minDistList, traj_, X, Y, taskNum): 52 | print('evaluating...') 53 | timeBaseline = 250 54 | dt = 0.05 55 | Cmax_cl = 6.0 # constrain of maximun distance for completing the loop 56 | Cavg = 3.0 # constrain of average distance 57 | Cmax_md = 6.0 # constrain of maximun distance 58 | fs = 20.0 # the full score you can get 59 | traj = traj_[1:len(traj_)-60,:] 60 | comGrad = clGrader(traj, X, Y, fs, Cmax_cl) # grade if you complete the loop 61 | if taskNum == 2: 62 | print('Score for complete the loop:', comGrad) 63 | avgGrad = adGrader(minDistList, fs, Cavg) # grade your average distance 64 | print('Score for average distance:', avgGrad) 65 | maxGrad = mdGrader(minDistList, fs, Cmax_md) # grade your maximum distance 66 | print('Score for maximum distance:', maxGrad) 67 | grade = comGrad + avgGrad + maxGrad 68 | print('Total score for task 4.2: ', grade) 69 | return grade 70 | else: 71 | if comGrad < fs: 72 | print('your vehicle did not finish the loop ' 73 | '\n you cannot enter competition' 74 | '\nTotal score for task 4.3 and 4.4:', 0) 75 | return 76 | else: 77 | timeCurrent = len(X) * dt 78 | beatBaselineScore = beatBaselineGrader(timeCurrent, timeBaseline) 79 | print('Your time is ', 80 | timeCurrent, 81 | '\nTotal score for task 4.3: ', beatBaselineScore) 82 | return beatBaselineScore -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Vehicle_Controller_Design_Project 2 | This project was completed on 12/10/2018, and was just uploaded to my Github account recently 3 | 4 | In this project, I designed a LQR controller to control a car following a given trajectory. 5 | The goal of this project is to make the car finish the path as quickly as possible while still maintaining good accuracy. 6 | 7 | The tasks for this project include: 8 | 1. Model the car using a bicycle model 9 | 2. linearize the model and decouple them into lateral and longitudinal models 10 | 3. Design a controller for both lateral and longitudinal models and tune the parameters to get the optimal peformance 11 | 12 | The detailed modeling of the car can be found in the reference. The liearization process can be found in the "Calculation and result" folder 13 | 14 | For this project, I designed 2 controllers. One is slow but accurate, the other is fast but barely satisfy the accuracy requirement. 15 | These two controllers can be found in controller_slow.py and controller_fast.py 16 | 17 | Here are some simulation results: 18 | 19 | ### Fast Controller ### 20 | * Total time: 74.55 s 21 | * Maximun distance from reference: 5.7268493347214475 m 22 | * Average distance from reference: 1.3019881974778402 m 23 | ![alt text](https://github.com/yymmaa0000/Vehicle_Controller_Design_Project/blob/master/Calculation%20and%20result/fast_controller_result_1.PNG) 24 | ![alt text](https://github.com/yymmaa0000/Vehicle_Controller_Design_Project/blob/master/Calculation%20and%20result/fast_controller_result_2.PNG) 25 | 26 | ### Slow Controller ### 27 | * Total time: 157.45000000000002 s 28 | * Maximun distance from reference: 5.6915563863996335 m 29 | * Average distance from reference: 0.7979645575292 m 30 | ![alt text](https://github.com/yymmaa0000/Vehicle_Controller_Design_Project/blob/master/Calculation%20and%20result/slow_controller_result_1.PNG) 31 | ![alt text](https://github.com/yymmaa0000/Vehicle_Controller_Design_Project/blob/master/Calculation%20and%20result/slow_controller_result_2.PNG) 32 | 33 | 34 | Reference 35 | 1. Rajamani Rajesh. Vehicle dynamics and control. Springer Science & Business Media,2011. 36 | 2. Kong Jason, et al. "Kinematic and dynamic vehicle models for autonomous driving 37 | control design." Intelligent Vehicles Symposium, 2015. 38 | -------------------------------------------------------------------------------- /controller.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from scipy import signal 3 | import BuggySimulator 4 | import scipy 5 | from util import * 6 | 7 | def controller(traj, currentState): 8 | # controller 9 | 10 | ''' parameters ''' 11 | lr = 1.7 12 | lf = 1.1 13 | Ca = 15000.0 14 | Iz = 3344.0 15 | f = 0.01 16 | m = 2000.0 17 | g = 10 18 | dt = 0.05 19 | 20 | ''' filter the track ''' 21 | if 'smooth_traj_x' not in controller.__dict__: 22 | x = traj[:,0] 23 | y = traj[:,1] 24 | sig = 5 25 | controller.smooth_traj_x = scipy.ndimage.gaussian_filter1d(x,sigma = sig,order = 0, mode = 'reflect') 26 | controller.smooth_traj_y = scipy.ndimage.gaussian_filter1d(y,sigma = sig,order = 0, mode = 'reflect') 27 | 28 | ''' calculate the derivative of track ''' 29 | if 'dx' not in controller.__dict__: 30 | x = traj[:,0] 31 | y = traj[:,1] 32 | sig = 5 33 | controller.dx = scipy.ndimage.gaussian_filter1d(x,sigma = sig,order = 1, mode = 'reflect') 34 | controller.dy = scipy.ndimage.gaussian_filter1d(y,sigma = sig,order = 1, mode = 'reflect') 35 | 36 | ''' calculate the curvature of track ''' 37 | if 'curvature' not in controller.__dict__: 38 | x = traj[:,0] 39 | y = traj[:,1] 40 | sig = 5 41 | dxdt = scipy.ndimage.gaussian_filter1d(x,sigma = sig,order = 1, mode = 'wrap') 42 | d2xdt2 = scipy.ndimage.gaussian_filter1d(dxdt,sigma = sig,order = 2, mode = 'wrap') 43 | dydt = scipy.ndimage.gaussian_filter1d(y,sigma = sig,order = 1, mode = 'wrap') 44 | d2ydt2 = scipy.ndimage.gaussian_filter1d(dydt,sigma= sig,order = 2, mode = 'wrap') 45 | controller.curvature = np.abs(dxdt*d2ydt2-dydt*d2xdt2)/np.power(dxdt**2+dydt**2,3.0/2.0) 46 | 47 | ''' vehicle model ''' 48 | Vx = currentState.xd 49 | if Vx < 0.1: 50 | Vx = 0.1 51 | A = np.array([[0,1,0,0],[0,-4*Ca/m/Vx,4*Ca/m,2*Ca*(lr-lf)/m/Vx], 52 | [0,0,0,1],[0,-2*Ca*(lf-lr)/Iz/Vx,2*Ca*(lf-lr)/Iz,-2*Ca*(lf*lf+lr*lr)/Iz/Vx]]) 53 | B1 = np.array([[0],[2*Ca/m],[0],[2*Ca*lf/Iz]]) 54 | B2 = np.array([[0],[-2*Ca*(lf-lr)/m/Vx - Vx],[0],[-2*Ca*(lf*lf+lr*lr)/Iz/Vx]]) 55 | 56 | ''' descretize the system ''' 57 | C = np.zeros((1,4)) 58 | D = 0 59 | sys = scipy.signal.cont2discrete((A,B1,C,D),dt) 60 | Ad = sys[0] 61 | Bd = sys[1] 62 | 63 | ''' find points on the track closest to the car ''' 64 | min_dis_sq = 1000000000; 65 | min_index = 0; 66 | for j in range(traj.shape[0]): 67 | distance_sq = (controller.smooth_traj_x[j] - currentState.X)**2 + (controller.smooth_traj_y[j] - currentState.Y)**2 68 | if distance_sq = traj.shape[0]: 81 | target_index = -1 82 | next_index = min_index+1 83 | if next_index >= traj.shape[0]: 84 | next_index =-1 85 | 86 | ''' find the current distance from car to the road ''' 87 | road_point1 = np.array([[controller.smooth_traj_x[min_index-1],controller.smooth_traj_y[min_index-1]]]) 88 | road_point2 = np.array([[controller.smooth_traj_x[next_index],controller.smooth_traj_y[next_index]]]) 89 | car = np.array([[currentState.X,currentState.Y]]) 90 | e1 = np.cross(road_point2-road_point1,car-road_point1)/np.linalg.norm(road_point2-road_point1) 91 | 92 | ''' find desired road angle ''' 93 | road_angle_x = controller.dx[target_index] 94 | road_angle_y = controller.dy[target_index] 95 | desired_road_angle = np.arctan2(road_angle_y,road_angle_x) 96 | if (desired_road_angle <0): 97 | desired_road_angle += 2*np.pi 98 | 99 | ''' find estimated angle error e2''' 100 | actual_angle = currentState.phi 101 | if(actual_angle < 0): 102 | actual_angle += 2*np.pi 103 | e2 = actual_angle - desired_road_angle 104 | if (e2 > np.pi): 105 | e2 -= 2*np.pi 106 | elif (e2 < -np.pi): 107 | e2 += 2*np.pi 108 | 109 | ''' find de1 ''' 110 | de1 = currentState.yd + Vx*(e2) 111 | 112 | ''' find de2 ''' 113 | desired_road_angle_rate = Vx*controller.curvature[target_index] 114 | de2 = currentState.phid - desired_road_angle_rate 115 | 116 | ''' find longitudinal speed depending on road angle ''' 117 | max_speed = 8; 118 | desired_speed = max_speed 119 | # desired_speed = max_speed/3 + max_speed *2/3* np.cos(desired_road_angle_rate*dt); 120 | 121 | ''' use PID to control longitudinal speed ''' 122 | gain = 10000; 123 | F = gain*(desired_speed - currentState.xd) 124 | 125 | ''' find k ''' 126 | # ================== infinite horizen LQR ======================== 127 | Q = np.array([[1,0,0,0],[0,1,0,0],[0,0,1,0],[0,0,0,1]]) 128 | R = 9 129 | s = np.matrix(scipy.linalg.solve_discrete_are(Ad, Bd, Q, R)) 130 | k = scipy.linalg.inv(Bd.T.dot(s).dot(Bd) + R).dot(Bd.T.dot(s).dot(Ad)) 131 | # eigval,eigvec = scipy.linalg.eig(Ad-Bd.dot(k)) 132 | # print(np.abs(eigval)) 133 | k = -k 134 | 135 | ''' output u = kx ''' 136 | estimated_x = np.array([[e1],[de1],[e2],[de2]]) 137 | delta = k.dot(estimated_x) 138 | delta_d = (delta - currentState.delta)/dt 139 | delta_d = float(delta_d) 140 | 141 | result = BuggySimulator.vehicle.command(F,delta_d) 142 | return result 143 | 144 | 145 | 146 | 147 | 148 | 149 | -------------------------------------------------------------------------------- /controller_fast.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from scipy import signal 3 | import BuggySimulator 4 | import scipy 5 | from util import * 6 | 7 | def controller(traj, currentState): 8 | # controller 9 | 10 | ''' parameters ''' 11 | lr = 1.7 12 | lf = 1.1 13 | Ca = 15000.0 14 | Iz = 3344.0 15 | f = 0.01 16 | m = 2000.0 17 | g = 10 18 | dt = 0.05 19 | 20 | ''' filter the track ''' 21 | if 'smooth_traj_x' not in controller.__dict__: 22 | x = traj[:,0] 23 | y = traj[:,1] 24 | sig = 5 25 | controller.smooth_traj_x = scipy.ndimage.gaussian_filter1d(x,sigma = sig,order = 0, mode = 'reflect') 26 | controller.smooth_traj_y = scipy.ndimage.gaussian_filter1d(y,sigma = sig,order = 0, mode = 'reflect') 27 | 28 | ''' calculate the derivative of track ''' 29 | if 'dx' not in controller.__dict__: 30 | x = traj[:,0] 31 | y = traj[:,1] 32 | sig = 5 33 | controller.dx = scipy.ndimage.gaussian_filter1d(x,sigma = sig,order = 1, mode = 'reflect') 34 | controller.dy = scipy.ndimage.gaussian_filter1d(y,sigma = sig,order = 1, mode = 'reflect') 35 | 36 | ''' calculate the curvature of track ''' 37 | if 'curvature' not in controller.__dict__: 38 | x = traj[:,0] 39 | y = traj[:,1] 40 | sig = 5 41 | dxdt = scipy.ndimage.gaussian_filter1d(x,sigma = sig,order = 1, mode = 'wrap') 42 | d2xdt2 = scipy.ndimage.gaussian_filter1d(dxdt,sigma = sig,order = 2, mode = 'wrap') 43 | dydt = scipy.ndimage.gaussian_filter1d(y,sigma = sig,order = 1, mode = 'wrap') 44 | d2ydt2 = scipy.ndimage.gaussian_filter1d(dydt,sigma= sig,order = 2, mode = 'wrap') 45 | controller.curvature = np.abs(dxdt*d2ydt2-dydt*d2xdt2)/np.power(dxdt**2+dydt**2,3.0/2.0) 46 | 47 | ''' vehicle model ''' 48 | Vx = currentState.xd 49 | if Vx < 0.1: 50 | Vx = 0.1 51 | A = np.array([[0,1,0,0],[0,-4*Ca/m/Vx,4*Ca/m,2*Ca*(lr-lf)/m/Vx], 52 | [0,0,0,1],[0,-2*Ca*(lf-lr)/Iz/Vx,2*Ca*(lf-lr)/Iz,-2*Ca*(lf*lf+lr*lr)/Iz/Vx]]) 53 | B1 = np.array([[0],[2*Ca/m],[0],[2*Ca*lf/Iz]]) 54 | B2 = np.array([[0],[-2*Ca*(lf-lr)/m/Vx - Vx],[0],[-2*Ca*(lf*lf+lr*lr)/Iz/Vx]]) 55 | 56 | ''' descretize the system ''' 57 | C = np.zeros((1,4)) 58 | D = 0 59 | sys = scipy.signal.cont2discrete((A,B1,C,D),dt) 60 | Ad = sys[0] 61 | Bd = sys[1] 62 | 63 | ''' find points on the track closest to the car ''' 64 | min_dis_sq = 1000000000; 65 | min_index = 0; 66 | for j in range(traj.shape[0]): 67 | distance_sq = (controller.smooth_traj_x[j] - currentState.X)**2 + (controller.smooth_traj_y[j] - currentState.Y)**2 68 | if distance_sq = traj.shape[0]: 80 | target_index = -1 81 | next_index = min_index+1 82 | if next_index >= traj.shape[0]: 83 | next_index =-1 84 | 85 | ''' find the current distance from car to the road ''' 86 | road_point1 = np.array([[controller.smooth_traj_x[min_index-1],controller.smooth_traj_y[min_index-1]]]) 87 | road_point2 = np.array([[controller.smooth_traj_x[next_index],controller.smooth_traj_y[next_index]]]) 88 | car = np.array([[currentState.X,currentState.Y]]) 89 | e1 = np.cross(road_point2-road_point1,car-road_point1)/np.linalg.norm(road_point2-road_point1) 90 | 91 | ''' find desired road angle ''' 92 | road_angle_x = controller.dx[target_index] 93 | road_angle_y = controller.dy[target_index] 94 | desired_road_angle = np.arctan2(road_angle_y,road_angle_x) 95 | if (desired_road_angle <0): 96 | desired_road_angle += 2*np.pi 97 | 98 | ''' find estimated angle error e2''' 99 | actual_angle = currentState.phi 100 | if(actual_angle < 0): 101 | actual_angle += 2*np.pi 102 | e2 = actual_angle - desired_road_angle 103 | if (e2 > np.pi): 104 | e2 -= 2*np.pi 105 | elif (e2 < -np.pi): 106 | e2 += 2*np.pi 107 | 108 | ''' find de1 ''' 109 | de1 = currentState.yd + Vx*(e2) 110 | 111 | ''' find de2 ''' 112 | desired_road_angle_rate = Vx*controller.curvature[target_index] 113 | de2 = currentState.phid - desired_road_angle_rate 114 | 115 | ''' find longitudinal speed depending on road angle ''' 116 | min_speed = 8; 117 | middle_speed1 = 23 118 | middle_speed2 = 20.6 119 | max_speed = 50; 120 | if min_index <= 649 or min_index >= 7800: 121 | desired_speed = max_speed 122 | elif min_index >= 3000 and min_index <= 5050: 123 | desired_speed = middle_speed1 124 | elif min_index >= 5880 and min_index <= 7430: 125 | desired_speed = middle_speed2 126 | else: 127 | desired_speed = min_speed 128 | 129 | # max_curvature = 0; 130 | # 131 | # speed_future = 500 132 | # speed_target_index = min_index+speed_future 133 | # if speed_target_index >= traj.shape[0]: 134 | # speed_target_index = -1 135 | # for i in range(min_index,speed_target_index): 136 | # if(controller.curvature[i]>max_curvature): 137 | # max_curvature = controller.curvature[i] 138 | # if max_curvature < 0.001: 139 | # desired_speed = max_speed 140 | ## elif max_curvature < 0.02: 141 | ## desired_speed = middle_speed 142 | # else: 143 | # desired_speed = min_speed 144 | 145 | ''' use PID to control longitudinal speed ''' 146 | gain = 10000; 147 | F = gain*(desired_speed - currentState.xd) 148 | 149 | ''' find k ''' 150 | # ================== infinite horizen LQR ======================== 151 | Q = np.array([[1,0,0,0],[0,1,0,0],[0,0,1,0],[0,0,0,1]]) 152 | R = 9 153 | s = np.matrix(scipy.linalg.solve_discrete_are(Ad, Bd, Q, R)) 154 | k = scipy.linalg.inv(Bd.T.dot(s).dot(Bd) + R).dot(Bd.T.dot(s).dot(Ad)) 155 | # eigval,eigvec = scipy.linalg.eig(Ad-Bd.dot(k)) 156 | # print(np.abs(eigval)) 157 | k = -k 158 | 159 | ''' output u = kx ''' 160 | estimated_x = np.array([[e1],[de1],[e2],[de2]]) 161 | delta = k.dot(estimated_x) 162 | delta_d = (delta - currentState.delta)/dt 163 | delta_d = float(delta_d) 164 | 165 | result = BuggySimulator.vehicle.command(F,delta_d) 166 | return result 167 | 168 | 169 | 170 | 171 | 172 | 173 | -------------------------------------------------------------------------------- /controller_slow.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from scipy import signal 3 | import BuggySimulator 4 | import scipy 5 | from util import * 6 | 7 | def controller(traj, currentState): 8 | # controller 9 | 10 | ''' parameters ''' 11 | lr = 1.7 12 | lf = 1.1 13 | Ca = 15000.0 14 | Iz = 3344.0 15 | f = 0.01 16 | m = 2000.0 17 | g = 10 18 | dt = 0.05 19 | 20 | ''' filter the track ''' 21 | if 'smooth_traj_x' not in controller.__dict__: 22 | x = traj[:,0] 23 | y = traj[:,1] 24 | sig = 5 25 | controller.smooth_traj_x = scipy.ndimage.gaussian_filter1d(x,sigma = sig,order = 0, mode = 'reflect') 26 | controller.smooth_traj_y = scipy.ndimage.gaussian_filter1d(y,sigma = sig,order = 0, mode = 'reflect') 27 | 28 | ''' calculate the derivative of track ''' 29 | if 'dx' not in controller.__dict__: 30 | x = traj[:,0] 31 | y = traj[:,1] 32 | sig = 5 33 | controller.dx = scipy.ndimage.gaussian_filter1d(x,sigma = sig,order = 1, mode = 'reflect') 34 | controller.dy = scipy.ndimage.gaussian_filter1d(y,sigma = sig,order = 1, mode = 'reflect') 35 | 36 | ''' calculate the curvature of track ''' 37 | if 'curvature' not in controller.__dict__: 38 | x = traj[:,0] 39 | y = traj[:,1] 40 | sig = 5 41 | dxdt = scipy.ndimage.gaussian_filter1d(x,sigma = sig,order = 1, mode = 'wrap') 42 | d2xdt2 = scipy.ndimage.gaussian_filter1d(dxdt,sigma = sig,order = 2, mode = 'wrap') 43 | dydt = scipy.ndimage.gaussian_filter1d(y,sigma = sig,order = 1, mode = 'wrap') 44 | d2ydt2 = scipy.ndimage.gaussian_filter1d(dydt,sigma= sig,order = 2, mode = 'wrap') 45 | controller.curvature = np.abs(dxdt*d2ydt2-dydt*d2xdt2)/np.power(dxdt**2+dydt**2,3.0/2.0) 46 | 47 | ''' vehicle model ''' 48 | Vx = currentState.xd 49 | if Vx < 0.1: 50 | Vx = 0.1 51 | A = np.array([[0,1,0,0],[0,-4*Ca/m/Vx,4*Ca/m,2*Ca*(lr-lf)/m/Vx], 52 | [0,0,0,1],[0,-2*Ca*(lf-lr)/Iz/Vx,2*Ca*(lf-lr)/Iz,-2*Ca*(lf*lf+lr*lr)/Iz/Vx]]) 53 | B1 = np.array([[0],[2*Ca/m],[0],[2*Ca*lf/Iz]]) 54 | B2 = np.array([[0],[-2*Ca*(lf-lr)/m/Vx - Vx],[0],[-2*Ca*(lf*lf+lr*lr)/Iz/Vx]]) 55 | 56 | ''' descretize the system ''' 57 | C = np.zeros((1,4)) 58 | D = 0 59 | sys = scipy.signal.cont2discrete((A,B1,C,D),dt) 60 | Ad = sys[0] 61 | Bd = sys[1] 62 | 63 | ''' find points on the track closest to the car ''' 64 | min_dis_sq = 1000000000; 65 | min_index = 0; 66 | for j in range(traj.shape[0]): 67 | distance_sq = (controller.smooth_traj_x[j] - currentState.X)**2 + (controller.smooth_traj_y[j] - currentState.Y)**2 68 | if distance_sq = traj.shape[0]: 81 | target_index = -1 82 | next_index = min_index+1 83 | if next_index >= traj.shape[0]: 84 | next_index =-1 85 | 86 | ''' find the current distance from car to the road ''' 87 | road_point1 = np.array([[controller.smooth_traj_x[min_index-1],controller.smooth_traj_y[min_index-1]]]) 88 | road_point2 = np.array([[controller.smooth_traj_x[next_index],controller.smooth_traj_y[next_index]]]) 89 | car = np.array([[currentState.X,currentState.Y]]) 90 | e1 = np.cross(road_point2-road_point1,car-road_point1)/np.linalg.norm(road_point2-road_point1) 91 | 92 | ''' find desired road angle ''' 93 | road_angle_x = controller.dx[target_index] 94 | road_angle_y = controller.dy[target_index] 95 | desired_road_angle = np.arctan2(road_angle_y,road_angle_x) 96 | if (desired_road_angle <0): 97 | desired_road_angle += 2*np.pi 98 | 99 | ''' find estimated angle error e2''' 100 | actual_angle = currentState.phi 101 | if(actual_angle < 0): 102 | actual_angle += 2*np.pi 103 | e2 = actual_angle - desired_road_angle 104 | if (e2 > np.pi): 105 | e2 -= 2*np.pi 106 | elif (e2 < -np.pi): 107 | e2 += 2*np.pi 108 | 109 | ''' find de1 ''' 110 | de1 = currentState.yd + Vx*(e2) 111 | 112 | ''' find de2 ''' 113 | desired_road_angle_rate = Vx*controller.curvature[target_index] 114 | de2 = currentState.phid - desired_road_angle_rate 115 | 116 | ''' find longitudinal speed depending on road angle ''' 117 | max_speed = 8; 118 | desired_speed = max_speed 119 | # desired_speed = max_speed/3 + max_speed *2/3* np.cos(desired_road_angle_rate*dt); 120 | 121 | ''' use PID to control longitudinal speed ''' 122 | gain = 10000; 123 | F = gain*(desired_speed - currentState.xd) 124 | 125 | ''' find k ''' 126 | # ================== infinite horizen LQR ======================== 127 | Q = np.array([[1,0,0,0],[0,1,0,0],[0,0,1,0],[0,0,0,1]]) 128 | R = 9 129 | s = np.matrix(scipy.linalg.solve_discrete_are(Ad, Bd, Q, R)) 130 | k = scipy.linalg.inv(Bd.T.dot(s).dot(Bd) + R).dot(Bd.T.dot(s).dot(Ad)) 131 | # eigval,eigvec = scipy.linalg.eig(Ad-Bd.dot(k)) 132 | # print(np.abs(eigval)) 133 | k = -k 134 | 135 | ''' output u = kx ''' 136 | estimated_x = np.array([[e1],[de1],[e2],[de2]]) 137 | delta = k.dot(estimated_x) 138 | delta_d = (delta - currentState.delta)/dt 139 | delta_d = float(delta_d) 140 | 141 | result = BuggySimulator.vehicle.command(F,delta_d) 142 | return result 143 | 144 | 145 | 146 | 147 | 148 | 149 | -------------------------------------------------------------------------------- /main.py: -------------------------------------------------------------------------------- 1 | from BuggySimulator import * 2 | import numpy as np 3 | #from controller_slow import * 4 | from controller_fast import * 5 | from util import * 6 | import matplotlib.pyplot as plt 7 | from Evaluation import * 8 | 9 | # get the trajectory 10 | traj = get_trajectory('buggyTrace.csv') 11 | # initial the Buggy 12 | vehicle = initail(traj, 0) 13 | n = 6000 14 | X = [] 15 | Y = [] 16 | delta = [] 17 | xd = [] 18 | yd = [] 19 | phi = [] 20 | phid = [] 21 | deltad = [] 22 | F = [] 23 | minDist =[] 24 | ''' 25 | your code starts here 26 | ''' 27 | # preprocess the trajectory 28 | passMiddlePoint = False 29 | nearGoal = False 30 | for i in range(n): 31 | print(i) 32 | command = controller(traj, vehicle.state) 33 | vehicle.update(command = command) 34 | 35 | # termination check 36 | disError,nearIdx = closest_node(vehicle.state.X, vehicle.state.Y, traj) 37 | stepToMiddle = nearIdx - len(traj)/2.0 38 | if abs(stepToMiddle) < 100.0: 39 | passMiddlePoint = True 40 | print('middle point passed') 41 | nearGoal = nearIdx >= len(traj)-50 42 | if nearGoal and passMiddlePoint: 43 | print('destination reached!') 44 | break 45 | # record states 46 | X.append(vehicle.state.X) 47 | Y.append(vehicle.state.Y) 48 | delta.append(vehicle.state.delta) 49 | xd.append(vehicle.state.xd) 50 | yd.append(vehicle.state.yd) 51 | phid.append(vehicle.state.phid) 52 | phi.append(vehicle.state.phi) 53 | deltad.append(command.deltad) 54 | F.append(command.F) 55 | minDist.append(disError) 56 | 57 | # to save the current states into a matrix 58 | currentState = vehicle.state 59 | cur_state_np = np.array([ 60 | [currentState.xd, 61 | currentState.yd, 62 | currentState.phid, 63 | currentState.delta, 64 | currentState.X, 65 | currentState.Y, 66 | currentState.phi]]) 67 | if i == 0: 68 | state_saved = cur_state_np.reshape((1, 7)) 69 | else: 70 | state_saved = np.concatenate((state_saved, cur_state_np.reshape((1, 7))), axis=0) 71 | 72 | showResult(traj,X,Y,delta,xd,yd,F,phi,phid,minDist) 73 | 74 | np.save('Actual_trajectory', state_saved) 75 | evaluation(minDist, traj, X, Y,4) 76 | 77 | plt.title('Trajectory of the Car') 78 | plt.plot(traj[:,0], traj[:,1], 'b', label = "reference trajectory") 79 | plt.plot(X[:],Y[:],'r', label = "actual trajectory") 80 | plt.xlabel("X-position (m)") 81 | plt.ylabel("Y-position (m)") 82 | plt.legend() 83 | plt.show() -------------------------------------------------------------------------------- /util.py: -------------------------------------------------------------------------------- 1 | # please do not change this file 2 | import numpy as np 3 | import matplotlib.pyplot as plt 4 | from BuggySimulator import * 5 | 6 | 7 | def wrap2pi(a): 8 | return (a + np.pi) % (2 * np.pi) - np.pi 9 | 10 | 11 | # initial your buggy 12 | def initail(traj,n): 13 | v = vehicle(vehicle.state(X=traj[n, 0], 14 | Y=traj[n, 1], 15 | xd=0.1, 16 | phi=np.arctan2(traj[n+1, 1] - traj[n, 1], traj[n+1, 0] - traj[n, 0]) 17 | )) 18 | return v 19 | 20 | 21 | # vectorized find_nearest point 22 | def closest_node(X, Y, traj): 23 | point = np.array([X, Y]) 24 | traj = np.asarray(traj) 25 | dist = point - traj 26 | dist_2 = np.sum(dist ** 2, axis=1) 27 | minIndex = np.argmin(dist_2) 28 | return np.sqrt(dist_2[minIndex]), minIndex 29 | 30 | 31 | # find the nearest point on the trajectory 32 | def find_nearest_points(X, Y, traj): 33 | dist_sq = np.zeros(traj.shape[0]) 34 | for j in range(traj.shape[0]): 35 | dist_sq[j] = (traj[j,0] - X)**2 + (traj[j,1] - Y)**2 36 | minDistSqure, minIdx = min((dist_sq[i], i) for i in range(len(dist_sq))) 37 | return np.sqrt(minDistSqure), minIdx 38 | 39 | 40 | # get the trajectory from .csv file 41 | def get_trajectory(filename): 42 | with open(filename) as f: 43 | lines = f.readlines() 44 | traj = np.zeros((len(lines), 2)) 45 | for idx, line in enumerate(lines): 46 | x = line.split(",") 47 | traj[idx, 0] = x[0] 48 | traj[idx, 1] = x[1] 49 | return traj 50 | 51 | 52 | # save the states 53 | def save_state(currentState): 54 | cur_state = [currentState.xd, 55 | currentState.yd, 56 | currentState.phid, 57 | currentState.delta, 58 | currentState.X, 59 | currentState.Y, 60 | currentState.phi] 61 | return cur_state 62 | 63 | 64 | # show result 65 | def showResult(traj,X,Y,delta,xd,yd,F,phi,phid,minDist): 66 | print('total steps: ', 0.05*len(X)) 67 | 68 | fig, axes = plt.subplots(nrows=4, ncols=2) 69 | fig.tight_layout() 70 | 71 | plt.subplot(421) 72 | plt.title('position') 73 | plt.plot(traj[:, 0], traj[:, 1], 'b') 74 | plt.plot(X, Y, 'r') 75 | 76 | plt.subplot(422) 77 | plt.title('delta') 78 | plt.plot(delta, 'r') 79 | 80 | plt.subplot(423) 81 | plt.title('xd') 82 | plt.plot(xd, 'r') 83 | 84 | plt.subplot(424) 85 | plt.title('yd') 86 | plt.plot(yd, 'r') 87 | 88 | plt.subplot(425) 89 | plt.title('phi') 90 | plt.plot(phi, 'r') 91 | 92 | plt.subplot(426) 93 | plt.title('phid') 94 | plt.plot(phid, 'r') 95 | 96 | plt.subplot(427) 97 | plt.title('minDist') 98 | plt.plot(minDist, 'r') 99 | 100 | plt.subplot(428) 101 | plt.title('F') 102 | plt.plot(F, 'r') 103 | 104 | avgDist = sum(minDist) / len(minDist) 105 | print('maxMinDist: ', max(minDist)) 106 | print('avgMinDist: ', avgDist) 107 | plt.show() 108 | 109 | 110 | 111 | if __name__ == "__main__": 112 | pass --------------------------------------------------------------------------------