I try to solve a nonlinear model predictive control problem about tracking certain trajectory.
Following is the code and I don’t really understand the meaning of “WARNING: Ipopt finished with status Restoration_Failed WARNING: Not solved to optimality, status: Error”.
Also I found that the result didn’t follow the constraint "@defVar(mpc, zmin[i] <= z[t=1:T+1,i=1:n] <= zmax[i]) " The reason is “Ipopt finished with status Restoration_Failed”? or the problem isn’t defined well?
Please help, thanks!
using JuMP, Ipopt
using PyPlot
using PyCall
using Distances
@pyimport scipy.spatial as spa
@pyimport matplotlib.pyplot as plts
@pyimport numpy as np
#-----------All relevant parameters for the model-------------
C0=0.4771*0.0525 # m*=0.0525
C1fw=0.5979*0.0525
Cc=0.365
C1sd=0.192*0.0525
CM0=7.571*0.0525
CM1=0.229*0.0525
I=3*10.0^-5
Ir=8*10.0^-7
M=0.04 #car weight(kg)
lf=0.029
lr=0.033
rr=0.008 #rear tire radius
B=20 #magic formula, tire stiffness
Cr=1.222 #-> C = 2 - 2/pi*asin(F_max,dynamic/F_max,static)
Cf=1.2186
Dr=0.15
Df=0.137
k=0.8
w=0.03 #width
n=7 #number of state
m=2 #number of input
path="/home/yard/Desktop/orca_input_0.08_0.1.txt"
#------states: z== X,Y,psi,Vx,Vy,omega,omega_r (7 states) ---------------
z0 = [0;0;0;0;0;0;0] #initial state
zT = []
zmax=[1 ; 2; 100; 1.2; 1.2; 16; 224]
zmin=[-1;-1; 0 ; 0 ; 0 ; 0 ; 0 ]
T = 10 # MPC horizon
dt=0.1 # Smapling time
Tsim = 10 # Length of time(steps) we simulate
Treplanning = 1 # Every Treplanning an MPC is solved
#--------------input: [delta,D] : delta , Dutycycle----------------------
umax=[0.4 ; 0.1]
umin=[0 ; 0]
function solveMPC(lf,lr,n,m,T,z0,zT,zmin,zmax,umax,dt)
mpc = Model(solver=IpoptSolver(print_level=0))
@defVar(mpc, zmin[i] <= z[t=1:T+1,i=1:n] <= zmax[i])
@defVar(mpc, umin[i] <= u[t=1:T,i=1:m] <= umax[i])
@defVar(mpc, C1[1:T])
@defVar(mpc, alpha[1:T])
@setObjective(mpc, Min,
sum{(z[t,1]-zT[t,1])^2+(z[t,2]-zT[t,2])^2+(z[t,3]-zT[t,3])^2+(u[k,1]-u[k+1,1])^2+(u[k,2]-u[k+1,2])^2, k=1:T-1,t= 2:T+1 } )
@NLexpression(mpc, sfy[i=1:T] , (z[i,4]*sin(u[i,1]))-((lf*z[i,6]+z[i,5])*cos(u[i,1])) )
@NLexpression(mpc, srx[i=1:T] , rr*z[i,7]-z[i,4] )
@NLexpression(mpc, sry[i=1:T] , z[i,6]*lr-z[i,5] )
@NLexpression(mpc, sr[i=1:T] , ((srx[i])^2+(sry[i])^2)^0.5 )
@NLexpression(mpc, Ffy[i=1:T] , Df*sin(Cf*atan(B*sfy[i])) )
@NLexpression(mpc, Fr[i=1:T] , Dr*sin(Cr*atan(B*sr[i])) )
@NLexpression(mpc, Frx[i=1:T] , Fr[i]*srx[i]/(sr[i]+10.0^(-6)))
@NLexpression(mpc, Fry[i=1:T] , k*Fr[i]*sry[i]/(sr[i]+10.0^(-6)))
for i=1:T
@addNLConstraint(mpc, C1[i] == ifelse( Frx[i]-Ffy[i]*sin(u[i,1])+M*z[i,5]*z[i,6] > 0, C1fw , C1sd) )
@addNLConstraint(mpc, alpha[i] == ifelse( abs(alpha[i])>1 , ifelse(alpha[i]>0 , 1, -1) , rr*z[i,7]*10) ) #this approximation of the sign?function is used in order to improve the numerical performance
end
@NLexpression(mpc, Fres1[i=1:T] , rr*z[i,7]*C1[i]+alpha[i]*C0 )
@NLexpression(mpc, Fres2[i=1:T] , alpha[i]*abs(Fry[i])*Cc )
@NLexpression(mpc, Fres[i=1:T] , Fres1[i]+Fres2[i] )
# Link state and control across the horizon
for t = 1:T
@addNLConstraint(mpc, z[t+1,1] == z[t,1] + dt*(z[t,4]*cos(z[t,3])-z[t,5]*sin(z[t,3])) )
@addNLConstraint(mpc, z[t+1,2] == z[t,2] + dt*(z[t,4]*sin(z[t,3])+z[t,5]*cos(z[t,3])) )
@addNLConstraint(mpc, z[t+1,3] == z[t,3] + dt*z[t,6] )
@addNLConstraint(mpc, z[t+1,4] == z[t,4] + dt*(Frx[t]-Ffy[t]*sin(u[t,1])+M*z[t,5]*z[t,6])/M )
@addNLConstraint(mpc, z[t+1,5] == z[t,5] + dt*(Fry[t]+Ffy[t]*cos(u[t,1])-M*z[t,4]*z[t,6])/M )
@addNLConstraint(mpc, z[t+1,6] == z[t,6] + dt*(lf*Ffy[t]*cos(u[t,1])-lr*Fry[t])/I )
@addNLConstraint(mpc, z[t+1,7] == ifelse( z[t,7] >400 && (rr*u[t,2]*(CM0-rr*z[t,7]*CM1)-rr*(Fres[t]+Frx[t]))/Ir > 0 , z[t,7] , z[t,7]+dt*(rr*u[t,2]*(CM0-rr*z[t,7]*CM1)-rr*(Fres[t]+Frx[t]))/Ir) )
end
# Initial conditions
@addConstraint(mpc, z[1,:] .== z0)
# Final state
# Solve the NLP
solve(mpc)
return getvalue(u), getvalue(z) #getValue ~ getvalue
end
#-------------Car slip dynamic------------------------------------
function zdot_fun(z,u,lf,lr)
delta=u[1]
D=u[2]
phi=z[3]
vx=z[4]
vy=z[5]
omega=z[6]
omega_r=z[7]
sfy = vx*sin(delta)-((lf*omega+vy)*cos(delta))
srx = rr*omega_r-vx
sry = omega*lr-vy
sr = (srx^2+sry^2)^0.5
Ffy = Df*sin(Cf*atan(B*sfy))
Fr = Dr*sin(Cr*atan(B*sr))
Frx = Fr*srx/(sr+10.0^(-6))
Fry = k*Fr*sry/(sr+10.0^(-6))
if Frx-Ffy*sin(delta)+M*vy*omega > 0
C1=C1fw
else
C1=C1sd
end
alpha = rr*omega_r*10
if abs(alpha) > 1
alpha = sign(alpha)
end
Fres1 = rr*omega_r*C1+alpha*C0
Fres2 = alpha*abs(Fry)*Cc
Fres = Fres1 + Fres2
xdot=vx*cos(phi)-vy*sin(phi)
ydot=vx*sin(phi)+vy*cos(phi)
phidot= omega
vxdot=(Frx-Ffy*sin(delta)+M*vy*omega)/M
vydot=(Fry+Ffy*cos(delta)-M*vx*omega)/M
omegadot=(lf*Ffy*cos(delta)-lr*Fry)/I
omega_rdot=(rr*D*(CM0-rr*omega_r*CM1)-rr*(Fres+Frx)) /Ir
zdot = [xdot;ydot;phidot;vxdot;vydot;omegadot;omega_rdot]
return zdot
end
#------------- Simulate car -------------------------------------
function simulate_car_MPC(T,Tsim,Tr,z0,zT,zmax,umax,lf,lr,n,m,dt)
u_history = zeros(m,Tsim)
z_history = zeros(n,Tsim)
start=1
z_t = z0[:]
for t = 0:Tr:Tsim-1
# SHRINKING HORIZON MPC
#if start >
ref = find_nearest(z_t,zT)
#print(start)
u_vec, z_vec= solveMPC(lf,lr,n,m,T,z_t,ref,zmin,zmax,umax,dt)
#print(length(u_vec))
# print("-MPC solved-")
for k = 1:min(Tr,length(u_vec))
u_t=u_vec[k,:]
z_history[:,t+k] = z_t[:]
u_history[:,t+k] = u_t[:]
z_t = z_t + dt*zdot_fun(z_t,u_t,lf,lr)
end
end
return u_history, z_history
end
#--------------- Get reference tragectory ------------------------
function get_trajectory()
f = np.loadtxt(path,usecols = (0,1,2,3,4,5,6)) # 7 state:X Y psi vx vy omega omega_r
print(size(f))
return f
end
#-------------Find the nearest point on the reference of the car ----------------
function find_nearest(x,f)
dis = Array(Float64,size(f,1))
ref = Array(Float64,T+1)
for i in 1:size(f,1)
dis[i] = euclidean(x[1:2],f[i,1:2])
end
refstep=10
ref_start = indmin(dis)+1
if (ref_start+T*refstep > size(f,1) ) && (ref_start < size(f,1))
#print(size(f,1)-ref_start)
r=(size(f,1)-ref_start)%refstep
if r != 0
ref = [ f[ref_start:refstep:size(f,1)-r,:] ; f[refstep-r:refstep:(T*refstep-(size(f,1)-ref_start)+r),:] ]
else
ref = [ f[ref_start:refstep:size(f,1),:] ; f[1:refstep:1+(T*refstep-(size(f,1)-ref_start)),:] ]
end
elseif ref_start > size(f,1)
ref_start = ref_start - size(f,1)
ref = f[ref_start:refstep:ref_start+T*refstep,:]
elseif ref_start == size(f,1)
ref = f[1:refstep:T*refstep+1,:]
else
ref = f[ref_start:refstep:ref_start+T*refstep,:]
#print(ref)
end
#println(ref_start)
return ref
end
#--------------------------------Main ------------------------------
# Closed-Loop MPC
f = get_trajectory()
println("finished load trajectory!")
# Open Loop MPC
tic()
u_vec,z_vec = simulate_car_MPC(T,Tsim,Treplanning,z0,f,zmax,umax,lf,lr,n,m,dt)
toc()
I use julia 0.6.2, Ipopt 0.3.0 and JuMP 0.18.1.