CN104483975A - 一种基于航路点的uuv转弯速度自适应调整方法 - Google Patents

Abstract

本发明公开了一种基于航路点的UUV转弯速度自适应调整方法。从使命文本读取UUV的前一航路点、当前航路点、下一航路点以及规划速度，将规划速度作为速度指令输出；计算出当前航路角；计算当前航路的转弯半径；惯性导航仪实时采集UUV的实际位置；判断UUV是否满足启用转弯速度的条件，如果满足条件，计算并输出转弯速度指令；光纤罗经实时采集UUV的实际航向；判断UUV是否到达当前航路点，如果到达当前航路点，判断使命文本中的航路点是否都已到达，如果都到达，任务结束。本发明可以减少UUV转弯时的航路超调，改善航路跟踪效果。

Description

一种基于航路点的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为：

${\mathrm{\&alpha;}}_k=\left\{\begin{array}{cc}{\tan }^{-1}\left(\frac{\left|\right|u_k\&times;u_{k+1}\left|\right|}{u_k\&CenterDot;u_{k+1}}\right)& \mathrm{if}1<k<N\\ 0& \mathrm{else}\end{array}\right.$

其中，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_k=R_{\max }-(R_{\max }-R_{\min })e^{-{\mathrm{\&alpha;}}_k^2/{\mathrm{\&sigma;}}_R^2}$

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

3、判断UUV是否满足启用转弯速度的条件为：

$\sqrt{{|x_k-x_{\mathrm{uuv}}|}^2+{|y_k-y_{\mathrm{uuv}}|}^2}<R_k.$

4、转弯速度指令v_{cmd_k}为：

$v_{\mathrm{cmd}\_k}=v_{\min }+(v_{\max }-v_{\min })e^{-{\mathrm{\&alpha;}}_k^2/{\mathrm{\&sigma;}}_v^2}$

其中，v_max为UUV转弯速度指令的最大限值，v_min为UUV转弯速度指令的最小限值，σ_v为速度因子。

5、判断UUV是否到达当前航路点的条件为：

$\left\{\begin{array}{ccc}|{\mathrm{\&psi;}}_{\mathrm{uuv}}-{\tan }^{-1}\left(\frac{y_{k+1}-y_k}{x_{k+1}-x_k}\right)|<\mathrm{\&Psi;}& \mathrm{if}& 1\&le;k<N\\ \sqrt{{|x_k-x_{\mathrm{uuv}}|}^2+{|y_k-y_{\mathrm{uuv}}|}^2}<R_k& \mathrm{else}& k=N\end{array}\right.$

其中，Ψ为航向偏差阈值。

本发明的有益效果：

可以使UUV在航行转弯时自适应的采取合理的转弯半径，并进行速度调整以降低转弯速度，避免了UUV急转弯的出现。

通过对UUV转弯速度的自适应调整，可以消除UUV转弯时的航路超调，减少了能耗并且可以获得更好的航路跟踪效果。

计算UUV的转弯半径和转弯速度时，只用到了航路点的位置信息，所需信息量少，计算简单，易于工程实现。

附图说明

图1为UUV航路点及航路角示意图；

图2为UUV转弯速度自适应调整方法流程图；

图3为UUV转弯半径生成示意图；

图4为UUV转弯速度指令生成示意图；

图5为UUV转弯速度自适应调整***示意图；

图6为未利用本发明的UUV航行转弯效果图；

图7为利用本发明的UUV航行转弯效果图。

具体实施方式

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

结合图1，UUV的航路点和航路角描述如下:

定义UUV当前将要到达的航路点称为当前航路点p_k，其位置信息为(x_k,y_k)；定义前一航路点为p_k-1，其位置信息为(x_k-1,y_k-1)；定义下一航路点为p_k+1，其位置信息为(x_k+1,y_k+1)。航路称为当前航路、航路称为下一航路，和之间的方向角α_k称为当前航路角。

结合图2，介绍UUV转弯速度自适应调整方法：

步骤一：初始化设定R_max、R_min、σ_R、v_max、v_min、σ_v和Ψ；

步骤二：从使命文本读取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：

${\mathrm{\&alpha;}}_k=\left\{\begin{array}{cc}{\tan }^{-1}\left(\frac{\left|\right|u_k\&times;u_{k+1}\left|\right|}{u_k\&CenterDot;u_{k+1}}\right)& \mathrm{if}1<k<N\\ 0& \mathrm{else}\end{array}\right.---\left(1\right)$

其中，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∈[-π,π]。

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

$R_k=R_{\max }-(R_{\max }-R_{\min })e^{-{\mathrm{\&alpha;}}_k^2/{\mathrm{\&sigma;}}_R^2}$

其中，R_k为当前航路的转弯半径，R_max为UUV转弯半径的最大限值，R_min为UUV转弯半径的最小限值，σ_R为半径因子。R_max、R_min和σ_R在初始化时设置。

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

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

判断方法如式(3)所示。

$\sqrt{{|x_k-x_{\mathrm{uuv}}|}^2+{|y_k-y_{\mathrm{uuv}}|}^2}<R_k---\left(3\right)$

如果式(3)满足，则UUV启用转弯速度；

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

$v_{\mathrm{cmd}\_k}=v_{\min }-(v_{\max }-v_{\min })e^{-{\mathrm{\&alpha;}}_k^2/{\mathrm{\&sigma;}}_v^2}---\left(4\right)$

其中，v_{cmd_k}为当前航路的转弯速度指令，v_max为UUV转弯速度指令的最大限值，v_min为UUV转弯速度指令的最小限值，σ_v为速度因子。v_max、v_min和σ_v在初始化时设置。

步骤七：光纤罗经实时采集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到达当前航路点，判断方法如式(5)所示。

$\left\{\begin{array}{ccc}|{\mathrm{\&psi;}}_{\mathrm{uuv}}-{\tan }^{-1}\left(\frac{y_{k+1}-y_k}{x_{k+1}-x_k}\right)|<\mathrm{\&Psi;}& \mathrm{if}& 1\&le;k<N\\ \sqrt{{|x_k-x_{\mathrm{uuv}}|}^2+{|y_k-y_{\mathrm{uuv}}|}^2}<R_k& \mathrm{else}& k=N\end{array}\right.---\left(5\right)$

式中，Ψ为航向偏差阈值，在初始化时设置，一般可取Ψ＜10°。如果式(5)满足，表明UUV的航向已经基本调整到下一航路的航向，即UUV转弯完成，已到达当前航路点，准备向下一航路点航行，转步骤九，如果不满足转步骤七；

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

结合图3介绍UUV的转弯半径生成示意图：

根据式(2)生成UUV当前航路的转弯半径。由式(2)可以知道，UUV的当前航路的转弯半径R_k和R_max、R_min、σ_R以及当前航路角α_k有关。其中，R_max和R_min是固定值，R_min与UUV的固有转弯特性有关，可根据实验确定；R_max一般可取R_min的1.5～2倍；半径因子σ_R一般取0.5～2，可以根据实际需要进行设定。一旦R_max、R_min、σ_R设定，那么UUV的转弯半径将由当前航路角α_k决定。图3给出了R_max＝35m、R_min＝20m，σ_R分别设定为0.5、1和2三种情况下，转弯半径与航路角α_k的关系。从图中可以看出，无论哪种半径因子，当前航路角的绝对值越大，UUV的转弯半径越大，即UUV越应该提前转弯并进行速度调整。

结合图4，介绍UUV的转弯速度指令生成示意图：

根据式(4)生成UUV当前航路的转弯速度指令。由式(4)可以知道，UUV的当前航路的转弯速度指令v_{cmd_k}和v_max、v_min、σ_v以及当前航路角α_k有关。其中，v_max和v_min与UUV的固有航速特性有关，可根据实验确定；速度因子σ_v一般也可取0.5～2，可以根据实际需要进行选择设定。一旦v_max、v_min、σ_v设定，那么UUV的转弯速度指令将由当前航路角α_k决定。图4给出了v_max＝5m/s、v_min＝0.5m/s，σ_v分别设定为0.5、1和2三种情况下，转弯速度指令与航路角α_k的关系。从图中可以看出，无论哪种速度因子，当前航路角的绝对值越大，UUV的转弯速度指令越小，即UUV越应该降低速度进行转弯。

结合图5，介绍UUV转弯速度自适应调整***：

航路角生成模块从使命文本读取航路点信息，并根据UUV前一航路点、当前航路点和下一航路点的位置信息，计算出当前航路角，输出到转弯半径生成模块和速度指令生成模块。

转弯半径生成模块根据当前航路角计算出UUV沿当前航路进行速度调整时的转弯半径，并输出到启用转弯速度模块

惯性导航仪采集UUV当前的位置信息，输出到启用转弯速度模块。

启用转弯速度模块根据UUV当前位置信息和转弯半径判断UUV是否启用转弯速度。如果启用，向速度指令生成模块发送启用转弯速度信号。

光纤罗经采集UUV当前航向信息，输出到停用转弯速度模块。

停用转弯速度模块从使命文本读取航路点信息，并根据UUV的当前航向信息、UUV当前位置信息、当前航路点信息、下一航路点信息和当前航路转弯半径判断UUV是否到达航路点并停用转弯速度，如果停用，向速度指令生成模块发送停用转弯速度信号。

速度指令生成模块如果收到启用转弯速度信号，则根据当前航路角信息计算并输出UUV的转弯速度指令；如果收到停用转弯速度信号，则从使命文本读取规划速度信息并输出规划速度指令。

图6和图7呈现了未利用和利用本发明方法的两种情况下UUV航行转弯效果的对比结果。

其中，图6中呈现的是未利用本发明的UUV航行转弯效果，图7呈现的是利用本发明的UUV航行转弯效果。从对比结果可以明显的看出，采用本发明后，UUV转弯时的航路超调被消除了，UUV的航路跟踪效果更好。另外，UUV的转弯过程更加平滑、转弯效率提高，减少了UUV的能耗。

Claims (6)

1.一种基于航路点的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是否到达当前航路点，如果到达当前航路点，转步骤八，如果没有到达当前航路点，重复步骤六～步骤七；

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

2.根据权利要求1所述的一种基于航路点的UUV转弯速度自适应调整方法，其特征在于：

所述的当前航路角α_k为：

${\mathrm{\&alpha;}}_k=\left\{\begin{array}{ccc}{\tan }^{-1}\left(\frac{\left|\right|u_k\&times;u_{k+1}\left|\right|}{u_k\&CenterDot;u_{k+1}}\right)& \mathrm{if}& 1<k<N\\ 0& \mathrm{else}& \end{array}\right.$

其中，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∈[-π,π]。

3.根据权利要求1所述的一种基于航路点的UUV转弯速度自适应调整方法，其特征在于：

所述的UUV当前航路的转弯半径R_k为：

$R_k=R_{\max }-(R_{\max }-R_{\min })e^{-{\mathrm{\&alpha;}}_k^2/{\mathrm{\&sigma;}}_R^2}$

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

4.根据权利要求1所述的一种基于航路点的UUV转弯速度自适应调整方法，其特征在于：

所述的判断UUV是否满足启用转弯速度的条件为：

$\sqrt{{|x_k-x_{\mathrm{uuv}}|}^2+{|y_k-y_{\mathrm{uuv}}|}^2}<R_k.$

5.根据权利要求1所述的一种基于航路点的UUV转弯速度自适应调整方法，其特征在于：

所述的转弯速度指令v_{cmd_k}为：

$v_{\mathrm{cmd}\_k}=v_{\min }+(v_{\max }-v_{\min })e^{-{\mathrm{\&alpha;}}_k^2/{\mathrm{\&sigma;}}_v^2}$

其中，v_max为UUV转弯速度指令的最大限值，v_min为UUV转弯速度指令的最小限值，σ_v为速度因子。

6.根据权利要求1所述的一种基于航路点的UUV转弯速度自适应调整方法，其特征在于：

所述的判断UUV是否到达当前航路点的条件为：

$\left\{\begin{array}{ccc}|{\mathrm{\&psi;}}_{\mathrm{uuv}}-{\tan }^{-1}\left(\frac{y_{k+1}-y_k}{x_{k+1}-x_k}\right)|<\mathrm{\&Psi;}& \mathrm{if}& 1\&le;k<N\\ \sqrt{{|x_k-x_{\mathrm{uuv}}|}^2+{|y_k-y_{\mathrm{uuv}}|}^2}<R_k& \mathrm{else}& k=N\end{array}\right.$

其中，Ψ为航向偏差阈值。