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

【發(fā)明內(nèi)容】

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