[發(fā)明專利]一種大地坐標(biāo)下等效閉環(huán)干擾速率補償自穩(wěn)定控制方法

有效

• 申請?zhí)枺?/td>: 202011517103.7
• 申請日: 2020-12-21
• 公開（公告）號: CN112729012B
• 公開（公告）日: 2022-12-23
• 發(fā)明（設(shè)計）人: 李偉;任海波;韓磊;位紅軍;馬捷
• 申請（專利權(quán)）人: 西北機(jī)電工程研究所
• 主分類號: F41G5/06
• 分類號: F41G5/06;G05B13/04
• 代理公司: 西北工業(yè)大學(xué)專利中心 61204
• 代理人: 顧潮琪

1.一種大地坐標(biāo)下等效閉環(huán)干擾速率補償自穩(wěn)定控制方法，其特征在于包括以下步驟：

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

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

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

(4)采集高低電機(jī)軸角度值ε_m(i)及其角速度ω_ε(i)、方位電機(jī)軸角度值β_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)通過高低和方向受信儀等效閉環(huán)角度計算火炮身管在空間中的指向；

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

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

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

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

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

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

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

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

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

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

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

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