本發(fā)明涉及SLAM(Simultaneous Localization And Mapping)、移動機(jī)器人、攝影測量、人工智能技術(shù)領(lǐng)域，具體涉及一種室內(nèi)移動定位與制圖方法。
背景技術(shù)：
室內(nèi)移動測量技術(shù)的關(guān)鍵在于室內(nèi)定位和制圖。一般在室外條件下主要依靠全球定位系統(tǒng)(GPS)、慣性測量單元(IMU)等傳感器定位實現(xiàn)高效的移動式測量方案。但是，室內(nèi)環(huán)境沒有GPS信號，傳統(tǒng)的定位方式在室內(nèi)環(huán)境下無用武之地，沒有位置信息就無法實現(xiàn)移動測量，另外傳統(tǒng)的定點室內(nèi)測量方式效率低下，制圖周期長，耗費(fèi)大量的人力。
技術(shù)實現(xiàn)要素：
為了解決上述技術(shù)問題，本發(fā)明提供了一種室內(nèi)移動定位和制圖方法，以解決傳統(tǒng)移動測量效率低下，制圖精度不高的問題。
本發(fā)明所采用的技術(shù)方案是：一種室內(nèi)移動定位與制圖方法，其特征在于，包括以下步驟：
步驟1：激光點云數(shù)據(jù)獲?。?/p>
步驟2：點云數(shù)據(jù)預(yù)處理；
步驟3：將輸入的極坐標(biāo)系下的點云數(shù)據(jù)轉(zhuǎn)換到笛卡爾坐標(biāo)系下；
步驟4：點云插值及下采樣；
步驟5：粒子濾波預(yù)測姿態(tài)；
步驟6：子地圖生成；
步驟7：虛擬標(biāo)靶及局部捆集約束計算；
步驟8：全局標(biāo)靶添加及約束計算；
步驟9：利用圖優(yōu)化進(jìn)行全局優(yōu)化；
步驟10：根據(jù)步驟9得到的優(yōu)化結(jié)果，更新軌跡和子地圖數(shù)據(jù)；
步驟11：將每一幀點云數(shù)據(jù)按照軌跡姿態(tài)進(jìn)行累加，生成全局地圖；
步驟12：輸出地圖。
本發(fā)明的有益效果包括：
1)不依賴GPS和IMU；
2)數(shù)據(jù)處理速度快；
3)易于并行處理；
4)制圖精度高，實現(xiàn)厘米級精度；
附圖說明
圖1是本發(fā)明實施例的流程圖；
具體實施方式
為了便于本領(lǐng)域普通技術(shù)人員理解和實施本發(fā)明，下面結(jié)合附圖及實施例對本發(fā)明作進(jìn)一步的詳細(xì)描述，應(yīng)當(dāng)理解，此處所描述的實施示例僅用于說明和解釋本發(fā)明，并不用于限定本發(fā)明。
請見圖1，本發(fā)明提供的一種室內(nèi)移動定位與制圖方法，包括以下步驟：
步驟1：激光點云數(shù)據(jù)獲取；
利用數(shù)據(jù)采集軟件同步獲取水平方向和豎直方向激光數(shù)據(jù)，且數(shù)據(jù)采集的前提是，激光需水平固定放置，不同激光的相對位置已標(biāo)定完成；
步驟2：點云數(shù)據(jù)預(yù)處理；
由于激光掃描儀在一定的掃描范圍內(nèi)時，數(shù)據(jù)精度較高，超過規(guī)定范圍，精度無法保證，因此設(shè)定合理閾值，剔除距離較遠(yuǎn)的點云數(shù)據(jù)，以保證輸入點云數(shù)據(jù)的精度；
步驟3：坐標(biāo)系變換；
將輸入的極坐標(biāo)系下的點云數(shù)據(jù)轉(zhuǎn)換到笛卡爾坐標(biāo)系下，便于后續(xù)處理；
步驟4：點云插值及下采樣；
由于激光掃描儀在掃描過程中，對于距離較遠(yuǎn)的物體，得到的點云數(shù)據(jù)較?。痪嚯x較近的物體，點云相對較密，這樣近處物體的激光數(shù)據(jù)位于同一點的概率比遠(yuǎn)處物體高，在概率統(tǒng)計模型中，近處點的概率會多次疊加，影響統(tǒng)計結(jié)果。為了消除此影響，對點云數(shù)據(jù)根據(jù)距離進(jìn)行非線性插值，這樣將點云密度增加為基本均勻的狀態(tài)，然后再對同一個點有多個值的進(jìn)行抽樣，這樣就達(dá)到了每一個位置對應(yīng)一個點云的效果。步驟如下：
步驟4.1：對于第i-1，i，i+1個點云數(shù)據(jù)d[i-1]，d[i]和d[i+1]，根據(jù)步驟3得到對應(yīng)的位置坐標(biāo)為(xi-1,yi-1)，(xi,yi)和(xi+1,yi+1)；
步驟4.2：判斷i-1與i之間是否產(chǎn)生了突變；如果min(di-1,i,di,i+1)<di-1,i+1，則認(rèn)為點云是連續(xù)的，未發(fā)生突變；則對這兩點點云之間進(jìn)行插值，插值個數(shù)為其中di-1,i表示i-1與i之間的距離，di,i+1表示i與i+1之間的距離，di-1,i+1表示i-1與i+1之間的距離；
步驟4.3：對插值后的點云，如果有某個(xi,yi)＝(xj,yj)，則丟掉其中一個點云，進(jìn)而達(dá)到下采樣的結(jié)果。
步驟5：粒子濾波預(yù)測狀態(tài)；
室內(nèi)定位最重要的部分就是預(yù)測每一幀掃描數(shù)據(jù)對應(yīng)的軌跡，粒子濾波提供了一種簡單快速的預(yù)測方法。首先給定目標(biāo)的初始位置，在該位置附近撒粒子，得到一個候選位置，對點云做如下處理：
步驟5.1：將當(dāng)前幀點云數(shù)據(jù)變換到候選位置所對應(yīng)的姿態(tài)下；
步驟5.2：根據(jù)已生成的地圖，統(tǒng)計每個點云數(shù)據(jù)是障礙物的概率，對概率求和；
步驟5.3：概率最大的位置則認(rèn)為是當(dāng)前的最優(yōu)位置，也是下一個候選位置；
步驟5.4：重復(fù)步驟5.1-步驟5.3，直到最優(yōu)位置不再變化或變化范圍小于預(yù)設(shè)閥值，則結(jié)束。
步驟6：子地圖生成；
即時定位和制圖過程中需要不斷探知新的地圖和軌跡，不斷擴(kuò)展的地圖將耗費(fèi)大量的內(nèi)存，且存在較大的冗余，另外我們發(fā)現(xiàn)在定位過程中與當(dāng)前幀鄰近的地圖數(shù)據(jù)才起到定位的參考作用，較遠(yuǎn)的地圖數(shù)據(jù)非但不能起到有效的定位參考作用甚至?xí)?dǎo)致定位錯誤。通過以上分析，設(shè)計了一個可擴(kuò)展的地圖集，地圖集內(nèi)可存放多個互為網(wǎng)格關(guān)系的子地圖，地圖集中的子地圖數(shù)量可任意擴(kuò)展，這樣不僅有效地減少了不必要的內(nèi)存冗余，還可根據(jù)采集環(huán)境的不同動態(tài)擴(kuò)展。對于動態(tài)地圖數(shù)據(jù)結(jié)構(gòu)，可以采取分段的形式保存子地圖。具體思路如下：
步驟6.1：設(shè)第一個落入地圖中的點云位置為地圖的起始位置start；
步驟6.2：當(dāng)有一個新的點云newPoint落入地圖中時，由于點云的連續(xù)性，其位置一定在start的8-鄰域中，則在該位置申請一小塊內(nèi)存，用于存儲newPoint的8-鄰域的點，n小塊內(nèi)存組成的地圖即為子地圖miniMap，且任意兩個miniMap需要有重疊；
步驟6.3：重復(fù)步驟6.2，生成最終地圖finalMap，且finalMap是不規(guī)則地圖的外接矩形，占用內(nèi)存最小。
步驟7：虛擬標(biāo)靶及局部捆集約束計算；
相鄰兩個子地圖之間通過虛擬標(biāo)靶和局部捆集約束聯(lián)系。首先計算兩個子地圖相交部分概率最大的幾個特征點，將這些特征點設(shè)置為虛擬標(biāo)靶，其次對每一個虛擬標(biāo)靶分別與這兩個子地圖包含的點云的軌跡計算約束，也就是在某個軌跡坐標(biāo)系中計算標(biāo)靶的姿態(tài)，即為局部捆集約束；
步驟8：全局標(biāo)靶添加及約束計算；
根據(jù)軌跡之間的距離可以初步計算出全局閉環(huán)，再利用它們所在的子地圖之間的匹配關(guān)系得到閉環(huán)約束。同樣地選取幾個顯著的特征點作為全局標(biāo)靶，并計算標(biāo)靶與對應(yīng)子地圖包含點云的軌跡之間的約束，具體方法為：
步驟8.1：設(shè)點A為子地圖miniMap_i的一個特征點，點B為子地圖miniMap_j的對應(yīng)于A的特征點，則(A,B)記為子地圖i和j的一對全局標(biāo)靶；
步驟8.2：分別計算A與子地圖miniMap_j，B與子地圖miniMap_i中所有軌跡的對應(yīng)關(guān)系，形成全局約束。
步驟9：利用圖優(yōu)化進(jìn)行全局優(yōu)化；
在點云逐幀匹配的過程中，誤差被逐漸累積，根據(jù)步驟7和步驟8中計算的約束，利用圖優(yōu)化算法解決大規(guī)模地圖生成過程中的累積誤差問題；
步驟10：更新軌跡和子地圖；
根據(jù)步驟9得到的優(yōu)化結(jié)果，更新軌跡和子地圖數(shù)據(jù)，得到較精確的結(jié)果；
步驟11：根據(jù)軌跡生成全局地圖；
將每一幀點云數(shù)據(jù)按照軌跡姿態(tài)進(jìn)行累加，得到最終的地圖；
步驟12：輸出地圖和軌跡；
根據(jù)需要的格式，保存地圖和軌跡數(shù)據(jù)。
本發(fā)明提供了一種不依賴GPS和IMU的室內(nèi)移動定位與制圖方法，該方法可以高效、快捷地獲取高精確、多細(xì)節(jié)的室內(nèi)建筑的二維和三維地圖，大大節(jié)約了時間成本和人為誤差，為室內(nèi)導(dǎo)航、VR體驗、展館虛擬瀏覽提供基礎(chǔ)數(shù)據(jù)服務(wù)，還可用于室內(nèi)基礎(chǔ)設(shè)施管理，快速全面采集室內(nèi)消防設(shè)施和室內(nèi)安防設(shè)施信息，實現(xiàn)室內(nèi)智能安防地理信息數(shù)據(jù)庫建設(shè)。
應(yīng)當(dāng)理解的是，本說明書未詳細(xì)闡述的部分均屬于現(xiàn)有技術(shù)。
應(yīng)當(dāng)理解的是，上述針對較佳實施例的描述較為詳細(xì)，并不能因此而認(rèn)為是對本發(fā)明專利保護(hù)范圍的限制，本領(lǐng)域的普通技術(shù)人員在本發(fā)明的啟示下，在不脫離本發(fā)明權(quán)利要求所保護(hù)的范圍情況下，還可以做出替換或變形，均落入本發(fā)明的保護(hù)范圍之內(nèi)，本發(fā)明的請求保護(hù)范圍應(yīng)以所附權(quán)利要求為準(zhǔn)。