How to define the expression for the terminal and running costs P and L for nonlinear MPC controller in the form of the quadratic one and based on x_model = ca.MX.sym('xm', (Dim_state, 1))
and u_model = ca.MX.sym('um', (Dim_ctrl, 1)), and based on the following requirements:

image.png
Image transcription text

Car Overtaking In this task. you will design a nonlinear MP0 on a
kinematic bicycle carmodel to overtake the leading vehicle. You
will need to complete the nmpc_controllen function to ...
Show more
image.png
Image transcription text

X Controller design: You ara supposed to design an MPC
controller of the following form to takeover the leading vehicle
with desired velocity and go back to the same lane as t...
Show more
image.png
Image transcription text

Constraints: 1 Cons1: Collision avoidance: We consider a
elliposoidal safety set for the vehicle when overtaking the car
(#)2+ (2)2 -120, We assign rx = 2 m, ry = 30 m. Cons1: ...
Show more
image.png
Image transcription text

Cons4: Input bounds The input is within the bounds: —1[)ml'52 =
"min 3 a g a".ax = 4 math:2 —fl.6rad : 5min g 6 3 5mm : 0.6rad
Submission detail Controller input The autograder will...
Show more
And the relvant codes are:

import casadi as ca
import numpy as np
import numpy.matlib
import matplotlib.pyplot as plt
import time

def nmpc_controller():
    # Declare simulation constants
    T = 4 # TODO: You are supposed to design the planning horizon
    N = 40 # TODO  You are supposed to design the planning horizon
    h = 0.1 # TODO: What is the time interval for simulation?

    # system dimensions
    Dim_state = 4 # TODO
    Dim_ctrl  = 2 # TODO

    # additional parameters
    x_init = ca.MX.sym('x_init', (Dim_state, 1)) # initial condition, # the state should be position to the leader car
    v_leader = ca.MX.sym('v_leader',(2, 1))      # leader car's velocity w.r.t ego car
    v_des = ca.MX.sym('v_des')
    delta_last = ca.MX.sym('delta_last')
    par = ca.vertcat(x_init, v_leader, v_des, delta_last)
   
    # Continuous dynamics model
    x_model = ca.MX.sym('xm', (Dim_state, 1))
    u_model = ca.MX.sym('um', (Dim_ctrl, 1))

    L_f = 1.0 # Car parameters, do not change
    L_r = 1.0 # Car parameters, do not change

    beta = ca.atan(L_r / (L_r + L_f) * ca.atan(u_model[1])) # TODO

    xdot = ca.vertcat(x_model[3] * ca.cos(x_model[2] + beta) - v_leader[0],
                      x_model[3] * ca.sin(x_model[2] + beta) - v_leader[1],
                      x_model[3] / L_r * ca.sin(beta),
                      u_model[0]) # TODO
 
    # Discrete time dynmamics model
    Fun_dynmaics_dt = ca.Function('f_dt', [x_model, u_model, par], [xdot * h + x_model]) # TODO
   
    # Declare model variables, note the dimension
    x = ca.MX.sym('x', (Dim_state, N+1)) # TODO
    u = ca.MX.sym('u', (Dim_ctrl, N)) # TODO

    # Keep in the same lane and take over it while maintaing a high speed, cost function
    P = x[2,N]**2 + x[3,N]**2 + x[4,N]**2 + x[0,N]**2 # TODO
    L = sum([x[2,i]**2 + x[3,i]**2 + x[4,i]**2 + x[0,i]**2 + u[:,i]**2  for i in range(N)]) # TODO

    Fun_cost_terminal = ca.Function('P', [x_model, par], [P])
    Fun_cost_running = ca.Function('Q', [x_model, u_model, par], [L])

    # state and control constraints
    state_ub = ca.DM([2, 3, np.inf, 4]) # TODO
    state_lb = ca.DM([-2, -1, -np.inf, 0]) # TODO
    ctrl_ub  = np.array([4, 0.6]) # TODO
    ctrl_lb  = np.array([-10, -0.6]) # TODO
   
    # upper bound and lower bound
    ub_x = np.matlib.repmat(state_ub, N + 1, 1)
    lb_x = np.matlib.repmat(state_lb, N + 1, 1)

    ub_u = np.matlib.repmat(ctrl_ub, N, 1)
    lb_u = np.matlib.repmat(ctrl_lb, N, 1)

    ub_var = np.concatenate((ub_u.reshape((Dim_ctrl * N, 1)), ub_x.reshape((Dim_state * (N+1), 1))))
    lb_var = np.concatenate((lb_u.reshape((Dim_ctrl * N, 1)), lb_x.reshape((Dim_state * (N+1), 1))))

    # dynamics constraints: x[k+1] = x[k] + f(x[k], u[k]) * dt
    cons_dynamics = []
    ub_dynamics = np.zeros((Dim_state * N, 1))
    lb_dynamics = np.zeros((Dim_state * N, 1))
    for k in range(N):
        Fx = Fun_dynmaics_dt(x[:, k], u[:, k], par)
        cons_dynamics.append(x[:, k+1] - Fx)
        ub_dynamics = np.concatenate((ub_dynamics, np.zeros((Dim_state,1))))
        lb_dynamics = np.concatenate((lb_dynamics, np.zeros((Dim_state,1)))) # TODO


    # state constraints: G(x) <= 0
    cons_state = []
    for k in range(N):
        #### collision avoidance:
        # TODO
        r_x = 30
        r_y = 2
        cons_state.append((x[0, k] / r_x)**2 + (x[1, k] / r_y)**2 - 1)

        #### Maximum lateral acceleration ####
        dx = (x[:, k+1] - x[:, k]) / h
        ay = x[3, k] * (x[2, k+1]-x[2, k]) / h # TODO: Compute the lateral acc using the hints
       
        gmu = (0.5 * 0.6 * 9.81) # TODO
        cons_state.append(ay - gmu) # TODO
        cons_state.append(-ay - gmu) # TODO


        #### lane keeping ####
        cons_state.append(x[1, k] - 3)
        cons_state.append(-x[1, k] + 1)

        #### steering rate ####
        if k >= 1:
            d_delta = (u[1, k] - u[1, k-1]) / h
            cons_state.append(d_delta - 0.6) # TODO
            cons_state.append(-d_delta - 0.6) # TODO
        else:
            d_delta = (u[1, k] - delta_last) / h # TODO, for the first input, given d_last from param
            cons_state.append(d_delta - 0.6) # TODO
            cons_state.append(-d_delta - 0.6) # TODO
 
    ub_state_cons = np.zeros((len(cons_state), 1))
    lb_state_cons = np.zeros((len(cons_state), 1)) - 1e5

    # cost function: # NOTE: You can also hard code everything here
    J = Fun_cost_terminal(x[:, -1], par)
    for k in range(N):
        J = J + Fun_cost_running(x[:, k], u[:, k], par)

    # initial condition as parameters
    cons_init = [x[:, 0] - x_init]
    ub_init_cons = np.zeros((Dim_state, 1))
    lb_init_cons = np.zeros((Dim_state, 1))
   
    # Define variables for NLP solver
    vars_NLP   = ca.vertcat(u.reshape((Dim_ctrl * N, 1)), x.reshape((Dim_state * (N+1), 1)))
    cons_NLP = cons_dynamics + cons_state + cons_init
    cons_NLP = ca.vertcat(*cons_NLP)
    lb_cons = np.concatenate((lb_dynamics, lb_state_cons, lb_init_cons))
    ub_cons = np.concatenate((ub_dynamics, ub_state_cons, ub_init_cons))
   
    # Define an NLP solver
    prob = {"x": vars_NLP, "p":par, "f": J, "g":cons_NLP}
    return prob, N, vars_NLP.shape[0], cons_NLP.shape[0], par.shape[0], lb_var, ub_var, lb_cons, ub_cons
Reference codes are:

import casadi as ca
import numpy as np
import numpy.matlib
import matplotlib.pyplot as plt
import time
#from nmpc_takeover_gsi import *
from nmpc_takeover_student import

def eval_controller(par_init: np.ndarray):
    '''
    par_init: [x, y, yaw, v, v_x_leader, v_y_leader, v_x_desired]
        x: x distance between our car and leader car
        y: y distance between our car and leader car
        yaw: yaw angle of our car
        v: velocity of our car
        v_x_leader: x velocity of leader car
        v_y_leader: y velocity of leader car
        v_x_desired: desired takeover x velocity of our car
    '''
   
    # We define the default evaluation rate and other constants here
    h = 0.1
    N_sim = int(np.ceil(17/ h))
    Dim_state = 4
    Dim_ctrl  = 2
   
    # define some parameters
    x_init = ca.MX.sym('x_init', (Dim_state, 1)) # initial condition, the state should be position to the leader car
    v_leader = ca.MX.sym('v_leader',(2, 1))      # leader car's velocity
    v_des = ca.MX.sym('v_des')                   # desired speed of ego car
    delta_last = ca.MX.sym('delta_last')         # steering angle at last step
    par = ca.vertcat(x_init, v_leader, v_des, delta_last) # concatenate them
   
    # Continuous dynamics model
    x_model = ca.MX.sym('xm', (Dim_state, 1))
    u_model = ca.MX.sym('um', (Dim_ctrl, 1))
   
    L_f = 1.0
    L_r = 1.0

    beta = ca.atan(L_r / (L_r + L_f) * ca.atan(u_model[1]))

    xdot = ca.vertcat(x_model[3] * ca.cos(x_model[2] + beta) - v_leader[0],
                      x_model[3] * ca.sin(x_model[2] + beta) - v_leader[1],
                      x_model[3] / L_r * ca.sin(beta),
                      u_model[0])

    # Discretized dynamics model
    Fun_dynmaics_dt = ca.Function('f_dt', [x_model, u_model, par], [xdot * h + x_model])
   
    # student controller are constructed here:
    prob, N_mpc, n_x, n_g, n_p, lb_var, ub_var, lb_cons, ub_cons = nmpc_controller()
    # students are expected to provide
    # NLP problem, the problem size (n_x, n_g, n_p), horizon and bounds
   
    opts = {'ipopt.print_level': 3, 'print_time': 0} # , 'ipopt.sb': 'yes'}
    solver = ca.nlpsol('solver', 'ipopt', prob , opts)
   
    # our initial conditions, see if we want to change it ...
    state_0 = par_init[:4] #np.array([-80, 0.0, 0.15, 30.])
    d_last = 0
    # our initial conditions, see if we want to change it ...
   
    # logger of states
    xt = np.zeros((N_sim+1, Dim_state))
    ut = np.zeros((N_sim, Dim_ctrl))
   
    # place holder for warm start
    x0_nlp    = np.random.randn(n_x, 1) * 0.01 # np.zeros((n_x, 1))
    lamx0_nlp = np.random.randn(n_x, 1) * 0.01 # np.zeros((n_x, 1))
    lamg0_nlp = np.random.randn(n_g, 1) * 0.01 # np.zeros((n_g, 1))

    xt[0, :] = state_0
   
    # main loop of simulation
    for k in range(N_sim):
        state_0 = xt[k, :]
       
        # the leader car's velocity and desired velocity will not change in the planning horizon
        par_nlp = np.concatenate((state_0, par_init[4:], np.array([d_last]))) # state_0 + [v_x_leader, v_y_leader, v_x_desired, d_last]

        sol = solver(x0=x0_nlp, lam_x0=lamx0_nlp, lam_g0=lamg0_nlp,
                     lbx=lb_var, ubx=ub_var, lbg=lb_cons, ubg=ub_cons, p = par_nlp)
       
        x0_nlp = sol["x"].full()
        lamx0_nlp = sol["lam_x"].full()
        lamg0_nlp = sol["lam_g"].full()
               
        ut[k, :] = np.squeeze(sol['x'].full()[0:Dim_ctrl])
        d_last = ut[k, 1]
       
        ut[k, 0] = np.clip(ut[k, 0], -10, 4)
        ut[k, 1] = np.clip(ut[k, 1], -0.6, 0.6)
       
        xkp1 = Fun_dynmaics_dt(xt[k, :], ut[k, :], par_nlp)

        xt[k+1, :] = np.squeeze(xkp1.full())

   
    return xt, ut
   
     # Initial test case
x_init = np.array([-49., # x: x distance between our car and leader car
                0.0, # y: y distance between our car and leader car
                0.0, # yaw: yaw angle of our car
                50., # v: velocity of our car
                20., # v_x_leader: x velocity of leader car
                0.0, # v_y_leader: y velocity of leader car, fixed as 0.
                50.0 # v_x_desired: desired takeover x velocity of our car
                ])      
xt, ut = eval_controller(x_init)
Which the expression may similar as something like: J = 100 * ((x[0, -1] - 10)**2 + (x[1, -1] - 10)**2)  + 100 * x[2, -1]**2 + 100 * x[3, -1]**2 as an example