一種基于點云的室內(nèi)動態(tài)場景slam方法及系統(tǒng)的制作方法
【專利摘要】本發(fā)明提出一種基于點云的室內(nèi)動態(tài)場景SLAM方法，包括進行基于多尺度的激光幀和概率圖配準(zhǔn)，結(jié)合當(dāng)前幀的信息對每一層歷史概率圖進行更新，包括根據(jù)配準(zhǔn)結(jié)果，將當(dāng)前幀轉(zhuǎn)換至歷史概率圖所在的坐標(biāo)系，得到轉(zhuǎn)換后的激光幀，進行概率圖生成，生成對應(yīng)的概率圖；結(jié)合當(dāng)前幀概率圖和歷史概率圖，對歷史概率圖進行更新，得到當(dāng)前歷史概率圖；直至所有激光幀處理完畢，得到激光器的運行軌跡和室內(nèi)動態(tài)環(huán)境的二維地圖。本技術(shù)方案能夠快速準(zhǔn)確的對動態(tài)場景進行構(gòu)圖，并完成自定位。
【專利說明】
一種基于點云的室內(nèi)動態(tài)場景SLAM方法及系統(tǒng)
技術(shù)領(lǐng)域
[0001] 本發(fā)明屬于機器人SLAM技術(shù)領(lǐng)域，特別是涉及一種基于點云的室內(nèi)動態(tài)場景SLAM 方法。
【背景技術(shù)】
[0002] 對機器人而言，在其做出下一個決定前，必須知道其自身所在的位置。在戶外場 景，全球定位系統(tǒng)(GPS)可以為機器人提供精確的定位和導(dǎo)航服務(wù)。然而在室內(nèi)場景，GPS信 號很微弱，機器人難以接收。因此，在室內(nèi)場景，機器人需要進行自定位。這一問題在機器人 領(lǐng)域中被定義為即時定位與地圖構(gòu)建(SLAM)，是實現(xiàn)真正全自主移動機器人的關(guān)鍵所在。
[0003] SLAM問題，作為機器人領(lǐng)域中最為經(jīng)典和基礎(chǔ)的問題，根據(jù)所使用的傳感器的不 同，其主要解決方法主要有三種:基于RGBD相機的SLAM，基于視覺的SLAM以及基于激光點云 的SLAM 〇
[0004] 基于RGBD相機的方法，如Kinect，該方法適用于較小的場景，可以高效準(zhǔn)確的構(gòu)建 地圖和進行實時定位。但是由于其傳感器的有效范圍有限，因此，并不適用于較大的場景。
[0005] 基于視覺的方法其成本較低，僅僅需要一個普通相機或者立體相機即可。但是當(dāng) 場景中紋理較為缺乏時，該方法很容易失敗。
[0006] 基于激光點云的方法，不存在上述兩種方法所存在的缺陷，該方法以其優(yōu)異的穩(wěn) 定性和有效性，被廣泛的應(yīng)用于無人車，無人機等智能機器人上。
[0007] 考慮到基于激光點云方法的優(yōu)異性，本發(fā)明擬采用基于激光點云的方法解決室內(nèi) 動態(tài)場景的定位和構(gòu)圖問題。

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

[0008] 為了穩(wěn)定準(zhǔn)確的對室內(nèi)陌生的動態(tài)場景進行構(gòu)圖，并進行自定位，本發(fā)明提供了 一種基于激光點云的動態(tài)場景下進行SLAM的技術(shù)方案。
[0009] 本發(fā)明所采用的技術(shù)方案包括一種基于點云的室內(nèi)動態(tài)場景SLAM方法，包括以下 步驟：
[0010] 步驟1，首先判斷是否t = 0,表示當(dāng)前激光掃描幀是否為第一幀，是則生成當(dāng)前幀 的概率圖M纟，并直接設(shè)置歷史概率圖Mt1 =M0S，然后等待下一幀，當(dāng)t不為0時，進入步驟 2;所述進行概率圖生成，包括進行激光幀輪廓提取，根據(jù)激光幀輪廓，結(jié)合二維高斯模型， 針對不同分辨率的概率圖，計算出每個格網(wǎng)的概率值大?。?br>[0011] 步驟2，基于多尺度的激光幀和概率圖配準(zhǔn)，實現(xiàn)如下，
[0012] 設(shè)待配準(zhǔn)的當(dāng)前幀為St，各層歷史概率圖分別為Mb，其中S = 1，2~S，S為概率圖 當(dāng)前層數(shù)，S為總層數(shù)，最頂層初始轉(zhuǎn)換矩陣為It1，采用上一幀最頂層的轉(zhuǎn)換矩陣，
[0013] 將當(dāng)前幀配準(zhǔn)至歷史概率圖時，在每一層中通過結(jié)合窮舉法和梯度下降法優(yōu)化能 量函數(shù)，尋找到兩者間的最優(yōu)轉(zhuǎn)化矩陣存，
[0014] 能量函數(shù)如下所示，
[0015]
[0016] 其中，x表示激光幀S t中的一個點，T/ <g) x表示x經(jīng)過尨轉(zhuǎn)換后的點， P(7^? XlMlL1)表示轉(zhuǎn)換后的點在歷史概率圖上的概率值，w(x)表示每個點的權(quán)重值， ML 1為某一層的歷史概率圖；
[0017] 步驟3,概率圖更新，當(dāng)前幀和歷史概率圖配準(zhǔn)后，結(jié)合當(dāng)前幀的信息對每一層歷 史概率圖進行更新，從而可以將新的概率圖用于下一幀的配準(zhǔn)中；
[0018] 對每一層的概率圖進行更新，實現(xiàn)方式如下，
[0019] 設(shè)當(dāng)前層數(shù)為s，令腿η = Mh，Mlt =廁Mt = Mf;根據(jù)步驟2的配準(zhǔn)結(jié)果，將 當(dāng)前幀St轉(zhuǎn)換至歷史概率圖M^1所在的坐標(biāo)系，得到轉(zhuǎn)換后的激光幀；，針對舄進行概率 圖生成，生成對應(yīng)的概率圖Mt;
[0020] 所述進行概率圖生成，包括進行激光幀輪廓提取，根據(jù)激光幀輪廓，結(jié)合二維高斯 模型，針對不同分辨率的概率圖，計算出每個格網(wǎng)的概率值大?。?br>[0021] 結(jié)合當(dāng)前幀概率圖Mt和歷史概率圖，對歷史概率圖進行更新，得到當(dāng)前歷史 概率圖Mt;
[0022] 步驟4,返回步驟1，直至所有激光幀處理完畢，則終止迭代，得到激光器的運行軌 跡和室內(nèi)動態(tài)環(huán)境的二維地圖。
[0023]而且，步驟2的實現(xiàn)方式包括如下步驟，
[0024]步驟2.1，點云法向量直方圖統(tǒng)計，包括統(tǒng)計激光幀中所有點的法向量分布直方圖 對，直方圖個數(shù)為B;
[0025] 步驟2.2，權(quán)值估算，每個點的權(quán)值Wb計算公式如下所示，
[0026]
[0027] 其中^表示第b個直方圖的大小，其中b = l，2，~B;
[0028] 步驟2.3,通過s是否等于1，判斷當(dāng)前層是否為最頂層，若為最頂層，則該層的初始 轉(zhuǎn)換矩陣為上一幀傳遞過來的轉(zhuǎn)換矩陣;若不是最頂層，則該層的初始轉(zhuǎn)換矩陣為上一 層傳遞下來的轉(zhuǎn)換矩陣
[0029] 步驟2.4，根據(jù)步驟2.3確定的轉(zhuǎn)化矩陣獲得LM算法初始值；
[0030] 步驟2.5,采用LM算法優(yōu)化能量函數(shù)以心M^1)，求解得到最優(yōu)轉(zhuǎn)換矩陣7?'，并估 計得到激光器的速度Ff;
[0031 ] 步驟2 · 6，當(dāng)s不等于S時，令s = s+1，返回步驟2 · 5，當(dāng)s等于S時，結(jié)束迭代進入步驟 3〇
[0032] 而且，步驟3中，結(jié)合當(dāng)前幀概率圖Mt和歷史概率圖Ut-i，對歷史概率圖進行更新， 實現(xiàn)方式如下，
[0033] 針對概率圖中的每一個格網(wǎng)g，表示為M(g) = fMI.p(g), M:氣gl Mn(g)j，其中 RT(g) e: [0,1]和e [ο,ι]，分別表示概率圖中該格網(wǎng)的概率值，和過去所達到的最高 的概率值，RT(g)表示該格網(wǎng)在過去被觀察到的次數(shù);概率圖的更新分為4種情況，分別如 下所示，
[0034] 如果該格網(wǎng)g在當(dāng)前幀概率圖Mt和歷史概率圖·上，概率值Mf(g)和M^1Cg)都 為〇,則不進行任何更新；
[0035] 如果在當(dāng)前幀概率圖Mt上，Mf(g) > OjmM^(g) 2 1，則按照下述公式進行更新，
[0036]
[0037] 其中，^是概率值增長率，計算方式如下，
[0038]
[0039]其中，W1是預(yù)設(shè)的學(xué)習(xí)率，τ是控制參數(shù)；
[0040] 如果在當(dāng)前幀概率圖Mt上，蹲(g) = 〇,同時，廁Ug) S nift，其中nth為預(yù)設(shè)的閾 值，則按照下述公式進行更新，
[0041]
[0042] 其中，Wd表示概率下降速率，其計算方式如下，
[0043]
[0044] 針對剩下的未處理的格網(wǎng)，采用內(nèi)插的方式進行更新，包括用《^和@^表示概率值 增長了和下降了的格網(wǎng)，針對待內(nèi)插的格網(wǎng)g，首先根據(jù)高斯函數(shù)內(nèi)插其增長和下降的概率 值Pu和Pd，計算公式如下所示：
[0045]
[0046] 如果|pu| > |pd|，則p=pu，反;2l，p=pd，通過十述公式對格N概率值進行更新：
[0047]
[0048] 其中，d(g，gk)表不格網(wǎng)g和格網(wǎng)gk之間的距離，其中g(shù)k代表*Sti或者:中的一個格 網(wǎng)，S 1表示高斯函數(shù)的核大，用p代表該格網(wǎng)增長或下降的概率值大小。
[0049] 本發(fā)明提供一種基于點云的室內(nèi)動態(tài)場景SLAM系統(tǒng)，包括以下模塊：
[0050] 第一模塊，用于首先判斷是否t = 0,表示當(dāng)前激光掃描幀是否為第一幀，是則生成 當(dāng)前幀的概率圖M0S，并直接設(shè)置歷史概率圖Mfq = M^然后等待下一幀，當(dāng)t不為0時，命 令第二模塊工作；
[0051] 所述進行概率圖生成，包括進行激光幀輪廓提取，根據(jù)激光幀輪廓，結(jié)合二維高斯 模型，針對不同分辨率的概率圖，計算出每個格網(wǎng)的概率值大?。?br>[0052]第二模塊，用于基于多尺度的激光幀和概率圖配準(zhǔn)，實現(xiàn)如下，
[0053] 設(shè)待配準(zhǔn)的當(dāng)前幀為St，各層歷史概率圖分別為Mt1，其中S = 1，2~S，S為概率圖 當(dāng)前層數(shù)，S為總層數(shù)，最頂層初始轉(zhuǎn)換矩陣為7t:1，采用上一幀最頂層的轉(zhuǎn)換矩陣，
[0054] 將當(dāng)前幀配準(zhǔn)至歷史概率圖時，在每一層中通過結(jié)合窮舉法和梯度下降法優(yōu)化能 量函數(shù)，尋找到兩者間的最優(yōu)轉(zhuǎn)化矩陣療，
[0055] 能量函數(shù)如下所示，
[0056]
[0057] 其中，X表示激光幀St中的一個點，療?^：表示X經(jīng)過:rf轉(zhuǎn)換后的點， Ρ0-/? χ|Μ乙J表示轉(zhuǎn)換后的點在歷史概率圖上的概率值，w(x)表示每個點的權(quán)重值， Mlf-I為某一層的歷史概率圖；
[0058]第三模塊，用于概率圖更新，當(dāng)前幀和歷史概率圖配準(zhǔn)后，結(jié)合當(dāng)前幀的信息對每 一層歷史概率圖進行更新，從而可以將新的概率圖用于下一幀的配準(zhǔn)中；
[0059] 對每一層的概率圖進行更新，實現(xiàn)方式如下，
[0060] 設(shè)當(dāng)前層數(shù)為S，令離= Mt =轉(zhuǎn)，Mt = MM艮據(jù)第二模塊的配準(zhǔn)結(jié) 果，將當(dāng)前幀St轉(zhuǎn)換至歷史概率圖蘭t_：t所在的坐標(biāo)系，得到轉(zhuǎn)換后的激光幀茹，針對S?進行 概率圖生成，生成對應(yīng)的概率圖M t;
[0061] 所述進行概率圖生成，包括進行激光幀輪廓提取，根據(jù)激光幀輪廓，結(jié)合二維高斯 模型，針對不同分辨率的概率圖，計算出每個格網(wǎng)的概率值大??；
[0062] 結(jié)合當(dāng)前幀概率圖Mt和歷史概率圖，對歷史概率圖進行更新，得到當(dāng)前歷史 概率圖Mt;:
[0063]第四模塊，用于命令第一模塊，直至所有激光幀處理完畢，則終止迭代，得到激光 器的運行軌跡和室內(nèi)動態(tài)環(huán)境的二維地圖。
[0064]而且，第二模塊的實現(xiàn)方式包括如下子模塊，
[0065] 第一子模塊，用于點云法向量直方圖統(tǒng)計，包括統(tǒng)計激光幀中所有點的法向量分 布直方圖:H%直方圖個數(shù)為B;
[0066] 第二子模塊，用于權(quán)值估算，每個點的權(quán)值Wb計算公式如下所示，
[0067]
[0068] 其中A表示第b個直方圖的大小，其中b = l，2，'"B;
[0069] 第三子模塊，用于通過s是否等于1，判斷當(dāng)前層是否為最頂層，若為最頂層，則該 層的初始轉(zhuǎn)換矩陣為上一幀傳遞過來的轉(zhuǎn)換矩陣Tt 1;若不是最頂層，則該層的初始轉(zhuǎn)換矩 陣為上一層傳遞下來的轉(zhuǎn)換矩陣-1;
[0070] 第四子模塊，用于根據(jù)第三子模塊確定的轉(zhuǎn)化矩陣獲得LM算法初始值；
[0071] 第五子模塊，用于采用LM算法優(yōu)化能量函數(shù)求解得到最優(yōu)轉(zhuǎn)換矩陣 符，并估計得到激光器的速度F/;
[0072]第六子模塊，用于當(dāng)s不等于S時，令s = s+l，命令第五子模塊工作，當(dāng)s等于S時，結(jié) 束迭代命令第三模塊工作。
[0073] 而且，第三模塊中，結(jié)合當(dāng)前幀概率圖Mt和歷史概率圖Mt-P對歷史概率圖進行更 新，實現(xiàn)方式如下，
[0074] 針對概率圖中的每一個格網(wǎng)g，表示為
，其中 M〃(g) e丨〇,1]和K#(g) e 分別表示概率圖中該格網(wǎng)的概率值，和過去所達到的最高 的概率值，Mra(g)表示該格網(wǎng)在過去被觀察到的次數(shù);概率圖的更新分為4種情況，分別如 下所示，
[0075] 如果該格網(wǎng)g在當(dāng)前幀概率圖Mt和歷史概率圖顯卜:上，概率值Mf(g)和_|_ Jg)都 為0,則不進行任何更新；
[0076] 如果在當(dāng)前幀概率圖Mt上，Mf (g) > O并且Mtn(g)乏：U則按照下述公式進行更新，
[0077]
[0078] 其中，^是概率值增長率，計算方式如下，
[0079]
[0080]其中，W1是預(yù)設(shè)的學(xué)習(xí)率，τ是控制參數(shù)；
[0081] 如果在當(dāng)前幀概率圖Mt上，Mf (g) = 0,同時，MUg) S 其中nth為預(yù)設(shè)的閾 值，則按照下述公式進行更新，
[0082]
[0083] 其中，Wd表示概率下降速率，其計算方式如下，
[0084]
[0085] 針對剩下的未處理的格網(wǎng)，采用內(nèi)插的方式進行更新，包括用表示概率值 增長了和下降了的格網(wǎng)，針對待內(nèi)插的格網(wǎng)g，首先根據(jù)高斯函數(shù)內(nèi)插其增長和下降的概率 值Pu和Pd，計算公式如下所示：
[0086]
[0087] 如果|pu| > |pd|，貝>Jp=pu，反之，p=pd，通過下述公式對格網(wǎng)概率值進行更新： [0088]
[0089] 其中，d (g，gk)表不格網(wǎng)g和格網(wǎng)gk之間的距離，其中g(shù)k代表Gu.或者Gd _中的一個格 網(wǎng)，S1表示高斯函數(shù)的核大，用p代表該格網(wǎng)增長或下降的概率值大小。
[0090] 相對于現(xiàn)有技術(shù)，本發(fā)明的有益效果是:本技術(shù)方案能夠快速準(zhǔn)確的對動態(tài)場景 進行構(gòu)圖，并完成自定位。在概率圖生成過程中，本技術(shù)方案對傳統(tǒng)的輪廓模型進行了改 進，使其能夠更好的對場景進行表達。在概率圖配準(zhǔn)過程中，本技術(shù)方案結(jié)合了常用的窮舉 法和梯度下降法解求旋轉(zhuǎn)平移矩陣，即利用窮舉法選擇在較大的窗口中選擇較為準(zhǔn)確的初 始值，再采用梯度下降法求得精確值，在保證結(jié)果的準(zhǔn)確性的基礎(chǔ)上盡可能的避免落入局 部最小。此外，還采用了由粗到精的配準(zhǔn)策略，現(xiàn)在最頂層(分辨率最低)的概率圖上進行配 準(zhǔn)，將結(jié)果傳遞至下一層，直至底層，多尺度的配準(zhǔn)方法很好的提高了配準(zhǔn)效率和精度。為 了補償由于機器人速度所帶來的激光點云的變形，在配準(zhǔn)過程中進行了速度補償。在概率 圖更新過程中，本技術(shù)方案借鑒背景建模的方法，通過控制概率圖中每個格網(wǎng)的概率值大 小的變化，即靜態(tài)目標(biāo)所在的格網(wǎng)概率值增大，動態(tài)目標(biāo)所在的格網(wǎng)概率值減小，從而減少 動態(tài)目標(biāo)對概率圖準(zhǔn)確性的影響。
【附圖說明】
[0091 ]圖1為本發(fā)明實施例的整體流程圖。
[0092]圖2為本發(fā)明實施例的最終概率示意圖。
【具體實施方式】
[0093]具體實施時，本發(fā)明技術(shù)方案所提供流程可由本領(lǐng)域技術(shù)人員采用計算機軟件技 術(shù)實現(xiàn)自動運行。為了更好地理解本發(fā)明的技術(shù)方案，下面結(jié)合附圖和實施例對本發(fā)明做 進一步的詳細(xì)說明。
[0094]參見附圖1，本發(fā)明實施例包括以下步驟：
[0095]步驟1:首先判斷是否t = 0,即當(dāng)前激光掃描幀是否為第一幀，是則生成當(dāng)前幀的 概率圖MJ，并直接設(shè)置歷史概率圖M^1 = Mg，然后等待下一幀，當(dāng)t不為0時，進入步驟2。
[0096] 具體實施時，可以在流程開始首先判斷當(dāng)前激光掃描幀，判斷是否t = 0。若當(dāng)前幀 為第一幀，也即t = 0,則轉(zhuǎn)換矩陣為0,則生成當(dāng)前幀各層概率圖，也即Mg，并直接設(shè)置歷 史概率圖Mt1 = 步驟2的配準(zhǔn)和步驟3的歷史概率圖更新實際上不需執(zhí)行。若當(dāng)前幀不 是第一幀，則正常執(zhí)行步驟2將當(dāng)前幀和歷史概率圖進行配準(zhǔn)，然后進入步驟3進行歷史概 率圖更新。
[0097] 本發(fā)明進一步提供概率圖生成方式。在傳統(tǒng)的激光幀配準(zhǔn)算法中，其所采用的策 略多為兩兩配準(zhǔn)。配準(zhǔn)過程中不可避免的會存在誤差，而在傳統(tǒng)的兩兩匹配中，誤差會進行 積累，最終，導(dǎo)致"漂移"現(xiàn)象的產(chǎn)生。本發(fā)明為了避免誤差的累積，將當(dāng)前幀和歷史所有幀 進行配準(zhǔn)，而存儲所有幀的數(shù)據(jù)，其所消耗的計算機內(nèi)存巨大。因此，本發(fā)明，擬采用概率圖 對歷史幀進行存儲。
[0098] 實施例所述進行概率圖生成，包括進行激光幀輪廓提取，根據(jù)激光幀輪廓，結(jié)合二 維高斯模型，針對不同分辨率的概率圖，計算出每個格網(wǎng)的概率值大小，具體包括以下子步 驟：
[0099] 步驟1.1:激光幀輪廓提取。在室內(nèi)場景中，其主要的場景可認(rèn)為由多個面組成，在 二維激光掃描幀中，則可以使用輪廓信息對室內(nèi)場景進行更加精確的表達。首先，需要從激 光幀中精確的提取出輪廓信息。實施例具體包括以下子步驟：
[0100] 步驟1. 1.1:相鄰點間距離計算。針對激光幀中的所有相鄰點對(HXk)，計算所 有點對之間的歐式距離，記為{cy Cift = Iixft -Xnim，其中κ為激光幀中點的個數(shù)。
[0101] 步驟1.1.2:種子點對選擇。當(dāng)點對(Xk-^xk)滿足如下兩個條件時，則認(rèn)為這是一 個種子點對:第一，d k為最小的距離且小于預(yù)定的距離閾值dth(具體實施時本領(lǐng)域技術(shù)人員 可預(yù)設(shè)取值，本實施例為150毫米）；第二，點 Xk-dPXkF屬于任何一條輪廓。
[0102] 步驟1.1.3:輪廓增長。針對當(dāng)前種子點對，根據(jù)其鄰域關(guān)系，得到其鄰域點{Xl}， 若滿足下述增長條件，則進行增長，否則，不增長。增長條件如下所示：
[0103]
[0104]其中，Cl1表示點X1和點XH之間的歐式距離，X謙示當(dāng)前輪廓中和X1相鄰的其中一 個端點，dj表示Xj和點Xj-i之間的歐式距離，Xi是Xj的下一個點或者上一個點，dth和rth分別是 距離閾值和比例閾值(具體實施時本領(lǐng)域技術(shù)人員可預(yù)設(shè)取值，本實施例中dth = 150毫米 rth=2)。迭代的進行增長，即當(dāng)前點增長完之后，再以增長后的點作為起點，再進行增長，直 至不能再增長，則當(dāng)前輪廓提取完成。
[0105] 步驟1.1.4:對下一種子點對，重復(fù)步驟1.1.2和步驟1.1.3,直至所有的輪廓提取 完成。
[0106] 步驟1.1.5:當(dāng)輪廓線長度小于Lth時(具體實施時本領(lǐng)域技術(shù)人員可預(yù)設(shè)取值，本 實施例設(shè)為5)，考慮其為噪聲，并將其去除，最終得到所有的有效的輪廓，記為C。
[0107] 步驟1.2:格網(wǎng)概率值內(nèi)插。根據(jù)所獲得的輪廓模型，結(jié)合二維高斯模型，針對不同 分辨率的概率圖，計算出其每個格網(wǎng)的概率值大小，得到多尺度的概率圖。
[0108] 設(shè)置不同格網(wǎng)大小，可以分別生成不同分辨率下的概率圖。具體實施時本領(lǐng)域技 術(shù)人員可預(yù)設(shè)多種不同的金字塔分辨率，實現(xiàn)多尺度格網(wǎng)大小設(shè)置。本實施例中，生成三層 不同分辨率的概率圖，其格網(wǎng)大小從大到小在本實施實例中分別設(shè)為40,20,10毫米，因此 可以在每一層生成得到總共三層概率圖。
[0109] 實施例中，對每種格網(wǎng)大小，分別具體執(zhí)行包括以下子步驟：
[0110] 步驟1.2.1:根據(jù)當(dāng)前的格網(wǎng)大小，確定X軸和y軸，針對同一條輪廓線的一對點 (Xi,Xj)，設(shè)定X軸為Xi到Xj的直線方向，垂直于X軸的方向為y軸。
[0111] 步驟1.2.2:計算格網(wǎng)概率值。其計算公式如下所示：
[0112]

[0113] 其中，e為數(shù)學(xué)常量，X為當(dāng)前格網(wǎng)中心點，代表其概率值，Pmax為最大的概率 值(具體實施時，本領(lǐng)域技術(shù)人員可自行設(shè)定取值，本實施例設(shè)為0.5)，δ4Ρδγ分別代表沿X 軸和y軸的高斯核大小（具體實施時，本領(lǐng)域技術(shù)人員可自行設(shè)定取值，本實施例分別設(shè)置 為1.5和10)。之(\([:)和40,￡)表示當(dāng)前格網(wǎng)沿1軸和 7軸到輪廓的距離，計算公式如下所 示：
[0114]
[0115] 其中，dx(x，xk)表示X沿X軸到輪廓上的點紅的距離，表示X到輪廓的距離。
[0116] 步驟2:基于多尺度的激光幀和概率圖配準(zhǔn)。假設(shè)當(dāng)前待配準(zhǔn)幀為St，各層歷史概 率圖分別為廁!_:1，其中 S = 1，2~S，S為概率圖當(dāng)前層數(shù)，S為總層數(shù)(本實施例中為3)，最頂 層初始轉(zhuǎn)換矩陣為FtL 1，即為上一幀最頂層的轉(zhuǎn)換矩陣。
[0117] 為了將當(dāng)前幀配準(zhǔn)至歷史概率圖上，本發(fā)明采用了一種由粗到精的配準(zhǔn)方式，在 每一層中通過結(jié)合窮舉法和梯度下降法優(yōu)化能量函數(shù)，尋找到兩者間的最優(yōu)轉(zhuǎn)化矩陣療， 轉(zhuǎn)換矩陣主要由三個參數(shù)進行控制，分別是旋轉(zhuǎn)角θ，χ軸方向的偏移量d x，y軸方向的偏移 量dy。能量函數(shù)如下所示：
[0118]
[0119] 其中，X表示激光幀St中的一個點，療?X表示X經(jīng)過7/轉(zhuǎn)換后的點， 表示轉(zhuǎn)換后的點在歷史概率圖上的概率值，w(x)表示每個點的權(quán)重值， 屬L1為某一層的歷史概率圖。
[0120] 實施例具體包括以下子步驟:步驟2.1:點云法向量直方圖統(tǒng)計。為了計算每個點 的權(quán)值大小，即計算其對能量函數(shù)的貢獻。首先，需要統(tǒng)計激光幀中所有點的法向量分布直 方圖況'，直方圖個數(shù)為B(本實施例中B = 12)。為了排除諸如噪聲等外點對法向量估算的影 響，本發(fā)明采用Robust Diagnostic PCA(RDPCA)算法魯棒的計算每個點的法向量。
[0121 ]步驟2.2:權(quán)值估算。每個點的權(quán)值Wb計算公式如下所示：
[0122]
[0123]其中私表示第b個直方圖的大小，也即落在該直方條內(nèi)的點的個數(shù)，其中b = l， 2, .·_Β〇
[0124]通過步驟2.1和步驟2.2，可以求得當(dāng)前幀每一個點的權(quán)值，該權(quán)值可以應(yīng)用于每 一層的概率圖配準(zhǔn)中。為了防止解算得到的配準(zhǔn)結(jié)果為局部最優(yōu)，采用了由粗到精的配準(zhǔn) 方式，因此，在實施例中，對每一層概率圖的配準(zhǔn)，迭代的執(zhí)行以下步驟，直至最底層。
[0125] 步驟2.3:判斷當(dāng)前層是否為最頂層，也即s是否等于1。若為最頂層，則該層的初始 轉(zhuǎn)換矩陣為上一幀傳遞過來的轉(zhuǎn)換矩陣，也即為7?^。若不是最頂層，則該層的初始轉(zhuǎn)換矩 陣為上一層傳遞下來的轉(zhuǎn)換矩陣，也即ΓΓ 1 〇
[0126] 步驟2.4:根據(jù)步驟2.3確定的轉(zhuǎn)化矩陣采用窮舉法獲得LM算法初始值：
[0127] 由于梯度下降法較易于落入局部最優(yōu)，因此，在采取梯度下降法優(yōu)化配準(zhǔn)能量函 數(shù)前，先采用窮舉法搜索到一個較好的初始值，提高梯度下降法的效率和穩(wěn)定性。根據(jù)步驟 2.3所得轉(zhuǎn)換矩陣的三個參數(shù)(Θ，d x，dy)，建議設(shè)定一個較窄的搜索窗口（ Θ ± WO，dx± Wx，dy 土 wy)(具體實施時本領(lǐng)域技術(shù)人員可預(yù)設(shè)取值，本實施例中旋轉(zhuǎn)平移矩陣中的三個參數(shù)其搜 索窗口大小分別設(shè)置如下:we = 5°，wx=10mm，wy=10mm)，再設(shè)定一個較大的搜索歩距(se, sx，S y)(具體實施時本領(lǐng)域技術(shù)人員可預(yù)設(shè)取值，本實施例中se = 0.5°，Sx= I，Sy = 1)。通過 在這個窗口內(nèi)搜索，找到一個最好的初始轉(zhuǎn)換矩陣Ivt，作為梯度下降法的初始值。在一個 較窄的搜索空間中，以一個較大的搜索歩距進行搜索，因此可以減少搜索的次數(shù)，從而提高 算法的效率，減少初始值搜索的時間。
[0128] 步驟2.5:梯度下降法精確計算轉(zhuǎn)換矩陣并進行速度估計和補償。為了獲得更加精 確的轉(zhuǎn)換矩陣，并對速度進行估計和補償，本發(fā)明采用Levenberg-Marquardt (LM)算法(梯 度下降算法的一種)優(yōu)化上述能量函數(shù)求解得到最優(yōu)轉(zhuǎn)換矩陣T/，并估計得到 激光器的速度Vts。實施例具體包括以下子步驟：
[0129] 步驟2.5.1:速度估算。為了補償由于激光器速度所造成的激光幀畸變，需要在配 準(zhǔn)的過趕中對激光器的諫庶講桿估笪和補償。當(dāng)前激光器速度V?估算公式如下所示：
[0130]
[0131]其中，Δ t為激光器掃描一幀所需要的時間（本實施例激光器頻率為50Hz，所以
表示上一幀的轉(zhuǎn)換矩陣的逆。
[0132] 步驟2.5.2:速度補償。通過估算的速度，在進行一次LM優(yōu)化迭代之前，按照下述公 式對激光幀St進行速度補償：
[0133]
[0134] 其中，4代表未補償?shù)募す鈳琒t中的一點，藏代表補償后的激光幀矣中的一點，其 中k=l，2,3…K，其中K為一個激光掃描幀中的點的個數(shù)(本實施例中K = 540)，而Ats = ·ρ 表不激光器掃描一個點所需要的時間。
[0135] 步驟2.5.3 :LM優(yōu)化。針對補償后的激光幀皂和歷史概率圖ML1，采用LM算法優(yōu)化 其配準(zhǔn)能量函數(shù)，一次迭代之后，得到新的轉(zhuǎn)換矩陣療，且迭代次數(shù)加1。
[0136] 步驟2.5.4:根據(jù)新得到的轉(zhuǎn)換矩陣療，根據(jù)步驟2.5.1對速度進行更新，并用新的 速度根據(jù)步驟2.5.2對速度進行補償，再進行步驟2.5.3。即返回重復(fù)步驟2.5.1-2.5.3，直 到滿足迭代結(jié)束條件，例如LM算法收斂，或迭代次數(shù)大于預(yù)設(shè)的最大迭代次數(shù)I max(具體實 施時，本領(lǐng)域技術(shù)人員可自行預(yù)設(shè)取值，本實施例設(shè)為1000次），進入步驟2.6。
[0137] 步驟2.6:為實現(xiàn)基于多尺度的概率圖配準(zhǔn)，當(dāng)s不等于S時，令s = s+l，返回步驟 2.5,當(dāng)s等于S時，結(jié)束迭代進入步驟3。迭代步驟2.3到步驟2.5,也即完成每一層的配準(zhǔn)。將 當(dāng)前層的配準(zhǔn)結(jié)果，也即當(dāng)前層得到的旋轉(zhuǎn)平移矩陣的三個參數(shù)(9，d x，dy)傳遞到下一層， 作為下一層的初始值，直至傳遞至最底層，從而完成由粗到精的配準(zhǔn)，結(jié)束迭代進入步驟3。
[0138] 步驟3:概率圖更新。當(dāng)前激光掃描幀和歷史概率圖配準(zhǔn)后，需要結(jié)合當(dāng)前幀的信 息對每一層歷史概率圖進行更新，從而可以將新的概率圖用于下一幀的配準(zhǔn)中。
[0139] 對每一層的概率圖更新，實施例具體包括以下子步驟：
[0140] 步驟3.1:層數(shù)設(shè)置。在概率圖更新中，需要對每一層的概率圖進行更新，每一層的更新 策略完全一致。假設(shè)當(dāng)前層數(shù)為8，在步驟3.1和步驟3.2中，麗4-1. =麗纟-1.，腿《；=麗?，.噸::=_財|0
[0141] 步驟3.2:當(dāng)前幀概率圖生成。通過步驟2,可以將當(dāng)前幀St轉(zhuǎn)換至歷史概率圖Mt-I 所在的坐標(biāo)系，得到轉(zhuǎn)換后的激光幀筘，針對$，采用如步驟1中的概率圖生成方法生成對應(yīng) 的概率圖M t。
[0142] 步驟3.3:概率圖更新。結(jié)合當(dāng)前幀概率圖Mt和歷史概率圖Mp1，對歷史概率圖進 行更新，得到當(dāng)前歷史概率圖Μ ?:。針對概率圖中的每一個格網(wǎng)g，本發(fā)明實施例將其表示為
其中MP(g) e [0,1]和S#(g) e [0 lj，分別表示概率圖中該 格網(wǎng)的概率值，和過去所達到的最高的概率值，Mn (g)表示該格網(wǎng)在過去被觀察到的次數(shù)。 概率圖的更新主要分為4種情況，分別如下所示：
[0143] 1.不更新。如果該格網(wǎng)在當(dāng)前幀概率圖Mt和歷史概率圖，其概率值Mf(g) 和M^g)都為〇,也即蚪(g) = ML(g) = 0,則不進行任何更新。
[0144] 2.概率值增加。如果在當(dāng)前幀概率圖Mt上，有點落在格網(wǎng)g中，也即Mfig) > 0并且 Mtn(g) 2 1，則在歷史概率圖M^1上，增加該格網(wǎng)的概率值，并按照下述公式進行更新：
[0145：
[0146] 其中^是概率值增長率，計算方式如下：
[0147]
[0148] 其中，W1是預(yù)設(shè)的學(xué)習(xí)率，τ是控制參數(shù)(具體實施時本領(lǐng)域技術(shù)人員可預(yù)設(shè)取值， 本實施例中Wi = O .01，τ = 3)。
[0149] 3.概率值下降。如果在當(dāng)前幀概率圖Mt上，不存在點落在格網(wǎng)g中，也即 Mf(g) = 〇,同時，在以往幀中，不存在足夠的點落于格網(wǎng)g中，也即其中nth 為預(yù)設(shè)的閾值(具體實施時本領(lǐng)域技術(shù)人員可預(yù)設(shè)取值，本實施例中設(shè)為30)，則在歷史概 率圖Mt-il，降低該格網(wǎng)的概率值，計算方式如下所示：
[0150]
[0151] 其中，Wd表示概率下降速率，其計算方式如下：
[0152]
[0153] 4.概率值增長和下降值內(nèi)插。針對剩下的未處理的格網(wǎng)，采用內(nèi)插的方法進行更 新。用％和^表示概率值增長了和下降了的格網(wǎng)，針對待內(nèi)插的格網(wǎng)g，首先根據(jù)高斯函數(shù) 內(nèi)插其增長和下降的概率值Pu和Pd，計算公式如下所示：
[0154]
[0155] 其中，d(g，gk)表不格網(wǎng)g和格網(wǎng)gk之間的距離，其中g(shù)k代表或者心中的一個格 網(wǎng)，31表示高斯函數(shù)的核大?。ň唧w實施時，本領(lǐng)域技術(shù)人員可自行設(shè)定，本實施例中設(shè)為 1.5)。用P代表該格網(wǎng)增長或下降的概率值大小，如果I pu I > I Pd I，則p = Pu，反之，P = Pd，通 過下述公式對格網(wǎng)概率值進行更新：
[0156]
[0157] 根據(jù)步驟3.1到步驟3.3,可以對單尺度的概率圖進行更新。本發(fā)明在步驟2中采用 的是一種多尺度的概率圖配準(zhǔn)方法，因此，需要按照步驟3.1和步驟3.3對多尺度的概率圖 分別進行更新，可初始化s = 1，執(zhí)行完畢后令s = s+1，再次執(zhí)行，直到s = S。
[0158] 步驟4:返回步驟1，可以對后續(xù)各幀迭代步驟2到步驟3,直至所有激光幀處理完 畢，則終止迭代，并得到激光器的運行軌跡和室內(nèi)動態(tài)環(huán)境的二維地圖。參見圖2,走廊場景 最終概率圖。走廊中間的環(huán)線為機器車運行軌跡，其余區(qū)域，灰度值越大(影像越白），概率 值越大，反之，概率值越小。
[0159] 具體實施時，本發(fā)明所提供方法可基于軟件技術(shù)實現(xiàn)自動運行流程，也可采用模 塊化方式實現(xiàn)相應(yīng)系統(tǒng)。本發(fā)明實施例提供一種基于點云的室內(nèi)動態(tài)場景SLAM系統(tǒng)，包括 以下模塊：
[0160] 第一模塊，用于首先判斷是否t = 0,表示當(dāng)前激光掃描幀是否為第一幀，是則生成 當(dāng)前幀的概率圖M〗'，并直接設(shè)置歷史概率圖Mf_i 然后等待下一幀，當(dāng)t不為0時，命 令第二模塊工作；
[0161] 所述進行概率圖生成，包括進行激光幀輪廓提取，根據(jù)激光幀輪廓，結(jié)合二維高斯 模型，針對不同分辨率的概率圖，計算出每個格網(wǎng)的概率值大??；
[0162] 第二模塊，用于基于多尺度的激光幀和概率圖配準(zhǔn)，實現(xiàn)如下，
[0163] 設(shè)待配準(zhǔn)的當(dāng)前幀為St，各層歷史概率圖分別為ML1，其中S=1，2-_S，s為概率圖 當(dāng)前層數(shù)，S為總層數(shù)，最頂層初始轉(zhuǎn)換矩陣為F tL1，采用上一幀最頂層的轉(zhuǎn)換矩陣，
[0164] 將當(dāng)前幀配準(zhǔn)至歷史概率圖時，在每一層中通過結(jié)合窮舉法和梯度下降法優(yōu)化能 量函數(shù)，尋找到兩者間的最優(yōu)轉(zhuǎn)化矩陣:rf，
[0165] 能量函數(shù)如下所示，
[0166]
[0167] 其中，X表示激光幀St中的一個點，Γ/ ? X表示X經(jīng)過g轉(zhuǎn)換后的點，代沒? XlMU 表示轉(zhuǎn)換后的點在歷史概率圖上的概率值，W(X)表示每個點的權(quán)重值，Mt 1為某一層的歷 史概率圖；
[0168] 第三模塊，用于概率圖更新，當(dāng)前幀和歷史概率圖配準(zhǔn)后，結(jié)合當(dāng)前幀的信息對每 一層歷史概率圖進行更新，從而可以將新的概率圖用于下一幀的配準(zhǔn)中；
[0169] 對每一層的概率圖進行更新，實現(xiàn)方式如下，
[0170] 設(shè)當(dāng)前層數(shù)為s，令= MU，Mi = Mf，= M&根據(jù)第二模塊的配準(zhǔn)結(jié) 果，將當(dāng)前幀St轉(zhuǎn)換至歷史概率圖Mp1所在的坐標(biāo)系，得到轉(zhuǎn)換后的激光幀發(fā)，針對客進行 概率圖生成，生成對應(yīng)的概率圖M t;
[0171]所述進行概率圖生成，包括進行激光幀輪廓提取，根據(jù)激光幀輪廓，結(jié)合二維高斯 模型，針對不同分辨率的概率圖，計算出每個格網(wǎng)的概率值大??；
[0172] 結(jié)合當(dāng)前幀概率圖Mt和歷史概率圖對歷史概率圖進行更新，得到當(dāng)前歷史 概率圖Mt:;
[0173] 第四模塊，用于命令第一模塊，直至所有激光幀處理完畢，則終止迭代，得到激光 器的運行軌跡和室內(nèi)動態(tài)環(huán)境的二維地圖。
[0174]進一步地，第二模塊的實現(xiàn)方式包括如下子模塊，
[0175] 第一子模塊，用于點云法向量直方圖統(tǒng)計，包括統(tǒng)計激光幀中所有點的法向量分 布直方圖:?%直方圖個數(shù)為B;
[0176] 第二子樽塊，用干權(quán)倌估筧,毎個點的權(quán)倌wb計算公式如下所示，
[0177]
[0178] 其中叫表示第b個直方圖的大小，其中b = l，2，~B;
[0179] 第三子模塊，用于通過s是否等于1，判斷當(dāng)前層是否為最頂層，若為最頂層，則該 層的初始轉(zhuǎn)換矩陣為上一幀傳遞過來的轉(zhuǎn)換矩陣Tt 1;若不是最頂層，則該層的初始轉(zhuǎn)換矩 陣為上一層傳遞下來的轉(zhuǎn)換矩陣療一1;
[0180] 第四子模塊，用于根據(jù)第三子模塊確定的轉(zhuǎn)化矩陣獲得LM算法初始值；
[0181] 第五子模塊，用于采用LM算法優(yōu)化能量函數(shù)MU求解得到最優(yōu)轉(zhuǎn)換矩陣 拉，并估計得到激光器的速度咬；
[0182] 第六子模塊，用于當(dāng)s不等于S時，令s = s+l，命令第五子模塊工作，當(dāng)s等于S時，結(jié) 束迭代命令第三模塊工作。
[0183] 各模塊具體實現(xiàn)可參見相應(yīng)步驟，本發(fā)明不予贅述。
[0184] 需要強調(diào)的是，本發(fā)明所述的實施例是說明性的，而不是限定性的。因此本發(fā)明包 括并不限于【具體實施方式】中所述的實施例，凡是由本領(lǐng)域技術(shù)人員根據(jù)本發(fā)明的技術(shù)方案 得出的其他實施方式，同樣屬于本發(fā)明保護的范圍。
【主權(quán)項】
1. 一種基于點云的室內(nèi)動態(tài)場景SLAM方法，其特征在于，包括以下步驟： 步驟1，首先判斷是否t = 0,表示當(dāng)前激光掃描幀是否為第一幀，是則生成當(dāng)前幀的概 率圖M0S，并直接設(shè)置歷史概率圖= M0S，然后等待下一幀，當(dāng)t不為0時，進入步驟2; 所述進行概率圖生成，包括進行激光幀輪廓提取，根據(jù)激光幀輪廓，結(jié)合二維高斯模 型，針對不同分辨率的概率圖，計算出每個格網(wǎng)的概率值大小； 步驟2，基于多尺度的激光幀和概率圖配準(zhǔn)，實現(xiàn)如下， 設(shè)待配準(zhǔn)的當(dāng)前幀為St，各層歷史概率圖分別為，其中8 = 1，2~5,8為概率圖當(dāng)前 層數(shù)，S為總層數(shù)，最頂層初始轉(zhuǎn)換矩陣為，采用上一幀最頂層的轉(zhuǎn)換矩陣， 將當(dāng)前幀配準(zhǔn)至歷史概率圖時，在每一層中通過結(jié)合窮舉法和梯度下降法優(yōu)化能量函 數(shù)，尋找到兩者間的最優(yōu)轉(zhuǎn)化矩陣7?， 能量函數(shù)如下所示，其中，X表示激光幀st中的一個點，療(g) X表示X經(jīng)過哎轉(zhuǎn)換后的點，POT (gi XIMLJ表 示轉(zhuǎn)換后的點在歷史概率圖上的概率值，w(x)表示每個點的權(quán)重值，Μ?ιΛ某一層的歷史 概率圖； 步驟3,概率圖更新，當(dāng)前幀和歷史概率圖配準(zhǔn)后，結(jié)合當(dāng)前幀的信息對每一層歷史概 率圖進行更新，從而可以將新的概率圖用于下一幀的配準(zhǔn)中； 對每一層的概率圖進行更新，實現(xiàn)方式如下， 設(shè)當(dāng)前層數(shù)為s，令= 根據(jù)步驟2的配準(zhǔn)結(jié)果，將當(dāng)前 幀St轉(zhuǎn)換至歷史概率圖所在的坐標(biāo)系，得到轉(zhuǎn)換后的激光幀說，針對發(fā)進行概率圖生 成，生成對應(yīng)的概率圖Mt; 所述進行概率圖生成，包括進行激光幀輪廓提取，根據(jù)激光幀輪廓，結(jié)合二維高斯模 型，針對不同分辨率的概率圖，計算出每個格網(wǎng)的概率值大??； 結(jié)合當(dāng)前幀概率圖Mt和歷史概率圖，對歷史概率圖進行更新，得到當(dāng)前歷史概率圖 步驟4,返回步驟1，直至所有激光幀處理完畢，則終止迭代，得到激光器的運行軌跡和 室內(nèi)動態(tài)環(huán)境的二維地圖。2. 根據(jù)權(quán)利要求1所述基于點云的室內(nèi)動態(tài)場景SLAM方法，其特征在于:步驟2的實現(xiàn) 方式包括如下步驟， 步驟2.1，點云法向量直方圖統(tǒng)計，包括統(tǒng)計激光幀中所有點的法向量分布直方間冗.直 方圖個數(shù)為Β; 步驟2.2，權(quán)值估算，每個點的權(quán)值wb計算公式如下所示，其中1^表示第b個直方圖的大小，其中b=l，2,…B; 步驟2.3,通過s是否等于1，判斷當(dāng)前層是否為最頂層，若為最頂層，則該層的初始轉(zhuǎn)換 矩陣為上一幀傳遞過來的轉(zhuǎn)換矩陣若不是最頂層，則該層的初始轉(zhuǎn)換矩陣為上一層 傳遞下來的轉(zhuǎn)換矩陣ΤΓ1;: 步驟2.4，根據(jù)步驟2.3確定的轉(zhuǎn)化矩陣獲得LM算法初始值； 步驟2.5，采用LM算法優(yōu)化能量函數(shù)f (? MU，求解得到最優(yōu)轉(zhuǎn)換矩陣7V，并估計得到 激光器的速度以_; 步驟2.6，當(dāng)s不等于S時，令s = s+1，返回步驟2.5，當(dāng)s等于S時，結(jié)束迭代進入步驟3。3.根據(jù)權(quán)利要求2所述基于點云的室內(nèi)動態(tài)場景SLAM方法，其特征在于:步驟3中，結(jié)合 當(dāng)前幀概率圖Mt和歷史概率圖，對歷史概率圖進行更新，實現(xiàn)方式如下， 針對概率圖中的每一個格網(wǎng)g，表示為蘭(g) = (g),(g),蘭#(g)}，其中廁p(g) e L〇,i]和l#(g) e ?〇,?]，分別表示概率圖中該格網(wǎng)的概率值，和過去所達到的最高的概率值， Mn(g)表示該格網(wǎng)在過去被觀察到的次數(shù);概率圖的更新分為4種情況，分別如下所示， 如果該格網(wǎng)g在當(dāng)前幀概率圖Mt和歷史概率圖Mg上，概率值Mf(g)和腿^(g)都為0， 則不進行任何更新； 如果在當(dāng)前幀概率圖Mt上，Mf(g) > 0并且M?(g) 2 1，則按照下述公式進行更新，其中，^是概率值增長率，計算方式如下， A 二 Μ (1 + τΜ丨f-"g)) 其中，W1是預(yù)設(shè)的學(xué)習(xí)率，τ是控制參數(shù)； 如果在當(dāng)前幀概率圖Mt上，Mf(g) = 〇,同時，，其中nth為預(yù)設(shè)的閾值，則 按照下述公式進行更新， M?(g) = (1 - 其中，Wd表示概率下降速率，其計算方式如下，針對剩下的未處理的格網(wǎng)，采用內(nèi)插的方式進行更新，包括用@?和《^表示概率值增長 了和下降了的格網(wǎng)，針對待內(nèi)插的格網(wǎng)g，首先根據(jù)高斯函數(shù)內(nèi)插其增長和下降的概率值Pu 和Pd，計算公式如下所示：如果|pu| > Ipd|，貝ljp = pu，反之，P = Pd，通過下述公式對格網(wǎng)概率值進行更新：其中，d(g，gk)表不格網(wǎng)g和格網(wǎng)gk之間的距離，其中g(shù)k代表心或者％中的一個格網(wǎng)，δι 表示高斯函數(shù)的核大，用Ρ代表該格網(wǎng)增長或下降的概率值大小。4. 一種基于點云的室內(nèi)動態(tài)場景SLAM系統(tǒng)，其特征在于，包括以下模塊： 第一模塊，用于首先判斷是否t = 0,表示當(dāng)前激光掃描幀是否為第一幀，是則生成當(dāng)前 幀的概率圖_，并直接設(shè)置歷史概率圖爾4 = M0S，然后等待下一幀，當(dāng)t不為0時，命令第 二模塊工作； 所述進行概率圖生成，包括進行激光幀輪廓提取，根據(jù)激光幀輪廓，結(jié)合二維高斯模 型，針對不同分辨率的概率圖，計算出每個格網(wǎng)的概率值大??； 第二模塊，用于基于多尺度的激光幀和概率圖配準(zhǔn)，實現(xiàn)如下， 設(shè)待配準(zhǔn)的當(dāng)前幀為St，各層歷史概率圖分別為Mfq，其中8 = 1，2 - 5,8為概率圖當(dāng)前 層數(shù)，S為總層數(shù)，最頂層初始轉(zhuǎn)換矩陣為7〗*^.采用上一幀最頂層的轉(zhuǎn)換矩陣， 將當(dāng)前幀配準(zhǔn)至歷史概率圖時，在每一層中通過結(jié)合窮舉法和梯度下降法優(yōu)化能量函 數(shù)，尋找到兩者間的最優(yōu)轉(zhuǎn)化矩陣療， 能量函數(shù)如下所示，其中，X表示激光幀S沖的一個點，G ? X表示X經(jīng)過砭轉(zhuǎn)換后的點，/>(紀(jì)^xlMU表 示轉(zhuǎn)換后的點在歷史概率圖上的概率值，w(x)表示每個點的權(quán)重值，為某一層的歷史 概率圖； 第三模塊，用于概率圖更新，當(dāng)前幀和歷史概率圖配準(zhǔn)后，結(jié)合當(dāng)前幀的信息對每一層 歷史概率圖進行更新，從而可以將新的概率圖用于下一幀的配準(zhǔn)中； 對每一層的概率圖進行更新，實現(xiàn)方式如下， 設(shè)當(dāng)前層數(shù)為=鋪f-i，廁t = ;根據(jù)第二模塊的配準(zhǔn)結(jié)果，將當(dāng) 前幀St轉(zhuǎn)換至歷史概率圖糜所在的坐標(biāo)系，得到轉(zhuǎn)換后的激光幀旯，針對進行概率圖 生成，生成對應(yīng)的概率圖Mt; 所述進行概率圖生成，包括進行激光幀輪廓提取，根據(jù)激光幀輪廓，結(jié)合二維高斯模 型，針對不同分辨率的概率圖，計算出每個格網(wǎng)的概率值大??； 結(jié)合當(dāng)前幀概率圖Mt和歷史概率圖Mm，對歷史概率圖進行更新，得到當(dāng)前歷史概率圖 第四模塊，用于命令第一模塊，直至所有激光幀處理完畢，則終止迭代，得到激光器的 運行軌跡和室內(nèi)動態(tài)環(huán)境的二維地圖。5. 根據(jù)權(quán)利要求4所述基于點云的室內(nèi)動態(tài)場景SLAM系統(tǒng)，其特征在于:第二模塊的實 現(xiàn)方式包括如下子模塊， 第一子模塊，用于點云法向量直方圖統(tǒng)計，包括統(tǒng)計激光幀中所有點的法向量分布直 方圖兄直方圖個數(shù)為B; 第二子模塊，用于權(quán)值估算，每個點的權(quán)值wb計算公式如下所示，其中％表示第b個直方圖的大小，其中b=l，2,…B; 第三子模塊，用于通過s是否等于1，判斷當(dāng)前層是否為最頂層，若為最頂層，則該層的 初始轉(zhuǎn)換矩陣為上一幀傳遞過來的轉(zhuǎn)換矩陣若不是最頂層，則該層的初始轉(zhuǎn)換矩陣為 上一層傳遞下來的轉(zhuǎn)換矩蘆I 了廣1; 第四子模塊，用于根據(jù)第三子模塊確定的轉(zhuǎn)化矩陣獲得LM算法初始值； 第五子模塊，用于采用LM算法優(yōu)化能量函數(shù)求解得到最優(yōu)轉(zhuǎn)換矩陣療，并 估計得到激光器的速度松； 第六子模塊，用于當(dāng)s不等于S時，令s = s+1，命令第五子模塊工作，當(dāng)s等于S時，結(jié)束迭 代命令第三模塊工作。6.根據(jù)權(quán)利要求5所述基于點云的室內(nèi)動態(tài)場景SLAM系統(tǒng)，其特征在于:第三模塊中， 結(jié)合當(dāng)前幀概率圖Mt和歷史概率圖Mtd，對歷史概率圖進行更新，實現(xiàn)方式如下， 針對概率圖中的每一個格網(wǎng)8，表示為賊8) = {1^(8),1#(5),爾"(8)；}，其中耐(8)€ [0,1]和腿氣g) e [0,1]，分別表示概率圖中該格網(wǎng)的概率值，和過去所達到的最高的概率值， Mn(g):表示該格網(wǎng)在過去被觀察到的次數(shù);概率圖的更新分為4種情況，分別如下所示， 如果該格網(wǎng)g在當(dāng)前幀概率圖Mt和歷史概率圖上，概率值,V/f (g)和M^g)都為0， 則不進行任何更新； 如果在當(dāng)前幀概率圖Mt上，Mf(g) > 0并且M?(g) 2 1，則按照下述公式進行更新，其中，^是概率值增長率，計算方式如下， % (1 + 1(g)) 其中，W1是預(yù)設(shè)的學(xué)習(xí)率，τ是控制參數(shù)； 如果在當(dāng)前幀概率圖Mt上，Mf(g) = 0,同時，MKg) S知，其中nth為預(yù)設(shè)的閾值，則 按照下述公式進行更新， Mf(g) = (1 - wd)FyD|；_1(g) 其中，Wd表示概率下降速率，其計算方式如下，針對剩下的未處理的格網(wǎng)，采用內(nèi)插的方式進行更新，包括用^表示概率值增長 了和下降了的格網(wǎng)，針對待內(nèi)插的格網(wǎng)g，首先根據(jù)高斯函數(shù)內(nèi)插其增長和下降的概率值Pu 和Pd，計算公式如下所示：如果|pu| > Ipd|，貝ljp = pu，反之，P = Pd，通過下述公式對格網(wǎng)概率值進行更新：其中，d(g，gk)表不格網(wǎng)g和格網(wǎng)gk之間的距離，其中g(shù)k代表必U,或者中的一個格網(wǎng)，δι 表示高斯函數(shù)的核大，用Ρ代表該格網(wǎng)增長或下降的概率值大小。
【文檔編號】G06T7/00GK106056643SQ201610268859
【公開日】2016年10月26日
【申請日】2016年4月27日
【發(fā)明人】姚劍, 李禮, 劉康, 謝仁平, 涂金戈
【申請人】武漢大學(xué)