针对传统A算法拓展节点冗余、路径贴近障碍物及DWA算法轨迹振荡、易陷局部极小值等问题,提出融合改进A与DWA的路径规划方法。改进A算法通过优化代价函数减少冗余节点,改进子节点选取策略避免路径贴近障碍物,并通过双向平滑度优化去除冗余转折点;在DWA算法评价函数中引入自适应距离因子减少轨迹振荡,将A先验路径离散节点作为DWA局部目标点实现算法融合。仿真表明,改进A拓展节点减少118个,规划时间减少29.9%,改进DWA规划速度提高5.3%。融合算法在保障全局最优的同时避免局部极小值,实现未知障碍物实时避障。
1 A算法及其改进
1.1 传统A算法
A算法是全局路径规划算法,代价函数为:
[ f(n) = g(n) + h(n) ]
其中,( g(n) ) 为起始节点到当前节点的实际距离代价,( h(n) ) 为当前节点到目标节点的估计距离代价(启发函数)。本文采用欧几里得距离:
[ g(n) = sqrt{(x_n - x_s)^2 + (y_n - y_s)^2} ]
[ h(n) = sqrt{(x_g - x_n)^2 + (y_g - y_n)^2} ]
( (x_s, y_s) )、( (x_n, y_n) )、( (x_g, y_g) ) 分别为起始节点、当前节点、目标节点坐标。
1.2 改进启发函数
传统A启发函数中 ( h(n) ) 与 ( g(n) ) 权重固定,导致搜索初期效率低。改进方法引入自适应距离权重系数,基于sigmoid函数动态调整 ( h(n) ) 权重:
[ f(n) = g(n) + left(1 + frac{1}{1 + (e^{d/D})^2} ight) h(n) ]
其中,( d ) 为起始节点到当前节点距离,( D ) 为起始节点到目标节点总距离。结合障碍物稠密程度 ( P ) 调整代价函数:
[ f(n) = (1+P)g(n) + left(1 + frac{1}{1 + (e^{d/D})^2} ight) h(n) ]
改进后拓展节点减少118个,规划时间减少29.9%(见表1)。
表1 A算法启发函数改进前后性能对比
| 算法 | 路径长度/m | 规划时间/s | 拓展节点数 |
|--------------|------------|-------------|------------|
| 传统A算法 | 41.3553 | 0.0177 | 273 |
| 改进A算法 | 41.3553 | 0.0124 | 155 |
1.3 改进子节点选取策略
传统A路径易斜穿障碍物顶点,改进策略根据障碍物与父节点位置关系(分为3类,如图2)筛选子节点:
- 第I类(障碍物位于上下子节点):保留子节点4、5、6、7、8,舍弃1、2、3;
- 第II类(障碍物位于左右子节点):保留子节点2、3、5、7、8,舍弃1、4、6;
- 第III类
1.3 改进子节点选取策略(续)
(障碍物位于对角子节点):不做处理。
改进后路径避免贴近障碍物顶点(见图3),但路径长度和转折点增加。
图3 改进子节点选取策略前后路径对比

(a) 传统A路径;(b) 改进子节点策略路径
1.4 双向平滑度优化
为减少改进A路径的冗余转折点,采用双向平滑优化:
1. 正向优化:从起点遍历至目标点,删除共线中间节点;
2. 二次正向优化:离散化路径,筛选无碰撞且距离障碍物最远的节点;
3. 反向优化:从目标点反向遍历,重复优化过程。
优化后路径转折点减少29个,转折角度减少162.9°(见表3)。
表3 规划路径对比
| 算法 | 路径长度/m | 转折角/(°) | 转折次数 |
|--------------------|------------|-------------|----------|
| 传统A算法 | 42.7696 | 315.0 | 8 |
| 改进子节点选取 | 44.6985 | 765.0 | 13 |
| 双向平滑度优化 | 42.6809 | 152.1 | - |
图5 规划路径对比

(a) 传统A路径;(b) 改进子节点策略路径;(c) 双向平滑度优化路径
2 DWA算法及改进
2.1 速度窗口
DWA算法速度窗口由机器人硬件约束(( V_s ))、驱动电机约束(( V_d ))和障碍物距离约束(( V_o ))的交集确定:
[ V = V_s cap V_d cap V_o ]
其中,障碍物距离约束为:
[ V_o = left{ (v, w) mid v leq sqrt{2 cdot ext{dist}(v, w) cdot v_{a1}}, w leq sqrt{2 cdot ext{dist}(v, w) cdot w_{a1}} ight} ]
( ext{dist}(v, w) ) 为轨迹末端到障碍物的最小距离。
2.2 轨迹预测
在速度窗口内均匀采样速度组合 ((v, w)),预测轨迹为:
[
egin{cases}
x_{k+1} = x_k + v cdot cos heta_k cdot Delta t \
y_{k+1} = y_k + v cdot sin heta_k cdot Delta t \
heta_{k+1} = heta_k + w cdot Delta t
end{cases}
]
((x_k, y_k, heta_k)) 为当前位姿,( Delta t ) 为采样间隔。
2.3 改进评价函数
传统DWA评价函数权重固定,改进后引入障碍物距离因子 ( frac{ ext{dist}^(v, w)}{R} ) 动态调整权重:
[
G(v, w) = sigma left(
egin{array}{l}
left(a + (1-a) cdot frac{ ext{dist}^(v, w)}{R} ight) cdot alpha cdot ext{heading}(v, w) + \
eta cdot ext{dist}(v, w) + \
left(a + frac{R}{ ext{dist}^(v, w)} ight) cdot gamma cdot ext{vel}(v, w)
end{array}
ight)
]
其中,( ext{dist}^(v, w) = max( ext{dist}(v, w) - R, R) ),( R ) 为安全半径,( a in (0,1) ) 为常数。改进后轨迹振荡消除,规划速度提高5.3%(见图6)。
图6 DWA改进前后对比

(a) 传统DWA路径;(b) 改进DWA路径;(c) 传统DWA速度曲线;(d) 改进DWA速度曲线
3 算法融合
融合流程如图7所示:
1. 全局规划:改进A算法生成全局路径并进行双向平滑优化;
2. 局部规划:将A路径离散节点作为DWA局部目标点,实时避障;
3. 动态更新:根据传感器数据调整局部路径,保持全局最优。
图7 算法融合流程
```mermaid
graph TD
A[初始化栅格地图] --> B[改进A规划全局路径]
B --> C[双向平滑度优化]
C --> D[设定局部目标点]
D --> E[改进DWA规划局部路径]
E --> F{局部目标点是否为全局目标点?}
F -- 是 --> G[结束]
F -- 否 --> H[更新机器人位置] --> D
```
4 实验结果与分析
4.1 改进A算法仿真
与传统A和文献[9]算法对比,改进A路径更平滑,冗余节点更少(见图8)。
(a) 传统A路径;(b) 改进A路径;(c) 文献[9]路径;(d) 融合算法路径
4.2 改进DWA算法仿真
面对半封闭障碍物时,传统DWA易陷局部极小值,融合算法通过A先验路径引导成功避障(见图9)。
图9 改进DWA算法仿真

(a) 传统DWA路径(目标不可达);(b) 融合算法路径(成功避障)
4.3 融合算法仿真
在多随机障碍物场景中,融合算法路径长度更短、速度曲线更平稳,且搜索效率不受障碍物数量显著影响(见图10-12)。
图10 无随机障碍场景

(a) 融合算法路径;(b) 融合算法速度曲线;(c) 文献[14]路径;(d) 文献[14]速度曲线
5 结论
本文提出的融合算法通过改进A和DWA的优势互补,解决了传统算法在全局规划和局部避障中的不足。实验表明,该算法在复杂环境中能有效减少路径冗余、避免振荡和局部极小值,提升机器人导航的效率和安全性。未来可探索与深度学习结合,进一步提升复杂场景适应性。
* 请认真填写需求信息,我们会在24小时内与您取得联系。