一种抗浪节能的无人艇路径跟踪方法与流程

本发明涉及水面无人艇路径跟踪领域，具体地说，是一种抗浪节能的无人艇路径跟踪方法。

背景技术：
：

无人水面艇(Unmanned Surface Vehicle,USV)是海洋空间智能无人运载器中的一种，是一种能够进入未知或者危险领域执行任务的装备。无人艇的路径跟踪是执行任务的关键技术，无人艇在路径跟踪过程中易受到波浪干扰，为提高抗干扰性能，现有控制方法多采用较大的控制增益，例如一种基于模糊PID的无人水面艇直线跟踪方法，杨钊，申请号：201410027287.7。但这样的方法会增大能源消耗，缩短续航时间。针对该问题，本发明提出一种抗浪节能的无人艇路径跟踪方法，在保证跟踪精度的前提下降低无人艇的能耗。

技术实现要素：
：

为了实现上述目的，本发明提出一种变参数的水面无人艇路径跟踪控制方法。本发明采取的技术方案是：基于现有的水面无人艇路径跟踪控制方法，增加一个波浪观测模块和一个模糊推理模块；以波浪观测模块检测的浪高和遭遇角作为模糊推理模块输入；模糊推理模块建立浪高、遭遇角与控制器结构参数的关系，动态调节控制器的结构参数，在保证跟踪精度的前提下达到了节能的目的；该方法包括以下步骤：

一、设置无人艇的起点和终点，并计算无人艇的导航角；假设无人艇当前位置为G(X,Y)，终点是一个以R(XR,YR)为圆心，R为半径，方向为顺时针的圆形轨迹；P′为无人水面艇所处位置G与圆心R的连线与圆周的交点；lc为过圆周上点P′的切线；根据圆形可视距离LOS导引律可以计算出导航角φlos：

其中，为无人艇当前位置与终点的距离；φGR为无人艇当前位置和终点的连线与Y方向的夹角；Δ为导航点Plos和切点P′的距离；Δh为无人艇当前位置和切点P′的距离；β为漂角；φusv为无人艇艏向角；

二、利用无人艇搭载的视觉检测功能，根据水面波浪环境具有不同的纹理特性，基于波浪图像的傅里叶变换，通过对不同等级、不同光照条件下波浪图像的频率特性分析，建立一种根据不同矩形环之间能量比的关系来检测波浪浪高和遭遇角；其具体步骤为：

1、对采集的彩色图像进行直方图均衡化，以增强图像特征，然后转换为灰度图像；

2、对步骤1中的灰度图像进行傅里叶变换，离散的傅里叶变换定义为：

其中，f(x,y)是图像的灰度值，x、y为空间域中图像的横、纵坐标轴；F(u,v)是两个实频率变量u和v的复值函数，频率u对应于x轴，频率v对应于y轴，u＝0,1,…,M-1；v＝0,1,…,N-1；傅里叶变换的振幅谱、相位谱和能量谱分别为：

其中，R(u,v)，I(u,v)分别表示F(u,v)的实部和虚部；傅里叶变换提取图像特征的常用方法是周向谱能量法，周向谱能量的计算公式为：

其中，P(r,θ)是P(u,v)的极坐标表示形式，P(u,v)，θ＝arctg(v/u)，即周向谱能量法是计算一系列同心圆环内能量的和值，这种算法得到的傅里叶周向频率分布图并不能完全反映图像的频率特性，而与图像相近的一系列逐渐变大矩形环内的功率谱能量则可以真正反映出图像的频率特性；

3、把步骤2得到的图像功率谱分成I个等宽度的矩形环，能量比分为I级，用Pei表示第i个矩形环内的能量与总能量之比；设图像大小为M×N，图像中心为(M/2，N/2)，则每个矩形环内能量比可以表示为：

式中表示第i个矩形环内能量，u，v取值范围分别为：

其中，m表示u方向的渐变幅度，n表示v方向的渐变幅度，m，n最大值分别为M、N；

4、由步骤3得到的特征量来检测水面的波浪等级；

三、以步骤二检测到的浪高和遭遇角作为模糊推理模块输入，建立浪高、遭遇角与控制器结构参数的关系；

波浪观测模块的输出浪高H的基本论域为(0,0.2,0.4,0.6)，其模糊子集为{Z，PS，PM，PB}；

波浪观测模块的输出遭遇角为ψ，由于顺浪、逆浪时遭遇角对称相等，可将遭遇角转化为同侧并定义遭遇角的论域，模糊子集为{NB，NM，NS，Z，PS，PM，PB}；

模糊推理模块输出kp论域为(0.3，2，4，6，8，10，12)，模糊子集为{NB，NM，NS，Z，PS，PM，PB}；

模糊推理模块输出kd论域为(0.1，1.65，3.3，4.95，6.31，7.96，12)；

模糊推理模块基于以下表1、表2获得kp、kd模糊输出，经过去模糊化处理得到kp、kd的精确值；

表1 Kp模糊推理规则

表2 Kd模糊推理规则

四、系统根据步骤三的输出动态调节PD控制器的参数，并通过计算得到无人艇的左侧推进电机的输入电压Ul和右侧推进电机的输入电压Ur，其Ul和Ur的计算方法为：

其中，kp、kd为模糊推理模块计算得到的值，ω为波浪的圆频率，U＝12V。

附图说明：

附图1是波浪干扰下无人水面艇受力分析图

附图2是传统控制系统结构框图

附图3是本发明控制系统结构框图

附图4是基于LOS导航的圆形路径跟踪原理图

附图5是不规则波干扰下的圆形路径跟踪仿真图

具体实施方式：

为了使本发明实现的技术手段、创作特性、达成目的与功效易于明白了解，下面结合一种实施方案中的双电力推进固定双桨无舵水面无人艇，进一步阐述本发明。

控制对象：

如图1所示为一种实施方案中的无人水面艇的路径跟踪模型原理图，建立无人水面艇波浪干扰环境下的运动模型：

其中，m为无人水面艇的总质量；和分别为航速V在附体坐标系X和Y轴上的加速度；fx、fy分别为艇体所受阻力在X和Y轴上的分量；Fl、Fr分别为左侧和右侧螺旋桨的推力；fl、fr分别为艇体左侧和右侧所受的阻力；J为无人水面艇的转动惯量；ω为无人水面艇转动的角速度；为无人水面艇转动的角加速度；d为无人水面艇的宽度；Cω为转动的阻尼系数常量；为波浪在X轴方向的漂移力；为波浪在Y轴上的漂移力；为波浪作用在在无人艇上的力矩。

系统结构：

如图3所示为基于图1的实施方案中无人艇的控制框图，基于传统的控制方法，比如基于LOS导航规律的PD控制，如图2所示，增加一个波浪观测模块和一个模糊推理模块；以波浪观测模块检测的浪高和遭遇角作为模糊推理模块的输入，通过模糊推理模块建立浪高、遭遇角与控制器结构参数的关系，动态调节控制器的结构参数，当无人艇遇到相对较大的浪时采取较大的增益，遇到较小的浪时采取较小的增益，避免了在现有控制方法中无人船在遇到大浪小浪时都是采取较大增益的问题。

波浪观测模块是利用无人艇搭载的视觉检测功能，根据水面波浪环境具有不同的纹理特性，基于波浪图像的傅里叶变换，通过对不同等级、不同光照条件下波浪图像的频率特性分析，建立一种根据不同矩形环之间能量比的关系来检测波浪浪高和遭遇角；其具体步骤为：

一、对采集的彩色图像进行直方图均衡化，以增强图像特征，然后转换为灰度图像；

二、对步骤一中的灰度图像进行傅里叶变换，离散的傅里叶变换定义为：

其中，f(x,y)是图像的灰度值，x、y为空间域中图像的横、纵坐标轴；F(u,v)是两个实频率变量u和v的复值函数，频率u对应于x轴，频率v对应于y轴，u＝0,1,…,M-1；v＝0,1,…,N-1；傅里叶变换的振幅谱、相位谱和能量谱分别为：

其中，R(u,v)，I(u,v)分别表示F(u,v)的实部和虚部；傅里叶变换提取图像特征的常用方法是周向谱能量法，周向谱能量的计算公式为：

其中，P(r,θ)是P(u,v)的极坐标表示形式，P(u,v)，θ＝arctg(v/u)，即周向谱能量法是计算一系列同心圆环内能量的和值，这种算法得到的傅里叶周向频率分布图并不能完全反映图像的频率特性，而与图像相近的一系列逐渐变大矩形环内的功率谱能量则可以真正反映出图像的频率特性；

三、把步骤二得到的图像功率谱分成I个等宽度的矩形环，能量比分为I级，用Pei表示第i个矩形环内的能量与总能量之比；设图像大小为M×N，图像中心为(M/2，N/2)，则每个矩形环内能量比可以表示为：

式中表示第i个矩形环内能量，u，v取值范围分别为：

其中，m表示u方向的渐变幅度，n表示v方向的渐变幅度，m，n最大值分别为M、N；

四、由步骤三得到的特征量来检测水面的波浪等级；

模糊推理模块包括模糊化模块、模糊推理单元、去模糊化模块；模糊化模块的功能是把浪高和遭遇角作为输入值，然后将基本论域的值映射到模糊论域；模糊推理单元的功能是根据相关经验推理出控制规则；去模糊化模块是根据模糊化方法将模糊化模块的模糊输出值转化为精确值。

导航角φlos的计算：

如图4所示，假设无人艇当前位置为G(X,Y)，终点是一个以R(XR,YR)为圆心，R为半径，方向为顺时针的圆形轨迹；P′为无人水面艇所处位置G与圆心R的连线与圆周的交点；lc为过圆周上点P′的切线；根据圆形可视距离LOS导引律可以计算出导航角φlos：

其中，为无人艇当前位置与终点的距离；φGR为无人艇当前位置和终点的连线与Y方向的夹角；Δ为导航点Plos和切点P′的距离；Δh为无人艇当前位置和切点P′的距离；β为漂角；φusv为无人艇艏向角。

波浪观测模块的输出浪高H的基本论域为(0,0.2,0.4,0.6)，其模糊子集为{Z，PS，PM，PB}。

波浪观测模块的输出遭遇角为ψ，由于顺浪、逆浪时遭遇角对称相等，可将遭遇角转化为同侧并定义遭遇角的论域，模糊子集为{NB，NM，NS，Z，PS，PM，PB}。

模糊推理模块输出kp论域为(0.3，2，4，6，8，10，12)，模糊子集为{NB，NM，NS，Z，PS，PM，PB}。

模糊推理模块输出kd论域为(0.1，1.65，3.3，4.95，6.31，7.96，12)。

模糊推理模块基于以下表1、表2获得kp、kd模糊输出，经过去模糊化处理得到kp、kd的精确值。

表1 Kp模糊推理规则

表2 Kd模糊推理规则

系统根据模糊推理模块的输出动态调节PD控制器的参数，并通过计算得到无人艇的左侧推进电机的输入电压Ul和右侧推进电机的输入电压Ur，其Ul和Ur的计算方法为：

其中，kp、kd为模糊推理模块计算得到的值，ω为波浪的圆频率，U＝12V。

在无人艇的路径跟踪过程中，以跟踪误差作为指标来考察无人艇的跟踪性能，以无人艇在跟踪过程中的平均耗能作为指标来考察节能性能。

无人艇进行路径跟踪时记录每一时刻的距离偏差，当无人艇跟踪至目标终点时，累加每个时刻距离偏差绝对值，除以这段时间的总采样点数，得到的数值记为平均误差，平均误差表达式为：

其中i为总采样点数。

无人艇的平均耗能是指无人艇在跟踪过程中每前进一米所消耗的能量。计算方法为：

其中Pa(k-1)为无人艇在k-1时刻的瞬时总功率，UL、Ur为无人艇跟踪过程中每个时刻左右两侧电机的电压，IL、Ir为与之相对应的电流值，Wa(k)为无人艇在k时刻消耗的总量，为仿真环境中相邻采样时间间隔，Δt取值为0.01s，Wp为无人艇每前进一米消耗的能量，S为无人艇驶过程过程前进的总路程。

仿真实验：

为了验证本发明方法的有效性，将本发明控制方法和现有的基于LOS导航的固定PD参数的控制方法比较。

仿真的对象为双电力推进固定双桨无舵水面无人艇，其数学模型如前公式(1)所述。在仿真实验中，无人艇所做的运动为回转运动，回转半径为20m，回转时左右电压分别为14V、2V，起点为(5，30)，起始航向为0°，波浪方向恒定90°，这是仿真实验的约束条件。

将不规则波的浪高和作用周期均值分别设为0.1m、0.2m、0.3m、0.4m变化幅度10％和3s、5s、7s变化幅度10％的随机波浪。仿真结果如图5(a)、5(b)、5(c)、5(d)所示。本文控制方法和基于LOS导航的固定PD参数控制方法的跟踪误差和能量消耗对比分别见表3、表4。

表3跟踪圆形路径一圈时不同浪高下平均跟踪误差

表4跟踪圆形路径一圈时不同浪高下平均能耗

由仿真结果图5和表3可以看出，在浪高等级比较小0.1m，0.2m时，本发明所提出的变参数控制方法和现有的基于LOS导航的固定PD参数控制方法的两种跟踪误差都比较接近，基于LOS导航的固定PD控制方法略好于本发明控制方法；但是随着浪高等级的增大在0.3m、0.4m时，本发明控制方法能够快速地响应，这在浪高较大的情况下效果更为明显。从跟踪精度上来看，低海况下，基于LOS导航的固定PD参数效果好；高海况下，本发明方法好，各有优势。下面，从能耗的角度来讨论两种控制方法的差异。

由表4可以看出，本发明所提控制方法在同样浪高条件下能耗始终小于基于LOS导航的固定PD参数控制方法，随着浪高的增大，效果越明显，在浪高0.4m时，本文控制方法单位能耗降低30％左右。

综上所述，本发明所提方法能在保证跟踪精度的同时，有效的降低无人艇跟踪能耗，提高无人艇的续航能力，具有实际意义。