Optimal Innacurate in solving Quadratic Program with CVXPY

75 Views Asked by At

In am implementing an MPC controller and for solving the QP I am using CVXPY. I have the issue that the feasibility of the problem depends on the cost function (this has no theoretical support) and depending on the cost matrices I choose I obtain "optimal innacurate" on "infeasible" problem.

I write the code below:

# MAIN CODE
ns = 2
ni = 1


def main():
    params = Parameters_Fast()
    t_end = 20.0
    t_hor = 10.0
    num_steps = int(t_end/params.dt)
 
    # Initialize Controller 
    MPC = MPCController(parameters=params)
    MPC.build_optcon_problem(t_hor=t_hor, dt=params.dt)

    # Collect vectors for plots
    xk = np.zeros((ns, num_steps))
    uk = np.zeros((ni, num_steps-1))

    Fmax = np.zeros(num_steps-1)
    Fmin = np.zeros(num_steps-1)

    # Initial conditions
    xk[0,0] = -5.0
    xk[1,0] = 1.0

    # Target point
    xk_tar = np.zeros(ns)
    xk_tar[0] = 0.0
    xk_tar[1] = 0.0

    for t in range(0, num_steps-1):

        # Find input constraints
        Fmax[t] = params.Cm1 - params.Cd * (xk[1,t]**2) - params.Croll
        Fmin[t] = -params.Cd * (xk[1,t]**2) - params.Croll

        xk_opt, uk_opt = MPC.solve_optcon_problem(xk_meas=xk[:,t], xk_tar=xk_tar, Fmax=Fmax[t], Fmin=Fmin[t])

        uk[:,t] = uk_opt[:,0]

        xk[:,t+1] = params.A @ xk[:,t] + params.B @ uk[:,t]

# Params class
class Parameters_Fast():
    dt = 0.1
        
    # Vehicle Parameters 
    m = 300   
    Cf = 10

    Cd = 2.15            
    Croll = 80             
    Cm1 = 920  


    A = np.array([[1, dt],
                  [0, 1-dt*(Cf/m)]])
    
    B = np.array([[0],
                  [dt/m]])

    Qo = np.diag([40, 40])
    Ro = np.array([[1e-1]])    

# Controller class
class MPCController():
    """ MPC for steering to a set point"""

    def __init__(self, parameters) -> None:
        self.params = parameters
        self.A = self.params.A
        self.B = self.params.B
        self.Qo = self.params.Qo
        self.Ro = self.params.Ro

    def build_optcon_problem(self, t_hor, dt):
        ns = 2
        ni = 1
        num_steps = int(t_hor/dt)

        self.xk = cp.Variable((ns, num_steps))
        self.uk = cp.Variable((ni, num_steps-1))

        self.x0 = cp.Parameter(ns)
        self.xk_tar = cp.Parameter(ns)
        self.Fmax = cp.Parameter()
        self.Fmin = cp.Parameter()

        constraints = []
        cost = 0.0

        constraints += [self.xk[:,0] == self.x0]

        for t in range(0, num_steps-1):
            constraints += [self.xk[:,t+1] == self.A @ self.xk[:,t] + self.B @ self.uk[:,t]]

            # Input constraints
            constraints += [self.uk[0,t] <= self.Fmax,
                            self.uk[0,t] >= self.Fmin]
            
            # Cost function
            cost += cp.quad_form(self.xk[:,t] - self.xk_tar, self.Qo) + cp.quad_form(self.uk[:,t], self.Ro)

        # Zero terminal constraint
        constraints += [self.xk[:,-1] == self.xk_tar]

        self.prob = cp.Problem(cp.Minimize(cost), constraints)


    def solve_optcon_problem(self, xk_meas, xk_tar, Fmax, Fmin):
        """ Solves optimal control problem for given initial condition """

        self.x0.value = xk_meas
        self.xk_tar.value = xk_tar
        self.Fmax.value = Fmax
        self.Fmin.value = Fmin

        self.prob.solve(solver = cp.OSQP, warm_start=True) 
        if self.prob.status != cp.OPTIMAL:
            print("Error in solving optimization problem")

        xk_opt = self.xk.value
        uk_opt = self.uk.value

        return xk_opt, uk_opt 

If I change the cost Qo matrix with for example the following

Qo = np.diag([40000, 40000])

the solver returns me "Optimal innacurate" and if I increase again the value I obtain "infeasible".

While the feasibility is cost-dependent?

I need to increase and be able to change the cost matrices without them to create an infeasible problem because I have to deal with a most complicated task that requires different weights.

0

There are 0 best solutions below