CN105929824B

CN105929824B - 一种基于几何绕行原理的uuv二维航路规划方法

Info

Publication number: CN105929824B
Application number: CN201610312431.0A
Authority: CN
Inventors: 陈涛; 徐达; 王玉柱; 周佳加; 徐健; 张宏瀚
Original assignee: Harbin Engineering University
Current assignee: Harbin Engineering University
Priority date: 2016-05-12
Filing date: 2016-05-12
Publication date: 2018-10-26
Anticipated expiration: 2036-05-12
Also published as: CN105929824A

Abstract

本发明提供的是一种基于几何绕行原理的UUV二维航路规划方法。一：从使命文本读取航路起点O_b、航路终点O_e和各障碍物的参数；二：对障碍物进行膨胀处理，计算膨胀后的各障碍物的参数；三：建立绕行点集合S；四：如果规划当前点O_c是航路终点O_e，或者规划当前点O_c和航路终点O_e可视，转步骤六，否则执行步骤五；五：搜索距规划当前点O_c最近的障碍物，根据障碍物的形状对障碍物进行绕行，得到绕行点并放入绕行点集合S中，更新规划当前点O_c，转步骤四；六：将航路终点O_e放入绕行点集合S中；七：对绕行点集合S进行消减，规划结束。本发明通过简单的几何原理实现对障碍物的绕行，可以使UUV在复杂障碍环境中快速、高效的获得一条安全无碰的二维航路。

Description

一种基于几何绕行原理的UUV二维航路规划方法

技术领域

本发明涉及的是一种UUV航路规划方法，具体的是一种基于几何绕行原理的UUV二维航路规划方法。

背景技术

航路规划是水下无人航行器(Unmanned Underwater Vehicle，UUV)的关键技术之一，是UUV自主能力的重要体现。航路规划是指在已知障碍环境下，规划出一条从起点出发绕过所有障碍物并到达终点的无碰路径。根据空间维度，航路规划可分为二维航路规划和三维航路规划。其中，二维航路规划是三维航路规划的基础，并且在UUV的应用也更为广泛，是UUV航路规划技术研究的热点。目前，UUV的航路规划方法很多，但是如何在复杂的障碍环境下既快速又可行的获得一条无碰路径，特别是规划方法能够适于工程应用，仍然是一个难点。

“基于几何算法的水下航行器路径规划”(《海军工程大学学报》，2009，21(6)：41-44页)中提到了将几何算法应用到水下航行器的路径规划中，属于与本发明申请接近的现有技术。但是该文献只考虑了圆形障碍物，并且对障碍物的绕行方法与本发明完全不同。

发明内容

本发明的目的是提供一种计算简单、规划效率高、规划速度快的基于几何绕行原理的UUV二维航路规划方法。

本发明的目的是这样实现的：

步骤一：从使命文本读取航路起点O_b、航路终点O_e和各障碍物的参数；

步骤二：分别对各障碍物进行膨胀处理，计算膨胀后的各障碍物的参数；

步骤三：建立绕行点集合S，令规划当前点O_c为起点O_b，并放入绕行点集合S中；

步骤四：如果规划当前点O_c是航路终点O_e，或者规划当前点O_c和航路终点O_e可视，转步骤六，否则执行步骤五；

步骤五：搜索距规划当前点O_c最近的障碍物，根据障碍物的形状对障碍物进行绕行，得到绕行点并放入绕行点集合S中，更新规划当前点O_c，转步骤四；

步骤六：将航路终点O_e放入绕行点集合S中；

步骤七：对绕行点集合S进行消减，得到消减后的绕行点集合S，规划结束。

本发明还可以包括：

1、所述根据障碍物的形状对障碍物进行绕行具体包括：

(1)对圆形障碍物的绕行的步骤为：

(1.1)判断规划当前点O_c是否在圆形障碍物上，如果在，令点O₁＝O_c；否则，求解规划当前点O_c和圆心O的连线与圆周的交点，并令其为点O₁；

(1.2)求解航路终点O_e和圆心O的连线与圆周的交点，并令其为点O₂；

(1.3)求解∠O₁OO₂的角平分线与圆周的交点，并令其为点O₃；

(1.4)求解过点O₁的圆的切线与过点O₃的圆的切线的交点，并令其为点O₄；

(1.5)计算过点O₂的圆的切线与过点O₃的圆的切线的交点，并令其为点O₅；

(1.6)将点O₁、点O₄、点O₅、点O₂作为圆形障碍物的绕行点放入绕行点集合S中，并更新规划当前点O_c为点O₂，绕行结束；

(2)对矩形障碍物绕行的步骤为：

(2.1)判断规划当前点O_c是否在矩形障碍物上，如果不在，找到距O_c最近的顶点V，将顶点V作为绕行点放入绕行点集合S中，并更新规划当前点O_c为顶点V；

(2.2)找到与点O_c相邻的顶点V₁、顶点V₂，如果点O_c、顶点V₁和顶点V₂三点中距终点O_e最近的点是点O_c，转步骤(2.4)；否则执行步骤(2.3)；

(2.3)令规划当前点O_c为顶点V₁和顶点V₂中距终点O_e近的点，将点O_c作为绕行点放入绕行点集合S中，转步骤(2.2)；

(2.4)点O_c作为绕行点放入绕行点集合S中，绕行结束。

2、所述对障碍物进行膨胀处理的膨胀方法是在正常障碍物几何形状的基础上，按照其形状边缘以安全半径r_uuv向外扩展出一个安全半径区域。

3、所述对绕行点集合S进行消减的方法是将所有可视绕行点之间的绕行点去掉，具体过程为：

步骤一：定义三个计数变量T1、T2、T3，并都初始化为0，获取绕行点集合S中绕行点的个数N；

步骤二：令T1为绕行点集合S中绕行点的个数N；

步骤三：令T2＝T1-1；

步骤四：判断绕行点S(T1)和S(T2)是否可视，如果可视令T3＝T2，转步骤五，如果不可视转步骤五；

其中，S(T1)∈S，S(T1)表示第T1个绕行点；S(T2)∈S，S(T2)表示第T2个绕行点；

步骤五：令T2＝T2-1，判断T2是否为0，如果为0转步骤六，否则转步骤四

步骤六：消去绕行点集合S中点S(T1)和S(T3)之间的所有绕行点；

步骤七：令T1＝T3，判断T1是否为1，如果为1，对绕行点集合S的消减结束；否则转步骤三。

本发明利用几何原理进行UUV的航路规划，在环境模型上采用了简单的几何模型，在计算无碰路径时采用简单的几何原理对障碍物进行绕行，避免了其他规划方法需要建立地图、循环搜索无碰路径所引起的信息量大、计算复杂的问题，不仅规划效率高、规划速度快，而且原理简单、计算量小，易于工程实现。

虽然“基于几何算法的水下航行器路径规划”中提到了将几何算法应用到水下航行器的路径规划中，也考虑了圆形障碍物。但本发明对圆形障碍物的碰撞部分与其主要区别在于：

1、本发明对圆形障碍物进行了膨胀处理，而背景技术“基于几何算法的水下航行器路径规划”没有进行膨胀处理。对圆形障碍物进行膨胀处理，避免了UUV沿规划航路航行时与圆形障碍物的碰撞，提高了UUV的航行安全性。

2、本发明和背景技术“基于几何算法的水下航行器路径规划”对单个圆形障碍物的绕行方法不同。本发明的绕行方法如图4和图5所示，形成的是多条线段组成的绕行航路；而背景技术形成的是线段和圆弧相结合的绕行航路，而圆弧航路不利于UUV的航路跟踪控制。本发明只有线段的绕行航路更利于UUV的航路跟踪控制，可以提高UUV的航路跟踪效果。

本发明的有益效果在于：

1、环境模型采用的是几何空间模型，相比于传统的栅格、地图模型，所需规划信息量少，规划效率高，特别适合复杂多障碍物的环境。

2、对障碍物的绕行算法只应用到了几何原理，计算简单、易于工程实现，而且计算量非常小，规划速度快。

3、以UUV的外形尺寸为安全半径对障碍物进行了膨胀处理，避免了UUV沿规划航路航行时与障碍物的碰撞。

附图说明

图1a规划环境模型中的圆形障碍物示意图，图1b规划环境模型中的矩形障碍物示意图；

图2障碍物膨胀处理的安全半径示意图；

图3a圆形障碍物的膨胀示意图，图3b矩形障碍物的膨胀示意图；

图4规划当前点不在圆形障碍物上时对圆形障碍物的绕行示意图；

图5规划当前点在圆形障碍物上时对圆形障碍物的绕行示意图；

图6圆形障碍物的绕行流程图；

图7规划当前点不在矩形障碍物上时对矩形障碍物的绕行示意图；

图8规划当前点在矩形障碍物上时对矩形障碍物的绕行示意图；

图9矩形障碍物的绕行流程图；

图10判断两点连成线段与圆形障碍物是否相交的流程图；

图11判断两点连成线段与矩形障碍物是否相交的流程图；

图12对绕行点集合S进行消减的示意图；

图13对绕行点集合S进行消减的流程图；

图14基于几何绕行原理的UUV二维航路规划流程图；

图15利用本发明进行UUV二维航路规划的效果图。

具体实施方式

下面结合附图举例对本发明做更详细的描述。

结合图1a-图1b介绍UUV二维航路规划的环境模型。

本发明中航路规划的环境模型采用的是二维几何空间模型。设规划的航路起点为O_b，航路终点为O_e，O_b和O_e分别用二维坐标表示为：

O_b＝(x_ob,y_ob)；O_e＝(x_oe,y_oe) (1)

另设航路规划过程中每一步用到的规划当前点为O_c，用二维坐标表示为：

O_c＝(x_oc,y_oc) (2)

设二维几何空间中存在一定数量的障碍物，并且用圆形和矩形来表示障碍物(如图1a-图1b所示)。

圆形障碍物为Z_circ，其参数化表示为：

Z_circ＝(x_circ,y_circ,r) (3)

式中，(x_circ,y_circ)表示圆心的二维坐标，r表示圆形障碍物的半径

矩形障碍物为Z_rect，其参数化表示为：

Z_rect＝(x_rect,y_rect,ψ_rect,x_ld,y_ld,x_lu,y_lu,x_rd,y_rd,x_ru,y_ru) (4)

式中，(x_rect,y_rect)表示矩形中心的二维坐标，ψ_rect表示矩形以X轴为参考的旋转角度，(x_ld,y_ld)、(x_lu,y_lu)、(x_rd,y_rd)、(x_ru,y_ru)分别表示矩形左下、左上、右下和右下顶点的二维坐标。

结合图2和图3a、图1b介绍障碍物膨胀模型的建立方法。

进行航路规划时，一般是把UUV当作质点来考虑的，因此规划的航路可能会距障碍物较近。但是实际上，UUV是有几何尺寸的实体，当规划的航路距障碍物较近时，很有可能导致UUV与障碍物发生碰撞。为此在进行航路规划时，设置一个安全半径来防止UUV沿规划航路航行时与障碍物发生碰撞。本发明采用的方法是以UUV的外形尺寸的外接圆半径r_uuv为安全半径(见图2所示)，然后在正常障碍物的几何形状的基础上，按照其形状边缘以半径r_uuv向外扩展出一个安全半径区域。图3a-图3b给出了圆形和矩形障碍物向外扩展安全半径后的膨胀示意图。

膨胀后圆形障碍物的参数化表示为：

Z′_circ＝(x_circ,y_circ,r′) (5)

式中：(x_circ,y_circ)仍然表示圆心的二维坐标；而r′＝r+r_uuv表示膨胀后的圆形障碍物半径。

膨胀后矩形障碍物的参数化表示为：

Z′_rect＝(x_rect,y_rect,ψ_rect,x′_ld,y′_ld,x′_lu,y′_lu,x′_rd,y′_rd,x′_ru,y′_ru) (6)

式中：(x_rect,y_rect)仍然表示矩形中心的二维坐标，ψ_rect仍然表示矩形以X轴为参考的旋转角度；而(x′_ld,y′_ld)、(x′_lu,y′_lu)、(x′_rd,y′_rd)和(x′_ru,y′_ru)分别表示膨胀后各顶点的二维坐标，并且有：

式中：k_lrd、b_lrd分别表示矩形左下顶点和右下顶点所连直线的斜率和截距，k_ldu、b_ldu分别表示矩形左上顶点和左下顶点所连直线的斜率和截距，k_lru、b_lru分别表示矩形左上顶点和右上顶点所连直线的斜率和截距，k_rdu、b_rdu分别表示矩形右上顶点和右下顶点所连直线的斜率和截距，并且有：

b_lrd＝y_ld-k_lrdx_ld，b_ldu＝y_ld-k_ldux_ld，b_lru＝y_lu-k_lrux_lu，b_rdu＝y_rd-k_rdux_rd。

结合图4、图5、图6介绍UUV对圆形障碍物的绕行方法。

对圆形障碍物的绕行分为规划当前点在圆形障碍物上和不在圆形障碍物上两种情况，图4给出了规划当前点不在圆形障碍物上的绕行示意图，图5给出了规划当前点在圆形障碍物上的绕行示意图。从图4和图5可以看出，对圆形障碍物的绕行，采用的是利用多条圆的切线的交点作为绕行点绕行圆形障碍物的方法，并利用几何原理求解各绕行点。

图6给出了圆形障碍物的绕行流程。

步骤一：判断规划当前点O_c是否在障碍物上，如果在障碍物上，令O₁＝O_c，转步骤三；否则转步骤二；

步骤二：求解规划当前点O_c和圆心O的连线与圆的交点O₁＝(x_O1,y_O1)^T，O₁的位置坐标按式(11)计算，有两个解，选取与当前点O_c距离近的解作为点O₁的坐标。

式中：k_c表示规划当前点O_c和圆心O所连直线的斜率，并且有

步骤三：求解航路终点O_e和圆心O的连线与圆的交点O₂＝(x_O2,y_O2)^T，O₂的位置坐标按式(12)计算，有两个解，选取与航路终点O_e距离近的解作为点O₂的坐标。

式中：k_e表示航路终点O_e和圆心O所连直线的斜率，并且有

步骤四：求解∠O₁OO₂的角平分线L₀与圆的交点O₃＝(x_O3,y_O3)^T，O₃的位置坐标按式(13)计算，有两个解，选取与点O₁(或点O₂)距离近的解作为点O₃的坐标。

式中：表示∠O₁OO₂的角平分线L₀的斜率，并且有

步骤五：分别求解过点O₁、O₂、O₃的圆的切线L₁、L₂、L₃，其切线方程分别按式(14)、式(15)和式(16)计算。

式中：k_L1、b_L1分别表示圆的切线L₁的斜率和截距；k_L2、b_L2分别表示圆的切线L₂的斜率和截距；k_L3、b_L3分别表示圆的切线L₃的斜率和截距。

步骤六：计算切线L₁和L₃的交点O₄＝(x_O4,y_O4)，O₄的位置坐标按式(17)计算

步骤七：计算切线L₂和L₃的交点O₅＝(x_O5,y_O5)，O₅的位置坐标按式(18)计算

步骤八：将点O₁、O₄、O₅、O₂即为圆形障碍物的绕行点放入绕行点集合S中，并更新当前点O_c为O₂，绕行结束。

结合图7、图8、图9，介绍UUV对矩形障碍物的绕行方法。

对矩形障碍物进行绕行时，将矩形障碍物的四个顶点作为候选绕行点。根据规划当前点O_c是否在矩形障碍物上分为两种绕行情况。

如图7所示，给出了规划当前点O_c不在矩形障碍物上的绕行示意。其绕行过程可以简述为：首先找到矩形障碍物的四个顶点中距离当前点O_c最近的顶点作为UUV的下一个绕行点，同时更新该顶点为O_c。然后令O_c以及与O_c相邻的两个顶点中距离目标点O_e最近的点为下一个绕行点，并更新该点为O_c。直到矩形障碍物四个顶点中只有规划当前点O_c到目标点O_e的距离最近，绕行结束。

如图8所示，给出了规划当前点O_c已经在矩形障碍物上的绕行示意。只需直接令O_c以及与O_c相邻的两个顶点中距离目标点O_e更近的点为下一个绕行点，并更新该点为O_c。直到矩形障碍物四个顶点中只有规划当前点O_c到目标点O_e的距离最近，绕行结束。

综合以上两种情况，图9给出了UUV对矩形障碍物的绕行流程：

步骤一：首先判断规划当前点O_c是否在矩形障碍物上，如果不在转步骤二；如果在转步骤四；

步骤二：找到距规划当前点O_c最近的顶点V；

步骤三：V作为一个绕行点，放入绕行点集合S中，并令规划当前点O_c为点V；

步骤四：找到与规划当前点O_c相邻的顶点V₁、V₂；

步骤五：找到点O_c、V₁和V₂三点中距终点O_e最近的点，并将其定义为点O_n；

步骤六：如果O_n就是规划当前点O_c，O_n作为绕行点O_n放入绕行点集合S中，转步骤八；如果O_n不是规划当前点O_c，转步骤七；

步骤七：令规划当前点O_c为点O_n，转步骤四；

步骤八：绕行结束。

结合图10、图11介绍判断两点是否可视的方法。

两点可视是指两点不被任何障碍物所阻挡。判断两点是否可视的方法就是判断两点连线所形成的线段是否与所有的障碍物相交，如果不与任何障碍物相交则表明两点可视。由于本发明只考虑圆形和矩形障碍物，下面介绍两点连成的线段与圆形和矩形障碍物是否相交的判断方法。

设任意两点为P₁＝(x_p1,y_p1)和P₂＝(x_p2,y_p2)，圆形障碍物和矩形障碍物的参数化表示如式(3)和式(4)所示。那么点P₁和P₂连成的线段与圆形障碍物是否相交的判断过程如图10所示：

步骤一：求解点P₁和点P₂两点连线和圆联立的二次方程的根的判别式Δ，求解方法如式(19)所示：

式中，k_p和b_p分别表示点P₁和点P₂所连成直线的斜率和截距，并且有b_p＝y_p1-k_px_p1；

步骤二：判断根的判别式Δ是否大于等于0，如果大于等于0转步骤三，否则转步骤六

步骤三：求解点P₁和点P₂两点连线和圆的两个交点的横坐标x_pc1和x_pc2，求解方法如式(20)所示：

步骤四：判断x_pc1的值在x_oc和x_oe之间或x_pc2值在x_oc和x_oe之间是否满足，如果满足转步骤五，否则转步骤六；

步骤五：点P₁和点P₂相交，判断结束；

步骤六：点P₁和点P₂不相交，判断结束。

点P₁和P₂连成的线段与矩形障碍物是否相交的判断过程如图11所示，具体为：

步骤一：求解P₁、P₂两点连线与矩形障碍物四条边所在直线的交点的横坐标x_plrd、x_pldu、x_plru、x_prdu，求解方法如式(21)所示：

式中：x_plrd表示O_c和O_e连线与矩形障碍物左下和右下顶点连线的交点的横坐标；x_pldu表示O_c和O_e连线与矩形障碍物左下和左上顶点连线的交点的横坐标；x_plru表示O_c和O_e连线与矩形障碍物左上和右上顶点连线的交点的横坐标；x_prdu表示O_c和O_e连线与矩形障碍物右上和右下顶点连线的交点的横坐标。k_lrd、b_lrd、k_ldu、b_ldu、k_lru、b_lru、k_rdu、b_rdu、k_p和b_p的含义和计算公式同前。

步骤二：如果求解的x_plrd存在，且满足x_plrd的值在x_p1和x_p2之间，且在x_ld和x_rd之间，转步骤六，否则转步骤三；

步骤三：如果求解的x_pldu存在，且满足x_pldu的值在x_p1和x_p2之间，且在x_ld和x_lu之间，转步骤六，否则转步骤四；

步骤四：如果求解的x_plru存在，且满足x_plru的值在x_p1和x_p2之间，且在x_lu和x_ru之间，转步骤六，否则转步骤五；

步骤五：如果求解的x_prdu存在，且满足x_prdu的值在x_p1和x_p2之间，且在x_rd和x_ru之间，转步骤六，否则转步骤七；

步骤六：点P₁和点P₂相交，判断结束；

步骤七：点P₁和点P₂不相交，判断结束。

结合图12和图13，介绍UUV对绕行点集合S进行消减的方法：

绕行点集合S中的所有绕行点，并不是最终的绕行航路点，因为集合S中的有些绕行点是非必须绕行点，可以消减掉，从而得到最简绕行点集合而才是最终的绕行航路点。对绕行点集合进行消减的关键就是判断绕行点之间是否可视，只要将所有可视绕行点之间的绕行点消减掉，就得到了最简绕行点集合。图12给出了对绕行点进行消减的示意图。如图12所示，从起点O_b到终点O_e的未消减绕行点集合为S＝{O_b,P₁,P₂,P₃,P₄,P₅,P₆,O_e}，而消减后的最简绕行点集合为图13给出了对绕行点集合进行消减的过程：

步骤二：令T1为绕行点集合S中绕行点的个数N；

步骤三：令T2＝T1-1；

结合图14介绍基于几何绕行原理的UUV二维航路规划的整个流程。

步骤二：建立障碍物膨胀模型，计算膨胀后的各障碍物的参数，建立绕行点集合S；

步骤三：令规划当前点O_c为起点O_b，并放入绕行点集合S中；

步骤四：判断规划当前点O_c是不是航路终点O_e，如果是转步骤十二，否则转步骤五；

步骤五：判断规划当前点O_c和航路终点O_e是否可视，如果可视转步骤十二，否则转步骤六；

步骤六：搜索距规划当前点O_c最近的障碍物，对其进行绕行；搜索方法为求解各障碍物中心(圆心或矩形中心)与当前点O_c的距离，距离最小的即为距规划当前点O_c最近的障碍物；

步骤七：判断需绕行障碍物的形状，如果是圆形障碍物，转步骤八，如果是矩形障碍物转步骤九；

步骤八：进行圆形障碍物绕行，绕行结束后转步骤十；

步骤九：进行矩形障碍物绕行，绕行结束后转步骤十；

步骤十：将绕行点放入绕行点集合S中；

步骤十一：更新规划当前点O_c，转步骤四；

步骤十二：将航路终点O_e放入绕行点集合S中；

步骤十三：对绕行点集合S进行消减，规划结束。

图15给出了利用本发明进行UUV航路规划的一个实现案例。

本案例中，共设置了5个矩形障碍物和5个圆形障碍物，航路的起点O_b和航路的终点O_e已在图中标出。首先，在规划时对每个障碍物进行了膨胀处理，各障碍物的膨胀边界已在图中用点划线标出。然后，对部分障碍物进行了绕行，得到了原始的绕行点集合S＝{O_b,P₁,P₂,P₃,…,P₁₄,O_e}，并在图中用虚线表示出了未消减的绕行航路。最后对绕行点集合进行消减，得到消减后的绕行点集合并用实线表示出了消减后的最终绕行航路。

Claims

1.一种基于几何绕行原理的UUV二维航路规划方法，其特征是：

步骤四：如果规划当前点O_c是航路终点O_e、或者规划当前点O_c和航路终点O_e可视，转步骤六，否则执行步骤五；

步骤六：将航路终点O_e放入绕行点集合S中；

步骤七：对绕行点集合S进行消减，得到消减后的绕行点集合规划结束；

所述根据障碍物的形状对障碍物进行绕行具体包括：

(1)对圆形障碍物的绕行的步骤为：

(2)对矩形障碍物绕行的步骤为：

(2.4)点O_c作为绕行点放入绕行点集合S中，绕行结束。

2.根据权利要求1所述的基于几何绕行原理的UUV二维航路规划方法，其特征是：所述对障碍物进行膨胀处理的方法是在正常障碍物几何形状的基础上，按照其形状边缘以安全半径r_uuv向外扩展出一个安全半径区域。

3.根据权利要求1或2所述的基于几何绕行原理的UUV二维航路规划方法，其特征是：所述对绕行点集合S进行消减的方法是将所有可视绕行点之间的绕行点去掉，具体过程为：

a：定义三个计数变量T1、T2、T3，并都初始化为0，获取绕行点集合S中绕行点的个数N；

b：令T1为绕行点集合S中绕行点的个数N；

c：令T2＝T1-1；

d：判断绕行点S(T1)和S(T2)是否可视，如果可视令T3＝T2，转步骤e，如果不可视转步骤e；

e：令T2＝T2-1，判断T2是否为0，如果为0转步骤f，否则转步骤d；

f：消去绕行点集合S中点S(T1)和S(T3)之间的所有绕行点；

g：令T1＝T3，判断T1是否为1，如果为1，对绕行点集合S的消减结束；否则转步骤c。