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