2. 中国地质大学(北京)工程技术学院,北京 100083;
3. 青岛海洋科学与技术试点国家实验室,山东 青岛 266237
2. School of Engineering and Technology, China University of Geosciences (Beijing), Beijing 100083, China;
3. Pilot National Laboratory for Marine Science and Technology,Qingdao 266237, China
无人水面艇(以下简称无人艇)目前在国内外军事和民用领域的应用日渐增多,而无人艇能够自主航行的关键则取决于路径规划技术[1]。
与其他路径规划方法相比,人工势场法在路径规划和避障领域有着极其广泛的应用,这是因为其理论较为简单、易于实现从而极具吸引力[2]。除了建立吸引/排斥力模型外,许多其他势场函数方法也正在被研究,这些方法能够非常有效快速地规划出路径,但都存在局部最小值的问题[3]。
这个问题会导致以下几种情况发生:
1)存在陷阱区域;
2)在相近的障碍物群中不能识别路径;
3)在障碍物前震荡;
4)在狭窄通道中摆动;
5)障碍物附近目标不可达。
为了克服这些问题,目前国内外研究者针对人工势场法有如下几种改进方式:结合基于搜索规划器的路径规划方法。如采用波前规划器来克服局部最小值问题[4-5]。改进势场函数,定义一个只有一个局部最小值的势场函数,从而解决上述问题。如斥力势场函数改进模型[6]。加入应激行为,来摆脱极值点,重新向终点前进。如加入一个随机扰动[7]。加入中间点[8]。利用几何方法重新规划路径,摆脱局部极小点[9]。结合智能算法的路径规划方法,如基于混沌优化改进的人工势场法[10]。通过蚁群算法来不断优化人工势场法获得的路径[11]。结合遗传算法来改进人工势场法来获得最优路径[12]。
上述方法可以解决局部最小值点导致的问题,但也存在着规划过于复杂等问题。
2011年,Hossein Adeli等提出了一种迭代势场算法[13,能够很好地解决移动机器人的路径规划问题,并依然保持了算法理论的简洁性和易于实现性。但上述方法存在需要调参、耗费较大的计算资源等缺点,不适用于局部路径规划。
本文提出一种基于栅格法的稀疏迭代势场方法。通过稀疏点约束方法和构建一种比传统迭代势场算法更加简单的势场函数,提高算法运行速度,并解决栅格法离散化所带来的缺陷(限制了路径方向变化只能为π/4的倍数,会导致无人艇不必要的运动转向),不但能规划一条任意角度的最短路径,还大幅减少了计算时间。此外针对无人艇局部路径规划需要满足海事规则的特点,建立了海事规则和转向避碰模型,最终让无人艇实现实时、快速、安全地避碰,规划出来的路径符合无人艇的运动特性且能够通过改变安全距离的方式,实现场景自适应功能。
1 传统迭代势场算法 1.1 栅格法利用栅格法对环境进行建模,将工作空间划分为一系列具有障碍物信息的正方形栅格网络单元,如图1所示。图中黑色区域为可行区域,白色区域为不可行区域。栅格中的每一个单元格就代表一个路径点。但栅格法的离散化建模方法,限制了无人艇的路径方向变化只能为π/4的倍数。当为稠密路径点时,S0无法直达P1,需要S0→S1→P1或S0→S8→P1。
![]() |
图 1 栅格法环境建模和栅格法离散化缺陷 Fig. 1 Environment modeling with grid method and Discretization defect of grid method |
势场函数如下式:
${U_{cell}}(c) = {U_{start}}(c) + {U_{goal}}(c) - {U_{obs}}(c){\text{。}}$ | (1) |
式中:
最优路径是由一系列最优点组成,只要得到每一个最优路径点,就可以得到最优路径。
根据式(1),对可行区域中每一点计算势能值,并存入一个集合中。
然后进行最优点寻找,并不断进行迭代二分搜索,直至生成一条最优路径,如图2所示。
![]() |
图 2 迭代二分搜索示意图 Fig. 2 Iterative binary search diagram |
迭代势场算法可以直接得到一条全局最优路径(此处的最优路径是指考虑到障碍物排斥势场的最优路径,而非最短路径),但其存在3个缺陷。
1)需要计算
为了得到势场函数,需要计算
2)需要迭代多次
为了得到考虑障碍物的排斥势场的最优平滑路径,需要迭代足够多的次数,但当迭代次数过多时,就会变成稠密点路径,将会出现栅格法离散化问题,如图1和图2(d)所示,此外运行时间也会大大增多。
3)需要调参
该算法可以通过调整
为了解决上述缺陷,本文提出了一种改进迭代势场算法。
2.1 改进势场函数改进势场函数如下式:
${U_{cell}}(c) = {U_{start}}(c) + {U_{goal}}(c){\text{。}}$ | (2) |
其中:各项含义等同于式(1),但是舍弃了
这样的好处是减少了计算
另一方面在改进势场函数的基础上,通过建立安全模型可以得到一条量化了安全距离的最优路径,这样就解决了传统迭代势场法需要调参的问题,从而让无人艇具备自适应场景的能力。同时也解决了由于舍弃了
基于改进势场函数进行迭代二分搜索得到的点应为最优点Poptimal,因为这个点只在可行区域中进行寻优且确保了路径的存在。此外,由于改进势场函数不考虑障碍物的排斥力,当障碍物挡住路径时,为了保证迭代路径点相互联通,此时最优点必将出现在障碍物边缘。定义这些出现在障碍物边缘的最优点Poptimal为关键点Pkey,易知最短路径一定可以由Poptimal中的某些点联通而成。
2.2 稀疏点约束为了防止出现栅格法离散化问题和提高计算时间,在算法中加入稀疏点约束。
稀疏点约束由联通约束与最短路径约束组成。
首先加入联通约束。采用Straight of line算法作为迭代终止条件,即只需最优点间两两联通即可,如图3所示。这样得到的最优点数量大幅减小,从稠密点减少为重要联通点。
![]() |
图 3 重要联通点 Fig. 3 Important connecting points |
联通约束的目的是得到少量的点序列来完成路径规划任务,其搜索量和时空开销会减小,收敛速度变快。不但减少了计算时间,而且得到任意角度的路径。
针对重要联通点,再加入最短路径约束。
${f_{n2}}(P) = \sum\limits_{i = 1,j = i + 1}^{n - 1} {d({P_i},{P_j})}{\text{,}}$ | (3) |
式中:
采用Floby算法对下式进行求解。
$P^* = \mathop {\arg \min }\limits_P {f_{n2}}(P){\text{。}}$ | (4) |
将求解出来的点定义为稀疏点Psparse,从而得到一条由Psparse连接而成的最短路径,如图4所示。
![]() |
图 4 稀疏点 Fig. 4 Sparse points |
此项约束不但能够将重要联通点优化为稀疏点,而且可以规划出最短路径。路径最短也就可以认为避开障碍物的时间最少(在符合无人艇运动学的情况下),从而满足了局部路径规划的时间需求,能够实现快速避碰。
2.3 安全距离约束基于稀疏点约束,得到一条由Psparse连接而成的最短路径。由于Psparse都在障碍物边缘,容易导致危险。因此加入安全距离约束,确保路径中的每一个路径点离障碍物有一定的距离,在快速避碰的同时确保安全。
安全距离约束为:
$\left\{ \begin{array}{l} d(c,obs) \geqslant {d_{safe}}{\text{,}} \\ \min \left(\sum\nolimits_{i = 1}^n {d({c_i},obs)}\right){\text{。}} \end{array} \right.$ | (5) |
式中:
采用障碍物膨化的方法,实现上述约束,使Psparse出现在膨化后的障碍物边缘。障碍物膨胀公式如下:
$E = X \oplus S = \left\{ \left. x \right|S + x \cup x \ne \phi \right\}{\text{。}}$ | (6) |
式中:X为障碍物集合;x为障碍物集合中的栅格元素;E为输出结果;S为膨胀结构元素,相关参数的选取如下式:
$\left\{ \begin{array}{l} {S_{shape}} = g({o_s},{m_s},{s_s}) {\text{,}}\\ {S_{size}} = 2{d_{safe}} + 1{\text{。}} \end{array} \right.$ | (7) |
其中:Sshap为S的形状;os为障碍物形状;ms为障碍物周边可行区域大小;ss为环境场景;g为关系函数;Ssize为S的尺寸;dsafe为安全距离。
选择不同大小与形状的膨胀结构元素,会改变路径关键点的位置,从而对路径长度与路径拐角的大小造成影响,如图5所示。图像大小为300*300像素,图像中心处有一个半径15像素的圆形障碍物。
![]() |
图 5 圆形小尺寸膨胀结构元素和方形膨胀结构元素 Fig. 5 Round small size and square big size |
图5左图是基于Sshap=circle,Ssize=11的膨胀结构元素的路径规划示意图,其中环形区域为dsafe=5的量化安全距离区域。
图5右图是基于Sshap=square,Ssize=41的膨胀结构元素的路径规划示意图,其中外圈区域为dsafe=20的量化安全距离区域。
通过图5的对比,可以看出,Ssize决定了dsafe的大小;Sshap影响着路径拐角的大小。
在加入了安全距离约束后,每一个路径点都会满足
现有的路径规划算法,往往把无人艇看作一个理想点,没有考虑到无人艇的运动性能。实际航行中,当无人艇以稳定速度转弯时,为了防止无人艇翻船,存在一个最小转弯半径Rm。当规划出的路径拐角较小时,无人艇无法跟踪此路径。
因此传统方法会对每一个路径拐点进行判断。当拐角为锐角时,进行拐角优化处理。如图6所示。利用半径为Rm的圆弧对路径点A前后的路径进行拟合,从而得到切点B1、B2,原来的路径为
![]() |
图 6 拐角约束 Fig. 6 Corner constraint diagram |
由于本文对势场函数进行了改进,并加入稀疏点约束,路径关键节点将出现在障碍物边缘,如图7(a)所示。由于又加入了安全距离约束,最后会得到一条量化安全距离的最短路径,因此这条路径不会存在为锐角的拐角,如图7(b)所示。图7(b)中的障碍物由图7(a)中障碍物膨化得到,膨胀结构元素的形状为正方形,尺寸为3。
![]() |
图 7 自动满足运动学约束示意图 Fig. 7 Automatically satisfying kinematic constraint |
从图7可以看出,加入了安全距离约束后,路径拐角将不会出现锐角。
2.5 海事规则约束国际海上避碰规则公约(International Collision Regulations,COLREGS)是由国际海事组织制定,为了防止、避免海事船舶相撞而制定的海上交通规则[14]。为了无人艇的航行安全,应使无人艇遵守国际海上避碰规则公约。
根据COLREGS、船舶避碰的经验数据以及实际船只在复杂海洋环境中航行范围,为无人艇定义了追赶(Overtaking,OT)、相遇(Head-on,HO)、右交叉(Crossing from right,CFR)、左交叉(Crossing from left,CFL)4种冲突局面,并可得到冲突局面的关系函数和航行规则表。
${f_{{c_1}}} = g(P,\Delta \theta ){\text{。}}$ | (8) |
式中:P为目标船只所在范围,如图8所示。
![]() |
图 8 无人艇与目标船只相对位置图 Fig. 8 Relative position map of USV and target vessel |
根据COLREGS可以得到无人艇在遇到目标船只的避碰转向模型:
$US{V_{tur{\rm{n}}}} = \left\{ \begin{array}{l} left,{f_{{c_1}}} = CFL \cup \left({f_{{c_1}}} = OT \cap \Delta \theta \in \left[0,\dfrac{{3{\text{π}} }}{8}\right)\right){\text{,}}\\ right,{f_{{c_1}}} = CFR \cup {f_{{c_1}}} = HO \; \cup\\ \;\;\;\;\;\;\;\; \left({f_{{c_1}}} = OT \cap \Delta \theta \in \left[\dfrac{{13{\text{π}} }}{8},2{\text{π}} \right)\right) {\text{。}}\\[-13pt] \end{array} \right.$ | (9) |
由于改进迭代势场法在加入了安全距离约束后,可以通过量化安全距离,沿膨化后障碍物边缘通行。因此可以通过环境感知系统检测目标船只的位置,再利用HMM的方法预测目标船只的位置,根据表1和式(9),判断将向哪个方向转向。然后对障碍物进行选择性膨胀,从而满足海上避障规则公约的要求,如图9所示。
![]() |
表 1 航行规则表 Tab.1 Navigation rules table |
![]() |
图 9 海事规则约束下的无人艇避障方向 Fig. 9 Obstacle avoidance direction under the COLREGS |
图9(a)表示在不加入海事规则约束下的无人艇动态避碰路线。由于此时无人艇会进入HO局面,按照式(9)和表1可知,无人艇应向右绕行。为了确保符合COLREGS,在障碍物中沿绕行方向的反方向进行选择性膨胀,如图11(b)所示。由于Psparse是能够构成最短路径的最优点,针对图11(b)中的非对称障碍物,将会规划出向右绕行的路径。
改进迭代势场算法流程如见图10所示。
![]() |
图 10 改进迭代势场法流程图 Fig. 10 Improved iterative potential field method flow chart |
由于无人艇航行实际场景中的障碍物分布往往较为简单与单一,为了能够清楚显示本文算法的优点,采用如图11所示的复杂迷宫环境来验证稀疏迭代算法的性能和有效性,并与传统迭代势场算法进行对比分析。其中图像大小为300*300像素。
![]() |
图 11 迭代势场与改进迭代势场法的路径规划图 Fig. 11 Path planning with IPF and SIPF |
图11(a)和图11(b)为传统迭代势场算法规划出来的路径,势场函数参数a=1,b=1。
![]() |
图 13 OT局面下无人艇动态航迹 Fig. 13 USV dynamic track under OT situation |
图11(b)是在传统迭代势场算法的基础上加入最短路径约束所规划出来的路径。
图11(c)和图11(d)为稀疏势场算法规划出来的路径,其中的势场函数参数。
图11(c)为稀疏迭代势场算法(没有加入安全距离约束)规划出来的路径。图11(d)为稀疏迭代势场算法(安全距离为4个像素,膨胀结构元素形状为正方形)规划出来的路径。
规划出来的路径相关参数见表2。其中time为算法运行时间,lenth为路径长度,dist为平均障碍物距离,即
![]() |
表 2 迭代势场法与稀疏迭代势场法的迷宫地图试验数据 Tab.2 Related data of IPF and SIPF on maze map |
$dist = \left(\sum\nolimits_{i = 1}^n {d({c_i},obs)} \right)/n{\text{。}}$ |
式中:n为路径点个数,
从图11(a)和表2可以看出,由于考虑
从图11(b)中可以看出,即使加入了最短路径约束,迭代势场算法规划出来的路径依然不是最短路径,这是由于
从图11(c)与图11(d)以及表2可以看出,稀疏迭代势场算法在加入了安全距离约束后可以规划出一条量化了安全距离的最短路径,符合快速避碰的需求。
综上,我们可以看出稀疏迭代势场算法的平均运行效率比传统迭代势场算法提高了14.4倍,此外能够量化安全距离,得到最短路径,满足海事规则和运动学约束,能够让无人艇实现实时、快速、动态地安全避碰。
3.2 无人艇静态障碍物避碰仿真设初始运动参数如下:航速v = 5 m/s,航向
图12(a)为迭代势场算法规划出来的路径,势场函数参数
![]() |
图 14 全局路径规划径示意图 Fig. 14 Static obstacle avoidance diagram |
图12(b)为稀疏迭代势场算法(没有加入安全距离约束)规划出来的路径,规划出来的路径相关参数见表3,表中参数含义与表2相同。
![]() |
表 3 采用迭代势场法与稀疏迭代势场法进行静态障碍物避障得到的相关数据 Tab.3 Related data of static obstacle avoidance using IPF and SIPF |
从表3可以看出,稀疏迭代势场算法的路径规划时间比迭代势场算法小了一个数量级。从图12(b),图5可以看出,稀疏迭代势场算法规划出来的路径是最短路径,且不存在有锐角的拐角,满足无人艇的运动学约束。当dsafe增大时,lenth,dist也会随之增大。表明稀疏迭代势场算法可以很好地规划出一条量化了安全距离的最短路径。此外平均规划效率比传统迭代势场法提高了16.6倍。
![]() |
图 12 静态障碍物避碰径示意图 Fig. 12 Static obstacle avoidance diagram |
设本艇初始运动参数如下:航速v=5 m/s,航向
动态障碍物以3 m/s的速度向东航行,且
针对算法开展实船试验。基于海图和航海雷达获知全局地图,并使用稀疏迭代势场法进行全局路径试验。基于航海雷达、激光雷达、声呐、摄像机等传感器获知局部环境信息,并基于稀疏迭代势场法进行局部路径规划,局部路径规划的感知距离为600 m。在距离障碍物100 m时开始进行避障,量化安全距离为25 m。
全局路径规划图如图14所示。其中无人艇以12 kn速度航行,航向为45°,目标船只以5 kn速度航行,航向为90°,无人艇航向坐标系见图8。
由于目标船只近似为矩形且周边只有一个障碍物,采用正方形结构算子进行膨化以满足安全距离约束。可以看出,该方法能够规划出一条安全、符合COLREGS、无人艇运动学约束的最短路径。在600*600像素的地图上能够以2 Hz的频率进行规划,满足无人艇局部避碰的实时性要求。
5 结 语本文针对无人水面艇的局部避碰进行了研究,所提出的稀疏迭代势场算法不但避免了传统人工势场法所产生的局部极小值问题,还解决了栅格法规划的路径无法满足无人艇运动约束的问题,而且可以实现实时、快速、安全地避碰,并且规划出来的路径符合无人艇的运动特性和海事规则。最终能够在多约束条件下,规划得到一条量化了安全距离的最短路径,让无人艇具备自适应场景功能。仿真分析和实船试验均验证了本文所提方法的有效性。且表明稀疏迭代势场法的运行效率比迭代势场算法提高了14倍。
[1] |
HU Chang-qing, ZHU Wei, HE Yuan-qing, et al.. Autonomous navigation technology of unmanned surface vehicle[J]. Navigation and Control, 2019, 18(01): 19-26+90. |
[2] |
MAC T T, COPOT C, Hernandez A, et al. Improved potential field method for unknown obstacle avoidance using UAV in indoor environment[C]//2016 IEEE 14th International Symposium on Applied Machine Intelligence and Informatics (SAMI). IEEE, 2016: 345–350.
|
[3] |
RAJA R, DUTTA A. Path planning in dynamic environment for a rover using A* and potential field method[C]//2017 18th International Conference on Advanced Robotics (ICAR). IEEE, 2017: 578–582.
|
[4] |
BENTES C, SAOTOME O. Dynamic swarm formation with potential fields and A* path planning in 3D environment[C]//2012 Brazilian Robotics Symposium and Latin American Robotics Symposium. IEEE, 2012: 74–78.
|
[5] |
WU X, GONG S, XIE P. Local path planning based on improved artificial potential field using fuzzy repulsion force for robot[J]. Journal of electrical Engineering, 2014, 2(3): 48-60. DOI:10.12677/JEE.2014.23007 |
[6] |
ZUNLI N, XUEJUN Z, XIANGMIN G. UAV formation flight based on artificial potential force in 3D environment[C]//2017 29th Chinese Control And Decision Conference (CCDC). IEEE, 2017: 5465–5470.
|
[7] |
FANG Peng-peng, YANG Jia-fu, SHI Yang-yan. et al.. Gradient descent method and improved artificiaI potential field method for obstacle avoidance of unmanned vehicle[J]. Manufacturing Automation, 2018, 40(11): 81-84. |
[8] |
WANG H, LYU W, YAO P, et al. Three-dimensional path planning for unmanned aerial vehicle based on interfered fluid dynamical system[J]. Chinese Journal of Aeronautics, 2015, 28(1): 229-239. DOI:10.1016/j.cja.2014.12.031 |
[9] |
WANG Bo, ZHANG Jian-xun, HOU Zhi-xu. Path pianning for inteiiigent vehicie based on artificiai potentiai fieid method[J]. Journal of Chongqing University of Technolog (Natural Science), 2015, 29(6): 107-111. |
[10] |
WU Yuanbo, LI Xingguang, CHEN Dianren, et al.. Obstacle avoidance research of the automated guided vehicle based on improved artificial potential field method with chaotic optimization[J]. Science and Technology Innovation Herald, 2017, 14(17): 150-153. |
[11] |
WANG H, WANG Z A, YU L, et al. Ant colony optimization with improved potential field heuristic for robot path planning[C]//2018 37th Chinese Control Conference (CCC). IEEE, 2018: 5317–5321.
|
[12] |
RAJA R, DUTTA A, VENKATESH K S. New potential field method for rough terrain path planning using genetic algorithm for a 6-wheel rover[J]. Robotics and Autonomous Systems, 2015, 72: 295-306. DOI:10.1016/j.robot.2015.06.002 |
[13] |
Hossein ADELI, M H N TABRIZI, Alborz MAZLOOMIAN, et al. Path planning for mobile robots using iterative artificial potential field method[J] 2011, (4).
|
[14] |
SHANG Ming-dong, ZHU Zhi-yu, ZHOU Tao. Research on dynamic intelligent anti-collision of USV[J]. Ship Science and Technology, 2017(17): 69-73. |