[發明專利]一種大地坐標下等效閉環干擾速率補償自穩定控制方法

有效

• 申請號: 202011517103.7
• 申請日: 2020-12-21
• 公開（公告）號: CN112729012B
• 公開（公告）日: 2022-12-23
• 發明（設計）人: 李偉;任海波;韓磊;位紅軍;馬捷
• 申請（專利權）人: 西北機電工程研究所
• 主分類號: F41G5/06
• 分類號: F41G5/06;G05B13/04
• 代理公司: 西北工業大學專利中心 61204
• 代理人: 顧潮琪

1.一種大地坐標下等效閉環干擾速率補償自穩定控制方法，其特征在于包括以下步驟：

(1)設定控制步數i初值為0；設定控制或采樣周期，每個控制或采樣周期步數i加1；

(2)采集車體上SINS的航姿角ψ_b(i),θ_b(i),其中ψ_b(i)、θ_b(i)、分別為SINS的航向角、俯仰角、橫滾角；

(3)采集高低受信儀的測量值ε_b(i)和方位受信儀的測量值β_b(i)；

(4)采集高低電機軸角度值ε_m(i)及其角速度ω_ε(i)、方位電機軸角度值β_m(i)及其角速度ω_β(i)；

(5)采集SINS的陀螺組測量的三軸角速率ω_b(j)＝[ω_b1(j),ω_b2(j),ω_b3(j)]^T；

(6)采集炮塔陀螺組測量的三軸角速率ω_h(j)＝[ω_h1(j),ω_h2(j),ω_h3(j)]^T；

(7)采集火炮俯仰陀螺測量的角速率ω_p(j)；

(8)通過高低和方向受信儀等效閉環角度計算火炮身管在空間中的指向；

(9)判斷是否同時接收到大地坐標下的火炮瞄準方位控制指令ψ_ref(j)、高低控制指令θ_ref(j)，若是則進入第(10)步；否則轉入第(12)步；

(10)計算方位位置控制策略，包括方位位置控制誤差、方位速度指令、方位滑模等效控制量、方位滑模變結構控制量、方位滑模控切換制量；

(11)計算高低位置控制策略，包括高低位置控制誤差、高低速度指令、高低滑模等效控制量、高低滑模變結構控制量、高低滑模控切換制量；

(12)計算高低補償角速率和方位干擾補償角速率；

(13)計算高低干擾濾波校正量和方位干擾濾波校正量；

(14)計算高低伺服總的速度指令和方位伺服驅動總的速度指令；

(15)計算方位驅動的電流指令；

(16)計算高低驅動的電流指令；

(17)設定驅動器工作在力矩模式，通過CAN總線向伺服驅動器發送電流指令，轉入第(1)步。

2.根據權利要求1所述的大地坐標下等效閉環干擾速率補償自穩定控制方法，其特征在于，所述的步驟(8)通過高低和方向受信儀等效閉環角度計算火炮身管在空間中的指向，

其中，ψ(i)為火炮身管的航向角，θ(i)為火炮身管的俯仰角，為火炮身管的橫滾角，ψ′、為計算過程中的中間變量，為火炮身管姿態矩陣，

其中，為車體姿態矩陣，為車體航向姿態矩陣，為車體俯仰姿態矩陣，為車體橫滾姿態矩陣，為火炮方位姿態矩陣，為火炮俯仰姿態矩陣。