问题描述
大家节日快乐!我终于有一些时间来处理我的项目了,当然我像往常一样卡住了哈哈。
我正在寻找可以让我对以下内容进行建模的指导/示例:
-
这对系统的第二个影响是,推动系统向前发展的一组动态会发生变化。 (在这种情况下,“跳跃”会将动力从驱动系统更改为飞行系统)。将来还会有一个“double_jump”,它不会改变动态,但仍然提供速度的瞬时变化。目前我正在尝试完成第一部分,然后我将尝试实现这一点。只是想让任何阅读本文的人都能清楚地看到我更大的愿景。
-
还有另一部分是针对模型的未来:我希望能够让系统与球对象交互,比如使用 if2/if3,如果系统的位置是某个半径从另一个物体的位置,将根据球和系统的速度等因素将冲量传递到球物体上。为了正确地做,我想我需要一种方法来定义在交互点发生的时间步长,我相信这意味着我需要某种可变时间向量。任何这些示例将不胜感激。
好的,所以 2 和 3 就在这里,并不是这个问题的真正要点。我想一旦我能够围绕实现这个奇怪的“跳转”输入,我就能弄清楚它们。
我目前的计划是制作一个名为“u_jump”的非整数 MV。然后有一个名为“jump_hist”的变量,它本质上是“u_jump”的“积分”,我给 jump_hist 一个上限为 1。我现在所做的只是假装这个 u_jump 是系统上的加速度速度.dt() 方程。这在理论上有效,但并不能真正代表我试图完美控制的系统。
什么是让我从中吸取经验教训的最佳示例?另一个问题是,有没有办法通过给整数一个容差来使 IPOPT 求解器适用于整数解?有点像 minlp 求解器选项“minlp_integer_tol 0.05”,这样我仍然可以获得 IPOPT 的速度,但能够合并整数样式变量/方程,如 if3() 等......如果没有,我有什么方法可以处理整数使用非整数解决方案的解决方案,这样当我在实际系统上实施控制时,非整数解决方案和整数解决方案之间的差异在可接受的容差范围内,以将其视为反馈控制器可以减轻的干扰?>
我知道的有点多,我的问题总是哈哈。希望这对未来的其他人有帮助!这是我目前的代码。如果代码给您带来问题或我可以在问题中澄清的任何内容,请告诉我。
哦,还有最后一点,这是目前设置为 2D 飞行系统。为了简单地实现这个“跳跃”输入,我删除了驱动动力学(c 样条被注释掉)。
再次祝大家节日快乐!
import numpy as np
import matplotlib.pyplot as plt
import math
import gekko
from gekko import GEKKO
import csv
from mpl_toolkits.mplot3d import Axes3D
class Optimizer():
def __init__(self):
#################GROUND DRIVING OPTIMIZER SETTTINGS##############
self.d = GEKKO(remote=False) # Driving on ground optimizer
ntd = 21
self.d.time = np.linspace(0,1,ntd) # Time vector normalized 0-1
# options
self.d.options.NODES = 3
self.d.options.soLVER = 3
self.d.options.IMODE = 6# MPC mode
self.d.options.MAX_ITER = 800
self.d.options.MV_TYPE = 0
self.d.options.DIAGLEVEL = 0
# self.d.options.OTOL = 1
# final time for driving optimizer
self.tf = self.d.FV(value=1.0,lb=0.1,ub=10.0,name='tf')
# allow gekko to change the tf value
self.tf.STATUS = 1
# time variable
self.t = self.d.Var(value=0)
self.d.Equation(self.t.dt()/self.tf == 1)
# acceleration variable
self.a = self.d.MV(fixed_initial=False,lb = 0,ub = 1,name='a')
self.a.STATUS = 1
# Jumping integer varaibles and equations
self.u_jump = self.d.MV(fixed_initial=False,lb=0,ub=1,integer=True)
self.u_jump.STATUS = 1
self.jump_hist = self.d.Var(value=0,name='jump_hist',ub=1)
self.d.Equation(self.jump_hist.dt() == self.u_jumP*(ntd-1))
# self.d.Equation(1.0 >= self.jump_hist)
# pitch input throttle (rotation of system)
self.u_p = self.d.MV(fixed_initial=False,lb = -1,ub=1)
self.u_p.STATUS = 1
# Final variable that allows you to set an objective function considering only final state
self.p_d = np.zeros(ntd)
self.p_d[-1] = 1.0
self.final = self.d.Param(value = self.p_d,name='final')
# Model constants and parameters
self.Dp = self.d.Const(value = 2.7982,name='D_pitch')
self.Tp = self.d.Const(value = 12.146,name='T_pitch')
self.pi = self.d.Const(value = 3.14159,name='pi')
self.g = self.d.Const(value = 0,name='Fg')
self.jump_magnitude = self.d.Param(value = 3000,name = 'jump_mag')
def optimize2D(self,si,sf,vi,vf,ri,omegai): #these are 1x2 vectors s or v [x,z]
# variables and intial conditions
# Position in 2d
self.sx = self.d.Var(value=si[0],lb=-4096,ub=4096,name='x') #x position
# self.sy = self.d.Var(value=si[1],lb=-5120,ub=5120,name='y') #y position
self.sz = self.d.Var(value = si[1])
# Pitch rotation and angular veLocity
self.pitch = self.d.Var(value = ri,name='pitch',lb=-1*self.pi,ub=self.pi)
self.pitch_dot = self.d.Var(fixed_initial=False,name='pitch_dot')
# VeLocity in 2D
self.v_mag = self.d.Var(value=(vi),name='v_mag')
self.vx = self.d.Var(value=np.cos(ri),name='vx') #x veLocity
# self.vy = self.d.Var(value=(np.sin(ri) * vi),name='vy') #y veLocity
self.vz = self.d.Var(value = (np.sin(ri) * vi),name='vz')
## Non-linear state dependent dynamics descired as csplines.
#curvature vs vel as a cubic spline for driving state
cur = np.array([0.0069,0.00398,0.00235,0.001375,0.0011,0.00088])
v_cur = np.array([0,500,1000,1500,1750,2300])
v_cur_fine = np.linspace(0,2300,100)
cur_fine = np.interp(v_cur_fine,v_cur,cur)
self.curvature = self.d.Var(name='curvature')
self.d.cspline(self.v_mag,self.curvature,v_cur_fine,cur_fine)
# throttle vs vel as cubic spline for driving state
ba=991.666 #Boost acceleration magnitude
kv = np.array([0,1410,2300]) #veLocity input
ka = np.array([1600+ba,0+ba,0+ba]) #acceleration ouput
kv_fine = np.linspace(0,100) # Higher resolution
ka_fine = np.interp(kv_fine,kv,ka) # Piecewise linear high resolution of ka
self.throttle_acceleration = self.d.Var(fixed_initial=False,name='throttle_accel')
self.d.cspline(self.v_mag,self.throttle_acceleration,kv_fine,ka_fine)
# Differental equations
# VeLocity diff eqs
self.d.Equation(self.vx.dt()/self.tf == (self.a*ba * self.d.cos(self.pitch)*self.jump_hist) + (self.a * self.throttle_acceleration * (1-self.jump_hist)) + (self.u_jump * self.jump_magnitude * self.d.cos(self.pitch + np.pi/2)))
self.d.Equation(self.vz.dt()/self.tf == (self.a*ba * self.d.sin(self.pitch)*self.jump_hist) - (self.g * (1-self.jump_hist)) + (self.u_jump * self.jump_magnitude * self.d.sin(self.pitch + np.pi/2)))
self.d.Equation(self.v_mag == self.d.sqrt((self.vx*self.vx) + (self.vz*self.vz)))
self.d.Equation(2300 >= self.v_mag)
# Position diff eqs
self.d.Equation(self.sx.dt()/self.tf == self.vx)
# self.d.Equation(self.sy.dt()/self.tf == self.vy)
self.d.Equation(self.sz.dt()/self.tf == self.vz)
# Orientation diff eqs
self.d.Equation(self.pitch_dot.dt()/self.tf == ((self.Tp * self.u_p) + (self.Dp * self.pitch_dot * (1 - self.d.abs2(self.u_p)))) * self.jump_hist)
self.d.Equation(self.pitch.dt()/self.tf == self.pitch_dot)
# Objective functions
# Final Position Objectives
self.d.Minimize(self.final*1e2*((self.sz-sf[1])**2)) # z final position objective
self.d.Minimize(self.final*1e2*((self.sx-sf[0])**2)) # x final position objective
# Final VeLocity Objectives
# self.d.Obj(self.final*1e3*(self.vz-vf[1])**2)
# self.d.Obj(self.final*1e3*(self.vx-vf[0])**2)
# Minimum Time Objective
self.d.Minimize(1e4*self.tf)
#solve
# self.d.solve('http://127.0.0.1') # Solve with local apmonitor server
self.d.open_folder()
self.d.solve(disp=True)
self.ts = np.multiply(self.d.time,self.tf.value[0])
return self.a,self.u_p,self.ts
def getTrajectoryData(self):
return [self.ts,self.sx,self.sz,self.vx,self.vz,self.pitch,self.pitch_dot]
def getInputData(self):
return [self.ts,self.a]
# Main Code
opt = Optimizer()
s_ti = [0,0]
v_ti = 0
s_tf = [1000,500]
v_tf = [00.00,00.0]
r_ti = 0 # inital orientation of the car
omega_ti = 0.0 # initial angular veLocity of car
acceleration,turning,t_star = opt.optimize2D(s_ti,s_tf,v_ti,v_tf,r_ti,omega_ti)
# Printing stuff
# print('u',acceleration.value)
# print('tf',opt.tf.value)
# print('tf',opt.tf.value[0])
# print('u jump',opt.jump)
# for i in opt.u_jump: print(i.value)
print('u_jump',opt.u_jump.value)
print('jump his',opt.jump_hist.value)
print('v_mag',opt.v_mag.value)
print('a',opt.a.value)
# Plotting stuff
ts = opt.d.time * opt.tf.value[0]
t_max = opt.tf.value[0]
x_max = np.max(opt.sx.value)
vx_max = np.max(opt.vx.value)
z_max = np.max(opt.sz.value)
vz_max = np.max(opt.vz.value)
# plot results
fig = plt.figure(2)
ax = fig.add_subplot(111,projection='3d')
# plt.subplot(2,1)
Axes3D.plot(ax,opt.sx.value,ts,opt.sz.value,c='r',marker ='o')
plt.ylim(0,t_max)
plt.xlim(0,x_max)
plt.ylabel('time')
plt.xlabel('Position x')
ax.set_zlabel('position z')
n=5 #num plots
fig = plt.figure(3)
ax = fig.add_subplot(111,opt.vx.value,opt.vz.value,t_max)
plt.xlim(-1*vx_max,vx_max)
# plt.zlim(0,2000)
plt.ylabel('time')
plt.xlabel('VeLocity x')
ax.set_zlabel('vz')
plt.figure(1)
plt.subplot(n,1)
plt.plot(ts,opt.a,'r-')
plt.ylabel('acceleration')
plt.subplot(n,2)
plt.plot(ts,np.multiply(opt.pitch,1/math.pi),'r-')
plt.ylabel('pitch orientation')
plt.subplot(n,3)
plt.plot(ts,opt.v_mag,'b-')
plt.ylabel('vmag')
plt.subplot(n,4)
plt.plot(ts,opt.u_p,'b-')
plt.ylabel('u_p')
plt.subplot(n,5)
plt.plot(ts,opt.u_jump,'b-')
plt.plot(ts,opt.jump_hist,'r-')
plt.ylabel('jump(b),jump hist(r)')
plt.show()
print('asdf')
解决方法
要尝试的一件事是使用IPOPT 进行初始化,然后使用APOPT 来获得整数解。另一件要尝试的事情是将 MPCC 用于不依赖于二进制变量的切换条件。我发现 MPCC 形式远不如二进制变量切换条件可靠,因为求解器经常卡在鞍点。然而,整数解通常需要更长的时间来求解。
self.d.options.SOLVER=3
self.d.solve(disp=True)
self.d.options.TIME_SHIFT=0
self.d.options.SOLVER=1
self.d.solve(disp=True)
这是IPOPT的解决方案:
EXIT: Optimal Solution Found.
The solution was found.
The final value of the objective function is 506284.8987787149
---------------------------------------------------
Solver : IPOPT (v3.12)
Solution time : 7.4613000000000005 sec
Objective : 506284.8987787149
Successful solution
---------------------------------------------------
整数解是通过 APOT 获得的。
--------- APM Model Size ------------
Variable time shift OFF
Number of state variables: 1286
Number of total equations: - 1180
Number of slack variables: - 40
---------------------------------------
Degrees of freedom : 66
----------------------------------------------
Dynamic Control with APOPT Solver
----------------------------------------------
Iter: 1 I: 0 Tm: 2.72 NLPi: 92 Dpth: 0 Lvs: 3 Obj: 5.07E+05 Gap: NaN
Iter: 2 I: -1 Tm: 0.53 NLPi: 17 Dpth: 1 Lvs: 2 Obj: 5.07E+05 Gap: NaN
Iter: 3 I: -9 Tm: 47.59 NLPi: 801 Dpth: 1 Lvs: 1 Obj: 5.07E+05 Gap: NaN
Iter: 4 I: 0 Tm: 2.26 NLPi: 35 Dpth: 1 Lvs: 3 Obj: 5.08E+05 Gap: NaN
--Integer Solution: 2.54E+07 Lowest Leaf: 5.08E+05 Gap: 1.92E+00
Iter: 5 I: 0 Tm: 3.56 NLPi: 32 Dpth: 2 Lvs: 2 Obj: 2.54E+07 Gap: 1.92E+00
Iter: 6 I: -9 Tm: 54.65 NLPi: 801 Dpth: 2 Lvs: 1 Obj: 5.08E+05 Gap: 1.92E+00
Iter: 7 I: -1 Tm: 2.18 NLPi: 83 Dpth: 2 Lvs: 0 Obj: 5.08E+05 Gap: 1.92E+00
No additional trial points,returning the best integer solution
Successful solution
---------------------------------------------------
Solver : APOPT (v1.0)
Solution time : 113.5842 sec
Objective : 2.5419931399165962E+7
Successful solution
---------------------------------------------------
APOPT 选择不跳跃以最小化最终目标。您可能需要添加一个硬约束,即 vsum()
的 u_jump
是 1
。有additional tutorials on MPCC and integer / binary forms of switching conditions in the Optimization course。
感谢您分享您的申请并让我们保持最新状态!