硬件Kalman濾波器的航拍云臺姿態(tài)如何獲取

0 引言

航空攝影（Aerial Photography）又稱航拍，是指在飛機(jī)或其他航空飛行器上利用航空攝影機(jī)攝取地面景物相片的技術(shù)。目前，航空攝影應(yīng)用廣泛，是獲取地理信息的主要手段[1-2]。在航空攝影中，存在著一個難題需要解決：飛機(jī)在飛行攝影時會受到本機(jī)和氣流等的影響，從而產(chǎn)生機(jī)體振動，相機(jī)的姿態(tài)角也會隨著改變，相機(jī)姿態(tài)時刻改變就會給高精度拍攝帶來困難。所以為提高航拍質(zhì)量，必須對相機(jī)姿態(tài)進(jìn)行校正[3]。

校正的前提是獲取相機(jī)姿態(tài)，所以慣性測量元件必不可少，但是單一的慣性測量元件無法給出準(zhǔn)確的姿態(tài)角，需要對多個慣性測量元件的數(shù)據(jù)進(jìn)行數(shù)據(jù)融合。使用比較普遍的慣性測量元件的組合是加速度計和陀螺儀。它們輸出的數(shù)據(jù)各有優(yōu)缺點(diǎn)，可以使用Kalman濾波對其進(jìn)行數(shù)據(jù)融合以得到準(zhǔn)確的姿態(tài)角[4-5]。

現(xiàn)有的實現(xiàn)Kalman濾波的方式主要有兩種：PC機(jī)和DSP，兩者都存在著系統(tǒng)性能和成本不能兼顧的問題。而FPGA卻可以很好地解決這個問題。本文主要講述的便是用FPGA實現(xiàn)Kalman濾波來獲取準(zhǔn)確的相機(jī)姿態(tài)。

1 坐標(biāo)模型

在進(jìn)行航拍作業(yè)時，相機(jī)搭載在云臺上，云臺和飛機(jī)是一體的，當(dāng)相機(jī)拍攝地面照片時，相機(jī)拍攝方向應(yīng)與地面垂直，如圖1所示。

為獲取相機(jī)姿態(tài)，首先以相機(jī)為原點(diǎn)，建立坐標(biāo)系，如圖2所示。其中，XY平面與地面平行，Z軸垂直于地面，Y軸方向為飛機(jī)飛行方向。

將加速度計固定在相機(jī)上，當(dāng)相機(jī)姿態(tài)發(fā)生改變時，加速度的3個敏感軸便可以輸出在其相應(yīng)方向產(chǎn)生的重力分量信號，如圖2中的Gx，Gy，Gz所示。陀螺儀是一種角速度傳感器，它可以輸出對應(yīng)軸上的瞬時角速度，如圖2中的Wx，Wy，Wz所示。

根據(jù)以上坐標(biāo)系，可以得到姿態(tài)角θx，θy，θz（分別代表著俯仰角、橫滾角、偏航角），如式1所示，其中ΔT為采樣時間，Δθ為采樣時間內(nèi)的姿態(tài)角變化。

可見，由加速度計或者陀螺儀都可以初步得到姿態(tài)角，但是無論是加速度計還是陀螺儀，它們輸出的原始數(shù)據(jù)都存在著固有缺陷：當(dāng)相機(jī)處于變速運(yùn)動狀態(tài)時，加速度計的輸出并不是重力加速度，而是重力加速度和其自身加速度的矢量和。陀螺儀是通過對角速度積分得到姿態(tài)角，隨著時間的推移，姿態(tài)角的誤差會越來越大。所以單一的加速度計或者陀螺儀都無法獲得正確的姿態(tài)角，必須對兩者進(jìn)行數(shù)據(jù)融合。

2 Kalman濾波模型的建立

Kalman濾波是在時域內(nèi)以信號的一、二階統(tǒng)計特性已知為前提、以均方誤差極小為判據(jù)，能自動跟蹤信號統(tǒng)計性質(zhì)的非平穩(wěn)變化，具有遞歸性質(zhì)的一種算法[5]。Kalman濾波的狀態(tài)方程和預(yù)測方程如式(2)所示：

其中，Xk+1為系統(tǒng)的狀態(tài)向量，Φk+1，k為狀態(tài)轉(zhuǎn)移矩陣，Tk+1，k為控制系數(shù)矩陣，Uk為系統(tǒng)控制輸入，Γk+1為噪聲系數(shù)矩陣，Wk為系統(tǒng)的過程噪聲序列，Zk+1為系統(tǒng)的觀測序列，Hk+1為觀測矩陣，Vk+1為觀測噪聲序列。

使用陀螺儀數(shù)據(jù)建立線性模型，如式(3)所示：

其中，θk為飛行姿態(tài)角，βk為陀螺儀輸出的誤差，Uk是陀螺儀輸出的角速度數(shù)據(jù)，ΔT是采樣時間間隔。因為陀螺儀輸出的誤差整體來看變化不大，可認(rèn)為βk+1=βk。因Wk對系統(tǒng)影響甚小，這里規(guī)定Wk=0。

由以上建立狀態(tài)矩陣方程，如式(4)所示：

陀螺儀和加速度計測量姿態(tài)角時各有優(yōu)缺點(diǎn)，為獲得比較可靠的姿態(tài)角數(shù)據(jù)，必須對兩者的數(shù)據(jù)優(yōu)勢互補(bǔ)。以上算法用加速度計數(shù)據(jù)得到不同階段的測量噪聲協(xié)方差，并帶入解算過程中，大大減少了姿態(tài)角的測量誤差。

3 Kalman硬件實現(xiàn)

目前Kalman濾波器的實現(xiàn)方式主要有PC機(jī)和DSP兩種方式，PC機(jī)可以同時滿足計算精度和實時性的要求，但是PC機(jī)體積大、質(zhì)量重、成本高；DSP實現(xiàn)的Kalman濾波器雖然體積小，質(zhì)量輕，但是因其指令順序執(zhí)行的CPU架構(gòu)，在系統(tǒng)復(fù)雜時無法滿足系統(tǒng)的實時性要求[6-7]。

因此，Kalman濾波器在工程應(yīng)用中的實現(xiàn)遇到了系統(tǒng)體積、重量、成本與系統(tǒng)精度、速度等性能不能兼顧的問題。FPGA是解決這一問題的有效途徑，F(xiàn)PGA具有極高并行度的信號處理能力、高度的靈活性、現(xiàn)場可以編程、較短的開發(fā)周期和較低的成本等優(yōu)勢，在數(shù)字信號處理領(lǐng)域已經(jīng)得到廣泛的應(yīng)用[8]。

本系統(tǒng)使用的FPGA芯片是Altera公司Cyclone II系列中的EP2C35F672C6，它共有33 216個邏輯單元、483 840個存儲單元、70個乘法器單元（9 bit）。雖然Altera公司提供了很多功能強(qiáng)大的IP核，其中就包括加減乘除單精度浮點(diǎn)數(shù)運(yùn)算，但是提供的IP核占用資源過多，所以使用自己編寫的運(yùn)算模塊。下面以除法模塊為例說明浮點(diǎn)數(shù)運(yùn)算的硬件實現(xiàn)。

3.1 浮點(diǎn)數(shù)除法模塊

根據(jù)IEEE754標(biāo)準(zhǔn)，單精度浮點(diǎn)數(shù)的格式如圖2所示。

通?？捎檬?11)表示一個單精度浮點(diǎn)數(shù)：

從式(12)中可以看到，符號位和階碼的運(yùn)算比較簡單，尾數(shù)的運(yùn)算較為復(fù)雜。

設(shè)被除數(shù)為a，除數(shù)為b，第i次的商為qi，第i次得到的臨時余數(shù)為mi，令m0=a，則有式(13)：

這樣一直進(jìn)行23次運(yùn)算便可以獲得全部的qi，23位qi按先后順序組合起來便是尾數(shù)運(yùn)算的結(jié)果。

除法模塊資源使用情況如圖3所示，只使用了1%的邏輯單元，相比較于IP核的實現(xiàn)方式，大大減少了使用資源。

再用ModelSim仿真觀察除法模塊運(yùn)算速度，結(jié)果如圖4所示。x1，x2是除數(shù)和被除數(shù)，y是商，isdone是標(biāo)志信號，當(dāng)完成一次除法運(yùn)算后產(chǎn)生一個正脈沖。

由圖4可以看到，除法模塊運(yùn)算一次共使用了25個CLK周期，完全可以滿足運(yùn)算要求。

3.2 Kalman硬件模塊

以第2節(jié)中建立的卡爾曼濾波模型搭建硬件模塊，硬件模塊的結(jié)構(gòu)如圖5所示。其中，

為整個Kalman濾波的結(jié)果矩陣，包括橫滾角、俯仰角、偏航角。

Kalman硬件模塊使用資源情況如圖6所示。可見，一共使用了45%的邏輯單元。雖然占用資源非常多，但是45%的資源使用還是可以接受的。

利用Multisim仿真，查看此Kalman硬件模塊的運(yùn)行速度，結(jié)果如圖7所示。

q1、q2、q3是Kalman濾波解算一次后的3個偏轉(zhuǎn)角（橫滾角、俯仰角、航向角），q1st、q2st、q3st是解算一次完成后產(chǎn)生的負(fù)脈沖信號。從仿真結(jié)果來看，在仿真頻率為100 MHz情況下，Kalman解算一次需要的時間是1 ms，可見用FPGA來實現(xiàn)Kalman濾波速度是很快的。

4 實驗結(jié)果

為檢驗Kalman濾波硬件模塊實際效果，設(shè)計了一次模擬實驗：將云臺置于振動環(huán)境中，改變云臺姿態(tài)角，觀察未經(jīng)濾波處理和經(jīng)過濾波處理的姿態(tài)角。

實驗中使用MPU6050作為系統(tǒng)的姿態(tài)傳感器，MPU-6050是一款整合了3軸陀螺儀、3軸加速度計的6軸運(yùn)動處理組件，相比較于多組件方案，免除了組合陀螺儀與加速度計時之軸間差的問題，減少了大量的包裝空間。

設(shè)置采樣頻率為1 kHz，以橫滾角為示例，實驗結(jié)果如圖8所示。

濾波前的橫滾角由三軸加速度和式（1）得到，可以看到明顯的噪聲和毛刺。經(jīng)過Kalman濾波后，大部分的噪聲和毛刺都被去除掉了，而且通過濾波前后的橫坐標(biāo)對比可以看到此濾波模塊有著很好的實時性能。

觀察濾波結(jié)果與真實值之間的誤差，如圖9所示。從圖中可以看到，濾波誤差控制在-1.5°～1.5°范圍內(nèi)?？梢詽M足航拍系統(tǒng)的精度要求。

5 結(jié)論

實驗證明，本文所介紹的硬件Kalman濾波器適用于航拍相機(jī)平臺的姿態(tài)獲取?；贔PGA的硬件架構(gòu)具有靈活的可移植特性和優(yōu)良的抗干擾能力，相比較于DSP和PC機(jī)的實現(xiàn)方式，它在保證航拍系統(tǒng)的精度要求的前提下解決了性能和成本不能兼顧的問題，因此它具有更廣泛的應(yīng)用性和實際工程意義。

參考文獻(xiàn)

[1] 劉先林，段福洲，宮輝力.航空攝影科技發(fā)展成就與未來展望[J].前沿科學(xué)，2007，3(3)：10-14.

[2] 張祖勛.數(shù)字?jǐn)z影測量發(fā)展與展望[J].地理信息世界，2004，2(3)：1-5.

[3] 高同躍，丁衛(wèi)，高新聞，等.機(jī)載慣性穩(wěn)定云臺系統(tǒng)的研究[J].2010，18(2)：344-347.

[4] 卞鴻巍,李安,覃方君，等.現(xiàn)代信息融合技術(shù)在組合導(dǎo)航中的應(yīng)用[M].北京：國防工業(yè)出版社，2010.

[5] 付夢印，鄧志紅，閆莉萍.Kalman濾波理論及其在導(dǎo)航系統(tǒng)中的應(yīng)用(第二版)[M].北京：科學(xué)出版社，2010.

[6] 謝鑫，郭美鳳，周斌.基于雙DSP和FPGA的高性能導(dǎo)航計算機(jī)設(shè)計[J].微計算機(jī)信息，2009(3)：1-2，60.

[7] 張沖，吳秋平.基于PC104和DSP的分布式導(dǎo)航計算機(jī)設(shè)計[J].微計算機(jī)信息，2010(8)：27-29.

[8] 米月琴，黃軍榮.基于FPGA的Kalman濾波器的設(shè)計[J].電子科技，2010，23(2)：52-55.

編輯：jq