必一運動科技：一種多傳感器融合定位方法與流程
1、多源數(shù)據(jù)融合技術(shù)是一種通過整合不同來源的信息，對其進行統(tǒng)一評價并最終得到一致信息的技術(shù)。在許多領(lǐng)域，如智能交通、醫(yī)療、金融和導(dǎo)航定位中，都有廣泛的應(yīng)用。
2、在導(dǎo)航定位領(lǐng)域，多源數(shù)據(jù)通常是來自各類傳感器。為了提高定位定姿的精度和可靠性，多傳感器融合定位成為一種重要的方法。多傳感器融合定位可以將傳感器在空間和時間上的互補與冗余信息組合起來，依據(jù)某種優(yōu)化準則進行組合，從而產(chǎn)生對觀測環(huán)境或?qū)ο蟮囊恢滦越忉尯兔枋觯@樣可以提高整個傳感器系統(tǒng)的有效性和正確性。
1、基于此，有必要針對上述問題，提出了一種多傳感器融合定位方法，使得可以得到高精度的綜合融合定位結(jié)果，以提高整個傳感器系統(tǒng)的有效性和正確性。
2、為實現(xiàn)上述目的，本發(fā)明在第一方面提供一種多傳感器融合定位方法，所述方法包括：
3、獲取rtk的rtk數(shù)據(jù)、imu的imu數(shù)據(jù)、激光雷達的激光雷達數(shù)據(jù)和雙目視覺相機的雙目視覺數(shù)據(jù)；
4、根據(jù)所述rtk數(shù)據(jù)確定rtk定位結(jié)果和rtk均方根誤差，根據(jù)所述imu數(shù)據(jù)確定imu定位結(jié)果和imu標定參數(shù)，根據(jù)所述雙目視覺數(shù)據(jù)確定視覺slam定位結(jié)果和視覺slam精度評估值，以及根據(jù)所述激光雷達數(shù)據(jù)確定激光slam定位結(jié)果和激光slam精度評估值；
5、將所述rtk均方根誤差、所述imu標定參數(shù)、所述視覺slam精度評估值和所述激光slam精度評估值作為先驗信息，確定各類觀測值的初始權(quán)；根據(jù)各類觀測值的初始權(quán)、所述rtk定位結(jié)果、所述imu定位結(jié)果、所述視覺slam定位結(jié)果和所述激光slam定位結(jié)果確定各類觀測值的首次平差解算；根據(jù)各類觀測值的首次平差解算進行迭代計算各類觀測值的單位權(quán)方差的估計值，在各類觀測值的單位權(quán)方差的估計值滿足預(yù)設(shè)迭代條件的情況下，根據(jù)當次迭代計算得到的各類觀測值的單位權(quán)方差的估計值確定綜合融合定位結(jié)果。
7、從所述雙目視覺數(shù)據(jù)提取雙目視覺圖像，所述雙目視覺圖像包括左目圖像和右目圖像；
8、基于所述左目圖像與所述右目圖像進行匹配，得到所述左目圖像與所述右目圖像之間的多對特征點；
9、根據(jù)所述左目圖像與所述右目圖像之間的多對特征點的像素點坐標計算單應(yīng)性矩陣；
12、可選地，所述基于所述左目圖像與所述右目圖像進行匹配，得到所述左目圖像與所述右目圖像之間的多對特征點，包括：
15、對去噪后的灰度左目圖像進行角點提取，得到去噪后的灰度左目圖像的多個候選特征點；
16、在去噪后的灰度左目圖像的第n個候選特征點周圍選取一組采樣點對，計算每個采樣點對的灰度差分值，將所有采樣點對的灰度差分值轉(zhuǎn)換為二進制串，并將二進制串作為去噪后的灰度左目圖像的第n個候選特征點的brief描述子；重復(fù)上述步驟，n依次取大于0的整數(shù)，得到去噪后的灰度左目圖像的所有候選特征點的brief描述子；
19、對去噪后的灰度右目圖像進行角點提取，得到去噪后的灰度右目圖像的多個候選特征點；
20、在去噪后的灰度右目圖像的第m個候選特征點周圍選取一組采樣點對，計算每個采樣點對的灰度差分值，將所有采樣點對的灰度差分值轉(zhuǎn)換為二進制串，并將二進制串作為去噪后的灰度右目圖像的第m個候選特征點的brief描述子；重復(fù)上述步驟，m依次取大于0的整數(shù)，得到去噪后的灰度右目圖像的所有候選特征點的brief描述子；
21、根據(jù)去噪后的灰度左目圖像的所有候選特征點的brief描述子和去噪后的灰度右目圖像的所有候選特征點的brief描述子，對去噪后的灰度左目圖像的多個候選特征點與去噪后的灰度右目圖像的多個候選特征點進行特征點匹配，得到匹配后的特征點對；
22、將匹配后的特征點對進行篩選，得到去噪后的灰度左目圖像與去噪后的灰度右目圖像之間的多對特征點；
23、將去噪后的灰度左目圖像與去噪后的灰度右目圖像之間的多對特征點作為所述左目圖像與所述右目圖像之間的多對特征點。
25、根據(jù)所述左目圖像與所述右目圖像之間的多對特征點的像素點坐標計算多對特征點的多個視差值；
26、根據(jù)多對特征點的多個視差值、所述雙目視覺相機的在x軸方向的焦距和所述雙目視覺相機的基線長度計算多對特征點對應(yīng)的多個深度信息；
27、根據(jù)多對特征點對應(yīng)的多個深度信息、所述旋轉(zhuǎn)矩陣、所述平移向量、所述雙目視覺相機的內(nèi)參數(shù)矩陣和所述左目圖像的多個特征點的像素點坐標計算所述左目圖像的多個特征點在世界坐標系的世界坐標；
28、根據(jù)所述左目圖像中第i個圖像幀的時間間隔、運動距離和視角變化與時間間隔閾值、運動距離閾值和視角變化閾值判斷第i個圖像幀是否為關(guān)鍵幀；
29、若第i個圖像幀為關(guān)鍵幀，則在所述第i個圖像幀的特征點的重投影誤差小于誤差閾值，且該特征點被兩個以上的其他關(guān)鍵幀對應(yīng)的圖像幀觀測到的情況下，根據(jù)該特征點的世界坐標將該特征點和關(guān)鍵幀更新到slam地圖中；
31、重復(fù)上述步驟，i依次取大于0的整數(shù)，直至i等于所述左目圖像中圖像幀的總個數(shù)；
33、可選地，所述根據(jù)最新優(yōu)化后的slam地圖計算所述視覺slam精度評估值，包括：
34、在最新優(yōu)化后的slam地圖中均勻選取在所述左目圖像有對應(yīng)特征點的j個地圖點；
35、根據(jù)最新優(yōu)化后的slam地圖的第j個地圖點的地圖坐標、第j個地圖點在所述左目圖像對應(yīng)的特征點的像素點坐標、所述旋轉(zhuǎn)矩陣、所述平移向量和所述雙目視覺相機的內(nèi)參數(shù)矩陣計算第j個地圖點到所述左目圖像的第一深度值；
36、根據(jù)第j個地圖點在所述左目圖像對應(yīng)的特征點的像素點坐標和所述單應(yīng)性矩陣計算第j個地圖點在所述右目圖像對應(yīng)的特征點的像素點坐標；
37、根據(jù)最新優(yōu)化后的slam地圖的第j個地圖點的地圖坐標、第j個地圖點在所述右目圖像對應(yīng)的特征點的像素點坐標、所述旋轉(zhuǎn)矩陣、所述平移向量和所述雙目視覺相機的內(nèi)參數(shù)矩陣計算第j個地圖點到所述右目圖像的第二深度值；
40、重復(fù)上述步驟，j依次取大于0的整數(shù)，得到所有地圖點的深度差值和深度權(quán)重；
41、根據(jù)所有地圖點的深度差值和深度權(quán)重計算第一方差累加值，以及根據(jù)所有地圖點的深度權(quán)重計算深度權(quán)重和值；
42、根據(jù)所述第一方差累加值和所述深度權(quán)重和值計算所述視覺slam精度評估值。
43、可選地，所述激光雷達數(shù)據(jù)包括相鄰兩次掃描幀激光雷達數(shù)據(jù)，根據(jù)所述激光雷達數(shù)據(jù)確定激光slam定位結(jié)果，包括：
44、基于相鄰兩次掃描幀激光雷達數(shù)據(jù)中的點云數(shù)據(jù)進行匹配，得到相鄰兩次掃描幀點云數(shù)據(jù)之間的多對特征點云；
45、基于構(gòu)建非線性優(yōu)化函數(shù)，使用非線性優(yōu)化的列文伯格-馬夸爾特方法，根據(jù)相鄰兩次掃描幀點云數(shù)據(jù)之間的多對特征點云進行迭代計算當前狀態(tài)轉(zhuǎn)移矩陣；
47、可選地，所述第一次掃描幀激光雷達數(shù)據(jù)包括第一次掃描幀點云數(shù)據(jù)和第一時間戳信息，所述第二次掃描幀激光雷達數(shù)據(jù)包括第二次掃描幀點云數(shù)據(jù)和第二時間戳信息，所述基于相鄰兩次掃描幀激光雷達數(shù)據(jù)中的點云數(shù)據(jù)進行匹配，得到相鄰兩次掃描幀點云數(shù)據(jù)之間的多對特征點云，包括：
48、根據(jù)所述第一時間戳信息對所述第一次掃描幀點云數(shù)據(jù)進行運動補償?shù)玫窖a償后的第一次掃描幀點云數(shù)據(jù)；
49、計算補償后的第一次掃描幀點云數(shù)據(jù)中每個點云的曲率，并將補償后的第一次掃描幀點云數(shù)據(jù)中每個時刻的點云數(shù)據(jù)劃分為多個相同的子區(qū)域；
50、在每個子區(qū)域中，將曲率最高的多個點云均作為邊緣特征點，將曲率最低的多個點云均作為平面特征點，得到補償后的第一次掃描幀點云數(shù)據(jù)的多個邊緣特征點和多個平面特征點；
51、根據(jù)所述第二時間戳信息對所述第二次掃描幀點云數(shù)據(jù)進行運動補償?shù)玫窖a償后的第二次掃描幀點云數(shù)據(jù)；
52、計算補償后的第二次掃描幀點云數(shù)據(jù)中每個點云的曲率，并將補償后的第二次掃描幀點云數(shù)據(jù)中每個時刻的點云數(shù)據(jù)劃分為多個相同的子區(qū)域；
53、在每個子區(qū)域中，將曲率最高的多個點云均作為邊緣特征點，將曲率最低的多個點云均作為平面特征點，得到補償后的第二次掃描幀點云數(shù)據(jù)的多個邊緣特征點和多個平面特征點；
54、根據(jù)補償后的第一次掃描幀點云數(shù)據(jù)的多個邊緣特征點與補償后的第二次掃描幀點云數(shù)據(jù)的多個邊緣特征點進行邊緣點匹配，得到匹配后的邊緣特征點對；以及根據(jù)補償后的第一次掃描幀點云數(shù)據(jù)的多個平面特征點與補償后的第二次掃描幀點云數(shù)據(jù)的多個平面特征點進行平面點匹配，得到匹配后的平面特征點對；
55、將匹配后的邊緣特征點對進行篩選，得到補償后的第一次掃描幀點云數(shù)據(jù)與補償后的第二次掃描幀點云數(shù)據(jù)之間的多對邊緣特征點；以及將匹配后的平面特征點對進行篩選，得到補償后的第一次掃描幀點云數(shù)據(jù)與補償后的第二次掃描幀點云數(shù)據(jù)之間的多對平面特征點；
56、將補償后的第一次掃描幀點云數(shù)據(jù)與補償后的第二次掃描幀點云數(shù)據(jù)之間的多對邊緣特征點和多對平面特征點作為相鄰兩次掃描幀點云數(shù)據(jù)之間的多對特征點云。
58、根據(jù)所述當前狀態(tài)轉(zhuǎn)移矩陣和第一次掃描幀點云數(shù)據(jù)的多個特征點云的點云坐標，將第一次掃描幀點云數(shù)據(jù)的多個特征點云更新到slam地圖中；
60、可選地，所述根據(jù)更新后的slam地圖計算所述激光slam精度評估值，包括：
61、在更新后的slam地圖中均勻選取在第一次幀點云數(shù)據(jù)有對應(yīng)特征點云的k個地圖點；
62、根據(jù)更新后的slam地圖的第k個地圖點的地圖坐標和在第一次幀點云數(shù)據(jù)對應(yīng)的特征點云的點云坐標計算第一激光距離值；
64、根據(jù)所述第一激光距離值和所述第二激光距離值計算第k個地圖點的距離差值；
65、根據(jù)所述第一激光距離值和所述第二激光距離值計算第k個地圖點的距離差值；
67、重復(fù)上述步驟，k依次取大于0的整數(shù)，得到所有地圖點的距離差值和距離權(quán)重；
68、根據(jù)所有地圖點的距離差值和距離權(quán)重計算第二方差累加值，以及根據(jù)所有地圖點的距離權(quán)重計算距離權(quán)重和值；
69、根據(jù)所述第二方差累加值和所述距離權(quán)重和值計算所述激光slam精度評估值。
70、可選地，所述在各類觀測值的單位權(quán)方差的估計值滿足預(yù)設(shè)迭代條件的情況下，根據(jù)當次迭代計算得到的各類觀測值的單位權(quán)方差的估計值確定綜合融合定位結(jié)果，包括：
71、在迭代計算的過程中，若兩兩類觀測值的單位權(quán)方差的估計值之間的估計差值均小于預(yù)設(shè)閾值，則根據(jù)當次迭代計算得到的各類觀測值的單位權(quán)方差的估計值確定各類觀測值的目標權(quán)；
73、為實現(xiàn)上述目的，本發(fā)明在第二方面提供一種多傳感器融合定位裝置，所述裝置包括：
74、獲取模塊，用于獲取rtk的rtk數(shù)據(jù)、imu的imu數(shù)據(jù)、激光雷達的激光雷達數(shù)據(jù)和雙目視覺相機的雙目視覺數(shù)據(jù)；
75、確定模塊，用于根據(jù)所述rtk數(shù)據(jù)確定rtk定位結(jié)果和rtk均方根誤差，根據(jù)所述imu數(shù)據(jù)確定imu定位結(jié)果和imu標定參數(shù)，根據(jù)所述雙目視覺數(shù)據(jù)確定視覺slam定位結(jié)果和視覺slam精度評估值，以及根據(jù)所述激光雷達數(shù)據(jù)確定激光slam定位結(jié)果和激光slam精度評估值；
76、融合定位模塊，用于將所述rtk均方根誤差、所述imu標定參數(shù)、所述視覺slam精度評估值和所述激光slam精度評估值作為先驗信息，確定各類觀測值的初始權(quán)；根據(jù)各類觀測值的初始權(quán)、所述rtk定位結(jié)果、所述imu定位結(jié)果、所述視覺slam定位結(jié)果和所述激光slam定位結(jié)果確定各類觀測值的首次平差解算；根據(jù)各類觀測值的首次平差解算進行迭代計算各類觀測值的單位權(quán)方差的估計值，在各類觀測值的單位權(quán)方差的估計值滿足預(yù)設(shè)迭代條件的情況下，根據(jù)當次迭代計算得到的各類觀測值的單位權(quán)方差的估計值確定綜合融合定位結(jié)果。
77、為實現(xiàn)上述目的，本發(fā)明在第三方面提供一種計算機可讀存儲介質(zhì)，存儲有計算機程序，所述計算機程序被處理器執(zhí)行時，使得所述處理器執(zhí)行如第一方面中任一項所述的方法。
78、為實現(xiàn)上述目的，本發(fā)明在第四方面提供一種計算機設(shè)備，包括存儲器和處理器，所述存儲器存儲有計算機程序，所述計算機程序被所述處理器執(zhí)行時，使得所述處理器執(zhí)行如第一方面中任一項所述的方法。
79、采用本發(fā)明實施例，具有如下有益效果：上述方法通過獲取rtk的rtk數(shù)據(jù)、imu的imu數(shù)據(jù)、激光雷達的激光雷達數(shù)據(jù)和雙目視覺相機的雙目視覺數(shù)據(jù)；根據(jù)rtk數(shù)據(jù)確定rtk定位結(jié)果和rtk均方根誤差，根據(jù)imu數(shù)據(jù)確定imu定位結(jié)果和imu標定參數(shù)，根據(jù)雙目視覺數(shù)據(jù)確定視覺slam定位結(jié)果和視覺slam精度評估值，以及根據(jù)激光雷達數(shù)據(jù)確定激光slam定位結(jié)果和激光slam精度評估值；將rtk均方根誤差、imu標定參數(shù)、視覺slam精度評估值和激光slam精度評估值作為先驗信息，確定各類觀測值的初始權(quán)；根據(jù)各類觀測值的初始權(quán)、rtk定位結(jié)果、imu定位結(jié)果、視覺slam定位結(jié)果和激光slam定位結(jié)果確定各類觀測值的首次平差解算；根據(jù)各類觀測值的首次平差解算進行迭代計算各類觀測值的單位權(quán)方差的估計值，在各類觀測值的單位權(quán)方差的估計值滿足預(yù)設(shè)迭代條件的情況下，根據(jù)當次迭代計算得到的各類觀測值的單位權(quán)方差的估計值確定綜合融合定位結(jié)果。即通過將rtk、imu、激光雷達和雙目視覺相機等多種傳感器的數(shù)據(jù)進行上述方式的融合定位，從而得到高精度的綜合融合定位結(jié)果，該高精度的綜合融合定位結(jié)果可以提高整個傳感器系統(tǒng)的有效性和正確性。
