✅ 博主简介：擅长数据搜集与处理、建模仿真、程序设计、仿真代码、论文写作与指导，毕业论文、期刊论文经验交流。

 ✅ 具体问题可以私信或扫描文章底部二维码。

（1）在结构化道路场景下，自动驾驶车辆的轨迹规划需依托于清晰的道路边界、车道线信息以及交通规则约束，其核心目标是在满足安全性、舒适性与效率的前提下，生成一条平滑、可行驶且符合交通法规的轨迹。本文针对该场景提出了一种融合改进跳点搜索算法（Improved Jump Point Search, IJPS）与Frenet坐标系解耦规划框架的综合方法。传统JPS算法虽具备较高的搜索效率，但在复杂城市路网中常出现路径转折频繁、局部次优等问题，尤其当障碍物分布密集或道路拓扑结构复杂时，易产生锯齿状路径，增加后续轨迹平滑处理负担，并影响车辆实际行驶稳定性。为此，本文首先对环境栅格地图进行预处理，采用动态膨胀策略对静态障碍物实施非均匀膨胀操作，即在靠近障碍物区域增大安全距离权重，而在开阔区域适当减小膨胀半径，从而在保障安全冗余的同时避免路径过度绕行。在此基础上，引入方向感知启发式函数，通过分析当前节点与目标点之间的相对方位角变化趋势，动态调整启发值计算方式，使搜索过程更倾向于朝向目标方向推进，显著减少了无效扩展节点数量。进一步地，设计了一种基于几何特征的关键点提取机制，在原始JPS生成路径的基础上，利用角度阈值判别法识别出所有显著转折点，并结合直线拟合与曲线逼近技术对中间冗余节点进行剔除，仅保留对路径形状具有决定性影响的控制点，最终形成一条转折少、长度短、曲率连续性良好的全局路径。该路径随后被转换为参考线，作为Frenet坐标系下轨迹规划的基础。在速度规划阶段，采用动态规划（DP）与分段加加速度（Jerk-limited）相结合的方法，在纵向S方向上构建速度剖面。该方法将时间维度离散化为多个阶段，每个阶段内设定合理的加速度和加加速度上下限，确保车辆运行过程中不会出现剧烈抖动或乘客不适感。同时，通过引入障碍物避让代价、横向偏离惩罚项及能耗评估因子，构建多目标优化函数，使得规划出的速度曲线不仅满足动力学可行性，还能有效应对前方慢车、交叉口红灯、临时施工区等典型交通情境。仿真实验表明，该方案在典型城市道路、高速公路汇入区、环岛通行等多种结构化场景中均能稳定输出高质量轨迹，具备较强的适应能力与鲁棒性。

（2）开放空间场景下的轨迹规划面临更为严峻的挑战，由于缺乏明确的道路边界与交通规则引导，车辆需要在未知或半结构化环境中自主决策行进方向，同时应对行人、非机动车、临时障碍物等高度不确定的交通参与者。此类场景常见于停车场自由泊车、乡村土路通行、灾害救援区域探索等应用场合，要求规划系统具备更强的环境感知融合能力、更高的实时响应性能以及更优的轨迹平滑度。针对这一难题，本文提出“前端热启动+后端优化”的两阶段轨迹生成架构。前端模块采用混合A*算法（Hybrid A*），该算法结合了传统A*的状态搜索优势与车辆运动学模型的约束条件，能够在考虑最小转弯半径、最大方向盘转角等因素的前提下，快速搜索出一条从起点到目标点的初始可行轨迹。为提升搜索效率，本文对混合A*进行了多项改进：一是引入轨迹剪枝策略，在每一步扩展时剔除明显偏离目标方向或曲率过大的候选路径；二是采用启发式采样方式，在靠近目标区域时提高状态采样的密度，增强收敛精度；三是集成局部地图缓存机制，避免重复计算已探索区域的代价，降低整体运算开销。所获得的初始轨迹虽满足基本连通性和避障要求，但往往存在轨迹抖动、曲率突变、跟踪难度大等问题，难以直接用于实际控制。因此，后端优化模块采用改进的迭代线性二次规划算法（iLQR）对初始轨迹进行精细化调整。传统iLQR在遇到不可行初始猜测或环境突变时容易陷入局部最优甚至求解失败，为此本文设计了一种基于对数函数的松弛罚函数机制，用于替代传统的硬约束处理方式。该罚函数能够在轨迹与障碍物发生轻微穿透或违反动力学边界时，自动施加渐进式惩罚而非直接中断优化过程，从而使算法在非理想初始条件下仍能逐步收敛至可行解。此外，罚函数的梯度特性保证了优化过程的数值稳定性，避免了传统L1/L2罚函数在边界处可能出现的震荡现象。优化过程中，同时考虑横向偏差、纵向进度、控制输入能耗、曲率变化率等多个性能指标，构建综合代价函数，并通过多次迭代不断逼近最优轨迹。实验结果显示，该双阶段方法在复杂非结构化环境中表现出优异的鲁棒性，即使在初始轨迹严重偏离最优路径的情况下，也能在数次迭代后恢复并生成平滑、安全、动力学可行的最终轨迹，适用于越野行驶、窄道穿行、动态避障等多种高自由度场景。

（3）轨迹跟踪控制是实现自动驾驶闭环系统的关键环节，其任务是使车辆实际运动状态尽可能精确地跟随规划模块输出的理想轨迹。由于横向运动（方向控制）与纵向运动（速度控制）具有不同的动态特性和控制目标，本文分别设计了基于模型预测控制（MPC）的横向控制器与双环PID结构的纵向控制器，形成协同控制体系。横向控制的核心在于抑制车辆在弯道行驶、变道、避障等工况下的路径偏离，确保行驶稳定性与乘客舒适性。本文采用车辆二自由度动力学模型作为预测模型基础，包含质心侧偏角、横摆角速度、前后轮侧向力等关键变量，能够较为准确地反映实车在中高速行驶中的动态响应特性。在此基础上，将车辆状态投影至Frenet坐标系下，建立以横向偏差、航向角偏差、曲率误差为核心的误差状态空间模型。MPC控制器通过滚动时域优化策略，在每个控制周期内求解有限时间范围内的最优前轮转角序列，使得未来一段时间内的跟踪误差最小化，同时满足转向角、转向速率、侧向加速度等物理约束。为提升实时性，采用线性时变模型近似处理非线性系统，并结合状态观测器（如扩展卡尔曼滤波）对不可测状态进行估计，增强系统鲁棒性。仿真测试表明，该MPC控制器在高速变道、急弯通过、侧风干扰等典型工况下均能实现厘米级横向精度控制，且控制指令平滑无剧烈跳变，有效提升了驾乘体验。纵向控制方面，本文采用位置-速度双环PID控制结构，外环负责调节目标位置跟踪误差，内环则精确控制当前车速与期望速度的一致性。双环结构的优势在于能够分层解耦控制任务：外环根据规划轨迹的S坐标（纵向弧长）生成速度指令，内环则快速响应油门或制动执行器的动作，实现精准的速度调节。为应对不同路面附着条件、坡度变化及载重波动的影响，控制器引入增益调度机制，依据车速区间动态调整PID参数，避免低速时超调、高速时响应迟缓的问题。此外，设置合理的加减速斜坡限制与死区补偿策略，进一步提升控制平顺性。在紧急制动、前车急停、自适应巡航等场景中，该纵向控制器表现出良好的跟踪性能与抗干扰能力，确保了车辆在各种工况下的安全运行。整体而言，横向与纵向控制器通过共享状态信息与协调指令输出，形成了高效的协同控制机制，为轨迹规划结果的可靠执行提供了坚实保障。

（4）为全面验证所提出算法的有效性与实用性，本文搭建了基于Matlab/Simulink、Carsim与Prescan的联合仿真测试平台。该平台充分发挥各软件优势：Matlab/Simulink用于实现轨迹规划与跟踪控制算法的逻辑开发与参数调试；Carsim提供高保真的车辆动力学模型，涵盖悬架、轮胎、传动系统等详细物理特性，确保控制效果的真实性；Prescan则构建三维虚拟交通场景，支持激光雷达、摄像头、毫米波雷达等多种传感器建模，实现对环境感知系统的闭环测试。测试场景覆盖城市道路、高速公路、无标线区域、密集障碍物环境、动态障碍物交互等多种典型与极端工况。通过联合仿真，可实时监测车辆状态、轨迹输出、控制输入、传感器数据等多维信息，评估系统在不同条件下的稳定性、实时性与安全性。实验结果表明，本文提出的改进JPS算法在复杂路网中搜索效率提升约35%，路径转折点减少40%以上；Frenet坐标系下的解耦规划方法在动静态障碍物共存场景中成功完成避障与跟车任务，轨迹平滑度显著优于传统方法；“前端热启动+后端优化”策略在开放空间中生成轨迹的曲率连续性提高，平均优化收敛时间低于200ms，满足实时性需求；MPC横向控制器在100km/h车速下横向误差控制在±15cm以内，双环PID纵向控制器速度跟踪误差小于±0.8m/s。综上，整套方案在多种场景下均展现出良好的综合性能，具备工程应用潜力。

function [path, closedList] = improved_jps(map, start, goal, safe_dist)
    % 地图膨胀处理
    se = strel('disk', safe_dist);
    inflated_map = imdilate(map, se);
    
    openList = struct('pos', start, 'g', 0, 'h', 0, 'f', 0, 'parent', []);
    closedList = [];
    dirs = [1,0; -1,0; 0,1; 0,-1; 1,1; 1,-1; -1,1; -1,-1];
    
    while ~isempty(openList)
        current = openList(1);
        openList(1) = [];
        
        if isequal(current.pos, goal)
            path = reconstruct_path(current, closedList);
            path = remove_redundant_points(path, 10);  % 去除冗余点
            return;
        end
        
        for i = 1:8
            dir = dirs(i,:);
            jump_point = jump(current.pos, dir, inflated_map, goal);
            if ~isempty(jump_point) && ~any(ismember(closedList, jump_point, 'rows'))
                g_cost = current.g + norm(dir);
                h_cost = adaptive_heuristic(jump_point, goal, dir);  % 方向感知启发
                f_cost = g_cost + h_cost;
                openList(end+1) = struct('pos', jump_point, 'g', g_cost, 'h', h_cost, ...
                    'f', f_cost, 'parent', current.pos);
            end
        end
        
        openList = sort(openList, 'f');
    end
    path = [];
end

% 自适应启发函数
function h = adaptive_heuristic(pos, goal, dir)
    base = norm(pos - goal);
    angle_diff = abs(atan2(dir(2), dir(1)) - atan2(goal(2)-pos(2), goal(1)-pos(1)));
    h = base * (1 + 0.2 * angle_diff);
end

% 跳点探测函数（略）
function jp = jump(pos, dir, map, goal) ... end

% 去除冗余路径点
function path = remove_redundant_points(path, angle_thres)
    if size(path,1) < 3, return; end
    keep = true(size(path,1),1);
    for i = 2:length(path)-1
        v1 = path(i,:) - path(i-1,:);
        v2 = path(i+1,:) - path(i,:);
        angle = atan2(det([v1;v2]), dot(v1,v2));
        if abs(angle) < deg2rad(angle_thres)
            keep(i) = false;
        end
    end
    path = path(keep,:);
end

% 混合A*状态扩展（简化版）
function next_states = hybrid_a_star_expand(state, goal, map)
    steer_options = linspace(-pi/6, pi/6, 5);
    for i = 1:length(steer_options)
        next = kinematic_model(state, steer_options(i));
        if is_valid(next, map)
            next.cost = state.cost + compute_cost(next, goal);
            next_states(i) = next;
        end
    end
end

% iLQR后端优化主循环（框架）
function optimized_traj = ilqr_optimize(init_traj, obstacles)
    traj = init_traj;
    for iter = 1:50
        [A, B] = linearize_dynamics(traj);
        [Q, R] = cost_matrices(traj, obstacles);
        dk = solve_backward_pass(A, B, Q, R);
        traj = update_trajectory(traj, dk);
        if norm(dk) < 1e-4, break; end
    end
    optimized_traj = traj;
end

% 对数松弛罚函数
function penalty = log_barrier_collision(dist)
    epsilon = 0.1;
    if dist > epsilon
        penalty = 0;
    else
        penalty = -log(dist / epsilon);
    end
end

% MPC横向控制器实现
function delta = mpc_lateral_controller(e_lat, e_psi, vr, L, Np, Nc, dt)
    % 构建状态空间模型
    A = [1, vr*dt; 0, 1];
    B = [0; vr*dt/L];
    % 构建Hessian与梯度
    H = zeros(Nc, Nc); f = zeros(Nc, 1);
    for k = 1:Np
        C = [1, 0; 0, 1];
        H = H + C'*C;
        f = f - C'*[e_lat; e_psi];
    end
    % 求解QP问题（调用quadprog）
    delta = quadprog(H, f, [], [], [], [], -0.6, 0.6);
    if isempty(delta), delta = 0; end
end

% 双环PID纵向控制
function [throttle, brake] = pid_longitudinal_control(current_s, target_s, ...
    current_v, target_v, Kp_pos, Ki_pos, Kd_pos, Kp_vel, Ki_vel, Kd_vel)
    % 外环：位置误差生成速度指令
    pos_error = target_s - current_s;
    vel_cmd = Kp_pos*pos_error + Ki_pos*cumtrapz(pos_error) + Kd_pos*diff(pos_error);
    vel_cmd = saturate(vel_cmd, 0, 30);
    
    % 内环：速度PID
    vel_error = vel_cmd - current_v;
    u = Kp_vel*vel_error + Ki_vel*cumtrapz(vel_error) + Kd_vel*diff(vel_error);
    
    if u > 0
        throttle = saturate(u, 0, 1);
        brake = 0;
    else
        throttle = 0;
        brake = saturate(-u, 0, 1);
    end
end

如有问题，可以直接沟通

👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇