相对位姿测量解算的DSP实现*

冯　禹，王向军，陈文亮（天津大学微光机电教育部重点实验室，天津300072）



相对位姿测量解算的DSP实现*

冯禹，王向军*，陈文亮
（天津大学微光机电教育部重点实验室，天津300072）

摘要：针对运动目标位姿测量中小型化和快速化的问题，设计了以TMS320DM6437数字信号处理器为核心的图像处理系统，实现了图像的实时采集和处理。采用乒乓缓冲技术，利用TMS320DM6437的视频处理子系统直接对CMOS采集的原始数据进行处理，有效地减小了系统尺寸并提高了帧率。最后对系统进行了软硬件调试和运动目标的位姿测量实验，在相机帧率为40 frame/s的情况下，可以完成实时解算，表明设计方案具有一定的实用价值。

关键词：位姿解算；DSP；CMOS；乒乓操作；图像处理

运动目标的位姿参数测量是军事、航天、交通等领域重点研究的前沿方向，在航天器、卫星、无人机等载体上有着广泛的应用，是实现自主定位、自主导航、自动跟踪等功能的重要依据［1］。目前位姿测量方法从原理上可分为四大类：基于惯性导航原理，基于GPS导航系统，基于惯性导航和GPS导航组合原理以及基于计算机视觉测量系统［2］。

而以计算机视觉为理论基础的视觉测量，受益于光电检测、嵌入式系统和图像处理技术的不断成熟和完善，得到了突飞猛进的发展，能够很好满足位姿参数测量高精度、非接触、实时性强的需求［3］。本文设计了一种以DSP器件为核心的单目CMOS影像测量系统，在直接对CMOS传感器输出的原始数据（RAW RGB）进行处理的同时，将乒乓操作应用于图像传输与处理过程中。与传统的模拟相机采集或者标准化接口的数字相机采集相比，可以在满足实时性要求的前提下，减小系统整体尺寸。

1　系统组成与工作原理

系统设计的主要目的是利用达芬奇DSP的视频处理子系统（VPSS）完成对CMOS输出数据的格式转换，并通过一系列图像处理解算出位姿信息。系统主要由4部分组成：CMOS图像传感器、DSP器件、SPI通讯模块和视频输出接口，其整体结构如图1所示。

图1　系统结构框图

该系统工作原理为：①系统上电启动后，DSP通过I2C对CMOS的寄存器进行配置；②DSP的视频处理子系统（VPSS）进行图像数据（RAW RGB格式）采集，并完成CFA插值，转化为YUV格式缓存于DDR2中；③对DDR2中的数据进行图像处理，并经过DAC电路向外输出图像；④将最终的位姿解算结果通过SPI发送给MCU。

2　图像采集与显示

2.1CMOS传感器模块

MT9M034的内部结构如图2所示。

图2　MT9M034内部结构

1 280像素×960像素的像素阵列在时序控制电路驱动下，可以将外界的光信号转换为模拟信号，再通过对应的模拟信号处理电路以及AD转换模块，转换成数字图像信号由12bit并行数据口输出。数据输出所需要的同步信号由时序控制电路产生［4］。控制寄存器可以通过I2C总线进行配置，完成芯片的初始化功能及各种参数的设置［5］。

2.2图像采集与显示

选用TI公司达芬奇系列的TMS320DM6437芯片作为图像处理核心，完成图像的采集、处理与显示。DM6437在视频处理方面性能优异，其附带的视频处理子系统（VPSS）尤为突出，本系统的图像处理部分也主要在VPSS中实现。接口方面，VPSS为图像传感器等成像设备提供了输入接口：视频处理前端（VPFE）；同时也为模拟SDTV显示器、LCD面板与HDTV视频编码器等显示设备提供了输出接口：视频处理后端（VPBE）［6］。图像采集与显示系统的结构如图3所示。

图3　图像采集与显示系统

视频处理前端主要由CCD控制器（CCDC）和预览引擎图像管道（IPIPE）组成。其中CCDC主要负责从CMOS或者CCD传感器获取未处理的原始视频数据（Bayer类型），并将其传递给IPIPE。IPIPE的作用是对从传感器中取得的未处理的图像数据进行CFA插值，将Bayer格式的RGB图像数据转换成RGB图像数据，再进一步完成RGB色彩空间到YcbCr色彩空间的转换。IPIPE中4：2：2下采样模块可以将图像数据最终转变为便于传输和处理的YcbCr 4：2：2格式［7］。

视频处理后端主要由视频显示引擎（OSD）和视频编码模块（VENC）组成。OSD模块的主要作用为收集混合视频数据和显示数据，即在多个显示窗口的基础上，将其叠加，组合成一个显示帧，以YUV格式送入VENC。它支持同时显示2个视频窗口（VIDWIN0/VIDWIN1）和2个OSD窗口（OSDWIN0/OSDWIN1）。VENC模块主要负责将OSD送入的显示帧编码成系统所需要的格式输出，其既支持标准的模拟视频输出，也支持标准的数字视频输出［8］。

图像传感器MT9M034在工作之前，需要通过I2C总线完成初始化配置。经过初始化的MT9M034感光外部景物并转换为序列像素的并行数字信号（Bayer类型），配合行、场、像素时钟等同步信号传入到DM6437的视频处理子系统中，利用硬件图像处理模块对Bayer类型的图像数据进行CFA插值，转换为更方便我们进行图像处理的YUV422格式。较之于常规的图像采集方式，此方法去除了对CMOS原始彩色数据的编码和解码工作，有效地减小了系统整体尺寸。样机的实际外观图如图4所示，采集效果如图5所示。

图4　样机实际外观图

图5　图像采集效果

3　软件设计

3.1EDMA乒乓缓冲传输

增强型存储器直接访问EDMA是TMS320DM643x的一种重要的数据访问方式，可以在无CPU接入的情况下在后台完成数据传输。针对实时图像处理中较为复杂的数据传输格式以及高速数据传输的需求，本系统在软件设计中引入了EDMA［9］。

分层强夯法在湿陷性黄土地区地基处理中的应用…………………………………… 李政林，金梦菡，邱昌胜（5-46）

同时为了充分利用EDMA的事件链接功能，以及避免在高速数据传输过程中，发生数据阻塞和缓冲区读空的情况，设计中采用了乒乓缓冲技术。在CPU完成初始化后，DSP在SDRAM中分配两个Buffer，分别为PingBuf和PongBuf。当第一次触发EDMA传输后，EDMA控制器先向PingBuf中传输，当传输完预先设定的数据后进人中断服务函数，在中断服务函数内获取下一帧视频数据，然后判断乒乓操作标志位：当标志位为0时，对PingBuf中的Y（亮度）数据进行相应的图像处理，同时触发EDMA传输，向PongBuf中传输数据；当标志位为1时，对PongBuf中的Y（亮度）数据进行相应的图像处理，同时触发EDMA传输，向PingBuf中传输数据［10］。如此反复交替进行，其流程如图6所示。

图6　乒乓操作流程

3.2图像处理

为了取得更好的图像处理效果以及简化图像处理流程，本系统在镜头前增加了730 nm~750 nm波长范围的窄带滤光片，能很好地将LED靶标（波长740 nm）以外的干扰滤除。在此基础上，图像处理部分主要完成残留噪声点的去除和目标连通域重心的提取。算法流程如图7所示。

图7　图像处理流程

采集到的图像经过乒乓缓冲传输，只剩下Y（亮度）分量，所以可当作灰度图直接进行处理：①对图像进行中值滤波，从而去除孤立的噪声点，考虑到实时性的要求，系统采用1×5的一维模板；②根据预先设置的经验阈值把灰度图像转换为二值图像，由于LED靶标亮度固定且CMOS传感器的曝光时间、增益等参数可设定为固定值，所以经验阈值能有效地完成分割；③使用两次扫描（TWO-PASS）法进行连通域标记，再通过重心法计算出各连通域的中心坐标［11］；④当太阳出现在视场内的时候，连通域个数将大于4个，此时根据连通域的面积可以将干扰快速排除；⑤根据4个靶标点的中心坐标以及平面单映性解算出位姿坐标［12］。

4　系统测试

根据项目的技术指标要求：测量输出频率>25 Hz、位姿角度测量精度<±2°，在室外对样机进行了测试。

在距离样机4米处布设如图8所示的LED靶标，CMOS相机分辨率设置为900像素×900像素，帧率为40 frame/s，视场角为50°。

图8　LED靶标示意图

图像处理结果如图9所示，图9（a）为采集的灰度图像，图9（b）为二值化之后的图像。

位姿解算中，以测量精度为±0.01度的倾角传感器所测出的滚转角和俯仰角为相对真值，总共进行了约1 200帧测试。原始测量数据（每隔50帧取一帧数据）如表1所示，解算出的位姿与相对真值的对比如图10所示，由图可见滚转角和俯仰角的最大误差均不超过2°。从测试结果可以看出，在相机帧率为40 frame/s的情况下，样机可以实时地完成图像处理，并保证位姿解算的结果达到±2°的精度，达到了项目所要求的技术指标。

图9　图像处理结果

图10　解算结果与相对真值的对比

表1　四点坐标的原始数据（部分）

5　结论

本文详细介绍了一种基于DM6437的CMOS传感测量系统，通过嵌入式C语言的设计实现了对运动目标的图像采集以及位姿解算。系统的整体测试也取得了良好效果，表明此方案能很好地满足实时性和精度的要求，具有一定的实用价值。

参考文献：

［1］苗锡奎，朱枫，郝颖明.多像机非共视场的非合作飞行器位姿测量方法［J］.红外与激光工程，2013，42（3）：709-715.

［2］Legostaev V P. Russian Space Programs：Achievements and Prospects of Automatic Control Applications［J］. Annual Review in Control，2005（29）：1-11.

［3］张国云，郭龙源，吴健辉，等.计算机视觉与图像识别［M］.北京：科学出版社，2012：15-25.

［4］江志东，高太长，孙海洋，等.线阵CMOS图像采集及编码传输的实现［J］.传感技术学报，2011，24（4）：554-558.

［5］李辉，张钰，杨牧，等.基于CMOS图像传感器的皮卫星成像系统设计［J］.传感技术学报，2010，23（1）：62-67.

［6］张德刚，陈文艺，陈瑞.基于DM6437的数字摄像机［J］.电子设计工程，2011，19（8）：130-132.

［7］蒋建国，李明，齐美彬.基于TMS320DM6437的运动目标实时检测和跟踪［J］.合肥工业大学学报，2011，34（7）：1007-1010.

［8］张起贵，张刚.最新DSP技术-“达芬奇”系统、框架和组件［M］.北京：国防工业出版社，2009：10-11.

［9］刘嵩，喻胜辉，刘海华，等.基于EDMA的多通道数据传输的实现［J］.计算机测量与控制，2009，17（7）：1429-1434.

［10］魏志博，章云.基于乒乓结构的实时视频采集系统设计［J］.工业控制计算机，2013，26（5）：58-62.

［11］Yue F，Erdem A T，Tekalp A M. Tracking Visible Boundary of Ob⁃jects Using Occlusion Adaptive Motion Snake［J］. IEEE Transac⁃tions on Image Processing，2000，9（12）：2051-2060.

［12］孙子淼，王鹏，孙长库.单目视觉位姿测量方法及数字仿真［J］.天津大学学报，2011，44（5）：440-444.

冯禹（1991-），男，江苏泰州人，天津大学精密仪器与光电子工程学院硕士，研究方向为DSP的系统开发与图像处理，msjjf0530@163.com；

王向军（1955-），男，黑龙江哈尔滨人，博士，天津大学精密仪器与光电子工程学院教授，博士生导师，主要研究方向为精密测试技术与仪器、光电探测与传感技术、影像与视觉测量，xdocuxjw@vip.163.com。

Study on the Posibility of the PIG Positioning Using MEMS-based IMU*

NIU Xiaoji*，KUANG Jian，CHEN Qijin
（GNSS Research Center，Wuhan University，Wuhan 4300079，China）

Abstract：MEMS-based inertial integrated algorithm for the pipeline surveying that combines updates such as pipe⁃line odometer，ground RFID and the motion constraint（e.g. non-holonomic constraint）is proposed in this paper to meet the urgent demand of low-cost and small-size pipeline surveying system. STIM3000，a classical MEMS IMU，is used to validate the proposed solution，and the contribution to positioning accuracy improvement of odometer，motion con⁃straint and optimal smoothing algorithm is quantitatively analyzed. Simulation results show that 3-D position of meter accuracy can be obtained based on the proposed algorithm even for a 2 000-m-long pipeline，which meets the position⁃ing accuracy requirement of the pipeline surveying. Odometer measurement and motion constraint are proved to be ef⁃fective updates to guarantee the above accuracy，and smoothing algorithm can make further accuracy improvement.

Key words：INS；pipeline measurement；EKF；reverse smoothing；MEMS IMU

doi：EEACC：723010.3969/j.issn.1004-1699.2016.01.008

收稿日期：2015-08-28修改日期：2015-10-14

中图分类号：TN36

文献标识码：A

文章编号：1004-1699（2016）01-0035-05

项目来源：其他企业委托科技项目