本發(fā)明涉及一種巡檢機(jī)器人定位方法及自動(dòng)充電方法。

背景技術(shù)：

市場(chǎng)上現(xiàn)有的自動(dòng)充電方式主要有兩種：(1)掃地機(jī)器人的自動(dòng)充電是通過(guò)充電基座不斷發(fā)出信號(hào)，然后機(jī)器人頂部的接收器接收到信號(hào)，最終找到“回家”的路，但是在某些情況下會(huì)出現(xiàn)無(wú)法導(dǎo)航，無(wú)法回到基座而擱淺在“半路上”的情況發(fā)生。(2)自動(dòng)接觸式充電技術(shù)，連接觸點(diǎn)位于機(jī)器人本體上方，當(dāng)機(jī)器人到達(dá)充電座時(shí)，與地面連接觸點(diǎn)自動(dòng)完成對(duì)接，充電完畢后自動(dòng)脫離。這種方法脫離定位，對(duì)對(duì)接誤差要求較高。

技術(shù)實(shí)現(xiàn)要素：

為解決現(xiàn)有技術(shù)存在的問(wèn)題，本發(fā)明提供一種巡檢機(jī)器人定位方法，實(shí)現(xiàn)巡檢機(jī)器人在充電房中的精確定位，提高與充電樁對(duì)接的成功率，方便巡檢機(jī)器人在充電房?jī)?nèi)充電。

本發(fā)明提供的巡檢機(jī)器人定位方法，包括以下步驟：

(1)利用激光傳感器采集巡檢機(jī)器人在充電房中的正前方和兩側(cè)墻壁的激光數(shù)據(jù)；

(2)對(duì)激光數(shù)據(jù)進(jìn)行二次點(diǎn)集分割及二次直線擬合；其中，第一次點(diǎn)集分割得到對(duì)應(yīng)充電房左邊墻面的左激光數(shù)據(jù)集、對(duì)應(yīng)充電房門和充電房門左、右角落的中間激光數(shù)據(jù)集、對(duì)應(yīng)充電房右邊墻面的右激光數(shù)據(jù)集；第一次直線擬合采用最小二乘法對(duì)左激光數(shù)據(jù)集和右激光數(shù)據(jù)集進(jìn)行擬合，得到充電房左邊墻面對(duì)應(yīng)直線L₃和右邊墻面對(duì)應(yīng)直線L₂；第二次點(diǎn)集分割從中間激光數(shù)據(jù)集中剔除左、右角落對(duì)應(yīng)的激光數(shù)據(jù)，得到充電房門直線L₁對(duì)應(yīng)激光數(shù)據(jù)集；第二次直線擬合通過(guò)最小二乘法對(duì)充電房門直線L₁對(duì)應(yīng)的激光數(shù)據(jù)集進(jìn)行擬合，得到充電房門對(duì)應(yīng)直線L₁；

(3)將直線L₁,L₂,L₃按照充電房特征進(jìn)行匹配，即判斷直線L₁是否同時(shí)垂直于直線L₂和直線L₃，如果同時(shí)垂直則匹配成功，否則返回步驟(1)；

(4)獲取充電房邊緣特征，即，將步驟(3)中匹配成功的直線L₁,L₂,L₃作為充電房的邊緣特征；

(5)通過(guò)步驟(4)得到的充電房邊緣特征L₁,L₂,L₃，通過(guò)直線相交獲取充電房的兩個(gè)關(guān)鍵點(diǎn)，即左邊墻面對(duì)應(yīng)直線L₃與充電房左角落、右邊墻面對(duì)應(yīng)直線L₂與充電房右角落形成的拐點(diǎn)；

(6)利用步驟(5)獲得兩個(gè)關(guān)鍵點(diǎn)與機(jī)器人之間的相對(duì)位置，確定機(jī)器人位置。

第一次點(diǎn)集分割具體包括：選取連續(xù)的2m+1個(gè)激光數(shù)據(jù)，將前m個(gè)激光數(shù)據(jù)坐標(biāo)取平均值得到前參考點(diǎn)A的坐標(biāo)，第m+1個(gè)激光點(diǎn)坐標(biāo)作為當(dāng)前點(diǎn)B的坐標(biāo)，后m個(gè)激光數(shù)據(jù)坐標(biāo)取平均值得到后參考點(diǎn)C的坐標(biāo)，然后計(jì)算后參考點(diǎn)C、前參考點(diǎn)A和當(dāng)前點(diǎn)B三個(gè)點(diǎn)形成的角度∠CAB，∠CAB大小記為θ，如果則判定當(dāng)前點(diǎn)A已經(jīng)接近充電房右角落，將當(dāng)前點(diǎn)作為右分割點(diǎn)P_r，其中，m為自然數(shù)，為設(shè)定閾值；以同樣方法得到左分割點(diǎn)P_l；利用左、右分割點(diǎn)對(duì)連續(xù)的激光數(shù)據(jù)進(jìn)行第一次分割，得到對(duì)應(yīng)充電房左邊墻面的左激光數(shù)據(jù)集、對(duì)應(yīng)充電房門和充電房門左、右角落的中間激光數(shù)據(jù)集、對(duì)應(yīng)充電房右邊墻面的右激光數(shù)據(jù)集。m的取值范圍為[30,80]，的取值范圍為

第二次點(diǎn)集分割具體包括：從右分割點(diǎn)P_r開(kāi)始遍歷中間激光數(shù)據(jù)集，當(dāng)中間激光數(shù)據(jù)集中的點(diǎn)到右邊墻面對(duì)應(yīng)直線L₂的距離大于或等于設(shè)定閾值threshold時(shí)，則認(rèn)為當(dāng)前激光數(shù)據(jù)點(diǎn)位于充電房門對(duì)應(yīng)直線L₁，并將當(dāng)前激光數(shù)據(jù)點(diǎn)作為充電房門直線L₁對(duì)應(yīng)激光數(shù)據(jù)集的右端點(diǎn)P_re；以同樣的方法，得到充電房門直線L₁對(duì)應(yīng)激光數(shù)據(jù)集的左端點(diǎn)P_le，利用左、右端點(diǎn)對(duì)中間激光數(shù)據(jù)集進(jìn)行第二次分割，得到充電房門直線L₁對(duì)應(yīng)激光數(shù)據(jù)集。

在獲取右端點(diǎn)P_re、左端點(diǎn)P_le時(shí)，設(shè)定閾值threshold分別略大于充電房右角落的寬度、充電房左角落的寬度。

為降低計(jì)算復(fù)雜度，同時(shí)提高算法的精確性，在步驟(2)之前，先對(duì)激光數(shù)據(jù)進(jìn)行去噪聲處理，去除噪聲數(shù)據(jù)。

本發(fā)明還提供一種巡檢機(jī)器人自動(dòng)充電方法，巡檢機(jī)器人通過(guò)定位導(dǎo)航自動(dòng)駛?cè)氤潆姺?；利用本發(fā)明提供的巡檢機(jī)器人定位方法對(duì)巡檢機(jī)器人進(jìn)行定位；根據(jù)定位調(diào)整巡檢機(jī)器人位置和朝向，直到巡檢機(jī)器人定位在充電樁正前方處；巡檢機(jī)器人充電部件與充電樁接觸進(jìn)行充電。

如果充電過(guò)程出現(xiàn)充電狀態(tài)中斷，判斷巡檢機(jī)器人位姿是否發(fā)生變化，若是，退出充電樁并調(diào)整位姿后重新與充電樁接觸充電，否則直接進(jìn)行充電。

本發(fā)明為了實(shí)現(xiàn)巡檢機(jī)器人安全準(zhǔn)確、快速高效地自動(dòng)充電，通過(guò)激光設(shè)備和充電房的幾何特性進(jìn)行機(jī)器人局部定位。為了實(shí)現(xiàn)定位的精確性，使用二次點(diǎn)集分割算法對(duì)連續(xù)激光數(shù)據(jù)進(jìn)行分割，通過(guò)最小二乘法對(duì)分割數(shù)據(jù)集進(jìn)行擬合，利用擬合得到的邊緣特征對(duì)充電房模型進(jìn)行重構(gòu)，最后通過(guò)重構(gòu)得到的充電房模型關(guān)鍵點(diǎn)對(duì)機(jī)器人進(jìn)行定位。

本發(fā)明通過(guò)激光設(shè)備進(jìn)行局部定位，精度高，可以達(dá)到±0.5cm的誤差范圍內(nèi)。利用高精度的定位，進(jìn)行充電樁對(duì)接成功率大大提高，經(jīng)測(cè)試，成功率達(dá)到99.8％。通過(guò)本發(fā)明可實(shí)現(xiàn)巡檢機(jī)器人在變電站長(zhǎng)期值守、完全自治，在無(wú)人干預(yù)情況下，安全準(zhǔn)確、快速高效地自動(dòng)充電，能實(shí)現(xiàn)與充電樁的成功對(duì)接，對(duì)外界干擾具有較好的魯棒性。

附圖說(shuō)明

圖1為巡檢機(jī)器人充電房局部定位算法流程圖；

圖2為二次點(diǎn)集分割算法原理圖；

圖3為第一次點(diǎn)集分割程序流程圖；

圖4為第二次點(diǎn)集分割程序流程圖；

圖5為機(jī)器人定位原理圖。

圖中：1-充電房，2-機(jī)器人，3-激光，4-充電房右角落，5-充電房左角落，6-充電房門對(duì)應(yīng)直線L₁，7-前參考點(diǎn)A，8-當(dāng)前點(diǎn)B，9-后參考點(diǎn)C，10-∠CAB，大小為θ，11-右分割點(diǎn)P_r(表示第r個(gè)激光數(shù)據(jù)點(diǎn))，12-充電房門直線L₁對(duì)應(yīng)激光數(shù)據(jù)集的右端點(diǎn)P_re，13-右邊墻面對(duì)應(yīng)直線L₂，14-左邊墻面對(duì)應(yīng)直線L₃，15-前參考點(diǎn)D，16-當(dāng)前點(diǎn)E，17-后參考點(diǎn)F，18-∠FDE，大小為α，19-左分割點(diǎn)P_l(表示第1個(gè)激光數(shù)據(jù)點(diǎn))，20-充電房門直線L₁對(duì)應(yīng)激光數(shù)據(jù)集的左端點(diǎn)P_le。

具體實(shí)施方式

如圖1所示，本發(fā)明提供的巡檢機(jī)器人定位方法主要步驟有：

1、噪聲剔除：對(duì)激光傳感器采集巡檢機(jī)器人2在充電房1中的正前方和兩側(cè)墻壁的激光數(shù)據(jù)進(jìn)行去噪處理，去除噪聲數(shù)據(jù)，降低計(jì)算復(fù)雜度的同時(shí)，提高精確性；

2、二次點(diǎn)集分割：將激光采集數(shù)據(jù)按連續(xù)性分割成多個(gè)連續(xù)區(qū)域。二次點(diǎn)集分割算法的原理如圖2所示。

①第一次點(diǎn)集分割：通過(guò)平滑擬合，將累計(jì)誤差平攤，提高分割的準(zhǔn)確性。

第一次點(diǎn)集分割的程序流程圖如圖3所示，N表示激光數(shù)據(jù)的數(shù)目，選取連續(xù)的2m+1個(gè)激光數(shù)據(jù)(對(duì)應(yīng)第i個(gè)到第2m+i個(gè)激光數(shù)據(jù)，m取值范圍為[30,80]，m值過(guò)小會(huì)導(dǎo)致遍歷次數(shù)增加，影響算法實(shí)時(shí)性，m值過(guò)大，會(huì)影響分割準(zhǔn)確性)，將前m個(gè)激光數(shù)據(jù)坐標(biāo)取平均值得到前參考點(diǎn)A7的坐標(biāo)，第m+1個(gè)激光點(diǎn)坐標(biāo)作為當(dāng)前點(diǎn)B 8的坐標(biāo)，后m個(gè)激光數(shù)據(jù)坐標(biāo)取平均值得到后參考點(diǎn)C 9的坐標(biāo)，然后計(jì)算后參考點(diǎn)C、前參考點(diǎn)A和當(dāng)前點(diǎn)B三個(gè)點(diǎn)形成的角度∠CAB 10(記為θ)，如果(的取值范圍為適當(dāng)?shù)娜≈悼梢员ＷC分割的準(zhǔn)確性)，則判定當(dāng)前點(diǎn)B已經(jīng)接近充電房右角落4，將當(dāng)前點(diǎn)B作為右分割點(diǎn)P_r 11。

同樣的方法，可以得到左分割點(diǎn)P_l，

選取連續(xù)的2m+1個(gè)激光數(shù)據(jù)(對(duì)應(yīng)第i個(gè)到第2m+i個(gè)激光數(shù)據(jù)，m取值范圍為[30,80])，將前m個(gè)激光數(shù)據(jù)坐標(biāo)取平均值得到前參考點(diǎn)D 15的坐標(biāo)，第m+1個(gè)激光點(diǎn)坐標(biāo)作為當(dāng)前點(diǎn)E 16的坐標(biāo)，后m個(gè)激光數(shù)據(jù)坐標(biāo)取平均值得到后參考點(diǎn)F 17的坐標(biāo)，然后計(jì)算后參考點(diǎn)F、前參考點(diǎn)D和當(dāng)前點(diǎn)E三個(gè)點(diǎn)形成的角度∠FDE 18(記為α)，如果(的取值范圍為)，則判定當(dāng)前點(diǎn)E已經(jīng)接近充電房左角落5，將當(dāng)前點(diǎn)E作為左分割點(diǎn)P_l 19。

利用左右分割點(diǎn)對(duì)連續(xù)的激光數(shù)據(jù)進(jìn)行第一次分割，即可得到三個(gè)激光數(shù)據(jù)：左激光數(shù)據(jù)集(對(duì)應(yīng)充電房左邊墻面)、中間激光數(shù)據(jù)集(對(duì)應(yīng)充電房門和兩個(gè)角落)和右激光數(shù)據(jù)集(對(duì)應(yīng)充電房右邊墻面)。

②第一次直線擬合：采用最小二乘法對(duì)左激光數(shù)據(jù)集和右激光數(shù)據(jù)集進(jìn)行擬合，得到充電房左邊墻面對(duì)應(yīng)直線L₃(14)和右邊墻面對(duì)應(yīng)直線L₂(13)，對(duì)應(yīng)直線表達(dá)式分別為：

L₂:a₂x'+b₂y'+c₂＝0

L₃:a₃x'+b₃y'+c₃＝0

其中，a₂、b₂、c₂為對(duì)應(yīng)L₂的參數(shù)，通過(guò)對(duì)右激光數(shù)據(jù)集進(jìn)行最小二乘直線擬合得到，a₃、b₃、c₃為對(duì)應(yīng)L₃的參數(shù)，通過(guò)對(duì)左激光數(shù)據(jù)集進(jìn)行擬合得到；L₂、L₃對(duì)應(yīng)的直線表達(dá)式位于機(jī)器人坐標(biāo)系X’O’Y’中。

③第二次點(diǎn)集分割：利用充電房幾何特征，從中間激光數(shù)據(jù)集中剔除兩個(gè)角落對(duì)應(yīng)的激光數(shù)據(jù)。

第二次點(diǎn)集分割的程序流程圖如圖4所示，從右分割點(diǎn)P_r 11開(kāi)始遍歷中間激光數(shù)據(jù)集，利用中間激光數(shù)據(jù)集中的點(diǎn)到右邊墻面對(duì)應(yīng)直線L₂的距離

當(dāng)d≥threshold(threshold為設(shè)置的距離閾值，大小略大于充電房右角落4的寬度，該寬度即充電房右角落在充電房門直線方向上的長(zhǎng)度)時(shí)，認(rèn)為當(dāng)前激光數(shù)據(jù)點(diǎn)位于充電房門對(duì)應(yīng)直線L₁，并將當(dāng)前激光數(shù)據(jù)點(diǎn)作為充電房門直線L₁對(duì)應(yīng)激光數(shù)據(jù)集的右端點(diǎn)P_re12；同樣的方法，可以得到充電房門直線L₁對(duì)應(yīng)激光數(shù)據(jù)集的左端點(diǎn)P_le 20，即，從左分割點(diǎn)P_l 19開(kāi)始遍歷中間激光數(shù)據(jù)集，利用中間激光數(shù)據(jù)集中的點(diǎn)到左邊墻面對(duì)應(yīng)直線L₃的距離

當(dāng)d≥threshold(threshold為設(shè)置的距離閾值，大小略大于充電房左角落5的寬度，該寬度即充電房左角落在充電房門直線方向上的長(zhǎng)度)時(shí)，認(rèn)為當(dāng)前激光數(shù)據(jù)點(diǎn)位于充電房門對(duì)應(yīng)直線L₁，并將當(dāng)前激光數(shù)據(jù)點(diǎn)作為充電房門直線L₁對(duì)應(yīng)激光數(shù)據(jù)集的左端點(diǎn)P_le20。

利用左、右端點(diǎn)對(duì)中間激光數(shù)據(jù)集進(jìn)行第二次分割，即可得到充電房門直線對(duì)應(yīng)的激光數(shù)據(jù)集。

④第二次直線擬合：通過(guò)最小二乘法對(duì)充電房門直線對(duì)應(yīng)的激光數(shù)據(jù)集進(jìn)行擬合，得到充電房門對(duì)應(yīng)直線L₁ 6，對(duì)應(yīng)直線表達(dá)式為：

L₁:a₁x'+b₁y'+c₁＝0，

其中，，a₁、b₁、c₁為對(duì)應(yīng)L₁的參數(shù)，通過(guò)對(duì)充電房門直線對(duì)應(yīng)的激光數(shù)據(jù)集進(jìn)行擬合得到；L₁對(duì)應(yīng)的直線表達(dá)式位于機(jī)器人坐標(biāo)系X’O’Y’中。

這樣，通過(guò)二次點(diǎn)集分割，實(shí)現(xiàn)對(duì)激光數(shù)據(jù)的分割。

3、線特征匹配：將分割后的直線L₁,L₂,L₃按照充電房特征進(jìn)行匹配，即判斷直線L₁是否同時(shí)垂直于直線L₂和直線L₃，驗(yàn)證提取直線特征的準(zhǔn)確性，如果同時(shí)垂直則匹配成功，如果匹配不成功，則重新獲取激光數(shù)據(jù)，重新執(zhí)行步驟1、2。

4、邊緣特征獲取：獲取充電房邊緣特征，即將步驟3中匹配成功的直線L₁,L₂,L₃作為充電房的邊緣特征。

5、關(guān)鍵點(diǎn)獲?。豪弥本€L₂、L₃以及同時(shí)垂直于直線L₂和直線L₃的直線L₁重構(gòu)充電房模型，即通過(guò)步驟4得到的充電房邊緣特征L₁,L₂,L₃，通過(guò)直線相交獲取重構(gòu)充電房模型的兩個(gè)關(guān)鍵點(diǎn)(拐點(diǎn)，即，左邊墻面對(duì)應(yīng)直線L₃與充電房左角落的交點(diǎn)、右邊墻面對(duì)應(yīng)直線L₂與充電房右角落的交點(diǎn))。

6、機(jī)器人定位：利用步驟5重構(gòu)充電房模型的兩個(gè)關(guān)鍵點(diǎn)與機(jī)器人之間相對(duì)位置，確定機(jī)器人位置。

機(jī)器人定位原理圖如圖5所示，坐標(biāo)系XOY為充電房模型對(duì)應(yīng)的全局坐標(biāo)系，坐標(biāo)系X’O’Y’為機(jī)器人坐標(biāo)系，其中O’對(duì)應(yīng)機(jī)器人位置，M、N對(duì)應(yīng)步驟5獲得的關(guān)鍵點(diǎn)，坐標(biāo)系XOY中M、N的坐標(biāo)分別為其中w，h對(duì)應(yīng)重構(gòu)充電房模型的寬和高，坐標(biāo)系X’O’Y’中M、N的坐標(biāo)分別為M(x_m,y_m),N(x_n,y_n)，過(guò)O’做直線MN的垂線，垂足為P，過(guò)M做Y’軸的垂線，垂足為Q，MN與Y’軸交點(diǎn)為R，確定機(jī)器人位置即求|MP|、|O′P|。

由M點(diǎn)坐標(biāo)為(x_m,y_m)有，|MQ|＝-x_m,|O′Q|＝y(tǒng)_m，由直線L₁的斜率角可得∠QMN＝θ₁，|MR|＝-x_m/cosθ₁，|QR|＝-x_m tanθ₁

所以|MP|＝|MR|+|PR|＝y(tǒng)_m sinθ₁-x_m cosθ₁，

|O′P|＝|O′R|cosθ₁＝(|O′Q|-|QR|)cosθ₁＝y(tǒng)_m cosθ₁+x_m sinθ₁

所以機(jī)器人位置為：

偏轉(zhuǎn)角度為θ₁，機(jī)器人需要轉(zhuǎn)動(dòng)角度θ₁進(jìn)行位置調(diào)整，通常單次轉(zhuǎn)動(dòng)角度為θ₁的六分之一，通過(guò)多次調(diào)整來(lái)完成對(duì)偏差角度θ₁的校正。

這樣，通過(guò)兩次線性分割重構(gòu)出充電房模型。然后利用重構(gòu)充電房模型的關(guān)鍵點(diǎn)，實(shí)現(xiàn)機(jī)器人的精準(zhǔn)定位。該方法首先通過(guò)快速高效的二次點(diǎn)集分割算法對(duì)連續(xù)的激光數(shù)據(jù)集進(jìn)行分割，獲取魯棒性較高的三個(gè)邊緣特征，然后對(duì)充電房模型進(jìn)行重構(gòu)，利用重構(gòu)充電房模型對(duì)應(yīng)的關(guān)鍵點(diǎn)對(duì)機(jī)器人進(jìn)行定位，在保證定位準(zhǔn)確性的同時(shí)，不直接依賴于充電房墻角拐點(diǎn)，對(duì)充電房建造工藝依賴較低。

同時(shí)，在保證機(jī)器人自主返回充電高成功率的基礎(chǔ)上，添加機(jī)器人充電過(guò)程中干擾自動(dòng)恢復(fù)的功能。通過(guò)對(duì)機(jī)器人充電狀態(tài)進(jìn)行監(jiān)測(cè)，對(duì)于外界環(huán)境造成的充電中斷進(jìn)行自動(dòng)恢復(fù)，提高機(jī)器人充電過(guò)程的效率和抗干擾能力。

自動(dòng)充電方法的具體方案描述如下：

1.當(dāng)機(jī)器人需要補(bǔ)充電力時(shí)，通過(guò)定位導(dǎo)航技術(shù)自動(dòng)駛向充電房，通過(guò)發(fā)送指令打開(kāi)房門，機(jī)器人進(jìn)入充電房中；

2.進(jìn)入充電房后，利用激光設(shè)備采集的激光數(shù)據(jù)，進(jìn)行充電房?jī)?nèi)幾何特征的檢測(cè)和提取，進(jìn)行直線擬合、分割以及關(guān)鍵點(diǎn)的提取，根據(jù)提取的直線和關(guān)鍵點(diǎn)，通過(guò)幾何運(yùn)算進(jìn)行機(jī)器人局部定位，必要時(shí)結(jié)合視覺(jué)信息；

3.持續(xù)根據(jù)定位調(diào)整機(jī)器人位置和朝向，直到機(jī)器人定位在充電樁正前方處；

4.確定機(jī)器人位于正確位置后，伸出充電臂，插入到充電樁中；

5.檢查機(jī)器人是否處于充電狀態(tài)，若是自動(dòng)充電完成，收回充電臂，否則，重新進(jìn)行調(diào)整并檢測(cè)；通過(guò)對(duì)機(jī)器人充電狀態(tài)進(jìn)行監(jiān)測(cè)，對(duì)于外界環(huán)境造成的充電中斷進(jìn)行自動(dòng)恢復(fù)，若充電過(guò)程中出現(xiàn)充電狀態(tài)打斷，判斷機(jī)器人位姿是否發(fā)生變化，若是，退出充電樁進(jìn)行重新調(diào)整，否則再次進(jìn)行充電樁和機(jī)器人端的上電操作，有效提高機(jī)器人充電過(guò)程的效率和抗干擾能力。

6.充電完成后機(jī)器人自動(dòng)退出充電樁，繼續(xù)進(jìn)行工作。