越野环境下势场搜索树智能车辆路径规划方法

doi:10.12382/bgxb.2023.0132

摘要/Abstract

摘要：

智能车辆路径规划是智能驾驶的一项关键技术,传统的车辆路径规划方法以最短通行距离或最小通行时间为优化目标,忽视了规划过程中的车辆运动风险。在快速随机搜索树算法的基础上,运用势场模型量化评估车辆运动风险。在快速获得车辆初始运动轨迹的基础上,以车辆运动轨迹的安全性以及通行距离和车辆转角作为运动轨迹评估依据,采用轨迹重构优化方法持续优化车辆运动轨迹。采用场景模拟仿真方法,验证规划轨迹的性能。仿真实验结果表明,在典型场景下,该方法具备平衡车辆运动效率与安全性能的特点,能在越野环境中规避障碍物和环境威胁,所规划的运动轨迹符合车辆运动学特性,运动轨迹的安全性好,通行效率较高。

关键词: 智能车辆, 越野环境, 势场模型, 风险评估, 随机搜索树, 路径规划

Abstract:

Path planning is a key technology for intelligent vehicles. The traditional vehicle path planning method takes the shortest traveling distance or minimum traveling time as the optimization goal, ignoring the risk of vehicle motion. A potential field based rapidly-exploring random tree (RRT) algorithm is proposed. The potential field model is used to quantitatively evaluate the driving risk, and a low-risk initial vehicle driving trajectory is obtained efficiently using the RRT algorithm. And then a trajectory reconstruction optimization method is adopted to continuously optimize the vehicle driving trajectory based on the driving safety, traveling distance and turning angle. The scenario simulation is used to verify the performance of the planning solution. The simulated results show that the proposed algorithm can balance the path planning efficiency and safety performance, while avoiding the obstacles and environmental threats in off-road environment. The planned trajectories conform to the vehicle kinematics features, good safety and high traveling efficiency.

Key words: intelligent vehicle, off-road environment, potential field model, risk evaluation, rapidly-exploring random tree, path planning

中图分类号:

TJ810.2

田洪清, 马明涛, 张博, 郑讯佳. 越野环境下势场搜索树智能车辆路径规划方法[J]. 兵工学报, 2024, 45(7): 2110-2127.

TIAN Hongqing, MA Mingtao, ZHANG Bo, ZHENG Xunjia. Potential Field Exploring Tree Path Planning for Intelligent Vehicle in Off-road Environment[J]. Acta Armamentarii, 2024, 45(7): 2110-2127.

图/表 35

图1 PFT*算法功能框架

Fig.1 PFT* algorithm framework

图2 场景势场图

Fig.2 Scenario potential field map

图3 PFT*路径规划算法流程图

Fig.3 PFT* path planning algorithm flowchart

图4 PFT*算法与RRTs算法对比

Fig.4 Comparison between PFT* and RRTs

图5 智能采样与均布采样搜索方向对比

Fig.5 Comparison of searching directions of intelligent sampling and general sampling methods

图6 智能采样流程图

Fig.6 Intelligent sampling flowchart

图7 车辆运动轨迹生成流程图

Fig.7 Vehicle trajectory generation flowchart

图8 车辆运动风险评估

Fig.8 Vehicle path risk evaluation

图9 RRT重构优化

Fig.9 RRT reconstruction

图10 RRT父节点重构流程图

Fig.10 RRT parent node reconstruction

图11 RRT子节点重构流程图

Fig.11 Flowchart of RRT child node reconstruction

表1 路径规划仿真参数

Table 1 Path planning simulation parameters

车辆参数	数值
外形尺寸(长×宽×高)/mm	4955×2137×1920
轴距/mm	3300
整车质量/kg	3200
行驶车速/(km·h^-1)	30

图12 仿真实验场景图

Fig.12 Simulation experiment scenarios

图13 场景仿真实验过程

Fig.13 Scenario simulation experiment process

图14 仿真实验轨迹对比(场景1)

Fig.14 Comparison of simulation experimental trajectories (Scenario1)

表2 仿真实验结果数据对比(场景1)

Table 2 Comparisonof simulated results (Scenario 1)

算法	距离/m	势场值	最近障碍距离/m	通行代价
FMT^*	1999	161	2.17	2160
Q-RRT^*	1876	265	1.62	2141
A-PRM	2102	60	12.22	2162
A^*	1946	415	5.0	2361
PFT^*	2018	33	22.80	2051

图15 仿真实验数据对比(场景1)

Fig.15 Comparison of simulated trajectories comparison (Scenario 1)

图16 仿真实验轨迹对比(场景2)

Fig.16 Comparison of simulated trajectories (Scenarios 2)

表3 仿真实验结果数据对比(场景2)

Table 3 Comparison of simulated results (Scenario 2)

算法	距离/m	势场值	越野距离/m	通行代价
FMT^*	702	503	560	1 205
Q-RRT^*	683	498	565	1 181
A-PRM	1 160	98	60	1 258
A^*	1 457	267	0	1 724
PFT^*	908	240	308	1 148

图17 仿真实验数据对比(场景2)

Fig.17 Comparison of simulated trajectories (Scenario 2)

图18 仿真实验轨迹对比(场景3)

Fig.18 Comparison of simulated trajectories (Scenario 3)

表4 仿真实验结果数据对比(场景3)

Table 4 Comparison of simulated results (Scenario 3)

算法	距离/m	势场值	最近障碍距离/m	越野距离/m	通行代价
FMT^*	1417	971	2.95	235	2389
Q-RRT^*	1341	676	1.82	200	2017
A-PRM	1628	318	17.04	70	1946
A^*	1616	326	13.41	0	1942
PFT^*	1629	109	23.30	42	1738

图19 仿真实验数据对比(场景3)

Fig.19 Comparison of simulated trajectories (Scenario 3)

图20 仿真实验轨迹对比(场景4)

Fig.20 Comparison of simulated trajectories (Scenario 4)

表5 仿真实验结果数据对比(场景4)

Table 5 Comparison of simulated results (Scenario 4)

算法	距离/ m	势场值	最近障碍距离/m	越野距离/m	最近威胁距离/m	通行代价
FMT^*	1138	230	3.41	0	105	1368
Q-RRT^*	1046	158	1.76	0	150	1204
A-PRM	1147	326	1.94	98	139	1473
A^*	1102	126	13.44	0	123	1228
PFT^*	1128	41	18.21	0	110	1169

图21 仿真实验数据对比(场景3)

Fig.21 Comparison of simulated trajectories (Scenario 3)

图22 仿真实验轨迹对比(场景5)

Fig.22 Comparison of simulated trajectories (Scenario 5)

表6 仿真实验结果数据对比(场景5)

Table 6 Comparison of simulated results (Scenario 5)

算法	距离/m	势场值	最近障碍距离/m	越野距离/m	通行代价
FMT^*	1 302	668	3.30	80	1 970
Q-RRT^*	1 244	530	1.96	0	1 774
A-PRM	1 276	236	11.11	0	1 512
A^*	1 262	186	5.01	0	1 448
PFT^*	1 338	97	18.12	0	1 435

图23 仿真实验数据对比(场景5)

Fig.23 Comparison of simulated trajectories (Scenario 5)

图24 仿真实验轨迹对比(场景6)

Fig.24 Comparison of simulated trajectories (Scenario 6)

表7 仿真实验结果数据对比(场景6)

Table 7 Comparison of simulated results (Scenario 6)

算法	距离/m	势场值	最近障碍距离/m	越野距离/m	最近威胁距离/m	通行代价
FMT^*	868	514	4.89	185	49	1382
Q-RRT^*	838	438	9.89	125	50	1276
A-PRM	967	232	29.31	70	82	1199
A^*	915	490	13	0	0.45	1405
PFT^*	914	132	34.15	60	89	1046

图25 仿真实验数据对比(场景6)

Fig.25 Comparison of simulated trajectories(Scenario 6)

图26 高程地图轨迹对比(场景7)

Fig.26 Trajectory comparison in elevation map (Scenario7)

表8 高程地图规划数据对比(场景7)

Table 8 Planning data comparison in elevation map (Scenario 7)

算法	距离/m	势场值	最近障碍距离/m	通行代价
FMT^*	2 218	410	2.9	2 628
Q-RRT^*	2 188	144	3.3	2 332
A-PRM	2 359	180	16.1	2 539
A^*	2 403	144	13.4	2 547
PFT^*	2 274	0	44.2	2 274

图27 高程地图规划数据对比(场景7)

Fig.27 Planning data comparison in elevation map(Scenario 7)

参考文献 35

[1]	陈慧岩, 张玉. 军用地面无人机动平台技术发展综述[J]. 兵工学报, 2014, 35(10):1696-1706. doi: 10.3969/j.issn.1000-1093.2014.10.026
	CHEN H Y, ZHANG Y. An overview of research on military unmanned ground vehicles[J]. Acta Armamentarii, 2014, 35(10):1696-1706. (in Chinese)
[2]	李立, 徐志刚, 赵祥模, 等. 智能网联汽车运动规划方法研究综述[J]. 中国公路学报, 2019, 32(6):20-33. doi: 10.19721/j.cnki.1001-7372.2019.06.002
	LI L, XU Z G, ZHAO X M, et al. Review on motion planning methods of intelligent connected vehicles[J]. China Journal of Highway and Transport, 2019, 32(6):20-33. (in Chinese) doi: 10.19721/j.cnki.1001-7372.2019.06.002
[3]	采国顺, 刘昊吉, 冯吉伟, 等. 智能汽车的运动规划与控制研究综述[J]. 汽车安全与节能学报, 2021, 12(3): 279-297.
	CAI G S, LIU H J, FENG J W, et al. Review on the research of motion planning and control for intelligent vehicles[J]. Automotive Safety and Energy, 2021, 12(3):279-297. (in Chinese)
[4]	孔国杰, 冯时, 于会龙, 等. 无人集群系统协同运动规划技术综述[J]. 兵工学报, 2023, 44(1):11-26. doi: 10.12382/bgxb.2022.0930
	KONG G J, FENG S, YU H L, et al. A review on cooperative motion planning of unmanned vehicles[J]. Acta Armamentarii, 2023, 44(1):11-26. (in Chinese) doi: 10.12382/bgxb.2022.0930
[5]	VDIJKSTRA E W. A note on two problems in connexion with graphs[J]. Numerische Mathematik, 1968, 1(1):100-107.
[6]	HART P E, NILSSON N J, RAPHAEL B. A formal basis for the heuristic determination of minimum cost paths[J]. IEEE Transactions on Systems Science & Cybernetics, 1972, 4(2):28-29.
[7]	KOENIG S, LIKHACHEV M, FURCY D. Lifelong planning A^*[J]. Artificial Intelligence, 2004, 15(2):93-146.
[8]	KOENIG S, LIKHACHEV M. Fast replanning for navigation in unknown terrain[J]. IEEE Transactions on Robotics, 2005, 21(3):354-363.
[9]	LIKHACHEV M, FERGUSON D. Planning long dynamically feasible maneuvers for autonomous vehicles[J]. The International Journal of Robotics Research, 2009, 28(8):933-945.
[10]	田洪清, 王建强, 黄荷叶, 等. 越野环境下基于势能场模型的智能车概率图路径规划方法[J]. 兵工学报, 2021, 42(7):1496-1505.
	TIAN H Q, WANG J Q, HUANG H Y, et al. Probabilistic roadmap method for path planning of intelligent vehicle based on artificial potential field model in off-road environment[J]. Acta Armamentarii, 2021, 42(7):1496-1505. (in Chinese)
[11]	KARAMAN S, FRAZZOLI E. Sampling-based algorithms for optimal motion planning[J]. The International Journal of Robotics Research, 2011, 30(7):846-894.
[12]	MASHAYEKHI R, IDRIS M Y I, ANISI M H, et al. Informed RRT^*-connect:an asymptotically optimal single-query path planning method[J]. IEEE Access, 2020, 8:19842-19852.
[13]	WANG X W, WEI J B, ZHOU X, et al. AEB-RRT^:an adaptive extension bidirectional RRT^ algorithm[J]. Autonomous Robots, 2022, 46(6):685-704.
[14]	JEONG I B, LEE S J, KIM J H. Quick-RRT^:triangular inequality-based implementation of RRT^ with improved initial solution and convergence rate[J]. Expert Systems with Applications, 2019, 123:82-90.
[15]	CHI W Z, WANG C Q, WANG J K, et al. Risk-DTRRT-based optimal motion planning algorithm for mobile robots[J]. IEEE Transactions on Automation Science and Engineering, 2018, 16(3):1271-1288.
[16]	JANSON L, SCHMERLING E, CLARK A, et al. Fast marching tree:a fast marching sampling-based method for optimal motion planning in many dimensions[J]. The International Journal of Robotics Research, 2015, 34(7):883-921.
[17]	周维, 过学迅, 裴晓飞, 等. 基于RRT与MPC的智能车辆路径规划与跟踪控制研究[J]. 汽车工程, 2020, 42(9): 1151-1158.
	ZHOU W, GUO X X, PEI X F, et al. Study on path planning and tracking control for intelligent vehicle based on RRT and MPC[J]. Automotive Engineering, 2020, 42(9): 1151-1158. (in Chinese)
[18]	冯来春, 梁华为, 杜明博, 等. 基于A^*引导域的RRT智能车辆路径规划算法[J]. 计算机系统应用, 2017, 26(8):127-133.
	FENG L C, LIANG H W, DU M B, et al. Guiding-area RRT path planning algorithm based on A^* for intelligent vehicle[J]. Computer Systems & Applications, 2017, 26(8):127-133. (in Chinese)
[19]	LI Y J, WEI W, GAO Y, et al. PQ-RRT*:an improved path planning algorithm for mobile robots[J]. Expert Systems with Applications, 2020, 152:113425.
[20]	GE Q Y, LI A J, LI S H, et al. Improved bidirectional RRT^* path planning method for smart vehicle[J]. Mathematical Problems in Engineering, 2021:1-14.
[21]	LI B H, CHEN B D. An adaptive rapidly-exploring random tree[J]. IEEE/CAA Journal of Automatica Sinica, 2021, 9(2):283-294.
[22]	LI Y, CUI R X, LI Z J, et al. Neural network approximation based near-optimal motion planning with kinodynamic constraints using RRT[J]. IEEE Transactions on Industrial Electronics, 2018, 65(11):8718-8729.
[23]	GAMMELL J D, SRINIVASA S S, BARFOOT T D. Informed RRT^*:optimal sampling-based path planning focused via direct sampling of an admissible ellipsoidal heuristic[C]//Proceedings of 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems.Chicago, IL, US:IEEE/RSJ, 2014:2997-3004.
[24]	YUAN L H, ZHAO J, LI W H, et al. Improved Informed-RRT^* based path planning and trajectory optimization for mobile robots[J]. International Journal of Precision Engineering and Manufacturing, 2023, 24(3):435-446.
[25]	张立彬, 林后凯, 谭大鹏. 基于栅格空间的自适应GB_RRT^*机械臂路径规划[J]. 计算机集成制造系统, 2022, 28(6):28-37.
	ZHANG L B, LIN H K, TAN D P T. Adaptive GB_RRT^* manipulator path planning based on grid space[J]. Computer Integrated Manufacturing Systems, 2022, 28(6):28-37. (in Chinese)
[26]	GAMMELL J D, BARFOOT T D, SRINIVASA S S. Batch informed trees (BIT^*): informed asymptotically optimal anytime search[J]. The International Journal of Robotics Research, 2020, 39(5): 543-567.
[27]	STRUB M P, GAMMELL J D. Adaptively informed trees (AIT):fast asymptotically optimal path planning through adaptive heuristics[C]//Proceedings of 2020 IEEE International Conference on Robotics and Automation.Paris, France:IEEE, 2020:3191-3198.
[28]	KANG J G, LIM D W, CHOI Y S, et al. Improved RRT-connect algorithm based on triangular inequality for robot path planning[J]. Sensors, 2021, 21(2):333-343.
[29]	SHEN H H, XIE W F, TANG J Y, et al. Adaptive manipulability-based path planning strategy for industrial robot manipulators[J]. IEEE/ASME Transactions on Mechatronics, 2023, 28(3):1742-1753.
[30]	XU J J, PARK K S. Kinematic performance-based path planning for cable-driven parallel robots using modified adaptive RRT[J]. Microsystem Technologies, 2022, 28(10):2325-2336.
[31]	潘作栋, 周悦, 郭威, 等. 基于CB-RRT^*算法的滩涂履带车路径规划[J]. 兵工学报, 2023, 2023, 21(4):1496-1505.
	PAN Z D, ZHOU Y, GUO W, et al. Path planning of tidal flat robot based on CB-RRT^* algorithm[J]. Acta Armamentarii, 2023, 21(4):1496-1505. (in Chinese)
[32]	KATIYAR S, DUTTA A. Dynamic path planning over CG-Space of 10DOF rover with static and randomly moving obstacles using RRT^* rewiring[J]. Robotica, 2022, 40(8):2610-2629.
[33]	WANG H, LI G Q, HOU J, et al. A path planning method for underground intelligent vehicles based on an improved RRT* algorithm[J]. Electronics, 2022, 11(3): 294.
[34]	李爱娟, 李舜酩, 李殿荣, 等. 智能车运动轨迹规划中的关键技术研究现状[J]. 机械科学与技术, 2013, 32(7):1022-1026.
	LI A J, LI S M, LI D R, et al. On the trajectory planning’s key technologies for intelligent vehicle[J]. Mechanical Science and Technology for Aerospace Engineering, 2013, 32(7):1022-1026. (in Chinese)
[35]	田洪清, 丁峰, 郑讯佳, 等. 基于势能场虚拟力的智能网联车辆运动规划[J]. 汽车工程, 2021, 43(4):518-526.
	TIAN H Q, DING F, ZHENG X J, et al. Motion planning based on virtual force of potential field for intelligent connected vehicles[J]. Automotive Engineering, 2021, 43(4):518-526. (in Chinese)