本發(fā)明屬于移動(dòng)機(jī)器人識(shí)別與控制，具體涉及一種基于二維點(diǎn)云的移動(dòng)機(jī)器人自主回充方法。

背景技術(shù)：

1、隨著移動(dòng)機(jī)器人在各種應(yīng)用場(chǎng)景下使用頻率越來(lái)越高，移動(dòng)機(jī)器人自主充電進(jìn)行續(xù)航問題受到重視。越來(lái)越多的機(jī)器人配備自動(dòng)回充的功能，即在機(jī)器人電池電量即將耗盡時(shí)，自動(dòng)尋找充電樁并進(jìn)行對(duì)接實(shí)現(xiàn)充電。

2、目前常用的自主回充方式包括的遠(yuǎn)距離的紅外光引導(dǎo)方案、視覺跟蹤引導(dǎo)方案存在問題：1.需要在充電樁處額外安裝紅外光發(fā)射器等裝置，機(jī)器人除激光雷達(dá)外需額外添加傳感器；2.紅外信號(hào)傳輸距離小，識(shí)別效率低、成本高；3.視覺跟蹤服務(wù)等需要耗費(fèi)大量計(jì)算資源，且受光線條件影響較大。

3、此外，目前使用二維激光雷達(dá)進(jìn)行直線特征檢測(cè)的移動(dòng)機(jī)器人自動(dòng)回充的方法，如現(xiàn)有文獻(xiàn)1：申請(qǐng)?zhí)枮?02210788028.0的中國(guó)專利，通過(guò)預(yù)設(shè)閾值進(jìn)行點(diǎn)云數(shù)據(jù)濾波、點(diǎn)云數(shù)據(jù)分割、直線過(guò)濾，從而得到點(diǎn)云數(shù)據(jù)中的直線數(shù)據(jù)，因激光雷達(dá)獲得的點(diǎn)云存在隨探測(cè)距離增大而點(diǎn)云到原點(diǎn)距離的測(cè)量誤差將增大的問題，從而該方法中通過(guò)預(yù)設(shè)唯一閾值濾除非直線點(diǎn)的方法在過(guò)濾距離較遠(yuǎn)的點(diǎn)云等情況時(shí)誤差將增大，使得識(shí)別精度下降。

4、另外，移動(dòng)機(jī)器人與充電樁的識(shí)別與對(duì)接精度、穩(wěn)定性也尤為重要。

技術(shù)實(shí)現(xiàn)思路

1、本發(fā)明為了現(xiàn)有技術(shù)誤差大、識(shí)別精度低的問題，提出了一種基于二維點(diǎn)云的移動(dòng)機(jī)器人自主回充方法，該方法能實(shí)現(xiàn)有效的充電樁精準(zhǔn)識(shí)別并進(jìn)行平穩(wěn)流暢的移動(dòng)對(duì)接。

2、為實(shí)現(xiàn)上述目的，本發(fā)明采用的技術(shù)方案：

3、一種基于二維點(diǎn)云的移動(dòng)機(jī)器人自主回充方法，包括以下步驟：

4、s1：建立移動(dòng)機(jī)器人在地圖坐標(biāo)系下的運(yùn)動(dòng)學(xué)模型方程；

5、s2:通過(guò)二維激光雷達(dá)實(shí)時(shí)采集機(jī)器人周圍環(huán)境點(diǎn)云信息，通過(guò)坐標(biāo)變換從雷達(dá)坐標(biāo)系轉(zhuǎn)換到地圖坐標(biāo)系；

6、s3:對(duì)二維點(diǎn)云進(jìn)行聚類、濾波，并使用機(jī)器學(xué)習(xí)方法進(jìn)行分類，得到直線點(diǎn)簇與非直線點(diǎn)簇；

7、s4:針對(duì)分類出的直線點(diǎn)簇，根據(jù)充電樁的長(zhǎng)度特征，選擇出對(duì)應(yīng)的直線點(diǎn)簇為目標(biāo)充電樁；

8、s5:根據(jù)目標(biāo)充電樁二維點(diǎn)云數(shù)據(jù)，構(gòu)建機(jī)器人理想充電位姿與當(dāng)前位姿在地圖坐標(biāo)系下誤差數(shù)學(xué)關(guān)系式；

9、s6:將地圖坐標(biāo)系下位姿誤差轉(zhuǎn)換得到機(jī)器人坐標(biāo)系下位姿誤差，當(dāng)位姿誤差小于某一閾值則認(rèn)為移動(dòng)機(jī)器人已移動(dòng)至理想充電位姿，流程結(jié)束；否則轉(zhuǎn)到s7；

10、s7:基于位姿誤差構(gòu)建李雅普諾夫函數(shù)，從而設(shè)計(jì)移動(dòng)機(jī)器人的反演鎮(zhèn)定控制率，控制移動(dòng)機(jī)器人的速度與角速度以實(shí)時(shí)調(diào)整其位姿，返回s2。

11、進(jìn)一步的，所述步驟s3具體包括：

12、步驟s3.1)采用歐式聚類方法將二維點(diǎn)云聚類成若干類點(diǎn)簇，并認(rèn)為點(diǎn)數(shù)過(guò)少的點(diǎn)簇是噪點(diǎn)或細(xì)柱等，進(jìn)行去除；

13、步驟s3.2)對(duì)所有點(diǎn)簇通過(guò)最小二乘法擬合成直線，計(jì)算各點(diǎn)簇中心點(diǎn)到原點(diǎn)的距離，并與殘差平方和均值一起作為特征，引入高斯核函數(shù)使用svm算法學(xué)習(xí)訓(xùn)練，分類出直線點(diǎn)簇和非直線點(diǎn)簇。

14、進(jìn)一步地，步驟s3.2)具體包括：

15、對(duì)所有點(diǎn)簇通過(guò)最小二乘法擬合成直線，得到的直線為：

16、

17、式中：

18、

19、xij為第i個(gè)點(diǎn)簇中第j個(gè)點(diǎn)的橫坐標(biāo)，yij為第i個(gè)點(diǎn)簇中第j個(gè)點(diǎn)的縱坐標(biāo)，n為第i個(gè)點(diǎn)簇點(diǎn)的個(gè)數(shù)。

20、殘差平方和均值計(jì)算公式為：

21、

22、因?yàn)槎S激光雷達(dá)存在點(diǎn)云距離增大，點(diǎn)云測(cè)量距離誤差增大的特性，選取最小二乘直線擬合殘差平方和均值與擬合直線到原點(diǎn)距離d作為特征使用svm算法進(jìn)行訓(xùn)練學(xué)習(xí)分類。選取點(diǎn)簇各點(diǎn)橫縱坐標(biāo)平均值為該點(diǎn)簇中心點(diǎn)的橫縱坐標(biāo)，計(jì)算其與雷達(dá)坐標(biāo)系原點(diǎn)的距離為d。

23、本方法采用的svm算法中,引入高斯核函數(shù)將點(diǎn)云二維特征空間中數(shù)據(jù)映射到三維空間進(jìn)行分類和識(shí)別：

24、尋找最優(yōu)超平面即求解訓(xùn)練樣本最大幾何間隔：

25、

26、式中，其中w表示權(quán)重，b表示偏距，yi是第i個(gè)點(diǎn)簇的類別(是直線點(diǎn)簇為1，不是為-1)，xi是其特征向量，n是訓(xùn)練的總點(diǎn)簇?cái)?shù)量。

27、引入松弛變量ξi，使得容許存在一定程度的違例：

28、

29、式中，c是懲罰因子，ξi為懲罰因子。

30、將約束條件融合到優(yōu)化目標(biāo)函數(shù)構(gòu)建拉格朗日公式：

31、

32、式中，αi,μi是拉格朗日乘子；

33、將拉格朗日函數(shù)轉(zhuǎn)換為對(duì)偶問題進(jìn)行求解，并引入高斯核函數(shù)κ(xi,xj)替換內(nèi)積xi·j，將樣本映射到高維空間進(jìn)行分類和識(shí)別：

34、

35、式中，xi,xj是兩個(gè)的特征向量ρ是帶寬。

36、對(duì)拉格朗日函數(shù)求解得到直線點(diǎn)簇與非直線點(diǎn)簇最優(yōu)分割超平面，即決策函數(shù)為：

37、

38、式中，x是輸入樣本特征向量，zi是支持向量。

39、進(jìn)一步的，步驟s5中構(gòu)建的誤差數(shù)學(xué)關(guān)系式：

40、

41、式中，旋轉(zhuǎn)矩陣r1為[xr,yr,θr],[x,y,θ]分別是機(jī)器人在地圖坐標(biāo)系xwowyw下理想充電位姿與當(dāng)前位姿，θ2,θ1分別是機(jī)器人理想充電位姿坐標(biāo)系橫軸xr和當(dāng)前位姿坐標(biāo)系橫軸x與xw的夾角，l是機(jī)器人中心到前端充電口長(zhǎng)度，dh是充電樁到機(jī)器人中心的垂直距離，通過(guò)計(jì)算s4中該點(diǎn)簇?cái)M合直線到機(jī)器人坐標(biāo)系原點(diǎn)的距離得到，θh是充電樁到機(jī)器人中心的垂線與xw之間的夾角，通過(guò)計(jì)算擬合直線與機(jī)器人坐標(biāo)系橫軸的x夾角與θ1之和得到，dm是充電樁中點(diǎn)到機(jī)器人中心距離，通過(guò)該點(diǎn)簇首尾各取6個(gè)點(diǎn)取坐標(biāo)均值作為點(diǎn)簇中心點(diǎn)，計(jì)算其到機(jī)器人坐標(biāo)系原點(diǎn)的距離得到，θm是充電樁中點(diǎn)到機(jī)器人中心的連線與x的夾角，通過(guò)計(jì)算點(diǎn)簇中心點(diǎn)到機(jī)器人中心的連線與x的夾角得到。

42、進(jìn)一步的，在所述步驟s6中，將地圖坐標(biāo)系下位姿誤差轉(zhuǎn)換至機(jī)器人坐標(biāo)系xoy下，定義機(jī)器人位姿誤差pδ：

43、

44、式中，旋轉(zhuǎn)矩陣r2為[xe,ye,θe]是機(jī)器人在機(jī)器人坐標(biāo)系xoy下位姿誤差。

45、進(jìn)一步的，步驟s7具體包括：

46、對(duì)位姿誤差pδ的微分為：

47、

48、式中，v,ω是機(jī)器人在機(jī)器人坐標(biāo)系下的線速度與角速度。

49、基于位姿誤差pδ構(gòu)建李雅普諾夫函數(shù)v：

50、

51、式(11)中，k1,k2,k3為控制系數(shù)且均大于零。

52、函數(shù)v的微分為：

53、

54、根據(jù)式(13)可設(shè)計(jì)控制率為：

55、

56、式中，k4也為控制系數(shù)且大于0。

57、將控制率帶入v函數(shù)微分可得：

58、

59、由函數(shù)v表達(dá)式與函數(shù)v微分可知，選取的v函數(shù)滿足李雅普諾夫法穩(wěn)定性判據(jù)，所提出的鎮(zhèn)定控制率穩(wěn)定。

60、本方法提取二維點(diǎn)云數(shù)據(jù)得到直線點(diǎn)簇?cái)?shù)據(jù)與現(xiàn)有文獻(xiàn)1相比，主要區(qū)別在于：

61、1)現(xiàn)有文獻(xiàn)1方法s2中通過(guò)預(yù)設(shè)outlinedist、minsplitdist閾值濾除點(diǎn)云數(shù)據(jù)中誤差較大的值以及預(yù)設(shè)minsplitdist、maxlinegap將點(diǎn)云數(shù)據(jù)進(jìn)行分割得到直線結(jié)構(gòu)體數(shù)組，再通過(guò)預(yù)設(shè)minlinelength過(guò)濾掉點(diǎn)數(shù)過(guò)少的直線數(shù)據(jù)，最后通過(guò)最小二乘法處理每條直線得到距離radius與偏轉(zhuǎn)角度angle；本方法s3通過(guò)歐式聚類將二維點(diǎn)云聚類成若干類點(diǎn)簇，選取點(diǎn)簇中心點(diǎn)到原點(diǎn)的距離以及最小二乘直線擬合的殘差平方和均值作為特征使用svm算法進(jìn)行學(xué)習(xí)分類，分類出直線點(diǎn)簇與非直線點(diǎn)簇。

62、2)現(xiàn)有文獻(xiàn)1方法s3中通過(guò)將radius、angle相同的直線歸為一組，尋找每組中一條直線起點(diǎn)與另一條直線終點(diǎn)的歐式距離di,j最接近充電樁長(zhǎng)度的兩條直線作為充電樁兩側(cè)墻壁，并根據(jù)起點(diǎn)、終點(diǎn)坐標(biāo)計(jì)算充電樁中點(diǎn)坐標(biāo)[xp,yp]；本方法s4通過(guò)直線特征從各直線點(diǎn)簇中判斷選擇出目標(biāo)充電樁點(diǎn)簇。

63、3)現(xiàn)有文獻(xiàn)1方法s4中，通過(guò)基于步驟s3和步驟s4的處理結(jié)果與保存的充電樁中點(diǎn)坐標(biāo)和直線信息，重復(fù)s2、s3控制機(jī)器人先轉(zhuǎn)動(dòng)角度angle，正對(duì)墻壁，再重復(fù)s2、s3控制機(jī)器人沿x軸運(yùn)動(dòng)距離x＝xp–dw，重復(fù)s2、s3沿y軸移動(dòng)距離yp直至yp小于閾值ytolerance，重復(fù)s2、s3旋轉(zhuǎn)機(jī)器人使機(jī)器人充電口正對(duì)充電樁向充電口方向移動(dòng)x′＝xp-d，最終完成對(duì)接充電；本方法根據(jù)目標(biāo)充電樁點(diǎn)簇點(diǎn)云信息特征構(gòu)造當(dāng)前位姿與理想位姿誤差模型，根據(jù)李雅普諾夫第二法設(shè)計(jì)鎮(zhèn)定控制率控制移動(dòng)機(jī)器人線速度與角速度實(shí)現(xiàn)閉環(huán)控制對(duì)接。

64、本發(fā)明的有益效果為：

65、1)本發(fā)明基于二維激光雷達(dá)采集點(diǎn)云識(shí)別充電樁，相較于紅外光引導(dǎo)、視覺跟蹤引導(dǎo)無(wú)需額外增加傳感器，方式簡(jiǎn)單、可靠，成本低；

66、2)本發(fā)明通過(guò)機(jī)器學(xué)習(xí)的方法，實(shí)現(xiàn)距離自適應(yīng)分類出直線點(diǎn)簇，提取直線數(shù)據(jù)的精度更高，效果更好。

67、3)本發(fā)明提出一種鎮(zhèn)定控制率，使得機(jī)器人平穩(wěn)、快速地以一條平滑路徑與充電樁對(duì)接，對(duì)接精度高，耗費(fèi)時(shí)間短，且避免了運(yùn)動(dòng)的振蕩。