[發(fā)明專利]變電站巡檢機(jī)器人定位導(dǎo)航系統(tǒng)及方法

有效

• 申請(qǐng)?zhí)枺?/td>: 201510290432.5
• 申請(qǐng)日: 2015-05-29
• 公開（公告）號(hào): CN104914865B
• 公開（公告）日: 2017-10-24
• 發(fā)明（設(shè)計(jì)）人: 肖鵬;王海鵬;李麗;欒貽青;慕世友;任杰;傅孟潮;孟瑜;李華東;李建祥;趙金龍;郝永鑫
• 申請(qǐng)（專利權(quán)）人: 國(guó)網(wǎng)山東省電力公司電力科學(xué)研究院;山東魯能智能技術(shù)有限公司;國(guó)家電網(wǎng)公司
• 主分類號(hào): G05D1/02
• 分類號(hào): G05D1/02;G05D1/08;G06F19/26;G01C21/20
• 代理公司: 濟(jì)南圣達(dá)知識(shí)產(chǎn)權(quán)代理有限公司37221
• 代理人: 張勇

1.變電站巡檢機(jī)器人定位導(dǎo)航系統(tǒng)的方法，其特征是，包括以下步驟：

步驟一：利用激光測(cè)距模塊采集的機(jī)器人周邊障礙物相對(duì)于激光傳感器掃描中心的距離信息，利用航位推算模塊得到的當(dāng)前機(jī)器人在全局坐標(biāo)系下的位置姿態(tài)估計(jì)，將激光測(cè)距模塊及航位推算模塊的結(jié)果分別輸出至地圖創(chuàng)建模塊與位姿解算模塊；

步驟二：地圖創(chuàng)建模塊根據(jù)得到的信息創(chuàng)建表示機(jī)器人運(yùn)行環(huán)境的激光路標(biāo)地圖與激光柵格地圖并輸入地圖存儲(chǔ)模塊進(jìn)行存儲(chǔ)；

步驟三：位姿解算模塊利用地圖存儲(chǔ)模塊中的地圖數(shù)據(jù)以及航位推算模塊的輸出數(shù)據(jù)實(shí)時(shí)解算機(jī)器人當(dāng)前位置和姿態(tài)并將所得到的位姿數(shù)據(jù)接入路徑規(guī)劃模塊與運(yùn)動(dòng)控制模塊；

步驟四：路徑規(guī)劃模塊生成機(jī)器人激光路標(biāo)拓?fù)涞貓D與全局路徑規(guī)劃用拓?fù)涞貓D，依據(jù)巡檢任務(wù)規(guī)劃?rùn)C(jī)器人巡檢運(yùn)行路線，路徑規(guī)劃得到的路徑數(shù)據(jù)輸入運(yùn)動(dòng)控制模塊執(zhí)行；

所述步驟二中，地圖創(chuàng)建模塊創(chuàng)建表示機(jī)器人運(yùn)行環(huán)境的激光路標(biāo)地圖與激光柵格地圖，具體包括以下步驟：

步驟：2-1)：地圖創(chuàng)建初始化；

步驟:2-2)：獲取k至k+1時(shí)刻過(guò)程中機(jī)器人位置姿態(tài)變化量，經(jīng)航位推算模塊計(jì)算得到機(jī)器人全局坐標(biāo)系下的位置姿態(tài)估計(jì)值；

步驟：2-3)：在k+1時(shí)刻，獲取激光測(cè)距模塊數(shù)據(jù)，與地圖存儲(chǔ)模塊中距機(jī)器人當(dāng)前位置最近的已有激光路標(biāo)包含的激光測(cè)距數(shù)據(jù)匹配，得到機(jī)器人相對(duì)于該路標(biāo)的距離和夾角的觀測(cè)向量；

步驟：2-4)：利用擴(kuò)展卡爾曼濾波方法將航位推算輸出機(jī)器人位置姿態(tài)估計(jì)值與匹配得到的路標(biāo)觀測(cè)值融合，從而得到機(jī)器人當(dāng)前位置姿態(tài)的精確估計(jì)；

步驟：2-5)：計(jì)算當(dāng)前機(jī)器人位置與路標(biāo)位置的距離，并與設(shè)定的相鄰路標(biāo)間隔距離閾值比較，再執(zhí)行下子步驟：

2-5-1)如果小于等于預(yù)先設(shè)定的閾值，機(jī)器人返回步驟2-2)執(zhí)行；

2-5-2)如果大于設(shè)定閾值，則讀取激光測(cè)距數(shù)據(jù)和機(jī)器人當(dāng)前位置姿態(tài)，形成新的激光路標(biāo)并進(jìn)行存儲(chǔ)，之后再返回步驟2-2)執(zhí)行；

步驟：2-6)：重復(fù)執(zhí)行步驟2-2)至2-5)，直至建立完整的機(jī)器人運(yùn)行環(huán)境電子地圖；

步驟：2-7)：對(duì)上述步驟生成地圖中的激光路標(biāo)數(shù)據(jù)進(jìn)行進(jìn)一步數(shù)據(jù)處理，路標(biāo)點(diǎn)相關(guān)激光掃描數(shù)據(jù)可柵格化后形成柵格地圖。

2.如權(quán)利要求1所述的變電站巡檢機(jī)器人定位導(dǎo)航系統(tǒng)的方法，其特征是，所述步驟：2-1)：地圖創(chuàng)建初始化，具體包括：

2-1-1)建立全局坐標(biāo)系，設(shè)定地圖創(chuàng)建起始點(diǎn)處機(jī)器人全局坐標(biāo)系下的初始位置和姿態(tài)，設(shè)定地圖中相鄰路標(biāo)間隔距離閾值；

2-1-2)讀取起始點(diǎn)處激光測(cè)距模塊測(cè)距數(shù)據(jù)；

2-1-3)將激光測(cè)距數(shù)據(jù)與機(jī)器人初始位置姿態(tài)數(shù)據(jù)組合后形成地圖創(chuàng)建初始點(diǎn)激光路標(biāo)，并輸入地圖存儲(chǔ)模塊進(jìn)行存儲(chǔ)。

3.如權(quán)利要求1所述的變電站巡檢機(jī)器人定位導(dǎo)航系統(tǒng)的方法，其特征是，所述步驟三，具體包括以下步驟：

步驟3-1)：定位解算初始化；

步驟3-2)：k時(shí)刻查詢地圖存儲(chǔ)模塊數(shù)據(jù)庫(kù)存儲(chǔ)的距離機(jī)器人當(dāng)前位置最近的路標(biāo)，并載入該路標(biāo)的激光測(cè)距數(shù)據(jù)以及路標(biāo)相關(guān)區(qū)域的柵格地圖；

步驟3-3)：讀取k至k+1時(shí)刻過(guò)程中機(jī)器人位置姿態(tài)變化量，經(jīng)航位推算模塊計(jì)算得到機(jī)器人全局坐標(biāo)系下的位置姿態(tài)估計(jì)值；

步驟3-4)：基于柵格地圖數(shù)據(jù)，結(jié)合航位推算數(shù)據(jù)和激光測(cè)距數(shù)據(jù)，利用粒子濾波算法估計(jì)機(jī)器人在全局坐標(biāo)系下的位置和姿態(tài)，實(shí)現(xiàn)基于柵格地圖的粗定位，定位精度由設(shè)定柵格尺寸決定；

步驟3-5)：如果機(jī)器人需要在該區(qū)域內(nèi)設(shè)定?？奎c(diǎn)進(jìn)行精確?？?，則機(jī)器人在進(jìn)入距離停靠點(diǎn)設(shè)定閾值范圍之后執(zhí)行以下子步驟，否則執(zhí)行步驟3-6)；

步驟3-6)：計(jì)算機(jī)器人與當(dāng)前路標(biāo)之間的距離，并與地圖分段加載閾值比較；

步驟3-7)：重復(fù)執(zhí)行步驟3-2)至3-6)直至整個(gè)定位任務(wù)完成；

所述步驟3-5)：機(jī)器人在進(jìn)入距離?？奎c(diǎn)設(shè)定閾值范圍之后執(zhí)行以下子步驟，具體為：

3-5-1)讀取激光測(cè)距模塊數(shù)據(jù)，與當(dāng)前所用激光路標(biāo)所包含的激光測(cè)距數(shù)據(jù)匹配，得到機(jī)器人相對(duì)于該路標(biāo)的位置姿態(tài)變化量的觀測(cè)值；

3-5-2)利用擴(kuò)展卡爾曼濾波方法對(duì)航位推算模塊輸出的機(jī)器人位置姿態(tài)估計(jì)值與激光測(cè)距模塊得到的機(jī)器人位置姿態(tài)觀測(cè)值進(jìn)行融合，得到機(jī)器人當(dāng)前位置姿態(tài)的精確估計(jì)，從而保證機(jī)器人?？奎c(diǎn)的停靠精度。