def LMPCinit(self, x, LapTime): print("Starting LMPC") self.TimeLMPC = LapTime self.points = int(self.TimeLMPC/self.dt) self.v0 = x[0] self.ClosedLoopLMPC = ClosedLoopData(self.points, self.v0) self.LMPCOpenLoopData = LMPCprediction(self.N, self.n, self.d, self.TimeLMPC, self.numSS_Points, self.Laps) self.LMPCSimulator = Simulator(self.Dynamic, map, 1, 1) self.LMPController = ControllerLMPC(self.numSS_Points, self.numSS_it, self.N, self.Qslack, self.Qlane, self.Q_LMPC, self.R_LMPC, self.dR_LMPC, self.dt, self.map, self.Laps, self.TimeLMPC, self.LMPC_Solver, self.inputConstr) PID_data = open('D:\CARLA\PythonAPI\examples\data\ClosedLoopDataPID.obj', 'rb') ClosedLoopDataPID = pickle.load(PID_data) LTV_data = open('D:\CARLA\PythonAPI\examples\data\ClosedLoopDataLTV_MPC.obj','rb') ClosedLoopDataLTV_MPC = pickle.load(LTV_data) LTI_data = open('D:\CARLA\PythonAPI\examples\data\ClosedLoopDataLTI_MPC.obj','rb') ClosedLoopDataLTI_MPC = pickle.load(LTI_data) # Run first 4 laps. Remaining laps run by LMPC self.LMPController.addTrajectory(ClosedLoopDataPID) self.LMPController.addTrajectory(ClosedLoopDataLTV_MPC) self.LMPController.addTrajectory(ClosedLoopDataPID) self.LMPController.addTrajectory(ClosedLoopDataLTI_MPC) PID_data.close() LTV_data.close() LTI_data.close() self.count = 0
ClosedLoopDataLTV_MPC = pickle.load(file_data) file_data.close() print("===== TV-MPC terminated") if plotFlagMPC_tv == 1: plotTrajectory(map, ClosedLoopDataLTV_MPC.x, ClosedLoopDataLTV_MPC.x_glob, ClosedLoopDataLTV_MPC.u) plt.show() # ====================================================================================================================== # ============================== LMPC w\ LOCAL LINEAR REGRESSION ====================================================== # ====================================================================================================================== raw_input("Finished TV-MPC - Start LMPC?") print("Starting LMPC") ClosedLoopLMPC = ClosedLoopData(dt, TimeLMPC, v0) LMPCOpenLoopData = LMPCprediction( N, n, d, TimeLMPC, numSS_Points, Laps) #to store open-loop prediction and safe sets LMPCSimulator = Simulator( map, 1, 1) #now this simulator only runs for one lap, with the LMPC flag ON if runPWAFlag == 1: LMPController = PWAControllerLMPC(10, numSS_Points, numSS_it, N, Qslack, Q_LMPC, R_LMPC, dR_LMPC, n, d, shift, dt, map, Laps, TimeLMPC, LMPC_Solver) else: LMPController = ControllerLMPC(numSS_Points, numSS_it, N, Qslack, Q_LMPC, R_LMPC, dR_LMPC, n, d, shift, dt, map, Laps, TimeLMPC, LMPC_Solver) onlyLMPController = ControllerLMPC(numSS_Points, numSS_it, N, Qslack, Q_LMPC, R_LMPC, dR_LMPC, n, d, shift,
simulator.Sim(ClosedLoopDataLTV_MPC, Controller_PathFollowingLTV_MPC) file_data = open(sys.path[0] + '/data/ClosedLoopDataLTV_MPC1.obj', 'w') pickle.dump(ClosedLoopDataLTV_MPC, file_data) file_data.close() else: file_data = open(sys.path[0] + '/data/ClosedLoopDataLTV_MPC1.obj', 'r') ClosedLoopDataLTV_MPC = pickle.load(file_data) file_data.close() print "===== TV-MPC terminated" # ====================================================================================================================== # ===========================================原 LMPC 控制 ====================================================== # ====================================================================================================================== print "Starting LMPC" ClosedLoopLMPC = ClosedLoopDataLMPC(dt, TimeLMPC, v0, Laps) LMPCOpenLoopData = LMPCprediction(N, n, d, TimeLMPC, numSS_Points, Laps) #生成所需大小的初始零矩阵 LMPCSimulator = Simulator(map, 1, 1) LMPController = ControllerLMPC(numSS_Points, numSS_it, N, Qslack, Qlane, Q_LMPC, R_LMPC, dR_LMPC, dt, map, Laps, TimeLMPC, LMPC_Solver, inputConstr) LMPController.addTrajectory(ClosedLoopDataPID) LMPController.addTrajectory(ClosedLoopDataLTV_MPC) x0 = np.zeros((1, n)) x0_glob = np.zeros((1, n)) x0[0, :] = ClosedLoopLMPC.x[0, :] x0_glob[0, :] = ClosedLoopLMPC.x_glob[0, :] if RunLMPC == 1: for it in range(2, Laps):
file_data = open('data/ClosedLoopDataLTI_MPC.obj', 'rb') ClosedLoopDataLTI_MPC = pickle.load(file_data) file_data.close() if plotFlagMPC == 1: plotTrajectory(map, ClosedLoopDataLTI_MPC.x, ClosedLoopDataLTI_MPC.x_glob, ClosedLoopDataLTI_MPC.u) plt.show() # ====================================================================================================================== # ============================== LMPC w\ LTI REGRESSION-=============================================================== # ====================================================================================================================== raw_input("Finished LTI Tracking MPC - Start LTI-LMPC?") ClosedLoopLMPC = ClosedLoopData(dt, TimeLMPC, v0) LMPCOpenLoopData = LMPCprediction( N, n, d, TimeLMPC, numSS_Points, Laps) #to store open-loop prediction and safe sets LMPCSimulator = Simulator( map, 1, 1) #now this simulator only runs for one lap, with the LMPC flag ON if runPWAFlag == 1: LMPController = PWAControllerLMPC(10, numSS_Points, numSS_it, N, Qslack, Q_LMPC, R_LMPC, dR_LMPC, n, d, shift, dt, map, Laps, TimeLMPC, LMPC_Solver) else: LMPController = ControllerLTI_LMPC(numSS_Points, numSS_it, N, Qslack, Q_LMPC, R_LMPC, dR_LMPC, n, d, shift, dt, map, Laps, TimeLMPC, LMPC_Solver) # add previously completed trajectories to Safe Set: