CN107168344B - 一种uuv抵近海底作业过程中航路生成方法 - Google Patents

Abstract

本发明公开了一种UUV抵近海底作业过程中航路生成方法。包括以下步骤，步骤一：UUV利用传感器采集当前自身信息，UUV接收任务目标点，UUV接收障碍物信息；步骤二：构建栅格地图，根据UUV的几何约束对障碍物进行膨胀处理，表示在相应的栅格地图中；步骤三：根据每个栅格障碍属性

将栅格地图分为可行区和不可行区；步骤四：根据障碍物位置误差和UUV导航误差计算可行区S₁中每一栅格

的潜在危险性步骤五：根据每个栅格的潜在危险性

利用逆向A^*算法生成航路。本发明能够提高航路精度，提高UUV抵近海底作业安全性与可靠性。

Description

一种UUV抵近海底作业过程中航路生成方法

技术领域

本发明属于水下无人航行器(Unmanned Underwater Vehicle，UUV)自主控制领域，尤其涉 及针对复杂作业环境UUV进行自主航行的，一种UUV抵近海底作业过程中航路生成方法。

背景技术

在水下无人航行器抵近海底作业过程中，由于距离远、环境复杂，一般要求UUV具有高 续航能力与自主能力，当UUV执行抵近海底作业任务时，UUV难以随时返回水面或者母船附 近进行任务下载和信息反馈，一般需要UUV在完成一系列任务后再返回。UUV需要先航行到 作业区，根据接收到的任务目标点进行航路生成，UUV沿着生成的航路避开障碍物到达目标 点。因此对UUV抵近海底作业的安全性要求非常高。

由于传感器本身就存在测量误差，导致已知障碍物的位置其实就存在原始误差；当UUV 进行抵近海底作业时，如果在生成UUV航路过程中对障碍物的原始位置误差不予考虑，那么 实际障碍物会严重影响UUV的航行安全。同样由于UUV深海作业的特殊性，UUV不能及时进 行导航校准，因此UUV的导航误差也会影响UUV的航行。

现有技术中，申请号201210102688.5的专利《一种水下航行器的避障方法》和申请号 201110086941.8的专利《基于迷你声呐的水下目标探测与AUV自动避碰方法及系统》。虽然 两个专利都涉及UUV对障碍物的避碰处理，但并没有涉及传感器测量障碍物导致障碍物位置 误差对UUV航行安全的影响。以及申请号201410264172.X的专利《基于多项式的UUV空间轨 迹规划方法》也没有涉及障碍物位置误差及UUV导航误差对规划轨迹的影响。因此在实际障 碍物的不确定性以及UUV导航误差影响下，如何生成高精度的航路，提高UUV深海作业安全 性与鲁棒性，成为我们亟待解决的问题。

发明内容

本发明的目的是提供一种能够生成最优航路，进一步提高UUV远距离作业安全性与可靠 性的，UUV抵近海底作业过程中航路生成方法。

一种UUV抵近海底作业过程中航路生成方法，包括以下几个步骤，

步骤一：UUV利用传感器采集当前自身位姿信息，包括：UUV的初始位置(x_S,y_S)，航行 速度V₀，航行时间t₀；UUV接收任务目标点(x_G,y_G)，UUV接收障碍物信息，其中UUV接收到的第k个障碍物O_k为以CO_k为圆心，以R_Ok为半径的圆，障碍物圆心坐标为

0≤k≤M；M为障碍物数量；

步骤二：构建栅格地图，栅格I_ij的坐标为(x_i,y_j)；根据UUV的几何约束对障碍物进行第 一次膨胀处理，膨胀宽度D1＝L，L为UUV体长，第一次膨胀后障碍物O′_k的圆心不变，半径 为R′_Ok＝R_Ok+D1，将第一次膨胀后的障碍物表示在相应的栅格地图中，有障碍物的栅格障碍 属性

记为1，没有障碍物的栅格障碍属性

记为0；

步骤三：根据栅格障碍属性

将栅格地图分为可行区和不可行区，如果栅格障碍属性

为0，该栅格属于可行区S₁，否则该栅格属于不可行区S₂；

步骤四：根据障碍物原始位置误差和UUV导航误差计算可行区S₁中栅格

的潜在危险性

(1)对障碍物进行第二次膨胀处理，在第一次膨胀后的障碍物基础上，将障碍物的半径 向外扩展宽度D2，第二次膨胀后障碍物O″_k的半径R″_Ok＝R′_Ok+D2，圆心不变，第二次膨胀 区域为障碍物的潜在危险区，潜在危险区属于栅格地图中的可行区，

潜在危险区的宽度D2＝αD_obs+βD_nav，其中D_obs为由障碍物位置误差引起危险宽度，D_nav为由UUV导航误差引起的危险宽度，α和β为系数，α+β＝1，0≤α≤1，0≤β≤1；

步骤五：根据栅格的潜在危险性

利用逆向A^*算法生成航路。

本发明一种UUV深海海底作业过程中航路生成方法，还可以包括：

1、由障碍物位置误差引起危险宽度D_obs取值为2～20米。

2、由UUV导航误差引起的危险宽度D_nav：

其中，b为系数。

3、所述的利用逆向A^*算法生成航路的过程中：

以UUV的初始位置(x_s,y_s)作为航路起始点S，以UUV接收的任务目标(x_G,y_G)点作为航路 目标点G；

将一个栅格对应一个节点，节点之间的边表示栅格的潜在危险性，用节点n表示栅格I_ij， 节点n的潜在危险性

待扩展节点n的最佳性评价函数f(n)为：

f(n)＝g(n)+h(n)

式中，f(n)表示从节点G开始约束通过节点n的一条最优路径的代价；g(n)表示从节点G 到节点n的最优路径代价，这个代价等于到目前为止已经产生的最优路径的代价g(p)加上从 当前节点p到节点n的边的代价g′(p,n)，具体为：g(n)＝g(p)+g′(p,n)；h(n)表示从节点n 到节点S的最小估计代价，从节点n到节点S的直线欧氏距离作为h(n)的最小估计代价；

栅格地图具有8个方向的连通性，从节点p到节点n的边的代价函数g′(p,n)为：

本发明具有如下有益效果：

1.本发明在UUV航路生成过程中考虑到障碍物原始位置误差以及UUV导航误差对UUV安 全性的影响，将障碍物原始位置误差视为障碍物自身不确定性，将UUV导航误差转化为障碍 物相对位置误差，因此根据障碍物原始位置误差和UUV导航误差求取障碍物的潜在危险区， 并求取潜在危险区对UUV威胁的概率。因此本发明减小了由于传感器器件自身测量误差导致 的障碍物原始位置误差，并且补偿了UUV导航误差对UUV航路生成的影响，进一步得到最优 航路，提高UUV海底作业安全性与可靠性。

2.本发明将障碍物原始位置误差和UUV导航误差均转化为障碍物的潜在危险区进行处理， 减少了复杂度，简单方便，效率高。

附图说明

图1本发明航路生成流程图。

图2圆形障碍物的模型图。

图3对障碍物进行第一次膨胀处理示意图。

图4栅格地图示意图。

图5对障碍物进行第二次膨胀处理示意图。

具体实施方式

下面将结合附图对本发明做进一步详细说明。

如图1所示，一种UUV抵近海底作业过程中航路生成方法，包括以下几个步骤：

步骤一：UUV利用传感器采集当前自身信息，包括：UUV的初始位置(x_S,y_S)；UUV接收任 务目标点(x_G,y_G)，UUV接收障碍物信息；M为障碍物数量；

步骤二：构建栅格地图，首先对UUV作业区域的二维环境空间进行栅格离散化，采用直 角坐标标识栅格，以栅格阵左下角为坐标原点。任一栅格I_ij均可用唯一的直角坐标标识 (x_i,y_j)其位置，0≤i≤N₁，0≤j≤N₂，N₁为栅格阵在x轴方向的栅格个数，N₂为栅格阵在 y轴方向栅格个数。栅格间距Δl一般为10L～20L之间，其中L为UUV的体长，一般可考虑 UUV的作业区域范围为100km×100km。

任一个栅格I_ij通过一个四元组表示(x_i,y_j)为栅格的位置坐标，

为栅格的障碍属性，

为栅格的潜在危险性。

根据UUV的几何约束对障碍物进行第一次膨胀处理。如图2所示，一般障碍物为不规则 形状，以一个半径为R_Ok的外切圆去包络该障碍物，从而将不规则障碍物扩充为规则的凸障 碍。因此，UUV接收到的第k个障碍物O_k为以CO_k为圆心，以R_Ok为半径的圆，障碍物圆心坐标为

0≤k≤M。考虑到UUV的几何约束，对障碍物进行第一次膨胀处理，膨胀 宽度D1＝L，即将包络障碍物的圆的半径延长D1，第一次膨胀后障碍物O′_k的半径 R′_Ok＝R_Ok+D1，圆心不变，如图3所示。

将第一次膨胀后的障碍物表示在相应的栅格地图中，有障碍物的栅格障碍属性

记为 1，没有障碍物的栅格障碍属性

记为0。

步骤三：根据每个栅格障碍属性

将栅格地图分为可行区和不可行区，如果栅格障碍属 性

为0，该栅格属于可行区S₁，否则该栅格属于不可行区S₂，如图4所示。

下面仅对可行区进行分析处理，大大减少了工作量。

步骤四：根据障碍物原始位置误差和UUV导航误差计算可行区S₁中每一栅格I_ij的潜在危 险性

具体过程为：

(1)对障碍物进行第二次膨胀处理，将第一次膨胀后的障碍物模型向外扩展宽度D2，得 到障碍物的潜在危险区，潜在危险区属于栅格地图中的可行区；

由于传感器器件自身存在测量误差，导致传送给UUV的已知障碍物本身存在位置误差， 此时为障碍物自身的不确定性。也就是说UUV获得的障碍物位置信息和实际障碍物位置之间 存在位置误差，即障碍物的原始位置误差。

由于在航行时，UUV不能及时进行导航校准，因此UUV的导航误差也会影响UUV的航行， 为了提高可靠性，将UUV的导航误差转化为障碍物的相对误差进行处理，减少导航误差对UUV 航行的影响。此处为由UUV导航误差转化为障碍物不确定性。

障碍物不确定性，也就是障碍物中心位置的不确定性，表现为UUV在航行过程中可能与 障碍物发生碰撞，对障碍物进行第二次膨胀处理，得到障碍物的潜在危险区，潜在危险区属 于栅格地图中的可行区。潜在危险区的引入是为了表示由于障碍物不确定性对航路生成的影 响。

因此潜在危险区的宽度由两部分因素决定，一个是障碍物原始位置误差，另一个是UUV 的导航误差。

潜在危险区的宽度D2＝αD_obs+βD_nav，其中D_obs为由障碍物原始位置误差产生危险宽度， D_nav为由UUV导航误差转化为障碍物相对位置误差产生的危险宽度，α和β为系数， α+β＝1，0≤α≤1，0≤β≤1；第二次膨胀后障碍物O″_k的半径R″_Ok＝R′_Ok+D2，圆心不变， 如图5所示，其中α＝0.37，β＝0.63。

障碍物原始位置误差产生的危险宽度D_obs的取值一般为2～20米。

将UUV的导航误差转化为障碍物相对误差进行处理，UUV的导航误差转化为障碍物相对误 差的危险宽度D_nav:

其中，b为系数，一般取值为0.4％～0.6％，最优为0.53％。

(2)计算可行区S1中每一栅格(x_i,y_j)的潜在危险性

障碍物不确定性，表现为障碍物O_k的圆心将不再固定在一个点上，而是以一定的概率密 度分布在以η＝E(C_ΩO_k)为圆心，以R_Ω为半径的一个圆域Ω内，用集合表示为：

其中(x_η,y_η)为圆域Ω圆心坐标。

定义p(r)为C_ΩO_k分布在以η＝E(C_ΩO_k)为圆心，以r为半径的圆D_r内的概率，则有：

其中，ρ(x,y)为C_ΩO_k分布在圆域Ω内点(x,y)处的概率密度，ρ(x,y)≥0。障碍物中心 C_ΩO_k在圆域Ω内服从均匀分布时，ρ(x,y)为：

栅格地图不可行区S₂包括M个独立的禁入区Fbd_k，0≤k≤M。一个禁入区对应一个第一 次膨胀后的障碍物O′_k，每个禁入区边界栅格集合为

可行区S₁中任一栅格的潜在危险 性

为：

其中，(x_E，y_E)为任一边界栅格的坐标，

为可行区中一个栅格，其坐标为(x_i，y_j)。

因此，障碍物的潜在危险区可以看作是第二次膨胀后障碍物O″_k的圆心CO_k以一定的概率 密度分布在以CO_k为圆心，以D2为半径的一个圆域Ω内，进而产生的障碍物的潜在危险区。

为栅格

受禁入区Fbd_k威胁的概率。进一步得到，可行区S₁中任一栅格的潜 在危险性

为：

步骤五：将每一个栅格对应一个节点，节点之间的边表示栅格的潜在危险性，如果用节 点n表示栅格I_ij，节点n的潜在危险性

利用逆向A^*算法生成航路。

令UUV的初始位置(x_S,y_S)作为栅格地图中的起始节点S，令UUV接收的任务目标(x_G,y_G) 点作为栅格地图中的目标节点G。

逆向A^*算法是最佳优先的启发式搜索方法，它从目标节点G出发，通过每次向路径中加 入“最佳节点”来逐步扩展路径，最终产生一条从目标节点G到起始节点S的最优路径。逆 向A^*算法的核心就是设计一个能表示待扩展节点n是否最佳的评价函数f(n)：

f(n)＝g(n)+h(n)

f(n)：表示从节点G开始约束通过节点n的一条最优路径的代价。

g(n)：表示从节点G到节点n的最优路径代价，由于逆向A^*算法是从目标节点G开始向 外扩展的，所以这个代价就等于到目前为止已经产生的最优路径的代价g(p)加上从当前节点 p到节点n的边的代价g′(p,n)，具体为：

g(n)＝g(p)+g′(p,n)。

h(n)：表示从节点n到节点S的最小估计代价。由于h(n)是一个估计值，所以h(n)被称 为启发函数。h(n)被设计为从节点n到节点S的直线欧氏距离作为h(n)的最小估计代价，这 个直线距离小于实际要经过的路径长度。

栅格地图具有8个方向的连通性，定义从节点p到节点n的边的代价函数g′(p,n)为：

基于逆向A^*算法的路径搜索算法为：

步骤1.将目标节点G放入未扩展节点表OPEN中，记f(n)＝h(n)，令扩展节点表CLOSED 为空。

步骤2.若OPEN表为空，则失败退出，无解。

步骤3.从OPEN表中选取具有最小f(n)值的节点n进行扩展。

步骤4.将节点n从OPEN表中移出，并把它放入CLOSED表中。

步骤5.若节点n为起始节点S，则成功退出，并给出S到G的路径。

步骤6.若节点n不是起始节点S，则扩展，并生成其后继节点集M_sn＝{m_i}。

步骤7.对每个后继结点m_i进行如下操作：

(1)计算g(m_i)＝g(n)+g′(n,m_i)。

(2)若m_i在OPEN表中和CLOSED表中出现过，则把它放入OPEN表中，并添加到节点n的后裔表中。

(3)若m_i在OPEN表中或CLOSED表中有重复节点k，并且g(m_i)＜g(k)，则

a、令g(k)＝g(m_i)，并修改节点k的父辈节点指针，使其指向n；

b、如果节点m_i是在CLOSED表中，还要根据后裔表修改k的所有在OPEN表和 CLOSED表中的后节点g值和f值。

c、将k添加到节点n后裔表中。

步骤8.根据更行的f(n)值，重排OPEN表的次序。

步骤9.转至步骤2。

Claims (4)

1.一种UUV抵近海底作业过程中航路生成方法，其特征在于,包括以下几个步骤，

步骤一：UUV利用传感器采集当前自身位姿信息，包括：UUV的初始位置(x_S,y_S)，航行速度V₀，航行时间t₀；UUV接收任务目标点(x_G,y_G)，UUV接收障碍物信息，其中UUV接收到的第k个障碍物O_k为以CO_k为圆心，以R_Ok为半径的圆，障碍物圆心坐标为0≤k≤M；M为障碍物数量；

步骤二：构建栅格地图，栅格I_ij的坐标为(x_i,y_j)；根据UUV的几何约束对障碍物进行第一次膨胀处理，膨胀宽度D1＝L，L为UUV体长，第一次膨胀后障碍物O′_k的圆心不变，半径为R′_Ok＝R_Ok+D1，将第一次膨胀后的障碍物表示在对应的栅格地图中，有障碍物的栅格障碍属性

记为1，没有障碍物的栅格障碍属性

记为0；

步骤三：根据栅格障碍属性

将栅格地图分为可行区和不可行区，如果栅格障碍属性为0，该栅格属于可行区S₁，否则该栅格属于不可行区S₂；

步骤四：根据障碍物原始位置误差和UUV导航误差计算可行区S₁中栅格

的潜在危险性

(1)对障碍物进行第二次膨胀处理，在第一次膨胀后的障碍物基础上，将障碍物的半径向外扩展宽度D2，第二次膨胀后障碍物O″_k的半径R″_Ok＝R′_Ok+D2，圆心不变，第二次膨胀区域为障碍物的潜在危险区，潜在危险区属于栅格地图中的可行区，

潜在危险区的宽度D2＝αD_obs+βD_nav，其中D_obs为由障碍物原始位置误差引起危险宽度，D_nav为由UUV导航误差引起的危险宽度，α和β为系数，α+β＝1，0≤α≤1，0≤β≤1；

(2)根据潜在危险区宽度D2计算可行区S₁中栅格

的潜在危险性

其中，栅格地图不可行区S₂包括M个独立的禁入区Fbd_t，0≤t≤M，一个禁入区对应一个第一次膨胀后的障碍物O′_k，禁入区边界栅格集合为

(x_E，y_E)为边界栅格的坐标，

为可行区中栅格，其坐标为(x_i，y_j)；

步骤五：根据栅格的潜在危险性

利用逆向A^*算法生成航路。

2.根据权利要求1所述的一种UUV抵近海底作业过程中航路生成方法，其特征在于：所述的由障碍物原始位置误差引起危险宽度D_obs取值为2～20米。

3.根据权利要求1所述的一种UUV抵近海底作业过程中航路生成方法，其特征在于：所述的由UUV导航误差引起的危险宽度D_nav：

其中，b为系数。

4.根据权利要求1所述的一种UUV抵近海底作业过程中航路生成方法，其特征在于：所述的利用逆向A^*算法生成航路的过程中：

以UUV的初始位置(x_s,y_s)作为航路起始点S，以UUV接收的任务目标(x_G,y_G)点作为航路目标点G；

将一个栅格对应一个节点，节点之间的边表示栅格的潜在危险性，用节点n表示栅格I_ij，节点n的潜在危险性

待扩展节点n的最佳性评价函数f(n)为：

f(n)＝g(n)+h(n)

式中，f(n)表示从节点G开始约束通过节点n的一条最优路径的代价；g(n)表示从节点G到节点n的最优路径代价，这个代价等于到目前为止已经产生的最优路径的代价g(p)加上从当前节点p到节点n的边的代价g′(p,n)，具体为：g(n)＝g(p)+g′(p,n)；h(n)表示从节点n到节点S的最小估计代价，从节点n到节点S的直线欧氏距离作为h(n)的最小估计代价；

栅格地图具有8个方向的连通性，从节点p到节点n的边的代价函数g′(p,n)为：

Images