三維景深數(shù)據(jù)采集和圖像重構(gòu)系統(tǒng)研究

吳 健，吳星明，陳偉海

（北京航空航天大學自動化科學與電氣工程學院，北京 100191）

0 引 言

移動機器人三維景深信息數(shù)據(jù)采集和圖像重構(gòu)技術是實現(xiàn)自主導航、路徑規(guī)劃、機器人自定位和地圖構(gòu)建的基礎[1]。對移動機器人來說，三維景深信息的處理直接影響到機器人的智能化程度和自主水平[2-3]。

德國學者Wulf[4]提出利用電機驅(qū)動激光測距儀運動來增加第三維方法得到了機器人小車周圍環(huán)境三維重構(gòu)圖。Christian Brenneke[5]也提出在室外環(huán)境利用三維激光掃描數(shù)據(jù)來實現(xiàn)定位與圖形繪制的方法。David M.Cole[6]提出了利用激光掃描數(shù)據(jù)對室外環(huán)境進行三維定位與圖形繪制的方法。

國內(nèi)卿慧玲[7]等也提出了利用二維激光測距儀得到三維重構(gòu)的方法。施華[8]等提出了采用RT-Linux操作系統(tǒng)改進機器人控制實時性效果的方法。

該文提出了另一種系統(tǒng)結(jié)構(gòu)，一個2D的激光測距儀與一個旋轉(zhuǎn)的六棱柱獲取距離數(shù)據(jù)，DSP采集距離數(shù)據(jù)與電機角度數(shù)據(jù)，使用ARM與嵌入式Linux系統(tǒng)來對采集到的距離數(shù)據(jù)與電機角度數(shù)據(jù)進行處理并顯示。

1 三維景深數(shù)據(jù)處理系統(tǒng)硬件設計

1.1 三維激光景深數(shù)據(jù)采集系統(tǒng)

項目采用的三維景深數(shù)據(jù)采集系統(tǒng)由二維激光測距儀LMS291、一個不斷旋轉(zhuǎn)的六棱柱和一塊DSP板卡構(gòu)成。在這套系統(tǒng)中，激光源LMS291被垂直擺放并固定，通過電機旋轉(zhuǎn)光源前的六棱柱來獲取三維景深數(shù)據(jù)。同傳統(tǒng)的三維數(shù)據(jù)采集系統(tǒng)相比，由于該系統(tǒng)的光源是固定的，避免了光源移動所帶來的誤差，因此獲得的數(shù)據(jù)更精準，而且數(shù)據(jù)量大幅增加。

通過LMS291得到的數(shù)據(jù)只是一些二維的距離數(shù)據(jù)，如果想得到帶有三維特征的景深數(shù)據(jù)必須要得到六棱柱的位置信息也就是電機的旋轉(zhuǎn)角度。項目中采用了TI公司的專門用于電機控制的TMS320F2812作為核心芯片獲得二維的景深數(shù)據(jù)和電機角度數(shù)據(jù)。該芯片具有高達150 MHz的工作主頻和豐富的外設資源，片上集成了串口模塊、PWM輸出和碼盤檢測的EV模塊、AD采樣模塊等，驅(qū)動電路采用3A H橋驅(qū)動芯片LMD18200。2812的事件管理器模塊通過驅(qū)動放大電路控制直流有刷電機運動帶動六棱鏡旋轉(zhuǎn)從而獲得三維景深數(shù)據(jù)，控制電機選用瑞士Maxon RE40直流有刷電機，功率為150W，完全滿足系統(tǒng)的需求，光電碼盤采用HEDS 5540，準確度為500線，經(jīng)DSP4倍頻后為2 000線，基本能滿足系統(tǒng)的精度要求。

1.2 三維景深圖形重構(gòu)系統(tǒng)

DSP2812獲得的數(shù)據(jù)只是具有三維特征的二維景深數(shù)據(jù)，如果想要對三維圖形進行重構(gòu)必須得到具有三維坐標的景深數(shù)據(jù)，并且三維圖形的顯示需要有一個顯示器。根據(jù)這些需求系統(tǒng)采用了Intel公司的XScale，采用的是ARM V5TE架構(gòu)。主要負責整個系統(tǒng)的調(diào)度、圖形的顯示、人機接口以及三維坐標的轉(zhuǎn)換。ARM進行三維坐標的轉(zhuǎn)換需要得到原始的二維景深數(shù)據(jù)和電機角度數(shù)據(jù)，因此ARM與DSP2812之間的通信問題也是需要仔細考慮的。項目中采用了64 KB DPRAM（Dual-port static RAM）CY7C028進行ARM與2812之間的通信。

2 三維景深數(shù)據(jù)處理系統(tǒng)軟件設計

2.1 三維激光景深數(shù)據(jù)采集系統(tǒng)

這部分的軟件代碼主要考慮的問題是LMS291與DSP2812之間數(shù)據(jù)通信協(xié)議的制定以及DSP2812向雙口RAM發(fā)送數(shù)據(jù)程序的編寫。綜合考慮數(shù)據(jù)精確度和數(shù)據(jù)顯示實時性2個因素，最終設定了LMS291的工作模式為MM模式，測量距離為8192mm，測量角度范圍為100°，測量點的角度分辨率為1°，完成一次線掃描的周期為13.33ms。制定的協(xié)議如圖1所示。

圖1 數(shù)據(jù)通信協(xié)議

DSP2812得到的數(shù)據(jù)需要傳輸給ARM做進一步的坐標轉(zhuǎn)換和顯示工作，這里采用了64KB DPRAM（Dual-port static RAM）CY7C028作為DSP2812與ARM之間通信的接口。為了提高實時性，采用pingpong傳輸機制，將雙口RAM分成2個分區(qū)，每個分區(qū)存儲一幀也就是一個鏡面掃描的數(shù)據(jù)，在2812寫一個分區(qū)時，ARM讀另一個分區(qū)的數(shù)據(jù)。這樣就有效地提高了數(shù)據(jù)傳輸?shù)男剩⑶冶苊饬擞捎诓煌珻PU的讀寫時序不同所帶來的數(shù)據(jù)錯誤等問題。

DSP2812對雙口RAM寫數(shù)據(jù)涉及到2812對外擴RAM的操作以及對數(shù)據(jù)位置的精確控制，也就是說2812在寫數(shù)據(jù)到雙口RAM時，必須保證每個分區(qū)的首地址是六棱鏡初始位置對應的電機角度，這樣才能保證雙口RAM每個分區(qū)內(nèi)保存的內(nèi)容是一幀完整的畫面所對應的數(shù)據(jù)。在2812中，對外部器件的讀、寫訪問都是通過外部接口模塊XINTF來實現(xiàn)的[9]，XINTF是一個非多路復用的異步總線接口，它可以將2812的外部接口分為5個存儲器映射區(qū)。其中XZCS6區(qū)對應的內(nèi)存空間大小為512KB，而使用的雙口RAM是64 KB的CY7C028，因此選用XZCS6分區(qū)的前64 KB的內(nèi)存空間作為雙口RAM的映射地址。2812對雙口RAM寫數(shù)據(jù)的程序流程圖如圖2所示。

由于ARM上使用了Linux操作系統(tǒng)，在Linux中，所有的硬件設備都像常規(guī)文件一樣看待，它們可以使用和操作文件相同的、標準的系統(tǒng)調(diào)用來進行打開、關閉和讀寫。用戶程序通過相應的系統(tǒng)調(diào)用來訪問硬件設備，設備驅(qū)動程序的作用正是為這些系統(tǒng)調(diào)用提供接口。根據(jù)功能上的要求，編寫了雙口RAM的驅(qū)動程序。實現(xiàn)的主要功能如下：（1）將雙口RAM的內(nèi)存空間分為2個分區(qū)；（2）實現(xiàn)用戶空間讀取雙口RAM內(nèi)存地址中的數(shù)據(jù)；（3）選擇不同的分區(qū)。

圖2 雙口RAM寫數(shù)據(jù)程序流程圖

2.2 三維景深圖形重構(gòu)系統(tǒng)

這部分所要完成的功能主要包括三維景深數(shù)據(jù)直角坐標系的轉(zhuǎn)換和三維圖形的顯示。根據(jù)功能的需求采用了XScale作為主控制器，使用Linux2.6.9作為操作系統(tǒng)，使用QtE與TinyGL相結(jié)合的方法編寫了上層應用程序。Qt/E（Qt/Embedded）是用于嵌入式Linux系統(tǒng)的Qt版本。Qt/E去掉了X Lib的依賴而直接工作于Frame Buffer上，因而效率更高，但它并不是Qt的子集，而應該是超集，部分機制（如QCOP等）不能用于Qt/X11中。OpenGL是一套繪圖函數(shù)的標準，TinyGL是OpenGL的程序庫。

首先ARM要從DSP中得到原始的距離數(shù)據(jù)和電機角度數(shù)據(jù)。前文提到，為了提高效率，該文將雙口RAM分成了2個分區(qū)并采用了一種ping-pong傳輸?shù)臋C制。在DSP里是對數(shù)據(jù)進行判斷并直接寫雙口RAM的內(nèi)存地址實現(xiàn)的，在ARM里由于使用了Linux操作系統(tǒng)，不能直接操作硬件，因此編寫了雙口RAM的驅(qū)動。在應用程序中，由于使用的是多線程的編程方式，為了保證共享數(shù)據(jù)的完整性，使用了互斥鎖來對2個分區(qū)進行讀取操作，并得到了很好的效果。

如果想顯示三維的數(shù)據(jù)，必須要得到每個掃描點在直角坐標系下的坐標。這樣就需要一個將二維的距離數(shù)據(jù)和電機角度數(shù)據(jù)轉(zhuǎn)換成直角坐標系下的三維坐標的程序。根據(jù)激光源和六棱柱的幾何關系推導出了三維直角坐標系的坐標轉(zhuǎn)換公式[10]：根據(jù)這一公式在ARM上編寫了坐標轉(zhuǎn)換程序。可以看到，這個公式具有大量的三角函數(shù)運算，而使用的主控制芯片是Intel的XScale，這款新型高性能、低功耗的微構(gòu)架兼容ARM V5TE指令集，不過不支持浮點指令集。這是為了節(jié)省處理器芯片體積和降低運行功耗，XScale體系結(jié)構(gòu)沒有實現(xiàn)昂貴的浮點運算部件和除法部件，這些是嵌入式應用中不常用的運算。當需要這類運算時，要通過軟件方法實現(xiàn)，即在上位機交叉編譯ARM上運行的程序時，使用支持浮點運算的交叉編譯工具鏈交叉編譯程序，這樣使用了浮點運算功能的程序才能在ARM上正常地運行。但畢竟ARM不是專門用來做運算的芯片，這種方法的效率與使用浮點運算部件做運算的效率相比是比較低的，需要實時顯示三維數(shù)據(jù)，因此提高計算的效率是十分必要的。這里將正弦函數(shù)第一象限的函數(shù)值制作一個函數(shù)表，在使用到三角函數(shù)時使用直接查表的方法，并根據(jù)不同象限下的計算公式在程序中進行相應的處理，這樣就節(jié)省了大量的運算時間，從而使系統(tǒng)的實時性能得到了很大的提高。優(yōu)化前后延遲時間比較如表1所示。

表1 優(yōu)化前后延遲時間比較

經(jīng)過坐標變換得到的數(shù)據(jù)是原始的三維景深信息，這種信息沒有顏色、紋理的渲染，很難識別，給景深圖像的處理增加了難度。在計算機圖形學中，多邊形表面模型因其具有簡單、靈活等特性，是最常用的一種三維物體模型表示方法[11]，而三角形表面模型是多邊形表面模型中最基本的表示形式。顯示的每幀畫面都是有很多個小三角形拼接而成的，而平面的法向量能夠體現(xiàn)出不同平面的位置關系，根據(jù)這一特點按照每個平面法向量的不同值對各個點進行著色，這樣就能有效地將不同平面的特征表現(xiàn)出來。

在程序?qū)崟r運行時，為了能夠使畫面連續(xù)顯示，使用了Qt中的定時器功能，每隔1s讀取一次雙口RAM中的數(shù)據(jù)，并進行一次窗口重繪，這樣就能將重構(gòu)后的三維圖像連續(xù)的顯示出來。

3 實驗與分析

選定了如圖3所示的實驗場景，在走廊的拐角站定一個人，讓系統(tǒng)掃描前方的空間，三維激光測距儀獲取到景深信息以后，在嵌入式平臺上重構(gòu)圖像，通過嵌入式控制平臺的LCD可以觀察到重構(gòu)的圖像，通過Qt的界面實現(xiàn)不同工作模式的切換功能。重構(gòu)后的三維景深圖像點云圖如圖4所示。三維景深圖像三角網(wǎng)格圖如圖5所示。

可以看出，三維景深重構(gòu)圖像被實時處理，并做了顏色的渲染。從圖像中可以清晰地分辨出兩面墻壁、地面、右面墻壁的門以及站立的人像。

圖3 實驗場景拐角的走廊

圖4 三維場景重構(gòu)點云圖

圖5 三維場景重構(gòu)三角網(wǎng)格圖

4 結(jié)束語

為了能夠?qū)崟r地采集三維景深數(shù)據(jù)，該文設計了以TMS320F2812為核心芯片的數(shù)據(jù)采集卡，數(shù)據(jù)采集卡與數(shù)據(jù)處理和圖像重構(gòu)模塊之間的通信使用雙口RAM并采用了一種ping-pong傳輸?shù)臋C制，有效地提高了傳輸速率。圖像重構(gòu)模塊使用TinyGL對原始圖像進行了著色，并使用歸類算法對重構(gòu)圖像中特征比較明顯的平面進行了歸類。實驗結(jié)果證明算法是可行的。

[1]Liu Y，Williams II，Robert L，et al.Integrated control and navigation for omni-directional mobile robot based on trajectory linearization [C]∥ Proceedings of the 2007 American Control Conference，ACC.2007：2153-2158.

[2]周慶瑞，原魁，劉俊承.移動機器人視覺導航控制研究[J].計算機工程與應用，2005（15）：5-8.

[3]陳光偉，向中凡，齊彥龍.基于DSP的全自主機器人運動控制系統(tǒng)的研究[J].西華大學學報：自然科學版，2009，28（6）：29-31.

[4]Wulf O，Wagner B.Fast 3D Scanning methods for laser measurement systems[C]∥14th International Conference on Control Systems and Computer Science Bucharest：Editura Politehnica Press，2003.

[5]Brenneke C，Wulf O，Wagner B.Using 3D laser range data for SLAM in outdoor environments[C]∥IEEE/RSJ International conference on Intelligent Robots and Systems，2003.

[6]Cole D M，Newman P M.Using laser range data for 3D SLAM in outdoor environments[C]∥Robotics and Automation，IEEE International Conference，2006.

[7]卿慧玲，蔡自興.移動機器人激光雷達數(shù)據(jù)的三維可視化方法[J].吉林大學學報：信息科學版，2004，22（4）：411-414.

[8]施華，陳一民.基于RT-Linux的實時機器人控制器研究[J].計算機工程與科學，2002，24（6）：91-95.

[9]李志軍，宋江鵬，馬軍.TMS320F2812慢速外設接口的時序控制[J].單片機及嵌入式系統(tǒng)應用，2005（11）：20-22.

[10]Rong L X，Chen W H，Yu S Q，et al.Mathematical model of coordinate transformations for 3D Depth-offield information collection system [C]∥Proceedings of the 6th IEEE InternationalConference on Industrial Informatics.Daejeon，2008：80-85.

[11]Kim J，Lee S，Kobbelt L.View-dependent streaming of progressive meshes[C]∥Proc of the Shape Modeling International Conference，2004：209-220.