[发明专利]一种基于几何绕行原理的UUV二维航路规划方法

摘要

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

主权项

1.一种基于几何绕行原理的UUV二维航路规划方法，其特征是：步骤一：从使命文本读取航路起点Ob、航路终点Oe和各障碍物的参数；步骤二：分别对各障碍物进行膨胀处理，计算膨胀后的各障碍物的参数；步骤三：建立绕行点集合S，令规划当前点Oc为起点Ob，并放入绕行点集合S中；步骤四：如果规划当前点Oc是航路终点Oe、或者规划当前点Oc和航路终点Oe可视，转步骤六，否则执行步骤五；步骤五：搜索距规划当前点Oc最近的障碍物，根据障碍物的形状对障碍物进行绕行，得到绕行点并放入绕行点集合S中，更新规划当前点Oc，转步骤四；步骤六：将航路终点Oe放入绕行点集合S中；步骤七：对绕行点集合S进行消减，得到消减后的绕行点集合规划结束；所述根据障碍物的形状对障碍物进行绕行具体包括：(1)对圆形障碍物的绕行的步骤为：(1.1)判断规划当前点Oc是否在圆形障碍物上，如果在，令点O1＝Oc；否则，求解规划当前点Oc和圆心O的连线与圆周的交点，并令其为点O1；(1.2)求解航路终点Oe和圆心O的连线与圆周的交点，并令其为点O2；(1.3)求解∠O1OO2的角平分线与圆周的交点，并令其为点O3；(1.4)求解过点O1的圆的切线与过点O3的圆的切线的交点，并令其为点O4；(1.5)计算过点O2的圆的切线与过点O3的圆的切线的交点，并令其为点O5；(1.6)将点O1、点O4、点O5、点O2作为圆形障碍物的绕行点放入绕行点集合S中，并更新规划当前点Oc为点O2，绕行结束；(2)对矩形障碍物绕行的步骤为：(2.1)判断规划当前点Oc是否在矩形障碍物上，如果不在，找到距Oc最近的顶点V，将顶点V作为绕行点放入绕行点集合S中，并更新规划当前点Oc为顶点V；(2.2)找到与点Oc相邻的顶点V1、顶点V2，如果点Oc、顶点V1和顶点V2三点中距终点Oe最近的点是点Oc，转步骤(2.4)；否则执行步骤(2.3)；(2.3)令规划当前点Oc为顶点V1和顶点V2中距终点Oe近的点，将点Oc作为绕行点放入绕行点集合S中，转步骤(2.2)；(2.4)点Oc作为绕行点放入绕行点集合S中，绕行结束。