[發明專利]連續沖擊載荷下的水下潛器運動姿態估計及控制方法

有效

• 申請號: 201710332816.8
• 申請日: 2017-05-12
• 公開（公告）號: CN107037821B
• 公開（公告）日: 2019-10-29
• 發明（設計）人: 李智生;閻肖鵬;劉可;王紅萍
• 申請（專利權）人: 中國人民解放軍91550部隊
• 主分類號: G05D1/08
• 分類號: G05D1/08
• 代理公司: 中國人民解放軍海軍專利服務中心 11044
• 代理人: 陳昭男

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算法計算得出的指令控制舵角，操縱升降舵對水下潛器的姿態進行控制。