def long_term_MPC(ego_car: Car, route_xs, route_ys, dt): assert len(route_xs) == len(route_ys) N = len(route_xs) ego_car_x, ego_car_y, ego_car_v, ego_car_theta, ego_car_a, ego_car_steer = ego_car.get_car_state( ) x_init = [ego_car_x] * N y_init = [ego_car_y] * N theta_init = [ego_car_theta] * N v_init = [ego_car_v] * N steer_init = [ego_car_steer] * (N - 1) a_init = [MAX_a] * (N - 1) initial_value = x_init + y_init + theta_init + v_init + \ steer_init + a_init # vars to be optimize x = SX.sym('x', N) y = SX.sym('y', N) theta = SX.sym('theta', N) v = SX.sym('v', N) steer = SX.sym('steer', N - 1) a = SX.sym('a', N - 1) all_vars = vertcat(x, y, theta, v, steer, a) # vars upper bound ub_constrains_x = np.array([ego_car_x] + [INF] * (N - 1)) ub_constrains_y = np.array([ego_car_y] + [INF] * (N - 1)) ub_constrains_theta = np.array([ego_car_theta] + [INF] * (N - 1)) ub_constrains_v = np.array([ego_car_v] + [MAX_v] * (N - 1)) ub_constrains_steer = np.array([ego_car_steer] + [MAX_steer] * (N - 2)) ub_constrains_a = np.array([ego_car_a] + [MAX_a] * (N - 2)) ub_constrains_vars = np.hstack([ ub_constrains_x, ub_constrains_y, ub_constrains_theta, ub_constrains_v, ub_constrains_steer, ub_constrains_a ]) # vars lower bound lb_constrains_x = np.array([ego_car_x] + [-INF] * (N - 1)) lb_constrains_y = np.array([ego_car_y] + [-INF] * (N - 1)) lb_constrains_theta = np.array([ego_car_theta] + [-INF] * (N - 1)) lb_constrains_v = np.array([ego_car_v] + [-MIN_v] * (N - 1)) lb_constrains_steer = np.array([ego_car_steer] + [MIN_steer] * (N - 2)) lb_constrains_a = np.array([ego_car_a] + [MIN_a] * (N - 2)) lb_constrains_vars = np.hstack([ lb_constrains_x, lb_constrains_y, lb_constrains_theta, lb_constrains_v, lb_constrains_steer, lb_constrains_a ]) # define constrain function g (variables update equation) x_constrain = SX.sym('x_constrain', N - 1) y_constrain = SX.sym('y_constrain', N - 1) theta_constrain = SX.sym('theta_constrain', N - 1) v_constrain = SX.sym('v_constrain', N - 1) SCALE = 0.002 for i in range(N - 1): theta_diff = atan(tan(steer[i]) / 2) * v[i] * dt * SCALE # theta_diff = steer[i] * dt x_constrain[i] = x[i + 1] - (x[i] + v[i] * dt * np.cos(theta[i])) y_constrain[i] = y[i + 1] - (y[i] + v[i] * dt * np.sin(theta[i])) theta_constrain[i] = theta[i + 1] - (theta[i] - theta_diff) v_constrain[i] = v[i + 1] - (v[i] + a[i] * dt) all_constrain = vertcat(x_constrain, y_constrain, theta_constrain, v_constrain) ub_constrains_g = np.zeros([4 * (N - 1)]) lb_constrains_g = np.zeros([4 * (N - 1)]) # define cost function f cost = 0 for i in range(N): # deviation cost += 20 / N**3 * (N - i)**4 * (x[i] - route_xs[i])**2 cost += 20 / N**3 * (N - i)**4 * (y[i] - route_ys[i])**2 # control cost if i < N - 2: cost += 5 * N * steer[i]**2 cost += 0.01 * N * a[i]**2 cost += 20 * N * (steer[i + 1] - steer[i])**2 # cost += 0.1 * N * (a[i + 1] - a[i]) ** 2 nlp = {'x': all_vars, 'f': cost, 'g': all_constrain} S = nlpsol('S', 'ipopt', nlp, { "print_time": False, "ipopt": { "print_level": 0 } }) result = S(x0=initial_value, lbg=lb_constrains_g, ubg=ub_constrains_g, lbx=lb_constrains_vars, ubx=ub_constrains_vars) # def print_result(): # print('route_x', [i[0] for i in route]) # print('x', result['x'][0:N]) # print('route_y', [i[1] for i in route]) # print('y', result['x'][N:2 * N]) # # print('theta', result['x'][2 * N:3 * N]) # print('v', result['x'][3 * N:4 * N]) # print('vx', result['x'][3 * N:4 * N] * np.cos(result['x'][2 * N:3 * N])) # print('vy', result['x'][3 * N:4 * N] * np.sin(result['x'][2 * N:3 * N])) # # print('steer', result['x'][4 * N:5 * N - 1]) # print('a', result['x'][5 * N - 1:6 * N - 2]) # print_result() xs = result['x'][0:N].elements() ys = result['x'][N:2 * N].elements() steer = float(result['x'][4 * N + 1]) a = float(result['x'][5 * N]) a = a / MAX_a if a > 0: control = Control(steer, a / 10, 0) else: control = Control(steer, 0, -a) # controls = [] # for a, steer in zip(result['x'][5 * N - 1:6 * N - 2].elements(), result['x'][4 * N:5 * N - 1].elements()): # # steer = float(result['x'][4 * N + 1]) # # a = float(result['x'][5 * N]) # a = a / MAX_a # if a > 0: # controls.append(Control(steer, a / 10, 0)) # else: # controls.append(Control(steer, 0, -a)) return control, xs, ys
def short_term_MPC(ego_car: Car, route_xs, route_ys, dt, verbose=False): assert len(route_xs) == len(route_ys) N = len(route_xs) ego_car_x, ego_car_y, ego_car_v, ego_car_theta, ego_car_a, ego_car_steer = ego_car.get_car_state( ) x_init = [ego_car_x] * N y_init = [ego_car_y] * N theta_init = [ego_car_theta] * N v_init = [ego_car_v] * N steer_init = [ego_car_steer] * (N - 1) a_init = [MAX_a] * (N - 1) initial_value = x_init + y_init + theta_init + v_init + \ steer_init + a_init # vars to be optimize x = SX.sym('x', N) y = SX.sym('y', N) theta = SX.sym('theta', N) v = SX.sym('v', N) steer = SX.sym('steer', N - 1) a = SX.sym('a', N - 1) all_vars = vertcat(x, y, theta, v, steer, a) # vars upper bound ub_constrains_x = np.array([ego_car_x] + [INF] * (N - 1)) ub_constrains_y = np.array([ego_car_y] + [INF] * (N - 1)) ub_constrains_theta = np.array([ego_car_theta] + [INF] * (N - 1)) ub_constrains_v = np.array([ego_car_v] + [MAX_v] * (N - 1)) ub_constrains_steer = np.array([ego_car_steer] + [MAX_steer] * (N - 2)) ub_constrains_a = np.array([ego_car_a] + [MAX_a] * (N - 2)) ub_constrains_vars = np.hstack([ ub_constrains_x, ub_constrains_y, ub_constrains_theta, ub_constrains_v, ub_constrains_steer, ub_constrains_a ]) # vars lower bound lb_constrains_x = np.array([ego_car_x] + [-INF] * (N - 1)) lb_constrains_y = np.array([ego_car_y] + [-INF] * (N - 1)) lb_constrains_theta = np.array([ego_car_theta] + [-INF] * (N - 1)) lb_constrains_v = np.array([ego_car_v] + [-MIN_v] * (N - 1)) lb_constrains_steer = np.array([ego_car_steer] + [MIN_steer] * (N - 2)) lb_constrains_a = np.array([ego_car_a] + [MIN_a] * (N - 2)) lb_constrains_vars = np.hstack([ lb_constrains_x, lb_constrains_y, lb_constrains_theta, lb_constrains_v, lb_constrains_steer, lb_constrains_a ]) # define constrain function g (variables update equation) x_constrain = SX.sym('x_constrain', N - 1) y_constrain = SX.sym('y_constrain', N - 1) theta_constrain = SX.sym('theta_constrain', N - 1) v_constrain = SX.sym('v_constrain', N - 1) SCALE = 0.002 for i in range(N - 1): # theta_diff = atan(tan(steer[i]) / 2) * v[i] * dt * SCALE theta_diff = steer[i] * v[i] * dt * SCALE x_constrain[i] = x[i + 1] - (x[i] + v[i] * dt * np.cos(theta[i])) y_constrain[i] = y[i + 1] - (y[i] + v[i] * dt * np.sin(theta[i])) theta_constrain[i] = theta[i + 1] - (theta[i] - theta_diff) v_constrain[i] = v[i + 1] - (v[i] + a[i] * dt) all_constrain = vertcat(x_constrain, y_constrain, theta_constrain, v_constrain) ub_constrains_g = np.zeros([4 * (N - 1)]) lb_constrains_g = np.zeros([4 * (N - 1)]) # define cost function f cost = 0 for i in range(N): # deviation cost += 20 * N * (x[i] - route_xs[i])**2 cost += 20 * N * (y[i] - route_ys[i])**2 if i < N - 2: cost += 1 * N * steer[i]**2 # cost += 0.01 * N * a[i] ** 2 cost += 50 * N * (steer[i + 1] - steer[i])**2 nlp = {'x': all_vars, 'f': cost, 'g': all_constrain} S = nlpsol('S', 'ipopt', nlp, { "print_time": False, "ipopt": { "print_level": 0 } }) result = S(x0=initial_value, lbg=lb_constrains_g, ubg=ub_constrains_g, lbx=lb_constrains_vars, ubx=ub_constrains_vars) steer = float(result['x'][4 * N + 1]) a = float(result['x'][5 * N]) a = a / MAX_a if a > 0: action = Control(steer, a / 10, 0) else: action = Control(steer, 0, -a) if verbose: xs = result['x'][0:N].elements() ys = result['x'][N:2 * N].elements() pprint(xs) pprint(ys) return action, None, None