dR_LMPC = 10 * np.array([1.0, 10.0]) # Input rate cost u
#输入约束的边界值
inputConstr = np.array([
[0.5, 0.5], # Min Steering and Max Steering
[1.0, 1.0]
]) # Min Acceleration and Max Acceleration
# Initialize LMPC simulator
LMPCSimulator = Simulator(map, 1, 1)
# ======================================================================================================================
# ======================================= PID path following ===========================================================
# ======================================================================================================================
print "Starting PID"
if RunPID == 1:
ClosedLoopDataPID = ClosedLoopData(dt, Time, v0)
PIDController = PID(vt)
simulator.Sim(ClosedLoopDataPID, PIDController)
file_data = open(sys.path[0] + '/data/ClosedLoopDataPID1.obj', 'wb')
pickle.dump(ClosedLoopDataPID, file_data)
file_data.close()
else:
file_data = open(sys.path[0] + '/data/ClosedLoopDataPID1.obj', 'rb')
ClosedLoopDataPID = pickle.load(file_data)
file_data.close()
print "===== PID terminated"
# ======================================================================================================================
# ====================================== LINEAR REGRESSION ============================================================
# ======================================================================================================================
print "Starting MPC"
]) # State cost x = [vx, vy, wz, epsi, s, ey]
R_LMPC = 1 * np.diag([1.0, 1.0]) # Input cost u = [delta, a]
dR_LMPC = 5 * np.array([1.0, 1.0]) # Input rate cost u
# Initialize LMPC simulator
LMPCSimulator = Simulator(
map, 1, 1) #flags indicate one lap, and using the LMPC controller
# ======================================================================================================================
# ======================================= PID path following ===========================================================
# ======================================================================================================================
print("Starting PID")
if RunPID == 1:
ClosedLoopDataPID = ClosedLoopData(dt, Time,
v0) #form matrices for experiment data
PIDController = PID(vt) #sets the reference velocity and some timers?
simulator.Sim(
ClosedLoopDataPID,
PIDController) #simulates the PID controller for Time timesteps
file_data = open('data/ClosedLoopDataPID.obj', 'wb')
pickle.dump(ClosedLoopDataPID, file_data)
file_data.close()
else:
file_data = open('data/ClosedLoopDataPID.obj', 'rb')
ClosedLoopDataPID = pickle.load(file_data)
file_data.close()
print("===== PID terminated")
if plotFlag == 1:
plotTrajectory(map, ClosedLoopDataPID.x, ClosedLoopDataPID.x_glob,
ClosedLoopDataPID.u)
def PIDinit(self, x):
self.v0 = x[0]
self.ClosedLoopDataPID = ClosedLoopData(self.points , self.v0)
self.PIDController = PID(self.vt)
print("Starting PID")
self.count = 0