改进PSO-PH-RRT*算法在智能车路径规划中的应用

doi:10.12068/j.issn.1005-3026.2025.20239047

摘要/Abstract

摘要：

在机器人控制、智能车自主导航等应用场景中，路径规划需要考虑到环境中的障碍物、地形等因素.针对路径规划中快速拓展随机树（RRT）算法拓展目标方向盲目、效率较低的问题，提出了基于粒子群算法优化的均匀概率快速拓展随机树（PSO-PH-RRT*）算法.该算法在基于均匀概率的快速拓展随机树（PH-RRT*）算法的基础上，利用粒子群算法更新方向概率作为随机树节点的速度方向，从而改善了节点的位置更新策略，并将节点到目标向量的距离和轨迹平滑度作为粒子群算法的适应度函数.最后在多种障碍环境下进行仿真.结果表明，PSO-PH-RRT*算法能大大减少迭代时间成本，同时改善路径长度和平滑度.

关键词: 路径规划, RRT算法, 改进粒子群优化算法, 目标向量, 代价函数, 适应度函数

Abstract:

In application scenarios like robot control and autonomous navigation of intelligent vehicle， path planning needs to account for factors including obstacles and terrain. To address the issues of directionless expansion target and low efficiency in rapidly-exploring random tree （RRT） algorithm in path planning， a particle swarm optimization for probabilistically homogeneous rapidly-exploring random tree （PSO-PH-RRT*） algorithm is proposed. This algorithm base on the probabilistically homogeneous rapidly-exploring random tree （PH-RRT*） algorithm by using the particle swarm optimization algorithm to update the probability of direction as the velocity direction for random tree nodes， thereby improving the node position update strategy. It also uses the distance between the node and the target vector， along with trajectory smoothness， as the fitness function in the particle swarm optimization algorithm. Finally， simulations across various scenarios demonstrate that the PSO-PH-RRT* algorithm can significantly reduce iteration time costs while improving path length and smoothness.

Key words: path planning, RRT algorithm, improved particle swarm optimization algorithm, target vector, cost function, fitness function

中图分类号:

TP 242.6

蒋启龙, 许健. 改进PSO-PH-RRT*算法在智能车路径规划中的应用[J]. 东北大学学报（自然科学版）, 2025, 46(3): 12-19.

Qi-long JIANG, Jian XU. Application of Improved PSO-PH-RRT* Algorithm in Intelligent Vehicle Path Planning[J]. Journal of Northeastern University(Natural Science), 2025, 46(3): 12-19.

图/表 13

图1 RRT*算法节点拓展示意图

Fig.1 Node expansion schematic diagram of RRT* algorithm

图2 RRT*算法拓展随机树流程图

Fig.2 Expanded random tree flowchart of RRT* algorithm

图3 RRT*算法重布线流程图

Fig.3 Rewire flowchart of RRT* algorithm

图4 PH-RRT*算法节点拓展示意图

Fig.4 Node expansion schematic diagram of PH-RRT* algorithm

图5 代价函数定义

Fig.5 Definition of cost function

图6 方向约束角γ定义

Fig.6 Definition of direction constraint angle γ

图7 PSO-PH-RRT*算法流程

Fig.7 PSO-PH-RRT* algorithm flowchart

图8 PSO-PH-RRT*算法适应度函数值变化曲线

Fig.8 PSO-PH-RRT* algorithm fitness function value change curve

表1 优化算法与代价函数对比实验方案

Table 1 Optimization algorithm and cost function comparative experiment plans

算法	步长代价函数	距离代价函数
原始RRT*算法	RRT*（lC）	RRT*（E）
均匀概率改进的RRT*算法	PH-RRT*（lC）	PH-RRT*（E）
粒子群优化的均匀概率RRT*算法	PSO-PH-RRT*（lC）	PSO-PH-RRT*（E）

表2 程序中的主要参数

Table 2 Main parameters in the program

参数	设定值
随机树增加步长ρ	5
均匀概率参数值P	0.9
补偿范围r	5
迭代次数h	50
学习因子c_h	1
惯性因子初值w₀	0.5

图9 随机障碍环境的重复实验箱形图（a）—单通路障碍环境下的路径长度；（b）— 随机障碍环境下的迭代时间.

Fig.9 Repeated experimental box plot in random obstacle environment

图10 实验结果统计图（a）— 路径长度；（b）— 搜索时间；（c）— 平滑度.

Fig.10 Experiment results statistical chart

图11 规划路径结果（a）—RRT*（E）；（b）— PH-RRT*（E）；（c）—PSO-PH-RRT*（E）；（d）—RRT*（E）；（e）—PH-RRT*（E）；（f）—PSO-PH-RRT*（E）；（g）—RRT*（E）；（h）—PH-RRT*（E）；（i）—PSO-PH-RRT*（E）.

Fig.11 Planning paths consequence

参考文献 17

1	车建涛，高方玉，解玉文，等. 基于Dijkstra算法的水下机器人路径规划［J］. 机械设计与研究， 2020， 36（1）：44-48.
	Che Jian-tao， Gao Fang-yu， Xie Yu-wen， et al. Path planning of underwater robot based on Dijkstra algorithm［J］. Machine Design & Research， 2020， 36（1）：44-48.
2	Tang F. Coverage path planning of unmanned surface vehicle based on improved biological inspired neural network［J］. Ocean Engineering， 2023， 278：114354.
3	Madridano Á， Kaff A， Martín D， et al. Trajectory planning for multi-robot systems：methods and applications［J］. Expert Systems with Applications， 2021， 173：114660.
4	La S M， Kuffner J J Jr. Randomized kinodynamic planning［J］. The International Journal of Robotics Research， 2001， 20（5）：378-400.
5	Urmson C， Simmons R. Approaches for heuristically biasing RRT growth［C］//RSJ International Conference on Intelligent Robots and Systems. Las Vegas， 2003：1178-1183.
6	Li Z， Ma H B， Zhang X F， et al. Path planning of the dual-arm robot based on VT-RRT algorithm［C］//Chinese Control Conference. Guangzhou： 2019：4359-4364.
7	Kuffner J J， LaValle S M. RRT-connect： an efficient approach to single-query path planning［C］//IEEE International Conference on Robotics and Automation. San Francisco， 2000：995-1001.
8	Karaman S， Frazzoli E. Sampling-based algorithms for optimal motion planning［J］. The International Journal of Robotics Research， 2011， 30（7）：846-894.
9	Wang B Y， Liu Z， Li Q B， et al. Mobile robot path planning in dynamic environments through globally guided reinforcement learning［J］. IEEE Robotics and Automation Letters， 2020， 5（4）： 6932-6939.
10	Noreen I， Khan A， Habib Z. A comparison of RRT， RRT* and RRT*-smart path planning algorithms［J］. International Journal of Computer Science and Network Security， 2016， 16（10）：20-27.
11	Hess R， Jerg R， Lindeholz T， et al. SRRT*-a probabilistic optimal trajectory planner for problematic area structures［J］. IFAC-PapersOnLine， 2016， 49（30）：331-336.
12	白晓兰，周文全，张振朋，等. 基于启发式粒子群算法的机器人平滑路径规划［J］. 组合机床与自动化加工技术， 2022（8）：44-47，52.
	Bai Xiao-lan， Zhou Wen-quan， Zhang Zhen-peng， et al. Smooth path planning of wheeled robot based on heuristic particle swarm optimization algorithm［J］. Modular Machine Tool & Automatic Manufacturing Technique， 2022（8）：44-47，52.
13	屈新怀，单笛，孟冠军. 基于靠近目标粒子群算法的AGV路径规划［J］. 合肥工业大学学报（自然科学版）， 2022， 45（1）：1-6.
	Qu Xin-huai， Shan Di， Meng Guan-jun. AGV path planning based on particle swarm optimization approaching the target ［J］. Journal of Hefei University of Technology（Natural Science Edition）， 2022， 45（1）：1-6.
14	Lin S W， Liu A， Wang J G， et al. An intelligence-based hybrid PSO-SA for mobile robot path planning in warehouse［J］. Journal of Computational Science， 2023， 67：101938.
15	Krell E， Sheta A， Balasubramanian A P R， al et， Collision-free autonomous robot navigation in unknown environments utilizing PSO for path planning［J］.Journal of Artificial Intelligence and Soft Computing Research， 2019， 9（4）：267-282.
16	彭君. 改进RRT算法在移动机器人路径规划中的应用研究［D］. 南京：南京邮电大学， 2022.
	Peng Jun. Application of improved RRT algorithm in mobile robot path planning［D］.Nanjing： Nanjing University of Posts and Telecommunications， 2022.
17	左国玉，陈国栋，刘月雷，等. 基于均匀概率的目标启发式RRT机械臂路径规划方法［J］. 北京工业大学学报， 2022， 48（8）：812-821.
	Zuo Guo-yu， Chen Guo-dong， Liu Yue-lei， et al. Target heuristic RRT based on uniform probability for manipulator path planning［J］. Journal of Beijing University of Technology， 2022， 48（8）：812-821.

[1]	裴玉龙, 翟双柱. 冰雪路面条件下多场景自动驾驶车辆主动避障路径规划[J]. 东北大学学报（自然科学版）, 2025, 46(3): 1-11.
[2]	徐金华, 汪飞, 韩飞, 李岩. 一种多车辆协同多植保无人机作业路径规划方法[J]. 东北大学学报（自然科学版）, 2024, 45(2): 296-304.
[3]	赵俊涛，罗小川，刘俊秘. 改进鲸鱼优化算法在机器人路径规划中的应用[J]. 东北大学学报（自然科学版）, 2023, 44(8): 1065-1071.
[4]	刘晓熙，姜慧研，骆敏. 面向肝癌消融术的多约束最优穿刺路径规划算法[J]. 东北大学学报（自然科学版）, 2023, 44(7): 922-930.
[5]	赵钊，原培新，唐俊文，陈锦林. 基于改进SNN-HRL的智能体路径规划算法[J]. 东北大学学报（自然科学版）, 2023, 44(11): 1548-1555.
[6]	王海芳，崔阳阳，李鸣飞，李广宇. 基于改进RRT*FN的移动机器人路径规划算法[J]. 东北大学学报（自然科学版）, 2022, 43(9): 1217-1225.
[7]	王振宇，张荣闯，于天彪. 圆柱直齿轮铣削加工无干涉刀具路径规划[J]. 东北大学学报（自然科学版）, 2022, 43(7): 988-995.
[8]	刘启冉，连静，陈实，范蓉. 考虑交互轨迹预测的自动驾驶运动规划算法[J]. 东北大学学报（自然科学版）, 2022, 43(7): 930-936.
[9]	张禹，李东升，王志伟，巩亚东. 基于图论和改进Dijkstra算法的STEP-NC复杂型腔最短刀具路径生成方法[J]. 东北大学学报:自然科学版, 2020, 41(9): 1285-1291.
[10]	刘杰，闫清东，马越，唐正华. 基于蚁群几何优化算法的全局路径规划[J]. 东北大学学报:自然科学版, 2015, 36(7): 923-928.
[11]	季策，王艳茹，王晓宇. 一种基于标准峭度的新型复数盲分离算法[J]. 东北大学学报:自然科学版, 2015, 36(5): 614-617.
[12]	黄月，吴成东，董晶晶，贾子熙. 基于WSN的灾难现场最优逃生路径规划[J]. 东北大学学报(自然科学版), 2013, 34(2): 162-165.
[13]	张琦，马家辰，谢玮，马立勇. 基于改进蚁群算法的移动机器人路径规划[J]. 东北大学学报（自然科学版）, 2013, 34(11): 1521-1524.
[14]	高巍;赵海;罗桂兰;宋纯贺;. 一种AAPF算法及其在多机器人路径规划中的应用[J]. 东北大学学报(自然科学版), 2009, 30(5): 644-647.