CN114966636A - 一种拖曳式测扫声呐高度姿态稳定装置及控制方法 - Google Patents

Abstract

本发明公开了一种拖曳式测扫声呐高度姿态稳定装置及控制方法，包括主体支撑架，主体支撑架的下端设有用于固定侧扫声呐的连接件；主体支撑架的前部两侧对称设有两个前推进器，后部两侧对称设有两个后推进器；两个前推进器的朝向为向前45度向下，两个后推进器的朝向为竖直方向；主体支撑架的前端设有拖曳缆，主体支撑架的中部设有数据交换仓；数据交换仓分别与侧扫声呐、两个前推进器和两个后推进器电连接；拖曳缆的一端连接至数据交换舱，另一端与船上的上位机连接；上位机用于接收数据交换舱的信息数据，并输出每个推进器的电机转速信号和拖曳绳的拉动信号。本发明能够根据环境和船速的变化实现高自主性运行，将侧扫声呐维持在其最佳工作高度。

Description

一种拖曳式测扫声呐高度姿态稳定装置及控制方法

技术领域

本发明属于水下探测与成像领域，尤其是涉及一种拖曳式测扫声呐高度姿态稳定装置及控制方法。

背景技术

侧扫声呐能够得到水下微地貌形态和分布。其基本工作原理与雷达相似，只是将雷达所发射的电磁波信号改成能在水下传播的声波，侧扫声呐左右各安装一条换能器线阵，发射一个短促的声脉冲，声波按球面波方式向外传播，碰到海底或水中物体会产生散射，其中的反向散射波会按原传播路线返回换能器被换能器接收，经换能器转换成一系列电脉冲。一般情况下，硬的、粗糙的、凸起的海底回波强；软的、平滑的、凹陷的海底回拨弱，被遮挡的海底不产生回波，距离越远回波越弱。

将每一发射周期的接收数据一线接一线地纵向排列，显示在显示器上，就构成了二维海底地貌声图，声图平面和海底平面成逐点映射关系，声图的亮度也包含了海底的特征，回波的赋值对应各点的亮度。随着水下声呐载体的不断移动，声呐阵在前进过程中不断发射、接收处理，记录逐行排列，在显示器上每一行扫描线上逐行显示出每次发射返回的回波数据。侧扫声呐具有精度高、距离远、分辨率高、覆盖能力强等优点，侧扫声呐在海洋测绘、海洋地质调查，海底打捞救援等方面都有重要应用。

如公开号为CN111352115A的中国专利文献公开了一种高分辨测深侧扫声呐，通过平行于航向且设置于电子密封舱两侧的收发合置换能器，可得到垂直于航向的声信号范围内的地貌侧扫脚印，从而得到地貌侧扫成像；通过垂直于航向且设置于电子密封舱艏部的接收换能器阵，得到横向排布的接收脚印，接收脚印与地貌侧扫脚印的重叠部分处理后可得到地形测深成像；通过地貌侧扫成像，根据亮度与阴影信息，判断是否为目标物；通过地形测深信息，根据其高度信息，结合地貌侧扫成像中的亮度与阴影信息，可有效判断目标形状大小，进而区分水下小目标与干扰，大大减少了虚警产生的概率，有效提高了水下探测的作业效率。

然而，常规侧扫声呐在水下拖曳工作时，声图的质量会受到侧扫声呐工作高度的影响，若离海底太近则其所探测到的区域会变得很小，若距离太远则声图的分辨率会显著降低，无法精确识别海底的地貌或其他物体。但是，由于海流和船速的影响，侧扫声呐的高度和姿态难以维持，例如当船速突然加大时，拖曳绳会传递一个向前的力，从而会拉着侧扫声呐上升，而打捞或者海底探测时经常需要在同一区域来回掉头跑，船速会随时变化，从而影响声图的质量。

发明内容

为弥补现有拖曳式侧扫声呐在拖曳过程中高度不断变化的问题，本发明提供了一种拖曳式测扫声呐高度姿态稳定装置，能够根据环境和船速的变化实现高自主性运行，将侧扫声呐维持在其最佳工作高度，调整速度快。

一种拖曳式测扫声呐高度姿态稳定装置，包括主体支撑架，所述主体支撑架的下端设有用于固定侧扫声呐的连接件；所述主体支撑架的前部两侧对称设有两个前推进器，后部两侧对称设有两个后推进器；两个前推进器的朝向为向前45度向下，两个后推进器的朝向为竖直方向；

所述主体支撑架的前端设有拖曳缆，主体支撑架的中部设有数据交换仓；所述的数据交换仓分别与侧扫声呐、两个前推进器和两个后推进器电连接；所述拖曳缆的一端连接至数据交换舱，另一端与船上的上位机连接；所述的上位机用于接收数据交换舱的信息数据，并输出每个推进器的电机转速信号和拖曳绳的拉动信号。

优选地，所述的连接件设有两个，每个连接件包括相互匹配的第一箍带、第二箍带和螺栓，其中，第一箍带与主体支撑架的下端焊接固定。

优选地，所述的主体支撑架为对称结构，通过316不锈钢焊接而成。

优选地，所述的数据交换舱为密封防水结构，包括舱主体以及通过O型圈与舱主体两端密封的端盖，舱主体采用钛合金材料。

进一步地，数据交换舱的内部设有推进器驱动模块、电流读取模块、惯性传感器IMU、数据整合模块；其中，所述的推进器驱动模块用于根据上位机的信号来驱动每个推进器的电机转速；所述的电流读取模块通过读取推进器的电流判断推进器的电机转速；所述的惯性传感器IMU用于测量整个装置的速度和加速度；所述的数据整合模块用于将侧扫声呐的声图信息、推进器的转速信息、惯性传感器IMU的姿态加速度信息通过拖曳缆传输至上位机。

进一步地，所述的侧扫声呐上带有高度计。

本发明还提供了一种拖曳式测扫声呐高度姿态稳定装置的控制方法，包括如下步骤：

建立整个装置的物理模型，数据交换仓将侧扫声呐的高度、加速度和速度通过拖曳缆传输至上位机，上位机结合船上测到的船速信息，利用控制算法对建立的物理模型实现控制，输出每个推进器的电机转速信号和拖曳缆的拉动信号，以实现对侧扫声呐的高度稳定，从而保证其声图质量。

进一步地，所述的物理模型为水动力模型，包括装置在惯性坐标系下的三维位置、装置在惯性坐标系下的六维速度、质量矩阵、整个装置受到的驱动力和运动的关系映射；具体如下：

选取惯性坐标系E-x_ey_ez_e和载体坐标系B-x_ey_ez_e；惯性坐标系的原点为地球上任意一点，Ex_e轴指向正北方向，Ey_e轴指向正东方向，Ez_e轴方向按右手螺旋规则确定；载体坐标系固定在装置上，其原点为装置的几何中心，bx_b为装置的前进方向，by_b为装置的前进水平面的右方向，bz_b为装置的竖直向下方向；

空间姿态表示为：

x、y、z为装置在惯性坐标系下的三维位置；φ为横滚角，θ为俯仰角，ψ为偏航角；

空间运动表示为

u、v、w分别为装置相对载体系的纵向速度、横向速度和垂直速度；p、q、r为装置相对于载体坐标系的横滚角速度、俯仰角速度和偏航角速度；

质量矩阵为M＝M_RB+M_A，其中，M_RB＝diag(m_x,m_y,m_z,I_x,I_y,I_z)为固有质量，m_x,m_y,m_z为计算xyz方向的运动时三个方向的质量，三个方向的质量相等；I_x,I_y,I_z分别表示角动量公式的转动惯量；M＝Iα，其中，I为转动惯量，α为加速度；M_A＝-diag(X_u,Y_u,Z_u,K_p,M_q,N_r)为附加质量；

定义Γ＝(X,Y,Z,K,M,N)^T，其中，X、Y、Z为整个装置在bx_b、by_b、bz_b分别所受的推力，K、M、N为整个装置绕bx_b、by_b、bz_b轴所受的力矩；

定义整个装置受到的驱动力向量F＝[F₁,F₂,F₃,F₄,F_T]^T，其中F_i代表推进器i产生的推力；i＝1、2、3、4，分别代表的推进器为左前、右前、左后、右后，F_T为拖曳缆的拉力；

将驱动力向量F映射到整个装置所受外力矩和外力，有Γ＝BF；

式中，B为推力分配矩阵，具体为：

其中，x₀、y₀、z₀分别为推进器螺旋桨到y、x、z轴的距离，b为拖曳缆到质心的距离。

所述的控制算法具体为：

将输入设置为侧扫声呐中高度计测得的h、数据交换舱中惯性传感器IMU所测量到的速度和加速度v_x,v_y,v_z,a_x,a_u,a_z；

PID比例项、积分项和微分项的参数分别为：

其中，T表示采样周期，T_i表示积分周期，T_d表示微分周期；此时的PID控制器等效参数为：

对于整个非线性系统有以下公式

BF_推＝PID·M(αa+βv)

其中，通过仿真调节加速度和速度在输入中的占比α,β；其中β的量纲为rad^-1以消除单位对数值的影响，变换后得到以下式子

F_推＝PID·B⁺M(αa+βv)

其中，B⁺表示B的广义逆矩阵，由以上所有公式求出以下推进器的推力和拖曳缆的拉力具体值：

F_推＝[F₁ F₂ F₃ F₄ F_T]。

与现有技术相比，本发明具有以下有益效果：

1、本发明的装置通过设置两个前推进器和两个后推进器，配合拖曳绳可以提供5个自由度，能够使得侧扫声呐在不同的工况下高效率调整自身姿态和高度。

2、本发明针对整个装置建立了动力学模型，上位机能够根据环境和船速的变化，以及侧扫声呐的姿态和加速度等数据实现高自主性运行，控制推进器的转速和拖曳缆的收放，将侧扫声呐维持在其最佳工作高度。

附图说明

图1为本发明一种拖曳式测扫声呐高度姿态稳定装置的结构示意图；

图2为本发明中连接件的示意图；

图3为本发明中数据交换舱的剖视图；

图4为本发明中推进器的结构示意图；

图5为本发明控制方法的控制拓扑图；

图6为本发明控制方法的控制框图。

具体实施方式

下面结合附图和实施例对本发明做进一步详细描述，需要指出的是，以下所述实施例旨在便于对本发明的理解，而对其不起任何限定作用。

如图1所示，一种拖曳式测扫声呐高度姿态稳定装置，包括主体支撑架2，主体支撑架2的下端设有用于固定侧扫声呐6的连接件7。主体支撑架2的前部两侧对称设有两个前推进器4，后部两侧对称设有两个后推进器1。

主体支撑架2的前端设有拖曳缆5，主体支撑架2的中部通过箍带固定有数据交换仓3；数据交换仓3分别与侧扫声呐6、两个前推进器4和两个后推进器1电连接。拖曳缆5的一端连接至数据交换舱3，另一端与船上的上位机连接。上位机通过对所建立的物理模型进行控制，最终通过控制四个推进器的旋转和拖曳绳5的拉动控制侧扫声呐6的高度和姿态。

主体支撑架2为316不锈钢架焊接而成，为轴对称结构，自身具有良好的平衡性，钢架结构的通用性强，便于在其上安装各种各样的部件。

如图2所示，本实施例中，连接件7设有前后两个，每个连接件7包括相互匹配的第一箍带71、第二箍带72和螺栓73，其中，第一箍带71与主体支撑架2的下端焊接固定。第一箍带71和第二箍带72通过螺栓73连接，将侧扫声呐6夹紧，使用的螺栓73长度足够长，当侧扫声呐6的直径比较大时可以用螺栓73将其夹紧，同样可以实现稳定连接。

如图4所示，四个推进器通过专用的连接头16与主体支撑架2的钢结构焊在一起，可通过驱动推进器实现角度转向。四个推进器均可正转反转。两个后推进器1为竖直上下朝向，能够为整个装置提供竖直方向的推力，两个前推进器4朝向为竖直平面内向前旋转45度，提供的推力可以分解到竖直方向和水平方向。推进器的底盘15与专用的连接头16通过底部螺栓17相连，连接头16通过加强筋与主体支撑架2相连，推进器用水下电机13为螺旋桨11提供旋转动力，推进器的导流板14可以提高推进器的推进效率，螺旋桨盖12可以保护内部的螺旋桨11，同时可在焊接时通过调整加强筋的角度以调整螺旋桨的角度。

拖曳缆5是带有电芯的抗拉绳，连接在主体支撑架2上，船航行时通过拖曳缆5拉动整个装置，同时拖曳缆5会提供斜向的拉力，同样可以分解到竖直方向和水平方向，该缆绳配合推进器，可以使侧扫声呐6始终以水平的姿态在需要的高度航行。

如图3所示，数据交换仓3为钛合金制作的耐压舱，包括舱主体33，后端盖31、前端盖34，内部有推进器驱动模块，端盖上连接有水密接插件与推进器相连，侧扫声呐6的数据信号通过水密缆和水密接插件连接至耐压舱内。耐压舱内有电流读取模块，通过读取推进器的电流判断推进器的转速，舱体内带有惯性传感器IMU，可以获取速度和加速度等信息。耐压舱内有数据整合模块，将侧扫声呐的声图信息、推进器的转速信息和姿态加速度等信息通过拖曳缆5传输至船上上位机。舱主体33和端盖之间采用O型圈32径向密封的方式实现水密性。

侧扫声呐6带有高度计和深度计。如图5所示，稳定侧扫声呐拖曳高度的控制方法如下：建立整个装置的物理模型，数据交换仓3主要进行通信中继，将侧扫声呐6的高度、加速度和速度等信息通过拖曳缆传输至上位机，上位机结合船上测到的船速信息，利用控制算法对建立的物理模型实现控制，输出每个推进器的电机转速信号和拖曳缆的拉动信号，以实现对侧扫声呐的高度稳定，从而保证其声图质量。

物理模型为整个装置的水动力模型，包括装置在惯性坐标系下的三维位置，装置在惯性坐标系下的六维速度，质量矩阵，整个装置受到的驱动力和运动的关系映射。具体如下：

选取惯性右手坐标系E-x_ey_ez_e和载体右手坐标系B-x_ey_ez_e。惯性坐标系和地球固定，原点可以选为地球上任意一点，Ex_e轴指向正北方向，Ey_e轴指向正东方向，Ez_e轴方向可以按右手螺旋规则确定；载体坐标系固定在所设计的装置上，其原点为装置的几何中心，bx_b为装置的前进方向，by_b为装置的前进水平面的右方向，bz_b为装置的竖直向下方向。空间姿态表示为：

x、y、z为装置在惯性系下的三维位置；φ、θ、ψ为装置的空间姿态，其中φ为横滚角，θ为俯仰角，ψ为偏航角。空间运动表示为

u、v、w分别为装置相对载体系的纵向速度、横向速度和垂直速度；p、q、r为整个装置相对于载体坐标系的横滚角速度、俯仰角速度和偏航角速度。载体坐标系到惯性坐标系的旋转矩阵表示为

其中，

为速度变换矩阵；

为角速度变换矩阵。

所以转动运动学方程为：

质量矩阵为M＝M_RB+M_A，其中M_RB＝diag(m_x,m_y,m_z,I_x,I_y,I_z)为固有质量；m_x,m_y,m_z用于计算xyz方向的运动(牛顿运动定理f＝ma中的m，此处三个方向的质量均为m，可统一写成m)；I_x,I_y,I_z分别表示角动量公式的转动惯量，M＝Iα，其中，I为转动惯量，α为加速度；M_A＝-diag(X_u,Y_u,Z_u,K_p,M_q,N_r)为附加质量。

定义Γ＝(X,Y,Z,K,M,N)^T，其中X,Y,Z为航行器在bx_b，by_b，bz_b分别所受的推力,K,M,N为航行器绕bx_b，by_b，bz_b轴所受的力矩。

定义整个装置受到的驱动力向量F＝[F₁,F₂,F₃,F₄,F_T]^T,其中F_i代表推进器i产生的推力；i＝1、2、3、4，分别代表的推进器为左前、右前、左后、右后，F_T为绳子的拉力。

将驱动力向量F映射到航行器所受外力矩和外力，有Γ＝BF。

式中，B为推力分配矩阵。根据力的合成和分解易知：

其中，x₀、y₀、z₀分别为推进器螺旋桨到y、x、z轴的距离，b为拖曳缆到质心的距离。

再利用此映射矩阵，依据如图6设计PID算法可以实现对姿态和高度的控制。

现将输入设置为高度计测得的h，IMU所测量到的速度和加速度v_x,v_y,v_z,a_x,a_y,a_z；PID比例项、积分项和微分项的参数分别为：

此时的PID控制器等效参数为：

对于整个非线性系统有以下公式：

BF_推＝PID·M(αa+βv)

其中通过仿真调节加速度和速度在输入中的占比α,β；变换后得到以下式子：

F_推＝PID·B⁺M(αa+βv)

其中，B⁺表示B的广义逆矩阵，由以上所有公式求出以下推进器的推力和拖曳缆的拉力具体值：

F_推＝[F₁ F₂ F₃ F₄ F_T]

以上所述的实施例对本发明的技术方案和有益效果进行了详细说明，应理解的是以上所述仅为本发明的具体实施例，并不用于限制本发明，凡在本发明的原则范围内所做的任何修改、补充和等同替换，均应包含在本发明的保护范围之内。

Claims (9)

1.一种拖曳式测扫声呐高度姿态稳定装置，其特征在于，包括主体支撑架(2)，所述主体支撑架(2)的下端设有用于固定侧扫声呐(6)的连接件(7)；所述主体支撑架(2)的前部两侧对称设有两个前推进器(4)，后部两侧对称设有两个后推进器(1)；两个前推进器(4)的朝向为向前45度向下，两个后推进器(1)的朝向为竖直方向；

所述主体支撑架(2)的前端设有拖曳缆(5)，主体支撑架(2)的中部设有数据交换仓(3)；所述的数据交换仓(3)分别与侧扫声呐(6)、两个前推进器(4)和两个后推进器(1)电连接；所述拖曳缆(5)的一端连接至数据交换舱(3)，另一端与船上的上位机连接；所述的上位机用于接收数据交换舱(3)的信息数据，并输出每个推进器的电机转速信号和拖曳绳(5)的拉动信号。

2.根据权利要求1所述的拖曳式测扫声呐高度姿态稳定装置，其特征在于，所述的连接件(7)设有两个，每个连接件(7)包括相互匹配的第一箍带(71)、第二箍带(72)和螺栓(73)，其中，第一箍带(71)与主体支撑架(2)的下端焊接固定。

3.根据权利要求1所述的拖曳式测扫声呐高度姿态稳定装置，其特征在于，所述的主体支撑架(2)为对称结构，通过316不锈钢焊接而成。

4.根据权利要求1所述的拖曳式测扫声呐高度姿态稳定装置，其特征在于，所述的数据交换舱(3)为密封防水结构，包括舱主体(33)以及通过O型圈(32)与舱主体(33)两端密封的端盖，舱主体(33)采用钛合金材料。

5.根据权利要求1所述的拖曳式测扫声呐高度姿态稳定装置，其特征在于，数据交换舱(3)的内部设有推进器驱动模块、电流读取模块、惯性传感器IMU、数据整合模块；其中，所述的推进器驱动模块用于根据上位机的信号来驱动每个推进器的电机转速；所述的电流读取模块通过读取推进器的电流判断推进器的电机转速；所述的惯性传感器IMU用于测量整个装置的速度和加速度；所述的数据整合模块用于将侧扫声呐的声图信息、推进器的转速信息、惯性传感器IMU的姿态加速度信息通过拖曳缆(5)传输至上位机。

6.根据权利要求1所述的拖曳式测扫声呐高度姿态稳定装置，其特征在于，所述的侧扫声呐(6)上带有高度计。

7.根据权利要求1～6任一所述的拖曳式测扫声呐高度姿态稳定装置的控制方法，其特征在于，包括如下步骤：

建立整个装置的物理模型，数据交换仓(3)将侧扫声呐的高度、加速度和速度通过拖曳缆(5)传输至上位机，上位机结合船上测到的船速信息，利用控制算法对建立的物理模型实现控制，输出每个推进器的电机转速信号和拖曳缆的拉动信号，以实现对侧扫声呐的高度稳定，从而保证其声图质量。

8.根据权利要求7所述的控制方法，其特征在于，所述的物理模型为水动力模型，包括装置在惯性坐标系下的三维位置、装置在惯性坐标系下的六维速度、质量矩阵、整个装置受到的驱动力和运动的关系映射；具体如下：

选取惯性坐标系E-x_ey_ez_e和载体坐标系B-x_ey_ez_e；惯性坐标系的原点为地球上任意一点，Ex_e轴指向正北方向，Ey_e轴指向正东方向，Ez_e轴方向按右手螺旋规则确定；载体坐标系固定在装置上，其原点为装置的几何中心，bx_b为装置的前进方向，by_b为装置的前进水平面的右方向，bz_b为装置的竖直向下方向；

空间姿态表示为：

x、y、z为装置在惯性坐标系下的三维位置；φ为横滚角，θ为俯仰角，ψ为偏航角；

空间运动表示为

u、v、w分别为装置相对载体系的纵向速度、横向速度和垂直速度；p、q、r为装置相对于载体坐标系的横滚角速度、俯仰角速度和偏航角速度；

质量矩阵为M＝M_RB+M_A，其中，M_RB＝diag(m_x，m_y，m_z，I_x，I_y，I_z)为固有质量，m_x，m_y，m_z为计算xyz方向的运动时三个方向的质量，三个方向的质量相等；I_x，I_y，I_z分别表示角动量公式的转动惯量；M＝Iα，其中I为转动惯量，α为加速度；M_A＝-diag(X_u，Y_u，Z_u，K_p，M_q，N_r)为附加质量；

定义Γ＝(X，Y，Z，K，M，N)^T，其中，X、Y、Z为整个装置在bx_b、by_b、bz_b分别所受的推力，K、M、N为整个装置绕bx_b、by_b、bz_b轴所受的力矩；

定义整个装置受到的驱动力向量F＝[F₁，F₂，F₃，F₄，F_T]^T，其中F_i代表推进器i产生的推力；i＝1、2、3、4，分别代表的推进器为左前、右前、左后、右后，F_T为拖曳缆的拉力；

将驱动力向量F映射到整个装置所受外力矩和外力，有Γ＝BF；

式中，B为推力分配矩阵，具体为：

其中，x₀、y₀、z₀分别为推进器螺旋桨到y、x、z轴的距离，b为拖曳缆到质心的距离。

9.根据权利要求8所述的控制方法，其特征在于，所述的控制算法具体为：

将输入设置为侧扫声呐中高度计测得的h、数据交换舱中惯性传感器IMU所测量到的速度和加速度v_x，v_y，v_z，a_x，a_y，a_z；

PID比例项、积分项和微分项的参数分别为：

其中，T表示采样周期，T_i表示积分周期，T_d表示微分周期；此时的PID控制器等效参数为：

对于整个非线性系统有以下公式

BF_推＝PID·M(αa+βv)

其中，通过仿真调节加速度和速度在输入中的占比α，β；其中β的量纲为rad^-1以消除单位对数值的影响，变换后得到以下式子

F_推＝PID·B⁺M(αa+βv)

其中，B⁺表示B的广义逆矩阵，由以上所有公式求出以下推进器的推力和拖曳缆的拉力具体值：

F_推＝[F₁ F₂ F₃ F₄ F_T]。

Images