CN106926979B - 一种动力浮标的自守位控制方法 - Google Patents

Abstract

本发明提供一种动力浮标的自守位控制方法，通过对GPS信息的解算，得到浮标当前速度、航向角以及浮标距守位点的距离和方位，根据这些信息分别设计了航向和速度控制器，将两个控制器的输出通过推力分配算法转化为三个推进器的推力。当浮标距守位点距离超出设定值时，可以自动驶回守位点。本发明实现将浮标保持在设定的守位区域内，防止了浮标受海流海浪的影响漂离作业区域。

Description

一种动力浮标的自守位控制方法

技术领域

本发明涉及一种动力浮标的自守位控制方法，属于动力定位领域。

背景技术

目前，在对水下目标进行定位跟踪时，主要采用按一定阵型定点布放声呐浮标，通过水声定位方法对目标进行定位和运动要素解算。由于布放时间的随机性较大，因此现有声呐浮标多为无动力漂流式浮标，这就使得浮标在定点作业时，易受风浪以及水流的影响而偏离目标点，造成对水下目标定位精度差甚至无法定位的后果。在浅海环境使用时，可通过在浮标下方坠锚钩来使浮标稳定在目标位置，但在深海该方法就会失效。因此该发明对通过对浮标加装动力系统，设计了一种可以使浮标自动保持在作业区域的控制方法。

哈尔滨工程大学严浙平等在申请号为201610244341.2的专利中给出了一种自守位声纳定位浮标，并且对该浮标的结构进行了详细阐述，但是没有对自守位浮标的守位控制方法进行描述。该发明在类似结构浮标基础上设计了动力浮标的自守位控制方法，实现了浮标的自守位功能。

发明内容

本发明的目的是针对声呐浮标在海上作业过程中，容易受到海浪和海流的作用使得作业位置发生变化的问题而提供一种动力浮标的自守位控制方法。

本发明的目的是这样实现的：在浮标的主体的底部设置有底推进器、上端对称设置有左推进器和右推进器，浮标顶端设置有GPS，包括如下步骤：

步骤1：由浮标上搭载的GPS获取浮标当前位置经度λ、当前纬度φ、当前速度U、当前速度方向的信息；

步骤2：由浮标当前位置经度λ、纬度φ和设定的守位点经度λ_d、纬度φ_d得出浮标距守位点距离D和守位点相对浮标的方位角

步骤3：将GPS给出的浮标当速度方向转换到-180°～180°范围：

步骤4：根据浮标距守位点距离D计算期望速度U_d：

U_d＝a×(D-D₀)

式中：a为常系数，D₀为机动停止半径；

步骤5：根据浮标距守位点距离D来置位或清零机动标志位flag：

式中：D₀为机动停止半径，D_max为守位区域半径；

步骤6：根据浮标当前速度U、当前速度方向期望速度U_d和守位点相对浮标的方位角得到速度控制器和航向控制器的输入e_U和e_φ：

步骤7：分别计算速度控制器输出F和航向控制器输出N：

F＝k_ue_U

式中：k_u为速度控制器比例控制系数，k_n航向控制器比例控制系数，β为航向阻尼增益系数；

步骤8：根据速度控制器输出F和航向控制器输出N进行推力分配：

if(n_l＜0)n_l＝0,n_r＝-N

if(n_r＜0)n_r＝0,n_l＝N

if(n_l＞m)n_l＝m

if(n_r＞m)n_r＝m

n_b＝d×(n_l+n_r)

式中：n_l、n_r、n_b分别左推进器、右推进器和底推进器的转速，m为推进器的最高转速，d为与底推进器距浮标重心距离有关的系数；

步骤9：根据机动标志位flag的值确定是否进行推力输出：若flag＝1，则控制推进器输出步骤8得到的转速；若flag＝0，则推进器不输出转速，重新进行第一步。

与现有技术相比，本发明的有益效果是：可以在浮标漂离守位区域时，自动控制浮标稳定驶回守位点，提高了声呐浮标的水下目标定位精度，加长了浮标的有效作用时间。具有较强的实用性，且在工程实现中简单易用。

附图说明

图1为本发明所控制浮标的结构示意图；

图2为本发明所提控制方法控制结构图；

图3为本发明所提控制方法控制流程图；

图4为趋近守位点过程浮标位置变化曲线图；

图5为趋近守位点过程浮标航向变化曲线图；

图6为趋近守位点过程浮标速度变化曲线图；

图7为趋近守位点过程推进器的推力变化图。

具体实施方式

下面结合附图与具体实施方式对本发明作进一步详细描述。

本发明通过对GPS信息的解算，得到浮标当前速度、近似航向角以及浮标距守位点的距离和方位，根据这些信息分别设计了航向和速度控制器，将两个控制器的输出通过推力分配算法转化为三个推进器的推力。当浮标距守位点距离超出设定值时，通过控制推进器驱动使浮标自动驶回到守位点。实现将浮标保持在设定的守位区域内，防止了浮标受海流海浪的影响漂离作业区域。

对GPS信息解算如下：

首先由浮标上搭载的GPS传感器获取浮标当前位置经度(λ)纬度(φ)和当前速度(U)和速度方向信息。

1.浮标距守位点距离D和守位点相对浮标的方位角期望速度U_d以及机动标志位取值计算；

由浮标当前位置经度(λ)纬度(φ)信息和设定的守位点经度(λ_d)纬度(φ_d)计算浮标距守位点距离D和守位点相对浮标的方位角可以通过下面公式计算：

根据浮标距守位点距离D计算期望速度U_d＝a×(D-D₀)，其中a为常系数，D₀为机动停止半径，两个参数可通过实验确定最佳取值。

根据浮标距守位点距离D来置位或清零机动标志位：

其中，D₀为机动停止半径，D_max为守位区域半径。

2、求速度控制器和航向控制器的输入e_U和

将GPS给出的浮标当速度方向转换到(-180°～180°)范围：

求速度控制器和航向控制器的输入e_U和公式如下：

e_U＝U_d-U

3、速度控制器和航向控制器的设计。

在浮标守位过程中对速度精度要求较低，只要满足在趋近守位点的过程速度逐渐降低，以防止反向超出机动停止区，导致浮标不断往复机动。因此速度控制器采用简单的比例控制方法，其控制律为：

F＝k_ue_U

其中：k_u为速度控制器比例控制系数。

由于浮标长宽比较小，从而导致浮标的航向稳定性较差。航向控制器通过加入航向偏差角的微分项，提高了浮标在机动过程中的偏航阻尼，提高了航行稳定性。航向控制器控制律为：

其中：k_n航向控制器比例控制系数，β为航向阻尼增益系数。

4、推力分配设计

根据速度控制器输出F和航向控制器输出N进行推力分配，分配原则为各推进器转速保证在0到最大转速之间，航向调节优于航速调节，分配的具体方法如下：

if(n_l＜0)n_l＝0,n_r＝-N

if(n_r＜0)n_r＝0,n_l＝N

if(n_l＞m)n_l＝m

if(n_r＞m)n_r＝m

n_b＝d×(n_l+n_r)

其中：n_l、n_r、n_b分别为左推进器，右推进器和底推进器转速；m为推进器的最高转速；d为与底部推进器距浮标重心距离有关的系数。

根据机动允许标志位flag的值确定是否进行推力输出。若flag＝1，则控制推进器输出转速；若flag＝0，则推进器不输出转速。

下面结合附图对本发明进行详细的描述：

图1为动力浮标的结构图。图1中的装置包括：浮标主体；2左推进器；3右推进器；4.底部推进器；5.GPS天线；6.通讯天线；7.天线杆；8.水听器。通过GPS天线解算获取浮标的位置、速度和航向信息；根据通讯天线接收到守位点位置坐标和浮标的位置计算如何驱动浮标运动；通过左右底三个推进器共同推动浮标向前运动，通过左右两个推进器的推力差控制浮标转向运动。

图2为动力浮标自守卫控制方法控制结构图，图3为浮标自守卫控制方法控制流程图

自守位控制原理为，根据浮标上GPS信息和守位点经纬度，解算出浮标当前位置、速度、航向，并根据浮标与守位点相对位置给出浮标期望航向与期望航速。分别由航向控制器和航速控制器对浮标的航向和航速进行控制。依据一定的法则将控制器输出对各推进器推力进行分配。

执行步骤具体如下：

101：GPS传感器获取浮标当前位置经度(λ)纬度(φ)和当前速度(U)和速度方向

102：计算浮标距守位点距离D和守位点相对浮标的方位角期望速度U_d以及机动标志位取值；

浮标距守位点距离

浮标期望航向角

浮标期望速度U_d＝a×(D-D₀)，其中a为常系数，D₀为机动停止半径，两个参数可通过实验确定最佳取值。

103：机动标志位

其中：D₀为机动停止半径，D_max为守位区域半径。

104：求取速度控制器和航向控制器的输入e_U和即期望航速与实际航速的偏差和期望航向与实际航向偏差。

首先将GPS给出的浮标当速度方向转换到(-180°～180°)范围内：

求速度控制器和航向控制器的输入e_U和公式如下：

e _U＝U_d-U

105：根据输入e_U和求取速度控制器和航向控制器输出：

F＝k_ue _U

其中：k_u为速度控制器比例控制系数，k_n航向控制器比例控制系数，β为航向阻尼增益系数。

106：根据速度控制器输出F和航向控制器输出N进行推力分配，由于推进器只能正转，且转速不能超过最大转速，因此推力分配要保证各推进器转速在0到最大转速m之间。在守位过程中航向控制对守位效果影响更大，所以在航向调节与速度调节冲突时，航向调节应该优于优于航速调节，推力分配的具体方法如下：

if(n_l＜0)n_l＝0,n_r＝-N

if(n_r＜0)n_r＝0,n_l＝N

if(n_l＞m)n_l＝m

if(n_r＞m)n_r＝m

n_b＝d×(n_l+n_r)

其中nl，nr，nb，分别为左推进器，右推进器和底推进器转速；m为推进器的最高转速；d为与底部推进器距浮标重心距离有关的系数。

107：根据机动允许标志位flag的值确定是否进行推力输出。若flag＝0，则推进器不输出转速。

108：若flag＝1，则控制推进器输出转速；完成一个时间拍的控制作用后，转回到101开始下一个时间拍对浮标的守位控制。

在仿真平台下，使浮标在北东坐标下的初始位置(50m，50m)、初始速度(0m/s，0/s，0rad/s)守位位置(0m，0m)，漂移速度(0.038m/s，0.034m/s)；

机动停止半径D₀取5米，守位区域半径D_max取50米。常系数a取0.1。速度控制器比例控制系数k_u取5，航向控制器比例控制系数k_n取10，航向阻尼增益系数β取4，推进器的最高转速m取1000；系数d取0.4。

仿真结果如图4-图7，图4显示了浮标位置在漂移力作用下在超出守位区域，然后机动的回到守位点附近的位置变化曲线；图5为航向曲线，由图中可见发生机动后浮标航向逐渐收敛到目标航向；图6为速度曲线，由图中可见开始机动后浮标加速到最高航速，随之浮标逐渐接近守位点，航速也逐渐下降；图7为推进器转速曲线，由图中可见左右推进器差动保持航向，三个推进器共同保持航速，随之浮标逐渐接近守位点，三个推进器转速逐渐下降到零。综合来看该控制方法较好的实现了动力浮标的自守位控制。

Claims (1)

1.一种动力浮标的自守位控制方法，其特征在于：在浮标的主体的底部设置有底推进器、上端对称设置有左推进器和右推进器，浮标顶端设置有GPS，包括如下步骤：

步骤1：由浮标上搭载的GPS获取浮标当前位置经度λ、当前纬度φ、当前速度U、当前速度方向的信息；

步骤2：由浮标当前位置经度λ、纬度φ和设定的守位点经度λ_d、纬度φ_d得出浮标距守位点距离D和守位点相对浮标的方位角

步骤3：将GPS给出的浮标的当前速度方向转换到-180°～180°范围：

步骤4：根据浮标距守位点距离D计算期望速度U_d：

U_d＝a×(D-D₀)

式中：a为常系数，D₀为机动停止半径；

步骤5：根据浮标距守位点距离D来置位或清零机动标志位flag：

式中：D₀为机动停止半径，D_max为守位区域半径；

步骤6：根据浮标当前速度U、当前速度方向期望速度U_d和守位点相对浮标的方位角得到速度控制器和航向控制器的输入e_U和e_φ：

步骤7：分别计算速度控制器输出F和航向控制器输出N：

F＝k_ue_U

式中：k_u为速度控制器比例控制系数，k_n航向控制器比例控制系数，β为航向阻尼增益系数；

步骤8：根据速度控制器输出F和航向控制器输出N进行推力分配：

if(n_l＜0)n_l＝0,n_r＝-N

if(n_r＜0)n_r＝0,n_l＝N

if(n_l＞m)n_l＝m

if(n_r＞m)n_r＝m

n_b＝d×(n_l+n_r)

式中：n_l、n_r、n_b分别左推进器、右推进器和底推进器的转速，m为推进器的最高转速，d为与底推进器距浮标重心距离有关的系数；

步骤9：根据机动标志位flag的值确定是否进行推力输出：若flag＝1，则控制推进器输出步骤8得到的转速；若flag＝0，则推进器不输出转速，重新进行第一步。