專利名稱：：一種三維激光和單目視覺間的自動標(biāo)定方法
技術(shù)領(lǐng)域：
：本發(fā)明屬于機(jī)器人自主環(huán)境感知
技術(shù)領(lǐng)域：
，涉及到三維激光測距系統(tǒng)和視覺傳感器之間的信息融合，特別涉及到三維激光和單目視覺間的自動標(biāo)定方法。
背景技術(shù)：
：工作在復(fù)雜場景中的機(jī)器人系統(tǒng)依靠單一傳感器無法有效完成自主環(huán)境感知與工作場景理解。不同類型傳感器之間的信息融合是提高機(jī)器人環(huán)境理解與認(rèn)知性能的必備手段，而多種傳感器之間的標(biāo)定又是其中的重要環(huán)節(jié)。以往的標(biāo)定工作大多是針對視覺傳感器與二維激光傳感器之間的標(biāo)定(ZhangQL，PlessR.ExtrinsicCalibrationofaCameraandLaserRangeFinderIEEEInternationalConferenceonIntelligentRobotsandSystems(IR0S)，2004)，主要是通過尋找二維激光數(shù)據(jù)的拐點(diǎn)信息或黑白格交錯(cuò)信息，完成視覺和激光數(shù)據(jù)點(diǎn)的匹配，但是存在的問題是依賴于掃描介質(zhì)的特性，特別當(dāng)激光測距距離較遠(yuǎn)或激光掃描數(shù)據(jù)點(diǎn)稀疏時(shí)，標(biāo)定效果受到顯著影響。通過實(shí)驗(yàn)對激光測距性能的結(jié)果分析得到激光測距性能會受到激光測距距離、被測物體介質(zhì)表面特性以及掃描入身寸角等因素的影響(YeC，BorensteinJ.Characterizationofa2_Dlasersca皿erformobilerobotobstaclenegotiation.Proceedingsofthe2002IEEEInternationalConferenceonRoboticsandAutomation.WashingtonDC，USA，2002:2512-2518;于金霞，蔡自興，鄒小兵，段琢華，移動機(jī)器人導(dǎo)航中激光雷達(dá)測距性能研究，傳感技術(shù)學(xué)報(bào)2006，19(2))，如何針對這些影響因素進(jìn)行標(biāo)定裝置設(shè)計(jì)和算法開發(fā)仍是需要探索和解決的問題。依賴于反射介質(zhì)獲得三維激光掃描數(shù)據(jù)的方法所獲取的激光測距數(shù)據(jù)的穩(wěn)定性無法保證，文獻(xiàn)(劉佳，張愛武，馬牧運(yùn)，楊帆，王書民，劉曉萌.三維激光掃描測量系統(tǒng)中的攝像機(jī)標(biāo)定.系統(tǒng)仿真學(xué)報(bào).200S，20(z1))，以玻璃作為反射媒介，無法有效給出標(biāo)定實(shí)驗(yàn)結(jié)果的驗(yàn)證分析，只能停留在仿真階段。文獻(xiàn)(ScaramuzzaD，HaratiA，andSiegwartR.ExtrinsicSelfCalibrationofaCameraanda3DLaserRangeFinderfromNaturalScenes.Proceedingsofthe2007)，依賴于人工手動點(diǎn)選匹配點(diǎn)進(jìn)行三維激光和視覺間外參標(biāo)定，由于人工點(diǎn)選的不準(zhǔn)確，存在引入的人為誤差無法進(jìn)行量化控制與統(tǒng)計(jì)的問題，同時(shí)手動點(diǎn)選時(shí)間開銷大。文獻(xiàn)(UnnikrishnanR，HebertM.FastExtrinsicCalibrationofaLaserRangefindertoaCamera.TechnicalReportCMU_RI_TR_05_09，RoboticsInstitute,CarnegieMellonUniversity,2005)在三維激光禾口視覺間的標(biāo)定過程中激光視覺匹配對的提取較大依賴于三維激光傳感器對黑白格標(biāo)定裝置的掃描效果，其局限性是僅在近距離和較小偏轉(zhuǎn)角度條件下才能保證良好的掃描效果。
發(fā)明內(nèi)容本發(fā)明要解決的問題是提供了自動提取三維激光數(shù)據(jù)和單目視覺圖像數(shù)據(jù)中特征匹配對的方法，實(shí)現(xiàn)三維激光和單目視覺之間的自動標(biāo)定，解決了手動點(diǎn)選匹配對所引入的人為誤差不可控和時(shí)間開銷大問題，提高了匹配對提取精度。以設(shè)計(jì)的一體化標(biāo)定裝置為基礎(chǔ)，減小了距離與偏轉(zhuǎn)角對標(biāo)定的影響，克服了標(biāo)定只能限定在近距離范圍內(nèi)這一局限性。本發(fā)明的技術(shù)方案如下1.激光傳感器特性分析及標(biāo)定裝置的設(shè)計(jì)不同介質(zhì)對激光傳感器所發(fā)射的激光束具有不同的反射率，其中有光澤物體的反射率要高于無光澤物體，同樣不同入射角對測距的影響也非常顯著，但目標(biāo)物體表面顏色對測距影響不大。針對上述激光特性，開發(fā)了三維激光與視覺一體化標(biāo)定裝置(如圖l所示)，裝置的材料為有光澤相紙，以保證對激光具有較高反射率。在相紙表面噴繪出4X5的正方形黑白格，每個(gè)黑白格的邊長為25cm。在每個(gè)黑白格正中心進(jìn)行鏤空，鏤空形狀為半徑為5cm的圓形。經(jīng)過實(shí)驗(yàn)驗(yàn)證，選擇圓形作為鏤空形狀是為了保證算法的魯棒性和避免激光邊緣效應(yīng)所產(chǎn)生的影響。2.標(biāo)定所需激光數(shù)據(jù)特征角點(diǎn)的提取標(biāo)定中需要提取的激光數(shù)據(jù)特征角點(diǎn)是標(biāo)定裝置中黑白格相交所構(gòu)成的角點(diǎn)，該角點(diǎn)是利用鏤空圓孔的中心點(diǎn)坐標(biāo)的統(tǒng)計(jì)平均與校正所獲得的。角點(diǎn)提取分為檢測和校正兩個(gè)環(huán)節(jié)，其中檢測環(huán)節(jié)利用激光測距數(shù)據(jù)的統(tǒng)計(jì)特性進(jìn)行粗提取，該方法可減少激光測距噪聲點(diǎn)的影響。校正階段與由標(biāo)定板中黑白格角點(diǎn)所構(gòu)造的標(biāo)準(zhǔn)模板進(jìn)行匹配，提高了角點(diǎn)檢測的精度，并可減小激光入射角對測距的影響。三維激光測距角點(diǎn)檢測算法步驟如下(1)確定標(biāo)定裝置的粗略位置范圍。選定標(biāo)定裝置所在大致位置，根據(jù)激光測距點(diǎn)云數(shù)據(jù)與激光掃描順序索引值之間的映射關(guān)系(如圖2所示)，篩選出該區(qū)域的數(shù)據(jù)(如圖3(a)(b)所示)。(2)根據(jù)范圍確定平面。根據(jù)區(qū)域內(nèi)的數(shù)據(jù)點(diǎn)擬合平面，即s:arg薩ui2^^(復(fù))其中a=[ai，a2，a3]T為平面法向量，qi=[x，y，z]T為區(qū)域坐標(biāo)點(diǎn)。為了提高計(jì)算效率，可根據(jù)裝置所在范圍的四個(gè)界限點(diǎn)直接確定平面方程。(3)激光測距數(shù)據(jù)二值化。由掃描序列索引值關(guān)系確定的區(qū)域包含激光穿過鏤空圓孔所掃描到的標(biāo)定裝置后側(cè)的測距數(shù)據(jù)，依據(jù)到所擬合平面的距離值d來對區(qū)域數(shù)據(jù)進(jìn)行二值化處理。<formula>formulaseeoriginaldocumentpage4</formula>其中Ii為區(qū)域內(nèi)i點(diǎn)二值化后的取值，D為經(jīng)驗(yàn)距離閾值，D=15cm。二值化后，鏤空圓孔區(qū)域的數(shù)據(jù)為O，其他區(qū)域數(shù)據(jù)為255，從而可以進(jìn)一步借鑒圖像處理方法進(jìn)行角點(diǎn)提取。(4)提取角點(diǎn)。如圖4(a)所示的標(biāo)定裝置模擬簡化圖，4X5的黑白格中，圓形表示鏤空區(qū)域，中心的紅色十字表示鏤空中心，黃色的圓點(diǎn)表示黑白格的角點(diǎn)。藍(lán)色虛線將平面進(jìn)行均勻劃分。利用3)中二值化后所得數(shù)據(jù)，首先按列統(tǒng)計(jì)0和255的點(diǎn)數(shù)分布直方圖，并根據(jù)峰值點(diǎn)確定鏤空中心的行坐標(biāo)，同理也可按行統(tǒng)計(jì)得到列坐標(biāo)，具體見如下公式<formula>formulaseeoriginaldocumentpage5</formula>其中i，j分別表示行列下標(biāo)，函數(shù)l()為0-1函數(shù)，如果括號內(nèi)條件為真則值為l，否則為O。median()為中值選擇函數(shù)，^表示二值化取值。經(jīng)過圖像到激光數(shù)據(jù)的映射(如圖2所示)，從而確定了4X5的鏤空中心坐標(biāo)，并可進(jìn)一步根據(jù)四鄰域關(guān)系確定3X4的黑白格角點(diǎn)坐標(biāo)。上述算法完成了對角點(diǎn)的初步檢測。該檢測算法的優(yōu)點(diǎn)在于角點(diǎn)檢測算法魯棒性好。正如圖4(a)所示，選擇圓形作為鏤空形狀既減少了噪聲的影響(其他形狀不利于鏤空中心提取)，同時(shí)其幾何特性保證了鏤空中心提取的正確性。此外，如果標(biāo)定裝置的傾斜角度過大，或區(qū)域的范圍選擇不理想，這些因素的影響可能會產(chǎn)生如圖4(b)所示的不合理間隔區(qū)域劃分。在該種情況下，上述算法同樣能正確的完成鏤空中心的提取。此外上述算法支持角點(diǎn)提取順序的可控選擇，從而易于實(shí)現(xiàn)與圖像中所提取角點(diǎn)的按次序一一對應(yīng)。由于受到激光入射角及噪聲的影B向，檢測到的角點(diǎn)無法保證嚴(yán)格位于同一平面上，同時(shí)相同間隔內(nèi)的角點(diǎn)也無法保證嚴(yán)格在同一直線上。因此需要下面給出的算法對檢測角點(diǎn)進(jìn)行進(jìn)一步優(yōu)化處理，從而提高精度。3.三維激光測距角點(diǎn)的校正算法由檢測算法可知，所得到的角點(diǎn)具有一定的誤差，需要進(jìn)一步校正以提高精度。校正算法借鑒最近點(diǎn)迭代(ICP)思想，利用檢測出來的激光角點(diǎn)作為初始值與已知模板進(jìn)行匹配，使得檢測點(diǎn)與標(biāo)準(zhǔn)模板的誤差總和最小，具體見公式<formula>formulaseeoriginaldocumentpage5</formula>其中R3X3，T3X1分別為旋轉(zhuǎn)和平移矩陣，Ii為檢測點(diǎn)的齊次坐標(biāo)，Ii為模型點(diǎn)的齊次坐標(biāo)。圖5所示為構(gòu)造的標(biāo)定模板模型，每個(gè)方格邊長為25cm，起始位置處于X-Y平面。采用四元數(shù)法求解三維映射變換。利用四元數(shù)法求三維映射變換的算法如下(1)求兩個(gè)三維點(diǎn)集的重心坐標(biāo)<formula>formulaseeoriginaldocumentpage5</formula>(2)求兩數(shù)據(jù)點(diǎn)集的協(xié)方差矩陣:AM=:Z(A,f<formula>formulaseeoriginaldocumentpage5</formula>(4)求H矩陣最大特征值對應(yīng)的特征向量，即為旋轉(zhuǎn)變換的四元數(shù)表達(dá)。利用與矩陣的轉(zhuǎn)換關(guān)系得到旋轉(zhuǎn)變換矩陣R3X3，進(jìn)一步根據(jù)兩點(diǎn)集間重心偏移計(jì)算平移變換向量1~3><1=p'_R3X3p。4.三維激光數(shù)據(jù)到二維視覺數(shù)據(jù)間變換關(guān)系的迭代計(jì)算選定圖像中與3X4激光測距角點(diǎn)相對應(yīng)的區(qū)域，由于鏤空孔后環(huán)境的影響使得視覺特征角點(diǎn)的提取受到干擾，所以首先對該區(qū)域采用自適應(yīng)閾值二值化處理，然后利用0penCV中黑白格角點(diǎn)檢測函數(shù)提取角點(diǎn)，提取的角點(diǎn)如圖6所示。根據(jù)已經(jīng)獲得的激光數(shù)據(jù)特征角點(diǎn)和視覺數(shù)據(jù)特征角點(diǎn)匹配對，采用迭代優(yōu)化方法來求解三維空間到二維空間的射影變換，此處采用高斯牛頓迭代法進(jìn)行外參數(shù)優(yōu)化求m=[X，Y，Z，1]是三維空間測量齊次坐標(biāo)向，#,#，#)，也即求m使得:(6)取。令m二[x，y，l]圖像二維齊次坐標(biāo)向量，其中fx，fy分別為x，y方向的焦距，(ux，uy)是主點(diǎn)相對于圖像中心的偏移向量。fy，ux，uy已知，是通過傳統(tǒng)的內(nèi)參標(biāo)定方法獲得的。巧，ivr3分別為旋轉(zhuǎn)矩陣的列，ty，t,分別為平移向量分量。由高斯牛頓迭代法，給定初始解|氣《*5附一其中Jacobian矩陣Jy二^T1P孕廣瑪0*》，在^附近有(7)》=p，尋找下一A卩榮沐使得令l，一/riA*0，得正規(guī)化方禾『、王此處fx，向量，tx利用該迭代格式求取滿足公式(6)的變換參數(shù)，得到標(biāo)定結(jié)果。圖8所示為三維激光測距數(shù)據(jù)點(diǎn)云化后所顯示的室外場景，圖9直觀的給出經(jīng)過標(biāo)定后，利用圖7的場景視覺圖像對激光點(diǎn)云數(shù)據(jù)進(jìn)行染色的效果圖，從而檢驗(yàn)了所提標(biāo)定算法的正確性和有效性。本發(fā)明的效果與益處是能夠有效的減小了噪聲、距離、入射角等對三維激光和單目視覺傳感器間標(biāo)定的影響，克服了標(biāo)定只能限定在近距離范圍內(nèi)這一局限性。標(biāo)定過程實(shí)現(xiàn)了激光_視覺數(shù)據(jù)匹配對提取與匹配的自動化和一體化，無手動點(diǎn)選匹配對所引入的人為誤差。標(biāo)定所使用的裝置設(shè)計(jì)簡單合理，并且便于攜帶與應(yīng)用，可完成在野外、廢墟等復(fù)雜環(huán)境下三維激光與單目視覺間的快速準(zhǔn)確標(biāo)定，從而保證利用三維彩色激光測距點(diǎn)云對被測場景的直觀渲染，以及相關(guān)虛擬現(xiàn)實(shí)技術(shù)的實(shí)現(xiàn)，而且對基于多傳感器信息融合的智能移動機(jī)器人技術(shù)的應(yīng)用奠定了良好基礎(chǔ)。iA—(/)J多*入前式的高斯牛頓法的迭代格式■t:rb豕〔8)(9)圖1是三維激光測距系統(tǒng)與單目視覺標(biāo)定裝置的示意圖。圖2是從三維激光點(diǎn)云數(shù)據(jù)到二維圖像數(shù)據(jù)所建立的多級映射關(guān)系圖。圖3(a)是某一角度和距離下的標(biāo)定裝置圖，(b)是與圖(a)相對應(yīng)的激光點(diǎn)云數(shù)據(jù)。圖4(a)是標(biāo)定裝置區(qū)域劃分與角點(diǎn)提取的模擬圖，(b)是大角度偏轉(zhuǎn)或區(qū)域偏移情況下區(qū)域劃分與角點(diǎn)提取的模擬圖。圖5是3X4的標(biāo)準(zhǔn)模板圖。圖6(a)是某一距離和角度下的標(biāo)定裝置圖，(b)是對(a)中視覺圖像的特征角點(diǎn)提取結(jié)果圖。圖7是大連理工大學(xué)管理學(xué)院正門入口場景圖。圖8是在圖7場景中采集三維激光數(shù)據(jù)點(diǎn)云化后的效果圖。圖9是根據(jù)標(biāo)定結(jié)果利用圖7對圖8進(jìn)行染色的得到的效果圖。圖10是三維激光測距系統(tǒng)與單目視覺環(huán)境數(shù)據(jù)采集設(shè)備圖。具體實(shí)施例方式以下結(jié)合技術(shù)方案和附圖詳細(xì)敘述本發(fā)明的具體實(shí)施方式。為了驗(yàn)證本方法的有效性，利用如圖10所構(gòu)建的設(shè)備來進(jìn)行標(biāo)定方法的驗(yàn)證。采用SICKLMS291型激光傳感器，其平面掃描角度為0-180度，縱向范圍動態(tài)可調(diào)，分為8米、16米、32米(距離分辨率為1毫米)、80米(距離分辨率為1厘米)四個(gè)掃描范圍，此處采用32米掃描范圍。云臺電機(jī)具有可選旋轉(zhuǎn)角度分辨率，包括0.192°、0.288°、0.480°和0.960°。此處選取0.192。，起始角度為30。，掃描范圍為30。-130°。利用電機(jī)帶動激光傳感器獲得場景的三維激光測距數(shù)據(jù)。標(biāo)定裝置的尺寸采用4X5的黑白格，每個(gè)黑白格的邊長為25cm，在每個(gè)黑白格中心進(jìn)行鏤空，選取圓形作為鏤空形狀，半徑為5cm，如圖1所示。單目攝像頭采用FlyC即ture單目攝像頭，分辨率為1024X768，經(jīng)過標(biāo)定內(nèi)部參數(shù)為fx=894.43884277fy=893.06408691ux=504.93334961uy=368.08633423其中fx，fy分別為x，y方向的焦距，(ux，uy)是主點(diǎn)相對于圖像中心的偏移向量。連續(xù)掃描三幅不同角度和不同位置的場景，提取三維激光測距數(shù)據(jù)和單目視覺的匹配點(diǎn)?；谠摌?biāo)定裝置，采用的角點(diǎn)提取算法所提取的角點(diǎn)匹配對穩(wěn)定，受距離遠(yuǎn)近和傾斜角度變化的影響較小。標(biāo)定裝置與采集設(shè)備之間距離的合適范圍為2m-10m之間，標(biāo)定裝置傾角的合適范圍為-70°—70°之間。對3幅場景共36個(gè)角點(diǎn)采用高斯牛頓迭代法求解所得到的迭代誤差表如下<table>tableseeoriginaldocumentpage7</column></row><table>迭代次數(shù)3幅場景角點(diǎn)總誤差(像素)平均每個(gè)角點(diǎn)誤差(像素)210.35690.2877310.3550.2876410.3550.2876得到的三維激光測距系統(tǒng)與單目攝像機(jī)傳感器的外參數(shù)為令旋轉(zhuǎn)矩陣一0.0272，590.999625741.75325444e—003'-0.129452995,27446438e—003-0.99157155證—0.991209690,02684253(U2954853_令平移矩陣T=[5.38621889e-0030.44423267-0.15120429]由上表可以看出經(jīng)過4次迭代后，誤差為0.2876個(gè)像素。從定性的角度分析，利用場景圖像對三維激光測距數(shù)據(jù)點(diǎn)進(jìn)行染色形成的彩色三維激光點(diǎn)云效果圖，可以對標(biāo)定效果進(jìn)行直觀驗(yàn)證。附圖9給出了室外場景下的三維激光點(diǎn)云染色效果圖。1二權(quán)利要求一種三維激光和單目視覺間的自動標(biāo)定方法，其特征在于利用激光掃描鏤空黑白格標(biāo)定板時(shí)所產(chǎn)生的測距突變特性，通過原始三維激光數(shù)據(jù)與二值化后的測距數(shù)據(jù)矩陣間的映射關(guān)聯(lián)，基于測距數(shù)據(jù)的分布模式檢測激光數(shù)據(jù)特征角點(diǎn)，進(jìn)一步利用模板匹配技術(shù)校正特征點(diǎn)，從而與視覺角點(diǎn)形成匹配對，并根據(jù)迭代優(yōu)化算法確定三維激光與單目視覺間的標(biāo)定關(guān)系。2.根據(jù)權(quán)利要求l所述的一種三維激光和單目視覺間的自動標(biāo)定方法，其特征是:在黑白格標(biāo)定板每個(gè)方格的正中按同一尺寸制作鏤空圓孔，根據(jù)激光數(shù)據(jù)在圓L邊界所發(fā)生的測距突變，并結(jié)合由界定的區(qū)域數(shù)據(jù)所定義的平面擬合公式fl-arg，Hfl141，其中a=[a丄，a2，a3]T為平面法向量，qi=[x，y，z]t為區(qū)域坐標(biāo)點(diǎn)，計(jì)算得到二值化測距數(shù)據(jù)矩陣并統(tǒng)計(jì)其直方圖，并將檢測到的直方圖峰值點(diǎn)列的中值點(diǎn)設(shè)置為圓孔中心點(diǎn)。3.根據(jù)權(quán)利要求1所述的一種三維激光和單目視覺間的自動標(biāo)定方法，其特征是利用檢測到的圓孔中心點(diǎn)，按照四鄰域關(guān)系計(jì)算與黑白格角點(diǎn)相對應(yīng)的激光數(shù)據(jù)特征角點(diǎn)，并使用四元數(shù)法與由標(biāo)定板中黑白格角點(diǎn)所構(gòu)造的標(biāo)準(zhǔn)模板相匹配，校正初始激光數(shù)據(jù)特征角點(diǎn)的誤差，從而與視覺中提取的黑白格角點(diǎn)形成匹配對。全文摘要本發(fā)明屬于機(jī)器人自主環(huán)境感知
技術(shù)領(lǐng)域：
，涉及到三維激光和單目視覺間的自動標(biāo)定方法。其特征在于通過設(shè)計(jì)的標(biāo)定裝置完成三維激光和單目視覺之間的自動聯(lián)合標(biāo)定。激光數(shù)據(jù)特征角點(diǎn)的提取過程分為檢測與校正兩階段。檢測階段將原始三維激光數(shù)據(jù)與二值化后的測距數(shù)據(jù)矩陣進(jìn)行映射關(guān)聯(lián)與統(tǒng)計(jì)分析；校正階段通過與由標(biāo)定板中黑白格角點(diǎn)所構(gòu)造的標(biāo)準(zhǔn)模板相匹配，提高了角點(diǎn)提取精度，進(jìn)而采用迭代優(yōu)化方法進(jìn)行求解。本發(fā)明的效果益處是能夠有效減少噪聲和入射角對激光測距數(shù)據(jù)的影響，標(biāo)定過程實(shí)現(xiàn)了激光－視覺數(shù)據(jù)特征角點(diǎn)提取與匹配的自動化和一體化，從而為多傳感器信息融合奠定基礎(chǔ)。文檔編號B25J19/00GK101698303SQ20091018734公開日2010年4月28日申請日期2009年9月11日優(yōu)先權(quán)日2009年9月11日發(fā)明者莊嚴(yán),楊生鵬,王偉,陳東申請人:大連理工大學(xué)