基于國產DSP多傳感器融合姿態解算系統

摘 "要： 隨著戰場上作戰方式的多樣化、智能化，對作戰輔助系統也提出了更高的要求。駕駛員在裝甲車輛內部（全封閉環境下），為更快捷地獲取不同角度的廣闊視野，亟需一種通過解算人體頭部姿態來控制光電設備隨動的系統。文中提出一種融合圖像傳感器和陀螺傳感器各自優勢的人體頭部姿態解算系統，該系統以雙圖像傳感器數據解算為主，陀螺傳感器數據解算為輔，融合了圖像傳感器解算精度高、陀螺傳感器解算速度快的優勢。該系統以國產DSP FT?M6678N為核心處理器，設計了三核并行處理架構，核0實現數據交互、多核數據融合，核1、核2實現雙相機各自通道姿態解算。搭建實裝驗證環境，結果表明該系統較以往單傳感器解算系統在精度、實時性、環境適應性上有明顯提升，為頭部姿態解算提供了新思路。

關鍵詞： 多傳感器融合； 圖像傳感器； 陀螺傳感器； FT?M6678N； 并行處理； 高精度； 實時性； 環境適應性

中圖分類號： TN219?34； TP391.41 " " " " " " " " " 文獻標識碼： A " " " " " " " " "文章編號： 1004?373X（2024）21?0177?06

Multi?sensor fusion attitude calculation system based on domestic DSP

HU Kai， LI Lei

（China Electronics Technology Group Corporation No. 58 Research Institute， Wuxi 214035， China）

Abstract： In recent years， the diversification and intelligence of combat modes on the battlefield have put forward higher requirements for combat assistance systems. To obtain a wide field of view from different angles more quickly， drivers， in armored vehicles （a fully?enclosed environment）， need a system to control the follow?up of the photoelectric equipment by calculating posture of human head. A human head posture calculation system which combines the advantages of image sensor and gyro sensor is presented. The system is mainly based on the data calculation of dual image sensors， supplemented by the data calculation of gyro sensor， which combines the advantages of high accuracy of image sensor and high speed of gyro sensor. In this system， a three?core parallel processing architecture is designed by using domestic DSP FT?M6678N as the core processor. Core 0 is used to realize data interaction and multi?core data fusion， and core 1 and core 2 are used to realize the attitude settlement of each channel of the dual cameras. A verification environment for real equipment is built. The results show that the system has improved the accuracy， real?time performance and environmental adaptability significantly in comparison with the previous single?sensor solution system， so it provides a new idea for head posture calculation.

Keywords： multi?sensor fusion; image sensor; gyro sensor; FT?M6678N; parallel processing; high precision; real?time performance; environmental adaptability

0 "引 "言

頭部是人體最重要的部位，能產生各類信息，包括運動、姿態、人臉特征、表情等。通過對人體頭部的運動、姿態以及人臉信息解析，來獲取人體大腦的指揮信息。人體頭部的信息研究已成為機器視覺等領域的焦點，目前，人體頭部姿態解算技術已在軍事和民用領域得到了廣泛應用[1?2]。

頭部姿態估計是利用計算機視覺和模式識別的方法在數字圖像中判斷人體頭部的朝向問題[3]，通過一個空間坐標系識別頭部的姿態方向參數[4]。在計算機視覺領域，姿態測量是研究的熱點和難點，目前已有的姿態測量系統大多基于單傳感器[5]。

慣性傳感器[6]是集成加速度計或者陀螺儀等運動傳感器的設備。其優勢在于體積小、功耗低、數據更新率高、不受環境光線干擾；不足之處在于長時間連續工作，存在累積漂移問題。

視覺類傳感器通常采用以下三種方法采集圖像數據：一是通過采集分析人臉的特征[7?8]，檢測人臉的特征點，根據特征點的旋轉變化解算出運動姿態，但基于該原理的系統受環境影響因素大，解算出的姿態角度誤差較大，在密閉黑暗的環境下甚至無法正常工作；二是通過單一的圖像傳感器采集圖像數據[9]，解算人體頭部運動的姿態，但受到處理器性能的限制[10]，無法滿足系統的實時性要求，解算出的姿態角存在滯后現象，同時存在人體頭部轉動角速度過大（大于100（°）/s）時，采集的圖像會有拖尾現象，導致通過圖像無法正常解算姿態角；三是使用單相機采集圖像數據，受相機的視場角限制，頭部運動角度有限[11]。

本文為解決以上不足，設計了一種多傳感器融合的姿態解算系統，采用融合測量的方法，極大吸取了兩種傳感器各自的優勢，提高了解算精度，提升了系統環境適應能力，為頭部姿態解算提供了新思路。

隨著國際芯片戰爭愈演愈烈，國產化已成為現今武器裝備的趨勢，國內自主可控高性能DSP、FPGA也趨于成熟，結合DSP、FPGA的硬件低功耗、小尺寸特征，以FT?M6678N為主處理器[12]、JFM7K325T為輔處理器搭建了一套全國產化姿態解算系統[13]。

1 "總體設計

姿態解算系統由圖像檢測及解算模塊（下文簡稱解算模塊）、高清檢測相機、安裝支架、LED目標靶板組成，其系統架構框圖如圖1所示。

目標靶板正面安裝3×3排布（中心LED燈點去除，共8個燈點，中心安裝MEMs陀螺儀）的近紅外LED燈點陣列，解算模塊通過燈點陣列圖像解算出姿態信息。MEMs陀螺儀用以與圖像解算姿態角進行數據融合，提高姿態測量頻率。

為確保成像系統能覆蓋（20±10）cm距離范圍內頭盔的絕大多數機動動作，設計了雙相機成像結構，解決在頭盔偏移或偏轉過大時可能遇到無法檢測全部燈點的問題。雙相機接收來自解算模塊的外同步觸發信號，同時曝光并將圖像數據傳輸至解算模塊。

解算模塊是整個模塊的核心處理單元，負責采集陀螺數據和圖像數據，解算圖像姿態信息及融合處理各數據源，也是軟件調度的執行主體，可提供外同步觸發信號同時采集雙相機圖像數據，并將融合的姿態數據通過串口實時發送至控制終端。

2 "工作模式設計

姿態解算系統工作模式分為正常模式、測試模式。根據需求切換工作模式，開機默認正常模式，如需單獨輸出圖像解算姿態數據和陀螺數據，可切換到測試模式。

姿態解算系統正常模式工作流程如圖2所示。

姿態解算系統正常模式上電后首先開始零位標定階段（駕駛員需坐在零位標定的有效范圍內），當采集到零位標定信號時（駕駛員通過按鍵或其他途徑發送），解算模塊觸發雙相機同步曝光，并采集和解算兩臺相機當前幀的姿態計算結果，將其作為兩臺相機的零位進行存儲，完成零位標定。

零位標定未完成前，模塊將保持各個姿態角度輸出為0。正常零位標定場景下，雙通道相機必須同時檢測到靶板燈點陣列，解算模塊才能解算雙相機的零位姿態計算結果；若只有單個通道相機有效檢測靶板燈點陣列，解算模塊判定該零位狀態無效，所有角度參數默認輸出0，重新進入零位標定模式，直至零位標定成功；用戶可在OSD輸出顯示中看到零位標定實時狀態，若零位狀態無效，駕駛員可切換至手動模式，不使用解算模塊輸出的姿態角信息。

零位標定完成后，進入正常姿態解算模式，姿態解算完成后，通過串口輸出姿態解算結果。

3 "硬件設計

姿態解算系統硬件部分主要包括目標靶板、相機及鏡頭、解算模塊。

3.1 "目標靶板設計

目標靶板用于點亮靶標燈點陣列，為解算模塊提供高可靠主動發光目標；再者，采集陀螺姿態數據轉發至解算模塊。目標靶板硬件原理框圖如圖3所示。

目標靶板由外部5 V供電，為MEMs陀螺、LED恒流驅動器、微控制器提供電源。

板上微處理器用于和解算模塊通信，管理數據收發，另外，接收MEMs陀螺采集的角速度信息，以及發送控制信號，控制LED亮度。微處理器可通過調節占空比參數、調節電流值完成對LED點陣亮度調節。

3.2 "高清相機設計

高清相機具備外同步功能，最高幀頻支持50 Hz（可降低圖像姿態輸出延時），敏感波段覆蓋近紅外。選取視場角可達60°的鏡頭，從而確保覆蓋駕駛員頭部活動范圍，同時輔以近紅外濾光片（940±40）nm濾除環境光干擾，提高信噪比。相機硬件框架如圖4所示。

3.3 "圖像檢測及解算模塊設計

解算模塊是整個姿態解算系統的核心處理單元，選用FPGA+多核DSP的處理架構，充分利用FPGA和DSP多核的并行處理能力，同步運行雙相機的高清圖像姿態檢測算法，確保滿足幀頻50 Hz的實時處理性能。該解算模塊中FPGA負責雙相機視頻同步采集，同時將雙路視頻輸出通過SRIO傳輸給DSP，DSP負責姿態解算及數據融合。

解算模塊硬件原理框圖如圖5所示。

FPGA作為該模塊中數據采集及交互的核心，為滿足國產化應用需求，采用了復旦微的K7芯片（JFM7K325T），具有高級串行數據傳輸功能和基于硬件GTX串行收發器，可以實現多路高速數據傳輸接口，用于并行執行大量計算和邏輯處理任務。

DSP實現圖像數據處理、姿態解算、數據融合，是該模塊的處理核心。采用飛騰的FT?M6678N，該處理器擁有8顆FT?M66x DSP內核，工作主頻高達1.25 GHz，定點運算能力可達到40 GMAC/s，浮點運算能力為20 GFLOPs，支持兩套RapidIO高速串行鏈路，每套RapidIO支持4 Lane，每Lane傳輸速率最高支持3.125 GBaud。

4 "結構設計

4.1 "目標靶板結構設計

目標靶板尺寸為40 mm×56 mm×15 mm，貼合頭盔后表面的結構安裝，工作電壓為5 V。靶板正面貼裝3×3排列的LED燈陣（中間燈點去除，8個LED，中間區域放MEMs陀螺），其橫向間隔為20.5 mm，縱向間隔為11.5 mm，陀螺姿態數據由RS 422延長線傳輸至圖像檢測板。

燈點陣列由8個近紅外LED（940±40）nm組成，其發光面為平面，在模擬車廂內昏暗環境下的成像效果如圖6所示，濾除背景噪聲帶來的圖像干擾。

4.2 "相機安裝結構方案

兩臺相機負責采集目標靶板圖像，相機自身體積較小，為25 mm×25 mm×22 mm，重量低于35 g。相機布局方式以成像系統總能采集到適于燈點檢測和姿態解算的圖像為目標。駕駛員常規活動范圍如圖7灰色區域所示，兩臺相機在[x]軸方向分別位于感興趣區域（ROI）邊緣位置，其光軸則在ROI中心處相交，從而保證ROI內運動的靶標盡可能靠近某一臺相機光軸，增強成像質量。由此計算得到相機安裝角度與[x]軸夾角為68°。又考慮到相機視場角為60°，兩臺相機的視場可以完全覆蓋ROI近端，并有50 mm的重疊視場區域。安裝方案立體圖如圖7所示，模擬安裝示意圖如圖8所示。

5 "軟件算法設計

姿態檢測軟件運行于解算模塊上，由解算模塊觸發圖像同步采集，同時接收并解析來自目標靶板發送的陀螺數據；再將采集到的數據發送至主處理器執行圖像處理算法。圖像處理算法包括圖像預處理、姿態解算和數據融合，最終將融合的姿態結果數據通過RS 422串口輸出。接收到“零位標定”指令，軟件會進入零位標定階段，零位標定完成后，存儲各通道的零位姿態角；若零位標定操作未完成，輸出的各個軸姿態角度為0。可根據工作模式選擇姿態結果輸出方式，輸出陀螺數據和圖像姿態融合結果、陀螺姿態結果、圖像姿態結果，軟件執行流程圖如圖9所示。

5.1 "雙相機同步

雙相機同步視頻采集主要由FPGA完成，FPGA發送頻率為50 Hz同步脈沖信號給兩個相機模組。相機收到同步脈沖信號后，通過Cameralink接口輸出分辨率為1 920×1 080的高清圖像數據至FPGA；FPGA軟件將采集到的雙路相機圖像數據通過SRIO接口發送至DSP的DDR3緩沖區，并觸發DSP的圖像接收中斷，準備啟動算法處理流程，雙相機同步數據采集完成。

5.2 "姿態解算

算法部分負責分析和處理采集的視頻信號，實現燈點檢測定位、點陣陣列識別、頭部姿態解算等功能。

近紅外燈在圖像空間可形成穩定的亮斑，采用近紅外濾光片輔助成像，去除環境雜波干擾，有效提高了亮斑與背景的對比度，通過求取亮斑成像區域形心坐標即可精確定位燈點坐標。

利用各陣列燈點相對位置信息，建立燈點排序數學模型，計算各燈點與其他燈點的歐氏距離及向量方向，引入角度約束條件，準確給定各目標點編號。

考慮平面景物深度不變性，燈點陣列成像模型是一個弱透視模型，可通過最小二乘法擬合估計出成像位置與燈點世界坐標的線性映射關系，利用該映射關系計算出目標姿態信息。

5.3 "雙通道同步數據融合

系統初始化及零位標定完成后，將進入正常工作模式，此時兩臺相機同步采集目標圖像信息，每一幀圖像都會獲得來自左右相機數據的兩組姿態處理結果。

根據左右相機提供的圖像數據，分別進行解算的姿態結果存在較大的誤差，需對左右相機解算結果的方位、俯仰姿態分別進行分段擬合，擬合方式如下。

方位數據擬合：俯仰數據從20°～30°，按照步進2°，在同一俯仰值下記錄方位數據，對方位數據進行擬合，每組擬合系數用于該俯仰值（1°，-1°）區間內的擬合。

俯仰數據擬合：方位數據從-50°～50°，按照步進2°，在同一方位值下記錄俯仰數據，對俯仰數據進行擬合，每組擬合系數用于該方位值（1°，-1°）區間內的擬合。

擬合前后數據誤差對比如表1所示。

5.4 "非同源數據融合

基于單幀圖像的姿態解算相比傳統MEMs光電陀螺技術的一大優勢在于有效避免了累積誤差，因此可以使用該方式作為MEMs的有力補充，不斷為MEMs提供校準信息。從而在保持MEMs技術高數據刷新率的前提下，消除其加速運動引起的[g]敏感誤差和多幀積分引起的累積誤差。非同源數據融合流程圖如圖10所示。

以單幀圖像傳輸時間（1 f）和處理時間（1 f）計算，可以認為當前幀解算結果對應2 f圖像周期以前的姿態數據。作為對比，MEMs陀螺提供的姿態數據被認為是實時的，因此設計以下融合方式，對于第[i]個陀螺周期時刻的姿態融合結果[Pi]，有：

[Pi=PT+Tn*j=TT+iwj]

式中：[PT]為圖像數據源最近時刻[T]的解算結果；[wj]表示從MEMs處讀取的第[j]個角速度值；[Tn]表示陀螺數據輸出周期。以這種方式，基于當前解算的圖像姿態對其2 f前的陀螺姿態數據進行校正，并累加陀螺在該時刻后的角度，融合數據將只包含最近約2 f圖像時間范圍內的累積誤差和[g]敏感性誤差，達到最大限度消除誤差的效果。本設計采用50 f/s的相機更新圖像姿態檢測參數（20 ms/f），則對應的融合數據只包含最近40～60 ms內陀螺的累積誤差。

6 "關鍵指標分析及對比驗證

影響姿態檢測精度結果的因素主要有兩方面：一是累積誤差；二是燈點陣列定位準確度。

MEMs陀螺在頭部線加速運動過程中會引入[g]敏感性誤差，長時間工作又會引入累積誤差。而對于基于單幀圖像的姿態解算技術，則免于這兩類誤差，圖像姿態解算算法顯式地將平移與旋轉獨立計算，避免了兩者的互相干擾，此外由于各幀獨立解算，其誤差統計特性基本是時間不變的。

考慮到圖像數據的傳輸和處理延時，對于目標在某一時刻的特定運動狀態，圖像姿態解算數據相比陀螺姿態輸出數據，有2 f圖像周期的延遲。因此，采用的融合方法是用當前圖像姿態解算結果對陀螺姿態歷史數據進行校正，而只引入最近2 f圖像周期時間對應的陀螺角速度周期作為實時采集量。用這種方式，融合數據輸出結果只包含最近2 f圖像周期時間陀螺的[g]敏感性誤差和累積誤差，對其結果質量的改善顯然是大有幫助的。

因此，來自兩個傳感器源的數據經過融合后，利用圖像方法姿態解算結果對傳統的MEMs陀螺儀姿態數據進行不斷校正，可以在保留MEMs陀螺儀的角速度測量動態范圍大、低延遲、刷新率高的優勢的同時，消除MEMs陀螺在頭部線加速階段引入的[g]敏感性誤差和長時間工作導致的累積誤差。

為確保燈點陣列定位準確，選用了經紅外波段的發光陣列，大大減少了環境光的影響，燈點陣列自身可根據環境光線強弱，實時調整燈點亮度，使其在相機端成像一致性提高，同時通過縮小燈點發光面準確定位燈點形心，增加了燈點陣列定位精度。

通過搭建模擬實驗臺，利用標準轉臺轉動角度作為真值，實測數據如表2所示。

為驗證基于國產DSP多傳感器融合姿態解算系統的各方面優勢，使用單一傳感器（相機）、可見光波段發光陣列在同一環境下進行對比實驗，對比結果如表3所示。

7 "結 "語

本文提出了以圖像傳感器和慣性傳感器相結合的姿態解算方法，利用兩種傳感器的優勢互補，解決了慣性傳感器的累積誤差、圖像傳感器的實時性問題；同時輔以主動近紅外發光式靶板，減少環境光的干擾，大幅增加了姿態解算的準確率。

注：本文通訊作者為胡楷。

參考文獻

[1] 張毅.多姿態人臉識別關鍵技術研究[D].成都：四川大學，2021.

[2] 韓俊杰，駱開慶，邱健，等.基于雙目相機的眼動儀頭部姿態估計方法[J].激光與光電子學進展，2021，58（14）：310?317.

[3] 黎佩馨，方向忠.基于頭部姿態估計的多人低頭跟蹤檢測系統[J].信息技術，2021，45（5）：68?73.

[4] 代歡歡，譚生祥，張旺.頭部姿態估計方法研究綜述[J].現代計算機，2021，27（7）：130?134.

[5] 黃鳳鳴，費強，劉再興.基于多功能傳感器的測量系統設計[J].電子與封裝，2021，21（3）：95?98.

[6] 魏旋旋.基于慣性傳感器的人體姿態識別算法[J].智能計算機與應用，2022，12（6）：97?101.

[7] 吳琦琦，李海霞.基于多面部特征的疲勞檢測方法研究[J].信息技術與信息化，2022（10）：99?102.

[8] 呂璐，程虎，朱鴻泰，等.基于深度學習的目標檢測研究與應用綜述[J].電子與封裝，2022，22（1）：72?80.

[9] 徐飛易.基于單目視頻的三維人體姿態估計研究[D].南京：南京郵電大學，2023.

[10] 王振宇，呂勇，廖健熙，等.基于多源信息源融合的疲勞檢測系統[J].電子設計工程，2022，30（19）：125?129.

[11] 劉臻.基于單目識別技術的頭部姿態估計研究[D].武漢：武漢理工大學，2019.

[12] 王正行，曾令將.基于飛騰M6678的向量數學庫優化技術研究[J].艦船電子工程，2021，41（3）：102?106.

[13] 王月新，劉明君.基于DSP+FPGA線性結構的計算機圖像處理系統設計[J].無線互聯科技，2023，20（14）：60?64.