一种UUV对矩形障碍物几何绕行的二维航路规划方法

摘要

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

说明书

技术领域

本发明涉及的是一种UUV航路规划方法，具体的是一种UUV对矩形障碍物几何绕行的二维航路规划方法。

背景技术

航路规划是水下无人航行器(Unmanned Underwater Vehicle，UUV)的关键技术之一，是UUV自主能力的重要体现。航路规划是指在已知障碍环境下，规划出一条从起点出发绕过所有障碍物并到达终点的无碰路径。根据空间维度，航路规划可分为二维航路规划和三维航路规划。而二维航路规划是三维航路规划的基础，并且在UUV的应用也更为广泛，是UUV航路规划技术研究的热点。

目前，UUV的航路规划方法很多，例如：申请号为201410011297.9的专利文件中公开了一种“运动障碍环境中UUV三维航路规划方法”；“无人水下自主航行器(AUV)避碰研究”(内蒙古大学硕士论文，2008年)；“无人水下航行器近海底空间路径规划方法”(《哈尔滨工程大学学报》2014年3月)等。但是如何在复杂的障碍环境下既快速又可行的获得一条无碰路径，特别是规划方法能够适于工程应用，仍然是一个难点。

与本发明较接近的背景技术是“基于几何算法的水下航行器路径规划”(《海军工程大学学报》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中，规划结束。

本发明还可以包括：

1、对矩形障碍物进行几何绕行的方法为：

(1)、首先判断规划当前点O_c是否在矩形障碍物上，如果不在转步骤(2)、如果在转步骤(4)；

(2)、找到距规划当前点O_c最近的顶点V；

(3)、V作为一个绕行点，放入绕行点集合S中，并令规划当前点O_c为点V；

(4)、找到与规划当前点O_c相邻的顶点V₁、V₂；

(5)、找到规划当前点O_c、顶点V₁和顶点V₂三点中距终点O_e最近的点，并将其定义为点O_n；

(6)、如果点O_n就是规划当前点O_c，点O_n作为绕行点放入绕行点集合S中，转步骤(8)；如果点O_n不是规划当前点O_c，执行步骤(7)；

(7)、令规划当前点O_c为点O_n，转步骤(4)；

(8)、绕行结束。

2、所述对矩形障碍物进行膨胀处理的方法为：

在正常矩形障碍物几何形状的基础上，按照形状边缘以安全半径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)。

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

膨胀后矩形障碍物为：

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

其中(x′_ld,y′_ld)、(x′_lu,y′_lu)、(x′_rd,y′_rd)和(x′_ru,y′_ru)分别表示膨胀后矩形各顶点的二维坐标，并且有：

$\left(\begin{array}{c}{x}_{ld}^{\prime }=(b_{ldu}-b_{lrd}+r_{uuv}\sqrt{1+k_{lrd}^2}-r_{uuv}\sqrt{1+k_{ldu}^2})/(k_{lrd}-k_{ldu})\\ y_{ld}^{\prime }=k_{lrd}{x}_{ld}^{\prime }+b_{lrd}-r_{uuv}\sqrt{1+k_{lrd}^2}\end{array}\right)$

$\left(\begin{array}{c}{x}_{rd}^{\prime }=(b_{rdu}-b_{lrd}+r_{uuv}\sqrt{1+k_{lrd}^2}-r_{uuv}\sqrt{1+k_{rdu}^2})/(k_{lrd}-k_{rdu})\\ y_{rd}^{\prime }=k_{lrd}{x}_{rd}^{\prime }+b_{lrd}-r_{uuv}\sqrt{1+k_{lrd}^2}\end{array}\right)$

$\left(\begin{array}{c}{x}_{lu}^{\prime }=(b_{lru}-b_{ldu}+r_{uuv}\sqrt{1+k_{ldu}^2}-r_{uuv}\sqrt{1+k_{lru}^2})/(k_{ldu}-k_{lru})\\ y_{lu}^{\prime }=k_{ldu}{x}_{lu}^{\prime }+b_{ldu}-r_{uuv}\sqrt{1+k_{ldu}^2}\end{array}\right)$

$\left(\begin{array}{c}{x}_{ru}^{\prime }=(b_{rdu}-b_{lru}+r_{uuv}\sqrt{1+k_{lru}^2}-r_{uuv}\sqrt{1+k_{ldu}^2})/(k_{lru}-k_{rdu})\\ y_{ru}^{\prime }=k_{lru}{x}_{ud}^{\prime }+b_{lru}-r_{uuv}\sqrt{1+k_{lru}^2}\end{array}\right)$

其中：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。

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

本发明的有益效果在于：

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

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

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

附图说明

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

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

图3矩形障碍物的膨胀示意图；

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

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

图6 UUV对矩形障碍物的几何绕行的流程图；

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

图8 UUV对矩形障碍物几何绕行的二维航路规划流程图；

图9利用本发明进行UUV对矩形障碍物的几何绕行的效果图。

具体实施方式

下面结合附图举例对本发明作进一步描述。

结合图1介绍本发明的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)

设二维几何空间中存在一定数量的矩形障碍物，如图1所示，设矩形障碍物为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)(3)

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

结合图2和图3介绍矩形障碍物膨胀模型的建立方法。

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

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

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)分别表示膨胀后各顶点的二维坐标，并且有：

$\{\begin{array}{c}{x}_{ld}^{\prime }=(b_{ldu}-b_{lrd}+r_{uuv}\sqrt{1+k_{lrd}^2}-r_{uuv}\sqrt{1+k_{ldu}^2})/(k_{lrd}-k_{ldu})\\ y_{ld}^{\prime }=k_{lrd}{x}_{ld}^{\prime }+b_{lrd}-r_{uuv}\sqrt{1+k_{lrd}^2}\end{array}---\left(5\right)$

$\{\begin{array}{c}{x}_{rd}^{\prime }=(b_{rdu}-b_{lrd}+r_{uuv}\sqrt{1+k_{lrd}^2}-r_{uuv}\sqrt{1+k_{rdu}^2})/(k_{lrd}-k_{rdu})\\ y_{rd}^{\prime }=k_{lrd}{x}_{rd}^{\prime }+b_{lrd}-r_{uuv}\sqrt{1+k_{lrd}^2}\end{array}---\left(6\right)$

$\left(\begin{array}{c}{x}_{lu}^{\prime }=(b_{lru}-b_{ldu}+r_{uuv}\sqrt{1+k_{ldu}^2}-r_{uuv}\sqrt{1+k_{lru}^2})/(k_{ldu}-k_{lru})\\ y_{lu}^{\prime }=k_{ldu}{x}_{lu}^{\prime }+b_{ldu}-r_{uuv}\sqrt{1+k_{ldu}^2}\end{array}\right)---\left(7\right)$

$\left(\begin{array}{c}{x}_{ru}^{\prime }=(b_{rdu}-b_{lru}+r_{uuv}\sqrt{1+k_{lru}^2}-r_{uuv}\sqrt{1+k_{ldu}^2})/(k_{lru}-k_{rdu})\\ y_{ru}^{\prime }=k_{lru}{x}_{ud}^{\prime }+b_{lru}-r_{uuv}\sqrt{1+k_{lru}^2}\end{array}\right)---\left(8\right)$

式中：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对矩形障碍物的绕行方法。

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

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

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

综合以上两种情况，图6给出了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，转步骤四。

步骤八：绕行结束。

结合图8介绍UUV对矩形障碍物几何绕行的二维航路规划的整个流程。

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

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

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

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

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

步骤六：搜索距规划当前点O_c最近的矩形障碍物；搜索方法为首先找到阻碍当前点O_c和航路终点O_e连线的所有矩形障碍物，然后求解以上各矩形障碍物中心与当前点O_c的距离，距离最小的即为距规划当前点O_c最近的矩形障碍物；

步骤七：对距规划当前点O_c最近的矩形障碍物进行几何绕行；

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

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

步骤十：将航路终点O_e放入绕行点集合S中，规划结束。

所述判断规划当前点O_c和航路终点O_e是否可视的方法为：

点O_c和点O_e可视是指两点不被任何矩形障碍物所阻挡。判断两点是否可视的方法就是判断两点连线所形成的线段是否与所有的矩形障碍物相交，如果不与任何矩形障碍物相交则表明两点可视。判断规划当前点O_c和航路终点O_e是否可视的流程如图7所示：

步骤一：选择第一个矩形障碍物

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

$x_{plrd}=\frac{b_{lrd}-b_{ce}}{k_{ce}-k_{lrd}},x_{pldu}=\frac{b_{ldu}-b_{ce}}{k_{ce}-k_{ldu}},x_{plru}=\frac{b_{lru}-b_{ce}}{k_{ce}-k_{lru}},x_{prdu}=\frac{b_{rdu}-b_{ce}}{k_{ce}-k_{rdu}}---\left(9\right)$

式中：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_ce和b_ce分别表示O_c和O_e所连成直线的斜率和截距，并且有b_ce＝y_oc-k_cex_oc；

步骤三：如果求解的x_plrd存在，且满足x_plrd的值在x_oc和x_oe之间，且在x_ld和x_rd之间，转步骤八，否则转步骤四；

步骤四：如果求解的x_pldu存在，且满足x_pldu的值在x_oc和x_oe之间，且在x_ld和x_lu之间，转步骤八，否则转步骤五；

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

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

步骤七：判断是否还有矩形障碍物，如果有，选择下一个矩形障碍物，转步骤二，否则转步骤九；

步骤八：当前点O_c和航路终点O_e不可视，判断结束；

步骤九：当前点O_c和航路终点O_e可视，判断结束。

图9给出了利用本发明进行UUV对矩形障碍物几何绕行二维航路规划的一个实现实例。

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