一種捷聯式水下載體運動參數測試系統

王 博，鄭 賓

（中北大學 計算機與控制工程學院，太原 030051）

隨著海洋資源的開發和競爭，水下運載體運動狀況的研究受到了人們的日益關注[1]。航行體的水下運動參數對其結構強度，控制及發射等方面具有重要的指導意義。

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

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

1 設備功能要求及技術指標

該產品測試記錄裝置用于測試某產品運動參數及運動軌跡。該裝置能對被測產品運動過程中的三軸線加速度、角速度和磁場強度、航向角、姿態角等信號進行采集、處理并存儲于Flash中，最后通過USB接口傳輸至上位機。后期通過對記錄的數據進行處理，可以方便地重現該產品的運動軌跡，達到對產品運動過程進行測試的目的。該設備技術指標如下：

（1）實現產品全姿態測量；

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

（3）姿態測量精度為 0.5°；

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

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

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

2 產品運動過程分析

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

圖1 產品運動過程分解圖Fig.1 Product motion process decomposition diagram

階段1產品在布放裝置中的運動。布設裝置升到一定角度θ后，打開定位銷，產品開始滑出布設裝置。

階段2產品在空中的運動。產品離開布設裝置在空中運動時，由于產品的重心與形心不重合使產品俯仰角發生變化。

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

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

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

3 水下姿態及航向測量原理

3.1 系統初始對準

由于微慣性測量器件中MEMS陀螺儀的精度較低，無法僅由陀螺來實現初始對準[8]。故在此采用加速度計和磁強計組合的方式，確定載體的航向和姿態，以實現系統方位和姿態的初始對準。

采用“東-北-天”地理坐標系為導航系。導航系到載體系的歐拉角轉動次序為“Z-Y-X”，姿態矩陣與歐拉角的關系如下：

設重力向量和地磁向量在導航坐標系的投影分別為

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

由于

由式（2），解出初始姿態矩陣為

系統處于勻速運動狀態時，由加速度計測得的初始姿態角為

由磁強計測得的初始航向角為

式中：ψn為當地的磁偏角。

靜基座條件下，利用加速度計和磁強計測得的姿態角和航向角作為卡爾曼濾波的量測值，實驗平臺的水平位置和方位作為預測值，進行估計，可得到濾波后的姿態角和航向角作為初始值[8]。

3.2 系統姿態更新

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

得到四元數初值。捷聯慣性導航系統的姿態四元數微分方程[9-10]為

其中

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

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

由式（4）得到更新后的姿態四元數。姿態矩陣與四元數的關系為

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

3.3 軌跡解算

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

4 測試記錄系統方案設計

4.1 系統的總體設計

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

根據相應的技術要求，該測試系統包括傳感器模塊、信號調理模塊、A/D轉換模塊、存儲模塊、數據傳輸模塊、主控芯片、電源管理模塊等。

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

圖2 系統整體設計框圖Fig.2 Overall design block diagram of the system

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

采集模塊模擬信號必須轉換成為數字信號才能與主控芯片通信，并做相應的處理、計算，因此對于模擬信號需要A/D轉換模塊。

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

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

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

該測試記錄系統由6大模塊構成。系統測試時分為2個線程，即數字信號處理線程和模擬信號處理線程。

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

（2）模擬信號處理線程 大量程加速度計和壓力傳感器輸出的模擬信號，經過信號調理電路進行阻抗匹配，然后發送至A/D模塊轉換成數字量，FPGA從A/D讀取轉換后的數據，再發送給另一塊Flash存儲器進行儲存。

測試系統回收數據時的工作流程：系統上電后等待上位機的命令，當收到命令后將Flash中的數據讀取并發送給傳輸模塊，最后由數據傳輸模塊將數據發送至上位機。

4.2 硬件部分

系統的硬件部分主要由3部分組成，如圖3所示。

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

（2）信號采集、存儲、傳輸電路，包括FPGA控制單元、ADC模塊、2片Flash存儲單元、USB數據傳輸模塊；

（3）電源管理模塊，包括濾波穩壓模塊和電平轉換模塊。

圖3 系統的硬件組成Fig.3 Hardware composition of the system

4.2.1 慣性組件單元

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

圖4 MTi慣性組件單元系統組成Fig.4 MTi inertial component unit system composition

4.2.2 三軸加速度計

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

圖5為MEMS加速度計的內部結構。除了采用質量塊梁式結構的慣性原理，MEMS加速度計更多地采用了壓阻原理。如圖所示集成了三軸加速度計，安裝時可根據產品的軸向位置固定，即可測得三軸加速度。

圖5 MEMS加速度計內部結構示意Fig.5 Internal structure of MEMS accelerometer

4.3 系統軟件設計

系統軟件設計主要分為2部分，如圖6所示。

圖6 軟件設計Fig.6 Software design

（1）數據的獲取與解算 主要是計算機通過USB，將慣性組件單元存儲設備、三軸加速度計和深度傳感器存儲設備中的數據提取出來，通過相關軟件解算出某產品的航向角和姿態角。

（2）產品運動的重現 是計算機利用解算出的姿態角與深度值，通過Matlab/LabVIEW等軟件將產品的姿態與軌跡重現的過程。

根據解算出來的產品運動姿態的數據以及深度傳感器的數據，運用Matlab和LabVIEW混合編程，計算分析并顯示。具體程序流程如圖7所示。

圖7 產品三維姿態顯示軟件程序流程Fig.7 Software 3D gesture display software program flow chart

最終將產品的姿態仿真圖顯示在軟件界面上，其中包括產品的運動軌跡及相關數據曲線，由此完成產品姿態的重現。產品姿態重現界面如圖8所示。

圖8 產品姿態重現軟件界面Fig.8 Product gesture reproduction software interface

5 標定試驗和實物模擬試驗

5.1 姿態及航向靜態標定試驗

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

表1 姿態及航向靜態標定結果Tab.1 Static calibration results of attitude and heading

由表可知，在靜態條件下即使存在一定的安裝誤差，系統的姿態角和航向角也能夠滿足技術指標的要求。

5.2 實物模擬試驗

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

試驗過程如下：①產品觸發開始測試并記錄數據；②布放箱開始升起，穩定一段時間后，準備入水；③解除鎖定，開始滑射入水；④產品在水下運動；⑤產品浮上水面；⑥產品被成功回收。

產品在入水沖擊時刻和出水時刻，其局部運動參數如圖9所示。

圖9 產品入水、出水時刻的局部運動參數Fig.9 local movement parameters of the product entering water and effluent time

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

通過試驗驗證，整個試驗過程符合產品在水下運動的預期情況，該測試系統能夠達到產品姿態測試的要求。

6 結語

通過對某載體布放過程的分析，結合水下存儲測試系統的功能要求和技術指標，提出了測試系統的設計方案。通過硬件電路設計和軟件設計，構建了產品水下布放過程的運動參數測試系統。對該測試系統進行了標定試驗和實物模擬試驗，驗證了水下載體運動參數測試系統的正確性。

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

[2] 宋玉珍，劉煉.MEMS-IMU/GPS組合導航系統仿真研究[J].雷達科學與技術，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微慣性航姿系統研究[D].黑龍江：哈爾濱工程大學，2008.

[6]趙功偉，王震，商秋芳，等.一種水下航行體運動參數測量系統的設計[J].宇航計測技術，2016，36（4）：32-37，54.

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

[8]李辰淑.捷聯慣性導航初始對準技術研究[D].遼寧：大連理工大學，2013.

[9]張榮輝，賈宏光，陳濤，等.基于四元數法的捷聯式慣性導航系統的姿態解算[J].光學精密工程，2008，16（10）：1963-1970.

[10]劉危，解旭輝，李圣怡.捷聯慣性導航系統的姿態算法[J].北京航空航天大學學報，2005，31（1）：45-50.

[11]陳偉.基于四元數和卡爾曼濾波的姿態角估計算法研究與應用[D].河北：燕山大學，2015.

[12]熊敏敏.單片集成三軸硅微機械震動陀螺儀結構設計與仿真[D].江蘇：東南大學，2005.