王 博，鄭 賓

（中北大學(xué) 計(jì)算機(jī)與控制工程學(xué)院，太原 030051）

隨著海洋資源的開發(fā)和競爭，水下運(yùn)載體運(yùn)動狀況的研究受到了人們的日益關(guān)注[1]。航行體的水下運(yùn)動參數(shù)對其結(jié)構(gòu)強(qiáng)度，控制及發(fā)射等方面具有重要的指導(dǎo)意義。

國外對微慣性姿態(tài)測試技術(shù)的研究已有30多年的歷史，美國在這方面一直處于領(lǐng)先地位。美國NJM公司研制出的戰(zhàn)術(shù)級慣性測試單元IMU[2]，比現(xiàn)有的IMU體積更小，成本更低，功耗更小?，F(xiàn)在的MEMS加速度計(jì)和陀螺儀具有很多優(yōu)點(diǎn)，但其測試精度尚有待提高。美國Draper實(shí)驗(yàn)室開發(fā)的微慣性姿態(tài)測試系統(tǒng)，可以承受20000 g的沖擊，精度可達(dá)8°/h，且體積和功耗都很小[3-4]。我國對微慣性姿態(tài)測試技術(shù)的研究始于20世紀(jì)90年代初，迄今也取得了很大的進(jìn)步，我國研制的微機(jī)械陀螺在真空條件下已經(jīng)能夠達(dá)到10°/h[5]。

由于水下試驗(yàn)航行體的運(yùn)動環(huán)境惡劣，且體積較小，測試系統(tǒng)的工作條件苛刻，傳統(tǒng)的測量方法難以滿足要求[6]。為此，提出了一種基于FPGA的捷聯(lián)式水下載體運(yùn)動參數(shù)測試系統(tǒng)，把整個測試系統(tǒng)，包括傳感器、適配電路、瞬態(tài)波形記錄儀、電源等，微縮到1個小體積的測試儀中，直接放入被測體內(nèi)進(jìn)行測試；被測體的相關(guān)參數(shù)記錄后，通過USB電纜傳輸至上位機(jī)進(jìn)行數(shù)據(jù)的讀取和處理。試驗(yàn)表明，該系統(tǒng)具有測量準(zhǔn)確，控制方式靈活，體積小，成本低，可靠性高且實(shí)現(xiàn)簡單等優(yōu)點(diǎn)，具有廣闊的應(yīng)用前景。

1 設(shè)備功能要求及技術(shù)指標(biāo)

該產(chǎn)品測試記錄裝置用于測試某產(chǎn)品運(yùn)動參數(shù)及運(yùn)動軌跡。該裝置能對被測產(chǎn)品運(yùn)動過程中的三軸線加速度、角速度和磁場強(qiáng)度、航向角、姿態(tài)角等信號進(jìn)行采集、處理并存儲于Flash中，最后通過USB接口傳輸至上位機(jī)。后期通過對記錄的數(shù)據(jù)進(jìn)行處理，可以方便地重現(xiàn)該產(chǎn)品的運(yùn)動軌跡，達(dá)到對產(chǎn)品運(yùn)動過程進(jìn)行測試的目的。該設(shè)備技術(shù)指標(biāo)如下：

（1）實(shí)現(xiàn)產(chǎn)品全姿態(tài)測量；

（2）航向測量精度為 1°；

（3）姿態(tài)測量精度為 0.5°；

（4）三軸角速度量程為 0～450 °/s；

（5）入水時刻需滿足：三軸加速度量程為100g；采樣頻率為10 kHz。

要重現(xiàn)產(chǎn)品在入水過程中的運(yùn)動軌跡，產(chǎn)品運(yùn)動過程中的航向和姿態(tài)信息顯得尤為重要。產(chǎn)品運(yùn)動的航姿信息可由運(yùn)動過程中的加速度、角速度和磁方位等信息解算、融合得到[7]。

2 產(chǎn)品運(yùn)動過程分析

產(chǎn)品運(yùn)動時的主要運(yùn)動參數(shù)有：姿態(tài)角（包括航向角、俯仰角和橫滾角），三軸加速度，運(yùn)動軌跡。產(chǎn)品的運(yùn)動過程分為5個階段，如圖1所示。

圖1 產(chǎn)品運(yùn)動過程分解圖Fig.1 Product motion process decomposition diagram

階段1產(chǎn)品在布放裝置中的運(yùn)動。布設(shè)裝置升到一定角度θ后，打開定位銷，產(chǎn)品開始滑出布設(shè)裝置。

階段2產(chǎn)品在空中的運(yùn)動。產(chǎn)品離開布設(shè)裝置在空中運(yùn)動時，由于產(chǎn)品的重心與形心不重合使產(chǎn)品俯仰角發(fā)生變化。

階段3產(chǎn)品入水的過程。產(chǎn)品從空中拋至水面的瞬間，會產(chǎn)生較大的沖擊。

階段4產(chǎn)品在入水后的過程。產(chǎn)品入水后連同外殼一起運(yùn)動，且俯仰角逐漸增加，受到水下多種力的綜合作用，為一個復(fù)雜的運(yùn)動過程。

階段5產(chǎn)品翻轉(zhuǎn)過程。入水后產(chǎn)品所受浮力大于重力，逐漸從殼體中脫離出來。由于產(chǎn)品的重心靠近尾部，所以會產(chǎn)生翻轉(zhuǎn)。

3 水下姿態(tài)及航向測量原理

3.1 系統(tǒng)初始對準(zhǔn)

由于微慣性測量器件中MEMS陀螺儀的精度較低，無法僅由陀螺來實(shí)現(xiàn)初始對準(zhǔn)[8]。故在此采用加速度計(jì)和磁強(qiáng)計(jì)組合的方式，確定載體的航向和姿態(tài)，以實(shí)現(xiàn)系統(tǒng)方位和姿態(tài)的初始對準(zhǔn)。

采用“東-北-天”地理坐標(biāo)系為導(dǎo)航系。導(dǎo)航系到載體系的歐拉角轉(zhuǎn)動次序?yàn)椤癦-Y-X”，姿態(tài)矩陣與歐拉角的關(guān)系如下：

設(shè)重力向量和地磁向量在導(dǎo)航坐標(biāo)系的投影分別為

式中：gn，mn為已知常量。它們在載體坐標(biāo)系下的投影分別為gb，mb，可由加速度和磁強(qiáng)計(jì)測得（磁強(qiáng)計(jì)的輸出已由磁偏角補(bǔ)償）。構(gòu)造3個正交向量g，r和s為

由于

由式（2），解出初始姿態(tài)矩陣為

系統(tǒng)處于勻速運(yùn)動狀態(tài)時，由加速度計(jì)測得的初始姿態(tài)角為

由磁強(qiáng)計(jì)測得的初始航向角為

式中：ψn為當(dāng)?shù)氐拇牌恰?/p>

靜基座條件下，利用加速度計(jì)和磁強(qiáng)計(jì)測得的姿態(tài)角和航向角作為卡爾曼濾波的量測值，實(shí)驗(yàn)平臺的水平位置和方位作為預(yù)測值，進(jìn)行估計(jì)，可得到濾波后的姿態(tài)角和航向角作為初始值[8]。

3.2 系統(tǒng)姿態(tài)更新

由于載體在水中運(yùn)動緩慢，可忽略不計(jì)交換誤差，且設(shè)計(jì)中所采用的MTI傳感器的輸出為角速度。因此，宜采用四元數(shù)法直接求解四元數(shù)，避免對角速度積分而放大誤差。由歐拉角與四元數(shù)的關(guān)系

得到四元數(shù)初值。捷聯(lián)慣性導(dǎo)航系統(tǒng)的姿態(tài)四元數(shù)微分方程[9-10]為

其中

為載體系相對導(dǎo)航系的角速度。采用龍格庫塔法對式（3）微分方程求數(shù)值解，有

式中：h 為姿態(tài)更新間隔，h=tm+1-tm；Qtm，Qtm+1分別為tm時刻、tm+1時刻的姿態(tài)四元數(shù)。

由式（4）得到更新后的姿態(tài)四元數(shù)。姿態(tài)矩陣與四元數(shù)的關(guān)系為

對比式（1）和式（5），可知橫滾角 γ，俯仰角 θ，航向角ψ分別為

3.3 軌跡解算

求解式（6）微分方程后，可得系統(tǒng)在導(dǎo)航系下的速度，再對速度積分就可以得到位置。

4 測試記錄系統(tǒng)方案設(shè)計(jì)

4.1 系統(tǒng)的總體設(shè)計(jì)

該測試記錄系統(tǒng)的總體設(shè)計(jì)方案如圖2所示，存儲測試單元測量并記錄產(chǎn)品運(yùn)動過程中的三軸線加速度、角速度和磁場強(qiáng)度、航向角、姿態(tài)角等信息，處理并存儲在Flash中，最后通過USB接口傳輸至上位機(jī)進(jìn)行數(shù)據(jù)的讀取和解算。

根據(jù)相應(yīng)的技術(shù)要求，該測試系統(tǒng)包括傳感器模塊、信號調(diào)理模塊、A/D轉(zhuǎn)換模塊、存儲模塊、數(shù)據(jù)傳輸模塊、主控芯片、電源管理模塊等。

傳感器模塊根據(jù)測試要求，該測試系統(tǒng)需要：慣性組件單元，用以測量產(chǎn)品的姿態(tài)角、加速度等參數(shù)；壓力傳感器，用于測量產(chǎn)品入水的深度；由于在產(chǎn)品運(yùn)動過程中，入水瞬間的加速度很大，需要使用大量程三軸加速度計(jì)，以測試瞬時沖擊加速度。故傳感模塊包括慣性組件單元、大量程加速度計(jì)、壓力傳感器。

圖2 系統(tǒng)整體設(shè)計(jì)框圖Fig.2 Overall design block diagram of the system

信號調(diào)理模塊對于壓力傳感器和大量程加速度傳感器為模擬輸出，需要信號調(diào)理電路，對輸出的模擬量進(jìn)行跟隨、降噪處理。慣性組件單元為數(shù)字輸出，需要對這些數(shù)字量進(jìn)行電平轉(zhuǎn)換，與主控芯片進(jìn)行通信。

采集模塊模擬信號必須轉(zhuǎn)換成為數(shù)字信號才能與主控芯片通信，并做相應(yīng)的處理、計(jì)算，因此對于模擬信號需要A/D轉(zhuǎn)換模塊。

存儲模塊技術(shù)指標(biāo)要求存儲為非易失性存儲，而且大量程加速度計(jì)采樣頻率為10 kHz，且采樣時間為1 h，所以選擇大容量的Flash芯片進(jìn)行存儲比較合適。

數(shù)據(jù)傳輸模塊由于存儲的數(shù)據(jù)量較大，傳輸速度是一個很重要的因素，選擇USB通信模式，可使數(shù)據(jù)傳輸速度很快，傳輸時間很少。故數(shù)據(jù)傳輸模塊采用USB方式傳輸。

主控芯片測試系統(tǒng)包括了A/D模塊、存儲模塊，USB模塊等，因此需要的資源較多，并且采集和存儲同時進(jìn)行，為多線程任務(wù)運(yùn)行，故選擇主控芯片為FPGA更適合本設(shè)計(jì)要求。

該測試記錄系統(tǒng)由6大模塊構(gòu)成。系統(tǒng)測試時分為2個線程，即數(shù)字信號處理線程和模擬信號處理線程。

（1）數(shù)字信號處理線程 慣性組件單元輸出的數(shù)字信號，先由電平轉(zhuǎn)換電路進(jìn)行電平轉(zhuǎn)換，然后再與FPGA通信，將測試到的數(shù)據(jù)發(fā)送給FPGA，最后FPGA將慣性組件發(fā)送過來的數(shù)據(jù)，發(fā)送給Flash存儲器進(jìn)行存儲。

（2）模擬信號處理線程 大量程加速度計(jì)和壓力傳感器輸出的模擬信號，經(jīng)過信號調(diào)理電路進(jìn)行阻抗匹配，然后發(fā)送至A/D模塊轉(zhuǎn)換成數(shù)字量，F(xiàn)PGA從A/D讀取轉(zhuǎn)換后的數(shù)據(jù)，再發(fā)送給另一塊Flash存儲器進(jìn)行儲存。

測試系統(tǒng)回收數(shù)據(jù)時的工作流程：系統(tǒng)上電后等待上位機(jī)的命令，當(dāng)收到命令后將Flash中的數(shù)據(jù)讀取并發(fā)送給傳輸模塊，最后由數(shù)據(jù)傳輸模塊將數(shù)據(jù)發(fā)送至上位機(jī)。

4.2 硬件部分

系統(tǒng)的硬件部分主要由3部分組成，如圖3所示。

（1）傳感器模塊，包括慣性組合單元、三軸加速度計(jì)、深度傳感器；

（2）信號采集、存儲、傳輸電路，包括FPGA控制單元、ADC模塊、2片F(xiàn)lash存儲單元、USB數(shù)據(jù)傳輸模塊；

（3）電源管理模塊，包括濾波穩(wěn)壓模塊和電平轉(zhuǎn)換模塊。

圖3 系統(tǒng)的硬件組成Fig.3 Hardware composition of the system

4.2.1 慣性組件單元

系統(tǒng)選用荷蘭Xsens公司的慣性測量組件MTi。該組件單元由3D硅微機(jī)械加速度計(jì)、3D硅微機(jī)械振動式陀螺和3D磁場強(qiáng)度計(jì)組成[12]。系統(tǒng)裝有溫度傳感器，組件單元內(nèi)的DSP將各個傳感器進(jìn)行信息融合。該單元的三軸角速度、加速度、磁強(qiáng)及姿態(tài)角均可通過RS232串行總線對外輸出。通過解算便可得到產(chǎn)品的運(yùn)動軌跡。該單元單通道采樣頻率高達(dá)10 kHz，完全可以滿足系統(tǒng)采樣要求。其系統(tǒng)組成如圖4所示。

圖4 MTi慣性組件單元系統(tǒng)組成Fig.4 MTi inertial component unit system composition

4.2.2 三軸加速度計(jì)

由于產(chǎn)品入水和產(chǎn)品分離時產(chǎn)生的沖擊加速度大于MTi的量程（5g），因此需額外增加大量程的加速度計(jì)以校準(zhǔn)MTI輸出的加速度值。

圖5為MEMS加速度計(jì)的內(nèi)部結(jié)構(gòu)。除了采用質(zhì)量塊梁式結(jié)構(gòu)的慣性原理，MEMS加速度計(jì)更多地采用了壓阻原理。如圖所示集成了三軸加速度計(jì)，安裝時可根據(jù)產(chǎn)品的軸向位置固定，即可測得三軸加速度。

圖5 MEMS加速度計(jì)內(nèi)部結(jié)構(gòu)示意Fig.5 Internal structure of MEMS accelerometer

4.3 系統(tǒng)軟件設(shè)計(jì)

系統(tǒng)軟件設(shè)計(jì)主要分為2部分，如圖6所示。

圖6 軟件設(shè)計(jì)Fig.6 Software design

（1）數(shù)據(jù)的獲取與解算 主要是計(jì)算機(jī)通過USB，將慣性組件單元存儲設(shè)備、三軸加速度計(jì)和深度傳感器存儲設(shè)備中的數(shù)據(jù)提取出來，通過相關(guān)軟件解算出某產(chǎn)品的航向角和姿態(tài)角。

（2）產(chǎn)品運(yùn)動的重現(xiàn) 是計(jì)算機(jī)利用解算出的姿態(tài)角與深度值，通過Matlab/LabVIEW等軟件將產(chǎn)品的姿態(tài)與軌跡重現(xiàn)的過程。

根據(jù)解算出來的產(chǎn)品運(yùn)動姿態(tài)的數(shù)據(jù)以及深度傳感器的數(shù)據(jù)，運(yùn)用Matlab和LabVIEW混合編程，計(jì)算分析并顯示。具體程序流程如圖7所示。

圖7 產(chǎn)品三維姿態(tài)顯示軟件程序流程Fig.7 Software 3D gesture display software program flow chart

最終將產(chǎn)品的姿態(tài)仿真圖顯示在軟件界面上，其中包括產(chǎn)品的運(yùn)動軌跡及相關(guān)數(shù)據(jù)曲線，由此完成產(chǎn)品姿態(tài)的重現(xiàn)。產(chǎn)品姿態(tài)重現(xiàn)界面如圖8所示。

圖8 產(chǎn)品姿態(tài)重現(xiàn)軟件界面Fig.8 Product gesture reproduction software interface

5 標(biāo)定試驗(yàn)和實(shí)物模擬試驗(yàn)

5.1 姿態(tài)及航向靜態(tài)標(biāo)定試驗(yàn)

將記錄儀安裝在手動雙自由度轉(zhuǎn)臺上，分別模擬傾斜、俯仰和航向角的變化，每個角度停留10 s，取均值。表1給出了轉(zhuǎn)臺輸出角度和儀器測試角度的標(biāo)定結(jié)果。

表1 姿態(tài)及航向靜態(tài)標(biāo)定結(jié)果Tab.1 Static calibration results of attitude and heading

由表可知，在靜態(tài)條件下即使存在一定的安裝誤差，系統(tǒng)的姿態(tài)角和航向角也能夠滿足技術(shù)指標(biāo)的要求。

5.2 實(shí)物模擬試驗(yàn)

試驗(yàn)利用彈道測試裝置，記錄從產(chǎn)品入海到產(chǎn)品浮出海面的整個過程中產(chǎn)品的俯仰角、橫滾角、3個軸向加速度、產(chǎn)品入水深度等參數(shù)。

試驗(yàn)過程如下：①產(chǎn)品觸發(fā)開始測試并記錄數(shù)據(jù)；②布放箱開始升起，穩(wěn)定一段時間后，準(zhǔn)備入水；③解除鎖定，開始滑射入水；④產(chǎn)品在水下運(yùn)動；⑤產(chǎn)品浮上水面；⑥產(chǎn)品被成功回收。

產(chǎn)品在入水沖擊時刻和出水時刻，其局部運(yùn)動參數(shù)如圖9所示。

圖9 產(chǎn)品入水、出水時刻的局部運(yùn)動參數(shù)Fig.9 local movement parameters of the product entering water and effluent time

由圖9（a）可知，產(chǎn)品和殼體隨著平板往水下沉，由于產(chǎn)品受到的浮力大于重力，會與殼體分離開然后翻轉(zhuǎn)，最終浮在水面上。在這一過程中俯仰角（產(chǎn)品軸向）初始值為定值，入水后變?yōu)檎?；產(chǎn)品與殼體分離后，產(chǎn)品發(fā)生翻轉(zhuǎn)，俯仰角變?yōu)樨?fù)值。由圖9（b）可知，出水前，產(chǎn)品懸浮在水中，軸向朝上，因此X軸加速度為1g，Y軸和Z軸加速度為0；在出水瞬間，產(chǎn)品的軸向加速度由1g變?yōu)?。

通過試驗(yàn)驗(yàn)證，整個試驗(yàn)過程符合產(chǎn)品在水下運(yùn)動的預(yù)期情況，該測試系統(tǒng)能夠達(dá)到產(chǎn)品姿態(tài)測試的要求。

6 結(jié)語

通過對某載體布放過程的分析，結(jié)合水下存儲測試系統(tǒng)的功能要求和技術(shù)指標(biāo)，提出了測試系統(tǒng)的設(shè)計(jì)方案。通過硬件電路設(shè)計(jì)和軟件設(shè)計(jì)，構(gòu)建了產(chǎn)品水下布放過程的運(yùn)動參數(shù)測試系統(tǒng)。對該測試系統(tǒng)進(jìn)行了標(biāo)定試驗(yàn)和實(shí)物模擬試驗(yàn)，驗(yàn)證了水下載體運(yùn)動參數(shù)測試系統(tǒng)的正確性。

[1]安江波.大時延水下機(jī)械手位置伺服控制技術(shù)研究[D].黑龍江哈爾濱工程大學(xué)，2008.

[2] 宋玉珍，劉煉.MEMS-IMU/GPS組合導(dǎo)航系統(tǒng)仿真研究[J].雷達(dá)科學(xué)與技術(shù)，2010，8（4）：301-305.

[3]Jesus Zegarra Flores，René Farcy.GPS and IMU （inertial measurement unit）as a navigation system for the visually impaired in cities[J].Journal of Assistive Technologies，2013，71.

[4]Bradford S.Davis.Using low-cost MEMS accelerometers and gyroscopes as strapdown IMUs on rolling projects.EEE0-78034330-1，1998：594-601.

[5]劉珺琇.基于MTi微慣性航姿系統(tǒng)研究[D].黑龍江：哈爾濱工程大學(xué)，2008.

[6]趙功偉，王震，商秋芳，等.一種水下航行體運(yùn)動參數(shù)測量系統(tǒng)的設(shè)計(jì)[J].宇航計(jì)測技術(shù)，2016，36（4）：32-37，54.

[7]喬汝椿.輕型艦載魚雷發(fā)射裝置系泊發(fā)射魚雷入水參數(shù)分析[J].魚雷技術(shù)，2000，8（4）：43-46.

[8]李辰淑.捷聯(lián)慣性導(dǎo)航初始對準(zhǔn)技術(shù)研究[D].遼寧：大連理工大學(xué)，2013.

[9]張榮輝，賈宏光，陳濤，等.基于四元數(shù)法的捷聯(lián)式慣性導(dǎo)航系統(tǒng)的姿態(tài)解算[J].光學(xué)精密工程，2008，16（10）：1963-1970.

[10]劉危，解旭輝，李圣怡.捷聯(lián)慣性導(dǎo)航系統(tǒng)的姿態(tài)算法[J].北京航空航天大學(xué)學(xué)報(bào)，2005，31（1）：45-50.

[11]陳偉.基于四元數(shù)和卡爾曼濾波的姿態(tài)角估計(jì)算法研究與應(yīng)用[D].河北：燕山大學(xué)，2015.

[12]熊敏敏.單片集成三軸硅微機(jī)械震動陀螺儀結(jié)構(gòu)設(shè)計(jì)與仿真[D].江蘇：東南大學(xué)，2005.