本發(fā)明涉及一種地圖構(gòu)建方法，具體涉及一種結(jié)合了慣性導(dǎo)航單元與激光雷達(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ì)于激光雷達(dá)量程不夠大的情況，這種情況位姿累積的誤差會(huì)較大。其他大面積情況會(huì)出現(xiàn)地圖整體扭曲的情況。
技術(shù)實(shí)現(xiàn)要素：
本發(fā)明將傳統(tǒng)的基于2d激光雷達(dá)的室內(nèi)地圖構(gòu)建方法與慣性導(dǎo)航相結(jié)合提升了地圖構(gòu)建的精度，降低了地圖的扭曲感。具體內(nèi)容如下：
一種基于慣性導(dǎo)航單元與激光雷達(dá)的地圖構(gòu)建方法，包括以下步驟：
1) 搭建掃描平臺(tái)
掃描平臺(tái)由具有雙輪移動(dòng)平臺(tái)的機(jī)器人構(gòu)成，平臺(tái)上搭載一激光雷達(dá)與慣性導(dǎo)航單元；
2) 掃描數(shù)據(jù)
遙控雙輪移動(dòng)平臺(tái)，用激光雷達(dá)進(jìn)行掃描，得到掃描數(shù)據(jù)，同時(shí)從平臺(tái)搭載的慣性導(dǎo)航單元讀取平臺(tái)的位姿信息；
3) 幀間拼接
對(duì)于激光雷達(dá)掃描出的數(shù)據(jù)，對(duì)相鄰的兩幀采取ICP最近點(diǎn)迭代方法得到平臺(tái)的位姿變化；
4) 閉環(huán)檢測(cè)
提取所有掃描幀的特征點(diǎn)并將特征點(diǎn)存入特征字典；對(duì)每個(gè)新的幀，在特征字典中查找匹配，若匹配成功則完成閉環(huán)檢測(cè)；
5) 圖優(yōu)化
對(duì)步驟3）得到的一系列幀間拼接的位姿進(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)化圖；
6) 加入慣性導(dǎo)航數(shù)據(jù)
在上述處理后的優(yōu)化圖中，結(jié)合每一幀激光雷達(dá)數(shù)據(jù)采集的瞬間讀取到的慣性導(dǎo)航單元的位姿變化信息，將對(duì)應(yīng)的節(jié)點(diǎn)之間加入由慣性導(dǎo)航單元得到的位姿變化，如此構(gòu)成的優(yōu)化圖的每?jī)晒?jié)點(diǎn)之間如果有邊則都有兩條。
7) 二次圖優(yōu)化
對(duì)上述處理后的優(yōu)化圖重新運(yùn)行圖優(yōu)化方法得到優(yōu)化出的每一幀的中心點(diǎn)位姿；
8) 渲染
從步驟7）得到每一幀的中心點(diǎn)位姿后，使用opengl繪制出occupancy grid map，其中采樣點(diǎn)標(biāo)記為黑色，從采樣點(diǎn)到中心點(diǎn)位置的連線繪制為白色。
由于慣性導(dǎo)航單元提供了大致的位姿變化，使得對(duì)激光雷達(dá)數(shù)據(jù)進(jìn)行ICP拼接時(shí)計(jì)算時(shí)間大大縮短。而且慣性導(dǎo)航單元可以測(cè)出機(jī)器人的側(cè)傾，據(jù)此可以識(shí)別側(cè)傾時(shí)激光雷達(dá)的無效數(shù)據(jù)，減少對(duì)定位的干擾。
具體實(shí)施方式
一種基于慣性導(dǎo)航單元與激光雷達(dá)的地圖構(gòu)建方法，包括以下步驟：
1)搭建掃描平臺(tái)
掃描平臺(tái)由具有雙輪移動(dòng)平臺(tái)的機(jī)器人構(gòu)成，平臺(tái)上搭載一激光雷達(dá)與慣性導(dǎo)航單元；
2) 掃描數(shù)據(jù)
遙控雙輪移動(dòng)平臺(tái)，用激光雷達(dá)進(jìn)行掃描，得到掃描數(shù)據(jù)，同時(shí)從平臺(tái)搭載的慣性導(dǎo)航單元讀取平臺(tái)的位姿信息；
3) 幀間拼接
對(duì)于激光雷達(dá)掃描出的數(shù)據(jù)，對(duì)相鄰的兩幀采取ICP最近點(diǎn)迭代方法得到平臺(tái)的位姿變化；
4) 閉環(huán)檢測(cè)
提取所有掃描幀的特征點(diǎn)并將特征點(diǎn)存入特征字典；對(duì)每個(gè)新的幀，在特征字典中查找匹配，若匹配成功則完成閉環(huán)檢測(cè)；
5) 圖優(yōu)化
對(duì)步驟3）得到的一系列幀間拼接的位姿進(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)化圖；
6) 加入慣性導(dǎo)航數(shù)據(jù)
在上述處理后的優(yōu)化圖中，結(jié)合每一幀激光雷達(dá)數(shù)據(jù)采集的瞬間讀取到的慣性導(dǎo)航單元的位姿變化信息，將對(duì)應(yīng)的節(jié)點(diǎn)之間加入由慣性導(dǎo)航單元得到的位姿變化，如此構(gòu)成的優(yōu)化圖的每?jī)晒?jié)點(diǎn)之間如果有邊則都有兩條。
7) 二次圖優(yōu)化
對(duì)上述處理后的優(yōu)化圖重新運(yùn)行圖優(yōu)化方法得到優(yōu)化出的每一幀的中心點(diǎn)位姿。
8) 渲染
從步驟7）得到每一幀的中心點(diǎn)位姿后，使用opengl繪制出occupancy grid map，其中采樣點(diǎn)標(biāo)記為黑色，從采樣點(diǎn)到中心點(diǎn)位置的連線繪制為白色。