本發(fā)明涉及的智能服務(wù)機(jī)器人定位導(dǎo)航系統(tǒng)，特別是涉及應(yīng)用于導(dǎo)航的面向復(fù)雜環(huán)境的智能服務(wù)機(jī)器人定位導(dǎo)航系統(tǒng)。

背景技術(shù)：

1、對于服務(wù)機(jī)器人來說，自主定位導(dǎo)航技術(shù)已經(jīng)成為產(chǎn)品的核心和焦點之一，它所起的作用直接關(guān)系著機(jī)器人的智能化水平。雖然現(xiàn)在常見的一些智能服務(wù)機(jī)器人能夠通過既定的程序?qū)崿F(xiàn)特定環(huán)境下的定位導(dǎo)航功能，例如家用掃地機(jī)器人能夠在家里完成清掃任務(wù)，但是應(yīng)用范圍僅限于室內(nèi)。目前市面上的服務(wù)機(jī)器人尚未具備如下功能：

2、1)環(huán)境切換時自動調(diào)節(jié)的能力，從而保持系統(tǒng)穩(wěn)定；

3、2)在復(fù)雜環(huán)境中確認(rèn)機(jī)器人所處的位置，從而實現(xiàn)對周圍環(huán)境的判斷能力；

4、3)在未知環(huán)境中深度挖掘有效信息來判斷機(jī)器人所處的位置，從而實現(xiàn)最優(yōu)路徑規(guī)劃等功能。

5、這必然導(dǎo)致機(jī)器人功能的單一性和對人為干預(yù)的依賴性高等缺點，從而嚴(yán)重限制了智能服務(wù)機(jī)器人的大規(guī)模開發(fā)和應(yīng)用。

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

1、針對上述現(xiàn)有技術(shù)，本發(fā)明要解決的技術(shù)問題是現(xiàn)有的服務(wù)機(jī)器人功能單一且對認(rèn)為干預(yù)的依賴性高。

2、為解決上述問題，本發(fā)明提供了面向復(fù)雜環(huán)境的智能服務(wù)機(jī)器人定位導(dǎo)航系統(tǒng)，包括安裝在機(jī)器人底盤上的工控機(jī)以及運動控制器，工控機(jī)與運動控制器信號連接，運動控制器包括電機(jī)驅(qū)動模塊、超聲波模塊、安裝在機(jī)器人底盤上的陀螺儀模塊、藍(lán)牙模塊以及安裝在機(jī)器人底盤上表面的數(shù)碼管模塊；

3、電機(jī)驅(qū)動模塊包括電源模塊、電流采集模塊以及兩個無刷直流電機(jī)；

4、超聲波模塊包括安裝在機(jī)器人底盤四周的多個超聲波探測器；

5、導(dǎo)航系統(tǒng)還包括感知基礎(chǔ)模塊，感知基礎(chǔ)模塊包括安裝在機(jī)器人上的3d激光雷達(dá)、2d激光雷達(dá)、imu模塊以及攝像頭；

6、導(dǎo)航系統(tǒng)又包括復(fù)雜場景定位模塊，復(fù)雜場景定位模塊包括安裝在機(jī)器人底盤內(nèi)的gps定位模塊以及uwb定位模塊；

7、基于導(dǎo)航系統(tǒng)，機(jī)器人在復(fù)雜環(huán)境中的定位導(dǎo)航的方法，包括以下步驟：

8、s1、服務(wù)機(jī)器人自主導(dǎo)航及障礙物檢測與定位；

9、s2、服務(wù)機(jī)器人動態(tài)環(huán)境中的實時路徑規(guī)劃：通過全局路徑規(guī)劃和局部路徑規(guī)劃相結(jié)合，實現(xiàn)實時路徑規(guī)劃；

10、s3、服務(wù)機(jī)器人在復(fù)雜場景中對自身位置的定位：通過機(jī)器人自身的感知基礎(chǔ)模塊，對環(huán)境進(jìn)行測量，當(dāng)環(huán)境測量結(jié)果超出給定范圍時，就判定當(dāng)前所處場景實在室外，并基于此切換定位算法，實現(xiàn)進(jìn)行室內(nèi)外的gps定位和uwb定位的切換，以及對室內(nèi)外場景的切換；

11、s4、基于圖像深度語義分割的機(jī)器視覺分析，建議環(huán)境的深度語義地圖；

12、s5、基于大數(shù)據(jù)的機(jī)器人自主認(rèn)知導(dǎo)航：機(jī)器人在相對確定的區(qū)域內(nèi)長時間運動，通過收集運行過程中的環(huán)境信息，以深度學(xué)習(xí)或概率學(xué)的方法分析出環(huán)境中各種信息的關(guān)聯(lián)性，然后再判定當(dāng)前運行狀態(tài)中的環(huán)境信息，實現(xiàn)自主認(rèn)知導(dǎo)航，同時對后續(xù)的導(dǎo)航進(jìn)行優(yōu)化。

13、作為本技術(shù)的進(jìn)一步改進(jìn)，步驟s1的具體操作如下：

14、s11、生成高度圖：

15、根據(jù)機(jī)器人的行走速度，考慮機(jī)器人前方5m范圍內(nèi)的障礙物即可滿足機(jī)器人實時障礙物檢測與躲避的要求，設(shè)前方5m、寬3m、高2m的范圍作為感興趣區(qū)域，超出該區(qū)域的點將被忽略；

16、首先將感興趣區(qū)域沿著x、z方向劃分成底面邊長為c的立方體，然后將每一個立方體沿著y方向劃分成高度為b的格子；假設(shè)(x0,y0,z0)、(xmax,ymax,zmax)分別為感興趣區(qū)域的兩個邊界點，p＝(x，y，z)為空間中任意一個觀測點，則該點所在的網(wǎng)格的索引(u，v，w)可以按下面的公式求得：

17、

18、將所有的測點映射到網(wǎng)格后，可以找到每一個正四面體中最高的包含觀測點的網(wǎng)格，并記錄下該網(wǎng)格的高度，進(jìn)而得到感興趣空間內(nèi)的高度分布圖；

19、s12、搜索初始點：

20、采用基于圖的廣度搜索方法找到所有的障礙物網(wǎng)格及機(jī)器人可通過的網(wǎng)格，同時采用螺旋形搜索算法確定機(jī)器人可通過的一個點作為搜索的初始點；

21、搜索點的確定方法具體為：

22、給定一個網(wǎng)格(uj,vj)，在其周圍定義一個邊長為l的搜索窗口，當(dāng)網(wǎng)格滿足下面的條件，則將網(wǎng)格作為要尋找的可通過點：

23、網(wǎng)格中有觀測到的三維數(shù)據(jù)點，并且該網(wǎng)格不在感興趣空間的邊界；

24、網(wǎng)格周圍的四個網(wǎng)格中均有數(shù)據(jù)點，即(ui-1,vj)，(ui+1,vj)，(ui,vj-1)和(ui,vj+1)；

25、窗口中的有效網(wǎng)格足夠多；

26、窗口中其它網(wǎng)格的高度與該網(wǎng)格的高度之差小于某個常數(shù)；

27、將機(jī)器人正前方0.5m處的網(wǎng)格(u0,v0)作為第一個搜索的網(wǎng)格，判斷該網(wǎng)格是否滿足上面的條件；若不滿足，則進(jìn)行螺旋形搜索，直到找到機(jī)器人可通過的初始點；

28、s13、障礙物分析：

29、在得到初始搜索點后，采用廣度優(yōu)索策略得到所有障礙物網(wǎng)格；對于每一個網(wǎng)格，判斷其4-鄰域的四個網(wǎng)格是否為障礙物；

30、假設(shè)當(dāng)前網(wǎng)格的高度為h，待判斷網(wǎng)格的高度為h'，當(dāng)滿足下面的條件，則認(rèn)為待判斷的網(wǎng)格是障礙物：

31、其中，hrobot為機(jī)器人能跨越的最大高度，θrobot為機(jī)器人能行走的斜坡的最大坡度；在得到所有障礙物網(wǎng)格后，將網(wǎng)格內(nèi)的所有點標(biāo)記為障礙物，其余點為可通行區(qū)域；

32、然后將得到的障礙物網(wǎng)格向x-z平面投影，并提取出投影曲線的最小外接四邊形輪廓；

33、四邊形的長和寬對應(yīng)于障礙物的長和寬，該四邊形中最高的網(wǎng)格對應(yīng)于障礙物的高度；機(jī)器人到該四邊形的最短距離為機(jī)器人到障礙物的距離，最短距離所連接的四邊形的點的坐標(biāo)即為障礙物的坐標(biāo)；

34、s14、障礙物地圖更新：

35、擬通過將障礙物空間內(nèi)點云信息與長時建立的全局點云地圖進(jìn)行比較，以確定檢測到障礙物是在環(huán)境動態(tài)變化后的產(chǎn)生的還是屬于場地的固有障礙物；對所有障礙物點云形成一個障礙物地圖，對每個障礙物區(qū)域進(jìn)行貝葉斯概率更新；

36、設(shè)初始條件下所有區(qū)域為靜態(tài)障礙物的先驗概率為p，在對一個區(qū)域a進(jìn)行了測量之后，得到該區(qū)域的為類別c的概率pc和對應(yīng)區(qū)域的為靜態(tài)障礙物的概率信息pac，對該區(qū)域為靜態(tài)障礙物的概率p'使用貝葉斯定理即得到：

37、作為本技術(shù)的再進(jìn)一步改進(jìn)，步驟s2中全局路徑規(guī)劃的具體操作為：

38、將dijkstra算法(從初始點出發(fā))和bfs算法(從目標(biāo)點出發(fā))的有效信息結(jié)合起來，構(gòu)建兩種代價：從初始點到任意結(jié)點n的代價h(n)和從任意結(jié)點n到目標(biāo)點的啟發(fā)式評估代價w(n)；

39、當(dāng)從初始點向目標(biāo)點移動時，通過對這兩種代價的權(quán)衡，構(gòu)建啟發(fā)式函數(shù)，不斷引導(dǎo)機(jī)器人搜索到全局最優(yōu)路徑；

40、其中，啟發(fā)函數(shù)為：f(n)＝μh(n)+γw(n)；

41、局部路徑規(guī)劃的具體操作為：

42、機(jī)器人局部路徑規(guī)劃的本質(zhì)是從起始點開始，通過在可行區(qū)域間的移動達(dá)到目標(biāo)區(qū)域，其導(dǎo)航過程可描述為：path＝lmaporg，free→l→lmapn，free→l→lmapgoal，free；

43、基于規(guī)劃出一條與目標(biāo)越來越近、自身行駛速度較快、與障礙物盡可能遠(yuǎn)的路徑的目的，對局部路徑進(jìn)行調(diào)優(yōu)；

44、g(v，w)＝α*heading(v，w)+β*dist(v，w)+γ*vel(v，w)，

45、其中heading用于評價機(jī)器人當(dāng)前行駛方向與目標(biāo)位置的夾角；dist表示機(jī)器人與障礙物的距離，即必須設(shè)定安全裕度，在機(jī)器人和障礙物之間要保留一定的間隙，且該間隙隨著速度提高而線性增長；安全速度指機(jī)器人能夠在碰撞障礙物之前能夠停下的最大速度，設(shè)機(jī)器人實際速度為(va，wa)，移動障礙物的實際速度為(vb，wb),

46、

47、采取動態(tài)窗口法在速度空間中進(jìn)行速度采樣，并且對隨機(jī)采樣的速度進(jìn)行限制，減少采樣數(shù)目，這樣在代價函數(shù)評估時會引入少的計算量，同時保證其有效性；設(shè)采樣時間間隔為t，考慮到機(jī)器人的動力加速度，將搜索空間降采樣到動態(tài)窗口，只保留以當(dāng)前加速度可到達(dá)的速度，則動態(tài)窗口中速度應(yīng)滿足：

48、v2＝{v，w|v＝v，w|v∈[va-vz*t，va+vk*t]∩[wa-wz*t，wa+wk*t]}

49、最終在路徑區(qū)間lmapnfree上得到的安全速度集合合φn＝v1∩v2，最終的代價函數(shù)vel是基于安全速度集合φn計算的。

50、作為本技術(shù)的更進(jìn)一步改進(jìn)，對室外的gps定位的定位信號采用用卡爾曼濾波的方法進(jìn)行處理，增強魯棒性；

51、設(shè)在k時刻接收到的定位結(jié)果為x0,y0,z0,則：

52、其中x，y，z為理想狀態(tài)變量εx，εy，εz為一階馬爾可夫國策過程誤差,ωx，ωy，ωz為測量誤差；

53、建立可觀測的線性卡爾曼模型：

54、x(k|k-1)＝φ1(k|k-1)x(k-1)

55、x(k)＝x(k|k-1)+k(k)[b(k)-h(k)x(k|k-1)]

56、k(k)＝p(k|k-1)ht(k)[h(k)p(k|k-1))ht(k)+r(k)]-1

57、p(k|k-1))＝φ(k|k-1)p(k|k-1)φt(k|k-1)+q(k|k-1)

58、上式中φ1(k|k-1)＝{φ1x(k|k-1)φ1y(k|k-1)φ1z(k|k-1)}

59、其中q(k)為系統(tǒng)的噪聲協(xié)方陣差，q的離散化矩陣。

60、作為本技術(shù)的更進(jìn)一步改進(jìn)，將室內(nèi)的uwb定位的測距測量符號化為ruwb，將基于lidar的測距測量符號化為rlid，讓xt＝[p0，x，t，p0，y，t，v0，x，t，v0，y，t]t，

61、

62、令代表由機(jī)器人信標(biāo)位置和信標(biāo)的速度組成的與uwb相關(guān)的競爭狀態(tài)，根據(jù)以下動力學(xué)模型演化狀態(tài)xt：xt＝ftxt-1+gtwt，

63、轉(zhuǎn)移矩陣ft為：gt為：

64、狀態(tài)噪聲wt為零均值，協(xié)方差而δ為采樣間隔；

65、令為在時間t測量的基于uwb的范圍，范圍由機(jī)器人到信標(biāo)范圍和信標(biāo)到信標(biāo)范圍它們是pt的非線性函數(shù)：

66、

67、其中i＝1,2,...,nt(nt+1)/2索引uwb節(jié)點的成對組合，假設(shè)[rt]i，i＝1,2,...,nt(nt+1)/2a中的所有對等范圍都被i.i.d破壞，加噪聲

68、

69、之后對得到的數(shù)據(jù)進(jìn)行優(yōu)化，提升定位數(shù)據(jù)魯棒性：

70、

71、其中和pt|t分別是更新后的狀態(tài)估計和協(xié)方差估計；

72、在室內(nèi)以及機(jī)器人上均布置足夠多的uwb，通過上述模型進(jìn)行定位，能得到更準(zhǔn)確的定位效果。

73、作為本技術(shù)的更進(jìn)一步改進(jìn)，步驟s4中，深度語義地圖的建立方法為：

74、s41、估計相機(jī)在在每一幀圖像的位置和姿態(tài)；

75、s42、將上一步驟中的每一幀的圖像轉(zhuǎn)化為點云，之后將這寫點云轉(zhuǎn)換到同一坐標(biāo)系下，進(jìn)行拼接，形成點云地圖，然后在點云地圖的基礎(chǔ)上，生成占據(jù)柵格地圖；

76、s43、在已有的柵格地圖上，對空間中3d點賦予語義信息，將包含目標(biāo)的語義信息映射到空間地圖中，根據(jù)極大似然估計進(jìn)行語義融合，即可得到語義地圖。

77、作為本技術(shù)的更進(jìn)一步改進(jìn)，語義信息的生成包括以下步驟：

78、sa、確定目標(biāo)可能存在的區(qū)域；

79、sb、對提取出的區(qū)域進(jìn)行識別，判斷出區(qū)域中物體的種類；

80、sc、對檢測出的目標(biāo)區(qū)域進(jìn)行分割，從背景中盡可能準(zhǔn)確的將目標(biāo)提取出來，將屬于目標(biāo)的像素點與屬于背景的像素點區(qū)分開。

81、作為本技術(shù)的更進(jìn)一步改進(jìn)，步驟s5中自主認(rèn)知導(dǎo)航的實現(xiàn)通過深度學(xué)習(xí)變化模式的模型實現(xiàn)，模型能表示路徑可通行性之間的相關(guān)性，并利用這種相關(guān)性在導(dǎo)航期間實時進(jìn)行預(yù)測；

82、深度學(xué)習(xí)的方法包括以下步驟：

83、s51、環(huán)境建模：

84、在機(jī)器人運行期間，假設(shè)環(huán)境整體的可通行性的集合是et，機(jī)器人無法觀察到所有路徑的可通行性，只能觀察到其中一部分將觀察不到的部分記為ut，即需要機(jī)器人通過zt來分析出ut的可能性，并將次問題表述為：

85、

86、其中η是給定當(dāng)前觀測值zt的歸一化器，而p(et)是環(huán)境中路徑的可通行性的聯(lián)合概率分布；分布p(et)定義了可能配置空間上的概率函數(shù)，獲取了路徑的可通行性之間的相關(guān)性；

87、然后通過chow-liu樹近似方法，使用樹結(jié)構(gòu)的貝葉斯網(wǎng)絡(luò)表示聯(lián)合概率；通過使用靈活但有界的因子圖表示來近似路徑可通行性上的聯(lián)合概率分布；

88、s52、因子圖：

89、定義每個邊緣的一個一元因子節(jié)點和每對拓?fù)溥吘墝Φ囊粋€二元因子節(jié)點來建模邊緣的可通行性之間的相關(guān)性，將邊緣上的關(guān)節(jié)分布近似為：

90、

91、通過僅存儲|e|(|e|-1)/2+|e|，因子圖表示可以通過獲取路徑可通行性之間的一些相關(guān)性來近似估計環(huán)境配置下的概率，因而只需要表示邊緣數(shù)量的二次空間，而不是像完整聯(lián)合概率分布一樣需要指數(shù)空間；

92、s53、觀察計算：

93、將因子圖的一元因子和二元因子定義為：

94、φi＝p(ei)

95、

96、其中p(ei)和p(ei,ej)分別是路徑的可通過或阻塞的一元和二進(jìn)制聯(lián)合概率，基于因子節(jié)點的定義，近似聯(lián)合概率分布為：

97、

98、根據(jù)機(jī)器人之前的運行z1：t-1中的觀察值，計算一元和二元聯(lián)合概率p(ei)和p(ei,ej)。

99、綜上，(1)本技術(shù)通過機(jī)器人自身的傳感器(激光雷達(dá)、攝像頭等)，對環(huán)境進(jìn)行測量，當(dāng)環(huán)境測量結(jié)果超出給定范圍時，就判定當(dāng)前所處場景實在室外，并基于此切換定位算法。

100、(2)開發(fā)基于圖像深度語義分割的機(jī)器視覺分析和理解方法，進(jìn)一步優(yōu)化機(jī)器人自主建圖、定位導(dǎo)航、動態(tài)避障的功能。機(jī)器人通過對環(huán)境的深度語義解析，可以建立出環(huán)境的深度語義地圖。語義地圖可以使機(jī)器人定位和導(dǎo)航的精度得到提升，并且針對不同物體實現(xiàn)更智能化的動態(tài)避障。

101、(3)針對機(jī)器人在復(fù)雜場景中長時間運行的導(dǎo)航問題，設(shè)計開發(fā)一套基于大數(shù)據(jù)分析的機(jī)器人自主認(rèn)知導(dǎo)航方法。機(jī)器人在相對確定的區(qū)域內(nèi)長時間運動，通過收集運行過程中的環(huán)境信息，以深度學(xué)習(xí)或概率學(xué)的方法分析出環(huán)境中某些信息的關(guān)聯(lián)性，然后再判定當(dāng)前運行狀態(tài)中的環(huán)境信息，進(jìn)而對后續(xù)的導(dǎo)航進(jìn)行優(yōu)化。