前车轨迹预测自动驾驶规划与MPC跟踪【附仿真】

✨ 长期致力于自动驾驶汽车、前车轨迹预测、轨迹规划、轨迹跟踪、变预测时域双反馈MPC研究工作擅长数据搜集与处理、建模仿真、程序编写、仿真设计。✅ 专业定制毕设、代码✅如需沟通交流点击《获取方式》1LSTM示教学习混合轨迹预测模型构建编码器-解码器结构编码器采用两层LSTM隐藏状态维度256输入序列长度为30帧0.1秒/帧每条轨迹包含坐标、速度、加速度和航向角共6维。解码器同样双层LSTM输出未来50帧5秒的轨迹分布高斯混合模型参数。在标准序列训练方式基础上融入示教学习率α即模型输出的动作空间保留α比例来自教师网络的指导α从初始0.9线性衰减至训练后期的0.05。教师网络是一个预训练的生成对抗网络轨迹预测器用于在训练早期提供稳定的梯度。训练数据来自NGSIM US-101和I-80数据集以及自建城市快速路数据集共12万条轨迹片段。测试集上均方根误差5秒预测时长下为0.64米比纯LSTM Encoder-Decoder降低23.6%。特别在弯道和换道场景中示教学习机制使模型更快速地收敛到合理的多模态分布多模态轨迹预测的minADE最小平均位移误差为0.42米。2前车预测嵌入的SLST轨迹规划将规划解耦为路径规划和速度规划。路径规划在SL图中进行纵向范围取0至120米横向范围为当前车道及左右邻接车道。构建五次多项式路径簇每条路径由终点横向偏移d1和纵向长度s1参数化参数组合产生约180条候选路径。代价函数包含路径长度、最大曲率、与障碍物距离含前车预测轨迹投影等项动态规划采样步长5米。速度规划在ST图中上下边界由前车预测轨迹的速度剖面和位置确定确保安全跟随。同样用DPQP两步法QP优化目标包含速度平滑性和可达性。预测的前车轨迹作为时变障碍物投射到ST图中使自车规划能提前应对前车的突然减速或变道。仿真中当预测到前车在2.0秒后将急剧减速至0自车规划的速度曲线提前0.8秒开始平滑减速避免了紧急制动最大减速度控制在-3.2米/秒²以内比未引入预测的规划峰值减速度-6.5米/秒²显著降低。3变预测时域双反馈MPC与双PID纵向控制横向控制器采用双反馈模型预测控制即在标准状态空间中加入横向偏差的积分和航向偏差的积分作为扩展状态从而构建带有积分作用的MPC。预测模型基于运动学模型控制增量作为优化变量代价函数不仅惩罚状态偏差还惩罚积分项。预测时域Np通过模糊推理根据车速和道路曲率在线调整规则库包含9条规则Np在15至30之间动态变化。双反馈机制使稳态横向偏差趋零有效抵抗侧风等持续扰动。纵向控制采用速度-位置双PID串级结构外环位置PID输出速度指令内环速度PID输出油门/制动指令油门-制动标定表通过实车试验标定。硬件在环平台测试包含自动驾驶控制器dSPACE与Carsim车辆模型。在包含弯道、直道和前方车辆切入的复合场景中横向跟踪最大偏差0.08米纵向速度跟踪均方根误差0.12米/秒在预测信息帮助下换道时机选择合理乘员感受到的纵向加速度变化平顺表明预测-规划-控制链条的有效性。import torch import torch.nn as nn class TrajPredictorLSTM(nn.Module): def __init__(self, input_dim6, hidden256, output_frames50): super().__init__() self.encoder nn.LSTM(input_dim, hidden, 2, batch_firstTrue) self.decoder nn.LSTM(hidden, hidden, 2, batch_firstTrue) self.fc nn.Linear(hidden, output_frames*5) # 输出高斯参数 def forward(self, x, teacher_forcingNone, alpha0.5): _, (h, c) self.encoder(x) # 使用教师信号混合 if teacher_forcing is not None: h alpha * teacher_forcing (1-alpha) * h # 解码过程 dec_input torch.zeros(x.size(0), 1, self.encoder.hidden_size) out, _ self.decoder(dec_input, (h, c)) return self.fc(out) # 双反馈MPC扩展状态 def extended_state_mpc(x, y, theta, v, e_lat, e_psi, integral_lat, integral_psi): # 构造扩展状态向量 X np.hstack([x, y, theta, v, e_lat, e_psi, integral_lat, integral_psi]) # MPC预测矩阵A, B根据运动学模型和误差动力学构建 A np.eye(8) B np.zeros((8,2)) # ... return A, B def fuzzy_Np_adjust(speed, curvature): if speed 8.0: Np 20 elif speed 15.0: Np 25 else: Np 30 if curvature 0.06: Np - 5 return max(15, Np) def dual_pid_control(pos_err, vel_err, dt, integral, prev_err): # 外环位置PID Kp_pos, Ki_pos, Kd_pos 1.2, 0.02, 0.3 vel_des Kp_pos*pos_err Ki_pos*integral[pos] Kd_pos*(pos_err-prev_err[pos])/dt # 内环速度PID Kp_vel, Ki_vel, Kd_vel 3.5, 0.1, 0.5 vel_ctrl Kp_vel*(vel_des-vel_err) Ki_vel*integral[vel] Kd_vel*(vel_err-prev_err[vel])/dt return vel_ctrl