基于拓扑地图与改进A*算法的路径规划及跟踪控制

来飞 ,  刘涛

科学技术与工程 ›› 2026, Vol. 26 ›› Issue (29) :12791-12802

PDF (14561KB)
科学技术与工程 ›› 2026, Vol. 26 ›› Issue (29) :12791-12802 DOI: 10.12404/j.issn.1671-1815.2507740
交通运输

基于拓扑地图与改进A*算法的路径规划及跟踪控制

作者信息 +

Path Planning and Tracking Control Based on Topological Map and Improved A* Algorithm

Author information +
文章历史 +

摘要

针对传统A*算法依赖栅格地图正交节点布局导致复杂环境适配性不足,且节点遍历冗余、搜索时间长及路径平滑性差等问题,提出一种基于拓扑地图与改进A*算法的路径规划与跟踪控制方法。首先,融合障碍物点云地图与OpenDrive地图构建拓扑地图,提取道路中心点与边界点生成核心点集,解决栅格地图冗余存储问题;其次,优化搜索邻域以适配非正交节点场景,并在评估函数中引入自适应权重的动态方向启发式函数,抑制背离目标的无效探索,提升搜索效率;再次,通过曲率约束的三次样条曲线优化路径平滑性,同时基于优化后路径的曲率分布进行速度规划;最后,采用预瞄控制跟踪策略验证所提算法的有效性。仿真结果显示,路径平滑性大幅提升,验证了所提算法在效率、平滑性与安全性上的协同优化效果。

Abstract

To address the issues of the traditional A* algorithm, such as insufficient adaptability to complex environments due to its reliance on the orthogonal node layout of grid maps, as well as redundant node traversal, long search time, and poor path smoothness, a path planning and speed collaborative optimization method based on topological maps and an improved A* algorithm was proposed. Firstly, the obstacle point cloud map and OpenDrive map were fused to construct a topological map, and road center points and boundary points were extracted to generate a core point set, which solves the problem of redundant storage in grid maps. Secondly, the search neighborhood was optimized to adapt to non-orthogonal node scenarios, and a dynamic direction heuristic function with adaptive weights was introduced into the evaluation function to suppress invalid exploration that deviates from the target and improve search efficiency. Thirdly, the path smoothness was optimized using cubic spline curves with curvature constraints, and speed planning was conducted based on the curvature distribution of the optimized path. Finally, a preview tracking strategy was adopted to verify the effectiveness of the path and speed planning. Simulation results show that the path smoothness is significantly improved, verifying the collaborative optimization effect of the method in terms of efficiency, smoothness, and safety.

Graphical abstract

关键词

路径规划 / A*算法 / 拓扑地图 / 样条曲线

Key words

path planning / A* algorithm / topological map / spline curve

引用本文

引用格式 ▾
来飞, 刘涛. 基于拓扑地图与改进A*算法的路径规划及跟踪控制[J]. 科学技术与工程, 2026, 26(29): 12791-12802 DOI:10.12404/j.issn.1671-1815.2507740
LAI Fei, LIU Tao. Path Planning and Tracking Control Based on Topological Map and Improved A* Algorithm[J]. Science Technology and Engineering, 2026, 26(29): 12791-12802 DOI:10.12404/j.issn.1671-1815.2507740

登录浏览全文

4963

注册一个新账户 忘记密码

路径规划是确保机器人安全稳定运行的关键步骤之一。如何在复杂的道路环境中寻找到一条高效、快速的路径是机器人导航领域所需要解决的主要问题,也是机器人安全移动和作业的关键[1]。
目前路径规划算法一般可分为全局路径规划算法和局部路径规划算法[2-3]两类。全局路径规划基于已知的完整环境地图,搜索一条能够避开障碍物并抵达目标点的最优路径。路径规划算法包括A*算法、Dijkstra算法[4-5]、快速探索随机树(rapidly-exploring random tree,RRT)[6]、遗传算法[7]、蚁群算法[8]、粒子群优化算法[9]等。A*算法在全局路径规划算法中应用最为广泛,但其存在随着地图尺寸的增大,算法所消耗的计算资源和路径搜索的时间也会快速增加的问题。研究者针对这些问题提出了许多改进方法。例如,陆辉山等[10]通过插点筛选+跳点搜索,剔除大量冗余节点,节点扩展数量显著低于传统A*算法。Tang等[11]通过提出过滤函数消除不规则路径,分别针对交叉路径和锯齿路径进行优化,但其优化逻辑针对港口网格场景设计,难以直接迁移至非港口场景。Allus等[12]设计改进剪枝算法,去除冗余节点与折线冗余,有效减少路径自交问题,但其角度排序的条件依赖几何直觉,无法保证最优性,需依赖后剪枝修正。Dossis等[13]通过基于估计成本动态调整权重系数平衡搜索效率与探索,但缺乏场景自适应性。Feng等[14]提出融合定向偏置变步长采样策略、分阶段采样、人工势场法及基于三角不等式的剪枝策略,实现了初始路径生成速度的大幅提升及最优路径的优化。上述A*系列算法从代价函数、启发函数、数据结构等方面进行了改进,但是其搜索节点预设的4、8方向离散搜索方向,难以适配复杂场景(城市匝道口等)节点连续变化的场景。
路径规划涉及多种地图类型,包括栅格地图、拓扑地图、语义地图[15]。每种类型的地图都具有影响路径规划策略和结果的独特特征,而基于A*算法的路径规划算法多以栅格地图为主。Sun等[16]采用“主节点+支持节点”混合架构,主节点存储压缩视觉与激光数据保障重定位精度,支持节点以低存储增强地图连通性;通过自适应节点生成、异步地图精修及多传感器融合,实现高精度重定位与高效路径规划。Liu等[17]提出“子图划分+双层表征”的地图构建方式,利用混合地图的多模态信息,使机器人能够在复杂的环境中更有效地导航。Ou等[18]对二维栅格地图进行障碍物膨胀处理,膨胀距离等于机器人直径,从而避免规划路径贴合障碍物边缘,防止机器人在实际运行中与障碍物发生摩擦而受损,显著提升机器人运行安全性。
然而,栅格地图通过离散化网格单元存储环境信息的本质,存在难以规避的性能瓶颈:随着网格精度升高,单元数量呈几何级数增长,不仅导致内存消耗指数级攀升,更使计算资源占用与搜索时间急剧增加,严重削弱路径规划的实时响应能力。同时,栅格地图生成的路径天然呈折线形态,充斥着大量尖角与曲率突变点,曲率分布极不均匀,直接应用于车辆会引发频繁急转向,与车辆动力学约束严重冲突。
为解决这一问题,通常采用曲线拟合技术对分段线性路径进行处理,有效减少路径的突变和抖动,使路径平滑自然[19]。常用的拟合方法包括贝塞尔曲线[20]、样条曲线[21-22]等,但不同方法的适配性存在显著差异:贝塞尔曲线因全局控制特性,调整任意控制点均会影响整条曲线形态,在局部优化时难以兼顾道路边界、曲率上限等全局约束,易导致路径偏离可行域;三次样条曲线通过“控制点仅影响局部曲线段”的特性,规避了贝塞尔曲线的路径越界风险,能通过二阶连续特性保证基础平滑性,但也存在缺乏主动的曲率约束机制,在非结构化环境的急弯、窄路等场景中极易出现局部曲率突变甚至超限的问题,无法直接满足车辆平稳行驶需求。
综上,现针对传统A*算法依赖栅格地图导致复杂环境适配性差、搜索效率低及路径平滑性不足等问题,结合拓扑地图构建与算法多维度改进,实现路径规划性能的协同优化。构建融合障碍物点云与OpenDrive地图的拓扑地图,解决传统栅格地图的冗余存储问题。对A*算法核心机制进行改进:动态邻接搜索突破固定方向约束,实现连续角度节点适配;引入含自适应权重的动态方向启发函数,平衡搜索效率与路径最优性。提出曲率约束的三次样条优化策略,采用连续最小二乘法(sequential least squares programming,SLSQP)求解约束优化模型,并对优化后的路径进行速度规划。采用预瞄跟踪控制策略,对优化后的路径与速度曲线进行仿真测试,验证路径的可跟踪性与行驶稳定性。

1 环境建模

环境建模可以将实际环境转化为计算机可处理的数据结构,如栅格地图、点云、拓扑地图等,这些模型可以精确地表示环境的几何信息,包括障碍物的位置、形状、尺寸等。基于这些信息,路径规划算法可以搜索并生成合适的路径。如点云地图经激光雷达扫描生成,可精准刻画道路环境的三维空间细节,包括障碍物的位置、形态及尺寸;OpenDrive地图则蕴含丰富的道路语义信息,涵盖道路中心线、车道属性等关键内容。二者融合构建的拓扑地图,兼具高精度几何数据与深度语义理解能力,为路径规划提供了单一栅格地图、点云地图等难以实现的全面数据支撑,能够在复杂城、郊道路场景中精准界定道路边界、识别可行驶区域。仅提取道路中心点和边界点作为核心点集,依据“可通行邻居关系”连接节点,避免了栅格地图的冗余存储,在保证道路信息完整的同时大幅降低内存需求,提升系统运行效率。

所提出的拓扑地图的构建方法依次为地图预处理、拓扑地图构建。进行地图预处理的主要目标是获取一组点的集合,拓扑地图构建步骤会在这一组点的集合中用边进行合理的连接,从而构成一张由点和边构成的拓扑图,然后利用图搜索的方法进行路径搜索。实现步骤如下。

首先从OpenDrive地图中提取道路中心点P1,然后采用迭代最近点(iterative closest point,ICP)算法实现点云地图与OpenDrive地图的坐标配准,统一坐标系;再基于随机抽样一致性算法(random sample consensus,RANSAC)从点云数据中拟合道路边界,提取边界点P2,最终生成点集P。如图1(a)为原始点云地图,图1(b)为障碍物点云与道路拓扑结构组成的拓扑地图,通过中心车道线、左边界、右边界三组数据定义道路的物理空间范围。其中灰色点为中心车道点,作为路径规划的核心参考点,由OpenDrive地图提取的二维坐标点集组成。对点云地图进行处理,得到紫色点组成的左右边界,并且建模为不可逾越的障碍,否则生成的路径点将直接横穿边界连接,从而确保路径仅在道路内部生成。代码通过构建道路网络的拓扑关系,每个节点以二维坐标元组表示(x,y)。每个节点连接一定范围内的其他节点,形成“可通行邻居关系”。

2 改进A*算法

2.1 算法原理

A*算法作为一种常用的路径规划算法,在机器人导航、车辆智能驾驶等领域得到了广泛的应用。它主要解决了Dijkstra算法中效率低的问题,该算法计算从单个源节点到所有节点的距离,遍历所有节点,并在所有可能的方向上扩展节点,直到找到目标节点。与之不同的是,A*算法是一种启发式算法,通过合并有关目标点的信息,在保持最佳路径搜索性能的同时,大大提高了路径搜索效率。评估函数f(n)为

f(n)=g(n)+h(n)

式(1)中:g(n)为起点到节点n的实际代价;h(n)为节点n到终点的启发式估计代价。算法通过优先队列(open set)和已访问节点集(closed set)实现高效搜索。距离成本一般采用欧几里得距离计算,公式如下。

$\left\{\begin{array}{l}\mathit{h}\left(\mathit{n}\right)=\sqrt{({\mathit{x}}_{\mathrm{g}\mathrm{o}\mathrm{a}\mathrm{l}}-{\mathit{x}}_{\mathit{n}}{)}^{2}+({\mathit{y}}_{\mathrm{g}\mathrm{o}\mathrm{a}\mathrm{l}}-{\mathit{y}}_{\mathit{n}}{)}^{2}}\\ \mathit{g}\left(\mathit{n}\right)=\sqrt{({\mathit{x}}_{\mathit{n}}-{\mathit{x}}_{\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{r}\mathrm{t}}{)}^{2}+({\mathit{y}}_{\mathit{n}}-{\mathit{y}}_{\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{r}\mathrm{t}}{)}^{2}}\end{array}\right.$

式(2)中:(xn, yn)、 (xstart, ystart)、 (xgoal,ygoal)分别为节点n、起点s和目标点g的坐标。

A*算法的工作流程如下:首先从起点s开始,将其加入开放列表并搜索其周围可遍历的节点,把这些节点加入开放列表并设s为其父节点,随后将s从开放列表移至关闭列表。接着计算开放列表中各节点的预估成本f(n),选择f(n)最小的节点,将其从开放列表移至关闭列表。之后搜索周围的节点,若节点是障碍物或在关闭列表中则忽略;若不在开放列表中,将其加入并计算f(n);若已在开放列表中,则用g(n)计算从s到该节点的新路径成本,若新g(n)值更低,就设为其父节点并重新计算f(n),否则不做更改。不断重复此过程,持续在开放列表中找f(n)最小的节点,将其移至关闭列表并处理相邻可访问节点,直至目标点g出现在开放列表中,此时表明已找到路径,算法结束。

2.2 算法框架

针对传统A*算法在搜索过程中存在搜索节点多、效率低,所有路径方向均为预设的固定角度,仅适用于简单的室内机器人导航等问题,改进A*算法路径规划框架以地图预处理、拓扑建模、动态搜索、曲率优化为核心流程,具体如图2所示。

2.3 动态方向启发式函数设计

为了提高A*算法的搜索效率,在传统的A*算法评估函数的基础上引入自适应权重值τt,代价函数写为

f(n)=g(n)+τth(n)

传统的A*算法评估函数对实际代价g(n)与启发式估计代价f(n)采用1∶1的固定权重配比。这种静态策略存在明显局限:当启发式权重偏高时,算法倾向于依赖h(n)快速推进搜索,虽提升效率却可能牺牲最优性;当实际代价权重占优时,算法虽能保障路径最优,却因过度探索而延长搜索时间。为突破这一局限,设计自适应权重τt动态调节两者占比:当搜索节点靠近起点时,增大τt以强化h(n)的引导作用,加速向目标扩展;当节点靠近目标时,减小τt以突出g(n)的实际约束,保障路径最优性。自适应权重值τt表示为

τt=1+k$\frac{\mathrm{\Delta }\mathit{\theta }}{\mathrm{\pi }}$

式(4)中:Δθ为当前节点运动方向与目标方向的夹角偏差,范围为[0,π];k为惩罚强度参数。

当节点运动方向与目标方向反向偏离时,τt会显著增大,通过强化对h(n)的惩罚性加权,抑制算法向背离目标的方向探索;当方向趋于一致时,τt保持适度值,既维持对方向合理性的基本约束,又避免过度惩罚影响正常搜索;参数k则控制方向偏差的惩罚强度,使算法在保障效率与最优性平衡的同时,能主动规避与目标反向的无效路径分支。新改进的代价函数为

f(n)=g(n)+τth(n)=g(n)+$\left(1+\mathit{k}\frac{\mathrm{\Delta }\mathit{\theta }}{\mathrm{\pi }}\right)$h(n)

2.4 动态邻接搜索

传统的A*算法的搜索逻辑以当前节点为原点,通过预设的4、8或12方向离散搜索方向,如图3所示,逐一评估相邻节点的代价函数,从而筛选出最优路径。由于节点在网格中严格遵循横纵对齐的正交规则,每个搜索方向都能精准对应网格内的相邻节点,无需额外处理角度的连续性问题,因此在工厂车间、室内建筑等规则网格环境中,能实现高效且精准的路径规划。

然而,在所构建的地图模型不再是传统的结构化道路,如图4(a)所示为真实场景,其节点角度不再局限于栅格地图中当前节点的4/8向邻居。这一转变从根本上动摇了传统A*算法的适用前提:其固定方向的搜索机制无法有效处理节点的连续角度朝向。如图4(b)所示为简化的真实场景道路拓扑动态搜索示例,实际节点方向呈连续多样化分布,与传统A*算法预设的固定搜索方向严重不符。这不仅造成相邻节点位置无法通过有限方向精确确定,也导致节点间移动代价在连续角度条件下难以准确计算。传统A*算法所依赖的固定搜索步长和方向无法适应这种灵活拓扑结构。例如,某一节点朝向60°时,其最优相邻节点可能恰好位于该方向某一位置,但传统A*算法缺乏60°方向的搜索能力,只能借助45°或90°方向进行近似搜索,从而引入路径偏差。若强制采用原有搜索机制,将不可避免遗漏最优节点或误算代价函数,最终降低路径规划的准确性与有效性。因此,从搜索方向的覆盖范围、节点空间布局的适配性以及代价计算的精准度3个层面来看,传统A*算法的节点搜索机制已完全丧失对本文地图模型的适配能力。

图4(b)展示了动态邻接搜索的过程,其核心是摒弃固定栅格的固定方向约束,以道路中心线点集中的离散点为节点。以道路中心点集P1为基础,筛选出可通行点作为节点,每个节点坐标为道路中心的坐标;对每个节点,将当前节点与周边一定距离阈值内的其他中心点视为候选邻居,而非固定方向的栅格。随后结合道路左右边界P2,剔除穿越障碍物或超出道路范围的邻接关系,仅保留完全位于可行驶区域内的有效邻居,无需适配固定栅格的步长规则,确保代价评估与节点空间分布的适配性。

3 路径曲率优化与速度规划

路径规划的最终目标不仅是找到从起点到终点的可行路径,更重要的是生成满足车辆动力学约束、平滑且易于跟踪的高质量路径。A*算法搜索所得原始路径通常为折线,存在大量尖角和突变点,导致曲率分布极不均匀,直接使用会造成车辆频繁急转向,影响行驶安全性与舒适性。因此提出基于三次样条曲线的路径曲率优化方法,通过特征点提取、曲线拟合与曲率约束优化,实现路径平滑性与可行性的统一。在完成路径曲率优化后,基于优化后的路径曲率分布进行针对性速度规划,通过曲率与速度的动态适配,进一步提升路径的实用性与行驶安全性。

3.1 约束优化三次样条曲线

三次样条曲线是一种分段多项式曲线,通过特征点集满足位置、一阶导数和二阶导数连续,能有效保证路径平滑性。以特征点对应的参数变量t为分段依据,曲线在区间[ti,ti+1]内的分段表达式为

$\left\{\begin{array}{l}\begin{array}{l}\mathit{x}\left(\mathit{t}\right)={\mathit{c}}_{\mathit{x},\mathit{i}}+{\mathit{d}}_{\mathit{x},\mathit{i}}(\mathit{t}-{\mathit{t}}_{\mathit{i}})+{\mathit{e}}_{\mathit{x},\mathit{i}}(\mathit{t}-{\mathit{t}}_{\mathit{i}}{)}^{2}+\\ \mathit{　}\mathit{　}\mathit{　}{\mathit{f}}_{\mathit{x},\mathit{i}}(\mathit{t}-{\mathit{t}}_{\mathit{i}}{)}^{3}\end{array}\\ \begin{array}{l}\mathit{y}\left(\mathit{t}\right)={\mathit{c}}_{\mathit{y},\mathit{i}}+{\mathit{d}}_{\mathit{y},\mathit{i}}(\mathit{t}-{\mathit{t}}_{\mathit{i}})+{\mathit{e}}_{\mathit{y},\mathit{i}}(\mathit{t}-{\mathit{t}}_{\mathit{i}}{)}^{2}+\\ \mathit{　}\mathit{　}\mathit{　}{\mathit{f}}_{\mathit{y},\mathit{i}}(\mathit{t}-{\mathit{t}}_{\mathit{i}}{)}^{3}\end{array}\end{array}\right.$

式(6)中:t为曲线的参数变量,用于描述曲线的连续变化过程;cx,i、cy,i分别为第i段曲线在x轴、y轴方向的常数项系数,其值等于第i个特征点的坐标; dx,i、dy,i分别为第i段曲线在x轴、y轴方向的一次项系数,对应曲线在t=ti处的一阶导数;ex,i、ey,i分别为第i段曲线在x轴、y轴方向的二次项系数;fx,i、fy,i分别为第i段曲线在x轴、y轴方向的三次项系数,用于控制曲线的弯曲形态;ti为第i个特征点对应的参数值,通常根据特征点间的Euclidean距离均匀分配。系数c、d、e、f通过以下约束求解。

位置连续:

$\left\{\begin{array}{l}\mathit{x}\left({\mathit{t}}_{\mathit{i}+1}\right)={\mathit{a}}_{\mathit{x}(\mathit{i}+1)}\\ \mathit{y}\left({\mathit{t}}_{\mathit{i}+1}\right)={\mathit{a}}_{\mathit{y}(\mathit{i}+1)}\end{array}\right.$

一阶导数连续:

$\left\{\begin{array}{l}\mathit{x}\mathit{\text{'}}\left({\mathit{t}}_{\mathit{i}+1}^{-}\right)=\mathit{x}\mathit{\text{'}}\left({\mathit{t}}_{\mathit{i}+1}^{+}\right)\\ \mathit{y}\mathit{\text{'}}\left({\mathit{t}}_{\mathit{i}+1}^{-}\right)=\mathit{y}\mathit{\text{'}}\left({\mathit{t}}_{\mathit{i}+1}^{+}\right)\end{array}\right.$

二阶导数连续:

$\left\{\begin{array}{l}\mathit{x}″\left({\mathit{t}}_{\mathit{i}+1}^{-}\right)=\mathit{x}″\left({\mathit{t}}_{\mathit{i}+1}^{+}\right)\\ \mathit{y}″\left({\mathit{t}}_{\mathit{i}+1}^{-}\right)=\mathit{y}″\left({\mathit{t}}_{\mathit{i}+1}^{+}\right)\end{array}\right.$

通常采用自然边界:

$\left\{\begin{array}{l}\mathit{x}″\left({\mathit{t}}_{0}\right)=0,\mathit{x}″\left({\mathit{t}}_{\mathit{K}-1}\right)=0\\ \mathit{y}″\left({\mathit{t}}_{0}\right)=0,\mathit{y}″\left({\mathit{t}}_{\mathit{K}-1}\right)=0\end{array}\right.$

通过求解上述线性方程组,可得到各段多项式系数,生成过所有特征点的平滑曲线。

普通三次样条曲线虽能保证平滑性,但可能出现局部曲率超限,从而超过车辆最大转向能力的问题。为此,引入约束优化算法调整特征点位置,在保持路径整体形态的同时控制曲率上限,确保路径满足车辆动力学约束。

首先固定起点和终点,来确保路径首尾位置与原始规划路径一致,以中间特征点的坐标为优化变量,表达式为

θ=$[{\mathit{a}}_{\mathit{x}1},{\mathit{a}}_{\mathit{y}1},{\mathit{a}}_{\mathit{x}2},{\mathit{a}}_{\mathit{y}2},\dots,{\mathit{a}}_{\mathit{x}(\mathit{K}-2)},{\mathit{a}}_{\mathit{y}(\mathit{K}-2)}{]}^{\mathrm{T}}$

式(11)中:axi、ayi分别为第i个中间特征点的x、y轴坐标。

为确保优化后的特征点仍位于道路可行域内,避免路径穿越障碍物,对优化变量添加边界约束。结合前文拓扑地图中提取的道路边界信息,所有中间特征点需满足条件为

xl≤axi≤xr, yb≤ayi≤yt

式(12)中:xl、xr、yb、yt分别为左右、上下坐标上限。

以优化后路径与原始三次样条曲线的偏差最小为目标,确保路径形态基本一致,表达式为

minJ=$\stackrel{\mathit{M}}{\sum _{\mathit{i}=0}}${$\left[{\mathit{x}}_{\mathit{i}}\right(\mathrm{\theta })-{\hat{\mathrm{x}}}_{\mathrm{i}}{]}^{2}$+$\left[{\mathit{y}}_{\mathit{i}}\right(\mathrm{\theta })-{\hat{\mathrm{y}}}_{\mathrm{i}}{]}^{2}$}

式(13)中:M为路径离散点总数;(xi,yi)为优化后路径点;(${\hat{\mathrm{x}}}_{\mathrm{i}}$,${\hat{\mathrm{y}}}_{\mathrm{i}}$)为原始三次样条路径点。

模型的核心约束为路径上所有离散点的曲率不超过车辆最大允许曲率kmax,约束条件表达式为

ki(θ)≤kmax

式(14)中:ki(θ)为优化后路径第i点的曲率,是关于θ的非线性函数,采用平面曲线三点圆弧拟合法计算。

3.2 基于弯道曲率的速度规划

速度规划是车辆路径跟踪系统中的核心模块,其核心目标是根据路径曲率分布特性,结合车辆动力学约束与行驶安全性要求,生成平滑、高效且符合实际驾驶场景的速度。

速度规划的核心在于提前感知前方路径点的曲率特征,通过计算所需减速距离,从根源上避免车辆在弯道内急刹或过度减速,保障行驶平顺性。其中,减速距离计算公式基于车辆运动学原理推导,可确保车辆在进入低车速要求的弯道前,平稳完成速度过渡,避免因减速不及时引发的操控风险。同时,为兼顾停靠安全性,在路径末端设计了渐进式减速策略,使车辆能够平稳降至静止状态,实现精准停靠。为适配不同路况的速度需求,引入曲率阈值分级机制,通过预设阈值将路径划分为直道、缓弯、急弯等场景。针对不同场景配置一一对应的速度因子,遵循“曲率越大,速度因子越小”的原则,具体的曲率阈值分级与速度分配规则如表1所示。

4 仿真分析

4.1 仿真环境及参数设置

为了验证算法的性能,对算法的实时性、路径搜索的最优性、计算资源的消耗量等进行分析。算法的实时性分析所参考的指标是算法在单次路径搜索中所消耗的时间,算法的路径搜索最优性所参考的指标是搜索出的路径的长度(路径点数量);算法的计算资源消耗量的参考指标是路径搜索过程中使用的节点数量。仿真使用Pycharm实现,所使用的python版本为3.10,CPU型号为I5-13500hx,内存为16G。

4.2 路径规划

为全面验证本文改进A*算法的性能,将其分别与Dijkstra算法、传统A*算法、文献[9]提出的改进A*算法、RRT算法以及改进蚁群算法进行对比分析。仿真共设置3个不同的道路场景,以考察各算法在不同复杂度环境下的路径搜索能力、寻优效果及环境适应性,仿真地图如图5所示。

仿真场景一如图5(a)所示,场景以横向主干道路及若干分支道路构成的城市多分支道路作为典型实例。整体结构较为规则,用于模拟常规道路环境。为确保对比条件的一致性,所有对比算法统一设置起点为[-125],终点为[67,49]。

仿真场景二如图5(b)所示,该场景为一条横向主线道路及两条匝道构成的典型实例,该场景涵盖主路直行、匝道汇入与分流等多种典型交通行为。该场景中,统一设置起点为[-170,-110],终点为[-94]。

仿真场景三如图5(c)所示,场景采用更为复杂的道路拓扑结构,包含闭合环路、U形回转以及多路径汇流等典型复杂路况,同时在道路区域内增设若干车辆作为静态障碍物,以模拟实际交通环境中可能出现的障碍干扰。该场景主要用于验证各算法在复杂路网及障碍约束条件下的路径规划能力与鲁棒性。该场景中,统一设置起点为[-400],终点为[-109,-133]。

4.2.1 地图一

地图一的路径规划仿真结果如图6所示。通过对比图6可以看出,各算法能够适应所设计的动态邻接搜索,完成规划任务。具体而言,Dijkstra算法由于缺乏启发式信息,引导性较弱,扩展节点数量较多,搜索范围较大,虽然能够获得较优路径,但计算效率相对较低。传统A*算法在启发函数的引导下有效减少了无效节点扩展,提高了搜索效率,但在分支连接区域仍存在一定程度的路径冗余。改进A*算法在一定程度上优化了路径搜索过程,使路径更加贴合道路结构,但在局部转弯处仍存在轻微偏移。RRT*算法能够生成可行路径,但由于其基于随机采样的搜索机制,路径存在明显弯折及不规则变化,整体平滑性较差。改进蚁群算法得到的路径整体较为连通,但在局部区域仍存在若干尖角,路径连续性有待提升。相比之下,本文改进A*算法在保证搜索效率的同时,有效约束了路径扩展方向,使得规划路径更加贴合道路结构。在主干道路与分支过渡区域,路径转弯更加平滑,整体连续性更好,同时避免了不必要的绕行现象,表现出更优的路径质量与稳定性。

表2为Dijkstra、传统A*、文献[9]改进A*、RRT*、蚁群算法及本文所提出的改进A*算法的性能参数。从表2中的数据可以看出,改进后的A*算法在搜索节点方面优于其他算法,其搜索节点和时间均有了显著降低。与传统A*算法相比,改进A*算法搜索节点减少了49.2%,搜索时间减少了49.3%,与文献[9]改进A*算法相比,搜索节点减少了30.8%,搜索时间减少了29.1%,与RRT*和蚁群算法搜索节点和时间分别减少了38.52%、64.06%和51.85%、75%。

4.2.2 地图二

通过图7可以看出,不同算法在节点扩展数量及扩展方向上存在显著差异。总体而言,各算法均能够完成从起点到终点的路径规划任务,但在路径质量方面表现不一。在主干道路与分支连接区域,RRT*算法生成的路径存在明显的弯折,平滑性较差。同时,改进蚁群算法所得路径中亦出现若干尖角,表明其在路径转折处仍存在局部不连续或转向突变的问题。相比之下,基于启发式搜索的算法在路径连续性与转弯平滑性方面表现更优。

4.2.3 地图三

地图三的路径规划仿真结果如图8所示。在左侧、上侧及右侧区域增设若干静态障碍物,以黄色方块标识。从仿真结果可以看出,RRT*算法规划路径存在与障碍物相交的情况,规划显然失败。从靛青色所示的搜索节点分布可知,大多数算法搜索节点会向有障碍物的地方延伸,而本文所提改进算法仍能保持最少的搜索节点数量,具有最高的规划效率。从表4数据对比中也可看出,本文路径规划算法能够保证搜索节点和时间处于较低水平。

4.3 路径曲率优化

规划的初始路径存在曲率分布极不均匀的问题,直接应用会导致车辆频繁急转向,严重影响行驶安全性与乘坐舒适性。为此,以所提算法生成的路径为原始基准,通过对比贝塞尔曲线、均匀采样、三次样条及约束样条四种方法的优化结果,系统分析路径曲率的优化效果。

以仿真地图一为例,开展路径曲率优化与后续的路径跟踪性能验证;仿真场景的曲率优化、速度规划及跟踪验证,均用相同方法完成。

从路径形态对比图9(a)可见,4条优化曲线在弯道区域的差异显著。结合图9(b)的曲率对比数据,在归一化路径长度0.6~0.7内,贝塞尔曲线、均匀采样、三次样条及原始路径的曲率峰值均达0.2左右,其中贝塞尔曲线的曲率峰值更是高达0.717,不仅显著高于原始路径,且曲线波动剧烈、尖峰密集。从运动学角度分析,此类路径会对执行机构产生高频冲击,直接降低运动平稳性。

进一步结合表5数据可知,经约束样条优化后的路径最大曲率低至0.071,整体曲率水平更低且变化平缓,表明曲率约束能有效将路径曲率控制在更优范围;其最大曲率变化率仅0.041 5,可显著优化路径平滑性,为提升车辆跟踪性能奠定基础。

4.4 路径跟踪

在路径跟踪环节,先对前文规划的路径开展速度规划,随后采用预瞄跟踪策略,同步实现路径与速度的跟踪。如图10所示的速度规划结果中,设置起点和终点的车速为0 km/h,最大车速为30 km/h;右侧颜色条以红色(低速)到蓝色(高速)的渐变方式,用于表征路径上各点的规划速度。可以看到,路径中曲率较大的弯道区域呈现红色,表明这些区域因受曲率约束被规划为低速行驶;而直道或曲率较小的区域呈现蓝紫色,这些区域被规划为高速行驶。

图11为车辆行驶幻影图,直观呈现了车辆在复杂路口环境下的完整行驶轨迹。图12(a)为速度规划及跟踪结果,红色曲线为规划速度,蓝色曲线为实际跟踪速度,二者整体趋势一致,验证了速度规划的可跟踪性。图12(b)展示路径跟踪误差与转向角响应,路径跟踪误差最大不超过0.7 m,转向角响应及时且无过度振荡,体现所提方法在实际跟踪中的稳定性与精度。图12(c)为横摆角速度响应曲线,可见车辆跟踪过程中横摆角速度整体平稳,仅在复杂路口转向阶段出现短暂合理波动,无持续震荡或超限。图12(d)为质心侧偏角曲线,其整体变化与横摆角速度、转向角一致,且波动幅度处于车辆动力学稳定范围内,从车辆动力学核心指标层面进一步验证了所提路径与速度规划方法在实际跟踪时的稳定性与协调性。

5 结论

针对传统A*算法与现有路径规划方法中存在的若干问题,譬如栅格地图建模内存消耗大、节点遍历冗余、搜索效率低、路径平滑性差以及过度依赖正交节点布局等,提出了一套融合拓扑地图构建与A*算法改进的路径规划跟踪控制方法,并通过仿真分析验证了所提算法的有效性。

(1)在地图建模方面,提出基于障碍物点云与OpenDrive地图融合的拓扑地图构建方法。提取道路中心点作为节点参考,同时采集边界点以构建障碍物约束,形成基础点集;再依据“可通行邻居关系”连接节点,形成完整拓扑图。该方法既避免了栅格地图的冗余内存占用,又通过精准采样的方式消除了随机性,实现了对通行区域的完整覆盖。

(2)在A*算法改进方面,从搜索适配性与效率两个层面进行优化。一方面,针对复杂道路(如城市匝道)中节点的任意朝向特性,将传统固定方向邻域搜索扩展为连续角度搜索,有效克服了原有方法在相邻节点定位与代价计算中的偏差问题;另一方面,引入动态方向启发函数,通过自适应权重调节实际代价与启发代价的平衡,以提升路径最优性,同时抑制无效扩展,显著减少冗余遍历。

(3)在路径后处理阶段,针对A*原始路径存在折线段多、曲率突变等问题,提出曲率约束的三次样条优化策略,并对优化后的路径进行速度规划及预瞄跟踪控制。

参考文献

原文顺序 | 出版日期 | 本文引用
[1]

Tang Y, Zakaria M A, Younas M. Path planning trends for autonomous mobile robot navigation: a review[J]. Sensors, 2025, 25(4): 1206.
[2]

Huang J, Chen C, Shen J, et al. A self-adaptive neighborhood search A-star algorithm for mobile robots global path planning[J]. Computers and Electrical Engineering, 2025, 123: 110018.
[3]

Lu Y, Da C. Global and local path planning of robots combining ACO and dynamic window algorithm[J]. Scientific Reports, 2025, 15(1): 9452.
[4]

Ahmad J, Ab Wahab M N. Enhancing the safety and smoothness of path planning through an integration of Dijkstra's algorithm and piecewise cubic Bezier optimization[J]. Expert Systems with Applications, 2025, 289: 128315.
[5]

Zhou X, Yan J, Yan M, et al. Path planning of rail-mounted logistics robots based on the improved Dijkstra algorithm[J]. Applied Sciences, 2023, 13(17): 9955.
[6]

Wang J, Li T, Li B, et al. GMR-RRT*: sampling-based path planning using Gaussian mixture regression[J]. IEEE Transactions on Intelligent Vehicles, 2022, 7(3): 690-700.
[7]

Ab Wahab M N, Nazir A, Khalil A, et al. Improved genetic algorithm for mobile robot path planning in static environments[J]. Expert Systems with Applications, 2024, 249: 123762.
[8]

Cui J, Wu L, Huang X, et al. Multi-strategy adaptable ant colony optimization algorithm and its application in robot path planning[J]. Knowledge-based Systems, 2024, 288: 111459.
[9]

Zhao J, Deng C, Yu H, et al. Path planning of unmanned vehicles based on adaptive particle swarm optimization algorithm[J]. Computer Communications, 2024, 216: 112-129.
[10]

陆辉山, 张燕飞, 王福杰, 等. 融合插点和跳跃点改进A*算法的路径规划[J]. 科学技术与工程, 2023, 23(24): 10387-10393.
[11]

Lu Huishan, Zhang Yanfei, Wang Fujie, et al. Path planning of improved A* algorithm by integrating interpolation and jump points[J]. Science Technology and Engineering, 2023, 23(24): 10387-10393.
[12]

Tang G, Tang C, Claramunt C, et al. Geometric A-star algorithm: an improved A-star algorithm for AGV path planning in a port environment[J]. IEEE Access, 2021, 9: 59196-59210.
[13]

Allus A, Unel M. Angle-based multi-goal ordering and path-planning using an improved A-star algorithm[J]. Robotics and Autonomous Systems, 2025, 190: 105001.
[14]

Chatzisavvas A, Dossis M, Dasygenis M. Optimizing mobile robot navigation based on A-star algorithm for obstacle avoidance in smart agriculture[J]. Electronics, 2024, 13(11): 2057.
[15]

Feng Z, Zhou L, Qi J, et al. DBVS-APF-RRT*: a global path planning algorithm with ultra-high speed generation of initial paths and high optimal path quality[J]. Expert Systems with Applications, 2024, 249: 123571.
[16]

郭晓宇, 刘峰, 黄祥权, 等. 地图加权遗传算法的油井自动巡检机器人路径规划[J]. 科学技术与工程, 2023, 23(7): 2910-2915.
[17]

Guo Xiaoyu, Liu Feng, Huang Xiangquan, et al. Path planning of oil well automatic inspection robot based on map weighted genetic algorithm[J]. Science Technology and Engineering, 2023, 23(7): 2910-2915.
[18]

Sun D, Sun Z, Shao P. Accelerated path planning for large-scale grid maps[J]. IEEE Access, 2024, 12: 121792-121807.
[19]

Liu C, Zhang G, Rong Y, et al. Hybrid metric-feature mapping based on camera and Lidar sensor fusion[J]. Measurement, 2023, 207: 112411.
[20]

Ou Y, Fan Y, Zhang X, et al. Improved A* path planning method based on the grid map[J]. Sensors, 2022, 22(16): 6198.
[21]

孙介东, 张旭东, 张硕, 等. 基于代价地图的非结构化道路参考线生成算法[J]. 汽车工程, 2026, 48(1): 37-49, 60.
[22]

Sun Jiedong, Zhang Xudong, Zhang Shuo, et al. Reference line generation algorithm for unstructured roads based on cost map[J]. Automotive Engineering, 2026, 48(1): 37-49, 60.
[23]

An L, Huang X, Yang P, et al. Adaptive Bézier curve-based path following control for autonomous driving robots[J]. Robotics and Autonomous Systems, 2025, 189: 104969.
[24]

Huo F, Zhu S, Dong H, et al. A new approach to smooth path planning of Ackerman mobile robot based on improved ACO algorithm and B-spline curve[J]. Robotics and Autonomous Systems, 2024, 175: 104655.
[25]

Reda M, Onsy A, Haikal A Y, et al. A novel reinforcement learning-based multi-operator differential evolution with cubic spline for the path planning problem[J]. Artificial Intelligence Review, 2025, 58(5): 142.

基金资助

国家自然科学基金面上项目(52472400)

重庆市自然科学基金面上项目(CSTB2024NSCQ-MSX0493)

重庆市教育委员会科学技术研究计划(KJZD-K202404001)

重庆理工大学研究生科研创新项目(gzlcx20252005)

PDF (14561KB)

17

访问

0

被引

详细
导航
相关文章

/

〈 〉

京ICP备05035734号-4
版权所有 © 《科学技术与工程》编辑部
地址： 北京市海淀区学院南路86号 《科学技术与工程》编辑部　邮编：100081
电话：(010)62118920 　E-mail：stae@vip.163.com
技术支持：北京玛格泰克科技发展有限公司 　
网站管理员登录