gpt4 book ai didi

gekko - 使用 GEKKO 作为运动自行车模型的不同控制算法的模拟器

转载 作者:行者123 更新时间:2023-12-04 03:50:42 27 4
gpt4 key购买 nike

EDIT2:好吧,我是个白痴。我关于横向误差的数学有问题。我已经将该等式重写为更简单的东西并且它现在可以工作了。我会离开这里以防它对任何人有用。但如果可能的话,我仍然希望有人能确保我没有做任何过分的事情,以进行完整性检查。

编辑:我通过将 heading_error 设为 GEKKO.Var 而不是 GEKKO.Intermediate 来解决我遇到的问题。既然这行得通,我已经尝试将方程的横向误差部分纳入其中。现在解决方案失败了。我认为这可能与我在中间方程中使用变量的方式有关,但我不完全确定。我已将 BikeSolver.py 替换为下面的当前版本。

在我陈述我的问题之前,让我解释一下我在这里尝试使用 GEKKO 做什么,也许我的一些方程建模方法可以改进以解决任何问题。

我有一个自动驾驶汽车学生团队参加 IGVC 竞赛。在我们开始直接在汽车上做任何事情之前,我希望有一个强大的仿真系统,可以让我们大致了解不同的控制算法将如何执行。在这种情况下,我首先为学生打下基础,然后他们可以继续测试不同的控制算法等。最终计划是使用 GEKKO 实时生成 MPC 控制算法。但现在我希望学生在我们进入基于模型的优化之前很好地掌握其他反馈控制方法。

好的,这是总体思路。我们正在使用简化的运动学自行车模型来模拟整辆车的动力学。 gekko 求解器接收到一个具有一些离散状态(位置和速度,我们在 xy 平面上进行控制)的轨迹设置,然后使用 scipy 中的 BPoly 类对该轨迹进行插值,以将轨迹作为一组不同的 GEKKO.parameters 给我们(例如 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,解决方案将失败。 super 奇怪。这是 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, 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 Controller 似乎可以快速收敛到一个解决方案。如果优化器失败,您可以鼓励学生首先进行初始化。
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)

关于gekko - 使用 GEKKO 作为运动自行车模型的不同控制算法的模拟器,我们在Stack Overflow上找到一个类似的问题: https://stackoverflow.com/questions/64451876/

27 4 0
Copyright 2021 - 2024 cfsdn All Rights Reserved 蜀ICP备2022000587号
广告合作:1813099741@qq.com 6ren.com