From 26fe721334834de9de7669ab147024ef12e7d2f2 Mon Sep 17 00:00:00 2001 From: Liufu27 <2183667346@qq.com> Date: Thu, 18 Dec 2025 21:51:49 +0800 Subject: [PATCH 1/3] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E2=80=9Cmain=5Fmpc=5Fcon?= =?UTF-8?q?troller.py=E2=80=9D=E6=96=87=E4=BB=B6=EF=BC=8C=E5=B9=B6?= =?UTF-8?q?=E8=B0=83=E4=BC=98=E6=B1=82=E8=A7=A3=E5=99=A8=E5=8F=82=E6=95=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../main_mpc_controller.py | 107 ++++++++++++++++++ 1 file changed, 107 insertions(+) create mode 100644 src/Unmanned_vehicle_control/main_mpc_controller.py 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..eb9174ad44 --- /dev/null +++ b/src/Unmanned_vehicle_control/main_mpc_controller.py @@ -0,0 +1,107 @@ +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 From eb6b6928b2c83b27b7ebb6ea380c74b5bb5948af Mon Sep 17 00:00:00 2001 From: Liufu27 <2183667346@qq.com> Date: Fri, 19 Dec 2025 13:19:32 +0800 Subject: [PATCH 2/3] =?UTF-8?q?=E6=9B=B4=E6=96=B0README.md=E6=96=87?= =?UTF-8?q?=E4=BB=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Unmanned_vehicle_control/README.md | 194 +++++++++++++++++++++++++ 1 file changed, 194 insertions(+) diff --git a/src/Unmanned_vehicle_control/README.md b/src/Unmanned_vehicle_control/README.md index e69de29bb2..2696fbf7bf 100644 --- a/src/Unmanned_vehicle_control/README.md +++ b/src/Unmanned_vehicle_control/README.md @@ -0,0 +1,194 @@ +# 无人车模型预测控制(mpc) + +##### 项目概述 + +这是一个基于Carla仿真环境的模型预测控制(MPC)轨迹跟踪系统。项目包含多种轨迹生成算法(8字形、圆形、螺旋形、直线)和MPC控制器,能够实现自动驾驶车辆在指定轨迹上的精确跟踪控制。 + +##### 项目结构 + +├── main.py # 主运行文件 + +├── main\_help\_functions.py # 轨迹生成和辅助函数 + +├── main\_mpc\_controller.py # MPC控制器实现 + +##### 环境依赖 + +###### 必需库 + +\# 基础依赖 + +numpy>=1.20.0 + +casadi>=3.5.0 + +\# Carla仿真(需要Carla 0.9.10+) + +\# Carla Python API通常随Carla安装包提供 + +###### 安装步骤 + +1.安装Anaconda或Miniconda + +2.创建虚拟环境: + +conda create -n mpc\_carla python=3.7 + +conda activate mpc\_carla + +3.下载并安装Carla仿真器(建议0.9.10及以上版本) + +4.将Carla的Python API添加到Python路径中 + +###### 运行说明 + +1\. 启动Carla服务器 + +\# 在Carla安装目录中 + +./CarlaUE4.exe + +2\. 运行MPC控制器 + +python main.py + +3\. 切换轨迹类型 + +在main.py中注释/取消注释相应代码块来切换轨迹 + +##### 功能特性 + +###### 轨迹生成器 + +8字形轨迹:使用参数方程生成8字形路径 + + + +圆形轨迹:可配置圆心和半径的圆形路径 + + + +螺旋轨迹:可配置圈数和缩放因子的螺旋路径 + + + +直线轨迹:简单的直线路径 + + + +###### MPC控制器特性 + +支持预测时域和代价函数权重配置 + + + +包含控制量约束和变化率约束 + + + +内置热启动和求解器优化 + + + +控制缓冲机制处理求解失败情况 + + + +横向偏差计算提高跟踪精度 + + + +###### 可视化功能 + +实时绘制参考轨迹(绿色) + + + +实时绘制预测轨迹(红色) + + + +鸟瞰视角观察车辆运动 + + + +###### 调试和调优 + +###### 常见问题 + +Carla连接失败:确保Carla服务器正在运行,且Python API路径正确 + + + +MPC求解失败:调整config.py中的权重系数,或增加求解时间限制 + + + +车辆控制不稳定:调整dt控制周期或减少控制量限制 + + + +###### 性能调优 + +调整N(预测时域):影响计算复杂度和预测能力 + + + +调整代价函数权重:平衡跟踪精度和控制平滑性 + + + +调整dt:控制频率,影响实时性和计算负荷 + + + +###### 输出数据 + +程序运行时会记录: + + + +车辆状态(位置、速度、航向角) + + + +参考轨迹信息 + + + +控制输出(方向盘、油门、刹车) + + + +MPC计算时间 + + + +##### 注意事项 + +确保Carla版本与Python API版本匹配 + + + +首次运行可能需要较长时间编译CasADi优化问题 + + + +建议在性能较好的计算机上运行,确保实时性 + + + +可调整config.py中的参数以适应不同的跟踪需求 + + + +##### 许可证 + +本项目仅供学习和研究使用,请遵守Carla仿真器的相关使用条款。 + + + +##### 参考 + +https://github.com/Slimercorp/MPC-Carla + From d258d15bf1b0d78a8365197e592bb7f718ce7758 Mon Sep 17 00:00:00 2001 From: Liufu27 <2183667346@qq.com> Date: Fri, 19 Dec 2025 17:10:34 +0800 Subject: [PATCH 3/3] =?UTF-8?q?=E5=9C=A8MPC=E6=8E=A7=E5=88=B6=E5=99=A8?= =?UTF-8?q?=E6=96=87=E4=BB=B6=E4=B8=AD=E6=B7=BB=E5=8A=A0=E7=83=AD=E5=90=AF?= =?UTF-8?q?=E5=8A=A8=E8=A1=A5=E5=85=85?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Unmanned_vehicle_control/main_mpc_controller.py | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/src/Unmanned_vehicle_control/main_mpc_controller.py b/src/Unmanned_vehicle_control/main_mpc_controller.py index eb9174ad44..341513f8e2 100644 --- a/src/Unmanned_vehicle_control/main_mpc_controller.py +++ b/src/Unmanned_vehicle_control/main_mpc_controller.py @@ -34,6 +34,15 @@ def reset_solver(self): } self.opti.solver('ipopt', opts) + # 定义状态和控制变量 + self.X = self.opti.variable(4, self.horizon + 1) + self.U = self.opti.variable(2, self.horizon) + + # 热启动:用上次的解初始化(首次运行可跳过) + if hasattr(self, 'prev_X') and hasattr(self, 'prev_U'): + self.opti.set_initial(self.X, self.prev_X) # 状态初始猜测 + self.opti.set_initial(self.U, self.prev_U) # 控制量初始猜测 + 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) @@ -81,6 +90,9 @@ def solve(self): try: self.sol = self.opti.solve() self.is_success = True + # 保存当前解作为下次的初始猜测 + self.prev_X = self.sol.value(self.X) + self.prev_U = self.sol.value(self.U) 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]