diff --git a/src/Unmanned_vehicle_control/main_help_functions.py b/src/Unmanned_vehicle_control/main_help_functions.py index ef4e8b76a0..1f42877a6c 100644 --- a/src/Unmanned_vehicle_control/main_help_functions.py +++ b/src/Unmanned_vehicle_control/main_help_functions.py @@ -84,6 +84,52 @@ def get_spiral_trajectory(x_init, y_init, total_points=200, turns=2, scale=2): return x_traj, y_traj, v_ref, theta_ref + +def get_square_trajectory(x_init, y_init, side_length=23, total_points=92): + """生成方形轨迹""" + # 计算方形四个顶点 + points = [ + (x_init, y_init), # 起点 + (x_init + side_length, y_init), # 右顶点 + (x_init + side_length, y_init + side_length), # 右上顶点 + (x_init, y_init + side_length), # 上顶点 + (x_init, y_init) # 回到起点(闭合) + ] + + # 计算每条边的点数(平均分配) + points_per_side = total_points // 4 + t_values = np.linspace(0, 1, points_per_side, endpoint=False) + + x_traj = [] + y_traj = [] + + # 生成四条边的轨迹点 + for i in range(4): + x0, y0 = points[i] + x1, y1 = points[i + 1] + x_segment = x0 + (x1 - x0) * t_values + y_segment = y0 + (y1 - y0) * t_values + x_traj.extend(x_segment) + y_traj.extend(y_segment) + + # 确保总点数正确 + x_traj = np.array(x_traj[:total_points]) + y_traj = np.array(y_traj[:total_points]) + + # 参考速度 + v_ref = [V_REF for _ in range(total_points)] + + # 计算参考角度(切线方向) + theta_ref = [] + for i in range(total_points - 1): + dx = x_traj[i + 1] - x_traj[i] + dy = y_traj[i + 1] - y_traj[i] + theta = np.arctan2(dy, dx) + theta_ref.append(theta) + theta_ref.append(theta_ref[-1]) # 最后一个点保持与前一个相同 + + return x_traj, y_traj, v_ref, theta_ref + def get_ref_trajectory(x_traj, y_traj, theta_traj, current_idx): if current_idx + N < len(x_traj): x_ref = x_traj[current_idx:current_idx + N] @@ -109,5 +155,4 @@ def calculate_lateral_deviation(x, y, x_ref1, y_ref1, x_ref2, y_ref2): if denom > 0.01: return num / denom else: - return 0 - + return 0 \ No newline at end of file diff --git a/src/Unmanned_vehicle_control/main_mpc_controller.py b/src/Unmanned_vehicle_control/main_mpc_controller.py new file mode 100644 index 0000000000..b5fa75d2f3 --- /dev/null +++ b/src/Unmanned_vehicle_control/main_mpc_controller.py @@ -0,0 +1,108 @@ +import casadi as ca + +from src.main_help_functions import calculate_lateral_deviation +from src.config import MAX_CONTROL_WHEEL_ANGLE_RAD, MAX_CONTROL_ACCELERATION_M_S_2, MAX_CONTROL_BRAKING_M_S_2, \ + PATH_TOLERANCE_M, FINE_X_COEF, FINE_Y_COEF, FINE_V_COEF, FINE_STEER_COEF, FINE_ACC_COEF, FINE_STEER_DOT_COEF, \ + FINE_ACC_DOT_COEF, FINE_THETA_COEF, FINE_LATERAL_COEF +from src.main_vehicle_model import vehicle_model + +class MpcController: + def __init__(self, horizon = 10, dt = 0.1): + self.horizon = horizon + self.opti = None + self.dt = dt + self.sol = None + self.cost = None + self.is_success = False + self.X = None + self.Y = None + self.control_buffer = {"acceleration": [0] * self.horizon, "wheel_angle": [0] * self.horizon} + self.buffer_index = 0 + + def reset_solver(self): + self.opti = ca.Opti() + opts = { + 'ipopt.print_level': 0, + 'print_time': 0, + 'ipopt.max_cpu_time': self.dt, # 严格限制求解时间不超过控制周期 + 'ipopt.max_iter': 50, # 减少最大迭代次数(默认300,可根据精度需求降低) + 'ipopt.tol': 1e-3, # 放宽收敛精度(默认1e-8,实时控制可放宽) + 'ipopt.warm_start_init_point': 'yes', # 启用热启动,加速收敛 + 'ipopt.warm_start_bound_push': 1e-6, + 'ipopt.warm_start_slack_bound_push': 1e-6, + 'ipopt.warm_start_mult_bound_push': 1e-6 + } + self.opti.solver('ipopt', opts) + + self.X = self.opti.variable(4, self.horizon + 1) # State: (x, y, theta, v) + self.U = self.opti.variable(2, self.horizon) # Control (delta, a) + + for k in range(self.horizon): + self.opti.subject_to(self.X[:, k + 1] == vehicle_model(self.X[:, k], self.U[:, k], self.dt)) + self.opti.subject_to(self.U[0, k] <= MAX_CONTROL_WHEEL_ANGLE_RAD) + self.opti.subject_to(self.U[0, k] >= -MAX_CONTROL_WHEEL_ANGLE_RAD) + self.opti.subject_to(self.U[1, k] <= MAX_CONTROL_ACCELERATION_M_S_2) + self.opti.subject_to(self.U[1, k] >= MAX_CONTROL_BRAKING_M_S_2) + + def set_init_vehicle_state(self, x, y, theta, v): + self.opti.subject_to(self.X[:, 0] == ca.vertcat(x, y, theta, v)) + + def update_cost_function(self, x_ref, y_ref, theta_ref, v_ref): + self.cost = 0 + for k in range(self.horizon): + fine_x = ca.if_else(ca.fabs(self.X[0, k] - x_ref[k]) > PATH_TOLERANCE_M, + FINE_X_COEF * (self.X[0, k] - x_ref[k]) ** 2, 0) + fine_y = ca.if_else(ca.fabs(self.X[1, k] - y_ref[k]) > PATH_TOLERANCE_M, + FINE_Y_COEF * (self.X[1, k] - y_ref[k]) ** 2, 0) + fine_v = FINE_V_COEF * (self.X[3, k] - v_ref[k]) ** 2 + fine_steer = FINE_STEER_COEF * self.U[0, k] ** 2 + fine_acc = FINE_ACC_COEF * self.U[1, k] ** 2 + + fine_theta = FINE_THETA_COEF * (self.X[2, k] - theta_ref[k]) ** 2 + + if k < self.horizon - 1: + lateral_deviation = calculate_lateral_deviation(self.X[0, k], self.X[1, k], x_ref[k], y_ref[k], x_ref[k + 1], y_ref[k + 1]) + else: + lateral_deviation = 0 + + fine_lateral = FINE_LATERAL_COEF * lateral_deviation ** 2 + + fine_steer_dot = 0 + fine_acc_dot = 0 + if k > 0: + fine_steer_dot += FINE_STEER_DOT_COEF * (self.U[0, k] - self.U[0, k - 1]) ** 2 + fine_acc_dot += FINE_ACC_DOT_COEF * (self.U[1, k] - self.U[1, k - 1]) ** 2 + + self.cost += fine_x + fine_y + fine_v + fine_steer + fine_acc + fine_steer_dot + fine_acc_dot + fine_theta + fine_lateral + + self.opti.minimize(self.cost) + + def solve(self): + try: + self.sol = self.opti.solve() + self.is_success = True + + wheel_angle_rad, acceleration_m_s_2 = self.get_controls_value() + self.control_buffer["acceleration"] = self.control_buffer["acceleration"][1:] + [acceleration_m_s_2] + self.control_buffer["wheel_angle"] = self.control_buffer["wheel_angle"][1:] + [wheel_angle_rad] + self.buffer_index = 0 + except Exception: + print("Error or delay upon MPC solution calculation. Previous calculated control value will be used") + self.is_success = False + + def get_controls_value(self): + if self.is_success: + wheel_angle_rad = self.sol.value(self.U[0, 0]) + acceleration_m_s_2 = self.sol.value(self.U[1, 0]) + else: + if self.buffer_index < self.horizon: + acceleration_m_s_2 = self.control_buffer["acceleration"][self.buffer_index] + wheel_angle_rad = self.control_buffer["wheel_angle"][self.buffer_index] + self.buffer_index += 1 + else: + raise Exception("Control buffer is empty.") + + return wheel_angle_rad, acceleration_m_s_2 + + def get_optimized_cost(self): + return self.sol.value(self.cost) \ No newline at end of file