From 552d9bd33dd05487821a612bba951f7e459a2f8f Mon Sep 17 00:00:00 2001 From: Liufu27 <2183667346@qq.com> Date: Mon, 8 Dec 2025 10:13:45 +0800 Subject: [PATCH 1/9] =?UTF-8?q?=E5=9C=A8main=5Fhelp=5Ffunctions.py?= =?UTF-8?q?=E4=B8=AD=E6=B7=BB=E5=8A=A0=E4=B8=80=E4=B8=AA=E6=96=B0=E7=9A=84?= =?UTF-8?q?=E8=BD=A8=E8=BF=B9=E2=80=9C=E5=9C=86=E5=BD=A2=E8=BD=A8=E8=BF=B9?= =?UTF-8?q?=E2=80=9D?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../main_help_functions.py | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/src/Unmanned_vehicle_control/main_help_functions.py b/src/Unmanned_vehicle_control/main_help_functions.py index 09ddbdb74b..17d78685a8 100644 --- a/src/Unmanned_vehicle_control/main_help_functions.py +++ b/src/Unmanned_vehicle_control/main_help_functions.py @@ -41,6 +41,24 @@ def get_eight_trajectory(x_init, y_init, total_points=100): theta_ref.append(theta_ref[-1]) return x_traj_rotated, y_traj_rotated, v_ref, theta_ref +def get_circle_trajectory(x_center, y_center, radius=25, total_points=200): + """生成圆形轨迹""" + t_values = np.linspace(0, 2 * np.pi, total_points) + x_traj = x_center + radius * np.cos(t_values) + y_traj = y_center + radius * np.sin(t_values) + + 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): From ffe1cbf3a651b94ba73ac9fbed53637813f84d07 Mon Sep 17 00:00:00 2001 From: Liufu27 <2183667346@qq.com> Date: Mon, 15 Dec 2025 10:02:42 +0800 Subject: [PATCH 2/9] =?UTF-8?q?=E6=9B=B4=E6=96=B0main=EF=BC=8C=E4=B8=BB?= =?UTF-8?q?=E5=87=BD=E6=95=B0=E6=96=B0=E6=B7=BB=E5=8A=A0=E8=B0=83=E7=94=A8?= =?UTF-8?q?=E5=9C=86=E5=BD=A2=E8=BD=A8=E8=BF=B9=EF=BC=8C=E5=B9=B6=E8=BF=90?= =?UTF-8?q?=E8=A1=8C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Unmanned_vehicle_control/main.py | 42 +++++++++++++++++++++++----- 1 file changed, 35 insertions(+), 7 deletions(-) diff --git a/src/Unmanned_vehicle_control/main.py b/src/Unmanned_vehicle_control/main.py index 8179babc2e..7d641fe115 100644 --- a/src/Unmanned_vehicle_control/main.py +++ b/src/Unmanned_vehicle_control/main.py @@ -1,10 +1,11 @@ import time import threading -from src.carla_simulator import CarlaSimulator +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.help_functions import get_eight_trajectory, get_ref_trajectory, update_reference_point -from src.logger import Logger -from src.mpc_controller import MpcController +from src.main_help_functions import get_eight_trajectory, get_ref_trajectory, get_circle_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: @@ -13,13 +14,40 @@ def draw_trajectory_in_thread(carla, x_traj, y_traj, 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) +#carla.spawn_ego_vehicle('vehicle.tesla.model3', x=X_INIT_M, y=Y_INIT_M, z=0.1) carla.print_ego_vehicle_characteristics() carla.set_spectator(X_INIT_M, Y_INIT_M, z=50, pitch=-90) -logger = Logger() +#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( + x_center=X_INIT_M, + y_center=Y_INIT_M, + radius=20, + 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 # 初始方向与轨迹一致 +) + +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_eight_trajectory(X_INIT_M, Y_INIT_M) +logger = Logger() current_idx = 0 laps = 0 From 7980325841b440bc859f5262f54a55748e909f8b Mon Sep 17 00:00:00 2001 From: Liufu27 <2183667346@qq.com> Date: Tue, 16 Dec 2025 09:28:00 +0800 Subject: [PATCH 3/9] =?UTF-8?q?=E7=94=9F=E6=88=90=E4=BA=86=E6=96=B0?= =?UTF-8?q?=E7=9A=84=E8=9E=BA=E6=97=8B=E7=BA=BF=E8=BD=A8=E8=BF=B9=EF=BC=8C?= =?UTF-8?q?=E5=B9=B6=E4=B8=94=E8=BD=A8=E8=BF=B9=E4=B8=8D=E8=B6=85=E5=87=BA?= =?UTF-8?q?=E5=9C=BA=E5=9C=B0=E8=8C=83=E5=9B=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../main_help_functions.py | 33 ++++++++++++++++--- 1 file changed, 29 insertions(+), 4 deletions(-) diff --git a/src/Unmanned_vehicle_control/main_help_functions.py b/src/Unmanned_vehicle_control/main_help_functions.py index 17d78685a8..ef4e8b76a0 100644 --- a/src/Unmanned_vehicle_control/main_help_functions.py +++ b/src/Unmanned_vehicle_control/main_help_functions.py @@ -1,7 +1,7 @@ import numpy as np from src.config import N, V_REF, A - +from src.config import CIRCLE_CENTER_X, CIRCLE_CENTER_Y, CIRCLE_RADIUS def update_reference_point(x0, y0, current_idx, x_traj, y_traj, min_distance=5.0): # 校验轨迹数组长度一致性与非空 if len(x_traj) != len(y_traj) or len(x_traj) == 0: @@ -41,14 +41,38 @@ def get_eight_trajectory(x_init, y_init, total_points=100): theta_ref.append(theta_ref[-1]) return x_traj_rotated, y_traj_rotated, v_ref, theta_ref -def get_circle_trajectory(x_center, y_center, radius=25, total_points=200): - """生成圆形轨迹""" +def get_circle_trajectory( + x_center=CIRCLE_CENTER_X, # 从config.py读取默认圆心X + y_center=CIRCLE_CENTER_Y, # 从config.py读取默认圆心Y + # radius=CIRCLE_RADIUS, # 从config.py读取默认半径 + radius=20, # 手动调节可适用无错误的半径 + total_points=200 +): + """生成圆形轨迹(默认参数从配置文件读取)""" t_values = np.linspace(0, 2 * np.pi, total_points) x_traj = x_center + radius * np.cos(t_values) y_traj = y_center + radius * np.sin(t_values) 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_spiral_trajectory(x_init, y_init, total_points=200, turns=2, scale=2): + """生成螺旋线轨迹""" + t_values = np.linspace(0, 2 * np.pi * turns, total_points) + # 极坐标方程:r = scale * t(半径随角度线性增加) + r = scale * t_values + x_traj = x_init + r * np.cos(t_values) + y_traj = y_init + r * np.sin(t_values) + v_ref = [V_REF for _ in range(total_points)] # 计算参考角度(切线方向) theta_ref = [] for i in range(total_points - 1): @@ -85,4 +109,5 @@ def calculate_lateral_deviation(x, y, x_ref1, y_ref1, x_ref2, y_ref2): if denom > 0.01: return num / denom else: - return 0 \ No newline at end of file + return 0 + From a04deab7342dca0c846d91fd189c9be2ffb3eff0 Mon Sep 17 00:00:00 2001 From: Liufu27 <2183667346@qq.com> Date: Tue, 16 Dec 2025 10:12:27 +0800 Subject: [PATCH 4/9] =?UTF-8?q?=E6=9B=B4=E6=96=B0main.py=EF=BC=8C=E5=B9=B6?= =?UTF-8?q?=E8=B0=83=E7=94=A8=E8=9E=BA=E6=97=8B=E8=BD=A8=E8=BF=B9=EF=BC=8C?= =?UTF-8?q?=E5=B9=B6=E8=BF=90=E8=A1=8C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Unmanned_vehicle_control/main.py | 51 +++++++++++++++++----------- 1 file changed, 32 insertions(+), 19 deletions(-) diff --git a/src/Unmanned_vehicle_control/main.py b/src/Unmanned_vehicle_control/main.py index 7d641fe115..0973b4c355 100644 --- a/src/Unmanned_vehicle_control/main.py +++ b/src/Unmanned_vehicle_control/main.py @@ -3,7 +3,7 @@ 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, update_reference_point +from src.main_help_functions import get_eight_trajectory, get_ref_trajectory, get_circle_trajectory, get_spiral_trajectory, update_reference_point from src.main_logger import Logger from src.main_mpc_controller import MpcController @@ -14,27 +14,20 @@ def draw_trajectory_in_thread(carla, x_traj, y_traj, 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) +#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 +""" +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( - x_center=X_INIT_M, - y_center=Y_INIT_M, - radius=20, - total_points=200 -) - -# 从圆形轨迹的第一个点生成车辆(确保车辆在轨迹上) -init_x, init_y = x_traj[0], y_traj[0] -# 获取初始角度(轨迹切线方向) -init_yaw = np.rad2deg(theta_traj[0]) +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, @@ -46,6 +39,26 @@ def draw_trajectory_in_thread(carla, x_traj, y_traj, dt): 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() current_idx = 0 From a645213666ccebf6864c117275e371eec2df603f Mon Sep 17 00:00:00 2001 From: Liufu27 <2183667346@qq.com> Date: Tue, 16 Dec 2025 11:31:13 +0800 Subject: [PATCH 5/9] =?UTF-8?q?=E6=8F=90=E4=BA=A4=E5=90=AF=E5=8A=A8carla?= =?UTF-8?q?=E6=96=87=E4=BB=B6=EF=BC=8C=E5=88=9D=E5=A7=8B=E5=8C=96=E5=9C=B0?= =?UTF-8?q?=E5=9B=BE=E5=9C=BA=E6=99=AF=E7=AD=89?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../main_carla_simulator.py | 105 ++++++++++++++++++ 1 file changed, 105 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..729af0edad --- /dev/null +++ b/src/Unmanned_vehicle_control/main_carla_simulator.py @@ -0,0 +1,105 @@ +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: + def __init__(self): + self.client = carla.Client('localhost', 2000) + self.client.set_timeout(10.0) + self.world = self.client.get_world() + self.ego_vehicle = None + for vehicle in self.world.get_actors().filter('*vehicle*'): + vehicle.destroy() + + def load_world(self, map_name): + 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): + 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 = 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.set_transform(spectator_transform) + + def clean(self): + for vehicle in self.world.get_actors().filter('*vehicle*'): + vehicle.destroy() + + 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, thickness=thickness, color=carla.Color(red, green, blue), life_time=life_time) + + def get_main_ego_vehicle_state(self): + transform = self.ego_vehicle.get_transform() + x = transform.location.x + y = transform.location.y + theta = np.deg2rad(transform.rotation.yaw) + 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): + 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): + if acceleration_m_s_2 == 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})") + + 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 77ff2c24832acfeebe883b641c09ffdd45c9e999 Mon Sep 17 00:00:00 2001 From: Liufu27 <2183667346@qq.com> Date: Tue, 16 Dec 2025 23:20:17 +0800 Subject: [PATCH 6/9] Delete src/Unmanned_vehicle_control/main_help_functions.py --- .../main_help_functions.py | 113 ------------------ 1 file changed, 113 deletions(-) delete mode 100644 src/Unmanned_vehicle_control/main_help_functions.py diff --git a/src/Unmanned_vehicle_control/main_help_functions.py b/src/Unmanned_vehicle_control/main_help_functions.py deleted file mode 100644 index ef4e8b76a0..0000000000 --- a/src/Unmanned_vehicle_control/main_help_functions.py +++ /dev/null @@ -1,113 +0,0 @@ -import numpy as np - -from src.config import N, V_REF, A -from src.config import CIRCLE_CENTER_X, CIRCLE_CENTER_Y, CIRCLE_RADIUS -def update_reference_point(x0, y0, current_idx, x_traj, y_traj, min_distance=5.0): - # 校验轨迹数组长度一致性与非空 - if len(x_traj) != len(y_traj) or len(x_traj) == 0: - raise ValueError("x_traj和y_traj长度必须一致且非空") - # 校验current_idx的合法性,若超出范围则修正为有效索引 - if current_idx < 0 or current_idx >= len(x_traj): - current_idx = 0 # 超出范围时重置为0索引 - distance_to_current_point = np.sqrt((x_traj[current_idx] - x0) ** 2 + (y_traj[current_idx] - y0) ** 2) - - if distance_to_current_point < min_distance: - current_idx = (current_idx + 1) % len(x_traj) - - return current_idx - -def get_eight_trajectory(x_init, y_init, total_points=100): - t = 2 * np.pi - - t_values = np.linspace(0, t, total_points) - x_traj = x_init + A * np.sin(t_values) - y_traj = y_init + A * np.sin(t_values) * np.cos(t_values) - - v_ref = [V_REF for _ in range(total_points)] - - cos_angle = np.cos(-np.pi / 4) - sin_angle = np.sin(-np.pi / 4) - - x_traj_rotated = x_init + cos_angle * (x_traj - x_init) - sin_angle * (y_traj - y_init) - y_traj_rotated = y_init + sin_angle * (x_traj - x_init) + cos_angle * (y_traj - y_init) - - theta_ref = [] - for i in range(total_points - 1): - dx = x_traj_rotated[i + 1] - x_traj_rotated[i] - dy = y_traj_rotated[i + 1] - y_traj_rotated[i] - theta = np.arctan2(dy, dx) - theta_ref.append(theta) - - theta_ref.append(theta_ref[-1]) - - return x_traj_rotated, y_traj_rotated, v_ref, theta_ref -def get_circle_trajectory( - x_center=CIRCLE_CENTER_X, # 从config.py读取默认圆心X - y_center=CIRCLE_CENTER_Y, # 从config.py读取默认圆心Y - # radius=CIRCLE_RADIUS, # 从config.py读取默认半径 - radius=20, # 手动调节可适用无错误的半径 - total_points=200 -): - """生成圆形轨迹(默认参数从配置文件读取)""" - t_values = np.linspace(0, 2 * np.pi, total_points) - x_traj = x_center + radius * np.cos(t_values) - y_traj = y_center + radius * np.sin(t_values) - - 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_spiral_trajectory(x_init, y_init, total_points=200, turns=2, scale=2): - """生成螺旋线轨迹""" - t_values = np.linspace(0, 2 * np.pi * turns, total_points) - # 极坐标方程:r = scale * t(半径随角度线性增加) - r = scale * t_values - x_traj = x_init + r * np.cos(t_values) - y_traj = y_init + r * np.sin(t_values) - 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] - y_ref = y_traj[current_idx:current_idx + N] - theta_ref = theta_traj[current_idx:current_idx + N] - else: - x_ref = np.concatenate((x_traj[current_idx:], x_traj[:N - (len(x_traj) - current_idx)])) - y_ref = np.concatenate((y_traj[current_idx:], y_traj[:N - (len(x_traj) - current_idx)])) - theta_ref = np.concatenate((theta_traj[current_idx:], theta_traj[:N - (len(x_traj) - current_idx)])) - return x_ref, y_ref, theta_ref - -def get_straight_trajectory(x_init, y_init, distance=3000, total_points=1000): - x_traj = np.linspace(x_init + 5, x_init + distance, total_points) - y_traj = np.full(total_points, y_init + 3) - - v_ref = [V_REF for _ in range(total_points)] - - return x_traj, y_traj, v_ref - -def calculate_lateral_deviation(x, y, x_ref1, y_ref1, x_ref2, y_ref2): - num = (y_ref2 - y_ref1) * x - (x_ref2 - x_ref1) * y + x_ref2 * y_ref1 - y_ref2 * x_ref1 - denom = np.sqrt((y_ref2 - y_ref1) ** 2 + (x_ref2 - x_ref1) ** 2) - if denom > 0.01: - return num / denom - else: - return 0 - From 325cf477486d34d98db7bca110c5ca5c5106737c Mon Sep 17 00:00:00 2001 From: Liufu27 <2183667346@qq.com> Date: Wed, 17 Dec 2025 21:24:13 +0800 Subject: [PATCH 7/9] Delete src/Unmanned_vehicle_control/main_carla_simulator.py --- .../main_carla_simulator.py | 105 ------------------ 1 file changed, 105 deletions(-) delete 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 deleted file mode 100644 index 729af0edad..0000000000 --- a/src/Unmanned_vehicle_control/main_carla_simulator.py +++ /dev/null @@ -1,105 +0,0 @@ -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: - def __init__(self): - self.client = carla.Client('localhost', 2000) - self.client.set_timeout(10.0) - self.world = self.client.get_world() - self.ego_vehicle = None - for vehicle in self.world.get_actors().filter('*vehicle*'): - vehicle.destroy() - - def load_world(self, map_name): - 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): - 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 = 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.set_transform(spectator_transform) - - def clean(self): - for vehicle in self.world.get_actors().filter('*vehicle*'): - vehicle.destroy() - - 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, thickness=thickness, color=carla.Color(red, green, blue), life_time=life_time) - - def get_main_ego_vehicle_state(self): - transform = self.ego_vehicle.get_transform() - x = transform.location.x - y = transform.location.y - theta = np.deg2rad(transform.rotation.yaw) - 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): - 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): - if acceleration_m_s_2 == 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})") - - 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 64c7d7639573d031237abe19d6f1a1999d2591af Mon Sep 17 00:00:00 2001 From: Liufu27 <2183667346@qq.com> Date: Wed, 17 Dec 2025 21:45:56 +0800 Subject: [PATCH 8/9] main_help_functions.py --- .../main_help_functions.py | 158 ++++++++++++++++++ 1 file changed, 158 insertions(+) create mode 100644 src/Unmanned_vehicle_control/main_help_functions.py diff --git a/src/Unmanned_vehicle_control/main_help_functions.py b/src/Unmanned_vehicle_control/main_help_functions.py new file mode 100644 index 0000000000..1f42877a6c --- /dev/null +++ b/src/Unmanned_vehicle_control/main_help_functions.py @@ -0,0 +1,158 @@ +import numpy as np + +from src.config import N, V_REF, A +from src.config import CIRCLE_CENTER_X, CIRCLE_CENTER_Y, CIRCLE_RADIUS +def update_reference_point(x0, y0, current_idx, x_traj, y_traj, min_distance=5.0): + # 校验轨迹数组长度一致性与非空 + if len(x_traj) != len(y_traj) or len(x_traj) == 0: + raise ValueError("x_traj和y_traj长度必须一致且非空") + # 校验current_idx的合法性,若超出范围则修正为有效索引 + if current_idx < 0 or current_idx >= len(x_traj): + current_idx = 0 # 超出范围时重置为0索引 + distance_to_current_point = np.sqrt((x_traj[current_idx] - x0) ** 2 + (y_traj[current_idx] - y0) ** 2) + + if distance_to_current_point < min_distance: + current_idx = (current_idx + 1) % len(x_traj) + + return current_idx + +def get_eight_trajectory(x_init, y_init, total_points=100): + t = 2 * np.pi + + t_values = np.linspace(0, t, total_points) + x_traj = x_init + A * np.sin(t_values) + y_traj = y_init + A * np.sin(t_values) * np.cos(t_values) + + v_ref = [V_REF for _ in range(total_points)] + + cos_angle = np.cos(-np.pi / 4) + sin_angle = np.sin(-np.pi / 4) + + x_traj_rotated = x_init + cos_angle * (x_traj - x_init) - sin_angle * (y_traj - y_init) + y_traj_rotated = y_init + sin_angle * (x_traj - x_init) + cos_angle * (y_traj - y_init) + + theta_ref = [] + for i in range(total_points - 1): + dx = x_traj_rotated[i + 1] - x_traj_rotated[i] + dy = y_traj_rotated[i + 1] - y_traj_rotated[i] + theta = np.arctan2(dy, dx) + theta_ref.append(theta) + + theta_ref.append(theta_ref[-1]) + + return x_traj_rotated, y_traj_rotated, v_ref, theta_ref +def get_circle_trajectory( + x_center=CIRCLE_CENTER_X, # 从config.py读取默认圆心X + y_center=CIRCLE_CENTER_Y, # 从config.py读取默认圆心Y + # radius=CIRCLE_RADIUS, # 从config.py读取默认半径 + radius=20, # 手动调节可适用无错误的半径 + total_points=200 +): + """生成圆形轨迹(默认参数从配置文件读取)""" + t_values = np.linspace(0, 2 * np.pi, total_points) + x_traj = x_center + radius * np.cos(t_values) + y_traj = y_center + radius * np.sin(t_values) + + 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_spiral_trajectory(x_init, y_init, total_points=200, turns=2, scale=2): + """生成螺旋线轨迹""" + t_values = np.linspace(0, 2 * np.pi * turns, total_points) + # 极坐标方程:r = scale * t(半径随角度线性增加) + r = scale * t_values + x_traj = x_init + r * np.cos(t_values) + y_traj = y_init + r * np.sin(t_values) + 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_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] + y_ref = y_traj[current_idx:current_idx + N] + theta_ref = theta_traj[current_idx:current_idx + N] + else: + x_ref = np.concatenate((x_traj[current_idx:], x_traj[:N - (len(x_traj) - current_idx)])) + y_ref = np.concatenate((y_traj[current_idx:], y_traj[:N - (len(x_traj) - current_idx)])) + theta_ref = np.concatenate((theta_traj[current_idx:], theta_traj[:N - (len(x_traj) - current_idx)])) + return x_ref, y_ref, theta_ref + +def get_straight_trajectory(x_init, y_init, distance=3000, total_points=1000): + x_traj = np.linspace(x_init + 5, x_init + distance, total_points) + y_traj = np.full(total_points, y_init + 3) + + v_ref = [V_REF for _ in range(total_points)] + + return x_traj, y_traj, v_ref + +def calculate_lateral_deviation(x, y, x_ref1, y_ref1, x_ref2, y_ref2): + num = (y_ref2 - y_ref1) * x - (x_ref2 - x_ref1) * y + x_ref2 * y_ref1 - y_ref2 * x_ref1 + denom = np.sqrt((y_ref2 - y_ref1) ** 2 + (x_ref2 - x_ref1) ** 2) + if denom > 0.01: + return num / denom + else: + return 0 \ No newline at end of file From 2fbd76a5224bf6ad6f408ee3de2fa8eb8a9811c0 Mon Sep 17 00:00:00 2001 From: Liufu27 <2183667346@qq.com> Date: Wed, 17 Dec 2025 22:29:42 +0800 Subject: [PATCH 9/9] =?UTF-8?q?=E6=B7=BB=E5=8A=A0MPC=E6=96=87=E4=BB=B6?= =?UTF-8?q?=EF=BC=8C=E5=B9=B6=E4=BC=98=E5=8C=96=E6=B1=82=E8=A7=A3=E5=99=A8?= =?UTF-8?q?=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 | 108 ++++++++++++++++++ 1 file changed, 108 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..b5fa75d2f3 --- /dev/null +++ b/src/Unmanned_vehicle_control/main_mpc_controller.py @@ -0,0 +1,108 @@ +import casadi as ca + +from src.main_help_functions import calculate_lateral_deviation +from src.config import MAX_CONTROL_WHEEL_ANGLE_RAD, MAX_CONTROL_ACCELERATION_M_S_2, MAX_CONTROL_BRAKING_M_S_2, \ + PATH_TOLERANCE_M, FINE_X_COEF, FINE_Y_COEF, FINE_V_COEF, FINE_STEER_COEF, FINE_ACC_COEF, FINE_STEER_DOT_COEF, \ + FINE_ACC_DOT_COEF, FINE_THETA_COEF, FINE_LATERAL_COEF +from src.main_vehicle_model import vehicle_model + +class MpcController: + def __init__(self, horizon = 10, dt = 0.1): + self.horizon = horizon + self.opti = None + self.dt = dt + self.sol = None + self.cost = None + self.is_success = False + self.X = None + self.Y = None + self.control_buffer = {"acceleration": [0] * self.horizon, "wheel_angle": [0] * self.horizon} + self.buffer_index = 0 + + def reset_solver(self): + self.opti = ca.Opti() + opts = { + 'ipopt.print_level': 0, + 'print_time': 0, + 'ipopt.max_cpu_time': self.dt, # 严格限制求解时间不超过控制周期 + 'ipopt.max_iter': 50, # 减少最大迭代次数(默认300,可根据精度需求降低) + 'ipopt.tol': 1e-3, # 放宽收敛精度(默认1e-8,实时控制可放宽) + 'ipopt.warm_start_init_point': 'yes', # 启用热启动,加速收敛 + 'ipopt.warm_start_bound_push': 1e-6, + 'ipopt.warm_start_slack_bound_push': 1e-6, + 'ipopt.warm_start_mult_bound_push': 1e-6 + } + self.opti.solver('ipopt', opts) + + self.X = self.opti.variable(4, self.horizon + 1) # State: (x, y, theta, v) + self.U = self.opti.variable(2, self.horizon) # Control (delta, a) + + for k in range(self.horizon): + self.opti.subject_to(self.X[:, k + 1] == vehicle_model(self.X[:, k], self.U[:, k], self.dt)) + self.opti.subject_to(self.U[0, k] <= MAX_CONTROL_WHEEL_ANGLE_RAD) + self.opti.subject_to(self.U[0, k] >= -MAX_CONTROL_WHEEL_ANGLE_RAD) + self.opti.subject_to(self.U[1, k] <= MAX_CONTROL_ACCELERATION_M_S_2) + self.opti.subject_to(self.U[1, k] >= MAX_CONTROL_BRAKING_M_S_2) + + def set_init_vehicle_state(self, x, y, theta, v): + self.opti.subject_to(self.X[:, 0] == ca.vertcat(x, y, theta, v)) + + def update_cost_function(self, x_ref, y_ref, theta_ref, v_ref): + self.cost = 0 + for k in range(self.horizon): + fine_x = ca.if_else(ca.fabs(self.X[0, k] - x_ref[k]) > PATH_TOLERANCE_M, + FINE_X_COEF * (self.X[0, k] - x_ref[k]) ** 2, 0) + fine_y = ca.if_else(ca.fabs(self.X[1, k] - y_ref[k]) > PATH_TOLERANCE_M, + FINE_Y_COEF * (self.X[1, k] - y_ref[k]) ** 2, 0) + fine_v = FINE_V_COEF * (self.X[3, k] - v_ref[k]) ** 2 + fine_steer = FINE_STEER_COEF * self.U[0, k] ** 2 + fine_acc = FINE_ACC_COEF * self.U[1, k] ** 2 + + fine_theta = FINE_THETA_COEF * (self.X[2, k] - theta_ref[k]) ** 2 + + if k < self.horizon - 1: + lateral_deviation = calculate_lateral_deviation(self.X[0, k], self.X[1, k], x_ref[k], y_ref[k], x_ref[k + 1], y_ref[k + 1]) + else: + lateral_deviation = 0 + + fine_lateral = FINE_LATERAL_COEF * lateral_deviation ** 2 + + fine_steer_dot = 0 + fine_acc_dot = 0 + if k > 0: + fine_steer_dot += FINE_STEER_DOT_COEF * (self.U[0, k] - self.U[0, k - 1]) ** 2 + fine_acc_dot += FINE_ACC_DOT_COEF * (self.U[1, k] - self.U[1, k - 1]) ** 2 + + self.cost += fine_x + fine_y + fine_v + fine_steer + fine_acc + fine_steer_dot + fine_acc_dot + fine_theta + fine_lateral + + self.opti.minimize(self.cost) + + def solve(self): + try: + self.sol = self.opti.solve() + self.is_success = True + + wheel_angle_rad, acceleration_m_s_2 = self.get_controls_value() + self.control_buffer["acceleration"] = self.control_buffer["acceleration"][1:] + [acceleration_m_s_2] + self.control_buffer["wheel_angle"] = self.control_buffer["wheel_angle"][1:] + [wheel_angle_rad] + self.buffer_index = 0 + except Exception: + print("Error or delay upon MPC solution calculation. Previous calculated control value will be used") + self.is_success = False + + def get_controls_value(self): + if self.is_success: + wheel_angle_rad = self.sol.value(self.U[0, 0]) + acceleration_m_s_2 = self.sol.value(self.U[1, 0]) + else: + if self.buffer_index < self.horizon: + acceleration_m_s_2 = self.control_buffer["acceleration"][self.buffer_index] + wheel_angle_rad = self.control_buffer["wheel_angle"][self.buffer_index] + self.buffer_index += 1 + else: + raise Exception("Control buffer is empty.") + + return wheel_angle_rad, acceleration_m_s_2 + + def get_optimized_cost(self): + return self.sol.value(self.cost) \ No newline at end of file