在本發(fā)明涉及一種地圖構(gòu)建方法，具體涉及一種結(jié)合了UWB與激光雷達(dá)的室內(nèi)平面地圖構(gòu)建方法。
背景技術(shù)：
基于激光雷達(dá)的室內(nèi)地圖構(gòu)建在機(jī)器人領(lǐng)域應(yīng)用越來越廣發(fā)。常見的有3d激光雷達(dá)和2d激光雷達(dá)。3d激光雷達(dá)掃描整個(gè)環(huán)境的3d模型，2d激光雷達(dá)至于機(jī)器人底部，掃面平面底圖。上述兩種方法都是依據(jù)關(guān)鍵幀拼接技術(shù)實(shí)現(xiàn)底圖構(gòu)建。這種底圖構(gòu)建方法由于對(duì)傳感器的位置估計(jì)是通過相鄰兩針的拼接得到的位移變化，在室內(nèi)大范圍掃描時(shí)，兩兩相鄰位移的積分會(huì)產(chǎn)生累積誤差，使得最終的底圖構(gòu)建效果不佳。尤其是對(duì)于走廊情況，狹長(zhǎng)的走廊隨著掃描的推進(jìn)和誤差累積會(huì)會(huì)變彎，其他大面積情況會(huì)出現(xiàn)地圖整體扭曲的情況。
技術(shù)實(shí)現(xiàn)要素：
本發(fā)明將傳統(tǒng)的基于2d激光雷達(dá)的室內(nèi)地圖構(gòu)建方法與UWB室內(nèi)定位基站相結(jié)合提升了地圖構(gòu)建的精度，降低了地圖的扭曲感。具體內(nèi)容如下：
一種基于UWB室內(nèi)定位技術(shù)與激光雷達(dá)的地圖構(gòu)建方法，包括以下步驟：
1) 安裝UWB基站
在室內(nèi)環(huán)境安裝基站，安裝在天花板上以減小人體對(duì)其遮擋，對(duì)無遮擋的情況，安裝至少三個(gè)基站即可，有遮擋的情況可增加基站數(shù)目，使得在任意位置都同時(shí)能直線與三個(gè)以上基站進(jìn)行通信；
2)基站標(biāo)定
對(duì)基站初始坐標(biāo)進(jìn)行標(biāo)定；
3)掃描數(shù)據(jù)
使用麥克納姆輪移動(dòng)平臺(tái)搭載激光雷達(dá)，遙控激光雷達(dá)進(jìn)行掃描，得到掃描數(shù)據(jù)；同時(shí)平臺(tái)搭載UWB定位標(biāo)簽，讀取定位基站的距離信息；
4)幀間拼接
對(duì)于激光雷達(dá)掃描出的數(shù)據(jù)，將相鄰的兩幀采取ICP最近點(diǎn)迭代方法得到平臺(tái)的位姿變化；
5)閉環(huán)檢測(cè)
提取所有掃描幀特征點(diǎn)并將該特征點(diǎn)存入特征字典，對(duì)每個(gè)新的幀，在特征字典中查找匹配，若匹配成功則完成閉環(huán)檢測(cè)；
6)圖優(yōu)化
對(duì)步驟4）得到的一系列幀間拼接的位姿進(jìn)行圖優(yōu)化，其中以每一幀觀測(cè)瞬間的位姿為節(jié)點(diǎn)，相鄰兩幀的位姿變化為邊構(gòu)建優(yōu)化圖，將優(yōu)化圖輸入isam優(yōu)化方法庫(kù)計(jì)算，得到優(yōu)化出的每一幀的中心點(diǎn)位姿，以及包含該中心點(diǎn)位姿的處理后的優(yōu)化圖；
7)加入定位錨點(diǎn)
在上述處理后的優(yōu)化圖中，結(jié)合每一幀激光雷達(dá)數(shù)據(jù)采集的瞬間，對(duì)于UWB基站的定位數(shù)據(jù)得到的坐標(biāo)，將該坐標(biāo)作為錨點(diǎn)，該錨點(diǎn)與該錨點(diǎn)對(duì)應(yīng)的優(yōu)化圖上的節(jié)點(diǎn)之間連接一條邊；
8)二次圖優(yōu)化
對(duì)所述處理后的優(yōu)化圖重新運(yùn)行圖優(yōu)化方法得到優(yōu)化出的每一幀的中心點(diǎn)位姿；
9)渲染
由步驟8）得到每一幀的中心點(diǎn)位姿后，使用opengl繪制出occupancy grid map，其中采樣點(diǎn)標(biāo)記為黑色，從采樣點(diǎn)到上述中心點(diǎn)位置的連線繪制為白色。
此方法可以保證，在室內(nèi)受到金屬遮擋，導(dǎo)致UWB定位不準(zhǔn)時(shí)，激光雷達(dá)的數(shù)據(jù)能讓誤差減小。當(dāng)激光雷達(dá)的數(shù)據(jù)由于運(yùn)動(dòng)物體的定位精度降低時(shí)，UWB定位又可以讓此誤差減小。
具體實(shí)施方式
一種基于UWB室內(nèi)定位技術(shù)與激光雷達(dá)的地圖構(gòu)建方法，包括以下步驟：
1) 安裝UWB基站
在室內(nèi)環(huán)境安裝基站，安裝在天花板上以減小人體對(duì)其遮擋，對(duì)無遮擋的情況，安裝至少三個(gè)基站即可，有遮擋的情況可增加基站數(shù)目，使得在任意位置都同時(shí)能直線與三個(gè)以上基站進(jìn)行通信；
2)基站標(biāo)定
對(duì)基站初始坐標(biāo)進(jìn)行標(biāo)定；
3)掃描數(shù)據(jù)
使用麥克納姆輪移動(dòng)平臺(tái)搭載激光雷達(dá)，遙控激光雷達(dá)進(jìn)行掃描，得到掃描數(shù)據(jù)；同時(shí)平臺(tái)搭載UWB定位標(biāo)簽，讀取定位基站的距離信息；
4)幀間拼接
對(duì)于激光雷達(dá)掃描出的數(shù)據(jù)，將相鄰的兩幀采取ICP最近點(diǎn)迭代方法得到平臺(tái)的位姿變化；
5)閉環(huán)檢測(cè)
提取所有掃描幀特征點(diǎn)并將該特征點(diǎn)存入特征字典，對(duì)每個(gè)新的幀，在特征字典中查找匹配，若匹配成功則完成閉環(huán)檢測(cè)；
6) 圖優(yōu)化
對(duì)步驟4）得到的一系列幀間拼接的位姿進(jìn)行圖優(yōu)化，其中以每一幀觀測(cè)瞬間的位姿為節(jié)點(diǎn)，相鄰兩幀的位姿變化為邊構(gòu)建優(yōu)化圖，將優(yōu)化圖輸入isam優(yōu)化方法庫(kù)計(jì)算，得到優(yōu)化出的每一幀的中心點(diǎn)位姿，以及包含該中心點(diǎn)位姿的處理后的優(yōu)化圖；
7) 加入定位錨點(diǎn)
在上述處理后的優(yōu)化圖中，結(jié)合每一幀激光雷達(dá)數(shù)據(jù)采集的瞬間，對(duì)于UWB基站的定位數(shù)據(jù)得到的坐標(biāo)，將該坐標(biāo)作為錨點(diǎn)，該錨點(diǎn)與該錨點(diǎn)對(duì)應(yīng)的優(yōu)化圖上的節(jié)點(diǎn)之間連接一條邊；
8二次圖優(yōu)化
對(duì)所述處理后的優(yōu)化圖重新運(yùn)行圖優(yōu)化方法得到優(yōu)化出的每一幀的中心點(diǎn)位姿；
9) 渲染
由步驟8）得到每一幀的中心點(diǎn)位姿后，使用opengl繪制出occupancy grid map，其中采樣點(diǎn)標(biāo)記為黑色，從采樣點(diǎn)到上述中心點(diǎn)位置的連線繪制為白色。