專利名稱：一種基于移動平臺的彩色激光點云成像系統(tǒng)的制作方法
技術(shù)領(lǐng)域：
本發(fā)明涉及一種基于移動平臺的彩色激光點云成像系統(tǒng)，屬于環(huán)境感知領(lǐng)域。
背景技術(shù)：
近年來，激光掃描技術(shù)得到了很大的發(fā)展。利用激光掃描雷達(dá)可以獲得真實場景 高度精確的3D數(shù)據(jù)，這些數(shù)據(jù)最直接反映了客觀事物的真實形態(tài)特性，因此人們將它作為 快速獲取被測物體的三維點云信息的有效手段，三維點云信息包含被測物體的度量信息 (距離、幾何形狀等)；但其缺點是不能提供關(guān)于場景物體的紋理以及顏色信息，因此單純 使用三維點云信息很難實現(xiàn)物體的識別等工作。攝像機(jī)能夠快速地獲取被測物體的圖像信 息，包括顏色及二維幾何形狀信息、紋理等，使物體圖像識別較為容易，但其缺點是被測物 體空間形狀、位置等信息的精度較差。由于激光掃描雷達(dá)與攝像機(jī)各有優(yōu)缺點，在性能上互相補(bǔ)足，因此組合使用激光 與攝像機(jī)可以充分發(fā)揮兩者的優(yōu)勢，將三維點云信息與圖像信息結(jié)合起來得到彩色三維點 云，在三維場景構(gòu)建中具有重要作用。彩色三維點云包含度量信息、顏色及紋理等信息。現(xiàn)有的將激光掃描雷達(dá)與攝像機(jī)結(jié)合使用，獲取彩色三維點云的方法，可根據(jù)激 光掃描技術(shù)的不同分為被動掃描技術(shù)和主動掃描技術(shù)。在被動掃描方式下掃描時被測 物體在輔助設(shè)備的帶動下進(jìn)行旋轉(zhuǎn)及平移運(yùn)動，激光掃描雷達(dá)和攝像機(jī)朝向被測物體，激 光掃描平面與攝像機(jī)光軸成固定夾角。當(dāng)被測物體進(jìn)行旋轉(zhuǎn)及平移運(yùn)動時，激光掃描雷達(dá) 和攝像機(jī)進(jìn)行數(shù)據(jù)采集，實現(xiàn)三維點云信息與圖像信息的獲取。顯然，這種方式只適用于可 放在轉(zhuǎn)臺上進(jìn)行測量的物體，無法適用于大范圍環(huán)境感知以及不便移動的物體的測量。在 主動掃描方式下掃描時，將激光掃描雷達(dá)與攝像機(jī)固定在一起，并使二者朝向同一場景，激 光掃描雷達(dá)的線結(jié)構(gòu)光對固定場景進(jìn)行掃描，獲取三維點云信息；然后，攝像機(jī)拍攝同一視 場的彩色圖像，得到圖像信息。但是這種安裝方式，二者的光心必然處于不同位置，導(dǎo)致二 者所感知的視場不一致，最終使融合的染色結(jié)果存在誤差并且二者所感知的視場重合區(qū)域 不能達(dá)到最大。因此對于大范圍環(huán)境感知等情況，尚需要對這種主動掃描方式加以改進(jìn)。目前，已有的相關(guān)文獻(xiàn)主要集中于對激光掃描雷達(dá)掃描得到的三維點云信息與攝 像機(jī)拍攝得到的圖像信息的融合算法方面的研究，或者所提及的彩色激光點云成像系統(tǒng)只 適用于固定平臺，還未見基于移動平臺的彩色激光點云成像系統(tǒng)的構(gòu)建方案。

發(fā)明內(nèi)容
本發(fā)明的目的是克服目前已有技術(shù)存在的缺陷，提出一種基于移動平臺的彩色激 光點云成像系統(tǒng)。該系統(tǒng)分別通過激光掃描雷達(dá)和攝像機(jī)獲取三維點云信息和圖像信息， 再利用激光掃描雷達(dá)與攝像機(jī)的空間對準(zhǔn)方法及激光點云染色方法，實現(xiàn)彩色激光點云的 成像。本發(fā)明的目的是通過下述技術(shù)方案實現(xiàn)的。一種彩色激光點云成像系統(tǒng)，包括移動平臺、支撐桿、攝像機(jī)、攝像機(jī)標(biāo)定模塊、運(yùn)動控制模塊、人機(jī)交互模塊、空間對準(zhǔn)模塊、機(jī)械腕、深度圖構(gòu)建模塊、濾波器模塊、激光 掃描雷達(dá)、姿態(tài)方位參考模塊、激光點云染色模塊、；其中，機(jī)械腕包括連接軸A、連接軸B以 及連接懸臂。支撐桿與機(jī)械腕組成支撐框架，支撐桿與連接懸臂通過連接軸A和連接軸B連接，連接軸A處于豎直狀態(tài)，連接軸B處于水平狀態(tài)，連接軸A和連接軸B可繞其軸心旋轉(zhuǎn)，并 且支撐桿與連接軸A、連接軸B之間非固定連接，連接懸臂與連接軸A、連接軸B之間固定連 接，即當(dāng)連接軸A圍繞其軸心轉(zhuǎn)動時，支撐桿不隨連接軸轉(zhuǎn)動，而連接懸臂保持與連接軸A 同速轉(zhuǎn)動；當(dāng)連接軸B圍繞軸心轉(zhuǎn)動時，支撐桿不隨連接軸轉(zhuǎn)動，而連接懸臂保持與連接軸 B同速轉(zhuǎn)動。支撐框架通過支撐桿固定在移動平臺上，連接懸臂不接觸移動平臺；攝像機(jī)和 激光掃描雷達(dá)固定安裝于連接懸臂上，并且攝像機(jī)的光心和激光掃描雷達(dá)的光心，與連接 軸B的軸心的距離相等，攝像機(jī)的光心與激光掃描雷達(dá)的光心在同一豎直平面上。攝像機(jī)與人機(jī)交互模塊、攝像機(jī)標(biāo)定模塊、激光點云染色模塊、機(jī)械腕連接；攝像 機(jī)標(biāo)定模塊與攝像機(jī)、空間對準(zhǔn)模塊、激光點云染色模塊連接；運(yùn)動控制模塊與機(jī)械腕、人 機(jī)交互模塊、移動平臺連接；人機(jī)交互模塊與激光掃描雷達(dá)、攝像機(jī)、運(yùn)動控制模塊、深度圖 構(gòu)建模塊、空間對準(zhǔn)模塊、姿態(tài)方位參考模塊連接；空間對準(zhǔn)模塊與人機(jī)交互模塊、濾波器 模塊、攝像機(jī)標(biāo)定模塊、激光點云染色模塊連接；機(jī)械腕與運(yùn)動控制模塊、激光掃描雷達(dá)、攝 像機(jī)連接；深度圖構(gòu)建模塊與激光掃描雷達(dá)、人機(jī)交互模塊連接；濾波器模塊與人機(jī)交互 模塊、空間對準(zhǔn)模塊連接；激光掃描雷達(dá)與機(jī)械腕、人機(jī)交互模塊、激光點云染色模塊連接； 姿態(tài)方位參考模塊與人機(jī)交互模塊、激光點云染色模塊連接；激光點云染色模塊與激光掃 描雷達(dá)、攝像機(jī)、人機(jī)交互模塊、姿態(tài)方位參考模塊、空間對準(zhǔn)模塊、攝像機(jī)標(biāo)定模塊連接；所述攝像機(jī)用于獲取攝像機(jī)圖像數(shù)據(jù)，并分別傳送給攝像機(jī)標(biāo)定模塊、人機(jī)交互 模塊和激光點云染色模塊。所述攝像機(jī)標(biāo)定模塊接收攝像機(jī)獲取的圖像數(shù)據(jù)，經(jīng)過處理后得到攝像機(jī)投影矩 陣K，并將攝像機(jī)投影矩陣K傳送到空間對準(zhǔn)模塊和激光點云染色模塊。所述運(yùn)動控制模塊根據(jù)人機(jī)交互模塊的控制指令，驅(qū)動機(jī)械腕的轉(zhuǎn)動及移動平臺 的運(yùn)動。所述人機(jī)交互模塊的主要功能是提供人機(jī)交互的輸入、輸出功能；包括但不限 于①機(jī)械腕的轉(zhuǎn)動參數(shù)輸入、②移動平臺的運(yùn)動參數(shù)輸入、③激光掃描雷達(dá)的掃描參數(shù)輸 入、④接收并顯示攝像機(jī)獲取的圖像及深度圖構(gòu)建模塊產(chǎn)生的深度像、⑤接收姿態(tài)方 位參考模塊傳輸過來的彩色三維點云數(shù)據(jù)并顯示、⑥提供在攝像機(jī)圖像上選取特征點的功 能，并將攝像機(jī)圖像中的特征點坐標(biāo)傳送給空間對準(zhǔn)模塊；⑦提供在深度圖中選取對應(yīng)特 征點候選區(qū)域點集的功能，并將其坐標(biāo)傳送給濾波器模塊。所述特征點候選區(qū)域點集為攝 像機(jī)圖像上選取的特征點在深度圖中的對應(yīng)候選區(qū)域。所述空間對準(zhǔn)模塊接收來自人機(jī)交互模塊所傳送的攝像機(jī)圖像特征點坐標(biāo)及濾 波器模塊所傳送的與攝像機(jī)圖像特征點對應(yīng)的深度圖特征點坐標(biāo)，并接收攝像機(jī)標(biāo)定模塊 傳送的攝像機(jī)投影矩陣K，根據(jù)文獻(xiàn)《視覺傳感器與激光測距雷達(dá)空間對準(zhǔn)方法》(付夢印， 劉明陽；紅外與激光工程，2009，38(1))中公開的空間轉(zhuǎn)換方法得到攝像機(jī)坐標(biāo)系到激光 掃描雷達(dá)坐標(biāo)系的坐標(biāo)轉(zhuǎn)換矩陣，包括旋轉(zhuǎn)矩陣和平移矩陣，并將旋轉(zhuǎn)矩陣和平移矩陣傳 送給激光點云染色模塊。
所述機(jī)械腕在運(yùn)動控制模塊驅(qū)動下能夠完成以下兩種運(yùn)動①控制連接軸B圍繞其軸心旋轉(zhuǎn)，使固定在其上的激光掃描雷達(dá)在俯仰方向上轉(zhuǎn)動；②控制連接軸A圍繞其軸 心旋轉(zhuǎn)，使激光掃描雷達(dá)在水平方向上轉(zhuǎn)動。所述深度圖構(gòu)建模塊接收來自激光掃描雷達(dá)的三維點云數(shù)據(jù)，經(jīng)過處理后得到深 度圖，并將深度圖傳送給人機(jī)交互模塊。所述濾波器模塊接收來自人機(jī)交互模塊傳送的深度圖特征點候選區(qū)域點集，通過 處理得到深度圖特征點坐標(biāo)，將深度圖特征點坐標(biāo)傳送給空間對準(zhǔn)模塊。所述激光掃描雷達(dá)接收人機(jī)交互模塊傳送的激光雷達(dá)掃描參數(shù)，并在機(jī)械腕的帶 動下對場景進(jìn)行掃描，獲取三維點云數(shù)據(jù)，并將其傳送給深度圖構(gòu)建模塊和激光點云染色 模塊。所述姿態(tài)方位參考模塊用于測量激光掃描雷達(dá)在大地坐標(biāo)系下的姿態(tài)信息建立 大地坐標(biāo)系與激光掃描雷達(dá)坐標(biāo)系的轉(zhuǎn)換關(guān)系，所述激光掃描雷達(dá)姿態(tài)信息包括但不限 于俯仰角、橫滾角及方位角；然后，將激光點云染色模塊傳送的彩色三維點云數(shù)據(jù)轉(zhuǎn)換到 大地坐標(biāo)系，并將轉(zhuǎn)換后的彩色三維點云數(shù)據(jù)傳送到人機(jī)交互模塊進(jìn)行顯示。所述激光點云染色模塊完成激光點云的染色，即接收激光掃描雷達(dá)傳送的三維 點云數(shù)據(jù)和攝像機(jī)傳送的圖像數(shù)據(jù)，并接收空間對準(zhǔn)模塊所提供的旋轉(zhuǎn)矩陣和平移矩陣， 以及攝像機(jī)標(biāo)定模塊提供的攝像機(jī)投影矩陣K，找出三維點云數(shù)據(jù)在攝像機(jī)圖像數(shù)據(jù)中的 對應(yīng)點，并提取出其的顏色信息。將三維點云數(shù)據(jù)的三維位置坐標(biāo)及在圖像數(shù)據(jù)中對應(yīng)點 的三維RGB顏色信息合并為六維坐標(biāo)，得到彩色三維點云數(shù)據(jù)，并傳送給人機(jī)交互模塊顯 示，完成激光點云的染色。其工作流程為步驟一、對攝像機(jī)進(jìn)行標(biāo)定。具體為由人機(jī)交互模塊輸入機(jī)械腕轉(zhuǎn)動參數(shù)并傳遞 給運(yùn)動控制模塊，運(yùn)動控制模塊根據(jù)接收到的機(jī)械腕的轉(zhuǎn)動參數(shù)驅(qū)動機(jī)械腕轉(zhuǎn)動使攝像機(jī) 朝向正前方，采集圖像，將得到的圖像傳送到攝像機(jī)標(biāo)定模塊進(jìn)行處理，得到攝像機(jī)標(biāo)定矩 陣K，將攝像機(jī)標(biāo)定矩陣K傳送給空間對準(zhǔn)模塊及激光點云染色模塊。所述機(jī)械腕的轉(zhuǎn)動參數(shù)包括但不限于起始角度、終止角度、步進(jìn)角。所述移動平臺的運(yùn)動參數(shù)包括但不限于起動、停止、方向、速度命令。所述激光掃描雷達(dá)掃描參數(shù)包括但不限于掃描視角和掃描分辨率。所述獲取攝像機(jī)標(biāo)定矩陣K的方法采用文獻(xiàn)《Flexible camera calibration byviewing a plane from unknown orientations.》(Zhang Zhengyou.Proceedings of International Conference on Computer Vision. 1999 :666 673)中公開的方法。步驟二、獲取空間對準(zhǔn)模板的深度圖。所述空間對準(zhǔn)模板采用文獻(xiàn)《視覺傳感器與 激光測距雷達(dá)空間對準(zhǔn)方法》(付夢印，劉明陽；紅外與激光工程，2009，38(1))中公開的空 間對準(zhǔn)模板。該步驟的操作與步驟一不分先后。具體為由人機(jī)交互界面輸入機(jī)械腕轉(zhuǎn)動參數(shù) 及激光掃描雷達(dá)掃描參數(shù)，分別傳送給運(yùn)動控制模塊和激光掃描雷達(dá)，運(yùn)動控制模塊驅(qū)動 機(jī)械腕轉(zhuǎn)動進(jìn)行空間對準(zhǔn)模板的激光掃描，得到的三維點云數(shù)據(jù)傳送到深度圖構(gòu)建模塊， 經(jīng)過深度圖構(gòu)建模塊的處理后生成空間對準(zhǔn)模板的深度圖，并傳送給人機(jī)交互模塊。步驟三、在步驟二的基礎(chǔ)上，獲取空間對準(zhǔn)模板的攝像機(jī)圖像。
由人機(jī)交互界面輸入機(jī)械腕轉(zhuǎn)動參數(shù)，傳送給運(yùn)動控制模塊，運(yùn)動控制模塊驅(qū)動 機(jī)械腕轉(zhuǎn)動使攝像機(jī)對準(zhǔn)步驟二激光掃描雷達(dá)掃描的場景，進(jìn)行拍攝，并將得到的攝像機(jī) 圖像傳送給人機(jī)交互模塊。步驟四、在步驟三的基礎(chǔ)上，獲取攝像機(jī)圖像特征點坐標(biāo)以及與其對應(yīng)的深度圖 特征點坐標(biāo)。通過人機(jī)交互模塊，在步驟三得到的攝像機(jī)圖像中，采用文獻(xiàn)《視覺傳感器與激光測距雷達(dá)空間對準(zhǔn)方法》(付夢印，劉明陽；紅外與激光工程，2009，38(1))中公開的空間對 準(zhǔn)方法，選取特征點，并將選取得到的攝像機(jī)圖像特征點坐標(biāo)傳送給空間對準(zhǔn)模塊；在深度 圖中，獲取與攝像機(jī)圖像特征點對應(yīng)的深度圖特征點，將其坐標(biāo)傳送給空間對準(zhǔn)模塊。所述獲取深度圖特征點坐標(biāo)的方法為第1步首先給出相關(guān)定義定義深度圖特征點候選區(qū)域點集中的混合像素及噪聲像素為壞點，其集合為壞點 集合A。p;S義除去深度圖特征點候選區(qū)域點集中的混合像素及噪聲像素以外的其它點為 好點，其集合為好點集合Ag;定義好點包括邊界點和正常點，其集合對應(yīng)按行順序分類和列 順序分類，分別為邊界點集合Aedl和正常點集合Aml、邊界點集合Aed2和正常點集合Am2，邊 界點是激光掃描場景中實際物體邊界所對應(yīng)的掃描點，正常點是好點集合Ag中除邊界點以 外的其它點；定義待定點包括待定壞點與待定邊界點，其集合為待定點集合Au。。第2步將深度圖特征點候選區(qū)域點集中的所有點進(jìn)行編號，用Py表示，其中i 表示行號，j表示列號，1彡i彡m+1，1彡j彡n+1且i，j，m，η為正整數(shù)；設(shè)定P 為激光 掃描雷達(dá)光心到Py的距離；Iy為Py與兩點之中到激光掃描雷達(dá)光心的距離較小 的那個點向距離較大的那個點與激光掃描雷達(dá)光心之間連線所作的垂線；△ P υ為Pi, j與 Pi, j+1之間的距離；Δ θ 為Piij與Pi,j+1之間的連線與Iiij的夾角；第3步設(shè)定i = 1，j = 1，即選取第一行的第一個點；第4步將該點分類為好點；第5步Δ ρ ^(Piij與Pi, j+1之間的距離)、Δ θ ^(ρ^與Pi,j+1之間的連線與Ii, j的夾角)的值可通過公式1計算得到
aPu 1 = + Ph _ 1PuPl^cos 0J12(1)
」Larcsin(fc0。. - sPiJ COS ers )/ ^PiJ ) J其中，1 ρ ι」為ρ ι」禾Π ρ ι J+1中的較大值；s P i,」為P i, j和P i, J+1中的較小值， θ rs為激光掃描雷達(dá)掃描角分辨率。第6步判斷點Pi, J+1的類型，即判斷點Pi, J+1是正常點、壞點或邊界點；具體操作 為①如果第5步得到的Pi,」與Pi’j+1之間的距離Δ ρ .j大于某一人為設(shè)定值Δ ρ th， 并且Pu與之間的連線與Im的夾角Δ θ u大于某一人為設(shè)定值Δ θ th，則設(shè)定一個 標(biāo)志量μ U，并將其值設(shè)定為1 ；否則，將標(biāo)志量μ i.j的值設(shè)定為0。②如果Pijj e Ag 并且 μ = 1，則 Pijj e Aedl，Pi,J+1 e Auc ；如果Pi,j e Ag 并且 Uijj. = 0，則 Pi,j e Anrl，Pi,J+1 e Ag;
如果Pi, j e Auc 并且 μ i, j = 1，則 Pi, j e Acp，Pi, J+1 e Auc ；如果Pi, j e Auc 并且 μ W = 0，則 Pi, j e Aedl，pijJ+1 e Ag ；如果Pi, J e Acp 并且 μ i, J = 1，則 Pi, j e Acp，Pi, J+1 e Auc ；如果j = n-1 并且 Uijj = 1，則 Pi,n e Acp ；如果j = n-1 并且 μ u = 0，則 Pi,η e Aedl ；第7步判斷j = η是否成立，如果成立，轉(zhuǎn)到第8步；否則，重置j = j+1 ；返回第 5步；第8步判斷i =m是否成立，如果成立，結(jié)束操作；否則，重置i = i+l，j = 1 ；返 回第4步；通過第2步到第8步的操作，將深度圖特征點候選區(qū)域點集中的全部點按照行順 序完成了正常點、壞點或邊界點的分類。第9步設(shè)定1 ‘ i,」為Pi, j與pi+1, j兩點之中到激光掃描雷達(dá)光心的距離較小的 那個點向距離較大的那個點與激光掃描雷達(dá)光心之間連線所作的垂線；Δ P ‘ Uj為與
之間的距離；Δ θ ‘ U為Pu與Pi+^之間的連線與1' 的夾角；第10步重新設(shè)定i = 1，j = 1，即選取第一列的第一個點；第11步將該點分類為好點；第12步Δ ρ' υ(Ρ ,」與pi+1,j之間的距離)、Δ θ ‘ ^」(Pi,」與pi+1,」之間的連 線與Γ i, j的夾角)的值可通過公式2計算得到
aPu 1 = (pLJ + pIJ -2PuP^J cos0rsΓ(2)
_Δ Oi j」Larcsin((/p;) - Spl j cos 0rs)/ Apl j)」其中，1 P ' 為P 和P i+1,j中的較大值；S ρ ' 為p 和p i+1,j中的較小 值，θ rs為激光掃描雷達(dá)掃描角分辨率。第13步判斷點ρ +1,」的類型，即判斷點Pi^是正常點、壞點或邊界點；具體操作 為①如果第12步得到的Piij與ρ +1,」之間的距離Δ ρ ‘ ^大于某一人為設(shè)定值 Δ pth，并且Pu與ρ +1,」之間的連線與1' i,」的夾角ΔΘ' w大于某一人為設(shè)定值Δ θ th， 則設(shè)定標(biāo)志量μ 的值為ι ；否則，將標(biāo)志量μ 的值設(shè)定為0。②如果Pi, j G Ag 并且 μ = 1，則 Pi, j e Aed2，Pi, j+1 e Auc ；如果Pi,j e Ag 并且 μ = 0，則 Pi,j e Anr2，Pi,J+1 e Ag ；如果Pi, J e Au。并且 μ i, j = 1，則 Pi, j e Acp，Pi, J+1 e Auc ；如果Piij e Auc 并且 μ W = 0，則 Piij e Aed2，Pi,J+1 e Ag ；如果Pijj e Acp 并且 Uijj = 1，則 Pijj e Acp，Pi,J+1 e Auc ；如果j = n-1 并且 μ U = 1，則 Pi,η e Acp ；如果j = n-1 并且 Uijj = 0，則 Pi,n e Aed2 ；第14步判斷i = m是否成立，如果成立，轉(zhuǎn)到第15步；否則，重置i = i+Ι ；返回 第12步；第15步判斷j = η是否成立，如果成立，結(jié)束操作；否則，重置i = 1，j = j+1 ；返回第11步；通過第9步到第15步的操作，將深度圖特征點候選區(qū)域點集中的全部點按照列順 序完成了正常點、壞點或邊界點的分類。第16步將邊界點集合Aedl中的所有點的三維坐標(biāo)中的χ和ζ坐標(biāo)值取均值作為 深度圖特征點的X和Z坐標(biāo)值，將邊界點集合Aed2中的所有點的三維坐標(biāo)中的y坐標(biāo)值取 均值作為深度圖特征點的y坐標(biāo)值。經(jīng)過上述步驟的操作，即可獲取深度圖特征點的坐標(biāo)。步驟五、獲取攝像機(jī)坐標(biāo)系到激光掃描雷達(dá)坐標(biāo)系的坐標(biāo)轉(zhuǎn)換矩陣。在步驟四的基礎(chǔ)上，空間對準(zhǔn)模塊根據(jù)得到的攝像機(jī)圖像特征點坐標(biāo)及深度圖特 征點坐標(biāo)，結(jié)合攝像機(jī)標(biāo)定模塊所傳送的攝像機(jī)標(biāo)定矩陣K，進(jìn)行激光掃描雷達(dá)與攝像機(jī)的 空間對準(zhǔn)，得到攝像機(jī)坐標(biāo)系到激光掃描雷達(dá)坐標(biāo)系的坐標(biāo)轉(zhuǎn)換矩陣，包括旋轉(zhuǎn)矩陣和平 移矩陣，并將其傳送給激光點云染色模塊。所述獲取攝像機(jī)坐標(biāo)系到激光掃描雷達(dá)坐標(biāo)系的坐標(biāo)轉(zhuǎn)換矩陣的方法采用文 獻(xiàn)《視覺傳感器與激光測距雷達(dá)空間對準(zhǔn)方法》(付夢印，劉明陽；紅外與激光工程，2009， 38(1))公開的方法。步驟六、使用激光掃描雷達(dá)獲取目標(biāo)場景的三維點云數(shù)據(jù)。在步驟五的基礎(chǔ)上，由人機(jī)交互界面輸入機(jī)械腕轉(zhuǎn)動參數(shù)及激光掃描雷達(dá)掃描參 數(shù)，分別傳送給運(yùn)動控制模塊和激光掃描雷達(dá)，運(yùn)動控制模塊驅(qū)動機(jī)械腕轉(zhuǎn)動進(jìn)行目標(biāo)場 景的激光掃描，掃描得到的三維點云數(shù)據(jù)傳送到激光點云染色模塊。步驟七、使用攝像機(jī)獲取目標(biāo)場景的攝像機(jī)圖像。在步驟六的基礎(chǔ)上，由人機(jī)交互界面輸入機(jī)械腕轉(zhuǎn)動參數(shù)，傳送給運(yùn)動控制模塊， 運(yùn)動控制模塊驅(qū)動機(jī)械腕轉(zhuǎn)動使攝像機(jī)朝向目標(biāo)場景，并使攝像機(jī)對準(zhǔn)與激光掃描雷達(dá)掃 描的目標(biāo)場景，拍攝得到目標(biāo)場景的攝像機(jī)圖像，然后傳送給激光點云染色模塊。步驟八、得到彩色三維點云數(shù)據(jù)。在步驟七的基礎(chǔ)上，激光點云染色模塊中根據(jù)激光掃描雷達(dá)傳送的目標(biāo)場景三維 點云數(shù)據(jù)和攝像機(jī)傳送的目標(biāo)場景圖像，攝像機(jī)標(biāo)定模塊提供的攝像機(jī)投影矩陣K，空間對 準(zhǔn)模塊所提供的旋轉(zhuǎn)矩陣和平移矩陣，通過坐標(biāo)轉(zhuǎn)換找到三維點云數(shù)據(jù)在攝像機(jī)圖像中的 對應(yīng)點，并提取出對應(yīng)點的顏色信息。將三維點云數(shù)據(jù)的三維位置坐標(biāo)及在攝像機(jī)圖像數(shù) 據(jù)對應(yīng)點的三維RGB顏色信息合并為六維坐標(biāo)，得到彩色三維點云數(shù)據(jù)，并傳送給姿態(tài)方 位參考模塊。步驟九、將彩色三維點云數(shù)據(jù)從激光掃描雷達(dá)坐標(biāo)系轉(zhuǎn)換到大地坐標(biāo)系并顯示。在步驟八的基礎(chǔ)上，姿態(tài)方位參考模塊將激光點云染色模塊傳送的彩色三維點云 數(shù)據(jù)轉(zhuǎn)換到大地坐標(biāo)系，并將轉(zhuǎn)換后的彩色三維點云數(shù)據(jù)傳送到人機(jī)交互模塊進(jìn)行顯示。經(jīng)過上述步驟，即可完成基于移動平臺的彩色激光點云成像。有益效果本發(fā)明與傳統(tǒng)方法相比，提高了激光掃描雷達(dá)與攝像機(jī)的配準(zhǔn)精度，得到的彩色 三維點云更加真實；同時借助于移動平臺，可以完成大范圍環(huán)境的彩色三維點云獲取，適用 范圍更廣。


圖1是本發(fā)明關(guān)于基于移動平臺的彩色激光點云成像系統(tǒng)的一種具體實施方式
的結(jié)構(gòu)框圖；圖2是本發(fā)明關(guān)于基于移動平臺的彩色激光點云成像系統(tǒng)的一種具體實施方式
的支撐框架的正視圖；圖3是本發(fā)明關(guān)于基于移動平臺的彩色激光點云成像系統(tǒng)的一種具體實施方式
的支撐框架的側(cè)視圖；其中1-連接軸A ；2-連接軸B ；3-連接懸臂；4_支撐桿。
具體實施例方式下面結(jié)合附圖和具體實施例對本發(fā)明做詳細(xì)描述。一種基于移動平臺的彩色激光點云成像系統(tǒng)，其結(jié)構(gòu)如圖1所示，包括移動平 臺、支撐桿4(未在圖1中畫出)、攝像機(jī)、攝像機(jī)標(biāo)定模塊、運(yùn)動控制模塊、人機(jī)交互模塊、空 間對準(zhǔn)模塊、機(jī)械腕、深度圖構(gòu)建模塊、濾波器模塊、激光掃描雷達(dá)、姿態(tài)方位參考模塊、激 光點云染色模塊。支撐桿4與機(jī)械腕組成支撐框架，其中，機(jī)械腕包括連接軸Al、連接軸B2以及連接 懸臂3，其正視圖如圖2所示，側(cè)視圖如圖3所示；支撐桿4與連接懸臂3通過連接軸Al和 連接軸B2連接，連接軸Al處于豎直狀態(tài)，連接軸B2處于水平狀態(tài)，連接軸Al和連接軸B2 可繞其軸心旋轉(zhuǎn)，并且支撐桿4與連接軸Al、連接軸B2之間非固定連接，連接懸臂3與連接 軸Al、連接軸B2之間固定連接，即當(dāng)連接軸Al圍繞其軸心轉(zhuǎn)動時，支撐桿4不隨連接軸 轉(zhuǎn)動，而連接懸臂3保持與連接軸Al同速轉(zhuǎn)動；當(dāng)連接軸B2圍繞軸心轉(zhuǎn)動時，支撐桿4不 隨連接軸轉(zhuǎn)動，而連接懸臂3保持與連接軸B2同速轉(zhuǎn)動。支撐框架通過支撐桿4固定在移 動平臺上，連接懸臂3不接觸移動平臺；攝像機(jī)和激光掃描雷達(dá)固定安裝于連接懸臂3上， 并且攝像機(jī)的光心和激光掃描雷達(dá)的光心，與連接軸B2的軸心的距離相等，攝像機(jī)的光心 與激光掃描雷達(dá)的光心在同一豎直平面上。攝像機(jī)與人機(jī)交互模塊、攝像機(jī)標(biāo)定模塊、激光點云染色模塊、機(jī)械腕連接；攝像 機(jī)標(biāo)定模塊與攝像機(jī)、空間對準(zhǔn)模塊、激光點云染色模塊連接；運(yùn)動控制模塊與機(jī)械腕、人 機(jī)交互模塊、移動平臺連接；人機(jī)交互模塊與激光掃描雷達(dá)、攝像機(jī)、運(yùn)動控制模塊、深度圖 構(gòu)建模塊、空間對準(zhǔn)模塊、姿態(tài)方位參考模塊連接；空間對準(zhǔn)模塊與人機(jī)交互模塊、濾波器 模塊、攝像機(jī)標(biāo)定模塊、激光點云染色模塊連接；機(jī)械腕與運(yùn)動控制模塊、激光掃描雷達(dá)、攝 像機(jī)連接；深度圖構(gòu)建模塊與激光掃描雷達(dá)、人機(jī)交互模塊連接；濾波器模塊與人機(jī)交互 模塊、空間對準(zhǔn)模塊連接；激光掃描雷達(dá)與機(jī)械腕、人機(jī)交互模塊、激光點云染色模塊連接； 姿態(tài)方位參考模塊與人機(jī)交互模塊、激光點云染色模塊連接；激光點云染色模塊與激光掃 描雷達(dá)、攝像機(jī)、人機(jī)交互模塊、姿態(tài)方位參考模塊、空間對準(zhǔn)模塊、攝像機(jī)標(biāo)定模塊連接； 其中，攝像機(jī)為模擬接口攝像機(jī)或USB接口的CMOS攝像機(jī)。其工作流程為步驟一、對攝像機(jī)進(jìn)行標(biāo)定。具體為由人機(jī)交互模塊輸入機(jī)械腕轉(zhuǎn)動參數(shù)并傳遞 給運(yùn)動控制模塊，運(yùn)動控制模塊根據(jù)接收到的機(jī)械腕的轉(zhuǎn)動參數(shù)驅(qū)動機(jī)械腕轉(zhuǎn)動使攝像機(jī) 朝向正前方，采集圖像，將得到的圖像傳送到攝像機(jī)標(biāo)定模塊進(jìn)行處理，得到攝像機(jī)標(biāo)定矩陣K，將攝像機(jī)標(biāo)定矩陣K傳送給空間對準(zhǔn)模塊及激光點云染色模塊。所述機(jī)械腕的轉(zhuǎn)動參數(shù)包括但不限于起始角度、終止角度、步進(jìn)角。所述移動平臺的運(yùn)動參數(shù)包括但不限于起動、停止、方向、速度命令。所述激光掃描雷達(dá)掃描參數(shù)包括但不限于掃描視角和掃描分辨率 。所述獲取攝像機(jī)標(biāo)定矩陣K的方法采用文獻(xiàn)《Flexible camera calibration byviewing a plane from unknown orientations.》(Zhang Zhengyou.Proceedings of International Conference on Computer Vision. 1999 :666 673)中公開的方法。步驟二、獲取空間對準(zhǔn)模板的深度圖。所述空間對準(zhǔn)模板采用根據(jù)文獻(xiàn)《視覺傳感 器與激光測距雷達(dá)空間對準(zhǔn)方法》(付夢印，劉明陽；紅外與激光工程，2009，38(1))中公開 的空間對準(zhǔn)模板。該步驟的操作與步驟一不分先后。具體為由人機(jī)交互界面輸入機(jī)械腕轉(zhuǎn)動參數(shù) 及激光掃描雷達(dá)掃描參數(shù)，分別傳送給運(yùn)動控制模塊和激光掃描雷達(dá)，運(yùn)動控制模塊驅(qū)動 機(jī)械腕轉(zhuǎn)動進(jìn)行空間對準(zhǔn)模板的激光掃描，得到的三維點云數(shù)據(jù)傳送到深度圖構(gòu)建模塊， 經(jīng)過深度圖構(gòu)建模塊的處理后生成空間對準(zhǔn)模板的深度圖，并傳送給人機(jī)交互模塊。步驟三、在步驟二的基礎(chǔ)上，獲取空間對準(zhǔn)模板的攝像機(jī)圖像。由人機(jī)交互界面輸入機(jī)械腕轉(zhuǎn)動參數(shù)，傳送給運(yùn)動控制模塊，運(yùn)動控制模塊驅(qū)動 機(jī)械腕轉(zhuǎn)動使攝像機(jī)對準(zhǔn)步驟二激光掃描雷達(dá)掃描的場景，進(jìn)行拍攝，并將得到的攝像機(jī) 圖像傳送給人機(jī)交互模塊。步驟四、在步驟三的基礎(chǔ)上，獲取攝像機(jī)圖像特征點坐標(biāo)以及與其對應(yīng)的深度圖 特征點坐標(biāo)。通過人機(jī)交互模塊，在步驟三得到的攝像機(jī)圖像中，采用文獻(xiàn)《視覺傳感器與激光 測距雷達(dá)空間對準(zhǔn)方法》(付夢印，劉明陽；紅外與激光工程，2009，38(1))中公開的空間對 準(zhǔn)方法，選取特征點，并將選取得到的攝像機(jī)圖像特征點坐標(biāo)傳送給空間對準(zhǔn)模塊；在深度 圖中，獲取與攝像機(jī)圖像特征點對應(yīng)的深度圖特征點，將其坐標(biāo)傳送給空間對準(zhǔn)模塊。所述獲取深度圖特征點坐標(biāo)的方法為第1步首先給出相關(guān)定義定義深度圖特征點候選區(qū)域點集中的混合像素及噪聲像素為壞點，其集合為壞點 集合A。p;s義除去深度圖特征點候選區(qū)域點集中的混合像素及噪聲像素以外的其它點為 好點，其集合為好點集合Ag;定義好點包括邊界點和正常點，其集合對應(yīng)按行順序分類和列 順序分類，分別為邊界點集合Aedl和正常點集合Aml、邊界點集合Aed2和正常點集合Am2，邊 界點是激光掃描場景中實際物體邊界所對應(yīng)的掃描點，正常點是好點集合Ag中除邊界點以 外的其它點；定義待定點包括待定壞點與待定邊界點，其集合為待定點集合Au。。第2步將深度圖特征點候選區(qū)域點集中的所有點進(jìn)行編號，用Py表示，其中i 表示行號，j表示列號，1彡i彡m+1，1彡j彡n+1且i，j，m，η為正整數(shù)；設(shè)定P 為激光 掃描雷達(dá)光心到Py的距離；Iy為Py與兩點之中到激光掃描雷達(dá)光心的距離較小 的那個點向距離較大的那個點與激光掃描雷達(dá)光心之間連線所作的垂線；△ P υ為Pi, j與 Pi, j+1之間的距離；Δ θ 為Piij與Pi,j+1之間的連線與Iiij的夾角；第3步設(shè)定i = l,j = 1，即選取第一行的第一個點；第4步將該點分類為好點；
第5步Δ ρ w (pu與Pi, j+1之間的距離)、Δ θ ^(ρ^與Pi,j+1之間的連線與Ii, j的夾角)的值可通過公式7計算得到
<formula>formula see original document page 15</formula>其中，Ipi, J為Pi, J禾口 Pi.j+i中的較大值；S Pi, J為Pi, J和Pij1中的較小值， θ rs為激光掃描雷達(dá)掃描角分辨率。第6步判斷點Pi, J+1的類型，即判斷點Pi, J+1是正常點、壞點或邊界點；具體操作 為①如果第5步得到的Pi, j與Pi, j+1之間的距離Δ P U大于某一人為設(shè)定值Δ pth， 并且Pu與之間的連線與Im的夾角Δ θ u大于某一人為設(shè)定值Δ θ th，則設(shè)定一個 標(biāo)志量μ U，并將其值設(shè)定為ι ；否則，將標(biāo)志量μ i.j的值設(shè)定為0。②如果Pi, j e Ag 并且 = UJpiij e Aedl，Pi,j+1 e Auc ；如果Pi,j e Ag 并且 Uijj = OJJpiij e Anrl，Pi,J+1 e Ag;如果Py e Auc 并且 μ^ = UJpiJ e Acp，Pi,J+1 e Auc ；如果Py e Auc 并且 μ W = 0，則？" e Aedl，Pi,J+1 e Ag;如果Py e Acp 并且 μ^ = UJpiJ e Acp,PijJ+1 e Auc ；如果j = n-1 并且 Py= 1，則 Pi,n e Acp ；如果j = n-1 并且 μ u = 0，則 Pi,n e Aedl ；第7步判斷j = η是否成立，如果成立，轉(zhuǎn)到第8步；否則，重置j = j+1 ；返回第 5步；第8步判斷i =m是否成立，如果成立，結(jié)束操作；否則，重置i = i+l，j = 1 ；返 回第4步；通過第2步到第8步的操作，將深度圖特征點候選區(qū)域點集中的全部點按照行順 序完成了正常點、壞點或邊界點的分類。第9步設(shè)定1 ‘ μ為Pi, j與pi+1, j兩點之中到激光掃描雷達(dá)光心的距離較小的 那個點向距離較大的那個點與激光掃描雷達(dá)光心之間連線所作的垂線；Δ ρ ‘ Uj為Pu與 pi+1,j之間的距離；Δ θ ‘ μ為Pu與ρ +1,」之間的連線與1' μ的夾角；第10步重新設(shè)定i = 1，j = 1，即選取第一列的第一個點；第11步將該點分類為好點；第12步Δ ρ' "(ρ"與ρ +1,」.之間的距離)、Δ θ ‘ i,(Pi,與pi+1,之間的連
線與Γ i, j的夾角)的值可通過公式8計算得到<formula>formula see original document page 15</formula>其中，IP'“為Piij和Pi+1,j中的較大值；SP ‘“為Piij和Pi+1,j中的較小 值，θ κ為激光掃描雷達(dá)掃描角分辨率。第13步判斷點Pi^的類型，即判斷點Pi^是正常點、壞點或邊界點；具體操作為①如果第12步得到的Piij與ρ +1,」之間的距離Ap ‘ ^大于某一人為設(shè)定值 Δ pth，并且Piij與ρ +1,」之間的連線與1' w的夾角Δ Θ' w大于某一人為設(shè)定值Δ θ th， 則設(shè)定標(biāo)志量μ i.j的值為ι ；否則，將標(biāo)志量μ i.j的值設(shè)定為0。②如果Py G Ag 并且 μ W = UJpiJ e Aed2，pijJ+1 e Auc ；如果e Ag 并且 Py = 0，則 Py e Am2,pUJ+1 e Ag;如果Pi, j e Auc 并且 μ W = 1，則 Pi, j e Acp, pijJ+1 e Auc ；如果Pi,j e Auc 并且 μ i,」=0，則 Pi,」e Aed2，pijJ+1 e Ag ；如果Py e Acp 并且 μ W = 1，則 Py e Acp，pijJ+1 e Auc ；如果j = n-1 并且 Py= 1，則 Pi,n e Acp ；如果j = n-1 并且 Py = 0，則 Pi,n e Aed2 ；第14步判斷i =m是否成立，如果成立，轉(zhuǎn)到第15步；否則，重置i = i+Ι ；返回 第12步；第15步判斷j = η是否成立，如果成立，結(jié)束操作；否則，重置i = 1，j = j+1 ； 返回第11步；通過第9步到第15步的操作，將深度圖特征點候選區(qū)域點集中的全部點按照列順 序完成了正常點、壞點或邊界點的分類。第16步將邊界點集合Aedl中的所有點的三維坐標(biāo)中的χ和ζ坐標(biāo)值取均值作為深度圖特征點的X和Z坐標(biāo)值，將邊界點集合Aed2中的所有點的三維坐標(biāo)中的y坐標(biāo)值取 均值作為深度圖特征點的y坐標(biāo)值。經(jīng)過上述步驟的操作，即可獲取深度圖特征點的坐標(biāo)。步驟五、在步驟四的基礎(chǔ)上，獲取攝像機(jī)坐標(biāo)系到激光掃描雷達(dá)坐標(biāo)系的坐標(biāo)轉(zhuǎn) 換矩陣?？臻g對準(zhǔn)模塊根據(jù)得到的攝像機(jī)圖像特征點坐標(biāo)及深度圖特征點坐標(biāo)，結(jié)合攝像 機(jī)標(biāo)定模塊所傳送的攝像機(jī)標(biāo)定矩陣K，進(jìn)行激光掃描雷達(dá)與攝像機(jī)的空間對準(zhǔn)，得到攝像 機(jī)坐標(biāo)系到激光掃描雷達(dá)坐標(biāo)系的坐標(biāo)轉(zhuǎn)換矩陣，包括旋轉(zhuǎn)矩陣和平移矩陣，并將其傳送 給激光點云染色模塊。所述獲取攝像機(jī)坐標(biāo)系到激光掃描雷達(dá)坐標(biāo)系的坐標(biāo)轉(zhuǎn)換矩陣的方法采用《視覺 傳感器與激光測距雷達(dá)空間對準(zhǔn)方法》(付夢印，劉明陽；紅外與激光工程，2009，38(1))中 公開的方法。步驟六、使用激光掃描雷達(dá)獲取目標(biāo)場景的三維點云數(shù)據(jù)。在步驟五的基礎(chǔ)上，由人機(jī)交互界面輸入機(jī)械腕轉(zhuǎn)動參數(shù)及激光掃描雷達(dá)掃描參 數(shù)，分別傳送給運(yùn)動控制模塊和激光掃描雷達(dá)，運(yùn)動控制模塊驅(qū)動機(jī)械腕轉(zhuǎn)動進(jìn)行目標(biāo)場 景的激光掃描，掃描得到的三維點云數(shù)據(jù)傳送到激光點云染色模塊。步驟七、使用攝像機(jī)獲取目標(biāo)場景的攝像機(jī)圖像。在步驟六的基礎(chǔ)上，由人機(jī)交互界面輸入機(jī)械腕轉(zhuǎn)動參數(shù)，傳送給運(yùn)動控制模塊， 運(yùn)動控制模塊驅(qū)動機(jī)械腕轉(zhuǎn)動使攝像機(jī)朝向目標(biāo)場景，并使攝像機(jī)對準(zhǔn)與激光掃描雷達(dá)掃 描的目標(biāo)場景，拍攝得到目標(biāo)場景的攝像機(jī)圖像，然后傳送給激光點云染色模塊。步驟八、在步驟七的基礎(chǔ)上，激光點云染色模塊中根據(jù)激光掃描雷達(dá)傳送的目標(biāo)場景三維點云數(shù)據(jù)和攝像機(jī)傳送的目標(biāo)場景圖像，攝像機(jī)標(biāo)定模塊提供的攝像機(jī)投影矩陣 K，空間對準(zhǔn)模塊所提供的旋轉(zhuǎn)矩陣和平移矩陣，通過坐標(biāo)轉(zhuǎn)換找到三維點云數(shù)據(jù)在攝像機(jī) 圖像中的對應(yīng)點，并提取出對應(yīng)點的顏色信息。將三維點云數(shù)據(jù)的三維位置坐標(biāo)及在攝像 機(jī)圖像數(shù)據(jù)對應(yīng)點的三維RGB顏色信息合并為六維坐標(biāo)，得到彩色三維點云數(shù)據(jù)，并傳送 給姿態(tài)方位參考模塊。進(jìn)行激光點云染色具體步驟為(1)設(shè)定激光掃描雷達(dá)掃描場景中，所得到的三維點云數(shù)據(jù)中，任 一掃描點P在激 光掃描雷達(dá)系的三維坐標(biāo)為=Xuk = [xLMS Ylms Zlms]T,(2)經(jīng)過旋轉(zhuǎn)、平移變換，得點P在攝像機(jī)坐標(biāo)系下的坐標(biāo)為
Camera<formula>formula see original document page 17</formula>(3)進(jìn)一步，得點P在圖像坐標(biāo)系下的坐標(biāo)為
<formula>formula see original document page 17</formula>
(4)通過點P的圖像坐標(biāo)，可以索引點P的顏色值R、G、B。(5)將點P的空間直角坐標(biāo)與顏色值合并，得到點P的空間位置及顏色特征的六維 表不
<formula>formula see original document page 17</formula>當(dāng)對激光掃描雷達(dá)掃描的所有點完成上述六維表示的步驟后，即得到彩色三維點云。步驟九、完成激光點云的染色。具體為在步驟八的基礎(chǔ)上，姿態(tài)方位參考模塊將激光點云染色模塊傳送的彩色三維點云 數(shù)據(jù)轉(zhuǎn)換到大地坐標(biāo)系，并將轉(zhuǎn)換后的彩色三維點云數(shù)據(jù)傳送到人機(jī)交互模塊進(jìn)行顯示。 至此，完成激光點云的染色。所述激光點云染色模塊傳送的彩色三維點云轉(zhuǎn)換到大地坐標(biāo)系的方法為設(shè)定姿態(tài)方位參考模塊測得激光掃描雷達(dá)的偏航角為α，俯仰角為β，橫滾角為 Y，激光掃描雷達(dá)的三維點云數(shù)據(jù)在激光掃描雷達(dá)坐標(biāo)系下的坐標(biāo)為Xd
<formula>formula see original document page 17</formula>
則激光掃描點在大地坐標(biāo)系下的坐標(biāo)為X6
<formula>formula see original document page 18</formula>在步驟八的基礎(chǔ)上，得到轉(zhuǎn)換到大地坐標(biāo)系下的彩色三維激光點云為Xe_KeB <formula>formula see original document page 18</formula>將轉(zhuǎn)換到大地坐標(biāo)系下的彩色三維激光點云傳送到人機(jī)交互界面，通過OPENGL 軟件進(jìn)行顯示。經(jīng)過上述步驟，即可完成基于移動平臺的彩色激光點云成像。本發(fā)明不限于此實例，凡是利用本設(shè)計的設(shè)計思路，做一些簡單變化的設(shè)計都應(yīng) 落入本發(fā)明的保護(hù)范圍之內(nèi)。
權(quán)利要求
一種彩色激光點云成像系統(tǒng)，其特征在于包括移動平臺、支撐桿(4)、攝像機(jī)、攝像機(jī)標(biāo)定模塊、運(yùn)動控制模塊、人機(jī)交互模塊、空間對準(zhǔn)模塊、機(jī)械腕、深度圖構(gòu)建模塊、濾波器模塊、激光掃描雷達(dá)、姿態(tài)方位參考模塊、激光點云染色模塊、；其中，機(jī)械腕包括連接軸A(1)、連接軸B(2)以及連接懸臂(3)；支撐桿(4)與機(jī)械腕組成支撐框架，支撐桿(4)與連接懸臂(3)通過連接軸A(1)和連接軸B(2)連接，連接軸A(1)處于豎直狀態(tài)，連接軸B(2)處于水平狀態(tài)，連接軸A(1)和連接軸B(2)可繞其軸心旋轉(zhuǎn)，并且支撐桿(4)與連接軸A(1)、連接軸B(2)之間非固定連接，連接懸臂(3)與連接軸A(1)、連接軸B(2)之間固定連接，即當(dāng)連接軸A(1)圍繞其軸心轉(zhuǎn)動時，支撐桿(4)不隨連接軸轉(zhuǎn)動，而連接懸臂(3)保持與連接軸A(1)同速轉(zhuǎn)動；當(dāng)連接軸B(2)圍繞軸心轉(zhuǎn)動時，支撐桿(4)不隨連接軸轉(zhuǎn)動，而連接懸臂(3)保持與連接軸B(2)同速轉(zhuǎn)動；支撐框架通過支撐桿(4)固定在移動平臺上，連接懸臂(3)不接觸移動平臺；攝像機(jī)和激光掃描雷達(dá)固定安裝于連接懸臂(3)上，并且攝像機(jī)的光心和激光掃描雷達(dá)的光心，與連接軸B(2)的軸心的距離相等，攝像機(jī)的光心與激光掃描雷達(dá)的光心在同一豎直平面上；攝像機(jī)與人機(jī)交互模塊、攝像機(jī)標(biāo)定模塊、激光點云染色模塊、機(jī)械腕連接；攝像機(jī)標(biāo)定模塊與攝像機(jī)、空間對準(zhǔn)模塊、激光點云染色模塊連接；運(yùn)動控制模塊與機(jī)械腕、人機(jī)交互模塊、移動平臺連接；人機(jī)交互模塊與激光掃描雷達(dá)、攝像機(jī)、運(yùn)動控制模塊、深度圖構(gòu)建模塊、空間對準(zhǔn)模塊、姿態(tài)方位參考模塊連接；空間對準(zhǔn)模塊與人機(jī)交互模塊、濾波器模塊、攝像機(jī)標(biāo)定模塊、激光點云染色模塊連接；機(jī)械腕與運(yùn)動控制模塊、激光掃描雷達(dá)、攝像機(jī)連接；深度圖構(gòu)建模塊與激光掃描雷達(dá)、人機(jī)交互模塊連接；濾波器模塊與人機(jī)交互模塊、空間對準(zhǔn)模塊連接；激光掃描雷達(dá)與機(jī)械腕、人機(jī)交互模塊、激光點云染色模塊連接；姿態(tài)方位參考模塊與人機(jī)交互模塊、激光點云染色模塊連接；激光點云染色模塊與激光掃描雷達(dá)、攝像機(jī)、人機(jī)交互模塊、姿態(tài)方位參考模塊、空間對準(zhǔn)模塊、攝像機(jī)標(biāo)定模塊連接；所述攝像機(jī)用于獲取攝像機(jī)圖像數(shù)據(jù)，并分別傳送給攝像機(jī)標(biāo)定模塊、人機(jī)交互模塊和激光點云染色模塊；所述攝像機(jī)標(biāo)定模塊接收攝像機(jī)獲取的圖像數(shù)據(jù)，經(jīng)過處理后得到攝像機(jī)投影矩陣K，并將攝像機(jī)投影矩陣K傳送到空間對準(zhǔn)模塊和激光點云染色模塊；所述運(yùn)動控制模塊根據(jù)人機(jī)交互模塊的控制指令，驅(qū)動機(jī)械腕的轉(zhuǎn)動及移動平臺的運(yùn)動；所述人機(jī)交互模塊的主要功能是提供人機(jī)交互的輸入、輸出功能；包括但不限于①機(jī)械腕的轉(zhuǎn)動參數(shù)輸入、②移動平臺的運(yùn)動參數(shù)輸入、③激光掃描雷達(dá)的掃描參數(shù)輸入、④接收并顯示攝像機(jī)獲取的圖像及深度圖構(gòu)建模塊產(chǎn)生的深度像、⑤接收姿態(tài)方位參考模塊傳輸過來的彩色三維點云數(shù)據(jù)并顯示、⑥提供在攝像機(jī)圖像上選取特征點的功能，并將攝像機(jī)圖像中的特征點坐標(biāo)傳送給空間對準(zhǔn)模塊；⑦提供在深度圖中選取對應(yīng)特征點候選區(qū)域點集的功能，并將其坐標(biāo)傳送給濾波器模塊；所述特征點候選區(qū)域點集為攝像機(jī)圖像上選取的特征點在深度圖中的對應(yīng)候選區(qū)域；所述空間對準(zhǔn)模塊接收來自人機(jī)交互模塊所傳送的攝像機(jī)圖像特征點坐標(biāo)及濾波器模塊所傳送的與攝像機(jī)圖像特征點對應(yīng)的深度圖特征點坐標(biāo)，并接收攝像機(jī)標(biāo)定模塊傳送的攝像機(jī)投影矩陣K，根據(jù)文獻(xiàn)《視覺傳感器與激光測距雷達(dá)空間對準(zhǔn)方法》中公開的空間轉(zhuǎn)換方法得到攝像機(jī)坐標(biāo)系到激光掃描雷達(dá)坐標(biāo)系的坐標(biāo)轉(zhuǎn)換矩陣，包括旋轉(zhuǎn)矩陣和平移矩陣，并將旋轉(zhuǎn)矩陣和平移矩陣傳送給激光點云染色模塊；所述機(jī)械腕在運(yùn)動控制模塊驅(qū)動下能夠完成以下兩種運(yùn)動①控制連接軸B(2)圍繞其軸心旋轉(zhuǎn)，使固定在其上的激光掃描雷達(dá)在俯仰方向上轉(zhuǎn)動；②控制連接軸A(1)圍繞其軸心旋轉(zhuǎn)，使激光掃描雷達(dá)在水平方向上轉(zhuǎn)動；所述深度圖構(gòu)建模塊接收來自激光掃描雷達(dá)的三維點云數(shù)據(jù)，經(jīng)過處理后得到深度圖，并將深度圖傳送給人機(jī)交互模塊；所述濾波器模塊接收來自人機(jī)交互模塊傳送的深度圖特征點候選區(qū)域點集，通過處理得到深度圖特征點坐標(biāo)，將深度圖特征點坐標(biāo)傳送給空間對準(zhǔn)模塊；所述激光掃描雷達(dá)接收人機(jī)交互模塊傳送的激光雷達(dá)掃描參數(shù)，并在機(jī)械腕的帶動下對場景進(jìn)行掃描，獲取三維點云數(shù)據(jù)，并將其傳送給深度圖構(gòu)建模塊和激光點云染色模塊；所述姿態(tài)方位參考模塊用于測量激光掃描雷達(dá)在大地坐標(biāo)系下的姿態(tài)信息建立大地坐標(biāo)系與激光掃描雷達(dá)坐標(biāo)系的轉(zhuǎn)換關(guān)系，所述激光掃描雷達(dá)姿態(tài)信息包括但不限于俯仰角、橫滾角及方位角；然后，將激光點云染色模塊傳送的彩色三維點云數(shù)據(jù)轉(zhuǎn)換到大地坐標(biāo)系，并將轉(zhuǎn)換后的彩色三維點云數(shù)據(jù)傳送到人機(jī)交互模塊進(jìn)行顯示；所述激光點云染色模塊完成激光點云的染色，即接收激光掃描雷達(dá)傳送的三維點云數(shù)據(jù)和攝像機(jī)傳送的圖像數(shù)據(jù)，并接收空間對準(zhǔn)模塊所提供的旋轉(zhuǎn)矩陣和平移矩陣，以及攝像機(jī)標(biāo)定模塊提供的攝像機(jī)投影矩陣K，找出三維點云數(shù)據(jù)在攝像機(jī)圖像數(shù)據(jù)中的對應(yīng)點，并提取出其的顏色信息；將三維點云數(shù)據(jù)的三維位置坐標(biāo)及在圖像數(shù)據(jù)中對應(yīng)點的三維RGB顏色信息合并為六維坐標(biāo)，得到彩色三維點云數(shù)據(jù)，并傳送給人機(jī)交互模塊顯示，完成激光點云的染色；其工作流程為步驟一、對攝像機(jī)進(jìn)行標(biāo)定；具體為由人機(jī)交互模塊輸入機(jī)械腕轉(zhuǎn)動參數(shù)并傳遞給運(yùn)動控制模塊，運(yùn)動控制模塊根據(jù)接收到的機(jī)械腕的轉(zhuǎn)動參數(shù)驅(qū)動機(jī)械腕轉(zhuǎn)動使攝像機(jī)朝向正前方，采集圖像，將得到的圖像傳送到攝像機(jī)標(biāo)定模塊進(jìn)行處理，得到攝像機(jī)標(biāo)定矩陣K，將攝像機(jī)標(biāo)定矩陣K傳送給空間對準(zhǔn)模塊及激光點云染色模塊；所述機(jī)械腕的轉(zhuǎn)動參數(shù)包括但不限于起始角度、終止角度、步進(jìn)角；所述移動平臺的運(yùn)動參數(shù)包括但不限于起動、停止、方向、速度命令；所述激光掃描雷達(dá)掃描參數(shù)包括但不限于掃描視角和掃描分辨率；步驟二、獲取空間對準(zhǔn)模板的深度圖；所述空間對準(zhǔn)模板采用文獻(xiàn)《視覺傳感器與激光測距雷達(dá)空間對準(zhǔn)方法》中公開的空間對準(zhǔn)模板；該步驟的操作與步驟一不分先后；具體為由人機(jī)交互界面輸入機(jī)械腕轉(zhuǎn)動參數(shù)及激光掃描雷達(dá)掃描參數(shù)，分別傳送給運(yùn)動控制模塊和激光掃描雷達(dá)，運(yùn)動控制模塊驅(qū)動機(jī)械腕轉(zhuǎn)動進(jìn)行空間對準(zhǔn)模板的激光掃描，得到的三維點云數(shù)據(jù)傳送到深度圖構(gòu)建模塊，經(jīng)過深度圖構(gòu)建模塊的處理后生成空間對準(zhǔn)模板的深度圖，并傳送給人機(jī)交互模塊；步驟三、在步驟二的基礎(chǔ)上，獲取空間對準(zhǔn)模板的攝像機(jī)圖像；由人機(jī)交互界面輸入機(jī)械腕轉(zhuǎn)動參數(shù)，傳送給運(yùn)動控制模塊，運(yùn)動控制模塊驅(qū)動機(jī)械腕轉(zhuǎn)動使攝像機(jī)對準(zhǔn)步驟二激光掃描雷達(dá)掃描的場景，進(jìn)行拍攝，并將得到的攝像機(jī)圖像傳送給人機(jī)交互模塊；步驟四、在步驟三的基礎(chǔ)上，獲取攝像機(jī)圖像特征點坐標(biāo)以及與其對應(yīng)的深度圖特征點坐標(biāo)；通過人機(jī)交互模塊，在步驟三得到的攝像機(jī)圖像中，采用文獻(xiàn)《視覺傳感器與激光測距雷達(dá)空間對準(zhǔn)方法》中公開的空間對準(zhǔn)方法，選取特征點，并將選取得到的攝像機(jī)圖像特征點坐標(biāo)傳送給空間對準(zhǔn)模塊；在深度圖中，獲取與攝像機(jī)圖像特征點對應(yīng)的深度圖特征點，將其坐標(biāo)傳送給空間對準(zhǔn)模塊；步驟五、獲取攝像機(jī)坐標(biāo)系到激光掃描雷達(dá)坐標(biāo)系的坐標(biāo)轉(zhuǎn)換矩陣；在步驟四的基礎(chǔ)上，空間對準(zhǔn)模塊根據(jù)得到的攝像機(jī)圖像特征點坐標(biāo)及深度圖特征點坐標(biāo)，結(jié)合攝像機(jī)標(biāo)定模塊所傳送的攝像機(jī)標(biāo)定矩陣K，進(jìn)行激光掃描雷達(dá)與攝像機(jī)的空間對準(zhǔn)，得到攝像機(jī)坐標(biāo)系到激光掃描雷達(dá)坐標(biāo)系的坐標(biāo)轉(zhuǎn)換矩陣，包括旋轉(zhuǎn)矩陣和平移矩陣，并將其傳送給激光點云染色模塊；步驟六、使用激光掃描雷達(dá)獲取目標(biāo)場景的三維點云數(shù)據(jù)；在步驟五的基礎(chǔ)上，由人機(jī)交互界面輸入機(jī)械腕轉(zhuǎn)動參數(shù)及激光掃描雷達(dá)掃描參數(shù)，分別傳送給運(yùn)動控制模塊和激光掃描雷達(dá)，運(yùn)動控制模塊驅(qū)動機(jī)械腕轉(zhuǎn)動進(jìn)行目標(biāo)場景的激光掃描，掃描得到的三維點云數(shù)據(jù)傳送到激光點云染色模塊；步驟七、使用攝像機(jī)獲取目標(biāo)場景的攝像機(jī)圖像；在步驟六的基礎(chǔ)上，由人機(jī)交互界面輸入機(jī)械腕轉(zhuǎn)動參數(shù)，傳送給運(yùn)動控制模塊，運(yùn)動控制模塊驅(qū)動機(jī)械腕轉(zhuǎn)動使攝像機(jī)朝向目標(biāo)場景，并使攝像機(jī)對準(zhǔn)與激光掃描雷達(dá)掃描的目標(biāo)場景，拍攝得到目標(biāo)場景的攝像機(jī)圖像，然后傳送給激光點云染色模塊；步驟八、得到彩色三維點云數(shù)據(jù)；在步驟七的基礎(chǔ)上，激光點云染色模塊中根據(jù)激光掃描雷達(dá)傳送的目標(biāo)場景三維點云數(shù)據(jù)和攝像機(jī)傳送的目標(biāo)場景圖像，攝像機(jī)標(biāo)定模塊提供的攝像機(jī)投影矩陣K，空間對準(zhǔn)模塊所提供的旋轉(zhuǎn)矩陣和平移矩陣，通過坐標(biāo)轉(zhuǎn)換找到三維點云數(shù)據(jù)在攝像機(jī)圖像中的對應(yīng)點，并提取出對應(yīng)點的顏色信息；將三維點云數(shù)據(jù)的三維位置坐標(biāo)及在攝像機(jī)圖像數(shù)據(jù)對應(yīng)點的三維RGB顏色信息合并為六維坐標(biāo)，得到彩色三維點云數(shù)據(jù)，并傳送給姿態(tài)方位參考模塊；步驟九、將彩色三維點云數(shù)據(jù)從激光掃描雷達(dá)坐標(biāo)系轉(zhuǎn)換到大地坐標(biāo)系并顯示；在步驟八的基礎(chǔ)上，姿態(tài)方位參考模塊將激光點云染色模塊傳送的彩色三維點云數(shù)據(jù)轉(zhuǎn)換到大地坐標(biāo)系，并將轉(zhuǎn)換后的彩色三維點云數(shù)據(jù)傳送到人機(jī)交互模塊進(jìn)行顯示；經(jīng)過上述步驟，即可完成基于移動平臺的彩色激光點云成像。
2.如權(quán)利要求1所述的一種彩色激光點云成像系統(tǒng)，其特征在于其工作過程的步驟 四中所述獲取深度圖特征點坐標(biāo)的方法為 第1步首先給出相關(guān)定義定義深度圖特征點候選區(qū)域點集中的混合像素及噪聲像素為壞點，其集合為壞點集合 八③；定義除去深度圖特征點候選區(qū)域點集中的混合像素及噪聲像素以外的其它點為好點， 其集合為好點集合Ag;定義好點包括邊界點和正常點，其集合對應(yīng)按行順序分類和列順序 分類，分別為邊界點集合Aedl和正常點集合Aml、邊界點集合Aed2和正常點集合Am2，邊界點 是激光掃描場景中實際物體邊界所對應(yīng)的掃描點，正常點是好點集合Ag中除邊界點以外的其它點；定義待定點包括待定壞點與待定邊界點，其集合為待定點集合Au。；第2步將深度圖特征點候選區(qū)域點集中的所有點進(jìn)行編號，用Pi, j表示，其中i表示 行號，j表示列號，1彡i彡m+1，1彡j彡n+1且i，j，m，η為正整數(shù)；設(shè)定P 為激光掃描 雷達(dá)光心到Py的距離；Iy為Py與兩點之中到激光掃描雷達(dá)光心的距離較小的那 個點向距離較大的那個點與激光掃描雷達(dá)光心之間連線所作的垂線；Δ ρ u為Py與pUj+1 之間的距離；Δ θ u為Py與之間的連線與Iy的夾角； 第3步設(shè)定i = 1，j = 1，即選取第一行的第一個點； 第4步將該點分類為好點；第5步Δ ρ u、Δ θ μ的值可通過公式1計算得到(1 = (P5+i + Pu _ 1PuP丨’辦 cos eJ'2(1) 」|_arcsin((/p, 7 — spt J cos θη)/ Apij)」其中，Ipi, j為ρ W和中的較大值；SPiij為Piij和Pij1中的較小值，ers為 激光掃描雷達(dá)掃描角分辨率；第6步判斷點的類型，即判斷點是正常點、壞點或邊界點；具體操作為①如果第5步得到的Py與之間的距離Δρ u大于某一人為設(shè)定值Δ pth，并且 Pi,j與之間的連線與Iy的夾角Δ θ u大于某一人為設(shè)定值Δ θ th，則設(shè)定一個標(biāo)志 量μ u，并將其值設(shè)定為1 ；否則，將標(biāo)志量U ,,j的值設(shè)定為O ；②如果Pi,」e Ag 并且 μ i,」=1，則 Pi,」e Aedl，pijJ+1 e Auc ； 如果 Pi,」e Ag 并且 μ i,」=0，則 Pi,」e Anrl, pijJ+1 e Ag ； 如果 Pi, j e Auc 并且 Piij= 1，則 Piij e Acp，Pi,J+1 e Auc ； 如果 Pi,」e Auc 并且 μ y = 0，則 Pi,」e Aedl，pijJ+1 e Ag ； 如果 Py e Acp 并且 Piij= 1，則 Piij e Acp，Pi,J+1 e Auc ； 如果 j = n-1 并且 Piij = 1，則 Pi,n e Acp ；如果 j = n-1 并且 μ i,」=0，則 Pi,n e Aedl ；第7步判斷j = η是否成立，如果成立，轉(zhuǎn)到第8步；否則，重置j = j+1 ；返回第5止少；第8步判斷i = m是否成立，如果成立，結(jié)束操作；否則，重置i = i+1，j = 1 ；返回第 4步；通過第2步到第8步的操作，將深度圖特征點候選區(qū)域點集中的全部點按照行順序完 成了正常點、壞點或邊界點的分類；第9步設(shè)定1 ‘ ,, j為Pi, j與pi+1, j兩點之中到激光掃描雷達(dá)光心的距離較小的那個 點向距離較大的那個點與激光掃描雷達(dá)光心之間連線所作的垂線；△ P ‘ 為Pu與Pi+1, j之間的距離；Δ θ' u為Pij與ρ +1,」之間的連線與1' μ的夾角； 第10步重新設(shè)定i = 1，j = 1，即選取第一列的第一個點； 第11步將該點分類為好點； 第12步Δ ρ' μ_、ΔΘ' u的值可通過公式2計算得到<formula>formula see original document page 6</formula>其中，Ipi,」為ρ w和ρ +1,」中的較大值；s P ‘ Uj為口“和Pi+1,j中的較小值，θ^ 為激光掃描雷達(dá)掃描角分辨率；第13步判斷點的類型，即判斷點是正常點、壞點或邊界點；具體操作為①如果第12步得到的Py與之間的距離Δρ υ大于某一人為設(shè)定值Δ pth，并 且Pu與之間的連線與Γ ^的夾角Δ θ，w大于某一人為設(shè)定值Δ θ th，則設(shè)定標(biāo) 志量μ 的值為1 ；否則，將標(biāo)志量μ 的值設(shè)定為O ；②如果Pi,」e Ag 并且 μ i,」=1，則 Pi,」e Aed2，pijJ+1 e Auc ； 如果 Pi,」e Ag 并且 μ i,」=0，則 Pi,」e Anr2, pijJ+1 e Ag ； 如果 Pi, j e Auc 并且 Piij= 1，則 Piij e Acp，Pi,J+1 e Auc ； 如果 Pi,」e Auc 并且 μ y = 0，則 Pi,」e Aed2，pijJ+1 e Ag ； 如果 Py e Acp 并且 Piij= 1，則 Piij e Acp，Pi,J+1 e Auc ； 如果 j = n-1 并且 Piij = 1，則 Pi,n e Acp ；如果 j = n-1 并且 μ i,」=0，則 Pi,n e Aed2 ；第14步判斷i = m是否成立，如果成立，轉(zhuǎn)到第15步；否則，重置i = i+Ι ；返回第12止少；第15步判斷j = η是否成立，如果成立，結(jié)束操作；否則，重置i = 1，j = j+1 ；返回 第11步；通過第9步到第15步的操作，將深度圖特征點候選區(qū)域點集中的全部點按照列順序完 成了正常點、壞點或邊界點的分類；第16步將邊界點集合Aedl中的所有點的三維坐標(biāo)中的χ和ζ坐標(biāo)值取均值作為深度 圖特征點的χ和ζ坐標(biāo)值，將邊界點集合Aed2中的所有點的三維坐標(biāo)中的y坐標(biāo)值取均值 作為深度圖特征點的y坐標(biāo)值；經(jīng)過上述步驟的操作，即可獲取深度圖特征點的坐標(biāo)。
3.如權(quán)利要求1或2所述的一種彩色激光點云成像系統(tǒng)，其特征在于其工作流程的 步驟一中所述獲取攝像機(jī)標(biāo)定矩陣K的方法采用文獻(xiàn)《Flexiblecamera calibration by viewing a plane from unknown orientations.》中公開的方法。
4.如權(quán)利要求1或2所述的一種彩色激光點云成像系統(tǒng)，其特征在于其工作流程的 步驟五中所述獲取攝像機(jī)坐標(biāo)系到激光掃描雷達(dá)坐標(biāo)系的坐標(biāo)轉(zhuǎn)換矩陣的方法采用文獻(xiàn) 《視覺傳感器與激光測距雷達(dá)空間對準(zhǔn)方法》公開的方法。
全文摘要
本發(fā)明涉及一種基于移動平臺的彩色激光點云成像系統(tǒng)，屬于環(huán)境感知領(lǐng)域。包括移動平臺、支撐桿、攝像機(jī)、攝像機(jī)標(biāo)定模塊、運(yùn)動控制模塊、人機(jī)交互模塊、空間對準(zhǔn)模塊、機(jī)械腕、深度圖構(gòu)建模塊、濾波器模塊、激光掃描雷達(dá)、姿態(tài)方位參考模塊、激光點云染色模塊；其中，機(jī)械腕包括連接軸A、連接軸B以及連接懸臂。該系統(tǒng)分別通過激光掃描雷達(dá)和攝像機(jī)獲取三維點云信息和圖像信息，再利用激光掃描雷達(dá)與攝像機(jī)的空間對準(zhǔn)方法及激光點云染色方法，實現(xiàn)彩色激光點云的成像。本發(fā)明與傳統(tǒng)方法相比，提高了激光掃描雷達(dá)與攝像機(jī)的配準(zhǔn)精度，得到的彩色三維點云更加真實；同時借助于移動平臺，可以完成大范圍環(huán)境的彩色三維點云獲取，適用范圍更廣。
文檔編號G01S7/48GK101825442SQ201010160499
公開日2010年9月8日 申請日期2010年4月30日 優(yōu)先權(quán)日2010年4月30日
發(fā)明者付夢印, 朱昊, 楊毅, 楊鑫 申請人:北京理工大學(xué)