[发明专利]连续冲击载荷下的水下潜器运动姿态估计及控制方法

摘要

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

主权项

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算法计算得出的指令控制舵角，操纵升降舵对水下潜器的姿态进行控制。