本發(fā)明涉及機器人自主導航，具體為一種面向特征退化場景多傳感器融合的slam方法。

背景技術：

1、隨著機器人技術的發(fā)展，機器巡檢代替人工巡檢作業(yè)已成為目前主流方式，特別是在電力行業(yè)中狹長隧道的巡檢工作，人工巡檢方式存在重復工作費時費力、效率較低、巡檢人員人身安全無保障、易受天氣環(huán)境影響等缺點，而智能機器狗巡檢在安全性、效率性和成本控制等方面存在明顯的優(yōu)勢，因此在智能機器狗巡檢工作中對slam技術展開研究。

2、在自主導航技術領域，slam技術為機器人自主導航提供了地圖和位姿信息，因此地圖構建的準確率將直接影響到機器狗導航精度，目前的slam技術，通常分為激光slam和視覺slam，激光slam是利用激光雷達傳感器，捕獲周圍點云信息，由于激光雷達的特征，因此激光slam直接捕獲遠距離環(huán)境信息，并且在較大場景中具有很好的建圖和定位效果，但是這種方法在無結構(即特征退化)的環(huán)境時會失敗，比如長隧道、長走廊和平坦開闊的地方，視覺slam是一種稠密建圖的方法，特別適合于位置識別，并且在紋理豐富的環(huán)境中表現良好，但是該方法對光照變化、快速運動和初始化很敏感。

3、雖然slam技術取得一定成果，但是在隧道場景中，由于特征嚴重退化或沒有顯著特征，將導致激光slam建圖和定位失敗，同時在隧道中環(huán)境通常相似，也即沒有明顯特征，而且在無人工作業(yè)時，通常沒有光源或光源分布或明或暗，使得視覺slam失效，因此在隧道場景中一般的視覺和激光slam不具備可行性。

技術實現思路

1、本發(fā)明提供了一種面向特征退化場景多傳感器融合的slam方法，具備實現智能機器狗在狹窄隧道中的建圖和定位，該方法融合激光雷達、imu和rfid傳感器數據，使用因子圖優(yōu)化，解決在隧道特征嚴重退化場景中建圖和定位失敗的問題，以建立高精度點云地圖和定位的優(yōu)點，解決了上述背景技術所提出的問題。

2、本發(fā)明提供如下技術方案：一種面向特征退化場景多傳感器融合的slam方法，包括以下步驟：

3、步驟s1：利用激光雷達獲取環(huán)境點云數據，然后對點云數據進行預處理，剔除無效的數據；其次，利用雷達點云數據并結合二維碼的強度信息構建時空指示特征，并賦予更大的權重，利用自適應權重點云配準算法構建激光雷達里程計；

4、步驟s2：構建多差自感知預測網絡模型和訓練效果評價目標函數，以預測imu誤差并實現對imu的角速度和加速度進行誤差補償，然后對補償后的imu數據積分構建imu里程計；

5、步驟s3：利用rfid閱讀器獲取id和測量標簽在不同載波頻率下的相位，利用相位差以得到當前機器狗到標簽的距離；

6、步驟s4：構建幾何標簽導向型里程計模型，該模型利用激光雷達的幾何點云數據分割隧道左右墻面得到墻面的直線或曲線方程，并且結合rfid標簽下的局部位姿為導向得到全局坐標系下的位姿關系，以此獲得rfid的里程計；

7、步驟s5：由于在隧道特征退化嚴重下，雷達里程計可能出現較大偏差，因此利用rfid里程計并采用偏值自修正融合算法修正雷達里程計和imu里程計；

8、步驟s6：利用因子圖優(yōu)化機器狗位姿和全局地圖，同時采用偏值自修正融合算法修正雷達里程計、imu里程計和rfid里程計。

9、優(yōu)選的，步驟s1具體包括：

10、①激光雷達的x軸為機器狗的正前方行駛方向，y軸為垂直于機器狗從右向左為正方向，從下向上為z軸的正方向，原點為o，剔除機器狗自身的點云數據，并且找出采集數據中不符合隧道場景中的數據并剔除，即剔除距離特別大和特別小的數據；

11、②利用雷達點云數據并結合二維碼的強度信息提取時空指示特征并賦予更大的自適應權重，若當前點云數據由增強特征xf的點云和非增強點云數據xp組成，則當前點云數據x可表示為x＝[xf,xp]，因此其權重分配如下：

12、

13、其中，k表示中提取增強特征點云子集的數量，nj表示第j個增強特征點云子集中點的個數，wfj表示第j個增強特征點云子集中點的權重，wf＝{wf1...wfj}，m表示非增強特征點云的點個數，wp表示其權重；

14、③利用s12確定的權重系數，采用自適應權重點云配準技術建立里程計約束，其配準目標函數如下：

15、q＝wf||(xf-yfrt-t)||2+wp||(xp-yprt-t)||2??(2)

16、其中，xf，yf分別表示上一幀和當前幀激光雷達特征點云數據，xp，yp分別表示上一幀和當前幀激光雷達非特征點云數據，r、t分別表示旋轉矩陣和平移向量，假設關鍵幀之間有n幀雷達數據，因此激光雷達關鍵幀間里程計位置xl和姿態(tài)posl可表示為：

17、

18、優(yōu)選的，步驟s2具體包括：

19、①讀取imu數據，利用長短時記憶深度神經網絡對網絡的輸入層、隱藏層、輸出層進行設計，使其能對imu的實際誤差進行預測和表征，其中隱藏層設計為5層，每層節(jié)點數為60的lstm網絡模型，其神經網絡輸入信息設計為：三軸陀螺儀的三維角速度w'和三軸加速度計的加速度a'，其輸出信息設計為：三軸陀螺儀的三維角速度誤差δwimu和三軸加速度計的加速度誤差δaimu；

20、②根據三軸陀螺儀的三維角速度誤差δwimu＝[δwx,δwy,δwz]t和三軸加速度計的加速度誤差δaimu＝[δax,δay,δaz]t，在誤差模型形式上設計如下的誤差模型：

21、

22、其中，式(4)和(5)中第一項δw0和δa0分別為陀螺和加速度計的隨機常值零偏向量，其中δw0＝[δw0x,δw0y,δw0z]t，其他變量以此類推，第二項δwt和δat分別為零偏隨溫度的漂移向量，第三項分別表示系數矩陣kw與角速度的乘積和系數矩陣ha與加速度的乘積，第四項分別表示系數矩陣ka與加速度的乘積和系數矩陣hw和角速度的乘積，第五項分別表示隨機誤差；

23、其次，為了確定網絡隱藏層結構，對不同結構的lstm網絡進行試探訓練，以輸出層誤差向量的均方差(mse)作為網絡訓練結果精度的衡量指標，公式如下：

24、

25、其中，yi'表示imu預測值，y表示imu真值，n為樣本數量，x表示向量長度；

26、③對imu數據進行補償則有：

27、

28、其次建立imu積分模型，考慮噪聲和零偏建立關鍵幀間imu傳感器模型如下：

29、

30、其中，wimu和aimu分別表示imu測量的角速度和加速度，r表示從世界坐標系到imu坐標系的轉動矩陣，w和a分別表示去除零偏和噪聲后的角速度和加速度，bw和ba分別表示角速度和加速度零偏，nw和na表示傳感器的高斯噪聲，g表示重力加速度；

31、④建立imu運動模型，假設在t時刻的δt時間后速度vt+δt、位移xt+δt和旋轉rt+δt可表示為：

32、

33、優(yōu)選的，步驟s3具體包括：

34、①首先利用rfid接收器短時多次讀取rfid標簽數據，防止誤觸發(fā)，同時利用均值濾波增加數據的可靠性；

35、②rfid標簽的發(fā)射頻率為f1和f2，接收端接收的相位為φ1和φ2，機器狗的距離標簽的位置d：

36、

37、其中，c是光速。

38、優(yōu)選的，步驟s4具體包括：

39、①首先利用激光雷達的點云數據在機器狗坐標系下分割出墻面，并投影為曲線；然后假設在隧道中彎道不會出現突變且近似為圓弧，利用曲線方程判斷機器狗是處于彎道還是直道內；

40、②如果是直道，則標簽rfid2在rfid1坐標系下的位置即向x方向偏移l距離；

41、③如果是彎道，利用該直線上的三個點求出隧道半徑r，并利用弧長l求出圓心角θ，其中θ繞z軸從負半軸向正半軸順時針為正，如圖3所示；

42、④因此可以得知rfid2標簽在rfid1坐標系下的位置，設其位置為x2＝[x2；y2]，rfid1標簽位置為x1＝[x1；y1]，則有：

43、

44、其中o表示圓心，在rfid1坐標系坐標為(0，-r)，由于在rfid標簽的安裝時標簽和接收器約在同一平面，因此不考慮垂直高度的變化，因此可以利用測得距離標簽的距離得出機器狗在rfid1坐標系下的坐標m1＝[mx1；my1]：

45、mx12+my12＝d12

46、(mx1-rsinθ)2+(my1-r+rcosθ)2＝d22(11)

47、其中，d1和d2分別表示接收器測得標簽rfid1和rfid2的距離，最后解該方程并利用機器狗始終保持在墻的內側來排除多余解；

48、⑤由s42可知在直道時rfid前后坐標系的變換關系為：

49、

50、由s44可知在彎道時rfid前后的坐標系的變換關系為：

51、

52、其中prfid1和prfid2分別表示在rfid1和rfid2的坐標系下機器狗的坐標，假設當前機器狗位置mj＝[mxj；myj]經過一系列rfid坐標系變換tc1、tc2....tcj、ts1、ts2得到在初始rfid1坐標系下的坐標為mgj：

53、mgj＝tc1tc2....tcjts1ts2mj??(14)

54、由于rfid1的坐標系和實際全局坐標系存在一個d_init的偏移，如圖3所示，因此可得機器狗在全局坐標系下的坐標為mgj＝mgj+[0?d_init]t，因此可構建rfid里程計。

55、優(yōu)選的，步驟s5具體包括：

56、利用偏值自修正融合算法修正s1的激光雷達里程計和s2的imu里程計以及s4的rfid里程計；

57、x＝α1xl+α2ximu+α3mgj??(15)

58、

59、其中ximu即為s2中得到的xt+δt，x表示融合后的位置，α1、α2和α3表示自適應權重，經過歸一化處理的權重通過不斷迭代，直到收斂。

60、優(yōu)選的，步驟s6具體包括：

61、①利用融合后的位置初始化關鍵幀之間scan-to-map點云配準的初始位置；

62、②在因子圖優(yōu)化后得到高精度的機器人位置和姿態(tài)，然后利用該位姿反饋給里程計以修正里程計累積誤差。

63、本發(fā)明具備以下有益效果：

64、1、該面向特征退化場景多傳感器融合的slam方法，相比于現有激光slam和視覺slam，利用黑色材料增強特征，并利用了激光雷達強度信息，實現穩(wěn)定可靠的特征檢測方法。

65、2、該面向特征退化場景多傳感器融合的slam方法，利用自適應多權重的點云配準方法，為高精度的雷達里程計提供保障。

66、3、該面向特征退化場景多傳感器融合的slam方法，利用rfid標簽定位為點云配準提供了初始值和并利用自適應權重融合方法修正累積里程計和imu里程計的誤差。

67、4、該面向特征退化場景多傳感器融合的slam方法，通過融合多傳感器數據，解決了slam在隧道特征嚴重退化時建圖和定位失敗問題，提高了slam建圖和定位能力的穩(wěn)定性、準確性以及魯棒性。