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

本发明属于水下无人航行器(unmannedunderwatervehicle，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的初始位置(xs,ys)，航行速度v0，航行时间t0；uuv接收任务目标点(xg,yg)，uuv接收障碍物信息，其中uuv接收到的第k个障碍物ok为以cok为圆心，以rok为半径的圆，障碍物圆心坐标为0≤k≤m；m为障碍物数量；

步骤二：构建栅格地图，栅格iij的坐标为(xi,yj)；根据uuv的几何约束对障碍物进行第一次膨胀处理，膨胀宽度d1＝l，l为uuv体长，第一次膨胀后障碍物o′k的圆心不变，半径为r′ok＝rok+d1，将第一次膨胀后的障碍物表示在相应的栅格地图中，有障碍物的栅格障碍属性记为1，没有障碍物的栅格障碍属性记为0；

步骤三：根据栅格障碍属性将栅格地图分为可行区和不可行区，如果栅格障碍属性为0，该栅格属于可行区s1，否则该栅格属于不可行区s2；

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

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

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

步骤五：根据栅格的潜在危险性利用逆向a^*算法生成航路。

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

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

2、由uuv导航误差引起的危险宽度dnav：

其中，b为系数。

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

以uuv的初始位置(xs,ys)作为航路起始点s，以uuv接收的任务目标(xg,yg)点作为航路目标点g；

将一个栅格对应一个节点，节点之间的边表示栅格的潜在危险性，用节点n表示栅格iij，节点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的初始位置(xs,ys)；uuv接收任务目标点(xg,yg)，uuv接收障碍物信息；m为障碍物数量；

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

任一个栅格iij通过一个四元组表示(xi,yj)为栅格的位置坐标，为栅格的障碍属性，为栅格的潜在危险性。

根据uuv的几何约束对障碍物进行第一次膨胀处理。如图2所示，一般障碍物为不规则形状，以一个半径为rok的外切圆去包络该障碍物，从而将不规则障碍物扩充为规则的凸障碍。因此，uuv接收到的第k个障碍物ok为以cok为圆心，以rok为半径的圆，障碍物圆心坐标为0≤k≤m。考虑到uuv的几何约束，对障碍物进行第一次膨胀处理，膨胀宽度d1＝l，即将包络障碍物的圆的半径延长d1，第一次膨胀后障碍物o′k的半径r′ok＝rok+d1，圆心不变，如图3所示。

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

步骤三：根据每个栅格障碍属性将栅格地图分为可行区和不可行区，如果栅格障碍属性为0，该栅格属于可行区s1，否则该栅格属于不可行区s2，如图4所示。

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

步骤四：根据障碍物原始位置误差和uuv导航误差计算可行区s1中每一栅格iij的潜在危险性具体过程为：

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

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

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

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

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

潜在危险区的宽度d2＝αdobs+βdnav，其中dobs为由障碍物原始位置误差产生危险宽度，dnav为由uuv导航误差转化为障碍物相对位置误差产生的危险宽度，α和β为系数，α+β＝1，0≤α≤1，0≤β≤1；第二次膨胀后障碍物o″k的半径r″ok＝r′ok+d2，圆心不变，如图5所示，其中α＝0.37，β＝0.63。

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

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

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

(2)计算可行区s1中每一栅格(xi,yj)的潜在危险性

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

其中(xη,yη)为圆域ω圆心坐标。

定义p(r)为cωok分布在以η＝e(cωok)为圆心，以r为半径的圆dr内的概率，则有：

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

栅格地图不可行区s2包括m个独立的禁入区fbdk，0≤k≤m。一个禁入区对应一个第一次膨胀后的障碍物o′k，每个禁入区边界栅格集合为可行区s1中任一栅格的潜在危险性为：

其中，(xe，ye)为任一边界栅格的坐标，为可行区中一个栅格，其坐标为(xi，yj)。

因此，障碍物的潜在危险区可以看作是第二次膨胀后障碍物o″k的圆心cok以一定的概率密度分布在以cok为圆心，以d2为半径的一个圆域ω内，进而产生的障碍物的潜在危险区。为栅格受禁入区fbdk威胁的概率。进一步得到，可行区s1中任一栅格的潜在危险性为：

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

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

令uuv的初始位置(xs,ys)作为栅格地图中的起始节点s，令uuv接收的任务目标(xg,yg)点作为栅格地图中的目标节点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，则扩展，并生成其后继节点集msn＝{mi}。

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

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

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

(3)若mi在open表中或closed表中有重复节点k，并且g(mi)＜g(k)，则

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

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

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

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

步骤9.转至步骤2。