【基于全局路径生成最优路径】基于D* Lite和横向避障算法无人驾驶地面车辆的路径规划(Matlab代码实现）

💥💥💞💞欢迎来到本博客❤️❤️💥💥
🏆博主优势：🌞🌞🌞博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。
⛳️座右铭：行百里者，半于九十。

💥1 概述

基于D* Lite和横向避障算法的无人驾驶地面车辆路径规划研究

摘要：本文研究了无人驾驶地面车辆的路径规划，基于全局路径生成最优路径。提出的算法使用了D* Lite和横向避障算法。该算法用于全局地图和障碍物搜索。通过自主车辆的速度提出了安全避障路径规划思路。根据速度进行安全障碍物避让。当前的自动驾驶车辆在驾驶研究中是安全可靠的。本文中，自动驾驶车辆作为一种逃避横向避障D* Lite算法的方式被使用。根据车辆速度进行光学横向避障控制，以便生成路径。该研究是UGV驾驶研究中的一部分，旨在探索并避开无限路径上的未知障碍物。

关键词：D* Lite 无人驾驶车辆路径规划全局路径横向避让

一、D* Lite算法的核心原理与优化

D* Lite是一种增量式启发式搜索算法，专为动态环境设计，具有以下核心特点：

增量更新与反向搜索
D* Lite从目标点（终点）向起点反向搜索，维护每个节点的实际代价g(n)g(n)和右侧值rhs(n)rhs(n)。当环境变化（如新增障碍物）时，仅需更新受影响节点的rhsrhs值，避免全局重规划，显著提升效率。例如，若某节点因障碍物出现导致其rhsrhs值增大，算法会将其标记为“局部过一致”，并通过优先级队列重新调整路径。
局部一致性判断
节点状态通过g(n)与rhs(n)的关系判定：
- 局部一致：g(n)=rhs(n)，节点无需处理；
- 局部过一致：g(n)>rhs(n)，通常因障碍物删除或首次规划触发；
- 局部欠一致：g(n)<rhs(n)，多由新增障碍物引起。
  这种机制使得算法能够快速响应动态变化，仅需处理不一致的节点。
启发式函数优化
结合启发式函数h(n)h(n)（当前点到起点的估计代价）和实际代价g(n)g(n)，生成键值key(n)=[min⁡(g(n),rhs(n))+h(n),min⁡(g(n),rhs(n))]以确定节点优先级。反向搜索结合启发式，确保路径最优性。
应用改进
针对路径安全性与平滑性问题，研究者通过引入安全距离约束、动力学模型（如转向角限制）和三阶贝塞尔曲线优化路径，解决传统D* Lite路径冗余和靠近障碍物的缺陷。例如，设置安全距离可避免车辆与障碍物碰撞风险，而贝塞尔曲线生成连续曲率路径，提升车辆操控稳定性。

二、横向避障算法的实现策略

横向避障算法关注车辆在动态环境中的实时避障与运动控制，主要方法包括：

基于模型预测控制（MPC）与多项式拟合
通过构建横向动力学模型，结合多项式轨迹生成（如五次多项式），在预瞄距离内生成多条候选路径，并基于避障安全距离、平顺性等指标选择最优路径。例如，南金瑞等提出的纵横向协同规划算法，利用多项式拟合生成平滑转向轨迹，并通过PID/LQR控制实现跟踪。
模糊逻辑与神经网络融合
以障碍物位置、车辆速度等为输入，转向角和目标速度为输出，构建模糊神经网络控制器。通过实时训练调整隶属度函数参数，实现动态避障决策。例如，激光雷达数据输入模糊化层，解模糊化层输出转向指令，适应复杂路况。
改进人工势场法
传统势场法易陷入局部最优，改进方案通过引入斥力因子、道路边界约束和车速自适应机制，解决目标不可达问题。黄林雄的研究显示，改进后算法在横向误差控制上提升30%以上，适应多障碍物场景。
动态窗口法（DWA）与滚动时域优化
DWA算法在速度-转向角空间中生成动态窗口，结合滚动时域方法实时评估轨迹可行性。优化后的评价函数可同时考虑避障距离、路径跟踪误差和动力学约束。例如，张方虎等融合改进A*与DWA，全局路径引导局部避障，减少计算冗余。

三、D* Lite与横向避障的协同工作机制

分层规划架构
- 全局层：D* Lite生成全局最优路径，并实时更新以应对动态障碍物。例如，当检测到新障碍物时，D* Lite更新受影响的节点代价，调整全局路径。
- 局部层：横向避障算法根据实时传感器数据（如激光雷达、视觉）调整车辆转向和速度。例如，利用DWA在动态窗口内生成局部避障轨迹，确保实时性。
信息交互机制
- 代价图共享：D* Lite维护的代价图（包含节点gg和rhsrhs值）与横向避障的局部代价图（如障碍物距离场）融合，提升路径安全性。
- 事件触发更新：当横向避障检测到无法绕行的障碍物时，触发D* Lite的全局重规划。例如，局部路径偏离阈值超过设定值，则调用D* Lite更新全局路径。
时间同步与优化
- 滚动时域协调：D* Lite以较低频率更新全局路径（如每秒1次），而横向避障以高频（如10Hz）调整局部轨迹，平衡计算负载。
- 路径平滑过渡：通过贝塞尔曲线或样条插值，将D* Lite的离散路径点转化为连续轨迹，供横向控制器跟踪。

四、研究案例与性能分析

改进DLite与动态窗口法的融合*
李雪梅等提出将改进D* Lite（引入安全距离和动力学约束）与动态窗口法结合。实验显示，在动态障碍物场景下，路径长度减少15%，计算效率提升20%，且无碰撞风险。
纵横向协同规划
姜岩等设计的算法在40km/h速度下，横向加速度控制在0.4g以内，路径跟踪误差小于0.3m，适应城市复杂交通环境。
模糊神经网络与DLite集成*
某研究中，模糊控制器实时输出转向指令，同时D* Lite每0.5秒更新全局路径。测试表明，横向误差降低40%，动态避障响应时间小于0.1秒。

五、挑战与未来方向

计算复杂度
D* Lite在大规模地图中维护节点代价的复杂度较高，需结合分层地图或稀疏化处理。横向避障的实时性依赖硬件算力，边缘计算与FPGA加速是潜在方案。
多传感器融合
激光雷达与视觉数据的时空同步问题可能影响避障精度，需优化标定与数据融合算法。
仿生算法结合
未来研究可探索D* Lite与强化学习的结合，例如利用Q-learning优化启发式函数，提升动态环境适应性。

六、结论

D* Lite与横向避障算法的协同机制通过分层规划与动态更新，实现了全局最优性与局部实时性的平衡。改进的D* Lite提升路径安全性与平滑性，而横向避障算法（如DWA、模糊控制）确保动态障碍物响应能力。两者结合在仿真与实车测试中表现优异，为无人驾驶的复杂环境适应性提供了有效解决方案。未来需进一步优化计算效率与多模态传感器融合，以应对更极端的交通场景。

📚2 运行结果

.......

部分代码：

%% parameters for Adaptive waypoints repair method
n_Oper= 3; % number of moving directions
win_size=3*win; % the window size before probabilities are updated
n_win=2; % none of the strategies improved the solutions in the m previous W windows, reinitialise
n_Ants=SwarmSize; % number of new solutions
Experience_Oper=ones(1, n_Oper);

ConvergenceData = ones(1, MaximumFEs+1)*10^5; % best fitness found

TrialIndex=1;
current_eval=1; %%% fitness function evaluations counter
%previous_eval=0;
iter=0;

%% Start initialization in the archive (PopSize, Dimension)
xant=InitPos';
fitx=SingleCostFunction(xant', TModelInfor, AgentIndex);

%% Sort the population based on fitx
[fitx, indecies ] = sort( fitx );
xant = xant( indecies, : );
ConvergenceData(1)=fitx(1);

%StandardDeviation=zeros(PopSize, Dimension);
NewAnt= zeros(n_Ants, Dimension);

NoImprove=0;
SolutionWeights=1/(q*SwarmSize*sqrt(2*pi))*exp(-0.5*(((1:SwarmSize)-1)/(q*SwarmSize)).^2);
Probability=SolutionWeights./sum(SolutionWeights);

t=SwarmSize:-1:1;
Pci=0.5+0.4*(exp(10*(t-1)/(SwarmSize-1))-1)/(exp(10)-1);

while current_eval<MaximumFEs
if flag_agent==1
xant(SwarmSize/2+1:end,:)=xmin'+rand(SwarmSize/2, Dimension).*(xmax'-xmin');

pp=repair1(xant(SwarmSize/2+1:end,:)',TModelInfor,AgentIndex, pRepair, flag_uniform);
xant(SwarmSize/2+1:end,:)=pp';
fitx=SingleCostFunction(xant', TModelInfor,AgentIndex);
[fitx, indecies ] = sort( fitx );
xant = xant( indecies, : );
eval_agent(AgentIndex)=current_eval+1;
current_eval=current_eval+n_Ants;

ConvergenceData(current_eval-n_Ants+1:current_eval)=fitx(1);

flag_agent=0;
else
iter=iter+1;

%% ---------------------Update individuals------------------------------
if mod(iter,win_size)==1
Prob_Oper=Experience_Oper/(sum(Experience_Oper)+realmin);
Experience_Oper=ones(1, n_Oper);
end

% generate new population
% Prob_Oper
flag_moving=RouletteWheelSelection(Prob_Oper);
flag_uniform=[0 flag_moving];
old_fitx=fitx(1);
ttemp=rand(SwarmSize,Dimension);
flag=(ttemp>Pci')*1;

for i=1:n_Ants

[NewAnt(i,:), Nfitx(i)]= NewSolConst(TModelInfor, xant, SwarmSize,Dimension, Probability, flag,zeta, AgentIndex, pRepair, flag_uniform);

end
%% ---------------------Evaluation----------------------------------------

allSwarm=[xant; NewAnt];
allFitnessValue= [fitx Nfitx];

% sort
[allFitnessValue, SortIndex]=sort(allFitnessValue);
allSwarm=allSwarm(SortIndex,:);

current_eval=current_eval+n_Ants;
ConvergenceData(current_eval-n_Ants+1:current_eval)=allFitnessValue(1);

% record the number without impovement
if fitx(1)<=allFitnessValue(1)