一種基于陀螺儀的船載穩像控制方法
【技術領域】
[0001] 本發明應用于自穩定的攝像控制系統領域，設及一種適用于基于慣性傳感器巧螺 儀的船載穩像系統的實時控制方法。
【背景技術】
[0002] 船載穩定攝像技術是穩像技術的一個衍生領域，設及傳感器數據采集、數據濾波 融合、運動控制、電機驅動等多個相關的各類學科。
[0003] 隨著近幾年安防監控行業的迅速發展，攝像機不僅在道路，樓宇等固定平臺上大 量使用，而且廣泛的應用于船只、汽車和飛行器等多種運動的載體中。與此同時，應用于該 些不穩定載體的攝像機監控都存在一個問題；由于安裝的載體存在搖晃，無法保證攝像機 視角持續瞄準載體W外的監控目標，并且該個問題在船艦上尤為突出。正是因為船艦監控 及安全存在的問題，民用、商用的船艦對裝備具有自穩定功能的監控云臺的需求越來越強 烈。
[0004] 船艦上穩像技術實現一般有兩種方式，其中一種是依靠將船載云臺安裝在額外添 加的穩像平臺上隔離船體運動干擾，W達到穩定攝像的效果。另外一種就是利用驅動攝像 云臺本身的電機W及對圖像的處理消除船艦搖晃帶來的圖像搖晃問題。外加穩像平臺的方 式相對于直接控制云臺電機的方式來說具有安裝繁瑣、體積龐大、增加成本等弊端。而通過 處理圖像來達到穩像目的的方式不適用于大角度搖晃的船艦上使用。并且處理方法復雜， 對處理器要求很高，設備往往昂貴。一般采用電機運動補償的控制處理方法太過簡單，使 用的穩像云臺不是具有俯仰軸與方位軸的通用云臺。擴展性較差。例如在授權公告號為 CN203037261U，發明名稱是"一種小型巧螺儀穩像系統"的實用新型專利中，使用具有俯仰 軸與翻滾軸的云臺作為控制對象。并且處理方式過于簡單，并沒有對加速度計與巧螺儀的 數據進行進一步處理，會導致精度不足。在步進電機控制方面，步進電機的運動方式是W步 進角為最小運動單位0U運行的，也就是說，系統具有一個高頻的噪聲，該對于控制器的微 分量11值）是一個很強的干擾對造成系統的自激擾動。由于沒有對步進電機加減速控制方 式的優化，很容易造成步進電機的失步越步導致系統不穩定。

【發明內容】

[0005] 為了克服現有的船載攝像技術的安裝復雜、通用性差、精度不足、穩定性不夠W及 成本過高的不足，本發明提供了一種基于巧螺儀的船載穩像控制方法，在保證穩像控制的 精度和系統穩定性性的同時，又具有體積小、通用性好、低成本等特點。
[0006] 本發明解決上述技術問題是通過W下技術方案實現的：
[0007] -種基于巧螺儀的船載穩像控制方法，所述方法包括如下步驟：
[0008] 1)使用具有俯仰軸與方位軸的基于步進電機的二自由度船載安防云臺作為穩像 平臺；
[0009] 2)利用加速度計和巧螺儀兩種傳感器分別獲得云臺的S軸加速度ay，ay，a,及 其S軸角速度
[0010]如將獲得的該兩種不同的俯仰軸上的數據aX與《y進行對運算量W及內存優化 的卡爾曼數據融合濾波，進而獲得云臺的精確俯仰角0pitehW及俯仰角速度《piteh，同樣的 方式求得云臺的翻滾角0"11;
[0011]4)利用得到的俯仰角0Pitch輸入改進的PD控制器中，輸出控制云臺的俯仰軸電機 的期望角速度值；
[0012] 所述改進的PD控制器中，PD算法根據穩像云臺的系統特點進行如下的改進步驟： [001 引error似=0pitch-etarget做
[0014] 將式（6)得到的采樣序號為k時刻的偏差數據error(k)代入式（7)離散PD控制 器中得到控制量輸出值U化）
[0015]
【主權項】
1. 一種基于陀螺儀的船載穩像控制方法，其特征在于：所述方法包括如下步驟： 1) 使用具有俯仰軸與方位軸的基于步進電機的二自由度船載安防云臺作為穩像平 臺； 2) 利用加速度計和陀螺儀兩種傳感器分別獲得云臺的三軸加速度ax，ay，az及其三 軸角速度《x、《y、《 z; 3) 將獲得的這兩種不同的俯仰軸上的數據a# ? y進行對運算量以及內存優化的卡 爾曼數據融合濾波，進而獲得云臺的精確俯仰角0pitc;hW及俯仰角速度《 pitc;h，同樣的方式 求得云臺的翻滾角9 4) 利用得到的俯仰角0piteh輸入改進的ro控制器中，輸出控制云臺的俯仰軸電機的期 望角速度值； 所述改進的ro控制器中，ro算法根據穩像云臺的系統特點進行如下的改進步驟：error(k) = 0 pitch- 0target (6) 將式（6)得到的采樣序號為k時刻的偏差數據error(k)代入式（7)離散H)控制器中 得到控制量輸出值u(k)
(7) 式中，err〇r(k-l)是采樣序號為k-1時刻的偏差數據； 控制器限幅輸出
（8) 當控制器輸出大于Max_U(k)時，系統輸出Max_U(k)，同理當控制器輸出大于Min_U(k)時，系統輸出Min_U(k); 根據不同的error(k)對kp參數作出改變，對kp與error(k)建立基于指數函數的變化 關系，實時改變系統的增益如下式（9)所示：
(9) 使用不完全微分的ro算法，在原來的微分量Ud(k)項中引入一階低通濾波器，如式 (10) 所示： ud (k) =kd (1-a)(error(k)-error(k-1)) +aud (k-1) (10) 同時結合了目標角改變微分消除算法對0 改變時帶來的微分量不代入計算，如式 (11) 所示： ud (k) =ud (k) -kd* [ 0target (k) - 0target (k-1) ] (11) 5) 解算穩像系統在非平衡狀態下，船體出現翻滾運動對穩像平臺航向角的影響，通過 控制方位角補償該影響；如式（14)所示：
(14) 式中，PymvCfc)是k時刻云臺方位角，1)為k-1時刻云臺方位角， 0pitc；h(k)為k時刻的俯仰角，0Mll(k)為k時刻的翻滾角，sin〇為正弦運算，asin〇為反 正弦運算； 6)最后將計算結果傳輸到底層步進電機驅動器。
2. 如權利要求1所述的一種基于陀螺儀的船載穩像控制方法，其特征在于：所述步驟 4)中，在ro控制器的后級加上步進電機非線性加減速控制器；步進電機的功率、扭矩和轉 速是相關聯的，具體關系為： P = a -Torque* ? (12) 式中P為步進電機功率，a為轉換系數，Torque是電機的扭矩，《是電機轉速； 當前允許最大加速度值與當前速度呈線性相關的關系：
(13) ro控制器的輸出角速度結果經過與后級的步進電機加減速控制相比較，選擇以后輸出 到步進電機上，檢驗過程中系統對比ro算法獲得的速度與步進電機加減速控制算法計算 得出的最大速度，并且始終選擇兩者的較小數值。
3. 如權利要求1或2所述的一種基于陀螺儀的船載穩像控制方法，其特征在于：在步 驟3)中，所述的卡爾曼融合算法過程如下： 首先建立系統的狀態方程：
上式中anglek是k時刻角度值，q_bias k是陀螺儀的偏差，dt是更新周期，gyro_m是陀 螺儀的過程噪聲，w_angle和w_gyro分別是是加速度計與陀螺儀的測量噪聲； 建立測量方程
構造過程噪聲矩陣 構造測量噪聲矩陣：LK_ang_Le」
角度預測： angle = angle_q-biask氺dt+gyrom^dt = angle+Rate氺dt 方差預測：
角度誤差更新：angle = incAngle-angle (2) 計算卡爾曼增益：
重復計算公式（1)~（5)直至找到最優的結果。
【專利摘要】一種基于陀螺儀的船載穩像控制方法，使用具有俯仰軸與方位軸的基于步進電機的二自由度船載安防云臺作為穩像平臺，將獲得的這兩種不同的俯仰軸上的數據αx與ωy進行對運算量以及內存優化的卡爾曼數據融合濾波，進而獲得云臺的精確俯仰角θpitch以及俯仰角速度ωpitch，云臺的翻滾角θroll。采用求得的θpitch輸入改進的PD控制器中，得到控制云臺的俯仰軸電機的輸出角速度，解算穩像系統在非平衡狀態下，船體出現翻滾運動對穩像平臺航向角的影響。通過控制方位角補償該影響，最后將計算得到的輸出角速度ωout通過串行總線RS232傳輸到底層步進電機驅動器中執行。本發明能對船載攝像系統的運動擾動被補償消除與抑制，從而達到抑制船載攝像時圖像晃動的目的。
【IPC分類】H04N5-14, H04N5-21
【公開號】CN104811588
【申請號】CN201510170010
【發明人】董輝, 賴宏煥, 王全強, 黃勝, 陳慧慧
【申請人】浙江工業大學, 杭州晨安視訊數字技術有限公司
【公開日】2015年7月29日
【申請日】2015年4月10日