使用 GEKKO 作为运动自行车模型的不同控制算法的模拟器
Using GEKKO as a simulator for a kinematic bike model's different control algorithms
EDIT2：好吧，我是个白痴。我关于横向误差的数学有问题。我已经将该等式重写为更简单的东西并且它现在可以工作了。我会离开这里以防它对任何人有用。但如果可能的话，我仍然希望有人能确保我没有做任何过分的事情，以进行完整性检查。
编辑：我通过将 heading_error 设为 GEKKO.Var 而不是 GEKKO.Intermediate 来解决我遇到的问题。既然这行得通，我已经尝试将方程的横向误差部分纳入其中。现在解决方案失败了。我认为这可能与我在中间方程中使用变量的方式有关，但我不完全确定。我已经用下面的当前版本替换了 BikeSolver.py。
在我陈述我的问题之前，让我解释一下我在这里尝试使用 GEKKO 做什么，也许我的一些方程建模方法可以改进以解决任何问题。
我有一个自动驾驶汽车学生团队参加 IGVC 竞赛。在我们开始直接在汽车上做任何事情之前，我希望有一个强大的仿真系统，可以让我们大致了解不同的控制算法将如何执行。在这种情况下，我首先为学生打下基础，然后他们可以继续测试不同的控制算法等。最终计划是使用 GEKKO 实时生成 MPC 控制算法。但是现在我希望学生们在我们进入基于模型的优化之前能够很好地掌握其他反馈控制方法。
好的，这是总体思路。我们正在使用简化的运动学自行车模型来模拟整辆车的动力学。 Gekko 求解器接收到一个带有一些离散状态（位置和速度，我们在 xy 平面上控制）的轨迹设置，然后使用 scipy 中的 BPoly class 对这个轨迹进行插值，得到轨迹作为一堆不同的 GEKKO.parameters（例如 x_interp、y_interp、vx_interp、vy_interp、yaw_interp、vmag_interp ).这一切都很好，我很幸运能够弄清楚如何让这种方法适用于不同的 solver.time 向量。
最后，我想使用一些矢量数学来生成转向输入控件。例如，使用“航向误差”来生成转向输入以及“横向误差”。这两种方法需要一些叉积和什么没有得到有用的值才能变成转向命令。
所以我现在遇到一个问题，我正在计算一个简单的航向误差 heading_error = yaw_interp - 偏航，其中 yaw_interp 是从期望的轨迹，偏航是状态方向变量。出于某种原因，当我将转向变量 (delta) 设置为等于 heading_error 时，如果我不将 heading_error 乘以 2，则解决方案将失败。超级奇怪。这是 BikeSolver.py.
中的第 118 行
所以很好奇有人认为我犯了什么错误，如果有更多 elegant/robust 方法可以使用 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, lineno, 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, vx, 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, 1, 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, 1, 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, 1, 4)
plt.plot(self.solver.time, self.delta_control.value, 'g-')
plt.ylabel('steering val')
# plt.ylim(-280, 280)
plt.subplot(num_subplots, 1, 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, 1, 6)
plt.plot(self.solver.time, self.a, 'y-')
plt.ylabel('acceleration')
plt.subplot(num_subplots, 1, 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, v0, 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, 1, 2)
plt.plot(self.solver.time, self.v.value, 'r-')
plt.ylabel('velocity')
plt.subplot(3, 1, 1)
plt.plot(self.solver.time, self.x.value, 'r-')
plt.ylabel('position')
plt.subplot(3, 1, 3)
plt.plot(self.solver.time, 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, 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, w0, wd, v_base, a1_bias, kp, 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, lb=-255, 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, 1, 1)
plt.plot(self.solver.time, self.w.value, 'r-')
plt.ylabel('angular velocity')
plt.subplot(3, 1, 2)
plt.plot(self.solver.time, self.a1.value, 'b-')
plt.ylabel('a1')
# plt.ylim(-280, 280)
plt.subplot(3, 1, 3)
plt.plot(self.solver.time, 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, 1)
# b.plot_data()
# print(b.x.value, b.y.value, b.yaw.value)
# Define a trajectory
t0 = 0
p0 = Vec3(0,0,0)
v0 = Vec3(0,5,0)
s0 = StateBuilder.build_from_pos_and_vel(t0, p0, v0)
t1 = 50
p1 = Vec3(50,0,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,0)
v2 = Vec3(0,1,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, 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, y, 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,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,0,yaw[2])
return State(t, pos, vel, orientation, a=Vec3(0,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, s1, s2]
return Trajectory(states)
if __name__ == "__main__":
t0 = 0
p0 = Vec3(-2,1,0)
v0 = Vec3(0,8**0.5,0)
s0 = StateBuilder.build_from_pos_and_vel(t0, p0, v0)
t1 = 1
p1 = Vec3(4,4,0)
v1 = Vec3(2,-5,0)
s1 = StateBuilder.build_from_pos_and_vel(t1, p1, v1)
t2 = 2
p2 = Vec3(6,5,0)
v2 = Vec3(8**0.5,0,0)
s2 = StateBuilder.build_from_pos_and_vel(t2, p2, v2)
q = TrajectoryBuilder.build_from_3_states(s0, s1, 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()
迈克尔，令人印象深刻的申请！很高兴您能够解决问题。您要求进行审查 以确保我没有做任何过分的事情，以进行健全性检查。 当前形式的一切看起来都不错。我确实看到了您可能打算做的几件事：
- 最终时间作为变量。如果您确实将最终时间设为变量，请不要忘记将所有导数项除以
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)
EDIT2：好吧，我是个白痴。我关于横向误差的数学有问题。我已经将该等式重写为更简单的东西并且它现在可以工作了。我会离开这里以防它对任何人有用。但如果可能的话，我仍然希望有人能确保我没有做任何过分的事情，以进行完整性检查。
编辑：我通过将 heading_error 设为 GEKKO.Var 而不是 GEKKO.Intermediate 来解决我遇到的问题。既然这行得通，我已经尝试将方程的横向误差部分纳入其中。现在解决方案失败了。我认为这可能与我在中间方程中使用变量的方式有关，但我不完全确定。我已经用下面的当前版本替换了 BikeSolver.py。
在我陈述我的问题之前，让我解释一下我在这里尝试使用 GEKKO 做什么，也许我的一些方程建模方法可以改进以解决任何问题。
我有一个自动驾驶汽车学生团队参加 IGVC 竞赛。在我们开始直接在汽车上做任何事情之前，我希望有一个强大的仿真系统，可以让我们大致了解不同的控制算法将如何执行。在这种情况下，我首先为学生打下基础，然后他们可以继续测试不同的控制算法等。最终计划是使用 GEKKO 实时生成 MPC 控制算法。但是现在我希望学生们在我们进入基于模型的优化之前能够很好地掌握其他反馈控制方法。
好的，这是总体思路。我们正在使用简化的运动学自行车模型来模拟整辆车的动力学。 Gekko 求解器接收到一个带有一些离散状态（位置和速度，我们在 xy 平面上控制）的轨迹设置，然后使用 scipy 中的 BPoly class 对这个轨迹进行插值，得到轨迹作为一堆不同的 GEKKO.parameters（例如 x_interp、y_interp、vx_interp、vy_interp、yaw_interp、vmag_interp ).这一切都很好，我很幸运能够弄清楚如何让这种方法适用于不同的 solver.time 向量。
最后，我想使用一些矢量数学来生成转向输入控件。例如，使用“航向误差”来生成转向输入以及“横向误差”。这两种方法需要一些叉积和什么没有得到有用的值才能变成转向命令。
所以我现在遇到一个问题，我正在计算一个简单的航向误差 heading_error = yaw_interp - 偏航，其中 yaw_interp 是从期望的轨迹，偏航是状态方向变量。出于某种原因，当我将转向变量 (delta) 设置为等于 heading_error 时，如果我不将 heading_error 乘以 2，则解决方案将失败。超级奇怪。这是 BikeSolver.py.
中的第 118 行所以很好奇有人认为我犯了什么错误，如果有更多 elegant/robust 方法可以使用 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, lineno, 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, vx, 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, 1, 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, 1, 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, 1, 4)
plt.plot(self.solver.time, self.delta_control.value, 'g-')
plt.ylabel('steering val')
# plt.ylim(-280, 280)
plt.subplot(num_subplots, 1, 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, 1, 6)
plt.plot(self.solver.time, self.a, 'y-')
plt.ylabel('acceleration')
plt.subplot(num_subplots, 1, 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, v0, 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, 1, 2)
plt.plot(self.solver.time, self.v.value, 'r-')
plt.ylabel('velocity')
plt.subplot(3, 1, 1)
plt.plot(self.solver.time, self.x.value, 'r-')
plt.ylabel('position')
plt.subplot(3, 1, 3)
plt.plot(self.solver.time, 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, 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, w0, wd, v_base, a1_bias, kp, 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, lb=-255, 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, 1, 1)
plt.plot(self.solver.time, self.w.value, 'r-')
plt.ylabel('angular velocity')
plt.subplot(3, 1, 2)
plt.plot(self.solver.time, self.a1.value, 'b-')
plt.ylabel('a1')
# plt.ylim(-280, 280)
plt.subplot(3, 1, 3)
plt.plot(self.solver.time, 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, 1)
# b.plot_data()
# print(b.x.value, b.y.value, b.yaw.value)
# Define a trajectory
t0 = 0
p0 = Vec3(0,0,0)
v0 = Vec3(0,5,0)
s0 = StateBuilder.build_from_pos_and_vel(t0, p0, v0)
t1 = 50
p1 = Vec3(50,0,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,0)
v2 = Vec3(0,1,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, 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, y, 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,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,0,yaw[2])
return State(t, pos, vel, orientation, a=Vec3(0,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, s1, s2]
return Trajectory(states)
if __name__ == "__main__":
t0 = 0
p0 = Vec3(-2,1,0)
v0 = Vec3(0,8**0.5,0)
s0 = StateBuilder.build_from_pos_and_vel(t0, p0, v0)
t1 = 1
p1 = Vec3(4,4,0)
v1 = Vec3(2,-5,0)
s1 = StateBuilder.build_from_pos_and_vel(t1, p1, v1)
t2 = 2
p2 = Vec3(6,5,0)
v2 = Vec3(8**0.5,0,0)
s2 = StateBuilder.build_from_pos_and_vel(t2, p2, v2)
q = TrajectoryBuilder.build_from_3_states(s0, s1, 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()
迈克尔，令人印象深刻的申请！很高兴您能够解决问题。您要求进行审查 以确保我没有做任何过分的事情，以进行健全性检查。 当前形式的一切看起来都不错。我确实看到了您可能打算做的几件事：
- 最终时间作为变量。如果您确实将最终时间设为变量，请不要忘记将所有导数项除以
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)