问题描述
EDIT2：好的，我是个白痴。我关于横向误差的数学值得怀疑。我已经将该方程重写为更简单的方法，并且现在可以正常工作。我会离开这里，以防对任何人有用。但是，如果可能的话，我仍然会希望有人确保我没有做任何过分的事情，以进行健全性检查。
编辑：我通过将其设置为GEKKO.Var而不是GEKKO.Intermediate来解决了heading_error所遇到的问题。既然可以了，我已经尝试将等式的侧向误差部分纳入其中。现在，解决方案失败了。我认为这可能与我在中间方程式中使用变量的方式有关，但我不确定。我已经用下面的当前版本替换了BikeSolver.py。
在陈述我的问题之前，让我先解释一下我在GEKKO上要做什么，也许我的某些方程式建模方法可能会得到改进，以解决出现的问题。
我有一个参加IGVC竞赛的自动驾驶汽车学生团队。在直接开始在汽车上进行任何操作之前，我想拥有一个强大的仿真系统，该系统可以使我们对不同的控制算法将如何执行有一个大致的了解。在这种情况下，我首先要为学生建立基础，然后他们可以继续进行测试不同的控制算法等。最终，计划将是使用GEKKO实时生成MPC控制算法。但是目前，我希望学生在进入基于模型的优化之前对其他反馈控制方法有所了解。
好的，这是总体思路。我们正在使用简化的运动自行车模型来建模整个汽车的动力学。 Gekko求解器接收具有几个离散状态（位置和速度，我们在xy平面上进行控制）的轨迹设置，然后使用来自scipy的Bpoly类对该轨迹进行插值，以将轨迹作为一堆不同的GEKKO提供给我们。 （例如x_interp，y_interp，vx_interp，vy_interp，yaw_interp，vmag_interp）。这一切都很好，我很幸运能够弄清楚如何使这种方法适用于各种Solver.Time向量。
最后，我想使用一些矢量数学来生成转向输入控件。例如，使用“航向误差”生成转向输入以及“横向误差”。这两种方法需要一些交叉乘积，并且要获得有用的值才能转化为转向命令。
所以我现在遇到一个问题，正在计算一个简单的航向误差，例如heading_error = yaw_interp-yaw，其中yaw_interp是从所需轨迹内插的偏航，而yaw是状态方向变量。由于某种原因，当我将操纵变量（delta）设置为与heading_error相等时，如果我不将heading_error乘以2，则解决方案将失败。超级怪异。这是BikeSolver.py中的第118行。
所以很好奇有人会误解我犯的错误，以及是否有更优雅/更稳健的方式将GEKKO用于此类仿真。
请注意，这两个文件都应位于名为reference_code的文件夹中。 BikeSolver.py上的9-12行通过将reference_code文件夹附加到系统路径来导入Physics.py。这是在所有学生计算机上都能完成这项工作的最快解决方案。
最好， 迈克尔·吉利亚
BikeSolver.py -------------------------
from gekko import GEKKO
import numpy as np
import matplotlib.pyplot as plt
import sys
import os
import linecache
# Deal with python relative importing stuff
PACKAGE_PARENT = '..'
SCRIPT_DIR = os.path.dirname(os.path.realpath(os.path.join(os.getcwd(),os.path.expanduser(__file__))))
sys.path.append(os.path.normpath(os.path.join(SCRIPT_DIR,PACKAGE_PARENT)))
from reference_code.Physics import *
def PrintException():
exc_type,exc_obj,tb = sys.exc_info()
f = tb.tb_frame
lineno = tb.tb_lineno
filename = f.f_code.co_filename
linecache.checkcache(filename)
line = linecache.getline(filename,lineno,f.f_globals)
print('EXCEPTION IN ({},LINE {} "{}"): {}'.format(filename,line.strip(),exc_obj))
class BikeSolver():
def __init__(self):
self.solver = GEKKO(remote=False)
self.ntd = 31
self.solver.time = np.linspace(0,100,self.ntd)
# Add time variable that way we can pass into the poly() function and return the interpolated positions (instead i've used the solver.time numpy array)
#self.t = self.solver.Var(value=0); self.solver.Equation(self.t.dt() == 1)
self.solver.options.NODES = 3
self.solver.options.soLVER = 3 #Solver Type IPOPT
self.solver.options.IMODE = 6 #MPC solution mode,will manipulate MV's to minimize Objective functions
# self.solver.options.MAX_ITER = 500
# final time for optimizer (currently unused)
self.tf = self.solver.FV(value=1.0,lb=0.1,ub=100.0)
# allow gekko to change the tf value
self.tf.STATUS = 0
def solve_open_loop(self,v0,d0,L):
# Initialize all state variables
self.x = self.solver.Var(value = 0)
self.y = self.solver.Var(value = 0)
self.yaw = self.solver.Var(value = 0) # state orientation variable
self.vx = self.solver.Var(value = v0*np.cos(0))
self.vy = self.solver.Var(value = v0*np.sin(0))
self.omega_yaw = self.solver.Var(value = (np.tan(d0)/L))
self.v_mag = self.solver.Param(value = v0)
self.delta = self.solver.Var(value = d0,ub=30,lb=-30) #steering angle
self.L = self.solver.Param(value = L)
# Add manipulatable variables
self.delta_control = self.solver.MV(value = self.delta)
self.delta_control.STATUS = 0 # Allow GEKKO to change this value
self.solver.Equation(self.vx == self.v_mag * self.solver.cos(self.yaw))
self.solver.Equation(self.vy == self.v_mag * self.solver.sin(self.yaw))
self.solver.Equation(self.omega_yaw == (self.solver.tan(self.delta_control) / self.L))
self.solver.Equation(self.x.dt() == self.vx)
self.solver.Equation(self.y.dt() == self.vy)
self.solver.Equation(self.yaw.dt() == self.omega_yaw)
# self.solver.Equation(self.delta == "pure pursuit equation")
self.solver.Obj((self.delta - self.delta_control)**2)
self.solver.solve(disp=True) # Solve the system and display results to terminal
def solve_cubic_spline_traj(self,q: Trajectory,s0: State,c0: ControlInput,L):
# Initialize all state variables
self.x = self.solver.Var(value = s0.position.x) # X position
self.y = self.solver.Var(value = s0.position.y) # Y positoin
self.yaw = self.solver.Var(value = s0.orientation.z) # state orientation variable
self.vx = self.solver.Var(value = s0.veLocity.x) # x veLocity
self.vy = self.solver.Var(value = s0.veLocity.y) # y veLocity
self.omega_yaw = self.solver.Var(fixed_initial=False) # angular veLocity
self.v_mag = self.solver.Var(value = s0.veLocity.magnitude(),lb=0.0) # veLocity magnitude
self.delta = self.solver.Var(fixed_initial=False) #steering angle (without bounds)
self.L = self.solver.Param(value = L) # Wheel base parameter
# Add manipulatable variables
self.delta_control = self.solver.MV(ub=np.pi/4,lb=-np.pi/4) # Steering angle with bounds
self.delta_control.dcosT = 0.0 # Penalize changing steering angle
self.delta_control.STATUS = 1 # Allow GEKKO to change this value
self.solver.free_initial(self.delta_control)
self.a = self.solver.MV(value = 0.0,lb=-2,ub=2) # Linear acceleration to adjust v_mag
self.a.dcosT = 1e-8 # Small Penilzation on changing acceleration value
self.a.STATUS = 1
self.solver.free_initial(self.a)
# Get poly spline form of trajectory
self.x_poly,self.y_poly = TrajectoryInterpolator.interpolate(q) # Interpolate discrete trajectory
self.x_interp = self.solver.Param(value = self.get_interp(self.x_poly,self.solver.time)) # X position of trajectory vs time
self.y_interp = self.solver.Param(value = self.get_interp(self.y_poly,self.solver.time)) # Y position of trajectory
self.vx_interp = self.solver.Param(value = self.get_interp(self.x_poly.derivative(),self.solver.time)) # X veLocity of trajectory
self.vy_interp = self.solver.Param(value = self.get_interp(self.y_poly.derivative(),self.solver.time)) # Y veLocity of trajectory
self.vmag_interp = self.solver.Param(value = self.get_vmag(self.vx_interp,self.vy_interp)) # VeLocity magnitude of trajectory
self.vx_unit_interp = self.solver.Param(value = self.vx_interp.value/self.vmag_interp.value) # X veLocity unit vector for geometric control equations
self.vy_unit_interp = self.solver.Param(value = self.vy_interp.value/self.vmag_interp.value) # Y veLocity unit vector for geometric control equations
self.yaw_interp = self.solver.Param(value = self.get_yaw(self.vx_unit_interp,self.vy_unit_interp)) # Orientation of trajectroy for geometric control equations
# Lateral error variables
self.x_traj_error = self.solver.Intermediate(equation = (self.x_interp - self.x))
self.y_traj_error = self.solver.Intermediate(equation = (self.y_interp - self.y))
self.lat_error = self.solver.Var(fixed_initial = False) #equation = ((self.x_interp - self.x)*self.vy/self.v_mag) - ((self.y_interp - self.y)*self.vx/self.v_mag))
# heading error variables
self.heading_error = self.solver.Var(fixed_initial=False)
# Steering input equation
self.solver.Equation(self.heading_error == self.yaw - self.yaw_interp)
self.solver.Equation(self.lat_error**2 == ((self.x_interp - self.x)**2) + ((self.y_interp - self.y)**2))
self.solver.Equation(self.delta == self.heading_error + 0.1*self.lat_error)
# Define system model equations
self.solver.Equation(self.v_mag.dt() == self.a)
self.solver.Equation(self.vx == self.v_mag * self.solver.cos(self.yaw))
self.solver.Equation(self.vy == self.v_mag * self.solver.sin(self.yaw))
self.solver.Equation(self.omega_yaw == self.v_mag * (self.solver.tan(self.delta_control) / self.L))
self.solver.Equation(self.yaw.dt() == self.omega_yaw)
self.solver.Equation(self.x.dt() == self.vx)
self.solver.Equation(self.y.dt() == self.vy)
self.solver.Obj((self.delta - self.delta_control)**2) # Force solver to set steering angle to delta variable
# self.solver.Obj((self.x_interp - self.x)**2 + (self.y_interp - self.y)**2) # Let solver find best acceleration and steering inputs
self.solver.Obj((self.vmag_interp - self.v_mag)**2) # Follow veLocity described by trajectory
# self.solver.open_folder()
self.solver.solve(disp=True) # Solve the system and display results to terminal
# self.solver.solve()
def get_interp(self,poly,t):
return poly(t)
def get_vmag(self,vx,vy):
return np.sqrt(vx.value**2 + vy.value**2)
def get_yaw(self,vy):
return np.arcsin(vy.value)
def plot_data(self):
"""Will plot
"""
num_subplots = 7
fig = plt.figure(2)
plt.subplot(num_subplots,1,1)
plt.plot(self.solver.time,self.x.value,'r-')
plt.plot(self.solver.time,self.x_interp.value,'r.')
plt.ylabel('x pos')
plt.subplot(num_subplots,2)
plt.plot(self.solver.time,self.y.value,'b-')
plt.plot(self.solver.time,self.y_interp.value,'b.')
plt.ylabel('y pos')
# plt.ylim(-280,280)
plt.subplot(num_subplots,3)
plt.plot(self.solver.time,self.yaw.value,'g-')
plt.plot(self.solver.time,self.yaw_interp.value,'g.')
plt.ylabel('yaw val')
plt.subplot(num_subplots,4)
plt.plot(self.solver.time,self.delta_control.value,'g-')
plt.ylabel('steering val')
# plt.ylim(-280,280)
plt.subplot(num_subplots,5)
plt.plot(self.x,self.y,'g-')
plt.plot(self.x_interp,self.y_interp,'r.')
plt.ylabel('xy pos')
plt.subplot(num_subplots,6)
plt.plot(self.solver.time,self.a,'y-')
plt.ylabel('acceleration')
plt.subplot(num_subplots,7)
plt.plot(self.solver.time,self.v_mag,'c-')
plt.plot(self.solver.time,self.vmag_interp,'c.')
plt.ylabel('v_mag')
plt.draw()
plt.ion()
plt.show()
plt.pause(0.001)
class Solver():
def __init__(self):
self.solver = GEKKO(remote=False)
ntd = 500
self.solver.time = np.linspace(0,50,ntd)
self.solver.options.NODES = 2
self.solver.options.soLVER = 3 #Solver Type IPOPT
self.solver.options.IMODE = 6 #MPC solution mode,will manipulate MV's to minimize Objective functions
def solve_pid(self,x0,kp,ki,kd,xd):
"""Look into slack variables to make this probelm solving quicker.
Args:
x0 ([type]): [description]
v0 ([type]): [description]
kp ([type]): [description]
ki ([type]): [description]
kd ([type]): [description]
xd ([type]): [description]
"""
self.x = self.solver.Var(value = x0) # Position of system
self.v = self.solver.Var(value = v0) # VeLocity of system
self.e = self.solver.Var(value = (xd-x0)) # Positional error of system
self.pid_output = self.solver.Var() # Output of PID algorithm
self.a = self.solver.MV(value = self.pid_output,lb=-10000,ub=10000) # acceleration input to system
self.a.STATUS = 1 # Allow GEKKO to change this value
self.solver.Equation(self.e == xd - self.x) # Define error function
self.solver.Equation(self.x.dt() == self.v) # Define derivative of position to equal veLocity
self.solver.Equation(self.v.dt() == self.a) # Define derivative of veLocity to equal acceleration
self.solver.Equation(self.pid_output == (self.e*kp) + (self.e.dt()*kd)) # Define pid_output from error multipled by gains (passed into this function)
self.solver.Obj((self.pid_output - self.a)**2) # Objective function to force acceleration to equal pid_output
# I had to do this so that I Could bound acceleration,for some reason was getting erros when useing IMODE=4 (simulation mode)
# And bounding variables,so I made acceleration an MV with bounds,and just penalized the solver for deviating from
# making acceleration != pid_output,squared error which is typical to remove sign
self.solver.solve(disp=True) # Solve the system and display results to terminal
def plot_data(self):
"""Will plot
"""
fig = plt.figure(1)
plt.subplot(3,self.v.value,'r-')
plt.ylabel('veLocity')
plt.subplot(3,'r-')
plt.ylabel('position')
plt.subplot(3,self.a.value,'r-')
plt.ylabel('acceleration')
plt.draw()
plt.show(block=True)
class RobotSolver():
def __init__(self):
self.solver = GEKKO(remote=False)
ntd = 100
self.solver.time = np.linspace(0,3,will manipulate MV's to minimize Objective functions
def solve_pid(self,w0,wd,v_base,a1_bias,kd):
"""[summary]
Args:
w0 ([type]): [description]
wd ([type]): [description]
v_base ([type]): [description]
a1_bias ([type]): [description]
kp ([type]): [description]
kd ([type]): [description]
"""
self.w = self.solver.Var(value = w0) # Angular VeLocity of system
self.a1 = self.solver.MV(value = v_base,lb=-255,ub=255) # Power of Wheel 1
self.a1.STATUS = 1
self.a2 = self.solver.MV(value = v_base,ub=255) # Power of Wheel 2
self.a2.STATUS = 1
self.a1_unbound = self.solver.Var(value = v_base) # Unbounded veLocities
self.a2_unbound = self.solver.Var(value = v_base)
self.e = self.solver.Var(value = wd - w0) # Error of angular veLocity (wd - w) wd is desired angular veLocity
self.pid_output = self.solver.Var(self.e*kp) # Output of PID algorithm
self.r = self.solver.Param(value = 2) # distance from rotation center to wheel
self.v_base = self.solver.Param(value = v_base) # Base veLocity of both wheels
self.solver.Equation(self.e == wd - self.w) # Error equation
self.solver.Equation(self.pid_output == self.e*kp + self.e.dt()*kd) # PID algorithm equation
self.solver.Equation(self.a1_unbound == self.v_base + self.pid_output) # Power of wheel 1 based off PID
self.solver.Equation(self.a2_unbound == self.v_base - self.pid_output) # VelocPowerity of wheel 2 based off PID
self.solver.Equation(self.w.dt() == (self.a2-self.a1) * self.r) # Derivative of angular veLocity based on wheel veLocities
self.solver.Obj((self.a2 - self.a2_unbound)**2) # Objective functions to allow bounding of variables
self.solver.Obj((self.a1 - self.a1_unbound)**2)
# self.solver.open_folder()
self.solver.solve(disp=True) # Solve the system of equations and objective functions
def plot_data(self):
"""Will plot
"""
fig = plt.figure(2)
plt.subplot(3,self.w.value,'r-')
plt.ylabel('angular veLocity')
plt.subplot(3,self.a1.value,'b-')
plt.ylabel('a1')
# plt.ylim(-280,280)
plt.subplot(3,self.a2.value,'g-')
plt.ylabel('a2')
# plt.ylim(-280,280)
plt.draw()
plt.show(block=True)
if __name__ == "__main__":
# s = Solver()
# s.solve_pid(x0=0,v0=0,kp=10,ki=0,kd=0,xd=10)
# print(s.x.value)
# s.plot_data()
# r = RobotSolver()
# r.solve_pid(w0 = 2,wd = 0,v_base = 100,a1_bias = 10,kp = 10,kd = 1)
# r.plot_data()
# b = BikeSolver()
# b.solve_open_loop(1,1)
# b.plot_data()
# print(b.x.value,b.y.value,b.yaw.value)
# Define a trajectory
t0 = 0
p0 = Vec3(0,0)
v0 = Vec3(0,5,0)
s0 = StateBuilder.build_from_pos_and_vel(t0,p0,v0)
t1 = 50
p1 = Vec3(50,0)
v1 = Vec3(5**0.5,5**0.5,0)
s1 = StateBuilder.build_from_pos_and_vel(t1,p1,v1)
t2 = 100
p2 = Vec3(100,0)
v2 = Vec3(0,0)
s2 = StateBuilder.build_from_pos_and_vel(t2,p2,v2)
q = TrajectoryBuilder.build_from_3_states(s0,s1,s2)
# Define controller initial input
c0 = ControlInput(0)
b = BikeSolver()
# b.solve_open_loop(1,1)
try:
b.solve_cubic_spline_traj(q,s0,c0,1)
except:
print('did not work')
PrintException()
b.plot_data()
print(b)
Physics.py ---------------------
import numpy as np
import scipy
from scipy.interpolate import Bpoly
import matplotlib.pyplot as plt
class Vec3:
def __init__(self,x,y,z):
self.x = x
self.y = y
self.z = z
self.asnumpy = np.array([x,z])
def normal(self):
mag = self.magnitude()
return np.array([self.x/mag,self.y/mag,self.z/mag],dtype=np.float)
def magnitude(self):
return np.sqrt(self.x**2 + self.y**2 + self.z**2)
class ControlInput:
def __init__(self,delta: float):
self.delta = delta
class State:
time: float
position: Vec3
veLocity: Vec3
orientation: Vec3
ang_vel: Vec3
def __init__(self,t,p,v,o,a):
self.time = t
self.position = p
self.veLocity = v
self.orientation = o
self.ang_vel = a
class StateBuilder:
def __init__(self):
pass
@staticmethod
def build_from_pos_and_vel(t,pos: Vec3,vel: Vec3):
# We kNow based off the bike kinematics that the car's orientation is always aligned with the car's veLocity.
# KNowing this we can calculate the orientation from veLocity directly.
# Calculate orientation from veLocity crossed with unit x vector
ux = Vec3(1,0).asnumpy
# Get normalized veLocity to cross with ux
v_norm = vel.normal()
# Cross v_norm x ux then arcsin to get yaw val in radians
yaw = np.arcsin(np.cross(ux,v_norm))
# Setup orientation vector as roll,pitch,yaw vector
orientation = Vec3(0,yaw[2])
return State(t,pos,vel,orientation,a=Vec3(0,0))
class Trajectory:
states: list
def __init__(self,s:list):
self.states = s
class TrajectoryInterpolator:
def __init__(self):
pass
@staticmethod
def interpolate(q: Trajectory):
# Generate Bpoly class from trajectory states
# Setup time vector,position vectors,first derivative vectors
t = []
x = []
x_dot = []
y = []
y_dot = []
for s in q.states:
t.append(s.time)
x.append(s.position.x)
x_dot.append(s.veLocity.x)
y.append(s.position.y)
y_dot.append(s.veLocity.y)
# Convert to numpy arrays
x = np.array(x,ndmin=2)
x_dot = np.array(x_dot,ndmin=2)
y = np.array(y,ndmin=2)
y_dot = np.array(y_dot,ndmin=2)
# Generate poly spline,np.hstack() sets up the arrays in the proper orientation for the method
x_poly = Bpoly.from_derivatives(t,np.hstack([x.T,x_dot.T]),orders=3)
y_poly = Bpoly.from_derivatives(t,np.hstack([y.T,y_dot.T]),orders=3)
return x_poly,y_poly
# Return Bpoly class as tuple (x,y)
class TrajectoryBuilder:
def __init__(self):
pass
@staticmethod
def build_from_3_states(s0: State,s1: State,s2: State):
states = [s0,s2]
return Trajectory(states)
if __name__ == "__main__":
t0 = 0
p0 = Vec3(-2,8**0.5,v0)
t1 = 1
p1 = Vec3(4,4,0)
v1 = Vec3(2,-5,v1)
t2 = 2
p2 = Vec3(6,0)
v2 = Vec3(8**0.5,s2)
print(q)
xp,yp = TrajectoryInterpolator.interpolate(q)
t_interp = np.linspace(0,t2,101)
plt.figure(1)
x_i = []
y_i = []
x_dot_i = []
y_dot_i = []
for i in t_interp:
x_i.append(xp(i))
y_i.append(yp(i))
x_dot_i.append(xp.derivative()(i))
y_dot_i.append(yp.derivative()(i))
plt.plot(x_i,y_i)
plt.figure(2)
plt.plot(x_dot_i,y_dot_i,'ro')
plt.show()
解决方法
Michael，令人印象深刻的应用程序！很高兴您能够解决问题。您要求进行审查，以确保我没有做任何令人震惊的事情，以进行健全性检查。在当前形式下，一切看起来都不错。我确实看到了您可能打算做的几件事：
- 最终时间作为变量。如果您确实将最终时间设为变量，请不要忘记将所有导数项除以
tf。
self.solver.Equation(self.x.dt()/tf == self.vx)
self.solver.Equation(self.y.dt()/tf == self.vy)
self.solver.Equation(self.yaw.dt()/tf == self.omega_yaw)
- MPC控制器似乎可以快速收敛到解决方案。如果优化程序失败，则可以鼓励学生作为第一步进行初始化。
m.options.IMODE=6
m.options.COLDSTART=1 # 2 is more effective but can take longer
m.solve()
m.options.TIME_SHIFT=0
m.options.COLDSTART=0
m.solve()
-
使用新的
m.Minimize()或m.Maximize()函数而不是m.Obj()来使模型更具可读性。 -
如果需要创建插值函数作为求解的一部分，则有一个cubic spline function in Gekko。看来您正在解决当前问题之后。如果您使用
m.options.NODES=3或更高版本和m.options.DIAGLEVEL=2或更高版本使用results_all.json在本地运行目录中生成GEKKO(remote=False)，则也可以获得内插的并置点，以提高解决方案的可见性。>
# get additional solution information
import json
with open(m.path+'//results_all.json') as f:
results = json.load(f)