汽车工程学报

类别	试验单元
1	721
2	631
3	541、532
4	451、442、433、424
5	343、334、352、361

类别	试验单元
1	721
2	631
3	541、532
4	451、442、433、424
5	343、334、352、361

试验单元	平均有效粒子数/个
721	8.78
631	8.58
541、532	8.70、8.74
451、442、433、424	8.75、8.71、8.84、8.42
343、334、352、361	8.79、8.70、8.64、8.82

试验单元	平均有效粒子数/个
721	8.78
631	8.58
541、532	8.70、8.74
451、442、433、424	8.75、8.71、8.84、8.42
343、334、352、361	8.79、8.70、8.64、8.82

项目	传统Gmapping算法	改进Gmapping算法
有效粒子数平均值	8.45	8.84
重采样次数	8	9
最优粒子权重统计量	最大值	平均值	最小值	最大值	平均值	最小值
0.401 7	0.112 5	0.002 1	0.416 2	0.118 8	0.000 5
轨迹平均差值	x轴/m	y轴/m	航向角/rad
0.015 5	0.009 7	0.005 3
最优粒子权重差值统计量	最大值	平均值	最小值
0.268 0	0.004 5	-0.310 8
蜕变指标	0.500	0.333
优势保持指标	0.375	0.444

项目	传统Gmapping算法	改进Gmapping算法
有效粒子数平均值	8.45	8.84
重采样次数	8	9
最优粒子权重统计量	最大值	平均值	最小值	最大值	平均值	最小值
0.401 7	0.112 5	0.002 1	0.416 2	0.118 8	0.000 5
轨迹平均差值	x轴/m	y轴/m	航向角/rad
0.015 5	0.009 7	0.005 3
最优粒子权重差值统计量	最大值	平均值	最小值
0.268 0	0.004 5	-0.310 8
蜕变指标	0.500	0.333
优势保持指标	0.375	0.444

算法	搜索节点/个	路径长度/ 栅格数	转折次数/次
传统A*算法	16	16.31	9
改进A*算法	10	17.24	3

算法	搜索节点/个	路径长度/ 栅格数	转折次数/次
传统A*算法	16	16.31	9
改进A*算法	10	17.24	3

参数	值
预测时间/s	3
最大横摆角速度/（rad/s）	20
最大线速度/（m/s）	1.0
步长/s	0.1
单个步长线速度采样点/个	8
线速度分辨率/（m/s）	0.04
角速度分辨率/（rad/s）	0.035
单个步长角速度采样点/个	5

参数	值
预测时间/s	3
最大横摆角速度/（rad/s）	20
最大线速度/（m/s）	1.0
步长/s	0.1
单个步长线速度采样点/个	8
线速度分辨率/（m/s）	0.04
角速度分辨率/（rad/s）	0.035
单个步长角速度采样点/个	5

试验	对照组	试验组
线速度	均值	0.866 3	0.865 0
方差	0.009 3	0.009 5
角速度绝对值	均值	0.032 5	0.031 4
方差	0.001 7	0.001 2
路径长度	最大值	0.209 4	0.122 1
均值	113.44	113.64

试验	对照组	试验组
线速度	均值	0.866 3	0.865 0
方差	0.009 3	0.009 5
角速度绝对值	均值	0.032 5	0.031 4
方差	0.001 7	0.001 2
路径长度	最大值	0.209 4	0.122 1
均值	113.44	113.64

关键参数	值
建图速度/（m/s）	0.2
粒子数/个	10
重采样阈值	0.8
有效测距长度/m	25
建图尺寸（长×宽）/m	61×17

关键参数	值
建图速度/（m/s）	0.2
粒子数/个	10
重采样阈值	0.8
有效测距长度/m	25
建图尺寸（长×宽）/m	61×17

线段检测项目	毫米级测距仪实测标准值/m	传统Gmapping测量值/m	传统Gmapping测量误差率/%	本文改进Gmapping 算法测量值/m	本文改进Gmapping 算法测量误差率/%
1号	13.460	13.506	-0.342	13.549	-0.661 2
2号	2.756	2.596	5.806	2.803	-1.705 4
3号	14.076	14.067	0.063 9	14.202	-0.895 1
4号	16.780	16.46	1.907	16.722	0.345 6
5号	34.041	33.775	0.781	34.056	-0.044 1
6号	17.432	17.426	0.034	17.452	-0.114 7

线段检测项目	毫米级测距仪实测标准值/m	传统Gmapping测量值/m	传统Gmapping测量误差率/%	本文改进Gmapping 算法测量值/m	本文改进Gmapping 算法测量误差率/%
1号	13.460	13.506	-0.342	13.549	-0.661 2
2号	2.756	2.596	5.806	2.803	-1.705 4
3号	14.076	14.067	0.063 9	14.202	-0.895 1
4号	16.780	16.46	1.907	16.722	0.345 6
5号	34.041	33.775	0.781	34.056	-0.044 1
6号	17.432	17.426	0.034	17.452	-0.114 7

角度检测项目	标准值/（°）	传统Gmapping测量值/（°）	传统Gmapping测量误差率/%	本文改进Gmapping 算法测量值/（°）	本文改进Gmapping 算法测量误差率/%
角A	168.536	170.718	-1.295	170.472	-1.1487
角B	5.622	4.508	19.86	4.662	17.075
角C	5.842	4.773	18.299	4.865	16.723 7

角度检测项目	标准值/（°）	传统Gmapping测量值/（°）	传统Gmapping测量误差率/%	本文改进Gmapping 算法测量值/（°）	本文改进Gmapping 算法测量误差率/%
角A	168.536	170.718	-1.295	170.472	-1.1487
角B	5.622	4.508	19.86	4.662	17.075
角C	5.842	4.773	18.299	4.865	16.723 7

关键参数	值
最大线速度/（m/s）	0.2
最大角速度/（rad/s）	20
控制指令发布频率/Hz	10
线速度采样点/个	6
角速度采样点/个	10
前向模拟时间/s	5
障碍物膨胀半径/m	0.5
加速度/（m/s²）	0.5
角加速度/（rad/s²）	3.5

关键参数	值
最大线速度/（m/s）	0.2
最大角速度/（rad/s）	20
控制指令发布频率/Hz	10
线速度采样点/个	6
角速度采样点/个	10
前向模拟时间/s	5
障碍物膨胀半径/m	0.5
加速度/（m/s²）	0.5
角加速度/（rad/s²）	3.5

试验指标结果	值
平均速度/（m/s）	0.192 5
平均角速度绝对值/（rad/s）	0.009
最大角速度/（rad/s）	0.085 9
路程长度/m	70.286
耗时/s	305.5
控制指令发布频率/Hz	10

试验指标结果	值
平均速度/（m/s）	0.192 5
平均角速度绝对值/（rad/s）	0.009
最大角速度/（rad/s）	0.085 9
路程长度/m	70.286
耗时/s	305.5
控制指令发布频率/Hz	10

基于改进Gmapping与DWA算法的室内智能车SLAM和路径规划算法研究

PDF下载

王冀白 ¹ , 余强 ¹ , 邵明志 ² , 刘湘安 ³ , 吴斗 ⁴ , 李智鹏 ⁵ , 刘永涛 ⁵

汽车工程学报 | 智能网联技术专栏/主编：高镇海 2025,15(4): 539-553

收起

汽车工程学报 | 智能网联技术专栏/主编：高镇海 2025, 15(4): 539-553

基于改进Gmapping与DWA算法的室内智能车SLAM和路径规划算法研究

全屏

王冀白¹, 余强¹, 邵明志², 刘湘安³, 吴斗⁴, 李智鹏⁵, 刘永涛⁵

作者信息

¹ 西安汽车职业大学，西安 710038

² 解放军 94456部队，山东，威海 264411

³ 广州城市理工学院，广州 510800

⁴ 中汽院车城融合（武汉）科技有限公司，武汉 430070

⁵ 长安大学，西安 710064

王冀白（1987-），男，陕西西安人，硕士，工程师，主要研究方向为智能车辆控制技术。514366153@qq.com

通讯作者:

刘永涛（1989-），男，安徽砀山人，博士，副教授，主要研究方向为人车系统安全、智能车辆控制技术。liuyongtao86@163.com

Research on SLAM and Path Planning Algorithms for Indoor Intelligent Vehicles Based on Improved Gmapping and DWA Algorithms

Jibai WANG¹, Qiang YU¹, Mingzhi SHAO², Xiang’an LIU³, Dou WU⁴, Zhipeng LI⁵, Yongtao LIU⁵

Affiliations

¹ Xi’an Vocational University of Automobile，Xi’an 710038，China

² Unit 94456 of PLA，Weihai 264411，Shandong，China

³ Guangzhou City University of Technology，Guangzhou 510800，China

⁴ China Automotive Research Institute Vehicle-City Integration（Wuhan）Technology Co.，Ltd.，Wuhan 430070，China

⁵ Chang’an University，Xi’an 710064，China

出版时间: 2025-07-20 doi: 10.3969/j.issn.2095‒1469.2025.04.11

文章导航

摘要

收起

针对智能车建图时常出现粒子的快速收敛、粒子多样性下降以及传统DWA算法常出现陷入局部最优解问题，提出基于K-Means分层重采样的改进Gmapping算法及基于融合A*和考虑转弯稳定性的改进DWA路径规划算法。改进Gmapping算法通过K-Means算法，将粒子集合划分为高、中、低权重3类粒子集合，结合合理的粒子权重设置，延缓粒子多样性衰退以提高建图准确性。通过增加考虑角速度大小的自适应速度评价函数和角速度评价函数，将A*全局路径转折点作为关键点，并融合A*与DWA算法以提高DWA算法全局寻优能力。仿真和实车试验结果表明，改进Gmapping算法在构建栅格地图时，平均有效粒子数提高4.6%，在设定场景下，改进DWA算法使智能车全局路径转折次数减少67%、搜索节点减少37.5%，有效提高智能车转弯稳定性。

关键词

室内智能车 / 分层重采样 / 改进Gmapping / DWA动态窗口法 / SLAM / 路径规划

Abstract

收起

To address the rapid particle convergence and the decrease of particle diversity during map construction, as well as the tendency of the traditional DWA to become trapped in local optima during the path planning, the paper proposes two improvements for intelligent vehicles. The first improvement is an enhanced Gmapping algorithm based on K-Means hierarchical re-sampling. The particle set is clustered into high-, medium- and low- weight groups by using K-Means algorithm, and the weights are adjusted to slow down the decline in particle diversity, thereby improving mapping accuracy. The second improvement is an enhanced DWA path planning algorithm that fuses A* global guidance with turn-stability awareness. The adaptive velocity evaluation function considering the angular velocity magnitude, and a separate angular velocity evaluation function are added. The A* global path turning points serve as the key points to integrate the A* and DWA algorithms. Together, these two efforts improve the global optimization ability of the DWA algorithm. The simulation and real vehicle testing results show that the improved Gmapping algorithm increases the average number of effective particles by 4.6% during grid-map construction. The improved DWA algorithm reduces the number of global path turns by 67% and the search nodes by 37.5% under the set scenario, effectively improving the turning stability of intelligent vehicles.

Key words

Indoor intelligent vehicle / hierarchical resampling / improved Gmapping / DWA dynamic windowing method / SLAM / path planning

引用本文

王冀白, 余强, 邵明志, 刘湘安, 吴斗, 李智鹏, 刘永涛. 基于改进Gmapping与DWA算法的室内智能车SLAM和路径规划算法研究. 汽车工程学报, 2025 , 15 (4) : 539 -553 . DOI: 10.3969/j.issn.2095‒1469.2025.04.11

Jibai WANG, Qiang YU, Mingzhi SHAO, Xiang’an LIU, Dou WU, Zhipeng LI, Yongtao LIU. Research on SLAM and Path Planning Algorithms for Indoor Intelligent Vehicles Based on Improved Gmapping and DWA Algorithms[J]. Chinese Journal of Automotive Engineering, 2025 , 15 (4) : 539 -553 . DOI: 10.3969/j.issn.2095‒1469.2025.04.11

正文

收起

同步地图构建与定位技术（Simultaneous Localization and Mapping，SLAM）是自动驾驶领域的热门研究方向，SLAM的理念最早由SMITH等^[1]在1986年IEEE机器人与自动化会议上提出。早期，有学者将SLAM问题转换为一个状态估计问题，利用扩展卡尔曼滤波、粒子滤波以及最大似然估计等方法来求解^[2-3]。

研究者们对在粒子滤波SLAM系统中粒子多样性的提高与粒子退化的减少方面进行了很多探索。GRISETTI等^[4]提出Gmapping，该算法通过改进提议分布和选择性重采样改善粒子退化。WANG Yifan 等^[5]提出一种基于权重优化组合粒子滤波SLAM算法，所有粒子的权重在粒子集中被重新优化，并且它们与粒子的简并和耗尽趋势相结合，增加低权粒子自我复制的机会，从而增加粒子集的多样性。SLOWAK等^[6]提出一种新颖的粒子样本加权思想，将粒子分类并使用重要性因子偏移量将粒子分组，试验表明，分层粒子滤波SLAM系统效果更准确、更稳健。LAI Xin等^［7]将粒子数与场景复杂程度做线性拟合，自适应增减粒子数以缓解粒子多样性耗散。

路径规划技术是自动驾驶规划决策系统中的主要内容，路径规划技术通过采用全局路径规划器及时生成适应环境变化的最佳路径，并通过局部路径规划器实时处理复杂决策问题，求解最优的智能车控制指令。针对路径规划问题，常见的算法有Dijkstra算法、A*算法、动态窗口方法（Dynamic Window Approach，DWA）算法、人工势场法等。

传统DWA算法在复杂环境下容易陷入局部最优解，导致规划任务失败，为提高DWA算法环境适应性和鲁棒性，学者们进行了大量研究。LAI Xin等 ^[7]提出一种增强动态窗口算法，通过使用距离函数权重和新的评估函数，优化了机器人的稳定性。XU Wan等^[8]提出了一种参数自适应动态窗口算法，通过优化速度采样空间和轨迹评估函数，减少了轨迹规划时间。FAN Jiazhe等^[9]采用一种改进的动态窗口方法，通过设计自适应函数改善评估函数并提出一种低速转向策略以避开U型障碍物。 WANG Shiqi等^[10]结合了深度确定性策略梯度（Deep Deterministic Policy Gradient，DDPG）和DWA，将避障问题视为学习激励问题，通过DDPG的奖励机制优化DWA的避障功能。KOBAYASHI等^[11]通过Q学习动态调整权重，提出DQDWA方法以适应复杂环境，增强了算法鲁棒性。SI Mingpeng等^[12]提出一种改进的DWA，通过障碍物速度动态调整安全距离，并在速度评估中加入新的评价项。KOBAYASHI等^[13]提出基于虚拟操纵器的动态窗口方法（DWV），在具有动态障碍物的环境下，会生成非直线和非弧形路径的可行路径。LI Xinying等^[14]提出一种基于多目标粒子群优化的改进DWA，并设计了一种复杂环境下障碍物密集区域的判断方法。YANG Dian等^[15]通过引入神经网络训练，实现自适应权重调整DWA以适应更广泛的路径规划场景。

当前研究粒子滤波SLAM算法，多聚焦于通过优化重采样和提议分布，以延缓粒子多样性下降，克服粒子退化问题，提高定位与地图构建的准确性。引入分层思想及精细化粒子滤波重采样策略来提高SLAM建图稳定性和精度的研究较少，鲜有研究通过关键点策略以融合A*全局路径规划和DWA局部路径规划，且DWA局部路径规划并未考虑智能车转弯稳定性问题。因此，本文通过K-Means分层聚类并分别进行重采样对Gmapping SLAM算法进行改进，同时，将A*算法的8方向节点扩展改进为16方向节点扩展，将改进规划的全局路径关键点作为DWA算法的局部目标点，并提出考虑角速度影响的自适应速度评价函数和角速度评价函数以提高智能车SLAM建图准确性及弯道行驶稳定性。

1 基于K-Means分层重采样策略的改进Gmapping SLAM算法研究

收起

SLAM是通过传感器测量数据、智能车控制量和智能车上一时刻位姿先验估计，来估计智能车下一时刻的位姿和环境地图后验概率分布，Gmapping算法由位姿估计、粒子重采样和环境地图构建3部分组成。

1.1 基于K-Means分层重采样的Gmapping SALM算法

K-Means算法是一种高效的无监督机器学习聚类算法，采用基于K-Means聚类的分层重采样方法为Gmapping算法引入一种创新的粒子管理机制，使重采样过程更加精细化，有效地平衡粒子低方差估计和多峰环境适应性之间的矛盾。基于K-Means分层重采样的改进Gmapping算法如图1所示。

分层重采样策略基本步骤如下。

1）当有效粒子数低于重采样阈值时，将各粒子的权重大小作为K-Means算法输入，K值的选取采取轮廓系数法进行确定，最佳K值为3，故设定3个初始聚类中心，由K-Means聚类算法依据粒子权重对各粒子进行聚类，粒子被分为3类。

2）分别在3个粒子集合中对粒子进行权重归一化处理，归一化公式为：

Q n i = Q i ∑ i = 1 N Q i

。（1）

式中：

Q i

为类别粒子集合中的第

i

个粒子的权重；

N

为该粒子集合的粒子总数；

Q n i

为第

i

个粒子归一化后的权重。

3）由试验确定各粒子集合的重采样权重，即该粒子集合重采样后产生新粒子的比例，按照归一化的权重分别对3类粒子进行重要性重采样，产生新一代粒子集合。

4）对新一代粒子集合的权重进行归一化处理，使各粒子权重相同。

1.2 仿真结果分析

本节采用公开数据集对粒子类别重采样权值差异和改进Gmapping算法对建图效果的影响进行仿真测试，选取的Seattle数据集如图2所示。

1.2.1 粒子类别重采样权重比例的选取

设计12组对比仿真试验，试验的粒子数为10，并根据高权重粒子类别重采样权重不同分成5个类别，用来选取特定重采样权重以达到最佳改进Gmapping算法建图效果，试验类别见表1。类别1的试验单元“721”表示高权重粒子：中权重粒子：低权重粒子为7

∶

∶

1，按照高权重粒子比例从30%至70%（当高权重粒子比例大于70%时，重采样后的粒子几乎全部由高权重粒子复制产生，极大地降低粒子的多样性，当高权重粒子比例低于30%时，会导致粒子估计的位姿误差过大），把其划分为5类，低权重粒子的比例基本小于中权重粒子的比例，以减小粒子总体位姿估计误差，并保证粒子估计的多样性。

选取3处粒子发生重采样时所处的位置，对这3处位置的局部地图进行评价，选取线是否笔直、物体相对位置是否正确和有效粒子数平均值等评价指标对地图进行评价并得出相对主观评分，3处局部地图如图3中红圈所示。

类别1、2、3、4的建图结果如图4～5所示，用同样的方法进行类别5试验建图效果分析，由表2可知，4

∶

∶

3重采样权重分配方案时建图整体效果最优。

1.2.2 公开数据集下对比结果分析

利用公开数据集，对改进Gmapping算法与传统Gmapping算法进行对比试验。改进Gmapping算法基于4

∶

∶

3重采样权重分配方案，地图评价指标包括平均有效粒子数以及位姿估计准确性等方面。

1）有效粒子数评价

图6为SLAM算法运行过程中有效粒子数的变化趋势，两者的趋势大致相同，改进Gmapping算法的有效粒子平均数8.84略大于未改进算法的8.45。

2）轨迹评价

由图7a位移差值可知，在1 500步长后，x轴方向的轨迹差值出现明显的变化，一开始下降并在1 800步长时陡然增长，最后回到0附近；由图8可知，200、1 200和1 500步长时，改进Gmapping算法与传统Gmapping算法的最优粒子呈现一高一低的现象，由于最优粒子权重是由地图与雷达数据匹配计算得出，权重越高表示位姿估计越准，因此，此时改进Gmapping算法的位姿估计更准。1 900步长时，改进Gmapping算法最优粒子权重达到最低并及时攀升，改进Gmapping算法最优粒子权重平均值0.118 8略大于未改进算法的0.112 5。由图9可知，在坐标（-6，-10）处之前两者的轨迹大致重合，在建图回环处，两者轨迹出现明显差异，说明建图算法在建图回环处，由于累计误差的存在，位姿估计精度会严重下降，而改进Gmapping算法在建图回环处的地图质量较好。

3）重采样评价

本文提出评价重采样优劣的特色指标，分别对应重采样过程中低权粒子适应环境蜕变为高权粒子、高权粒子持续增殖为高权粒子。评价如式（2）和式（3）所示。

f t = p t n

。（2）

f c = p c n

。（3）

式中：

f t

为蜕变指标；

p t

为上一重采样时刻的高权粒子在下一重采样后被上一重采样时刻中的低权粒子所取代的次数；

f c

为优势保持指标数；

p c

为上一重采样时刻的高权粒子在下一重采样后继续成为高权粒子次数；

n

为重采样次数。

重采样粒子蜕变经常发生于最优粒子权重较低时，Gmapping算法最优粒子在200、1 200、1 500、1 800步时权重较低，正是这几处发生粒子蜕变；同样，改进Gmapping算法最优粒子在1 200、1 600、1 900步时发生粒子蜕变。蜕变指标和优势保持指标见表3，蜕变指标越小，优势保持指标数值越大，最优粒子权重更稳定，有利于保持较高有效粒子数。

综上所述，在有效粒子数评价中，改进Gmapping算法的平均有效粒子数提高4.6%；在轨迹评价中，由于轨迹真值未知，因此，采用最优粒子权重大小间接对比轨迹优劣，改进Gmapping算法的最优粒子权重总和是传统Gmapping算法的104.6%，改进Gmapping算法的轨迹更符合轨迹真值；在重采样评价中，改进Gmapping算法的优势保持指标比传统Gmapping算法高18.4%，蜕变指标低33.4%，改进Gmapping算法的最优粒子权重更稳定。

2 基于16邻域搜索模型的改进A*算法

收起

2.1 传统A*算法基本原理

A*算法是一种静态网路中求解最短路径最有效的直接搜索算法。传统A*算法一般用欧几里得距离计算启发式代价函数，其代价评价函数如式（4）所示，欧几里得距离计算如式（5）所示。

F (n) = G (n) + H (n)

。（4）

h (n) = (x t - x n) 2 + (y t - y n) 2

。（5）

式中：

G (n)

为起始点到当前节点的欧几里得距离；

H (n)

为当前节点到终点的欧几里得距离。

2.2 基于16邻域搜索模型的改进A*算法

针对传统A*算法路径转折点过多、路径不平滑的问题，本文采用16邻域搜索模型扩展A*算法的邻域搜索范围，如图10b所示，引入新的8个搜索方向，新的搜索方向位于传统的8个方向之间。

2.3 仿真结果对比分析

设置如图11所示场景，对传统A*算法与改进的A*算法进行仿真试验，红色方块为起始点、蓝色方块为终点。

改进A*算法与传统A*算法相比，规划出的路径平滑，转折次数显著减少67%，同时搜索涉及的节点数量也减少37.5%，路径长度虽有所增加，但转折次数的减少促进了智能车辆行驶速度的提升，改进A*与传统A*算法仿真结果数据对比见表4。

3 基于融合A*和考虑转弯稳定性的改进DWA算法

收起

3.1 传统DWA算法基本原理

DWA算法是根据智能车当前的位置状态和速度状态在速度空间中确定一个满足智能车约束的采样速度空间；然后，计算智能车在采样速度下移动一定时间内的轨迹，并通过评价函数对该轨迹进行评价；最后，选出评价得分最高轨迹所对应的速度作为智能车运动速度，如此循环直至智能车到达目标点，具体步骤如下。

1）速度采样。在速度空间中对智能车的角速度和线速度进行离散采样，传统DWA算法速度约束条件包括最大最小速度约束、加速度约束和安全制动约束。

智能车的线速度和角速度取值需满足智能车的速度边界约束，如式（6）所示。

V s = {(v, ω) | v ∈ [v m i n, v m a x] ∧ ω ∈ [ω m i n, ω m a x]}

。（6）

式中：

v m i n

和

v m a x

分别为智能车的最小和最大线速度；

ω m i n

和

ω m a x

分别为智能车的最小和最大角速度。

在规定采样时间内，智能车的速度受限于智能车的加速度大小，如式（7）所示。

V a = {(v, m) | v ∈ [v t - v M Δ t, v t + v M Δ t] ⋂ ω ∈ [ω t - ω M Δ t, ω t + ω M Δ t]}

。（7）

式中：

v M

和

ω M

分别为智能车的最大线加速度和最大角加速度；

Δ t

为采样时间。

为避免与障碍物发生碰撞，速度的选取需考虑智能车与障碍物的最小距离，在采用最大制动加速度的情况下，智能车可以无碰撞刹停，如式（8）所示。

V d = {(v, ω) | v ≤ 2 d i s t (v, ω) v M ⋂ ω ≤ 2 d i s t (v, ω) ω M}

。（8）

2）轨迹生成。在考虑智能车上述3类速度约束后，将约束内的速度取值范围划分成许多小网格，每一个网格表示一个采样轨迹，通过设置线速度与角速度分辨率确定网格大小，如式（9）所示。

n = [(v m a x - v m i n) / E v] [(ω m a x - ω m i n) / E ω]

。（9）

式中：E_v和E_ω为线速度与角速度分辨率。

3）轨迹评价。通过速度采样，预测

n

条轨迹，最优轨迹的选取通过轨迹评价函数评价每条轨迹，将得分最高的轨迹所对应的速度作为智能车运动速度。传统评价函数考虑3个评价指标，分别是方向角偏差、行驶效率与碰撞安全，如式（10）所示。

G (v, ω) = α × h e a d (v, ω) + β × v e l (v, ω) + γ × d i s t (v, ω)

。（10）

式中：

α

、

β

、

γ

分别为方向角偏差、行驶效率与碰撞安全指标权重，其权重过大或过小都会影响智能车行驶方向、行驶速度及碰撞风险，其最佳权重通过智能车试验测试来综合选取。

方向角偏差评价函数会促使智能车不断向目标点方向运动，

θ

越小代表智能车向目标点靠近；

π - θ

值越大，得分越高，其归一化如式（11）所示。

h e a d (i) = π - θ ∑ i = 1 N (π - θ)

。（11）

式中：

θ

为智能车预测位姿航向角与当前智能车位置和目标点连线的夹角；

N

为采样时间内预测轨迹的总数。

行驶效率评价函数设计方面，线速度

v

越高，行驶效率越高，得分越高，其归一化如式（12）所示。

v e l (v, ω) = v i ∑ i = 1 N v i

。（12）

碰撞安全评价函数设计是为减少与障碍物碰撞的风险，其归一化如式（13）所示。

d i s t (v, ω) = d i ∑ i = 1 N d i

。（13）

式中：

d i

为第

i

个预测位姿与最近障碍物的距离。

3.2 改进DWA算法

为避免传统DWA算法在运行过程中出现规划失败，陷入局部最优解的情况，将改进A*算法规划的全局路径关键点作为DWA算法的局部目标点；同时，考虑智能车的弯道稳定性问题，提出考虑角速度的自适应线速度评价函数，并新增角速度评价函数对角速度进行峰值限制处理。算法流程如图12所示。

3.2.1 融合策略

将DWA算法的终点分成多个目标点依次进行跟踪，各个子目标点由A*算法所生成路径关键点确定。关键点提取算法是提取A*路径转折处的节点作为关键点，通过判断路径相邻3个节点是否在同一直线上来确定转折处的节点，如图13所示。计算相邻节点的角度，通过角度是否相等判断是否在同一直线上，不相等则中间节点为关键点，为避免在全局路径转折点密集局域相邻关键点间隔过近，特设定距离阈值。

3.2.2 考虑转弯稳定性的改进评价函数

传统DWA的轨迹评价函数仅有方向角偏差、行驶效率与碰撞安全3个因素，未能考虑因智能车在急转弯时线速度仍保持较高值，导致急转弯时过大的角速度。因此，本文首先新增角速度评价函数对角速度所能达到的最大值进行限制，角速度评价函数，如式（14）～（16）所示。

α i = | ω m a x - ω i |

。（14）

W E (i) = 0, α i < 0 1, α i ≥ 0

。（15）

v e l a n g u l a r v, ω = W E i ∑ i = 1 N W E i

。（16）

式中：

ω i

为当前时刻采样得到的第

i

个角速度；

α i

为角速度峰值评价因子；

W E (i)

为第

i

个角速度的评价值；

v e l a n g u l a r v, ω

为归一化的角速度评价值。

并在角速度峰值评价因子的基础上，考虑角速度对线速度的影响，设计线速度评价因子，该评价因子可根据角速度的大小自适应地调整线速度的限值，使智能车在转弯时角速度增大而能自适应地降低线速度，保障智能车的转弯稳定性与行驶安全，线速度评价因子如式（17）所示，自适应速度评价函数如式（21）所示。

β i = α i | ω m a x - ω m i n |

。（17）

V l i m i t = V m a x × β i

。（18）

V f l o g = V l i m i t - a b s v i

。（19）

V E (i) = 0, V f l o g < 0 a b s (v), V f l o g ≥ 0

。（20）

v e l a d a p t i o n v, ω = V E i ∑ i = 1 N V E i

。（21）

式中：

β i

为考虑角速度因子的线速度评价因子；

V m a x

为线速度最大值；

V l i m i t

为当前采样得到线速度的最大限制值；

V f l o g

为自适应速度评价函数评价标志位；

V E (i)

为第

i

个线速度的自适应线速度评价值；

v e l a d a p t i o n v, ω

为归一化后的自适应线速度评价值。

3.2.3 考虑阿克曼运动学约束的控制指令采样

传统DWA算法是根据双轮差速运动学模型设计，因此，基于阿克曼运动学模型的智能车不能直接使用DWA算法，为使DWA算法适用于阿克曼运动学模型，需在DWA算法的速度采样中加入前轮转角约束。智能车在非直线道路行驶时，DWA算法进行车辆线速度和角速度采样，由于智能车存在线速度与角速度的比值等于转弯半径的关系，当此比值小于智能车的最小转弯半径时，舍弃此时智能车的线速度和角速度数值采样，以此来进行智能车前轮转角约束。

3.3 仿真结果对比分析

依据前文所述公开数据集构建的室内地图对改进路径规划算法进行仿真测试，探究改进评价函数对智能车运动轨迹的影响。表5为试验的关键技术参数。

试验场景如图14所示，图中绿色星号为起始点，红色星号为目标点，黑色星号为局部目标点，红色虚线为全局路径，黑色实线为运动轨迹。

表6为智能车运动轨迹数据，将新增改进评价函数的路径规划算法作为试验组，新增改进评价函数后，角速度绝对值的最大值呈现降低趋势。

通过对比对照组和试验组，结合图15可知，改进评价函数中角速度评价函数与自适应速度评价函数两者的作用并未发生冲突，一方面限制转弯处的角速度尖峰，另一方面在弯道时进行降速，平滑轨迹，共同提高智能车行驶安全。

4 基于ROS平台的室内智能车实车试验

收起

4.1 室内智能车试验平台

搭建的室内智能车试验平台，主要包含感知系统、控制系统和执行系统，如图16所示。室内智能车试验软件架构流程如图17所示。

4.2 室内智能车行驶环境地图构建试验

本文选取一个典型走廊场景，形状为长方形，最长距离达到61 m，实际场景如图18所示。采用表7所示的关键参数值，构建的地图如图19所示。

4.2.1 定性分析

如图19a所示，选取4个有明显区别的位置进行定性分析。在位置1、2、3处，采用传统Gmapping算法得到的地图显示出较清晰的线条，表明在这些区域内，传统Gmapping算法能较好地捕捉到环境的细节。在位置4处，即建图的回环检测处，传统Gmapping算法生成的地图出现墙体线条缺失及轻微的地图偏移现象，说明传统Gmapping算法存在回环检测和闭环优化处理的不足。与此相对，改进Gmapping算法在位置4的地图没有出现明显的墙体线条缺失或地图偏移，显示其在回环处理上的优势。

4.2.2 定量分析

为描述地图形变程度的大小及墙体相对位置距离的误差，如图20所示选取9处位置并测量6个线段距离。用4、5、6三个线段所围成三角形的3个内角评价地图的形变程度，角A、B、C分别为4、5、6号线段所对应角度，测得的数据见表8～9。

在图21中，相比于传统Gmapping算法，改进Gmapping算法的误差率普遍偏低。同时可以看出，虽然图21中三角形各边，4、5、6线段误差都较小，误差率在2%以内，但是角度误差率却很大，最大误差率达到19.86%，角度误差是导致地图变形的主要原因。综合以上分析，长直走廊场景下改进Gmapping算法的建图效果整体是较好的。

4.3 栅格地图下室内智能车改进路径规划算法试验

根据所构建的长直走廊场景的先验环境地图，如图22所示，采用膨胀障碍物的方法，即以障碍物占用栅格为圆心，以膨胀半径画圆，将圆内的非占用栅格近似为占用栅格，膨胀后的地图如图23所示。

在长直走廊场景下分别进行融合全局路径的改进DWA算法与传统DWA算法导航试验，试验算法参数见表10。试验选取一条L形路线，包含一段直角弯道和两段直线，如图23中红色箭头所示。

4.3.1 改进DWA算法路径规划试验

改进DWA算法的路径规划试验中，在地图中设置终点，全局规划器首先规划出一条全局路径，如图24绿色曲线所示。然后改进DWA算法依据全局路径局部目标点，对控制指令采样并评价，得出较优的当前时刻控制指令并执行。

由表11试验数据可知，运动路程为70.286 m，耗时305.5 s，平均速度0.192 5 m/s，达到最高设定速度96.25%，智能车基本能以设定的最高速度运动；平均角速度绝对值0.009 rad/s，除直角转弯外智能车以直线运动居多。

在图25a、b中，智能车保持0.2 m/s匀速运动，在转弯时速度有所降低；在直角转弯处出现角速度最大值，在长楼道时，角速度波动较剧烈，由图25c可知，轨迹转折拐弯较多，这主要由全局路径转折点多、地图定位精度差共同导致的现象。

4.3.2 传统DWA算法路径规划试验

在传统DWA算法导航试验中，对智能车进行多次试验，均在直角转弯时发生碰撞，根据理论分析可知，这是由于缺乏全局路径的局部目标点指引，传统DWA的目标指向角评价函数对控制指令采样产生错误的影响，指向角评价函数根据当前航向角与当前位置点和目标点连线角之差作为评价量，会使智能车过早地转向，从而导致智能车与墙体发生碰撞。

图26所示为智能车发生碰撞时的运动数据。在500步长时，智能车与墙体发生碰撞，在该处未移动且驱动轮出现打滑，故在此地图中，智能车定位出现偏差导致轨迹估计错误；如图26a所示，智能车首先向右小转向，之后向左大转向，但仍未能使智能车顺利行驶。

综上分析，在长直走廊场景中，通过结合全局路径信息的改进DWA算法与传统DWA算法相比，表现出显著的性能提升。特别在直角转弯的路段，改进DWA算法能无碰撞地完成转弯动作，并且在地图定位不精准或环境复杂且充满挑战的情况下，仍然能顺利完成长距离的路径规划任务。此外，改进DWA算法展现出更强的抵抗外界干扰的能力和更高的鲁棒性。

5 结论

收起

1）提出一种基于K-Means聚类分析的分层重采样策略下改进Gmapping SLAM算法，选取真实的公开数据集进行测试，结果表明，在多粒子数下，高、中和低权重3类粒子类别权重比例为4

∶

∶

3时，可以有效平衡建图精度与粒子多样性，与已有算法对比，所构建的栅格地图更精准，有效粒子数提升4.6%。

2）提出一种融合改进A*路径关键点和考虑转弯稳定性的DWA局部路径规划算法，在所构建的高精栅格地图上进行路径规划仿真测试。结果表明，对比传统算法，在多障碍物场景下全局路径转折次数减少67%、搜索节点减少37.5%，提高了改进算法的搜索效率和路径平滑度；弯道场景下角速度绝对值均值降低5%，其最大值降低41.7%。

3）搭建室内智能车试验平台，在长直走廊下验证本文所提出的相关算法的有效性。试验结果表明，改进Gmapping SLAM算法的距离误差率在2.1%以内，角度误差率比传统Gmapping算法平均值低5.339%；改进路径规划算法能在室内各场景下安全无碰撞行驶。

基金

收起

国家重点研发计划项目(2021YFB2501202)
陕西省自然科学基础研究计划项目(2023-JC-QN-0664)
陕西省教育厅科学研究计划项目(23JK0592)
西安汽车职业大学校长基金项目(2023KJ001)

参考文献

收起

文献

收起

参考文献引证文献

排序方式：

[1]

SMITH

， CHEESEMAN

.On the Representation and Estimation of Spatial Uncertainty[J].The International Journal of Robotics Research，1986，5：56-68.

[2]

MONTEMERLO

， THRUN

， KOLLER

，et al.FastSLAM：A Factored Solution to the Simultaneous Localization and Mapping Problem[C]// 18th National Conference on Artificial Intelligence，and 14th Conference on Innovative Applications of Artificial Intelligence （AAAI-02，IAAI-02），Jan.1，2002，Edmonton，Alberta，Canada.Palo Alto CA：AAAI Press，c2002：593-598.

[3]

MONTEMERLO

， THRUN

， KOLLER

，et al.FastSLAM2.0：An Improved Particle Filtering Algorithm for Simultaneous Localization and Mapping that Provably Converges[C]//Proceedings of the 18th International Joint Conference on Artificial Intelligence，Aug. 9-15，2003，Acapulco，Mexico.San Francisco：Morgan Kaufmann Publishers Inc.，2003：1151-1156.

[4]

GRISETTI

， STACHNISS

， BURGARD

.Improved Techniques for Grid Mapping with Rao-Blackwellized Particle Filters[J].IEEE Transactions on Robotics，2007，23（1）：34-46.

[5]

WANG

Yifan

， WANG

Xiaoyan

.Research on SLAM Road Sign Observation Based on Particle Filter[J].Compu-tational Intelligence and Neuroscience，2022，2022：4478978.1-4478978.9.

[6]

SlOWAK

， KANIEWSKI

.Stratified Particle Filter Monocular SLAM[J].Remote Sensing，2021，13（16）：13163233.1-13163233.26.

[7]

LAI

Xin

， WU

Dan

， WU

，et al.Enhanced DWA Algorithm for Local Path Planning of Mobile Robot[J].Industrial Robot，2023，50（1）：186-194.

[8]

Wan

， ZHANG

Yuhao

， YU

Leitao

，et al.A Local Path Planning Algorithm Based on Improved Dynamic Window Approach[J].Journal of Intelligent & Fuzzy Systems，2023，45（3）：4917-4933.

[9]

FAN

Jiazhe

， HUANG

，et al.An Improved Path Planning Algorithm with Adaptive Parameters and Predictions[J].IEEE Systems Journal，2023，17（3）：4911-4921.

[10]

WANG Shiqi，HU Yiyi，LIU Zhenni，et al.Research on Adaptive Obstacle Avoidance Algorithm of Robot Based on DDPG-DWA[J].Computers and Electrical Engineering，2023，109：108753.1-108753.10.

[11]

KOBAYASHI M，ZUSHI H，NAKAMURA T，et al.Local Path Planning：Dynamic Window Approach with Q-Learning Considering Congestion Environments for Mobile Robot[J].IEEE Access，2023，11：96733-96742.

[12]

SI Mingpeng，ZHOU Xiangyu，ZHANG Yi.Improvement of Dynamic Window Approach in Dynamic Obstacle Environment[J].Journal of Physics：Conference Series，2023，2477（1）：012059.1-012059.7.

[13]

KOBAYASHI M，MOTOI N.Local Path Planning： Dynamic Window Approach with Virtual Manipulators Considering Dynamic Obstacles[J].IEEE Access，2022，10：17018-17029.

[14]

LI Xinying SHAN Liang，CHANG Lu，et al.DWA Path Planning Algorithm Based on Multi-Objective Particle Swarm Optimization in Complex Environment[J].Journal of National University of Defense Technology，2022，44（4）：52-59.

[15]

YANG Dian，SU Chen，WU Hang，et al.Construction of Novel Self-Adaptive Dynamic Window Approach Combined with Fuzzy Neural Network in Complex Dynamic Environments[J].IEEE Access，2022，10：104375-104383.

2025年第15卷第4期

PDF下载

528

226

引用本文

BibTeX

文章信息

doi: 10.3969/j.issn.2095‒1469.2025.04.11

接收时间：2024-11-24
首发时间：2025-09-10
出版时间：2025-07-20

补充材料

相关文章

2种不同金属材料的力学参数

科 Family	属数 Number of genus	种数 Number of species	占总种数比例 Percentage of total species (%)	属 Genus	种数 Number of species	占总种数比例 Percentage of total species (%)
鹅膏菌科Amanitaceae	2	11	5.26	鹅膏菌属 Amanita	10	4.78
小菇科 Mycenaceae	2	12	5.74	丝盖伞属 Inocybe	5	2.39
多孔菌科 Polyporaceae	8	14	6.70	蜡蘑属 Laccaria	5	2.39
红菇科 Russulaceae	3	23	11.00	小皮伞属 Marasmius	6	2.87
				小菇属 Mycena	11	5.26
				光柄菇属 Pluteus	5	2.39
				红菇属 Russula	17	8.13
				栓菌属 Trametes	5	2.39

关闭全屏

BibTeX
EndNote
RefWorks
TxT

类别	试验单元
1	721
2	631
3	541、532
4	451、442、433、424
5	343、334、352、361

类别

试验单元

721

631

541、532

451、442、433、424

343、334、352、361

试验单元	平均有效粒子数/个
721	8.78
631	8.58
541、532	8.70、8.74
451、442、433、424	8.75、8.71、8.84、8.42
343、334、352、361	8.79、8.70、8.64、8.82

试验单元

平均有效粒子数/个

721

8.78

631

8.58

541、532

8.70、8.74

451、442、433、424

8.75、8.71、8.84、8.42

343、334、352、361

8.79、8.70、8.64、8.82

项目	传统Gmapping算法	改进Gmapping算法
有效粒子数平均值	8.45	8.84
重采样次数	8	9
最优粒子权重统计量	最大值	平均值	最小值	最大值	平均值	最小值
0.401 7	0.112 5	0.002 1	0.416 2	0.118 8	0.000 5
轨迹平均差值	x轴/m	y轴/m	航向角/rad
0.015 5	0.009 7	0.005 3
最优粒子权重差值统计量	最大值	平均值	最小值
0.268 0	0.004 5	-0.310 8
蜕变指标	0.500	0.333
优势保持指标	0.375	0.444

项目

传统Gmapping算法

改进Gmapping算法

有效粒子数平均值

8.45

8.84

重采样次数

最优粒子

权重统计量

最大值

平均值

最小值

最大值

平均值

最小值

0.401 7

0.112 5

0.002 1

0.416 2

0.118 8

0.000 5

轨迹平均

差值

x轴/m

y轴/m

航向角/rad

0.015 5

0.009 7

0.005 3

最优粒子

权重差值

统计量

最大值

平均值

最小值

0.268 0

0.004 5

-0.310 8

蜕变指标

0.500

0.333

优势保持

指标

0.375

0.444

算法	搜索节点/个	路径长度/ 栅格数	转折次数/次
传统A*算法	16	16.31	9
改进A*算法	10	17.24	3

算法

搜索节点/个

路径长度/

栅格数

转折次数/次

传统A*算法

16.31

改进A*算法

17.24

参数	值
预测时间/s	3
最大横摆角速度/（rad/s）	20
最大线速度/（m/s）	1.0
步长/s	0.1
单个步长线速度采样点/个	8
线速度分辨率/（m/s）	0.04
角速度分辨率/（rad/s）	0.035
单个步长角速度采样点/个	5

参数

值

预测时间/s

最大横摆角速度/（rad/s）

最大线速度/（m/s）

1.0

步长/s

0.1

单个步长线速度采样点/个

线速度分辨率/（m/s）

0.04

角速度分辨率/（rad/s）

0.035

单个步长角速度采样点/个

试验	对照组	试验组
线速度	均值	0.866 3	0.865 0
方差	0.009 3	0.009 5
角速度绝对值	均值	0.032 5	0.031 4
方差	0.001 7	0.001 2
路径长度	最大值	0.209 4	0.122 1
均值	113.44	113.64

试验

对照组

试验组

线速度

均值

0.866 3

0.865 0

方差

0.009 3

0.009 5

角速度绝对值

均值

0.032 5

0.031 4

方差

0.001 7

0.001 2

路径长度

最大值

0.209 4

0.122 1

均值

113.44

113.64

关键参数	值
建图速度/（m/s）	0.2
粒子数/个	10
重采样阈值	0.8
有效测距长度/m	25
建图尺寸（长×宽）/m	61×17

关键参数

值

建图速度/（m/s）

0.2

粒子数/个

重采样阈值

0.8

有效测距长度/m

建图尺寸（长×宽）/m

61×17

线段检测项目	毫米级测距仪实测标准值/m	传统Gmapping测量值/m	传统Gmapping测量误差率/%	本文改进Gmapping 算法测量值/m	本文改进Gmapping 算法测量误差率/%
1号	13.460	13.506	-0.342	13.549	-0.661 2
2号	2.756	2.596	5.806	2.803	-1.705 4
3号	14.076	14.067	0.063 9	14.202	-0.895 1
4号	16.780	16.46	1.907	16.722	0.345 6
5号	34.041	33.775	0.781	34.056	-0.044 1
6号	17.432	17.426	0.034	17.452	-0.114 7

线段检测项目

毫米级测距仪实测

标准值/m

传统Gmapping测量值/m

传统Gmapping测量

误差率/%

本文改进Gmapping

算法测量值/m

本文改进Gmapping

算法测量误差率/%

1号

13.460

13.506

-0.342

13.549

-0.661 2

2号

2.756

2.596

5.806

2.803

-1.705 4

3号

14.076

14.067

0.063 9

14.202

-0.895 1

4号

16.780

16.46

1.907

16.722

0.345 6

5号

34.041

33.775

0.781

34.056

-0.044 1

6号

17.432

17.426

0.034

17.452

-0.114 7

角度检测项目	标准值/（°）	传统Gmapping测量值/（°）	传统Gmapping测量误差率/%	本文改进Gmapping 算法测量值/（°）	本文改进Gmapping 算法测量误差率/%
角A	168.536	170.718	-1.295	170.472	-1.1487
角B	5.622	4.508	19.86	4.662	17.075
角C	5.842	4.773	18.299	4.865	16.723 7

角度检测项目

标准值/（°）

传统Gmapping测量值/（°）

传统Gmapping测量

误差率/%

本文改进Gmapping

算法测量值/（°）

本文改进Gmapping

算法测量误差率/%

角A

168.536

170.718

-1.295

170.472

-1.1487

角B

5.622

4.508

19.86

4.662

17.075

角C

5.842

4.773

18.299

4.865

16.723 7

关键参数	值
最大线速度/（m/s）	0.2
最大角速度/（rad/s）	20
控制指令发布频率/Hz	10
线速度采样点/个	6
角速度采样点/个	10
前向模拟时间/s	5
障碍物膨胀半径/m	0.5
加速度/（m/s²）	0.5
角加速度/（rad/s²）	3.5

关键参数

值

最大线速度/（m/s）

0.2

最大角速度/（rad/s）

控制指令发布频率/Hz

线速度采样点/个

角速度采样点/个

前向模拟时间/s

障碍物膨胀半径/m

0.5

加速度/（m/s²）

0.5

角加速度/（rad/s²）

3.5

试验指标结果	值
平均速度/（m/s）	0.192 5
平均角速度绝对值/（rad/s）	0.009
最大角速度/（rad/s）	0.085 9
路程长度/m	70.286
耗时/s	305.5
控制指令发布频率/Hz	10

试验指标结果

值

平均速度/（m/s）

0.192 5

平均角速度绝对值/（rad/s）

0.009

最大角速度/（rad/s）

0.085 9

路程长度/m

70.286

耗时/s

305.5

控制指令发布频率/Hz