[發(fā)明專利]一種基于環(huán)境直線特征的地圖匹配方法

在審

• 申請(qǐng)?zhí)枺?/td>: 201410677023.6
• 申請(qǐng)日: 2014-11-24
• 公開(kāi)（公告）號(hào): CN104501811A
• 公開(kāi)（公告）日: 2015-04-08
• 發(fā)明（設(shè)計(jì)）人: 陳如申;黎勇躍;王海瑞;馬麗艷
• 申請(qǐng)（專利權(quán)）人: 杭州申昊科技股份有限公司
• 主分類號(hào): G01C21/20
• 分類號(hào): G01C21/20
• 代理公司: 杭州之江專利事務(wù)所(普通合伙)33216
• 代理人: 朱楓

1.一種基于環(huán)境直線特征的地圖匹配方法，在至少具有運(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ī)器人從起點(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；

3）對(duì)雜亂無(wú)規(guī)律的激光掃描數(shù)據(jù)點(diǎn)進(jìn)行濾波處理，去除在有效計(jì)算距離范圍之外的無(wú)用點(diǎn)；

4）將濾波后數(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ù)處理速度；

5）采用IEP算法對(duì)每一個(gè)類別進(jìn)行直線特征識(shí)別，在每個(gè)類別中找出構(gòu)成直線特征的數(shù)據(jù)點(diǎn)集；設(shè)置合理的閾值thr_cate，將數(shù)據(jù)點(diǎn)較少的直線點(diǎn)集剔除；

6）利用最小二乘準(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)p1(x,y)，p2(x,y)和線性相關(guān)系數(shù)r；其中d，theta直線方程參數(shù)，利用線段端點(diǎn)p1，p2計(jì)算線段長(zhǎng)度和直線匹配；利用線性系數(shù)r作為判別其直線特征相似程度，優(yōu)先利用線性系數(shù)高的直線特征進(jìn)行定位計(jì)算；

7）將上述步驟中得到的直線特征從激光坐標(biāo)系下變換到全局坐標(biāo)系下，并添加到環(huán)境特征地圖中；所述環(huán)境特征地圖為記錄特征直線參數(shù)的數(shù)據(jù)庫(kù)，每條記錄為一特征直線，記錄的屬性分別為直線的特征參數(shù)d，theta，p1，p2；

8）使機(jī)器人在整個(gè)環(huán)境中運(yùn)動(dòng)重復(fù)上述步驟，以得到整個(gè)環(huán)境的特征地圖；

9）機(jī)器人從起點(diǎn)開(kāi)始運(yùn)動(dòng)，慣性導(dǎo)航輸出當(dāng)前位姿的粗略值；機(jī)器人所載激光傳感器掃描當(dāng)前環(huán)境，得到以激光傳感器為原點(diǎn)的角度、距離極坐標(biāo)點(diǎn)數(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；

10）將上述步驟中得到的直線特征變換到全局坐標(biāo)系，在已創(chuàng)建的環(huán)境特征地圖中查找相對(duì)應(yīng)的直線特征，判別地圖中是否已有直線與當(dāng)前特征直線相對(duì)應(yīng)；

11）將已創(chuàng)建的環(huán)境特征地圖中可找到對(duì)應(yīng)的特征直線記錄作為基礎(chǔ)，結(jié)合當(dāng)前的掃描數(shù)據(jù)，計(jì)算出機(jī)器人當(dāng)前位姿。