一種基于環(huán)境直線特征的地圖創(chuàng)建方法
【專利摘要】本發(fā)明公開(kāi)了一種基于環(huán)境直線特征的地圖創(chuàng)建方法，在至少具有運(yùn)動(dòng)控制系統(tǒng)、慣性導(dǎo)航系統(tǒng)、激光掃描傳感器的機(jī)器人上使用；包括以下步驟：1）首先設(shè)置全局坐標(biāo)系；2）自主創(chuàng)建環(huán)境特征地圖；3）機(jī)器人從起點(diǎn)開(kāi)始不斷掃描環(huán)境信息；4）對(duì)掃描數(shù)據(jù)點(diǎn)進(jìn)行濾波處理；5）將濾波后數(shù)據(jù)點(diǎn)分割為不同的類別；6）采用IEP算法對(duì)每一個(gè)類別進(jìn)行直線特征識(shí)別；7）利用最小二乘準(zhǔn)則從直線特征的數(shù)據(jù)點(diǎn)集中擬合出直線，并計(jì)算其特征參數(shù)；8）將得到的直線特征從激光坐標(biāo)系下變換到全局坐標(biāo)系下，并添加到環(huán)境特征地圖中；9）重復(fù)上述步驟，以得到整個(gè)環(huán)境的特征地圖。本發(fā)明無(wú)需改變環(huán)境即可完成機(jī)器人自主創(chuàng)建地圖，且精度較高。
【專利說(shuō)明】一種基于環(huán)境直線特征的地圖創(chuàng)建方法

【技術(shù)領(lǐng)域】
[0001]本發(fā)明屬于移動(dòng)機(jī)器人定位【技術(shù)領(lǐng)域】，特別涉及一種基于環(huán)境直線特征的地圖創(chuàng)建方法。

【背景技術(shù)】
[0002]在移動(dòng)機(jī)器人的應(yīng)用中，導(dǎo)航定位是機(jī)器人系統(tǒng)的關(guān)鍵功能。導(dǎo)航是指移動(dòng)機(jī)器人通過(guò)傳感器獲得環(huán)境信息和自身位姿狀態(tài)，實(shí)現(xiàn)在障礙物的環(huán)境中自主運(yùn)動(dòng)到目的地。導(dǎo)航的關(guān)鍵和前提是定位，定位是指實(shí)時(shí)確定機(jī)器人自身在環(huán)境中的位置和姿態(tài)。
[0003]目前較為常見(jiàn)的機(jī)器人定位技術(shù)主要是根據(jù)先驗(yàn)的環(huán)境信息，結(jié)合當(dāng)前機(jī)器人位置信息和傳感器獲取的信息確定機(jī)器人位姿狀態(tài)。定位技術(shù)主要分為絕對(duì)定位和相對(duì)定位；絕對(duì)定位常用的有衛(wèi)星GPS定位、導(dǎo)航路標(biāo)、地圖匹配等，相對(duì)定位通過(guò)計(jì)算相對(duì)于初始位姿的位姿變化來(lái)確定當(dāng)前的位姿狀態(tài)，常見(jiàn)的有里程計(jì)、慣性導(dǎo)航系統(tǒng)。GPS定位誤差較大，且容易受電磁干擾；導(dǎo)航路標(biāo)定位需要建立導(dǎo)航路標(biāo)改變已有環(huán)境，費(fèi)時(shí)費(fèi)力；而已有的地圖匹配算法需要手動(dòng)創(chuàng)建環(huán)境地圖或者自主創(chuàng)建的地圖精度不高無(wú)法形成閉環(huán)等，無(wú)法準(zhǔn)確、省力地為機(jī)器人提供位姿信息。


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

[0004]本發(fā)明的目的是為了解決上述問(wèn)題，提供一種無(wú)需改變環(huán)境即可完成機(jī)器人自主創(chuàng)建地圖，且精度較高的基于環(huán)境直線特征的地圖創(chuàng)建方法。
[0005]為此，本發(fā)明的技術(shù)方案是:一種基于環(huán)境直線特征的地圖創(chuàng)建方法，在至少具有運(yùn)動(dòng)控制系統(tǒng)、慣性導(dǎo)航系統(tǒng)、激光掃描傳感器的機(jī)器人上使用；其特征在于:它包括以下步驟:
1)首先在新的未知環(huán)境下，設(shè)置起點(diǎn)位姿Z0，并以此為基準(zhǔn)設(shè)置全局坐標(biāo)系，即以起點(diǎn)位置為坐標(biāo)系原點(diǎn)，以起點(diǎn)運(yùn)動(dòng)朝向?yàn)镮軸正向，按右手法則確定X軸正向；
2)控制機(jī)器人在環(huán)境中運(yùn)動(dòng)以自主創(chuàng)建環(huán)境特征地圖；
3)機(jī)器人從起點(diǎn)開(kāi)始運(yùn)動(dòng)，機(jī)器人所載激光傳感器不斷掃描環(huán)境信息得到以激光傳感器為原點(diǎn)的角度、距離極坐標(biāo)點(diǎn)數(shù)據(jù)；機(jī)器人每運(yùn)動(dòng)一定距離就要處理一次激光掃描數(shù)據(jù)；
慣性導(dǎo)航系統(tǒng)不斷輸出機(jī)器人當(dāng)前的航位信息，利用該航位信息不斷計(jì)算當(dāng)前點(diǎn)與上一次掃描數(shù)據(jù)處理位置的距離delta_d，當(dāng)該距離大于等于設(shè)定的閾值delta_dthreshod時(shí)則進(jìn)行下面的激光掃描數(shù)據(jù)處理，并將delta_d清0，從當(dāng)前點(diǎn)重新開(kāi)始計(jì)算delta_d ；
4)對(duì)雜亂無(wú)規(guī)律的激光掃描數(shù)據(jù)點(diǎn)進(jìn)行濾波處理，去除在有效計(jì)算距離范圍之外的無(wú)用點(diǎn)；
5)將濾波后數(shù)據(jù)點(diǎn)根據(jù)其分布的連續(xù)特性聚類處理，分割為不同的類別；計(jì)算相鄰兩激光掃描點(diǎn)距離nd_laser，若該距離小于等于設(shè)定閾值thr_cate則判定為同一類別，同一類別代表該點(diǎn)集來(lái)自障礙物的連續(xù)表面；同時(shí)設(shè)置合適的閾值thr_cate，將數(shù)據(jù)點(diǎn)較少的類別剔除以加快數(shù)據(jù)處理速度；
6)采用IEP算法對(duì)每一個(gè)類別進(jìn)行直線特征識(shí)別，在每個(gè)類別中找出構(gòu)成直線特征的數(shù)據(jù)點(diǎn)集；設(shè)置合理的閾值thr_cate，將數(shù)據(jù)點(diǎn)較少的直線點(diǎn)集剔除；
7)利用最小二乘準(zhǔn)則從直線特征的數(shù)據(jù)點(diǎn)集中擬合出直線，并計(jì)算其特征參數(shù)；
以機(jī)器人為中心坐標(biāo)系與激光傳感器為中心的坐標(biāo)系重合，所述參數(shù)為在激光傳感器坐標(biāo)系下，直線到原點(diǎn)距離d、直線與坐標(biāo)原點(diǎn)的垂線與極坐標(biāo)軸夾角theta、直線端點(diǎn)坐標(biāo)pi (X，y)，p2(x, y)和線性相關(guān)系數(shù)r ;其中d，theta直線方程參數(shù)，利用線段端點(diǎn)pl，p2計(jì)算線段長(zhǎng)度和直線匹配；利用線性系數(shù)r作為判別其直線特征相似程度，優(yōu)先利用線性系數(shù)高的直線特征進(jìn)行定位計(jì)算；
8)將上述步驟中得到的直線特征從激光坐標(biāo)系下變換到全局坐標(biāo)系下，并添加到環(huán)境特征地圖中；所述環(huán)境特征地圖為記錄特征直線參數(shù)的數(shù)據(jù)庫(kù)，每條記錄為一特征直線，記錄的屬性分別為直線的特征參數(shù)d，theta, pi, p2 ；
9)使機(jī)器人在整個(gè)環(huán)境中運(yùn)動(dòng)重復(fù)上述步驟，以得到整個(gè)環(huán)境的特征地圖。
[0006]本發(fā)明以慣性導(dǎo)航的輸出位姿為基礎(chǔ)，利用激光掃描獲取自然環(huán)境中的直線特征，創(chuàng)建高精度的環(huán)境特征地圖；無(wú)需改變環(huán)境即可完成機(jī)器人自主創(chuàng)建地圖，且精度較尚O

【專利附圖】

【附圖說(shuō)明】
[0007]以下結(jié)合附圖和本發(fā)明的實(shí)施方式來(lái)做進(jìn)一步詳細(xì)說(shuō)明。
[0008]圖1本發(fā)明的工作流程圖。

【具體實(shí)施方式】
[0009]參見(jiàn)附圖。本實(shí)施例包括以下步驟:
1)首先在新的未知環(huán)境下，設(shè)置起點(diǎn)位姿Z0，并以此為基準(zhǔn)設(shè)置全局坐標(biāo)系，即以起點(diǎn)位置為坐標(biāo)系原點(diǎn)，以起點(diǎn)運(yùn)動(dòng)朝向?yàn)镮軸正向，按右手法則確定X軸正向；
2)控制機(jī)器人在環(huán)境中運(yùn)動(dòng)以自主創(chuàng)建環(huán)境特征地圖；
3)機(jī)器人從起點(diǎn)開(kāi)始運(yùn)動(dòng)，機(jī)器人所載激光傳感器不斷掃描環(huán)境信息得到以激光傳感器為原點(diǎn)的角度、距離極坐標(biāo)點(diǎn)數(shù)據(jù)；機(jī)器人每運(yùn)動(dòng)一定距離就要處理一次激光掃描數(shù)據(jù)；
慣性導(dǎo)航系統(tǒng)不斷輸出機(jī)器人當(dāng)前的航位信息，利用該航位信息不斷計(jì)算當(dāng)前點(diǎn)與上一次掃描數(shù)據(jù)處理位置的距離delta_d，當(dāng)該距離大于等于設(shè)定的閾值delta_dthreshod時(shí)則進(jìn)行下面的激光掃描數(shù)據(jù)處理，并將delta_d清0，從當(dāng)前點(diǎn)重新開(kāi)始計(jì)算delta_d ；
4)如圖1所示從激光掃描數(shù)據(jù)中提取直線特征過(guò)程:對(duì)雜亂無(wú)規(guī)律的激光掃描數(shù)據(jù)點(diǎn)進(jìn)行濾波處理，去除在有效計(jì)算距離范圍之外的無(wú)用點(diǎn)；
5)將濾波后數(shù)據(jù)點(diǎn)根據(jù)其分布的連續(xù)特性聚類處理，分割為不同的類別；方法為計(jì)算相鄰兩激光掃描點(diǎn)距離nd_laser，若該距離小于等于設(shè)定閾值thr_cate則判定為同一類另IJ，同一類別代表該點(diǎn)集來(lái)自障礙物的連續(xù)表面；同時(shí)設(shè)置合適的閾值thr_cate，將數(shù)據(jù)點(diǎn)較少的類別剔除以加快數(shù)據(jù)處理速度；
6)采用IEP算法對(duì)每一個(gè)類別進(jìn)行直線特征識(shí)別，在每個(gè)類別中找出構(gòu)成直線特征的數(shù)據(jù)點(diǎn)集；設(shè)置合理的閾值thr_cate，將數(shù)據(jù)點(diǎn)較少的直線點(diǎn)集剔除；
7)利用最小二乘準(zhǔn)則從直線特征的數(shù)據(jù)點(diǎn)集中擬合出直線，并計(jì)算其特征參數(shù)； 假設(shè)以機(jī)器人為中心坐標(biāo)系與激光傳感器為中心的坐標(biāo)系重合，所述參數(shù)為在激光傳感器坐標(biāo)系下，直線到原點(diǎn)距離d、直線與坐標(biāo)原點(diǎn)的垂線與極坐標(biāo)軸夾角theta、直線端點(diǎn)坐標(biāo)pi (X，y)，p2(x，y)和線性相關(guān)系數(shù)r ;其中d、theta直線方程參數(shù)，利用線段端點(diǎn)pl，p2計(jì)算線段長(zhǎng)度和直線匹配；利用線性系數(shù)!■作為判別其直線特征相似程度，優(yōu)先利用線性系數(shù)高的直線特征進(jìn)行定位計(jì)算；
8)按照上述步驟中得到的直線特征從激光坐標(biāo)系下變換到全局坐標(biāo)系下，并添加到環(huán)境特征地圖中，所述環(huán)境特征地圖為記錄特征直線參數(shù)的數(shù)據(jù)庫(kù)，每條記錄為一特征直線，記錄的屬性分別為直線的特征參數(shù)d，theta, pi, p2 ；
9)使機(jī)器人在整個(gè)環(huán)境中運(yùn)動(dòng)重復(fù)上述步驟，以得到整個(gè)環(huán)境的特征地圖。
【權(quán)利要求】
1.一種基于環(huán)境直線特征的地圖創(chuàng)建方法，在至少具有運(yùn)動(dòng)控制系統(tǒng)、慣性導(dǎo)航系統(tǒng)、激光掃描傳感器的機(jī)器人上使用；其特征在于:它包括以下步驟: 1)首先在新的未知環(huán)境下，設(shè)置起點(diǎn)位姿Z0，并以此為基準(zhǔn)設(shè)置全局坐標(biāo)系，即以起點(diǎn)位置為坐標(biāo)系原點(diǎn)，以起點(diǎn)運(yùn)動(dòng)朝向?yàn)镮軸正向，按右手法則確定X軸正向； 2)控制機(jī)器人在環(huán)境中運(yùn)動(dòng)以自主創(chuàng)建環(huán)境特征地圖； 3)機(jī)器人從起點(diǎn)開(kāi)始運(yùn)動(dòng)，機(jī)器人所載激光傳感器不斷掃描環(huán)境信息得到以激光傳感器為原點(diǎn)的角度、距離極坐標(biāo)點(diǎn)數(shù)據(jù)；機(jī)器人每運(yùn)動(dòng)一定距離就要處理一次激光掃描數(shù)據(jù)； 慣性導(dǎo)航系統(tǒng)不斷輸出機(jī)器人當(dāng)前的航位信息，利用該航位信息不斷計(jì)算當(dāng)前點(diǎn)與上一次掃描數(shù)據(jù)處理位置的距離delta_d，當(dāng)該距離大于等于設(shè)定的閾值delta_dthreshod時(shí)則進(jìn)行下面的激光掃描數(shù)據(jù)處理，并將delta_d清O，從當(dāng)前點(diǎn)重新開(kāi)始計(jì)算delta_d ； 4)對(duì)雜亂無(wú)規(guī)律的激光掃描數(shù)據(jù)點(diǎn)進(jìn)行濾波處理，去除在有效計(jì)算距離范圍之外的無(wú)用點(diǎn)； 5)將濾波后數(shù)據(jù)點(diǎn)根據(jù)其分布的連續(xù)特性聚類處理，分割為不同的類別；計(jì)算相鄰兩激光掃描點(diǎn)距離nd_laser，若該距離小于等于設(shè)定閾值thr_cate則判定為同一類別，同一類別代表該點(diǎn)集來(lái)自障礙物的連續(xù)表面；同時(shí)設(shè)置合適的閾值thr_cate，將數(shù)據(jù)點(diǎn)較少的類別剔除以加快數(shù)據(jù)處理速度； 6)采用IEP算法對(duì)每一個(gè)類別進(jìn)行直線特征識(shí)別，在每個(gè)類別中找出構(gòu)成直線特征的數(shù)據(jù)點(diǎn)集；設(shè)置合理的閾值thr_cate，將數(shù)據(jù)點(diǎn)較少的直線點(diǎn)集剔除； 7)利用最小二乘準(zhǔn)則從直線特征的數(shù)據(jù)點(diǎn)集中擬合出直線，并計(jì)算其特征參數(shù)； 以機(jī)器人為中心坐標(biāo)系與激光傳感器為中心的坐標(biāo)系重合，所述參數(shù)為在激光傳感器坐標(biāo)系下，直線到原點(diǎn)距離d、直線與坐標(biāo)原點(diǎn)的垂線與極坐標(biāo)軸夾角theta、直線端點(diǎn)坐標(biāo)pi (X，y)，p2(x, y)和線性相關(guān)系數(shù)r ;其中d，theta直線方程參數(shù)，利用線段端點(diǎn)pl，p2計(jì)算線段長(zhǎng)度和直線匹配；利用線性系數(shù)r作為判別其直線特征相似程度，優(yōu)先利用線性系數(shù)高的直線特征進(jìn)行定位計(jì)算； 8)將上述步驟中得到的直線特征從激光坐標(biāo)系下變換到全局坐標(biāo)系下，并添加到環(huán)境特征地圖中；所述環(huán)境特征地圖為記錄特征直線參數(shù)的數(shù)據(jù)庫(kù)，每條記錄為一特征直線，記錄的屬性分別為直線的特征參數(shù)d，theta, pi, p2 ； 9)使機(jī)器人在整個(gè)環(huán)境中運(yùn)動(dòng)重復(fù)上述步驟，以得到整個(gè)環(huán)境的特征地圖。
【文檔編號(hào)】G01C21/16GK104501794SQ201410677142
【公開(kāi)日】2015年4月8日 申請(qǐng)日期:2014年11月24日 優(yōu)先權(quán)日:2014年11月24日
【發(fā)明者】陳如申, 黎勇躍, 王海瑞, 馬麗艷 申請(qǐng)人:杭州申昊科技股份有限公司