一種基于激光測距傳感器的樓宇室內(nèi)平面圖建立方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及一種樓宇室內(nèi)平面圖建立方法，涉及室內(nèi)定位技術(shù)領(lǐng)域。
【背景技術(shù)】
[0002] 目前，基于WLAN技術(shù)的室內(nèi)定位技術(shù)日益成熟，一些以位置指紋算法為基礎(chǔ)的 算法能夠有效計(jì)算出室內(nèi)位置信息來實(shí)現(xiàn)室內(nèi)定位的功能。位置指紋定位法（Location Fingerprint，LF)是通過采集所需定位區(qū)域各參考節(jié)點(diǎn)（ReferencePoint，RP)位置的 信號特征參數(shù)即RSSI值，將一組指紋信息對應(yīng)與一個(gè)特定的位置形成位置指紋數(shù)據(jù)庫， 然后利用接收機(jī)測定接收信號的參數(shù)，采用匹配算法來判斷與數(shù)據(jù)庫中哪一組數(shù)據(jù)匹配 程度高，從而得出用戶的實(shí)際位置。LF算法由于不需要添加任何硬設(shè)備即可實(shí)現(xiàn)定位，且 精度較高。但如需快速采集并建立RadioMap則要求在較為精確的室內(nèi)地圖的基礎(chǔ)上才 能實(shí)現(xiàn)。一般精確的已知室內(nèi)地圖需要從建筑施工方去獲得，但如果在未知的室內(nèi)環(huán)境 中，或者在未能獲取準(zhǔn)確的室內(nèi)建筑圖紙的情況下，用戶需要能夠快速地自主建立地圖即 SLAM(SimultaneousLocalizationAndMapping)技術(shù)來構(gòu)建樓宇室內(nèi)圖紙，以此能夠在 此基礎(chǔ)上快速建立RadioMap，從而實(shí)現(xiàn)室內(nèi)定位。
[0003] 同時(shí)，依靠該激光測距傳感器的樓宇室內(nèi)平面圖建立方法可以估測移動(dòng)平臺和所 在環(huán)境特征的狀態(tài)，從而能夠?qū)崿F(xiàn)室內(nèi)環(huán)境中運(yùn)動(dòng)物體的軌跡跟蹤與定位。由于室內(nèi)環(huán)境 的整體輪廓基本上可認(rèn)為不變，對自身運(yùn)動(dòng)的激光測距裝置定位較為準(zhǔn)確。通過該算法快 速地求出前后相同環(huán)境的激光測量數(shù)據(jù)之間的旋轉(zhuǎn)和平移關(guān)系，從而依靠已求得的旋轉(zhuǎn)矩 陣R與平移向量T可以實(shí)現(xiàn)在室內(nèi)環(huán)境下的軌跡跟蹤。

【發(fā)明內(nèi)容】

[0004] 本發(fā)明的目的是基于激光測距傳感器對外部環(huán)境進(jìn)行數(shù)據(jù)測量，利用數(shù)據(jù)配準(zhǔn)和 校正優(yōu)化進(jìn)行二維地圖的快速構(gòu)建，同時(shí)能夠?qū)崿F(xiàn)室內(nèi)運(yùn)動(dòng)物體的軌跡跟蹤，以解決在室 內(nèi)定位領(lǐng)域快速建立RadioMap的過程中缺少室內(nèi)地圖的問題。
[0005] 本發(fā)明為解決上述技術(shù)問題采取的技術(shù)方案是：
[0006] -種基于激光測距傳感器的樓宇室內(nèi)平面圖建立方法，通過激光測距設(shè)備快速建 立樓宇室內(nèi)二維地圖，所述方法的實(shí)現(xiàn)過程為：
[0007] 步驟一、樓宇室內(nèi)激光測量數(shù)據(jù)的獲?。?br>[0008] 激光測距傳感器在待建立平面圖的室內(nèi)沿既定軌跡移動(dòng)，獲取多組激光測量數(shù) 據(jù)，各組激光測量數(shù)據(jù)為不同坐標(biāo)系下的獨(dú)立點(diǎn)集，且各組激光測量數(shù)據(jù)的坐標(biāo)原點(diǎn)為既 定軌跡上的點(diǎn)；
[0009] 步驟二、將多組激光測量數(shù)據(jù)產(chǎn)生的激光測量數(shù)據(jù)圖像進(jìn)行拼接：
[0010] 步驟二一、利用改進(jìn)ICP算法求取相鄰兩組激光測量數(shù)據(jù)的旋轉(zhuǎn)矩陣和平移矩 陣：以既定軌跡的出發(fā)點(diǎn)獲得的第一組激光測量數(shù)據(jù)的坐標(biāo)系為基準(zhǔn)坐標(biāo)系，通過改進(jìn) ICP算法求得相鄰下一組激光測量數(shù)據(jù)與其前一組激光測量數(shù)據(jù)的旋轉(zhuǎn)矩陣和平移矩陣， 即求得相鄰兩組數(shù)據(jù)產(chǎn)生圖像之間（即兩坐標(biāo)系間）的旋轉(zhuǎn)矩陣和平移矩陣，所述改進(jìn)ICP算法（基于激光測距傳感器的數(shù)據(jù)配準(zhǔn)算法）的輸入是相鄰的點(diǎn)集，輸出的是旋轉(zhuǎn)矩陣和 平移矩陣；
[0011] 步驟二二、然后將第二組及其以后的激光測量數(shù)據(jù)依靠所求取的對應(yīng)旋轉(zhuǎn)矩陣和 平移矩陣換算到前一坐標(biāo)系下的點(diǎn)集，不斷向前迭代，直至所有點(diǎn)集迭代到基準(zhǔn)坐標(biāo)系下， 從而完成激光測量數(shù)據(jù)圖像的拼接；
[0012] 步驟三、對拼接后的圖像依次進(jìn)行誤差幾何校正、多余數(shù)據(jù)的刪除、噪聲點(diǎn)濾除：
[0013] 通過對拼接后的圖像進(jìn)行旋轉(zhuǎn)校正或平移校正，使得拼接后的圖像大幅減少變形 誤差；
[0014] 通過數(shù)據(jù)抽稀的過程來刪除多余數(shù)據(jù)并同時(shí)保留特征點(diǎn)，采用垂距限值法進(jìn)行抽 ??；
[0015] 通過隨機(jī)濾波法對離散噪聲點(diǎn)進(jìn)行濾除；
[0016] 得到基于激光測距傳感器的樓宇室內(nèi)平面圖。
[0017] 在步驟二一中，利用改進(jìn)ICP算法求取相鄰兩組激光測量數(shù)據(jù)的旋轉(zhuǎn)矩陣和平移 矩陣，其具體過程為：
[0018] -、選取任意相鄰兩組激光測量數(shù)據(jù)的點(diǎn)集p1與q1，為求取兩組點(diǎn)集的旋轉(zhuǎn)矩陣， 將兩組點(diǎn)集中的點(diǎn)分別減去其質(zhì)心，得到新點(diǎn)集P1'與q/ ;
[0019] 二、根據(jù)剛體特性，將所述新點(diǎn)集中的點(diǎn)進(jìn)行卷積求平均，得到矩陣M與對稱矩陣 Ma;
[0020] 三、利用對稱矩陣Ma，采用四元數(shù)法計(jì)算旋轉(zhuǎn)矩陣R與平移矩陣T;
[0021] 四、計(jì)算旋轉(zhuǎn)平移后的點(diǎn)集Q1"與原始激光測量數(shù)據(jù)點(diǎn)集Q1的誤差，若小于給定 的閾值T，則返回步驟一重新選取新點(diǎn)集，直到迭代次數(shù)k大于默認(rèn)的最大迭代次數(shù)為止。
[0022] 對于利用改進(jìn)ICP算法求取相鄰兩組激光測量數(shù)據(jù)的旋轉(zhuǎn)矩陣和平移矩陣的過 程再進(jìn)一步限定為：
[0023] 步驟一：選取激光測量數(shù)據(jù)的相鄰兩組點(diǎn)集，點(diǎn)集P1與點(diǎn)集qi，并計(jì)算兩個(gè)點(diǎn)集的 質(zhì)心：
[0025] 為了排除兩點(diǎn)集平移帶來的影響，將原點(diǎn)集中的點(diǎn)分別減去質(zhì)心gp與gq，得到新 點(diǎn)集：
[0026] Pi' =Pi-gpq；' =qi-gq
[0027]步驟二：基于剛體上每個(gè)點(diǎn)的旋轉(zhuǎn)參量和平移參量都是相同的特性，將點(diǎn)集中的 每一個(gè)點(diǎn)的坐標(biāo)進(jìn)行卷積求平均，得到整體平均值，即利用新點(diǎn)集P1'和Q1'構(gòu)造3X3階 矩陣M:
[0029] 進(jìn)一步利用矩陣M構(gòu)造4X4對稱矩陣Ma:
[0031] 步驟三：旋轉(zhuǎn)參量和平移參量利用7個(gè)四元數(shù)（q。，qi,q2,q3,q4,q5,q6)表示，其中 (q0,qi,q2,q3)表征旋轉(zhuǎn)，（q4,q5,q6)表征平移，同時(shí)需滿足條件qQ>0且q^+q^+q^+q/=I; 計(jì)算對稱矩陣Ma的最大特征根對應(yīng)的單位特征向量即得到q(q。，qpq2,q3);
[0032] 那么旋轉(zhuǎn)矩陣R表達(dá)式為：
[0034] 再利用T=gq-Rgp得到平移矩陣T;
[0035] 步驟四：計(jì)算P1點(diǎn)集通過旋轉(zhuǎn)矩陣R和平移矩陣T后得到新點(diǎn)集：Pl"= R-P^ +T}
[0036] 并計(jì)算兩者之間的誤差
[0038] 如果d不小于給定的閥值T，返回步驟一重新選取新點(diǎn)集qi，直到迭代次數(shù)k大 于默認(rèn)的最大迭代次數(shù)為止。
[0039] 在步驟三中，對拼接后的圖像進(jìn)行多余數(shù)據(jù)的刪除、噪聲點(diǎn)濾除，其具體過程為：
[0040] -、采用垂距限值法對多余數(shù)據(jù)點(diǎn)進(jìn)行抽稀處理。首先以第一點(diǎn)為起點(diǎn)，計(jì)算第二 點(diǎn)至第一點(diǎn)和第三點(diǎn)連接的垂直距離，若此距離大于預(yù)定閾值，則保留第二點(diǎn)，并將其作為 新起點(diǎn)，計(jì)算第三點(diǎn)至第二點(diǎn)和第四點(diǎn)連接的距離；否則，去除第二點(diǎn)，計(jì)算第三點(diǎn)至第一 點(diǎn)和第四點(diǎn)聯(lián)機(jī)距離，進(jìn)行重復(fù)循環(huán)，直至曲線上最后一點(diǎn)。一般來說，閾值通常取相應(yīng)地 最大允許誤差或更小。
[0041] 二、由于激光測量設(shè)備在數(shù)據(jù)采樣過程中，不可避免地在真實(shí)數(shù)據(jù)點(diǎn)中混有不合 理的噪聲點(diǎn)，其結(jié)果將導(dǎo)致重構(gòu)的二維地圖不光滑，不連續(xù)。因此，對于獲取的噪聲點(diǎn)數(shù)據(jù) 進(jìn)行隨機(jī)濾波算法處理。該方法通過比較連續(xù)點(diǎn)之間的相對位置，給定一個(gè)閾值，將于其連 續(xù)點(diǎn)距離大于閥值的點(diǎn)判定為噪聲點(diǎn)并予以去除。通過對該數(shù)據(jù)點(diǎn)的周圍數(shù)據(jù)點(diǎn)數(shù)目多少 來判斷是否為噪聲點(diǎn)集，并進(jìn)行循環(huán)處理。
[0042] 本發(fā)明通過對采集到的室內(nèi)激光測量數(shù)據(jù)進(jìn)行配準(zhǔn)得到室內(nèi)地圖的拼接。數(shù)據(jù)配 準(zhǔn)是將兩個(gè)或兩個(gè)以上的不同坐標(biāo)系下的數(shù)據(jù)點(diǎn)集轉(zhuǎn)換到統(tǒng)一坐標(biāo)系下的數(shù)學(xué)過程。對于 二維地圖構(gòu)建來說，即需要求得兩組數(shù)據(jù)之間的關(guān)系（即旋轉(zhuǎn)矩陣和平移矩陣），通過旋轉(zhuǎn) 矩陣和平移矩陣進(jìn)行坐標(biāo)系變換實(shí)現(xiàn)將多組數(shù)據(jù)點(diǎn)集轉(zhuǎn)換到統(tǒng)一坐標(biāo)系下，實(shí)現(xiàn)二維平面 地圖的顯示。
[0043] 本發(fā)明通過采用ICP算法（迭代最近點(diǎn)法）并對其改進(jìn)，實(shí)現(xiàn)快速并精準(zhǔn)的數(shù)據(jù) 配準(zhǔn)。改進(jìn)的ICP算法其配準(zhǔn)的本質(zhì)即為求取2個(gè)不同坐標(biāo)系間的關(guān)系，通過循環(huán)反復(fù)進(jìn) 行對應(yīng)關(guān)系點(diǎn)集的剛體變換計(jì)算，直到表示正確匹配的收斂準(zhǔn)則得到滿足，即求取旋轉(zhuǎn)和 平移變換矩陣，使得來自兩點(diǎn)集的共同元素整體歐式距離最小。
[0044] 本發(fā)明通過對ICP算法中最優(yōu)化解析策略的改進(jìn)，選取四元數(shù)法來計(jì)算最優(yōu)匹配 準(zhǔn)則。其優(yōu)勢在于針對剛體運(yùn)動(dòng)時(shí)能減少運(yùn)算量、提高計(jì)算效率。
[0045] 由于激光數(shù)據(jù)拼接后成像的過程中會由于算法誤差累計(jì)等因素產(chǎn)生幾何畸變，如 偏移、拉伸、扭曲等。因此需要進(jìn)行誤差進(jìn)行幾何校正。本發(fā)明通過對拼接后的圖像進(jìn)行旋 轉(zhuǎn)平移校正，使得圖像大幅減少變形誤差。同時(shí)對于拼接后的圖像來說，數(shù)據(jù)會有重復(fù)，圖 像成像效果較不清晰，可以通過數(shù)據(jù)抽稀的過程來刪除一些多余點(diǎn)并同時(shí)保留特征點(diǎn)。其 中垂距限值法抽稀精度較高，且循環(huán)簡單，易于程序設(shè)計(jì)處理，對激光測距點(diǎn)的抽稀效果較 為理想。
[0046] 由于被測對象表面的粗糙度、紋理以及表面凹陷等，還有激光點(diǎn)抖動(dòng)的影響，其結(jié) 果將導(dǎo)致構(gòu)建的二維地圖不光滑，不連續(xù)。本發(fā)明通過隨機(jī)濾波法進(jìn)行噪聲點(diǎn)濾除將大幅 降低其影響。隨機(jī)濾波算法較為簡單易行，該方法通過比較連續(xù)點(diǎn)之間的相對位置，給定一 個(gè)閾值，將于其連續(xù)點(diǎn)距離大于閥值的點(diǎn)判定為噪聲點(diǎn)并予以去除。
[0047] 本發(fā)明的有益效果是：
[0048] 本發(fā)明針對在未知的室內(nèi)環(huán)境中缺少樓宇室內(nèi)地圖的問題，提出一種通過激光測 距傳感器對外部環(huán)境進(jìn)行數(shù)據(jù)測量，利用數(shù)據(jù)配準(zhǔn)（改進(jìn)ICP算法）和校正優(yōu)化進(jìn)行二維 地圖的快速構(gòu)建，同時(shí)能夠?qū)崿F(xiàn)室內(nèi)運(yùn)動(dòng)物體的軌跡跟蹤。
[0049] 本發(fā)明在室內(nèi)環(huán)境中快速建立RadioMap的過程中，為了用戶能快速得到樓宇室 內(nèi)地圖，通過對激光測距設(shè)備獲得的環(huán)境深度數(shù)據(jù)進(jìn)行相應(yīng)的數(shù)據(jù)配準(zhǔn)與進(jìn)一步的優(yōu)化， 從而滿足用戶對快速建立室內(nèi)二維地圖的要求，同時(shí)能夠?qū)崿F(xiàn)室內(nèi)運(yùn)動(dòng)物體的軌跡跟蹤。
[0050] 本發(fā)明方法能很好地快速構(gòu)建二維樓宇室內(nèi)地圖，從而保證了快速建立Radi〇Map，為解決室內(nèi)定位問題打下了基礎(chǔ)。<