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

摘要

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

主权项

1.一种UUV对圆形障碍物几何绕行的二维航路规划方法，其特征是：步骤一：从使命文本读取航路起点Ob、航路终点Oe和各圆形障碍物的参数；步骤二：对各圆形障碍物进行膨胀处理，计算膨胀后的各圆形障碍物的参数；步骤三：建立绕行点集合S，令规划当前点Oc为起点Ob，并放入绕行点集合S中；步骤四：如果规划当前点Oc是航路终点Oe，或者规划当前点Oc和航路终点Oe可视，转步骤六，否则执行步骤五；步骤五：对距规划当前点Oc最近的圆形障碍物进行几何绕行，得到绕行点并放入绕行点集合S中，更新规划当前点Oc，转步骤四；对圆形障碍物进行几何绕行的方法为：(1)、判断规划当前点Oc是否在圆形障碍物上，如果在，令点O1＝Oc；否则，求解规划当前点Oc和圆心O的连线与圆周的交点，并令其为点O1；(2)、求解航路终点Oe和圆心O的连线与圆周的交点，并令其为点O2；(3)、求解∠O1OO2的角平分线与圆周的交点，并令其为点O3；(4)、求解过点O1的圆的切线与过点O3的圆的切线的交点，并令其为点O4；(5)、计算过点O2的圆的切线与过点O3的圆的切线的交点，并令其为点O5；(6)、将点O1、点O4、点O5、点O2作为圆形障碍物的绕行点放入绕行点集合S中，并更新规划当前点Oc为点O2，绕行结束；步骤六：将航路终点Oe放入绕行点集合S中，规划结束。