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/8] =?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/8] =?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/8] =?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] From d6edc67ab0e265e514d9a00fed708e6e33104b2b Mon Sep 17 00:00:00 2001 From: Liufu27 <2183667346@qq.com> Date: Sat, 20 Dec 2025 14:29:01 +0800 Subject: [PATCH 4/8] =?UTF-8?q?=E6=8F=90=E4=BA=A4Carla=E4=BA=A4=E4=BA=92?= =?UTF-8?q?=E6=96=87=E4=BB=B6=EF=BC=8C=E5=B9=B6=E4=B8=94=E6=B7=BB=E5=8A=A0?= =?UTF-8?q?=E6=84=9F=E7=9F=A5=E8=A7=84=E5=88=92=E7=9B=B8=E5=85=B3=E7=9A=84?= =?UTF-8?q?=E7=BB=98=E5=88=B6=E6=96=B9=E6=B3=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../main_carla_simulator.py | 301 ++++++++++++++++++ 1 file changed, 301 insertions(+) create mode 100644 src/Unmanned_vehicle_control/main_carla_simulator.py diff --git a/src/Unmanned_vehicle_control/main_carla_simulator.py b/src/Unmanned_vehicle_control/main_carla_simulator.py new file mode 100644 index 0000000000..e9f0a77f97 --- /dev/null +++ b/src/Unmanned_vehicle_control/main_carla_simulator.py @@ -0,0 +1,301 @@ +import carla # 导入Carla客户端库,用于与Carla模拟器交互 +import numpy as np # 导入numpy库,用于数值计算 + +# 从配置文件导入车辆控制相关的常量 +from src.config import MAX_BRAKING_M_S_2, MAX_WHEEL_ANGLE_RAD, MAX_ACCELERATION_M_S_2 + + +class CarlaSimulator: + """Carla模拟器交互类,封装了与Carla模拟器的主要交互操作""" + + def __init__(self): + """初始化Carla模拟器连接及相关参数""" + # 连接到本地Carla服务器(默认地址localhost:2000) + self.client = carla.Client('localhost', 2000) + # 设置客户端超时时间(10秒) + self.client.set_timeout(10.0) + # 获取当前世界对象 + self.world = self.client.get_world() + # 初始化主车辆(ego vehicle)为None + self.ego_vehicle = None + # 销毁场景中所有现有车辆(初始化环境) + for vehicle in self.world.get_actors().filter('*vehicle*'): + vehicle.destroy() + + def load_world(self, map_name): + """ + 加载指定名称的Carla地图 + + 参数: + map_name: 地图名称(如'Town01') + """ + self.client.load_world(map_name) + + def spawn_ego_vehicle(self, vehicle_name, x=0, y=0, z=0, pitch=0, yaw=0, roll=0): + """ + 在指定位置生成主车辆(ego vehicle) + + 参数: + vehicle_name: 车辆蓝图名称(用于筛选车辆模型) + x, y, z: 生成位置的坐标(米) + pitch, yaw, roll: 生成时的旋转角度(度),分别对应俯仰角、偏航角、横滚角 + """ + # 获取当前世界的蓝图库 + blueprint_library = self.world.get_blueprint_library() + # 根据车辆名称筛选蓝图(取第一个匹配结果) + vehicle_bp = blueprint_library.filter(vehicle_name)[0] + # 设置生成位置 + spawn_location = carla.Location(x, y, z) + # 设置生成旋转角度 + spawn_rotation = carla.Rotation(pitch, yaw, roll) + # 组合位置和旋转为变换矩阵 + spawn_transform = carla.Transform(location=spawn_location, rotation=spawn_rotation) + # 在指定位置生成车辆并赋值给主车辆 + self.ego_vehicle = self.world.spawn_actor(vehicle_bp, spawn_transform) + + def set_spectator(self, x=0, y=0, z=0, pitch=0, yaw=0, roll=0): + """ + 设置 spectator( spectator 是Carla中的视角控制者)的位置和角度 + + 参数: + x, y, z: spectator 位置坐标(米) + pitch, yaw, roll: spectator 旋转角度(度) + """ + # 获取当前spectator + spectator = self.world.get_spectator() + # 设置位置 + location = carla.Location(x=x, y=y, z=z) + # 设置旋转角度 + rotation = carla.Rotation(pitch=pitch, yaw=yaw, roll=roll) + # 组合为变换矩阵 + spectator_transform = carla.Transform(location, rotation) + # 应用变换到spectator + spectator.set_transform(spectator_transform) + + def clean(self): + """清理场景中所有车辆(销毁所有车辆actor)""" + for vehicle in self.world.get_actors().filter('*vehicle*'): + vehicle.destroy() + + def draw_perception_planning(self, x_ref, y_ref, current_idx, look_ahead_points=20): + """ + 绘制感知规划信息:只显示车辆前方需要跟踪的轨迹点 + + 参数: + x_ref: 参考轨迹x坐标 + y_ref: 参考轨迹y坐标 + current_idx: 当前轨迹点索引 + look_ahead_points: 向前看的点数 + """ + # 清除之前的绘制(通过绘制空列表) + self.draw_trajectory([], [], life_time=0.1) + + # 计算要显示的轨迹点范围 + total_points = len(x_ref) + if total_points == 0: + return + + # 获取前方轨迹点 + end_idx = min(current_idx + look_ahead_points, total_points) + + # 提取要显示的轨迹点 + if current_idx < end_idx: + display_x = x_ref[current_idx:end_idx] + display_y = y_ref[current_idx:end_idx] + else: + # 处理循环轨迹 + display_x = list(x_ref[current_idx:]) + list(x_ref[:end_idx]) + display_y = list(y_ref[current_idx:]) + list(y_ref[:end_idx]) + + # 绘制前方轨迹(绿色) + if len(display_x) > 1: + self.draw_trajectory( + display_x, + display_y, + height=0.5, + thickness=0.15, + green=255, + life_time=0.1 + ) + + # 绘制当前目标点(红色) + if len(display_x) > 0: + target_location = carla.Location( + x=display_x[0], + y=display_y[0], + z=0.5 + ) + self.world.debug.draw_point( + target_location, + size=0.3, + color=carla.Color(255, 0, 0), + life_time=0.1 + ) + + # 绘制从车辆到目标点的连线 + vehicle_location = self.ego_vehicle.get_transform().location + self.world.debug.draw_line( + vehicle_location, + target_location, + thickness=0.1, + color=carla.Color(0, 255, 255), + life_time=0.1 + ) + + def draw_frenet_frame(self, x_ref, y_ref, current_idx): + """ + 绘制Frenet坐标系参考线(用于显示横向偏差) + """ + total_points = len(x_ref) + if total_idx + 1 >= total_points: + return + + # 获取当前参考线段 + x1, y1 = x_ref[current_idx], y_ref[current_idx] + x2, y2 = x_ref[(current_idx + 1) % total_points], y_ref[(current_idx + 1) % total_points] + + # 绘制参考线段(蓝色) + start_point = carla.Location(x=x1, y=y1, z=0.3) + end_point = carla.Location(x=x2, y=y2, z=0.3) + + self.world.debug.draw_line( + start_point, + end_point, + thickness=0.08, + color=carla.Color(0, 0, 255), + life_time=0.1 + ) + def draw_trajectory(self, x_traj, y_traj, height=0, thickness=0.1, red=0, green=0, blue=0, life_time=0.1): + """ + 在世界中绘制轨迹线 + + 参数: + x_traj: 轨迹点的x坐标列表 + y_traj: 轨迹点的y坐标列表 + height: 轨迹线的z轴高度(米) + thickness: 线的粗细 + red, green, blue: 线的颜色(0-255) + life_time: 线在世界中的存在时间(秒) + """ + # 遍历轨迹点,绘制连续线段 + for i in range(len(x_traj) - 1): + # 线段起点 + start_point = carla.Location(x=x_traj[i], y=y_traj[i], z=height) + # 线段终点 + end_point = carla.Location(x=x_traj[i + 1], y=y_traj[i + 1], z=height) + # 在世界中绘制线段 + self.world.debug.draw_line( + start_point, + end_point, + thickness=thickness, + color=carla.Color(red, green, blue), + life_time=life_time + ) + + def get_main_ego_vehicle_state(self): + """ + 获取主车辆的当前状态 + + 返回: + x: 车辆位置x坐标(米) + y: 车辆位置y坐标(米) + theta: 车辆航向角(弧度,从yaw角度转换而来) + v: 车辆瞬时速度(米/秒,x和y方向速度的合速度) + """ + # 获取车辆变换信息(位置和旋转) + transform = self.ego_vehicle.get_transform() + x = transform.location.x + y = transform.location.y + # 将yaw角度(度)转换为弧度(航向角) + theta = np.deg2rad(transform.rotation.yaw) + # 计算x和y方向速度的合速度(标量) + v = np.sqrt(self.ego_vehicle.get_velocity().x ** 2 + self.ego_vehicle.get_velocity().y ** 2) + return x, y, theta, v + + def apply_control(self, steer, throttle, brake): + """ + 向主车辆应用控制指令 + + 参数: + steer: 转向指令(-1到1之间,对应左右转向) + throttle: 油门指令(0到1之间) + brake: 刹车指令(0到1之间) + """ + # 调用Carla的VehicleControl接口应用控制 + self.ego_vehicle.apply_control(carla.VehicleControl(throttle=throttle, steer=steer, brake=brake)) + + @staticmethod + def process_control_inputs(wheel_angle_rad, acceleration_m_s_2): + """ + 处理原始控制输入(轮角和加速度),转换为Carla需要的标准化控制指令 + + 参数: + wheel_angle_rad: 车轮转角(弧度) + acceleration_m_s_2: 加速度指令(米/秒²,正为加速,负为刹车) + + 返回: + throttle: 标准化油门指令(0到1) + brake: 标准化刹车指令(0到1) + steer: 标准化转向指令(-1到1) + """ + if acceleration_m_s_2 == 0: + # 无加速度指令时,油门和刹车均为0 + throttle = 0 + brake = 0 + elif acceleration_m_s_2 < 0: + # 负加速度(刹车),标准化到刹车指令(除以最大刹车加速度) + throttle = 0 + brake = acceleration_m_s_2 / MAX_BRAKING_M_S_2 + else: + # 正加速度(加速),标准化到油门指令(除以最大加速度) + throttle = acceleration_m_s_2 / MAX_ACCELERATION_M_S_2 + brake = 0 + # 车轮转角标准化(除以最大轮角) + steer = wheel_angle_rad / MAX_WHEEL_ANGLE_RAD + return throttle, brake, steer + + def print_ego_vehicle_characteristics(self): + """打印主车辆的物理特性参数(如车轮信息、扭矩曲线、质量等)""" + if not self.ego_vehicle: + print("Vehicle not spawned yet!") # 车辆未生成时提示 + return None + + # 获取车辆物理控制参数 + physics_control = self.ego_vehicle.get_physics_control() + + print("Vehicle Physics Information.\n") + + # 打印车轮信息 + print("Wheel Information:") + for i, wheel in enumerate(physics_control.wheels): + print(f" Wheel {i + 1}:") + print(f" Tire Friction: {wheel.tire_friction}") # 轮胎摩擦系数 + print(f" Damping Rate: {wheel.damping_rate}") # 阻尼率 + print(f" Max Steer Angle: {wheel.max_steer_angle}") # 最大转向角 + print(f" Radius: {wheel.radius}") # 车轮半径 + print(f" Max Brake Torque: {wheel.max_brake_torque}") # 最大刹车扭矩 + print(f" Max Handbrake Torque: {wheel.max_handbrake_torque}") # 最大手刹扭矩 + print(f" Position (x, y, z): ({wheel.position.x}, {wheel.position.y}, {wheel.position.z})") # 车轮位置 + + # 打印扭矩曲线(RPM-扭矩关系) + print(f" Torque Curve:") + for point in physics_control.torque_curve: + print(f"RPM: {point.x}, Torque: {point.y}") + print(f" Max RPM: {physics_control.max_rpm}") # 最大转速 + print(f" MOI (Moment of Inertia): {physics_control.moi}") # 转动惯量 + print(f" Damping Rate Full Throttle: {physics_control.damping_rate_full_throttle}") # 全油门时的阻尼率 + print( + f" Damping Rate Zero Throttle Clutch Engaged: {physics_control.damping_rate_zero_throttle_clutch_engaged}") # 零油门离合器结合时的阻尼率 + print( + f" Damping Rate Zero Throttle Clutch Disengaged: {physics_control.damping_rate_zero_throttle_clutch_disengaged}") # 零油门离合器分离时的阻尼率 + print( + f" If True, the vehicle will have an automatic transmission: {physics_control.use_gear_autobox}") # 是否自动变速箱 + print(f" Gear Switch Time: {physics_control.gear_switch_time}") # 换挡时间 + print(f" Clutch Strength: {physics_control.clutch_strength}") # 离合器强度 + print(f" Final Ratio: {physics_control.final_ratio}") # 最终传动比 + print(f" Mass: {physics_control.mass}") # 车辆质量 + print(f" Drag coefficient: {physics_control.drag_coefficient}") # 空气阻力系数 + # 打印转向曲线(速度-转向关系) + print(f" Steering Curve:") + for point in physics_control.steering_curve: + print(f"Speed: {point.x}, Steering: {point.y}") \ No newline at end of file From 561e2df11d825a613a05c1a2350504f001ceff98 Mon Sep 17 00:00:00 2001 From: Liufu27 <2183667346@qq.com> Date: Sat, 20 Dec 2025 21:08:04 +0800 Subject: [PATCH 5/8] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E9=85=8D=E7=BD=AE?= =?UTF-8?q?=E6=96=87=E4=BB=B6=EF=BC=8C=E5=B9=B6=E4=BF=AE=E6=94=B9=EF=BC=8C?= =?UTF-8?q?=E5=B9=B6=E4=B8=94=E6=9B=B4=E6=96=B0main?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Unmanned_vehicle_control/config.py | 44 +++++++++ src/Unmanned_vehicle_control/main.py | 125 ++++++++++++++++--------- 2 files changed, 123 insertions(+), 46 deletions(-) create mode 100644 src/Unmanned_vehicle_control/config.py diff --git a/src/Unmanned_vehicle_control/config.py b/src/Unmanned_vehicle_control/config.py new file mode 100644 index 0000000000..ede562973f --- /dev/null +++ b/src/Unmanned_vehicle_control/config.py @@ -0,0 +1,44 @@ +import numpy as np + +X_INIT_M = 210 # Start X coordinate for vehicle +Y_INIT_M = 362 # Start Y coordinate for vehicle +A = 25 # Size of 8-trajectory +LAPS = 1 #Laps of simulation + +MAX_ACCELERATION_M_S_2 = 10 # Vehicle characteristics (from CARLA simulator, see get_physics_control()) +MAX_BRAKING_M_S_2 = -4.1 # Vehicle characteristics (from CARLA simulator, see get_physics_control()) +MAX_WHEEL_ANGLE_RAD = 70 / 180 * np.pi # Vehicle characteristics (from CARLA simulator, see get_physics_control()) +L = 2.875 #The wheelbase length of the vehicle (meters) + +### MPC settings +N = 5 # Horizon of planning for MPC controller +dt = 0.1 #Sample time of control +MAX_CONTROL_WHEEL_ANGLE_RAD = 70 / 180 * np.pi # Maximum value which can use MPC controller in terms of steering +MAX_CONTROL_ACCELERATION_M_S_2 = 10 # Maximum value which can use MPC controller in terms of acceleration +MAX_CONTROL_BRAKING_M_S_2 = -4.1 # Maximum value which can use MPC controller in terms of braking +PATH_TOLERANCE_M = 0.5 # Acceptance radius of each waypoint +V_REF = 5 #Speed of vehicle + +FINE_X_COEF = 10 #Fine for deviation from reference X coordinate +FINE_Y_COEF = 10 #Fine for deviation from reference Y coordinate +FINE_STEER_COEF = 0 #Fine for use high value of steering +FINE_ACC_COEF = 0 #Fine for use high values of acceleration +FINE_STEER_DOT_COEF = 100 #Fine for rapid steering +FINE_ACC_DOT_COEF = 1 #Fine for rapid changing of acceleration +FINE_V_COEF = 20 #Fine for deviation from reference speed +FINE_THETA_COEF = 0 #Fine for deviation from reference theta +FINE_LATERAL_COEF = 40 #Fine for deviation from path line +# 圆形轨迹参数 +CIRCLE_CENTER_X = X_INIT_M # 圆心X坐标 +CIRCLE_CENTER_Y = Y_INIT_M # 圆心Y坐标 +CIRCLE_RADIUS = 50 # 圆半径 + +#添加 +VISUALIZATION_CONFIG = { + 'show_full_trajectory': False, # 是否显示完整轨迹 + 'show_perception_planning': True, # 是否显示感知规划信息 + 'show_frenet_frame': False, # 是否显示Frenet坐标系 + 'look_ahead_points': N, # 向前看的点数 + 'target_point_color': (255, 0, 0), # 目标点颜色 (R, G, B) + 'trajectory_color': (0, 255, 0), # 轨迹线颜色 +} \ No newline at end of file diff --git a/src/Unmanned_vehicle_control/main.py b/src/Unmanned_vehicle_control/main.py index 0973b4c355..c91e02ee7e 100644 --- a/src/Unmanned_vehicle_control/main.py +++ b/src/Unmanned_vehicle_control/main.py @@ -3,70 +3,36 @@ import numpy as np from src.main_carla_simulator import CarlaSimulator from src.config import X_INIT_M, Y_INIT_M, N, dt, V_REF, LAPS -from src.main_help_functions import get_eight_trajectory, get_ref_trajectory, get_circle_trajectory, get_spiral_trajectory, update_reference_point +from src.main_help_functions import get_eight_trajectory, get_ref_trajectory, get_circle_trajectory, \ + get_spiral_trajectory, get_square_trajectory, update_reference_point from src.main_logger import Logger from src.main_mpc_controller import MpcController +""" +# 注释掉的轨迹绘制线程 def draw_trajectory_in_thread(carla, x_traj, y_traj, dt): while True: carla.draw_trajectory(x_traj, y_traj, height=0.2, green=255, life_time=dt * 2) time.sleep(dt) +""" carla = CarlaSimulator() carla.load_world('Town02_Opt') -#carla.spawn_ego_vehicle('vehicle.tesla.model3', x=X_INIT_M, y=Y_INIT_M, z=0.1) #“8”字形 +carla.spawn_ego_vehicle('vehicle.tesla.model3', x=X_INIT_M, y=Y_INIT_M, z=0.1) # "8"字形 carla.print_ego_vehicle_characteristics() carla.set_spectator(X_INIT_M, Y_INIT_M, z=50, pitch=-90) -""" -logger = Logger() -x_traj, y_traj, v_ref, theta_traj = get_eight_trajectory(X_INIT_M, Y_INIT_M) #“8”形状轨迹 -current_idx = 0 -laps = 0 -""" -""" -# 圆形轨迹参数:圆心(X_INIT_M, Y_INIT_M),半径20米,200个点 -x_traj, y_traj, v_ref, theta_traj = get_circle_trajectory() # 一行生成轨迹 -init_x, init_y = x_traj[0], y_traj[0] # 2行获取初始位置 -init_yaw = np.rad2deg(theta_traj[0]) # 1行获取初始角度 -carla.spawn_ego_vehicle( - 'vehicle.tesla.model3', - x=init_x, - y=init_y, - z=0.1, - yaw=init_yaw # 初始方向与轨迹一致 -) - -carla.print_ego_vehicle_characteristics() -# 调整 spectator 位置以便更好观察圆形轨迹 -carla.set_spectator(X_INIT_M, Y_INIT_M, z=80, pitch=-90) # 从圆心正上方俯视 -""" -#螺旋轨迹 -x_traj, y_traj, v_ref, theta_traj = get_spiral_trajectory( - x_init=X_INIT_M, - y_init=Y_INIT_M, - turns=2, # 螺旋圈数 - scale=2 # 螺旋缩放因子 -) # 一行生成螺旋轨迹 -init_x, init_y = x_traj[0], y_traj[0] # 获取初始位置 -init_yaw = np.rad2deg(theta_traj[0]) # 获取初始角度 -carla.spawn_ego_vehicle( - 'vehicle.tesla.model3', - x=init_x, - y=init_y, - z=0.1, - yaw=init_yaw # 初始方向与轨迹一致 -) - -# 调整 spectator 位置以便观察螺旋轨迹 -carla.set_spectator(X_INIT_M, Y_INIT_M, z=50, pitch=-90) # 降低高度,适应缩小的轨迹 logger = Logger() +x_traj, y_traj, v_ref, theta_traj = get_eight_trajectory(X_INIT_M, Y_INIT_M) # "8"形状轨迹 current_idx = 0 laps = 0 +""" +# 注释掉的轨迹绘制线程 trajectory_thread = threading.Thread(target=draw_trajectory_in_thread, args=(carla, x_traj, y_traj, dt)) trajectory_thread.daemon = True trajectory_thread.start() +""" mpc_controller = MpcController(horizon=N, dt=dt) try: @@ -79,7 +45,12 @@ def draw_trajectory_in_thread(carla, x_traj, y_traj, dt): mpc_controller.set_init_vehicle_state(x0, y0, theta0, v0) x_ref, y_ref, theta_ref = get_ref_trajectory(x_traj, y_traj, theta_traj, current_idx) - carla.draw_trajectory(x_ref, y_ref, height=1.0, red=255, life_time=dt * 2) + + # 使用感知规划绘制,只显示前方轨迹 + carla.draw_perception_planning(x_ref, y_ref, current_idx=0, look_ahead_points=N) + + # 绘制Frenet参考线(可选) + # carla.draw_frenet_frame(x_traj, y_traj, current_idx) logger.log_controller_input(x0, y0, v0, theta0, x_ref[0], y_ref[0], V_REF, theta_ref[0]) mpc_controller.update_cost_function(x_ref, y_ref, theta_ref, v_ref) @@ -111,4 +82,66 @@ def draw_trajectory_in_thread(carla, x_traj, y_traj, dt): break finally: carla.clean() - logger.show_plots() \ No newline at end of file + logger.show_plots() +""" +# 圆形轨迹参数:圆心(X_INIT_M, Y_INIT_M),半径20米,200个点 +x_traj, y_traj, v_ref, theta_traj = get_circle_trajectory() # 一行生成轨迹 +init_x, init_y = x_traj[0], y_traj[0] # 2行获取初始位置 +init_yaw = np.rad2deg(theta_traj[0]) # 1行获取初始角度 +carla.spawn_ego_vehicle( + 'vehicle.tesla.model3', + x=init_x, + y=init_y, + z=0.1, + yaw=init_yaw # 初始方向与轨迹一致 +) + +carla.print_ego_vehicle_characteristics() +# 调整 spectator 位置以便更好观察圆形轨迹 +carla.set_spectator(X_INIT_M, Y_INIT_M, z=80, pitch=-90) # 从圆心正上方俯视 +""" +""" +#螺旋轨迹 +x_traj, y_traj, v_ref, theta_traj = get_spiral_trajectory( + x_init=X_INIT_M, + y_init=Y_INIT_M, + turns=2, # 螺旋圈数 + scale=2 # 螺旋缩放因子 +) # 一行生成螺旋轨迹 + +init_x, init_y = x_traj[0], y_traj[0] # 获取初始位置 +init_yaw = np.rad2deg(theta_traj[0]) # 获取初始角度 +carla.spawn_ego_vehicle( + 'vehicle.tesla.model3', + x=init_x, + y=init_y, + z=0.1, + yaw=init_yaw # 初始方向与轨迹一致 +) + +# 调整 spectator 位置以便观察螺旋轨迹 +carla.set_spectator(X_INIT_M, Y_INIT_M, z=50, pitch=-90) # 降低高度,适应缩小的轨迹 +""" +""" +# 使用新的方形轨迹 +x_traj, y_traj, v_ref, theta_traj = get_square_trajectory( + x_init=X_INIT_M, + y_init=Y_INIT_M, + side_length=23, # 方形边长 + total_points=200 # 轨迹点数量 +) + +# 设置初始位置和方向 +init_x, init_y = x_traj[0], y_traj[0] +init_yaw = np.rad2deg(theta_traj[0]) +carla.spawn_ego_vehicle( + 'vehicle.tesla.model3', + x=init_x, + y=init_y, + z=0.1, + yaw=init_yaw # 初始方向与轨迹一致 +) + +# 调整 spectator 位置以便观察方形轨迹 +carla.set_spectator(X_INIT_M + 20, Y_INIT_M + 20, z=60, pitch=-90) # 从方形中心上方俯视 +""" \ No newline at end of file From e79b3c9d4c64d9024bf47ba9bebf7890c05eab06 Mon Sep 17 00:00:00 2001 From: Liufu27 <2183667346@qq.com> Date: Mon, 22 Dec 2025 10:39:04 +0800 Subject: [PATCH 6/8] =?UTF-8?q?=E6=B3=A8=E9=87=8A=E6=8E=89=E5=8E=9F?= =?UTF-8?q?=E6=9D=A5=E7=9A=84=E7=BA=BF=E6=80=A7=E8=BD=A8=E8=BF=B9=E5=92=8C?= =?UTF-8?q?=E7=A9=BA=E6=97=B7=E5=9C=BA=E6=99=AF=EF=BC=8C=E6=8D=A2=E4=B8=BA?= =?UTF-8?q?=E9=81=93=E8=B7=AF=E5=9C=BA=E6=99=AF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Unmanned_vehicle_control/main.py | 277 ++++++++++++++++-- .../main_mpc_controller.py | 14 +- 2 files changed, 272 insertions(+), 19 deletions(-) diff --git a/src/Unmanned_vehicle_control/main.py b/src/Unmanned_vehicle_control/main.py index c91e02ee7e..43f22cdf88 100644 --- a/src/Unmanned_vehicle_control/main.py +++ b/src/Unmanned_vehicle_control/main.py @@ -1,20 +1,17 @@ import time -import threading import numpy as np from src.main_carla_simulator import CarlaSimulator -from src.config import X_INIT_M, Y_INIT_M, N, dt, V_REF, LAPS -from src.main_help_functions import get_eight_trajectory, get_ref_trajectory, get_circle_trajectory, \ - get_spiral_trajectory, get_square_trajectory, update_reference_point +from src.config import N, dt, V_REF, LAPS +from src.main_help_functions import get_ref_trajectory, update_reference_point from src.main_logger import Logger from src.main_mpc_controller import MpcController """ -# 注释掉的轨迹绘制线程 +# 注释掉的"8"字形轨迹场景 def draw_trajectory_in_thread(carla, x_traj, y_traj, dt): while True: carla.draw_trajectory(x_traj, y_traj, height=0.2, green=255, life_time=dt * 2) time.sleep(dt) -""" carla = CarlaSimulator() carla.load_world('Town02_Opt') @@ -27,18 +24,234 @@ def draw_trajectory_in_thread(carla, x_traj, y_traj, dt): current_idx = 0 laps = 0 -""" -# 注释掉的轨迹绘制线程 trajectory_thread = threading.Thread(target=draw_trajectory_in_thread, args=(carla, x_traj, y_traj, dt)) trajectory_thread.daemon = True trajectory_thread.start() """ +# ================ 新的道路场景 - 使用CARLA Waypoint系统 ================ + +# 初始化CARLA模拟器 +carla = CarlaSimulator() + +# 加载一个有道路的地图 +carla.load_world('Town01') # 使用Town01地图,它有清晰的道路网络 + +# 获取地图的生成点 +spawn_points = carla.world.get_map().get_spawn_points() +if not spawn_points: + print("Warning: No spawn points found in the map!") + raise RuntimeError("No spawn points available") + +# 尝试多个生成点直到成功 +vehicle_spawned = False +for i, spawn_point in enumerate(spawn_points): + try: + print(f"Attempting to spawn vehicle at spawn point {i + 1}/{len(spawn_points)}...") + + carla.spawn_ego_vehicle( + 'vehicle.tesla.model3', + x=spawn_point.location.x, + y=spawn_point.location.y, + z=spawn_point.location.z + 0.1, + pitch=spawn_point.rotation.pitch, + yaw=spawn_point.rotation.yaw, + roll=spawn_point.rotation.roll + ) + + vehicle_spawned = True + print(f"Successfully spawned vehicle at spawn point {i + 1}") + break + except Exception as e: + print(f"Failed to spawn at spawn point {i + 1}: {e}") + continue + +if not vehicle_spawned: + raise RuntimeError("Failed to spawn vehicle at any spawn point") + +carla.print_ego_vehicle_characteristics() + +# 设置spectator位置 +vehicle_location = carla.ego_vehicle.get_location() +carla.set_spectator( + x=vehicle_location.x - 15, # 在车辆后方15米 + y=vehicle_location.y, + z=12, # 高度12米 + pitch=-25, # 稍微向下看 + yaw=0 # 看向车辆前方 +) + +logger = Logger() + + +# ================ 使用CARLA Waypoint生成轨迹 ================ + +def generate_road_trajectory(carla_simulator, distance_ahead=100.0, waypoint_interval=2.0): + """ + 使用CARLA的Waypoint系统生成沿着道路的轨迹 + + 参数: + carla_simulator: CarlaSimulator实例 + distance_ahead: 向前看的距离(米) + waypoint_interval: waypoint之间的间隔(米) + """ + if not carla_simulator.ego_vehicle: + raise ValueError("Vehicle not spawned yet") + + # 获取当前车辆的waypoint + vehicle_location = carla_simulator.ego_vehicle.get_location() + carla_map = carla_simulator.world.get_map() + current_waypoint = carla_map.get_waypoint(vehicle_location) + + if not current_waypoint: + raise ValueError("Cannot get waypoint at vehicle location") + + # 收集前方的waypoints + waypoints = [current_waypoint] + distance_traveled = 0.0 + + while distance_traveled < distance_ahead: + # 获取下一个waypoint + next_waypoints = current_waypoint.next(waypoint_interval) + + if not next_waypoints: + print(f"Warning: No more waypoints after {distance_traveled} meters") + break + + # 选择第一个(最直接的道路方向) + current_waypoint = next_waypoints[0] + waypoints.append(current_waypoint) + distance_traveled += waypoint_interval + + print(f"Generated {len(waypoints)} waypoints for trajectory") + + # 提取轨迹点 + x_traj = [] + y_traj = [] + v_ref = [] + theta_traj = [] + + for i, wp in enumerate(waypoints): + transform = wp.transform + location = transform.location + + x_traj.append(location.x) + y_traj.append(location.y) + v_ref.append(V_REF) # 使用参考速度 + + # 计算航向角(从yaw角度转换) + yaw_deg = transform.rotation.yaw + theta_traj.append(np.deg2rad(yaw_deg)) + + return np.array(x_traj), np.array(y_traj), v_ref, theta_traj + + +# 生成沿着道路的轨迹 +try: + x_traj, y_traj, v_ref, theta_traj = generate_road_trajectory( + carla_simulator=carla, + distance_ahead=200.0, # 生成200米长的轨迹 + waypoint_interval=2.0 # 每2米一个waypoint + ) + print(f"Trajectory generated with {len(x_traj)} points") +except Exception as e: + print(f"Failed to generate road trajectory: {e}") + # 如果生成失败,使用简单的直线轨迹作为后备 + print("Using straight trajectory as fallback...") + vehicle_location = carla.ego_vehicle.get_location() + # 生成简单的直线轨迹 + x_traj = np.array([vehicle_location.x + i * 2.0 for i in range(100)]) + y_traj = np.array([vehicle_location.y for i in range(100)]) # 直线 + v_ref = [V_REF] * 100 + theta_traj = [0.0] * 100 # 朝向正东方向 + +current_idx = 0 +laps = 0 + + +# ================ 添加碰撞检测功能 ================ + +def check_collision(carla_simulator): + """ + 检查车辆是否发生碰撞 + + 返回: + bool: 是否发生碰撞 + str: 碰撞对象类型(如果发生碰撞) + """ + collision_sensor = None + + try: + # 创建碰撞传感器 + blueprint_library = carla_simulator.world.get_blueprint_library() + collision_bp = blueprint_library.find('sensor.other.collision') + + # 将传感器附加到车辆 + collision_transform = carla.Transform(carla.Location(x=0.0, y=0.0, z=0.0)) + collision_sensor = carla_simulator.world.spawn_actor( + collision_bp, + collision_transform, + attach_to=carla_simulator.ego_vehicle + ) + + # 设置碰撞事件处理器 + collision_detected = [False] + collision_actor = [None] + + def on_collision(event): + collision_detected[0] = True + collision_actor[0] = event.other_actor + print(f"Collision detected with {event.other_actor.type_id}") + + collision_sensor.listen(on_collision) + + # 等待一小段时间检查碰撞 + time.sleep(0.1) + + # 销毁传感器 + if collision_sensor: + collision_sensor.destroy() + + return collision_detected[0], collision_actor[0].type_id if collision_actor[0] else "unknown" + + except Exception as e: + print(f"Error in collision detection: {e}") + if collision_sensor: + collision_sensor.destroy() + return False, "error" + + +# ================ MPC控制器初始化 ================ + mpc_controller = MpcController(horizon=N, dt=dt) + +# 降低速度以减少碰撞风险 +SAFE_V_REF = 3.0 # 降低到3 m/s (约10.8 km/h) + try: + consecutive_collisions = 0 + max_consecutive_collisions = 3 + while True: start_time = time.time() + # 检查碰撞 + collision_detected, collision_type = check_collision(carla) + if collision_detected: + consecutive_collisions += 1 + print(f"COLLISION WARNING #{consecutive_collisions}: Collision with {collision_type}") + + if consecutive_collisions >= max_consecutive_collisions: + print("Too many consecutive collisions. Stopping simulation.") + break + + # 碰撞后短暂停止 + carla.apply_control(0.0, 0.0, 1.0) # 刹车 + time.sleep(0.5) + continue + else: + consecutive_collisions = 0 + mpc_controller.reset_solver() x0, y0, theta0, v0 = carla.get_main_ego_vehicle_state() @@ -49,41 +262,71 @@ def draw_trajectory_in_thread(carla, x_traj, y_traj, dt): # 使用感知规划绘制,只显示前方轨迹 carla.draw_perception_planning(x_ref, y_ref, current_idx=0, look_ahead_points=N) - # 绘制Frenet参考线(可选) - # carla.draw_frenet_frame(x_traj, y_traj, current_idx) - - logger.log_controller_input(x0, y0, v0, theta0, x_ref[0], y_ref[0], V_REF, theta_ref[0]) - mpc_controller.update_cost_function(x_ref, y_ref, theta_ref, v_ref) + logger.log_controller_input(x0, y0, v0, theta0, x_ref[0], y_ref[0], SAFE_V_REF, theta_ref[0]) + mpc_controller.update_cost_function(x_ref, y_ref, theta_ref, [SAFE_V_REF] * len(x_ref)) mpc_controller.solve() wheel_angle_rad, acceleration_m_s_2 = mpc_controller.get_controls_value() - # 增加:限制控制量范围,避免极端值导致错误 + # 限制控制量范围,避免极端值导致错误 wheel_angle_rad = max(min(wheel_angle_rad, 0.5), -0.5) - acceleration_m_s_2 = max(min(acceleration_m_s_2, 3.0), -3.0) + acceleration_m_s_2 = max(min(acceleration_m_s_2, 2.0), -2.0) # 进一步限制加速度 throttle, brake, steer = CarlaSimulator.process_control_inputs(wheel_angle_rad, acceleration_m_s_2) logger.log_controller_output(steer, throttle, brake) carla.apply_control(steer, throttle, brake) prev_current_idx = current_idx - current_idx = update_reference_point(x0, y0, current_idx, x_traj, y_traj) + current_idx = update_reference_point(x0, y0, current_idx, x_traj, y_traj, min_distance=3.0) # 减小最小距离 + + # 如果到达轨迹终点,重新生成轨迹 + if current_idx >= len(x_traj) - N: + print("Reached end of trajectory. Regenerating...") + try: + x_traj, y_traj, v_ref, theta_traj = generate_road_trajectory( + carla_simulator=carla, + distance_ahead=200.0, + waypoint_interval=2.0 + ) + current_idx = 0 + print(f"New trajectory generated with {len(x_traj)} points") + except Exception as e: + print(f"Failed to regenerate trajectory: {e}") + break + if prev_current_idx == len(x_traj) - 1 and current_idx == 0: laps += 1 end_time = time.time() mpc_calculation_time = end_time - start_time - print(f"Calculation time of MPC controller: {mpc_calculation_time:.6f} seconds") + + # 显示更多调试信息 + print(f"MPC calculation: {mpc_calculation_time:.3f}s | " + f"Position: ({x0:.1f}, {y0:.1f}) | " + f"Speed: {v0:.1f}m/s | " + f"Controls: steer={steer:.2f}, throttle={throttle:.2f}, brake={brake:.2f}") time.sleep(max(dt - mpc_calculation_time, 0)) + # 退出条件 if laps == LAPS: + print(f"Completed {laps} lap(s). Stopping simulation.") break + + # 安全停止条件:如果车辆偏离轨迹太远 + if len(x_traj) > current_idx: + distance_to_ref = np.sqrt((x0 - x_traj[current_idx]) ** 2 + (y0 - y_traj[current_idx]) ** 2) + if distance_to_ref > 10.0: # 如果偏离超过10米 + print(f"Vehicle deviated too far from reference ({distance_to_ref:.1f}m). Stopping.") + break + finally: carla.clean() logger.show_plots() + """ +# 注释掉的其他轨迹选项 # 圆形轨迹参数:圆心(X_INIT_M, Y_INIT_M),半径20米,200个点 x_traj, y_traj, v_ref, theta_traj = get_circle_trajectory() # 一行生成轨迹 init_x, init_y = x_traj[0], y_traj[0] # 2行获取初始位置 diff --git a/src/Unmanned_vehicle_control/main_mpc_controller.py b/src/Unmanned_vehicle_control/main_mpc_controller.py index 341513f8e2..24f4a5d1cd 100644 --- a/src/Unmanned_vehicle_control/main_mpc_controller.py +++ b/src/Unmanned_vehicle_control/main_mpc_controller.py @@ -1,4 +1,5 @@ import casadi as ca +import numpy as np 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, \ @@ -105,10 +106,19 @@ 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]) + # 控制量饱和(与config约束一致) + wheel_angle_rad = np.clip(wheel_angle_rad, + -MAX_CONTROL_WHEEL_ANGLE_RAD, + MAX_CONTROL_WHEEL_ANGLE_RAD) + acceleration_m_s_2 = np.clip(acceleration_m_s_2, + MAX_CONTROL_BRAKING_M_S_2, + MAX_CONTROL_ACCELERATION_M_S_2) 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] + # 求解失败时,控制量渐进衰减(减少突变) + decay = 0.9 # 衰减系数 + acceleration_m_s_2 = self.control_buffer["acceleration"][self.buffer_index] * decay + wheel_angle_rad = self.control_buffer["wheel_angle"][self.buffer_index] * decay self.buffer_index += 1 else: raise Exception("Control buffer is empty.") From 6d84eae1fdc85528831a354e6d6aa3576723b814 Mon Sep 17 00:00:00 2001 From: Liufu27 <2183667346@qq.com> Date: Mon, 22 Dec 2025 19:07:33 +0800 Subject: [PATCH 7/8] =?UTF-8?q?=E5=9C=A8=E9=81=93=E8=B7=AF=E4=B8=8A?= =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E9=9A=9C=E7=A2=8D=E7=89=A9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Unmanned_vehicle_control/main.py | 122 +++++++++++++++++++++++++++ 1 file changed, 122 insertions(+) diff --git a/src/Unmanned_vehicle_control/main.py b/src/Unmanned_vehicle_control/main.py index 43f22cdf88..94497d4b9a 100644 --- a/src/Unmanned_vehicle_control/main.py +++ b/src/Unmanned_vehicle_control/main.py @@ -84,6 +84,118 @@ def draw_trajectory_in_thread(carla, x_traj, y_traj, dt): logger = Logger() +# ================ 添加障碍物生成功能 ================ + +def spawn_obstacles(carla_simulator, num_obstacles=3): + """ + 在道路上生成障碍物车辆 + + 参数: + carla_simulator: CarlaSimulator实例 + num_obstacles: 障碍物数量 + """ + obstacles = [] + blueprint_library = carla_simulator.world.get_blueprint_library() + + # 获取地图的所有生成点 + all_spawn_points = carla_simulator.world.get_map().get_spawn_points() + + if not all_spawn_points: + print("Warning: No spawn points found for obstacles!") + return obstacles + + # 获取主车辆的位置 + ego_location = carla_simulator.ego_vehicle.get_location() + + # 筛选出距离主车辆一定距离的生成点 + valid_spawn_points = [] + for spawn_point in all_spawn_points: + distance = np.sqrt( + (spawn_point.location.x - ego_location.x) ** 2 + + (spawn_point.location.y - ego_location.y) ** 2 + ) + # 选择距离主车辆20-50米的生成点 + if 20 < distance < 50: + valid_spawn_points.append(spawn_point) + + # 如果没有合适的生成点,放宽条件 + if not valid_spawn_points: + print("No suitable spawn points found, using all spawn points...") + valid_spawn_points = all_spawn_points + + # 随机选择生成点 + import random + random.shuffle(valid_spawn_points) + + # 生成障碍物 + for i in range(min(num_obstacles, len(valid_spawn_points))): + try: + # 随机选择障碍物类型 + obstacle_types = [ + 'vehicle.audi.tt', + 'vehicle.mercedes.coupe', + 'vehicle.nissan.micra', + 'vehicle.dodge.charger_police', + 'vehicle.bmw.grandtourer', + 'static.prop.box' # 静态箱子 + ] + + obstacle_type = random.choice(obstacle_types) + + if obstacle_type.startswith('vehicle.'): + # 生成车辆障碍物 + obstacle_bp = blueprint_library.filter(obstacle_type)[0] + spawn_point = valid_spawn_points[i] + + obstacle = carla_simulator.world.spawn_actor( + obstacle_bp, + spawn_point + ) + + # 设置车辆为静止状态 + obstacle.apply_control(carla.VehicleControl( + throttle=0.0, + brake=1.0, + steer=0.0, + hand_brake=True + )) + + # 设置车辆颜色 + if obstacle_type != 'static.prop.box': + obstacle.set_autopilot(False) + + print( + f"Spawned obstacle {i + 1}: {obstacle_type} at ({spawn_point.location.x:.1f}, {spawn_point.location.y:.1f})") + obstacles.append(obstacle) + + elif obstacle_type == 'static.prop.box': + # 生成静态箱子障碍物 + box_bp = blueprint_library.find('static.prop.box') + spawn_point = valid_spawn_points[i] + + # 稍微抬高箱子避免陷入地面 + box_location = carla.Location( + x=spawn_point.location.x, + y=spawn_point.location.y, + z=spawn_point.location.z + 0.5 + ) + + obstacle = carla_simulator.world.spawn_actor( + box_bp, + carla.Transform(box_location, spawn_point.rotation) + ) + + print(f"Spawned static box obstacle {i + 1} at ({box_location.x:.1f}, {box_location.y:.1f})") + obstacles.append(obstacle) + + except Exception as e: + print(f"Failed to spawn obstacle {i + 1}: {e}") + continue + + print(f"Successfully spawned {len(obstacles)} obstacles") + return obstacles + + # ================ 使用CARLA Waypoint生成轨迹 ================ def generate_road_trajectory(carla_simulator, distance_ahead=100.0, waypoint_interval=2.0): @@ -168,6 +280,9 @@ def generate_road_trajectory(carla_simulator, distance_ahead=100.0, waypoint_int current_idx = 0 laps = 0 +# 生成障碍物 +obstacles = spawn_obstacles(carla, num_obstacles=5) + # ================ 添加碰撞检测功能 ================ @@ -322,6 +437,13 @@ def on_collision(event): break finally: + # 清理障碍物 + for obstacle in obstacles: + try: + obstacle.destroy() + except: + pass + carla.clean() logger.show_plots() From 3e1e9210f9e359becaa6264b8e75c9d185ac7d63 Mon Sep 17 00:00:00 2001 From: Liufu27 <2183667346@qq.com> Date: Mon, 22 Dec 2025 20:39:47 +0800 Subject: [PATCH 8/8] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E4=B8=80=E5=AE=9A?= =?UTF-8?q?=E7=9A=84=E9=81=BF=E9=9A=9C=E5=8A=9F=E8=83=BD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Unmanned_vehicle_control/config.py | 66 ++- src/Unmanned_vehicle_control/main.py | 455 +++++++-------- .../main_carla_simulator.py | 543 +++++++++++++----- .../main_help_functions.py | 47 +- .../main_mpc_controller.py | 140 ++++- 5 files changed, 837 insertions(+), 414 deletions(-) diff --git a/src/Unmanned_vehicle_control/config.py b/src/Unmanned_vehicle_control/config.py index ede562973f..2c76148b60 100644 --- a/src/Unmanned_vehicle_control/config.py +++ b/src/Unmanned_vehicle_control/config.py @@ -1,5 +1,68 @@ import numpy as np +X_INIT_M = 210 +Y_INIT_M = 362 +A = 25 +LAPS = 1 + +MAX_ACCELERATION_M_S_2 = 10 +MAX_BRAKING_M_S_2 = -4.1 +MAX_WHEEL_ANGLE_RAD = 70 / 180 * np.pi +L = 2.875 + +### MPC settings +N = 5 +dt = 0.1 +MAX_CONTROL_WHEEL_ANGLE_RAD = 70 / 180 * np.pi +MAX_CONTROL_ACCELERATION_M_S_2 = 10 +MAX_CONTROL_BRAKING_M_S_2 = -4.1 +PATH_TOLERANCE_M = 0.5 +V_REF = 5 + +""" +原始代码被注释掉: +FINE_X_COEF = 10 +FINE_Y_COEF = 10 +FINE_STEER_COEF = 0 +FINE_ACC_COEF = 0 +FINE_STEER_DOT_COEF = 100 +FINE_ACC_DOT_COEF = 1 +FINE_V_COEF = 20 +FINE_THETA_COEF = 0 +FINE_LATERAL_COEF = 40 +""" + +FINE_X_COEF = 10 +FINE_Y_COEF = 10 +FINE_STEER_COEF = 0 +FINE_ACC_COEF = 0 +FINE_STEER_DOT_COEF = 100 +FINE_ACC_DOT_COEF = 1 +FINE_V_COEF = 20 +FINE_THETA_COEF = 0 +FINE_LATERAL_COEF = 40 + +# 避障相关参数 +OBSTACLE_PENALTY_WEIGHT = 100.0 +OBSTACLE_SAFETY_DISTANCE = 3.0 +OBSTACLE_AVOIDANCE_ENABLED = True + +# 圆形轨迹参数 +CIRCLE_CENTER_X = X_INIT_M +CIRCLE_CENTER_Y = Y_INIT_M +CIRCLE_RADIUS = 50 + +VISUALIZATION_CONFIG = { + 'show_full_trajectory': False, + 'show_perception_planning': True, + 'show_frenet_frame': False, + 'look_ahead_points': N, + 'target_point_color': (255, 0, 0), + 'trajectory_color': (0, 255, 0), + 'obstacle_color': (255, 0, 0), + 'safety_zone_color': (255, 165, 0), +} +""" X_INIT_M = 210 # Start X coordinate for vehicle Y_INIT_M = 362 # Start Y coordinate for vehicle A = 25 # Size of 8-trajectory @@ -41,4 +104,5 @@ 'look_ahead_points': N, # 向前看的点数 'target_point_color': (255, 0, 0), # 目标点颜色 (R, G, B) 'trajectory_color': (0, 255, 0), # 轨迹线颜色 -} \ No newline at end of file +} +""" \ No newline at end of file diff --git a/src/Unmanned_vehicle_control/main.py b/src/Unmanned_vehicle_control/main.py index 94497d4b9a..1930699536 100644 --- a/src/Unmanned_vehicle_control/main.py +++ b/src/Unmanned_vehicle_control/main.py @@ -6,36 +6,8 @@ from src.main_logger import Logger from src.main_mpc_controller import MpcController -""" -# 注释掉的"8"字形轨迹场景 -def draw_trajectory_in_thread(carla, x_traj, y_traj, dt): - while True: - carla.draw_trajectory(x_traj, y_traj, height=0.2, green=255, life_time=dt * 2) - time.sleep(dt) - carla = CarlaSimulator() -carla.load_world('Town02_Opt') -carla.spawn_ego_vehicle('vehicle.tesla.model3', x=X_INIT_M, y=Y_INIT_M, z=0.1) # "8"字形 -carla.print_ego_vehicle_characteristics() -carla.set_spectator(X_INIT_M, Y_INIT_M, z=50, pitch=-90) - -logger = Logger() -x_traj, y_traj, v_ref, theta_traj = get_eight_trajectory(X_INIT_M, Y_INIT_M) # "8"形状轨迹 -current_idx = 0 -laps = 0 - -trajectory_thread = threading.Thread(target=draw_trajectory_in_thread, args=(carla, x_traj, y_traj, dt)) -trajectory_thread.daemon = True -trajectory_thread.start() -""" - -# ================ 新的道路场景 - 使用CARLA Waypoint系统 ================ - -# 初始化CARLA模拟器 -carla = CarlaSimulator() - -# 加载一个有道路的地图 -carla.load_world('Town01') # 使用Town01地图,它有清晰的道路网络 +carla.load_world('Town01') # 获取地图的生成点 spawn_points = carla.world.get_map().get_spawn_points() @@ -43,7 +15,7 @@ def draw_trajectory_in_thread(carla, x_traj, y_traj, dt): print("Warning: No spawn points found in the map!") raise RuntimeError("No spawn points available") -# 尝试多个生成点直到成功 +# 生成主车辆 vehicle_spawned = False for i, spawn_point in enumerate(spawn_points): try: @@ -71,146 +43,50 @@ def draw_trajectory_in_thread(carla, x_traj, y_traj, dt): carla.print_ego_vehicle_characteristics() -# 设置spectator位置 -vehicle_location = carla.ego_vehicle.get_location() -carla.set_spectator( - x=vehicle_location.x - 15, # 在车辆后方15米 - y=vehicle_location.y, - z=12, # 高度12米 - pitch=-25, # 稍微向下看 - yaw=0 # 看向车辆前方 -) - -logger = Logger() - - -# ================ 添加障碍物生成功能 ================ - -def spawn_obstacles(carla_simulator, num_obstacles=3): - """ - 在道路上生成障碍物车辆 - - 参数: - carla_simulator: CarlaSimulator实例 - num_obstacles: 障碍物数量 - """ - obstacles = [] - blueprint_library = carla_simulator.world.get_blueprint_library() - - # 获取地图的所有生成点 - all_spawn_points = carla_simulator.world.get_map().get_spawn_points() - - if not all_spawn_points: - print("Warning: No spawn points found for obstacles!") - return obstacles +# ================ 生成间距合适的障碍物 ================ +print("\n" + "=" * 50) +print("Generating spaced obstacles...") +print("=" * 50) - # 获取主车辆的位置 - ego_location = carla_simulator.ego_vehicle.get_location() - - # 筛选出距离主车辆一定距离的生成点 - valid_spawn_points = [] - for spawn_point in all_spawn_points: - distance = np.sqrt( - (spawn_point.location.x - ego_location.x) ** 2 + - (spawn_point.location.y - ego_location.y) ** 2 - ) - # 选择距离主车辆20-50米的生成点 - if 20 < distance < 50: - valid_spawn_points.append(spawn_point) - - # 如果没有合适的生成点,放宽条件 - if not valid_spawn_points: - print("No suitable spawn points found, using all spawn points...") - valid_spawn_points = all_spawn_points - - # 随机选择生成点 - import random - random.shuffle(valid_spawn_points) - - # 生成障碍物 - for i in range(min(num_obstacles, len(valid_spawn_points))): - try: - # 随机选择障碍物类型 - obstacle_types = [ - 'vehicle.audi.tt', - 'vehicle.mercedes.coupe', - 'vehicle.nissan.micra', - 'vehicle.dodge.charger_police', - 'vehicle.bmw.grandtourer', - 'static.prop.box' # 静态箱子 - ] - - obstacle_type = random.choice(obstacle_types) - - if obstacle_type.startswith('vehicle.'): - # 生成车辆障碍物 - obstacle_bp = blueprint_library.filter(obstacle_type)[0] - spawn_point = valid_spawn_points[i] - - obstacle = carla_simulator.world.spawn_actor( - obstacle_bp, - spawn_point - ) +# 生成间距合适的障碍物 +blocking_obstacles = carla.spawn_spaced_obstacles() - # 设置车辆为静止状态 - obstacle.apply_control(carla.VehicleControl( - throttle=0.0, - brake=1.0, - steer=0.0, - hand_brake=True - )) - - # 设置车辆颜色 - if obstacle_type != 'static.prop.box': - obstacle.set_autopilot(False) - - print( - f"Spawned obstacle {i + 1}: {obstacle_type} at ({spawn_point.location.x:.1f}, {spawn_point.location.y:.1f})") - obstacles.append(obstacle) - - elif obstacle_type == 'static.prop.box': - # 生成静态箱子障碍物 - box_bp = blueprint_library.find('static.prop.box') - spawn_point = valid_spawn_points[i] - - # 稍微抬高箱子避免陷入地面 - box_location = carla.Location( - x=spawn_point.location.x, - y=spawn_point.location.y, - z=spawn_point.location.z + 0.5 - ) - - obstacle = carla_simulator.world.spawn_actor( - box_bp, - carla.Transform(box_location, spawn_point.rotation) - ) - - print(f"Spawned static box obstacle {i + 1} at ({box_location.x:.1f}, {box_location.y:.1f})") - obstacles.append(obstacle) +# 设置spectator位置 - 调整视角以更好观察障碍物 +if carla.ego_vehicle: + vehicle_location = carla.ego_vehicle.get_location() + vehicle_transform = carla.ego_vehicle.get_transform() + + # 设置spectator在车辆后上方,以便观察前方障碍物 + camera_distance = 25.0 # 相机距离 + camera_height = 15.0 # 相机高度 + camera_pitch = -25.0 # 相机俯仰角 + + # 计算相机位置(在车辆后方) + yaw_rad = np.radians(vehicle_transform.rotation.yaw) + camera_x = vehicle_location.x - camera_distance * np.cos(yaw_rad) + camera_y = vehicle_location.y - camera_distance * np.sin(yaw_rad) + camera_z = vehicle_location.z + camera_height + + carla.set_spectator( + x=camera_x, + y=camera_y, + z=camera_z, + pitch=camera_pitch, + yaw=vehicle_transform.rotation.yaw, + roll=0 + ) - except Exception as e: - print(f"Failed to spawn obstacle {i + 1}: {e}") - continue + print(f"Spectator set to: ({camera_x:.1f}, {camera_y:.1f}, {camera_z:.1f})") - print(f"Successfully spawned {len(obstacles)} obstacles") - return obstacles +logger = Logger() # ================ 使用CARLA Waypoint生成轨迹 ================ - def generate_road_trajectory(carla_simulator, distance_ahead=100.0, waypoint_interval=2.0): - """ - 使用CARLA的Waypoint系统生成沿着道路的轨迹 - - 参数: - carla_simulator: CarlaSimulator实例 - distance_ahead: 向前看的距离(米) - waypoint_interval: waypoint之间的间隔(米) - """ + """使用CARLA的Waypoint系统生成沿着道路的轨迹""" if not carla_simulator.ego_vehicle: raise ValueError("Vehicle not spawned yet") - # 获取当前车辆的waypoint vehicle_location = carla_simulator.ego_vehicle.get_location() carla_map = carla_simulator.world.get_map() current_waypoint = carla_map.get_waypoint(vehicle_location) @@ -223,7 +99,6 @@ def generate_road_trajectory(carla_simulator, distance_ahead=100.0, waypoint_int distance_traveled = 0.0 while distance_traveled < distance_ahead: - # 获取下一个waypoint next_waypoints = current_waypoint.next(waypoint_interval) if not next_waypoints: @@ -249,9 +124,8 @@ def generate_road_trajectory(carla_simulator, distance_ahead=100.0, waypoint_int x_traj.append(location.x) y_traj.append(location.y) - v_ref.append(V_REF) # 使用参考速度 + v_ref.append(V_REF) - # 计算航向角(从yaw角度转换) yaw_deg = transform.rotation.yaw theta_traj.append(np.deg2rad(yaw_deg)) @@ -259,49 +133,56 @@ def generate_road_trajectory(carla_simulator, distance_ahead=100.0, waypoint_int # 生成沿着道路的轨迹 +print("\n" + "=" * 50) +print("Generating road trajectory...") +print("=" * 50) + try: x_traj, y_traj, v_ref, theta_traj = generate_road_trajectory( carla_simulator=carla, - distance_ahead=200.0, # 生成200米长的轨迹 - waypoint_interval=2.0 # 每2米一个waypoint + distance_ahead=100.0, # 减小距离,以便更集中观察避障行为 + waypoint_interval=2.0 ) print(f"Trajectory generated with {len(x_traj)} points") + + # 打印轨迹点信息 + print(f"First 5 trajectory points:") + for i in range(min(5, len(x_traj))): + print(f" Point {i}: ({x_traj[i]:.1f}, {y_traj[i]:.1f}), theta={np.degrees(theta_traj[i]):.1f}°") + except Exception as e: print(f"Failed to generate road trajectory: {e}") # 如果生成失败,使用简单的直线轨迹作为后备 print("Using straight trajectory as fallback...") - vehicle_location = carla.ego_vehicle.get_location() - # 生成简单的直线轨迹 - x_traj = np.array([vehicle_location.x + i * 2.0 for i in range(100)]) - y_traj = np.array([vehicle_location.y for i in range(100)]) # 直线 + if carla.ego_vehicle: + vehicle_location = carla.ego_vehicle.get_location() + vehicle_transform = carla.ego_vehicle.get_transform() + yaw_rad = np.radians(vehicle_transform.rotation.yaw) + + # 生成直线轨迹 + x_traj = np.array([vehicle_location.x + i * 2.0 * np.cos(yaw_rad) for i in range(100)]) + y_traj = np.array([vehicle_location.y + i * 2.0 * np.sin(yaw_rad) for i in range(100)]) + else: + x_traj = np.array([i * 2.0 for i in range(100)]) + y_traj = np.array([0.0 for i in range(100)]) v_ref = [V_REF] * 100 - theta_traj = [0.0] * 100 # 朝向正东方向 + theta_traj = [yaw_rad] * 100 current_idx = 0 laps = 0 -# 生成障碍物 -obstacles = spawn_obstacles(carla, num_obstacles=5) - - -# ================ 添加碰撞检测功能 ================ +# ================ 碰撞检测功能 ================ def check_collision(carla_simulator): - """ - 检查车辆是否发生碰撞 + """检查车辆是否发生碰撞""" + if not carla_simulator.ego_vehicle: + return False, "no_vehicle" - 返回: - bool: 是否发生碰撞 - str: 碰撞对象类型(如果发生碰撞) - """ collision_sensor = None try: - # 创建碰撞传感器 blueprint_library = carla_simulator.world.get_blueprint_library() collision_bp = blueprint_library.find('sensor.other.collision') - - # 将传感器附加到车辆 collision_transform = carla.Transform(carla.Location(x=0.0, y=0.0, z=0.0)) collision_sensor = carla_simulator.world.spawn_actor( collision_bp, @@ -309,7 +190,6 @@ def check_collision(carla_simulator): attach_to=carla_simulator.ego_vehicle ) - # 设置碰撞事件处理器 collision_detected = [False] collision_actor = [None] @@ -319,11 +199,8 @@ def on_collision(event): print(f"Collision detected with {event.other_actor.type_id}") collision_sensor.listen(on_collision) + time.sleep(0.05) # 减小等待时间 - # 等待一小段时间检查碰撞 - time.sleep(0.1) - - # 销毁传感器 if collision_sensor: collision_sensor.destroy() @@ -337,63 +214,139 @@ def on_collision(event): # ================ MPC控制器初始化 ================ +print("\n" + "=" * 50) +print("Initializing MPC controller...") +print("=" * 50) mpc_controller = MpcController(horizon=N, dt=dt) -# 降低速度以减少碰撞风险 -SAFE_V_REF = 3.0 # 降低到3 m/s (约10.8 km/h) +# 启用避障功能 +mpc_controller.enable_obstacle_avoidance(True) + +# 降低速度以减少碰撞风险,同时确保能够有效避障 +SAFE_V_REF = 3.0 # 稍微提高速度,因为障碍物间距增大了 + +print(f"MPC Horizon: {N}") +print(f"Control dt: {dt}") +print(f"Safe speed: {SAFE_V_REF} m/s") +print(f"Obstacle avoidance: Enabled") try: consecutive_collisions = 0 max_consecutive_collisions = 3 + simulation_steps = 0 + max_simulation_steps = 500 # 限制模拟步数 - while True: + print("\n" + "=" * 50) + print("Starting simulation with obstacle avoidance...") + print("=" * 50) + + while simulation_steps < max_simulation_steps: + simulation_steps += 1 start_time = time.time() # 检查碰撞 - collision_detected, collision_type = check_collision(carla) - if collision_detected: - consecutive_collisions += 1 - print(f"COLLISION WARNING #{consecutive_collisions}: Collision with {collision_type}") + try: + collision_detected, collision_type = check_collision(carla) + if collision_detected: + consecutive_collisions += 1 + print(f"COLLISION WARNING #{consecutive_collisions}: Collision with {collision_type}") + + if consecutive_collisions >= max_consecutive_collisions: + print("Too many consecutive collisions. Stopping simulation.") + break + + # 碰撞后短暂停止 + carla.apply_control(0.0, 0.0, 1.0) + time.sleep(0.5) + continue + else: + consecutive_collisions = 0 + except Exception as e: + print(f"Error checking collision: {e}") - if consecutive_collisions >= max_consecutive_collisions: - print("Too many consecutive collisions. Stopping simulation.") - break + # 获取障碍物信息并传递给MPC控制器 + try: + obstacles_info = carla.get_obstacle_positions() + mpc_controller.set_obstacles(obstacles_info) + + # 打印障碍物信息 + if obstacles_info and simulation_steps % 30 == 0: + print(f"\nCurrent obstacles ({len(obstacles_info)} total):") + for i, obs in enumerate(obstacles_info): + obs_type = obs['type'].split('.')[-1] + distance_to_vehicle = np.sqrt((obs['x'] - x0) ** 2 + (obs['y'] - y0) ** 2) + print(f" Obstacle {i + 1}: {obs_type} at ({obs['x']:.1f}, {obs['y']:.1f}), " + f"distance: {distance_to_vehicle:.1f}m, safe_dist: {obs.get('safe_distance', 3.0):.1f}m") - # 碰撞后短暂停止 - carla.apply_control(0.0, 0.0, 1.0) # 刹车 - time.sleep(0.5) - continue - else: - consecutive_collisions = 0 + except Exception as e: + print(f"Error getting obstacle info: {e}") + obstacles_info = [] + # 重新初始化求解器 mpc_controller.reset_solver() + # 获取车辆状态 x0, y0, theta0, v0 = carla.get_main_ego_vehicle_state() + + # 打印车辆状态 + if simulation_steps % 15 == 0: + print(f"\nVehicle state: Position=({x0:.1f}, {y0:.1f}), " + f"Theta={np.degrees(theta0):.1f}°, Speed={v0:.1f}m/s") + mpc_controller.set_init_vehicle_state(x0, y0, theta0, v0) - x_ref, y_ref, theta_ref = get_ref_trajectory(x_traj, y_traj, theta_traj, current_idx) + # 获取参考轨迹 + try: + x_ref, y_ref, theta_ref = get_ref_trajectory(x_traj, y_traj, theta_traj, current_idx) + except Exception as e: + print(f"Error getting reference trajectory: {e}") + break - # 使用感知规划绘制,只显示前方轨迹 - carla.draw_perception_planning(x_ref, y_ref, current_idx=0, look_ahead_points=N) + # 使用感知规划绘制,包括障碍物 + try: + carla.draw_perception_planning(x_ref, y_ref, current_idx=0, + look_ahead_points=N, + obstacles_info=obstacles_info) + except Exception as e: + print(f"Error drawing perception planning: {e}") + # 记录日志 logger.log_controller_input(x0, y0, v0, theta0, x_ref[0], y_ref[0], SAFE_V_REF, theta_ref[0]) - mpc_controller.update_cost_function(x_ref, y_ref, theta_ref, [SAFE_V_REF] * len(x_ref)) - mpc_controller.solve() + try: + # 更新代价函数 + mpc_controller.update_cost_function(x_ref, y_ref, theta_ref, [SAFE_V_REF] * len(x_ref)) + + # 求解MPC + mpc_controller.solve() + + # 获取控制量 + wheel_angle_rad, acceleration_m_s_2 = mpc_controller.get_controls_value() - wheel_angle_rad, acceleration_m_s_2 = mpc_controller.get_controls_value() + # 限制控制量范围 + wheel_angle_rad = max(min(wheel_angle_rad, 0.8), -0.8) # 增大转向限制,确保能避开障碍物 + acceleration_m_s_2 = max(min(acceleration_m_s_2, 3.0), -3.0) # 增大加速度限制 - # 限制控制量范围,避免极端值导致错误 - wheel_angle_rad = max(min(wheel_angle_rad, 0.5), -0.5) - acceleration_m_s_2 = max(min(acceleration_m_s_2, 2.0), -2.0) # 进一步限制加速度 + # 处理控制输入 + throttle, brake, steer = CarlaSimulator.process_control_inputs(wheel_angle_rad, acceleration_m_s_2) - throttle, brake, steer = CarlaSimulator.process_control_inputs(wheel_angle_rad, acceleration_m_s_2) - logger.log_controller_output(steer, throttle, brake) - carla.apply_control(steer, throttle, brake) + # 记录并应用控制 + logger.log_controller_output(steer, throttle, brake) + carla.apply_control(steer, throttle, brake) - prev_current_idx = current_idx - current_idx = update_reference_point(x0, y0, current_idx, x_traj, y_traj, min_distance=3.0) # 减小最小距离 + except Exception as e: + print(f"Error in MPC control: {e}") + # 发生错误时应用零控制 + carla.apply_control(0.0, 0.0, 0.5) # 轻微刹车 + + # 更新参考点 + try: + prev_current_idx = current_idx + current_idx = update_reference_point(x0, y0, current_idx, x_traj, y_traj, min_distance=2.0) + except Exception as e: + print(f"Error updating reference point: {e}") + current_idx = (current_idx + 1) % len(x_traj) # 如果到达轨迹终点,重新生成轨迹 if current_idx >= len(x_traj) - N: @@ -401,7 +354,7 @@ def on_collision(event): try: x_traj, y_traj, v_ref, theta_traj = generate_road_trajectory( carla_simulator=carla, - distance_ahead=200.0, + distance_ahead=100.0, waypoint_interval=2.0 ) current_idx = 0 @@ -416,11 +369,14 @@ def on_collision(event): end_time = time.time() mpc_calculation_time = end_time - start_time - # 显示更多调试信息 - print(f"MPC calculation: {mpc_calculation_time:.3f}s | " - f"Position: ({x0:.1f}, {y0:.1f}) | " - f"Speed: {v0:.1f}m/s | " - f"Controls: steer={steer:.2f}, throttle={throttle:.2f}, brake={brake:.2f}") + # 显示调试信息 + if simulation_steps % 8 == 0: + print(f"Step {simulation_steps}: MPC={mpc_calculation_time:.3f}s | " + f"Position=({x0:.1f}, {y0:.1f}) | " + f"Speed={v0:.1f}m/s | " + f"Controls: steer={steer:.2f}, throttle={throttle:.2f}, brake={brake:.2f} | " + f"Obstacles={len(obstacles_info)} | " + f"Ref idx={current_idx}/{len(x_traj)}") time.sleep(max(dt - mpc_calculation_time, 0)) @@ -429,24 +385,59 @@ def on_collision(event): print(f"Completed {laps} lap(s). Stopping simulation.") break - # 安全停止条件:如果车辆偏离轨迹太远 + # 安全停止条件 if len(x_traj) > current_idx: distance_to_ref = np.sqrt((x0 - x_traj[current_idx]) ** 2 + (y0 - y_traj[current_idx]) ** 2) - if distance_to_ref > 10.0: # 如果偏离超过10米 + if distance_to_ref > 30.0: # 增大偏离阈值,因为车辆可能需要绕行 print(f"Vehicle deviated too far from reference ({distance_to_ref:.1f}m). Stopping.") break +except KeyboardInterrupt: + print("\nSimulation interrupted by user") +except Exception as e: + print(f"\nUnexpected error: {e}") + import traceback + + traceback.print_exc() finally: - # 清理障碍物 - for obstacle in obstacles: - try: - obstacle.destroy() - except: - pass + print("\n" + "=" * 50) + print("Simulation completed") + print("=" * 50) + + # 显示最终统计信息 + if carla.ego_vehicle: + final_x, final_y, final_theta, final_v = carla.get_main_ego_vehicle_state() + print(f"Final position: ({final_x:.1f}, {final_y:.1f})") + print(f"Final speed: {final_v:.1f} m/s") + print(f"Total steps: {simulation_steps}") + print(f"Collisions: {consecutive_collisions}") carla.clean() logger.show_plots() +""" +# 注释掉的"8"字形轨迹场景 +def draw_trajectory_in_thread(carla, x_traj, y_traj, dt): + while True: + carla.draw_trajectory(x_traj, y_traj, height=0.2, green=255, life_time=dt * 2) + time.sleep(dt) + +carla = CarlaSimulator() +carla.load_world('Town02_Opt') +carla.spawn_ego_vehicle('vehicle.tesla.model3', x=X_INIT_M, y=Y_INIT_M, z=0.1) # "8"字形 +carla.print_ego_vehicle_characteristics() +carla.set_spectator(X_INIT_M, Y_INIT_M, z=50, pitch=-90) + +logger = Logger() +x_traj, y_traj, v_ref, theta_traj = get_eight_trajectory(X_INIT_M, Y_INIT_M) # "8"形状轨迹 +current_idx = 0 +laps = 0 + +trajectory_thread = threading.Thread(target=draw_trajectory_in_thread, args=(carla, x_traj, y_traj, dt)) +trajectory_thread.daemon = True +trajectory_thread.start() +""" +#后面 """ # 注释掉的其他轨迹选项 # 圆形轨迹参数:圆心(X_INIT_M, Y_INIT_M),半径20米,200个点 diff --git a/src/Unmanned_vehicle_control/main_carla_simulator.py b/src/Unmanned_vehicle_control/main_carla_simulator.py index e9f0a77f97..428bf9aa74 100644 --- a/src/Unmanned_vehicle_control/main_carla_simulator.py +++ b/src/Unmanned_vehicle_control/main_carla_simulator.py @@ -1,113 +1,399 @@ -import carla # 导入Carla客户端库,用于与Carla模拟器交互 -import numpy as np # 导入numpy库,用于数值计算 - -# 从配置文件导入车辆控制相关的常量 +import carla +import numpy as np from src.config import MAX_BRAKING_M_S_2, MAX_WHEEL_ANGLE_RAD, MAX_ACCELERATION_M_S_2 class CarlaSimulator: - """Carla模拟器交互类,封装了与Carla模拟器的主要交互操作""" - def __init__(self): - """初始化Carla模拟器连接及相关参数""" - # 连接到本地Carla服务器(默认地址localhost:2000) self.client = carla.Client('localhost', 2000) - # 设置客户端超时时间(10秒) self.client.set_timeout(10.0) - # 获取当前世界对象 self.world = self.client.get_world() - # 初始化主车辆(ego vehicle)为None self.ego_vehicle = None - # 销毁场景中所有现有车辆(初始化环境) - for vehicle in self.world.get_actors().filter('*vehicle*'): - vehicle.destroy() + self.obstacles = [] # 存储障碍物对象 + self.destroy_all_actors() + + # 障碍物颜色 + self.obstacle_colors = { + 'vehicle.audi.tt': (255, 0, 0, 200), # 红色半透明 + 'vehicle.tesla.model3': (255, 165, 0, 200), # 橙色半透明 + 'static.prop.trafficcone': (255, 255, 0, 200), # 黄色半透明 + 'static.prop.streetbarrier': (128, 0, 128, 200), # 紫色半透明 + } + + def destroy_all_actors(self): + """销毁所有现有actor(车辆、障碍物等)""" + for actor in self.world.get_actors(): + if actor.type_id.startswith('vehicle.') or actor.type_id.startswith('static.prop.'): + actor.destroy() + self.obstacles = [] def load_world(self, map_name): - """ - 加载指定名称的Carla地图 - - 参数: - map_name: 地图名称(如'Town01') - """ self.client.load_world(map_name) def spawn_ego_vehicle(self, vehicle_name, x=0, y=0, z=0, pitch=0, yaw=0, roll=0): - """ - 在指定位置生成主车辆(ego vehicle) - - 参数: - vehicle_name: 车辆蓝图名称(用于筛选车辆模型) - x, y, z: 生成位置的坐标(米) - pitch, yaw, roll: 生成时的旋转角度(度),分别对应俯仰角、偏航角、横滚角 - """ - # 获取当前世界的蓝图库 blueprint_library = self.world.get_blueprint_library() - # 根据车辆名称筛选蓝图(取第一个匹配结果) vehicle_bp = blueprint_library.filter(vehicle_name)[0] - # 设置生成位置 spawn_location = carla.Location(x, y, z) - # 设置生成旋转角度 spawn_rotation = carla.Rotation(pitch, yaw, roll) - # 组合位置和旋转为变换矩阵 spawn_transform = carla.Transform(location=spawn_location, rotation=spawn_rotation) - # 在指定位置生成车辆并赋值给主车辆 self.ego_vehicle = self.world.spawn_actor(vehicle_bp, spawn_transform) + return self.ego_vehicle - def set_spectator(self, x=0, y=0, z=0, pitch=0, yaw=0, roll=0): + def spawn_obstacle(self, obstacle_type='vehicle.tesla.model3', x=0, y=0, z=0, + pitch=0, yaw=0, roll=0, static=True, color=None): """ - 设置 spectator( spectator 是Carla中的视角控制者)的位置和角度 + 在指定位置生成障碍物 参数: - x, y, z: spectator 位置坐标(米) - pitch, yaw, roll: spectator 旋转角度(度) + obstacle_type: 障碍物类型 ('vehicle.*', 'static.prop.*', 或 'walker.*') + x, y, z: 位置坐标 + pitch, yaw, roll: 旋转角度 + static: 是否静态障碍物 + color: 自定义颜色 (R,G,B,A) + """ + try: + blueprint_library = self.world.get_blueprint_library() + + # 根据类型获取蓝图 + if obstacle_type.startswith('vehicle.'): + obstacle_bp = blueprint_library.filter(obstacle_type)[0] + # 设置车辆为静止 + if static: + obstacle_bp.set_attribute('role_name', 'static_obstacle') + elif obstacle_type.startswith('static.prop.'): + obstacle_bp = blueprint_library.filter(obstacle_type)[0] + elif obstacle_type.startswith('walker.'): + obstacle_bp = blueprint_library.filter(obstacle_type)[0] + else: + print(f"Unknown obstacle type: {obstacle_type}") + return None + + # 设置障碍物颜色(如果提供) + if color: + if obstacle_bp.has_attribute('color'): + obstacle_bp.set_attribute('color', f'{color[0]},{color[1]},{color[2]}') + + spawn_location = carla.Location(x, y, z) + spawn_rotation = carla.Rotation(pitch, yaw, roll) + spawn_transform = carla.Transform(location=spawn_location, rotation=spawn_rotation) + + obstacle = self.world.spawn_actor(obstacle_bp, spawn_transform) + self.obstacles.append(obstacle) + + print(f"Spawned obstacle at ({x:.1f}, {y:.1f}) of type {obstacle_type}") + return obstacle + + except Exception as e: + print(f"Failed to spawn obstacle: {e}") + return None + + def spawn_spaced_obstacles(self): """ - # 获取当前spectator + 生成间距合适的障碍物,避免过于密集 + 在车辆前方不同距离和横向位置生成3个障碍物 + """ + if not self.ego_vehicle: + print("No ego vehicle, cannot generate obstacles") + return [] + + vehicle_location = self.ego_vehicle.get_location() + vehicle_transform = self.ego_vehicle.get_transform() + vehicle_yaw = vehicle_transform.rotation.yaw + + # 获取车辆前方的waypoint + map = self.world.get_map() + current_waypoint = map.get_waypoint(vehicle_location) + + if not current_waypoint: + print("Cannot get current waypoint") + return [] + + # 定义障碍物配置:位置、类型、横向偏移 + obstacle_configs = [ + {'distance': 20.0, 'type': 'vehicle.audi.tt', 'lane_offset': 0.0, 'yaw_offset': 180, + 'color': (255, 0, 0, 200)}, # 正前方15米,掉头的车辆 + {'distance': 40.0, 'type': 'static.prop.trafficcone', 'lane_offset': 1.5, 'yaw_offset': 0, + 'color': (255, 255, 0, 200)}, # 前方30米,右侧交通锥 + {'distance': 50.0, 'type': 'vehicle.tesla.model3', 'lane_offset': -1.5, 'yaw_offset': 0, + 'color': (255, 165, 0, 200)}, # 前方45米,左侧车辆 + ] + + spawned_obstacles = [] + + for i, config in enumerate(obstacle_configs): + try: + distance = config['distance'] + obstacle_type = config['type'] + lane_offset = config['lane_offset'] # 正值=右侧,负值=左侧 + yaw_offset = config['yaw_offset'] + color = config['color'] + + # 获取前方waypoint + next_waypoints = current_waypoint.next(distance) + + if not next_waypoints: + print(f"Warning: No waypoint found at distance {distance}m") + continue + + target_waypoint = next_waypoints[0] + lane_width = target_waypoint.lane_width + + # 计算障碍物位置 + location = target_waypoint.transform.location + + # 如果有横向偏移,调整位置 + if lane_offset != 0: + # 计算横向偏移方向 + perpendicular_yaw = vehicle_yaw + 90 if lane_offset > 0 else vehicle_yaw - 90 + location.x += np.cos(np.radians(perpendicular_yaw)) * abs(lane_offset) + location.y += np.sin(np.radians(perpendicular_yaw)) * abs(lane_offset) + + # 设置朝向 + yaw = target_waypoint.transform.rotation.yaw + yaw_offset + + # 生成障碍物 + obstacle = self.spawn_obstacle( + obstacle_type=obstacle_type, + x=location.x, + y=location.y, + z=location.z + 0.1, + yaw=yaw, + static=True, + color=color + ) + + if obstacle: + spawned_obstacles.append(obstacle) + + side = "center" + if lane_offset > 0: + side = "right" + elif lane_offset < 0: + side = "left" + + print(f"Obstacle {i + 1}: {obstacle_type.split('.')[-1]} " + f"at {distance:.1f}m {side}, Position=({location.x:.1f}, {location.y:.1f})") + + except Exception as e: + print(f"Error spawning obstacle {i + 1}: {e}") + continue + + return spawned_obstacles + + def get_obstacle_positions(self): + """获取所有障碍物的位置和边界框信息""" + obstacle_info = [] + + for obstacle in self.obstacles: + try: + transform = obstacle.get_transform() + location = transform.location + + # 根据障碍物类型设置不同的安全距离 + obstacle_type = obstacle.type_id + if 'vehicle' in obstacle_type: + safe_distance = 5.0 # 车辆类障碍物需要更大的安全距离 + radius = 2.5 + elif 'streetbarrier' in obstacle_type: + safe_distance = 2.0 + radius = 1.0 + elif 'trafficcone' in obstacle_type: + safe_distance = 1.5 + radius = 0.5 + else: + safe_distance = 2.0 + radius = 1.0 + + obstacle_info.append({ + 'id': obstacle.id, + 'type': obstacle_type, + 'x': location.x, + 'y': location.y, + 'z': location.z, + 'radius': radius, + 'safe_distance': safe_distance + }) + + except Exception as e: + print(f"Error getting obstacle info: {e}") + continue + + return obstacle_info + + def draw_obstacles_with_info(self, obstacles_info, life_time=0.1): + """绘制障碍物及其相关信息""" + for obs_info in obstacles_info: + try: + location = carla.Location(x=obs_info['x'], y=obs_info['y'], z=obs_info['z'] + 0.5) + radius = obs_info.get('radius', 2.0) + safe_distance = obs_info.get('safe_distance', 3.0) + + # 根据障碍物类型设置颜色 + if 'vehicle' in obs_info['type']: + color = carla.Color(255, 0, 0) # 红色 + elif 'streetbarrier' in obs_info['type']: + color = carla.Color(128, 0, 128) # 紫色 + elif 'trafficcone' in obs_info['type']: + color = carla.Color(255, 255, 0) # 黄色 + else: + color = carla.Color(255, 165, 0) # 橙色 + + # 绘制障碍物位置(大点) + self.world.debug.draw_point( + location, + size=0.3, + color=color, + life_time=life_time + ) + + # 绘制障碍物半径 + self.draw_circle(location, radius, color=color, life_time=life_time) + + # 绘制安全距离圆(虚线) + self.draw_dashed_circle(location, safe_distance, + color=carla.Color(255, 255, 255, 100), # 白色半透明 + dash_length=0.3, gap_length=0.3, + life_time=life_time) + + # 绘制障碍物类型标签 + text_location = carla.Location(x=location.x, y=location.y, z=location.z + 1.5) + self.world.debug.draw_string( + text_location, + obs_info['type'].split('.')[-1], + draw_shadow=True, + color=carla.Color(255, 255, 255), + life_time=life_time, + persistent_lines=False + ) + + except Exception as e: + print(f"Error drawing obstacle info: {e}") + continue + + def draw_circle(self, center, radius, color=carla.Color(255, 0, 0), life_time=0.1): + """绘制圆形""" + num_segments = 32 + for i in range(num_segments): + angle1 = 2 * np.pi * i / num_segments + angle2 = 2 * np.pi * (i + 1) / num_segments + + p1 = carla.Location( + center.x + radius * np.cos(angle1), + center.y + radius * np.sin(angle1), + center.z + ) + p2 = carla.Location( + center.x + radius * np.cos(angle2), + center.y + radius * np.sin(angle2), + center.z + ) + + self.world.debug.draw_line( + p1, p2, + thickness=0.05, + color=color, + life_time=life_time + ) + + def draw_dashed_circle(self, center, radius, color=carla.Color(255, 255, 255), + dash_length=0.5, gap_length=0.5, life_time=0.1): + """绘制虚线圆形""" + num_segments = 64 + dash_pattern = True # True表示绘制,False表示间隙 + + for i in range(num_segments): + angle1 = 2 * np.pi * i / num_segments + angle2 = 2 * np.pi * (i + 1) / num_segments + + # 计算圆弧长度 + arc_length = radius * (angle2 - angle1) + + # 如果圆弧长度大于dash_length+gap_length,我们需要进一步细分 + if arc_length > dash_length + gap_length: + sub_segments = int(arc_length / (dash_length + gap_length)) + for j in range(sub_segments): + sub_angle1 = angle1 + j * (angle2 - angle1) / sub_segments + sub_angle2 = angle1 + (j + 1) * (angle2 - angle1) / sub_segments + + if dash_pattern: + p1 = carla.Location( + center.x + radius * np.cos(sub_angle1), + center.y + radius * np.sin(sub_angle1), + center.z + ) + p2 = carla.Location( + center.x + radius * np.cos(sub_angle2), + center.y + radius * np.sin(sub_angle2), + center.z + ) + + self.world.debug.draw_line( + p1, p2, + thickness=0.02, + color=color, + life_time=life_time + ) + + dash_pattern = not dash_pattern + else: + if dash_pattern: + p1 = carla.Location( + center.x + radius * np.cos(angle1), + center.y + radius * np.sin(angle1), + center.z + ) + p2 = carla.Location( + center.x + radius * np.cos(angle2), + center.y + radius * np.sin(angle2), + center.z + ) + + self.world.debug.draw_line( + p1, p2, + thickness=0.02, + color=color, + life_time=life_time + ) + + # 切换绘制模式 + if arc_length > dash_length: + dash_pattern = not dash_pattern + + def set_spectator(self, x=0, y=0, z=0, pitch=0, yaw=0, roll=0): spectator = self.world.get_spectator() - # 设置位置 location = carla.Location(x=x, y=y, z=z) - # 设置旋转角度 rotation = carla.Rotation(pitch=pitch, yaw=yaw, roll=roll) - # 组合为变换矩阵 spectator_transform = carla.Transform(location, rotation) - # 应用变换到spectator spectator.set_transform(spectator_transform) def clean(self): - """清理场景中所有车辆(销毁所有车辆actor)""" - for vehicle in self.world.get_actors().filter('*vehicle*'): - vehicle.destroy() + self.destroy_all_actors() - def draw_perception_planning(self, x_ref, y_ref, current_idx, look_ahead_points=20): + def draw_perception_planning(self, x_ref, y_ref, current_idx, look_ahead_points=20, obstacles_info=None): """ - 绘制感知规划信息:只显示车辆前方需要跟踪的轨迹点 - - 参数: - x_ref: 参考轨迹x坐标 - y_ref: 参考轨迹y坐标 - current_idx: 当前轨迹点索引 - look_ahead_points: 向前看的点数 + 绘制感知规划信息,包括障碍物 """ - # 清除之前的绘制(通过绘制空列表) + # 清除之前的绘制 self.draw_trajectory([], [], life_time=0.1) - # 计算要显示的轨迹点范围 + # 绘制障碍物信息 + if obstacles_info: + self.draw_obstacles_with_info(obstacles_info, life_time=0.1) + + # 绘制前方轨迹 total_points = len(x_ref) if total_points == 0: return - # 获取前方轨迹点 end_idx = min(current_idx + look_ahead_points, total_points) - # 提取要显示的轨迹点 if current_idx < end_idx: display_x = x_ref[current_idx:end_idx] display_y = y_ref[current_idx:end_idx] else: - # 处理循环轨迹 display_x = list(x_ref[current_idx:]) + list(x_ref[:end_idx]) display_y = list(y_ref[current_idx:]) + list(y_ref[:end_idx]) - # 绘制前方轨迹(绿色) if len(display_x) > 1: self.draw_trajectory( display_x, @@ -118,7 +404,6 @@ def draw_perception_planning(self, x_ref, y_ref, current_idx, look_ahead_points= life_time=0.1 ) - # 绘制当前目标点(红色) if len(display_x) > 0: target_location = carla.Location( x=display_x[0], @@ -128,33 +413,28 @@ def draw_perception_planning(self, x_ref, y_ref, current_idx, look_ahead_points= self.world.debug.draw_point( target_location, size=0.3, - color=carla.Color(255, 0, 0), + color=carla.Color(0, 255, 0), # 绿色目标点 life_time=0.1 ) - # 绘制从车辆到目标点的连线 - vehicle_location = self.ego_vehicle.get_transform().location - self.world.debug.draw_line( - vehicle_location, - target_location, - thickness=0.1, - color=carla.Color(0, 255, 255), - life_time=0.1 - ) + if self.ego_vehicle: + vehicle_location = self.ego_vehicle.get_transform().location + self.world.debug.draw_line( + vehicle_location, + target_location, + thickness=0.1, + color=carla.Color(0, 255, 255), + life_time=0.1 + ) def draw_frenet_frame(self, x_ref, y_ref, current_idx): - """ - 绘制Frenet坐标系参考线(用于显示横向偏差) - """ total_points = len(x_ref) - if total_idx + 1 >= total_points: + if current_idx + 1 >= total_points: return - # 获取当前参考线段 x1, y1 = x_ref[current_idx], y_ref[current_idx] x2, y2 = x_ref[(current_idx + 1) % total_points], y_ref[(current_idx + 1) % total_points] - # 绘制参考线段(蓝色) start_point = carla.Location(x=x1, y=y1, z=0.3) end_point = carla.Location(x=x2, y=y2, z=0.3) @@ -165,25 +445,11 @@ def draw_frenet_frame(self, x_ref, y_ref, current_idx): color=carla.Color(0, 0, 255), life_time=0.1 ) - def draw_trajectory(self, x_traj, y_traj, height=0, thickness=0.1, red=0, green=0, blue=0, life_time=0.1): - """ - 在世界中绘制轨迹线 - 参数: - x_traj: 轨迹点的x坐标列表 - y_traj: 轨迹点的y坐标列表 - height: 轨迹线的z轴高度(米) - thickness: 线的粗细 - red, green, blue: 线的颜色(0-255) - life_time: 线在世界中的存在时间(秒) - """ - # 遍历轨迹点,绘制连续线段 + def draw_trajectory(self, x_traj, y_traj, height=0, thickness=0.1, red=0, green=0, blue=0, life_time=0.1): for i in range(len(x_traj) - 1): - # 线段起点 start_point = carla.Location(x=x_traj[i], y=y_traj[i], z=height) - # 线段终点 end_point = carla.Location(x=x_traj[i + 1], y=y_traj[i + 1], z=height) - # 在世界中绘制线段 self.world.debug.draw_line( start_point, end_point, @@ -193,109 +459,70 @@ def draw_trajectory(self, x_traj, y_traj, height=0, thickness=0.1, red=0, green= ) def get_main_ego_vehicle_state(self): - """ - 获取主车辆的当前状态 + if not self.ego_vehicle: + return 0, 0, 0, 0 - 返回: - x: 车辆位置x坐标(米) - y: 车辆位置y坐标(米) - theta: 车辆航向角(弧度,从yaw角度转换而来) - v: 车辆瞬时速度(米/秒,x和y方向速度的合速度) - """ - # 获取车辆变换信息(位置和旋转) transform = self.ego_vehicle.get_transform() x = transform.location.x y = transform.location.y - # 将yaw角度(度)转换为弧度(航向角) theta = np.deg2rad(transform.rotation.yaw) - # 计算x和y方向速度的合速度(标量) v = np.sqrt(self.ego_vehicle.get_velocity().x ** 2 + self.ego_vehicle.get_velocity().y ** 2) return x, y, theta, v def apply_control(self, steer, throttle, brake): - """ - 向主车辆应用控制指令 - - 参数: - steer: 转向指令(-1到1之间,对应左右转向) - throttle: 油门指令(0到1之间) - brake: 刹车指令(0到1之间) - """ - # 调用Carla的VehicleControl接口应用控制 - self.ego_vehicle.apply_control(carla.VehicleControl(throttle=throttle, steer=steer, brake=brake)) + if self.ego_vehicle: + self.ego_vehicle.apply_control(carla.VehicleControl(throttle=throttle, steer=steer, brake=brake)) @staticmethod def process_control_inputs(wheel_angle_rad, acceleration_m_s_2): - """ - 处理原始控制输入(轮角和加速度),转换为Carla需要的标准化控制指令 - - 参数: - wheel_angle_rad: 车轮转角(弧度) - acceleration_m_s_2: 加速度指令(米/秒²,正为加速,负为刹车) - - 返回: - throttle: 标准化油门指令(0到1) - brake: 标准化刹车指令(0到1) - steer: 标准化转向指令(-1到1) - """ if acceleration_m_s_2 == 0: - # 无加速度指令时,油门和刹车均为0 throttle = 0 brake = 0 elif acceleration_m_s_2 < 0: - # 负加速度(刹车),标准化到刹车指令(除以最大刹车加速度) throttle = 0 - brake = acceleration_m_s_2 / MAX_BRAKING_M_S_2 + brake = max(min(acceleration_m_s_2 / MAX_BRAKING_M_S_2, 1.0), 0.0) else: - # 正加速度(加速),标准化到油门指令(除以最大加速度) - throttle = acceleration_m_s_2 / MAX_ACCELERATION_M_S_2 + throttle = max(min(acceleration_m_s_2 / MAX_ACCELERATION_M_S_2, 1.0), 0.0) brake = 0 - # 车轮转角标准化(除以最大轮角) - steer = wheel_angle_rad / MAX_WHEEL_ANGLE_RAD + steer = max(min(wheel_angle_rad / MAX_WHEEL_ANGLE_RAD, 1.0), -1.0) return throttle, brake, steer def print_ego_vehicle_characteristics(self): - """打印主车辆的物理特性参数(如车轮信息、扭矩曲线、质量等)""" if not self.ego_vehicle: - print("Vehicle not spawned yet!") # 车辆未生成时提示 + print("Vehicle not spawned yet!") return None - # 获取车辆物理控制参数 physics_control = self.ego_vehicle.get_physics_control() print("Vehicle Physics Information.\n") - # 打印车轮信息 print("Wheel Information:") for i, wheel in enumerate(physics_control.wheels): print(f" Wheel {i + 1}:") - print(f" Tire Friction: {wheel.tire_friction}") # 轮胎摩擦系数 - print(f" Damping Rate: {wheel.damping_rate}") # 阻尼率 - print(f" Max Steer Angle: {wheel.max_steer_angle}") # 最大转向角 - print(f" Radius: {wheel.radius}") # 车轮半径 - print(f" Max Brake Torque: {wheel.max_brake_torque}") # 最大刹车扭矩 - print(f" Max Handbrake Torque: {wheel.max_handbrake_torque}") # 最大手刹扭矩 - print(f" Position (x, y, z): ({wheel.position.x}, {wheel.position.y}, {wheel.position.z})") # 车轮位置 - - # 打印扭矩曲线(RPM-扭矩关系) + print(f" Tire Friction: {wheel.tire_friction}") + print(f" Damping Rate: {wheel.damping_rate}") + print(f" Max Steer Angle: {wheel.max_steer_angle}") + print(f" Radius: {wheel.radius}") + print(f" Max Brake Torque: {wheel.max_brake_torque}") + print(f" Max Handbrake Torque: {wheel.max_handbrake_torque}") + print(f" Position (x, y, z): ({wheel.position.x}, {wheel.position.y}, {wheel.position.z})") + print(f" Torque Curve:") for point in physics_control.torque_curve: print(f"RPM: {point.x}, Torque: {point.y}") - print(f" Max RPM: {physics_control.max_rpm}") # 最大转速 - print(f" MOI (Moment of Inertia): {physics_control.moi}") # 转动惯量 - print(f" Damping Rate Full Throttle: {physics_control.damping_rate_full_throttle}") # 全油门时的阻尼率 - print( - f" Damping Rate Zero Throttle Clutch Engaged: {physics_control.damping_rate_zero_throttle_clutch_engaged}") # 零油门离合器结合时的阻尼率 + print(f" Max RPM: {physics_control.max_rpm}") + print(f" MOI (Moment of Inertia): {physics_control.moi}") + print(f" Damping Rate Full Throttle: {physics_control.damping_rate_full_throttle}") print( - f" Damping Rate Zero Throttle Clutch Disengaged: {physics_control.damping_rate_zero_throttle_clutch_disengaged}") # 零油门离合器分离时的阻尼率 + f" Damping Rate Zero Throttle Clutch Engaged: {physics_control.damping_rate_zero_throttle_clutch_engaged}") print( - f" If True, the vehicle will have an automatic transmission: {physics_control.use_gear_autobox}") # 是否自动变速箱 - print(f" Gear Switch Time: {physics_control.gear_switch_time}") # 换挡时间 - print(f" Clutch Strength: {physics_control.clutch_strength}") # 离合器强度 - print(f" Final Ratio: {physics_control.final_ratio}") # 最终传动比 - print(f" Mass: {physics_control.mass}") # 车辆质量 - print(f" Drag coefficient: {physics_control.drag_coefficient}") # 空气阻力系数 - # 打印转向曲线(速度-转向关系) + f" Damping Rate Zero Throttle Clutch Disengaged: {physics_control.damping_rate_zero_throttle_clutch_disengaged}") + print(f" If True, the vehicle will have an automatic transmission: {physics_control.use_gear_autobox}") + print(f" Gear Switch Time: {physics_control.gear_switch_time}") + print(f" Clutch Strength: {physics_control.clutch_strength}") + print(f" Final Ratio: {physics_control.final_ratio}") + print(f" Mass: {physics_control.mass}") + print(f" Drag coefficient: {physics_control.drag_coefficient}") print(f" Steering Curve:") for point in physics_control.steering_curve: print(f"Speed: {point.x}, Steering: {point.y}") \ No newline at end of file diff --git a/src/Unmanned_vehicle_control/main_help_functions.py b/src/Unmanned_vehicle_control/main_help_functions.py index ef4e8b76a0..3c79e1aa9a 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] @@ -110,4 +156,3 @@ def calculate_lateral_deviation(x, y, x_ref1, y_ref1, x_ref2, y_ref2): return num / denom else: return 0 - diff --git a/src/Unmanned_vehicle_control/main_mpc_controller.py b/src/Unmanned_vehicle_control/main_mpc_controller.py index 24f4a5d1cd..5e8226a691 100644 --- a/src/Unmanned_vehicle_control/main_mpc_controller.py +++ b/src/Unmanned_vehicle_control/main_mpc_controller.py @@ -1,14 +1,14 @@ import casadi as ca import numpy as np - 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): + def __init__(self, horizon=10, dt=0.1): self.horizon = horizon self.opti = None self.dt = dt @@ -20,18 +20,26 @@ def __init__(self, horizon = 10, dt = 0.1): self.control_buffer = {"acceleration": [0] * self.horizon, "wheel_angle": [0] * self.horizon} self.buffer_index = 0 + # 避障相关参数 + self.obstacles = [] # 障碍物列表,每个元素是字典包含位置和半径 + self.obstacle_avoidance_enabled = True + self.obstacle_safety_distance = 3.0 # 安全距离 + self.obstacle_penalty_weight = 500.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.max_cpu_time': self.dt, + 'ipopt.max_iter': 100, # 增加最大迭代次数,确保找到可行解 + 'ipopt.tol': 1e-3, + '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 + 'ipopt.warm_start_mult_bound_push': 1e-6, + 'ipopt.acceptable_tol': 1e-2, # 可接受容差 + 'ipopt.acceptable_iter': 5, } self.opti.solver('ipopt', opts) @@ -39,14 +47,12 @@ def reset_solver(self): 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) + self.opti.set_initial(self.X, self.prev_X) + self.opti.set_initial(self.U, self.prev_U) + # 动力学约束 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) @@ -57,8 +63,18 @@ def reset_solver(self): def set_init_vehicle_state(self, x, y, theta, v): self.opti.subject_to(self.X[:, 0] == ca.vertcat(x, y, theta, v)) + def set_obstacles(self, obstacles): + """设置障碍物信息""" + self.obstacles = obstacles + + def enable_obstacle_avoidance(self, enable=True): + """启用/禁用避障功能""" + self.obstacle_avoidance_enabled = enable + 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) @@ -67,11 +83,12 @@ def update_cost_function(self, x_ref, y_ref, theta_ref, v_ref): 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]) + 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 @@ -85,28 +102,104 @@ def update_cost_function(self, x_ref, y_ref, theta_ref, v_ref): self.cost += fine_x + fine_y + fine_v + fine_steer + fine_acc + fine_steer_dot + fine_acc_dot + fine_theta + fine_lateral + # 避障代价 + if self.obstacle_avoidance_enabled and self.obstacles: + obstacle_cost = self._calculate_obstacle_cost() + self.cost += obstacle_cost + self.opti.minimize(self.cost) + def _calculate_obstacle_cost(self): + """计算障碍物惩罚项 - 更有效的避障代价函数""" + obstacle_cost = 0 + + for k in range(self.horizon): + vehicle_x = self.X[0, k] + vehicle_y = self.X[1, k] + + for obstacle in self.obstacles: + obs_x = obstacle.get('x', 0) + obs_y = obstacle.get('y', 0) + radius = obstacle.get('radius', 1.0) + safe_distance = obstacle.get('safe_distance', self.obstacle_safety_distance) + + # 计算车辆到障碍物的距离 + dx = vehicle_x - obs_x + dy = vehicle_y - obs_y + distance_sq = dx ** 2 + dy ** 2 + distance = ca.sqrt(distance_sq) + + # 障碍物半径加上车辆半径(假设车辆半径约2米) + total_radius = radius + 2.0 + + # 使用强排斥力场:当距离小于安全距离时,施加极大的惩罚 + # 使用反比例函数,距离越近惩罚越大 + penalty_strength = self.obstacle_penalty_weight + + # 计算惩罚项 + if total_radius > 0: + # 使用障碍物的实际半径计算 + normalized_distance = distance / (total_radius + 0.1) # 加0.1避免除零 + else: + normalized_distance = distance + + # 当距离很近时,使用更大的惩罚 + obstacle_penalty = penalty_strength * ca.exp(-normalized_distance / 2.0) + + # 额外的近距离惩罚(当距离小于总半径+安全距离时) + collision_distance = total_radius + 1.0 # 碰撞距离 + if collision_distance > 0: + collision_factor = ca.fmax(0, collision_distance - distance) / collision_distance + collision_penalty = 1000.0 * collision_factor ** 2 + obstacle_penalty += collision_penalty + + # 添加方向引导:如果车辆朝向障碍物运动,增加惩罚 + if k > 0: + prev_x = self.X[0, k - 1] + prev_y = self.X[1, k - 1] + vel_x = vehicle_x - prev_x + vel_y = vehicle_y - prev_y + + # 计算速度方向与到障碍物方向的内积(如果为正表示朝向障碍物运动) + vel_mag = ca.sqrt(vel_x ** 2 + vel_y ** 2 + 0.01) + dir_to_obs_x = dx / (distance + 0.01) + dir_to_obs_y = dy / (distance + 0.01) + + dot_product = (vel_x / vel_mag) * dir_to_obs_x + (vel_y / vel_mag) * dir_to_obs_y + + # 如果朝向障碍物运动,增加惩罚 + approach_penalty = 200.0 * ca.fmax(0, dot_product) * ca.exp(-distance / 5.0) + obstacle_penalty += approach_penalty + + obstacle_cost += obstacle_penalty + + return obstacle_cost + 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] self.buffer_index = 0 - except Exception: - print("Error or delay upon MPC solution calculation. Previous calculated control value will be used") + + except Exception as e: + print(f"Error or delay upon MPC solution calculation: {e}") + print("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]) - # 控制量饱和(与config约束一致) + + # 控制量饱和 wheel_angle_rad = np.clip(wheel_angle_rad, -MAX_CONTROL_WHEEL_ANGLE_RAD, MAX_CONTROL_WHEEL_ANGLE_RAD) @@ -115,15 +208,18 @@ def get_controls_value(self): MAX_CONTROL_ACCELERATION_M_S_2) else: if self.buffer_index < self.horizon: - # 求解失败时,控制量渐进衰减(减少突变) - decay = 0.9 # 衰减系数 + decay = 0.9 acceleration_m_s_2 = self.control_buffer["acceleration"][self.buffer_index] * decay wheel_angle_rad = self.control_buffer["wheel_angle"][self.buffer_index] * decay self.buffer_index += 1 else: - raise Exception("Control buffer is empty.") + # 如果没有历史控制量,使用零控制 + acceleration_m_s_2 = 0 + wheel_angle_rad = 0 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 + if self.sol: + return self.sol.value(self.cost) + return float('inf') \ No newline at end of file