一种基于几何绕行原理的uuv二维航路规划方法
【专利摘要】本发明提供的是一种基于几何绕行原理的UUV二维航路规划方法。一：从使命文本读取航路起点Ob、航路终点Oe和各障碍物的参数；二：对障碍物进行膨胀处理，计算膨胀后的各障碍物的参数；三：建立绕行点集合S；四：如果规划当前点Oc是航路终点Oe，或者规划当前点Oc和航路终点Oe可视，转步骤六，否则执行步骤五；五：搜索距规划当前点Oc最近的障碍物，根据障碍物的形状对障碍物进行绕行，得到绕行点并放入绕行点集合S中，更新规划当前点Oc，转步骤四；六：将航路终点Oe放入绕行点集合S中；七：对绕行点集合S进行消减，规划结束。本发明通过简单的几何原理实现对障碍物的绕行，可以使UUV在复杂障碍环境中快速、高效的获得一条安全无碰的二维航路。
【专利说明】
一种基于几何绕行原理的uuv二维航路规划方法
技术领域
[0001 ]本发明涉及的是一种UUV航路规划方法，具体的是一种基于几何绕行原理的UUV二 维航路规划方法。
【背景技术】
[0002] 航路规划是水下无人航行器(Unmanned Underwater Vehicle，UUV)的关键技术之 一，是UUV自主能力的重要体现。航路规划是指在已知障碍环境下，规划出一条从起点出发 绕过所有障碍物并到达终点的无碰路径。根据空间维度，航路规划可分为二维航路规划和 三维航路规划。其中，二维航路规划是三维航路规划的基础，并且在UUV的应用也更为广泛， 是UUV航路规划技术研究的热点。目前，UUV的航路规划方法很多，但是如何在复杂的障碍环 境下既快速又可行的获得一条无碰路径，特别是规划方法能够适于工程应用，仍然是一个 难点。
[0003] "基于几何算法的水下航行器路径规划"（《海军工程大学学报》，soogdKehu-dd 页） 中提到了将几何算法应用到水下航行器的路径规划中，属于与本发明 申请接近的现 有技术。但是该文献只考虑了圆形障碍物，并且对障碍物的绕行方法与本发明完全不同。
【发明内容】
[0004] 本发明的目的是提供一种计算简单、规划效率高、规划速度快的基于几何绕行原 理的UUV二维航路规划方法。
[0005] 本发明的目的是这样实现的：
[0006] 步骤一:从使命文本读取航路起点〇b、航路终点〇e和各障碍物的参数；
[0007] 步骤二:分别对各障碍物进行膨胀处理，计算膨胀后的各障碍物的参数；
[0008] 步骤三:建立绕行点集合S，令规划当前点0。为起点0b，并放入绕行点集合S中；
[0009] 步骤四：如果规划当前点0。是航路终点0e，或者规划当前点0。和航路终点0e可视， 转步骤六，否则执行步骤五；
[0010] 步骤五:搜索距规划当前点0。最近的障碍物，根据障碍物的形状对障碍物进行绕 行，得到绕行点并放入绕行点集合S中，更新规划当前点0。，转步骤四；
[0011] 步骤六:将航路终点a放入绕行点集合S中；
[0012] 步骤七:对绕行点集合S进行消减，得到消减后的绕行点集合S，规划结束。
[0013] 本发明还可以包括：
[0014] 1、所述根据障碍物的形状对障碍物进行绕行具体包括：
[0015] (1)对圆形障碍物的绕行的步骤为：
[0016] (1.1)判断规划当前点0。是否在圆形障碍物上，如果在，令点(^ = 0。;否则，求解规 划当前点〇。和圆心〇的连线与圆周的交点，并令其为点〇i;
[0017] (1.2)求解航路终点0e和圆心0的连线与圆周的交点，并令其为点02;
[0018] (1.3)求解ZChOCfe的角平分线与圆周的交点，并令其为点03;
[0019] (1.4)求解过点Oi的圆的切线与过点〇3的圆的切线的交点，并令其为点〇4;
[0020] (1.5)计算过点02的圆的切线与过点〇3的圆的切线的交点，并令其为点〇5;
[0021] (1.6)将点(h、点〇4、点05、点02作为圆形障碍物的绕行点放入绕行点集合S中，并更 新规划当前点0。为点〇2，绕行结束；
[0022] (2)对矩形障碍物绕行的步骤为：
[0023] (2.1)判断规划当前点0。是否在矩形障碍物上，如果不在，找到距0。最近的顶点V， 将顶点V作为绕行点放入绕行点集合S中，并更新规划当前点0。为顶点V;
[0024] (2.2)找到与点0。相邻的顶点Vi、顶点V2，如果点0。、顶点Vi和顶点V2三点中距终点 〇e最近的点是点0。，转步骤(2.4);否则执行步骤(2.3);
[0025] (2.3)令规划当前点0。为顶点Vi和顶点乂2中距终点0e近的点，将点0。作为绕行点放 入绕行点集合S中，转步骤(2.2);
[0026] (2.4)点0。作为绕行点放入绕行点集合S中，绕行结束。
[0027] 2、所述对障碍物进行膨胀处理的膨胀方法是在正常障碍物几何形状的基础上，按 照其形状边缘以安全半向外扩展出一个安全半径区域。
[0028] 3、所述对绕行点集合S进行消减的方法是将所有可视绕行点之间的绕行点去掉， 具体过程为：
[0029]步骤一:定义三个计数变量11、了2、了3，并都初始化为0，获取绕行点集合5中绕行点 的个数N;
[0030] 步骤二:令T1为绕行点集合S中绕行点的个数N;
[0031] 步骤三:令 T2 = T1_1;
[0032] 步骤四：判断绕行点S(T1)和S(T2)是否可视，如果可视令T3 = T2,转步骤五，如果 不可视转步骤五；
[0033] 其中，S(T1)GS，S(T1)表示第II个绕行点;S(T2)GS，S(T2)表示第T2个绕行点； [0034] 步骤五:令T2 = T2-1，判断T2是否为0，如果为0转步骤六，否则转步骤四
[0035] 步骤六:消去绕行点集合S中点S(T1)和S(T3)之间的所有绕行点；
[0036] 步骤七:令T1=T3,判断T1是否为1，如果为1，对绕行点集合S的消减结束；否则转 步骤三。
[0037]本发明利用几何原理进行UUV的航路规划，在环境模型上采用了简单的几何模型， 在计算无碰路径时采用简单的几何原理对障碍物进行绕行，避免了其他规划方法需要建立 地图、循环搜索无碰路径所引起的信息量大、计算复杂的问题，不仅规划效率高、规划速度 快，而且原理简单、计算量小，易于工程实现。
[0038] 虽然"基于几何算法的水下航行器路径规划"中提到了将几何算法应用到水下航 行器的路径规划中，也考虑了圆形障碍物。但本发明对圆形障碍物的碰撞部分与其主要区 别在于：
[0039] 1、本发明对圆形障碍物进行了膨胀处理，而【背景技术】"基于几何算法的水下航行 器路径规划"没有进行膨胀处理。对圆形障碍物进行膨胀处理，避免了 UUV沿规划航路航行 时与圆形障碍物的碰撞，提高了 UUV的航行安全性。
[0040] 2、本发明和【背景技术】"基于几何算法的水下航行器路径规划"对单个圆形障碍物 的绕行方法不同。本发明的绕行方法如图4和图5所示，形成的是多条线段组成的绕行航路； 而【背景技术】形成的是线段和圆弧相结合的绕行航路，而圆弧航路不利于UUV的航路跟踪控 制。本发明只有线段的绕行航路更利于UUV的航路跟踪控制，可以提高UUV的航路跟踪效果。
[0041] 本发明的有益效果在于：
[0042] 1、环境模型采用的是几何空间模型，相比于传统的栅格、地图模型，所需规划信息 量少，规划效率高，特别适合复杂多障碍物的环境。
[0043] 2、对障碍物的绕行算法只应用到了几何原理，计算简单、易于工程实现，而且计算 量非常小，规划速度快。
[0044] 3、以UUV的外形尺寸为安全半径对障碍物进行了膨胀处理，避免了UUV沿规划航路 航行时与障碍物的碰撞。
【附图说明】
[0045] 图la规划环境模型中的圆形障碍物示意图，图lb规划环境模型中的矩形障碍物示 意图；
[0046] 图2障碍物膨胀处理的安全半径示意图；
[0047]图3a圆形障碍物的膨胀示意图，图3b矩形障碍物的膨胀示意图；
[0048] 图4规划当前点不在圆形障碍物上时对圆形障碍物的绕行示意图；
[0049] 图5规划当前点在圆形障碍物上时对圆形障碍物的绕行示意图；
[0050] 图6圆形障碍物的绕行流程图；
[0051 ]图7规划当前点不在矩形障碍物上时对矩形障碍物的绕行示意图；
[0052]图8规划当前点在矩形障碍物上时对矩形障碍物的绕行示意图；
[0053]图9矩形障碍物的绕行流程图；
[0054] 图10判断两点连成线段与圆形障碍物是否相交的流程图；
[0055] 图11判断两点连成线段与矩形障碍物是否相交的流程图；
[0056] 图12对绕行点集合S进行消减的示意图；
[0057] 图13对绕行点集合S进行消减的流程图；
[0058]图14基于几何绕行原理的UUV二维航路规划流程图；
[0059] 图15利用本发明进行UUV二维航路规划的效果图。
【具体实施方式】
[0060] 下面结合附图举例对本发明做更详细的描述。
[0061 ]结合图la-图lb介绍UUV二维航路规划的环境模型。
[0062] 本发明中航路规划的环境模型采用的是二维几何空间模型。设规划的航路起点为 〇b，航路终点为0e，Ob和0 e分别用二维坐标表示为：
[0063] Ob = (Xob , yob ) ； 0e = ( Xoe , y〇e ) (1)
[0064] 另设航路规划过程中每一步用到的规划当前点为0C，用二维坐标表示为：
[0065] 0〇= (x〇c,y〇c) (2)
[0066] 设二维几何空间中存在一定数量的障碍物，并且用圆形和矩形来表示障碍物(如 图la-图lb所示）。
[0067]圆形障碍物为Zcirc，其参数化表示为：
[0068] Zcirc-(Xcirc，ycirc，r) (3)
[0069] 式中，（^^。，7&。)表示圆心的二维坐标4表示圆形障碍物的半径
[0070] 矩形障碍物为Zrect，其参数化表示为：
[0071 ] Zrect - ( Xrect，yrect，street，Xld，yId，Xlu，yiu，Xrd，yrd，Xru，yru ) ( 4 )
[0072] 式中，（Xrect，yrect)表示矩形中心的二维坐标，ilWt表示矩形以X轴为参考的旋转角 度，（11〇1，71(〇、（1111，7111)、（11^，71^)、^11，71^)分别表示矩形左下、左上、右下和右下顶点的二 维坐标。
[0073]结合图2和图3a、图lb介绍障碍物膨胀模型的建立方法。
[0074]进行航路规划时，一般是把UUV当作质点来考虑的，因此规划的航路可能会距障碍 物较近。但是实际上，UUV是有几何尺寸的实体，当规划的航路距障碍物较近时，很有可能导 致UUV与障碍物发生碰撞。为此在进行航路规划时，设置一个安全半径来防止UUV沿规划航 路航行时与障碍物发生碰撞。本发明采用的方法是以UUV的外形尺寸的外接圆半为安 全半径(见图2所示），然后在正常障碍物的几何形状的基础上，按照其形状边缘以半径r uuv 向外扩展出一个安全半径区域。图3a-图3b给出了圆形和矩形障碍物向外扩展安全半径后 的膨胀示意图。
[0075] 膨胀后圆形障碍物的参数化表示为：
[0076] Z circ -(Xcirc，ycirc，r ) (5)
[0077] 式中：（1^。，5^。)仍然表示圆心的二维坐标;而卜=0^^表示膨胀后的圆形障碍 物半径。
[0078] 膨胀后矩形障碍物的参数化表示为：
[0079] Z7 rect_ ( Xrect, Yrect, ^rect, X ld,y Id , X lu , y lu , X rd , y rd , X ru , y ru) (6)
[0080] 式中：（Xrect，yrect)仍然表示矩形中心的二维坐标，Let仍然表示矩形以X轴为参考 的旋转角度;而(^^、。、（^^、^^^、^(^^、彡分别表示膨胀后各顶点 的二维坐标，并且有：
[0085] 式中：klrd、blrd分别表示矩形左下顶点和右下顶点所连直线的斜率和截距，k ldu、 bldu分别表示矩形左上顶点和左下顶点所连直线的斜率和截距，klru、blru分别表示矩形左上 顶点和右上顶点所连直线的斜率和截距，krdu、brdu分别表示矩形右上顶点和右下顶点所连 直线的斜率和截距，并且有：
[0087] 结合图4、图5、图6介绍UUV对圆形障碍物的绕行方法。
[0088] 对圆形障碍物的绕行分为规划当前点在圆形障碍物上和不在圆形障碍物上两种 情况，图4给出了规划当前点不在圆形障碍物上的绕行示意图，图5给出了规划当前点在圆 形障碍物上的绕行示意图。从图4和图5可以看出，对圆形障碍物的绕行，采用的是利用多条 圆的切线的交点作为绕行点绕行圆形障碍物的方法，并利用几何原理求解各绕行点。
[0089]图6给出了圆形障碍物的绕行流程。
[0090] 步骤一：判断规划当前点0。是否在障碍物上，如果在障碍物上，令(h = 0。，转步骤 三;否则转步骤二；
[0091] 步骤二:求解规划当前点0。和圆心0的连线与圆的交点〇1=&〇1，7〇 1)\〇1的位置坐 标按式(11)计算，有两个解，选取与当前点0。距离近的解作为点〇:的坐标。
(11)
[0093] 式中：kc表示规划当前点0。和圆心0所连直线的斜率，并且有
[0094] 步骤三:求解航路终点0e和圆心0的连线与圆的交点〇2 = (102,702)7,〇2的位置坐标 按式（12)计算，有两个解，选取与航路终点0e距离近的解作为点0 2的坐标。
(12)
[0096] 式中：ke表示航路终点0e和圆心0所连直线的斜率，并且有
[0097] 步骤四：求解ZOiOO〗的角平分线Lo与圆的交点03 = (1〇3,7〇3) |，03的位置坐标按式 (13)计算，有两个解，选取与点(h(或点02)距离近的解作为点0 3的坐标。 (13)
[0099] 式中： < 表示Z〇i〇〇2的角平分线L〇的斜率，并且有
[0100] 步骤五:分别求解过点的圆的切线1^山山，其切线方程分别按式（14)、式 (15)和式（16)计算。
(14) (15) (16)
[0104]式中：kL1、bu分别表示圆的切线。的斜率和截距;kL2、bL2分别表示圆的切线L 2的斜 率和截距;kL3、bL3分别表示圆的切线L3的斜率和截距。
[0105] 步骤六:计算切线Ll和L3的交点〇4=(XQ4，y〇4)，〇4的位置坐标按式（17)计算
(17)
[01 07] 步骤七:计算切线L2和L3的交点〇5 = ( XQ5，y〇5 )，〇5的位置坐标按式（18)计算
(18)
[0109] 步骤八:将点01、04、05、02即为圆形障碍物的绕行点放入绕行点集合5中，并更新当 前点0。为〇2,绕行结束。
[0110] 结合图7、图8、图9,介绍UUV对矩形障碍物的绕行方法。
[0111] 对矩形障碍物进行绕行时，将矩形障碍物的四个顶点作为候选绕行点。根据规划 当前点0。是否在矩形障碍物上分为两种绕行情况。
[0112] 如图7所示，给出了规划当前点0。不在矩形障碍物上的绕行示意。其绕行过程可以 简述为:首先找到矩形障碍物的四个顶点中距离当前点0。最近的顶点作为UUV的下一个绕 行点，同时更新该顶点为0。。然后令0。以及与0。相邻的两个顶点中距离目标点〇 e最近的点为 下一个绕行点，并更新该点为0。。直到矩形障碍物四个顶点中只有规划当前点0。到目标点a 的距离最近，绕行结束。
[0113] 如图8所示，给出了规划当前点0。已经在矩形障碍物上的绕行示意。只需直接令0。 以及与相邻的两个顶点中距离目标点〇e更近的点为下一个绕行点，并更新该点为〇。。直到 矩形障碍物四个顶点中只有规划当前点〇。到目标点〇e的距离最近，绕行结束。
[0114] 综合以上两种情况，图9给出了 UUV对矩形障碍物的绕行流程：
[0115] 步骤一:首先判断规划当前点0。是否在矩形障碍物上，如果不在转步骤二;如果在 转步骤四；
[0116] 步骤二:找到距规划当前点0。最近的顶点V;
[0117]步骤三:V作为一个绕行点，放入绕行点集合S中，并令规划当前点0。为点V;
[0118] 步骤四：找到与规划当前点0。相邻的顶点V^Vs;
[0119] 步骤五:找到点0C、V^V2三点中距终点0e最近的点，并将其定义为点0 n;
[01 20]步骤六：如果On就是规划当前点0。，On作为绕行点On放入绕行点集合S中，转步骤 八;如果On不是规划当前点0。，转步骤七；
[0121] 步骤七:令规划当前点0。为点0n，转步骤四；
[0122] 步骤八:绕行结束。
[0123] 结合图10、图11介绍判断两点是否可视的方法。
[0124] 两点可视是指两点不被任何障碍物所阻挡。判断两点是否可视的方法就是判断两 点连线所形成的线段是否与所有的障碍物相交，如果不与任何障碍物相交则表明两点可 视。由于本发明只考虑圆形和矩形障碍物，下面介绍两点连成的线段与圆形和矩形障碍物 是否相交的判断方法。
[0125] 设任意两点为和P2 = (Xp2，yp2)，圆形障碍物和矩形障碍物的参数化 表示如式(3)和式(4)所示。那么APjPP 2连成的线段与圆形障碍物是否相交的判断过程如 图10所示：
[0126] 步骤一:求解点Pi和点内两点连线和圆联立的二次方程的根的判别式A，求解方法 如式(19)所示：
[0127] A = (2kpbp -2xmc -2y.irckpY -4(1 + kp)(x；lir +b^p+ ydrc -2y：l;tJ)p -V) (19)
[0128] 式中，匕和匕分别表示APi和点P2所连成直线的斜率和截距，并且有 bp = ypi_kpXpi ；
[0129] 步骤二:判断根的判别式A是否大于等于〇,如果大于等于〇转步骤三，否则转步骤 、- /、
[0130] 步骤三:求解点Pi和点P2两点连线和圆的两个交点的横坐标xpcl和x pc2,求解方法如 式(20)所示：
(20)
[0132] 步骤四：判断xpc；i的值在X。。和X%之间或xpc；2值在X。。和X%之间是否满足，如果满足转 步骤五，否则转步骤六；
[0133] 步骤五：点丹和点P2相交，判断结束；
[0134] 步骤六：APi和点P2不相交，判断结束。
[0135] 点丹和内连成的线段与矩形障碍物是否相交的判断过程如图11所示，具体为：
[0136] 步骤一:求解两点连线与矩形障碍物四条边所在直线的交点的横坐标Xplrd、 Xpldu、Xplru、Xprdu， 求解方法如式(21)所示：
[0138] 式中：Xplrd表示0C和06连线与矩形障碍物左下和右下顶点连线的交点的横坐标； xpldu表示0。和〇e连线与矩形障碍物左下和左上顶点连线的交点的横坐标;XplrU表示0。和〇e连 线与矩形障碍物左上和右上顶点连线的交点的横坐标; Xprdu表示0。和0e连线与矩形障碍物 右上和右下顶点连线的交点的横坐标。1^(；1、1311^、1^1( ：111、131(：111、1^11、131^1上( ：111、1^(：111、1^和131)的含义 和计算公式同前。
[0139] 步骤二：如果求解的Xplrd存在，且满足Xplrd的值在XP1和XP2之间，且在Xld和Xrd之间， 转步骤六，否则转步骤三；
[0140] 步骤三:如果求解的Xpldu存在，且满足Xpldu的值在XpjPxp2之间，且在Xld和Xlu之间， 转步骤六，否则转步骤四；
[0141] 步骤四：如果求解的Xplru存在，且满足Xplru的值在X P1和XP2之间，且在Xl4Pxru之间， 转步骤六，否则转步骤五；
[0142] 步骤五：如果求解的Xprdu存在，且满足Xprdu的值在X P1和XP2之间，且在Xrd和Xru之间， 转步骤六，否则转步骤七；
[0143] 步骤六：APi和点P2相交，判断结束；
[0144] 步骤七:点?:和点P2不相交，判断结束。
[0145] 结合图12和图13，介绍UUV对绕行点集合S进行消减的方法：
[0146] 绕行点集合S中的所有绕行点，并不是最终的绕行航路点，因为集合S中的有些绕 行点是非必须绕行点，可以消减掉，从而得到最简绕行点集合I，而歹才是最终的绕行航路 点。对绕行点集合进行消减的关键就是判断绕行点之间是否可视，只要将所有可视绕行点 之间的绕行点消减掉，就得到了最简绕行点集合。图12给出了对绕行点进行消减的示意图。 如图1 2所示，从起点Ob到终点0e的未消减绕行点集合为S = { Ob，Pi，P2，P3，P4，P5，P 6，〇e }，而消 减后的最简绕行点集合为孓=KL巧，巧,cth图13给出了对绕行点集合进行消减的过程：
[0147] 步骤一:定义三个计数变量!1、12、13，并都初始化为0，获取绕行点集合3中绕行点 的个数N;
[0148] 步骤二:令T1为绕行点集合S中绕行点的个数N;
[0149] 步骤三:令 T2 = T1_1;
[0150] 步骤四：判断绕行点S(T1)和S(T2)是否可视，如果可视令T3 = T2,转步骤五，如果 不可视转步骤五；
[0151] 其中，S(T1)GS，S(T1)表示第II个绕行点;S(T2)GS，S(T2)表示第T2个绕行点；
[0152] 步骤五:令T2 = T2_1，判断T2是否为0，如果为0转步骤六，否则转步骤四
[0153] 步骤六:消去绕行点集合S中点S (T1)和S (T3)之间的所有绕行点；
[0154] 步骤七:令T1=T3,判断T1是否为1，如果为1，对绕行点集合S的消减结束；否则转 步骤三。
[0155]结合图14介绍基于几何绕行原理的UUV二维航路规划的整个流程。
[0156]步骤一:从使命文本读取航路起点Ob、航路终点0e和各障碍物的参数；
[0157]步骤二:建立障碍物膨胀模型，计算膨胀后的各障碍物的参数，建立绕行点集合S; [0158]步骤三:令规划当前点0。为起点0b，并放入绕行点集合S中；
[0159]步骤四：判断规划当前点0。是不是航路终点0e，如果是转步骤十二，否则转步骤五； [0160]步骤五:判断规划当前点0。和航路终点0e是否可视，如果可视转步骤十二，否则转 步骤六；
[0161]步骤六:搜索距规划当前点0。最近的障碍物，对其进行绕行;搜索方法为求解各障 碍物中心（圆心或矩形中心）与当前点0。的距离，距离最小的即为距规划当前点0。最近的障 碍物；
[0162] 步骤七:判断需绕行障碍物的形状，如果是圆形障碍物，转步骤八，如果是矩形障 碍物转步骤九；
[0163] 步骤八:进行圆形障碍物绕行，绕行结束后转步骤十；
[0164] 步骤九:进行矩形障碍物绕行，绕行结束后转步骤十；
[0165] 步骤十:将绕行点放入绕行点集合S中；
[0166] 步骤十一:更新规划当前点0。，转步骤四；
[0167] 步骤十二:将航路终点0e放入绕行点集合S中；
[0168] 步骤十三:对绕行点集合S进行消减，规划结束。
[0169] 图15给出了利用本发明进行UUV航路规划的一个实现案例。
[0170] 本案例中，共设置了5个矩形障碍物和5个圆形障碍物，航路的起点0b和航路的终 点〇e已在图中标出。首先，在规划时对每个障碍物进行了膨胀处理，各障碍物的膨胀边界已 在图中用点划线标出。然后，对部分障碍物进行了绕行，得到了原始的绕行点集合S={0 b， Pi，P2，P3，…，Pl4，0e }，并在图中用虚线表示出了未消减的绕行航路。最后对绕行点集合进行 消减，得到消减后的绕行点集合$ = 乂3,..荇，尸8:，.巧，.朽2,:，巧3，化}，并用实线表示出了 消减后的最终绕行航路。
【主权项】
1. 一种基于几何绕行原理的UUV二维航路规划方法，其特征是： 步骤一:从使命文本读取航路起点Ob、航路终点Oe和各障碍物的参数； 步骤二:分别对各障碍物进行膨胀处理，计算膨胀后的各障碍物的参数； 步骤三:建立绕行点集合S，令规划当前点0。为起点Ob，并放入绕行点集合S中； 步骤四：如果规划当前点是航路终点Oe、或者规划当前点和航路终点Oe可视，转步骤 六，否则执行步骤五； 步骤五:搜索距规划当前点0。最近的障碍物，根据障碍物的形状对障碍物进行绕行，得 到绕行点并放入绕行点集合S中，更新规划当前点0。，转步骤四； 步骤六:将航路终点仏放入绕行点集合S中； 步骤七:对绕行点集合s进行消减，得到消减后的绕行点集合5，规划结束。2. 根据权利要求1所述的基于几何绕行原理的UUV二维航路规划方法，其特征是所述根 据障碍物的形状对障碍物进行绕行具体包括： (1) 对圆形障碍物的绕行的步骤为： (1.1) 判断规划当前点0。是否在圆形障碍物上，如果在，令点0: = 0^否则，求解规划当 前点0。和圆心0的连线与圆周的交点，并令其为点〇1; (1.2 )求解航路终点0e和圆心0的连线与圆周的交点，并令其为点〇2 ; (1.3) 求解20:002的角平分线与圆周的交点，并令其为点〇3; (1.4) 求解过点&的圆的切线与过点03的圆的切线的交点，并令其为点04; (1.5) 计算过点02的圆的切线与过点03的圆的切线的交点，并令其为点05; (1.6) 将点(h、点〇4、点05、点02作为圆形障碍物的绕行点放入绕行点集合S中，并更新规 划当前点0。为点〇2，绕行结束； (2) 对矩形障碍物绕行的步骤为： (2.1) 判断规划当前点0。是否在矩形障碍物上，如果不在，找到距0。最近的顶点V，将顶 点V作为绕行点放入绕行点集合S中，并更新规划当前点0。为顶点V; (2.2) 找到与点0。相邻的顶点V!、顶点V2,如果点0。、顶点V!和顶点V2三点中距终点0 e最近 的点是点0。，转步骤(2.4);否则执行步骤(2.3); (2.3) 令规划当前点0。为顶点Vi和顶点V2*距终点0e近的点，将点0。作为绕行点放入绕 行点集合S中，转步骤(2.2); (2.4) 点0。作为绕行点放入绕行点集合S中，绕行结束。3. 根据权利要求1或2所述的基于几何绕行原理的UUV二维航路规划方法，其特征是:所 述对障碍物进行膨胀处理的方法是在正常障碍物几何形状的基础上，按照其形状边缘以安 全半径r_向外扩展出一个安全半径区域。4. 根据权利要求1或2所述的基于几何绕行原理的UUV二维航路规划方法，其特征是:所 述对绕行点集合S进行消减的方法是将所有可视绕行点之间的绕行点去掉，具体过程为： a:定义三个计数变量ΤΙ、T2、T3，并都初始化为0，获取绕行点集合S中绕行点的个数N; b:令Τ1为绕行点集合S中绕行点的个数Ν; c:令 T2 = T1-1; d:判断绕行点S(T1)和S(T2)是否可视，如果可视令Τ3 = Τ2,转步骤e，如果不可视转步 骤e; 其中，S(Tl)eS，S(Tl)表示第T1个绕行点；S(T2)eS，S(T2)表示第T2个绕行点； e:令Τ2 = Τ2-1，判断Τ2是否为0，如果为0转步骤f，否则转步骤d; f:消去绕行点集合S中点S (T1)和S (T3)之间的所有绕行点； g:令ΤΙ = T3，判断T1是否为1，如果为1，对绕行点集合S的消减结束;否则转步骤c。5.根据权利要求3所述的基于几何绕行原理的UUV二维航路规划方法，其特征是:所述 对绕行点集合S进行消减的方法是将所有可视绕行点之间的绕行点去掉，具体过程为： a:定义三个计数变量ΤΙ、Τ2、Τ3，并都初始化为0，获取绕行点集合S中绕行点的个数Ν; b:令Τ1为绕行点集合S中绕行点的个数Ν; c:令 T2 = T1-1; d:判断绕行点S(T1)和S(T2)是否可视，如果可视令Τ3 = Τ2,转步骤e，如果不可视转步 骤e; 其中，S(Tl)eS，S(Tl)表示第T1个绕行点；S(T2)eS，S(T2)表示第T2个绕行点； e:令T2 = T2-1，判断T2是否为0，如果为0转步骤六，否则转步骤d; f:消去绕行点集合S中点S (Τ1)和S (T3)之间的所有绕行点； g:令ΤΙ = T3，判断T1是否为1，如果为1，对绕行点集合S的消减结束;否则转步骤c。
【文档编号】G05D1/02GK105929824SQ201610312431
【公开日】2016年9月7日
【申请日】2016年5月12日
【发明人】陈涛, 徐达, 王玉柱, 周佳加, 徐健, 张宏瀚
【申请人】哈尔滨工程大学