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

背景技術：

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

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

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

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

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

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

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

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

(5)通過步驟(4)得到的充電房邊緣特征L₁,L₂,L₃，通過直線相交獲取充電房的兩個關鍵點，即左邊墻面對應直線L₃與充電房左角落、右邊墻面對應直線L₂與充電房右角落形成的拐點；

(6)利用步驟(5)獲得兩個關鍵點與機器人之間的相對位置，確定機器人位置。

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

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

在獲取右端點P_re、左端點P_le時，設定閾值threshold分別略大于充電房右角落的寬度、充電房左角落的寬度。

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

本發(fā)明還提供一種巡檢機器人自動充電方法，巡檢機器人通過定位導航自動駛入充電房；利用本發(fā)明提供的巡檢機器人定位方法對巡檢機器人進行定位；根據(jù)定位調(diào)整巡檢機器人位置和朝向，直到巡檢機器人定位在充電樁正前方處；巡檢機器人充電部件與充電樁接觸進行充電。

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

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

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

附圖說明

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

圖2為二次點集分割算法原理圖；

圖3為第一次點集分割程序流程圖；

圖4為第二次點集分割程序流程圖；

圖5為機器人定位原理圖。

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

具體實施方式

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

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

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

①第一次點集分割：通過平滑擬合，將累計誤差平攤，提高分割的準確性。

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

同樣的方法，可以得到左分割點P_l，

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

5、關鍵點獲取：利用直線L₂、L₃以及同時垂直于直線L₂和直線L₃的直線L₁重構充電房模型，即通過步驟4得到的充電房邊緣特征L₁,L₂,L₃，通過直線相交獲取重構充電房模型的兩個關鍵點(拐點，即，左邊墻面對應直線L₃與充電房左角落的交點、右邊墻面對應直線L₂與充電房右角落的交點)。

6、機器人定位：利用步驟5重構充電房模型的兩個關鍵點與機器人之間相對位置，確定機器人位置。

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

由M點坐標為(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θ₁

所以機器人位置為：

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

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

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

自動充電方法的具體方案描述如下：

1.當機器人需要補充電力時，通過定位導航技術自動駛向充電房，通過發(fā)送指令打開房門，機器人進入充電房中；

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

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

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

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

6.充電完成后機器人自動退出充電樁，繼續(xù)進行工作。