[发明专利]一种UUV对矩形障碍物几何绕行的二维航路规划方法

摘要

本发明提供的是一种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中，规划结束。本发明通过简单的几何原理实现对矩形障碍物的绕行，可以使UUV在复杂多矩形障碍环境中快速、高效的获得一条安全无碰的二维航路。

主权项

1.一种UUV对矩形障碍物几何绕行的二维航路规划方法，其特征是：步骤一：从使命文本读取航路起点Ob、航路终点Oe和各矩形障碍物的参数；步骤二：对矩形障碍物进行膨胀处理，计算膨胀后的各矩形障碍物的参数；所述对矩形障碍物进行膨胀处理的方法为：在正常矩形障碍物几何形状的基础上，按照形状边缘以安全半径ruuv向外扩展出一个安全半径区域，膨胀前的矩形障碍物为：Zrect＝(xrect,yrect,ψrect,xld,yld,xlu,ylu,xrd,yrd,xru,yru)，其中(xrect,yrect)表示矩形中心的二维坐标，ψrect表示矩形以X轴为参考的旋转角度，(xld,yld)、(xlu,ylu)、(xrd,yrd)、(xru,yru)分别表示矩形左下、左上、右下和右下顶点的二维坐标；膨胀后矩形障碍物为：Z′rect＝(xrect,yrect,ψ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)分别表示膨胀后矩形各顶点的二维坐标，并且有：其中：klrd、blrd分别表示矩形左下顶点和右下顶点所连直线的斜率和截距，kldu、bldu分别表示矩形左上顶点和左下顶点所连直线的斜率和截距，klru、blru分别表示矩形左上顶点和右上顶点所连直线的斜率和截距，krdu、brdu分别表示矩形右上顶点和右下顶点所连直线的斜率和截距，并且有：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；步骤三：建立绕行点集合S，令规划当前点Oc为起点Ob，并放入绕行点集合S中；步骤四：如果规划当前点Oc是航路终点Oe，或者规划当前点Oc和航路终点Oe可视，转步骤六，否则执行步骤五；步骤五：对距规划当前点Oc最近的矩形障碍物进行几何绕行，得到绕行点并放入绕行点集合S中，更新规划当前点Oc，转步骤四；步骤六：将航路终点Oe放入绕行点集合S中，规划结束。