基于国产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.