2012年3月14日 星期三

自製低成本3D Laser掃描測距儀(3D雷射雷達)

改良作者: 自製低成本3D鐳射掃描測距儀(3D雷射雷達),第一部分

改良作者: 自製低成本3D鐳射掃描測距儀(3D雷射雷達),第二部分

作者其它作品:自製的低成本鐳射投射鍵盤

LINK
http://nuigroup.com/touchlib/
http://www.tuio.org/

1. 設備設計
核心元件原型
在第一篇文章的原理介紹[1]中,已經大致提到了本次製作的核心元件:攝像頭、雷射器以及進行掃描的伺服電機的 選型要求。

對於我期望的精度和性能,一般市面常見的USB VGA攝像頭即可滿足要求。

圖:本製作使用的USB攝像頭(已經拆除外殼)
對於雷射器的選擇,主要是考慮他的發射波長和功率。由於我的製作並不用像產品那樣考慮雷射器功率安全[2]問題 ,因此,採用了200mW的紅外一字線雷射器。較大功率的優勢是可以通過縮短攝像機曝光速率,從而從畫面上過濾到環 境光的干擾,同時也可以掃描較遠的距離。當然,200mW的雷射器功率的確有點太大了,在使用時注意不能用 眼睛直視,並且紅外雷射器人肉眼不可見,所以需要額外的當心

圖:製作所使用的紅外一字線雷射器

在使用了紅外雷射器後,可以通過給攝像頭加裝紅外濾光片。它可以將肉眼可見光過濾,僅允許雷射器發出的紅外 光進入攝像頭。從而有效地過濾環境光帶來的干擾。對於紅外濾光片,最佳的選擇是使用與雷射器發射波長相匹配的濾 光片,比如如果使用的是808nm的雷射器,那麼濾光片選擇808nm的窄帶濾光片最合適,這樣做可以最大程度的降低干擾 。因為現實中,日光、白熾燈、遙控器也都會發出紅外光譜。

但是這樣的濾光片一般價格偏貴,在本製作中,我使用了800nm截至的低通濾光片。它允許任何波長低於800nm的紅 外光通過。不過實際效果還是不錯的。

圖:本製作採用的低通紅外光濾光片

對於用於掃描的伺服電機,由於攝像頭的幀率是30fps。掃描速度不需要很快,因此這裏使用了普通的標準舵機。他 的優勢是可以直接控制定位到特定角度,驅動也相對容易。不過精度不高,對於0.3度的角度定位,已經有些吃力了。 這也是值得改進的地方。

圖:本製作選用的舵機

這裏統一列出他們的參數:
攝像頭:VGA畫質的USB攝像頭,30fps (市面普遍可以購買的型號)。非廣角
雷射器:50mW 紅外一字線鐳射 808nm
濾光片:10mm直徑紅外低通濾光片
舵機:HS-322hd 43g標準舵機

安裝考慮
這裏主要針對原理[1]中提到的幾個參數的選擇,決定雷射器、攝像頭的安裝方式。在[1]中,我提到了攝像頭焦距 和攝像頭-雷射器距離(s)的乘積f*s應當滿足:

fs>=700

一般市面USB非廣角鏡頭的攝像頭的焦距在4.5mm左右,因此,s一般選擇160mm左右。也就是說,攝像頭和雷射器的 間距在160mm或者以上。當然,如果覺得這間距太大了,也可以稍微縮小,正如[1]提到的,目前攝像頭的圖元尺寸一般 比較小。

另外一個在安裝中要考慮的參數是雷射器夾角beta[1]中同樣提到他的值在83deg左右。對此,安裝的時候不必也 不能死板的測量角度並安裝,這是因為除非使用了工業精工級別的雷射器,否則雷射器發出的鐳射射線也存在夾角。

beta
角度可以在安裝完成後進行多次修正,保證在較遠處,畫面中仍然可以看到鐳射軌跡,再進行固定。

2. 機械和結構部分
    對攝像頭的改裝由於我們使用了紅外雷射器,因此需要對攝像頭做一些修改。

首先是要移除攝像頭鏡片中的紅外截止濾光片。該濾光片的作用與前文提到的紅外濾光片功能恰好相反:它將紅外 光譜過濾。一般攝像頭內都會含有這種濾光片。如果不移除,則只能夠感受到很微弱的紅外信號。(題外話,可以用拆 除截至濾光片的攝像頭觀察kinect投射出來的紅外圖案)

圖:位於攝像頭鏡頭中的紅外截止濾光片

其次就是將前面提到的紅外低通/帶通濾光片安裝到攝像頭中,這裏採用比較山寨的組裝方式:用膠布固 定在鏡頭前面,不過效果也可以接受。

圖:將紅外低通濾光片安裝在攝像頭上

製作雷射器、攝像頭的固定平臺

這裏使用了輕質的木板來安裝攝像頭和雷射器。選擇它的主要原因是容易加工。如果有條件,可以考慮使用熱縮漲 比率小的金屬或者塑膠材料來固定。

圖:使用木板作為固定攝像頭、雷射器的材料

在木板上打孔,保證雷射器和攝像頭能正好裝入,2個孔之間的距離是前文提到的160mm左右。不過不必 很精確,因為它的精確數值可以通過校正得到。這樣安裝的時候就比較省心。在給雷射器打孔的時候要注意角度的問題 。

圖:木板打孔後,將攝像頭和雷射器裝入

圖:在木板底部打孔,安裝螺絲柱,用於固定在舵機轉盤上

底座和舵機安裝

我是用了用於安裝儀器的塑膠盒子作為掃描器的底座,這類盒子可以在taobao上找到。

圖:選用的塑膠盒子和舵機

將盒子頂板開孔,使得舵機能夠放進去:


圖:將盒子開孔

圖:將舵機嵌入盒子頂板

最後將之前的攝像頭固定支架安裝於舵機上:

3. 電子系統製作

這裏的電子系統主要功能是從PC接受指令並控制舵機轉到指定角度的。由於攝像頭已經是USB介面了,所以這裏的電 子系統不用去處理攝像頭的信號。

目前PC外設使用USB幾乎已經成為了一種標準,為了使得設備使用盡可能方便,如果能做到免驅動,那就更加完美了 。

這裏我才用了我們RoboPeak機器人團隊設計的一款AVR開源控制版:RoboPeak Usb Connector[2]來實現這部分功能。它使用單片AVR(Atmega88)晶片通過軟 件方式類比出USB協定棧,並且使用了HID協定使得無需在PC上安裝驅動程式。對於這部分的細節屬於固件實現部分,將 在後文提到。

: 我們RoboPeak團隊的開源USB方案:RP USB Connector(藍色PCB

這裏列出本製作中電子系統所實現的功能

實現了一個HID類的USB設備,無需第三方驅動程式支援

支援通過USB接受指令,控制舵機運轉到指定角度

可通過USB指令,開啟/關閉雷射器

通過一個雙米字LED顯示幕,顯示當前舵機的角度

支援用戶通過設備上按鈕手工設置舵機角度

對於USB設備的實現、接收處理USB指令等屬於固件範疇的職責,這裏將不做討論。這裏使用了RP USB Connector, 因此其固件已經支援了此部分的基礎操作,也可以在他的介紹頁面[2]查看詳情。

米字LED數碼管的驅動

由於2個米字形LED含有16多個獨立的LED需要獨立控制,RP USB Connector上並沒有如此多空閒IO口。因此這裏的做 法是很傳統的採用74595串列轉並行輸出晶片。RP USB Connector上通過SPI匯流排進行控制。

圖:使用274595晶片驅動雙米字LED數碼管

擴充RP Usb Connector
 由於RP Usb Connector主要功能是做為AVR晶片編程器和通用USB開發版的,上面並沒有舵機控制和鐳射 器控制功能(需要三極管擴流)。因此需要做一塊額外的電路擴充它的功能。電路比較簡單,這裏就不介紹了,可以參 考本章節模組給出的電路圖。

圖:在RP USB Connector基礎上做擴充電路

製作角度手工控制面板
這部分做的比較粗糙,主要就是在塑膠盒子外邊加裝按鈕,就不多介紹了,給一些製作的照片。

圖:按鈕(正面)

圖:按鈕(背面)

總裝
最後將所有線路連接起來,並關閉盒子,設備的安裝就大功告成。

 電子系統電路原理圖:

RoboPeak USB Connector電路圖:

這部分請訪問位於google code託管的開源項目:http://code.google.com/p/rp-usb-connector/

擴展電路/LED驅動電路原理圖:

目前本製作的所有源代碼和電路也已經託管於Googe code, 請訪問如下位址下載:

http://code.google.com/p/rp-3d- scanner

4. 固件以及PC通訊
這裏的固件自然就是運行在RoboPeak USB Connector AVR晶片上的固件代碼了。這裏只大致列出實現的思路和原理 。具體的細節以及基礎知識不做介紹。源代碼可以在本製作的google code專案頁面[3]上找到。

固件實現的功能

如前文所述,固件實現了:

     1.     通過軟體方式類比出USB1.1協定棧(使用了v-usb[4])

2.     實現了HID(Human Input Device)類的USB設備進行通訊,在PC上無需外驅動

3.     控制舵機角度

4.     驅動LED數碼管顯示

同時,固件代碼採用了我之前寫的Arduino-Lite[5]羽量級AVR固件庫。因此如果需要使用這裏的固件代碼,請在 google code上下載並配置Arduino-Lite[6]

接下來我將跳出一些典型問題介紹

HID-USB
設備的類比及與PC通訊

RoboPeak USB Connector
使用的是不含有USB介面的AVR晶片。因此,USB通訊支援是採用軟體方式進行的。索性目前 v-usb開源庫[4]已經提供了很優秀的封裝。

軟體類比usb的優勢在於可以降低設備成本(帶有USBAVR晶片成本較高),缺點是穩定性和速率上不如硬體實現。 對於本製作,穩定性和速度並不關鍵。

在使用USBPC通訊時有一個煩惱的問題是驅動。直接為系統編寫驅動程式會提升本製作的難度,同時對於64位元 Windows系統,微軟要求驅動程式進行簽名。目前也有一種可以在用戶態實現usb驅動的方案:libusb。但是,同樣在64 位元Windows系統上,這個做法就不管用了。

這裏採用了另一種思路:利用那些OS自帶驅動支援的USB類設備。(USB協定中預先定義了幾類USB設備的種類,比如 Mass Storage就是平時用的移動硬碟、UVC設備常用於實現攝像頭、HID設備用於實現鍵盤滑鼠。OS往往會自帶驅動支援 這類設備)。這裏我也使用了HID類設備。理由如下:

1.     協定比較簡單,通訊單位也是資料包

2.     支援USB1.1低速率規範,可以用v-usb實現

3.     WindowsLinuxMac上均可以通過OS提供的用戶態APIHID設備通訊


採用了HID設備後,及時在電腦上第一次使用本掃描器,OS也能直接識別,不會要求安裝驅動或者配置檔。

圖:本掃描器在PC上識別為HID設備

接下來的問題是如何利用HID設備進行通訊了。這裏的做法千差萬別。為了提高通訊品質,這裏我使用 了帶有checksum交驗的,基於資料包的通訊協定,將它運行在HID協定之上。因為本固件是基於RP USB Connector固件 的緣故,我直接採用了用於AVR晶片燒錄器的STK500v2協議。

圖:本製作的通訊構架
舵機驅動邏輯
AVR上有很多舵機驅動庫,這裏我使用的是RoboPeakArduino自帶舵機庫的修改版本,該版本使用 Arduino-Lite進行了重寫,精度上也較高。但這部分不影響具體效果,因此就不多介紹了。
數碼管驅動邏輯
RP USB Connector上引出了SPI匯流排,因此很自然的我使用SPI協定驅動74595晶片,直接通過AVR控制 數碼管的每個LED,從而支援顯示任意的圖案。

要注意的是,由於雙米字管的每個字元上相同位置的LED公用信號線,因此在驅動上需要採用傳統的掃 描方式,輪流點亮LED

5. 圖像處理和渲染
    圖像處理部分就是第一篇文章[1]所介紹的幾個步驟:

1.     獲取攝像頭原始畫面

2.     通過攝像頭校正參數,消除畫面扭曲

3.     提取和識別鐳射光斑的位置

4.     通過鐳射光斑的位置,代入距離求解公式,算出對應點真實距離

本製作使用了OpenCV庫簡化圖像計算的開發難度。同時這樣也有利於代碼的跨平臺移植。對於攝像頭的校正,會在 後文介紹。

這部分的細節已經在第一篇文章中詳細介紹了,因此這裏給出程式運作中的截圖作為示意:

圖:對攝像頭畫面進行扭曲校正並識別鐳射點的過程

由於採用了紅外雷射器,在加裝濾光片後,背景光干擾被有效的去處。畫面上除了鐳射光斑外,幾乎看不到別的內 容。紅外鐳射在攝像頭中以偏紫色的色彩顯示。

上圖中顯示了畫面中心點的鐳射中心座標值。這樣做的目的是用於後期進行測距參數的校正。而使用中心點的原因 在第一篇文章中已經介紹。

渲染點雲

在視頻中看到了即時掃描並顯示點雲的畫面。在明白了掃描原理後,這點就沒什麼特別的了。基本上都是基本的3D 渲染得技巧。我使用了Irrlicht[7]開源3D引擎簡化了這部分的實現工作。

除了自行編寫軟體外,也有很多工具可以用來查看3D點雲。比如開源的MeshLab[8]Blender[9]以及Matlab。在後 文我將給出可以在Meshlab中觀看的點雲資料。

要產生可被這類軟體讀取的檔很容易,像MeshLab支援如下格式的文本點雲資料

x1,y1,z1

x2,y2,z2



xn,yn,zn

6. 校正
當完成所有部分製作和PC端軟體後,就可以進行校正工作了。校正主要分為:相機校正和測距校正。

相機校正
我使用自己列印的Chessboard圖案,分別在不同距離和位置下拍攝了不同畫面。校正工具使用了matlabCamera Calibration Toolbox。它的資訊請參考第一篇文章的參考文獻[10]OpenCV也包含了相機校正功能,也可以直接使用 。

圖:用棋盤圖在不同位置下拍攝畫面

圖:使用matlab進行校正時識別出的Inner Corner

圖:校正計算的外部參數(對本製作無用)

在完成校正後,將得到如下的校正參數:

Calibration results (with uncertainties):

Focal Length:          fc = [ 935.44200   929.73860 ] ? [ 11.29945   10.64268 ]
Principal point:       cc = [ 149.00014   233.25474 ] ? [ 17.13538   11.11605 ]
Skew:             alpha_c = [ 0.00000 ] ? [ 0.00000  ]   => angle of pixel axes = 90.00000 ? 0.00000 degrees
Distortion:            kc = [ - 0.13196   -0.05787   -0.00358   -0.01149  0.00000 ] ? [ 0.04542   0.12717   0.00195   0.00565  0.00000 ]
Pixel error:          err = [ 0.24198   0.25338 ]

Note: The numerical errors are approximately three times the standard deviations (for reference).

對於這裏的應用,之需要關心Focal LengthPrincipal pointDistortion的幾組參數。可以使用OpenCV cvInitUndistortMap/cvRemap讀取校正參數並完成畫面扭曲消除。
測距參數校正
需要校正的參數請參考前一篇原理文章。要開始測距校正,首先要求PC用戶端軟 件已經能夠得到鐳射光斑中心位置了。這裏給出對畫面中心位置鐳射光斑測距參數校正的過程。至於這樣做的理由,已 經在第一篇文章中詳細介紹了。其他的參數可以用相同的思路進行。

理想的校正環境是比較空曠的區域,前方有垂直的白牆用於反射鐳射光斑。並配 備高精度的測距儀器參考。我沒有這樣的條件,也沒有必要如此,因此採用了比較山寨的參考設備:卷尺。

:進行測距參數校正

上圖正是在製作本測距儀時進行校正所拍攝的照片。所需要的設備就是一把卷尺 ,當然最好能夠足夠長,有5-6m。這樣可以校正到較遠的距離。一般校正到5是必須的。

校正至少需要採集2個參數:實際的距離值,鐳射光斑中心點位置。校正採集到的資料自然是越多也好 ,但一般6個以上的點即可。

如下是本製作校正採集的資料:

Dist X
146.8 14.79
246.8 259.63
346.8 356.72
446.8 420.24
5 46.8 457.9
746.8 503.58
946.8 528.9
1146.8 546.71
1546.8 567.54
1846.8 576.87
2246.8 586.08
3046.8 597.04
4046.8 604.6
5546.8 611.9

在完成了資料獲取後,可以在matlab等工具幫助下,進行曲線擬合。擬合的曲線公式正式前一篇文章提 到的式(4)。可以看出採集的資料和理論曲線非常吻合:

圖:對校正資料進行的擬合

7. 結果和討論
本製作的結果在文章開始的時候已經給出了,簡單說,就是達到了我的預期:-)

指標如下:

絕對測距精度:1m-/+10mm與實際值的偏差,5m處最大80mm與實際值的偏差
掃描角度: 0-180
最小步進 :0.3
掃描解析度: 480 points per sample
掃描速度:30 samples per sec (180度,1度步進需時6)
成本:~150

這裏給出前面圖像和視頻中出現的我的掃描像的點雲資料,可以在MeshLab中導入察看:

http://www.csksoft.net/data/pic/3d scanner/sample/face_scan.zip

同時也給出一些額外的圖片和視頻:

圖:在Matlab中查看點雲(點擊查看原始圖像)

視頻:另一段即時掃面渲染

視頻:matlab中觀察點雲
性能分析
這裏主要看看掃描精度實現的情況。如果之前校正用的基準資料沒有任何誤差(實際不可能),鐳射光點提取演算法沒有問題,那麼實際工作時刻的誤差主要就體現在擬合的曲線與實際的函數曲線的差距。換句話說,就是擬合得到的參數於實際正確的參數(我們並不知道)。

當然,上面2個假設實際都是不成立的,不過,我們先加設他們都沒有誤差,先來分析校正曲線與實際曲線的誤差:

Dist   X      Calc        Diff
146.8  14.79  123.3103843 23.48961566
246.8  259.63 231.8752505 14.92474953
346.8  356.72 328.8075565 17.99244355
446.8  420.24 440.8004392 5.99956081
546.8  457.9  546.254414  0.545586001
746.8  503.58 758.5428124 -11.74281238
946.8  528.9  958.9149139 -12.11491386
1146.8 546.71 1172.910412 -26.11041169
1546.8 567.54 1578.227294 -31.42729368
1846.8 576.87 1862.988106 -16.18810582
2246.8 586.08 2262.96604  -16.16604012
3046.8 597.04 3030.93851  15.86149048
4046.8 604.6  3948.153444 98.64655635
5546.8 611.9  5564.223928 -17.42392847

上表是在之前校正中收集的資料基礎上得到的,其中Calc列的資料是通過校正後的擬合曲線,通過式(4)的計算得到的測距資料,而Diff就是計算得到的距離和真實距離(Dist)的差值。這裏的單位除了X列外都是毫米。

從資料上很直觀的可以看到在4046mm處進行測距時,計算結果和實際值相差了98.64mm。相比這個,對於近距離的資料,誤差也比較大。對於第二個現象,在前一篇文章的文獻[3]猜測這是由於鏡頭扭曲造成的。但是,大家可能會有疑問了,我的實現不是已經做過相機校正了嗎?為何還會有鏡頭扭曲?這裏給出幾個可能的解釋

1.     相機校正結果存在誤差,造成仍舊有細微扭曲

2.     相機校正也是基於實現設計出來的數學模型,實際情況有很多其他因素均能導致畫面扭曲,但他們無法通過目前的校正修正

3.     紅外光的折射率與自然光不同,校正是針對自然光頻率範圍進行的,因此對於紅外光,扭曲依然存在

實際的情況可能是這些原因中的幾種組合。

另外,別忘了我們之前做的2個假設,實際他們也是不成立的。在得到了這個誤差表後,接下來的問題是:可否繼續校正來彌補這個誤差?前一篇文章的文獻[3]表示這也是可以做得,至少有了上面的表格後,可以在對應的距離下直接校正出正確結果。不過這樣做的有效性有待驗證。

除了測距精度外,這裏也提一下掃描的解析度。在前文中我提到過目前的舵機可以實現0.3度的角度定位精度。但實際上對於近距離物體掃描,這是不夠的。目前的設備比較適合進行大範圍掃描,這也比較符合他作為雷射雷達的用途。

8. 下一步工作
目前本製作已搞一段落。這裏說說我的製作動機和下一步打算。

其實動機在前一篇文章中已經點到,就是用於我們RoboPeak團隊的機器人,進行SLAM。這也是接下來我即將進行的事情。當然,其實能做的事情還有很多,比如:

進行多視角掃描,併合成為一個全局點雲

其實商業的掃描器都會支援這個應用。目前一次3D掃描只能採集物體的一個表面,而他的背面則無法掃描。如果對物體的背面也進行3D掃描,那就能得到完整的3D模型了。

要實現它,可以有2種辦法:

1.     旋轉物體本身,掃描器固定

2.     掃描器在不同方位掃描目標物體

1的實現和原理很簡單,2的核心問題是如何將2次掃描的點雲對應起來?除了人肉拼接外,實際上也有比較成熟的演算法,這類演算法成為Surface Registration。如ICP-Slam也是採用了這樣的演算法。

提高掃描精度和速度

更高的精度和速度永遠是對這個作品的需求,這就不多解釋了。我在原理部分也提到了改善性能的方法。

基於3D點雲進行物體識別

這個應用就很類似於Kinect。其實實現的演算法也是相同的。

這裏也提一下一個可用的工具和庫:PCL (Point Cloud Library)[10]。他也是又機器人公司WillowGarage推出的開源庫。其中包含了可以實現上述擴展的基礎庫。可以嘗試。

好了,這個系列的文章就告一個段落了,感謝能耐住性子看到這裏的朋友們:-) 如果你們願意,可以在我Blog上的本文留個言讓我知道:-) 每次寫好這類文章,都感慨寫文章要比製作複雜漫長,其中要把自己的想法轉化成大家能懂得語言,對我這個表達能力較差的人來說還是蠻辛苦的。

時隔幾個月,我blog終於迎來一次比較大的更新。希望自己也能堅持下去,不過,業餘時間的確也比以前少了些。不知道我下一篇文章是何時發佈呢?當然,我業餘時間的另一個投入就是我們RoboPeak團隊,大家也可今後在我們團隊動態中瞭解我正在做的事情:-)

目前本製作已經開源,代碼託管於google code。請訪問專案頁面下載

http://code.google.com/p/rp-3d- scanner
參考文獻
[1] 自製低成本3D鐳射掃描測距儀(3D雷射雷達),第一部分
http://www.csksoft.net/blog/post/lowcost_3d_laser_ranger_1.html

[2]: Driverless USB AVR/51 ISP Programmer powered by RoboPeak
http://code.google.com/p/rp-usb-connector/

[3]: 本製作在Google Code的開源專案頁面
http://code.google.com/p/rp-3d-scanner

[4]: V-USB, Virtual USB port for AVR microcontrollers
http://www.obdev.at/products/vusb/index.html

[5]: Arduino-Lite, Lightweight AVR library developed by RoboPeak
http://www.robopeak.net/blog/?p=131

[6]: Arduino-Lite Project
http://code.google.com/p/arduino-lite/

[7]: Irrlicht Engine – A free open source 3d engine
http://irrlicht.sourceforge.net/

[8]: MeshLab OpenSource Project
http://meshlab.sourceforge.net/

[9]: Blender OpenSource Project
http://www.blender.org/

[10]: PCL – Point Cloud Library
http://pointclouds.org/

沒有留言:

張貼留言

注意:只有此網誌的成員可以留言。