在 GEKKO 中使用非线性模型预测控制实现横向控制器
Implementation of a Lateral contoller using non linear model predictive control in GEKKO
我正在尝试为由横向动态定义的自动驾驶车辆实施横向控制器 model.Well，我的问题是 CV 没有达到 SP 设置的所需参考点或目标点。我正在使用以下运动方程和 objective 函数。我正在使用半经验公式 (pacejka) 来计算 Fyf Fyr 捐赠的轮胎力。这是equations of motion and objective function。提前致谢。
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()
对于参考轨迹，您需要包含时间常数 TAU，用于 达到设定点的速度 。
ypos.TAU = 1.5
psipos.TAU = 1.5
关于 tuning an MPC application in the Dynamic Optimization exercises 有更多信息。
您需要的另一个更正是 Ffy = m.Var(value=0.0, lb=-1.0*maxF, ub=maxF) 中的 -1.0。否则，它永远无法达到设定点。似乎无法达到两个设定点，因此它优先尝试满足具有更高权重的 ypos 设定点。您可能需要另一个 MV 来控制 ypos 和 psipos。否则，您可以考虑打开 steering 边界，看看它是否可以找到限制更少的更好的解决方案。我还将结束时间设置为 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()
我正在尝试为由横向动态定义的自动驾驶车辆实施横向控制器 model.Well，我的问题是 CV 没有达到 SP 设置的所需参考点或目标点。我正在使用以下运动方程和 objective 函数。我正在使用半经验公式 (pacejka) 来计算 Fyf Fyr 捐赠的轮胎力。这是equations of motion and objective function。提前致谢。
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()
对于参考轨迹，您需要包含时间常数 TAU，用于 达到设定点的速度 。
ypos.TAU = 1.5
psipos.TAU = 1.5
关于 tuning an MPC application in the Dynamic Optimization exercises 有更多信息。
您需要的另一个更正是 Ffy = m.Var(value=0.0, lb=-1.0*maxF, ub=maxF) 中的 -1.0。否则，它永远无法达到设定点。似乎无法达到两个设定点，因此它优先尝试满足具有更高权重的 ypos 设定点。您可能需要另一个 MV 来控制 ypos 和 psipos。否则，您可以考虑打开 steering 边界，看看它是否可以找到限制更少的更好的解决方案。我还将结束时间设置为 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()