[发明专利]一种基于航路点的UUV转弯速度自适应调整方法

说明书

技术领域

本发明属于一种UUV速度调整方法，尤其涉及UUV航行转弯时的一种基于航路点的UUV转弯速度自适应调整方法。

背景技术

目前，水下无人航行器(Unmanned Underwater Vehicle—UUV)执行使命时最常用的航行路径是以多个航路点联接而成的直线段路径。当UUV从一个航路转换到另一个航路时就必须要执行转弯动作。目前UUV进行水下航行任务时，接收的使命文本中仅包含航路点的位置信息和规划出的UUV速度信息，使命文本中并没有包含转弯速度信息，所以通常的做法是设定一个固定的转弯半径，当UUV与当前航路点的距离小于等于转弯半径时，UUV开始转弯，但并不对转弯时的速度进行调整，主要是不降速，整个航行过程均采用统一的规划速度。这样的做法存在以下两个问题：(1)固定的转弯半径并不适合所有的转弯角(两个航路之间的航路角)情况；(2)将会导致UUV以较大的速度进行转弯。以上两个问题综合起来将会造成UUV出现急转弯，使转弯效果变差，能耗增加并且产生航路超调。特别地，当转弯角度较大时(直角或钝角度时)，更加明显。

发明内容

本发明的目的是提供一种能够减少转弯航路超调、改善航路跟踪效果的基于航路点的UUV转弯速度自适应调整方法。

本发明是通过以下技术方案实现的：

一种基于航路点的UUV转弯速度自适应调整方法，包括以下几个步骤：

步骤一：从使命文本读取UUV的前一航路点p_k-1的位置(x_k-1,y_k-1)、当前航路点p_k的位置(x_k,y_k)、下一航路点p_k+1的位置(x_k+1,y_k+1)以及规划速度信息，将规划速度信息作为UUV速度指令输出，根据读取的信息计算出当前航路角α_k；

步骤二：根据当前航路角α_k，计算UUV当前航路的转弯半径R_k；

步骤三：惯性导航仪实时采集UUV的实际位置(x_uuv,y_uuv)；

步骤四：根据UUV的实际位置(x_uuv,y_uuv)和当前航路点p_k的位置(x_k,y_k)，判断UUV是否满足启用转弯速度的条件，如果满足条件，进行步骤五，如果不满足条件，重复步骤三～步骤四；

步骤五：根据当前航路角α_k，计算并输出UUV转弯速度指令；

步骤六：光纤罗经实时采集UUV的实际航向ψ_uuv；

步骤七：根据UUV的实际航向ψ_uuv、实际位置(x_uuv,y_uuv)、当前航路点p_k的位置(x_k,y_k)、下一航路点p_k+1的位置(x_k+1,y_k+1)以及当前航路的转弯半径R_k，判断UUV是否到达当前航路点，如果到达当前航路点，转步骤八，如果没有到达当前航路点，重复步骤六～步骤七；

步骤八：判断使命文本中的航路点是否都已到达，如果都到达，任务结束，如果没有，重复步骤一～步骤八。

本发明一种基于航路点的UUV转弯速度自适应调整方法，还可以包括：

1、当前航路角α_k为：

其中，N为航路点的总个数，u_k＝[x_k-x_k-1,y_k-y_k-1]^T，u_k+1＝[(x_k+1-x_k),(y_k+1-y_k)]^T，并且有α_k∈[-π,π]。

2、UUV当前航路的转弯半径R_k为：

其中，R_max为UUV转弯半径的最大限值，R_min为UUV转弯半径的最小限值，σ_R为半径因子。