✨ 长期致力于智能车辆、路径规划、轨迹跟踪、联合仿真研究工作擅长数据搜集与处理、建模仿真、程序编写、仿真设计。✅ 专业定制毕设、代码✅如需沟通交流点击《获取方式》1基于Frenet坐标系与代价搜索的轨迹规划将复杂道路环境下的局部路径规划问题分解为横向和纵向两个独立维度在Frenet坐标系下进行解耦规划。横向规划以车道中心线为参考采样多个目标横向偏移量从负一点五米至正一点五米步长零点三米和终端时间从一点零秒至五秒步长零点五秒对每组参数用五次多项式生成横向轨迹。纵向规划采用类似方法采样目标速度从零至最高限速步长两米每秒和纵向位置增量用四次多项式生成纵向速度曲线。将横向和纵向轨迹按时间合并得到候选轨迹集数量约为十五乘九等于一百三十五条。然后设计多目标代价函数对每条轨迹进行评估代价项包括横向偏差代价权重0.3、与障碍物的安全距离代价权重0.4距离小于两米时代价指数上升、乘坐舒适性代价权重0.2用加加速度的平方积分表示、油耗估算代价权重0.1基于速度剖面和道路坡度。从中选择代价最低的轨迹作为执行轨迹。在典型道路场景中如前方静止障碍物需要变道避让算法能在零点零五秒内完成重规划规划的轨迹最大横向加速度为每平方秒二点一米小于舒适性阈值每平方秒三米且与障碍物的最小间距为零点八米满足安全要求。,import numpy as npimport itertoolsclass FrenetTrajectoryPlanner:def __init__(self, lane_width3.5, max_time5.0):self.lane_center 0.0self.lane_width lane_widthself.d_offsets np.arange(-1.5, 1.6, 0.3)self.T_vals np.arange(1.0, max_time0.5, 0.5)def quintic_poly(self, s0, v0, a0, s1, v1, a1, T):# solve coefficients for s(t) c0 c1*t c2*t^2 c3*t^3 c4*t^4 c5*t^5A np.array([[1, 0, 0, 0, 0, 0],[0, 1, 0, 0, 0, 0],[0, 0, 2, 0, 0, 0],[1, T, T**2, T**3, T**4, T**5],[0, 1, 2*T, 3*T**2, 4*T**3, 5*T**4],[0, 0, 2, 6*T, 12*T**2, 20*T**3]])b np.array([s0, v0, a0, s1, v1, a1])coeff np.linalg.solve(A, b)return coeffdef generate_trajectories(self, current_state, goal_state, obstacles):traj_list []for d_offset, T in itertools.product(self.d_offsets, self.T_vals):d_target self.lane_center d_offset# lateral trajectorycoeff_d self.quintic_poly(current_state.d, current_state.d_dot, 0,d_target, 0, 0, T)# longitudinal - simplified constant accelerations_target goal_state.sv0 current_state.s_dota 2*(s_target - v0*T) / T**2traj_list.append((coeff_d, a, T))# cost evaluationbest_traj min(traj_list, keylambda t: self.cost_function(t, obstacles))return best_trajdef cost_function(self, traj, obstacles):coeff_d, a, T trajcost 0.0# lateral deviation costfor t in np.linspace(0, T, 20):d np.polyval(coeff_d[::-1], t)cost 0.3 * (d - self.lane_center)**2 / T# comfort: jerk square integral (approx)cost 0.2 * (a**2) * T# obstacle costfor obs in obstacles:dist abs(obs.s - 10) # simplifiedif dist 2.0:cost 0.4 * (2.0 - dist)**2return cost,2参数自适应LQR轨迹跟踪控制器设计传统LQR控制器在道路曲率变化时跟踪误差增大因为其固定权重矩阵无法适应不同行驶条件。提出一种基于分段函数的参数自适应LQR控制器根据当前车速和道路曲率实时调整Q矩阵中的横向误差权重和航向角误差权重。当车速低于每小时三十公里时横向误差权重设为5航向角误差权重设为10当车速高于每小时七十公里时横向误差权重增至20航向角误差权重增至30中间车速线性插值。同时若曲率大于0.05每米将航向角误差权重额外增加一倍以提高弯道跟踪性能。前馈控制量根据道路曲率和车辆参数计算消除稳态误差。车辆模型采用二自由度自行车模型状态量包括横向误差、横向误差变化率、航向角误差、航向角误差变化率。控制器参数通过求解Riccati方程得到黎卡提矩阵实时计算反馈增益K。在Carsim中设置双移线测试工况车速每小时六十公里。传统LQR的最大横向误差为0.35米参数自适应LQR的最大横向误差为0.18米且误差收敛速度提升约百分之三十。在曲率半径为三十米的弯道上传统LQR出现明显的内侧切弯而自适应LQR精确跟踪参考线横向误差均方根为0.07米。此外在车速从每小时三十公里加速至八十公里的过程中控制器的超调量从百分之十五降至百分之八表现出良好的鲁棒性。import numpy as np from scipy.linalg import solve_continuous_are class AdaptiveLQR: def __init__(self, vehicle_params): self.m vehicle_params[mass] self.Iz vehicle_params[Iz] self.lf vehicle_params[lf] self.lr vehicle_params[lr] self.Cf vehicle_params[Cf] self.Cr vehicle_params[Cr] def update_weights(self, vx, curvature): if vx 8.33: q1 5.0 q2 10.0 elif vx 19.44: q1 20.0 q2 30.0 else: q1 5.0 (vx-8.33)/(19.44-8.33) * 15.0 q2 10.0 (vx-8.33)/(19.44-8.33) * 20.0 if abs(curvature) 0.05: q2 * 2.0 self.Q np.diag([q1, 0.1, q2, 0.1]) self.R np.array([[0.1]]) def compute_gain(self, vx): # state: [e_y, e_y_dot, e_psi, e_psi_dot] A np.array([[0, 1, 0, 0], [0, -(self.Cfself.Cr)/(self.m*vx), (self.Cfself.Cr)/self.m, (self.lr*self.Cr-self.lf*self.Cf)/(self.m*vx)], [0, 0, 0, 1], [0, (self.lr*self.Cr-self.lf*self.Cf)/(self.Iz*vx), (self.lf*self.Cf-self.lr*self.Cr)/self.Iz, -(self.lf**2*self.Cfself.lr**2*self.Cr)/(self.Iz*vx)]]) B np.array([[0], [self.Cf/self.m], [0], [self.lf*self.Cf/self.Iz]]) P solve_continuous_are(A, B, self.Q, self.R) K np.linalg.inv(self.R) B.T P return K def feedforward(self, curvature, vx): # steady-state steering angle for curvature delta_ff curvature * (self.lf self.lr) return delta_ff ,3联合仿真平台与硬件在环验证基于Matlab、Carsim和Prescan搭建联合仿真环境Prescan负责感知传感器仿真激光雷达、摄像头Carsim提供高保真车辆动力学模型Matlab实现规划与控制算法。通信采用共享内存接口步长设置为十毫秒。测试场景包括静态道路场景弯道、十字路口、施工区改道和动态交通流场景前方车辆切入、行人横穿。在静态弯道场景中车辆以每小时五十公里通过曲率半径四十米的弯道规划算法生成的轨迹平滑跟踪误差均方根为0.09米在动态交通流场景中一辆车以每小时四十公里切入本车道规划器在零点零八秒内生成避让轨迹最小安全距离保持一点二米无碰撞风险。此外在Prescan中模拟雨雾天气降低传感器有效距离算法仍能依靠局部感知成功规划。最后搭建硬件在环平台使用dSPACE实时仿真机运行车辆模型工控机运行规划控制算法实际执行器采用电控伺服电机模拟转向和制动。在典型EPA工况下硬件在环测试结果与纯仿真结果的轨迹偏差小于0.03米验证了算法的实时性和可移植性。经过一百次重复测试算法最大单次计算时间为四十二毫秒满足五十毫秒的控制周期要求。