融合TangentBug与Dubins曲线的智能轮式车辆局部路径规划算法

doi:10.19562/j.chinasae.qcgc.2021.06.006

摘要/Abstract

摘要：

由于环境条件限制，某些采用Ackermann转向的智能轮式车辆仅能获取局部地图和定位信息，给路径规划造成了困难。针对这一问题，本文中提出了一种融合TangentBug和Dubins曲线的局部路径规划算法。首先通过采样的方法构建了规划参考点集合，然后以Dubins曲线作为规划路径，旨在满足车辆最小转向半径的运动约束和目标点处的航向要求，并加入了沿规划路径的碰撞检测和考虑定位误差的状态转换规则。最后通过实车实验证明：本文算法能使车辆按规定位姿到达目标点，并可保证规划路径的安全性和实时性；本文算法可有效避免定位误差对车辆状态的影响；相对于使用圆弧曲线，本文算法规划出的路径更有利于路径跟随控制。

关键词: 智能轮式车辆, 局部路径规划, TangentBug算法, Dubins曲线

Abstract:

Due to the limitation of environmental conditions， some intelligent wheeled vehicle with Ackermann steering can only obtain local map and location， which makes path planning difficult. To solve the problem， this paper proposes a local path planning algorithm combining TangentBug and Dubins path. Firstly， a set of reference points is established by sampling method. Then， Dubins path is generated as the planning path to satisfy the motion constraints of the minimum turning radius of the vehicle and the desired heading at the target point. Besides， collision checking along the planning path and mode switching rules considering the positioning error are added. Finally， real vehicle tests are conducted with results showing that the proposed algorithm can navigate the vehicle to the target with the required state while meeting the safety and real-time performance requirements of path planning， and eliminate the disturbance from the error of positioning. Compared with using arc curve， the path planned by this algorithm is more beneficial to path following control.

Key words: intelligent wheeled vehicle, local path planning, TangentBug, Dubins path

张旭东,徐福康,邹渊,郭宁远,张宇. 融合TangentBug与Dubins曲线的智能轮式车辆局部路径规划算法[J]. 汽车工程, 2021, 43(6): 833-841.

Xudong Zhang,Fukang Xu,Yuan Zou,Ningyuan Guo,Yu Zhang. A Local Path Planning Algorithm for Intelligent Wheeled Vehicle Combining TangentBug and Dubins Path[J]. Automotive Engineering, 2021, 43(6): 833-841.

图/表 20

图 1

图 2

图 3

图 4

图 5

表 1

表 2

图 6

图 7

图 8

表 3

图 9

表 4

图 10

图 11

图 12

图 13

图 14

图 15

表 5

参考文献 18

1	陈慧岩，熊光明，龚建伟，等. 无人驾驶汽车概论［M］. 北京：北京理工大学出版社， 2014.
	CHEN H Y， XIONG G M， GONG J W， et al. Introduction to self⁃driving car［M］. Beijing： Beijing Institute of Technology Press， 2014.
2	DIJKSTRA E W. A note on two problems in connexion with graphs［J］. Numerische Mathematik， 1959， 1（1）： 269-271.
3	STRUB M P, GAMMELL J D. Advanced BIT^（ABIT^）： sampling⁃based planning with advanced graph⁃search techniques［C］. 2020 IEEE International Conference on Robotics and Automation（ICRA）. IEEE， 2020： 130-136.
4	LAVALLE S M. Rapidly⁃exploring random trees： a new tool for path planning［J］. Research， 1999.
5	KAVRAKI L E， SVESTKA P， LATOMBE J C， et al. Probabilistic roadmaps for path planning in high⁃dimensional configuration spa⁃ces［J］. IEEE Transactions on Robotics and Automation， 1996， 12（4）： 566-580.
6	SHIMODA S， KURODA Y， IAGNEMMA K. High⁃speed navigation of unmanned ground vehicles on uneven terrain using potential fields［J］. Robotica， 2007， 25（4）： 409-424.
7	CETIN O， YILMAZ G. Sigmoid limiting functions and potential field based autonomous air refueling path planning for UAVs［J］. Journal of Intelligent & Robotic Systems， 2014， 73（1-4）： 797-810.
8	BELLINI A C， LU W， NALDI R， et al. Information driven path planning and control for collaborative aerial robotic sensors using artificial potential functions［C］. 2014 American Control Conference. IEEE， 2014： 590-597.
9	ZHAO Y， GU J. Robot path planning based on improved genetic algorithm［C］. 2013 IEEE International Conference on Robotics and Biomimetics （ROBIO）. IEEE， 2013： 2515-2522.
10	CHEN X， KONG Y， FANG X， et al. A fast two⁃stage ACO algorithm for robotic path planning［J］. Neural Computing and Applications， 2013， 22（2）： 313-319.
11	LUMELSKY V J， STEPANOV A A. Path⁃planning strategies for a point mobile automaton moving amidst unknown obstacles of arbitrary shape［J］. Algorithmica， 1987， 2（1-4）： 403-430.
12	SANKARANARAYANAN A， VIDYASAGAR M. A new path planning algorithm for moving a point object amidst unknown obstacles in a plane［C］. Proceedings.， IEEE International Conference on Robotics and Automation. IEEE， 1990： 1930-1936.
13	KAMON I， RIMON E， RIVLIN E. Tangentbug： a range⁃sensor⁃based navigation algorithm［J］. The International Journal of Robotics Research， 1998， 17（9）： 934-953.
14	KIM D H， SHIN K， HAN C S， et al. Sensor⁃based navigation of a car⁃like robot based on bug family algorithms［J］. Proceedings of the Institution of Mechanical Engineers， Part C： Journal of Mechanical Engineering Science， 2013， 227（6）： 1224-1241.
15	XU Q L， YU T， BAI J. The mobile robot path planning with motion constraints based on Bug algorithm［C］. 2017 Chinese Automation Congress （CAC）. IEEE， 2017： 2348-2352.
16	龚建伟，姜岩，徐威. 无人驾驶车辆模型预测控制［M］. 北京：北京理工大学出版社， 2014.
	GONG J W， JIANG Y， XU W. Model predictive control for self⁃driving vehicles［M］. Beijing： Beijing Institute of Technology Press， 2014.
17	DUBINS L E. On curves of minimal length with a constraint on average curvature， and with prescribed initial and terminal positions and tangents［J］. American Journal of Mathematics， 1957， 79（3）： 497-516.
18	SHAN T， ENGLOT B. LEGO⁃LOAM： Lightweight and ground⁃optimized lidar odometry and mapping on variable terrain［C］. 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems （IROS）. IEEE， 2018： 4758-4765.

算法1 局部路径规划算法
输入：集合F、F’，局部地图，车辆、目标位姿
输出：一条参考路径path
1：	初始化相关变量
2：	loop
3：	ifunreachable is true then
4：	return
5：	end if
6：	if规划参考点为目标点and l_d < d_tthen
7：	return
8：	end if
9：	Motion_Toward_the_Target（） // 见表2
10：	Boundary_Following（） // 见表3
11：	更新d'（P， T）
12：	end loop

算法2 朝目标移动状态算法
函数：Motion_Toward_the_Target（）
1：	if state is “boundary following” then
2：	return
3：	end if
4：	if d_i（P， T） – d’_i-₁（P， T） > d_mthen
5：	state ← “boundary following”
6：	记录此时目标的方位，确定环绕方向
7：	return
8：	end if
9：	if T’ ≠ ? then
10：	按目标航向生成Dubins曲线
11：	if 生成的Dubins曲线通过碰撞检测 then
12：	path ← 生成的Dubins曲线
13：	l_d ← Dubins曲线的长度
14：	return
15：	end if
16：	end if
17：	对集合F中的元素按代价从小到大的顺序进行排序
18：	while F ≠ ? do
19：	取集合F的首元素作为参考点生成Dubins曲线
20：	if 生成的Dubins曲线通过碰撞检测 then
21：	path ← 生成的Dubins曲线
22：	l_d ← Dubins曲线的长度
23：	return
24：	end if
25：	从集合F中删去当前元素
26：	end while

算法3 环绕障碍物状态算法
函数：Boundary_Following（）
1：	if state is “motion toward the target” then
2：	return
3：	end if
4：	更新d_min，以及对应的点M_i和SW_i
5：	更新d_leave，以及对应的点L_i和L_R_i
6：	if d_leave < d_minthen
7：	state ←“motion toward the target”
8：	return
9：	end if
10：	if 车辆环绕障碍物一周回到点SW_ithen
11：	unreachable ← true
12：	return
13：	end if
14：	if 顺时针环绕障碍物then
15：	对集合F’中的元素按方位角从小到大的顺序排序
16：	else
17：	对集合F’中的元素按方位角从大到小的顺序排序
18：	end if
19：	while F’ ≠ ? do
20：	取集合F’的首元素作为参考点生成Dubins曲线
21：	if 生成的Dubins曲线通过碰撞检测 then
22：	path ← 生成的Dubins曲线
23：	l_d ← Dubins曲线的长度
24：	return
25：	end if
26：	从集合F’中删去当前元素
27：	end while

参数	符号	数值	单位
最大前轮偏角	δ_max	π/9	rad
车辆轴距	l_w	2.65	m
最小转向半径	r_min	7.28	m
测距传感器最大探测距离	ρ_max	20	m
测距传感器角度分辨率	φ_r	π/180	rad
相邻点个数	m	2
判断障碍物边界点的距离阈值	d_e	3	m
安全系数	k_safe	1.3
采样比例系数	k_sample	2
覆盖圆半径	r_safe	1.5	m
判断接近目标点的距离阈值	d_t	3	m
状态转换距离阈值	d_m	1	m
判断环绕障碍物一周的距离阈值	d_sw	1	m

项目	融合圆弧曲线	融合Dubins曲线	提升百分比/%
标准差	0.045 2	0.025 3	44
最大值	0.128 4	0.068 0	47