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