本发明涉及一种无人水下航行器的避碰方法。
背景技术：
无人水下航行器是辅助人类完成海洋开发工作的有效手段。由于海洋环境的复杂性、不确定性和非结构性，无人水下航行器航行过程中需要在全局路径规划的基础上，以声呐为主要探测手段对周围的环境信息实时感知，根据具体的局部环境做出合理的动作响应，以规避先验知识以外、全局路径规划未预知的风险和障碍。避碰能力是无人水下航行器智能水平的集中体现，对其实现真正自主具有重要意义。
无人水下航行器常用的避碰方法包括人工势场法、智能计算方法和VFH方法等。人工势场法原理简单、反应迅速，但是存在易陷入局部极小点、在几个障碍物的排斥场相交的区间内运动时易发生振荡等固有缺陷。鉴于此，部分学者提出改进措施，例如，焦鹏等人通过添加两种虚拟力，可脱离“无限大”障碍物与U形障碍物形成的局部最小；朱大奇等人提出一种指数势场函数，可解决传统势场函数引力斥力数值变化过快问题。以神经网络、遗传算法和粒子群优化为代表的智能计算方法为避碰问题提供了在线学习、在线优化的解决方案，但是往往在计算过程中需要较大的存储空间和运行时间，难以满足实时性要求，可能导致无人水下航行器未来得及做出避碰决策就发生碰撞。VFH方法自1991年提出并获得广泛应用以来，一方面有效证明了其实用性，另一方面也逐渐暴露出该方法对阈值敏感、缺乏对机器人运动学及动力学特性的考虑等不足，针对以上缺陷，陆续提出一系列改进的VFH方法：VFH+方法考虑了机器人宽度和轨线，使之能逐步转向预定航向，满足机器人运动学及动力学约束，但该方法只是一种纯局部避障算法，容易在多个障碍物中迷失方向；VFH*方法在VFH+方法基础上加入预测机制，即预测以下几个周期机器人的位置和周围环境的关系，在几个可能角度之间进行优化选择，使得机器人能在局部范围找到一个较优运动方向，但是该方法本质上仍是一种局部避障算法，并且，该方法没有考虑到周围障碍物的速度，它选择出的运动方向有可能不是最优的，并且运用该算法时存在执行时间和预测准确性之间的矛盾；VFH#算法首先进行局部环境预测，重新确定静态动态栅格，为后续的最优选择提供准确的参数，该算法不再是静态环境下的避障算法，而是一种局部动态环境下的避障算法，利用该方法，机器人能在局部动态环境下选择较优的行进方向。
技术实现要素：
本发明提出了一种无人水下航行器二维IVFH(Intelligent Vector Field Histogram，智能向量场直方图)避碰方法，使无人水下航行器在航行过程中通过处理传感器数据获得障碍物位置信息后，能够着眼于安全性和快速性，使无人水下航行器具有一定的类人智能，综合障碍物距离、目标点距离、自由栅格百分比和已知视域百分比等因素，决策出合理的避碰动作，即：确定航向、航速指令，以规避障碍物，避免危险。
无人水下航行器IVFH避碰方法，它包括以下步骤：
步骤一、根据障碍物距离、目标点距离、自由栅格百分比和已知视域百分比进行环境态势评估；
步骤二、综合各项归一化的环境态势指标，应用模糊推理，确定避碰航速以及避碰动作幅度，其中，避碰动作幅度体现为单障碍物对各个候选航向安全因子计算公式中的安全系数；
步骤三、计算各个候选航向的安全因子和快捷因子；
步骤四、计算各个候选航向的适应度及行进代价，根据行进代价评选最佳航向作为下一时刻航向指令；
步骤五、输出航向指令、航速指令，驱动无人水下航行器航行。
步骤二具体为：
步骤二一、将障碍物距离的归一化数值目标点距离的归一化数值自由栅格百分比和已知视域百分比按照对速度的影响加权求和，得到综合环境因素对无人水下航行器航速的影响因子：
式中：和分别表示和对航速的影响权重，且满足：
步骤二二、将av模糊化，经过模糊推理及反模糊化，得到航速指令的清晰值vcmd；在此过程中，av和vcmd均采用{VB、B、M、S、VS}，即：{很大、大、中、小、很小}五个模糊集合进行描述；
步骤二三、将障碍物距离的归一化数值目标点距离的归一化数值自由栅格百分比和已知视域百分比按照对避碰动作幅度的影响加权求和，得到综合环境因素对避碰动作幅度的影响因子避碰动作幅度体现为单障碍物对各个候选航向安全因子计算公式中的安全系数σs：
式中，和分别为和对避碰动作幅度的影响权重，且满足：
步骤二四、将模糊化，经过模糊推理及反模糊化，得到安全系数的清晰值σs；在此过程中，和σs均采用{VB、B、M、S、VS}，即{很大、大、中、小、很小}五个模糊集合进行描述。
步骤二二采用的模糊规则为：
1)、如果av是VB，则σs是VB；
2)、如果av是B，则σs是B；
3)、如果av是M，则σs是M；
4)、如果av是S，则σs是S；
5)、如果av是VS，则σs是VS。
步骤二四采用的模糊规则为：
1)、如果是VB，则σs是VB；
2)、如果是B，则σs是B；
3)、如果是M，则σs是M；
4)、如果是S，则σs是S；
5)、如果是VS，则σs是VS。
步骤三中计算各个候选航向的快捷因子的具体方法是：
各个候选航向的快捷因子计算公式为：
式中，Nc表示候选扇区的数目，ψc(i)表示候选航向i，αdest表示目标点相对于无人水下航行器的航向，σr表示快捷系数。
步骤三中计算各个候选航向的安全因子的具体方法是：
障碍物j对候选航向i的安全因子是：
式中，Nobj表示障碍物数目，αobj(j)表示障碍物j相对于无人水下航行器的方向，dobj(j)表示障碍物j相对于无人水下航行器的距离，σs表示安全系数；
全部障碍物对各个候选航向的安全因子是：
。
步骤四中，计算各个候选航向的适应度及行进代价，根据行进代价评选最佳航向作为下一时刻航向指令的具体方法是：
各个候选航向的适应度为：
fa(i)＝fr(i)fs(i),i＝1,2,...,Nc
各个候选航向的行进代价为：
fc(i)＝1-fa(i),i＝1,2,...,Nc
根据行进代价选择航向指令为：
。
本发明的有益效果是：通过比较无人水下航行器各个候选航向的行进代价评选出最佳航向作为下一时刻的航向指令，其中，行进代价与适应度之和为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个边长为wg的栅格构成矩形区域作为栅格地图，描述声呐感知到的障碍物分布情况，该栅格地图随无人水下航行器的运动而间歇移动，并保证无人水下航行器及声呐探测范围位于栅格地图内；以无人水下航行器船体坐标系的原点为中心建立极坐标系，将无人水下航行器可能的航行方向(即平面内的360度范围)划分为Nc个扇区，即共有Nc个候选航向。进一步，无人水下航行器每一步航行过程中，根据障碍物位置信息，通过以下步骤决策出下一时刻的航向指令和航速指令：
步骤一、从障碍物距离、目标点距离、自由栅格百分比和已知视域百分比方面进行环境态势评估。
(1)、计算无人水下航行器到达最近障碍物的距离，并进行归一化：
式中，dobjmin表示无人水下航行器到达最近障碍物的距离，D表示无人水下航行器所装配传感器的最大探测距离。
(2)、计算无人水下航行器到达目标点的距离，并进行归一化：
式中，ddest表示无人水下航行器到达目标点的距离。
(3)、计算栅格地图中自由栅格的百分比：
式中，Nfree表示自由栅格的数目。
(4)、计算声呐探测范围内已知视域的百分比，已知视域定义为先前时刻已经探测过的区域：
式中，Vkn表示声呐探测范围内的已知视域的面积，Vd表示声呐探测范围的面积。
步骤二、综合各项归一化的环境态势指标，应用模糊推理，确定避碰航速以及避碰动作幅度，其中，避碰动作幅度体现为单障碍物对各个候选航向安全因子计算公式中的安全系数σs，如式(10)所示。
(1)、将和按照对速度的影响加权求和，得到综合环境因素对无人水下航行器航速的影响因子：
式中，和分别表示和对无人水下航行器航速的影响权重，满足：
(2)、将av模糊化，经过模糊推理，及反模糊化，得到航速指令的清晰值vcmd。在此过程中，av和vcmd均采用{VB、B、M、S、VS}，即{很大、大、中、小、很小}五个模糊集合进行描述，模糊规则概括为：
1)、IF av is VB,THEN vcmd is VB；
2)、F av is B,THEN vcmd is B；
3)、IF av is M,THEN vcmd is M；
4)、IF av is S,THEN vcmd is S；
5)、IF av is VS,THEN vcmd is VS。
(3)、将和按照对避碰动作幅度的影响加权求和，得到综合环境因素对避碰动作幅度的影响因子
式中，和分别表示和对避碰动作幅度的影响权重，满足：
(4)、将模糊化，经过模糊推理及反模糊化，得到安全系数的清晰值σs。在此过程中，和σs均采用{VB、B、M、S、VS}，即{很大、大、中、小、很小}五个模糊集合进行描述。在此过程中，和σs均采用{VB、B、M、S、VS}，即{很大、大、中、小、很小}五个模糊集合进行描述，模糊规则概括为：
1)、IFis VB,THENσs is VB；
2)、IFis B,THENσs is B；
3)、IFis M,THENσs is M；
4)、IFis S,THENσs is S；
5)、IFis VS,THENσs is VS。
步骤三、计算各个候选航向的安全因子和快捷因子。
(1)、计算各个候选航向的快捷因子：
式中，ψc(i)表示候选航向i，αdest表示目标点相对于无人水下航行器的航向，σr表示快捷系数。
(2)、计算单障碍物对各个候选航向的安全因子，障碍物j对候选航向i的安全因子是：
式中，αobj(j)表示障碍物j相对于无人水下航行器的方向，dobj(j)表示障碍物j相对于无人水下航行器的距离，σs表示安全系数。
(3)、计算全部障碍物对各个候选航向的安全因子：
步骤四、计算各个候选航向的适应度fa(i)及行进代价fc(i)，根据行进代价评选最佳航向作为下一时刻的航向指令：
fa(i)＝fr(i)fs(i),i＝1,2,...,Nc (12)
fc(i)＝1-fa(i),i＝1,2,...,Nc (13)
步骤五、输出航向指令、航速指令，驱动无人水下航行器航行。
具体实施例：
图1，建立地球坐标系OENE、船体坐标系OBxy、栅格地图G和极坐标系P。根据声呐探测到的障碍物信息，统计各个栅格的障碍物可信度(Certainty Value，CV)，将各个栅格标记为自由栅格或者占有栅格。
图2，无人水下航行器每一步航行过程中，根据障碍物分布情况，通过以下步骤决策出下一时刻的航向指令和航速指令：
步骤一：从障碍物距离、目标点距离、自由栅格百分比、已知视域百分比等方面进行环境态势评估。
(1)、计算无人水下航行器到达最近障碍物的距离，并进行归一化：
式中，dobjmin表示无人水下航行器到达最近障碍物的距离，D表示无人水下航行器所装配传感器的最大探测距离。
(2)、计算无人水下航行器到达目标点的距离，并进行归一化：
式中，ddest表示无人水下航行器到达目标点的距离，D表示无人水下航行器所装配传感器的最大感知距离。
(3)、计算栅格地图中自由栅格的百分比：
式中，Nfree表示自由栅格的数目。
(4)、计算声呐探测范围已知视域的百分比，已知视域定义为先前时刻已经探测过的区域：
式中，Vkn表示声呐探测范围内的已知视域，Vd表示声呐探测范围。
步骤二、综合各项归一化的环境态势指标，应用模糊推理，确定避碰航速以及避碰动作幅度，其中，避碰动作幅度体现为单障碍物对各个候选航向安全因子计算公式中的安全系数σs，如式(10)所示。
(1)、将和按照对速度的影响加权求和，得到综合环境因素对无人水下航行器航速的影响因子：
式中，和分别表示和对无人水下航行器航速的影响权重，满足：
(2)、将av模糊化，经过模糊推理，及反模糊化，得到航速指令的清晰值vcmd。在此过程中，av和vcmd均采用{VB、B、M、S、VS}，即{很大、大、中、小、很小}五个模糊集合进行描述，模糊规则概括为：
1)IF av is VB,THEN vcmd is VB；
2)IF av is B,THEN vcmd is B；
3)IF av is M,THEN vcmd is M；
4)IF av is S,THEN vcmd is S；
5)IF av is VS,THEN vcmd is VS。
(3)、将和按照对避碰动作幅度的影响加权求和，得到综合环境因素对避碰动作幅度的影响因子
式中，和分别表示和对避碰动作幅度的影响权重，满足：
(4)、将模糊化，经过模糊推理及反模糊化，得到安全系数的清晰值σs。在此过程中，和σs均采用{VB、B、M、S、VS}，即{很大、大、中、小、很小}五个模糊集合进行描述，模糊规则概括为：
1)IFis VB,THENσs is VB；
2)IFis B,THENσs is B；
3)IFis M,THENσs is M；
4)IFis S,THENσs is S；
5)IFis VS,THENσs is VS。
步骤三：计算各个候选航向的安全因子和快捷因子。
(1)、计算各个候选航向的快捷因子：
式中，ψc(i)表示候选航向i，αdest表示目标点相对于无人水下航行器的航向，σr表示快捷系数。σr越小，则曲线越尖，表明无人水下航行器更倾向于向目标点航行；σr越大，则曲线越钝，表明无人水下航行器在遇到障碍物时，容易调整航向。
(2)、计算单障碍物对各个候选航向的安全因子，障碍物j对候选航向i的安全因子是：
式中，αobj(j)表示障碍物j相对于无人水下航行器的方向，dobj(j)表示障碍物j相对于无人水下航行器的距离，σs表示安全系数。
(3)、计算全部障碍物对各个候选航向的安全因子：
步骤四、计算各个候选航向的适应度fa(i)及行进代价fc(i)，根据行进代价评选最佳航向作为下一时刻的航向指令：
fa(i)＝fr(i)fs(i),i＝1,2,...,Nc (12)
fc(i)＝1-fa(i),i＝1,2,...,Nc (13)
步骤五、输出航向指令、航速指令，驱动无人水下航行器航行。
图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当前航向与避碰角的叠加。