CN109799817B

CN109799817B - 一种基于光反射特性的无人船全局路径规划方法

Info

Publication number: CN109799817B
Application number: CN201910035226.8A
Authority: CN
Inventors: 王晓原; 夏媛媛; 姜雨函; 刘亚奇; 高杰; 唐学大; 朱慎超; 柴垒; 赵新越
Original assignee: Navigation Brilliance Qingdao Technology Co Ltd
Current assignee: Navigation Brilliance Qingdao Technology Co Ltd
Priority date: 2019-01-15
Filing date: 2019-01-15
Publication date: 2021-12-14
Anticipated expiration: 2039-01-15
Also published as: CN109799817A

Abstract

本发明涉及一种基于光反射特性的无人船全局路径规划方法，包括：S1、获取待规划的路径的起点、目标点以及环境信息；S2、获取从起点发射的N条不同辐射角的发射射线，以及从目标点发射的N条不同辐射角的目标射线；S3、基于光反射特性，对起点发射出的每一条发射射线进行反射扩张处理，直至该射线抵达目标区域；S4、从目标点开始，对每一条抵达目标区域的射线轨迹进行修正，获得修正后的从起点到目标点的多条路径；S5、输出多个路径中最短的路径，获得规划路径。本发明算法收敛速度快，能在较短的时间内寻找到安全、可靠的路径，在广阔海域、复杂海况中，能在规定时间内，有效、迅速的寻找到最优路径。

Description

一种基于光反射特性的无人船全局路径规划方法

技术领域

本发明属于无人船舶路径规划领域，尤其涉及一种基于光反射特性的无人船全局路径规划方法。

背景技术

无人船全局路径规划方法是根据已知的全局环境信息，寻找一条从起始点到目标点的安全可行路线，是无人船路径规划与导航方面需要研究的关键问题之一，直接关系着无人船在海上航行的安全性和经济性。用于无人船路径规划的算法需要满足两个准则：收敛时间越短越好，路径越短越好。

目前，全局路径规划算法主要有基于图搜索算法、人工势场法、向量场法、概率性算法、基于网格的算法、利用启发函数提高收敛速度的算法等。基于图搜索算法首先使用可视图法、维诺图法等基于图形成路线图，然后使用最短路径法，如Dijkstra等，在路线图中寻找最短路径。基于图搜索算法由于时间复杂度高，在广阔海域、复杂海况中寻路耗时巨大，所以很难应用于实际的无人船路径规划中。人工势场法、向量场法等算法将船在周围环境中的运动，设计成一种抽象的人造引力场中的运动，目标点对船产生“引力”，障碍物对船产生“斥力”，最后通过求合力来控制无人船的运动。人工势场法、向量场法等算法规划出的路径一般比较光滑且安全，但存在局部最优的问题，当目标点附近有障碍物时，无人船永远达到不了目标点。概率性算法，如快速搜索随机树法、随机路线法等，此类算法是基于随机抽样的算法，由于算法的随机性，不能保证此算法得出的路径最优。基于网格的算法利用了深度优先搜索，由于局部最小化的可能性，此类算法不能保证搜索出的路径最优，例如，基于网格的人工势场法，它规划出的路径形状，完全取决于网格的大小，网格越细密，寻找的路径越接近最优路径，但是，相应的寻路时间也会变长。利用启发函数提高收敛速度的算法，例如A*算法，theta*算法，快速行进算法(Fast Marching Method，FMM)等，此类算法的寻路速度同样取决于环境的大小和栅格的大小。

上述算法无法适用于在动态环境中，在规定时间内规划出无人船的下一步路线图的场景，特别是无人船需要在规定时间内判断是否会碰撞静态的、动态的障碍物的应用环境。因此，在无人船的路径规划中，亟需提出一个使用限制少、寻路速度快的无人船全局路径规划方法。

发明内容

(一)发明目的

本发明利用自然界中光沿直线传播和反射的特性进行建模，使得无人船在探索自由空间时，不受步长、栅格大小等其他因素的限制。可以快速的进行寻路，在规定时间内规划出无人船全局路径。

(二)技术方案

为了达到上述目的，本发明采用的主要技术方案包括：

一种基于光反射特性的无人船全局路径规划方法，包括：

S1、获取待规划的路径的起点、目标点以及环境信息；所述环境信息包括无人船舶航线区域的环境边界信息以及障碍物信息；

S2、获取从起点发射的N条不同辐射角的发射射线，以及从目标点发射的N条不同辐射角的目标射线；其中，N为大于等于1的自然数，所述发射射线以及目标射线碰到障碍物或环境边界即止；

S3、基于光反射特性，对起点发射出的每一条发射射线进行反射扩张处理，直至该射线抵达目标区域；所述反射扩张处理包括：对抵达障碍物边界和环境边界的发射射线进行多次反射，直至多次反射的发射射线抵达目标区域，获得多条从起点到目标点的射线轨迹；所述目标区域为N条不同辐射角的目标射线；

S4、从目标点开始，对每一条抵达目标区域的射线轨迹进行修正，获得修正后的从起点到目标点的多条路径：

沿着射线轨迹获取射线轨迹上的与起点的连线与障碍物边界没有交点的路径点，并将获得的路径点设为起点进行重复迭代，直到目标点与起点的连线与障碍物边界没有交点，获得射线轨迹上的多个路径点；

从起点开始，输出射线轨迹上的多个路径点，获得修正后的从起点到目标点的多条路径；

S5、输出多个路径中最短的路径，获得规划路径。

所述步骤S1包括：

获取待规划路径的起点和目标点的坐标，并获取障碍物边界直线方程和环境边界直线方程；

根据障碍物边界直线方程和环境边界直线方程，由逆时针法则，获得障碍物边界和的环境边界的方向向量v_k,k+1；

所述步骤S2包括：

S21、获取从起点发射的N条不同辐射角的发射射线

其中，id为射线的编号，start为起点坐标；

S22、获取从目标点发射的N条不同辐射角的目标射线。

所述步骤S3包括：

S31、获取任意一条发射射线，从起点开始，根据公式一依次获取发射射线与障碍物边界或环境边界的反射点

同时，根据公式二获取反射点

对应的反射角度

直至多次反射的发射射线与目标射线相交；

公式一：

公式二：

其中，seq为反射点的编号，起点seq＝1，

为第id条射线的第seq个反射点，

为第id条射线的第seq个反射点的反射角度，T表示矩阵的转置，

f(x)为碍物边界直线方程或环境边界直线方程，k为碍物边界直线方程或环境边界直线方程的系数，

为第id条射线的第seq个反射点的x轴坐标，

为第id条射线的第seq个反射点的y轴坐标，

rand为

之间的随机值；

S32、重复步骤S31，获得多条不同辐射角的发射射线到目标点的射线轨迹

可选地，所述步骤S31之前还包括：

S30、获取任意一条发射射线，判断发射射线是否与任意一条目标射线相交，若是，则输出该发射射线以及和该发射射线相交的目标射线，获得规划路径。

可选地，所述步骤S3还包括：

S33、判断当前获取的射线轨迹的条数是否大于预设的路径数，若是，则执行步骤S4；若否，则返回步骤S31；

其中，预设路径数为起点到目标点的连线与障碍物边界的交点的个数加1。

所述步骤S4包括：

S41、将目标点x_goal设为迭代移动点x_e，将起始点x_start设为迭代起点x_s；

S42、从目标点出发，迭代移动点x_e沿着射线轨迹按预设的步长移动，直至当前迭代移动点x_e与迭代起点x_s的连线与障碍物边界没有交点，获取当前迭代移动点x_e的坐标；

S43、将当前迭代移动点x_e的坐标设为迭代起点x_s，将目标点x_goal设为迭代移动点x_e，重复步骤S42，直至迭代起点x_s与迭代移动点x_e的连线与障碍物边界没有交点；

S44、从起点开始，输出射线轨迹上的多个路径点，获得修正后的从起点到目标点的多条路径。

另一方面，本发明还提供一种位于无人船舶上的电子设备，包括：存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述计算机程序被所述处理器执行时实现如上所述的方法的步骤。

(三)有益效果

本发明的有益效果是：本发明算法收敛速度快，能在较短的时间内寻找到安全、可靠的路径，可以在广阔海域、复杂海况中，在规定时间内，有效、迅速的寻找到最优路径。

附图说明

图1为本发明一个实施例提供的一种基于光反射特性的无人船全局路径规划方法示意图；

图2为本发明一个实施例提供的获取发射射线和目标射线方法示意图；

图3为本发明一个实施例提供的反射扩张处理方法示意图；

图4为本发明一个实施例提供的获取碰撞点和碰撞角度示意图；

图5为本发明一个实施例提供的射线轨迹修正方法示意图；

图6为本发明一个实施例提供的反射扩张处理流程信令图；

图7为本发明一个实施例提供射线轨迹修正流程信令图；

图8为本发明一个实施例提供的一种基于光反射特性的无人船全局路径规划方法仿真结果示意图；

图9为本发明一种基于光反射特性的无人船全局路径规划方法与A*算法对比图。

具体实施方式

为了更好的解释本发明，以便于理解，下面通过具体实施方式，对本发明作详细描述。

本发明一种基于光反射特性的无人船全局路径规划方法，是利用自然界中光沿直线传播和反射的特性来建模，并用于全局单向路径规划。本算法的基本思想是：从起点投射出N条方向不同的射线，这N条射线通过障碍物边或环境边界进行反射，直到到达目标辐射区，目标辐射区是从目标点投射出的N条方向不同的射线。获得多条纠缠在一起的射线轨迹，这些射线轨迹通过修正，即可得最终路径。

如图1所示，本实施例提供一种基于光反射特性的无人船全局路径规划方法，具体地，方法包括下述步骤：

S1、获取待规划的路径的起点、目标点以及环境信息。

所述环境信息包括无人船舶航线区域的环境边界信息以及障碍物信息。

获取待规划路径的起点和目标点的坐标，并获取障碍物边界直线方程和环境边界直线方程。

障碍物边界，环境边界为船舶障碍物区域的边界，环境边界为船舶可航线区域的边界。在实际应用作用中，无人船可根据船舶的感知模块获取船舶起点的位置坐标、船舶障碍物，根据航行任务指令获取目标点的位置坐标，同时结合电子海图，获得船舶可航线区域和船舶障碍物区域。基于船舶可航线区域和船舶障碍物区域，获取障碍物边界直线方程和环境边界直线方程。

根据障碍物边界直线方程和环境边界直线方程，由逆时针法则，获得障碍物边界和的环境边界的方向向量v_k,k+1。

逆时针法则，即障碍物顶点按逆时针方向连接，形成障碍物边，由此，获得障碍物边的法向量n为：

n＝[cosσ,sinσ]^T。

其中，

Arg为求向量角度的算子。

S2、获取从起点发射的N条不同辐射角的发射射线，以及从目标点发射的N条不同辐射角的目标射线；其中，N为大于等于1的自然数，所述发射射线以及目标射线碰到障碍物或环境边界即止。

S21、获取从起点发射的N条不同辐射角的发射射线

其中，id为射线的编号，start为起点坐标。

从起始点投射出N条方向不同的射线，如图2中所示的虚线所示，碰到障碍物边界或环境边界即止。

定义N条链表

来存储这N条射线，其中id对应射线的标号，seq表示对应链表的位置，

表示第id条射线的第seq个投射点，

表示对应的投射角度。然后，初始化链表，

S22、获取从目标点发射的N条不同辐射角的目标射线。

获取从目标点发射的N条不同辐射角的目标射线，碰到障碍物边界或环境边界即止，如图2中的实线所示，并将从目标点投射出的方向不同的N条射线的集合定义为目标区域G。

S3、基于光反射特性，对起点发射出的每一条发射射线进行反射扩张处理，直至该射线抵达目标区域；所述反射扩张处理包括：对抵达障碍物边界和环境边界的发射射线进行多次反射，直至多次反射的发射射线抵达目标区域，所述目标区域为N条不同辐射角的目标射线，直至多次反射的发射射线抵达目标区域即与目标射线相交，获得多条从起点到目标点的射线轨迹。

对每一条发射射线进行反射扩张处理过程是一个迭代随机抽样的过程，也就是说N条射线，通过障碍物边或环境边界反射搜索自由空间，直到抵达目标区域停止反射。或者当抵达目标区域的射线数达到预定数目时，停止扩张过程。如图3所示，是一条射线的扩张过程，图中的折线是一条射线通过反射抵达目标区域形成一条轨迹的过程。反射扩张处理过程的每次迭代，又可分为两部分：求每条射线的碰撞点和反射角，求出来的碰撞点和反射角即为下一次迭代的投射点和投射方向，具体地，本步骤包括：

S30、获取任意一条发射射线，判断发射射线是否与任意一条目标射线相交，若是，则输出该发射射线以及和该发射射线相交目标射线，获得规划路径。

判断发射射线是否直接抵达目标区域，若是，则已获得规划路径，输出该发射射线、该发射射线与目标射线的交点、与该发射射线相交的目标射线，获得的折线即为规划路径。

同时，根据公式二获取反射点

对应的反射角度

直至多次反射的发射射线与目标射线相交；

公式一：

公式二：

其中，seq为反射点的编号，起点seq＝1，

为第id条射线的第seq个反射点，

为第id条射线的第seq个反射点的x轴坐标，

为第id条射线的第seq个反射点的y轴坐标，

rand为

之间的随机值。

如图4所示，首先，获得碰撞点。

假如当前过程的投射点和投射角为

碰撞点可由下面参数方程求得：

其中，

T表示矩阵的转置，t可通过障碍物边或环境边界所在直线方程获得。

即为当前射线的碰撞点，也是下一次迭代的投射点。

令，

x₀,x₁，y₀，y₁，v₀，v₁都表示已知数。

在实际应用中，边界方程为：y＝f(x)＝kx+b，k和b均为已知数。

将公式

右侧式子代入障碍物边界或环境边界的直线方程，可直接获得：

y₀+tv₁＝k(x₀+tv₀)+b，

通过求解上式，可得：

然后，求反射角。

求反射角前，我们需要先求得障碍物边的法向量n，n是由逆时针法则求得的。所谓逆时针法则，即障碍物顶点按逆时针方向连接，形成障碍物边，则法向量为：

n＝[cosσ,sinσ]^T。

其中，

v_k,k+1是按逆时针法则求得的障碍物边的方向向量，Arg是求向量角度的算子。然后，由下式即可求得反射角：

其中，rand在

之间随机取值。

本算法是基于随机抽样的算法。

在实际应用中，可以对目标点发射的每一条发射射线完成反射扩张处理后，再执行步骤S4，也可以执行以下步骤：

S33、判断当前获取的射线轨迹的条数是否大于预设的路径数，若是，则执行步骤S4；若否，则返回步骤S31。

预先设定需要获取的射线轨迹的路径数，若当前获取的射线轨迹的条数大于预设的路径数则执行步骤S4，若否，则返回步骤S31继续获取发射射线的射线轨迹。

其中，预设路径数n_sol为起点到目标点的连线与障碍物边界的交点的个数c加1。即：n_sol＝c+1。

S4、从目标点开始，对每一条射线轨迹进行修正，获得修正后的从起点到目标点的多条路径：

从起点开始，输出射线轨迹上的多个路径点，获得修正后的从起点到目标点的多条路径。

由于反射扩张的随机性，反射扩张阶段得出的轨迹是多条纠缠在一起的复杂路径，因此，我们需要对得到的N条轨迹进行修整，才能得到最后的路径。修整阶段是通过迭代去除冗余轨迹的过程。如图5所示，图中的细实线是扩张阶段得到的一条轨迹，粗实线是经过修正后得到的路径。具体地，本步骤包括：

S41、将目标点x_goal设为迭代移动点x_e，将起始点x_start设为迭代起点x_s。

修正过程，是由一个起始点x_s和一个结束点x_e通过迭代来完成的。在最开始，令x_s＝x_start，x_e＝x_goal。

S42、从目标点出发，迭代移动点x_e沿着射线轨迹按预设的步长移动，直至当前迭代移动点x_e与迭代起点x_s的连线与障碍物边界没有交点，获取当前迭代移动点x_e的坐标。

x_e沿着轨迹往x_s方向移动，直到x_s与x_e的连线与障碍物没有交点，停止移动。

S43、将当前迭代移动点x_e的坐标设为迭代起点x_s，将目标点x_goal设为迭代移动点x_e，重复步骤S42，直至迭代起点x_s与迭代移动点x_e的连线与障碍物边界没有交点。

此时，令x_s＝x_e，x_e＝x_goal，重复步骤S42，直到x_s与x_goal的连线与障碍物没有交点，停止迭代。

S5、输出多个路径中最短的路径，获得规划路径。

修正完成后，寻找一条最短路径，即为最优路径。

实施例二

如图6和图7所示，为本发明实施例中一种基于光反射特性的无人船全局路径规划方法的信令图，即本实施例以从起点发射20条发射射线为例，包括以下步骤：

101、获取待规划的路径的起点、目标点、障碍物边界和环境边界。

102、获取从起点发射的N条不同辐射角的发射射线，以及从目标点发射的N条不同辐射角的目标射线。

具体地如图6所示，本步骤包括：

102a、获取从起点发射的n_res条不同辐射角的发射射线。

从起点投射出n_res条方向不同的射线，n_res为大于0的自然数，本实施例中，n_res为20。将发射射线存储到L中。

102b、获取从目标点发射的n_res条不同辐射角的的目标射线。

从目标点投射出n_res条方向不同的射线，将目标射线存储到目标区域G中。

102c、定义n_res条链表

并初始化链表，

seq为反射点的编号，起点seq＝1，发射射线为

start为起点坐标，id为射线的编号，

为第id条射线的第seq个反射点的反射角度。

102d、获取预设的路径数n_sol。

计算起始点到目标点的可能路径数n_sol，n_sol＝c+1，c是起点和目标点的连线与障碍物边界交点的个数。

103、基于光反射特性，从起点开始对每一条发射射线进行反射扩张处理。

103a、初始化处理，令当前抵达目标区域的射线数sol＝0，定义含有n_sol个元素的数组traj[n_sol]，用来存储抵达目标区域的射线的编号，令traj[n_sol]＝0。

103b、判断当前抵达目标区域的射线的条数sol是否小于预设的路径数n_sol，若是，则令射线的编号id＝1并执行步骤103c，若否，则执行步骤103h。

判断是否满足条件sol＜n_sol，若是，则令id＝1并执行步骤103c，若否则执行步骤103h，输出获取的射线轨迹。

103c、判断当前射线的编号id是否小于等于射线的条数n_res，如果是则执行步骤103d，若否则返回步骤103b。

判断是否满足条件id≤n_res，若是，则执行步骤103d，若否则执行步骤返回步骤103b。

103d、判断当前射线的编号id是否是到达目标点的射线轨迹编号，若是则令当前射线的编号id加1，并返回步骤103c，若否，则执行步骤103e。

如果id∈traj，则令id＝id+1，并转至步骤103c；若否，则继续下一步。

103e、根据公式一和公式二对当前编号id节点seq的射线

进行反射扩张处理，获取当前编号id节点seq的射线与障碍物边界或环境边界的反射点

和反射角度

对链表

进行扩张。

103f、判断当前编号id节点seq的射线是否与目标射线相交，若是，则令sol＝sol+1，并记录当前到达目标点的射线轨迹编号，若否，则令id＝id+1，并返回步骤103c。

如果

则令sol＝sol+1，traj[sol]＝id；

103g、判断当前获取的射线轨迹的条数sol是否等于预设的路径数n_sol，若是，则执行步骤103h，若否，则id＝id+1，并返回步骤103c。

如果sol＝n_sol，转至步骤103h；否则，令id＝id+1，并转至步骤103c。

103h、输出多条不同辐射角的发射射线到目标点的射线轨迹

104、从目标点开始，对每一条射线轨迹进行修正，获得修正后的从起点到目标点的多条路径。

对链表

进行修整，得修整后的路径；在修整后的路径中选取最短路径，作为最终路径，具体地，如图7所示，本步骤包括：

104a、读取多条不同辐射角的发射射线到目标点的射线轨迹

并令迭代次数i＝1。

获取扩张后的链表L，traj，n_sol，令i＝1；

104b、判断迭代次数i是否小于等于预设的路径数n_sol，若是，则执行步骤104c，若否，则执行步骤104h。

如果i≤n_sol，继续下一步；否则，执行步骤105。

104c、将目标点x_goal设为迭代移动点x_e，将起始点x_start设为迭代起点x_s。

令xs＝start，xe＝goal，seq＝0，L_sol(traj(i),seq++)＝xs；

104d、判断迭代移动点x_e与迭代起点x_s的连线与障碍物边界是否有交点，若是则执行步骤104e，若否，则令i＝i+1，并将当前迭代移动点x_e的坐标设为迭代起点x_s，返回步骤104b。

判断xs与xe的连线是否穿越障碍物，如果是继续下一步；否则，令i＝i+1,L_sol(traj(i),seq++)＝xe，并转至步骤104b。

104e、迭代移动点x_e沿着射线轨迹按预设的步长移动一个步长。

xe沿L(traj(i))往前移动一个步长。

104f、判断迭代移动点x_e与迭代起点x_s的连线与障碍物边界是否有交点，若是则返回步骤104e，若否，则执行步骤104g。

判断xs与xe的连线是否穿越障碍物，如果是转至步骤104e，继续前移动一个步直至xs与xe的连线是与障碍物边界没有交点。

104g、将当前迭代移动点x_e的坐标设为迭代起点x_s，将目标点x_goal设为迭代移动点x_e，并返回步骤104d。

L_sol(traj(i),seq++)＝xe，xs＝xe，xe＝goal，转至步骤104d。

104h、输出修正后的从起点到目标点的多条路径。

获得修整后的路径L_sol，在L_sol中选取最短路径；

105、输出多个路径中最短的路径，获得规划路径。

本发明MATLAB仿真如图8所示。

本发明提出的算法在路径规划方面最大的优势是收敛速度快，即，本算法能在较短的时间内寻找到安全、可靠的路径。与本发明所提算法相比，其他算法在膨胀探索自由空间阶段会受到一定限制，使得它们在广阔海域、复杂海况中，不能有效、迅速的寻找到最优路径。如快速搜索随机树算法(Rapidly Exploring Random Tree，RRT)在膨胀阶段会受到步长的影响，步长决定了RRT算法的收敛速度，因为步长会影响算法在每次迭代过程中探索自由空间的范围。再如A*算法，在探索自由空间过程中，同样受到栅格大小的影响。上述类型的限制，会影响算法的收敛速度，使得算法在广阔海域、复杂海况中寻路效率非常低。还有基于图搜索的算法，如可视图法，Dijkstra算法等，此类算法的时间复杂度会受障碍物形状，顶点个数的影响，在复杂海况中，这些方法的搜索效率是非常低的。

本发明所提算法相比于其他算法的收敛速度优势，是由射线沿直线传播，遇到障碍物即止的特性决定的，也就是说，本算法的探索阶段是没有限制的，它以发射点与碰撞点之间的最大距离探索自由空间，直到到达目标区域。图9是本发明算法与A*算法的对比，图9中，a1为本发明算法循环1次搜索的区域示意图，a2为本发明算法循环4次搜索的区域示意图，a3为本发明算法循环16次搜索的区域示意图；b1为A*算法循环1次搜索的区域示意图，b2为A*算法循环4次搜索的区域示意图，b3为A*算法循环16次搜索的区域示意图；可见，本发明算法和A*算法在相同的循环次数下，探索空间远远比A*算法更大，因此，本算法能在较短的时间内寻找到安全、可靠的路径，在广阔海域、复杂海况中，能在规定时间内，有效、迅速的寻找到最优路径。

需要明确的是，本发明并不局限于上文所描述并在图中示出的特定配置和处理。为了简明起见，这里省略了对已知方法的详细描述。在上述实施例中，描述和示出了若干具体的步骤作为示例。但是，本发明的方法过程并不限于所描述和示出的具体步骤，本领域的技术人员可以在领会本发明的精神后，作出各种改变、修改和添加，或者改变步骤之间的顺序。

最后应说明的是：以上所述的各实施例仅用于说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述实施例所记载的技术方案进行修改，或者对其中部分或全部技术特征进行等同替换；而这些修改或替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。