(19)國家知識產(chǎn)權(quán)局(12)發(fā)明專利地址710072陜西省西安市友誼西路127號陳霖有限公司61291(56)對比文件KunLi.《UWB-VO:Ultra-Wide權(quán)利要求書4頁說明書10頁附圖1頁統(tǒng)(57)摘要的定位方法及系統(tǒng)，將可見光相機與UWB傳感器UWB錨點；首先通過定位設(shè)備距離信息和圖像信息，構(gòu)建UWB錨點距離與圖像信息組成的信息幀；對圖像數(shù)據(jù)進行視覺初始化，并建立UWB初始化隊列，對后續(xù)采集的信息幀進行跟蹤，且對UWB初信息幀位姿，并判斷新的信息幀是否是關(guān)鍵幀；合優(yōu)化方程，實現(xiàn)關(guān)鍵幀位姿、地圖點，錨點位置及縮放因子優(yōu)化。本發(fā)明能夠僅依賴一個UWB圖像幀生成信息幀(BA優(yōu)化)UB初特征點與地圖點匹配地圖數(shù)據(jù)關(guān)鍵幀生成樹回環(huán)檢測重定位模型數(shù)據(jù)集是單應(yīng)稅覺初視覺初UWB測距信息特征提取2步驟1:在環(huán)境內(nèi)任意位置放置一個或多個UWB錨點，將為定位設(shè)備，將定位設(shè)備固定安裝在移動設(shè)備上；過定位設(shè)備上的相機采集環(huán)境圖像信息，同時實現(xiàn)環(huán)境圖像信息與距離信息的時間對齊，以圖像幀為基礎(chǔ)，構(gòu)建UWB錨點距離與圖像信息組成的信息幀；步驟3:對采集的信息幀中的圖像數(shù)據(jù)進行視覺初始化，估計連續(xù)兩個信息幀的相對位姿及其共同觀測到的地圖點的位置；步驟4:完成視覺初始化后，設(shè)置第一個視覺初始化幀的位姿為系統(tǒng)的坐標(biāo)系；建立UWB初始化隊列，并對后續(xù)新采集的信息幀進行跟蹤，包括建立后續(xù)信息幀中圖像特征點與地圖點的匹配關(guān)系，基于重投影誤差建立視覺跟蹤優(yōu)化方程，計算后續(xù)幀的位姿，判斷是否將新采集的信息幀加入UWB初始化隊列；步驟5.1:當(dāng)UWB初始化幀序列內(nèi)信息幀的數(shù)目大于設(shè)定的UWB錨點初始化閾值時，對距離與信息幀中的UWB測量距離之差，其中的計算距離指根據(jù)信息幀估計的位姿計算的定間的縮放因子；步驟5.2:使用步驟5.1得到的縮放因子，利用系統(tǒng)內(nèi)存儲的信息幀位姿及地圖點的位置乘以縮放因子，從而將局部地圖內(nèi)關(guān)鍵幀及地圖點恢復(fù)到公制尺度；步驟6:對于后續(xù)采集到的新的信息幀，建立所述新的信息幀中圖像特征點與地圖點的匹配關(guān)系，并根據(jù)匹配關(guān)系建立特征點的重投影誤差，同時利用所述新的信息幀中的測量通過求解聯(lián)合優(yōu)化方程，得到新的信息幀的位姿；并判斷所述新的信息幀是否是關(guān)鍵幀：計算所述新的信息幀與其參考關(guān)鍵幀的相對位姿差值、UWB測量距離差值以及采集時間差值，若任一差值大于設(shè)定的對應(yīng)閾值，則將所述新的信息幀設(shè)置為關(guān)鍵幀，并將所述新的信息幀及其觀測到的地圖點加入局部地圖及全局地圖中；步驟7:采用新的關(guān)鍵幀加入局部地圖的關(guān)鍵幀隊列后，建立局部地圖聯(lián)合優(yōu)化方程，通過求解聯(lián)合優(yōu)化方程，優(yōu)化關(guān)鍵幀隊列內(nèi)的關(guān)鍵幀位姿、局部地圖內(nèi)的地圖點，UWB錨點步驟8:對新加入全局地圖的關(guān)鍵幀執(zhí)行回環(huán)檢測線程，當(dāng)檢測到新加入全局地圖的關(guān)鍵幀與全局地圖內(nèi)的關(guān)鍵幀相匹配時，建立匹配的關(guān)鍵幀之間的視覺與UWB信息緊耦合的優(yōu)化方程，進行重定位，消除移動設(shè)備運行過程中產(chǎn)生的位姿估計漂移量。2.根據(jù)權(quán)利要求1所述一種UWB與視覺緊耦合SLAM算法的定位方法，其特征在于：步驟2中，根據(jù)圖像幀前后的UWB測量幀，采用線性插值的方法得到圖像幀對應(yīng)時刻的距離測量值，實現(xiàn)測距信息與圖像信息時間對齊。3.根據(jù)權(quán)利要求2所述一種UWB與視覺緊耦合SLAM算法的定位方法，其特征在于：實現(xiàn)測距信息與圖像信息時間對齊的過程為：以相機接收到的第二幀圖像幀的時間為系統(tǒng)初始時間T?,以第一幀圖像幀的前后兩個3保存第K-1幀圖像幀與第K+1幀圖像幀之間采集時間區(qū)間內(nèi)采集到的UWB測距信息，檢步驟3.1:對采集的連續(xù)兩個信息幀中的圖像數(shù)據(jù)，分別提取其中的特征點，并對提取步驟3.2:根據(jù)步驟3.1作為初始化幀的兩個信息幀，分別計算信息幀間的基礎(chǔ)矩陣及步驟3.3:根據(jù)地圖點位置及信息幀位姿計算信息幀特步驟4.2.1:將最新采集的信息幀設(shè)置為當(dāng)前幀，最新創(chuàng)建的關(guān)鍵幀為當(dāng)前幀的參考關(guān)4如果運動模型有效，則建立局部地圖點與當(dāng)前幀中如果運動模型失效，則進行當(dāng)前幀中圖像特征點與其參考關(guān)鍵幀中的圖像特征點匹根據(jù)所述新的信息幀中的UWB測量距離，以及基于勻速運動模型所估計位姿而得到的如果所述判斷公式得到滿足，表示可用勻速運動模型進行跟蹤粗估計，則基于所述新的信息幀的前兩幀的相對位姿變化初步估計所述新的信息幀的位投影到所述新的信息幀的圖像中，建立地圖點與所述新的信息幀中圖像特征點的匹配關(guān)如果所述判斷公式不能得到滿足，表示不可用勻速運動模型進行跟蹤粗估計，則建立將重投影誤差與UWB距離誤差加權(quán)求和，從而建立所述新的信息幀位姿聯(lián)合優(yōu)化方程步驟7.1:將加入關(guān)鍵幀隊列的關(guān)鍵幀的共視關(guān)鍵幀作為優(yōu)化關(guān)鍵幀加入優(yōu)化幀序列，5將優(yōu)化幀序列內(nèi)中的優(yōu)化關(guān)鍵幀觀測到的地圖點設(shè)置為優(yōu)化地圖點，建立優(yōu)化關(guān)鍵幀與優(yōu)化地圖點間的重投影誤差；步驟7.2:當(dāng)優(yōu)化幀序列中的優(yōu)化關(guān)鍵幀數(shù)量大于設(shè)定值時，建立優(yōu)化關(guān)鍵幀與UWB錨點的距離誤差，即根據(jù)優(yōu)化關(guān)鍵幀位姿計算與UWB錨點距離，并得到計算距離與優(yōu)化關(guān)鍵幀中的測量距離的距離誤差；步驟7.3:對重投影誤差與距離誤差加權(quán)求和，建立局部優(yōu)化聯(lián)合優(yōu)化方程，優(yōu)化局部的具體過程為：步驟8.1:在全局地圖內(nèi)使用詞袋模型檢索新加入全局地圖的關(guān)鍵幀的回環(huán)幀，當(dāng)檢索到回環(huán)幀后，對回環(huán)幀的共視幀與新加入全局地圖的關(guān)鍵幀的共視幀進行匹配，根據(jù)匹配相似度是否大于設(shè)定閾值判定該回環(huán)是否有效；步驟8.2:當(dāng)回環(huán)有效，則計算有效回環(huán)的兩幀之間的相似變換優(yōu)化方程，求解得到相似變換矩陣，將新加入全局地圖的關(guān)鍵幀位姿修正；并且建立并求解回環(huán)間關(guān)鍵幀的全局優(yōu)化方程，以修正回環(huán)間關(guān)鍵幀位姿，且若求解全局優(yōu)化方程時得到的相似變換矩陣尺度因子大于設(shè)定閾值時，進行UWB錨點及系統(tǒng)尺度與公制單位間縮放因子優(yōu)化。10.一種基于權(quán)利要求1～9任一所述方法的移動設(shè)備定位系統(tǒng)，其特征在于：包括：信息幀生成模塊，對實時獲取的UWB測距信息與采集圖像信息進行時間同步后，生成包含圖像信息與距離信息的信息幀；系統(tǒng)初始化模塊，對生成的信息幀，先進行基于圖像的視覺初始化，完成視覺初始化后系統(tǒng)篩選滿足系統(tǒng)初始化的具有視覺位姿的信息幀，利用篩選的系統(tǒng)初始化信息幀進行系統(tǒng)初始化，估計錨點位置及系統(tǒng)尺度與公制尺度間的縮放因子；跟蹤線程，完成系統(tǒng)初始化后，系統(tǒng)根據(jù)當(dāng)前幀特征點與系統(tǒng)內(nèi)存在地圖點的關(guān)系建立聯(lián)合優(yōu)化方程，估計當(dāng)前幀的位姿；系統(tǒng)根據(jù)當(dāng)前幀與其參考關(guān)鍵幀位姿、UWB距離及時間差距判定是否將當(dāng)前幀設(shè)置為關(guān)鍵幀；局部優(yōu)化線程，信息幀跟蹤過程中，將設(shè)置的關(guān)鍵幀加入局部優(yōu)化線程的滑動窗口中，新的關(guān)鍵幀加入滑動窗口后進行局部優(yōu)化，優(yōu)化局部滑動窗口內(nèi)的關(guān)鍵幀位姿及地圖點位回環(huán)檢測線程，對新加入全局地圖的關(guān)鍵幀采用詞袋模型匹配回環(huán)幀，匹配到回環(huán)關(guān)鍵幀后，根據(jù)共視關(guān)系建立回環(huán)幀的生成樹，對生成樹與其關(guān)鍵幀建立全局優(yōu)化方程，優(yōu)化生成樹內(nèi)關(guān)鍵幀位姿、觀測到的地圖點位置、系統(tǒng)尺度與公制尺度的縮放因子及UWB錨點位6UWB與視覺緊耦合SLAM算法的定位方法及系統(tǒng)技術(shù)領(lǐng)域法的定位方法及系統(tǒng)。背景技術(shù)[0002]近年來，移動設(shè)備的自主定位方法成為移動機器人技術(shù)研究的重點與難點。SLAM技術(shù)因其不依賴外部設(shè)備，自主性強的特點成為移動機器人自主定位的主要方案。[0003]可見光相機因其傳感器廉價、體積小、質(zhì)量輕等特點成為SLAM方案的主要傳感器。單目視覺SLAM通過相機獲取外界環(huán)境信息，對外界環(huán)境信息處理能夠?qū)崟r估計移動設(shè)備的位姿并建立外界環(huán)境的稀疏點云地圖。單目視覺SLAM系統(tǒng)因其初始化時求解齊次線性方程[0004]超寬帶(UltraWideBand,UWB)定位技術(shù)具有厘米級定位精度的無線通信技術(shù)，其信號具有極強的穿透力，極高的時域分辨率等特性，結(jié)合一些定位算法成為目前室內(nèi)定位應(yīng)用最廣泛的定位技術(shù)之一?，F(xiàn)有的UWB定位技術(shù)需要放置位置確定的至少3個固定錨點才能實現(xiàn)3維空間的定位，限制了該方案在未知環(huán)境的使用。個已知位置的UWB錨點，實現(xiàn)UWB設(shè)備的定位，僅利用UWB定位信息進行重定位，未使用U位信息與視覺信息耦合。顏成鋼提出的基于融合UWB的視覺SLAM的室內(nèi)定位與導(dǎo)航的方法，同樣采用3個UWB錨點進行UWB設(shè)備定位，使用UWB信息輔助重定位。當(dāng)前方案僅使用多個已限制了定位方案的實現(xiàn)。發(fā)明內(nèi)容[0006]為解決現(xiàn)有技術(shù)存在的問題，本發(fā)明提出一種UWB與視覺緊耦合SLAM算法的定位方法及系統(tǒng)，通過將可見光相機與UWB傳感器剛性連接得到的定位設(shè)備，并在空間任意位置度。[0007]本發(fā)明的技術(shù)方案為：組合為定位設(shè)備，將定位設(shè)備固定安裝在移動設(shè)備上；[0010]步驟2:通過定位設(shè)備上的UWB傳感器實時采集定位設(shè)備與UWB錨點間的距離信息，并通過定位設(shè)備上的相機采集環(huán)境圖像信息，同時實現(xiàn)環(huán)境圖像信息與距離信息的時間對齊，以圖像幀為基礎(chǔ)，構(gòu)建UWB錨點距離與圖像信息組成的信息幀；7對UWB初始化幀隊列內(nèi)的信息幀建立基于UWB距離誤差的優(yōu)化方程，所述UWB距離誤差指計算距離與信息幀中的UWB測量距離之差，其中的計算距離指根據(jù)信息幀估計的位姿計算的[0015]步驟5.2:使用步驟5.1得到的縮放因子，利用系統(tǒng)內(nèi)存儲的信息幀位姿及地圖點[0017]計算所述新的信息幀與其參考關(guān)鍵幀的相對位姿差值、UWB測量距離差值以及采的關(guān)鍵幀與全局地圖內(nèi)的關(guān)鍵幀相匹配時，建立匹配的關(guān)鍵幀之間的視覺與UWB信息緊耦[0022]以相機接收到的第二幀圖像幀的時間為系統(tǒng)初始時間T?,以第一幀圖像幀的前后兩個UWB測量距離幀的時間戳均值為UWB測量距離初始幀的時刻：8[0028]保存第K-1幀圖像幀與第K+1幀圖像幀之間采集時間區(qū)間內(nèi)采集到的[0035]步驟3.2:根據(jù)步驟3.1作為初始化幀的兩個信息幀，分別計算信息幀間的基礎(chǔ)矩陣及單應(yīng)矩陣，并分別根據(jù)基礎(chǔ)矩陣以及單應(yīng)矩陣恢復(fù)信前幀中圖像特征點的重投影誤差建立光束平差9[0046]根據(jù)所述新的信息幀中的UWB測量距離，以及基于勻速運動模型所估計位姿而得[0050]將重投影誤差與UWB距離誤差加權(quán)求和，從而建立所述新的信息幀位姿聯(lián)合優(yōu)化[0054]步驟7.1:將加入關(guān)鍵幀隊列的關(guān)鍵幀的共視關(guān)鍵幀作為優(yōu)化關(guān)鍵幀加入優(yōu)化幀[0055]步驟7.2:當(dāng)優(yōu)化幀序列中的優(yōu)化關(guān)鍵幀數(shù)量大于設(shè)定值時，建立優(yōu)化關(guān)鍵幀與[0059]步驟8.2:當(dāng)回環(huán)有效，則計算有效回環(huán)的兩幀之間的相似變換優(yōu)化方程，求解得到相似變換矩陣，將新加入全局地圖的關(guān)鍵幀位姿修正；并且建立并求解回環(huán)間關(guān)鍵幀的全局優(yōu)化方程，以修正回環(huán)間關(guān)鍵幀位姿，且若求解全局優(yōu)化方程時得到的相似變換矩陣尺度因子大于設(shè)定閾值時，進行UWB錨點及系統(tǒng)尺度與公制單位間縮放因子優(yōu)化。[0062]信息幀生成模塊，對實時獲取的UWB測距信息與采集圖像信息進行時間同步后，生成包含圖像信息與距離信息的信息幀；[0063]系統(tǒng)初始化模塊，對生成的信息幀，先進行基于圖像的視覺初始化，完成視覺初始化后系統(tǒng)篩選滿足系統(tǒng)初始化的具有視覺位姿的信息幀，利用篩選的系統(tǒng)初始化信息幀進行系統(tǒng)初始化，估計錨點位置及系統(tǒng)尺度與公制尺度間的縮放因子；[0064]跟蹤線程，完成系統(tǒng)初始化后，系統(tǒng)根據(jù)當(dāng)前幀特征點與系統(tǒng)內(nèi)存在地圖點的關(guān)系建立聯(lián)合優(yōu)化方程，估計當(dāng)前幀的位姿。系統(tǒng)根據(jù)當(dāng)前幀與其參考關(guān)鍵幀位姿、UWB距離及時間差距判定是否將當(dāng)前幀設(shè)置為關(guān)鍵幀；[0065]局部優(yōu)化線程，信息幀跟蹤過程中，將設(shè)置的關(guān)鍵幀加入局部優(yōu)化線程的滑動窗口中，新的關(guān)鍵幀加入滑動窗口后進行局部優(yōu)化，優(yōu)化局部滑動窗口內(nèi)的關(guān)鍵幀位姿及地圖點位置；[0066]回環(huán)檢測線程，對新加入全局地圖的關(guān)鍵幀采用詞袋模型匹配回環(huán)幀，匹配到回環(huán)關(guān)鍵幀后，根據(jù)共視關(guān)系建立回環(huán)幀的生成樹，對生成樹與其關(guān)鍵幀建立全局優(yōu)化方程，優(yōu)化生成樹內(nèi)關(guān)鍵幀位姿、觀測到的地圖點位置、系統(tǒng)尺度與公制尺度的縮放點位置。[0067]有益效果[0069]本發(fā)明的方法僅依賴一個UWB錨點即可實現(xiàn)移動設(shè)備的高精度定位信息，且確保定位信息具有絕對的尺度信息。[0070]本發(fā)明采用UWB與視覺緊耦合SLAM方法實現(xiàn)估計移動設(shè)備的定位信息，僅在外界點位置進行了準(zhǔn)確估計，并可實現(xiàn)環(huán)境中未知點的定位。[0072]本發(fā)明的附加方面和優(yōu)點將在下面的描述中部分給出，部分將從下面的描述中變得明顯，或通過本發(fā)明的實踐了解到。附圖說明[0073]本發(fā)明的上述和/或附加的方面和優(yōu)點從結(jié)合下面附圖對實施例的描述中將變得[0078]步驟2:定位設(shè)備接收到不同頻率的UWB距離測量信息及外界環(huán)境的可見光圖像信[0080]這里根據(jù)圖像幀前后的UWB測量幀，采用線性插值的方法得到圖像幀對應(yīng)時刻的[0087]保存第K-1幀圖像幀與第K+1幀圖像幀之間采集時間區(qū)間采集到的UWB測距信息，提取的特幀點進行匹配，若匹配數(shù)目大于設(shè)定的初始化閾值時，設(shè)置當(dāng)前兩個信息幀為初始化幀；[0094]步驟3.2:根據(jù)步驟3.1作為初始化幀的兩個信息幀，分別計算信息幀間的基礎(chǔ)矩陣及單應(yīng)矩陣，并分別根據(jù)基礎(chǔ)矩陣以及單應(yīng)矩陣恢復(fù)信息幀的位姿，且根據(jù)恢復(fù)的信息幀位姿將信息幀內(nèi)圖像中匹配的特征點對進行三角化，得到特征點對所對應(yīng)的地圖點的位[0095]步驟3.3:根據(jù)地圖點位置及信息幀位姿計算信息幀特征點的重投影誤差，將重投影誤差的小的位姿與地圖點作為初始化幀的位姿及地圖點位置。[0096]步驟4:完成視覺初始化后，設(shè)置第一個視覺初始化幀的位姿為系統(tǒng)的坐標(biāo)系；建立UWB初始化隊列，并對后續(xù)新采集的信息幀進行跟蹤，包括建立后續(xù)信息幀中圖像特征點與地圖點的匹配關(guān)系，基于重投影誤差建立視覺跟蹤優(yōu)化方程，計算后續(xù)幀的位姿，判斷是[0097]由于UWB錨點位置初始化需要至少3個不共面的信息幀建立基于距離誤差的優(yōu)化擇相對位姿變化較大的幀為UWB初始化幀。因此計算連續(xù)信息幀間距離測量值的差值，當(dāng)差值大于設(shè)定的UWB初始化幀選擇閾值時，設(shè)定這一信息幀為UWB初始化幀，并將其存儲在UWB[0098]步驟4.1:將兩個視覺初始化幀加入UWB初始化幀隊列中，并設(shè)置這兩個視覺初始化幀為關(guān)鍵幀，將關(guān)鍵幀加入局部地圖與全局地圖中。[0099]步驟4.2:對后續(xù)新采集的信息幀進行跟蹤：[0100]步驟4.2.1:將最新采集的信息幀設(shè)置為當(dāng)前幀，最新創(chuàng)建的關(guān)鍵幀為當(dāng)前幀的參考關(guān)鍵幀，將當(dāng)前幀之前兩幀的位姿變換設(shè)置為系統(tǒng)運動模型中的相對位姿變換；在系統(tǒng)接收到當(dāng)前幀后，使用系統(tǒng)運動模型估計當(dāng)前幀的位姿，并根據(jù)估計位姿計算與UWB錨點距離，如果計算距離與當(dāng)前幀中的UWB測量距離之差小于設(shè)定閾值，則表示系統(tǒng)運動模型有[0101]如果運動模型有效，則建立局部地圖點與當(dāng)前幀中圖像特征點的聯(lián)系，并基于當(dāng)前幀中圖像特征點的重投影誤差建立光束平差方程求解當(dāng)前幀的位姿；[0102]如果運動模型失效，則進行當(dāng)前幀中圖像特征點與其參考關(guān)鍵幀中的圖像特征點匹配，并根據(jù)匹配關(guān)系建立當(dāng)前幀中圖像特征點與地圖點的對應(yīng)關(guān)系，并計算特征點的重投影誤差，建立光束平差方程求解得到當(dāng)前幀的位姿。[0103]步驟4.2.2:分別計算當(dāng)前幀與上一信息幀的相對位姿、UWB測量距離差值以及當(dāng)前幀與其參考關(guān)鍵幀的時間差值，如果相對位姿差異或者任一差值大于對應(yīng)的閾值，則將當(dāng)前幀設(shè)置為關(guān)鍵幀并將其存儲到局部地圖與全局地圖內(nèi)；[0104]步驟4.2.3:計算當(dāng)前幀的UWB錨點測距信息與UWB