├── .gitignore ├── Media ├── mpc_test_lateral_model.png ├── mpc_test_long_model.png └── t-spline_representation.png ├── README.md ├── mpc_test_lateral_model.py ├── mpc_test_long_model.py ├── rbs_mpc_py └── rbs_mpc.py ├── rbs_spline_module └── spline_module.py └── spline_generation.py /.gitignore: -------------------------------------------------------------------------------- 1 | __pycache__/ 2 | -------------------------------------------------------------------------------- /Media/mpc_test_lateral_model.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/navil2000/ITSC2023-MPC-and-Splines/6f835b0d3845bba83bec1190cb567251a89e08ef/Media/mpc_test_lateral_model.png -------------------------------------------------------------------------------- /Media/mpc_test_long_model.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/navil2000/ITSC2023-MPC-and-Splines/6f835b0d3845bba83bec1190cb567251a89e08ef/Media/mpc_test_long_model.png -------------------------------------------------------------------------------- /Media/t-spline_representation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/navil2000/ITSC2023-MPC-and-Splines/6f835b0d3845bba83bec1190cb567251a89e08ef/Media/t-spline_representation.png -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ITSC2023-MPC-and-Splines 2 | 3 | This repository houses the essential code components required to implement the method proposed by the author in the article presented at the ITSC2023 conference in Bilbao, titled "Hybrid MPC and Spline-based Controller for Lane Change Maneuvers in Autonomous Vehicles." 4 | 5 | On one hand, it provides code for spline generation and its corresponding time parametrization. Traditionally, this task was carried out in C++; however, for this project, it has been migrated to Python, along with the controllers. 6 | 7 | On the other hand, you will find the MPC controller class. This coding has been developed by following the steps outlined in the examples of the OSQP framework. I highly recommend reading the framework's documentation for a deeper understanding (https://osqp.org/docs/examples/mpc.html) 8 | -------------------------------------------------------------------------------- /mpc_test_lateral_model.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | 4 | from rbs_mpc_py.rbs_mpc import rbs_mpc 5 | from scipy import sparse 6 | 7 | if __name__ == "__main__": 8 | 9 | # Simulation parameters 10 | nsim = 200 11 | Ts = 0.1 12 | # MPC Hyper-parameters 13 | q11 = 100000 14 | q22 = 100 15 | r = 0.1 16 | 17 | Q = sparse.diags([q11, q22]) 18 | R = r*sparse.eye(1) 19 | 20 | # Model matrices, constraints and more 21 | Ad = sparse.csc_matrix([[1, Ts], 22 | [0, 1]]) 23 | 24 | Bd = sparse.csc_matrix([[(Ts**2/2)], 25 | [ Ts]]) 26 | 27 | u0 = 0 28 | umin = np.array([-10]) 29 | umax = np.array([10]) 30 | x0 = np.array([0, 0]) 31 | xmin = np.array([-8, -8]) 32 | xmax = np.array([8, 8]) 33 | 34 | # State vector reference 35 | xr = np.array([0.0, 0.0]) 36 | 37 | # Prediction horizont 38 | N = 50 39 | 40 | # MPC object 41 | mpc_lateral = rbs_mpc(A=Ad,B=Bd, 42 | u0=u0,umin=umin,umax=umax, 43 | x0=x0,xmin=xmin,xmax=xmax, 44 | Q=Q,R=R,N=N,xr=xr) 45 | 46 | # Lateral deviation reference 47 | de_ref = 0 48 | 49 | # Some list for plots 50 | x_list = [] 51 | r_list = [] 52 | 53 | for i in range(nsim): 54 | 55 | # Step 70 -> Change reference 56 | if i > 70 and i < 140: 57 | de_ref = 3.5 58 | else: 59 | de_ref = 0 60 | 61 | # Update lateral mpc reference 62 | mpc_lateral.set_x_ref(np.array([de_ref, 0.0])) 63 | # Solve the mpc 64 | ctrl = mpc_lateral.mpc_move(x0) 65 | # Plant simulation 66 | x0 = Ad.dot(x0) + Bd.dot(ctrl) 67 | 68 | r_list.append(de_ref) 69 | x_list.append(x0) 70 | 71 | tiempo = [Ts*i for i in range(nsim)] 72 | 73 | plt.plot(tiempo, x_list) 74 | plt.plot(tiempo, r_list) 75 | plt.ylabel(f'de') 76 | plt.xlabel(f'time') 77 | plt.legend([f'de', f'v_lat', f'Reference']) 78 | plt.title(f'q11 = {q11}, q22 = {q22}, r = {r}') 79 | plt.savefig("./Media/mpc_test_lateral_model.png") 80 | plt.show() 81 | -------------------------------------------------------------------------------- /mpc_test_long_model.py: -------------------------------------------------------------------------------- 1 | from rbs_mpc_py.rbs_mpc import rbs_mpc 2 | import numpy as np 3 | from scipy import sparse 4 | import matplotlib.pyplot as plt 5 | 6 | if __name__ == "__main__": 7 | 8 | # Simulation parameters 9 | Ts = 0.1 10 | nsim = 500 11 | 12 | # Model matrices, constraints and more 13 | Ad = sparse.csc_matrix([[1, Ts, (Ts**2/2)], 14 | [0, 1, Ts], 15 | [0, 0, 1]]) 16 | 17 | Bd = sparse.csc_matrix([[(Ts**3/6)], 18 | [(Ts**2/2)], 19 | [Ts]]) 20 | 21 | u0 = 0 22 | umin = np.array([-10]) 23 | umax = np.array([10]) 24 | 25 | x0 = np.array([5, 0, 0]) 26 | xmin = np.array([-8, -2, -8]) 27 | xmax = np.array([8, 2, 8]) 28 | 29 | xr = np.array([-2, 0.0, 0.0]) 30 | 31 | # MPC Hyper-parameters 32 | Q = sparse.diags([1000, 5000, 1000]) 33 | R = 500*sparse.eye(1) 34 | 35 | # Prediction horizont 36 | N = 50 37 | 38 | # MPC object 39 | mpc_lateral = rbs_mpc(A=Ad,B=Bd, 40 | u0=u0,umin=umin,umax=umax, 41 | x0=x0,xmin=xmin,xmax=xmax, 42 | Q=Q,R=R,N=N,xr=xr) 43 | 44 | # Some list for plots 45 | x_list = [] 46 | r_list = [] 47 | 48 | for _ in range(nsim): 49 | # Solve the mpc 50 | ctrl = mpc_lateral.mpc_move(x0) 51 | # Plant simulation 52 | x0 = Ad.dot(x0) + Bd.dot(ctrl) 53 | 54 | r_list.append(xr[0]) 55 | x_list.append(x0) 56 | 57 | tiempo = [Ts*i for i in range(nsim)] 58 | 59 | plt.figure(figsize=(10, 6)) 60 | plt.plot(tiempo, x_list) 61 | plt.plot(tiempo, r_list, color="purple") 62 | plt.ylabel(f'de') 63 | plt.xlabel(f'tiempo') 64 | plt.legend([f'd_lon', f'v_lon', f'a_lon', f'Reference']) 65 | plt.title(f'Q = {Q.todense()} \n- R = {R.todense()}') 66 | plt.savefig("./Media/mpc_test_long_model.png") 67 | plt.show() 68 | -------------------------------------------------------------------------------- /rbs_mpc_py/rbs_mpc.py: -------------------------------------------------------------------------------- 1 | # Importamos las librerías necesarias. 2 | import osqp 3 | import numpy as np 4 | import scipy as sp 5 | from scipy import sparse 6 | 7 | class rbs_mpc: 8 | def __init__(self, A, B, u0, umin, umax, x0, xmin, xmax, Q, R, N, xr): 9 | 10 | # Recogemos las variables 11 | self.A = A 12 | self.B = B 13 | self.u0 = u0 14 | self.umin = umin 15 | self.umax = umax 16 | self.x0 = x0 17 | self.xmin = xmin 18 | self.xmax = xmax 19 | self.Q = Q 20 | self.QN = self.Q 21 | self.R = R 22 | self.N = N 23 | self.xr = xr 24 | 25 | #Comenzamos a configurar el MPC 26 | 27 | # Definimos las dimensiones de nuestro problema en función a B 28 | [self.nx, self.nu] = self.B.shape 29 | 30 | # Expadimos el sistema de entrada para ajustarlo a un MPC 31 | # tal que X = (x(0),...,x(N),u(0),...,u(N-1)) 32 | 33 | # Objetivo cuadrático 34 | self.P = sparse.block_diag([sparse.kron(sparse.eye(N), Q), self.QN, 35 | sparse.kron(sparse.eye(N), R)], format='csc') 36 | # Objetivo lineal 37 | self.q = np.hstack([np.kron(np.ones(N), -Q.dot(xr)), -self.QN.dot(xr), 38 | np.zeros(N*self.nu)]) 39 | 40 | # Dinámica Lineal 41 | self.Ax = sparse.kron(sparse.eye(N+1),-sparse.eye(self.nx)) + sparse.kron(sparse.eye(N+1, k=-1), A) 42 | self.Bu = sparse.kron(sparse.vstack([sparse.csc_matrix((1, N)), sparse.eye(N)]), B) 43 | self.Aeq = sparse.hstack([self.Ax, self.Bu]) 44 | self.leq = np.hstack([-x0, np.zeros(N*self.nx)]) 45 | self.ueq = self.leq 46 | # Redefinimos las restricciones 47 | self.Aineq = sparse.eye((N+1)*self.nx + N*self.nu) 48 | self.lineq = np.hstack([np.kron(np.ones(N+1), xmin), np.kron(np.ones(N), umin)]) 49 | self.uineq = np.hstack([np.kron(np.ones(N+1), xmax), np.kron(np.ones(N), umax)]) 50 | self.Af = sparse.vstack([self.Aeq, self.Aineq], format='csc') 51 | self.l = np.hstack([self.leq, self.lineq]) 52 | self.u = np.hstack([self.ueq, self.uineq]) 53 | 54 | # Creamos el objeto de la clase problema OSQP y lo inicializamos 55 | self.problema = osqp.OSQP() 56 | self.problema.setup(P=self.P,q=self.q,A=self.Af,l=self.l,u=self.u, warm_start=True, verbose=False) 57 | 58 | def set_u_limits(self, umin, umax): 59 | self.umax = umax 60 | self.umin = umin 61 | self.lineq = np.hstack([np.kron(np.ones(self.N+1), self.xmin), np.kron(np.ones(self.N), self.umin)]) 62 | self.uineq = np.hstack([np.kron(np.ones(self.N+1), self.xmax), np.kron(np.ones(self.N), self.umax)]) 63 | self.l = np.hstack([self.leq, self.lineq]) 64 | self.u = np.hstack([self.ueq, self.uineq]) 65 | self.problema.update(l=self.l, u=self.u) 66 | 67 | def set_x_limits(self, xmin, xmax): 68 | self.xmax = xmax 69 | self.xmin = xmin 70 | self.lineq = np.hstack([np.kron(np.ones(self.N+1), self.xmin), np.kron(np.ones(self.N), self.umin)]) 71 | self.uineq = np.hstack([np.kron(np.ones(self.N+1), self.xmax), np.kron(np.ones(self.N), self.umax)]) 72 | self.l = np.hstack([self.leq, self.lineq]) 73 | self.u = np.hstack([self.ueq, self.uineq]) 74 | self.problema.update(l=self.l, u=self.u) 75 | 76 | def set_x_ref(self, xref): 77 | self.xr = xref 78 | self.q = np.hstack([np.kron(np.ones(self.N), -self.Q.dot(self.xr)), -self.QN.dot(self.xr), 79 | np.zeros(self.N*self.nu)]) 80 | self.problema.update(q=self.q) 81 | 82 | def mpc_move(self, x0): 83 | 84 | # Feedback de la planta para actualizar el problema. 85 | self.x0 = x0 86 | self.l[:self.nx] = -self.x0 87 | self.u[:self.nx] = -self.x0 88 | self.problema.update(l=self.l, u=self.u) 89 | 90 | # Resolvemos el sistema 91 | self.res = self.problema.solve() 92 | 93 | # Check de que hayamos podido solucionar el problema 94 | if self.res.info.status != 'solved': 95 | print('No solution found for the given conditions') 96 | 97 | return self.res.x[-self.N*self.nu:-(self.N-1)*self.nu] -------------------------------------------------------------------------------- /rbs_spline_module/spline_module.py: -------------------------------------------------------------------------------- 1 | from math import cos, sin, pi, sqrt 2 | import matplotlib.pyplot as plt 3 | import numpy as np 4 | from mpl_toolkits.mplot3d import Axes3D 5 | 6 | # Define a class for Rbs2_ControlPose for clarity 7 | class Rbs2_ControlPose: 8 | def __init__(self, x, y, o): 9 | self.x = x 10 | self.y = y 11 | self.o = o 12 | 13 | # Spline 14 | def n_spline(waypoints_path, nu, lateral_offsets=[]): 15 | ''' 16 | Input: 17 | · waypoints: (X,Y,O). 18 | · nu: smoothing factor 19 | · lateral_offsets: list of lateral offsets for each waypoint. 20 | If it is None, no lateral offsets are applied (offset = 0). 21 | 22 | Output: 23 | · coeffs: List of tuples defining the polynomials. 24 | 25 | ''' 26 | waypoints = waypoints_path[:] 27 | offsets = lateral_offsets[:] 28 | 29 | m = len(waypoints) 30 | lam = [] 31 | deta = [] 32 | D = [] 33 | coefs = [] 34 | 35 | if len(offsets) != 0: 36 | for offset, waypoint in zip(lateral_offsets, waypoints): 37 | waypoint.x = waypoint.x + offset*sin(waypoint.o) 38 | waypoint.y = waypoint.y - offset*cos(waypoint.o) 39 | 40 | # X(u) # 41 | lam.append(0) 42 | deta.append(nu*cos(waypoints[0].o)) 43 | 44 | for i in range(1, m - 1): 45 | lam.append(1 / (4 - lam[i - 1])) 46 | deta.append((3 * (waypoints[i + 1].x - waypoints[i - 1].x) - deta[i - 1]) * lam[i]) 47 | deta.append(nu*cos(waypoints[-1].o)) 48 | 49 | for _ in range(m): 50 | D.append(0) 51 | 52 | D[m - 1] = deta[m - 1] 53 | 54 | for i in range(m - 2, 0, -1): 55 | D[i] = deta[i] - lam[i] * D[i + 1] 56 | 57 | for i in range(m - 1): 58 | coef = [ 59 | waypoints[i].x, # Add lateral offset 60 | D[i], 61 | 3 * (waypoints[i + 1].x - waypoints[i].x) - 2 * D[i] - D[i + 1], 62 | 2 * (waypoints[i].x - waypoints[i + 1].x) + D[i] + D[i + 1], 63 | ] 64 | coefs.append(coef) 65 | 66 | # Y(u) # 67 | lam[0] = 0 68 | deta[0] = nu*sin(waypoints[0].o) 69 | 70 | for i in range(1, m - 1): 71 | lam[i] = 1 / (4 - lam[i - 1]) 72 | deta[i] = ((3 * (waypoints[i + 1].y - waypoints[i - 1].y) - deta[i - 1]) * lam[i]) 73 | deta[-1] = nu * sin(waypoints[-1].o) 74 | 75 | D[m - 1] = deta[m - 1] 76 | 77 | for i in range(m - 2, -1, -1): 78 | D[i] = deta[i] - lam[i] * D[i + 1] 79 | 80 | for i in range(m - 1): 81 | coef = [ 82 | waypoints[i].y, # Add lateral offset 83 | D[i], 84 | 3 * (waypoints[i + 1].y - waypoints[i].y) - 2 * D[i] - D[i + 1], 85 | 2 * (waypoints[i].y - waypoints[i + 1].y) + D[i] + D[i + 1], 86 | ] 87 | coefs[i].extend(coef) 88 | 89 | return coefs 90 | 91 | # T-spline 92 | def t_spline(waypoints_path, vel, coeffs_spline): 93 | ''' 94 | Generates time-parameterized splines. 95 | Input: 96 | · waypoints: (X,Y,O). 97 | · nu: smoothing factor 98 | · vel: object/vehicle velocity 99 | 100 | Output: 101 | · t: list with the time boundaries of each segment 102 | ·t_coeffs: coefficients of the parameterized polynomial 103 | ''' 104 | 105 | # Aux variables 106 | waypoints = waypoints_path[:] 107 | coeffs = coeffs_spline[:] 108 | m = len(waypoints) 109 | t = [] 110 | inc = [] 111 | t_coeffs = [] 112 | 113 | # Euclidean distances between waypoints 114 | dist = [sqrt((waypoints[i+1].x-waypoints[i].x)**2 115 | + (waypoints[i+1].y-waypoints[i].y)**2) for i in range(m-1)] 116 | 117 | # Vector of time boundaries 118 | k = 1 / vel 119 | t.append(0) 120 | for i in range(1, m): 121 | t.append(k * dist[i-1] + t[i-1]) 122 | 123 | # Time base and variation 124 | b=t 125 | for i in range(m-1): 126 | inc.append(t[i+1]-t[i]) 127 | 128 | # T-Coeffs 129 | for i in range(m-1): 130 | # For each segment 131 | coef = [coeffs[i][0] - (b[i]**3 * coeffs[i][3] / inc[i]**3) + (b[i]**2 * coeffs[i][2] / inc[i]**2) - (b[i] * coeffs[i][1] / inc[i]), 132 | (coeffs[i][1] / inc[i]) + (3 * b[i]**2 * coeffs[i][3] / inc[i]**3) - (2 * b[i] * coeffs[i][2] / inc[i]**2), 133 | (-3 * b[i] * coeffs[i][3] / inc[i]**3) + (coeffs[i][2] / inc[i]**2), 134 | coeffs[i][3] / inc[i]**3, 135 | coeffs[i][4] - (b[i]**3 * coeffs[i][7] / inc[i]**3) + (b[i]**2 * coeffs[i][6] / inc[i]**2) - (b[i] * coeffs[i][5] / inc[i]), 136 | (coeffs[i][5] / inc[i]) + (3 * b[i]**2 * coeffs[i][7] / inc[i]**3) - (2 * b[i] * coeffs[i][6] / inc[i]**2), 137 | (-3 * b[i] * coeffs[i][7] / inc[i]**3) + (coeffs[i][6] / inc[i]**2), 138 | coeffs[i][7] / inc[i]**3] 139 | 140 | t_coeffs.append(coef) 141 | 142 | return t, t_coeffs 143 | 144 | -------------------------------------------------------------------------------- /spline_generation.py: -------------------------------------------------------------------------------- 1 | from rbs_spline_module.spline_module import Rbs2_ControlPose, n_spline, t_spline 2 | import numpy as np 3 | from math import pi, sqrt 4 | import matplotlib.pyplot as plt 5 | 6 | if __name__ == "__main__": 7 | 8 | path = [ Rbs2_ControlPose(10, 40, -pi/4), 9 | Rbs2_ControlPose(20, 20, 0), 10 | Rbs2_ControlPose(40., 50, pi/4), 11 | Rbs2_ControlPose(60, 30, pi+pi/4), 12 | Rbs2_ControlPose(40, 10, pi+pi/4)] 13 | 14 | t = [] 15 | t_coeffs = [] 16 | 17 | # Generate spline, and t-spline 18 | nu = 1 19 | u_coeffs = n_spline(path, nu) 20 | t, t_coeffs = t_spline(path, 1.0, u_coeffs) 21 | t2, t_coeffs2 = t_spline(path, 2.0, u_coeffs) 22 | 23 | # Some lists for plots 24 | Xt = [] 25 | Yt = [] 26 | Tt = [] 27 | Xt2 = [] 28 | Yt2 = [] 29 | Tt2 = [] 30 | Xu = [] 31 | Yu = [] 32 | 33 | # Spline data extraction for plots 34 | for coef in u_coeffs: 35 | ax, bx, cx, dx, ay, by, cy, dy = coef 36 | u = np.linspace(0, 1, 100) 37 | x_spline = ax + bx*u + cx*u**2 + dx*u**3 38 | y_spline = ay + by*u + cy*u**2 + dy*u**3 39 | 40 | Xu.extend(x_spline) 41 | Yu.extend(y_spline) 42 | 43 | # T-Spline data extraction for plots 44 | for i, coef in enumerate(t_coeffs2): 45 | ax, bx, cx, dx, ay, by, cy, dy = coef 46 | t_segment = np.arange(t2[i], t2[i+1], 0.05) 47 | x_spline = ax + bx*t_segment + cx*t_segment**2 + dx* t_segment**3 48 | y_spline = ay + by*t_segment + cy*t_segment**2 + dy* t_segment**3 49 | 50 | Xt2.extend(x_spline) 51 | Yt2.extend(y_spline) 52 | Tt2.extend(t_segment) 53 | 54 | # T-Spline data extraction for plots 55 | for i, coef in enumerate(t_coeffs): 56 | ax, bx, cx, dx, ay, by, cy, dy = coef 57 | t_segment = np.arange(t[i], t[i+1], 0.05) 58 | x_spline = ax + bx*t_segment + cx*t_segment**2 + dx* t_segment**3 59 | y_spline = ay + by*t_segment + cy*t_segment**2 + dy* t_segment**3 60 | 61 | Xt.extend(x_spline) 62 | Yt.extend(y_spline) 63 | Tt.extend(t_segment) 64 | 65 | # 3D Graph 66 | fig = plt.figure() 67 | ax = fig.add_subplot(111, projection='3d') 68 | 69 | # Add wayoints 70 | ax.scatter([p.x for p in path], [p.y for p in path], color='purple', label='Waypoints') 71 | 72 | # Add 3D representation for temporal splines and its projection in the plain X-Y 73 | ax.plot(Xt, Yt, Tt, color='green', label='T-spline Path (v=1)') 74 | ax.plot(Xt2, Yt2, Tt2, color='grey', label='T-spline Path (v=2)') 75 | ax.plot(Xu, Yu, color='red', label='Spline') 76 | 77 | # Add initial and last point of the path 78 | ax.scatter(Xt[0], Yt[0], Tt[0], color='blue') 79 | ax.scatter(Xt[-1], Yt[-1], Tt[-1], color='red') 80 | ax.scatter(Xt2[0], Yt2[0], Tt2[0], color='blue') 81 | ax.scatter(Xt2[-1], Yt2[-1], Tt2[-1], color='red') 82 | 83 | # Axis label and legend 84 | ax.set_xlabel('X') 85 | ax.set_ylabel('Y') 86 | ax.set_zlabel('Time') 87 | ax.legend() 88 | plt.grid() 89 | 90 | # Save graph and show it 91 | plt.savefig("./Media/t-spline_representation.png") 92 | plt.show() --------------------------------------------------------------------------------