一种uuv对矩形障碍物几何绕行的二维航路规划方法
【专利摘要】本发明提供的是一种UUV对矩形障碍物几何绕行的二维航路规划方法。一：从使命文本读取航路起点Ob、航路终点Oe和各矩形障碍物的参数；二：对矩形障碍物进行膨胀处理，计算膨胀后的各矩形障碍物的参数；三：建立绕行点集合S，令规划当前点Oc为起点Ob，并放入绕行点集合S中；四：如果规划当前点Oc是航路终点Oe，或者规划当前点Oc和航路终点Oe可视，转步骤六，否则执行步骤五；五：对距规划当前点Oc最近的矩形障碍物进行几何绕行，得到绕行点并放入绕行点集合S中，更新规划当前点Oc，转步骤四；六：将航路终点Oe放入绕行点集合S中，规划结束。本发明通过简单的几何原理实现对矩形障碍物的绕行，可以使UUV在复杂多矩形障碍环境中快速、高效的获得一条安全无碰的二维航路。
【专利说明】
-种UUV对矩形障碍物几何绕行的二维航路规划方法
技术领域
[0001] 本发明设及的是一种UUV航路规划方法，具体的是一种UUV对矩形障碍物几何绕行 的二维航路规划方法。
【背景技术】
[0002] 航路规划是水下无人航行器(Unmanned化derwater Vehicle，UUV)的关键技术之 一，是UUV自主能力的重要体现。航路规划是指在已知障碍环境下，规划出一条从起点出发 绕过所有障碍物并到达终点的无碰路径。根据空间维度，航路规划可分为二维航路规划和 S维航路规划。而二维航路规划是S维航路规划的基础，并且在UUV的应用也更为广泛，是 UUV航路规划技术研究的热点。
[0003] 目前，UUV的航路规划方法很多，例如：申请号为201410011297.9的专利文件中公 开了一种"运动障碍环境中UUVS维航路规划方法"；"无人水下自主航行器(AUV)避碰研究" (内蒙古大学硕±论文，2008年）；"无人水下航行器近海底空间路径规划方法"（《哈尔滨工 程大学学报》2014年3月）等。但是如何在复杂的障碍环境下既快速又可行的获得一条无碰 路径，特别是规划方法能够适于工程应用，仍然是一个难点。
[0004] 与本发明较接近的【背景技术】是"基于几何算法的水下航行器路径规划"（《海军工 程大学学报》2009,21(6) :41-44页），其中提到了将几何算法应用到水下航行器路的径规划 中，但是该文献考虑的是圆形障碍物，并且其对障碍物的绕行方法与本发明不同。
【发明内容】
[0005] 本发明的目的在于提供一种计算简单、规划效率高、规划速度快的UUV对矩形障碍 物几何绕行的二维航路规划方法。
[0006] 本发明的目的是运样实现的：
[0007] 步骤一:从使命文本读取航路起点Ob、航路终点Oe和各矩形障碍物的参数；
[000引步骤二:对矩形障碍物进行膨胀处理，计算膨胀后的各矩形障碍物的参数；
[0009] 步骤S :建立绕行点集合S，令规划当前点Oc为起点Ob,并放入绕行点集合S中；
[0010] 步骤四：如果规划当前点0。是航路终点〇6,或者规划当前点0。和航路终点Oe可视， 转步骤六，否则执行步骤五；
[0011] 步骤五:对距规划当前点Oc最近的矩形障碍物进行几何绕行，得到绕行点并放入 绕行点集合S中，更新规划当前点0。，转步骤四；
[001 ^ 步骤六:将航路终点Oe放入绕行点集合S中，规划结束。
[001引本发明还可W包括；
[0014] 1、对矩形障碍物进行几何绕行的方法为：
[001引（1)、首先判断规划当前点0。是否在矩形障碍物上，如果不在转步骤(2)、如果在转 步骤(4);
[0016] (2)、找到距规划当前点Oc最近的顶点V;
[0017] (3)、V作为一个绕行点，放入绕行点集合S中，并令规划当前点0。为点V;
[001引（4)、找到与规划当前点Oc相邻的顶点Vi、V2;
[0019] (5 )、找到规划当前点Oc、顶点Vi和顶点VsS点中距终点Oe最近的点，并将其定义为 点On;
[0020] (6 )、如果点On就是规划当前点Oc，点On作为绕行点放入绕行点集合S中，转步骤 (8);如果点On不是规划当前点Oc,执行步骤(7);
[0021] (7)、令规划当前点Oc为点On，转步骤(4);
[0022] (8)、绕行结束。
[0023] 2、所述对矩形障碍物进行膨胀处理的方法为：
[0024] 在正常矩形障碍物几何形状的基础上，按照形状边缘W安全半径ruuv向外扩展出 一个安全半径区域，
[0025] 膨胀前的矩形障碍物为：
[0026] Zrect =( Xrect, Yrect, llWt, Xld , y Id , Xlu , yiu , Xrd , Yrd , Xru , Yru )。
[0027] 其中（Xrect，yrect)表示矩形中屯、的二维坐标，如ec康示矩形Wx轴为参考的旋转角 度，佔(1，71(1)、佔。，71。）、（虹(1，7封）、（虹11，7^1)分别表示矩形左下、左上、右下和右下顶点的二 维坐标；
[00%]膨胀后矩形障碍物为：
[0029] Z' rect = ( Xrect, yrect, 4rect, X' Id , y' Id , X' Iu , y' Iu , X' rd , y' rd , X' ru , y' ru )
[0030] 其中 ^'1<1，7'13)、^'1。，7'1。）、^'^，7'封)和^',。，7',。)分别表示膨胀后矩形各顶 点的二维坐标，并且有：
[0031]
[0032]
[0033]
[0034]
[0035] 其中：klrd、blrd分别表示矩形左下顶点和右下顶点所连直线的斜率和截距，kldu、 bl化分别表示矩形左上顶点和左下顶点所连直线的斜率和截距，klru、blru分别表示矩形左上 顶点和右上顶点所连直线的斜率和截距，krdu、br化分别表示矩形右上顶点和右下顶点所连 直线的斜率和截距，并且有：
[0037] 本发明利用几何原理进行UUV的航路规划，在环境模型上采用了简单的几何模型， 在计算无碰路径时采用简单的几何原理对障碍物进行绕行，避免了其他规划方法需要建立 地图、循环捜索无碰路径所引起的信息量大、计算复杂的问题，不仅规划效率高、规划速度 快，而且原理简单、计算量小，易于工程实现。
[0038] 本发明的有益效果在于：
[0039] 1、环境模型采用的是几何空间模型，相比于传统的栅格、地图模型，所需规划信息 量少，规划效率高，特别适合复杂多障碍物的环境。
[0040] 2、对矩形障碍物的绕行算法只应用到了几何原理，计算简单、易于工程实现，而且 计算量非常小，规划速度快。
[0041] 3、WUUV的外形尺寸为安全半径对障碍物进行了膨胀处理，避免了UUV沿规划航路 航行时与矩形障碍物的碰撞。
【附图说明】
[0042] 图1规划环境模型中的矩形障碍物示意图；
[0043] 图2矩形障碍物膨胀处理的安全半径示意图；
[0044] 图3矩形障碍物的膨胀示意图；
[0045] 图4规划当前点不在矩形障碍物上时对矩形障碍物的绕行示意图；
[0046] 图5规划当前点在矩形障碍物上时对矩形障碍物的绕行示意图；
[0047] 图6 UUV对矩形障碍物的几何绕行的流程图；
[0048] 图7判断两点连成线段与矩形障碍物是否相交的流程图；
[0049] 图8 UUV对矩形障碍物几何绕行的二维航路规划流程图；
[0050] 图9利用本发明进行UUV对矩形障碍物的几何绕行的效果图。
【具体实施方式】
[0051] 下面结合附图举例对本发明作进一步描述。
[0052] 结合图1介绍本发明的UUV二维航路规划的环境模型。
[0053] 本发明中航路规划的环境模型采用的是二维几何空间模型。设规划的航路起点为 Ob，航路终点为Oe，化和Oe分别用二维坐标表示为：
[0054] Ob=Uob,y〇b);0e=(x〇e，y〇e) (1)
[0055] 另设航路规划过程中每一步用到的规划当前点为0。，用二维坐标表示为：
[0056] 0c=(x〇c,y〇c) (2)
[0057] 设二维几何空间中存在一定数量的矩形障碍物，如图1所示，设矩形障碍物为 Zrect,其参数化表示为：
[005引 Zrect =( Xrect, Yrect, ^rect, Xld , y Id , Xlu , yiu , Xrd , Yrd , Xru , Yru ) (3)
[0059] 式中，（虹ect，yrect)表示矩形中屯、的二维坐标，如ec康示矩形Wx轴为参考的旋转角 度，佔(1，71(1)、佔。，71。）、（虹(1，7封）、（虹11，7^1)分别表示矩形左下、左上、右下和右下顶点的二 维坐标。
[0060]结合图2和图3介绍矩形障碍物膨胀模型的建立方法。
[0061 ]进行航路规划时，一般是把UUV当作质点来考虑的，因此规划的航路可能会距障碍 物较近。但是实际上，UUV是有几何尺寸的实体，当规划的航路距障碍物较近时，很有可能导 致UUV与障碍物发生碰撞。为此在进行航路规划时，设置一个安全半径来防止UUV沿规划航 路航行时与障碍物发生碰撞。本发明采用的方法是WUUV的外形尺寸的外接圆半径ruuv为安 全半径(见图2所示），然后在正常矩形障碍物的几何形状的基础上，按照其形状边缘W半径 ruuv向外扩展出一个安全半径区域。图3给出了矩形障碍物向外扩展安全半径后的膨胀示意 图。
[0062] 膨胀后矩形障碍物的参数化表示为：
[0063] Z' rect = (Xrect, yrect,4rect, X' Id , y' Id , X' Iu , y' Iu , X' rd , y' rd , X' ru , y' ru) (4)
[0064] 式中：（虹ect，yrect)仍然表示矩形中屯、的二维坐标，如ect仍然表示矩形Wx轴为参考 的旋转角度;而^/1<1，/13)、^/1。，/1。）、^/,<1，/^)和^/,。，/,。)分别表示膨胀后各顶点 的二维坐标，并且有：
[00 化]
[0066]
[0067]
[006引
[0069] 式中：klrd、blrd分别表示矩形左下顶点和右下顶点所连直线的斜率和截距，kldu、 bl化分别表示矩形左上顶点和左下顶点所连直线的斜率和截距，klru、blru分别表示矩形左上 顶点和右上顶点所连直线的斜率和截距，krdu、br化分别表示矩形右上顶点和右下顶点所连 直线的斜率和截距，并且有：
?bidu = yid-kiduxid，
[0071 ]结合图4、图5、图6，介绍UUV对矩形障碍物的绕行方法。
[0072] 对矩形障碍物进行绕行时，将矩形障碍物的四个顶点作为候选绕行点。根据规划 当前点0。是否在矩形障碍物上分为两种绕行情况。
[0073] 如图4所示，给出了规划当前点0。不在矩形障碍物上的绕行示意。其绕行过程可W 简述为:首先找到矩形障碍物的四个顶点中距离当前点Oc最近的顶点作为UUV的下一个绕 行点，同时更新该顶点为Oc。然后令OcW及与Oc相邻的两个顶点中距离目标点Oe最近的点为 下一个绕行点，并更新该点为Oc。直到矩形障碍物四个顶点中只有规划当前点Oc到目标点Oe 的距离最近，绕行结束。
[0074] 如图5所示，给出了规划当前点Oc已经在矩形障碍物上的绕行示意。只需直接令Oc W及与Oc相邻的两个顶点中距离目标点Oe更近的点为下一个绕行点，并更新该点为Oc。直到 矩形障碍物四个顶点中只有规划当前点Oc到目标点Oe的距离最近，绕行结束。
[007引综合W上两种情况，图6给出了 UUV对矩形障碍物的绕行流程：
[0076] 步骤一:首先判断规划当前点0。是否在矩形障碍物上，如果不在则执行步骤二;如 果在则转步骤四；
[0077] 步骤二:找到距规划当前点Oc最近的顶点V;
[0078] 步骤=:V作为一个绕行点，放入绕行点集合S中，并令规划当前点Oc为点V;
[0079] 步骤四：找到与规划当前点Oc相邻的顶点Vi、V2;
[0080] 步骤五:找到〇c、Vi和V2S点中距终点Oe最近的点，并将其定义为点On;
[0081] 步骤六:如果On就是规划当前点0c，0n作为绕行点将On放入绕行点集合S中，转步骤 八;如果On不是规划当前点Oc，则执行步骤屯；
[0082] 步骤屯:令规划当前点Oc为点On,转步骤四。
[0083] 步骤八:绕行结束。
[0084] 结合图8介绍UUV对矩形障碍物几何绕行的二维航路规划的整个流程。
[0085] 步骤一:从使命文本读取航路起点化、航路终点Oe和各矩形障碍物的参数；
[0086] 步骤二:建立矩形障碍物膨胀模型，计算膨胀后的各矩形障碍物的参数，建立绕行 点集合S;
[0087] 步骤令规划当前点Oc为起点化，并放入绕行点集合S中；
[0088] 步骤四：判断规划当前点Oc是不是航路终点Oe,如果是转步骤十，否则执行步骤五；
[0089] 步骤五:判断规划当前点Oc和航路终点Oe是否可视，如果可视转步骤十，否则执行 步骤六；
[0090] 步骤六:捜索距规划当前点Oc最近的矩形障碍物;捜索方法为首先找到阻碍当前 点Oc和航路终点Oe连线的所有矩形障碍物，然后求解W上各矩形障碍物中屯、与当前点Oc的 距离，距离最小的即为距规划当前点Oc最近的矩形障碍物；
[0091] 步骤屯:对距规划当前点Oc最近的矩形障碍物进行几何绕行；
[0092] 步骤八:将绕行点放入绕行点集合S中；
[0093] 步骤九:更新规划当前点0。，转步骤四；
[0094] 步骤十:将航路终点Oe放入绕行点集合S中，规划结束。
[00M]所述判断规划当前点Oc和航路终点Oe是否可视的方法为：
[0096]点Oc和点Oe可视是指两点不被任何矩形障碍物所阻挡。判断两点是否可视的方法 就是判断两点连线所形成的线段是否与所有的矩形障碍物相交，如果不与任何矩形障碍物 相交则表明两点可视。判断规划当前点Oc和航路终点Oe是否可视的流程如图7所示：
[0097] 步骤一:选择第一个矩形障碍物
[009引步骤二:求解0c、0e两点连线与矩形障碍物四条边所在直线的交点的横坐标Xplrd、 Xpldu、Xplru、Xprdu，求解方法如式（9 )所示：
[0099]
<貧)
[0100] 式中=Xplrd表示0。和Oe连线与矩形障碍物左下和右下顶点连线的交点的横坐标； Xpldu表示Oc和Oe连线与矩形障碍物左下和左上顶点连线的交点的横坐标;Xplru表示Oc和Oe连 线与矩形障碍物左上和右上顶点连线的交点的横坐标;Xprdu表示Oc和Oe连线与矩形障碍物 右上和右下顶点连线的交点的横坐标。41,(1、131,(1、41化、131化、41^1、13扣心化和13油的含义和计算 公式同前。kce和bee分别表示Oc和Oe所连成直线的斜率和截距，并且菊
，bce = y〇c~kceXoc ；
[0101 ] 步骤二：如果求解的Xplrd存在，且满足Xplrd的值在Xoc和Xoe之间，且在Xld和Xrd之间， 转步骤八，否则转步骤四；
[010^ 步骤四：如果求解的Xpldu存在，且满足Xpldu的值在Xoc和Xoe之间，且在Xld和Xlu之间， 转步骤八，否则转步骤五；
[010；3] 步骤五：如果求解的Xplru存在，且满足Xplru的值在Xoc和Xoe之间，且在Xlu和Xru之间， 转步骤八，否则转步骤六；
[0104] 步骤六：如果求解的Xprdu存在，且满足Xprdu的值在Xoc和Xoe之间，且在Xrd和Xru之间， 转步骤八，否则转步骤屯；
[0105] 步骤屯:判断是否还有矩形障碍物，如果有，选择下一个矩形障碍物，转步骤二，否 则转步骤九；
[0106] 步骤八：当前点Oc和航路终点Oe不可视，判断结束；
[0107] 步骤九：当前点Oc和航路终点Oe可视，判断结束。
[0108] 图9给出了利用本发明进行UUV对矩形障碍物几何绕行二维航路规划的一个实现 实例。
[0109] 本实例中，共设置了 10个矩形障碍物，航路的起点Ob和航路的终点Oe已在图中标 出。首先，在规划时对每个障碍物进行了膨胀处理，各障碍物的膨胀边界已在图中用点划线 标出。然后，UUV对部分矩形障碍物进行了绕行，得到了绕行点集合S= {Ob，Pi，P2，P3,…，Pii, OeK并在图中用虚线表示出了由绕行点组成的UUV绕行航路。
【主权项】
1. 一种UUV对矩形障碍物几何绕行的二维航路规划方法，其特征是： 步骤一:从使命文本读取航路起点Ob、航路终点Oe和各矩形障碍物的参数； 步骤二:对矩形障碍物进行膨胀处理，计算膨胀后的各矩形障碍物的参数； 步骤三:建立绕行点集合S，令规划当前点Oc为起点Ob，并放入绕行点集合S中； 步骤四：如果规划当前点Oc是航路终点Oe，或者规划当前点Oc和航路终点Oe可视，转步骤 六，否则执行步骤五； 步骤五:对距规划当前点Oc最近的矩形障碍物进行几何绕行，得到绕行点并放入绕行点 集合S中，更新规划当前点0。，转步骤四； 步骤六:将航路终点仏放入绕行点集合S中，规划结束。2. 根据权利要求1所述的UUV对矩形障碍物几何绕行的二维航路规划方法，其特征是对 矩形障碍物进行几何绕行的方法为： (1) 、首先判断规划当前点0。是否在矩形障碍物上，如果不在转步骤(2)、如果在转步骤 (4)； (2) 、找到距规划当前点Oc最近的顶点V; (3 )、V作为一个绕行点，放入绕行点集合S中，并令规划当前点0。为点V; (4) 、找到与规划当前点Oc相邻的顶点V2; (5) 、找到当前点0。、顶点V1和顶点V2三点中距终点Oe最近的点，并将其定义为点O n; (6 )、如果点0"就是规划当前点Oc，点On作为绕行点放入绕行点集合S中，转步骤(8);如 果点仏不是规划当前点0。，执行步骤(7); (7) 、令规划当前点Oc为点On，转步骤(4); (8) 、绕行结束。3. 根据权利要求1或2所述的UUV对矩形障碍物几何绕行的二维航路规划方法，其特征 是所述对矩形障碍物进行膨胀处理的方法为： 在正常矩形障碍物几何形状的基础上，按照形状边缘以安全半径ruuv向外扩展出一个 安全半径区域， 膨胀前的矩形障碍物为：其中,y^t)表示矩形中心的二维坐标，IlWt表示矩形以X轴为参考的旋转角度， (xid，yid)、（xiu，yiu)、（xrd，yrd)、（x ru，yru)分别表示矩形左下、左上、右下和右下顶点的二维 坐标； 膨胀后矩形障碍物为：其中（X^yld) lu) rd)和(X^yru)分别表示膨胀后矩形各顶点的 二维坐标，并且有：其中：klrd、blrd分别表示矩形左下顶点和右下顶点所连直线的斜率和截距，k ldu、bldu分 别表示矩形左上顶点和左下顶点所连直线的斜率和截距，klru、blru分别表示矩形左上顶点 和右上顶点所连直线的斜率和截距，k rdu、brdA别表示矩形右上顶点和右下顶点所连直线 的斜率和截距，并且有：
【文档编号】G05D1/06GK106020213SQ201610312413
【公开日】2016年10月12日
【申请日】2016年5月12日
【发明人】陈涛, 徐达, 王玉柱, 严浙平, 张宏瀚, 徐健
【申请人】哈尔滨工程大学