具体实施方式

本实施方式包括耙吸挖泥船耙管姿态测量显示报警子系统和耙臂姿态控制子系统，共两个子系统，由一套耙臂设备、各类传感器、可编程控制器与计算机组成；耙臂设备包括耙臂和控制耙臂动作的执行机构，耙臂包括上耙管、万向节、下耙管，执行机构包括耙中、耙头绞车；耙臂上安装有检测耙臂角度变化的上耙管加速度陀螺仪1和下耙管加速度陀螺仪2，以及安装有检测上耙管、万向节、下耙管与船体的距离的上耙管超声波传感器3、万向节超声波传感器4以及下耙管超声波传感器5。

耙吸挖泥船耙管姿态测量显示报警子系统主要使用的是上、下耙管加速度陀螺仪、上、下耙管以及万向节超声波传感器、数据采集处理PLC模块、信号电缆、网络交换机、计算机；耙臂姿态控制子系统主要使用的是耙中、耙头绞车、控制反馈PLC模块、信号电缆、网络交换机、计算机；两个子系统共用网络交换机和计算机。

上述各个传感器的安装位置是：

上耙管加速度陀螺仪安装于上耙管水平面处，用来测量上耙管绕X轴的角速度Δα₁(t₁)和绕Z轴的角速度Δβ₁(t₁)；

下耙管加速度陀螺仪安装于下耙管水平面处，用来测量下耙管绕X轴的角速度Δα₂(t₁)和绕Z轴的角速度Δβ₂(t₁)；

上耙管超声波传感器安装于上耙管侧边垂直处，用来测量上耙管与船体的距离D_UP；

万向节超声波传感器安装在下耙管靠近万向节处，用来测量万向节与船体的距离D_UF；

下耙管超声波传感器安装于下耙管侧边垂直处，用来测量下耙管与船体的距离D_LO。

上述各个传感器经过信号电缆与PLC的检测信号输入端连接；数据通过网络交换机将所有采集到的设备信息汇总至计算机里进行数据处理计算，最终输出至计算机的输出模块。

所述计算机包括：测量模块、计算模块、判决模块、告警模块、控制模块和输出模块。其中，所述测量模块负责配置数据采集PLC模块并收集测量数据，提供给计算模块；计算模块根据测量数据计算上耙管与下耙管水平/垂直夹角、上耙管与船体的距离、万向节与船体的距离和下耙管与船体的距离等参数，提供给判决模块；判决模块判断计算模块获得的参数是否在安全范围，决定反馈控制的动作；告警模块根据判决模块的判决结果负责告警信息的生成并通过输出模块显示；同时，控制模块根据判决模块的判决结果，发送控制命令给控制反馈PLC模块，由控制反馈PLC模块控制耙中绞车或耙头绞车完成姿态修正。

所述测量模块收集的测量数据包括：上耙管绕X轴的角速度Δα₁(t₁)和绕Z轴的角速度Δβ₁(t₁)、下耙管绕X轴的角速度Δα₂(t₁)和绕Z轴的角速度Δβ₂(t₁)、上耙管与船体的距离D_UP、万向节与船体的距离D_UF、下耙管与船体的距离D_LO。此为原始测量数据。

上述原始测量数据由计算模块做进一步处理，得到下列参数，计算过程如下：

1)计算上耙管垂直角度和水平角度

通过耙臂的上耙管加速度陀螺仪测得当前时刻t₁的上耙管绕X轴的角速度，即上耙管垂直角速度变化量Δα₁(t₁)，进而计算当前时刻t₁的上耙管垂直角度α₁(t₁)，即：

α₁(t₁)＝α₁(t₀)+Δα₁(t₁)×(t₁-t₀)+ω₁

其中，α₁(t₀)代表前一时刻t₀的上耙管垂直角度；ω₁代表上耙管垂直角度的补偿量，由加速度陀螺仪的安装位置决定。

通过耙臂的上耙管加速度陀螺仪测得当前时刻t₁的上耙管的绕Z轴的角速度，即上耙管水平角速度变化量Δβ₁(t₁)，进而计算当前时刻t₁的上耙管水平角度β₁(t₁)，即：

β₁(t₁)＝β₁(t₀)+Δβ₁×(t₁-t₀)+γ₁

其中，β₁(t₀)代表前一时刻t₀的上耙管水平角度；γ₁代表上耙管水平角度的补偿量，由加速度陀螺仪的安装位置决定。

2)计算下耙管垂直角度和水平角度

同理，通过耙臂的下耙管加速度陀螺仪测得下耙管的绕X轴旋转的角速度，即上耙管垂直角速度变化量Δα₂(t₁)，进而计算当前时刻t₁的下耙管垂直角度α₂(t₁):

α₂(t₁)＝α₂(t₀)+Δα₂(t₁)×(t₁-t₀)+ω₂

其中，α₂(t₀)代表前一时刻t₀的下耙管垂直角度；ω₂代表下耙管垂直角度的补偿量，由加速度陀螺仪的安装位置决定；

通过耙臂的下耙管加速度陀螺仪测得下耙管的绕Z轴旋转的角速度，即上耙管水平角速度变化量Δβ₂(t₁)，进而计算当前时刻的下耙管水平角度β₂(t₁):

β₂(t₁)＝β₂(t₀)+Δβ₂(t₁)×(t₁-t₀)+γ₂

其中，β₂(t₀)代表前一时刻t₀的下耙管水平角度；γ₂代表下耙管水平角度的补偿量，由加速度陀螺仪的安装位置决定；

3)计算上耙管与下耙管垂直夹角和水平夹角

基于1)、2)的结果，计算当前时刻t₁的上耙管与下耙管垂直夹角α_MID(t₁)：

α_MID(t₁)＝α₁(t₁)+α₂(t₁)

其中，α₁(t₁)代表当前时刻t₁的上耙管垂直角度α₁(t₁)，α₂(t₁)代表当前时刻t₁的下耙管垂直角度。

同理，计算当前时刻t₁的上耙管与下耙管水平夹角β_MID(t₁)：

β_MID(t₁)＝β₁(t₁)+β₂(t₁)

其中，β₁(t₁)代表当前时刻t₁的上耙管水平角度，β₂(t₁)代表当前时刻t₁的下耙管水平角度。

4)计算万向节深度

通过上耙管的垂直以及水平角度，计算当前时刻t₁的万向节深度H_UP(t₁)

H_UF(t₁)＝L_UP×cos(β₁(t₁))×sin(α₁(t₁))

其中，L_UP表示上耙管的长度。

5)计算耙头深度

同理，通过上、下耙管的垂直以及水平角度，计算当前时刻t₁的耙头深度H_HEAD(t₁)

H_HEAD(t₁)＝L_UP×cos(β₁(t₁))×sin(α₁(t₁))+L_LO×cos(β₁(t₁)+β₂(t₁))×sin(α₁(t₁)+α₂(t₁))

其中，L_UP表示上耙管的长度，L_LO表示下耙管的长度。

6)计算上耙管距离船体的最短距离

通过超声波传感器测得的距离变化结合上耙管加速度陀螺仪来计算耙臂与船体的最小距离和万向节超声波传感器，计算当前时刻t₁的上耙管距离船体的最短距离：

D_{UP_min}(t₁)＝min{D_UP(t₁),L_UP×sin(β₁(t₁))-D_ELBOW×sgn(D_ELBOW+L_UP×sin(β₁(t₁)))}

其中，D_UP(t₁)表示当前时刻t₁的上耙管超声波传感器测得其所在位置与船体的距离，D_ELBOW表示垂直于吸口处的耙管长度，sgn是符号函数。

7)计算下耙管距离船体的最短距离

同理，通过超声波传感器测得的距离变化结合上耙管加速度陀螺仪来计算耙臂与船体的最小距离和万向节超声波传感器，计算当前时刻t₁的下耙管距离船体的最短距离D_{LO_min}(t₁):

D_{LO_min}(t₁)＝min{D_LO(t₁),L_LO×sin(β₂(t₁))-D_{UP_min}(t₁)×sgn(L_LO×sin(β₂(t₁))-D_{UP_min})}

其中，D_LO(t₁)表示当前时刻t₁的下耙管超声波传感器测得其所在位置与船体的距离。

8)计算耙管距离船体的最短距离

最后，计算当前时刻t₁的耙管距离船体的最短距离D_min(t₁):

D_min(t₁)＝min{D_{UP_min}(t₁),D_UF(t₁),D_{LO_min}(t₁)}

其中，D_UF(t₁)表示当前时刻t₁的万向节超声波传感器测得其所在位置与船体的距离。

计算得到上述参数后，计算机判决模块判断其是否超出安全值范围，并进行以下反馈控制

反馈控制一

计算得到上耙管、万向节以及下耙管位置与船体的距离后，所述判决模块将其与工程安全值进行比较，如果超出了安全值范围，所述控制模块通过网络交换机发送控制命令给控制反馈PLC模块，由控制反馈PLC模块控制耙中绞车和耙头绞车，以修正上耙管、万向节以及下耙管位置；如果在安全值范围内，则不进行反馈控制。如图4所示。具体过程如下：

如果判决模块判定上耙管、万向节或者下耙管位置与船体的距离小于工程安全极小值(设定距离最小值)，则计算机报警模块显示报警，同时，控制模块发送控制命令给控制反馈PLC模块，启动耙中绞车以及耙头绞车，耙中绞车向上提升，耙头绞车向上提升，使上耙管、万向节或者下耙管距离船体的位置增大，直到其位于工程极小值与极大值区间内；

如果判决模块判定上耙管、万向节或者下耙管位置与船体的距离大于工程安全极大值(设定距离最大值)的时候，则计算机报警模块显示报警，同时，控制模块发送控制命令给控制反馈PLC模块，启动耙中绞车以及耙头绞车，耙中绞车向上提升，耙头绞车向上提升，使上耙管、万向节或者下耙管位置与船体的距离减小，直到其位于工程极小值与极大值区间内；

反馈控制二

计算得到上耙管与下耙管的夹角后，所述判决模块将其与工程安全值进行比较，如果超出了安全值范围，所述控制模块通过网络交换机发送控制命令给控制反馈PLC模块，由控制反馈PLC模块控制耙中绞车，以修正上耙管与下耙管夹角；如果在安全值范围内，则不进行反馈控制。如图3所示。具体过程如下：

如果判决模块判定上耙管与下耙管的夹角为负值，且绝对值大于工程安全值(设定夹角角度最大值)的时候，则计算机报警模块显示报警，同时，控制模块发送控制命令给控制反馈PLC模块，启动耙中绞车提升上耙管，使上耙管与下耙管的夹角增大，直到上耙管与下耙管的夹角处于工程安全值区间内。

如果判决模块判定上耙管与下耙管的夹角为正值，且绝对值大于工程安全值(设定夹角角度最大值)的时候，则计算机报警模块显示报警，同时，控制模块发送控制命令给控制反馈PLC模块，启动耙中绞车下放上耙管，使上耙管与下耙管的夹角减小，直到上耙管与下耙管的夹角处于工程安全值区间内。