本發(fā)明涉及一種面向長直廊環(huán)境的室內(nèi)地圖自動構(gòu)建系統(tǒng)。

背景技術(shù)：

室內(nèi)地圖自動構(gòu)建技術(shù)一直是機器人自主移動導(dǎo)航領(lǐng)域的研究熱點。地圖自動構(gòu)建技術(shù)通常需要借助機器人底盤編碼器、視覺里程計和激光測距儀等數(shù)據(jù)用于判斷實際的距離與大小，然而在長直廊環(huán)境場景下，由于兩側(cè)墻體相似度高，受特征點稀少、環(huán)境相似度較高等因素影響，使得現(xiàn)有技術(shù)并不能較為精確地完成長直廊環(huán)境下地圖的構(gòu)建，尤其是在長廊的長度計算上有較大誤差。

目前世界上幾種流行的二維激光建圖算法，如gmapping、hectorslam等，由于二維激光數(shù)據(jù)特征較少，在長直廊環(huán)境下容易誤將機器人直線運動判定為編碼器里程計漂移，從而導(dǎo)致算法重新定位機器人位置，導(dǎo)致最后建立的直廊地圖長度相較于真實距離短，且會出現(xiàn)重疊的情況。

因此，如何提高長直廊場景下的建圖精度，已成為目前亟需的一個問題。

技術(shù)實現(xiàn)要素：

本發(fā)明要解決的技術(shù)問題是提供一種面向長直廊環(huán)境的室內(nèi)地圖自動構(gòu)建系統(tǒng)，與傳統(tǒng)的建圖構(gòu)建系統(tǒng)相比，本發(fā)明在長直廊環(huán)境下能有效提高建圖精度，減少了因建圖算法的回環(huán)檢測誤差所造成的長直廊環(huán)境下地圖長度出現(xiàn)短缺和彎曲的問題。

為解決上述問題，本發(fā)明采用的技術(shù)方案由機器人本體(1)、視覺采集裝置(2)、自動伸縮支架(3)、激光測距儀(4)、控制器(5)組成，其特征在于，視覺采集裝置(2)通過自動伸縮支架(3)安裝在機器人本體(1)上，機器人本體（1）由驅(qū)動輪(11)、支撐輪(12)、底板(13)、第二層夾板(14)、頂層夾板(15)構(gòu)成，控制器(5)安裝在底層夾板(13)之上，激光測距儀(4)安裝于第二層夾板(14)之上，機器人的底板(13)安裝有直流電機(131)和編碼器(132)。

所述的視覺采集裝置(2)由rgbd攝像頭(22)、底層支撐板(23)、u型支撐架(24)、頂層支撐板(25)、單軸舵機(26)和雙軸舵機(27)構(gòu)成；底層支撐板(23)可通過單軸舵機(26)帶動u型支撐架(24)沿偏擺方向進行360度旋轉(zhuǎn)，u型支撐架(24)可通過雙軸舵機(27)帶動頂層支撐板(25)沿俯仰方向進行±45度旋轉(zhuǎn)，頂層支撐板(25)通過安裝孔固定在雙軸舵機(27)上，底層支撐板(23)可以通過帶齒導(dǎo)軌(31)固定在升降板(32)上。

所述的機器人本體(1)上設(shè)有自動伸縮支架(3)，支架上設(shè)有帶齒導(dǎo)軌(31)、升降板(32)及環(huán)形支撐架(342)，環(huán)形支撐架(342)通過安裝孔固定于機器人的頂層夾板(15)之上，電機支撐架(341)通過焊接的方式固定在環(huán)形支撐架(342)上，步進電機(332)通過定位螺絲(343)固定在電機支撐架(341)上，并通過齒輪(331)嚙合實現(xiàn)視覺采集裝置(2)的升降功能，帶齒導(dǎo)軌(31)豎直安裝，通過滾動軸承(311)與環(huán)形支撐架(342)連接；兩個步進電機(332)、單軸舵機(26)、雙軸舵機(27)、直流電機(131)與控制器(5)的輸出端連接，激光測距儀(4)、rgbd攝像頭(22)和編碼器(132)與控制器(5)的輸入端連接，控制器(5)能夠同時控制自動伸縮支架(3)和直流電機(131)，以及能夠?qū)崟r獲取激光測距儀(4)、rgbd攝像頭(22)和編碼器(132)的數(shù)據(jù)。

所述的控制器(5)通過激光測距儀(4)采集周圍環(huán)境的深度信息，激光測距儀旋轉(zhuǎn)360度共產(chǎn)生360個深度數(shù)據(jù)(p1，p2...pn)及與其一一匹配的360個角度數(shù)據(jù)(a1，a2...an)，控制器(5)以機器人當前朝向為y軸正方向，建立符合右手準則的笛卡爾坐標系，將當前獲得的深度數(shù)據(jù)和角度數(shù)據(jù)進行二維重建，所有的深度數(shù)據(jù)通過連接相鄰數(shù)據(jù)得到n-1組線段，計算不同線段之間的夾角，若兩線段夾角小于5度則判定這兩條線段在同一線段組內(nèi)，采用最小二乘法對采集到各信息點的位置坐標進行擬合得到線段組，并計算不同線段之間的夾角，如果兩條線段相距的最小距離大于機器人本體最大寬度且傾角相差小于5度，則判定機器人進入長直廊環(huán)境。

進入長直廊環(huán)境后，控制器(5)調(diào)節(jié)自動伸縮支架(3)并驅(qū)動單軸舵機(26)，通過視覺采集裝置(2)采集周圍環(huán)境圖像信息及深度信息，采用柵格地圖的方式構(gòu)建整體地圖，柵格最小單位為0.04m2。

機器人在長直廊環(huán)境下的建圖方法，包括以下步驟：步驟1:當機器人判定自身進入直廊環(huán)境后，將當前位置坐標點標為d1點，并將機器人所經(jīng)過的所有柵格點按照增序標號(d1,d2...dn)，用以檢測該柵格點是否訪問過，并采集里程計信息，進行長廊建圖模式初始化。

步驟2:rgbd攝像頭(22)由機器人的前進方向為起點，通過自動伸縮支架(3)控制視覺采集裝置(2)分別運動到自動伸縮支架(3)的最高點、中間點及最低點位置。

步驟3:在每一個位置高度，以機器人前進方向為正方向，通過單軸舵機(26)控制rgbd攝像頭(31)分別向左、向右轉(zhuǎn)動45度采集圖像及深度信息，共獲取六組不同高度及方位的環(huán)境信息。

步驟4:采用surf算法對采集到的數(shù)據(jù)進行特征描述子提取，并進行三角化特征點匹配，結(jié)合rgbd攝像頭(22)的旋轉(zhuǎn)角度,將獲取到的六組數(shù)據(jù)進行點云拼接，組成一個點云幀。

步驟5：根據(jù)電機編碼器(132)的反饋信息控制機器人從當前柵格地圖d1點移動到機器人前進方向的相鄰柵格地圖d2點，記錄下柵格偏移量為t1，柵格偏移量按照增序編號(t1,t2...tn),再重復(fù)步驟2-4，得到第二個點云幀。

步驟6:將步驟3、步驟4得到的兩個點云幀進行特征描述子匹配，進行粗拼接，篩去非水平匹配特征描述子，通過icp算法進行精細拼接，對篩選后的匹配子幾何計算平均距離值，計算出步驟3、步驟4兩個點云幀間機器人運動的距離，并按照增量編號(m1，m2...mn)。

步驟7:將完成建圖后生成的所有柵格偏移量(t1，t2...tn)以及計算得到的距離(m1，m2...mn)分別計算標準偏差s1、s2，并比較二者大小，選擇其中標準偏差值較小的一組數(shù)據(jù)作為建圖里程計數(shù)據(jù)，修正整體構(gòu)建地圖。

附圖說明

圖1為系統(tǒng)整體示意圖。

圖2為視覺采集裝置。

圖3為圖1中所示電機支架及帶齒導(dǎo)軌連接部分的細節(jié)圖。

圖4為圖1中所示底盤運動系統(tǒng)。

圖5為機器人在柵格地圖中運動的示意圖。

具體實施方式

下面結(jié)合附圖對本發(fā)明的一種實施例做進一步說明。

如圖1所示，本發(fā)明實施例中的一種面向長直廊環(huán)境的室內(nèi)地圖自動構(gòu)建系統(tǒng)，包括機器人本體(1)、視覺采集裝置(2)、自動伸縮支架(3)、激光測距儀(4)和控制器(5)，其中，自動伸縮支架(3)安裝在機器人本體(1)頂層夾板(15)的兩側(cè)，帶齒導(dǎo)軌(31)豎直安裝，升降板(32)通過兩端凹槽與帶齒導(dǎo)軌(31)嚙合實現(xiàn)固定；機器人可通過控制器(5)控制步進電機(332)帶動齒輪(331)，通過嚙合傳動來驅(qū)動帶齒導(dǎo)軌(31)，帶動升降板(32)垂直于水平面運動；激光測距儀(4)安裝在機器人第二層夾板(14)上，控制器(5)安裝在機器人底板(13)上，環(huán)形支撐架(342)固定在機器人頂層夾板(15)上，電機支撐架(341)通過焊接方式固定在環(huán)形支架(342)前側(cè)。

本發(fā)明實施例中，激光測距儀(4)能夠采集二維平面平面內(nèi)360度范圍內(nèi)的距離信息，掃描頻率為20hz，探測范圍為：0.15~6m；精度：±50mm；控制器(5)為高性能嵌入式控制板；視覺采集裝置(2)由rgbd攝像頭和一個二自由度云臺組成，攝像頭通過usb接口與控制器(5)連接；步進電機(332)與控制器(5)的輸入端連接。

如圖2所示，本發(fā)明實施例中視覺采集裝置(2)，包括底層支撐板(23)、u型支撐架(24)和頂層支撐板(25)，單軸舵機(26)和雙軸舵機(27)；控制器(5)與單軸舵機(26)，雙軸舵機(27)通過杜邦線連接，可通過控制算法控制單軸舵機(26)帶動u型支撐架(24)進行360度轉(zhuǎn)動，從而使rgbd攝像頭(22)達到沿偏擺方向旋轉(zhuǎn)的效果；同理，控制器(5)可控制雙軸舵機(27)俯仰旋轉(zhuǎn)，使rgbd攝像頭(22)達到沿俯仰方向旋轉(zhuǎn)的效果。

如圖3所示，本發(fā)明實施例中機器人的自動伸縮支架(3),包括帶齒導(dǎo)軌(31)、升降板(32)、滾動軸承(311)、齒輪(331)、步進電機(332)、電機支撐架(341)、環(huán)形支撐架(342)及螺絲(343)，環(huán)形支撐架(342)通過安裝孔固定于機器人的頂層夾板(15)之上，電機支撐架(341)通過焊接的方式固定在環(huán)形支撐架(342)上,步進電機(332)通過定位螺絲(343)固定在電機支撐架(341)上，并通過齒輪(331)嚙合實現(xiàn)視覺采集裝置(2)的升降功能，帶齒導(dǎo)軌(31)豎直安裝，通過滾動軸承(311)與環(huán)形支撐架(342)連接。

如圖4所示，本發(fā)明實施例中機器人的底盤運動系統(tǒng)，包括兩個驅(qū)動輪(11)、兩個支撐輪(12)、兩個直流電機(131)和兩個編碼器(132)組成；控制器(5)通過采集編碼器數(shù)據(jù)來計算機器人的運動距離:

(1)

其中：△x為機器人x軸方向前進距離，

r為機器人驅(qū)動輪(11)的半徑，

△t為單位采樣時間，

△wl,△wr分別為機器人在單位采樣時間內(nèi)運動的距離，

θ為機器人旋轉(zhuǎn)角度。

本發(fā)明實施例中，激光測距儀的消息分為角度信息和深度信息，控制器(5)可由此計算出以機器人當前位置為原點的二維坐標系下每個激光點數(shù)據(jù)相對于機器人的唯一位置，并對激光測距儀(4)的深度數(shù)據(jù)信息進行擬合，得到不同線段組，擬合具體方法為：

(2)

其中：xi，yi，xi+1，yi+1為相鄰激光數(shù)據(jù)的二維坐標，以機器人當前位置為二維坐標系原點，ωi為激光數(shù)據(jù)i＋1與激光數(shù)據(jù)i的夾角；

若相鄰的ωi相差小于5度，則認定這兩點在一條線段內(nèi)，并得到線段組，如果兩條線段相距的距離大于機器人本體最大寬度且傾角相差小于5度，則判定機器人進入長直廊環(huán)境。

方法具體包括以下步驟:

步驟1：機器人采用柵格地圖方式存儲地圖，即每個柵格點為正方形，本發(fā)明實例中柵格地圖分辨率為0.2m，即每個柵格點面積為0.04m2；機器人將自身進入直廊環(huán)境的坐標點在已有地圖上標記為d1，并按照增序標記新經(jīng)過的柵格點(d1，d2...dn)，若柵格點已訪問過則跳過；

步驟2：控制器(5)通過發(fā)送一系列指令，控制自動伸縮支架(3)以及升降板(32)使rgbd攝像頭(22)到達相對于機器人本體(1)坐標系不同的位姿，分別為自動伸縮支架(3)的最高點、中間點及最低點；

步驟3：rgbd攝像頭在不同的三個高度以機器人前進方向為中軸線，由控制器(5)控制可旋轉(zhuǎn)攝像頭支撐臺(32)分別向左向右各旋轉(zhuǎn)45度，從而采集到高度不同的6組點云數(shù)據(jù)，按照順序為不同方位的點云數(shù)據(jù)編號，為下一步拼接點云幀做準備；

步驟4：將步驟3中獲取的6組點云幀數(shù)據(jù)分別依照surf算法進行特征描述子的提取，并結(jié)合自動伸縮支架(3)升降距離和單軸舵機(26)的旋轉(zhuǎn)角度，可得到不同組點云數(shù)據(jù)之間的旋轉(zhuǎn)角度和平移距離，從而進行點云拼接，點云拼接公式為：

其中：t為點云位置轉(zhuǎn)換時所用的變換矩陣，

r3x3是3x3的旋轉(zhuǎn)正交矩陣，為計算所得的相機旋轉(zhuǎn)狀態(tài)，即單軸舵機(26)轉(zhuǎn)動角度，

t3x1為3x1的位移矩陣，為計算出相機平移距離，即為自動伸縮支架(3)的升降距離，

o1x3為1x3的變換矩陣，記錄不同組圖像間圖像縮放比例；

步驟5：如圖4所示，控制器(5)計算出當前距離機器人最近的柵格，并標記為d2，將電機編碼器(132)反饋的解算值作為柵格偏移量的數(shù)據(jù)t1，柵格偏移量按照增序編號(t1，t2...tn)，t1為柵格d1中心到柵格d2中心距離的差值；

步驟6：將相鄰兩個位置的點云幀數(shù)據(jù)分別依據(jù)surf算法提取特征點，生成特征點數(shù)據(jù)集c1、c2，由于機器人水平移動，故將c1、c2數(shù)據(jù)集中匹配特征點縱向偏移量超過3cm的匹配數(shù)據(jù)刪除，將過濾后的數(shù)據(jù)集通過icp算法進行精細拼接，得到精細濾波后的數(shù)據(jù)集c1、c2，計算c1、c2數(shù)據(jù)集中匹配子的偏移量，生成集合v，計算集合v的平均值，得到相鄰兩個柵格位置d1、d2的距離m1，并按照增序編號(m1，m2...mn)；

步驟7：重復(fù)步驟1到步驟6，進行自動地圖構(gòu)建；建圖完成后，共得到所有柵格偏移量(t1，t2...tn)和運動距離(m1，m2...mn)，分別計算標準偏差s1、s2，其公式為:

其中：s為標準偏差值，

n為采集數(shù)據(jù)總量，

xi代表柵格偏移量集合ti和運動距離集合mi，

μ為xi集合的均值；

s1與s2中數(shù)值越小表示對應(yīng)數(shù)據(jù)集越穩(wěn)定，采取標準偏差值小的一組數(shù)據(jù)，作為機器人標準運動數(shù)據(jù)輸入，生成最后的地圖。