Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
27 commits
Select commit Hold shift + click to select a range
552d9bd
在main_help_functions.py中添加一个新的轨迹“圆形轨迹”
Liufu27 Dec 8, 2025
e5cafb3
Merge branch 'OpenHUTB:main' into main
Liufu27 Dec 8, 2025
aa68609
Merge branch 'OpenHUTB:main' into main
Liufu27 Dec 15, 2025
f58f3b6
Merge branch 'OpenHUTB:main' into main
Liufu27 Dec 15, 2025
65a80e0
Merge branch 'OpenHUTB:main' into main
Liufu27 Dec 15, 2025
ffe1cbf
更新main,主函数新添加调用圆形轨迹,并运行
Liufu27 Dec 15, 2025
ce595ee
Merge branch 'OpenHUTB:main' into main
Liufu27 Dec 16, 2025
7980325
生成了新的螺旋线轨迹,并且轨迹不超出场地范围
Liufu27 Dec 16, 2025
1c83fe7
Merge branch 'OpenHUTB:main' into main
Liufu27 Dec 16, 2025
25d4068
Merge branch 'OpenHUTB:main' into main
Liufu27 Dec 16, 2025
a04deab
更新main.py,并调用螺旋轨迹,并运行
Liufu27 Dec 16, 2025
27d8907
Merge branch 'OpenHUTB:main' into main
Liufu27 Dec 16, 2025
c3e374b
Merge branch 'OpenHUTB:main' into main
Liufu27 Dec 16, 2025
a645213
提交启动carla文件,初始化地图场景等
Liufu27 Dec 16, 2025
8b09ddf
Merge branch 'OpenHUTB:main' into main
Liufu27 Dec 16, 2025
de30406
Merge branch 'OpenHUTB:main' into main
Liufu27 Dec 16, 2025
77ff2c2
Delete src/Unmanned_vehicle_control/main_help_functions.py
Liufu27 Dec 16, 2025
46ae829
Merge branch 'OpenHUTB:main' into main
Liufu27 Dec 17, 2025
325cf47
Delete src/Unmanned_vehicle_control/main_carla_simulator.py
Liufu27 Dec 17, 2025
7fbb8dc
Merge branch 'OpenHUTB:main' into main
Liufu27 Dec 17, 2025
8e36a7f
Merge branch 'OpenHUTB:main' into main
Liufu27 Dec 17, 2025
0575a1a
Merge branch 'OpenHUTB:main' into main
Liufu27 Dec 17, 2025
64c7d76
main_help_functions.py
Liufu27 Dec 17, 2025
53f7f6f
Merge branch 'OpenHUTB:main' into main
Liufu27 Dec 17, 2025
85c7c27
Merge branch 'OpenHUTB:main' into main
Liufu27 Dec 17, 2025
731d594
Merge branch 'OpenHUTB:main' into main
Liufu27 Dec 17, 2025
2fbd76a
添加MPC文件,并优化求解器参数
Liufu27 Dec 17, 2025
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
49 changes: 47 additions & 2 deletions src/Unmanned_vehicle_control/main_help_functions.py
Original file line number Diff line number Diff line change
Expand Up @@ -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]
Expand All @@ -109,5 +155,4 @@ def calculate_lateral_deviation(x, y, x_ref1, y_ref1, x_ref2, y_ref2):
if denom > 0.01:
return num / denom
else:
return 0

return 0
108 changes: 108 additions & 0 deletions src/Unmanned_vehicle_control/main_mpc_controller.py
Original file line number Diff line number Diff line change
@@ -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)