利用非线性模型预测控制在GEKKO中实现横向控制

Question

我正在尝试为一个由横向动力学模型定义的自动车辆实施横向控制器。我的问题是CVs没有达到SP设定的理想参考点或目标点。我使用以下运动方程和目标函数。我使用半经验公式（pacejka）计算Fyf Fyr提供的轮胎力。这是运动方程和目标函数。提前谢谢。

from gekko import GEKKO
import numpy as np
import matplotlib.pyplot as plt
import time 
import math


#%% NMPC model
T = 5
nt = 51
m = GEKKO(remote=False)
m.time = np.linspace(0,T,nt)

#Model Parameters
X_speed = m.Param(value=10.0)
mass=m.Param(value=1611.0)
c=m.Param(value=1.351) 
b=m.Param(value=1.5242)
Iz=m.Param(value=3048.1) 

Cyf=m.Param(value=1.30)
Dyf=m.Param(value=3449.94238709)
Byf=m.Param(value=0.223771457713)
Eyf=m.Param(value=-0.6077272729)

Cyr=m.Param(value=1.30)
Dyr=m.Param(value=3846.47835351)
Byr=m.Param(value=0.207969093485)
Eyr=m.Param(value=-0.7755647971)

#Variables
slip_angle_front_tire = m.Var(value=0.0, lb=-10.0, ub=14.0 )
slip_angle_rear_tire = m.Var(value=0.0, lb=-10.0, ub=14.0 )

phi_f = m.Var(value=0.0)
phi_r = m.Var(value=0.0)

maxF = 5000

Ffy = m.Var(value=0.0, lb=-.0*maxF, ub=maxF )
Fry = m.Var(value=0.0, lb=-1.0*maxF, ub=maxF )

 
xpos = m.Var(value=0.0)
dy = m.Var(value=0.0)
dpsi = m.Var(value=0.0)

#MV
steering = m.MV(value=0, lb=-0.40, ub=0.40 )

#CV
ypos = m.CV(value=0.0 ,lb =-200.0,ub=200.0 )
psipos = m.CV(value=0.0,lb=-3.5,ub=3.5)

#Equations
m.Equation(ypos.dt() == dy)
m.Equation(psipos.dt() == dpsi)

m.Equation(slip_angle_front_tire == steering - m.atan( (dy+b*dpsi)/X_speed ) )
m.Equation(slip_angle_rear_tire == -1.0*m.atan( (dy-c*dpsi) / X_speed))

m.Equation(phi_f == (1-Eyf)*(slip_angle_front_tire) + (Eyf/Byf)*(m.atan(Byf*slip_angle_front_tire) ) )
m.Equation(phi_r == (1-Eyr)*(slip_angle_rear_tire) + (Eyr/Byr)*(m.atan(Byr*slip_angle_rear_tire) ) )

m.Equation(Ffy == (Dyf*( m.sin(Cyf*m.atan(Byf*phi_f ) ) ) ) *2.0 )
m.Equation(Fry == (Dyr*( m.sin(Cyr*m.atan(Byr*phi_r ) ) ) ) *2.0 )

m.Equation(mass*dy.dt() == (Ffy*m.cos(steering) ) + (Fry) - (X_speed*dpsi*mass) ) 
m.Equation(dpsi.dt()*Iz == ( b*Ffy*m.cos(steering) ) - ( c*Fry) )

#Global options
m.options.IMODE = 6 #MPC
m.options.CV_TYPE = 2
m.options.MV_TYPE = 0

#MV tuning
steering.STATUS = 1
steering.DCOST = 0.01

#CV Tuning
ypos.STATUS = 1
psipos.STATUS = 1

ypos.TR_INIT = 2
psipos.TR_INIT = 2


ypos.WSP = 100 
psipos.WSP = 10

ypos.SP = 9.2
psipos.SP = 1.5

print('Solver starts ...')
t = time.time()
m.solve(disp=True)
print('Solver took ', time.time() - t, 'seconds')

plt.figure()

plt.subplot(4,1,1)
plt.plot(m.time,steering.value,'b-',LineWidth=2)
plt.ylabel('steering wheel')

plt.subplot(4,1,2)
plt.plot(m.time,ypos.value,'r--',LineWidth=2)
plt.ylabel('y-point')

plt.subplot(4,1,3)
plt.plot(m.time,psipos.value,'r--',LineWidth=2)
plt.ylabel('yaw angle')
plt.xlabel('time')
plt.show()

Answer 1

对于参考轨迹，您需要包括时间常数TAU，以了解达到设定点的速度。

ypos.TAU = 1.5
psipos.TAU = 1.5

在动态优化练习中，还有关于调优MPC应用程序的其他信息。

您需要的另一个修正是Ffy=m.Var（value=0.0，lb=-1.0*maxF，ub=maxF）中的-1.0。否则，它永远无法达到设定点。似乎无法达到两个设定点，因此它优先尝试满足权重较高的ypos设定点。您可能需要另一个MV来控制ypos和psipos。否则，您可以考虑打开<代码>转向< /代码>界限，看看它是否可以找到更少的限制更好的解决方案。我还将结束时间设置为10，有101个点，因为它需要额外的时间来稳定到新的设定点。

from gekko import GEKKO
import numpy as np
import matplotlib.pyplot as plt
import time 
import math


#%% NMPC model
T = 10
nt = 101
m = GEKKO(remote=False)
m.time = np.linspace(0,T,nt)

#Model Parameters
X_speed = m.Param(value=10.0)
mass=m.Param(value=1611.0)
c=m.Param(value=1.351) 
b=m.Param(value=1.5242)
Iz=m.Param(value=3048.1) 

Cyf=m.Param(value=1.30)
Dyf=m.Param(value=3449.94238709)
Byf=m.Param(value=0.223771457713)
Eyf=m.Param(value=-0.6077272729)

Cyr=m.Param(value=1.30)
Dyr=m.Param(value=3846.47835351)
Byr=m.Param(value=0.207969093485)
Eyr=m.Param(value=-0.7755647971)

#Variables
slip_angle_front_tire = m.Var(value=0.0, lb=-10.0, ub=14.0 )
slip_angle_rear_tire = m.Var(value=0.0, lb=-10.0, ub=14.0 )

phi_f = m.Var(value=0.0)
phi_r = m.Var(value=0.0)

maxF = 5000

Ffy = m.Var(value=0.0, lb=-1.0*maxF, ub=maxF )
Fry = m.Var(value=0.0, lb=-1.0*maxF, ub=maxF )


xpos = m.Var(value=0.0)
dy = m.Var(value=0.0)
dpsi = m.Var(value=0.0)

#MV
steering = m.MV(value=0, lb=-0.4, ub=0.4 )

#CV
ypos = m.CV(value=0.0 ,lb =-200.0,ub=200.0 )
psipos = m.CV(value=0.0,lb=-3.5,ub=3.5)

#Equations
m.Equation(ypos.dt() == dy)
m.Equation(psipos.dt() == dpsi)

m.Equation(slip_angle_front_tire == steering - m.atan( (dy+b*dpsi)/X_speed ) )
m.Equation(slip_angle_rear_tire == -1.0*m.atan( (dy-c*dpsi) / X_speed))

m.Equation(phi_f == (1-Eyf)*(slip_angle_front_tire) + (Eyf/Byf)*(m.atan(Byf*slip_angle_front_tire) ) )
m.Equation(phi_r == (1-Eyr)*(slip_angle_rear_tire) + (Eyr/Byr)*(m.atan(Byr*slip_angle_rear_tire) ) )

m.Equation(Ffy == (Dyf*( m.sin(Cyf*m.atan(Byf*phi_f ) ) ) ) *2.0 )
m.Equation(Fry == (Dyr*( m.sin(Cyr*m.atan(Byr*phi_r ) ) ) ) *2.0 )

m.Equation(mass*dy.dt() == (Ffy*m.cos(steering) ) + (Fry) - (X_speed*dpsi*mass) ) 
m.Equation(dpsi.dt()*Iz == ( b*Ffy*m.cos(steering) ) - ( c*Fry) )

#Global options
m.options.IMODE = 6 #MPC
m.options.CV_TYPE = 2
m.options.MV_TYPE = 1

#MV tuning
steering.STATUS = 1
steering.DCOST = 0.1

#CV Tuning
ypos.STATUS = 1
psipos.STATUS = 1

ypos.TR_INIT = 2
psipos.TR_INIT = 2

ypos.WSP = 100 
psipos.WSP = 10

ypos.SP = 9.2
psipos.SP = 1.5

ypos.TAU = 1.5
psipos.TAU = 1.5

print('Solver starts ...')
t = time.time()
m.solve(disp=True)
print('Solver took ', time.time() - t, 'seconds')

plt.figure()

plt.subplot(3,1,1)
plt.plot(m.time,steering.value,'b-',LineWidth=2)
plt.ylabel('steering wheel')

plt.subplot(3,1,2)
plt.plot([0,10],[9.2,9.2],'k-')
plt.plot(m.time,ypos.value,'r--',LineWidth=2)
plt.ylabel('y-point')

plt.subplot(3,1,3)
plt.plot([0,10],[1.5,1.5],'k-')
plt.plot(m.time,psipos.value,'g:',LineWidth=2)
plt.ylabel('yaw angle')
plt.xlabel('time')
plt.show()