基于可通行度估计的无人履带车辆路径规划

doi:10.12382/bgxb.2023.0262

摘要/Abstract

摘要：

针对现有路径规划方法对地形特征考虑不足的问题,以无人履带车辆为研究对象,提出一种基于可通行度估计的路径规划方法。基于卷积长短期记忆(Conv LSTM)网络,从连续轨迹上提取激光雷达点云的空间特征和时间关联特征,融合车辆运动特征,估计地形可通行度。基于地形可通行度,改进A^*算法的节点扩展方式和代价函数,输出满足无碰撞约束和低可通行代价的离散路点;使用无梯度迭代平滑算法减小路径松弛度和可通行度代价;再使用三次B样条曲线对离散路径进行拟合,输出平滑参考路径。以参考路径建立Frenet坐标系,构建基于可通行度代价的安全走廊,在满足无碰撞约束、低可通行度代价的前提下,在走廊内生成满足车辆运动学约束的平滑路径。试验结果表明,所提出的方法能够充分考虑地形特征,提升路径规划结果的稳定性和可通行性。

关键词: 无人履带车辆, 可通行度估计, 路径规划, 改进A^*算法, 二次规划

Abstract:

In response to the insufficient consideration of terrain features in the existing path planning methods, a path planning method based on the estimation of terrain traversability for unmanned tracked vehicles is proposed. In the proposed method, a ConvLSTM-based deep neural network is used to extract the spatial and temporal correlation features of LiDAR point clouds from continuous trajectories, fuse the vehicle motion state features, and estimate the terrain traversability. Based on the terrain traversability, the node expansion mode and cost function of A^* algorithm are improved, and the discrete waypoints that meet the collision-free constraints and the low traversability cost are output. A gradient-free iterative smoothing algorithm is used to reduce the cost of path relaxation and traversability. Then a cubic B-spline curve is used to generate a smooth reference path which is used to establish the Frenet coordinate system. A safety corridor based on the traversability is constructed in the Frenet coordinate system. On the premise of meeting the collision-free constraints and the low traversability cost, a smooth path meeting the vehicle kinematics constraints is generated in the corridor. The experimental results indicate that the proposed method can fully consider the terrain features and improve the stability of path planning results and the traversability of path.

Key words: unmanned tracked vehicle, traversability estimation, path planning, improved A^* algorithm, quadratic programming

中图分类号:

TP242.6

陶俊峰, 刘海鸥, 关海杰, 陈慧岩, 臧政. 基于可通行度估计的无人履带车辆路径规划[J]. 兵工学报, 2023, 44(11): 3320-3332.

TAO Junfeng, LIU Hai’ou, GUAN Haijie, CHEN Huiyan, ZANG Zheng. Path Planning of Unmanned Tracked Vehicle Based on Terrain Traversability Estimation[J]. Acta Armamentarii, 2023, 44(11): 3320-3332.

图/表 23

图1 基于可通行度估计的无人履带车辆路径规划框架

Fig.1 Framework of path planning of unmanned tracked vehicle based on terrain traversability estimation

图2 hdl-graph-slam框架

Fig.2 Framework of hdl-graph-slam

图3 点云转图片

Fig.3 Transformation from 3D point cloud to 2D image

图4 IMU信号处理图

Fig.4 Diagram of IMU signal processing

图5 可通行度估计网络

Fig.5 Traversability estimation network

图6 改进A*搜索

Fig.6 Improved A* search

图7 无梯度迭代示意图

Fig.7 Schematic diagram of gradient-free iteration

图8 可通行走廊构建

Fig.8 Construction of traversable corridor

图9 可通行走廊伪代码

Fig.9 Pseudocode of traversable corridor

图10 Frenet坐标系下车辆运动学模型

Fig.10 Vehicle kinematic model in Frenet coordinate system

图11 试验车辆和试验场

Fig.11 Experimental vehicle and site

表1 试验参数

Table 1 Experimental parameters

参数	数值
车辆行驶速度v/(km·h^-1)	5
节点扩展半径上界R_max/m	3
节点扩展半径下界R_min/m	1.2
无梯度迭代平滑thresh/m	0.0001
自适应采样s_min/m	0.3
自适应采样s_max/m	5
自适应采样κ_m/m^-1	0.02
自适应采样κ_M/m^-1	0.2
A^*距离代价权重ω_m	10
A^*可通行度代价权重ω_i	0.1
无梯度平滑路径长度代价权重ω_s	10
无梯度平滑可通行度代价权重ω_t	0.1
横向采样步长step/m	0.2
可通行走廊最大宽度b_w/m	4
参考线偏差代价权重ω_d	0.01
平滑性代价曲率项权重ω_κ	20
平滑性代价曲率变化率项权重ω_μ	100
终点状态横向位置允许偏差Δl/m	0.2
终点状态航向角允许偏差Δθ/(°)	5
曲率上界κ_max	0.2
曲率变化率上界κ'_max	0.05
可通行度检测pr	0.5

图12 Cp/C随路径长度变化趋势

Fig.12 Changes in Cp/C with s

图13 基于Cp的可通行度代价地图

Fig.13 Traversability cost map based on Cp

表2 Cp和C统计参数

Table 2 Statistical parameters of Cp and C

MSE	MAE	RMSE	R
0.0005	0.0197	0.0236	0.8450

图14 基于栅格高度差的可通行度代价地图

Fig.14 Traversability cost map based on grid height difference

图15 试验场景实拍

Fig.15 Real time shots of experimental sites

图16 C1场景参考路径规划结果自对比

Fig.16 Self comparison of C1 with reference paths

图17 C1场景可通行走廊和优化结果自对比

Fig.17 Self omparison of C1 with traversable orridor and optimized results

图18 C2场景规划结果对比

Fig.18 Comparison of C2 with planning results

图19 车辆俯仰角和侧倾角变化曲线(本文方法)

Fig.19 Change curves of pitch and roll angles (in this article)

图20 车辆俯仰角和侧倾角变化曲线(文献[6]方法)

Fig.20 Change curves of pitch and roll angles (Ref.[6] method)

表3 场景C2规划路径指标对比

Table 3 Comparison of planned route indicators of C2

规划方法	时间/ ms	平均俯仰角/ ((°)·m^-1)	平均侧倾角/ ((°)·m^-1))	平均曲率/ m^-1
本文方法	65	0.4942	1.0089	0.0332
文献[6]方法	105	2.4772	3.3869	0.0283

参考文献 26

[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]	CLAUSSMANN L, REVILLOUD M, GRUYER D, et al. A review of motion planning for highway autonomous driving[J]. IEEE Transactions on Intelligent Transportation Systems, 2019, 21(5): 1826-1848. doi: 10.1109/TITS.6979 URL
[3]	HELMICK D, ANGELOVA A, MATTHIES L. Terrain adaptive navigation for planetary rovers[J]. Journal of Field Robot, 2009, 26(4):391-410. doi: 10.1002/rob.v26:4 URL
[4]	MENG X R, CAO Z Q, LIANG S, et al. A terrain description method for traversability analysis based on elevation grid map[J]. International Journal of Advanced Robotic Systems, 2018, 15(1):1-12.
[5]	SINGH S, SIMMONS R, SMITH T, et al. Recent progress in local and global traversability for planetary rovers[C]// Proceedings 2000 ICRA. Millennium Conference. IEEE International Conference on Robotics and Automation. Symposia Proceedings(Cat. No. 00CH37065).Washington,D.C.,US:IEEE, 2000, 2:1194-1200.
[6]	THORESEN M, NIELSEN N H, MATHIASSEN K, et al. Path planning for UGVs based on traversability hybrid A[J]. IEEE Robotics and Automation Letters, 2021, 6(2): 1216-1223. doi: 10.1109/LSP.2016. URL
[7]	KREBS A, PRADALIER C, SIEGWART R. Comparison of boosting based terrain classification using proprioceptive and exteroceptive data[C]// Proceedings of the Experimental Robotics: The Eleventh International Symposium. Berlin,Germany: Springer, 2009: 93-102.
[8]	SURBLYS V, ŽURAULIS V, SOKOLOVSKIJ E. Estimation of road roughness from data of on-vehicle mounted sensors[J]. Eksploatacja i Niezawodność, 2017, 19(3):369-374. doi: 10.17531/ein URL
[9]	OLIVEIRA F G, SANTOS E R S, NETO A A, et al. Speed-invariant terrain roughness classification and control based on inertial sensors[C]// Proceedings of the 2017 Latin American Robotics Symposium and 2017 Brazilian Symposium on Robotics. Washington,D.C.,US:IEEE, 2017: 1-6.
[10]	TCHAPMI L, CHOY C, ARMENI I, et al. SEGCloud: Semantic segmentation of 3D point clouds[C]// Proceedings of the 2017 International Conference on 3D Vision. Washington,D.C., US: IEEE, 2017: 537-547.
[11]	DOUILLARD B, UNDERWOOD J, KUNTZ N, et al. On the segmentation of 3D LiDAR point clouds[C]// Proceedings of the 2011 IEEE International Conference on Robotics and Automation.Washington,D.C., US:IEEE, 2011: 2798-2805.
[12]	袁夏, 赵春霞. 一种激光雷达可通行区域提取算法[J]. 兵工学报, 2010, 31(12): 1702-1707.
	YUAN X, ZHAO C X. Traversable area extraction using LADAR sensor[J]. Acta Armamentarii, 2010, 31(12): 1702-1707. (in Chinese)
[13]	SHARMA S, BALL J E, TANG B, et al. Semantic segmentation with transfer learning for off-road autonomous driving[J]. Sensors, 2019, 19(11):2577. doi: 10.3390/s19112577 URL
[14]	黄庭鸿, 聂卓赟, 王庆国, 等. 基于区块自适应特征融合的图像实时语义分割[J]. 自动化学报, 2021, 47(5): 1137-1148.
	HUANG T H, NIE Z Y, WANG Q G, et al. Real-time image semantic segmentation based on block adaptive feature fusion[J]. Acta Automatica Sinica, 2021, 47(5):1137-1148. (in Chinese)
[15]	刘忠泽, 陈慧岩, 崔星, 等. 无人平台越野环境下同步定位与地图创建[J]. 兵工学报, 2019, 40(12): 2399-2406. doi: 10.3969/j.issn.1000-1093.2019.12.002
	LIU Z Z, CHEN H Y, CUI X, et al. Real-time LiDAR SLAM in off-road environment for UGV[J]. Acta Armamentarii, 2019, 40(12): 2399-2406. (in Chinese) doi: 10.3969/j.issn.1000-1093.2019.12.002
[16]	彭晓燕, 谢浩, 黄晶. 无人驾驶汽车局部路径规划算法研究[J]. 汽车工程, 2020, 42(1):1-10.
	PENG X Y, XIE H, HUANG J. Research on local path planning algorithm for unmanned vehicles[J]. Automotive Engineering, 2020, 42(1): 1-10. (in Chinese)
[17]	周孝添, 任宏斌, 苏波, 等. 基于微分平坦的分层轨迹规划算法[J]. 兵工学报, 2023, 44(2): 394-405. doi: 10.12382/bgxb.2021.0756
	ZHOU X T, REN H B, SU B, et al. Hierarchical trajectory planning algorithm based on differential flatness[J]. Acta Armamentarii, 2023, 44(2): 394-405. (in Chinese) doi: 10.12382/bgxb.2021.0756
[18]	邓海鹏, 麻斌, 赵海光, 等. 自主驾驶车辆紧急避障的路径规划与轨迹跟踪控制[J]. 兵工学报, 2020, 41(3): 585-594. doi: 10.3969/j.issn.1000-1093.2020.03.020
	DENG H P, MA B, ZHAO H G, et al. Path planning and tracking control of autonomous vehicle for obstacle avoidance[J]. Acta Armamentarii, 2020, 41(3):585-594. (in Chinese) doi: 10.3969/j.issn.1000-1093.2020.03.020
[19]	LI X H, SUN Z P, CAO D P, et al. Development of a new integrated local trajectory planning and tracking control framework for autonomous ground vehicles[J]. Mechanical Systems and Signal Processing, 2017, 87: 118-137. doi: 10.1016/j.ymssp.2015.10.021 URL
[20]	YANG T, XIONG G M, ZHANG Y, et al. A practical planning framework and its implementation for autonomous navigation in off-road environment[C]// Proceedings of the 2019 IEEE Intelligent Vehicles Symposium.Washington,D.C.,US:IEEE, 2019: 1571-1576.
[21]	刘龙龙, 陈慧岩, 刘海鸥, 等. 基于拓扑路网的多挡无人履带平台路径重规划[J]. 兵工学报, 2023, 44(1): 279-289. doi: 10.12382/bgxb.2021.0827
	LIU L L, CHEN H Y, LIU H O, et al. Path replanning of multi-speed unmanned tracked platforms based on topological road network[J]. Acta Armamentarii, 2023, 44(1): 279-289. (in Chinese) doi: 10.12382/bgxb.2021.0827
[22]	王博洋, 关海杰, 龚建伟, 等. 面向异构履带车辆的统一运动规划方法[J]. 兵工学报, 2022, 43(2): 241-251.
	WANG B Y, GUAN H J, GONG J W, et al. Unified motion planning method for heterogeneous tracked vehicles[J]. Acta Armamentarii, 2022, 43(2): 241-251. (in Chinese) doi: 10.3969/j.issn.1000-1093.2022.02.001
[23]	田洪清, 王建强, 黄荷叶, 等. 越野环境下基于势能场模型的智能车概率图路径规划方法[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)
[24]	KOIDE K, MIURA J, MENEGATTI E. A portable 3D LIDAR-based system for long-term and wide-area people behavior measurement[J]. International Journal of Advanced Robotic Systems, 2019, 16(2):1-16.
[25]	SHI X J, CHEN Z R, WANG H, et al. Convolutional LSTM network:a machine learning approach for precipitation nowcasting:arXiv:1506.04214[R]. Ithaca,NY,US:Cornell University, 2015:1506.04214.
[26]	STELLATO B, BANJAC G, GOULART P, et al. OSQP: an operator splitting solver for quadratic programs[J]. Mathematical Programming Computation, 2020, 12(4): 637-672. doi: 10.1007/s12532-020-00179-2