无人水下航行器IVFH避碰方法

摘要

无人水下航行器IVFH避碰方法，涉及一种无人水下航行器的避碰方法。本发明提出一种无人水下航行器二维IVFH避碰方法，使无人水下航行器在航行过程中通过处理传感器数据获得障碍物位置信息后，能够着眼于安全性和快速性，使无人水下航行器具有一定的类人智能，综合障碍物距离、目标点距离、自由栅格百分比和已知视域百分比等因素，决策出合理的避碰动作，即：确定航向、航速指令，以规避障碍物，避免危险。本发明适用于无人水下航行器的避碰场合。

说明书

技术领域

本发明涉及一种无人水下航行器的避碰方法。

背景技术

无人水下航行器是辅助人类完成海洋开发工作的有效手段。由于海洋环境的复杂 性、不确定性和非结构性，无人水下航行器航行过程中需要在全局路径规划的基础上，以声 呐为主要探测手段对周围的环境信息实时感知，根据具体的局部环境做出合理的动作响 应，以规避先验知识以外、全局路径规划未预知的风险和障碍。避碰能力是无人水下航行器 智能水平的集中体现，对其实现真正自主具有重要意义。

无人水下航行器常用的避碰方法包括人工势场法、智能计算方法和VFH方法等。人 工势场法原理简单、反应迅速，但是存在易陷入局部极小点、在几个障碍物的排斥场相交的 区间内运动时易发生振荡等固有缺陷。鉴于此，部分学者提出改进措施，例如，焦鹏等人通 过添加两种虚拟力，可脱离“无限大”障碍物与U形障碍物形成的局部最小；朱大奇等人提出 一种指数势场函数，可解决传统势场函数引力斥力数值变化过快问题。以神经网络、遗传算 法和粒子群优化为代表的智能计算方法为避碰问题提供了在线学习、在线优化的解决方 案，但是往往在计算过程中需要较大的存储空间和运行时间，难以满足实时性要求，可能导 致无人水下航行器未来得及做出避碰决策就发生碰撞。VFH方法自1991年提出并获得广泛 应用以来，一方面有效证明了其实用性，另一方面也逐渐暴露出该方法对阈值敏感、缺乏对 机器人运动学及动力学特性的考虑等不足，针对以上缺陷，陆续提出一系列改进的VFH方 法：VFH+方法考虑了机器人宽度和轨线，使之能逐步转向预定航向，满足机器人运动学及动 力学约束，但该方法只是一种纯局部避障算法，容易在多个障碍物中迷失方向；VFH*方法在 VFH+方法基础上加入预测机制，即预测以下几个周期机器人的位置和周围环境的关系，在 几个可能角度之间进行优化选择，使得机器人能在局部范围找到一个较优运动方向，但是 该方法本质上仍是一种局部避障算法，并且，该方法没有考虑到周围障碍物的速度，它选择 出的运动方向有可能不是最优的，并且运用该算法时存在执行时间和预测准确性之间的矛 盾；VFH#算法首先进行局部环境预测，重新确定静态动态栅格，为后续的最优选择提供准确 的参数，该算法不再是静态环境下的避障算法，而是一种局部动态环境下的避障算法，利用 该方法，机器人能在局部动态环境下选择较优的行进方向。

发明内容

本发明提出了一种无人水下航行器二维IVFH(IntelligentVectorField Histogram，智能向量场直方图)避碰方法，使无人水下航行器在航行过程中通过处理传感 器数据获得障碍物位置信息后，能够着眼于安全性和快速性，使无人水下航行器具有一定 的类人智能，综合障碍物距离、目标点距离、自由栅格百分比和已知视域百分比等因素，决 策出合理的避碰动作，即：确定航向、航速指令，以规避障碍物，避免危险。

无人水下航行器IVFH避碰方法，它包括以下步骤：

步骤一、根据障碍物距离、目标点距离、自由栅格百分比和已知视域百分比进行环 境态势评估；

步骤二、综合各项归一化的环境态势指标，应用模糊推理，确定避碰航速以及避碰 动作幅度，其中，避碰动作幅度体现为单障碍物对各个候选航向安全因子计算公式中的安 全系数；

步骤三、计算各个候选航向的安全因子和快捷因子；

步骤四、计算各个候选航向的适应度及行进代价，根据行进代价评选最佳航向作 为下一时刻航向指令；

步骤五、输出航向指令、航速指令，驱动无人水下航行器航行。

步骤二具体为：

步骤二一、将障碍物距离的归一化数值目标点距离的归一化数值自由 栅格百分比和已知视域百分比按照对速度的影响加权求和，得到综合环境因素对无 人水下航行器航速的影响因子：

$a_v=w_v^{obj}{\bar{d}}_{obj\min }+w_v^{dest}{\bar{d}}_{dest}+w_v^{free}{\bar{N}}_{free}+w_v^{kn}{\bar{V}}_{kn}$

式中：和分别表示和对航速的影响权 重，且满足：

步骤二二、将a_v模糊化，经过模糊推理及反模糊化，得到航速指令的清晰值v_cmd；在 此过程中，a_v和v_cmd均采用{VB、B、M、S、VS}，即：{很大、大、中、小、很小}五个模糊集合进行描 述；

步骤二三、将障碍物距离的归一化数值目标点距离的归一化数值自由 栅格百分比和已知视域百分比按照对避碰动作幅度的影响加权求和，得到综合环境 因素对避碰动作幅度的影响因子避碰动作幅度体现为单障碍物对各个候选航向安全因 子计算公式中的安全系数σ_s：

$a_{{\sigma }_s}=w_{{\sigma }_s}^{obj}{\bar{d}}_{obj\min }+w_{{\sigma }_s}^{dest}{\bar{d}}_{dest}+w_{{\sigma }_s}^{free}{\bar{N}}_{free}+w_{{\sigma }_s}^{kn}{\bar{V}}_{kn}$

式中，和分别为和对避碰动作幅度的 影响权重，且满足：

步骤二四、将模糊化，经过模糊推理及反模糊化，得到安全系数的清晰值σ_s；在此 过程中，和σ_s均采用{VB、B、M、S、VS}，即{很大、大、中、小、很小}五个模糊集合进行描述。

步骤二二采用的模糊规则为：

1)、如果a_v是VB，则σ_s是VB；

2)、如果a_v是B，则σ_s是B；

3)、如果a_v是M，则σ_s是M；

4)、如果a_v是S，则σ_s是S；

5)、如果a_v是VS，则σ_s是VS。

步骤二四采用的模糊规则为：

1)、如果是VB，则σ_s是VB；

2)、如果是B，则σ_s是B；

3)、如果是M，则σ_s是M；

4)、如果是S，则σ_s是S；

5)、如果是VS，则σ_s是VS。

步骤三中计算各个候选航向的快捷因子的具体方法是：

各个候选航向的快捷因子计算公式为：

$f_r\left(i\right)=e^{-\frac{{\left({\psi }_c\right(i)-{\alpha }_{dest})}^2}{2{{\sigma }_r}^2}},i=1,2,...,N_c$

式中，N_c表示候选扇区的数目，ψ_c(i)表示候选航向i，α_dest表示目标点相对于无人 水下航行器的航向，σ_r表示快捷系数。

步骤三中计算各个候选航向的安全因子的具体方法是：

障碍物j对候选航向i的安全因子是：

$f_{ss}(i,j)=1-e^{-\frac{{\left({\psi }_c\right(i)-{\alpha }_{obj}(j\left)\right)}^2{d}_{obj}{\left(j\right)}^2}{2{{\sigma }_s}^2}},i=1,2...,N_c,j=1,2,...,N_{obj}$

式中，N_obj表示障碍物数目，α_obj(j)表示障碍物j相对于无人水下航行器的方向， d_obj(j)表示障碍物j相对于无人水下航行器的距离，σ_s表示安全系数；

全部障碍物对各个候选航向的安全因子是：

$f_s\left(i\right)=\underset{j=1}{\overset{N_{obj}}{\Pi }}{f}_{ss}(i,j),i=1,2,...,N_c.$

步骤四中，计算各个候选航向的适应度及行进代价，根据行进代价评选最佳航向 作为下一时刻航向指令的具体方法是：

各个候选航向的适应度为：

f_a(i)＝f_r(i)f_s(i),i＝1,2,...,N_c

各个候选航向的行进代价为：

f_c(i)＝1-f_a(i),i＝1,2,...,N_c

根据行进代价选择航向指令为：

${\psi }_{cmd}=\arg \underset{{\psi }_c\left(i\right)}{min}{f}_c\left(i\right),i=1,2,...,N_c.$

本发明的有益效果是：通过比较无人水下航行器各个候选航向的行进代价评选出 最佳航向作为下一时刻的航向指令，其中，行进代价与适应度之和为1，而适应度表现为安 全因子与快捷因子的乘积以兼顾安全性与快速性，由高斯函数直观地表示各个候选航向的 快捷性与安全性，计算简单，易于理解；综合考虑障碍物距离、目标点距离、自由栅格百分比 和已知视域百分比等因素确定无人水下航行器航速指令和避碰动作幅度，以保证无人水下 航行器具有足够的时间充分感知环境信息，使无人水下航行器具有一定的类人智能，在碰 撞障碍物以前有充足的时间做出规避决策并予以响应。

附图说明

图1是IVFH避碰方法的流程图；

图2是IVFH避碰方法所涉及各个坐标系的示意图；

图3为目标点方向为0°，存在单障碍物，方向为0°，距离为80米时，各个候选航向的 快捷因子、安全因子和适应度的仿真示意图；

图4为目标点方向为0°，存在单障碍物，方向为5°，距离为20米时，各个候选航向的 快捷因子、安全因子和适应度的仿真示意图；

图5为目标点方向为0°，存在两个障碍物，方向分别为5°、-20°，距离分别为20米、 30米时，各个候选航向的快捷因子、安全因子和适应度的仿真示意图；

图6显示了某次具体航行过程中障碍物分布情况、UUV的预定航线以及实际航线的 仿真示意图；

图7显示了UUV航行过程中为规避障碍物而产生的避碰角、UUV航向指令和实际航 向的仿真示意图，其中，航向指令是UUV当前航向与避碰角的叠加。

具体实施方式

具体实施方式一、无人水下航行器IVFH避碰方法，本发明实现其目的所采取的技 术方案是：通过比较无人水下航行器各个候选航向的行进代价评选出最佳航向作为下一时 刻的航向指令，其中，行进代价与适应度之和为1，而适应度表现为安全因子与快捷因子的 乘积以兼顾安全性与快速性；综合考虑障碍物距离、目标点距离、自由栅格百分比和已知视 域百分比等因素确定无人水下航行器航速指令、避碰动作幅度，以保证无人水下航行器具 有足够的时间充分感知环境信息，在碰撞障碍物以前有充足的时间做出规避决策并予以响 应。

具体而言，建立M×N个边长为w_g的栅格构成矩形区域作为栅格地图，描述声呐感 知到的障碍物分布情况，该栅格地图随无人水下航行器的运动而间歇移动，并保证无人水 下航行器及声呐探测范围位于栅格地图内；以无人水下航行器船体坐标系的原点为中心建 立极坐标系，将无人水下航行器可能的航行方向(即平面内的360度范围)划分为N_c个扇区， 即共有N_c个候选航向。进一步，无人水下航行器每一步航行过程中，根据障碍物位置信息， 通过以下步骤决策出下一时刻的航向指令和航速指令：

步骤一、从障碍物距离、目标点距离、自由栅格百分比和已知视域百分比方面进行 环境态势评估。

(1)、计算无人水下航行器到达最近障碍物的距离，并进行归一化：

${\bar{d}}_{obj\min }=\frac{d_{obj\min }}{D}---\left(1\right)$

式中，d_objmin表示无人水下航行器到达最近障碍物的距离，D表示无人水下航行器 所装配传感器的最大探测距离。

(2)、计算无人水下航行器到达目标点的距离，并进行归一化：

${\bar{d}}_{dest}=\frac{d_{dest}}{D}---\left(2\right)$

式中，d_dest表示无人水下航行器到达目标点的距离。

(3)、计算栅格地图中自由栅格的百分比：

${\bar{N}}_{free}=\frac{N_{free}}{MN}---\left(3\right)$

式中，N_free表示自由栅格的数目。

(4)、计算声呐探测范围内已知视域的百分比，已知视域定义为先前时刻已经探测 过的区域：

${\bar{V}}_{kn}=\frac{V_{kn}}{V_d}---\left(4\right)$

式中，V_kn表示声呐探测范围内的已知视域的面积，V_d表示声呐探测范围的面积。

步骤二、综合各项归一化的环境态势指标，应用模糊推理，确定避碰航速以及避碰 动作幅度，其中，避碰动作幅度体现为单障碍物对各个候选航向安全因子计算公式中的安 全系数σ_s，如式(10)所示。

(1)、将和按照对速度的影响加权求和，得到综合环境因素 对无人水下航行器航速的影响因子：

$a_v=w_v^{obj}{\bar{d}}_{obj\min }+w_v^{dest}{\bar{d}}_{dest}+w_v^{free}{\bar{N}}_{free}+w_v^{kn}{\bar{V}}_{kn}---\left(5\right)$

式中，和分别表示和对无人水下航行 器航速的影响权重，满足：

$w_v^{obj}+w_v^{dest}+w_v^{free}+w_v^{kn}=1---\left(6\right)$

(2)、将a_v模糊化，经过模糊推理，及反模糊化，得到航速指令的清晰值v_cmd。在此过 程中，a_v和v_cmd均采用{VB、B、M、S、VS}，即{很大、大、中、小、很小}五个模糊集合进行描述，模 糊规则概括为：

1)、IFa_visVB,THENv_cmdisVB；

2)、Fa_visB,THENv_cmdisB；

3)、IFa_visM,THENv_cmdisM；

4)、IFa_visS,THENv_cmdisS；

5)、IFa_visVS,THENv_cmdisVS。

(3)、将和按照对避碰动作幅度的影响加权求和，得到综合 环境因素对避碰动作幅度的影响因子

$a_{{\sigma }_s}=w_{{\sigma }_s}^{obj}{\bar{d}}_{obj\min }+w_{{\sigma }_s}^{dest}{\bar{d}}_{dest}+w_{{\sigma }_s}^{free}{\bar{N}}_{free}+w_{{\sigma }_s}^{kn}{\bar{V}}_{kn}---\left(7\right)$

式中，和分别表示和对避碰动作幅度 的影响权重，满足：

$w_{{\sigma }_s}^{obj}+w_{{\sigma }_s}^{dest}+w_{{\sigma }_s}^{free}+w_{{\sigma }_s}^{kn}=1---\left(8\right)$

(4)、将模糊化，经过模糊推理及反模糊化，得到安全系数的清晰值σ_s。在此过程 中，和σ_s均采用{VB、B、M、S、VS}，即{很大、大、中、小、很小}五个模糊集合进行描述。在此 过程中，和σ_s均采用{VB、B、M、S、VS}，即{很大、大、中、小、很小}五个模糊集合进行描述， 模糊规则概括为：

1)、IFisVB,THENσ_sisVB；

2)、IFisB,THENσ_sisB；

3)、IFisM,THENσ_sisM；

4)、IFisS,THENσ_sisS；

5)、IFisVS,THENσ_sisVS。

步骤三、计算各个候选航向的安全因子和快捷因子。

(1)、计算各个候选航向的快捷因子：

$f_r\left(i\right)=e^{-\frac{{\left({\psi }_c\right(i)-{\alpha }_{dest})}^2}{2{{\sigma }_r}^2}},i=1,2,...,N_c---\left(9\right)$

式中，ψ_c(i)表示候选航向i，α_dest表示目标点相对于无人水下航行器的航向，σ_r表 示快捷系数。

(2)、计算单障碍物对各个候选航向的安全因子，障碍物j对候选航向i的安全因子是：

$f_{ss}(i,j)=1-e^{-\frac{{\left({\psi }_c\right(i)-{\alpha }_{obj}(j\left)\right)}^2{d}_{obj}{\left(j\right)}^2}{2{{\sigma }_s}^2}},i=1,2,...,N_c,j=1,2,...,N_{obj}---\left(10\right)$

式中，α_obj(j)表示障碍物j相对于无人水下航行器的方向，d_obj(j)表示障碍物j相 对于无人水下航行器的距离，σ_s表示安全系数。

(3)、计算全部障碍物对各个候选航向的安全因子：

$f_s\left(i\right)=\underset{j=1}{\overset{N_{obj}}{\Pi }}{f}_{ss}(i,j),i=1,2,...,N_c---\left(11\right)$

步骤四、计算各个候选航向的适应度f_a(i)及行进代价f_c(i)，根据行进代价评选最 佳航向作为下一时刻的航向指令：

f_a(i)＝f_r(i)f_s(i),i＝1,2,...,N_c(12)

f_c(i)＝1-f_a(i),i＝1,2,...,N_c(13)

${\psi }_{cmd}=\arg \underset{{\psi }_c\left(i\right)}{min}{f}_c\left(i\right),i=1,2,...,N_c---\left(14\right)$

步骤五、输出航向指令、航速指令，驱动无人水下航行器航行。

具体实施例：

图1，建立地球坐标系O_ENE、船体坐标系O_Bxy、栅格地图G和极坐标系P。根据声呐探 测到的障碍物信息，统计各个栅格的障碍物可信度(CertaintyValue，CV)，将各个栅格标 记为自由栅格或者占有栅格。

图2，无人水下航行器每一步航行过程中，根据障碍物分布情况，通过以下步骤决 策出下一时刻的航向指令和航速指令：

步骤一：从障碍物距离、目标点距离、自由栅格百分比、已知视域百分比等方面进 行环境态势评估。

(1)、计算无人水下航行器到达最近障碍物的距离，并进行归一化：

${\bar{d}}_{obj\min }=\frac{d_{obj\min }}{D}---\left(1\right)$

式中，d_objmin表示无人水下航行器到达最近障碍物的距离，D表示无人水下航行器 所装配传感器的最大探测距离。

(2)、计算无人水下航行器到达目标点的距离，并进行归一化：

${\bar{d}}_{dest}=\frac{d_{dest}}{D}---\left(2\right)$

式中，d_dest表示无人水下航行器到达目标点的距离，D表示无人水下航行器所装配 传感器的最大感知距离。

(3)、计算栅格地图中自由栅格的百分比：

${\bar{N}}_{free}=\frac{N_{free}}{MN}---\left(3\right)$

式中，N_free表示自由栅格的数目。

(4)、计算声呐探测范围已知视域的百分比，已知视域定义为先前时刻已经探测过 的区域：

${\bar{V}}_{kn}=\frac{V_{kn}}{V_d}---\left(4\right)$

式中，V_kn表示声呐探测范围内的已知视域，V_d表示声呐探测范围。

步骤二、综合各项归一化的环境态势指标，应用模糊推理，确定避碰航速以及避碰 动作幅度，其中，避碰动作幅度体现为单障碍物对各个候选航向安全因子计算公式中的安 全系数σ_s，如式(10)所示。

(1)、将和按照对速度的影响加权求和，得到综合环境因素对 无人水下航行器航速的影响因子：

$a_v=w_v^{obj}{\bar{d}}_{obj\min }+w_v^{dest}{\bar{d}}_{dest}+w_v^{free}{\bar{N}}_{free}+w_v^{kn}{\bar{V}}_{kn}---\left(5\right)$

式中，和分别表示和对无人水下航行 器航速的影响权重，满足：

$w_v^{\mathrm{obj}}+w_v^{\mathrm{dest}}+w_v^{\mathrm{free}}+w_v^{\mathrm{kn}}=1$

(2)、将a_v模糊化，经过模糊推理，及反模糊化，得到航速指令的清晰值v_cmd。在此过 程中，a_v和v_cmd均采用{VB、B、M、S、VS}，即{很大、大、中、小、很小}五个模糊集合进行描述，模 糊规则概括为：

1)IFa_visVB,THENv_cmdisVB；

2)IFa_visB,THENv_cmdisB；

3)IFa_visM,THENv_cmdisM；

4)IFa_visS,THENv_cmdisS；

5)IFa_visVS,THENv_cmdisVS。

(3)、将和按照对避碰动作幅度的影响加权求和，得到综合 环境因素对避碰动作幅度的影响因子

$a_{{\sigma }_s}=w_{{\sigma }_s}^{obj}{\bar{d}}_{obj\min }+w_{{\sigma }_s}^{dest}{\bar{d}}_{dest}+w_{{\sigma }_s}^{free}{\bar{N}}_{free}+w_{{\sigma }_s}^{kn}{\bar{V}}_{kn}---\left(7\right)$

式中，和分别表示和对避碰动作幅度 的影响权重，满足：

$w_{{\sigma }_s}^{obj}+w_{{\sigma }_s}^{dest}+w_{{\sigma }_s}^{free}+w_{{\sigma }_s}^{kn}=1---\left(8\right)$

(4)、将模糊化，经过模糊推理及反模糊化，得到安全系数的清晰值σ_s。在此过 程中，和σ_s均采用{VB、B、M、S、VS}，即{很大、大、中、小、很小}五个模糊集合进行描述，模 糊规则概括为：

1)IFisVB,THENσ_sisVB；

2)IFisB,THENσ_sisB；

3)IFisM,THENσ_sisM；

4)IFisS,THENσ_sisS；

5)IFisVS,THENσ_sisVS。

步骤三：计算各个候选航向的安全因子和快捷因子。

(1)、计算各个候选航向的快捷因子：

$f_r\left(i\right)=e^{-\frac{{\left({\psi }_c\right(i)-{\alpha }_{dest})}^2}{2{{\sigma }_r}^2}},i=1,2,...,N_c---\left(9\right)$

式中，ψ_c(i)表示候选航向i，α_dest表示目标点相对于无人水下航行器的航向，σ_r表 示快捷系数。σ_r越小，则曲线越尖，表明无人水下航行器更倾向于向目标点航行；σ_r越大，则 曲线越钝，表明无人水下航行器在遇到障碍物时，容易调整航向。

(2)、计算单障碍物对各个候选航向的安全因子，障碍物j对候选航向i的安全因子 是：

$f_{ss}(i,j)=1-e^{-\frac{{\left({\psi }_c\right(i)-{\alpha }_{obj}(j\left)\right)}^2{d}_{obj}{\left(j\right)}^2}{2{{\sigma }_s}^2}},i=1,2,...,N_c,j=1,2,...,N_{obj}---\left(10\right)$

式中，α_obj(j)表示障碍物j相对于无人水下航行器的方向，d_obj(j)表示障碍物j相 对于无人水下航行器的距离，σ_s表示安全系数。

(3)、计算全部障碍物对各个候选航向的安全因子：

$f_s\left(i\right)=\underset{j=1}{\overset{N_{obj}}{\Pi }}{f}_{ss}(i,j),i=1,2,...,N_c---\left(11\right)$

步骤四、计算各个候选航向的适应度f_a(i)及行进代价f_c(i)，根据行进代价评选最 佳航向作为下一时刻的航向指令：

f_a(i)＝f_r(i)f_s(i),i＝1,2,...,N_c(12)

f_c(i)＝1-f_a(i),i＝1,2,...,N_c(13)

${\psi }_{cmd}=\arg \underset{{\psi }_c\left(i\right)}{min}{f}_c\left(i\right),i=1,2,...,N_c---\left(14\right)$

步骤五、输出航向指令、航速指令，驱动无人水下航行器航行。

图3、4、5为举例说明，图3给出目标点方向为0°，存在单障碍物，方向为0°，距离为 80米时，各个候选航向的快捷因子、安全因子和适应度；图4给出目标点方向为0°，存在单障 碍物，方向为5°，距离为20米时，各个候选航向的快捷因子、安全因子和适应度；图5给出目 标点方向为0°，存在两个障碍物，方向分别为5°、-20°，距离分别为20米、30米时，各个候选 航向的快捷因子、安全因子和适应度。

下面结合具体实施例对本发明进一步说明。

在北东坐标系中，UUV起点设置为(0,-80)，目标点设置为(300,-80)，在UUV预定航 路中设置一条宽度为200米的阻拦带。UUV每一步航行过程中，通过环境态势评估、参数决 策、计算快捷因子和安全因子、计算适应度和行进代价、指令输出等步骤实现障碍物规避。 图6显示了障碍物分布情况、UUV的预定航线以及实际航线。图7显示了UUV航行过程中为规 避障碍物而产生的避碰角、UUV航向指令和实际航向，其中，航向指令是UUV当前航向与避碰 角的叠加。