技术领域technical field
本发明涉及的是一种是UUV航路规划方法，具体的是一种UUV对圆形障碍物几何绕行的二维航路规划方法。The present invention relates to a UUV route planning method, in particular to a two-dimensional route planning method for the UUV to geometrically circumvent circular obstacles.
背景技术Background technique
航路规划是水下无人航行器(Unmanned Underwater Vehicle，UUV)的关键技术之一，是UUV自主能力的重要体现。航路规划是指在已知障碍环境下，规划出一条从起点出发绕过所有障碍物并到达终点的无碰路径。根据空间维度，航路规划可分为二维航路规划和三维航路规划。其中，二维航路规划是三维航路规划的基础，并且在UUV的应用也更为广泛，是UUV航路规划技术研究的热点。目前，UUV的航路规划方法很多，但是如何在复杂的障碍环境下既快速又可行的获得一条无碰路径，特别是规划方法能够适于工程应用，仍然是一个难点。Route planning is one of the key technologies of Unmanned Underwater Vehicle (UUV), and it is an important embodiment of UUV's autonomy. Route planning refers to planning a non-collision path starting from the starting point, bypassing all obstacles and reaching the destination in the known obstacle environment. According to the spatial dimension, route planning can be divided into two-dimensional route planning and three-dimensional route planning. Among them, two-dimensional route planning is the basis of three-dimensional route planning, and it is more widely used in UUV, which is a hot spot in the research of UUV route planning technology. At present, there are many UUV route planning methods, but how to obtain a collision-free path quickly and feasible in complex obstacle environments, especially the planning method that is suitable for engineering applications, is still a difficult point.
与本发明相关的已有技术为“基于几何算法的水下航行器路径规划”(《海军工程大学学报》，2009，21(6)：41-44页)，其中提到了考虑圆形障碍物时水下航行器基于几何算法的路径规划，但该文献对圆形障碍物绕行的航路规划方法与本发明不同。The existing technology related to the present invention is "underwater vehicle path planning based on geometric algorithm" ("Journal of Naval Engineering University", 2009, 21 (6): 41-44 pages), which mentions the consideration of circular obstacles The path planning of the underwater vehicle based on the geometric algorithm, but the route planning method of this document for circumventing circular obstacles is different from that of the present invention.
发明内容Contents of the invention
本发明的目的在于提供一种计算简单、规划效率高、规划速度快的UUV对圆形障碍物几何绕行的二维航路规划方法。The purpose of the present invention is to provide a two-dimensional route planning method for a UUV to geometrically circumvent a circular obstacle with simple calculation, high planning efficiency and fast planning speed.
本发明的目的是这样实现的：The purpose of the present invention is achieved like this:
步骤一：从使命文本读取航路起点Ob、航路终点Oe和各圆形障碍物的参数；Step 1: Read the parameters of the route starting point Ob , the route end point Oe and each circular obstacle from the mission text;
步骤二：对各圆形障碍物进行膨胀处理，计算膨胀后的各圆形障碍物的参数；Step 2: Perform expansion processing on each circular obstacle, and calculate parameters of each expanded circular obstacle;
步骤三：建立绕行点集合S，令规划当前点Oc为起点Ob，并放入绕行点集合S中；Step 3: Establish a detour point set S, make the current planning point Oc the starting point Ob , and put it into the detour point set S;
步骤四：如果规划当前点Oc是航路终点Oe，或者规划当前点Oc和航路终点Oe可视，转步骤六，否则执行步骤五；Step 4: If the planned current pointOc is the end pointOe of the route, or the planned current pointOc and the end point Oe of the routeare visible, go to step 6, otherwise execute step 5;
步骤五：对距规划当前点Oc最近的圆形障碍物进行几何绕行，得到绕行点并放入绕行点集合S中，更新规划当前点Oc，转步骤四；Step 5: Perform a geometric detour to the circular obstacle closest to the planned current pointOc , obtain the detour point and put it into the detour point set S, update the planned current pointOc , and go to step 4;
步骤六：将航路终点Oe放入绕行点集合S中，规划结束。Step 6: put the route end point Oe into the detour point set S, and the planning ends.
本发明还可以包括：The present invention may also include:
1、对圆形障碍物进行几何绕行的方法为：1. The method of geometrically circumventing circular obstacles is as follows:
(1)、判断规划当前点Oc是否在圆形障碍物上，如果在，令点O1＝Oc；否则，求解规划当前点Oc和圆心O的连线与圆周的交点，并令其为点O1；(1), judge whether the planned current pointOc is on the circular obstacle, if so, make point O1 =Oc ; otherwise, solve the intersection of the line connecting the planned current pointOc and the center O with the circle, and set which is point O1 ;
(2)、求解航路终点Oe和圆心O的连线与圆周的交点，并令其为点O2；(2), solve the point of intersection of the line connecting the end point Oe of the route and the center O and the circumference, and make it a point O2 ;
(3)、求解∠O1OO2的角平分线与圆周的交点，并令其为点O3；(3), solve the intersection point of the angle bisector of ∠O1 OO2 and the circumference, and let it be point O3 ;
(4)、求解过点O1的圆的切线与过点O3的圆的切线的交点，并令其为点O4；(4), solve the point of intersection of the tangent of the circle of passing point O1 and the tangent of the circle of crossing point O3 , and make it point O4 ;
(5)、计算过点O2的圆的切线与过点O3的圆的切线的交点，并令其为点O5；(5), calculate the point of intersection of the tangent of the circle passing through point O2 and the tangent of the circle passing through point O3 , and make it point O5 ;
(6)、将点O1、点O4、点O5、点O2作为圆形障碍物的绕行点放入绕行点集合S中，并更新规划当前点Oc为点O2，绕行结束。(6) Put the points O1 , O4 , O5 , and O2 into the set of detour points S as the detour points of circular obstacles, and update the planned current point Oc to point O2 , The detour ends.
2、对圆形障碍物进行膨胀处理的方法是在正常规划结束障碍物几何形状的基础上，按照障碍物形状边缘以安全半径ruuv向外扩展出一个安全半径区域。2. The method of expanding the circular obstacle is to expand a safe radius area outwards with a safe radius ruuv according to the edge of the obstacle shape on the basis of the normal planning end obstacle geometry.
本发明利用几何原理进行UUV的二维航路规划，在环境模型上采用了简单的几何模型，在计算无碰路径时采用简单的几何原理对障碍物进行绕行，避免了其他规划方法需要建立地图、循环搜索无碰路径所引起的信息量大、计算复杂的问题，不仅规划效率高、规划速度快，而且原理简单、计算量小，易于工程实现。The present invention uses geometric principles to plan the two-dimensional route of UUV, adopts a simple geometric model on the environment model, and uses simple geometric principles to bypass obstacles when calculating the collision-free path, avoiding the need to establish maps for other planning methods , The problem of large amount of information and complex calculation caused by cyclic search of non-touch paths, not only has high planning efficiency and fast planning speed, but also has simple principle, small calculation amount, and is easy to implement in engineering.
本发明与背景技术“基于几何算法的水下航行器路径规划(《海军工程大学学报》，2009，21(6)：41-44页)”的主要区别在于：The main difference between the present invention and the background technology "underwater vehicle path planning based on geometric algorithm ("Journal of Naval Engineering University", 2009, 21 (6): 41-44 pages)" is:
1、本发明对圆形障碍物进行了膨胀处理，而背景技术“基于几何算法的水下航行器路径规划”没有进行膨胀处理。对圆形障碍物进行膨胀处理，避免了UUV沿规划航路航行时与圆形障碍物的碰撞，提高了UUV的航行安全性。1. The present invention performs expansion processing on circular obstacles, while the background art "underwater vehicle path planning based on geometric algorithms" does not perform expansion processing. The circular obstacle is expanded to avoid the UUV's collision with the circular obstacle when navigating along the planned route, which improves the navigation safety of the UUV.
2、本发明和背景技术“基于几何算法的水下航行器路径规划”对单个圆形障碍物的绕行方法不同。本发明的绕行方法如图4和图5所示，形成的是多条线段组成的绕行航路；而背景技术形成的是线段和圆弧相结合的绕行航路，而圆弧航路不利于UUV的航路跟踪控制。本发明只有线段的绕行航路更利于UUV的航路跟踪控制，可以提高UUV的航路跟踪效果。2. The present invention and the background technology "Underwater Vehicle Path Planning Based on Geometric Algorithm" are different in the way of circumventing a single circular obstacle. As shown in Figure 4 and Figure 5, the detour method of the present invention forms a detour route composed of multiple line segments; while the background technology forms a detour route combined with line segments and arcs, and the arc route is not conducive to Path tracking control for UUV. In the present invention, only the circumnavigation route of the line segment is more conducive to the route tracking control of the UUV, and can improve the route tracking effect of the UUV.
本发明的有益效果在于：The beneficial effects of the present invention are:
1、环境模型采用的是几何空间模型，相比于传统的栅格、地图模型，所需规划信息量少，规划效率高，特别适合复杂多障碍物的环境。1. The environment model uses a geometric space model. Compared with the traditional grid and map models, the amount of planning information required is less and the planning efficiency is high. It is especially suitable for complex and multi-obstacle environments.
2、对圆形障碍物的绕行算法只应用到了几何原理，计算简单、易于工程实现，而且计算量非常小，规划速度快。2. The circumvention algorithm for circular obstacles is only applied to geometric principles, which is simple to calculate and easy to implement in engineering, and the amount of calculation is very small, and the planning speed is fast.
3、以UUV的外形尺寸为安全半径对圆形障碍物进行了膨胀处理，避免了UUV沿规划航路航行时与圆形障碍物的碰撞，提高了UUV的航行安全性。3. Taking the UUV's external dimensions as the safety radius, the circular obstacle is expanded, which avoids the UUV's collision with the circular obstacle when navigating along the planned route, and improves the navigation safety of the UUV.
4、形成的绕行航路只有线段，利于UUV的航路跟踪控制，可以提高UUV的航路跟踪效果。4. The formed detour route is only a line segment, which is beneficial to the UUV route tracking control and can improve the UUV route tracking effect.
附图说明Description of drawings
图1规划环境模型中的圆形障碍物示意图；Figure 1 schematic diagram of circular obstacles in the planning environment model;
图2圆形障碍物膨胀处理的安全半径示意图；Fig. 2 Schematic diagram of the safety radius of circular obstacle expansion processing;
图3圆形障碍物的膨胀示意图；Figure 3 Expansion schematic diagram of a circular obstacle;
图4规划当前点不在圆形障碍物上时对圆形障碍物的绕行示意图；Fig. 4 is a schematic diagram of planning a circumvention of a circular obstacle when the current point is not on the circular obstacle;
图5规划当前点在圆形障碍物上时对圆形障碍物的绕行示意图；Fig. 5 is a schematic diagram of circumventing a circular obstacle when the current point is planned on the circular obstacle;
图6UUV对圆形障碍物的几何绕行的流程图；Figure 6 Flow chart of UUV's geometric detour to circular obstacles;
图7判断两点连成线段与圆形障碍物是否相交的流程图；Fig. 7 is a flow chart for judging whether two points are connected into a line segment and a circular obstacle intersects;
图8UUV对圆形障碍物几何绕行的二维航路规划流程图；Fig. 8 Flowchart of two-dimensional route planning for UUV to geometrically circumvent circular obstacles;
图9利用本发明进行UUV对圆形障碍物的几何绕行的效果图。Fig. 9 is an effect diagram of UUV's geometric circumvention of a circular obstacle by using the present invention.
具体实施方式Detailed ways
下面举例对本发明进行详细说明。The following examples illustrate the present invention in detail.
结合图1介绍UUV二维航路规划的环境模型。Combined with Figure 1, the environment model of UUV two-dimensional route planning is introduced.
本发明中航路规划的环境模型采用的是二维几何空间模型。设规划的航路起点为Ob，航路终点为Oe，Ob和Oe分别用二维坐标表示为：The environment model of route planning in the present invention adopts a two-dimensional geometric space model. Suppose the starting point of the planned route is Ob , and the end point of the route is Oe , and Ob and Oe are respectively represented by two-dimensional coordinates as:
Ob＝(xob,yob)；Oe＝(xoe,yoe) (1)Ob ＝(xob ,yob ); Oe ＝(xoe ,yoe ) (1)
另设航路规划过程中每一步用到的规划当前点为Oc，用二维坐标表示为：In addition, it is assumed that the current planning point used in each step of the route planning process is Oc , which is expressed in two-dimensional coordinates as:
Oc＝(xoc,yoc) (2)Oc ＝(xoc ,yoc ) (2)
设二维几何空间中存在一定数量的圆形障碍物，如图1所示，设圆形障碍物为Zcirc，其参数化表示为：Suppose there are a certain number of circular obstacles in the two-dimensional geometric space, as shown in Figure 1, let the circular obstacles beZcirc , and its parameterization is expressed as:
Zcirc＝(xcirc,ycirc,r) (3)Zcirc ＝(xcirc , ycirc , r) (3)
式中，(xcirc,ycirc)表示圆心的二维坐标，r表示圆形障碍物的半径。In the formula, (xcirc , ycirc ) represents the two-dimensional coordinates of the center of the circle, and r represents the radius of the circular obstacle.
结合图2和图3介绍圆形障碍物膨胀模型的建立方法。Combining Figure 2 and Figure 3, the establishment method of the circular obstacle expansion model is introduced.
进行航路规划时，一般是把UUV当作质点来考虑的，因此规划的航路可能会距障碍物较近。但是实际上，UUV是有几何尺寸的实体，当规划的航路距障碍物较近时，很有可能导致UUV与障碍物发生碰撞。为此在进行航路规划时，设置一个安全半径来防止UUV沿规划航路航行时与障碍物发生碰撞。本发明采用的方法是以UUV的外形尺寸的外接圆半径ruuv为安全半径(见图2所示)，然后在正常圆形障碍物的几何形状的基础上，按照其形状边缘以半径ruuv向外扩展出一个安全半径区域。图3给出了圆形障碍物向外扩展安全半径后的膨胀示意图。When planning a route, UUV is generally considered as a particle, so the planned route may be closer to obstacles. But in fact, UUV is an entity with geometric dimensions. When the planned route is close to the obstacle, it is likely to cause the UUV to collide with the obstacle. For this reason, when planning the route, a safety radius is set to prevent the UUV from colliding with obstacles when navigating along the planned route. The method adopted in the present invention is to use the circumscribed circle radius ruuv of the external dimension of the UUV as the safety radius (as shown in Figure 2), and then on the basis of the geometric shape of a normal circular obstacle, according to its shape edge, the radius ruuv Expand a safe radius area outward. Figure 3 shows a schematic diagram of the expansion of the circular obstacle after the safety radius is expanded outward.
膨胀后圆形障碍物的参数化表示为：The parametric representation of the circular obstacle after dilation is:
Z′circ＝(xcirc,ycirc,r′) (4)Z'circ = (xcirc , ycirc , r') (4)
式中：(xcirc,ycirc)仍然表示圆心的二维坐标；而r′＝r+ruuv表示膨胀后的圆形障碍物半径。In the formula: (xcirc , ycirc ) still represents the two-dimensional coordinates of the center of the circle; and r′=r+ruuv represents the radius of the circular obstacle after expansion.
结合图4、图5、图6介绍UUV对圆形障碍物的绕行方法。Combined with Figure 4, Figure 5, and Figure 6, the method for UUV to circumvent circular obstacles is introduced.
对圆形障碍物的绕行分为规划当前点在圆形障碍物上和不在圆形障碍物上两种情况，图4给出了规划当前点不在圆形障碍物上的绕行示意图，图5给出了规划当前点在圆形障碍物上的绕行示意图。从图4和图5可以看出，对圆形障碍物的绕行，采用的是利用多条圆的切线的交点作为绕行点绕行圆形障碍物的方法，并利用几何原理求解各绕行点。The circumnavigation of circular obstacles can be divided into two situations: the planned current point is on the circular obstacle and not on the circular obstacle. Figure 4 shows a schematic diagram of the circumvention when the planned current point is not on the circular obstacle. Figure 4 5 gives a schematic diagram of planning the current point's detour on a circular obstacle. It can be seen from Fig. 4 and Fig. 5 that to circumnavigate circular obstacles, the method of circumventing circular obstacles is to use the intersection of tangent lines of multiple circles as detour points, and use geometric principles to solve the OK.
图6给出了圆形障碍物的绕行流程。Figure 6 shows the circumvention process of circular obstacles.
步骤一：判断规划当前点Oc是否在障碍物上，如果在障碍物上，令O1＝Oc，转步骤三；否则执行步骤二；Step 1: Determine whether the current planning point Oc is on an obstacle, if it is on an obstacle, set O1 =Oc , and go to step 3; otherwise, go to step 2;
步骤二：求解规划当前点Oc和圆心O的连线与圆的交点O1＝(xO1,yO1)，O1的位置坐标按式(5)计算，有两个解，选取与当前点Oc距离近的解作为点O1的坐标。Step 2: Solve and plan the intersection point O1 = (xO1 , yO1 ) of the line connecting the current point Oc and the center O of the circle and the circle. The position coordinates of O1 are calculated according to formula (5). There are two solutions, choose the same as the current The solution with the closest distance to point Oc is taken as the coordinate of point O1 .
式中：kc表示规划当前点Oc和圆心O所连直线的斜率，并且有In the formula: kc represents the slope of the straight line connecting the planned current point Oc and the center O, and there is
步骤三：求解航路终点Oe和圆心O的连线与圆的交点O2＝(xO2,yO2)，O2的位置坐标按式(6)计算，有两个解，选取与航路终点Oe距离近的解作为点O2的坐标。Step 3: Solve the intersection point O2 =(xO2 , yO2 ) of the line connecting the end point Oe and the center O of the airway and the circle. The position coordinates of O2 are calculated according to formula (6). There are two solutions, select the end point of the airway The solution with the closest distance to Oe is taken as the coordinates of point O2 .
式中：ke表示航路终点Oe和圆心O所连直线的斜率，并且有In the formula: ke represents the slope of the straight line connecting the route end point Oe and the circle center O, and
步骤四：求解∠O1OO2的角平分线L0与圆的交点O3＝(xO3,yO3)，O3的位置坐标按式(7)计算，有两个解，选取与点O1(或点O2)距离近的解作为点O3的坐标。Step 4: Solve the intersection point O 3 of the angle bisector L0 of ∠O1 OO2 and the circle O3 =(xO 3 , yO 3 ), the position coordinates of O3 are calculated according to formula (7), there are two solutions, select and point The solution with the closest distance to O1 (or point O2 ) is used as the coordinates of point O3 .
式中：表示∠O1OO2的角平分线L0的斜率，并且有In the formula: Indicates the slope of the angle bisector L0 of ∠O1 OO2 , and has
步骤五：分别求解过点O1、O2、O3的圆的切线L1、L2、L3，其切线方程分别按式(8)、式(9)和式(10)计算。Step 5: Solve the tangent lines L1 , L2 , and L3 of the circles passing through the points O1 , O2 , and O3 respectively, and the tangent line equations are calculated according to formula (8), formula (9) and formula (10).
式中：kL1、bL1分别表示圆的切线L1的斜率和截距；kL2、bL2分别表示圆的切线L2的In the formula: kL1 and bL1 respectively represent the slope and intercept of the tangent line L1 of the circle; kL2 and bL2 represent the slope and intercept of the tangent line L2 of the circle respectively
斜率和截距；kL3、bL3分别表示圆的切线L3的斜率和截距。Slope and intercept; kL3 and bL3 respectively represent the slope and intercept of the tangent line L3 of the circle.
步骤六：计算切线L1和L3的交点O4＝(xO4,yO4)，O4的位置坐标按式(11)计算Step 6: Calculate the intersection point O4 of tangent lines L1 and L3 =(xO4 , yO4 ), and the position coordinates of O4 are calculated according to formula (11)
步骤七：计算切线L2和L3的交点O5＝(xO5,yO5)，O5的位置坐标按式(12)计算Step 7: Calculate the intersection point O5 of tangent line L2 and L3 =(xO5 , yO5 ), the position coordinate of O5 is calculated according to formula (12)
步骤八：将点O1、O4、O5、O2作为圆形障碍物的绕行点放入绕行点集合S中，并更新当前点Oc为O2，绕行结束。Step 8: Put the points O1 , O4 , O5 , and O2 as the circumvention points of the circular obstacle into the detour point set S, and update the current point Oc to O2 , and the detour ends.
结合图7介绍判断规划当前点Oc和航路终点Oe是否可视的方法。A method for judging whether the planned current pointOc and the route end pointOe are visible is introduced in conjunction with FIG. 7 .
点Oc和点Oe是指两点不被任何圆形障碍物所阻挡。判断两点是否可视的方法就是判断两点连线所形成的线段是否与所有的圆形障碍物相交，如果不与任何圆形障碍物相交则表明两点可视。判断规划当前点Oc和航路终点Oe是否可视的流程如图7所示：。Point Oc and point Oe mean that the two points are not blocked by any circular obstacles. The way to judge whether two points are visible is to judge whether the line segment formed by connecting the two points intersects all circular obstacles. If it does not intersect any circular obstacles, it indicates that the two points are visible. The process of judging whether the planned current pointOc and the route end pointOe are visible is shown in Figure 7:.
步骤一：选择第一个圆形障碍物；Step 1: Select the first circular obstacle;
步骤二：求解当前点Oc和航路终点Oe两点连线和圆联立的二次方程的根的判别式Δ，求解方法如式(13)所示：Step 2: Solve the discriminant Δ of the root of the quadratic equation that connects the two points of the current pointOc and the end pointOe of the route and the circle, and the solution method is shown in formula (13):
式中，kce和bce分别表示点Oc和点Oe所连成直线的斜率和截距，并且有bce＝yoc-kcexoc；In the formula, kce and bce respectively represent the slope and intercept of the line connecting the point Oc and the point Oe , and have bce =yoc -kce xoc ;
步骤三：判断根的判别式Δ是否大于等于0，如果大于等于0转步骤四，否则转步骤六Step 3: Determine whether the discriminant Δ of the root is greater than or equal to 0, if it is greater than or equal to 0, go to step 4, otherwise go to step 6
步骤四：求解当前点Oc和航路终点Oe两点连线和圆的两个交点的横坐标xpc1和xpc2，求解方法如式(14)所示：Step 4: Solve the abscissas xpc1 and xpc2 of the two intersection points of the line between the current pointOc and the end pointOe of the route and the circle, and the solution method is shown in formula (14):
步骤五：判断xpc1的值在xoc和xoe之间或xpc2值在xoc和xoe之间是否满足，如果满足转步骤七，否则转步骤六；Step 5: Determine whether the value of xpc1 is between xoc and xoe or whether the value of xpc2 is between xoc and xoe is satisfied, if it is satisfied, go to step 7, otherwise go to step 6;
步骤六：判断是否还有圆形障碍物，如果有，选择下一个圆形障碍物，转步骤二，否则转步骤八；Step 6: Determine whether there are circular obstacles, if yes, select the next circular obstacle, go to step 2, otherwise go to step 8;
步骤七：当前点Oc和航路终点Oe不可视，判断结束；Step 7: The current pointOc and the route end pointOe are invisible, and the judgment is over;
步骤八：当前点Oc和航路终点Oe可视，判断结束。Step 8: The current pointOc and the route end pointOe are visible, and the judgment is completed.
结合图8介绍UUV对圆形障碍物几何绕行的二维航路规划的整个流程。In conjunction with Figure 8, the entire process of two-dimensional route planning for UUV geometrically circumventing circular obstacles is introduced.
步骤一：从使命文本读取航路起点Ob、航路终点Oe和各圆形障碍物的参数；Step 1: Read the parameters of the route starting point Ob , the route end point Oe and each circular obstacle from the mission text;
步骤二：建立各圆形障碍物膨胀模型，计算膨胀后的各圆形障碍物的参数，建立绕行点集合S；Step 2: Establish the expansion model of each circular obstacle, calculate the parameters of each circular obstacle after expansion, and establish a detour point set S;
步骤三：令规划当前点Oc为起点Ob，并放入绕行点集合S中；Step 3: Let the planning current point Oc be the starting point Ob , and put it into the detour point set S;
步骤四：判断规划当前点Oc是不是航路终点Oe，如果是转步骤十，否则转步骤五；Step 4: Judging whether the planned current point Oc is the end of the route Oe , if so, go to step 10, otherwise go to step 5;
步骤五：判断规划当前点Oc和航路终点Oe是否可视，如果可视转步骤十，否则转步骤六；Step 5: Judging whether the planned current pointOc and the route end pointOe are visible, if visible, go to step 10, otherwise go to step 6;
步骤六：搜索距规划当前点Oc最近的圆形障碍物；搜索方法为首先找到阻碍当前点Oc和航路终点Oe连线的所有圆形障碍物，然后求解以上各圆形障碍物中心与当前点Oc的距离，距离最小的即为距规划当前点Oc最近的圆形障碍物；Step 6: Search for the circular obstacle closest to the planned current point Oc ; the search method is to first find all circular obstacles that block the line between the current point Oc and the end point Oe of the route, and then solve the center of each circular obstacle above The distance from the current pointOc , the smallest distance is the nearest circular obstacle to the planned current pointOc ;
步骤七：对距规划当前点Oc最近的圆形障碍物进行几何绕行；Step 7: Perform geometric detour to the circular obstacle closest to the planned current pointOc ;
步骤八：将绕行点放入绕行点集合S中；Step 8: Put the detour points into the set S of detour points;
步骤九：更新规划当前点Oc，转步骤四；Step 9: Update and plan the current point Oc , go to step 4;
步骤十：将航路终点Oe放入绕行点集合S中，规划结束。Step 10: Put the end point Oe of the route into the detour point set S, and the planning ends.
图9给出了利用本发明进行UUV对圆形障碍物几何绕行二维航路规划的一个实现案例。Fig. 9 shows an implementation case of using the present invention to plan a two-dimensional route for a UUV to geometrically circumnavigate a circular obstacle.
本案例中，共设置了10个圆形障碍物，航路的起点Ob和航路的终点Oe已在图中标出。首先，在规划时对每个障碍物进行了膨胀处理，各障碍物的膨胀边界已在图中用点划线标出。然后，UUV对部分圆形障碍物进行了绕行，得到了绕行点集合S＝{Ob,P1,P2,P3,…,P16,Oe}，并在图中用虚线表示出了由绕行点组成的UUV绕行航路。In this case, a total of 10 circular obstacles are set, and the starting point Ob of the airway and the end point Oe of the airway have been marked in the figure. First, each obstacle is expanded during planning, and the expansion boundary of each obstacle has been marked with a dotted line in the figure. Then, the UUV circumnavigates part of the circular obstacle, and obtains the set of circumnavigation points S={Ob , P1 , P2 , P3 ,…,P16 ,Oe }, and uses the dotted line in the figure The table shows the UUV detour route composed of detour points.
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201610312415.1ACN105843234B (en) | 2016-05-12 | 2016-05-12 | A kind of two-dimentional Route planner that UUV detours to round barrier geometry |
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201610312415.1ACN105843234B (en) | 2016-05-12 | 2016-05-12 | A kind of two-dimentional Route planner that UUV detours to round barrier geometry |
| Publication Number | Publication Date |
|---|---|
| CN105843234A CN105843234A (en) | 2016-08-10 |
| CN105843234Btrue CN105843234B (en) | 2018-08-31 |
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201610312415.1AActiveCN105843234B (en) | 2016-05-12 | 2016-05-12 | A kind of two-dimentional Route planner that UUV detours to round barrier geometry |
| Country | Link |
|---|---|
| CN (1) | CN105843234B (en) |
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108073166A (en)* | 2016-11-16 | 2018-05-25 | 阳光暖果（北京）科技发展有限公司 | A kind of shortest time obstacle-avoiding route planning method |
| CN107168344B (en)* | 2017-05-17 | 2020-01-17 | 哈尔滨工程大学 | A method for generating a route during UUV approaching the seabed |
| CN107491068B (en)* | 2017-08-29 | 2020-12-04 | 歌尔股份有限公司 | Mobile robot path planning method and device and path planning equipment |
| CN109990782A (en)* | 2017-12-29 | 2019-07-09 | 北京欣奕华科技有限公司 | A kind of method and apparatus of avoiding obstacles |
| CN108279692B (en)* | 2018-01-17 | 2020-12-22 | 哈尔滨工程大学 | A UUV dynamic programming method based on LSTM-RNN |
| CN108829635B (en)* | 2018-03-09 | 2019-07-19 | 中国人民解放军海军大连舰艇学院 | A method for calculating the maneuvering area of enemy aircraft in surface ship's air defense |
| CN111290376B (en)* | 2018-11-22 | 2021-05-04 | 中国科学院沈阳自动化研究所 | Method for tracking circular track of unmanned underwater vehicle |
| CN109460045B (en)* | 2019-01-14 | 2022-02-22 | 哈尔滨工程大学 | Improved ant colony optimization-based collision avoidance planning method for USV under dynamic obstacle online perception |
| CN110658819B (en)* | 2019-09-30 | 2022-04-15 | 北京猎户星空科技有限公司 | Obstacle avoidance method and device, electronic equipment and storage medium |
| CN112179351B (en)* | 2020-09-30 | 2023-03-28 | 上海电机学院 | Three-dimensional obstacle avoidance track planning method based on pre-planned path optimization RRT algorithm |
| CN112859864A (en)* | 2021-01-15 | 2021-05-28 | 大连海事大学 | Unmanned ship-oriented geometric path planning method |
| CN114442626B (en)* | 2022-01-24 | 2024-04-26 | 深圳市拓普智造科技有限公司 | A global path planning method |
| JP2024029789A (en)* | 2022-08-23 | 2024-03-07 | 学校法人早稲田大学 | Robots, trajectory planning devices and programs |
| CN115179970B (en)* | 2022-09-14 | 2022-11-29 | 毫末智行科技有限公司 | Path planning method and device, electronic equipment and storage medium |
| CN119002468A (en)* | 2023-05-09 | 2024-11-22 | 广州视源电子科技股份有限公司 | Obstacle detouring path planning method, nonvolatile readable storage medium and robot |
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103576685A (en)* | 2013-11-14 | 2014-02-12 | 哈尔滨工程大学 | Method for determining path of UUV in process of recycling mother ship |
| CN103777639A (en)* | 2014-01-10 | 2014-05-07 | 哈尔滨工程大学 | UUV three-dimension sea route planning method in moving obstacle environment |
| CN104020770A (en)* | 2014-06-13 | 2014-09-03 | 哈尔滨工程大学 | UUV space trajectory planning method based on polynomial |
| CN105549600A (en)* | 2016-02-05 | 2016-05-04 | 哈尔滨工程大学 | Evading method based on opposite-direction sailing of virtual puffed motion obstacle and UUV |
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103576685A (en)* | 2013-11-14 | 2014-02-12 | 哈尔滨工程大学 | Method for determining path of UUV in process of recycling mother ship |
| CN103777639A (en)* | 2014-01-10 | 2014-05-07 | 哈尔滨工程大学 | UUV three-dimension sea route planning method in moving obstacle environment |
| CN104020770A (en)* | 2014-06-13 | 2014-09-03 | 哈尔滨工程大学 | UUV space trajectory planning method based on polynomial |
| CN105549600A (en)* | 2016-02-05 | 2016-05-04 | 哈尔滨工程大学 | Evading method based on opposite-direction sailing of virtual puffed motion obstacle and UUV |
| Title |
|---|
| 基于几何算法的水下航行器路径规划;刘晓;《海军工程大学学报》;20091231;第21卷(第6期);正文第42-44页* |
| Publication number | Publication date |
|---|---|
| CN105843234A (en) | 2016-08-10 |
| Publication | Publication Date | Title |
|---|---|---|
| CN105843234B (en) | A kind of two-dimentional Route planner that UUV detours to round barrier geometry | |
| CN105929824B (en) | A kind of UUV two dimension Route planners based on geometry detour principle | |
| Wu et al. | Guaranteed infinite horizon avoidance of unpredictable, dynamically constrained obstacles | |
| CN106441303A (en) | Path programming method based on A* algorithm capable of searching continuous neighborhoods | |
| WO2019042296A1 (en) | Method, apparatus and system for detecting obstacle collision in automatic parking path | |
| CN105082156B (en) | Space trajectory smoothing method based on speed optimum control | |
| CN108828955A (en) | Precise Track Tracking Control Method Based on Finite Time Extended State Observer | |
| CN106371445A (en) | Unmanned vehicle planning control method based on topology map | |
| CN106125764A (en) | A Dynamic Path Planning Method for UAV Based on A* Search | |
| CN112925342B (en) | Dynamic obstacle avoidance method for UAV based on improved mutual velocity obstacle method | |
| CN106020213B (en) | A kind of two-dimentional Route planner that UUV detours to rectangular obstruction geometry | |
| CN107990906B (en) | How to determine the path | |
| CN115562290A (en) | Robot path planning method based on A-star penalty control optimization algorithm | |
| CN104156520B (en) | Linear projection based convex-polyhedron collision detection method | |
| CN117075607A (en) | An improved JPS unmanned vehicle path planning method suitable for complex environments | |
| Do et al. | Narrow passage path planning using fast marching method and support vector machine | |
| CN117451068A (en) | A hybrid path planning method based on improved A* algorithm and dynamic window method | |
| CN117824643A (en) | Path planning method in long-range autonomous underwater robot ocean current environment | |
| Kim et al. | Any-angle path planning with limit-cycle circle set for marine surface vehicle | |
| CN109959383A (en) | Automatic parking path planning method | |
| Liu et al. | Trajectory planning for AGV based on the improved artificial potential field-A* algorithm | |
| CN102359783B (en) | Vision-based mobile robot positioning method | |
| CN105538309B (en) | A kind of robot barrier object Dynamic Recognition algorithm of limited sensing capability | |
| Wu et al. | Time‐Optimal Trajectory Planning along Parametric Polynomial Lane‐Change Curves with Bounded Velocity and Acceleration: Simulations for a Unicycle Based on Numerical Integration | |
| Yun et al. | Dynamic path planning for underwater vehicles based on modified artificial potential field method |
| Date | Code | Title | Description |
|---|---|---|---|
| C06 | Publication | ||
| PB01 | Publication | ||
| C10 | Entry into substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |