CN105843234A - 一种uuv对圆形障碍物几何绕行的二维航路规划方法 - Google Patents

Abstract

本发明提供的是一种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在复杂多圆形障碍环境中快速、高效的获得一条安全无碰的二维航路。

Description

一种UUV对圆形障碍物几何绕行的二维航路规划方法

技术领域

本发明涉及的是一种是UUV航路规划方法，具体的是一种UUV对圆形障碍物几何绕行的二维航路规划方法。

背景技术

航路规划是水下无人航行器(Unmanned Underwater Vehicle，UUV)的关键技术之一，是UUV自主能力的重要体现。航路规划是指在已知障碍环境下，规划出一条从起点出发绕过所有障碍物并到达终点的无碰路径。根据空间维度，航路规划可分为二维航路规划和三维航路规划。其中，二维航路规划是三维航路规划的基础，并且在UUV的应用也更为广泛，是UUV航路规划技术研究的热点。目前，UUV的航路规划方法很多，但是如何在复杂的障碍环境下既快速又可行的获得一条无碰路径，特别是规划方法能够适于工程应用，仍然是一个难点。

与本发明相关的已有技术为“基于几何算法的水下航行器路径规划”(《海军工程大学学报》，2009，21(6)：41-44页)，其中提到了考虑圆形障碍物时水下航行器基于几何算法的路径规划，但该文献对圆形障碍物绕行的航路规划方法与本发明不同。

发明内容

本发明的目的在于提供一种计算简单、规划效率高、规划速度快的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中，规划结束。

本发明还可以包括：

1、对圆形障碍物进行几何绕行的方法为：

(1)、判断规划当前点O_c是否在圆形障碍物上，如果在，令点O₁＝O_c；否则，求解规划当前点O_c和圆心O的连线与圆周的交点，并令其为点O₁；

(2)、求解航路终点O_e和圆心O的连线与圆周的交点，并令其为点O₂；

(3)、求解∠O₁OO₂的角平分线与圆周的交点，并令其为点O₃；

(4)、求解过点O₁的圆的切线与过点O₃的圆的切线的交点，并令其为点O₄；

(5)、计算过点O₂的圆的切线与过点O₃的圆的切线的交点，并令其为点O₅；

(6)、将点O₁、点O₄、点O₅、点O₂作为圆形障碍物的绕行点放入绕行点集合S中，并更新规划当前点O_c为点O₂，绕行结束。

2、对圆形障碍物进行膨胀处理的方法是在正常规划结束障碍物几何形状的基础上，按照障碍物形状边缘以安全半径r_uuv向外扩展出一个安全半径区域。

本发明利用几何原理进行UUV的二维航路规划，在环境模型上采用了简单的几何模型，在计算无碰路径时采用简单的几何原理对障碍物进行绕行，避免了其他规划方法需要建立地图、循环搜索无碰路径所引起的信息量大、计算复杂的问题，不仅规划效率高、规划速度快，而且原理简单、计算量小，易于工程实现。

本发明与背景技术“基于几何算法的水下航行器路径规划(《海军工程大学学报》，2009，21(6)：41-44页)”的主要区别在于：

1、本发明对圆形障碍物进行了膨胀处理，而背景技术“基于几何算法的水下航行器路径规划”没有进行膨胀处理。对圆形障碍物进行膨胀处理，避免了UUV沿规划航路航行时与圆形障碍物的碰撞，提高了UUV的航行安全性。

2、本发明和背景技术“基于几何算法的水下航行器路径规划”对单个圆形障碍物的绕行方法不同。本发明的绕行方法如图4和图5所示，形成的是多条线段组成的绕行航路；而背景技术形成的是线段和圆弧相结合的绕行航路，而圆弧航路不利于UUV的航路跟踪控制。本发明只有线段的绕行航路更利于UUV的航路跟踪控制，可以提高UUV的航路跟踪效果。

本发明的有益效果在于：

1、环境模型采用的是几何空间模型，相比于传统的栅格、地图模型，所需规划信息量少，规划效率高，特别适合复杂多障碍物的环境。

2、对圆形障碍物的绕行算法只应用到了几何原理，计算简单、易于工程实现，而且计算量非常小，规划速度快。

3、以UUV的外形尺寸为安全半径对圆形障碍物进行了膨胀处理，避免了UUV沿规划航路航行时与圆形障碍物的碰撞，提高了UUV的航行安全性。

4、形成的绕行航路只有线段，利于UUV的航路跟踪控制，可以提高UUV的航路跟踪效果。

附图说明

图1规划环境模型中的圆形障碍物示意图；

图2圆形障碍物膨胀处理的安全半径示意图；

图3圆形障碍物的膨胀示意图；

图4规划当前点不在圆形障碍物上时对圆形障碍物的绕行示意图；

图5规划当前点在圆形障碍物上时对圆形障碍物的绕行示意图；

图6 UUV对圆形障碍物的几何绕行的流程图；

图7判断两点连成线段与圆形障碍物是否相交的流程图；

图8 UUV对圆形障碍物几何绕行的二维航路规划流程图；

图9利用本发明进行UUV对圆形障碍物的几何绕行的效果图。

具体实施方式

下面举例对本发明进行详细说明。

结合图1介绍UUV二维航路规划的环境模型。

本发明中航路规划的环境模型采用的是二维几何空间模型。设规划的航路起点为O_b，航路终点为O_e，O_b和O_e分别用二维坐标表示为：

O_b＝(x_ob,y_ob)；O_e＝(x_oe,y_oe) (1)

另设航路规划过程中每一步用到的规划当前点为O_c，用二维坐标表示为：

O_c＝(x_oc,y_oc) (2)

设二维几何空间中存在一定数量的圆形障碍物，如图1所示，设圆形障碍物为Z_circ，其参数化表示为：

Z_circ＝(x_circ,y_circ,r) (3)

式中，(x_circ,y_circ)表示圆心的二维坐标，r表示圆形障碍物的半径。

结合图2和图3介绍圆形障碍物膨胀模型的建立方法。

进行航路规划时，一般是把UUV当作质点来考虑的，因此规划的航路可能会距障碍物较近。但是实际上，UUV是有几何尺寸的实体，当规划的航路距障碍物较近时，很有可能导致UUV与障碍物发生碰撞。为此在进行航路规划时，设置一个安全半径来防止UUV沿规划航路航行时与障碍物发生碰撞。本发明采用的方法是以UUV的外形尺寸的外接圆半径r_uuv为安全半径(见图2所示)，然后在正常圆形障碍物的几何形状的基础上，按照其形状边缘以半径r_uuv向外扩展出一个安全半径区域。图3给出了圆形障碍物向外扩展安全半径后的膨胀示意图。

膨胀后圆形障碍物的参数化表示为：

Z′_circ＝(x_circ,y_circ,r′) (4)

式中：(x_circ,y_circ)仍然表示圆心的二维坐标；而r′＝r+r_uuv表示膨胀后的圆形障碍物半径。

结合图4、图5、图6介绍UUV对圆形障碍物的绕行方法。

对圆形障碍物的绕行分为规划当前点在圆形障碍物上和不在圆形障碍物上两种情况，图4给出了规划当前点不在圆形障碍物上的绕行示意图，图5给出了规划当前点在圆形障碍物上的绕行示意图。从图4和图5可以看出，对圆形障碍物的绕行，采用的是利用多条圆的切线的交点作为绕行点绕行圆形障碍物的方法，并利用几何原理求解各绕行点。

图6给出了圆形障碍物的绕行流程。

步骤一：判断规划当前点O_c是否在障碍物上，如果在障碍物上，令O₁＝O_c，转步骤三；否则执行步骤二；

步骤二：求解规划当前点O_c和圆心O的连线与圆的交点O₁＝(x_O1,y_O1)，O₁的位置坐标按式(5)计算，有两个解，选取与当前点O_c距离近的解作为点O₁的坐标。

式中：k_c表示规划当前点O_c和圆心O所连直线的斜率，并且有

步骤三：求解航路终点O_e和圆心O的连线与圆的交点O₂＝(x_O2,y_O2)，O₂的位置坐标按式(6)计算，有两个解，选取与航路终点O_e距离近的解作为点O₂的坐标。

式中：k_e表示航路终点O_e和圆心O所连直线的斜率，并且有

步骤四：求解∠O₁OO₂的角平分线L₀与圆的交点O₃＝(x_O3,y_O3)，O₃的位置坐标按式(7)计算，有两个解，选取与点O₁(或点O₂)距离近的解作为点O₃的坐标。

式中：表示∠O₁OO₂的角平分线L₀的斜率，并且有

步骤五：分别求解过点O₁、O₂、O₃的圆的切线L₁、L₂、L₃，其切线方程分别按式(8)、式(9)和式(10)计算。

$L_1:\left\{\begin{array}{c}y=k_{L1}x+b_{L1}\\ k_{L1}=-1/k_c\\ b_{L1}=y_{O1}-k_{L1}{x}_{O1}\end{array}\right.---\left(8\right)$

$L_2:\left\{\begin{array}{c}y=k_{L2}x+b_{L2}\\ k_{L2}=-1/k_e\\ b_{L2}=y_{O2}-k_{L2}{x}_{O2}\end{array}\right.---\left(9\right)$

$L_3:\left\{\begin{array}{c}y=k_{L3}x+b_{L3}\\ k_{L3}=-1/k_{L0}\\ b_{L3}=y_{O3}-k_{L3}{x}_{O3}\end{array}\right.---\left(10\right)$

式中：k_L1、b_L1分别表示圆的切线L₁的斜率和截距；k_L2、b_L2分别表示圆的切线L₂的

斜率和截距；k_L3、b_L3分别表示圆的切线L₃的斜率和截距。

步骤六：计算切线L₁和L₃的交点O₄＝(x_O4,y_O4)，O₄的位置坐标按式(11)计算

$x_{O_4}=\frac{b_{L3}-b_{L1}}{k_{L1}-k_{L3}};y_{O_4}=\frac{k_{L1}(b_{L3}-b_{L1})}{k_{L1}-k_{L3}}+b_{L1}---\left(11\right)$

步骤七：计算切线L₂和L₃的交点O₅＝(x_O5,y_O5)，O₅的位置坐标按式(12)计算

$x_{O_5}=\frac{b_{L3}-b_{L2}}{k_{L2}-k_{L3}};y_{O_5}=\frac{k_{L2}(b_{L3}-b_{L2})}{k_{L2}-k_{L3}}+b_{L2}---\left(12\right)$

步骤八：将点O₁、O₄、O₅、O₂作为圆形障碍物的绕行点放入绕行点集合S中，并更新当前点O_c为O₂，绕行结束。

结合图7介绍判断规划当前点O_c和航路终点O_e是否可视的方法。

点O_c和点O_e是指两点不被任何圆形障碍物所阻挡。判断两点是否可视的方法就是判断两点连线所形成的线段是否与所有的圆形障碍物相交，如果不与任何圆形障碍物相交则表明两点可视。判断规划当前点O_c和航路终点O_e是否可视的流程如图7所示：。

步骤一：选择第一个圆形障碍物；

步骤二：求解当前点O_c和航路终点O_e两点连线和圆联立的二次方程的根的判别式Δ，求解方法如式(13)所示：

$\mathrm{\Δ}={({2k}_{\mathrm{ce}}{b}_{\mathrm{ce}}-2x_{\mathrm{circ}}-2y_{\mathrm{circ}}{k}_{\mathrm{ce}})}^2-4(1+k_{\mathrm{ce}}^2)(x_{\mathrm{circ}}^2+b_{\mathrm{ce}}^2+y_{\mathrm{circ}}^2-2y_{\mathrm{circ}}{b}_{\mathrm{ce}}-r^2)---\left(13\right)$

式中，k_ce和b_ce分别表示点O_c和点O_e所连成直线的斜率和截距，并且有b_ce＝y_oc-k_cex_oc；

步骤三：判断根的判别式Δ是否大于等于0，如果大于等于0转步骤四，否则转步骤六

步骤四：求解当前点O_c和航路终点O_e两点连线和圆的两个交点的横坐标x_pc1和x_pc2，求解方法如式(14)所示：

$x_{pc1},x_{pc2}=\frac{-(2k_{ce}{b}_{ce}-2x_{circ}-2y_{circ}{k}_p)\±\sqrt{\mathrm{\Δ}}}{2(1+k_{ce}^2)}---\left(14\right)$

步骤五：判断x_pc1的值在x_oc和x_oe之间或x_pc2值在x_oc和x_oe之间是否满足，如果满足转步骤七，否则转步骤六；

步骤六：判断是否还有圆形障碍物，如果有，选择下一个圆形障碍物，转步骤二，否则转步骤八；

步骤七：当前点O_c和航路终点O_e不可视，判断结束；

步骤八：当前点O_c和航路终点O_e可视，判断结束。

结合图8介绍UUV对圆形障碍物几何绕行的二维航路规划的整个流程。

步骤一：从使命文本读取航路起点O_b、航路终点O_e和各圆形障碍物的参数；

步骤二：建立各圆形障碍物膨胀模型，计算膨胀后的各圆形障碍物的参数，建立绕行点集合S；

步骤三：令规划当前点O_c为起点O_b，并放入绕行点集合S中；

步骤四：判断规划当前点O_c是不是航路终点O_e，如果是转步骤十，否则转步骤五；

步骤五：判断规划当前点O_c和航路终点O_e是否可视，如果可视转步骤十，否则转步骤六；

步骤六：搜索距规划当前点O_c最近的圆形障碍物；搜索方法为首先找到阻碍当前点O_c和航路终点O_e连线的所有圆形障碍物，然后求解以上各圆形障碍物中心与当前点O_c的距离，距离最小的即为距规划当前点O_c最近的圆形障碍物；

步骤七：对距规划当前点O_c最近的圆形障碍物进行几何绕行；

步骤八：将绕行点放入绕行点集合S中；

步骤九：更新规划当前点O_c，转步骤四；

步骤十：将航路终点O_e放入绕行点集合S中，规划结束。

图9给出了利用本发明进行UUV对圆形障碍物几何绕行二维航路规划的一个实现案例。

本案例中，共设置了10个圆形障碍物，航路的起点O_b和航路的终点O_e已在图中标出。首先，在规划时对每个障碍物进行了膨胀处理，各障碍物的膨胀边界已在图中用点划线标出。然后，UUV对部分圆形障碍物进行了绕行，得到了绕行点集合S＝{O_b,P₁,P₂,P₃,…,P₁₆,O_e}，并在图中用虚线表示出了由绕行点组成的UUV绕行航路。

Claims (3)

1.一种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中，规划结束。

2.根据权利要求1所述的UUV对圆形障碍物几何绕行的二维航路规划方法，其特征是对圆形障碍物进行几何绕行的方法为：

(1)、判断规划当前点O_c是否在圆形障碍物上，如果在，令点O₁＝O_c；否则，求解规划当前点O_c和圆心O的连线与圆周的交点，并令其为点O₁；

(2)、求解航路终点O_e和圆心O的连线与圆周的交点，并令其为点O₂；

(3)、求解∠O₁OO₂的角平分线与圆周的交点，并令其为点O₃；

(4)、求解过点O₁的圆的切线与过点O₃的圆的切线的交点，并令其为点O₄；

(5)、计算过点O₂的圆的切线与过点O₃的圆的切线的交点，并令其为点O₅；

(6)、将点O₁、点O₄、点O₅、点O₂作为圆形障碍物的绕行点放入绕行点集合S中，并更新规划当前点O_c为点O₂，绕行结束。

3.根据权利要求1或2所述的UUV对圆形障碍物几何绕行的二维航路规划方法，其特征是对圆形障碍物进行膨胀处理的方法是：在正常规划结束障碍物几何形状的基础上，按照障碍物形状边缘以安全半径r_uuv向外扩展出一个安全半径区域。