CN107037821B - 连续冲击载荷下的水下潜器运动姿态估计及控制方法 - Google Patents

Abstract

本发明公开了一种连续冲击载荷下的水下潜器运动姿态估计及控制方法，在本发明中通过分析水下潜器的运动响应情况，基于其六自由度运动方程和动力学方程，结合水下潜器水下状态的平衡方程，建立水下潜器低速定深航行运动仿真模型，考虑连续发射水下航行体时产生的连续冲击载荷，添加发射水下航行体的反冲力和水锤效应模型，对连续冲击载荷下的水下潜器运动姿态进行估计，并建立相应的水下潜器深度与纵倾的PID姿态控制方法。

Description

连续冲击载荷下的水下潜器运动姿态估计及控制方法

技术领域

本发明属于水下运动目标的姿态估计及控制领域，尤其涉及一种连续冲击载荷影响下的水下潜器运动姿态估计及控制技术。

背景技术

作为发射平台的水下潜器必须满足苛刻的平衡状态才能为被发射的水下航行体提供良好的初始状态。但是被发射的水下航行体一旦点火，水下潜器受到剧烈冲击，引起升沉、纵倾、横倾等一系列运动和姿态变化，使其平衡状态遭到破坏，这直接影响所发射的水下航行体的发射状态和精度。因此，如何对连续冲击载荷环境下的水下潜器姿态进行估计和控制，为其所发射的水下航行体提供良好的初始状态，是一个值得研究的问题。

发明内容

本发明通过分析水下潜器的运动响应情况，基于其六自由度运动方程和动力学方程，结合水下潜器水下状态的平衡方程，建立水下潜器低速定深航行运动仿真模型，考虑连续发射水下航行体时产生的连续冲击载荷，添加发射水下航行体的反冲力和水锤效应模型，对连续冲击载荷下的水下潜器运动姿态进行估计，并建立相应的水下潜器深度与纵倾的PID姿态控制方法。

建立固连于大地的地面坐标系ox₀y₀z₀，原点o置于运动时刻潜器浮心在水面上的投影点，ox₀z₀坐标平面与水面重合，y₀轴垂直向上。再建立原点位于浮心的潜器坐标系oxyz，ox轴指向潜器头部，oy轴在潜器纵对称面内与ox轴垂直，oz轴垂直于oxy平面，指向按右手系确定。速度坐标系原点与潜器坐标系原点重合，速度坐标系ox'轴与原点处潜器的速度矢量方向重合，oy'轴位于潜器的纵对称面内，垂直于ox'轴并指向上方，oz'轴与ox'y'z'平面垂直，指向按右手系确定。地面坐标系、速度坐标系和潜器坐标系示意图如附图1所示。

潜器在空间的位置由潜器浮心在地面坐标系内的坐标给出，潜器在空间的姿态由三个欧拉角确定。这三个欧拉角的定义为：将潜器坐标系平移至与其原点与地面坐标系原点重合的位置，再从该位置开始，依次绕oy、oz、ox轴旋转三个角度使得两个坐标系完全重合，我们将三个角度的组合称为欧拉角，并将这三个角度用于表示潜器的空间姿态，分别称为偏航角、俯仰角和横滚角。

由地面系ox₀y₀z₀到潜器系oxyz的转换矩阵为

反之，由潜器系oxyz到地面系ox₀y₀z₀的转换矩阵为的转置，即

潜器运动的攻角与侧滑角由潜器坐标系与速度坐标系之间的相对位置定义。速度轴ox'在潜器纵对称面oxy内的投影与潜器纵轴ox之间的夹角，称为攻角，记为α；速度轴ox'与潜器纵对称面oxy平面的夹角称为侧滑角，记为β。

水下潜器漂浮于水面任一水线或者潜伏于水下保持平衡的充分必要条件为：

(1)水下潜器受到的重力等于所受到的浮力；

(2)重力与浮力的作用线在同一条铅垂线上。

水下潜器漂浮于水面或者潜伏于在水中的姿态称为水下潜器的浮态，对于每一种浮态，上述平衡条件均可表述为平衡方程。本发明基于平滑方程建立水下潜器运动方程模型。用φ、θ分别表示潜器水下状态的横倾和纵倾角，横倾右上为正，纵倾艇艏向上为正。水下潜器处于无横倾、无纵倾(φ＝0,θ＝0)的潜伏状态称为正浮状态，其平衡方程为：

式中Δ↓——水下状态的水下潜器排水量，简称水下排水量；

▽↓——水下状态的水密艇体排体积；

x_G↓,y_G↓——水下状态时的水下潜器重心的G的横纵坐标；

x_B↓,y_B↓——水下状态时的水下潜器浮心的B的横纵坐标；

↓——表示水下状态。

水下潜器处于无横倾(φ＝0)、有纵倾(θ≠0)的潜浮状态称为纵倾状态，其平衡方程为：

式中θ为水下潜器水下状态的纵倾角，首倾为正。

水下潜器处于有横倾(φ≠0)、无纵倾(θ＝0)的潜伏状态称为横倾状态，其平衡方程为：

潜器处于任意状态时，其平衡方程为：

由水下潜器处于水下状态时满足的平衡方程可以看出，水下状态的潜器不具有自行调整平衡的能力，因此，潜器在水下状态时受到的重力和浮力有微小的变化就会导致潜器平衡状态的破坏，并会引起潜器的下潜或者上浮。

根据动量定理和动量矩定理，基于潜器的运动参数与受力之间的关系，得到潜器的水下空间运动方程组为：

式中，

为潜器浮心处的线加速度在三个坐标轴上的分量；

为潜器角加速度在三个坐标轴上的分量；

v_ox,v_oy,v_oz为潜器浮心处的线速度在三个坐标轴上的分量；

ω_x,ω_y,ω_z为潜器角速度在三个坐标轴上的分量；

A_mλ为惯性矩阵，即

其中：

m为潜器质量；

λ_ij,i,j＝1,2,...,6，为附加质量；

x_c,y_c,z_c，为潜器质心位置；

J_i,j,i,j＝x,y,z，为潜器转动惯量、惯性矩；

A_mλ矩阵中的各个参数可根据潜器参数得到，为已知常数量；

A_vω为速度矩阵，即

A_FM为力矩阵，即

其中：

X_B,Y_B,Z_B为浮力在潜器坐标系下的三个坐标轴分量；

X_G,Y_G,Z_G为重力在潜器坐标系下的三个坐标轴分量；

M_Gx,M_Gy,M_Gz为重力矩在潜器坐标系下的三个坐标轴分量；

X_αμ,Y_αμ,Z_αμ为粘性位置力在潜器坐标系下的三个坐标轴分量；

M_αμx,M_αμy,M_αμz为粘性位置力矩在潜器坐标系下的三个坐标轴分量；

Y_ωμ,Z_ωμ为粘性阻尼力在潜器坐标系下的Y和Z坐标轴分量；

M_ωμx,M_ωμy,M_ωμz为粘性阻尼力矩在潜器坐标系下的三个坐标轴分量；

X_T,Y_T,Z_T为推力矢量在潜器坐标系下的三个坐标轴分量；

M_Ty,M_Tz为推力矢量产生的力矩在潜器坐标系下的Y和Z坐标轴分量。

下面分别给出各个力的表达式。

潜器浮力大小为

B＝ρVg (6)

式中，V为潜器的排水体积，ρ为水密度。

浮力作用方向沿地面坐标系的oy₀轴正向，其在潜器坐标系内的三个坐标轴分量为

潜器重力的大小为

G＝mg (8)

m为潜器质量。水下潜器在执行发射水下航行体等特定任务时，一般作无纵倾的等速直线定深的垂直面运动，此时速度v_x＝v₀＝常数，冲角α、纵倾角θ、潜伏角ψ、z向速度v_z等皆为0，即

α＝θ＝ψ＝v_z＝0。当水下潜器执行发射任务时，由于被发射水下航行体的质量较大，对于备用的水下航行体一般设有专门的目标补重水舱来进行替换。当水下航行体发射以后，直接在发射管中注水补重进行替换。注水后还存有少量的质量差值和纵倾力矩，可以采用浮力调整水舱和首尾纵倾调整水舱进行均衡。通过调节浮力调整水舱的注排水量和补偿静载差来对水下潜器姿态进行调整，此时重力G＝mg替换为：

式中m为水下潜器在空气中的质量；ΔP为发射水下航行体时产生的静载差；ΔW_adjust为注排水量；v_adjust为注排水速度；Tp为发射水下航行体的时间点；水下潜器通过调整水舱的注排水量来平衡静载差，注排水量ΔW_adjust满足时间t的线性关系：

ΔW_adjust＝-ΔP(t-Tp)/v_adjust,(Tp≤t≤ΔP/v_adjust+Tp) (10)

重力作用方向沿地面坐标系的oy₀轴负向，其在潜器坐标系下的三个分量为

重力矩在潜器坐标系下的三个分量为

在需要对航行姿态进行估计的有限的时间及空间范围内，粘性阻尼力和粘性位置力可看做一个常数，其数值可根据水下潜器的相关参数进行灵活设定。

在垂直发射水下航行体时，对作为发射平台的水下潜器的平衡造成冲击的主要因素为推力，推力包括两个方面，一方面是水下航行体出筒后点火发射时对水下潜器产生的发射反力，另一方面是水下潜器离筒时海水倒灌产生的振幅不断震荡的水锤压力，这两个方面的力均会使水下潜器产生下沉运动；从作用时间看来前者会产生巨大的瞬时作用力，不会使水下潜器产生后期的位移，而后者是负浮力的叠加，将对水下潜器的位移与姿态产生长期的影响。

当所发射的水下航行体出筒后，点火发射的瞬间会对水下潜器发射位置处的甲板产生强大的发射反冲力，方向沿航行体尾部竖直向下。该力作用时间很短，并随着远离筒口而逐渐减小。

水下潜器垂直发射水下航行体时，筒内的高温高压气体随着水下航行体的完全出水，逐渐从筒内溢出，推动筒口周围的海水向周围运动，由于其运动惯性，筒内的压力会逐渐减小，直到低于所在海域的流体静压，此时海水倒灌，加上流体的重力作用，海水像“水锤”一样砸向筒底，从而形成水锤压力。

在发射水下航行体时刻及后续一定时间内，发射反冲力可通过水下潜器的压力传感器测量得到。压力传感器每隔一定的时间间隔，采集发射反冲力数据。

水锤压力的计算公式为：

P＝ρ·c·v (13)

其中ρ为海水密度，v为发射筒内涌入海水到达发射筒底的水流速度，c为水锤压缩波传播速度，该压力值通过在垂直发射过程中采用流体动力学方法(CFD)进行计算。

在发射水下航行体前，水下潜器的初始位置可通过定位装置测量得到，因此在发射前其运动参数是已知的。发射水下潜器后，在各个离散时刻，基于压力传感器采集的数据得到发射反冲力，根据式(13)计算水锤压力，并将水下潜器的运动方程组进行离散化，采用迭代的方式对每个时刻的各个运动参数进行近似求解，从而得到各个运动参数的估计值。

为使发射水下航行体后的水下潜器处于动力平衡状态，需要操纵升降舵对其姿态进行控制，控制方式采用PID方式来对其深度与纵倾进行控制。在每个离散时刻，根据水下空间运动方程组估计得到当前水下潜器运动参数，并根据运动参数计算其姿态角和倾角变化率，利用水下潜器上的深度计测量该时刻其所处的深度，将计算得到的姿态角和倾角变化率、测量得到的深度与设定的目标姿态角、倾角变化率和深度进行比较，利用PID算法计算达到设定的姿态角和深度所需要的指令控制舵角，具体公式是：

式中：δ_h，δ_v为指令控制舵角，H_d为设定的潜器位置，ψ_d，θ_d为设定的潜器姿态角，H为测量到的潜器深度，ψ,θ为计算得到的潜器姿态角，ω_z和ω_y为倾角变化率，K_h，K_θ，K_ψ，为PID控制参数。指令控制舵角的饱和舵角为30°。潜器姿态角与运动参数之间的表达式为：倾角变化率ω_z和ω_y通过计算倾角θ和ψ的时间变化率得到。根据PID算法计算得出的指令控制舵角，操纵升降舵对水下潜器的姿态进行控制。

综上所示，本发明的具体步骤为：

首先建立表示水下潜器运动参数与受力关系的水下空间运动方程组，水下潜器所受力包括浮力、粘性位置力、粘性阻尼力、重力和推力，其中浮力、粘性位置力和粘性阻尼力为常数，重力通过浮力调整水舱的注排水量来计算得到，推力包括发射反冲力和水锤压力，发射反冲力通过水下潜器的压力传感器测量得到，水锤压力根据公式P＝ρ·c·v，采用流体动力学方法进行计算，其中P为水锤压力，ρ为海水密度，v为发射筒内涌入海水到达发射筒底的水流速度，c为水锤压缩波传播速度；

利用水下空间运动方程组对水下潜器的运动参数进行估计：将水下潜器的运动方程组进行离散化，对微分方程进行近似迭代求解，得到各个运动参数的估计值；

在每个离散时刻，根据当前时刻水下潜器运动参数估计值，计算其潜器姿态角和倾角变化率，利用水下潜器上的深度计测量该时刻其所处的深度H，将计算得到的姿态角、倾角变化率和测量得到的深度与设定的姿态角、倾角变化率和深度进行比较，利用PID算法计算达到设定姿态角和深度所需要的指令控制舵角，具体公式是：

式中：δ_h，δ_v为指令控制舵角，H_d为设定的潜器位置，ψ_d，θ_d为设定的潜器姿态角，H为测量到的潜器深度，ψ,θ为计算得到的潜器姿态角，ω_z和ω_y为倾角变化率，K_h，K_θ，K_ψ，为PID控制参数。

根据PID算法计算得出的指令控制舵角，操纵升降舵对水下潜器的姿态进行控制。

在某参数设置情况下，其PID控制器参数如表1所示。

表1 PID控制器参数

控制器	深度控制器	纵倾控制器	航向控制器
				调节系数	0.3	0.6	0.5
一阶导数调节系数	0.2	0.5	0.2

附图说明

附图1是地面坐标系、速度坐标系及水下潜器坐标系示意图；

附图2是俯仰角曲线估计结果；

附图3是纵倾角曲线估计结果。

具体实施方式

假设2000s时水下航行体发射，重浮力调节水舱开始排水，高压燃气开始涌出，随后海水倒灌，第2001s水下航行体施加动力载荷，发射反力作用于水下潜器，在水锤压力模块和发射反力作用下对水下潜器姿态进行了估计。其中，以俯仰角的估计结果如附图2所示，纵倾角的估计结果如附图3所示。

从图中可以看出在水下潜器发射单发水下航行体瞬间，受发射反力作用发生上下摇摆，随即重浮力调节水舱以及深度控制系统开始工作，在控制系统的调节下，水下潜器在发射单发水下航行体大约1000s时间后，恢复到初始深度，进入正常航行状态。

附图3所示为水下航行体在不同速度情况下，水下潜器纵倾角的变化情况。vsub为水下航行体速度。由图中可以看出，在水下航行体出筒后，水下潜器在水锤压力作用下开始下潜，并在点火发射后，较大的发射反冲力使得水下潜器下倾速度进一步加快，随后水锤压力逐渐衰减，水下潜器受回复力矩以及控制舵角作用，艏倾角开始变小，并在惯性作用下开始产生艉倾，并在长时间内以逐渐衰减的振幅来回震荡，在发射后200s左右全部稳定在一定范围内。与深度变化曲线一致，水下潜器的航速越大，控制效果越好，倾角震荡周期越短，恢复初始状态所需时间越短。

Claims (4)

1.一种连续冲击载荷下的水下潜器运动姿态估计及控制方法，其步骤是：

首先建立表示水下潜器运动参数与受力关系的水下空间运动方程组，水下潜器所受力包括浮力、粘性位置力、粘性阻尼力、重力和推力，其中浮力、粘性位置力和粘性阻尼力为常数，重力通过浮力调整水舱的注排水量来计算得到，推力包括发射反冲力和水锤压力，发射反冲力通过水下潜器的压力传感器测量得到，水锤压力根据公式P＝ρ·c·v，采用流体动力学方法进行计算，其中P为水锤压力，ρ为海水密度，v为发射筒内涌入海水到达发射筒底的水流速度，c为水锤压缩波传播速度；潜器的水下空间运动方程组为：

式中，

为潜器浮心处的线加速度在三个坐标轴上的分量；

为潜器角加速度在三个坐标轴上的分量；

v_ox,v_oy,v_oz为潜器浮心处的线速度在三个坐标轴上的分量；

ω_x,ω_y,ω_z为潜器角速度在三个坐标轴上的分量；

A_mλ为惯性矩阵，即

其中：

m为水下潜器在空气中的质量；

λ_ij,i,j＝1,2,...,6，为附加质量；

x_c,y_c,z_c，为潜器质心位置；

J_i,j,i,j＝x,y,z，为潜器转动惯量、惯性矩；

A_mλ矩阵中的各个参数可根据潜器参数得到，为已知常数量；

A_vω为速度矩阵，即

A_FM为力矩阵，即

其中：

X_B,Y_B,Z_B为浮力在潜器坐标系下的三个坐标轴分量；

X_G,Y_G,Z_G为重力在潜器坐标系下的三个坐标轴分量；

M_Gx,M_Gy,M_Gz为重力矩在潜器坐标系下的三个坐标轴分量；

X_αμ,Y_αμ,Z_αμ为粘性位置力在潜器坐标系下的三个坐标轴分量；

M_αμx,M_αμy,M_αμz为粘性位置力矩在潜器坐标系下的三个坐标轴分量；

Y_ωμ,Z_ωμ为粘性阻尼力在潜器坐标系下的Y和Z坐标轴分量；

M_ωμx,M_ωμy,M_ωμz为粘性阻尼力矩在潜器坐标系下的三个坐标轴分量；

X_T,Y_T,Z_T为推力矢量在潜器坐标系下的三个坐标轴分量；

M_Ty,M_Tz为推力矢量产生的力矩在潜器坐标系下的Y和Z坐标轴分量；

利用水下空间运动方程组对水下潜器的运动参数进行估计：将水下潜器的运动方程组进行离散化，对微分方程进行近似迭代求解，得到各个运动参数的估计值；

在每个离散时刻，根据当前时刻水下潜器运动参数估计值，计算其潜器姿态角和倾角变化率，利用水下潜器上的深度计测量该时刻其所处的深度H，将计算得到的姿态角、倾角变化率和测量得到的深度与设定的姿态角、倾角变化率和深度进行比较，利用PID算法计算达到设定姿态角和深度所需要的指令控制舵角：

根据PID算法计算得出的指令控制舵角，操纵升降舵对水下潜器的姿态进行控制。

2.如权利要求1所述的水下潜器运动姿态估计及控制方法，所述的利用PID算法计算达到设定姿态角和深度所需要的指令控制舵角，其具体公式为：

式中：δ_h，δ_v为指令控制舵角，H_d为设定的潜器位置，ψ_d，θ_d为设定的潜器姿态角，H为测量到的潜器深度，ψ,θ为计算得到的潜器姿态角，ω_z和ω_y为倾角变化率，K_h，K_θ，K_ψ，为PID控制参数。

3.如权利要求1所述的水下潜器运动姿态估计及控制方法，所述的重力通过浮力调整水舱的注排水量来计算得到，其重力G的计算公式为：

式中，m为水下潜器在空气中的质量，ΔP为发射目标时产生的静载差，ΔW_adjust为注排水量，Tp为水下航行体发射的时间点，v_adjust为注排水速度。

4.如权利要求1所述的水下潜器运动姿态估计及控制方法，所述的根据PID算法计算得出的指令控制舵角，该指令控制舵角的饱和舵角为30°。