Solving MPC using JuMP and Gurobi

Specific Domains Optimization (Mathematical)
4

Updating the constraints as you showed above (with a couple tweaks) helped me find my typo and it is working now. Here is the final linear_mpc_control() function. I will post the rest of the code for others looking for a simple MPC example when I finish the iteration code.

Thanks again for the quick response!

function linear_mpc_control(xref, xbar, x0, dref)
    model = Model(Gurobi.Optimizer)
    # generate variable matrix for states
    @variable(model, x[1:NX, 1:(T+1)])
    # generate variable matrix for controls
    @variable(model, u[1:NU, 1:(T)])

    #fix variable x[:,1] to intial state
    for i in 1:NX
        fix(x[i,1], x0[i])
    end

    #set upper and lower bound for each state
    #cant set contraint for first x since it is fixed above
    for t in 2:(T+1)
        set_upper_bound(x[3, t], MAX_SPEED)
        set_lower_bound(x[3, t], MIN_SPEED)
    end
    for t in 1:T
        set_upper_bound(u[1, t], MAX_ACCEL)
        set_lower_bound(u[1, t], -MAX_ACCEL)
        set_upper_bound(u[2, t], MAX_STEER)
        set_lower_bound(u[2, t], -MAX_STEER)
    end

    #get linear model matrix for each t and add the LQR contraint
    for t in 1:T
        A, B, C = get_linear_model_matrix(xbar[3, t], xbar[4, t], dref[1, t])
        @constraint(model, x[:, t + 1] .== A * x[:, t] + B * u[:, t] + C)
    end

    #add constraints for maximum steering rate
    #pretty sure this should be to T-1, not 2
    for t in 1:(T-1)
        @constraint(model, u[2, t + 1] - u[2, t] <= MAX_DSTEER * DT)
        @constraint(model, u[2, t + 1] - u[2, t] >= -MAX_DSTEER * DT)
    end
    #generate objective (cost) function
    @objective(
        model, Min, 
        sum(u[:, t]' * R * u[:, t] for t in 1:T) +
        sum((xref[:, t] - x[:, t])' * Q * (xref[:, t] - x[:, t]) for t in 2:T) +
        sum((u[:, t+1] - u[:, t])' * Rd * (u[:, t+1] - u[:, t]) for t in 1:(T-2)) +
        (xref[:, T] - x[:, T])' * Qf * (xref[:, T] - x[:, T]),
    )

    optimize!(model)
    if termination_status(model) == MOI.OPTIMAL
        #solution is optimal
        solution_summary(model)
        # display(value(x))
        # display(value(u))

        #get optimzal values from variables
        ox = value.(x[1,:])
        oy = value.(x[2,:])
        ov = value.(x[3,:])
        oyaw = value.(x[4,:])
        oa = value.(u[1,:])
        odelta = value.(u[2,:])
    else
        print("\n\ncannot solve\n\n\n")
        ox = nothing
        oy = nothing
        ov = nothing
        oyaw = nothing
        oa = nothing
        odelta = nothing
    end

    return oa, odelta, ox, oy, oyaw, ov
end
1 Like