未知环境下基于控制障碍函数的无人车轨迹规划

doi:10.12382/bgxb.2023.0882

摘要/Abstract

摘要：

无人车凭借其适应性强、成本低的优点,成为代替人类前往高风险、高污染区域执行任务的一种切实可行的方案。轨迹规划作为无人车任务系统的关键技术,其目的是根据任务目标制定满足约束条件的运动轨迹。然而,目前的导航控制器主要依赖于预先获取的地图和先验知识,对未知环境的应对能力有所欠缺,无法适应复杂多变的任务环境。为此,提出了一种基于控制障碍函数的轨迹规划方法,用于解决无人车在未知环境下的自主避障、轨迹规划问题。该方法前端利用激光雷达感知环境深度信息,后端采用最小二乘支持向量机拟合障碍物边界,以估计控制障碍函数,并对负样本错分类进行补偿,最终通过求解二次规划得到安全控制指令。针对无人车可能陷入停滞状态的死锁问题,提出一种分段式的检测与消除方法来避免。数值仿真实验和半实物实验结果表明,所提方法在多种障碍物环境下具备较好的避障避撞和轨迹规划能力,在路径长度等方面优于传统的控制方法。

关键词: 无人车, 轨迹规划, 控制障碍函数, 最小二乘支持向量机

Abstract:

The ummaned ground vehicles (UGVs)have become a practical alternative to humans in performing tasks in high-risk, high-pollution are as due to their strong adaptability and low cost. As a key technology of mission system for UGV, the trajectory planning is to develop a motion trajectory that meets the constraints based on mission objectives. However, the current navigation controllers rely primarily on the pre-acquired maps and prior knowledge, and are lacking in the ability to cope with unknown environments, making UGVs unable to adapt to complex and changing task environments. Therefore, a trajectory planning method based on the control barrier function (CBF) is proposed to solve the autonomous obstacle avoidance and trajectory planning problems of UGVs in unknown environments. The front end of the method utilizes a laser radar to perceive environmental depth information, while its back end uses a least squares support vector machine (LSSVM) to fit the boundary of obstacles to estimate the control barrier function and compensate for negative sample misclassification. Finally, the safety control instruction is obtained by solving a quadratic programming problem. A segmented detection and elimination method is proposed to address the issue of deadlocks that may occur when UGVs come to a standstill. The results of numerical simulation experiments and semi-physical experiments show that the proposed method has good obstacle avoidance and trajectory planning capabilities in various obstacle environments, and is superior to traditional control methods in terms of path length and other aspects.

Key words: unmanned ground vehicle, trajectory planning, control barrier function, least squares support vector machine

中图分类号:

TP273.1

方秋雨, 张蕴霖, 麻壮壮, 邵晋梁. 未知环境下基于控制障碍函数的无人车轨迹规划[J]. 兵工学报, 2023, 44(S2): 90-102.

FANG Qiuyu, ZHANG Yunlin, MA Zhuangzhuang, SHAO Jinliang. Control Barrier Functions-based Trajectory Planning for Unmanned Ground Vehicles in Unknown Environment[J]. Acta Armamentarii, 2023, 44(S2): 90-102.

图/表 11

图1 算法流程图

Fig.1 Flow diagramof algorithm

算法1 基于在线CBF估计的控制算法
1.初始化数据集T和历史数据集T_h 2.REPEAT 3. 获取当前位置坐标x和激光雷达数据z_t 4. IF ‖x-x_g‖>η 5. 依概率式(11)重组数据集 $T ˜$ 6. 数据预处理及模型训练 7. 估计安全边界,基于式(9)、式(11)生成CBF 8. 求解安全控制器式(13) 9. 更新控制量u←u^*(x) 10.END IF 11.UNTIL轨迹规划任务完成

算法1 基于在线CBF估计的控制算法
1.初始化数据集T和历史数据集T_h 2.REPEAT 3. 获取当前位置坐标x和激光雷达数据z_t 4. IF ‖x-x_g‖>η 5. 依概率式(11)重组数据集 $T ˜$ 6. 数据预处理及模型训练 7. 估计安全边界,基于式(9)、式(11)生成CBF 8. 求解安全控制器式(13) 9. 更新控制量u←u^*(x) 10.END IF 11.UNTIL轨迹规划任务完成

算法2 死锁检测与消除算法

1.初始化样本集X和死锁持续时间t_s
2.REPEAT
3. 更新样本集X
4. 计算方差S²和死锁持续时间t_s
5. 根据式(16)计算δ
6. IF δ=1
7. 进入死锁消除算法,建立搜索空间H
8. 更新集合H₀
9. 根据式(13)计算下一子目标点
10. 生成相应标称控制器k^*(x)
11. 求解安全控制器式(8)
12. 更新控制量u←u^*(x)
13. END IF
14.UNTIL死锁消除任务完成

图2 障碍物边界拟合效果图

Fig.2 Effectiveness visualization of obstacle boundary fitting

表1 实际障碍物面积与估计面积

Table 1 Actual obstacle area and estimated area

障碍物类型	实际面积/m	圆形近似面积/m²	LSSVM近似面积/m²
圆形	3.783	3.901	4.399
椭圆形	4.875	9.975	6.953
长方形	2.522	6.283	4.757
多边形	3.300	5.726	4.379
不规则型	3.320	5.147	3.971

图3 轨迹规划对比图

Fig.3 Trajectory planning comparison

表2 平均轨迹长度与障碍物距离

Table 2 Average trajectory length and obstacle distance

轨迹长度和障碍物距离	A^*算法	APF算法	CBF	直线距离
轨迹长度/m	10.348	12.698	11.375	8.731
障碍物距离/m	0.987	1.276	1.132	—

图4 死锁消除算法轨迹图

Fig.4 Trajectories of deadlock elimination algorithm

表3 不同参数下的平均死锁时间

Table 3 Average deadlock time under different parameters

扩展类函数 (d₀=0.05)	死锁时间/s	搜索距离 (α=0.3* $h ˜ 3$ (x))	死锁时间/s
α= $h ˜$ (x)	—	d₀=0.5	19.691
α= $h ˜ 3$ (x)	16.691	d₀=0.2	15.726
α=0.5* $h ˜ 3$ (x)	16.223	d₀=0.1	14.304
α=0.3* $h ˜ 3$ (x)	15.066	d₀=0.05	15.066
α=0.1* $h ˜ 3$ (x)	16.187	d₀=0.01	26.548
α=0.01* $h ˜ 3$ (x)	—	d₀=0.001	—

表3 不同参数下的平均死锁时间

Table 3 Average deadlock time under different parameters

扩展类函数 (d₀=0.05)	死锁时间/s	搜索距离 (α=0.3* $h ˜ 3$ (x))	死锁时间/s
α= $h ˜$ (x)	—	d₀=0.5	19.691
α= $h ˜ 3$ (x)	16.691	d₀=0.2	15.726
α=0.5* $h ˜ 3$ (x)	16.223	d₀=0.1	14.304
α=0.3* $h ˜ 3$ (x)	15.066	d₀=0.05	15.066
α=0.1* $h ˜ 3$ (x)	16.187	d₀=0.01	26.548
α=0.01* $h ˜ 3$ (x)	—	d₀=0.001	—

图5 半实物平台与实验场地

Fig.5 Semi-physical platform and experimental ground

图6 半实物实验运行图

Fig.6 Semi-physical experimental operation diagram

参考文献 23

[1]	JIA C H, HUANG M H, SUI L Y. An autonomous vehicle motion planning method based on dynamic programming[C]//Proceedings of the 2023 17th International Computer Conference on Wavelet Active Media Technology and Information Processing. Chengdu, China:IEEE, 2020: 394-398.
[2]	JIA H B, ZHANG L, WANG Z P. A dynamic lane-changing trajectory planning scheme for autonomous vehicles on structured road[C]// Proceedings of the 2020 IEEE 9th International Power Electronics and Motion Control Conference. Nanjing, China:IEEE, 2020: 2222-2227.
[3]	BETZ J, ZHENG H R, LINIGER A, et al. Autonomous vehicles on the edge: A survey on autonomous vehicle racing[J]. IEEE Open Journal of Intelligent Transportation Systems, 2022, 3: 458-488. doi: 10.1109/OJITS.2022.3181510 URL
[4]	LIU B X, LIU C. Path planning of mobile robots based on improved RRT algorithm[J]. Journal of Physics: Conference Series, 2022, 2216: 012020. doi: 10.1088/1742-6596/2216/1/012020
[5]	SONG P C, PAN J S, CHU S C. A parallel compact cuckoo search algorithm for three-dimensional path planning[J]. Applied Soft Computing, 2020, 94: 106443. doi: 10.1016/j.asoc.2020.106443 URL
[6]	SUN Y H, FANG M, SU Y X. AGV path planning based on improved Dijkstra algorithm[J]. Journal of Physics: Conference Series, 2021, 1746: 012052. doi: 10.1088/1742-6596/1746/1/012052
[7]	LI B C, DONG C Y, CHEN Q M, et al. Path planning of mobile robots based on an improved A^* algorithm[C]// Proceedings of the 2020 4th High Performance Computing and Cluster Technologies Conference & 2020 3rd International Conference on Big Data and Artificial Intelligence. Qingdao, China: ACM, 2020: 49-53.
[8]	SUN J X, XIN L. Research on path planning of AGV based on improved ant colony optimization algorithm[C]// Proceedings of the 2021 33rd Chinese Control and Decision Conference. Kunming, China: IEEE, 2021: 7567-7572.
[9]	GUO P, JIANG B. Research on path planning of three-dimensional UAV based on levy flight strategy and improved particle swarm optimization algorithm[C]// Proceedings of the 2020 7th International Conference on Information Science and Control Engineering. Changsha, China: IEEE, 2020: 1199-1203.
[10]	ANDRIY D, SERGII R, DARYA V. Simulation tool for the drone trajectory planning based on genetic algorithm approach[C]// Proceedings of the 2020 IEEE KhPI Week on Advanced Technology. Kharkiv, Ukraine: IEEE, 2020: 387-390.
[11]	BAO B L. Research on real-time path planning and obstacle avoiding for mobile robot swarms based on an advanced artificial potential field method[C]// Proceedings of the 2023 IEEE 2nd International Conference on Electrical Engineering, Big Data and Algorithms. Changchun, China: IEEE, 2023: 1747-1752.
[12]	YUAN X Y. Research on the limitations of UAV path planning based on artificial potential field method[C]// Proceedings of the 2022 9th International Forum on Electrical Engineering and Automation. Zhuhai, China: IEEE, 2022: 619-622.
[13]	TAN X, CORTEZ W S, DIMAROGONAS D V. High-order barrier functions:Robustness, safety, and performance-critical control[J]. IEEE Transactions on Automatic Control, 2021, 67(6): 3021-3028. doi: 10.1109/TAC.2021.3089639 URL
[14]	AMES A D, COOGAN S, EGERSTEDT M, et al. Control barrier functions: theory and applications[C]// Proceedings of the 2019 18th EuropeanControl Conference. Naples, Italy: IEEE, 2019: 3420-3431.
[15]	TAYLOR A J, AMES A D. Adaptive safety with control barrier functions[C]// Proceedings of the 2020 American Control Conference. Denver, CO, US: IEEE, 2020: 1399-1405.
[16]	KOSURU V S R, VENKITARAMAN A K. Developing a deep Q-learning and neural network framework for trajectory planning[J]. European Journal of Engineering and Technology Research, 2022, 7(6): 148-157. doi: 10.24018/ejeng.2022.7.6.2944 URL
[17]	YANG Y X, CHEN Z X. Optimization of dynamic obstacle avoidance path of multirotor UAV based on ant colony algorithm[J]. Wireless Communications and Mobile Computing, 2022(3):1-9.
[18]	SWDDAOUI A, SAAJ C M. Collision-free optimal trajectory generation for a space robot using genetic algorithm[J]. Acta Astronautica, 2021, 179: 311-321. doi: 10.1016/j.actaastro.2020.11.001 URL
[19]	WANG D, LI C H, GUO N, et al. Local path planning of mobile robot based on artificial potential field[C]// Proceedings of the 2020 39th Chinese Control Conference. Shenyang, China: IEEE, 2020: 3677-3682.
[20]	杜宏宝, 王正杰, 唐礼喜, 等. 基于控制障碍函数的飞行器避障与制导控制[J]. 兵工学报, 2023, 44(9): 2814-2823. doi: 10.12382/bgxb.2022.1002
	DU H B, WANG Z J, TANG L X, et al. Control barrier function-based control for aircraft avoidance and guidance with a dynamic obstacle[J]. Acta Armamentarii, 2023, 44(9): 2814-2823. (in Chinese)
[21]	SRINIVASAN M, DABHOLKAR A, COOGAN S, et al. Synthesis of control barrier functions using a supervised machine learning approach[C]// Proceedings of the 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems. Las Vegas, ND, US: IEEE, 2020: 7139-7145.
[22]	TAYLOR A, SINGLETARY A, YUE Y, et al. Learning for safety-critical control with control barrier functions[C]// Proceedings of the 2nd Learning for Dynamics and Control. Cambridge, MA, US: PMLR, 2020: 708-717.
[23]	龙离军, 胡腾飞. CLF-CBF-QP新形式下非线性系统的安全攸关控制与优化[J]. 控制理论与应用, 2022, 39(8):1387-1396.
	LONG L J, HU T F. Safety-critical control and optimization of nonlinear systems based on new forms of CLF-CBF-QP[J]. Control Theory & Applications, 2022, 39(8): 1387-1396. (in Chinese)