✨ 长期致力于自动驾驶技术、轨迹规划、模型预测控制、避障模型、实车验证研究工作擅长数据搜集与处理、建模仿真、程序编写、仿真设计。✅ 专业定制毕设、代码✅如需沟通交流点击《获取方式》1参考路径平滑算法与Frenet坐标系转换针对传感器采集的参考路径离散点噪声大、曲率不连续的问题设计基于二次规划的路径平滑算法。首先将路径点从笛卡尔坐标系转换到Frenet坐标系以路径弧长s为自变量定义横向偏移l(s)和纵向位置。构造代价函数包括平滑项二阶导数平方积分、接近项偏离原始点的距离平方和紧凑项路径长度惩罚转化为二次规划问题求解。约束条件为曲率不超过0.2 rad/m路径点横向偏差不超过0.3m。使用OSQP求解器在MATLAB中实现对一段含噪声的80米参考路径进行平滑平滑后路径的曲率最大值从0.35降至0.19曲率突变点消除。同时推导Frenet与笛卡尔坐标系的转换公式用于后续轨迹规划的投影计算。对比平滑前后的路径跟踪仿真平滑后车辆跟踪的横向偏差标准差从0.08m降至0.03m。2基于融合动力学模型的模型预测轨迹跟踪控制建立车辆二自由度动力学模型状态为横向速度、横摆角速度、横向位移、横摆角和二自由度运动学模型将两者融合在高速时增加动力学权重。预测时域Np15控制时域Nc5采样周期0.05s。状态方程为线性时变系统在每个采样点线性化。目标函数包含横向偏差、航向偏差、控制量增量以及终端代价。约束条件为前轮转角±0.5rad转角增量±0.1rad。将问题转化为二次规划求解。在MATLAB中与纯跟踪控制PP和基于运动学的MPC对比。双移线工况车速72km/h所提融合MPC的最大横向偏差0.12m基于运动学MPC为0.21mPP为0.35m。在低附路面μ0.4上融合MPC仍能稳定跟踪而运动学MPC出现轻微摆动。3基于三元素避障模型的轨迹规划与实车验证针对动态障碍物场景提出三元素改进避障模型包括自车与碰撞点距离、动态障碍物与碰撞点距离、相对速度。在Frenet坐标系下进行轨迹采样每条轨迹由纵向位移s(t)和横向位移l(s)五次多项式表示采样空间包括不同的终点时间2-5秒和终点横向偏移-3m至3m。每条轨迹的代价函数包含三元素避障代价、平顺性代价和偏差代价使用非线性规划求解最优轨迹。在仿真中设置两个场景场景1为前方车辆以36km/h切入自车道场景2为相邻车道同时有两辆障碍物。算法在场景1中规划出平顺换道轨迹最小安全距离2.5m场景2中决策为减速跟车最大减速度2.5m/s²。实车验证基于赛力斯SF5平台搭载ROSSimulink联合软件。在封闭道路中设置锥桶模拟障碍物车速36km/h车辆成功避让并返回原车道轨迹跟踪误差平均0.08m。实车试验10次全部成功验证了算法的可行性和实用性。import numpy as np import cvxpy as cp from scipy.interpolate import CubicSpline class PathSmoothingQP: def __init__(self, ref_points): self.ref_points ref_points self.n len(ref_points) def smooth(self, lambda_smooth1.0, lambda_close0.1): # 二次规划平滑 x cp.Variable(self.n) y cp.Variable(self.n) # 平滑代价二阶差分平方和 smooth_cost 0 for i in range(1, self.n-1): smooth_cost (x[i-1] - 2*x[i] x[i1])**2 (y[i-1] - 2*y[i] y[i1])**2 # 接近代价 close_cost cp.sum_squares(x - self.ref_points[:,0]) cp.sum_squares(y - self.ref_points[:,1]) objective cp.Minimize(lambda_smooth * smooth_cost lambda_close * close_cost) prob cp.Problem(objective) prob.solve(solvercp.OSQP) return np.vstack([x.value, y.value]).T class DynamicMPCTracker: def __init__(self, dt0.05, Np15, Nc5): self.dt dt self.Np Np self.Nc Nc def linearize_dynamics(self, x, u): # 线性化车辆动力学 A np.array([[1, dt, 0, 0], [0, 1, 0, 0], [0, dt, 1, 0], [0, 0, dt, 1]]) B np.array([[0], [dt], [0], [0]]) return A, B def solve_mpc(self, x0, ref_trajectory): n_states 4 n_controls 1 u cp.Variable((n_controls, self.Nc)) x cp.Variable((n_states, self.Np1)) cost 0 constraints [x[:,0] x0] for t in range(self.Np): A, B self.linearize_dynamics(None, None) if t self.Nc: constraints [x[:,t1] A x[:,t] B.flatten() * u[0,t]] cost cp.sum_squares(x[:2,t] - ref_trajectory[t]) cost 0.1 * cp.sum_squares(u[:,t]) else: constraints [x[:,t1] A x[:,t] B.flatten() * u[0,self.Nc-1]] cost cp.sum_squares(x[:2,t] - ref_trajectory[t]) problem cp.Problem(cp.Minimize(cost), constraints) problem.solve(solvercp.OSQP) return u[:,0].value[0] if u.value is not None else 0 def three_element_obstacle_cost(self, ego_pos, obs_pos, obs_vel, collision_point): d1 np.linalg.norm(ego_pos - collision_point) d2 np.linalg.norm(obs_pos - collision_point) v_rel np.dot(obs_vel, (collision_point-obs_pos)/max(d2,0.1)) cost 10/d1 5/d2 2/max(0.1, v_rel) return cost