相對位姿測量解算的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

項目來源：其他企業委托科技項目