[發(fā)明專利]單目視覺和慣性傳感器融合的遠(yuǎn)距離測距系統(tǒng)及方法

在審

• 申請?zhí)枺?/td>: 201910916404.8
• 申請日: 2019-09-26
• 公開（公告）號: CN110617814A
• 公開（公告）日: 2019-12-27
• 發(fā)明（設(shè)計）人: 鄒旭東;肖麟慧;李志天;楊伍昊;熊興崟;劉云飛
• 申請（專利權(quán)）人: 中國科學(xué)院電子學(xué)研究所;中國科學(xué)院大學(xué)
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C21/20;G01C3/00
• 代理公司: 11021 中科專利商標(biāo)代理有限責(zé)任公司
• 代理人: 吳夢圓

1.一種單目視覺和慣性傳感器融合的遠(yuǎn)距離測距系統(tǒng)，包括：

單目視覺SLAM模塊，包括單目相機，用于采集視覺圖像數(shù)據(jù)；

IMU模塊，用于獲得IMU模塊的IMU位姿數(shù)據(jù)；以及

尺度估計模塊，將所述視覺圖像數(shù)據(jù)和IMU位姿數(shù)據(jù)進(jìn)行融合，實現(xiàn)尺度估計和定位建圖，獲得具備真實尺度的地圖信息，完成待測目標(biāo)的距離測距。

2.根據(jù)權(quán)利要求1所述的單目視覺和慣性傳感器融合的遠(yuǎn)距離測距系統(tǒng)，通過對單目相機采集的無絕對尺度信息的視覺圖像數(shù)據(jù)進(jìn)行處理，獲得視覺位姿數(shù)據(jù)。

3.根據(jù)權(quán)利要求2所述的單目視覺和慣性傳感器融合的遠(yuǎn)距離測距系統(tǒng)，所述視覺位姿數(shù)據(jù)包括：單目相機位置，單目相機旋轉(zhuǎn)矩陣，單目相機旋轉(zhuǎn)四元數(shù)，以及單目相機旋轉(zhuǎn)角速度。

4.根據(jù)權(quán)利要求1所述的單目視覺和慣性傳感器融合的遠(yuǎn)距離測距系統(tǒng)，所述IMU模塊包括陀螺儀、加速度計；所述IMU位姿數(shù)據(jù)包括：IMU加速度、IMU角速度。

5.一種單目視覺和慣性傳感器融合的遠(yuǎn)距離測距方法，通過如權(quán)利要求1至4任一項所述的單目視覺和慣性傳感器融合的遠(yuǎn)距離測距系統(tǒng)進(jìn)行遠(yuǎn)距離測距，所述測距方法，包括：

步驟S1：通過單目相機采集視覺圖像數(shù)據(jù)，并進(jìn)行視覺定位和重投影構(gòu)建地圖；

步驟S2：在步驟S1所構(gòu)建的地圖中標(biāo)注出待測距的目標(biāo)物體；

步驟S3：對所構(gòu)建的地圖構(gòu)建的地圖進(jìn)行尺度估計；以及

步驟S4：根據(jù)尺度估計結(jié)果，對構(gòu)建的地圖進(jìn)行尺度縮放，從而獲得具備真實尺度的地圖信息，獲得待測目標(biāo)距離數(shù)據(jù)。

6.根據(jù)權(quán)利要求5所述的單目視覺和慣性傳感器融合的遠(yuǎn)距離測距方法，步驟S1中，單目視覺SLAM模塊通過單目相機觀測路標(biāo)點，將三維路標(biāo)點投影到相機的成像平面，得到像素坐標(biāo)；成像時的模型采用針孔成像模型；單目視覺SLAM模塊的單個路標(biāo)點按照如下方程進(jìn)行觀測：

ZP_uv＝K(RP_W+t)＝KTP_W；

其中，P_u，v是圖像上的像素坐標(biāo)，P_W＝(X Y Z)^T是路標(biāo)點在相機坐標(biāo)系下的三維坐標(biāo)，矩陣K是相機內(nèi)參數(shù)矩陣，T是相機從相機坐標(biāo)系到世界坐標(biāo)系的變換矩陣，R為相機的旋轉(zhuǎn)矩陣，t是相機從相機坐標(biāo)系到世界坐標(biāo)系的平移向量，Z是路標(biāo)點在相機坐標(biāo)系下的Z軸坐標(biāo)，也即路標(biāo)點在相機坐標(biāo)系下的深度值；之后將觀測結(jié)果進(jìn)行最小化重投影誤差，π_(·)是投影函數(shù)，從而可以得到視覺觀測的單目相機位姿結(jié)果，即相機的旋轉(zhuǎn)矩陣R和相機的位置p：

其中，Xⁱ是第i個路標(biāo)點在相機坐標(biāo)系下的位置，是第i個路標(biāo)點在像素平面的投影坐標(biāo)，ρ()是魯棒核函數(shù)，χ是從單目相機捕獲的圖像中提取的所有特征點(也即路標(biāo)點)的集合，∑是求和符，是指使其后面式子達(dá)到最小值時的變量R，t的取值，p是相機的位置數(shù)據(jù)，也即相機的平移向量t。

7.根據(jù)權(quán)利要求5所述的單目視覺和慣性傳感器融合的遠(yuǎn)距離測距方法，單目視覺SLAM模塊使用ORB特征點和三個主要的并行線程：實時跟蹤特征點的前端跟蹤線程；用于構(gòu)建局部集束調(diào)整的后端局部建圖優(yōu)化線程；以及用于消除累積漂移和全局位姿圖優(yōu)化的后端回環(huán)檢測線程。

8.根據(jù)權(quán)利要求5所述的單目視覺和慣性傳感器融合的遠(yuǎn)距離測距方法，步驟S2中，在定位和建圖的過程中，當(dāng)穩(wěn)定的相機圖像畫面之中出現(xiàn)了待測距的目標(biāo)物體，則在圖像中手動選取目標(biāo)物體的特征點，之后保持相機穩(wěn)定繼續(xù)移動一定距離，繼續(xù)建圖，并再次在圖像中手動選取之前已選定的特征點，使包含目標(biāo)物體的場景在所構(gòu)建的地圖中形成閉環(huán)；當(dāng)構(gòu)建穩(wěn)定的閉環(huán)地圖之后，操作結(jié)束，否則繼續(xù)移動和選取目標(biāo)物體特征點。