三維景深數據采集和圖像重構系統研究

吳 健，吳星明，陳偉海

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

0 引 言

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

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

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

該文提出了另一種系統結構，一個2D的激光測距儀與一個旋轉的六棱柱獲取距離數據，DSP采集距離數據與電機角度數據，使用ARM與嵌入式Linux系統來對采集到的距離數據與電機角度數據進行處理并顯示。

1 三維景深數據處理系統硬件設計

1.1 三維激光景深數據采集系統

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

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

1.2 三維景深圖形重構系統

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

2 三維景深數據處理系統軟件設計

2.1 三維激光景深數據采集系統

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

圖1 數據通信協議

DSP2812得到的數據需要傳輸給ARM做進一步的坐標轉換和顯示工作，這里采用了64KB DPRAM（Dual-port static RAM）CY7C028作為DSP2812與ARM之間通信的接口。為了提高實時性，采用pingpong傳輸機制，將雙口RAM分成2個分區，每個分區存儲一幀也就是一個鏡面掃描的數據，在2812寫一個分區時，ARM讀另一個分區的數據。這樣就有效地提高了數據傳輸的效率，并且避免了由于不同CPU的讀寫時序不同所帶來的數據錯誤等問題。

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

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

圖2 雙口RAM寫數據程序流程圖

2.2 三維景深圖形重構系統

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

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

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

表1 優化前后延遲時間比較

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

在程序實時運行時，為了能夠使畫面連續顯示，使用了Qt中的定時器功能，每隔1s讀取一次雙口RAM中的數據，并進行一次窗口重繪，這樣就能將重構后的三維圖像連續的顯示出來。

3 實驗與分析

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

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

圖3 實驗場景拐角的走廊

圖4 三維場景重構點云圖

圖5 三維場景重構三角網格圖

4 結束語

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

[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的全自主機器人運動控制系統的研究[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]卿慧玲，蔡自興.移動機器人激光雷達數據的三維可視化方法[J].吉林大學學報：信息科學版，2004，22（4）：411-414.

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

[9]李志軍，宋江鵬，馬軍.TMS320F2812慢速外設接口的時序控制[J].單片機及嵌入式系統應用，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.